0 引　言

自主性是自主水下机器人（autonomous underwater vehicle，AUV）一个很重要的特征^[1]，而强大的自主定位能力是实现AUV完全自主运动能力的关键。当AUV执行水下对接、水下设备检查等近距离作业任务时，要求有更高的定位精度，而基于视觉传感器与IMU融合的视觉惯性里程计（visual-inertial odometry，VIO）^[2]比传统的水下自主定位方式会有更大的优势。

VIO是同时定位与地图构建方法（simultaneous localization and mapping，SLAM）^[3-4]的主要组成部分，是目前移动机器人领域的一个研究热点。VIO系统根据图像间运动的推算方法可分为特征点法和直接法。特征点法，如VINS-Mono^[5]，ORB-SLAM3 ^[6]等是目前VIO系统的主流方法，它需要提取图像中的角点，并进行特征描述子的计算和图像间角点的匹配，之后通过最小化重投影误差对相机位姿和路标点的坐标值进行估计^[7]，该方法有效使用的前提是图像中有大量代表性的角点。直接法，如VI-DSO^[8]等，基于连续图像间灰度不变的假设，通过最小化光度误差来估算系统的运动状态，该方法不需要计算特征描述子，同时不要求严格的角点匹配，只要图像中存在明显的梯度该方法就可以工作，所以直接法可以应用在缺乏角点、存在大量重复纹理的场景中。

目前，对VIO系统的研究与应用主要集中在陆上环境，并且已经取得了一定的进展，而对水下VIO系统的研究相对较少，其主要原因在于VIO系统在水下场景中的应用存在一些局限性，比如水下是一个弱纹理环境，缺乏明显的角点，存在大量重复纹理，同时视觉传感器在水中的作用范围有限等。针对这些问题，从提高水下VIO系统在复杂水下环境中运行的精度和可靠性出发，对基于直接法的水下VIO系统进行研究。

1 针对水下场景特征点的提取方法

相机拍摄的水下图像如图1所示，可看出水下图像通常有较严重的模糊问题，并且图像中存在较多的一种视觉特征是重复性的纹理，而十分明显的角点较少。由于基于特征点法的VIO系统需要依靠图像中明显的角点进行数据关联，当在水下图像中提取不到足够多有效的角点时，该方法会工作失败。

相较于特征点法，直接法在进行数据关联时不需要提取复杂的特征点，并且不需要特征匹配，所以它对图像特征的处理比特征点法要简单。只要一个像素点有明显的梯度变化，就可以将它作为一个特征点。如图2所示，由于使用这种策略，直接法既可以提取到特征点法中特征明显的角点，也可以提取出大量的除角点外的特征点，所以基于直接法的VIO系统可以在角点缺失、存在大量重复纹理的水下环境中工作。

该VIO系统特征点的提取方式是基于DSO^[9]系统中的方法，其具体提取过程如下：

1）将原图像均匀地划分为大小为 $d \times d$ 的像素块。

2）比较各像素块中每个像素点的梯度模与梯度阈值g_t的大小。如果某一像素块中存在多个大于g_t的像素点，则从中选取沿某一随机方向上梯度模最大的一个像素点作为该像素块中提取到的像素点。如果提取不到，则放大像素块到 ${\text{2}}d \times {\text{2}}d$ ，同时降低g_t，然后重新进行提取。如果仍然提取不到，则将像素块放大到 ${\text{4}}d \times {\text{4}}d$ ，并再次降低g_t，继续提取。该过程如图3所示。

3）如果在图像中提取出的特征点数量过多或过少，则增大d值，否则减小d值，之后再重新提取一遍。

4）如果最终提取到的特征点数量仍然较多，则从中随机删除一些多余的点。

这种特征点提取方式会优先在更小的像素块中取点，如果已经在该区域提取到了特征点，则会放弃在更大的区域中取点。另外，该过程中还会动态调整d的大小，这样做可以在图像上提取到分布均匀且满足数量要求的像素点。上述过程中的梯度阈值g_t是提取点时的一个很重要参数，该阈值的大小会直接影响在不同像素块中提取出的特征像素点数量。对g_t的设置过程如下：

1）将原图像均匀分为32×32大小的像素块。

2）在每个像素块中建立如图4所示的关于像素梯度模的直方图，并统计梯度模的分布。

