CN113744337A - 一种融合视觉、imu与声纳的同步定位与建图方法 - Google Patents

Abstract

本发明提供了一种融合视觉、IMU与声纳的同步定位与建图方法，通过求取局部时间与空间场的载体位姿变换再与IMU数据互补，同时利用图像的纹理信息辅助识别与框取环境中的目标信息，形成有界的体素边框，回环部分先利用声纳进行大范围回环，然后在相机图像融合的点云数据中，对已经聚类的目标特征选用三维矢量参数进行定义，建立词袋模型（BoW）用分类器判断，利用k‑d树对目标特征进行快速搜索，完成粗略匹配，最后使用迭代最近点（ICP）完成精确匹配完成回环检测，可以使原本稀疏的声纳点云图像通过与视觉图像融合，获得带有颜色信息的，并能对选定目标信息补充使之成为更易于框取的图像，有利于对特定目标的提取与识别，同时获得被测物体的距离信息。

Description

一种融合视觉、IMU与声纳的同步定位与建图方法

技术领域

本发明涉及同步定位与建图方法的技术领域，具体涉及一种融合视觉、IMU与声纳的同步定位与建图方法。

背景技术

声纳、视觉与IMU是水下机器人同步定位与建图的重要技术手段，但是由于水下环境的特殊性，视觉能见度距离有限，但可以提供临近场景的丰富环境信息，而声纳可以不受水下浑浊度，湍流等因素的影响，可提供准确的环境目标距离信息，但采样率低，易受水下环境噪声且在近处数据缺失的影响，声纳测量中存在的噪声是阻碍现有的声纳数据与视觉融合的主要原因之一，且数据形式大多是稀疏的，同时利用IMU与视觉互补形成一个传统的VIO，协同提供较高精度的位姿估计,再利用声纳为主视觉辅助融合建图，使之获得包含视觉信息的地图，IMU数据存在零偏，对于静止与缓慢运动测量误差大，但却能对于短时间的快速运动提供较好的估计，相机数据漂移较少，但在快速运动时会出现运动模糊，可以有效的修正IMU数据，因此，将水下声纳、IMU和视觉信息融合可有效提高水下SLAM的准确性与鲁棒性。

发明内容

本发明为了克服声纳点云图像过于稀疏且没有纹理信息且声纳点云图像存在缺失，不利于对特定目标框取识别，声纳点云图由于采样率低，载体运动情况下存在一定的畸变等问题，因此提供了一种融合视觉、IMU与声纳的同步定位与建图方法。

本发明解决其技术问题所采用的技术方案是：

一种融合视觉、IMU与声纳的同步定位与建图方法，包括以下步骤：

S1：进行相机标定，获得相机内参外参，求解声纳坐标系与相机坐标系之间的关系，获得声纳与相机的位姿变换，使相机与声纳实现同空间信息获取，完成联合标定；

S2：设定同步晶振触发方式，使用特定电路硬件记录时间戳信息，根据声纳传感器与摄像头的采样频率设定触发规则，在融合时刻进行同时触发，以解决时间同步问题；

S3：采用与相机松耦合的IMU，用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计，用于运动预测与对基于视觉得出的位姿估计结果融合，其主要方案为基于卡尔曼滤波的方法；

S4：初始化，将相机进行平移，根据相机获取的图像对当前相机姿态进行运动估计，用于在缓慢运动的情况下去除声纳的点云畸变。

S5:对相机图像利用Tenengrad梯度算法进行模糊检验，设定阈值T，当超过阈值时认为存在运动模糊，此时利用IMU完成因相机快速运动造成运动模糊无法进行位姿估计时的运动补偿，完成点云畸变去除；

S6：将声纳点云图像体素栅格滤波，建立八叉树地图，将相机图像特征点映射到声纳点云图像中，从图像的纹理信息分析环境目标特征，构建有界约束框架。

S7：对框取目标的声纳点云图像与相机图像进行分辨率匹配，采用高斯过程回归对目标点云进行插值，完成数据关联获得带有声纳提供深度信息的目标的相机图像，相机针孔模型中，将一个三维点投影到一个二维成像平面，设现实空间中存在一点P坐标为[X，Y，Z]^T，投影后P’坐标为[X’，Y’，Z’]^T，f为焦距，

