• 98
•
中船重工第七一五研究所 赵 瑜 周 波 綦俊峰 刘雨聪 王 平
九轴微MEMS 传感器内部主要集成三轴陀螺仪、三轴加速度计和三轴磁力仪芯片，通过数据融合算法将各种独立的传感信息进行处理，以获得欧拉角数据或额外的信息。文章介绍一种双阶卡尔曼滤波融合算法，结合建立的数据融合数学模型，对多组原始数据进行运算及误差估计，得到估计后的姿态及航向数据。实测数据与基准对比表明，该融合算法解算的航向角误差＜1.5°rms ，姿态角误差＜0.5°rms 。 1 引言
九轴微MEMS 传感器成本低、体积小、功耗低，被广泛应用于各种消费场景和工业领域中，其内部核心部分为一枚三轴MEMS 陀螺仪芯片、一枚三轴加速度计芯片、一枚三轴磁力仪芯片和微处理器芯片。相比于六轴微MEMS 传感器（电子罗盘）而言，其内部额外集成一枚三轴陀螺仪芯片，可补偿瞬时角速度导致的角度干扰，因此在动态应用场合具备很高的跟随性和精度可靠性，可应用于航空、陆地、水下载体的实时姿态和方位跟踪、室内导航、航迹推算等（互补滤波和卡尔曼滤波的融合姿态解算方法：传感器与微系统，2017）。
微型陀螺仪芯片的优势在于体积小、功耗低、短时间的数据精度很高，但是长时间输出精度相对不高，角度随机游走和高斯白噪声会导致原始数据积分误差的累积，进而影响到角度估计的准确性。加速度计芯片的优势在于静态输出稳定，可精准的测量重力加速度在各轴方向的分量，但在运动过程中会引入线性加速度，且重力加速度和线性加速度无法区分，如果不采取措施，会导致角度解算的精度下降。磁传感器测量地磁场的矢量信息，地磁场在无磁干扰的条件下稳定不变，不随时间的变化而变化。
通过数据融合算法，将陀螺仪数据和加速度计数据进行融合并代入第一次卡尔曼滤波，充分发挥各自的优势，并对双方的不足进行补偿，使输出性能到达最优，二者的数据融合能够提供精确的姿态数据（横滚角、俯仰角），但是基于原理性的原因，无法获取准确的航向角。故在算法中引入磁传感器数据，在前一次卡尔曼滤波的基础上进行第二次卡尔
曼滤波，获得可靠的航向角数据。
2 姿态融合算法模型
2.1 预先系统估计
根据卡尔曼滤波理论，需要定义一个离散的状态方程，用于描述系统状态的进化过程，设当前的系统状态为xk ，前一时刻的系统状态为xk-1，进化过程的矩阵方程为Ak ，进化方程为Bk，系统的输入为uk ，则离散状态方程表示为：
xk=Ak xk-1+Bkuk (1)考虑到实际的系统状态是未知的，卡尔曼滤波只是提供一个估计，即所谓的“预先”系统估计，上述方程需要做出稍微的改变，便于区分估计状态和实际状态，方程表示如下：
(2)
其中“︿”标识表示实际系统状态未知，“-”标识表示给出所谓的预先系统估计（V.Kubelka and M.Reinstein,Complementary filtering approach to orientation estimation using inertial sensors only:in Proc.of.IEEE International Conference on Robotics and Automation(ICRA),2012）。
椒图科技2.2 系统估计表示方法
当前采用四元数来表示系统估计，在大多数情况下，四元数比欧拉角能更灵活的表示角度位置信息；此外，利用四元数，不存在角度奇异问题，系统可以更好地线性化。如果需要的话，四元数可以很容易地转换成其它旋转表示方法，比如旋转矩阵或欧拉角序列。
使用四元数q
来表示角度位置信息如下：
(3)
四元数由实数和向量组成：
表示实数，
表示向量。
一般情况下，需要将四元数进行归一化处理，以便于正确地表示旋转信息，归一化的计算方法如下：
(4)
若将四元数转换成欧拉角形式，表示方式如下，设翻滚角、俯仰角、航向角分别为r 、p 、y ，则转换公式为：
(5)
• 99
•
2.3 状态估计与修正
计算机学报在公式(1)中定义的为一个预先估计方程，该方程只使用了陀螺仪数据进行角度位置更新，当前来进行状态估计及修正，即定义卡尔曼滤波的校正方程。系统输入的原始传感数据包括陀螺仪数据、加速度计数据和磁力仪数据，校正过程中，首先使用加速度数据，对陀螺仪更新的角度位置进行修正，获取
修正后的状态估计，即第一阶卡尔曼滤波校正；然后使用磁力仪数据，对第一阶校正之后的状态估计进行再次校正，即第二阶卡尔曼滤波校正。根据卡尔曼滤波理论，修正方程如下：
(6)
其中表示实际测量值，在该系统中为加速度计数据或磁罗盘数据。
是预期的测量值，由预先系统状态方程计算得到。实际测量
值和预期测量值之间的差值称为残差。残差必须与增益
加权处理以
获得校正因子，然后用来计算“后处理”估计的系统状态，
是
这个概率问题的最小二乘解（David Tedaldi,Alberto Pretto and Emanuele Menegatti.A Robust and Easy to Implement Method for IMU Calibration without External Equipments:IEEE International Conference on Robotics & Automation,2014
）。
图1 姿态融合算法原理框图
联想v460驱动
3 数据分析及实测验证
算法的验证是通过实际测试的数据进行的，通过搭建的九轴微MEME 传感器样机，分别输出原始三轴陀螺仪数据、磁力仪数据和加速度计数据，样机通过串口将数据传输到上位机采集软件，采集软件实时记录并存储不同姿态下的三种原始数据，然后通过MATLAB 软件对存储的原始数据进行提取和处理，并送入双阶卡尔曼滤波算法，解算出实时的航向角和姿态角数据。
为验证算法的有效性和精度，传感器样机固定在一个三轴基准转台上，通过变换转台的角度和方位，实时记录原始数据用于角度的计算，并实时记录转台的基准角度读数，对比两者的误差以得出算法的效果。如下为横滚角[0,360°]范围内，俯仰角[-80,80°]范围内，航向角[0,360°]范围内，实测原始数据解算的读数与三轴转台基准读数的差值曲线（Foster C,Elkaim T.Extension of a non-linear,two-step calibration methodology to include non-orthogonal sensor axes:IEEE Journal of Aerospace Electronic Systems,2008
）。
超级电容器
图2 Roll/Pitch/Yaw解算角度与基准角度差值曲线图
其中横滚角误差为0.48°rms （＜0.5°rms ），俯仰角误差为0.42°rms （＜0.5°rms ），航向角误差为1.21°rms （＜1.5°rms ）。表明该融合算法能够有效的获取欧拉角信息，算法处理的精度较高。同时从上图可以看出，各曲线上有大小不等的噪声，这是因为原始数据代入融合算法之前，未对原始数据进行噪声滤波处理，该工作为今后的一个研究方向，以其使解算效果更佳（王正林，刘明，精通MATLAB 7：电子工业出版社，2006）。
疯狂英语中学版4 总结
通过对九轴微MEMS 传感器的核心芯片进行介绍，分别阐述了各自的优缺点，并提出了一种基于双阶卡尔曼滤波的姿态融合算法，从原理上分析了该算法的搭建模型及校正方法。为了验证该算法的有效性，进行了实测数据与基准转台的精度对比测试，数据表明，解算后的航向角误
差＜1.5°rms ，姿态角误差＜0.5°rms ，表明该算法切实有效。鉴于原始数据的噪声会影响解算精度，后续的研究方向为原始数据噪声滤波处理。令孩子惊奇的72个科学异想
作者简介：赵瑜（1986—），男，汉族，湖北随州人，硕士研究生，工程师，研究方向：传感器数据融合算法研究。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议