Geometric Integral Attitude Control on SO(3)

Abstract

This article proposes a novel integral geometric control attitude tracking scheme, utilizing a coordinate-free representation of attitude on the Lie group of rigid body rotations, SO(3). This scheme exhibits almost global asymptotic stability in tracking a reference attitude profile. The stability and robustness properties of this integral tracking control scheme are shown using Lyapunov stability analysis. A numerical simulation study, utilizing a Lie Group Variational Integrator (LGVI), verifies the stability of this tracking control scheme, as well as its robustness to a disturbance torque. In addition, a numerical comparison study shows the effectiveness of the proposed geometric integral term, when compared to other state-of-the-art attitude controllers. In addition, software-in-the-loop (SITL) simulations show the advantages of utilizing the proposed attitude controller in PX4 autopilot compared to using PX4’s original attitude controller.

1. Introduction

Classical PID (Proportional-Integral-Derivative) control schemes are widely used in practice and have several applications due to their ease of design and tunable properties. PID controllers create a control input based on a tracking error, which is the difference between the actual output and a desired (reference) output. The control input has three terms: one proportional to the error, one proportional to the time integral of the error, and another term proportional to the time derivative of the error. Using PID feedback has the advantage of eliminating steady-state errors by using an integral term [1]. In addition, when the mathematical model of a plant is not known and hence analytical design methods cannot be used, PID controllers prove to be very useful [2]. The popularity of PID controllers can be attributed partly to their good performance in a wide range of operating conditions and partly to their functional simplicity [3]. PID control schemes have been proposed for rigid body attitude tracking by utilizing local coordinates or quaternions, such as the ones in [4,5,6,7]. However, local coordinate descriptions suffer from singularities while attitude control based on continuous feedback of quaternions suffers from unwinding if antipodal quaternion pairs are not identified [8]. Unwinding occurs when in response to certain initial conditions, a closed-loop trajectory undergoes a homoclinic-like orbit that initiates near the desired attitude equilibrium. For more details on unwinding and its adverse effects on attitude control, see [8,9,10].

Geometric mechanics is the study of mechanical systems evolving on (non-Euclidean) configuration manifolds. This approach results in preserving the geometry of the configuration space without requiring local coordinates or parameters. It is worth noting that the dynamics of mechanical systems are defined on the tangent space of the configuration manifold. An early work extending classical PD (Proportional-Derivative) control to mechanical systems evolving on configuration manifolds is [11], where PD-type control was used to stabilize a desired configuration. In subsequent years, others have proposed various geometric PD-type controllers, such as in [12,13,14,15,16]. In [17], a geometric adaptive PD-type controller utilizing neural networks is shown to compensate for unknown dynamics resulting from wind disturbance. The authors used a neural network to alleviate the impact of wind disturbances by adjusting the weights of the neural network according to an adaptive control law. Another adaptive controller for vertical take-off and landing (VTOL) unmanned aerial vehicles (UAVs) is proposed in [18], in which authors address gyro-bias, unknown inertia, and actuator loss if effectiveness. Another approach to tackle the attitude control problem is utilizing sliding mode control, however, that can result in chattering [19] and structural vibrations [20].

For bounded parameter errors or disturbances, a geometric PD controller can guarantee global boundedness of tracking errors, although they might not converge to zero. By choosing sufficiently large PD gains, the errors can be made arbitrarily small. However, this can result in amplifying undesirable noise, saturating actuators, and requiring large control effort. Overcoming these drawbacks, along with adding robustness, motivates adding a geometric integral term to a PD-type controller.

Research on integral geometric control includes [21], in which the authors consider control of a mechanical system on a Lie group. They propose an integral action, evolving on the Lie group, to compensate for the drift resulting from a constant bias in velocity and torque inputs. However, they assume a constant time-invariant bias and only discuss feedback stabilization and not the feedback tracking problem. The work in [22] defines an integral term by putting the derivative of integral error equal to the intrinsic gradient of the error function plus a velocity error term. However, as the derivative of the integral term is not on the tangent space, the integrator is not intrinsic. Therefore, the integrator depends on the coordinates chosen for the Lie algebra of the Lie group, unlike the intrinsic PID controller proposed in [23]. A more recent work [24] considers the tracking problem and proposes a geometric PID controller for a rigid body with internal rotors. This builds on the previous PID controller designed in [23], where it is shown that an intrinsic (geometric) integral action ensures that tracking errors converge to zero, in response to constant velocity commands. Following up on this work, Ref. [24] develops an intrinsic PID controller on SO(3) for attitude tracking applications, where SO(3) is the Lie group of rigid body orientations (attitudes) in three-dimensional Euclidean space [25]. Note that [23,24] have used a Morse–Bott function in the geometric PID tracking controller designs that give a connected set of equilibria for the tracking error dynamics, whereas we use a Morse function that leads to a set of four disjoint equilibria in the state space for the attitude and angular velocity tracking errors. This means that the proposed controller in this work has a larger domain of attraction for the desired equilibrium than that of [23,24].

Another application utilizing an integral controller is given in [26], where a quadrotor UAV carrying a cable-suspended rigid body is controlled. This work shows how, without an integral term, uncertainties can result in significant deviations for the suspended rigid body in tracking its desired path. Another application of integral control is presented in [27], where multivariable infinite-dimensional systems are dealt with. However, that work only considers linear systems.

The integral geometric control and tracking algorithm proposed here can work in conjunction with attitude estimators such as the one presented in [25], in which nonlinear stochastic estimators on SO(3) with systematic convergence are discussed. Our proposed algorithm can also work in conjunction with trajectory generation algorithms as in [28,29,30,31,32,33,34,35], and UAV safety frameworks as in [36,37,38]. A trajectory generated by any of these methods can be considered as the desired trajectory to be tracked by the algorithm presented in this paper. The two main contributions of this work are: (1) proposing a new nonlinear integral geometric attitude controller that includes a geometric integral term and proving it to be asymptotically stable with an almost global domain of convergence; and (2) providing an analytical proof of robustness to disturbance torques that is confirmed through supporting numerical simulation results. The results of this paper can be generalized to other Lie groups, however, that is currently out of the scope of this project.

