考虑洋流影响的SINS/DVL组合导航算法

郭玉胜，付梦印，邓志红，邓继权，艾瀛涛

（北京理工大学自动化学院，北京100081；北京自动化控制设备研究所，北京100074）

捷联惯性导航系统(SINS)/多普勒计程仪(DVL)组合导航系统是长航时水下导航常用的一种导航方式[1-5]。DVL是利用安装在载体的超声换能器向海底发射超声波，并根据多普勒效应的原理测量载体速度的仪器[6-7]。目前使用的 DVL通常都有两种工作模式：底跟踪模式和水跟踪模式。当载体相对于海底距离在 DVL有效打底深度之内时，DVL工作在底跟踪模式，可以提供精确、稳定的载体前向和侧向对底速度。当载体对底距离超过了DVL测速最大射程时，DVL会自动切换到水跟踪模式，此时测量得到的是载体相对于DVL设定水流层的速度[8]。当DVL工作于水跟踪模式时，还需要知道水流层的水流速度，两者之和才是载体的对底速度。因此，当 DVL由底跟踪模式切换到水跟踪模式后，洋流速度将会对SINS/DVL组合导航系统的定位精度产生很大的影响。

针对该问题，当前研究的主要思路是基于机动目标“当前”统计模型的水下组合导航模式，通过增加加速度观测信息，采用加速度均值、方差自适应Kalman滤波算法实现线运动参数的估计和流速修正，进而提高了航位推算的精度[9-10]。但是该方法计算量大，工程实现较为麻烦，而且该方法不能解决 DVL在不同模式切换下的流速修正问题。

在高精度水下导航定位中必须考虑洋流速度的影响。在极端条件下，渤海的表层水流速度可达3.47m/s，黄海的表层水流速度可达2.88 m/s，东海的表层水流速度可达3.26 m/s。此时，对于运行速度比较慢的载体，水流的速度可能超过载体自身的行驶速度。因此针对海底深度变化导致DVL在不同模式下切换的问题，开展便于工程实现的载体周围小范围内的洋流速度估计对提高 SINS/DVL组合导航定位精度有着非常重要的意义。

DVL测速精度越高要求的声波发射频率越高，声波发射频率越高，射程就会越小[11]。当海底深度超过了DVL射程范围，DVL测速精度就会严重下降。两者的具体切换情况如图1所示。当载体位于A区域时，由于载体对底距离超过了DVL测速最大射程，DVL工作于水跟踪模式，测量得到的是载体相对于DVL设定水流层的速度，此时，还需要知道水流层的水流速度，两者之和才是载体的对底速度。当载体行驶至B区域时，由于载体对底距离在DVL测速最大射程之内，DVL切换到底跟踪模式，可以提供精确、稳定的载体前向和侧向对底速度。此外，由于B区域海底深度大于DVL设定的水流层深度，DVL在输出载体对底速度的同时也可以输出载体相对于水流层的速度。当载体行驶至C区域时，由于海底深度小于DVL设定的水流层的深度，DVL只能输出载体对底速度。在实际工程应用中，由于海底深度的变化，DVL可能频繁地在水跟踪和底跟踪模式之间切换。而通常情况下DVL设定的水流层的水流速度大多是未知的，所以当DVL工作于水跟踪模式时，DVL测得的载体速度将会带有很大的误差，导致SINS/DVL组合导航系统存在很大的定位误差。

SINS/DVL组合导航采用速度组合方式，利用SINS解算得到的导航系下的速度与DVL直接测得的速度经转换修正后作差，将速度差作为 Kalman滤波的观测量。当 DVL工作于底跟踪模式时，能够输出较为准确的载体对底速度信息。然而由于水流速度的存在，当 DVL工作于水跟踪模式时，测得的载体速度有较大误差。在实际工程应用中，由于海底深度的变化，DVL可能频繁地在底跟踪模式和水跟踪模式之间切换。针对这种情况，根据 DVL的工作模式，设置不同的观测量以及量测矩阵，当 DVL进入水跟踪模式时，通过观测量引入洋流速度信息，利用Kalman滤波器对洋流速度进行实时估计。

图1 DVL工作模式示意图Fig.1 Schematic diagram of DVL working mode

1 SINS/DVL组合滤波方程及算法

DVL测速误差主要与声速误差、载体姿态误差等有关[12-13]。考虑到这些误差在实际产品中已经经过补偿，现只考虑 DVL刻度因子误差以及安装角误差。由于水平方向上的两个安装角误差对系统组合导航定位精度的影响相对较小，所以可以忽略水平安装角误差的影响，并且可将DVL刻度因子误差δkD以及航向安装角误差β都当作常值处理。此外，根据现有海洋模型可以得知，在小范围内，洋流速度变化较为缓慢，因此可以假定短期内 DVL设定水流层的洋流速度为恒定值。令洋流速度在地理坐标系东向、天向、北向的分量可分别表示为 Vcn、 Vcu、Vce：

