Robust Robot Arm Control with Nonlinear Feedback

<:l>P\ligilt

:I:' 11·\(

j{(dH)1 ( 'lIlll)')

1"'\ I()( () ·"".-)1. l).ll( dll\l,j, ..... 1).1111. 1~1,"·1

ROBUST ROBOT ARM CONTROL WITH NONLINEAR FEEDBACK A. K. Bejczy"'. T.

II(

!'ll'III/;'I!!1I

1.1/11I11f{{1I1\.

J.

Tarn""" and X. Yun*'"

(if/I!IIIIIII/

11/,(/(1//,

11/

/"IIIIIIII/II:.!.'\, !'u,rlll,llll.

( \ '1//1111. IS\ /J111IIIIII!!/i1

"f

\'\1(,/11\

.\llllill

Ii/III

\lil//IIIIIIII/II,

\\rj\jllll:.!.!U/I

{"/il,/!\II\.

\(

/.111111,

\ I () I, > / >11. 1 \ \

Abstract. ~onlinear feedback with diffeomorphic state transfor~ation is proposed for dynamic control of robot arms. This ne\-.' dynamic control method externally (or exactly) linearizes the whole system and provides simllltaneous output decoupling. 10 render the control robust, the nonlinear feedback is augmented with optimal error correcting controller which operates on the error in the task space. Several computer simulations were performed using the JPL-Stanford robot arm as an example to evaluate the performance of the new dynamic control ~ethod. The simulations show very good tracking performance and are discussed in some detail together with the characteristics of the nonlinear feedback controller. A key feature of this new dynamic control method is that the control system parameters do not need readjustment from task to task. In that sense this controller is l1intelligent l' since it is capable of directly responding to changing task commands. Kevwords. Nonlinear Systems, Feedback, Controllers, Computer Control, Optimization, Ro~ots, Stability.

Vecettio, the method presented by Tarn, Bejczy, Isidori and Chen is general: (i) It provides both necessary and sufficient conditions, the output can be block decoupled, and each block can be a vector. (ii) It provides a constructive algorithm for finding the required nonlinear feedback for systems in which the dimension of output can be less than or equal to the dimension of input. (iii) It provides output decoupling by nonlinear feedback 1n ter~s of the system's state.

I NTRODl'CT 101\

Advances in differential geometric systems and control theory permit to take a new approach to the problem of dynamic control of robot arms. ~ithin the frame of differential geometric control theories we can approach the state feedback control problem with the following questions: (i) For a nonlinear and coupled system with certain tasks, does there exist a feedback which, by suitable changes of coordinates, converts the original nonlinear and coupled syste~ to a linear and input/output decollpled syste~? (ii) tiow tl' construct a nonlinear feedback with sllitablp changes of coordinates which accomplistlEs overall systen linearization and inpllt!outPlJt decoupling 0 (iii) Is it possible to construct a nonlinear feedback for dynamic wodel of rigid body robot arms whicil linearizes the overall systew (original s)'ste~ with added nonlinear feedhack) and produces input output decoupling after ~llitable ch3nges of coordinates? The answer to this last qllcsti(,n is affirmative.

The purpose of this poaper is (i) to elaborate on l)Ur new general nonlinear feedback control method which also contains an optimal error correcting feedback acting on errors in the external task output variables and (ii) to present new computer simulation results 'w'hich illustrate the performance of Ollr new dyna~ic control method.

~nSLI~EAR

FEEDBACK

The nonlinear feedback control method is normally stated in state eauation forffi. Therefore, we first briefly su~~arize the dynamic control model 0f rigid body robot arms in state equation form. Then the application of nonlinear feedback to the control of rigid body robot arrrs will be sUrlmarized.

The feasibility ef compensatirg for coupling and for nonlinearities in robot arm d~·namic control by a suitable nonlinear feedback was shown by Lentini, Kicolo, Kicosia and Vecettio (1980), h)" I\icolo and ~icosia (1980), bv Fr~und (1977 and 1982), and recently by Tarn, Pejczy, lsidori and Chen (1984). The details of the recent work can be found in Chen (1984) ~ith sone sinulated results given by Bejczy, Tar~, Chen (1985).

Rigi~

Robot Arrr Dvnamic

~~odel

The matherratical model of a complete m-degree-offreedoffi robot arm systerr comprises the mechanical part of the system and the actuators (including the drive transmission components) that drive one degree-ai-freedom each. Since our simulation results are related to the JPL-Stanford electric arm and. in order to keep the mathematical writing brief, \o,'€ \o,;ill state the dynamic model equations in state equation form for the first three degrees of freedom of the JPL-Stanford electric arm, omitting - without loss of generality - the actuator

The methods presented hy Freund, Lentini, ~icoI0, Nicosia and \'ecettio are restrictive and s0rne~hat l'ad hoc": they use formal restructurint: of equations, and the number of inputs must equal the number of outputs. Moreover, in Freund's ~ethod, linearization is not in terms of the state but in terms of the output, and only sufficient ar.d nc" necessary conditions are given for the existence of a suitable nonlinear feedback. In contrast to methods of Freund, Lentini, Kicolo, Nicosia and

-,:1

532

A. K. Bejezy. T.

J. Tarn and

models from the equations. With these restrictions, we have the following dynamic control model for the JPL-Stanford arm, using homogeneous coordinates together with the Oenavit-Hartenberg fourparameter representation of robot arm kinematics, and using the Lagrangian formulation of kinetics:

X. Yun

y(x)

3

f(x) +

L

(1)

g. (X)w. 1

i=l

1

( s)

where x = (Xl, ••• , x2i) state vector with x = (Xl' x 2 ' x3 ) being the position variables for the first three joints and x4 = Xl' Xs = x2' x6 = x3' and wi being the control vector (generalized joint force or torque in this case). The content of the nonlinear functions f(x) and gi(x) in Eq. (1) is defined by writing the component form of Eq. (1):

This equation is the general forward kinematic solution for end effector position in the task space as a function of joint variables for the JPL-Stanford electric arm, as it is also shown in Paul (1981). Application of Nonlinear Feedback

Xl

Xl

x

x

2

x3 X

+

x3

[Wo]

'\

x

4

Xs

Xs

x6

x6

[[O(X~l-lJ

It is shown in Tarn, Bejczy, Isidori and Chen (1984) and described in detail in Chen (1984), that the dynamic control model of the JPL-Stanford robot arm described by Eqs. (1-4) with output given by Eq. (S) satisfies the necessary and sufficient conditions for external (or exact) linearization and simultaneous output decoupling for all x£U, an open neighborhood at equilibrium point xo. Therefore there must exist

2

+

[[O(X)~-lp(X)]

[:]

O(x)

['H

(i)]

'13 021 (i) (X) ° 22 (X) 023 (X)

°

31

(X) °

32

A nonlinear feedback w = a (x) + 8 (x)v where 8 (x) is invertible.

2.

A diffeomorphic transformation T(x) such that, after the nonlinear f eedback and diffeomorphic transformation, the new system is linear in the Brunovsky canonical form and the outputs are decoupled.

(2)

where 012 (x)

1.

(3)

(X) 033 (x)

The mathematical details of finding the appropriate nonlinear feedback co ntrol law and diffeomorphic transformation are given in Ch en (1984). The results are: 1.

The required nonlinear feedback is w= a (x)+ 8 (x)v

3

3

L L

j =1 k=l 3

p(X)

- p(x) + O(x)J

-1

h

v

( 6)

3

L L j=l k=l 3

0lJ'k (X)x 3 + ,x 3 +k + 01 (X) J

3

L L

j=l k=l

°2J'k (x)x 3 + · x3 +k + °2(X) J 03J'k
where J h is the Jacobian matrix of h(xl,x2,x3)' O('X) and p(x) are defined previousl y , and where we i n tr oduce d the foll owing no tations:

Here, 0i · (x) is the inertia load projection function to 30int "i" related to acceleration at joint "j", 0ijk(X) is centripetal (j=k) or Coriolis (j;!k) force projection function to joint "i" related to velocities at joints "j" and "k", and 0i(x) is the gravity load at joint "i". with The general function definitions of the 0t" 0ij and O"k functions can be found in Paul ( 981) and B~~czy (1974); the explicit form of these functions, including the numeric values of the geometric and inertia parameters, related to the JPLStanford electric arm, are listed in Bejczy (1974) and in Tarn, Bejczy, Isidori and Chen (1984). The "general positioning task" or "trajectory following task" equation which, in our case, is equivalent to the "system output" y(x) is given by

:, X . 1

;x . 1

: X. 1

=

:X. 1

~· x. 1

:X.

1

1,2,3

Robust Robot Arm Control 2.

The required

dif~eomorphic

transformation is:

h/X) L \

f

z

z

T(x)

(x) 0

1 I

(7)

h2 (X)

1 1 I

j~ ,

L h (x) f 2

1 I

L _ _ l ~ ~~e.!:..oc_I( ___

h3 (X)

l, '

.1, ,1

'

:l, ' ,

_______ ___

I

~

L h (x) f 3 where Lf is the Lie derivative, Lfh = [f, hi = ah f - of h with f and h defined in Eqs. (1-4). oX 3x By using the nonlinear feedback and diffeomorphic transformation given above, the nonlinear dynamic system (1-4) with task (5) is converted into the following Brunovsky canonical form and simultaneously output decoupled: =

z

z

Az + Bv

(8) (9)

y - Cz

where

z

~ [;lj,

A

Z5 z6

nl]

z

0] 0 0~ g g

0 0 0 0 0 1 0 0 0 0 0 0

[o 0 0 001

[l -u Yl

y =

Y2

C

STABILIZATION AND OPTIMAL ERROR CORRECTION First we stabilize the new open loop linear and decoupled system. Then, to render the stabilized system robust relative to deterministic uncertainties in machine and task model parameter values, we add an optimal error-correcting control loop to each individual (linear and decoupled) subsystem. Stabilization

[1 g1

1 0 0

B

Figure 10 New Dynamic Control Method: Optimal Error Correcting Controller Around Nonlinear Feedback.

0]

000 o 010 o 0 000 1 0

Y3

As a matter of fact, this system consists of three (i=l,2,3) independent subsystems of the following form

Stabilization is accomplished by adding a linear feedback loop F (a PD controller) to the system and assigning the poles arbitrarily. As long as F is a constant block-diagonal matrix, the overall system will remain an °output decoupled linear system what we call L & D Block in Figure 1. For the new feedback loop we have v = v - Fz, and Eqs. (8,9) become:

z=

(A - BF)z + Bv

(10)

y

Cz

(1ll

Thus the new system (L & D block in Figure 1) can be considered to be three independent subsystems. Each has the following second order form:

(12)

1, 2, 3

where

It is noted that the damping ratio ; and natural frequency wn of this second order system are so

z o =[Z2i-l] 1 z 2i

2

that - n = f il , The new linear and output decoupled system is shown schematically as block BCLS in Figure 1. Note that linearization accomplished by nonlinear feedback is an "externa 1 1 inear iza t ion" a s opposed to the conventional "internal linearization" (Taylor series expansion). That is, the nonlinear character of the original system is not changed here by any approximation. Therefore, system linearization by nonlinear feedback can be called "exact linearization tl in a control sense.

( 13)

Note

also that the exact overall system linearization and output decoupling by nonlinear feedback and diffeomorophic transformation produces an overall linear system which is unstable since each linear subsystem has double poles at the origin.

2 ~wn ~

f i2 •

We can consider Eqs. (12-13) as the new mathematical model of the real system which is now externally (or exactly) linearized, output decoupled and stabilized. Let the output error ei for each decoupled subsystem be denoted as follows:

o

e

=

i

e1 l ] = [ e i2

[Y O1

y

0

1

yd] 1

y~

(14)

1

where superscript "d" indicates "desired" or model quantities; the same variables without superscript "d" are the real (or measured) variables.

.-\. K. Bejel\'. T. From Eqs. (12-14) we then have :

J.

' hlrn and X. Yun

(i)

Tha t is

(ii)

or

[~fil ~fJ

e.1

- A. e. 1

1

"i +

(iii)

[~] (v.

vd)

1

1

+ b. ll v . . 1

(15)

Convert the original nonlinear robot arm control dynamics into an externally linearized and simultaneously output decoupled system by using the required nonlinear feedback and diffeomorphic transformations. Stabilize the externally linearized and simultaneously output decoupled system by designing a linear error correction PD controller for each individual (decoupled) subsystem. Render the control robust versus uncertainties in machine and task model parameter values by adding an optimal errorcorrecting loop to each individual (decoupled and linear) subsystem.

1

This last equation is the error state equation for each externally (or exact ly) linearized, output decoupled and stabilized subsystem.

Extensive computer simulations were carried out to

Error Optimization

evaluate the performance of our new dynamic control method schematically shown in Figure 1 for

We introduce an optimal error-correcting control loop by minimizing the following cost function for ~vi defined in Eq. (15): T

J( 6v. )

6v:R llv.dt + 1

1

1

Jo

e. (t)' Qe. (tld t 1

1

where R Q

S T

1

positiv e definite matrix positive semidefinite matrix positive semidefinite matrix

and the mark' denotes transpose. From the well-known optimal linear control theory the optimal correction is

pet) -- [pp 11 P P 12] 12 22 is a positive solution of th e Riccati equation: pet) - -P(t)A. - A!P(t) + P(t)b.R-lb!P(t) - Q 1

1

1

We consider the steady state (t - ,, ) solution of the Riccati equation. This gives:

o

-' v i

-R

-1

In particular, the

initial conditions were set away from the trajectory to be tracked, the load in the r obot hand and dynamic projection functions were different from the nominal values by a give n percent, and the available control t orque or force was set lower than the assumed nominal value by a give n percent. This la st difference was designed to compensate for uncertainties in joint drive friction. In our recent simulation studies - partly described in Bejczy, Ta rn and Chen (1984) - we investigated two questions: (i) how well will the nonlinear feedback scheme, augmented with a simple PD controller, converge and track a prescribed ci rcular traject ory for small (5-10%) and larger (20-30%) causal uncertainties and model imperfections, and (ii) how much impr ovement will the optimal error correcting feedback loop provide over a simple linear PD controller for the same error conditions.

s.

with peT)

The simulations were aimed at answer

ing the following question: how stable and robust is the new dynamic control method versus model imperfections and other causal uncertainties? It is noted that the nonlinear feedback is essentially based on robot arm and load (geometric and inertia) parameters the knowledge of which is typically approximative only.

"real" robot arm dynamics.

terminal time.

1

robot arms.

The computer simulations were based on the following procedure: use of a nominal set of parameter values in the nonlinear feedback law, and use of an assigned set of parameter values (different from the nominal set of values) in computing th e

+ e.(T)'Se.(T) 1

COMPUTER SIMULATIONS

b: Pe. (t) 1

(17)

1

where

-2Rf. +R

12-

Our new strategy for dynamic control of robot arms is summarized in block diagram form in Figure 1. It contains three major steps.

A comprehensive description o f the s imulati on studies can be found in Tarn, Bejczy and Yun (1985) . Here we quote results related to the tracking of (i) a horizontal circle and (ii) a circle tilted by 45 degrees relative to the horizontal plane. The circle had a radius of 0.3 m and was placed in front of the JPL-Stanford r obot arm. Tables 1 and 2 show representative result s for various combinations of model er ror conditions and for varying speed of moti on. Note that 4 sec. and 8 sec. total trajectory time correspond to 48 cm / sec and 24 cm/sec e nd effector speed, respectively. As seen in these tables, the optimal error correcting addition ~ v to the nonlinear feedback provides considerable better tracking performance than the simple linear PD con tr oller. It is also seen in the tables that for some model error combinations and for some higher speeds the linear PD controller provides divergent or unsatisfactory tracking performance, while the optimal error correcting controller still provides excellent performance for the same model e rror conditions.

Robust Robot .\n n Contro l Table 1. Simulation Results For Horizontal Circle (R = 0 . 3 m), With and Without Optimal Task Outp ut Error Correction D.'! For Vary in g Model Err ors.

5"10 0 5"10

5"10 5"10 0

PARArv\ETER ERROR LOAD ERROR TORQUE /FORCE ERROR

5"10 5"10

15"10 30'10 15'70

5'70

10'10 20'10 20'10

15"10 20'10 30'10

I\\AX. POSITION WITH .1v ERROR \VITHOUT .1v Imm)

0.3 O. 3 0. 3 0.5 0.5 0.5 O. 6 0.6 0.6 2. 2 2.2 2. 2 2. 2 2.2 2. 2 3.6 3.6 3.6 5.6 5.5 5.5 9.3 9.3 9.3 10.5 10.4 10.4 40.4 40 40 39. 6 39. 1 39 66 65 64

MAX . VELOC ITY IV ITH Jv ERROR WITHOUT .1v Imm /sec)

0.8 0. 4 O. 3 1. 0 O. 6 0.4 1.0 1. 0 0.5 2.8 1.9 1.4 2.8 2. 8 1.4 4.4 3.0 2. 2 6. 7 4.4 3.3 11 7.5 5. 6 12 8.1 6.1 44. 3 29.8 22.4 44.1 29. 7 22.4 76 50 37.6

TOTAL TRAJECTORY TI ME Isec I

4

6

8 4

6

4

8

6

8

4

6

4

8

6

4

8

6

8

Table 2 . Simul a tion Results Fo t Tilt e d Ci r c le (R = 0 . 3 m), With and Without Optima l Task Output Er r o r Correction D.v For Var y in g ~o del Errors .

5% 0 0

PARAMETER ERROR LOAD ERROR TORQUE /FORCE ERROR MAX. POSITION WITH L1 v ERROR WITHOUT M (mm) MAX. VELOC I TY ERROR (mm /sec )

WITH I1v WITHOUT .1v

TOTAL TRAJECTORY TI ME (sec)

5% 0 5%

0 5% 5%

15%

15% 5% 15%

30'1,

15%

1.4 O. 7 0.4 2. 1 1. 1 0.8 1 0.5 0.4 8. 1 4.6 3. 2 7 23.9 11.7 8 49 19 14 18 10 7 9. 5 2.5 1 12 3 117 30 12 217 45 4

6

8

4 6

Tables 1 and 2 quote the maximum position and ve l ocity errors in the task space. The average er r or s are considerably l ess as seen in Fi gu r e 2 which shows the time history of position and velocity e rr o r components in th e t ask space fo r a tilted c ir c l e tracking case with T = 6 sec and with paramete r er r o r 5%, load e rr or zero, and

force-to r que e rror 5%. As se e n in this figur e, the op t imal er r or correcting loop reduces the tracking error of the simple linear PD controller by more than an orde r of magnitude . Note also that the speed of motion has no effec t on the trackin g error for a horiz ontal circle as seen in

Table 2. But the trackin g error improves for lower speeds for a tilted circle as seen in Tab l e 2. This difference in tracking error performance as a function of speed fo r the two c ases is explained by the effec t of g ra v it y.

CONCLUS IONS The simulations have shown good performance of ou r new dynam i c control method for robot arm con trol. The augmenta ti on of nonlinear feedback with an op tima l e rr or co rr ecting cont r o l le r provides robust pe r formance and assu r es acceptable tra cking erro rs even fo r relativ e l y large (15 - 30%) causal model u nc ertain ti es . Fo r relatively small (5 -1 0%) causa l model uncertain ti es th e ave r age tra cking

10'1, 20'1, 20'1,

3.9 2.8 7. 3 4. 2 3 300 51 56

1.4 7.3 1.9 0.8 42 17 80 20 7. 7 -

12 4. 9 36 10.5 4.1 37 - - - 1194 92 -

8

6

4

6

8 4

8 4

6

8

4

10.6 4.2 95 6

8

erro r i s about 0 .1 percent of th e radius of th e circle to be track ed . A ma j or new f eatu r e of our dynamic control me th od i s that the optima l e rr or correction loop directly ope rat es on th e task level and n ot on the j o int servo control l evel. The task level erro r s are then decomposed by the n onlinear gain matrix E(x) into joint force or torque drive commands as schematical l y shown i n Figure 1. Note also t hat the ~ (x) compensa tin g vector and the B (x) gai n matrix in the nonlinear feedback eq uation (see Eq . (6» do not change f r om task to task, unless the load carried by the r obot end effector i s c han ging drastically r e lati ve t o a nominal va lu e assumed in the design. In that case the changes in th e " (x) vector and B (x) matrix can be ca rri ed out by u s in g st r aig htf.o rward analytic formulas which ca n be found in Bejczy ( 197 4). Consequently , th e basic contro l sys t em parameters in our dynamic control method do not need readjustment from task to task . In that sense our control me th od is "int el ligent" si nce it is capable of directl y respondin g to cha nging task commands . ACKNOWLEDGEMENT This r esea r ch was supported by th e Nat i onal Sc i ence Foundation under gran t s ECS-8309527, ECS - 8017184 -01 and INT-8201554.

.-\. K. IkjcZ\. T . .J. Tarn a nd x . Yun POS ITI ON ER ROR IN TASK SPACE (WITHOUT t:N )

POS ITI ON ERRO R IN TASK SPACE (W ITH ::N)

1.2 1.0

2

0.8 0. 6 0.4 ~ 0.2 i 0.0 S -0.2

E

<=

'":"

S

0

-0.4 -0.6 - O. 8

-1

-1.0 - 1.2

-2 0

2

3 4

5 6

8

9 10 11 12 13

3

0

TIME (sec)

6 8 9 10 11 12 TI ME (sec)

VELOCITY ER ROR IN TASK SP ACE (WITHOUT t:,.V)

VELOC ITY ERROR IN TASK SPACE (W ITH ~ V) e

4

4

e

5

2

2

4

3

3

2

€6

e

6

2 u

u

~ E

'""

S

~

E

0

N

S

-I

0 -1

-2

·2

-3

2 6

4

0

2

3

-3

e e

e2 e

-4

-4 4

6

8

10

11 12

TIME (sec) Fi g ur ~

e4 ,

1

e

2

3 4 TI ME (sec)

2 . Time Hi s tory o f Pos iti on an d Velo c it y Err or s With and Witho u t Optimal Corre c tion ~v For Task Output Error s . (Tilt e d Circl e , R = 0.3 m, Parameter Error 5%, Load Error 0 %, For ce -Torque Error 5% , To tal Tr a j ectory Time 6 Sec . )

REFERENCES Bejczy, A. K. (1974) . Robot Arm Dynamics and Control, JPL TM 33-669 . Bejczy, A.K . , Tarn, T. J . , Chen,Y . L. (1985). Proceedings, IEEE I n ternational Confe r ence on Robotics and Automa t ion , St. Louis, MO. Chen, Y.L. (1984) . No n linea r Feedback and Computer Control of Robo t Arms . Ph.D . Dissertation, Department of Systems Science and Mathematics, Washington Universi t y, St . Louis, MO. Freund, E. (1977) . A Nonlinear Control Concept for Computer Controlled Manipulators . Pro ceed i ngs of I FAC Sym posium on Mul t ivari~e Techno l ogical Systems, Fr edericton, Canada. Fr eund, E. (1982). Fast Non l inear Cont r ol with Arbitrary Pole Placement for Industrial Robots and Manipulators, The International Journal of

Robotics Resea r ch, Vo l . 1 , no. 1 , The MIT Press, Cambridge, MA . Lentini, D., Nicolo, F., Nicos i a, S., Vecettio, L.

(1980). Decoupling and No nl inearities Compen sation in Robo t Con tr ol Systems, Re port 80-08, I ns t itu t e of Automation, Un iversity of Rome, Italy . Ni colo, F., Nicosia , S., Dynamic Contro l of Indust r ial Robots. Proceedings of 5th Polish Italian Conference on Systems Theo r y, Economics Man . Tech. Paul, R. P. (198 1 ) . Robo t Ma ni pulato r s: Mathema tics, Pr og r amming and Con t rol . The MIT Press, Cambridge, MA . Tarn, T.J., Be j czy, A. K., Is id o r i, A., Chen, Y. L. (1984). Nonlinear Feedback in Robot Arm Con tro l , Proceedings of 23 r d I EEE Confe r ence on Decision and Cont r o l , Las Vegas, Nevada.