JPH035605B2 - - Google Patents

Description

[Detailed Description of the Invention] (1) Technical Field of the Invention The present invention relates to a trajectory control method for an articulated robot, and particularly to a trajectory control method for smoothly moving an articulated robot with 6 degrees of freedom in a Cartesian coordinate system. Regarding.

(2) Background of the technology Research into general-purpose robots is focused on how to make machines automatically perform actions similar to humans. Therefore, the arm of a general-purpose robot is controlled by a computer and has multiple joints. With so-called multi-jointed robots, it is not easy to intuitively know which joints should be moved and how much to move the hand to which position. Therefore, by knowing the amount of rotation of each joint, it is necessary to be able to calculate the position of each part of the arm and hand using a computer immediately, that is, in real time.
Generally, a method using a matrix, that is, a calculation method using a coordinate transformation matrix, is used.

Problems in moving a six-degree-of-freedom multi-jointed robot in which each joint has a rotational degree of freedom in an orthogonal coordinate system will be explained using the robot having the configuration shown in FIG. 1 as an example. Regarding coordinate transformation calculations used in robot trajectory control, for example, see ``Study on Artificial Hand Motion Control,'' Japan Society of Mechanical Engineers (C
(ed.), Vol. 45, No. 391 (March 1972), pages 314-322.

Joint angle vector Θ=(θ ₁ , θ ₂ ,
θ ₃ , θ ₄ , θ ₅ , θ ₆ ), the hand position/posture vector P=(x, y, z, α, β,
To find γ), a total coordinate transformation matrix between the coordinate systems fixed to each joint is used.

O ₀ −x ₀ y ₀ z ₀ in Figures 1a and b is the absolute fixed coordinate system set for the robot, O _i −X _i Y _i Z _i (i=1
~5) is the coordinate system fixed to each joint, O _h −x _h y _h z _h
is a coordinate system fixed to the hand.

Coordinate transformation matrix A from O _h −x _h y _h z _h to O ₀ −x ₀ y ₀ z ₀
is calculated as A=A ₁ A ₂ A ₃ A ₄ A ₅ A _h (1). . Here, A _i (i=1 to 5 and i=
h) is the transformation matrix from O _i −X _i Y _i Z _i to O _i-1 −X _i-1 Y _i-1 Z _i-1 (however, A _h is from O _h −x _h y _h z _h O ₅ −x ₅ y ₅ z _{5
( conversion matrix to} ^_ _{_} ^_ A ₁ = cosθ ₁ sinθ ₁ 0 0−sinθ ₁ cosθ ₁ 0 00 ₀ 1 00 0 l 1 1 A ₂ = cosθ ₁ 0 −sinθ ₂ 00 1 0 0 sinθ ₂ cosθ ₂ 0l ₂・cosθ ₂ 0 −l ₂・sinθ ₂ 1 A ₃ =θ ₃ 0 -sinθ ₃ 10 1 0 0sinθ ₃ 0 cosθ ₃ 0l ₃・cosθ ₃ 0 −l ₃・sinθ ₃ 1 A ₄ =1 0 0 00 cosθ ₄ sinθ ₄ 00 −sin ₄ cosθ ₄ 0l ₄ 0 0 1 A ₅ = cosθ ₅ 0−sinθ _{5 00} 1 0 0 sinθ ₅ 0 cosθ 5 0l ₅・cosθ ₅ 0 −l ₅・sinθ ₅ 1 A _h =1 0 0 00 cosθ ₆ sinθ ₆ 00 −sinθ ₆ cosθ ₆ 0l ₅ 0 0 1 ......(3)

Therefore, given the joint angle vector Θ, ^the position of the hand in the absolute coordinate system O ₀ −x ₀ y ₀ z ₀
= (x, y, z) is the coordinate system O _h fixed to the hand
If the position at -x _h y _h z _h is defined as X ^h = (x _h , y _h , z _h ), it is calculated by X ^O 1 = AX ^h 1 (4).

Moreover, as shown in FIG. 1b, the coordinate system O ₀ −
The direction cosine of each axis x _h , y _h , z _h of O _{h −x h} y _h z _h with respect to x ₀ y ₀ z 0 is h ₁ [= (h ₁₁ , h ₂₁ , h ₃₁ ) ^T ], h ₂ [=
h ₁₁ , h ₂₂ , h ₃₂ ) ^T ], h ₃ [=(h ₁₃ , h ₂₃ , h ₃₃ ) ^T ],
If the coordinates of the origin _{O h of the coordinate system O h −x h y h} z _h in the coordinate system O 0 −x ₀ y ₀ z ₀ are X ^h ₀ [= (X _h0 , Y _h0 , Z _h0 ) ^T ] , the matrix A in equation (1) can be set as A=h ₁₁ h ₁₁ h ₁₁ 0h ₁₂ h ₁₂ h ₁₂ 0h ₁₃ h ₁₃ h ₁₃ 0Xh ₀ Yh ₀ Zh ₀ 1 (5).

Therefore, the postures α, β, and γ of the hand at O ₀ −x ₀ y ₀ z ₀ are α=tan ⁻¹ (h ₂₁ /h ₁₁ ) β=−tan ⁻¹ (h ₃₁ /√ ₁₁ + ₂₁ ) γ=tan ^-1 (h ₃₂ /h ₃₃ ) ......calculated by (6).

When the joint angle vector Θ is given in this way, the position and posture vector P of the hand are calculated by finding the matrix A of equation (1).

Conversely, when P is given and the robot's hand is moved in an orthogonal coordinate system, Θ is found from the equality relationship in equation (5). Θ is θ ₁ = tan ^-1 [Y _h0 − (l ₅ + l ₆ ) sinαcosβ/X _h0 − (l ₅ +
l ₆ )cosαcosβ] θ ₂ =±cos ^-1 [R ² +l〓-(l ₃ +l ₄ ) ² /2l ₂ R] +φ where R ² = P ² −Q ² , φ=tan ^-1 (Q/ P) P={x _h0 −(l ₅ +l ₆ )cosαcosβ}/cosθ ₁ Q=−Z _h0 −(l ₅ +l ₆ )cosβ+l ₁ θ ₃ =tan ^-1 [Q−l ₂・sinθ ₂ /P− l ₂・cosθ ₂ ]−θ
₂ θ ₄ = tan ^-1 [cosβ・sin(α−θ ₁ )/sinβ・cos(
θ ₂ + θ ₃ )−cosβ・cos(α−θ ₁ )・sin(θ ₂ +θ ₃ )
] θ ₅ = ±cos ^-1 [sinβ・sin(θ ₂ +θ ₃ )+cosβ・cos
(α−θ ₁ ) cos (θ ₂ + θ ₃ )] θ ₅ = tan ^-1 [cos (θ ₂ + θ ₃ ) sin _{θ 4} / sin (θ ₂ + θ ₃
)・sinθ ₅ −cos (θ ₂ +θ ₃ )cosθ ₄ cosθ ₅ ]+γ……
…Calculated by (7). However, the solution for Θ is limited to -π<θ _i ≦π (i=1 to 6), and there are eight modes.

