JPH0584677A - Manipulator controller - Google Patents

Abstract

(57) [Summary] [Purpose] To obtain a control device capable of shifting to any target position / posture without abrupt angle change of each joint. [Arrangement] A coordinate transformation device 11 for calculating a joint angle that realizes a target position of an intersection of three joint axes intersecting at one point, a roll axis coordinate transformation device 13 on the base side of the tip three axes, and two of the tip three axes.
A roll axis target angle correction device 14 for correcting the target angle of the roll axis on the root side so as to approach the reference angle when the axes of the two roll axes are closer than a certain reference, and the roll shaft on the root side. Tip joint axis target angle calculation unit 15 that calculates the target joint angles of the two tip axes other than the roll axis corresponding to the corrected target angles of the above, and the joint axis control device 16 that controls the rotation of each joint axis to the target joint angle. Have and.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a manipulator having at least 6 joints and constructed so that the three joint axes at the ends intersect at one point.

[0002]

2. Description of the Related Art Generally, a manipulator having six joint axes, and the three joints at the tip end having a roll-pitch-roll configuration such that the joint axes intersect at one point is used for industrial purposes in a factory. It is mainly used for robots.

FIG. 7 is a diagram showing an example of a manipulator in which the above-described 6-axis multi-joint type, the three axes at the tip end have a roll-pitch-roll configuration, and the three axes intersect at one point. The base end of the first link 2 is rotatably connected about a horizontal axis (first axis) and a vertical axis (second axis), and the second link 3 is attached to the tip of the first link 2. Of which the base end is orthogonal to the longitudinal axis of the second link 3 (third axis) and the longitudinal axis of the second link 3 (fourth axis)
Are rotatably connected to each other. And
A hand portion 4 is connected to the tip of the second link 3 so as to be rotatable around an axis line (fifth axis) orthogonal to the fourth axis and an axis line (sixth axis) orthogonal to the fifth axis. ing.

Therefore, such a manipulator has 6
Since it has joints, it is possible to realize an arbitrary target position and posture in a three-dimensional space within the motion range by appropriately rotating each joint.

As the target position and posture, the manipulator tip, that is, the position and posture of the hand portion 4, or the tip 3 is used.
The position of the intersection of the joint axes (the fourth axis, the fifth axis, the sixth axis) and the tip posture may be designated from the outside. Then, in this manipulator control device, a control system that controls the coordinate axes of the normally specified target position / orientation to each joint angle and then controls each joint axis to realize those target joint angles is constructed. However, particularly in the latter case of giving the target position / posture, the manipulator is treated as a three-dimensional positioning mechanism that realizes the target position with the three axes on the base side and the target posture with the three axes at the tip. The coordinate conversion formula from the target position / posture to the target joint angle can be easily derived.

[0006]

However, as described above, when the three joint axes of the distal end have the roll-pitch-roll configuration, any posture can be realized, but there are the following problems.

That is, first, when the postures of the two roll axes coincide with each other, that is, when the axes of the link 3 and the hand coincide with each other in FIG. 7, the posture change in the yaw direction becomes impossible. There may be a peculiar attitude. Further, when the two roll axes do not completely coincide with each other, it is possible to change the posture in the yaw direction, but in order to realize a subtle posture change in the yaw direction, a sharp angle change must be given to the two roll axes. In some cases, it may be impossible to controllably achieve accurate tracking of such a target joint angle.

When the movable range of the roll axis is about ± 90 degrees, the target joint angle of the roll axis may change from +90 degrees to -90 degrees or from -90 degrees to +90 degrees depending on the target tip posture. There are discontinuities that change discontinuously. It is possible to achieve an arbitrary target posture even if the roll axis movable range is ± 90 degrees, but when the posture to be realized by the tip 3 axes is expressed by roll, pitch, and roll, the pitch angle is 0 degree. The target joint angle of the roll axis changes discontinuously before and after.

Any of these peculiar postures or discontinuities are not preferable because they cause a sudden angle change during the transition to the target joint angle. In actual operation, these peculiar postures / discontinuities are used. This is avoided by making the robot move so that it does not move, but when the robot's movement becomes large, it is quite necessary to determine the robot's movement that does not approach such a peculiar posture / discontinuous area on the way. However, there is a problem that trial and error must be repeated in some cases.

