JPH09207089A - Articulated structure control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To control the attitude of an articulated structure to determine the solution of a joint angle only by assigning some joint of the articulated structure to a target position and to control an attitude in such a manner that an obstacle is avoided when the obstacle is detected during control of an attitude, in the articulated structure to control the attitude of the articulated structure. SOLUTION: A joint angle calculating means 102 determines the joint angle of an articulated structure based on a joint position inputted by a joint position input means 101. An attitude control device 105 sets the joint angle of the articulated structure based on the joint angle determined by the joint angle calculating means 102. When an obstacle is detected by a sensor attached to a joint during control of an attitude, the obstacle is avoided in such a way that a different solution determined by the joint angle calculating means 102 is set at the joint angle.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a multi-joint structure control device capable of remotely controlling a multi-joint structure.

[0002]

2. Description of the Related Art Conventionally, as an articulated structure control device used to control the posture of an articulated structure, Japanese Patent Laid-Open No.
There is a robot controller disclosed in Japanese Patent No. 182712. The robot controller is shown in FIG. 16, and as shown in FIG. 16, the image display device displays the image from the camera and the cursor position which is the position of the mouse. The mouse specifies the hand coordinate of the robot on the screen display device. The coordinate input unit receives input from the mouse. The coordinate conversion unit converts the position input by the mouse into the hand coordinate of the robot.

[0003]

However, in the above configuration, the coordinates of joints other than the tips of the multi-joint structure cannot be designated. Also, the user himself / herself has to determine whether the joints or the tips of the multi-joint structure collide with an obstacle such as a wall while designating the tip coordinates.

The present invention solves the above-mentioned problems. Simply by designating a joint having a multi-joint structure as a target position, a solution of a joint angle that realizes the position is obtained to determine the posture of the multi-joint structure. The purpose is to control.

[0005]

In order to solve this problem, the invention of claim 1 provides a joint position designating means for designating a joint position of a multi-joint structure and a position designated by the joint position designating means. A joint angle calculating means for obtaining a solution of a joint angle of the multi-joint structure based on the joint angle calculation means; and a display device for displaying the posture of the multi-joint structure on the monitor screen based on the joint angle solution obtained by the joint angle calculating means. A posture control device is provided for setting the joint angle of the multi-joint structure based on the solution of the joint angle obtained by the joint angle calculation means.

According to a second aspect of the present invention, when a plurality of joint angle solutions are obtained by the joint angle calculation means, the joint angle selection means compares the joint angle with the joint angle before designation by the joint position designation means and determines the change amount of the joint angle. Select the joint angle solution that minimizes the sum of squares.

According to a third aspect of the present invention, a sensor for detecting an obstacle such as a wall is attached to the joint or the tip of the multi-joint structure to detect the obstacle, and the posture control device prevents the collision from occurring. Set the angle.

According to the invention of claim 4, there is provided a display device for displaying the posture of the multi-joint structure on the monitor screen based on the joint angle obtained by the joint angle calculating means.

According to a fifth aspect of the invention, a camera is provided at the tip of the multi-joint structure in order to obtain an image near the tip of the multi-joint structure.

[0010]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram of a multi-joint structure control device. In FIG. 1, 101 is a joint position designating means, 102 is a joint angle calculating means, 103 is a joint angle selecting means, 104 is a display device, and 105 is An attitude control device 106 is a joystick. The joint position specifying means 101 allows the user to specify the position of the joint or the tip of the multi-joint structure. The joint angle calculation means 102 calculates the joint angle of the multi-joint structure based on the joint position designated by the joint position designation means 101. Joint angle selection means 1
When the joint angle calculation unit 102 obtains a plurality of joint angles, the joint 03 obtains the joint angle of the multi-joint structure based on the joint angle and the position before the joint position is designated by the joint position designation unit 101. The display device 104 displays the posture of the multi-joint structure and an image obtained from a camera attached to the tip of the multi-joint structure based on the joint angle obtained by the joint angle calculating means 102. Attitude control device 1
Reference numeral 05 sets the joint angle of the multi-joint structure based on the joint angle obtained by the joint angle calculation means 102. The joystick 106 is used to indicate the movement direction of the tip of the multi-joint structure. FIG. 2 shows an example of connection between the articulated structure and this device.

