Semi-Globally Exponential Trajectory Tracking for a Class of Spherical Robots

Semi-globally Exponential Trajectory Tracking for a Class of Spherical Robots

T.W.U. Madhushani a, D.H.S. Maithripala b, J.V. Wijayakulasooriya c, J.M. Berg d

aPostgraduate and Research Unit, Sri Lanka Technological Campus, CO 10500, Sri Lanka. [email protected]

bDept. of Mechanical Engineering, Faculty of Engineering, University of Peradeniya, KY 20400, Sri Lanka. [email protected]

cDept. of Electrical and Electronic Engineering, Faculty of Engineering, University of Peradeniya, KY 20400, Sri Lanka. [email protected]

dDept. of Mechanical Engineering, Texas Tech University, TX 79409, USA. [email protected]

Abstract

A spherical robot consists of an externally spherical rigid body rolling on a two-dimensional surface, actuated by an auxiliary mechanism. For a class of actuation mechanisms, we derive a controller for the geometric center of the sphere to asymptotically track any suﬃciently smooth reference trajectory, with robustness to bounded, constant uncertainties in the inertial properties of the sphere and actuation mechanism, and to constant disturbance forces including, for example, from constant inclination of the rolling surface. The sphere and actuator are modeled as distinct systems, coupled by reaction forces. It is assumed that the actuator can provide three independent control torques, and that the actuator center of mass remains at a constant distance from the geometric center of the sphere. We show that a necessary and suﬃcient condition for such a controller to exist is that for any constant disturbance torque acting on the sphere there is a constant input such that the sphere and the actuator mechanism has a stable relative equilibrium. A geometric PID controller guarantees robust, semi-global, locally exponential stability for the position tracking error of the geometric center of the sphere, while ensuring that actuator velocities are bounded.

1 Introduction ence of constant parameter variations and disturbance forces and moments. The surface upon which the body A spherical robot consists of a main spherical component rolls is assumed to be planar, but it is allowed to have whose angular velocity can be actuated by one or more an unknown, constant, non-zero inclination. auxiliary components. For brevity we will subsequently refer to the main spherical component simply as “the A sizable body of existing results address controllabil-

arXiv:1608.01494v2 [math.OC] 1 Mar 2017 body.” The actuating components may be internal or ity and open-loop path planning with momentum ac- external to the body. Generally, motion of the body can tuation [4,5,6,7,13,14,15]. Fewer studies consider closed- be produced by imbalanced actuators that shift the cen- loop control or barycentric actuation. To our knowledge, ter of gravity of the assembly, or by balanced actuators the only previous controllers for barycentric actuation that transfer angular momentum to and from the body are the open loop path planning schemes proposed by [1,2,3]. The former mechanism is referred to as barycen- [16,11,12] for a spherical robot rolling on a horizontal tric actuation and the latter mechanism is referred to as plane. To our knowledge, the only previous trajectory- momentum actuation [4,5,6,7,8,9,10,11,12]. This paper tracking feedback controller is the nonlinear geometric considers the robust trajectory tracking problem for a PD momentum controller derived for an inertially sym- class of spherical robots using either barycentric actu- metric spherical robot on a perfectly horizontal plane ation or momentum actuation. Speciﬁcally, this paper [17,18]. To our knowledge, only one study [17] takes presents a geometric PID controller to ensure that the into account the inclination of the rolling surface. This geometric center of a spherical robot body asymptoti- controller combines feedback linearization with sliding cally tracks any twice-diﬀerentiable reference trajectory, mode control [17]. This controller of [17] is formulated with semi-global asymptotic convergence, in the pres- in a single coordinate patch, and hence convergence is

Preprint submitted to Automatica 9 November 2018 only guaranteed to be local. The controller of [17] also re- a non-zero constant velocity reference command, torque quires perfect knowledge of the inclination of the rolling disturbance, or inclination of the rolling surface. On the surface. To our knowledge, the result presented in the other hand, for barycentric actuation, easily verifiable present paper significantly extends the state of the art in conditions ensure trajectory tracking with bounded ac- feedback control of spherical robots. The class of actua- tuation. tion mechanisms considered here is large, although not completely general. In particular, we constrain the dis- In Section 2 we derive the open-loop equations of mo- tance between the center of mass of the actuator and the tion by considering the system as comprising the rigid geometric center of the body to remain constant. Despite sphere plus each of the separate rigid bodies of the actu- the constraint, this actuator class includes balanced re- ation mechanism. Euler’s rigid body equations are used action wheels [5,6,15,7,18], control moment gyros [9,8], to model each of the subsystems. The exterior of the “hamster-wheel” carts driven along the inner spherical body is assumed to be perfectly spherical, but the dis- surface [12], and spherical pendulums with a fixed point tribution of mass on the interior need not be uniform; at the geometric center of the body [11]. Notably, this that is, the principal inertias need not all be equal. A is the first demonstration of a tracking controller that is no-slip constraint is applied at the point of contact be- robust to constant inclination of the rolling surface. tween the body and the ground; that is, the linear velocity of the contact point is assumed to be zero. Con- The potentially complex coupled dynamics of the straint forces and moments between the body, surface, actuator-body system present a challenge for control- and actuators are written explicitly, along with the equal ling the spherical robot. In this paper we circumvent and opposite reaction terms. This approach to modeling this obstacle by actively controlling only the location is equivalent to the Lagrange-d’Alembert principle [11], of the geometric center of the body, designated as the the constrained connection method [21], and the Euler- output system. Here we apply geometric PID control to Poincare formulation [12,16,22]. This chosen approach provide semi-global, exponential asymptotic tracking allows the spherical shell and the actuators to be treated by the output system. Analogously to the linear case, separately, which plays a crucial role in the subsequent geometric PID provides an intuitive and robust control controller development. Additionally, this approach al- framework for the control of mechanical systems [19]. lows barycenter and momentum actuation to be repre- This is because the geometric representation of a me- sented in a uniform framework. chanical system, obtained by replacing the usual time derivative by the covariant derivative, is a double inte- Section 3 presents our main results. It begins with the grator. Although the geometric double integrator is in tracking error dynamics for the unifying model incorpo- general nonlinear, it preserves essential features of stan- rating the distinct types of actuators considered here, dard linear PID control [19]. Geometric PID control re- and concludes with the development of a geometric PID quires a fully actuated simple mechanical system, which controller for trajectory tracking. Section 4 presents sim- for brevity we refer to here as a regular system. The ulations to demonstrate the versatility of the control complete spherical robot system of body and actuators framework. Subsection 4.1 considers a hamster-ball type is not regular. A previous study of tracking control for actuation mechanism, Subsection 4.2.1 considers a bal- a hoop robot—a planar version of the spherical robot— anced control moment gyro, and Subsection 4.2.2 con- introduced a procedure called feedback regularization siders a balanced reaction wheel actuator. The Appendix that allowed application of a geometric PID controller presents a proof of the main theorem. [20]. For the 3D spherical robot considered here, the error dynamics may be split into a linear combination of two left-invariant systems and a right-invariant sys- 2 Equations of Motion for the Spherical Robot tem on the group of rigid body motions. These error dynamics are still not regular, due to quadratic velocity We derive the equations of motion for a spherical robot terms arising from the constraint and actuator reaction with one or more barycentric or momentum actuators. forces. Feedback regularization may be used to give this Each actuator consists of a rigid body connected to the error system the form of a simple mechanical system, spherical body through a combination of constrained however in this paper we instead prove that the geo- and controlled degrees of freedom. For each controlled metric PID controller is robustly stable to the presence degree of freedom, a specified force or moment may be of these quadratic velocity terms. applied by the actuator on the spherical body. For each constrained degree of freedom, the corresponding force In this formulation, the system zero dynamics coincide or moment felt by the spherical body must be deter- with the controlled actuator dynamics. For practical im- mined from the dynamics of the actuator system. The plementation, these must remain bounded, giving the ac- control and constraint forces and moments are matched tuator characteristics a crucial role in the position track- by equal and opposite reaction forces and moments on ing problem. It is shown below that for momentum actu- the actuator body. The challenge of the spherical robot ation, boundedness of the actuator velocities cannot be control problem largely arises from coupling caused by guaranteed using continuous control in the presence of the actuator constraints. Here we restrict slightly the

