基于平方根容积卡尔曼滤波的SINS大失准角快速对准方法分析

基于平方根容积卡尔曼滤波的SINS大失准角快速对准方法分析

彭云龙  ,鄢思仪 ,施成功  ,邱玉芬

（航空工业江西洪都航空工业集团有限责任公司  江西 南昌  330024）

摘要：针对基座摇摆运动条件下，用递推最小二乘参数辨识法对初始失准角进行估计时，存在方位失准角收敛速度慢、估计精度受到北向失准角估计精度影响等问题，提出一种基于粒子群优化（PSO）算法的参数辨识法。该方法在保证初始对准精度的同时，直接以三个失准角作为待估参数对其进行估计，大大降低了方位失准角的估计时间。本文提出一种基于粒子群（PSO）算法的参数辨识法， 以东向、 北向、 方位失准角以及杆臂效应速度作为待辨识参数来实现摇摆基座下的快速精确初始对准。

关键词：平方根容积卡尔曼滤波；SINS大失准角；快速对准

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

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

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

其中，姿态误差矩阵、表示理想导航坐标系（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）无海浪情况。

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

（2）有海浪情况。

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

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

4、结论

本文提出了一种基于粒子群参数辨识法的SINS初始对准方法，该算法通过参数分离的方法，不仅克服了北向失准角和待估参数a2N对方位失准角估计精度和收敛速度的不利影响，还提高了水平失准角和方位失准角的收敛速度，从而达到在保证对准精度的同时缩短SINS初始对准的时间。研究結果可为舰载导弹发射等要求快速初始对准的场合提供设计参考依据。

参考文献：

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

[2]丁翠玲，陈帅.GNSS/SINS深组合导航系统研究现状及展望[J].航空兵器，2015（5）

[3]周亢，闫建国.卡尔曼滤波在捷联惯导系统初始对准中的应用[J].计算机仿真，2015（9）

[4]严恭敏，翁浚，赵长山，等.捷联惯导系统改进参数辨识初始对准方法[J].中国惯性技术学报，2018（5）