JPH07132474A - Manipulator control device - Google Patents

Abstract

PURPOSE:To enable the movement of an elbow position in any finger position- attitude area by determining a control parameter according to whether the position and attitude of the fingers are within the range of the command value on a redundant parameter on the basis of the command value on the finger position and attitude, and performing the inverse coordinate conversion of each joint angle at real time. CONSTITUTION:The joint angle of an articulated arm 1 with seven freedom or more formed of rotary joints is controlled by each shaft control part 2. Such a manipulator control device is additionally provided with an inverse coordinate converting part 3. On the basis of the command value on the position and attitude of the fingers, the inverse coordinate converting part 3 determines a control parameter according to whether the position and attitude are within the range of the command value on a redundant parameter and performs the inverse coordinate conversion of each joint angle at real time using this control parameter. The redundant freedom of a manipulator is thereby controlled by off-line or on-line without being limited by the position and attitude of the fingers.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator control device, and more particularly to a manipulator control device having a 7-degree-of-freedom articulated arm in which roll rotating shafts and pitch rotating shafts are alternately coupled.

Most of the robots used for industrial purposes are mainly manipulators having a maximum of 6 degrees of freedom. This is useful when positioning an object with your hand.
This is because the degree of freedom is sufficient.

On the other hand, the human arm has seven degrees of freedom. With this, a human can, for example, position an object with a hand and simultaneously control the position of the elbow to avoid an obstacle.

Therefore, there is a need for a robot manipulator having redundant 7 degrees of freedom like a human arm.

[0005]

2. Description of the Related Art Conventionally, since a redundant manipulator is moved in real time, it is necessary to convert a position / orientation command value of a hand and a command value for controlling redundant degrees of freedom into a joint angle by using an inverse coordinate conversion formula. It was done.

This will be schematically described below with reference to FIGS. 9 and 10.

FIG. 9 is a diagram showing the structure of the manipulator. The manipulator 10 has an RPRPRPR type rotary joint (7 degrees of freedom) in which a roll rotation axis and a pitch rotation axis are alternately coupled, arms 11 to 17 and joints 11a to 17a, and a wrist 30 is fixed to a tip of the arm 17. Has been done. The rotation directions of the joints are indicated by θ1 to θ7.

With this mechanism, like the human arm, the position of the elbow joint (fourth joint) can be changed by moving the arm portion on the base side of the wrist (sixth joint) in a state where the hand is fixed. In the case of 6 degrees of freedom, there is no third joint 13a and 7
You cannot work like a degree of freedom.

The lower side of the first joint 11a, which is a rotary joint, is fixed to the upper end of the base 10a.
The lower end of the first arm 11 is fixed to the upper side of the. A second joint 12a, which is a bending joint, is provided at the upper end of the first arm 11.
One is fixed, and the lower end of the second arm 12 is fixed to the other of the second joint 12a. Further, the lower side of the rotary joint 13a is fixed to the upper end of the second arm 12, and the lower end of the third arm 12 is fixed to the upper side of the third joint 13a. The third arm is bent 90 ° on the way. One of the fourth joints 14a, which is a bending joint, is fixed to the other end of the third arm 13. The fourth arm 14 is fixed to the other side of the fourth joint 14a. 4th arm 1
4 also bends 90 ° in the middle.

The lower side of the fifth joint 15a, which is a rotary joint, is fixed to the upper end of the fourth arm 14. Fifth joint 15
The lower end of the fifth arm is fixed to the upper side of a. Fifth
One of the sixth joints 16a, which is a bending joint, is fixed to the upper end of the arm, and the lower end of the sixth arm 16 is fixed to the other of the sixth joints 16a. The lower side of the seventh joint 17a, which is a rotary joint, is fixed to the upper end of the sixth arm 16, and the lower end of the seventh arm 17 is fixed to the upper side of the seventh joint 17a. The tip of the seventh arm 17 has a fingertip 30.
Is fixed. That is, the seventh joint 17a controls the rotation of the hand.