可以得到：

SINS/DVL组合导航系统状态变量由主系统SINS的状态量和辅助子系统 DVL的状态量一起构成。不考虑天向通道的影响，主系统SINS状态量选取如下：北向、东向速度误差δVN、δVE；北向、天向、东向姿态误差φN、φU、φE；北向、东向位置误差δφ、δλ；载体系各轴向的陀螺漂移；加速度计零偏。选取DVL刻度因子误差δkD、航向安装角误差β以及北向、东向洋流速度误差分量δVcn、 δVce为子系统DVL的状态量。可以得到SINS/DVL组合导航系统误差状态方程如下：

根据捷联惯导系统误差方程可以得到系统状态矩阵 FSINS。又有 XDVL=[δkDβ δVcnδVce]T，根据上述分析可以得到系统矩阵 FDVL中的元素均为零。

理想情况下，DVL相对于自身测量坐标系的速度在导航系（n系）下的投影为，其为 DVL测得载体在仪器系（d系）下的真实速度，仪器系到载体系（b系）姿态转换矩阵，为载体系到导航系的姿态转换矩阵。

考虑刻度因子、安装误差等影响因素，DVL实际输出的速度为：

SINS/DVL组合导航Kalman滤波采用速度匹配模式。当DVL工作于底跟踪模式时，无需考虑洋流速度的影响，直接以捷联惯导解算得到的速度和DVL测得的速度之差值作为Kalman滤波器的观测量，即观测量：

式中， Cij(i = 1 ,2,3; j = 1 ,2,3)为姿态转换矩阵 Cnb中相应的元素。对应的系统量测矩阵为：

当DVL工作于水跟踪模式时，DVL输出速度有较大的误差，需考虑洋流速度的影响。即观测量，此时系统量测矩阵变为：

式中，1H、2H 与 DVL工作于底跟踪模式时对应的量测矩阵相同。

选择合适的状态初始值 X0、初始估计均方误差阵P0、系统噪声初始方差阵 Q0以及量测噪声方差阵 Rk，按照离散型 Kalman滤波计算公式即可进行SINS/DVL组合导航Kalman滤波计算。

2 仿真分析

2.1 仿真参数设置

根据水下环境特点以及SINS/DVL组合导航系统的实际使用情况，设置如图2所示的仿真运行轨迹。

图2 仿真运行轨迹Fig.2 Simulation running track

具体仿真条件设置如下：

①轨迹设置

仿真总时间： t=2000s；

转弯时间：400s；

初始位置：φ0=39.8°，λ0=116.2°，h0=-80m；

初始速度： VN0=0m/s， VU0=0m/s， VE0=0m/s；

匀速直航速度： Vb=6m/s；

转弯速度： Vb=3m/s；

初始姿态角：航向角ψ=-90°，俯仰角θ=0°，滚动角γ=0°；初始姿态误差：δψ=0.1°，δθ=0.01°，δγ=-0.01°。

②惯性器件设置

惯性器件采样周期： Tn=5ms；

陀螺漂移：0.01(°)/h的随机噪声；

加速度计零偏：0.0005m/s2的随机噪声。

③DVL参考信息设置

DVL测速采样周期：T=1s；

DVL测速误差：0.02m/s的随机噪声；

DVL 仅对水有效时间段：200～600s，800～1200s。

200～600s时间段：

Vcn=0.3m/s， Vcu=0m/s， Vce=0.3m/s；

800～1200s时间段：

Vcn=0.5m/s， Vcu=0m/s， Vce=0.5m/s。

2.2 仿真结果及分析

仿真结果如图3～5所示，可以得到如下结论：

①200～600s期间载体处于匀速直航状态，800 ～1200s期间载体处于转弯以及加速状态。从洋流速度估计结果可以看出，洋流速度无论在匀速直航还是在加速或者转弯条件下都能够被准确地估计出来，如图3所示。

图3 洋流速度估计曲线Fig.3 Current velocity estimation

图4 未考虑洋流速度情况下组合导航定位误差Fig.4 Navigation error when not taking into account the current velocity

3 湖面试验结果及分析

选取某水库开阔水面开展船载试验。惯导系统选用零偏稳定性 0.02(°)/h的光纤陀螺和零偏稳定性80μg的石英挠性加速度计，选取的 RDI Workhorse Navigator DVL，标称精度为0.4%航程，工作频率为300kHz，整个导航过程中DVL都处于底跟踪模式，对准时间6min。湖面试验所在水域水深较浅，平均水深只有34m，最大深度不到100m。因此，该试验环境无法对该SINS/DVL组合导航算法的进行全面、充分的验证。

考虑到DVL在底跟踪模式下可以同时输出载体对底以及对水速度，假设在指定时间段内DVL连续对底无效，组合导航系统使用DVL输出的载体对水速度参与解算，基于该考虑洋流速度的SINS/DVL组合导航算法可以得到北向、东向洋流速度估计值，如图6～7所示。

