基于MEMS AHRS/GNSS的全组合导航

◆毛红瑛 陈至坤

（华北理工大学 电气工程学院 河北 063210）

多传感器信息融合技术在20世纪80年代被广泛应用在无人机导航系统、控制系统、机器人的运动控制，游戏机、虚拟现实以及各种可穿戴设备中[1]。全球导航卫星系统（GNSS）技术设计之初被用来提供位置、航迹等信息，该技术可以实现全天24 小时实时监测、几乎覆盖全球所有地域[2]。随着国内外学者对惯性器件的研究越来越深入，发现陀螺仪短时精度高、动态性能较好，受震动的影响较小，但是它长时间使用会出现累计误差；而加速度计电子磁罗盘在性能上可以很好与陀螺仪互补，虽然其输出可以得到姿态角，也不存在误差累积，但对环境要求较高，难以实现飞行器姿态的准确度量。综合上述对三种传感器优缺点的分析，笔者提出了更为完善的姿态解算方法，即将陀螺仪、加速度计、电子磁罗盘三者结合组成航姿参考系统（AHRS），利用传感器间的互补融合[3]，达到提高解算精度的目的。

关于组合导航技术，国外发展的较早，未来的发展主要集中在进一步提升GNSS 的抗干扰能力、以及与多种辅助传感器（如里程计、摄像机和光流计等）的结合这两方面[4]。本文采用扩展卡尔曼滤波算法利用加速度计、陀螺仪和电子磁罗盘求解姿态角，然后再采用松组合方式结合GNSS 来求解导航信息，提供了一种既适用于短时间也适用于长时间内的全姿态参数。GNSS 可以防止AHRS 在导航时产生的较大偏差，AHRS 可以缓解GNSS 信号中断时所造成的严重后果，因此将两者互补使结果最优。将优缺点总结如表1所示：

1 姿态解算系统

1.1 姿态初校准

当物体处于水平状态时，令俯仰角和横滚角等于零，此时将地球磁场在空间坐标轴上的三个分量依次记为故通过补偿地磁偏角，得到偏航角φ：

1.2 姿态融合算法

根据四元数微分方程[5]，将其离散模型可由一阶龙格-库塔法得到，如下所示：

对于加速度计来说，在n系中加速度计的输出为经过转换之后得到机体坐标系下的值

其中，

因此，将加速度计和子磁罗盘的测量值记作量测方程的量测量，建立量测方程：

2 组合导航系统

组合导航系统的状态向量包括位置、速度、平台失准角误差分量以及加速度计零偏、陀螺漂移误差分量，列写系统状态方程的15 维方阵如下：

组合导航系统的量测方程分为位置误差和速度误差，其中位置误差量为惯性导航计算得到的经度、纬度、高度信息与GNSS 接收机给出的相应信息的差值，速度误差同理，因此量测方程列写如公式11。

在卡尔曼滤波算法中，采集的数据都是离散的，因此在解决问题时需要利用泰勒公式进行线性化处理，然后再根据卡尔曼滤波算法的五大核心公式[6]来完成对输出的最优估计。

3 实验结果与讨论

3.1 平台搭建

关于硬件的选取，需综合考虑成本、精度等要求，来选取满足条件的最佳传感器件。硬件平台的搭建是以STM32F051 的微处理器作为控制和计算单元，实验过程的数据采集是在搭载有陀螺仪和加速度计的ICM42605 芯片上获取载体速度、加速度信息，AK8963 型号的电子磁罗盘芯片获得载体的磁场强度信息，WT-NEO6M 型号的GPS芯片获取速度和位置信息，并把该传感器集成模块与计算机利用USB-TTL 的方式连接起来。实验开始前给整个模块供电，让系统正常工作5min 进行预热过程。

3.2 初始对准结果分析和姿态解算

预热结束后，将传感器的输出数据进行采集，然后利用第1.1 节的姿态解算公式进行初始校准。实验过程是模块以任意姿态静止放置在桌面上，并保持这种状态大概1min，让周围远离其他磁场环境的干扰，然后对三轴加速度和磁场强度数据进行采集。

