JP6547319B2 - Robot control device and control method - Google Patents

Description

  The present invention relates to a control device and control method of a robot.

  Conventionally, when the movement speed of the control point of the robot exceeds the reference speed at the time of manual operation of the robot, the movement target position is corrected so that the movement speed becomes equal to or less than the reference speed to operate the robot (patented Reference 1).

Patent No. 3994487

  However, even if the moving speed of the tip of the arm set as the control point of the robot is controlled to be equal to or lower than the reference speed, the moving speed of the arm may not be sufficiently suppressed in some cases. The person paid attention.

  The present invention has been made in view of such circumstances, and has as its main object to provide a control apparatus and control method of a robot capable of sufficiently suppressing the moving speed of an arm.

  The first means is applied to a robot including an arm including a plurality of rotating parts, a joint rotatably connecting the rotating parts to each other, and a servomotor for driving the rotating parts, and the tip of the arm It is a control device of a robot which controls a position and an attitude of the control point by CP control which sets an operation track as a control point and sets an operation trajectory when moving the control point to a target. Current monitoring position calculating means for calculating the current position of each monitoring unit, next cycle position and attitude calculating means for calculating the position and attitude of the control point after the operation cycle of the position and attitude of the control point, and the next cycle position and attitude Next cycle monitoring position calculating means for calculating the position of each monitoring unit after the operation cycle based on the position and attitude after the operation cycle calculated by the calculating means, and the current monitoring position calculating means Speed calculation means for calculating the speed of each monitoring section based on the current position of each monitoring section and the position after the operation cycle of each monitoring section calculated by the next cycle monitoring position calculation section And the speed of the target monitoring unit which is the monitoring unit which becomes the maximum speed on condition that the maximum speed among the speeds of the monitoring units calculated by the speed calculating means is higher than a reference speed. The posture after the operation cycle of the control point is calculated based on the target correction means for correcting the position of the target monitoring unit so as to be equal to or lower than the reference speed, and the position of the target monitoring unit corrected by the target correction unit. The posture correction means for correcting, and the current position and posture of the control point are before the position of the control point calculated by the next cycle position and posture calculation means and the posture of the control point corrected by the posture correction means As controlled after operation period, characterized in that it comprises a driving means for driving the respective servomotors.

  According to the above configuration, the arm of the robot includes a plurality of rotating parts, and the rotating parts are rotatably connected to each other by joints. Then, the tip of the arm is set as a control point, and the current position and attitude of the control point are controlled by CP control that sets an operation trajectory when moving the control point to a target.

  Here, even if the moving speed of the tip of the arm set as the control point of the robot is controlled to be equal to or lower than the reference speed, depending on the posture of the arm (robot), movement of parts other than the control point in the arm The inventor noted that the speed may be higher than the reference speed.

  Therefore, in the above configuration, the monitoring unit is set in each rotating unit. For example, when rotating each rotation unit, a portion farthest from the joint serving as the rotation center is set as a monitoring unit of each rotation unit. Then, the position and orientation of the control point and orientation after the operation cycle are calculated, and the position of each monitoring unit after the operation cycle is calculated based on the calculated position and orientation after the operation cycle. Subsequently, the speed of each monitoring unit is calculated based on the current position of each monitoring unit and the position after the operation cycle of each monitoring unit. Then, the position of the target monitoring unit is corrected such that the speed of the target monitoring unit, which is the monitoring unit having the maximum speed, becomes equal to or lower than the reference speed.

  Subsequently, the posture after the operation cycle of the control point is corrected based on the corrected position of the target monitoring unit. Then, each servomotor is driven such that the current position and attitude of the control point are controlled after the operation cycle up to the position of the calculated control point and the attitude of the corrected control point. Therefore, by correcting the attitude of the control point, it is possible to reduce the speed of each monitoring unit to the reference speed or less while maintaining the position (speed of the control point) after the operation cycle of the control point. In particular, when the robot performs a pick and place operation, the posture of the control point (robot) is not important, and the position of the control point is important, so the above control is effective.

  As a result of correcting the position of the target monitoring unit so that the speed of the target monitoring unit is less than or equal to the reference speed, it may not be possible to realize the position of the control point after the operation cycle. In this case, not only the attitude of the control point but also the position of the control point needs to be corrected.

  In this respect, in the second means, the posture correction means is after the operation cycle of the control point calculated by the next cycle position and posture calculation means based on the position of the target monitoring unit corrected by the target correction means. The position and attitude of the control point after the operation cycle are corrected on the operation trajectory when it is determined that the position of the position can not be realized, and the drive means is configured to correct the current position and attitude of the control point The servomotors are driven to be controlled after the operation cycle up to the position and attitude of the control point corrected by the means.

  According to the above configuration, when it is determined that the position after the operation cycle of the calculated control point can not be realized based on the corrected position of the target monitoring unit, after the operation cycle of the control point on the operation trajectory Position and attitude are corrected. Then, each servomotor is driven such that the current position and attitude of the control point are controlled after the operation cycle up to the corrected position and attitude of the control point. For this reason, control of the position and attitude of the control point can be continued while maintaining the position of the control point on the motion trajectory.

  In the third means, the posture correction means calculates the position of the control point after the operation cycle calculated by the next cycle position / posture calculation means based on the position of the target monitoring unit corrected by the target correction means. When it is determined that the control point can not be realized, the position after the operation cycle of the control point is located on the motion trajectory to a feasible position closest to the position after the operation cycle of the control point calculated by the next cycle position and orientation calculation unit. Correct the position.

  According to the above configuration, when it is determined that the position after the operation cycle of the calculated control point can not be realized based on the corrected position of the target monitoring unit, the position of the control point calculated on the operation trajectory The position after the operating cycle of the control point is corrected to the closest possible position to the position after the operating cycle. Therefore, the control point can be moved to a position closest to the position after the operation cycle of the calculated control point among the positions that can be realized after the operation cycle of the control point, and the decrease in speed of the control point is minimized. You can fasten it.

  In a robot provided with a support that supports the end of the arm opposite to the tip of the arm, in order to make the speed of the target monitor equal to or lower than the reference speed, to correct the position of the target monitor In the arm, the speed of the rotating portion on the support side may be reduced rather than the target monitoring portion.

  In this respect, according to the fourth means, the robot is provided with a support portion which supports the end of the arm opposite to the tip end of the arm so that the object correction means is the arm The position of the target monitoring unit is corrected by reducing the amount of change in the angle of the servomotor provided closer to the support than the target monitoring unit.

  According to the above configuration, the position of the object monitoring unit is corrected by reducing the amount of change in the angle of the servomotor provided closer to the support than the object monitoring unit in the arm. Therefore, it is possible to efficiently reduce the speed of the target monitoring unit while suppressing the number of servomotors that reduce the amount of change in angle.

  In a fifth means, the target correction means corrects the position of the target monitoring unit based on the value of the ratio between the maximum speed and the reference speed.

  In the above configuration, the position of the target monitoring unit is corrected based on the value of the ratio of the maximum speed to the reference speed among the calculated speeds of the monitoring units. Therefore, it is possible to appropriately reduce the speed of the target monitoring unit so that the speed of the monitoring unit at which the speed is the maximum is equal to or lower than the reference speed. The value of the ratio between the maximum velocity and the reference velocity is a value obtained by dividing the maximum velocity by the reference velocity (ratio value = maximum velocity / reference velocity).

  In the sixth means, a portion which is the furthest from the joint, which is the center of rotation when rotating each of the rotating portions, is set as the monitoring portion of each of the rotating portions.

  According to the above configuration, when rotating each rotation unit, the part farthest from the joint serving as the rotation center is set as the monitoring unit of each rotation unit. For this reason, in each rotation part, the part with high possibility that speed becomes high can be set to a monitoring part, and the movement speed of an arm can fully be suppressed.

  As the speed calculation means, specifically, as in the seventh means, the distance between the current position of each monitoring unit and the position after the operation cycle of each monitoring unit is divided by the operation cycle. A configuration may be adopted in which the speed of each monitoring unit is calculated.

  Further, as the next cycle monitoring position calculation means, specifically, as in the eighth means, the position and attitude after the operation cycle of the control point calculated by the next cycle position and attitude calculation means are inversely converted The position after the operation cycle of each of the monitoring units can be calculated based on the angle after the operation cycle of each of the servomotors obtained thereby and the size of each of the rotating units.

  A ninth means is applied to a robot comprising an arm including a plurality of rotating parts, a joint rotatably connecting the rotating parts to each other, and a servomotor for driving the rotating parts, and the tip of the arm It is a control device of a robot which controls a position and an attitude of the control point by CP control which sets an operation track as a control point and sets an operation trajectory when moving the control point to a target. A current monitoring position calculating step of calculating the current position of each monitoring unit, a next cycle position and posture calculating step of calculating the position and posture of the control point after the operation cycle of the position and posture of the control point, and the next cycle position and posture The next cycle monitoring position calculating step of calculating the position of each monitoring unit after the operation cycle based on the position and attitude after the operation cycle calculated in the calculating step, and the current monitoring position calculating step Speed calculation process of calculating the speed of each monitoring section based on the current position of each monitoring section and the position after the operation cycle of each monitoring section calculated in the next cycle monitoring position calculating step And the speed of the target monitoring unit, which is the monitoring unit that becomes the maximum speed, on the condition that the maximum speed among the speeds of the monitoring units calculated in the speed calculating step is higher than a reference speed. The posture after the operation cycle of the control point is calculated based on the target correction process of correcting the position of the target monitoring unit so as to be equal to or lower than the reference speed and the position of the target monitoring unit corrected by the target correction process. The posture correction process to be corrected, and the current position and posture of the control point are before the position of the control point calculated in the next cycle position and posture calculation process and the posture of the control point corrected in the posture correction process As controlled after operation period, characterized in that it comprises a driving step of driving the respective servomotors.

  According to the above-mentioned process, the same operation and effect as the first means can be achieved.

