0 引　言

自主水下航行器（Autonomous Underwater Vehicle，AUV）具有自主性高、机动性好、探测地域大、隐蔽性强等显著优势，在水下环境中得到广泛应用，而要实现AUV的这些优势，最关键的是要实现AUV的自主导航与探测。然而，精准的AUV自主导算法需要系统提供远距离、长时间的精准速度、位置和姿态信息，而这一技术主要利用SLAM算法完成AUV的自主定位与制图。该算法可以描述为^[1]：AUV从初始的位置出发，在未知环境中运动，依靠自身所携带的传感器对未知环境进行感知，处理计算机对传感器数据进行处理并创建地图，同时依据此地图对AUV进行导航。

SLAM算法最先由国外学者提出^[2]，并在国外发展迅速，由于在机器人的自主定位中广泛应用，算法的种类也越来越多，技术较为成熟。国内SLAM算法研究起步较晚，目前已在地面机器人、空中无人机和水下航行器等领域展开应用。本文提出一种基于EKF的水下SLAM算法^[3]，通过对特征进行2次探测，提高了AUV和特征的定位精度，以便更好实现AUV的自主精确导航与探测。

1 水下航行器模型构建

航行器的运动模型和环境探测模型是SLAM系统的关键部分，AUV在水下的运动状态由航行器系统的运动模型体现，AUV装载的声呐传感器由环境探测模型表示。模型的创建需要在一致的坐标系内，本文采用全局坐标系作为航行器系统模型的基础坐标系。

1.1 AUV水下运动模型

AUV相对于全局坐标系的位置和姿态，均可以通过运动坐标系相对全局坐标系的坐标值 $\left( {{x_g},{y_g},{z_g}} \right)$ 和运动坐标系相对于全局坐标系的旋转角 $\left( {\varphi ,\theta ,\psi } \right)$ 确定。AUV在水下的运动模型为六自由度，则AUV的运动方程可表示为：

$\begin{split} {{\dot x}_g} = & u\cos \psi \cos \theta + v\left( {\cos \psi \sin \theta \sin \varphi - \sin \psi \cos \varphi } \right) + \\& w\left( {\cos \psi \sin \theta \cos \varphi + \sin \psi \sin \varphi } \right) \text{，}\end{split} $

(1)

$\begin{split} {{\dot y}_g} = & u\sin \psi \cos \theta + v\left( {\sin \psi \sin \theta \sin \varphi + \cos \psi \cos \varphi } \right) + \\&w\left( {\sin \psi \sin \theta \cos \varphi - \cos \psi \sin \varphi } \right) \text{，}\end{split} $

(2)

${\dot z_g} = - u\sin \theta + v\cos \theta \sin \varphi + w\cos \theta \cos \varphi \text{，}$

(3)

$\dot \varphi = p + q\sin \varphi \tan \theta + r\cos \varphi \tan \theta\text{，} $

(4)

$\dot \theta = q\cos \varphi - r\sin \varphi \text{，}$

(5)

$\dot \psi = q\sin \varphi /\cos \theta + r\cos \varphi /\cos \theta \text{。}$

(6)

式中： $u$ 为前进速度，m/s； $v$ 为侧向速度，m/s； $w$ 为升降速度，m/s； $p$ 为滚转角速度，rad/s； $q$ 为偏航角速度，rad/s； $r$ 为俯仰角速度，rad/s。

1.2 水下观测模型

声呐的探测信息是AUV测量环境特征位置、方向的信息来源^[4]，特征模型在全局坐标系^[5]中的表示，需要对特征模型在传感器坐标系中的位置和方向 $Z = {\left[ {\rho ,{\psi _s},{\theta _s}} \right]^{\rm{T}}}$ 进行坐标变换。已知传感器安装位置到AUV重心的距离，即传感器坐标原点到运动坐标系的坐标原点距离已知，假设两坐标系坐标原点在同一水平面上且不考虑AUV滚转运动，AUV的观测模型如图1所示。

则观测方程可以表示为：

