Switched Passivity—Based Control of the Chaplygin Sleigh

10th IFAC Symposium on Nonlinear Control Systems 10th IFAC Symposium on Nonlinear Control Systems August 23-25, 2016. Monterey, California, USA 10th IFAC IFAC Symposium on Nonlinear Nonlinear Control Systems 10th Symposium on Control Systems August 23-25, 2016. Monterey, California, USA Available August 23-25, 2016. Monterey, California, USA August 23-25, 2016. Monterey, California, USA online at www.sciencedirect.com

ScienceDirect IFAC-PapersOnLine 49-18 (2016) 1012–1017

Switched Switched Switched Switched

Passivity–Based Control Passivity–Based Control Passivity–Based Control Passivity–Based Control Chaplygin Sleigh Chaplygin Sleigh Chaplygin Sleigh Chaplygin Sleigh

of of of of

the the the the

∗ Joel Ferguson ∗∗ Alejandro Donaire ∗∗ ∗∗ Richard H. Middleton ∗ Joel Ferguson Donaire H. Middleton ∗ ∗∗ ∗ ∗ Alejandro ∗∗ Richard Joel Ferguson Alejandro Donaire Richard H. Middleton Joel Ferguson Alejandro Donaire Richard H. Middleton ∗ ∗ ∗ School of Electrical Engineering and Computer Science and PRC of Electrical Engineering and Computer Science and PRC ∗ ∗ School School of Electrical Computer Science and PRC CDSC, The of Newcastle,and Callaghan, NSW 2308, Australia School of University Electrical Engineering Engineering and Computer Science and PRC CDSC, The University of Newcastle, Callaghan, NSW 2308, Australia CDSC, The University of Newcastle, Callaghan, NSW 2308, Australia (e-mail: [email protected]; CDSC, The University of Newcastle, Callaghan, NSW 2308, Australia (e-mail: (e-mail: [email protected]; [email protected]; [email protected]) (e-mail: [email protected]; [email protected]) ∗∗ [email protected]) University of Naples Federico II, Italy, and The [email protected]) ∗∗ Prisma Lab, Lab, University of Naples Federico II, Italy, and The ∗∗ ∗∗ Prisma Prisma Lab, of Federico II, of Newcastle, Prisma University Lab, University University of Naples NaplesAustralia Federico (e-mail: II, Italy, Italy, and and The The University of Newcastle, Australia (e-mail: University of Newcastle, Australia (e-mail: [email protected]) University of Newcastle, Australia (e-mail: [email protected]) [email protected]) [email protected])

