Almost-global tracking for a rigid body with internal rotors

TL;DR

This paper introduces a new PID control law for a rigid body with three internal rotors, enabling almost-global orientation tracking from nearly all initial states, addressing a less-studied problem in geometric control.

Contribution

It presents the first geometric PID control law for internally actuated rigid bodies, expanding the scope of trajectory tracking control methods.

Findings

Achieves almost-global tracking from nearly all initial conditions.

Demonstrates effectiveness through theoretical analysis and simulations.

Addresses a gap in geometric control for internally actuated systems.

Abstract

Almost-global orientation trajectory tracking for a rigid body with external actuation has been well studied in the literature, and in the geometric setting as well. The tracking control law relies on the fact that a rigid body is a simple mechanical system (SMS) on the $3-$dimensional group of special orthogonal matrices. However, the problem of designing feedback control laws for tracking using internal actuation mechanisms, like rotors or control moment gyros, has received lesser attention from a geometric point of view. An internally actuated rigid body is not a simple mechanical system, and the phase-space here evolves on the level set of a momentum map. In this note, we propose a novel proportional integral derivative (PID) control law for a rigid body with $3$ internal rotors, that achieves tracking of feasible trajectories from almost all initial conditions.

Figures

Equations

$${\stackrel{\mathbb{G}}{\nabla }}_{\dot{\gamma }(t)}\dot{\gamma }(t)=-gradV(\gamma (t))+{\mathbb{G}}^{♯}(F(\dot{\gamma }(t)))$$

$$\xi$$

$$\dot{\xi }-{\mathbb{I}}^{♯}a{d}_{\xi }^{\ast }\mathbb{I}\xi$$

$$\dot{R}=R\hat{\Omega }$$

$$\dot{\Omega }-I^{♯}a{d}_{\Omega }^{\ast }I\Omega =u_{ext}$$

$$L(q,\dot{q})=\frac{1}{2}⟨\Omega ,I\Omega ⟩+\frac{1}{2}⟨\Omega +{\Omega }_r,K(\Omega +{\Omega }_r)⟩$$

$$l(\Theta ,\dot{\Theta },\Omega )=\frac{1}{2}⟨\Omega ,I\Omega ⟩+\frac{1}{2}⟨\Omega +{\Omega }_r,K(\Omega +{\Omega }_r)⟩$$

$$\frac{\mathrm{d}}{\mathrm{d}t}\bigg{(}\frac{\partial l}{\partial\Omega}\bigg{)}=ad^{*}_{\Omega}\bigg{(}\frac{\partial l}{\partial\Omega}\bigg{)},$$

$$\frac{\mathrm{d}}{\mathrm{d}t}\bigg{(}\frac{\partial l}{\partial\Omega_{r}}\bigg{)}+\frac{\partial l}{\partial\Theta}=u_{int}$$

$$(I+K)\dot{\Omega }+K{\dot{\Omega }}_r=\Pi \times \Omega ,$$

$$K(\dot{\Omega }+{\dot{\Omega }}_r)=u_{int}$$

$$\dot{R}=R\hat{\Omega },\dot{\Theta }={\Omega }_r$$

$$\left(\begin{array}{cc}I+K& K\\ K& K\end{array}\right)\left(\begin{array}{c}\dot{\Omega }\\ \dot{{\Omega }_r}\end{array}\right)=\left(\begin{array}{c}\Pi \times \Omega \\ 0\end{array}\right)+\left(\begin{array}{c}0\\ u_{int}\end{array}\right)$$

$$⟨J(q,v),\xi ⟩=\ll v,{\xi }_Q(q)\gg$$

$$A(\Theta ,R,{\Omega }_r,\Omega )=A(\Theta ){\Omega }_r+\Omega$$

$$A(\Theta ,R,{\Omega }_r,\Omega )=(I+K{)}^{-1}K{\Omega }_r+\Omega$$

$$A(\Theta ,R,{\Omega }_r,\Omega )={\mathbb{I}}^{-1}(\Theta ,R)J(\Theta ,R,{\Omega }_r,\Omega )$$

$$\Pi =\mathbb{I}((I+K{)}^{-1}K{\Omega }_r+\Omega )=(I+K)\Omega +K{\Omega }_r$$

