HIERARCHICAL TRAJECTORY GENERATION FOR A CLASS OF NONLINEAR SYSTEMS. 1. Introduction

Transcription

1 HIERARCHICAL TRAJECTORY GENERATION FOR A CLASS OF NONLINEAR SYSTEMS PAULO TABUADA AND GEORGE J PAPPAS Abstract Trajectory generation and motion planning for nonlinear control systems is an important and difficult problem In this paper, we provide a constructive method for hierarchical trajectory generation and hierarchical motion planning The approach is based on the recent notion of φ-related control systems Given a control affine system satisfying certain assumptions, we project the trajectory planning problem onto a φ-related control system of smaller dimension Trajectories designed for the smaller, abstracted system are guaranteed, by construction, to be feasible for the original system Constructive procedures are provided for refining trajectories from the coarser system to the more detailed system Introduction Research in motion planning and trajectory generation for classes of nonlinear control systems has resulted in various motion planning approaches for nonholonomic systems [8] as well as real-time trajectory generation methods [25] for differentially flat systems [7] The rapidly growing interest in unmanned aerial vehicles (UAVs has also emphasized the need to generate aggressive trajectories for individual UAVs ([9, ] as well as large numbers of autonomous UAVs ([2] Generating trajectories for complex nonlinear systems as well as generating trajectories for large numbers of interconnected nonlinear (UAV systems naturally guide us towards a hierarchical approach to motion planning and trajectory generation In this paper, we present such a hierarchical approach to motion planing and trajectory generation for nonlinear systems The proposed methodology builds upon the notion of φ- related systems, which has been introduced in [2] Given a control system Σ with state space X, and a map φ : X X 2, a φ-related system is an abstracted control system Σ 2 on the smaller state space X 2, that captures the φ-image of all Σ trajectories A construction is provided in [2] which given nonlinear model Σ and map φ, generates the abstracted model Σ 2 Furthermore, given control theoretic properties such as controllability and stabilizability, we can obtain natural conditions on the map φ in order for Σ and Σ 2 to have equivalent properties These include controllability for linear [2], nonlinear [2], and Hamiltonian systems [24] and stabilizability of linear systems [9] This paper presents an approach to the following problem: Given a trajectory of the abstracted model Σ 2, refine this trajectory to a trajectory of the original model Σ A solution to the above problem provides an hierarchical approach to motion planning, since we can transfer motion planning problems from Σ to Σ 2, solve the motion planning problem on the simpler model Σ 2 using any existing method, and then refine the trajectory back to Σ The explicit construction of this approach along with conditions that guarantee feasibility is the main contribution of this paper The solution critically relies on our understanding of how state/input trajectories of the detailed system relate to the state/input trajectories of its abstraction The idea of reducing the synthesis of control systems to simpler, lower dimensional systems has appeared in various forms in the literature For mechanical systems, one such approach is based on the existence of symmetries, which allows to reduce a given control system to a simpler quotient system [6, 5] Recently, a different approach has been reported in [4, 3], where kinematic models of mechanical systems (kinematic reductions generating trajectories refinable to trajectories of the full dynamical model are introduced In This research is partially supported by ARO MURI DAAD

2 2 PAULO TABUADA AND GEORGE J PAPPAS the same spirit, the so-called inclusion principle [23] allows to carry analysis and design of systems to simpler models Trajectory morphing [2] is a homotopy based approach that is, in spirit, hierarchical The related problem of characterizing regularity of the original system input trajectories from regularity of the map φ and the abstracted system input trajectories is discussed in [] Backstepping has been a very successful approach for the recursive (or hierarchical design of stabilizing controllers for nonlinear systems [22] and was a source of inspiration for the results presented in this paper However, the focus of this paper is motion planning and not controller design Our results systematically lead to a formal methodology that can be thought of as open-loop backstepping In Section 5, we will see how the class of nonlinear systems being addressed coincides with strict feedback systems in the single input case, while in Section 6, we outline an extention of the proposed results to a larger class of nonlinear systems, even for single input systems Such extension will follow from the observation that strict feedback systems are affine in the controlled variables and therefore nilpotent systems This observation immediately allows to generalize our results to nilpotent, but not necessarily affine systems Other uses of nilpotency in systems and control theory include [3, 4, 6] A different approach which bears some connections with the proposed approach is flatness [7] Flatness can also be used for hierarchical motion planning, since trajectories of the flat output space completely define trajectories for the original system Our approach differs from flatness based approaches in that not every trajectory of the abstraction can be lifted to a trajectory of the original system In addition, it is also not the case that trajectories of the abstraction uniquely define state/input trajectories of the original system as is the case for flat systems On the other hand, these relaxations allow to refine trajectories for not necessarily flat systems The structure of this paper is the following In Section 2 we introduce some notation, review the notion of φ-related control systems and the canonical construction of such control systems originally introduced in [2] for nonlinear systems In Section 3 we focus on a class of nonlinear systems for which explicit and constructive solutions for trajectory refinement are given These results are illustrated by an example in Section 4 and specializations of the main result are given for several classes of systems in Section 5 Extensions of the presented results to a larger class of systems is given in Section 6 while some conclusions finalize the paper in Section 7 2 φ-related Control Systems In this section, we review the notion of φ-related control systems introduced in [2] We will consider control systems defined on R m Following the notation of [], we say that a given object is smooth when it is infinitely differentiable Given a smooth map φ : R m R n, we say that φ is a submersion when its associated tangent map T x φ is surjective for every x R m Given two vector fields: a (x b (x a 2 (x b 2 (x X(x = a m (x Y (x = b m (x we will denote by: their Lie bracket defined by: c (x c 2 (x [X, Y ](x = c m (x

3 HIERARCHICAL TRAJECTORY GENERATION 3 c i (x = m j= ( bi (xa j (x a i (xb j (x x j x j while the notation X x i will be used to denote the vector field [ a a 2 x i x i x i ] T collections of vector fields, [S, S 2 ] will denote the set of vector fields defined as: a m When S and S 2 are [S, S 2 ] = X S,X 2 S 2 {[X, X 2 ]} In this paper, we shall consider control systems which are affine in the control inputs Definition 2 A control affine system Σ = (R m, R k, F consists of state space R m, input space R k, and system map F : R m R k R m of the form: F = X where X, Y,, Y k are smooth vector fields on R m Given control affine system Σ = (R m, R k, F, it is natural to associate with Σ the affine distribution (state dependent affine subspace of R m defined by: k i= u i A (x = X (x span{y (x,, Y k (x} Throughout the paper we will assume that dim span{y (x,, Y k (x} is independent of the point x This is a natural assumption since the set os points satisfying this assumption is an open and dense subset of R m In addition, we will also assume that the vector fields Y (x,, Y k (x are linearly independent This assumption results in no loss of generality, since it can always be achieved through a preliminary feedback canceling the linearly dependent vector fields Trajectories of affine control systems are defined as follows: Definition 22 Let Σ = (R m, R k, F be a control affine system and I R an open interval containing the origin A smooth curve x : I R m is said to be a state trajectory if there exists a (not necessarily smooth input trajectory u : I R k satisfying the differential equation for almost all t I ẋ (t = F (x (t, u (t We are interested in relating the trajectories of two models, possibly of different dimension This is provided by the notion of φ-related control systems: Definition 23 (φ-related control systems [2] Let Σ = (R m, R k, F and Σ 2 = (R n, R l, F 2 be control systems and let φ : R m R n be a smooth map Control system Σ 2 is said to be φ-related to control system Σ if for every x R m : (2 T x φ ( A (x A 2 (φ(x In the context of hierarchical trajectory generation we are interested in φ-related control systems where Σ 2 is lower dimensional than Σ, therefore m n The notion of φ-related control systems allows to relate the trajectories of the two control systems Theorem 24 ([2] Control system Σ 2 is φ-related to control system Σ if and only if for every trajectory x (t of Σ, φ(x (t is a trajectory of Σ 2

4 4 PAULO TABUADA AND GEORGE J PAPPAS Even though Σ 2 captures the φ-image of every trajectory of Σ, it may also generate trajectories that are not feasible for the Σ model The goal of this paper is to reverse the direction of the above theorem, and hence refine trajectories of the coarser model Σ 2 to trajectories of the more detailed model Σ This frequently occurs when, for example, trajectories of kinematic models must be refined to trajectories of dynamic models In particular, in this paper, we shall address the following two problems Problem 25 (Hierarchical Trajectory Generation Let Σ 2 be a control system that is φ-related to control system Σ Given a state trajectory x 2 (t of Σ 2 corresponding to smooth input trajectory u 2 (t, construct an input trajectory u (t for Σ such that the resulting state trajectory x (t satisfies the relation φ(x (t = x 2 (t Problem 26 (Hierarchical Motion Planning Let Σ 2 be a control system that is φ-related to control system Σ Consider desired initial and final states x, x F R m for system Σ Given a state trajectory x 2 (t of Σ 2 satisfying x 2 ( = φ(x and x 2 (T = φ(x F for given time T R, construct an input trajectory u (t for Σ such that the resulting state trajectory x (t satisfies φ(x (t = x 2 (t, x ( = x and x (T = x F Even if Σ 2 is φ-related to Σ, Σ 2 may generate trajectories that not feasible for Σ Hence, in addition to φ-relatedness, additional conditions will be required to solve Problems 25 and 26 We start by reviewing the construction of φ-related control affine systems introduced in [2] Definition 27 (Canonical construction [2] Let Σ = (R m, R k, F be a control affine system, A = X the affine distribution associated with Σ and let φ : R m R n be a smooth surjective submersion The following distributions: K = ker(t φ = {X : R m R m T φ(x = } L = [K, X ] = L [K, L ] [K, [K, L ]] A = X allow to introduce the affine distribution on R n defined by: A 2 (y = T x φ(a (x for any x R m satisfying φ(x = y Control system Σ 2 = (R n, R l, F 2 defined by: l F 2 = X 2 j= Y j 2 uj 2 with X 2 (y = T x φ(x (x, T x φ( 2 (x = span{y 2 (y, Y 2 2 (y,, Y l 2 (y} is said to be φ-related to Σ Control system Σ 2 obtained through the canonical construction is not only φ-related to Σ as it is also the smallest φ-related affine control system: Theorem 28 (Adapted from [2] Let Σ = (R m, R k, F be a control affine system and φ : R m R n a smooth surjective submersion Control system Σ 2 = (R n, R l, F 2 obtained through the canonical construction described in Definition 27 is the smallest (in the sense of set inclusion control affine system φ-related to Σ We now illustrate the construction of φ-related systems through a simple example Consider the following control system: (22 and the the surjective submersion: [ y ẋ = x x 2 x 3 ẋ 2 = x 2 2x 3 x 2 u ẋ 3 = x x 3 x 3 u ] [ ] x2 = φ(x y, x 2, x 3 = 2 x 3

5 HIERARCHICAL TRAJECTORY GENERATION 5 Following the canonical construction we compute the following objects: [ ] T x φ = K = span{k} = span{ } [K, X ] = x 2x 3 x 3 L = [K, X ] = span{y, [K, X ]} = span{ x 2, x 2x 3 } x 3 x 3 [K, L ] = {} since [K, Y ] = and [K, [K, X ]] = = L [K, L ] [K, [K, L ]] = L = span{y, [K, X ]} Finally we obtain the affine distribution A 2 as: { A 2 (y = T x φ(x span T x φ ( Y (x, T x φ ( [K, X ](x } [ ] [ ] [ x 2 = 2 x 3 x2 span{, } x x 3 x x3] [ ] [ ] [ y 2 = y 2 y span{, } y2] where we have chosen for any (y, y 2 R 2, x = (, x 2, x 3 which satisfies φ(x = (y, y 2 The resulting control system is then given by: (23 ẏ = y 2 y 2 y u 2 ẏ 2 = y 2 u 2 2 We thus see that while input u 2 in (23 corresponds to input u in (22 since T φ(y u = T φ(y u = Y 2 u, input u 2 2 does not correspond to an input of (22 However, u 2 2 can be identified with the state x of (22 that has been abstracted away To fully exploit the relation between the states/inputs of the abstraction and the state/inputs of the original system we restrict our attention to a particular class of nonlinear systems and maps φ in the next section 3 Hierarchical trajectory generation For general control systems the relationships between state/inputs of the original and abstracted system can be very complex As these relations are crucial for hierarchical trajectory generation we will focus on a particular class of nonlinear systems more amenable to analysis and design This class of systems is characterized by the following assumptions that will hold throughout the paper: AI: Control system Σ 2 is φ-related to control system Σ by the canonical projection φ = π : R m R n π (x,, x n, x n,, x m = (x,, x n, AII: ker(t φ = ker(t π, AIII: Control system Σ is of the form: F = X a i= u i k i=a v i

6 6 PAULO TABUADA AND GEORGE J PAPPAS where ker(t φ = ker(t π = span{y a, Y a2,, Y k }, AIV: Affine distribution A = X span{y,, Y a } satisfies: [[ A, ker(t φ ], ker(t φ ] ker(t φ Assumption AI will simplify the presentation of forthcoming results considerably as it allows to split the states of Σ as x = (x 2, x c R m, where π (x = x 2 R n and x c R m n There is no loss of generality is making assumption AI since any surjective submersion is locally a projection up to a change of coordinates Assumption AIII also results in no loss of generality since such decomposition of the vector fields Y i can always be achieved through preliminary feedback Assumptions AII and AIV are the critical conditions that will enable hierarchical trajectory generation and hierarchical motion planning Intuitively, assumption AII requires states projected out in the abstraction process to be directly controlled This will ensure the existence of control inputs to generate the desired refinements Assumption AIV is a partial nilpotency requirement between the controlled system, and the abstracted directions In Section 5, we shall present classes of systems for which assumptions AI to AIV hold, and in Section 6 we outline how to extend the presented results to higher order nilpotency assumptions, thus relaxing AIV We also note that AIV is a coordinate free characterization of the required relation between A and the map φ This means that even if we are not using the right coordinates in which AI holds, assumption AIV must still be satisfied In the coordinates prescribed by AI, assumption AIV takes the following form: (3 (32 π ( 2 X x p x q = for p, q {n, n 2,, m} ( 2 π = for p, q {n, n 2,, m}, i {, 2,, k} x p x q which simply states that the first n components of the vector fields X and are affine functions of the variables x n, x n2,, x m The canonical construction described in Definition 27 can now be simplified for the class of systems under consideration Proposition 3 Let Σ = (R m, R k, F be a control affine system satisfying assumptions AI to AIV with respect to the canonical projection π : R m R n Then, the canonical construction described in Definition 27 can be simplified as follows: ( ( X 2 (x 2 = T (x2,π X (x 2, ( = π X (x 2, ( (2 Y2 i (x 2 = T (x2,π Y i (x 2, ( = π Y i (x 2, ( ( for i =, 2,, a (3 Y aj 2 (x 2 = T X (x2,π x j = π X x x=(x2, j for j =, 2,, m n ( ( x=(x2, (4 j 2 (x Y i 2 = T (x2,π Y i x j = π x x=(x2, j for i =, 2,, a and j =, 2,, m n x=(x2, F 2 = X 2 a i= m n Y2 i u i 2 j= a,m n Y aj 2 v j 2 i=,j= j 2 wij 2 Proof The proof is a straightforward application of Definition 27, equalities (3 and (32 and the fact that [ x i, Z] = Z x i for any vector field Z

7 HIERARCHICAL TRAJECTORY GENERATION 7 Not only assumptions AI through AIV simplify the canonical construction, they also simplify the relationships between states and inputs of systems Σ and Σ 2 to the following: (33 π (x = x 2 u i = u i 2 i =,, a x j c = v j 2 j =,, m n Note the mixing of inputs and states in the last equation Inputs w ij 2 of Σ 2 do not correspond to a state or input in Σ, but capture abstracted Lie bracket information that will be important in the refinement process Equalities (33 are justified by the next result which provides a solution to Problem 25: Theorem 32 (Hierarchical Trajectory Generation Let Σ = (R m, R k, F be a control affine system satisfying assumptions AI through AIV with respect to the canonical projection π : R m R n, and let Σ 2 = (R n, R l, F 2 be the π -related control system obtained as described in Proposition 3 Given any smooth state trajectory x 2 (t of Σ 2 corresponding to a smooth input trajectory (u 2 (t, v 2 (t, w 2 (t satisfying w ij 2 (t = ui 2(tv j 2 (t i =,, a, j =,, m n there exists a trajectory x (t of Σ satisfying π (x (t = x 2 (t Furthermore, the state trajectory x (t of Σ is given by: (34 x (t = (x 2 (t, x c (t = (x 2 (t, v 2 (t and corresponds to input trajectory: (35 u (t u 2(t u a (t v a = u a 2(t α a (t α k (t where α(t = (α a (t,, α k (t is given by the solution of: (36 v 2 = π 2 (X π 2 ( k v k i=a α i with π 2 : R m ker(t π = R m n, the canonical projection mapping π 2 (x = π 2 (x 2, x c = x c Remark: Before proving this result we note that (36 can always be solved for α To see this, we construct the pointwise linear transformation represented by matrix Y = [Y a Y a2 Y k ] : R m n R m and note that π 2 Y : R n m ker(t π = R n m is an isomorphism This follows from the fact that the vector fields Y i for i = a, a 2,, k form a basis for ker(t π The input trajectories α(t can then be obtained by: α = (π 2 Y ( v 2 π 2 (X Although the vector π 2 (X and matrix π Y are state dependent, we can express x (t as a function of x 2 (t and v 2 (t in virtue of (34 thereby defining α as a function of time This process is exemplified in Section 4 The proof of Theorem 32 will require the following simple preliminary Lemma: Lemma 33 Let Σ 2 be a control system φ-related to a control system Σ and let x (t be a state trajectory for Σ corresponding to input trajectory u (t The state trajectory x 2 (t of Σ 2 corresponding to input trajectory u 2 (t satisfies φ x (t = x 2 (t if and only if (37 (38 φ x ( = x 2 ( T x(tφ F (x (t, u (t = F 2 (x 2 (t, u 2 (t

8 8 PAULO TABUADA AND GEORGE J PAPPAS Proof Assume that φ x (t = x 2 (t holds Then by setting t = we obtain (37 while by time differentiation we get (38 Conversely, assume that (38 holds Noting that (38 is equivalent to: T x(tφ ẋ (t = ẋ 2 (t d dt (φ x (t = d dt x 2(t we obtain that φ x (t = x 2 (t c for some constant vector c R n However, (37 implies that c = from which φ x (t = x 2 (t follows Proof of Theorem 32 The result follows from Lemma 33 provided that (37 and (38 hold We start by showing that (38 is satisfied Input trajectory u (t = (u 2 (t, α(t defines state trajectory x (t by: ẋ (t = X (x (t a i= = X (x 2 (t, a i= k i=a u i 2(t m n j= ( (x 2 (t, α i (t ( X k a α i (t x nj x=(x2(t, m n j=i ( Y i x nj (t x nj x=(x2(t, x nj (t u i 2(t where we have used the fact that any vector field Z(x = Z(x 2, x c satisfying assumption AIV can be written as: Z(x 2, x c = Z(x 2, m n j= ( Z x nj x=(x2, x nj Projecting on R n using T π results in: T x(tπ (ẋ (t = X 2 (x 2 (t m n j= a i= a,m n i=,j= Y j 2 (x 2(tx nj (t 2 u i 2(t j 2 (x 2(tx nj (tu i (t

9 HIERARCHICAL TRAJECTORY GENERATION 9 Since α(t satisfies (36, it follows that x nj (t = v j 2 (t Furthermore, by making use of the equality wij 2 (t = u i 2(tv j 2 (t we obtain: T x(tπ (ẋ (t = X 2 (x 2 (t m n j= a i= a,m n i=,j= Y j 2 (x 2(tv j 2 (t 2 u i 2(t j 2 (x 2(tw ij 2 (t = F 2 (x 2 (t, u 2 (t, v 2 (t, w 2 (t which shows that (38 is satisfied We now see that if the initial condition x ( satisfies (37, which is always possible, then the trajectory x (t corresponding to input trajectory (u 2 (t, α(t satisfies π (x (t = x 2 (t in virtue of Lemma 33 Theorem 3 provides a constructive procedure for refining state/input trajectories of Σ 2 to state/input trajectories of Σ In addition to trajectory refinement, Theorem 3 can also be used for hierarchical motion planning, thus leading to the solution of Problem 26 Suppose we wish to determine a trajectory of control system Σ connecting point x R m to point x F R m Then, Theorem 3 states that this reduces to finding a trajectory x 2 (t for Σ 2 joining π (x to π (x F However, the refinement process only ensures that trajectory x (t satisfies π (x (t = x 2 (t and additional assumptions are necessary to ensure that x (t links x to x F Such additional requirements are summarized in the next corollary of Theorem 32: Corollary 34 (Hierarchical Motion Planning Let Σ = (R m, R k, F be a control affine system satisfying assumptions AI through AIV with respect to the canonical projection π : R m R n, and let Σ 2 = (R n, R l, F 2 be the π -related control system obtained as described in Proposition 3 Consider any two states x = (x 2, x c and x F = (x F 2, x F c in R m and let x 2 (t be any smooth state trajectory of Σ 2 such that x 2 ( = x 2 and x 2 (T = x F 2 for some T R If x 2 (t corresponds to a smooth input trajectory (u 2 (t, v 2 (t, w 2 (t satisfying w ij 2 (t = ui 2(tv j 2 (t i =,, a, j =,, m n v 2 ( = x c v 2 (T = x F c then the state trajectory x (t of Σ described in Theorem 3 satisfies x ( = x and x (T = x F Proof The input trajectory u (t = (u 2 (t, α(t defined in Theorem 3 satisfies (36, which implies that x nj (t = v j 2 (t We thus have that x ( = (x 2 (, x c ( = (x 2, v 2 ( = (x 2, x c = x and similarly one shows that x (T = (x 2 (T, x c (T = (x F 2, v 2 (T = (x F 2, x F c = x F Theorem 3 and Corollary 34 reflect the natural tradeoff that arises in hierarchical methods: ignoring some Σ dynamics can be performed using this framework, as long as Σ 2 satisfies certain algebraic constraints For example, the algebraic input constraints w ij 2 (t = ui 2(tv j 2 (t at the level of Σ 2 accommodate certain Lie bracket conditions between ignored input vector fields of Σ Algebraic constraints are clearly much more desirable computationally than differential constraints 4 An illustrative example We now illustrate the results of the previous Section through an example Consider control system: F = X Y u Y 2 u 2

10 PAULO TABUADA AND GEORGE J PAPPAS with state (x, x 2, x 3 R 3, inputs u,u 2 R and vector fields defined by: (4 X = x ( x 2 x x 3 x (x x 2 x 2 x 2 (42 Y = x 3 Y 2 = x (x 2 x 3 Our goal is to construct a state trajectory joining the states: x x 2 x 3 (43 = 2 x F 2 = 4 /4 x F 3 /36 We proceed by choosing the projection map π to be π (x, x 2, x 3 = y = x, which satisfies: (44 ker(t π = span, = span { Y, Y 2 } Hence assumptions AI, AII and AIII are clearly satisfied Assumption AIV also holds as can be seen by computing the partial derivatives of vector fields (4 and (42 with respect to x 2 and x 3 (the ignored states We now follow the construction described in Proposition 3 to obtain system Σ 2 which is π -related to system Σ Steps 2 and 4 of the construction can be ignored since in this case we have a = due to equality (44 This leaves us with steps and 3 for the construction x F X 2 (x = [ ] X (x,, = [ ] x x 2 = x 3 Y2 a = Y2 = [ ] X x 2 = [ ] x = x x 2 (x,, Y2 a2 = Y2 2 = [ ] X x 3 = [ ] x2 x = x 2 (x,, This results in control system Σ 2 defined : with state y R and inputs v 2,v 2 2 R ẏ = F 2 (y, v 2, v 2 2 = y yv 2 y 2 v 2 2 We must now find a Σ 2 trajectory connecting y( = 2 = π (x and y(2 = 4 = π (x (hence T = 2 One such trajectory can be obtained by simply canceling the drift term through the feedback v 2 =,

11 HIERARCHICAL TRAJECTORY GENERATION v2 2 = /y 2 thus obtaining ẏ = The motion planning problem for Σ 2 is now trivial since if one starts at y( = 2 = π (x we have y(2 = 4 = π (x F Furthermore v2( = = v2(2 2 = x 2 ( = x 2 (2 and v2(t 2 = /((t 2 2 v2( 2 = /4 = x 3 ( and v2(2 2 = /36 = x 3 (2 which shows that the conditions of Corollary 34 are indeed satisfied We now refine the designed trajectory to a trajectory of Σ Since a =, the input trajectory for Σ is simply given by α, the solution of (36: [ ] [ ] [ ] [ ] v 2 x (x v 2 = x 2 x3 α 2 x 2 x 2 x (x 2 x 3 α 2 which can be solved for α,α 2 resulting in α = x (x x 2 x 3 α 2 = 2( x 2 x x 3 x 4 x 2 x 3 (x2 x 3 Expressing (x (t, x 2 (t, x 3 (t as functions of time using x (t = t2, v 2(t = x 2 (t = and v 2 2(t = x 3 (t = /x 2 (t = /(t 2 2 we obtain: α (t = (t 2 3 (t α 2 (t = (t (t 2 2 ((t 2 4 Corollary 34 now ensures that by using inputs u (t = α (t and u 2 (t = a 2 (t, and starting at (x (, x 2 (, x 3 ( = (x, x 2, x 3, the trajectory (x (t, x 2 (t, x 3 (t of Σ will satisfy (x (T, x 2 (T, x 3 (T = (x F, x F 2, x F 3 5 Corollaries 5 Abstracting all controlled directions When the projection map π can be chosen so as to satisfy ker(t π = span{y,, Y k } the construction described in Proposition 3 can be considerably simplified by eliminating steps 2 and 4 This follows from the fact that in this case we have a = which implies that F is of the form: and the resulting π -related system simplifies to: F = X F 2 = X 2 We thus see that the lack of the coupling input vector fields j 2 implies that condition w ij 2 (t = ui 2(tv j 2 (t required by Theorem 3 is automatically satisfied It then follows that in this case for every trajectory of Σ 2 there exists a trajectory of Σ This is formally stated in the next corollary of Theorem 32: Corollary 5 Let Σ = (R m, R k, F be a control system and π : R m R n the canonical projection If, in addition to assumptions AI through AIV the following equality: also holds, then: ( Construction 3 reduces to steps and 3 k i= k i= v i 2 v i 2 ker(t π = span{y, Y k }

12 2 PAULO TABUADA AND GEORGE J PAPPAS (2 For every trajectory x 2 (t of Σ 2 corresponding to a smooth input trajectory v 2 (t, there exists a trajectory x (t of Σ satisfying π (x (t = x 2 (t Furthermore, x (t and u (t are given by: where α(t is defined by (36 x (t = (x 2 (t, v 2 (t u (t = α(t 52 Single input systems For control systems with a single input, assumption AIII is automatically satisfied since there is a single vector field Y i (therefore i = and from ker(t π span{y,, Y k } (assumption AII we conclude that ker(t π = span{y } which recovers the situation described in the previous section 53 Strict Feedback Systems We now consider single input strict feedback systems ([22] that are defined by the following triangular structure: ẋ = f (x g (x x 2 ẋ 2 = f 2 (x, x 2 g 2 (x, x 2 x 3 ẋ m = f m (x, x 2,, x m g m (x, x 2,, x m u This class of systems is in fact defined by assumptions AI to AIV in the following case: Proposition 52 Let Σ = (R m, R, F be a single input control affine system and let Σ 2, Σ 3,, Σ m be control affine systems where Σ i is obtained from Σ i through the canonical construction described by Definition 27 System Σ is a strict feedback system iff Σ i satisfies assumptions AI through AIV for i =, 2,, m Proof Assume that Σ is a strict feedback system Then is one dimensional, which leads to the unique choice of π (x, x 2,, x m = (x, x 2,, x m for φ and shows that AI through AIII hold Since ẋ m is an affine function of x m and ẋ j does not depend on x m for j < m, it follows that AIV also holds Following the steps described in Proposition 3 we see that Σ 2 is also a single input strict feedback system By the same arguments, Σ 2 also satisfies AI to AIV and the fact that every Σ i satisfies AI to AIV now follows by induction Conversely, assume that Σ is single input system satisfying AI to AIV Since is one dimensional and AII holds, we can assume (possibly after a reordering of the state variables that π is of the form π (x, x 2,, x m = (x, x 2,, x m Assumption AIV now ensures that ẋ j for j < m are affine functions of x m This shows that ẋ m has the form required by a single input strict feedback system Computing Σ 2 we see that x m = v 2 since Σ 2 satisfies AIV, ẋ m 2 has the form required by a single input strict feedback system Inductively using the same argument one concludes that Σ is a single input strict feedback system as desired The previous result shows that for single input systems the refinement procedure presented in this paper can be thought as an open loop equivalent to the well known backstepping method [22] However, for multi input systems, this is no longer true since the multi input version of strict feedback systems: ẋ = f (x g (x x 2 ẋ 2 = f 2 (x, x 2 g 2 (x, x 2 u with x, x 2, u R k and g 2 a nonsingular matrix implies that is an involutive distribution Assumptions AI to AIV only require that ker(t π is involutive so that our framework captures both cases ker(t π and ker(t π = Furthermore, the higher order partial nilpotency relaxations discussed in Section 6 further enlarge the class of systems amenable to hierarchical designs, even for single input systems Proposition 52 and Theorem 32 lead to the following corollary for single input strict feedback systems:

13 ( The abstraction Σ 2 ẋ = v HIERARCHICAL TRAJECTORY GENERATION 3 Corollary 53 Let Σ = (R m, R k, F be a single input strict feedback control system and π (x, x 2,, x m = (x, x 2,, x m the canonical projection Then: ( Construction 3 is reduces to steps and 3 (2 For every trajectory (x,, x m (t of Σ 2 corresponding to a smooth input trajectory v 2 (t = x m (t there exists a trajectory (x (t,, x m (t of Σ corresponding to input α(t as defined by (36 54 Fully actuated mechanical systems We now consider the class of fully actuated mechanical systems described by the following equations: (5 ẋ = v v = m/2 f(x, v g i (x, vu i i= with x R m/2 and v R m/2 For mechanical systems, it is natural to extract the kinematic model from the dynamic model This can be captured in the current framework by choosing the projection map to be π (x, v = x Assuming that the vector fields g i v are linearly independent we immediately obtain that ker(t π = span{y,, Y k } Furthermore, it is not difficult to see that this class of systems also satisfies assumption AIV This results in the following corollary: Corollary 54 Let Σ = (R m, R k, F be a mechanical control system as defined in (5 and π (x, v = x the canonical projection Then: (where v is now an input is π -related to Σ (2 For every trajectory x(t of Σ 2 corresponding to a smooth input trajectory v(t there exists a trajectory (x(t, v(t of Σ satisfying π (x(t, v(t = x(t Furthermore, u(t is given by: u(t = α(t where α(t is defined by (36 For more general classes of mechanical control systems, in particular, underactuated control systems, additional assumptions and restrictions need to be taken into account as discussed in [4, 3] We note that for general mechanical systems, one can always compute a kinematic abstraction, but not every trajectory of the kinematic model can be pulled back to a trajectory of the dynamic model This contrasts with the notion of kinematic reductions [3] where every trajectory is refinable to a dynamic s trajectory, even though there are trajectories of the dynamics model that are not captured by the kinematic model 6 Higher order partial nilpotency assumptions In Section 3 we have presented a constructive methodology for hierarchical trajectory generation and motion planning for a class of nonlinear systems characterized by assumptions AI through AIV Assumptions AI and AIII resulted in no loss of generality and were only motivated by the clarity of presentation while assumption AII is in fact necessary for hierarchical control design and has already appeared several times in different contexts [2, 2, 9, 24] This, however, is not the case for AIV as can be seen by regarding AIV as a nilpotency condition ensuring an affine representation of control system Σ 2 Naturally, higher order polynomials can also be considered at the expense of more complex algebraic constraints between the inputs of system Σ 2 In this section, we outline such generalization through a concrete example In particular we will see how AIV can be replaced by: (6 ad p K A K for some p N

14 4 PAULO TABUADA AND GEORGE J PAPPAS in which we have denoted by ad the recursively defined operation: ad KA = [A, K] ad i KA = [ad i K A, K] We consider an underactuated surface vessel, which can be described by the following equations ([8]: (62 u = m 22 m vr d m u m τ v = m m 2 2 ur d 22 m 2 2 v ṙ = m m 22 uv d 33 r τ 3 m 33 m 33 m 33 ẋ = cos(ψu sin(ψv ẋ = sin(ψu cos(ψv φ = r where u, v and r represent the velocities in surge, sway and yaw, respectively The parameters m ii R are defined by the ship inertia and added masses, while the parameters d ii R are defined by hydrodynamic damping The surge control force τ and yaw control moment τ 3 are the controlled inputs and x, y and φ represent ship position and orientation As described in [7], after feedback and a change of coordinates the equations of motion can be equivalently described by: u = τ u v = cur dv ṙ = τ r ż = u z 2 r ż 2 = v z r ż 3 = r Choosing for projection map π (u, v, r, z, z 2, z 3 = (v, z, z 2, z 3 we see that AI to AIII are satisfied while AIV is not However, since: (63 3 X α β γ = α, β, γ {u, r} condition (6 is satisfied with p = 3, that is: [[[A, K ], K ]K ] K Furthermore, = ker(t π implies that every vector field in A 2 can be obtained as the projection of X so that we focus on the consequences of (6 on X From (6 or (63 we see that X is a polynomial of

15 HIERARCHICAL TRAJECTORY GENERATION 5 degree 2 on the variables u and r thus admitting the following finite Taylor series representation: X (x, x 2 = X (x, X u X r 2 X u x=(x, r x=(x, u r x=(x,ur dv c = u z 2 r (64 ur v z This suggests the extension of the construction described in Proposition 3 by incorporating also the projection of second derivatives of X : ( X 2 (x 2 = π X (x 2, = [ dv v ] T ( Y2 X (x 2 = π u = [ ] T x=(x2, ( Y2 2 X (x 2 = π r = [ z 2 z ] T x=(x2, ( Y2 3 2 ( X (x 2 = π 2 X = π r u u r The resulting abstraction is then given by: or equivalently: x=(x2, X 2 Y 2 v 2 Y 2 2 v 2 2 Y 3 2 v 3 2 v = dv cv 3 2 ż = v 2 z 2 v 2 2 ż 2 = v z v 2 2 ż 3 = v 2 2 x=(x2, = [ c ] T As in Theorem 32 the inputs v 2 and v 2 2 can be identified with the abstracted variables u and r, respectively Input v 3 2, however, is not associated with any input or state of Σ Nevertheless from (64 we see that if u = v 2 and r = v 2 2, then we necessarily have v 3 2 = ur = v 2v 2 2 Replacing AIV by (6 with p = 3 thus creates additional algebraic constraints between the inputs of the reduced model Σ 2 The refinement process is now captured by Theorem 32 since the knowledge of v 2 (t and v 2 2(t determines, by differentiation, the input trajectories τ u (t and τ r (t for system Σ Control systems satisfying (6 for p > 3 can be similarly abstracted by noting that input v i 2 associated with a vector field 2 satisfies: (65 v2 i = x j if Y2 i = T π ( X x j u o 2x j if Y2 i = T π ( Y o x j x j x q if Y2 i = T π ( 2 X x j x q u o 2x j x q if Y2 i = T π ( 2 Y o x j x q x j x q x s if Y2 i = T π ( 3 X u o 2x j x q x s if Y2 i = T π ( x j x q x s 2 Y o x j x q x s

16 6 PAULO TABUADA AND GEORGE J PAPPAS The refinement process for such systems can still be obtained through the methods described in Theorem 32 since an input trajectory for Σ 2 determines the evolution of x c, which by differentiation and (36 determines the input trajectory of Σ 7 Conclusions In this paper we have presented a hierarchical approach for trajectory and motion planning problems Based on the notion of φ-related control systems we have proposed a constructive solution to refine trajectories from reduced models to the original models The proposed methods were developed for a class of nonlinear systems similar to strict feedback systems although extensions to broader classes of systems have also been introduced The presented results also suggest interesting relations with other design approaches described in the literature such as backstepping [22] (discussed in Section 5, flatness [7], and kinematic reductions [4] Such relationships are the subject of current investigations References [] R Abraham, J Marsden, and T Ratiu Manifolds, Tensor Analysis and Applications Applied Mathematical Sciences Springer-Verlag, 988 [2] Calin Belta and Vijay Kumar Optimal motion generation for groups of robots: a geometric approach ASME Journal of Mechanical Design, 23 In press [3] Francesco Bullo and Andrew D Lewis Low-order controllability and kinematic reductions for affine connection control systems SIAM Journal on Control and Optimization, 23 Submitted [4] Francesco Bullo and Kevin M Lynch Kinematic controllability for decoupled trajectory planning in underactuated mechanical systems IEEE Transactions on Robotics and Automation, 7(4:42 42, August 2 [5] Paulo Sérgio Pereira da Silva and Carlos Corrêa Filho Relative flatness and flatness of implicit systems SIAM Journal on Control and Optimization, 39(6:929 95, 2 [6] G S de Alvarez Controllability of poisson control systems with symmetries In J E Marsden, P S Krishnaprasad, and J C Simo, editors, Dynamics and Control of Multi-body Systems, volume 97 of Contemporary Mathematics AMS, 989 [7] M Fliess, J Levine, P Martin, and P Rouchon Flatness and defect of nonlinear systems: Introductory theory and examples International Journal of Control, 6(6:327 36, 995 [8] T I Fossen Guidance and Control of Ocean Vehicles Wiley, New York, 994 [9] E Frazzoli, M Dahleh, and E Feron Real-time motion planning for agile autonomous vehicles In Proceedings of the 2 American Control Conference, pages 43 48, Arlington, VA, June 2 [] Kevin A Grasse Admissibility of trajectories for control systems related by smooth mappings Mathematics of Control, Signals and Systems, 6(2:2 4, 23 [] J Hauser and A Jadbabaie Aggressive maneuvering of a thrust vectored flying wing : A receding horizon approach In Proceedings of the 39th IEEE Conference on Decision and Control, pages , Sydney, Australia, December 2 [2] J Hauser and DG Meyer Trajectory morphing for nonlinear systems In Proceedings of the 998 American Control Conference, pages , Philadelphia, PA, June 998 [3] H Hermes Nilpotent approximations of control systems and distributions SIAM Journal on Control and Optimization, 24:73 736, 986 [4] Matthias Kawski Stabilizability and nilpotent approximations In Proceedings of the 27th Conference on Decision and Control, Austin Texas, 988 [5] WS Koon and JE Marsden Optimal control for holonomic and nonholonomic mechanical systems with symmetry and Lagrangian reduction SIAM Journal on Control and Optimization, 35:9 929, 997 [6] G Lafferriere A general strategy for computing steering controls of systems without drift In Proceedings of the 3th Conference on Decision and Control, Brighton, England, 99 [7] Frederic Mazenc, Kristin Pettersen, and Henk Nijmeijer Global uniform asymptotic stabilization of an underactuated surface vessel IEEE Transactions on Automatic Control, 47(: , 22 [8] RM Murray and S Sastry Nonholonomic motion planning: Steering using sinusoids IEEE Transactions on Automatic Control, 38(5:7 76, 993 [9] George J Pappas and Gerardo Lafferriere Hierarchies of stabilizability preserving linear systems In Proceedings of the 4th IEEE Conference on Decision and Control, pages , Orlando, Florida, December 2 [2] George J Pappas, Gerardo Lafferriere, and Shankar Sastry Hierarchically consistent control systems IEEE Transactions on Automatic Control, 45(6:44 6, June 2 [2] George J Pappas and Slobodan Simic Consistent hierarchies of affine nonlinear systems IEEE Transactions on Automatic Control, 47(5: , 22

17 HIERARCHICAL TRAJECTORY GENERATION 7 [22] R Sepulchre, M Jankovic, and Petar V Kokotovic Constructive Nonlinear Control Communications and Control Engineering Springer-Verlag, New York, January 997 [23] Srdjan S Stankovic and Dragoslav D Siljak Model abstraction and inclusion principle: A comparison IEEE Transactions on Automatic Control, 47(3: , 22 [24] Paulo Tabuada and George J Pappas Abstractions of Hamiltonian control systems Automatica, 39(2: , December 23 [25] M J van Nieuwstadt and RM Murray Real-time trajectory generation for differentially flat systems International Journal of Robust and Nonlinear Control, (8:995 2, 998 Department of Electrical Engineering, 268 Fitzpatrick Hall, University of Notre Dame, Notre Dame, IN address: ptabuada@ndedu Department of Electrical and Systems Engineering, 2 South 33rd Street, University of Pennsylvania, Philadelphia, PA 94 address: pappasg@eeupennedu