一种水下组合导航系统行进间对准与姿态估计方法

摘要

本发明涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。运用本发明方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。

说明书

技术领域

本发明涉及捷联惯性导航技术领域，具体涉及到一种水下组合导航系统行进间对准与姿态估计方法。

背景技术

自主式水下航行器(AUV)被广泛应用于国家海防建设、海洋资源勘探等军民领域。高精度、高可靠性的自主导航能力是AUV顺利完成任务的前提。捷联式惯性导航系统(SINS)由于结构简单、体积小、隐蔽性强、便于与其他设备集成化设计等优点,在水下定位导航与授时领域受到广泛关注，成为AUV自主导航定位的重要手段。SINS本质上是以牛顿第二定律为基础的积分推算系统，需要对惯性测量单元(IMU)的输出进行积分运算。因此，任何初始误差都会带入到积分运算系统使得误差不断累积，进而影响SINS长航时、远距离的导航定位精度。水下环境对全球导航卫星系统(GNSS)信号具有屏蔽作用，而多普勒计程仪(DVL)测速精度稳定，可为SINS提供实时外部速度辅助融合信息，SINS/DVL组合导航是当前水下导航的主流方案。

初始对准是SINS进行导航解算的前置步骤，高精度的初始对准技术是水下SINS/DVL组合系统长航时、远距离导航定位的重要保证。初始对准作为一种有效减小初始误差以提升导航精度的重要方法，一般分为粗对准和精对准两个阶段：粗对准将运载体的姿态快速地估计在一个较小的范围内，再通过精对准以获得更高的精度；精对准分为静基座精对准和行进间对准两种。静基座精对准要求运载体在起始阶段处于完全静止状态，先前的研究较多且技术已相对成熟。但由于风浪、水流等外部因素影响，水下初始对准难以满足静止条件，因此对水下行进间对准与姿态估计展开研究更具有现实意义。

在水下行进间对准与组合导航姿态估计过程中，外界环境复杂多变，水下生物、地形地质、水流变化等因素都会对DVL的输出速度产生影响，卡尔曼滤波建立的高斯无偏模型无法准确反映水下噪声。水下行进间对准面临的困难主要有：1)水下环境复杂导致高斯噪声模型不再适用；2)水下环境容易导致量测出现野值。

文献1[A.Sage and G.Husa,“Adaptive filtering with unknown priorstatistics,”Joint Automatic Control Conference,vol.7,pp.760-769,1969.]针对卡尔曼滤波中高斯噪声模型不准确的问题提出了Sage-Husa自适应卡尔曼滤波(Sage-Husaadaptive kalman filter，SHAKF)，但是利用SHAKF方法得到的噪声协方差阵的正定性和噪声模型的准确性难以保证，导致滤波效果变差甚至发散；文献2[B.Zhu and H.He,“Integrated navigation for doppler velocity log aided strapdown inertialnavigation system based on robust IMM algorithm,”Optik,vol.217,p.164871,2020.]提出了交互多模型(interacting multiple model，IMM)方法以匹配不同的噪声模型，但是此算法需要大量先验信息来建立模型库，计算量庞大，实时性较差，制约了其在工程实践中的应用；文献3[沈忱,徐定杰,沈锋,蔡佳楠.GPS/INS组合导航的变分贝叶斯自适应卡尔曼滤波[J].哈尔滨工业大学学报,2014,46(05):59-65.]在GPS/INS组合导航系统中提出了一种变分贝叶斯自适应卡尔曼滤波(variational bayesian adaptive kalmanfilter，VBAKF)以应对不准确的噪声模型，但是此算法需要GPS信息且在受到野值影响时滤波性能下降，难以满足水下行进间对准的要求；文献4[X.Lyu,B.Hu,K.Li and L.Chang,“Anadaptive and robust UKF approach based on Gaussian process regression-aidedvariational Bayesian,”IEEE Sensors Journal,vol.21,no.7,pp.9500–9514,2021.]等将基于机器学习的高斯过程回归模型引入到变分贝叶斯自适应估计算法中，从而增强了系统的鲁棒性，但是算法对系统状态维数和样本数量有严格要求；文献5[G.Chang,“RobustKalman filtering based on Mahalanobis distance as outlier judging criterion,”Journal of Geodesy,vol.88,no.4,pp.391–401,2014.]提出了一种基于马氏距离(mahalanobis diatance,MD)的鲁棒卡尔曼滤波方法(robust kalman filter，RKF)，能够有效地剔除观测数据中野值的影响，但是RKF不能有效自适应噪声模型的变化，这就导致当噪声模型不准确时其性能效果不佳。

基于上述已有研究，现有相关技术的主要缺点是：不能同时满足行进间对准与姿态估计滤波算法的自适应性、鲁棒性与普适性，当存在不准确的噪声模型问题及野值污染问题时(这也是水下环境的面临的一大问题以及技术难点)滤波性能较差。