$$J(\Theta ,R,{\Omega }_r,\Omega )=\mathbb{I}(\Theta ,R)A(\Theta ,R,{\Omega }_r,\Omega )=R\Pi$$

$$S=\{(R_d,{\Theta }_d,{\Omega }_d,{\Omega }_{rd})\in TQ:J({\Omega }_d,{\Omega }_{rd})=\mu \}$$

$$E(g,g_r)=L_{g_r}{g}^{-1}.$$

$$u$$

$$+\frac{\mathrm{d}}{\mathrm{d}t}{E}^{-1}{\mathrm{d}}_{2}E({\dot{g}}_r))g-I^{♯}a{d}_{\xi }^{\ast }I\xi$$

$${\stackrel{{\mathbb{G}}_{\mathbb{I}}}{\nabla }}_{\dot{E}}{\xi }_I={\mathbb{G}}_{\mathbb{I}}^{♯}\mathrm{d}\psi (E).$$

$$u_{int}$$

$$u_{ext}$$

$$-I^{-1}a{d}_{\eta }^{\ast }I\eta )R+\widehat{\dot{{\Omega }_d}}+\widehat{[\Omega ,{\Omega }_d]}-I^{♯}a{d}_{\hat{\Omega }}^{\ast }I\hat{\Omega },$$

$$(I+K)\dot{\Omega }+u_{int}-K\dot{\Omega }=\Pi \times \Omega$$

$$I\dot{\Omega }-\Pi \times \Omega =-u_{int}$$

$$I\dot{\Omega }-I\Omega \times \Omega =-u_{int}+K(\Omega +{\Omega }_r)\times \Omega$$

$${\dot{\Omega }}_r=(I+K{)}^{-1}{u}_{int}-I^{-1}\Pi \times \Omega$$

$${\stackrel{{\mathbb{G}}_{\mathbb{I}}}{\nabla }}_{\dot{E}(g(t),g_r(t))}{v}_e=-k_p{\mathbb{G}}^{♯}\mathrm{d}\psi (E)-k_d\dot{E}-k_I{\xi }_I$$

$$E_{cl}(E,\dot{E},{\mathbb{G}}^{♯}\mathrm{d}\psi (E))$$

Full text

I Introduction

Spacecrafts are actuated either through internal or external mechanisms. External mechanisms include gas jet thrusters while internal mechanisms include spinning rotors and control moment gyros. In the recent past, there has been an increased interest in the design of coordinate-free control laws for simple mechanical systems ([4], defined in section II) which evolve on Lie groups. Results on stabilization of a rigid body, which is an SMS on $SO(3)$, about a desired configuration in $SO(3)$ using proportional plus derivative (PD) control are found in [2], [7], [5]. Geometric tracking of specific mechanical systems such as a quadrotor, which is an SMS on $SE(3)$, and a rigid body, can be found in [13], [12]. Almost-global tracking of a reference trajectory for an SMS on a Lie group implies tracking of the reference from almost all initial conditions in the tangent bundle of the Lie group. A general result on almost-global asymptotic tracking (AGAT) for an SMS on a class of compact Lie groups is found in [16]. In all these results, the rigid body is assumed to be externally actuated and the control torque is supplied through actuators such as gas jets. Almost-global stabilization and tracking of the externally actuated rigid body is, therefore, a sufficiently well studied problem. However, the problem of geometric tracking for an internally actuated rigid body has received much less attention.

Interconnected mechanical systems have been studied in the context of spherical mobile robots in [8], [6], [10] et al.The stabilization of the internally actuated rigid body is studied in [3]. It is shown that any feedback torque on the externally actuated rigid body can be realised with $3$ internal rotors attached to the rigid body. This paper illustrates the fact that despite the feedback forces acting on an externally actuated rigid body, it is still Hamiltonian and behaves like a heavy rigid body. In other words, if a certain class of feedback torque is applied to the rotors, the rigid body with the rotors is a fully actuated SMS on $SO(3)$. This motivates us to study the AGAT problem for a rigid body with rotors as AGAT for an SMS has been studied extensively ([16],[19], [15]).