The figure which shows the outline of a robot, a controller, and a teaching pendant. The front view which shows the specific attitude | position of a robot. The flowchart which shows the process sequence of speed suppression control of the arm in 1st Embodiment. The figure which shows position correction of an object monitoring part, and attitude | position correction of TCP. The schematic diagram which shows the position correction of an object monitoring part, and the position and attitude | position correction | amendment of TCP. The flowchart which shows the process sequence of the speed suppression control of the arm in 2nd Embodiment.

First Embodiment
Hereinafter, a first embodiment embodied in a control apparatus of a vertical articulated robot will be described with reference to the drawings. The robot according to this embodiment is used, for example, as an industrial robot in an assembly system such as a machine assembly factory. Then, the robot picks up the work and performs a pick and place operation to put it in a predetermined position.

  First, an outline of the robot 10 will be described based on FIG.

  As shown in the figure, the robot 10 sets a first axis J1, a second axis J2, a third axis J3, a fourth axis J4, a fifth axis J5, as central axes of rotation of the joints that connect the rotating portions to each other. And a sixth axis J6. The operating angle of each part in each of these axes is adjusted through driving of a drive source composed of a servomotor or the like and deceleration by a reduction gear or the like. The servomotors are both capable of rotating in both forward and reverse directions, and drive of the servomotors causes each rotating portion to operate (drive) on the basis of the home position. Each servo motor is provided with an electromagnetic brake for braking its output shaft, and an encoder for outputting a pulse signal according to the rotation angle of the output shaft.

  The robot 10 is installed on the floor, and a first axis J1 extends in the vertical direction. In the robot 10, the base 11 has a fixed portion 12 fixed to a floor or the like, and a rotating portion 13 (first rotating portion) provided above the fixed portion 12; It can rotate in the horizontal direction about the 1 axis J1 as a rotation center. That is, the rotating portion 13 extends in the direction of the first axis J1 and is supported by the fixing portion 12 so as to be rotatable about the first axis J1. In the present embodiment, the fixing portion 12 corresponds to a support portion.

  The lower arm 15 (second rotating portion) is rotatably coupled in a clockwise direction or a counterclockwise direction around a second axis line J2 extending in the horizontal direction. That is, the lower arm 15 extends in a direction away from the second axis J2 included in a plane orthogonal to the first axis J1, and is rotatably supported by the rotating portion 13 around the second axis J2. The lower arm 15 is provided to extend in the vertical direction as a basic posture.

  An upper arm 16 is connected to an upper end portion of the lower arm 15 so as to be rotatable clockwise or counterclockwise about a third axis J3 extending in the horizontal direction. That is, the upper arm 16 extends in a direction away from the third axis J3 parallel to the second axis J2, and is rotatably supported by the lower arm 15 about the third axis J3. The upper arm 16 is provided to extend in the horizontal direction as a basic posture.

  The upper arm 16 is divided into two arm portions at the base end side (the joint side rotating about the third axis J3 at the time of rotation) and the tip end side, and the base end side is the first upper side. The arm 16A (third rotating portion), and the tip end side is a second upper arm 16B (fourth rotating portion). The second upper arm 16B is rotatable in a twisting direction with respect to the first upper arm 16A with a fourth axis J4 extending in the longitudinal direction of the upper arm 16 as a rotation center. That is, the second upper arm 16B extends in the direction of the fourth axis J4 included in a plane orthogonal to the third axis J3, and is rotatably supported by the first upper arm 16A about the fourth axis J4. .

  A wrist portion 17 (fifth rotating portion) is provided at the tip of the upper arm 16 (specifically, the second upper arm 16B). The wrist portion 17 is rotatable with respect to the second upper arm 16B around a fifth axis J5 extending in the horizontal direction. That is, the wrist portion 17 extends in a direction away from the fifth axis J5 orthogonal to the fourth axis J4, and is rotatably supported about the fifth axis J5 by the second upper arm 16B.

  A hand portion 18 (sixth rotating portion) for attaching a work, a tool or the like is provided at the tip of the wrist portion 17. The hand portion 18 is rotatable in a twisting direction around a sixth axis J6 which is a center line of the hand portion 18. That is, the hand portion 18 extends in the direction of the sixth axis J6 orthogonal to the fifth axis J5, and is supported by the wrist 17 so as to be rotatable about the sixth axis J6. As described above, the arm of the robot 10 is configured by the rotating portion 13, the lower arm 15, the upper arm 16, the wrist portion 17, and the hand portion 18.

  The controller 30 (control device) includes a CPU, a ROM, a RAM, a drive circuit, a position detection circuit, and the like. The ROM stores a system program, an operation program, and the like of the robot 10. The RAM stores parameter values and the like when executing these programs. The detection signal of each encoder is input to the position detection circuit. The position detection circuit detects the rotation angle of the servomotor provided in each joint based on the detection signal of each encoder.

  The CPU executes a preset operation program (program) to control the position and attitude of the control point at the tip of the arm based on the position information input from the position detection circuit. Specifically, the CPU performs CP (Continuous Path) control. In CP control, when moving the control point of the arm tip to the target, the operation trajectory (position and attitude) of the control point is set as a time function. The CPU controls the rotation angle (attitude of the arm) of each joint in the arm by CP control so that the position and attitude of the control point follow the motion trajectory. In this embodiment, TCP (Tool Center Point) which is the center point 18 a of the hand 18 of the arm is set as the control point. In addition, the CPU calculates the angle of each joint to realize the position and posture based on the position and posture of the TCP by inverse transformation, and the position and posture of the TCP based on the angles of each joint. It has a function to calculate by forward conversion.

  In the present embodiment, the controller 30 executes the speed suppression control to suppress the moving speed of the arm of the robot 10 to the reference speed or less at the time of teaching the robot 10 (manual operation). The reference speed is defined, for example, at 250 mm / s according to a standard such as JIS or ISO.

  The teaching pendant 40 (operation device) includes a microcomputer including a CPU, a ROM, and a RAM, various manual operation keys, a display 42, and the like. The pendant 40 is connected to the controller 30 and can communicate with the controller 30. The operator (user) can manually operate the pendant 40 to create, correct, register, and set various parameters of the operation program of the robot 10. In teaching which performs correction of an operation program etc., it teaches a teaching point (operating point) through which a TCP, which is a control point, passes in an operation. Then, the operator can operate the robot 10 through the controller 30 based on the taught operation program. In other words, the controller 30 controls the operation of the arm of the robot 10 based on the preset operation program and the operation of the pendant 40.

  Here, even if the moving speed of TCP is controlled to be equal to or lower than the reference speed at the time of teaching (manual operation) of the robot 10, the moving speed of a portion other than the TCP in the arm is dependent on the posture of the robot 10. The inventor of the present application has noted that it may be higher than the reference speed. For example, when the robot 10 is in the posture shown in FIG. 2, when the rotating unit 13 is rotated, the moving speed of the TCP (point C5) is sufficiently smaller than the reference speed. However, the moving speed of the tip of the lower arm 15 (point C2) and one end (point C3) of the upper arm 16 may be higher than the reference speed.

  Moreover, in CP control, when TCP passes a singular point, the posture of the robot 10 may change rapidly, and in this case, the moving speed of the point C2 or the point C3 may be higher than the reference speed. is there.

  Then, when rotating each rotation part, the part most distant from the joint (rotation center axis line of each rotation part) used as a rotation center is set as a monitoring part (point C1-C5) of each rotation part. For example, when the lower arm 15 is rotated, a point C2 farthest from the joint (the connecting portion between the rotating portion 13 and the lower arm 15) which is the rotation center is set as the monitoring portion of the lower arm 15. Similarly, when the upper arm 16 is rotated, the points C3 and C4 farthest from the joint (the connecting portion between the lower arm 15 and the upper arm 16) serving as the rotation center are set as the monitoring portion of the upper arm 16 I do. In addition, when another part (part) is attached to rotation parts, such as upper arm 16, you may set the front-end part etc. of the parts as a monitoring part. Then, the angular velocity of each servomotor is suppressed so that the moving speeds of all the monitoring units become equal to or less than the reference speed.

  FIG. 3 is a flowchart showing a processing procedure of speed suppression control for suppressing the moving speed of the arm of the robot 10 to the reference speed or less. This series of processing is repeatedly executed by the controller 30 at each operation cycle Tr for operating the arm. The operation cycle Tr (control cycle) is, for example, 8 ms. Here, a case where a linear motion trajectory is set as a motion trajectory when moving a control point to a target (linear interpolation) will be described as an example.

  In this series of processing, first, the current angle θk1 of each servomotor is detected (S11). Specifically, based on the detection signal of the encoder provided in each servomotor, the position detection circuit detects the current angle θk1 of each servomotor. In addition, k is a number of 1 to 6 respectively corresponding to the first axis J1 to the sixth axis J6.

  Subsequently, the current position Pi1 of each monitoring unit is calculated based on the current angle θk1 of each servomotor and the size of each rotating unit (S12). i is a number from 1 to 5 corresponding to points C1 to C5, respectively. First, the distance from the center of rotation of each rotating unit to the monitoring unit is calculated based on the size of each rotating unit and the set position of each monitoring unit. . Then, the positions of the points C1 to C5 are calculated by combining the current angle θk1 of each servomotor, the size of each rotating portion, and the distance.

  Subsequently, the amount of change ΔP in the operation period Tr of the position and orientation of the TCP is calculated (S13). Specifically, at the time of teaching, a linear motion trajectory is set based on a taught point taught as a point through which the TCP passes. Then, the amount of change ΔP in the operation cycle Tr of the position and orientation on the linear operation trajectory is calculated.

  Subsequently, the position and orientation P2 after the operation cycle Tr of the TCP are calculated (S14). Specifically, the position and posture P2 after the TCP operation period Tr are calculated by adding the TCP position and posture change amount ΔP to the TCP's current position and posture P1 (P2 = P1 + ΔP).

  Subsequently, an angle θk2 after the operation cycle Tr of each servomotor is calculated (S15). Specifically, the position & orientation P2 after the operation cycle Tr of the TCP is inversely converted to calculate the angle θk2 after the operation cycle Tr of each servo motor.

  Subsequently, the position Pi2 after the operation cycle Tr of each monitoring unit is calculated (S16). i is a number from 1 to 5 corresponding to points C1 to C5, respectively. Here, the position Pi2 after the operation cycle Tr may be calculated based on the angle θk2 after the operation cycle Tr of each servomotor and the size of each rotating portion, as in the process of S12.

  Subsequently, the velocity Vi of each monitoring unit is calculated (S17). Specifically, the speed Vi is calculated by dividing the distance between the current position Pi1 of each monitoring unit and the position Pi2 after the operation cycle Tr by the operation cycle Tr. In addition, i is a number of 1-5 corresponding to points C1-C5, respectively.

  Subsequently, the maximum velocity Vmx, which is the maximum velocity Vi among the velocities Vi of the monitoring units, is calculated (S18), and it is determined whether the maximum velocity Vmx is higher than the reference velocity Vlm (S19). Here, among the monitoring units, the monitoring unit having the maximum velocity Vmx is set as a target monitoring unit as a target to be reduced in velocity. In the above determination, when it is determined that the maximum velocity Vmx is higher than the reference velocity Vlm (S19: YES), a value α of the ratio of the maximum velocity Vmx to the reference velocity Vlm is calculated (S20). That is, the value α of the ratio is calculated by the equation α = Vmx / Vlm (α> 1).

  Subsequently, the position after the operation cycle Tr of the target monitoring unit is corrected based on the value α of the ratio (S21). Specifically, the amount of movement from the current position of the target monitoring unit to the position after the operation cycle Tr is reduced. Specifically, a value obtained by dividing the distance between the current position of the target monitoring unit and the position after the operation cycle Tr by the ratio value α is set as a new movement amount. Here, when the position of the object monitoring unit is corrected so as to make the speed of the object monitoring unit equal to or less than the reference speed, the speed of the rotating unit on the fixed unit 12 side of the object monitoring unit in the arm is reduced. As a result, the change amount Δθ of the angle θ of the target servomotor, which is a servomotor provided on the fixed part 12 side of the target monitoring part in the arm, is reduced.

  Subsequently, the posture after the TCP operation cycle Tr is corrected (S22). Specifically, based on the corrected position of the target monitoring unit, the posture after the TCP operation cycle Tr is corrected so as to realize the position after the TCP operation cycle Tr. In order to realize the position after the operation cycle Tr of the TCP from the position of the object monitoring unit, the rotation amount of the rotating unit on the tip side of the object monitoring unit in the arm may be changed. Therefore, the position after the operation cycle Tr of the TCP is realized by changing the amount of change in the angle of the servomotor provided on the tip end side of the object monitoring unit in the arm. Then, using the angle θk2 after the operation cycle Tr of each servomotor, which is determined by the corrected position after the operation cycle Tr of the object monitoring unit and the corrected attitude after the operation cycle Tr of the TCP, the process from S16 is performed again Run.

  On the other hand, when it is determined in S19 that the maximum velocity Vmx is less than or equal to the reference velocity Vlm (S19: NO), the current position and orientation P1 of TCP operate to the position and orientation P2 after the TCP operation cycle Tr. Each servo motor is driven to be controlled after the period Tr (S23). Here, in the case of going through the processing of S21 and S22, the operation period Tr is the current position and posture P1 up to the position of TCP calculated in the processing of S14 and the posture of TCP corrected in the processing of S22. Drive each servo motor to be controlled later. Then, this series of processing is temporarily ended (END).

  The process of S12 corresponds to the process (currently monitoring position calculating process) as the current monitoring position calculating means, and the process of S14 corresponds to the process (next cycle position and posture calculating process) as the next cycle position and attitude calculating means. The process of S16 corresponds to the process (next cycle monitoring position calculating process) as the next cycle monitoring position calculating means, and the process of S17 corresponds to the process (speed calculating process) as the speed calculating means. Further, the process of S21 corresponds to the process (target correction process) as the target correction means, the process of S22 corresponds to the process (posture correction process) as the posture correction means, and the process S23 as the drive (process) Corresponds to the driving process).

  FIG. 4 is a diagram showing position correction of the object monitoring unit and posture correction of the TCP. In the current operation cycle, it is assumed that the state of the robot 10 (the position and posture of the TCP) is the state shown in FIG. Then, when the drive amount of each servomotor is calculated so as to shift to the state shown in FIG. 4B in the next operation cycle, the speed of the target monitoring unit may exceed the reference speed Vlm. In this case, according to the speed suppression control shown in FIG. 3, the position of the object monitoring unit in the next operation cycle is corrected to the position shown in FIG. 4 (c). Thereby, the speed of the object monitoring unit is reduced to the reference speed Vlm or less. Then, starting from the corrected position of the target monitoring unit, the drive amount of the rotating unit on the tip side of the target monitoring unit in the arm is realized so that the position of the TCP shown in FIG. Is corrected. As a result, while maintaining the position in the next operation cycle of TCP, only the attitude in the next operation cycle of TCP is changed.

  The embodiment described above has the following advantages.

  The speed Vi of each monitoring unit is calculated based on the current position Pi1 of the monitoring unit (points C1 to C5) set in each rotating unit and the position Pi2 after the operation cycle Tr of each monitoring unit. Then, the position of the target monitoring unit, which is the monitoring unit with the maximum speed, is corrected such that the calculated velocity Vi of each monitoring unit becomes equal to or less than the reference velocity Vlm. Then, based on the corrected position of the target monitoring unit, the posture after the operation cycle Tr of the TCP is corrected. Each servomotor is driven such that the current position and orientation P1 of the TCP are controlled after the operation cycle Tr up to the calculated TCP position and the corrected TCP orientation. Therefore, by correcting the TCP attitude, the speed of each monitoring unit can be reduced to the reference speed Vlm or less while maintaining the position (TCP speed) after the TCP operation cycle Tr. In particular, when the robot 10 performs a pick and place operation, the posture of the TCP (robot) is not important, and the position of the TCP is important, so the above control is effective.

  The amount of movement from the current position of the object monitoring unit to the position after the operation cycle Tr is reduced. Specifically, a value obtained by dividing the distance between the current position of the target monitoring unit and the position after the operation cycle Tr by the ratio value α is taken as a new movement amount. As a result, the change amount Δθ of the angle θ of the target servomotor provided closer to the fixed unit 12 than the target monitoring unit in the arm is reduced. Therefore, it is possible to efficiently reduce the speed of the target monitoring unit while suppressing the number of target servomotors for reducing the change amount Δθ of the angle θ.

  The position of the target monitoring unit is corrected based on the value α of the ratio between the maximum velocity Vmx and the reference velocity Vlm. Therefore, it is possible to appropriately reduce the speed of the target monitoring unit so that the speed of the target monitoring unit at which the speed is maximum is equal to or less than the reference speed Vlm.

  -The part most distant from the joint which becomes the rotation center when rotating each rotation part is set as a monitoring part of each rotation part. For this reason, in each rotation part, the part with high possibility that speed becomes high can be set to a monitoring part, and the movement speed of an arm can fully be suppressed.

