JPS6160446B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a robot trajectory control system, and more particularly to a control system that automatically moves each point in a three-dimensional space at a constant speed without stopping.

Conventionally, when a robot arm reaches a destination point while avoiding obstacles, it sequentially passes through each position R ₁ to _{R o} in a three-dimensional space specified by teaching or robot language, as shown in Figure 1. I had reached my destination. Here, when passing through each of the positions R ₁ to _{R o} , it is difficult to pass through them continuously, and acceleration, deceleration, and stopping must be performed between each designated point. For this reason, it takes time to move through a large number of designated points, and the form of movement is not uniform, so acceleration and deceleration are large, and the arm tends to vibrate, which has the disadvantage of hindering the ability to obtain desirable movement performance. Ta.

An object of the present invention is to relax the strictness of passing through each specified way point in the movement of a robot's arm, and to allow the operator to arbitrarily specify the amount of relaxation, that is, the amount of nearby passing error. This provides a trajectory control method for a robot that allows the robot to move continuously through points at a smooth speed without accelerating, decelerating, or stopping point by point.

In order to achieve the above object, the present invention moves a controlled object of a robot at a constant linear velocity between a plurality of pre-specified transit positions, and in the vicinity of the predetermined positions, the robot moves the controlled object at a constant linear velocity between the pre-specified transit positions and The orbit is controlled so as to move smoothly at the same circumferential speed as the linear speed on the inscribed circle determined by the nearby passing error amount.

An embodiment of the present invention will be described with reference to the drawings. First, the parameters to be prepared for the operation of the robot arm will be explained with reference to FIG. The following three points are important in clarifying the mutual relationship between these parameters.

() If the arm moves in a straight line between each of the positions R _i-1 , R _i , and R _i+1 at a speed V, then the circumference of the inscribed circle near each position will also have the same circumferential speed. It must move smoothly.

() The operator can arbitrarily give the nearby passing error amount Δi according to the work content and accuracy, and can plan the work while grasping the movement quality.

() By focusing on the fact that a plane is constituted by the positions R _i −1, R _i , and R _{i +1} , and that the above operations can be described within that plane, complex procedures in three-dimensional space can be simplified. To be something.

The contents of each parameter will be explained in detail based on the above three points.

Let R _i be an arbitrary position among designated positions R ₁ to _{R o} in the three-dimensional space that the robot arm passes between the start position and the target position of the movement of the robot arm,
The designated positions before and after that are R _i-1 and R _i+1 . The direction unit vector of the straight line connecting R _i and R _i-1 is e _i-
If e _i is the directional unit vector of the straight line connecting ₁ , R _i and R _i+1 , e _i-1 and e _i are given by the following equations, respectively.

e _i-1 =(R _i −R _i-1 )/|R _i −R _i-1 | ...(1) e _i =(R _i+1 −R _i )/|R _i+1 −R _i |...(2) If the inner product angle determined by the above two unit vectors e _i and e _i-1 is π-2θi, θi is obtained by the following equation.

θi={π−cos ⁻¹ (e _i−1 ·e _i )}/2 (3) This θi is obtained by bisecting the direction change angle of the linear movement at the position R _i . If the distance between the intersection point where the bisector of the direction angle of this linear movement intersects with the inscribed circle of position R _i and position R _i is Δ _i , this Δ _i can be determined arbitrarily by the operator. It is the amount of error.

Let O _i be the position where the movement of the arm switches from linear motion to circular motion, and let L _i be the distance between positions R _i and O _i
Then, this distance L _i is uniquely determined by the bisector angle θ _i and the neighborhood passing error amount Δ _i from the following equation.

L _i =Δ _i・cos θ _i /(1−sin θ _i )...(4) Furthermore, the above-mentioned position O _i and the position P _i-1 where the movement of the arm switches from circular motion to linear motion are the positions R _i , It can be obtained from the following equation using the distance L _i and the unit vector e _i-1 .

