The 34 th IEEE Conference on Decision and Control

Transcription

1 Submitted to the 995 IEEE Conference on Decision and Controls Control of Mechanical Systems with Symmetries and Nonholonomic Constraints Jim Ostrowski Joel Burdick Division of Engineering and Applied Science California Institute of Technology, Mail Code 4{44, Pasadena, CA March 3, 995 Regular Paper, 34 th CDC Keywords: Nonholonomic Constraints, Nonlinear Control, Locomotion, Lie Group Symmetries Abstract This paper presents initial results on the control of mechanical systems for which group symmetries exist (i.e., the dynamics are invariant under the action of a Lie group) and are not fully annihilated by the addition of nonholonomic constraints. These types of systems are characterized by the persistence of momentum-like drift terms which are not directly controllable via the inputs to the system. We show that for systems with nonholonomic constraints (in direct contrast with unconstrained systems or systems with holonomic constraints) there exists the possibility for controlling these momentum terms. The snakeboard is used as a motivating example, and some comment is given as to the utility of these equations for robotic locomotion, as well as to more general problems which can be treated using this framework. I. Introduction MAKING use of modern advances in geometric mechanics, researchers have made great progress in analyzing the mechanics of locomotion. The problem of locomotion can be found in many dierent areas of study because it asks the fundamental question of how does a system use its control inputs to eect motion from one place to another. By utilizing the inherent mathematical structure found in these types of problems, one can formulate the dynamics of a wide variety of locomotion problems in a very intuitively appealing and insightful manner. Doing so leads to a stronger comprehension of the mechanics of locomotion, but leaves open some very basic questions about the control of such types of systems. Preliminary research [5, 7] suggests that the geometric tools used to formulate the mechanics will also provide a basis for unlocking the answers to many of the questions regarding the control theoretic issues involved. An important by-product of the mechanics research in locomotion has been the development of a theoretical bridge between systems with two dierent types of nonholonomic constraints. On one hand, there are systems with external (often called kinematic) constraints which include wheeled vehicles [3], grasping with point-nger contacts, and some models of snakes [7], paramecia [8], and even legged locomotion [5]. Recent work by Kelly and Murray [5] provides kinematically constrained models for a wide range of systems that locomote and controllability results for these types of systems. However, the models are restricted to purely kinematic systems, which require active input controls to generate movement. A kinematically constrained body in motion will remain in motion only if its control inputs are continually active. Thus it is not possible to build momentum or to \coast" it is exactly this component of locomotion that has been added in the models considered here. Characteristic of all of these systems, however, is a second type of nonholonomic constraint, which arises due to Lie group symmetries. For locomotive systems, this is often a \pick-and-place" symmetry, whereby Submitted to: The 34 th IEEE Conference on Decision and Control New Orleans, LA, December 3-5, 995

2 the rigid body dynamics are invariant with respect to inertial positioning. In the absence of external constraints, these invariances imply the existence of internal (sometimes called dynamic) constraints on the system, which very often take the form of momentum conservation laws. Examples of systems with internal nonholonomic constraints include satellites in space [6, 4] and the problem of the \falling cat" []. Naturally, there exist problems for which both internal and external constraints may exist and interact in a nontrivial manner. Examples of these problems fall generally into two realms: one in which certain of the conservation laws may remain after the addition of constraints, such as in the rolling penny or the constrained particle []; and one in which the conservation laws are transformed into what Bloch et al., term a generalized momentum equation, where the momenta are governed by a dierential equation. There is strong evidence to suggest that many dierent modes of locomotion (such as undulatory, legged, etc.) are governed by equations of this form. What is present in the case of mixed constraints (i.e., kinematic and dynamic) is the ability to change the momentum of a locomotive body, which is crucial for most types of locomotion. Thus, by using internal, shape controls it is possible not only to change the position of the system, but to generate velocities and hence truly locomote in a dynamic sense. Recent investigations have led to a unifying geometric framework within which to analyze these types of problems. Along with the problem of mechanics comes a host of associated issues to investigate, including (optimal) control, stabilization, trajectory generation, and path planning. Certainly, extensive work has been done in analyzing these issues for systems with purely kinematic or purely dynamic constraints. The additional mathematical structure aorded by these two classes of problems has led to the development of very interesting results on controllability [4, 6, 2], stabilization [2], and trajectory generation [3, 5]. Along these lines, Bloch, Reyhanoglu, and McClamroch [3], studied the case of fully dynamic systems with nonholonomic constraints. Their paper, however, required that all of the unconstrained dynamics be controlled, and was not intended to address the special structure inherent in systems with Lie group symmetries. It did include, though, proofs of controllability using Sussman's criterion [9] that will be used in a modied form for this paper. But to date, no analyses of systems which combine these two types of nonholonomic constraints have occurred. This paper is not intended to be a nal statement regarding controllability for these types of systems, but is rather meant to bring the structure of these equations to the attention of the controls community, while providing some initial results on easily computable sucient conditions for accessibility and controllability. In establishing these conditions for controllability, the authors have employed the most advanced tools of which we are presently aware for showing small-time local controllability of nonlinear control systems with drift. Finally, since the structure of the equations was largely motivated by developments in locomotion, some mention will be given to the relationship between bracketing of vector elds and trajectory/gait generation. As an example, we will examine the snakeboard model, which has been an important motivating example behind the theoretical progress for this mixed kinematic and dynamic constraint case [,, 6]. II. Background and formulation The use of Lie groups will be important for the analysis performed in this paper. The principal motivation for using Lie groups arises from our studies of robotic locomotion, where displacements occur in some subgroup of SE(3), most often SE(2) or SO(3). However, the analysis here is valid for general mechanical systems which have some or all of their dynamics evolving on a Lie group. We will tend to make reference to the former use of Lie groups as describing position and orientation of a robotic system, but the reader should keep in mind that the results hold much more generally. Formally, the displacement of a robot's body xed frame is considered as a left translation. That is, if the initial position of a rigid body is denoted by g, and it is displaced by an amount h, then its nal position is hg. This displacement can be thought of as a map L h : G! G given by L h (g) = hg for g 2 G. Hence, we can describe the evolution of the position of the robot by referencing it using a Lie group with respect to some inertial frame. The remaining components of the system are assumed to be controllable, and these conguration variables will be represented by a manifold M. Most often for locomotion systems, these variables will describe the internal shape of the system. Again, this designation of the controlled subspace with internal shape is convenient for giving an intuitive picture of a locomotive body, but it should be remembered that the manifold M is quite general, and so can describe whatever variables are necessary to complement the Lie group, G. Thus, the conguration manifold will be the product manifold given by Q = G M and the left translation induces a left action of G on Q. In the mechanics literature, the manifold Q denes a trivial principal ber bundle that is said to have bers, G, over a base space, M. We will assume the coordinates

3 on Q to be decomposable into ber and base coordinates, i.e., for q 2 Q, we can write q = (g; r) 2 G M. Denition. A left action of a Lie group G on a manifold Q is a smooth map : G Q! Q such that: () (e; q) = q for all q 2 Q, and e the identity element of G; and (2) (h; (g; q)) = (hg; q) for every g; h 2 G and q 2 Q. It will be useful to consider the left action as a map from Q into Q, with the element h 2 G held xed. Notationally, h : Q! Q is given by (g; r) 7! ((h; g); r) = (hg; r). The lifted action, which describes the eect of h on velocity vectors in T Q, is the linear map, T q h : T q Q! T hq Q. Since we are working with mechanical systems, we will assume the existence of a Lagrangian function, L(q; _q), on T Q. In the absence of constraints, the robot's dynamical equations can be derived from Lagrange's equations: i? i = ; () where is a forcing function. Our interest, however, is in systems in which nonholonomic constraints are present. These constraints can take many forms, including no-slip wheel conditions and viscous friction (though we do not deal here with viscous forces). Let us restrict our attention to Pfaan constraints, which are linear in the velocities. Given k such constraints, we can write them as a vector-valued set of k equations:! i j(q) _q j = ; for i = : : : k: (2) This class of constraints includes most commonly investigated nonholonomic constraints. The constraints can be incorporated into the dynamics through the use of Lagrange multipliers. That is, Eq. is modied by adding a force of constraint with an unknown multiplier,, as + i j! j i? i = : (3) However, once the Lagrange multipliers are solved for, much of the physical intuition of the problem may be lost. For this reason, researchers have focused on rewriting the system in terms of quantities that have physical signicance to the problem and that highlight the inherent geometric structure found in these types of systems. For systems in which the Lagrangian and the constraints are left-invariant, i.e., for which L( h q; T q h _q) = L(q; _q) and! i (hq) = T hq h?!(q); it was shown in [, 7] that the equations of motion can be transformed into the following form: g? _g =?A(r) _r + I? (r)p; (4) _p = 2 _rt _r _r (r) _r + p T p _r (r) _r + 2 pt pp (r)p; (5) _r = u: (6) These equations, of course, deserve a good deal of comment (to gain a much better insight into these equations, the reader is referred to the paper by Bloch et al. []). Eqs. 4 and 6 are the ber and base equations, respectively. They will dene velocity vectors for the conguration variables. Eq. 5 is called the generalized momentum equation, where p is a momentum vector associated with the momentum along each of the kinematically unconstrained ber directions. Notice that in Eq. 6 we have assumed the base (shape) space to be fully controllable, with velocity inputs, u. Of particular importance, however, is the term A(r) in Eq. 4. According to the standard mechanics nomenclature, A is said to dene a connection on the ber bundle Q with structure group G and base space M. The connection will satisfy certain geometric properties, the most important of which, for our purposes, will be a specication of the relationship between control velocities on T M and group velocities on T G. When investigating issues of controllability, the connection will be extremely useful, because it is the dening relationship governing the role of the control inputs in generating motion along the ber. As might be expected, derivatives of A will have a direct correspondence to Lie brackets of the control and drift vector elds.

4 The I? p term determines the eect of the momentum on the ber equations. I is called the locked inertia tensor, but its development is beyond the scope of this paper. For the terms, _r _r, p _r, and pp of the generalized momentum equation, we mention only that they are strictly functions of the base variables, r, and so the generalized momentum equation can be written as a function only of the base and momentum variables [7]. In the case where _r _r, the system can be written in standard form for a control system with drift: _z = f(z) + h i (z)u i : (7) However, this will generally not be the case. In order to write the system in the form of Eq. 7, it is necessary to dynamically extend our control inputs by redening them as higher derivatives of the input variables. This is equivalent to specifying the control inputs as accelerations, which makes sense for analyzing a fully dynamic, mechanical system. Let v = _u, and dene the manifold N for our problem to be N = G R p M T r M, where dim R p is the dimension of the unconstrained ber directions (i.e., the dimension of p). Then, using z = (g; p; r; _r) 2 N, we see that Eqs. 4{6 can be written in the form of Eq. 7 with f(z) = B@ g(?a(r) _r + I? (r)p) 2 _rt _r _r _r + p T p _r _r + 2 pt pp p _r CA ; h i(z) = where e i is the m-vector (m = dim M) with a \" in the i th row and \" otherwise. B@ e i CA ; (8) ψ φ f back wheels φ b (x,y) l θ front wheels Fig.. The simplied model of the Snakeboard. Fig. 2. Demo prototype of the snakeboard. Example. Now let us turn to a formulation of the snakeboard problem in terms of the relationships derived above. The Snakeboard is a commercial variant of the skateboard, which allows for independent rotation of the wheel trucks. The simplied model of the Snakeboard (referred to as the snakeboard model) is shown in Figure, along with a robotic version built in our lab to verify theoretical simulations (shown in Figure 2). We will briey recall the description of the snakeboard as developed in []. As a mechanical system the snakeboard has a conguration manifold given by Q = SE(2) S S S. Here SE(2) is the group of rigid motions in the plane, and is to be thought of as describing the position of the board with respect to some inertial reference frame. By S we mean the unit circle in R 2. As coordinates for Q we shall use (x; y; ; ; b ; f ) where (x; y; ) describes the position of the board with respect to a reference frame, is the angle of the rotor with respect to the board, and b and f are, respectively, the angles of the back and front wheels with respect to the board. Notice that the conguration space easily splits into a trivial ber bundle structure, with q = (g; r) given by g = (x; y; ) 2 G = SE(2) and r = ( ; b ; f ) 2 M = S S S. The left action for a group element, h = (a ; a 2 ; ) 2 G, is given by the map: h (x; y; ; ; b ; f ) = (x cos? y sin + a ; x sin + y cos + a 2 ; + ; ; b ; f ); and the lifted action takes the form: T q h (x; y; ; ; b ; f ) = (x cos? y sin ; x sin + y cos ; ; ; b ; f ):

5 Parameters for the problem are: m J J r J w l : the mass of the board, : the inertia of the board, : the inertia of the rotor, : the inertia of the wheels (assumed to be the same), and : the length from the board's center of mass to the wheels. For the snakeboard, the unconstrained Lagrangian is given simply by kinetic energy terms as L = 2 m( _x2 + _y 2 ) + 2 J _ J r( _ + _ ) J w? ( _ b + _ ) 2 + ( _ f + _ ) 2 ; The control torques are assumed to be applied to the rotation of the wheels and the rotor, and the wheels of the snakeboard are assumed to roll without lateral sliding. At the back wheels, the nonholonomic constraint assumes the form Similarly at the front wheels the constraint appears as? sin( b + ) _x + cos( b + ) _y? l cos( b ) _ = : (9)? sin( f + ) _x + cos( f + ) _y + l cos( f ) _ = : () For our formulation, we will write the constraints as the kernel of two dierential one-forms on Q. To be specic, all velocities must lie in kerf! ;! 2 g, where! =? sin( b + )dx + cos( b + )dy? l cos( b )d! 2 =? sin( f + )dx + cos( f + )dy + l cos( f )d: () A quick set of calculations shows that both the Lagrangian and the constraint one-forms are invariant with respect to the lifted group action. The momentum is dened along directions which are tangent to the ber and lie in the constraint distribution (i.e, satisfy the constraints). Thus, we dene the constrained ber distribution for this problem to be the one-dimensional subspace, where S q g a =?l[cos b cos( f + ) + cos f cos( b + )] b =?l[cos b sin( f + ) + cos f sin( b + )] c = sin( b? f ): All vectors in S q are tangent to the group G and satisfy the constraints in Eqs. 2,. The momentum, p, dened by Bloch et al. [] will then also be one-dimensional. Let hh; ii denote the inner product dened by the kinetic energy metric for our mechanical system. Then p = hh _q; X(q)ii = hh( _x; _y; _ ; _ ; _ b ; _ f ); (a; b; c; ; ; )ii = ma _x + mb _y + ^Jc _ + Jr c _ + J w c( _ b + _ f ); where X(q) 2 S q and ^J = J + Jr + 2J w is the sum of the moments of inertia. Before continuing with the derivation of the generalized momentum equation, we rst would like to make two simplifying assumptions which will greatly reduce the complexity of the derivations to follow. First, we will assume that the wheels are controlled to move out of phase with each other, in opposite directions. In other words, let = b =? f. Second, along the lines of Bloch et al., we will assume that ^J = J + J r + 2J w = ml 2. This will signicantly simplify the analysis to follow, without overly constraining the example.

6 Then, writing the equations as in the form of Eqs. 8: we nd that for _r = ( _ ; _ ), A = g? _g =?A(r) _r + I? J? r 2ml The generalized momentum equation is _p = 2 _rt _r _r _r + p T p _r _r + 2 pt pp p r = u; sin 2 J r 2ml sin 2 2 A and I? = _p = 2J r cos 2? tan _ p: Finally, we can write the dynamics on the base space 2l 2ml 2 A : tan (? J r ml 2 sin2 ) = J r 2ml sin 2? _ p + 2 2ml 2 J r = ; 2J w which is easily seen to be controllable, and can be rewritten as = u and = u : Let z = (x; y; ; p; ; ; _ ; _ ) 2 N, and then we can write the snakeboard equations in the form of Eqs. 8: A. Denitions f = B@ cos (?p+ _ J r sin 2) 2ml sin (?p+ _ J r sin 2) 2ml?2 _ J r sin 2 +p tan 2ml 2 2 J r cos 2? p _ tan CA ; h = B@ CA ; and h = Here we give notions of accessibility and controllability. Let R V (z ; T ) denote the set of reachable points in N from z at time T >, using admissible controls, u(t), and such that the trajectories remain in the neighborhood V of z for all t T. Furthermore, let R V T (z ) = [ tt R V (z ; t) be the set of all reachable points from z within time T. These two denitions lead us naturally to dene the following: Denition 2. [5] The system given by Eq. 7 is locally accessible if for all z 2 N, R V T (z) contains a non-empty open set of N for all neighborhoods V of z and all T >. Denition 3. [9] The system given by Eq. 7 is called small-time locally controllable (STLC) if for any neighborhood V, time T > and z 2 N, z is an interior point of R V T (z) for all T >. For driftless systems, local accessibility and local controllability are equivalent. Notice, however, that the general types of systems in which we are interested will require the presence of a drift vector eld, since this is how the momenta enter into the dynamic equations (notice the I? (r)p term in Eqs. 4{6). For an excellent example illustrating the distinction between accessibility and controllability for systems with drift, the reader is referred to Example 3.4 of [5]. B@ CA :

7 B. Accessibility For general systems of the form: _z = f(z) + h i (z)u i ; z 2 N a standard method for computing accessibility is to begin with the accessibility distribution. To do so, we dene a sequence of distributions. Let = spanff; h ; : : : ; h m g; spanning over C functions of N, and iteratively dene k = k? + spanf[x; Y ] j X; Y 2 k? g: This is a nondecreasing sequence of distributions on N, and so will terminate at some k inf, under certain regularity conditions. We will call kinf the accessibility distribution, C = kinf = : A standard result from nonlinear control theory, known as the Lie algebra rank condition (LARC), states that the system will be globally (and hence locally) accessible if C = T N. C. The kinematic case We review here results given by Kelly and Murray [5] for what is commonly called the purely kinematic case (sometimes called the \Chaplygin" case). This refers to the case in which the constraints alone fully specify the dynamics in the ber variables, g. The conditions they give for controllability will be useful in the present context for checking accessibility and controllability of the fully dynamic case, where conservation laws will arise in the presence of Lie group symmetries. In the kinematic case, however, momenta arising from symmetries are annihilated by the nonholonomic constraints. Therefore, the momentum terms, p, do not exist, and the equations of motion become g? _g =?A(r) _r; _r = u: (2) The connection, A(r) will in the kinematic case determine the motion in the full conguration space, given the motion in the controlled variables. Notice also that for the purposes of establishing controllability, Eq. 2 can be viewed as a special case of the general result for control of nonholonomic systems where the unconstrained degrees of freedom are controllable given by Theorem 5 in [3]. In their paper, Kelly and Murray established two important results that will be used in the sequel. First, they observe that by taking the appropriate derivatives, the analysis can be performed on the Lie algebra, i.e., at g = e. Furthermore, similar to [2] for systems with internal constraints, they show that the controllability of a kinematic system can be determined solely from the connection, A, its curvature, and higher derivatives. To see what is meant by this, dene h = spanfa(x) : X 2 T r Mg h 2 = spanfda(x; Y ) : X; Y 2 T r Mg h 3 = spanfl Z DA(X; Y )? [A(Z); DA(X; Y )]; (3). [DA(X; Y ); DA(W; Z)] : W; X; Y; Z 2 T r Mg h k = spanfl X? [A(Z); ]; [; ] : X 2 T r M; 2 h k? ; 2 h 2 h k? g: Notice that in the above equations, the connection has been placed in the appropriate mathematical context as a Lie algebra valued one-form on M. Thus, derivatives of A will take their values in g when evaluated along the appropriate vector elds on M. We point out that the curvature of the connection is dened with respect to the structure equations as DA(X; Y ) = da(x; Y ) + [A(X); A(Y )];

8 (x,y) θ φ 2 φ w ρ Fig. 3: Two wheeled planar mobile robot. where [A(X); A(Y )] is the Lie bracket on g and d represents exterior dierentiation. Next recall that for driftless systems local controllability and local accessibility are equivalent, so that the results given below in terms of the accessibility distribution will apply to controllability for systems with purely kinematic constraints. Kelly and Murray dene two types of local controllability, adapted for problems of locomotion. Fiber controllability implies that we can use control inputs to move to any position in the ber, but without regards to the intermediate or nal conditions of the controlled variables. On the other hand, total controllability is a slightly stronger condition, basically equivalent to STLC, which includes the ability to fully specify the motion of the controlled variables. Proposition 4. [5] The system given by Eqs. 2 is locally ber controllable at q 2 Q if and only if and is locally totally controllable if and only if g = h h 2 h 3 ; g = h 2 h 3 : The subspaces, h k g, will be used below to give sucient conditions for local accessibility (and later controllability) of the dynamic system given by Eqs. 8. In order to illustrate the above denitions (and to make clearer the distinction between ber and total controllability), we include the example of the two wheeled mobile robot, presented by Kelly and Murray in [5]. Example 2. Consider the two wheeled planar mobile robot shown in Figure 3. The robot's position, (x; y; ) 2 SE(2), is measured via a frame located at the center of the wheel base. The position of the wheels is measured relative to vertical and is denoted ( ; 2 ). Similar to the snakeboard, each wheel is controlled independently and is assumed to roll without slipping. The conguration space is then Q = G M = SE(2) (S S ). The constraints dening the no-slip condition can be written as in Eq. 2: _x cos + _y sin? 2 ( _ + _ 2 ) =? _x sin + _y cos = (4) _? 2w ( _? _ 2 ) = : Assuming the base space to be controllable (this is easily shown), we can rewrite the constraints in terms of Eq. 2, with the connection given by A(r) 2 2w 2? 2w A : (5) The connection is used to dene h for the controllability calculations. Noting this, we nd that h = spanf(; ; w )T ; (; ;? w )T g: A quick calculation of the curvature, DA, applied to certain vectors, shows that (; ; ) T 2 h 2 :

9 Thus, the two-wheeled mobile robot is ber controllable, since h + h 2 = g. However, Kelly and Murray show in [5] that the higher order derivatives of A(r) will never lead to terms with nonzero elements in the third slot (i.e., terms like (; ; )), and so the mobile robot in this example is not totally controllable (since h 2 + h 3 + 6= g). This surprising result is related to the geometric relationship between the two wheels, and the paths which they must follow. III. Local accessibility We begin this section by examining a few of the lower order brackets in the accessibility distribution, C, which play an important role in the accessibility and controllability analyses to follow. Notice that we have chosen the control vector elds in such a manner that they are mutually orthogonal, and such that [h i ; h j ] = ; 8 i; j 2 f; : : : ; mg: The remaining rst order brackets (those in ) will be of the form of a control vector eld bracketed with the drift vector eld. A quick calculation shows that i := [f; h i ] = B@?A i (r) ( _r _r ) ij _r j + ( p _r ) j i p j e i The reader at this point should notice the similarity of this set of vector elds with those for the kinematic case. If one looks at only the variables which would appear in the kinematic case, i.e., the ber and base directions, then the two are identical. A loose mathematical interpretation of this similarity is that the brackets, i are equivalent to \integrating" the input torque controls, converting them to something approximating velocity controls (used in the kinematic case). Moving to the second order brackets, an interesting thing happens when we bracket h i with j : ij := [h i ; j ] = [h i ; [f; h j ] ] = B@ CA : ( _r _r ) ij Thus, the _r _r term, which is a cross-coupling term for the base variables that drives the momenta, directly aects the momentum variables via the ij brackets. Viewing this coupling as a map, _r _r : T M T M! R p, then _r _r being onto implies that all of the momentum directions can be generated via this second order bracket. This mapping will be quite useful for a variety of reasons, as detailed below. Proposition 5. Assume that _r _r is onto and that g = h 2 + h 3 + ; where the h k 's are dened as above using the local form of the connection in Eqs. 8. Then the system given by Eqs. 8 is locally accessible. Proof. To show accessibility, we will need to show that the distribution, spans T N at each point z. The assumption on _r _r implies that the bracket given by [h i ; [f; h j ] ] will span the momentum directions, so it remains only to show that contains the ber and base directions. To do this, we begin with. It will contain vectors of the form, h i = B@ i CA and i = B@?A i (r) CA : ( _r _r ) ij _r j + ( p _r ) j i p j e i Thus, the base directions (velocity and acceleration vectors on M) will be contained in. Next, we examine 2. It will contain the vectors, i, and also vectors of the form, ij = B@ ( _r _r ) ij CA : CA :

10 Thus, for each z 2 N we can cancel o the terms in i which act in the momentum direction, and so dene a new set of vector elds to operate on: Using these vector elds, we dene ~ i = B@?A i(r) e i CA : ~ 2 := spanff; h i ; ij ; ~ i g: and the subsequent distributions, ~ k, similar to before. Then ~. As in the kinematic case, higher order bracketing of ~ i and ~ j will lead to higher order derivatives of the connection, A(r). By the assumption on A(r) and its derivatives, this implies that ~ = T z N, for each z 2 N, and so the result follows since = T z N. IV. Local Controllability Unfortunately, for nonlinear systems with drift, local accessibility may be quite dierent from local controllability. In order to provide a result for controllability, we will need to show that certain of the higher order brackets will either vanish or that they can be written as a linear combination of lower order brackets. This result is due to Sussman [9], and is the strongest statement of general local controllability that the authors are currently aware of for nonlinear control systems with drift. For reasons of clarity and brevity, the treatment here will not be completely mathematically rigorous. For details on how this can be done more rigorously, please refer to [3, 9]). Let h := f so that = spanfh = f; h ; : : : ; h m g. Dene the degree of a bracket with respect to h i, denoted i (X), to be the number of times that h i appears in a given set of brackets, X. As alluded to above, this can be made rigorous using indeterminates instead of vector elds, but basically will be taken to imply that we can determine the order of each of the brackets that form the basis for the distributions, i, by counting the number of times a particular vector eld would appear in the bracket. For example, suppose that m = 2. Then the degrees for X = [f; [h ; h 2 ]; [f; h ]]; Y = [h ; h 2 ] will be (X) = 2; (X) = 2; 2 (X) = and (Y ) = ; (Y ) = ; 2 (Y ) =. Similar to Sussman, dene the degree,, of a bracket X to be = mx i= i (X): (6) (This is the equivalent to the -degree dened in [9], with set to.) Then we have the following theorem due to Sussman: Theorem 6. [9] Given the system of Eq. 7, with h (z ) = f(z ) = at an equilibrium point z 2 N, assume that (h ; : : : ; h m ) satisfy the LARC at z. Further, assume that whenever X is a bracket for which (X) is odd and (X); : : : ; m (X) are all even, then there exist brackets Y ; : : : ; Y k such that for some ; : : : ; k 2 R, and Then the system dened by Eq. 7 is STLC from z. X = i Y i ; (Y i ) < (X); for i = ; : : : ; m: In other words, we will dene a \bad" bracket to be those for which the drift term appears an odd number of times and for which the control vector elds each appear an even number of times (including zero times). The requirement for small-time local controllability, then, will be that all \bad" brackets can be written in terms of brackets of lower degree.

11 Proposition 7. Assume that the system dened by Eqs. 8 is locally accessible, that the map _r _r is onto, and that ( _r _r ) ii for i = ; : : : ; m (no summation over i). Then this system is small-time locally controllable (STLC) from all equilibrium points, z 2 N. Proof. (sketch) As mentioned above, we will not provide here a fully rigorous proof, but instead give enough details so that the necessary extensions should be obvious. In order to show controllability, we begin by demonstrating that all \bad" brackets as dened by Sussman will be either zero or expressible in terms of lower order \good" brackets (in fact, of order 3). This, along with the assumption that the LARC is satised (using the results from the kinematic case), will give the result via Theorem 6. First, we will restrict our attention to the point z = (; ; ; ) 2 G R p M T r M. It is easy to show that the result will hold for all equilibrium points, z 2 N (of the form z = (g; ; r; )), by translating Eqs. 8 appropriately. Also notice that f(z ) =, satisfying the rst requirement of Theorem 6. Next, recall the denition of the degree of a bracket, Eq. 6, and notice P two important facts that will be true of any bad bracket, X: ) (X) must be odd, and 2) m (X) 6= i= i (X). These are made true by virtue of there being exactly one odd term in the summation of Eq. 6. The rst condition implies that all even order brackets are necessarily \good" brackets, while the second condition implies that the quantity (X) := (X)? mx i= i (X) is always odd, and thus never zero. More specic to the system of Eqs. 8, let O(k) denote a function in (z; _z) which is a homogeneous polynomial of order k in ( _r; p). Thus, f(z) = (O(); O(2); O(); ). A straightforward set of calculations shows that for any bracket which involves the drift vector eld, f, bracketing by f will increase the order of these functions by, and that bracketing by any of the h i 's will decrease the order of the bracket by. Thus, we will nd that for any bad bracket, X, (which by denition must contain at least one f in the bracket), it will be of the form X = (O((X)); O((X) + ); O((X)); ), or will be identically zero, e.g., any bracket involving [h ; h 2 ]. Viewed in this form, it is easy to see that for all bad brackets such that (X) 6=?, X(z ) =. Thus, the only bad bracket that we need worry about are those with (X) =?, for which X = (; O(); ; ). But we have already assumed that the map _r _r is onto, and is provided by a bracket of degree 3. This, along with our assumption that ( _r _r ) ii, implies that any bad bracket which is not zero at z can be rewritten in terms of brackets of the form [h i ; [f; h j ] ], where i 6= j. Example : (cont'd) We return to the snakeboard example to investigate controllability. Obviously, the bracket of the control inputs, [h ; h ], is identically zero. The only other rst order brackets are those mixing the drift vector eld with the control inputs: and = [f; h ] =?Jr sin 2 2ml cos ;?J r sin 2 2ml sin ; J r sin 2 ml 2 ;?2 _ Jr cos 2 ;?; ; ; = [f; h ] =? ; ; ;?2 _ Jr cos 2 + p tan ; ;?; ; T : Notice that these vector elds have \-'s" in the appropriate velocity directions. As mentioned above, this loosely corresponds to integrating the control torques to velocity torques. Notice that this will also encode the information given by the connection, A(r), since the connection relates input velocities to ber velocities. The vector elds above imply control of the base (controlled dynamics). In order to show accessibility and controllability (STLC), one of the rst criteria to be satised is the conditions on _r _r, given by the following third order brackets. First, we need the diagonal elements of _r _r to be zero. This is given by = = : Then, we look at o diagonal terms to show that _r _r is onto (and hence that the momentum direction is contained in the accessibility distribution). To see this, we simply give the necessary bracket: which is nonzero for all 6= 2. = [ ; h ] =? ; ; ; 2J r cos 2 ; ; ; ; T ; We have allowed (X) to be negative, and so dene O(k) = for all k <. T

12 Finally, to demonstrate that the snakeboard is controllable, we need show that g = h 2 + h 3 + : : :, using the connection, A(r). We begin by computing [ ; ], which gives us the curvature of the connection, DA. This yields terms of the form: Then, [ ; [ ; ] ] yields? Jr ml cos 2; ;? J r ml 2 sin 2 T 2 h2 : and [ ; [ ; [ ; [ ; ] ] ] ] gives?? 2J r ml sin 2; ;? 2J r ml 2 cos 2 T 2 h3 ; ; 2J r 2 m 2 l 3 cos 2; T 2 h5 : Thus, g = h 2 + h 3 + h 5, and the conditions for Proposition 7 are satised. Remarks: The reader should note that while the condition that ( _r _r ) ii in Proposition 7 may appear slightly articial, it is required in order to satisfy Sussman's criterion for controllability. In fact, research by Lewis and Murray [9] suggest that similar conditions may be needed for general mechanical systems. They study accessibility and controllability for unconstrained mechanical systems, and report similar conditions on these third order brackets of the type [h i ; [f; h i ] ]. In their case, these brackets may be nonzero if they are contained in the control input vector eld; however, it is not dicult to show that for our purposes these brackets must be identically zero. One of the main tenets driving this research is that the process of locomotion can be described as a coupling of internal shape changes creating net external motion and that this process can be modeled using a mathematical connection on a principal ber bundle. Naturally, there arises the question of what role the connection and its derivatives really do play in describing the actual motion of the system. In particular, what is the relationship between the connection and its derivatives and locomotive gaits? Although the results at present are only qualitative, it should be pointed out that for each of the snakeboard brackets dening elements in h k above, there directly correspond gaits (integrally related sinusoidal input patterns) generated by the snakeboard (see [] for more details). For example, the bracket [ ; [ ; [ ; [ ; ] ] ] ] given above (with appearing 3 times and appearing 2 times) produces a Lie algebra element pointing transversely to the direction of the board. This corresponds directly to the transverse motion of the 3 : 2 gait (in which the rotor oscillates sinusoidally 3 cycles for every 2 cycles of the wheels) found in the snakeboard. Leonard and Krishnaprasad have used averaging theory to study the relationship between Lie brackets and periodic inputs for systems on Lie groups without drift [8]. However, additional research needs to be conducted before making any more concrete statements regarding the relationship between Lie brackets and sinusoidal gaits for these systems which include drift. V. Conclusion This paper establishes easily computable controllability results for systems on principal ber bundles with external nonholonomic constraints. These types of systems are characterized by the existence of a connection, which relates the motion of the system to motion in its control inputs. The connection has been discussed in its direct implications for accessibility and controllability. Further, these types of systems will very often include drift vector elds, in the form of momentum terms. This added complexity makes the analysis slightly more dicult, but gives the additional possible benet of increased control over the system dynamics. Research has shown that many problems of locomotion can be formulated in terms of this dynamical structure, and so motivate further analysis of the controllability of these types of systems. Future work will be concerned with further exploiting the geometric structure of the problem, and in developing means for establishing results governing trajectory generation and optimal control of locomotive gaits.

13 Acknowledgments The authors would like to acknowledge Scott Kelly, Andrew Lewis, and Richard Murray for their help in proof-reading various drafts of this paper and for providing helpful insights into the controllability analysis. Also, we would like to thank Jerry Marsden for his comments and encouragement throughout the development of this material. References [] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden, and R. M. Murray. Nonholonomic mechanical systems and symmetry. Submitted to the archive for rational mechanics and analysis. Available electronically via California Institute of Technology, Pasadena, CA, 994. [2] A. M. Bloch, N. H. McClamroch, and M. Reyhanoglu. Control and stabilization of nonholonomic Chaplygin dynamic systems. In Proc. IEEE Conf. Decision Contr., pages 27{32, Brighton, U.K., 99. [3] A. M. Bloch, M. Reyhanoglu, and N. H. McClamroch. Control and stabilization of nonholonomic dynamic systems. IEEE Transactions on Automatic Control, 37():746{757, 992. [4] R. W. Brockett. Control theory and singular Riemannian geometry. In P. J. Hilton and G. S. Young, editors, New Directions in Applied Mathematics. Springer-Verlag, New York-Heidelberg-Berlin, 982. [5] S. D. Kelly and R. M. Murray. Geometric phases and locomotion. Available electronically via To appear, J. Robotic Sys., CIT, Pasadena, CA, 994. [6] P. S. Krishnaprasad. Geometric phases and optimal reconguration for multibody systems. In Proc. of the 99 American Control Conference, pages 244{2444, Philadelphia, 99. American Automatic Control Council. [7] P. S. Krishnaprasad and D. P. Tsakiris. G-snakes: Nonholonomic kinematic chains on Lie groups. In Proc. for the 33 rd IEEE Conference on Decision and Control, Lake Buena Vista, FL, December 994. [8] N. E. Leonard. Averaging and Motion Control of Systems on Lie Groups. PhD thesis, University of Maryland, 994. [9] A. D. Lewis and R. M. Murray. Equilibrium controllability of a class of mechanical systems. Submitted to the 34th IEEE Conference on Decision and Control, 995. Available electronically via [] A. D. Lewis, J. P. Ostrowski, R. M. Murray, and J. W. Burdick. Nonholonomic mechanics and locomotion: the snakeboard example. In Proc. IEEE Int. Conf. Robotics and Automation, pages 239{2397, San Diego, CA, May 994. [] R. Montgomery. Isoholonomic problems and some applications. Communications in Mathematical Physics, 28(3):565{592, 99. [2] R. Montgomery. Nonholonomic control and guage theory. In Z. Li and J. F. Canny, editors, Nonholonomic Motion Planning, pages 343{378. Kluwer, 993. [3] R. M. Murray and S. S. Sastry. Nonholonomic motion planning: Steering using sinusoids. IEEE Transactions on Automatic Control, 38(5):7{76, 993. [4] Y. Nakamura and R. Mukherjee. Exploiting nonholonomic redundancy of free-ying space robots. IEEE Transactions on Robotics and Automation, 9:4:499{56, August 993. [5] H. Nijmeijer and A. J. van der Schaft. Nonlinear Dynamical Control Systems. Springer-Verlag, New York-Heidelberg-Berlin, 99. [6] J. P. Ostrowski, J. W. Burdick, A. D. Lewis, and R. M. Murray. The mechanics of undulatory locomotion: The mixed kinematic and dynamic case. In Proc. IEEE Int. Conf. Robotics and Automation, Nagoya, Japan, May 995. [7] J. P. Ostrowski, A. D. Lewis, R. M. Murray, and J. W. Burdick. Reduction of mechanical systems with constraints, 995. In preparation. [8] A. Shapere and F. Wilczek. Geometry of self-propulsion at low Reynolds number. J. Fluid Mech., 98:557{585, 989. [9] H. J. Sussman. A general theorem on local controllability. SIAM Journal on Control and Optimization, 25::58{94, Jan 987.