基于5阶降维平方根-容积卡尔曼滤波的动基座对准应用研究

黄湘远,汤霞清,武萌,吴伟胜(装甲兵工程学院控制工程系,北京100072)



基于5阶降维平方根-容积卡尔曼滤波的动基座对准应用研究

黄湘远,汤霞清,武萌,吴伟胜
(装甲兵工程学院控制工程系,北京100072)

摘要:为提高动基座下捷联惯导系统的对准精度、数值稳定性和减小计算量,将5阶容积卡尔曼滤波(CKF)、降维算法、多次离散和平方根(SR)滤波结合起来,形成5阶降维SR-CKF非线性对准方案。为减小5阶CKF的计算量,建立非线性-线性分离的系统模型,引入降维算法;为提高1阶龙格-库塔法的逼近精度,设计多次离散和时间更新的滤波框架;为提高数值稳定性,推导了5阶降维SR-CKF;比较常规3阶SR-CKF、5阶CKF和5阶降维SR-CKF的各项特性。实车动基座对准实验结果表明:该方案对准精度高、数值稳定性强、计算量小,满足应用需要。

关键词:兵器科学与技术;容积卡尔曼滤波;降维;平方根滤波;多次离散

0　引言

由于准备时间缩短、使用环境苛刻、应用功能拓展等各方面需求,捷联惯导系统(SINS)动基座非线性对准受到了越来越多的关注。

容积卡尔曼滤波[1](CKF)计算量小,数值稳定性强,精度较高,一经提出获得了大量应用[2-3]。为了提高CKF的滤波精度,出现了正交采样5阶CKF[4-5]和非正交简化采样5阶CKF[6]。系统维数较大时,5阶算法计算量急剧增加,数值稳定性变差。

为了减小非线性滤波的计算量,文献[7-8]分别使用3阶降维CKF和边缘采样无迹卡尔曼滤波(UKF),只对非线性部分进行采样,文献[9-10]设计了容积卡尔曼滤波-卡尔曼(CKF-Kalman)和扩展CKF-Kalman组合滤波方案,将非线性部分和线性部分分开处理。本文将降维滤波引入到正交采样5阶CKF,形成5阶降维CKF.实际应用中,需对非线性系统进行离散,常用4阶龙格-库塔离散算法;降维方案中,只能进行1阶离散,为了避免滤波精度降低,设计了一种多次离散的方式。

SINS/北斗2代卫星导航系统(BD2)非线性对准中,常规3阶CKF和5阶降维CKF是绝对数值稳定的,5阶CKF不是数值稳定的。绝对数值稳定并不代表滤波器一定稳定,受计算机长度和算法精度的限制,方差阵Pk也可能失去正定,导致Cholesky分解失败,滤波失去稳定。增强滤波稳定性的方式有平方根(SR)滤波[11-12],奇异值分解(SVD)代替Cholesky分解[13]等。SR算法不需进行Cholesky分解,数值稳定性最好。本文推导了5阶降维SRCKF,有效提高了非线性对准的稳定性。

为了提高SINS的动基座对准精度、数值稳定性和降低计算量,本文将5阶CKF、降维滤波、多次离散和SR滤波结合起来,推导了5阶降维SR-CKF,进行了实验验证。结果表明该方案大失准角下对准精度高,数值稳定性强,计算量小,综合性能优于常规3阶CKF,满足应用需要,具有重要的工程应用价值。

1　欧拉角误差的非线性对准模型

1.1大失准角下非线性对准模型

记地心惯性坐标系为i系;地球系为e系;导航系n系为东北天(OENU)坐标系;计算平台系为p系;载体系b系为右前上(Oχyz)坐标系。

n系到b系的旋转角度为航向角ψ、俯仰角θ和横滚角γ,姿态矩阵为Cnb.n系到p系的旋转角度为失准角ΦU、ΦE和ΦN,记Φ= (ΦE,ΦN,ΦU)T.载体速度vn= (υE,υN,υU)T,速度误差δvn= (δυE,δυN,δυU)T,纬度L、经度λ和高度h,位置误差δL,δλ,δh.陀螺测量误差δωbib,加速度计测量误差δfb.