In recent years, an offline teaching system that uses a computer to teach movements of a robot has become widespread, and the number of those partially having a trajectory generation function for automatically calculating the movement trajectory of a robot is increasing. When realizing automatic calculation, it is not impossible to reliably create a trajectory that does not approach the singular posture / discontinuity region, but even if this is not done, the load of trajectory generation that requires a large amount of calculation will be further increased. Therefore, there is a problem that the number of robots increases, which makes it difficult to automate the teaching of the robot.

In view of such a point, the present invention allows a target joint of each joint to be moved to any target position / posture without abrupt angle change of the joint within the movable range of the manipulator. An object of the present invention is to obtain a control device for a manipulator provided with a coordinate conversion device that calculates an angle.

[0012]

A first aspect of the present invention is a manipulator having at least 6 joints, wherein the 3 joints at the distal ends have a roll-pitch-roll configuration such that their joint axes intersect at a single point. A manipulator control device for controlling a three-dimensional position of an intersection of joint axes and a tip posture to target values, a coordinate conversion device for calculating a joint angle that realizes a target position of an intersection of three joint axes intersecting at one point, and a tip 3
When the roll axis coordinate conversion device on the base side of the axis and the axis lines of the two roll axes of the three tip axes are close to each other by a certain reference or more,
A roll axis target angle correction device that corrects the target angle of the roll axis on the root side so as to approach the reference angle according to the value of the joint angle of the pitch axis that is the difference between the two roll axes in the axial direction, and the root side. The target joint angle target angle calculation unit for calculating the target joint angles of the two front end axes other than the roll shaft so as to realize the target posture of the manipulator front end portion corresponding to the corrected target angle of the roll axis And a joint axis control device for controlling rotation to a target joint angle.

A second aspect of the present invention is a control device for a manipulator, which has seven joints, and the three joints at the tips have a roll-pitch-roll configuration so that their joint axes intersect at one point. The coordinate conversion device that calculates the joint angle that realizes the target position of the intersecting joint axis intersection is limited to the case where the posture realized by the three tip joints is expressed in roll, pitch, and yaw, and the pitch angle is in the vicinity of O degrees. Then, the target joint angle is calculated such that the link tip direction and the target tip direction at the intersection point of the three tip joint axes are as equal as possible while realizing the target position.

[0014]

According to the first aspect of the invention, when the axes of the two roll axes of the three tip axes are close to each other by a reference amount or more, the value of the joint angle of the pitch axis which is the difference in the axial direction of the two roll axes. According to the above, the target angle of the roll axis on the root side is corrected so as to approach the reference angle, and corresponding to the corrected target angle of the roll axis, the other two target directions other than the yaw direction are realized. In order to calculate the target joint angle,
Even if the target posture changes slightly in the yaw direction when the axes of the two roll axes match, even if the target posture changes slightly in the yaw direction, the target joint angle of the roll axis on the root side of the three tip axes rapidly increases from the vicinity of the reference angle. Does not change, the value of the target joint angle of the distal two axes does not change rapidly with respect to the change of the value of the target posture, and the angle of the distal three axes does not change rapidly.

According to the second invention, when the target pitch angle becomes a certain reference angle, for example, 10 degrees or less,
By using the redundant degrees of freedom of the manipulator, while achieving the target position, the tip-direction posture at the intersection of the three tip joint axes is made to coincide with the tip target posture, so that all three tip axes are used to achieve the target posture. There is no need to do it. In other words, the tip posture can be realized even if the joint angle of the root-side roll axis in the three tip axes is kept constant at the reference angle.

Further, by combining the first invention and the second invention, the target joint angle of the roll shaft is not discontinuously changed even in the manipulator having the movable range of the roll shaft of about ± 90 degrees. , Any desired posture can be realized.

[0017]

FIG. 1 is a block diagram showing a control device for the manipulator shown in FIG. 7. When the tip of the manipulator, that is, the target position of the hand portion is given from the outside,
The target position at the intersection of the three joint axes of the manipulator is 1
The coordinates are converted into the target joint angles of the first to third axes based on the target position and the current joint angle.

