基于变结构多模型的SINS/DVL组合导航算法

李 倩，王健成，尹冬寒，臧新乐，奔粤阳，赵玉新

(1.哈尔滨工程大学智能科学与工程学院，哈尔滨 150001；2.中国船舶工业集团公司第七○八研究所，上海 201100)

0 引言

捷联惯性导航系统(Strapdown Inertial Navigation System, SINS)是一种自主式导航系统，被认为是水下导航领域中最可靠的导航系统，在短时间内具有较高的导航精度。但是由于陀螺漂移和加速度计零偏的存在， 导致其误差不断积累，因此无法长时间单独工作。对于水下环境，由于全球定位系统(Global Positioning System，GPS)信号会迅速衰减[1-3]，因此常用多普勒计程仪(Doppler Velocity Log, DVL)辅助SINS为无人水下航行器提供精确的导航信息[4]。DVL有两种工作模式：底跟踪模式和水跟踪模式。DVL一般工作在底跟踪模式，将DVL提供的相对地面的速度与SINS融合，为载体提供精确的导航信息。然而，DVL受作用深度限制，在超过一定范围时，DVL将工作在水跟踪模式下，此时DVL只能为载体提供相对于水的速度。若将相对于水的速度作为相对地面的速度信息提供给卡尔曼滤波器(Kalman Filter, KF)对SINS的系统误差进行估计，此时洋流速度会成为DVL的测量误差，将导致SINS / DVL组合导航性能降低，甚至导航失败[5-7]。

为了提高SINS/DVL组合导航系统的性能，文献[8]提出了一种解决方案，将洋流速度建模为常数，考虑其对系统的影响，并将其增广到KF的状态量中， 基于长基线/超短基线(Long Baseline/Ultra-short Baseline， LBL/USBL)和DVL提供的位置和速度信息，采用KF估计洋流速度。文献[9]采用模型辅助惯性导航系统对自主水下航行器(Autono-mous Underwater Vehicle, AUV)导航解算方法进行改进，该模型可以在线估计洋流速度参数,但需要提供精确的AUV动态模型以及外部定位信息。以上方法均需一些外部定位信息，而对于水下环境，连续的外部位置信息有时难以获得。此外，简单地将洋流速度建模为常数以补偿DVL水跟踪模式下的测量误差，在短时间内有一定的效果，但不适用于长航时和广海域环境。为了进一步降低洋流常值模型对SINS/DVL组合导航系统造成的影响，文献[10]使用一阶马尔可夫过程来描述洋流速度的变化，并通过KF实时估算洋流速度，从而校正系统误差。在很多情况下，一阶马尔可夫过程洋流速度模型中的相关时间常数会随着季节、位置、盐度和天气条件等外部因素的变化而变化[11-12]。因此，单一的洋流速度模型往往无法表征其流速变化。

针对洋流速度模型的不确定性，文献[13]利用交互式多模型(Interactive Multiple Model, IMM)算法估计洋流速度，以减小其对导航精度的影响。在该算法中，两个KF在不同的洋流速度模型下并行运行，然后进行输出融合，得到洋流速度的估计值。然而，在实际应用中，仅仅建立一个由两个洋流速度模型组成的小模型群是不够的。为了提高估计效果就要使用较大的模型群，这样就会增大计算量，难以满足导航系统的实时性要求[14]。同时，某些不必要的模型间会存在过度竞争的问题，而在状态估计过程中利用太多的模型也会降低算法的性能[15]。

利用IMM算法进行组合导航时，一方面要保证模型群有足够多的模型来覆盖系统的实际模型；另一方面，为了保证计算速度，需要尽量使用较小的模型数量。显然，这两个要求是矛盾的。针对这一问题，1992年，X.Li和Y. Bar-Shalom提出了一种变结构多模型(Variable Structure Multiple Model, VSMM)方法[16-17]。在本文中，采用有向图切换的变结构多模型(Digraph Switching-Variable Structure Multiple Model, DS-VSMM)算法来实时估计洋流速度。基于有向图切换方法，设置洋流速度模型群，利用所设置的模型群切换策略对模型群进行激活和终止，使用较少的模型数量实现了洋流速度的准确估计，进而消除了其对SINS/DVL组合导航的影响[18]。

1 SINS/DVL组合导航系统模型建立

SINS/DVL组合导航系统误差状态方程为

(1)

式中，状态变量X为14维列向量；状态矩阵F为14×14维矩阵；系统噪声输入矩阵G和系统噪声矩阵W为14维列向量。

