Robust Geometric Control for a Quadrotor UAV with Extended Kalman Filter Estimation

Abstract

This study proposes a robust geometric controller tailored for quadrotor unmanned aerial vehicles (UAVs). The original geometric controller exhibits excellent performance in quadrotor UAV maneuvers. However, as a model-based nonlinear control method, it is sensitive to system model parameters. By integrating a novel extended Kalman filter (EKF)-based estimator for real-time, online estimation of the quadrotor’s inertia parameters, the controller adeptly handles internal uncertainties and external perturbations during flight maneuvers. This approach significantly improves the robustness of the control system against model inaccuracies. Empirical evidence is provided through both simulation and extensive real-world flight tests, demonstrating the controller’s effectiveness and its practical applicability in dynamic environments. The results confirm that this integration substantially enhances system reliability and performance under varied operational conditions.

1. Introduction

In recent years, quadrotor UAVs have become a central focus of research due to their simplicity, cost-effectiveness, and adaptability, finding applications in diverse fields such as commercial light shows, search and rescue operations [1], transportation [2], and agriculture [3]. In order to ensure the successful implementation of these applications, quadcopter UAVs need to have good maneuverability and agility, and an effective controller plays a crucial role in this.

Among effective controllers, linear control approaches like the PID [4] and LQR [5] are particularly valued for their structural simplicity and straightforward deployment, seeing broad utilization across quadrotor UAVs. Nonetheless, inherent limitations, such as the small-angle approximation used for model linearization, typically limit these methods to exhibiting merely local theoretical stability. Therefore, nonlinear controllers, including sliding mode control [6] and backstepping [7], have been pursued for application on quadrotor UAVs. However, these methods inevitably encounter singularity and ambiguity challenges. Lee et al. [8] have proposed a coordinate-free geometric controller developed on the SE(3) manifold, utilizing rotation matrices for attitude representation. This method effectively circumvents the aforementioned issues and achieves near-global exponential stability, making it extensively applicable in maneuverability scenarios of UAVs.

Numerous studies on geometric controllers have emerged in recent decades. In Ref. [9], an extended Kalman filter developed on SE(3) was designed to estimate states during sensor failure or in GPS-denied environments. To enhance UAV safety in complex environments, the study in [10] integrated a control barrier function (CBF) into the geometric control framework. This approach ensures UAV safety by defining a protected region through the CBF and modifying control inputs with quadratic programming (QP), thereby allowing for UAV operation within this region without compromising safety constraints. Moreover, the issue of disturbances during flight was tackled in [11,12]. A finite-time disturbance observer was integrated into a geometric controller and was capable of estimating disturbances with unknown bounds within a finite time. In Ref. [12], the researchers employed an extended state observer (ESO) to estimate not only external disturbances but also internal uncertainties in the quadrotor UAV system. Furthermore, the studies in [13,14] primarily focused on the application of geometric controllers in payload transportation. In Ref. [13], a sophisticated geometric control strategy was implemented, managing dual-loop control for effective trajectory tracking and swing angle stabilization in quadrotor UAVs with suspended loads. The researchers in [14] tackled the challenges posed by elastic cables in similar UAV setups by using geometric control techniques combined with the singular perturbation theory to simulate a system with inelastic cables, thus enhancing control precision.

However, these SE(3) geometric controllers rely on accurate modeling of the quadrotor UAV, which is difficult to measure and may even change due to external influences or changes in load [15]. Large deviations between measured model parameters and their true values can affect the flight performance of a quadrotor UAV, and may even lead to controller divergence. Traditional methods for parameter calibration, such as CAD modeling and frequency domain analysis [9,15], which typically depend on predetermined models and offline testing, are complex and struggle to provide real-time responses to environmental uncertainties and internal dynamic modifications. Current precise modeling methods typically utilize inputs and outputs, usually the rotational speeds of four rotors and measurements from onboard sensors for quadrotor UAVs, to model a system’s dynamic characteristics. To accurately model quadrotor UAVs, machine learning methods have been extensively applied [16,17]. These techniques primarily enable offline estimation of a quadrotor UAV’s dynamic model based on pre-sampled datasets. Other time-domain methods, including recursive least squares (RLSs) [18], maximum likelihood [19], Bayesian filtering [15,20,21,22], and sliding mode observer [23], are also widely utilized. The estimated parameters typically encompass mass, inertia matrix, and aerodynamic coefficients, all of which significantly influence control performance.

Due to the inherent noise in sensor measurements on quadrotor UAVs, methods such as those described in [18,19] start by using a Kalman filter (KF) to preprocess the flight data before identification. In Ref. [18], the researchers initially filtered out sensor noise and then employed RLSs to estimate the observable parameters. The work in [19] utilized an improved preprocessing method, Flight Path Reconstruction (FPR), which integrated the EKF and a Rauch–Tung–Striebel smoother. Then, maximum likelihood estimation was used to identify the system model. In addition to the estimation of inertia parameters, the studies in [21,22] utilized Bayesian filtering methods, such as the EKF and unscented Kalman filter (UKF), to estimate the quadrotor system’s geometric parameters, including the position of the center of mass (CoM) and the orientations of onboard sensors and rotors, significantly reducing the geometric assumptions commonly found in related research.