On the other hand, the target posture of the tip of the manipulator is the 4-axis coordinate transformation device 13 of the tip 3 joint target angle transformation device 12.
And the target joint angle of the fourth axis for accurately realizing the target posture analytically is calculated based on the current joint angle and the like. Then, the calculation result is input to the 4-axis target angle correction unit 14.

The 4-axis target angle correction unit 14 determines whether the current joint angle of the fifth axis is below a predetermined reference value Δ, for example, 3 degrees or less, and the absolute value of the joint angle of the fifth axis is above the reference value. In the following cases, the target joint angle of the fourth axis already calculated by the four-axis coordinate conversion device 13 is output as it is.

On the other hand, when the absolute value of the joint angle of the fifth axis is less than the reference value Δ, the value of the target joint angle of the fourth axis calculated by the four-axis coordinate conversion device 13 is calculated according to the following formula. Will be fixed.

Θ _dn4 = θ _d4 × g _ain ² (1) gain = sin (| θ ₅ | × π / (Δ · 2)) θ _d4 : Fourth axis target joint angle calculated by the four-axis coordinate conversion device θ ₅ : Joint angle of the 5th axis Δ: Reference angle for determining whether or not to correct by the 4th axis target angle correction section θ _dn4 : _Modified 4th axis target angle that is the output of the 4th axis target angle correction section (1) According to the equation, when the absolute value of the joint angle of the 5th axis is near the reference value Δ, the target angle of the 4th axis is almost equal to the value calculated by the target posture → the 4th axis coordinate conversion device 3, and is a peculiar posture θ.
The value of the 4-axis target angle approaches 0 degrees as it approaches ₅ = 0 degrees. In the vicinity of θ ₅ = 0 degree, the value of θ _dn4 becomes almost 0 degree regardless of the value of θ _{d4 in} the equation (1). Therefore, even if the value of the yaw angle of the target posture fluctuates a little, the value of θ _d4 becomes The value of θ _dn4 hardly changes even if it changes abruptly. In addition, the sensitivity of the yaw angle of the tip posture to the angle change of the four axes becomes small in the vicinity of the singular posture, and in particular, in the peculiar posture of θ ₅ = 0 degree, the yaw angle becomes 0 degree regardless of the angle of the four axes. It will not change at all. Therefore, even if the value of the four-axis target angle is corrected as in the equation (1), the change in the tip posture is small.

The fourth axis target joint angle θ _d4 calculated by the four-axis coordinate conversion device 13 or the corrected correction value θ _dn4.
Are input to the 5th and 6th axis target joint angle calculation unit 15, where the 5th axis and the 6th axis are analytically analyzed based on the above input values, the remaining roll pitch values of the target postures, the respective joint angle values of the manipulator, and the like. The target joint angle of the fourth axis and the target joint angle of the first to third axes calculated by the first to third axis coordinate conversion device 11, and the first to sixth axis control device 1
6, and controls each joint so that the joint angle of each joint follows the target angle.

In this way, all the joints of the manipulator do not suddenly change, and the target position / posture can be almost achieved.

2, 3 and 4 show the fourth and fifth axes accurately and analytically based on the desired joint angle changes and the desired postures of the fourth, fifth and sixth axes generated by the present invention. , Is a comparison of the results of calculating the target joint angle of the sixth axis. That is, each of the above figures is in the posture of the manipulator tip target,
When changing only the pitch direction attitude from -10 ° to 10 ° while keeping the roll and yaw attitudes constant,
It is the figure which compared the value A of the joint target angle calculated by this invention, and the value B calculated by the usual analytical calculation.

FIG. 5 shows changes in the posture of the tip in the yaw direction realized by the target joint angles of the fourth, fifth and sixth axes calculated by the two methods. It is assumed that the manipulator has 6-axis articulation and the 3 axes at the tip end have a roll-pitch-roll configuration and intersect at one point. The movable range of the 4th and 6th axes is ± 180 degrees.

The calculation conditions are 1, 2, 3 of the manipulator.
Axial joint angle is constant at 0 degree. The angle of the roll axis in the target posture is fixed at 0 degrees and yaw = 1 degree, and the pitch is changed from -10 degrees to 1
It is changed to 0 degrees.

