JPH0569354A - Articulated link mechanism control device - Google Patents

Abstract

(57) [Abstract] [Purpose] It is possible to calculate even if there are redundant degrees of freedom in the motion controller of an articulated link mechanism, the calculation does not fail even near the singular point, and the continuity of the solution is maintained. To provide a dynamic control device. [Constitution] When the target position is above the plane including the joint and the tip position of the link mechanism, the joint angle is corrected in the positive direction,
A plurality of arithmetic units 1, which corrects in the negative direction when below
A shared memory device 10 for storing the joint angles and the coordinate conversion matrix is provided. [Effect] Various link mechanisms including those having redundant degrees of freedom can be controlled by one general-purpose operation procedure. Further, continuous mechanical control can be realized even near a peculiar posture. Furthermore, control data can be calculated at high speed by parallel calculation.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a motion control device for a link mechanism having a plurality of joints.

[0002]

2. Description of the Related Art In the control of an articulated link mechanism, the position / orientation of the tip of the link mechanism is given, the joint angle that realizes this is calculated, and this is given to the control mechanism to control the motion. Many. Conventionally, the following method is used for the inverse transform calculation for calculating the joint angle from the tip position / posture.

The first method is a method of directly solving a simultaneous equation that gives the relationship between the joint angle and the tip position to obtain the angle.

The second method is a method in which the simultaneous equations are locally linearly approximated by the Jacobian and a solution is recursively calculated from a certain initial value by iterative calculation.

The position / orientation of the tip of the link mechanism is represented by six parameters. Therefore, when the link mechanism has seven or more degrees of freedom (the number of independent joints) (when there is a redundant degree of freedom), it is generally impossible to uniquely determine the solution. Therefore, some constraint conditions such as kinetic energy minimization are introduced, and the degree of freedom is artificially reduced to perform the calculation.

[0006]

In the first method, since the simultaneous equations are non-linear equations including many trigonometric functions, it is often difficult to obtain a solution analytically. Even when a solution is obtained, the calculation procedure depends on the shaft configuration of the mechanism, so versatility cannot be ensured.

The second method is subject to the Jacobian being regular. In the vicinity of the singular pose in which the Jacobian is no longer regular, numerical calculation tends to fail and the solution is often discontinuous. This is an obstacle to continuous mechanical control.

When there is a redundancy degree of freedom, it is necessary to properly select the constraint condition to be introduced.

The object of the present invention is that calculation is possible even when there are redundant degrees of freedom, and the calculation does not break even in the vicinity of a singular point,
It is to provide a control device in which the continuity of solution is maintained.

[0010]

The above object can be achieved by a joint angle correction amount calculating means for giving an independent angle increasing / decreasing criterion for each joint and calculating a solution cooperatively.

That is, when the joint angle at a certain time t for each joint i is θ _i (t) and the correction amount of θ _{i that} brings the tip end position closer to the target position is Δθ _i (t), the joint angle θ at the time t + 1. _i (t + 1)

[0012]

[Equation 1]

Given by By repeating this operation independently for each joint, the tip position is brought closer to the target position.

Now, as shown in FIG. 3, the vectors z _i , r _i , and x _i are the unit vectors in the rotation axis direction of the i-th joint J _i , the vector from J _i to the tip position R, and the target from J _i to the target. Assuming that the vector to the position X, Δθ _i (t) is, for example, Δ
θ _i may be determined as follows.

[0015]

[Equation 2]

Where α _i is a positive correction amount, and | (z _i r _i x _i ) |
Is a determinant of a 3 × 3 matrix having vectors z _i , r _i , and x _i as columns.

[0017]

The determinant shown in equation 2 is

[0018]

[Equation 3]

It can be modified as follows. This is a plane S determined by the joint rotation axis z _i and the tip position R as shown in FIG.
_With respect to _ri , it is positive when the target position X is above (front side), and is negative when it is below (back side). Therefore,
The Δθ _i of Equation 1 increases θ _i when the target position is above S _ri and decreases θ _i when the target position is below (on the back side), while independently performing the operation for each joint, the tip is set to the target position. Get closer.

The vectors z _i , r _i , and x _i can be calculated if the angle θ (t) of each joint at the time t and the target position X are given.

[0021]

[Equation 4]

It can be expressed as However,

[0023]

[Equation 5]

It is assumed that Hereinafter, Equation 4 will be referred to as a joint control function, and Equation 5 will be referred to as a code determination condition. The structure of Expression 4 is in a form in which the joint correction amount which is the control input is explicitly expressed by the joint angle and the tip position which are the state amounts. Therefore, the calculation procedure is simple and various types of control characteristics can be easily incorporated. In addition, since it is an explicit expression form, the inverse matrix calculation as in the case of using the Jacobian is not necessary, so the calculation does not fail even near the singular point, and the continuity of the solution is maintained.

