基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法

段敏 赵凌 周莹

摘  要：为提高四旋翼无人机姿态参数获取的准确性，确保后续姿态控制精度，采用STM32F407微控制器以及多传感器构成姿态测量系统。对各传感器原始误差进行校准，应用扩展卡尔曼滤波（EKF）进行基于陀螺仪的状态预测和基于加速度计/磁力计的测量校正，融合信息并估计出3姿态角，与3自由度姿态算法验证系统测量出的姿态角真实值对比，3个角度的平均误差为0.7°，相对于基于单一陀螺仪积分和基于加速度计/磁力计的姿态解算，误差分别下降了3.034°和0.174°，该方法可有效提高EKF估计精度。

关键词：扩展卡尔曼滤波;四旋翼无人机;姿态估计

中图分类号：TP368;V279            文献标识码：A文章编号：2096-4706（2022）04-0007-05

Attitude Estimation Method for Quad-rotor UAV Based on Extended Kalman Filter

DUAN Min， ZHAO Ling， ZHOU Ying

（The College of Post and Telecommunication of WIT， Wuhan  430073， China）

Abstract： In order to improve the accuracy of attitude parameters acquisition of quad-rotor UAV and ensure the subsequent attitude control accuracy， STM32F407 microcontroller and multi-sensor are used to form an attitude measurement system. The original error of each sensor is calibrated. The extended Kalman filter （EKF） is used for gyro-based state prediction and accelerometer/magnetometer-based measurement correction. The information is fused and the three attitude angles are estimated. Compared with the real value of the attitude angle measured by the 3-DOF attitude algorithm verification system， the average error of the three angles is 0.7°， compared with the attitude solution based on single gyro integral and the attitude solution based on accelerometer/magnetometer， the errors are reduced by 3.034° and 0.174° respectively. This method can effectively improve the accuracy of EKF estimation.

Keywords： extended Kalman filter; quad-rotor UAV; attitude estimation

0  引  言

四旋翼無人机因其成本低廉、维护操作简单、可替代人力完成特殊任务等优点，近年来被广泛应用于军用和民用领域[1，2]。在四旋翼无人机飞控中，获取稳定可靠、高精度的姿态信息是首要目标和任务，决定着后续姿态控制的精度，在四旋翼无人机的相关研究中至关重要。

无人机依靠姿态测量系统获取实时姿态。四旋翼无人机应其体积、成本和载荷的限制，通常使用微型化、高集成化的微惯性测量单元（MEMS-IMU）来获取姿态，IMU不受外界环境影响，可随时随地为无人机提供状态信息。但是IMU中的陀螺仪存在时间积分误差，长期稳定性差的缺点;IMU中的加速度计受运动加速度影响，动态精度低[3]。一些姿态测量系统里会加入磁力计辅助测量，但磁力计也会受磁场影响，不能完全信赖[4]。最终，需要算法融合不同传感器的数据，并估计出最佳的姿态角。常用于数据融合、姿态估计的滤波算法有：互补滤波[5]、梯度下降法[6]、卡尔曼滤波（KF）[7]等，文献[8]简单对比了这几种算法，互补滤波和梯度下降法算法简单，适合用于处理性能受限的飞行器，在硬件性能满足的条件下，大多选用KF[9];KF效果优越、应用广泛，但不适用于包括无人机飞控系统在内的非线性系统[10]。针对此局限，有许多基于KF的扩展算法，如无迹卡尔曼滤波（UKF）[11]、粒子滤波（PF）[12]、扩展卡尔曼滤波（EKF）[13]，前两者都有计算量大、实时性差的问题，而EKF数据存储量小、计算量相对较小，综上考虑，EKF是小成本四旋翼无人机数据融合、姿态估计的最佳选择[14]。文献[15]采用了EKF算法直接融合MPU9250中陀螺仪、加速度计、磁力计9项原始数据，通过实验证明EKF估计精度确实优于互补滤波和梯度下降法。传感器由于自身制作工艺缺陷以及环境影响，原始数据存在一定误差，直接融合对EKF估计精度有影响。本文在文献[15]的基础上，对各传感器原始数据误差进行了初步校准，提高EKF输入信息的精度，保证融合的有效性。

采用基于ARM Cortex-M4内核的STM32F407作为核心微控制器，3轴加速度计、3轴陀螺仪和3轴磁力计构成姿态测量系统，其中陀螺仪/加速度计采用芯片MPU6050，磁力计采用HMC5883L。对各传感器原始误差进行初步校准后采用EKF融合，利用角速度信息进行基于模型的状态预测，利用加速度和磁强度信息进行基于观测量的量测修正，估计出无人机的3个姿态角供后续姿态控制使用，结构图如图1所示。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB

1  姿态估计数学模型

1.1  相关坐标系

在研究四旋翼无人机的飞行状态时，主要用到导航坐标系（n系）和机体坐标系（b系）。n系是求解导航参数时的参考坐标系，选用地理的北东地（NED）方向作为n系;b系与飞机固连，随机体运动而运动，x轴在飞行器对称平面内，沿飞行器设计轴线指向机首，y轴垂直于对称平面，指向机身右弦，z轴在对称平面内垂直于另外两轴，指向机身下方。n系和b系示意图如图2所示。

