惯性测量单元姿态融合的动态分析*

谢 敏，赵来定，王召文

(1.南京邮电大学 通信与信息工程学院，江苏 南京210003；2.南京邮电大学 通信与网络技术国家地方联合工程研究中心，江苏 南京210003)

0 引言

微机电系统内部集成了动态传感器、数字信号处理模块、串口通信等模块，是信息技术和机械工程的结合[1]。目前，微机电传感器具有成本低、体积小、功耗低、可靠性高等优势，广泛用于高智能化的行业中。因此，惯性测量单元(Inertial Measurement Unit，IMU)常采用微机电传感器。

惯性测量单元应用广泛，远至国家军事防御，近至日常智能设备。惯性测量单元可以测量装置或载体自身的加速度、角度等状态数据，一般通过加速度计等传感器进行测量。其中，陀螺仪用于测量设备自身的旋转运动。加速度计用于测量装置或载体的受力情况[2]。磁力计用于确定设备所处的方位。

陀螺仪易受温度、摩擦等因素的影响，存在误差，即零点漂移。加速度计的实时性较差，不能快速、准确做出响应，存在测量噪声。在短时间内，加速度计数据可信度较差；相反，陀螺仪的加速度数据在短时间内可信度较强，而长时间工作后，会因漂移产生测量误差。磁力计只针对水平情况下方位角的测量，且易受周围环境影响，可信度单一。因此，需要有效结合传感器，提高数据可信度。

现有的惯性器件处理算法，利用互补滤波算法修正陀螺仪的值，椭圆拟合解算得到运动载体的航向角[3]；Mahony互补滤波方法可以提高解算精度[4]；在捷联惯导的初始对准方面有维纳滤波、卡尔曼滤波[5]，在最优化算法方面有梯度下降法[6]、Newton法等。其中，梯度下降法可以有效抑制角度噪声。互补滤波可以利用系数结合多组数据。卡尔曼滤波可以提高角度和角速度精度，同时抑制零点漂移。

实验利用动态三轴摇摆台测试惯性测量单元的动态特性。实验证明，将梯度下降算法、互补滤波算法、卡尔曼滤波算法结合后，对传感器数据进行处理，可以有效提高惯性测量单元的数据精度，使其具有良好的连续性与稳定性。

1 姿态角计算

1.1 几何模型下的姿态角

一般地，加速度计可以测到一个动载体平台相对于地球的大地坐标系(N系)的运动状态。假设平台(联动惯性测量单元)做自由落体运动，则三轴加速度值均为零。若将加速度计测得的加速度值看作矢量R，在载体坐标系(B系)中分解成三个方向矢量R x、R y、R z，即R在xyz轴上的投影，利用三轴矢量可以得到姿态角中的俯仰角和横摇角[7]，其几何关系如图1所示。

图1 加速度计几何模型

根据公式：

求得矢量R在yoz平面上的投影R yz[3]，R x和R yz存在正切关系，得横摇角ϕ，同理可得俯仰角θ。

姿态角中的航向角Ψ，在装置水平放置时，可由磁力计单独获得[4]。磁力计是利用地球的南北极的微弱磁场作为判断依据。地磁场也可看作一个矢量H地磁，在某一地点，这个矢量可以被分解为三个相互垂直的矢量，其中H x、H y与水平面相平行，H z与水平面垂直[8]。若保持磁力计xoy面和水平面平行，则磁力计的三轴与这三个矢量对应，如图2所示。

图2 磁力计分量示意图

水平方向的H x和H y，矢量和指向磁北。航向角Ψ是当前方向和磁北的夹角。装置水平可得航向角Ψ：

经过推导，可以得到一组姿态角。由三轴加速度计数据可以得到横摇角、俯仰角，由磁力计水平分量可以得到航向角，但是，该组姿态角直接由惯性器件得到，受周围环境影响较大，稳定性较差。

1.2 基于梯度下降法的姿态角

四元数是一种姿态角的表示方式。四元数q0、q1、q2、q3，其中q0作为实部，其余作为虚部[5]，并可由三轴角度表示。

依照四元数乘法可以得到一个代表三轴旋转角度的四元数q。

捷联惯导的初始对准有两个常用坐标系[9]：一个是导航坐标系(N系)，导航坐标系是导航基准的坐标系；另一个是载体坐标系(B系)，是关于载体的坐标系。

姿态矩阵生成四元数，将导航坐标系转换到载体坐标系。假设空间矢量在载体系和导航系中的投影分别为r′和r，四元数在两个坐标系内转换可以通过p′=qpq-1实现。

梯度下降法定义为标量场中某一点上的梯度指向标量场增长最快的方向，梯度的长度是最大变化率[6]，并沿着最大变化率所在的负方向寻找最优解。

通过梯度下降法将姿态矩阵存在的误差减至最小。将误差定义成一个关于q的函数向量f(q)，偏微分处理得到最速下降方向，扩展到三轴得到雅克比矩阵J(q)。

下面引入梯度下降法的公式，代入四元数。

将加速度计数据ax、ay、az与归一化的重力加速度矢量[0 0 1]之间的差值作为误差函数fg(q)，微分得雅克比矩阵J(q)。

