微惯性组件多传感器数据融合测姿方法研究

王 磊，孟志敏，关 英，宋 博

（沈阳理工大学机械工程学院，辽宁 沈阳 110159）

1 引言

微机电系统（MEMS）惯性器件及其组成的各类惯性测量系统一直是各国大力发展的核心和前沿技术，欧美和日本等显示出了明显的领先优势。MEMS 惯性测量单元（MEMS IMU）成为了MEMS 领域研究和发展最重要的方向之一[1]。由微惯性组件构成的导航系统往微小型、中高等精度的组合导航方向发展。

以MEMS 陀螺仪、加速度计等微惯性组件所构成微惯导系统，可广泛应用于机器人、无人飞行器、人体姿态检测、飞机/导弹飞行控制、姿态控制等领域，相关理论及应用研究具有重要意义[2-5]。

采用微惯性测量组件与其他传感器，卫星接收系统，诸如GPS、北斗、地磁传感器，高度气压计等进行组合，多传感器数据融合[3-5]、MEMS 惯性器件标定与补偿[6-7]、高精度滤波、姿态测量[8-10]是研究的重点和热点。

针对由MEMS 陀螺仪、加速度计、磁阻传感器所构成多传感器数据融合系统，根据MEMS 微惯性器件不同噪声的频率特点，通过加速度计和陀螺仪融合测姿得到滚转角和俯仰角，通过磁阻传感器计算航向角。利用不同传感器得到的姿态数据，作为观测数据，构建无迹卡尔曼滤波器，有效融合三种传感器信息，对载体姿态预测估计，进一步提高测姿精度。相关算法在实验室开发的微惯导系统中进行了实验验证。

2 低成本微惯性组件开发

针对微惯导系统在诸如无人飞行器等环境下的位姿检测，实验室自主研发了微惯导系统，包括数字信号处理器（DSP28335），电源模块、Flash、SDRAM、三个单轴陀螺仪、加速度计和磁阻传感器等。微惯导系统尺寸（45×45×30）mm，系统采样率 100Hz，如图1 所示。

微惯导系统包括6 层PCB 布线的弹载机和微惯性组件构成。其中陀螺仪通过SPI 总线接收、加速度计和磁力计通过16 位A/D 采集数据，提出的算法通过Matlab 仿真验证后，转化为C 语言，并移植到DSP 嵌入式系统CCS3.3 中实时运行。

图1 微惯性导航系统Fig.1 Micro Inertial Navigation System

3 多传感器姿态测量方法

微惯性测量组件常由陀螺仪，加速度计辅以磁阻传感器构成。四元数和欧拉角方法可通过陀螺仪输出完成姿态更新[8]，求解载体的横滚、俯仰和偏航角度。

式中：q—四元数向量，q=［q0q1q2q3］T；运算符“⊗”—四元数乘法—导航和载体坐标系的相对角速率。

静止或低动态条件下，可由加速度计输出计算俯仰和滚转角。

式中：mb—载体坐标系下磁阻传感器输出；mp—地磁分量在水平坐标系的投影，满足：

航向角ψm可由下式计算

考虑到不同MEMS 微惯性器件不同噪声的频率特点，对于陀螺仪在低频情况下会产生较大漂移，会噪声误差积累；对于加速度计/磁阻传感器，易受到高频干扰。采用互补滤波器对陀螺仪、加速度计和磁力计测量数据进行数据融合，可以提高载体姿态的测量精度。动态环境下，滚转角和俯仰角通过加速度计和陀螺仪融合测姿得到，航向角通过磁阻传感器计算得到。

4 多传感器信息融合与滤波算法

微惯导系统中，单一传感器的测姿方法均存在缺陷，由陀螺仪更新计算姿态角的误差会累积，由加速度计/磁阻传感器计算姿态角误差存在高频噪声。为克服以上不足，利用不同传感器得到的姿态数据，构建卡尔曼滤波器可以有效的融合三种传感器信息，获取更高的测姿精度。采用多传感器组合测姿与无迹卡尔曼滤波（UKF）相结合方法预测和估计载体姿态。

设系统的状态方程和观测方程分别为：

式中：Xk、Zk—状态向量和量测向量；f（Xk-1）、h（Xk）—非线性状态函数和量测函数；wk-1、vk—服从于 N（q，Q）和 N（r，R）的系统噪声和观测噪声，它们是互不相关的高斯白噪声。UKF的具体控制流程如下：