This paper is organized as follows. Section 2 formulates the problem by introducing coordinate frames used, reference attitude trajectory, and attitude dynamics. Section 3 discusses tracking error kinematics and dynamics. Section 4 proposes the integral geometric controller, along with its stability proof. It also shows the robustness of the controller to persistent but norm-bounded disturbance torques. Section 5 provides two simulations to show the validity of the purposed control scheme. For the first one, a Lie Group Variational Integrator (LGVI) for discretization and numerical simulation of this integral geometric controller, comparison with a geometric PD type controller, and discussion of the disturbance-free case are shown. The second simulation is the SITL simulation, with a more detailed model and carried out in a more realistic simulation environment. Finally, Section 7 provides concluding remarks and lists some directions for future work.

2. Problem Formulation and Preliminaries

The treatment in this paper is general and can be applied to vehicles whose motion can be modeled as rigid bodies in three-dimensional Euclidean space, e.g., spacecraft, unmanned underwater vehicles, and unmanned aerial vehicles.

2.1. Coordinate Frames

The two coordinate systems used to define the attitude of a rigid body are an inertial coordinate system and a body-fixed coordinate system. The attitude of the vehicle is defined as the rotation from the body-fixed frame to the inertial frame and is denoted $R\in \mathrm{SO}\left(3\right)$. For attitude tracking, we also define a desired attitude trajectory in time, denoted $R^d\left(t\right)\in \mathrm{SO}\left(3\right)$. In addition, we denote by $\mathsf{\Omega }\in {\mathbb{R}}^3$ the angular velocity of the body, while ${\mathsf{\Omega }}^d\in {\mathbb{R}}^3$ denotes the desired angular velocity. $R^d$ and ${\mathsf{\Omega }}^d$ are related through the kinematics equation:

$$\begin{array}{cc}\hfill \dot{R^d}& =R^d{\left({\mathsf{\Omega }}^d\right)}^{\times },\hfill \end{array}$$

where the cross map:${(·)}^{\times }:{\mathbb{R}}^3\to \mathfrak{so}\left(3\right)$ is given by [39]:

$$x^{\times }={\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]}^{\times }=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right].$$

The notation ${(.)}^{\times }$ is also used to denote the cross product, i.e., $y^{\times }z=y\times z$ for any $y,z\in {\mathbb{R}}^3$.

2.2. Reference Attitude Generation

The desired attitude trajectory for the rigid body is assumed to be generated and available a priori. As an example, for rotorcraft UAVs, the desired attitude trajectory can be generated from the position trajectory, by using the known dynamics model and actuation. Let J denote the inertia of a rigid body. The rotational kinematics and dynamics of the rigid body under the action of a control input torque $\tau \in {\mathbb{R}}^3$, respectively, are given by:

$$\begin{array}{cc}\hfill \dot{R}& =R{\mathsf{\Omega }}^{\times },\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill J\dot{\mathsf{\Omega }}& =J\mathsf{\Omega }\times \mathsf{\Omega }+\tau .\hfill \end{array}$$

(2)

The state space, denoted by $\mathrm{TSO}\left(3\right)\approx \mathrm{SO}\left(3\right)\times {\mathbb{R}}^3$, is the tangent bundle of SO(3). The rigid body rotational equations of motion are expressed by Equations (1) and (2).

3. Tracking Error Kinematics and Dynamics on TSO(3)

The attitude tracking error is defined by [39]:

$$\begin{array}{c}\hfill Q={\left(R^d\right)}^{\mathrm{T}}R.\end{array}$$

(3)

Taking the time derivative results in:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \dot{Q}& ={\left(\dot{R^d}\right)}^{\mathrm{T}}R+{\left(R^d\right)}^{\mathrm{T}}\dot{R}={\left(R^d{\left({\mathsf{\Omega }}^d\right)}^{\times }\right)}^{\mathrm{T}}R+{\left(R^d\right)}^{\mathrm{T}}R{\mathsf{\Omega }}^{\times }\hfill \\ \hfill & ={\left(R^d\right)}^{\mathrm{T}}R{\mathsf{\Omega }}^{\times }-{\left({\mathsf{\Omega }}^d\right)}^{\times }{\left(R^d\right)}^{\mathrm{T}}R=Q{\mathsf{\Omega }}^{\times }-{\left({\mathsf{\Omega }}^d\right)}^{\times }Q\hfill \\ \hfill & =Q{(\mathsf{\Omega }-Q^{\mathrm{T}}{\mathsf{\Omega }}^d)}^{\times }=Q{\omega }^{\times },\hfill \end{array}\end{array}$$

(4)

where $\omega =\mathsf{\Omega }-Q^{\mathrm{T}}{\mathsf{\Omega }}^d$ is the body angular velocity tracking error. As a result,

$$\begin{array}{cc}\hfill J\dot{\mathsf{\Omega }}& =J\frac{d}{dt}(\omega +Q^{\mathrm{T}}{\mathsf{\Omega }}^d)=J(\dot{\omega }+{\dot{Q}}^{\mathrm{T}}{\mathsf{\Omega }}^d+Q^{\mathrm{T}}{\dot{\mathsf{\Omega }}}^d)\hfill \\ \hfill & =J(\dot{\omega }+{\left(Q{\omega }^{\times }\right)}^{\mathrm{T}}{\mathsf{\Omega }}^d+Q^{\mathrm{T}}{\dot{\mathsf{\Omega }}}^d)\hfill \\ \hfill & =J(\dot{\omega }+Q^{\mathrm{T}}{\dot{\mathsf{\Omega }}}^d-{\omega }^{\times }{Q}^{\mathrm{T}}{\mathsf{\Omega }}^d)\hfill \end{array}$$

