KF/UPF在SINS大方位失准角初始对准中的应用

王 健,张 涛,童金武,颜亚雄

(1.东南大学仪器科学与工程学院,江苏 南京 210096;2.微惯性仪表与先进导航技术教育部重点实验室,江苏 南京 210096)

0 引 言

对准精度和实时性是惯导系统初始对准的两项重要技术指标。初始对准精度影响捷联惯导系统(strapdown inertial navigation system,SINS)的性能,初始对准实时性标志着系统的快速反应能力[1]。针对捷联惯导大失准角初始对准非线性、非高斯噪声的特点,需采用高斯近似的非线性滤波器进行处理。非线性滤波是基于贝叶斯理论推导次优滤波器,采用高斯假设的方法解决解析求解中出现的多维加权积分的问题,例如,扩展卡尔曼滤波(extended Kalman filter,EKF)、无迹卡尔曼滤波(unscented Kalman filter,UKF)、容积卡尔曼滤波(cubature Kalman filter,CKF)等等。粒子滤波 (particle filter,PF)则是基于随机采样的方法进行解析求解,其不受模型线性及高斯假设的限制,相比EKF、UKF和CKF,PF是非高斯非线性系统状态估计的最优滤波器[2-3]。

针对粒子滤波存在粒子退化、粒子易贫化、计算量大等缺陷,已有很多学者将无迹粒子滤波(unscented particle filter,UPF)方法成功地应用到大失准角初始对准中[4-7],其实质就是基于最新观测值,通过UKF计算后验分布的均值和方差来实现对状态向量后验概率密度近似,结合重采样方法可以有效地缓解粒子的退化问题,提高初始对准的精度和收敛速度。但是,在大方位失准角下,SINS初始对准非线性模型的状态维数比较高,采用UPF算法需要较多的粒子数才能更好地模拟后验概率密度,随着粒子数增多,计算量急剧增加使其实时性严重降低。为减小粒子滤波的计算量,目前研究主要集中在两个方面:一是对构建动态粒子的研究,构建动态粒子数的方法是在线实时调整滤波器所需的粒子数,在不损失滤波精度的前提下,尽可能地减少粒子数。文献[8]以后验估计精度为依据,提出了在重采样过程实现KL散度(Kullback-Leibler divergence,KLD)自适应采样方法,比在采样过程更为有效和方便。文献[9]基于特设阈值,将粒子数控制在一定硬性范围内。结合文献[8]和文献[9]的想法,文献[10]通过设置阈值和检测PF后验分布的收敛性,对滤波器所需的粒子数进行在线实时调整,但增加了算法的复杂性。二是对降低状态空间维度的研究,一种行之有效的解决思路即是对状态空间进行边缘化[11]。

本文从降低状态空间维度角度出发,基于简化理论Rao-Blackwellization定理[12],将非线性初始对准模型分解为线性与非线性两部分,分别采用KF和UPF对系统线性和非线性部分的状态进行估计。需要进行UPF滤波的状态维数由原来的10 维降为3 维,在保证对准精度的同时增强了滤波算法的实时性。

1 大方位失准角捷联惯导系统非线性误差模型

1.1 SINS大方位失准角静基座非线性误差模型

在初始对准中,速度误差和失准角误差的状态方程分别为[13-14]

(1)

(2)

(4)

(5)

1.2 SINS大方位失准角静基座初始对准的线性/非线性滤波模型[15]

1.2.1 线性部分的滤波模型

将式(5)展开,可得线性部分的状态方程为

(6)

(7)

1.2.2 非线性部分的滤波模型

由式(5)可得非线性部分的状态方程为

(8)

为了求取非线性部分的观测方程及其观测量,将式(6)东向、北向速度误差的微分方程离散化，即

(9)

(10)

(11)

(12)

结合式(8)和式(11),非线性部分的状态方程和量测方程为

(13)

2 KF/UPF组合滤波算法设计

KF/UPF组合滤波具体的步骤如下:

步骤1滤波初始化

(14)

(15)

下面的步骤按照k=1,2,…,N执行。

图1 KF/UPF组合滤波的流程图Fig.1 KF/UPF integrated filter flow chart

步骤2采用KF算法估计

(1) 卡尔曼滤波的时间更新为

(16)

(2) 卡尔曼滤波的量测更新为

(17)

步骤3采用UPF算法估计

步骤3.1UKF算法

(18)

步骤3.2选取重要推荐概率密度函数为

(19)

步骤3.3计算粒子的权重系数为

(20)

步骤3.4权值归一化为

(21)

步骤3.5计算有效粒子的尺寸为

(22)

当Neff小于阈值Nth时,为解决粒子存在的退化问题,按照顺序执行步骤3.6,否则直接跳转到步骤3.7进行初始对准状态估计。

步骤3.6重采样

步骤3.7初始对准状态估计，即

第k时刻初始对准状态估计值和估计误差方差阵分别为

(23)

步骤4再次采用KF算法估计