The researchers in [15,24,25] focus on modeling aerodynamic effects. Specifically, in [15], the UKF was utilized for online estimation of the drag-force coefficients and external wind conditions, enhancing the drone’s capability to reject gusts. In Ref. [24], the researchers only employed an inertial measurement unit (IMU) and motor speeds to estimate inertia parameters and model the blade-flapping moment. The experiments validated that blade-flapping modeling effectively improves estimations of the moment of inertia. In Ref. [25], the behavior and responses of asymmetric quadrotor UAVs under complex aerodynamic effects were discussed in detail, with the KF method applied for system identification. The stable convergence of recursive methods, such as gradient descent and least squares, requires persistence of the excitation conditions. In Ref. [26], the researchers employed the dynamic regressor extension and mixing (DREM) technique. By preprocessing the regressor both algebraically and dynamically, the persistence of excitation constraints was effectively alleviated.

In this article, a robust geometric controller is introduced, integrating the geometric controller with an online EKF-based estimator to estimate the UAV’s inertia parameters. The contributions of this paper are summarized as follows:

(1)

A novel control scheme is presented for a quadrotor UAV that incorporates an online estimation of inertia parameters. This enables the system to effectively address uncertainties during maneuvers, enhance robustness, and improve performance in diverse operating environments.

(2)

By leveraging the equivalence between quaternions and rotation matrices, the proposed approach represents attitude with quaternions within the EKF parameter estimator. Compared to the rotation matrix representation, this significantly reduces estimation dimensions and computational time while maintaining effective control performance.

(3)

The effectiveness of the proposed scheme is validated through both simulations and real-world experiments, demonstrating superior performance compared to the traditional geometric controller. The real-world experiment can be viewed online at https://youtu.be/p4RYlQRqmow (accessed on 27 May 2024).

The organization of this paper is as follows: Section 2 illustrates the dynamic model of quadrotor UAVs, accompanied by an analysis of system observability across various sensor configurations. Section 3 details the implementation of a parameter estimation algorithm utilizing the EKF as well as the architecture of the geometric controller. Section 4 showcases the efficacy of the proposed control scheme through both simulations and experiments.

2. Quadrotor System Model

2.1. Quadrotor Dynamics

This section elaborates on the dynamic model of the quadrotor UAV. As illustrated in Figure 1, the quadrotor in the × configuration is chosen to demonstrate the principles. The inertial reference frame and the body-fixed frame are introduced to clarify the description, represented by $\{i_x,i_y,i_z\}$ and $\{b_x,b_y,b_z\}$, respectively. It is assumed that the four rotors of the quadrotor UAV are positioned on the plane spanned by $b_x$ and $b_y$. Additionally, the CoM of the quadrotor UAV aligns with the origin of the body-fixed frame, simplifying the dynamics analysis.

The rotor i generates a thrust $f_i\in \mathbb{R}$ and a torque ${\tau }_i\in \mathbb{R}$, both aligned along $b_z$, for $1\le i\le 4$. Simplifying the complex aerodynamic effects, the thrust can be given by $f_i=c_f{{\omega }_i}^2$, where $c_f\in \mathbb{R}$ is the lift coefficient and ${\omega }_i\in \mathbb{R}$ is the rotation speed of the i-th rotor [15]. The torque is directly proportional to the thrust, which can be represented by ${\tau }_i={(-1)}^i{c}_{\tau f}{f}_i$, with ${c_{\tau }}_f$ representing the proportionality constant. The perpendicular distances from the center of rotor i to $b_x$ and $b_y$ are denoted by $l_{i_x}\in \mathbb{R}$ and $l_{i_y}\in \mathbb{R}$, respectively. The total thrust f and the total moment $M={\left[M_x{M}_y{M}_z\right]}^T\in {\mathbb{R}}^3,$ can be calculated from the following equation:

$$\left[\begin{array}{c}f\\ M_x\\ M_y\\ M_z\end{array}\right]=c_f·\left[\begin{array}{cccc}1& 1& 1& 1\\ -l_{1_x}& l_{2_x}& l_{3_x}& -l_{4_x}\\ -l_{1_y}& -l_{2_y}& l_{3_y}& l_{4_y}\\ -c_{\tau f}& c_{\tau f}& -c_{\tau f}& c_{\tau f}\end{array}\right]\left[\begin{array}{c}{{\omega }_1}^2\\ {{\omega }_2}^2\\ {{\omega }_3}^2\\ {{\omega }_4}^2\end{array}\right]$$

(1)

The state of a quadrotor UAV is characterized by its position $p\in {\mathbb{R}}^3$, velocity $v\in {\mathbb{R}}^3$, attitude $R\in \mathrm{SO}\left(3\right)$, and angular velocity $\mathrm{\Omega }\in {\mathbb{R}}^3$, where the rotation matrix R represents the orientation of the body-fixed frame with respect to the inertial reference frame and the $\mathrm{\Omega }$ is expressed in the body-fixed frame. As the position and attitude of the quadrotor are, respectively, represented in ${\mathbb{R}}^3$ and $\mathrm{SO}\left(3\right)$, the configuration manifold of the quadrotor is the special Euclidean group $\mathrm{SE}\left(3\right)$ [8]. The mass and inertial matrix of the quadrotor are denoted by $m\in \mathbb{R}$ and $J=\mathrm{diag}\left(J_x{J}_y{J}_z\right)\in {\mathbb{R}}^{3\times 3}$, where $\mathrm{diag}(·)$ refers to the operation of converting a vector into its corresponding symmetric matrix form. Therefore, according to Newton’s second law and the Newton–Euler equations, the dynamic model of a quadrotor can be represented as follows:

$$\begin{array}{cc}\hfill & \dot{p}=v\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill & \dot{R}=R\widehat{\mathrm{\Omega }}\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & m\dot{v}=fR{e}_3-mg{e}_3\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill & J\dot{\mathrm{\Omega }}=M-\mathrm{\Omega }\times J\mathrm{\Omega }\hfill \end{array}$$

(5)