2 class of actuators considered, requiring the center of angular momentum—can be considered as directed line mass of the actuator to remain at a constant distance segments in space. Such quantities make sense irrespec- from the geometric center of the sphere. tive of the choice of frame and hence are called geometric invariants. If γ is the representation of such an invariant The spherical body is assumed to be balanced – that is, in the frame e then we will use the notation Γ to denote the center of mass of the body plus the body-fixed bal- its b representation. These two representations are re- anced momentum actuators coincides with its geometric lated by γ = RΓ. Subsequently, we refer to Γ as the body center. The body may be inertially asymmetric – that representation and γ as the spatial representation of the is, its principal moments of inertias need not be equal. corresponding invariant. We shall always use lower case The external surface of the body is assumed to be per- symbols for the latter, and upper case for the former. In fectly spherical, and the supporting surface upon which this paper we denote by Rci the rotation that relates the it rolls is assumed to be perfectly planar. The support- orientation of actuator i with respect to the body. That ing surface need not be horizontal. The no-slip condition is ci = bRci (t) where Rci ∈ SO(3). Since b = eR(t) used in this paper constrains the translational velocity we have that ci = bRci (t) = eRRci and hence define of the point of contact relative to the supporting sur- Ri , RRci . face to be zero. Our procedure can also accommodate an additional “no-twist” or “rubber rolling” constraint The unit direction of gravity has the representation −eg to require the surface normal component of the sphere in the frame e. The radius, mass, inertia tensor, and an- angular velocity to be zero relative to the supporting gular velocity of the spherical body in body-fixed and surface. We do not require this constraint, however as spatial frames are, respectively, r, mb, Ib, Ω and ω. For demonstrated in the simulation section our controller is each actuator, the mass, inertia tensor, displacement of capable of enforcing this constraint. the actuator center of mass from the body geometric center, and angular velocity in the body-fixed and spa- In this paper we represent points in space using right- tial frames are, respectively, mi, Ii, Xi,Ωi and ωi. De- handed orthonormal cartesian coordinate systems—that note li , kXik, where by assumption, li is constant. is, using right-handed reference frames defined by three We choose the actuator-fixed frame ci such that the c3 orthogonal unit vectors. The orthonormal frame e = axis points towards the geometric center of the spherical {e1, e2, e3} is fixed in the supporting surface, with the body. Then Xi = −liRc e3. Because the body and actu- unit direction e coinciding with the outward surface i 3 ators are rigid, Ib and the Ii are constant when written in, normal. The orthonormal frame b = {b1, b2, b3} is fixed respectively, the body and actuator frames. These ten- in the spherical body, with origin coinciding with the ge- R T sors are represented in the spatial frame by Ib , RIbR ometric center. The orthonormal frame ci = {c1, c2, c3} Ri T th and Ii , RiIiRi . The representation of the position of is fixed in the i actuator, with origin coinciding with th the actuator center of mass. We also define the 3 × 1 the center of mass of the i actuator is oi with respect to matrix e [0 0 1]T . the the spatial frame, oi = o+RXi. The total mass of all 3 , Pn Pn actuators is ma , i=1 mi and X , ( i=1 miXi)/ma The motion of the spherical body in space is entirely is the collective center of mass of all actuators with re- captured by specifying the motion of a specific point, O, spect to b. of the sphere and the motion of an orthonormal frame b fixed to the sphere. We will choose the point O to be the Actuators satisfying the constraint of constant kXik in- geometric center of the sphere and let the origin of b co- clude cart-driven barycentric actuators, discussed for incide with O. The orientation of the frame b is related instance in [12], barycentric pendulum actuators, dis- to the earth fixed frame e by a rotation R(t) such that cussed for instance in [11], and reaction-wheel momen- b = e R(t). Denote by x the representation of a point P tum actuators, discussed for example in [5,6]. Figure 1 in space with respect to the earth fixed frame e and by depicts the problem geometry, for clarity shown in only X the representation of P in the body fixed frame b. If two dimensions and for a single actuator. For barycen- the representation of the position of O with respect to tric actuators, the center of mass position Xi(t) is non- 3 constant. For momentum-based actuators, X is con- the frame e at time t is o(t) ∈ R then the Euclidean i nature of space implies that x = o + RX. The fact that stant. Because the system center of mass includes all e and b are right hand oriented orthonormal frames im- fixed masses, without loss of generality we write Xi ≡ 0 ply that R(t) is a special orthogonal matrix. Conversely, for momentum-based actuators. any special orthogonal matrix represents a rotation of a frame with respect to another. The Lie group SO(3) The trajectory of the spherical body is specified by the represents all such possible rotations of a rigid body in pair (o(t),R(t)), and the corresponding velocity profile space with composition of sequential rotations as the is (o ˙(t), R˙ (t)). In this paper we explicitly incorporate group operation. the constraint forces between the spherical body and the supporting surface, and so the translational velocity Some quantities associated with rigid body motion— o˙(t) lies in the tangent plane to R3 at o(t), which is including force, moment of force, angular velocity, and also R3. The angular velocity R˙ (t) lies in the tangent

3 There are many different choices of Riemannian metric for the same Lie group, which give rise to different covariant derivatives. If a metric for a Lie group is chosen so that the value of the inner product does not change under left translation, then the metric is called left-invariant. Similarly, if the metric is invariant under right translation, it is called right-invariant. Metrics invariant under both left and right translation are called bi-invariant. In the case of rigid body motion, invariance of the metric corresponds to invariance of the inertia tensor, Iν . Specifically it can be shown, using the Koszul formula [23], that for left-invariant metrics induced by Iν , 1 ∇ν η dη(ξ)+ (± (ξ × η) − ( η × ξ + ξ × η)) , Iν ξ , Iν 2 Iν Iν Iν Fig. 1. Reference frames for the spherical robot analysis, for clarity shown in the plane and for a single actuator. and for right-invariant metrics induced by Iν ,

ν 1 plane to SO(3) at R(t), which is isomorphic to the Lie Iν ∇ξ η , Iν dη(ξ)+ (± Iν (ξ × η) + (Iν η × ξ + Iν ξ × η)) , algebra so(3). The Lie algebra so(3) may be identified 2 with the vector space of 3×3 skew-symmetric matrices. It and for any bi-invariant metric is convenient to define an isomorphism between elements 3 of so(3) and elements of R . To do this, given any X,Y ∈ 1 3 ∇biη dη(ξ) ± ξ × η. R , define Xb to be the unique 3 × 3 skew-symmetric ξ , 2 matrix such that XYb = X × Y for all Y , where X × Y denotes the usual vector cross product. In terms of When ξ, η are body angular velocities the ‘+’ are chosen; components, when ξ, η are spatial angular velocities, the ‘−’ are chosen. These expressions are well-defined even for singular Riemannian metrics, where is positive semi-definite. \   Iν X1 0 −X3 X2 For further details on the covariant derivative in general,     see [24,21,23,22]. Xb = X  =  X 0 −X  .  2  3 1 b X3 −X2 X1 0 Subsequently we let ∇ denote the covariant derivative on SO(3) corresponding to the left-invariant Rieman- nian metric hhζ, ηii ζ · η on SO(3) with explicit Then R˙ = R Ω = ω R , and R˙ = R Ω = ω R . so(3) , Ib i i bi bi i ci ci bci bci ci R b R R expression Ib ∇ωω = Ib ω˙ − (Ib ω) × ω. We use Newton’s Here Ωi,Ωci and ωi represent the angular velocity of the ith actuator in, respectively, the actuator-fixed, body- equations to model the body and the actuator dynamics. fixed and spatial frames. These representations are re- For the spherical body, Newton’s equations specialize to Euler’s equation, lated by ωi = ω + Rωci = ω + RiΩci . R b Ib ∇ωω = τ, (1) Given smooth vector fields U and V , a connection ∇U V is a vector field that describes the change of V with re- where τ is the resultant of the moments, with respect to spect to U at each point. When U and V are vector fields the geometric center of the sphere, acting on the body. describing velocities, ∇ V is the rate of change of V (q) U Euler’s equation for the ith actuator is as the system follows the trajectories solvingq ˙ = U at each point q. It is refered to as the covariant derivative Ri ∇i ω = τ , (2) of V along U. Specifically, ∇U U can be thought of as Ii ωi i i the vector field of accelerations corresponding to velocity vector field U. For a Riemannian metric on a manifold, where ∇i is the unique Levi-Civita connection corre- there is a unique covariant derivative referred to as the sponding to the left-invariant metric Ii and τi is the re- Levi-Civita connection. For a submanifold of Euclidean sultant moment with respect to the center of mass of the space, the Levi-Civita connection on the submanifold actuator. We assume that each momentum-based actu- describes the projection of the acceleration onto the tan- ator is symmetric about its actuation axis, that is, Ii gent plane to the submanifold. Physically, this means takes the form Ii = diag([Ip,i Ip,i Iz,i]). The translation that constraint forces corresponding to motion on the of the center of mass of the rigid body obeys submanifold are suppressed, and do not explicitly ap- pear in the equations of motion. mbo¨ = f, (3)

