两种四旋翼无人机姿态解算算法仿真分析

李俊杰 张晓龙 王聪 张峻豪

摘  要： 近年来随着微电子技术和控制技术的发展，四旋翼无人机也得到了快速的发展，为了精确的得到四旋翼无人机的姿态角，从而提高无人机控制系統的控制精度，由传感器测到的信息需要进行解算处理，常用的解算算法有互补滤波算法和卡尔曼滤波算法。本文对两种常用的姿态解算算法原理进行推导并进行了仿真实验，实验结果表明两种解算算法均能优化所测得的信息，互补滤波算法可以有效融合陀螺仪和加速度计的特性从而提高滤波性能，在卡尔曼滤波算法中可以自由配置Q和R的值来调整观测信息和预测信息的权重。

关键词： 四旋翼无人机;姿态解算;互补滤波;卡尔曼滤波

中图分类号： TP391    文献标识码： A    DOI：10.3969/j.issn.1003-6970.2020.10.063

本文著录格式：李俊杰，张晓龙，王聪，等. 两种四旋翼无人机姿态解算算法仿真分析[J]. 软件，2020，41（10）：250252

【Abstract】： In recent years， with the development of microelectronics and control technology， quadrotor UAV has also been rapid development. In order to accurately obtain the attitude angle of quadrotor UAV and improve the control accuracy of UAV control system， the information measured by sensors needs to be solved. The commonly used algorithms are complementary filtering algorithm and Kalman filtering algorithm. In this paper， the principle of two commonly used attitude calculation algorithms is derived and simulated. The experimental results show that the two algorithms can optimize the measured information， and the complementary filtering algorithm can effectively integrate the characteristics of gyroscope and accelerometer， so as to improve the filtering performance. In Kalman filter algorithm， the values of Q and R can be freely configured to adjust the observation information and prediction information Weight.

【Key words】： Quadrotor UAV; Attitude solution; Complementary filtering; Kalman filter

0  引言

无人机是一种可以回收利用的可执行各种飞行任务的飞行器。它的主要特点是可以远程遥控、成本较低且有着不错的性能，许多飞行任务可以用无人机去替代人类执行。自第一台无人机问世到现在，无人机技术得到了飞速的发展应用面也越来越广。早期的无人机主要是军工企业研发和使用，但随着微型计算机控制技术和传感技术的发展，无人机技术的日趋成熟，民用和商业使用无人机也得到了很大的发展[1]。

多旋翼无人机是靠控制旋翼是驱动飞机执行各种飞行动作，按旋翼数量又可以分为四旋翼、六旋翼、八旋翼无人机等。不同的旋翼数量影响着无人机执行任务的性能，其中最常见的是四旋翼无人机，作为一种小型无人机不仅结构简单，而且适应性很强，相较于固定翼的无人机它有着许多不可比拟的优势，还可以做到固定翼无人机做不到的动作如悬停、垂直起飞或降落等，被广泛应用于摄影、地形测绘等领域。

随着微机电系统的发展，四旋翼无人机的控制技术也随之发展。为了能得到好的控制效果，在无人机上装有传感器实时监测无人机的姿态数据信息。但是由于传感器自身的问题或机体振动噪声的影响，传感数据会存在不可避免的误差，为了尽可能的接近无人机真实的姿态需要建立相应的算法去对传感器的测量值进行修正。

1  互补滤波算法

1.1  互补滤波原理

在无人机的姿态角测量时用到的MEMS传感器因为无人机电机驱动的振动会使传感信号带有噪声，这些噪声是不利于无人机控制的。所以需要对MEMS测到的信号进行滤波处理，在四旋翼无人机上测量角速度的传感器选用到的是陀螺仪[3]。虽然陀螺仪的动态性能不错，但是它存在零漂和温漂问题，所以在使用陀螺仪的时候会有低频噪声，可以使用高频滤波器对测得的信号去噪。在重力加速度测量选用的是加速度

计传感器，测量地球磁场的磁力使用的是磁力计。加速度传感器和磁力计在动态特性方面较差，但是静态性能很好，它们在测量时存在高频噪声，可以使用低频滤波器去噪。陀螺仪测量公式如下：

式（1）中，表示旋转角，为前一次计算的值，为测得的角速度值，t为两次测量的时间差。

由式（1）可知陀螺仪测量会累积误差，而磁力计和加速度计不会，所以在较短的时间段内可以用陀螺仪测量姿态角。若测量时间跨度较大则需要加速度计和磁力计对其进行修正，确保测量结果尽可能接近真实值。互补滤波算法原理如图1。

为了使结果更加准确，先取加速度计测得的数据的平均值，然后把陀螺仪所测数据和加速度计融合。加速度计算公式为：

由式（5）知系数s和k决定了估计值和真实之间的差值。

