固定翼无人机动态逆控制方法与工程实现

Dynamic Inversion Control Method and Engineering Implementation for Fixed-Wing Unmanned Aerial Vehicle

摘要: 无人机的稳定性和平稳性是飞行控制的一项重要指标，而无人机飞行时产生的非线性问题是控制率设计中亟需解决的问题。本文以固定翼无人机为研究对象，研究了固定翼飞行控制的动态逆控制方法，搭建了固定翼无人机飞行控制系统框架图，并设计了自适应动态逆控制器及多种飞行控制模式，完成数据接收和解算，在飞控硬件设备上完成了工程实施并进行了飞行试验。初步飞行试验表明，所设计的动态逆控制器满足了固定翼无人机飞行控制的需求。

Abstract: The stability and smoothness of UAV are important indicators of flight control, while the nonlinear problems generated during UAV flight are crucial issues that need to be addressed in control rate design. This paper focuses on fixed-wing UAV, studying the dynamic inverse control method for fixed-wing flight control. A framework diagram of the fixed-wing UAV flight control system was constructed, and an adaptive dynamic inverse controller and multiple flight control modes were designed to complete the data reception and decalculation. Engineering implementation was carried out on the flight control hardware, followed by flight tests. Preliminary flight tests indicate that the designed dynamic inverse controller meets the requirements for fixed-wing UAV flight control.

1. 绪论

无人驾驶飞行器，也即无人机，英文名称为Unmanned Aerial Vehicle，简称为“UAV”，是利用机载飞控系统和无线遥控设备的不载人飞机，是先进控制技术、机械制造技术、航空动力推进技术、传感器技术、通信技术及信息处理技术等结合的综合产物[1]。

要想让无人机脱离人的控制，实现自主飞行[2]，需要分步进行。首先无人机需要实现稳定平衡[3]，其次实现无人机在有遥控的情况下按照遥控者的意愿飞行，最后实现无人机在无遥控的情况下利用自身的传感器实现定点悬停和位置控制。飞行控制系统在硬件方面主要包括主控板、各种传感器模块、遥控器等。其中，主控板上有各种芯片[4]，包括主控芯片、陀螺仪、加速度计、磁力计、气压计、数传电台等。在软件方面包括各个传感器模块的驱动设计、控制算法设计、数据处理等在无人机机体设计方面，目前市场上主要流行的是以旋翼无人机、固定翼无人机为研究对象的飞控系统[5]。

本文以固定翼无人机为研究对象，研究了固定翼飞行控制的动态逆控制方法，搭建了固定翼无人机飞行控制系统框架图，并设计了自适应动态逆控制器及多种飞行控制模式，在飞控硬件设备上完成了工程实施并进行了飞行试验。

2. 动态逆控制方法

固定翼无人机控制系统的数学模型具有高度复杂性，是高强度耦合的欠驱动系统，因此各种非线性控制方法常用于固定翼无人机飞行轨迹控制[6]-[8]。本文提出一种基于动态逆控制理论的控制方法，构造系统的全状态反馈，实现对系统的非线性特性。例如，固定翼无人机在飞行过程中会受到气流和风的干扰，而这些干扰会导致飞行器的非线性响应。通过应用动态逆控制方法，可以对系统进行反馈补偿，使其具有线性传递关系，从而实现对飞行器的精确控制[9] [10]。

给定一种非线性系统仿射型控制系统：

$\dot{x}=g\left(x\right)u+f\left(x\right)+\Delta$ (1)

其中，$x\in R^n$ 、$u\in R^m$ 分别为状态和控制向量且$m=n$ 。$g\left(x\right)$ 、$f\left(x\right)$ 连续可微且$g\left(x\right)$ 可逆。$\Delta$ 为模型误差或外部干扰，且$\dot{\text{Δ}}\approx 0$，$\Vert \text{Δ}\Vert \le C$ ，C为常数。

构造动态逆输入量：

$u=g^{-1}\left(x\right)\left(-f\left(x\right)+\left(k_i{\displaystyle {\int }_0^{t}e\text{d}t}+k_{p}e\right)\right)$ (2)

其中，$e=x_r-x$ 为反馈误差。式(2)代入式(1)，得到闭环状态方程：

$\dot{x}=\left(k_{p}e+k_i{\displaystyle {\int }_0^{t}e\text{d}t}\right)+\Delta$(3)