1.2  姿态表示

通常用欧拉角、旋转变换矩阵、四元数3种方式来描述无人机的姿态。基于欧拉角的姿态解算存在万向节死锁问题;旋转变换矩阵有9个向量，计算量大。因此，本文选择用四元数Q（q0，q1，q2，q3）来描述姿态，用它表示姿态角（俯仰角θ，偏航角ψ，横滚角?）如式（1）所示，表示从b系到n系的旋转变换矩阵如式（2）：

2  传感器原始数据误差校准

考虑陀螺仪、加速度计、磁力计都存在的3项确定性误差：零偏误差、刻度因子误差、安装误差，建立如式（3）所示的误差校准模型，当式中x为a，ω，m时，式（3）分别为加速度计、陀螺仪、磁力计的误差模型。

x是校准后的准确数据，x′是原始数据，bx是零偏误差，x′-bx补偿零偏，Sx补偿刻度因子误差，Rx补偿安装误差。求出3维向量bx及3维矩阵SxRx即完成了校准。对加速度计使用6面校准法，依次使加速度计±X，±Y，±Z轴指向地向，理论上，只有沿着地向的轴输出为g，其他两轴输出均为0，记录实际输出代入式（3）可求得校准参数。对陀螺仪使用3轴转台法，将陀螺仪固定在高精度转台，以确定角速率旋转，测量实际输出，代入式（3）即可求出参数。对磁力计使用简单标定法，各轴向零偏等于该轴最大最小输出的平均数，SxRx为各轴向最大最小输出差值的比值。使用上述方法，本文测得各传感器校准参数为：

3  基于EKF的姿态估计方法

3.1  确定EKF滤波器状态量和观测量

姿态四元数和3轴陀螺仪输出组成7维状态量，3轴加速度计、3轴磁力计、3轴陀螺仪输出以及四元数组成13维观测量，分别如式（4）和式（5）所示（上标b表示是在b系下的输出值）：

3.2  确定初始状态值和初始误差协方差

利用加速度计在静态时的输出可以计算初始俯仰角和初始横滚角，如式（6）：

利用磁力计輸出可以求初始偏航角，先左乘得到n下的输出mn，再计算偏航角：

将初始姿态角转换成四元数，加上初始陀螺仪输出，就完成了初始状态的求解。

初始误差协方差（PQ和Pω可调）：P（0）=diag{PQ，PQ，PQ，PQ，PQ，Pω，Pω，Pω}

3.3  滤波迭代过程

3.3.1  基于陀螺仪的状态预测

在每个时钟采样周期T里，已知k-1时刻的四元数Qk-1，利用陀螺仪输出ΩωQk-1可以对k时刻的四元数进行预测，如式（8）所示：

Q（k）=Q（k-1）+T/2Ωω（k-1）Q（k-1）（8）

其中：

由式（8）可推出状态预测方程：

式中W是系统噪声。这种预测的误差协方差P（k）也可由上一时刻的值推算得到：

P（k）- =F*P（k-1）*FT+Q    （10）

其中Q是系统噪声的协方差矩阵。F是从 k-1时刻到  k时刻的状态转移矩阵，通过求偏导的雅克比矩阵获得：

3.3.2  基于加速度计/磁力计的量测修正

单纯依靠陀螺仪更新状态值，长期结果可能会发散，通常需要加速度计和磁力计数据作为观测量，对（1）中预测值进行修正。

在理想状态下，飞行器能够达到和n系XOY面水平的状态，此时的加速度计输出在归一化后为（0，0，1），则在b系下的加速度计理想输出为：

在理想状态下，水平面内磁场强度应该相等，此时磁力计输出应为其中，，则在b系下的磁力计理想输出值：

依据式（12）和式（13）可以建立量测方程：

当k时刻传感器真实测量值Z（k）到达时，计算新息和卡尔曼增益如式（15）和式（16）所示：

式（16）中R是量测噪声协方差矩阵，H是量测矩阵，通过求偏导的雅克比矩阵获得：

利用新息和卡尔曼增益，对状态预测值和误差协方差预测值进行修正：

不断重复3.3.1～3.3.2小节内容，形成迭代，完成每一个时刻的状态预测和校正。

4  实验结果与分析

4.1  传感器数据采集实验

为了验证2节中误差补偿的有效性，设计实验采取传感器原始数据，使校准后的陀螺仪、加速度计、磁力计依次绕X轴、Y轴、Z轴从0°旋转至±90°，采样频率为500 Hz，记录并保存数据文件，在MATLAB上显示波形，如图3所示。

理论上，陀螺仪在绕某一轴转动时，另两轴应输出为0;加速度计在旋转过程中只有指向地向的轴输出为1，另两轴输出为0;磁力计的3轴在转到相同位置时有相同的输出，从输出波形来看，传感器数据准确，误差补偿有效。

4.2  姿态角解算实验

