量测野值与波束故障条件下SINS/DVL紧组合导航方法

徐 博，王连钊，李盛新

（哈尔滨工程大学 智能科学与工程学院，哈尔滨 150001）

复杂的水下环境给航行器长时间、高精度导航带来了巨大挑战，在某些特殊应用领域又要求水下导航系统具有隐蔽性和自主性。捷联惯性导航系统（Strapdown Inertial Navigation System, SINS）因其可靠性高、自主性强，能够提供姿态、速度、位置等多维度导航参数被广泛应用于水下导航领域，但是其特殊的导航原理决定着导航参数的误差会随着时间累积，这一特点限制了惯性导航系统长时间的应用。由于水下环境电磁信号衰减严重，因此，SINS/全球卫星导航系统（Global Navigation Satellite System, GNSS）组合导航系统在水下导航难以实现[1]。相对电磁信号，声波在水下环境中具有衰减小、传播范围广等优势，因此声学导航设备在水下环境得到大量应用。但基于声学基线的水下定位系统应用条件苛刻，需要在附近水面或海面设置接收应答器[2]，自主性较差。多普勒计程仪（Doppler Velocity Log, DVL）可以利用发射信号与接收信号之间的多普勒频移测量载体的速度，具备相对稳定的误差特性，且不受工作范围约束、自主性强、可靠性高。因此，DVL与SINS构成的SINS/DVL组合导航系统被广泛应用于水下导航领域[3,4]。

传统的水下组合定位方式是基于罗经和DVL的航位推算方法，因为其没有充分考虑各子导航系统的误差，所以通常定位精度较低[5]。SINS/DVL组合导航都采用基于导航系下速度匹配方式，Øyvind Hegrenæs等对AUV水下动力学模型进行分析简化，提出了水下航行器模型辅助SINS/DVL组合导航系统的方案，利用3自由度的AUV动力学模型对DVL输出速度进行改善[6-8]，并证明了所提出的方法，提高了水下导航能力。Asaf T. 等人从Janus配置构型的原理出发，直接采用4个DVL波束的测量信息作为匹配量[9-11]，构建了SINS/DVL紧组合模型。Øyvind Hegrenæs在新型DVL验证中提到对每一个波束信息进行校正补偿可以提高测速精度。Xu等人对SINS/DVL紧组合系统做出研究，通过对四波束DVL模型分析提出了DVL波束出现故障情况下的解决方案[12]，但所提出的波束重构方案并未考虑船舶颠簸、安装偏差角等引起的波束构造误差。

以上都是通过完善模型的方式提高导航精度的方案，所提解决方案无法对误差量精准地建模，同时会引入新的速度量测噪声等问题。长时间的先验信息与噪声统计特性不匹配将导致组合导航结果精度变低[13]，针对水下导航过程中DVL测量异常或故障情况，单一的波束重构方案是不够的，还应该从信息融合方面出发，对带有变化测量噪声的速度信息进行合理的融合利用。为应对不确定干扰等因素，有学者提出了鲁棒滤波算法，其核心为以下三个方面：1、在假设模型下估计值最优或接近最优；2、实际模型与假设模型有微小差异时，其估计值或统计参数值所受影响较小；3、当实际模型与假设模型有严重偏离时，其估计结果不严重偏离。

Huber鲁棒滤波器是基于广义极大似然估计提出的一种鲁棒滤波器[14,15]，文章[15]采用Huber滤波器信息融合方法提高了复杂环境下移动机器人的定位精度。朱兵等针对水下导航中DVL量测信息受混合高斯分布和高强度野值污染的问题提出了一种基于马氏距离算法的Huber鲁棒自适应算法[16]。但以上改进算法都是在传统Huber算法的基础上，调节因子是状态残差的函数，先验估计的准确程度将一定程度上影响最终的估计精度。

借助鲁棒滤波的思想，本文提出了一种适用于SINS/DVL紧组合的改进Huber鲁棒滤波算法。不同于现有滤波方法，所提出的鲁棒滤波算法在屏蔽大粗差的同时，还对量测噪声分布特性进行估计。首先，根据四波束DVL工作特点建立了SINS/DVL紧组合模型，针对DVL波束故障提出了故障检测及故障处理方案；然后，针对DVL量测误差及系统误差异常情况提出了适用于SINS/DVL紧组合的基于改进Huber的鲁棒滤波方法；所提出方法通过了水面实验验证。

1 SINS/DVL紧组合导航系统模型及容错处理

