基于改进Sage-Husa 自适应滤波算法的SINS/DVL 组合导航*

朱天高，刘 勇，李文魁*，楼鉴路，祝西枰

（1.海军工程大学，武汉 430000；2.海警局南海分局，广东 深圳 518048）

0 引言

自主水下航行器（autonomous underwater vehicle，AUV）在军事和民用方面均得到广泛的应用，在AUV 在水下执行任务时，精确、自主、可靠的导航系统是关键技术［1-2］。由于在水下工作，AUV 无法接收卫星导航信号。捷联惯性导航系统（strapdown inertial navigation system，SINS）和多普勒计程仪（doppler velocity log，DVL）组合导航是目前应用较多的水下导航算法［3］。

组合导航系统信息处理的核心是导航滤波器［4］。在水下航行过程中，难免遇到鱼群、水下地形等复杂环境，从而导致DVL 数据出现异常。若遇到此等情况，常用的手段是通过自适应滤波对量测噪声矩阵进行相应的在线调整，一定程度上抵消DVL精度下降带来的负面影响［5-6］。而在自适应滤波过程中，渐消因子对于系统参数估计性能的影响比较大，以往的自适应滤波中渐消因子都是事先根据经验进行设定的，若遇到高动态的复杂外界环境，难以及时有效对噪声矩阵进行在线估计［7-10］。

针对以上问题，本文提出一种基于渐消因子动态调整的改进Sage-Husa 自适应滤波，引入量测故障检测与调整机制，解决自适应滤波估计量测噪声参数性能对渐消因子依赖性强的问题。当观测量存在异常时对量测矩阵进行故障检测与在线调整，从而抑制误差。最后，通过实测数据验证了对于组合导航系统，该自适应滤波算法能够有效抑制复杂外界环境导致的DVL 测速失准带来的定位误差，当DVL 测速精度明显降低时，实现定位精度的保持。

1 SINS/DVL 组合导航数学模型

1.1 SINS 姿态误差微分方程

经典SINS 微分方程包含姿态矩阵微分方程、速度微分方程和位置微分方程，分别如下

由于实际应用中存在各种误差，理想导航坐标系n'系和计算导航坐标系系之间存在一定的转动误差，记n 系和n'系之间的3 次转动角为。根据文献［5］，可得SINS 非线性误差微分方程为

1.2 DVL 误差模型

本文中认为多普勒计程仪安装角误差在组合导航之前已经进行了标定，多普勒计程仪的测量误差主要为刻度系数误差，则速度表达式为

其中，vD为实际速度；δKD为刻度系数因子，由于b 系和m 系重合，则载体坐标系下的多普勒计程仪速度为

建议可区分ABC类设施，确定建设方向和投资比例。A类为重点投资设施（例如：机房、值班室、办公室、门厅配套及装饰），B类为必须投资设施（例如：基础、结构、综合管线，消防，防水，隔热），C类为可节省投资设施（例如：楼梯间、休息室、食堂、地下室装潢修缮）。

投影到导航系下的多普勒计程仪速度为

1.3 SINS/DVL 误差模型

SINS/DVL 采用速度组合方式，即采用DVL 速度信息辅助惯导，将DVL 和惯导输出的速度差值作为量测值，利用卡尔曼滤波器对惯导系统误差进行估计，然后对惯导误差进行修正。组合导航中，一般需要对器件误差进行建模估计，此处考虑陀螺漂移、加速度计零偏和DVL 刻度系数误差，且认为其为常值，因此，系统的状态量取为

系统状态方程为

其中，FSINS/DVL为状态转移矩阵，可由上文的SINS 误差模型和DVL 误差模型得出；w 为系统噪声；G 为系统噪声矩阵。

取惯导与DVL 的速度之差为组合导航观测量，即：

系统观测方程为

2 SINS/DVL 组合导航滤波算法设计

由于姿态误差阵的存在，速度误差方程为非线性的，因此，考虑对SINS/DVL 组合导航系统采用UKF 方法进行滤波，在此基础上通过自适应滤波算法对噪声的量测矩阵进行实时估计修正，达到有效抑制定位误差的目的。

2.1 UKF 滤波

对卡尔曼滤波算法中的一步预测方程使用UT变换来处理均值和协方差的非线性传递，就得到了UKF 算法。对于非线性误差模型，标准的UKF 滤波算法如下：

1）初始化

2）Sigma 点计算与时间更新

3）量测更新

滤波增益矩阵为

状态估计及其均方误差阵为

当量测模型为线性时，量测更新过程可简化为

式中，Hk为线性量测矩阵。

2.2 Sage-Huga 自适应滤波改进

Sage-Huga 自适应滤波是在卡尔曼滤波的基础上加入时变噪声的滤波，以实时估计并修正量测噪声矩阵，从而减小模型误差，抑制滤波器发散，进而提高滤波精度。利用上节讲述的UKF 滤波算法，Sage-Huga 自适应滤波算法如下：

