一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法技术

本发明专利技术公开了一种基于深度相机的室内移动机器人稠密建图与自主导航方法，属于机器人同时定位与建图、机器人导航领域。本发明专利技术使用深度相机并基于对图像ORB特征的帧间匹配来实现对机器人进行实时定位，通过融合彩色图像与深度图像，并为了剔除冗余的视频帧引入了空间域上的关键帧提取方法，实现了实时的稠密三维点云地图构建，并将其转化成适用于导航的八叉树地图格式以及栅格地图格式，之后通过ROS Navigation功能包将其与现有移动机器人导航方法结合，实现基于纯视觉方案的室内移动机器人自主导航方案。人自主导航方案。人自主导航方案。

【技术实现步骤摘要】
一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法


[0001]本专利技术属于机器人同时定位与建图、机器人导航领域，具体涉及一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法。

技术介绍

[0002]同步定位与地图创建(Simultaneous Localization and Mapping,SLAM)是指移动机器人在没有环境先验信息的情况下，通过自身搭载的传感器，于运动过程中建立所处环境的地图模型，同时估计自身的运动。SLAM同时包含定位与建图两个问题，被认为是实现机器人自主性的关键问题之一，对机器人的导航、控制、任务规划等领域有重要的研究意义。
[0003]根据选择传感器的不同，可大致分为基于激光雷达的激光SLAM以及基于相机的视觉SLAM。激光SLAM目前已经相当成熟，但是激光雷达硬件成本较高，同时难以维护的缺点造成了其成本高昂的问题。相比之下，视觉SLAM方案则硬件成本较低，且使用方便，渐渐成为SLAM的主流研究方向。
[0004]现有的SLAM系统，大都研究的是定位问题，包括通过特征点的定位、直接法的定位，以及后端优化等，对建图模块不是很重视，建立的稀疏特征点地图也主要是服务于定位问题。但是在具体应用中，地图的用途不仅仅用于辅助定位，其明显还带有许多其他的需求，如：对机器人进行路径规划(即导航)、避障等。需要告知机器人地图中哪些地方可以通行，而哪些地方不能通行，这就超出了稀疏特征点地图的能力范围，需要建立一种稠密的地图；同时如何将建立的稠密地图与现有的移动机器人路径规划方案相结合也是需要解决的重要问题。

技术实现思路

