JP2676721B2 - Control device for articulated robot - Google Patents

Description

TECHNICAL FIELD OF THE INVENTION The present invention relates to a hand position and posture control device for an articulated robot. (Prior Art and Problems) Conventionally, for example, an articulated robot has a plurality of arms 10, 11, 12, ... As shown in FIG. 1, and bending joints 21, 22 ,. ..., revolving joints 31, 32, ... incorporated in the arm, and a hand 40 attached to the tip of the arm. In addition to this, there is also one in which a slide mechanism is incorporated into the arm to make the length of the arm variable. The work content is to grasp a part at a certain position, carry it to another position and place it or attach it, etc., but the freedom required for such movement is 3 axes for determining the hand position, posture control of the hand There are 3 axes and 6 axes in total. In the example of FIG. 1, since there are three flexion joints 21, 22, 23 and three rotary joints 31, 32, 33, it is a 6-joint type, and the above work is possible. If a certain bending or rotation angle is given to each joint, the position and posture of the hand are determined accordingly. One method of controlling the robot is to obtain the position and orientation of the hand by knowing the flexion and rotation angle of each joint. On the contrary, first, the position and orientation of the hand are determined and the position and orientation are set. Another control method of the robot is to obtain the bending and rotation angles of each joint required for the robot. The former is relatively easy to calculate, and the position and orientation of the hand can be obtained using the length of each arm, the bending rotation angle of each joint, the coordinate conversion matrix, and the like. Hereinafter, a method for obtaining the position and orientation of the hand will be described. In FIG. 1, O ₀ −x ₀ y ₀ x ₀ is an absolute coordinate system defined for the robot, and O _i −x _i y _i z _i (1 ≦ i ≦ 6; the same applies below) is each joint. And the coordinate system defined for the hand. θ ₁ , θ ₂ , θ ₃ , θ ₄ , θ ₅ and θ ₆ are in the initial state (θi = ₀₎ when the arm is straightened in the X ₀ axis direction without twisting.
0), that is, when the rotation center axes of the flexion joints θ ₂ , θ ₃ , and θ ₅ are aligned in parallel, the rotation joints θ ₁ , θ
_{4. Even} if θ ₆ rotates, the rotation angle of each joint is treated as 0 °, and the rotation angle of each flexion joint is 0 ° at the same time. It represents the rotation angle of each joint, and L1, L2, L3 + L4, L5 + L6 are The length of the arm 10 indicating the waist height (the length including the upper and lower arms of the rotary joint 31), the length of the upper arm 11, the length of the forearm 12 and the wrist to the root of the hand. Indicates the length of the arm 13. Vector of hand position / posture (Here, “α, β, γ angle” = “Euler angle”,
^T means a transposed vector. The definition of “Euler angle” is the absolute coordinate system O ₀ −x ₀ according to the description in “Shuhei Goda and Genichiro Kinoshita,“ Robot Engineering ”(Corona Publishing Co., Ltd., August 10, 1977), page 159” The angle formed by the posture vector with respect to y ₀ z ₀ is the Euler angle), and the joint angle vector Θ = (θ ₁ , θ ₂ , θ ₃ , θ ₄ , θ ₅ , θ ₆ ) ^T is given. When The value of is calculated as follows. Fig. 2 shows Euler angles α, β, γ. O _i-1 −x _i-1 y _i-1 z _i-1 to O _i −x _i y _i z _i
Coordinate transformation matrix to Then, the coordinate value of a point at O _i-1 −x _i-1 y _i-1 z _i-1 is O _i −x _i y _i z _i And when Holds. here Is a 4 × 4 matrix as follows. However, Ri is a 3 × 3 matrix that represents the rotation of the coordinate system, Is a three-dimensional vector that represents the translation of the coordinate system, Is a three-dimensional zero vector. Is as follows. Therefore, the total coordinate transformation matrix from O ₀ −x ₀ y ₀ z ₀ to O ₆ −x ₆ y ₆ z ₆ Is Therefore, the position of the hand in the absolute coordinate system O ₀ −x ₀ y ₀ z ₀ Is its position in the coordinate system O ₆ −x ₆ y ₆ z ₆ defined in the hand. Then, it is calculated as follows. Also, as shown in FIG. 2, with respect to the x ₀ axis, y ₀ axis, and z ₀ axis
x _6-axis, y ₆ axes, the direction cosine of z ₆ axes sequentially and _{H1, H2, H3, O 6} -
the coordinates of the origin of x ₆ y ₆ z ₆ Then, the coordinate transformation matrix described above The following relations hold. Therefore, the posture of the hand in the absolute coordinate system O ₀ −x ₀ y ₀ z ₀ Is the same as the posture of the coordinate system O ₅ −x ₅ y ₆ z ₆ defined in the hand, α, β, γ are Is calculated. As described above, given the joint angle vector Θ, the hand position / orientation vector Can be uniquely determined. However, the latter method is considered important from the viewpoint of using the robot. That is, in the robot, the operation is designated by teaching data. That is, the robot is moved by using a teaching box or the like, or by holding the tip of the robot to store the joint angle of each arm at the coordinates of each point, and then operating the robot based on the stored teaching data. ing. However, such a teaching operation of the robot takes time and the operation is very troublesome. For this reason, in recent years, robot movements have been designated by robot language or CAD (Computer Aided Decode).
sign) I am trying to do according to the output. That is, this output is a command value only for a specific movement amount, and the robot is required to be able to perform an operation according to this command value. However, with this method, the rotation angle of each joint must be obtained from the given position and posture of the hand, and the calculation is difficult. Hereinafter, a conventional method for obtaining the rotation angle of each joint when a vector of the position and orientation of the hand is given will be described. As described above, according to the present invention, the Euler angle is the absolute coordinate system.
Since it is defined by O ₀ −x ₀ y ₀ z ₀ , as shown in FIG. The origin of the coordinate system defined for the hand, given Is determined as follows. From the definition of Euler angles (α, β, γ), the absolute coordinate system O ₀
The transformation matrix of the rotation around the z ₀ axis, the y ₀ axis, and the x ₀ axis in −x ₀ y ₀ z ₀ is It is. At this time, X _{6 of} the coordinate system O ₆ −x ₆ y ₆ z ₆ defined by the hand
Direction cosine for axis, Y ₆ axis, Z ₆ axis Becomes Where the coordinate transformation matrix The following relations hold. From the above, Is obtained. The joint angles of the respective joints are obtained by using the respective expressions relating to ▲ x ⁰ ₆ ▼, ▲ y ⁰ ₆ ▼, and ▲ z ⁰ ₆ ▼ obtained above. First, Origin of the coordinate system defined by the fifth joint Ask for. From this, θ ₁ is next, Since the fourth column vector of is equal, This is transformed into the following. Next, perform ² + ² calculation, Get. Substitute in Get. Apply the composite formula of sine and cosine to the left side (Here, φ = tan ^-1 (Q / R)) Therefore, θ ₂ is Also, from the above, θ ₃ is Is obtained. Then for the X ₆ axis Since the first column vector of is equal, Next, the operation of cos θ ₁ × + sin θ ₁ × is performed, and cos (α−θ ₁ ) · cos β = cos (θ ₂ + θ ₃ ) · cos θ ₅ −sin (θ ₂ + θ ₃ ) · cos θ ₄ · sin θ ₅ ...... sin (θ ₂ + θ ₃ ) × + cos (θ ₂ + θ ₃ ) × sin β · sin (θ ₂ + θ ₃ ) + cos (α−θ ₁ ) cosβ · cos (θ ₂ + θ ₃ ) = cos θ ₅ ... From this, θ ₅ is, θ ₅ = cos ^-1 _{[sinβ · sin (θ 2 + θ} 3) + cos (α-θ 1) · cosβ · cos (θ 2 + θ 3) ] Next, the operation of -sinθ ₁ × + cosθ ₁ × , Sin (α−θ ₁ ) · cos β = sin θ ₄ · sin θ ₅ is substituted into sin β · cos (θ ₂ + θ ₃ ) −cos (α−θ ₁ ) · cos β · sin (θ ₂ + θ ₃ ) = cos θ ₄ · sin θ _{5 From} this, θ ₄ is next, From the relation of the third row vector of Next, the operation of _{cosθ 5 × -sinθ 6 ×, cosβ} · sin (θ 6 -γ) = -cos (θ 2 + θ 3) · sinθ 4 Next, the operation of sinθ ₆ × -cosθ ₆ × , Cos β · cos (θ ₆ −γ) = cos (θ ₂ + θ ₃ ) · cos θ ₄ · cos θ ₅ −sin (θ ₂ + θ ₃ ) · sin θ _{5 From} this, θ ₆ is As described above, the calculation of θ ₁ to θ ₆ is very troublesome, and it is difficult to realize hardware, and there is a drawback that the processing time by software becomes long. Also,
Even if it is made into hardware, it will be specialized, and there is a drawback that this hardware cannot be used when making robots with other joint configurations. Also, the articulated robot can have the position and posture of the hand as desired if it has six joints, but there are obstacles around it and it can take an arbitrary posture while avoiding it, or there are two articulated robots. It is difficult to use 6 joints when performing an emphasizing operation with the main arm. For this purpose, a 7-joint type having more degrees of freedom, that is, having redundant degrees of freedom is effective. (Object of the Invention) An object of the present invention is to obtain the flexion and the rotation angle of each joint for making the position and posture of the hand as desired, without hardware and without increasing the calculation time by a computer. An object of the present invention is to provide a control method for an articulated robot that can perform high-speed operation and can improve accuracy. (Structure of the Invention) The present invention relates to a control device for a multi-joint robot in which a plurality of arms are connected by joints, and a position vector and a posture vector of a hand of the plurality of arms instructed by a host device. Vector calculation means for obtaining the direction unit vector of each arm and each normal unit vector of a plurality of surfaces including each arm, and the rotation angle of each joint based on the direction unit vector and the normal unit vector. It is characterized by comprising a rotation angle calculation means to be obtained and a driving means for driving each joint according to the rotation angle thus obtained. (Embodiment of the Invention) Hereinafter, an embodiment of a control method for an articulated robot on which the control device for an articulated robot of the present invention is based will be described in detail with reference to the drawings. FIG. 3 is a diagram showing the definition of the vector of the 6-joint type robot shown in FIG. A method of calculating a joint angle using a vector will be described with reference to FIG. (1) Joint constraint rule by introduction of surface vector As shown in FIG. 3, the target surface is switched for each rotary joint, so that π ₁ , π ₂ , π ₃ ,
A plurality of planes πi are set like the π plane, and each vector is defined as follows. L ₁ , L ₂ , L ₃ ＋ L ₄ , L ₅ ＋ L ₆ : In order, waist height, upper arm length, forearm length, wrist to hand root The vector of is in a position shifted from the x ₆ axis, but this is because when the air gun or air driver is attached to the robot's hand, its tip position is from the coordinate system of O ₆ −x ₆ y ₆ z _6. This is because it is in a position that is translated in parallel. (A) Flexion joint restraint rules As shown in FIG. 4 (a), the flexion joints share a surface, Because it is orthogonal to The joint angle θk is given by the following equation. (B) Restriction rules for rotary joints As shown in FIG. 4 (b), rotary joints share an axis, Because it is orthogonal to The joint angle θk is given by the following equation. Therefore, in the case of the 6-joint robot having the configuration shown in FIG. Then, the joint angle vector Θ = (θ ₁ , θ ₂ , θ ₃ , θ ₄ , θ
₅ , θ ₆ ) ^T can be determined. (2) Determining conditions for plane vector and direction vector (a) Orthogonal relationship between plane vector and direction vector Arranging plane vectors in the upper row and direction vectors in the lower row, and connecting orthogonal vectors with lines Call it a diagram. In the case of the multi-finished robot having the configuration shown in FIG. 3, when the orthogonal relation is extracted using the joint restriction rule (,) shown in (1), the orthogonal diagram shown in FIG. 5 is obtained. (B) Position vector And the direction vector relationship (C) Rule of length of each arm Since the arms are connected by a rotary joint and a bending joint, the length of each arm is invariable. Therefore, The plane vector and the direction vector are determined using the above three conditions. (3) Derivation of joint angle (determination of plane vector and direction vector) (a) Root vector Is a vector determined by definition and its value is invariant. Where the position vector Is the position and orientation vector of the hand described with reference to FIG. Origin of the coordinate system defined for the hand when is given And Position of the hand in the coordinate system defined for the hand When is given, it is calculated by the following formula. Also, the hand posture vector Since it is defined to be the same as the pose vector of the coordinate system defined in the hand, From the definition of Euler angles, It is. as a result Is a unit vector with the same direction as x _{6 in} Fig. 3, Is a unit vector with the same direction as y ₆ . (C) Plane vector The vector shown in Fig. 3 Is introduced. Present on the [pi ₂ surface, the normal vector of [pi ₂ sides It is. Therefore, no matter how the robot joint bends and rotates, Are orthogonal to each other (the orthogonal diagram is shown in FIG. 5).
This vector Is a known vector represented by the following equation. Therefore, Is determined by the following equation. (D) Direction vector The decision above Is on the π ₂ plane, so it can be expressed as follows. Here, a new vector orthogonal to each other on the π ₂ plane Is defined. At this time, Can be expressed using. Substitute, for, The inner product of and is A = L ₂ a ₁ + (L ₃ + L ₄ ) ・ b ₁ …… 0 = L ₂ a ₂ + (L ₃ + L ₄ ) ・ b ₂ …… Because ,,Than Substituting the coefficient of into, Is obtained. (E) Decision More than Is used to determine the joint angle Θ. Next, the case of a 7-joint type robot will be described with reference to FIG. As is clear from comparison with FIG. 3, the arm
In addition to 11, the rotary joint is added to make the flexion joint 3 and the rotary joint 4 7 joints. This robot is able to perform a movement very similar to that of a human arm. In the figure, 21,22,23 are flexion joints, 31,32,33,34 are rotary joints, Are the direction unit vectors of arms 10 to 13, L ₁ , L ₂ + L ₇ , L _{3 respectively.}
+ L ₄ , L ₅ + L ₆ are the lengths of the arms 10 to 13, respectively, θ ₁ to θ ₇ are the bending and rotation (hereinafter referred to as rotation) angles of each joint, Is the target position of arm tip 40, Is the position and orientation vector of the hand 40. Further, the target surface is switched for each rotary joint (in the present embodiment, it is defined that the surface of the flexion joint is not switched, but as described later with reference to FIG. 7A). The surface is switched for each flexion joint depending on how the surface is defined.) Therefore, π ₁ , π ₂ , π ₃ , π ₄ , π for each cut of the rotary joint.
₅ planes and these normal vectors And The arms 11 and 12 correspond to the arms and elbows, but the line segment connecting these arms and the flexion joints 21 and 23 (this vector is And vector Is also called an invariant vector) The angle of inclination with respect to the axis is η (η = 0 indicates that the triangle is aligned with the Z axis with the joint 22 facing upward). That is the eleventh
As shown in Figure (a), the vector The rotation angle when θ ₃ is rotated in the direction in which the right-hand screw advances about is η, and the rotation direction at this time is the increasing direction of the rotation angle. In the robot having the configuration shown in FIG. 6, by setting the length of the arm between the appropriate rotary joint and flexion joint to 0, the vector The joint 22 can be rotated about the axis. For example, in FIG. 6, the arm length between the rotary joint θ ₁ and the flexion joint θ ₂ may be set to 0 to provide a two-degree-of-freedom motion mechanism. Similarly, between θ ₇ and θ ₃ or between θ ₄ and θ ₅ If the length of each arm between is set to 0, θ ₃ becomes a vector as shown in Fig. 11 (b). Can be rotated about the axis. In this robot, the starting end of the arm 10 is fixed to the origin 0 of the orthogonal coordinate axis, and the arm 10 is fixed upward (Z-axis direction). Therefore the direction unit vector Normal unit vector of plane π ₁ And, when displaying arm 10 as a vector Becomes here Referring to FIG. 7 in relation to FIG. 7, FIG. 7 (a) shows an example in which the bending joints 24 and 25 are attached to the arms 14 to 16 by changing the direction by 90 °, and the arms 14 and 15 have π ₆ On the surface, arms 15 and 16
Is on the π ₇ plane. In flexion joints arms
Regardless of the angle formed by 14,15, since the arms 14,15 are on the same plane, the planes π ₆ and π ₇ are orthogonal to each other, and thus the planes π ₆ ,
π ₇ normal unit vector The inner product of is 0, that is, It is. Also the direction unit vector Because they share the planes π ₆ and π _7. There is also a relationship. here Subscripts, such as _{5, 6,} ... are i, j, ... are those (these _{1, 2, 3,} one ...) can as a generalization, the point is the same even less. The refraction angle θ at the joint 24 is Given by FIG. 7 (b) shows a rotary joint 35 and a flexion joint 24.
In the example using, the arm 14 is on the π ₆ plane, and the arms 15 and 16
Is on the π ₇ plane. The direction unit vector in this case is Because it is orthogonal to It is. The rotation angle θ of the joint 35 is It is. 7 (c) and 7 (d) show a slide joint, and FIG.
Is a parallel slide, and (d) is a general slide. 45 indicates a slide. With parallel slides The length of the arm expands and contracts by the amount of slide. On a general slide Takes a value determined by the structure. In general, the slide amount of the slide joint cannot be calculated from the surface vector and the direction vector. However, if the sliding amount of the slide joint is different from the other joint angles θ ₁ , θ ₂ ,
If there is a condition that is determined independently of θ _3, that is, a condition that the slide amount of the slide joint can be determined without using a surface vector or a direction vector, for example, in a scalar type (horizontal multi-joint type) robot, The present invention can be applied to a robot having a slide joint in the case where there is no slide joint and the vertical end has a slide joint only in the case. Direction unit vector (abbreviated as direction vector) And normal unit vector (abbreviated as surface vector) Are orthogonal to, and these will continue in an articulated robot. This relationship can be seen at a glance by arranging the surface vector in the upper part and the direction vector in the lower part as shown in FIG. 8 and connecting the vectors in the orthogonal relationship by connecting them with straight lines. FIG. 8 (a) is an example of the robot shown in FIG. Orthogonal to It is obvious that the relationship is orthogonal to. FIG. 8 (b) shows the robot of FIG. It is a relationship diagram. This robot has an arm with no revolute joint, and also has a flexion joint 25 with a 90 ° reorientation. Although the relationship is complicated, drawing the relationship diagram in Fig. 8 (b) shows that Are on the same plane, and Are on the same plane, Forms an orthonormal system. Etc. can be immediately understood. This 8th
The orthogonal diagram in the figure shows all the directions of the robot arm direction and all joints. Returning to FIG. 6 again, the vector of the arm 10 is And, if the arms 11 to 13 are represented in a similar manner, the vector representing the origin of the coordinate system defined in the hand is It is. This is a vector It is nothing but η (note that the hand vector Indicates the tip of a painting air gun or screwdriver attached to the hand), and the direction vector representing the coordinate system defined in the hand is The surface vector that represents the coordinate system defined in the hand is And the direction vector of each part from these Can be requested. The formula is as follows. Coordinate system, posture, and tilt angle defined for the hand as a result, Is a unit vector with the same direction as x _{6 in} Fig. 3, Is a unit vector with the same direction as y ₆ . (However, Z ₁ is a code that takes +1 or -1, and details will be described later.) The in the axial [pi ₃ when the joint 22 is rotated is rotated [pi ₃ sides
Normal vector of π ₃ plane rotated as the plane rotates give. By the way, the 7-joint robot shown in FIG. 6 includes one redundant joint and cannot be solved analytically as it is. Therefore, η is given from the outside here. Then, when θ ₇ = 0, η = 0, and at this time And Therefore η is It is the angle between and, that is, cos η. Because it is orthogonal to Also vector of Perpendicular to Are in the same plane, so Is obtained. here The code Z ₁ introduced during the calculation of is effective for moving the robot smoothly and will be explained later. Are introduced as Z ₂ and Z ₃ respectively during the calculation of. These symbols Z (Z ₁ , Z ₂ , Z ₃ ) are, for example, 24 in FIG. 7 (a).
If it is a flexion joint such as, the flexion angle of this joint 24 is θ, and θ = Z | θ | If a rotary joint such as 35 in FIG. 7 (b) is used, the rotation angle of this joint is θ, and θ = Z | θ | Is determined by Where Sing is Indicates that only the sign of the inner product of and is taken. Same direction or opposite direction, so the inner product is positive,
There is a negative value, and the positive and negative values indicate the positive and negative values of θ. next The meaning of Z ₁ in the calculation of is explained. As previously mentioned, Consider the three cases of θ ₂ > 0, θ ₂ = 0, and θ ₂ <0 shown in FIGS. 12 (a), (b), and (c). Note that in order to simplify the explanation, θ _{1 in} FIG.
= 0 °, θ ₇ = 0 °, and θ ₄ = 0 °. This allows
In the case of Figure 12 (a) The direction of is the same as that of the Y ₀ axis, and in the case of Fig. 12 (b) Becomes In the case of Fig. 12 (c) Is the same as the direction of the −Y ₀ axis. Assuming that the robot continuously moves from the state of FIG. 12 (a) to the state of FIG. 12 (c), The direction of was oriented toward the Y ₀ axis, but suddenly turned toward the −Y ₀ axis. By the way, when θ ₇ = 0 ° And before considering the sign Z ₁ . Therefore, do not consider the code Z _1. If we obtain θ ₇ using And in the case of FIG. 12 (a), θ ₁ = 0 °, And θ ₇ = cos ⁻¹ (1) = 0 °. Next, consider the case of FIG. 12 (c). θ ₀ = 0
゜, So, Then, θ ₇ = cos ⁻¹ (1) = 180 °, and the initial assumption of θ ₇ = 0 ° cannot be satisfied, causing a contradiction. This can be a factor that makes control unstable. Therefore, in order to prevent this from happening, the sign of the state of the starting point in FIG. The code is returned to the original when the code changes. Code Z ₁
Is used for this purpose, and Z ₁ = 1 in the state of FIG. 12 (a), And And When the sign of changes, Z = -1, Therefore, the above-mentioned contradiction does not occur. The state of FIG. 12 (b) is generally called a singular point, and the solution of θη becomes indefinite. But in this case Can be avoided by uniquely determining in advance. Operation of ^{_{a 1 = {A 2 + (}} L 2 + L 7) 2 - (L 3 + L 4) 2} / 2A (L 2 + L
₇ ) _{b 2 = -a 2 · L 2} / L 3 (However, Z ₂ and Z ₃ are codes that take +1 or −1, and details will be described later.) Here, the meaning of Z ₂ will be described. For simplicity,
In FIG. 6, assuming θ ₁ = 0 ° and θ ₇ = 0 °, θ ₂
As for the above, three cases of θ ₂ > 0, θ ₂ = 0, θ ₂ <0 are considered as shown in FIGS. 13 (a), 13 (b) and 13 (c). No.
In the case of Fig. 13 (a), Direction is the same as the Y ₀ direction (0,1,0), and in the case of FIG. 13 (b), And in the case of FIG. 13 (c) The direction of is the same as the direction of the −Y ₀ axis (0, −1,0). θ ₁
Is Is the angle formed by Therefore, when the robot continuously moves from the state of FIG. 13 (a) to the state of FIG. 13 (c), Direction changes from the Y-axis direction to the -Y-axis direction, and θ ₁ suddenly changes from 0 ° to 180 °, which is inconsistent with the first assumption that θ ₁ = 0 °. Z ₂ is this When the sign of is changed, the sign is restored and the contradiction does not occur. Next, the meaning of Z ₃ will be explained. In this case as well, in order to simplify the explanation, it is assumed that θ ₄ = 0 °, and regarding θ ₅ , the three cases of FIGS. 14 (a), (b), and (c) are considered. In Figure 14 (a) Faces the Y ₀ axis, and in FIG. 14 (b) In FIG. 14 (c), it faces the −Y ₀ axis direction. As in the case of Z ₁ and Z ₂ , when the robot moves from the state of FIG. 14 (a) to the state of FIG. 14 (c), Since the direction of is changed and there is a contradiction with the assumption that θ ₄ = 0 °, Z ₃ is used to avoid this. The meanings of Z ₁ , Z ₂ and Z ₃ are as described above. The code Z of the place currently used is stored and compared with the code Z of the place to proceed to. Only when the code changes, the Z The code is forcibly returned to the original code. What happens when the code is not forcibly returned to the original code by using such a Z? In order to reach the target position / posture given by the person, the distance between the current position / posture and the target position / posture is reduced from 50 msec to 10 msec.
The robot moves while interpolating at a sampling interval of about 0 msec (this time interval depends on the ability of the robot control computer), but due to the sign reversal, rotation of the joint near 180 ° is required between about 50 msec and 100 msec. And then
With the performance of existing motors, it will be a value that can not be fully rotated. That is, the high-speed control performance is adversely affected. Calculation of θi As is clear from the above calculation situation, most of the calculations are simple, and they are not complicated expressions in which sin and cos are complicated. Therefore, the analysis is easy and can be completed in a short time. For example, Cross product operation, such as Most of the combinational operations of the inner product calculation and the inverse trigonometric function are as follows, and high-speed calculation is possible by making this part hardware. Further, this calculation is common to both the 6-joint robot and the 7-joint robot, and even if this portion is implemented as hardware, this hardware can be used for general purposes. The robot not only specifies the final position and orientation, but also specifies some intermediate points and interpolates during the intermediate points to move along a desired trajectory, so the amount of calculation is enormous. Therefore, unless it is a relational expression that can be calculated quickly, the movement of the robot is slow and unavoidable, but this can be improved. In addition, if the calculation is completed quickly, a large number of intermediate points can be specified, and accurate robot control along the planned trajectory becomes possible. FIG. 9 is a block diagram for explaining an embodiment of a control device for an articulated robot according to the present invention, and FIG.
It is a block diagram which calculates each joint angle in the case of a joint type robot. In the figure, 50 is based on the command value from the host device,
Vector calculation means for obtaining a direction unit vector and a normal unit vector of each joint, 60 is a rotation angle calculation means for obtaining a joint angle θi of each joint based on each vector obtained by the vector calculation means, and 70 is a robot arm 80. It is a drive means for causing the joint angle obtained by the rotation angle calculation means 60 to be obtained. (Effects of the Invention) As described above, in the present invention, the direction vector and the surface vector of each arm are obtained from the target position and posture vector of the hand, and the rotation angle of each joint is obtained from these, so that the calculation is easy. It can be done quickly, and thus the movement of the robot can be made accurate and quick. Needless to say, the present invention is not limited to the robot having the configuration shown in the above embodiment, but can be applied in various ways.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a schematic explanatory view of a 6-joint robot, FIG. 2 is a diagram for explaining Euler angles, and FIG. 3 is a vector definition of the joint robot shown in FIG. Fig. 4, Fig. 7, Fig. 7 are explanatory diagrams of direction vector and plane vector, Fig. 5 and Fig. 8 are explanatory diagrams of orthogonal diagram, and Fig. 6 is a diagram showing vector definition of 7-joint robot, FIGS. 9, 10 and 11 are FIGS. 12 and 13 for explaining the embodiment of the present invention.
FIG. 14 is an explanatory diagram of reference numeral Z. In the figure, 10 to 13 are arms, 21 to 23 are flexion joints, and 31 to
34 is a rotary joint, θ _{1 to} θ ₇ are rotation angles of each joint, 40 is a hand, 50 is a vector calculation means, 60 is a rotation angle calculation means, 70 is a drive means, and 80 is a robot arm.

   ────────────────────────────────────────────────── ─── Continuation of front page    (72) Inventor Makoto Araki               Fujitsu, 1015 Ueodanaka, Nakahara-ku, Kawasaki-shi               Inside the corporation                (56) References JP-A-52-25365 (JP, A)                 JP-A-57-3102 (JP, A)                 JP-A-57-33989 (JP, A)                 "Research on Machinery" Vol. 25, No. 7               (1973) pp. 76-80

Claims (1)

(57) [Claims] Joint multiple arms (10-13,40) (21-23,31-)
34) In the control device of the articulated robot connected to each other, the plurality of arms (10 to 13
Position vector of minion (40) of (0) And posture vector Root vector And the invariant vector between the lateral flexion joints obtained from these vectors Direction unit vector for each arm based on The invariant vector And the direction unit vector Normal vectors of a plurality of planes (π _{1 to} π ₅ ) each including each arm divided by a rotary joint based on Vector calculating means (50) for obtaining the direction unit vector The rotation angle (θ ₂ , θ ₃ , θ ₅ ) of each flexion joint is calculated based on The rotation angle calculation means (60) for obtaining the rotation angles (θ ₁ , θ ₄ , θ ₆ , θ ₇ ) of each rotary joint based on the above, and the rotation angles according to the obtained rotation angles (θ _{1 to} θ ₇ ). A control device for an articulated robot, comprising a drive means for driving a joint.