For example, cases in which the value of θ is different in the same state are shown in FIGS. 2a, b, and c. That is, there are eight solutions of Θ to P, and the mapping P→Θ has a one-to-eight correspondence. The type of solution must be determined depending on the work the robot is to perform, but in order to change the type of solution, the robot must pass through a singular point.

The relationship between the minute change SP when the position/posture vector P of the hand changes to the next state and the corresponding minute change δΘ in the joint angle vector can be expressed using the Jacobian matrix J as follows: δP→=JδΘ→ (8) It can be expressed as Here, J is a matrix obtained by differentiating the static transformation formula P→=f(Θ→) (9) of equation (7), and J _ij =∂f _i / ∂Θ _j (i, j=1 to 6) ......(10) This is a coefficient of the dynamic conversion equation (8). Therefore, when δP→ is given, that is, the variation (difference) on the orthogonal coordinate system is given,
To find the angular variation δΘ→, inverse the Jacobian matrix J
J ^-1 is required, but in order for this inverse matrix to exist, the determinant detJ≠0 must hold true.

A singular point is an angular relationship such that detJ=0, and its distribution is determined by the mechanism of the robot. In the case of a robot having the configuration shown in FIG. 1, the following will occur.

The state of the robot exhibiting a singular point satisfying θ ₂ +θ ₃ /2=nπ θ ₃ =nπ (11) θ ₅ =nπ is as shown in FIG.