(5)

Figure 1 shows a schematic of tracking errors for a quadrotor UAV model, as the difference between the desired trajectory and actual trajectory. In this figure, $\tilde{b}$ denotes position tracking error and $\tilde{v}$ denotes translational velocity tracking error, both defined in the inertial frame.

4. Main Result

In this section, the proposed control law and its stability analysis are presented, followed by an analysis of its robustness to a bounded but persistent disturbance torque.

4.1. Torque Control Law and Stability Analysis

First, we present a lemma that will be used in the proof of the main result.

Lemma 1.

Let $〈X,Y〉$ denote $tr\left(X^{\mathrm{T}}Y\right)$ and $e_1$, $e_2$, $e_3$ be the unit vectors denoting the standard basis vector in ${\mathbb{R}}^3$, respectively. Let I denote the $3\times 3$ identity matrix and K be:

$$K=\left[\begin{array}{ccc}{k}_1& 0& 0\\ 0& k_2& 0\\ 0& 0& k_3\end{array}\right]\mathit{where}{k}_i\mathit{are}\mathit{distinct}\mathit{positive}\mathit{scalars},$$

and define $\mathcal{U}\left(Q\right)=\langle K,I-Q\rangle$ and $S_K\left(Q\right)$ as:

$$\begin{array}{c}\hfill S_K\left(Q\right)={\displaystyle \sum _{i=1}^3}{k}_i\left(Q^{\mathrm{T}}{e}_i\right)\times e_i.\phantom{x}\end{array}$$

(6)

Then $\frac{d}{dt}\mathcal{U}\left(Q\right)={\omega }^{\mathrm{T}}{S}_K\left(Q\right)$ and $\mathcal{U}\left(Q\right)$ is a Morse function on SO(3).

Lemma 2.

The critical points of $\mathcal{U}\left(Q\right)$, as defined in Lemma 1, are given by $S_K\left(Q\right)=0$. These critical points are non-degenerate, and given by the set:

$$\begin{array}{cc}\hfill \mathcal{C}& =\{I=\mathrm{diag}([1,1,1]),\mathrm{diag}([-1,-1,1]),\mathrm{diag}([1,-1,-1]),\hfill \\ \hfill & \mathrm{diag}\left(\right[-1,1,-1\left]\right)\}.\hfill \end{array}$$

(7)

Furthermore, $\mathcal{U}\left(Q\right)$ has a global minimum at $Q=I$.

The proofs of Lemmas 1 and 2 are given in [40] and are omitted here for brevity. Note that the only assumption for designing the attitude controller is constant inertia parameters of the UAV.

Theorem 1.

Let $k_P,k_D,k_I\in {\mathbb{R}}^{+}$ denote proportional, derivative, and integral feedback gains, respectively, with $k_D>k_I$, and let $S_K\left(Q\right)$ be defined as in Lemma 1. Let $F_I\in {\mathbb{R}}^3\simeq \mathfrak{so}\left(3\right)$ be an integral error term given by:

$$\begin{array}{c}\hfill J{\dot{F}}_I=k_p{S}_K\left(Q\right)+k_D\omega +J\mathsf{\Omega }\times F_I,F_I\left(0\right)=0.\end{array}$$

(8)

Considering the attitude kinematics and dynamics of a rigid body as given by Equations (1) and (2), the control law:

$$\begin{array}{cc}\hfill \tau =& -k_P{S}_K\left(Q\right)-k_I{F}_I-k_D\omega \hfill \\ \hfill & +J(Q^{\mathrm{T}}{\dot{\mathsf{\Omega }}}^d-{\omega }^{\times }{Q}^{\mathrm{T}}{\mathsf{\Omega }}^d)-J\mathsf{\Omega }{Q}^{\mathrm{T}}{\mathsf{\Omega }}^d,\hfill \end{array}$$

(9)

leads to almost global asymptotically stable (AGAS) tracking of $(R^d,{\mathsf{\Omega }}^d)$, where $(Q,\omega )$ are tracking errors given by (3) and (4).

Note that AGAS is defined in the following proof.

Proof. 

By replacing $J\dot{\mathsf{\Omega }}$ expression from Equation (2) into Equation (5) we get:

$$\begin{array}{c}\hfill J\dot{\omega }=J\mathsf{\Omega }\times \mathsf{\Omega }+J({\omega }^{\times }{Q}^{\mathrm{T}}{\mathsf{\Omega }}^d-Q^{\mathrm{T}}{\dot{\mathsf{\Omega }}}^d)+\tau .\end{array}$$

(10)

The closed-loop feedback dynamics is obtained from substituting the proposed controller $\tau$ given by (9) into (10), to get:

$$\begin{array}{c}\hfill J\dot{\omega }=J\mathsf{\Omega }\times \omega -k_P{S}_K\left(Q\right)-k_I{F}_I-k_D\omega .\end{array}$$

(11)

Note that the control torque $\tau$ in Equation (9) is carefully designed such that when we replace it in Equation (10), we get Equation (11). As a result of Equation (11), we get:

$$\begin{array}{c}\hfill {\omega }^{\mathrm{T}}J\dot{\omega }=-k_P{\omega }^{\mathrm{T}}{S}_K\left(Q\right)-k_I{\omega }^{\mathrm{T}}{F}_I-k_D{\omega }^{\mathrm{T}}\omega .\end{array}$$

(12)

Now let $V:\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3\to {\mathbb{R}}^{+}$ be a Lyapunov candidate given by:

$$\begin{array}{c}\hfill V=\overline{V}+k_p\mathcal{U}\left(Q\right),\end{array}$$

(13)

where

$$\begin{array}{c}\hfill \overline{V}=\frac{1}{2}\left\{{\omega }^{\mathrm{T}}J\omega +{(F_I+\omega )}^{\mathrm{T}}J(F_I+\omega )\right\}.\end{array}$$

(14)