O _i = R _i −L _i・e _i-1 ...(5) P _i-1 = R _i-1 +L _i-1・e _i-1 ...(6) Here, the subscript i-1 is This is attached to the parameter determined by the previous position R _i-1 .

Also, the radius D _i of the inscribed circle can be obtained from the following equation.

D _i = L _i ·tanθ _i ...(7) If the speed during linear motion is V, then the angular velocity of circular motion is set to the radius D _i so that there is no change in circumferential speed even if this becomes circular motion. It is determined from the following equation depending on the speed V.

ω _i = _i = V/D _i (8) _i is the displacement angle of circumferential movement.

Furthermore, the vector β _i that defines the surface that moves circumferentially is the unit vector e _i , e _i-1 in the linear direction, and the bisector angle θ _i
It is determined as follows.

β _i = {e _i-1 −e _i・cos2θ _i }/｜sin2θ _i |
...(9) The above unit vector β _i is the unit vector e _i , e _i
−1 and the position R _i and is a vector orthogonal to the unit vector e _i-1 .

If the linear movement distance from P _i-1 to O _i is l _i ,
The distance l _i is determined by the moving time Δt _s per unit distance on a straight line, the number of steps S for unit movement, and the speed V, and the moving displacement angle _i is determined by the moving time Δt s per unit distance on the circumference. _c , the number of steps per unit movement C, and the angular velocity _ωi . That is, l _i-1 = V・Δt _s・S (10) _i = ω _i・Δtc・C (11).

Furthermore, if the moving positions of the linear/circular arm are r _i-1 and r _i , the moving position on the linear motion is determined by the position P _i-1 , the distance l _i-1 , and the unit vector e _i-1 The moving position on the circumferential motion is determined by the position O _i , the radius D _i , the movement _i , the unit vector e _i-1 , and β _i . That is, r _i-1 = P _i-1 + l _i-1・e _i-1 ...(12) r _i =O _i +D _i {sin _i・e _i-1
+(1−cos _i ) β _i } ...(13).

From the above, the mutual relationships among all the parameters to be prepared for the operation of the arm shown in FIG. 2 have been clarified.

Next, the details of the movement of the robot arm based on the above parameters will be explained with reference to the flowchart of FIG.

First, as shown in step (A), the parameters to be determined by the operator are the position R _p ... R _i ... R _o in the three-dimensional space, and the amount of neighboring passing error Δ1...Δ _i ...Δ _o-
1. Linear speed V, linear and circular unit travel time Δ
There are t _s and Δt _c . The neighborhood passing error amount Δ _i may be a value common to each position. By providing these parameters, the robot arm begins to move. Step (B) First step i = 1 (C)
Now, prepare the various parameters mentioned above.

Unit direction vector e _i- for linear movement
₁ , e _i are determined by step (D). The directional displacement angle θ _i of linear movement is determined by step (E). The switching points O _i , P _i and the distance L _i between the switching point O _i , P _i and the position R _i necessary for switching from a straight line to a circle or vice versa are determined by step (F). The radius D _i of the inscribed circle is determined by step (G). The angular velocity ω _i is determined by the step (H). The vector that defines the surface that moves circumferentially is determined by step (I). In this way, the necessary parameters are determined.

Next, the linear movement step starts.

First, set the starting point and target point. i
When = 1 {Step (J)} Replace with P _i-1 = R ₀ .
{Step (K)} If i≠1, go to the next step. When i=N-1 {Step (L)} Replace O _i =R _N . {Step (M)}·Step s=1 {Step (N)}. The linear movement distance l _inax between positions P _i and O _i is determined by step (O). During this moving distance, the length l _i-1 from the starting point at each step number S is determined by the step (P). The position r _i-1 of each step of the moving distance is determined by step (Q). The arm of the robot moves to the movement position r _i determined in this way by the servo function {step (R)} while the movement distance l _i-1 is smaller than the maximum linear movement distance l _inax {step (S) } advances the step number S one step at a time {step (T)},
The steps from step (P) to step (S) are repeated. When l _i-1 becomes equal to l _inax , linear motion ends and shifts to circular motion. However, here i=N
When the value is -1 {step (U)}, the target point has been reached, so the movement is complete and the "OWARI" signal is output.