${Z_s}\left( k \right) \!\!=\! \!\left[\!\!\!\! {\begin{array}{*{20}{c}} {\rho \left( k \right)} \\ {\psi \left( k \right)} \\ {\theta \left( k \right)} \end{array}} \!\!\!\!\right] \!\!=\!\! \left[\!\!\!\!\!\!\! {\begin{array}{*{20}{c}} {\sqrt {{{\left( {{x_g} \!\!-\!\! {x_s}\left( k \right)} \right)}^2} \!\!+ \!\!{{\left( {{y_g} \!\!-\!\! {y_s}\left( k \right)} \right)}^2} \!\!+\!\! {{\left( {{z_g} \!\!-\!\! {z_s}\left( k \right)} \right)}^2}} } \\ {\arctan \left( {\frac{{{y_g} - {y_s}\left( k \right)}}{{{x_g} - {x_s}\left( k \right)}}} \right) - {\psi _s}} \\ {\arctan \left( {\frac{{{z_g} - {z_s}\left( k \right)}}{{\sqrt {{{\left( {{x_g} - {x_s}\left( k \right)} \right)}^2} + {{\left( {{y_g} - {y_s}\left( k \right)} \right)}^2}} }}} \right) - {\theta _s}} \end{array}} \!\!\!\!\!\!\right] \!\!+\!\! \nu \left( k \right)\text{。}$

(7)

其中， $\left( {{x_s},{y_s},{z_s}} \right)$ 为特征模型在观测坐标系中的坐标表示，根据声呐坐标系与全局坐标系和位置坐标系间的方位关系，可以得出观测模型 $Z\left( k \right)$ 。

2 基于EKF的水下SLAM算法

在建立水下自主航行器（AUV）系统模型基础上，提出采用基于捷联惯导系统与声呐传感器的水下自主航行器同时定位与制图算法EKF-SLAM算法。捷联惯导的导航原理^[6]是通过惯性器件测量水下自主航行器的角速度和3个敏感轴向的加速度，并通过四元数法解算出自主水下航行器的位姿和速度，但是计算所得到的是近似值，存在计算误差，随时间增长导航偏差会不断增大。基于捷联惯导系统与声呐传感器的水下自主航行器同时定位与制图技术的EKF-SLAM算法，利用捷联惯导的输出参数与声呐传感器探测的环境特征估计定位，通过算法的递推更新滤除误差，提高导航定位精度^[7]。

2.1 状态方程

AUV的位置、姿态信息是通过对惯性导航器件的输出进行积分运算得到的，惯导器件的输出参数含有随机误差，将会影响到AUV的定位精度，且随着时间的累积，误差的影响也明显，最终导致AUV迷失方向。本文解决这一问题的方法是通过在AUV的状态向量中引入惯导器件的高斯误差^[8]，通过滤波器消除这一误差。

水下自主航行器的状态方程可以表示成：

${X_v}\left( {k{\rm{ + }}1} \right){\rm{ = }}{F_v}\left[ {{X_v}\left( k \right),{U_v}\left( k \right),k + 1} \right] + \omega \left( k \right)\text{。}$

(8)

