抗差自适应UKF的INS/GNSS组合导航算法

付心如，徐爱功，孙 伟

(辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000)



抗差自适应UKF的INS/GNSS组合导航算法

付心如，徐爱功，孙 伟

(辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000)

为了进一步提高滤波精度和稳定性，针对INS/GNSS组合导航系统的特点建立系统非线性误差模型，并提出一种抗差自适应UKF算法：基于UKF滤波中新息序列动态调整观测噪声数学统计特性，并结合残差序列构造自适应因子，改善初始协方差误差对标准UKF的影响，提高标准UKF自适应能力，减少存在观测粗差情况下滤波精度下降问题。仿真结果表明，该算法能够较好抑制观测粗差对定位精度及稳定性的影响。

组合导航；抗差；自适应滤波；UKF

0 引言

解决非线性问题的最优算法是基于条件后验概率完整描述的数学模型，而精确的数学模型需要无尽参数描述，难以应用于实际系统；所以对于非线性系统来说，核心在于近似方法[1-2]。为此人们提出对非线性数学模型进行线性化、采样方法近似等多种次优滤波器算法。常用扩展Kalman滤波(extended Kalman filter, EKF)属于前者，采用对模型高阶项逼近或忽略的局部线性最优技术；但其算法精度取决于导航系统的非线性强度以及展开点的选择[3-6]，且难以模块化应用。无迹Kalman滤波(unscented Kalman filter，UKF)利用对系统状态非线性函数概率密度分布的近似，基于先验知识选择一组加权Sigma点集，随后对每个Sigma点进行非线性转换，捕捉到较高精度的后验均值及协方差[7-9]。由文献[10-11]可知：在计算量上UKF与EKF相当，但性能优于EKF；但UKF对初值的选取较为敏感，且噪声数学统计特性的不确定性或观测异常[12-14]等都会使UKF滤波精度下降。

论文拟将抗差自适应引入标准UKF，设计基于速度/位置组合导航抗差自适应UKF滤波器，以期有效抵制观测粗差对滤波精度及稳定性的影响。

1 抗差自适应UKF算法

1.1 UKF算法

UKF核心是通过Unscented变换(简称UT变换)传播和更新非线性数学模型中状态均值与误差协方差阵[15-16]。UT变换是利用后验概率密度进行近似得到次优解，即利用一组确定性选择加权Sigma点，使这些点的某些特性与先验分布相匹配，随后对每个Sigma点施加UT变换，并对其结果点(均值、协方差)进行加权处理，得到最终状态估值及其协方差阵，其原理如图1。将UT变换应用在Kalman滤波框架中得到UKF滤波算法。

UT变换的重点在于确定Sigma点集个数、位置，及对应的权值[11]。为避免粒子点退化问题，本文采用2N+1个Sigma点对称确定性采样。

INS/GNSS组合导航系统的非线性状态方程和以速度与位置为观测量的线性观测方程分别为：

xk=f(xk-1,ωk)；

(1)

yk=Hkxk+vk。

(2)

式中：xk为状态量；yk为观测量；k为滤波次数；ωk、vk分别为系统过程噪声序列和观测噪声序列；f表示非线性状态方程；Hk为观测矩阵。

基于状态协方差计算Sigma点集为

(3)

Sigma点集对应权值为

(4)

(5)

(6)

UKF时间更新。对输入变量Sigma点集χ中的每个点进行非线性转换，并对其进行加权处理，具体计算过程如下：

χk|k-1=f(χk-1,k-1)；

(7)

(8)

(9)

UKF测量更新。标准UKF中不仅时间更新需要多次运行非线性函数以保证状态空间的准确性，而且测量更新中也同样需要。基于速度/位置的组合导航系统是线性观测方程，在系统噪声和观测噪声为加性的前提下，对标准UKF的观测方程进行简化，减少由于多次运行Sigma采样点引起的巨大计算量对滤波实时性的影响。简化的测量更新过程为：

(10)

(11)

(12)

(13)

(14)

(15)

1.2 抗差自适应UKF

标准UKF模型框架中噪声协方差是确切的、带有一定数学统计特性的数值，但在实际应用中通常是未知或随时间变化的。错误先验统计信息的使用很可能导致滤波较大的定位误差，甚至发散。

(16)

(17)

根据零均值高斯白噪声数学特征及状态协方差公式，可得新息序列协方差为

(18)

实际观测中样本协方差为

(19)

故

(20)

即将