In [2] and [22] the almost-global asymptotic stabilization (AGAS) problem of a rigid body with $3$ internally mounted rotors is solved using proportional plus derivative (PD) control. In [9], rigid body tracking is achieved using both external and internal actuation using local representation for rotation matrices. In [14], the trajectory tracking problem is considered for a hoop robot with internal actuation such as a pendulum. It is shown that a class of internal actuation configurations exist for which the underactuated mechanical system can be converted to a fully actuated SMS by feedback torques.

AGAT of an SMS on a Lie group is often achieved by a proportional plus derivative type control([19], [16]). A configuration error is chosen on the Lie group with the help of the group operation along with a compatible navigation function. A navigation function is a Morse function with a unique minimum. The closed loop error dynamics, for a control force proportional to the negative gradient covector field generated by the navigation function plus a dissipative covector field, is then an SMS,. This control drives the error dynamics to the lifted minimum of the navigation function on the tangent bundle of the Lie group from all but the lifted saddle points and maxima of the navigation function. As the critical points of a Morse function are isolated, this convergence is almost global. The compatibility conditions in [19] ensure that the error function is symmetric and achieves its minimum when two configurations coincide. In [15], the authors propose an ’integral’ action to the existing PD control law in [16]. The addition of an integral term makes the control law robust to bounded parametric uncertainty.

A rigid body with internal rotors is an underactuated, interconnected simple mechanical system. The control torque provided to the rotors gets reflected through the interconnection mechanism to the rigid body. Due to absence of external forces, the total angular momentum is conserved. This restriction implies that only a certain class of angular velocities can be attained at any configuration in $SO(3)$. Also, due to the presence of quadratic rotor velocity terms, the rigid body alone is not an SMS. We isolate the rigid body dynamics by introduction of feedback control terms in the system dynamics so that the closed loop rigid body dynamics is a fully actuated SMS. Thereafter we apply the existing AGAT control to the rigid body and obtain the corresponding rotor trajectories from the rotor dynamics. As the control objective is to track a suitable reference trajectory on $SO(3)$, the rotor speeds are allowed to be arbitrary. The paper is organised as follows- in section II, after presenting a few mathematical preliminaries, we derive equations for the rigid body with external actuation and with $3$ internal rotors. In section III we append an integral term to the control law for AGAT of an SMS on a Lie group in [19] and propose the AGAT control for the rigid body with $3$ rotors for an admissible class of reference trajectories. In section IV we present simulation results for the proposed control law.

II Preliminaries

This section introduces conventional mathematical notions to describe simple mechanical systems which can be found in [4], [17], [1]. A Riemannian manifold is denoted by the 2-tuple $(Q,\mathbb{G})$, where $Q$ is a smooth connected manifold and $\mathbb{G}$ is the metric on $Q$. $\nabla$ denotes the Riemannian connection on $(Q,\mathbb{G})$ ([21],[23]).The flat map $\mathbb{G}^{\flat}:T_{q}Q\to T_{q}^{*}Q$ is given by $\mathbb{G}(v_{1},v_{2})=\langle\mathbb{G}^{\flat}(v_{1});v_{2}\rangle$ for $v_{1}$,$v_{2}\in T_{q}Q$ and the sharp map is its dual $\mathbb{G}^{\sharp}:T_{q}^{*}Q\to T_{q}Q$, and given by $\mathbb{G}^{-1}(w_{1},w_{2})=\langle\mathbb{G}^{\sharp}(w_{1});w_{2}\rangle$ where $w_{1}$, $w_{2}\in T_{q}^{*}Q$. Therefore if $\{e^{i}\}$ is a basis for $T^{*}_{q}Q$, $\mathbb{G}^{\flat}(v_{1})=\mathbb{G}_{ij}{v_{1}}^{j}e^{i}$ and $\mathbb{G}^{\sharp}(w_{1})=\mathbb{G}^{ij}{w_{1}}_{j}e_{i}.$

Definition 1

(SMS) A simple mechanical system (or SMS) on a smooth manifold $Q$ with a metric $\mathbb{G}$ is denoted by the 7-tuple $(Q,\mathbb{G},V,F,\mathcal{F},U)$, where $V$ is a potential function on $Q$, $F$ is an external uncontrolled force, $\mathcal{F}=\{F^{1}\dotsc F^{m}\}$ is a collection of covector fields on $Q$, and $U\subset\mathbb{R}^{m}$ is the control set. The system is fully actuated if $T_{q}^{*}Q=span\{\mathcal{F}_{q}\}$, $\forall q\in Q$. The governing equations for the above SMS without any control input is given by

