一种基于激光SLAM与单目SLAM融合的里程计方法

一种基于激光slam与单目slam融合的里程计方法
技术领域
1.本发明属于slam中里程计的融合方法，具体涉及一种基于激光slam与单目视觉slam融合的里程计方法。


背景技术：

2.同时定位与地图创建（simultaneous localization and mapping，slam）是移动机器人在解决未知环境中的探索、侦查、导航等各项问题的基础。根据搭载的传感器不同，主流的slam方式分为激光slam与视觉slam。
3.视觉slam，相机具有成本低、图像信息丰富等优点，因此在几何机构相似的环境、处理回环检测等问题上表现良好。但容易受到光照变化的影响，尤其是采用单目相机作为传感器时会产生尺度漂移等问题。
4.激光slam，理论相对于视觉slam较为成熟，但存在几何结构相似的环境下效果不佳以及难以处理动态物体等问题。
5.多传感器融合技术与机器人技术的结合成为现阶段机器人发展方向的研究热点。多种传感器能够实现取长补短，尤其是在slam方面的应用，能够协助机器人更好的完成地图构建与定位功能，从而为后续路径规划、自主导航与避障等工作打下良好基础。


技术实现要素：

6.本发明的目的是解决现有slam技术的缺陷，提供一种基于激光slam与单目视觉slam融合的里程计方法，该技术的方案包括。
7.步骤1，数据采集。
8.机器人安装单目相机以及激光雷达传感器，通过单目相机获取rgb图像数据；三维激光雷达获取点云数据，工控机实时读取上述传感器数据。
9.步骤2，通过激光点云赋予特征点深度。
10.对单目相机采集的rgb图像进行特征提取，在特征提取后进行特征匹配工作，同时将激光雷达采集的点云投影在rgb图上。在特征点周围选取方形区域，提取区域内点云的深度值，通过对各个点云的深度值进行插值计算，从而得到该特征点的深度。
11.步骤3，通过判断特征点深度信息估计先验位姿。
12.在帧与帧之间估计位姿时，先对前一帧的中特征点的深度信息进行判断：
①
若前一帧中的特征点全部具有深度信息，则与当前帧中的特征点构建pnp(perspective-n-point)问题从而求得帧间位姿；
②
若前一帧中的存在不具备深度信息的特征点，采用前一帧中有深度特征点与当前帧中对应的特征点构建pnp(perspective-n-point)问题，提取该位姿中的平移向量作为两帧之间估计位姿的位移向量；同时，对两帧所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵；
③
若前一帧中的特征点均没有深度信息，则对两帧所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵。并采用优化对极几何的方式计算位移向量作为
两帧之间的估计位姿的位移向量。
[0013] 步骤4，后端bundle adjustment优化。
[0014]
在得到初始先验位姿后，通过后端的bundle adjustment优化得到最终的优化后位姿。
[0015]
本发明的有益效果：
①
本发明克服了单目slam中因缺乏深度信息而产生的尺度漂移、纯激光slam无法处理几何结构相似的环境等问题；
②
本发明在计算帧间位姿时，通过对于前一帧中特征点深度信息的判断分别构建pnp(perspective-n-point)问题、pnp(perspective-n-point)问题与对极几何问题、改进对极几何问题的方式，从而提高slam算法的定位精度。
附图说明
[0016]
图1为算法整体流程图。
[0017]
图2为对于帧间位姿算法。
具体实施方式
[0018]
本实施例提供一种基于激光slam与单目视觉slam融合的里程计方法，主要包括以下步骤。
[0019]
步骤1，数据采集。
[0020]
步骤2，通过激光点云赋予特征点深度。
[0021]
步骤3，通过判断特征点深度情况估计先验位姿。
[0022]
步骤4，后端bundle adjustment优化。
[0023]
为了更好理解上述技术方案，下面结合具体实施方式对上述技术方案进行详细的说明。
[0024]
步骤1，机器人安装单目相机以及激光雷达传感器，通过单目相机获取rgb图像数据；3维激光雷达获取点云数据，工控机实时读取上述传感器数据。
[0025]
步骤2，在进行特征的提取与匹配后，通过激光点云赋予特征点深度。
[0026]
步骤2.1，本发明中采用特征点法作为视觉slam方法，因此从单目相机采集的rgb图像中提取orb特征。
[0027]
步骤2.2，提取特征点后采取快速近似最近邻算法完成特征的匹配。
[0028]
步骤2.3，在特征点周围选取方形区域，提取区域内点云的深度值，使用区域内点云的深度值进行插值计算，从而得到该特征点的深度。
[0029]
步骤3，判断特征点深度信息估计先验位姿。
[0030]
步骤3.1，若前一帧中的特征点全部具有深度信息，则与当前帧中的特征点构建pnp(perspective-n-point)问题从而计算位姿。
[0031]
步骤3.2 当上一帧中特征点不全具有深度值时，采用具有深度值的特征点和当前图像帧中对应的特征点构建pnp问题方式求解位姿，取中的位移部分作为估计位姿中的位移向量，并对所有特征点构建对极几何问题求解位姿，取中的旋转部分
前作为估计位姿中的旋转矩阵，与构成实际估计位姿；式中为上一帧中所有特征点，为当前帧中所有特征点。
[0032]
步骤3.3，当前一帧中的特征点均不具有深度时，采用对两帧中所有特征点进行构建对极几何问题求解位姿，取中的旋转部分作为估计位姿中的旋转矩阵，中的位移向量为：式中为中的位移向量，为两帧中特征点的实际距离，取和构成实际估计位姿。
[0033]
步骤4，后端ba优化。在得到初始先验位姿后，通过后端的bundle adjustment优化得到最终的优化后位姿。