大失准角下,基于欧拉角误差的姿态、速度和位置误差[14]为

式中:Cpn、Cω-1、δωnie、δωnen等见文献[14]。模型要求纬度误差δL为小量。

陆用导航一般忽略高度通道δh和δυU.对准过程中,需对陀螺和加速度计误差进行估计和补偿,可将陀螺误差δωbib近似为常值零漂εb加白噪声wbg,加速度计误差δfb近似为常值零偏Δb加白噪声wba, 即

将Δb和εb扩充为系统状态,状态χ、系统噪声w和观测量z分别取为

式中:vnI、LI、λI等为SINS的解算值;vnB、LB、λB等为BD2的测量值。

大失准角下初始对准的非线性系统为

式中:f(χ)和G可由(1)式推导;H = [04×3,I4×4, 04×5];v为测量噪声。

1.2非线性-线性分离模型

为简化(4)式,静基座下可不进行位置更新,动基座下可通过外界辅助信息(如BD2提供位置、速度)形成阻尼误差模型[14],缺点是要求BD2连续稳定。

SINS/ BD2组合导航或动基座对准中,位置和速度可观测度高,短时间内能够有效估计。为了保证位置误差δL作为小量,可通过位置、速度间歇性闭环反馈来抑制速度和位置误差的快速发散。误差模型(1)式可改写为

(5)式中,Cω-1、Cpn和Cpb为与失准角Φ有关的矩阵,A1、A2、B1、B2、C1和C2均与Φ无关。非线性系统中非线性因素由Φ引起,与其他变量无关,可将状态量χ分解成非线性部分α和线性部分β,即χ= [αT,βT]T,α=Φ,β= [δvT,δpT, (Δb)T, (εb)T]T.

从而可将非线性系统(4)式改写为如下非线性-线性分离的框架:

式中:F(α)为与α相关的状态转移矩阵;g(α)为与α相关的多维非线性函数。

常规系统将所有状态均定义为非线性状态,其维数为12,非线性-线性分离系统中非线性状态只有失准角Φ,维数为3,线性状态维数为9.

2　多次离散的降维滤波

为了降低滤波计算量,非线性-线性分离系统中可使用降维CKF[7]。CKF等高斯非线性滤波使用如下的Guass-Bayes最优滤波框架[15-16]:

2.13阶降维CKF

考虑如下非线性离散系统:

式中:αk-1为χk-1的前m个元素;系统噪声wk～N(wk;0,Qk);观测噪声vk～N(vk;0,Rk).

定理[7]已知n维随机变量χ～N(χ;,Pχ),变量α为χ的前m个分量,即α= [χ1,…,χm]T,则因变量y = F(α)χ+ g(α)的期望,方差Py只与随机变量α相关,值为

式中:Pα为Pχ的前m行、m列子矩阵;Rn为积分区域;Sχ、Sα分别为Pχ和Pα的Cholesky分解阵,且

根据定理和3阶CKF,可推导如下3阶降维CKF算法:

1)时间更新。

步骤1 容积采样。对Pk-1进行Cholesky分解,即Pk-1= Sk-1STk-1;取为k-1的前m个变量, Sα为Sk-1的前m行、m列子矩阵。令i =1,…,2m, 且

步骤2 计算容积传播点:

2)量测更新。

步骤5 状态更新:

3阶降维CKF从χk-1的统计特性(k-1,Pk-1)中提取α的统计特性(,Pα),进行2m次采样并经过相关变换求得k|k-1和Pk|k-1.相比于3阶CKF 的2n采样,减少了2(n-m)个,减轻了计算负担。

2.2连续系统的多次离散

实际应用中需对连续系统进行离散,常用4阶龙格库塔算法。然而, (6)式经4阶离散无法得到离散形式(8)式,难以直接应用降维CKF.经1阶龙格库塔离散能得到该形式,但离散精度会降低,导致滤波性能下降,需减小滤波周期才能保证滤波精度。然而滤波周期受到系统观测周期的限制,不一定能够满足滤波要求。为了解决该问题,可使用多次离散的方案。

