0 引　言

船舶在长期航行和停泊过程中，各种水生生物会附着于船体表面，会产生船体表面被腐蚀，行驶中阻力增大、油耗升高等问题，因此，定期清洗是保障船舶正常航行的必要条件。船舶清洗机器人是为解决人工洗船效率低、劳动强度大、危险度高等问题而设计的自动化装备^[1]。在船舶清洗机器人的相关研究中，定位技术是其研究重点之一^[2]。

在目前的船舶清洗机器人定位方式中，惯性定位仍是主流，但低精度惯性测量单元（Inertial Measurement Unit，IMU）在较长时间的使用中漂移过大，很难实现高精度定位，高精度IMU又价格高昂，限制了其大规模应用。视觉定位在陆上环境中表现良好，但水下环境复杂，纯视觉定位方式极易由于水质混浊等原因而无法正常工作，可靠性不高。视觉惯性里程计（Visual-Inertial Odometry，VIO）通过将相机与低成本IMU进行结合，不仅能准确估计系统尺度信息，还能在图像质量较差、视觉估计失效时保持系统正常运行，是一种兼顾精度与成本的定位系统^[3]。目前针对VIO的研究多为陆地环境，针对水下环境的VIO系统较少。Zhang等^[4]和Xu等^[5]将多波束声呐融入到视觉惯性同步定位与建图（Simultaneous Localization and Mapping，SLAM）中，有效提高了系统的稳健性和适应性，但声呐价格昂贵，不适合大规模应用；Ma等^[6]将深度数据引入单目VIO的初始化中用于尺度估计，改善了水下单目惯性里程计尺度估计困难的问题，但系统长时间运行仍会产生较大尺度漂移；赵洪全等^[7]使用基于单目直接法的视觉惯性里程计对自主水下机器人进行水下定位，在多个数据集上取得了比ORB-SLAM3更高的精度，解决了水下特征提取与匹配困难的问题，但直接法依赖于光度不变假设，在光照变化环境下并不适用。

本文以水下双目VIO系统为研究对象，通过双目相机获得尺度信息，通过图像清晰化处理提高图像质量，并将深度信息融入到双目惯性联合优化中来提高位姿估计的准确性和鲁棒性，从而实现船舶清洗遥控水下机器人（Remotely Operated Vehicle，ROV）的精准水下定位。

1 水下图像清晰化处理

与在空气中不同，由于水的折射、吸收和散射作用，相机拍摄的水下图片往往存在色彩失真、对比度低、细节模糊等问题，这些问题统称为水下图像的质量退化问题。水下图像质量的退化会严重影响图像特征的提取与匹配，从而降低视觉系统的定位精度，因此在使用图像前要对图像进行清晰化处理。

光线在水中传播时，水对不同波段的光吸收作用不同。在可见光中，红光的波长最长，最容易被吸收，蓝绿光的波长较短，不易被吸收，因此相机拍摄的水下图像多成蓝绿色，水下物体颜色失真。同时，由于船舶清洗机器人多通过高压射流装置或刷盘来进行清洗作业，船上附着物往往会被破碎，并随搅动的水流扩散到水中，在光的散射作用下使图片产生较强雾感。

针对颜色失真问题，本文使用基于灰度世界的白平衡算法校正蓝绿色偏，然后使用暗通道先验算法^[8]对校正后的图像进行去雾处理。进行图像处理的目的是为了提高后续特征提取与匹配的精度，为了验证图像处理效果，选择一组水下机器人清洗作业时拍摄的图片进行处理，处理前后ORB特征提取与匹配效果如图1所示。经统计，图像清晰化处理后特征点正确匹配对数为103对，相比于处理前的74对提高了39.2%，验证了处理的有效性。

2 双目-惯性-深度里程计 2.1 系统框架