Note that by Lemmas 1 and 2, $\mathcal{U}\left(Q\right)$ is a Morse function on SO(3), and therefore V is a candidate Morse–Lyapunov function on $\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$. Taking the time derivative of V in Equation (13) and applying Lemma 1, we get:

$$\begin{array}{c}\hfill \dot{V}=\dot{\overline{V}}+k_p{\omega }^{\mathrm{T}}{S}_K\left(Q\right).\end{array}$$

(15)

The time derivative of $\overline{V}$ in Equation (14) is obtained as:

$$\begin{array}{c}\hfill \dot{\overline{V}}={\omega }^{\mathrm{T}}J\dot{\omega }+{(F_I+\omega )}^{\mathrm{T}}J(\dot{F_I}+\dot{\omega }),\end{array}$$

(16)

where $J\dot{\omega }$ is as expressed in Equation (11), and $J\dot{F_I}$ as in Equation (8). Note that $F_I$ in Equation (8) is thoughtfully proposed such that when we replace $J{\dot{F}}_I$ in Equation (16), $\dot{V}$ becomes as shown in Equation (17). Using Equation (12) and substituting Equation (16) in (15), after some simplifications we get:

$$\begin{array}{c}\hfill \dot{V}=-k_D{\omega }^{\mathrm{T}}\omega -2k_I{\omega }^{\mathrm{T}}{F}_I-k_I{F}_I^{\mathrm{T}}{F}_I.\end{array}$$

(17)

By setting $k_D=k_I+k_{DI}$ where $k_{DI}>0$, we get:

$$\begin{array}{c}\hfill \dot{V}=-k_{DI}{\omega }^{\mathrm{T}}\omega -k_I{(F_I+\omega )}^{\mathrm{T}}(F_I+\omega )\le 0,\end{array}$$

(18)

which is negative semi-definite. Considering Equation (18), the set where $\dot{V}=0$ is:

$$\begin{array}{cc}\hfill {\dot{V}}^{-1}\left(0\right)=& \{(Q,\omega ,F_I)\phantom{x}:\phantom{x}(F_I+\omega )=0\mathrm{and}\omega =0\}\hfill \\ \hfill =& \{(Q,\omega ,F_I)\phantom{x}:\phantom{x}{F}_I=0\mathrm{and}\omega =0\}.\hfill \end{array}$$

(19)

Using the invariance-like theorem 8.4 in [41], we can conclude that as $t\to \infty$, $(Q,\omega ,F_I)$ converges to the set:

$$\begin{array}{c}\hfill \mathtt{S}=\{(Q,\omega ,F_I)\phantom{x}:\phantom{x}{F}_I=0,\omega =0\mathrm{and}{S}_K\left(Q\right)=0\}.\end{array}$$

(20)

From Lemma 2, this is equivalent to:

$$\begin{array}{c}\hfill \mathtt{S}=\{(Q,\omega ,F_I)\phantom{x}:\phantom{x}{F}_I=0,\omega =0\mathrm{and}Q\in \mathcal{C}\}.\end{array}$$

(21)

This means that the closed-loop system given by Equations (4), (8), and (11) has $\mathtt{S}\subset \mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$ as its set of equilibria to which all initial tracking error states ultimately converge. The only stable equilibrium in $\mathtt{S}$ is $(I,0,0)\in \mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$ while the other three are unstable hyperbolic equilibria, which differ from the stable attitude by ${180}^{\circ }$ of rotation about each of the three body-fixed axes. As shown in [8,39,42], these unstable equilibria have stable manifolds that are embedded subsets of $\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$. Therefore, the union of these stable manifolds has measure zero and is nowhere dense in $\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$. This implies that all solutions that converge to the three unstable equilibria lie in a nowhere dense set, whereas almost all closed-loop solutions converge to the desired equilibrium $(Q,\omega ,F_I)=(I,0,0)$. Therefore, the tracking errors $(Q,\omega )$ converge to $(I,0)$ in an asymptotically stable manner from almost all initial conditions. As convergence to this desired tracking error state of the feedback attitude dynamics occurs from almost all initial states except those in a set of zero measure in the state space of rigid-body attitude motion, its stability is almost global with this continuous integral, geometric type state feedback control law. This means that the proposed control law in Equations (8) and (9) leads to almost global asymptotically stable tracking of the desired attitude trajectory $(R^d,{\mathsf{\Omega }}^d)$. □

Note that global attitude stabilization with continuous feedback is not possible due to the non-contractible nature of the compact manifold SO(3) [8,9]. The almost global domain of convergence of the tracking errors $(Q,\omega )$ to $(I,0)$ given by the above result is the largest that can be achieved with continuous state feedback. Further, note that the use of the Morse function leads to a set of four disjoint equilibria in the state space for the attitude and angular velocity tracking errors, $\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\ni (Q,\omega )$, according to Lemma 2 and Theorem 1. This in turn leads to a larger domain of attraction for the desired equilibrium than that obtained by using a Morse–Bott function in the geometric PID tracking controller designs in [23,24], which give a connected set of equilibria for the tracking error dynamics.

4.2. Robustness to Disturbance Torque

The stability result of Theorem 1 guarantees almost global asymptotic convergence of the tracking errors $(Q,\omega )$ to $(I,0)$ when there is no disturbance. In the presence of a bounded disturbance torque D, tracking errors can be shown to converge to a bounded neighborhood of $(I,0)$. Theorem 2 gives a specific relation between the size of the neighborhood of (0, 0) to which the tracking errors $(\omega ,F_I)$ are guaranteed to converge and the bound on the magnitude of the disturbance torque D. Then Corollary 1 shows that, under an additional assumption on the time derivative of D, the attitude tracking error $Q\in \mathrm{SO}\left(3\right)$ converges to a neighborhood of the identity.

Theorem 2.

Consider the neighborhood of $(0,0)\in {\mathbb{R}}^3\times {\mathbb{R}}^3$ defined by

