多传感器互补滤波器数据融合算法设计

马 龙，代超璠，周 航，裴 昕，黄 超，苏志刚

(中国民航大学 中欧航空工程师学院，天津 300300)

0 引 言

多传感器姿态测量系统是利用多个传感器的信息测量载体姿态的设备，输出载体相对于导航坐标系的角位置信息，包含横滚角、俯仰角及航向角[1]。姿态角作为导航中不可或缺的重要信息，是保证载体运行安全的重要参数[2]。近年来，姿态测量系统多以多传感器组合方式实现，组成多传感器姿态测量系统，其姿态解算的关键技术为数据融合技术。

基于MEMS的传感器具有可靠性高、小体积、低成本和高集成度等特点[6-10]，随着MEMS技术的迅速发展，MEMS传感器的精度不断提高，其在多传感器组合方面的应用也随之不断增加[11-13]。同时，FPGA以其高逻辑集成度和内嵌高性能硬件模块的优势弥补了单处理器系统在姿态测量系统中复杂度太高且计算能力不足的缺点，并且FPGA的强大的并行处理能力，使得多传感器的数据融合得以实现[14]。利用惯性传感器测量载体姿态存在诸多不足之处，如仅由陀螺仪构成的姿态测量系统虽然动态性能良好，但是其累计误差不符合长时间导航要求[15]；而加速度计通过重力加速度来解算载体的姿态角要求载体处于静止或者匀速直线运动状态；此外，电子罗盘易受当地磁场的干扰，其输出的精确性不能得到有效保证[16]。其次，国内外在互补滤波器的研究方面，对于两种不同传感器数据融合颇为注重[17]，致使多传感器数据资源没有得到充分的利用，此外，多用陀螺仪辅助磁罗盘完成故障检测[18]，并且对陀螺仪的精度要求较高。例如，文献[19]利用视觉的方法辅助惯导的方法获得载体的姿态角，使其在局域的空间内作用显著。文献[20]对扩展卡尔曼滤波器(EKF)和互补滤波器进行了比较验证，表明了互补滤波器相对于EKF具有较快的收敛速度，但是由于只限于两种传感器，其精度还是有限。文献[21]利用三传感器提出四元数扩展卡尔曼滤波器，但是未对地磁矢量加以考虑，磁场的干扰仍未得到有效的解决。

基于上述问题，本文以FPGA为核心，利用MEMS陀螺仪、MEMS加速度计和电子罗盘构建了一套基于互补滤波器的多传感器姿态测量系统，针对各个传感器的频率特性分别设计了多传感器下俯仰角和横滚角互补滤波器以及航向角互补滤波器，实现多数据源的数据融合。实验结果表明，互补滤波器能在保证输出精确性前提下完成多传感器的数据融合。

1 多传感器姿态解算

基于互补滤波器的多传感器姿态信息融合，首先在传感器级别利用惯性传感器进行姿态解算，得到各个传感器解算的结果，随后利用互补滤波器进行数据融合得到载体的姿态信息。其中，俯仰角和横滚角互补滤波器融合的数据来源为陀螺仪，电子罗盘和加速度计解算得到的姿态信息，航向角互补滤波器融合的数据来源为陀螺仪和电子罗盘解算得到的姿态信息。

1.1 陀螺仪四元数姿态解算

四元数将复平面推广到了四维空间，广泛应用在刚体或者坐标系的旋转运动描述中，四元数的标量部分和矢量部分分别表示了转角一半的余弦值和其瞬时转轴的方向余弦值，因此可以用于表示刚体运动过程中坐标系间的相对运动和姿态变化信息。

刚体定点转动过程中四元数Q的表示如式(1)所示

(1)

式中：q0、q1、q2和q3值如式(2)所示

(2)

式中：四元数Q用于描述刚体绕定点的旋转，可以认为载体坐标系b系经过无中间过程的一次性等效旋转为导航坐标系n系。并且四元数Q中包含绕定点等效旋转有关的信息为：θ为等效旋转转过的角度，un为等效旋转的旋转轴和转动的方向。

