固定翼无人机飞行参数评估模型与实验

Fixed-Wing UAV Flight Parameter Evaluation Model and Experiment

摘要: 固定翼无人机飞行过程中，为减小模型参数的不确定性和外界扰动对控制器的影响，本文提出了一种飞行参数估计模型，在STM32上通过编程实现了该模型，通过飞行试验验证了该飞行参数估计模型的可行性。

Abstract: In order to reduce the uncertainty of model parameters and the influence of external disturbances on the controller during the flight of fixed-wing UAV, a flight parameter estimation model was proposed in this paper. The model was implemented through programming on STM32, and the feasibility of the flight parameter estimation model was verified through flight tests.

1. 引言

固定翼无人机因其续航时间长、飞行速度快、载重能力强等突出优点，广泛应用于军事和民用领域。在无人机的飞行过程，姿态解算、路径跟踪是整个飞控系统中不可或缺的重要部分 [1] [2] 。

AHRS互补滤波算法在保证良好的动态响应特性的基础上，减少了陀螺仪产生的累积误差，提高了固定翼无人机的准确性，使无人机具有良好的方向保持能力、抗磁扰能力 [3] [4] 。

迎角和侧滑角是飞行力学的两个重要的飞行状态参数，也是飞行控制及导航系统所需要的两个主要参数。他们目前主要是通过安装在飞机上的风标传感器、压差式传感器和零压差式传感器来测量的 [5] [6] 。这类传感器由于受到结冰及与飞行状态有关的局部环流的影响，几乎不可避免的会造成很大的零点偏差。迎角的容限偏差对于民航客机将会导致安全距离的缩小，对于战斗机而言将对控制性能的计算造成很大的误差，并且有可能在某种迎角情况下导致飞机不可控迎角也是飞机失控系统和失速告警所需要的重要参数。侧滑角的偏差将会导致飞机产生不必要的能量损失(如某些研究表明，1度侧滑角会增加1%的阻力) [7] [8] [9] 。迎角和侧滑角传感器测量的信号并以电压信号的方式输入到瞄准具用于修正航炮、导弹发射器的射击方位因此其测量准确性对于飞机的射击准确度有很大的影响。在一般情况下，要求这两个量的测量值精度不低于0.2度6。针对固定翼无人机的侧滑角、功角根本无法精确的测量，采用侧滑角、功角的预测的模型来辅助动态调节无人机的飞行参数 [10] [11] 。

本文将AHRS互补滤波和侧滑角、功角的预测模型结合起来，提出了一种实时校正轨迹跟踪和姿态角的模型，固定一组理想飞行参数代入固定翼原始模型中，作为参考模型，通过参考模型与实际系统之间的误差不断修正控制器参数，使实际系统可以逼近参考模型的输出，补偿系统因建模误差及外部干扰所产生的误差，提高系统稳定性和抗扰动性。在飞行参数和姿态角预测补偿的嵌套传递中，该方法能在提升控制效果的同时不增加控制器的复杂程度。最后通过飞行实验，获取实验数据，分析实验数据，验证了飞行参数估计模型的有效性。

2. 飞行参数估计模型

飞行参数估计模型主要包含两部分：姿态角的预测与补偿；功角、侧滑角以及飞行的预测与补偿。通过对固定翼无人机下一时刻的飞行参数进行预测来调节飞控的控制强度从而增加飞行的稳定性。

2.1. 姿态角的预测与补偿

2.1.1. 姿态角预测与补偿的设计

姿态角的预测与补偿的设计框图如下图1所示。

Figure 1. Block diagram of attitude angle prediction and compensation design

图1. 姿态角预测与补偿设计框图

2.1.2. 姿态角的预测的实现

采用陀螺仪获取固定翼无人机的三轴速度，根据雅可比矩阵提出本文姿态角预测的转移矩阵，预测的转移矩阵如下：

$Ft=\left(\begin{array}{cccc}1& -\frac{g_x\cdot d_t}{2}& -\frac{g_y\cdot d_t}{2}& -\frac{g_z\cdot d_t}{2}\\ \frac{g_x\cdot d_t}{2}& 1& \frac{g_z\cdot d_t}{2}& -\frac{g_y\cdot d_t}{2}\\ \frac{g_y\cdot d_t}{2}& -\frac{g_z\cdot d_t}{2}& 1& \frac{g_x\cdot d_t}{2}\\ \frac{g_z\cdot d_t}{2}& \frac{g_y\cdot d_t}{2}& -\frac{g_y\cdot d_t}{2}& 1\end{array}\right)$ (2-1)