将滤波周期T分成nt个滤波子周期Δt,在子周期k + jΔt时刻上利用k + (j-1)Δt对系统进行1阶离散和时间更新,获得状态预测k + jΔt.当获得新观测值zk +1时,进行量测更新,获得k + 1时刻状态估计k +1.具体算法如下:

步骤1 k + jΔt时,进行1阶龙格-库塔离散, 即

步骤3 如果j＜nt,令k + jΔt=k + jΔt|k + (j-1)Δt和方差阵Pk + jΔt= Pk + jΔt|k + (j-1)Δt;如果j = nt,利用观测量zk +1进行量测更新,获得估计值k +1和Pk +1.

3　5阶降维SR-CKF算法

式中:f(·)为非线性函数。

令χ= ry,yTy = 1,r∈[0,∞),积分式I(f)经spherical-radial变换可分离为spherical积分S(r)和radial积分R两部分,为

式中:Un为n维单位球面;σ(·)为Un上的元素。

3.1两种5阶CKF性能比较

文献[17]总结了S(r)的多种5阶多项式逼近形式,文献[4,6]采用不同形式构建了不同的5阶spherical规则,采用计算量最小的5阶radial规则,形成5阶正交采样CKF和5阶非正交简化采样CKF两种形式。

数值稳定性上,通过计算积分计算的稳定因子I[18],表明常规非线性对准中,由于非线性状态维数n =12,两种5阶CKF均不是绝对数值稳定的,方差阵Pk易失去正定性,精度有限甚至导致滤波失败。非线性-线性分离对准中,非线性状态维数m = 3,两种5阶CKF均是绝对数值稳定的。

计算量上,时间更新中,5阶正交采样CKF需采样2n2+1次,5阶非正交简化采样CKF需采样n2+ 3n +4次。常规非线性对准中,后者计算量较小;非线性-线性分离对准中,前者计算量较小。

数值稳定性和计算量分析表明,非线性-线性分离对准中,5阶正交采样CKF具有一定优势。

3.25阶降维CKF

1)时间更新。

步骤1 容积采样。对Pk-1进行Cholesky分解,Pk-1= Sk-1STk-1;取为k-1的前m个变量,Sα为Sk-1的前m行、m列子矩阵。令i =1,…,2m, j = 1,…,2m(m-1),且

式中:[e]i为集合的第i个列向量,{e}mt =1= {[1,0,…,0]T,[0,1,…,0]T,…, [0, 0,…, 1 ]T; [ s ]j为集合,的第j列向量,= {(ek±el) /2;k,l =1,…,m;k＜l}.

步骤2 计算容积传播点:

2)量测更新。

由于观测方程为线性方程,量测更新与降维3阶CKF相同。

3.35阶降维SR-CKF

5阶降维SR-CKF中,需进行方差阵Pk的Cholesky分解,计算量大,易出错,数据处理精度受计算机字长限制。SR滤波利用矩阵QR分解代替Cholesky分解,降低计算机字长对精度的影响,提高滤波的数值稳定性。

结合SR-UKF[18]、3阶SR-CKF[1]以及线性观测SR-CKF[12],在5阶降维CKF的基础上可推导5阶降维SR-CKF,具体如下:

1)时间更新。

步骤1 容积采样,与5阶降维CKF相同。

步骤2 计算容积传播点:

式中:Θ(α) = F(α)Sχ

式中:S = qr(A)为[Q,R]= QR(AT),S = RT,QR(·)为矩阵QR分解[1]。S = cholupdate(S,u,±υ)为平方根矩阵S的一次Cholesky分解更新[18]。 2)量测更新。

步骤5 状态更新:

3.4算法性能分析

常规非线性对准利用4阶龙格-库塔对12维非线性系统进行离散,使用3阶SR-CKF和5阶CKF 在1次滤波周期内进行1次时间更新和1次量测更新。

