CN111415417A

CN111415417A - 一种集成稀疏点云的移动机器人拓扑经验地图构建方法

Info

Publication number: CN111415417A
Application number: CN202010291189.XA
Authority: CN
Inventors: 刘冬; 丛明; 陈飞
Original assignee: Jiangsu Research Institute Co Ltd of Dalian University of Technology
Current assignee: Jiangsu Research Institute Co Ltd of Dalian University of Technology
Priority date: 2020-04-14
Filing date: 2020-04-14
Publication date: 2020-07-14
Anticipated expiration: 2040-04-14
Also published as: CN111415417B

Abstract

本发明属于机器人技术与应用领域，涉及一种集成稀疏点云的移动机器人拓扑经验地图构建方法，该方法根据稀疏点云获取的关键帧数据与相机采集的视觉图像增量式构建可直接用于移动机器人的路径规划与导航的拓扑经验地图。本发明解决了稀疏点云地图用于自主导航任务的信息不充分问题，通过稀疏点云在线构建环境的拓扑经验地图，拓扑经验地图可以直接用于移动机器人导航。

Description

一种集成稀疏点云的移动机器人拓扑经验地图构建方法

技术领域

本发明属于机器人技术与应用领域，涉及一种集成稀疏点云的移动机器人拓扑经验地图构建方法。

背景技术

随着机器人技术的发展，移动机器人在科学研究、工业领域、家庭和商业环境中的应用日益普遍。定位与导航是移动机器人的核心能力，而地图构建是完成定位与导航任务的基础。近年来，随着计算能力的提高与图像处理技术的发展，基于视觉的同步定位与地图构建技术(Visual Simultaneous Localization and Mapping,vSLAM)成为了热门研究课题并实现快速发展。vSLAM算法构建地图包含稠密地图与稀疏地图，在实际应用中，稀疏地图用于导航任务具有信息不充分的问题。稠密地图对于导航任务提供了很多冗余信息，而且稠密地图的构建、优化、存储与更新是具有挑战性的。

发明内容

针对现有技术存在的问题，本发明提出一种集成稀疏点云的移动机器人拓扑经验地图构建方法，基于稀疏点云进行定位，利用相机采集的外部环境的视觉图像信息重建环境的稀疏点云，在线增量学习并构建拓扑经验地图用于路径规划与导航。

本发明所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，该方法根据稀疏点云获取的关键帧数据与相机采集的视觉图像增量式构建可直接用于移动机器人的路径规划与导航的拓扑经验地图。

优选的，具体按照以下步骤进行：

(1)构建稀疏点云；

(2)拓扑经验地图建模；拓扑经验地图建模为经验节点集合与节点转移网络两者的集合；

(3)构建拓扑经验地图；拓扑经验地图的构建基于稀疏点云进行定位，根据相机位姿计算移动机器人位姿，采集视觉经验，封装成经验信息的节点三元组；最后将移动机器人位姿和经验信息应用到拓扑经验地图建模中完成拓扑经验地图的构建。

其中，优选的，步骤(1)中的构建稀疏点云包含前端跟踪与后端优化两部分；所述前端跟踪采用基于特征点法的视觉前端对每帧图像提取特征点，然后根据特征匹配关系求解当前帧位姿的初始估计并对初始位姿评估进行调优；所述后端优化首先加入新的关键帧，优化更新局部地图，并对每一个关键帧做闭环检测处理，一旦检测到闭环，则使用全局位姿图优化纠正前端视觉里程计的累积误差。

优选的，步骤(2)中的拓扑经验地图G建模为经验节点集合E与节点转移网络W两者的集合：

G＝{E,W}

其中，经验节点集合E的序列根据移动机器人的位姿与当前场景的经验信息构建，具体为：

E＝{e₁,e₂,…,e_k-1,e_k}

每个节点e由三元组构成：

e＝(id,pose,exp)

上式中，id为节点编号；pose为移动机器人在世界坐标系下的位姿pos^k＝(x^k,y^k,yaw^k)，包含移动机器人在世界坐标系的二维坐标与航向角；exp为经验信息，即视觉特征，具有可观测特性，可以在线学习与更新。

节点转移网络W由节点间的权值组成，任意权值w_ij表示移动机器人从节点i到节点j的转移代价，由节点间的欧式距离与其视觉经验的匹配度两部分计算得到。

优选的，步骤(3)中的移动机器人位姿具体实现过程如下：

首先，根据手眼标定获取相机坐标系到移动机器人坐标系的变换矩阵T_RC：