where $e_3={\left[001\right]}^T\in {\mathbb{R}}^3$, $g\in \mathbb{R}$ is the gravity, and $\widehat{(·)}$ is the map from ${\mathbb{R}}^3$ to $\mathrm{so}\left(3\right)$. For $\mathrm{\Omega }={\left[{\mathrm{\Omega }}_x{\mathrm{\Omega }}_y{\mathrm{\Omega }}_z\right]}^T\in {\mathbb{R}}^3$, $\widehat{\mathrm{\Omega }}$ is given by

$$\widehat{\mathrm{\Omega }}=\left[\begin{array}{ccc}0& -{\mathrm{\Omega }}_z& {\mathrm{\Omega }}_y\\ {\mathrm{\Omega }}_z& 0& -{\mathrm{\Omega }}_x\\ -{\mathrm{\Omega }}_y& {\mathrm{\Omega }}_x& 0\end{array}\right]$$

The geometric controller implemented on the SE(3) manifold circumvents the singularities of Euler angles and the ambiguities of quaternions. However, when it comes to state and parameter estimation, utilizing the rotation matrix to represent attitude significantly augments the estimation dimension, leading to increased computational complexity and challenges in achieving estimation convergence. To avoid the long-term singularity issues in attitude estimation [27], quaternions are employed to represent rotation during the estimation process. The estimated quaternion is subsequently converted into an equivalent rotation matrix to calculate the control inputs. This transformation theoretically preserves the performance of the controller. Let the quaternion $q\in {\mathbb{R}}^4$ denote the same rotation as R, Equation (3) can also be represented as follows:

$$\dot{q}=\frac{1}{2}q\otimes \mathrm{\Omega }$$

(6)

The state space representation of the quadrotor dynamics is formulated as $\dot{\chi }=f(\chi ,u)$, where

$$\chi =\left[\begin{array}{c}p\\ v\\ q\\ \mathrm{\Omega }\end{array}\right]\in {\mathbb{R}}^{13}$$

(7)

is the state vector and $u={\left[f{M}^T\right]}^T\in {\mathbb{R}}^4$ is the control input. A primary objective of this study is to estimate the mass and inertial matrix of the quadrotor system. Consequently, in our model, these inertia parameters are no longer considered constants in the traditional sense. We define ${\chi }_p={\left[m{J}_x{J}_y{J}_z\right]}^T\in {\mathbb{R}}^4$ as the parameter vector, which is considered to have zero dynamics, and ${\chi }_e={\left[{\chi }^T{\chi }_p^T\right]}^T\in {\mathbb{R}}^{17}$ as the extended state vector. Thus, the full dynamics is as follows:

$$\begin{array}{cc}\hfill & {\dot{\chi }}_e={\left[{\chi }^T{\chi }_p^T\right]}^T=f^{*}({\chi }_e,u)\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill & {\dot{\chi }}_p=0\hfill \end{array}$$

(9)

2.2. Observability Analysis

In control systems, observability denotes the ability to uniquely determine complete information on the internal state of a system through its outputs. The observability of nonlinear systems can be contingent upon the system’s initial conditions and the specific trajectories of inputs. This implies that a system may exhibit observability under certain conditions while becoming unobservable under others, a characteristic distinct from linear systems. Consequently, the analytical methods employed for observability assessment in linear systems are not directly applicable to their nonlinear counterparts. In this section, we first introduce the common measurement configurations utilized for quadrotor UAVs and subsequently employ a differential geometric approach to analyze the observability of the system.

Sensors are key components that enable quadrotor UAVs to achieve stable flight and navigation. These sensors, including GPS receivers, IMUs, optical flow sensors, ultra-wideband (UWB) systems, and motion capture systems [21], are either directly mounted on the quadrotor UAV or deployed within the flight area. These sensors are categorized into two types based on the measurements they provide: pose sensors and IMUs [22]. Pose sensors offer data on the position and attitude of a quadrotor UAV, while IMUs provide measurements of angular velocity and acceleration. Assuming that the installation orientations of both pose sensors and IMUs align with the body-fixed frame, the measurement equations for both are as follows:

$$\begin{array}{cc}\hfill & z_{pose}=h_{pose}\left(\chi \right)=\left[\begin{array}{c}p+p_{bias}\\ q\end{array}\right]\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill & z_{imu}=h_{imu}\left(\chi \right)=\left[\begin{array}{c}\mathrm{\Omega }\\ R^{-1}\dot{v}-g{e}_3\end{array}\right]\hfill \end{array}$$

(11)

Herein, $p_{\mathrm{bias}}\in {\mathbb{R}}^3$ denotes the bias of the pose sensor relative to the body frame, which is assumed to be known. $z_{\mathrm{pose}}\in {\mathbb{R}}^7$ and $z_{\mathrm{imu}}\in {\mathbb{R}}^6$, respectively, represent the measurements from the pose sensor and the IMU.

The observability of nonlinear systems can be evaluated by constructing an observability matrix $\mathcal{O}$ and examining its rank. If $\mathrm{rank}\left(\mathcal{O}\right)=\dim \left({\chi }_e\right)$, the system can be considered locally weakly observable [22]. Given the system’s measurement equation $z=h\left(\chi \right)$, the observability matrix can be calculated using Lie derivatives:

$$\mathcal{O}=\left[\begin{array}{c}\nabla L_{f^{*}}^{0}h\\ \nabla L_{f^{*}}^{1}h\\ \nabla L_{f^{*}}^{2}h\\ ⋮\\ \nabla L_{f^{*}}^{n-1}h\end{array}\right],n=\dim \left({\chi }_e\right)=17$$

(12)

with