发明内容

为了综合解决水下组合导航行进间姿态估计中不准确的噪声模型问题以及野值污染问题，本发明提出一种水下组合导航系统行进间对准与姿态估计方法。该方法基于马氏距离的变分贝叶斯鲁棒自适应卡尔曼滤波器(variational bayesian robust adaptivekalman filter，VBRAKF)，来进行AUV的SINS/DVL组合导航系统行进间姿态估计。

在AUV水下组合导航系统进行行进间对准及姿态估计时，本发明先通过马氏距离方法判断观测信息是否被野值污染：如果判定DVL观测信息没有被污染，利用变分贝叶斯方法自适应观测噪声协方差矩阵；反之，如果DVL观测信息被污染，通过计算膨胀因子以衡量被污染程度，将膨胀因子引入变分贝叶斯方法自适应过程，自适应得到观测噪声协方差。最后通过VBRAKF状态估计得到高精度的水下运载体姿态信息以及导航信息。

本发明采用的技术方案为，一种水下组合导航系统行进间对准与姿态估计方法，该方法分为以下步骤：

S1：将自主式水下航行器(AUV)的捷联惯性导航系统(SINS)与多普勒测速仪(DVL)接入导航计算机，将AUV投放至任务区域。当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；

S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：

定义坐标系：选取“东—北—天(E-N-U)”地理坐标系为导航坐标系，记为n系；选取“右—前—上”坐标系为载体坐标系，记为b系；地心惯性坐标系记为i系；地球坐标系记为e系；计算导航坐标系记为n′系。

S2.1执行导航计算机计时器，初始化时刻k＝0

S2.2计时器时间更新k＝k+1

S2.3执行惯性导航解算

通过陀螺输出的采样角增量和加速度计输出的采样比力速度增量解算得到k时刻的SINS姿态

S3：构建13维离散化的基于马氏距离的VBRAKF滤波模型与观测模型；

具体步骤如下：

S3.1构建组合导航系统13维基于马氏距离的VBRAKF的滤波模型：

S3.1.1执行VBRAKF状态一步预测更新为：

式(1)中，

