- Email: [email protected]
Optimal trajectory control of robotic manipulators
Mechanism and Machine Theory Vol. 19, No. 2, pp. 26%273, 1984 Printed in Great Britain.
0094-114X/84 $3.00 + .00 © 1984 Pergamon Press Ltd.
OPTIMAL TRAJECTORY CONTROL OF ROBOTIC MANIPULATORS A. H A N A F I and F. W. W R I G H T Electrical, Electronic and Control Engineering Department, Sunderland Polytechnic. Sunderland, England and J. R. H E W l T Mechanical Engineering Department. University of Newcastle upon Tyne, Newcastle, England
(Receivedfor publication 26 January 1983) Abstract--The problems associated with the achievement of desired trajectories for non-linear robotic manipulatory systems are discussed. Optimisation of both free and fixed structured systems is considered and projection approximations to the open-loop controls for the free structured system are used to provide a practical feedback solution to the problem.
1. INTRODUCTION--ROBOT CONTROL METHODS
A robotic manipulator conventionally consists of a number of rigid links connected as an open chain by a number of powered joints. The power inputs to the joint motors are converted into movement of the links and must be controlled so as to cause the "end effector" or "tool" to take up the required position and orientation within the working space. As the required position and orientation of the tool change with time so the tool co-ordinates trace out a trajectory which must be close enough to the ideal to permit satisfactory progress of the task being undertaken by the tool. This control system design poses a number of severe problems, particularly where high operational accuracy and high speed are simultaneously demanded. A number of approaches have been proposed recently, most of which make liberal use of high speed dedicated computing power. None, however, can as yet claim to offer a universal solution to what, in analysis, turns out to be a multivariable, non-linear, non-autonomous and poorly-modelled control problem. The methods proposed to deal with this problem fall loosely into three main classes. The first "direct" class consists of methods which attempt directly to compute the motor torques and forces from some description of the required trajectory[I-4]. These methods are based upon complex computational algorithms which operate on data describing either the desired tool co-ordinates or the desired task coordinates. In most cases the various derivatives are also required to completely describe either the desired tool co-ordinates or the desired task co-ordinates. In most cases the various derivatives are also required to completely describe the dynamic state of the system. In particular numerical optimisation of the dynamic system model provides resulting control laws which are openloop in nature. The main features of these "direct" methods are con-
cerned with the need to obtain real-time solutions and include tabular look-up schemes, ingenious formulations of the dynamic equations to avoid recursion and the exploitation of parallel processing. Their common deficiency lies in the necessity of providing an accurate model of the manipulator kinematics and dynamics. They are also intrinsically incapable of providing solutions which compensate for random disturbance forces. To overcome these deficiencies some form of feedback compensation is a necessity. The second class of methods are those which might be termed "adaptive" [5]. Here a mathematical model of a simple, usually linear, kind is constructed and is subjected to the same input commands as the manipulator itself. The time evolution of the manipulator co-ordinates may be compared with that of the reference model and the error is used via some suitable optimisation scheme to drive variable control parameters to values which cause the manipulator to emulate the model as closely as possible. This is similar in concept to the self-tuning control strategies which are evoking considerable recent interest in the process industries[5]. Obviously since the manipulator is being forced by its varying control parameters to behave according to the dynamics of a simple model, no highly accurate model of the manipulator itself is required. However, many problems remain in assuring that the resulting overall time varying system is stable over its entire operating envelope. The third class of control methods may, again loosely, be termed "anthropomorphic"; they attempt in some way to use knowledge or even conjecture of how humans achieve co-ordinated control of their limbs in designing robot controls[7-9]. Since, despite many years of research activity, little is yet known of the extremely complex movement control systems which humans have evolved,,only limited success has been achieved via this route. However it is reasonable to speculate that effort in this 267
A. HANAFIet al.
268
direction could lead to the most useful results since the human movement control system is clearly very effective and robust in the face both of model inaccuracies and external force disturbances. It also seems intuitively clear that this effective and robust control does not require extremely fast calculation of complex algorithms since it can be achieved at a subconscious level. In what follows, an approach is described based upon the application of optimal control theory. Here, at the outset a measure of performance is set up, the minimisation of which represents ideal co-ordinated movement. A set of open-loop controls is obtained which could be used directly to command the manipulator using additional compensatory feedback to cope with small deviations. Instead, however, we adopt the strategy of invoking the well known "projection theorem" of vector analysis to obtain directly the set of feedback parameters which most closely generates the optimal control[10]. The first part of this method may be considered to fall into the direct class of methods in that a set of control input trajectories is computed. However, the projection approximation has the obvious advantage that a flexible feedback, rather than an open-loop solution is obtained so that the need for additional compensation is avoided. Also, since the feedback parameters are chosen to satisfy the minimisation of an operational cost function, no other set of fixed feedback parameters for a selected structure would be able to provide a better control for the desired trajectory. In addition, as will be indicated, it is straighforward to combine certain features of the third, anthopomorphic class of control methods with this optimal scheme to provide a composite method processing the benefits of both. 2. THEORY OF OPTIMAL CONTROL
2.1 The optimum control problem Consider the system governed by the set of first order differential equations: 2(0 = f(x(t), u(t), p);
x(to) = xo
(2.1)
where: x is an n dimensional state vector; u is an m dimensional input control vector; p is an I dimensional vector of adjustable parameters; f is an n dimensional nonlinear vector function, continuously differentiable in x, u and p; and t ~ [to, tr], a closed time interval. The control problem is to determine the values of the input control u(t) and the adjustable parameters p that minimize a performance index of the form J(u,p)
=
tO(p) +
ft ot! L(x(t),
u(t))
dt
in eqn (2.2) is normally quadratic in x and u in which case deviations from zero are penalised over the complete trajectory. This constrains the form of eqns (2.1) to be error-co-ordinates of the form x~ = X, - Xo, where Xo~ is the operating point value and X~ the actual variation. Initial conditions on eqn (2.1) are derived by considering an alternative operating point and calculating the initial perturbation from the final operating point. This formulation allows the final time tr to be fixed at some large value compared to the time response of the uncontrolled system which further ensures that all state and control trajectories may be allowed to regulate to the origin of the state space defined in error co-ordinates. The optimal control problem, here, is considered in the space V consisting of ordered pairs (p,u(t)), where p E E ~ and u(t)~L2~[to, tt], i.e. V = E ~x L2m[to, tr]. For any two vectors v,, v2 E V define the inner product (vl'v2)=
ff
and the norm
IM: sq.((v, v)) Hence V is a Hilbert space. Solution of the nonlinear optimisation problem is most easily accomplished by a direct search numerical algorithm to provide the optimum t'* explicitly. Application of the conjugate gradient algorithm to this problem requires an expression for the gradient vector. There are several approaches[17,16,14,12] available to derive the necessary gradient vector. A popular approach[16,12] is to convert the Bolza control problem into a Mayer problem by adding a new state variable to account for integral terms in the objective function. Define:
.%+,=L(x(t),u(t));
x.+,(to) = O.
(2.3)
Equation (2.2) can now be expressed in the form:
J(u,p) = ~h(x(tl),p).
(2.4)
The gradient of the objective function with respect to the optimisation variables is then given by g(v) = (gp, g,,) where
(2.2)
when transferring the nonlinear system of eqn (2.1) from the initial condition xo to the origin of the state space. Constraints on the size of allowable parameter variation are included in the scalar term tO. The integral term
l ,I u/ru2dt+ptrP2
gp= fjJft, rz(t)dt + V~,~) and
g, = f, rz(t )
(2.5)
Optimal trajectory control of robotic manipulators and z(t) is the solution to the adjoint system 2(t)=-f.~z(t);
z(tl)=Vx&(x(tr),p).
(2.6)
The conjugate gradient method generates a sequence vj that converges to the minimising value v* by searching for conditions of zero gradient of J(v) along mutually conjugate directions. From an initial guess Vo and an initial search direction so = - go the algorithm is
269
2.3 Projection approximation of input by output feedback We will denote the optimum input and output variables as u* and ~* on the assumption that the output distribution matrix may be adjusted so that ~)* also contains measurable state variables. The required solution is obtained by reconstructing the m dimensional vector u* from a linear combination of the elements of the r dimensional vector ~*. This approach provides a constant feedback control law of the form
Set j = 0 u*(t)= t~*(t). 1 ..............
Determine a j E [ O , + ~ ] (d//da)(vj + asj) = 0
such
(2.7)
that
Set vi+~ = vi + ajsi Compute/3j - (gJ+" gj+') (gJ, gi) Set s~+, = -gj+, +/3jsj Set j = j + 1
To find the best value of /£ we use the projection theorem[10-12]. In the present problem we may approximate the control u*(t) by linear combinations of the elements of ~*(t), by choosing the number of basis functions to be the order of the output vector k = r. It then follows that ~)*(t) is approximately given by, ~*(t) -~ P(~*(t): &,, &2 . . . . . &r) = M&
(2.8)
where M is an r x r matrix with elements m~j given by
Go to 1.
m,j=(y~*,&)
The computation involved in the algorithm is straightforward. The only difficulties appear in the computation of u and the gradient g. In computing a, we use Mukai's linear search method[12,15], which is based on a combined use of step size estimate and an Armijo [12,15] type step size determination scheme. In computing the gradient, we require the solution of the adjoint system (2.6). Note that the latter must be solved in backward time. 2.2 Generation of suboptimum feedback controllers The optimum open loop control u* derived in Section 2.1 may usefully be approximated by a fixed structure controller composed of a linear combination of output measurements
i,j= 1,2 ..... r.
If ,b is formed from p*(t) then & = L~*. Similarly the projection of u*(t) onto the same space provides u*(t)=P(u*(t):&,,&2 ..... &r)= N&
(2.9)
where N is a m x r matrix with elements n~i given by, n~=(u,&j)
i=l,=...m:
j=l,2...r;
(2.10) P(u*(t): &.,&2 ..... &r) = N M - ' ( P ( y * ( t ) : &,&2 ..... &r)
or the mean squared error approximation of u* with feedback from y* is: u*(t)~- N M
'y*(t).
(2.11)
u = K~ where ~ is an r dimensional vector related to the state via an output distribution matrix C by
Comparison of eqns (2.11) and (2.7) provides the desired output feedback coefficient matrix,/~, where (2.12)
I~ = N M -I.
This is the normal practical situation in which case u = KCx replaces a search for u* by a search for further variable parameters K~i, i = l .... m:
/'=1 .... r.
If the problem is retained as a free structure with no unknown parameters preparation time is reduced and some flexibility in comparison of different structures to provide the optimum u* can be achieved using projection approximations to close the loop.
Note that when & is formed from linearly independent elements of ~*M -~= L and no inversion is required to compute the matrix gain/~. 3. OPTIMUM CONTROL OF A ROBOT ARM
Consider the robotic arm shown in Fig. 1. This is the manipulator considered by Freund[13]. Applying the Euler-Lagrange approach to the system of Fig. 1, yields: I
(m + m L ) ~ - ( m + mL)rO ~ + m ~ 0 2 f(t) (k - mrl + (m + mL)r2)i/ + {[2(m + m,~)r - ml]P}O = M(t)
A. HANAFIet al.
270
large signal model. -1(I = X2
22 = 0.696x42 +
x,x42 + 0.0286u,
xa = x4
(33)
24 = - 1.393x2x4 - 2x~x2x4+ 0.0286/12 x, 2 + 1.393x~ + 0.6455
/
with initial conditions xT(to) = ( - 0.35.0, - 0.75, 0). The state variables x(t) and control variables are now error co-ordinates from the operating point 3(o2. The first objective is to determine the open loop control vector u(t) that steers the system to the origin from the point if(to) = (-0.35, 0, -0.75, 0). This problem can be reformulated as an optimal control problem in which the aim is to choose u(t) such as to minimize the cost' functional
v
FORCE f(t)
Fig. 1. Robotic arm.
where r(t) and O(t) are the-generalised position coordinates', m*: mass of the upright column, and r* its radius; mL: load mass; m: arm mass assumed at midpoint; and k = (1/2)m*r*2+ m(12[4). Equations (3.1) are a set of second order nonlinear differential equations which describe the dynamics of the robot manipulator. These equations can be converted into a set of first order differential equations by defining new state variables as follows:
J = ~f,'f(xrQx + uTRu)dt
(3.4)
subject to the dynamics equations (3.3). Define
Q=
Xl=? ,
200 0 0 0
0 0 0 0
0 0 450 0
0 0 0 0
X2 = andU=(U1)={f(t)]
This problem can be reformulated as a Mayer problem: Determine u(t). tE[O, tt], which minimizes J = x,(t t) subject to:
\M(t)/
X3 = 0
x4=0
-~1 = X 2
This definition yields: 2
2-. = 0.696X4- + XlX4 + 0.0286u,
/•1
= X2
.Y(2 = X~X42 - 0.429 X42+ 0.0286 U1 X'~ = x~ YG = - 2 X , X 2 X 4 + 0.857X~X4+ 0.0286 U2 X, 2 -0.857X, +0.344
(3.2)
.i, = x,
(3.5)
- 1.393xzx4- 2XlX2X4 + 0.0286u2 z+ x~ 1.393x, + 0.6455
where we have arbitrarily chosen m = 20 kg, I = 1.5 m, m* = 40kg, r* = 0.2 m, mL = 15kg, and k = 15.8. The control problem, here, is to choose the control vector u r ( t ) = (Udt), U2(t)) which would drive the arm whose dynamics are described by eqn (3.2) from Xor, = (0.775, 0, 0.75, 0) to the final operating point X ~ = (1.125, O, 1.5, 0). Expanding eqn (3.2) about the operating point Xo~ = (1.125, 1.5, 0) without neglecting higher order terms, and subtracting the steady values provides the following
~, = 100(2x~ + 4.5x;) + ~(u," + u2z) xr(0) = (-0.35, 0.0, -0.75, 0.0, 0.0). The adjoint system is described by 5(t) = - f , rz(t) where
2 Ix =
fx, [ 400x,
0
0 0 0
0 - 1.393x4- 2x,x4 den 0 0 900x~
0 2x4(0.696+ x,) 0 1 0 - 1.393x, + 2x,x2 0 den
0
0l
0
(3.6)
Optimal trajectory control of robotic manipulators
271
and zr(tl) = (0,0,0, l) where
fx~ = ( - 2x2x4)den - (2xt + 1.393){- 1.393xzx,, - 2xtxzx4+ 0.0286u2} (den):
den = x, 2 + 1.393x, + 0.6455.
Oll ~
2
3
45
/
o/
I 1
I [,-~.----~ ,' ~
o I'/
X.~O.1
The gradient vector is
(O.0286z2(t) + u,(t)z~(t) g(u(t)) = \O.0286z4(t) + u2(t)z~(t)/"
(3.7)
The optimal control problem was solved using the conjugate gradient method. The control u*(t) is approximated by linear state feedback to provide a suboptimal feedback gain of
',
I
I
t
6
7
8
9
1
~
I
I
I
10 11 12 13 14 time ('s)
-0"3 -0"4
o'2 F . .
x2
0
"
1
O It
~
11 2'
.
3' ~' 4 5 6 7 1 0 o '
911;
,[1112 ,3114 time ( t )
K = F - 14.726 -43.635 1.38 L 0.99
3.295 3.976 ] - 37.023 - 30.108J' (3.8)
1
0"5 V
2
-O'5 [".d'#
3
5
6
7
8
9
IO 11 12 13 14 time ($)
I O t-
Note that this structure is equivalent to the structure derived from optimal regulator solution of the linearised system where the optimisation interval is infinite. Figures 2 and 3 show the optimal controls and optimum trajectories respectively. Note that the suboptimum trajectories coincide with the optimum trajectories when full state feedback is considered.
,°,F&
o,i 7\. o,
t
,, i'V
'
i
'
I
,,
,12
time (s)
Fig. 3. Optimum and suboptimum trajectories. 4. AN EXAMPLE OF PARAMETER OPT1MISATION
The purpose of this example is to show the efficiency of the gradient algorithm[12] when parameter optimisation is considered. We shall consider the robotic problem
with a fixed control structure of the form (4.1) Combining eqns (4.1) and (3.5) provides
.... ui
.i~ = X2
"~(" '. x.l". I l _1 I~_1 \X . 1 "4"r' 5
22 = 0.696x42 + xlx42 + 0.0286(k,xl + k2x2 + k~x3 + k6x4) [
I_
IO
2~ = x4
x4 =
(4.2)
- 1.393x2x4- 2XIX2X4+ 0.0286(k3X3 + k4x4)
x, ~+ 1.393x~ + 0.6455
.it = lO0(2xl 2 + 4.5x3 2) + 0.5((k~x~ + k2x2 + ksx3 + k6x4)2 ~- (k3x 3 ~'- k4X4) 2)
x~(0) = (-0.35, 0.0, -0.75, 0.0, 0.0).
02
The control problem is converted to a simple parameter optimisation problem, i.e. "Determine the vector of control parameters
I. t" t" l:
t:
":~:
k T = ( k h k2,k3,k4,k.~,/(6)
,.,-.~.."
5
I0
time(s)
-...
Fig. 2. Optimal controls, small signal . . . . . ; large signal . . . . .
which minimizes the cost functional I = x~(b) subject to the dynamic equations (4.2)", The adjoint system is
~(t) = -f~rz(t), z'r(tt) = (0,0,0,0, 1)
(4.3)
A. HANAFI et al.
272 where 0 1 x42 +0.0286k~ 0.0286k2 0
0 2x4(0.696 + x,) + 0.0286k6
0 0.0286k~
0
0 1.393x4- 2x~x4 0.0286k3 den den kjt~ 900x, + ksu~ + k3u~
f4x, 400xl +k,ul
0 0
1
- 1.393x2- 2x~x2 + 0.286k4 den k6u I + k4u2
where ( - 2x2xa)den- (2x~ + 1.393)(- 1.393x2x4- 2x,xzx4 + 0.0286u2) f4xl = --
(den):
Ul = k l x l + k2x2 + ksx3 + k6x4 1,12 = k3x3 + k4x4.
The formulation attempts to control the angular movement actuator by feedback of only angular measurements. The gradient vector is g = fo'IfkTZ(t) dt
Table t. Tabulationof cost function and norm square gradient against iteration number Iteration
Normsquare gradient
Criterion
0 I 2
22.43 2.83 0.30
271.611 250.116 248.77
(4.4)
where I fk =
0 0.286x~ 0 0
0 0 0.0286x2 0 0 0 0 0.0286x, den
0 0 0 0.0286x4 den
0 0.0286x3 0 0
0 0.0286x4 0 0
X1 Ill
X21t I
X4112
X ~H I
X4ll I
X31t2
The initial guess was simply chosen to be the vector of feedback gains obtained from the design based on the small signal model[12], i.e. k r = ( - 2 0 , - 3 7 . 3 9 , -30, - 3 6 . 7 9 , 0, 0). 200 Runge Kutta integration steps were used over a time interval of [0,10s], and the conjugate gradient procedure was terminated when Ilgl[<0.31. After two iterations, the algorithms converged to the following control feedback structure: u(t) = kx(t) where k = [ - 1 8 " 8 7 -37.60 2 . 6 6 - 1 . 0 3 ] [ 0 0 -37.13 -31.17]' The performance index and corresponding norm square gradient as functions of the iteration number are shown in Table 1. Note that the suboptimum responses shown in Fig. 4 are very close to the response obtained using full state feedback. This indicates that simplifications in feedback structure do not necessarily reduce optimum performance by a significant amount.
5. CONCLUSION
The application of numerical optimisation procedures to the robotic model has been shown to converge to an optimum open loop control law. The control law can adequately be approximated by a feedback structure which gives some immunity from inaccuracies in the assumed model. The proposed feedback controller is only valid for the considered amplitude deviations from the assumed operating point. Larger amplitudes on initial conditions are likely to increase the performance index and produce some variation of the fixed structure feedback gains. Confirmation of low sensitivity of feedback gains with feasible initial conditions over the full operating range of a manipulator is required before any practical implementation. Since this phenomenon is solely due to nonlinearity an alternative approach may be to compute a two stage controller. The first stage may involve a nonlinear decoupling method which effectively eliminates the nonlinearity. For known nonlinearities this may simply be achieved by a
273
Optimal trajectory control of robotic manipulators
REFERENCES
I /I//i~'....5'
xI.
~4
I0
/ / /
/
"21 ,.. x20 v -.I[
J
~3 0
I A-
-I
t
i
,"~A
5 I
I
xlX~l
]
I
lO
I
5
IO
I $
I0
I//
X X OL
j
I
l
J time ( s )
Fig. 4. Optimum trajectories (fixed structure).
nonlinear feedback controller[13]. An alternative approach which does not require an explicit knowledge of the nonlinearities has recently been proposed[8]. With the system nonlinearities eliminated the optimisation method presented in Section 2.2 may then be used to provide the second stage of a feedback control solution.
I. J. Y. S. Luh, M. W. Walker and R. P. C. Paul, ASME Trans., J. Dynamic Systems Measurement and Control 102 (1980). 2. J. M. Hollerbach, IEEE Trans. SMC-10 (1980). 3. A. Liegeois, Revue RAIRO J.l (Feb. 1975). 4. J. R. Hewit and J. Padovan, Decoupled feedback control of robot and manipulator arms. 3rd CISM--IFFoMM Int. Syrup. Theory and Practice o[ Robots and Manipulators, Udine, Italy, Sept. 1978. 5. S. Dubowsky and D. T. Desforges, Trans. ASME, J. Dynamic Systems Measurement and Control 10t (1979). 6. P. E. Wellstead, Intelligent digital control systems. S.E.R.C. Vacation School on Robot Technology, University of Hull, Department of Electronic Engineering (1981). 7. J. S. Albus, Trans. ASME, J. Dynamic Systems Measurement and Control (1975). 8. J. R. Hewit and N. Tan, Dynamic co-ordination of robot movement. 4th CIBM-IFToMM Syrup. Theory and Practice of Robots and Manipulators, Warsaw, Sept. 1981. 9. C. S. Chandler, J. R. Hewit and J. S. G. Miller, Dynamic control of robotic and human limbs. IEE Collog. NeuroMuscular Control Systems, St. Thomas' Hospital, London, Sept. 1981. 10. F. W. Wright, Optimum design algorithms for the control of a synchronous generator. Ph.D. thesis, Sunderland Polytechnic (1977). ll. F. W. Wright and B. C. Laws, Proc. IEE 123(7), 713 (1976). 12. A. Hanafi, Optimal control of multivariable nonlinear systems: a numerical approach. M.Phil. thesis, Sunderland Polytechnic, Electrical Department (1981). 13. E. Freund and M. Syrbe, Control of industrial robots by means of microprocessors. Colloq. IRIA, Versailles, Rocquencourt, 13-17 December 1976, p. 178. 14. A. E. Bryson and Ho Yu-Chi Jr., Applied Optimal Control. (1975). 15. H. Mukai, A scheme for determining step sizes for unconstrained optimization methods. IEEE Trans. on Automatic Control AC-23(6) (1978). 16. Y. Barness and I. G. Ben-Tovim, Int. J. Systems Sci. 7(4), 361-368 (1976). 17. E. Polak, SIAM Rev. 15(2) (1973).
OPTIIJJu!] Bfdl]JSTELE~ULG VOI'] ROBOTER-ItiNIIULATOREN A. llanafi, J. R. liew±t und F. W. Wright Eurzfassung - D~e Theorie der optimalen ReElung wird angewendet, ur~ die ~ e ~ s e h t e B ~ n des Arbeitskopfes eines Industrieroboters zu bestimmen. Das Problem wird als eine ilininierun~ im I;ilbert-Raum bctrachtet und mit der ~:ethode des konjun~ierten Gef!illes ~el~st. Die so abgeleitete Regelfunktion fHr die offene Steuerkette wlrd nicht direht verwendet, sondern ergibt die Basis eines festen Re~elkreises dutch Gef~lleprojek-tion.
0094-114X/84 $3.00 + .00 © 1984 Pergamon Press Ltd.
OPTIMAL TRAJECTORY CONTROL OF ROBOTIC MANIPULATORS A. H A N A F I and F. W. W R I G H T Electrical, Electronic and Control Engineering Department, Sunderland Polytechnic. Sunderland, England and J. R. H E W l T Mechanical Engineering Department. University of Newcastle upon Tyne, Newcastle, England
(Receivedfor publication 26 January 1983) Abstract--The problems associated with the achievement of desired trajectories for non-linear robotic manipulatory systems are discussed. Optimisation of both free and fixed structured systems is considered and projection approximations to the open-loop controls for the free structured system are used to provide a practical feedback solution to the problem.
1. INTRODUCTION--ROBOT CONTROL METHODS
A robotic manipulator conventionally consists of a number of rigid links connected as an open chain by a number of powered joints. The power inputs to the joint motors are converted into movement of the links and must be controlled so as to cause the "end effector" or "tool" to take up the required position and orientation within the working space. As the required position and orientation of the tool change with time so the tool co-ordinates trace out a trajectory which must be close enough to the ideal to permit satisfactory progress of the task being undertaken by the tool. This control system design poses a number of severe problems, particularly where high operational accuracy and high speed are simultaneously demanded. A number of approaches have been proposed recently, most of which make liberal use of high speed dedicated computing power. None, however, can as yet claim to offer a universal solution to what, in analysis, turns out to be a multivariable, non-linear, non-autonomous and poorly-modelled control problem. The methods proposed to deal with this problem fall loosely into three main classes. The first "direct" class consists of methods which attempt directly to compute the motor torques and forces from some description of the required trajectory[I-4]. These methods are based upon complex computational algorithms which operate on data describing either the desired tool co-ordinates or the desired task coordinates. In most cases the various derivatives are also required to completely describe either the desired tool co-ordinates or the desired task co-ordinates. In most cases the various derivatives are also required to completely describe the dynamic state of the system. In particular numerical optimisation of the dynamic system model provides resulting control laws which are openloop in nature. The main features of these "direct" methods are con-
cerned with the need to obtain real-time solutions and include tabular look-up schemes, ingenious formulations of the dynamic equations to avoid recursion and the exploitation of parallel processing. Their common deficiency lies in the necessity of providing an accurate model of the manipulator kinematics and dynamics. They are also intrinsically incapable of providing solutions which compensate for random disturbance forces. To overcome these deficiencies some form of feedback compensation is a necessity. The second class of methods are those which might be termed "adaptive" [5]. Here a mathematical model of a simple, usually linear, kind is constructed and is subjected to the same input commands as the manipulator itself. The time evolution of the manipulator co-ordinates may be compared with that of the reference model and the error is used via some suitable optimisation scheme to drive variable control parameters to values which cause the manipulator to emulate the model as closely as possible. This is similar in concept to the self-tuning control strategies which are evoking considerable recent interest in the process industries[5]. Obviously since the manipulator is being forced by its varying control parameters to behave according to the dynamics of a simple model, no highly accurate model of the manipulator itself is required. However, many problems remain in assuring that the resulting overall time varying system is stable over its entire operating envelope. The third class of control methods may, again loosely, be termed "anthropomorphic"; they attempt in some way to use knowledge or even conjecture of how humans achieve co-ordinated control of their limbs in designing robot controls[7-9]. Since, despite many years of research activity, little is yet known of the extremely complex movement control systems which humans have evolved,,only limited success has been achieved via this route. However it is reasonable to speculate that effort in this 267
A. HANAFIet al.
268
direction could lead to the most useful results since the human movement control system is clearly very effective and robust in the face both of model inaccuracies and external force disturbances. It also seems intuitively clear that this effective and robust control does not require extremely fast calculation of complex algorithms since it can be achieved at a subconscious level. In what follows, an approach is described based upon the application of optimal control theory. Here, at the outset a measure of performance is set up, the minimisation of which represents ideal co-ordinated movement. A set of open-loop controls is obtained which could be used directly to command the manipulator using additional compensatory feedback to cope with small deviations. Instead, however, we adopt the strategy of invoking the well known "projection theorem" of vector analysis to obtain directly the set of feedback parameters which most closely generates the optimal control[10]. The first part of this method may be considered to fall into the direct class of methods in that a set of control input trajectories is computed. However, the projection approximation has the obvious advantage that a flexible feedback, rather than an open-loop solution is obtained so that the need for additional compensation is avoided. Also, since the feedback parameters are chosen to satisfy the minimisation of an operational cost function, no other set of fixed feedback parameters for a selected structure would be able to provide a better control for the desired trajectory. In addition, as will be indicated, it is straighforward to combine certain features of the third, anthopomorphic class of control methods with this optimal scheme to provide a composite method processing the benefits of both. 2. THEORY OF OPTIMAL CONTROL
2.1 The optimum control problem Consider the system governed by the set of first order differential equations: 2(0 = f(x(t), u(t), p);
x(to) = xo
(2.1)
where: x is an n dimensional state vector; u is an m dimensional input control vector; p is an I dimensional vector of adjustable parameters; f is an n dimensional nonlinear vector function, continuously differentiable in x, u and p; and t ~ [to, tr], a closed time interval. The control problem is to determine the values of the input control u(t) and the adjustable parameters p that minimize a performance index of the form J(u,p)
=
tO(p) +
ft ot! L(x(t),
u(t))
dt
in eqn (2.2) is normally quadratic in x and u in which case deviations from zero are penalised over the complete trajectory. This constrains the form of eqns (2.1) to be error-co-ordinates of the form x~ = X, - Xo, where Xo~ is the operating point value and X~ the actual variation. Initial conditions on eqn (2.1) are derived by considering an alternative operating point and calculating the initial perturbation from the final operating point. This formulation allows the final time tr to be fixed at some large value compared to the time response of the uncontrolled system which further ensures that all state and control trajectories may be allowed to regulate to the origin of the state space defined in error co-ordinates. The optimal control problem, here, is considered in the space V consisting of ordered pairs (p,u(t)), where p E E ~ and u(t)~L2~[to, tt], i.e. V = E ~x L2m[to, tr]. For any two vectors v,, v2 E V define the inner product (vl'v2)=
ff
and the norm
IM: sq.((v, v)) Hence V is a Hilbert space. Solution of the nonlinear optimisation problem is most easily accomplished by a direct search numerical algorithm to provide the optimum t'* explicitly. Application of the conjugate gradient algorithm to this problem requires an expression for the gradient vector. There are several approaches[17,16,14,12] available to derive the necessary gradient vector. A popular approach[16,12] is to convert the Bolza control problem into a Mayer problem by adding a new state variable to account for integral terms in the objective function. Define:
.%+,=L(x(t),u(t));
x.+,(to) = O.
(2.3)
Equation (2.2) can now be expressed in the form:
J(u,p) = ~h(x(tl),p).
(2.4)
The gradient of the objective function with respect to the optimisation variables is then given by g(v) = (gp, g,,) where
(2.2)
when transferring the nonlinear system of eqn (2.1) from the initial condition xo to the origin of the state space. Constraints on the size of allowable parameter variation are included in the scalar term tO. The integral term
l ,I u/ru2dt+ptrP2
gp= fjJft, rz(t)dt + V~,~) and
g, = f, rz(t )
(2.5)
Optimal trajectory control of robotic manipulators and z(t) is the solution to the adjoint system 2(t)=-f.~z(t);
z(tl)=Vx&(x(tr),p).
(2.6)
The conjugate gradient method generates a sequence vj that converges to the minimising value v* by searching for conditions of zero gradient of J(v) along mutually conjugate directions. From an initial guess Vo and an initial search direction so = - go the algorithm is
269
2.3 Projection approximation of input by output feedback We will denote the optimum input and output variables as u* and ~* on the assumption that the output distribution matrix may be adjusted so that ~)* also contains measurable state variables. The required solution is obtained by reconstructing the m dimensional vector u* from a linear combination of the elements of the r dimensional vector ~*. This approach provides a constant feedback control law of the form
Set j = 0 u*(t)= t~*(t). 1 ..............
Determine a j E [ O , + ~ ] (d//da)(vj + asj) = 0
such
(2.7)
that
Set vi+~ = vi + ajsi Compute/3j - (gJ+" gj+') (gJ, gi) Set s~+, = -gj+, +/3jsj Set j = j + 1
To find the best value of /£ we use the projection theorem[10-12]. In the present problem we may approximate the control u*(t) by linear combinations of the elements of ~*(t), by choosing the number of basis functions to be the order of the output vector k = r. It then follows that ~)*(t) is approximately given by, ~*(t) -~ P(~*(t): &,, &2 . . . . . &r) = M&
(2.8)
where M is an r x r matrix with elements m~j given by
Go to 1.
m,j=(y~*,&)
The computation involved in the algorithm is straightforward. The only difficulties appear in the computation of u and the gradient g. In computing a, we use Mukai's linear search method[12,15], which is based on a combined use of step size estimate and an Armijo [12,15] type step size determination scheme. In computing the gradient, we require the solution of the adjoint system (2.6). Note that the latter must be solved in backward time. 2.2 Generation of suboptimum feedback controllers The optimum open loop control u* derived in Section 2.1 may usefully be approximated by a fixed structure controller composed of a linear combination of output measurements
i,j= 1,2 ..... r.
If ,b is formed from p*(t) then & = L~*. Similarly the projection of u*(t) onto the same space provides u*(t)=P(u*(t):&,,&2 ..... &r)= N&
(2.9)
where N is a m x r matrix with elements n~i given by, n~=(u,&j)
i=l,=...m:
j=l,2...r;
(2.10) P(u*(t): &.,&2 ..... &r) = N M - ' ( P ( y * ( t ) : &,&2 ..... &r)
or the mean squared error approximation of u* with feedback from y* is: u*(t)~- N M
'y*(t).
(2.11)
u = K~ where ~ is an r dimensional vector related to the state via an output distribution matrix C by
Comparison of eqns (2.11) and (2.7) provides the desired output feedback coefficient matrix,/~, where (2.12)
I~ = N M -I.
This is the normal practical situation in which case u = KCx replaces a search for u* by a search for further variable parameters K~i, i = l .... m:
/'=1 .... r.
If the problem is retained as a free structure with no unknown parameters preparation time is reduced and some flexibility in comparison of different structures to provide the optimum u* can be achieved using projection approximations to close the loop.
Note that when & is formed from linearly independent elements of ~*M -~= L and no inversion is required to compute the matrix gain/~. 3. OPTIMUM CONTROL OF A ROBOT ARM
Consider the robotic arm shown in Fig. 1. This is the manipulator considered by Freund[13]. Applying the Euler-Lagrange approach to the system of Fig. 1, yields: I
(m + m L ) ~ - ( m + mL)rO ~ + m ~ 0 2 f(t) (k - mrl + (m + mL)r2)i/ + {[2(m + m,~)r - ml]P}O = M(t)
A. HANAFIet al.
270
large signal model. -1(I = X2
22 = 0.696x42 +
x,x42 + 0.0286u,
xa = x4
(33)
24 = - 1.393x2x4 - 2x~x2x4+ 0.0286/12 x, 2 + 1.393x~ + 0.6455
/
with initial conditions xT(to) = ( - 0.35.0, - 0.75, 0). The state variables x(t) and control variables are now error co-ordinates from the operating point 3(o2. The first objective is to determine the open loop control vector u(t) that steers the system to the origin from the point if(to) = (-0.35, 0, -0.75, 0). This problem can be reformulated as an optimal control problem in which the aim is to choose u(t) such as to minimize the cost' functional
v
FORCE f(t)
Fig. 1. Robotic arm.
where r(t) and O(t) are the-generalised position coordinates', m*: mass of the upright column, and r* its radius; mL: load mass; m: arm mass assumed at midpoint; and k = (1/2)m*r*2+ m(12[4). Equations (3.1) are a set of second order nonlinear differential equations which describe the dynamics of the robot manipulator. These equations can be converted into a set of first order differential equations by defining new state variables as follows:
J = ~f,'f(xrQx + uTRu)dt
(3.4)
subject to the dynamics equations (3.3). Define
Q=
Xl=? ,
200 0 0 0
0 0 0 0
0 0 450 0
0 0 0 0
X2 = andU=(U1)={f(t)]
This problem can be reformulated as a Mayer problem: Determine u(t). tE[O, tt], which minimizes J = x,(t t) subject to:
\M(t)/
X3 = 0
x4=0
-~1 = X 2
This definition yields: 2
2-. = 0.696X4- + XlX4 + 0.0286u,
/•1
= X2
.Y(2 = X~X42 - 0.429 X42+ 0.0286 U1 X'~ = x~ YG = - 2 X , X 2 X 4 + 0.857X~X4+ 0.0286 U2 X, 2 -0.857X, +0.344
(3.2)
.i, = x,
(3.5)
- 1.393xzx4- 2XlX2X4 + 0.0286u2 z+ x~ 1.393x, + 0.6455
where we have arbitrarily chosen m = 20 kg, I = 1.5 m, m* = 40kg, r* = 0.2 m, mL = 15kg, and k = 15.8. The control problem, here, is to choose the control vector u r ( t ) = (Udt), U2(t)) which would drive the arm whose dynamics are described by eqn (3.2) from Xor, = (0.775, 0, 0.75, 0) to the final operating point X ~ = (1.125, O, 1.5, 0). Expanding eqn (3.2) about the operating point Xo~ = (1.125, 1.5, 0) without neglecting higher order terms, and subtracting the steady values provides the following
~, = 100(2x~ + 4.5x;) + ~(u," + u2z) xr(0) = (-0.35, 0.0, -0.75, 0.0, 0.0). The adjoint system is described by 5(t) = - f , rz(t) where
2 Ix =
fx, [ 400x,
0
0 0 0
0 - 1.393x4- 2x,x4 den 0 0 900x~
0 2x4(0.696+ x,) 0 1 0 - 1.393x, + 2x,x2 0 den
0
0l
0
(3.6)
Optimal trajectory control of robotic manipulators
271
and zr(tl) = (0,0,0, l) where
fx~ = ( - 2x2x4)den - (2xt + 1.393){- 1.393xzx,, - 2xtxzx4+ 0.0286u2} (den):
den = x, 2 + 1.393x, + 0.6455.
Oll ~
2
3
45
/
o/
I 1
I [,-~.----~ ,' ~
o I'/
X.~O.1
The gradient vector is
(O.0286z2(t) + u,(t)z~(t) g(u(t)) = \O.0286z4(t) + u2(t)z~(t)/"
(3.7)
The optimal control problem was solved using the conjugate gradient method. The control u*(t) is approximated by linear state feedback to provide a suboptimal feedback gain of
',
I
I
t
6
7
8
9
1
~
I
I
I
10 11 12 13 14 time ('s)
-0"3 -0"4
o'2 F . .
x2
0
"
1
O It
~
11 2'
.
3' ~' 4 5 6 7 1 0 o '
911;
,[1112 ,3114 time ( t )
K = F - 14.726 -43.635 1.38 L 0.99
3.295 3.976 ] - 37.023 - 30.108J' (3.8)
1
0"5 V
2
-O'5 [".d'#
3
5
6
7
8
9
IO 11 12 13 14 time ($)
I O t-
Note that this structure is equivalent to the structure derived from optimal regulator solution of the linearised system where the optimisation interval is infinite. Figures 2 and 3 show the optimal controls and optimum trajectories respectively. Note that the suboptimum trajectories coincide with the optimum trajectories when full state feedback is considered.
,°,F&
o,i 7\. o,
t
,, i'V
'
i
'
I
,,
,12
time (s)
Fig. 3. Optimum and suboptimum trajectories. 4. AN EXAMPLE OF PARAMETER OPT1MISATION
The purpose of this example is to show the efficiency of the gradient algorithm[12] when parameter optimisation is considered. We shall consider the robotic problem
with a fixed control structure of the form (4.1) Combining eqns (4.1) and (3.5) provides
.... ui
.i~ = X2
"~(" '. x.l". I l _1 I~_1 \X . 1 "4"r' 5
22 = 0.696x42 + xlx42 + 0.0286(k,xl + k2x2 + k~x3 + k6x4) [
I_
IO
2~ = x4
x4 =
(4.2)
- 1.393x2x4- 2XIX2X4+ 0.0286(k3X3 + k4x4)
x, ~+ 1.393x~ + 0.6455
.it = lO0(2xl 2 + 4.5x3 2) + 0.5((k~x~ + k2x2 + ksx3 + k6x4)2 ~- (k3x 3 ~'- k4X4) 2)
x~(0) = (-0.35, 0.0, -0.75, 0.0, 0.0).
02
The control problem is converted to a simple parameter optimisation problem, i.e. "Determine the vector of control parameters
I. t" t" l:
t:
":~:
k T = ( k h k2,k3,k4,k.~,/(6)
,.,-.~.."
5
I0
time(s)
-...
Fig. 2. Optimal controls, small signal . . . . . ; large signal . . . . .
which minimizes the cost functional I = x~(b) subject to the dynamic equations (4.2)", The adjoint system is
~(t) = -f~rz(t), z'r(tt) = (0,0,0,0, 1)
(4.3)
A. HANAFI et al.
272 where 0 1 x42 +0.0286k~ 0.0286k2 0
0 2x4(0.696 + x,) + 0.0286k6
0 0.0286k~
0
0 1.393x4- 2x~x4 0.0286k3 den den kjt~ 900x, + ksu~ + k3u~
f4x, 400xl +k,ul
0 0
1
- 1.393x2- 2x~x2 + 0.286k4 den k6u I + k4u2
where ( - 2x2xa)den- (2x~ + 1.393)(- 1.393x2x4- 2x,xzx4 + 0.0286u2) f4xl = --
(den):
Ul = k l x l + k2x2 + ksx3 + k6x4 1,12 = k3x3 + k4x4.
The formulation attempts to control the angular movement actuator by feedback of only angular measurements. The gradient vector is g = fo'IfkTZ(t) dt
Table t. Tabulationof cost function and norm square gradient against iteration number Iteration
Normsquare gradient
Criterion
0 I 2
22.43 2.83 0.30
271.611 250.116 248.77
(4.4)
where I fk =
0 0.286x~ 0 0
0 0 0.0286x2 0 0 0 0 0.0286x, den
0 0 0 0.0286x4 den
0 0.0286x3 0 0
0 0.0286x4 0 0
X1 Ill
X21t I
X4112
X ~H I
X4ll I
X31t2
The initial guess was simply chosen to be the vector of feedback gains obtained from the design based on the small signal model[12], i.e. k r = ( - 2 0 , - 3 7 . 3 9 , -30, - 3 6 . 7 9 , 0, 0). 200 Runge Kutta integration steps were used over a time interval of [0,10s], and the conjugate gradient procedure was terminated when Ilgl[<0.31. After two iterations, the algorithms converged to the following control feedback structure: u(t) = kx(t) where k = [ - 1 8 " 8 7 -37.60 2 . 6 6 - 1 . 0 3 ] [ 0 0 -37.13 -31.17]' The performance index and corresponding norm square gradient as functions of the iteration number are shown in Table 1. Note that the suboptimum responses shown in Fig. 4 are very close to the response obtained using full state feedback. This indicates that simplifications in feedback structure do not necessarily reduce optimum performance by a significant amount.
5. CONCLUSION
The application of numerical optimisation procedures to the robotic model has been shown to converge to an optimum open loop control law. The control law can adequately be approximated by a feedback structure which gives some immunity from inaccuracies in the assumed model. The proposed feedback controller is only valid for the considered amplitude deviations from the assumed operating point. Larger amplitudes on initial conditions are likely to increase the performance index and produce some variation of the fixed structure feedback gains. Confirmation of low sensitivity of feedback gains with feasible initial conditions over the full operating range of a manipulator is required before any practical implementation. Since this phenomenon is solely due to nonlinearity an alternative approach may be to compute a two stage controller. The first stage may involve a nonlinear decoupling method which effectively eliminates the nonlinearity. For known nonlinearities this may simply be achieved by a
273
Optimal trajectory control of robotic manipulators
REFERENCES
I /I//i~'....5'
xI.
~4
I0
/ / /
/
"21 ,.. x20 v -.I[
J
~3 0
I A-
-I
t
i
,"~A
5 I
I
xlX~l
]
I
lO
I
5
IO
I $
I0
I//
X X OL
j
I
l
J time ( s )
Fig. 4. Optimum trajectories (fixed structure).
nonlinear feedback controller[13]. An alternative approach which does not require an explicit knowledge of the nonlinearities has recently been proposed[8]. With the system nonlinearities eliminated the optimisation method presented in Section 2.2 may then be used to provide the second stage of a feedback control solution.
I. J. Y. S. Luh, M. W. Walker and R. P. C. Paul, ASME Trans., J. Dynamic Systems Measurement and Control 102 (1980). 2. J. M. Hollerbach, IEEE Trans. SMC-10 (1980). 3. A. Liegeois, Revue RAIRO J.l (Feb. 1975). 4. J. R. Hewit and J. Padovan, Decoupled feedback control of robot and manipulator arms. 3rd CISM--IFFoMM Int. Syrup. Theory and Practice o[ Robots and Manipulators, Udine, Italy, Sept. 1978. 5. S. Dubowsky and D. T. Desforges, Trans. ASME, J. Dynamic Systems Measurement and Control 10t (1979). 6. P. E. Wellstead, Intelligent digital control systems. S.E.R.C. Vacation School on Robot Technology, University of Hull, Department of Electronic Engineering (1981). 7. J. S. Albus, Trans. ASME, J. Dynamic Systems Measurement and Control (1975). 8. J. R. Hewit and N. Tan, Dynamic co-ordination of robot movement. 4th CIBM-IFToMM Syrup. Theory and Practice of Robots and Manipulators, Warsaw, Sept. 1981. 9. C. S. Chandler, J. R. Hewit and J. S. G. Miller, Dynamic control of robotic and human limbs. IEE Collog. NeuroMuscular Control Systems, St. Thomas' Hospital, London, Sept. 1981. 10. F. W. Wright, Optimum design algorithms for the control of a synchronous generator. Ph.D. thesis, Sunderland Polytechnic (1977). ll. F. W. Wright and B. C. Laws, Proc. IEE 123(7), 713 (1976). 12. A. Hanafi, Optimal control of multivariable nonlinear systems: a numerical approach. M.Phil. thesis, Sunderland Polytechnic, Electrical Department (1981). 13. E. Freund and M. Syrbe, Control of industrial robots by means of microprocessors. Colloq. IRIA, Versailles, Rocquencourt, 13-17 December 1976, p. 178. 14. A. E. Bryson and Ho Yu-Chi Jr., Applied Optimal Control. (1975). 15. H. Mukai, A scheme for determining step sizes for unconstrained optimization methods. IEEE Trans. on Automatic Control AC-23(6) (1978). 16. Y. Barness and I. G. Ben-Tovim, Int. J. Systems Sci. 7(4), 361-368 (1976). 17. E. Polak, SIAM Rev. 15(2) (1973).
OPTIIJJu!] Bfdl]JSTELE~ULG VOI'] ROBOTER-ItiNIIULATOREN A. llanafi, J. R. liew±t und F. W. Wright Eurzfassung - D~e Theorie der optimalen ReElung wird angewendet, ur~ die ~ e ~ s e h t e B ~ n des Arbeitskopfes eines Industrieroboters zu bestimmen. Das Problem wird als eine ilininierun~ im I;ilbert-Raum bctrachtet und mit der ~:ethode des konjun~ierten Gef!illes ~el~st. Die so abgeleitete Regelfunktion fHr die offene Steuerkette wlrd nicht direht verwendet, sondern ergibt die Basis eines festen Re~elkreises dutch Gef~lleprojek-tion.