g_x：陀螺仪输出的x轴的速度；g_y：陀螺仪输出的y轴的速度；g_z：陀螺仪输出的z轴的速度；d_t：飞行设备的预测步长(本文算法设计为10 ms)。

根据公式(2-2)~(2-4)，将陀螺仪传感器的三轴加速度转化为固定翼无人机的姿态角。

$\varphi =a\tan 2\left(accl.y,accl.z\right)$ (2-2)

$\theta =\text{atan}2\left(-accl.x,\sqrt{accl.y^2+accl.z^2}\right)$ (2-3)

$\psi =\text{atan}2(mag.z\times \sin \left(\varphi \right)-mag.y\times \cos \left(\varphi \right),mag.x\times \cos \left(\theta \right)+\sin \left(\theta \right)\times \left(mag.y\times \sin \left(\varphi +mag.z\times \cos \left(\varphi \right)\right)\right)$ (2-4)

Accx：陀螺仪输出的x轴加速度；accy：陀螺仪输出的y轴加速度；accz：陀螺仪输出的z轴加速度；φ：姿态角中的滚转角；θ：姿态角中的俯仰角； $\psi$ ：姿态角中的偏航角。

根据公式(2-5)将计算的姿态角转换为四元数，以便模型的计算。

$q_t={\left(\begin{array}{l}{q}_{\omega }\\ q_x\\ q_y\\ q_z\end{array}\right)}_{}=\left(\begin{array}{c}\text{cos}\left(\frac{\varphi }{2}\right)\text{cos}\left(\frac{\theta }{2}\right)\text{cos}\left(\frac{\psi }{2}\right)+\text{sin}\left(\frac{\varphi }{2}\right)\text{sin}\left(\frac{\theta }{2}\right)\text{sin}\left(\frac{\psi }{2}\right)\\ \text{sin}\left(\frac{\varphi }{2}\right)\text{cos}\left(\frac{\theta }{2}\right)\text{cos}\left(\frac{\psi }{2}\right)-\text{cos}\left(\frac{\varphi }{2}\right)\text{sin}\left(\frac{\theta }{2}\right)\text{sin}\left(\frac{\psi }{2}\right)\\ \text{cos}\left(\frac{\varphi }{2}\right)\text{sin}\left(\frac{\theta }{2}\right)\text{cos}\left(\frac{\psi }{2}\right)+\text{sin}\left(\frac{\varphi }{2}\right)\text{cos}\left(\frac{\theta }{2}\right)\text{sin}\left(\frac{\psi }{2}\right)\\ \text{cos}\left(\frac{\varphi }{2}\right)\text{cos}\left(\frac{\theta }{2}\right)\text{sin}\left(\frac{\psi }{2}\right)-\text{sin}\left(\frac{\varphi }{2}\right)\text{sin}\left(\frac{\theta }{2}\right)\text{cos}\left(\frac{\psi }{2}\right)\end{array}\right)$ (2-5)

其中 $\phi$ ：姿态角中的滚转角； $\theta$ ：姿态角中的俯仰角； $\psi$ ：姿态角中的偏航角；q_t：四元数。

根据当前时刻的四元数和状态转移矩阵F_t，建立预测方程如公式(2-6)计算预测四元数 $q_{t.step}$ 。

$q_{t.step}=F_t{q}_t$ (2-6)

2.1.3. 姿态角的补偿

仅陀螺仪输出的加速度作为预测姿态角输入，陀螺输出值含有误差，采用积分计算，误差会随着时间累积。陀螺仪动态响应特性良好，但计算姿态时会产生累积误差，磁力计和加速度计测量姿态没有累积误差。因此采用加速度计和磁力计的输出来获取准确的姿态角对预测四元数 $q_{t.step}$ 进行矫正。将矫正后的四元数转换为下一时刻的飞行参数。使用加速度计的输出和磁力计的输出带入公式(2-7)计算当前时刻准确的四元数 $q_{tMeat}$ 。补偿预测方程的输出如公式(2-7)：

$q_t=q_{tstep}*w+q_{tMeat}*\left(1-w\right)$ (2-7)

q_tStep：预测的下一时间步长的四元数；q_tMear：根据磁力计和加速度计计算出来的四元数；w：陀螺仪和加速度计的输入权重

2.1.4. 获取预测姿态角

根据公式(2-8)~(2-10)，将预测的四元数转换为预测的姿态角，作为下一时刻固定翼无人机的姿态角。

$\phi =\arctan \left(\frac{\left(qw*qx+qy*qz\right)*2}{1-2\left(qx*qx+qy*qy\right)}\right)$ (2-8)

$\theta =\arcsin \left(\left(qw*qy-qz*qx\right)*2\right)$ (2-9)

$\psi =\arctan \left(\frac{2*\left(qw*qz+qx*qy\right)}{1-2*\left(qy*qy+qz*qz\right)}\right)$ (2-10)

$\phi$ ：姿态角中的滚转角； $\theta$ ：姿态角中的俯仰角； $\psi$ ：姿态角中的偏航角；q_t：四元数。

2.2. 功角、侧滑角以及飞行速度的预测与补偿

2.2.1. 功角、侧滑角以及飞行速度的预测与补偿设计

功角、侧滑角以及飞行速度的预测与补偿设计框图如下图2所示。

Figure 2. Block diagram of power angle, sideslip angle and flight speed

图2. 功角、侧滑角以及飞行速度的设计框图

2.2.2. 功角、侧滑角以及飞行速度的预测

根据预测的姿态角、加速度传感器输出的加速的以及当下计算出来的体轴速度(初始化为0)，预测下一时刻的体轴坐标系三轴速度，如公式(2-11)：

${\left[\begin{array}{c}{u}_{b_{i+1}}\\ v_{b_{i+1}}\\ w_{b_{i+1}}\end{array}\right]}_p=\left[\begin{array}{c}{u}_{b_i}+{\delta }_{t}g\left(\sin \phi +a_x\right)\\ v_{b_i}+{\delta }_{t}g\left(\cos \phi \sin \theta -a_y\right)\\ w_{b{i}_{}}+{\delta }_{t}g\left(\cos \phi \cos \theta -a_z\right)\end{array}\right]$ (2-11)

$u_{b_{i+1}}$ ：预测的当前时刻的体轴坐标系x轴速度； $v_{b_{i+1}}$ ：预测的当前时刻的体轴坐标系y轴速度； $w_{b_{i+1}}$ ：预测的当前时刻的体轴坐标系z轴速度； $u_{b_i}$ ：上一时刻的体轴坐标系x轴速度； $v_{b_i}$ ：上一时刻的体轴坐标系y轴速度； $w_{bi}$ ：上一时刻的体轴坐标系y轴速度； $\phi$ ：当前时刻姿态角中的滚转角(依据姿态角的补偿后得出的)； $\theta$ ：当前时刻姿态角中的俯仰角(依据姿态角的补偿后得出的)； $\psi$ ：当前时刻姿态角中的偏航角(依据姿态角的补偿后得出的)； ${\delta }_t$ ：积分时间； $a_x$ 为加速度传感器体轴坐标系下x方向的加速度分量； $a_y$ 为加速度传感器体轴坐标系下y方向的加速度分量； $a_z$ 为加速度传感器体轴坐标系下z方向的加速度分量。

根据公式(2-12)可以将三轴速度转换为飞行参数，从而获取预测的功角、侧滑角以及飞行速度。

$\begin{array}{l}{v}_t=\sqrt{u_b^2+v_b^2+w_b^2}\\ \alpha =a\tan \left(w_b/u_b\right)\\ \beta =a\sin \left(v_b/v_t\right)\end{array}$ (2-12)

$u_b$ ：无人机体轴坐标系x轴速度； $v_b$ ：无人机体轴坐标系y轴速度， $w_b$ ：无人机体轴坐标系z轴速度； $V_t$ ：无人机飞行速度； $\alpha$ ：无人机功角； $\beta$ ：无人机侧滑角。

2.2.3. 功角、侧滑角以及飞行速度的补偿

根据公式(2-13)，采用获取的GPS数据中的经度、维度、高度，计算当前固定翼无人机的地轴坐标系三轴速度分量。

$\begin{array}{l}{V}_x=k*\left(la{t}_{i+1}-la{t}_i\right)/dt\\ V_y=k*\left(\ln g_{i+1}-\ln g_i\right)/dt\\ V_z=k*\left(al{t}_{i+1}-al{t}_i\right)/dt\end{array}$ (2-13)

$V_x$ ：飞行设备地轴坐标系x轴速度； $V_y$ ：飞行设备地轴坐标系y轴速度； $V_z$ ：飞行设备地轴坐标系z轴速度； $k$ ：GPS距离换算为实际距离系数； $la{t}_{i+1}$ ：当前时刻的维度； $la{t}_i$ ：上一时刻的维度； $\ln g_{i+1}$ ：当前时刻的维度； $\ln g_i$ ：上一时刻的维度； $al{t}_{i+1}$ ：当前时刻的高度； $al{t}_i$ ：上一时刻的高度； $dt$ ：间隔时间。

根据公式(2-14)，采用姿态角将飞行设备的地轴坐标系的三轴速度转换为体轴坐标系的三轴速度：

${\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]}_c={\left[\begin{array}{ccc}\cos \theta \cos \psi & -\cos \phi \sin \psi +\sin \phi \sin \theta \cos \psi & \sin \phi \sin \psi +\cos \phi \sin \theta \cos \psi \\ \cos \theta \sin \psi & \cos \phi \cos \psi +\sin \phi \sin \theta \sin \psi & -\sin \phi \cos \psi +\cos \phi \sin \theta \sin \psi \\ -\sin \theta & \sin \phi \cos \theta & \cos \phi \cos \theta \end{array}\right]}^{-1}\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$ (2-14)