其中，k为对角矩阵。对e求导可得：

$\dot{e}={\dot{x}}_r-\dot{x}$ (4)

式(4)代入式(3)，并化简可得：

$\dot{e}=-k_{p}e-k_I{\displaystyle {\int }_0^{t}e\text{d}t}-\Delta +{\dot{x}}_r=-k_{p}e-\delta$(5)

其中，$\delta =-k_I{\displaystyle {\int }_0^{t}e\text{d}t}-\Delta +{\dot{x}}_r$，对其求导，并忽略模型误差的慢变化有：

$\dot{\delta }=-k_{\text{I}}e$(6)

由于参考轨迹${\dot{x}}_r$ 在特定的时间范围内变化缓慢或无变化，可看作常数并且外部干扰和模型误差可认为是有界的[11]。公式(5)和公式(6)构成被控对象的动态闭环系统模型。这样解耦了非线性部分，且闭环系统模型为线性模型。

3. 逆方法构建固定翼无人机飞行控制系统

1) 姿态角指令

固定翼无人机飞行的位置变化视其期望的运动轨迹分为连续的一系列的序列段点。在某段时间内无人机从w₁飞到w₂，固定翼无人机飞行方式分为两种：目标航向点跟踪以及盘旋。这两种模式的航点w₁以及w₂之间的近似距离为：

$d=dis{t}_{\left(w_2,w_1\right)}=\sqrt{{\left(w_2.lat-w_1.lat\right)}^2+{\left(w_2.lng-w_1.lng\right)}^2}{R}_e$ (7)

其中，lat为纬度，lng为经度，$R_e$ 为地球半径。

其俯仰角指令${\theta }_c$ 模型为：

${\theta }_c=\arctan \left(\left(w_2.alt-w_1.alt\right),dis{t}_{\left(w_2,w_1\right)}\right).$ (8)

从当前航点w₁跟踪飞行到目标航点w₂，其运动轨迹可近似为一条w₁到w₂的一条直线。其偏航角指令模型为：

${\psi }_c=180+\arctan \left(\left(w_2.lng-w_1.lng\right),\left(w_2.lat-w_1.lat\right)\right)$ (9)

固定翼跟踪和盘旋飞行的偏航角与滚转角的关系为：

$\dot{\psi }=\frac{g}{V_t}\tan \left(\varphi \right)$ (10)

对式(10)求逆得到滚转角${\varphi }_c$ 动态逆控制指令：

${\varphi }_c=\arctan \left(\frac{\dot{\psi }{v}_t}{g}\right).$ (11)

2) 姿态控制

应用动态逆方法，姿态角控制的角速度指令如下：

$\left[\begin{array}{c}{p}_c\\ q_c\\ r_c\end{array}\right]={\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]}^{-1}\left[\begin{array}{c}{k}_{\varphi }\left({\varphi }_c-\varphi \right)\\ k_{\theta }\left({\theta }_c-\theta \right)\\ k_{\psi }\left({\psi }_c-\psi \right)\end{array}\right]$ (12)

固定翼飞机的体轴力矩L、M、N与转动角速度的关系[12]，应用动态逆方法，有：

$\left[\begin{array}{c}L\\ M\\ N\end{array}\right]={\left[\begin{array}{ccc}{c}_3& 0& c_4\\ 0& c_7& 0\\ c_4& 0& c_9\end{array}\right]}^{-1}\left[\left[\begin{array}{c}{k}_p\left(p_c-p\right)+k_i{\displaystyle {\int }_0^{dt}\left(p_c-p\right)\text{d}t}\\ k_p\left(q_c-q\right)+k_i{\displaystyle {\int }_0^{dt}\left(q_c-q\right)\text{d}t}\\ k_p\left(r_c-r\right)+k_i{\displaystyle {\int }_0^{dt}\left(r_c-r\right)\text{d}t}\end{array}\right]-\left[\begin{array}{c}\left(c_{1}r+c_{2}p\right)q\\ c_{5}pr-c_6\left(p^2-r^2\right)\\ \left(c_{8}p-c_{2}r\right)q\end{array}\right]\right]$ (13)

其中，p、q、r为体轴角速度，c₁~c₉为和飞机转动惯量有关的系数[13] [14]。其中，I_x、I_y、I_z、I_xz为体轴转动惯量。