本文采用非线性-线性分离对准中,1次滤波周期内,使用1阶龙格-库塔进行4次系统离散,使用5阶降维SR-CKF进行4次时间更新和1次量测更新。

计算量上,常规3阶SR-CKF需进行1次矩阵QR分解和2n = 24次采样,每次采样进行4次(共96次)非线性函数计算;5阶CKF需进行1次矩阵SVD分解和2n2+ 1 = 289次采样,每次采样进行4次(共1 156次)非线性函数计算;5阶降维SRCKF需进行4次QR分解和4次Cholupdate更新,进行4(2m2+ 1) = 40次采样,每次采样进行1次(共40次)非线性函数计算。由于矩阵QR分解和Cholupdate更新的计算量大于非线性函数计算,计算量上5阶降维SR-CKF相对3阶SR-CKF具有一定的劣势,相对于5阶CKF计算量大大降低。

滤波精度上,常规3阶SR-CKF的逼近精度为3阶,5阶降维SR-CKF为5阶精度,后者滤波精度高于前者。

4　实验验证

实验室将某型光纤陀螺SINS安装在某战车上,行驶过程中使用高精度BD2导航芯片提供速度、位置参考信号。陀螺零偏稳定性小于0.02°/ h,加速度计偏值重复性小于5×10-5g,BD2的位置精度为10 m,速度精度为0.1 m/ s.静基座下初始对准10 min,开始行驶并进行SINS/ BD2组合导航,行驶过程包括加速、匀速、转弯、上下坡、颠簸路面等。

使用开始跑车时的1 000 s数据进行动基座对准,分别进行常规3阶SR-CKF非线性对准、5阶CKF非线性对准和5阶降维SR-CKF非线性-线性分离对准。实验过程中由于无法获得准确的俯仰、横滚和方位角,使用SINS/ BD2组合导航结果作为参考姿态。

实验步骤:静基座下完成初始对准,系统开始进入SINS/ BD2组合导航,实验过程中存储所有数据。车辆开始行进时,在参考姿态的基础上设置4组不同的失准角进行离线对准实验,失准角分别为Φ1= (1°,1°,1°)、Φ2= (1°,1°,10°)、Φ3= (10°,10°, 30°)和Φ4= (15°,15°,50°)。每次跑车实验分别进行4个非线性对准实验,每个对准实验分别使用3阶SR-CKF、5阶CKF、5阶降维SR-CKF等3种方法进行对准。总共进行6次跑车实验,将同一对准算法和同一失准角下的6次实验的方位角均方根误差作为该算法在该失准角下对准精度评价指标。

由于5阶CKF数值稳定性较差,采用协方差矩阵的SVD分解代替常规的Cholesky分解[13]。

图1～图4为6次动基座对准中方位对准均方根误差曲线,表1给出了对准结束时方位对准的均方根误差。均方根误差越大,对准精度越差。图1表明小失准角下3种算法的对准结果相当;图2表明方位失准角为10°时,三者的对准结果没有太大区别。图3和图4为方位角较大时的对准结果,此时三者有了一定的区别。

图1　失准角1下方位角均方根误差Fig.1 Root mean square error of azimuth angle at misalignment angle 1

图2　失准角2下方位角均方根误差Fig.2 Root mean square error of azimuth angle at misalignment angle 2

图3　失准角3下方位角均方根误差Fig.3 Root mean square error of azimuth angle atmisalignment angle 3

图4　失准角4下方位角均方根误差Fig.4 Root mean square error of azimuth angle at misalignment angle 4

表1　不同失准角下方位对准均方根误差Tab.1 Root mean square errors of azimuth angle at misalignment angles　(°)

对准速度上,3阶SR-CKF和两种5阶CKF算法相比,误差曲线收敛速度趋势大致相似,严格意义上来说5阶CKF并不能加快对准速度,这是因为对准速度由系统可观测决定,高阶滤波算法并不能改变系统的可观测性。当失准角较大时,但5阶CKF能够使误差收敛到同一范围内的速度快于3阶SRCKF,从某种意义上讲5阶CKF加快了滤波速度。

