组合导航滤波器的在线初值设置及收敛判定方法

吕建强 邓博炜 叶 松 高晓颖,3

1.北京航天自动控制研究所，北京 100854 2.北京科技大学自动化学院，北京 100083 3.宇航智能控制技术国家级重点实验室，北京 100854



组合导航滤波器的在线初值设置及收敛判定方法

吕建强1邓博炜2叶 松1高晓颖1,3

1.北京航天自动控制研究所，北京 100854 2.北京科技大学自动化学院，北京 100083 3.宇航智能控制技术国家级重点实验室，北京 100854

组合导航滤波时需要设置初值并对滤波是否收敛进行判断。针对初值设置问题，提出一种根据滤波模型递推值对状态变量协方差阵在线初始化的方法；针对判断收敛问题，提出了一种利用状态变量协方差阵进行在线判断滤波是否收敛的方法。通过实时计算状态量协方差的相对值，得出收敛条件，并给出收敛程度。最后通过数学仿真，验证了本文方法的有效性。

组合导航；卡尔曼滤波；分段滤波；在线收敛判定；在线初值设置

组合导航是提高导航精度和可靠性，弥补单一导航方式缺陷的有效方法。惯性导航和卫星导航因为其互补的特点被称为黄金组合[1]。对于捷联惯导与卫星导航的松耦合，在某些特定条件下，观测值会出现短时间或者较长时间的中断。进行组合导航计算时，如果GPS信号中断时间过长(10s以上)，信号恢复后再直接进行滤波，滤波器会产生发散现象[2]。对于这种情况，可以分段进行组合导航，即认为在卫星信号中断之前进行了1次组合，在信号恢复之后又进行了1次组合。

分段进行组合导航时，将面临2个问题：1)再次进行组合导航时，滤波初值如何设置；2)第1次组合导航时，滤波是否收敛以及收敛的程度。

针对组合导航初值的设置问题，有学者做了大量的研究，给出了设置状态协方差阵初值、系统噪声协方差阵和观测噪声协方差阵的经验设置方法[4]。这种设置方法在首次组合导航滤波时被实践证明是有效的。但是再次进行滤波时，根据经验进行线下设置的初值很不准确，这将影响滤波的收敛速度[4]。

针对如何在线判断滤波收敛和发散的问题，目前的研究比较少，工程上通常根据经验以时间作为收敛条件，这种判断方法一般比较保守[6]，而且在面对观测值突然中断的情况时，无法进行有效的判断[6]。

针对初值设置问题，本文在所述的特定条件下提出了一种根据滤波模型递推值对状态变量协方差阵在线初始化的方法。考虑到文献[4]和[5]均指出适当放大状态变量协方差阵有利于提高收敛速度。本文将在上述初值的基础上乘以与时间相关的放大倍数，得到实际使用的状态变量协方差阵的初值。针对在线收敛的判定问题，本文假设所用滤波器满足文献[7]所述的3条假设和4个定理，并根据文献[7]所定义的滤波过程稳定性，通过计算状态量协方差的相对值，得到了在线判定滤波收敛的条件，并定量给出了滤波收敛程度。

1 惯导与卫星组合导航滤波模型

本文采用发射点重力惯性坐标系，选择速度误差、位置误差、姿态角误差、加速度计零漂和陀螺零漂作为状态变量[9]，建立捷联惯导与卫星(SINS/GNSS)的松耦合滤波模型，如式(1)和(2)所示，

(1)

Z(t)=H(t)X(t)+v(t)

(2)

其中，

X(t)=[δV,δr,δf,δK,δD]T

(3)

w(t)=[▽x1,▽y1,▽z1,εx1,εy1,εz1]T

(4)

v(t)=[ΔVgx,ΔVgy,ΔVgz,Δrgx,Δrgy,Δrgz]T

(5)

式中，状态向量X(t)中δVx,δVy,δVz为速度误差；drx,dry,drz为位置误差；δfx,δfy,δfz为姿态角误差；δK0x,δK0y,δK0z为加速度零位误差；δD0x,δD0y,δD0z为陀螺常值误差；系统白噪声w(t)中▽x1,▽y1,▽z1为加速度计的噪声，εx1,εy1,εz1为陀螺的噪声；观测向量Z(t)中δVx,δVy,δVz为惯性解算速度与卫星测得的速度之差；δrx,δry,δrz为惯性解算位置与卫星测得的位置之差；观测白噪声v(t)中ΔVgx,ΔVgy,ΔVgz为卫星导航的速度精度误差，Δrgx,Δrgy,Δrgz为卫星导航的位置精度误差。

