0 引　言

惯性导航（Inertial Navigation System，INS）是一种自主导航方法，通过测量运载体本身的加速度来完成导航任务^[1]。根据牛顿惯性原理，利用惯性元器件（陀螺仪、加速度计）测量出运载体的加速度，经过积分和运算，可得出速度和位置信息以供导航使用。多普勒测速仪（Doppler Velocity Log，DVL）可以提供全天候、长时间的导航，在水下导航领域得到广泛应用^[2]。SINS/DVL组合导航系统通过集成了两者的技术优势，提供了在水下环境中高精度的姿态、位置和速度信息，为水下航行器和水下机器人等应用提供了可靠的导航解决方案^[3]。

传统的SINS/DVL组合导航误差模型在初始失准角较大的条件下，计算坐标系与理想坐标系偏差较大，出现位置信息迅速发散的结果，不利于航行器的高精度导航。相比之下，李群状态误差模型可以将系统状态纳入到一个坐标系中计算，能够有效抑制大失准角的发散结果。在李群状态右误差模型中姿态误差存在于导航坐标系中。DVL提供的观测信息为载体系下的速度，此时姿态误差也存在于导航系下，因此理论上SINS/DVL组合导航更适合使用李群状态的右误差模型^[4]。

滤波技术也是影响组合导航性能的关键因素之一^[5]。卡尔曼滤波（Kalman Filter，KF）因在高斯分布条件下能得到系统的地推最小方差估计而广泛应用。但是当DVL遇到复杂的水下环境时，DVL的测速数据会受到严重干扰，直接影响到组合导航系统的性能，此时量测信息受到非高斯噪声污染，卡尔曼滤波在应对非高斯噪声时会出现发散的问题^[6]。通常利用抗差滤波和Huber滤波来应对DVL出现异常值，通过修正量测噪声协方差矩阵，提高滤波的鲁棒性^[7]。但是普通抗差滤波面对较大野值，固定不变的自适应因子无法将量测噪声协方差矩阵膨胀至极大值，因此在状态估计中野值的占比仍然较高，最终滤波估计值也将产生较大偏差。Huber滤波在处理较大数据集时的收敛速度较慢，导致计算时间较长；同时Huber滤波在对信号的变化过程引入一定程度的平滑，可能导致对信号特性的损失或模糊化。在自适应抗差滤波中，使用量测新息构建检验统计量，以此判断量测数据是否存在异常，根据统计量所处的区间构造相应等价权值，来调节量测噪声协方差矩阵，进而调整卡尔曼滤波增益，减小量测异常值对滤波结果的影响。

1 SINS/DVL组合导航 1.1 SINS/DVL松组合模型

SINS/DVL组合导航的原理图如图1所示。

其中，$ v/sins $表示捷联惯导系统的结算结果，$ v/dvl $表示DVL的测速信息，量测值为计算值与实际值的差。在组合导航系统中，通过惯性元器件（IMU）和DVL采集传感器数据，捷联惯导系统通过IMU采集的数据进行计算，系统将DVL的速度信息与捷联惯导的速度计算值之差作为量测值，利用卡尔曼滤波器进行数据融合，得到更准确的姿态、速度和位置估计，滤波器的结果同时反馈到捷联惯导系统模型中进行校正，最后将组合导航结果输出。

在该模型中，当初始失准角较大时，在组合导航结果中会出现位置信息偏差较大并且发散的情况，造成导航精度下降。同时当模型中的DVL遇到复杂环境，引起$ v/dvl $出现异常值，使量测信息出现非高斯噪声污染，不符合卡尔曼滤波的高斯分布要求，因此会造成滤波结果发散^[8]。

1.2 李群状态右误差模型

在传统误差模型中，姿态通常利用欧拉角表示，而欧拉角在姿态解算过程中存在万向节死锁的问题导致其在某一转动过程中丢失一个自由度，出现奇异值影响其对姿态的描述。为了应对欧拉角模型存在的问题，在捷联惯导计算中一般将姿态角转化为四元数，而四元数具有非唯一性，一对共轭的单位四元数对应同一个姿态角。李群理论与旋转矩阵之间联系的出现，将姿态和速度纳入同一个群中，可以构成一种特殊欧式群，从而构建新的误差模型。在新的误差模型中，李群解算在不损失精度的前提下，省去了四元数方法中的归一化约束计算，同时能够解决四元数解算的非唯一性问题^[9]。