1.2  互补滤波算法MATLAB仿真

选用上述传感器测得的数据[3]来验证互补滤波算法并进行MATLAB编程，生成了对比图。

由图2两曲线对比，陀螺仪所测的信号既没有高频误差也没有高频噪声，但陀螺仪相较于互补滤波后的信号存在误差。在20 s之前两曲线没太大差别，但20 s之后开始出现较大偏差且随时间累积。由图3两曲线对比，加速度计曲线十分杂乱波动很大，但和互补滤波曲线走势一样，互补滤波后曲线平缓了许多。

2  卡尔曼滤波算法

卡尔曼滤波算法因为其无偏性，估计方差最小，实时性好等优点广泛应用于各种控制领域，也是四旋翼无人机姿态解算常用的一种算法。

2.1  卡爾曼滤波算法推导

系统观测方程和状态方程如下式：

式中是转移矩阵，前一时刻系统输入为，观测矩阵。系统的观测噪声，系统的过程噪声，其中服从期望为0方差为Q的正态分布服从期望为0方差为R的正态分布。

使用上一时刻系统的状态加输入和当前时刻观测到的值来预估当前的系统状态，这就是卡尔曼滤波算法的原理[7]。

式（15）里面，是用所估算的上一时刻的系统状态和模型来预估系统在k时刻的状态，是k时刻观测量的预估值，它是k时刻的预估值使用观测模型估算出来的。上式的意义为状态的估计值是观测的信息乘上卡尔曼系数再加上对状态的预测值。

在估值的过程中Q和R值是很重要的，Q和R的增大或减小都会对结果造成影响。

2.2  MATLAB仿真分析

需要验证卡尔曼滤波算法在四旋翼无人姿态解算上是否有效，使用到陀螺仪测到的一组数据[3]，在matlab软件中用卡尔曼滤波算法程序。结果如图4。

在解算图4a的基础上增大Q值得到图b，在图a的基础上增大R值得到图c。图a可以看出卡尔曼滤波算法可以很好的优化传感器所测结果，在b图中原始数据线条和卡尔曼滤波后的线条无太大差异，也就是在该算法中观测值占比较大。所以可以知道，在卡尔曼滤波算法中如果需要提高观测信息的占比只需要增大Q值即可，此时还会加快滤波响应速度和收敛速度。相应的，若需要提高预测值的占比则需要增大R值，此时对噪声处理能力也会增强。

3  结论

在无人机姿态解算中常用的互补滤波和卡尔曼滤波方法都能对信号进行处理，两种方法侧重点各不相同。

（1）陀螺仪在前20 s内测得的数据较为准确，   但在20 s后由于累计误差导致数据偏差越来越大，   互补滤波算法能有效得融合陀螺仪和加速度计所测得数据。

（2）加速度计曲线和结算后曲线总体走势相同，但是十分曲折所测数据很难使用，互补滤波算法能很好的去除噪音使曲线图平缓。在使用时，前面短时间段内可以用陀螺仪数据，之后需要两个传感器数据进行互补滤波进行融合。

（3）卡尔曼滤波算法使用的关键是配置好Q值和R值，Q值大的时候滤波效率较高此时观测信息占比较大，R值大的时候对噪声的处理效果好此时预测信息占比较大。

参考文献

[1]常于敏. 无人机技术研究现状及发展趋势[J]. 电子技术与软件工程， 2014（1）： 242-243.

[2]余后明. 四旋翼无人机姿态控制研究[D]. 中北大学， 2019.

[3]何瑜. 四轴飞行器控制系统设计及其姿态解算和控制算法研究[D]. 电子科技大学， 2015.

[4]贾文哲， 王剑平. 两类改进非线性滤波器 UKF 算法综述[J]. 化工自动化及仪 表， 2016， 43（10）： 1011-1014.

[5]卞月根， 张伟. 基于非线性卡尔曼滤波的车辆定位优化算法[J]. 计算机技术与发展， 2015， 25（8）： 80-83+89.

[6]汪大理， 王先春. 四旋翼自主飞行器设计[J]. 电子世界， 2016（23）： 105-106.

[7]王宽田， 党选举， 郑飞宇， 等. 基于卡尔曼滤波四旋翼无人机控制系统设计[J]. 电子世界， 2017（8）： 103-104.

[8]李松柏. 四旋翼飞行器姿态控制模型研究[J]. 信息技术与信息化， 2019（11）： 238-240.

[9]吴杰， 闫建国. 基于修正的卡尔曼滤波的姿态估计算法研究[J]. 计算机仿真， 2012， 29（2）： 54-57.

[10]朱岩， 付巍. 基于无迹卡尔曼滤波的四旋翼无人飞行器姿态估计算法[J]. 测试技术学报， 2014， 28（3）： 194-198.