一种融合直接法与特征法的RGB-D同时定位与地图创建方法技术

本发明专利技术请求保护一种融合直接法与特征法的RGB‑D同时定位与地图创建方法。主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对非关键帧进行跟踪，通过最小化光度图像误差来进行相机的初始位姿估计以及像素点的对应关系计算，利用最小化局部地图点重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位；在局部建图线程中对关键帧提取并匹配ORB特征，并执行局部BA(光束平差法)，对局部关键帧位姿和局部地图点的位置进行优化，提高SLAM的局部一致性；在闭环线程中执行对关键帧的闭环检测和优化，从而保证SLAM全局一致性。另外，根据RGB‑D图像和相机位姿信息，通过基于Octomap的建图框架，构建完整准确的3D稠密环境地图。

【技术实现步骤摘要】
一种融合直接法与特征法的RGB-D同时定位与地图创建方法
本专利技术属于移动机器人视觉SLAM领域，特别是，具体涉及一种融合直接法与特征法的RGB-D同时定位与地图创建(SLAM)算法。
技术介绍
同时定位与地图构建(SLAM)是机器人领域的一个关键挑战和主要研究重点。未知环境中的自主移动机器人在执行各种任务时需要对环境地图进行构建，例如路径规划，避障和探索等。移动机器人必须知道其位置才能创建地图。另一方面，移动机器人需要通过地图来获取位置信息。因此，移动机器人必须对环境地图进行创建，同时在地图中定位。完整的视觉SLAM框架由跟踪前端、优化后端、闭环检测以及地图构建等四部分组成。当跟踪丢失时，闭环检测可以实现重定位，对于长时间导航的正确性起着至关重要的作用。视觉SLAM中的VO(visualodometry，视觉里程计)可分为两类：直接法与特征法。特征法通过提取图像中的特征点和描述子，之后进行特征点匹配，通过最小化重投影误差来计算获得机器人的位姿。匹配的特征点可用于闭环检测和重定位，这使得VO成为完整的SLAM。然而，特征提取与匹配需要耗费大量的计算时间，使得特征法比直接法耗时更多。直接法通过最小化光度误差来获得机器人的位姿信息，无需进行特征提取。然而基于直接法的SLAM的闭环检测仍然是一个待解决的问题。长时间导航出现的漂移问题仍然是直接法遇到的主要问题之一。
技术实现思路
本专利技术的目的在于提供一种融合特征法与直接法的RGB-D稠密视觉SLAM方法，以结合直接法的快速性能与直接法的高精度和闭环能力。对于关键帧提取与匹配ORB(OrientedFASTandRotatedBRIEF)特征，并通过最小化光度误差对非关键帧进行跟踪，而不需要特征提取与匹配。关键帧用于局部建图线程与闭环线程，而非关键帧用于跟踪与定位，这使得其耗时更少。此外，本专利技术还可根据RGB-D图像和相机位姿信息，通过在基于八叉树的建图框架Octomap上构建3D占用网格地图，为移动机器人完成导航和避障等任务提供更加准确的环境地图。提出了一种融合直接法与特征法的RGB-D同时定位与地图创建方法。本专利技术的技术方案如下：一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其包括以下步骤：步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；步骤S4，在局部建图线程中，对当前关键帧图像中的ORB(OrientedFASTandRotatedBRIEF)特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；步骤S5，当处理完所有关键帧之后，执行局部BA(光束平差法)优化，对周围的关键帧位姿以及地图点位置进行优化；步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。进一步的，所述步骤S2相机的初始位姿估计包括以下几个步骤：步骤S2.1，跟踪线程通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型(即假定摄像头处于匀速运动状态)来对当前相机的位姿进行预测。步骤S2.2，根据步骤S2.1中估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中，当前帧与参考帧的相对位姿变换Tcr∈SE(3)则可以通过最小化光度误差计算得到，该误差主要建立在同一个3D点对应的图像块之间，即其中：u是参考帧的像素位置，ρ表示二范数，T表示变换矩阵。其中xri是参考帧坐标系中第i个被跟踪的3D地图点的位置。lr表示参考帧的像素灰度值。进一步的，所述步骤S2求取像素点对应关系的过程为：通过初始位姿估计，得到经过优化且与上一帧像素点相关联的地图点集，从该地图点集中，得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系，最小化光度误差的方法包括以下步骤：为寻找与像素点p1对应着的同一像素点p2，首先根据当前相机的位姿估计值，来寻找p2的位置。但若相机的位姿不够好，p2的外观与p1会有明显差别。从而假设一个空间点在各个视角下，成像的灰度是不变的。基于灰度值不变的假设，通过非线性优化的方法最小化p的两个像素点的亮度误差来优化相机位姿，寻找与p1点更相似的p2点，其中优化变量为相机位姿。进一步的，所述将局部地图点在当前帧中进行投影，具体包括：由初始的投影像素坐标栅格化投影到当前帧中的地图点，采用10×10栅格，并根据深度信息对栅格中的地图点进行排列，在计算像素点匹配时，对每个栅格至多选取6个不同深度的地图点，通过考虑曝光时间引起的灰度变化，引入光度响应函数对其进行优化式中：u′i表示两个像素点之间的对应关系，Ik表示局部地图点所在关键帧对应的灰度数据，Ir表示局部地图点所在参考帧对应的灰度数据，Ai表示仿射变换矩阵，Δtr和Δtk分别是Ir和Ik的曝光时间，f(·)光度响应函数，f-1(·)为逆光度响应函数。最后利用G-N(GaussNewton)算法求解上式。G-N(GaussNewton)算法步骤如下：(1)给定初始值；(2)对于第次迭代，求出当前的雅可比矩阵J和误差f；(3)求解增量方程：HΔxk；若Δxk足够小，则停止。否则，令Δxk+1＝xk+Δxk，返回(2)。进一步的，所述步骤S3跟踪局部地图的过程为：在完成局部地图点与当前帧像素点匹配后，利用g2o通过最小化重投影误差再次优化相机的相对位姿Tcw为其中pwi是世界坐标系中第i个Covisibility地图点，g2o是一个基于图优化的库。此过程的优化问题仅对相机位姿执行BA优化，由于单个位姿节点的BA问题对于地图点的约束太过单一，因此将地图点的优化放在局部BA线程中执行，利用最小化局部地图点的重投影误差再次优化相本文档来自技高网...

