JPH0677205B2 - Robot control method - Google Patents

Description

Description: FIELD OF THE INVENTION The present invention relates to a method for controlling an industrial robot or the like, and more particularly to a robot suitable for avoiding an uncontrollable state near a singular point of a 6-DOF robot. Regarding control method.

[Background of the Invention]

As a method for avoiding a state of being uncontrollable in the vicinity of a singular point of a conventional industrial robot or the like, there is, for example, Japanese Patent Laid-Open No. 58-1148.
As shown in Japanese Patent Laid-Open No. 88, a method has been proposed in which a predetermined amount is added to a target joint angle near a singular point. However, this conventional method cannot handle the robot control when the mode of the joint angle changes only by passing the robot near the singular point, and the robot cannot be driven at a predetermined speed near the singular point. Had.

[Object of the Invention]

The object of the present invention is to solve the above-mentioned problems of the prior art.
In the control of a robot mechanism that has a hand at the end of one pair and is configured with 6 degrees of freedom, even if the posture of the robot mechanism becomes a singular point or its vicinity, the reference point of the hand will follow the specified route. This is to provide a robot control method in which a smooth drive control is possible by making a translational motion and passing near a singular point.

[Outline of Invention]

In order to achieve the above object, the present invention provides a method for controlling a robot mechanism having a hand at the end of six kinematic pairs and having six degrees of freedom, in which one virtual kinematic pair whose axis passes through a reference point of the hand. When the robot mechanism is controlled by using the robot control model with 7 degrees of freedom and the virtual kinematic pair is fixed, and the posture of the robot mechanism approaches the singular point, In the two or more predetermined kinematic pairs (corresponding to the kinematic pairs 4 and 6 in the embodiment described later) whose movements greatly change in (1), the movement (rotational speed) of any one kinematic pair (eg kinematic pair 6) ₆ ) is determined by equation (8) described below so as to reach the final target value (Θ _6f ), and the hand motion is subjected to coordinate conversion for the remaining six pairs including the virtual pair at each sampling (described later). Do (3), (4), ( Based on equations (5) and (6), (9)
By solving the equation), the control command values (rotational speeds ₁ , ₂ , ₃ , ₄ , ₄ ,
₅ ) and by controlling the six pairs of the robot mechanism based on the determined control command value at each sampling and the determined one motion command value (rotation speed ₆ ) of the robot mechanism. The robot control method is characterized in that the reference point is passed through the vicinity of the singular point along a designated path.

Example of Invention

An embodiment of the present invention will be described below with reference to FIGS.

First, FIG. 2 is an external perspective view of a 6-degree-of-freedom articulated robot mechanism as an object of an embodiment of the robot control method according to the present invention. In FIG. 2, a hand 8 for gripping a work or the like is connected to the wrist portion of the robot mechanism 9.

FIG. 3 is an explanatory view showing the skeleton of the robot mechanism 9 of FIG. In FIG. 3, the robot mechanism 9 for connecting the hand 8 of FIG. 2 is composed of six rotating pairs 1 to 6 of 1 to 6, and the axis ₁ ( ₁ is parallel to the axis) of the rotating pairs 1 and 2. Unit vector) and ₂ are right angles (twist angles), axes of pairs ₂ and ₃ and ₄ are parallel, axes of ₃ and ₄ are parallel, and axes of pairs 4 and 5 are
₄ and ₅ are set at right angles, and axes ₅ and ₆ of the pair 5 and 6 are set at right angles. Note that O ₁ -X ₁ Y ₁ Z ₁ is the orthogonal coordinate system of the kinematic pair 1 (Z ₁ is the Z axis of the kinematic pair 2), and Θ _{1 to} Θ ₆ are the rotation angles around the axes of the kinematic pairs _{1 to 6} .

In this robot, there is a position where the pair 6 becomes parallel to the pairs 2, 3 and 4 when the pair 5 rotates, and at this time, the robot mechanism 9 takes a peculiar posture. Further, when the value of the rotation angle Θ ₅ of the even number 5 at this time is γ, Θ ₅ is obtained by this robot mechanism.
If a peculiar posture is taken when = γ, a peculiar posture is taken even with Θ ₅ = γ + π.