在李群状态右误差模型中，定义误差状态为：

$ \boldsymbol{X} = \left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{\varphi }}_r}}&{{\boldsymbol{\tau }}_r^v}&{\delta {{\boldsymbol{p}}_r}}&{{{\boldsymbol{\varepsilon }}^b}}&{{\nabla ^b}} \end{array}} \right]。$

(1)

式中：$ {{\boldsymbol{\varphi }}_r} $和$ {\boldsymbol{\tau }}_r^v $为右误差模型的姿态误差和速度误差；$ \delta {{\boldsymbol{p}}_r} $为载体位置误差，m；$ {{\boldsymbol{\varepsilon }}^b} $和$ {\nabla ^b} $为陀螺仪常值漂移误差和加速度计常值漂移误差，单位分别为(°)/h和g。

定义误差状态方程为^[4]：

$ \boldsymbol{\dot X} = \boldsymbol{FX} + \boldsymbol{G{{\boldsymbol{\eta }}^{\boldsymbol{b}}}} 。$

(2)

式中：$ \boldsymbol{F} $为系统矩阵；$ \boldsymbol{G} $为噪声转移矩阵；$ {{\boldsymbol{\eta }}^{\boldsymbol{b}}} $为系统噪声矩阵。表达式分别为^[10]：

$ \boldsymbol{F}=\left[\begin{array}{*{20}{c}}\mathit{\boldsymbol{N}}_2\left(\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times\right)-\tilde{\boldsymbol{\omega}}_{\boldsymbol{in}}^{\boldsymbol{n}} & -\boldsymbol{N}_2 & \boldsymbol{N}_1+\boldsymbol{N}_3 & -\tilde{\boldsymbol{C}}_b^n & 0_{3\times3} \\ (\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times)\left(\boldsymbol{\omega}_{\boldsymbol{ie}}^{\boldsymbol{n}}\times\right)+\left(\tilde{\boldsymbol{g}}^{\boldsymbol{n}}\times\right) & -\left(2\tilde{\boldsymbol{\omega}}_{\boldsymbol{ie}}^{\boldsymbol{n}}+\tilde{\boldsymbol{\omega}}_{\boldsymbol{en}}^{\boldsymbol{n}}\right)\times & -(\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times)\boldsymbol{N}_1 & -(\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times)\tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} & -\tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} \\ \boldsymbol{N}_{\boldsymbol{rv}}\left(\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times\right) & -\boldsymbol{N}_{\boldsymbol{rv}} & \boldsymbol{\boldsymbol{N}}_{\boldsymbol{rr}} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}\end{array}\right] ，$

(3)

$ \boldsymbol{G}=\left[\begin{array}{*{20}{c}}-\tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} & 0_{3\times3} \\ -(\tilde{\boldsymbol{v}}^{\boldsymbol{n}}\times)\tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} & -\tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} \\ 0_{9\times3} & 0_{9\times3}\end{array}\right]，$

(4)

$ \boldsymbol{\eta}^b=\left[\begin{array}{*{20}{c}}\delta\boldsymbol{\omega}_{\boldsymbol{ib}}^{\boldsymbol{b}} & \delta\boldsymbol{f}_{\boldsymbol{ib}}^{\boldsymbol{b}}\end{array}\right] 。$

(5)

