An improved initial alignment method based on adaptive robust CKF algorithm

Pu LI; Tao YANG; Hongwei MU

doi:10.1051/jnwpu/20224010103

All issues

Volume 40 / No 1 (February 2022)

JNWPU, 40 1 (2022) 103-109

Full HTML

Open Access

Issue		JNWPU Volume 40, Number 1, February 2022


Page(s)		103 - 109
DOI		https://doi.org/10.1051/jnwpu/20224010103
Published online		02 May 2022

JNWPU 2022, 40(1): 103–109

An improved initial alignment method based on adaptive robust CKF algorithm

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

Pu LI (李璞)¹^,2 (lipu_306@163.com), Tao YANG (杨涛)¹ and Hongwei MU (牟宏伟)²

¹ National University of Defense Technology, Changsha 410073, China
² China Academy of Launch Vehicle Technology, Beijing 100076, China

Received: 20 April 2021

Abstract

When the misalignment angle is a large angle in the strapdown inertial navigation system (SINS), it is necessary to establish a nonlinear error model to estimate the error. Hence, an improved initial alignment method based on adaptive robust CKF algorithm is proposed in this paper. Firstly, based on the analysis results, SINS/GPS nonlinear error model is established. Secondly, in the view of observation gross errors and inaccurate noise statistical characteristics, adaptive robust CKF algorithm is designed. Finally, according to simulation analysis and experiment, adaptive robust CKF algorithm can augment the stability, improve filter estimation accuracy and convergence rate, which significantly improves the initial alignment ability of strapdown inertial navigation system at large azimuth misalignment angle.

摘要

捷联惯导初始对准过程中存在大方位失准角的情况, 需要通过建立非线性误差模型来对误差进行估计, 因此对相应初始对准技术进行了研究。通过分析, 构建捷联惯性导航系统和全球定位系统的精准非线性误差模型; 基于自适应抗差理论对容积卡尔曼滤波(CKF)算法对随机干扰的统计特性以及观测粗差计算模型进行改进; 设计相应的仿真评估测试和实物验证方法, 试验结果证明提出的方法在初始对准中具有更强的滤波稳定性、更高的滤波估计精度和更短的算法收敛时间。

Key words: large azimuth misalignment / integrated alignment / nonlinear error model / adaptive robust CKF

关键字 : 大方位失准角 / 初始对准 / 非线性误差模型 / 自适应抗差CKF

This is an Open Access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/4.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

捷联惯性导航系统(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大方位失准角误差模型

用欧拉平台误差角ϕ=[ϕ_x ϕ_y ϕ_z]^T描述导航坐标系n系与数学平台坐标系n′系之间的转换矩阵为

假设n′系相对于n系的角速度为ω_nn′^n′, 则欧拉平台误差角微分方程为

式中

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

速度误差方程为

零均值高斯白噪声w_g以及随机常数漂移误差ε^b叠加构成了陀螺仪的测量误差δω_ib^b。随机常数零偏▽^b以及零均值高斯白噪声w_a组成了加速度计的测量误差δf^b。假设忽略高度误差, 以小角度为水平失准角, 从而得到SINS大方位失准角误差模型

假设系统状态量为

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

速度和位置信息为

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

式中: G_w是干扰的输入矩阵, f(x(t), t)和G_w之间的关系如公式(5)所示; h(t)是量测阵, 且有h(t)=[0_2×3 I_2×2 0_2×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滤波的关键是权因子的选择, 而观测干扰协方差阵的方法经常会使粗差的影响权重变大。因此, 本文采用权函数来取代上述方法从而减小或者消除这种影响。

假设原观测值权为P_i, 则其等价权P={P_i}={P_iw_i}, 权因子如下

式中: v_i为观测量Z的残差; σ_{v_i}是v_i的标准差, σ_{v_i}=σ₀/, q_{v_i}是v_i的权倒数; 方差因子σ₀可据σ₀=med{| , v_i|}/0.674 5得到; k₀与k_i的取值范围分别为1.5, 2.5, 3.0, 5.0。

3 自适应抗差CKF滤波算法

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

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

1) 系统干扰估计算法

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

式中, d_k-1=(1-b)(1-b^k), 0.95 < b < 0.99为遗忘因子, b的取值越大, 所受干扰统计变化越快。

2) 滤波发散抑制算法

当使用Sage-Husa估值器时, 发散问题往往难以避免, 本文利用协方差匹配判据, 由公式判断是否发散, 如果发散则通过修正P_k/k-1来抑制发散。其中, 是残差序列。

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

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

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

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

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

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

step1 容积点和权值计算

step2 时间更新

step3 量测更新

式中, P为等价权, 可由M估计等价权原理得到。

假设原观测值权为P, 则等价权P={P_i}= 。且有权因子