4 where f is the resultant force acting on the body. The The total resultant reaction forces, reaction moments, usual component-wise derivative is used here instead of and moments due to the reaction forces are, respectively, the covariant derivative, because in Euclidean space the f = Pn f , τ = Pn τ , and τ = Pn τ . c i=1 ci c i=1 ci fc i=1 fci two are equivalent. Note that all these moments are deﬁned with respect to the geometric center of the sphere. The constraint forces

The body is acted on by the reaction forces and moments −fci at the actuator contact points can be determined by due to the actuator mechanism, forces and moments due twice differentiating the expression oi = o + RXi. Sub- to gravity, and the constraint forces and moments at the stituting these expressions into (4) gives the final form contact point. We denote by fc and τc the resultant force of the constrained equations of motion for the spherical and moment, respectively, directly applied to the body body: by the actuators. We denote by τfc the additional moment arising from the offset of the line of action of fc b 2 2 T from the body center of mass. The gravity force acting Ib∇ωω − (mb + ma)r eb3 + rmaeb3RXRb ω˙ = on the sphere is f = −m g e where −e is the repre- n g b g g X sentation of the direction of gravity in the earth fixed −rmae3 ×ω×ω×RX−re3 × mi RXï + 2ω × RX˙ i frame e. i=1 n X − r(m + m )g e × e + ∆ + (τ + τ ), (5) Let the position of a point fixed on the surface of the b a 3 g I d ci fci spherical body be y(t) in spatial coordinates and Y in i=1 body coordinates. The velocity of such a point satisfies y˙ =o ˙ + R(Ω × Y ) =o ˙ + (ω × y), where the second Substituting the constraint moments into (2) gives the equality follows because the cross product is invariant equations of motion for the ith actuator: under rigid rotation. The no-slip condition requires that the velocity at the point of contact is zero, and soo ˙ + Ri i Ii ∇ω ωi = RXi × fci − (τci + τfc ). (6) (ω × y) = 0. At the point of contact, y = −re3, and we i i obtain the no-slip constrainto ˙ = reb3ω. The requirement that the sphere remain in contact with the supporting Remark 1 Equations (5) and (6) completely define the surface at all times gives the constraint e3 · o˙ = 0. We dynamics of an inertially asymmetric sphere rolling with- denote by fλ the constraint force acting on the body out slip on a, possibly inclined, planar surface. These at the point of contact, and write f = fg + fc + fλ. equations are independent of the actuation mechanisms Differentiating the no-slip constraint, substituting into that drive the sphere. Specific types of actuators, leading to different interaction forces and moments, are consid- (3), and solving for fλ gives fλ = − (mbreb3ω˙ + fg + fc). The corresponding moment about the geometric center ered next. of the body is τ = −re ×f = m r2e 2 ω˙ +re ×(f + fλ 3 λ b b3 3 g fc) and the total resultant moment acting on the body The controls act on the system through the moments about the geometric center is τ = τ + τ + τ + ∆ , (τ + τ ). The exact form of the controls depend on c fc fλ I d ci fci where I∆d represents unmodelled moments. Then the how the actuator interacts with the sphere. For actua- governing equation for the body is tors interacting though rolling contact, such as cart type

actuators, fci can be directly controlled and τci ≡ 0. R b 2 2 ∇ ω − m r e ω˙ = −rm g e × e + ∆ Hence the τui τfc are the controls. For actuators in- Ib ω b b3 b 3 g I d , i teracting through a ﬁxed point, such as a pendulum or + re3 × fc + τc + τfc . (4) a gyroscopic actuator τci can be directly controlled and τ ≡ 0. For reaction wheel type actuators τ ≡ 0 and fci fci the entire τ is not available for controls. Part of the τ The equations of motion for the actuators are similarly ci ci derived. We assume that the ith actuator is in contact will be used to enforce the constraint that the wheel is with the sphere at p separate points. Let the position of restricted to rotate only about an axis ﬁxed with respect the jth contact of the ith actuator have representation to the sphere. If Γi is a unit vector along this axis then we can write τ = u RΓ + τ e where u ∈ is the con- Y in the actuator frame, c , with j = 1, 2, ··· , p. Let ci i i ci i R ij i troll and −τ e is the moment that enforces the wheel to −fij be the interaction force acting on the body at the ci rotate only about Γi with respect to the body. ij-contact, let −τij be the interaction moment acting at the ij-contact. The corresponding moment about the actuator center of mass is −RiYij × fij, and about the In the following we will consider two classes of actuators depending on whether they are balanced or not. Bal- geometric center of the body is τfij = (RXi + RiYij) × Pp Pp anced actuators will be referred to as momentum based fij. Let fc fij, τc τij, and τf i , j=1 i , j=1 ci , actuators and unbalanced actuators will be referred to Pp τ . The total resultant moment acting about the j=1 fij as barycentric actuators. In this paper, when reaction Pp center of mass of the actuator is τi = −τci − j=1 RiYij× wheels are considered, we will restrict our attention to f = RX × f − (τ + τ ). balanced pairs of reaction wheels. ij i ci ci fci

5 2 2 2.1 Barycentric Actuators Pn 2 Ir , Ib − i=1 Γbi Ii + miXci , Ami = (I3×3 + RcΓi ), Bm = I3×3, and For barycentric actuators, and hence the sphere dynam- i ics (5) becomes τm = τr(ω, ωi) , n R b Ra X ∇ ω + sω˙ + e3 × (e3 × ω˙ ) = 2 T T T Ib ω I Ia − RΓbi IiR ω × R ω + Ii (Ri (ωi − ω)) × ω n X i=1 τg + τb + Bbi τui , (7) T T T T + IiR ω × (Ri (ωi − ω)) + (IiRi (ωi − ω)) × R ω . i=1

n n for balanced reaction wheel type actuators. where m P m , and Ra P Ri , a , i=1 i Ia , i=1 Ia˜i

