一种基于自适应抗差CKF算法的改进初始对准方法

李璞, 杨涛, 牟宏伟

(1.国防科技大学, 湖南 长沙 410073; 2.中国运载火箭技术研究院, 北京 100076)

捷联惯性导航系统(strapdown inertial navigation system,SINS)的核心是依托内部惯性器件开展持续数据采集和累计,通过积分方法进行导航结果计算,且系统的初始值对于导航最终结果的影响非常大。在开始工作之前,SINS需要利用初始对准来确定载体平台的起始姿态,且起始姿态的测量精度对于最终的定位精度具有重大影响。对于载体平台是静基座的情况,假定系统失准角较小,系统误差模型符合线性特性,SINS一般采取卡尔曼滤波器(Kalman filter,KF)来进行初始对准。然而,在某些恶劣或复杂工作场景下,基于粗对准后的平台姿态误差存在不满足小失准角假设的可能,在这种情况下线性精对准方法往往无法获取较高精度的对准结果,进而直接影响最终的导航精度。面对上述情况,诸多学者对基于非线性滤波算法和误差模型的方法进行了研究。例如,刘海鹏介绍了扩展卡尔曼滤波器(extended Kalman filter,EKF)和无迹卡尔曼滤波器(unscented Kalman filter,UKF)在初始对准方位失准角大角度问题中的解决方法,并设计了相应的仿真评估测试。基于测试结果,可以发现UKF比EKF更适应大失准角度的情况;龙瑞则介绍了中心差分卡尔曼滤波器(central difference Kalman filter,CDKF)在初始对准方位失准角大角度问题中的解决方法,并基于仿真测试结果验证了该方法的高效性;Jamshaid针对航向大失准角初始对准问题,也提出了比EKF 和 UKF 精度更高的二阶分离插值滤波器(second-order divided difference filter,DDF2)[1-4]。

然而,上述传统的非线性滤波器却具有一定的缺点,如EKF由于复杂的导数,很难解析雅可比矩阵,且对于高度非线性化的系统,滤波效果并不好;UKF存在算法精度不高、稳定性差、对动态适应性低等缺点[5-7]。针对上述方法缺点,基于Spherical-Radial Cubature准则的容积卡尔曼滤波(CKF)在近段时间被广泛应用,它可以适应非线性误差模型,相比于传统非线性滤波算法有所改进。

此外,如果卫星信号不稳定,观测量也会存在粗差,进而影响滤波算法的估计效果。同时,多数情况下无法得到准确的干扰特性,使得滤波估计性能受到了很大限制。针对上述问题,本文基于自适应抗差理论设计了一种改进CKF算法。具体而言,通过稳健M估计和改进Sage-Husa次优无偏极大后验估值器，使改进CKF算法具有更强的滤波稳定性和更好的自适应性。

1 SINS大方位失准角误差模型

(1)

(2)

式中

根据文献[1],可得SINS姿态误差方程为

(3)

速度误差方程为

(4)

(5)

假设系统状态量为

陀螺及加速度计的白噪声为

速度和位置信息为

则系统状态方程和量测方程可以表示为

(6)

式中:Gw是干扰的输入矩阵,f(x(t),t)和Gw之间的关系如公式(5)所示;h(t)是量测阵,且有h(t)=[02×3I2×202×5];v(t)是量测干扰,w(t)是过程干扰,这2种都服从于v(t)～N(0,R)分布形式。

2 抗差Kalman滤波算法

在SINS/GPS初始对准中,粗差将对最终估计结果产生影响,个别较大的粗差会使结果偏差恶化[2-3,8]。传统Kalman滤波不能对粗差进行有效处理[9-11]。为了解决上述难题,本文提出了一种基于抗差Kalman的SINS滤波算法。稳健M估计在抗差估计理论中十分实用,所以本文的算法便是以这种估计方法为基石,同时为了转化为最小二乘形式,算法还使用了等加权原理。抗差Kalman滤波的关键是权因子的选择,而观测干扰协方差阵的方法经常会使粗差的影响权重变大。因此,本文采用权函数来取代上述方法从而减小或者消除这种影响。

wi=

(7)

3 自适应抗差CKF滤波算法

3.1 基于自适应Kalman滤波算法设计

不准确的干扰特性通常会使滤波不稳定,甚至发散,因此诸多学者对自适应滤波技术进行了研究[11-13]。在诸多算法中,从计算复杂度、算法复杂度等多角度进行对比,Sage-Husa次优无偏极大后验(MAP)噪声估值器具有更低的计算复杂度和算法复杂度,因此被广泛研究[14]。然而,Sage-Husa噪声估值器不能同时估计系统和量测受到的干扰,否则会导致滤波发散[15]。在实际环境中,量测受到的干扰可由传感器的物理特性得到,但由于测量仪器精度、外界干扰等因素影响,系统受到的干扰则很难准确获得。为了解决上述问题,同时提高系统的自适应能力,本文以协方差匹配为基础设计了自适应衰减因子。

