蔡阳 胡杰

摘要：AHRS航姿参考系统中通常需要融合MEMS传感器数据来进行姿态解算，由于MEMS传感器自身的一些缺陷导致在姿态解算中会出现较为严重的误差。AHRS中常见对加速度计、陀螺仪和磁力计进行卡尔曼滤波、互补滤波的方法，由于使用单一的滤波算法时会出现误差，导致姿态角解算精度不高。本文采用卡尔曼滤波融合互补滤波的滤波算法，通过卡尔曼滤波对加速度计和陀螺仪起抑制漂移作用，进而得到最优估计姿态角，减小传感器引起的误差，再由估计值和磁力计经过互补滤波滤除噪声，提高姿态角的解算精度。仿真实验表明：融合滤波算法可以抑制漂移和滤除噪声，在静态和动态条件下，都有良好表现。

关键词：AHRS;MEMS;姿态解算;卡尔曼滤波;互补滤波

中国分类号：TP301  文献标识码：A

文章编号：1009-3044（2021）10-0230-03

Abstract： AHRS heading and attitude reference system usually needs to fuse MEMS sensor data for attitude calculation. Due to some defects of MEMS sensor itself， there will be more serious errors in attitude calculation. Kalman filtering and complementary filtering methods for accelerometers， gyroscopes， and magnetometers are common in AHRS. Due to errors when a single filtering algorithm is used， the accuracy of the attitude angle calculation is not high. In this paper， the Kalman filter fusion complementary filter filter algorithm is used to suppress drift of the accelerometer and gyroscope through Kalman filter， and then obtain the optimal estimated attitude angle， reduce the error caused by the sensor， and then pass the estimated value and the magnetometer. Complementary filtering filters out noise and improves the accuracy of attitude angle calculation. Simulation experiments show that the fusion filtering algorithm can suppress drift and filter noise， and it performs well under static and dynamic conditions.

Keywords： AHRS; MEMS;attitude calculation; Kalman filter;complementary filter

航姿參考系统AHRS（Attitude and Heading Reference System）由MEMS（Micro-Electro Mechanical System）惯性传感器三轴陀螺仪、三轴加速度计和磁力计的数据融合来进行姿态解算[1]。在进行姿态解算过程中，由于MEMS器件固有的一些缺陷，陀螺仪测量角度时使用积分，会存在积分误差，而且当陀螺仪静止不动时，也会产生漂移，加速度计会受到重力和振动的影响，磁力计会受到地理环境和自身环境干扰，导致进行姿态角解算时精度较低。

为了能够准确测量姿态角，提高精度，通常会将不同的传感器进行数据融合，并采用卡尔曼滤波算法和互补滤波算法以及以此为基础的衍生算法。文献[2]提出基于卡尔曼滤波的进行融合算法解算姿态角，采用时间序列建模，根据量测方程和状态方程对数据进行滤波处理，可是误差会在量测方程线性是引入;文献[3]提出基于互补滤波对加速度计和陀螺仪进行滤波处理，并且采用量子粒子滤波为滤波算法作参数寻优;文献[4]提出广义互补滤波在AHRS中的姿态估计，通过分析乘性误差，将加入运动加速度的补偿，来提高姿态角解算精度;文献[5]中提出改进型显性互补滤波，采用巴特沃斯低通滤波和设置缓存区，进而减少加速度计姿态角解算的影响。

本文采用两种滤波方式分别为卡尔曼滤波、互补滤波，结合两者特性将其融合。先通过卡尔曼滤波抑制原始数据中的漂移等因素，对陀螺仪，加速计的输出数据进行融合，从而得到的姿态角，作为系统状态的最优估算。再利用互补滤波特性，将kalman滤波得到的姿态角与磁力计数据进行融合，并做姿态角的解算，能够提高姿态角解算精度。

AHRS结合MEMS传感器各自优点，通过融合滤波算法，从而能够得出精度较高的航姿姿态角。

1 互补滤波

1.1 互补滤波原理

针对MEMS惯性传感器件中，加速计由于受重力影响和瞬时误差加大，但是具有良好的低频特性，而陀螺仪因计算角度过程中，不断积分或积累误差，然而有良好的高频特性。

互补滤波针对加速计低频特性，滤去高频信号和陀螺仪高频特性，滤除低频信号，通过陀螺仪的数据进行积分得出姿态角的最优估计值，再对加速计数据得出姿态来进行校正姿态角[6]。将陀螺仪、加速计的原始数据融合并结合磁力计输出，从而在高、低频段都能够得到准确的姿态角，互补滤波工作原理如图1：

1.2 四元数

四元数基本表示形式为：