Now, when this robot takes a singular posture with a rotation value Θ ₅ = γ of the even number ₅ , a paired surface 4-joint link mechanism is constituted by the even numbers 2, 3 and 4 and 6, and the position / orientation of the hand 8 is at this position. Even if they are fixed inside, the pairs 2, 3 and 4 and 6 can rotate about the axis. This means that the four kinematic pairs 2 and 3 and 4 and 6 have the same degree of freedom inside the mechanism, and the outside has only three degrees of freedom. At this time, the robot has one degree of freedom. The degree of freedom is reduced, and the 6-DOF robot has only the function of the 5-DOF robot. Therefore, even if a free speed command is given to the hand 8 at such a singular point and its vicinity and the robot mechanism 9 is controlled so as to follow the free speed command, in the 6-DOF robot, the 5-DOF robot is set at the singular point. Since it becomes the same, it becomes impossible to control. However, if the robot has 7 degrees of freedom, it will have 6 degrees of freedom even at the singular point, and it will not be out of control. Therefore, according to the present invention, this 6-DOF robot has 7
The robot can be controlled in the vicinity of the singular point based on the virtual 7-degree-of-freedom robot with the second virtual kinematic pair added.

FIG. 1 is an explanatory view showing a virtual pair number setting method of # 7 of an embodiment of a robot control method according to the present invention. In FIG. 1, a 7-degree-of-freedom robot having a new one-degree-of-freedom added to the actual robot mechanism 9 of FIGS. 2 and 3 or a similar 6-degree-of-freedom robot mechanism is hypothesized, and the robots of FIGS. For mechanism 9, the axis of virtual turn pair 7 of # 7
₇ ( ₇ is a unit vector in the axial direction) is a stationary Cartesian coordinate system O
Position of -xyz (position vectors starting at the origin O) passes through the reference point O _h of the hand 8 in, and the axis ₆ of the kinematic pair 6 of # 6 _(6-axis unit vector in the direction) at right angles and To set. When the robot is driven, the rotation angle Θ ₇ of the kinematic pair 7 of the virtual # 7 usually takes a specific value, for example, Θ ₇ = 0, and the rotation speed ₇ is
It is assumed that ₇ = 0.

FIG. 4 shows the position of the hand 8 of the robot mechanism 9 shown in FIGS.
It is explanatory drawing which shows the display method of a posture. Now, as shown in FIG. 4, the position and posture of the hand 8 of the robot mechanism 9 is set to the origin of the position vector from the origin 0 of the stationary rectangular coordinate system O-XYZ of the reference point O _h of the hand 8 and the origin O _h. A unit vector parallel to the three axes of the Cartesian coordinate system of the hand 8,
Display with. Here, the target position / orientation of the hand 8 is _{f 1} ,
_{Let f 1} , _{f 2} , and _f be their current position and orientation,
, In order to make the hand 8 reach the target position / posture from the current position / posture, the translation speed and the rotation speed of the hand 8 may be set as follows.

However, The i-th (i = 1,2, ... 7) parallel to the unit vector in the position _i with the axis of the kinematic pair of even number _i is six kinematic rotation angle theta ₁ of, theta _2, obtained in decreasing ... theta _6, Hand The current position and attitude of 8 can be calculated from Θ ₁ , Θ ₂ , ..., Θ ₆ , Θ ₇ (normally Θ ₇ = 0). Here, _i = ( _i− ) × _i (3) _i = _i (4), and the six-dimensional vector _i is represented by the following equation.

= ( ₁ , _i ) ^T (5) Here, T represents a transposed matrix. Further, if the speed of the hand 8 is S = (,) ^T (6), the following relationship holds.

However, Here, since it is normally assumed that ₇ = 0, by solving the equation (7) for the equation (6) calculated from the equations (1) and (2), That is, ₁ to ₆ are obtained, and if the actuators of the pairs ₁ to ₆ are controlled with this value as the target speed, the hand 8 reaches the final target positions / postures _{f 1} , _{f 2} , _{f 3} , and _{f 2} .

However, when the actual 6-DOF robot approaches the singular pose, the values of the matrix | ₁ , ₂ , ₂ ,…, ₆ | approach zero, The value of any of the elements ₁ to ₆ of the above suddenly becomes large, and cannot be controlled by the above method. Therefore, it is determined whether or not the above 6-degree-of-freedom robot is in the vicinity of the singular point by whether or not the value of Θ ₅ is close to the value of γ or γ + π. That is, an appropriate minute value ε is determined, and it is determined whether or not γ−ε <Θ ₅ <γ + ε or γ + π−ε <Θ ₅ <γ + π + ε is satisfied. When Θ ₅ satisfies this condition, the control method is as follows. To change.

