- Email: [email protected]
Optimal control for principal kinematic systems on lie groups
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON ...
14th World Congress oflFAC
Copyright © 1999 IFAC 14th Triennial World Congress, Beijing, P.R. China
D-2c-04-3
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON LIE GROUPS
Jallles P. Ostrowski
Department of Mechanical Engineering and Applied Mechanics University of Pennsylvania 297 Towne Bldg., 220 S. 33rd St. Philadelphia, PA 19104-6315 , USA Email: [email protected] Web addr'ess: http://www.cis.upenn.edujjpo/
Abstract: In this paper we present a Simplified formulation of the necessary conditions for optimal controls of principal kinematic systems evolving on Lie groups. This class of systems is particularly meaningful because a number of different types of locomotion systems, such as kinematic snakes, paramecia, inchworms, mobile carts, and even the falling cat, can be represented in this form. Furthermore, it is shown that for systems on Abelian Lie groups, the equations describing the optimal control inputs take on an even simpler form, based purely on the curvature of the kinematic connection describing the locomotion. These ideas are presented with several examples, including the cylinder (paramecium) swimming at low Reynolds' number and a normal form for N-trailer systems. Copyright ©1999 IFAC Keywords: Optimal Control, Robotics, Differential Geometry, Nonlinear Systems
1.
INTR.ODUCTIO~
The use of tools from differential geometry has recently revealed surprising new insights into the mechanics and controls of a wide array of biological and robotic locomotion systems. This research has resulted in what amounts to a set of normal forms that model a variety of kinematic and dynamic locomotion systems. This paper develops an insightful formulation of the optimal controls for so-called principal kinematic locomotion systems. These systems include many traditional kinematic nonholonomic systems, requiring only the additional structure of a Lie group symmetry. This additional structure allows one t.o simplify the equations governing the optimal inputs. The research into the control of nonholonomic systems is quite extensive and we will not go into depth here . Some excellent references in-
elude Li and Canny (1993) and Murray and Sastry (1993). Coupling the research int o nonholonomic systems with tools from differential geometry has led t.o significant progress in understanding locomotion. Kinematic locomotion systems that have been studied to date include inchworms (Kelly and Murray, 1995), paramecia (Kelly and Murray, 1996), mobile robotic vehicles (KeUy and Mnrray, 1995) , robotic snakes (Ostrowski and Burdick, 1998), and even some basic models of walking (Good\vine and Burdick, 1997; Kelly and Murray, 1995). At the same time, unconstrained mechanical systems with symmetrics admit the same normal form as principal kinematic systems when restricted to a zero momentum manifold. Systems that have been studied in this context include the falling cat (Montgomery, 1990), satellites in space (vValsh and Sastry, 1995), and floating fourbar linkages (Yang and Krishnaprasad, 1994).
2107
Copyright 1999 IFAC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON ...
2. OPTIMAL FORMULATION The study of kinematic locomotion systems relies on a simple principle found in most, if not all, locomotion systems to date. This principle holds that the process of locomotion can roughly be divided into two classes of variables: internal, or shape, variables and position, or group, variables. The internal variables are those which are assumed to be directly controlled as a part of the internal shape of the system. These could be the shape of a wriggling snake, the wheel angles of a mobile robot, or the internal gyrations of a falling cat. In all cases of locomotion, the motions of these internal shape variables couple to produce a net change in the posit-ion and orientation of the moving body. The position variables can naturally be described in terms of a Lie group (usually IRk, SE(2), or 50(3)), and so provide a useful mathematical structure with which to work. Additionally, locomotion systems are characterized by the fact that. t.he constraints tend to also be invariant v,rith respect to the group action. This generally implies that the forces of constraint are typically body fixed forces that are independent of the relative position and orientation from which the mot.ion is started. Let's begin with the basic equations for a principal kinematic system (sec KelIy and Murray (1995); Ostrowski and Burdick (1998). Letting g E G denote t.he Lie group variables, and r E }'1 denote the shape variables, the equations can be written quite succinctly as:
e = g~lg = f
-A(r)f,
= u.
(1)
(2)
This is the simplest, yet most general, form of kinematic locomotion systems, and so wc will sometimes refer to this as a "normal form" for principal kinematic locomotion. To take a closer look at this equation, and to understand it in the context of locomot.ion, we first note the manner in which the equations decouple. In Eq. 2, the internal shape r is assumed to be directly controlled. The invariance of the system is seen in Eq. 1, where the group variables can be fully isolated in the equation. Here, they are written in terms ot a Lie algebra description of the group velocity as ~ E {I (equivalently, a body velocity representation). Finally, the critical map relating shape change to position change is encoded in A, a Lie algebra valued map from TrM to 9 that is called a connection. The entire theory, including ext.ensions to dynamic systems with nonzero momenta, has a deep mathematical framework based on the study of connections on principal fibcr bundles (Kelly and Murray, 1995; Ostrowski, 1995; Ostrowski and Burdick, 1998). For our purposes, we work with a kinematic con-
14th World Congress ofTFAC
nection on a trivial principal fiber bundle (kinematic because there are no second order dynamics, and trivial because it is a simple product bundle between a group, G, and a shape space lVf). The results discussed below are certainly motivated by our desire to develop methods for controlling locomotion systems. However, it should be remembered that the results apply generally for kinematic connections on principal fiber bundles. There do seem to be a rich number of systems satisfying these criteria, including free-floating rigid body systelns and SOIIle nOTInal forms, such as a particular version of chain forIll. As mentioned above, a great deal of effort has been given to understanding issues of dynamics and controllability for these types of systems. We are interested in investigating opt.imal control for these systems. N amcly, we assume the existence of a quadratic cost function, C(r, r) = ~Ca.6raf·6, and a cost functional J = C(r, r)dt. Notice that the cost function is chosen solely as a function of the shape variable, which corresponds to calculating the cost of the control effort (as opposed, for example, to the total distance travcled). The extension to more general cost functions is st.raightforward. The optimal control problem is to solve for the inputs that will give the minimal cost, J, while steering the state fr·om the initial condition, (ro, go), to the final condition, (rI, gd (which we assume to occur at t = 1).
J;
\Ve can formulate the problem of minimizing J as a constrained variational problem:
= 0, (dO), g(O» = (ro, go), = h,gl), and (3) = g~l.iJ = -A(r)f. (4)
Find u such that {)J (r(I),g(1» ~
Alternatively, we can re-formulate this as an unconstrained variational problem using a multiplier technique to construct an extended Hamiltonian,
+ AT(~ + A(r)f)
H = C(r,r)
= !C 2 a6 .r
Ot
f{3
(5)
+ Aa(C + AaIT fC<).
(6)
The necessary conditions for optimality of solutions (~(t), r(t), r(t), A(t» are then d aH 8H c r:b 0 dt 8t;,Ct - af,c Cba <" = ,
(7)
.!i oH _ oH
(8)
dt
af'"
ora
= o.
.
where cba are the structure constants for the Lie algebra. This is essentially a corollary to the results derived by Koon and :Marsden (1996), where they show these equat.ions to be the EulerPoincare equations for H. Again, details on the appearanc.e of the Lie algebra structure constants are not addressed here~ see Ostrowski (1995); Kelly and Murray (1995); Ostrowski and Burdick 2108
Copyright 1999 IF AC
ISBN: 008 0432484
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
14th World Congress ofIFAC
(1998). It should be pointed out however, that for Abelian groups the left and right actions commute, and the structure constants are identically zero. This leads to simplifications in the resulting equations.
COROLLARY 2.2. For a principal kinematic system on an Abelian Lie group, the optimal cont.rols given in terms of the shape variables, r must satisfy
Using Eqs. 7 and 8, we can derive the following result.
(21)
= -ADA(r,·) .:\=0.
= ->..DA(r,·) ,\ = ->"£i-,
(i.e.,
Next, we turn our attention to several examples. EXAMPLE
BH _
·./3
\
",a
Building on the models developed by Blake (1971) and Kelly and Murray (1996), we see that the kinematic connection for t.he paramecium can be determined by examining the Stokesian flow around a ' deformable cylindrical body. Let us parameterize the body in polar coordinates (R,O) as
(12)
+ Aal'll.""
af31
&A»'(3
(13)
-a TO ->'a-a ro' r,
which, upon substitution of the constraints in
R = 1 + f(kl (t) cos 2() + k2(t) cos(3()).
Eq. 1, leads to \
Aa -
C a(3r..(3
+ ;.
Aa
+ >.
aAa
a:
"\ . e ",b .{3 Accba"'.B r -
0
(22)
Thus, the shape variables for this system are = (kl,k2). As always, we assume that we have full control of the shape.
, (14)
aNl .(3 _ , _13_·{3 Aa Or'" T'
3.1. Locomotion at low Reynolds'
number
(11) -
•
3. EXAMPLES
Proof: The proof is a simple calculation (with some substitutions) of the optimality conditions. First,
_c
cbe := 0).
(9) (10)
w'here DA is the curvature (exterior derivative) of the connection and £~f3 = c~bA~ .
aH ar a
(20)
Proof: The equations follow directly from the proposition by realizing that the structure constants for an Abelian group are identically zero
2.1. For a principal kinematic syst.em defined as in Eqs. 1 and 2, the optimal controls given in terms of the shape variables, T'must satisfy PROPOSITION
Cr
= -AdA(r,·)
Cl'
T'
= 0•
Let x denote the motion of the centroid of the body in a direction given by () = O. Symmetry arguments show that all resultant motion must Defining E~.13 = c~bA~ in Eq. 14 yields Eq. 10. be directed along this ray. Kelly and Murray Substituting Eq. 14 into Eq. 15 yields (1996) show that the viscous connection for the ' · 13 ( a ... b lAc·/3 ,oA~ ·.13 _ \ aA~ '/3) _ 0 paramecium can be written (to first order) as C Aa
'"
a
or/3 r
(15)
o.f3T'
+ Aa
C bc "'{3=aT'
+ Aa ori1 r
Aa
ar a
r
-.
• .
(16)
X
=
-A(r)r,
where
(17) More intrinsically we see that we can write this as ef
+ A(dA(r,.) - [Ai, A(.)]) = 0,
(18)
Cr = -ADA(r, .),
(19)
or where DA is the exterior derivative of A, taken as a Lie algebra-valued one-form on the base space M. The derivative of the connection is traditionally referred to as the curvature of the connection. It is used in sever al contexts of control for this type of system. •
Since this is an Abelian system (the Lie group is (JR, +)), in order to find the optimal controls associated with the simple quadratic cost function, GCT, r) = ki +k~, we write down the connection as a Lie algebra valued one-form on the shape space: €2
A = 4'(k2 dk 1
+ 2k 1 dk 2 },
and invoke Eq. 20. The exterior derivative of A is ~z
dA
= 4' (dk2
1\
dk 1
+ 2dk 1 1\ dk 2 )
€2
For systems on Abelian Lie groups, the equations are even more simple.
= 4'dk 1
1\
dk2.
2109
Copyright 1999 IF AC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
\Ve can evaluate the term ..\dA(r , ·) as
=).:
2
).dA(r,·)
2
(k 1 dk2 - k2dkd
= ,.\: (-k2 kd
14th World Congress oflFAC
(note: we have written it using ooth differential forms (useful for calculating curvature) and as a linear matrix map). Then ,
2
Setting a = A~, t he equations for the optimal control inputs are
(23)
dA _ _
-
kl
(25)
.4.
Substituting this into Eq. 23 gives
kl
= -0'2k 1
aA,
-
k2(t)
1
1\ dk2) 1\ dk3
),2kS)
_1'~2 Al kl -'"ff2 A2
k1 •
As before, these inputs can be shown t o be sinusoidal, taking the form
kl (t)
(26)
=
B coso:t + C sinat + DJa 2 0:1
al
a
a
.
kz(t) = --Bcosat+ -Csmat+E a Q a2 a2 ks(t) = -~Bcoso:t+ ~Csino:t+F
which has a solution
k1(t)
(dkdkl
= 1'E2(..\lk2 +
kz = k3 =
Integrating Eq. 24 once gives
= -o:kl +
2
and the optimal inputs must satisfy
(24)
k2
r~
A
= Bcoso:t + Csinat - 0' = B sinat - C cos at + D,
J
for some constant s A, E , C, D. Thus, we see that the optimal gaits are sinusoidal (this result was also deriveu indep cndently by ~lason (1997», with kl and k2 90° out of phase.
where 0:1 = ..\I1'€Z, a2 = ..\z~f",2, a = o:i + Q~. and B, C, D , E , F are unknowns to be determined (we have not included a term line ar in t that can easily b e eliminated with t.he appropriate choice of
constants). Similar results on trajectory planning can also then be d erived.
As mentioned above, for systcms on Abelian Lie groups, we can explicitly formulate an a rea rule based on the curvature of the connection. For this system, that rule implies that after OIle period of input (6.t = 2;), 6x
211"
= x( - ) -
f
- x(O)
0:
f
dA -
Ak'. 2
EXAMPLE 3.2. Canonica1 forIll for Illultibracket systeIlls One of the many ways of ex-
pressing the canonical form for the N-trailer or two-input multi-trailer steering problem (Murray and Sastry, 1993, p. 709) is as Xl =
(2
XZ = X3 =
4"dkl dk2
Aklk2
= Area (kl' k 2 ),
(27)
1 2 X 4 =2"X 1 1J. z Xi> =
2 71f
(B2
+
C 2 ).
Next, we can perform the same procedure for the 2-D paramecium, whose model was developed by Mason (1997). In t.his case, the shape is parameterized by
R
.
(28)
4 This could then be used to plan optimal paths, given a desired goal (though the practical caveat exists that the formulation of the problem is valid only for small oscillations on the boundary of the cylinder) .
= 1 + E(kl (t) c06(20) + k2(t) cos(30) + k3 sin(30» . (29)
The extra degree of freedom allows the para m edum to move in both x and y directions in the plane. Thus, the group is (JR2, +), and the connection is
XIUZ
.
which for this problem is just .6.x =
Ul
U2
x 6
xIX2U2
1
3
= fiX1U2.
(obviously this structure can be extended indefinitely- we'll only consider the six state system here). The derived syst em for this set of equations has a particularly nice structure. If we denote the input vector fields by Xl and X 2 , then we see that the directions X3, X4, X5, X6 a re generated by Lie brackets of the form [Xl ,X2 ],[Xl , [X 1 ,X2 ]], [X 2 , [X I ,X2 ]], and [Xl, [Xl, [Xl, X2]]], respectively. If we treat
and X2 as shape variables (i.e ., then the connection for this system can be written for 9 = (X3,X4,X5,X6) as
r
Xl
= (Xl, X2)),
(30)
2110
Copyright 1999 IFAC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
Again, this is an Abelian group (rn.4 with addition as the action), with the curvature of the connection equal to
(31)
14th World Congress ofIFAC
this type of system conserve "energy", implying that (36) where E is a constant. Thus, solutions to Eq. 35 are of the form .
Xl
the simple cost function, C ~(xi + xD, and introducing constant multipliers AI, ... , A4, we have
Choosing
Xl = (AI X2
=
+ XIA2 + X2A3 +
1 2 ) . 2"X 1A4 X2
(37)
which can easily shown to be periodic in the phase space of Xl (Wiggins, 1990). Solutions to these equations, as well as the higher order brackets can generally be found in terms of Bessel functions, though we do not pursue the full explicit solutions here.
1 2 A4 )Xl· . - (AI +XIA2 +X2 A3 + 2"X I
Finding an explicit solution to this set of equationsis unlikely. However, if we recognize the multipliers as the forces applied to enforce the constraints (Eq. 1), we can isolate optimal solutions necessary to generate each level of bracketing simply by setting the other multipliers to zero. For example, if we set A2 = A3 = A4 == 0, we arrive at the "optimal 1 " solution for motion in the :r:{ direction (i.e., a single level of bracketing). The answer here, of cour8e, is the well-known answer shown by Brockett (1982) of sinusoids. Notice, however, that it corresponds to exactly the same structure of the equations seen in the swimming cylinder example above. The two systems have different connections, but identical (modulo a constant) curvatures, and it is exactly the curvature that defines the optimal trajectories. For the higher order levels of bracketing, we get slightly more complex solutions as "optimal." For example, the [Xl, [X},X2 ]] direction, we keep .\2 -f. 0, yielding:
= A2XtX2
(32) 1 d X2 = -A2XIXl = --A2~(xi). (33) 2 dt "Ve can integrate the second equation and substitute it into the first to get
Xl
4. CONCLUSIONS
\Ve have presented the optimal control formulation for principal kinematic systems on Lie groups. These tools can be us'ed to deriye optimal controls for a wide range of applications, including many robotic locomotion systems, such as mobile robots, cars with trailers, space vehicles, inchworms, and eyen some legged robots . The results presented are basically just simplifications to the more involved dynaInic case derived by Koon and Marsden, but we have shown how the equations can be greatly simplified by looking at principal kinematic systems. In particular, we have shown that for Abelian systems the multipliers are constants and so the optimal solutions can very often be explicitly integrated using only the curvature of the kinematic connection.
ACKNO\\>'LEDGMENTS The author would like to acknowledge t.he input and suggestions from \\Tang-Sang Koon and Jerry Marsden. This work was partially funded by NSF grant IRI-9711834.
(34)
where A is a constant of integration. If we set A = 0, we can then attempt to solve the resultant equation
(35) First, we notice this equation has a first integral (it is very similar to the Duffing equation, and can be thought of as a Lagrangian system, with L = ~xf - p~xt (Wiggins, 1990, p. 29)). Solutions to
1
1 2 4 l. = ±(E+ SA2Xt)2,
This is most likely a sub-optimal solution, since we have
effectively relaxed the other constraints,
References J. R. Blake. Self propulsion due to oscillations on the surface of a cylinder at low reynolds number. Bull. Austral. Math. Soc., 5:255-264,
1971. Roger \V . Brockett. Control theory and singular Riemannian geometry. In P. J. Hilton and G. S. Young, editors, New Directions in Applied Mathematics, pages 11-27. SpringerVerlag, New York, 1982. Bill Goodwine and Joel W. Burdick. Trajectory generation for kinematic legged robot.s. In Proc. IEEE Int. Con/. Robotics and Automation, pages 2689-2696, Albuquerque, NM, 1997. 2111
Copyright 1999 IF AC
ISBN: 0 08 043248 4
14th World Congress ofIFAC
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON ...
Scott D. Kelly and Richard M. Murray. Geometric phases and locomotion.
j. Robotic Systems,
12(6):417-431, 1995.
Scott n. Kelly and Richard M. Murray. The geometry and control of dissipativc systems. In IEEE Conf. on Decision and Control, Kobe, 1996. Wang-Sang Koon and Jerrold E. Marsden. The Hamiltonian and Lagrangian approaches to the dynamics of nonholonomic systems. Tech. Report CIT Icns 96-020, Caltech, Pasadena, 1996. Z. Li and J. F. CanIl}" editors. Nonholonomic Motion Planning. KluweI', 1993. Richard Mason. Swimming motions of amoeba. Personal communication- work in progress, 1997. Richard MOIltgomery. Isoholonomic problems and some applications . Communications in Mathematical Physics, 128(3):565-592, 1990. Richard ::Vi. Murray and S. Shankar Sastry. Nonholonomic motion planning: Steering using sinusoids. IEEE Tmnsactions on Automatic Control, 38(5):700-716, 1993. James P. Ostrowski. The Mechanics and Control of Undnlatory Robotic Locomotion. PhD thesis, California Institute of Technology, Pasadena, CA, 1995. Available electronically at http://www.cis.upenn.edu/~jpo/papers.html.
James P. Ostrowski and Joel 'N. Burdick. The geometric mechanics of undulatory robotic locomotion. Intematwnal Journal of Robotics Research, 17(7):683-702, 1998. Gregory C. Walsh and S. Shankar Sastry. On reorienting linked rigid bodies using internal motions. IEEE Transactions on Robotics and Automation, 11(1):139-146,1995. Stephen \Viggins. Introduction to Applied Nonlinear Dynamical Systems and Chaos, volume 2 of Texts in Applied Mathematics. Springer- Verlag, New York, 1990. Rui Yang and P. S. Krishnaprasad. On the geometry and dynamics of floating four-bar linkages. Dynamics and Sta.bility of Systems , 9(1):19--45, 1994.
2112
Copyright 1999 IF AC
ISBN: 0 08 043248 4
14th World Congress oflFAC
Copyright © 1999 IFAC 14th Triennial World Congress, Beijing, P.R. China
D-2c-04-3
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON LIE GROUPS
Jallles P. Ostrowski
Department of Mechanical Engineering and Applied Mechanics University of Pennsylvania 297 Towne Bldg., 220 S. 33rd St. Philadelphia, PA 19104-6315 , USA Email: [email protected] Web addr'ess: http://www.cis.upenn.edujjpo/
Abstract: In this paper we present a Simplified formulation of the necessary conditions for optimal controls of principal kinematic systems evolving on Lie groups. This class of systems is particularly meaningful because a number of different types of locomotion systems, such as kinematic snakes, paramecia, inchworms, mobile carts, and even the falling cat, can be represented in this form. Furthermore, it is shown that for systems on Abelian Lie groups, the equations describing the optimal control inputs take on an even simpler form, based purely on the curvature of the kinematic connection describing the locomotion. These ideas are presented with several examples, including the cylinder (paramecium) swimming at low Reynolds' number and a normal form for N-trailer systems. Copyright ©1999 IFAC Keywords: Optimal Control, Robotics, Differential Geometry, Nonlinear Systems
1.
INTR.ODUCTIO~
The use of tools from differential geometry has recently revealed surprising new insights into the mechanics and controls of a wide array of biological and robotic locomotion systems. This research has resulted in what amounts to a set of normal forms that model a variety of kinematic and dynamic locomotion systems. This paper develops an insightful formulation of the optimal controls for so-called principal kinematic locomotion systems. These systems include many traditional kinematic nonholonomic systems, requiring only the additional structure of a Lie group symmetry. This additional structure allows one t.o simplify the equations governing the optimal inputs. The research into the control of nonholonomic systems is quite extensive and we will not go into depth here . Some excellent references in-
elude Li and Canny (1993) and Murray and Sastry (1993). Coupling the research int o nonholonomic systems with tools from differential geometry has led t.o significant progress in understanding locomotion. Kinematic locomotion systems that have been studied to date include inchworms (Kelly and Murray, 1995), paramecia (Kelly and Murray, 1996), mobile robotic vehicles (KeUy and Mnrray, 1995) , robotic snakes (Ostrowski and Burdick, 1998), and even some basic models of walking (Good\vine and Burdick, 1997; Kelly and Murray, 1995). At the same time, unconstrained mechanical systems with symmetrics admit the same normal form as principal kinematic systems when restricted to a zero momentum manifold. Systems that have been studied in this context include the falling cat (Montgomery, 1990), satellites in space (vValsh and Sastry, 1995), and floating fourbar linkages (Yang and Krishnaprasad, 1994).
2107
Copyright 1999 IFAC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON ...
2. OPTIMAL FORMULATION The study of kinematic locomotion systems relies on a simple principle found in most, if not all, locomotion systems to date. This principle holds that the process of locomotion can roughly be divided into two classes of variables: internal, or shape, variables and position, or group, variables. The internal variables are those which are assumed to be directly controlled as a part of the internal shape of the system. These could be the shape of a wriggling snake, the wheel angles of a mobile robot, or the internal gyrations of a falling cat. In all cases of locomotion, the motions of these internal shape variables couple to produce a net change in the posit-ion and orientation of the moving body. The position variables can naturally be described in terms of a Lie group (usually IRk, SE(2), or 50(3)), and so provide a useful mathematical structure with which to work. Additionally, locomotion systems are characterized by the fact that. t.he constraints tend to also be invariant v,rith respect to the group action. This generally implies that the forces of constraint are typically body fixed forces that are independent of the relative position and orientation from which the mot.ion is started. Let's begin with the basic equations for a principal kinematic system (sec KelIy and Murray (1995); Ostrowski and Burdick (1998). Letting g E G denote t.he Lie group variables, and r E }'1 denote the shape variables, the equations can be written quite succinctly as:
e = g~lg = f
-A(r)f,
= u.
(1)
(2)
This is the simplest, yet most general, form of kinematic locomotion systems, and so wc will sometimes refer to this as a "normal form" for principal kinematic locomotion. To take a closer look at this equation, and to understand it in the context of locomot.ion, we first note the manner in which the equations decouple. In Eq. 2, the internal shape r is assumed to be directly controlled. The invariance of the system is seen in Eq. 1, where the group variables can be fully isolated in the equation. Here, they are written in terms ot a Lie algebra description of the group velocity as ~ E {I (equivalently, a body velocity representation). Finally, the critical map relating shape change to position change is encoded in A, a Lie algebra valued map from TrM to 9 that is called a connection. The entire theory, including ext.ensions to dynamic systems with nonzero momenta, has a deep mathematical framework based on the study of connections on principal fibcr bundles (Kelly and Murray, 1995; Ostrowski, 1995; Ostrowski and Burdick, 1998). For our purposes, we work with a kinematic con-
14th World Congress ofTFAC
nection on a trivial principal fiber bundle (kinematic because there are no second order dynamics, and trivial because it is a simple product bundle between a group, G, and a shape space lVf). The results discussed below are certainly motivated by our desire to develop methods for controlling locomotion systems. However, it should be remembered that the results apply generally for kinematic connections on principal fiber bundles. There do seem to be a rich number of systems satisfying these criteria, including free-floating rigid body systelns and SOIIle nOTInal forms, such as a particular version of chain forIll. As mentioned above, a great deal of effort has been given to understanding issues of dynamics and controllability for these types of systems. We are interested in investigating opt.imal control for these systems. N amcly, we assume the existence of a quadratic cost function, C(r, r) = ~Ca.6raf·6, and a cost functional J = C(r, r)dt. Notice that the cost function is chosen solely as a function of the shape variable, which corresponds to calculating the cost of the control effort (as opposed, for example, to the total distance travcled). The extension to more general cost functions is st.raightforward. The optimal control problem is to solve for the inputs that will give the minimal cost, J, while steering the state fr·om the initial condition, (ro, go), to the final condition, (rI, gd (which we assume to occur at t = 1).
J;
\Ve can formulate the problem of minimizing J as a constrained variational problem:
= 0, (dO), g(O» = (ro, go), = h,gl), and (3) = g~l.iJ = -A(r)f. (4)
Find u such that {)J (r(I),g(1» ~
Alternatively, we can re-formulate this as an unconstrained variational problem using a multiplier technique to construct an extended Hamiltonian,
+ AT(~ + A(r)f)
H = C(r,r)
= !C 2 a6 .r
Ot
f{3
(5)
+ Aa(C + AaIT fC<).
(6)
The necessary conditions for optimality of solutions (~(t), r(t), r(t), A(t» are then d aH 8H c r:b 0 dt 8t;,Ct - af,c Cba <" = ,
(7)
.!i oH _ oH
(8)
dt
af'"
ora
= o.
.
where cba are the structure constants for the Lie algebra. This is essentially a corollary to the results derived by Koon and :Marsden (1996), where they show these equat.ions to be the EulerPoincare equations for H. Again, details on the appearanc.e of the Lie algebra structure constants are not addressed here~ see Ostrowski (1995); Kelly and Murray (1995); Ostrowski and Burdick 2108
Copyright 1999 IF AC
ISBN: 008 0432484
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
14th World Congress ofIFAC
(1998). It should be pointed out however, that for Abelian groups the left and right actions commute, and the structure constants are identically zero. This leads to simplifications in the resulting equations.
COROLLARY 2.2. For a principal kinematic system on an Abelian Lie group, the optimal cont.rols given in terms of the shape variables, r must satisfy
Using Eqs. 7 and 8, we can derive the following result.
(21)
= -ADA(r,·) .:\=0.
= ->..DA(r,·) ,\ = ->"£i-,
(i.e.,
Next, we turn our attention to several examples. EXAMPLE
BH _
·./3
\
",a
Building on the models developed by Blake (1971) and Kelly and Murray (1996), we see that the kinematic connection for t.he paramecium can be determined by examining the Stokesian flow around a ' deformable cylindrical body. Let us parameterize the body in polar coordinates (R,O) as
(12)
+ Aal'll.""
af31
&A»'(3
(13)
-a TO ->'a-a ro' r,
which, upon substitution of the constraints in
R = 1 + f(kl (t) cos 2() + k2(t) cos(3()).
Eq. 1, leads to \
Aa -
C a(3r..(3
+ ;.
Aa
+ >.
aAa
a:
"\ . e ",b .{3 Accba"'.B r -
0
(22)
Thus, the shape variables for this system are = (kl,k2). As always, we assume that we have full control of the shape.
, (14)
aNl .(3 _ , _13_·{3 Aa Or'" T'
3.1. Locomotion at low Reynolds'
number
(11) -
•
3. EXAMPLES
Proof: The proof is a simple calculation (with some substitutions) of the optimality conditions. First,
_c
cbe := 0).
(9) (10)
w'here DA is the curvature (exterior derivative) of the connection and £~f3 = c~bA~ .
aH ar a
(20)
Proof: The equations follow directly from the proposition by realizing that the structure constants for an Abelian group are identically zero
2.1. For a principal kinematic syst.em defined as in Eqs. 1 and 2, the optimal controls given in terms of the shape variables, T'must satisfy PROPOSITION
Cr
= -AdA(r,·)
Cl'
T'
= 0•
Let x denote the motion of the centroid of the body in a direction given by () = O. Symmetry arguments show that all resultant motion must Defining E~.13 = c~bA~ in Eq. 14 yields Eq. 10. be directed along this ray. Kelly and Murray Substituting Eq. 14 into Eq. 15 yields (1996) show that the viscous connection for the ' · 13 ( a ... b lAc·/3 ,oA~ ·.13 _ \ aA~ '/3) _ 0 paramecium can be written (to first order) as C Aa
'"
a
or/3 r
(15)
o.f3T'
+ Aa
C bc "'{3=aT'
+ Aa ori1 r
Aa
ar a
r
-.
• .
(16)
X
=
-A(r)r,
where
(17) More intrinsically we see that we can write this as ef
+ A(dA(r,.) - [Ai, A(.)]) = 0,
(18)
Cr = -ADA(r, .),
(19)
or where DA is the exterior derivative of A, taken as a Lie algebra-valued one-form on the base space M. The derivative of the connection is traditionally referred to as the curvature of the connection. It is used in sever al contexts of control for this type of system. •
Since this is an Abelian system (the Lie group is (JR, +)), in order to find the optimal controls associated with the simple quadratic cost function, GCT, r) = ki +k~, we write down the connection as a Lie algebra valued one-form on the shape space: €2
A = 4'(k2 dk 1
+ 2k 1 dk 2 },
and invoke Eq. 20. The exterior derivative of A is ~z
dA
= 4' (dk2
1\
dk 1
+ 2dk 1 1\ dk 2 )
€2
For systems on Abelian Lie groups, the equations are even more simple.
= 4'dk 1
1\
dk2.
2109
Copyright 1999 IF AC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
\Ve can evaluate the term ..\dA(r , ·) as
=).:
2
).dA(r,·)
2
(k 1 dk2 - k2dkd
= ,.\: (-k2 kd
14th World Congress oflFAC
(note: we have written it using ooth differential forms (useful for calculating curvature) and as a linear matrix map). Then ,
2
Setting a = A~, t he equations for the optimal control inputs are
(23)
dA _ _
-
kl
(25)
.4.
Substituting this into Eq. 23 gives
kl
= -0'2k 1
aA,
-
k2(t)
1
1\ dk2) 1\ dk3
),2kS)
_1'~2 Al kl -'"ff2 A2
k1 •
As before, these inputs can be shown t o be sinusoidal, taking the form
kl (t)
(26)
=
B coso:t + C sinat + DJa 2 0:1
al
a
a
.
kz(t) = --Bcosat+ -Csmat+E a Q a2 a2 ks(t) = -~Bcoso:t+ ~Csino:t+F
which has a solution
k1(t)
(dkdkl
= 1'E2(..\lk2 +
kz = k3 =
Integrating Eq. 24 once gives
= -o:kl +
2
and the optimal inputs must satisfy
(24)
k2
r~
A
= Bcoso:t + Csinat - 0' = B sinat - C cos at + D,
J
for some constant s A, E , C, D. Thus, we see that the optimal gaits are sinusoidal (this result was also deriveu indep cndently by ~lason (1997», with kl and k2 90° out of phase.
where 0:1 = ..\I1'€Z, a2 = ..\z~f",2, a = o:i + Q~. and B, C, D , E , F are unknowns to be determined (we have not included a term line ar in t that can easily b e eliminated with t.he appropriate choice of
constants). Similar results on trajectory planning can also then be d erived.
As mentioned above, for systcms on Abelian Lie groups, we can explicitly formulate an a rea rule based on the curvature of the connection. For this system, that rule implies that after OIle period of input (6.t = 2;), 6x
211"
= x( - ) -
f
- x(O)
0:
f
dA -
Ak'. 2
EXAMPLE 3.2. Canonica1 forIll for Illultibracket systeIlls One of the many ways of ex-
pressing the canonical form for the N-trailer or two-input multi-trailer steering problem (Murray and Sastry, 1993, p. 709) is as Xl =
(2
XZ = X3 =
4"dkl dk2
Aklk2
= Area (kl' k 2 ),
(27)
1 2 X 4 =2"X 1 1J. z Xi> =
2 71f
(B2
+
C 2 ).
Next, we can perform the same procedure for the 2-D paramecium, whose model was developed by Mason (1997). In t.his case, the shape is parameterized by
R
.
(28)
4 This could then be used to plan optimal paths, given a desired goal (though the practical caveat exists that the formulation of the problem is valid only for small oscillations on the boundary of the cylinder) .
= 1 + E(kl (t) c06(20) + k2(t) cos(30) + k3 sin(30» . (29)
The extra degree of freedom allows the para m edum to move in both x and y directions in the plane. Thus, the group is (JR2, +), and the connection is
XIUZ
.
which for this problem is just .6.x =
Ul
U2
x 6
xIX2U2
1
3
= fiX1U2.
(obviously this structure can be extended indefinitely- we'll only consider the six state system here). The derived syst em for this set of equations has a particularly nice structure. If we denote the input vector fields by Xl and X 2 , then we see that the directions X3, X4, X5, X6 a re generated by Lie brackets of the form [Xl ,X2 ],[Xl , [X 1 ,X2 ]], [X 2 , [X I ,X2 ]], and [Xl, [Xl, [Xl, X2]]], respectively. If we treat
and X2 as shape variables (i.e ., then the connection for this system can be written for 9 = (X3,X4,X5,X6) as
r
Xl
= (Xl, X2)),
(30)
2110
Copyright 1999 IFAC
ISBN: 0 08 043248 4
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON .. .
Again, this is an Abelian group (rn.4 with addition as the action), with the curvature of the connection equal to
(31)
14th World Congress ofIFAC
this type of system conserve "energy", implying that (36) where E is a constant. Thus, solutions to Eq. 35 are of the form .
Xl
the simple cost function, C ~(xi + xD, and introducing constant multipliers AI, ... , A4, we have
Choosing
Xl = (AI X2
=
+ XIA2 + X2A3 +
1 2 ) . 2"X 1A4 X2
(37)
which can easily shown to be periodic in the phase space of Xl (Wiggins, 1990). Solutions to these equations, as well as the higher order brackets can generally be found in terms of Bessel functions, though we do not pursue the full explicit solutions here.
1 2 A4 )Xl· . - (AI +XIA2 +X2 A3 + 2"X I
Finding an explicit solution to this set of equationsis unlikely. However, if we recognize the multipliers as the forces applied to enforce the constraints (Eq. 1), we can isolate optimal solutions necessary to generate each level of bracketing simply by setting the other multipliers to zero. For example, if we set A2 = A3 = A4 == 0, we arrive at the "optimal 1 " solution for motion in the :r:{ direction (i.e., a single level of bracketing). The answer here, of cour8e, is the well-known answer shown by Brockett (1982) of sinusoids. Notice, however, that it corresponds to exactly the same structure of the equations seen in the swimming cylinder example above. The two systems have different connections, but identical (modulo a constant) curvatures, and it is exactly the curvature that defines the optimal trajectories. For the higher order levels of bracketing, we get slightly more complex solutions as "optimal." For example, the [Xl, [X},X2 ]] direction, we keep .\2 -f. 0, yielding:
= A2XtX2
(32) 1 d X2 = -A2XIXl = --A2~(xi). (33) 2 dt "Ve can integrate the second equation and substitute it into the first to get
Xl
4. CONCLUSIONS
\Ve have presented the optimal control formulation for principal kinematic systems on Lie groups. These tools can be us'ed to deriye optimal controls for a wide range of applications, including many robotic locomotion systems, such as mobile robots, cars with trailers, space vehicles, inchworms, and eyen some legged robots . The results presented are basically just simplifications to the more involved dynaInic case derived by Koon and Marsden, but we have shown how the equations can be greatly simplified by looking at principal kinematic systems. In particular, we have shown that for Abelian systems the multipliers are constants and so the optimal solutions can very often be explicitly integrated using only the curvature of the kinematic connection.
ACKNO\\>'LEDGMENTS The author would like to acknowledge t.he input and suggestions from \\Tang-Sang Koon and Jerry Marsden. This work was partially funded by NSF grant IRI-9711834.
(34)
where A is a constant of integration. If we set A = 0, we can then attempt to solve the resultant equation
(35) First, we notice this equation has a first integral (it is very similar to the Duffing equation, and can be thought of as a Lagrangian system, with L = ~xf - p~xt (Wiggins, 1990, p. 29)). Solutions to
1
1 2 4 l. = ±(E+ SA2Xt)2,
This is most likely a sub-optimal solution, since we have
effectively relaxed the other constraints,
References J. R. Blake. Self propulsion due to oscillations on the surface of a cylinder at low reynolds number. Bull. Austral. Math. Soc., 5:255-264,
1971. Roger \V . Brockett. Control theory and singular Riemannian geometry. In P. J. Hilton and G. S. Young, editors, New Directions in Applied Mathematics, pages 11-27. SpringerVerlag, New York, 1982. Bill Goodwine and Joel W. Burdick. Trajectory generation for kinematic legged robot.s. In Proc. IEEE Int. Con/. Robotics and Automation, pages 2689-2696, Albuquerque, NM, 1997. 2111
Copyright 1999 IF AC
ISBN: 0 08 043248 4
14th World Congress ofIFAC
OPTIMAL CONTROL FOR PRINCIPAL KINEMATIC SYSTEMS ON ...
Scott D. Kelly and Richard M. Murray. Geometric phases and locomotion.
j. Robotic Systems,
12(6):417-431, 1995.
Scott n. Kelly and Richard M. Murray. The geometry and control of dissipativc systems. In IEEE Conf. on Decision and Control, Kobe, 1996. Wang-Sang Koon and Jerrold E. Marsden. The Hamiltonian and Lagrangian approaches to the dynamics of nonholonomic systems. Tech. Report CIT Icns 96-020, Caltech, Pasadena, 1996. Z. Li and J. F. CanIl}" editors. Nonholonomic Motion Planning. KluweI', 1993. Richard Mason. Swimming motions of amoeba. Personal communication- work in progress, 1997. Richard MOIltgomery. Isoholonomic problems and some applications . Communications in Mathematical Physics, 128(3):565-592, 1990. Richard ::Vi. Murray and S. Shankar Sastry. Nonholonomic motion planning: Steering using sinusoids. IEEE Tmnsactions on Automatic Control, 38(5):700-716, 1993. James P. Ostrowski. The Mechanics and Control of Undnlatory Robotic Locomotion. PhD thesis, California Institute of Technology, Pasadena, CA, 1995. Available electronically at http://www.cis.upenn.edu/~jpo/papers.html.
James P. Ostrowski and Joel 'N. Burdick. The geometric mechanics of undulatory robotic locomotion. Intematwnal Journal of Robotics Research, 17(7):683-702, 1998. Gregory C. Walsh and S. Shankar Sastry. On reorienting linked rigid bodies using internal motions. IEEE Transactions on Robotics and Automation, 11(1):139-146,1995. Stephen \Viggins. Introduction to Applied Nonlinear Dynamical Systems and Chaos, volume 2 of Texts in Applied Mathematics. Springer- Verlag, New York, 1990. Rui Yang and P. S. Krishnaprasad. On the geometry and dynamics of floating four-bar linkages. Dynamics and Sta.bility of Systems , 9(1):19--45, 1994.
2112
Copyright 1999 IF AC
ISBN: 0 08 043248 4