X=[δLδλδvEδvNφxφyφz

ΔAxΔAyεEεNεUθCEθCN]T

(2)

式中，δL、δλ分别为纬度和经度误差；δvE、δvN分别为东向和北向速度误差；φx、φy、φz为平台失准角；ΔAx、ΔAy分别为东向和北向加速度计零位偏移；εE、εN、εU分别为东向、北向和天向陀螺漂移；θCE、θCN分别为洋流的东向和北向速度。本文主要对洋流速度进行模型群建模，故此处只给出洋流状态矩阵块Fon，其余将省略。

(3)

式中，βE、βN分别为东向和北向洋流模型的时间常数。

W=[0 0 0 0 0 0 0ωdEωdNωgE

ωgNωgUωEωN]T

(4)

式中，W为零均值的高斯白噪声，它的协方差矩阵为Q；ωdE、ωdN分别为加速度计东向和北向噪声；ωgE、ωgN、ωgU分别为陀螺东向、北向和天向噪声；ωE、ωN分别为洋流速度模型对应的东向和北向白噪声。

在水下环境中，如果DVL工作在水跟踪模式下，则其测得的是对水的相对速度，即

(5)

式中，vE、vN分别为惯导系统计算的东向和北向速度；vDVL(E)、vDVL(N)分别为DVL测得的相对于水的东向和北向速度，对式(5)整理可得

(6)

本文利用惯导解算的速度与DVL测得的速度之差作为滤波器的量测量，则SINS/DVL组合导航系统的量测方程为

(7)

式中，量测噪声V为零均值白噪声，它的协方差矩阵为R，式(7)中量测矩阵H为

H=

(8)

2 SINS/DVL组合导航系统模型群的建立

基于DS-VSMM算法对导航参数进行估计，首先建立系统的模型群。根据第1节，洋流速度模型设置为一阶马尔可夫过程，然而在实际中，洋流速度模型中的一阶马尔科夫过程相关时间常数是未知的，它会随着外部因素的变化而变化。因此，为了能够准确地估计洋流的速度，在可能的范围内设置一个大的洋流速度模型群(不同模型对应不同的相关时间常数)。此外，由于水下环境复杂，SINS/DVL组合导航系统中量测噪声的统计特性也会随机变化，因此量测噪声协方差矩阵R也是一个未知的参数。

本文基于不同的相关时间常数β和量测噪声协方差矩阵R，设置了13个模型。根据式(3)，洋流速度模型相关时间常数β对应状态转移矩阵块Fon，各个模型中对应的状态矩阵块Fon和量测噪声方差矩阵R设置如表1所示。

表1 各个模型与洋流模型状态矩阵块、量测噪声方差矩阵的对应关系

假设系统模型的跳变不会出现断层式的情况，即只在相邻模型之间切换。例如：系统不会出现从Fon直接越过2Fon跳变到4Fon的情况，R的跳变情况同上，那么SINS/DVL组合导航系统模型跳变的拓扑有向图如图1所示。

图1 SINS/DVL组合导航系统整体模型群的有向图

图1中，箭头所指两个模型可以互相进行切换。将SINS/DVL组合导航系统可能出现的跳变分为四种类型:β变大的跳变、β变小的跳变、R变大的跳变和R变小的跳变。

假设系统当前时刻处于模型M1，随后可能向模型M2跳变(β增大)，也可能向模型M4跳变(β减小)，也可能向模型M3跳变(R增大)，也可能向模型M5跳变(R减小)，这种情况对应的子模型群为A1={M1,M2,M3,M4,M5}，M1为子模型群A1的中心模型。子模型群A1的有向图如图2所示。

图2 SINS/DVL组合导航系统子模型群A1的有向图

与图2所示的子模型群A1类似，还有分别以M2、M3、M4和M5为中心的子模型群A2、A3、A4和A5

A2={M2,M10,M6,M1,M9}A3={M3,M11,M7,M1,M6}A4={M4,M12,M8,M1,M7}A5={M5,M13,M9,M1,M8}

3 模型群切换算法

模型群切换(Model-GroupSwitching,MGS)算法首先需要设置模型群，随后要制定MGS算法的自适应策略。MGS算法在运行过程中一般分为两部分：模型群激活与终止，最后在匹配的有向图内进行IMM滤波。MGS算法步骤如下：

步骤1：将计数时间k增加1，即k→k+1。运行VSMM[Mk,Mk-1]一个周期。

步骤2：检测是否有候选模型群被激活。当满足以下概率条件时

μk

