上海理工大学机械工程学院，上海

收稿日期：2023年12月17日；录用日期：2023年12月28日；发布日期：2024年1月31日

摘要

四旋翼无人机的执行机构一旦发生故障就会发生坠毁，会导致重大经济损失，并对地面行人的安全构成严重威胁。本文应用增量式非线性动态逆(incremental nonlinear dynamic inverse, INDI)实现无人机的容错控制，分析了INDI控制器在空气动力学扰动下的稳定性，并通过搭建仿真平台对控制器的性能进行了半物理仿真测试。结果表明，INDI容错控制在执行器故障和有空气动力学扰动情况下仍然能够悬停和跟踪期望轨迹，从而避免坠毁和人员伤亡。基于INDI的无人机容错控制和稳定性分析，以及所构建的无人机控制半物理仿真验证平台，可以为无人机控制算法研究和验证方法提供参考。

关键词

四旋翼无人机，INDI，空气动力学扰动，稳定性分析，半物理仿真

Stabilization Analysis and Simulation of a Quadrotor UAV Applying INDI for Fault-Tolerant Control

Jinyao Wu

School of Mechanical Engineering, University of Shanghai for Science and Technology, Shanghai

Received: Dec. 17^th, 2023; accepted: Dec. 28^th, 2023; published: Jan. 31^st, 2024

ABSTRACT

Crashes of quadrotor UAVs can occur in the event of actuator failure, which can lead to significant economic losses and pose a serious threat to the safety of pedestrians on the ground. In this paper, incremental nonlinear dynamic inverse (INDI) is applied to achieve fault-tolerant control of UAVs, the stability of the INDI controller under aerodynamic perturbations is analysed, and the performance of the controller is tested in semi-physical simulation by building a simulation platform. The results show that the INDI fault-tolerant control is still able to hover and track the desired trajectory in the presence of actuator failures and aerodynamic perturbations, thus avoiding crashes and casualties. The INDI-based fault-tolerant control and stability analysis of UAVs and the constructed semi-physical simulation verification platform for UAV control can provide a reference for UAV control algorithm research and verification methods.

Keywords:Quadrotor UAV, INDI, Aerodynamic Perturbations, Stability Analysis, Semi-Physical Simulation

Copyright © 2024 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

四旋翼无人机的执行机构(电机，电调和旋翼)长时间运转后容易发生故障而引起坠毁事故，可能导致重大经济损失和人身安全隐患 [1] 。为确保四旋翼无人机在执行机构故障情况下的飞行安全，本文研究了基于非线性动态逆(incremental nonlinear dynamic inverse, INDI)算法的四旋翼无人机的容错控制，同时分析其在空气动力学扰动下的稳定性，并通过半物理仿真验证其在单个执行机构完全失效和空气动力学扰动下的有效性。

目前，常见的四旋翼无人机运动控制算法有比例–积分–微分(PID)控制、线性二次型调节器(LQR)控制、滑模控制(SMC)、神经网络控制、非线性动态逆控制(nonlinear dynamic inverse, NDI)和增量非线性动态逆(INDI) [2] [3] [4] [5] 。其中，非线性动态逆(NDI)方法在解耦控制和消除非线性因素方面具有优势，尤其对多变量系统有效，同时该方法能在非线性和线性系统兼有情况下直接设计控制律。然而，NDI控制对系统模型依赖强，控制性能与数学模型精度密切相关，模型不确定性引发逆误差增大，制约了非线性耦合特性抵消，使其鲁棒性难以保证。尤其针对飞行器，在高速飞行中，某些动力学参数无法测量和预测，导致数学模型不确定，从而使NDI鲁棒性不足。INDI算法可以很好地解决NDI控制算法存在的问题。INDI是在NDI方法的基础上设计，与NDI不同，INDI通过直接的传感器测量并计算处理的量来抵消不确定参数和与模型输入项无关的干扰，降低了NDI对精确系统模型的依赖 [6] 。

本文将研究应用INDI方法实现四旋翼无人机容错控制，并分析其在空气动力学扰动下稳定性。首先，建立四旋翼无人机的降阶动力学模型，推导容错控制率；然后，采用李雅普诺夫方法分析了INDI容错控制律在空气动力扰动下的稳定性。最后，搭建了基于Pixhawk飞控板的半物理仿真环境，验证了该方法在模型不确定和空气动力学扰动情况下的性能。