$$\begin{array}{cc}\hfill & L_{f^{*}}^{0}h=h\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill & L_{f^{*}}^{i}h=\nabla \left(L_{f^{*}}^{i-1}h\right)f^{*}\hfill \end{array}$$

(14)

The dimension of the observability matrix $\mathcal{O}$ is $119\times 17$, and its rank can be computed numerically using symbolic computation libraries. It is important to note that the attitude q is represented by quaternions, with the inherent constraint $q{q}^{*}=1$, where $q^{*}$ is the conjugate of q. Numerical calculations demonstrate that all elements in the extended state vector are locally weakly observable, regardless of the measurement configuration employed.

3. Robust Geometric Controller

3.1. EKF Estimator

The analysis of observability in Section 2 demonstrates that the quadrotor system is observable under certain measurement conditions. This observability allows for estimations of the system’s state and parameters using a series of measured values. The EKF is an advanced signal-processing algorithm used to estimate the states or parameters of nonlinear dynamic systems from a series of incomplete and noisy measurements [9]. As an extension of the classic Kalman filter, the EKF is applicable to quadrotor systems, which exhibit pronounced nonlinear characteristics. This section applies the EKF algorithm to the quadrotor system illustrated in Section 2 and presents detailed steps for online parameter estimation.

The EKF algorithm typically involves two phases: Initially, it calculates the current state’s estimate and covariance based on the state transition function, with the prior state’s optimal estimate and covariance as inputs. Subsequently, the Kalman gain is computed based on the measurement model. This gain is then used, along with measurement data, to update the estimate of the state and its covariance, thereby establishing the current optimal estimate. To apply the EKF algorithm, the dynamic Equation (8) is discretized into the state transition form:

$${\chi }_{e_k}=f_d^{*}({\chi }_{e_{k-1}},u_{k-1})+w_{k-1}$$

(15)

where $f_d^{*}$ represents the different form of $f^{*}$, and $w\in {\mathbb{R}}^{17}$ denotes the additive noise within the state transition.

In practical applications, it is challenging to obtain a perfect model that accurately describes a quadrotor system’s dynamic behavior. Additive noise can aid in compensating for a model’s uncertainty, enhancing the filter’s robustness. Moreover, the randomness of external environments also impacts a system’s state transitions. Additive noise can simulate this randomness under such conditions, making the state estimation more closely aligned with actual scenarios.

Let ${\mathsf{\mu }}_k\in {\mathbb{R}}^{17}$ and ${\Sigma }_k\in {\mathbb{R}}^{17\times 17}$, respectively, be the optimal estimates of the mean and covariance of ${\chi }_e$ at time k, and $Q\in {\mathbb{R}}^{17\times 17}$ be the covariance of w. In actual quadrotor systems, the input force and torque, typically derived from motor PWM signals and simplified models of thrust and aerodynamics, are not measured directly. Consequently, substantial noise is inevitably introduced. Let $P\in {\mathbb{R}}^{4\times 4}$ represent the covariance of the system input u.

For the prediction phase,

$$\begin{array}{cc}\hfill & {\mathsf{\mu }}_k^{-}=f_d^{*}({\mathsf{\mu }}_{k-1},u_{k-1})\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill & {\Sigma }_k^{-}=F_k{\Sigma }_{k-1}{F}_k^T+G_k{P}_{k-1}{G}_k^T+Q_{k-1}\hfill \end{array}$$

(17)

with

$$F_k={\left.\frac{\partial f_d^{*}}{\partial {\chi }_e}\right|}_{\chi ={\mathsf{\mu }}_{k-1},u=u_{k-1}},G_k={\left.\frac{\partial f_d^{*}}{\partial u}\right|}_{\chi ={\mathsf{\mu }}_{k-1},u=u_{k-1}}$$

(18)

The measurement model is represented by $z=h\left(\chi \right)+v$, where z denotes sensor measurements and v is the measurement error, which is characterized as white noise with a mean of zero and a covariance of R. The dimensions of z and v depend on the measurement configuration. Then, the equations of the update phase are given by

$$\begin{array}{cc}\hfill & {\mathsf{\mu }}_k={\mathsf{\mu }}_{k-1}+K_k(z_k-h\left({\mathsf{\mu }}_k^{-}\right))\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill & {\Sigma }_k=(I-K_k{H}_k){\Sigma }_{k-1}\hfill \end{array}$$

(20)

with

$$\begin{array}{cc}\hfill & K_k={\Sigma }_k^{-}{H}_k^T{(H_k{\Sigma }_k^{-}{H}_k^T+Q_k)}^{-1}\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill & H_k={\left.\frac{\partial h}{\partial {\chi }_e}\right|}_{\chi ={\mathsf{\mu }}_k^{-}}\hfill \end{array}$$

(22)

With the initial values, noise covariance, and observation data appropriately configured, the EKF algorithm will, in theory, recursively estimate the current state and parameters.

3.2. SE(3) Geometric Controller

Quadrotor UAVs often experience significant changes in pose within a short period during maneuvering flight, a scenario that current linear PID controllers struggle to manage effectively. The geometric controller configures the UAV on the SE(3) manifold, effectively avoiding the ambiguity and singularity issues arising from quaternion or Euler angle representations of attitude. This approach also fully leverages the system model, making it well suited for the maneuvering flight of UAVs.

Quadrotor UAVs, with six degrees of freedom in motion and only four control inputs, are inherently underactuated systems. This underactuation limits their ability to simultaneously track both position and attitude commands. This study primarily focuses on the trajectory tracking performance of quadrotor UAVs, with the control variables being their desired position and the orientation of the axis $b_x$, denoted as $p_d$ and $b_{x_d}$, respectively. The position and velocity tracking errors, represented as $e_p$ and $e_v$, can be derived as follows:

$$e_p=p-p_d,e_v=v-{\dot{p}}_d$$

(23)

Referring to the translational dynamic model presented in Section 2, we can derive an approximate form of the desired thrust:

$$f_c=-K_p{e}_p-K_v{e}_v+m_{e}g{e}_3+m_e{\ddot{p}}_d$$

(24)

where $m_e$ is the estimate of mass m, and $K_x$ and $K_v$ are positive definite symmetric matrices. The desired orientation of $b_{z_d}$ and $b_{y_d}$ is defined as follows:

$$\begin{array}{cc}\hfill & b_{z_d}=\frac{-K_p{e}_p-K_v{e}_v+m_{e}g{e}_3+m_e{\ddot{p}}_d}{\parallel -K_p{e}_p-K_v{e}_v+m_{e}g{e}_3+m_e{\ddot{p}}_d\parallel }\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill & b_{y_d}=\frac{b_{z_d}\times b_{x_d}}{\parallel b_{z_d}\times b_{x_d}\parallel }\hfill \end{array}$$

(26)

Then, the desired attitude is

$$\left.R_d=\left[\begin{array}{ccc}{b}_{y_d}\times b_{z_d}& b_{y_d}& b_{z_d}\end{array}\right.\right]$$

(27)

Compared to the form of attitude error proposed in [8], Invernizzi et al. [11] introduced the left value, defined as

$$R_e=R{R}_d^T$$

(28)

which simplifies the final controller structure. The attitude error function on SO(3) is defined as

$$\mathrm{\Psi }=\frac{1}{2}\mathrm{tr}(I-R{R}_d^T)$$

(29)

where $\mathrm{tr}(·)$ represents the trace of the matrix. Defining ${(·)}^{\vee }$ to be the map from SO(3) to ${\mathbb{R}}^3$, the attitude error in the SO(3) form, $e_R$, is as follows:

$$\begin{array}{c}\hfill e_R=\frac{1}{2}{(R{R}_d^T-R_d{R}^T)}^{\vee }\end{array}$$

(30)

Given that ${\dot{R}}_d$ and $\dot{R}$ reside in different tangent spaces, we convert ${\dot{R}}_d$ into the tangent space of $\dot{R}$:

$$\dot{R}-{\dot{R}}_d=R{(\mathrm{\Omega }-{\mathrm{\Omega }}_d)}^{\vee }$$

(31)

Consequently, the velocity error $e_{\mathrm{\Omega }}$ in the SO(3) form can be chosen as

$$e_{\mathrm{\Omega }}=\mathrm{\Omega }-{\mathrm{\Omega }}_d$$

(32)

Let $J_e$ be the estimate of the inertial matrix. Based on the preceding definitions of tracking errors, the controller can ultimately be derived as follows:

$$\begin{array}{cc}\hfill & f_c=-K_p{e}_p-K_v{e}_v+m_{e}g{e}_3+m_e{\ddot{p}}_d\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill & M=-R_d^T{e}_R-K_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}+\mathrm{\Omega }\times J_e\mathrm{\Omega }+J_e{\dot{\mathrm{\Omega }}}_d\hfill \end{array}$$

(34)

where $K_{\mathrm{\Omega }}$ is a positive definite symmetric matrix. Given that a quadrotor UAV can only generate thrust perpendicular to the plane of the body directed upwards, the final desired thrust is taken as $f_d=\left|\right|f_c\left|\right|$. The stability of the complete system is illustrated in the subsequent analysis.

Theorem 1. 

The zero equilibrium point of the closed-loop attitude error system maintains stability prior to the inertia matrix estimation error converging to zero. Following this convergence, it transitions to exhibiting exponential stability.

Proof of Theorem 1.

The Lyapunov candidate is chosen as

$$V_R=\frac{1}{2}{e}_{\mathrm{\Omega }}^{T}J{e}_{\mathrm{\Omega }}+\mathrm{\Psi }+c_1{e}_R^{T}J{e}_{\mathrm{\Omega }}$$

(35)

where $c_1$ is a positive constant. For the derivatives of the attitude tracking errors,

$$\begin{array}{cc}\hfill & {\dot{e}}_R=E(R,R_d)e_{\mathrm{\Omega }}=\frac{1}{2}(tr\left(R_e\right)I_{3\times 3}-R_e^T)R_d{e}_{\mathrm{\Omega }}\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill & J{\dot{e}}_{\mathrm{\Omega }}=-R_d^T{e}_R-K_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}+\mathrm{\Omega }\times J_b\mathrm{\Omega }+J_b{\dot{\mathrm{\Omega }}}_d\hfill \end{array}$$

(37)

where $J_b=J_e-J$ is the estimation error of the inertia matrix. $J_b$ is artificially set to have boundaries throughout the entire estimation. Defining $M_b=\mathrm{\Omega }\times J_b\mathrm{\Omega }+J_b{\dot{\mathrm{\Omega }}}_d\in {\mathbb{R}}^3$, it can be concluded that $M_b$ is bounded by limiting the ${\dot{\mathrm{\Omega }}}_d$. Then, the derivate of the Lyapunov function is