像素坐标系为成像平面加上一个缩放α(μ)，β(υ)与一个原点平移[C_x,C_y]^T,可得P’的像素坐标[μ,υ]^T

μ＝αX′+C_x (3)

υ＝βY′+C_y (4)

将αf合并为f_x,把βf合并为f_y,得：

在声纳模型中，声纳投影的模型直接建立在球坐标系上

由于声纳会丢失标高信息φ；

坐标系间的转换，将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换：

P_s＝RP_c+T (9)

得：

立体通信约束：

未知量φ可以消除：

最终得到：

通过下式，可得到相机像素Zc的场景深度：

此时完成数据关联，获得带有声纳提供深度信息的目标物的相机图像；

S8：使用声纳点云图像进行初步的回环检测，利用进行过插值处理的点云图像建立submap(子图)，提取稀疏的点云关键帧与submap匹配，匹配后进行下一步的图像精匹配；

S9：对融合后图像提取稀疏关键帧，针对框架约束的点云信息，采用K-means++算法进行聚类，设定以直线度，平面度，散度这三维为代表的矢量特征建立词袋模型，通过分类器进行判断，利用k-d树对特征目标进行快速搜索，完成第一步匹配，然后通过使用迭代最近点(ICP)完成进一步的匹配，完成回环检测。

本发明的进一步改进在于：对于步骤S4中对当前相机姿态进行估计的操作包括以下步骤：

S4-1：提取ORB特征点；

S4-2：使用FLANN匹配特征点；

S4-3：进行对极几何约束计算获得相机的基础矩阵F与本质矩阵E；

S4-4：对本质矩阵E进行奇异值分解得到运动参数[R,t],从IMU提供的数据中获得初始尺度与重力方向。

本发明的进一步改进在于：所述步骤S5中利用视觉与IMU互补进行运动补偿的操作包括以下步骤：

S5-1：计算相机图像的Tenengrad梯度函数；

S5-2：设定阈值T，判断图像是否存在运动模糊；

S5-3：若无运动模糊，将基于相机图像进行位姿估计，用于去除缓慢运动下的声纳点云畸变；

S5-4：若存在运动模糊，将提取运动模糊前邻近且不连续的三帧图像，与运动模糊后邻近且不连续的三帧图像进行比对，若存在明显相似部分则认为相机运动为原地抖动，此时将利用相似帧进行位姿估计去除声纳点云畸变；若无相似部分，则认为相机做了快速运动，此时将利用IMU进行运动补偿。

本发明的进一步改进在于：所述步骤S6中的相机图像的特征点映射到声纳点云图像实施过程包括以下步骤：

S6-1：获取拥有相同时间戳的声纳点云图像与相机图像；

S6-2：将三维点云进行分割栅格，设定栅格大小，栅格数；

S6-3：对每个栅格内的点云进行双边滤波，去除部分点云，达到向下采样同时保全点云几何结构；

S6-4：将滤波过的声纳点云图像转换到相机坐标系中；

S6-5：运用针孔模型将三维点云映射到图像坐标系中，接着离散化到像素坐标系中；

S6-6：判断二维像素平面内的声纳三维点云是否在二维相机图像平面中；

S6-7：将存在该平面内的三维点云保留并对齐；

S6-8：用RANSAC算法进行校验；

S6-9：完成映射。

本发明的进一步改进在于：步骤S9中的提取稀疏关键帧过程包括以下步骤：

S9-1：设定一个最小关键帧间隔，使关键帧尽量稀疏；

S9-2：检测到回环后，将成功回环的相似关键帧聚类，避免无效回环；

S9-3：设立验证机制，仅当一段时间持续检测到回环，认为回环成功。

本发明的进一步改进在于：步骤S5中若存在运动模糊，且存在明显相似部分则认为相机运动为原地抖动，将利用相似帧进行位姿估计去除声纳点云畸变；若无相似部分，此时将利用IMU进行运动补偿。

本发明的进一步改进在于：所述利用IMU进行运动补偿的操作包括以下步骤：

S5-4-1：积分一个声纳帧扫描起始和结束时刻之间的IMU测量；

S5-4-2：利用每个激光测量点的时间戳对IMU积分结果进行线性插值；

S5-4-3：将每个点投影到激光帧的起始时刻，完成相机数据失效时的运动补偿。

与现有技术相比，本发明的有益效果为：

