JP4992702B2 - Robot motion control apparatus and motion control method thereof - Google Patents

Description

  The present invention relates to a robot motion control apparatus and a motion control method for controlling a robot motion by giving a command value to each of a plurality of drive units and interlocking the plurality of drive units.

There is a configuration in which an operation value of a robot is controlled by giving a command value to each of a plurality of drive units and interlocking the plurality of drive units (see, for example, Patent Documents 1 and 2).
JP-A-56-76394 JP 2002-127053 A

  By the way, in the above-described configuration, there is a case where a difference between the target locus with respect to the command value and the actual operation locus is generated as the servo delay time. In this case, if the servo delay time generated for each axis is the same, a delay in the time axis will occur, but the actual motion trajectory will not deviate from the target trajectory. If the times are different, there is a problem that not only the delay in the time axis occurs but also the actual motion locus deviates from the target locus. As a result, there is a problem that the dynamic accuracy of the robot is lowered and the time required for teaching is prolonged.

  The present invention has been made in view of the above-described circumstances, and an object of the present invention is to improve the dynamic accuracy of the robot and reduce the time required for teaching and the operation of the robot. It is to provide a control method.

According to the first and fourth aspects of the present invention, for each axis corresponding to a plurality of drive units, the difference between the target trajectory with respect to the command value and the actual motion trajectory is calculated as the servo delay time. The reference delay time is determined so that the servo delay time for each axis is the same for all of the multiple axes, and based on the calculated servo delay time for each axis and the determined reference time Since the compensation amount for each axis is calculated, the command value reflecting the determined compensation amount for each axis is given to each of the plurality of drive units, and the operation of the robot is controlled by linking the plurality of drive units. By controlling so that the servo delay time for each axis is the same for all of the multiple axes, the actual motion trajectory can be brought closer to the target trajectory, and the robot's dynamic accuracy can be improved. Reduce the time required Can.

According to the invention described in claim 2 and 5, the trajectory error is estimated based on the servo delay for each axis calculated compensation amount for each determined axial time, to determine the trajectory error that the estimated, the estimated The robot's movement speed is reduced when it is determined that the trajectory error is greater than the allowable value, so the robot's movement can be stabilized within a certain range and interference with surrounding equipment is minimized. be able to.