现被广泛应用的SINS/DVL松组合方式是建立在利用DVL输出载体系速度的基础上，而在实际中，DVL是通过获取波束方向的频移及速度信息后进一步计算获取的载体坐标系的3维度速度信息。对于四波束配置的DVL，其相当于对信息进行了降维度利用，这样的方案可以在一定程度上抑制载体动态引起的测量误差，在做航位推算时具有较好的优势，但是没有充分使用所有可利用信息。

在复杂的应用环境下，DVL也会存在测速异常等故障情况[17]：

1、当航行器航行在沟渠上时，海底与航行载体之间的距离超过DVL发射声波的测量范围，波束测量的速度信息包含对水速度。

2、当航行器航行过海洋生物时，DVL发出的声波受到其遮挡，无法完全到达海底，受遮挡回波不能准确反映航行器速度信息。

3、当水下机器人执行大角度运动时，在横滚和俯仰的情况下，DVL可能会中断。

在松组合中，若DVL波束方向测速出现故障将直接引起DVL速度信息的失效，单一波束测速的噪声特性变化也将直接引起量测噪声误差特性的变化。合理的处理方式能给导航精度带来较低的损失，本文针对以上DVL辅助SINS在复杂水下环境出现故障及测速异常导致导航精度下降的问题，提出SINS/DVL紧组合导航系统故障处理方法，为特殊情况下提高组合系统的导航精度提供了新的思路，SINS/DVL紧组合导航系统信息流程图如图2所示。

图2 SINS/DVL紧组合导航系统信息流程图Fig.2 Information flow chart of SINS/DVL deeply integrated system

1.1 SINS/DVL紧组合系统量测模型

基于波束信息匹配的SINS/DVL紧组合模型更充分地利用了DVL测量信息，因此具有更良好的可靠性。SINS/DVL紧组合仍然采用SINS导航参数误差作为状态量，与松组合相比，仅在量测方程上存在区别，本文也只对SINS/DVL紧组合系统量测方程进行介绍。考虑DVL四波束Janus配置结构如图3所示。

图3 四波束Janus阵列配置结构Fig.3 Four beam Janus array configuration

忽略刻度系数误差及安装偏差角影响，载体坐标系三维速度和每个波束方向速度的关系为：

其中，α是由DVL结构决定的固定波束发射角度，β是四波数Janus配置安装角，“+”配置时为0 °，“×”配置时为45 °，C为波束方向矩阵，表示由波束方向测量速度构成的速度矢量信息。紧组合方法即利用波束方向的测速信息作为量测量，对SINS误差状态进行估计。因为SINS输出导航坐标系下的速度，需要利用惯性测速信息构造DVL波束方向速度信息，根据SINS输出导航参数有：

以DVL输出速度为匹配量的SINS/DVL紧组合量测方程如下：

1.2 SINS/DVL紧组合导航系统故障诊断及容错处理技术

1) 基于新息的χ2故障检测

根据四波束Janus阵列结构有对称性，定义测速固有误差公式来量化评估底部回波信号的质量，通常也用来判断测量速度的有效性。以载体前向为起点逆时针定义四个波束方向分别为beam1、beam2、beam3、beam4，测速误差为ve，则测速误差公式可以表示为：

其中，Vbeami表示第i个波束方向上测得的速度，从四波束Janus阵列结构配置可以看出，以上公式适用于“+”或“×”型配置方式。通常DVL在式(5)基础上设定有效阈值Vth对故障情况进行判断，若ve趋近于零则认为回波有效；若ve大于Vth则认为回波信号不理想，测得速度不可用；当DVL测得速度为对水速度时，ve反应了洋流场的不均匀程度。在本文中，不仅要对故障情况进行判断，还要求检测出故障波束，以针对不同故障波束实施不同的应对方案，因此采用基于新息的χ2检验方法对故障波束进行确定。

设SINS/DVL紧组合导航系统离散化系统方程和量测方程如下：

SINS/DVL紧组合系统卡尔曼滤波残差为[18]：

2) SINS/DVL紧组合导航容错处理方案

由式(1)可以看出当采用DVL四波束测速作为匹配信息时存在冗余信息，即三个波束获得有效测量数据时就可以辨识出载体系下的三维速度矢量，以下将分别为不同的故障环境提供不同的解决方案，以提高在波束出现故障条件下的SINS/DVL紧组合导航系统导航精度。

a. 单波束故障

在单个波束故障时，故障波束测速信息可以等价为其他三波束测速信息的线性组合，可以重构故障波束方向测速Vres为：

i、j表示与故障波束相邻的波束，k为与故障波束构成对角线关系的波束。通过式(9)方式既可以完成故障量测信息的重构，且无需对组合系统进行调整就可以应对单波束故障情况，同时可以发现这一方法可以适用于四波束DVL的“+”和“×”型配置方式。