A singular point is a point where the types of solutions change, and the movement of Θ→ becomes discontinuous after this point. Furthermore, even in the vicinity of the singularity, Θ→ changes greatly due to a small change in P→.

Therefore, there is a limit to the range in which the robot can continuously move in the orthogonal coordinate system.

(3) Problems with conventional technology Conventionally, when moving a robot over a wide range,
Instead of using only the orthogonal coordinate system, methods are used in which the orthogonal coordinate system and other coordinate systems are used together, such as changing the type of solution by using a coordinate system whose coordinate value is the component of Θ→. .

(4) Purpose of the Invention The present invention aims to develop a robot that can move globally on an orthogonal coordinate system, that is, continuously in the entire space, by checking the existence of singular points specific to a 6-degree-of-freedom articulated robot. The purpose of the present invention is to provide a trajectory control method.

(5) Structure of the Invention The present invention checks whether the type of solution of the calculated value has changed, that is, whether it has become close to a singular point, based on the difference between the calculated target joint angle of the robot and the current joint angle. , we have provided a robot trajectory control method that calculates an angular velocity that allows the robot to smoothly pass through the singularity by correcting the target joint angles when the type of solution changes, that is, when the singularity gets closer. be.

(6) Embodiments of the Invention The control system of the present invention will be explained with reference to FIG. .
The configuration of the controlled robot is shown in FIG.

The arithmetic processing unit 1 calculates the position at each sample time point k+1 on the robot's indicated path in the orthogonal coordinate system.
The joint angle Θ K+ ₁ is obtained from the posture vector P _K+1 using equation (7), and this Θ _K+1 is combined with the current joint angle Θ' _K obtained from the encoder position counter in the servo mechanism 3. The angular velocity θ〓 _K = (θ _K+1 −θ′ _K )/Δt (Δt: sampling period) is generated from the equation and output to the velocity function generator 2.

The servo mechanism 3 is arranged at each joint of the robot and follows the output of the velocity function generator 2.

In this embodiment, singular points are checked only at the points θ ₂ +θ ₃ /2=nπ and θ ₅ =nπ, and when θ ₃ =nπ
The point is set in one of the solution types without being checked, that is, it is set in one of the regions divided by the straight line connecting θ ₂ to θ ₃ . The reason for this is that, as shown in FIG. 3b, even if the third joint can only rotate in one area, it does not affect the movement range of the robot's hand.

When checking the two singular points mentioned above, when the robot passes through nπ, θ ₁ shifts when θ ₂ +θ ₃ /2 passes through nπ.

When θ ₅ passes through nπ, θ ₄ and θ ₆ are shifted by π.

Take advantage of that. This is derived from conversion equation (7). When the maximum angular velocity of the joint determined by the upper limit of the mechanical operation speed of the actuator, etc. is θ〓max, the change in θ ₁ or θ ₄ (θ ₆ ), that is, the next The difference in angle between the state and the current state |θ _K+1 −θ′ _K | is larger than the difference between the angle π at the singular point and the physical limit angle θ〓max・Δt that can be exceeded by the actual actuator, etc. In other words, if |θ _K+1 −θ′ _K |>π−θ〓max・Δt is regarded as a singularity, then θ ₁ →θ ₁ ±π θ ₂ →π−θ ₂ θ ₄ →θ ₄ ±π θ ₅ →−θ ₅ θ ₆ →θ ₆ ±π The discontinuous change at the angle singularity is avoided before the singularity is reached.
The reason why we set the discriminant value to π−θ〓max・Δt instead of θ〓max・Δt is to take into account changes in the vicinity of the singularity, and to avoid discontinuities at the above singularity in the vicinity of the singularity. This is because. In order to remove the discontinuity at the singular point, it is sufficient to change θ ₁ to _{θ 6} before the singular point, as described above, but next, how should the actuator etc. be driven near the singular point? Let's talk about.