在实际的船舶清洗过程中，清洗作业会清除船舶表面附着物，从而使环境特征发生变化，传统SLAM系统中的回环检测功能无法正常工作，系统的定位误差会迅速累积，因此陆上的SLAM算法直接应用于船舶清洗机器人上无法达到令人满意的定位精度。深度传感器是水下机器人的常用传感器，其通过测量水压来获取水下机器人相对于水平面的垂直距离信息，具有精度高、价格低等优点。相比于相机、IMU等局部传感器，作为全局传感器的深度传感器没有累计误差，因此当水下机器人运动时，利用深度传感器可以获得直接和精确的重力轴运动^[9]。本文参考ORB-SLAM3系统框架，采用紧耦合方式将相机状态、IMU状态、深度传感器状态融合在一起，共同构建运动方程和观测方程，采用滑动窗口的方式优化求解，从而获得精确的位姿估计，其系统框架如图2所示。

跟踪线程用于实时估计关键帧的位姿，主要包括对3个传感器输入数据的预处理、位姿估计与跟踪局部地图、新关键帧决策；局部建图线程对关键帧进行局部建图和优化，主要包括新关键帧的插入和旧关键帧的剔除、新地图点的创建和旧地图点的剔除、局部地图的光束法平差（Bundle Adjustment，BA）优化、IMU初始化、重力方向细化；地图部分用于保存地图点和关键帧数据，并维护它们之间的共视关系。

2.2 优化模型的建立

在视觉-惯性-深度里程计中，为了进行数据融合，以图像时间戳为基准，将各传感器数据进行对齐。在第$ i $帧时需要估计的状态向量可以表示为：

$ {\boldsymbol{S}}_{i}=\left\{\begin{array}{cccc}{\boldsymbol{T}}_{i}, {\boldsymbol{v}}_{i},{\boldsymbol{b}}_{i}^{g}, {\boldsymbol{b}}_{i}^{a}\end{array}\right\}。$

(1)

式中：$ {\boldsymbol{S}}_{i} $为第$ i $帧时的状态向量；$ {\boldsymbol{T}}_{i} $为机器人在世界坐标系中的位姿，包括旋转矩阵$ {\boldsymbol{R}}_{i} $和平移向量$ {\boldsymbol{p}}_{i} $；$ {\boldsymbol{v}}_{i} $为机器人在世界坐标系下的速度；$ {\boldsymbol{b}}_{i}^{g} $为陀螺仪偏差；$ {\boldsymbol{b}}_{i}^{a} $为加速度计偏差。

对于IMU，根据文献[10]中关于流形的公式，对连续视觉关键帧第$ i-1 $帧和第$ i $帧之间的IMU观测值进行预积分，从而得到预积分的旋转、平移和速度以及整个测量向量的协方差矩阵，分别表示为$ \Delta {\boldsymbol{R}}_{i-1,i} $、$ \Delta {\boldsymbol{p}}_{i-1,i} $、$ \Delta {\boldsymbol{v}}_{i-1,i} $、$ {\sum }_{{I}_{i-1,i}} $。根据这些预积分项与状态向量$ {\boldsymbol{S}}_{i-1} $、$ {\boldsymbol{S}}_{i} $，惯性残差$ {\boldsymbol{r}}_{{I}_{i-1,i}} $被定义为：

$ {\boldsymbol{r}}_{{I}_{i-1,i}}=\left[\begin{array}{ccc}{r}_{\Delta {\boldsymbol{R}}_{i-1,i}}\text{，} {r}_{\Delta {\boldsymbol{v}}_{i-1,i}}\text{，} {r}_{\Delta {\boldsymbol{p}}_{i-1,i}}\end{array}\right]，$

(2)

$ {r}_{\Delta {\boldsymbol{R}}_{i-1,i}}=\mathrm{Log}(\Delta {\boldsymbol{R}}_{i-1,i}^{{\rm{T}}}{\boldsymbol{R}}_{i-1}^{{\rm{T}}}{\boldsymbol{R}}_{\boldsymbol{i}})，$

(3)