其中，$ \tilde{\boldsymbol{C}}_{\boldsymbol{b}}^{\boldsymbol{n}} $和$ \tilde{\boldsymbol{v}}^{\boldsymbol{n}} $为含有误差的姿态转移矩阵和速度参数；$ \tilde{\boldsymbol{\omega}}_{\boldsymbol{in}}^{\boldsymbol{n}} $、$ \tilde{\boldsymbol{\omega}}_{\boldsymbol{ie}}^{\boldsymbol{n}} $和$ \tilde{\boldsymbol{\omega}}_{\boldsymbol{en}}^{\boldsymbol{n}} $为含有误差的导航系相对于惯性系的角速度、地球自转相对于惯性系的角速度和导航系相对于地球的角速度在导航系下的投影，rad/s；$ {{L}} $为载体所处位置的纬度，（°）；$ {{{R}}_{{M}}} $和$ {{{R}}_{{N}}} $分别为地球的子午圈和卯酉圈曲率半径，m；$ {\boldsymbol{v}}_{{E}}^{{n}} $、$ {\boldsymbol{v}}_{{N}}^{{n}} $和$ {\boldsymbol{v}}_U^{{n}} $分别为载体速度$ {\boldsymbol{v}^{\boldsymbol{n}}} $在导航坐标系的投影，m/s。$ \delta {\boldsymbol{\omega }}_{\boldsymbol{ib}}^{\boldsymbol{b}} $和$ \delta {\boldsymbol{f}}_{\boldsymbol{ib}}^{\boldsymbol{b}} $为陀螺仪误差和加速度计误差，单位分别为(°)/h和g。$ {\boldsymbol{N}_1} $、$ {\boldsymbol{N}_2} $、$ {\boldsymbol{N}_3} $、$ {\boldsymbol{N}_{\boldsymbol{rv}}} $和$ {\boldsymbol{N}_{\boldsymbol{rr}}} $表达式分别为^[11]：

$ \boldsymbol{N}_1=\left[\begin{array}{*{20}{c}}0 & 0 & 0 \\ -\mathbf{\boldsymbol{\omega}}_{ie}\sin{L} & 0 & 0 \\ \mathbf{\boldsymbol{\omega}}_{ie}\cos{L} & 0 & 0\end{array}\right]，$

(6)

$ \boldsymbol{N}_2=\left[\begin{array}{*{20}{c}} 0 & -\dfrac{1}{{R}_{{M}}{ + h}} & 0 \\ \dfrac{1}{{R}_{{N}}{ + h}} & 0 & 0 \\ \dfrac{\tan{L}}{{R}_{{N}}{ + h}} & 0 & 0\end{array}\right]，$

(7)

$ {\boldsymbol{N}_3} = \left[ {\begin{array}{*{20}{c}} 0&0&{\dfrac{{{\boldsymbol{v}}_{{N}}^{{n}}}}{{{{\left( {{{{R}}_{{M}}}{{ + h}}} \right)}^2}}}} \\ 0&0&{ - \dfrac{{{\boldsymbol{v}}_{{E}}^{{n}}}}{{{{\left( {{{{R}}_{{N}}}{{ + h}}} \right)}^2}}}} \\ {\dfrac{{{\boldsymbol{v}}_{{E}}^{{n}}}}{{\left( {{{{R}}_{{N}}}{{ + h}}} \right){{\cos }^2}{{L}}}}}&0&{ - \dfrac{{{\boldsymbol{v}}_{{E}}^{{n}}\tan {{L}}}}{{{{\left( {{{{R}}_{{N}}}{{ + h}}} \right)}^2}}}} \end{array}} \right]，$

(8)

$ {\boldsymbol{N}_{rv}} = \left[ {\begin{array}{*{20}{c}} 0&{\dfrac{1}{{{{{R}}_{{M}}}{{ + h}}}}}&0 \\ {\dfrac{1}{{\left( {{{{R}}_{{N}}}{{ + h}}} \right)\cos {{L}}}}}&0&0 \\ 0&0&1 \end{array}} \right]，$

(9)

$ {\boldsymbol{N}_{rr}} = \left[ {\begin{array}{*{20}{c}} 0&0&{ - \dfrac{{{\boldsymbol{v}}_{{N}}^{{n}}}}{{{{\left( {{{{R}}_{{M}}}{{ + h}}} \right)}^2}}}} \\ {\dfrac{{{\boldsymbol{v}}_E^n\tan {{L}}}}{{\left( {{{{R}}_{{N}}}{{ + h}}} \right)\cos {{L}}}}}&0&{ - \dfrac{{{\boldsymbol{v}}_{{E}}^{{n}}}}{{{{\left( {{{{R}}_{{N}}}{{ + h}}} \right)}^2}\cos {{L}}}}} \\ 0&0&0 \end{array}} \right] 。$