3）取直方图中占总像素数50%处的梯度模g，然后将该像素块的梯度阈值取为 $ {g'_{{t}}} = g + {g_{{{th}}}} $ 。其中参数g_th表示对g的偏移量，该值的大小会影响提取的特征像素点的数量和质量。

4）对不同像素块间的 $ {g'_{{t}}} $ 进行3×3的均值滤波，将处理后的值作为各像素块的梯度阈值g_t。

为保证在图像中提取到足够多有效的特征点，应该在 $d \times d$ 大小的像素块中提取到更多的特征点。但是相机在水中的可视距离通常在10 m以内，并且随着距离的增大，图像的模糊程度会急剧加重。如果g_th取较大的值，则在距离稍微变大一些后，容易出现在 $d \times d$ 的像素块中提取不到足够多特征点而在 ${\text{2}}d \times {\text{2}}d$ 或 ${\text{4}}d \times {\text{4}}d$ 的像素块中提取出较多的特征点的情况。而如果g_th取较小的值，则在较短距离下会提取到更多的外点。为此，本系统在提取特征点时采用了动态调节g_th的方法：首先使用较大的g_th提取特征像素点，如果在 $d \times d$ 的像素块中提取出的特征点的比例小于92%，则将g_th减小到原来的75%，然后重新提取特征点。

2 基于直接法的视觉状态量的估计方法

直接法比较的是2幅图像间像素点的灰度值，所以该方法构造的能量函数统计了2幅图像间所有特征点的光度误差。

设特征点所在的图像帧为i，这些特征点被投影到的目标帧为j，根据相机的投影模型可以将i帧上的p点投影到j帧上的p′点，即有如下的投影过程：

$ {\boldsymbol{p}}' = {{\boldsymbol{\varPi}} _{\text{c}}}\left( {{\boldsymbol{R}}{\boldsymbol{\varPi}} _{\text{c}}^{ - 1}\left( {\boldsymbol{p}} \right) + {\boldsymbol{t}}} \right)。$

(1)

其中： ${{\boldsymbol{\varPi}} _{\text{c}}}$ 表示投影矩阵； ${\boldsymbol{\varPi}} _{\text{c}}^{ - 1}$ 表示反投影矩阵；R和t表示i帧到j帧间的旋转矩阵和位移向量。

根据直接法的灰度不变假设可构造出p点和p′点间的光度误差为：

$ {e_{{\text{photo}}}} = \left( {{I_j}\left( {{\boldsymbol{p}}'} \right) - {b_j}} \right) - \frac{{{t_j}{{\text{e}}^{{a_j}}}}}{{{t_i}{{\text{e}}^{{a_i}}}}}\left( {{I_i}\left( {\boldsymbol{p}} \right) - {b_i}} \right) 。$

(2)

式中： $ {I_i}\left( p \right) $ 和 $ {I_j}\left( {p'} \right) $ 表示p点和p′点在i帧和j帧上的灰度值；a_i，b_i和a_j，b_j分别表示i帧和j帧的光度系数；t_i和t_j表示i和j帧的相机曝光时间。

由于单个像素的亮度没有区分性，于是采用图5所示的由8个像素组成的像素块来计算每对点的残差，并且假设一个像素块上的点具有相同的逆深度。将i帧所有的点投影到j帧上，可构造出如下的能量函数：

$ {E_{{\text{photo}}}} = \sum\limits_{p \in {O_{{p}}}} {\sum\limits_{p \in {N_{{p}}}} {{{\left\| {{e_{{\text{photo}}}}} \right\|}_{{h}}}} }。$

(3)

其中：N_p表示图5中的像素块；O_p表示i帧中的所有的特征点； ${\left\| \cdot \right\|_{{h}}}$ 表示Huber核函数 $H\left( e \right)$ ，它表示为：

$ H\left( e \right) = \left\{ {\begin{split} &\dfrac{1}{2}{e^2},\quad\left| e \right| \leqslant k ，\\ &{k\left| e \right| - \dfrac{1}{2}{k^2}{\kern 1pt} , \quad\left| e \right| > k} 。\end{split}} \right. $

(4)

式中：k表示残差e的阈值。

由式（3）需要估值的视觉状态量为：

$ x = {\left( {{{\left( {{T_{ij}}} \right)}^{\text{T}}},{a_i},{b_i},{a_j},{b_j},d_{{p_1}}^i,d_{{p_2}}^i, \cdots ,d_{{p_n}}^i} \right)^{\text{T}}}。$

(5)

其中：T_ij表示i和j帧间的相对位姿； $d_{{p_n}}^i$ 表示i帧上的特征点p_n的逆深度。

求出光度误差e_photo关于x的雅可比J后需要通过非线性优化的方法对状态量进行迭代优化，该过程需要构造如下的正规方程：

$ \left( {{J^{\rm{T}}}WJ} \right)\Delta x = - {J^{\rm{T}}}W{e_{\rm{photo}}} 。$

(6)

式中，W表示e_photo的权重。

当最小化公式（3）的能量函数时，为保证解收敛到全局最优值，需要引入图像金字塔来求解。在如图6所示的图像金字塔中，将原始图像作为金字塔的底层L₀，上面一层图像通过对下层进行一定倍率的缩放得到。利用图像金字塔可以使在原图中相距d₀的两像素点，在最高层L_M上的距离减小到d_M，从而增大了解的收敛半径。在求解时，首先从图像金字塔的最顶层L_M层开始计算，然后将该层的计算结果作为下一层求解时的初始值，直到求解到L₀层。

3 实验结果与分析

为比较基于直接法的VIO系统与基于特征点法的VIO系统在不同水下场景中的运行效果，将提出的VIO系统与ORB-SLAM3进行对比实验。该实验使用AQUALOC^[10]水下数据集中3个水下场景中的数据集序列进行了实验，这些序列包括A07，A03和H01，其中序列A07和A03只用了其中的一段数据。这3个序列中的水下图像如图7所示，其中序列A07中的水下图像在3个序列中质量最好，其图像的模糊程度较低，并且图像中具有大量明显的角点。序列A03中的水下图像质量次之，在该图像中存在较多的重复特征，并且含有较多的角点等视觉特征。序列H01的水下图像质量最差，由于该序列记录的是一段海港区域，所以水体中存在大量的悬浮物质，并且该图像具有较严重的模糊问题，同时图像中明显的角点较少。

实验中使用了绝对轨迹误差(absolute trajectory error，ATE) ^[11]作为算法精度的评价指标，并且使用均方根误差（root mean squared error，RMSE）来统计该误差。同时为保证实验的可靠性，在实验过程中将2个系统在每个序列分别重复运行了7次，然后取其中的中值作为实验结果。

这2个系统均方根误差的对比结果如表1所示，运行轨迹的对比如图8所示。由表1可知，ORB-SLAM3在序列A07和序列A03上比本文的VIO系统有更高的定位精度，而本文的VIO系统在序列H01上获得了比ORB-SLAM3更高的定位精度。由于ORB-SLAM3是一个基于特征点法的SLAM系统，所以当图像质量较好，并且环境中具有丰富的角点时，该系统会有更好的性能。但当图像有比较严重的退化问题，并且环境中缺乏明显的角点时，ORB-SLAM3的定位精度会显著的降低，而本文的VIO系统在该水下环境中依然能保证较高的定位精度。

本文的VIO和ORB-SLAM3在序列H01中提取的特征点如图9所示。由于ORB-SLAM3使用了ORB特征，所以它只能提取出图像中特征明显的角点，但是该图像中存在更多的视觉特征是重复性的纹理，而ORB-SLAM3无法充分利用这些重复性的特征，所以当图像中缺乏角点时，ORB-SLAM3的性能会降低。本文的VIO系统由于使用了直接法，所以该系统不但能提取出图像中的角点，还能够在重复性的纹理中提取出一些特征点，所以直接法相较于特征点法可以利用图像中更多的信息。由于ORB-SLAM3提取了图像中较少的特征点，所以只能构造出比较稀疏的地图，而本文的VIO由于使用了直接法，该方法利用了图像中较多的像素点，所以该系统可以构造出相对稠密一些的地图。

4 结　语

为将水下视觉惯性里程计有效地应用在缺乏角点以及存在大量重复纹理的水下场景中，在DSO系统的基础上，在系统的视觉部分采用了基于直接法的数据关联方式，并对该数据关联方式使用了适用于水下场景的特征点的提取方法，并利用直接法构造出关于视觉状态量的估计模型。实验结果表明，该系统在相对恶劣的水下场景中有更好的性能。