技术特征：
1.一种基于激光slam与单目视觉slam融合的里程计方法，其特征在于，包括以下步骤：步骤1，视觉数据与激光数据的采集机器人安装单目相机以及激光雷达传感器，通过单目相机获取rgb图像数据；三维激光雷达获取点云数据，工控机实时读取上述传感器数据；步骤2，通过激光点云赋予特征点深度对单目相机采集的rgb图像数据进行特征提取，在特征提取后进行特征匹配，同时将激光雷达采集的点云投影在rgb图上，在特征点周围选取方形区域，提取区域内点云的深度值，通过对各个点云深度值的进行插值计算，从而得到该特征点的深度；步骤3，通过判断特征点深度情况估计先验位姿在估计帧与帧之间的先验位姿时，先对前一帧的中特征点的深度信息进行判断：
①
若前一帧中的特征点全部具有深度信息，则与当前帧中全部的特征点构建pnp(perspective-n-point)问题从而计算帧间位姿；
②
若前一帧中存在不具备深度信息的特征点，采用前一帧中有深度特征点与当前帧中对应的特征点构建pnp(perspective-n-point)问题，提取该位姿中的平移向量作为两帧之间估计位姿的位移向量；同时，对两帧中所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵；
③
当前一帧中的特征点均没有深度信息时，对两帧中所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵；并采用优化对极几何的方式计算位移向量作为两帧之间的估计位姿的位移向量；步骤4，后端bundle adjustment优化在得到初始先验位姿后，通过后端的bundle adjustment优化得到最终的优化后位姿。2.根据权利要求1所述的基于激光slam与单目视觉slam融合里程计方法，其特征在于：所述步骤1中，单目相机获取rgb图像数据，激光雷达通过扫描获得具有坐标数值、角度、距离等信息的激光点云数据。3.根据权利要求1所述的基于激光slam与单目视觉slam融合里程计方法，其特征在于：所述步骤2中，在本算法中，利用图像特征点作为路标；提取orb特征点，并通过描述子的差异来完成特征匹配；从单目相机采集的rgb图像中提取的特征点并不具有深度信息，此时将激光点云投影到rgb图像上，在特征点周围选取方形区域并提取区域内点云的深度值，通过插值计算得到特征点的深度信息。4.根据权利要求1所述的基于激光slam与单目视觉slam融合里程计方法，其特征在于：所述步骤3中，对于前一帧中特征点的深度信息进行判断后；
①
若前一帧中的特征点全部具有深度信息，则与当前帧中的特征点构建pnp(perspective-n-point)问题从而计算估计位姿；
②
若前一帧中特征点不全具有深度信息时，则采用具有深度信息的特征点和当前图像帧中对应的特征点构建pnp问题方式求解位姿，取中的位移部分作为估计位姿中的位移向量，并对所有特征点构建对极几何问题求解位姿，取中的位移部分作为估计位姿中的旋转矩阵，与构成实际估计位姿；
式中为前一帧中所有特征点，为当前帧中所有特征点；
③
若前一帧中的特征点均不具有深度信息时，则采用对两帧中所有特征点进行构建对极几何问题的方式求解位姿，取中的旋转部分作为估计位姿中的旋转矩阵，中的位移向量为：式中为中的位移向量，为两帧中特征点的实际距离，取和构成实际估计位姿。

技术总结
本发明提出了一种基于激光SLAM与单目视觉SLAM融合的里程计方法。首先本发明采用激光雷达和单目相机同时采集数据，再对单目相机采集的图像进行特征点提取与匹配，之后再将激光雷达的点云投影在单目相机采集的图像上，从而计算之前提取出的特征点深度。在估计先验位姿时，对前一帧中的特征点深度信息进行判断，通过不同的三种情况分别进行位姿的估计，从而得到更精确的先验位姿，最后通过Bundle Adjustment得到最终优化后的位姿。极大的改善了单目SLAM中当机器人沿光轴方向运动时导致的位姿估计不准确以及激光SLAM在几何特征相似的环境中失效的问题。似的环境中失效的问题。


技术研发人员：何丽 齐继超 袁亮
受保护的技术使用者：新疆大学
技术研发日：2020.11.23
技术公布日：2022/5/25