Near the singularity, changes in θ ₁ and θ ₄ (θ ₆ ) change significantly, even if not discontinuously, so the angular velocity that moves in one sample period Δt is calculated as the maximum angular velocity that the actuator can drive θ〓max That's all there is to it. To deal with this, near the singularity, the change in θ ₁ or θ ₄ (θ ₆ ) becomes π−θ〓max・Δt＞｜θ _K+1 −θ′ _K |≧θ〓max・Δt Here, instead of the angular velocity θ〓 _K obtained from the calculated value, θ ₁ or θ ₄ (θ ₆ ) is moved at the actual maximum angular velocity θ〓max of the actuator (θ〓 _K > θ〓max), and the sampling period is increased accordingly. By setting |θ〓 _K /θ〓max|·Δt so as to increase the distance, it is possible to make the movement distance given as a calculated value the same as when the actuator is moved by θ〓max. In this case, θ _i other than θ ₁ or θ ₄ (θ ₆ )
For, the angular velocity obtained from the calculated value |
θ〓max/ _θ〓K | Of course, it can be multiplied.

(7) Effects of the invention As described above, in the present invention, by checking the singular point or simultaneously checking the vicinity of the singular point, the joint angle of each joint can be calculated from the given orthogonal coordinates. Points and their vicinity can be smoothly transformed, and the robot can continuously pass through singular points unique to the mechanism of a 6-degree-of-freedom articulated robot and their vicinity, making it possible for the robot to move globally on the orthogonal coordinate system. can.

[Brief explanation of the drawing]

Figures 1a and b are diagrams showing the configuration of the 6-degree-of-freedom articulated robot applied to the invention and the position and orientation of the hand expressed in Euler angles, and Figures 2a, b, and c
3A, 3B, 3C are diagrams showing joint states at three types of singular points, and FIG. 4 is a configuration diagram of a speed control system. 1... Arithmetic processing unit, 2... Speed function generation unit,
3... Servo mechanism, O ₀ -x ₀ y ₀ z ₀ ... Absolutely fixed coordinate system of the robot, O _i -x _i y _i z _i ... Coordinate system fixed to each joint (i = 1 to 5) , O _h −x _h y _h z _h ...Coordinate system fixed to the hand, l _i ...Length of the i-th arm,
θ _i ...Rotation angle of the i-th joint, P...O ₀ −
6-dimensional vector representing the position and orientation of the hand _on x ₀ y ₀ z 0, h ₁ , h ₂ , h ₃ ... O _h − _{x h} y _h z _h 's x, y, and z axes O ₀ − Direction cosine for x ₀ y ₀ z ₀ .

Claims (1)

[Scope of Claims] 1. Interpolation points to be passed sequentially after a predetermined time from a prespecified starting point in a Cartesian coordinate system defined on the base of an articulated robot are calculated, and the joints of the robot at each of the interpolation points are calculated. The angle is calculated by a coordinate conversion operation from the orthogonal coordinate system to a joint coordinate system specific to the robot's form, and the joint angle is calculated from the difference between the joint angle at the interpolation point and the joint angle at the current moment during movement. In a robot trajectory control method that calculates the driving speed and moves the robot tip to an end point, when the difference in the joint angles becomes larger than a predetermined amount,
Assuming that the joint angle is approaching a singular point where the joint angle changes discontinuously, the joint angle at the interpolation point is changed to another solution of the coordinate transformation that has a small amount of change and allows the tip to take the same position and posture. A robot trajectory control method characterized by the following. 2 Calculate the interpolation points that the robot should pass through after a predetermined period of time from the starting point specified in advance in the orthogonal coordinate system defined on the base of the articulated robot, and calculate the joint angles of the robot at each of the interpolation points in the orthogonal coordinate system. A calculation that calculates the speed at which the joint is driven until it reaches the interpolation point from the difference between the joint angle at the interpolation point and the joint angle at the current moment of movement. In a robot trajectory control method in which the tip of the robot is moved to the end point, when the difference in the joint angles becomes larger than a predetermined amount,
Assuming that we have approached a singular point where the joint angle changes discontinuously, we change the joint angle at the interpolation point to another solution of the above coordinate transformation that has a small amount of change and allows the tip to take the same position and orientation. , furthermore, when the difference between the joint angles is smaller than the predetermined amount, but the determined joint drive speed becomes larger than the mechanically drivable maximum joint angular speed, it is considered that the joint angle has approached the vicinity of the singular point, A trajectory control method for a robot, characterized in that the joint driving speed to the interpolation point is set to the maximum joint angular speed, and the time for driving at that speed is reset to a length corresponding to the ratio of speed reduction.