b. 多波束故障

在多波束故障时，有效波束个数低于波束方向矩阵C的秩，故障波束速度信息不能准确通过有效波束速度信息构造，在这种情况下需要结合载体运动约束来构造故障波束信息，现有提出的解决方案多是基于DVL波束Janus结构提出的直接速度重构[10-12],这些方法都忽略了载体的运动状态，直接采用替换方式对波束速度信息进行重构，这是十分不合理的，因为DVL和SINS之间存在着安装误差角，同时载体也不能保持水平姿态为零，载体天向运动约束不等于DVL的垂向速度为零。当载体存在2 °倾角时，以上基于载体运动约束的方法将造成构造波束存在6.98%vcosα的速度误差，v表示载体合速度。本文同样以载体运动约束出发，将载体速度误差视为约束对象，即认为SINS天向速度误差为零，由SINS的速度推导出重构波束的速度信息，在一定程度上提高了重构速度的精度。

当一个波束方向速度信息有效时，波束方向矩阵C(i, :),(i∊ {1,2,3,4})的秩等于1，需要同时引入两个维度的SINS速度信息才能重构所有的波束速度，考虑载体在垂向及侧向速度较小，所造成侧向速度误差及垂向速度误差也较小，所以通常情况下仅采用引入来重构四波束DVL速度，而在“+”型Janus配置结构波束2或4波束方向信息有效时，此时有效波束信息中不包含载体前向速度信息，故需引入进行波束速度信息重构，由于分类比较多，详细的方案不具体给出。

综合以上分析，可得四波束Janus配置波束故障处理方案如图4所示，其中不同颜色的线型描述了不同重构方案及所用的SINS速度信息。

图4 四波束Janus配置波束故障处理方案Fig.4 Janus configuration beam fault processing scheme

由于引入惯性速度会存在一定误差，以上通过重构的速度信息不能准确反映真实值，同时引入越高维数的SINS速度重构速度的准确性越低，即波束故障数量越多，组合精度也会越低，这一点在后面实验中得到了验证。因此，在重构测速信息的同时应适当调节相应量测对应的噪声先验信息，以降低构造测速误差对导航精度的影响。

1.3 基于改进Huber的SINS/DVL鲁棒滤波算法

组合导航精度一定程度上依赖于系统及量测噪声先验信息。在本文中，量测噪声不能满足标准的零均值高斯分布，主要体现为：1、量测设备出现故障时，我们所提出的故障处理方案会在一定程度引入量测噪声；2、量测信息出现干扰噪声（野值）引起量测噪声特性变化。在以上情况下，基于先验信息的高斯噪声描述模型将不能很准确地反映实际状态下的噪声特性，最终导致组合导航系统的精度下降。为此，本文提出一种基于改进Huber鲁棒滤波算法，在消除量测及状态粗差影响的同时对误差协方差矩阵进行估计，使误差协方差矩阵与噪声统计特性相匹配，以提高长时间组合导航中系统的鲁棒性能。

现有的鲁棒估计是基于加权最小二乘准则，在式(6)模型基础上，考虑观测向量Zk和状态预测向量均服从污染分布，则根据加权最小二乘概念有：

其中，ρ(*)为核函数，不同的核函数反映了鲁棒滤波的不同准则，一般要求具有对称、连续、严凸性质。

状态Xk的鲁棒滤波估计即满足以上公式的极值条件的解，式(14)两边对Xk求导得：

特别的，当核函数为状态向量的二次函数，鲁棒滤波器退化为常规卡尔曼滤波。当核函数为高斯核函数，鲁棒滤波器为最大熵卡尔曼滤波器[19]。本文在Huang[20]所描述的统计相似度的思想基础上提出基于Huber权函数的鲁棒滤波方法。

在式(15)基础上定义权函数如下：

其中鲁棒滤波初始状态和状态预测协方差为：

鲁棒滤波量测误差协方差为：

对式(24)整理可得：

利用同样的方式对式(22)求期望可得：

鲁棒滤波的状态估计及状态估计误差协方差为：

本文所提出鲁棒滤波器状态估计需要对状态预测及观测异常同时做出判断，因此观测及状态预测的异常都容易引起状态估计的歪曲及等价权重歪曲等结果。考虑在组合导航系统中异常噪声多出现在量测信息中，利用分步迭代方式来解决鲁棒滤波器状态估计中容易造成误判的问题，所提鲁棒滤波器分布迭代解方案过程如下所示：

?

?

