基于增量式非线性动态逆的四旋翼无人机姿态控制

Incremental Nonlinear Dynamics Inversion for Attitude Control of Quadrotor

摘要: 针对四旋翼无人机的姿态控制问题，本文设计了一种基于增量控制和非线性动态逆的控制器，用来降低传统动态逆控制策略对于模型依赖程度。以Pixhawk开源飞控板为基础，使用非线性动态逆、增量式非线性动态逆的控制方法构建姿态控制模块的角度、角速度控制环，通过PX4固件的软件在环仿真技术以及Gazebo仿真软件，验证该非线性控制器在外部扰动下的稳定性和鲁棒性。

Abstract: In view of the attitude control of quadrotor, a controller based on Incremental Control and Non-linear Dynamic Inversion (NDI) is designed to reduce the dependence of the traditional Dynamic Inversion control strategy on the model. Based on the Pixhawk open source control board, the angle and angular velocity control loop of the attitude control module is constructed by using the Nonlinear Dynamic Inversion (NDI) and Incremental Nonlinear Dynamic Inversion (INDI) control method. The stability and robustness of the nonlinear controller under external disturbances are verified through the software in the loop (SITL) simulation technology of PX4 firmware and Gazebo simulation software.

1. 引言

近年来四旋翼无人机已经大量应用于民用、军事领域，愈发复杂的环境和操作都对无人机的控制系统提出了更加严格的要求。随着现代控制理论的发展，考虑到四旋翼无人机的不稳定以及非线性特性，出现了许多非线性控制方法。文献 [1] 对几种经典的非线性控制方法进行了介绍，如反馈线性化、反步控制、滑模变结构控制等，并分析了各自的优缺点。在这其中，文献 [2] 提出的动态逆技术通过大量仿真实验及成功的飞行试验，得到了广泛认可。这种方法利用状态反馈抵消系统的非线性特性，将原系统补偿为一个具有线性传递关系的伪线性新系统，具有很大的灵活性，并且可以应用在不同的无人机模型上。但由于对系统模型的严格要求，设计控制律时出现的近似值会导致系统误差的产生，从而无法保证系统的鲁棒性和稳定性。针对动态逆控制器的缺点，文献 [3] 提出的基于在线神经网络的动态逆控制器和文献 [4] 设计的自适应动态逆控制器，都可以最大程度的估计不确定参数来消除误差，仿真验证也表明控制器具有良好的控制性能。

为了更好的解决动态逆控制器的模型依赖和测量值误差等问题，在其基础上演变出了一种叫做增量式非线性动态逆的控制方法，它在延续动态逆控制器优良性能的同时，大大降低了对系统模型的依赖程度，提高了系统的鲁棒性。文献 [5] [6] 分别在固定翼、多旋翼无人机姿态控制中应用了增量式非线性动态逆的控制方法，并与动态逆、传统PID控制方法进行仿真比较，证明了该方法在模型和参数不确定情况下可以良好的解决姿态跟踪和干扰抑制的问题。文献 [7] 提出了一种自适应控制与增量式非线性动态逆控制相结合的方式，它可以在线估计控制增益，以消除人工调参或估计参数的需要，并且通过四旋翼无人机进行了实际飞行实验，验证了自适应能力。

本文旨在Pixhawk飞控板搭载的PX4开源自驾仪软件中，应用非线性动态逆、增量式非线性动态逆的控制策略分别实现四旋翼无人机的外、内环姿态控制，并通过软件在环仿真考核系统在模型不确定和外部扰动情况下的鲁棒性。

本文的后续从以下几个部分进行展开说明。第二节对四旋翼无人机的动力学模型进行简单的介绍。第三节对非线性动态逆、增量式非线性动态逆理论进行说明，然后对姿态控制的外、内环设计将在第四节展示。第五节是仿真验证阶段，通过飞行效果图、姿态曲线验证控制器效果。第六节是总结与讨论。

2. 四旋翼建模

2.1. 运动学方程