采用UT 变换对每个Sigma 点进行非线性变换，计算k 时刻的一步预测模型：

将系统状态向量和协方差矩阵进行增广处理，计算k 时刻的一步预测增广样本点和观测量。

计算增益矩阵：

使用Kalman 增益对系统的均值和协方差矩阵进行更新，得到后验的状态估计

式中：q—四元数；

b0—陀螺仪的常值零偏，状态方程，如式（22）所示。

式中：τ—高斯马尔可夫时间常数；

ωr—高斯白噪声。

为有效融合陀螺仪、加速度计、磁阻传感器信息，观测量可采用加速度计，陀螺仪，磁阻传感器计算的姿态角，观测方程为

式中：φacc&gyro，θacc&gyro—通过加速度计和陀螺仪，应用互补滤波器计算的姿态角；ψmag—磁阻传感器计算的航向角—测量噪声。

5 人体姿态测量实验

为验证上述所提姿态测量方法有效，实验室开展了人体姿态测量实验，如图2 所示。实验选用了一款商用的航姿系统MTi，该系统是荷兰XSENS 公司生产的一款集成了陀螺仪、加速度计、磁阻传感器等微型惯性测试系统，具有较高的姿态测量精度。

图2 人体姿态测试系统Fig.2 Testing System of Body Attitude

将微惯性组件、MTi 固定在手臂上，经由RS-232 接口与上位机连接。实验开始后，人体随意摇摆手臂，做大幅度姿态变化，同时采集传感器数据。为验证算法的有效性，采用MTi 提供的姿态信息作为参考姿态，用于实验比对。实验中，分别采用基于四元数解算的陀螺仪测姿方法，加速度计和磁阻传感器组合测姿方法，提出的不同传感器组合测试与无迹卡尔曼滤波（UKF）融合的测姿方法分别对载体姿态进行解算，实验结果，如图3～图5 所示。滚转角、俯仰角和航向角的变化曲线，如图3（a）～图5（a）所示。相应的误差曲线，如图3（b）、图4（b）、图5（b）所示。基于四元数的陀螺仪测姿方法，受到了其漂移误差的影响，随着时间积累，误差增加，如图3～图5 所示。以滚转角测姿为例，如图4 所示。测姿结果在（0～10）s 内的姿态变化与MTi 的参考曲线很接近，而在（20～30）s 时间段内解算曲线虽能跟随参考轨迹变化趋势，但解算误差逐步增加。加速度计/磁阻传感器组合算法得到的姿态变化曲线围绕参考姿态上下波动，解算姿态角虽然不会累积，但易受到高频噪声的较大影响，如图3（b）、图4（b）和图5（b）误差曲线所示。如图3（b）所示，当航向角发生反向变化时（在9s 和18s时），加速度计/磁阻传感器组合算法计算的航向角噪声明显比航向角同向变化时大。因此，依靠单一传感器很难取得满意的测姿精度。互补滤波器分别采用低通和高通滤波分别对不同MEMS 微惯性器件滤波处理，提高了一定的测姿精度。提出的UKF 融合测姿方法，克服了单一传感器测姿的不足，充分利用不同传感器的测姿结果作为观测量，可以实现对量测噪声和方差的有效跟踪，姿态估计值逐渐趋于收敛，可以较好完成对参考姿态轨迹的跟踪。俯仰，滚转和航向角的动态误差分别为0.71°，0.88°和 1.10°。

图3 航向角及误差Fig.3 Yaw Angle and Yaw Angle Error

图4 滚转角及误差Fig.4 Roll Angle and Roll Angle Error

图5 俯仰角及误差Fig.5 Pitch Angle and Pitch Angle Error

6 结论

针对低成本微惯导系统，提出了一种利用不同微惯性传感器组合测量与UKF 融合滤波方法，该方法可有效测量载体姿态。搭建了实验平台，分别应用基于四元数的陀螺仪测姿，基于加速度计和磁力计的组合测姿方法，提出的融合滤波方法进行了人体姿态测试实验，通过实验对比结果，UKF 融合测姿方法可以较好的实现对参考姿态的跟踪，俯仰，滚转和航向角的动态误差分别为 0.71°，0.88°和 1.10°。