Mechanism and Machine Theor) Vol. 18, No. 1, pp. 5742, 1983 Printed in Greal Britain.
0094114X[83/010057~6503.00[0 © 1983Pergamon Press Ltd.
ON THE FORCE FEEDBACK CONTROL OF A MANIPULATOR WITH A COMPLIANT WRIST FORCE SENSOR H. OZAKI,* A. MOHRI:~ and M. TAKATA§ Department of MechanicalEngineeringProductionDivision,Faculty of Engineering,Kyushu University36, 6101, Hakozaki, Higashiku,Fukuoka, Japan
(Received[or publication 20 April 1982) AbstractThe force feedback algorithmis presented for the manipulatorwhich consists of a high stiffnessposition servo system with sixdegreeof freedom and has a compliantsixcomponentforce sensor at its wrist. The obtained algorithm has the following three features. First, this algorithm gives the relation between the position and orientation of the wrist at the present step and that of the wrist to be attained at the next step. Second,an efficient calculation method to obtain each joint movement satisfyingthis relation is proposed. Third, this algorithmreduces the force feedback control to the position feedback control of each joint in case the force sensor is sufficiently compliant compared with its environment.
I. INTRODUCTION
In addition, above two accommodation methods take account of the stiffness between a manipulator and its environment. The positional relation between them, however, varies its stiffness. Therefore it is not easy to estimate the stiffness even if the environment is known in advance. From this point of view, the force feedback strategy with the stiffness neglecting the influence of the environment seems more universal. In this paper an accommodation algorithm is obtained for the manipulator which consists of a high stiffness position servo system with sixdegree of freedom and has a compliant sixcomponent force sensor at its wrist. In this type of manipulator, the compliance is represented only by that of the force sensor. The design and the production of the force sensor used here are relatively easy, because it need not have such a common coordinate system as Whitney [5] has used to unify compliant motion and force information.
The usefulness of the positioncontrolled multijoint manipulator has been confirmed in many fields. This type of manipulator, however, cannot be applied to the tasks constrained by uncertain or variable environments. In these cases one solution is to provide the manipulator with a force sensor and to control its joints based on the measured forces. The applicability of the manipulator to complex tasks depends on how to connect the force feedback with the position control or how to convert the measured force into a motion. The sixdegree of freedom manipulator with a sixcomponent force sensor has theoretically sufficient information and manipulation flexibility. Nevertheless, there is not yet a satisfactory force feedback strategy called accommodation. On the other hand some studies are turned to practical alternatives in order to settle an important and essential pegintohole problem in industrial assembly tasks. One of them is based on the principle of the passive accommodation using a builtin compliant wrist[l, 2]. Another one has an actuator in each force sensing direction at a wrist and simplifies the active accommodation[3, 4]. But these have the following disadvantages. Namely, the former lacks universality and the latter increases the number of actuators. As one of the accommodation methods, Whitney[5] proposed the diagonal force feedback gain matrix which operates on a measured forcevector and creates modified commands. This method is very simple but components of its matrix must be selected corresponding to the given task and affect the stability of the force feedback system. Paul[6] proposed the algorithm in which the joints movements are matched to the outputs of the forcesensing elements using the stiffness in the task coordinates and compliant motion is determined based on these movements. In this algorithm its matching processes complicate the whole procedure.
2. FORCE FEEDBACK ALGORITHM USING COMPLIANCE OF FORCE SENSOR
Various types of the structure of the manipulator with sixdegree of freedom are proposed. Figure 1 shows three representative examples. A compliant force sensor is attached to each manipulator's wrist, namely the part between joint 6 and its hand. The control of these manipulators with sensors essentially requires online digital computation. A computational force feedback algorithm is given in the following. 2.1 Force balancing strategy Let a manipulator be at kth sampling time. The condition that the manipulator can exert a given force (actually forces the torques) on its environment at k + lth sampling time are discussed as follows. The relationship between two adjacent coordinate frames, frame n fixed to nth link and frame n + 1 fixed to n + lth link is described by the following 4 x 4 transformation matrix [7]; that is
tResearch Assistant. *Associate Professor. §Professor.
n
A"+'=L 57
k
0
I
1
n k
1 J
58
H. OZAKIet al. • k
where n
k
n
k
n
k
n
k
R . + I = [ Rx.+, R,.,,+, R=.+J
/
(2)
and "R~+~ = a 3 x 3 rotation matrix of frame n + 1 relative to frame n at step k; "R~,+~, "R~,+~, "R~,+~ = 3 x 1 directional cosine vectors; "rk,+~ = a 3 x 1 position vector from the origin of frame n to that of frame n + 1 at step k. Transformation matrix °Ah,k at step k representing the position and orientation of a hand relative to the base coordinates is given by
/
2Z
o6
~,\'N " / ~
• Yo
°A.3 °A6~6Ahk " A h , k = =
°A6k 6Ah , ,
(3)
where 6, h and h' indicate the wrist frame, the imaginary hand frame having a rigid force sensor and the actual hand frame respectively (see Fig. 2). The transformation matrix, hAwkwhich represents the translation and the rotation caused by the deformation of a force sensor, is hawk=
where the rotation matrix A~0y~ and AhO=k;
'__
hRh,k
l
,
(4)
is the function of ah0xk,
hRh'k hRh'k(AhOxk, Ah0y k, Ah0z k) =
Xo/30
Fig. 2. Relation between the deformation of force sensor and the position and orientation of hand. Let us consider the force A"'Q k, which acts on the manipulator at the origin of its actual hand frame, moves this frame h' to the frame h". In the following we assume that all the components of AhUk are small and their second order terms are negligible. In this case the transformation matrix h'Ah,,k is described, in the same way as before, by
(5)
(6)
See Fig. 3. Measuring the force exerted on the origin of the actual hand frame, the deformation AhUk is given by
= K~ h'Qk
(7)
where h'Qk is a measured force vector and Kc is a 6 x 6 compliance matrix of the force sensor, measurable and known in advance. :R:;taflicnal
(8)
i ]'
h'Rh',k = h'Rh,,k(~hOxk,~hOyk,~hOzk)
(9)
~"'rh,, k = [~" rxk 8" r,.k 8hrzk] T.
(10)
and
Therefore deformations 6hr~ k, 6"ryk. . . . . 6"0zk just written are obtained as follows. Let A"'Qk be the force error given by subtracting the virtual external force h'Qvk at step k from the force h,Qk+, to be attained at the next
jcinfi (R)
Joint (P f ' ~~ a# ' ~6 # )
:Prismatic
#i
i
where
AhU k A_[Ahrxk Ahryk Ahrzk AhOxk Ahoyk AhOzk]T
:Position Of joint i T =#3
#4
~ #1 (b) R%P,R~~ M.,>Y/ #6
Crtst fo
RpPRP,F~
li
.."
Y~
#3
Hand
Jx~ (a)
/
[o
k h' k h' k T =K~[ h' F.,k h' Fy~k h' Fz~k h' M~, My~ M~,]
x
Position and orientation of hand at step k and at step k+l
and "r.," = [A"r~k A"r~k Ahrzk]T.
h
(c)
RRPrRP,[~
Fig. 1. Examples of the structure of manipulator.
/
~ x~
Fig. 3. Translation and rotation caused by deformation of force
sensor.
On the force feedback control of a manipulator step k + 1. In quasistatic motion, the virtual external force h'Q~ is the difference between the measured force "'Q,~ and the weight of the hand h'Qwk. Then the force error is given by A~'QE_ [Z~'Fxk A~'F~~ A~'F~~ a~'M~~ A~'M~~ a~'M ~]•
= ~,Qk+, _ h'Qk
(I1)
1 F l  8hOzk
After all it is shown that the transformation matrix °A6~+~ is obtained by using the known values, i.e. the transformation matrix °A6E, measured forces and given forces. Here we assume that all the components ~ U ~ are small and their second order terms are negligible. This assumption is acceptable when "'Qs~ and Ah'Q~ are not so large. Further, using 6rh = [0016,] ~ eqn (17) is simplified as
~hOzk
 8boy k I
1
~aO~ I
1_0
o
0
where h'Qk = h'Qk_h'Qk. Using this Ah,Qk gives
8hU k __.[8%xk 8" r~k 8hr~~ 8~0~k 8hO~k 8hO k]T = K~Ah'Qk.
(12)
In order to absorb the above force error ~th'Qk, the force sensor must have the deformation which is given by the elements of the transformation matrix "Ah,,k, because 6Ah"k=6AhkhAh "k and 6Ahk expresses the constant translation. Now let us consider the movement of joint 6 which satisfies the conditions that at step k + 1 the position and orientation of the actual hand frame is equal to that of step k and the force error Ah'Qk is absorbed (see Fig. 2). First condition is °Ah'k+l = °A6~~16Ah>+t= OAh,k.
59
16hShOrk  8hrx k + ~hryk)
 (16h~hOx k
q, . . . . . .
(18)
( .....
2.2 Decision o.f joint movement Let us consider the joint angles Oo k+l= k+l T (0tk~I 02k~...060) which satisfy (17) or (18). In general O0 T M cannot be obtained straightforwardly except the case where the manipulator has a special structure, for example Fig. l(a). For the general case joint movement AOok= Oo k+~ Oo k is given by using RMRC (Resolved motion rate control)[18]. RMRC, however, requires a large number of multiplications and additions to calculate the inverse of 6x6 Jacobian matrix. Sixdegree of freedom manipulators usually have such a convenient structure that the hand orientation is arbitrarily changed by joints 4, 5 and 6 (see Fig. 1). For these manipulator, we can obtain the algorithm which is compared favorably with RMRC in the cost of calculation. We call it "Decoupled RMRC", because in this algorithm LxOok is separable into two vectors; ~O~o= [~0~o A0~o ~03ko]T giving the positional movement of joint 5 and AO~o=[~Ok4oAO~oAO~o] T giving the change of the orientation of frame 6. Before describing
(13)
Next one is deduced as follows. The relation between joint 6 and the actual hand frame is described as 6Ah,k+~. So as to absorb Ah'Qk the deformation of the sensor which gives the transformation matrix 6Ah,k+t satisfies the 6Ah,,k.Therefore we obtain 6Ah
.k+ I = 6Ah..k= 6A, .kh'A,>.
(14)
Using (13) and (14) gives 02~k+l = °A~k
6Ah,k(h'Ah,3)'(6Ah,k)1.
(15)
Also 6
6Ah,k = 6Ahk h A h k

kl
[ Rh,
6 t
Substituting (8) and (16) into (15) gives
OA6k+1 =OAk
k rh,
"1
(16)
Fig. 4. Orientation of frame 6 with respect to base coordinates.
[(~__h_.k(__.~I~h,,~_)T k)T~ _6Rh.k('R...) h k T{(6Rh.k )T6 rh._+8_'_'~..__}+_r_~ k h k61"*] _(__~R_h._ •
I
"
(17)
H. OZAKI el al.
60
this algorithm, we express the orientation of frame 6 using the components of °A6k+l. Let this orientation of frame 6 at step k + 1 with respect to the base coordinates, be °a6k÷~= [0t6 k+l ~6 k+l 'yrk+t] T (see Fig. 4). Using this °ark+l, °Ark+l is given as
Then the resultant change of the orientation is the difference between the change A°a6k °a6k + '   O a f k and the above change A0a~o. k That is A°a k = A°a6 k  A 0 a krQ.
(27)
7 C~C~
S~S v + C~S~Cv
C~Cv + S~SflS v
 C~S v + S~S~C~
,
C~S. v.
C~Cv 6 . .
I
S~C~
O~k+l
 S~ . .
. . . . . .
. . . . . . . . . .
where co cos (a6k+~), s, = sin (oL6k+l), e t c . On the other hand, from (17) or (18) °A6k÷~ is given as =
(OD k+l ~
(0R yk6+ l) l 0 k+l (Rr6)2
•, ~ 6 , O k+l (R~6)2
°A6k+ 1 =
iODk+1~ I lXX6 )3
0
k+t
( R y 6 )3
0 k+l (R~6)~ I ° rx6 k÷l fORk+l ~ I 0 k + l ~ ~6 n , ry6
z6 .,3
'~ o k+t , rz6
o
!
¢ORk+l ~
~,
. . . . . . . . . . . . . . . . . . . . . . .
o
o
'
 S Cv + C S~S v
. . . .
~
I
O k~l
rx6 0 k 1
rv6
0
k+l
rz6
[
1
(19)
Therefore AO~o must satisfy AO~o = Jo 1A°ak
(28)
0°a6"~ J~ =(~ OOa''~ ] O=O k .
(29)
where
i. . . . . .
(20) After all joint movements
Comparing (19) with (20) gives a6 k÷l=tan
1
O
k+l
O
k+l
(( R~6 )2/( R~6 )0
. 1 k+l /36k÷~=sm ((°Rx6 h) k~l
V6
= tan
1
O
k+l
(( Ry6
(AOok) T = [(aO~) T (aO~o) T]
)
! /
(21)
k+l
)3/~R~6 )3). J
Now the algorithm of calculating the elements of the vectors A O ~ and AO~o mentioned above is given as follows. (1) Calculation of AO~o. From Fig. 1 the position of joint 5 is °rsk+~ = "r~k''  °R~rk+ll~6.
(22)
So as to satisfy the movement A°r5k = °rsk+ ~°rsk, AO ~k0 must satisfy A O ~ = J r  1A°rsk
(23)
j~ A ( a°r5  \8~Q]o=ok.
(24)
where
This angle Ao,ko gives the position of joint 5 to realize the position and orientation of the hand to be attained at step k + 1. ' (2) Calculation of AO~o. The change of the orientation given by AO~o is O k
k
A a~ = J~AO~
(25)
where J,,, A( 08~~) s =s k.
(26)
which gives the forces to be attained are obtained. Since this algorithm does not require the calculation of the inverse of 6 × 6 Jacobian matrix but that of two 3 × 3 matrices, J, and J~, it is easily carried out. Also it is assumed that L and Ja are nonsingular. This assumption is usually valid except a few singular points and even in those points expedient solutions are obtained. But this problem is not treated in this paper, because it mainly depends on the structure of the used manipulator. 3. POSITION CONTROL SYSTEM WITH FORCEFEEDBACK LOOP
Let us add the force feedback loop, which is composed of the algorithm obtained in Chap. 2, to the position control system (see Fig. 5). In this block diagram the weight of the hand "'Qw is neglected to simplify the expression. In Fig. 5, °Xha = a 6 × 1 target position and orientation vector in the base coordinates; ~ = a n operator which transforms a joint angle vector to a position and orientation vector of the hand in the base coordinates; Opa = a 6 x 1 joint angle vector corresponding to °Xha; CQ,a = a 6 × 1 given force and torque vector, which is expressed in task coordinates and acts on the top of the tool; [h'Tc] = a 6 × 6 transformation matrix which transforms CQ,a to h'Od ; h'Qd = a 6 × 1 given force and torque vector, which is expressed in the actual hand coordinates and acts on its origin (this corresponds to ,,Qk+l in Chap. 2); O, = a 6 × 1 reference input vector to the hardware of the manipulator; Gp(s) = a 6 × 6 diagonal matrix transfer function whose components are transfer functions of the position controlled joints; 0 , = a 6 × 1 actual joint angle vector; °Xh, = a 6 × 1 imaginary position and orientation vector of the hand with a rigid force
On the force feedback control of a manipulator Software o](~
,
Hardware
I Opd 4
I Fig. 5. Block diagram of position control system with force feedback loop.
sensor; °Xc = a 6 × 1 position and orientation vector of the obstacle (or the environment); Ah[l = the translation and the rotation of the imaginary hand frame and this corresponds to the difference °Xh, °Xc with respect to the base coordinates; [hTo]=a 6 × 6 transformation matrix defined by AhU = [hTo](°Xh, °X,.); I{c = a 6x 6 combined matrix of the force sensor and the environment; h,Q, = a 6 × 1 force vector which is expressed in the actual hand coordinates and measured at its origin (this corresponds to ~,Qk in Chap. 2); Kc = a compliance matrix of the force sensor; Go(s)= a 6 x 6 diagonal t '  .   • q matrix of the force feedback control law; L_. _; = the force feedback algorithm which has been obtained in Chap. 2. In this control system it is possible to use both a target position and orientation vector °Xha and a given force and torque vector CQ,a (or h'Qa) as reference inputs. However "'Qa is given priority over °Xha, if both two inputs contradict each other (see Fig. 5), As °×ha expresses a desired gross motion, Opa corresponding to
61
it is obtained in advance. The force feedback loop gives the fine motion which absorbs the position error and copes with the variation of environment. Let us describe the ~'~] part of the block diagram in Fig. 5 in detail (see Fig. 6). If one feedback loop is switched off, in other word, if one component of "'Qd is equal to that of h'Qs, the force feedback is not added to its component. This means that the component of Ah'Qk in eqn (11) is equal to zero. As the result, if one component of
[email protected] is zero (virtually the absolute value of this does not exceed a threshold), corresponding joint moves according to the reference input. Therefore the fewer components are forcecontrolled, the more joints are positioncontrolled. If h'Qd= 0, this control system works as a bilateral one. Let us transform Fig. 5 to Fig. 7(a) from the view point of force feedback control. Then the joint angle Opa is considered to be a disturbance to the force feedback loop. If I{c is nearly equal to Kc, i.e. if the compliance of the environment is negligible as compared with that of the force sensor, the transformation from Fig. 7(a) to Fig. 7(b) is possible. In this case the force feedback control is reduced to the position feedback control of each joint. Therefore it is possible to determine a control law Gic(s) (i = 1. . . . . 6), an element of the diagonal matrix Go(s), based only on the corresponding joint's transfer function Gip(s) and its disturbance. 4. C O N C L U S I O N
This paper has presented the force feedback algorithm of a sixdegree of freedom manipulator with a compliant sixcomponent force sensor. This algorithm has the following features. First, this algorithm gives the relation between the position and orientation of the wrist at step k (present step) and that of the wrist to be attained at step k + 1 (next step). This relation is simplified in case the deformations of the sensor are small. Second, an efficient calculation method which gives the joint movements satisfying the above relation is proposed. This method is
4Olpd
+
~ypd ~pd
k~+
+
• +
"
~s
~d'
, yS "
"
"
Mz~J 
hQ d =[~xd EFyd "
"
"
l{Mzd]
] z106Q]"
sw
~
feedback
algorithm
( 
+
,
control : J'Q:O
Bilateral control:GQd: 0 Force feedback control : ~Od~ 0

)~ xs

,Fvs
Fzd i~Mz d
Position
I
I
~vlbj I K c
+~
Fxd
Fyd
~,
o
Force Force feedback
=L
•
+
_
KFzs
~Mzs
Fig. 6. Adding force feedback algorithm to position servo system.
62
H. OZAKIet al. ()pd '
HQd
()u
+
h'Qs
~ I
Xc (a)
Force
feedback
loop
II
h'Qd
(b)
Simpli_fied
force
feedback
loop
Fig. 7. Block diagram of force feedback control.
compared favorably with RMRC in the cost of calculation. Third, this algorithm explicitly explains the relationship between the position control and the force feedback control. In addition, the force feedback control is reduced to the position feedback control of each joint in case the force sensor is sufficiently compliant compared with its environment. In this case it is possible to determine the force feedback control law in the individual loop separately. Consequently the force feedback control of this type of manipulator is done efficiently according to this presented algorithm. The authors carry on the experiment to verify the efficiency of this algorithm using a hydraulic servo manipulator with a trial sensor.
0BER DIE DRUCERUCKF~HRUNGSBTEUERUNG DRUCKSENSOR
REFERENCES
1. S. H. Drake, P. C. Watson and S. N. Simunovic,Proc. 7th Int. Syrup. on Industrial Robots, pp. 8798 (1977). 2. D. E. Whitney and J. L. Nevins, Proc. 9th Int. Symp. on Industrial Robot, pp. 135152(1979). 3. H. V. Brussel and J. Simons, Proc. 8th Int. Syrup. on In. dustrial Robots, pp. 181193(1978). 4. H. V. Brussel and J. Simons, Proc. 9th Int. Syrup. on Industrial Robots, pp. 167181 (1979). 5. D. E. Whitney, Trans. ASME, J. Dyn. Sys., Meas. and Control 99(2), 9197 (1977). 6. R. Paul and B. Shimano, JACC AUTM2(2),694699(1976). 7. J. Denevit and R. S. Hartenberg, J. Appl. Mech. 22, 215221 (1955). 8. D. E. Whitney, IEEE Trans MMSI0, 4753 (1969).
EINES MANIPULATORS
~IIT EIN~M NACHGIEBIGEN
HANDGELENK
H. Ozaki, A. ~:oh~i und M. Taka~a K u r z f a s s u n g  Ein D~uckr~c~ftthrungAl6ori~hmus wird vorges~ellt f~tr einen Manipulator, der aus einem s~arren Servomechanismus mi~ 6 Freihei~sgraden bes~eh~ und einen nachgieblgen 6Eomponen~enDrucksensor an seinem Handgelenk b e s i ~ z ~ . D e r Algori~hmus ha~ die folgenden d~el Charakteristika: I. Der Algor£~hmus zeigt das Verh~l~nis zwischen der S~ellung und de~ Orientierung des Handgelenks in der gegenw§I~igen S~ufe an sowie das~enige Ve~h~itnis, das in der n~chsten Stufe erreicht werden wlrd. 2. Eine wi~ksame Berechnungsme~hode wi~d vorgeschlagen, um die~enige Gelenksteuerung zu erzielen, die dleses Verh~Itnis befriedigt. 3. De~ Algori~hmus ftthr~ die Druckr~ck~hrungsS~eueru~g zun~chst aLtf die Stellungr~c~~(ih~ungsSteue~ung Jedes Gelenks, sofern de~ Drucksensor g e n ~ e n d nach6iebig ist im Verh~l~nis zu den zu behandelnden Ob~ek~en.