其中，b（0＜b＜1）称为渐消因子。b 取得越小，则系统对于新量测噪声变化的自适应能力越强，常取b=0.9～0.999。然而，在高动态的变化环境中，b 保持常值对于系统的自适应能力有很大的消极影响，不利于系统保持高精度的导航定位。

因此，为了增强滤波的自适应能力，本文提出基于一种基于渐消因子动态调整的改进Sage-Husa自适应滤波，具体如下：

其中，κ 为常值，由通过经验值进行设定，可控制渐消因子的变化速度。在滤波过程中，当量测噪声发生变化时，可通过对u 滤波的状态进行判断。若u的值较大，说明量测噪声变化较大，滤波异常，则调节渐消因子取较小的值以提高对量测噪声矩阵的跟踪速度；当量测噪声变化较小且比较稳定时，u 值相应较小，渐消因子的取值适当增大，以便能够更精确地估计出量测噪声矩阵。通过本文提出的基于渐消因子的自适应滤波，系统的鲁棒性与准确性大大增强，更有利于实际的导航应用。

3 实验与结果分析

下面通过实验船实测数据验证设计的基于非线性误差模型的SINS/DVL 自适应组合滤波算法的有效性。测量设备包括IMU、DVL 和GPS 接收机，记录4 000 s 的实验数据。IMU 参数为：陀螺零偏为0.02°/h，加速度计零偏稳定性为50 μg，采样频率200 Hz。DVL 的参数为：测速精度约为刻度因子误差为1%，采样频率1 Hz。GPS 水平定位精度10 m，测速精度0.1 m/s，采样频率1 Hz。以SINS/GNSS 组合滤波输出作为参考基准，对应的实验轨迹图如图1 中黑色曲线所示。为了更好模拟复杂的海洋环境，设置初值失准角为，且对DVL 速度叠加2 m/s 的速度误差随机噪声，以模拟较差的观测输入。

图1 实验轨迹图Fig.1 Experimental trajectory diagram

基于非线性线性误差模型，采用常规UKF 滤波，Sage-Husa 自适应UKF 滤波以及本文提出的基于渐消因子的自适应滤波等3 种组合模式进行SINS/DVL 组合导航，并对估计误差进行对比分析。实验采用的组合导航卡尔曼滤波如下：

UKF：无迹卡尔曼滤波

AUKF：Sage-Husa 无迹自适应卡尔曼滤波

SAUKF：基于渐消因子的无迹自适应卡尔曼滤波

图2～图4 分别为3 种组合滤波的SINS/DVL组合导航姿态误差曲线。由图可知，对于3 种模型，都表现为水平误差角明显比方位误差角收敛速度快，方位误差角收敛则较为缓慢，这主要是因为DVL提供的是载体系下的速度观测值，SINS/DVL 方位误差角的可观测度较弱。对4 种组合模型的性能比较，SAUKF 最优，AUKF 次之，UKF 最差。改进的算法相比较原Sage-Husa 自适应滤波算法在2 500 s 处可提升航向角精度约30%，本文提出的基于渐消因子动态调整的的Sage-Husa 自适应卡尔曼具有更优的性能。由此可知，3 种滤波中，SAUKF 性能最优，对姿态估计最为准确。

图2 纵倾角误差Fig.2 Trim angle error

图4 航向角误差Fig.4 Course angle error

图5～图6 分别为东向速度误差和北向速度误差，图7～图8 分别为纬度误差和经度误差。由图可知，与其他两种组合滤波相比，SAUKF 滤波算法可获得更高的姿态角估计精度，进而提高了速度和位置的精度，相比于AUKF 和UKF 滤波整体优势较大，尤其在2 500 s～4 000 s 时SAUKF 滤波的速度估计优势更为明显。同时图1 也给出了3 种滤波以及参考轨迹对比图，可明显看出，SAUKF 轨迹最接近参考轨迹，而AUKF 滤波和UKF 滤波则离参考轨迹较远。

图5 东向速度误差Fig.5 Eastward velocity error

图6 北向速度误差Fig.6 Northern velocity error

图7 纬度误差Fig.7 Latitude error

图8 经度误差Fig.8 Longitude error

4 结论

SINS/DVL 组合导航是水下导航的最常用方式之一。但在实际应用过程中，水下环境复杂多变从而出现DVL 数据异常，导致组合导航精度下降。Sage-Husa 自适应卡尔曼滤波常用于解决此类问题，但该自适应滤波估计量测噪声参数性能对渐消因子依赖性较强，常用的自适应滤波中渐消因子往往为事先设定的常值，难以应对高动态的复杂水下环境。针对此情况，本文提出一种基于渐消因子动态调整的改进自适应滤波，通过引入渐消因子的变化函数，对渐消因子进行相应的动态调节，克服了常值渐消因子对高动态复杂水下环境跟踪能力差的局限性，从而提高滤波算法的估计精度和鲁棒性，在实际工程应用中具有一定的应用价值。