(10)

定义右误差模型的观测方程为：

$ \boldsymbol{Z} = \boldsymbol{HX} + {\boldsymbol{V}^{\boldsymbol{b}}}。$

(11)

式中：$ \boldsymbol{Z} $为观测量，其值为惯导解算的速度与DVL测量的速度之差；$ \boldsymbol{H} $为观测矩阵；$ {\boldsymbol{V}^{\boldsymbol{b}}} $为量测噪声；其协方差矩阵为$ {\boldsymbol{R}_{{k}}} $；$ \boldsymbol{Z} $和$ \boldsymbol{H} $表达式如下：

$ \boldsymbol{Z}=\left[\begin{array}{*{20}{c}}\tilde{\boldsymbol{v}}_{\boldsymbol{E}}^{\boldsymbol{n}}-\boldsymbol{v_E^{\boldsymbol{n}}} & \boldsymbol{\tilde{v}_N^n}-\boldsymbol{v_N^n} & \boldsymbol{\tilde{v}_U^n}-\boldsymbol{v_U^n}\end{array}\right]\boldsymbol{^{\mathrm{T}}}，$

(12)

$ \boldsymbol{H}=\left[\begin{array}{*{20}{c}}\boldsymbol{\tilde{C}_n^b} & -\boldsymbol{I}_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}\end{array}\right]^{\rm{T}} 。$

(13)

2 基于量测新息的自适应抗差滤波

在传统卡尔曼滤波中，将量测新息的定义为：

$ \boldsymbol{e}_k=\boldsymbol{Z}_k-\boldsymbol{H}_k{\hat{\boldsymbol{X}}}_{k/k-1} 。$

(14)

式中：$ {\boldsymbol{Z}_{{k}}} $为$ {{k}} $时刻系统观测量；$ {{\boldsymbol{\hat X}}_{{{k/k - 1}}}} $为状态一步预测值，计算公式为：

$ {{\boldsymbol{\hat X}}_{{{k/k - 1}}}} = {F_{{k}}}{{\boldsymbol{X}}_{{{k - 1}}}} 。$

(15)

在实际应用中，系统的理论滤波模型与实际情况存在一定偏差。随机系统的量测噪声特性往往难以准确估计^[12]，这导致了状态一步预测均方误差矩阵的计算存在较大误差，严重时可能会导致滤波器发散。当滤波器发散时，误差协方差矩阵的计算值与实际估计值相差较大。在实际应用过程中，可以利用量测新息构造统计量，用作比较计算值与实际估计值的依据^[13]。

基于$ {e_k} $的协方差矩阵理论计算值与实际估计值分别为：