$ {r}_{\Delta {\boldsymbol{v}}_{i-1,i}}={\boldsymbol{R}}_{i-1}^{{\rm{T}}}\left({\boldsymbol{v}}_{i}-{\boldsymbol{v}}_{i-1}-\boldsymbol{g}\Delta {t}_{i-1,i}\right)-\Delta {\boldsymbol{v}}_{i-1,i}，$

(4)

$ {{r}_{\Delta {\boldsymbol{p}}_{i-1,i}} = {\boldsymbol{R}}_{i-1}^{{\rm{T}}}\left({\boldsymbol{p}}_{i} - {\boldsymbol{p}}_{i-1} - {\boldsymbol{v}}_{i-1}\Delta {t}_{i-1,i} - \dfrac{1}{2}\boldsymbol{g}\Delta {t}_{i-1,i}^{2}\right)-\Delta {\boldsymbol{p}}_{i-1,i}。} $

(5)

式中：g为重力加速度；$ \Delta {t}_{i-1,i} $为第i−1帧和第i帧之间的时间差；Log为从SO(3)到$ {\boldsymbol{R}}^{3} $的变换。

同惯性残差类似，视觉残差使用重投影误差$ {r}_{ij} $来表示，其形式为：

$ {r}_{ij}={u}_{ij}-\displaystyle\prod {\boldsymbol{T}}_{CB}{\boldsymbol{T}}_{i}^{-1}\oplus{x}_{j}。$

(6)

式中：$ \prod $为相应相机模型从$ {\boldsymbol{R}}^{3} $到$ {\boldsymbol{R}}^{n} $的投影模型；$ {u}_{ij} $为在图像i中对空间点j的观测值，具有协方差矩阵$ \displaystyle{\sum }_{ij} $；$ {\boldsymbol{T}}_{CB} $为IMU坐标系到相机坐标系的变换矩阵，通过标定得到；$ \oplus $为SE(3)到$ {\boldsymbol{R}}^{3} $的变换。

由于深度传感器测量的是传感器相对于水平面的相对位置，因此深度变化等效于重力轴位置变化。在系统初始化时，重力方向可以通过IMU进行估计，然后旋转世界坐标系使z轴与重力方向对齐，从而将深度变化转换为z轴坐标的变化。

对于深度传感器，深度测量值可以表示为：

$d=\overline{d}+{\eta }_{{\rm{depth}}}\text{，}{\eta }_{{\rm{depth}}}\sim N\left(0,{\sigma }_{{\rm{depth}}}^{2}\right)。$

(7)

式中：d为深度测量值；$ \overline{d} $为真实深度值；$ \eta\mathrm{_{depth}} $为均值为0、方差为$ \sigma_{\mathrm{depth}}^2 $的高斯白噪声。

系统初始化时世界坐标系的原点为机器人的位置，为了使深度传感器坐标系与世界坐标系对齐，定义下式：

$ {D}_{k}={d}_{k}-{d}_{0}。$

(8)

式中：$ {D}_{k} $为k时刻深度传感器与世界坐标系对齐后的相对深度值；$ {d}_{k} $为k时刻深度测量值；$ {d}_{0} $为系统初始化时的深度测量值。

为了充分利用深度数据，减少测量误差，在将深度数据对齐到视觉关键帧时，将距离当前关键帧最近时刻的深度值与IMU的预积分值相结合，然后投影到当前关键帧，从而得到更准确的深度对应值。图像和深度时序示意如图3所示。

对于关键帧$ K{F}_{i} $，深度测量值$ {D}_{2} $距其最近，时间上在其后侧，那么其对应深度值$ {D}_{i} $表示为：

$ {D}_{i}={D}_{2}+\varepsilon {\boldsymbol{e}}^{3}\Delta {\boldsymbol{p}}_{i-1,i} 。$

(9)

式中：$ {\boldsymbol{e}}^{3} $为$ \left[\begin{array}{ccc}0& 0& 1\end{array}\right] $；$ \varepsilon $的计算式为：

$\varepsilon =\dfrac{{{t}_{i}-t}_{{D}_{2}}}{{t}_{i}-{t}_{i-1}}。$