$$\begin{array}{cc}\hfill {\dot{V}}_R& =-e_{\mathrm{\Omega }}^T{K}_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}+e_{\mathrm{\Omega }}^T{M}_b-c_1{e}_R^T{R}_d^T{e}_R-c_1{e}_R^T{K}_{\mathrm{\Omega }}{e}_{\mathrm{\Omega }}\hfill \\ \hfill & +c_1{e}_R^T{M}_b+c_{1}E{e}_{\mathrm{\Omega }}^T{e}_{\mathrm{\Omega }}\hfill \\ \hfill & \le -({\lambda }_m\left(K_{\mathrm{\Omega }}\right)-c_1{\lambda }_M\left(J\right))\left|\right|e_{\mathrm{\Omega }}{\left|\right|}^2-c_1\left|\right|e_R{\left|\right|}^2\hfill \\ \hfill & +c_1{\lambda }_M\left(K_{\mathrm{\Omega }}\right)\left|\right|e_R\left|\right|\left|\right|e_{\mathrm{\Omega }}\left|\right|+\left|\right|e_{\mathrm{\Omega }}\left|\right|\left|\right|M_b\left|\right|+c_1\left|\right|e_R\left|\right|\left|\right|M_b\left|\right|\hfill \end{array}$$

(38)

where ${\lambda }_m(·)$ and ${\lambda }_M(·)$ represent the minimum and maximum eigenvalues of a matrix. Let $z_R=\left[\right||e_R\left|\right|\left|\right|e_{\mathrm{\Omega }}{\left|\right|]}^T$,

$${\dot{V}}_R\le -z_R^{T}A{z}_R+\left|\right|M_b\left|\right|D{z}_R\le -{\lambda }_m\left(A\right)\left|\right|z_R{\left|\right|}^2+\left|\right|M_b\left|\right|\left|\right|D\left|\right|\left|\right|z_R\left|\right|$$

(39)

with

$$\begin{array}{cc}\hfill & \left.A=\left[\begin{array}{cc}{c}_1& -\frac{c_1}{2}{\lambda }_M\left(K_{\mathrm{\Omega }}\right)\\ -\frac{c_1}{2}{\lambda }_M\left(K_{\mathrm{\Omega }}\right)& {\lambda }_m\left(K_{\mathrm{\Omega }}\right)-c_1{\lambda }_M\left(J\right)\end{array}\right.\right]\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill & D=\left[c_{1}1\right]\hfill \end{array}$$

(41)

Finally, it can be derived as

$${\dot{V}}_R\le -{\lambda }_m\left(A\right)\left|\right|z_R{\left|\right|}^2\forall \left|\right|z_R\left|\right|\ge \frac{\left|\right|M_b\left|\right|\left|\right|D\left|\right|}{{\lambda }_m\left(A\right)}$$

(42)

As the estimation of the inertial matrix converges, the $\left|\right|M_b\left|\right|$ will approach 0, and the attitude dynamics will be rendered exponentially stable. □

Theorem 2. 

Before the estimation errors of the inertia parameters converge to zero, the entire closed-loop error system remains stable. Afterward, the system exhibits exponential stability.

Proof of Theorem 2.

Consider the following Lyapunov candidate for the translational dynamics:

$$V_p=\frac{1}{2}m{e}_v^T{e}_v+\frac{1}{2}{e}_p^T{K}_p{e}_p+c_{2}m{e}_p^T{e}_v$$

(43)

where $c_2$ is a positive constant. Let $m_b=m_e-m$ be the estimation error of the mass; the derivatives of the position tracking error are

$$\begin{array}{cc}\hfill & {\dot{e}}_p=v-v_d={\dot{e}}_v\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill & \begin{array}{cc}\hfill m{\dot{e}}_v& =-mg{e}_3+f_{d}R{e}_3-m{\dot{v}}_d\hfill \\ \hfill & =-K_p{e}_p-K_v{e}_v+f_b+\Delta f\hfill \end{array}\hfill \end{array}$$

(45)

with

$$\begin{array}{cc}\hfill & f_b=m_{b}g{e}_3+m_b{\dot{v}}_d\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill & \Delta f=f_{d}R{e}_3-f_c\hfill \end{array}$$

(47)

The estimation error of the mass is bounded before the estimation converges. Thus, $f_b$ is also bounded by limiting ${\dot{v}}_d$. The derivative of the Lyapunov function can be given by

$$\begin{array}{cc}\hfill {\dot{V}}_p& =-e_v^T{K}_v{e}_v+e_v^T{f}_b+e_v^T\Delta f+c_{2}m{e}_v^T{e}_v-c_2{e}_p^T{K}_p{e}_p\hfill \\ \hfill & -c_2{e}_p^T{K}_v{e}_v+c_2{e}_p^T{f}_b+c_2{e}_p^T\Delta f\hfill \\ \hfill & \le -({\lambda }_m\left(K_v\right)-c_{2}m)\left|\right|e_v{\left|\right|}^2-c_2{\lambda }_m\left(K_x\right)\left|\right|e_p{\left|\right|}^2\hfill \\ \hfill & +\left|\right|e_v\left|\right|\left|\right|f_b\left|\right|+\left|\right|e_v\left|\right|\left|\right|{\Delta }_f\left|\right|+c_2{\lambda }_M\left(K_v\right)\left|\right|e_p\left|\right|\left|\right|e_v\left|\right|\hfill \\ \hfill & +c_2\left|\right|e_p\left|\right|\left|\right|f_b\left|\right|+c_2\left|\right|e_p\left|\right|\left|\right|{\Delta }_f\left|\right|\hfill \end{array}$$

(48)

Here, the conclusions about attitude error $\mathrm{\Psi }$ and the upper bound of $\left|\right|{\Delta }_f\left|\right|$ in [11] are refered to. For $\mathrm{\Psi }<\psi <2$, $\mathrm{\Psi }$ is locally quadratic

