0 引　言

随着现代社会无人技术的不断进步和发展，海洋领域的探索需求也进一步提高，而无论是对于海洋资源的挖掘还是军事力量的体现^[1]，自主式水下航行器（Autonomous Underwater Vehicle，AUV）扮演了举足轻重的角色，成为了海洋探索与作战的重要工具。而精准的自主导航技术是AUV研究中不可或缺的部分，如何在复杂的海洋环境中保持导航系统的准确性和稳定性，也成为当前全球科研领域中的研究热点之一^[2]。

在水下导航领域中，因无线电信号衰弱而无法利用GPS等设备进行定位，惯性导航系统的累积误差又会随时间增长而不断发散，卡尔曼滤波器成为了水下导航系统中当前较为热门的最优估计器。但传统的KF算法无法解决非线性系统的问题^[3]，针对复杂的水下环境，王迪等^[4]提出一种自适应粒子群优化的SINS/DVL组合导航方法。首先，针对模型中方差估计不一致问题，构建了基于状态变换原理的SINS/DVL导航模型；其次，考虑到复杂水下环境中基于卡尔曼滤波的噪声难以估计问题，构造粒子滤波适应度函数，并设计了一种自适应粒子群优化算法。Bucci等^[5]实现了2种不同的框架：一种是基于集中式迭代UKF的导航方法，另一种是带有并行局部UKF的传感器融合框架，结果分析既注重了导航质量，也保证了滤波器在减少可用测量值时的鲁棒性。而针对单模型的局限性，Zhang等^[6]提出了基于变分贝叶斯（VB）的UKF算法作为改进交互式多模型（IIMM）的自适应子模型，以自适应地估计时变测量噪声协方差。

由于AUV组合导航系统具有较强的非线性和耦合性，而上述所采用的滤波器都没有很好的解决高维非线性系统的滤波估计^{[7 − 9]}。Arasaratnam等^[10]于2009年提出了容积卡尔曼滤波（Cubature Kalman Filter，CKF）算法。该算法通过选取一组对称的容积点在非线性系统模型中传播，从而进行下一时刻的状态估计，其既避免了系统模型线性化的误差，又利用选点的特殊性保持滤波权值一直为正^{[11 − 12]}。

本文在CKF算法的基础上，针对AUV系统模型及测量噪声的不确定性，采用了Huber-M估计理论中的最小化函数进行优化，尽量减少观测噪声所带来的误差干扰；又将自适应因子与先验估计误差协方差矩阵相结合，动态地调整滤波器的参数以降低模型误差对滤波结果的影响。最终利用IMM算法对2种改进算法进行交互融合，最终通过Matlab仿真实验与实际海试实验验证了算法的可行性。

1 AUV组合导航系统 1.1 AUV坐标系转换

水下航行器的导航系统需要处理来自多个传感器的数据，这些数据通常以不同的坐标系表示。在导航过程中，常常需要将这些数据从固定坐标系转换到航行器的运动坐标系，以便更好地理解和控制水下航行器的运动。使用惯性测量单元等传感器获取水下航行器的姿态信息，包括航向、俯仰和横摆角。利用姿态信息，可以将固定坐标系下的数据转换到航行器的运动坐标系中^[13]。

坐标系的建立如图1所示，利用其介绍2种坐标系之间的转换。图中，XOY表示固定坐标系（绝对坐标系），xoy表示运动坐标系（传感器自身的相对坐标系），h表示航向角。

以XOY平面为例，其转换关系可以表示为：

$X = x\cos h - y\sin h ，$

(1)

$Y = x\sin h + y\cos h ，$

(2)

$Z = z。$

(3)

同理可得ZOX平面与YOZ平面的转换关系，按照先绕x轴旋转、再绕y轴旋转、最后绕z轴旋转的规则，利用3个平面的转换关系，最终完成固定坐标系和运动坐标系的转换，可以表示为：