其中，R是旋转矩阵，维度为3×3，t是平移向量，维度为3×1，0^T为3×1的零向量；

然后，利用相机采集图像信息，根据提取的视觉特征与稀疏点云匹配计算得到相机位姿X^c：

X^c＝[timestamp,x,y,z,qx,qy,qz,qw]

其中，timestamp为时间戳，x，y，z为相机空间位置，qx，qy，qz，qw为旋转四元数。

最后，根据变换矩阵和相机位姿计算移动机器人位姿：

X^R＝T_RC·X^C

其中，X^R为移动机器人位姿。

优选的，步骤(3)中的经验信息具体实现过程如下：为了实时且高效的应用经验信息，引入BOW词袋模型，首先应用大量相似场景图片信息，提取描述子，使用聚类算法将差异较小的描述子归为一个单词，最终获得由大量单词构成的字典，在每个经验节点提取固定数量的描述子后，每个描述子在字典中找到对应的单词并计算单词的TF-IDF(TermFrequency-Inverse Document Frequency，译频率-逆文档频率)权重，最终用单词向量作为经验信息存储在节点三元组中：

其中，ω_i表示第i个单词，η_i为单词ω_i的TF-IDF权重，v_i为图像的词袋向量描述。

优选的，所述的相机为视觉传感器Kinect2。

本发明的优点在于：解决稀疏点云地图用于自主导航任务的信息不充分问题，通过稀疏点云在线构建环境的拓扑经验地图，拓扑经验地图可以直接用于移动机器人导航。

附图说明

图1为本发明实施例1中所述的构建稀疏点云中的前端跟踪流程；

图2为本发明实施例1中拓扑经验地图构建过程示意图；

图3为本发明实施例1中“相机-移动机器人”坐标关系示意图；

图4为本发明实施例1中构建的关键帧与拓扑经验地图。

具体实施方式

为使本发明实施的目的、技术方案和优点更加清楚，下面将结合本发明实例中的附图，对本发明实例中的技术方案进行清楚、完整的描述，显然，所描述的实例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1：

一种集成稀疏点云的移动机器人拓扑经验地图构建方法，按照以下步骤进行：

(1)构建稀疏点云；

图1为本实施例构建稀疏点云中的前端跟踪流程，如图1所示，视觉前端跟踪算法作为视觉里程计实时跟踪相机位姿并决定当前帧是否作为关键帧。首先，先进行特征检测，对每一帧图像提取FAST关键点和BRIEF描述符，帧间特征匹配得到3D-2D匹配点集，使用RANSAC算法优化匹配关系，利用PnP算法求解当前帧位姿。然后，根据相机投影约束使用motion-only BA算法位姿调优，即固定地图点仅优化当前帧位姿。如果跟踪失败，则进行重定位。前端跟踪过程中，移动机器人长时间根据视觉里程计估计位姿会出现累积误差，为得到更加准确的位姿估计，在后端优化中，根据局部地图不断优化局部相机位姿与空间地图点位置，同时对每一帧图像做闭环检测处理，一旦检测到闭环则进行全局位姿图优化，从而消除大尺度环境下的累积误差。

图2为本发明实例拓扑经验地图构建过程示意图，如图2所示，拓扑经验地图的构建基于稀疏点云进行定位，根据相机位姿计算移动机器人位姿，采集视觉经验，封装成经验信息的节点三元组。具体过程如下：

首先，根据手眼标定获取相机坐标系到移动机器人坐标系的变换矩阵T_RC，移动机器人与相机的位姿关系如图3所示，其包含旋转R和平移t两部分：

其中，R是旋转矩阵，维度为3×3，t是平移向量，维度为3×1，0^T为3×1的零向量。

然后，利用视觉传感器Kinect2采集图像信息，根据特征匹配进行相机定位得到相机位姿。

X^c＝[timestamp,x,y,z,qx,qy,qz,qw]

最后，根据外参矩阵计算移动机器人位姿。

X^R＝T_RC·X^C

其中，X^R为移动机器人位姿。

为了实时且高效的应用经验信息，引入BOW词袋模型，首先应用大量相似场景图片信息，提取描述子，使用聚类算法将差异较小的描述子归为一个单词。最终获得由大量单词构成的字典。在每个经验节点提取固定数量的描述子后，每个描述子在字典中找到对应的单词并计算单词的TF-IDF(Term Frequency-Inverse Document Frequency，译频率-逆文档频率)权重，最终用单词向量作为经验信息存储在节点三元组中：

