JPS6357195B2 - - Google Patents

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a multi-joint arm robot (hereinafter also simply referred to as a "robot"), and in particular, the present invention relates to a multi-joint arm type robot (hereinafter also simply referred to as a "robot"), and in particular, a multi-joint arm type robot (hereinafter also simply referred to as a "robot"). This relates to a method for positioning the tip of an arm of a connected robot.

Multi-jointed arm robots have a large degree of freedom,
They are widely used in industrial robots and other applications because they can perform complex movements and multiple units can work together.

[Prior art and problems to be solved by the invention]

When operating a robot, one of the important issues is to position the tip of the arm to a desired position as quickly as possible. Conventionally, methods for positioning the arm tips of multi-joint arm robots include the so-called learning control method in which a human first moves the arm tip to its target position and memorizes the angle information of each arm at that time; There is a method that calculates the angles of all joints of the arm and controls based on this. However, with the learning control method, it is impossible to memorize all points within the arm's movable range, so smooth movement cannot be achieved, and it also lacks applicability. Furthermore, human effort is required each time a new target position is memorized. On the other hand, the method of calculating and controlling joint angles is
If the number of joints increases, there will be a large number of solutions and calculations will become more complex, so calculations will take longer on a small computer such as a minicomputer or smaller, and in some cases calculations may become impossible. Furthermore, it is wasteful to calculate the angles of all joints in order to determine the arm tip position.

Therefore, an object of the present invention is to provide a novel arm positioning method for a multi-jointed arm robot such as the one described above, which enables the tip of the arm to be rapidly positioned at a specified position by simple calculations. be.

[Means for solving problems]

The arm positioning method according to the present invention includes a plurality of arm pairs including two equal-length arm parts and an intermediate rotary joint that rotatably connects the arm parts to each other, and a plurality of arm pairs that are fixed to a base part of the arm pairs. a base joint that is a base joint that rotatably holds a pair of arms, a connecting joint that rotatably connects the pair of arms, a position of the tip A of the pair of arms, and a target position R where the tip should be positioned. A multi-jointed arm type robot comprising coordinate detection means for detecting each coordinate of the arm pair, the robot is configured to sequentially rotate only one of the root-side and tip-side joints provided on one of the arm pairs. At that time, the coordinates of a plurality of locations where the tip A is positioned are detected by the coordinate detection means, and from the obtained coordinates, the coordinates of the joints on the base side and the tip side that are rotated are determined respectively. a second step for determining a target position at which the tip of the arm pair should be positioned within a range reachable by subsequent arm pairs from the target position R; and a second step for determining the base side joint position of the arm pair. A process of matching the slope of a straight line connecting the joint positions of the base side and the tip side of the arm pair with the slope of the straight line connecting the target position; or the step of matching the slope of the straight line connecting the joint positions of the base side and the tip side of the arm pair; or the process of adjusting the slope of the straight line connecting the target position of the arm pair; Determine each distance to the root side joint position, compare the magnitude of the obtained distances, and determine the first rotation angle of the root side joint and the second rotation angle of the intermediate rotation joint of the arm pair according to the comparison result. and a step of changing the first and second rotation angles by setting the ratio of changes in the first and second rotation angles to 1:2 so that the respective distances are equal to each other; carrying out either one first, and then the other. The third step of executing the step is performed in order from the arm pair on the base side to position each arm pair, and for the arm pair including the arm tip, the target position R is set as the target position to be positioned. The third step is to position the tip of the arm.

In short, by using the isosceles triangular property of an equal-length arm pair, each arm pair is
Positioning is performed pair by pair to finally position the tip of the arm at the target position, which simplifies calculation and control and enables high-speed operation. Also,
The method of the present invention requires a system capable of detecting the position of the arm tip and its target position, and for this purpose, for example, a system for recognizing the position using a television camera can be used. However, since only the tip of the arm and the target position are to be detected, the detection accuracy can be improved and the detection system can also be downsized.

Incidentally, the problem with conventional multi-jointed robots is that when positioning the tip of the arm, all angles of each joint that connects the arm pieces,
If it is a rotary joint, all rotation angles must be determined at the same time. Therefore, by calculating the well-known coordinate transformation, it is possible to calculate the rotation angle of each joint with respect to a given target position (for example, given in the X-Y-Z absolute coordinate system with the base joint of the arm as the reference (origin)). Need to convert. At this time, each joint must be given in relative coordinates with respect to the preceding arm, which requires extremely complicated coordinate transformation processing.