将上述模型离散化[11]，得

Xk+1=Φk+1,kXk+Γkwk

(6)

Zk+1=Hk+1Xk+1+vk+1

(7)

其中，状态转移矩阵，

(8)

系统输入矩阵，

(9)

系统噪声协方差矩阵，

(10)

观测噪声协方差阵

(11)

本文采用如下的滤波公式[5]，

(12)

(13)

(14)

(15)

Pk+1,k+1=Pk+1,k-Kk+1Hk+1Pk+1,k

(16)

2 状态协方差阵的初值设置方法

状态协方差阵P中主对角线元素实际上反映了相应状态变量的精度[5]。在捷联惯性与卫星组合导航中，P中主对角线元素反映了惯导的误差积累情况。而滤波模型中的状态变量实际上是惯导的误差量，其与P中主对角线元素的开平方量纲一致。由此，在不进行组合导航的时间段内，考虑式(12)，如果继续递推，则得到当前时刻的误差。虽然这样得到的误差不够准确，但也在一定程度上反映误差的大小及变化方向。取

(17)

本文滤波模型的建立过程中，忽略了二阶及以上的小量，因此是一种近似的模型。由其递推得到的状态变量随着时间的增加，会越来越不准确。另外，目前的捷联惯导递推算法[9]本身会带来一定的误差，综合考虑上述因素，可以根据经验离线设置μ(τ)。对μ(τ)影响较大的因素是弹道和惯性仪表的误差参数。由于本文所用弹道变化比较平缓，在此条件下，惯性仪表误差对μ(τ)的影响比较明显。

记第l次滤波结束到第l+1次滤波启动时间间隔为τ。根据本文中多次仿真结果，中断时间τ在百秒级别时，这里取线性方式即可得到较好的效果，即

μ(τ)=Aτ+B

(18)

式中，A=[a1,a2,…an]T，B=[b1,b2,…bn]T。A中与速度对应的参数可参考惯性仪表误差指标，与位置对应的参数可在速度基础上缩小τ倍。当τ达百秒时，B可取为0；当τ比较小时，B可取小于1的数值，限制P0的过度膨胀。

由上述方法得到实际使用的状态协方差阵，

(19)

3 滤波收敛的在线判定方法

随着滤波次数的增加，状态协方差矩阵趋于0或者趋于稳定，同时系统误差不再减小，则判断滤波收敛，如图1所示。本文假设滤波器满足文献[7]所述的假设条件，所以只需要考查状态协方差阵P，就可以判断滤波是否收敛。

图1 收敛曲线示意图

记状态协方差阵P的主对角线元素为p1,p2,…,pn。其中,pi(1≤i≤n)描述了第i个状态的精度，记pi(k)为pi第k时刻的值。

由于滤波收敛后，P的大小几乎不再变化，即其增量接近0。但是由于状态变量量纲的不同，其对应的pi在数值上差别很大。这里将pi进行无量纲化处理

(20)

式中，1≤i≤n。当pi的增量接近0时，ηi接近数值1。判断ηi与数值1的接近程度，即可以判断滤波是否收敛。在滤波收敛过程中，ηi的值将从0逐渐接近1。ηi实际上反映了滤波的实时收敛情况，可以用来表征收敛程度。由上述分析可以得到

(21)

将上述方法概括为判定方法1：随着滤波次数k的增加，当满足ηi(k)-1≤εi时，则判定第i个状态已经收敛，当所有进行滤波修正的状态都收敛时，认为滤波收敛。其中,εi是根据实际需要设置的一个较小阈值，ηi的定义如式(20)所示。

实际飞行中，观测值可能出现野值，实际噪声也可能出现尖峰等现象，这些都可能使P阵发生跳变。此时计算出的ηi值也会发生跳变，导致收敛误判。为了避免ηi突然跳变造成的误判，需要收敛判定方法2：当连续出现Ni个ηi小于预先设置的阈值εi时，则判定第i个量收敛。当所有进行修正的状态变量收敛时，则判定滤波收敛。其中，参数Ni根据需要进行预先设置，εi的含义同判定方法1。

