基于双级互补滤波的姿态测量算法设计*

高 宁,李 杰,冯凯强,许廷金,高诗尧,李炳臻

(中北大学电子测试技术重点实验室,太原 030051)

在广泛的导航应用领域中,精确的姿态估计是必不可少的,如无人机导航、车辆导航、室内导航、人体姿态控制等[1-4]。由于体积小、成本低、功耗小等优点,MEMS器件被广泛应用于姿态测量领域[5]。然而,由于MEMS陀螺仪存在漂移,长时间工作有累积误差;利用加速度计和磁力计来估计姿态角会受载体线性加速度和磁场干扰的影响[6-7]。因此,单独使用某一种传感器不能获得精确可靠的姿态信息,必须利用姿态融合算法对不同传感器的数据进行融合,以提高系统的精度和抗干扰能力[8-10]。

至今,已有许多国内外学者相继研究出了不同的数据融合算法用于姿态测量领域。Sabatini等[11]人提出了一种基于四元数的扩展卡尔曼滤波器(EKF),将旋转四元数以及加速度计和磁力计误差作为状态矢量,同时引入了一种自适应测量噪声协方差矩阵构造方法,以减小载体线性加速度和磁场干扰的影响。但是EKF存在观测方程在线性化时会引入线性误差等缺点。为此,Yun等[12]人提出了一种基于Gauss-Newton的外部观测方法,有效解决了观测方程的线性化问题。但其计算量大,低成本处理器难以实现。为了降低计算量,Mahony等[13]人提出了一种用于无人机姿态估计的显式互补滤波器(ECF)。Madgwick等[14]人使用梯度下降算法优化提出了固定增益互补滤波器。这些互补滤波算法利用加速度计和磁力计的测量数据来补偿陀螺仪的累积误差,但是由于磁力计的测量值极易受周围磁场干扰的影响,给航向角估计带来误差的同时也会给水平角带来不确定的影响。

针对上述问题,本文提出了一种双级互补滤波姿态融合算法。该算法利用加速度计和磁力计的测量值分步对MEMS陀螺仪估计的四元数进行补偿,避免了磁场干扰情况下航向角误差对水平角测量的影响,同时引入了载体非重力加速度误差和磁干扰误差自适应补偿方案,提高了姿态测量精度。并且算法实现过程简单,计算量小,降低了对处理器运算速度的要求。最后,基于现有的惯性测量系统对所提算法进行了实验评估。

1 基于四元数的姿态确定分析

取载体的重心为坐标系原点,3个坐标轴分别与载体的纵轴、横轴和竖轴相重合,定义为载体坐标系b,与之相对应的绝对坐标系通常称为导航坐标系n。导航坐标系n向载体坐标系b的转换可以通过四元数法或Euler角法实现,四元数法由于可以避免Euler角的奇异问题而得到了广泛的应用[15]。

载体坐标系和导航坐标系之间的转换关系可以表示为:

(1)

用四元数q=q0+q1i+q2j+q3k表示坐标系的转换,则旋转矩阵可表示为:

(2)

(3)

式中:Ω(ω)表示载体坐标系相对于导航坐标系角速度在载体坐标系上分量的四元数。矩阵形式可以表示为:

(4)

通常假设在时间间隔(t,t+ΔT)内,角速度ω是一个固定值,其中ΔT是旋转四元数更新时间间隔。因此可以获得式(3)对应的离散时间模型:

(5)

利用陀螺仪输出的角速度数据,结合式(5)对四元数进行更新,从而获得旋转矩阵。

2 双级互补滤波姿态融合算法

首先,利用上述更新的四元数获得重力矢量和磁场矢量在载体坐标系下的计算值:

(6)

而加速度计和磁力计的实际测量矢量为:

(7)

2.1 一级互补滤波算法设计

第一级互补滤波利用加速度计的测量数据对陀螺仪估计的四元数进行补偿修正,输出准确的水平姿态。如图1所示,通过将k时刻由式(5)计算的四元数qk绕向量na旋转角度Δθa来补偿水平角误差。因此,相应的误差四元数qae和修正四元数qa可以由下式获得:

(8)

图1 利用加速度计测量值补偿估计的重力矢量

在静止状态下,加速度计通过测量由于重力引起的加速度,可以准确地计算出载体的水平姿态角。但是,当载体存在线性加速度时,加速度计测得的加速度矢量的大小和方向与重力矢量存在偏差,利用加速度计输出值计算载体姿态角将会出现比较大的误差。然而,陀螺仪的测量值不会受线性加速度的影响,因此在这种情况下应当把陀螺仪数据作为估计的主要来源,用来进行相对准确的姿态估计。为了解决这一问题,本文在式(8)中引入自适应增益系数μa来减少载体非重力加速度误差对姿态测量精度的影响。

首先,定义载体非重力加速度误差ea由下式获得:

(9)

式中:‖gb‖为加速度计测得的加速度矢量模值,g为当地重力加速度。滤波增益系数μa与载体非重力加速度误差ea之间的关系由下式表示:

μa=f(ea)

(10)