According to the third and sixth aspects of the present invention, the command value is interpolated with a polynomial for each of the axes corresponding to the plurality of drive units, so that the difference between the target locus and the actual operation locus with respect to the instruction value is servo delayed. Since the calculation is performed as time, the calculation accuracy of the servo delay time can be increased, and the dynamic accuracy can be improved reliably.

  Hereinafter, an embodiment in which the present invention is applied to a control device for controlling the operation of a vertical articulated robot will be described with reference to the drawings. As shown in FIG. 1, a robot apparatus 1 as an industrial robot apparatus includes a vertical articulated robot (hereinafter referred to as a robot) 2 and a control device 3 for controlling the operation of the robot 2 (operation control in the present invention). Means, servo delay time calculation means, reference time determination means, compensation amount calculation means, trajectory error estimation means, trajectory error determination means, operation speed reduction means) and a teaching pendant 4 connected to the control device 3. It is configured.

  The robot 2 is a robot having, for example, a six-axis vertical articulated joint, and is supported by the base 5, a shoulder portion 6 supported by the base 5 so as to be able to turn in the horizontal direction, and supported by the shoulder portion 6 so as to be able to turn in the vertical direction. The lower arm 7, the first upper arm 8 supported by the lower arm 7 so as to be pivotable in the vertical direction, and the second upper arm 8 supported by the tip portion of the first upper arm 8 so as to be able to rotate. An upper arm 9, a wrist 10 supported by the second upper arm 9 so as to be rotatable in the vertical direction, and a flange 11 supported so as to be rotatable (twisted) by the wrist 10. Yes.

  The shoulder portion 6, the lower arm 7, the first upper arm 8, the second upper arm 9, the wrist 10, and the flange 11 including the base 5 described above function as a link (driving portion in the present invention) in the robot 2. Each link except for the base 5 is rotatably connected to the lower link by a rotary joint. A flange (not shown) for gripping a workpiece can be attached to the flange 11 which is the most advanced link. In addition, the rotary joint that connects the links is provided with a reduction gear that reduces the rotation of the motor fixed to the previous link and transmits it to the next link.

  In this embodiment, the joint axis of the rotary joint that connects the base 5 that is the first link and the shoulder portion 6 that is the second link is the first axis, and the shoulder portion 6 that is the second link. The joint axis of the rotary joint that connects between the first link and the lower arm 7 that is the third link is the second axis, and the lower arm 7 that is the third link and the first upper arm 8 that is the fourth link The joint axis of the rotary joint that connects between the first upper arm 8 that is the fourth link and the second upper arm 9 that is the fifth link is the third axis. The fourth axis, the fifth link as the fifth link, and the wrist 10 as the sixth link as the joint axis of the rotary joint that connects the second upper arm 9 as the fifth link and the wrist 10 as the sixth link. And the joint 11 of the rotary joint that connects the flange 11 that is the seventh link is shown as a sixth axis.

  As illustrated in FIG. 2, the control device 3 that controls the operation of the robot 2 includes a CPU 12, a drive circuit 13, and a position detection circuit 14. The CPU 12 is connected with a ROM 15 for storing a robot language for creating a system program and an operation program for the entire robot 2 and a RAM 16 for storing an operation program for the robot 2 and used for teaching work. Teaching pendant 4 is connected. As shown in FIG. 1, the teaching pendant 4 includes various operation units 4a and a display 4b.

  The position detection circuit 14 is for detecting the positions of the shoulder unit 6, the arms 7 to 9, the wrist 10 and the flange 11. The position detection circuit 14 is a drive source for the shoulder unit 6, the arms 7 to 9, the wrist 10 and the flange 11. A rotary encoder 18 provided in a certain motor 17 is connected. Based on the detection signal input from the rotary encoder 18, the position detection circuit 14 rotates the shoulder portion 6 with respect to the base 5, the lower arm 7 with respect to the shoulder 6, and the first upper arm 8 with respect to the lower arm 7. The angle, the rotation angle of the second upper arm 9 relative to the first upper arm 8, the rotation angle of the wrist 10 relative to the second upper arm 9, and the rotation angle of the flange 11 relative to the wrist 10 are detected, and the detected position detection information Is output to the CPU 12. The CPU 12 controls the operation of the shoulder unit 6, the arms 7 to 9, the wrist 10 and the flange 11 based on the operation program using the position detection information input from the position detection circuit 14 as a feedback signal. To do.

  As shown in FIG. 1, three-dimensional coordinates are defined for each link. Among these, the coordinate system of the base 5 installed on the floor surface is a reference coordinate of the robot 2 as a stationary coordinate system, and the horizontal coordinate axes Xb and Yb are set with the center of the lower end of the base 5 as the origin. And one coordinate axis Zb in the vertical direction is defined. In the coordinate system of the other links, the position and orientation on the reference coordinates change due to the rotation of each rotary joint, and the CPU 12 receives the shoulder 6, the arms 7 to 9, the wrist 10, and the flange input from the position detection circuit 14. 11 based on the position detection information of each rotary joint and the length information of each joint stored in advance, the position and orientation of the coordinates of each joint are converted to the position and orientation on the reference coordinates by the coordinate conversion calculation function. Convert to and recognize.

  Of the coordinate systems of the joints described above, the coordinate system of the flange 11 has the rotation center of the tip surface of the flange 11 as the origin, the two coordinate axes Xm and Ym on the tip surface of the flange 11 and the rotation axis of the flange 11. Defines one coordinate axis Zm. Note that the rotation angles with respect to the coordinate axes Xm, Ym, and Zm are represented by a yaw angle RX, a pitch angle RY, and a roll angle RZ.