2 2 3 Intrinsic Nonlinear PID Control for Trajec- Is , −(mb + ma)r eb3 , (8) tory Tracking − m l2e 2 (9) Iai , Ii i i b3 Ri 2 2 2 −1 T The control problem solved in this paper is asymptotic −r m l Rie3 e3R , (10) Ia˜i , i i b Iai b i n tracking of a desired trajectory by the geometric cen- X τ m l r e R e −1RT ω Ri ω ter of the spherical body. Specifically, oref (t) is a twice- b , i i 3 i 3Iai i iIai i b b b differentiable reference trajectory, satisfying e3 ·oref (t) = i=1 r for all t. This constraint has differential form e · 2 3 + milir eb3ωbi Rie3 (11) o˙ref (t) = 0. Define error dynamics oe(t) , o(t) − oref (t). g Then the control objective is to ensure limt→∞ oe(t) = 0. τ −r(m + m )g e × e + e × Ra e (12) g , b a 3 g r 3 Ia g For given o (t) with e · o˙ = 0, all reference B m l r e R e −1RT + I . (13) ref 3 ref bi , i i b3 i b3Iai i 3×3 angular velocity trajectories satisfying the no-slip constrainto ˙ref = re3 × ωref (t) are of the form For barycentric actuators the actuator dynamics (6) be- 1 ωref (t; β) = r (o ˙ref (t) × e3) + βe3 for some value of the comes scalar parameter β. Subsequently we assume that a smooth β(t) has been chosen to give a suitable ωref (t). Ri ∇ai ω = C Rω × ω + τ + τ + ∆ Iai ωi i bi Ib g b I d For example, β(t) ≡ 0 gives the ωref (t) that satisfies the n “no twist” condition. This is the condition applied in X + τgi − τui + Cbi Bbk τuk , (14) the simulations presented below. k=1 Now consider the control objective of ensuring that ai where ∇ is the unique Levi-Civita connection corre- limt→∞ ω(t) = ωref (t). Let ωe , (ω − ωref ). Then the sponding to the left-invariant Riemannian metric in- error dynamics can be expressed as, duced by Iai and o˙e = −r e3 × ωe, (17) −1 2 R Ri n Cb (Ri) = −milir Rie3 + s + e3 e3 , X i b Ib I b Ia˜i b R Ieω˙ e = Ib ωe × ωe + τα + τg + τref + I∆d + Bαi τui ,

τgi (Ri) = milig (Rie3) × eg. i=1 (18)

2.2 Momentum Based Actuators where

R Ra Ie Iα + Is + e3 × Ia e3 , (19) For momentum-based actuators, since Xi = 0, (5) and , b R R (6) become τref , Iα ωref × ωe + Iα ωe × ωref R + Iα ωref × ωref − Ieω˙ ref . (20) R m Im∇ω ω + Isω˙ = −r(m + ma)g e3 × eg n X + τm + I∆d + Bmi τui , (15) i=1 Remark 2 The above equations are specialized to barycentric actuators and momentum actuators by set- A Ri ∇i ω = −τ , (16) ting the subscript α = b and α = m respectively. mi Ii ωi i ui R R where Im = Ib , Ami = Bmi = I3×3, and τm = 0 for The objective is to ﬁnd a controller that will ensure R non-reaction wheel type of balanced actuators and Im = limt→∞ ωe(t) = 0. Observe that these error dynamics

6 do not have the structure of a mechanical system on a the rolling surface is perfectly horizontal and the distur- Lie group and thus preventing us from using the PID bances and reference velocities tend asymptotically to controller developed in [19]. zero.

3.1 The zero dynamics We now show that for barycentric actuators, there exists a constant control τ˜¯u that ensures ωe(t) ≡ 0, and ¯ i that for this τ˜ui the actuator trajectories correspond to The zero dynamics of the error system are the internal relative equilibria of the zero dynamics. We assume that dynamics of the system with oe and ωe constrained to these relative equilibria are stable—that is, there exists be identically zero. From (14) and (16) it follows that a positive semi-deﬁnite function Vi : SO(3) 7→ R such the zero dynamics of the coupled system with respect to ¯ that dVi = −(τgi (Ri)−τ˜ui ). Since these relative equilib- the output ωe(t) must satisfy ria correspond to relative equilibria of rigid body rotations it is clear that they are bounded. Thus, designing a A Ri ∇ai ω = τ (R ) − τ˜ , (21) ¯ αi Iai ωi i gi i ui controller that ensures limt→∞(τgi (Ri) − τ˜ui ) = 0 exponentially, guarantees limt→∞ ωe(t) = 0 while ensuring where from (18) we see that the output zeroing controls that the actuator velocities ωi(t) remain bounded. We

τ˜ui must satisfy begin by proving the following lemma:

n Lemma 1 For barycentric actuators, if the dis- X Bαi τ˜ui = −(τg + τα + τref + I∆d). (22) turbances, I∆d and velocity references, ωref , are i=1 constant then the output satisﬁes ωe(t) ≡ 0, with

τui (t) ≡ constant, if and only if τui (t) satisfies (22) and Here we use the convention of setting the subscript α = the resulting trajectory of the actuator (Ri(t), ωi(t)) sat- b or α = m if the actuator is of barycentric type or isfies Ri(t)e3 ≡ constant. Such trajectories necessarily momentum type respectively. Furthermore we also note correspond to relative equilibria of (21) with Aαi = I3×3. that τgi = 0 for momentum actuators and Aαi = I3×3 for all actuators except reaction wheel actuators for which We see from (11)—(13) that Bα , τα and τg are con- 2 i stant if and only if Rie3 ≡ constant. Physically what we have Aαi = (I3×3 + RcΓi ). this means is that Bαi , τα and τg are constant if and When the output is zero, a necessary condition for the only if the actuator is stationary with respect to the in- actuator states to remain bounded is that the right- ertial frame modulo a rotation about its third body axis. When ωe ≡ 0 equation (22) implies that for constant dis- hand sides of (21) satisfy the condition limt→∞(τgi (Ri)− τ˜ ) 6= constant. On the other hand it can be shown that turbances, I∆d, constant velocity references, ωref , and ui constant control,τ ˜ , the output ω ≡ 0, if and only lim (τ (R ) − τ˜¯ ) = 0 exponentially, for some con- ui e t→∞ gi i ui if R e ≡ is constant. From (21), with A = I , we stant control τ˜¯ , is sufficient for the actuator states to i 3 αi 3×3 ui ˜ ˜ remain bounded if there exists a positive semi-definite see that (Ri, ω˜i) satisfies Rie3 ≡ constant if and only if (R˜ , ω˜ ) is a relative equilibrium of (21). Therefore function Vi : SO(3) 7→ R such that dVi = −(τgi (Ri) − i i ¯ ˜ τ˜ui ). (Ri, ω˜i) must necessarily satisfy

For momentum actuators, B = I , τ = 0 and τ = ˜ αi 3×3 gi g gmili(Rie3) × eg − τ˜ui = 0. (23) −r(mb +ma)g e3 ×eg. Thus the right hand side of (21) is equal to τref +I∆d −rg(mb +ma) eb3eg, and the necessary Recall that (22) is also necessary for the output to be condition for actuator velocity boundedness is violated zero also requires. Thus the relative equilibria (R˜ , ω˜ ) if the disturbances or the velocity reference are constant i i will ensure that ω ≡ 0 and R˜ e ≡ constant if and only or if the rolling surface is not perfectly horizontal. e i 3 if R˜i(t) satisﬁes