$\boldsymbol{R}=\left[\begin{array}{*{20}{c}}\cos p\cos h & -\cos p\sin h & \sin p \\ \sin r\sin p\cos h+\cos r\sin h & -\sin r\sin p\sin h+\cos r\cos h & -\sin r\cos p \\ -\cos r\sin p\cos h+\sin r\sin h & \cos r\sin p\sin h+\sin r\cos h & \cos r\cos p\end{array}\right]。$

(4)

式中：R为最终的旋转矩阵；r为YOZ平面的横滚角；p为ZOX平面的俯仰角。

1.2 硬件设备组成

AUV导航系统面临的是复杂的海洋环境，单一的传感器难以完成导航任务，组合导航系统已经成为AUV导航系统的主流，而关键优势就在于其具有不同传感器的互补性和冗余性，从而提高整个系统的鲁棒性。因此本文采用组合导航系统，包括GNSS全球定位系统、INS惯性导航系统以及DVL多普勒测速系统。

全球导航卫星系统（GNSS）是一种通过卫星信号来确定接收器位置的导航系统。GNSS系统包括多个卫星定位系统，如美国的GPS、中国的北斗系统、俄罗斯的GLONASS、欧盟的Galileo等。AUV通常会配备至少一种GNSS接收器，用于在水面上浮时获取位置信息，本文所采用的是GPS定位系统。然而，在水下或水面被遮挡的情况下，GNSS信号可能会受到影响，因此AUV通常需要依赖其他导航系统来提供精确的位置信息。

惯性导航系统（INS）是一种利用陀螺仪和加速度计等惯性传感器来跟踪和测量机器人运动的系统。通过测量加速度和角速度，INS可以计算出AUV的速度、姿态和位置信息。相比于GNSS系统，INS系统不受环境影响，可以在水下和水面上都提供可靠的导航信息。然而，由于惯性传感器存在漂移等问题，INS系统通常需要与其他导航系统结合使用，以提高导航的精度和稳定性。

多普勒测速系统（DVL）是一种通过声波来确定水下机器人速度的导航设备。多普勒测速仪的工作原理是基于多普勒频移的，多普勒频移是通过换能器发散的声波与接收的声波之间频率的差值。在已知多普勒频移和发送声波的频率的情况下，即可求出多普勒测速仪相对反射物体的速度值。与GNSS和INS系统相比，DVL系统对水下导航特别有用，因为它不受水下环境的影响，并且可以提供实时的速度信息。

本文所采用的AUV组合导航系统具体原理如图2所示。

2 改进算法实现

在AUV组合导航实际应用中，面临的系统通常是高维且复杂的。传统的卡尔曼滤波（KF）算法、扩展卡尔曼滤波（EKF）以及无迹卡尔曼滤波（UKF）算法在滤波模型为非线性或高维系统时，往往显示出局限性^[7]。针对这一问题，Arasaratnam等^[10]研究开发了容积卡尔曼滤波（CKF）算法。本文以CKF算法为基础，针对系统模型与观测噪声的不确定性而进行改进。

2.1 容积卡尔曼滤波

容积卡尔曼的算法流程像基础的卡尔曼滤波一样，分为预测和校正更新两部分，不同的是，CKF算法是遵循球面径向准则选取对称的容积点，从而获得先验估计状态变量和误差协方差矩阵。下面着重介绍CKF算法的不同之处。

首先是预测部分，将先验误差协方差矩阵进行Cholesky分解：

${{\boldsymbol{P}}_{k - 1|k - 1}} = {{\boldsymbol{S}}_{k - 1|k - 1}}{\boldsymbol{S}}_{k - 1|k - 1}^{\text{T}}。$

(5)

计算容积点：

$X_{i,k|k}^h = {{\boldsymbol{}}S_{k - 1|k - 1}}{\xi _i} + {\hat x_{k - 1|k - 1}} ，$

(6)

