CN116380079A - 一种融合前视声呐与orb-slam3的水下slam方法 - Google Patents

Abstract

本发明公开了一种融合前视声呐与ORB‑SLAM3的水下SLAM方法，步骤包括：1、获取前视声呐、双目相机以及惯性测量单元（IMU）的量测；2、对声呐图像进行特征跟踪，通过最小化重投影误差得到声呐的相对位姿估计；3、通过ORB‑SLAM3得到相机的位姿估计；4、构建因子图，在视觉跟踪有效时，将3中视觉里程计的位姿估计值作为先验因子，对IMU预积分作为位姿约束因子，将2中声呐相对位姿估计作为约束因子，求解因子图，更新载体位姿估计；5、当视觉跟踪成功，因子图的输出将跟随视觉里程计；如果视觉跟踪丢失，因子图中的声呐和IMU因子继续起作用，系统在到达可跟踪区域后立即初始化新地图，使用4中位姿估计作为地图初始值来连接两个地图，有利于扩大系统的使用范围。

Description

一种融合前视声呐与ORB-SLAM3的水下SLAM方法

技术领域

本发明属于机器人、自动驾驶领域，具体为一种融合前视声呐与ORB-SLAM3的水下SLAM方法。

背景技术

为了保障机器人顺利进行水下任务，需要对海底环境进行建图和对机器人进行定位。同时定位和建图技术(SLAM)通过安装在水下机器人上的传感器(例如摄像头、声呐等)进行自身定位以及环境地图的构建，SLAM能够通过系统自身的回环与重定位功能修正累积误差。

目前陆地上有很多优秀的视觉SLAM算法，能够为机器人提供高精度的定位，并且在室内和户外都已有了成熟的应用。ORB-SLAM3提供了一种基于多地图系统的双目视觉-惯性SLAM方法，具有很强的鲁棒性和很高的精度。由于水下光线和颜色的衰减、动态物体、水下灰尘等因素，相机往往不总是能检测到特征，在检测不到视觉特征时使用IMU航位推算往往具有较大累积误差。前视声呐能够在浑浊的水下环境中工作，但价格昂贵、分辨率低。在一些清晰的水域(如珊瑚礁)，摄像头能够捕获丰富的信息。因此在ORB-SLAM3中融合前视声呐，利用视觉和前视声呐的优势互补，扩展SLAM系统使用范围的同时保障系统精度。

将前视声呐与ORB-SLAM3融合，实质上是一个多源传感器融合导航问题。融合方法分为基于滤波的方法与基于非线性优化的方法。非线性优化的方法精度高，但实现难度大，需要修改ORB-SLAM3原本的后端结构，并且多数后端在其中一个传感器失效后就不可用，灵活性较差。此外，在进行融合之前往往需要对传感器进行时间同步，声呐与相机的频率较低，时间同步后会得到一些频率更低的数据。而因子图具有灵活多变的特点，能够很自然的处理不同速率的测量信息，并且支持传感器的即插即用，如果有传感器失效，也可以从因子图框架中及时删除，可以有效地融合前视声呐与ORB-SLAM3。

1、与“一种基于点特征稀疏策略的视觉同步定位与地图构建方法”的技术对比：

“一种基于点特征稀疏策略的视觉同步定位与地图构建方法”对声呐提取了SIFT算法对声呐图像进行点特征提取，然后通过极线约束、视差约束、方向约束、尺度约束、唯一匹配约束多项匹配约束进行SIFT特征点的匹配。SLAM后端使用了EKF滤波器优化算法，本文的关键在于对路标进行稀疏性约束，控制路标的位置空间和特征空间的分布，以限制路标的上升速度，减少计算量、缩短定位时间。

而本发明是“融合前视声呐与ORB-SLAM3的水下SLAM方法”，使用了AKAZE算法对声呐图像进行特征提取，并通过随机采样一致性算法剔除误匹配。本发明的SLAM算法后端使用了基于因子图的非线性优化，不会出现随着路标增加，计算量急剧增长的情况。

2、与“基于关键约束帧的前视成像声呐水下定位与导航方法”的技术对比：

“基于关键约束帧的前视成像声呐水下定位与导航方法”采用传感器是前视声呐、IMU、深度计，提出了一种基于关键约束帧的前视成像声呐水下定位与导航方法。本文的目的在于识别并充分利用过往声呐图像中的有效约束信息对当前位姿和水下特征进行定位。主要步骤有：前端数据处理，包括二维声呐图像的特征提取与匹配、惯性测量数据处理；后端弹性滑动窗口优化，包括声呐关键约束帧的识别与存储、从关键约束帧库中选择可加入滑动窗口优化的过往帧、窗口大小控制与优化、更新机器人位姿和水下特征位置。

