JP2006167820A - Control method of robot arm - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a control method of a robot arm, effectively relaxing a collision shock to an obstacle in the robot arm having a flexible mechanism. <P>SOLUTION: A collision detecting part 2 estimates disturbance torque according to the angle information determined on the basis of output information of an angle sensor 68 by an angle information determination part 21 by a reverse dynamical operating part 21 and a disturbance torque estimating part 22 to determine a collision to an obstacle by a collision determination part 23. When a collision to an obstacle is determined, a corrective movement target value generating part 12 corrects a movement target on the basis of a joint angle in a collision, and adjusts a target angle or an angular speed by a command value adjusting part 13 to transmit a control command for a driving motor 60 from a command value switching part 14 to a control command part 3. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a method for controlling a robot arm, and more particularly to a method for controlling a robot arm having a flexible mechanism at a joint when it collides with an obstacle.

  When a robot arm such as an industrial robot performs a predetermined motion, if an obstacle exists on the path, the obstacle prevents the arm from being driven. If the joint drive command is continued in this state, the drive motor, the joint, or the arm main body may be damaged. In order to prevent the occurrence of such a situation, a technique is known in which the occurrence of a collision is detected and the motor is immediately stopped (see, for example, Patent Document 1).

For this control, it is necessary to quickly detect a collision when the collision occurs, and Patent Document 1 discloses a technique for estimating a collision by estimating a disturbance torque of a drive shaft motor.
JP-A-11-254380

  By the way, there is a robot arm that realizes a flexible movement by using a flexible mechanism for a joint portion and can reduce a collision impact on the obstacle at the time of a collision with the obstacle. In this type of robot arm, even if the motor is stopped immediately upon occurrence of a collision, the link angle and the motor angle are inconsistent due to the influence of the spring that calibrates the flexible mechanism and the damping of the damper. For this reason, the arm part ahead of the drive joint cannot be stopped immediately, and the collision impact on the obstacle cannot be sufficiently reduced, and the joint part and the arm main body may be damaged.

  Therefore, an object of the present invention is to provide a robot arm control method capable of effectively mitigating a collision impact on an obstacle in a robot arm having a flexible mechanism.

  In order to solve the above problems, a control method for a robot arm according to the present invention is a control method for a robot arm having a flexible mechanism at a joint, and (1) a step of detecting a displacement angle of each joint of the flexible mechanism. And (2) a step of estimating a disturbance torque acting on each joint based on the detected displacement angle information, (3) a step of detecting the presence or absence of a collision with an obstacle from the estimated disturbance torque, and (4) A step of setting a new movement target value according to the collision force applied to the obstacle when a collision is detected; and (5) giving a dynamic characteristic to the set movement target value to correct the movement target value. And (6) a step of driving the joint portion by a normal motion target value and a corrected motion target value after the collision is detected when no collision is detected. .

  The collision determination is performed by estimating the joint torque generated by the collision phenomenon by the disturbance torque estimated from the displacement angle information of the joint portion. In a robot arm having a flexible mechanism, even if the motor is stopped immediately after a collision, the movement of the arm cannot be immediately stopped due to the influence of an elastic body constituting the flexible mechanism. Therefore, not only to stop the motor, but also to set the motion target value necessary to stop the arm according to the collision force, and to correct this according to the dynamic characteristics and drive the joint part, Take actions such as stopping the arm or avoiding obstacles.

  According to the present invention, by detecting the collision, correcting the movement target value according to the collision force, and driving the joint portion, the movement of the arm is immediately stopped at the time of the collision or separated from the obstacle immediately after the collision. be able to. As a result, the collision impact can be reduced, and the contact time with the obstacle can be shortened to reduce the impact force. In addition, by appropriately correcting the movement target value, it is possible to perform a movement following the movement of the obstacle.

  DESCRIPTION OF EXEMPLARY EMBODIMENTS Hereinafter, preferred embodiments of the invention will be described in detail with reference to the accompanying drawings. In order to facilitate the understanding of the description, the same reference numerals are given to the same components in the drawings as much as possible, and duplicate descriptions are omitted.

  FIG. 1 is a schematic view of a robot arm controlled by the robot arm control method according to the present invention. This robot arm 100 is a multi-joint multi-axis robot arm 100 having multiple degrees of freedom, and shows the configuration of the right arm. A hand part 53 is connected to the housing 50 via an upper arm part 51 and a lower arm part 52. Both the upper arm portion 51 and the lower arm portion 52 are mounted with joint portions that drive in three axial directions with respect to the extending direction of the arm. FIG. 2 is a diagram schematically showing the configuration of each joint.

  Each flexible joint includes pulleys 61 and 62, a gear box 63, a damper 64, and a spring 65 between an output shaft of the motor 60 fixed to the link body on the housing side and an input shaft of the link body 66 on the hand side. Connected and configured. The pulleys 61 and 62 and the gear box 63 are deceleration elements that decelerate the output of the motor 60, and the damper 64 and the spring 65 are respectively a damping element and an elastic element for absorbing the impact of sudden disturbance such as an external collision. Function. Here, the damper 64 and the spring 65 are arranged in parallel to constitute a damping element / elastic element system, and this and the speed reduction elements 61 to 63 are connected in series.

  FIG. 3 is a block configuration diagram of the control unit 200 of the robot arm 100. The control unit 200 includes a CPU, a ROM, a RAM, and the like, and includes a command value creation unit 1, a collision detection unit 2, a control command unit 3, and an angle information determination unit 4. The command value creation unit 1 includes an initial motion target value generation unit 11, a corrected motion target value generation unit 12, a command value adjustment unit 13, and a command value switching unit 14. The collision detection unit 2 includes an inverse dynamics calculation unit 21, a disturbance torque estimation unit 22, and a collision determination unit 23. The control command unit 3 outputs a drive signal to each drive motor 60 of the robot arm 100. In addition, the output of the angle sensor 68 arranged at each joint is input to the angle information determination unit 4, and the determination result of the angle information determination unit 4 is the inverse dynamics calculation unit 21, the disturbance torque estimation unit 22, and the corrected motion target. The value is input to the value generator 12 and the command value adjuster 13.

  Hereinafter, the control method of the robot arm in the present embodiment will be specifically described. Here, collision detection and impact force relaxation after detection will be described. FIG. 4 is a control flowchart thereof.

  First, the angle information determination unit 4 obtains each angle information and motor rotation angle information of the current joint from each angle sensor 68 (step S1). Next, the angle information determination unit 4 obtains angular velocity and angular acceleration information by differentiating each angle information of the joint part and the rotation angle information of the motor with respect to time differentiation or second order differentiation (step S3). From the obtained angle information, angular velocity information, and angular acceleration information, the inverse dynamics calculation unit 21 obtains a joint drive torque based on the inverse dynamic method (step S5). Next, the disturbance torque acting on the link is estimated from the joint driving torque (step S7).

  Taking a one-link arm as shown in FIG. 5 as an example, the equation of motion can be expressed by the following equations (1.1) and (1.2).

