针对四旋翼无人机的姿态控制问题，本文设计了一种基于增量控制和非线性动态逆的控制器，用来降低传统动态逆控制策略对于模型依赖程度。以Pixhawk开源飞控板为基础，使用非线性动态逆、增量式非线性动态逆的控制方法构建姿态控制模块的角度、角速度控制环，通过PX4固件的软件在环仿真技术以及Gazebo仿真软件，验证该非线性控制器在外部扰动下的稳定性和鲁棒性。 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.
董宁，王立峰
北方工业大学电气与控制工程学院，北京
收稿日期：2018年8月16日；录用日期：2018年8月30日；发布日期：2018年9月6日
针对四旋翼无人机的姿态控制问题，本文设计了一种基于增量控制和非线性动态逆的控制器，用来降低传统动态逆控制策略对于模型依赖程度。以Pixhawk开源飞控板为基础，使用非线性动态逆、增量式非线性动态逆的控制方法构建姿态控制模块的角度、角速度控制环，通过PX4固件的软件在环仿真技术以及Gazebo仿真软件，验证该非线性控制器在外部扰动下的稳定性和鲁棒性。
关键词 :增量式非线性动态逆，四旋翼，姿态控制
Copyright © 2018 by authors and beplay安卓登录
This work is licensed under the Creative Commons Attribution International License (CC BY).
http://creativecommons.org/licenses/by/4.0/
近年来四旋翼无人机已经大量应用于民用、军事领域，愈发复杂的环境和操作都对无人机的控制系统提出了更加严格的要求。随着现代控制理论的发展，考虑到四旋翼无人机的不稳定以及非线性特性，出现了许多非线性控制方法。文献 [
为了更好的解决动态逆控制器的模型依赖和测量值误差等问题，在其基础上演变出了一种叫做增量式非线性动态逆的控制方法，它在延续动态逆控制器优良性能的同时，大大降低了对系统模型的依赖程度，提高了系统的鲁棒性。文献 [
本文旨在Pixhawk飞控板搭载的PX4开源自驾仪软件中，应用非线性动态逆、增量式非线性动态逆的控制策略分别实现四旋翼无人机的外、内环姿态控制，并通过软件在环仿真考核系统在模型不确定和外部扰动情况下的鲁棒性。
本文的后续从以下几个部分进行展开说明。第二节对四旋翼无人机的动力学模型进行简单的介绍。第三节对非线性动态逆、增量式非线性动态逆理论进行说明，然后对姿态控制的外、内环设计将在第四节展示。第五节是仿真验证阶段，通过飞行效果图、姿态曲线验证控制器效果。第六节是总结与讨论。
通常，描述四旋翼无人机速度、位置和姿态角等信息时，需要建立地面坐标系 E = ( O e X e Y e Z e ) 和机体坐标系 B = ( O b X b Y b Z b ) 来共同表示四旋翼的状态，见图1。
通过坐标系B相对于坐标系E的方向余弦阵R：
R = [ cos ( ψ ) cos ( θ ) sin ( ψ ) cos ( θ ) − sin ( θ ) − sin ( ψ ) cos ( ϕ ) + cos ( ψ ) sin ( θ ) sin ( ϕ ) cos ( ψ ) cos ( ϕ ) + sin ( ψ ) sin ( θ ) sin ( ϕ ) cos ( θ ) sin ( ϕ ) sin ( ψ ) sin ( ϕ ) + cos ( ψ ) sin ( θ ) cos ( ϕ ) − cos ( ψ ) sin ( ϕ ) + sin ( ψ ) sin ( θ ) cos ( ϕ ) cos ( θ ) cos ( ϕ ) ] (1)
可以得到如下关系的运动学方程：
P ˙ = R V (2)
Ω ˙ = R w ω (3)
P = ( X , Y , Z ) T 代表四旋翼的位置信息， V = ( U , V , W ) T 是四旋翼在机体坐标系B下的线速度， Ω = ( ϕ , θ , ψ ) T 表示四旋翼的姿态角， ω = ( P , Q , R ) T 为四旋翼机体坐标系B下的角速度。则旋转运动学方程用公式(4)表示。
根据牛顿-欧拉方程可以建立四旋翼的合外力、合外力矩方程：
I是四旋翼无人机关于质心的惯性张量矩阵，机体坐标系的原点Ob位于四旋翼的质心，由于四旋翼无人机为一刚体且关于X、Y轴对称，所以 I x y = I x z = I y x = I y z = I z x = I z y = 0 ，惯性矩阵可以转换为式(6)形式。
(6)
图1. 地面坐标系E与机体坐标系B
给定这样一种多输入多输出的非线性系统， x ∈ R m 为系统的状态量， u ∈ R n 为输入量， y ∈ R n 为输出量，系统的输入量与输出量的维度一致。
x ˙ = f ( x ) + g ( x ) u y = h ( x ) (7)
对上式输出部分进行微分得到式(8)。
y ˙ = ∂ h ∂ x x ˙ = ∂ h ∂ x f ( x ) + ∂ h ∂ x g ( x ) u = F ( x ) + G ( x ) u (8)
其中 G ( x ) ∈ R m × n 必须是可逆的，定义隐式输入 替代
相比于非线性动态逆，增量式非线性动态逆对输入动态没有要求，因此采用一个更典型的非线性系统：
x ̇ = f ( x , u ) (10)
将其在当前时刻进行泰勒展开，其中 x 0 ∈ R m 表示当前时刻的状态量， u 0 ∈ R n 表示当前时刻的系统输入量。
x ˙ ≈ f ( x 0 , u 0 ) + ∂ f ( x , u ) ∂ x | x = x 0 , u = u 0 ( x − x 0 ) + ∂ f ( x , u ) ∂ u | x = x 0 , u = u 0 ( u − u 0 ) x ˙ ≈ x 0 ̇ + F ( x 0 , u 0 ) ( x − x 0 ) + G ( x 0 , u 0 ) ( u − u 0 ) (11)
这个线性化的过程是假设在非常小的采样时间间隔内，并且系统输入量u的变化要远远快于状态量x。因此在假设时间足够小的情况下
x ˙ = x ˙ 0 + G ( x 0 , u 0 ) ( u − u 0 ) (12)
定义隐式输入 υ = x ˙ ，则可以得到新的控制律，控制增益矩阵 G ( x 0 , u 0 ) ∈ R m × n 必须是可逆的。
u = u 0 + G − 1 ( x 0 , u 0 ) ( υ − x ˙ 0 ) (13)
将式(12)离散化后得到离散时间的增量式非线性动态逆控制律式(13)，以下标k标记的变量为当前时刻的变量，k + 1即为未来时刻的时间标记，k − 1表示上一时刻。
u k + 1 = u k + G − 1 ( x k , u k ) ( υ k + 1 − x k − x k − 1 Δ t ) (14)
υ k + 1 = x k + 1 − x k Δ t (15)
角度外环的设计思想是作用于角度误差得到期望的角速度，即姿态的误差通过角速度来弥补。而角速度与角度之间的关系由第二节中运动学方程已经推导得出，因此选择非线性动态逆的控制策略来设计外环控制器。
u = G − 1 υ (16)
υ = K a n g e = [ K ϕ ( ϕ d − ϕ ) K θ ( θ d − θ ) K ψ ( ψ d − ψ ) ] (17)
G − 1 = [ 1 0 0 0 cos ( ϕ ) − sin ( ϕ ) − sin s ( θ ) cos ( θ ) sin sin ( ϕ ) cos ( θ ) cos cos ( ϕ ) ] (18)
角速度内环的设计思路为作用于角速度误差得到期望的角加速度，即角速度误差通过角加速度来弥补。
由系统的力距方程可知式19。
∑ M = M a + M c (19)
M a ∈ R 3 代表气动力距， M c ∈ R 3 表示系统的控制力距。在一个无穷小的时间间隔内，如果控制力据发生很小的变化 Δ M c ，那么角加速度 ω ˙ ∈ R 3 将随之变换，但是气动力距 M a 和角速度 并不会发生变化。这样，可以得到增量形式的表达式
Δ M c = I Δ ω ˙ = I ( ω ˙ d − ω ˙ ) (20)
对 Δ M c 进行线性化并整理式(20)。
Δ M c = M c ( Δ u ) (21)
Δ u = M c − 1 I ( υ − ω ˙ ) (22)
υ = K v e l e = [ K p ( P d − P ) K q ( Q d − Q ) K r ( R d − R ) ] (23)
P d , Q d , R d ∈ R 是期望的角速度， P , Q , R ∈ R 是测量的角速度， K v e l ∈ R 3 是比例环节的增益。这样新的控制律中将不再有参数 M a ，不需要建立完整的气动力距方程，降低了对参考模型的依赖程度。图2是完整的姿态控制结构图。
PX4固件支持软件在环仿真和硬件在环仿真两种仿真模式。软件在环仿真可以将完整的飞行控制栈运行在电脑上，通过仿真软件和地面站查看模拟飞行情况和飞行曲线，可以很好的验证改写代码的运行效果。硬件在环仿真是飞控板与仿真软件相连并将模拟固件运行在飞控板上的仿真，它可以测试代码在实际处理器中的运行结果。本文应用增量式非线性动态逆的控制策略对PX4固件进行改写，以Gazebo仿真软件为平台，搭建软件在环仿真的仿真环境，验证控制器在模拟飞行中的飞行效果，仿真数据流如图3所示。
图2. 姿态控制结构图
图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米位置，将油门杆置于死区内，高度曲线可以表明飞行器具有良好的定高效果。
| 参数 | 描述 | 值 |
|---|---|---|
| m | 四旋翼总重 | 1.5 kg |
| Ixx | 机体关于x轴的转动惯量 | 0.03 kg/m2 |
| Iyy | 机体关于y轴的转动惯量 | 0.05 kg/m2 |
| Izz | 机体关于z轴的转动惯量 | 0.10 kg/m2 |
| Ct | 旋翼升力系数 | 8.06 * 10−4 |
| Cq | 旋翼扭矩系数 | 1.0 * 10−6 |
表1. 四旋翼无人机模型参数
图4. 四旋翼“iris”无人机
图5. Mission模式航迹图
图6. 航点“2”姿态曲线图
图7. 航点“6”姿态曲线图
图8. 定高模式姿态曲线图
本文采用增量式非线性动态逆的控制策略，设计了新的四旋翼控制器。通过对PX4固件的改写并应用软件在环仿真技术，使用Gazebo仿真软件在由“风”的环境下模拟四旋翼的飞行。凭借着四旋翼在航迹规划和定高飞行实验中的良好表现，验证了控制器的稳定性及在外部扰动下的鲁棒性。
董 宁,王立峰. 基于增量式非线性动态逆的四旋翼无人机姿态控制Incremental Nonlinear Dynamics Inversion for Attitude Control of Quadrotor[J]. 动力系统与控制, 2018, 07(04): 253-261. https://doi.org/10.12677/DSC.2018.74028