对准精度上,基于3阶SR-CKF和5阶CKF对准均采用基于高阶离散的滤波框架,5阶CKF误差较小表明5阶CKF的滤波精度高于3阶SR-CKF,由表1可知失准角越大优势越明显。5阶CKF和5阶降维SR-CKF算法采用不同的滤波框架,二者误差曲线类似,表明5阶降维SR-CKF多次离散方案和5阶CKF高阶离散方案精度相当。

滤波计算量和数值稳定性前文做了比较详细的分析。综上,如果能够将初始失准角控制10°左右,建议选择常规3阶SR-CKF完成对准;当失准角较大时,应使用5阶降维SR-CKF.

图1～图4表明,随着失准角增大,3种算法的对准结果变差。这是因为系统非线性程度随着失准角增大而变强,龙格-库塔离散的逼近精度和非线性滤波的估计精度会逐渐降低。因此条件允许下,应尽量保证失准角足够小。当无法确定失准角的大致范围时,建议选择5阶降维SR-CKF.

5　结论

为了提高动基座下SINS的对准精度、数值稳定性和降低计算量,本文推导了非线性-线性分离的系统结构;引入了降维CKF,为了避免1阶龙格-库塔离散造成滤波精度降低而设计了多次离散滤波;为了提高滤波精度和数值稳定性,推导了适合于动基座对准的5阶降维SR-CKF;对常规3阶SR-CKF高阶离散、5阶CKF高阶离散和5阶降维SR-CKF多次离散对准进行了综合比较和实验验证。5阶降维SR-CKF多次离散方案对准精度高,数值稳定性强,计算量较小,对失准角大小具有较强的鲁棒性。

参考文献(References)

[1]Arasaratnam I, Haykin S.Cubature Kalman filters [ J].IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[2]Ge Q b, Xu D X, Wen C L.Cubature information filters with correlated noises and their applications in decentralized fusion [J].Signal Processing, 2014, 94: 434-444.

[3]Gadsden S A, Al-Shabi M, Arasaratnam I, et al.Combined cubature Kalman and smooth variable structure filtering: a robust nonlinear estimation strategy [J].Signal Process, 2014, 96:290-299.

[4]Jia B, Xin M, Cheng Y.High-degree cubature Kalman filter [J].Automatica, 2013, 49(2): 510-518.

[5]Jia B, Xin M.Rauch-Tung-Striebel high-degree cubature Kalman smoother [ C]∥Proceddings of American Control Conference.Washington, DC, US:the American Automatic Control Council, 2013:2472-2477.

[6]Wang S Y, Feng J C, Tse C K.Spherical simplex-radial cubature Kalman filter [ J].IEEE Signal Processing Letters, 2014, 21(1): 43-46.

[7]钱华明,葛磊,黄蔚,等.降维CKF算法及其在SINS初始对准中的应用[J].系统工程与电子技术, 2013, 35(7):1492-1497.QIAN Hua-ming, GE Lei, HUANG Wei, et al.Reduced dimension CKF algorithm and its application in SINS initial alignment [J].Systems Engineering and Electronics, 2013, 35(7): 1492-1497.(in Chinese)

[8]李方能,许江宁,亓洪标.基于边缘采样UKF滤波的捷联惯导初始对准方法[J].中国惯性技术学报, 2014, 22(5): 612-618.LI Fang-neng, XU Jiang-ning, QI Hong-biao.Initial alignment of strapdown inertial navigation system based on marginalized unscented Kalman filter[J].Journal of Chinese Inertial Technology, 2014, 22(5):612-618.(in Chinese)

[9]孙枫,唐李军.Cubature卡尔曼滤波-卡尔曼滤波算法[J].控制与决策, 2012, 27(10): 1561-1565.SUN Feng, TANG Li-jun.Cubature Kalman filter-Kalman filter algorithm [J].Control and Decision, 2012, 27 (10): 1561-1565.(in Chinese)