在Keil5的软件编译环境下调试代码，加载程序至飞控板。采用3自由度姿态算法验证系统G-TZ-4002测量真实姿态角，将加载有EKF姿态解算程序的飞控板置于验证平台上，平台和飞控板分别通过总线和无线数传将数据发送到地面站，地面站显示姿态角随时间变化的曲线。

验证平台传输波特率为115200Baud，解算频率为200 Hz，IMU更新频率为1 000 Hz，磁力计更新频率为200 Hz。经过调试后的Ra=0.2、Rω=0.1、Rm=20。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB

俯仰角、横滚角、偏航角随时间变化的部分曲线图分别如图4、图5、图6所示。

由图4可知，静态时，EKF估计的俯仰角保持在-12°，几乎呈直线，稳定性优秀;动态时，EKF估计误差保持在1°以内，总体精度优异。图5静态时，EKF估计的横滚角保持在26°，非常稳定;动态时，最大误差2.5°。由图6可知，静态时，偏航角估计值维持在-1°，无明显波动，稳定性非常好;动态时，误差保持在2°以内。总体来说EKF估计的3个姿态角曲线都非常平滑，代表EKF姿态估计具有优异的稳定性和准确度。

将本文中EKF估计的姿态角均方误差（MSE）与基于单一陀螺仪积分以及基于加速度计/磁力计解算的进行对比，列出表1。从表中可以看出，基于单一陀螺仪积分的解算误差较为明显，最大接近10°。基于加速度计/磁力计的解算误差相对较小，但是其数据是在静态条件下获得，如果是动态，误差会更大。基于EKF估计的姿态角误差明显小于前面两者，与基于陀螺仪的解算相比，3个角度的均方误差平均下降了3.034°;与基于加速度计/磁力计的解算相比，下降了0.174°。上述数据证明基于EKF的姿态估计确实提高了姿态数据的精度，效果优异。另外，加入了文献[15]中的EKF估计结果进行对比，本文的基于传感器校准的EKF估计误差下降了0.619°，证明本文中对传感器的校准提高了EKF输入数据精度，保证了估计的有效性和精度。

5  结  论

本文首先对各姿态传感器原始数据进行了校准，然后使用扩展卡尔曼滤波（EKF）融合了校准后的数据并估计出姿态角。将估计出的姿态角与真实值对比，发现误差能控制在2.1°以内（3角度平均值），与基于单一传感器（陀螺仪、加速度计/磁力计）的姿态解算对比，误差分别下降了3.034°和0.174°，说明本文的EKF融合效果较好。与文献[15]中的EKF对比，本文的估计误差下降了0.619°，证明本文中对传感器的校准有效，基于传感器校准的EKF姿态估计能提高精度，效果优异。

參考文献：

[1] 陈彦强，张淑瑞，张永富.军用无人机发展现状和趋势 [C]//中国国际无人驾驶航空器系统大会.北京：出版社不详，2016：1-5.

[2] 黄爱凤，邓克绪.民用无人机发展现状及关键技术 [C]//航空航天科技创新与长三角经济转型发展分论坛，南京江苏省航空航天学会，2012：29-35.

[3] 秦永元.惯性导航 [M].北京：科学出版社，2006.

[4] 潘佳虹.四旋翼无人机的姿态估计与控制研究 [D].杭州：杭州电子科技大学，2016.

[5] 万晓凤，康利平，余运俊，等.互补滤波算法在四旋翼飞行器姿态解算中的应用 [J].测控技术，2015，34（2）：8-11.

[6] 刘青文，郭剑东，浦黄忠，等.基于梯度下降法的四旋翼无人机姿态估计系统 [J].电光与控制，2018，25（5）：17-21.

[7] 杭成，朱海霞，许毅立，等.卡尔曼滤波在四旋翼飞行器姿态解算中的应用 [J].江苏科技信息，2016（17）：67-68.

[8] 王晓初，卢琛.四旋翼姿态解算算法的对比与研究 [J].制造业自动化，2015，37（2）：120-122+138.

[9] 林庆峰，谌利，奚海蛟.多旋翼无人飞行器嵌入式飞控开发指南 [M].北京：清华大学出版社，2017.

[10] 秦永元.卡尔曼滤波与组合导航原理 [M].西安：西北工业大学出版社，2012.

[11] JULIER S J，UHLMAN J K，DURRANT H F. A new Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators [J].IEEE Transactions on Automatic Control，2000，45 （3）：477-482.

[12] 刘胜，张红梅.最优估计理论 [M].北京：科学出版社，2011：129-130.

[13] 杨兆，沈作军.基于扩展卡尔曼滤波的小型固定翼无人机姿态估计方法分析 [J].航空科学技术，2017，28（11）：15-21.

[14] 杨越.基于捷联惯导系统的小型无人机姿态解算及控制算法研究 [D].西安：西安电子科技大学，2017.

[15] 石川，林达，张果，等.基于QEKF的四旋翼飞行器姿态估计 [J].现代雷达，2018，40（11）：49-52+56.

作者简介：段敏（1994.08—），女，汉族，湖北黄冈人，助教，硕士，主要研究方向：信号处理与智能控制。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB