A Geometric Analysis of Trajectory Design for Underwater Vehicles ∗ †

Designing trajectories for a submerged rigid body motivates this paper. Two approaches are addressed: the time optimal approach and the motion planning approach using concatenation of kinematic motions. We focus on the structure of singular extremals and their relation to the existence of rank-one kinematic reductions; thereby linking the optimization problem to the inherent geometric framework. Using these kinematic reductions, we provide a solution to the motion planning problem in the under-actuated scenario, or equivalently, in the case of actuator failures. We ﬁnish the paper comparing a time optimal trajectory to one formed by concatenation of pure motions.


Introduction
The need to use autonomous robots provides some of the motivation for research on the control of mechanical systems. We focus in this paper on autonomous underwater vehicles (AUVs). These fall into the class of simple mechanical systems; their Lagrangians are of the form kinetic energy minus potential energy. Geometric control theory provides a useful framework for the study of simple mechanical systems. We address some of the complex non-linearity in these systems by exploiting their natural geometric structures, such as Lie symmetry groups, distributions of vector fields, and affine connections. We use these techniques to study the motion planning problem and an optimization problem.
Previous work based on a geometrical approach to analyse specific motion properties of underwater vehicles can be found in [11,12]. Also, the time minimum problem for underwater vehicles in an ideal fluid has been examined in [5,6,7] under a geometric framework, which mainly focus on conditions for an extremal to be singular. Here we revisit these results on extremality and generalize them to include a rigid body submerged in a viscous fluid, i.e. subject to dissipative forces. We establish a relationship between singular extremals and the geometric notion of decoupling vector fields [3]. Here, decoupling vector fields are identified for under-actuated scenarios of a six degree-of-freedom (DOF) underwater vehicle submerged in an ideal fluid. Characterizing and identifying decoupling vector fields for a vehicle submerged in a real fluid is an open problem and an area of current research. We use the geometric properties of singular extremals and their relationship with decoupling vector fields to examine this problem. This theoretical geometric analysis is also important in the practical use and motion planning of mechanical systems, see [2]. Through the study of decoupling vector fields in the under-actuated scenario for underwater vehicles, we discover solutions to the motion planning problem under a distressed situation. In this paper, we provide in a realistic scenario minimal conditions in terms of actuation for which the vehicle is still kinematically controllable.
Finally, let us mention that in [4] we examine the implementation of different trajectory structures on a testbed AUV with the goal of minimizing time. The concatenation of pure motion trajectories through rest configurations, although practical and easy to implement, is far from time optimal. The same holds true when considering energy consumption as the optimization cost. Moreover, implementing the theoretically computed time optimal trajectory is impractical due to its highly complex control structure. Thus, we must consider a middle ground that is time efficient, but takes advantage of the piecewise constant control structure of the pure motions. Analysis and characterization of decoupling vector fields for the mechanical system can help with this hybridization.