Second Embodiment
Hereinafter, the second embodiment will be described focusing on differences from the first embodiment. In the present embodiment, when it is determined that the position after the calculated TCP operation cycle Tr can not be realized based on the corrected position of the target monitoring unit, the position after the TCP operation cycle Tr on the operation trajectory and Correct posture P2.

  FIG. 5 is a schematic view showing the position correction of the object monitoring unit and the position and posture correction of the TCP. As shown in FIG. 5A, when moving from the TCP position in the current operation cycle to the TCP position in the next operation cycle, it is assumed that the speed of the target monitoring unit exceeds the reference speed Vlm. In this case, according to the first embodiment, the position of the target monitoring unit is corrected as shown in FIG. 5 (b).

  Here, as a result of correcting the position of the target monitoring unit so that the speed of the target monitoring unit becomes equal to or less than the reference speed Vlm, it may not be possible to realize the position in the next operation cycle of TCP. Specifically, when the position of the corrected target monitoring unit is used as the starting point, the TCP may not reach the position at the next operation cycle of TCP. In this case, not only the TCP attitude but also the TCP position needs to be corrected.

  Therefore, in the present embodiment, as shown in FIG. 5C, the position of TCP in the next operation cycle is corrected to the position of TCP that can be realized on the operation trajectory. Then, the posture of the TCP is corrected starting from the position of the corrected target monitoring unit so as to realize the corrected TCP position.

  FIG. 6 is a flowchart showing the processing procedure of the speed suppression control of the present embodiment. This series of processing is repeatedly executed by the controller 30 at each operation cycle Tr for operating the arm. The operation cycle Tr (control cycle) is, for example, 8 ms. Here, a case where a linear motion trajectory is set as a motion trajectory when moving a control point to a target (linear interpolation) will be described as an example. The same processes as those in the flowchart of FIG. 3 will not be described by giving the same step numbers. The processes of S11 to S21 are the same as the processes of S11 to S21 of FIG. 3.

  Subsequently, based on the corrected position of the target monitoring unit, it is determined whether the position after the TCP operation cycle Tr can be realized (S21a). In this determination, when it is determined that the position after the TCP operation cycle Tr can be realized (S21a: YES), the posture after the TCP operation cycle Tr is corrected without correcting the position after the TCP operation cycle Tr. It corrects (S22). The process of S22 is the same as the process of S22 of FIG. 3, and in this case, the control is the same as that of the first embodiment.

  On the other hand, if it is determined in S21a that the position after the TCP operation cycle Tr can not be realized (S21a: NO), the position after the TCP operation cycle Tr is corrected (S21b). Specifically, the position after the TCP operation cycle Tr is corrected to the feasible position closest to the position after the TCP operation cycle Tr calculated in the process of S14 on the TCP operation trajectory. For example, with the position of the corrected object monitoring unit as the center, the arm moves the TCP in the state in which the rotating unit on the tip end side of the object monitoring unit is most extended in the arm, and the intersection of the operation trajectory and the spherical surface The position after the operation cycle Tr is taken. Thereafter, the posture after the operation cycle Tr of the TCP is corrected based on the corrected position of the target monitoring unit (S22). The process of S22 is the same as the process of S22 of FIG. The process of S21b corresponds to the process as the posture correction means.

  The embodiment described above has the following advantages. Here, only the advantages different from the first embodiment will be described.

  -When it is determined that the position after the calculated TCP operation cycle Tr can not be realized based on the corrected position of the target monitoring unit, the position and posture P2 after the TCP operation cycle Tr on the operation trajectory are It is corrected. Then, each servomotor is driven such that the current position and orientation P1 of the TCP are controlled after the operation cycle Tr up to the position and orientation of the corrected control point. For this reason, control of the position and attitude of the TCP can be continued while maintaining the position of the TCP on the motion trajectory.

  -When it is determined that the position after the calculated TCP operating cycle Tr can not be realized based on the corrected position of the target monitoring unit, the position after the calculated TCP operating cycle Tr on the operation trajectory The position after the operation cycle Tr of the TCP is corrected to the closest possible position. Therefore, the TCP can be moved to a position closest to the position after the calculated TCP operation cycle Tr among the positions that can be realized after the TCP operation cycle Tr, and the decrease in the TCP speed can be minimized. Can.

  The above embodiment can be modified as follows.

  In CP control, when operating the TCP to the target, an arc operation trajectory may be set as an operation trajectory, and interpolation between two operation points may be performed by an arc. In the second embodiment, the amount of change in the position of the TCP may be reduced on the set arc movement trajectory. And, between the position and attitude P1 and the position and attitude after correction, interpolation is performed by the arc movement trajectory. According to such a configuration, even if the angular velocity of each servomotor is reduced, it is possible to suppress that the TCP deviates from the arc operation trajectory.

  In S19 of FIG. 3, it is determined whether or not the maximum velocity Vmx is higher than the reference velocity Vlm, but even if it is determined whether the maximum velocity Vmx is higher than the determination velocity set slightly higher than the reference velocity Vlm. Good. In this case, the speed suppression control of the arm can be ended quickly.

  In S21 of FIG. 3, the amount of change in the position of the object monitoring unit is divided by the value α of the ratio to reduce the amount of change in the position of the object monitoring portion, but divided by a value slightly larger than the value α of the ratio Thus, the amount of change in the position of the target monitoring unit may be reduced. Also in this case, the speed suppression control of the arm can be terminated promptly.

  In the above embodiment, 250 mm / s defined by the standard such as JIS or ISO is used as the reference speed Vlm, but a speed slightly lower than that, for example, 230 mm / s may be used as the reference speed Vlm. In this case, the moving speed of the arm can be reliably and easily reduced to less than 250 mm / s.

  In the above embodiment, instead of the vertical articulated robot 10, a horizontal articulated robot or the like may be employed.

  DESCRIPTION OF SYMBOLS 10 ... Robot, 13 ... Rotation part, 15 ... Lower arm, 16 ... Upper arm, 16A ... 1st upper arm, 16B ... 2nd upper arm, 17 ... Wrist part, 18 ... Hand part, 30 ... Controller.