$$\begin{array}{c}\hfill {\overline{\mathcal{N}}}_{(0,0)}:=\{\parallel \omega \parallel \le {\omega }_{max}\phantom{x}and\phantom{x}\parallel F_I+\omega \parallel \le {\zeta }_{max}\},\end{array}$$

(22)

where ${\omega }_{max}$ is the upper bound on $\parallel \omega \parallel$ and ${\zeta }_{max}$ is the upper bound on $\parallel F_I+\omega \parallel$. Let D be a disturbance torque that is bounded in norm by

$$\begin{array}{c}\hfill \parallel D\parallel \le \frac{k_{DI}\phantom{x}{{\omega }_{\mathit{max}}}^2+k_I\phantom{x}{{\zeta }_{\mathit{max}}}^2}{{\omega }_{\mathit{max}}+{\zeta }_{\mathit{max}}},\end{array}$$

(23)

perturbing the attitude dynamics given by Equation (2) as follows:

$$\begin{array}{cc}\hfill J\dot{\mathsf{\Omega }}& =J\mathsf{\Omega }\times \mathsf{\Omega }+\tau +D.\hfill \end{array}$$

(24)

Then, with the control law given by Equation (9), the tracking error signals $(\omega ,F_I)$ converge to the neighborhood ${\overline{\mathcal{N}}}_{(0,0)}$ defined by (22) in an asymptotically stable manner.

Proof. 

Considering the perturbed dynamics (24) and following similar steps as in the proof of Theorem 1, it can be verified that for this perturbed system:

$$\begin{array}{c}\hfill \dot{V}=-k_{DI}{\omega }^{\mathrm{T}}\omega -k_I{(F_I+\omega )}^{\mathrm{T}}(F_I+\omega )+{(2\omega +F_I)}^{\mathrm{T}}D.\end{array}$$

(25)

The $\phantom{x}{(2\omega +F_I)}^{\mathrm{T}}D\phantom{x}$ term is upper bounded by

$$\begin{array}{c}\hfill \parallel (2\omega +F_I)\parallel \phantom{x}\parallel D\parallel \le \left(\parallel \omega \parallel +\parallel \omega +F_I\parallel \right)\phantom{x}\parallel D\parallel ,\end{array}$$

and hence, $\dot{V}$ is upper bounded by

$$\begin{array}{cc}\hfill \dot{V}& \le -k_{DI}{\parallel \omega \parallel }^2-k_I\parallel F_I{+\omega \parallel }^2+\left(\parallel \omega \parallel +\parallel \omega +F_I\parallel \right)\parallel D\parallel .\hfill \end{array}$$

(26)

Consequently, $\dot{V}$ is guaranteed to be negative semi-definite along the boundary of ${\overline{\mathcal{N}}}_{(0,0)}$ if

$$\begin{array}{c}\hfill -k_{DI}\phantom{x}{{\omega }_{\mathit{max}}}^2-k_I\phantom{x}{{\zeta }_{\mathit{max}}}^2+\left({\omega }_{\mathit{max}}+{\zeta }_{\mathit{max}}\right)\phantom{x}\parallel D\parallel \le 0,\end{array}$$

(27)

which gives the sufficient condition in Equation (23) for asymptotic convergence of $(\omega ,F_I)$ to the neighborhood ${\overline{\mathcal{N}}}_{(0,0)}$. Note that this neighborhood is continuous, compact, and connected, and outside this neighborhood $\dot{V}$ is negative. The size of this neighborhood is given by (22). □

While the above result shows asymptotic stability of only the error states $(\omega ,F_I)\in {\mathbb{R}}^3\times {\mathbb{R}}^3$, it does not show that the attitude error Q in SO(3) converges to a neighborhood of the identity matrix. To show the convergence of the attitude error, we state and prove the corollary below.

Corollary 1.

Let $R^d\left(t\right)\in {\mathcal{C}}^2\left(SO\left(3\right)\right)$ be a twice continuously differentiable trajectory on SO(3) that has bounded first and second derivatives. Consider the control law given by Equations (8) and (9) applied to the system given by Equations (1) and (24), where D is bounded as in Equation (23). This leads to the attitude tracking error $Q\left(t\right)={\left(R^d\left(t\right)\right)}^{\mathrm{T}}R\left(t\right)\in \mathit{SO}\left(3\right)$ converging asymptotically to a neighborhood of the identity $I\in \mathit{SO}\left(3\right)$.

Proof. 

We know from Theorem 1 that the trajectory tracking errors $(Q,\omega ,F_I)$ converge asymptotically to $(I,0,0)$ from almost all initial values in $\mathrm{SO}\left(3\right)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$, with the control law (8) and (9) applied to the system (2) without any disturbance torque (i.e., $D=0$). Further, from Theorem 2 we know that the tracking errors $(\omega ,F_I)$ converge to the bounded neighborhood ${\overline{\mathcal{N}}}_{(0,0)}$ defined by (22) when the disturbance torque D is bounded as in (23). The dynamics of the tracking error $\omega$ in the presence of the disturbance torque D is given by:

$$\begin{array}{c}\hfill J\dot{\omega }=J\mathsf{\Omega }\times \omega -k_P{S}_K\left(Q\right)-k_I{F}_I-k_D\omega +D.\end{array}$$

(28)

All the quantities on the right-hand side of Equation (28) are bounded if $R^d\left(t\right)$ is as defined in the statement above, because $\mathsf{\Omega }=\omega +Q^{\mathrm{T}}{\mathsf{\Omega }}^d$ is bounded in that case. Therefore $\dot{\omega }$ is bounded, which means that the first two time derivatives of Q are bounded. From the proof of Theorem 2, we know that the Lyapunov function $V(Q,\omega ,F_I)=\overline{V}(\omega ,F_I)+k_P\mathcal{U}\left(Q\right)$ decreases in value until the tracking errors $(\omega ,F_I)$ converge asymptotically to the neighborhood ${\overline{\mathcal{N}}}_{(0,0)}$. Let $V_{\mathit{max}}^{\mathcal{N}}$ be the maximum value of V at the boundary of this neighborhood. If $J_{\mathit{min}}$ is the minimum eigenvalue of the inertia matrix J, then we know that