Next, the operation of the above-described configuration will be described with reference to FIGS.
In the control device 3, the CPU 12 calculates a command value for each axis when giving an operation command to the robot 2 (step S1), and outputs the calculated command value to the servo (drive unit) of each axis (step S1). S2). Here, when the robot 2 receives a command value from the control device 3, the motor 17, which is a drive source for the shoulder unit 6, the arms 7 to 9, the wrist 10, and the flange 11, operates based on the command value that has been input. The motor 17 which is a driving source of the shoulder portion 6, the arms 7 to 9, the wrist 10 and the flange 11 operates.

  In the control device 3, the CPU 12 calculates the actual motion trajectory by detecting the angle of each axis (step S3), and the difference between the target trajectory for the command value and the actual motion trajectory for each axis is used as the servo delay time. Calculate (step S4). That is, as shown in FIG. 4, for example, the second axis and the third axis, the CPU 12 calculates the delay time of the actual speed with respect to the target speed, and the time corresponding to “ta−t2” for the second axis. “T2” is calculated as the servo delay time at time “ta”, and for the third axis, time “T3” corresponding to “ta−t3” is calculated as the servo delay time at time “ta”. The CPU 12 interpolates the command value for each axis with a polynomial to calculate the difference between the target locus with respect to the command value and the actual motion locus as the servo delay time, thereby improving the calculation accuracy of the servo delay time. Yes.

  Next, the CPU 12 compares the calculated servo delay times for each axis, and determines a reference time (reference value) so that the servo delay times for each axis are the same for all of the plurality of axes (step S5). In other words, the CPU 12 will explain the above-described second axis and third axis, for example, in the state shown in FIG. 4, the servo delay time “T2” of the second axis and the servo delay time “T3” of the third axis. Since the servo delay time of the second axis is shorter than the servo delay time of the third axis, the servo delay time of the second axis is determined as the reference time. Note that the CPU 12 may not determine the shortest servo delay time as the reference time in this way, but may determine another time such as a time corresponding to the average of the two as the reference time.

  Next, the CPU 12 calculates a compensation torque (compensation amount) for each axis based on the calculated servo delay time for each axis and the determined reference time (step S6). Then, the CPU 12 estimates a trajectory error based on the calculated compensation torque for each axis and the servo delay time for each axis (step S7), and whether or not the estimated trajectory error is equal to or greater than a preset allowable value. Is determined (step S8).

  If the CPU 12 determines that the estimated trajectory error is not equal to or greater than the allowable value (less than the allowable value) (“NO” in step S8), the CPU 12 outputs a command value reflecting the compensation torque to the servo of each axis. The feedback control is performed so that the servo delay time is the same for all axes, that is, the difference in servo delay time between the axes is eliminated (step S9), and the process returns to step S3 described above. Repeat the process. The CPU 12 performs P control (proportional control), PI control (proportional integral control) or PID control (proportional integral differential control) as feedback control. On the other hand, when the CPU 12 determines that the estimated trajectory error is greater than or equal to the allowable value (“YES” in step S8), the operation speed is reduced (step S10), and the process returns to the above-described step S3. Repeat the process. Although the above description has been made on the two axes of the second axis and the third axis, the same applies to other axes, and the same applies to three or more axes.

  As described above, according to this embodiment, the difference between the target trajectory for the command value and the actual motion trajectory is calculated as the servo delay time for each axis, and the calculated servo delay time for each axis is compared. The reference time is determined so that the servo delay time for each axis is the same for all the axes, and the compensation torque for each axis is calculated based on the calculated servo delay time for each axis and the determined reference time. Since the command value reflecting the calculated compensation torque for each axis is output to the servo of each axis to control the operation of the robot 2, the actual motion trajectory can be brought close to the target trajectory. Dynamic accuracy can be improved, and the time required for teaching can be shortened.

  Moreover, since the shortest servo delay time among the calculated servo delay times for each axis is determined as the reference time, the delay on the time axis can be suppressed as much as possible. Further, when a trajectory error is estimated based on the calculated compensation torque for each axis and the calculated servo delay time for each axis, and it is determined that the estimated trajectory error is greater than or equal to an allowable value, the operation speed of the robot 2 is determined. Since it is made to reduce, operation | movement of a robot can be stabilized within a fixed range, and interference with surrounding facilities etc. can be suppressed as much as possible.

  Furthermore, by interpolating the command value for each axis with a polynomial, the difference between the target trajectory for the command value and the actual motion trajectory is calculated as the servo delay time, so the servo delay time calculation accuracy can be improved. The dynamic accuracy of the robot 2 can be improved with certainty.

