改进自适应强跟踪容积卡尔曼滤波器在动基座大失准角初始对准中的应用

赵海军 高大远 王超

摘要：捷联惯导在动基座大方位失准角初始对准时可能会出现模型失配和滤波发散等现象，为提高对准性能，对强跟踪容积卡尔曼滤波算法（STCKF）进行改进，引入非线性系统Sage-Husa噪声估计器、模型失配判据和噪声估计器收敛性判据，根据判据对CKF和STCKF进行切换。仿真结果表明：改进后的STCKF算法可以有效地解决捷联惯导在动基座初始对准时出现的模型失配和滤波发散问题。

关键词：捷联惯导;初始对准;STCKF算法;噪声估计器

中图分类号：U666.1;TH89 文献标识码：A 文章编号：1007-9416（2020）07-0106-05

0 引言

水下无人航行器（Unmanned Underwater Vehicle，UUV）在海洋探测及各类活动中应用广泛，受其体积、能源和成本的限制，UUV无法装备高精度惯导，通常使用低成本的捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）或航姿系统。导航精度不仅依赖于其惯性传感器（陀螺仪和加速度计）的精度，还依赖于高精度的初始对准。

UUV通常位于母船甲板或海面进行初始对准，受海浪影响其初始航向角难以获得，水平姿态角不停变化，导致在对准过程中使用的小失准角模型不再适用。而在大失准角条件下，初始对准模型变为非线性，传统的卡尔曼滤波难以适用。随着非线性滤波器方法的发展，针对大失准角条件的初始对准方法得到深入研究。Unscented 卡尔曼滤波器（UKF）[1-2]采用unscented变换用数值方法代替基于Jacob矩阵的线性变换计算统计特性，在计算量不显著增加的情况下得到较好的非线性滤波效果，文献[3]应用球面径向容积准则进行相应数值积分计算，对强非线性滤波效果较好。对于实际的系统，建立非线性模型及后续进行的近似计算会带来误差累积，这将导致随着滤波过程的进行，旧数据对参数更新的牵制增强而抑制新数据在预测校正计算中发挥的作用。文献[4]基于渐消记忆滤波理论基础，提出强跟踪滤波器，有效解决存在模型不确定性是鲁棒性差、滤波发散等问题，得到广泛应用。而将强跟踪滤波方法与容积卡尔曼滤波方法结合，形成强跟踪容积卡尔曼滤波，则继承了二者非线性滤波效果好、针对模型不确定时鲁棒性强的特点。文献[5]分析了强跟踪平方根容积卡尔曼滤波STRCKF的收敛性，利用船舶动力定位仿真实验验证其有效性。

在STRCKF中，渐消因子的计算需要利用噪声统计特性参数，而实际滤波过程中的噪声特性难以知晓，需要采用自适应方滤波方法进行估计。针对此问题，本文将Sage-Husa噪声估计器引入强跟踪容积卡尔曼滤波算法（STCKF），并加入模型失配判据和噪声估计器收敛性判据，基于此提出了一种自适应强跟踪容积卡尔曼滤波算法（ASTCKF），并对此方法进行仿真分析。首先建立捷联惯导大失准角对准的非线性模型，然后介绍对STCKF进行改进的方法和实施步骤，基于使用MEMS传感器的SINS进行仿真，并对结果进行分析，最后进行小结。

1 SINS大方位失准角初始对准模型

假设陀螺仪和加速度计的测量误差和主要由随机常值和零均值高斯白噪声组成;同时当UUV在水面时，SINS进行初始对准，不考虑高度误差，假设水平失准角较小，方位失准角为大角度，则可以推导大方位失准角的误差模型如下[6-7]：

其中，姿态误差矩阵、表示理想导航坐标系（n系）、载体坐标系（b系）与实际SINS模拟数学平台坐标系（系）之间的关系;为系相对于惯性系（系）的转动角速度，、分别为的计算值和计算误差;为系相对于系的转动角速度，为其计算误差;为加速度计的测量值，为加速度计测量误差;为重力补偿误差;为地球自转角速率;和分别为子午圈曲率半径和卯酉圈的曲率半径。

令状态向量包含失准角、（水平）位置误差和速度误差、传感器误差（不包含垂向加速度误差），则可写为：

