- Email: [email protected]
Robust Microprocessor Control of Robot Manipulators
COlnriglll
©
I F,\(
R.llilll l (:1I111rld "p.li l1. I ~ I ;-.i .-I
( S\nJ(u ';-.i,-H . Itlnt'lull ;!.
ROBUST MICROPROCESSOR CONTROL OF ROBOT MANIPULATORS M. W. Spong'\ ::' / )f,/Hlrllflt'lll ( 'IIIi'!'fllly
(1/
11/
J. S. Thorp** and J. M. Klei nwaks***
(11111 Ilu ' ( :flOrd/ llo/nl \(il'll(l' Lo/Ju1'olun', 1111111111 f/I ( ' I II//I/lI-C/I/III//Jiligll, ('rhl/III/, 1/_ (>!Slil. ( ·S.4 .
(;"III'}(// FIf ,!.!. 'i llf'f'}'ill}!,'
""" S,-IIIIIIIII/ FII'r/ri'l/l F Il!!.·illl'l'rill,!!.. ( :111"111'1/ ( ·lIh'I'ni/\". Ilhl/{(t, .\" ) ' 1-I85J, [ '.'1. 4 ""''''' Sillg!'f CIIII//JilIIL fl illgllli/I/IIIII. '\T. ( '.'i..\
ABSTRACT: In thi s paper w e study the problem of ro bust tracking for robot manipulators in the presence of u ncertaint y and input constrai nts . Lsing the theory of uncertain dynamical s....',-rems we derive rob us t nonlinear control strategies. with guaranteed tracking properties that can be quantified given bounds on the extent of model uncertainty, senso r noise. input disturba nces. etc. We a lso utili ze a wrque oprimi:arion Slraregv to optimize the jo int torques in the event of actuator saturation. The algorithm has been implemented usi ng a single \1 0to rola 12 \ lh z '\1(:611000 microrrocessor on a three link re vo lu te j0int manipulator cons tructed by the Schoo l of \ Iech an ica l Engi neering a t Cornell Lni versity . Experimental results are prese nted here s howi ng the feasibility and perfo rmance of the control s trategy . Ke y word s: robotics. microprocessor . robust control. actuator sat uration, o pt imal control I. INTRODUCTION
H. BACKGROUND
In this paper we continue o ur investigations on the control of ro bot manipulators s ubject to bounds on the avai lable torque at each joint [6).[S), and s ubjec t to uncertaint y (7). In the first part of this work [6) a po int ""ise o ptima l contro l scheme. know n as an Optimal Decision SU'aregv (ODS) ""as derived wh ich com putes the vector of input torq ues to the manipulator at each sample instant l in s uch a way that the joint accelerations are optimized based on a prescribed pa th trac king criterion. The res ulting control la"" . the design of w hich was reduced to the solution of a quadrat ic prog rammin g probl em . was s hown to ha ve certain practical and theoretical Sign ifica nce . Firs t. by o ptimi zing the joint accelerations when one or m ore of the inruts saturates. s ignificant improvement in the resu lting performance of the system may result. w hi ch allows for fa ster motion of the man ipulator w hi le si multa neo usly reduc ing the need fo r exten s ive off - line traject ory rlannin g.
Consider the equations of motion of an 71 -joint r igid ma n ipulato r deri ved from the Euler-Lagrange equations [I)
Second. it was s how n that. because 0f the s tructure of the manipulato r dvnamic eq uations , anv contro l sc heme ma v be incorporated into thi s framewo rk , The resulting torqu.e opti~i =a tion schemer 12) rrov id es a mec hanism for handlin g input co nstraints fo r any control scheme, In the rresent rarer \I,e st ud y the iss ue of robustness to uncertainties which are present in any physical system , For a r obo t ma nirulator. t hese ma y inc lude unknown loa ds. u nmodeled dynamics . f riction. neglected s mall time delays . etc. l"sing the theory of uncertain system s d eve lored in [3).[9).[1 1).[ 14). we deri ve a res ult on ro bust trac k ing con trol for manirulat ors wi th bounded uncertai nt y. Incorpora t ing the abow roh ust cllntrol ler design in to the rrevio us torque optimi za ti o n scheme results in a ro bus t. rointwise o pt imal cont ro l sc heme wh ich is Imrlement ab le in real time . FinallY. using a si ngle \I oto rola 12\l hz \IC6S000 microrrocesso r. o ur co ntro l algo rithm was implemented o n a three re vo lute jo int m anirul ato r co nstr ucted b\ the Schoo l of \ lechanical Eng ineering at Co rn ell l"ni \"e rsity We rrese nt so me of the exper imental res ults in thI S r a rer. which s how the feasibility and perfor mance of t he algorithm . In what fol lo"" s R " will denote the us ual n-dim ensiona l vector srace O\' er R end o",'ed with the Euclidean or L r norm . All secto rs are colu mn \·ecto rs. A s uperscript T indicates tran spose of a vector or matrix . Amm(.\/ ). A""x(M) denote the min imum o r max im um eigen\'alues of a matrix \ 1. respecti\'ely , \Iatr ix norms are those induced by the corresponding vect or no rm ,
t
D, } (q )ii)
J =1
+
t
t D ,j, (q
)g}ej, + Di(q) = Ti + Tdi
(2.1)
J =H = 1
ii, + a,g, +
I,T,
b,u, . i
=
=1. '
.71 .
where q (t ) E R n are the generalized coordi nates representing joi nt positions. Equation (2. 1) represents the mechanical part of the system . T d , is a disturbance torque at the i - th joint . which could for ex a mple represen t frictional forces and torques. si nce these are d iffIcult to model precisel y. Equation (2.2) represents the d y namics of the i - th actuator, where a , .f, .b, are constants. and u, is the input for the i - th ac tuator . Equations (2.1 )-( 2 .2) ca n be written in matrix form as 1>1 (q
)(1 + h (q .g ) = Bu + FT d
where 1>1 (I{ ) is a positi ve de fi nite m, } (q ) =
m u (q )
h (q
.q ) is an 71
f, D,} for
=I
(23)
matrix with elements
71 Xn
i ... j and
(2.4 )
+ I ,Du .
-vecto r val ued function h (q .cj )
= ( h 1(1{ .g ).
(25 )
w here h , ( q.q ) = a, q,
t
+ 1, (
t
D ,},
g}g, + D ,)
(26)
} =H =1
a nd
F =diag (/,.
.f n ).
B = diag (b!.
.bn
).
( 27)
\\' e assume that none of the I, .b , is zero and for simpliCity ""e ""i1 1 s urpose that the input and disturbance coefficients ha v e been nor mali zed to unit y so that (2.3) may be written A1 (q
)il + h
(q
.g ) =
u
+ Td
(2.11 )
Thu s u (r ) represents the inrut torque to each joi nt at time T .! is an input disturbance which is assumed to be Lebesgue meas urable. sa tisfyi ng
t and
(2 .9) \\'e assu me that the inrut to rque T(t ) is constrained v ia ( 2 10) i =1.
,n
w hic h we ma y wr ite as
\(. \\'. Spollg . .J. S. Thorp and.J. \1. I\.leill\\aks
NT(C)
~
dc)
(2.11)
where the 2n Xn matrix N and the 2n vector e (C ) are given by
o
o o
-I 0
N
7
T lm ,"
. e(c)
o o
0 0
1mo
._
Ct)
-I
We will need to make the following assumptions on the plant (2.8) and the model (2.19). A]) The inertia matrix M (q) in (2.8) is positive definite, and there exists a positive constant a such that
(c)
=
re-adjust the input torques in the event that they would otherwise saturate .
I I M (q )-1 I I ~ a
(2.12) T nma ,
(c )
7 nmm
Ct )
<
00
for q E Rn
A2) There exists a nonnegative constant Q < for all q ER n and for all w such that I I w I I < W.
(2.22) such that (2.23)
In practice it is important to allow for time varying torque limits as we have done. Since our later torque optimization is performed pointwise, i.e .. at eacb sample instant, the time varying input constraints are easily bandIed.
A3) There exists a continuous function cp(x.w.r), bounded on each finite time interval [O.T). such that for all q ,q ERn and all W I ,W2 with IlwI11 ,l l w211 ~ W.
We suppose that we have available for feedback the measured joint positions and velocities represented as
where x (C) := (q r (c
Y
11
Y2,
= q, =q,
+
W I,
(2.13)
+W21
i=I, .. · ,n. Here w(c) (WI,(C), ' w l n (c),W21(C), . . . 'W' n (c))' represents measurement uncertainty (noise, etc. ) and we assume only that w" (c ) are Lebesgue measurable func tions of time satisfying 5upllw(c)11 ~ W I
~o
<00
I I h (q
+W I,q +w2)-h (q ,q) 11 ~
).q r
cp(x ,w ,c)
(2.24)
(C ))' denotes the system state.
Assumption A2) is the most crucial and, in a real sense. tells us how accurate an estimate of the inertia of the system we must have in order to control it.
Ill. ERROR SPACE ANALYSIS AND DESIGN For the problem of tracking the desired trajecto ries (2.15) we form the position and v elocit y error vectors
(2.14)
el=q-qd
We let
(3.])
e2=q-i/
(2.15) represent a desired trajectory in joint space that we wish the manipulator to track. We shall assume that qd (c) is continuously differentiable with the acceleration ii d (c) bounded in norm. i.e., I I ii d I I ~
B ~
00
The error dynamics may then be written as a first order vector differential equation
e1 = e2 .
(2.17)
" d
= -M-Ih
(2.16)
We shall implement an overall feedback control scheme of the following form:
..
(32)
e2=q -q
+ M - I(u + T d
"d
) _
so that in "error space" the problem of path tracking reduces to one of stabilizing the (time-varying) system (3.2). If the control law u is now given b y (2.17) (with Au 2 for the present) the closed loop dynamics are given by
with
e1 = e 2 e2=-M - Ih +M - I(M(v +AUI)+h +Td )-q d
:\ote that in (2.17)-(2.18) we use the measured, hence uncertain, state variables. In the expression (2.17), M ,h correspond to an "available model" of the manipulator 1>1(q )ii
+ h(q
,q) = u
(219)
which may differ from (2.S) in several respects. M C),h C) may be simplified ve rsions of M C).h ( .) to facilitate their online computation. For example, one may choose to ignore the actuator dynj1mic~ 0.2). o r certain coupling terms in M or h ete. Also . 1>1 C).h ( .) ma y contain "nominal" or estimated values based on. ~ay, an u~loaded arm, in which case the error represented b y 1>1 - M, h - h would be due in part to the manipulator load. The ju st ification fo r the form o f our control law (2 .17) is that if AliI
=
M (Y I ) -
AI (q ) := 0
Ah :=h(YI'Y 2) -h(q.q):=O
(3.4)
a straightforward calculation shows that
e2 ::::; A
le I
+ A ze::o + 1') + tl.u
I
(35)
where 1] is given by 1]=-A l w l-A , w 2 + E (\' +Aul)+M -I( Ah +1 d )(3. 6 ) :=Kw +E(v +!:wl)+M - I(Ah +1 d
)
where
K = [-A I,-A 2] 1] thus incorpo rates all the uncertaint y that we hO"e assumed
exists In the closed loo p system . and the term Au I is to be des igned t o overcome the effects of this uncertainty. Set =1=YI-q J
Au = 0
Au = Au I + Au 2 ·
(3 .3)
Setting
(220)
then (2.17) is just the ideal inverse dynamICS control law [J]. which yieldS an owrall system which is decoupled. linear. time- in va riant with presc ribed eigenvalues determined bv A 1..4 2' Howewr, the ideal computed torque is unachieY'ab le in practice due the aforementioned difficulties of uncertaint y. saturallon, etc .. and additional compensation ( represented b y Au ) must be introduced. We propose here a two-stage deSIgn procedure for computing Au .
=0
(37)
=2 =y : o -cj J so that
= :=(=\.=jJT
=e +w
(U)
is the "measured" tracking error. We sha ll assume only that the uncertainty 1] in (3.6) can be bounded by' a kn ow n continuous function p(= .r ) .i.e .. 1 11) 1 i ~p(=.I)
0 .9)
(22])
First. Au I is designed assuming n o torque constraints to overcome the effects of the model uncertainty (represented by ;1M ,::'h ), the input disturbance T d and the measurement uncer taint y w (r). AU, is then designed using the point wise torque optimization strategy of [7].[12] and the known value of Au I to
See Appendix 1 for a disc uss ion of how p ( = .r ) may be es timated . Finally the error equati o ns 0.3 ) may be written in matrix form as
e= with
Ae
+ B (1] + ;1u I)
(310)
COlltrol of' Robot
~() ;j
~lalliplllalOrs
IV. TORQUE OPTIMIZATION (311) Since the matrix A is at the discretion of the designer it ma y be chosen stable and with prescribed eigenvalues . A typical choice might be AI =_.\2/ . A 2 =-2.\/ so that the nominal closed loop system. i.e .. the system in the absence of uncertainty is decoupled and critically damped with "closed loop bandwidth .\ ".
Since A is stable. choose a 2n X2n symmetric. positive definite matrix Q and let P be the unique positive definite symmetric solution to the Liapunov equation
+
A TP
PA = Q
The preceding results show that given bounds on the extent of uncertainty in (3 .10) whether due to modeling errors. unknown loads or disturbances, etc ., uniform ultimate boundedness of the tracking error can be achieved with either the saturating non linear control law (3 .13).or the linear control (3.16 ). These results do not take into account actuator saturation howeve r. In this section we apply the torque optimi:alion scheme of [7].[ 12J to account for input constraints. The reader should consult the above references for details. Given the vector of input torques from (2,17). call it aCt). together with (3.13) or (3.16) we choose the actual input torque from the following single stage torque optimization procedure:
(3 .12)
min(a - u )T TICa - u )
(4.1 )
subject to Nu :::; c. with n = it-IQit - 1
Definicion 3./: Given a solution
of (3.10). we say the e (-) is uniformly ulcimacely bounded (u.u.b.) with respect to a set S if there is a nonnegative constant T(eo.s) < 00 such that e(c) E S for all c ~co + T.
Q is an additional design parameter that can be used to weight the components of the torque vector. It is important to note that (4.1) is solvable even for an unknown plant since a(/) is known and it is known . The optimal control input that results is then
We next present two designs for choosing /:'u I. one linear and one nonlinear. each of which guarantees uniform ultimate boundedness of e (c) in (3.10).
where.\' is the solution of the dual optimization
eC): [c o.oo ) ~ R2n. etc~) =e o
Theorem 3 .2: (nonlinear compensation) The error e (c) governed by (3.10) is u.u.b. with respect to the set S (defined below) if the control /:'u I is chosen as
-pCz /:'UI
.1 )
B T pz
: if 1 I B T pz I 1
II B T pz 1I
=
>
.c ) B T pz : if
l i B T pz I I
<
E
with I<
= .\max(P )w
E R
~n
1 eT Pe
with
J = Nu + C
!!.u
Proof: See Appendix 2 where it is shown that the uniform ulti mate boundedness set S is the ellipsoid
le
< 1<)
= /:'u I + !!.u ~
with !!.ul given by (3.13) or (3.16) and /:'u2
= -Q ~itN T .\'
(3.15)
with C u defined b y (A 19). k" = ef,Pe". Remark 3.3: The control law (3.13) is continuous in : for anv E >0. and thus (3 . 10)-(3.13) has a unique solution which can be defined over [c ". 00 ) for any initial error e ". An inspection of A .20 shows that the s ize of the set S . and hence the tracking precision. depends both on E and on the tightness of the estimate of the uncertaint y p ( : .1). For s maller values of E the radiu s of the uniform ultimate boundedness set decreases and hence the track ing precision increases. In theor y if W (I ) :; O. i.e .. there is no measu rement uncertainty, then w - 0 as € - O. In o ther words the S\'stem can be made asymptotically stable by the control law (3 . 13 ). Howe\·er. for E = 0 the contro l law (3.13) is discontinu ous and result s in a chattering controller. Further. for /:'u I discontinuous. one is forced into the noti on of generalized ordinar y differential equations.
In the terminology of Barmish. el.al.[IIJ Theorem 3.2 says that the uncertain syste m (3 .10) is uniformly and ulcimacely bound able in a quadratic manner (u .u.b.q.) with the control law (3 . 13) . It immediatel\' follo"'s then from ([It]. Theorem 4.1) that (3.10) is u.u.b.q . via linear COrllral. Specifically we have
In order to demonstrate the feaSibilit y of implementing the robusc torque optimi:ation algorithm (4 .1). a real-time implemen tation of the control strategy was developed for a three degree of freedom manipulator built by the Schoo l of \lechanical Engineering at Cornell. The robo t configuration is similar to that mode led for the simulation tests in [6]. The goal of this implementation was to dri ve the robot arm from a commercially available microprocessor single board computer ( SBC) using a high level language to implement the control algorithm. The manipulator arm consists of three rotary Joints with links two and three each ten inches in length . The arm is drI ven by three DC servomotors and can support up to a two pound load at the end of link two . The s pecifiC parameters for this manipulator are defined in Table 3 .1. The available or nominal chosen of the form 0 .19 ) with Thu s the inertia matrix .';1 ( q ) for the rigid mechanical part of
model of the manipulator was the actuat or dvnamics neglected . is taken to be'the inertia matrIX the sys tem .
Link Parameters
0.02245
Theorem 3.3 (linea r compensation) There exists y > 0 suffiCiently large so that the error e Cc) governed by (3 .10 ) is u .u .b. with ~u I chosen as
The detail s of the proof of Theorem 3 .3 can be [IIJ and are therefore om itted here . In the case of no ment uncertaintv. w Ct) :; O. Theorem 3.3 also follows earlier work of 'Thorp and Barmish [9J . We refer the references [9J.[IIJ for the calculation of y and the boundedness set S in this case.
(46)
V. REAL TIME IMPLEMENT A TION
o : e o E S(I<)
~ uCt) = -yB ' P:
(45)
(314)
and w defined by (.0.20) and that k o-I< ---c;;: e o e S (I<)
(4.3)
The overall control law that results is then (2.17) with !!.U in (2.21) given b y
for a given E>O and p.P.B.z as above.
2
(4.2)
(4.4)
E
S (I<) =
minl..!...\ T TJ.\ +.\T J} ,, ~o 2
E
(313)
-p(z
u'(c) = a(t) - n-IN T.\'
(316) found in measurefrom the reader to ultimate
0 .0165 0 .00&23
Link I length :
10 in .
Link 2 length :
10 in.
\1. \\'. SpOil).; .. 1. S. Tllmp alld
206
J.
\1. Kkill\\ ,lks
generates code that performs 32 bit arithmetic regardless of the size of the operands. To improve performance. software variable size was defined to be 16 bits. the standard word size of the '.1C68000. although the processor can also perform 32 bit arith-
Joint Parameters
metic.
jOint I
joint 3
joint 2
max torqueCin.lb.)
119
191
119
motor gear ratio
14&.51 :1
153 .0: 1
14&.51:1
± 120
range(deg. )
-IOto+160
± 120
Table 3.1 Robot Parameters The computer controller is a SBC based on the :vIOlorola MC68000 16-bit microprocessor running at 12 MHz. The SBC provides torque commands to the robot motors through 12- bit O/ A·s. The manipulator state. angular pOSition and velocities. are fed back to the computer from optical encoders mounted at the motor shafts. The encoder outputs are converted to a count representing position and are read by the computer through a 16-bit parallel port. The system is structured such that at each sample time. a programmable timer generates an interrupt to the processor. The interrupt service routine reads the count from the optical encoders and then calls the torque optimization routine. The remainder of the time the processor monitors the system terminal for input commands or other control information. \lotorola \lC68000
Clock s peed
12 \lhz
Data Bus
16 bits
Address Bus
24 bits
Internal Regi ster Size
32 bits
Add Ins!. Time
0.33/.! sec
\Iultipl y Instr. Time
3.5 p. sec
Divide Instr. Time
10.16 p. sec
The compiler library functions that perform the multiplication and division operations have been replaced by custom routines that perform these operations using 16 bit arithmetic operations. This provides a factor of three reduction in compu tation time for these particular operations. which are the most time consuming operations in the processor's instruction se!. One other modification was made in the name of effiCiency . The compiler's library routines for sine and cosine were replaced by an assembly language function that evaluates the sin and cos of an operand through a 256 element table look up with interpolation. Apart from these two modifications to the library routines. all code generated for the controller was wrilten in C. The full controller was implemented in the \lC68000 micropros:essor. The dynamjcs of the robot speCified by the matrix M and the vector h were coded into the controller. Lnmodeled parameters include motor dynamics and friction. Additionally. robustness of the controller to parameter va riation and un mode led dynamics was addressed by adding linear error feedback terms according to (3 .16). The controller algorithm is as follows: (I)
Read optical encoder outputs and comrute joint angles and
(2)
Compute trigonometric terms used in /If and h computations (done only once each sample period for efficiency).
(3)
Compute
(4)
Compute trajectory model dynamics. iid'
velocities.
NI
matrix.;; vector. and unconstrained torque. toq~
(5)
Add robustness terms
(6)
Compute IJ matrix terms ( IJ = N direction vector. and initial guess at A.
from equation (3.13) or (3.16).
(7)
While A's have not converged: compute A.
(8)
Compute inrut torques from (4 16).
T
NI TNil!
). initial
Lpon command from the system terminal. joint angles and torques are stored and can be uploaded to a \,A,\ 111750 for analysis and plolling. The reference trajectory model for the controller was set up to be a second order criticall y damped tra jector y with a 12 radian bandwidth. i.e.
where r is the input command. nomina lly a step command in position. A rem ovable two round loa d is placed on the end of the manipulator. Tests runs were made both with and without this load . Changes in the load were not accounted for in the con troller in order to test rob ust ness of the controller. The manipu lat or was run with various command inputs . H owever, shown
here are just step joint responses from an initial posi tion in radi -
Table 3.1 \l otorola \lC6S()()() Characteristics Softwa re for the sy"em was developed in the C programming language. a hi gh le\'el genera l purpose computer lan guage. The impetus for using C was rrimarily due to the a\'ailabillly of a C cross-compiler for the \lC6S000 . In as much as the SBC does not h3\'e a fioatlng pOint processor. all floating point arithmetic orerations are performed in
~oftware.
which causes a
significant perfor mance bOllleneck. Therefore . in order to max imize performance. fixed poi nt arithmetic was utilized rather than floating point operall ons. Fixed point ari thmetic is not part of the C language speCification. so fixed pOint opera tions were implemented utilizing a system referred to as B-scaling. The
ans of (0.0.0) to a terminal state of
(.y.2j-'i)' ThiS replicates
the tes t cases run in the SI\l:\O:\ simu latlOns of [6j. The results are shown In Figure 3.2 f or the maneuver described abo ve " ' ith the load allached. Figure 3.2 shows the joint angles. each profile labeled for its respect ive joint. Figures 3.3. 3.4. 3.5 show the corresponding torque commands reqUired to rerform the maneuver. Figure 3.6 sho"'s the joint angle resronses with the load remo\'ed from the end of link 2. The s tead y state errors for the two configurati o ns afe ~ h own in TabJe 3.3.
maximum va riation of a parameter is determined and rounded t o
the next larges t power of two. Thi s speCifies the location of the decimal point in the variable. which is now represented by an integer. Therefore. all operations are executed usi ng integer arithmetic. This res ults in significantly faster computation. since these operations are part of the \lC6&000 instruction se!. Care must be taken in USing s uch a syste m to ensure that computed variables do not exceed their prespecifled range. resulting in an overflo".. COndlllOn . Additionallv. the order of computation of a given equation can affect the precision of the result. The C compiler standard word size IS 32 bits. although declarations of short words. 16 bits. are possible. The compiler
Joi nt >.ngle Position Frror I
Tes t Case
Joint I
i
Joint 2
I i
Joi nt 3 1
!
with load
0.40'7,
0.22('",
I
I 15'7,
I
without load
OAO<;',
022 <;',
I
1.71 '7,
i
Table 3.3 Joint Angle Error
207
Control of Rohot "Iallipulators The controller was run at 62.5 H2. or a 16 millisecond sample interval. This is the maximum effective rate at which the controller can run and still rro v ide enough time for a reasonable number of iterations in the minimi zation section of the algorithm . Table 3 .4 sho w s the breakd own of time spent in the con tro ller. Hence the total contro ller time is (3.4 + 2.2n) milliseconds.
n is the number of iterations for the minimization to
APPENDIX 2 Proof of Tfworem 13 .21: Given that we have arrived at the set-up (3.10)-(3.13) the proof of u .u.b. foll o ws directly from [14). from which our proof derives. :\ote that for I I B I P; I I > E. I I ~u I I I = p(: .I ) and for I I B I pz I I
<
(A .1ll
E
I I ~u 1 1 I = p (z .1 ) I I B I pz I I
con v erge . t y rically 2 to 4 when the torques exceed the constraints. So if the torques are within the constraints the controller takes 5 .6 milliseconds to execute .
<
p(: .I )
The proof proceed s by choosing the Liapunov function candidate
Timing Informatio n
v (e) =
(A.13)
e I Pe
A calculation shows that al o ng trajectories of (3.10) Sample Interval
(A 12)
E
16 m s.
V satisfies
VVe
VCe) =
=-e I Qe +2e T PB[~ul+'T))
'.Iodel Dynamics and '.Iinimization Initial Comrutations
3.4 m s.
'.Iinimization Loop
2.2 m s.
=-e I Qe +2(B T Pe )T [~ul+7]) = e T Qe + 2(B I pz ) T [~u 1+ 7])
(AI4)
- 2(B I Pw ) T [~UI + 7])
APPENDIX 1
We have defined the uncertainty
'T)=Kw +E(v
-2(B T Pw)1 [~u 1 -
+~ul)+M - I(~h
+T d
(A.!)
)
2(B I pz)1 [~B T pz
with
B T Pw I'
li B Pw 11
+
E
\' = =
A 2z 2
I; I -
which attains a maximum value of The maximum I I B I pz I I > E and
Therefore
I I 'T) I I ~ 1 I Kw I 1+ 1 I E I I ( I I gd I I + I I K; I I +p( z .1 ))
v alue
rrovided
+ 1 I K I I 1 I; 1 I) + a ( + K J
)}
In practi ce a d etailed a na lys is of the uncertaint y IS probabl y no t required. Due t o limitati o ns o n the wo rk en v elope of the man ipu lator and bo un ds o n t he maximum attainable joint v elo c ilie~ . a rea sonab le assumption is to take
+d
I ; 1!
I
(AS )
w ith esti mated " alues of the co ns tants c and d based on the degree o f co uplin g in the manipulato r mechanical des ign and the maximum a ttainable v el ocities o f the link s. With this assumpti o n. then a strai ghtfor w ard calculatio n gi v es the eSllmate p ( ;.I ) =y+Ii I I; 11
B I pz I I B I pz I I
It foll ows that ~'. ( e)
term
occurs for
(A .16)
(A.17)
Ii = aA' + ad
Am, nCQ ) ! l e 11 2 -(}-+4 11 B T P I I 11 w 11)p ( e.1 »O(A .20) Thus it f o ll o ws f ro m s t a ndard arguments t ogether with (A .9 ) that the erro r is decreasi n g f o r e o ut s ide the ball of radius w where 2 I I B C P I I IV +
w=
(A .6)
(A .8)
( 2 1 ! B c P I I II'+-=- r
+b ( l i e I !
+
I
!A2ll 4 11 B T P I I W+-=-
2
____~--~~._-4___ b 2 + ----,-~"'~--2_ a Amn..( Q
r
An",,c Q )
The unif o rm ultimate bo undedness of e (I) now follows. :\ ote that (I( ) IS the s malles t le v el s urface of V (I) containing the ball B ( w) of radius w centered at € = O. If f o ES (I() then T (e " .S ) = 0 For € liS ( I( ) . \. (I ) is decreasing as long as e(1 ) EiS (I() and the so luti on reaches (I( ) in finite time. An urper bound o f th is interval is o btained by conSidering
as
as
I; I 1
~a ' +b !l e li
4b
(A .7)
:\o w define p(e .I ) fr o m
= a +b
(A.19)
Hovev er. Am, n( Q ) ll e 1 12 ~ e TQe ~ Am,,(Q )l l e 11 2 Thus . V is negati v e whene ver
+ II +aCc+KJ) 1
Y= ( I-Q )- I! I. A' I I \\·+O I q
< 0 prov ided that
e I Qe-{-=-+4 1I B l p l ll lw l l }p(e.I»O 2
where
I "' I 1 )
(A9 )
( A.22) where
:= p ( e.l ) with
=
third
}-'
Thus we have
+ a( I Ig'd I I (A.4 )
a'
the
I I B T pz 1 1
2
p(; .I ) = ( I-a )- 1{ Il K I ! W
~ a
}-p when
~' (e)~e I Qe +{-=-+4 1 I B T P II I l w I I }p(e.t).(A .18)
This definition of p(;.I) is w ell defined I ( Assumptio n A2 ) in ",hich case w e ha v e
p e; .r)
(A . 15)
and is equal to
)+KJ ) := p ( Z.1 )
(; .I ) = c
p)
4 1 I B I Pw I I p(z.l ) (A3)
~ I I KI I W+a(ll q JII + II K II Il z ll +p(z.I) )
a <
for
B I pz I I B I P: I I
B I Pw
+ II Arl ll ( I IM 1 1 + II Td 11 )
.1
p(Z.l))
p ( z .I ))
I I B I Pw 11
(A2) and so
+a ((z
11 B T P: 1 1
Lsing our definition of ~u I the second term above vanishes for I I B I pz 11 > E. If I IB I pz 11 < E then it becomes
from (3 .6) via
'T)
B r pz
~-e r Qe +2(B I Pz)T[~ul+
Table 3 .4 Controller Timing Data
a
+ bW
(A .10)
C,,=min{e TQe -(4 ;i B c P I I \\ ' +-=-)p(e.l) 2 e ES (k ., )\:nIS (fl}
(A 23)
'.1. \r. Spollg.
J.
S. Thorp and
(A.24) If
t
is the time until the solutio n intersects (A.23) implies
0> \ '(e(t» - \ '(e,,)
~
OS IT)
-C,,(t-t o )
J.
1\1. Kleill\\'aks
150
then
100
(A.25)
50
and so
(A .26)
-50 :il
1
:= T(e(,.s(f»
'" ~ 3
-100 -150 0.5
0.0
REFERENCES
Figure 3.3 Jo1 nt
[I]
Bejczy.A.C. "Robot Arm Dynamics and Control" . JPL Technical \lemorandum 33 -669. Feb. 1974.
[2]
Cvet ko vic.\'.. and VukobralOvic. \L "One Robust. Dynamic Control Algorithm for Man ipulation Systems". The 1nl. 1 . oj Robotics Research . Vol.l. 1\0. 4. pp. 15-28 . winter 1982.
200
[3]
Gutman. S .. "L'ncertain Dynamical Systems--A Liapunov min - max .-\pproach". IEEE hans . Aut. Cont . Vol. AC-24 :\0.3. pp437 -443. June 1979.
50
[4]
Luenberger. Optimi:ation by Vector Space Metfwds, John Wiley and Sons. Inc .. :\ew York. 1969.
[5]
Pau l. R.P .. Robot Manipulators: Mathematics, Programming and Control. \ IIT Press. Cambridge. Mass .. (1982).
[6]
Spong. ,\I.W .. Thorp. J.S .. and Kleinwaks. 1.'\1.. "The Cont r ol of Robot '\1anipulato rs with Bounded Input". to appear in IEEE Transactions on Automatic Control.
[7]
[8]
[9]
1.0
1.5
tlm. (t.c)
100
-sa :il
1
'" ~ 3
-100
-ISO -200 0.0
0.5
1.0
1.5
tlm. <.eel
Figure 3 . 4 Jo1 at 2
Commanded
TorQ ue
150 100
Spong, ,\I.W .. Thorp. 1.S .. and Kheradpir. S .. "The Control of Robo t \la nipu lalOrs using an Optimal Decision Strategy". Proc. 21st Allerton Conference, L·ni v. of Ill. pp . 303 - 311. Oct. 1983.
[10] Ha. In -loong. and Gilbert. E.G .. "Robust Tracking in :\onlinear Systems and its Application to Robotics". preprint ( 19(5)
COlllmanded Torq ue
150
Spong. W.W .. Thorp. 1.S .. a nd Kleinwaks. 1.\1 .. "The Control of Robot \Ianipulators with Bounded Input Part 11 : Robustness and Disturbance Rejection" . Proe. 23rd IEEE Conference on Decision And Control. Las Vegas. (Dec. 1984).
Thorp. lS .. and Barmish, B.R .. "On Guaranteed Stability of L'ncertain Linear Systems v ia Linear Control". 1. of Opt. Theory and Appl.. \ '01. 35. :\0. 4, pp. 559- 579. Dec. 198 1.
,
50
-50 ~
I
-100
~
3
-150 0.0
[11] Barmish. B.R .. Petersen, I.R .. and Feuer. A .. "Linear Uti mate Boundedness Contro l of L'ncertain Dvnamical Svsterns". Autonwtica.\'. 19. :\0. 5. pp 523-532. ( 1983 ). .
0 .5
1.0
1 .5
tlm. (uc l
Figure 3.5 Jo1 n t
[12] Spong. \l.W . "Torque Optimization for Robust Control of Robot \l anipulalOrs". SME Conference on Robotics Research. Lehigh l'niversity. (Aug. 1984 ).
3
Commanded
Torq ue
I.B 1.6
[13] Spo ng, '.1.W .. Thorp. 1.S. and k lei nwa ks. J'.I. . "On Point wise Opt im al Contro l Strategies for Robot \l anip ul ators". Proc 18th Confer'ence on I nforTTUll ion Sciences and Sys· terns .. Princeton. ( \l arch 1984).
1,
1.4 1.2
i.
1.0
[14] Le itmann. G .. "On t he EffIcac\' of :\onlinear Contro l in L'ncertain Linear Svstems". lour~al of Dynamic Systems. Mea surement. and Control, lune 1981. \ '01. 102.
ut
O. B 0.6 'a
~,
1.4
,g
id
1.2
0 .2 0 .0 0.0
I.°
1"
t
~)
0.4
0.2
0 .4
0.6 \lm.
O. B
1.0
1.2
1.4
(ue )
o. B
F1,ure 3.6 Trajectories Cor Unloaded Syat ••
1,
0.4
O. O,+-~""f'=--+----j--l----+---+---+---4 0.0
0.2
0.4
0.6
O. B
1.0
1.2
1.4
FIgure 3 . 2 OOS Control Joint TraJectorie.
©
I F,\(
R.llilll l (:1I111rld "p.li l1. I ~ I ;-.i .-I
( S\nJ(u ';-.i,-H . Itlnt'lull ;!.
ROBUST MICROPROCESSOR CONTROL OF ROBOT MANIPULATORS M. W. Spong'\ ::' / )f,/Hlrllflt'lll ( 'IIIi'!'fllly
(1/
11/
J. S. Thorp** and J. M. Klei nwaks***
(11111 Ilu ' ( :flOrd/ llo/nl \(il'll(l' Lo/Ju1'olun', 1111111111 f/I ( ' I II//I/lI-C/I/III//Jiligll, ('rhl/III/, 1/_ (>!Slil. ( ·S.4 .
(;"III'}(// FIf ,!.!. 'i llf'f'}'ill}!,'
""" S,-IIIIIIIII/ FII'r/ri'l/l F Il!!.·illl'l'rill,!!.. ( :111"111'1/ ( ·lIh'I'ni/\". Ilhl/{(t, .\" ) ' 1-I85J, [ '.'1. 4 ""''''' Sillg!'f CIIII//JilIIL fl illgllli/I/IIIII. '\T. ( '.'i..\
ABSTRACT: In thi s paper w e study the problem of ro bust tracking for robot manipulators in the presence of u ncertaint y and input constrai nts . Lsing the theory of uncertain dynamical s....',-rems we derive rob us t nonlinear control strategies. with guaranteed tracking properties that can be quantified given bounds on the extent of model uncertainty, senso r noise. input disturba nces. etc. We a lso utili ze a wrque oprimi:arion Slraregv to optimize the jo int torques in the event of actuator saturation. The algorithm has been implemented usi ng a single \1 0to rola 12 \ lh z '\1(:611000 microrrocessor on a three link re vo lu te j0int manipulator cons tructed by the Schoo l of \ Iech an ica l Engi neering a t Cornell Lni versity . Experimental results are prese nted here s howi ng the feasibility and perfo rmance of the control s trategy . Ke y word s: robotics. microprocessor . robust control. actuator sat uration, o pt imal control I. INTRODUCTION
H. BACKGROUND
In this paper we continue o ur investigations on the control of ro bot manipulators s ubject to bounds on the avai lable torque at each joint [6).[S), and s ubjec t to uncertaint y (7). In the first part of this work [6) a po int ""ise o ptima l contro l scheme. know n as an Optimal Decision SU'aregv (ODS) ""as derived wh ich com putes the vector of input torq ues to the manipulator at each sample instant l in s uch a way that the joint accelerations are optimized based on a prescribed pa th trac king criterion. The res ulting control la"" . the design of w hich was reduced to the solution of a quadrat ic prog rammin g probl em . was s hown to ha ve certain practical and theoretical Sign ifica nce . Firs t. by o ptimi zing the joint accelerations when one or m ore of the inruts saturates. s ignificant improvement in the resu lting performance of the system may result. w hi ch allows for fa ster motion of the man ipulator w hi le si multa neo usly reduc ing the need fo r exten s ive off - line traject ory rlannin g.
Consider the equations of motion of an 71 -joint r igid ma n ipulato r deri ved from the Euler-Lagrange equations [I)
Second. it was s how n that. because 0f the s tructure of the manipulato r dvnamic eq uations , anv contro l sc heme ma v be incorporated into thi s framewo rk , The resulting torqu.e opti~i =a tion schemer 12) rrov id es a mec hanism for handlin g input co nstraints fo r any control scheme, In the rresent rarer \I,e st ud y the iss ue of robustness to uncertainties which are present in any physical system , For a r obo t ma nirulator. t hese ma y inc lude unknown loa ds. u nmodeled dynamics . f riction. neglected s mall time delays . etc. l"sing the theory of uncertain system s d eve lored in [3).[9).[1 1).[ 14). we deri ve a res ult on ro bust trac k ing con trol for manirulat ors wi th bounded uncertai nt y. Incorpora t ing the abow roh ust cllntrol ler design in to the rrevio us torque optimi za ti o n scheme results in a ro bus t. rointwise o pt imal cont ro l sc heme wh ich is Imrlement ab le in real time . FinallY. using a si ngle \I oto rola 12\l hz \IC6S000 microrrocesso r. o ur co ntro l algo rithm was implemented o n a three re vo lute jo int m anirul ato r co nstr ucted b\ the Schoo l of \ lechanical Eng ineering at Co rn ell l"ni \"e rsity We rrese nt so me of the exper imental res ults in thI S r a rer. which s how the feasibility and perfor mance of t he algorithm . In what fol lo"" s R " will denote the us ual n-dim ensiona l vector srace O\' er R end o",'ed with the Euclidean or L r norm . All secto rs are colu mn \·ecto rs. A s uperscript T indicates tran spose of a vector or matrix . Amm(.\/ ). A""x(M) denote the min imum o r max im um eigen\'alues of a matrix \ 1. respecti\'ely , \Iatr ix norms are those induced by the corresponding vect or no rm ,
t
D, } (q )ii)
J =1
+
t
t D ,j, (q
)g}ej, + Di(q) = Ti + Tdi
(2.1)
J =H = 1
ii, + a,g, +
I,T,
b,u, . i
=
=1. '
.71 .
where q (t ) E R n are the generalized coordi nates representing joi nt positions. Equation (2. 1) represents the mechanical part of the system . T d , is a disturbance torque at the i - th joint . which could for ex a mple represen t frictional forces and torques. si nce these are d iffIcult to model precisel y. Equation (2.2) represents the d y namics of the i - th actuator, where a , .f, .b, are constants. and u, is the input for the i - th ac tuator . Equations (2.1 )-( 2 .2) ca n be written in matrix form as 1>1 (q
)(1 + h (q .g ) = Bu + FT d
where 1>1 (I{ ) is a positi ve de fi nite m, } (q ) =
m u (q )
h (q
.q ) is an 71
f, D,} for
=I
(23)
matrix with elements
71 Xn
i ... j and
(2.4 )
+ I ,Du .
-vecto r val ued function h (q .cj )
= ( h 1(1{ .g ).
(25 )
w here h , ( q.q ) = a, q,
t
+ 1, (
t
D ,},
g}g, + D ,)
(26)
} =H =1
a nd
F =diag (/,.
.f n ).
B = diag (b!.
.bn
).
( 27)
\\' e assume that none of the I, .b , is zero and for simpliCity ""e ""i1 1 s urpose that the input and disturbance coefficients ha v e been nor mali zed to unit y so that (2.3) may be written A1 (q
)il + h
(q
.g ) =
u
+ Td
(2.11 )
Thu s u (r ) represents the inrut torque to each joi nt at time T .! is an input disturbance which is assumed to be Lebesgue meas urable. sa tisfyi ng
t and
(2 .9) \\'e assu me that the inrut to rque T(t ) is constrained v ia ( 2 10) i =1.
,n
w hic h we ma y wr ite as
\(. \\'. Spollg . .J. S. Thorp and.J. \1. I\.leill\\aks
NT(C)
~
dc)
(2.11)
where the 2n Xn matrix N and the 2n vector e (C ) are given by
o
o o
-I 0
N
7
T lm ,"
. e(c)
o o
0 0
1mo
._
Ct)
-I
We will need to make the following assumptions on the plant (2.8) and the model (2.19). A]) The inertia matrix M (q) in (2.8) is positive definite, and there exists a positive constant a such that
(c)
=
re-adjust the input torques in the event that they would otherwise saturate .
I I M (q )-1 I I ~ a
(2.12) T nma ,
(c )
7 nmm
Ct )
<
00
for q E Rn
A2) There exists a nonnegative constant Q < for all q ER n and for all w such that I I w I I < W.
(2.22) such that (2.23)
In practice it is important to allow for time varying torque limits as we have done. Since our later torque optimization is performed pointwise, i.e .. at eacb sample instant, the time varying input constraints are easily bandIed.
A3) There exists a continuous function cp(x.w.r), bounded on each finite time interval [O.T). such that for all q ,q ERn and all W I ,W2 with IlwI11 ,l l w211 ~ W.
We suppose that we have available for feedback the measured joint positions and velocities represented as
where x (C) := (q r (c
Y
11
Y2,
= q, =q,
+
W I,
(2.13)
+W21
i=I, .. · ,n. Here w(c) (WI,(C), ' w l n (c),W21(C), . . . 'W' n (c))' represents measurement uncertainty (noise, etc. ) and we assume only that w" (c ) are Lebesgue measurable func tions of time satisfying 5upllw(c)11 ~ W I
~o
<00
I I h (q
+W I,q +w2)-h (q ,q) 11 ~
).q r
cp(x ,w ,c)
(2.24)
(C ))' denotes the system state.
Assumption A2) is the most crucial and, in a real sense. tells us how accurate an estimate of the inertia of the system we must have in order to control it.
Ill. ERROR SPACE ANALYSIS AND DESIGN For the problem of tracking the desired trajecto ries (2.15) we form the position and v elocit y error vectors
(2.14)
el=q-qd
We let
(3.])
e2=q-i/
(2.15) represent a desired trajectory in joint space that we wish the manipulator to track. We shall assume that qd (c) is continuously differentiable with the acceleration ii d (c) bounded in norm. i.e., I I ii d I I ~
B ~
00
The error dynamics may then be written as a first order vector differential equation
e1 = e2 .
(2.17)
" d
= -M-Ih
(2.16)
We shall implement an overall feedback control scheme of the following form:
..
(32)
e2=q -q
+ M - I(u + T d
"d
) _
so that in "error space" the problem of path tracking reduces to one of stabilizing the (time-varying) system (3.2). If the control law u is now given b y (2.17) (with Au 2 for the present) the closed loop dynamics are given by
with
e1 = e 2 e2=-M - Ih +M - I(M(v +AUI)+h +Td )-q d
:\ote that in (2.17)-(2.18) we use the measured, hence uncertain, state variables. In the expression (2.17), M ,h correspond to an "available model" of the manipulator 1>1(q )ii
+ h(q
,q) = u
(219)
which may differ from (2.S) in several respects. M C),h C) may be simplified ve rsions of M C).h ( .) to facilitate their online computation. For example, one may choose to ignore the actuator dynj1mic~ 0.2). o r certain coupling terms in M or h ete. Also . 1>1 C).h ( .) ma y contain "nominal" or estimated values based on. ~ay, an u~loaded arm, in which case the error represented b y 1>1 - M, h - h would be due in part to the manipulator load. The ju st ification fo r the form o f our control law (2 .17) is that if AliI
=
M (Y I ) -
AI (q ) := 0
Ah :=h(YI'Y 2) -h(q.q):=O
(3.4)
a straightforward calculation shows that
e2 ::::; A
le I
+ A ze::o + 1') + tl.u
I
(35)
where 1] is given by 1]=-A l w l-A , w 2 + E (\' +Aul)+M -I( Ah +1 d )(3. 6 ) :=Kw +E(v +!:wl)+M - I(Ah +1 d
)
where
K = [-A I,-A 2] 1] thus incorpo rates all the uncertaint y that we hO"e assumed
exists In the closed loo p system . and the term Au I is to be des igned t o overcome the effects of this uncertainty. Set =1=YI-q J
Au = 0
Au = Au I + Au 2 ·
(3 .3)
Setting
(220)
then (2.17) is just the ideal inverse dynamICS control law [J]. which yieldS an owrall system which is decoupled. linear. time- in va riant with presc ribed eigenvalues determined bv A 1..4 2' Howewr, the ideal computed torque is unachieY'ab le in practice due the aforementioned difficulties of uncertaint y. saturallon, etc .. and additional compensation ( represented b y Au ) must be introduced. We propose here a two-stage deSIgn procedure for computing Au .
=0
(37)
=2 =y : o -cj J so that
= :=(=\.=jJT
=e +w
(U)
is the "measured" tracking error. We sha ll assume only that the uncertainty 1] in (3.6) can be bounded by' a kn ow n continuous function p(= .r ) .i.e .. 1 11) 1 i ~p(=.I)
0 .9)
(22])
First. Au I is designed assuming n o torque constraints to overcome the effects of the model uncertainty (represented by ;1M ,::'h ), the input disturbance T d and the measurement uncer taint y w (r). AU, is then designed using the point wise torque optimization strategy of [7].[12] and the known value of Au I to
See Appendix 1 for a disc uss ion of how p ( = .r ) may be es timated . Finally the error equati o ns 0.3 ) may be written in matrix form as
e= with
Ae
+ B (1] + ;1u I)
(310)
COlltrol of' Robot
~() ;j
~lalliplllalOrs
IV. TORQUE OPTIMIZATION (311) Since the matrix A is at the discretion of the designer it ma y be chosen stable and with prescribed eigenvalues . A typical choice might be AI =_.\2/ . A 2 =-2.\/ so that the nominal closed loop system. i.e .. the system in the absence of uncertainty is decoupled and critically damped with "closed loop bandwidth .\ ".
Since A is stable. choose a 2n X2n symmetric. positive definite matrix Q and let P be the unique positive definite symmetric solution to the Liapunov equation
+
A TP
PA = Q
The preceding results show that given bounds on the extent of uncertainty in (3 .10) whether due to modeling errors. unknown loads or disturbances, etc ., uniform ultimate boundedness of the tracking error can be achieved with either the saturating non linear control law (3 .13).or the linear control (3.16 ). These results do not take into account actuator saturation howeve r. In this section we apply the torque optimi:alion scheme of [7].[ 12J to account for input constraints. The reader should consult the above references for details. Given the vector of input torques from (2,17). call it aCt). together with (3.13) or (3.16) we choose the actual input torque from the following single stage torque optimization procedure:
(3 .12)
min(a - u )T TICa - u )
(4.1 )
subject to Nu :::; c. with n = it-IQit - 1
Definicion 3./: Given a solution
of (3.10). we say the e (-) is uniformly ulcimacely bounded (u.u.b.) with respect to a set S if there is a nonnegative constant T(eo.s) < 00 such that e(c) E S for all c ~co + T.
Q is an additional design parameter that can be used to weight the components of the torque vector. It is important to note that (4.1) is solvable even for an unknown plant since a(/) is known and it is known . The optimal control input that results is then
We next present two designs for choosing /:'u I. one linear and one nonlinear. each of which guarantees uniform ultimate boundedness of e (c) in (3.10).
where.\' is the solution of the dual optimization
eC): [c o.oo ) ~ R2n. etc~) =e o
Theorem 3 .2: (nonlinear compensation) The error e (c) governed by (3.10) is u.u.b. with respect to the set S (defined below) if the control /:'u I is chosen as
-pCz /:'UI
.1 )
B T pz
: if 1 I B T pz I 1
II B T pz 1I
=
>
.c ) B T pz : if
l i B T pz I I
<
E
with I<
= .\max(P )w
E R
~n
1 eT Pe
with
J = Nu + C
!!.u
Proof: See Appendix 2 where it is shown that the uniform ulti mate boundedness set S is the ellipsoid
le
< 1<)
= /:'u I + !!.u ~
with !!.ul given by (3.13) or (3.16) and /:'u2
= -Q ~itN T .\'
(3.15)
with C u defined b y (A 19). k" = ef,Pe". Remark 3.3: The control law (3.13) is continuous in : for anv E >0. and thus (3 . 10)-(3.13) has a unique solution which can be defined over [c ". 00 ) for any initial error e ". An inspection of A .20 shows that the s ize of the set S . and hence the tracking precision. depends both on E and on the tightness of the estimate of the uncertaint y p ( : .1). For s maller values of E the radiu s of the uniform ultimate boundedness set decreases and hence the track ing precision increases. In theor y if W (I ) :; O. i.e .. there is no measu rement uncertainty, then w - 0 as € - O. In o ther words the S\'stem can be made asymptotically stable by the control law (3 . 13 ). Howe\·er. for E = 0 the contro l law (3.13) is discontinu ous and result s in a chattering controller. Further. for /:'u I discontinuous. one is forced into the noti on of generalized ordinar y differential equations.
In the terminology of Barmish. el.al.[IIJ Theorem 3.2 says that the uncertain syste m (3 .10) is uniformly and ulcimacely bound able in a quadratic manner (u .u.b.q.) with the control law (3 . 13) . It immediatel\' follo"'s then from ([It]. Theorem 4.1) that (3.10) is u.u.b.q . via linear COrllral. Specifically we have
In order to demonstrate the feaSibilit y of implementing the robusc torque optimi:ation algorithm (4 .1). a real-time implemen tation of the control strategy was developed for a three degree of freedom manipulator built by the Schoo l of \lechanical Engineering at Cornell. The robo t configuration is similar to that mode led for the simulation tests in [6]. The goal of this implementation was to dri ve the robot arm from a commercially available microprocessor single board computer ( SBC) using a high level language to implement the control algorithm. The manipulator arm consists of three rotary Joints with links two and three each ten inches in length . The arm is drI ven by three DC servomotors and can support up to a two pound load at the end of link two . The s pecifiC parameters for this manipulator are defined in Table 3 .1. The available or nominal chosen of the form 0 .19 ) with Thu s the inertia matrix .';1 ( q ) for the rigid mechanical part of
model of the manipulator was the actuat or dvnamics neglected . is taken to be'the inertia matrIX the sys tem .
Link Parameters
0.02245
Theorem 3.3 (linea r compensation) There exists y > 0 suffiCiently large so that the error e Cc) governed by (3 .10 ) is u .u .b. with ~u I chosen as
The detail s of the proof of Theorem 3 .3 can be [IIJ and are therefore om itted here . In the case of no ment uncertaintv. w Ct) :; O. Theorem 3.3 also follows earlier work of 'Thorp and Barmish [9J . We refer the references [9J.[IIJ for the calculation of y and the boundedness set S in this case.
(46)
V. REAL TIME IMPLEMENT A TION
o : e o E S(I<)
~ uCt) = -yB ' P:
(45)
(314)
and w defined by (.0.20) and that k o-I< ---c;;: e o e S (I<)
(4.3)
The overall control law that results is then (2.17) with !!.U in (2.21) given b y
for a given E>O and p.P.B.z as above.
2
(4.2)
(4.4)
E
S (I<) =
minl..!...\ T TJ.\ +.\T J} ,, ~o 2
E
(313)
-p(z
u'(c) = a(t) - n-IN T.\'
(316) found in measurefrom the reader to ultimate
0 .0165 0 .00&23
Link I length :
10 in .
Link 2 length :
10 in.
\1. \\'. SpOil).; .. 1. S. Tllmp alld
206
J.
\1. Kkill\\ ,lks
generates code that performs 32 bit arithmetic regardless of the size of the operands. To improve performance. software variable size was defined to be 16 bits. the standard word size of the '.1C68000. although the processor can also perform 32 bit arith-
Joint Parameters
metic.
jOint I
joint 3
joint 2
max torqueCin.lb.)
119
191
119
motor gear ratio
14&.51 :1
153 .0: 1
14&.51:1
± 120
range(deg. )
-IOto+160
± 120
Table 3.1 Robot Parameters The computer controller is a SBC based on the :vIOlorola MC68000 16-bit microprocessor running at 12 MHz. The SBC provides torque commands to the robot motors through 12- bit O/ A·s. The manipulator state. angular pOSition and velocities. are fed back to the computer from optical encoders mounted at the motor shafts. The encoder outputs are converted to a count representing position and are read by the computer through a 16-bit parallel port. The system is structured such that at each sample time. a programmable timer generates an interrupt to the processor. The interrupt service routine reads the count from the optical encoders and then calls the torque optimization routine. The remainder of the time the processor monitors the system terminal for input commands or other control information. \lotorola \lC68000
Clock s peed
12 \lhz
Data Bus
16 bits
Address Bus
24 bits
Internal Regi ster Size
32 bits
Add Ins!. Time
0.33/.! sec
\Iultipl y Instr. Time
3.5 p. sec
Divide Instr. Time
10.16 p. sec
The compiler library functions that perform the multiplication and division operations have been replaced by custom routines that perform these operations using 16 bit arithmetic operations. This provides a factor of three reduction in compu tation time for these particular operations. which are the most time consuming operations in the processor's instruction se!. One other modification was made in the name of effiCiency . The compiler's library routines for sine and cosine were replaced by an assembly language function that evaluates the sin and cos of an operand through a 256 element table look up with interpolation. Apart from these two modifications to the library routines. all code generated for the controller was wrilten in C. The full controller was implemented in the \lC68000 micropros:essor. The dynamjcs of the robot speCified by the matrix M and the vector h were coded into the controller. Lnmodeled parameters include motor dynamics and friction. Additionally. robustness of the controller to parameter va riation and un mode led dynamics was addressed by adding linear error feedback terms according to (3 .16). The controller algorithm is as follows: (I)
Read optical encoder outputs and comrute joint angles and
(2)
Compute trigonometric terms used in /If and h computations (done only once each sample period for efficiency).
(3)
Compute
(4)
Compute trajectory model dynamics. iid'
velocities.
NI
matrix.;; vector. and unconstrained torque. toq~
(5)
Add robustness terms
(6)
Compute IJ matrix terms ( IJ = N direction vector. and initial guess at A.
from equation (3.13) or (3.16).
(7)
While A's have not converged: compute A.
(8)
Compute inrut torques from (4 16).
T
NI TNil!
). initial
Lpon command from the system terminal. joint angles and torques are stored and can be uploaded to a \,A,\ 111750 for analysis and plolling. The reference trajectory model for the controller was set up to be a second order criticall y damped tra jector y with a 12 radian bandwidth. i.e.
where r is the input command. nomina lly a step command in position. A rem ovable two round loa d is placed on the end of the manipulator. Tests runs were made both with and without this load . Changes in the load were not accounted for in the con troller in order to test rob ust ness of the controller. The manipu lat or was run with various command inputs . H owever, shown
here are just step joint responses from an initial posi tion in radi -
Table 3.1 \l otorola \lC6S()()() Characteristics Softwa re for the sy"em was developed in the C programming language. a hi gh le\'el genera l purpose computer lan guage. The impetus for using C was rrimarily due to the a\'ailabillly of a C cross-compiler for the \lC6S000 . In as much as the SBC does not h3\'e a fioatlng pOint processor. all floating point arithmetic orerations are performed in
~oftware.
which causes a
significant perfor mance bOllleneck. Therefore . in order to max imize performance. fixed poi nt arithmetic was utilized rather than floating point operall ons. Fixed point ari thmetic is not part of the C language speCification. so fixed pOint opera tions were implemented utilizing a system referred to as B-scaling. The
ans of (0.0.0) to a terminal state of
(.y.2j-'i)' ThiS replicates
the tes t cases run in the SI\l:\O:\ simu latlOns of [6j. The results are shown In Figure 3.2 f or the maneuver described abo ve " ' ith the load allached. Figure 3.2 shows the joint angles. each profile labeled for its respect ive joint. Figures 3.3. 3.4. 3.5 show the corresponding torque commands reqUired to rerform the maneuver. Figure 3.6 sho"'s the joint angle resronses with the load remo\'ed from the end of link 2. The s tead y state errors for the two configurati o ns afe ~ h own in TabJe 3.3.
maximum va riation of a parameter is determined and rounded t o
the next larges t power of two. Thi s speCifies the location of the decimal point in the variable. which is now represented by an integer. Therefore. all operations are executed usi ng integer arithmetic. This res ults in significantly faster computation. since these operations are part of the \lC6&000 instruction se!. Care must be taken in USing s uch a syste m to ensure that computed variables do not exceed their prespecifled range. resulting in an overflo".. COndlllOn . Additionallv. the order of computation of a given equation can affect the precision of the result. The C compiler standard word size IS 32 bits. although declarations of short words. 16 bits. are possible. The compiler
Joi nt >.ngle Position Frror I
Tes t Case
Joint I
i
Joint 2
I i
Joi nt 3 1
!
with load
0.40'7,
0.22('",
I
I 15'7,
I
without load
OAO<;',
022 <;',
I
1.71 '7,
i
Table 3.3 Joint Angle Error
207
Control of Rohot "Iallipulators The controller was run at 62.5 H2. or a 16 millisecond sample interval. This is the maximum effective rate at which the controller can run and still rro v ide enough time for a reasonable number of iterations in the minimi zation section of the algorithm . Table 3 .4 sho w s the breakd own of time spent in the con tro ller. Hence the total contro ller time is (3.4 + 2.2n) milliseconds.
n is the number of iterations for the minimization to
APPENDIX 2 Proof of Tfworem 13 .21: Given that we have arrived at the set-up (3.10)-(3.13) the proof of u .u.b. foll o ws directly from [14). from which our proof derives. :\ote that for I I B I P; I I > E. I I ~u I I I = p(: .I ) and for I I B I pz I I
<
(A .1ll
E
I I ~u 1 1 I = p (z .1 ) I I B I pz I I
con v erge . t y rically 2 to 4 when the torques exceed the constraints. So if the torques are within the constraints the controller takes 5 .6 milliseconds to execute .
<
p(: .I )
The proof proceed s by choosing the Liapunov function candidate
Timing Informatio n
v (e) =
(A.13)
e I Pe
A calculation shows that al o ng trajectories of (3.10) Sample Interval
(A 12)
E
16 m s.
V satisfies
VVe
VCe) =
=-e I Qe +2e T PB[~ul+'T))
'.Iodel Dynamics and '.Iinimization Initial Comrutations
3.4 m s.
'.Iinimization Loop
2.2 m s.
=-e I Qe +2(B T Pe )T [~ul+7]) = e T Qe + 2(B I pz ) T [~u 1+ 7])
(AI4)
- 2(B I Pw ) T [~UI + 7])
APPENDIX 1
We have defined the uncertainty
'T)=Kw +E(v
-2(B T Pw)1 [~u 1 -
+~ul)+M - I(~h
+T d
(A.!)
)
2(B I pz)1 [~B T pz
with
B T Pw I'
li B Pw 11
+
E
\' = =
A 2z 2
I; I -
which attains a maximum value of The maximum I I B I pz I I > E and
Therefore
I I 'T) I I ~ 1 I Kw I 1+ 1 I E I I ( I I gd I I + I I K; I I +p( z .1 ))
v alue
rrovided
+ 1 I K I I 1 I; 1 I) + a ( + K J
)}
In practi ce a d etailed a na lys is of the uncertaint y IS probabl y no t required. Due t o limitati o ns o n the wo rk en v elope of the man ipu lator and bo un ds o n t he maximum attainable joint v elo c ilie~ . a rea sonab le assumption is to take
+d
I ; 1!
I
(AS )
w ith esti mated " alues of the co ns tants c and d based on the degree o f co uplin g in the manipulato r mechanical des ign and the maximum a ttainable v el ocities o f the link s. With this assumpti o n. then a strai ghtfor w ard calculatio n gi v es the eSllmate p ( ;.I ) =y+Ii I I; 11
B I pz I I B I pz I I
It foll ows that ~'. ( e)
term
occurs for
(A .16)
(A.17)
Ii = aA' + ad
Am, nCQ ) ! l e 11 2 -(}-+4 11 B T P I I 11 w 11)p ( e.1 »O(A .20) Thus it f o ll o ws f ro m s t a ndard arguments t ogether with (A .9 ) that the erro r is decreasi n g f o r e o ut s ide the ball of radius w where 2 I I B C P I I IV +
w=
(A .6)
(A .8)
( 2 1 ! B c P I I II'+-=- r
+b ( l i e I !
+
I
!A2ll 4 11 B T P I I W+-=-
2
____~--~~._-4___ b 2 + ----,-~"'~--2_ a Amn..( Q
r
An",,c Q )
The unif o rm ultimate bo undedness of e (I) now follows. :\ ote that (I( ) IS the s malles t le v el s urface of V (I) containing the ball B ( w) of radius w centered at € = O. If f o ES (I() then T (e " .S ) = 0 For € liS ( I( ) . \. (I ) is decreasing as long as e(1 ) EiS (I() and the so luti on reaches (I( ) in finite time. An urper bound o f th is interval is o btained by conSidering
as
as
I; I 1
~a ' +b !l e li
4b
(A .7)
:\o w define p(e .I ) fr o m
= a +b
(A.19)
Hovev er. Am, n( Q ) ll e 1 12 ~ e TQe ~ Am,,(Q )l l e 11 2 Thus . V is negati v e whene ver
+ II +aCc+KJ) 1
Y= ( I-Q )- I! I. A' I I \\·+O I q
< 0 prov ided that
e I Qe-{-=-+4 1I B l p l ll lw l l }p(e.I»O 2
where
I "' I 1 )
(A9 )
( A.22) where
:= p ( e.l ) with
=
third
}-'
Thus we have
+ a( I Ig'd I I (A.4 )
a'
the
I I B T pz 1 1
2
p(; .I ) = ( I-a )- 1{ Il K I ! W
~ a
}-p when
~' (e)~e I Qe +{-=-+4 1 I B T P II I l w I I }p(e.t).(A .18)
This definition of p(;.I) is w ell defined I ( Assumptio n A2 ) in ",hich case w e ha v e
p e; .r)
(A . 15)
and is equal to
)+KJ ) := p ( Z.1 )
(; .I ) = c
p)
4 1 I B I Pw I I p(z.l ) (A3)
~ I I KI I W+a(ll q JII + II K II Il z ll +p(z.I) )
a <
for
B I pz I I B I P: I I
B I Pw
+ II Arl ll ( I IM 1 1 + II Td 11 )
.1
p(Z.l))
p ( z .I ))
I I B I Pw 11
(A2) and so
+a ((z
11 B T P: 1 1
Lsing our definition of ~u I the second term above vanishes for I I B I pz 11 > E. If I IB I pz 11 < E then it becomes
from (3 .6) via
'T)
B r pz
~-e r Qe +2(B I Pz)T[~ul+
Table 3 .4 Controller Timing Data
a
+ bW
(A .10)
C,,=min{e TQe -(4 ;i B c P I I \\ ' +-=-)p(e.l) 2 e ES (k ., )\:nIS (fl}
(A 23)
'.1. \r. Spollg.
J.
S. Thorp and
(A.24) If
t
is the time until the solutio n intersects (A.23) implies
0> \ '(e(t» - \ '(e,,)
~
OS IT)
-C,,(t-t o )
J.
1\1. Kleill\\'aks
150
then
100
(A.25)
50
and so
(A .26)
-50 :il
1
:= T(e(,.s(f»
'" ~ 3
-100 -150 0.5
0.0
REFERENCES
Figure 3.3 Jo1 nt
[I]
Bejczy.A.C. "Robot Arm Dynamics and Control" . JPL Technical \lemorandum 33 -669. Feb. 1974.
[2]
Cvet ko vic.\'.. and VukobralOvic. \L "One Robust. Dynamic Control Algorithm for Man ipulation Systems". The 1nl. 1 . oj Robotics Research . Vol.l. 1\0. 4. pp. 15-28 . winter 1982.
200
[3]
Gutman. S .. "L'ncertain Dynamical Systems--A Liapunov min - max .-\pproach". IEEE hans . Aut. Cont . Vol. AC-24 :\0.3. pp437 -443. June 1979.
50
[4]
Luenberger. Optimi:ation by Vector Space Metfwds, John Wiley and Sons. Inc .. :\ew York. 1969.
[5]
Pau l. R.P .. Robot Manipulators: Mathematics, Programming and Control. \ IIT Press. Cambridge. Mass .. (1982).
[6]
Spong. ,\I.W .. Thorp. J.S .. and Kleinwaks. 1.'\1.. "The Cont r ol of Robot '\1anipulato rs with Bounded Input". to appear in IEEE Transactions on Automatic Control.
[7]
[8]
[9]
1.0
1.5
tlm. (t.c)
100
-sa :il
1
'" ~ 3
-100
-ISO -200 0.0
0.5
1.0
1.5
tlm. <.eel
Figure 3 . 4 Jo1 at 2
Commanded
TorQ ue
150 100
Spong, ,\I.W .. Thorp. 1.S .. and Kheradpir. S .. "The Control of Robo t \la nipu lalOrs using an Optimal Decision Strategy". Proc. 21st Allerton Conference, L·ni v. of Ill. pp . 303 - 311. Oct. 1983.
[10] Ha. In -loong. and Gilbert. E.G .. "Robust Tracking in :\onlinear Systems and its Application to Robotics". preprint ( 19(5)
COlllmanded Torq ue
150
Spong. W.W .. Thorp. 1.S .. a nd Kleinwaks. 1.\1 .. "The Control of Robot \Ianipulators with Bounded Input Part 11 : Robustness and Disturbance Rejection" . Proe. 23rd IEEE Conference on Decision And Control. Las Vegas. (Dec. 1984).
Thorp. lS .. and Barmish, B.R .. "On Guaranteed Stability of L'ncertain Linear Systems v ia Linear Control". 1. of Opt. Theory and Appl.. \ '01. 35. :\0. 4, pp. 559- 579. Dec. 198 1.
,
50
-50 ~
I
-100
~
3
-150 0.0
[11] Barmish. B.R .. Petersen, I.R .. and Feuer. A .. "Linear Uti mate Boundedness Contro l of L'ncertain Dvnamical Svsterns". Autonwtica.\'. 19. :\0. 5. pp 523-532. ( 1983 ). .
0 .5
1.0
1 .5
tlm. (uc l
Figure 3.5 Jo1 n t
[12] Spong. \l.W . "Torque Optimization for Robust Control of Robot \l anipulalOrs". SME Conference on Robotics Research. Lehigh l'niversity. (Aug. 1984 ).
3
Commanded
Torq ue
I.B 1.6
[13] Spo ng, '.1.W .. Thorp. 1.S. and k lei nwa ks. J'.I. . "On Point wise Opt im al Contro l Strategies for Robot \l anip ul ators". Proc 18th Confer'ence on I nforTTUll ion Sciences and Sys· terns .. Princeton. ( \l arch 1984).
1,
1.4 1.2
i.
1.0
[14] Le itmann. G .. "On t he EffIcac\' of :\onlinear Contro l in L'ncertain Linear Svstems". lour~al of Dynamic Systems. Mea surement. and Control, lune 1981. \ '01. 102.
ut
O. B 0.6 'a
~,
1.4
,g
id
1.2
0 .2 0 .0 0.0
I.°
1"
t
~)
0.4
0.2
0 .4
0.6 \lm.
O. B
1.0
1.2
1.4
(ue )
o. B
F1,ure 3.6 Trajectories Cor Unloaded Syat ••
1,
0.4
O. O,+-~""f'=--+----j--l----+---+---+---4 0.0
0.2
0.4
0.6
O. B
1.0
1.2
1.4
FIgure 3 . 2 OOS Control Joint TraJectorie.