Here, θ _m and θ _l are the rotation angles of the motor and the link, τ _f is the joint drive torque, τ _d is the disturbance torque acting on the link, and J _m and J _l are the inertia moments of the motor and the link, respectively. , G (θ) is the gravity component, K is the elastic coefficient of the joint, and D is the viscous friction coefficient of the joint.

  Here, the force generated by the flexible element can be expressed by equation (2).

Since each parameter of the flexible element is known and the motor rotation angle, the link rotation angle, and the angular velocities thereof can be calculated, the torque disturbance acting on the link portion can be expressed by equation (3).

It can be understood from the equation (3) that the disturbance torque τ _d can be calculated by calculating the inertia term and the gravity term in real time. However, since the frictional force must be estimated from experimental values and the like, the estimated value of torque disturbance is expressed by the following equation (4) when the estimated value is distinguished by adding the symbol ^ above. I can express.

  If the estimated value and the true value are equal, the estimated torque disturbance value is 0 in an ideal non-contact state. In practice, however, the estimated torque disturbance value often does not become zero even in a non-contact state due to the effects of parameter errors, friction, and the like. Therefore, the threshold value μ is set, and the collision determination unit 23 determines that there is a contact when the estimated torque disturbance value is greater than or equal to μ, and determines that there is no contact when the estimated value is less than μ (step S9).