[TABLE]

where $gradV(\gamma(t))=\mathbb{G}^{\sharp}\mathrm{d}V(\gamma(t))$ and $\gamma(t)$ is the system trajectory.

Let $G$ be a Lie group and let $\mathfrak{g}$ denote its Lie algebra. Let $\phi:G\times G\to G$ be the left group action in the first argument defined as $\phi(g,h)\coloneq L_{g}(h)=gh$ for all $g$, $h\in G$. The infinitesimal generator corresponding to $\xi\in\mathfrak{g}$ is $\xi_{Q}\in\Gamma^{\infty}(TQ)$ which is defined as $\xi_{Q}(q)=\frac{\mathrm{d}}{\mathrm{d}t}|_{t=0}\phi(exp(t\xi),q)$, where $exp$ denotes the exponential map. The Lie bracket on $\mathfrak{g}$ is $[,]$. The adjoint map, $ad_{\xi}:\mathfrak{g}\to\mathfrak{g}$ for $\xi\in\mathfrak{g}$ is defined as $ad_{\xi}\eta\coloneq[\xi,\eta]$ for $\eta\in\mathfrak{g}$. Let $\mathbb{I}:\mathfrak{g}\to\mathfrak{g}^{*}$ be an isomorphism from the Lie algebra to its dual. The inverse is denoted by $\mathbb{I}^{\sharp}:\mathfrak{g}^{*}\to\mathfrak{g}$. $\mathbb{I}$ induces a left invariant metric on $G$ ([4]), which we denote by $\mathbb{G}_{\mathbb{I}}$ and define by the following $\mathbb{G}_{\mathbb{I}}(g).(X_{g},Y_{g})=\langle\mathbb{I}(T_{g}L_{g^{-1}}(X_{g})),T_{g}L_{g^{-1}}(Y_{g})\rangle$ for all $g\in G$ and $X_{g}$, $Y_{g}\in T_{g}G$. The equations of motion for the SMS $(G,{\mathbb{I}},F)$ where $F\in\mathfrak{g}^{*}$ are derived from (1) given by

[TABLE]

where $g(t)$ describes the system trajectory. $\xi(t)$ is called the body velocity of $g(t)$.

II-A Dynamics of a rigid body with external actuation

Consider a rigid body with external actuation provided through gas jets mounted on the principal axes. Let $I\in\mathbb{R}^{3\times 3}$ denote the moment of inertia of the rigid body in the body frame, $u\in\mathfrak{so(3)}\sim\mathbb{R}^{3}$ be the control vector field applied to the gas jets, $R(t)\in SO(3)$ be the system trajectory and $\Omega(t)$ be the body velocity of $R(t)$. As this is a SMS given by $(SO(3),I,Iu)$, the equations of motion are given by (2) with $F=Iu$ as follows

[TABLE]

II-B Dynamics of a rigid body with internal actuation

In the internal actuation case, for a rigid body with 3 rotors, the configuration space is $Q=SO(3)\times S^{1}\times S^{1}\times S^{1}$ and the configuration variable is denoted as $q=(R,\Theta)$ where $R\in SO(3)$ and $\Theta=\begin{pmatrix}\theta_{1}&\theta_{2}&\theta_{3}\end{pmatrix}^{T}$, $\theta_{i}\in S^{1}$ for $i=1,2,3$. The rotors are assumed to be mounted on the principal axes of the rigid body as shown in Figure  1. The moment of inertia of the rigid body is $I\in\mathbb{R}^{3\times 3}$ in the rigid body frame. $K=diag(k_{1},k_{2},k_{3})$ is the inertia matrix of the $3$ rotors in the rigid body frame, where $k_{i}$ is moment of inertia of $i$th rotor, $i=1,2,3$. In the absence of potential energy, the Lagrangian is chosen to be the kinetic energy and given as

[TABLE]