(21)

换为

(22)

式中：n为实际观测样本数目，也叫窗口大小。利用新息序列匹配算法能保持与实际观测噪声数学特性一致。即当观测值出现粗差时，也能通过实时调整状态协方差矩阵改善增益矩阵Kk值。为更好地实时调整滤波器中的增益矩阵，提高滤波跟踪性能，必须使滤波因子自适应当前时刻实际噪声协方差，利用残差序列构造误差判别统计量

(23)

基于判别统计量ΔVk的自适应因子为

(24)

式中：αk为自适应因子；k为经验数值，一般取1

(25)

换为

(26)

即当UKF初始协方差选取有偏差或载体处于异常扰动情况下，αk小于1，自适应重新分配状态预报值与当前观测信息的权重，提高最终滤波定位解的精度与稳定性。当动力学模型稳定且无异常扰动时，αk值为1，滤波为标准UKF滤波。

2 INS/GNSS组合导航数学模型

设计以速度/位置为观测量的15维INS/GNSS组合导航抗差自适应UKF滤波器，其中观测参数为

x=[φEφNφUδvEδvNδvUδλδL

非线性姿态误差与速度误差方程分别为：

(27)

(28)

位置误差方程为：

(29)

(30)

(31)

(32)

3 实验与结果分析

设载体的初始位置为42°N、121°E，高度150m。陀螺常值漂移为0.03(°)/h，加速度计常值偏置为5ug，惯性测量单元采样周期0.01s，卫星接收机采样周期1s。其他Kalman滤波参数设置如表1所示。载体运动轨迹包括：爬高、转弯、俯冲、加速、减速、匀速等过程，总共历时1 600s，如图2所示。轨迹模拟器给出的各时间点速度、位置信息作为计算导航误差参考值。

表1 Kalman滤波器参数

根据抗差自适应UKF滤波模型及上述参数，在没有外界观测粗差干扰情况下，对标准UKF、抗差自适应UKF(RAUKF)进行仿真模拟，得到的滤波结果分别如图3、图4所示。

对比无粗差情况下UKF与RAUKF定位误差，抗差自适应滤波在一定程度上使误差值趋于平稳。具体精度比较如表2所示。

表2 无粗差时精度比较 m

表2中数学统计值也证明了上述结论，即在无观测粗差情况下，抗差自适应UKF滤波精度较标准UKF滤波定位精度较高。

为研究本文算法在外界观测粗差扰动情况下的定位精度，分别在600 s、700 s、800 s、900 s时刻分别加入持续5 s时间30 m的观测粗差。得到的定位结果分别如图5、图6所示。

由图5中导航定位结果可以看出，加入的粗差对标准UKF导航解有明显影响，在600 s、700 s、800 s、900 s处滤波解出现了定位误差突变现象，而且由于Kalman滤波算法的递推性，突变误差对该时间点以后的定位结果有一定的影响。而对于RAUKF，由于误差协方差阵估计能够实时适应误差数学特性及其滤波因子自适应调整，从而可以调节滤波循环周期中的增益矩阵Kk，合理调整状态预测值与观测值对组合导航解的贡献，达到抵制观测粗差的作用，如图6所示。

表3给出了加入观测粗差后各时间段内UKF与RAUKF滤波结果的定位误差。

表3 存在观测粗差时定位结果比较

通过统计数据可以看出，采用RAUKF算法能较好地提升导航定位性能，有效控制观测粗差对定位精度的影响。

4 结束语

在分析标准UKF算法的基础上，针对UKF受初值误差及外界扰动影响的问题，利用新息序列与残差序列进行实时观测噪声统计特性匹配并自适应调整Kk，提高UKF抗差与自适应能力。文中对抗差自适应UKF在INS/GNSS组合导航系统中的应用进行了研究，仿真实验结果表明，无论粗差是否存在，RAUKF定位精度均高于标准UKF滤波，而且在一定程度上能够抵制外界扰动对导航定位精度的影响。

[1] 毛克诚, 孙付平, 李海丰.UKF在GPS/INS组合导航系统中的应用[J]. 测绘科学技术学报, 2007, 24(3): 200-202.

[2]RIGATOSGG.NonlinearKalmanfiltersandparticlefiltersforintegratednavigationofunmannedaerialvehicles[J].Robotics&AutonomousSystems, 2012, 60(7): 978-995.

[3]JULIERS,UHLMANNJ.AnewextensionoftheKalmanfiltertononlinearsystems[J].SPIEProceedingsSeries, 1997, 3068: 182-193.

[4]RHUDYM,GUY,GROSSJ,etal.SensitivityandrobustnessanalysisofEKFandUKFdesignparametersforGPS/INSsensorfusion[J].AIAAJournalofAerospaceInformationSystems, 2013, 10(3): 131-143.

[5] 高为广, 杨元喜, 张婷. 一种提高神经网络泛化能力的自适应UKF滤波算法[J]. 武汉大学学报(信息科学版), 2008, 33(5): 500-503.

[6] 高社生, 王建超, 焦雅林. 自适应SVD-UKF算法及在组合导航的应用[J].中国惯性技术学报, 2010, 18(6): 737-741.

[7]MENEGAZHMT,ISHIHARAJY,BORGESGA,etal.AsystematizationoftheunscentedKalmanfiltertheory[J].IEEETransactionsonAutomaticControl, 2015, 60(10): 2583-2598.

[8]CHANGLB,QINFJ,ZHAF.Pseudoopen-loopunscentedquaternionestimatorforattitudeestimation[J].IEEESensorsJournal, 2016, 16(11): 4460-4469.

[9] 胡高歌, 刘逸涵, 高社生,等. 改进的强跟踪UKF算法及其在INS/GPS组合导航中的应用[J].中国惯性技术学报, 2014, 22(5): 634-639.

[10]GROSSJ,GUY,RHUDYM,etal.FlighttestevaluationofGPS/INSsensorfusionalgorithmsforattitudeestimation[J].IEEETransonAerospaceElectronicSystems, 2012, 48(3): 2128-2139.

[11]潘泉, 杨峰, 叶亮, 等.一类非线性滤波器—UKF综述[J]. 控制与决策, 2005(5): 481-489.

[12]何正斌, 吴富梅, 聂建亮.先验协方差阵误差对动态Kalman滤波解的影响[J].武汉大学学报(信息科学版), 2011, 36(6): 34-38.

[13]杨元喜,何海波, 徐天河.论动态自适应滤波[J].测绘学报, 2001, 30(4): 293-298.

[14]杨元喜, 徐天河. 基于移动开窗法协方差估计和方差分量估计的自适应滤波[J]. 武汉大学学报(信息科学版), 2003, 28(6): 714-718.

[15]SORENONHW.Kalmanfiltering:theoryandapplication[M].NewYork:IEEEPress, 1985.

[16]JWODJ,YANGCF,CHUANGCH,etal.Performanceenhancementforultra-tightGPS/INSintegrationusingafuzzyadaptivestrongtrackingunscentedKalmanfilter[J].NonlinearDynamics, 2013, 73(1): 377-395.

An INS/GNSS integrated navigation algorithm based on robust adaptive UKF

FUXinru,XUAigong,SUNWei

(School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China)

In order to further improve the filtering accuracy and stability, the paper established the systematic nonlinear error model according to the characteristics of INS/GNSS integrated navigation system, and proposed a robust adaptive unscented Kalman filter algorithm: the statistical characteristics of the observation noise were dynamically estimated and adjusted based on innovation vectors of UKF, and the adaptive factors based on residual sequences were construct, the influence of the values of initial parameters on standard UKF was reduced, the adaptive capability of the standard UKF was enhanced, and the decreased filter accuracy when gross errors existed in the measurement values was improved.Simulation results showed that the method could effectively restrain the influence of observed gross errors on the positioning accuracy and stability of the integrated navigation.

integrated navigation; robust; adaptive filter; UKF

2016-09-06

国家重点研发计划项目(2016YFC0803102)；国家自然科学基金项目(41304032)；辽宁工程技术大学测绘与地理科学学院优秀研究生论文培养计划(YS201502)。

付心如(1991—)，女，河北邢台人，硕士研究生，研究方向为组合导航。

徐爱功(1963—)，男，山东日照人，博士，教授，博生生导师，研究方向为卫星定位与地理信息系统相关理论与应用。

付心如，徐爱功，孙伟.抗差自适应UKF的INS/GNSS组合导航算法[J].导航定位学报,2017,5(2):111-116.(FU Xinru, XU Aigong, SUN Wei.An INS/GNSS integrated navigation algorithm based on robust adaptive UKF[J].Journal of Navigation and Positioning,2017,5(2):111-116.)

10.16547/j.cnki.10-1096.20170220.

P228

A

2095-4999(2017)02-0111-06