Here, in Formula (5), H = 1 means a contact state, and H = 0 means a non-contact state. In the case of the non-contact state, the process proceeds to step S19, where the initial motion target value generation unit 11 sets the motion target value θ _mref , and the command value creation unit 1 drives the motor based on the motion target value θ _mref. The amount is set (step S15), and each drive motor 60 is controlled (step S17), thereby controlling the drive of the robot arm.

  Here, the initial motion target value generation unit 11 performs drive control by regarding the joint as a rigid body. In the one-link arm shown in FIG. 5, when the joint is regarded as a rigid body, the equation of motion can be expressed by the following equation.

  On the other hand, when it determines with a contact state in step S9, it enters into impact force relaxation control. Here, the corrected motion target value generation unit 12, the command value adjustment unit 13, the command value switching unit 14, and the angle information determination unit 4 have the feedback control structure shown in FIG. 6 together with the drive motor 60, the flexible element, and the link system. Constitute.

Here, the link target angle after the collision (which defines the behavior of the arm after the collision, and in addition to stopping the arm at the time of the collision, the operation of returning the arm and avoiding the obstacle, etc.) is set. _lref , if the target angle of the drive system set by the feedback control 1 in the corrected motion target value generation unit 12 in FIG. 6 is θ _mref , the feedback control 2 in the command value adjustment unit 13 is represented by the following expression: Is used.

Here, the control gains K _P2 , K _i2 , and K _d2 are selected so that the equation (6) is stabilized. On the other hand, as feedback control 1 for correcting the target command, control represented by the following expression is used.

Here, the control gains K _P1 , K _i1 , and K _d1 are adjusted so that the impact force falls within the target value. Specifically, the link target angle θ _lref is set (step S11), the drive system target angle θ _mref is set by feedback control 1 (step S13), and the motor drive amount u is set by feedback control 2. (Step S15). A desired link motion is realized by controlling the motor in accordance with the set drive amount (step S17).

  In this way, when a collision is detected, the movement target value is switched, and feedback control is performed in consideration of the behavior of the flexible element, so that the arm movement immediately follows the desired state after the collision is detected. It is possible to reduce the impact force at the time of collision. Here, the case of one link arm has been described as an example, but even in the case of a multi-joint (multi-link), it is applicable by performing the same control for each joint.

  The results of comparing the arm behavior by the robot arm control method according to the present invention and the arm behavior at the time of collision by the conventional control method by simulation will be described below. FIG. 7 shows a link system to be simulated. Here, a one-link arm similar to the link system shown in FIG. 5 is considered. The motor 60 is connected to a link 66 via a damper 64 and a spring 65 connected in parallel, and comes into contact with an obstacle 67 having a spring element.

  Here, at a constant angular velocity of 0 ° to 2.5 ° / sec, the link angle reaches 5 ° after 2 seconds and is maintained until after 5 seconds, and then at a constant angular velocity of −2.5 ° / sec. Then, a target trajectory for returning to a link angle of 0 ° is set, and an obstacle is touched at a position of a link angle of 4 °. In the simulation, in addition to the case 1 for maintaining the same trajectory without changing the target trajectory after the collision, the case 2 for changing the target value to the link angle at the moment of the collision after the collision, and the case 2 Comparison was made for case 3 (corresponding to the control method according to the present invention) to which feedback control 1 was added.

  The simulation results are shown in FIGS. Among these, FIGS. 8 to 10 are diagrams showing temporal changes in the target values, link angles, and motor angles of cases 1 to 3, respectively, and FIG. 11 is a diagram showing temporal changes in the collision impact force. In case 1, as shown in FIG. 8, the link angle θl exceeds the collision angle in order to make the motor angle θm match the target value even after the collision. In other words, the arm bites into the obstacle, which may cause the obstacle to be deformed or broken.

  In the case 2, as shown in FIG. 9, the motor angle θm can be quickly followed immediately after the collision, but since it has a flexible mechanism, there is a time delay for the link angle θl to follow by the inertial force. Arise. As a result, the link angle θl exceeds the collision angle for about 0.2 seconds, so that an impact force is applied to the obstacle.

  On the other hand, in the case of the case 3, as shown in FIG. 10, the target angle is greatly changed in the direction opposite to the traveling direction compared to the case 1 and the case 2 at the moment of the collision. By reversing (the motor angle θm rotates in the opposite direction), the link is quickly avoided from the obstacle. For this reason, the collision impact is mitigated by shortening the amount and time of the link angle exceeding the collision angle. At this time, since the link angle does not substantially exceed the collision angle, the deformation and breakage of the obstacle can be prevented. In particular, in the event of a collision with a human body, etc., there is little risk of hurting the colliding person, and in order to take a collision avoidance action, not only a stationary person or object but also a moving person or object Appropriate avoidance actions can be taken to mitigate collision impacts.

  As shown in FIG. 11, in the case 1, the impact force after the collision is large, and thereafter, a large force is applied to the obstacle for a long time until the initially set target angle falls below the collision angle. It will be. In the case 2, the impact force is limited to a short time immediately after the occurrence of the collision as compared to the case 1, and the impact force at the time of the collision is reduced to about one third of that in the case 1. Furthermore, in the case of the case 3, the generation of the collision force is only an instant at the time of the collision (about one third of the case 2), and the generated collision force is also reduced to about one tenth of the case 1. . As described above, it has been found that the case 3, that is, the method for controlling the robot arm according to the present invention has a high effect of reducing the collision impact.