Equations of Motion
We derive the equations of motion for a controlled rigid body immersed in an ideal fluid (air) and in a real fluid (water). By real fluid, we mean a fluid which is viscous and incompressible with rotational flow. Here, we consider water to be viscous fluid (real fluid) in order to emphasize the inclusion of the dissipative terms in the equations of motion. This motivation comes from our desire to apply our results to the design of trajectories for test-bed underwater vehicles.
In the sequel, we identify the position and the orientation of a rigid body with an element of SE(3): (b, R). Here b = (b 1 , b 2 , b 3 ) t ∈ R 3 denotes the position vector of the body, and R ∈ SO(3) is a rotation matrix describing the orientation of the body. The translational and angular velocities in the body-fixed frame are denoted by ν = (ν 1 , ν 2 , ν 3 ) t and Ω = (Ω 1 , Ω 2 , Ω 3 ) t respectively. Notice that our notation differs from the conventional notation used for marine vehicles. Usually the velocities in the body-fixed frame are denoted by (u, v, w) for translational motion and by (p, q, r) for rotational motion, and the spatial position is usually taken as (x, y, z). However, since this paper focuses on the theory, the chosen notation will prove more efficient especially for the use of summation notation in our results.
It follows that the kinematic equations for a rigid body are given by: where the operatorˆ: R 3 → so (3) is defined byŷ z = y × z; so(3) being the space of skew-symmetric 3 × 3 matrices.
To derive the dynamic equations of motion for a rigid body, we let p be the total translational momentum and π be the total angular momentum, in the inertial frame. Let P and Π be the respective quantities in the body-fixed frame. It follows thatṗ = are the external forces (torques), given in the inertial frame, and x i is the vector from the origin of the inertial frame to the line of action of the force f i . To represent the equations of motion in the body-fixed frame, we differentiate the relations p = R P, π = R Π +b p to obtaiṅ where E F = R t (∑ k i=1 f i ) and E T = R t (∑ l i=1 τ i ) represent the external forces and torques in the body-fixed frame respectively.
To obtain the equations of motion of a rigid body in terms of the linear and angular velocities, we need to compute the total kinetic energy of the system. The kinetic energy of the rigid body, T body , is given by: where m is the mass of the rigid body, I 3 is the 3 × 3-identity matrix and r C G is a vector which denotes the location of the body's center of gravity with respect to the origin of the body-fixed frame. J b is the body inertia matrix. Based on Kirchhoff's equations [10] we have that the kinetic energy of the fluid, T f luid , is given by: where M f , J f and C f are respectively referred to as the added mass, the added mass moments of inertia and the added cross-terms. These coefficients depend on the density of the fluid as well as the body geometry. Summarizing, we have obtained that the total kinetic energy of a rigid body submerged in an unbounded ideal or real fluid is given by: This can also be written as T = 1 2 (ν t I 11 ν + 2ν t I 12 Ω + Ω t I 22 Ω). Using P = ∂ T ∂ ν and Π = ∂ T ∂ Ω , we have: The kinetic energy of a rigid body in an interconnected-mechanical system is represented by a positive-semidefinite (0, 2)-tensor field on the configuration space Q. The sum over all the tensor fields of all bodies included in the system is referred to as the kinetic energy metric for the system. In this paper, the mechanical system is composed of only one rigid body, and the kinetic energy metric is actually a Riemannian metric given by on Q = SE(3) × R 3 : For the rest of this paper, we take the origin of the body-fixed frame to be C G , in other words,r C G = 0. Moreover, we assume the body to have three planes of symmetry with body axes that coincide with the principal axes of inertia. This implies that J b , M f and J f are diagonal, while C f is zero. We have the equations P = (mI 3 + M f )ν = Mν and Π = (J b + J f )Ω = JΩ where M = mI 3 + M f and J = J b + J f . It follows from equations (3) and (4) that The terms Mν × Ω, JΩ × Ω and Mν × ν account for the Coriolis and centripetal effects. These effects can also be expressed in the language of differential geometry via a connection, see [3] for a treatise on affine differential geometric control. A Riemannian metric determines a unique affine connection which is both symmetric and metric compatible. This Levi-Civita connection provides the appropriate notion of acceleration for a curve in the configuration space by guarenteeing that the acceleration is in fact a tangent vector field along γ. This setting for acceleration is handled by jet bundles which can be studied in depth in [16]. Explicitly, if γ(t) = (b(t), R(t)) is a curve in SE(3), and γ ′ (t) = (ν(t), Ω(t)) is its pseudo-velocity, the acceleration is given by where ∇ denotes the Levi-Civita connection and ∇ γ ′ γ ′ is the covariant derivative of γ ′ with respect to itself. The affine connection formulation of our system will be used later in our paper to establish a connection between singular extremals and decoupling vector fields. Gravity, buoyancy and dissipative forces can be modeled by adding external forces and torques f i and τ i . We assume the vehicle to be neutrally buoyant, which means that the buoyancy force and the gravitational force are equal. Since the origin of the body-fixed frame is C G , the only moment due to the restoring forces is the righting moment −r C B × R t ρgV k, where r C B is the vector from C G to the center of buoyancy C B , ρ is the fluid density, g the acceleration of gravity, V the volume of displaced fluid and k the unit vector pointing in the direction of gravity.
Additional hydrodynamic forces experienced by a rigid body submerged in a real fluid are due to drag effects. We assume here that form drag is dominant for our application (specific AUV test-bed) and our estimations of this include any other drag terms (such as fluid sheer stresses due to rotational viscous flow). In this paper, we make the assumption that we have a drag force D ν (ν) and a drag momentum D Ω (Ω), we neglect the off-diagonal terms. The contribution of these forces is quadratic in the velocities, more precisely we have Drag = C D ρA |ν i | ν i where C D is the drag coefficient, ρ is the density of the fluid and A is the projected surface area of the object. The drag force and momentum are then non differentiable functions which causes difficulties in theoretical analysis. To overcome this, some assume the vehicle to move in a single direction, hence |ν i | ν i = ν 2 i . We do not want to make this assumption, because at least rotations are needed in both directions. Experimental results for our test-bed vehicle suggest that the total drag force versus velocity can be approximated by a cubic function with no quadratic or constant term. This is what we assume here. To summarize, the translational drag is given by D ν (ν) = diag(D i1 ν ν 3 i + D i2 ν ν i ) and the rotational drag by D Ω (Ω) = diag(D i1 Ω are the constant drag coefficients.
DEFINITION 2.1. Under our assumptions, the equations of motion in the body-fixed frame for a rigid body submerged in a real fluid are given by: where M accounts for the mass and added mass, J accounts for the body moments of inertia and the added moments of inertia. The matrices D ν (ν), D Ω (Ω) represent the drag force and drag momentum, respectively. The term −r C B × R t ρgV k is the righting moment induced by the buoyancy force. Finally, ϕ ν = (ϕ ν 1 , ϕ ν 2 , ϕ ν 3 ) t and τ Ω = (τ Ω 1 , τ Ω 2 , τ Ω 3 ) t account for the control. For a rigid body moving in ideal fluid (air), we neglect the drag effects: D ν (ν) = D Ω (Ω) = 0. REMARK 2.2. In (14) we assume that we have three forces acting at the center of gravity along the body-fixed axes and that we have three pure torques about these three axes. We will refer to these controls as the six DOF controls. This is not realistic from a practical point of view since underwater vehicle controls may represent the action of the vehicle's thrusters or actuators. The forces from these actuators generally do not act at the center of gravity and the torques are obtained from the momenta created by the forces. As a consequence, to set up experiments with a real vehicle, we must compute the transformation between the six DOF controls and the controls corresponding to the thrusters. We address such a transformation for our actual test-bed vehicle in [4].
Together, equations (1), (2) and (14) form a first-order affine control system on the tangent bundle T SE(3) which represents the second-order forced affine-connection control system on SE(3) Introducing σ = (ϕ ν , τ Ω ), equation (15) takes the form: with I −1 i being column i of the matrix I −1 = M −1 0 0 J −1 and Y (γ(t)) accounts for the external forces (a restoring force r C B × R t ρg∇k, a drag momentum D Ω (Ω)Ω, and a drag force D ν (ν)ν). In the absence of these external forces the equations of motion in (15) represent a left-invariant affine-connection control system on the Lie group SE(3), More generally, just as equation (15) on SE(3) is equivalent to equations (1), (2) and (14) on T SE(3), a forced affine-connection control system on a manifold Q is equivalent to an affine control system on T Q with a drift. This equivalence is realized via the geodesic spray of an affine-connection and the vertical lift of tangent vectors to Q.
DEFINITION 2.4. The geodesic spray of ∇ is the vector field S, on T Q, that generates geodesic flow. Specifically, for v ∈ T q Q, From Equation (13), in the special case of our Levi-Civita connection, the geodesic spray is given by: For this presentation of S(b, R, ν, Ω), the components are expressed relative to the standard left-invariant basis of vector fields on T SE(3) rather than coordinate vector fields. Equations (1) and (2) can be used to recover expressions forḃ andṘ. Now, the affine control system on T SE(3) with its associated drift is as follows. We denote by η = (b 1 , b 2 , b 3 , φ , θ , ψ) t the position and orientation of the vehicle with respect to the earth-fixed reference frame. The coordinates φ , θ , ψ are the Euler angles for the body frame. We introduce χ = (η, ν, Ω), and let χ 0 = χ(0) and χ T = χ(T ) be the initial and final states for our submerged rigid body. Then our equations of motion can be written as:χ where the drift Y 0 is given by where Θ is the transformation matrix between the body-fixed angular velocity vector (Ω 1 , Ω 2 , Ω 3 ) t and the Euler rate vector (φ ,θ ,ψ) t , see [9].
The input vector fields are given by Y i = (0, 0, I −1 i ) t , or in other words Y i = vlft(I −1 i ). In [3, p224] the authors show that trajectories for the affine-connection control system on Q map bijectively to trajectories for the affine control system on T Q whose initial points lie on the zero-section. The bijection maps the trajectory γ : In local coordinates, the equations of motion for a submerged rigid body are derived as follows. The coordinates corresponding to translational and rotational velocities in the body frame are ν = (ν 1 , ν 2 , ν 3 ) t and Ω = (Ω 1 , Ω 2 , Ω 3 ) t . Equations (1) and (2) can be written in local coordinates asη and where R 12 = − sin ψ cos φ +cosψ sin θ sin φ , R 13 = sin ψ sin φ +cosψ cos φ sin θ , R 22 = cos ψ cos φ + sin φ sin θ sin ψ and R 23 = − cosψ sin φ + sin ψ cos φ sin θ . Notice that the transformation depends on the convention used for the Euler angles. Our choice reflects the fact that the rigid body goes through a singularity for an inclination of ± π 2 . In the sequel we denote the diagonal elements of the added mass matrix, the inertia matrix, and the added inertia matrix respectively by f }, respectively. The restoring forces in local coordinates are: LEMMA 2.5. The equations of motion for a submerged rigid body in a real fluid with external forces expressed in coordinates are given by the following affine control system:ḃ