(3)

(4)

故描述刚体转动的四元数是规范四元数，式(3)可写成式(5)

(5)

因此，可以建立四元数和姿态矩阵之间的关系，同时姿态矩阵可用于求解载体的姿态角，令

(6)

则由式(6)可得式(7)

(7)

式(7)和式(1)可推出式(8)

(8)

四元数通过四元数微分方程完成得到[20]，并根据式(5)可得到刚体转动四元数和姿态矩阵之间的对应关系，进而利用姿态矩阵可以确定刚体转动后的姿态角。

1.2 加速度计姿态解算

(9)

(10)

(11)

则将式(11)和式(9)代入式(10)可解算得到俯仰角θ和横滚角γ，如式(12)所示

(12)

载体单独的航向角变化并不能引起加速度计的输出变化，因此利用加速度计并不能计算载体的航线角。

1.3 电子罗盘姿态解算

电子罗盘内部集成了磁强计和MEMS加速度计，利用磁强计可以根据旋转过程中磁场变化计算得到载体航向角，并结合加速度计可测得载体的姿态角。

考虑初始载体坐标系n沿矢量[0 0ψ]T进行旋转，得到坐标系为d，地磁场矢量m在d系和n系中分量的关系如式(13)所示

(13)

(14)

利用磁强计输出的磁场强度mb和已经确定的横滚角和俯仰角，可以得到md如式(14)所示。结合式(13)，并且查询得到当地的磁场分量mn，则可解算出载体航向角ψ，如下式

(15)

2 互补滤波器数据融合方法

多传感器姿态测量系统首先利用MEMS陀螺仪、MEMS加速度计和电子罗盘分别解算出姿态角，随后利用各个传感器的姿态信息进行姿态信息融合，得到最终的姿态角。在姿态信息融合时，首先需要对各个传感器解算的姿态角噪声进行频谱分析。传感器做旋转运动时，MEMS陀螺仪通过四元数法解算出载体姿态角，并与姿态参考系统输出值作差即可得陀螺仪姿态角计算偏差。同理，可以求得MEMS加速度计和电子罗盘的姿态角计算偏差，并且由于各个传感器的三轴的相似性，本实验以俯仰轴为例设计实验。

利用快速傅里叶变换在频率域上对传感器的噪声进行分析，可以更加直观和准确的得到各个传感器的噪声特性，方便对噪声频率特性进行进一步的分析。结果如图1～图3所示，分别为MEMS陀螺仪、加速度计和电子罗盘姿态角输出偏差的幅频曲线，其能量截取比例均在95%以上。

图1 陀螺仪姿态角计算偏差幅频曲线

图2 加速度计姿态角计算偏差幅频曲线

图3 电子罗盘姿态角计算偏差幅频曲线

对比图1～图3可知陀螺仪和加速度计噪声主要集中在低频带，而电子罗盘的噪声主要集中在高频带。由此可知，电子罗盘得到的姿态角噪声与MEMS陀螺仪和加速度计得到的姿态角的噪声在频率上具有一定的互补性。根据噪声频率互补性可设计互补滤波器，如图4为俯仰角和横滚角滤波器基本原理框架，其中Zprm(s)、Zprg(s)和Zpra(s)分别为利用电子罗盘、MEMS陀螺仪和加速度计解算的俯仰角和横滚角，Gprm(s)、Gprg(s)和Gpra(s)为对应的滤波器，其表达如式(16)所示。根据图4和式(16)可求出式(17)，并做出图5俯仰角和横滚角互补滤波器解算框架。

图4 横滚角和俯仰角解算原理框架

图5 俯仰角和横滚角互补滤波器解算框架

(16)

(17)

同理，图6为航向角互补滤波器基本原理框架，图中Zym(s)和Zyg(s)分别为电子罗盘和陀螺仪解算值，Gym(s)和Gyg(s)为两传感器对应的滤波器，其表达如式(18)所示，根据图6和式(18)可求出式(19)，并做出图7航向角互补滤波器解算框架

(18)

(19)