[10]赵曦晶,刘光斌,汪立新,等.扩展容积卡尔曼滤波-卡尔曼滤波组合算法[J].红外与激光工程, 2014,43(2): 647-653.ZHAO Xi-jing, LIU Guang-bin, WANG Li-xin, et al.Augmented cubature Kalman filter/ Kalman filter integrated algorithm [J].Infrared and Laser Engineering, 2014, 43(2): 647-653.(in Chinese)

[11]Chandra K P B, Gu D W, Postlethwaite I.Square root cubature information filter[J].IEEE Sensors Journal, 2013, 13(2): 750-758.

[12]Wang S Y, Feng J C, Tse C K.Novel cubature Kalman filtering for systems involving nonlinear states and linear measurements [J].International Journal of Electronics and Communications, 2015, 69: 314-320.

[13]张秋昭,张书毕,刘志平,等.基于奇异值分解的鲁棒容积卡尔曼滤波及其在组合导航中的应用[J].控制与决策,2014, 29(2): 341-346 ZHANG Qiu-zhao, ZHANG Shu-bi, LIU Zhi-ping, et al.Robust cubature Kalman filter based on SVD and its application to integrated navigation[J].Control and Decision, 2014,29(2): 341-346.(in Chinese)

[14]严恭敏,严卫生,徐德民.基于欧拉平台误差角的SINS非线性误差模型研究[J].西北工业大学学报, 2009,27(4): 511-516.YAN Gong-min, YAN Wei-sheng, XU De-min.A SINS nonlinear error model reflecting better characteristics of SINS errors[J].Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.(in Chinese)

[15]Ito K F, Xiong K Q.Gaussian filter for nonlinear filtering problem [ J].IEEE Transactions on Automatic Control, 2000, 45(5):910-927.

[16]Julier S J, Uhlmann J K, Durrant-Whyte H F.Unscented filtering and nonlinear estimation [ J].Proceedings of the IEEE Aerospace and Electronic Systems, 2004, 92(3):401-422.

[17]Lu J, Darmofal D L.Higher-dimensional integration with Gaussian weight for applications in probabilistic design [ J].SIAM Journal on Scientific Computing, 2004, 26(2):613-624.

[18]Wu Y X, Hu D W, Wu M P, et al.A numerical-integration perspective on Gaussian filters [J].IEEE Transactions on Signal Processing, 2006, 54(8): 2910-2921.

Research on Initial Alignment of Moving Base with 5th-degree Dimensionality Reduction SR-CKF

HUANG Xiang-yuan, TANG Xia-qing, WU Meng, WU Wei-sheng
(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

Abstract:In order to achieve higher alignment precision, stronger numerical stability and lower computational cost for nonlinear alignment of strapdown inertial navigation system (SINS) on moving base, a scheme of 5th-degree dimensionality reduction SR-CKF nonlinear alignment is proposed,which combines 5th-degree cubature Kalman filter (CKF), dimensionality reduction algorithm, multiple discretization, and square root(SR) filter.A nonlinear-linear separation system model is established, and the dimensionality reduction algorithm is introduced to reduce the calculated amount.A multiple discretization and time update filter framework is designed to improve the approximation accuracy.The 5th-degree dimensionality reduction SR-CKF is deduced to improve the numerical stability.The features of the conventional 3rd-degree SR-CKF, 5th-degree CKF and the proposed algorithm are compared.The experimental results show that the proposed method has a high alignment precision, strong numerical stability and little calculated amount, which meets the application requirements.

Key words:ordnance science and technology; cubature Kalman filter; dimensionality reduction; square root filter; multiple discretization

作者简介:黄湘远(1988—),男,博士研究生。E-mail: huangxiangyuan.623@163.com;汤霞清(1965—),男,教授,博士生导师。E-mail: tangxiaqing_001@163.com

基金项目:军队计划项目(51309030106)

收稿日期:2015-06-02

DOI:10.3969/ j.issn.1000-1093.2016.02.004

中图分类号:U666.1

文献标志码:A

文章编号:1000-1093(2016)02-0219-07