式中:f为分段连续函数,如图2所示。当载体线性加速度很小且非重力加速度误差不大于事先设定的阈值xa,滤波增益系数随着非重力加速度误差的增加而线性减小。如果载体存在较大的线性加速度,非重力加速度误差大于阈值,那么滤波增益系数等于零。误差阈值 的大小可通过衡量惯性器件各自的精度、载体的动态强度等条件,由试验获得最优值。

图2 滤波增益系数与非重力加速度误差的关系

2.2 二级互补滤波算法设计

第二级互补滤波利用磁力计的测量数据对上一级获得的四元数进行补偿,来修正航向角。在第一级工作的基础上,利用磁力计测量值求得磁场在导航坐标系下的估计矢量:

(11)

忽略垂直分量,得

(12)

如图3所示,通过将qa绕nm旋转小角Δθm来补偿偏航角误差,因此,相应的误差四元数qme和补偿四元数qm可以由下式获得:

(13)

式中:

mn=(0 1 0)为导航坐标系下忽略垂直分量的地磁参考矢量。滤波增益系数μm的获取办法与上述μa获取办法类似,在此不再赘述。

图3 利用磁力计测量值补偿估计的磁场矢量

由上述可以看出,航向角误差四元数的旋转轴nm与坐标轴z轴重合,也就是说第二级补偿的四元数为上一级获得的四元数仅绕z轴旋转获得,即使在磁场受到干扰的情况下也不会影响水平角的估计。

2.3 算法总体设计

根据上面的分析,设计的双级互补滤波算法流程图如图4所示。

图4 双级互补滤波姿态测量算法流程图

3 实验验证

利用实验室现有的惯性测量系统对算法进行验证。惯性测量单元包含三个单轴CRM100 MEMS陀螺仪、三个单轴MS9000 MEMS加速度计、一个三轴HMC1043L AMR磁力计和一个采集存储单元。内部微处理器实时采集并存储传感器测量的原始数据。本文采用MATLAB设计所提滤波器,并对原始数据进行处理,来获得载体的姿态信息。设计了2种实验方式来验证算法的有效性:磁干扰静态试验;跑车动态试验。第一组试验用来验证算法能否有效避免磁干扰对水平姿态的影响;第二组试验用来验证是否能有效消除载体线性加速度的影响。数据处理时算法中的误差阈值取为xa=xm=0.2。

图5 磁干扰实验姿态角对比图

3.1 磁干扰静态实验

将测量单元水平静止放置,拿一根铁棒在不同时间段内靠近测量单元,以此来模拟磁场受到干扰的情况。将采集的数据用不同算法处理结果如图5所示。

在图5中可以看出,惯性测量单元分别在5 s～13 s 和22 s～29 s两个时间段内受到了磁场的干扰。利用Mahony算法解算出来的航向角在磁干扰的情况下产生了15°左右的误差,同时也给俯仰/滚动角带来了不同程度的影响。相比于Mahony算法,双级互补滤波姿态测量算法在磁场受到干扰的情况下,有效降低了磁干扰对航向角的影响,同时俯仰/滚动角完全不受航向角误差的影响,性能明显优于Mahony算法。在没有磁干扰的情况下,所提算法可以输出稳定可靠的三维姿态角信息,误差均小于0.2°。

3.2 车载动态实验

为验证算法实时性以及能否有效消除载体线性加速度的影响,设计了跑车动态实验。采用加拿大NovAtel公司高精度光纤组合导航系统span-lci为参考基准,考核算法的姿态精度。将系统与高精度母惯导捷联安装在与车体固联的测试工装上进行跑车实验,两套系统同时采集车体的运动信息。跑车现场图如图6所示。

采集到的加速度矢量的模值如图7所示,从图中可以看出,载体存在线性加速度。将融合滤波后输出的三轴姿态角信息与母惯导输出的姿态角相比较,对比图如图8所示。实验系统与母惯导之间的姿态角误差如图9所示。

图7 跑车试验加速度矢量模值

从图8中可以看出,本文所提双级互补滤波融合算法得到的姿态角与高精度母惯导提供的姿态信息吻合度较高,并且具备对动态变化的实时跟踪性能。对比Mahony算法,本文所提算法由于引入的自适应滤波增益而有效得减小了载体线性加速度对姿态测量精度的影响,精度得到了明显的提升。从图9姿态角误差曲线可以看出,由于磁力计容易受到干扰,航向角误差在三维姿态角中误差最大,达到5°左右。俯仰角与滚动角误差相对较小,俯仰角误差小于2°,滚动角误差小于3°,误差曲线没有发散趋势。

图8 三维姿态角对比图

图9 三维姿态角误差曲线

4 结论

本文采用双级互补滤波算法对MARG传感器的数据进行融合,实现了三自由度的姿态测量。与传统的Mahony互补滤波算法相比较,采用双级互补滤波融合算法将水平与航向姿态分级融合修正,消除了磁干扰情况下航向角误差对水平角测量的影响。同时,针对载体非重力加速度误差和磁干扰误差引入两个独立的滤波增益系数,可分别由加速度计与磁力计不同频段的噪声自适应调节,大大提高了姿态角测量精度。基于惯性测量单元的静态、动态实验结果表明:本文所提算法能够有效的解决磁场干扰和载体线性加速度对姿态测量的影响,显著提高了三维姿态角的测量精度。