本发明为一种融合视觉、IMU与声纳的同步定位与建图方法，通过视觉与IMU松耦合，弥补了视觉数据无效时无法获得位姿信息的问题，同时可以使原本稀疏的声纳点云图像通过与视觉图像融合，获得带有颜色信息的，并能对选定目标信息补充使之成为更易于框取的图像，有利于对特定目标的提取与识别，同时获得被测物体的距离信息。

附图说明

图1是本发明方法的流程图；

图2是本发明的传感器联合标定示意图。

具体实施方式

为了使本发明的目的，技术方案与优点更明确，因此对本发明进一步详细说明，一种融合视觉、IMU与声纳的图像处理方法，包括以下步骤：

S1：相机的标定，获得内参外参；相机与声纳的联合标定：将相机标定中的世界坐标系替换为声纳坐标系，求出两者之间的关系，构建极大似然估计模型，用LM算法(Levenberg-Marquardt Algorithm)求得最优解。得到一个刚体旋转，实现相机与声纳数据的对齐，完成联合标定。

S2：声纳的扫描周期约为6s一次，而相机却1s有10帧图像，为了实现数据同步，设定同步晶振触发方式，使用特定电路硬件记录时间戳信息，根据声纳传感器与摄像头的采样频率设定触发规则，在融合时刻进行同时触发，以解决时间同步问题。

S3:采用松耦合的IMU，用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计，用于运动预测与对基于视觉得出的位姿估计结果融合，其主要方案为基于卡尔曼滤波的方法。

S4:初始化，将相机进行一定程度的平移，根据相机获取的图像对当前相机姿态进行估计,操作为在图像中进行ORB特征点的提取，ORB特征点由关键点(Oriented Fast)与描述子(BRIEF)构成。

关键点的尺度信息由构建的图像金字塔进行匹配获得，旋转信息通过计算特征点附近的图像灰度质心获得。

使用FLANN(快速近似最邻近)匹配特征点,FLANN是一个对大数据集合和高维特征进行最近邻近搜索的合集，其中的算法主要包括随机k-d树算法，优先搜索k-means树算法和层次聚类树；

通过对极几何约束构建关于本质矩阵E的方程，先求解得到一个E的初值，用八点法和SVD恢复相机的[R,t]，从IMU提供的数据中获得初始尺度与重力方向。

将得到的运动矩阵用于补偿去除当相机缓慢运动情况下的声纳点云图像的畸变。将由于载体运动所造成的基准坐标系不同的每列声纳点的坐标，乘上基于运动矩阵得出的相对转换，转换成在初始坐标系下的声纳点。

S5:计算相机图像的Tenengrad梯度函数，采用Sobel算子提取水平与垂直方向的梯度值，计算像素点处于Sobel水平与垂直方向边缘检测算子的卷积，设定边缘检测阈值T，判断图像是否存在运动模糊；若无运动模糊，将基于相机图像进行位姿估计，用于去除缓慢运动下的声纳点云畸变；若存在运动模糊，将提取运动模糊前邻近且不连续的3帧图像，与运动模糊后邻近且不连续的3帧图像进行比对；若存在明显相似部分则认为相机运动为原地抖动，此时将利用相似帧进行位姿估计去除声纳点云畸变；若无相似部分，则认为相机做了快速运动，此时将利用IMU进行运动补偿。

IMU进行运动补偿，首先积分一个声纳帧扫描起始和结束时刻之间的IMU测量，然后利用每个激光测量点的时间戳对IMU积分结果进行线性插值，从而将每个点投影到声纳帧的起始时刻，实现运动补偿并完成相机快速运动造成运动模糊情况下的点云畸变去除。

S6：对声纳点云图像预处理，去除深度值无效的点，利用统计滤波器方法去除孤立点，将声纳3D点云图像用体素栅格滤波，通过建立一个三维体素格栅，用体素中所有点的重心来表示近似的点，达到向下采样同时保全点云几何结构，使分割栅格更容易，接着设定栅格大小，栅格数，此时每个栅格中都有一定数量的点云，然后采用双边滤波，采取临近采样点的加权平均来修正当前采样点的位置，同时有选择的剔除部分差异过大的相邻采样点，剔除部分点云并建立八叉树地图以便于目标配对。

