method (see []). In x we present our model for a pair of uncontrolled underwater vehicles and describe the symmetry group and the reduced dynamics (se

Transcription

1 Orientation Control of Multiple Underwater Vehicles with Symmetry Breaking Potentials Troy R. Smith 1 Dept. of Mechanical and Aerospace Engineering Princeton University Princeton, NJ 085 USA trsmith@princeton.edu Naomi Ehrich Leonard Dept. of Mechanical and Aerospace Engineering Princeton University Princeton, NJ 085 USA naomi@princeton.edu Heinz Hanßmann Prog. in Applied and Comp. Mathematics Princeton University Princeton, NJ 085 USA heinz@math.princeton.edu Abstract We present a control strategy for stable orientation alignment of autonomous vehicles traveling together as a coordinated group in three dimensional space. The control law derives from an artificial potential that depends only on the relative orientation of pairs of vehicles. The result is a controlled system of coupled rigid bodies with partially broken rotational symmetry. Semidirect product reduction theory is used to study the closed loop dynamics, and the energy Casimir method is applied to the reduced dynamics to prove stability of an alignment of vehicles translating in parallel along the same body axis. For clarity, the theory is described in detail for the case of two underwater vehicles, and the extension to an arbitrary number of underwater vehicles is summarized. 1 Introduction The study of control laws for groups of autonomous vehicles has emerged as a challenging new research topic in recent years. There are currently few examples of, and yet many possible applications for, groups of highly autonomous agents that exhibit complex collective behavior in the man made world. Researchers in this area are finding much inspiration from biological examples. Animal aggregations, such as schools of 1 Research partially supported by awufellowship in Engineering from Princeton University. Research supported by the Max Kade Foundation. Research partially supported by the National Science Foundation under grants CCR and BES 950 and by the Office of Naval Research under grant N fish, are believed to use simple, local traffic rules at the individual level but exhibit remarkable capabilities at the group level. These include rapid maneuverability, the ability toquickly process data and a markedly intelligent decision making ability []. Numerous researchers have attempted to model animal aggregation with each agent considered as a point mass (see, for example, [1, 9]). Likewise in engineering applications, group control problems are often formulated with point mass vehicle models (e.g. [8, 11]). Here we consider a group of underwater vehicles, which are modeled as rigid bodies from the outset. We note that the closest biological analogue to this system, that of a school of fish, is believed to maintain group cohesion through each individual's desire to match the speed and direction of nearby fish, and to be surrounded by a certain amount of open space (see [10]). Our focus in this paper is purely on the orientation matching part of the problem. The control law we derive is generated through judicious selection of an artificial potential function designed to break system symmetry. This work complements the development in [5] in which potential functions are used to address group geometry and inter vehicle spacing. The use of symmetry breaking potentials here leads to the method of semidirect product reduction, for which our primary reference is []. Useful examples of this technique are contained in [, ]. In [] stabilization of an arbitrary translation of a single underwater vehicle was achieved using symmetry breaking potentials. The potentials used in that work inspire our choice of potentials here for orientation alignment of multiple underwater vehicles. As in [], we prove stability by constructing a Lyapunov function on the reduced phase space by means of the energy Casimir

2 method (see []). In x we present our model for a pair of uncontrolled underwater vehicles and describe the symmetry group and the reduced dynamics (see also []). In x we review semidirect production reduction theory [] and in x we apply this to the dynamics of the pair of underwater vehicles with orientation alignment control derived from a symmetry breaking potential. In x5 we prove stability oftwo vehicles moving in alignment and in x we describe extension of the development to N vehicles. Final remarks are given in x. Model and Reduction We consider two underwater vehicles, which we shall label with the letters A and B. We assume they are identical and model each as an ellipsoidal body of mass m, as in []. For each vehicle, let the matrix J be the sum of the body inertia and the added inertia from the potential flow model of the fluid. Similarly, let M denote the sum of the body mass m multiplied by theidentity matrix and the added mass matrix. We assume that m is also the mass of the displaced fluid so that each vehicle is neutrally buoyant. If each ellipsoid has uniformly distributed mass, the center of buoyancy is coincident with the center of gravity and both J and M are diagonal in a coordinate system defined by the ellipsoid's principal axes. In these body coordinates, vehicle A moves through the fluid with translational velocity v A =(v A; 1 va; va ) T and angular velocity Ω A =(Ω A 1 ; Ω A ; Ω A ) T, with the analogous quantities for vehicle B defined similarly. The length of the ith principal axis of each ellipsoid is denoted L i and we assume that L 1 >L L. Let the diagonal elements of M be (m 1 ;m ;m ) and the diagonal elements of J be (J 1 ;J ;J ). The configuration space of the two vehicle system is SE() SE(), where SE() is the Euclidean group which globally describes rigid body positions and orientations in three dimensional space. An element in SE() SE() is given by (R A ;b A ;R B ;b B ), where R k SO() is a rotation matrix that maps body coordinates into inertial coordinates, and describes the orientation of vehicle k. Here b k is the vector from the origin of the inertial coordinate frame to the origin of the body frame of vehicle k and describes the position of this vehicle. We now find the equations of motion for the uncontrolled, two body system by means of reduction, as was done for the single underwater vehicle in []. With the above definitions and the crucial assumption that the vehicles are dynamically decoupled, the Lagrangian of this system, T T SE() T SE()! R, isgiven by the kinetic energy T (q A ;q B ) = 1 (Ω A ) T JΩ A +(v A ) T Mv A (.1) + 1 (Ω B ) T JΩ B +(v B ) T Mv B where, for k = A; B, we use the shorthand q k = (R k ;b k ;R k bω k ;R k v k ) T SE(). The mappingb R! so(), where so() is the space of skew symmetric matrices, is defined such that for x; y R,^xy = x y. Under the lift of the left action of SE() SE() on itself, by means of left translation L ( μ RA ; μ ba ; μ RB ; μ bb ) SE() SE()! SE() SE(), the Lagrangian (.1) becomes T (T L ( μ RA ; μ ba ; μ RB ; μ bb ) (q A ;q B )) = T ( μ R A R A ; μ R A b A + μ b A ; μ R A R A bω A ; μ R A R A v A ; μr B R B ; μ R B b B + μ b B ; μ R B R B bω B ; μ R B R B v B ) = T (q A ;q B ) This shows the Lagrangian (and therefore the Hamiltonian and thus the equations of motion) to be invariant under translations and rotations of the inertial frame, so we say the system has full SE() SE() symmetry. Accordingly, Lie Poisson reduction (see []) of the dynamics by the action of SE() SE() induces a Lie Poisson system on (se() se()) Λ = se() Λ se() Λ, where se() is the Lie algebra of SE() and se() Λ is the dual of se(). That is, because of symmetry, the equations of motion can be reduced from those on the dimensional phase space T Λ SE() T Λ SE() to the 1 dimensional space se() Λ se() Λ. We denote an element in the reduced phase space by μ = (Π A ;P A ; Π B ;P B ) where for k = A; B Π k = JΩ k ; P k = Mv k (.) Written in terms of μ se() Λ se() Λ the reduced Hamiltonian is H(μ) = 1 (Π A ) T J 1 Π A +(P A ) T M 1 P A +(Π B ) T J 1 Π B +(P B ) T M 1 P B Here, Π k and P k are the angular and linear momentum vectors, respectively, for the kth vehicle. Given two differentiable functions F; Q on se() Λ se() Λ,thePoisson bracket on se() Λ se() Λ which makes se() Λ se() Λ apoisson manifold is ff; Qg(μ) =rf T Λ(μ)rQ; (.) ψ! Π b Λ(μ) = diag(λ A ; Λ B ); Λ k = k P bk bp k 0 The Lie Poisson equations of motion are given by _μ i = fμ i ;Hg(μ); (.)

3 equivalently, by _Π A 5 = P_ A _Π B P_ B Π A Ω A + P A v A P A Ω A Π B Ω B + P B v B P B Ω B 5 ; (.5) which are simply Kirchhoff's Equations (see [] and references therein). Semidirect Product Reduction Theory In the case when the Hamiltonian is dependent ona parameter, a, and is invariant under the action of a subgroup of the Lie group G, reduction can be achieved by means of semidirect products. Following [], we denote the space containing a by V Λ and define the semidirect product group of G with the vector space V as S = G ρ V with multiplication given by (g 1 ;u 1 )(g ;u )=(g 1 g ;u 1 + ρ(g 1 )u ) (.1) where ρ is a left representation of G on V,i.e.ρ G! Aut(V) is a group homomorphism, with Aut(V ) the Lie group of all isomorphisms of V. Let s = g ρ 0 V be the Lie algebra of S (where g is the Lie algebra of G) with Lie bracket given by [(ο 1 ;ν 1 ); (ο ;ν )]=([ο 1 ;ο ];ρ 0 (ο 1 )ν ρ 0 (ο 1 )ν 1 ) (.) where ρ 0 g! End(V ) is the induced Lie algebra representation, and End(V ) is the Lie algebra of Aut(V ) the space of all linear maps of V into itself. The ±" Poisson bracket of two functions F; Q s Λ ±! R at the point (μ; a) s Λ = g Λ ρ 0 V Λ is given by fi» ffif ff; Qg ± (μ; a) =± μ; fi ffif ± a; ρ 0 ffiq ffiμ ffia ρ0 ffiμ ; ffiq fl ffiμ ffiq ffiμ ffif ffia fl The Hamiltonian vector field of H s Λ ±! R is (.) X H (μ; a) =ΛrH (.) where Λ is the matrix representation of the Poission bracket given in (.). We additionally define L g to be the left action on G (i.e. L g G! G, h! gh), while T Λ L g is its cotangent lift to the cotangent bundle T Λ G (see []). We are now in a position to state the Semidirect Product Reduction Theorem. Theorem.1 (Marsden et al. []) Let H a T Λ G! R be a Hamiltonian depending smoothly on a parameter a V Λ, and left invariant under the action on T Λ G of the stabilizer G a, defined as G a = fg G j ρ Λ (g)a = ag (.5) where ρ is a left representation of G on the vector space V,andρ Λ is the associated right representation of G on V Λ. The family of Hamiltonians fh a j a V Λ g induces a Hamiltonian function H on the space s Λ, defined byh((t e L g ) Λ ff g ;ρ Λ (g)a) =H a (ff g ), thus yielding Lie Poisson equations on s Λ. Semidirect Product Reduction Orientation Control Application We now revisit our system of two underwater vehicles and introduce an artificial potential to stabilize the equilibrium R A = KR B, where K SO() is a (matrix) parameter we are free to choose. We are particularly interested in the case K = I, the identity matrix, since this corresponds to two vehicles moving with orientations in alignment. The form of our potential, V p,isgiven by V p = ff Tr(I (R A ) T KR B ) (.1) where Tr is the trace operator for square matrices and ff is a scalar control gain which, if chosen to be strictly positive, ensures that V p has a global minimum of zero at R A = KR B. With this potential the Lagrangian becomes L(q A ;q B )=T (q A ;q B ) ff Tr(I (R A ) T KR B ) with kinetic energy (1). Under the left action of SE() SE() the Lagrangian becomes L(T L ( μ RA ; μ ba ; μ RB ; μ bb ) (q A ;q B )) = T (q A ;q B ) ff Tr(I (R A ) T ( μ R A ) T K μ R B R B ) The Lagrangian (and likewise the Hamiltonian and the equations of motion) will be invariant provided ( μ R A ) T K μ R B = K (.) The mechanical system is thus invariant under action of the group G K = f(r A ;b A ;R B ;b B ) SE() SE() j (R A ) T KR B = Kg (.) We remark that G K is (isomorphic to) a semidirect product of SO() with R R, where the pertinent representation of SO() on R R depends on the choice of K. In case K = I this representation is simply the diagonal action R! (R; R) andg I is (isomorphic to) the double semidirect product S considered in Section.. of []. The potential energy coupling terms have thus broken the full SE() SE() symmetry, leaving the (left) symmetry group of the system to be G K. This means

4 that the Lagrangian is unchanged by arbitrary translations of bodies A and B and arbitrary rotations R such that R B = R and R A = KRK T (i.e. R A = R B for K = I ). Define ρ SE() SE()! Aut(SO()) by ρ(r A ;b A ;R B ;b B )K = R A K(R B ) T where Aut(SO()) is the Lie group of all automorphisms of SO(). Given two matrices Y R n1 m1, Z R n m,we define their Kronecker Product, denoted by Y Ω Z R n1n m1m, as y 11 Z y 1m1 Z Y Ω Z = (.) y n11 Z y n1m1 Z With this definition it is easy to check that there is a direct correspondence between the elements of R A K(R B ) T R and those of (R A Ω R B ) K e R 9, where K e = (e T K; 1 et K; et K)T. We can thus equivalently define ρ SE() SE()! Aut(R 9 ) by ρ(r A ;b A ;R B ;b B ) K e = (R A Ω R B ) K, e where Aut(R 9 ) denotes the set of invertible linear mappings on R 9. An element in the Lie group S = (SE() SE()) ρ R 9 can be represented by an matrix of the form R A b A R B b B R A Ω R B K e and the group action is given simply by matrix multiplication. Let ρ Λ be the associated right representation of SE() SE() on R 9, given by ρ Λ (R A ;b A ;R B ;b B ) e K =((R A ) T Ω (R B ) T ) e K (.5) Then G K as defined in (.) is the stabilizer of e K R 9 under ρ Λ, i.e. 5 G K = f(r A ;b A ;R B ;b B ) SE() SE() j ρ Λ (R A ;b A ;R B ;b B ) e K = e Kg By Theorem.1 the Hamiltonian dynamics on T Λ (SE() SE()) can be reduced to a Lie Poisson system on the 1 dimensional space s Λ, the dual of the Lie algebra s of S. In fact, s =(se() se()) ρ 0 R where ρ 0 is the induced Lie algebra representation, given by ρ 0 (bff; ν A ; b fi;ν B )» = bff»» b fi; (.) where (bff; ν A ), ( b fi;ν B ) se(),» R. From (.) we can compute the Lie bracket of two elements of the Lie algebra s. Elements in the Lie algebra s can be represented by matrices of the form bff ν A fi b ν B bff Ω I + I Ω fi b e» where e» = (e T»; 1 et»; et»)t. The Lie bracket of two Lie algebra elements in this form is given simply by the matrix commutator. Recalling the definition μ = (Π A ;P A ; Π B ;P B ), where the elements of this vector are defined in (.), we note that μ s = (μ; X) is an element in s Λ where X SO() ρ R is defined by X = ρ Λ (R A ;b A ;R B ;b B )K =(R A ) T KR B (.) For future notational convenience we write X T =( ; ±; ) where ; ±; R. Written in terms of these new variables the reduced Hamiltonian is 5 H s (μ s ) = 1 (Π A ) T J 1 Π A +(P A ) T M 1 P A + (Π B ) T J 1 Π B +(P B ) T M 1 P B + ff ( e 1 +± e + e ) (.8) Since we have performed reduction by means of the left action, the reduced phase space is s Λ (with lower signs in (.)). The first term in the expression for the Poisson bracket (.) is given by the expression in (.). Using this, and defining the pairing between matrices in R and R Λ to be <Y;Z>=Tr(Y T Z) yields the following expression for the Poisson bracket ff; Qg(Π A ;P A ; Π B ;P B ;X) = Π A (r Π A F r Π A Q) Π B (r Π B F r Π B Q) P A (r P A F r Π A Q) P B (r P B F r Π B Q) " " dffif Tr X T ffiq d ffiπ A ffix ffiq ffif d ffiq ffix ffiπ ffif d B ffiπ + ffif ffiq ## A ffix ffix ffiπ B We summarize our results in the following Proposition. Proposition.1 The dynamics of two rigid underwater vehicles coupled by means of a control law that realizes the artificial potential (.1) describes a system that is Hamiltonian on T Λ SE() T Λ SE() with symmetry group G K. Reduction by this symmetry group yields Lie Poisson dynamics on s Λ, the dual of the Lie algebra of the semidirect product S = (SE() SE()) ρ R. In the coordinates μ s =

5 (Π A ;P A ; Π B ;P B ; ; ±; ) of the reduced space s Λ the differential equations governing the reduced dynamics are _Π A =Π A Ω A + P A v A u _Π B =Π B Ω B + P B v B + u _ P A = P A Ω A _ P B = P B Ω B _ =[ 0 ± ]Ω A + Ω B _± =[ 0 ]Ω A +± Ω B _ =[ ± 0 ]Ω A + Ω B (.9) where thecontrol torque u on vehicles A and B is given by u = ff ( e 1 +± e + e ) These dynamics correspond to _μ s = Λ s (μ s )rh s (μ s ) where H s is given in (.8) and Λ s (μ s )= bπ A P ba 0 0 a b c bp A bπ B P bb b b± b 0 0 P bb a T 0 b b T 0 b± c T 0 b The matrices a; b and c are given by 0 a = T 5 ;b= T 0 5 ;c= ±T T ± T T 0 5 (.10) The maximal rank of the Poisson bracket (.10) on the whole 1 dimensional space s Λ being 1, there may be up to nine independent Casimirs, i.e. functions that Poisson commute with any other function and as a result are constant along system trajectories for any Hamiltonian. We found the following six Casimir functions C 1 = kp A k C = kp B k C = (P B ) T ( ; ±; )P A C = k k + k±k + k k C 5 = k ±k + k± k + k k C = ( ±) The functions k k ; k±k ; k k ; ±; ± ; arenot Casimirs on the whole space s Λ ; they only become constant along system trajectories once we restrict to the subset t Λ =(se() se()) ρ 0 SO() of s Λ. On this 15 dimensional submanifold the Casimirs C ;C 5 ;C assume the fixed values C, C 5, C 1. 5 Proposition. On the subset t Λ sλ given by k k = k±k = k k =1; ±=± = =0 the 1 dimensional symplectic leaves are obtained by fixing the values of the three functionally independent Casimir functions C 1 ;C and C. 5 Stability of the Reduced System We examine dynamic stability at equilibria of the reduced system of the form (Π A ;P A ; Π B ;P B ; ; ±; ) =(0;P A 0 e ; 0;P B 0 e ;e 1 ;e ;e ) (5.1) with P A 0 ;P B 0 both different from zero. Physically, such equilibria correspond to underwater vehicles A and B being aligned, non rotating and each translating along its shortest axis. We note that motion of an underwater vehicle along any but its shortest axis is unstable unless it is spinning, or stabilized by a controlled moment ([,, 1]. The control laws derived in this paper provide no such moment and thus, without additional control terms, stability along the shortest axis is the best that can be hoped for. To findalyapunov function, we make the ansatz H Φ = H s +Φ(C 1 ;C ;C ;C ;C 5 ;C ) Let Φ (l) be the derivative of Φ with respect to C l at the equilibrium (5.1). The first variation of H Φ will be zero at this equilibrium provided P A 0 +P A 0 Φ (1) + P B 0 Φ () m = 0 P B 0 +P B 0 Φ () + P A 0 Φ () m = 0 ff +Φ () +Φ (5) +Φ () = 0 ff + P A 0 P B 0 Φ () +Φ () +Φ (5) +Φ () = 0 This implies Φ () = 0, and taking Φ (5) =Φ () =0 we may choose Φ to have the following simple form Φ = (kp A k P A 0 P A 0 ) kp A k =(m ) + (kp B k P B 0 P B 0 ) kp B k =(m ) + ff (k k + k±k + k k ) We nowcheck for definiteness of the second variation at the equilibrium point. The Hessian of H Φ has an associated matrix given by diag(j 1 ; f M A ;J 1 ; f M B ;ffi ;ffi ;ffi ) where f M k =diag(1=m 1 1=m ; 1=m 1=m ; 8(P k 0 ) ), for k = A; B. The Hessian is thus positive definite provided ff>0. Stability follows by the energy Casimir method [].

6 Proposition 5.1 Consider two rigid underwater vehicles with controlled dynamics described by (.9). Take ff>0. Then, the equilibrium (5.1) corresponding to the two vehicles with full (three dimensional) alignment in orientation and each translating along its shortest axis is stable. The Case of N Vehicles The case of N underwater vehicles, when considered as N(N 1)= possible pairwise interactions, can be treated as a natural extension of the two vehicle problem. We label each ofourn vehicles with an index (i) for i =1;N, and now seek an appropriate artificial potential to stabilize the equilibrium R (j) = K (jk) R (k), for all pairs j; k satisfying 1» j<k» N (.1) where K (jk) SO() is a (matrix) parameter relating the orientation of body (j) to that of body (k). For consistency, we must include the requirement that K (ji) K (ik) = K (jk) 8 j; k and j<i<k This requirement will be trivially satisfied in the case K (jk) = I 8 j; k satisfying (1) As in x, this choice corresponds to our desire to stabilize the equilibrium consisting of all N vehicles having the same orientation. Adjusting the definitions of V p ; e K and ρ appropriately the semidirect product reduction follows through in much the same wayasinx. Further details will be provided in a future publication. Concluding Remarks We haveshown in this paper how to use symmetry breaking potentials to generate a control law to effect stable orientation control for a pair of underwater vehicles. To prove asymptotic stability we can add a dissipation term to the control law and use the same Lyapunov function H Φ constructed in x5. In fact, the Lyapunov function can be used like a control Lyapunov function to prescribe the form of the dissipation control term and to study stability in the presence of physical dissipation, i.e., drag (see [1]). We remark that the various symmetry groups G K for (non aligning) relative orientation K SO() are all isomorphic to G I through an inner automorphism of SE() SE(). This may allow for an extension of the approach presented here to the case of prescribed time varying relative orientations K(t). In a future publication we will present the details of the extension of this work to N vehicles that was introduced in x. Further, we plan to couple this work with the theory presented in [] to allow the stabilization of motion of our underwater vehicles with arbitrary steady translations, and to prevent translational drift of our vehicle system. This will allow for stabilization of the vehicles in alignment and moving along their long axes (long axis motion being the preferred motion for streamlined bodies). It is also of interest to investigate the addition of control terms which depend on the relative distance between vehicles. These terms will be intended to break translational symmetry, forcing P A 0 = P B 0 in (5.1). For example, relative distance dependent factors might be included in the orientation control such that each vehicle tries to align itself only with its nearest neighbors. We will also consider adding to the orientation control the type of control laws featured in [1, 5] such that we produce formations of aligned vehicles with desirable group geometries and inter vehicle spacing. References [1] Heppner, F. and Grenander, U. [1990] A stochastic nonlinear model for coordinated bird flocks, In S. Krusna, editor, The Ubiquity of Chaos, AAAS Pubs., 8. [] Parrish, J.K. and Edelstein Keshet, L. [1999] >From individuals to emergent properties Complexity, pattern, evolutionary trade offs in animal aggregation, Science 8, [] Leonard, N.E. [199] Stability of a bottom heavy underwater vehicle, Automatica, 1. [] Leonard, N.E. [199] Stabilization of underwater vehicle dynamics with symmetry breaking potentials, Systems and Control Letters, 5. [5] Leonard, N.E. and Fiorelli, E. [001] Virtual leaders, artificial potentials and coordinated control of groups, Proc. IEEE CDC. [] Marsden, J. E. and Ratiu, T. [1999] Introduction to Mechanics and Symmetry, nd Ed., Springer Verlag, New York, NY. [] Marsden, J. E., Ratiu, T. and Weinstein, A. [198] Semidirect products and reduction in mechanics, Transactions of the AMS 811, 1 1. [8] McInnes, C.R. [1995] Distributed control of maneuvering vehicles for on orbit assembly, Journal of Guidance, Control and Dynamics 185, [9] Okubo, A. [1985] Dynamical aspects of animal grouping swarms, flocks, schools and herds, Advances in Biophysics, 19. [10] Partridge, B.L. [198] The structure and function of fish schools, Scientific American, [11] Wang, P.K.C. [1989] Navigation strategies for multiple autonomous mobile robots moving in formation, IEEE/RSJ International Workshop on Intelligent Robots and Systems, 8 9. [1] Woolsey, C.A. and Leonard, N.E. [1999] Global asymptotic stabilization of an underwater vehicles using internal rotors. Proc. IEEE CDC, 5 5.