From the structure of Equation 4, the joint correction amount determination procedure can be given independently for each joint. Therefore, it can be calculated even when there is a redundant degree of freedom.

Since the equation 2 does not include the calculation procedure peculiar to the shaft configuration of the link mechanism, the present invention has general versatility applicable to the link mechanism of any shaft configuration.

[0027]

[Embodiment] The number 4 of the joint control function is simply the joint angle correction amount Δθ from the joint angle θ (t) and the target position X (t) at the time t.
The shape of the function f may be any shape as long as the function satisfies the condition specified by the code determination condition number 5, only by specifying to determine _i . As an example of the present invention,
The case where f is an expression that gives variable gain type feedback type control is shown below.

First, as a joint control function

[0029]

[Equation 6]

Consider the equation Here, θ _ei is a value obtained by converting the amount of displacement of the tip position with respect to the target position into the amount of displacement at the joint angle, k _i (χ) is the feedback gain, and the feedback gain is a function of the joint sensitivity χ _i .
θ _ei , k _i (χ _i ), and χ _i are calculated as follows, respectively.

Considering the following as the vectors n _ri and n _xi ,

[0032]

[Equation 7]

[0033]

[Equation 8]

N _ri is a unit normal vector of the plane S _ri determined by the rotation axis z _i and the tip position R in FIG. 3, and n _xi is determined by the rotation axis z _i and the target position X in FIG. Becomes a unit normal vector of the plane S _xi . From this, the joint angle conversion value θ _ei of the displacement amount of the tip position is given by the following equation.

[0035]

[Equation 9]

However, atan2 (x, y) is x = sin θ, y = cos θ
Is a function that gives θ. Also, n _ri = 0, or n
_{When xi} = 0, θ _ei = 0. θ _ei is the sign judgment condition
It is a function that satisfies (5).

Next, the joint sensitivity χ _i is defined as follows.

[0038] In FIG. 3, the vector x _i from the joint i to tip position X when it is almost parallel to the rotation axis z _i, the rotation of the joint i hardly contributes to the movement to bring the tip position to the target position. On the other hand, the joint i may change significantly with a slight displacement of x _i , resulting in unnecessary rotational movement. Therefore, joint sensitivity is defined as the degree of contribution of each joint to the movement of the tip position, and the feedback gain is adjusted accordingly.

The joint sensitivity χ _i is given by the following equation.

[0040]

[Equation 10]

Here, if the angle formed by the vectors z _i and x _i is φ, then || z _i || = 1,

[0042]

[Equation 11]

That is, the joint sensitivity is nothing but sin φ.

By using this χ _i , the feedback gain k _i (χ _i ) is

[0045]

[Equation 12]

Calculate by However, k _i0 is a standard gain and is a positive constant. k _i (χ _i ) has a non-negative value.

From the above, the equation 5 is specifically

[0048]

[Equation 13]

It is given in the form of

Next, a calculation procedure of z _i , r _i , and x _i necessary for calculating the vector of n _ri , n _xi, etc. will be shown. These are given by the following equations.

[0051]

[Equation 14]

[0052]

[Equation 15]

[0053]

[Equation 16]

Here, R _i is a vector from the root of the link mechanism to the joint J _i , and R _{n + 1} is a vector from the root to the tip. e _z is a unit vector in the z-axis direction of the world coordinate system F _W. B _i is a rotation conversion matrix between the joint coordinate system F _i and the world coordinate system F _W with the joint position as the origin and the joint rotation axis direction as the z axis. When the coordinate transformation matrix between F _W and F _i and A _i, R _i is given by three elements on the far right column vector A _i, the upper left 3 B _i is A _i
× 3 submatrix. That is,

[0055]

[Equation 17]

[0056]

[Equation 18]

It becomes

If the coordinate transformation matrix to F _i F _{i + 1} is T _i , T _i is a function of the joint angle θ _i.

[0059]

[Formula 19]

There is the following relationship.

When the above relationships are summarized, θ _i (t) can be converted to θ
_The specific procedure for calculating _i (t + 1) is shown below.

[0062]

[Equation 20]

[0063]

[Equation 21]

That is, A _i (t) (i = 1 ... n) is calculated using the joint angle θ (t) at time t, and A _i (t) and A _{n + 1} (t) are calculated.
And X (t) are used to calculate each joint control function f _i . FIG. 4 shows a chart summarizing the calculation procedure. In the figure,
Reference numeral 401 is data at time t, 402 is data at time t + 1, 403 is timing for referring to data, and 404 is timing for rewriting data.