而本发明使用的传感器是光学相机、前视声呐、IMU，侧重于使用光学和声学两种传感器的协同定位。本发明的技术核心在于使用因子图进行多源传感器融合。由于考虑了声呐特征倾角的恢复，因此本发明能够应用到水下三维空间运动的定位和建图中。而基于关键约束帧的前视声呐SLAM算法，将声呐特征倾角近似当作零，适用于二维平面上的定位和建图，如果应用到三维场景则会导致较大的定位误差。

3、与“一种基于高斯分布聚类的水下声呐图像匹配方法”的技术对比：

“一种基于高斯分布聚类的水下声呐图像匹配方法”旨在通过二维声呐图像进行准确的三维重建，提供了一种基于高斯分布聚类的水下声呐图像匹配方法，通过图像配准和优化声纳三维运动参数，对二维声纳图像进行精准的三维重建，包括以下步骤：步骤A：提取特征，建立特征间的匹配关系，获得地图的高程信息；步骤B：进行位姿估计，将特征地图更新，从而生成三维空间地图。

本发明旨在使用前视声呐进行水下载体的同步定位和地图构建，事先不清楚周围环境和载体的运动姿态，而基于高斯分布聚类的三维重建方法基于根据平面性假设的方法，计算扫描方位的投射阴影点的仰角的测量值，随着载体的运动，平面性假设可能会不成立。本发明提出的最小化重投影误差和声视觉数据关联的方法恢复声呐倾角不设置前提假设，更适用于水下SLAM系统。

发明内容

针对ORB-SLAM3在水下容易跟踪丢失，导致较大的累积误差的问题，本发明提出一种融合前视声呐与ORB-SLAM3的水下SLAM方法，该方法构建因子图以融合声呐约束和ORB-SLAM3视觉惯性里程计的位姿约束，将视觉惯性里程计作为因子图先验因子，声呐里程计和IMU预积分作为约束因子。当视觉跟踪到物体时，因子图的输出将跟随视觉惯性里程计；当视觉跟踪丢失后，因子图中的声呐约束和IMU预积分因子继续起作用以限制航位推算的累积误差。在载体再次到达视觉可跟踪区域后立刻初始化新的子地图，以因子图的位姿估计作为子地图初始值来连接相邻的两个子地图，检测到回环后合并子地图以进一步减小累积误差。

为实现上述目的，本发明采取的技术方案是：

一种融合前视声呐与ORB-SLAM3的水下SLAM方法，其特征在于，具体步骤如下：

(1)对IMU测量值进行预积分，使用IMU预积分来获得两个相机帧之间的相对位姿；

(2)对声呐图像进行AKAZE特征提取，通过计算描述子汉明距离以对相邻两帧声呐图像进行特征匹配；

(3)构建因子图，在视觉跟踪有效时，将(1)中载体位姿估计值作为位姿先验因子，通过比较匹配特征点个数与设定阈值判断视觉跟踪是否有效；

(4)如果视觉跟踪丢失，让系统在到达可跟踪区域后立即初始化新地图，使用步骤(3)中计算的载体位姿作为子地图初始值来链接两个子地图。利用DBoW2进行闭环检测，提取每个新的视觉关键帧的词袋向量并将它们与之前提取的描述符进行匹配，如果检测到闭环，则将新地图与之前的子地图进行合并。

作为本发明进一步改进，步骤(1)具体如下：

检测当前双目相机帧的ORB特征点，使用滑动窗口中构建一个带有关键帧特征点的局部地图，当每个新的帧到来时，系统会在本地地图中搜索旧的特征点，通过特征描述符匹配新的特征点，当跟踪到的特征点数小于阈值时，前端会将这一帧标记为关键帧，并将关键帧加入到滑动窗口中，边缘化滑动窗口中最旧的关键帧，以维持固定的滑动窗大小；

将视觉-惯性SLAM问题表述成成本函数J(x)的一个联合优化，其中包含视觉重投影误差e_r和来自IMU e_s的时间误差项：

其中i是相机索引，k表示相机帧索引，j表示路标索引，通过BA优化更新载体位姿估计值与视觉路标的局部地图，

和/>

分别表示第k帧的视觉路标和IMU预积分的信息矩阵。

作为本发明进一步改进，步骤(2)具体如下：