The following Table 1 shows the correspondence between the link distance d _i , the twist angle α _i , and the link length a _i with respect to the joint angle θ _i .

[0012]

[Table 1]

Here, each joint 11 of the manipulator 10
One of the joints a to 17a is assigned as a joint for redundant degree of freedom control. Here, the third joint 13a is a redundant degree of freedom control joint. 6 other joints 11a,
12a, 14a, 15a, 16a, and 17a are assigned as joints for controlling hand movements. These six hand movement control joints 11a, 12a, 14a, 15a, 16a, 17a
Determines the movement of the hand with 6 degrees of freedom.

Then, as shown in FIG. 10, the following control is performed.

[Step S21]: Current joint angle θ ₁ ~
Detect θ ₇ .

[Step S22]: The current hand position / posture is calculated.

When the hand position / posture is determined in this way, the shoulder
(2nd joint), elbow (4th joint) and wrist (6th joint)
The triangle formed is uniquely determined. The angle at the elbow position of this triangle
Degree is θ _FourAnd can be expressed by the following equation (1).
It

[0018]

[Equation 1]

After obtaining the angle θ ₄ at the elbow position in this manner, next, from the coordinate conversion relationship between the shoulder (second joint) position and the wrist (sixth joint) position, θ ₁ , θ ₂ , and θ ₃ are calculated. The relational expression is calculated as follows.

[0020]

[Equation 2]

In the above equations (2a) to (2c), θ ₅ to
Although θ ₇ is not included, since θ ₄ has already been obtained, the unknowns are three, θ ₁ to θ ₃ .

However, these relational expressions (2a)-
Since (2c) satisfies the cosine theorem relation, there are substantially two relational expressions, and there are variables that can be arbitrarily taken, and the joint angle θ can be obtained only by these relational expressions (2a) to (2c).
It is not possible to obtain _{1 to} θ ₃ . This θ _{1 to} θ ₃ represents the redundancy of the arm.

[Step S23]: The third joint is parameterized. Since the parameters are redundant as above,
Here, the angle θ ₃ of the third joint is used as a parameter. This is because, as described above, in the case of 6 degrees of freedom, there is no third joint 13a, and even if it has 6 degrees of freedom, it is sufficient as a manipulator for constant movement.

[Step S24]: The third joint is fixed and each joint angle is calculated from the inverse coordinate transformation formula (inverse kinematic equation).

When the value is fixed using the joint angle θ ₃ as a parameter as described above, the joint angle θ ₁ is obtained from the equation (2c) as follows.

[0026]

[Equation 3]

Further, the joint angle θ ₂ of the second joint is as follows.

[0028]

[Equation 4]

Further, the joint angles θ ₅ and θ ₆ can be calculated by the following equations from the relationship between the position and the posture of the hand viewed from the coordinate system at the elbow position.

[0030]

[Equation 5]

[0031]

[Equation 6]

Furthermore, the joint angle θ ₇ can be calculated from the relationship between the position and posture of the hand as seen from the coordinate system at the wrist position as in the following equation.

[0033]

[Equation 7]

Here, the vector component "a" in the above equations (5) to (7) will be briefly described. First, FIG.
Each joint 11a to 17 of the 7-DOF manipulator shown in FIG.
The conversion matrix in a is represented by the following equation.

[0035]

[Equation 8]

Therefore, the coordinate system of the hand position / posture (seventh joint) viewed from the coordinate system at the elbow position (fourth joint) as described above is expressed by the following equation.

[0037]

[Equation 9]

Here, the above coordinate transformation matrix ⁴ T ₇ is replaced by the following equation.

[0039]

[Equation 10]

Therefore, in this case, the direction of the z-axis of the coordinate system of the seventh joint as seen from the coordinates of the fourth joint is as in the following equation.

[0041]

[Equation 11]

