In recent years, quadcopter unmanned aerial vehicles (UAVs) have gained significant attention due to their versatile applications in areas such as aerial photography, agricultural monitoring, and emergency response. However, controlling quadcopters remains challenging due to their underactuated nature, strong coupling, and nonlinear dynamics. Additionally, real-world environments often introduce bounded uncertainties and obstacles that can compromise both tracking performance and safety. This paper addresses the problem of obstacle avoidance and tracking control for a quadcopter system subject to bounded unknown disturbances. We propose a novel control strategy that integrates sliding mode backstepping with control barrier functions (CBFs) and quadratic programming (QP) to ensure robust tracking while maintaining safety.
The quadcopter dynamics are modeled with consideration of inertial and body-fixed frames. The system is described by the following equations:
$$ \dot{\mathbf{p}} = \mathbf{v}, $$
$$ \dot{\mathbf{v}} = \frac{1}{m} \mathbf{F} – g \mathbf{e}_3 + \mathbf{d}_p, $$
$$ \dot{\boldsymbol{\eta}} = \boldsymbol{\omega}, $$
$$ \dot{\boldsymbol{\omega}} = -\mathbf{J}^{-1} \boldsymbol{\omega} \times \mathbf{J} \boldsymbol{\omega} + \mathbf{J}^{-1} \boldsymbol{\tau} + \mathbf{d}_{\eta}, $$
where \(\mathbf{p} = [x, y, z]^T\) and \(\mathbf{v} = [v_x, v_y, v_z]^T\) denote the position and linear velocity in the inertial frame, \(\boldsymbol{\eta} = [\phi, \theta, \psi]^T\) represents the Euler angles, and \(\boldsymbol{\omega} = [\omega_\phi, \omega_\theta, \omega_\psi]^T\) is the angular velocity in the body frame. The control inputs are the thrust \(u\) and torques \(\boldsymbol{\tau} = [\tau_1, \tau_2, \tau_3]^T\), with \(m\) as the mass, \(g\) as gravity, and \(\mathbf{J} = \text{diag}(J_{xx}, J_{yy}, J_{zz})\) as the inertia matrix. The disturbances \(\mathbf{d}_p\) and \(\mathbf{d}_{\eta}\) are bounded by a known constant \(\bar{d}\), i.e., \(|\mathbf{d}_i| < \bar{d}\) for \(i = x, y, z, \phi, \theta, \psi\). The virtual control force \(\mathbf{F} = u \mathbf{R} \mathbf{e}_3\) is introduced to transform the underactuated system into a fully actuated one, where \(\mathbf{R}\) is the rotation matrix.
We design a nominal tracking controller using sliding mode backstepping to achieve trajectory tracking. The tracking errors for position and attitude are defined as \(\mathbf{z}_p = \mathbf{p} – \mathbf{p}_d\) and \(\mathbf{z}_{\eta} = \boldsymbol{\eta} – \boldsymbol{\eta}_d\), with reference signals \(\mathbf{p}_d = [x_d, y_d, z_d]^T\) and \(\boldsymbol{\eta}_d = [\phi_d, \theta_d, \psi_d]^T\). Sliding surfaces are constructed as:
$$ \mathbf{s}_p = \mathbf{A}_{p1} \mathbf{z}_p + \dot{\mathbf{z}}_p, $$
$$ \mathbf{s}_{\eta} = \mathbf{A}_{\eta 1} \mathbf{z}_{\eta} + \dot{\mathbf{z}}_{\eta}, $$
where \(\mathbf{A}_{p1}\) and \(\mathbf{A}_{\eta 1}\) are positive definite diagonal matrices. Lyapunov functions are chosen as \(V_{p1} = \frac{1}{2} \mathbf{z}_p^T \mathbf{z}_p\) and \(V_{\eta 1} = \frac{1}{2} \mathbf{z}_{\eta}^T \mathbf{z}_{\eta}\), and their derivatives are computed to derive control laws. The nominal control inputs are:
$$ \mathbf{F}_{\text{norm}} = m \left[ -\mathbf{z}_p – \mathbf{A}_{p1} (-\mathbf{A}_{p1} \mathbf{z}_p + \mathbf{s}_p) + g \mathbf{e}_3 – \frac{c_1}{4} \mathbf{s}_p + \ddot{\mathbf{p}}_d – \mathbf{A}_{p2} \mathbf{s}_p – \tanh\left(\frac{\mathbf{s}_p}{\epsilon}\right) \right], $$
$$ \boldsymbol{\tau} = \mathbf{J} \left[ -\mathbf{z}_{\eta} – \frac{c_2}{4} \mathbf{s}_{\eta} + \ddot{\boldsymbol{\eta}}_d – \mathbf{A}_{\eta 1} (-\mathbf{A}_{\eta 1} \mathbf{z}_{\eta} + \mathbf{s}_{\eta}) – \mathbf{A}_{\eta 2} \mathbf{s}_{\eta} – \tanh\left(\frac{\mathbf{s}_{\eta}}{\epsilon}\right) \right] + \boldsymbol{\omega} \times \mathbf{J} \boldsymbol{\omega}, $$
where \(\mathbf{A}_{p2}\), \(\mathbf{A}_{\eta 2}\), \(c_1\), \(c_2\), and \(\epsilon\) are design parameters. The hyperbolic tangent function reduces chattering inherent in sliding mode control. The stability analysis shows that the tracking errors converge to a small neighborhood of zero, ensuring practical stability.
For obstacle avoidance, we employ control barrier functions to define safe sets. Consider a spherical obstacle with center \(\mathbf{p}_c^0\) and radius \(r_0\). The safe set is \(\mathcal{C}_0 = \{ \mathbf{p} \in \mathbb{R}^3 : \|\mathbf{p} – \mathbf{p}_c^0\|^2 – r_0^2 \geq 0 \}\). To account for the relative degree, we introduce a CBF that incorporates velocity:
$$ h_1(\mathbf{p}, \mathbf{v}) = \|\mathbf{p} – \mathbf{p}_c^0\|^2 – r_0^2 – \delta_0^2 – \mathbf{v}^T \mathbf{v}, $$
where \(\delta_0\) is a tuning parameter. The set \(\mathcal{C}_1 = \{ (\mathbf{p}, \mathbf{v}) \in \mathbb{R}^3 \times \mathbb{R}^3 : h_1(\mathbf{p}, \mathbf{v}) \geq 0 \}\) is forward invariant if the control input satisfies the CBF constraint. We define an extended class \(\mathcal{K}\) function \(\alpha(h_1) = \gamma h_1\) with \(\gamma > 0\), leading to the constraint:
$$ \dot{h}_1(\mathbf{p}, \mathbf{v}) + \gamma h_1(\mathbf{p}, \mathbf{v}) \geq 0. $$
Substituting the dynamics, this becomes:
$$ 2 \mathbf{v}^T (\mathbf{p} – \mathbf{p}_c^0) – 2 \mathbf{v}^T \left( \frac{1}{m} \mathbf{F} – g \mathbf{e}_3 \right) – c_3 \|\mathbf{v}\|^2 – \frac{1}{c_3} \bar{d}^2 + \gamma h_1 \geq 0, $$
where \(c_3 > 0\) is a design constant. We formulate a quadratic programming problem to minimize the deviation from the nominal controller while satisfying the safety constraint:
$$ \mathbf{F}^* = \arg \min_{\mathbf{F} \in \mathbb{R}^3} \frac{1}{2} \|\mathbf{F} – \mathbf{F}_{\text{norm}}\|^2, $$
$$ \text{subject to } 2 \mathbf{v}^T (\mathbf{p} – \mathbf{p}_c^0) – 2 \mathbf{v}^T \left( \frac{1}{m} \mathbf{F} – g \mathbf{e}_3 \right) – c_3 \|\mathbf{v}\|^2 – \frac{1}{c_3} \bar{d}^2 + \gamma h_1 \geq 0. $$
The solution yields a safe controller \(\mathbf{F} = \mathbf{F}_{\text{norm}} + \mathbf{F}_{\text{safe}}\), where the safety correction term is:
$$ \mathbf{F}_{\text{safe}} = \begin{cases}
\frac{m \mathbf{v}}{2 \|\mathbf{v}\|^2} \Psi, & \text{if } \Psi < 0, \\
0, & \text{if } \Psi \geq 0,
\end{cases} $$
with \(\Psi = 2 \mathbf{v}^T (\mathbf{p} – \mathbf{p}_c^0) – 2 \mathbf{v}^T \left( \frac{1}{m} \mathbf{F}_{\text{norm}} – g \mathbf{e}_3 \right) – c_3 \|\mathbf{v}\|^2 – \frac{1}{c_3} \bar{d}^2 + \gamma h_1\). This ensures that the quadcopter maintains a safe distance from obstacles while closely tracking the reference trajectory.
We present simulation results to validate the proposed approach. The quadcopter parameters are \(m = 1 \, \text{kg}\), \(\mathbf{J} = \text{diag}(0.02, 0.08, 0.07) \, \text{kg·m}^2\), and disturbances are \(\mathbf{d}_p = [0.2 \sin(0.1\pi t), 0.2 \cos(0.1\pi t), 0.2 \sin(0.1\pi t)]^T\) and \(\mathbf{d}_{\eta} = [0.2 \sin(0.1\pi t) + 0.1, 0.2 \cos(0.1\pi t) + 0.1, 0.2 \sin(0.1\pi t) + 0.2]^T\). The reference trajectory is \(\mathbf{p}_d = [2 \cos t + 1, 2 \sin t + 1, 0.2t + 0.1]^T\) and \(\psi_d = \sin(\cos 0.5t)\). Controller parameters are summarized in Table 1.
| Parameter | Value |
|---|---|
| \(\mathbf{A}_{p1}\) | \(\text{diag}(1, 3, 7)\) |
| \(\mathbf{A}_{p2}\) | \(\text{diag}(2, 4, 5)\) |
| \(\mathbf{A}_{\eta 1}\) | \(\text{diag}(10, 15, 7)\) |
| \(\mathbf{A}_{\eta 2}\) | \(\text{diag}(15, 10, 7)\) |
| \(c_1\) | 10 |
| \(c_2\) | 5 |
| \(c_3\) | 5 |
| \(\gamma\) | 5 |
| \(\epsilon\) | 0.1 |
| \(\delta_0\) | 0 |
The obstacle is centered at \(\mathbf{p}_c^0 = [2, 3, 4]^T\) with radius \(r_0 = 0.5\). Initial conditions are \(\mathbf{p}_0 = [3, 1, 0]^T\), \(\mathbf{v}_0 = [0, 0, 0]^T\), \(\boldsymbol{\eta}_0 = [-0.5, 0.6, 0.7]^T\), and \(\boldsymbol{\omega}_0 = [1, 1, 0]^T\). The simulation results demonstrate that the quadcopter successfully avoids the obstacle while tracking the reference signal. Tracking errors converge to a small range, and control inputs are adjusted promptly when obstacles are encountered. The integration of CBF-QP ensures safety without significant compromise on tracking performance.
In conclusion, we have developed a comprehensive control framework for quadcopter systems that addresses both tracking and safety requirements. The combination of sliding mode backstepping and control barrier functions provides robustness to disturbances and guarantees obstacle avoidance. Quadratic programming enables efficient computation of safe control inputs. Future work will extend this approach to dynamic environments and multiple obstacles, further enhancing the applicability of quadcopter control in real-world scenarios.