由于声呐成像过程中丢失了路标的倾角，通过在区域Φ＝{φ_min,φ_min+Δφ,...,φ_max-Δφ,φ_max}中搜索确定倾角，其中范围Φ由声呐的最大垂直孔径决定，Δφ是一个常数设计参数，决定了搜索空间的分辨率，从而在性能和精度之间进行权衡，且声呐的最佳仰角为：

其中π(·)表示笛卡尔坐标中的点到极坐标的转换，C(·)是从球坐标到笛卡尔坐标的映射，

表示从前一帧声呐参考系A变换到后一帧声呐参考系B下的坐标变换，由IMU预积分确定/>

的初始值，^Bz_i表示声呐。对每个地标都执行此过程，建立一组最佳仰角φ^*作为声呐BA的观察模型；

使用视觉声呐路标的数据关联来剔除，以声呐路标估计值为中心建立椭圆形关联门，椭圆门是由残差矢量和范数来表示的，设G为椭圆门的门限值，S为信息向量的协方差矩阵，如果信息矢量的范数满足下式，则观测位于关联门之内：

当视觉检测到物体时，对声呐和视觉路标进行数据关联，如果没有视觉路标位于关联门内，换言之声呐路标附近不存在视觉路标，就认为声呐的解无效；如果两个声呐路标都能关联到视觉路标，取位于上方的路标点，因为前视声呐成像时，上方的路标点会覆盖下方的路标点；

在确定路标位置后，以声呐特征重投影误差为代价函数，通过BA优化求解得到声呐的相对位姿估计值。

作为本发明进一步改进，步骤(3)具体如下：

在空旷的环境下，由于环境特征不明显，约束较差，声呐里程计会得到错误的估计，通过声呐失效性检测剔除错误的位姿估计；

首先，对声呐测量雅克比矩阵A进行SVD奇异值分解：

A_m×n＝USV^T

其中U_m×m和V_n×n是正交矩阵，S_m×n是奇异值(σ₁≤...≤σ_n)越来越大的对角矩阵，选择来自S的适当阈值σ_thresh，并将小于σ_thresh的特征值设置为0，当小于阈值的特征值足够多时，则认为当前声呐的位姿约束是无效的；

在判别声呐有效后，将步骤(2)中声呐相对位姿估计值加入因子图，将步骤(1)中IMU预积分项作为位姿约束因子，载体的状态估计问题表述为如下：

其中，Θ_i是与因子节点f_i相邻的变量节点的集合，f_i(Θ_i)是测量函数，z_i是一个测量值。通过使用iSAM2在因子图中联合优化IMU预积分约束、视觉里程约束、声呐里程约束的贡献来求解。

有益效果：本发明公开了一种基于前视声呐、视觉、惯性的水下SLAM方法，该方法构建因子图以融合声呐里程计、IMU航位推算以及视觉惯性里程计的位姿估计结果，在视觉跟踪丢失后，声呐和惯性里程计能继续提供位姿估计，并且在载体再次到达可跟踪区域后立刻初始化新的子地图，检测到回环后合并子地图以进行全局优化。其中，通过声呐和视觉特征数据关联解决了声呐路标的二义性问题；此外，针对欠约束环境，对声呐约束因子的有效性做了判别。与现有技术相比，该方法具有使用范围大、定位精度高、稳定性强的优点。

附图说明

图1是本发明公开方法的系统框图；

图2是本发明公开的方法中视觉成功检测和跟踪失效时的两种因子图结构。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

融合前视声呐与ORB-SLAM3的水下SLAM方法，如图1所示，包括如下步骤：

步骤1、将前视声呐、双目相机以及IMU安装在载体上，操纵载体在水中航行，获取传感器的测量值。来自IMU的原始角速度和加速度测量值表示为：

其中

和/>

是t时刻的原始IMU测量值。ω_t和/>

受偏置b_t和加性噪声n_t的影响，我们假设加性噪声是高斯的，加速度偏差和陀螺仪偏差被建模为随机游走。/>

是从世界坐标系W到IMU坐标系B的旋转矩阵。g是世界坐标系W中的恒定重力向量。

使用IMU预积分来获得两个相机帧之间的相对位姿。IMU预积分的因子可以定义为：

其中

是使用两个连续相机帧之间的噪声加速度计和陀螺仪测量构成的IMU预积分测量项；

步骤2、检测当前双目相机帧的ORB特征点，将左右摄像机之间成功匹配的那些新特征三角化到真实世界的位置。使用滑动窗口中构建一个带有关键帧特征点的局部地图。当每个新的帧到来时，系统会在本地地图中搜索旧的特征点，通过它们的特征描述符匹配新的特征点。将匹配上的旧特征所对应的路标点投影到当前帧，考虑到第i帧观察到的第l个特征，重投影误差定义为：