As can be seen from the figure, although an error occurs in the yaw direction when the pitch is near 0 degrees, the amount of change in the angles of the fourth axis and the sixth axis is considerably smaller than that calculated analytically. I can see it. If the target angle change of the joint is rapid, the actual manipulator movement may not be able to accurately follow the change in the target angle depending on the actuator performance of the manipulator.Therefore, depending on the actuator controller performance, the actual tip position of the manipulator may change. The error from the target value of 1 may be smaller in the present invention than in the case of being calculated analytically. In addition, the manipulator has 7 or more joints, and the tip, 3 joints are roll-pitch-roll configuration, and the target position is realized by 4 joints on the root side.
In the case where the target posture is realized by the three tip joints, the tip yaw direction posture near the peculiar posture is realized by the four joints on the root side, so that it is possible to eliminate the error in the tip posture.

FIG. 7 is a block diagram showing a manipulator control device of the second invention, which is applied to a manipulator having seven joints.

In FIG. 7, the target position of the intersection of the three joint axes of the tip of the manipulator, which is given from the outside, is input to the coordinate conversion device 20 for the first to fourth axes, and the target posture of the tip of the manipulator is the target angle conversion device for the three tip joints. 21 is input.

In the 1- to 4-axis coordinate conversion device 20, first, the posture realized by only the three distal end joints is calculated from the current joint angles of the distal end three joint axes, and the value of the pitch direction angle is a reference value Δ (eg, If it is less than 10 degrees), when the coordinate conversion is performed, by utilizing the fact that only the target position is realized, the degree of freedom remains in the joint angles of the four joints, and the link is established at the intersection of the tip three joint axes while satisfying the target position. The target joint angle is calculated so that the tip direction posture and the target tip posture are as equal as possible.

That is, the calculation in the 1- to 4-axis coordinate conversion device 20 is performed as follows.

Now, where x is the position of the intersection of the three axes of the manipulator and θ is the joint axis vector, the relationship between x and θ can be expressed as follows.

X = f (θ) = (f1, f2, f3) ^t (2) Then, the following relationship is established between the small change Δx in x and the small change Δθ in the joint angle.

[0034]

[Equation 1] J (θ) is called the Jacobian matrix. Δx in equation (3)
By reversing the relationship between Δθ and Δθ, the value of Δθ can be calculated when Δx is given by the following formula.

[0035]

[Equation 2] Therefore, the current tip angle of the three joint axes is calculated from the current joint angle by the equation (2), the difference from the target position of the tip three joint axis intersection is calculated to obtain Δx, and the equation (4) is calculated. The target joint angle is obtained by adding the value of Δθ calculated by the equation (4) to the joint angle of. When the value of Δx becomes large, the equation (4) cannot be accurately established, but in the manipulator control device, it is several msec to several tens mse.
There is no problem because this calculation is repeated in a short cycle of c.

Further, the value of k in the equation (4) is the tip 3
From the current joint angle of the joint axis, the posture realized by only the three front end joints is calculated. When the value of the pitch axis direction angle is equal to or greater than the reference value Δ (for example, 10 degrees), the formula (4) is used, for example. The value of the k vector may be set to 0 for calculation.
If the value of the angle in the pitch axis direction is less than the reference value Δ, the link vector tip direction attitude and the target tip attitude at the tip 3 joint axis intersection point are achieved as much as possible by calculating the k vector as follows. Target joint angles that are equal can be calculated. 1) From the given value of the target tip posture, the tip direction unit vector p = (p ¹ , p ² , p at the tip of the manipulator
³ ) is calculated. 2) manipulator tip 3 what a distal direction unit vector at the intersection of the axis represented by the first through fourth axis of articulation angle θi q (θ1, θ2, θ3 , θ4) = (q 1, q 2, q 3 )
And when

[0037]

[Equation 3] Then, the k vector is obtained. However, L is the distance between the tip direction unit vector p at the tip of the manipulator and the tip direction unit vector q at the intersection of the three axes of the tip of the manipulator, and L = (p ¹ −q ¹ ) ² + (p ² −q ² ) ² + (p ³ −q ³ ) ² (6)