The operation of the multi-joint structure control device configured as described above will be described below with reference to the flowchart of FIG. FIG. 5 shows the joint angles of the multi-joint structure. The joint angle is positive in the counterclockwise direction with respect to the reference line,
The clockwise direction is negative. The position of joint A is fixed,
The lengths of the respective parts of the multi-joint structure are L1, L2, and L3, and the angles of the joints A, B, and C immediately before the user inputs the joint position are α1, α2, and α3, respectively. (See FIG. 6) The processing of this device is performed in the order of inputting the joint position, calculating the joint angle of the multi-joint structure, displaying the posture of the multi-joint structure, and controlling the posture of the multi-joint structure. First, in the joint position designating means 101, the user designates the joint position by selecting the joint or the tip of the multi-joint structure on the display device. The joint position is a coordinate (X, Y) on the display device. The mouse is used to select the joint and specify the joint position (step S
101).

Next, the joint angle calculation means 102 calculates the joint angle based on the joint position designated by the user in the joint position designation means 101. It is assumed that the user specifies the position coordinates of the tip D of the multi-joint structure as (X1, Y1). The position coordinate of the joint A is (X0, Y0), and the position coordinate of the joint B immediately before the designation (X0 + L1 * cos (α1), Y
The distance between 0 + L1 * sin (α1)) and (X1, Y1) is DIST.

When DIST> = L2 + L3 (1) The angle of the joint C is 0, AD is one side, and the length of AB is L
1. The three corners of the triangle ABD having the length of BD as L2 + L3 are obtained using the cosine theorem and the inverse function of cos. (See FIG. 7) Two sets of solutions (β1, β2) (γ1, γ2) for joints A and B are obtained from the three angles obtained (see FIG. 8).

L2 + L3>DIST> √ (L2 * L2 +
In the case of L3 * L3) (2) The angle of the joint A remains α1, that is, the position of the joint B is fixed and BD is set to one side B
Triangle BCD with the length of C as L2 and the length of CD as L3
The three corners of are obtained in the same manner as in (1). Then, two sets of solutions (β2, β3) (γ2, γ3) for joints B and C
Is required (see FIG. 9).

DIST <= √ (L2 * L2 + L3 * L
In the case of 3) ... (3) First, let AD be one side, the length of AB is L1, the length of BD is √ (L2 * L2 + L3 * L3), and the angle A of the triangle ABD is (1). Find in the same way. Next, three corners of a triangle BCD having BD as one side, BC length as L2, and CD length as L3 are obtained in the same manner as in (1). (See FIG. 10) Two solutions β1 and γ1 are obtained for the joint A from the solution obtained from the triangle ABD. When the joint A is β1, the joint B, as in (2),
Two sets of solutions (β2, β3) (γ2, γ3) are obtained for C, and when the joint A is γ1, two sets of solutions (δ2, δ3), (λ2, λ3) for joints B and C are obtained. ) Is obtained (see FIG. 11) (step S102).

Next, in the joint angle selecting means 103, when a plurality of joint angles are obtained in the joint angle calculating means 102, the joint angle that minimizes the sum of squares of the amount of change in the joint angle compared with the immediately preceding joint angle. Select. The solutions not selected are retained here. If the joint angle calculation unit 102 has not obtained a plurality of joint angles, this step is omitted.

When DIST> = L2 + L3, joint A,
Two sets of solutions of B and C (β1, β2, 0) (γ1, γ2,
Change amounts S1 and S2 are calculated for 0). Since both angles of joint C are 0, only joints A and B are calculated and compared. S1 = (α1-β1) * (α1-β1) + (α2-
β2) * (α2-β2) S2 = (α1-γ1) * (α1
−γ1) + (α2-γ2) * (α2-γ2) S1 <= S
When 2, the angle of the joint A is β1, the angle of the joint B is β2, and when S1> S2, the angle of the joint A is γ1 and the angle of the joint B is γ2.

L2 + L3>DIST> √ (L2 * L2 +
In the case of L3 * L3), two sets of solutions of joints A, B, and C (α
1, β2, β3) (α1, γ2, γ3), the change amounts T1 and T2 are calculated. Since the angle of joint A is α1, both joints B and C are calculated and compared. T1 = (α2-
β2) * (α2-β2) + (α3-β3) * (α3-β
3) T2 = (α2-γ2) * (α2-γ2) + (α3-
When γ3) * (α3-γ3) T1 <= T2, the angle of the joint B is β2, the angle of the joint C is β3, and when T1> T2, the angle of the joint B is γ2 and the angle of the joint C is γ3.