[0042] ⁴ in x of such vector, y, the components of the z-axis direction is determined above equation (5) and (6)
a _7X to ⁴ a _7Z are obtained, and the joint angles θ ₅ and θ ₆ are obtained by substituting these.

Similarly, ⁶ a _7X and ⁶ a in equation (7)
_{7Z is} also obtained, and the joint angle θ ₇ can be obtained.

[Step S25]: The arm is moved. The control system moves each arm with respect to each joint angle thus obtained, whereby the hand position / posture given in step S27 can be realized.

In this way, each joint angle θ _{1 to} θ of 7 degrees of freedom is
_{If 7} is given, the position / orientation of the hand can be obtained by the coordinate conversion formula, but since this coordinate conversion formula is non-linear (irreversible), even if the position / orientation of the hand is given, each joint angle θ ₁ ~
θ ₇ cannot be uniquely determined. Therefore, if only θ ₃ among them is calculated as a fixed parameter as described above, the other joint angles θ ₁ , θ ₂ , θ ₄ , θ ₅ , θ ₆ , θ ₇ are obtained. .

In other words, in a manipulator having 7 or more degrees of freedom, there is arbitrariness in the inverse conversion from the position / orientation value of the hand as described above into the joint angle, and the joint angle having the same hand / position is obtained. Exists infinitely while satisfying certain constraints. Redundant manipulators are used to avoid mechanical limitations (such as singular points and joint limits) of manipulators and avoid obstacles in the entire arm by utilizing the arbitrariness of the joint angles.

[0047]

However, in the conventional control device as described above, since the control command value of the redundant degree of freedom is represented by one variable, for example, the angle variable θ ₃ of the third joint, the redundant degree of freedom is reduced. Hand position / posture area (Fig. 5)
The shaded area A) exists, and the redundancy degree of freedom could not be fully utilized. .

When this is confirmed by computer simulation, as shown in FIG. 8A, the elbow position (the angle formed by the triangle consisting of the shoulder position, the elbow position and the wrist position and the vertical plane).
When given the hand position trajectory in a state in which the elbow is Tsu folded from the state extending while moving the, when the redundancy control parameters using only theta _3, in between times from 0.5 to 0.83 seconds joints The angles θ ₁ and θ ₂ are not found as shown, indicating that the elbow position cannot be moved.

Therefore, according to the present invention, in the manipulator control device for controlling the joint angle of the joint arm having seven degrees of freedom, which is composed of the rotary joint, by each axis control section, what kind of hand position
The purpose is to be able to move the elbow position in the posture area as well.

[0050]

In order to achieve the above object, in the manipulator control device according to the present invention,
As shown in principle in FIG. 1, the control parameter is determined based on whether or not the position / orientation is in the region of the command value of the redundant parameter based on the given command value of the position / orientation of the hand. It is characterized by including an inverse coordinate transformation unit 3 for performing inverse coordinate transformation of each joint angle in real time using parameters.

Further, the inverse coordinate transformation unit 3 obtains the wrist position on the work coordinate plane with respect to the origin of the pedestal on the basis of the given command value of the position / orientation of the hand, and the wrist position is triaxial with the wrist position. Each joint angle can be inversely transformed by using the angle of the first joint or the third joint as the redundant parameter depending on whether it is larger or smaller than the distance from the straight line passing through.

[0052]

As described above, in the conventional manipulator control device, since the control is performed using only one redundant control parameter, there is a hand position / posture area in which the redundant degree of freedom cannot be used.

Therefore, in the present invention, the inverse coordinate transformation unit 3 shown in FIG. 1 performs the processing shown in FIG. 2 in real time.

That is, after the command position of the position / orientation of the hand is given, it is determined whether or not this position / orientation is in the region of the redundant control parameter φ _i (step S1).