On the other hand, in the method of the present invention, the part of calculating the slope of the straight line and the part of calculating the distance information regarding the root side joint for each arm pair are somewhat complicated, but such calculation processing can be done in a microprocessor. This processing can be fully handled at the processor level.

〔Example〕

DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be described in detail below based on embodiments and with reference to the drawings.

First, FIG. 1 schematically shows the entire system of an example of an articulated arm robot to which the arm positioning method according to the present invention is applied. The illustrated example is a two-joint arm type robot, and on the base 1,
An arm 2 and an arm drive section 3 are provided, each consisting of two arm sections of equal length. Reference numeral 4 denotes a television camera, which recognizes within the field of view 4 the position of the tip A of the arm 2 and the target position R to which the arm tip should be positioned, as will be described later.
Reference numeral 6 denotes an X-Y tracker, which reads the X-Y coordinates of each position recognized by the television camera 4 and outputs them as analog voltages. X-
The output from the Y tracker 6 is inputted through an AD converter 7 to a control device 8 consisting of, for example, a minicomputer or other control circuit. Calculations are performed in the control device 8 by a method described later, and the results are input to the arm drive device 3 so that the arm tip A is positioned at the target position R.
In addition, in order to facilitate recognition by television cameras,
A light emitting element such as an LED is provided at the arm tip A and the target position R. Furthermore, a pulse motor or a DC motor is used for the arm drive section 3.

Now, in principle, the arm positioning method of the present invention skillfully utilizes the following two features.

First, the coordinates of the center point of a circle can be calculated from the coordinates of three points on the circumference, so if only the X-Y coordinates of the tip of the arm are detected, the position coordinates of each joint of the arm can be determined. That's what you can ask for.

The second is the isosceles triangular nature of each pair of arms of equal length.

The above points will be explained with reference to FIGS. 2 and 3. FIG. 2 shows the most basic two-joint arm using only one pair of arms each consisting of two arm pieces of equal length (r). The first joint _P1 of this arm is the base joint of the arm pair, and is also the arm root joint. In this arm, if the second joint angle θ ₂ is kept constant and only the first joint angle θ ₁ is changed, the arm tip A moves on a circle M (indicated by a dotted line) centered on the first joint P ₁ . . So θ ₂
By changing twice, three points A ₁ (x ₁ , y ₁ ), A ₂ (x ₁ ,
y ₂ ), A ₃ (x ₃ , y ₃ ). At this time, if the coordinates of the first joint P ₁ are (x ₀ , y ₀ ) and the radius of the circle M is r ₀ , then the following three equations hold true.

(x ₁ − x _n ) ² + (y ₁ − _{y n} ) ² = r ₀ ² (x ₂ − x _n ) ² + (y ₂ − y _n ) ² = r ₀ ² (x ₃ − x _n ) ² +(y ₂ −y _n ) ² = r ₀ ^2From these three equations, x _n =(y ₂ −y ₁ )(x ₁ ² −y ₁ ² −x ₃ ² −y ₃ ² )−(y ₃ −
y ₁ )(x ₁ ² +y ₁ ² −x ₂ ² −y ₂ ² )/2{(x ₂ −x ₁ )(y ₃ −y ₁
) − (x ₃ − x ₁ ) (y ₂ − y ₁ )} y _n = (x _{2 − x 1} ) ⁽ _{x 1} ^{2 +} _{y 1} ² −
x ₁ ) (x ₁ ² +y ₁ ² −x ₂ ² −y ₂ ² )/2{(x ₃ −x ₁ )(y ₂ −y ₁
)−(y ₃ −y ₁ )(x ₂ −x ₁ )}, and the coordinates (x _n , y _n ) of the first joint P ₁ are determined.

When the position of the first joint _P1 is determined as above,
Using this position as a reference, the tip of the arm can be easily positioned using the properties of an isosceles triangle. Now, as shown in Figure 3, the first joint P ₁
Considering the case where the arm tip A moves from position A1 to A2 along the straight line N (indicated by a dotted line) passing through , ΔP ₁ P ₂₁ A1 and ΔP ₁ P ₂₂ A2 are both isosceles triangles, so The following formula holds true.