式(2)中，位置误差为δP＝[δL,δλ]，δL和δλ分别为SINS的纬度误差与经度误差；速度误差δv＝[δv

式(1)中，Φ

Φ

式(3)中，I为13维单位矩阵，T

其中0

式(5)-式(14)中，R

S3.1.2执行VBRAKF状态一步预测均方误差矩阵更新：

式(15)中，P

P

其中diag表示矩阵对角线元素，μg为加速度计零偏误差单位，1μg＝10

Q(k-1)称为k-1时刻的系统噪声矩阵，是一个13×13维的对角矩阵，由SINS陀螺和加速度计的随机误差决定；由于SINS误差特性稳定，因此系统噪声矩阵是恒定的，即Q(k-1)＝Q(0)，初始值为：

S3.2将导航计算机SINS解算速度

式(18)中，

式(18)中第二至第六行是根据观测量Z(k)构建观测模型，

式(18)中，v

Z(k)＝H(k)X(k)+V(k)为组合导航系统的观测模型，H(k)为k时刻的观测矩阵，其表达式为：

式(20)中，I

V(k)为k时刻的观测噪声矩阵，由k时刻的东向速度观测噪声V

设R

E[·]表示求其期望值；以往滤波方法的研究中，当速度观测设备稳定工作时，普遍认为V(k)是高斯白噪声，此时R

观测噪声协方差矩阵R(0)的初始值为：

R(0)＝diag{(0.1m/s)

S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新；

S4.1将k时刻观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布：

观测噪声协方差矩阵未知且时变，服从多元正态分布。在贝叶斯统计中,通常将逆Wishart分布作为多元正态分布协方差矩阵的共轭先验分布。这是本发明处理未知且时变的观测噪声协方差矩阵的方法，将观测噪声协方差矩阵R(k)的先验分布选择为逆Wishart分布，其表达式为：

p(R(k)|Z(1:(k-1)))＝IW(R(k)；u

式(25)中，p(R(k)|Z

S4.2更新逆Wishart分布的自由度参数与逆尺度矩阵

通过k-1时刻输出的后验估计u

U

u

式(26)、(27)中ρ表示变分衰减因子。在实践应用中，ρ∈[0.9,1)，依据经验设定ρ＝1-2

其中，逆Wishart分布U

U

初值u

S4.3构建基于马氏距离的鲁棒化模块，计算k时刻的新息e(k)的马氏距离，并计算膨胀因子ω(k)：

新息

S4.3.1计算新息e(k)马氏距离：

式(29)中M(k)为k时刻新息e(k)的马氏距离。马氏距离公式中间部分

S4.3.2通过S4.3.1得到的马氏距离判断观测量Z(k)是否为野值，计算膨胀因子ω(k)以衡量观测量Z(k)被污染的程度；

具体判断方法为：

选择统计门限函数

如果马氏距离满足M(k)

式(30)中，上标(j)表示牛顿迭代次数，牛顿迭代次数上限设置为100，当前时刻观测噪声协方差阵R(k)利用上一时刻估计值

S4.4构建基于马氏距离的变分贝叶斯鲁棒自适应模块，通过变分贝叶斯方法进行固定点迭代，估计观测噪声协方差矩阵及状态向量；

利用式(1)、(15)、(26)、(27)得到的

变分贝叶斯固定点迭代估计的步骤为：

S4.4.1计算第i次迭代变分贝叶斯估计均方误差矩阵B(k)

S4.4.2计算第i+1次迭代逆尺度矩阵

S4.4.3计算第i+1次迭代的滤波增益K(k)

S4.4.4计算第i+1次迭代近似的状态后验估计

S4.4.5循环执行S4.4.1-S4.4.4，当i＝N-1时，结束迭代；其中，N代表迭代次数；输出k时刻迭代计算完成的状态向量

理论上，N取值越大迭代近似的信息越精确。但是N值过大会增加计算负担，影响系统的实时性。综合考虑滤波精度及计算速度，依据工程经验取N＝5。

输出的状态向量

S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值，并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：

S5.1将S3.3.6得到的滤波估计误差值反馈补偿给S2得到导航解算结果：

S5.1.1将k时刻姿态误差

其中，简记三角函数

S5.1.2将S5.1.1得到的姿态误差矩阵

式(39)中，

S5.2进入下一时刻，循环执行S2.2-S5.2，得到k+1时刻的高精度姿态

本发明具有以下技术效果：

本发明针对水下组合导航系统在行进间对准及姿态估计中，观测噪声未知且时变、观测值易受野值污染时，卡尔曼滤波器性能大大下降的问题，提出了一种水下组合导航系统VBRAKF行进间对准及姿态估计方法。在VBRAKF更新过程中，通过计算新息的马氏距离并计算膨胀因子来增强算法的鲁棒能力，同时通过变分贝叶斯方法实时估计未知且时变的噪声协方差以增强算法的自适应能力。运用本发明方法进行组合导航，系统有着更高的导航精度，更强的可靠性，满足自主、精确水下长航时、长航程的导航定位能力。

附图说明

图1：基于VBRAKF的SINS/DVL组合导航系统结构框图

图2：VBRAKF实施流程

图3：本发明提供的试验船行进轨迹

图4：本发明提供的试验船采集的野值污染情况下的DVL输出

图5：本发明提供的试验船采集的无野值污染情况下的DVL输出

图6：I[0.5° 0.5° 0.5°]条件下的姿态估计俯仰角误差

图7：图6的局部放大图

图8：I[0.5° 0.5° 0.5°]条件下的姿态估计横滚角误差

图9：图8的局部放大图

图10：I[0.5° 0.5° 0.5°]条件下的姿态估计航向角误差

图11：图10的局部放大图

图12：Ⅱ[1° 1° 1°]条件下的姿态估计俯仰角误差

图13：图12的局部放大图

图14：Ⅱ[1° 1° 1°]条件下的姿态估计横滚角误差

图15：图14的局部放大图

图16：Ⅱ[1° 1° 1°]条件下的姿态估计俯仰角误差

图17：图16的局部放大图

图18：Ⅲ[2° 2° 2°]条件下的姿态估计俯仰角误差

图19：图18的局部放大图

图20：Ⅲ[2° 2° 2°]条件下的姿态估计横滚角误差

图21：图20的局部放大图

图22：Ⅲ[2° 2° 2°]条件下的姿态估计航向角误差

图23：图22的局部放大图

图24：俯仰角均方根误差

图25：图24的局部放大图

图26：横滚角均方根误差

图27：图26的局部放大图

图28：航向角均方根误差

图29：图28的局部放大图

具体实施方式

为了使本申请的技术方法和优点更加清晰，下面结合附图对本发明做进一步说明，此处描述的具体实施例仅用以解释本申请，并不用于限定本申请。

图1为本发明所述方法组合导航系统的结构框架；图2为本发明所示方法的实施流程，本发明所述方法包含以下步骤：

S1：将自主式水下航行器(AUV)的捷联惯性导航系统(SINS)与多普勒测速仪(DVL)接入导航计算机，将AUV投放至任务区域。当AUV潜入水下，无法接收到GNSS信号时，通过传统方法进行姿态粗对准；

S2：通过导航计算机接收到捷联惯性导航系统中陀螺与加速度计输出的角速度与加速度增量，利用导航计算机进行导航解算：

S3：构建13维离散化的基于马氏距离的VBRAKF滤波模型与观测模型；

S4：构建基于马氏距离的VBRAKF鲁棒自适应模块，进行VBRAKF状态估计更新；

S5：构建导航误差反馈补偿回路，得到k时刻AUV的姿态、位置、速度在任意时刻的修正值。并进入k+1时刻循环执行S2.2-S4.2，直到导航结束：

本实施例利用船载试验系统验证水下行进间对准与姿态估计算法。船载试验系统的DVL安装于船底，导航效果与自主水下航行器的效果一致。船载试验系统导航设备包括捷联惯导系统，DVL传感器和GNSS接收机，以GNSS/SINS组合导航结果作为参考基准，利用SINS/DVL组合导航验证行进间对准及姿态估计方法的效果。试验船行进轨迹如图3所示，船载试验设备的参数如表1所示。

表1 SINS与DVL的设备参数

为了验证本发明VBRAKF姿态估计方法的有效性，在航行过程中，选取了两段不同时间的600秒数据来进行行进间对准，两段数据的DVL输出分别如图4、图5所示。图4为船载试验系统中DVL的输出，可以看出在多个时刻出现幅值较大的野值。由前面分析可知由于水下环境的复杂性，野值是不可避免且不可预测的，因此水下行进间对准算法的鲁棒化是十分必要的。同时，由于复杂的水下环境，观测噪声协方差矩阵并不是固定的，为了验证所提算法的鲁棒自适应能力，选择另一段不含野值污染的DVL输出，如图5所示，通过人为添加混合高斯分布噪声的方式进行试验验证。

为比较本发明专提出的方法与现有技术的性能，证明其优越性。选取4种方案进行对比分析：三种现有方案KF、RKF、VBAKF以及本发明提出的VBRAKF行进间姿态估计方法。

首先验证水下组合导航系统行进间姿态估计方法的鲁棒性。图4的DVL输出数据段进行了行进间对准以及姿态估计，验证算法的鲁棒性。在实际中，船载试验系统因为一直在执行姿态估计方法，所以姿态是相对准确的。为了验证对准效果，在本次验证中，分别人为设置粗对准结束后水平失准角和方位失准角均为0.5°、1°、2°，即SINS的初始失准角分别为I[0.5° 0.5° 0.5°]、Ⅱ[1° 1° 1°]、Ⅲ[2° 2° 2°]，经过600秒的行进间对准，得到的对准结果如图6至图23所示。

从图6至图23可以看出，KF受观测野值影响明显，当野值出现时，KF估计结果发散。其他三种方法的水平姿态角(俯仰角和横滚角)的滤波精度与收敛速度效果均很理想，且明显好于航向角估计结果，这主要是因为在观测模型中，水平姿态角的可观测性强于航向角。对于航向角而言，当野值出现时，VBAKF虽然可以通过自适应观测噪声协方差阵在一定程度上抑制其影响，但仍有发散趋势，RKF和VBRAKF则具有良好的鲁棒性。计算了不同初始条件下各个方法在对准最后100秒估计误差绝对值的平均值，结果如表2所示。

表2对准估计误差对比

从表2可以看出，KF的姿态估计结果发散，失去了姿态估计的意义。VBRAKF的估计效果明显优于VBAKF，且略优于RKF。试验结果表明了VBRAKF在野值干扰环境下的鲁棒能力。

以下进行水下组合导航系统行进间姿态估计方法的鲁棒自适应性能力评估。在水下环境中，观测量往往会受到各种类型的非高斯噪声污染。评估采用混合高斯分布噪声来模拟非高斯噪声污染。

利用图5的数据进行本发明方法的验证，设置粗对准结束后水平失准角和方位失准角均为1°，即SINS的初始失准角为[1° 1° 1°]。

利用不同姿态估计方法进行50次重复试验的结果如表3所示。

表3姿态估计误差对比

通过图24至图29可以看出，由于水平姿态角(俯仰角和横滚角)的可观测性更强，所以四种滤波方法的滤波结果都在0.1°以下，但是KF的滤波精度和稳定性要明显差于其他方法。对于方位姿态角(航向角)的估计结果，由于受到非高斯噪声的影响，KF的效果极差，已无法满足下步组合导航的要求；RKF虽然能在一定程度上抑制非高斯噪声的污染，但是其效果显然不如VBAKF和VBRAKF。表3反映了对准最后100秒的平均RMSE结果，对于航向角而言，VBRAKF算法的估计结果比KF算法提高了95.61％，比RKF提高了62.22％，比VBAKF提高了14.72％。试验结果验证了VBRAKF行进间对准及姿态估计方法在非高斯环境下具有鲁棒自适应能力。