[论文笔记|sonar+VIO]SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor

Ⅰ. 摘要

本文提出了一种基于紧耦合关键帧的SLAM系统，该系统具有针对水下环境的闭环和重定位能力。我们之前的工作，SVIn，对OKVIS进行扩展，以适应声定位系统在非线性优化框架下的声学数据。本文主要解决了定位漂移和丢失问题(水下领域中影响其它packages的主要问题之一)。
主要贡献:
1.一个健壮的初始化方法——使用深度测量来细化尺度，
2. 一个快速的预处理步骤——提高图像质量,
3. 一种实时闭环和重定位方法——使用词袋(BoW)。
4. 将从压力传感器获得的深度测量值增加到紧耦合优化公式。

利用定制的水下传感器套件和自主水下航行器从能见度较差的水下环境中收集的数据集进行实验。结果表明，在准确性和鲁棒性方面，性能达到了前所未有的水平。

Ⅱ. INTRODUCTION

1. 水下环境的大部分测量工作由潜水员完成的——>使用网格和卷尺进行手工测量$or$使用手持传感器进行测量——>然后对数据进行后处理。
2. 自动水下航行器(AUVs)为这一过程提供了自动化机会;然而，对于可靠的部署，仍然有几个问题需要解决，包括本文的重点——健壮的同步定位和映射(SLAM)
3. 大多数水下导航算法是基于声学传感器(多普勒速度测井(DVL)、超短基线(USBL)和声纳)——>这些传感器的数据收集昂贵，有时不适合由于高度非结构化的水下环境
4. 基于视觉的状态估计算法被开发出来(使用单目、双目或多摄像机系统，主要用于室内和室外环境)——>视觉+惯性测量单元(IMU)用于在具有挑战性的环境中改进姿态估计(VIO)——>水下环境具有挑战性(悬浮粒子、浑浊度、光和颜色的衰减导致产生的特征不像水面上那样清晰)——>产生大量的异常值，导致不准确的估计甚至完全的跟踪损失。
5. 本文提出了SVIn2，这是一种新型的SLAM系统，专门针对水下环境，例如沉船和水下洞穴，很容易适应不同的传感器配置:声学(机械扫描剖面声纳)、视觉(双目摄像机)、惯性(线性加速度和角速度)和深度数据。这使得我们的系统通用性强，适用于不同的传感器套件和水下航行器。

在SVIn的工作中，将声学、视觉和惯性数据融合在一起，通过扩展OKVIS来绘制不同的水下结构。因为声纳提供了关于障碍物存在的可靠信息，而且具有精确的比例，因此这改进了轨迹估计，特别是在水下能见度变化的情况下。然而，在长轨迹中，漂移可能会累积，导致产生错误的轨迹。

在这篇论文中，我们对扩展SVIn工作进一步扩展，包括针对水下领域的图像增强技术，在优化过程中引入深度测量，回环，以及一个更健壮的初始化。这些增加使得所提出的方法能够稳健而准确地估计传感器的轨迹。

Ⅲ. PROPOSED METHOD

本节将介绍图2所示的SVIn2系统。完整的状态估计系统可以由一个拥有双目摄像机、IMU、声纳和深度传感器的机器人进行操作。

由于低能见度和动态障碍物，很难找到好的特征进行跟踪。除了水下视觉的限制，例如光和颜色的衰减，基于视觉的系统也会受到低对比度的影响。因此，我们增加了一个图像预处理步骤来增加管道pipeline，其中对比度调整和直方图均衡化应用于改进水下特征检测。特别地，我们在图像预处理步骤中使用CLAHE滤波器。接下来，在定义状态之后，我们描述了所提出的初始化、传感器融合优化、回环、重定位步骤。

图2：SVIn2;黄色的是来自定制传感器套件的频率传感器输入，绿色的是来自OKVIS的组件，红色的是来自SVIn的贡献，蓝色的是本文中的新贡献。

A. Notations and States