(10)

从而，深度残差$ {r}_{{D}_{i-1,i}} $就可定义为当前时刻深度值与z轴坐标之差，即：

$ {r}_{{D}_{i-1,i}}={D}_{i}-{{\boldsymbol{e}}^{3}\boldsymbol{p}}_{i} 。$

(11)

对于关键帧$ K{F}_{i+1} $，深度测量值$ {D}_{3} $距其最近，时间上在其前侧，那么其对应深度值$ {D}_{i+1} $表示为：

${D}_{i+1}={D}_{3}-\varepsilon {\boldsymbol{e}}^{3}\Delta {\boldsymbol{p}}_{i+1,i+2}，$

(12)

$\varepsilon =\dfrac{{t}_{{D}_{3}}-{t}_{i+1}}{{t}_{i+2}-{t}_{i+1}}。$

(13)

从而，深度残差定义为：

${r}_{{D}_{i,i}+1}={D}_{i+1}-{{\boldsymbol{e}}^{3}\boldsymbol{p}}_{i+1} 。$

(14)

结合惯性、视觉、深度残差项，视觉-惯性-深度里程计可以被视为基于关键帧的最小化问题。给定一组 k + 1 个关键帧及其状态$ { \overline{\boldsymbol{S}}_{k}}=\left\{{\boldsymbol{S}}_{0}\cdot \cdot \cdot {\boldsymbol{S}}_{k}\right\} $以及一组 L个 3D 点及其状态 $ \boldsymbol{X}=\left\{{\boldsymbol{x}}_{0}\cdot \cdot \cdot {\boldsymbol{x}}_{L-1}\right\} $，视觉-惯性-深度优化问题可以表述为：

${\underset{{\overline{S}_{k}},X}{\mathrm{min}}\left({\displaystyle\sum }_{j=0}^{L-1}{\displaystyle\sum }_{i\in {K}_{j}}{\boldsymbol{\rho }}_{{\rm{Hub}}}(\| {r}_{ij}\Vert_{\sum ij})+{\displaystyle\sum }_{i=1}^{k}\| {\boldsymbol{r}}_{{I}_{i-1,i}}\Vert_{{\sum }_{{I}_{i-1,i}}}^{2}+{\displaystyle\sum }_{i=1}^{k}\| {r}_{{D}_{i-1,i}} \Vert_{{\sigma }_{{\rm{depth}}}^{2}}^{2}\right)。} $

(15)

式中：$ {K}_{j} $为观察到3D点j的关键帧集合；$ \boldsymbol{\rho}_{\mathrm{Hub}} $为Huber核，用来减少误匹配对重投影误差的影响。

该优化可以用图4因子图表示。

每一帧对应于状态向量$ \left\{\begin{array}{cccc}{\boldsymbol{T}}_{i},& {\boldsymbol{v}}_{i},& {\boldsymbol{b}}_{i}^{g},& {\boldsymbol{b}}_{i}^{a}\end{array}\right\} $ ；惯性偏差b符合随机游走模型，相邻帧偏差受先验随机游走残差的约束；视觉残差约束帧位姿；惯性残差约束位姿、速度和惯性偏差；深度残差约束位姿$ {\boldsymbol{T}}_{i} $中的$ {\boldsymbol{p}}_{i} $。

2.3 双目-惯性-深度联合初始化

系统初始化是为了获得惯性变量的良好初始值，包括重力方向、速度和IMU偏差，然后旋转世界坐标系，使世界坐标系的Z轴与重力方向对齐。参考ORB-SLAM3^[11]的初始化策略，本文所提系统分三步进行初始化，并将深度数据融合到最大后验概率（Maximum A Posteriori，MAP）估计中，从而获得更好的惯性变量初始值。

1）仅双目视觉MAP估计