It is the schematic of the robot arm controlled by the control method of the robot arm concerning the present invention. It is a figure which shows typically the structure of each joint of the robot arm of FIG. It is a block block diagram of the control part of the robot arm of FIG. It is a control flowchart of the control method of the robot arm which concerns on this invention. It is a figure which shows 1 link arm model. It is a figure which shows the feedback control structure in this invention. It is a figure which shows the link type | system | group used as the object of the comparison simulation. 6 is a graph showing a simulation result of case 1. 10 is a graph showing a simulation result of case 2. 10 is a graph showing a simulation result of Case 3. It is a figure which shows the simulation result of cases 1-3.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Command value preparation part, 2 ... Collision detection part, 3 ... Control command part, 4 ... Angle information determination part, 11 ... Initial motion target value generation part, 12 ... Correction motion target value generation part, 13 ... Command value adjustment part , 14 ... Command value switching part, 21 ... Inverse dynamics calculation part, 22 ... Disturbance torque estimation part, 23 ... Collision determination part, 50 ... Housing, 51 ... Upper arm part, 52 ... Lower arm part, 53 ... Hand part, 60 DESCRIPTION OF SYMBOLS ... Drive motor, 61 ... Pulley, 63 ... Gear box, 64 ... Damper, 65 ... Spring, 66 ... Link main body, 67 ... Obstacle, 68 ... Angle sensor, 100 ... Robot arm, 200 ... Control part.

Claims (1)

A control method of a robot arm having a flexible mechanism in a joint part,
Detecting a displacement angle of each joint of the flexible mechanism;
Estimating the disturbance torque acting on each joint based on the detected displacement angle information;
Detecting the collision with the obstacle from the estimated disturbance torque;
When a collision is detected, setting a new motion target value according to the collision force applied to the obstacle; and
A process of giving dynamic characteristics to the set exercise target value and correcting the exercise target value;
When the collision is not detected, the step of driving the joint portion by the normal movement target value, the corrected movement target value after the collision detection,
A method for controlling a robot arm, comprising:

Images