where $\hat{\Omega}={R^{-1}\dot{R}}$ and $\Omega_{r}=\begin{pmatrix}\dot{\theta}_{1}&\dot{\theta}_{2}&\dot{\theta}_{3}\end{pmatrix}^{T}$. The manifold $Q$ is a trivial principal $G-$ bundle as $Q=G\times S$ where the Lie group is $G=SO(3)$ and the shape space is $S=S^{1}\times S^{1}\times S^{1}$. Using the trivialization in $Q$ it can be shown that $(TQ)$ is locally diffeomorphic to $TS\times\mathfrak{g}\times G$. Therefore, $(TQ)/G$ is diffeomorphic to $TS\times\mathfrak{g}$. Further, as the lagrangian in (4) is invariant under the action of the $G$, it reduces from a function on $TQ$ to a function on $(TQ)/G$. By the local trivialization coordinates on $(TQ)/G$ are $(\Theta,\Omega_{r},\Omega)$ where, $(\Theta,\Omega_{r})$ are coordinates for $TS$ and $\Omega$ is the coordinate for $\mathfrak{g}$. Therefore, the reduced Lagrangian $l:(TQ)/G\to\mathbb{R}$ is

[TABLE]

The variational principle with the reduced Lagrangian is applied by dividing variations $\delta q$ of $q$ into those only in $\Theta$ and those only in $R$. In $\Theta$, we get the usual Euler Lagrange equations, while in $R$, we obtain Euler Poincare equations. The equations of motion are together called Hamel equations ([18])

[TABLE]

where $u_{int}\in T_{\Theta}(S^{1}\times S^{1}\times S^{1})$ is the control input applied to the rotors and the reconstruction equation is given as $\hat{\Omega}={R^{-1}\dot{R}}$. Substituting for $l$ from (5) in (6),

[TABLE]

where $\Pi=(I+K)\Omega+K\Omega_{r}$ is the body angular momentum. The reconstruction equation for attitude of spacecraft and angular displacement of rotors is given by

[TABLE]

respectively.

Remark: (7) can be written as

[TABLE]

Therefore, (8) is an underactuated SMS.

III Geometric objects and admissible trajectories

The momentum map $J:T(SO(3)\times S^{1}\times S^{1}\times S^{1})\to\mathfrak{so(3)}^{*}$ gives the conserved quantity along trajectories to (6) as the Lagrangian (in (4)) is invariant with respect to action of $SO(3)$. $J:TQ\to\mathfrak{g}^{*}$ is defined as

[TABLE]

for $(q,v)\in TQ$ and $\xi\in\mathfrak{g}$. The mechanical connection $A:TQ\to\mathfrak{g}$ is expressed in terms of connection coefficient $A(\Theta)$ in the body frame as

[TABLE]

$A(\Theta):T_{\Theta}(S^{1}\times S^{1}\times S^{1})\to\mathfrak{so(3)}$ is the obtained from (4) as $A(\Theta)=(I+K)^{-1}K$ (details in [20]) and therefore,

[TABLE]

The locked inertia tensor is $\mathbb{I}(\Theta,R):\mathfrak{so(3)}\to\mathfrak{so(3)}^{*}$ in body frame is the obtained from (4) as $\mathbb{I}=(I+K)$ and in the inertial frame as $\mathbb{I}(\Theta,R)=R(I+K)R^{-1}$. It is observed that

[TABLE]

where $J:TQ\to\mathfrak{g}$ is defined in (9). Details of this result can be found in [18] and [17]. Therefore, from (12), the momentum map in rigid body frame is

[TABLE]

and the momentum map in the inertial frame gives the conserved angular momentum which is

[TABLE]

Here we assume the reference trajectory is generated by another rigid body with $3$ rotors. If the spatial angular momentum of the system is $\mu$, any reference trajectory must lie in the $\mu$ level set of $J$. In other words, the set of reference trajectories is given as

[TABLE]

where $\mu$ of the system is given by (13) and $\Omega_{d}=T_{R_{d}}L_{R_{d}^{-1}}\dot{R}_{d}$.

IV AGAT control

In this section we first state the result from [19] for AGAT of a fully actuated rigid body with external actuators and subsequently extend it to AGAT for the rigid body with $3$ internal rotors.

Definition 2

A function $\psi:G\to\mathbb{R}$ on a Lie group $G$ is a navigation function ([11]) if

$\psi$* has a unique minimum.* 2. 2.

All critical points of $\psi$ are non-degenerate; $Det(Hess\psi(q))\neq 0$ whenever $\mathrm{d}\psi(q)=0$ for $q\in G$.

Definition 3