图6 航向角互补滤波器基本原理框架

图7 航向角互补滤波器解算框架

3 实验结果与分析

图8为多传感器姿态测量系统硬件模块。控制运算模块以FPGA芯片作为系统处理核心，利用其内部的通用浮点计算资源和可配置IP核，构建以MicroBlaze为处理核心的小型流水线系统。利用ISE/XPS(EDK+SDK)开发环境配置AXI(Advanced eXtensible Interface)总线协议的各子模块，并完成以MicroBlaze为核心的片上系统设计。其中AXI总线协议是一种面向高性能、高带宽、低延迟的片内总线，可以完成处理核心与各个模块接口之间的数据通信，以及传输FPGA芯片对其内部运算单元的控制指令。利用微处理器硬件规范文件(microprocessor hardware standardization，MHS)和微处理器外围设备描述文件(microproces-sor periphery document，MPD)完成硬件系统的中断处理和总线结构的配置；利用微处理器软件规范文件(MSS)完成软件系统中标准I/O设备和外围设备连接方式以及各个模块驱动的配置。

传感器模块包含MEMS惯性传感器ADIS16385和电子罗盘HMR3500，可单独计算载体的姿态角信息。其中，惯性传感器内置三轴陀螺仪和三轴加速度计；电子罗盘内置三轴加速度计、三轴磁强计，可用于计算载体所处环境的磁场强度。系统利用256 KB高速Flash存储器，弥补了FPGA内程序掉电易擦除的缺点，并完成系统上电自加载功能。电源模块为系统提供稳定的5.0 V、3.3 V和1.2 V电压，以保证系统的正常运行。图9为姿态测量系统硬件平台实物图。

图8 姿态测量系统硬件框架

图9 多传感器姿态测量系统实物

姿态测量实验过程中，根据运动状态和磁场状况设定滤波器参数，通过姿态测量实验将滤波器输出结果与AHRS测量值作差比较，进行姿态测量实验验证算法。为了验证互补滤波器在多传感器姿态测量系统的有效性，利用三维转台设计姿态旋转实验。表1为姿态旋转实验运动的参数，三维转台围绕3个轴旋转轴旋转角度均为±25°，俯仰角、横滚角和航向角旋转周期分别为47 s、35 s和15 s三维转台旋转运动均为15个周期，并且转动过程中静止60 s，用于采集周围的磁场强度和重力加速度，作为当地磁场强度和重力的参考值。

表2为俯仰角和横滚角滤波器参数设定值，表3为航向角滤波器参数设定值。设计载体旋转实验，使系统分别绕三轴旋转±25°，将多传感器姿态测量系统和姿态参考系统输出进行对比，结果如图10、图11所示，分别为多传感器姿态测量系统姿态输出偏差幅值-频率曲线图和多传感器姿态测量系统互补滤波器输出与AHRS偏差图。由图可知，多传感器姿态测量系统噪声水平明显降低，并且横滚角和俯仰角精度保持在±0.35°以内，航向角精度保持在±0.7°以内。

实验结果表明，互补滤波器能有效降低多传感器姿态测量系统的输出噪声，提高了系统精度，验证了系统所设计的基于多传感器的互补滤波器数据融合算法的有效性。

表2 俯仰角和横滚角互补滤波参数设定

表3 航向角互补滤波参数设定

图10 互补滤波器输出偏差幅值-频率曲线

图11 姿态测量系统互补滤波器输出与AHRS偏差值

4 结束语

本文以FPGA为处理核心，以MEMS陀螺仪、MEMS加速度计和电子罗盘建立多传感器组合，根据传感器噪声互补性设计三传感器组成的俯仰角和横滚接互补滤波器以及双传感器组成的航向角互补滤波器，实现了数据融合。构建了一套基于FPGA的多传感器姿态测量系统并进行互补滤波器的实验验证。实验结果表明，相比于姿态航向参考系统(AHRS)输出值，俯仰角和横滚角精度在±0.37°以内，航向角精度在±0.8°以内，验证了该系统具有较高精度的输出。