Robotics & ComputerIntegrated Manufacturing, Vol. 3, No. 4, pp. 381387, 1987
07365845/87/$3.00 + 0.00 Pergamon Journals Ltd.
Printed in Great Britain
• Paper
PATH GENERATION BY RATE CONTROL OF MANIPULATORS SHIYUKI SAKAUE and KOICHI SUGIMOTO Production Engineering Research Laboratory, Hitachi, Ltd., 292 Yoshidacho, Totsukaku, Yokohama 244, Japan This paper proposes a realtime control algorithm, in which both the actual errors of position and of orientation are fed back to the control value calculated for each sampling time. This paper also defines the distance between two orientations, and presents a set of formulas for orientational error using quaternions. An algorithm using screw algebra is developed for the realtime computation of joint velocities. Finally, the accuracy of trajectory control using this algorithm is studied experimentally.
1. INTRODUCTION One basic function of industrial robots consists of moving an endeffector along a straight line between a start point and an end point. A set of data designates the position and orientation of the endeffector in a workspace. Conventional control uses joint displacement feedback control (called position control), with renewal of target displacements at each sampling time. These target values of joint displacements are calculated using the linear interpolation of positions and orientations of two terminal points in the workspace, as well as coordinate transformation between the workspace and the joint displacement space. The actual position and orientation in a sampling time however, is completely ignored in calculating the target value for the next sampling time. Therefore, it is difficult to maintain high tracking accuracy along a straight line because of external disturbances and response delays. Recently a new method was proposed in which the actual position is fed back to determine the target displacements of position control. 1'2 However the following problems still remain: • Actual orientation should also be considered in the determination of control values. • A highspeed computation algorithm must be developed if control values are determined for actual position and orientation for each sampling time because the longer the sampling time, the worse the tracking accuracy and stability of the system.
This paper proposes a realtime control algorithm, in which both the actual errors of position and orientation are fed back to the control value calculated for each sampling time. The distance between two orientations is defined, and a set of formulas for orientational error is deduced using quaternions. An algorithm using screw algebra is developed for realtime computation of joint velocities. Finally, the accuracy of trajectory control using this algorithm is studied experimentally. 2. DISTANCE BETWEEN TWO ORIENTATIONS The position and orientation of an endeffector can be expressed by a position vector p and three unit vectors f, g, and h parallel to the coordinate axes respectively, as shown in Fig. 1. The following relationship exists between the vectors f, g and h: h = f x g.
(1)
z
g
Fig. 1. End effectorpositionand orientation. 381
382
Robotics & ComputerIntegrated Manufacturing • Volume 3, Number 4, 1987
Hence, the two vectors if, g) are sufficient to express an orientation. Assuming the vector fo is equal to the vector ff when it is rotated ~0 around the vector e, ff can be determined by the following equation. ff = cos tp fo + (1  cos tp) ( e o . fo) e + sin q, e × fo
(2) Subtracting fo from both sides of (2) and taking e scalar products lead to the following relationship.
= 0 (4)
(fo  if) × (go  gf)
lifo  if) × (go  gf) l
The numerator on the right side of (5) becomes
(1
When e and h are orthogonal, the right side of (6) vanishes. In this case, e is determined by the following equation. e 
ho x hf
(7)
I ho x hfl
If e is equal to fo, f is replaced by g or h in equation (11). The distance between two orientations is the rotation by ¢p around e expressed by the equations above. 3. R O T A T I O N A L
TRAJECTORY
q (a, ~) = cos ~ + a sin ~.
ERROR
e x ho.
(8)
On the other hand, when e is determined, the rotation angle ~o can be obtained by equation (2). Taking the scalar product of equation (2) and fo leads to the following equation. f t . fo  ( e . fo) 2 1  (e fo) 2.
(13)
If the rotation from (f, g, h) to (if, gf, hf) is rotation ¢p around e after rotation fl of vector b, then by using the following equation, q (b, 13) = cos fl ~ + b sin fl ~
The numerator on the right side of (7) becomes zero when tp is zero. The following equation can then be used:
cos ¢p 
(1 1)
fo) 2 + 1
 cos ~p)(e • ho) e. (6)