Φ＝{φ_min,φ_min+Δφ,...,φ_max-Δφ,φ_max} (4)

其中u_j是特征在图像上的坐标，K是相机投影矩阵，P_j是点在世界坐标系W中的位置。ξ_i∈SE(3)表示相机的位姿。

当跟踪到的特征点数小于阈值时，前端会将这一帧标记为关键帧，并将当前帧加入到滑动窗口中，并移除滑动窗口中最旧的关键帧，以维持固定的滑动窗大小。

以视觉特征点重投影误差以及步骤1中IMU预积分因子作为约束构造代价函数，求解滑动窗以优化载体位姿估计值以及视觉路标点构成的局部地图；

步骤3、对声呐图像进行AKAZE特征提取，通过计算描述子汉明距离以对相邻两帧声呐图像进行特征匹配。由于声呐成像过程中丢失了路标的倾角，通过在区域Φ＝{φ_min,φ_min+Δφ,...,φ_max-Δφ,φ_max}中搜索确定倾角，其中范围Φ由声呐的最大垂直孔径决定，Δφ是一个常数设计参数，它决定了搜索空间的分辨率，从而在性能和精度之间进行权衡。大多数前视声呐都有一个小的垂直孔径(～12°)，可以快速计算。声呐的最佳仰角为：

其中π(·)表示笛卡尔坐标中的点到极坐标的转换，C(·)是从球坐标到笛卡尔坐标的映射，

表示从前一帧声呐参考系A变换到后一帧声呐参考系B下的坐标变换，由IMU预积分确定/>

的初始值，^Bz_i表示声呐。对每个地标都执行此过程，建立一组最佳仰角φ^*作为声呐BA的观察模型。

由于搜寻声呐路标过程中存在二义性，即可能会存在两个解使得重投影误差最小，但实际上只有一个解对应真实路标的位置，使用视觉声呐路标的数据关联来剔除。由于噪声一般服从高斯分布，故以声呐路标估计值为中心建立椭圆形关联门。椭圆门是由残差矢量和范数来表示的，设G为椭圆门的门限值，S为信息向量的协方差矩阵，如果信息矢量的范数满足下式，就可以说观测位于关联门之内：

当视觉检测到物体时，对声呐和视觉路标进行数据关联，如果没有视觉路标位于关联门内，换言之声呐路标附近不存在视觉路标，就认为声呐的解无效；如果两个声呐路标都能关联到视觉路标，取位于上方的路标点，因为前视声呐成像时，上方的路标点会覆盖下方的路标点。

在确定路标位置后，以声呐特征重投影误差为代价函数，通过BA优化求解得到声呐的相对位姿估计值。

步骤4、构建因子图，在视觉跟踪有效时，将步骤2中载体位姿估计值作为位姿先验因子，通过比较匹配特征点个数与设定阈值判断视觉跟踪是否有效。

在空旷的环境下，由于环境特征不明显，约束较差，声呐里程计会得到错误的估计，通过声呐失效性检测可以剔除错误的位姿估计。首先，对声呐测量雅克比矩阵A进行SVD奇异值分解：

A_m×n＝USV^T (7)

其中U_m×m和V_n×n是正交矩阵，S_m×n是奇异值(σ₁≤...≤σ_n)越来越大的对角矩阵。选择来自S的适当阈值σ_thresh，并将小于σ_thresh的特征值设置为0，当小于阈值的特征值足够多时，则认为当前声呐的位姿约束是无效的。

在判别声呐有效后，将步骤3中声呐相对位姿估计值加入因子图，将步骤1中IMU预积分因子作为位姿约束因子，因子图结构如图2所示，载体的状态估计问题可以表述为如下：

其中，Θ_i是与因子节点f_i相邻的变量节点的集合，f_i(Θ_i)是测量函数，z_i是一个测量值。通过使用iSAM2在因子图中联合优化IMU预积分约束、视觉里程约束、声呐里程约束的贡献来求解。

步骤5、如果视觉跟踪丢失，让系统在到达可跟踪区域后立即初始化新地图，使用步骤4中计算的载体位姿作为子地图初始值来链接两个子地图。利用DBoW2进行闭环检测，提取每个新的视觉关键帧的词袋向量并将它们与之前提取的描述符进行匹配。如果检测到闭环，则将新地图与之前的子地图进行合并。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (4)

1.一种融合前视声呐与ORB-SLAM3的水下SLAM方法，其特征在于，具体步骤如下：

(1)对IMU测量值进行预积分，使用IMU预积分来获得两个相机帧之间的相对位姿；