首先，初始化纯双目视觉里程计并运行2 s，构建一个包含k=10个关键帧和数百个3D点的局部地图，然后通过BA对机器人位姿进行优化，将其转换到世界坐标系后得到机器人初始轨迹$ {\boldsymbol{T}}_{0:k}={[\boldsymbol{R},\boldsymbol{p}]}_{0:k} $。

2）惯性-深度联合MAP估计

在这一步，忽略纯双目视觉里程计产生的误差，以纯视觉BA优化后的初始轨迹作为先验，即固定姿态$ {\boldsymbol{T}}_{0:k}={[\boldsymbol{R},\boldsymbol{p}]}_{0:k} $，使用k个关键帧之间的惯性测量值即加速度计测量的线加速度$ {a}_{k} $、陀螺仪测量的角速度$ {\omega }_{k} $和深度传感器测量的深度值$ {D}_{k} $来获得惯性变量的最优估计。由于系统使用的是双目相机，不存在尺度问题，因此，在0～k帧间IMU对应的状态向量可表示为：

${\boldsymbol{y}}_{k}=\left\{{\boldsymbol{R}}_{\mathrm{w}\mathrm{g}},\boldsymbol{b},{\boldsymbol{v}}_{0:k}\right\} 。$

(16)

式中：b为惯性偏差，包括陀螺仪偏差$ {\boldsymbol{b}}_{k}^{g} $和加速度计偏差$ {\boldsymbol{b}}_{k}^{a} $，由于初始化时间较短，假设初始化期间其为常数；$ {\boldsymbol{v}}_{0:k} $表示从第0帧到第k帧的速度；$ {\boldsymbol{R}}_{\mathrm{w}\mathrm{g}}\in \boldsymbol{S}\boldsymbol{O}\left(3\right) $，为世界坐标系下的重力方向，结合重力大小G，重力在世界坐标系下可以表示为：

$ \boldsymbol g={\boldsymbol{R}}_{wg}{\left(\mathrm{0,0},G\right)}^{{\rm{T}}}。$

(17)

那么优化问题可表示为：

$ {{y}_{k}^{*}=\underset{{\boldsymbol{y}}_{k}}{\mathrm{argmin}}\left({\| \boldsymbol{b}\Vert}_{{\sum }_{b}^{}}^{2}+{\displaystyle\sum }_{i=1}^{k}\left|\right|{\boldsymbol{r}}_{{I}_{i-1,i}}\| _{{\sum }_{{I}_{i-1,i}}}^{2}+{\displaystyle\sum }_{i=1}^{k} \Vert{r}_{{D}_{i-1,i}}|{|}_{{\sigma }_{{\rm{depth}}}^{2}}^{2}\right) 。}$

(18)

式中：$ {\sum }_{{b}}^{} $为先验协方差矩阵，表示IMU偏差可能取值的范围，用来保证IMU的偏差为0。

在流形中优化时，需要定义一个变量$ \boldsymbol{R}_{\mathrm{w}\mathrm{g}}^{\mathrm{new}} $来表示重力方向的增量更新，其可用参数$ \left(\delta {\alpha }_{g}\right.,\left.\delta {\beta }_{g}\right) $表示为：

$ {\boldsymbol{R}}_{\mathrm{w}\mathrm{g}}^{{\rm{new}}}={\boldsymbol{R}}_{\mathrm{w}\mathrm{g}}^{{\rm{old}}}{\rm{Exp}}\left(\delta {\alpha }_{g}\right.,\left.\delta {\beta }_{g},0\right)。$

(19)

式中：$ \mathrm{Exp} $为从旋转向量到旋转矩阵的转换；$ \boldsymbol{R}_{\mathrm{w}\mathrm{g}}^{\mathrm{old}} $为上次增量。

由于位姿被固定不优化，这里深度残差只用来估计状态向量$ {\boldsymbol{y}}_{k} $和重力方向$ {\boldsymbol{R}}_{\mathrm{w}\mathrm{g}} $。惯性-深度联合优化后就可以旋转世界坐标系，使z轴与重力方向对准并更新偏差，重新预积分，以减少将来的线性化误差。

