JPH0553590B2 - - Google Patents

Description

[Detailed description of the invention]

[Industrial Field of Application] The present invention relates to a method for controlling a six-axis articulated robot having three basic axes and three wrist axes offset with respect to the robot arm. [Prior Art] A six-axis articulated robot consisting of three basic axes and three wrist axes is composed of the following six axes, as shown in FIG. (1) Provided on the top of the leg 11 fixed to the ground,
A torso 12 having degrees of freedom around the perpendicular (Z _p axis). (2) Z ₁ provided at the tip of the body 12 and perpendicular to the Z _p axis
The lower arm 13 has a degree of freedom around the axis. (3) Provided at the tip of the lower arm 13 and parallel to the _Z1 axis.
Upper arm 14 having degrees of freedom around _two Z axes. (4) Provided at the tip of the upper arm 14 and perpendicular to the _Z2 axis.
Wrist 15 with degrees of freedom around Z ₃ axes. (5) Provided at the tip of the wrist 15 and perpendicular to the _Z3 axis.
Z Wrist 16 with degrees of freedom around ₄ axes. (6) Provided at the tip of the wrist 16 and perpendicular to the _Z4 axis.
Wrist 47 having degrees of freedom around the _Z5 axis. As described above, each joint has one degree of freedom, making this robot a total of six degrees of freedom. In such a six-axis articulated robot, not only the position but also the posture of the wrist 17 can be controlled, as reported by Professor RP Paul of Badieu University in the United States.
He wrote “ROBOT MANIPULATORS:
MATHEMATICS, PROGRAMING AND
He is also familiar with "CONTROL" (1981). This book is famous as the world's first systematic and academic description of computer control for robots, and is known in Japan as ``Robots''.
Published as ``Manibulator'' (translated by Tsuneo Yoshikawa, Corona Publishing). Now, according to these conventional techniques, by fixing each coordinate system for each joint in the following procedure, the posture of the wrist 17 at the sixth joint can be determined. Z _o-1 (n=1 to 6) Rotate by an angle θ _o around the axis. Translate by a distance d _o along the Z _o-1 axis. Translate by a length a _o along the X _o-1 axis after rotation, that is, the X _o axis. Rotate around the X _o axis by a torsion angle α _o . That is, the robot is rotated by an angle θ ₁ around the Z _p axis, then translated by a distance d ₁ along the Z _p axis,
Next, the robot is moved by a length a _p along the rotated X _p axis, that is, X ₁ o'clock, and rotated by a torsion angle α ₁ around the X ₁ axis. As a result, lower arm 1
The coordinate system X ₁ Y ₁ Z ₁ of No. 3 is fixed. From then on, the coordinate systems X ₂ Y ₂ Z ₂ , X ₃ Y ₃ Z ₃ , X 4 Y of the upper arm 14, wrists 15, 16, and ₁₇ are fixed. ₄ Z ₄ and X ₅ Y ₅ Z ₅ are fixed, and the posture of the wrist 17 is established. The above operations are expressed as the product of four homogeneous transformations that relate the coordinates of link _o to link _o-1 (this is called the A matrix). That is, A _o = ROT (Z, θ _o ) trans (0, 0, d _o ) trans (
a _o 0, 0) Rot(x, α _o ) = cosθ _o sin _o θ 0 0−sinθ _o cosθ _o 0 00 0 1 00 0 0 11 0 0 a 0 1 0 0 0 0 1 d 0 0 0 11 0 0 00 cosα _o sinα _o 00 −sinα _o cosα _o 00 0 0 1. Therefore, if the tip of the wrist is T ₆ , then T ₆ = Z・A ₁ A ₂ A ₃ A ₄ A ₅ A ₆ However, Z = 1 0 0 P _x 0 1 0 P _y 0 0 1 P _z 0001 , expressed as the direction cosine (n _x , n _y , n _z ) of the fixed coordinate axes (X _O , Y _O , Z _p ), T ₆ = n _x n _y n _z 00 _x 0 _y 0 _z 0a _x a _y a _z 0P _x P _y P _z 1 . That is, specifying each element of T ₆ , the link angle θ ₁
The conventional calculation method is to find ~θ ₆ . In this case, the calculation formula for calculating the link angle θ ₁ is based on T6, as shown in Figure _6a , where the wrist is not offset (the origin of the coordinate system of the wrist is on the x-axis of the coordinate system of the upper arm). ) in the case of a robot, −sinθ ₁・P _x + cosθ ₁・P _y = d ₂ , which can be solved by trigonometric substitution as follows. FIG. 8 is a diagram showing the relationship between the link angle θ ₁ , the positions Px and Py, and the distance d ₂ . Px=Mcosθ ₀ and Py=Msinθ ₀ , which is −
Substituting into sinθ ₁・Px＋cosθ ₁・Py=d ₂ , −sinθ ₁・Mcosθ ₀ +cosθ ₁・Msinθ ₀ = d ₂ Msin(θ ₀ −θ ₁ )=d ₂ sin(θ ₀ −θ ₁ )=d ₂ / If we set M θ ₀ −θ ₁ =ω, then tanω=d ₂ /K θ ₁ =θ ₀ −ω tanθ ₁ =tan(θ ₁ −ω)=tanθ ₀ −tanω/1+tanθ ₀
・tanω = Py/Px−d ₂ /K/1+Py/Px・d ₂ /K=PyK−Pxd ₂
/PxK−Pyd ₂ θ ₁ can be found. [Problem to be solved by the invention] However, as shown in FIG. 6b, the wrist is offset (the origin of the coordinate system of the wrist is
In the case of a robot (which is not on the x-axis of the upper arm coordinate system, that is, the axis of the wrist moves parallel to the axis of the arm), -sinθ ₁・P _x cosθ ₁・P _y = d ₂ + cosθ ₄・It becomes _d5 . In this equation, the distance corresponding to the offset of the wrist 16 is added to the right side of the equation for calculating the link angle θ ₁ mentioned above.
It takes cos θ ₄ ·d ₅ into account, and since the link angles θ ₁ and θ ₄ exist as variables, it cannot be solved by trigonometric substitution. Therefore, in the past, it was only possible to control robots whose wrists were not offset. Incidentally, Japanese Patent Laid-Open No. 60-48291 proposes a technique in which the structure of the wrist portion is devised to prevent offset. However, as shown in Figure 7, a wrist that is not offset has the fatal drawback that its operating range is limited by itself, and the structure is inevitably complicated, making the wrist part It has various drawbacks, such as being heavier than necessary. SUMMARY OF THE INVENTION An object of the present invention is to provide a method for controlling a robot having an offset wrist. [Means for Solving the Problems] The present invention provides a robot control method which, when controlling the position and posture of the wrist, includes a first step of calculating link angles of three basic axes from position data, and a posture. The second step calculates the link angle of the wrist from the data and link angles of the basic three axes obtained in the first step.
The command value for each link angle can be obtained by repeating the following steps, and the third step of recalculating the link angles of the basic three axes from the position data and the wrist link angle obtained in the second step. Features. [Operation] The present invention was made based on the knowledge that the reason why calculations could not be performed using the conventional technology was that calculations were attempted in one operation, and the above series of calculation processes were repeated. By doing this, the command value of each link angle is obtained.If this process is repeated multiple times, the solution will be infinitely approached. The number of repetitions is determined by the required accuracy and the calculation processing speed of the robot control device, but the solution is almost converged within eight repetitions. In particular, it has been confirmed through experiments that 2 to 3 times is sufficient in fields that do not require extreme precision. [Example] Hereinafter, an example of the present invention will be described with reference to the drawings. FIG. 1 is a flowchart showing an embodiment of the robot control method of the present invention, and FIG. 2 is a projection diagram of the fixed coordinate axes (X _p , Y _p , Z _p ) of the robot onto the X _p -Y _p plane. FIG. 3 is a projection view of the robot onto the M _p -Z _p plane. Note that the robot to be controlled is shown in FIG. First, the position data Px, Py, Pz (indicating a fixed coordinate system X _p Y _p Z _p ) of the control point P of the robot on the wrist 17 of the robot are given (step 1). 17 is the basic 3 axes (torso 12, lower arm 13, upper arm 14), assuming that it does not move.
The link angles θ ₁ , θ ₂ , and θ ₃ are calculated. (Step 2). This calculation is performed in the following steps. As shown in FIG. 2, P _x and P _y are determined by projecting the robot onto the X _p -Y _p plane of fixed coordinate axes. Here, FIG. 2 is a projection of the axis from the robot's first axis rotation center (Z _p axis) to the robot's control point P onto the robot's fixed coordinates X _p Y _p . Then, the link angle θ ₁ is found by θ ₁ =tan ⁻¹ (P _y /P _x ). Next, the M _p -Z _p plane is assumed, with the spectrum up to the control point P being M _p . Then, as shown in Fig. 3, the robot is projected onto the M _p -Z _p plane, that is, the X _p -Z _p2 plane, and Pz',
Find Pz″, M′, M″. Then, the link angle θ ₂ and the link angle θ ₃ are found as follows: θ ₂ = tan ^-1 (Pz''/M) θ ₃ = tan ^-1 (Pz'/M). Here, the links θ ₁ , θ ₂ , The specific calculation formula for θ ₃ can be found based on geometric relationships, but its derivation is extremely complicated, so we will not explain it here. With the above, the link angles θ ₁ , θ ₂ , and θ ₃ have been found. However, these angles θ ₁ θ ₂ and θ ₃ are solutions assuming that the wrist is fixed and are only provisional solutions.Next, posture data n _x , n _y , n _z , O _x , O _y , O _z ,
Input a _x , a _y , a _z (step 3), and use these attitude data and the previously determined link angles θ ₁ , θ ₂ , θ ₃ (temporary solutions).
Calculate the link angles θ ₄ , θ ₅ , and θ ₆ at the wrist.
This uses a calculation method for determining the wrist portion of the conventional posture control method. In this case, position data is not considered. From the above T ₆ = A ₁ A ₂ A ₃ A ₄ A ₅ A ₆ , A ₃ ^-1 A ₂ ^-1 A ₁ ^-1 T ₆ = A ₄ A ₅ A ₆

[Table] However, S, L, U, R, B, and T are each
Z ₁ , Z ₂ , ..., Z _6- axis SS = SiN (θ ₁ ), S _LU = SiN (θ ₂ + θ ₃ ), S _u = SiNθ ₃ CS = COS (θ ₁ ), C _LU = COS (θ ₂ +θ ₃ ), _Cu = COSθ ₃ SR = SiN (θ ₄ ), SB = SiN (θ ₅ ), ST = SiN (θ ₆ ) CR = COS (θ ₄ ), CB = COS (θ ₅ ), CT = COS (θ ₆ ) a ₂ = L-axis arm length d ₂ = Distance between S-axis rotation center and R-axis rotation center d ₄ = U-axis arm length d ₅ = Distance between R-axis rotation center and T-axis rotation center (1) From the formula, −CR・SB=a _x CS・S _LU +a _y SS・S _LU −a _z C _LU ……(2) −SR・SB=−a _x SS＋a _y CS ……(3) (2), ( From formula 3) θ ₄ = tan ^-1 {a _x SS−a _y CS／a _z C _LU −S _LU (a _x CS+a _y SS)
} Also, A ₄ ^-1 A ₃ ^-1 A ₂ ^-1 A ₁ ^-1 T ₆ =A ₅ A ₆

〔Effect of the invention〕

As explained above, the present invention recalculates the link angles of the three basic axes so that even a robot with a mechanism in which the wrist of the robot is offset from the robot arm is the same as a robot with a mechanism without offset. This has the great effect of controlling posture.

[Brief explanation of the drawing]

FIG. 1 is a flowchart showing an embodiment of the robot control method of the present invention, and FIG. 2 is a projection diagram of the robot's fixed coordinate axes (X _p Y _p Z _p ) onto the X _p -Y _p plane.
Fig. 3 is a projection view of the robot onto the _Mp - _Zp plane, Fig. 4 is a block diagram of the control section of the robot to which the control method of the present invention is applied, and Fig. 5 is an external view of the 6-axis articulated robot. , Figures 6a and b are external views of the wrist that is not offset and the wrist that is offset, Figure 7 is a diagram showing how the operating range of the wrist that is not offset is restricted, and Figure 8 is is a diagram showing the relationship between link angle θ ₁ , positions Px and Py, and distance d ₂ . 1-7...step, 11...leg, 12...
Torso, 13... lower arm, 14... upper arm, 15, 16,
17...Wrist.

Claims (1)

[Claims] 1. When controlling the position and posture of the wrist in a 6-axis articulated robot that has three basic axes and three wrist axes that are offset from the robot arm, the basic method is based on position data. The first step is to calculate the link angles of the three axes, and the second step is to calculate the link angles of the wrist from the posture data and the link angles of the basic three axes obtained in the first step.
The command value for each link angle can be obtained by repeating the following steps and the third step of recalculating the link angles of the basic three axes from the position data and the wrist link angle obtained in the second step. Characteristic robot control method.