捷联惯导互补滤波姿态融合算法设计

杜 瑾,赵华超,郑 哲,王 祥,司迎利

(中国空空导弹研究院,河南 洛阳 471000)

捷联式惯导是当今惯性导航的主流发展方向。捷联算法的核心是捷联姿态算法,姿态角的解算精度直接影响到整个系统的精度。由于本身成本低、功耗小,MEMS器件被广泛应用于姿态测量领域[1]。由于MEMS陀螺存在漂移,长时间工作有累积误差,需要借助其他传感器的辅助才能完成姿态测量。常用的辅助传感器有加速度计、磁阻传感器等[2-3]。加速度计和磁阻传感器在静态和无外磁场干扰情况下均能表现出良好的测量性能,然而,在动态和有磁场干扰的情况下,这两种传感器的测量精度会受到严重影响[4]。

可以看出,单独使用某一种传感器不能获得精确可靠的姿态信息,必须利用姿态融合算法对不同传感器的数据进行融合,提高系统的精度和抗干扰能力。常用的融合算法有扩展卡尔曼滤波(EKF),无迹卡尔曼滤波(UKF),它们均从时域出发,对系统噪声精确建模[5-6]。这两种方法计算量大,对MCU主频要求高且模型建立比较复杂[7]。互补滤波(CF)与卡尔曼滤波相比,具有简单方便,计算量小,易于工程实现等优点[8]。

本文提出一种基于方向余弦矩阵(DCM)的多轴互补滤波姿态融合算法,利用加速度计和磁阻传感器良好的静态性能补偿MEMS陀螺仪的漂移误差。设计了静态试验与动态试验,分别验证了本算法的性能。

1 坐标系描述

定义2个坐标系:载体系(b系)与导航系(n系)。导航坐标系(n系)采用北,东,地坐标系,符合右手法则。载体坐标系固联于运载体,xb,yb,zb分别代表了运载体的前进方向,右侧和正下方。

2 方向余弦矩阵(DCM)

有许多方法可以描述运动坐标系相对于参考坐标系的方位关系,例如欧拉角法,四元数法和方向余弦矩阵(DCM)[9]。用DCM求解姿态角与四元数法相比更加直观,可避免欧拉角法中大角度运动时的奇异性,实现全姿态工作[10]。从载体系到导航系的转化可以通过三次旋转实现,依次绕z,x,y得到航向角ψ,滚动角φ和俯仰角θ。通过三次旋转,n系和b系的关系可以通过一个矩阵表达(见式(1)),即DCM。

(1)

DCM具有正交性和单位性两大特征:矩阵中任意一对行或一对列向量都是相互垂直的,且内积为零[11]。DCM是正交矩阵;DCM的任意列向量或行向量都是单位向量。

3 DCM的更新与姿态角的提取

当时间间隔t趋向于无穷小时,航向,俯仰,滚转角度的变化dψ、dθ、dφ趋向于0,此时,转动与次序无关。DCM随时间更新的关系式如下:

(2)

当dt很小时,得到DCM的导数:

(3)

DCM矩阵的更新过程如图1所示。

图1 DCM矩阵更新流程图

得到DCM后,很容易推导出俯仰角,航向角,滚转角的表达式:

每年国家及省级财政拨付资金用于蔬菜产业的发展以及从政策上对农村农民减免和补贴，但是部分地区还处于比较落后的贫困村，农民自身无资金发展蔬菜产业，县级财政也是捉襟见肘，无法配套及自筹剩余款项，对于落后的民族地区，以及薄弱的农业基础条件，无法起到很好的效果。而且金融信贷体系的不完善以及乡镇农技站资金欠缺和技术人员不足，很难适应目前的新型农民要求。在蔬菜种植的铁盖乡、龙羊峡镇等地区，农业综合服务中心没能起到争取省州县资金，依靠等、“喂”的消极观念，人员配备大多数是非专业人员。

(4)

为了保证解算出的姿态角的单值性,必须对3个姿态角的范围做出限定。具体角度取舍过程不再赘述。

4 互补滤波姿态融合