【技术保护点】
1.一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，包括以下步骤：/n步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；/n步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；/n步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；/n步骤S4，在局部建图线程中，对当前关键帧图像中的ORB特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；/n步骤S5，当处理完所有关键帧之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化；/n步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；/n步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；/n步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；/n步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；/n步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。/n...

【技术特征摘要】
1.一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，包括以下步骤：
步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；
步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；
步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；
步骤S4，在局部建图线程中，对当前关键帧图像中的ORB特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；
步骤S5，当处理完所有关键帧之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化；
步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；
步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；
步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；
步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；
步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。


2.根据权利要求1所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2相机的初始位姿估计包括以下几个步骤：
步骤S2.1，跟踪线程通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型即假定摄像头处于匀速运动状态来对当前相机的位姿进行预测；
步骤S2.2，根据步骤S2.1中估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中，当前帧与参考帧的相对位姿变换则可以通过最小化光度误差计算得到，该误差主要建立在同一个3D点对应的图像块之间，即



其中：是参考帧的像素位置，ρ表示二范数，是图像块区域，T表示变换矩阵；



其中是参考帧坐标系中第i个被跟踪的3D地图点的位置，表示参考帧的像素灰度值。


3.根据权利要求1或2所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2求取像素点对应关系的过程为：通过初始位姿估计，得到经过优化且与上一帧像素点相关联的地图点集，从该地图点集中，得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系，最小化光度误差的方法包括以下步骤：为寻找与像素点p1对应着的同一像素点p2，首先根据当前相机的位姿估计值，来寻找p2的位置。但若相机的位姿不够好，p2的外观与p1会有明显差别，从而假设一个空间点在各个视角下，成像的灰度是不变的。基于灰度值不变的假设，通过非线性优化的方法最小化p的两个像素点的亮度误差来优化相机位姿，寻找与p1点更相似的p2点，其中优化变量为相机位姿。


4.根据权利要求3所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述将局部地图点在当前帧中进行投影，具体包括：由初始的投影像素坐标栅格化投影到当前帧中的地图点，采用10×10栅格，并根据深度信息对栅格中的地图点进行排列，在计算像素点匹配时，对每个栅格至多选取6个不同深度的地图点，通过考虑曝光时间引起的灰度变化，引入光度响应函数对其进行优化



式中：u′i表示两个像素点之间的对应关系，Ik表示局部地图点所在关键帧对应的灰度数据，Ir表示局部地图点所在参考帧对应的灰度数据，Ai表...

【专利技术属性】
技术研发人员：胡章芳，张杰，
申请(专利权)人：重庆邮电大学，
类型：发明
国别省市：重庆;50