以上方案中M,N分别表示观测量和系统状态维度，N1,N2分别表示鲁棒滤波器状态滤波迭代次数和量测滤波迭代次数，文章中未对迭代次数最优值进行分析，故两者都设置为固定值50次。右上标m1,m2分别表示当前矩阵或向量状态滤波循环迭代次数和量测滤波迭代次数。

2 仿真分析

2.1 实验情况概述

受条件所限，本文利用湖面试验采集数据对本方案进行验证。船上配备了光纤SINS、法国光纤捷联惯性导航系统（PHINS）、DVL和GPS，其中PHINS与GPS组合系统（PHINS/GPS）作为导航基准用来评价SINS与DVL紧组合导航系统的精度，由于试验在湖面进行，实验设备中没有水下压力传感器，为此我们对组合系统的高度通道进行阻尼处理。实验所用船只、试验用具及设备如图5所示，试验主要传感器参数如表1所示。

表1 主要传感器参数Tab.1 Mainsensor parameters

图5 试验船只及实验设备图Fig.5 Test vessel and equipment diagram

试验前，SINS和DVL之间安装偏差角及刻度系数误差已通过文献[21]所提方法进行标定，由于试验中环境较为良好，没有出现DVL量测异常情况，为了验证所提方案的有效性，我们在DVL测量数据中加入包括量测野值及波束故障两种干扰，在10000s至20000s区间以10%概率在波束测量速度信息中添加标准差为10m/s的速度量测野值干扰，在14000s至17600s设置一小时的波束故障情况。试验轨迹及测量干扰添加情况如图6所示。

图6 湖试轨迹及测量干扰情况Fig.6 Lake test track and measurement interference

2.2 湖面试验验证

1)滤波器鲁棒性验证

为体现所提鲁棒滤波器的有效性，我们在波束无故障条件下对标准卡尔曼滤波器及所提鲁棒滤波器性能进行对比分析，其中速度误差及定位误差如图7、图8所示。

图7 、图8中红色虚线为所提鲁棒滤波器方法，黑色实线为卡尔曼滤波器方法。由图7可以看出在异常量测信息添加之前，两种方法精度几乎相同，然而在10000s之后我们在测量速度信息中随机加入标准差为10m/s的异常速度信息，标准卡尔曼滤波器由于不具备鲁棒性，异常速度信息将导致组合导航速度受噪声影响产生较大波动，进而影响定位精度；本文所提出的鲁棒滤波器可以通过调整量测信息权重的方式对异常信息进行筛选排除，所以在量测异常情况下几乎不受到干扰，在滤波器参数稳定后，速度精度保持在0.05m/s以内，位置精度无较大波动。

图7 合速度误差比较Fig.7 Comparison of total speed er ror

图8 定位误差比较Fig.8 Comparison of positioning error

为了在异常量测情况下定量描述两种滤波方法的性能，我们选择最大误差(Maximum Error,ME)、平均定位误差(Averaged Localization Error,ALE)、误差均方根(Root Mean SquareError,RMSE）、标准差(Standard Deviation,STD)等作为多AUV协同定位性能指标，速度误差和定位误差具体的数值统计如表2所示。

表2 量测异常情况速度误差及定位误差统计Tab.2 Statisticsof velocity error andpositioning error in abnormal measurement

由表2可以看出所提鲁棒滤波器几乎不受量测异常的影响，在28.47 n mile的里程内，位置误差小于57.76m，速度误差小于0.27m/s；卡尔曼滤波器方法虽然速度误差均值较小，但受异常噪声影响，先验量测信息不能匹配噪声协方差信息，引起速度误差标准差较大，进而造成定位误差较大。

2)故障处理方案验证

在SINS/DVL松组合系统中，DVL波束信息缺失将导致辅助速度信息失效，仅能采用纯惯性导航方式，然而在四波束DVL辅助SINS的情况下，由1.2节介绍可知，当波束测量信息未全部出现故障时，我们可以采用波束速度信息之间的关系重新构建速度信息，并用来辅助SINS进行导航。

为验证我们所提出SINS/DVL紧组合故障处理方案在DVL测速出现故障情况下的有效性，我们在上一小节量测异常基础上，在14000s~17600s一个小时内设置不同的DVL测量波束故障，利用所提出的故障检测及处理方案对故障进行处理，并与在此区间内利用纯惯性导航的方案进行对比。故障设置情况如表3所示（“O”表示波束正常，“F”表示波束故障）。在以上故障情况下所提处理方案与采用惯性导航方案的速度误差及径向定位误差结果对比如图9-10所示。

表3 波束故障设置情况Tab.3 Setting of beamfault

图9 故障情况合速度误差及统计情况Fig.9 Speed error and statisticsin fault condition