(9)

步骤3：如果模型群Ma被激活，则令Mo=Mk，同时进行如下运算：

1)运行VSMM[Mn,Mk-1]一个周期，其中Mn=Ma-Mo是新模型的集合。

2)令Mk=Mn∪Mo=Ma∪Mo。

3)估计融合：对模型群Mk计算模式概率、状态估计以及误差的协方差矩阵，分别如式(10)～式(12)所示

(10)

(11)

(12)

步骤5：模型群Ml=Mo或Ml=Ma，则有式(13)～式(15)

(13)

(14)

(15)

当式(16)或式(17)成立时

(16)

(17)

则终止模型群Ma，令Mk+1=Mo，然后跳转到步骤1。

当式(18)与式(19)同时成立时

(18)

(19)

则终止模型群Mo，并令Mk+1=Ma，然后跳转到步骤1。

步骤6：将计数时间k增加1，即k→k+1，同时令Mk+1=Mk，运行VSMM[Mk,Mk-1]一个周期，然后跳转到步骤4。

4 仿真验证

为了验证基于DS-VSMM的SINS/DVL组合导航算法的性能，在仿真环境相同的情况下，对比DS-VSMM算法与IMM算法。

(20)

根据式(20)以及表1，仿真中真实的洋流速度曲线如图3所示。

图3 真实的洋流速度曲线图

此外，这里还给出了仿真环境中设置的载体运动轨迹以及速度变化图，如图4和图5所示。

图4 载体运动轨迹

图5 载体的速度变化曲线图

根据上述仿真环境，分别应用DS-VSMM算法和IMM算法，最终得到两种算法在SINS/DVL组合导航系统中的误差，仿真结果如图6～图11所示。

图6 DS-VSMM速度误差曲线图

图7 IMM速度误差曲线图

图8 DS-VSMM位置误差曲线图

图9 IMM位置误差曲线图

图10 DS-VSMM姿态角误差曲线图

图11 IMM姿态角误差曲线图

由上述仿真结果曲线对比可知，在横滚以及俯仰角误差方面，应用DS-VSMM算法与IMM算法二者无明显差别；在速度误差、位置误差和航向角误差方面，应用DS-VSMM算法明显优于IMM算法。综上所述，本文设计的基于DS-VSMM的SINS/DVL组合导航算法可以更好地消除洋流速度对系统的影响，进一步提高了组合导航系统的性能。

两种算法运行10次的时间均值如表2所示，表明本文提出的算法具有更优的费效比。

表2 两种算法运行10次的时间均值对比

IMM算法每次滤波要使用全部13个子模型滤波，而DS-VSMM算法每次滤波只需要使用相应的5个子模型滤波。DS-VSMM算法运行时间理论上是IMM算法运行时间的38.5%，但由于需要进行子模型群切换，进而增加了一定的运行时间，因此根据表2，DS-VSMM算法最终的运行时间是IMM算法的65.4%。综上所述，基于DS-VSMM算法的SINS/DVL组合导航与应用IMM算法相比，降低了计算量，且进一步提高了系统的性能。

5 结论

本文针对无人水下航行器面临的不确定性洋流速度影响SINS/DVL组合导航系统导航精度的问题，提出了一种基于DS-VSMM的组合导航算法。当工作在水跟踪模式下，DVL只能提供相对于水的速度信息，因此洋流速度会影响SINS/DVL组合导航系统的性能。为了能够准确地对洋流速度进行估计，本文基于有向图切换方法，建立了洋流速度模型群，通过所设置的模型群切换策略对模型群进行激活和终止，最终实现了对洋流速度的准确估计，进而消除了其对SINS/DVL组合导航系统的影响。仿真结果表明：基于DS-VSMM的SINS/DVL组合导航算法的位置误差控制在50m之内，而IMM算法的位置误差接近300m，前者的定位精度明显优于后者。本文提出的算法可以很好地克服多变的洋流速度引起的导航误差，同时减少了一定的计算量，提高了效费比，也提高了导航系统的精度，为进一步实现水下高精度SINS/DVL组合导航提供了参考。为进一步提高SINS/DVL组合导航系统的稳定性及水下高精度导航能力，还需要更深入的研究：

1)本文提出的算法构建的洋流速度模型较少，在实际应用中，可以根据具体的导航精度要求设置更多的洋流速度模型；

2)本文的滤波方法为KF，并未进行非线性近似，可使用其他非线性滤波方法进行同条件仿真，比较不同滤波方法的优劣，从而选出最优的滤波算法。