$\begin{array}{l}{c}_1=\left(I_y-I_z\right)I_z-I_{xz}^2,c_2=\left(I_x-I_y+I_z\right)I_{xz},c_3=I_z\\ c_4=I_{xz},c_5=\frac{I_z-I_x}{I_y},c_6=\frac{I_{xz}}{I_y}\\ c_7=\frac{1}{I_y},c_8=I_x\left(I_x-I_y\right)+I_{xz}^2,c_9=I_x\end{array}$ (14)

对于常用小型固定翼无人机，体轴转动惯量参考值参见表1。

Table 1. UAV system parameter

表1. 无人机系统参数

参数	值	参数	值	参数	值
Jy	0.144 kg∙m^2	Jz	0.162 kg∙m^2	Jx	0.0894 kg∙m^2
Jxz	0.01 kg∙m^2

3) 舵面指令

S为翼展面积，b为翼展长度，c为弦长，$Q=\rho V_t^2/2$ 。指令力矩C_l、C_m和C_n分别为：

$\begin{array}{l}{C}_l=L/\left(QSb\right)\\ C_m=M/\left(QSc\right)\\ C_n=N/\left(QSb\right)\end{array}$ (15)

副翼${\delta }_a$ 、升降舵${\delta }_e$ 、方向舵${\delta }_r$ 的等效偏转量为：

$\begin{array}{l}{\delta }_a\approx k_a{C}_l\\ {\delta }_e\approx k_e{C}_m\\ {\delta }_r\approx k_r{C}_n\end{array}$ (16)

其中，k_a、k_e、k_r分别为副翼、升降舵和方向舵比例常数。固定翼力矩是攻角、侧滑角和速度等参数的函数，由于这些参数测量不准确，因此模型还有待研究。

4. 系统框架图

本文已论证基于自适应动态逆控制算法的控制理论。其整体控制系统框架见图1。

Figure 1. Dynamic inversion control system block diagram

图1. 动态逆控制系统框架图

在图1中，(1)~(4)为遥控器通道，分别控制飞机的滚转、俯仰、偏航角度以及油门的大小。其中，Inv1、Inv2、Inv3、Inv4动态逆模型分别对应式(10)、式(12)、式(13)以及式(15)。

5. 控制模式框架图

1) AUX手动模式

手动模式即通过遥控器进行控制。主要控制滚转角、俯仰角、偏航角和油门。

2) 起飞模式

起飞模式即从起飞点到目标航点的过程。起飞点斜向5度大力掷出，此时飞机会进行航点跟踪，在起飞的这段时间不需要人为参与，飞行为自主飞行。经过一段时间后飞机会达到目标航点，其地面位移约为巡航高度的五倍。其起飞模式见图2。

Figure 2. Takeoff mode diagram

图2. 起飞模式示意图

3) Auto模式

Auto模式，即航点跟踪，预先通过地面站管理器设置好多个航点，无人机会按照航点规划完成从第一个航点到最后一个航点的飞行。最后会在最后一个航点进行盘旋。在此过程中不需要飞手参与，其飞行过程为自主飞行。其Auto模式见图3。

Figure 3. Auto mode diagram

图3. Auto模式示意图

4) LOITER盘旋模式

在此模式下，无人机会绕航点盘旋，此时飞控系统会根据盘旋半径以及航点之间的距离计算坡度角(滚转角)指令和航向角(偏航角)指令，其飞行过程为自主飞行。其盘旋模式见图4。

Figure 4. Loiter mode diagram

图4. 盘旋模式示意图

5) RTL模式

RTL模式即返航模式。在此模式中，返航点的经纬度即为无人机起飞时Home点的经纬度坐标，而返航点的高度设置为巡航高度。执行RTL模式后，无人机会飞行返航点。当无人机回到返航点后，切换模式Aux人为接管，手动控制遥控器控制油门，并逐渐减小油门。此时无人机会绕返航点进入盘旋模式，最终回到起飞点。其RTL模式见图5。

Figure 5. RTL mode diagram

图5. RTL模式示意图

6. 飞行控制模式实现

飞控系统为无人机提供了多种飞行模式，使操作者能够根据不同的任务要求和环境条件选择最合适的飞行模式。在无人机上电并收到GPS数据且没有任何飞行模块失效后解锁。解锁之后，无人机会记录当前坐标为返航点。操作人员可以通过遥控器或地面站选择合适的飞行模式，每种模式都有不同的逻辑和算法，保证无人机稳定飞行。飞行模式程序流程图见图6。