已知误差函数及其雅克比矩阵，可以利用梯度公式求出误差函数的梯度▽fg(q)：

将该梯度代入梯度下降的公式，其中步长μt与实际运动时的角速度和采样时间有关。这样就获得了姿态四元数q▽，t。

1.3 四元数微分方程

姿态四元数q是时间的变量，自变量为t时刻角度θt，由陀螺仪积分得到，因变量为四元数q t。u为三轴矢量i、j、k归一化后的矢量，w t为角速度矢量[10]。

对q t求导得到另一组姿态四元数q w，t：

2 互补滤波和卡尔曼滤波

通过加速度计和磁力计数据所得姿态角，仅反映出惯性测量单元当前状态，受到环境噪声影响大，数据容易出现野值。虽然基于梯度下降法的数据和基于微分方程的数据在一定程度上可以抑制测量噪声，但是梯度下降法具有“碗底”效应，步长过大时不能快速收敛。因此，需要将梯度下降法数据和微分方程数据相结合[11]。

2.1 微积分的互补滤波

将 姿态 四 元 数qΔ，t[12-13]与 姿 态 四 元 数q w，t互 补结合，得到q t：

基于四元数的微分方程适合用于物体高速运动状态，梯度下降法适合用于低速运动。因此，高速运动下α取值要小一些，低速运动下α要大一些。从收敛速度的角度来看，当梯度下降的收敛速度等于微分方程的收敛速度时，可得最优解。设四元数微分方程的收敛速度为β。当α近似为0时适合对动态物体的测量。

式(20)中q▽，t-1相比于μt▽fg(q)所占比重较小，可忽略不计，q▽，t得到近似表达，并求得q t。 根据姿态四元数q t和姿态矩阵得到姿态角ϕt、θt和Ψt。

2.2 基于PI的姿态角互补滤波

对于梯度下降法，经过仿真后发现，若梯度下降的步长μ过大，则寻至最优解时间较长，就像“碗底”一样。步长过大会越过最优值，在最优值附近持续振荡，如图3所示；步长太小，到达“碗底”的时间就会变长。

图3 梯度下降法“碗底”示意图

因为大步长会引起振荡，使数据产生毛刺，所以在实验过程中采用的步长较小。

磁力计和加速度计所测得角度没有累积误差，但动态响应较差[14]。利用互补滤波器融合三种传感器的数据，提高测量精度和系统的动态性能。互补滤波法需要用到加速度计数据ax、ay、az，陀螺仪数据wx、wy、wz，磁力计 数据mx、my、mz。 互补滤波器 公式如下，其中C0(s)为加速度计和磁力计观测到的姿态矩阵，Cw(s)为陀螺仪测量数据计算得到的姿态矩阵。

互补滤波器传递函数：

姿态矩阵：

通过与梯度下降法相同的方法得到重力分量Vx、Vy、Vz及ax、ay、az，误差ex、ey、ez可以表示为两者叉乘：

角速度更新四元数，并转化为姿态角：

2.3 姿态角之间的互补

基于几何的姿态角ϕ、θ、Ψ，基于梯度下降法的姿 态 角ϕt、θt、Ψt和 基 于 互 补 滤 波 的 姿 态 角ϕn、θn、Ψn，这三组不同的姿态角进行姿态角之间的互补，其中滤波系数k是根据加速度计数据ax、ay、az所得[11]：

当载体处于低速运动时，梯度下降法通过小步长迭代能够快速收敛；当载体处于高速运动时，PI互补滤波跟踪性能更好。几何的姿态角作为参照标准，可以防止低速所带来的震动以及超速运动。

2.4 卡尔曼滤波

梯度下降法和PI互补滤波解决了加速度计和陀螺仪的部分噪声和收敛问题，但陀螺仪因为重力存在零点漂移，漂移角度经过较长时间的累计，会使测量出现偏差。

卡尔曼滤波是一种基于无偏最小方差递推准则的滤波算法[16]，它既适用于平稳随机过程，也适用于非平稳过程。其核心是通过建立状态模型和测量模型，利用上一状态的估计值和这一状态的测量值得到状态转移方程，并更新状态模型。相比于需要知晓所有状态的维纳滤波，卡尔曼滤波更适合于传感器输出滤波处理。

卡尔曼滤波第一个公式给出状态方程、观测方程：

其中X k为k时刻的系统状态矩阵；A为状态转移矩阵；u k为k时刻的角速度矢量；B为输入加权矩阵；w k为系统噪声，为零均值白噪声过程。以t时刻俯仰角θt，陀螺仪输出wt，零漂Biast，采样周期Δt为例，对状态更新方程进行二维矩阵描述。

卡尔曼滤波的第二个公式计算的是系统的协方差矩阵：

其中需要给出两个值Qaccl、Qgyro，分别为加速度计所得到的角度噪声、陀螺仪的角速度噪声。Q为矩阵[θtBiast]T的协方差矩阵：

设矩阵P k-1=[a b；c d]，A已知，根据方程可得：