通常，描述四旋翼无人机速度、位置和姿态角等信息时，需要建立地面坐标系 $\text{E}=\left({\text{O}}_{\text{e}}{\text{X}}_{\text{e}}{\text{Y}}_{\text{e}}{\text{Z}}_{\text{e}}\right)$ 和机体坐标系 $\text{B}=\left({\text{O}}_{\text{b}}{\text{X}}_{\text{b}}{\text{Y}}_{\text{b}}{\text{Z}}_{\text{b}}\right)$ 来共同表示四旋翼的状态，见图1。

通过坐标系B相对于坐标系E的方向余弦阵R：

$R=\left[\begin{array}{c}\cos \left(\psi \right)\cos \left(\theta \right)\\ \sin \left(\psi \right)\cos \left(\theta \right)\\ -\sin \left(\theta \right)\end{array}\begin{array}{c}-\sin \left(\psi \right)\cos \left(\varphi \right)+\cos \left(\psi \right)\sin \left(\theta \right)\sin \left(\varphi \right)\\ \cos \left(\psi \right)\cos \left(\varphi \right)+\sin \left(\psi \right)\sin \left(\theta \right)\sin \left(\varphi \right)\\ \cos \left(\theta \right)\sin \left(\varphi \right)\end{array}\begin{array}{c}\sin \left(\psi \right)\sin \left(\varphi \right)+\cos \left(\psi \right)\sin \left(\theta \right)\cos \left(\varphi \right)\\ -\cos \left(\psi \right)\sin \left(\varphi \right)+\sin \left(\psi \right)\sin \left(\theta \right)\cos \left(\varphi \right)\\ \cos \left(\theta \right)\cos \left(\varphi \right)\end{array}\right]$ (1)

可以得到如下关系的运动学方程：

$\stackrel{\dot{}}{P}=RV$ (2)

$\stackrel{\dot{}}{\Omega }=R_w\omega$ (3)

$P={\left(X,Y,Z\right)}^{\text{T}}$ 代表四旋翼的位置信息， $V={\left(U,V,W\right)}^{\text{T}}$ 是四旋翼在机体坐标系B下的线速度， $\Omega ={\left(\varphi ,\theta ,\psi \right)}^{\text{T}}$ 表示四旋翼的姿态角， $\omega ={\left(P,Q,R\right)}^{\text{T}}$ 为四旋翼机体坐标系B下的角速度。则旋转运动学方程用公式(4)表示。

(4)

2.2. 动力学方程

根据牛顿-欧拉方程可以建立四旋翼的合外力、合外力矩方程：

(5)

I是四旋翼无人机关于质心的惯性张量矩阵，机体坐标系的原点O_b位于四旋翼的质心，由于四旋翼无人机为一刚体且关于X、Y轴对称，所以 $I_{xy}=I_{xz}=I_{yx}=I_{yz}=I_{zx}=I_{zy}=0$ ，惯性矩阵可以转换为式(6)形式。

$$ (6)

Figure 1. The ground coordinate system E and the body coordinate system B

图1. 地面坐标系E与机体坐标系B

3. 动态逆控制器设计

3.1. 非线性动态逆

给定这样一种多输入多输出的非线性系统， $x\in R^m$ 为系统的状态量， $u\in R^n$ 为输入量， $y\in R^n$ 为输出量，系统的输入量与输出量的维度一致。

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

对上式输出部分进行微分得到式(8)。

$\stackrel{\dot{}}{y}=\frac{\partial h}{\partial x}\stackrel{\dot{}}{x}=\frac{\partial h}{\partial x}f\left(x\right)+\frac{\partial h}{\partial x}g\left(x\right)u=F\left(x\right)+G\left(x\right)u$ (8)

其中 $G\left(x\right)\in R^{m\times n}$ 必须是可逆的，定义隐式输入 $$ 替代，可以得到完整的动态逆控制器。

(9)

3.2. 增量式非线性动态逆

相比于非线性动态逆，增量式非线性动态逆对输入动态没有要求，因此采用一个更典型的非线性系统：

$ẋ=f\left(x,u\right)$ (10)

将其在当前时刻进行泰勒展开，其中 $x_0\in R^m$ 表示当前时刻的状态量， $u_0\in R^n$ 表示当前时刻的系统输入量。