$$\overline{V}(\omega ,F_I)\ge \frac{1}{2}{J}_{\mathit{min}}\left({\omega }_{\mathit{max}}^2+{\zeta }_{\mathit{max}}^2\right).$$

Therefore an upper bound on $\mathcal{U}\left(Q\right)$ can be obtained as follows:

$$\begin{array}{cc}\hfill k_P\mathcal{U}\left(Q\right)& =V(Q,\omega ,F_I)-\overline{V}(\omega ,F_I))\hfill \\ \hfill & \le V_{\mathit{max}}^{\mathcal{N}}-\frac{1}{2}{J}_{\mathit{min}}\left({\omega }_{\mathrm{m}ax}^2+{\zeta }_{\mathit{max}}^2\right)\hfill \end{array}$$

(29)

From Lemma 2, we also know that $\mathcal{U}\left(Q\right)=\langle K,I-Q\rangle$ has a global minimum at $Q=I$; the minimum value is $\mathcal{U}\left(I\right)=0$. Therefore, the value of $\mathcal{U}\left(Q\right)$ remains bounded between 0 and the (conservative) upper bound given by the right side of the inequality (29). This implies that the attitude tracking error Q converges to a bounded neighborhood of the identity ($Q=I$) and a conservative bound on the size of this neighborhood is given by (29). □

From Theorem 2 and Corollary 1, we conclude that the tracking errors for the feedback tracking error system given by Equations (4), (8), and (28), converges to a bounded neighborhood of $(Q,\omega ,F_I)=(I,0,0)$.

It is worth noting that the proposed algorithm is computationally lightweight, especially much less demanding when compared to algorithms that utilize neural networks to control quadcopter UAVs. For numerical simulations in the next section, we introduce a time-varying bounded disturbance torque and show how the proposed controller effectively compensates for disturbance, compared to a geometric nonlinear PD-type controller. In addition, we compare the performance of our integral geometric controller with that of a classic non-geometric PID controller.

5. Numerical Simulation

The results of two numerical simulations on the proposed control schemes are presented here. For these two simulations, a quadrotor UAV is simulated during the flight to test the validity of the attitude control scheme. The first simulation uses MATLAB to simulate a quadrotor with a simplified model but maintaining the properties of rigid body dynamics. The second simulation is an SITL simulation for the flight of a commercial quadrotor UAV. The simulation is carried out in the simulation tool Gazebo. The purposed geometric controller is implemented into the open-source autopilot software PX4 to replace its original attitude/attitude-rate controller, which is described with details in [43]. The performances of the proposed geometric controller and original PX4 controller are compared with each other under the same flight task during the same simulation setup.

For the simulation carried out in MATLAB, the complete control of a quadrotor UAV has two loops: the outer loop position control (for translational motion) and the inner loop attitude control (for rotational motion). The attitude should change such that the desired thrust direction required to follow the position trajectory is achieved. In this work, we are looking at the inner loop of attitude control and numerically simulate our integral geometric controller. For the outer loop, we utilize a position controller that enables us to generate the desired attitude trajectory for our proposed attitude tracking controller. Figure 2 shows the block diagram of the control system proposed here for controlling a quadrotor UAV to follow a time-varying desired position and attitude.

For this purpose, we use the following position controller in Equation (18) of [44] for the outer loop:

$$\begin{array}{c}\hfill \mathrm{f}=e_3^{\mathrm{T}}{R}^{\mathrm{T}}\left(mg{e}_3+P\tilde{b}+L_v(R\nu -v_d)-m{\dot{v}}_d\right).\end{array}$$

(30)

Here $P,L_v\in {\mathbb{R}}^{3\times 3}$ are positive definite matrices, $b\in {\mathbb{R}}^3$ is the UAV’s inertial position vector, $\tilde{b}=b-b_d$, and $b_d,v_d\in {\mathbb{R}}^3$ are the desired inertial position and velocity vectors, respectively. To make meaningful comparisons, we use this outer loop position controller in all the following simulations to generate the desired attitude trajectory, while varying the inner loop attitude controller for comparison purposes. The feedback position controller (30) tracks a desired position trajectory in the form of a vertical helix going up in the z direction, as shown with a black line in Figure 3.

In Section 5.1, the numerical integration method for numerical simulations is given. In Section 5.2, numerical simulation of the proposed integral geometric attitude controller under the influence of a disturbance torque is presented. To show the effectiveness of the geometric integral term, we compare the performance of a geometric PD-type attitude controller with our integral geometric controller under the influence of a disturbance torque in Section 5.3. Section 5.4 compares the performance of the integral geometric controller between the cases when disturbance torque acts, and when it does not. Section 6.1 uses an SITL simulation tool to show the performance of the proposed controller in a simulated environment. In addition, its performance is compared with the performance of a benchmark classical (non-geometric) PID attitude controller that is based on Euler angles, under the influence of disturbance torque.

5.1. Discretization as a Lie Group Variational Integrator

In order to numerically simulate the proposed integral geometric attitude control scheme, we discretize the equations of motion in the form of a Lie Group Variational Integrator (LGVI). In contrast to general-purpose numerical integrators, an LGVI preserves the structure of the configuration space without parameterization or re-projection. The LGVI scheme used in this work was first proposed in [45]. The time step for discretization is a constant $h=t_{k+1}-t_k$. Here ${(.)}_k$ denotes a parameter of the system at time step k. The discrete equations of motions are:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill R_{k+1}& =R_k{\mathcal{F}}_k,\hfill \\ \hfill {\left({\mathsf{\Omega }}_{k+1}^d\right)}^{\times }& =\frac{1}{h}\log \left({\left(R_k^d\right)}^{\mathrm{T}}{R}_{k+1}^d\right),\hfill \\ \hfill J{\mathsf{\Omega }}_{k+1}& ={\mathcal{F}}_k^{\mathrm{T}}J{\mathsf{\Omega }}_k+h{\tau }_k,\hfill \end{array}\end{array}$$