当姿态角发生变化时，相当于载体坐标系分别绕地理坐标系X，Y，Z进行旋转，得到

四元数的载体坐标系b旋转到地理坐标系n的矩阵为[Cnb]为：

由公式（1）（2）（3）可以得出，欧拉角与四元数之间的关系为：

2 卡尔曼滤波

在AHRS系统中通常采用卡尔曼滤波，对原始传感器数据进行建模，通过过程的预测更新，进而对下一个时刻的姿态角进行预测[9]。

状态方程（5）和测量方程（6）的建立，其中：

状态方程：

本文中以陀螺儀的角速度ω和静态漂移?作为输入量，来建立量测方程：

公式中：[θk]为k时刻传感器角度、ωk-1为k-1时刻的陀螺输出角速度、?k-1为k-1时陀螺仪静态的漂移、[Wk-1]为量测的噪声，[dt]是采样的时间[8]。

公式中：ω为陀螺仪输出;T为采样时间;[Zk]为加速计量测角度;[Vk]为系统测量的噪声。

3 卡尔曼滤波融合互补滤波

卡尔曼滤波通过线性状态方程和递归运算，结合输入输出观测数据，估算最优角度，可以解决单一使用互补滤波时，传感器带的漂移问题。互补滤波由于运算量不大，并且相对简单，可以过滤噪声，通过卡尔曼滤波预估的角度值，结合磁力计和四元数，可得出准确的姿态角[10-11]。设计图如图2所示.

4 仿真实验与分析

本实验采用MPU9255MEMS九轴传感器原始数据，对融合滤波进行验证，将传感器采集的数据加速计和陀螺数据经过卡尔曼滤波后，传递给互补滤波器结合磁力计输出姿态角。

在仿真软件MATLAB中，对融合算法进行编程设计，并对MEMS传感器数据进行融合滤波分析对比滤波效果。

实验结果图3，图4中的输出表明，单一的卡尔曼滤波或者单一的互补滤波在输出姿态角时仍存在不稳定的因素，得到的角度存在波动。相比较而言融合滤波输出姿态角精度更高，在图形中波形更为平稳。

在动态测量过程中，融合滤波输出姿态角波动较小，图形收敛速度较快;在静态滤波处理过程中，融合滤波能有效抑制漂移，过滤噪声。相比单一滤波算法有一定的提升，同样能够提高姿态角的测量精度，这对于AHRS系统的稳定性，也有相应的提升。

5 结语

本文采用对AHRS中的MEMS器件进行卡尔曼滤波与互补滤波的融合滤波，通过卡尔曼滤波抑制漂移和对互补滤波过滤噪声进行融合，可以提高姿态角解算精度，从而为AHRS系统姿态解算提升了性能。

参考文献：

[1] 刘浩，杨薇秀，焦胜海，等.基于AHRS算法的小型无人机导航精度改进方法[J].中国电子科学研究院学报，2020，15（5）：461-469.

[2] 刘浩宇，李奇.卡尔曼滤波算法在AHRS姿态角解算中的应用[J].工业控制计算机，2018，31（6）：69-71.

[3] 史露强，何怡刚，罗旗舞，等.基于传感器数据融合的倾斜角度测量方法研究[J].仪器仪表学报，2017，38（7）：1683-1689.

[4] 王勇军，李智，李翔.小型无人飞行器机动过程中航姿互补滤波算法研究[J].电子测量与仪器学报，2020，34（7）：141-150.

[5] 付雷，章政，余义.基于改进型显性互补滤波的MEMS姿态解算[J].自动化与仪表，2018，33（11）：7-12.

[6] 李二闯，张建杰，袁亮，等.基于四元数互补滤波的小型四旋翼姿态解算[J].组合机床与自动化加工技术，2019（3）：37-39，43.

[7] 张波，李杰，刘伟，等.基于卡尔曼滤波的动态角度测量系统设计[J].电子器件，2018，41（2）：375-379.

[8] 王丹.卡尔曼滤波在动态倾角传感器测量中的应用研究[D].北京：北京信息科技大学，2013.

[9] 张栋，焦嵩鸣，刘延泉.互补滤波和卡尔曼滤波的融合姿态解算方法[J].传感器与微系统，2017，36（3）：62-65，69.

[10] 李庆党，张义龙.基于卡尔曼滤波和互补滤波的改进型姿态解算方法[J].青岛科技大学学报（自然科学版），2019，40（6）：85-89，98.

[11] 黄镇，张浩磊，刘梅，等.一种二阶互补滤波与卡尔曼滤波的姿态解算方法设计[J].电子工艺技术，2018，39（3）：168-170，178.

【通联编辑：梁书】