Singular Extremals
In this section we study the singular arcs as defined by the Maximum Principle for the time minimal problem.

Maximum Principle
Assume that there exists an admissible time-optimal control σ = (ϕ ν , τ Ω ) : [0, T ] → F × T , such that the corresponding trajectory χ = (η, ν, Ω) is a solution of equations (23)-(34) and steers the body from χ 0 to χ T . For the minimum time problem, the Maximum Principle, see [15], implies that there exists an absolutely continuous vector such that the following conditions hold almost everywhere: where the Hamiltonian function H is given by: Furthermore, the maximum condition holds: The maximum of the Hamiltonian is constant along the solutions of (36) and must satisfy H(χ(t), λ (t), σ (t)) = λ 0 , λ 0 ≥ 0. A triple (χ, λ , σ ) which satisfies the Maximum Principle is called an extremal, and the vector function λ (·) is called the adjoint vector. The maximum condition (38), along with the control domain F × T , is equivalent almost everywhere to (M, J diagonal and positive), i = 1, 2, 3: Clearly, the zeros of the functions λ ν i determine the structure of the solutions to the Maximum Principle, and hence of the time-optimal control. DEFINITION 3.1. We denote the i th switching function by: for i = 1, . . . , 6.
such that a switching function is identically zero, the corresponding component of the control is said to be singular on [t 1 ,t 2 ]. A singular component control is said to be strict if the other controls are bang.
Assume a given component of the control to be piecewise constant; for example, when the component is bang-bang. Then, we say that

Switching Functions
LEMMA 3.4. The first derivative of the switching function δ i is an absolutely continuous function. Using Y 0 , . . . ,Y 6 and σ 1 , . . . , σ 6 from equation (18), the first and second derivatives of δ i are given by: where [ , ] denotes the Lie bracket of vector fields.
Proof. It is a standard fact that the derivative of δ i along an extremal is given byδ The vector fields Y i are vertical lifts; it follows that their Lie brackets are zero. Differentiating once more, we obtain (43).
Another direct consequence of the form of the input vector fields Y i is the symmetric property described in Lemma 3.6. It will play a major role when computing the second derivative of the switching functions. Notice that this lemma holds with or without external forces. LEMMA 3.6. For i, j = 1, · · · , 6 we have Proof. The result comes from the fact that , and the partial derivatives commute.
To derive conclusions about the singular arcs for our system, such as their order, we need to explicitly describe the Lie brackets involved in (42) and (43). Let S = R 0 0 Θ be the transformation matrix between the coordinates expressed in the inertial frame and the coordinates expressed in the body-fixed frame, and let S i be the i-th column. We begin by deriving the results for the simplified case of a rigid body moving in an ideal fluid (air).
For our computations, we introduce U = {1, 2, 3} and V = {4, 5, 6}. The next three propositions are a result of straightforward but heavy computations. We decided to omit these computations since only the results are important for the rest of the paper. The vectors e i for i ∈ U represent the standard basis for R 3 . PROPOSITION 3.7. For a rigid body moving in an ideal fluid, we have that: To study the Lie brackets ] air , let us introduce a new piece of notation. Without loss of generality we may assume i ≤ j from Lemma 3.6. We define: Then, we get the following Proposition.
We now extend the computations to incorporate motion in a real fluid. Remember here that we consider dissipative forces acting on the vehicle. However, notice that the restoring forces do not play any role in the expression of the Lie brackets, yet the drag forces have a significant impact. PROPOSITION 3.9. For a rigid body moving in a real fluid, we have that: More explicitly, for the Lie brackets of order 2 the above proposition says that: An important consequence of the previous computations that we will exploit in this paper is stated in Proposition 3.11. PROPOSITION 3.11. For a rigid body moving in an ideal fluid, we have that: (58) In a real fluid, the previous Lie bracket is not zero but satisfies: Proof. This result is a direct consequence of our computations on Lie brackets. Indeed, equation (58) comes from the fact that (48) implies that B ii and B i−3,i−3 equal zero. The factors multiplying Y i in (59) are given by (53) and (54).

Order of the Singular Arcs
We now demonstrate that Proposition 3.11 can be stated in terms of the order of singular extremals.
DEFINITION 3.12. Along a strict σ i -singular arc, let q be such that d 2q dt 2q δ i is the lowest order derivative in which σ i appears explicitly with a nonzero coefficient. We define q as the order of the singular control σ i . This definition uses the well known result that a singular control σ i first appears explicitly in an even order derivative of δ i , see [14]. Proof. Let χ be a strict σ i -singular extremal. By definition, the function δ i is identically zero along the extremal. The singular control σ i is obtained from equation (43) providing that the is non zero. However, from Proposition 3.11, this is zero for movement in air and is a multiple of λ t Y i for motion in a real fluid. But since along a σ i -singular extremal we have is zero in a real fluid as well. This means that we must compute at least the fourth derivative of the switching function to obtain the singular control as a feedback. REMARK 3.14. For a rigid body moving in an ideal fluid, the is identically zero everywhere. In this case, we say that the order is intrinsic. For a real fluid is zero only along the singular arc.
To determine the exact order of strict singular controls, we need to compute the fourth derivative of the switching functions. The coefficient of the singular control σ i in δ (4) i is represented by the following Lie brackets

Chattering Arcs
It has been established in [17] that there is a close relationship between the existence of chattering arcs and singular extremals of order two. Such arcs are very interesting from a theoretical point of view, however these arcs are impossible to implement in practice. Let us consider a simplified situation to carry out the computations such as in [5]. We will assume that the vehicle moves in the xz-plane and is submerged in an ideal fluid. The equations of motion, in local coordinates, are given by (60)-(65).
In the above equations, we assume M ν 1 f = M ν 3 f . Hence we write m = m + M ν i f and I = I b + J Ω 2 f . REMARK 3.17. Kelley's strict necessary condition for the singular control τ Ω to be optimal holds. Indeed, it is an easy computation to show that λ (4) Since along a strict τ Ω -singular arc the controls ϕ ν 1 and ϕ ν 3 are bang, Analysis of the τ Ω -singular arcs follows the procedure described in [17]. First, we put the Hamiltonian system (36) into a semi-canonical form. We assume that ϕ ν 1 and ϕ ν 3 are bang. Since a τ Ω -singular arc is of intrinsic order two, the four first coordi- To completely define a new coordinate system we need to find ξ such that the Jacobian D(κ, ξ )/D(χ, λ ) is of full rank. We suggest The corresponding D(κ, ξ )/D(χ, λ ) is then of full rank and the canonical Hamiltonian system is Since we were able to reduce our system to a semi-canonical form, using Remark 3.17 it is clear that Kelley's condition holds; it is now possible to apply the results from [17]. In this reference, the authors describe the behavior of all extremals in the vicinity of the singular manifold S = {(χ, λ )|κ i = 0, i = 1, · · · , 4}. In particular, we can conclude that for each point (χ 0 , λ 0 ) in S there exists a 2-dimensional integral manifold of the Hamiltonian system such that the behavior of the solutions inside this manifold is similar to that of the chattering arcs in the Fuller problem (we also have the existence of untwisted chattering arcs). To be more specific, there is a one-parameter family of solutions of system (67) which reach (χ 0 , λ 0 ) in a finite time. However there are infinitely many switching times for the τ Ω control and the switching times follow a geometric progression. It is important to notice that this result does not imply the optimality of such trajectories nor does it imply (assuming ϕ ν 1 , ϕ ν 3 are constants) that every junction between a τ Ω -singular and a τ Ω bang-bang trajectory includes chattering in the control. In order for such a junction to have chattering, the control must be discontinuous, [13]. This is realized at the junction where the angular velocity vanishes (i.e. Ω = 0). In [7] the reader can see an example of a chattering junction computed in the non-symmetric case.

Time Optimal Trajectories
In this subsection, we display an example of a time optimal trajectory for a submerged rigid body in a real fluid containing singular arcs.
The initial configuration of the body is set to be the origin, and we wish to reach a final configuration η T = (6, 4, 1, 0, 0, 0), with both configurations being at rest. The experimental values of the hydrodynamic coefficients and the bounds on the control that we assume for these simulations can be found in [4]. Figure 1 shows the time optimal strategy numerically computed using a direct method. The time for this trajectory is ≈ 25.85s. The structure is mostly bang-bang, except for the τ Ω 3 control which contains singular arcs. These singular arcs depend on our choice of initial and final configurations. In this case, orientation is the key to optimality; first orient, then move. We orient the vehicle such that we can use the maximum available translational thrust to realize the motion, and the vehicle needs to maintain this orientation over the entire trajectory. Singular arcs do not appear in τ Ω 1 and τ Ω 2 because their full power is needed to offset the righting moments. The translational controls ϕ ν 1 ,ν 2 ,ν 3 are used to their full extent, as one would expect for a time optimal translational displacement.

Decoupling Vector Fields
In terms of affine differential geometry, Proposition 3.11 has important consequences. Indeed, there is a relation between our result and the existence of decoupling vector fields. This is what we establish in this section.
We consider a rigid body moving in an ideal fluid (air). Moreover, we make the following additional assumptions. We assume C G coincides with C B . Since we also assume the vehicle to be neutrally buoyant, there are no restoration forces or moments acting on the vehicle. In other words, the system is void of external forces.
In the sequel, to ease the notation we will use m i = m + M ν i f and j i = I b i + J Ω i f . As we will see, our results depend on the symmetries of the rigid body, hence we introduce some terminology. DEFINITION 4.1. We call our system kinetically unique if all the eigenvalues in the kinetic energy metric G are distinct.
In particular, Defintion 4.1 that for a kinetically unique system the added mass (m i ) and added mass moment of inertia ( j i ) coefficients are all distinct. Since the added mass is a measure of the fluid that must be accelerated with the body, unique m i 's imply that the view of the body along each body-frame axis is different. Note that you can have 3 axes of symmetry with 3 unique added mass coefficients, as is the case with an ellipsoidal body with three distinct axis lengths. Unique j i 's imply a nonuniform mass distribution for the body. In practice, this is generally the case.
Under our assumptions, the equations of motion have the form: In the sequel we denote by I −1 the set of input vector fields to our system: I −1 = {I −1 1 , . . . , I −1 6 }. We note here that under our assumptions, I −1 is diagonal, and thus each I −1 i , i = 1, ..., 6, is a single degree of freedom input to the system. DEFINITION 4.2. We refer to I −1 i , i ∈ U as the translational control vector fields and I −1 j , j ∈ V the rotational control vector fields.
In this paper we are interested in kinematic reductions of rank one for the system in (69); namely decoupling vector fields. Let us first introduce some definitions and terminology.
Suppose we have a general affine-connection control system given by where u 1 (t), . . . , u k (t) are measurable controls and {Z 1 , . . . , Z k } is a set of locally defined independent vector-fields on the configuration space M whose images lie in a rank-k smooth distribution Z ⊂ T M. In the under-actuated setting, decoupling vector fields are found by solving a system of homogeneous quadratic polynomials in several variables. Given a vector field V , we must have that Starting with the middle of the above equation, we get that Thus, we are concerned with calculating ∇ Z a Z b for a, b ∈ {1, ..., k} to find the coefficients h 1 , . . . , h k such that V is decoupling. The equations of motion (69) for a submerged body in an ideal fluid are fully actuated. As mentioned previously, in this case there are no quadratic polynomials to solve and every left-invariant vector field is a decoupling vector field. However, the situation is not as straightforward in the under-actuated scenario; practically speaking, the case of actuator failure. In this situation, the body may be unable to apply a force or torque in one or more of the six DOF, limiting the vehicle's controllability. This is an interesting case because it is likely that an underwater vehicle loses actuator power for one reason or another but still needs to move. For example, we would like the vehicle to be able to return home in a distressed situation. Decoupling vector fields give possible trajectories for the return home which the vehicle is able to realize in an under-actuated condition. In [2], the authors consider an under-actuated situation that differs from the ones we are considering here (they assume three body-fixed control forces that are applied at a point different from the center of gravity). Here we asume that actuator failure leaves the ability to control less than six DOF.
In other words, we consider the under-actuated systems with k < 6, {Ĩ −1 1 , . . . ,Ĩ −1 k } an independent subset of I −1 , andσ 1 , . . . ,σ k the corresponding controls; see (16). We define We first give a classification of the decoupling vector fields with respect to the number degrees of freedom we can input to the system; a one-input system can be controlled in only one degree of freedom. More details on this classification can be found in a forthcoming article [8].
Let us first discuss the degenerate situation of one single DOF input vector field, k = 1. This can also be viewed as a loss of 5 DOF situation. Clearly the only possible motion for the body is a motion along or about a single principle axis of inertia. Since in our case ∇ I i −1 I i −1 = 0, the decoupling vector fields of the single input system ∇ γ ′ (t) γ ′ (t) = I i −1 (γ(t))σ i (t) are multiples of the input vector field I i −1 . These motions are then either purely translational or purely rotational corresponding to exactly one principal axis of inertia. This gives us the following definition. i . Its action corresponds to a single principal axis of inertia of the vehicle; the integral curves of the vector field are either purely translational or purely rotational. We call the integral curves of such a vector field pure motions.
Note that generic single-input affine-connection control systems have no decoupling vector fields since a generic vector field will not satisfy the condition that ∇ Z Z ∈ Span{Z}. However, if a vector field Z does satisfy ∇ Z Z ∈ Span{Z}, then via a reparameterization we get ∇ Z Z = 0. Geometrically, we refer to Z as auto-parallel; the integral curves of Z are geodesics for the corresponding connection ∇.
Suppose now that we use two input vector fields; k = 2. A calculation of the terms shows the following (see also Equation (72)).
i + h j I −1 j and ε i jk be the standard permutation symbol. We have: We can deduce that given a kinetically unique two-input system I −1 2 = {Ĩ −1 1 ,Ĩ −1 2 } in which both inputs do not act upon the same principle axis of inertia, a vector field V is decoupling if and only if V ∈ SpanI −1 2 and has all but one of its components equal to zero. In particular, it has the form V = h 1Ĩ −1 2 . These are pure vector fields. If both inputs act on the same principle axis of inertia (i ∈ U , I −1 If we loosen the kinetically unique assumption and let m i = m j for i, j ∈ U or j k = j l k, l ∈ V , then every vector field V ∈ SpanI −1 2 is decoupling if and only if i, j are both in U or both in V or i + 3 = j.
After introducing some additional terminology, we will summarize the results pertaining to all mutli-inputs systems in a theorem.

DEFINITION 4.5. A vector field V is called an axial vector field if it is of the form
We use the term axial motions since the corresponding kinematic motions are a translation and rotation acting on the same principle axis of inertia. We call these integral curves axial motions. They can be seen as an extension of the pure motions. We choose the term coordinate vector field since all three principal axes of the inertial coordinate frame are represented. A kinematic motion for such a vector is referred to as a coordinate motion. uniqueness assumption does not provide any additional decoupling vector fields in this case.
Case 6: Six input system. Every vector field is decoupling.
The major application of computing vector fields is the design of trajectories for our system. This is addressed in the following section.

Motion Planning
The aforementioned theory can be used to design trajectories for our mechanical system, see [2]. Based on Theorem 4.7, we give partial answers to the motion planning problem for the under-actuated scenarios considered in the previous section. By using kinematic motions to design trajectories, we reduce the order of the dynamical system under consideration.
The motion planning problem for the submerged rigid body is the following. Given an initial configuration q 0 ∈ Q and a final configuration q 1 ∈ Q both being at rest (i.e. q o , q 1 have zero velocity), produce a trajectory that steers the system from q 0 to q 1 . For simplicity we assume in the sequel that the initial configuration is always the origin.
A first obvious remark is that if we have control in the six DOF (i.e. we are fully actuated), we can reach any configuration from our initial configuration by a concatenation of pure motions. At the other extreme, with only one input vector field the rigid body is restricted to movement in only one degree of freedom. An interesting question is the minimal number of inputs which we need in order to reach any configuration from the origin using exclusively kinematic motions. But before we address that question, let us introduce some terminology. Notice that we can reparameterize each kinematic motion to satisfy boundary constraints on the controls, and to begin and end at rest. Hence, in what follows, we assume that each kinematic motion starts and ends at rest. The main objective of this section is to determine how many input vector fields, each controlling one degree of freedom, are needed to provide enough decoupling vector fields for kinematic controllability. We begin with the following obvious lemma.  To check the other cases, we will use the following result. THEOREM 4.11. Consider an underactuated rigid body submerged in an ideal fluid: with k < 6, {Ĩ −1 1 , . . . ,Ĩ −1 k } an independent subset of I −1 , and denote by X a set of decoupling vector fields. Suppose that the involutive closure of X , denoted by LieX , span the tangent space T SE (3). Then, the system is kinematically controllable.
Proof. Following our construction, we have reduced the motion planning for the underactuated system to computing integral curves of single-input driftless systems defined on the configuration manifold SE (3). The theorem is then a consequence of a well-known generalization of Chow's theorem. This generalization insure the controllability of a driftless system using only the integral curves of the input vector fields. See [3, Thm. 13.2] for instance. Global controllability follows from the connectedness of SE (3).
In order to determine whether our system is kinematically controllable we will need to determine the involutive closure of the set of decoupling vector fields. The following shows the procedure used for computing Lie brackets to find the involutive closure. Since we have Q = R 6 × SE(3), the linear space of body-fixed velocities is the Lie algebra se (3): If ζ = (ν, Ω) t represents the body-fixed velocity, we let [ζ , η] denote the Lie bracket operation on se (3). Given ζ ∈ se(3), we define the adjoint operator ad ζ : se(3) → se (3) as and since se(3) = R 3 × R 3 ∋ (ν, Ω) we can write Thus, we can define the adjoint operator ad (ν,Ω) : Thus, over this matrix Lie group, the operation of Lie bracket is the same as the matrix commutator. This formulation allows the computation of Lie brackets without differentiation. Now we are ready to display the results. Proof. Assume that the decoupling vector fields for our system contain the vector fields An easy computation shows that } are six linearly independent vectors which span R 6 . Thus, there exists a path between any two zero velocity configurations through the concatenation of integral curves of decoupling vector fields for which each segment is reparameterized to start and end at zero velocity.

