（中国船舶重工集团有限公司第七一〇研究所，湖北宜昌443003）
摘要针对九轴磁罗盘姿态解算的问题，提出了一种基于三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁力计的自适应姿态解算算法：将陀螺仪数据作为预测数据，估计出卡尔曼的过程协方差；加速度计和磁力计数据作为观测数据，结合陀螺仪的误差估计出测量噪声协方差，通过自适应卡尔曼滤波实现多传感器数据融合，解算出九轴磁罗盘准确的姿态。实验结果表明：本文设计的自适应滤波算法有效的抑制了噪声干扰，降低随机误差干扰；并且能够通过自适应补偿位移加速度、外界磁干扰等因素对姿态估计的影响。
关键词九轴磁罗盘；姿态解算；卡尔曼滤波
0引言
随着MEMS技术的不断发展，体积小、质量轻、功耗低的MEMS惯性传感器的应用范围由传统的航空航天、工业控制等拓展到了更为广泛的领域[1]，尤其对微小型运载体、便携设备等的姿态解算具有巨大的推动作用。基于MEMS惯性器件构建低成本微型航姿系统正成为一个研究热点[2~3]。
目前，常用测量姿态的传感器有陀螺仪、磁力计、加速度计等。陀螺仪测得的姿态角实时性好、短期精度高，但易受到温漂、时漂的影响；磁力计、加速度计不受初始位置限制，误差不随时间积累，但易受外部磁场以及载体自身加速度干扰[4]，因此单个传感器很难得到相对准确的姿态角信息。为了解决这类问题，需要对多传感器数据进行融合处理，才能够提高姿态解算的精度。三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁力计是测量载体姿态的九轴磁罗盘常用组合[5]。
1基于Kalman滤波算法的算法设计
1.1加速度计/磁力计姿态解算
九轴磁罗盘中三轴MEMS陀螺仪的输出即为载体的横滚角速率，俯仰角速率和方位角速率，通过相应的解算方法可以得到载体的姿态角信息，通过对三轴加速度计和三轴磁力计的信息进行计算可以得到静止或匀速运动状态下的方位角、俯仰角和横滚角。本文中方位角α定义为：载体纵轴在水平面上的投影与地球子午线之间的夹角，数值以地理北向为起点顺时针方向为正，定义域为[0°~360°]；俯仰角β定义为：载体的纵向轴和当地水平面之间的夹角，向上为正，向下为负，定义域为[-90°~90°]；横滚角γ定义为：载体的横向轴与当地水平面之间的夹角，右倾为正，左倾为负，定义域为[-180°~180°]。
当载体在地理坐标系下处于水平静止时，加速度计各轴测量输出为A=[0,0,g]。当载体处于任意姿态静止时，假设加速度计测量值为B=[ax,ay,az]，可计算俯仰、横滚角：
1.2.2系统量测方程
利用空间矢量与转动四元数间的关系，可以推导出一种基于伪量测方程的新型四元数Kalman滤波方法，避免了系统线性化引起的不稳定[6]。
2自适应Kalman滤波在姿态解算中的应用
Kalman滤波自产生以来，得到了广泛的应用。传统的卡尔曼滤波是在标准条件下获得的，是一种无偏的线性最小方差估计算法，在动态系统的数学模型和噪声统计特性已知的情况下，卡尔曼滤波通过测量值对预测估计进行修正，可以得到状态的精确估计。但是在实际应用中很难得到系统精确的数学模型和噪声的统计特性，使滤波精度降低甚至产生滤波发散的现象。Sage和Husa提出了一种自适应滤波算法，可以在线计算系统噪声Q和量测噪声R。但实际上，Sage-Husa自适应滤波只能在Q已知时估计出R，或者已知R时估计出Q。一般认为组合导航系统中系统噪声具有稳定性，通常仅对量测噪声进行估计。
对建立的离散系统得到简化的Sage-Husa自适应滤波算法为：
3实验验证
为了验证自适应Kalman滤波算法的可行性，现在对九轴磁罗盘分别进行静态实验（有磁干扰）、模拟动态水平台实验（无磁干扰）、实际运动实验。得到的结果如图2~4所示。
九轴磁罗盘在静止无磁干扰条件下启动，启动后静止一段时间，拿磁铁在罗盘附近来回晃动模拟磁干扰对罗盘产生影响。此时，九轴磁罗盘的输出和六轴磁罗盘（三轴加速度计与三轴磁力计组合）与真实输出姿态角的对比如图2所示。
从图2中可以看到，当系统处于静止无磁干扰状态时，六轴罗盘与九轴罗盘输出角度均在真值附近，且九轴磁罗盘输出经过自适应Kalman滤波处理后的姿态角方差明显减小，输出平稳，且均值未偏离加速度计估计的原始角度值。说明该算法能够有效滤除噪声干扰，消除随机误差。且在有磁干扰影响时方位角输出不受影响，说明自适应Kalman滤波算法能够有效消除磁干扰的影响。
九轴磁罗盘在静止无磁干扰条件下启动，启动后静止一段时间，将罗盘在水平台面上来回震荡（此时罗盘的俯仰横滚角保持不变），再保持静止，然后在水平震荡之后保持静止。此时，九轴磁罗盘的输出和六轴磁罗盘（三轴加速度计与三轴磁力计组合）与真实输出姿态角的对比如图3所示。
从图3中可以看到，当罗盘在水平台上运动时，六轴罗盘由于受到载体加速度干扰输出的俯仰、横滚角存在较大误差（最大接近50°），导致此时虽然没有磁干扰，但是方位角的输出依然会有很大误差。而九轴罗盘的输出结果来看，自适应Kalman滤波算法能够很好地消除载体加速度的影响。
九轴磁罗盘在静止无磁干扰条件下启动，启动后静止一段时间，然后将罗盘拿在手上晃动(用来模拟在船上或车上的状态)，再保持静止，然后再将罗盘拿在手上晃动一段时间后保持静止。此时，九轴磁罗盘的输出和六轴磁罗盘（三轴加速度计与三轴磁力计组合）与真实输出姿态角的对比如图4所示。
同样的从图4~5中可以看到，将罗盘在手上晃动时，六轴罗盘由于受到载体加速度干扰输出的俯仰、横滚角存在较大误差，导致此时虽然没有磁干扰，但是方位角的输出依然会有受到影响，而九轴磁罗盘的输出结果来看，自适应Kalman滤波算法能够很好地消除载体加速度的影响。
4结论
本文研究了九轴磁罗盘中的姿态解算问题，提出一种基于MEMS传感器的自适应Kalman滤波姿态解算方法，建立姿态解算滤波器模型；该方法有效的抑制了噪声干扰、降低随机误差对姿态解算的影响；并且能够自适应补偿非重力位移加速度、外界磁干扰等因素对姿态解算的影响。对于低精度应用场合，该算法具有计算速度快、实时性好等优点，已经满足了低精度应用的要求。
参考文献
[1]胡佳佳，周翟和，沈超等．一种自适应残差补偿算法在移动机器人姿态估计中的应用研究[J]．传感技术学报，2015，28(3)：363-366．
[2]鲁平，赵龙，陈哲．改进的Sage-Husa自适应滤波及其应用[J]．系统仿真学报，2007，17(15)：3503-3505．
[3]韩辅君，徐静，宋世忠．基于低成本多传感器的自适应组合滤波[J]．光学精密工程，2022，19(12)：3007-3015．
[4]刘付强．船用卫星天线微型姿态测量系统关键技术研究[D]．哈尔滨工程大学，2008．
[5]马敏，许中冲，常辰飞．低成本无人机姿态解算研究[J]．计算机测量与控制，2016，24(6)：161-167．
[6]邹波，张华，姜军．多传感信息融合的改进扩展卡尔曼滤波定姿[J]．计算机应用研究，2014，31(4)：1035-1038,1042．