[0005]本专利技术提供了一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，解决了对室内移动机器人周围环境的稠密建图问题，同时与移动机器人路径规划功能相结合，实现了室内移动机器人基于纯视觉方案自主导航的一体化实现。
[0006]为达到以上目的，本专利技术采用以下技术方案：
[0007]一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，包括以下步骤：
[0008]1)建立基于ORB特征的移动机器人定位与稀疏建图的SLAM方案；
[0009]2)通过对步骤1)中选取的关键帧进行适用于移动机器人自主导航的稠密地图构建以及栅格地图转换；
[0010]3)实现基于ROS的移动机器人稠密建图与自主导航一体化。
[0011]以上所述步骤中，步骤1)中基于ORB特征的移动机器人定位与稀疏建图包括以下步骤：
[0012](a)提取图像ORB特征
[0013]对深度相机输入的彩色图像流进行ORB特征的提取，以提取的ORB特征作为路标贯穿SLAM系统的全流程，之后对路标点进行特征匹配而非处理整张图像以加快运行速度：
[0014](b)基于ORB特征点的跟踪定位
[0015]对连续图像帧提取ORB特征并进行匹配，并将彩色图与深度图融合，获得ORB地图点的深度和三维坐标，后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿，通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧；
[0016](c)服务于定位的局部建图
[0017]处理上一步创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点，当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度。
[0018](d)用于地图重定位的闭环检测
[0019]处理局部建图过程中插入的关键帧，主要包含三个过程，分别是闭环检测、计算相似变换矩阵和闭环矫正，闭环检测通过计算词袋相似得分选取候选关键帧。随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(Essential Graph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹，优化位姿公式如下:
[0020][0021]其中e
i，j
为：
[0022][0023]其中，X
i，j
是位姿X
w，j
到X
w，i
的相似变换，log
sim(3)
将转移矩阵的位姿误差映射到7维欧氏空间，Δ
i，j
为边的信息矩阵。
[0024]步骤2)中通过对步骤1)中选取的关键帧进行三维稠密建图来实现，具体包括以下步骤：融合关键帧的彩色图与深度图计算得到三维点云在世界坐标系下的空间坐标，计算公式如下：
[0025][0026]其中，f
x
,f
y
,c
x
,c
y
为相机内参，(u，v)为图像坐标，(x，y，z)为图像坐标系坐标，即当前关键帧的相机位姿，d为深度相机测得像素点的距离，单位为毫米，s为实际距离和测得深度d之间的比例系数，从相机坐标系点云到全局坐标系点云的变换公式如下:
[0027][0028]其中，为第i个关键帧位姿，为在第i个关键帧坐标系上的点云，W
w，j
是变换后获得的在全局坐标系上的点云；
[0029]之后使用外点滤波器以及降采样滤波器对三维点云进行滤波去除外点以及重复的点，方法为：当相机在相邻帧运动了一定大小时才把该帧视为关键帧，并把它的点云叠加到现有的地图中去，其计算公式如下:
[0030]min_norm≤||Δt||+min(2π
‑
||r||，||r||)≤max_norm
ꢀꢀꢀꢀꢀꢀꢀ
(5)
[0031]其中，Δt为相机在相邻帧之间的位移矢量；r为相机在相邻帧之间的旋转角度，用它们的范数和来描述相机运动大小；min_norm为相机的最小运动，max_norm为相机的最大运动，即当运动大于max_norm时，认为是相机位姿估计错误，剔除该帧；
[0032]最后使用ICP方法将两关键帧之间的点云进行配准以及点云地图的拼接来构建稠密地图，使用Octomap功能包将稠密点云地图压缩并滤除地面信息转换成占用资源更小的八叉树地图格式，之后为了实现一体化自主导航功能，再将八叉树地图通过2D投影转换成占据栅格地图格式。
[0033]步骤3)通过调用ROS Navigation来实现，具体流程为：
[0034]首先，将步骤1)以及步骤2)的整个SLAM系统封装成ROS节点，将构建的栅格地图以/projected_map话题发布；其次，使用ROS Navigation功能包中的costmap_2d功能包订阅栅格地图的话题，根据栅格地图生成全局代价地图costmap，然后使用move_base功能包根据costmap使用A*算法生成全局规划路径并以nav_msgs/Path话题格式发布，最后，base_本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，包括以下步骤：1)建立基于ORB特征的移动机器人定位与稀疏建图的SLAM方案；2)通过对步骤1)中选取的关键帧进行适用于移动机器人自主导航的稠密地图构建以及栅格地图转换；3)实现基于ROS的移动机器人稠密建图与自主导航一体化。2.根据权利要求1所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，步骤1)中基于ORB特征的移动机器人定位与稀疏建图包括以下步骤：(a)提取图像ORB特征对深度相机输入的彩色图像流进行ORB特征的提取，以提取的ORB特征作为路标贯穿SLAM系统的全流程，之后对路标点进行特征匹配而非处理整张图像以加快运行速度；(b)基于ORB特征点的跟踪定位对连续图像帧提取ORB特征并进行匹配，并将彩色图与深度图融合，获得ORB地图点的深度和三维坐标，后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿，通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧；(c)服务于定位的局部建图处理上一步创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点，当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度；(d)用于地图重定位的闭环检测处理局部建图过程中插入的关键帧，主要包含三个过程：闭环检测、计算相似变换矩阵和闭环矫正。3.根据权利要求2所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，处理局部建图过程中插入的关键帧的具体过程为：闭环检测通过计算词袋相似得分选取候选关键帧，随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(Essential Graph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹，优化位姿公式如下：其中e
i，j
为：其中，X
i，j
是位姿X
w，j
到X
w，i
的相似变换，log
sim
(3)将转移矩阵的位姿误差映射到7维欧氏空间，Δ
i，j
为边的信息矩阵。4.根据权利要求1或2所述的基于深度相机的室内移动机器人稠密建图与自主导航一体化方法，其特征在于，步骤2)具体包括以下步骤：融合关键帧的彩色图与深度图计算得到三维点云在世界坐标系下的空间坐标，计算公式如下：
其中，f
x
，f
y
，c
x
，c
y
为相机内参，(u，v)为图像坐标，(x，y，z)为图像坐标系坐标，即当前关键帧的相机...

【专利技术属性】
技术研发人员：解明扬，李嘉铭，高韵婉，屈蔷，张民，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：