$$\begin{array}{cc}\hfill & \frac{1}{2}\parallel e_R{\parallel }^2\le \mathrm{\Psi }\le \frac{1}{2-\psi }{\parallel e_R\parallel }^2\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill & \left|\right|\Delta f\left|\right|<\sqrt{\frac{2}{2-\psi }}\left|\right|e_R\left|\right|f_M=c_3\left|\right|e_R\left|\right|f_M\hfill \end{array}$$

(50)

where $\psi$ is a positive constant and $f_M$ is the maximum thrust generated by the quadrotor UAV. Further, the Equation (48) can be written as

$${\dot{V}}_p\le -z_p^{T}B{z}_p+c_3{f}_M\left|\right|e_R\left|\right|E{z}_p+\left|\right|f_b\left|\right|E{z}_p$$

(51)

with

$$\begin{array}{cc}\hfill & \left.B=\left[\begin{array}{cc}{c}_2{\lambda }_m\left(K_x\right)& -\frac{c_2}{2}{\lambda }_M\left(K_v\right)\\ -\frac{c_2}{2}{\lambda }_M\left(K_v\right)& {\lambda }_m\left(K_v\right)-c_{2}m{\lambda }_M\left(J\right)\end{array}\right.\right]\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill & E=\left[c_{2}1\right]\hfill \end{array}$$

(53)

Then, the following can be derived:

$$\begin{array}{cc}\hfill {\dot{V}}_p& \le -{\lambda }_m\left(B\right)\left|\right|z_x{\left|\right|}^2\forall \left|\right|z_x\left|\right|\ge \frac{\left(c_3{f}_M\right|\left|e_R\right||+|\left|f_b\right|\left|\right)\left|\right|E\left|\right|}{{\lambda }_m\left(B\right)}\hfill \end{array}$$

(54)

It can be noted that after the convergence of the attitude error and the estimation error of mass, $\left|\right|f_b\left|\right|$ and $\left|\right|e_R\left|\right|$ tend to 0, and the translational dynamics exhibit exponential stability. By combining Equations (35) and (43), the Lyapunov candidate for full dynamics is

$$V=V_p+V_R$$

(55)

Thus, it can be proved that the closed loop error dynamics, which are driven by the control laws illustrated in Equations (33) and (34), are stable before the estimation and attitude error convergence and are rendered exponentially stable. □

Since the geometric controller heavily relies on the system model, the EKF-based parameter estimator is introduced to the control system loop to estimate system parameters online, thus enhancing the system’s robustness and control performance. Figure 2 illustrates the control scheme. The estimator calculates the state and inertial parameters of the quadrotor system online based on its inputs and outputs. In the geometric controller, the outer controller responsible for trajectory tracking computes the required thrust and the desired direction of $b_z$ using the position commands, estimated current state, and mass. The inner controller responsible for attitude tracking first calculates the desired attitude based on the expected directions of $b_x$ and $b_z$. It then calculates the total torque using the state and parameter estimates from the estimator.

4. Simulation and Experiment Results

4.1. Simulation

To evaluate the efficacy of the proposed EKF estimation algorithm and the new control structure, numerical simulations are conducted. The dynamic model used in the simulation adopts the form presented in Section 2, with white noise added to simulate real-world scenarios. The primary parameters of the quadrotor UAV are outlined in

Table 1. Parameters of the quadrotor UAV.

Parameters	Values	Units
m	4.34	kg
$J_x$	0.08	kg·${\mathrm{m}}^2$
$J_y$	0.08	$\mathrm{kg}·{\mathrm{m}}^2$
$J_z$	0.14	$\mathrm{kg}·{\mathrm{m}}^2$
$l_{i_x}$	0.15	m
$l_{i_y}$	0.15	m
$c_f$	$1.7\times {10}^{-8}$	${\mathrm{N}/\mathrm{RPM}}^2$
$c_{\tau f}$	$8.0\times {10}^{-3}$	m

.

The initial state of the quadrotor UAV was set to

$$p_0={\left[000\right]}^T,v_0={\left[000\right]}^T,q_0={\left[1000\right]}^T,{\mathrm{\Omega }}_0={\left[000\right]}^T$$

The mass and inertial matrix of the quadrotor UAV utilized in the estimator were initialized at

$$\begin{array}{cc}\hfill & {\mathsf{\mu }}_{m_0}=2,{\Sigma }_{m_0}=1\hfill \\ \hfill & {\mathsf{\mu }}_{J_0}=\left[\begin{array}{ccc}0.8& 0& 0\\ 0& 0.8& 0\\ 0& 0& 0.8\end{array}\right],{\Sigma }_{J_0}=\left[\begin{array}{ccc}1& 0.01& 0.01\\ 0.01& 1& 0.01\\ 0.01& 0.01& 1\end{array}\right]\hfill \end{array}$$

The coefficients of the controller were set to

$$K_p=13I_{3\times 3}{K}_v=8I_{3\times 3}{K}_R=30I_{3\times 3}{K}_{\mathrm{\Omega }}=5I_{3\times 3}$$

The measurement sources were selected as the pose sensor and IMU. According to the observability analysis in Section 2, all variables in the extended state vector ${\chi }_e$ are observable. The quadrotor UAV was commanded to track a circular trajectory:

$$p_d={[3cos\left(t\right)3sin\left(t\right)2]}^T,b_{x_d}={[-cos\left(t\right)-sin\left(t\right)2]}^T$$