由上述获得的机器人位姿和经验信息，构建拓扑经验地图为经验节点与节点转移网络G＝{E,W}。根据移动机器人的位姿与当前场景的视觉信息构建序列经验节点E的集合。

E＝{e₁,e₂,…,e_k-1,e_k}

其中，每个节点e由三元组构成：e＝(id,pose,exp)。各元素的含义如下：id:节点编号；pos:机器人在世界坐标系下的位姿，pos^k＝(x^k,y^k,yaw^k)，包含机器人在世界坐标系的二维坐标与航向角；exp:经验信息，即视觉特征，具有可观测特性，可以在线学习与更新。

节点转移网络W由节点间的权值组成，任意权值w_ij表示移动机器人从节点i到节点j的转移代价，连接权值由节点间的欧式距离与其视觉经验的匹配度两部分计算得到。

图4为本发明实施例中构建的关键帧数据与拓扑经验地图，如图4所示，实验平台为移动机器人搭载视觉传感器Kinect2，实验场合为典型的办公室环境。移动机器人根据视觉信息获取环境序列图像，利用特征点法的前端与基于优化的后端重建环境的显著视觉路标点，实现稀疏点云构建并保存，关键帧的位姿由基于RANSAC优化的PnP算法计算得到，记录关键帧数据，如图4(a)所示，其中每个点记录关键帧相机的位姿信息：X^c＝[timestamp,x,y,z,qx,qy,qz,qw]。根据“相机-机器人”坐标关系，计算得到移动机器人位姿：(x,y,yaw)。利用词袋模型获取当前帧图像的经验信息exp，封装成节点三元组，构建经验节点，根据节点间的欧式距离与视觉经验的匹配度计算节点转移权值，由此实现拓扑经验地图的构建，如图4(b)所示，图中每一个圆圈表示一个经验节点，包含三元组信息。根据节点三元组信息与节点转移网络，移动机器人可以规划路径并实现导航。

最后应说明的是：以上实例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照本实施实例对本发明进行了详细的说明，本领域的普通人员应当理解；其依然可以对前述各实例所记载的技术方案进行修改，或者对其中部分技术特征进行同等替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实例技术方案的精神和范围。

Claims

1.一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，该方法根据稀疏点云获取的关键帧数据与相机采集的视觉图像增量式构建可直接用于移动机器人的路径规划与导航的拓扑经验地图。

2.根据权利要求1所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，按照以下步骤进行：

(1)构建稀疏点云；

3.根据权利要求2所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，步骤(1)中的构建稀疏点云包含前端跟踪与后端优化两部分；所述前端跟踪采用基于特征点法的视觉前端对每帧图像提取特征点，然后根据特征匹配关系求解当前帧位姿的初始估计并对初始位姿评估进行调优；所述后端优化首先加入新的关键帧，优化更新局部地图，并对每一个关键帧做闭环检测处理，一旦检测到闭环，则使用全局位姿图优化纠正前端视觉里程计的累积误差。

4.根据权利要求3所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，步骤(2)中的拓扑经验地图G建模为经验节点集合E与节点转移网络W两者的集合：

G＝{E,W}

E＝{e₁,e₂,…,e_k-1,e_k}

每个节点e由三元组构成：

e＝(id,pose,exp)

上式中，id为节点编号；pose为移动机器人在世界坐标系下的位姿pos^k＝(x^k,y^k,yaw^k)，包含移动机器人在世界坐标系的二维坐标与航向角；exp为经验信息；

5.根据权利要求4所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，步骤(3)中的移动机器人位姿具体实现过程如下：

X^c＝[timestamp,x,y,z,qx,qy,qz,qw]

最后，根据变换矩阵和相机位姿计算移动机器人位姿：

X^R＝T_RC·X^C

其中，X^R为移动机器人位姿。

6.根据权利要求5所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，步骤(3)中的经验信息具体实现过程如下：引入BOW词袋模型，首先应用大量相似场景图片信息，提取描述子，使用聚类算法将差异较小的描述子归为一个单词，最终获得由大量单词构成的字典，在每个经验节点提取固定数量的描述子后，每个描述子在字典中找到对应的单词并计算单词的TF-IDF权重，最终用单词向量作为经验信息存储在节点三元组中：

7.根据权利要求1、2、3、4、5或6所述的一种集成稀疏点云的移动机器人拓扑经验地图构建方法，其特征在于，所述的相机为视觉传感器Kinect2。