$u_b$ ：无人机体轴坐标系x轴速度； $v_b$ ：无人机体轴坐标系y轴补偿速度； $w_b$ ：无人机体轴坐标系z轴速度； $\phi$ ：当前时刻姿态角中的滚转角(依据姿态角的补偿后得出的)； $\theta$ ：当前时刻姿态角中的俯仰角(依据姿态角的补偿后得出的)； $\psi$ ：当前时刻姿态角中的偏航角(依据姿态角的补偿后得出的)； $V_x$ ：无人机地轴坐标系x轴速度； $V_y$ ：无人机地轴坐标系y轴速度 $V_z$ ：无人机地轴坐标系z轴速度。

根据公式(2-15)，采用GPS计算出来的体轴速度补偿预测的体轴速度，最终完成飞行参数中功角、侧滑角以及飞行速度的预测与补偿。

$\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]=\left(1-w\right){\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]}_p+w{\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]}_c$ (2-15)

${\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]}_c$ ：由GPS计算的补偿的体轴速度； ${\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]}_p$ ：预测的体轴速度； $\left[\begin{array}{c}{u}_b\\ v_b\\ w_b\end{array}\right]$ ：最终无人机的体轴速度； $w$ ：权值。

3. 软件实现

本文采用STM32F407作为飞行控制器，采用keil作为软件编译环境，使用C和C++语言完成可飞行参数估计模型的实现。