First, the final target position / posture _{f of} the robot hand 8,
rotation angles Θ _{1f of} pairs 1 to 7 corresponding to _{f 1} , _{f 2} , and _{f 3} ,
Θ _2f , ..., Θ _6f (where Θ ₇ = 0) are defined. The values of these rotation angles are usually given by teaching, but if not, they are set to appropriate values by calculation of coordinate conversion. At this time (when the robot with 6 degrees of freedom approaches the singular posture), one movement of the kinematic pair 6 (rotational speed ₆ ) is given a translational velocity u so as to reach the final target value (Θ _6f ). Rotational speed
₆ is defined by the following equation.

Here, the value of Θ ₇ is obtained by integrating the value of the rotation speed _{7 in} the control device.

Also, depending on the values of the rotation angles Θ _{1 to} Θ ₇ , the current position of the hand 8
Calculate the posture ,,, and from this, (1) to (6)
Calculate ₁ , ₂ , ..., ₇ using the formula. here
_Since the value of ₆ is determined by the equation (8), the equation (7) is transformed into the following equation. Incidentally, - _{6 6,} because the robot is moved to the final target position along the position of a predetermined path reference point O _h of the hand 8 is also turned singular.

However, Therefore, even if the robot is near the singular point, the equation (9) can be solved, and the rotation speeds ₁ , ₂ , ₃ , ₄ , ₅ and ₇ are determined. Here, in combination with the equation (8), the rotational speed _{1 of} pairs 1 to 6
By controlling the six kinematic pairs of the robot with the command values of ~ ₆ , the posture of the hand 8 is changed by the virtual kinematic pair 7 even if the robot takes a singular posture, but the reference point O of the hand 8 is changed.
The position of _h can be moved to a final target position along a predetermined path. The value of ₇ is the rotation angle Θ inside the controller.
It is used only in the integral operation to determine ₇ . If the value of ₆ changes abruptly when switching to this operation mode, the actual command value is _{calculated from} the previous command value _6l of ₆ and the value _6d obtained by equation (8) as follows. You can set it. ₆ = _6d + α ( _6d − _6l ) However, α is an arbitrary value in the range of 0 to 1. As described above, even if the robot approaches the peculiar posture, each pair 1 to 6 of the robot has the rotation speed command values ₁ to 6 of the pair 1 to 6.
_Since it is driven and controlled by ₆ , the actual movement of the hand 8 neglects only the movement around the axis of the kinematic pair 7 of the virtual # 7 and passes in the vicinity of the singular point. When handling a rotationally symmetrical tool around the axis, if the virtual rotating pair 7 is made coaxial with the torch axis, it does not have any adverse effect on the work, and the tip of the torch is translated along a predetermined path to perform welding smoothly. be able to.

If such control is performed, finally the rotation angle Θ _{6 of the even number 6}
Reaches the target value Θ _6f and the rotation angles Θ ₁ , Θ ₂ , Θ ₃ , Θ ₄ , Θ ₅ , Θ
_{Since 7} also reaches the target values Θ _1f , Θ _2f , Θ _3f , Θ _4f , Θ _5f , Θ _7f , the rotation angles Θ _{1 to} Θ ₆ of the even pairs _{1 to 6} are the target ΑΘ _1f.
~Θ _6f next, that is, the position and orientation of the hand 8 is the target position and posture _{f, f, f,} and _f. What is neglected in the above control is the rotational movement of the virtual 7 around the even 7 axis, and the even axis ₇ is the reference point O of the hand 8.
_Since it passes _h , the translation speed of the hand 8 is as instructed, and the reference point O _{h of the} hand moves along a predetermined path.

FIG. 5 is a block diagram of a robot system including a robot mechanism and a controller of an embodiment of a robot control method according to the present invention. In FIG. 5, an actuator for driving the robot and its displacement detector (not shown) are built in the robot mechanism 9, and a signal is sent from the displacement detector to the controller 10. The control device 10 has a built-in computing device that executes various computations and a storage device 12 that stores the data that is computed by the computing device 11. The robot mechanism 9 is driven by an actuator via a servo amplifier 13 in response to a command from the control device 10, and the robot hand 8 moves in a predetermined posture along a predetermined path to perform work on a grasped work or the like.