DIST <= √ (L2 * L2 + L3 * L
In the case of 3), four sets of solutions of joints A, B, and C (β1, β2, β
3) (β1, γ2, γ3) (γ1, δ2, δ3) (γ
1, λ2, λ3) change amounts U1, U2, U3, U
Calculate 4 and sort. U1 = (α1-β1) * (α
1-β1) + (α2-β2) * (α2-β2) + (α3
-Β3) * (α3-β3) U2 = (α1-β1) * (α
1-β1) + (α2-γ2) * (α2-γ2) + (α3
-Γ3) * (α3-γ3) U3 = (α1-γ1) * (α
1-γ1) + (α2-δ2) * (α2-δ2) + (α3
−δ3) * (α3-δ3) U4 = (α1-γ1) * (α
1-γ1) + (α2-λ2) * (α2-λ2) + (α3
When −λ3) * (α3-λ3) U1 is the minimum, the angle of the joint A is β1, the angle of the joint B is β2, and the angle of the joint C is β3. When U2 is minimum, the angle of the joint A is β1, the angle of the joint B is γ2, and the angle of the joint C is γ3. When U3 is minimum, the angle of joint A is γ1, the angle of joint B is δ2, and joint C is
The angle of is δ3. When U4 is minimum, the angle of the joint A is γ1, the angle of the joint B is λ2, and the angle of the joint C is λ3 (step S103).

Next, the display device 104 displays the posture of the multi-joint structure by computer graphic based on the joint angle selected by the joint angle selecting means 103 (step S104).

Next, the attitude control device 105 sets the joint angle of the multi-joint structure on the basis of the joint angle obtained by the joint angle selecting means 103. Obstacles are also detected when setting the joint angle. The obstacles are detected by sensors attached to the joints and the tip, and are detected when the distance between the obstacles and the joints becomes W or less during posture control. (See FIG. 12) When an obstacle is detected, the movement of the joint is stopped so that the joint does not approach the obstacle any more. When the joint B detects, the movement of the joint A is stopped, the movement of the joint B is stopped in the case of the joint C, and the movement of the joint C is stopped in the case of the tip D (step S105).

If an obstacle is detected when the joint angle is set, the process proceeds to the next step, and if not, the subsequent steps are omitted and the process ends (step S106).

Next, the avoidance of the collision after the obstacle is detected will be described with reference to the drawings. When the posture control device 105 detects an obstacle when setting the joint angle, another joint angle held in the joint angle selecting means 103 is selected. If more than one is held, select the one with the smallest amount of change.

When DIST> = L2 + L3, joint A,
Two sets of solutions of B and C (β1, β2, 0) (γ1, γ2,
(Β1, β2, 0) is selected and set for 0), but when an obstacle is detected, (γ1, γ2, 0) is selected (see FIG. 13).

L2 + L3>DIST> √ (L2 * L2 +
In the case of L3 * L3), DIST <= √ (L2 * L2 + L
Similarly, in the case of 3 * L3), if an obstacle is detected,
Another joint angle held in the joint angle selecting means 103 is selected (step S107).

After that, similarly to step S104, the processing is performed in the order of displaying the posture of the multi-joint structure and setting the joint angle.

Next, the designation of the posture of the multi-joint structure by the joystick will be described with reference to the flowchart of FIG. The joystick 106 controls the posture by designating the desired direction of movement of the tip position of the multi-joint structure.
First, the up, down, left and right are specified by the joystick 106 (step S201).

Next, the attitude control device 105 changes the attitude of the multi-joint structure. When the joystick 106 designates an upper position, the angles of the joints B and C remain the same, and the angle of the joint A increases, and when the lower designation designates the joints B and C, the angle of the joint A remains the same. Decrease (Fig. 1
4)), when the right is specified, the joints A, B, and C are rotated as they are, and when the left is specified, the angles of the joints A, B, and C are left unchanged. The joint structure is rotated counterclockwise by dθ (see FIG. 15) (step S202).

If an obstacle is detected when setting the joint angle, the process proceeds to the next step (step S203).

When an obstacle is detected, the posture change is stopped (step S204). The fact that an obstacle is detected is displayed on the display device (step S205).

The designated posture of the articulated structure is displayed on the display device (step S206). According to the above-described embodiment, the joint angle that minimizes the amount of change in the joint angle can be obtained by this joint angle calculation method.
Also, by installing a sensor at the joint position, it is possible to detect obstacles during posture control. In that case,
It is possible to take another posture so as to avoid an obstacle by another solution held when the joint angle is calculated. Moreover, the posture of the multi-joint structure can be easily controlled by the joystick.