$ \left\{\begin{array}{*{20}{c}}\boldsymbol{P}_{ek}=\boldsymbol{H}_k\boldsymbol{P}_{k/k-1}\boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{R}_k，\\ \boldsymbol{\hat{P}}_{ek}=\dfrac{1}{m}\displaystyle\sum_{i=1}^N\boldsymbol{e}_{k+1-i}\boldsymbol{e}_{k+1-i}^{\mathrm{T}}。\end{array}\right. $

(16)

式中：$ {{\boldsymbol{P}}_{{{k}}/{{k}} - 1}} $为一步预测值的协方差矩阵，$ {{\boldsymbol{R}}_{{k}}} $为系统量测噪声矩阵。

基于量测新息构造的等价权矩阵如下：

$ \boldsymbol{M}({{\boldsymbol{i}}},{{\boldsymbol{i}}}) = \left\{ {\begin{aligned} & {1，\quad \left| {{{{\boldsymbol{\tilde v}}}_{{{k}},{{i}}}}} \right| \leqslant {{{k}}_0}} ，\\ & {\dfrac{{\left| {{{{\boldsymbol{\tilde v}}}_{{{k,i}}}}} \right|}}{{{{{k}}_0}^{}}}\dfrac{{{{\left( {{{{k}}_1} - {{{k}}_0}} \right)}^2}}}{{{{\left( {{{{k}}_1} - \left| {{{{\boldsymbol{\tilde v}}}_{{{k}},{{i}}}}} \right|} \right)}^2}}}，{{{k}}_0} < \left| {{{{\boldsymbol{\tilde v}}}_{{{k,i}}}}} \right| \leqslant {{{k}}_1}}，\\ & {\infty，\quad \left| {{{{\boldsymbol{\tilde v}}}_{{{k,i}}}}} \right| > {{{k}}_1}} 。\end{aligned}} \right. $

(17)

式中：$ {{{k}}_0} $和$ {{{k}}_1} $为分段阈值，$ {{{k}}_0} $取值范围是1.5～3.5，$ {{{k}}_1} $取值范围是3.5～4.5^{[14, 8]}；$ {\boldsymbol{\tilde v}_{{k}}} = {\boldsymbol{e}_{{k}}}({{i}})/\sqrt {{{tr}}({\boldsymbol{P}_{{{ek}}}}({{i,i}}))} $^[15]；$ {\boldsymbol{e}_{{k}}}({{i}}) $为量测新息的第$ {{i}} $个分量^[9]；$ \left| {{{\boldsymbol{\tilde v}}_{{{k,i}}}}} \right| $为$ {\boldsymbol{\tilde v}_{{k}}} $第$ {{i}} $个分量的绝对值；$ \boldsymbol{M} $矩阵为一个对角阵。当$ \left| {{{\boldsymbol{\tilde v}}_{{{k,i}}}}} \right| $<$ {{{k}}_0} $时，认为观测正常，$ \boldsymbol{M} $矩阵对应第$ {{i}} $个分量为1；当$ \mathit{k}_0 $<$ \left| {{{\boldsymbol{\tilde v}}_{{{k,i}}}}} \right| $<$ {{{k}}_1} $时，认为观测值第$ {{i}} $个分量存在异常，利用$ \boldsymbol{M} $矩阵对$ \boldsymbol{R_k} $进行膨胀，进而降低$ \boldsymbol{\bar K_k} $的值，减小观测在状态估计中的占比^[16]；当$ \left| {{{\boldsymbol{\tilde v}}_{{{k,i}}}}} \right| $>$ {{{k}}_1} $时，此时认为观测值第$ {{i}} $个分量存在严重偏差，失去参考意义，利用$ \boldsymbol{M} $矩阵将$ \boldsymbol{R_k} $膨胀至一个极大值，降低卡尔曼滤波增益，将观测值在状态估计中占比降至一个极小值，通过$ \boldsymbol{M} $矩阵与$ \boldsymbol{R_k} $矩阵结合，得到等价量噪声协方差矩阵$ \boldsymbol{\bar R_k} $，利用$ \boldsymbol{\bar R_k} $代替$ \boldsymbol{R_k} $进行标准卡尔曼滤波，即可得到自适应抗差滤波（IAEKF）。

面对随机系统动态模型存在偏差以及观测模型异常的情况，一种可行的方法是采用基于贝叶斯估计的M-LS抗差滤波算法。该算法的核心思想是首先对观测值序列进行M估计，然后对状态参数进行最小二乘估计^[17]。通过这种方式，可以得到M-LS估计的抗差滤波解的等价递推形式，解的递推形式为^[18]：

$ \hat{\boldsymbol{X}}_{{k}}=\hat{\boldsymbol{X}}_{{k/k - 1}}+\overline{\boldsymbol{K}}_{{k}}(\boldsymbol{Z}_{{k}}-\boldsymbol{H}_{{k}}\hat{\boldsymbol{X}}_{{k/k - 1}}) 。$

(18)

式中：$ {\boldsymbol{\bar K}_{{k}}} $为等价增益矩阵，可表示为

$ \overline{\boldsymbol{K}}_k=\boldsymbol{P}_{k/k-1}\boldsymbol{H}_k^{\rm T}\left(\boldsymbol{H}_k\boldsymbol{P}_{k/k-1}\boldsymbol{H}_k^{\rm T}+\overline{\boldsymbol{R}}_{k-1}\right)^{-1}。$

(19)

式中：$ {\boldsymbol{\bar R}_{{{k - 1}}}} $为等价量测噪声协方差矩阵，表达式为

$ \overline{\boldsymbol{R}}_{k-1}=\boldsymbol{M}\times\boldsymbol{R}_{k-1} 。$

(20)

IAEKF算法流程如图2所示。

3 实验验证 3.1 误差模型对比

本节开展误差模型验证，通过船载实验对李群右误差模型和传统误差模型的导航误差进行对比分析，实验数据为一套安装有IMU和DVL的实验系统中采集获得。船载实验在长江水域进行，实验船同时安装了一个单天线的GNSS接收机，用于接收GPS数据，输出速度和位置。测线起点经纬度为东经110°53´37"北纬30°53´31"，终点为东经110°58´30"北纬30°51´4"，轨迹如图3所示。选取实验数据中的3600 s实测数据用与仿真验证。实验装置所用陀螺仪的常值漂移为$ \left[ {\begin{array}{*{20}{c}} {{{0.01}^ \circ }}&{{{0.01}^ \circ }}&{{{0.01}^ \circ }} \end{array}} \right]/h $，角度游走系数为$ \left[ {\begin{array}{*{20}{c}} {{{0.001}^ \circ }}&{{{0.001}^ \circ }}&{{{0.001}^ \circ }} \end{array}} \right]/\sqrt h $，加速度计常值漂移为$ \left[ {\begin{array}{*{20}{c}} {1 \times {{10}^{ - 4}}}&{1 \times {{10}^{ - 4}}}&{1 \times {{10}^{ - 4}}} \end{array}} \right]g $，加速度计游走系数为$ \left[ {\begin{array}{*{20}{c}} {1 \times {{10}^{ - 5}}}&{1 \times {{10}^{ - 5}}}&{1 \times {{10}^{ - 5}}} \end{array}} \right]/\sqrt {Hz} $。设置系统的过程噪声$ {\boldsymbol{R}_{{k}}} = \left[ {\begin{array}{*{20}{c}} {0.01}&{0.01}&{0.01} \end{array}} \right] $，初始状态误差协方差矩阵$ {\boldsymbol{P}_{{k}}} = \mathrm{diag}\left[ {0_{1 \times 3}^ \circ }\quad{0.1}_{1 \times 3}\mathrm{m/s}\quad{10}_{1 \times 3}\mathrm{m}\right. \left.{{{0.1}_{1 \times 3}}^ \circ /\mathrm{h}}\quad{{1_{1 \times 3}}\mathrm{mg}} \right]^2 $。

表1和表2为系统内IMU和DVL的主要性能指标。

本实验首先在小失准角条件下进行组合导航实验，在小失准角条件下，李群右误差模型与传统误差模型差别较小，下面进行失准角为$ \left[ {\begin{array}{*{20}{c}} {{{10}^ \circ }}&{{{10}^ \circ }}&{{{30}^ \circ }} \end{array}} \right] $的实验。2种模型的姿态、速度和位置估计误差利用均方根误差（Root Mean Squared Error，RMSE）公式计算，其表达式为^[6]：

$ \begin{split} &{{{{\varphi }}_{{\rm{error}}}}=\sqrt {{{\left( {{{\theta}_{{\rm{error}}}}} \right)}^{{2}}}+{{\left( {{{\gamma}_{{\rm{error}}}}} \right)}^{{2}}}+{{\left( {{{{\psi }}_{{\rm{error}}}}} \right)}^{{2}}}} } ,\\ &{{{{v}}_{{\rm{error}}}}=\sqrt {{{\left( {{{v}}_{_{{\rm{error}}}}^{{E}}} \right)}^{{2}}}+{{\left( {{{v}}_{_{{\rm{error}}}}^{{N}}} \right)}^{{2}}}+{{\left( {{{v}}_{_{{\rm{error}}}}^{{U}}} \right)}^{{2}}}} } ,\\ &{{{{p}}_{{\rm{error}}}}=\sqrt {{{\left( {{{{L}}_{{\rm{error}}}}} \right)}^{{2}}}+{{\left( {{{{\lambda }}_{{\rm{error}}}}} \right)}^{{2}}}} } 。\end{split}$

(21)

式中：$ {{{\theta }}_{{{\rm{error}}}}} $、$ {{{\gamma }}_{{{\rm{error}}}}} $和$ {{{\psi }}_{{{\rm{error}}}}} $分别为俯仰角、横滚角和航向角误差；$ {{v}}_{_{{{\rm{error}}}}}^{{E}} $、$ {{v}}_{_{{{\rm{error}}}}}^{{N}} $和$ {{v}}_{_{{{\rm{error}}}}}^{{U}} $分别为东北天三向速度误差；$ {{{L}}_{{{\rm{error}}}}} $和$ {{{\lambda }}_{{{\rm{error}}}}} $分别为纬度和经度误差。2种误差模型的导航误差如图4所示，对应的RMSE如表3所示。

可知，当失准角为$[ {{{10}^ \circ }}\;{{{10}^ \circ }}\;{{{30}^ \circ }} ] $时，传统误差模型的估计效果显著低于李群右误差模型的估计结果。这是因为在大失准角条件下，理想坐标系和计算坐标系的方向差异非常明显，导致传统误差模型的估计效果降低，而李群右误差模型将系统状态纳入同一坐标系，可以有效改善大失准角带来的不良影响。

根据表3可得，在大失准角条件下，李群右误差模型相比与传统误差模型，在姿态、速度和位置误差方面的估计结果分别提升62.5%、31.6%和15.2%。综合导航参数的估计结果，可知在SINS/DVL组合导航系统中，大失准角条件下，李群右误差模型更适用。

3.2 滤波结果对比

在上述实验数据中，DVL的速度测量值存在一些异常值，导致仿真结果在异常值附近发生较大波动，对导航参数计算造成不利影响^[19]。在上述实测数据基础上，使用不同滤波进行仿真验证，观察传统滤波、自适应抗差滤波（IAEKF）、Huber滤波（HRKF）和普通抗差滤波（RKF）的抗干扰效果，其中IAEKF滤波的$ {{{k}}_0} $和$ {{{k}}_1} $分别设置为3.5和4.5，HRKF的$\gamma $值设置为1.6，在该实验中载体的初始失准角设置为$ [ {{0^ \circ }}\;\;{{0^ \circ }}\;\;{{0^ \circ }} ] $，其余条件保持不变。图5为4种滤波的姿态、速度和位置的均方根误差，仍然利用式(21)计算，对应的RMSE如表4所示。

由图5可以看出，对于姿态角和速度导航误差，KF的误差最大，RKF、HRKF与IAEKF基本相当。对于位置误差，KF误差最大，RKF和HRKF次之，IAEKF最小。并且在实验过程中，2000 s附近传统滤波发生较大波动，原因是此时船只所处水域存在大量淤泥，DVL的速度测量值产生较大偏差，针对DVL的测量野值，传统滤波不能有效抗干扰，造成滤波结果在此处发生较大波动，影响导航精度。相比之下，其他3种算法在此处能够有效分辨野值并对其协方差矩阵进行修正，进而达到抗干扰的目的，提高导航精度。

通过RMSE结果可知，IAEKF相较于KF，对于速度和位置估计分别提高了56.3%和69.1%；相较于RKF，速度和位置估计分别提高7.9%和12.5%；相较于HRKF，速度和位置估计分别提高2.4%和7.6%。因此IAEKF在SINS/DVL导航系统出现观测异常值时，具有更好的滤波效果。

4 结　语

针对SINS/DVL组合导航传统误差模型在量测更新和误差传递过程未考虑误差定义的坐标系不统一，以及DVL量测值异常导致速度和位置计算不准确的问题。本文首先利用李群状态右误差模型将姿态和速度误差纳入一个坐标系，来降低大失准角对导航性能的影响。在该误差框架下，引入自适应抗差卡尔曼滤波，对量测异常值进行判断，自适应调整量测噪声协方差矩阵。通过船载实验，对比分析了传统KF，RKF、HRKF和IAEKF的性能。分析得出在SINS/DVL组合导航系统中，应用自适应抗差滤波算法可以有效抑制DVL观测异常值带来的滤波发散，而且最终的导航误差均优于其他3种算法。因此IAEKF具有更强的抗DVL异常能力，在复杂水域具备更好的鲁棒性和实用性。