JPS59194210A - Track controlling method of robot - Google Patents

Abstract

PURPOSE:To facilitate swing motion by representing the revolving center of an articulated robot as a target position vector, a revolving surface as a surface vector, and the initial direction of the arm tip of the robot as a direction vector. CONSTITUTION:A component part 50 to be operated is provided to the arm tip of the articulated robot 10 and this arm tip is put in a rolving motion; the arm tip while facing, for example, the center 51 of revolution move on a revolving track 52 to perform sectional operation. This operation specifies the vector (r) indicating the target position of the arm tip of the robot 10, the direction vector (e) indicating the attitude. The center 51 of the revolution, the surface of the revolution, and the direction of the arm tip are calculated from those vectors (r), (e), and (n), and a CPU calculates the revolving angles of respective joints by denoting the revolving speed of the arm tip of this robot 10 as (w) which revolves while the surface is held in parallel, thereby supplying them to the robot 10 as indication values.

Description

DETAILED DESCRIPTION OF THE INVENTION Technical Field of the Invention The present invention relates to a method for controlling the trajectory of a hand of an articulated robot.

Conventional technology and problems Articulated robots have multiple arms 1 as shown in Figure 1.
1,12. . . . and bending joints 21, 22 that connect each arm. ..., the rotary joints 31 and 32 built into the arm, and the hand 4 attached to the tip of the arm.
0, and performs work such as grasping a part at one position, carrying it to another position, placing it there, or attaching it there. To perform such work, it is necessary to control the position and posture of the hands, and this specifically means adjusting the bending or rotation angle (herein simply referred to as rotation angle) of each joint to an appropriate value. be. It is relatively easy to determine the hand position and posture from the rotation angle of each joint, and can be solved using a coordinate transformation matrix. However, the problem of first determining the position and posture of the hand and then calculating the rotation angle of each joint is not easy because it requires inverse transformation, it becomes non-linear and difficult to analyze, it falls into approximate calculations and has poor accuracy, and it can also be calculated by a computer. Problems arise such as the time is long and the robot moves slowly.

Therefore, the present inventor developed a vector method in which the normal vector of the plane defined by the arms of each part and the direction vector created by each arm are Heas, and the problem of finding the required rotation angle of each joint from the hand position and posture is simplified. We succeeded in solving the problem in a short calculation time.

The details of this vector method are detailed in the separately filed specification, but if we dig deeper into the Lohosoto in FIG. 1, we will see the following.

Treating the arm as a hector, unit direction vectors (also simply referred to as direction vectors) e+ to e4 as shown in the figure. vector symbols are omitted for simplicity), the surface formed by e] and the X axis is π1, the surface formed by sel~e4 is π2, O4
Letting the surface formed by Let it be n. Note that since a rotary joint simply rotates, the direction vectors of the arms before and after it are the same. Furthermore, since the bending joint only bends, O2, e:+, and ea are always on the same plane. Also, the bending joint 24 is of the same type as the bending joints 21.22, etc., but the angle of attachment to the arm is 90° compared to 21.22 etc.
°Different. Since e and n are orthogonal, the inner product is 01, for example (e
There is a relationship such as +・n+)=0. This orthogonal relationship is illustrated in FIG. 2, and this diagram is extremely effective in understanding the mutual relationships among the various parts. In this orthogonal diaphragm, n is placed in the upper row and e is placed in the lower row, and those in an orthogonal relationship are connected by straight lines.

A vector r indicating the hand 40 is the target position, and n and e are the target posture. The arm 11 at the base is fixed in the Z-axis direction, so rz-(0,1°0), e+= (
0,0,1). The vector r is the arm 11, 12,
13, 14. The length of 15 is LI L2. L3. L4.
As L, r=Ltc+++L2e2+L3e3+Laea
+Le.

From these, n2 can be determined as follows.

