基于自适应鲁棒滤波的SINS/DVL动基座初始对准方法

肖 强，文笃石，段明磊，杨 童，张成林

(1. 云南公路联网收费管理有限公司，云南 昆明 650100; 2. 西安邮电大学计算机学院，陕西 西安 710000)

0 引 言

随着惯性器件的发展，捷联惯导系统（strapdown inertial navigation system, SINS）在民用和军用等方面的应用越来越广泛[1-2]。捷联惯导和多普勒测速仪（Doppler velocimeter, DVL）组合的导航系统在水下设备中应用成熟。水下的自主导航对初始姿态的精度要求非常高，如何获得一个高精度的初始姿态是一个热点课题。

文献[3]提出了在静基座下的解析对准方法，通过加速度计和陀螺仪的量测值和双矢量定姿方法[4]获得姿态。为了解决在晃动基座上的对准问题，文献[5]提出了坐标系凝固的思路，将姿态阵分解为三个矩阵，其中两个由惯性凝固的坐标系确定，另一个是初始时刻的姿态阵，并通过双矢量定姿方法确定姿态，但是该方法对量测信息的利用率不高。文献[6]提出了基于最优化的对准方法，将对准问题转化为连续姿态确定问题，通过构造包含所有测量信息的对准矩阵获得当前姿态。但这些方法都是基于晃动基座的，为了能够在对准时保持载体的高机动性，需要研究基于动基座的对准方法。文献[7]提出了交错矢量的方法抑制仪表的累积误差，文献[8]推导了严格速度、位置积分公式解决仪表不同频采样的问题。文献[9-10]基于积分公式，推导了位置轨迹积分公式和滑动窗位置轨迹积分公式，提高了SINS/GNSS对准的鲁棒性。这些基于SINS/GNSS的对准方法在水下会因卫星信号受限而无法使用。基于严格积分公式，文献[11]推导了SINS/DVL的积分公式，并研究了精对准过程。文献[12]基于逆向导航原理，提高了对准的速度。但是这些方法都未考虑DVL信息异常值干扰的问题。文献[13]提出了一种抑制DVL异常值噪声的鲁棒对准方法，利用模值匹配技术检测异常噪声、构造鲁棒滤波权值，通过鲁棒四元数滤波器确定姿态，提高了对准的鲁棒性。文献[14]提出了一种基于参数估计的方法实现DVL辅助行进间对准，通过自适应噪声削弱了DVL噪声的影响，提高了对准的稳定性，但对DVL异常值检测不足，鲁棒性不强。文献[15]基于矢量重构技术，通过重构观测矢量隔离了DVL异常干扰，提高了对准精度。但是鲁棒滤波器对DVL的量测噪声敏感，无法动态跟踪噪声，从而降低了对准的精度。

为解决文献[15]方法中的问题，本文提出基于自适应噪声的鲁棒滤波技术动态跟踪DVL量测噪声，通过自适应量测噪声削弱传统鲁棒滤波对噪声的敏感度，优化参数估计过程，实现提高矢量重构精度的目的，进而提高粗对准的精度。

1 坐标系定义

1)i系是以地球为中心的惯性系，x轴在赤道平面内且指向春分点，z轴指向地球自转方向，三轴构成右手坐标系；

2)e系是以地球为中心的地球坐标系，x轴在赤道平面内且指向中央子午线，z轴指向地球自转方向，三轴构成右手坐标系；

3)n系是导航坐标系，x、y、z分别指向的东、北、天方向；

4)b系是载体坐标系，x、y、z分别指向载体的右、前、上方向；

5)n0系是惯性凝固导航系，与对准初始时刻的n系一致，并相对于i系无转动；

6)b0系是惯性凝固载体系，与对准初始时刻的b系重合，并与惯性系之间无转动。

2 DVL辅助严格积分公式

3 基于自适应鲁棒滤波的矢量重构方法

首先研究了观测矢量的误差模型，然后分析参考矢量的解析式构成，确定了观测矢量的重构模型，接着提出了自适应鲁棒滤波方法估计参数，提高了参数估计的精度，最后给出了粗对准的算法流程。

3.1 观测矢量误差模型

3.2 观测矢量重构模型

3.3 自适应鲁棒滤波

本文方法较文献[15]的方法的优势就在于自适应噪声的构造。通过重构模型计算观测矢量，并根据计算的观测矢量和测量的观测矢量构造自适应量测噪声，提高滤波的鲁棒性。

3.4 算法原理与总结

本文方法设计了自适应量测噪声的鲁棒滤波器估计矢量重构所需的参数，解决了传统方法在参数估计方面的不足：对量测信息的噪声敏感，无法动态跟踪量测噪声，从而降低了参数估计的精度，进而影响到粗对准的精度和速度。流程如表1所示。