将视觉图像纹理映射到点云图像上，选取拥有相同时间戳的三维点云和图像，将三维点云转换到相机坐标系中，通过针孔模型将三维点云映射到图像坐标，接着离散化到像素坐标系，确定尺寸范围，三维点云对齐，用RANSAC算法进行校验，完成映射，对于点云图像缺失部分，进行最邻近插值算法(NNIA)，将成像数据对应回波点的极坐标值，计算各点在笛卡尔坐标下的位置并将成像数据表示的回波点像素值填充到对应坐标位置，完成插值，从而便于后续视觉图像信息的补全，最终获得了包含视觉信息的地图。

对相机图像进行灰度处理，利用图像梯度算法提取边框实现对目标物的框取，使目标物图像易于提取，通过上述的映射关系得到该目标物的点云信息，并利用二维相机图像完成对目标物点云信息的补充。

S7:对框取目标的声纳点云图像与相机图像进行分辨率匹配，再采用高斯过程回归对目标点云进行插值。

相机针孔模型中，将一个三维点投影到一个二维成像平面，设现实空间中存在一点P坐标为[X,Y,Z]^T，投影后P’坐标为[X’,Y’,Z’]^T，f为焦距。

像素坐标系为成像平面加上一个缩放α(μ)，β(υ)与一个原点平移[C_x,C_y]^T,可得P’的像素坐标[μ,v]^T

μ＝αX′+C_x (3)

υ＝βY′+C_y (4)

将αf合并为f_x,把βf合并为f_y,得：

在声纳模型中，声纳投影的模型直接建立在球坐标系上

由于声纳会丢失标高信息φ

坐标系间的转换，将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换。

P_s＝RP_c+T (9)

得：

立体通信约束：

未知量φ可以消除：

最终得到：

通过下式，可得到相机像素Zc的场景深度：

此时完成数据关联，获得带有声纳提供深度信息的目标物的相机图像。

S8：使用声纳点云图像进行初步的回环检测，利用进行过插值处理的点云图像建立submap(子图)，提取稀疏的点云关键帧与submap进行匹配，一旦发现匹配就立刻进行下一步的图像精匹配。

S9：对融合后的图像提取关键帧，同时设定一个最小关键帧间隔，使关键帧尽量稀疏，采用K-means++算法对点云进行聚类，从而实现分割，特征提取通过计算聚类C_i的矢量特征f_i＝{f_i1，f_i2，f_i3}建立词袋模型，分别代表直线度，平面度，散度这三维的几何特性。

采用由随机森林算法驱动的分类器进行判断，利用k-d树对所有特征向量进行快速搜索，得到潜在匹配对象，由一系列的决策树分类投票，最终得到匹配的特征目标，完成第一步匹配。

使用迭代最近点(ICP)求解，先对点云进行滤波、清理部分数据等预处理；应用Least Squares来求解变换，找出最近点；调整一些对应点对的权重；剔除不合理对应点对；计算loss，最小化loss，求解当前最优变换；回到第二步进行迭代，直到收敛。完成较为精细的匹配，完成回环检测。

当检测到回环后，将成功回环的相似关键帧聚类，避免无效回环，同时设立验证机制，仅当一段时间持续检测到回环，认为回环成功。

Claims (6)

1.一种融合视觉、IMU与声纳的同步定位与建图方法，其特征在于，包括以下步骤：

S1：进行相机标定，获得相机内参外参，求解声纳坐标系与相机坐标系之间的关系，获得声纳与相机的位姿变换，使相机与声纳实现同空间信息获取，完成联合标定；

S2：设定同步晶振触发方式，使用特定电路硬件记录时间戳信息，根据声纳传感器与摄像头的采样频率设定触发规则，在融合时刻进行同时触发；

S3：采用与相机松耦合的IMU，用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计，用于运动预测与对基于视觉得出的位姿估计结果融合；

S4：初始化，将相机进行平移，根据相机获取的图像对当前相机姿态进行运动估计，用于在缓慢运动的情况下去除声纳的点云畸变。

S5：对相机图像利用Tenengrad梯度算法进行模糊检验，设定阈值T，当超过阈值时认为存在运动模糊，利用IMU完成点云畸变去除；

S6：将声纳点云图像体素栅格滤波，建立八叉树地图，将相机图像特征点映射到声纳点云图像中，构建有界约束框架；