Remark 3 For momentum actuators there exists no n ! 1 ˜ continuous controller that can ensure limt→∞ ωe(t) = 0 X [˜ Ri g miliRie3 + e3Ia˜ − (mb + ma)re3 eg while ensuring that the actuator velocities ωi(t) re- r b i b main bounded for non-vanishing disturbances or non- i=1 vanishing reference velocities or non-zero inclination of + τref + I∆d = 0. (24) the rolling plane. If the disturbances and velocity reference are zero, then In particular this means that one can not stabilize the ˜ sphere at a point on an inclined plane, using a balanced it can be shown that there exists a Ri(t) that satisfies actuator, while ensuring that the actuator velocities re- (24) if the inclination β ∈ (−π/2, π/2) of the inclined surface satisfies sin β ≤ mili , for each actuator. main bounded. Since for momentum actuators τgi = 0, (mb+mi)r the sufficient condition will be satisfied if and only if Since for barycenter actuators li < r, this shows that

7 for a given actuator there exists an upper bound on the the form inclination beyond which there exist no relative equilibria. The upper bound becomes larger for larger li and n R α s X Ri a˜i mi. In principle one can thus design an appropriate ac- ∇ ωe + s∇ ωe + e3 × ∇ (e3ωe) = Iα ωe I ωe Ia˜i ωi b tuator such that a relative equilibrium is guaranteed to i=1 exist when the disturbances and reference velocities are τe(ωe, ω1, ··· ωn) + τα(ωe, ω1, ··· ωn) constant and suﬃciently small. This proves Lemma-1. n X + τg + I∆d + Bαi τui , i=1

3.2 PID controller development The error dynamics written down in this form can be considered as a split mechanical system. Given the split mechanical structure of the error dynamics we are mo- In this section we develop a controller to ensure that the tivated to propose the intrinsic potential shaping plus limt→∞(oe(t), ωe(t)) = (0, 0) semi-globally and expo- nonlinear ‘split’ PID controller ¯ nentially while ensuring limt→∞ τ˜ui , τ˜ui = constant. This combined with Lemma-1 ensures the boundedness s R α Is∇ω oI + Iα ∇ω oI of the actuator velocities. Summarizing the conclusions e e n of the preceding discussion, we explicitly state the con- X Ri a˜i + e3 × I ∇ω (e3 × oI ) = Ieηe, (25) ditions under which this can be ensured: a˜i i i=1

Assumption 1 n X Bαi τui = (1) For reaction wheel actuators, the rolling plane is i=1 perfectly horizontal, and ω and ∆ tend to zero g ref I d − e × Ra e + (k η + k ω + k o ) , (26) exponentially, r 3 Ia g0 Ie p e d e I I (2) For barycentric actuators, there exists a positive semi-deﬁnite function Vi : SO(3) 7→ such that R where ηe e3oe. Here the potential shaping part of ¯ ¯ , b g dVi = −(gmili(Rie3) × eg − τ˜ui ) for constant τ˜ui . Ra the controller given by ( r e3 × Ia eg0 ) is a model of τg for some nominal inclination. Thus the implementation of the controller does not require the knowledge of the The nonlinear PID controller, that was proposed in [19], inclination of the rolling surface. for conﬁguration tracking was based on exploiting the inherent mechanical systems structure of the error dynamics. This structure is the nonlinear equivalent of a With this controller we see that the closed loop error linear double integrator. For the rolling sphere it is not dynamics are given by (25) and possible to do this in a straightforward manner since the error dynamics given by (18) do not have the struc- o˙e = −r e3 × ωe (27) ture of an invariant mechanical system on a Lie group. n s R α X Ri a˜i One problem is that the candidate inertia term e = s∇ ωe + ∇ ωe + e3 × ∇ (e3ωe) = I I ωe Iα ωe b Ia˜i ωi b R Ra Iα + Is + eb3Ia eb3 is neither left-invariant nor right in- i=1 variant. However we notice that it may be considered to − Ie(kpηe + kdωe + kI oI ) + I∆d + I∆g + τe + τα, be in some sense the sum of two left-invariant Rieman- (28) nian metrics induced by Iα and Iai and a singular right invariant metric induced by . Motivated by this obser- Is where ∆ is given by vation if we add the quadratic velocity term I g g Ra I∆g , −r(mb + ma)g e3 × eg − e3 × Ia (eg0 − eg). τ (ω , ω , ··· , ω ) ( ω × ω ) r e e 1 n , Is e e (29) n 1 X Ri + e3 × I (ωi × (e3 × ωe)) 2 a˜i i=1 This term arises due to the ignorance of the angle of inclination in the potential shaping part of the controller Ri Ri g R +Ia˜ ωi × (e3 × ωe) + Ia˜ (e3 × ωe) × ωi , a i i −( r e3 × Ia eg0 ). Notice that since for balanced mechanisms Ia˜i = 0 this term will be absent in the controller for a balanced mechanism. However in both cases I∆g 6= 0 to both sides of the error dynamics (18), it then takes and hence we have the following remark:

8 Remark 4 A bounded non-vanishing unknown distur- sphere while in Section-4.2 we consider balanced actua- bance given by (29) will always act on the error dynamics tion mechanisms. We simulate the performance for two if the plane of rolling is not horizontal. types of balanced mechanisms. In Section 4.2.1 we simulate the performance of a balanced gyroscopic moment In the Appendix we prove that if the PID controller actuated sphere and in Section 4.2.2 we simulate the per- gains are chosen as follows then it is possible to ensure formance of a balanced reaction wheel actuated sphere. limt→∞(oe(t), ωe(t)) ≡ (0, 0) semi-globally and exponentially for bounded constant velocity references and In all simulations the nominal mass of the spherical disturbances in the presence of bounded parametric un- shell was chosen to be mb = 1.00 kg, the nominal iner- certainty. Consider a polar Morse function V : 2 7→ ν R R tia tensor of the spherical shell was chosen to be Ib = 2 with a unique minimum at (0, 0) such that dVν = Iν ηe for diag{0.0213, 0.0205, 0.0228}kg m , while the radius of ν = α, s, a˜i. Let ϑν > 0 such that hhηe, ηeiiν /(2ϑν ) ≤ Vν . the spherical shell was chosen to be r = 0.18 m. These ν Let ϑmax , {ϑs, ϑb}. Let µν = maxkIν ∇ηe k on Xu, parameters were chosen to correspond to a 3 mm thick −3 µmin = min{µs, µb, µa˜i }, and µmax = max{µs, µb, µa˜i }. plastic shell, with density 850 kg m . To demonstrate The existence of these constants are guaranteed since Vν robustness of the controller the system parameters used is a polar Morse function. in the simulations were chosen to be 50% different from the nominal parameters used for the controller. In all Let the controller gains kp, kI , kd > 0 be chosen to satisfy simulations the initial position of the sphere was as- the following inequalities. sumed to be o(0) = [2, −2, r]T m and the initial angular velocity of the sphere was chosen to be ω(0) = 3 2 [−0.1, −0.2, 0.5]T rad/s. kd(1 − δ ) 0 < kI < , (30) µmax 2 2kd 4.1 Barycenter-Controlled Inner Cart Actuation kp > max k1, k2, , (31) µmax Consider a sphere actuated by an omni-directional wheel where, δ = (1 − µmin/µmax), driven cart. The total mass of the cart was chosen to be mi = 3.28 kg, while its inertia tensor was chosen to be s ! k 16ϑk2 k = I 1 + d − 1 , 1 2 = diag{0.0353, 0.0378, 0.0368} kg m2. 2kd µmaxkI Ii s 2 3 2 2 3 3 ! ϑkI 4kd(µmaxkI + 4kd(µmax + kd)) For these parameters one finds that the maximum incli- k2 = 4 1 + 1 + 2 3 . 2kd ϑµmaxkI nation for which an equilibrium for the controlled cart exists is 25◦. The sphere is assumed roll on a 20◦ inclined plane in the y-direction and is assumed to be unknown. Theorem 1 Let the conditions of Assumption-1 hold ◦ and consider arbitrary compact sets X ⊂ 2 × so(3) × A nominal value of 30 inclination is assumed in the con- s R troller. so(3) and Xa ⊂ SO(3) × so(3). Then, if the gains of the nonlinear PID controller (25)–(26) are chosen to be sufficiently large while satisfying (30) and (31) then for all Simulation results are presented in figure (2)–(6) for initial conditions in Xs × Xa, the followings hold in the tracking a sinusoidal path, circular path and a fixed point presence of bounded parametric uncertainty: at (3, 0). In all simulations the initial conditions used T for the inner cart were ωi(0) = [0.2 − 0.1 0.1] rad/s. (1) if the reference velocities and the disturbances are The controller gains were chosen to be kp = 100, kd = bounded (oe(t), ωe(t)) converge to an arbitrarily 60, kI = 10. small neighborhood of (0, 0), (2) if the reference velocities and the disturbances are 4.2 Balanced actuation Mechanisms constant then limt→∞(oe(t), ωe(t)) = (0, 0), while ensuring that ||ωi(t)|| remains bounded. The con- In this section we simulate the behavior of the controller vergence is guaranteed to be exponential. for two types of balanced actuation mechanisms: balanced gyroscopic moment actuation in Section-4.2.1 and balanced reaction wheel actuation in Section-4.2.2. Sim- 4 Simulation Results ulation results are presented for tracking a sinusoidal path and a circular path. In these simulations the rolling In this section we simulate the performance of the in- surface is assumed to be perfectly horizontal. In both gy- trinsic nonlinear PID controller (25)–(26). In Section 4.1 roscopic and reaction wheel methods the controller gains we simulate the performance of an inner cart actuated were chosen to be kp = 55, kd = 10, kI = 1.

9 (a) Sinusoidal path (b) Circular path

(a) Sinusoidal path (b) Circular path

Fig. 4. The spatial angular velocity error, ωe(t), for the cart actuated sphere for the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%.

Fig. 2. The path followed by the cart actuated sphere for the PID controller (25)–(26) in the presence of parameter (a) Sinusoidal path (b) Circular path uncertainties as large as 50%. The blue curve shows the reference trajectory while the red curve shows the trajectory of the center of mass of the sphere.

(a) Sinusoidal path (b) Circular path Fig. 5. The spatial angular velocities of the cart, ωi(t), for the cart actuated sphere for the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%.

(a) Sinusoidal path (b) Circular path

Fig. 3. The position error, oe(t), for the cart actuated sphere for the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%.

4.2.1 Balanced Gyroscopic Moment Actuation (c) Fixed point In this section we present corresponding simulation results of a balanced gyroscopic moment actuator driven by omni-directional wheels similar to [9,8], but with the Fig. 6. Control input, τu(t), for the cart actuated sphere for center of mass at the geometric center of the sphere. The the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%. total mass of the mechanism was chosen to be mi = 4.58 kg, while the inertia tensor was chosen to be In all simulations the initial conditions used for the in- 2 ner vehicle were same as in section 4.1. The simulation Ii = diag{0.0535, 0.0516, 0.0480} kg m results are shown in ﬁgure (7)–(10).

10 4.2.2 Balanced Reaction Wheel Actuation

In this section we present corresponding simulation results of actuation mechanism for balanced three pairs of reaction wheels. All pairs were not assumed to be iden- tical. Instead we assumed the following nominal parameters for the sets of wheels: m1 = 5.78 kg, m2 = 4.21 kg, m3 = 4.91 kg,

(a) Sinusoidal path (b) Circular path 2 I1 = diag{0.0105, 0.0105, 0.0204} kg m , 2 I2 = diag{0.0070, 0.0070, 0.0138} kg m , = diag{0.0086, 0.0086, 0.0169} kg m2. Fig. 7. The path followed by the balanced gyroscopic mo- I3 ment actuated sphere for the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%. The These parameters correspond to wheels made of lead. blue curve shows the reference trajectory while the red curve of radius r1 = 0.084 m, r2 = 0.081 m, r3 = 0.083 m and shows the trajectory of the center of mass of the sphere. thickness d1 = 0.023 m, d2 = 0.018 m, d3 = 0.020 m respectively. All wheels were located at distance li = 0.11 m from the center of the sphere. Nominal values for the rest of the components of actuation mechanism were chosen to be mi = 4.337 kg,

2 Ii = diag{0.0166, 0.0195, 0.0053} kg m (a) Sinusoidal path (b) Circular path ˙ initial conditions used for the wheels were ψc1 (0) = ˙ ˙ 0.2 rad/s, ψc2 (0) = −0.1 rad/s, ψc3 (0) = 0.1 rad/s. Here ˙ we use the notation ψci = e3 · Ωci . Fig. 8. The position error, oe(t), for the balanced gyroscopic moment actuated sphere with the PID controller (25)–(26) Figure (11)–(14) demonstrates the performance of the in the presence of parameter uncertainties as large as 50%. PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%.

(a) Sinusoidal path (b) Circular path

Fig. 9. The spatial angular velocity error, ωe(t), for the balanced gyroscopic actuated sphere with the PID controller (a) Sinusoidal path (b) Circular path (25)–(26) in the presence of parameter uncertainties as large as 50%.

Fig. 11. The path followed by the center of mass of the sphere for the reaction wheel actuated sphere with the PID controller (25)–(26) in the presence of parameter uncertainties as large as 50%.

(a) Sinusoidal path (b) Circular path 5 Conclusion

This paper considers the robust semi-global exponential Fig. 10. The spatial angular velocities of the actuation mech- tracking of an inertially non-symmetric spherical robot anism, ωi(t), for the balanced gyroscopic moment actuated actuated by the class of actuation mechanisms where the sphere with the PID controller (25)–(26) in the presence of distance between the center of the sphere and the center parameter uncertainties as large as 50%. of mass of the actuation mechanism remains constant. The sphere is assumed to roll on a plane of unknown,

11 [2] R. Chase, A. Pandya, A review of active mechanical driving principles of spherical robots, in: Robotics, 2012, pp. 3–23. doi:10.3390/robotics1010003. [3] R. H. Armour, J. F. Vincent, Rolling in nature and robotics: A review, Journal of Bionic Engineering 3 (4) (2006) 195 (a) Sinusoidal path (b) Circular path – 208. doi:http://dx.doi.org/10.1016/S1672-6529(07) 60003-1. URL http://www.sciencedirect.com/science/article/ pii/S1672652907600031 [4] S. Bhattacharya, S. Agrawal, Design, experiments and Fig. 12. The position error, oe(t) of the reaction wheel actu- motion planning of a spherical rolling robot, in: Robotics ated sphere with the PID controller (25)–(26) in the presence and Automation, 2000. Proceedings. ICRA ’00. IEEE of parameter uncertainties as large as 50%. International Conference on, Vol. 2, 2000, pp. 1207–1212 vol.2. doi:10.1109/ROBOT.2000.844763. [5] M. Svinin, A. Morinaga, M. Yamamoto, A motion planning strategy for a spherical rolling robot driven by two internal rotors, in: IEEETransactions on Robotics, Vol. 30, 2014, pp. 993–1002. [6] A. V. Borisov, A. A. Kilin, I. S. Mamaev, How to (a) Sinusoidal path (b) Circular path control chaplygin’s sphere using rotors, Regular and Chaotic Dynamics 17 (3) (2012) 258–272. doi:10.1134/ S1560354712030045. URL http://dx.doi.org/10.1134/S1560354712030045 Fig. 13. The angular velocity error, ωe(t) of the reaction [7] V. A. Joshi, R. N. Banavar, R. Hippalgaonkar, Design and wheel actuated sphere with the PID controller (25)–(26) in analysis of a spherical mobile robot, in: Mechanism and the presence of parameter uncertainties as large as 50%. Machine Theory, Vol. 45, 2009, pp. 130–136. [8] H. Ghariblu, A new mobile ball robot-dynamic modeling and simulation, in: Applied Mathematical Modelling, Vol. 39, 2014, pp. 3103–3115. [9] A. Halme, J. Suomela, T. Schonberg, Y. Wang, A spherical mobile-micro robot for scientiﬁc applications, in: ASTRA 96, ESTEC, Noordwijk, The Netherlands, 1996. (a) Sinusoidal path (b) Circular path [10] J. Alves, J. Dias, Design and control of a spherical mobile robot, in: The Institution of Mechanical Engineering, Vol. 217 I,J, 2003. [11] Y. Cai, Q. Zhan, X. Xi, Path tracking control of a spherical Fig. 14. The angular velocities of the reaction wheels, Ω (t), bi mobile robot, in: Mechaniism and Machine Theory, Vol. 51, for the reaction wheel actuated sphere with the PID con- 2012, pp. 58–73. troller (25)–(26) in the presence of parameter uncertainties as large as 50%. [12] Y. L. Karavaev, A. A. Kilin, The dynamics and control of a spherical robot with an internal omniwheel platform, in: constant, inclination. From a practical point of view this Regular and Chaotic Dynamics, Vol. 20, 2015, pp. 134–152. doi:10.1134/S1560354715020033. implies that one can robustly stabilize the sphere at a point on an inclined surface with bounded controls using [13] V. Jurdjevic, The geometry of the plate-ball problem, Archive for Rational Mechanics and Analysis 124 (4) (1993) 305–328. barycenter actuation. To our knowledge, this is the ﬁrst doi:10.1007/BF00375605. appearance in the literature of a feedback controller ca- URL http://dx.doi.org/10.1007/BF00375605 pable of tracking a desired position on an inclined plane, [14] A. Marigo, A. Bicchi, Rolling bodies with regular surface: in the presence of parameter uncertainty and uncertainty controllability theory and applications, IEEE Transactions of the inclination of the rolling surface. on Automatic Control 45 (9) (2000) 1586–1599. doi:10. 1109/9.880610. [15] R. Mukherjee, M. Minor, J. Pukrushpan, Motion planning Acknowledgements for a spherical mobile robot: Revisiting the classical ball- plate problem, in: Journal of Dynamic Systems Measurement J. M. Berg gratefully acknowledges the support of the and Control, Vol. 124, 2002, pp. 502–511. doi:10.1115/1. 1513177. US National Science Foundation under the Independent Research/Development program. [16] J. Shen, D. A. Schneider, A. M. Bloch, Controllability and motion planning of a multibody chaplygin’s sphere and chaplygin’s top, International Journal of Robust and Nonlinear Control 18 (9) (2008) 905–945. doi:10.1002/rnc. References 1259. URL http://dx.doi.org/10.1002/rnc.1259 [1] V. Crossley, A literature review on the design of spherical [17] T. Yu, H. Sun, Q. Jia, H. Zhang, W. Zhao, Stabilization and rolling robots, in: Pittsburhge PA, 2006. control of a spherical robot on an inclined plane, Research

12 Journal of Applied Sciences, Engineering and Technology largest level set of W that is contained in the set where 5 (6) (2013) 2289–2296. ||zs||< . Let ϑ > 0 be such that hhηe, ηeiiν /(2ϑν ) ≤ Vν [18] S. Gajbhiye, R. N. Banavar, Geometric approach to on Wu. Then we have tracking and stabilization for a spherical robot actuated by internal rotors, Preprint Submitted for Review: Automatica 1 T 1 T abs/1511.00428. zν Plzν ≤ Wν ≤ zν Puzν URL http://arxiv.org/abs/1511.00428 2 2 [19] D. H. S. Maithripala, J. M. Berg, An intrinsic pid controller where for mechanical systems on lie groups, Automatica 54 (2015) 189 – 200. doi:http://dx.doi.org/10.1016/j.automatica.     2015.01.005. γ σ β γ −σ −β URL http://www.sciencedirect.com/science/article/  kp   kp  pii/S0005109815000060 Pu = σ ϑ α ,Pl = −σ ϑ −α ,  Vi   Vi  [20] T. W. U. Madhushani, D. H. S. Maithripala, J. M. β α 1 −β −α 1 Berg, Feedback regularization and geometric pid control for trajectory tracking of coupled mechanical systems: Hoop robots on an inclined plane, in: 2017 American Control T ϑmax {ϑs, ϑb}, and zν [||oI ||ν ||ηe||ν ||ωe||ν ] . It can Conference, Seattle, WA, USA, May 24–26. , , be shown that Pl positive definite as long as kp satisfies [21] F. Bullo, A. D. Lewis, Geometric Control of Mechanical (31). Since Systems, Vol. 49 of Texts in Applied Mathematics, Springer Verlag, New York-Heidelberg-Berlin, 2004. 4 4 4 ! 1 m l r λmax(Pu) W ≥ (m + m )2r4 + λ2 ( ) λ (P ) − i i ||z ||2, [22] A. M. Bloch, Nonholonomic Mechanics and Control, b i min Ib min l 2 s 2 λ (Ii) Springer-Verlag New York, 2003. min [23] O. C. Stoica, On singular semi-riemannian manifolds,, it follows that, W is positive definite, if kp is chosen International Journal of Geometric in Modern Physics, such that (31) and 0(0):1450041doi:10.1142/S0219887814500418. URL http://arxiv.org/abs/1105.0201 2 [24] J. E. Marsden, T. S. Ratiu, Introduction to Mechanics λmin(Ii) λmin(Ib) + r (mb + mi) λmax(Pu) and Symmetry: A Basic Exposition of Classical Mechanical 2 2 2 > , m l r λmin(Pl) Systems, Springer Publishing Company, Incorporated, 2010. i i

are satisﬁed. Diﬀerentiating Wν along the dynamics of A Proof of Theorem-1: the closed loop system we have

Consider a compact set X ⊂ 2×so(3)×so(3), constant W˙ = h ∇ν o , βω + γo + ση i s R ν Iν ωe I e I e ν ν ka > 0, and some small > 0. Define ηe e3oe and let + h ∇ ω , ω + αη + βo i + h ∇ η , αω + σo i. , b Iν ωe e e e I Iν ωe e e I Vν be such that dVν = Iν ηe. For a given α, β, γ, σ > 0 2 define the function Wν : R × so(3) × so(3) → R as for ν equal to b or s while 1 γ ˙ Ri ai Wν kpVν (ηe) + hhωe, ωeiiν + hhoI , oI iiν Wai = hIa ∇ω e3oI , βe3ωe + γe3oI + σe3ηei , 2 2 i i b b b b + h Ri ∇ai e ω , e ω + αe η + βe o i + αhhηe, ωeiiν + βhhoI , ωeiiν + σhhoI , ηeiiν , Iai ωi b3 e b3 e b3 e b3 I + h Ri ∇ai e η , αe ω + σe o i. Iai ωi b3 e b3 e b3 I and define the function W : R2 × so(3) × so(3) → R as

Then taking the closed loop dynamics (25), (27), (28), and (21) into account we can show that W = W (η , ω , o ) + W (η , ω , o ) − W (e η , e ω , e o ). s e e I b e e I ai b3 e b3 e b3 I

W˙ = W˙ s + W˙ α − W˙ a = DWs + DWb − DWa where Ws corresponds to the singular right invariant + h + (k η + k ω + k o ), ω + αη + βo i, Reimannian metric hh·, ·iis, Wb corresponds to the left τref I p e d e I I e e I invariant Reimannian metric hh·, ·iib, and Wai corre- + hτe + I∆d + I∆g, ωe + αηe + βoI i, sponds to the left invariant Reimannian metric hh·, ·iiai . In the following for notational simplicity we will only where consider the case of a single actuator. DWν , −βkI hIν oI , oI i − (αkp − σ)hIν ηe, ηei Let Wu be the set contained by the smallest level set of − kdhIν ωe, ωei + (γ − αkI − βkp)hIν oI , ηei W that contains X and let ks > 0 be the smallest value + hσIν ∇ωe ηe − (kI + βkd)Iν ωe, oI , i such that ||zs||< ks for all (oe, ωe, oI ) ∈ Wu where zs , T + αh ∇ η , ω i + (β − αk )h ω , η i, [||ηs||2 ||ωe||2 ||oI ||2] . Let Wl be the set contained by the Iν ωe e e d Iν e e

13 for ν equal to b and s while Then we have W˙ ≤ −(Ξ||zs||−g4)||zs|| on Wu provided that ||ωi(t)||< ka. The right hand side of this inequality DW −βk h Ri e o , e o i − (αk − σ)h Ri e η , e η i is negative if ai , I Iai b3 I b3 I p Iai b3 e b3 e − k h e ω , e ω i + (γ − αk − βk )h Ri e o , e η i d Iai b3 e b3 e I p Iai b3 I b3 e 2 3(||I∆d||+||I∆g||) + g1ka Ri ai Ri + hσ ∇ e3ηe − (kI + βkd) e3ωe, e3oI , i ||zs||> c = . Iai ωi b Ia b b Ξ + αh Ri ∇ e η , e ω i + (β − αk )h Ri e ω , e η i. Iai ωi b3 e b3 e d Iai b3 e b3 e If λmin(Ql), λmin(Qu) > 0 the condition ν Let µν = max||Iν ∇ηe || on Xu, µmin = min{µs, µb, µai }, and µ = max{µ , µ , µ }. Also let λ ( ) λ ( ) + r2(m + m ) max s b ai min Ii min Ib b i λmin(Qu) + g0I 2 2 2 > mi li r λmin(Ql) − g0I kI kI kI (kI + kpkd) 2kI α = 2 , β = , γ = 2 , σ = . kd kd kd µ ensures that Ξ > 0. We have shown that it is possible to find gains kp, kI , kd such that λmin(Ql), λmin(Qu) are ar- Then using Lemma-2 proven below, and the proper- bitrarily large. Thus for a given , ks, ka > 0 it is possible T ties of the inner product we have −zν Quzν ≤ DWν ≤ to find gains kp, kI , kd such that c < . Thus it is possi- T −zν Qlzν , for all (oe, ωe, oI ) ∈ Wu where for δ , (1 − ble to find gains kp, kI , kd such that W˙ < 0 on Wu/Wl µmin/µmax) for bounded disturbances and reference velocities and parametric uncertainty, provided that ||ωi(t)||< ka and 2  kI  0 −δkI the actuator satisfies kd 2  (kI −αkd)  Ql =  0 (αkp − 2kI /µmin)  , 2 2kd λmin(Ii) λmin(Ib) + r (mb + mi)  2  (kI −αkd) 2 2 2 −δkI kd − αµmax m l r 2kd i i  2  λ (P ) λ (Q ) + g kI max u min u 0 I 0 δkI > max , kd 2 λmin(Pl) λmin(Ql) − g0  (kI −αkd)  I Qu =  0 (αkp − 2kI /µmax) −  . 2kd  2  (kI −αkd) Without loss of generality we will assume that at the δkI − kd − αµmin 2kd design stage the actuator is choses such that λmin(Ii) is sufficiently larger than m l2. Thus proving that It can be shown that we can pick gains such that i i (o (t), ω (t), o (t)) can be made to converge to an ar- λ (Q ) is arbitrary and α, β < 1. e e I min ν bitrarily small neighborhood of (0, 0, 0) provided that ||ω (t)||< k . Since W is quadratically bounded from Since τ is quadratic in the velocity we see that i a e below the convergence is guaranteed to be exponential. hτ , ω +αη +βo i ≤ g ||ω ||2||z ||+g ||ω ||||z ||2+g ||z ||3. e e s I 1 i s 2 i s 3 s If the disturbances and the velocity references are Then we have constant then the Lasalle’s invariance theorem says that the trajectories converge to the largest invariant W˙ ≤ − (λ (Q ) − 9g )(||z ||2+||z ||2) + (λ (Q ) + 9g ) ||z ||2 ˙ min l 0 I s b s s min u 0 I s a set in the set where W ≡ 0. These invariant sets are 3 2 2 3 + ref ||zs|| +g1||ωi||2||zs||+g2||ωi||2||zs|| +g3||zs|| exactly the equilibrium solutions of (27) – (28) and + 3(||I∆d||+||I∆g ||)||zs|| (25) and take the form (0, 0, o¯I ). Thus we see that if 4 4 4 ! 2 4 2 χur li mi 2 the disturbances and the velocity references are con- ≤ − χl(λmin(Ib) + r (mb + mi) ) − − g2 ||zs|| λ2 ( ) min Ii stant then limt→∞(oe(t), ωe(t), oI (t)) = (0, 0, o¯I ) for all 3 2 + (ref + g3)||zs|| + 3(||I∆d||+||I∆g ||) + g1||ωi||2 ||zs|| (oe(0), ωe(0), oI (0)) ∈ X provided that ||ωi(t)||< ka. The convergence is exponential since W is bounded below by a positive definite quadratic form. for all (oe, ωe, oI ) ∈ Wu where χl , (λmin(Ql) − g0I), χu , (λmax(Qu) + g0I), and I and ref are small constants that depend on the uncertainty of the knowledge We will now show that the exponential convergence of of the system parameters. limt→∞(oe(t), ωe(t), oI (t)) = (0, 0, o¯I ) guarantees the existence of a ka > 0 such that ||ωi(t)||≤ ka for all We will show below that there exists a compact set Xa ⊂ t > 0 and (Ri(0), ωi(0)) ∈ Xa. The exponential conver- SO(3) × so(3) such that limt→∞(oe(t), ωe(t), oI (t)) = gence implies that there exists κ > 0 such that ||ωe(t)||≤ −κt (0, 0, o¯I ) exponentially and ||ωi(t)||2< ka for all t > 0 for ||ωe(0)||e . Since from Lemma-1 and (26) it follows any (o (0), ω (0), o (0)) ∈ X and (R (0), ω (0)) ∈ X . ¯ e e I s i i a that limt→∞ ωe(t) = 0 implies that limt→∞(˜τui (t)−τ˜) , 2 Let g4 3(|| ∆d||+|| ∆g||) + g1ka , and τa = 0 exponentially, we have that there exists ν > 0 , I I −κt such that ||τa||< νe . This constant κ is an increasing 4 4 4 ! ! χum l r function of (λmin(Ql) − g0 ) and since we have shown Ξ χ λ2 ( ) + r4 χ (m + m )2 − i i − g − (g + )k . I , l min Ib l b i 2 2 3 ref s λmin(Ii) that λmin(Ql) can be arbitrarily assigned it follows that

14 we can make κ arbitrarily large as well provided that the parametric uncertainty is suﬃciently small.

Consider the positive semi-definite function Wi : SO(3)×so(3) 7→ R defined to be Wi = Vi +hhωi, ωiii/2. The derivative of this function along the dynamics of the −κt closed loop system satisfies W˙ i ≤ 2 (ν + ksgi) e Wi. 2(ν+ksgi) This gives that Wi ≤ Wi(0) exp κ . Hence we have that

(ν + k ga) ||v (t)||≤ (2V (0) + ||v (0)||2) exp s 1 . a a a κ

0 2 0 Let ka > 0 be such that (2Vi(Ri) + ||ωi|| ) ≤ ka on Xa. We have shown that by picking suﬃciently large PID gains kp, kI , kd one can make λmin(Qs) and hence κ suﬃciently large. Thus there exists gains such that (ν+k g ) 2 s i (2Vi(0) + ||ωi(0)|| ) e κ can be made less than ka > 0 ka for all (Ri, ωi) ∈ Xi and hence ensure that ||ωi(t)||< ka for all time t > 0. 2

Lemma 2 Let V be a Morse function on a smooth Rie- mannian manifold G with a metric induced by the inertia tensor I. Let η ∈ TG be the gradient of V (ie. dV = Iη). For a given compact K ⊂ G there exists κ > 0 and 0 ≤ δ ≤ 1 such that ||κI∇η − In×n||≤ δ on K.

Proof of Lemma-2

Since

2 2 hhζ, κ∇ζ η − ζii = κhhζ, ∇ζ ηii − ||ζ|| ≤ (κ||I∇η||−1)||ζ|| , we have that ||κ∇η − In×n||= |(κ||I∇η||−1)|< 1 as long as 0 < κ||I∇η||< 2. By assumption V is a Morse function and hence it is non-degenerate at all its critical points thus we have that µmin < ||I∇η||< µmax for some 0 ≤ µmin < µmax on K. This implies that κ should satisfy 0 < κ < 2/µmax. Choose κ = 1/µmax. Then we have that µmin ||κ∇η − In×n|| = |(κ||I∇η||−1)|≤ δ , 1 − . µmax on K. 2