The present invention is not limited to the above-described embodiment, and can be modified or expanded as follows.
The vertical articulated robot is not limited to the six-axis robot.

The perspective view of an industrial robot apparatus which shows one Embodiment of this invention Functional block diagram flowchart Diagram showing servo delay time

Explanation of symbols

  In the drawings, 2 is a vertical articulated robot (robot), 3 is a control device (operation control means, servo delay time calculation means, reference time determination means, compensation amount calculation means, locus error estimation means, locus error determination means, operation (Speed reduction means), 6 is a shoulder part (drive part), 7-9 are arms (drive part), 10 is a wrist (drive part), and 11 is a flange (drive part).

Claims (6)

In a robot motion control apparatus comprising motion control means for controlling a robot motion by giving a command value to each of a plurality of drive units and interlocking the plurality of drive units,
Servo delay time calculating means for calculating a difference between a target locus for the command value and an actual operation locus as a servo delay time for each axis corresponding to the plurality of drive units;
A reference time determining means for comparing the servo delay time for each axis calculated by the servo delay time calculating means and determining the reference time so that the servo delay time for each axis is the same for all of the plurality of axes;
Compensation amount calculation means for calculating a compensation amount for each axis based on the servo delay time for each axis calculated by the servo delay time calculation means and the reference time determined by the reference time determination means,
The servo delay time calculating means is configured to calculate the time ta based on a target speed calculated by modeling from a command value at a time ta and an actual speed of an axis corresponding to the plurality of driving units at the time ta. A difference from the time t at which the command value corresponding to the actual speed at the time ta is output at the modeled target speed is calculated as a servo delay time.
The compensation amount calculating means uses the average value of the servo delay times of the axes corresponding to the plurality of driving units as a reference time T, and the reference time T and the servo delay times for the axes corresponding to the plurality of driving units, And calculating the compensation amount based on
The motion control means controls the operation of the robot by giving a command value reflecting the compensation amount for each axis calculated by the compensation amount calculation means to each of the plurality of driving sections and interlocking the plurality of driving sections. A motion control apparatus for a robot. The robot motion control apparatus according to claim 1,
Trajectory error estimation means for estimating a trajectory error based on the compensation amount for each axis calculated by the compensation amount calculation means and the servo delay time for each axis calculated by the servo delay time calculation means;
Trajectory error determination means for determining the trajectory error estimated by the trajectory error estimation means;
And an operation speed reduction means for reducing the operation speed of the robot when the trajectory error determination means determines that the trajectory error estimated by the trajectory error estimation means is greater than or equal to an allowable value. Robot motion control device. In the robot motion control apparatus according to claim 1 or 2,
The servo delay time calculation means interpolates the command value with a polynomial for each of the axes corresponding to the plurality of drive units, so that a difference between the target locus and the actual operation locus with respect to the command value is used as a servo delay time. A robot motion control device characterized by calculating. In a method of controlling the operation of the robot by giving a command value to each of a plurality of driving units and interlocking the plurality of driving units,
For each axis corresponding to the plurality of drive units, a target speed calculated by modeling the command value at time ta and an actual operation speed of the axis corresponding to the plurality of drive units at time ta Based on this, the difference between the time ta and the time t at which the command value corresponding to the actual operating speed at the time ta is output at the modeled target speed is calculated as a servo delay time, and the calculated axis An average value of each servo delay time is determined as a reference time T, and a compensation amount for each axis is calculated based on the calculated servo delay time for each axis and the determined reference time. A robot motion control method, comprising: giving a command value reflecting a compensation amount to each of the plurality of drive units to control the operation of the robot by interlocking the plurality of drive units. In the robot operation control method according to claim 4,
Based on the determined compensation amount for each axis and the calculated servo delay time for each axis, the trajectory error is estimated, the estimated trajectory error is determined, and it is determined that the estimated trajectory error is greater than the allowable value. Sometimes, the robot operation control method is characterized by reducing the operation speed of the robot. The robot motion control method according to claim 4 or 5,
A robot that calculates a difference between a target locus for the command value and an actual motion locus as a servo delay time by interpolating the command value with a polynomial for each axis corresponding to the plurality of driving units. Operation control method.

Images