3）双目视觉-惯性-深度联合MAP估计

为进一步优化参数，完成惯性参数的估计后，将执行视觉-惯性-深度联合优化，其优化模型在2.2节中已完成推导。这次优化针对初始化期间的10个关键帧，在优化中相机位姿不再被固定，并将上一步中获得的惯性参数估计值作为优化中惯性偏差的初始估计，从而获得关键帧位姿、关键帧速度、地图点、IMU偏差的最优估计。优化的因子图与图4类似，不同的是所有关键帧具有共同的偏差，并且包括与在仅惯性-深度联合优化步骤中的偏差相同的先验信息。

由于船舶清洗机器人运动缓慢，惯性参数可观测性较差，初始化可能无法在短时间内收敛到准确解，特别是重力方向的估计误差将影响到深度测量值的准确性。因此，在三步初始化后将进行数次重力方向调整。这种调整包含所有插入的关键帧，但仅对重力方向这个参数进行估计。在这种情况下，IMU偏差不变的假设将不再适用，优化中使用局部地图构建中估计的值，并将这些值固定不优化。这种调整每10 s执行一次，直到初始化完成60 s。

3 试验结果与数据分析

为了验证基于双目-惯性-深度融合的船舶清洗ROV定位系统的性能，将其部署于船舶清洗ROV上，并与使用双目惯性模式的ORB-SLAM3系统进行精度对比。

试验中将双目相机置于防水外壳中并安装于船舶清洗ROV前端，IMU置于耐压舱中，深度传感器安装于耐压舱上。搭载的双目相机分辨率为1280×720，帧率配置为15 fps，IMU数据频率为100 Hz，深度传感器数据频率为2 Hz。实验在25 m×6 m×3.5 m（长×宽×深）的室内水池中进行，以水池侧壁模仿船体表面，机器人在池壁上进行往复运动以模仿实际清洗过程。由于水池壁面是静态环境，与实际清洗过程中的动态环境不符，因此在运行ORB-SLAM3时关闭其回环检测功能，即将其当作视觉惯性里程计使用，简称为ORB-SLAM3-VIO。

在水下轨迹对比中一般以IMU轨迹作为参考轨迹，但由于船舶清洗ROV所使用IMU精度较差，在系统运行过程中漂移严重，其轨迹与真实轨迹差距过大，参考意义不大，因此在实验中采用人工测量的方式在机器人行进过程中每隔1～1.5 m进行一次位置测量，通过拟合这些位置点生成参考轨迹。实验分2次进行，由轮式里程计测得2次实验运行里程分别约为35 m和73 m，本文系统与ORB-SLAM3-VIO的轨迹对比如图5所示。

实验采用绝对轨迹误差（Absolute Trajectory Error，ATE）的均方根误差（Root Mean Squared Error，RMSE）作为评估估计轨迹精度的度量，结果见表1。

由图5可知，相比于ORB-SLAM3-VIO，本文算法所得轨迹在z轴方向与参考轨迹更为接近；由表1可知，相比于ORB-SLAM3-VIO，本文算法的均方根误差较小，平均减小了41.6%，并且随着运行里程的增加，ORB-SLAM3-VIO的均方根误差迅速增大，而本文算法的均方根误差虽也略有增大，但增幅明显小于ORB-SLAM3-VIO，从而证明了本文所提算法的有效性。

4 结　语

本文提出一种融合了双目相机、IMU和深度传感器的船舶清洗ROV定位系统。首先通过分析水下图像质量退化原因对水下图像进行了清晰化处理，提高了特征提取与匹配的效果；然后利用深度传感器测量值在重力轴方向没有累计误差的特点，通过投影的方式将距离视觉关键帧最近的深度值对齐到关键帧，进而建立起双目-惯性-深度联合优化模型，提高位姿估计的精度。水池实验结果表明，相比于ORB-SLAM3-VIO，本文所提双目-惯性-深度里程计系统的均方根误差平均减小了41.6%，有效提高了ROV的定位精度。