[0055] Then, the control parameter when it is found in the region of the redundancy control parameter phi _i and the parameter phi _i (step S3), and based on an inverse coordinate transformation to the phi _i as a parameter to the command value of phi _i By executing the above (step S4), each joint angle θ can be obtained.

If it is determined in step S1 that the position / orientation is not within the area of the redundant control parameter φ _i , this "i" is changed to another redundant control parameter φ _i.
It is determined whether or not the position / orientation is within that area. (Step S2).

For example, when the manipulator has 7 degrees of freedom, the redundancy control parameter φ _i in step S1 is as shown in FIG.
The joint angles θ ₁ and θ ₃ shown in FIG.

Then, these control parameters θ ₁ or θ
_In selecting ₃ , whether the wrist position on the work plane with respect to the origin of the pedestal, which is obtained based on the given position / orientation command value, is larger or smaller than the distance between this wrist position and the straight line passing through the three axes Can be decided.

Therefore, in the case of a region having θ ₁ as a parameter, each joint angle θ can be obtained by performing inverse coordinate transformation with the value of θ _{1 being} a fixed value. Similarly, in the case of the region of θ ₃ , Each joint angle θ can be obtained by performing inverse coordinate transformation with θ ₃ as a fixed value.

As described above, in the present invention, the redundant manipulator degenerates the inverse coordinate transformation in the case of 6 degrees of freedom by parameterizing the extra (redundant) joint angle, and at that time, the redundant degree of freedom is moved. By switching the parameter according to the position / orientation of the hand, the redundant degree of freedom of the manipulator can be controlled without being limited to the position / orientation of the hand.

[0061]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 2 shows a block diagram of the hardware of the control system of the manipulator. Control system is workstation 20
0, a control device 20, and a controlled manipulator 10 for processing in real time. Note that the manipulator 10 has seven degrees of freedom and has seven joints, but in FIG. 2, only a motor that drives one joint is shown in FIG. 2 for simplification of description, and the control system also has one joint. Only the controlling elements are shown.

The controller 20 is mainly composed of the board computers 21 and 22. 32 on board calculators 21 and 22
Bit RISC is used. Board calculator 21,
22 to the A / D converters 23 and D via the bus 21a.
IO (input / output interface) 24, D / A converter 2
5, a counter 26, and an A / D converter 27 are connected.

The workstation 200 and the board computers 21 and 22 are connected by a communication line 201,
From the workstation 200, board computer 21,2
The operation program of the manipulator 10 and the necessary data are sent to 2. The board computers 21 and 22 control the manipulator 10 according to this operation program.

The board computer 21 controls the position and speed of the hand 30 of the manipulator 10 and calculates the position, speed and acceleration of each joint angle. In addition, overall control such as control of redundant joint angles described below is performed. The position, velocity, acceleration, and redundant joint angle data of each joint angle are sent to the board computer 22. The board computer 22 receives these data,
The position, velocity and acceleration of each joint of the manipulator 10 are controlled.

The A / D converter 23 converts the output from the force detector of each joint of the manipulator 10 into a digital value. The DIO 24 is a manipulator 10 and a board computer 2.
This is an interface for exchanging input / output signals with the I / Os 1 and 22. The D / A converter 25 receives the speed command of each joint from the board computer 22 and sends it to the motor driver 28, and the motor driver 28 follows the command to the motor 1
Drive 0A.

