基于信息融合四旋翼飞行器稳定性改进设计

赵世荣，孙运强，石喜玲

（中北大学（山西）仪器与电子学院，仪器科学与动态测试教育部重点实验室，太原 030051）

近年来由于新材料（碳纤维等）、新型微机电（MEMS）传感器、微型惯性导航控制器（MIMU）以及计算机自动控制算法的不断发展和进步，以四旋翼飞行器为代表的多旋翼飞行器得以快速发展。当前世界上四旋翼飞行器的研究热点是飞行器的自主飞行器及执行任务，这对飞行器的稳定起降及悬停提出很高要求。

多传感器信息融合是充分利用不同时间与空间的多传感器数据资源，采用计算机技术对按时间序列获得的多传感器观测数据，在一定准则下进行分析、综合、支配和使用，获得对被测对象的一致性解释与描述，进而实现相应的决策和估计，使系统获得比它的各组成部分更充分的信息［1］。由于四旋翼飞行器采用的MEMS传感器的精度、漂移及环境等的影响，单个传感器测得的值可靠性差，影响飞行器的稳定性起降及悬停，所以需要飞行器多传感器采集到的数据的实时融合［2-3］。

无迹卡尔曼滤波简写为UKF（Unscented Kalman Filter）是一种新型的滤波估计算法。UKF以UT变换为基础，摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹（UT）变换来处理均值和协方差的非线性传递，就成为UKF算法。UKF是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要求导计算Jacobian矩阵。UKF没有线性化忽略高阶项，因此非线性分布统计量的计算精度较高。基于上述优点，UKF被广泛应用于导航、目标跟踪、信号处理和神经网络学习等多个领域［4］。

将三维空间中的目标和场景对应于二维图像平面运动时，他们在二维图像平面的投影就形成了运动，这种运动以图像平面亮度模式表现出来的流动就称为光流。光流法是对运动序列图像进行分析的一个重要方法，光流不仅包含图像中目标的运动信息，而且包含了三维物理结构的丰富信息，因此可用来确定目标的运动情况以及反映图像其他等信息。目前，已经研制出了光流传感器，具有高感光度、高像素、高数据更新速度，并且能够通过地面站软件实时获取数据等优点，在低空下可以通过它较准确的测得飞行器相对地面的运动速度。

图1 四旋翼飞行器的动力学模型

1 四旋翼飞行器的力学模型及其简化

四旋翼飞行器的空间自由度为6（沿3个坐标轴作平移和旋转动作），这6个自由度的控制是通过调节不同电机的转速进而改变4个旋翼的升力而实现的，其基本的运动状态包括垂直运动、前后运动、侧向运动、滚转运动、俯仰运动、偏航运动。

其动力学公式为如下：

其中，U1为垂直速度控制量，U2为翻滚输入控制量，U3为俯仰控制量，U4为偏航控制量。

2 多传感器的布置及其简化模型

光流技术是一种广泛用于视觉处理而不需要精确地跟踪特征点就能实现运动检测的仿生技术。这里选用了PX4FLOW光流传感器，PX4Flow是一款智能光学流动传感器。传感器拥有原生752×480像素分辨率，计算光学流的过程中采用了4倍分级和剪裁算法，计算速度达到250 Hz（白天，室外），具备非常高的感光度［5］。与其他滑鼠传感器不同，它可以以120 Hz（黑暗，室内）的计算速度在室内或者室外暗光环境下工作，而无需照明LED。也可以对它重新编程，用于执行其他基础的，高效率的低等级机器视觉任务。这里将它安装在四旋翼飞行器的下方，用以检测飞行器相对地面的运动状况。为了把传感器输出值转换成实际运动的距离，我们需要考虑高度。从下面的两张图中可以看到，如果两个四轴运动了同样距离，但是一个低，一个高，低的一个会看到表面特征运动得更远，所以光流数值会更大，测得更远，所以光流数值会更大。如图2所示。

图2 光流传感器高度不同

飞行器的侧倾和俯仰的变化也将导致传感器返回的X和Y值的变化。与横向运动的计算不同，这些都不依赖于可见物体的距离。在下面的图片中，可以看到的四轴已经侧倾了10°，但第1张图中视角中心的两个花都移到了第2张图片的视角边缘。如图3所示。

图3 光流传感器倾斜

传感器值预期的变化可以根据下面的公式用侧倾和俯仰的变化算出。我们把这些预期的变化从传感器返回的实际值中减去。

我们得到的x，y轴的运动后，就可以与当前的偏航相结合结合，用于估计位置。

需要注意的是，传感器只能在光线好的环境下工作；因为使用定焦镜头，所以不能对近于30 cm的物体对焦，同时旋转传感器将使传感器混乱［6］。

超声波发送端发射超声波，当超声波遇到物体后被反射回来，接收传感器接收超声波。渡越时间法就是通过测量超声波从发射到接收的时间差t，从而求出待测距离L，计算公式为：