A=L2e 2+L3 e 3+L4 e a=r
L 1 e+-Le (A-n2)-〇 Therefore, n 2 = Z-n 2(0) n 2O-(e + XA) / 1e t xA l
From the relationship shown in FIG. 2, O4 is O4-±(n2xn)/1n2xnl e2. O3 is R=L2e2+L3e3=A Lae,
Using the relationship of l etc., e 2 = a 1 u+a 2Ve3-b1u→
-b2■ Here u-R/ l R1,v= (n 2 xu)a
+-(lRI2 → (L 2 ) 2 (L 3 ) 2
) / 21RIL 2a2-±f t7 positive bl-(lRI L2a+l/L3 b 2 = -a 2 I-2/ L Above 3, all the directions and surface vectors in Fig. 1 are determined, and from this, each joint 31, Rotation angle θ1.θ of 21, 22, 23, 32.24
2. θ3. θd, θ5°O6 are determined as follows.

θ1=cos'(n+°n2)-cos-
'(n)y O2=CO3' (e 1 ・e 2 ) -cos 1
(O27,) θ3°cos (e2°e3) θa=cos (e 3 e 4) θ5°cos (n2°n) θs = cos' (e a -e) 'The rotation angle can be clockwise or counterclockwise. Positive such as ka.

Negative polarity is involved, and this polarity is determined as follows. As shown in FIG. 3(a), the direction of clockwise rotation from el to O2 with n2 as the axis (O2 is the angle of rotation) is defined as positive. In other words, it is a right-handed system. Therefore, considering the following inner and outer product of vectors, (e+ ・ (e2xn2)) If this is positive, O2>0, and if negative, 02<0.

Therefore, if we represent the symbol to extract only positive and negative signs as Sign, then Z=Sign (e + ・(e 2 x n 2) )
θ2−Z1θ21. Although FIG. 3(a) corresponds to a flexion joint, the figure (bl corresponds to a rotational joint).

In this case as well, take the right-handed system, θ1−Z1θI It can be expressed as Z=Sign(nl·(n2xe1)).

In this way, the robot can relatively easily and quickly determine the rotation angle of each joint from the target position and posture of the hand.

What's more, this robot performs an operation in which it rotates while always pointing its hand toward the center on a circular trajectory centered on the target position.
(This is called round cutting) can be easily performed. Of course, the above operation can be performed by a robot that can control the position and posture of the robot hand. However, in the conventional method in which the position and orientation are indicated one by one using coordinate components, these coordinate components change every moment while moving on the orbit, and it is not easy to obtain them in advance and use them as command values. It is also a process that is difficult to understand visually.

OBJECTS OF THE INVENTION The present invention provides a control method that allows a robot controlled by a vector method to command and execute a round cutting operation in a manner that can be easily grasped visually. The round cutting operation is an important technique when, for example, a part transported on a belt conveyor is recognized by a visual recognition device, and the robot's hands are brought out from the direction in which the part is to be gripped.

Structure of the Invention The robot trajectory control method of the present invention is provided with a vector indicating the target position of the robot hand, a direction vector and a plane vector indicating the posture, and rotation of each joint using the direction vector and plane vector created by each arm. Using an articulated robot that is controlled using the rotation angle as a command value, the center of rotation is given by the target position vector, the turning plane is given by a surface vector, and the initial direction of the robot hand is given by a direction vector. The present invention is characterized in that the robot hand is caused to perform a turning motion around the turning center by giving a turning speed in the form of an angular velocity.This will be explained next with reference to embodiments.

Embodiment of the Invention FIG. 4 is a diagram illustrating a round cutting operation. 10 is the articulated robot shown in Fig. 1, and 50 is a part of the object to be operated, such as a hole into which a part (not shown) carried by the robot is inserted; It is used as a landmark to explain. Figure 4 ib),
fcl, [dl shows only the hand part of the robot, but the main body part is the same as (al). Fig. 4 te+ is a diagram schematically showing the turning movement of the robot hand, and 51 is the center of turning (this is the center of the turn of 50). (also the center) 52 is the orbit of rotation.Stooge 4
0 moves on a circular orbit 52 while always facing the center of rotation (this is the target movement called "round cutting" that is intended to be performed in the present invention). Figure 4 (al, (bl, (cl,
(di is θ−20°, 0°, +2 shown in tel in the same figure.
It is a figure corresponding to 0 degree and +40 degree. In the present invention, the round cutting operation is executed by commanding as follows.

To make a round cut, a cube centered at the tip of the vector r shown in Figure 1 is cut by turning the tip of the cube (this will serve as a knife) parallel to the plane on the plane with no as the normal vector. I will try to do this by doing this. The cutting speed is ω ra
d/sec. Also, the initial values of the hands are ro, no, eo.
Specify. r is the x, y, and z axis components, nO is the Euler angle, and eO is the angle between eo and the X axis. That is, 1
,. −(roX, roy, ro7.) no=(
α, β), eo=(r) Next, no and eo are converted from Euler angles to directions. That is, 0°−(nOX'n03/'no,,)no
x: CO3α cosβ, nQy =
sinα cosβnOz ``' sinβ and assuming that eo is an unknown angle η, eo−(γ, η) − (eo)(, eoy, eoz) Therefore” ox:cosγ cosη+ eoy−stn
Since r cosη, eOy = 00X 'tanT Also, (no-eo)-〇, but eO, is -1 ((noX+n, y-tanr) /n0
7) e! It becomes IX. The angles α, β, T specified in this way
After determining the initial normal and direction vectors no and eo, the direction vector at an arbitrary angle θ)1. =e is e:cos
θ"eo+sinθ'u u= [noXeo], θ=ω・ΔT−N Since the center is the tip of r, rQ=r, so if e is found, n, e, r,
Now that all el+nl have been determined, the rotation angles θ1 to θ6 of each joint can be determined in the manner described above. This is of course determined at intervals of ΔT, and the tip of the robot is located at the arrow 40 in Fig. 4(e).
Move like this.

FIG. 5 is a flowchart showing the above processing procedure. (
a) shows how to control the robot by calculating e from the instruction values ro, α, β, γ, and ω; (bl is the instruction value r;

(C) shows how to calculate θI to θ6 from e2.
The procedure for finding °e3 is shown below.

Figure 6 is a block diagram of the control system of the robot, where CPU is the central processing unit, M is the storage device, APU is the arithmetic data processor, Do and DI are the data I10 ports, and FS
I is a sensor interface, SC is a servo control, PSI is a position sensor interface, S is an interface for various sensors S, TP is a teaching pendant, TPI is its interface, CRT is a display, and KB is a keyboard. r using KB.

Input (α, β, γ) and send it to the CPU as described above.
θ5 is calculated and given to the robot 10 as an instruction value.

As described in detail, according to the present invention, hector r designating the center of rotation, hector no normal to the turning surface, initial direction hector eO1 of the robot hand, turning speed ω (rad/sec
), it is possible to make the robot hand turn along a circular trajectory while the hand direction is directed toward the center of rotation, which is extremely effective in controlling the posture of the robot hand and performing round cutting operations.

[Brief explanation of drawings]

Fig. 1 is a schematic explanatory diagram of the robot targeted by the present invention, Fig. 2 is an orthogonal diagram showing the relationship between directions and surface vectors, Fig. 3 is an explanatory diagram of positive and negative polarity of rotation angle, Fig. 4 5 is a flowchart, and FIG. 6 is a block diagram showing the configuration of the robot control system. In the drawing, 40 is the hand of the robot, r is the target position hector, n and e are the plane and direction hector, θ5 to θ6 are the rotation angles of each joint, and ω is the turning speed. Applicant Fujitsu Co., Ltd. Representative Patent Attorney Minoru Aoyagi Figure 1el e2e3 e4 eFigure 5 (a) (b) (C)

Claims (1)

[Claims] Given a vector indicating the target position of the robot hand, and a direction vector and plane vector indicating the posture, the rotation angle of each joint is determined using the direction vector and plane vector created by each arm, and this rotation angle is used as a command value. Using a controlled articulated robot, the center of rotation is given by the target position vector, the turning plane is given by a surface vector, the initial direction of the robot hand is given by a direction vector, and the turning speed is given by an angular velocity. A method for controlling the trajectory of a robot, comprising causing a hand to perform a turning motion around the turning center.