step4 发散判断

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

基于上述方法, 可以有效解决观测粗差和不准确的系统干扰这2种情况所带来的影响: 设系统干扰为Q_k, 同时选取目前较为有效的Sage-Husa自适应滤波, 使用该算法可以对Q_k进行在线估计, 从而减小了未知的Q_k造成的影响; 对于观测干扰误差, 本文使用等价权系数来调节, 从而得到了最优和P_k/k。

4 验证与分析

4.1 仿真测试与验证

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

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

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

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具有更高的估计精度和更快的收敛速度, 并且滤波更稳定。

2) 基于第1)组仿真条件, 假定Q′=10Q, 分别比较加入抗差和未加入抗差的2种方法, 仿真时间300 s。仿真结果见图 4~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算法的改进初始对准方法, 该方法用于解决初始对准中失准角过大情况下观测粗差和不确定系统干扰造成的误差影响。通过仿真实验和实物测试验证了本文方法的有效性及其在初始对准中具有更强的滤波稳定性、更高的滤波估计精度和更短的算法收敛时间, 进而显著提升了初始对准效率。

References

Gao Weiguang, He Haibo, Chen Jinping. An adaptive UKF algorithm and its application for GPS/INS integrated navigation system[J]. Transations of Beijing Institute of Technology, 2008, 28(6): 505–509 [Article] (in Chinese) [Google Scholar]
Zhou Feng, Meng Xiuyun. In-flight alignment research for airborne INS/GPS based on adaptive unscented Kalman filter algorithm[J]. Systems Engineering and Electronics, 2010, 32(2): 367–371 [Article](in Chinese) [Google Scholar]
Yang Bo, Qin Yongyuan. Research and simulation on the alignment of IMU/GPS integrated navigation system for shells on moving base[J]. Computer Measurement & Control, 2006, 14(4): 505–507 [Article] (in Chinese) [Google Scholar]
Liu Haipeng, Zhang Ke, Li Hengnian, et al. Research on initial alignment of SINS with large misalignment angles[J]. Aeronautical Computing Technique, 2012, 42(4): 39–42 [Article] (in Chinese) [Google Scholar]
Xia Jiahe, Qin Yongyuan, You Jinchuan. Nonlinear error model based alignment method for SINS on swing base[J]. Journal of Astronautics, 2010, 31(2): 410–415 [Article] (in Chinese) [Google Scholar]
Guo Ze, Miao Lingjuan. KF/UKF based SINS initila alignment under large azimuth misalignment angle[J]. Journal of Astronautics, 2014, 35(2): 163–170 [Article] (in Chinese) [Google Scholar]
Chen Jizheng, Yuan Jianping, Fang Qun. Attitude estimation using modified Rodrigues parameters and UKF[J]. Journal of Astronautics, 2008, 29(5): 1622–1626 [Article] (in Chinese) [Google Scholar]
Jiang Y F. Error analysis of analytic coarse alignment methods[J]. IEEE Trans on Aerospace and Electronic Systems, 1998, 34(1): 334–337 [Article] [NASA ADS] [CrossRef] [Google Scholar]
Havlicek M, Friston K J, Jan J, et al. Dynamic modeling of neuronal responses in fMRI using cubature Kalman filtering[J]. Neurolmage, 2011, 56(4): 2109–2128 [Article] [CrossRef] [Google Scholar]
Huang Jianjun, Zhong Jiali, Jiang Feng. A CKF based spatial alignment of radar and infrared sensors[C]//IEEE 10th International Conference on Signal Processing, 2010: 2386-2390 [Google Scholar]
Long Rui, Qin Yongyuan, Xia Jiahe. Applying CDKF to initial alignment of SINS for large misalignment angles[J]. Journal of Northwestern Polytechnical University, 2010, 28(3): 364–368 [Article] (in Chinese) [Google Scholar]
Xiang Li, Liu Yu, Su Baoku. A QR factorization-based adaptive divided difference filter for initial alignment of large azimuth misalignment of SINS[J]. Journal of Astronautics, 2010, 31(2): 509–513 [Article] (in Chinese) [Google Scholar]
Zhou Benchuan, Cheng Xianghong. Federated filtering algorithm based on fuzzy adaptive UKF for marine SINS/GPS/DVL integrated system[C]//Control and Decision Conference, 2010 [Google Scholar]
Aparicio A G, Reinoso O, Paya L, et al. Assessing the influence in the parameters of a Rao-blackwellised particle filter to solve the SLAM problem[J]. IEEE Latin America Transactions, 2008, 6(1): 18–27 [Article] [CrossRef] [Google Scholar]
Zheng Jiahua, Wang Hongli, Duan Xiaoqing, et al. Application of adaptive Kalman filter in initial alignment of SINS on stationary base[J]. Electronics Optics & Control, 2008, 15(6): 88–90 [Article] (in Chinese) [Google Scholar]