步骤5令k=k+1,返回到步骤3,循环运算,得到SINS大方位失准角初始对准过程中各个时刻失准角的估计值。

3 试验验证与讨论

图2～图4为UPF算法和KF/UPF组合滤波算法失准角的误差曲线。

图2 俯仰角误差曲线Fig.2 Error curves of pitch angle

图3 横滚角误差曲线Fig.3 Error curves of roll angle

图4 航向角误差曲线Fig.4 Error curves of azimuth angle

由图2～图4可知,两种滤波算法下姿态角误差曲线的收敛性几乎一致,水平失准角误差曲线很快收敛到稳定值。由图4还可知,本文设计的KF/UPF组合滤波算法下航向角误差曲线较UPF算法提前收敛。为了定量地对对准结果进行分析,分别对第400～600 s各失准角误差的均值进行统计。由于仿真时随机粒子和白噪声特性不同等原因,每次的稳态精度略有差异,通过对系统进行20次蒙特卡罗仿真取均值,其结果如表1 所示。

表1 失准角误差的比较Table 1 Comparison of misalignment angle errors

由表1可知,这两种算法状态估计的精度相当,均接近由惯性器件性能所决定的理论极限精度。但是,本文设计的KF/UPF组合滤波算法需要进行UPF滤波的状态维数由原来的10 维降为3 维,与UPF算法相比,可以显著地缩短算法的运算时间。粒子数同时取1 000时,UPF算法平均运算的时间为947.18 s,而KF/UPF组合滤波算法运算的时间仅为499.05 s,时间缩短至原来的52.69%。因此,KF/UPF组合滤波和UPF滤波算法精度相当,但在一定程度上解决了UPF算法计算负荷太大导致对准时间较长的问题。

为了体现KF/UPF在计算效率上的优势,取KF/UPF组合滤波的粒子数为100,UPF算法仍然取粒子数为1 000,将两者算法进行比较。由于水平失准角相对于方位失准角很快收敛到稳态值,两种滤波算法下其收敛时间和滤波精度一致。因此,对计算结果只列出方位失准角估计误差曲线,如图5所示。由图5可知,KF/UPF组合滤波算法与UPF算法下航向角误差均能收敛到稳定值。同样取第400 s到600 s的算术平均值进行比较,其俯仰角的稳态误差为-0.176 8′,滚动角误差的稳态精度为0.203 7′,航向角误差的稳态精度为-2.895 7′,接近由惯性器件性能所决定的理论极限精度。此时,KF/UPF组合滤波算法下平均运算的时间为56.83 s,与UPF算法(1 000个粒子数)相比,时间缩短至原来的6.0%。

图5 航向失准角误差曲线Fig.5 Curves of azimuth misalignment angle error

为了更直观对初始对准过程中的计算效率进行比较,取粒子数为1 000时UPF算法与KF/UPF组合算法分别为模式1和模式2,取粒子数为100下KF/UPF组合算法为模式3,其运算时间如图6所示。

图6 运算时间的对比图Fig.6 Comparison of the computation time

由图6可知,对比UPF算法,KF/UPF组合算法显著地提高了运算速度。

为了验证算法的有效性,在三轴转台上进行静基座对准的实验,IMU采用的是航天三院三十三所FOSN,FOSN的技术手册上主要的数据参数为:陀螺的常值漂移为0.006°/h,加速度计的常值偏置为5×10-5g,产品由于时间和环境因素,加上安装误差等的影响,实际的参数会有所偏差,需要进一步标定确定,而本文主要是用半物理仿真数据来验证算法有效性,仿真的结果如图7～图9所示。为了定量地对对准结果进行分析,分别对第400 s到600 s各失准角误差的均值进行统计,如表2所示。

图7 东向失准角误差Fig.7 East misalignment angle error

图8 北向失准角误差Fig.8 North misalignment angle error

图9 方位失准角误差Fig.9 Azimuth misalignment angle error

表2 对准结果比较Table 2 Comparison of the alignment results

由图7～图9失准角的误差曲线和表2统计结果可知,在半物理仿真下,KF/UPF组合滤波和UPF算法对准精度相当,例如,对于重要的方位失准角误差,两种滤波算法对准精度相差仅0.07角分左右。因此,KF/UPF组合滤波虽然进行了降维处理,却综合了卡尔曼滤波对线性系统和UPF对非线性系统最优估计的优势,是整体效果最优的滤波,保证了对滤波模型的最优估计,同时,解决了UPF算法计算负荷太大的问题。

4 结 论

为解决UPF应用于捷联惯导非线性初始对准中计算量过大的问题,本文基于静基座对准误差模型的特点,推导了KF/UPF组合滤波算法。该方法综合了KF对线性系统和UPF对非线性系统最优估计的优势,降低了需要进行非线性滤波的维数。仿真结果表明,KF/UPF组合滤波算法在保证初始对准估计精度和收敛速度的同时,减少了计算量,提高了运算速度,增强了滤波算法的实时性。