1) 系统干扰估计算法

本文利用改进的Sage-Husa对不易计算的系统所受干扰qk和Qk进行估计

式中,dk-1=(1-b)(1-bk),0.95

2) 滤波发散抑制算法

(10)

(11)

(12)

(13)

式中:0<ρ≤1是衰减系数,一般取值为0.95左右;ρ的值越大,当前残差向量的影响越大。

3.2 基于自适应抗差的改进设计

不准确的系统干扰会给滤波收敛带来影响,为解决该问题,本文利用自适应抗差理论对容积卡尔曼滤波(CKF)进行了改进优化。

首先,对于非线性系统来说,设定其状态方程和量测方程为

(14)

式中,wk-1和vk为互不相关均值为零的高斯白噪声,且方差阵分别为Q和R。

本文提出的基于自适应抗差CKF算法的改进初始对准方法如下:

step1 容积点和权值计算

(15)

step2 时间更新

step3 量测更新

(26)

(27)

step4 发散判断

(28)式是发散的判断依据,如果根据公式判断发散就按照(29)～(32)式修正Pk/k-1,如果不发散就继续下一步。

4 验证与分析

4.1 仿真测试与验证

针对本文提出的基于自适应抗差CKF算法的改进初始对准方法,设计对应的仿真测试方法,并对其滤波性能进行测试。

首先,对仿真参数进行设定,如表1所示。

表1 仿真参数表

根据上述参数,系统初始方差矩阵P(0)、系统所受干扰矩阵Q和量测所受干扰矩阵R分别为：

P(0)=diag{(1°)2,(1°)2,(10°)2,(0.1 m/s)2,

(0.1 m/s)2,(0.02°/h)2,(0.02°/h)2,(0.02°/h)2,

(1×10-4g)2,(1×10-4g)2}

Q=diag{(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,

(5×10-5g)2,(5×10-5g)2,0,0,0,0,0};

R=diag{(10 m)2,(10 m)2,(0.1 m/s)2,

(0.1 m/s)2}

1) 在某段时间内,速度观测量增加3 m/s粗差,且在对滤波采样时进行频率增加,设置仿真时间为300 s,分别比较2种方法。仿真结果见图1～3。

根据图1和图2,CKF算法收敛时间依次为241.355,18.675 s,误差精度绝对值依次为0.026°,0.008°;本文提出的抗差CKF算法收敛时间依次为0.001,0.001s,误差精度绝对值依次为0.003°,0.002°,则本文方法在收敛时间上具有明显优势,在误差精度方面也相比未加入抗差的算法精度高。根据图3,在方位失准角估计方面,CKF算法收敛时间为228.425 s、误差精度为0.471°;抗差CKF收敛时间为45.355 s、误差精度为0.017°,表明抗差CKF具有更高的估计精度和更快的收敛速度,并且滤波更稳定。

图1 东向失准角估计误差曲线 图2 北向失准角估计误差曲线 图3 方位失准角估计误差曲线

2) 基于第1)组仿真条件,假定Q′=10Q,分别比较加入抗差和未加入抗差的2种方法,仿真时间300 s。仿真结果见图4～6。

图4 东向失准角估计误差曲线 图5 北向失准角估计误差曲线 图6 方位失准角估计误差曲线

根据图4至6,CKF算法收敛时间依次为0.006,250.148,140.635 s,误差精度依次为0.005°,0.031°,0.114°;本文提出的抗差CKF算法收敛时间依次为0.001,0.001,38.112 s,误差精度依次为0.002°,0.002°,0.065°,则对于东向失准角,抗差CKF和自适应抗差CKF估计精度相差不大;对于北向失准角和方位失准角,加入抗差之后的算法的估计精度明显低于同时加入自适应和抗差的算法。

针对2次测试结果,最终收敛的误差精度抗差CKF算法更高,收敛速度更快,能够更好满足当前对失准角估计的实际使用需求。

4.2 实物测试与验证

在实物测试中,选取某次SINS实测数据,为了验证本文提出的算法的实际可行性,对数据进行离线分析,得到了图7的试验结果。

从图7可以得知,在此次SINS试验中,自适应抗差CKF算法收敛于-3.82′,CKF算法收敛于-5.11′,则自适应抗差CKF算法性能更好。因此,本文提出的算法可以有效降低不准确的系统干扰特性以及异常量测信息引起的测量误差,改善了系统的初始对准精度。

图7 方位失准角估计误差曲线

5 结 论

本文提出了一种基于自适应抗差CKF算法的改进初始对准方法,该方法用于解决初始对准中失准角过大情况下观测粗差和不确定系统干扰造成的误差影响。通过仿真实验和实物测试验证了本文方法的有效性及其在初始对准中具有更强的滤波稳定性、更高的滤波估计精度和更短的算法收敛时间,进而显著提升了初始对准效率。