由于正交性和单位性的约束,DCM的行,列向量都是单位向量,且任意一对行或列向量都是相互垂直的。然而,在DCM不断更新的过程中,由于数值计算舍入误差的累积,MEMS陀螺仪存在漂移等各种原因,导致DCM的边长增长或缩短,不再是等长的单位向量,使得原本正交的矩阵倾斜,如图2所示。

图2 DCM非正交型变示意图

DCM的型变势必会影响到姿态角的解算精度,对于同一确定的向量,用不同坐标系表示时,它的模值和方向一定是相同的。正如前面所说,DCM存在偏差,所以该已知向量经过旋转得到的在新坐标系下的向量一定也存在误差,通过计算这个偏差来修正旋转矩阵,从而达到修正姿态角的目的。

本文选取两个方向基准向量:一个是通过加速度计测得的重力场向量,另一个是通过电子磁罗盘测得的航向向量。方向基准向量与通过DCM矩阵转化得到的向量表示的是理论上同一方向的向量,它们的外积定义为我们所求取的误差向量。该外积的模值恰好与两个向量所呈夹角的大小成正比。

4.1 利用重力场计算俯仰角、滚转角的误差向量

令[vx,vy,vz]T为重力场向量[0,0,1]T在b系下的投影向量。由DCM可推出:

(5)

这里求出的重力场在b系下的投影[vx,vy,vz]T,就是我们在上文中提出的方向基准向量。现在,需要求出与该基准向量相关且同向的另一个向量,与基准向量作叉乘,求出误差向量。

(6)

式中,离心加速度的计算公式如下:

(7)

(8)

(9)

4.2 利用地磁场计算航向角的误差向量

地球表面的磁场是从地球磁南极出发,回到磁北极。测量的地磁场水平分量来计算载体与磁北极之间的夹角[12-13]。在北东地坐标系下,令x轴对准北向,显然,y轴没有分量,磁场强度为零。设x,z轴的磁场为bx,bz,与上文中用重力场作为基准向量修正加速度计的方法类似,这里,我们选取n系磁阻传感器的理论值[bx0bz]T为基准向量,计算误差向量来修正航向角。

当载体处于任意姿态时,通过坐标转换矩阵,磁阻传感器在b系下的输出[mx,my,mz]T转换到水平坐标系,转换过程如下:

在n系XOZ平面上,磁阻传感器测得的向量模值一定相同,所以有:

(10)

Z轴上,两种方式得到的向量大小相等,有bz=hz。现在,我们得到了n系磁阻传感器的理论值[bx0bz]T,由该向量再反推回b系得到向量[mbx,mby,mbz]T,计算过程如下:

(11)

至此,用来修正航向角的误差向量可以由[mx,my,mzT]和[mbx,mby,mbz]T的外积算出。记该误差向量为erroryaw,其表达式为:

(12)

综上,总的误差向量为e=[ex,ey,ez]T,其大小为:

e=errorpitch,roll+erroryaw

(13)

4.3 多轴互补滤波器设计与融合策略

一阶低通滤波器和一阶高通滤波器的传递函数模型分别为:

(14)

(15)

可以看出,互补滤波器能够有效滤除低频和高频噪声的干扰,得到接近真实值的重构信号。

以误差向量errorpitch,roll,erroryaw作为滤波器的输入,输出陀螺仪角速率测量值的修正量。输入的误差向量为e=[ex,ey,ez]T,经过PI调节系统输出可表示为:

(16)

修正后的陀螺角速率ω的表达式为:ω=ωg+Ku(k),ωg为陀螺仪测量到的角速率,Kp为比例环节系数,其大小决定了滤波器的截止频率和响应速度,Ki为积分环节系数,积分环节的加入有利于消除系统的稳态误差。显然,这种互补滤波的形式与一般互补滤波不同,其中加入了比例积分环节,整个系统是一个二阶系统,常称其为显式互补滤波[15]ECF(Explicit Complementary Filter)。ECF滤波器的结构示意图如图3所示。

合适滤波器参数的选取就等价于确定滤波器的截止频率。截止频率取的低,滤波结果依赖陀螺多一点;取得高,依赖加计、磁阻传感器多一点。由于实际应用中由于震动等因素的影响,高频噪声干扰较多,互补滤波器截止频率应在合理范围内尽量选取较小的值,有效去除加速度计和磁阻传感器的噪声影响。具体参数需根据实际应用中的噪声水平确定。