The measurement frequency for both the IMU and the pose sensor is 100 Hz. To address inconsistencies in the sampling times, interpolation methods are employed to align the timestamps. Figure 3 illustrates the estimated parameters of the quadrotor UAV throughout the simulation. It can be noted that the initial values for mass and inertial matrix in the estimator differ significantly from the actual values, with an initial error in the mass of $2.34\mathrm{kg}$ and an initial error in the inertial matrix of $\mathrm{diag}(0.72,0.72,0.66)\mathrm{kg}·{\mathrm{m}}^2$. The estimated values for mass and inertial matrix converge within one second, with an estimation error of approximately ${10}^{-3}$. As the dynamics involving the inertial matrix possess stronger nonlinear characteristics, the estimation is more susceptible to the impact of numerical calculation accuracy. In the initial stages of trajectory tracking, the quadrotor UAV experienced significant changes in position and attitude, and the limited numerical calculation accuracy led to a relatively large error in the estimation of the inertial matrix.

For comparison with traditional control structures, we tested the trajectory tracking performance of the proposed robust geometric controller and a traditional geometric controller [8] using the same configuration. The results are shown in Figure 4, where the dashed yellow line represents the desired value, and the solid blue and orange lines illustrate the performance of control structures with and without the estimator, respectively. During the stable phase from 4 s to 10 s, the tracking errors across the three dimensions are ${\left[0.0320.0440.023\right]}^T\mathrm{m}$ for the proposed robust controller and ${\left[0.170.170.26\right]}^T\mathrm{m}$ for the geometric controller alone. The root mean squared errors (RMSEs) are 0.033 m and 0.2 m, respectively.

As can be observed, after the stabilization of parameter estimation, the quadrotor’s tracking precision in all three dimensions generally exceeds that of the control system without an estimator, with improvements of approximately ${\left[0.1380.1260.237\right]}^T\mathrm{m}$. The comparison of tracking precision along the $i_z$ axis is particularly remarkable. This is because gravity, influenced by the mass parameter, predominantly affects control performance in the vertical direction. The estimator can timely compensate for deviations in mass, thereby enabling the quadrotor UAV to minimally reduce the tracking error along $i_z$. Additionally, for attitude control precision, the control structure incorporating an estimator significantly outperforms the one without, reducing the average attitude tracking error by 0.069. This indicates that the inertia parameters estimated by the estimator effectively enhance the control performance of the geometric controller. Collectively, these results prove the effectiveness and superiority of the proposed control scheme.

4.2. Experiments

The quadrotor UAV used in experiments, as shown in Figure 5, was designed by us. The system’s architecture is illustrated in Figure 6. The quadrotor UAV is equipped with a Jetson Xavier NX (Nvidia) onboard computer and a CUAV X7 Pro autopilot (CUAV, Guangzhou, China). The geometric controller and EKF-based estimator running on the onboard computer utilize Robot Operating System 2 (ROS2) as the programming framework. The estimator utilizes the measurements from the IMU on the autopilot and the motion capture system to calculate state and parameter estimates, which are then utilized by the geometric controller. The autopilot runs the PX4 flight stack as the low-level controller, converting commands from the geometric controller into PWM signals to actuate the motors. In the quadrotor system, the onboard computer and autopilot are connected via a wired serial connection, and the pose calculated by the motion capture system is sent to the computer via a local area network.

The parameters of the quadrotor UAV used in the experiments are identical to those in the simulations. The inertia matrix was calculated from the CAD model, while the thrust and torque coefficients were measured using a thrust testbed. In the experiment, the quadrotor UAV was required to follow a circular trajectory similar to the simulation but with a fixed yaw:

$$\begin{array}{c}\hfill p_d={[cos\left(1.5t\right)sin\left(1.5t\right)2]}^T,b_{x_d}={\left[102\right]}^T\end{array}$$

It is important to note that in practice, the observability of the mass and inertia matrix parameters weakens when the system’s thrust and torque inputs are near zero. This can lead to incorrect parameter estimation, potentially causing divergence. To address this, this phenomenon was monitored by examining the covariance of the estimated parameters. Stable control performance was ensured by setting reasonable boundaries for the estimates and promptly resetting them to the last stable estimated values whenever a divergence occurred. The experimental results are shown in Figure 7. The tracking errors across the three dimensions during the stable phase are ${\left[0.0190.0220.019\right]}^T\mathrm{m}$ for the proposed robust controller and ${\left[0.0620.0680.055\right]}^T\mathrm{m}$ for the geometric controller alone. Furthermore, the RMSE values are 0.020 m and 0.061 m, respectively.

From the comparison of experimental results, it is evident that the proposed control structure, with the estimator’s compensation for parameters, achieved improved trajectory tracking accuracy. The enhancements in tracking precision across the three dimensions were $\left[0.0430.0460.036\right]\mathrm{m}$. This further demonstrates its advantages in practice.

5. Conclusions

In conclusion, this paper introduces a robust geometric controller for a quadrotor UAV, formulated directly on the nonlinear SE(3) configuration manifold. To enhance its robustness against system uncertainties, an EKF-based parameter estimator was designed to estimate the inertia parameters of the quadrotor UAV, including mass and the inertial matrix. By integrating this estimator into the geometric controller loop, the entire control system was able to effectively identify uncertainties in the quadrotor UAV system, efficiently compensating for the effects of parameter perturbations. In the design of the estimator, quaternions were used to represent attitude and to avoid the singularities caused by Euler angles and the computational complexity brought by rotation matrices without affecting the control performance. The Lyapunov stability of the entire system is demonstrated, showing that the system remains stable during the convergence of estimation errors and exhibits exponential stability after the errors converge to zero.

To demonstrate the efficacy of the proposed method, a series of simulations and experiments were performed on a quadrotor UAV. The results demonstrated that the proposed controller performs much better than conventional geometric controllers, thereby underscoring its efficacy in advancing quadrotor control for real-world applications.