Figure 6. Fixed-wing flight mode flowchart

图6. 固定翼飞行模式流程图

其中，姿态计算包括式(8)~(12)，舵面计算包括式(13)~(16)。

7. 数据采集结果

由控制模式框架图可知，在搭建逆方法固定翼无人机飞行控制系统时，需要获取遥控器数据、GPS数据、气压计数据、电池电量数据和IMU姿态角数据。

通道1、2、3、4分别为滚转角通道、俯仰角通道、油门通道和偏航角通道。遥控器数据接收见图7。

GPS信号数据接收包括经度、纬度、高度、卫星数量和地速，提供飞行控制系统位置信息和下一目标航点w₂位置信息。GPS数据接收见图8。

气压计提供无人机飞行高度、大气温度、大气压力和飞行速度。气压计数据接收见图9。

通过ADC采集电池电量。电池电压数据接收见图10。

Figure 7. Remote control data reception

图7. 遥控器数据接收

Figure 8. GPS data reception

图8. GPS数据接收

Figure 9. Barometer data reception

图9. 气压计数据接收

Figure 10. Battery voltage data reception

图10. 电池电压数据接收

无人机姿态角通过构造互补滤波器完成对姿态角的滤波[15]。互补滤波器包含预测模型和补偿模型。通过预测陀螺仪的角速度，再补偿加速度和磁场强度计算出的姿态角，最终得到滤波后的姿态角。滤波过程见图11。

Figure 11. Filtration flowchart

图11. 滤波流程图

模拟无人机飞行，测试结果表明姿态角滤波符合飞行控制系统的需求。姿态角接收见图12。

Figure 12. Attitude receive

图12. 姿态角接收

8. 飞行试验

1) 飞行试验准备

飞行试验是理论分析与实践结合的一个过程，飞行试验的顺利进行证明了动力学模型的建立和控制算法的研究是成功的。本实验以硬件电路采用模块化设计，以单片STM32F407VET6为主控芯片，搭配双IMU、气压计、磁力计和GPS。将动态逆控制程序下载到飞控板中，并放入到无人机的机舱中进行地面调试，并对其舵面和电调进行校准。在飞行前需进行AUX自平衡，验证无人机动态逆控制方法的可行性。

无人机通电后，连接GPS并解锁飞控板，在地面站选择AUX手动模式，检查无人机的AUX自平衡。将无人机机头朝下，水平尾翼此时应向上，将无人机机头朝上，水平尾翼此时应向下。水平尾翼AUX自平衡测试见图13。

Figure 13. Horizontal tail AUX self-balancing test

图13. 水平尾翼AUX自平衡测试

无人机向左倾斜，左副翼应向下，右副翼应向上；无人机向右倾斜，左副翼应向上，右副翼应向下。副翼AUX自平衡测试见图14。

Figure 14. Aileron AUX self-balancing test

图14. 副翼AUX自平衡测试

无人机向右偏转，垂直尾翼应向左偏转；无人机向左偏转，垂直尾翼应向右偏转。垂直尾翼AUX自平衡测试见图15。

Figure 15. Vertical tail AUX self-balancing test

图15. 垂直尾翼AUX自平衡测试

2) 初步飞行试验

调试成功后进行飞行试验，再次对无人机进行AUX自平衡测试。AUX自平衡测试见图16。

Figure 16. AUX self-balancing test again before takeoff

图16. 起飞前再次测试AUX自平衡

图17和图18为初步飞行试验现场照片。无人机手抛起飞后，稳定飞行一段时间后，遭遇5级阵风，飞行实验平台抗风等级较弱，飞行失控并坠毁。后续还需要大量的飞行试验用于验证飞行动态逆控制方法和多种模式的飞行。

Figure 17. Hand-throw takeoff

图17. 手抛起飞

Figure 18. Photo of preliminary flight test

图18. 初步飞行试验的照片

9. 结论

本文以固定翼无人机为研究对象，研究了固定翼飞行控制的动态逆控制方法，搭建了固定翼无人机飞行控制系统框架图，并设计了自适应动态逆控制器及多种飞行控制模式。初步飞行试验表明，所设计的动态逆控制器基本满足了固定翼无人机飞行控制的需求。