On the force feedback control of a manipulator with a compliant wrist force sensor

On the force feedback control of a manipulator with a compliant wrist force sensor

Mechanism and Machine Theor) Vol. 18, No. 1, pp. 5742, 1983 Printed in Greal Britain. 0094-114X[83/010057~6503.00[0 © 1983Pergamon Press Ltd. ON THE...

425KB Sizes 0 Downloads 10 Views

Mechanism and Machine Theor) Vol. 18, No. 1, pp. 5742, 1983 Printed in Greal Britain.

0094-114X[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 MechanicalEngineering-ProductionDivision,Faculty of Engineering,Kyushu University36, 6-10-1, Hakozaki, Higashiku,Fukuoka, Japan

(Received[or publication 20 April 1982) Abstract--The force feedback algorithmis presented for the manipulatorwhich consists of a high stiffnessposition servo system with six-degreeof freedom and has a compliantsix-componentforce 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 six-degree of freedom and has a compliant six-component 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 position-controlled 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 six-degree of freedom manipulator with a six-component 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 peg-into-hole problem in industrial assembly tasks. One of them is based on the principle of the passive accommodation using a built-in 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 force-vector 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 force-sensing 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 six-degree 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 on-line 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

R-p-P-R-P,-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)

R-R-Pr-R-P,-[~

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 quasi-static 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. Six-degree 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 force-controlled, the more joints are position-controlled. 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 six-degree of freedom manipulator with a compliant six-component 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

~v-lbj -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~HRUNGS-BTEUERUNG DRUCKSENSOR

REFERENCES

1. S. H. Drake, P. C. Watson and S. N. Simunovic,Proc. 7th Int. Syrup. on Industrial Robots, pp. 87-98 (1977). 2. D. E. Whitney and J. L. Nevins, Proc. 9th Int. Symp. on Industrial Robot, pp. 135-152(1979). 3. H. V. Brussel and J. Simons, Proc. 8th Int. Syrup. on In. dustrial Robots, pp. 181-193(1978). 4. H. V. Brussel and J. Simons, Proc. 9th Int. Syrup. on Industrial Robots, pp. 167-181 (1979). 5. D. E. Whitney, Trans. ASME, J. Dyn. Sys., Meas. and Control 99(2), 91-97 (1977). 6. R. Paul and B. Shimano, JACC AUTM2(2),694--699(1976). 7. J. Denevit and R. S. Hartenberg, J. Appl. Mech. 22, 215-221 (1955). 8. D. E. Whitney, IEEE Trans MMS-I0, 47-53 (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~ftthrung-Al6ori~hmus wird vorges~ellt f~tr einen Manipulator, der aus einem s~arren Servomechanismus mi~ 6 Freihei~sgraden bes~eh~ und einen nachgieblgen 6-Eomponen~en-Drucksensor 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~hrungs-S~eueru~g zun~chst aLtf die Stellungr~c~~(ih~ungs-Steue~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.