Therefore, by calculating the equation (4) using the k vector calculated by the equation (5) and calculating the target joint angles of the 1st to 4th axes, L is satisfied while satisfying the target position.
Is calculated as small as possible, that is, the target joint angle is calculated so that the tip direction of the manipulator and the tip direction at the intersection of the three axes of the manipulator match as much as possible.

On the other hand, the conversion device 2 for converting the target angles of the three joints of the distal end
In 1, the target joint angles of the 5th to 7th axes are calculated by the same processing as in the first invention. However, in this embodiment, the tip 3
Since the attitude of the link tip direction at the intersection of the joint axes and the desired tip attitude are controlled so as to be almost equal, the yaw direction component of the attitude change to be realized in the 5 to 7 axes, regardless of the value of the target attitude given from the outside. Is almost 0, and there is no error from the target tip posture as in the first embodiment. Further, the values of the target joint angles of the 1st to 7th axes calculated by the target position → the 1st to 4th axes coordinate conversion device 20 and the conversion device 21 to the tip 3 joints target angles are input to the 1st to 7th axes control device 25, Each joint is controlled so that the joint angle of each joint follows the target joint angle. In this way, the target position / posture can be realized without sudden changes in all the joints of the manipulator.

[0040]

As described above, according to the present invention, when the peculiar posture in which the two roll axes of the three joint axes of the tip end coincide with each other, or the posture to be realized by the three tip axes is expressed by roll, pitch, and roll. It is possible to calculate a continuous target joint angle that does not change abruptly with respect to a continuous target position / orientation, without the target joint angle of the roll axis changing abruptly when the pitch angle is near 0 degrees.

[Brief description of drawings]

FIG. 1 is a block diagram of a manipulator control device according to a first invention.

FIG. 2 is a diagram showing a comparison between a target joint angle of a fourth axis according to the present invention and a usual analytical calculation.

FIG. 3 is a diagram showing a comparison between a target joint angle of a fifth axis according to the present invention and a usual analytical calculation.

FIG. 4 is a diagram showing a comparison of the target joint angle of the sixth axis by the present invention and ordinary analytical calculation.

FIG. 5 is a diagram showing a posture change in the tip yaw direction according to a target joint angle calculated according to the present invention.

FIG. 6 is a block diagram of a manipulator control device according to a second invention.

FIG. 7 is a diagram showing a schematic configuration of a manipulator having a 6-axis multi-joint structure, the tip 3 axes being a roll / pitch / roll configuration so that the 3 axes intersect at one point.

[Explanation of symbols]

 11 1 to 3 axis coordinate conversion device 12 tip 3 joint target angle conversion device 13 4 axis coordinate conversion device 14 4 axis target angle correction unit 15 5, 6 axis target joint angle calculation unit 16 1 to 6 axis control device 20 1 to 4 Axial coordinate converter 21 Target 3 joint target angle converter.

Claims (2)

[Claims] 1. A three-dimensional position and a tip of an intersection of the three joint axes in a manipulator having at least six joints, and the three joints at the tip have a roll-pitch-roll configuration such that the joint axes intersect at one point. In the manipulator control device for controlling the posture to the target value, the intersection 3
The coordinate conversion device that calculates the joint angle that achieves the target position of the joint axis intersection point, the roll axis coordinate conversion device on the root side of the three tip axes, and the axis lines of the two roll axes of the three tip axes come closer than a certain standard. In this case, the roll axis target angle correction device that corrects the target angle of the roll axis on the root side so as to approach the reference angle according to the value of the joint angle of the pitch axis that is the difference between the two roll axes in the axial direction. And a tip joint axis target angle calculation unit that calculates a target joint angle of the two tip axes other than the roll axis so as to realize the target posture of the manipulator tip portion corresponding to the corrected target angle of the roll axis on the root side. , And a joint axis control device for controlling rotation of each joint shaft to a target joint angle, a manipulator control device. 2. A manipulator control device having 7 joints, wherein the 3 joints at the tip end have a roll-pitch-roll configuration such that their joint axes intersect at one point. Only when the coordinate conversion device that calculates the joint angle that realizes the position expresses the posture realized by the three distal end joints by roll, pitch, and yaw, is the pitch angle near O degrees? A manipulator control device, characterized in that a target joint angle is calculated such that the link tip direction and the target tip direction at the intersection points of the three joint axes are as equal as possible while realizing the target position.