2. 旋翼无人机的容错控制器设设计

2.1. 容错控制器设计

“X”形四旋翼无人机因其结构灵活、机动性强，同时避免视野遮挡问题，适用范围广，特别适合搭载相机等载荷，因此本文选择此作为研究机型。

通常，为了描述无人机的速度、位姿等信息，需要建立机体坐标系 $B_b=\left(O_b{X}_b{Y}_b{Z}_b\right)$ 相和地面坐标系 $B_e=\left(O_e{X}_e{Y}_e{Z}_e\right)$ ，两个坐标系示意图如图1所示。

图1. 四旋翼无人机坐标系示意图

通过拉格朗日方程可得四旋翼无人机飞行姿态动力学模型如下：

$\{\begin{array}{l}F=ma=m\stackrel{\dot{}}{\nu }\hfill \\ M=J\stackrel{\dot{}}{\Omega }+\Omega \times J\Omega \hfill \end{array}$ (1)

其运动学模型如下：

$\{\begin{array}{l}{\stackrel{\dot{}}{\xi }}^e={\nu }^e\hfill \\ \stackrel{\dot{}}{\Theta }=R{\Omega }^b\hfill \end{array}$ (2)

整合运动学与动力学公式，得到非线性六自由度模型，如下式：

$\{\begin{array}{l}\ddot{x}=\frac{{\lambda }_1}{m}\left(c\psi s\theta c\varphi +s\psi s\varphi \right)\hfill \\ \stackrel{\dot{}}{y}=\frac{{\lambda }_1}{m}\left(s\psi s\theta c\varphi -c\psi s\varphi \right)\hfill \\ \ddot{z}=\frac{{\lambda }_1}{m}\varphi c\theta -g\hfill \\ \ddot{\varphi }=\frac{1}{I_{xx}}\left[{\lambda }_2+qr\left(I_{yy}-I_{zz}\right)-J_{r}qW\right]\hfill \\ \stackrel{\dot{}}{\theta }=\frac{1}{I_{yy}}\left[{\lambda }_3+pr\left(I_{zz}-I_{xx}\right)+J_{r}pW\right]\hfill \\ \stackrel{\dot{}}{\psi }=\frac{1}{I_{zz}}\left[{\lambda }_4+pq\left(I_{xx}-I_{yy}\right)\right]\hfill \end{array}$ (3)

然而，在实际飞行中，无人机在空中存在明显的空气动力学效应，例如上下推力的变化、机身空气动力学等因素 [7] [8] 。这些空气动力学效应增加了模型的不确定性，所以要添加扰动项，得到真实的运动方程，如下所示：

$\{\begin{array}{l}m{\stackrel{\dot{}}{\nu }}^e=mg{e}_z-L_b^e\left(f-F_a\right)\hfill \\ J{\stackrel{\dot{}}{\Omega }}^b=-{\Omega }^b\times J{\Omega }^b+G_a+\tau +\left[\begin{array}{c}0\\ 0\\ -\gamma r\end{array}\right]+M_a\hfill \end{array}$ (4)

其中， $F_a$ 和 $M_a$ 分别表示空气动力学对无人机模型产生的力与力矩， $\gamma$ 为气动偏摆阻尼系数 [9] 。在执行器故障后分析对无人机质心的受力。以右下单转子失效为例，无人机偏航力矩不再平衡，总合力与力矩亦不再平衡，剩余三个转子难以维持足够升力，同时破坏了原四转子间相互抵消的反扭力矩平衡，导致无人机绕垂直轴线旋转。受力情况如图2所示：

图2. 单转子失效情况下无人机的受力(f)与力矩(T)

无人机在单转子失效后的位姿发生改变，但其减少的位姿是恒定的。因此通过可以牺牲其它部分的控制来维持飞行状态，以求解周期解使得无人机在平衡点附近稳定 [10] 。定义在机体坐标系下有单位向量

$e^b={\left[e_x^b,e_y^b,e_z^b\right]}^{\text{T}}$ ，在故障时， $e^b$ 与平均推力方向对齐，平均推力与故障前瞬时推力方向对齐，无人机不存在摆动，此时 $e_x^b=0,e_y^b=0$ 。此时，受力情况如图3所示：

图3. e^b与瞬间升力同向对齐

现将另一个向量定义为 $n_d^b$ ，表示故障后三个轴方向的向量分布情况 [11] ，其中 $n_d^b$ 的一阶导数推导：

${\stackrel{\dot{}}{n}}_d^b=-n_d^b\times {\Omega }^b+L^T{n}_d^e$ (5)

其中，令 $n_d^b={\left[h_1,h_2,h_3\right]}^{\text{T}},L^{\text{T}}{n}_d^e={\left[{\delta }_1,{\delta }_2,{\delta }_3\right]}^{\text{T}}$ ，为了维持无人机的飞行状态，需将期望状态向量 $n_d^b$ 与 $e^b$ 对齐。该机体所受拉力、力矩与旋翼转速的关系式如下：

${\left[\begin{array}{cccc}f& {\tau }_x& {\tau }_y& {\tau }_z\end{array}\right]}^{\text{T}}=Gu$ (6)

其中，G为控制效率矩阵，具体如下所示：

$G={\kappa }_0\left[\begin{array}{cccc}-1& -1& -1& -1\\ b\sin \beta & -b\sin \beta & -b\sin \beta & b\sin \beta \\ b\cos \beta & b\cos \beta & -b\cos \beta & -b\cos \beta \\ \sigma & -\sigma & \sigma & -\sigma \end{array}\right]$ (7)

${\kappa }_0$ 为悬停条件下的推力系数， $\sigma$ 是转子的推力系数与阻力系数之比， $b$ 为转子中心到质心的距离， $\beta$ 为X形四旋翼无人机的几何参数。故障时输入变为 $\tilde{u}={\left[\begin{array}{ccc}{\varpi }_1^2& {\varpi }_2^2& {\varpi }_3^2\end{array}\right]}^{\text{T}}$ 。此时已经不能保证无人机的稳定状态，需要对动力重新进行分配，求解新的控制效率矩阵，即 $\tilde{G}$ ，式子如下：

$\left[\begin{array}{c}{T}^{\prime }\\ {{\tau }^{\prime }}_x\\ {{\tau }^{\prime }}_x\end{array}\right]=\tilde{G}\tilde{u}=\left[\begin{array}{c}-{\kappa }_0{\sigma }_1^2-{\kappa }_0{\sigma }_2^2-{\kappa }_0{\sigma }_3^2\\ b{\kappa }_0\sin \beta {\varpi }_1^2-b{\kappa }_0\sin \beta {\varpi }_2^2-b{\kappa }_0\sin \beta {\varpi }_3^2\\ b{\kappa }_0\cos \beta {\varpi }_1^2+b{\kappa }_0\cos \beta {\varpi }_2^2-b{\kappa }_0\cos \beta {\varpi }_3^2\end{array}\right]$ (8)

在此基础上，设计基于INDI的容错控制器。其中外环位置控制为PID控制，如下式：

$a_{ref}=\left[\begin{array}{c}-k_p{e}_x-k_d{\stackrel{\dot{}}{e}}_x-k_i{{\displaystyle \int }}^{\text{}}{e}_x\text{d}t\\ -k_p{e}_y-k_d{\stackrel{\dot{}}{e}}_y-k_i{{\displaystyle \int }}^{\text{}}{e}_y\text{d}t\\ {\ddot{z}}_r\end{array}\right]$ (9)

其中 $e_x$ 、 $e_y$ 为水平误差， $k_p$ 、 $k_d$ 、 $k_i$ 为PID控制增益。内环姿态控制为INDI控制来实现容错控制，并排除扰动项 $F_a$ 和 $M_a$ 的影响。考虑一个多输入多输出的非线性系统：

$\begin{array}{l}\stackrel{\dot{}}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}$ (10)

其输入、输出线性化是将输出 $y_i$ 不断微分，系统输出展开简写为： $y^{\left(\rho \right)}=\mu \left(x\right)+\eta \left(x\right)u$ ，在线性化之前，将系统转化为Byrnes-Isidori标准型 [12] 。定义 $z=T\left(x\right)={\left[\begin{array}{c}{T}_1\left(x\right),T_2\left(x\right)\end{array}\right]}^{\text{T}}={\left[\begin{array}{c}\xi ,\lambda \end{array}\right]}^{\text{T}}$ 邻域 $R_0$ 上的微分同胚。根据弗罗比尼厄斯(Frobenius)定理得到新的非线性系统，如下式：

$\begin{array}{l}\stackrel{\dot{}}{\xi }=f_{\xi }\left(\xi ,\lambda \right)\\ \stackrel{\dot{}}{\lambda }=A_c\lambda +B_c\nu \\ y=C_c\lambda \end{array}$ (11)

其中 $A_c,B_c$ 与 $C_c$ 是 ${\rho }_i$ 个积分器的规范形矩阵。当矩阵 $\eta$ 具有满秩时，NDI的控制律为：

$u_{NDI}=\eta {\left(x\right)}^{+}\left[\nu -\mu \left(x\right)\right]$ (12)

其中， ${\left(\text{*}\right)}^{+}$ 表示矩阵的伪逆，ν为虚拟输入。但是在实际运用中，由于模型的简化与外部干扰等因素， $\mu \left(x\right)$ 是不可能完全消去的，也无法建立精准的数学模型。因此通过INDI控制律来进一步减少NDI对模型的依赖。设计增量控制输入： $\Delta u=\hat{\eta }{\left(x_0\right)}^{†}\left[\nu -y_0^{\left(\rho \right)}\right]$ ，得到INDI控制律：

$u_{INDI}=\hat{\eta }{\left(x_0\right)}^{+}\left(\nu -y_0^{\left(\rho \right)}\right)+u_0$ (13)

其中， $\hat{\eta }\left(x_0\right)$ 为控制效率矩阵。带入非线性系统的表达式可得到基于INDI线性化条件下的系统表达式：

$\begin{array}{l}\stackrel{\dot{}}{\xi }=f_{\xi }\left(\xi ,\lambda \right)\\ \stackrel{\dot{}}{\lambda }=A_c\lambda +B_c\left[\nu +\delta \left(z,\Delta t\right)\right]\\ y=C_c\lambda \end{array}$ (14)

此时定义系统的状态向量 $x_{in}$ 和为系统的输出参考函数 $y_r$ ：

$\{\begin{array}{l}{x}_{in}={[z,V_z,h_1,h_2,p,q,r]}^{\text{T}}\hfill \\ y_r={\left[z_r,h_1-e_x^b,h_2-e_y^b\right]}^{\text{T}}\hfill \end{array}$ (15)

得到的状态方程为：

$\left[\begin{array}{c}\stackrel{\dot{}}{z}\\ {\stackrel{\dot{}}{\nu }}_z\\ {\stackrel{\dot{}}{h}}_1\\ {\stackrel{\dot{}}{h}}_2\\ \stackrel{\dot{}}{p}\\ \stackrel{\dot{}}{q}\\ \stackrel{\dot{}}{r}\end{array}\right]=\left[\begin{array}{c}{\nu }_z\\ g+F_{a,z}-G_{3,3}{\kappa }_0\left(u_1+u_2+u_3\right)/m\\ h_{3}p-h_{1}r+{\delta }_2\\ -h_{3}q+h_{2}r+{\delta }_1\\ A_{x}rq-2a_{x}q\bar{o}+M_{a,x}+G_p\left(u_1-u_2-u_3\right)\\ A_{y}rp+2a_{y}p\bar{\omega }+M_{a,y}+G_q\left(u_1+u_2-u_3\right)\\ A_{z}pq-\gamma r/I_z+M_{a,z}+G_r\left(u_1-u_2+u_3\right)\end{array}\right]$ (16)

该系统存在六个外部状态 ${\lambda }_1{\lambda }_2{\lambda }_3{\lambda }_3{\lambda }_3{\lambda }_3$ ：

$\left[\begin{array}{c}{\lambda }_1\\ {\lambda }_2\\ {\lambda }_3\\ {\lambda }_4\\ {\lambda }_4\\ {\lambda }_5\\ {\lambda }_6\end{array}\right]=\left[\begin{array}{c}{y}_1\\ {\stackrel{\dot{}}{y}}_1\\ y_2\\ {\stackrel{\dot{}}{y}}_2\\ {\stackrel{\dot{}}{y}}_2\\ y_3\\ {\stackrel{\dot{}}{y}}_3\end{array}\right]=\left[\begin{array}{c}z\\ {\nu }_z\\ h_1-e_x^b\\ -h_{3}q+h_{2}r\\ h_2-e_y^b\\ h_{3}p-h_{1}r\end{array}\right]$ (17)

带入得到虚拟输入 $v$ 的表达式：

$v=\left[\begin{array}{c}-k_{z,p}\left(z-z_r\right)-k_{z,d}\left({\nu }_z-{\stackrel{\dot{}}{z}}_r\right)+{\ddot{z}}_r\\ -k_{a,p}\left(h_1-e_x^b\right)-k_{a,d}\left(h_{2}r-h_{3}q\right)\\ -k_{a,p}\left(h_2-e_y^b\right)-k_{a,d}\left(h_{3}p-h_{1}r\right)\end{array}\right]$ (18)

上式可得到姿态控制的输出量，继而可以求出剩余转子转速，其中控制效率矩阵为：

$\hat{\eta }\left(x_0\right)=\left[\begin{array}{ccc}-{\kappa }_0& -{\kappa }_0& -{\kappa }_0\\ b{\kappa }_0\sin \beta & -b{\kappa }_0\sin \beta & -b{\kappa }_0\sin \beta \\ b{\kappa }_0\cos \beta & b{\kappa }_0\cos \beta & -b{\kappa }_0\cos \beta \end{array}\right]$ (19)

同时：

$\begin{array}{l}\tilde{u}={\left[\begin{array}{ccc}{\varpi }_1^2& {\varpi }_2^2& {\varpi }_3^2\end{array}\right]}^{\text{T}}\\ u_{INDl}=\hat{\eta }{\left(x_0\right)}^{+}\left(\nu -y_0^{\left(\rho \right)}\right)+u_0\end{array}$ (20)

其中 $\hat{\eta }\left(x_0\right)$ 与 $v$ 已得， $y_0^{\left(\rho \right)}$ 有 $y_0^{\left(\rho \right)}={\ddot{y}}_r={\left[\begin{array}{c}{a}_{z,r},{\ddot{h}}_{1,r},{\ddot{h}}_{2,r}\end{array}\right]}^{\text{T}}$ 其值可以通过直接读取滤波输出的二阶导数得到，

$u_0$ 则是此时传感器测量得到的电机转速值。通过输入输出反馈线性化公式求得执行器故障后剩余三个电机转子转速的调整值，同时不存在空气动力学的干扰。

2.2. 空气动力学扰动下的稳定性分析

本节将讨论与状态变化相关的项对闭环系统稳定性的影响，同时针对前文设计的控制系统进行零动态稳定性分析。在INDI控制下的闭环系统的原点 $z=0$ 处进行稳定性分析。定义虚拟输入 $V=-K\lambda$ ，则有：

$\begin{array}{l}\xi =f_{\xi }\left(\xi ,\lambda \right)\\ \stackrel{\dot{}}{\lambda }=\left(A_c-B_{c}K\right)\lambda +B_c\delta \left(z,\Delta t\right)\end{array}$ (21)

满足 $A_c-B_{c}K$ 赫尔维茨稳定。由于输出为线性表达式，此处可以略去。针对扰动项 $\delta \left(z,\text{Δ}t\right)$ ，项应该保留在闭环系统的表达式中，同时影响着控制系统的稳定性。考虑以下系统作为标准的控制系统：

$\begin{array}{l}\stackrel{\dot{}}{\xi }=f_{\xi }\left(\xi ,\lambda \right)\\ \stackrel{\dot{}}{\lambda }=\left(A_c-B_{c}K\right)\lambda \end{array}$ (22)

其中 $\stackrel{\dot{}}{\xi }=f_{\xi }\left(\xi ,0\right)$ 被称为零动态 [13] ，如果非线性系统的零动态具有一个渐近稳定的平衡点，则称其为渐近最小相位。系统扰动项的范数值为：

${\Vert \delta \left(x,\Delta t\right)\Vert }_2={\Vert \frac{\partial \left[\mu \left(x\right)+\eta \left(x\right)u\right]}{\partial x}\Vert }_0\Delta x+{\Vert \mathcal{O}\left(\Delta x^2\right)\Vert }_2$ (23)

且 $\mu \left(x\right)$ 与 $\eta \left(x\right)$ 关于 $x$ 的偏导数有界、 $x$ 是连续的，带入扰动项有：

${\lim }_{\Delta t\to 0}{\Vert \delta \left(z,\Delta t\right)\Vert }_2=0,\forall x\in R^n$ (24)

这个式子说明对于非线性系统，随着采样频率的不断提高，系统中存在的空气扰动项的范数值可以大幅降低，扰动项的影响可以忽略不计。同时方程表明，存在一个 $\text{Δ}t$ ，可以保证 $\delta \left(z,\text{Δ}t\right)$ 的有界性。并且增加采样频率，可以进一步减小界限值。接下来分析系统自身的稳定性只需分析内动态的稳定性即可验证整个系统的稳定性。前面提到 $x_{in}$ 总共有7个状态，其中有6个外动态，故只需要确定一个满足上文约束条件的内动态即可。因此可以对内部状态做出以下选择：

${\xi }_1=r+{\mu }_1{\nu }_z+{\mu }_{2}p+{\mu }_{3}q$ (25)

其中 ${\mu }_1,{\mu }_2,{\mu }_3$ 为常数，与转子机械特性有关。此时基于INDI原理排除系统中的扰动项，同时参考姿态 $n_d^e$ 是慢时变的 [14] ，因此有如下约束条件：

$\begin{array}{l}{\lambda }_i=0,i=1,2,\cdots ,6\\ u_1=u_2=u_3=0,\bar{\omega }=0\\ F_{a,x}=F_{a,y}=F_{a,z}=0,M_{a,z}=0\\ {\stackrel{\dot{}}{n}}_d^e={\left[{\delta }_1,{\delta }_2,{\delta }_3\right]}^{\text{T}}\bigsqcup 0\end{array}$ (26)

对公式 ${\xi }_1=r+{\mu }_1{\nu }_z+{\mu }_{2}p+{\mu }_{3}q$ 取一阶微分并整理，可以得到：

$\{\begin{array}{l}{\stackrel{\dot{}}{\xi }}_1=-\frac{\gamma }{\Gamma }{\xi }_1+\frac{\Lambda }{h_3{\Gamma }^2}{\xi }_1^2+g{\mu }_1\hfill \\ \Gamma =\frac{e_x^b{\mu }_2}{h_3}+\frac{e_y^b{\mu }_3}{h_3}+1\hfill \\ \Lambda =A_x{e}_y^b{\mu }_2+A_y{e}_x^b{\mu }_3+\frac{A_z{e}_x^b{e}_y^b}{h_3}\hfill \end{array}$ (27)

将 $e_x^b=e_y^b=0$ 代入并化简可得：

${\stackrel{\dot{}}{\xi }}_1=-\gamma \left({\xi }_1-g{\mu }_1\right)=-\gamma \left({\xi }_1-{\bar{\xi }}_1\right)$ (28)

上式(28)为控制系统的零动态方程，其中偏航阻尼 $\gamma >0$ 。式(27)为一阶线性非齐次微分方程，由其通解形式可知系统的内动态 ${\xi }_1$ 在平衡点 ${\bar{\xi }}_1$ 处稳定。通过INDI控制原理设计的容错控制器，只要传感器采样频率足够高，就可以降低扰动项范数值，从而减小无人机飞行中的空气动力学影响，增强飞控系统的鲁棒性，同时也验证了零动态稳定性。

3. 半物理仿真环境搭建

无人机的PX4固件有两种仿真模式——软件在环仿真和硬件在环仿真。本文主要进行硬件在环仿真(半物理仿真)。硬件在环仿真环境将Pixhawk控制器集成到仿真过程，更加接近无人机在真实世界中的情形。在安装了Ubuntu系统的计算机环境中进行半物理仿真，测试容错控制器的性能。本文选择Pixhawk进行无人机系统的开发，在Offboard模式下进行Pixhawk与Gazebo的联合半物理仿真，如图4所示。

图4. 半物理仿真环境示意图

本研究的仿真测试采用了RotorS固件，其对Linux系统具有出色的兼容性。此外，仿真平台配备了IMU、里程计等传感器，能够实时且高效地监测并反馈无人机在故障发生后的位置和姿态变化。随后创建ROS工作空间并编译固件。同时，本文选用的无人机仿真模型带有精度较高的ADIS-16448 IMU，其可以提供较高精度的姿态与位置信息，能够符合INDI控制算法对传感器精度的要求。

同时，为了监控无人机四个旋翼电机的转速变化，并获取故障后无人机位姿的详细数据，加入rqt-plot和rqt-bag这两个工具。rqt-plot用于实时展示电机转速的波动情况，而rqt-bag则用于订阅并记录在执行器故障后的飞行位姿数据。这些数据对于分析和评估容错控制器的性能至关重要。如下图5所示：

图5. 四旋翼无人机各个电机转速

仿真条件设置为：为确保无人机在单转子完全失效情况下仍能飞行并保持稳定，我们整合了所设计的容错控制器至底层PID控制框架。当检测到电机转子故障时，系统会自动切换到相应的容错控制算法。通过ROS的话题发布与订阅机制，这一过程在Gazebo模拟器中得以实现。进一步地，以支持故障注入和飞行轨迹设定，我们移除了自动轨迹跟踪，并通过脚本动态发送实时话题，在控制器中加入了对脚本话题的订阅。

主要流程为在Gazebo软件中载入无人机模型与仿真环境模型，将加入INDI容错控制律的固件刷写到Pixhawk主板中，通过地面站使无人机进入Offboard模式，与MAVROS进行通信，在接收二次开发程序进行外部精准控制。通过ROS节点发布话题实现无人机飞行控制与转子故障注入。最后订阅无人机位姿话题得到仿真数据以分析半物理仿真下容错控制器的性能。

4. 仿真结果与讨论

图6. 半物理仿真无人机飞行轨迹曲线(a) 无人机空间飞行轨迹；(b) 飞行轨迹俯视图

图7. 仿真结果图(a) 无人机质心各分轴位置变化曲线；(b) 无人机各分轴速度变化曲线；(c)滚转角与俯仰角变化曲线图；(d) 滚转角速度与俯仰角速度变化曲线

启动半物理仿真平台后，本文通过ROS话题发布轨迹命令，使无人机飞至A (1, 1, 1)点保持悬停状态，后沿B点(3, 2, 3)、C点(2, 3, 2)和A点(1, 1, 1)巡航飞行并观察无人机变化，同时订阅话题得到无人机位姿数据，如图6、图7所示：

由上述仿真结果分析可知：图6可以看到，在故障后无人机仍能按照一系列期望点执行飞行任务，其飞出一段距离后无人机调整姿态逐渐回正至转向点，最大偏离量为0.2m。图7(a)可以看出无人机在保证较小偏移量的前提下能够快稳地飞到预设点位，并且调整姿态保持平衡以继续飞行。由图7(b)可以发现，无人机不同时间段飞行速度的快慢导致了其质心运动轨迹的不同。由图7(c)可以看出无人机在改变飞行方向时翻滚角与俯仰角有较大的变化，在到达目的地后趋于稳定。由图7(d)可以看到在到达目标点后沿直线飞行的过程中，角速度经历突然增大，之后逐步减小到零，符合无人机实际飞行情况。

总之，在单个执行器完全失效的情况下，通过Pixhawk飞控执行INDI容错控制器进行在线控制变量调节和姿态修正，无人机在空气动力学扰动下仍能保持较为良好的轨迹跟踪能力。尽管受硬件和串口影响，飞行轨迹仍存在明显的偏离，但仍能在可接受的误差范围内可靠地飞向目标点，有效避免无人机的碰撞与坠毁。这验证了容错控制器的有效性、稳健性和在实际硬件上的可执行性。

5. 结论

为了实现四旋翼无人机在单个执行机构完全失效和有空气动力学扰动情况下的安全飞行和轨迹跟踪，本文研究了基于INDI的容错控制器和稳定性分析。首先，本文推导了四旋翼无人机在一个螺旋桨失效时的降阶动力学模型，并推导得到了INDI容错控制律。然后，分析了论证了INDI容错控制在空气动力学扰动下的稳定性。最后，搭建了基于Pixhawk飞控和ROS-Gazebo的无人机控制半物理仿真环境。在硬件在环仿真测试中，我们对执行器故障后的无人机进行了精准的悬停和轨迹飞行实验，仔细分析了其位姿数据。实验结果显示，容错控制器在故障发生后能迅速调整剩余转子的转速，实现高度稳定的飞行。悬停状态下的偏离量控制在±0.05米以内，轨迹飞行时的偏离量也仅在±0.2米范围内，有效确保了无人机在故障情况下的安全飞行，极大减少了碰撞和坠落的风险。仿真结果表明，虽然失去一个偏航角的自由度，四旋翼无人机在风扰动下仍然具有良好的轨迹跟踪能力，验证了INDI容错控制算法的有效性和稳健性。

文章引用

吴金遥. 四旋翼无人机应用INDI实现容错控制的稳定性分析与仿真
Stabilization Analysis and Simulation of a Quadrotor UAV Applying INDI for Fault-Tolerant Control[J]. 建模与仿真, 2024, 13(01): 902-912. https://doi.org/10.12677/MOS.2024.131087

参考文献