陀螺和加速度计的高斯白色噪声向量为：

观测量取卫星定位结果与SINS输出的位置和速度之差，，则可得到SINS大方位失准角初始对准的非线性方程如下：

2.2 非线性Sage-Husa自适应滤波算法

上面方法中，在计算时，需要知道噪声统计特性参数，而当参数未知时，可采用自适应滤波。传统自适应滤波算法复杂且计算量大，很难应用于工程实际。近年来以Sage-Husa自适应滤波为代表的“开窗”平滑估计噪声的自适应滤波算法得到广泛的应用。Sage-Husa自适应滤波利用量测残差信息，按极大后验噪声统计估值定义得到次优估值器，然后对系统噪声进行在线实时估计。根据极大后验噪声统计估值定义设计次优无偏估值器，并将其扩展到非线性系统，则可以得到非线性Sage-Husa的次优无偏递推噪声统计估值器：

其中，、分别表示通过非线性变换和后的均值，，为遗忘因子，滤波增益、最优估计值和输出一步預测方差阵等由滤波器给出。

非线性系统Sage-Husa噪声估计器不需要计算雅克比矩阵，计算量小;而且通过引入遗忘因子以增大新近时刻的数据加权系数，减小陈旧数据的加权系数，从而达到对时变噪声统计量、、和的自适应估计。

2.3 模型失配判据

通过强跟踪容积卡尔曼滤波算法的推导[4]，可以看出对误差阵的调整以及对滤波增益矩阵的优选都是通过调整渐消因子矩阵而实现的，因此可以选取渐消因子矩阵为模型失配判据，即当时，非线性系统模型未失配，采用标准容积卡尔曼滤波算法;否则，说明非线性系统模型失配，采用自适应强跟踪容积卡尔曼滤波算法。

2.4 噪声估计器收敛性判据

在稳态条件下，可认为非线性系统噪声统计特性相对稳定，此时可以不使用非线性系统Sage-Husa噪声估计器，从而降低算法的计算负担。由于模型失配往往伴随着系统噪声特性的改变，因此模型失配判据是调用非线性系统Sage-Husa噪声估计器的前提条件。同时增加噪声估计器收敛性判据，其表述形式如下：

如果，则判定噪声估计器发散，调用非线性系统Sage-Husa噪声估计器，否则不调用。其中为常值小量。

2.5 改进ASTCKF算法的具体步骤

根据上面分析，优化的ASTCKF算法步骤如下：

（1）一步预测估计，首先按照式（5）-（7）计算，即CKF方法的第（i）步至第（iii）步。

（2）按照（12）计算渐消因子，并进行模型失配判定。若，模型未失配，则仍然按照标准容积卡尔曼滤波算法的（7）式计算方差矩阵估计，否则采用STCKF方法，用（12）（13）式计算。

（3）计算一步测量预测、滤波增益矩阵、状态估计值和状态误差协方差阵，仍然按照相应的（8）-（11）式计算，只不过要使用采取渐消因子的结果。

（4）若判定噪声估计器未收敛，采用非线性Sage-Husa自适应滤波对噪声估计：

由此实现改进的ASTCKF方法。此方法在滤波正常进行时，是一个标准的容积卡尔曼滤波器，计算负担不大，精度有保证，而在模型失配、噪声估计发散时，通过简单的判据，采用自适应Sage-Husa噪声估计器，形成ASTCKF，以提高滤波性能。总体来说此方法在计算负担和滤波性能之间进行自动切换，具有满意的效果。

3 仿真研究

3.1 仿真条件

（1）UUV的航迹设计。

UUV初始对准航迹设计如下：水下航行器从（纬度36°N，经度120°E）位置出发，前50秒作匀加速直线运动，加速度为，然后以（约5节）的速度匀速直线运动，总仿真时间为300秒。

（2）滤波器初始参数设置。

MEMS陀螺仪的常值漂移为，随机漂移均为;三个加速度计的零偏均为10mg，随机白噪声标准差为1mg;忽略陀螺仪和加速度计的安装误差;初始失准角为。仿真中系统状态的初始方差阵、系统噪声初始方差阵以及观测噪声初始方差阵分别设置为：

3.2 仿真验证