Claims (9)

The present invention is applied to a robot including an arm including a plurality of rotating parts, a joint rotatably connecting the rotating parts to each other, and a servomotor for driving the rotating parts, and the distal end of the arm is set as a control point A control device for a robot that controls a position and an attitude of the control point by CP control that sets an operation trajectory when moving the control point to a target,
Current monitoring position calculation means for calculating the current position of each monitoring unit set in each of the rotating units;
Next cycle position and attitude calculation means for calculating the position and attitude of the control point after the operation cycle of the position and attitude of the control point;
Next cycle monitoring position calculating means for calculating the position after the operation cycle of each of the monitoring units based on the position and attitude after the operation cycle calculated by the next cycle position and posture calculating means;
Each of the monitoring units based on the current position of each of the monitoring units calculated by the current monitoring position calculation unit and the position after the operation cycle of each of the monitoring units calculated by the next cycle monitoring position calculation unit Speed calculation means for calculating the speed of
The speed of the target monitoring unit, which is the monitoring unit that becomes the maximum speed, is the reference speed, provided that the maximum speed among the speeds of the monitoring units calculated by the speed calculation unit is higher than the reference speed. Target correction means for correcting the position of the target monitoring unit such that:
Posture correction means for correcting the posture after the operation cycle of the control point based on the position of the target monitoring unit corrected by the target correction unit;
The current position and attitude of the control point are controlled after the operation cycle to the position of the control point calculated by the next cycle position and attitude calculation means and the attitude of the control point corrected by the attitude correction means Drive means for driving each of the servomotors;
A control device of a robot comprising: When the posture correction means determines that the position after the operation cycle of the control point calculated by the next cycle position and posture calculation means can not be realized based on the position of the object monitoring unit corrected by the object correction means Correcting the position and attitude of the control point after the operation cycle on the operation trajectory,
The driving means drives each of the servomotors so that the current position and attitude of the control point are controlled after the operation cycle to the position and attitude of the control point corrected by the attitude correcting means. The control apparatus of the robot of claim 1.   When the posture correction means determines that the position after the operation cycle of the control point calculated by the next cycle position and posture calculation means can not be realized based on the position of the object monitoring unit corrected by the object correction means The position after the operation cycle of the control point is corrected to a feasible position closest to the position after the operation cycle of the control point calculated by the next cycle position and orientation calculation means on the operation trajectory. The control device of the robot according to 2. The robot includes a support portion which supports the opposite ends of the both ends of the arm so as to be opposite to the tip end of the arm.
The object correction means corrects the position of the object monitoring unit by reducing the amount of change in the angle of the servomotor provided closer to the support unit than the object monitoring unit in the arm. The control device of the robot according to any one of 3.   The control device of a robot according to any one of claims 1 to 3, wherein the object correction means corrects the position of the object monitoring unit based on a value of a ratio of the maximum velocity to the reference velocity.   The control device of a robot according to any one of claims 1 to 5, wherein a portion farthest from the joint, which is a rotation center when rotating each rotating unit, is set as the monitoring unit of each rotating unit. .   The speed calculating means calculates the speed of each monitoring unit by dividing the distance between the current position of each monitoring unit and the position after the operation cycle of each monitoring unit by the operation cycle. The control device of the robot according to any one of the above.   The next cycle monitoring position calculation means obtains the angle after the operation cycle of each of the servomotors obtained by inversely converting the position and attitude of the control point after the operation cycle calculated by the next cycle position and attitude calculation means The control device of a robot according to any one of claims 1 to 7, wherein the position after the operation cycle of each of the monitoring units is calculated based on the size of each of the rotating units. A plurality of rotating parts, a joint rotatably connecting the rotating parts to each other, and an arm including a servomotor for driving the rotating parts;
The present invention is applied to a robot installed at a predetermined robot installation site and having a support portion that supports the end of the arm opposite to the tip of the arm movably, and the tip of the arm is a control point A control device for a robot that controls the position and the attitude of the control point by CP control that sets the motion trajectory when setting the motion of the control point to a target,
A current monitoring position calculating step of calculating a current position of each monitoring unit set in each of the rotating units;
Next cycle position and orientation calculation step of calculating the position and orientation of the control point after the operation cycle of the position and orientation of the control point;
A next cycle monitoring position calculating step of calculating a position after the operation cycle of each of the monitoring units based on the position and attitude after the operation cycle calculated in the next cycle position and posture calculating step;
Each monitoring unit is calculated based on the current position of each monitoring unit calculated in the current monitoring position calculating step and the position after the operation period of each monitoring unit calculated in the next cycle monitoring position calculating step. Speed calculation process for calculating the speed of
The speed of the target monitoring unit which is the monitoring unit which becomes the maximum speed is the reference speed, provided that the maximum speed among the speeds of the monitoring units calculated in the speed calculating step is higher than the reference speed. A target correction step of correcting the position of the target monitoring unit such that:
A posture correction step of correcting the posture after the operation cycle of the control point based on the position of the target monitoring unit corrected by the target correction step;
The current position and attitude of the control point are controlled after the operation cycle to the position of the control point calculated in the next cycle position and attitude calculation process and the attitude of the control point corrected in the attitude correction process Driving the respective servomotors;
A control method of a robot, comprising:

Images