图3 ECF滤波器结构示意图

5 实验与分析

5.1 转台静态实验

转台静态实验是为了验证在姿态测量系统稳定放置的静态条件下,姿态角的解算精度。本文利用高精度三轴位置速率转台为静态实验的平台,通过设定程序,控制转台转动,并在不同位置稳定,使系统采集到不同角度下稳定状态时的传感器输出。以转台实时反馈的角度信息为参考值,通过将本算法解算得到的姿态角信息与其相比较,验证算法的精度及可靠性。静态实验的实物图如图4所示。

图4 静态试验实物图

实验分两步进行,第1步将系统安装在转台上,令转台转动,使俯仰角分别稳定在0°,30°,45°,60°,75°,0°,-30°,-60°,0°。第2步将转台三轴回零,使系统滚转角分别稳定在相同的角度,将传感器数据存储在系统内部FLASH中。由于转台为铁质,地磁传感器受到影响,所以没有进行航向角测试。利用本文提出的基于互补滤波的姿态算法(图5中ECF解算)与转台反馈的角度值相比对,将后者视为参考基准,对比图如图5所示。

根据国军标GJB 729-1989,静态实验的姿态角精度评定标准如下,即

(17)

式中,RMS为姿态角误差的平均平方均方根值,ΔEi,j为第i次实验第j个采样时刻的姿态角误差,mi为第i次实验的数据采样点数,n为有效实验的次数。由以上公式计算得到的RMSpitch为0.114 8°,RMSroll为0.085 9°,可见当载体处于平稳放置状态下时,姿态角解算精度高,姿态角误差小于0.2°。

图5 姿态角解算系统静态实验

5.2 系统动态跑车试验

为了验证在动态条件下的姿态角解算精度,以及静态实验中未能验证的航向角精度,本文设计了以高精度母惯导为参考基准的动态跑车试验。由于所用陀螺仪精度有限,所以将高精度母惯导输出的三轴姿态角传递给航姿系统,进行初始对准,作为航姿系统的初始姿态角。将系统与高精度母惯导并排安装在与车体固联的测试工装上,开动试验车,两套系统同时采集车体的运动信息,试验车开车到停止的间隔时间约为180 s。跑车试验现场图如图6所示。

图6 车载实验实物图

将测试时间内试验系统输出的三轴姿态角信息与母惯导输出的姿态角相比较,对比图如图7所示,试验系统与母惯导之间的姿态角误差如图8所示。

图7 姿态角对比图

从图7中姿态角参考值与本算法的计算结果相对比可以看出,由于MEMS陀螺仪漂移导致姿态角发散的情况得到了很好的抑制。在跑车实验的动态环境下,本文提出的ECF算法得到的姿态角与高精度母惯导提供的姿态信息吻合度很高,并且具备对动态变化的实时跟踪性能。从图8姿态角误差曲线可以看出,由于航向角需要地磁传感器提供信息,本身容易受到干扰,航向角误差在三轴姿态角中误差最大,达到4°左右,但后期航向角误差发散得到抑制,误差值稳定。滚转角与俯仰角误差相对较小,滚转角误差小于2.3°,俯仰角误差小于3.1°,误差曲线没有发散的趋势。由以上实验数据可算得航向角、滚转角、俯仰角的均方根误差(RMSE)分别为3.268 2°、1.325 7°、1.595 4°。

图8 姿态角误差曲线

6 结论

为了满足低成本、高性能的载体姿态测量系统的迫切需求,针对MEMS陀螺仪漂移导致姿态无法准确测量的难题,提出了基于方向余弦矩阵更新的互补滤波姿态融合方法(ECF)。该滤波算法既保持了经典互补滤波的简洁形式,又在原理上运用了自动控制中的PI调节。具有运行计算量小、占用内存空间少、稳定性好等突出优点。从滤波效果上来看,选取合适的参数可使本算法在静态及动态的应用环境中均取得不错的效果。因此本文提出的姿态融和算法具有较高的工程应用价值。