θ ₂₁ +2 (θ ₁₁ −α) = 2π θ ₂₂ +2 (θ _{12 −} α) = 2π ∴θ ₂₁ −θ ₂₂ = −2 (θ ₁₁ −θ ₁₂ ) In other words, along the straight line passing through the first joint P ₁ When moving the arm tip A, the angles θ ₁ and θ ₂ of the first and second joints are changed by the change in the first joint angle (θ ₁₁
-θ ₁₂ ) and the change in the second joint angle (θ ₂₁ -θ ₂₂ ) may be controlled so that the relationship is 1:-2. Further, as shown in Fig. 2, when moving the arm tip A along a circle centered on the first joint _P1 , the angle _θ2 of the second joint is kept constant, and the first joint angle It is sufficient to change only θ ₁ .

The fourth positioning method for a two-joint arm according to the present invention utilizing the above-mentioned features will be described below.
This will be explained in detail with reference to the drawings. FIG. 4 shows the case where the arm tip A is positioned from the current position (x, y) to the target position (a, b). However, the range of movement of the arm tip A is y>0.

(1) First, find the position (x _n , y _n ) of the first joint P ₁ from the three points A1, A2, and A3 of the arm tip when θ ₂ is kept constant and θ ₁ is changed twice. .

(2) Next, the arm tip A and the target position R are aligned with respect to the first joint P ₁ (x _n , y _n ).
Figure 4 shows the movement from A to A'. In other words, the function f = (x-x _n )/representing each direction of the arm tip A and the target position R with respect to the first joint P ₁
(y-y _n ) and F=(a-x _n )/(b-y _n ), and perform the following control.

(b) When f>F, keep θ ₂ constant and increase θ ₁ .

(b) When f<F, θ ₂ is kept constant and θ ₁ is decreased.

Then, the above control is performed until f=F.

(3) After completing the orientation, next
The distance between A' and the target position R is adjusted. Figure 4 shows the movement from A' to R. In other words, the tip of the arm
The function l = _{(x-x n )} ² + (y
-y _n ) ² and L=(a-x _n ) ² +(b-y _n ) ² to perform the following control.

(a) When l>L, θ ₁ is increased, θ ₂ is decreased, and the ratio of the changes (absolute values) is set to 1:2.

(b) When l<L, θ ₁ is decreased, θ ₂ is increased, and the ratio of the changes (absolute values) is set to 1:2.

Then, the above control is performed until l=L.

In this way, the alignment of the arm tip A to the target position R is completed. In the above explanation, θ ₁ and θ ₂ are treated as positive in the counterclockwise direction, and
The range was ≦θ ₂ ≦π. If π≦θ ₂ ≦2π, then
In some cases, the terms "increase" and "decrease" are interchanged.

The above method first performs direction adjustment and then distance adjustment, but conversely, there is a method in which distance adjustment is performed first and then direction adjustment (see A in Fig. 4).
→A″→R), exactly the same positioning is possible. In short, it is sufficient to perform either distance adjustment or orientation adjustment first, and then perform the other one.

Now, as another example, the case of a four-joint arm formed by connecting two pairs of arms is shown in Figures 5 and 6.
This will be explained with reference to the figures. Incidentally, in an articulated robot, the position accuracy is determined by the accuracy of the joint angle, so the longer the arm piece, the worse the accuracy of the tip position. However, the reach of the arm will expand. On the other hand, the shorter the arm piece is, the better the accuracy of its tip position will be. However, the reach range of the arm is narrow.
The four-jointed robot shown in the figure compensates for these shortcomings, and has two long arm parts (length r ₁ ).
This arm structure is formed by connecting a first pair of arms consisting of a pair of arms and a second pair of arms consisting of two short arm pieces (length r ₂ ). The first joint of this arm P ₁
is the arm base joint and is the base joint of the first arm pair, and the third joint _P3, which is the second joint from the first joint, is the base joint of the second arm pair.