判定方法1与2的关系：εi固定的条件下，Ni越大，则判定的收敛时间越长，需要根据导航的需要进行设置；Ni固定的条件下，εi越大，则判定收敛的时间越短，但是εi不能太大，否则失去了判断收敛的功能。通过调节参数Ni和εi，可以灵活的得到期望的收敛时刻，如图1所示的3个时刻。

4 仿真验证

仿真条件：惯导计算周期为0.02s，滤波周期为

1s，仿真时间1000s，滤波器启动2次，第1次启动时间在起飞后200s，第2次启动时间在起飞后800s。观测值噪声方差阵真实值与仿真值均设为：

R=[0.052,0.052,0.052,52,52,52]T；

系统噪声方差阵真实值与仿真值均设为：

Q=[4E-6,4E-6,4E-6,2.5E-9,
2.5E-9,2.5E-9]T

滤波收敛判定条件：

N=[5,9,9,5,5,5]T，
ε=[0.05,0.05,0.05,0.001,0.001,0.001]T。

A=[1.8E-5,1.8E-5,1.8E-5,
1.8E-7,1.8E-7,1.8E-7]T，
B=[0,0,0,0,0,0]T。

4.1 验证滤波的状态协方差阵初值设置方法

2.76E-8,2.33E-6,2.21E-8,
3.88E-11,4.41E-8,3.99E-8,
1.27E-15,1.52E-14,1.30E-17])

图2 速度误差曲线

图3 位置误差曲线

4.2 验证滤波收敛判定方法

判断滤波的收敛时刻。从200s开始持续滤波到400s，得到的相应速度位置方差如图4和5所示。由判定方法进行滤波收敛判断可以得到x向、y向和z向速度和位置分别在第8次、第9次、第35次、第17次、第12次滤波时收敛。最终得到本次滤波在第35次修正后，速度位置均已收敛。相应的速度位置误差随时间的变化曲线如图6和7所示。

滤波中断时，判断状态变量的收敛程度。假设进行了3次滤波后，观测值突然消失，考查ηi值，得到3个方向的速度位置收敛程序如下η=[0.7604,0.9960,1.0047,0.7860,0.8166,0.8186]T

此时对应的速度位置误差分别为：-0.0536m/s, -0.0797m/s, 0.0862m/s, -2.1194m, -3.7508m, 0.8357m。虽然误差已经减小，但是由局部误差图(图8和9)可以看出，滤波仍然存在震荡，并没有稳定收敛。η较好的表征了每个量收敛的程度。

图4 速度误差收敛曲线

图5 位置误差收敛曲线

图6 速度误差随时间变化曲线

图7 位置误差随时间变化曲线

图8 速度误差局部放大曲线

图9 位置误差局部曲线

5 结论

以SINS/GNSS组合导航分段滤波为背景，提出了一种在线判定滤波收敛的方法和状态变量协方差阵的初值设置方法。根据滤波递推模型，给出了在特定条件下再次滤波状态协方差阵初值的参考值。通过计算卡尔曼滤波状态协方差阵不同时刻的相对值，提出了在线判断滤波收敛的判定方法，并定量的给出了不同时刻的滤波收敛程度。仿真结果表明，本文所述方法是合理有效的。

[1] 董绪荣, 张守信, 陈哲.GPS/INS组合导航定位及其应用[M]. 长沙：国防科技大学出版社，1998.(DongXurong,ZhangShouxin,ChenZhe.GPS/INSIntegratedNavigationandPositioningandItsApplications[M].Changsha:NationalDefenseTechnologyPress,1998.)

[2] 李增科, 王坚, 高井祥, 谭兴龙.利用SVM的GPS/INS组合导航滤波发散抑制方法研究[J]. 武汉大学学报自然科学版, 2013, 38(10): 1216-1220.(LIZengke,WangJian,GaoJingxiang,TanXinglong.AMethodtoPreventGPS/INSIntegratedNavigationFilteringDivergenceBasedonSVM[J].GeomaticsandInformationScienceofWuhanUniversity, 2013, 38(10): 1216-1220.)