(31)

where ${\mathcal{F}}_k\approx exp\left(h{\mathsf{\Omega }}_k^{\times }\right)\in \mathrm{SO}\left(3\right)$ is evaluated using Rodrigues’ formula:

$$\begin{array}{c}\hfill {\mathcal{F}}_k=exp\left({f_k}^{\times }\right)=I+\frac{sin\parallel f_k\parallel }{\parallel f_k\parallel }{f}_k^{\times }+\frac{1-cos\parallel f_k\parallel }{{\parallel f_k\parallel }^2}{\left(f_k^{\times }\right)}^2,\end{array}$$

(32)

where

$$\begin{array}{c}\hfill f_k=h{\mathsf{\Omega }}_k.\end{array}$$

This guarantees that $R_k$ evolves on SO(3). Further details on discretization using LGVI schemes, including the derivation of Equation (31), are given in [45].

5.2. Simulation Results

The quadrotor model considered in the simulations here has the following mass and inertia properties [46]:

$$\begin{array}{c}\hfill J=\mathrm{diag}(0.0820,0.0845,0.1377)\mathrm{kg}\text{-}{\mathrm{m}}^2;\phantom{x}\mathrm{m}=4.34\mathrm{kg}.\end{array}$$

(33)

The time step size used in these simulations is $h=0.01$ s. The control gains are selected as $k_P=10$, $k_I=0.5$ and $k_D=5$. To demonstrate the performance of the proposed controller, and show effectiveness of the novel geometric integral term, we introduce a time-varying disturbance torque to the system. The time-varying disturbance torque consists of the sum of a constant term and sinusoidal terms, as follows:

$$D=\left[\begin{array}{c}{d}_1\\ d_2\\ d_3\end{array}\right]=(3.0123\times {10}^{-2})\left[\begin{array}{c}\sin \left(21\pi t\right)+0.91\\ \cos \left(18.9\pi t\right)-8.1\\ \cos \left(11.2\pi t\right)+1.49\end{array}\right](\mathrm{N}.\mathrm{m}),$$

(34)

where $d_1$, $d_2$, and $d_3$ are time-varying components of the disturbance torque. The magnitude of this disturbance is similar to the torque exerted to the drone by wind gusts. The simulations were done using MATLAB to encode the LGVI algorithm and the control laws.

Figure 3 shows how the position and attitude converge to a neighborhood of the desired trajectory using the proposed attitude tracking control in conjunction with the position tracking control in Equation (30) under the influence of the disturbance torque.

Figure 4 shows the magnitude of the position tracking error over time in the presence of the disturbance torque. It shows this tracking error reaches very low levels within 3 s. Figure 5 shows the associated velocity tracking error magnitude varying with time under the influence of disturbance torque. Figure 6 shows the thrust magnitude required to track the desired trajectory, under the influence of disturbance torque. Note that the thrust here is the sum of the four forces generated by rotors of the quadcopter. Each of these forces vary with time and affect the dynamics of the vehicle according to the Equation (44) in [44], which is omitted here for brevity.

Figure 7 shows components of the angular velocity tracking error, $\omega$, and how within 3 seconds they converge to a small neighborhood of the zero vector such that $\parallel \omega \parallel \le 1.7\times {10}^{-2}$ rad/s, under the influence of disturbance torque. The magnitude of the attitude tracking error is given by the principal angle $\mathsf{\Phi }$ of the attitude tracking error Q. $\mathsf{\Phi }$ is obtained from the following expression:

$$\begin{array}{c}\hfill \mathsf{\Phi }={cos}^{-1}\left(\frac{\left(\mathrm{tr}\right(Q)-1)}{2}\right).\end{array}$$

(35)

Figure 8 shows the time profile of $\Phi$ under the influence of the disturbance torque.

Figure 9 shows the magnitude of the proposed control torque over time under the influence of the disturbance torque.

5.3. Effectiveness of the Geometric Integral Term

The effectiveness of the geometric integral term in the proposed integral geometric attitude control is shown by the following comparison. Under the existence of disturbance torque, we first use the geometric PD-type attitude controller of [44] followed by our proposed attitude controller to track the same trajectory, and compare the results.

Figure 10 shows that for the PD-type controller, components of $\omega$ do not converge to zero but oscillate with noticeable amplitudes about it. On comparing this figure with Figure 7, we see that the proposed controller shows significantly better performance.

Figure 11 shows rapid oscillations in control that will likely not be realizable by, and therefore should not be implemented on, a quadrotor UAV. Comparing Figure 9 with Figure 11, the proposed controller shows remarkably better performance with negligible oscillations. In addition, Figure 11 shows the large magnitude of the required control torque given by the geometric PD-type controller. Note that much less control effort is needed by the proposed controller (Figure 9), compared to the geometric PD-type controller (Figure 11), to track the same trajectory.

Figure 12 shows the norm of the attitude tracking error (given by the principal angle of the rotation matrix Q) for both PD and the proposed controllers. The proposed controller shows this error to decrease more smoothly and with less oscillations than the PD-type controller.

Overall, the comparison in this subsection shows that the proposed controller has significant advantages over a PD-type controller in steady-state performance as well as disturbance attenuation. It tracks the same maneuvering attitude trajectory better while requiring significantly less overall control effort under the influence of a time-varying disturbance torque.

5.4. Comparisons in the Zero Disturbance Case

Here, we make observations based on numerical simulations comparing the performance of the proposed controller under influence of disturbance that was presented in Section 5.2, with its performance when there is no disturbance ($D=0$) while tracking the same desired trajectory. If the disturbance is zero, then the proposed controller gives the following results in numerical simulation.

