Optimal path planning on matrix Lie groups

WP-11 4:30

Proceedingsof the 33rd Conference on Decision and Control Lake Buena Vista, FL December 1994

-

OPTIMAL PATH PLANNING O N MATRIX LIE GROUPS

Gregory C. Walsh and Richard Montgomery and S. Shankar Ssstry Electronics Research Laboratory University of California Berkeley, CA 94720 ABSTRACT W e desire to p l a n a n optimal trajectory f o r a n airplane moving at a constant velocity f r o m some starting point and orientation t o some final point and orientation. The problem is formulated a s a n optimal control problem of a left invariant control s y s t e m o n the L i e group S E ( 3 ) in keeping with our previous work [ l , 111. T h i s paper considers the problem of optimal control o n L i e groups in general, and relates the control tower problem to classic work o n elastica. Through the u s e of Pontryagin's m a z i m u m principle and the techniques of numerical optimization, a solution to the problem i s presented. Several examples are worked out 211 detazl, including not only the model airplane o n S E ( 3 ) but also similar systems o n S E ( 2 ) and SO(3).

1.

Introduction

Motivating this study is the problem of providing trajectories for airplanes near airports. The congestion resulting from increasing traffic has already spurred much research [4]. Auto-pilots and automation in general has already helped and made air travel more safe. We would like to develop algorithms that can plan good trajectories automatically. We consider a simplified kineiiiatic model of the airplane: it will always fly forward and the controls may yaw, pitch and roll the aircraft. The cost assigned to a particular solution will be the sum square of the inputs. A good solutioii will solve the boundary conditions and minimize the cost.. This cost somehow encodes our desire t o miniinize the amount of maneuvering an aircraft will do. Certainly Euler considered the planar restriction of our problem. If roll were free, then the curvature of the R3 problem is to be minimized. This variant is known as Radon's problem. Recent work of Jurdjevic [ 5 ] and Faybusovich has done an excellent job of framing in modern language these classic problems. The siiiiplified model we chose is a left-invariant control system with drift and state g iii S E ( 3 ) . The problem is then one of optimal control of control systems on matrix Lie groups. See also [ 3 ] . This research was supported in part by the NSF under grant IRI-9011490 axid by N A S A under grant NAG2-243. Richard Montgomery is with the Department of Mathematics, University of California a t Santa Cruz, Santa Crus, C A 95064.

Control Tower Problem

....'

Figure 1: The control tower problem which motivates this study in optimal control. The airplane, from some initial position and orientation, is assigned a final position and orientation plus a final time.

2.

The Landing Tower Problem

Consider the landing tower problem for an n-dimensional matrix Lie group, G. We identify T G with G x g, where g is the Lie algebra of G. The Lie algebra element chosen to represent g E T,G is given by 6 = g-'g. Elements of g are written 6 while elements of the dual space to the Lie algebra are written $. The control system is as follows 111

i= 1

The cost to be minimized is =

;

I'uT(7)Mu(7)d7.

(2)

with M E R"'""' positive definite, symmetric, subject t o g(0) = go and g ( T ) = g f .

Proposition 1 (Maximum Principle) Trajectories of the control s y s t e m (1) generated by inputs which minimize the cost as described by (2) are solutions of the Hamiltonian system wzth Hamiltonian H ( g , $ ) equal to

or satisfy p ( g ( 6 , ) )

0-7803-1968-O/94$4.O001994 IEEE

-

1258

0,i = 1 , 2 , . . . m.

Proof: Pontryagin's maximum principle considers the lift of the optimization problem t o the cotangent manifold, T8G, which we identify with G x g*. We write the control Hamiltonian as follows

H(g,$, U )

= $(CO)

+

m

1 2

ui$(ai)- P O - U ~ M (U4 ) i=l

where $ E g* and po E R, a constant of the motion. If po = 0, the extremals are called abnormal. We will concentrate only on normal extremals, where po = 1. Because our vector fields are left invariant, the controlled Hamiltonian is independent of g. The maximum principle states t h a t the optimal controls U * will maximize the control Hamiltonian at every point of T*G for fixed g,$. The control Hamiltonian is a quadratic function of the scalar U , further when PO # 0, - H U U ( g , $ ) = M > 0. The unique maximizing inputs are

of the solution t o this problem as the shape of a n elastic rod of a fixed length forced t o have some initial and final position and orientation. Alternatively, one could think of this as an optimal control problem for a Hilare like robot car. Given t h a t the car always drives forward at a fixed velocity, our job is t o find the steering controls so t h a t the robot, starting from an initial position and orientation, arrives at some final goal position and orientation at a fixed time. We represent a state in S E ( 2 ) by a 3 x 3 matrix, g , below

=

9

;I.

[:

with R E S 0 ( 2 ) ,a 2 x 2 rotation matrix, and with The dynamics will be given as

0

E 88'.

I

1

-U

m

(5) j=1

Substitution of these controls into the control Hamiltonian completes the proof of (3) for the case of the normal extremals. For abnormal extremals, which are not considered as trajectories by the path planning algorithms which follow, the Hamiltonian is a linear function of the inputs. To extremize it, we need p ( g ( d i ) ) E 0 for i = 1 , 2 , . . .m. Clearly the momentum function $(&) is of importance. Define Pi = $(a;), the coordinate functions of $. Since the Pi are real numbers directly related to the controls, we keep track of them instead of $. The optimal Hamiltonian is then

where P, = ( P I , .. . , P , ) ~ . Because H is independent of g , the evolution of the P;'s does not depend on g . This is known in the mechanics literature as reduction. We can evolve the co-states Pi separately and later compute the phase (change in the state of 9 ) . Since the Hamiltonian is G-invariant, there are conserved quantities:

Proposition 2 (Noether's Theorem [2, 71) Given: a left-invariant control system on G , whose evolution described by (6), Then: there exists n 1 constants of the motion, given by H and p; = $ ( g - ' d i g ) .

with U corresponding t o turning velocity. This can be written more compactly as

= g&u+gd1

g

(7)

where the d ; , i E { 1 , 2 , 3 } form a basis set for t h e tangent space of S E ( 2 ) at the identity in this particular representation, and hence form a basis for the Lie algebra se(2). The matrices d l , dz correspond t o translations in t h e body I and y directions respectively, and the matrix 6 3 corresponds t o rotation. T h e cost of is u2(-r)d-r. The optimal input is U = P3. &:'dynamics of P are

P2

= P2P3 = -PIP3

P3

=

$1

-P2

.

(8)

To see this, simply apply the results of the last section this special case. The optimal Hamiltonian is

H(g,P) =

Pl

1 + TP:

.

Now we use the Poisson bracket [2]t o solve for t h e dynamics of Pi. Note t h a t Pi = {Pi,H } , and t h a t {P;,Pj} = -$([e,, e j ] ) . A Poisson bracket table proves useful.

+

-Pl

Proof: Certainly H = { H , H } = 0. For the others, simply differentiate the formula.

3.

Related Example Systems

Proposition 3 (Pendulum Solutions) Given: a Hamiltonian control system o n T * S E ( 2 ) evolution described by (8), with rotation matriz R(O(t)), Then: the angle O ( t ) evolves exactly as a pendulum, f o r constants L , AO,

G(t) = Lsin(O(t) + AO).

S E ( 2 ) Planar Elastica The solutions of the path planning problem on S E ( 2 ) are sometimes called planar elastica, because one may think 3.1

1259

(9)

Homoclinic Elastica

'Pz

1.4-

CZ

T h e maximizing inputs are given by u1 = $PI, uz = where Pi's are solutions o f

1.2-

1

- -P2P3 2c2

Pl

=

P2

p2

=

1 -P1+ -PIP3 2Cl

P3

=

c1 - c2 Pl P 2 . 2ClCZ

1-

To verify, we simply apply Proposition 1. We leave the calculation to t h e reader. As before, a Poisson bracket table will prove useful. -0.

26

-5

-4

-2

-3

-1

x axis

0

2

I

3

I

{y}

4

Figure 2: This trajectory corresponds t o a near homoclinic trajectory in t h e co-state space. T h e associated pendulum s t a r t s almost upright, swings down and around, slows, and then swings back. This trajectory was numerically approximated using a qfh order Runge-Kutta integrator.

T h e angle 0 corresponds t o t h e orientation of Hilare the robot at any given point in time. Because t h e orientation of Hilare evolves as a pendulum, we may classify optimal trajectories with regards t o pendulum motions.

Proof: Notice t h a t there is a constant of the motion for the reduced equations of motion, given by Pf P:. We write L2 = P: P 2 . Consequently, we see =

Pl(t) Pz(t)

LCOS(4(t))

= Isin($(t))

,

(10)

Differentiation reveals t h a t $(t) = -P3(t). If we differentiate again, we see

q(t) =

PZ

= Lsin(4(t)).

(11)

These are t h e pendulum equations. To finish the proof, note t h a t u ( t ) = O(t) = P3(t), thus $ ( t ) and O ( t ) are related by a phase constant.

3.2 Path planning on S O ( 3 ) We now consider a control system on S O ( 3 ) with 2 inputs and drift. We represent a s t a t e in S O ( 3 ) by a 3 x 3 rotation matrix g, and dynamics =

jl

0 1

-1 0

-U2

U1

g[

g

=

gc3

s',

where t h e matrices e^;, i E { 1 , 2 , 3 } , form a basis set for t h e Lie algebra so(3). T h e cost of a trajectory is given by s,'(clu:(T) C ~ U :(7))dT.

+

Pz

Pl O-

-pl 0

+

Ul(t)

= acos(bt+c)

uz(t)

=

asin(bt+c).

(14)

Proof: Simply compute t h e derivatives of /cl, k 2 . T h e inputs are a consequence of having P3 = 0. Proposition 5 (Phase Equation) Given: a control system on S O ( 3 ) described by (le), a desired final time T and position g f , and inputs given by ( 1 4 ) Then: the time evolution of the system is given by: g(t)

=

Ezp(t(a,O,b)T) Ezp(-(bt+

.)e,)

Proof: Consider t h e following time-varying diffeomorphisni of g(t). gT(t) =

g(t) Exp((bt

+ c)Z3)

Notice t h a t t h e dynamics of 9,. have a particularly simple form. jlT = -

(12)

P 3

-P3

Proposition 4 (Sinusoidal Solutions) Given: a Hamiltonian system on S O ( 3 ) described by (12), with cost U: u;dT, Then: the optimal inputs are given by

U2

+ gdlul + ge^suz.

P2

0

T h e controls introduce asymmetry due t o their differing weight costs. If c1 = C Z , there are two additional constants of t h e motion and t h e system is integrable, and the optimal inputs correspond t o sinusoids.

-U+ 0

where u l , uz are the scalar inputs corresponding t o twisting about t h e z and y axe in t h e body's frame. This can be written more compactly as

Pl

P32 1 p -3P2 p

+

+

I I

Pl

gZ Exp((bt

+ c)d3) + bgrd3

gr&(t)

T h e expression e^,.(t)may b e shown t o a constant of t h e motion.

-

& ( t ) = (a,O,b)T We may now integrate the formula for g(t). g(t)

=

=

+

gr(t) Exp(-(bt .)a,) Exp(td,) Exp(-(bt c)d3)

+

T h e last term, made independent by c, spins t h e solution about the e3 axis, leaving g(t)e3 invariant.

1260

T h e inverse problem naturally decomposes into two subproblems: first, find a, b so t h a t g,.(T, U , b)e3 = g f e 3 , second, find t h e c so t h a t E x p ( & ( b T + c ) )= g ; ' ( T , a , b ) g f . An application of Rodrigues formula will aid in solving the first sub-problem.

-

4.

11

-e

*(1-

cos(td2TF))

2+b2

1 - &(1-

sin(td2T-F) C O S ( t d Z x 7 ) )

4.1 The Lagrange Top If t h e cost weights C I , c2, c3 are equal, then t h e resulting system of equatioiis is integrable a n d dubbed t h e Lagrange t o p [2]. Given c = c~ = c2 = c3, there are additional constants of t h e motion, called casimiers.

1 J

The Airplane Problem

We finally consider t h e problem of steering a kinematic airplane, with t h e 6 dimensional configuration space S E ( 3 ) . W e represent a point in S E ( 3 ) by t h e 4 x 4 matrix, g , below:

L

71

Helical solutions are a 3 dimensional subset of all possible elastica. They correspond t o constant inputs and satisfy p ( t )E P ( O ) , and P4 = P5 = P6 = 0. In this case we only have three independent parameters t o choose from, a n d we have a 6 dimensional configuration space t o reach. We cannot reach any frame. However, t h e inverse problem is solvable if we restrict our attention t o a three dimensional quotient of S E ( 3 ) . It is a classic problem in robotics [8]. We will briefly present a solution.

with R E SO(3) a n d x E R3. The dynamics are given by

r

o 0 0

0

1

with U ~ , U Zand u3 scalar inputs. This can be written more compactly as g

=

gdiui

+ g6zaz + g 6 3 ~ 3+ 964.

(15)

where t h e matrices 6, form a basis set for the tangent space of S E ( 3 ) at t h e identity in this particular representation. As these vector fields are left-invariant, we may use this set t o compactly represent t h e fields and their Lie brackets. T h e cost of a particular trajectory is

T h e optimal inputs are given by u1 = ' P I , c1 LP2, u3 = LP3, where Pi's are solutions of CZ c3

[ 21 [ =

P3

p2 p3

p6+Eplp3 -P5

=

+ *PlP2

1

-(P1PzP3)T x M - l ( P 1 P z P 3 ) T

u2

=

-

el x (P4PSP6)*.

Proposition 6 ( H e l i c a l T r a j e c t o r i e s ) Given: a Hamiltonian system on S E ( 3 ) described by (15), with cost (16) so that c = C I = c2 = c3, and initial co-state values P4(0) = P5(0) = P e ( 0 ) = 0, Then: the constant inputs, which we label U = ( P I ,P z , P3), with 77 = llullz # 0 , generate a trajectory i n S E ( 3 ) described b y g ( t ) equal to

Proof: For a proof, see [ 8 ] . T h e general solution is given by t h e following. P r o p o s i t i o n 7 ( E l a s t i c Trajectories) Given: a Hamiltonian system o n S E ( 3 ) described b y (17), with cost constants c = c1 = c2 = c3 and hence constants of the m o t i o n kl,k2, k3 as defined earlier, Then: the optimal inputs are given by u l ( t ) = i k 3 , u z ( t ) = i r ( t )cos(tf)(t)), and u 3 ( t ) = i r ( t ) sin(9(t)), where r ( t ) and O ( t ) satisfy the following

with H the value of the Hamiltonian and P 4 ( t ) being the Weierstruss P f u n c t i o n under the a f i n e transformation Again, verification is a m a t t e r of computation. As before, a Poisson bracket table will prove useful.

P4(t)= 2 i P

1261

+ -31H .

Proof: We would like t o solve for P4 in terms of the constants and P4. From (17),

= P,‘P,’

P: Also since P i

+ P,”P,’ - 2PsP6PZP3

+ Pl + Pz and H kl - P,‘ 2 H - k i -2P4

(20)

are casimiers,

+

P,“ P,‘ = P,” + P,” =

Multiply these equations we get

( k l - P,2)(2H - k i - 2P4) =

@P,”

+ P,‘ + P z p l + PsP,”.

Using other casimiers we get

+

( k z - k3P4) = PzP5 P3Ps (kz - k3P4)’ = P,”P,” P3P,‘ ~ P z P ~ P s P G .

+

+

Figure 3: Sheets of constant U: +U: look much like doughnuts. The angle of the slice determines the proportions between yd, Z d , and time of flight picks t h e end point.

Therefore we may substitute these into (20) and find (Ij4)’

= =

( k f - P:)(2H - k i - 2P4) - ( k z - k3P4)’ f(P4,H,ki,kz,k3). (21)

The function f is then a cubic function of P4. The solutions are hence given in terms of elliptic functions [6]. To solve for PZ and P3, note

P;

+ P,’

=

2 H - IC; - 2P4 .

This suggests usings polar coordinates for ( P z ,P3). Thus we have 6 = arctan( and by calculation 9 =

2)

z2-Ji:2p4.

One may explicitly solve for the radius, as the length is hidden in the optimal Hamiltonian. The radius is then r

=

J2H-k,2-2P4.

(22)

technique is handled by using an Armijo step size rule [9]. The methods converge quickly, provided we start with good seeds. The seeds used t o solve the S E ( 3 ) elasticaproblem are helices. The helices form a three dimensional subset of all elastica, and therefore can only reach a three dimensional subset of the goal configurations. We take these goals t o be the positions in R3, ignoring the orientation of the final frame. Recall we have found a closed-form expression for g(t) (19) in the case of helices. Apply the Rodrigues formula [8] for the rotation matrix. The vector portion of g;’g(t) is as follows.

So the optimal controls are given by the P,(t), which are

P i ( t ) = k3 pz(t) = T(t)COS(6(t)) P3(t) = r(t)sin(O(t)).

77

=

ll4lz

x(t)

=

tu: q2

We will now solve for P4(t) in general. There is a canonical equation for the Weierstrass P function

( P ) z= 4P3 - g z P - g 3 .

(23)

+

If we choose the affine transformation P4 = U P b in ( 2 1 ) , then we may solve for the higher order terms t o insure t h a t the leading coefficients is 4 and t h a t there are no second order terms. For details see [lo]. The required transform is then a = 2 4 , b = $I€. H e l i c a l Seeds Given IT co-states or constants of the motion, we can approxiniateor compute exactly the resulting phase, g(T,P ) . This may be compared t o the final desired state g f , by setting 4.2

ge(T,PI = g(T,P l g j ’ .

1 +7 (4+ U:) sin(qt) 7

tula3 ‘U.lu3 _ _ _ - -U2 (1 - cos(qt)) - sin(7t)

v2

v2

713

The simplest helices are circles. They are obtained by setting the roll u l = 0. Alternatively, it may be desirable t o have no orientation difference between the starting and finishing configurations. If there is none, this implies t h a t q t = 2 7 ~ kfor some integer k > 0, consequently the equations simplify. T h e actual formulae for both inverse solutions are presented in the following proposition. P r o p o s i t i o n 8 (Inverse S o l u t i o n s ) Given: xd,y d and Z,J a desired location, suppose that 24 # 0, and that y j zi # 0, Then: the optimal inputs which drive the airplane to the desired position

+

u1

T o convert this error term to a vector in W“, we take the logarithm of g e ( T , P ) . T h a t is t o say, we find qe such t h a t Exp(ve) = g,(T, P ) . Consequently we have a root finding problem, and Newton-Raphson is used in the final version of t h e software. The inherent instability 191 in the

= 0 uz

= psin(()

u3

-* -

T where p = $ sin($), $ = 2atan&(

d m ,I),

1262

= pcos( 0 , Then: the optimal inputs which drive the airplane t o the desired position i n time T = Z k , with k loops, is

[7] J. Marsden and T. Ratiu. Mechanics and S y m m e t r y . Spinger-Verlag, 1994. Texts in Applied Math, Vol. 17. [8] R. Murray, Z. Li, and S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [9] E. Polak. Fundamentals of Optimization. Preprint, Univeristy of California a t Berkeley, 1993.

[lo] G. Walsh, R. Montgomery, and S. S. Sastry. Optimal path planning on matrix lie groups. Technical report, Department of EECS at the Univerisity of California, Berkeley, 1994.

I d ,y d

u1=-

xd cy

uz=E a

U3

Zd

[ll] G. Walsh, A. Sarti, and S. S. Sastry. Algorithms for steering on the group of rotations. In Proceedings of the American Control Conference, pages 1312-1316, 1993. Also as Electronics Research Laboratory Memo M93/44.

=a

where a and T are given by:

Proof: Simply insert t h e controls into the formula obtained using the Rodrigues formula. One could also try solving the nonlinear equations for a fixed time t . No solution may exist; for example, the system simply may not have enough t o time t o reach the destination. Numerical methods are used t o perturb the exact inverses described above towards the solutions actually desired. Because we have exact formulas for the forward problem, this process is very rapid. What follows is an example plot, in P I ,Pz, P3 space of the solutions t o the inverse problem for various times T . All solutions arrive a t the same final point (1.0,2.0,4.0).

5.

References

G. Walsh A. Sarti and S. Sastry. Steering leftinvarient control systems on matrix lie groups. In Proceedings of the I E E E Conference on Decision and Control, 1993. R. Abraham and J. Marsden. Foundations of Mechanics. Benjamin/Cunimings Publishing Company, 1978.

1263