第三个公式通过协方差矩阵计算卡尔曼增益：

其中P k和H已知，R为观测噪声矩阵。在惯性测量单元系统中有两种状态变化量：角度θt和零漂Biast，分别对应各自的卡尔曼增益，设为K k=[k0k1]。

得到[k0k1]后，通过第四个公式对状态矩阵X k进行修正。

写成矩阵形式如式(49)所示，其中Z k=θaccl，是加速度计所得到的角度值，作为观测输入。

零点漂移值可以对陀螺仪角速度修正：

最后一个公式对协方差矩阵进行更新：

至此，即为卡尔曼滤波的五大方程[17]。输入参数为加速度计所得角度值和陀螺仪的角速度值，以及角度、角速度噪声值，输出为修正后的角度和角速度。

3 算法总结

上文中算法的数据处理流程可以分为3个部分：(1)根据几何模型计算出姿态角，即从器件寄存器直接计算得到；(2)利用加速度计和陀螺仪数据分别通过梯度下降法和PI互补滤波法得出姿态角，两组姿态角再进行互补计算；(3)将第二步得到的姿态角和角速度通过卡尔曼滤波修正，得到最终三轴的角度和角速度。卡尔曼滤波流程图如图4所示。

图4 卡尔曼算法流程图

4 实验结果与分析

为了验证该姿态融合算法，本次实验采用了STM32F405RGT6作为芯片平台，ADIS16460作为六轴传感器模块，HMC5883_L作为磁力计模块进行实验，通过串口将数据反馈给上位机。其中，对传感器初始化做出了改进：通过对ADIS16460的DEC_RATE将原先的采样率从512 S/s提升至2 048 S/s，增强了惯导输出的连续性。ADIS16460的FLTR_CTRL寄存器可以提供对数字低通滤波器的控制。此滤波器包含两个级联均值滤波器，它们提供Bartlett窗口、FIR滤波器响应。为了保证数据输出的实时性，将其滤波器抽头数从64降至16。原先数据输出是通过外部中断结束后输出，现改进为2 ms的计时器中断，一方面是为了数据的连续性，另一方面固定的间隔时间数据可以保证数据的实时性。

4.1 静态实验分析

实验装置分为两套：第一套是原始算法，由惯性器件直接输出数据的惯性测量单元；第二套是经过姿态融合滤波算法处理的惯性测量单元。将两套实验装置静置于桌面，以俯仰轴的输出数据为例，进行对零点漂移的验证。

从图5中虚线可以看出，未经过融合滤波算法的俯仰角在静止时总是朝着负角度方向呈现增加趋势，最终会偏离真实值。从图中实线可以看出，经过融合滤波算法的俯仰角在静止时的角度总是围绕着一个数值上下小幅度变化，不会出现偏移。因此，姿态融合滤波算法能够有效地抑制角度偏移。

图5 静止俯仰角度

4.2 动态实验验证

将惯性测量单元放置在摇摆台的重心位置，并设置摇摆台运动方式为正弦运动，其幅度为20°，运动周期为8 s。共进行3次试验，3次试验分别包括横摇、俯仰和方位。实验结果如图6～图10所示。

图10 动态方位角度

在动态实验中，未进行过滤波算法修正的横摇、俯仰和方位可以总结出以下三个问题：

(1)从图6动态俯仰角度可以看出，原始算法数据存在着滞后的问题，在前四分之一周期内延时了约1 000个采样点，并随时间逐渐增多；

图6 动态俯仰角度

(2)原始算法下俯仰角度区间会随时间出现角度偏移，不能始终保持在正负20°之间；

(3)从图9动态横摇局部角度可以看出，原始数据在角度变化过程中呈现不连续的阶梯状、断崖式的变化。

图7 动态俯仰局部角度

图8 动态横摇角度

图9 动态横摇局部角度

经过姿态融合滤波算法的修正后，解决了俯仰轴的滞后问题，不会出现相位偏移现象；减小了零点漂移带来的时间偏移，俯仰角度总是保持在正负20°之间；平滑了输出角度，使其在局部呈现为一条光滑曲线。因此，经过姿态融合滤波算法的惯导装置在动态特性的追踪和稳定性上优于原始程序。

5 结论

本文介绍了一种基于梯度下降法、互补滤波法和卡尔曼滤波的姿态融合算法。首先，从原理上分析了梯度下降法和四元数微分法的优势；其次，介绍了互补滤波法和卡尔曼滤波算法对姿态数据进行结合的过程；最后，通过动态分析实验验证了姿态融合算法的可行性。

实验结果证明，基于梯度下降法、互补滤波法和卡尔曼滤波的姿态融合算法能够成功地抑制加速度计的测量噪声，并减小陀螺仪的零点漂移所带来的影响。相比于传统算法输出数据的滞后、不连续，基于姿态融合算法的数据实时性强，角度曲线连续且平滑，能够有效、准确地描述运动中物体的姿态。同时，在硬件上调整了滤波器的参数，提高了数据输出速率。因此，基于该姿态融合算法的惯性测量单元具有良好的稳定性与可靠性，在工程应用中有一定的参考价值。