(2)对声呐图像进行AKAZE特征提取，通过计算描述子汉明距离以对相邻两帧声呐图像进行特征匹配；

(3)构建因子图，在视觉跟踪有效时，将(1)中载体位姿估计值作为位姿先验因子，通过比较匹配特征点个数与设定阈值判断视觉跟踪是否有效；

(4)如果视觉跟踪丢失，让系统在到达可跟踪区域后立即初始化新地图，使用步骤(3)中计算的载体位姿作为子地图初始值来链接两个子地图。利用DBoW2进行闭环检测，提取每个新的视觉关键帧的词袋向量并将它们与之前提取的描述符进行匹配，如果检测到闭环，则将新地图与之前的子地图进行合并。

2.根据权利要求1所述的一种融合前视声呐与ORB-SLAM3的水下SLAM方法，其特征在于，步骤(1)具体如下：

检测当前双目相机帧的ORB特征点，使用滑动窗口中构建一个带有关键帧特征点的局部地图，当每个新的帧到来时，系统会在本地地图中搜索旧的特征点，通过特征描述符匹配新的特征点，当跟踪到的特征点数小于阈值时，前端会将这一帧标记为关键帧，并将关键帧加入到滑动窗口中，边缘化滑动窗口中最旧的关键帧，以维持固定的滑动窗大小；

将视觉-惯性SLAM问题表述成成本函数J(x)的一个联合优化，其中包含视觉重投影误差e_r和来自IMU e_s的时间误差项：

其中i是相机索引，k表示相机帧索引，j表示路标索引，通过BA优化更新载体位姿估计值与视觉路标的局部地图，

和/>

分别表示第k帧的视觉路标和IMU预积分的信息矩阵。

3.根据权利要求1所述的一种融合前视声呐与ORB-SLAM3的水下SLAM方法，其特征在于，步骤(2)具体如下：

由于声呐成像过程中丢失了路标的倾角，通过在区域Φ＝{φ_min,φ_min+Δφ,...,φ_max-Δφ,φ_max}中搜索确定倾角，其中范围Φ由声呐的最大垂直孔径决定，Δφ是一个常数设计参数，决定了搜索空间的分辨率，从而在性能和精度之间进行权衡，且声呐的最佳仰角为：

其中π(·)表示笛卡尔坐标中的点到极坐标的转换，C(·)是从球坐标到笛卡尔坐标的映射，

表示从前一帧声呐参考系A变换到后一帧声呐参考系B下的坐标变换，由IMU预积分确定/>

的初始值，^Bz_i表示声呐。对每个地标都执行此过程，建立一组最佳仰角φ^*作为声呐BA的观察模型；

使用视觉声呐路标的数据关联来剔除，以声呐路标估计值为中心建立椭圆形关联门，椭圆门是由残差矢量和范数来表示的，设G为椭圆门的门限值，S为信息向量的协方差矩阵，如果信息矢量的范数满足下式，则观测位于关联门之内：

当视觉检测到物体时，对声呐和视觉路标进行数据关联，如果没有视觉路标位于关联门内，换言之声呐路标附近不存在视觉路标，就认为声呐的解无效；如果两个声呐路标都能关联到视觉路标，取位于上方的路标点，因为前视声呐成像时，上方的路标点会覆盖下方的路标点；

在确定路标位置后，以声呐特征重投影误差为代价函数，通过BA优化求解得到声呐的相对位姿估计值。

4.根据权利要求1所述的一种融合前视声呐与ORB-SLAM3的水下SLAM方法，其特征在于，步骤(3)具体如下：

在空旷的环境下，由于环境特征不明显，约束较差，声呐里程计会得到错误的估计，通过声呐失效性检测剔除错误的位姿估计；

首先，对声呐测量雅克比矩阵A进行SVD奇异值分解：

A_m×n＝USV^T

其中U_m×m和V_n×n是正交矩阵，S_m×n是奇异值(σ₁≤...≤σ_n)越来越大的对角矩阵，选择来自S的适当阈值σ_thresh，并将小于σ_thresh的特征值设置为0，当小于阈值的特征值足够多时，则认为当前声呐的位姿约束是无效的；

在判别声呐有效后，将步骤(2)中声呐相对位姿估计值加入因子图，将步骤(1)中IMU预积分项作为位姿约束因子，载体的状态估计问题表述为如下：

其中，Θ_i是与因子节点f_i相邻的变量节点的集合，f_i(Θ_i)是测量函数，z_i是一个测量值。通过使用iSAM2在因子图中联合优化IMU预积分约束、视觉里程约束、声呐里程约束的贡献来求解。

Images