式中： ${X_v}\left( {k + 1} \right)$ 为 $k + 1$ 时刻的状态向量； ${F_v}$ 为状态转换矩阵； ${U_v}\left( k \right)$ 为 $k$ 时刻的控制向量； $\omega \left( k \right)$ 为高斯白噪声向量，设其均值为零 $E\left[ {\omega (k)} \right] = 0$ ，协方差为 ${Q_v}\left( k \right)$ ，可以表示为 $E\left[ {\omega \left( i \right) \cdot \omega {{\left( j \right)}^{\rm T}}} \right] = \left\{ {\begin{aligned}& {{Q_v}\left( k \right),{\text{若}}i = j = k}\\& {0,{\text{其他}}}\end{aligned}} \right.$ 。

不考虑AUV的俯仰和滚转运动，即 $\theta {\rm{ = }}0,\varphi {\rm{ = }}0$ ，则状态方程写成矩阵形式为：

${\left[\!\!\!\! {\begin{array}{*{20}{c}} {{x_g}} \\ {{y_g}} \\ {{z_g}} \\ \psi \\ u \\ v \\ w \\ r \end{array}} \!\!\!\!\right]_{\left( {k + 1} \right)}} = {\left[\!\!\! {\begin{array}{*{20}{c}} {{x_g} + \left( {uT + \frac{{{T^2}}}{2}} \right)\cos \psi - \left( {vT + \frac{{{T^2}}}{2}} \right)\sin \psi } {{y_g} + \left( {uT + \frac{{{T^2}}}{2}} \right)\sin \psi + \left( {vT + \frac{{{T^2}}}{2}} \right)\cos \psi } {{z_g} + wT + \frac{{{T^2}}}{2}} {\psi + rT + \frac{{{T^2}}}{2}} {u + T} {v + T} {w + T} {r + T} \end{array}} \!\!\right]_{\left( k \right)}} \!\!\!\!+ \omega \left( k \right)\text{。}$

(9)

其中，

$X_V^G = {\left[ {\begin{array}{*{20}{c}} {{x_g}}&{{y_g}}&{{z_g}} \end{array}} \right]^{\rm{T}}}$ 为AUV在全局坐标系中的位置；

${V^G} = {\left[ {\begin{array}{*{20}{c}} v&w&u&r \end{array}} \right]^{\rm{T}}}$ 为AUV各坐标线速度和转向角速度；

$X_P^V = \left[ {\begin{array}{*{20}{c}} {uT + \frac{{{T^2}}}{2}} \\[6pt] {vT + \frac{{{T^2}}}{2}} \\[6pt] {wT + \frac{{{T^2}}}{2}} \end{array}} \right]$ 为AUV相对于前一时刻的坐标位置。

2.2 观测方程

AUV利用装载在机身上的主动声呐，观测环境信息从而获得特征模型和探测声呐之间的几何位置关系，并以此为参考校正AUV的坐标。声呐探测模型是通过构建声呐传感器同特征模型之间的距离和方向获得的，则第 $i$ 个特征的观测方程为：

$Z\left( k \right) = {H_i}\left[ {{X_v}\left( k \right),{m_i},k} \right] + v\left( k \right)\text{。}$

(10)

其中： $Z\left( k \right)$ 为 $k$ 时刻特征模型观测向量； $v\left( k \right)$ 为高斯观测噪声，均值为零, $E\left[ {v(k)} \right] = 0$ ，协方差为 ${R_i}\left( k \right)$ ，可以表示为 $E\left[ {v\left( i \right) \cdot v{{\left( j \right)}^T}} \right] = \left\{ {\begin{aligned}& {{R_i}\left( k \right),{\text{若}}i = j = k}\\& {0,{\text{其他}}}\end{aligned}} \right.$ 。

已知 ${m_i}$ 是特征模型的状态向量，即 ${m_i} \!\!=\!\! {\left[\!\!\!\! {\begin{array}{*{20}{c}} {{x_m}}\!\!&\!\!{{y_m}}\!\!&\!\!{{z_m}} \end{array}}\!\!\!\! \right]^{\rm{T}}}$ ，那么 ${H_i}\left[ {{X_v}\left( k \right),{m_i},k} \right]$ 即为表示在 $k$ 时刻的AUV状态向量和特征模型状态向量函数。设AUV在运动坐标系中的状态向量为 ${\left[ {\begin{array}{*{20}{c}} {{x_v}}&{{y_v}}&{{z_v}}&{{\varphi _v}}&{{\theta _v}} \end{array}} \right]^{\rm{T}}}$ ，则观测方程可以表示为:

${Z_s}\left( k \right) \!\!=\!\! \left[\!\!\!\!\! {\begin{array}{*{20}{c}} {\rho \left( k \right)} \\ {\psi \left( k \right)} \\ {\theta \left( k \right)} \end{array}} \!\!\!\!\!\!\right] \!\!=\!\! \left[\!\!\!\!\!\! {\begin{array}{*{20}{c}} {\sqrt {{{\left( {{x_g}\!\! -\!\! {x_s}\left( k \right)} \right)}^2}\!\! +\!\! {{\left( {{y_g} \!\!-\!\! {y_s}\left( k \right)} \right)}^2} \!\!+\!\! {{\left( {{z_g}\!\! -\!\! {z_s}\left( k \right)} \right)}^2}} } \\ {\arctan \left( {\frac{{{y_g} - {y_s}\left( k \right)}}{{{x_g} - {x_s}\left( k \right)}}} \right) - {\psi _s}} \\ {\arctan \left( {\frac{{{z_g} - {z_s}\left( k \right)}}{{\sqrt {{{\left( {{x_g} - {x_s}\left( k \right)} \right)}^2} + {{\left( {{y_g} - {y_s}\left( k \right)} \right)}^2}} }}} \right) - {\theta _s}} \end{array}} \!\!\!\!\!\!\right] \!\!+\!\! \nu \left( k \right)\text{。}$

(11)

由于 $Z\left( k \right)$ 是非线性的，只有对观测方程进行线性化，才能通过滤波器实现SLAM算法。利用对AUV状态向量进行雅克比替代^[9]，如下：

$H = \nabla h = \frac{{\partial h}}{{\partial {x_h}}}{|_{\left( {{x_k},{u_k}} \right)}}\text{，}$

(12)

便可实现观测方程的线性化，观测方程可写为：

$\delta Z\left( {k + 1} \right) = H\delta X\left( k \right)\text{。}$

(13)

3 仿真试验与分析

AUV工作环境为100 m×100 m水下模拟环境，以特征点表示水下信标、海丘、海岭等特征明显的海底地貌，仿真图用*表示特征点。AUV是以3±0.3 m/s的恒定航速做匀速运动，初始角为0°，最大转弯角30±3°，最大转弯角速度为20 rad/s。声呐探测的最远距离为30 m，测量误差为0.1 m，则状态方程的误差协方差 $Q{\rm{ = }}\left[ {\begin{array}{*{20}{c}}{{{0.3}^2}}&{0;}&0&{{3^2}}\end{array}} \right]$ ，观测方程的误差协方差 $R{\rm{ = }}\left[ {\begin{array}{*{20}{c}}{{{0.1}^2}}&{0;}&0&{{1^2}}\end{array}} \right]$ 。将初始化后的系统参数载入EKF-SLAM算法的工作环境，开始进行仿真。检验算法在环境特征点为50个时做圆周运动的定位与制图精度，如图2所示。

从图3和图4可以看出，在环境特征点为50个时，AUV的定位与制图精度较高，且呈收敛趋势，算法具有较好的稳定性。AUV在x轴向的定位误差在沿x轴向运动时误差较小，沿y轴向运动时误差较大；在y轴向的定位误差与x轴向的情况相反。

为检验算法的数据关联作用，在环境特征点为50个时做2次圆周运动的定位与制图精度，如图5所示。

从图6和图7可以看出，AUV采用2次探测路径运动时，定位误差明显降低，其主要原因是由于通过数据关联对特征点的重复关联匹配，使特征点位置更好地修正。在AUV的定位误差较大时，对已探测过的区域进行重复探测，可以提高AUV的定位与制图精度。

4 结　语

本文在建立AUV运动模型与声呐模型的基础上，研究了基于EKF的SLAM算法，并采用数据关联方法优化了滤波算法，且提高算法定位精度。在环境特征点较多时，仿真验证了算法在圆形路径下的定位与制图精度，结果表明算法的精度符合实际要求；对比相同环境特征密度下，采用2次探测路径，结果表明该算法可有效提高AUV定位精度，从而验证了该算法的有效性和可行性。实际应用中，外界的三维环境非常复杂，可能还包含许多动态目标，对此还需进一步深入研究。