= (hohf).e
Therefore, vector e is described by the following equation.
(fo  if) × (go  gf) = 2
•
The rotation from (f, g, h) to (if, gf, hf), in a sampling time movement, is also expressed by a quaternion as follows:
The error between two orientations, fin, go, hn) and (if, gf, hf), is the rotation around the axis in a certain direction. If e is the unit vector parallel to this axis, the following equation can be obtained from equation (3).
e =
e . (fo × if)
if" fo  2 (e
(5)
(3)
= (gogf).e
1
When an endeffector moves from fin, go, ho) to (if, gf, hf) with uniform rotational velocity, the rotational velocity vector is parallel to a certain vector e. Therefore, e is chosen as the rotation axis and determined by the equations deduced in the previous section. The actual axis will, however, be different from e because of response delay and external "disturbances. The error between these two axes should be fed back effectively. When an endeffector reaches (if, gf, hf) by rotation ~0 around e, this rotation can be expressed as follows using a quaternion: ~p . tp q (e, ¢p) = cos ~ + e sin 2" (12)
(fo  i f ) " e : 0
(foff).e
tp = 2 tan
(14)
it can be expressed as follows: q (a, ~) = q (e, ~o) q (b, fl).
(15)
The above quaternion relationship is described by great circles on a sphere with a unit radius, as shown in Fig. 2. The circular arc of length ~/2 corresponds to quaternion q(a, ~). Vector a is vertical to the plane that includes the circular arc and the center of the sphere, fl/2 and ~0/2 correspond to q(b, fl) and q(e, ~0) in the same manner respectively. These
(9)
Further, taking the scalar product of equation (2) and e x fo yields the following equation. if" (e × fo) sin~0 = (e × f o ) ' ( e x f o ) =
e. (fox
1(e.fo)
if)
(10)
2
Angle ¢p is uniquely determined within 2n rad in equations (9) and (10), and expressed by the following equation.
d J0
~/2
Fig. 2. Spherical trigonometry that expresses quaternions.
P a t h g e n e r a t i o n • S. SAKAUE a n d K. SUOIMOTO
383
circular arcs form a spherical triangle by equation (15). Therefore, the following equations can be obtained by using spherical trigonometry: p
at
.
sin ~ sin 0 = sin ~ sm 6
(16)
• ~o sin ~ cos ~at + sin ~at cos ~Pcos (n  6) =  sin ~ cos 0 (17) where 6 indicates the angle between vector a and e. The vector b is minimum when 0 is n/2 rad, and (16) and (17) are simplified as follows: 8
at
sin ~ = sin ~ sin 6
(18)
tan tp = tan ~at cos 6.
(19)
On the other hand, the following relationship exists: q(b, 8) = q(e, tp) q(a, at) at tp at tp = cos ~ cos ~ + sin ~ sin ~ cos 6 at
+ a sin ~ cos tp _ e s i n tp ~ c o s ~at+ a x e s i n ~ s i nat
tp ~.
(20) Since the vector on the right side of (20) is parallel to b, the following equation is obtained when the vector part is replaced by a unit vector using (18) and (19): at
at
acos ~ecos~cost+a
7Fig. 3. Straight line trajectory error. And vh will be obtained by v ( p f  p) vh = .[ pf _ P l + k l c + k2 f c dt
in which the proportional and integral feedback terms are added. In a discrete control system, (24) can be described as follows: vn 
v ( p f  pn) + kl cn + k2 T ~ ci
IPf 
Pnl
at
at
b' = a cos ~  e ( a . e) cos ~ + a × e ( a . e) sin
at
1  sin E~ sin 2 6
(21)
The quantities b and 8, obtained by (18) and (21) respectively, form a set of minimum rotation to q(e, tp). Therefore, b and 8 are defined as the orientational error from a prescribed rotation. 4. TRAJECTORY CONTROL ALGORITHM The endeffector moves to a target point pf along a straight line whose direction is represented by unit vector r. The translational velocity vh is expressed by the following equation, where p represents the actual position, and v the speed (an absolute scalar velocity):
b
ib, I
(26)
at/
f l = 2 s i n x{sin~
(27) 1(a.e)
E
(22)
(23)
(28)
in which e is the rotation axis to be taken. The rotational velocity wh is expressed using the error (b, 8) as follows: w h = w a + k3 f l b + k 4 f 8 b dt
(29)
where w is an absolute scalar value of rotational velocity. Generally, the rotational translational movements terminate simultaneously. To meet this requirement, the rotational and translational velocity must satisfy the following relationship: w l p f  P l = vat.
However, if the position p is not on the straight line r, and the velocity is calculated by (22), the error vector c will be reduced very slowly or not at all. Therefore, error c between position p and the straight line r should be fed back to determine the control value. Error c is given as follows: c = p f  p  { ( p f  p ) . r}r.
at
xesin~cos6 b'
vh= v(pfp) I Pf x P l
(25)
i=~
where T is the sampling time, pn the position to be taken, and cn its eror for the nth sampling time. Orientation can be dealt with in the same manner. The distance from current orientation, (f, g, h), to the target one, (if, gf, hf), is rotation at around a, computed by the equations mentioned earlier. Therefore, error (b, 8) can be obtained by the following equations:
b= sin 6
(24)
(30)
In a discrete system (30) can be expressed as follows: wn = w an + k3 8n b n + k4 T • 8 i bi.
(31)
After calculating the rotational velocity wh and the translational velocity vh, the joint velocity of a robot with six degrees of freedom can be determined as follows:  (01, 02, 03, 04, 05, 06) T.
(32)
384
Robotics & ComputerIntegrated Manufacturing • Volume 3, N u m b e r 4, 1987
The velocity of the endeffector is expressed by the screw (dual vector) Sn in the static coordinate 0  X Y Z as follows: Sn = wh + ~ (vh + p x wh).
Ni=
SI S1
(33)
(Revolutional Pair) (Prismatic Pair)
in which di is the unit vector parallel to the ith joint axis, and ri the position vector of an arbitrary point on the joint axis as shown in Fig. 4.
*i SI
SiI
......
Si
S1 * $2 . . . . . .
i i • i
In this expression, ~ indicates a dual operator, and p a current position. On the other hand, screw Si, designating the ith joint velocity of the robot, is: Si = di + e ri x di or = e di
$2
Si * Si
i i i i
*SI
Si1
i
*$2 ......
Si!
*Si
(37) After an orthogonal basis is determined by the above method, the following relationship exists if i
(38)
k
0
"" x
r,
Therefore, the following expression can be obtained by taking the scalar product of (34) and N6: Sh * N6 06 $6 * N6 (39)
~
Fig. 4. Joint position and orientation.
Screw Sh, which describes the velocity of the end
Generally, the joint velocity can be c o m p u t e d by the following equation, with i = 5, 4, 3, 2, l:
effector, represents the primary coupling of a joint screw, which can be expressed as follows:
6
0i
Sh = V b i Si.
%~ 0 j Sj) * Ni
(Sh 
J+++' Si * Ni
:
(34)
When a scalar product of 2 screws, S1 = si + e sol and $2 = s2 + e so2, is defined as follows, S1 * $2 = sl • s2 + sol • so2
(35)
then the orthogonal basis of a sixdimensional screw space (N1, N2, N3, N4, N5, N6) can be determined by using the computation below: N1 = S1
(36)
(40)
Figure 5 shows a block diagram for the trajectory controller that uses the above algorithm. The joint displacement for each axis is determined by the displacement detector attached to each actuator. The current position and orientation vectors, p, f, g, h, are c o m p u t e d based on the joint displacement 0. Next, the velocity wh and vr are calculated based on target position data (pf, if, gf, hf) and trajectory data (e and r). Then the corresponding joint velocity b is
~f, ~f, %f, ~f
e, r
I of
Velocity
Wh, Vh~ Coordinates ~ _ _ TransformationI I I
I I
Coordinates ~ _ _ Transformationr~~_+p, f, g, h M : Motor TG : Tachometer Generator PE : Pulse Encoder
,
Counter
( ~
I
l
h
I
I
Fig. 5. Trajectory control block diagram.
I
Path generation • S. SAKAUE and K. SUGIMOTO
obtained. The result is used as a set of inputs for the servoamplifiers which control the rotational speed of each actuator. For the movement of an endeffector from a start point (po, fo, go, ho) to an endpoint (pf, if, gf, hf) with acceleration and deceleration, the speeds of velocity wh and vd are determined by the following method. The maximum translational velocity, vmax, is determined first. It is necessary to reduce the speed as the endeffector nears the end point. Therefore, the speed vd will be determined as follows: vd  min {v max, u (Ipf  Pl)}
u ([pf  Pl) = klpf
plm

m = 1 or m = 1/2.
(42)
When m = 1, the speed is reduced exponentially with time. When m = 1/2, it is reduced almost linearly (Fig. 6). However, during acceleration, vd
>~ Vmax 43 .,4
u o
r
~ I
L/'~v
>•
0
Time Fig. 6. Velocity curve.
changes from zero to vmax. Therefore, the actual translational velocity v is given as follows: vn = / ~ v n
~3 xJ Fig. 7. Robot mechanism.
(41)
in which min { } means selecting the smaller of the volumes within braces. The function u is defined as follows:
 1 +/a2 vd
(43)
where ~1 + ~2 = 1, vn is the v value for the nth sampling time, and/~1,/~2 are constants. If the initial value of v is zero and vd remains constant, vn is approximately given as follows: vn = vd (1  e "~1)
(44)
by which the response is exponential (shown by a broken line in Fig. 6). The rotational speed can be obtained by (30) after determining the translational speed.
the hand orientation, is fixed on the 6th joint axis and coaxis, and the origin of the hand coordinates is set at the 6th joint axis and at a position 60 mm from the intersection of the 5th and 6th joint axes. The arm length is 400 ram. The Z axis of static coordinates X Y Z is determined at the 1st joint axis and coaxis; the origin is determined at the intersection of the common perpendicular line of the 1st and 2nd joint axes and the 1st joint axis, as shown in Fig. 7. A DC servomotor is used as robot drive. The servomotor output is decelerated by a reduction gear (harmonic drive), to drive the arm through a link mechanism. Also, the reduction gear output is transmitted by a chain and gear train to drive a wrist with three degrees of freedom. The control system utilizes two Intel 8086 microprocessors. Each microprocessor is equipped with an Intel 8087 floating point processor for performing 80bit floatingpoint operations. A pulsewidth modulation speedcontrol amplifier is utilized as a servoamplifier in which the output of the tachometer generator affixed to the motor, is fed back to the amplifer. The actuator displacement can be obtained by computing the number of pulseencoder pulses, a value used for microcomputer control computation. Figure 8 shows the positional error for a trajectory when trajectory control is performed. The error value is an absolute value of expression (23) for c, that was derived based on the actuator displacement. The start and end points of the trajectory are as follows (the trajectory is the segment that connects these points): po:
fo: 5. E X P E R I M E N T A L
RESULTS
Trajectorycontrol experimentation was performed using a robot with six degrees of freedom. The structure, dimensions, and external view of this robot are shown in Fig. 7. Vector h, which expresses
385
go: h0: pf:
if: gf: hf:
(507.9 (0.824 (0.555 (0.112 (507.9 (0.827 (0.551 (0.112
539 0.566 0.810 0.153 330.6 0.562 0.813 0.153
2448) 0.006) 0.189) 0.982) 245.0) 0.007) 0.189) 0.982).
386
R o b o t i c s & C o m p u t e r  I n t e g r a t e d M a n u f a c t u r i n g • V o l u m e 3, N u m b e r 4, 1987 K]
}<~
:
0
0
.....
:
0.i
0

:
0.2
0
....
:
0.2
0.02
0.07 rad exists when K3 = K4 = 0 (where K3 = k3 T, K4 = k4 T). However, the orientational error was reduced to 0.007 rad by error feedback, as was the positional error. Orientational error feedback, moreover, influences the positional error. The result is shown in Fig. 10. The solid line in Fig. 10 indicates
6 14 o ~4 m
4
c o .,~ 4.) .,q [q o
Kl
F
K
:
I).2
0.02
{}
c;
:
.~
0.2
0
2
2
. . . .
0
1
2
2
0
3
Time
' .',
7
s
'P i m e
:~
Fig. 8. Positional error.
The acceleration and deceleration conditions at this time are/~1 = 0.2,/12 = 0.8, m  1/2, and the sampling time T = 40 ms. The horizontal axis in Fig. 8 indicates the time, and the vertical axis the positional error. The parameter is a feedback constant indicated by a normalized value with sampling times K1 = k l T, K2 = k2 T. The positional error of 4.5 m m when K1  K2 = 0 was reduced to less than 1 m m by error feedback. This is apparent in Fig. 8. With the chained line in Fig. 8, where integral compensation was performed, the error was even reduced to 0.2 m m at steady state. The error is large at the start and end points because of the influence of acceleration and deceleration. Figure 9 shows the measurement result of the orientation error # from the trajectory, when driven under the conditions mentioned above. Under these conditions, the change in orientation is almost imperceptible. However, the operation of each actuator is linked by the influence of the linkage mechanism and gear train. Therefore, it is necessary for all actuators to be operated even if driven with constant orientation. Thus, an orientation error of
.....
:0
0
:0.i
0
0.1 k4 0
:0.2
0
~4 [a3
....
0.02
c o
:0.2
0.05
4J c~ Jc
E o
0
1
2 Time
Fig. 9. O r i e n t a t i o n a l error.
the positional error only during positional error feedback, while the broken line indicates the positional error when both the positional and orientational errors were fed back. The difference between these two cases b e c o m e s obvious for deceleration. Figure 11 shows the positional error from a trajectory when the sampling time was reduced to 30 ms. It is only slightly improved when compared to the experimental result in Fig. 8, in which the sampling time is 40 ms.   " S
6 ~
.....
:
Ki
K,
0
0
0.2
0.<)2
I
t 4 M
,\\\
% c
2~
2
o
o
c~
"2 .........
2
1
T i me
"''
"
;
s
Fig. 11. Positional error w h e n Y  30 ms.
These results are based on data obtained when the endeffector was driven horizontally, and was therefore less influenced by gravity. To show the influence of gravity, the results of vertical m o v e m e n t are shown in Figs 12 and 13. The position and orientation at the start and end points are as follows:
K3 ~3 f~ k4
Fig. 10. Influence o f o r i e n t a t i o n a l error feed back on positional error.
3 s
po: f0: go: ho: pf: if: gf: hf:
(490.2 ( 0.987 ( 0.073 ( 0.143 (464.8 ( 0.959 ( 0.255 ( 0.171
15.3 0.081 0.995 0.052 156.6 0.240 0.968 0.073
155.1) 0.138) 0.063) 0.988) 449.6 ) 0.149) 0.111) 0.983).
Path generation • S. SAKAUE and K. SUOIMOTO K, : 0
0
:
0.01
0.2
The influence of gravity is not evident in the orientational error shown in Fig. 13. This occurs because the operation of the 2nd joint hardly affects orientation, and because the gravity load applied to the actuator that drives the wrist is small.
K2 0
..... : 0 . 1
o= 42
0
1
2 Time
3 s
Fig. 12. Positional error during vertical operation.
The sampling time used was 30 ms, while the other conditions remained as before. Figure 12 shows the positional error while Fig. 13 shows the orientational error. The positional error is largest around 2.2 seconds. The direction of movement of the 2nd joint is changed upward at first, then downward later. The error is increased in the middle because the parameter of the 2nd joint servomechanism fluctuates due to the influence of gravity. Although integral compensation is effective for reducing a steadystate error, it cannot counteract a sudden parameter fluctuation, as shown in the example.
0.1
K~
K~
~ :
0
0
.....
: 0.1
0
:
0.2
0.01
o~~= ~ o.05 ~
o2
o
I
387
2
3
Time
s
Fig. 13. Orientational error during vertical operation.
6. CONCLUSIONS The conclusions are as follows: • A new concept, "orientational error from a given trajectory", was introduced based on analysis using quaternions and a set of computational formulas was obtained. • A robot trajectory control algorithm was developed that performs precise trajectory tracking, by computing both the positional error and orientational error in real time. • Some experimental results showed that tracking accuracy was considerably improved by feedback of the positional and orientational errors. Integral compensation was particularly effective with a steady motion. Positional and orientational errors were 4.5 mm and 0.07 rad, respectively, with conventional control. They were reduced to 0.2 mm and 0.007 rad when the new control method was employed. REFERENCES 1. IIasegawa, K., Mizutani, T.: On the MultiVariable Servomechanism with Autonomous Trajectory Generating Function. Trans. of the Society of Instrument and Control Engineers (in Japanese). 18, 845850 1982. 2. Awa, K., Kurata, T.: Method to Realize Accurate Trajectory Control by a Steering Apparatus Appended to Servomechanismof Rectangular Coordinate System. Trans. of the Japan Society of Precision Engineering (in Japanese). 48, 218223 1982. 3. Brand, L.: Vector and Tensor Analysis, John Wiley & Sons, 1964. 4. Sugimoto, K., Duffy, J.: Application of Linear Algebra to Screw Systems,Mechanism and Machine Theory, 17, 7383 1982.