1. 完整的传感器套件由以下坐标系组成:摄像机(双目)、IMU、声纳、深度和世界坐标系，分别记为$C,I,S,D,W$
2. 任意坐标系X和Y之间的变换用齐次变换矩阵$x{T}_Y$表示,$x{R}_Y$具有相应四元数的旋转矩阵$x{q}_Y$,$p$是位置向量。
   
3. 机器人$R$的状态$x_R$:
   
   包含陀螺仪和加速度计的偏置$b_g和b_a$。
   状态向量的扰动：
   

B. Tightly-coupled Non-Linear Optimization with Sonar-Visual-Inertial-Depth measurements

对于紧耦合非线性优化，我们使用以下代价函数$J(x)$，其中包括重投影误差$e_r$,IMU误差$e_s$,声纳误差$e_t$，深度误差$e_u$:

（1）重投影误差描述了摄像机坐标系C中的关键点测量值与双目投影模型中相应的路标投影值之间的差异。IMU误差项结合了所有的加速度计和陀螺仪测量，通过在连续的相机测量之间进行IMU预积分，代表了基于先前和当前状态的预测之间的位姿、速度和偏差误差。详见OKVIS论文。

（2）声纳测距误差,详见SVIn。大致思想是，如果声纳在一段距离上探测到某障碍,更有可能的是视觉特征将位于障碍的表面,从而声呐传感器到声呐路标的距离将大约等于声呐传感器到视觉特征的距离。该步骤包括计算在每个声纳点附近检测到的视觉patch，利用声纳到patch的距离引入额外的约束。前提条件是，假设基于视觉特征的patch足够小，并且与声纳点近似共面。