式中，L为待测距离，v为声速，t是测得的时间。

飞行器的高度由气压计和超声波进行检测。MS5611-01BA气压传感器是由MEAS（瑞士）推出的一款SPI和I2C总线接口的新一代高分辨率气压传感器，分辨率可达到10 cm。该传感器模块包括1个高线性度的压力传感器和一个超低功耗的24 bit Σ模数转换器。MS5611提供了一个精确的24 bit数字压力值和温度值以及不同的操作模式，可以提高转换速度并优化电流消耗。通过搭建测试平台可得到它的静态测量波动在±0.1 m以内，动态测量误差为大约0.2 m～0.3 m，可以满足飞行器在高空中飞行的需求，但稳定起降及低空飞行精度相对差不满足要求。超声波是一种频率在40 kHz左右的声波，其在大气中的传播速度与大气温度、湿度、压力有密切的关系。从超声波测距原理可知，超声波传播速度的准确性直接影响到超声波测距模块的距离测量精度，因此对超声波测距模块进行温度补偿、湿度补偿以及压力补偿，可以有效地提高超声波测距模块测距精度。利用超声波传感器搭建的测量高度系统在10 m内的测量偏差在0.01 m内，精度远高于气压计。

3 基于卡尔曼滤波的信息融合

3.1 基于卡尔曼的飞行器高度

图4、图5分别为飞行器在悬停和运动时超声波和气压计测量值的滤波效果；可以看到，由于气压计的测量偏差相对超声波较大，经过卡尔曼滤波处理后偏离理想值仍然相对较大；而超声波测量值经过滤波处理后与理想值很接近，误差很小而且不随时间推移明显增大，实时性很高。

图4 稳定悬停时滤波效果

图5 高度变换时滤波效果

3.2 基于卡尔曼的飞行器速度

在低空且光线满足的情况下，利用光流法得到的运动位移偏移在0.06 m内，运动速度偏移在0.03 m/s内。图6为X，Y轴光流传感器测得位移的滤波效果，可以看出滤波后的位移和理想值的接近程度很好，误差较小同时没有明显累积，相比加速度计测量效果好；图7为X，Y轴的速度的滤波效果。

图6 X，Y轴速度滤波效果

图7 X，Y运动加速度滤波效果

3.3 基于无迹卡尔曼的姿态解算

设飞行器的初始姿态角分别为1°，2°，3°，陀螺仪的白噪声为0.5°/h，陀螺仪的输出采样频率为100 Hz，初始姿态角加速度都为1°，通过UKF滤波仿真，效果如图8所示。

通过模拟仿真得到，经过融合滤波后的姿态角与理想值比较接近，偏差在0.2°以内，满足飞行器的稳定性要求。

图8 三轴姿态角UKF滤波效果

4 研究结果

本次实验搭建了一个基于STM32F427的四旋翼飞行器，通过加入上述改进算法实现了平稳性飞行。

但是实际进行悬停及起降时发现它的稳定性没有达到理想效果，考虑可能问题是多传感器信息融合在硬件平台实际应用中没有达到应有效果；同时控制算法的不完善也影响飞行器的稳定性。因此，改进信息融合算法在硬件平台上的运行效率及控制算法是下一步的重点工作。

［1］Di L，Fromm T，Chen Y Q，et al.A Data Fusion System for Attitude Estimation of Low-Cost Miniature UAVs［J］.Journal of Intelligent and Robotic Systems，201265（1-4）：621-635.

［2］Schwarz S，Sjostrom M，Olsson R，et al.A Weighted Optimization Approach to Time-of-Flight Sensor Fusion［J］.IEEE Transactions on Image Processing，2014，23（1）：214-225.

［3］Xi C.The Application of Measuring Robot Based on the Measure⁃ment Dynamic Measurement Technology at High-Pressure Line Pa⁃trol.2012，IEEE：492-495.

［4］郝晓静，李国新，李明珠，等.无迹卡尔曼滤波算法在目标跟踪中的研究［J］.电子设计工程，2012（13）：161-164.

［5］Baker S，Szeliski R，Scharstein D，et al，A Database and Evalua⁃tion Methodology for Optical Flow［J］.International Journal of Computer Vision，2011，92（1）：1-31.

［6］刘小明，陈万春，邢晓岚，等.光流/惯导多传感器信息融合方法［J］.北京航空航天大学学报，2012（5）：620-624.

［7］陈航科，张东升，盛晓超，等.四旋翼飞行器悬停状态姿态控制建模与仿真［J］.计算机仿真，2013（11）：41-45.

［8］吕强，倪佩佩，王国胜，等.基于光流传感器的四旋翼飞行器悬停校正［J］.装甲兵工程学院学报，2014（3）：68-72.

赵世荣（1988-）男，汉族，山西孝义人，硕士研究生，研究方向为多旋翼飞行器设计，550158357@qq.com；

孙运强（1963-），男，汉族，中北大学教授，syq@nuc.edu.cn。