The configuration error on a Lie group $G$ is the map $E:G\times G\to G$ defined as

[TABLE]

Definition 4

Consider a Lie group $G$ and a navigation function $\psi:G\to\mathbb{R}$. The configuration error map $E$ is compatible with a navigation function $\psi$ for the tracking problem if

• •

$\psi\circ E:G\times G\to\mathbb{R}$* is symmetric; or, $\psi(E(g,h))=\psi(E(h,g))$ for all $g,h\in G$.*

• •

$E(e)=q_{0}$, where $q_{0}$ is the minimum of the navigation function and $e$ is the identity of $G$.

The AGAT problem for a rigid body with rotors is solved in two parts. In the first part a PID control law is proposed for AGAT of a rigid body with external actuation and in the second part a feedback control law is chosen so that the equations for a rigid body with rotors reduce to an externally actuated rigid body. The first problem is well addressed in literature ([15], [16], [19]). In [19], a proportional derivative (PD) and feed-forward (FF) control law achieves AGAT of a reference trajectory on $SO(3)$. We introduce an additional integral control term to the PD+FF tracking control along the lines of [15] in the following theorem.

Theorem 1

(AGAT for an SMS on a Lie group) Let $G$ be a compact Lie group and $\mathbb{I}:\mathfrak{g}\to\mathfrak{g}^{*}$ be an isomorphism on the Lie algebra. Consider the SMS on the Riemannian manifold $(G,{\mathbb{I}})$ given by (2) and a smooth reference trajectory with bounded velocity $g_{r}:\mathbb{R}\to G$ on the Lie group. Let $\psi:G\to\mathbb{R}$ be a navigation function compatible with the error map in (16). Then there exists an open dense set $S$ in $G\times\mathfrak{g}$ such that AGAT of $g_{r}$ is achieved for all $(g(0),\xi(0))\in S$ with $u=\mathbb{I}^{\sharp}(F)$ in (2) given by the following equation

[TABLE]

where $\eta:=T_{E}L_{E^{-1}}\dot{E}$, $k_{p}$, $K_{d}$ and $k_{I}$ are constants to be chosen as shown in Appendix A and $\xi_{I}$ is defined as

[TABLE]

Proof:

Appendix  A. ∎

Theorem 2

(AGAT for a rigid body with $3$ rotors) Consider the rigid body with $3$ rotors in (7) and a smooth, bounded reference trajectory $R_{d}:\mathbb{R}\to SO(3)$ so that $(R_{d},\Omega_{d})\in S$ given by (15) for some $\Omega_{rd}\in\mathbb{R}^{3}$. Let $\psi:G\to\mathbb{R}$ be a navigation function compatible with the error map in (16). Then there exists an open dense set $P$ in $G\times\mathfrak{g}$ such that AGAT of $g_{r}$ is achieved for all $(g(0),\xi(0))\in P$ with $u_{int}$ in (7) given by the following equation

[TABLE]

where

[TABLE]

$P$* is a positive definite symmetric matrix, $E=R_{d}R^{-1}$ and $\xi_{I}$ is defined in (18).*

Proof:

Substituting for $K\dot{\Omega}_{r}$ from the second equation in (6) to the first

[TABLE]

Therefore,

[TABLE]

From (13),

[TABLE]

From (2), the left hand side is a SMS on $SO(3)$ similar to (3) where $\Omega$ is the body velocity of the rigid body and the right hand side is the control field which is to be designed for AGAT of the rigid body. Therefore, theorem  1 is applicable. As the objective is AGAT of the rigid body and not the rotors, we set $\breve{u_{ext}}=-u_{int}+K(\Omega+\Omega_{r})\times\Omega$ where $u_{ext}\in\mathfrak{so(3)}$ is obtained from (17) by choosing $\psi(R)=trace(P(I-R))$ for a positive definite symmetric matrix $P$ and $E\coloneq R_{d}R^{-1}$ is the configuration error defined in (16). It can be shown that $\psi$ is a navigation function compatible with $E$. Details of this result can be found in [19]. The rotor dynamics is given by substituting for $\dot{\Omega}$ from (22) in (7) as

[TABLE]

∎