FIG. 6 is the flow chart of FIG. In FIG. 6, the control device 10 first reads the value of the displacement detector (encoder) of the actuator, and from this, the rotation angles Θ _{1 to} Θ ₆ , Θ ₇ (but usually Θ ₇ = 0) of the pairs _{1 to} 6,7. ) Value, the current position / posture of the hand 8 is calculated,
If the deviations between this and the target positions / postures _{f 1} , _{f 2} , _{f 3} , and _{f 2} do not become substantially zero, the following equations (1) to (6)
Calculate ~ ₇ . Where usually | Θ ₅ | <ε or | Θ
₅ - [pi] | <unless epsilon (but gamma = _0), as 7 = _{0, 1-6} (7) calculated from the Jacobian matrix of the _equation, the output value of _1-6 to the servo amplifier 13 as a target speed Then, the actuators of each pair 1 to 6 are controlled so that the deviation becomes substantially zero, and the hand 8 moves to the target position / posture _{f 1} , _{f 2} ,
It ends when it reaches _{f 1} , _{f 2} . However, if | Θ ₅ | <ε or | Θ ₅ −π | <ε near the singular point, ₆ is determined from the target value Θ _6f of Θ _{6 by} the equation (8), and ₁ to ₅ ,
₇ (9) calculated from the Jacobian matrix of the equation Note ₇
+ ₇ T (T is a sampling period) as the updated value of the theta _7,
The values ₁ to ₆ are output to the servo amplifier 13 as target speeds to control the actuators of the pairs 1 to 6. In this case again reads the value of the displacement detector of the actuator, thereby the position and orientation of the hand 8 is calculated, a deviation between this and the target position and orientation is calculated, the deviation is again unless substantially zero, the _1-7 The calculation is repeated and the above operation is repeated, and the operation is ended when the deviation becomes almost zero.

〔The invention's effect〕

According to the present invention, in the control of a robot mechanism which has a hand at the end of six kinematic pairs and has six degrees of freedom, the robot mechanism is added without stopping even when the robot mechanism approaches or has a singular point. Only by changing the posture of the hand by one kinematic pair of the virtual model, the reference point of the hand can be moved in a desired manner along the designated path to smoothly pass the vicinity of the singular point and be driven, For example, there is an effect that welding work can be performed without any trouble.

[Brief description of drawings]

FIG. 1 is an explanatory view showing a virtual pair setting method of # 7 of an embodiment of a robot control method according to the present invention, FIG. 2 is an external view showing the same robot mechanism, and FIG. 3 is the same as FIG. FIG. 4 is an explanatory view showing the skeleton, FIG. 4 is an explanatory view showing the display method of the position and posture of the hand of FIG. 1, 3 similarly, FIG. 5 is a configuration diagram showing the robot system, and FIG. 6 is also FIG. It is a flow chart. 1 to 6 ... # 1 to # 6 rotating pair, 7 ... # 7 virtual rotating pair, 8 ... hand, 9 ... robot mechanism, 10 ... control unit, 11 ... arithmetic unit, 12 ... memory Device, 13 ... Servo amplifier.

Claims (1)

[Claims] 1. A method of controlling a robot mechanism having a hand at the end of six pairs and having six degrees of freedom, and seven degrees of freedom including one virtual pair whose axis passes through a reference point of the hand. Using the control model of the robot, the robot mechanism is usually controlled by regarding the virtual kinematic pair as a fixed one,
When the posture of the robot mechanism approaches a singular point, its movement changes greatly in the vicinity of the singular point, and one of command values out of two or more predetermined kinematic pairs is the final rotation angle of the kinematic pair. Each of the remaining six pairs is sampled by performing a coordinate transformation on the remaining six pairs including the virtual pair at each sampling by setting the value to drive in the direction to reach the target value. Of the hand of the robot mechanism by determining the control command value at time and controlling the six pairs of the robot mechanism based on the determined control command value at each sampling and the determined one motion command value of the kinematic pair. A method for controlling a robot, characterized in that a reference point is passed along a designated path in the vicinity of the singular point.