First, the number of circular motion steps C=1 is set. {Step (V)} The movement displacement angle _i is determined by the step (W). The moving position r _i on the inscribed circle is determined by step (X). The arm of the robot moves to the movement position r _i determined in this way by the servo function. {Step (Y)} While the movement displacement angle _i is smaller than the inner product angle π-2θ _i of the unit vector, {Step (Z)} advances the step number C one by one {Step (Z')} and steps (W ) to step (Z) are repeated. When the movement displacement angle _i becomes equal to π-2θ _i , the circular motion is completed and the next step of linear motion is started. In the next step, i' is changed to i+1 {step (Z'')}, and the process returns to step (D).

From the above, the details of the arm's operation have been clarified.

Next, a control block system diagram for actually controlling these operations will be explained.

FIG. 4 is an overall diagram of the control system. When a start signal from an external control device such as a computer is input to the reading device (address counter) 1, the address counter 1 is reset to the initial state and the address counter 1 is set to 1. In the memory 2, a first parameter set by the operator shown in FIG. 5 is stored at each address. These first parameters are transferred to the coordinate transformation circuit 3. In this coordinate conversion circuit 3, calculations are performed to obtain second preparation parameters necessary for performing linear and circular movement based on the first parameters. These parameters -
is input to the linear movement trajectory output circuit 4 and the circular movement trajectory output circuit 5. The linear movement trajectory output circuit 4 calculates the linear movement distance and 3 based on the above parameters.
The dimensional space movement position is determined and these data are sent to the robot servo drive circuit 6. The circular movement locus output circuit 5 determines the movement displacement angle and the three-dimensional space movement position based on the above parameters, and sends these data to the robot servo drive circuit 6. The robot servo drive circuit 6 receives data from the linear movement trajectory output circuit 4 and the circular movement trajectory output circuit 5 and drives the controlled object of the robot. Here, switching between linear movement and circular movement is determined by the state determination circuit 7. The above is an overview of the control system. In FIG. 4, solid arrows indicate the flow of control signals, and thick arrows with diagonal lines indicate the flow of data.

Next, details of each block will be explained. As mentioned above, the memory 2 stores the contents shown in FIG. For addresses 0 to N, 3
Specified position in dimensional space R _O ~ O _N , addresses 2 to N have neighborhood passing error amount Δ ₁ ~ Δ _N-1 , address 0 has velocity V, linear unit movement time Δt _s , circumferential unit movement time Δt _c is memorized.

The contents of the coordinate transformation circuit 3 of the next block are as follows:
depicted in the diagram. First, necessary data is input from the memory 3 to the input buffer memory 8 in response to the designation of an address from the address counter 1.
For example, as shown in FIG. 7(I), when the count of the address counter 1 is i, the information at the position R _{i-2, which is the content of the address i-2} of the memory 3, is transferred to the address I ₃ of the input buffer memory 8. from I5 to _I5 , the information of the position R _{i-1 at address i-1} is transferred from _I6 to _I8 , the information of the position R _i at address i is transferred from _I9 to _I10 , and the information of the position R _i at address i+1 is transferred from I6 to I8. ₊₁ information is input from _I12 to _I14 , and the neighborhood passing error amount _Δi is inputted to _I15 . Here, in calculating the i address, the reason why up to the position vector R _{i-2 is} required is that
In equation (6), in order to obtain the distance L _i-1 , θ _i-1 is required from equation (4), and furthermore, in order to obtain θ _i-1 , R _i-2 is required from equation (3). This is because R _i-2 is necessary from equation (1) in order to obtain e _i-1 . However, i=
1, the speed V which is i-1, that is the content of address 0, is
The linear unit movement time Δt _s is input _{to I 1 ,} and the circumferential unit movement time Δt _c is input to I ₀ . In this way, parameters necessary for the next calculation in the conversion procedure memory 9 are prepared.

In the conversion procedure memory 9, the input buffer memory 8
The necessary data is called from the arithmetic unit selection circuit 10 according to the type of operator, a desired arithmetic unit is selected from the arithmetic unit storage unit 11, calculation is performed, and the result is stored in the buffer memory 12 for intermediate results. Alternatively, it is stored in the output buffer memory 13. These conversion procedure memory 9, intermediate result buffer memory 1
2. The mutual functional relationship of the output buffer memories 13 will be explained based on a specific example with reference to FIGS. 7 and 8.

For example, the directional unit vector e _i =(R _i −R
_i-1 )/|R _i −R _i-1 |. As shown in FIG. 8, the subtraction operator and the necessary data (R _i )x component and (R _i-1 )x component in the input buffer memory 8 are stored at address C0 of the conversion procedure memory 9. Addresses I ₉ and I ₆ have been entered. I ₉ and I ₆ contain the x components of the position vectors R _i and R _i-1 . These necessary information are address C.
Even if it is called with data within 0, the necessary operation is executed and its output is input to address M0 of buffer memory 12 for intermediate results. This is,
T _1x shown in FIG. Similarly,
The calculation result of the y component is output from the address C1, and the calculation result of the z component is output from the address C2.
1 and M2 in the form of T _1y and T _1z , respectively. Next, the data from the address M0 of the buffer memory 2 for intermediate results is called by the instruction from the conversion procedure memory C3, and the multiplier is selected by the instruction from the multiplication operator, and the operation is carried out so that the data at x ² is transferred to the address. It is input to M3 as T _2x . Similarly, data y ² and z ² are output from addresses C4 and C5 and input as T _2y and T ₂₂ to addresses M4 and M5. Furthermore, the data at addresses M3 and M4 are added at address C6, and the data at address M
The data at addresses M5 and M6 are added at address C7 and input as x ² + y ² + z ² to address M7, and the data at address M7 is squared at address C8 and input to address M8 as W3. be done. Next, at address C9, the data at addresses M0 and M8 are called and divided, and the result is input to address 118 of output buffer memory 13. Address C10, C1
Similarly, the output from 1 is set to address 0.
19,020. In this way, information on the directional unit vector e _i is input to addresses 018 to 020 of the output buffer memory 13. Other preparation parameters are obtained by the same procedure and input to each address of the output buffer memory 13 as shown in FIG. The above is the content of the coordinate conversion circuit 3.

Next, the state determination circuit 7 will be explained. Assuming that the state of address counter 1 is i, i=1
The determination circuit 14 determines whether i=1 or not, and
If =1, a rewriting command is issued as shown in (K) in the flowchart of FIG. That is, the rewriting command circuit 15 rewrites, for example, the x, y, and z components at addresses I ₆ , I ₇ , and I ₈ to addresses 07, 08, and 09. At the same time, a signal indicating that i=1 is input to the linear movement start signal output circuit 16, and the linear movement start signal output circuit 16 receives this signal and issues a drive command to the linear movement locus output circuit 4. If i≠1, the next i=N-1 judgment circuit 17
The signal of address counter 1 is input to the address counter 1. Here i=
If it is N-1, a command to rewrite as shown in M in FIG. 3 is issued. That is, the rewriting command circuit 18 rewrites, for example, the x, y, and z components at addresses I ₁₂ , I ₁₃ , and I ₁₄ to addresses 04, 05, and 06.
At the same time, a signal i=N-1 is output as a movement completion signal. When the signal of the address counter 1 is i≠1 and i≠N-1, the signal is input to the circular movement start signal output circuit 19, and the circular movement start signal output circuit 19 receives this signal and moves the circular movement. A drive command is issued to the trajectory output circuit 5. In this way, the linear movement locus output circuit 4 and the circular movement locus output circuit 5 are driven by commands from the state determination circuit 7. When the linear movement is completed, a linear movement completion signal is output from the linear movement locus output circuit 4.
=N-1 is input to the determination circuit 16. The signal is i
= N-1, the i=N-1 determination circuit 17 outputs a movement completion signal, and when the signal is i≠N-1, the circular movement start signal output circuit 1
The signal is input to 9 as described above. When the circular movement is completed, the circular movement locus output circuit 5 outputs a circular movement completion signal, which is input to the linear movement start signal output circuit 16.

Next, the contents of the linear movement locus output circuit 4 will be explained based on FIG. The linear movement start signal output from the state determination circuit 7 is sent to the latch circuit 2.
It is latched at 0, resets the up counter 21, and drives the upper limit movement length determining circuit 22. The clock signal from the linear movement clock oscillator 23 is input to the up counter 21 by the latched linear movement start signal. On the other hand, after the maximum linear movement distance limax is determined by the upper limit movement length determination circuit 22, the movement distance determination circuit 24 inputs the data of the linear movement distance l _i-1 according to the state of the up counter 21 at a clock cycle Δt. 3 discretely for each _s
It is output to the dimensional position determination circuit 25 and l _i-1 <l _inax determination circuit 26 . The three-dimensional position determining circuit 25 calculates and determines the moving position r _i-1 in the three-dimensional space based on the data of the linear moving distance l _i-1 , sends the result to the robot servo drive circuit 6, and sends the result to the robot servo drive circuit 6. 6 drives the robot using this data. l _i-1 <l _inax judgment circuit 26 judges whether the linear movement distance l _i-1 exceeds the maximum linear movement distance l _inax , and if it exceeds it, it outputs a linear movement end signal to the state judgment circuit 3 and locks the latch. Reset circuit 20. In this way, the linear movement locus output circuit 4 determines the three-dimensional movement position r _i-1 and drives the robot servo drive circuit 6.

Next, the circular movement locus output circuit 5 will be explained with reference to FIG. 11. The circular movement start signal output from the state determining circuit 7 is latched by the latch circuit 27, resets the up counter 28, and drives the upper limit movement angle determining circuit 29. The clock signal from the circle move clock oscillator 30 is applied to the up counter 28 by the latched start of circle move. On the other hand, the upper limit movement angle determining circuit 29
After the maximum circumferential movement angle imax is determined, the movement angle determining circuit 31 inputs the data of the movement displacement angle _i to the three-dimensional position determining circuit 32 discretely at each clock cycle Δt _c according to the state of the up counter 28.
and output to the _i < _inax determination circuit 33. The three-dimensional position determining circuit 32 calculates and determines the moving position r _i in the three-dimensional space based on the data of the moving displacement angle _i ,
Send the result to the robot servo drive circuit 6,
The robot servo drive circuit 6 drives the robot based on this data. _{The i} < _inax determination circuit 33 determines whether the movement displacement angle _i exceeds the maximum circumferential movement angle _inax , and if it does, outputs a circular movement end signal to the state determination circuit 3, and outputs a circular movement end signal to the latch circuit 27.
Reset. In this way, the circular movement locus output circuit 5 determines the three-dimensional movement position r _i and drives the robot servo drive circuit 6. Furthermore, the circular movement end signal from _{the i} < _inax determination circuit 33 of the circular movement locus output circuit 5 is output to the address counter 1.
It is also sent to address counter 1 and increments it by one.

When address counter 1 advances from i to i+1,
Data in memory 2 is sent to input buffer memory 7. That is, in the input buffer memory 7, the contents of addresses _I6 to _I8 are transferred to addresses _I3 to _I5 , the contents of addresses _I9 to _I11 are transferred to addresses _I6 to _I8 ,
The contents of addresses I ₁₂ to I ₁₄ are moved to addresses I ₉ to I ₁₁ , and the contents of addresses I ₁₂ to I ₁₄ are transferred from memory 2 to location R _i+
2 information is written. Further, the neighborhood passing error amount Δi of address _I15 is rewritten to Δi ₊₁ . By inputting the necessary information for the next step into the input buffer memory 7 as described above, each of the circuits described above operates in the same manner based on this information.
Control of linear movement and circular movement is performed. When the robot arm reaches the target point, the address counter i becomes N-1 as described above, and the output buffer memory rewriting circuit 17 changes the switching position to O.
_i is replaced by the final target position R _N . Also, i
The movement completion signal from the =N-1 determination circuit 17 is sent to an external control such as a computer, and the drive of the robot servo control circuit 6 is completely stopped.

As described above, the present invention moves a controlled object of a robot at a constant speed between a plurality of prespecified transit positions, and in the vicinity of the predetermined positions, the amount of passing error in the vicinity of the predetermined position is calculated. By controlling the trajectory to move smoothly at the same circumferential speed as the linear velocity on the inscribed circle determined by Acceleration - deceleration - point by point
The effect is that the vehicle can continuously move through the waypoints at a smooth speed without stopping, and as a result, there is no vibration at the waypoints and favorable motion performance can be obtained.

[Brief explanation of the drawing]

Fig. 1 is a state diagram showing the movement of the robot arm, Fig. 2 is a trajectory diagram of one embodiment of the robot trajectory control method of the present invention, and Fig. 3 is a flowchart for executing the trajectory shown in Fig. 2. 4 is an overall block system diagram of one embodiment of the robot trajectory control system of the present invention, FIG. 5 is a diagram showing the contents of the memory in FIG. 4, and FIG. 6 is a diagram showing the coordinate transformation of FIG. 4. 7 is a diagram showing the contents of each memory in FIG. 6, FIG. 8 is a diagram showing an example of the contents of the conversion procedure memory in FIG. 6, and FIG. 9 is a block diagram showing the contents of the circuit. 4 is a block system diagram showing the contents of the state determination circuit diagram, FIG. 10 is a book system diagram showing the contents of the linear movement trajectory output circuit of FIG. It is a block system diagram showing the contents. 1...address counter, 2...memory, 3...
...Coordinate conversion circuit, 4...Linear movement trajectory output circuit,
5... Circular movement locus output circuit, 6... Robot servo drive circuit, 7... Status determination circuit, 8... Input buffer memory, 9... Conversion procedure memory, 10...
...Arithmetic unit selection circuit, 11...Arithmetic unit storage section, 12
... Buffer memory for intermediate results, 13 ... Buffer memory for output, 14 ... i=1 judgment circuit, 15
... Rewriting command circuit, 16 ... Linear movement start signal output circuit, 17 ... i=N-1 judgment circuit, 18
... Rewriting command circuit, 19 ... Circular movement start signal output circuit, 20 ... Latch circuit, 21 ... Up counter, 22 ... Upper limit movement length determining circuit, 23 ...
...Clock oscillator for linear movement, 24...Movement distance determining circuit, 25...Three-dimensional position determining circuit, 26...
...l _i-1 <l _inax determination circuit, 27...latch circuit,
28...Up counter, 29...Upper limit movement angle determining circuit, 30...Clock oscillator for circular movement, 3
1... Movement angle determining circuit, 32... Three-dimensional position determining circuit, 33... _i < _inax determining circuit.

Claims (1)

[Claims] 1. The controlled object of the robot is moved at a constant linear velocity between a plurality of designated transit positions specified in advance, and in the vicinity of the designated transit position, the controlled object is moved between the specified transit position and the designated transit position set in advance. A trajectory control method for a robot, characterized in that the trajectory is controlled so that the robot moves at a circumferential velocity having the same value as the linear velocity on an inscribed circle determined by a passing error amount.