${\xi _j} = \left\{ {\begin{array}{*{20}{l}} {\sqrt {n{e_i}} ,}& {i = 1,2, \cdots ,n}，\\ { - \sqrt {n{e_{i - n}}} ,}& {i = n + 1,n + 2, \cdots ,2n} 。\end{array}} \right.$

(7)

式中：${\hat x_{k - 1|k - 1}}$为k−1时刻的预测状态估计。

再将其代入非线性系统状态函数中可得k时刻的预测状态估计，并可求其误差协方差矩阵：

${{\boldsymbol{P}}_{k|k - 1}} = \frac{1}{2} \sum\limits_{i = 1}^{2n} {X_{i,k - 1|k - 1}^*} X_{i,k - 1|k - 1}^{*{\text{T}}} - {\hat x_{k|k - 1}}\hat x_{k|k - 1}^{\text{T}} + {{\boldsymbol{Q}}_{k - 1}} 。$

(8)

式中：${{\boldsymbol{Q}}_{k - 1}}$为过程噪声协方差矩阵。

其次是测量更新，与预测部分一样计算得到容积点后，将其代入非线性系统测量函数中可得测量状态估计，可求其误差协方差矩阵为：

${{\boldsymbol{P}}_{zz,k|k - 1}} = \frac{1}{{2n}}\sum\limits_{i = 1}^{2n} {Z_{i,k|k - 1}^*} Z_{i,k|k - 1}^{*{\text{T}}} - {\hat z_{k|k - 1}}\hat z_{k|k - 1}^{\text{T}} + {{\boldsymbol{R}}_k}。$

(9)

式中：${{\boldsymbol{R}}_k}$为测量噪声协方差矩阵。根据式(7)和式(8)可更新卡尔曼增益，进而更新状态估计误差协方差矩阵。

然而，CKF算法在组合导航系统的应用中也存在一些限制^[14]。该算法是在过程噪声和测量噪声都服从高斯分布的假设中完成的，然而这在某些实际应用中可能并不成立，可能会导致AUV组合导航系统的精度下降。因此本文将针对上述问题，对CKF算法进行优化改进。

2.2 鲁棒CKF算法

在AUV水下航行过程中，导航系统多传感器的测量误差不可避免，系统的鲁棒性尤为重要。此外，海洋环境具有很大的不确定性，尤其是未知的水下区域导致噪声统计具有很大的不确定性，因此实时的估计测量噪声对滤波精度的提升十分重要。本节提出的鲁棒CKF算法利用Huber-M估计中的最小化函数实时估计测量噪声，旨在提高滤波系统对噪声和异常数据的鲁棒性，还能高效的排查各时刻状态估计中所出现的异常值，从而提高了滤波器对噪声和异常数据的抵抗能力。

鲁棒CKF是针对测量误差不确定性的问题，因此将CKF算法中的测量误差协方差矩阵，即式(8)修正为：

${\bar {\boldsymbol{P}}_{zz,k|k - 1}} = \frac{1}{{2n}}\sum\limits_{i = 1}^{2n} {Z_{zz,k|k - 1}^*} - {\hat z_{k|k - 1}}\hat z_{k|k - 1}^{\text{T}} + {\bar {\boldsymbol{R}}_k} 。$

(10)

式中：${\bar {\boldsymbol{R}}_k}$为利用M估计优化后的测量噪声协方差矩阵。

令$\overline {\boldsymbol{R}} = {\overline {\boldsymbol{P}} ^{ - 1}}$，利用M估计最小化函数将其表示为：

${\bar p_{{t_{ii}}}} = \left\{ {\begin{array}{*{20}{l}} { 1/{\sigma _{ii}}}，{ \left| {{r_i}/{\sigma _{{r_i}}}} \right| = \left| {{r_i}^\prime } \right| \leqslant c}，\\ {c/{\sigma _{ii}}\left| {{r_i}^\prime } \right|}，{ \left| {{r_i}^\prime } \right| > c} 。\end{array}} \right.$

(11)

${\bar p_{{t_{ij}}}} = \left\{ {\begin{array}{*{20}{l}} {1/{\sigma _{ij}}}，\left| {{r_i}^\prime } \right| \leqslant c\;{{\text{and}}}\;{\left| {{r_j}^\prime } \right| \leqslant c} ，\\ {\displaystyle\frac{c}{{{\sigma _{ij}}\max \left\{ {\left| {{r_i}^\prime } \right|,\left| {{r_j}^\prime } \right|} \right\}}}}，\left| {{r_i}^\prime } \right| > c\; {\text{or}} \; \left| {{r_j}^\prime } \right| > c 。\end{array}} \right.$

(12)

式中：${\bar p_{{t_{ij}}}}$、${\bar p_{{t_{ii}}}}$分别为$\overline {\boldsymbol{P}}$的元素；${\sigma _{ij}}$、${\sigma _{ii}}$分别为$\overline {\boldsymbol{R}}$的元素；${r_i}$为测量值的残差分量；$c$为常数，取值为1.3～2.0。

2.3 自适应CKF算法

在组合导航系统对AUV进行定位的过程中，动态系统的动力学模型与实际模型之间的偏差，以及载体在水下运动过程受到的多种外部扰动，都可能对状态估计结果产生影响。为了应对这些问题，引入自适应因子对预测状态误差协方差矩阵进行实时调整。

将自适应因子与预测状态估计协方差矩阵相结合，得：

$\begin{split} &{{\boldsymbol{P}}'_{k|k - 1}} = {\omega _k}^{ - 1}{{\boldsymbol{P}}_{k|k - 1}} = \\& {\omega _k}^{ - 1}\left( {\frac{1}{{2n}} \sum\limits_{i = 1}^{2n} {X_{i,k - 1|k - 1}^*} X_{i,k - 1|k - 1}^{*{\text{T}}} - {{\hat x}_{k|k - 1}}\hat x_{k|k - 1}^{\text{T}} + {{\boldsymbol{Q}}_{k - 1}}} \right)。\end{split}$

(13)

式中：${\omega _k}$为自适应因子。

取值为$0 < {\omega _k} \leqslant 1$，将其近似为：

${\omega }_{k} \approx \left\{ \begin{array} {l} 1，其他，\\ \displaystyle\frac{{tr}\left({\boldsymbol{P}}_{zz,k|k-1}\right)}{{tr}\left({{\boldsymbol{P}}}_{zz,k|k-1}^{}\prime \prime \right)}，{tr}\left({{\boldsymbol{P}}}_{zz,k|k-1}^{}\prime \prime \right) > {tr}\left({\boldsymbol{P}}_{zz,k|k-1}\right)。\end{array} \right.$

(14)

式中：$tr\left( {{\boldsymbol{P}}_{zz,k|k - 1}^{}\prime\prime } \right)$可由观测残差的估计，通过滑动窗法计算而得，即：

${{tr}}\left( {{\boldsymbol{P}}_{zz,k|k - 1}^{}\prime\prime} \right) = \sum\limits_{i = 1}^n {{{\left( {{z_k} - {{\hat z}_{k\mid k - 1}}} \right)}_i}} 。$

(15)

式中：${z_k}$和${\hat z_{k\mid k - 1}}$分别为k时刻的测量值和测量状态估计，其差值即为观测残差。

2.4 交互式多模型融合算法

2.2节与2.3节分别针对测量噪声与系统模型噪声模糊而进行了相应的改进，为了同时解决上述问题，本节采用交互式多模型（IMM）算法将其各自的优势融合到一起，以实时解决不同类型的问题。

IMM算法具有独特的结构，它集成了多个并行的子滤波器，每个子滤波器都专门设计用于捕捉目标动态行为的某一特定方面。IMM系统会对前一时刻每个子滤波器的状态估计进行加权求和，这个过程是基于每个子滤波器在当前时刻的可信度来进行的。通过这种方式，系统能够综合各个子滤波器的信息，得出一个更为准确和全面的最终状态估计。最终状态估计不仅被用作当前时刻的系统输出，还会被反馈给各个子滤波器以进行修正。通过比较每个子滤波器的预测与实际观测之间的差异，系统能够调整它们的内部参数和状态，从而提高它们在下一时刻的预测精度。

此外，IMM系统还通过似然函数来更新模型概率转移矩阵，其描述了系统在不同状态之间转移的概率，各个子系统模型概率的大小直接体现了它们当前时刻状态估计的准确程度。通过实时更新模型概率，IMM算法能够确保当前时刻子系统状态估计的权重是精确的。

IMM算法共有4个环节：输入交互、状态滤波、模型概率更新和输出交互，如图3(a)所示。本文提出的交互式多模型自适应鲁棒容积卡尔曼滤波算法（IMM-ARCKF）基于IMM算法的理论框架，有效结合了RCKF和ACKF算法的优势。具体流程如图3(b)所示。

3 算法仿真实验

为了验证本文提出的IMM-ARCKF算法的有效性，进行实验仿真，并与CKF、IMM-CKF算法进行对比分析。仿真实验中模仿真实水下航行器进行导航规划，其中增加了随机误差模拟海浪带来的滤波器数据波动。数据来源选用的先前海试实验中的历史数据。

实验旨在验证交互式模型本身以及CKF改进策略的性能改善，当观测量出现较大波动时，即观测噪声与滤波器初始噪声误差协方差矩阵相差较大时，单一的滤波器并不能进行最优的状态估计，而加入了融合策略的IMM算法，将多个CKF作为子滤波器，并根据其状态估计和误差协方差进行权值分配，最终得到融合后的最优估计，较好地应对了观测模型模糊所带来的滤波误差，如图4所示。而融合了改进CKF算法的仿真结果证明了其对于系统误差以及测量误差的修正。具体数据如表1所示。

此外，仿真实验还截取了IMM算法中的模型概率数据，模型概率可以反映IMM多模型中各个子系统的滤波精度，如图5所示。可以看出，IMM-ARCKF算法中的改进CKF子模型，无论是滤波精度还是实时调整能力，都要优于经典CKF子模型。

4 AUV海试实验

海试实验的AUV设备采用中国海洋大学UVL实验室自研的260型“旗鱼”AUV，外径260 mm，全长2.5 m。舱体主要由5类舱体构成，分别为艏舱、主控舱、电池舱、透水舱、以及电机艉舱。本文所研究的导航系统主要在透水舱与主控舱，其组合导航系统主要由GPS定位系统、中船第七〇七研究所研发的捷联惯性导航设备以及Teledyne Marine公司研发的Pathfinder DVL组成。在进行海试实验前，首先在实验室内部水池中，对AUV设备的各项功能进行调测。经过前期的仿真工作以及水池测试后，最终AUV在青岛团岛湾进行海试实验验证，如图6所示。

海试轨迹如图7所示，转弯处的噪声对AUV导航系统的影响最为明显，但IMM-ARCKF的抗干扰能力以及收敛速度更优于IMM-CKF算法。其中，IMM-CKF算法的误差峰值达到了2.31 m，而IMM-ARCKF算法的误差峰值为1.63 m，峰值均出现于AUV转弯处。改进算法将导航系统位置估计的最大误差降低了29%，均方根误差降低了43%，具体数据如表2所示。

海试实验验证了IMM-ARCKF算法在实际AUV组合导航系统应用中的可行性和优越性，提高了AUV导航精度以及抗干扰能力。

5 结　语

针对传统CKF算法在未知噪声统计特性下精度降低的问题，以及动态系统模型与实际模型之间可能存在的误差，本文分别提出了2种改进CKF算法。通过引入自适应因子实时调节后验估计协方差矩阵，同时通过鲁棒M估计理论精确估计未知噪声，以应对AUV系统模型和测量噪声统计误差的不确定性。最终采用IMM模型融合改进的ACKF和RCKF滤波器。结合其优势，提高了算法在高动态场景下的自适应性和鲁棒性。未来将继续对组合导航算法进行改进，并将其与人工智能相结合，进而不断提高其导航精度。