Figure 13 shows components of the angular velocity tracking errors over time, which converge asymptotically to zero. This case ($D=0$) can be thought of as an ideal case, and by comparing Figure 7 with the above figure, we see there is little change in the performance of the proposed controller under the influence of a disturbance. The time plots in Figure 7 show a similar tracking profile to the ideal case of zero disturbance (Figure 13).

Figure 14 shows the control torque magnitude, as given by the proposed controller when there is no disturbance, i.e., $D=0$. Again, considering this case as an ideal case, we see that the proposed controller performs robustly in the presence of disturbance. The profile in Figure 9 is similar (with minor oscillations) to that of Figure 14. The following zoomed-in Figure 15 shows that when the disturbance torque acts, components of $\omega$ converge to, and remain in, a small neighborhood of the zero vector, which is the expected result from Theorem 2. In addition, note that when there is no disturbance, components of $\omega$ converge to zero, as shown in Figure 16.

To summarize, in this section we compared the performance of the proposed integral geometric attitude tracking controller for two situations, one when there is no disturbance torque (ideal case) and the other when a disturbance torque acts. We observed that under the influence of a disturbance torque the proposed controller performs well, and results in similar but slightly degraded performance, compared to the ideal case of zero disturbance.

6. Software-in-the-Loop and Hardware Implementation

6.1. Software-in-the-Loop (SITL) Simulation

An SITL simulation of the proposed integral geometric attitude controller is carried out to show its performance in a realistic autonomous flight scenario. Our controller is implemented on the PX4 autopilot software [47,48], which is an open-source flight management system composed of different subsystems such as estimation, control, mission planning, and UAV data link. The main PX4 controllers form a standard cascaded control architecture; these controllers are a mix of P and PID controllers. Specifically, position is controlled by a P controller, velocity by a PID controller, attitude by a nonlinear P controller, and angular rate by a PID controller. Block diagrams for the main PX4 controllers are presented in [49], and more details about the attitude control module of PX4 are given in [50]. In this paper, specifically, the rate control and attitude control part of the PX4 is replaced with our integral control scheme (https://github.com/nswang1994/GeometricPX4, accessed on 26 August 2022). The quadcopter simulated is the 3DR Solo, and the simulation tool used to visualize the performance of the controller is Gazebo. During the simulation, the 3DR solo is in autonomous flight mode and assigned to follow a helical trajectory similar to the desired trajectory in Figure 3. An animated video of this simulation that shows the quadcopter is tracking a reference trajectory is given here (https://github.com/nswang1994/ReseachVideos, accessed on 26 August 2022). The flight control performance of the proposed control scheme is compared with the original controller in PX4 autopilot by carrying out the aforementioned autonomous flight scenario. The PX4 autopilot’s control scheme is described with details in [43]. The inertia information of the simulated 3DR Solo quadcopter is shown as follows [48]:

$$\begin{array}{c}\hfill J=\mathrm{diag}(0.011,0.015,0.021)\mathrm{kg}\text{-}{\mathrm{m}}^2;\phantom{x}\mathrm{m}=1.5\mathrm{kg}.\end{array}$$

(36)

The gains used for the proposed control scheme are:

$$\begin{array}{c}\hfill k_P=0.25,k_I=0.06,k_D=0.075\end{array}$$

(37)

Figure 17, Figure 18 and Figure 19 show the performance of the proposed control scheme in the SITL simulation. The results are compared with those from the simulation of the original PX4 attitude controller. In these figures IGAC refers to the proposed Integral Geometric Attitude Controller.

The magnitude of the attitude tracking error is given by the principal angle $\mathsf{\Phi }$ of the attitude tracking error Q, defined by (35) and shown in Figure 17. It can be clearly seen that the proposed geometric control scheme has a better attitude tracking control performance compared to the original PX4 controller. Figure 18 shows that the angular velocity tracking performance of the proposed control scheme is similar to the default controller in PX4. However, it is clear that the latter shows more high-frequency oscillations in angular velocity error than the former. Figure 19, shows that the proposed geometric PID control scheme requires less control torque magnitude than the PX4 controller during most of the flight. To summarize, compared with the original PX4 autopilot controller, the proposed control scheme demonstrates better control performance with less control effort and less high-frequency oscillations.

6.2. Flight Experiments

For real flight experiments, a custom-made quadrotor UAV platform was built to carry out indoor experiments using a customized version of PX4 firmware. The control module in this customized version replaced the default PX4 controller with the proposed geometric PID attitude controller presented in this paper. A short video of a flight test conducted by our GeoPID controller is given here (https://youtu.be/Yni6hoflUEI, accessed on 26 August 2022), which shows the good performance of the very well-tuned proposed controller in the response to the given attitude commands by the pilot and its ability to preserve the quadcopter in stable attitudes during the flight. This control scheme was also implemented and tested in real autonomous flight experiments using an autonomous flight framework that is presented in detail in Chapter 6 of the dissertation [51]. More videos of manual and autonomous flights with this control scheme are available in AUSLab’s research website (https://aksanyal.expressions.syr.edu/research-videos/, accessed on 26 August 2022).

7. Conclusions

In this work, an integral geometric attitude tracking control scheme is proposed, and its almost global asymptotic stability is proved theoretically, using Lyapunov analysis and a Morse–Lyapunov function on the state space of rigid-body attitude motion. Analysis of robustness to bounded but persistent disturbance torque is also presented. Numerical simulations confirm the performance of the proposed attitude controller for a challenging helical trajectory, even in the presence of an oscillating disturbance torque, in comparison with a state-of-the-art geometric PD-type attitude controller and a non-geometric PID controller. We also implemented this attitude control scheme on a customized version of the PX4 autopilot software. We saw that for a similar helical flight trajectory, using the proposed controller results in a lower control effort and less high-frequency oscillations, compared to using the original PX4 controller. Real flight experiments based on this modified PX4 autopilot software were also carried out for experimental validation of this work. These flight experiments showed the validity of this attitude control scheme for stable attitude control of quadrotor UAVs.