The calculation time Δt per step according to this calculation procedure is t _{A, the} time required to calculate t _f ,
Let be the time required to calculate equation 21

[0066]

[Equation 22]

It becomes Δt becomes longer in proportion to the number of joints n.

Now, assuming that Δt can be made sufficiently small, it can be considered that A _i (t) and A _i (t + 1) are almost equal.

[0069]

[Equation 23]

It can be modified as follows. As a result, the calculation procedure is shown in Fig. 5.
The calculation time Δt is

[0071]

[Equation 24]

Therefore, it can be made constant regardless of n.

FIG. 1 shows the configuration of an apparatus that executes the above calculation procedure.

In the figure, reference numerals 1, 2 and 3 represent the joint angle θ _i (t) at the time t, the target position X (t), the coordinate transformation matrix A _i (t) and A _{n + 1} (t). A according to 23
_{i + 1} (t) and the joint angle θ _i (t + 1) at the next time t + 1
Is an arithmetic unit for calculating This arithmetic device is prepared by the number of joints of the articulated link mechanism to be controlled. 10
Is a shared storage device for storing the information of the link mechanism in each calculation step. The inside is divided into areas according to the stored information. 4, 6, 8 is θ _i (t)
Is a storage area for storing the value A _i (t) at the time t of the coordinate conversion matrix for obtaining the vectors z _i , r _i , and x _i from X and (t). Reference numerals 5, 7, and 9 are areas for storing the joint angle θ _i (t) at time t.

FIG. 2 shows a calculation procedure executed in the calculation devices 1, 2, and 3.

Now, assuming that the arithmetic unit 2 is the arithmetic unit for controlling the i-th joint angle, 2 first calculates z _i from A _i (t) stored in 8 according to equation 16 (step 1 ) Calculating r _i from A _{n + 1} (t) stored in 4 and A _i (t) stored in 8 according to Equation 14 (step 2), and inputting X (t) and A _{From i} (t), x _i is calculated according to equation 15 (step 3). Next, χ _i is calculated from z _i and x _i according to the equation 10 (step 4), and x _i , r _i ,
θ _ei is calculated from z _i according to _Equation 9 (step 5). Then, from χ _i and θ _ei , the joint control amount, that is,
Calculate Δθ _i (step 6) and add it to the current joint angle θ _i (t) stored in 7 to obtain the next target joint angle θ _i (t + 1), and set this to 7. Write (step 7). Also, 2 is θ _i (t) of 7 and A _i (t) of 8
Then, A _{i + 1} (t) is calculated according to the equation (19) and is written in 6 (step 8).

The control operation of the joint angle is executed by the above procedure.

Next, a second embodiment using the present invention will be described.

[0079] In the first embodiment, although one of the target position X (t) is given to the control device, in the second embodiment, as shown in FIG. 6, a plurality of target positions X _j ( t) (j =
1 ... m) is prepared and a device for controlling the positions of a plurality of joints J _cj is shown.

The joint control function in this case is given by the following equation.

[0081]

[Equation 25]

Here, a _ij is a positive weighting constant, and f _ij
(Θ, X _j ), where the c _j- th joint J _cj is the tip position, X
The joint control function when _j is the target position,

[0083]

[Equation 26]

It is assumed that J _cm is the tip of the link mechanism.

[0085] Thus, specific steps number 20 for calculating a theta _i from _{(t) θ i (t +} 1) is modified as follows.

[0086]

[Equation 27]

FIG. 7 shows the configuration of the control device. In the figure, 4 to 10 are the same as in FIG. 501,50
2, 503 are joint angles θ _i (t) at time t, target position X _j (t) (j = 1 ... m), coordinate transformation matrix A _i (t), A
It is an arithmetic unit for calculating A _{i + 1} (t) and the joint angle θ _i (t + 1) at the next step t + 1 according to the equations 27 and 23 from _{n + 1} (t).

FIG. 8 shows a calculation procedure executed by the calculation devices 501, 502 and 503.

Now, assuming that the arithmetic unit 502 is an arithmetic unit for controlling the i-th joint angle, the 502 first calculates z _i from A _i (t) stored in 8 according to the equation 16 (step 1 ). Next, for j = 1 ... m, r _i is calculated from A _{n + 1} (t) and A _i (t) stored in 4 according to the equation 14 (step 2), and the input X _j ( x _ij is calculated from t) and A _i (t) (step 3), χ _ij is calculated from z _i and x _ij (step 4), and θ _eij is calculated from x _ij , r _i , and z _i (step 4). Step 5). Then, from χ _ij and θ _eij , Δθ _i
Is calculated (step 6), and this is added to the current joint angle θ _i (t) stored in 7 to obtain the next target joint angle θ _i (t + 1), which is written in 7 ( Step 7). Further, 502 calculates A _{i + 1} (t) from θ _i (t) of 7 and A _i (t) of 8 according to the equation 19, and writes this in 6 (step 8).