$\begin{array}{l}\stackrel{\dot{}}{x}\approx f\left(x_0,u_0\right)+{\frac{\partial f\left(x,u\right)}{\partial x}|}_{x=x_0,u=u_0}\left(x-x_0\right)+{\frac{\partial f\left(x,u\right)}{\partial u}|}_{x=x_0,u=u_0}\left(u-u_0\right)\\ \stackrel{\dot{}}{x}\approx \dot{x_0}+F\left(x_0,u_0\right)\left(x-x_0\right)+G\left(x_0,u_0\right)\left(u-u_0\right)\end{array}$ (11)

这个线性化的过程是假设在非常小的采样时间间隔内，并且系统输入量u的变化要远远快于状态量x。因此在假设时间足够小的情况下但 $u\ne u_0$ ,可以将方程简化成式(12)。

$\stackrel{\dot{}}{x}={\stackrel{\dot{}}{x}}_0+G\left(x_0,u_0\right)\left(u-u_0\right)$ (12)

定义隐式输入 $\upsilon =\stackrel{\dot{}}{x}$ ，则可以得到新的控制律，控制增益矩阵 $G\left(x_0,u_0\right)\in R^{m\times n}$ 必须是可逆的。

$u=u_0+G^{-1}\left(x_0,u_0\right)\left(\upsilon -{\stackrel{\dot{}}{x}}_0\right)$ (13)

将式(12)离散化后得到离散时间的增量式非线性动态逆控制律式(13)，以下标k标记的变量为当前时刻的变量，k + 1即为未来时刻的时间标记，k − 1表示上一时刻。

$u_{k+1}=u_k+G^{-1}\left(x_k,u_k\right)\left({\upsilon }_{k+1}-\frac{x_k-x_{k-1}}{\Delta t}\right)$ (14)

${\upsilon }_{k+1}=\frac{x_{k+1}-x_k}{\Delta t}$ (15)

4. 四旋翼姿态控制器设计

4.1. 角度外环

角度外环的设计思想是作用于角度误差得到期望的角速度，即姿态的误差通过角速度来弥补。而角速度与角度之间的关系由第二节中运动学方程已经推导得出，因此选择非线性动态逆的控制策略来设计外环控制器。

$u=G^{-1}\upsilon$ (16)

$\upsilon =K_{ang}e=\left[\begin{array}{c}{K}_{\varphi }\left({\varphi }_d-\varphi \right)\\ K_{\theta }\left({\theta }_d-\theta \right)\\ K_{\psi }\left({\psi }_d-\psi \right)\end{array}\right]$ (17)

$G^{-1}=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\begin{array}{c}0\\ \cos \left(\varphi \right)\\ -\sin \left(\varphi \right)\end{array}\begin{array}{c}-\sin \mathrm{s}\left(\theta \right)\\ \cos \left(\theta \right)\sin \sin \left(\varphi \right)\\ \cos \left(\theta \right)\cos \cos \left(\varphi \right)\end{array}\right]$ (18)

是期望的欧拉角，是测量的欧拉角， $K_{ang}\in R^3$ 即为比例环节的增益。

4.2. 角速度内环

角速度内环的设计思路为作用于角速度误差得到期望的角加速度，即角速度误差通过角加速度来弥补。

由系统的力距方程可知式19。

${{\displaystyle \sum }}^{\text{}}M=M_a+M_c$ (19)

$M_a\in R^3$ 代表气动力距， $M_c\in R^3$ 表示系统的控制力距。在一个无穷小的时间间隔内，如果控制力据发生很小的变化 $\Delta M_c$ ，那么角加速度 $\stackrel{\dot{}}{\omega }\in R^3$ 将随之变换，但是气动力距 $M_a$ 和角速度 $$ 并不会发生变化。这样，可以得到增量形式的表达式

$\Delta M_c=I\Delta \stackrel{\dot{}}{\omega }=I\left({\stackrel{\dot{}}{\omega }}_d-\stackrel{\dot{}}{\omega }\right)$ (20)

对 $\Delta M_c$ 进行线性化并整理式(20)。

$\text{Δ}{M}_c=M_c\left(\text{Δ}u\right)$ (21)

$\Delta u=M_c^{-1}I\left(\upsilon -\stackrel{\dot{}}{\omega }\right)$ (22)