[3]AndersonBDO.StabilitypropertiesofKalman-Bucyfilters[J].JournalofFranklinInstitute, 1971, 291(2): 137-144.

[4] 程向红, 郑梅.捷联惯导系统初始对准中Kalman参数优化方法[J].中国惯性技术学报, 2006, 14(4): 12-17.(ChengXianghong,ZhengMei.OptimizationonKalmanFilterParametersofSINSDuringInitialAlignment[J].JournalofChineseInertialTechnology, 2006, 14(4): 12-17.)

[5] 秦永元, 张洪钺, 汪叔华.卡尔曼滤波与组合导航原理[M].西安：西北工业大学出版社，2012.(QinYongyuan,ZhangHongyue,WangShuhua.TheoryofKalmanFilteringandIntegratedNavigation[M].Xi′an:NorthwesternPolytechnicalUniversityPress, 2012.)

[6] 朱立华, 程向红, 何应云.传递对准中滤波器的收敛判据[J]. 中国惯性技术学报, 2011, 19(3): 277-285.(ZhuLihua,ChengXianghong,HeYingyun.FilterConvergenceCriterioninTransferAlignment[J].JournalofChineseInertialTechnology, 2011, 19(3): 277-285.)

[7] 邱凯, 黄国荣, 陈天如, 杨亚莉.卡尔曼滤波过程的稳定性研究[J].系统工程与电子技术, 2005, 27(1): 33-35.(QiuKai,HuangGuorong,ChenTianru,YangYali.StudyoftheStabilityofKalmanFilteringProcess[J].SystemsEngineeringandElectronics, 2005, 27(1): 33-35.)

[8] 邱恺, 黄国荣, 陈天如, 杨亚莉.基于滤波过程的卡尔曼滤波发散判定方法[J].系统工程与电子技术, 2005, 27(2): 229-231.(QiuKai,HuangGuorong,ChenTianru,YangYali.MethodofdivergencedetectionforKalmanfilterbasedonfilteringprocess[J].SystemsEngineeringandElectronics, 2005, 27(2): 229-231.)

[9]DavidTitterton,JohnWeston.StrapdownInertialNavigationTechnology[M].AmerInstofAeronautics, 2004.

[10] 胡小平. 自主导航理论与应用[M].长沙：国防科技大学出版社, 2002. (HuXiaoping.PrinciplesandApplicationsofAutonomousNavigation[M].Changsha:NationalUniversityofDefenseTechnologyPress, 2002.)

[11]Chi_TsongChen.Linersystemtheoryanddesign[M].NewYork:OxfordUniversityPress, 1999.

A Method of Online Setting up Initial Values and Online Convergence Criterion of Integrated Navigation Filter

Lv Jianqiang1, Deng Bowei2, Ye Song1, Gao Xiaoying1,3

1.Beijing Aerospace Automatic Control Institute, Beijing 100854, China 2.School of Automation Engineering, University of Science and Technology Beijing, Beijing 100083, China 3.National Key Laboratory of Science and Technology on Aerospace Intelligent Control, Beijing 100854, China

Theinitialvaluesandjudgingconvergencearerequiredduringintegratednavigationfiltering.Regardingthesetupoftheinitialvalues,anonlinemethodbasedonfilteringmodel’srecursivevaluesisproviedtosetthecovariancematrixofstatevariables.Fortheproblemofhowtojudgeconvergence,anewmethodispresentedbyusingstatevariablecovariancematrix.Bycalculatingtherelativevaluesofstatevariablecovarianceinrealtime,convergencecriterionandthedegreeofconvergenceareobtained.Intheend,mathematicalsimulationisusedtoverifythevalidityofthenewmethods.

Integratednavigation; Kalmanfilter;Segmentfiltering;Onlineconvergencecriterion;Onlinesettinginitialvalues

2015-11-25

吕建强(1988-)，男，山东东营人，硕士研究生，主要研究方向为飞行器导航与制导；邓博炜(1995-)，男，本科生，自动化专业；叶 松(1979-)，男，安徽南陵人，硕士，高级工程师，主要研究方向为飞行器制导与控制；高晓颖(1969-)，男，河北深县人，博士，研究员，主要研究方向为飞行器制导与控制。

TN967.2

A

1006-3242(2016)03-0056-06