Optimal Trajectory Tracking of Nonholonomic Mechanical Systems: a geometric approach

Optimal T rajectory T racking of Nonholonomic Mechanical Systems: a geometric appr oach Aradhana Nayak 1 , Rodrigo Sato Mart ´ ın de Almagro 2 , Leonardo Colombo 2 and David Mart ´ ın de Diego 2 Abstract — W e study th e tracking of a t rajectory for a nonholonomic sys tem by r ecasting the pr oblem a s an optimal control problem. The cost function is chosen to minimize the error in positions and velocities between the trajectory of a non- holonomic system and the desired refer ence trajectory ev olving on the d istribution whi ch defines the n onholonomic constraints. W e pr ep ose a geom etric framew ork since it describes the class of nonlinear systems under study in a coo rdinate-free framewo rk. Necessary conditi ons fo r the existence of extrema are determined by the Pontryagin Min imum Principle. A nonholonomic fully actuated particle is used as a benchmark example to show how th e proposed method is app lied. I . I N T RO D U C T I O N Nonholo nomic optimal co n trol pro blems arise in many engineer in g applicatio n s, for instance systems with wheels, such as ca rs and bicycles, and systems with blades or sk ates. There are thus mu ltiple application s in the co n text of wheeled motion, space or mo bile robo tics and rob o tic manipulation. The earliest work on contr ol of non holono mic system s is by R. W . Brockett in [9]. A. M. Bloch [2], [3] h as examin ed sev- eral contro l the o retic issues which pertain to both h olonom ic and non holono mic systems in a very general form. The seminal works about stabilization in n onholo nomic control systems were d one by A. M. Bloch, N. H. Mc Clamroch, a nd M. Reyhanoglu in [3], [5], [6], [ 7], and more recently by A. Zuryev [27]. A geometrical dynamica l system of mechanica l type is completely determin ed by a Riemannian ma nifold Q , a kinetic energy , which is defined throug h th e Riemannian metric G o n the manif old an d the p otential f o rces encode d into a p otential (con servati ve) function V : Q → R . These objects, togeth er with a non-in tegrable d istribution D ⊂ T Q on th e tangent bundle of the co nfiguration space determines a nonholo n omic mechanical system. Stabilization of an equilibrium point of a mechanical system on a Riemannian man ifold h as been a p roblem well studied in the literatur e fro m a ge o metric fram e work along the last decad e s (see [2] an d [11] for a revie w on the to pic). Further extensions of these results to the pro blem of track ing a smoo th and b o unded trajectory can be foun d in [11] where 1 A. Nayak is with Systems and Control Engineering, IIT Bombay , Mumbai India 400076 arad hana@sc.iitb.ac .in 2 L. Colombo, D. Mart ´ ın de Diego and R. Sato de Almagro are with Instituto de Cienc ias Matem ´ atica s (ICMA T - CSIC), Call e Nicol ´ as Cabrera 15, Campus U AM, Cantoblanc o, 28049, Madrid, Spain. leo.colombo@i cmat.es, davi d.martin@icmat. es, rodrigo.sato@ icmat.es *The work of A. Nayak has been partial ly supported by Alia nza 4 Erasmus Fello ws hip. The work of L. Colombo, D. Mart ´ ın de Diego and R. Sato Mart´ ın de Almagro has been partiall y supporte d by MINECO (Spain) grant MTM2016-76072-P a propo rtional and der ivati ve plus fee d fo r ward (PD+FF) feedback control law is proposed for tracking a trajecto ry on a Riema nnian m anifold using er r or fu nctions. For trajector y tracking , the usual app roach of stabilization of error dyn amics [19], [22], [23], [25] canno t be utilized for nonho lonomic systems. This is becau se ther e does not e xist a C 1 (even continuo us) state feedback which can a sy mp- totically stabilize the trajectory o f a nonholo nomic system about a desired equilibrium po int. The clo sed loop tra je c tory violates Brockett’ s condition [10], [7] which states that any system of the f orm ˙ x = f ( x, u ) m ust have a ne ighborh ood of zer o in the im a ge of the map x → f ( x, u ) fo r some u in the control set. This result appears in Theo rem 4 in [7]. In this paper, we introduc e a geometric a l framework in nonho lonomic mechanics to study tracking of trajecto ries for nonholo nomic systems based on [12], [1 6], [17]. Th e application o f mo dern tools from d ifferential geometry in th e fields of mechanics, con trol theor y and nu merical integration has led to significant prog ress in these research ar eas. For instance, the study in a g eometrical form ulation of th e nonho lonomic equations o f motio n has led to b e tter und er- standing of dif ferent engin eering pro blems such loco motion generation , controllability , motion planning , an d traje c tory tracking. Combining the idea s of geom etric m ethods in con trol theory , nonho lonomic systems and op tim ization techniques, in th is pap er , we stu dy the und erlying geometr y of a track ing problem f or no nholono mic systems by un derstanding it as an optimal control p roblems for mechanical systems subject to nonho lonomic constrain ts. Giv en a referen c e trajector y γ r ( t ) = ( q r ( t ) , v r ( t )) on D the problem studied in this work consists on findin g an admissible curve γ ( t ) ∈ D , solvin g a dyn a mical contro l system, with prescribed bounda r y conditio ns on D and min- imizing a cost fu nctional which in volves the error between the refe r ence trajectory and the trajectory we want to find (in terms of bo th, positions and velocities), and the effort of the control inp u ts. This cost functional is accom plished with a weighted terminal cost (also known as Mayer term) which induces a constraint into the dy n amics on D . The interval length fo r the cost function al T may either be fixed, o r app ear as a degree of freed om in th e optim ization prob lem, o r be time ho rizon. In this work, we r e strict to the case when T is fixed. T o test th e e ffi ciency of the propo sed meth od, we use a Runge Kutta integrator to gether with a sho oting method in the solution of a trajectory optim ization for a simple but challengin g bench mark m echanical system: a fully actuated particle subject to a nonho lonomic con straint into the dy- namics. W e prop ose a geometric d eriv ation of the equ ations o f motion fo r tr acking a trajectory o f a no nholon omic system as an op timal c ontrol pro blem find we find necessary con ditions via the Pontryag in Min imum Prin c ip le (PMP), wh ere the optimal Hamiltonian is defined on the cotangen t bundle of the con straint distribution. This appro ach allow for the reduction in the degrees of freedom of the equa tio ns for the o p timal contro l pr oblem, compa r ed with typical methods describing the dynam ics o f a no n holono mic system, as the ones arising from the ap plication of Lagrang e-d’Alemb e r t principle. The main advantages in th is geo metric framework consist in the use of a basis of vector field s o n D allowing the reduction of some degrees of freedo m in the d ynamics for a non holono mic mech anical system. The paper is struc tured as follows: we introd uce me c hanical sy stems on a manifold, connectio ns on a Riemannian manifold and the geom etry of nonholon omic dynamical systems on Section II, tog ether with the example we used as ben chmark the nonholo nomic particle. Section III introd uces the de tails of the problem under study motiv ated by the n on-existence of a C 1 feedback control to asymp totically stabilize th e error dyna m ics in no n- holono mic systems. Nece ssary con ditions for th e existence of extrema in the prop osed optimal con trol prob lem a r e studied from the PMP in Section IV. W e also show numer ical r esults and analyze the results we obtain. I I . N O N H O L O N O M I C M E C H A N I C A L S Y S T E M S A. Preli minaries Let Q be a n -dimen sional d ifferentiable m anifold with lo- cal coo rdinates ( q i ) , with 1 ≤ i ≤ n , the con figuration space of a mechanical system. Den ote by T Q its tan gent bundle with induce d local coo rdinates ( q i , ˙ q i ) . Given a Lagrang ian function L : T Q → R , its Euler-Lagrange equa tio ns are d dt  ∂ L ∂ ˙ q i  − ∂ L ∂ q i = 0 , 1 ≤ i ≤ n. (1) These equations determ in e a system o f implicit second- order d ifferential equatio ns in general. If we assume that the Lagrang ian is regular, tha t is, the n × n ma trix  ∂ 2 L ∂ ˙ q i ∂ ˙ q j  is non- degenerate, th e local existence an d uniqueness of solutions is g u aranteed for any gi ven initial condition . V ector fields are used to calculate the directio nal deriv ativ e of a functio n d efined on Q . In the realm of differential geometry a more gen eral op erator is defin ed to per form deriv a tion of a wider range of geom etric objects (tensors). This o perator is called con nection (linear, covariant, or affine connectio n). The definition of the conne c tion is a wish list of properties wh ich it is expected to have Definition 2.1: An (affine) connection o n a smooth man- ifold Q is a map wh ich takes a pair co nsisting of a vector (or a vector field), an d a ( p, q ) -tensor field, T , and r eturns a ( p, q ) -ten sor field, such that it satisfies the following axioms • ∇ X f = X ( f ) , for f ∈ C ∞ ( Q ) , • ∇ X ( T + S ) = ∇ X T + ∇ X S , for T an d S tensors o f the same ty pe, • ∇ X T ( f , g ) = ( ∇ X T )( f , g ) + T ( ∇ X f , g ) + T ( f , ∇ X g ) . This d efinition o f a c o nnection is com plete, i.e., this list of proper ties results in a un iquely defined g eometric o p erator; howe ver, an extra stru cture on the manif o ld is n eeded to define this o bject in a chart. T o d o so, we nee d to kn ow how it ac ts on the basis of th e tan g ent vector space. The result is a tan gent vector field, and at e a c h point it is spanned by the basis of the tangen t space at that po in t ∇ ∂ ∂ q i  ∂ ∂ q j  = Γ k ij ∂ ∂ q k . Denote by X ( Q ) the set of vector fields on Q . A metric G o n a smooth manifold is a (0 , 2 ) -tensor field satisfying • Symmetry : G ( X, Y ) = G ( Y , X ) X, Y ∈ X ( Q ) , • Non-d egeneracy: G ( X, Y ) = 0 if and on ly if w h en X = 0 then Y = 0 . Locally , th e metric is determined b y th e matrix M = ( G ij ) 1 ≤ i,j ≤ n where G ij = G ( ∂ /∂ q i , ∂ /∂ q j ) . Using the m etric G we may co mpute the Christo ffel symbols associated with the metric as Γ k ij =  G − 1  ks  ∂ G sj ∂ q i + ∂ G si ∂ q j + G i j ∂ q s  where G − 1 is defined as the inv erse of the metric with compon ents determined by the inv erse matrix o f M . B. Nonho lonomic mechanic a l systems Most no nholon o mic systems have lin ear constraints, an d these a r e the o n es we will consider . Linear con straints o n the velocities (o r Pfaf fian constra ints) a r e locally gi ven b y equations of the f o rm φ a ( q i , ˙ q i ) = µ a i ( q ) ˙ q i = 0 , 1 ≤ a ≤ m , depend ing, in genera l, on their co n figuration coordin ates and their velocities. Fr om an intrinsic point of view , the linear constraints are defined by a regular distribution D on Q of constan t r a nk n − m such that the annihilator of D is lo cally giv en at e ach point of Q b y D o q = span  µ a ( q ) = µ a i dq i ; 1 ≤ a ≤ m  where the on e- forms µ a are independent at each point of Q . Now we restrict ourselves to the case of nonholon omic mechanical systems wher e the Lagran gian is of mech anical type, th at is, a Lagrang ian systems L : T Q → R defined by L ( v q ) = 1 2 G ( v q , v q ) − V ( q ) , with v q ∈ T q Q , where G denotes a Riemannian metric on the configu ration space Q representing the kine tic en ergy of the systems an d V : Q → R is a potential f unction. Next, assume that the system is subject to nonholo nomic constraints, defin e d by a regular distribution D on Q with corank ( D ) = m . Denote by τ D : D → Q the canonical projection of D onto Q and by Γ( τ D ) the set of section s of τ D which in this case is just the set of vector fields X ( Q ) taking values on D . If X , Y ∈ X ( Q ) , then [ X , Y ] den o tes the standard Lie br acket of vector field s. Definition 2.2: A non holono mic mechanical system o n a smooth man ifold Q is giv en b y the trip le ( G , V , D ) , wh ere G is a Riemann ian m etric on Q, r epresenting the kinetic energy of th e system, V : Q → R is a smooth fun ction representin g the po tential en ergy and D a non-integrab le regular distribution on Q representing the non holono mic constraints. Giv en X , Y ∈ Γ( τ D ) tha t is, X ( x ) ∈ D x and Y ( x ) ∈ D x for all x ∈ Q, then it may h appen tha t [ X, Y ] / ∈ Γ( τ D ) sinc e D is non integrable. W e want to obtain a b racket d efinition for sections of D . Using the Riemannian metric G we can define two complementary o rthogo nal projector s P : T Q → D and Q : T Q → D ⊥ , with respect to the tangent bundle orthog onal decom p osition D ⊕ D ⊥ = T Q . Th erefore, g i ven X , Y ∈ Γ( τ D ) we define the nonh o lonomic brack e t [ [ · , · ] ] : Γ( τ D ) × Γ( τ D ) → Γ( τ D ) as [ [ X A , X B ] ] := P [ X A , X B ] . Th is Lie bracket verifies the u sual prop erties of a Lie bracket except the Jacobi identity (see [4], [14] for example). Definition 2.3: Consider the restriction o f th e Rieman nian metric G to the distribution D , G D : D × Q D → R an d define the Levi-Civita co nnection ∇ G D : Γ( τ D ) × Γ( τ D ) → Γ( τ D ) determined by the following two prop erties: 1) [ [ X , Y ] ] = ∇ G D X Y − ∇ G D Y X , 2) X ( G D ( Y , Z )) = G D ( ∇ G D X Y , Z ) + G D ( Y , ∇ G D X Z ) . Let ( q i ) be local co ordinates on Q and { e A } be indepen - dent vector fields on Γ( τ D ) (that is, e A ( x ) ∈ D x ) such th at D x = span { e A ( x ) } , x ∈ U ⊂ Q. Then, we can deter mine the Christoffel symbols Γ A B C of the connectio n ∇ G D by ∇ G D e B e C = Γ A B C ( q ) e A . As when we work in tang ent bundles, it is possible to deter mine the Christoffel symbols associated with the connectio n ∇ G D by ∇ G D e B e C = Γ A B C e A . No te that the coefficients Γ C AB of the connec tio n ∇ G D are (see [1] fo r details) Γ C AB = 1 2 ( C B C A + C A C B + C C AB ) (2) where the constant structures C C AB are defined as [ [ X A , X B ] ] = C C AB X C . Definition 2.4: A curve γ : I ⊂ R → D is admissible if γ ( t ) = dσ dt ( t ) , where τ D ◦ γ = σ . Giv en loca l co ordinates on Q, ( q i ) with i = 1 , . . . , n ; and { e A } sections on Γ( τ D ) , with A = 1 , . . . , n − m , such that e A = ρ i A ( q ) ∂ ∂ q i we introd uce induc ed coordin ates ( q i , v A ) on D , where, if e ∈ D x then e = v A e A ( x ) . The r efore, γ ( t ) = ( q i ( t ) , v A ( t )) is ad missible if ˙ q i ( t ) = ρ i A ( q ( t )) v A ( t ) . Consider the re stricte d Lagran gian fun c tion ℓ : D → R , ℓ ( v ) = 1 2 G D ( v , v ) − V ( τ D ( v )) , with v ∈ D . Definition 2.5: A solu tion of the nonholon omic pr ob lem is an admissible curve γ : I → D such that ∇ G D γ ( t ) γ ( t ) + g r ad G D V ( τ D ( γ ( t ))) = 0 . Here the section g r ad G D V ∈ Γ( τ D ) is characterized by G D ( g r ad G D V , X ) = X ( V ) , for e very X ∈ Γ( τ D ) . These equa tio ns ar e equiv alent to the nonho lonomic equa - tions . Locally , these equation s are given by ˙ q i = ρ i A ( q ) v A (3) ˙ v C = − Γ C AB v A v B − ( G D ) C B ρ i B ( q ) ∂ V ∂ q i , (4) where ( G D ) AB denotes the coefficients o f the in verse matrix of ( G D ) AB where G D ( e A , e B ) = ( G D ) AB . Remark 2.6: The n onholo nomic eq uations only depend on the coor d inates ( q i , v A ) on D . Therefo re the n onholo n omic equations are free of Lag range multipliers. These equation s are e quiv alen t to th e no nholon omic Ha mel equ ations (see [8], for examp le, and reference s therein). C. Exa mple: The nonho lonomic pa rticle Consider a particle of unit mass e volving in Q = R 3 with Lagra ngian L ( x, y , z , ˙ x, ˙ y , ˙ z ) = 1 2 ( ˙ x 2 + ˙ y 2 + ˙ z 2 ) , and subject to th e co nstraint ˙ x + y ˙ z = 0 . This nonho lonomic system is defined by the ann hilation of the one-fo rm µ ( x, y , z ) = (1 , 0 , y ) . T he non holonom ic equations, der ived f rom the Lagran ge-d’Alem bert principle, are gi ven by ˙ v x = λ, v x = ˙ x, ˙ v y = 0 , v y = ˙ y , (5) ˙ v z = y λ, v z = ˙ z , v x + y v z = 0 , which, after su bstituting the Lagrange mu ltiplier λ = − v z v y 1 + y 2 , lea d to ˙ v x = − y 1 + y 2 v z v y , ˙ v z = − 1 1 + y 2 v z v y , (6) v z = ˙ z , v y = ˙ y , v x = ˙ x, ˙ v y = 0 , (7) such that v x + y v z = 0 . Let D ⊂ T R 3 ≃ R 3 × R 3 denote the no n holono mic distribution corresp onding to this system. Then th ese equations define a time-co n tinuous flow F t : D → D , i.e. F t (( q (0) , v (0))) = ( q ( t ) , v ( t )) , where q ( t ) = ( x ( t ) , y ( t ) , z ( t )) T and v ( t ) = ( v x ( t ) , v y ( t ) , v z ( t )) T , ( q (0) , v (0)) ∈ D . The distribution D is determined by D = sp a n { Y 1 , Y 2 } = span n ∂ ∂ y , ∂ ∂ z − y ∂ ∂ x o . Th en, D ⊥ = { ∂ ∂ x + y ∂ ∂ z } . Let ( x, y , z , v 1 , v 2 ) be induced co o rdinates o n D . Giv en the vector field s Y 1 and Y 2 generating the distrib u- tion we o btain the relatio n s fo r q ∈ R 3 Y 1 ( q ) = ρ 1 1 ( q ) ∂ ∂ x + ρ 2 1 ( q ) ∂ ∂ y + ρ 3 1 ( q ) ∂ ∂ z , Y 2 ( q ) = ρ 1 2 ( q ) ∂ ∂ x + ρ 2 2 ( q ) ∂ ∂ y + ρ 3 2 ( q ) ∂ ∂ z . Then, ρ 1 1 = ρ 3 1 = ρ 2 2 = 0 , ρ 2 1 = ρ 3 2 = 1 , ρ 1 2 = − y . Each elemen t e ∈ D q is expr essed as a linear combina tion of these vector fields: e = v 1 Y 1 ( q ) + v 2 Y 2 ( q ) , q ∈ R 3 . Therefo re, th e vector sub bundle τ D : D → R 3 is locally described by th e co ordinates ( x, y , θ ; v 1 , v 2 ) ; the first th ree for the b a se and the last tw o, for th e fiber s. Observe that e = v 1 ∂ ∂ y + v 2  ∂ ∂ z − y ∂ ∂ x  and, in con sequence, D is described by the conditions (admissibility cond itions): ˙ x = − y v 2 , ˙ y = v 1 , ˙ z = v 2 as a vector subbundle of T Q wh ere v 1 and v 2 are the a dapted velocities rela tive to the basis of D define d before. The nonh olonomic bracket giv en by [ [ · , · ] ] = P ([ · , · ]) satisfies [ [ Y 1 , Y 2 ] ] = P [ Y 1 , Y 2 ] = P  − ∂ ∂ x  = y 1 + y 2  ∂ ∂ z − y ∂ ∂ x  . Therefo re, b y using (2) all the Christo ffel symb ols for the connection ∇ G D vanish except Γ 2 12 which is g i ven by Γ 2 12 = y 1 + y 2 . The restriction of the Lagran gian function L on D in the adapted co ordinates ( v 1 , v 2 ) is gi ven by ℓ ( x, y , z , y 1 , y 2 ) = 1 2  ( v 1 ) 2 + ( v 2 ) 2 ( y 2 + 1)  . Then, the nonholo nomic equa tio ns for the constrained particle are given b y ˙ v 1 = 0 , ˙ v 2 = − y 1 + y 2 v 1 v 2 (8) together with the admissibility co n ditions ˙ x = − y v 2 , ˙ y = v 1 and ˙ z = v 2 . Then these equa tions define a time - continuo us flow F D t : D → D , i.e. F t (( q (0) , v (0))) = ( q ( t ) , v ( t )) , where q ( t ) = ( x ( t ) , y ( t ) , z ( t )) T and v ( t ) = ( v 1 ( t ) , v 2 ( t )) T , ( q (0) , v (0)) ∈ D . The previous systems can b e integrated exp licitly , and solutions are g iven b y: x ( t ) = x 0 − c 2 c 1 p ( y 0 + c 1 t ) 2 + 1 , y ( t ) = c 1 t + y 0 z ( t ) = c 2 2 c 1  ( y 0 + c 1 t ) p ( y 0 + c 1 t ) 2 + 1 + sinh − 1 ( y 0 + c 1 t )  − z 0 c 2 , (9) v 1 ( t ) = c 1 , v 2 ( t ) = c 2 p ( y 0 + c 1 t ) 2 + 1 , for x 0 , y 0 , z 0 , c 1 , c 2 constants to be determined b y the in itial condition s. Remark 2.7: Note that pr evious equations have a singu - larity at c 1 = 0 . The constan t c 1 arrises fro m the equatio n for ˙ v 1 . If c 1 = 0 , and ther efore v 1 ( t ) = 0 , then the solu tion for the system o f equ ations is given by x ( t ) = − y 0 v 2 0 t + x 0 , y ( t ) = y 0 , z ( t ) = v 2 0 t + z 0 , v 2 ( t ) = v 2 0 , wh ere x 0 , y 0 , z 0 , v 2 0 are constants. ⋄ I I I . O P T I M A L T R A J E C T O RY T R AC K I N G P RO B L E M The p u rpose o f this section is to present th e tra cking problem for no nholon omic systems as an optim a l con trol problem . The objective is the track ing of a suitable refer ence trajectory Υ ( t ) for a me c hanical system with nonholo n omic velocity constrain ts as describ ed in the p revious section . It is assumed tha t Υ( t ) ∈ D . W e will an alyze the case wh en the dimension of the inputs set o r control d istribution is equal to the rank of D . If th e rank of D is equ a l to the d imension o f the contro l distribution, the system will b e called a fully actuated nonholo nomic system . Definition 3.1: A solution o f a fully actuated n onholo - nomic pr oblem is an adm issible cur ve γ : I → D such that ∇ G D γ ( t ) γ ( t ) + g r ad G D V ( τ D ( γ ( t ))) ∈ Γ( τ D ) , or , equi valently , ∇ G D γ ( t ) γ ( t ) + g r ad G D V ( τ D ( γ ( t ))) = u A ( t ) e A ( τ D ( γ ( t ))) , where u A are the con tr ol in puts. Locally , th e above e q uations are g iven by ˙ q i = ρ i A v A (10) ˙ v A = − Γ A C B v C v B − ( G D ) AB ρ i B ( q ) ∂ V ∂ q i + u A . (11) As we mentioned in th e Intr oduction , For trajectory track - ing, the usu al appro a c h o f stabilization of error dynam ics [19], [22], [23], [25] cannot be utilized f or nonholon omic systems becau se the closed loop trajectory v iolates Brockett’ s condition . A c o mmon app roach to trajector y tracking for nonho lonomic systems found in the literatu re is the back - stepping pr o cedure [1 5], [18]. This approa ch is done on a per example basis, in particu lar , mobile robo ts or u nycicle models. I n [15], [18] the error dynam ic s of the unicycle model is shown to be in strict feedback form. There a f ter , integrator backsteppin g is employed to choo se an ap propr iate L yapunov fu nction f or stabilization of the error dynamics. This err or dyn a mics does n ot ev olve on the con strained mani- fold (un like ou r appro ach). Th erefore, Bro ckett’ s condition is not v io lated. Howe ver, since ρ i A ( q ) is unknown in a general framework, the approach can not be generalized to solve the tracking problem fo r a ge neral no nholon o mic system with our method and then back stepping needs to be studied for each system. So we propose a new ap p roach to co nsider track in g problem as an optim al control problem and we call th is optimal trac king . In the following, we shall assum e that all the control systems u nder c onsideration ar e co ntrollable in the configu- ration space, that is, for any two points q 0 and q f in the configur ation space Q , there exists a n admissible control u ( t ) de fined on the control manif old U ⊆ R n such that the system with in itial condition q 0 reaches the point q f at time T (see [2] for mor e details). Given a cost fun ction C : D × U → R the optimal contr o l pr oblem consists of finding an adm issible curve γ : I → D wh ic h is a solutio n of the fully actuated nonh olonom ic prob lem given initial and final boun d ary conditio ns on D an d minimizing the co st function al J ( γ ( t ) , u ( t )) := Z T 0 C ( γ ( t ) , u ( t )) dt. For trajec to ry tracking of a non holono mic system we consider the follo wing p r oblem Problem (optimal tr ajectory tracking) : Given a ref er- ence trajectory γ r ( t ) = ( q r ( t ) , v r ( t )) on D , find an ad- missible curve γ ( t ) ∈ D , solving (1 0)-(11), with prescribed bound ary con ditions on D and minimizin g th e cost functiona l J ( γ ( t )) = 1 2 Z T 0  || γ ( t ) − γ r ( t ) || 2 + ǫ || u A || 2  dt + ω Φ( γ ( T )) = 1 2 Z T 0  || q i ( t ) − q i r ( t ) || 2 + || v A ( t ) − v A r ( t ) || 2 + ǫ || u A || 2  dt + ω Φ( γ ( T )) where ǫ > 0 is a regularization parame ter , Φ : T Q → R is a termin al cost (Maye r term), ω > 0 is a weigh t for the term inal co st. C and Φ are assumed to be continu ously differentiable fu nctions, and the final state γ ( T ) is req u ired to fulfill a co nstraint r ( γ ( T ) , γ r ) = 0 with r : D × D → R d and γ r ∈ D giv en. The interval length T may either b e fixed, or appear as degree of freedom in th e o ptimization problem. In this work we restrict to the case when T is fixed. Remark 3.2: Note that if ǫ = 0 then the optimal contr ol problem turn s in to a singu lar optimal contro l p roblem (see [21] Section 3 . 2 ) ⋄ . I V . N E C E S S A RY C O N D I T I O N S F O R O P T I M A L I T Y In this section we ap ply Po ntryagin’ s m in imization prin- ciple to the optimal tra cking problem. T h e Hamiltonian H : T ∗ D × U → R for the problem is given by H ( q , v , λ, µ, u ) = J ( q i , v A , u A ) + λ i ρ i A ( q ) v A (12) + µ A ˙ v A ( q i , v A , u A ) where ˙ v A comes from eq uation ( 11). Note that λ i and µ A are th e costate v ariables or Lagrange multipliers. The last two term s in (12) correspond s with the nonh olonom ic dynamics giv e n in equatio ns (3) and (4) paired with the costate variables, wh ich represents the standard constructio n of the Hamiltonian f or the PM P . Also note that H is d e fined on a su bset of T ∗ ( T Q ) . Denote by t 7→ u ⋆ ( t ) a cu rve that satisfies along a trajectory t 7→ ( q ( t ) , v ( t ) , λ ( t ) , µ ( t )) ∈ T ∗ D , H opt ( q , v , λ, µ, u ⋆ ) = min u ∈U H ( q , v , λ, µ, u ) , then u ∗ may be d etermined implicitly a s a func tio n of ( q ( t ) , v ( t ) , λ ( t ) , µ ( t )) ∈ T ∗ D using the p revious equ ation and then we ma y define the op tim al Hamiltonian H opt : T ∗ D → R by p rescribing the con trol u as u ⋆ . Giv en that u ⋆ minimizes H , th en u ⋆ is a critical point for H and may b e u niquely determined by ∂ H ∂ u ( q ( t ) , v ( t ) , λ ( t ) , µ ( t ) , u ⋆ ( t )) = 0 , t ∈ [0 , T ] . (13 ) The PMP app lied to our particu la r proble m gives th e following necessary conditio n s • Stationary condition: from equa tio n (13) µ A = − ǫu A , • State equation: Equations (10) and (11), • Adjoint equations (or co state equ ations): − ˙ λ i = ∂ H ∂ q i = ( q i − q i r ) + λ i ∂ ρ i A ( q ) ∂ q i + µ A ∂ ˙ v A ∂ q i , − ˙ µ A = ∂ H ∂ v A = ( v A − v A r ) + λ i ρ i A ( q ) + µ A ∂ ˙ v A ∂ v A , • Constraint induced by term inal c o st: r ( γ ( T ) , γ r ) = 0 , • Boundar y cond itions: γ (0) := ( q (0) , v (0)) ∈ D , ∂ Φ ∂ q i ( γ ( T )) = λ i ( T ) , ∂ Φ ∂ v A ( γ ( T )) = µ A ( T ) . A. Optimal trajectory tracking for the nonho lonomic particle Consider the situation of Exam ple II -C. Let γ r be the r ef- erence trajec to ry , γ r = ( x r ( t ) , y r ( t ) , z r ( t ) , v 1 ,r , v 2 ,r ) wh ich follows th e constraint ˙ x r = y r ˙ z r at all t and the dy namical equations for the nonho lonomic particle. W e wish to control the velocity o f the nonholon o mic particle. W e add th e n control inputs in the fiber c oordinate s v 1 and v 2 . Theref o re the control dynamical system to study is given by ˙ v 1 = u 1 , ˙ v 2 = u 2 − y 1 + y 2 v 1 v 2 (14) together with the admissibility co n ditions ˙ x = − y v 2 , ˙ y = v 1 and ˙ z = v 2 . The co st fun ction C : D × U → R f or the optimal c ontrol problem is g i ven by C ( q , v , u ) = 1 2  || x − x r || 2 + || y − y r || 2 + || z − z r || 2 + || v 1 − v 1 r || 2 + || v 2 − v 2 r || 2 + ǫ (( u 1 ) 2 + ( u 2 ) 2 )  . and the term inal c ost function is given by Φ( x, y , z , v 1 , v 2 ) = || x ( T ) − x r ( T ) || 2 + || y ( T ) − y r ( T ) || 2 + || z ( T ) − z r ( T ) || 2 + || v 1 ( T ) − v 1 r ( T ) || 2 + || v 2 ( T ) − v 2 r ( T ) || 2 with T ∈ R + fixed. The Hamiltonian fo r the PMP is gi ven a s H ( q , v , λ, µ, u ) = 1 2  || x − x r || 2 + || y − y r || 2 + || z − z r || 2 + || v 1 − v 1 r || 2 + || v 2 − v 2 r || 2 + ǫ ( u 1 ) 2 + ǫ ( u 2 ) 2  − λ 1 y v 2 + λ 2 v 1 + λ 3 v 2 + µ 1 u 1 + µ 2  u 2 − y 1 + y 2 v 1 v 2  . In order for u ( t ) to be the optimal control we employ the stationary c o ndition. Ther efore, u ⋆ 1 = − µ 1 ǫ and u ⋆ 2 = − µ 2 ǫ . T he final co st is given by Φ( γ ( T )) = || γ ( T ) − γ r || 2 which in duces the constraint r ( γ ( T ) , γ r ) = || x ( T ) − x r || 2 + || y ( T ) − y r || 2 + || z ( T ) − z r || 2 + || v 1 ( T ) − v 1 r || 2 + || v 2 ( T ) − v 2 r || 2 = 0 . Finally , the optim al Ham ilto nian H opt is gi ven by H ( q , v , λ ) = 1 2  || x − x r || 2 + || y − y r || 2 + || z − z r || 2 + || v 1 − v 1 r || 2 + || v 2 − v 2 r || 2  − λ 1 y v 2 + λ 2 v 1 + λ 3 v 2 . The a d joint eq uations are ˙ λ 1 = − ( x − x r ) , ˙ λ 3 = − ( z − z r ) , ˙ λ 2 = λ 1 v 2 − ( y − y r ) + ǫv 1 v 2 µ 2  y 2 − 1 ( y 2 + 1) 2  , (15) ˙ µ 1 = − λ 2 − ( v 1 − v 1 r ) − µ 2 y 1 + y 2 v 2 , ˙ µ 2 = − λ 3 + λ 1 y − ( v 2 − v r 2 ) − µ 2 y 1 + y 2 v 1 . The state equations were gi ven in Example II-C in equation (8) togeth er with the a d missibility cond itions. Bou ndary con- ditions must satisfy the constraints in o rder for th e trajectory to ev olve on D , that is ˙ x 0 + y 0 ˙ z 0 = 0 where ˙ x 0 , y 0 , ˙ z 0 denotes the b oundar y conditio ns for the variables ˙ x , y and ˙ z resp ecti vely . B. Numerical r esu lts W e now test with nu merical simulatio ns how the p roposed methods w ork. Denote F λ µ : [0 , T ] × T ∗ D → T ∗ D , the in tegral flow giv en b y equations (15) on T ∗ D and γ (0) ∈ D the initial condition f or the state dynam ics. The initial guess fo r th e initial condition o f the costate variables is denoted by α = F λ µ (0) . W e wish to find the in itial condition of the costates for which F λ µ ( T , γ (0) , α ) = (0 1 × 5 ) T . The goal is to find the root o f the polyno mial F λ µ ( α ) =      λ 1 ( T , γ (0) , α ) + ω ( x ( T , α ) − x r ( T )) λ 2 ( T , γ (0) , α ) + ω ( y ( T , α ) − y r ( T )) λ 3 ( T , γ (0) , α ) + ω ( z ( T , α ) − z r ( T )) µ 1 ( T , α ) µ 2 ( T , α )      where T ∈ R + is the fina l time, ω ∈ R + is a weight for the terminal cost an d F λ µ ( τ , γ (0) , p 0 ) is the flow of the a d joint equations (1 5) starting at ( γ (0) , p 0 ) . The root finder used in both situations was the fsolve routine in MA T LAB. For the intial condition γ (0) =  0 . 5 0 . 2 0 . 7; 0 . 5 0 . 4  and ref erence tra jectory γ r ( t ) = ( 1 , 0 , t + 1 , 0 , 1) , p 0 = 0 1 × 5 , T = 4 and ǫ = 7 we exhibit the results in Figure IV -B. 0.4 0.5 0.6 0.7 0.8 0.9 1 −0.4 −0.2 0 0.2 0.4 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 x y z γ (t) γ r (t) 0 0.5 1 1.5 2 2.5 3 3.5 4 0.4 0.5 0.6 0.7 0.8 0.9 1 t x(t) x r (t) 0 0.5 1 1.5 2 2.5 3 3.5 4 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 t y(t) y r (t) 0 0.5 1 1.5 2 2.5 3 3.5 4 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 t z(t) z r (t) 0 0.5 1 1.5 2 2.5 3 3.5 4 −0.08 −0.06 −0.04 −0.02 0 0.02 0.04 0.06 0.08 t u 1 0 0.5 1 1.5 2 2.5 3 3.5 4 −0.2 −0.18 −0.16 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 u 2 t Fig. 1. Arbitrary reference trajec tory: Trajec tories minimizing the cost functio n J , e volvi ng on D and tracki ng the reference trajectory γ r in time T and control inp uts Acknowledgments: The au thors wish to thank Prof . Ravi Banav ar for f ruitful discussions along th e development of this work . R E F E R E N C E S [1] M. Barbero Li ˜ nan, M. de Le ´ on, J.C. Marrero, D. Mart ´ ın de Diego and M. Mu ˜ noz L ecanda . Kinematic reduc tion and the Hamilton-J acobi equatio n. J. Geometri c Mecha nics, Issue 3 (2012), 207–237. [2] A. M. Bloch. Nonholonomic Mechanic s and Contr ol. Interdisciplin ary Applied Mathemat ics Series, 24, Springer -V erlag, New Y ork (2003). [3] A. M. Bloch. Stabiliz abili ty of nonholonomic control systems. Auto- matica, vol. 28, no. 2, pp. 431-435, 1992. [4] A. Bloch, L . Colombo, R. Gupta and D. Mart´ ı n de Diego. A Geometric Approach to the Optimal Control of Nonholo nomic Mec hanica l Sys- tems. Analysis and Geometry in Contr ol Theory and its Applicat ions . INdAM series. V ol 12. 2015. [5] A. M. Bloch and N. H. McCl amroch. Control of mechanic al systems with classical nonho lonomic const raints. Proc. IEEE Conference De- cision and Control, 1989, T ampa, FL , pp. 201-205. [6] A. Bl och, N. McClamroc h, and M. Re yhanoglu . Controlla bility and stabili zabil ity propert ies of a nonholonomic contro l system. Proc. IEEE Conferenc e on Decision and Control, 1990, 1312-1314. [7] A. M Bl och, M. Reyhanoglu , and N. H. McClamroch. Control and stabili zatio n of nonholonomic dynamic systems. IEEE Transacti ons on Automatic control, 37(11):1746-17 57, 1992 [8] A. M. Bloch, J.E. Marsden and D. Zenko v . Quasiv elocities and symmetries in non-hol onomic systems. Dynamical Systems, 24 (2), (2009), 187–222. [9] R. W . Brocke tt. Control theory and singular Riemannia n geometry . Ne w Directions in Applied Mathematics, P . J . Hilton and G. S. Y oung, Eds. New Y ork Springer -V erlag, 1982. [10] R. W . Brocket . Asymptoti c stab ility and fee dback sta biliza tion. Dif- ferenti al Geomet ric Control Theory . R. W . Brocke tt, R. S. Millman, and H. J. Sussmann, Eds. Boston, MA: Birkhauser , 1983. [11] F . Bullo and A. Le wis. Geometric Control of Mechanic al Systems: Modelin g, Analy sis, and Design for Simple Mechanical Control Sys- tems. T exts in Applied Mathematic s, Springer V erlag 2005. [12] L. Colombo. Geometric and numerical m ethods for optimal control of mechanic al s ystems. PhD thesis, Instit uto de Cienc ias Matem ´ atica s, ICMA T (CSICU AM-UCM-UC3M), 2014. [13] L. Colombo. A vari ationa l-geometr ic appro ach for the optimal control of nonholonomic systems. Internation al Jou rnal of Dynamics and Contr ol . V ol 6 (2), 652-662, 2018. [14] L Colombo, R Gupta, A Bloch, DM de Diego. V ariational dis- cretiz ation for optimal control problems of nonholonomic mechanica l systems. Decision and Control (CDC), 2015 IEEE 54th Annual Conferen ce on, 4047-4052. [15] H Hajieghr ary , D Kularat ne, M. A. Hsieh. Diff erenti al Geometric Approach to Traj ectory Planning: Cooperat i ve Tran sport by a T eam of Autonomous Marine V ehicles. arXiv preprint arXi v:1805.00959 . [16] J. Cort ´ es. Geometric, control, and numerica l aspect s of nonholonomic systems. Lecture notes in Mathematics, Spring er V erlag, 2002. [17] J. Cort ´ es and E. Mart ´ ınez E. Mec hanical contr ol systems on Lie alge br oids . IMA J. Math. Control. Inf. 21, 457-492, 2004. [18] Z.-P . Jinag and H. Nijmeije r . Track ing control of mobile robots: A case study in backstepping. Automatica, 33(7): 1393-1399, 1997. [19] D. Koditsc hek. The applic ation of total energy as a Lya punov functio n for mechanica l control systems. Contemporary Math. 97-131, 1989 [20] F . Lewis. Optimal con tr ol . John Wi le y & Sons, Inc, 1986. [21] J. L ¨ o ber . Optimal trajec tory tr ack ing . PhD. T hesis. T U Berlin, 2015. [22] A. Nayak and R. N. Banav ar . On Almost-Global Tracking for a Certain Class of Simple Mechanica l Systems . T o appear in IEEE Transacti ons on Automatic Control, 2018. [23] A. Nayak, R. N. Banav ar and D. H. S. Maithrip ala. Almost-globa l tracki ng for a rigid body with internal rotors. European Journal of Control V ol 42, 59-66, 2018. [24] A. Saccon, J. Hauser , A. P . Aguilar . Exploration of Kinematic Optimal Control on the Lie Group S O (3) . 8th IF AC Symposium on Nonlinea r Control Systems. 1302-1307, 2010. [25] A. Sanyal, N. Nordkvist, M. Chyba. An almost global track ing control scheme for maneuverabl e autonomous vehic les and its discretizat ion. IEEE Tran sactions on Automatic control, 56.2:4 57-462, 2011. [26] S Ober -Blobaum. Galerki n va riatio nal integrat ors and modified sym- plect ic RungeK utta method s. IMA Journal of Numerical Analysis 37 (1), 375-406. [27] A. Zuyev . Exponential stabilizat ion of nonholonomi c s ystems by means of osci llati ng controls. S IAM J. on Control and Optimiza tion, 54(3):1678 1696, 2016.