All Tables

表1

仿真参数表

In the text

All Figures

	图1 东向失准角估计误差曲线
In the text

	图2 北向失准角估计误差曲线
In the text

	图3 方位失准角估计误差曲线
In the text

	图4 东向失准角估计误差曲线
In the text

	图5 北向失准角估计误差曲线
In the text

	图6 方位失准角估计误差曲线
In the text

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

Current usage metrics show cumulative count of Article Views (full-text article views including HTML views, PDF and ePub downloads, according to the available data) and Abstracts Views on Vision4Press platform.

Data correspond to usage on the plateform after 2015. The current usage metrics is available 48-96 hours after online publication and is updated daily on week days.

Initial download of the metrics may take a while.

[1] Gao Weiguang, He Haibo, Chen Jinping. An adaptive UKF algorithm and its application for GPS/INS integrated navigation system[J]. Transations of Beijing Institute of Technology, 2008, 28(6): 505–509 [Article] (in Chinese) [Google Scholar]

[2] Zhou Feng, Meng Xiuyun. In-flight alignment research for airborne INS/GPS based on adaptive unscented Kalman filter algorithm[J]. Systems Engineering and Electronics, 2010, 32(2): 367–371 [Article](in Chinese) [Google Scholar]

[3] Yang Bo, Qin Yongyuan. Research and simulation on the alignment of IMU/GPS integrated navigation system for shells on moving base[J]. Computer Measurement & Control, 2006, 14(4): 505–507 [Article] (in Chinese) [Google Scholar]

[4] Liu Haipeng, Zhang Ke, Li Hengnian, et al. Research on initial alignment of SINS with large misalignment angles[J]. Aeronautical Computing Technique, 2012, 42(4): 39–42 [Article] (in Chinese) [Google Scholar]

[5] Xia Jiahe, Qin Yongyuan, You Jinchuan. Nonlinear error model based alignment method for SINS on swing base[J]. Journal of Astronautics, 2010, 31(2): 410–415 [Article] (in Chinese) [Google Scholar]

[6] Guo Ze, Miao Lingjuan. KF/UKF based SINS initila alignment under large azimuth misalignment angle[J]. Journal of Astronautics, 2014, 35(2): 163–170 [Article] (in Chinese) [Google Scholar]

[7] Chen Jizheng, Yuan Jianping, Fang Qun. Attitude estimation using modified Rodrigues parameters and UKF[J]. Journal of Astronautics, 2008, 29(5): 1622–1626 [Article] (in Chinese) [Google Scholar]

[8] Jiang Y F. Error analysis of analytic coarse alignment methods[J]. IEEE Trans on Aerospace and Electronic Systems, 1998, 34(1): 334–337 [Article] [NASA ADS] [CrossRef] [Google Scholar]

[9] Havlicek M, Friston K J, Jan J, et al. Dynamic modeling of neuronal responses in fMRI using cubature Kalman filtering[J]. Neurolmage, 2011, 56(4): 2109–2128 [Article] [CrossRef] [Google Scholar]

[10] Huang Jianjun, Zhong Jiali, Jiang Feng. A CKF based spatial alignment of radar and infrared sensors[C]//IEEE 10th International Conference on Signal Processing, 2010: 2386-2390 [Google Scholar]

[11] Long Rui, Qin Yongyuan, Xia Jiahe. Applying CDKF to initial alignment of SINS for large misalignment angles[J]. Journal of Northwestern Polytechnical University, 2010, 28(3): 364–368 [Article] (in Chinese) [Google Scholar]

[12] Xiang Li, Liu Yu, Su Baoku. A QR factorization-based adaptive divided difference filter for initial alignment of large azimuth misalignment of SINS[J]. Journal of Astronautics, 2010, 31(2): 509–513 [Article] (in Chinese) [Google Scholar]

[13] Zhou Benchuan, Cheng Xianghong. Federated filtering algorithm based on fuzzy adaptive UKF for marine SINS/GPS/DVL integrated system[C]//Control and Decision Conference, 2010 [Google Scholar]

[14] Aparicio A G, Reinoso O, Paya L, et al. Assessing the influence in the parameters of a Rao-blackwellised particle filter to solve the SLAM problem[J]. IEEE Latin America Transactions, 2008, 6(1): 18–27 [Article] [CrossRef] [Google Scholar]

[15] Zheng Jiahua, Wang Hongli, Duan Xiaoqing, et al. Application of adaptive Kalman filter in initial alignment of SINS on stationary base[J]. Electronics Optics & Control, 2008, 15(6): 88–90 [Article] (in Chinese) [Google Scholar]