Remark: In [15], the externally actuated rigid body is allowed to have bounded parametric uncertainty in inertia and actuation models and AGAT is achieved for the proposed PID control law. For the rigid body with rotors, however, the presence of bounded parametric uncertainty and bounded constant disturbances leads to semi-global convergence as shown in [14] for interconnected mechanical systems.

V Simulation results

Consider a rigid body with rotors having the following parameters $I=\begin{pmatrix}4&1&1\\ 1&5.2&2\\ 1&2&6.3\end{pmatrix}$,$K=diag(5,6,7)$ and the following initial conditions $R(0)=\begin{pmatrix}0.36&0.48&-0.8\\ -0.8&0.6&0\\ 0.48&0.64&0.60\\ \end{pmatrix}$, $\Omega(0)=I^{-1}\begin{pmatrix}1&2.2&5.1\end{pmatrix}$ and $\Omega_{r}(0)=\begin{pmatrix}.5&1.9&1.5\end{pmatrix}$. The reference trajectory is generated by a dummy rigid body with rotors having the following parameters $I_{d}=\begin{pmatrix}1&0&0\\ 0&1.2&0\\ 0&0&2\\ \end{pmatrix}$, $K_{d}=diag(4,3,2)$ and the following initial conditions $R_{d}(0)=id(3)$, $\Omega_{d}(0)=I_{d}^{-1}\begin{pmatrix}-0.8&-0.3&-0.5\end{pmatrix}^{T}$, $\Omega_{rd}(0)$ given by the momentum conservation equation (15) and, a constant input vector field $u_{int}=\begin{pmatrix}0;0;0\end{pmatrix}^{T}$ is applied. The dynamics of the dummy rigid body is given by (7). In order to find $K_{p}$, $K_{d}$ and $K_{I}$ we use the bounds in Appendix  A. $\mu=2*(\lambda_{min}(I)+\lambda_{max}(I))/\lambda_{min}(I)=2.0048$, $\lambda=2\frac{\lambda_{max}(I)}{\lambda_{min}(I)^{2}}=1.42$. We choose $K_{I}=1$, $K_{d}=3$ and $K_{p}=1$ to find $u_{1}$ in (17) and subsequently $u$ in (19). The simulation results are shown in figure  3. (https://www.dropbox.com/s/7ji88vri7mp2pxd/circle.avi?dl=0 ) is a video link showing $3$-D tracking of the axis representation in the quaternion representation. In both the cases, the reference trajectory is in red and the controlled trajectory is in blue and the plots show both the trajectories in matrix representation of $SO(3)$.

Now the reference trajectory is generated by the same dummy rigid body with $u_{int}=\begin{pmatrix}sin(t)&cos(t)&sin(t)\end{pmatrix}$ and $u_{int}=\begin{pmatrix}0.2&0.1&0.2\end{pmatrix}$. The simulations are shown in figures  4 and  5 respectively.

We compare the control effort by considering the $2-$ norm of $\breve{u_{ext}}$ in (20) with the AGAT control for a rigid body in [15]. The trace function is considered as a navigation function on $SO(3)$ with the same $P$, $k_{d}$, $k_{p}$ and $k_{I}$ values for both the simulations. The trajectories for tracking the same reference are plotted in figure  6. The control law for internal actuation is obtained from (19) for the AGAT tracking law in [15] and compared with the proposed control in figure  7.

Appendix A

We define $v_{e}\coloneq\dot{E}$ where $E(g(t),g_{r}(t))$ is the error trajectory on $G$ defined in (16). The error dynamics for the SMS in (2) is

[TABLE]

where $\xi_{I}$ is defined in (18). Therefore, $u_{i}\in T_{E}G$. Consider $E_{cl}:TG\times TG\to\mathbb{R}$ given as

[TABLE]

for some constants $k_{p}$, $k_{I}$, $k_{d}$, $\alpha$, $\beta$, $\tau$, $\delta$ that will be determined shortly. The following shows $E_{cl}$ is negative semi-definite. We essentially follow the proof outlined in [15].

Remark: The objective is to show that $E_{cl}$ is non-increasing along trajectories to (7). The derivative of the first term is given by (25). Therefore, appropriately weighted cross terms in (26) must be introduced so that $E_{cl}$ is negative semi-definite.

[TABLE]

Grouping together terms and using the fact that $\nabla_{\dot{E}}\xi_{I}=\mathbb{G}^{\sharp}\mathrm{d}\psi(E)$, we have,

[TABLE]

From (25),

[TABLE]

The $Hess(\psi(q))$ is a $(0,2)$ tensor on $T_{q}G$ defined as $Hess(\psi(q))(X,Y)=\ll\nabla_{X}\mathbb{G}^{\sharp}\mathrm{d}\psi(q),Y\gg$ for $X$,$Y\in T_{q}G$, $q\in G$. As $\psi$ is a navigation function with a unique minimum at $q_{0}\in G$, there exists an compact neighborhood $S_{q_{0}}$ of $(q_{0},0,0)$ in $TG\times TG$ in which the $Hess(\psi)$ is postive-definite and $||Hess(\psi)||_{2}$ is bounded. This implies there is a $\mu>0$ such that $\ll\nabla_{X}\mathbb{G}^{\sharp}\mathrm{d}\psi(q),Y\gg<\mu\ll X,Y\gg$ for all $(q,X,Y)\in S_{q_{0}}$. Therefore,

[TABLE]

So,

[TABLE]

where $v=\begin{pmatrix}||\dot{E}||&||\mathbb{G}^{\sharp}\mathrm{d}\psi(E)||&||\xi_{I}||\end{pmatrix}$ and

[TABLE]

We shall now express $\alpha$, $\beta$, $\delta$ and $\tau$ in terms of $k_{p}$, $k_{d}$ and $k_{I}$. We set $\alpha=\frac{\beta}{k_{d}}$ so that $Q_{12}=Q_{21}=0$. Next, we choose $\tau=k_{p}\beta+\alpha k_{I}$ makes $Q_{23}=Q_{32}=0$. Let $\beta=\frac{k_{I}}{k_{d}}$ and $\delta=2\kappa k_{I}$ such that $\frac{1}{\mu}<\kappa<\frac{2}{\mu}$. This makes $Q_{13}=Q_{31}=\mu\delta-2k_{I}=-\sigma k_{I}$ where $\sigma>0$.

$Q_{11}=k_{d}-\mu\alpha=k_{d}-\mu\frac{k_{I}}{k^{2}_{d}}$,

$Q_{22}=\frac{k_{I}}{k^{2}_{d}}(k_{p}-2\kappa k_{d}^{2})$ and $Q_{33}=\frac{k_{I}^{2}}{k_{d}}$. Then,

[TABLE]

It is observed that the leading principal minors of $Q$ are positive if $k_{p}>2\kappa k_{d}^{2}$ and $0<k_{I}<\frac{k_{d}^{3}}{\mu}(1-\sigma^{2})$. Let $\lambda\coloneq\mathrm{sup}_{S_{q_{0}}}\frac{\ll\mathbb{G}^{\sharp}\mathrm{d}\psi(E),\mathbb{G}^{\sharp}\mathrm{d}\psi(E)\gg}{2\psi(E)}$. It can be shown ([15]) that $E_{cl}$ is positive definite for all $(E,\dot{E},\mathbb{G}^{\sharp}\mathrm{d}\psi(E))\in S_{q_{0}}$ with $k_{p}>\lambda\frac{(\sigma^{2}+\tau\alpha^{2})}{(\tau-\beta^{2})}=\lambda\frac{4\kappa^{2}k_{I}k_{d}^{6}+k_{I}^{3}+k_{I}^{2}k_{d}k_{p}}{k_{p}k_{d}^{5}}$. Therefore, we choose k_{p}>max\Bigg{\{}2\kappa k_{d}^{2},\frac{\lambda k_{I}^{2}}{2k_{d}^{4}}\left(1+\sqrt{1+\frac{4k_{d}^{3}(k_{I}^{2}+4\kappa^{2}k_{d}^{6})}{\lambda k_{I}^{3}}}\right)\Bigg{\}} so that both $Q$ and $E_{cl}$ are positive definite.

The error dynamics (25) is a dissipative SMS with a control vector field proportional to gradient of a navigation function. From the result in [11], therefore, $u_{i}$ achieves AGAS of the error dynamics. From [19], AGAT control for (3) is given by

[TABLE]

ACKNOWLEDGMENT

A part of this work was carried out when the second author was visiting IIT-Gandhinagar. Ravi N Banavar acknowledges with pleasure the support provided by IIT-Gandhinagar.