将引入洋流速度估计前后系统SINS/DVL组合导航的定位误差进行比较，对比结果如图8～11所示。

图5 考虑洋流速度情况下组合导航定位误差Fig.5 Navigation error when taking into account the current velocity

图6 北向洋流速度估计Fig.6 Northward current velocity estimation

图7 东向洋流速度估计Fig.7 Eastward current velocity estimation

图8 考虑洋流速度前后系统北向定位误差对比Fig.8 Comparison on north positioning errors with/without considering the current velocity

图9 考虑洋流速度前后系统东向定位误差对比Fig.9 Comparison east positioning errors with/without considering the current velocity

图10 系统综合定位误差对比Fig.10 Comparison on positioning errors with/without considering the current velocity

图11 考虑洋流速度前后系统定位误差对比Fig.11 Comparison on percentages of posi-tionning errors with/without considering the current velocity

从上述结果中可以看出，虽然试验所在水域DVL设定水流层的洋流速度只有0.02m/s左右，但在考虑洋流速度前后，系统定位误差仍然有明显区别。2500s至3100s连续对底无效期间，未考虑洋流速度的情况下，系统定位误差增大了20m以上，而引入洋流速度估计后，系统定位误差只增大了5m左右，组合导航精度由0.42%航程提高到0.24%航程。

4 结 论

针对SINS/DVL工作于水跟踪模式条件下的高精度定位需求，提出一种考虑洋流影响的SINS/DVL组合导航算法。分析了DVL水跟踪模式下洋流速度对组合导航精度的影响，基于速度匹配组合导航的方式提出了实时SINS/DVL组合导航系统设计方案。湖面试验结果表明，系统在6min初始对准条件下SINS/DVL组合导航定位精度优于0.24%D，考虑洋流影响的SINS/DVL组合导航算法能够有效提高DVL仅对水速度有效情况下系统组合导航定位精度，该算法具有较好的工程适用性。

（

）：

[1] Paull L,Saeedi S,Seto M,et al.AUV navigation and localization:A review[J].IEEE Journal of Oceanic Engineering,2014,39(1):131-149.

[2] Morgado M,Oliveira P,Silvestre C.Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles:An experimental validation[J].Journal of Field Robotics,2013,30(1):142-170

[3] 陈俊博.SINS/DVL组合导航滤波算法研究[D].哈尔滨:哈尔滨工程大学,2015.Chen J B.Research on SINS/DVL filtering method[D].Harbin:Harbin Engineering University,2015.

[4] 张涛,徐晓苏,李瑶,等.基于惯导及水下声学辅助系统的 AUV 容错导航技术[J].中国惯性技术学报,2013,21(4):512-516.Zhang T,Xu X S,Li Y,et al.AUV fault-tolerant technology based on inertial navigation and underwater acoustics assisted navigation system[J].Journal of Chinese Inertial Technology,2013,21(4):512-516.

[5] 王磊.深海AUV多源导航信息融合方法研究[D].南京:东南大学,2015.Wang L.Research on multi-source navigation information fusion method for deep-sea AUV[D].NanJing:Southeast University,2015.

[6] Sahu B K,Subudhi B.The state of art of autonomous underwater vehicles in current and future decades[C]//First International Conference on Auto-mation,Control,Energy and Systems.2014:1-6.

[7] Liu X X,Zhao Y,Liu Z P,et al.A novel self-alignment method for SINS based on parameter recognition and dual-velocity vectors[J].Proceedings of the Institution of Mechanical Engineers,Part G:Journal of Aerospace Engineering,2015,19(1):75-78.

[8] RD Instruments.Workhorse commands and output data format[M].P/N957-6156-00,March2005.

[9] 刘强,徐晓苏,许江宁.一种基于“当前”统计模型的 DVL/SIMU 船位推算方法[J].中国惯性技术学报,2012,20(4):450-454.Liu Q,Xu X S,Xu J N.DVL/SIMU dead-reckoning method based on“current”statistical model[J].Journal of Chinese Inertial Technology,2012,20(4):450-454.

[10] Wang L,Cheng X H,Li S X,et al.Adaptive interacting multiple model filter for AUV integrated navigation[J].Journal of Chinese Inertial Tech-nology,2016,24(4):511-515.

[11] RD Instruments.Navigator Doppler velocity log technical manual[M].P/N957-6023-00,October1997(Change1).

[12] 于玖成,何昆鹏,王晓雪.SINS/DVL 组合导航系统的标定[J].智能系统学报,2015,10(1):143-144.Yu J C,He K P,Wang X X.Calibration for strapdown inertial navigation/Doppler velocity log integrated navigation system[J].CAAI Transactions on Intelligent Systems,2015,10(1):143-144.

[13] Hodges B A,Fratantoni D M.AUV observations of the diurnal surface layer in the north Atlantic salinity maximum[J].Journal of Physical Oceanography,2014,44(6):1595-1604.