软件实现流程图如图3所示。

Figure 3. Software implementation flowchart

图3. 软件实现流程图

无人机姿态角预测与补偿模块：时间步长设置为10 ms，每10毫秒对姿态角进行一次预测与补偿从而减少飞行过程中的干扰。

无人机功角、侧滑角以及速度预测模块：每20 ms进行一次体轴速度的更新，能够快速响应飞行过程中的参数变化。

无人机功角、侧滑角以及速度预测补偿模块：每250 ms进行一次无人机功角、侧滑角以及速度预测的补偿，削减飞行预测的累积误差。在保证飞行参数的响应性良好的基础上满足稳定性。

LoRa数据上传模块：采用LoRa的WIFI实时传输飞行参数，记录飞行过程中固定翼无人机的实时状态用于实验分析。

4. 实验结果分析

使用LoRa进行无线通信，实时上传无人机的姿态角、侧滑角、功角和速度用来验证模型的稳定性。

Figure 4. Attitude angle data recorded by LoRa

图4. LoRa记录的姿态角数据

如图4所示，横坐标为时间，纵坐标为姿态角的角度，当姿态角产生扰动时，模型可以快速响应扰动，对扰动进行抑制，使姿态角快速恢复的效果。

Figure 5. LoRa records power angle, sideslip angle, and flight speed data

图5. LoRa记录的功角、侧滑角以及飞行速度数据

如图5所示，当飞行产生扰动时，模型可以快速响应扰动，对扰动进行抑制，使飞行参数快速恢复的效果。

5. 总结

根据实验数据的分析，本文设计的固定翼无人机对姿态角和飞行参数的补偿算法，在本次实验中，当飞行参数出现干扰未稳定时，软件会通过该算法进行快速响应，及时进行飞行调整，保证实验中固定翼无人机的稳定性和抗干扰性。