An encoder 10B is coaxially coupled to the motor 10A and feeds back the rotational position of the motor 10A. That is, the encoder 10B generates a pulse according to the rotation of the motor. This pulse is counter 2
It is counted by 6, and the board computers 21 and 22 can recognize the rotational position of the motor 10A by reading the counter 26. In addition, this pulse is F / V (frequency /
The voltage is converted into a voltage by the voltage converter 29 and sent to the A / D converter 27.

Then, it is converted into a digital value by the A / D converter 27. By reading this digital value, the board computers 21 and 22 can recognize the speed of the motor 10A, in other words, the speed of each joint.

Of course, since only one joint is shown in FIG. 2, the A / D converter 23, the DIO 24, and the D / A converter 2 are shown.
5, the counter 26, the A / D converter 27, and the like are required only in the number corresponding to the number of joints. However, the DIO 24 and the like may be configured to be commonly used. Further, seven motors 10A for driving the joints are actually required, and the motor driver 28 and the F / V converter 29 are also the same.

The arm 1 shown in FIG. 1 corresponds to the manipulator 10 shown in FIG. 3, each axis control section 2 corresponds to the motor driver 28 and the motor 10a shown in FIG. 3, and the inverse coordinate conversion section 3 is a control device. Corresponds to 20.

The operation of the above embodiment will be described below. This operation is performed in real time. In the embodiment of the present invention, the above equations (1) to (11) are similarly used in the present invention. Therefore, the flowchart of FIG. 4 for explaining the operation of the embodiment of the present invention is schematic. Is shown in.

First, in FIG. 4, when the command value x of the position / orientation of the hand is given, the wrist position ( ⁰ x
₅ , ⁰ y ₅ , ⁰ z ₅ ) is obtained (step S11).

From this wrist position, the angle θ ₄ at the elbow position of the triangle formed by the shoulders, elbows and wrists can be obtained as shown in equation (1), and further from the coordinate conversion relationship between the shoulder position and the wrist position. As shown in equations (2a) to (2c), the joint angle θ ₁ to
The relational expression of θ ₃ can be obtained.

In the three relational expressions, as described above, there are _three unknowns θ ₁ to θ 3, whereas in practice there are two relational expressions due to the cosine theorem relation, and therefore the parameter is arm redundancy. It represents.

When the equation (2c) is modified, the following equation is obtained.

[0075]

[Equation 12]

[0076] Further, as described above be determined the position and attitude of the end from ⁰ x _5, ⁰ y _5, and theta ₄ is determined, Equation (12) is theta ₁
It can be regarded as a function of θ ₃ and θ ₃ and corresponds to the movement of the elbow with the hand fixed, so it can be seen that the elbow can be controlled by θ ₁ or θ ₃ .

Therefore, which of θ ₁ and θ ₃ is used as the redundant control parameter is selected according to the position and orientation of the hand as follows.

√ ( ⁰ x ₅ ² + on the left side of equation (12)
⁰ y ₅ ² ) is the distance between the origin and the position of the wrist 30 on the work coordinate plane (xy plane) as shown in FIG. 6A, and r (1-cos θ ₄ ) + L in the equation (12). _{Since 2} sin θ ₄ is a distance from a straight line passing through the three axes of θ ₁ to θ ₃ as shown in FIG. 7B, when the elbow is relatively extended, the following equation is obtained.

[0079]

[Equation 13]

When relatively folded, the following equation is obtained.

[0081]

[Equation 14]

Here, when θ ₁ is a control parameter,
From the equation (12), θ ₃ is given by the following equation.

[0083]

[Equation 15]

However, in the case where the position of the hand has the relationship represented by the equation (13), θ ₁ is given by the following equation.

[0085]

[Equation 16]

Therefore, θ ₁ can be applied only to the area B in FIG. 5, and the shaded area A is limited, and the redundant degree of freedom cannot be continuously controlled with θ ₁ . This discontinuity is as shown in FIG.

Therefore, when the hand position has the relationship shown in the equation (13), θ ₃ is set as the redundant control parameter by the following equation.

[0088]

[Equation 17]

This is the state shown in FIG. 7A, and θ ₃
It turns out that is continuous.

On the contrary, when the hand position / posture is in the state shown in the equation (14), if θ ₃ is the redundant control parameter, it is restricted as in the following equation, and it is not possible to control with θ ₃ (FIG. 7).
(See (c)).

[0091]

[Equation 18]

Therefore, returning to the flowchart of FIG. 4, the position / orientation of the hand is in the state of the formula (13) or the formula (1)
Determines whether the state of 4) (step S12), the formula (13 theta ₂ by the inverse coordinate conversion formula as control parameter theta ₁ if the relationship _{of), θ 3, θ 5,} θ 6, θ ₇
Is calculated (step S13).