.19. A three input rigid body submerged in an ideal fluid with one translational and two rotational input is kinematically controllable. A four input rigid body submerged in an ideal fluid with at least two rotational inputs vector fields is kinematically controllable. A m input rigid body submerged in an ideal fluid with with m ∈ {5, 6} is kinematically controllable.
Proof. This is a consequence of Theorem 4.7 and Theorem 4.14.
We now wish to apply the results of this section to the motion planning of real AUV's. The majority of AUVs are controlled using external thrusters, which do not act directly at C G , and possibly movable wings, foils or rudders. Since actuation of movable wings, foils and rudders implies an applied force or torque to the vehicle, without loss of generality, we can assume that the vehicle is controlled strictly via external thruster actuation. This assumption will also make the following examples easier to visualize. We shall call a thruster oriented such that the output force is parallel to the (body-frame) z-axis a vertical thruster, and a thruster oriented such that the output force is perpendicular to the (body-frame) z-axis a horizontal thruster. Clearly, a vertical thruster contributes to heave, roll and pitch controls, while a horizontal thruster contributes to surge, sway and yaw controls.
Suppose we begin with a fully-actuated submersible which controls heave, roll and pitch with one set thrusters we will call V. While surge, sway, and yaw are controlled with another set of thrusters called H. In order to utilize the notion of decoupling vector fields in the under-actuated situation, suppose we lose the ability to control either H or V. From Theorem 4.13, losing V would limit the motion of the vehicle to a plane. However, losing H would not affect the kinematic controllability of the vehicle by the result of Theorem 4.16. Thus, in the design process of the vehicle, we could save money by requiring that robustness or redundancy need only be implemented onto a portion of the system; the V thrusters. Also, for energy conservation, it may be better to use only one set of thrusters to save battery life. This knowledge and ability to pre-plan can save time and money for the AUV designer and end-user alike. Now, we demonstrate two practical applications to summarize the results of this section. Suppose that we want to start at the origin (η 0 = (0, 0, 0, 0, 0, 0)) and end at η f inal = (4, 3, 2, 0, 0, −90 • ). Positive b values are in the direction of gravity. In the first scenario, suppose we have a vehicle designed as above. Also suppose that we are only able to control the V thrusters. In particular, we are only able to directly control heave, roll and pitch, and the input vector fields are I −1 3 = {I −1 3 , I −1 5 , I −1 6 }. By Theorem 4.16, the vehicle is kinematically controllable, and by Theorem 4.7 we know that the decoupling vector fields for this system are the pure vector fields, V = h i I −1 i for i ∈ {3, 5, 6}. This means that the trajectory can be fully decoupled into a concatenation of pure motions. The basic idea to realize this displacement is use the pitch and roll controls to point the bottom of the vehicle in the direction of η f inal and then use pure heave for the translational displacement. Upon reaching (4, 3, 2, φ , θ , ψ) we can do pitch and roll movements to realize η f inal . For this example, the vehicle needs to apply a pure pitch to reach tan −1 ( apply the opposite roll and pitch controls as above to set roll and pitch angles to zero, then apply a 90 • pure pitch followed by a −90 • pure roll followed by a −90 • pure pitch. This concatenation results in a −90 • yaw, and the motion is realized. Or, we could simply apply a pure pitch to reach (4, 3, 2, −63.4 • , 90 • , 0), then apply a pure roll to reach (4, 3, 2, −90 • , 90 • , 0) and finally a −90 • pure pitch to realize η f inal . Since we have direct control on pitch and roll for this example, it should be clear that any other rotational configuration is also possible. The thrust control strategy for this motion is shown in Figure 2, while Figure 3 display the corresponding trajectory. Note that  here we do not compute the exact duration of the thrust since this will depend on specific dynamics and power of the vehicle and the chosen reparameterization of the motion. We remark that the assumptions of instantaneous actuator switching and equal acceleration and deceleration phases for each motion are not practically applicable to a test bed AUV. Work is ongoing to implement these strategies formed from decoupling vector fields.
For the second scenario, suppose that we are only able to control the V thrusters on the vehicle. Additionally, we assume that the vehicle has a separate system to control buoyancy which is still in operation. This assumption is practically valid, and also creates a non-trivial example. In particular, we are able to directly control surge, sway, heave, and yaw; the input vector fields are I −1 4 = {I −1 1 , I −1 2 , I −1 3 , I −1 6 }. By Theorem 4.14, we know that the vehicle is not kinematically controllable. We are able to realize any position in R 3 , but the vehicle is not able to achieve any angular displacements in roll or pitch. Note, the vehicle could definitely return home in a distressed situation, but it is not performing any fancy manuevers. By Theorem 4.7 we know that the decoupling vector fields for this system are the pure vector fields V i = h i I −1 i for i ∈ {1, 2, 3, 6}, the axial vector field, V a = h 3 I −1 3 + h 6 I −1 6 and the coordinate vector field 6 . This means that the trajectory must follow the integral curves of V a , V b and V i , i ∈ {1, 2, 3, 6} for motion planning. The basic idea to realize this motion is to realize the angular displacement while travelling along the diagonal from (0, 0, 0, 0, 0, 0) to (4, 3, 0, 0, 0, 0), then apply a pure heave to reach η f inal . For this example, the vehicle first needs to follow the integral curves of Then, we follow the integral curves of h 3 V 3 with h 3 = 2 to achieve the desired displacement. The thrust control strategy for this motion is shown in Figure 4 and the corresponding trajectory in Figure 5.

Time Optimality and Decoupling Vector Fields
Proposition 3.11 on singular extremals is actually related to the results of Theorem 4.7 on decoupling vector fields. We can see this based on an identity which relates the Lie Bracket to the affine connection associated with the control system. More precisely, let us consider an affine connection control system defined on a configuration manifold M such as in (70): We denote by S the geodesic spray of the connection ∇, and let X be any vector field defined on T M. It is easy to verify that Applying (82) to the underactuated equations of motion of a submerged rigid body in an ideal fluid: Clearly, now Proposition 3.11 simply states that in an ideal fluid vlft(2∇ I −1 i I −1 i ) = 0 which is a sufficient condition for I −1 i to be decoupling for system (69) as long as I −1 i ∈ I −1 k . Summarizing, Proposition 3.11 implies that in each of the underactuated scenarios considered in Theorem 4.7, the pure motions resulting from the integral curves of the input vector fields are decoupling vector fields. It is remarkable that for a submerged rigid body in an ideal fluid, our knowledge of the structure of singular extremals provides information on the nature of decoupling vector fields.
We can exploit nice relations like this for the system submerged in an ideal fluid, as many authors have done. However, the question arises as to how to extend the theory to include viscosity and potential forces. Can we find similar relations if we consider applying the theory to a real testbed underwater vehicle which experiences drag forces and probably has C g = C B . Let us first begin by addressing the question of the dissipative forces in our model, namely the drag. In other words, consider a real fluid with C G = C B . First, note that the pure motions are still produced using a single DOF input. The main impact of the dissipative forces on the motion of the submerged rigid body is that assuming bounded control inputs, a maximum velocity in the prescribed direction of motion is attained. For instance, in the case of a bodypure surge in the positive direction equation (63) is written asν 1 = 1 m+M ν 1 f (D ν (ν 1 ) + ϕ ν 1 ), and assuming that we start at rest ν 1 (0) = 0, the solution is given by ν 1 (t) = ϕ ν 1 −D ν (ν 1 ) tanh(t −D ν (ν 1 ) ϕ ν 1 m). Since tanh(t −D ν (ν 1 ) ϕ ν 1 m) → 1 when t → ∞, if we impose 0 ≤ ϕ ν 1 ≤ α max ν 1 , the vehicle can only realize a maximum velocity of α max ν 1 D ν (ν 1 ) m/s. The backward surge motion is symmetric, and the other pure translations and rotations in the body fixed-frame are similar. Due to the form of the system, we can show that the reparametrizations of the integral curves ofγ(t) = I −1 i (t)σ i (t) are still solutions to the forced affine connection control system as long as I −1 i ∈ I −1 k . This is explained in detail in [8] through the construction of a new connection∇, and under the assumption that the drag forces are quadratic with respect to the velocity. Notice that with bounded controls, not every reparameterization is a solution to the forced affine-connection control system. This follows from the fact that a maximum velocity constraint imposes a lower bound on the travel time for the rigid body along a given trajectory. However, since the initial and final states of the trajectory are at rest, we can always reparameterize a trajectory to accommodate the bound constraints on the controls. Here we do make the important assumption that the bounds on the controls are such that the vehicle can move through the fluid.
Up to this point we have kept the assumption that C G = C B . However, if the rigid body is an underwater vehicle, this is not a desirable assumption. Having C B and C G coincident is a neutral equilibrium, and hence very sensitive to any external forces. Practically, we impose C G = C B in order to create a righting arm, and thus situating the vehicle in a stable equilibrium. In this situation, the vehicle will restore pitch and roll angles from a listed configuration even if no control force is applied. Thus, the effect opf these restoring moments means that we may not be able to realize a bodypure motion with a single degree of freedom input control vector field. As an example, let us consider a body-pure surge while maintaining a pitch angle of −45 • ; a diagonal dive. Assuming C G = C B , we could first set the orientation, and then use a single control input to realize the motion. However, once we assume that C G = C B , we have to compensate for the induced righting moment by applying pitch control during the entire surge to maintain the desired orientation. In general, we need to apply control to the pitch and roll angular velocities to maintain the desired orientation and compensate for the righting moments while realizing a body-pure motion. Thus, at least three input control vectors are now needed for a generic body-pure motion; pitch, roll and the prescribed direction of motion. In practice, four input control vector fields are usually controlled so that one could compensate the righting moments and run a feedback control in yaw to maintain the proper heading angle during the trajectory. However, there is no restoring moment in yaw and thus theoretically does not need to be directly controlled. If we additionally assume that the vehicle is not neutrally buoyant, we then also have to apply constant heave control in order to maintain a prescribed depth. With this additional assumption, we would need at least four input control vectors to realize a body-pure motion. Notice that when considering bounded controls it also implies a controllability restriction due to the righting moments acting on the angular velocities. If the separation between C G and C B is large, the righting moments will be significant and the vehicle may not be able to realize all orientations in pitch and roll.
We summarize our remarks in the next proposition. In this proposition, a decoupling vector field V is such that every reparametriztion of its integral curves is a solution of the given forced affine connection control system. Notice that we do not assume any bounds on the control for this proposition. PROPOSITION 5.1. Let ∇ be the affine connection (13) and consider the forced affine connection control system  Figure 6: Pure surge, pure sway then pure heave ending at η T = (6, 4, 1, 0, 0, 0). acceleration for t acc heave ≈ 2.92 s and a deceleration for t dec heave ≈ 5.24 s. The non-symmetry of the acceleration and deceleration phases is due to drag forces and physical actuator asymmetries (our computations are based on a real underwater vehicle). The total transfer time for this trajectory is t pure ≈ 79.92 s. This duration is more than triple the optimal time, see Figure 1. This is actually not that surprising since the pure motion trajectory uses only a fraction of the available thrust at any given time.