Abstract: In this paper, a switched controller for the Chaplygin Sleigh system based on passivAbstract: In this paper, a switched controller for the Chaplygin Sleigh system based on passivAbstract: In paper, a switched for Chaplygin Sleigh system passivity and energy shaping The Chaplygin cannot be asymptotically stabilised Abstract: In this this paper,is apresented. switched controller controller for the thesleigh Chaplygin Sleigh system based based on on passivity and energy shaping is presented. The Chaplygin sleigh cannot be asymptotically stabilised ity and energy shaping is presented. The Chaplygin sleigh cannot be asymptotically stabilised with a smooth control law, since Brockett’s necessarysleigh conditions forbesmooth stabilisation is not ity and energy shaping is presented. The Chaplygin cannot asymptotically stabilised with a smooth control law, since Brockett’s necessary conditions for smooth stabilisation is not with a smooth control law, since Brockett’s necessary conditions for smooth stabilisation is satisfied. To asymptotically stabilise the origin, two potential energy shaping control laws are with a smooth control law, since Brockett’s necessary conditions for smooth stabilisation is not not satisfied. To asymptotically stabilise the origin, two potential energy shaping control laws are satisfied. To To asymptotically stabilise the origin, origin, two two potential energy shapingmanifolds, control laws laws are developed that render the system asymptotically stable to two equilibrium which satisfied. asymptotically stabilise the potential energy shaping control are developed that render the system asymptotically stable to two equilibrium manifolds, which developed that render the system asymptotically stable to two equilibrium manifolds, which intersect atthat the render origin. the A switching strategy between the to energy shaping controllers is derived developed system asymptotically stable two equilibrium manifolds, which intersect at the origin. A switching strategy between the energy shaping controllers is derived intersect at switching strategy between the energy shaping controllers that ensures theorigin. systemA the intersection equilibrium intersect at the the origin. Aconverges switchingto strategy between of thethe energy shapingmanifolds. controllers is is derived derived that ensures the system converges to the intersection of the equilibrium manifolds. that ensures the system converges to the intersection of the equilibrium manifolds. that ensures the system converges to the intersection of the equilibrium manifolds. © 2016, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Constraints, Nonholonomic, Passivity Based Control, Path planning, Keywords: Constraints, Nonholonomic, Nonholonomic, Passivity Based Based Control, Path Path planning, Keywords: Constraints, Port-Hamiltonian. Keywords: Constraints, Nonholonomic, Passivity Passivity Based Control, Control, Path planning, planning, Port-Hamiltonian. Port-Hamiltonian. Port-Hamiltonian. 1. INTRODUCTION The stabilisation of mechanical system subject to Pfaf1. INTRODUCTION INTRODUCTION The stabilisation stabilisation of of mechanical mechanical system system subject subject to to PfafPfaf1. The fian constraints be achieved using smooth con1. INTRODUCTION The stabilisationcannot of mechanical system subject to Pfaffian constraints constraints cannot be achieved achieved using smooth confian cannot be using smooth controller since Brocketts’s necessary condition issmooth not satisfied fian constraints cannot be achieved using controller since since Brocketts’s Brocketts’s necessary necessary condition condition is is not not satisfied satisfied Mechanical systems are often subject to constraints that troller (Bloch, 2003). The case of stabilisation to an appropriately Mechanical systems are often subject to constraints that troller since Brocketts’s necessary condition is not satisfied Mechanical systems are often subject to that (Bloch, 2003). 2003). The The case case of stabilisation stabilisation to to an an appropriately must be satisfied along valid trajectory of the system. Mechanical systems areany often subject to constraints constraints that (Bloch, dimensioned of the configuration space is not must be be satisfied satisfied along any valid trajectory of the the system. system. (Bloch, 2003).sub-manifold The case of of stabilisation to an appropriately appropriately must along any dimensioned sub-manifold of the configuration space is is not Holonomic constraints are valid thosetrajectory that can of bethe expressed must be satisfied along any valid trajectory of system. dimensioned sub-manifold of the configuration space ruled out by this restriction and has been achieved by Holonomic constraints are those that can be expressed dimensioned sub-manifold of the configuration space is not not Holonomic are those that can be expressed ruled out out by by this this restriction restriction and and has has been been achieved achieved by as relations constraints between configuration variables and can be ruled Holonomic constraints are those that can be expressed by Bloch et al. (1992) and Donaire et al. (2015). as relations between configuration variables and can be ruled out by this restriction and has been achieved by as relations between configuration variables and can be Bloch et al. (1992) and Donaire et al. (2015). eliminated from the dynamic equations of motion via as relations from between configuration variables and can via be Bloch et al. (1992) and Donaire et al. (2015). eliminated the dynamic equations of motion Bloch et al. (1992) and Donaire et al. (2015). eliminated from the equations of via an appropriate of coordinates (Goldstein, 1965). this paper, we design a switched controller for the eliminated fromchoice the dynamic dynamic equations of motion motion via In an appropriate appropriate choice of coordinates coordinates (Goldstein, 1965). In this this paper, paper, we we design design aa switched switched controller controller for for the the an (Goldstein, 1965). Constraints are choice called of nonholonomic if they cannot be In Chaplygin sleigh asymptotic stability of an appropriate choice of coordinates (Goldstein, 1965). In this paper, wewhich designensures a switched controller for the Constraints are called nonholonomic if they cannot be Chaplygin sleigh which ensures asymptotic stability of Constraints called nonholonomic if they cannot be Chaplygin sleigh which ensures asymptotic stability of expressed as are relationships between configuration variables the state–space origin. This is achieved by switching two Constraints are called nonholonomic if they cannot be Chaplygin sleigh which ensures asymptotic stability of expressed as relationships between configuration variables the state–space origin. This is achieved by switching two expressed as relationships between configuration state–space origin. This achieved switching two alone (Goldstein, 1965). The Chaplygin sleigh is avariables classical the energy shaping control laws.is Each of theby energy shaping expressed as relationships between configuration variables the state–space origin. This is achieved by switching two alone (Goldstein, (Goldstein, 1965). 1965). The The Chaplygin sleigh sleigh is is aa classical classical energy energy shaping shaping control control laws. laws. Each Each of of the the energy energy shaping alone example of a nonholonomic system, subject a single controllers asymptotically stabilises a 1–dimensional sub– alone (Goldstein, 1965). The Chaplygin Chaplygin sleigh isto energy shaping control laws. Each of the energy shaping shaping example of aa nonholonomic nonholonomic system, subject subject toa classical a single single controllers controllers asymptotically stabilises a 1–dimensional 1–dimensional sub– example of system, to a asymptotically stabilises a nonholonomic constraint (Kolmanovsky and McClamroch, manifolds ofasymptotically the configuration space. aThe equilibrium sub– example of a nonholonomic system, subject to a single controllers stabilises 1–dimensional sub– nonholonomic constraint (Kolmanovsky and McClamroch, manifolds of the configuration space. The equilibrium sub– nonholonomic constraint (Kolmanovsky and McClamroch, manifolds of configuration The equilibrium sub– 1995; Bloch, 2003). arethe chosen such thatspace. they intersect at the origin. nonholonomic constraint (Kolmanovsky and McClamroch, manifolds of the configuration space. The equilibrium sub– 1995; Bloch, 2003). manifolds are chosen such that they intersect at the origin. 1995; Bloch, 2003). manifolds are chosen such that they intersect the origin. A switching strategy is proposed based on theat closed loop 1995; Bloch, 2003). manifolds are chosen such that they intersect at the origin. A switching switching strategy strategy is is proposed proposed based based on on the the closed closed loop loop Port-Hamiltonian (pH) systems are a generalisation of A energies arising from the energy shaping control laws. We Port-Hamiltonian (pH) systems are a generalisation of A switching strategy is proposed based on the closed Port-Hamiltonian systems aa generalisation of energies arising arising from from the the energy energy shaping shaping control control laws. laws.loop We both Hamiltonian (pH) dynamics andare port-based modelling Port-Hamiltonian (pH) systems are generalisation of energies We show that the switched controller ensures that trajectories both Hamiltonian dynamics and port-based modelling energies arising from the energy shaping control laws. We both Hamiltonian dynamics and port-based modelling show that that the the switched switched controller controller ensures ensures that that trajectories trajectories (van der Schaft and Jeltsema, 2014; Duindam et al., 2009). show both Hamiltonian dynamics and port-based modelling thethat system asymptotically converge to that the intersection (van der der Schaft Schaft and and Jeltsema, Jeltsema, 2014; 2014; Duindam Duindam et et al., al., 2009). 2009). of show the switched controller ensures trajectories (van of the system system asymptotically asymptotically converge converge to to the the intersection The formalism highlights physical ofal., a system (van der Schaft and Jeltsema, 2014; properties Duindam et 2009). of the two manifolds, which isconverge the origin. The formalism formalism highlights physical properties of aa system system of system asymptotically to the intersection intersection The highlights properties of the the two two manifolds, which is is the the origin. origin. by having explicit terms inphysical the dynamic modelof associated The formalism highlights physical properties of a system of manifolds, which by having explicit terms in the dynamic model associated of the two manifolds, which is the origin. by having explicit terms model associated with dissipation, and energy the sys- Lee (2007) addressed a similar problem by controlling by having explicit interconnection terms in in the the dynamic dynamic modelof with dissipation, interconnection and energy energy ofassociated the syssys- Lee (2007) (2007) addressed addressed aa similar similar problem problem by by controlling controlling with dissipation, interconnection and of the tem. A standard approach for stabilisation of mechanical the Chaplygin sleigh to a compact set, containing the with dissipation, interconnection and energy of the sys- Lee Lee (2007) addressed a similar problem by controlling tem. A A standard standard approach approach for for stabilisation stabilisation of of mechanical mechanical the Chaplygin Chaplygin sleigh to to a compact compact set, containing containing the tem. the sleigh a set, the systems is energyapproach shaping.forThis approachofproposes to origin, by switching between energy shaping controllers. tem. A standard stabilisation mechanical the Chaplygin sleigh to a compact set, containing the systems is energy shaping. This approach proposes to origin, by switching between energy shaping controllers. systems is energy shaping. This proposes to origin, by switching between energy shaping controllers. design a state–feedback control law approach that assigns an desired The proposed approach has the advantage of causing the systems is energy shaping. This approach proposes to origin, by switching between energy shaping controllers. design aa state–feedback state–feedback control control law law that that assigns assigns an an desired desired The The proposed proposed approach approach has has the the advantage advantage of of causing causing the design energy the closed–loop system. desiredan energy is Chaplygin system to approach origin asymptotically, design ato state–feedback control law This that assigns desired The proposed approach has thethe advantage of causing the the energy to the closed–loop system. This desired energy is Chaplygin system to approach the origin asymptotically, energy to system. This desired energy approach the origin asymptotically, constructed to closed–loop have a minimum at the desired equilibrium. rather thansystem to someto containing origin. energy to the the closed–loop system. This desired energy is is Chaplygin Chaplygin system toset approach the the origin asymptotically, constructed to have a minimum at the desired equilibrium. rather than to some set containing the origin. constructed have a minimum at desired equilibrium. constructed to to have minimum at the the equilibrium. rather rather than than to to some some set set containing containing the the origin. origin. Constraints that cana be described as adesired linear combination Constraints that can be described as a linear combination 2. MECHANICAL PORT-HAMILTONIAN SYSTEMS Constraints can described as combination of velocities that are called 2005). Pfaffian, 2. MECHANICAL MECHANICAL PORT-HAMILTONIAN PORT-HAMILTONIAN SYSTEMS SYSTEMS Constraints can be bePhaffian described(Choset, as aa linear linear combination 2. of velocities velocities that are called called Phaffian (Choset, 2005). Pfaffian, 2. MECHANICAL PORT-HAMILTONIAN SYSTEMS of are Phaffian (Choset, 2005). Pfaffian, nonholonomic constraints can be(Choset, eliminated fromPfaffian, the dyof velocities are called Phaffian 2005). nonholonomic constraints constraints can can be be eliminated eliminated from from the the dydy- Pfaffian constraints are those that can be expressed as a nonholonomic namics equations of motion of mechanical systems modnonholonomic constraints canofbemechanical eliminatedsystems from themoddy- Pfaffian Pfaffian constraints constraints are are those those that that can can be be expressed expressed as as aa namics equations equations of motion motion non-integrable linearare combination a systems velocity. Pfaffian constraints those thatof can be expressed as If namics of of mechanical systems modelled in equations the pH framework. The elimination is achieved namics of motion of mechanical systems modnon-integrable linear combination of a systems velocity. Ifa n elled in the pH framework. The elimination is achieved non-integrable linear combination of a systems velocity. , k linearly the systems configuration is given by q ∈ R non-integrable linear combination of a systems If elled the pH framework. The elimination achieved n velocity. If via a in momentum transformation, reducing theis dimension elled in the pH framework. The elimination is achieved the systems systems configuration is is given given by by q ∈ ∈ R Rn , k via aa momentum momentum transformation, transformation, reducing reducing the the dimension dimension independent k linearly linearly the Pfaffian constraints canbybeq as the systems configuration configuration is given q expressed ∈ Rn ,, k linearly via of the momentumtransformation, space (Maschke and Van der Schaft, independent via a momentum reducing the dimension Pfaffian constraints can be expressed as of the the momentum momentum space (Maschke (Maschke and Van Van der Schaft, Schaft, independent Pfaffian constraints can be expressed expressed as as (1) of ˙ = can 1994). 0, be A  (q)q of the momentum space space (Maschke and and Van der der Schaft, independent Pfaffian constraints ˙  1994). (q) q = 0, (1) A  (q)q ˙ 1994). = 0, (1) A 1994). (1) A (q)q˙ = 0, Copyright © 2016, 2016 IFAC 1030Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016 IFAC 1030 Copyright 2016 IFAC 1030 Peer review© of International Federation of Automatic Copyright ©under 2016 responsibility IFAC 1030Control. 10.1016/j.ifacol.2016.10.300

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, Joel USAFerguson et al. / IFAC-PapersOnLine 49-18 (2016) 1012–1017

y

where A(q) ∈ Rn×k . Pfaffian constraints can be eliminated from the dynamics of a mechanical system described as a pH system by via a momentum transformation (Maschke and Van der Schaft, 1994). After elimination, mechanical systems can be written as a pH system   ∂H       p˙ G(q) J(p, q) − D(p, q) −S (q)  ∂p  = + u   ∂H q˙ 0 S(q) 0 ∂q ∂H  y = G (q) ∂p 1  −1 H = p M (q)p + V(q), 2 (2) where q ∈ Rn is the configuration, p ∈ Rn−k is the momentum, u ∈ Rm is the input, y ∈ Rm is the output, M(q) = M (q) > 0 is the mass matrix, D(p, q) = D (p, q) ≥ 0 is the dissipation term, J(p, q) = −J (p, q) is the interconnection structure which contains the centrifugal and Coriolis terms, S(q) relates the momentum to the velocities, G(q) is the input mapping matrix and V(q) > 0 is the potential energy. The matrix dependencies on the states p, q will be omitted for the remainder of the paper. m, k, n are constants that satisfy m ≤ n − k ≤ n. Consider a new set of generalised coordinates z for the system (2) that are related to q via z = fz (q). The system dynamics, expressed in z are   ∂Hz       p˙ Gz Jz − Dz −Q  ∂p  =  ∂H  + 0 u z˙ Q 0 z ∂z (3)  ∂Hz y = Gz ∂p 1  −1 Hz = p Mz (z)p + Vz (z), 2  where Q = ∂∂qfz S|q=fz−1 (z) and (∗)z = (∗)|q=fz−1 (z) . 3. CHAPLYGIN SLEIGH SYSTEM The Chaplygin sleigh (figure 1) is subject to a single nonholonomic, Pfaffian constraint y˙ cos θ − x˙ sin θ = 0, (4) and exhibits a non-trivial interconnection matrix J. The dynamics of the Chaplygin sleigh can be described as a pH system of the form (2), where the constraint equation has been eliminated by the appropriate selection of momentum variables (Astolfi et al., 2010). One possible realisation of this system is given by     ml cos θ 0 0 p 2  J + ml2  S = sin θ 0 J=  ml 0 1 − p2 0 2    J+ ml d1 0 10 D= G= 01 0 d2   m 0 V(q) = 0, M= 0 J + ml2 where m is the mass of the sleigh, J is the rotational inertia about the centre of mass, d1 and d2 are the

1013

u l

θ

(x, y)

x

Fig. 1. The Chaplygin sleigh is fixed to the ground at the point (x, y). It is able to pivot about this point and move forwards in the u direction. The point (x, y) is constrained from moving in a direction perpendicular to u. The center of mass is indicated by the red spot and is a distance l from the point (x, y). linear viscous friction coefficients due to translational and rotation velocity respectively and V(q) is the potential energy of the system. Defining a new set of generalised coordinates,      cos θ sin θ 0 x z1 0 0 1 y , z = fz (q)  z2 = (5) − sin θ cos θ 0 θ z3 the Chaplygin sleigh can be described as a pH system of the form (3) with   1 z3 (6) Q= 0 1 . 0 −z1 4. MANIFOLD REGULATION In this section, two energy shaping controllers are presented for the regulation of the system to a 1-dimensional sub-manifolds of the configuration space. The two manifolds are parametrised by M1 = {z|z1 = 0, z3 = 0} and M2 = {z|z2 = 0, z3 = 0}, which intersect at the origin.

For a desired closed loop potential energy, Vd (z), the corresponding energy shaping controller for system (3) is  ∂Vd . (7) u = −G−1 z Q ∂z This control law results in the closed loop Hamiltonian H d = H + Vd .

If Vd is a positive and radially unbounded function, LaSalle’s invariance principle ensures that we will converge to some point in the configuration space satisfying p = p˙ = 0. Evaluating (3) at this limit results in the condition ∂Vd = 0, (8) Q ∂z which must be uniquely satisfied by the target manifold. A solution to this PDE is constructed by first choosing a change of coordinates:     w1 fw1 (z) w= = fw (z) = (9) fw2 (z) w2 where dim w1 = 2 and dim w2 = 1.

1031

IFAC NOLCOS 2016 1014 August 23-25, 2016. Monterey, California, Joel USAFerguson et al. / IFAC-PapersOnLine 49-18 (2016) 1012–1017

Lemma 1. Consider a partial transformation for (9) given by i fw (z)

= h(z) (10) g(z) j fw , (z) = h(z) where i, j are arbitrary integers between 1 and dim w such that i = j, h(·), g(·) are smooth, scalar valued i (z) is the ith component of fw (z). The functions and fw ∂f i ∂f j space spanned by ∂zw , ∂zw is the same as the space ∂g spanned by ∂h ∂z , ∂z except on the set defined by h(z) = 0. Proof.    i j ∂fw g(z) ∂h ∂fw ∂h 1 ∂g span , = span , − ∂z ∂z ∂z h(z) ∂z h2 (z) ∂z   ∂h ∂g , . = span ∂z ∂z (11) Lemma 1 will be used in the construction of fw1 (z) for each manifold. PDE (8), expressed in the w coordinates, takes the form    ∂fw ∂Vd  Q = 0. (12) ∂z ∂w z=fw−1 (w)

The coordinates w1 will be chosen such that w1 = 0 describes the target manifold in the new coordinates and the matrix    ∂fw1  Q (13) ∂z  −1 z=fw (w)

is full rank along the systems trajectories except possibly at the equilibrium manifold. The control law (7) with a potential function convex in w1 would then render the target manifold asymptotically stable. The remaining coordinate transformation fw2 (z) is then selected such that a convex potential function in w ensures convergence to the target manifold as well as boundedness within the manifold. PDE (12) can be rearranged to  −1  ∂fw1 ∂fw2 ∂Vd  ∂Vd = − Q Q . (14) ∂w1 ∂z ∂z ∂w2  −1

Taking w2 = z2 +

results in  w  1:2 ∂Vd ∂Vd − = , w1:1 ∂w1 ∂w 2 0

4.1 Regulation of M1 The first target manifold is defined by z1 = 0, z3 = 0.  Consider the choice fw1 (z) = [z1 z3 ] , which results in the matrix (13) being full rank everywhere except z1 = 0. Using lemma 1, the choice is updated to be   z1 fw1 (z) = z3 , (15) z1 which ensures that the matrix (13) is full rank everywhere that the transformation is defined. To ensure boundedness of z2 , fw2 (z) must be defined using (14),   z 3 z3  1− 2 ∂fw2 ∂Vd  ∂Vd z z =− . (16) 1 1 ∂w1 ∂z ∂w2 z=fw−1 (w) 0 −1 z1

(17)

where w1:i refers to the ith element of w1 . If Vd is convex in w1:2 , the second line of this equation implies that w1:2 = zz31 = 0. Rearranging (17) results in ∂Vd ∂Vd (18) = −w1:2 = 0. w1:1 ∂w1:1 ∂w2 If Vd is chosen appropriately then the only solution to ∂Vd w1:1 ∂w = 0 is w1:1 = 0. The potential energy function 1 for the regulation to M1 is chosen to be   2  2 1 z3   1 z12 + 5 z3 z2 + + , if z1 = 0. 2 z1 2 z1 Vd1 = 2   1 z2, if z1 = 0. 2 2 (19) Using (19), a potential energy shaping control law can is defined everywhere except z1 = 0 as per (7). On the set z1 = 0, the control is defined to be zero so that the dynamics are invariant on the target manifold. This combination results  in the control law ∂Vd1  −G−1 Q , if z1 = 0. u1 = (20) ∂z 0, if z = 0. 1

Lemma 2. Consider the Chaplygin sleigh system together with the control law (20). The potential energy function (19) is continuous with respect to time.

Proof. Consider the ratio zz31 (t) (t) in the case where limt→T z1 (t) = 0 for some finite T : z3 (t) z˙3 (t) = lim lim t→T z1 (t) t→T z˙1 (t) z˙2 (t) = lim −z1 (t) t→T z˙1 (t) z2 (t)−z2 (T ) t−T 1 (t)−z1 (T ) t−T

= lim −z1 (t) z t→T

(21)

z2 (t) − z2 (T ) z1 (t) = lim −z2 (t) + z2 (T )

= lim −z1 (t) t→T

z=fw (w)

The choice of coordinates for w2 and the potential function Vd (w1 , w2 ) should be chosen such that (14) is uniquely satisfied by w1 = 0.

z3 z1

t→T

= 0, where we have used L’Hˆopital’s rule and the system dynamics (6). Substituting (21) into (19) reveals that the potential energy is continuous in time. Lemma 2 establishes the behaviour of the discontinuous potential function (19) as the system approaches the singularity. The following lemma shows that the system can only spend some non-trivial amount if time in M1 if it has in fact converged. Lemma 3. Consider the Chaplygin sleigh system together with the control law (20). If the system reaches the singularity z1 = 0 at some finite time T and remains there for some finite time, then the configuration z =  [0 z2 (T ) 0] is positively invariant. Proof. As z1 (T ) = 0 and z1 (t) = 0 for some finite time, we can define an open set U = (T, T  ) such that z1 (t) = 0

1032

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, Joel USAFerguson et al. / IFAC-PapersOnLine 49-18 (2016) 1012–1017

for all t ∈ U . As z1 (t) is constant on U , z˙1 (t) = 0 for all t ∈ U . By (6), z˙3 (t) = 0 for all t ∈ U . As z3 (T ) = 0, z3 (t) = 0 for all t ∈ U . Considering the dynamics (6), ∂H 1 = m p1 (t) = 0 for all z3 (t) = 0 implies that z˙1 (t) = ∂p 1 t ∈ U . As p1 (t) is constant on this interval, p˙1 (t) for all t ∈ U . As there is no control input on the set z1 = 0 ml 2 (20), p˙1 (t) = (J+ml 2 )2 p2 = 0, which implies that p2 (t) = 0 for all t ∈ U . As p2 (t) is constant on U , p˙2 (t) = 0 and z˙2 (t) = 0 for all t ∈ U , hence the system in invariant. In the case that the system reaches M1 in finite time and passes through, the potential energy (19) is continuous by lemma 2. Alternatively, if the system M1 and does not immediately pass through, the point at which the system entered M1 will be positively invariant by lemma 3. 4.2 Regulation of M2 The second target manifold is defined by z2 = 0, z3 = 0.  Taking fw1 (z) = [z2 z1 z2 + z3 ] , results in the matrix (13) being full rank everywhere except z2 = 0. Using lemma 1, the choice is updated to be     z2 z2 z (22) fw1 (z) = z1 z2 + z3 = z + 3 , 1 z2 z2 which ensures that the matrix (13) is full rank everywhere except possibly at z2 = 0. To ensure boundedness of z1 , fw2 (z) must be defined using (14),   z3  z1 + 1 −z1 ∂fw2 ∂Vd  ∂Vd 2 z2 = − z2 . (23) ∂w1 ∂z ∂w2 z=fw−1 (w) 1 0 0 Taking fw2 (z) =

z3 z2

results in  w  1:2 ∂Vd ∂Vd − = . w1:1 ∂w1 ∂w 2 0

(24)

If Vd is convex in w1:2 , the second line of (24) implies that w1:2 = z1 + zz32 = 0. Equation (24) can be rearranged to ∂Vd ∂Vd (25) = −w1:2 = 0. ∂w1:1 ∂w2 If Vd is chosen appropriately then the only solution to ∂Vd w1:1 ∂w = 0 is w1:1 = 0. 1 w1:1

The potential energy function for the regulation to M2 is chosen to be    2 2 1 z3   1 z22 + 1 z1 + z3 + , if z2 = 0. 2 z2 2 z2 Vd2 = 2   1 z2, if z2 = 0. 2 1 (26) Using (26), a potential energy shaping control law can be defined everywhere except z2 = 0 as per (7). On the set z2 = 0, the control is defined to be zero so that the dynamics are invariant to the target manifold. This combination results  in the control law ∂Vd2  , if z2 = 0. −G−1 Q u2 = (27) ∂z 0, if z = 0. 2

Lemma 4. Consider the Chaplygin sleigh system together with the control law (27). The potential energy function (26) is continuous with respect to time.

1015

Proof. Consider the case where the system converges to z2 = 0 in some finite time T . The limiting value of zz32 (t) (t) is z3 (t) z˙3 (t) = lim = lim −z1 (t), (28) z2 (t) t→T z˙2 (t) t→T where we have used L’Hˆopital’s rule and the system dynamics (6). Substituting (28) into (26) reveals that the potential energy is continuous. lim

t→T

Lemma 4 establishes the behaviour of the discontinuous potential function (26) as the system approaches the singularity. Once the system reaches the singularity there are two possibilities, either the system remains at z2 = 0 for some finite time or the system immediately passes through. Lemma 5. Consider the Chaplygin sleigh system together with the control law (27). If the system reaches the singularity z2 = 0 at some finite time T and remains there for some finite time, then M2 is positively invariant. Proof. Using similar arguments to the proof of lemma 3, dynamics of the system reduce to       ∂H p˙1 −d1 −1  (29) = ∂p1  , 1 0 z˙1 0 which are invariant on M2 .

In the case that the system reaches M2 in finite time and passes through, the potential energy (26) is continuous by lemma 4. Alternatively, if the system reaches M2 and does not immediately pass through, M2 will be positively invariant by lemma 5. Remark 1. It is possible for the potential function (26) to increase on M2 if the system converges to M2 with z˙1 = 0 and remains positively invariant on M2 . Remark 2. The closed loop dynamics generated by the potential functions (19) and (26) are smooth and locally Lipschitz on any open set, excluding their respective singularities. This ensures the existence and uniqueness of solutions away from the singularities. Existence and uniqueness of solutions passing through the singularities has not been shown, which is considered beyond the scope of this paper. 5. SWITCHING LAW In this section a switching law is developed such that switching between the control laws (20) and (27) ensures that the Chaplygin sleigh converges towards the origin. For the remainder of this section let Σd1 and Σd2 denote the closed loop dynamics resulting from the control laws (20) and (27) respectively. These dynamics have closed loop potential energies given by (19) and (26) respectively. The switching law for the Chaplygin system is based on the following lemma: Lemma 6. Consider the closed–loop systems Σd1 and Σd2 . Evaluating the potential functions (19) and (26) along the trajectories of Σd1 results in lim Vd1 (t) = lim Vd2 (t). t→∞

t→∞

The same result if obtained by evaluating Vd1 and Vd2 along the trajectories of Σd2 .

1033

IFAC NOLCOS 2016 1016 August 23-25, 2016. Monterey, California, Joel USAFerguson et al. / IFAC-PapersOnLine 49-18 (2016) 1012–1017

Proof. Consider the case when Σd1 is active. From section 4.1, z1 → 0, z3 → 0 and Vd1 → 12 z22 . Substitute these limiting values into the potential field Vd2 results in Vd2 → 1 2 2 z2 . Thus, both Vd1 and Vd2 are convergent towards the same value along Σd1 . By similar calculation, Vd1 and Vd2 are both convergent to 12 z12 along Σd2 . Σd1 and Σd2 are switched according the switching law in table 1, where d(·, ·) is the euclidean norm. 1 2

3

4

Switching Law Pick i, j ∈ {1, 2} with i = j such that Hdi ≤ Hdj . Choose the controller parameters a1 , a2 ∈ (0, 1) Activate Σdi and set ts to be the time of activation. Operate Σdi until |Hdj (t) − Hdi (t)| ≤ a1 |Hdi (ts ) − Hdi (t)|, d(z(t), Mi ) ≤ a2 d(z(ts ), Mi ) and t ≥ ts +, for some constant  > 0. If Σd2 is active and z(t) ∈ M2 , wait for  seconds. If z(t) is still within M2 , switch to Σd1 and no further switches occur. If z(t) is no longer within M2 , go to step 2. Swap i, j and go to step 2

Table 1. Two cases must be considered to establish the stability of the switched system. In case A, we consider the scenario that the system never becomes positively invariant to M2 along Σ2 . In case B, we consider the case where M2 does become positively invariant along Σ2 . 5.1 Case A Along the trajectories of Σdi , Hdi is non-increasing except possibly in the case we Σd2 is active and z ∈ M2 . The following lemma shows that, with the exception of the aforementioned case, the active Hamiltonian must loose some non-trivial amount of energy within each switching cycle. Lemma 7. Consider the dynamics Σd1 and Σd2 , switched according to Table 1. Excluding the case where Σd1 is activated on M2 according to step 3, the energy strictly decreases within each switching cycle. + Proof. Suppose Σdi becomes active at time t+ k . If z(tk ) ∈ + + + Mi , d(z(tk ), Mi ) = 0 and Hdj (tk ) = Hdi (tk ) by Lemma 6. This would cause a switch to occur at t = t+ k + . + ) ∈ / M . As z(t ) ∈ / M , there exists Now suppose z(t+ i i k k ), M ) > 0. The dynamics Σ some δ − = d(z(t+ 1 di are k asymptotically stable to Mi , which implies that for any a2 ∈ (0, 1), there exists some finite time ∆tk such that + − δ + = d(z(t− where t− k+1 ), M1 ) = a2 δ k+1 = tk + ∆tk .  −1 Along the trajectories of Σdi , H˙ di = −p M DM−1 p. ˜ denote the top 2 × 2 block of (6). ˜  [z1 z2 ] and Q Let z Now, consider the change in Hamiltonian from time t+ k to t− k+1 ,  t− k+1 − + p M−1 DM−1 pdt Hdi (tk+1 ) − Hdi (tk ) = − t+ k

≤ −λm



t− k+1

t+ k

(30)

˜˙  z ˜˙ dt z

˜ − DQ ˜ −1 where λm is the minimum singular value of Q ˜˙ has been substituted in for p using the dynamics and z (3). From the Lagrange equation, the extremal solution ¨ ˜ = 0. The resulting path to this integral will satisfy z

˜(t+ from this equation is just a straight line connecting z k) − ˜(tk+1 ). All other paths would dissipate more energy and z from the system. Now consider the integral (30) with the constant velocity ˜(t+ ˜(t− z k)−z k) ˜˙ = z . ∆tk

(31)

˜(t+ ˜(t− As z k )−z k ) is non-zero and ∆tk is finite, (31) is nonzero. Thus, we conclude that the integral (30) is strictly less than zero, resulting in a strict decrease in energy as desired. Lemma 7 guarantees that the system energy will decrease between switching cycles. Next it is show that by following the switching law in Table 1, the energy must approach zero. Proposition 1. Consider the dynamics Σd1 and Σd2 , switched according to Table 1. Excluding the case where Σd1 is activated on M2 according to step 3, if the switches occur at times (t1 , t2 , t3 , . . . ), then the sequence (Hdi (t1 ), Hdj (t2 ), Hdi (t3 ), . . . ) is convergent to zero. Proof. Suppose Σi becomes active at some time tk , lemma 7 ensures that Hdi will loose a non-trivial amount of energy as it tends towards Mi , which implies that a1 |Hdi (tk ) − Hdi (t)| will become non-zero. The term |Hdi (t)−Hdj (t)| will tend towards zero along Σi by Lemma 6, thus, the first criteria for switching will be satisfied. As Σi tends towards Mi , d(z(t), Mi ) ≤ a2 d(z(0), Mi ) will be satisfied which means that a switching event will occur. By Lemma 7, Hdi (tk )−Hdi (t) will become strictly positive along the trajectories of Σi . Rearranging the switching inequality: Hdj (t) − Hdi (t) ≤ a1 (Hdi (tk ) − Hdi (t)) Hdj (t) < Hdi (tk ),

(32)

which implies that Hdj (tk+1 ) < Hdi (tk ). Thus, the sequence (Hdi (t1 ), Hdj (t2 ), Hdi (t3 ), . . . ) is strictly decreasing and bounded below. By the monotone convergence theorem, the sequence is convergent. Now consider, for the sake of contradiction, that the sequence (Hdi (t1 ), Hdj (t2 ), Hdi (t3 ), . . . ) tends towards some limit L = 0. The systems states are confined to the closed set Xi = {(p, z)|Hdi (p, z) < Hdi (0)} as H˙ di ≤ 0. As Xi is a closed set, there exists some (p∗ , z∗ ) ∈ Xi such that Hdi (p∗ , z∗ ) = L. Suppose the dynamics Σdi are initialised at the limit point, resulting in Hdi (0) = L. As this is a limit point of the system, Hdi (t) is constant which implies that H˙ di = −p M−1 RM−1 p = 0, which further implies that p = 0. As the dynamics are of the form (3), z is also constant. Since Σi is asymptotically stable to Mi , (p∗ , z∗ ) = (0, z∗ ) ∈ Mi . 



∂ H ∂ H As p˙ = 0 and z˙ = 0, H˙ dj = ∂p dj p˙ + ∂z dj z˙ = 0, which implies that Hdj (0) = L by lemma 6. This arrangement of states satisfies the requirements for a control switch as per table 1. But lemma 7 requires that there be a strict decrease in energy in the next switching cycle. This is in contradiction with our assumption that the system had some positive limit L = 0. So the sequence (Hdi (t1 ), Hdj (t2 ), Hdi (t3 ), Hdj (t4 ), . . . ) must approach 0.

1034

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, Joel USAFerguson et al. / IFAC-PapersOnLine 49-18 (2016) 1012–1017

1017

Closed loop Hamiltonians 2

Hamiltonian 1 (active) Hamiltonian 1 (inactive) Hamiltonian 2 (active) Hamiltonian 2 (inactive)

log 10 of the Hamiltonians

0

-2

-4

-6

-8

0

Fig. 2. Chaplygin sleigh initialised at x = 3, y = 2, θ = − π2 . The system is controlled by a switched energy shaping controller which is convergent towards the origin. In case A, proposition 1 ensures the Chaplygin sleigh will converge towards some configuration corresponding to Vd1 = Vd2 = 0, which is the origin. 5.2 Case B Proposition 2. If M2 becomes positively invariant along Σ2 , then M2 remains positively invariant and the system asymptotically approaches origin. Proof. If M2 becomes positively invariant along Σ2 then If step 3 of table 1 is eventually satisfied. The gradient of (19), evaluated at z2 = z3 = 0 is   z1 ∂Vd1 = 0 . (33) ∂z 0 The dynamics on z1 = 0 become    ∂H    −d1 −1  p˙ 1 (34) = ∂p1  , 1 0 z˙1 z1 which are invariant on M2 and asymptotically stable to z1 = 0. The combination of proposition 1 and proposition 2 ensures that the system is asymptotically stable in both case A and B. Thus we conclude that the system is asymptotically stable to the origin. 6. SIMULATION Numerical simulation of the Chaplygin sleigh was performed to asses the performance of switching controller in Table 1. The parameters used for the simulations are m = 2, J = 1, d1 = 4, d2 = 8, l = 1, a1 = 0.1, a2 = 0.1,  = 1. The Chaplygin sleigh is initialised in the position x = 3, y = 2, θ = − π2 . The path taken by the sleigh is shown in Figure 2 and the time history of the closed loop energies is shown in Figure 3. 7. CONCLUSIONS This paper presents a switched, energy shaping controller for the Chaplygin sleigh to asymptotically stabilise the

5

10

15

20 time (s)

25

30

35

40

Fig. 3. Closed loop Hamiltonians of the Chaplygin sleigh initialised at x = 3, y = 2, θ = − π2 . origin. Two energy shaping controllers are used drive the system to sub-manifolds in the configuration space that intersect at the origin. Currently, the existence and uniqueness of solutions of the manifold regulating control laws has not been shown for the case where the system passes through the singularities with non-zero momentum. This problem requires further investigation in future work. REFERENCES Astolfi, A., Ortega, R., and Venkatraman, A. (2010). A globally exponentially convergent immersion and invariance speed observer for mechanical systems with nonholonomic constraints. Automatica, 46(1), 182–189. Bloch, A.M. (2003). Nonholonomic Mechanics and Control. Springer Science & Business Media. Bloch, A.M., Reyhanoglu, M., and McClamroch, N.H. (1992). Control and stabilization of nonholonomic dynamic systems. IEEE Transactions on Automatic Control, 37(11), 1746–1757. Choset, H.M. (2005). Principles of Robot Motion: Theory, Algorithms, and Implementation. MIT press. Donaire, A., Romero, J.G., Perez, T., and Ortega, R. (2015). Smooth stabilisation of nonholonomic robots subject to disturbances. 4385–4390. Duindam, V., Macchelli, A., Stramigioli, S., and Bruyninckx, H. (2009). Modeling and Control of Complex Physical Systems: The Port-Hamiltonian Approach. Goldstein, H. (1965). Classical Mechanics. Pearson Education India. Kolmanovsky, I. and McClamroch, N.H. (1995). Developments in nonholonomic control problems. Control Systems, IEEE, 15(6), 20–36. Lee, D. (2007). Passivity-based switching control for stabilization of wheeled mobile robots. Robotics: Science and Systems III. Maschke, B. and Van der Schaft, A. (1994). A Hamiltonian approach to stabilization of nonholonomic mechanical systems. 3, 2950–2950. van der Schaft, A. and Jeltsema, D. (2014). PortHamiltonian Systems Theory: An Introductory Overview. Now Publishers Incorporated.

1035