Further, in the case of the relation shown in the equation (14), θ
Since may be controlled at _3, theta ₁ by inverse coordinate conversion formula that the theta ₃ as a _{parameter, θ 2, θ 5, θ} 6, all joints by calculating the theta ₇ angle theta ₁ through? ₇ is obtained It will be.

The angles θ ₁ , θ ₂ , and θ _{5 to} θ ₇ can be obtained by the above equations (3) to (7).

The graph shown in FIG. 7B shows the locus when the wrist moves on the boundary line between the shaded portion and the other portion in FIG.

Next, the redundancy control parameter is set to θ as described above.
FIG. 8B shows a diagram obtained by confirming by computer simulation whether it is possible to control by switching between ₁ and θ ₃ .

According to this, it is shown that the elbow position can be continuously moved when the parameter is switched from θ ₃ to θ ₁ at the time of 0.5 seconds, and the conventional method shown in FIG. As shown in the example, it can be seen that the state in which θ ₁ and θ ₂ are not obtained during the time 0.5 to 0.83 seconds and the elbow position cannot be moved has disappeared.

[0098]

As described above, according to the manipulator control device of the present invention, the position of a given hand is
The control parameter is determined depending on whether or not the command value of the posture is within the range of the command value of the redundant parameter, and each joint angle is inversely transformed using this control parameter. Instead, it becomes possible to control the redundant degree of freedom of the manipulator offline or online.

[Brief description of drawings]

FIG. 1 is a block diagram showing a configuration principle of a manipulator control device according to the present invention.

FIG. 2 is a flowchart showing the operating principle of the manipulator control device according to the present invention.

FIG. 3 is a block diagram showing an embodiment of a manipulator control device according to the present invention.

FIG. 4 is a flowchart showing an embodiment of an inverse coordinate transformation unit used in the manipulator control device according to the present invention.

FIG. 5 is a diagram for explaining a region where an elbow can be moved by the manipulator control device according to the present invention.

FIG. 6 is a diagram for explaining expansion and contraction of an elbow in the manipulator control device according to the present invention.

7 is a graph showing the relationship between the angle of the first joint of the manipulator control device theta ₁ and the angle theta ₃ of the third joint according to the present invention.

FIG. 8 is a computer simulation diagram for explaining the difference in parameters between the conventional example and the present invention.

FIG. 9 is a diagram showing a general configuration of a seven-degree-of-freedom manipulator in the manipulator control device according to the present invention.

FIG. 10 is a flowchart illustrating the operation of the conventional technique.

[Explanation of symbols]

1 arm 2 each axis control unit 3 inverse coordinate conversion unit 10a, 11 to 17 arm 11a to 17a joint L _{0 to} L ₃ link distance r link length θ _{1 to} θ ₇ joint angle 30 wrist Indicates the same or corresponding part.

Claims (2)

[Claims] 1. A manipulator control device for controlling a joint angle of a joint arm (1) having 7 degrees of freedom or more, which is composed of a rotary joint, by each axis control section (2). An inverse coordinate conversion unit (3) that determines a control parameter based on whether or not the position / orientation is in the region of the command value of the redundant parameter based on the above, and uses the control parameter to inversely convert each joint angle in real time A manipulator control device comprising: 2. The inverse coordinate conversion section (3) obtains a wrist position on a work coordinate plane with respect to the origin of the pedestal on the basis of the given command values of the position and orientation of the hand, and the wrist position is the wrist position. 2. The inverse coordinate transformation of each joint angle is performed by using the angle of the first joint or the third joint as the redundant parameter according to whether the distance is larger or smaller than the distance between a straight line passing through the three axes. Manipulator control device.