S7：对框取目标的声纳点云图像与相机图像进行分辨率匹配，采用高斯过程回归对目标点云进行插值，完成数据关联获得带有声纳提供深度信息的目标的相机图像，相机针孔模型中，将一个三维点投影到一个二维成像平面，设现实空间中存在一点P坐标为[X,Y,Z]^T，投影后P’坐标为[X’,Y’,Z’]^T，f为焦距，

像素坐标系为成像平面加上一个缩放α(μ)，β(υ)与一个原点平移[C_x,C_y]^T,可得P’的像素坐标[μ,υ]^T

μ＝αX′+C_x (3)

υ＝βY′+C_y (4)

将αf合并为f_x,把βf合并为f_y,得：

在声纳模型中，声纳投影的模型直接建立在球坐标系上

坐标系间的转换，将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换：

P_s＝RP_c+T (9)

得：

立体通信约束：

未知量φ可以消除：

最终得到：

通过下式，可得到相机像素Zc的场景深度：

此时完成数据关联，获得带有声纳提供深度信息的目标物的相机图像；

S8：使用声纳点云图像进行初步的回环检测，利用进行过插值处理的点云图像建立submap，提取稀疏的点云关键帧与submap匹配，匹配后进行下一步的图像精匹配；

S9：对融合后图像提取稀疏关键帧，针对框架约束的点云信息，采用K-means++算法进行聚类，设定以直线度，平面度，散度这三维为代表的矢量特征建立词袋模型，通过分类器进行判断，利用k-d树对特征目标进行快速搜索，完成第一步匹配，然后通过使用迭代最近点完成进一步的匹配，完成回环检测。

2.根据权利要求1所述的方法，其特征在于，所述步骤S5中利用视觉与IMU互补进行运动补偿的操作包括以下步骤：

S5-1：计算相机图像的Tenengrad梯度函数；

S5-2：设定阈值T，判断图像是否存在运动模糊；

S5-3：若无运动模糊，将基于相机图像进行位姿估计，用于去除缓慢运动下的声纳点云畸变；

S5-4：若存在运动模糊，将提取运动模糊前邻近且不连续的三帧图像，与运动模糊后邻近且不连续的三帧图像进行比对，若存在明显相似部分则认为相机运动为原地抖动，此时将利用相似帧进行位姿估计去除声纳点云畸变；若无相似部分，则认为相机做了快速运动，此时将利用IMU进行运动补偿。

3.根据权利要求1所述的方法，其特征在于，所述步骤S6中的相机图像的特征点映射到声纳点云图像实施过程包括以下步骤：

S6-1：获取拥有相同时间戳的声纳点云图像与相机图像；

S6-2：将三维点云进行分割栅格，设定栅格大小，栅格数；

S6-3：对每个栅格内的点云进行双边滤波，去除部分点云，达到向下采样同时保全点云几何结构；

S6-4：将滤波过的声纳点云图像转换到相机坐标系中；

S6-5：运用针孔模型将三维点云映射到图像坐标系中，接着离散化到像素坐标系中；

S6-6：判断二维像素平面内的声纳三维点云是否在二维相机图像平面中；

S6-7：将存在该平面内的三维点云保留并对齐；

S6-8：用RANSAC算法进行校验；

S6-9：完成映射。

4.根据权利要求1所述的方法，其特征在于，步骤S9中的提取稀疏关键帧过程包括以下步骤：

S9-1：设定一个最小关键帧间隔，使关键帧尽量稀疏；

S9-2：检测到回环后，将成功回环的相似关键帧聚类，避免无效回环；

S9-3：设立验证机制，仅当一段时间持续检测到回环，认为回环成功。

5.根据权利要求1所述的方法，其特征在于，步骤S5中若存在运动模糊，且存在明显相似部分则认为相机运动为原地抖动，将利用相似帧进行位姿估计去除声纳点云畸变；若无相似部分，此时将利用IMU进行运动补偿。

6.根据权利要求6所述的方法，其特征在于，所述利用IMU进行运动补偿的操作包括以下步骤：

S5-4-1：积分一个声纳帧扫描起始和结束时刻之间的IMU测量；

S5-4-2：利用每个激光测量点的时间戳对IMU积分结果进行线性插值；

S5-4-3：将每个点投影到激光帧的起始时刻，完成相机数据失效时的运动补偿。

Images