首先给模块上电，对传感器的输出数据采集分析，实验将采集频率设定在100Hz，时间为1min，初始化过程按照偏航角所在四个象限进行划分，得到误差对比如表2所示。

表2 误差对比/（°）

由表2 可以看出，当载体处于静止状态，通过加速度计和电子磁罗盘计算得到的姿态角与参考角的对比可知数据较为稳定，平均误差较小，均未超过0.7°。通过分析可知，此时的误差基本上是由惯性器件本身引起的，如果想要减小静态情况下的误差，需要选用精度更高的传感器。因此上述实验证明初始化过程良好，满足无人机对初始姿态的测量条件。

3.3 姿态解算实验结果

初对准结束后，接下来就要验证基于加速度计、陀螺仪和电子磁罗盘的扩展卡尔曼滤波算法在实际中测量姿态角的效果，手持模块缓慢的进行较大幅度的转动，通过人为绕着XYZ 轴任意转动去控制模块运动的姿态，并且保持模块所在的位置不发生变化，整个过程持续3min 左右，实验的采样频率设置为100Hz，得到姿态解算结果如下图1所示：

图1 姿态解算结果

由该实验得到的结算结果可知，此时γ平均误差为1.6063°，θ平均误差为1.5060°，φ平均误差为1.5106°，由于手持缓慢转动，故此时的加速度计测量噪声较小，通过扩展卡尔曼滤波算法得到的曲线足够平滑，与参考角度基本吻合，因此该算法性能较好，能够给出正确的姿态角度信息。

3.4 组合导航实验结果

手持模块先沿着Y 轴方向作直线运动，再绕Z 轴旋转大致90°沿着X 轴方向做直线运动，因此整个过程中除偏航角外的姿态变化并不是很大。但是由于手持运动，会产生较为明显的抖动。实验的采样频率设置为100Hz，经度118.59°、纬度39.19°，加速度计零偏误差为0.0001g，陀螺仪的常值漂移为，GPS 的位置误差为10米，速度误差为0.2m/s，运动时间为4min 左右，其中GPS 信号无遮挡为前3min 左右，然后数据读入写好的程序中。得到AHRS 姿态解算结果如下图2所示：

图2 姿态解算结果

由图2 可知，蓝色的实线表示通过扩展卡尔曼滤波算法的融合后得到的姿态角，红色实线代表参考角度，实验表明，经过扩展卡尔曼滤波算法以后的姿态角与参考角度接近，由于实验过程为手持沿着直角做直线运动，因此扰动较大，得到的曲线不够平滑，特别是在120s~130s 的拐弯处时，横滚角产生的最大误差为7°。整个实验横滚角的平均误差为1.1883°，俯仰角的平均误差为1.4075°，偏航角的平均误差为4.0337°，所设计的航姿参考系统误差结果符合预期，因此该算法可以得到较好的解算结果，使得最终的误差结果较小，满足低成本姿态解算需求。

从实验轨迹图3 可知，蓝色曲线代表组合导航的解算结果，红色曲线表示参考轨迹，通过经纬度误差对比可知，在220s 以前，GPS未被遮挡，此时组合导航效果良好，最大误差是5 米，组合导航能够较好跟踪GPS，而在220s 以后，GPS 被遮挡，此时只能通过惯性导航来定位，由于误差随着时间的不断累积，最终误差达到了20 米，由实验可知，当GPS 被遮挡失去信号，在10s 以内的累积误差最大为6 米，该结果在可接受的范围内。

图3 运动轨迹图

4 结论

由于惯性导航在长时间工作中容易产生累积误差，于是将其与电子磁罗盘、GPS 组合，通过融合算法得到最优估计，首先在静止条件下对系统进行初校准，然后通过陀螺仪得到相关状态向量，通过加速度计和磁力计得到相关量测向量，进行扩展卡尔曼滤波算法迭代求最优四元数，进而得到最终的姿态角，最后使用GPS 的松组合算法来辅助修正航姿参考系统，实验从低动态环境和高动态环境两方面展开，结果表明该算法能够较好反应数据的真实情况，并且在GPS 失锁的短时间内能够起到一定的导航定位作用，达到了预期效果。