JP2607515B2 - Trajectory control device for articulated robot - Google Patents

Description

The present invention relates to a trajectory control device for an articulated robot such as a space manipulator, a marine manipulator, a nuclear inspection robot, and a repair robot.

[Conventional technology]

Generally, the command speed D (D1, D2,
… The command speed D (1D, 2D,
… Needs to be converted to 6D). To this end, (1) is established between the work variable P of the hand and the joint variable Θ of the robot.

p = f (Θ) (1) When time-differentiating the equation (1), Becomes

However, J (Θ) gives the Jacobian matrix of f (Θ), and de
If tJ (Θ) = 0, it can be solved as = J ^-1 (Θ) · · · (3), and the position and orientation speed of the hand given in the work coordinate system X0-Y0-Z0 becomes the speed in the joint coordinate system. Can be changed. In the expressions (2) and (3), the dot above the variable symbol indicates time differentiation.

Here, a conventional trajectory control device for an articulated robot will be described with reference to the control device of FIG. 8 and the flowchart of FIG.

In FIG. 8, the operator 1 operates the control lever 2 and inputs the command speed PD in the working coordinate system of the hand and the inverse transformation calculator 100. The inverse transformation calculator 100 solves the equation (3) according to the flowchart shown in FIG. 9, converts the equation to the command velocity D in the joint coordinate system, and outputs the command velocity D to the servo amplifier 21 for the θ1 axis to θ6 axis, as shown in FIG. Control an articulated robot. In the robot shown in FIG. 4, J (Θ) is specifically represented by a 6 × 6 Jacobian matrix as shown in FIG. 6, and detJ (Θ) is detJ (Θ) = DETO = −12 · 13 ·. C5 · S3 (12 S2 + 13 S23 + 14 S234) ... (4)

 However, the shorthand notation of C5 = cos θ5 S2 = sin θ2 S23 = sin (θ2 + θ3) S234 = sin (θ2 + θ3 + θ4) is used.

In FIG. 8, 31 is a motor, 41 is a mobility detector, 51 is a joint angle detector, 11 is a θ1-axis drive system that collectively calls them, 12, 13,... 15, 16 are θ2-axis, θ3-axis, respectively. It is a θ6-axis drive system.

[Problems to be solved by the invention]

 However, in order to solve equation (3), it is necessary that detJ (Θ) ≠ 0 in equation (4).

However, at the singular point where detJ (Θ) = 0 and in the vicinity thereof, the hand cannot move in a specific direction, for example, the direction u of the link 12 and the link 13 in the example of FIG.

This cannot be solved unless the configuration is changed due to the problem of the link configuration of the robot, for example, from a 6-DOF robot to a 7-DOF robot.

However, in spite of being mechanically operable in other directions, it could not be controlled by the control device using equation (3).

Therefore, the present invention provides a trajectory control device for an articulated robot in which control can be performed in the remaining direction other than a specific direction even at a singular point and the vicinity thereof, the trajectory control function is improved, and the operability is improved. With the goal.

[Means for solving the problem]

In order to achieve the above object, the present invention provides a trajectory control device for an articulated robot, wherein first input means for inputting a priority order of work coordinates, and second input means for inputting a hand command speed at the work coordinates.
Means for obtaining a determinant DTEO corresponding to the Jacobi matrix J0 previously obtained from the robot mechanism; and determining whether or not the determinant DTEO is equal to or less than a certain value ε0 (= is a singular point). A means for outputting i = 0 in the above case, and a Jacobi matrix Ji excluding a row and a column corresponding to a joint axis to be fixed and a degenerate working coordinate i determined in advance in the singularity analysis when the singularity is the singularity. Calculating means for calculating the absolute value of the corresponding determinant DTEi in accordance with the priority order of the work coordinates input by the first input means; and the determinant DTEi exceeds a value εi predetermined for each i Means for judging that the number i has been exceeded, and a Jacobi matrix Ji (if i = 0, J
And a means for converting the angular velocity into each joint based on 0).

[Action]

According to the present invention as described above, it is possible to control the singular point and its vicinity in the remaining directions other than the specific direction, so that the trajectory control function is improved and the operability is improved.

〔Example〕

Hereinafter, embodiments of the present invention will be described with reference to the drawings. FIG. 1 is a configuration diagram of a trajectory control device according to the present invention.
What is different from the conventional example of FIG. 8 is that a discriminator 200 for judging the vicinity of a singular point from the value of the Jacobian determinant, a priority order of the work variables is set, and an appropriate joint variable is combined. The configuration and operation of the other elements in the inverse transform operation unit 110 that creates the degenerate Jacobian matrix, calculates the determinant, and selects the optimal combination are the same as those in the conventional example.

Before describing the operation of each component, a general principle will be described. The singular point where detJ (Θ) = 0, which could not be controlled conventionally, is when the robot takes a posture as shown in FIG. 5, and at this time, the hand cannot move in a specific direction. Since this is a mechanical problem, it cannot be solved unless the link configuration of the robot itself is changed.

However, it can operate mechanically in other directions.
In spite of that, I can not control it in a nutshell,
This is because the degree of freedom that cannot be moved in the work coordinate system becomes the redundant degree of freedom in the joint coordinate system. More specifically, referring to FIG. 5 (a), when trying to move the hand in, for example, the Y direction, the action on the θ2 axis and the θ3 axis hand is affected because the link 13 of the θ3 axis is fully extended. They are exactly the same. In other words, this indicates that it is not determined from equation (3) which of the θ2 and θ3 axes should be moved in this posture.

That is, equation (3) is synonymous with solving the 6-way simultaneous linear equation of equation (2).
(When the singularity conditions do not overlap).

Therefore, equation (2) can be solved by setting any one of 1 to 6 as a constant and reducing the equation to five-order simultaneous primary.

 However, the work variables also need to be reduced by one variable.

At the singular point shown in FIG. 5, the determinant is obtained by using θ2 as a constant as described later, and it is understood that the remaining joint variables are independent. Note that the same applies to the case where θ1, θ3 to θ6 are selected for the degree of redundancy, and the description is omitted.

At this time, using the quintuple running linear equation corresponds to removing the second column and the i-th row from the Jacobian matrix in FIG.

Now, the Jacobi matrix excluding the i-th row is written as Ji, and detJi
= DETi, DETi is invalid DET6 as shown in Fig. 7.
Except for 5 are obtained.

That is, if Ji is used, the i-th work variable cannot be controlled, but if DETi ≠ 0, control can be performed on the remaining work variables. For example, in the case of FIGS. 5A and 5B, DET1 to DET5 ≠ 0 as can be seen from FIG. In the case of FIG. 5 (c), it can be controlled by any of DET3 to DET5.

Based on the above idea, the discriminator 200 inputs the current joint angle ΘA of the robot and the command speed D, and performs an operation according to the flowchart shown in FIG. The command speed (operability command value) D 201 in FIG. 2 is input (given) by the operator, and the joint angle ΘA of the robot is the actual angle read from the position detector of each axis of the robot. (Position).

At 202 and 203, the vicinity of the singular point is determined from DET0 = detJ (Θ). If | DET0 | <ε0 at 204, the priority of the hand command speed (order of which of the work variables p1 to p6 is controlled) is determined. | DETi is calculated from the highest one, and i is selected to be | DETi (degenerate Jacobian matrix) | εi (appropriate small value) at 205 and output to the inverse transform calculator 110.

The calculation at 204 in FIG. 2 calculates DETi according to the priority when the robot is actually operated and enters the vicinity of the singular point.

This is because the higher priority DETi is not always | DETi |> ε. I with the highest priority satisfying | DETi |> ε
Is output from the discriminator 200. The priority order among the work variables in 204 is input by the operator in advance.

In the inverse transform calculator 110, 301, 3 in the flowchart of FIG.
In 02, Ji to be calculated is determined based on the output signal i from the discriminator 200, and the inverse transform is calculated. At 303 i =
When it is 0, the joint speed is obtained in 304 as before. When i ≠ 0, the θ2 axis is selected as redundant in 305, so 2D = θ2Dold (2Dold is the value at the time of the previous sampling.
2d) and the remaining joint velocity iD Determined by

At 306, the joint velocities determined at 304 and 305 are output as command values to the servo amplifiers of each of the 11 to 16 axes.

In FIG. 3, i = 0 represents a 6 × 6 Jacobi row, i = 1 to 5 represent a 5 × 5 degenerate Jacobi matrix, and the singularity is obtained when | DETi | = 0. DET0 = DETi (i = 1 to 5) indicates a singular point when a 5-axis arm is assumed.

In the above-described embodiment, a specific shaft configuration is described as an example, but other shaft configurations are also possible. The embodiment described above can be applied not only to a robot arm but also to a robot leg.

〔The invention's effect〕

According to the present invention described above, the singular point and its vicinity cannot be controlled in a specific direction, but can be controlled in the remaining directions as before, and the function of controlling the trajectory of the robot is improved. It is possible to provide a trajectory control device for an articulated robot with improved operability.

[Brief description of the drawings]

FIG. 1 is a block diagram of a control device for implementing the method of the present invention, FIG. 2 is a flowchart for explaining the operation of the discriminator of FIG. 1, and FIG. FIG. 4 is a diagram illustrating an articulated robot to which the present invention is applied, FIG. 5 is a diagram illustrating a singular point of the robot, and FIG. 6 is an example of a Jacobian determinant. FIG. 7 is a diagram showing an example of a degenerated Jacobian matrix, and FIGS. 8 and 9 are diagrams for explaining a conventional technique. DESCRIPTION OF SYMBOLS 1 ... Operator, 2 ... Steering lever (including command speed signal generator), 100 ... Conventional inverse conversion arithmetic unit, 110 ... Reverse conversion arithmetic unit of the present invention, 200 ... Discriminator, 21 ... Servo amplifier, 31: Motor, 41: Speed detector, 51: Joint angle detector, 11 to 16: Driving system of θ1 axis to θ6 axis.

 ──────────────────────────────────────────────────続 き Continuing on the front page (72) Inventor Etsushi Sakino 2-1-1, Shinhama, Araimachi, Takasago City Inside the Takasago Research Laboratory, Mitsubishi Heavy Industries, Ltd. (72) Atsushi Tokioka 1-1-1, Wadazakicho, Hyogo-ku, Kobe-shi No. Mitsubishi Heavy Industries, Ltd. Kobe Shipyard (56) References JP-A-60-16385 (JP, A) JP-A-62-189504 (JP, A)

Claims (1)

(57) [Claims] 1. A trajectory control device for an articulated robot, comprising: first input means for inputting a priority order of work coordinates; and second input means for inputting a hand command speed in the work coordinate system.
Means for obtaining a determinant DTEO corresponding to the Jacobi matrix J0 previously obtained from the robot mechanism; and determining whether or not the determinant DTEO is equal to or less than a certain value ε0 (= is a singular point). A means for outputting i = 0 in the above case, and a Jacobi matrix Ji excluding a row and a column corresponding to a joint axis to be fixed and a degenerate working coordinate i determined in advance in the singularity analysis when the singularity is the singularity. Calculating means for calculating the absolute value of the corresponding determinant DTEi in accordance with the priority order of the work coordinates input by the first input means; and the determinant DTEi exceeds a value εi predetermined for each i Means for judging that the number i is exceeded and outputting the number i, and a Jacobi matrix Ji in response to the number i (J0 when i = 0)
Means for converting each joint angular velocity on the basis of: a trajectory control device for an articulated robot.