$\upsilon =K_{vel}e=\left[\begin{array}{c}{K}_p\left(P_d-P\right)\\ K_q\left(Q_d-Q\right)\\ K_r\left(R_d-R\right)\end{array}\right]$ (23)

$P_d,Q_d,R_d\in R$ 是期望的角速度， $P,Q,R\in R$ 是测量的角速度， $K_{vel}\in R^3$ 是比例环节的增益。这样新的控制律中将不再有参数 $M_a$ ，不需要建立完整的气动力距方程，降低了对参考模型的依赖程度。图2是完整的姿态控制结构图。

5. 仿真结果

PX4固件支持软件在环仿真和硬件在环仿真两种仿真模式。软件在环仿真可以将完整的飞行控制栈运行在电脑上，通过仿真软件和地面站查看模拟飞行情况和飞行曲线，可以很好的验证改写代码的运行效果。硬件在环仿真是飞控板与仿真软件相连并将模拟固件运行在飞控板上的仿真，它可以测试代码在实际处理器中的运行结果。本文应用增量式非线性动态逆的控制策略对PX4固件进行改写，以Gazebo仿真软件为平台，搭建软件在环仿真的仿真环境，验证控制器在模拟飞行中的飞行效果，仿真数据流如图3所示。

Figure 2. Attitude control structure diagram

图2. 姿态控制结构图

Figure 3. Simulation data flow chart

图3. 仿真数据流

为了考核四旋翼飞行器在外部扰动情况下的鲁棒性，故在仿真环境中模拟“风”来阻碍无人机的运动。将“风”插件的源代码文件gazebo_wind_plugin.cpp及msg文件Wind. proto添加至CMakeLists.txt文件中，便可以使得“风”功能被编译。在四旋翼无人机“iris”(见图4)的数据文件iris.sdf中，添加一个沿x轴正方向、平均风力为2的“风”。编译通过后，在四旋翼飞行过程中，便会受到“风”的阻碍。模拟的四旋翼无人机“iris”模型参数如表1所示，参数值来源于无人机的数据文件iris.sdf。

搭建好仿真环境后，在QGroundControl地面站设置航点位置及航点任务，在mission模式下启动四旋翼按照航点飞行，并对四旋翼横滚和俯仰姿态曲线进行跟踪观察。从航迹图5中可以看出四旋翼的实际飞行轨与理论的飞行轨迹具有很高的重合度，在自动起飞航点“1”与自动返航航点“10”可以很好的达到指令要求。图6、图7展示了飞行过程中横滚、俯仰姿态曲线，紫色曲线为横滚角曲线，蓝色曲线为俯仰角曲线。图6中的凸起点表明四旋翼在航点“2”处的横滚弧度，因此四旋翼将有明显的向左横滚的动作。图7中的俯仰曲线稳定在0.6 rad左右并且无横滚信号，说明此时无人机在航点“6”处进行向前的俯仰运动。两幅图中姿态曲线都能维持在相对稳定的范围内，不会因为姿态变化过大或过快而导致四旋翼飞行的不稳定。另外，在四旋翼定高模式下也做了飞行实验(图8)，使用定高模式起飞达到约30米位置，将油门杆置于死区内，高度曲线可以表明飞行器具有良好的定高效果。

Table 1. Quadrotor model parameters

表1. 四旋翼无人机模型参数

Figure 4. “iris” quadrotor

图4. 四旋翼“iris”无人机

Figure 5. Mission mode track map

图5. Mission模式航迹图

Figure 6. The attitude curve of the 2nd navigation point

图6. 航点“2”姿态曲线图

Figure 7. The attitude curve of the 6th navigation point

图7. 航点“6”姿态曲线图

Figure 8. Alt Hold mode attitude curve

图8. 定高模式姿态曲线图

6. 总结

本文采用增量式非线性动态逆的控制策略，设计了新的四旋翼控制器。通过对PX4固件的改写并应用软件在环仿真技术，使用Gazebo仿真软件在由“风”的环境下模拟四旋翼的飞行。凭借着四旋翼在航迹规划和定高飞行实验中的良好表现，验证了控制器的稳定性及在外部扰动下的鲁棒性。