The control operation of the joint angles for a plurality of target positions is executed by the above procedure.

In the above embodiment, as the joint control function,
Although the joint angle deviation amount as shown in Formula 6 is multiplied by the feedback gain, various types of structures can be used as the structure of Formula 6 as long as Formula 5 is satisfied.

In the above embodiments, the arithmetic units 1, 2, 3 (or 501, 502, 503) are independent, but the arithmetic operations performed by these arithmetic units are performed by one arithmetic unit. Is also good. However, in that case, a high calculation speed cannot be expected. The areas 4, 6 and 8 for storing the coordinate conversion matrix may be storage areas provided inside the arithmetic unit. The areas 5, 7 and 9 for storing the joint angle may be the joint angle detection device and the joint drive device. In this case, the operation of writing a value in the storage area corresponds to giving a command to the joint drive device, and the operation of reading the value from the storage area corresponds to reading the value from the joint angle detection device.

In this embodiment, the vector R _i representing the tip position and the joint position is calculated by the arithmetic device, but these values may be obtained by the position detecting device which detects the tip position and the joint position.

Next, a third embodiment will be described with reference to FIG.

In the figure, 201, 202 and 203 are arithmetic units, 204, 205 and 206 are joint angle detecting units and 2
Reference numerals 07, 208, and 209 denote position detection devices, and 10 denotes an articulated link mechanism. FIG. 10 shows the connection relationship between the joint, the joint angle detection device, and the position detection device, and the relationship in the rotation direction. i
Assuming that the 101st link is 101, the position detection device S _i (7) attached to the joint J _i (111) is fixed to the _i− 1th link L _i-1 (102). The position detecting device uses the tip end position 401 and the target position 402 of the link mechanism as cylindrical positions 301 and 302 about the rotation axis of the joint J _i.
And the tip position 3 on the cylinder as shown in FIG.
If 01 is in the right rotation direction with respect to the target position 302, +1 is output, and if it is in the left rotation direction, -1 is output.

When the output value of the position detecting device is +1, the arithmetic unit determines the angle detected by the joint angle detecting device by a small amount Δ.
The value increased by θ is output to the actuator of the link mechanism. When the output value of the position detection device is -1, a value obtained by subtracting the angle detected by the joint angle detection device by a minute amount Δθ is output to the actuator of the link mechanism. By continuously operating these mechanisms, the tip position of the link mechanism can be always brought close to the target position. In the present embodiment, even if the actuator of the link mechanism is partially defective, it can operate normally within the range allowed by the degree of freedom of the tip position by the remaining joints.

[0097]

By using the control device according to the present invention, it is possible to control various types of link mechanisms by one general-purpose arithmetic procedure without depending on the axis configuration of the mechanism. In addition, it is possible to control even a link mechanism having an axial configuration in which it is difficult to analytically obtain a solution.

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of the present invention.

FIG. 2 is an explanatory diagram of the operation of the arithmetic device.

FIG. 3 is an explanatory diagram of an operation principle of the present invention.

FIG. 4 is a control procedure calculation chart.

FIG. 5 is an improved calculation procedure chart of a controlled variable.

FIG. 6 is an explanatory diagram of a second embodiment of the present invention.

FIG. 7 is a block diagram showing a second embodiment of the present invention.

FIG. 8 is an explanatory diagram of the operation of the arithmetic unit according to the second embodiment of the present invention.

FIG. 9 is a block diagram showing a third embodiment of the present invention.

FIG. 10 is a perspective view showing a structure near a joint according to a third embodiment of the present invention.

FIG. 11 is an explanatory diagram of a position detection device according to a third embodiment of the present invention.

[Explanation of symbols]

1, 2, 3 ... Arithmetic device, 4, 6, 8 ... Area for storing coordinate transformation matrix, 5, 7, 9, ... Area for storing joint angle, 1
0 ... Shared memory device.

Claims (1)

[Claims] 1. A plurality of arithmetic devices corresponding to joints and a shared memory device for storing the current angle and coordinate conversion matrix of the joint, wherein the arithmetic device is stored in a shared memory for each joint. The current joint angle, the coordinate transformation matrix between the joints, and the target position are input, and the value obtained by correcting the joint angle in the direction in which the difference between the tip position and the target position calculated by the arithmetic unit group becomes small is output. A multi-joint link mechanism control device, which outputs a new coordinate conversion matrix that changes according to the change.