自主水下航行器(Autonomous Underwater Vehicle，AUV)无论在军事还是民用领域都有广泛的应用。然而，目前可用于水下导航的技术仍然十分有限。全球卫星导航系统(Global Navigation Satellite System，GNSS)在陆地、空中及水面舰艇导航得到了广泛的应用，但是GNSS信号在水下很快会被阻隔。捷联式惯性导航系统(SINS)可以提供完备的导航参数，且更新频率高，是实现水下自主导航的一个重要手段。但是，由于惯性导航系统的误差会随时间累积，难以满足长航时高精度导航的需求，为此，需要额外的辅助传感器[1-3]。多普勒测速仪(DVL)是根据多普勒效应测量载体速度的仪器[4]。为实现水下远程、长航时、高精度自主导航，多采用惯导/多普勒(SINS/DVL)组合，以卡尔曼滤波的方式进行组合导航[5-7]。
DVL利用安装在载体上的超声换能器向海底发射超声波，并根据多普勒效应测量载体的速度。DVL的测速精度对组合导航的精度显然是至关重要的。然而，多普勒在使用的过程中，由于遇到鱼群、水下地形的突变等原因，DVL的测速精度会出现下降的情况[8]。若水深超过测速的工作范围，DVL只能获得相对于水流的速度，甚至出现失效的情况。为此，需要对DVL测速精度发生变化时的组合导航方法进行研究。目前，常用的手段主要有两种:一是多普勒测速精度下降或者多普勒失效时，通过载体的运动学模型代替DVL输出速度观测量[9],该方式可以在DVL失效的条件下短时维持组合导航的精度，但是在一定程度上增加了算法的复杂度，需要增加一个子滤波器实时估计载体的速度;二是采用自适应滤波的方式[10],典型的自适应滤波算法有Sage-Husa自适应滤波、基于遗忘因子的自适应滤波算法、多模卡尔曼自适应滤波算法、过程噪声自适应滤波算法等[11-14]。自适应滤波通过在线调整系统噪声矩阵或者量测噪声矩阵来降低系统模型或者噪声统计特性不准确所带来的影响。可以在一定程度上降低DVL精度下降的影响。然而，自适应滤波算法在选择和使用的时候需要考虑它的可靠性，否则容易出现滤波发散的现象[15]。
量测噪声反映的是组合导航系统量测值的精度特性，若DVL的精度下降，则可以对量测噪声进行在线调整，以此来降低对组合导航的精度。基于此，本文提出一种基于新息的抗差自适应滤波算法。算法通过χ2检验判断DVL的测速精度是否发生变化，DVL的测速精度若发生明显变化，组合导航系统则切换到自适应滤波算法,以降低测速精度下降带来的影响。
1 SINS/DVL组合导航系统
1.1 系统方程
SINS/DVL采用速度组合方式，即用DVL输出的速度信息辅助惯导，用DVL和惯导输出的速度信息的差值作为量测值，利用卡尔曼滤波对惯导系统误差进行估计，然后对惯导系统进行校正。根据惯性导航系统的误差方程可以建立组合导航系统的状态方程。选取北-天-东(NUE)地理坐标系为导航参考坐标系，记为n系。惯性导航系统的速度误差方程、姿态误差方程可以表示为：
(1)
(2)
式(1)、(2)中：δv为速度误差； φ为姿态误差； fn为在导航坐标系中的比力；
为地球自转角速度在导航坐标系中的表示；
为导航坐标系下的n系相对于e系的转动角速度。
加速度计零偏与陀螺零偏建模为常值，则有：
(3)
(4)
组合导航系统的状态量选择如下：北向和东向的速度误差δVN和δVE；姿态误差角φx、φy、φz；加速度计零偏▽x、▽y、▽z；陀螺零偏εx、εy、εz。即系统的状态量为：
(5)
则系统的状态方程可表示为：
(6)
式(6)中：W(t)为零均值高斯白噪声，由陀螺仪、加速度计的性能确定； F(t)为系统矩阵，其具体形式可参见文献[16]。
1.2 量测方程
DVL的速度观测值可以通过惯性导航系统姿态转换到导航坐标系下：
(7)
式(7)中：b为载体系；d为DVL坐标系。方向余弦阵
的计算值可以表示为：
(8)
式(8)中，φ×为姿态误差φ的斜对称矩阵。则式(7)可化为：
(9)
惯性导航系统与DVL的速度之差即为组合导航系统的观测量，即：
(10)
由于惯性导航系统的天向通道是发散的，去掉天向通道的观测值，则观测方程为：
(11)
式(11)中，η(t)为零均值高斯白噪声。观测矩阵H(t)为：
(12)
式(12)中，vDN、vDU、vDE分别为多普勒测得的北向、天向、东向速度。其可由以下公式得到：
(13)
2 SINS/DVL组合导航系统
2.1 基于新息的卡尔曼滤波算法
组合导航系统的状态方程和观测方程经过离散化，可化为：
(14)
式(14)中：Φk为状态转移矩阵； Γk为噪声分配矩阵；Wk与ηk是互不相关的高斯白噪声，它们的噪声方差分别用Q和R来表示。在卡尔曼滤波中，一般采用固定系统噪声与量测噪声方差矩阵的形式。其中，量测噪声方差代表了测量数据可信的精度。量测噪声方差越大，说明观测值越不可靠，反之亦然。为保证组合导航的精度，可以在滤波中根据实时的量测数据自动调整量测噪声方差。基于新息的自适应滤波算法正是基于这个思想，根据新息序列，实时估计量测噪声方差。
新息序列定义为：
εk=Zk-HkXk，k-1=Hk(Xk-Xk，k-1)+ηk
(15)
当新息序列为零均值高斯白噪声时，卡尔曼滤波为最优。对式(15)两边求方差，可得：
(16)
式(16)中，Pk,k-1为k时刻卡尔曼滤波协方差阵矩阵的预测值。因此，利用新息序列就可以求出量测噪声Rk的值，即：
(17)
计算时仅保留对角线上的元素。在实际使用的过程中，通常用一个滑动数据窗来保存量测噪声的在线估计值，并且用前m个时刻量测噪声的平均值作为当前噪声值，即：
(18)
式(18)中，m为滑动平均窗的长度。
2.2 自滤波算法流程
在实际使用的过程中，为了保证卡尔曼滤波的稳定性，仅在多普勒观测值的精度有明显变化或者异常的时候再切换到自适应滤波。本文采用χ2检验法来判断DVL的测量值是否出现异常[6]。
定义新息的协方差矩阵为：
(19)
利用新息可以构造一个统计量：
(20)
统计量γk服从χ2分布，通过设定误警率可以确定阈值TD，则可以利用以下准则来判断DVL的测量值是否出现异常，即：
(21)
若DVL数据异常，则切换到自适应滤波方式。综合2.1节、2.2节，本文所提出的抗差自适应滤波算法整体流程如图1所示。DVL数据正常时，组合导航系统工作在普通卡尔曼滤波的形式，并利用滑动数据窗口实时估计量测噪声。当通过χ2检验判断DVL数据出现异常时，则切换到自适应滤波模式，以降低DVL数据异常对组合导航精度的影响。
图1 抗差自适应滤波算法流程框图
3 数据处理与结果分析
3.1 组合导航性能分析
为了验证本文的算法，采用长江某次船载实验的实测数据进行分析。实验航程约30 km，航行时间约6 600 s。试验采用某型激光陀螺捷联惯导系统，陀螺零偏稳定性0.02 deg/h(1σ)，加速度计零偏稳定性5×10-5g(1σ)，采样频率为200 Hz，姿态更新频率为100 Hz。多普勒测试仪的发射频率为300 kHz，速度更新频率为1 Hz，测速精度约为0.5% V±0.5 cm/s。实验船上装载GNSS接收机，以SINS/GNSS组合导航的输出结果作为基准，评估SINS/DVL组合导航的性能。船载实验的轨迹如图2所示。
图2 船载实验行船轨迹
图3为SINS/DVL组合导航姿态误差曲线。由于DVL提供的是载体系d下速度观测值，SINS/DVL方位误差角的可观测度较弱。水平误差角在组合导航模式下快速收敛，而航向误差角收敛较为缓慢。但是，从图4的姿态误差局部放大曲线可以看出，SINS/DVL组合导航的姿态误差能够随着时间收敛，与SINS/GNSS组合导航的结果比较，组合导航最终的方位角误差稳定在0.02°以内，水平姿态角误差稳定在0均值附近。
图3 SINS/DVL组合导航姿态误差曲线
图4 SINS/DVL姿态误差曲线(局部放大)
图5为SINS/DVL组合导航位置误差曲线。受方位误差角收敛缓慢的影响，位置误差在组合导航的前半段(0～3 000 s)变化较为剧烈，且变化规律具有一定的随机性。随着组合导航的姿态误差趋于稳定，在组合导航的后半段，位置误差曲线的变化也较为平稳。然而，在无位置观测的模式下，SINS/DVL组合导航的误差累积是无法消除的。组合导航最终的定位误差约为110 m，相当于航程的3.67‰。
图5 SINS/DVL组合导航位置误差曲线
3.2 量测噪声在线估计
为分析量测噪声自适应滤波算法的性能，采用不同长度的滑动窗口长度(m=25,50,100)、不同的量测噪声初值估计量测噪声。图6为在不同量测噪声初值条件下利用自适应滤波算法实时估计量测噪声曲线。量测噪声的初始值分别设为0.5 m2·s-2、0.1 m2·s-2、0.01 m2·s-2，滑动窗口的长度设定为50。由图6结果可以看出，采用不同初始值所估计的量测噪声方差很快收敛到几乎同一条曲线上。由此可见，基于新息的自适应滤波算法在估计量测噪声时，对量测噪声的初始值并不敏感，具有较强的鲁棒性。
图6 不同量测噪声初值估计量测噪声曲线
图7为采用不同长度滑动数据窗口估计量测噪声曲线。量测噪声的初始定为0.5 m2·s-2。实验结果表明，采用不同长度滑动数据窗口所估计的量测噪声方差值很快收敛到同样的数值附近。从图8局部放大曲线可以看出，当用于估计量测噪声的滑动窗口比较短时，量测噪声的估计值抖动比较厉害，而滑动窗口较长时，则量测噪声的估计曲线则比较平稳。在实际应用中，若想尽量消除历史观测数据对滤波的影响，则可以选取较短的滑动数据窗。
图7 不同长度滑动窗口估计量测噪声曲线
图8 不同长度滑动窗口估计量测噪声曲线(局部放大)
3.3 自适应滤波算法性能评估
为评估本文所提出的抗差自适应滤波算法的性能，在DVL的原始观测值中人为加入噪声，如图9所示，在DVL数据4 000～4 300 s这个时段，3个方向的速度观测值中均加入均值为0、标准差为1的正态分布随机噪声。以SINS/GNSS组合导航的结果作为基准，图10为SINS/DVL组合导航速度误差曲线。由图10中的结果看出，以普通卡尔曼滤波进行组合导航，无法消除附加观测噪声的影响。在速度误差曲线的4 000～4 300 s附近，组合导航的速度误差明显增大。而抗差自适应滤波算法则可以明显改善量测噪声所带来的影响，速度误差曲线比较平稳。
图9 DVL速度观测值
图10 SINS/DVL组合导航速度误差曲线
由图11所示的SINS/DVL组合导航位置误差曲线可以看出，在加入了噪声的位置(4 000～4 300 s)，普通卡尔曼滤波算法所得到的位置误差有明显增大。而基于抗差自适应滤波算法的位置误差增长程度略低于普通卡尔曼，进一步验证了自适应滤波算法的优越性。值得注意的是，自适应滤波算法组合导航的结果仍然比不加观测噪声的结果稍差一些。也就是说自适应滤波算法并不能完全消除观测噪声增大的影响。
图11 SINS/DVL组合导航位置误差曲线
4 结论
1) 基于信息的自适应滤波算法能有效地在线估计量测噪声，在不同长度的滑动数据窗口、不同的量测噪声初值条件下，量测噪声估计值都能收敛。
2) 当DVL数据出现异常时，抗差自适应滤波算法能在一定程度上抑制组合导航误差的增长，性能优于普通卡尔曼滤波。
[1] 吕召鹏.SINS/DVL组合导航技术研究[D].长沙:国防科学技术大学，2011.
[2] 王进达,鲁浩,程海彬,等.基于改进Huber M的组合导航鲁棒滤波算法[J].兵器装备工程学报，2019,40(02):104-108.
[3] 王宏力,许强,何星，等.基于强跟踪滤波的单脉冲星/惯性/星光组合导航算法[J].兵器装备工程学报，2018,39(12):101-105.
[4] PAN X,WU Y.Underwater Doppler Navigation with Self-calibration[J].Journal of Navigation,2016,69(02):295-312.
[5] ZHU Y,CHENG X,HU J,et al.A novel hybrid approach to deal with DVL malfunctions for underwater integrated navigation systems[J].Applied Sciences,2017(7):759.
[6] LI W,YANG L,ZHANG,L,et al.A robust method for alignment calibration of an inertial measurement unit (IMU) and Doppler sensors[J].Lasers in Engineering.2016,34:93-106.
[7] 李贤义,齐建宇,赵鹏飞.SINS/DVL水下初始对准与定位导航[J].兵器装备工程学报，2017(04):132-136.
[8] LIU P,WANG B,DENG Z,et al.INS/DVL/PS tightly coupled underwater navigation method with limited DVL measurements[J].IEEE Sensor Journal,2018,18(07):2994-3002.
[9] ZHAO L,LIU X,WANG L,et al.A pretreatment method for the velocity of DVL based on the motion constraint for the integrated SINS/DVL[J].Applied.Sciences,2016(6):79.
[10] 魏延辉,刘静,郝晟功.基于改进自适应滤波的SINS/DVL组合导航算法研究[J].自动化与仪表,2019,34(05):96-101.
[11] WANG Q,CUI X,LI,Y,et al.Performance enhancement of a USV INS/CNS/DVL integration navigation system based on an adaptive information sharing factor federated filter[J].Sensors,2017,17(02):239.
[12] 陈雨,王健博,张凌东等.基于强跟踪滤波的捷联惯导/里程计组合导航[J].现代防御技术,2018,46(04):31-36.
[13] 李明,许川佩,李翔.基于UD分解的Sage-Husa自适应滤波算法[J].计算机仿真,2017(11):296-300.
[14] YAO Y,XU X,LI,Y,et al.A hybrid IMM based INS/DVL integration solution for underwater vehicles[J].IEEE Transactions on Vehicular Technology,2019,68(06):5459-5470.
[15] 宋杰,晋帆.组合导航中抑制自适应滤波发散的研究[J].弹箭与制导学报,2012,32(02):46-48.
[16] 李万里.惯性多普勒组合导航回溯算法研究[D].长沙:国防科学技术大学，2011.