Proceedingsof the American Control Conference Anchorage, AK May 8-10, 2002

Motion Planning Via Optimal Control Theory

Karlheinz Spindler, Fachhochschule Wiesbaden, Fachbereich MNDU, Kurt-Schumacher-Ring 18, D-65 197 Wiesbaden, Germany, spindlerQr5 .mnd .fh-wiesbaden .de sider a right-invariant dynamical system g ( t ) = U ( t ) g ( t ) evolving on G where U ( t ) is the sum of a controlled In this paper we propose to use optimal control theuiE; l where ory to solve motion planning problems. This idea, already term ELl ui(t)Ej and a drift term ~ ~ . , , + u m + l , , . . ,un are given constants. Given elements go E G propagandised by Letov in the 1960s (see [8]) and later a p plied by Kovalev (see [SI), is not new, but never had much and g1 E G , controls t e u i ( t ) (where 1 5 i <_ m) impact on practical applications, presumably because the are sought which steer the system from g ( t 0 ) = go to delicate numerical treatment of optimal control problems g ( t 1 ) = g1 while minimising a cost functional of the form @ ( g ( t ) , u ( t ) , t ) d t .Z f t e u * ( t ) is an optimol control was often less suited to practical implementation than other available methods. However, in the meantime the and if t e g ( t ) is the resulting state trajectory in G so development of geometric control theory (cf. [2], [4], [5]), that g*(t) = U * ( t ) g * ( t )then there enst a curve t I+p * ( t ) using differential geometric methods to deal with control in g \ (0) and a-number E 5 0 such that problems on arbitrary manifolds (in particular Lie groups a@ and their symmetric spaces), has provided new methods $(t) = -E-(g*(t),u*(t),t) -p(t) oad(U*(t)), as which are particularly useful for motion planning proba@ lems in areas such as robotics, spacecraft attitude control p - ( t ) E i = - & -b(U g*(t),u*(t),t) (1 5 i 5 m). and the control of underwater vehicles (cf. [7], [9]-[12]), as i the kinematics involved in such problems can always be represented as a dynamical system on a Lie group G of admissible motions. To find suitable control laws for such 2. Attitude Slews Avoiding a Given Direction motion planning problems, we propose to do the following: The attitude or orientation of a spacecraft (modelled 0 formally treat the velocity variables as controls, as a rigid body) is the matrix g E SO(3) whose rows are thereby considering only the kinematics of the situa- the directions of the bodys principal axes with respect tion, not the dynamics (and then find the physically to some reference coordinate system. Let us denote by realisable controls by studying the inverse dynamics); 11, Iz,I3 the moments of inertia, by W ~ , W Zw3 , the anguT2, T 3 the exerted torques about lar velocities and by TI, 0 introduce time-dependent weighting functions in the cost functional to be minimised, which ensure that the principal axes. Then the attitude kinematics of the boundary conditions on the formal controls (i.e., the spacecraft are described by the equation kinemat.ica1variables) are guaranteed to be satisfied; 0 introduce penalty terms in the cost functional which ensure that applicable state constraints are not v i e where lated; 0 formulate the problem as a fixed-endtime problem, 0 0 -1 but derive duration estimates which ensure that the control algorithm can be performed during the chosen time interval in the presence of actuator constraints. Following this approach gives us some freedom in the whereas t,he dynamics are governed by Eulers equations choice of the cost functional to be minimised, which should be rea~onable. but is not at all unique. The idea is not so much that the minimisation of a specific cost functional is a strict requirement, but rather that introducing such a cost functional makes available powerful methods from optimal control theory as tools to solve motion planning problems. Our main tool will be the following version of Attitude control is the discipline concerned with devising (a weak form of) Pontryagins Principle (cf. [lo], Theorem control laws to produce desirable attitude motions of the 2) which is tailored to control problems on Lie groups in spacecraft (cf. [13]). In many space missions it is imwhich the governing differential equation is invariant un- portant that certain body axes stay away from forbidden der the group (whereas the cost functional can arbitrarily space directions, a typical example being the re-pointing of a space telescope from one observation target to the depend on both the state and the controls). next during which the telescope must not be pointed toTheorein 1. Let G be a Lie group with Lie aIgebra wards bright objects even for a short time. Typically, g and let ( E l , . . ., E,) be a vector space basis of g . Con- control specifications to effect such manceuvres are not

1. Introduction

h2

0-7803-7298-0/02/$17.00 0 2002 AACC

1972

determined by the on-board software, but in the control centre and then uplinked to the spacecraft, and avoidance of forbidden directions is ensured in a rather roundabout way by performing, if necessary, not a single slew, but a sequence of slew manceuvres connecting the initial attitude to the target attitude v i a a sequence of intermediate attitudes chosen in such a way that forbidden directions are avoided. However, the current tendency of using formations and constellations of small low-cost spacecraft rather than flying many instruments on a single large platform (see [3]) necessitates a much higher level of automation and on-board autonomy, including the autonomous specification and execution of attitude manoeuvres. As one step in this direction, we address the following problem: Steer a rigid spacecraft within a given time interval from rest to rest between prescribed attitudes in such a way that a given body-fixed direction b is guaranteed to avoid a given space-fixed direction d during the motion. The a p proach proposed in the introduction is now implemented by choosing a cost functional which penalises both high angular velocities and proximity to forbidden attitudes.

Proof. We first verify that each of the functions @ i ( t ):= llQ(t)112, @ z ( t ) := ( n ( t ) , b ) and Q3(t) := ( R ( t ) , g ( t ) d )is a solution of the differential equation

Then we solve (2.6) by observing that 2F(R,(p) equals so that the function x[c] is an integrating (d/dt)(ln~[c]) factor for (2.6);consequently, (2.6) can be rewritten in the form (dldt) (x[c]O) = 0 which yields x[cJO = C.

Remark 1. Plugging the first formula in (2.5) into the cost functional (2.3) yields CIq(t)-ldt = C l ; hence the constant C1 indicates the control effort. As a possible comparison manaeuvre which avoids the forbidden direction we can always choose an eigenaxis slew with angular velocities wi t = ci q ( t ) where the ci are constants such that c:+ ci+ci < 2a (see [SI); hence any available upper bound x* for X ( ( b , g ( t ) d ) )yields the estimate C1 5 47r2x* for C1.

J2

Theorem 3 can now be used to write the optimal anTheorem 2. Let g o , g 1 E SO(3) be given attitudes, gular velocities at any time as a function of the,current let [to,tl]be a given time interval, [et b E W3 the body coordinates of a body-fixed direction (telescope direction) and attitude. let d E R 3 be the space coordinates of a given space-fixed Theorem 4. Let 2 I+ w ( t ) = q(t)-R(t) be the opdirection (forbidden direction). Let q : (to,tl) -+ (0,co) be a function which is absolutely continuous on each closed timal angular velocity evolution and let t I+ g ( t ) be the subinterval of ( t o , t l ) , satisfies q(t) + co as t + to and associated attitude evolution. Writing U ( t ) := g(t)d and V t := b x g(t)d (so that IlU(t)((= t + t l and also satisfies the condition & : q(t)-dt = 1. and letting u ( t ) := U ( t ) / ] l U ( t ) ]and l Moreover! let x : I -+ (0.03) be an absolutely continuous we haoe Q ( t ) = 61( t ) b+ &.(t)u(t) + function defined on some interval I [-11 1 1 containing y ( t ) := V(t)/llV(t)l(, (b,god) and ( b , g l d ) . Let the angular velocities t I-+ w i ( t ) R 3 ( t ) v ( t )where of a rotating rigid body be such that

;$i!ii?dd)
R --,
c 2

(2.3)

It1 x(
to

( b , g ( t ) d ) ) ~ ( t ) ( w l ( t ) 2 + ~ z ( t )k 2w + ) dt

- x[cl

Rz=

c3-c2c
X [ C ] G

becomes minimal. Writing R := ( q w 1 , q w Z , q ~ 3 ) ~ c . := (b,gd), F := x[c]/(2px[c]) and cp := b x gd (so that in particular c(t) = cos(8(t)) where O(t) is the angle between the telescope direction and the forbidden direction at any time t ) , we have

6zu + Q
(2.5).

Pcoof. Take on both sides of the equation R = 6 l b +

~ the c

inner product with b, g d and C 2 while using

(2.4)

d + SF(S1,cp)R

= FIIR((2p.

Proof. This follows from a direction application

of Theorem 1 in conjunction with the bracket relations [El,Ez] = -E3, [ E z E ,3 1 = -El and [E3,E l ] = -Ez. The system (2.4) turns out to be completely integrable, as will be shown in the following theorem.

Theorem 3. There are constants C1, C2 and C3 such that along the trajectories of (2.4) we have

Ideally, we would like to plug the expression just obtained for w = w(.; C1,CZ, C s ) into (2.1), integrate the resulting differential equation (starting from the initial condition g ( t o ) = go) to obtain t C) g ( t ; C l , C2, C3) and then i by matching g ( t 1 ; C1, Cz,C3) determine the constants C with g l . . Unfortunately, the integration cannot be carried out by quadratures. Therefore, we try to derive three equivalent scalar equations which, a t least in principle, can be used to determine the constants of integration. We start by deriving from (2.5)a differential equation for the function c which gives us c in analytical (if implicit) form.

1973

Theorem 5. The function t ferential equation

c) c ( t )

satisfies the dif-

in which the sign ambiguities are global; i.e., one and only one choice of sign is valid throughout the whole maneuure interval. Proof. The result follows from differentiating (2.9) and invoking (2.5) and (2.8).

We now derive an estimate for the torque to be a p Proof. On one hand we have c = (b,gd) = -(b,w x gd) = -det(b,w,gd) = det(R,b,gd)/q. On the other plied in order t o effect the angular velocity evolution (2.4). hand, using (2.5) and the fact that (det A) = det(ATA), we find that Theorem 7. Let 0 be the maximal moment of inertia of the spacecraj? and let A be the maaimal difference between the moments of inertia. Then

Evaluating the determinant yields (2.8).

m
Proof. Denoting by I the inertia tensor of the spacecraft, we can write Eulers equations (2.2) in t h t form T = w x Iw + ILL = q-2R x I R - qq-21Cl + q - l I i 2 , i.e., T = q-2(QxIIR+I(gd-gR)). Theclaimthenfollowsby using elementary estimates invoking (2.4) and (2.5).
To ease our life we now make the special choice which a purely analytical treatment is possible. It turns out that or this choice there are only two possibilities for the function c: either it is strictly monotonic (which means that the angle between the telescope axis and the forbidden direction either increases or decreases throughout the manczuvre), or else c is negative for the first part of the manceuvre and positive for the second (which means that the telescope axis first moves away from the forbidden direction for some time and then approaches it again until the end of the manoeuvre).
~ ( z := ) 1/(1- z) for

Remark 2. In (2.7) we have 8 3 = (R,u)_= det(R, 6,g d ) / J s = qk/-; hence the sign of R3, which was left undetermined in (2.7), is that of c. Remark 3. On each time interval on which 1 5 does not change sign the differential equation (2.8) can be solved by separating variables; hence, starting with the value c ( t 0 ) = C O - the function c can, in principle, be determined. While the choice of the functional (2.3) is likely to ensure the avoidance of the forbidden direction, we have not been able to deduce in general that any solution of (2.8) avoids the value 1; however, this will be established for the special choice ~ ( z:= ) 1/(1- z). We show next how knowledge of the function c (which means knowledge of the angle between the telescope direction and the forbidden direction during the motion) can be exploited to determine the constants of integration C1, C z and C3. The key will be to study the angular functions t c) P ( t ) and t ++ a ( t ) which measure the rotation of the telescope direction about the forbidden direction and the rotation of the plane perpendicular to the forbidden direction during the motion, respectively. (In particular, knowledge of c and 0 means knowledge of the motion of the telescope direction.) Theorem 6. Let d l , write s ( t ) := defined by

, / -

(<,v) be

an orthonormal basis of and let t c)P ( t ) be the angle

Figure 1 : Typical behaviour of the functions t

Moreover! let U ,V,u,.u be us in Theorem 4 and define the angular function t I+ a ( t ) by u ( t ) = cos(a(t)) u(t0) sin(a(t)) u ( t 0 ) . Then the functions a and p satisfy the diflerential equations

I+ c ( t ) .

The exact determination of the constants C1, C 2 and C 3 from the initial attitude and the target attitude is too technical to be described (see [ll]for details); suffice it t o say that the calculations boil down to solving a quartic polynomial equat,ion and making several caseby-case distinctions, with the consequence that the control algorithm

1974

does not require iterative methods and hence can be executed in a fixed number of steps which is known a p n o r i : this is important if the algorithm is supposed to be autonomously executed by the on-board software. Moreover, since the angle between the telescope direction and the forbidden direction during the manoeuvre is never smaller than at the beginning or at the end of the manceuvre, there is no need to prescribe a safety margin for this angle during the manoeuvre. This observation also provides the upper bound x* := max{,y((b,god)), x ( ( b , g l d ) ) } for ~ ( ( bg ,( t ) d ) ) . Applying Theorem 7 in conjunction with given in Remark 1 and the a prrorz estimate C1 5 47r2~* using the fact that -1 5 c(t) < 1 for all t , we find that,

Figure 2: Angular velocities about the body axes.

Choosing the weighting function q ( t ) := D 3 / ( 6 t ( D- t ) ) where D is the - arbitrary - manmuvre duration, this leads to the estimate IIT(t)llI ( A + O ) R ' ~ * / ( ~ Dthis ' ) ) ;is an a pnorz bound on the required torque in terms of the spacecraft characteristics (embodied in A and the desired attitude change (embodied in x*)and the manceuvre L duration D . Conversely, if there is an actuator constraint Figure 3 : Torques about the body axes for an inertially llT(t)ll 5 Tmax then, an admissible manoeuvre is guaran- symmetric spacecraft. teed to be possible in time

e),

I
To discuss a numerical example, let us choose
Figure 4: Distance between current direction and target direction for each of the body axes.

Thus we simply want to accomplish a rotation by 90 degrees about the y-axis; however, a pure rotation about this axis would align the telescope direction with the forbidden direction halfway through the manoeuvre. Implementation of our algorithm with the weighting function q ( t ) = D 3 / ( 6 t ( D - 2)) leads to a rotational manceuvre which is visualised in the following diagrams. The first two diagrams show the angular velocities and the required torques which give rise to these angular velocities for an inertially symmetric spacecraft; the last two diagrams show how the axis directions approach the corresponding target directions during the manoeuvre and highlight the fact that the second axis first moves away from its target before approaching it; the cone in the last diagram represents the forbidden direction.

Figure 5: Spacecraft motion from the initial attitude (light grey) to the target attitude (black).

1975

3. Parking a Car
As a second application of the approach proposed in the introduction, we consider the problem of parking a car. With respect to a fixed Cartesian coordinate system, we denote by (2, y) the position of the car's centre of mass and by p the angle between the car's axis and the horizontal; moreover, we denote by U the velocity and by w the angular velocity of the car. Then the kinematics are governed by the equations
(3.1)

so that (3.1) becomes

which is a right-invariant control system on the Lie group G. We now ask for the controls t I+ u ( t ) and t e w(t) which steer (3.2) from a given initial state g(t0) = go to a given target state g(t1) = g1 while minimising a cost functional of the form

[:[:;]= [

u(t) sincp(t) '

+(t) = w(t)
with a given function q : ( t o , t l ) + (0, CO) with q ( t ) + 00 for t + t o and !i + t l and given positive constants a, j3 E R. (A typical choice would be the mass of the vehicle for a and its moment of inertia about the z-axis for a.) We apply Theorem 1, after first convincing ourselves that the case of abnormal extremals ( E = 0) cannot occur in our case. Thus choosing E = -1/2, we find

hence (d/dt)(Pqw) = EO) = -p([wEo uEl,Eo]) = To exhibit the symmetry of this control system, it is useful -p(u&) = -up(Ez), next (d/dt)(aqu) = i ( E 1 ) = to recast it as a control system on the group G = SE(2,W) -P([wEo UEI, E l ] ) = -p(-wEz) = wp(E2) and fiof planar motions, which we represent as the matrix Lie nally (d/dt)p(t)Ez = -y ([wEo uE1, Ez]) = - p ( w E l ) = group -W p ( E l ) = -aqwu. Letting t ( t ):= p(t)E2, this reads

Figure 6: Geometry of the parking problem.

G =

y)

)DES0(2,R),vER2}.
(3.4)

d dt

-((pqw)

= - U < , dt -(aqu) = w s ,

d &= -aqwu.

The Lie algebra g of G is spanned by the generators

which satisfy the bracket relations [Eo,El] = -Ez, [EO, Ez] = El and [ E l ,E21 = 0. Associating with each , ( t ) , p(t)) of (3.1) the trajectory trajectory t I+ ( z ( t ) y 2 I+ g ( t ) in G defined by cosp(t) sincp(t) 0 -sin p(t) cosp(t) 0 , Y(t) 1 4t) we find that

The first two equations in (3.4) show that w(8qw)' + u(aqu)* = 0, which implies that aq2u2 Pq2w2 is constant (and hence represents an integral of motion). Consequently, there is a constant E such that ( u ( t ) / f 1 2 + ( u ( t ) / a Z= E2/q(t)'. Thus there exists a function @ such that

.P equals

Plugging (3.5) into (3.3) gives OPE' q(t)-'dt, which shows that the constant E measures the control effort. To determine a, we observe that the first two equations in (3.4) imply that

& :

-w sinp w cos p 0 -wcosp -wsinp 0 ucosp usinp 0

Now the left-hand side of (3.6) is

1976

whereas the right-hand side equals

(3.8)

E . E(g&sin Q - crJ;?cosB)/q
= -<.~fl(+cosc~-fisinc~)/q.
is

satisfies the differential equation I = A r , which implies that the function h(t) := g(t)-F(Q(t)) satisfies i= n-s-l(r(Q)-A(&)r(&)) = 0 and hence isconstant. Consequently, g(t) = r(Q(t))r(Q(to))-go = r(Q(t))so and thus glgol = r(Q(tl)); this last equation can now be used to determine the constants of integration E , C and k in terms of the desired change y := g l g i l in the state.

Comparing (3.7) and (3.8) we find that the function given by [ ( t )= -&$q(t)b(t) which implies (3.9)

6=

-m(d/dt)(q&).

4. References
R. W. Brockett, L. Dai: The role of elliptic functions in constructive controllability, in Zexiang Li, John F. Canny (eds.), Nonholonomic Motion Planning, Kluwer Academic Publishers 1993, pp. 1-21 R. W. Brockett, R. S. Milman, H. J . Sussmann: Differential Geometric Control Theory, BirkhauserVerlag 1983 D. Folta, L. K. Newman, D. Quinn: Design and Implementation of Satellite Formations and Constellations, Advances in the Astronautical Sciences 100 (l),pp. 57-70 B. Jakubczyk, W. Respondek: Geometry of Feedback and Optimal Control, Marcel Dekker 1998 V. Jurdjevic: Geometric Control Theory, Cambridge University Press 1996 A. M. Kovalev: Nonlinear Problems of Control and Observation in the Theory of Dynamic Systems (Russian), Kiev, Naukova Dumka 1980 N. E. Leonard, J . E. Marsden: Stability and drift of underwater vehicle dynamics: mechanical systems with rigid motion symmetry, Physica D 105 (1-3), 1997, pp. 130-162 A. M. Letov: Engineering Philosophy of optimization in the problem of analytical design of optimal controllers, Automatic and Remote Control III, vol. 1, pp. 22-28; London 1967 K. Spindler: Optimal Control on Lie Groups With Applications to Attitude Control, Mathematics of Control, Sagnals, and Systems 11 (3), 1998, pp. 197219 I<. Spindler: Attitude Control of Underactuated Spacecraft, European Journal of Control 6 (3), 2000, pp. 229-242 I<. Spindler: Attitude Manoeuvres which Avoid a Forbidden Direction ; submitted for publication G. C. Walsh, R. Montgomery, S. S.Sastry: Optimal Path Planning on Matrix Lie Groups, Proc. 33rd Conf. on Decision and Control, Lake Buena Vista, Dec. 1994 J. Wertz: Spacecraft Attitude Determination and Control, Kluwer Academic Publishers 1990

On the other hand, plugging (3.5) into the last equation in (3.4), we find that (3.10)

--? a @ E 2 sin a cos a.

a E2 Q

Comparing (3.9) and (3.10), we obtain (3.11)

( d / d t ) ( q & ) = -sin CP cos a.

Somewhat miraculously (but maybe not; cf. [l]), this equation can be explicitly integrated in terms of elliptic functions. Indeed, if we let @(t) := F ( Q ( t ) )+(n/2) where Q is an antiderivative.of l / q (and where F still needs to be determined) then Q = F(Q)/q so that (3.11) reads (3.12) F(Q) = --sin(2F(Q)). 2

(rE2

Integrating (3.12) yields 2F(Q) = CO +&E2 cos(2F(Q)) = (2aE2/k2)~(1-k2sin F2 ( Q ) )where Coissomeconstant of integration and where k2 := 2aE2/(Co + crE2). Integrating again, we find that ( ( F ( Q )k) ; = f(@EQ/k)+C where C is a constant of integration and where CE is the - k2sin2<)-1/2d<. Deelliptic integral ( ( z ; k ):= s,(l noting by 3 := (E- the inverse of (E (which is sometimes called Jacobis amplitude function), we thus have F ( Q ) = 3 ( f ( f i E Q / k ) C ;IC). Using (3.5), equation (3.2) becomes j ( t ) = q(t)-lA(Q(t))g(t) where

A ( z ) :=

-Ja%cos F ( z )
- f l E sin F ( z )

+EcosF(z)

0
0 OI.

To solve this equation, we introduce the functions c(z) := &E a(.)

:= -&E

cos F ( < )d t ,

LX

sinF([)cosc(()d(,

b ( z ) := -fiE/ssinF(C)sinc(()d( 0
and observe that the matrix function cosc(z) sinc(z) 0 -sinc(z) cosc(z) 0 4.) b(z) 1

1
1977