When the position of the joint C is designated by the joint position designating means 101, the triangle ABD in the case of DIST> = L2 + L3 is replaced with the triangle ABC and the joint C is replaced.
If the angle of is left as it is before designation, the posture of the multi-joint structure can be similarly obtained.

[0033]

According to the first aspect of the present invention, it is possible to calculate the other joint angles and control the posture of the multi-joint structure simply by designating a certain joint of the multi-joint structure as a target position.

According to the invention of claim 2, when a plurality of solutions exist at the time of calculating the joint angle, the joint angle is compared with the posture before the change,
It is possible to select a joint angle solution that minimizes the sum of squares of the change amount of the entire joint angle.

According to the third aspect of the present invention, when a collision of the multi-joint structure with an obstacle such as a wall is detected, the joint angle can be set and the posture can be controlled so as to avoid the collision.

According to the invention of claim 4, the posture of the multi-joint structure can be displayed on the monitor screen based on the joint angle obtained.

According to the invention of claim 5, it is possible to obtain an image near the tip of the multi-joint structure.

[Brief description of drawings]

FIG. 1 is a configuration diagram of a multi-joint structure control device according to an embodiment of the present invention.

FIG. 2 is a diagram for explaining an articulated structure and an articulated structure according to an embodiment of the present invention.

FIG. 3 is a flowchart for explaining the posture control operation of the multi-joint structure according to the embodiment of the present invention.

FIG. 4 is a flowchart for explaining a posture control operation of a multi-joint structure by a joystick according to an embodiment of the present invention.

FIG. 5 is a diagram for explaining each joint angle of the multi-joint structure according to the embodiment of the present invention.

FIG. 6 is a diagram for explaining the posture of the multi-joint structure and the length of each part of the multi-joint structure before designation of the joint position according to the embodiment of the present invention.

FIG. 7: DIST> = L2 in one embodiment of the present invention
Diagram for explaining a joint angle calculation method of a multi-joint structure in the case of + L3

FIG. 8: DIST> = L2 in one embodiment of the present invention
Diagram for explaining the joint angle calculation result of the multi-joint structure in the case of + L3

FIG. 9 shows L2 + L3> DI in one embodiment of the present invention.
Diagram for explaining the joint angle calculation result of the multi-joint structure in the case of ST> √ (L2 * L2 + L3 * L3)

FIG. 10 shows DIST <= √ in one embodiment of the present invention.
Diagram for explaining the joint angle calculation of the multi-joint structure in the case of (L2 * L2 + L3 * L3)

FIG. 11 shows DIST <= √ in one embodiment of the present invention.
Diagram for explaining the joint angle calculation result of the multi-joint structure in the case of (L2 * L2 + L3 * L3)

FIG. 12 is a diagram for explaining obstacle detection according to the embodiment of the present invention.

FIG. 13 is a diagram for explaining a joint angle when an obstacle is detected in the embodiment of the present invention.

FIG. 14 is a diagram for explaining the posture control when the up or down is designated by the joystick according to the embodiment of the present invention.

FIG. 15 is a diagram for explaining posture control when the right or left is specified by the joystick according to the embodiment of the present invention.

FIG. 16 is a configuration diagram for explaining a robot controller according to a conventional technique.

Claims (5)

[Claims] 1. A joint position designating means for designating a joint position of a multi-joint structure, and a joint angle calculating means for obtaining a solution of a joint angle of the multi-joint structure based on the position designated by the joint position designating means. And a posture control device that sets the joint angle of the multi-joint structure based on the joint angle obtained by the joint angle calculation means. 2. When there are a plurality of solutions of the joint angle obtained by the joint angle calculating means, the sum of squares of the change amount of the joint angle is the minimum as compared with the joint angle immediately before being designated by the joint position designating means. The multi-joint structure control device according to claim 1, further comprising joint angle selection means for selecting a solution of the joint angle. 3. The multi-joint structure according to claim 1, wherein a sensor provided at a joint or a tip of the multi-joint structure detects an obstacle and the joint angle is set so as to avoid a collision. Body control device. 4. The multi-joint structure control according to claim 1, further comprising a display device for displaying the posture of the multi-joint structure on a monitor screen based on the joint angle obtained by the joint angle calculation means. apparatus. 5. An image from a camera provided at the tip of the multi-joint structure is displayed on a display device.
The articulated structure control device described.