Positioning in this four-joint robot is performed in the following manner. First, a method for calculating the positions of the first joint _P1 and the third joint _P3 will be explained with reference to FIG. For the first joint P ₁ , θ ₂ ,
When θ ₃ and θ ₄ are kept constant and only θ ₁ is changed twice, the arm tip positions A ₁ , A ₂ , and A ₃ are detected, and the coordinates (x _n , y _n ) are calculated. seek. 3rd joint
Regarding P ₃ , the arm tip positions _{A 1} , A ₄ , when only θ ₃ is changed twice while keeping θ ₁ , θ 2 , and θ ₄ constant are
Detect the three points _A5 and calculate the coordinates (x _i , y _i ).

FIG. 6 shows the case where the arm tip A(x, y) is positioned at its target position R(a, b), which is carried out in the following procedure. However, the coordinates of the first joint P ₁ and the third joint P ₃ are calculated by the method shown in FIG. 5 each time the robot moves, but their description will be omitted in the following explanation.

(1) First, with reference to the first joint _P1 , which is the base joint of the first arm pair, the base joint (third joint) _P3 of the second arm pair is set to the target position R( Target position P ₃₀ (intermediate target position) within an area of radius 2r ₂ (i.e., within the reachable range of the second arm pair centered on the third joint P ₃ ) from the final target position)
position.

Note that the target position P ₃₀ of the third joint P ₃ is set as follows. First, the target position P ₃₀ needs to be within the reachable range of the second arm pair as described above.

On the other hand, the target position P ₃₀ is within this reach range.
It is desirable to determine the positioning operation so that the positioning operation of the second arm pair to the target position R can be performed at high speed. Such a point is preferably a point on the circumference of a circle having a radius _r2 , which is half the distance of the reach range _2r2 of the second arm pair, with the target position R as the center. This is because in this case, no matter what the posture of the second arm pair is, the movement distance to the target position R is short and high-speed operation is possible.
This point is geometrically obvious.

Furthermore, since there is a limit to the operating range of the first pair of arms, it is desirable that the range be between the base joint _P1 of the first pair of arms and the target position R.

Therefore, the point where the straight line connecting the base joint P ₁ and the target position R intersects with the circle of radius r ₂ is the target position P ₃₀
It is desirable to do so.

Based on the target position _P30 set as described above, the arm tip A is positioned at the silent evaluation position R as follows.

(b) First, the first joint P ₁ (x _n , y _n ) is used as a reference to align the third joint P ₃ (x _i , y _i ) and its target position P ₃₀ (c, d). I do. 6th
In the figure, it is the movement of P ₃ →P ₃ ′. In other words, the function f ₁ = (x _i - x _n )/representing the respective directions of the third joint P ₃ and the target position P ₃₀ with respect to the first joint P ₁
(y _i −y _n ), F ₁ = (c−x _n )/(d − y _n ), and only the first joint angle θ ₁ is changed until f ₁ = F ₁ , and the third joint angle is Move P ₃ to position P ₃ ′.

(b) Next, the distance between the third joint P ₃ ′ and the target position P ₃₀ is adjusted. Figure 6 shows the movement from P ₃ ' to P ₃₀ . In other words, the third joint P ₃ ′ and the target position P ₃₀
The function l ₁ = (x _i ⁻ x _n ) ² + (y _i − y _n ) ₂ , L ₁ = (c−
x _n ) ² + (d-y _n ) ² and change the first and second joint angles θ ₁ and θ ₂ until l ₁ = L ₁ (the ratio of changes is 1:- 2), 3rd joint
Move P ₃ ′ to target position P ₃₀ . When the positioning is completed, the first and second joints P ₁ and P ₂ are locked, and the entire first arm pair is fixed.

(2) Next, the arm tips A are positioned based on the positioned base joint P ₃₀ of the second arm pair. In this case, the second arm pair is moving as shown by the dotted line in FIG. 6, and the arm tips at this time are at the position A _p (x _p , y _p ).

(b) First, the arm tip A _p (x _p , y _p ) and the target position R are calculated based on the base joint P ₃₀ (c, d).
Perform orientation with (a, b). In Figure 6, A _p
→A _p ′ movement. This is the arm tip A _p
and a function f ₂ = (x _p - c) / (y _p - d), F ₂ that represents the direction of the target position R with respect to the base joint P ₃₀ .
= (a-c)/(b-d), f ₂ =
Only the third joint angle θ ₃ is changed until F ₂ is reached, and the arm tip A _p is moved to the position A _p ′.