图9 中可以看出，在故障数据段采用惯性导航方案的速度产生较大偏移，在分布概率图中，速度误差在-0.2m/s~ -0.4m/s区间段内存在较大分布概率，即速度误差存在偏置，这是引起图10中定位误差较大的直接原因；在Case 1、Case2、Case 3情况下，因为波束故障情况不同，所采用的波束补偿方式也不同，所以速度误差也有较大出入，其中Case 3由于故障波束较多，经补偿之后的速度误差也相对较大，Case 1仅有单波束出现故障，最终导航误差也几乎不受故障影响，导致这一现象的原因在1.2节中进行了说明。从误差分布图中可以看出，经过所提波束补偿方案的处理之后，组合系统的速度误差呈现出零均值的高斯分布，即估计速度近似为真实速度的无偏估计。

图1 水下复杂工作环境Fig.1 Underwater complex working environment

图10 中可以看出在波束故障时段内采用惯性导航方式，在短时间内定位误差呈正弦趋势增长，一个小时定位误差增长约550 m。同时在速度信息恢复时，位置误差并没有减小，也说明了速度匹配情况下，初始位置误差是不可观测的。本文提出的故障处理方案在定位精度上相较于采用惯性导航方案有了较大提高，其中Case1在故障时间段误差几乎没有增加，在Case 2中定位误差约增长55m，Case3中定位误差增长约90m，约为纯惯性方案定位误差的6.96%、12.95%、19.95%。

图10 故障情况下径向定位误差Fig.10 Radial positioning error under fault condition

为具体分析定位误差的来源，东向定位误差与北向定位误差情况在图11给出，由1.2节可知，针对故障Case 2的应对方案是将天向速度认为是零，天向速度误差经俯仰角的投影转为载体前向的速度误差，由此造成前向定位误差，从轨迹图6可知，载体在东向位移较大，北向位移较小，所以定位误差主要表现在东向定位误差约为75m；针对Case3的应对方案将载体侧向及天向速度认为是零，此时造成了侧向及前向速度的不准确，由图11可知，东向及北向定位误差分别接近100m及50m。

图11 东向、北向定位误差Fig.11 East andnorth positioning error

进一步说明所提误差处理方案的有效性，径向定位误差累计概率图由图12给出，曲线描述了紧组合在导航过程中定位误差分布情况，曲线越接近直线，斜率越大表明定位误差分布越均匀且定位误差较小。由图12可以看出在波束故障段，采用惯性导航方法曲线呈明显阶梯型，即波束故障段定位误差增长较快；而采用本文所提故障处理方案，曲线阶梯状不明显，累计概率更快速地接近1，即定位误差最大值较小。

图12 径向定位误差累积概率Fig.12 Cumulative probability of radialpositioning err or

一小时故障段的定位误差及速度误差统计情况如表4所示，由表4可以看出，在1小时的DVL波束异常情况下，采用纯惯性导航方案将产生最大0.49m/s的速度误差，平均速度误差为-0.20m/s，造成位置误差最大值为577.93m，平均位置误差为241.50m；采用本文所提故障处理方案，速度最大误差小于0.15m/s，平均速度误差小于0.06m/s，Case 1、Case 2、Case 3最大定位误差分别为40.20 m、74.83m、115.31m，仅是采用惯性导航方案定位误差的6.96%、12.95%、19.95%，由此也可以看出在多波束故障情况下，所提出故障处理方案是对部分速度做出了补偿，实现了一部分速度误差的估计。均方根误差和标准差分别反应了统计量偏离真实值程度及均匀程度，可以看出，本文所提的故障处理方案所得速度均方根误差值小于0.07m/s，标准差值小于0.04 m/s，远小于纯惯性导航方式的相对应数值，定位误差也有同样的结论。

表4 量测故障段速度误差及定位误差统计Tab.4 Statisticsof speederror andlocationerror in fault section

3 结论

本文以SINS/DVL水下组合导航系统量测信息异常及故障情况为背景，传统松组合方式中，单一DVL波束出现故障将导致速度量测信息无效，仅能采用纯惯性进行接替导航方式，长时间的惯性导航将引起较大的速度误差。本文针对四波束DVL辅助SINS紧组合导航系统提出了一种故障处理方案，实现了对故障波束速度信息的重构，并用重构后的速度信息对SINS误差项进行校正，针对量测异常情况、速度重构过程中产生的模型不精确误差及引入的不确定性噪声提出了改进Huber鲁棒滤波器，最终通过试验验证了所提故障处理方案的有效性，试验结果表明，波束故障数量分别为1、2、3时，对应的1小时定位误差分别为采用纯惯性定位的6.96%、12.95%、19.95%。