(3) 深度误差：本文介绍的压力传感器可以根据水压提供精确的深度测量。由于紧密耦合的IMU集成,深度值沿着重力方向提取(重力方向与观测到的世界坐标系$W$的$z$方向一致。k时刻的深度数据:

考虑IMU沿z轴的初始位移，更精确的表示为：

通过深度测量值$z_u^k$，深度误差项$e_u^k(W{p}_z{I}^k,z_u^k)$可以计算机器人在$z$方向上的位置与深度数据之间的差值，从而修正机器人的位置。误差项可以定义为:

信息矩阵的计算方法与声纳相似，雅可比矩阵可以直接推导出来。

在Ceres求解器的非线性优化框架中加入所有的误差项来表示误差状态
(Eq.(2))和估计机器人状态(Eq.(1))。

C. Initialization: Two-step Scale Refinement

为了成功地实现紧密耦合非线性系统，需要一个健壮而准确的初始化。本论文利用来自双目相机、IMU和深度传感器的数据信息进行水下状态估计。使用这三个传感器的原因是为了在scale上引入约束，以便在初始化时进行更准确的估计。
【注意】初始化没有使用声呐测量值，因为声呐range和视觉特征之间有一个时间差，如果机器人不移动，这将不允许有任何声学和视觉特征之间的匹配。这是由于声纳能够扫描机器人周围超过360度的平面，摄像机探测到机器人前面的特征;参见图3。

图三：定制的传感器套件安装在双DPV上。声纳能够扫描到周围的传感器，而摄像头只能看到前面。

初始化步骤：
（1）确保系统只在出现最少数量的视觉特征进行跟踪时才进行初始化(本论文实验中15个工作良好)。
（2）对双目视觉的初始尺度进行两步细化。

1. 第一次细化：

深度传感器提供精确的深度测量，用于从双目摄像机中提取初始比例因子。考虑到比例因子$s_1$，摄像机C与深度传感器D之间的转换可以表示为：

对于关键帧$k$，为$s_1$求解Eq.(10)，提供了初始双目尺度$W{p}_{r_1}C$的第一个细化$r_1$,即

2. 第二次细化：
   第二步中，Eq.(11)中的双目相机的细化测量与IMU的预积分值对齐。同样，考虑比例因子$s_2$，相机$C$与IMU传感器$I$之间的变换可以表示为：
   

除了细化尺度外，我们还近似了初始速度和重力矢量，类似于Vins_MONO中描述的方法。

OKVIS中，带有IMU测量值$z_I$的 IMU集成的状态预测

其服从条件协方差
，
IMU集成的状态预测可以写成：

每个式子中的最后一项${\alpha }_{Ii}^{i+1},{\beta }_{I_i}^{i+1},{\gamma }_{I_i}^{i+1}$为IMU前积分项，定义了两个连续的关键帧$i和i+1$在时间间隔$∆t_i$之间的运动，只能从IMU测量中得到。

对Eq.(13)中的${\alpha }_{Ii}^{i+1},{\beta }_{I_i}^{i+1}$重新整理得：

将Eq.(12)替换到Eq.(14)，我们可以通过求解线性最小二乘问题估计${\chi }_S=[v_I^i,v_I^{i+1},Wg,s_2{]}^T$:

D. Loop-closing and Relocalization

在基于滑动窗口和边缘化的优化方法中，漂移随时间在位姿估计上累积。为了消除这种漂移并实现全局一致性，全局优化和重定位方案是必要的。

我们采用DBoW2，一个二进制单词(BoW)位置识别模块，并增强OKVIS进行循环检测和重定位。对于每个关键帧，只使用在本地跟踪过程中检测到的关键点的描述符来构建BoW数据库。在循环结束步骤中不会检测到任何新特性。

使用一个pose-gragh来表示关键帧之间的连接。其中，一个节点表示一个关键帧，如果两个关键帧之间匹配的关键点比率大于0.75，则存在两个关键帧之间的边(在实践中，这会导致一个非常稀疏的图)。对于位置图中的每个新关键帧，回环模块将在词袋数据库中搜索候选值。

用于检测到BoW数据库的回环的查询只返回当前边缘化窗口之外的候选关键帧，这些候选关键帧的得分大于或等于pose-gragh中该节点的相邻关键帧的得分。如果检测到回环，则保留得分最高的候选帧，并获得局部窗口中当前关键帧与回环候选关键帧之间的特征匹配，以建立它们之间的连接。因此，利用回环信息来更新pose-gragh。

通过2D-2D描述子匹配和3D-2D匹配当前窗口关键帧中的已知路标和被PnP RANSAC拒绝的回环候选对象，来进行几何验证。

当检测到一个循环时，通过将位姿中的漂移发送回窗口中的sonar-visual-inertial-depth优化线程，全局重定位模块将当前局部窗口中的关键帧位姿与位置图中的循环关键帧位姿进行对齐。此外，一个额外的优化步骤，类似于式(3)，为了计算声纳误差项和重投影误差，只采取对回环候选的路标进行匹配：

在回环检测之后，进行6自由度（位置$x_p$和旋转$x_q$）位姿图优化，以优化位姿之间的相对约束，从而校正漂移。
通过

可以计算出当前窗口的关键帧$i$和关键帧$j$（回环候选关键帧或连接关键帧）中当前关键帧的两个pose $T_i和T_j$之间的相对变换。在切空间中，对关键帧$i和j$之间的误差项$e_{x_p,x_q}^{i,j}$进行了最小化表示:

其中，$(ˆ)$表示从局部sonar-visual-inertial-depth优化后获得的估计值。最小化的损失函数表示为：

$P_{x_p,x_q}^{i,j}$为设置为单位矩阵的矩阵信息,$\rho$是Huber损失函数,可能会降低任何不正确回环的权重。

IV. EXPERIMENTAL RESULTS

（1）所提出的状态估计系统SVIn2首先在一个标准数据集上进行了定量验证，以确保环路闭合和初始化工作也能在水面上进行。
（2）与其他最先进的方法即基本的OKVIS，VINS-Mono以及MSCKF进行实验分析。
（3）利用一个定制的传感器套件和一个Aqua2 AUV对所收集的几个不同的数据集进行了定性测试。

A. Validation on Standard dataset

在这里，实验展示了EuRoC数据集的结果，它是许多视觉-惯性状态估计系统使用的基准数据集之一。为了比较性能，实验在本论文的方法中禁用深度和声纳集成，只评估回路闭合方案。

通过最小化估计/ground truth位置之间的最小均方误差，实现了地面真值与估计轨迹之间的对齐。得到的度量是平移的均方根误差(RMSE)，如表I所示。实验结果表明，本篇论文的方法在OKVIS的每个序列中都显示出RMSE的减少，验证了环合后的位置估计的改进。SVIn2的RMSE也低于MSCKF，在某些序列中稍高，但与vins - mono的结果相当。

图4显示了每种方法的轨迹以及the Machine Hall 04 Diffificult sequence的ground truth。

B. Underwater datasets

SVIn2中的双目摄像机被配置为捕捉帧速率为15帧/秒，IMU频率为100赫兹，声纳频率为100赫兹，深度传感器频率为1赫兹。这里展示了来自三个不同水下环境的四个不同数据集的结果。在水下数据集中，获取任何ground truth都是一个挑战，因为它是一个GPS禁用的非结构化环境。因此，评估是定性的，通过收集数据的潜水员对预先测量的环境大小进行粗略的估计。

图6-9显示了SVIn2、OKVIS和vins - mono在上述数据集中的轨迹。MSCKF只能跟踪所有数据集中的一小部分，因此排除在图之外。为了公平的比较，当轨迹相互比较时，声纳和深度在SVIn2中被禁用。所有的轨迹都是按照每个方法的原始比例绘制的。

1. 图6显示了淹没公共汽车的数据集结果。VINS-Mono在相当长一段时间内都没有追踪到exposure的增加；它试图重新初始化，但无法成功跟踪；即使使用直方图均衡化或对比度调整直方图均衡化滤波器(CLAHE)，VINS-Mono也无法跟踪。即使尺度漂移，OKVIS在图像预处理步骤能够跟踪使用对比度调整直方图均衡化滤波器(CLAHE)。如果没有滤波器，它在高曝光位置就会失去踪迹。该方法具有良好的跟踪、检测和纠错能力。
   此数据集：潜水员从巴士外面开始，绕了一圈，从后门进入车内，从对面下车，最后在巴士的前部结束。

2. 如图7所示，在岩洞中，VINS-Mono全程跟踪成功。但是，从图7©可以看出，在收集数据的过程中，基于经验观察得出的尺度是不正确的。相反，OKVIS产生了一个良好的轨迹，SVIn2也能够检测并关闭回路。
   
   此数据集：潜水员绕着第二个数据集洞穴n1中的一个点进行了几次循环。环境受到完全缺乏自然光的影响。

3. 在图8中，vins - mono在开始时丢失了轨迹，重新初始化，可以跟踪一段时间，并检测到一个循环，然后再次丢失轨迹;即使使用不同的过滤器对图像进行预处理，vins-mono也有类似的行为。OKVIS追踪得很好，但是随着时间的推移，它不能加入当前的位姿和之前的位姿，期望一个循环。SVIn2能够通过成功的闭环回环来跟踪并减小轨迹中的漂移。
   
   此数据集：潜水员绕着第三个数据集洞穴n2中的两个点进行了几次循环。环境受到完全缺乏自然光的影响。

4. 在墓地数据集图9中，VINS-Mono和OKVIS都能够跟踪，但是VINS-Mono不能减少轨迹中的漂移，而SVIn2能够融合并纠正回环。
   

此数据集：在一个假的水下墓地上收集数据，并在墓碑周围以正方形的模式做了几个循环。能见度、亮度和对比度都很低。

V. CONCLUSIONS

本篇论文提出了一个状态估计系统——SVIn2，专门针对水下环境，具有鲁棒初始化、深度传感器融合声纳、视觉和惯性数据，以及环路闭合能力。