(b) Finally, align the arm tip A _p ' with the target position R. Figure 6 shows the movement from A _p ′→R. In other words, the function l ₂ = (x _p - c) ² + (y _p - _d ) ² , L ₂ = (a - c) 2 expressing the distance between the arm tip position A _p ' and the target position R with respect to the basic function P ^30.
+(b-d) ² , change the third and fourth joint angles θ ₃ and θ ₄ until l ₂ = L ₂ (ratio of changes is 1:-2), and adjust the arm tip A _p is moved to the target position R. As a result, the arm tip A is finally positioned at the target position R.

Although not shown, even if the equal-length arm pairs are further connected to the third, fourth, ..., n-th pair, the base joint P ₃ of the second arm pair is determined in the same way as above.
Starting from the positioning of the base joint P _2k-1 of each arm pair
By positioning (k=1, 2, . . . , n), the tip of the arm can finally be positioned.

As described above, according to the method of the present invention, calculations for positioning the end of the arm of an articulated arm robot are performed using only four very simple arithmetic operations, and therefore a computer smaller than a minicomputer can be used. , and the calculation time is shortened. Moreover, as a particularly important advantage, since the arm position can be detected only by the arm tip A and its target position, detection accuracy can be improved and the detection device can be downsized. As a result of the above, the entire robot can be made smaller, lower in price, and higher in performance. Furthermore, the method of the present invention allows positioning to all positions within the reachable range of the arm, and has very high applicability.

[Brief explanation of the drawing]

FIG. 1 is a schematic diagram of the entire system of an example of a multi-joint arm type robot to which the arm positioning method according to the present invention is applied, and FIGS. 2 and 3 are related to the principle of an equal-length two-joint arm. Fig. 2 shows how to obtain the coordinates of the base joints, Fig. 3 is a diagram for explaining the basic positioning operation, and Fig. 4 is a diagram showing the arm positioning method according to the present invention in a two-joint arm type robot. , FIG. 5 and FIG. 6 relate to a four-joint arm type robot, in which FIG. 5 shows how to determine the coordinates of the base joints, and FIG. 6 is a diagram showing the arm positioning method according to the present invention. Pi (i = 1, 2, ......) ...Arm joint, A
...Arm tip, θ _i (i=1, 2, ......)...
Angle of arm joint, R...Target position of arm tip.

Claims (1)

[Scope of Claims] 1. A plurality of arm pairs including two equal-length arm pieces and an intermediate rotary joint that rotatably connects the arm pieces to each other; and a plurality of arm pairs fixed to a root portion of the arm pairs. A root joint that is a base joint that rotatably holds the pair of arms, a connecting joint that rotatably connects the pair of arms, and a position of the tip A of the pair of arms and a target position R where the tip should be positioned. A multi-jointed arm robot comprising: coordinate detection means for detecting each coordinate; At that time, the coordinates of a plurality of locations where the tip A is positioned are detected by the coordinate detection means, and from the obtained coordinates, the coordinates of the joints on the base side and the tip side that are rotated are determined respectively. a second step for determining a target position at which the tip of the arm pair should be positioned within a range reachable by subsequent arm pairs from the target position R; and a root side joint position of the arm pair. A process of matching the slope of a straight line connecting the joint positions of the base side and the tip side of the arm pair with the slope of the straight line connecting the target position; or the step of matching the slope of the straight line connecting the joint positions of the base side and the tip side of the arm pair; or the process of adjusting the slope of the straight line connecting the target position of the arm pair; Determine each distance to the root side joint position, compare the magnitude of the obtained distances, and determine the first rotation angle of the root side joint and the second rotation angle of the intermediate rotation joint of the arm pair according to the comparison result. and a step of changing the first and second rotation angles by setting the ratio of changes in the first and second rotation angles to 1:2 so that the respective distances are equal to each other; carrying out either one first, and then the other. and a third step of executing the above in order from the arm pair on the base side to position each arm pair, and for the arm pair including the arm tip, set the target position R as the target position to be positioned. A method for positioning a robot arm, characterized in that the third step is performed to position the tip of the arm.