表1 本文对准方法的流程总结

4 仿真实验与分析

简记表23种方法。

表2 对准算法和测试条件

图1 运动姿态和轨迹信息

图2 DVL输出值

图3是3种方法的观测矢量重构的结果。从图中可以看到，DVLR1和DVLAR基本重合，说明在DVLR1方法中的鲁棒滤波估计参数时，当量测噪声参数R准确时，参数估计得到的重构矢量和DVLAR的自适应滤波一致，而DVLR2因其R小于实际仪器误差，所以在估计参数时出现了不足，导致重构的观测矢量出现如图3的误差，其中x轴出现了漂移误差，y轴在 0 ～100s出现一个向下的误差波动。

图3 观测矢量重构

图4是3种方法的滤波器量测噪声参数。DVLR1和DVLR2是传统的滤波方法，量测噪声是一个定值，而DVLAR方法是一个动态变化的自适应噪声。随着对准的进行，DVLAR的自适应噪声从初始的较大值逐渐收敛至一个自适应值，也正是这个自适应噪声参数使得DVLAR方法可以动态跟踪DVL的噪声，削弱了传统方法[15]对仪表噪声的敏感度，提高了参数估计的准确性，使得观测矢量重构精度更高，对准的鲁棒性更强，对准精度更高。

图4 滤波器量测噪声

图5是三种方法的对准误差对比图。图5(a)中俯仰角对准，DVLR1和DVLAR在100 s内达到收敛精度并达到稳定，DVLR2用了300 s左右才达到和另两种方法相当的精度，对准速度慢了两倍。文献[15]的传统方法因其对DVL噪声敏感，使得DVLR2在R与DVL实际噪声不匹配时表现了和DVLR1方法不同的性能。对准600 s，DVLR1的误差 为 - 8.886×10-4°，DVLR2误差 为 - 6.245×10-3°，DVLAR为 - 6.085×10-4°，DVLAR比DVLR2准确度提高了90%。

图5 对准误差

从图5(b)横滚角误差中可以看到，DVLR2方法进行到120 s才达到收敛，而DVLR1和本文方法DVLAR在30 s左右就达到收敛精度，对准速度提高了75%。并且随着对准过程进行到400 s，DVLR2方法开始出现一个向下的误差漂移，精度下降。600 s对准结束时刻，DVLR1误差为-9.298×10-4°，DVLR2误 差 为 - 9.251×10-2°，DVLAR为-1.301×10-3°，DVLAR较DVLR2误差降低了98%。

图5(c)航向角对准中，DVLR1和DVLAR都在不到200 s处达到收敛，但是DVLR2方法用了近500 s的时间才达到和DVLR1和DVLAR方法相当的精度，DVLAR方法对准速度较DVLR2提高了60%。600 s对准DVLR1误差为 - 1.969×10-2°，DVLR2为0 . 1507°，DVLAR为- 3.138×10-2°，DVLAR相比于DVLR2准确度提高了79%。

DVLAR方法因自适应噪声的鲁棒滤波技术估计参数重构矢量，增强了对DVL测量噪声的动态跟踪能力，弱化了文献[15]传统方法对DVL噪声的敏感度，提高了粗对准的速度和精度。

从表3可以看出，在对DVL的噪声设置不准确时，传统方法DVLR2的均值和标准差误差都明显大于DVLAR，并且设置准确的DVLR1和DVLAR的误差相当。从误差的降低率可以看到，DVLAR方法要显著优于DVLR方法[15]。DVLAR方法得益于动态跟踪DVL的测量噪声，使得参数估计更准确，重构的观测矢量更接近真实值，从而粗对准的速度更快，精度更高。

表3 航向角误差统计

基于自适应噪声的本文方法也更适合实际使用，在组合导航中可能存在DVL多路径效应和环境恶劣等意外情况，同时传统方法需要根据测速仪器件设置测量噪声，而本文方法无需该参数设置，使用简单，采用算法进行自适应噪声更新，动态跟踪测量噪声，鲁棒性更强，抗干扰能力显著优于传统方法。

5 结束语

针对传统的基于矢量重构的动基座鲁棒对准方法对DVL测量噪声敏感的问题，本文研究一种基于自适应噪声鲁棒滤波技术的动基座鲁棒粗对准方法。通过对鲁棒滤波估计观测矢量重构参数的过程构造自适应噪声，实现动态跟踪DVL测量噪声，削弱传统鲁棒滤波技术对DVL测量噪声的敏感度，改善矢量重构的估计精度，提高动基座粗对准的速度和精度。仿真实验结果表明，本文动基座鲁棒对准方法较传统方法具有更好的鲁棒性，对准更稳定，精度更高，速度更快。