为了验证改进STCKF算法的有效性，分别采用本文提出的改进ASTCKF滤波算法、强跟踪容积卡尔曼滤波算法（STCKF）和标准容积卡尔曼滤波算法（CKF），来进行捷联惯导系统大方位失准角海上动基座对准仿真实验，仿真实验分为无海浪和有海浪两种情况。

（1）无海浪情况。

当没有海浪作用时，认为系统噪声和观测噪声为零均值的高斯白噪声，仿真结果如图1所示。

可以看出，在没有海浪影响的情况下，系统噪声和观测噪声可以看作零均值的高斯白噪声，此时利用CKF、STCKF、优化ASTCKF三种滤波算法对SINS进行初始对准，均具有较好估计精度。稳定后，三种方法的北向失准角估计误差为20.84`、23.9`，28.12`，天向失准角估计误差分别为：31.01`、34.56`、37.24`。优化的STCKF算法的精度略好于其他两种，而收敛速度要优于其它两种算法。

（2）有海浪情况。

假设UUV的实际姿态角在海浪的作用下分别以如下形式做正弦摇摆运动：

其中、、分别表示各姿态角的摇摆幅值;、、分别为摇摆角频率;而，，为相应的摇摆周期;为UUV真实航向;仿真过程中分别取：，，，，，，。此时，系统噪声和量测噪声为相互独立的带时变均值和协方差的正态白噪声序列。仿真结果如图2所示。

当有海浪影响的情况下，系统噪声和量测噪声可以认为是相互独立的带时变均值和协方差的正态白噪声序列，其统计特性未知，此时，STCKF对于方向失准角的滤波收敛速度和估计精度都明显优于CKF，优化ASTCKF的滤波收敛速度更快，估计精度也更高。稳定后优化方法的北向失准角和天向失准角误差分别为30.61′和53.26′，STCKF算法的失准角误差分别为43.65′和77.34′，CKF算法的失准角误差分别为80.70′和153.30′，可以看出，优化ASTCKF在估计精度上优势明显。这是由于是优化ASTCKF引入非线性系统Sage-Husa噪声估计器，能够实时调整和估计噪声统计特性的变化，从而克服系统噪声统计特性不准确的局限性。同时引入了强跟踪滤波器，可以对于系统部分参数失配引起的模型误差具有较强的跟踪能力。因此对北向失准角和天向失准角的估计能够保持较高的估计性能。

4 结语

在复杂海洋环境的影响下，UUV在水面或近水面时SINS进行初始对准的过程存在模型失配和滤波发散等问题，针对上述问题，本文将非线性系统Sage-Husa噪声估计器、模型失配判据和噪声估计器收敛性判据融入到强跟踪容积卡尔曼滤波算法中，推导了优化的强跟踪容积卡尔曼滤波算法，最后通过仿真实验验证了此方法对SINS进行大方位失准角初始校准的有效性。

参考文献

[1] Julier S，Uhlmann J，Durrant-Whyte H F.A New Method for the Nonlinear Transformations of Means and Covariances in Filters and Estimators[J].IEEE Transactions on Automatic Control，2004，45（3）：477-482.

[2] 丁杨斌，王新龙，王镇，等.Unscented卡尔曼滤波在SINS静基座大方位失准角初始对准中的应用研究[J].宇航学报，2006，27（6）：1201-1204.

[3] Arasaratnam I，Hayken S.Cubature Kalman filters[J].IEEE Trans. Automatic Control，2009，54（6）：1254-1269.

[4] 周东华，席裕庚，张钟俊.一种带多重次优渐消因子的扩展卡尔曼滤波器[J].自动化学报，1991，16（7）：689-695.

[5] 徐树生，林孝工，赵大威，等.强跟踪SRCKF及其在船舶动力定位中的应用[J].仪器仪表学报，2013，34（6）：1266-1272.

[6] M.J Yu.Equivalent Nonlinear Error Models of Strapdown Inertial Navigation System[M].presented at AIAA conference proceedings，Guidance，Navigation and Control，1997.

[7] 严恭敏，严卫生，徐德民.基于欧拉平台误差角的SINS非线性误差模型的研究[J].西北工业大学学报，2009，27（4）：511-516.

[8] 孙枫，唐李军.基于CKF的SINS大方位失準角初始对准[J].仪器仪表学报，2012，33（2）：327-333.