CN107160395B - 地图构建方法及机器人控制系统 - Google Patents

Abstract

本发明实施例公开一种地图构建方法及机器人控制系统，能够更易于实现地图构建。该方法包括提取当前帧的特征点；获取参考帧的特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。

Description

地图构建方法及机器人控制系统

技术领域

本发明涉及计算机领域，尤其涉及一种地图构建方法及机器人控制系统。

背景技术

在移动机器人领域，SLAM(Simultaneous Localization And Mapping是一个 非常基础而关键的问题，是机器人学领域最热门的研究方向之一，在过去的近 30年中得到了广泛的关注与研究。SLAM研究领域的学者们提出了大量的 SLAM方法，包括各种各样的传感器、优化技术和地图描述。获得机器人工作 空间的地图是实现定位、避障、导航以及自主性任务的关键问题，尤其当移动 机器人工作在复杂、动态环境中，仅仅使用在板传感器快速地生成并维护工作 空间的3维地图是非常重要的。激光扫描仪能够以高频率提供精确的环境的几 何测量，许多先进的SLAM系统结合各种改进的ICP(Iterative Closest Point) 算法，计算观测之间的相对运动。

在移动机器人上配备机械臂可以通过2维激光扫描仪实现精确的3维定 位，结合IMU能够在移动机器人平台上实现地图构建，通过两个激光扫描仪 和ICP算法能够以毫米级的精度估计机器人位姿。但是，激光扫描仪存在笨重、 昂贵和获取信息量少的问题，此外，ICP的缺陷包括依赖于良好的初始化估计 以防止局部最小值和缺乏整体匹配质量的衡量。

激光扫描仪存在的主要不足是价格高、耗能大和体积重量大，如何通过更 易于实现的方式实现地图构建，是当前需要解决的技术问题。

发明内容

本发明提供一种地图构建方法及机器人控制系统，能够更易于实现地图构 建。

本发明实施例采用如下技术方案：

一种地图构建方法，包括：

提取当前帧的特征点；

获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点， 得出当前位姿；

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特 征点投影到所述当前帧；

当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。

可选的，所述提取当前帧的特征点包括：

对所述当前帧采用ORB算法检测特征点并计算特征点的方向，通过随机 选取特征点邻域内的像素点对进行比较获得所述特征点二进制描述符；

所述根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿包 括：

根据运动模型预估所述参考帧的特征点在所述当前帧的区域内利用所述 二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，通过帧间 匹配计算出所述当前位姿。

可选的，还包括：所述局部地图构建过程中检测冗余的关键帧并删除。

可选的，还包括：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环 关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环 关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点。

可选的，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧 作为候选回环关键帧包括：

根据局部地图优化所述关键帧K_i的位姿，检测新插入的所述关键帧K_i与其 它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧K_i和互可见图 中相邻的关键帧之间的相似性，记录最低得分s_min，使用同样的方法计算所述关 键帧K_i与地图中其它关键帧之间的相似性，忽略所有得分低于s_min的关键帧，在 某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作 为候选回环关键帧；

所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和 闭环关键帧，包括：

检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似 变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧 地图点之间的对应关系，得到一组3D到3D的对应点，采用RANSAC迭代计 算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧；

所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合 重复的地图点，包括：

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点，更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键 帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成 回环，消除回环中的累积误差。

可选的，还包括：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图。

可选的，所述根据所述关键帧，生成点云地图包括：

确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：

d＝z·s

其中，f_x、f_y、c_x和c_y是摄像机内部参数，通过校准摄像机校准获得，s表 示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置， 采用矩阵的表达方式：

其中，C为摄像机内参矩阵，R和t分别表示摄像机的旋转和位移。

可选的，所述根据所述点云地图构建3D环境的八叉树地图，包括：

八叉树中的叶子，用概率的形式表示是否被占据，对于叶子节点n，在1,2,…t 时刻的观测数据为z₁,z₂,...z_t，则该节点记录的信息：

其中，P(n)一般情况下假设为0.5，此处引入logit变换：

两边取logit变换，代入P(n)＝0.5，化简后得：

L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)

对于任意空间中任意一点，每次获得新的观测能够直接加到原来的上面， 因此更新方式灵活，父节点可以根据子节点的数值计算其占据概率，主要有取 平均值

和取最大值

两种方式，因此基于八叉树地图库 能够在生成点云图的过程中，实时的构建3D环境的八叉树地图。

一种机器人控制系统，包括：传感器、计算机、控制板、机器人；

所述传感器获取距离参数；

所述控制板用于控制所述机器人；

所述计算机用于执行：

提取当前帧的特征点；

获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点， 得出当前位姿；

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特 征点投影到所述当前帧；

当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。

可选的，所述计算机还用于执行：

所述局部地图构建过程中检测冗余的关键帧并删除；

所述计算机还用于执行：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环 关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环 关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点；

所述计算机还用于执行：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图。

本发明实施例提供的地图构建方法及机器人控制系统，提取当前帧的特征 点；获取参考帧的特征点，根据当前帧的特征点及参考帧的特征点，得出当前 位姿；通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的 特征点投影到当前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键 帧。从而能够更易于实现地图构建。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明 的实施例，并与说明书一起解释本发明的原理。

图1为本发明实施例提供的一种地图构建方法的流程图。

图2为本发明实施例提供的一种机器人控制系统的结构示意图。

图3为本发明实施例提供的一种机器人控制系统的中传感器成像示意图。

图4为本发明实施例提供的一种机器人控制系统的控制板示意图。

具体实施方式

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

实施例1

本发明实施例提供一种地图构建方法，如图1所示，该方法包括：

11、提取当前帧的特征点；

12、获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征 点，得出当前位姿；

13、通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧 的特征点投影到所述当前帧；

14、当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。

可选的，所述提取当前帧的特征点包括：

对所述当前帧采用ORB(Oriented FAST and Rotated BRIEF)算法检测特 征点并计算特征点的方向，通过随机选取特征点邻域内的像素点对进行比较获 得所述特征点二进制描述符。具体的，对当前帧采用ORB算法检测特征点并 计算特征点的方向，方向信息在关键点特征描述符的匹配中非常有用，通过随 机选取特征点邻域内的像素点对进行比较获得该特征点二进制描述符。视觉特 征点的提取和描述基于OpenCV库实现，其中每一帧提取1000个特征点，尺 度因子为1.2，尺度分层数为8。

所述根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿包 括：

根据运动模型预估所述参考帧的特征点在所述当前帧的区域内利用所述 二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，通过帧间 匹配计算出所述当前位姿。

具体的，当前帧的特征点与参考帧的特征点存在对应关系，根据运动模型 预估参考帧的特征点在当前帧的区域，在该区域内利用上一步骤计算的二进制 描述符搜索匹配点，并通过方向一致性校验优化初始对应点。至此，获得一系 列3D到2D的对应关系，在RANSAC方案中解决PnP(Perspective-n-Point) 问题，通过帧间匹配计算出摄像机的当前位姿。

可选的，所述通过帧间配准获取的位姿估计存在累积误差，将局部地图中 的关键帧的特征点投影到所述当前帧包括：

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特 征点投影到当前帧，构造优化特征估计位姿的残差。优化后得到的特征点的位 置，比之前通过摄像机位姿估计的更精确，由此可优化地图中的特征点位置和 摄像机的位姿。

可选的，还包括：所述局部地图构建过程中检测冗余的关键帧并删除。

具体的，精确的局部地图有利于优化帧间匹配的累积误差，当前帧符合新 增关键帧的条件，则向局部地图插入关键帧并采用视觉词袋模型进行描述。检 查新插入的关键帧中的地图点在相关的关键帧中的重投影误差，忽略不能满足 极线约束的错误匹配。剔除不符合要求的地图点以提高算法的鲁棒性，减少存 在误差的地图点对位姿估计的优化造成不利影响。采用局部捆集调整(Bundle Adjustment，BA)优化新的局部地图，包括新插入的关键帧及其相关联的关键 帧，以及这些关键帧中的全部地图点。

由于BA的复杂度是关键帧数量的三次方，局部地图构建过程中检测冗余 的关键帧并删除。在相同的场景下，关键帧的数量不会随着摄像机的重复运动 而无限制的增加。关键帧的增加仅在摄像机进入新的场景时而产生，有利于长 时间的构图，删除冗余关键帧能够有效提升算法的实时性又能保持鲁棒性。

可选的，还包括：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环 关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环 关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点。

可选的，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧 作为候选回环关键帧包括：

根据局部地图优化所述关键帧K_i的位姿，检测新插入的所述关键帧K_i与其 它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧K_i和互可见图 中相邻的关键帧之间的相似性，记录最低得分s_min，使用同样的方法计算所述关 键帧K_i与地图中其它关键帧之间的相似性，忽略所有得分低于s_min的关键帧，在 某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作 为候选回环关键帧。

具体的，根据局部地图优化关键帧K_i的位姿，尤其是大尺度范围中，仍存 在误差累积问题。检测新插入的关键帧K_i与其它关键帧是否构成回路闭合，二 者位姿偏差即为累积的误差，进而优化闭合回路中的所有关键帧。首先，基于 视觉词袋模型计算K_i和互可见图中相邻的关键帧之间的相似性，记录最低得分 s_min。然后，使用同样的方法计算K_i与地图中其它关键帧之间的相似性，忽略所 有得分低于s_min的关键帧。最后，在某个关键帧的互可见图中连续检测出3个候 选回环关键帧，即确定该关键帧作为候选回环关键帧。

所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和 闭环关键帧，包括：

检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似 变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧 地图点之间的对应关系，得到一组3D到3D的对应点，采用RANSAC迭代计 算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧。

具体的，检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之 间的相似变换，该相似变换即回环中的累积误差。计算当前关键帧和候选回环 关键帧地图点之间的对应关系，得到一组3D到3D的对应点，采用RANSAC 迭代计算相似变换。如果拥有足够的内点，那么就能确定闭环检测和闭环关键 帧。

所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合 重复的地图点，包括：

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点，更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键 帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成 回环，消除回环中的累积误差。

具体的，根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环， 融合重复的地图点。更正当前关键帧的位姿，进而将更正传递到闭合环路中的 所有关键帧以及地图点。所有的关键帧都将产生新的边以将重新优化后的关键 帧连接成回环，消除了回环中的累积误差。

可选的，还包括：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图。

可选的，所述根据所述关键帧，生成点云地图包括：

确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：

d＝z·s

其中，f_x、f_y、c_x和c_y是摄像机内部参数，通过校准摄像机校准获得，s表 示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置， 采用矩阵的表达方式：

其中，C为摄像机内参矩阵，R和t分别表示摄像机的旋转和位移。

具体的，上述稀疏地图主要为提升定位的精度和鲁棒性服务，而不适合于 避障等应用，因此在高精度和高鲁棒性的关键帧的基础之上，采用点云图和八 叉树地图技术，构建适合机器人导航等应用的3D网格地图。首先，3D环境可 由一系列点描述：X＝{x₁,x₂,……,x_n}，其中x_i＝[r,g,b,x,y,z]，分别表示该点的颜色和 空间位置共6个元素。对于RGB-D传感器而言，颜色信息存储于彩色图像， 绝对尺度的空间位置可以通过针孔摄像机模型和深度图像的数据推算获得。

如图2所示的针孔摄像机模型，根据几何关系可得空间位置[x,y,z]和像素 坐标[u,v,d]之间的对应关系：

d＝z·s

其中，f_x、f_y、c_x和c_y是摄像机内部参数，通过校准摄像机校准获得，s表 示深度缩放因子，d表示图像深度。那么，可以推导出像素在空间中的位置， 采用矩阵的表达方式：

其中，C为摄像机内参矩阵，R和t分别表示摄像机的旋转和位移。

可选的，所述根据所述点云地图构建3D环境的八叉树地图，包括：

八叉树地图能够描述任意形式的3D环境，能够以概率的形式更新，具有 更新方式灵活、无需先验知识和地图形式非常紧凑等优点。八叉树中的叶子， 用概率的形式表示是否被占据，对于叶子节点n，在1,2,…t时刻的观测数据为z₁,z₂,...z_t，则该节点记录的信息：

其中，P(n)一般情况下假设为0.5，此处引入logit变换：

两边取logit变换，代入P(n)＝0.5，化简后得：

L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)

由上式可知，对于任意空间中任意一点，每次获得新的观测能够直接加到 原来的上面，因此更新方式灵活。父节点可以根据子节点的数值计算其占据概 率，主要有取平均值

和取最大值

两种方式，因此基于 八叉树地图库能够在生成点云图的过程中，实时的构建3D环境的八叉树地图。

本发明实施例提供的地图构建方法，提取当前帧的特征点；获取参考帧的 特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间配 准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当前 帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。从而能够更易 于实现地图构建。

实施例2

本发明实施例提供一种机器人控制系统，该机器人控制系统可以应用上述 实施例1提供的方法。如图3所示，该系统包括：传感器21、计算机22、控 制板23、机器人24；

所述传感器21获取距离参数；

所述控制板23用于控制所述机器人；

所述计算机22用于执行：

提取当前帧的特征点；

获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点， 得出当前位姿；

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特 征点投影到所述当前帧；

当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧。

可选的，所述计算机22还用于执行：

所述局部地图构建过程中检测冗余的关键帧并删除；

所述计算机还22用于执行：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环 关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环 关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复 的地图点；

所述计算机22还用于执行：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图。

本发明实施例的计算机22的详细功能可以参照上述实施例1，此处不赘 述。

在一个实施例中，该机器人控制系统可以采用如下平台硬件设计：

传感器21采用硬件Kinect2.0、计算机22采用微型计算机(例如i7处理 器的微型计算机。)、控制板23采用PIXHAWK、机器人23为移动机器人，移 动机器人平台基座为AxialAX10。

可选的，移动机器人平台的基座为AX-10攀爬车的底座，具备多功能爬行 能力和灵活的速度控制特性，由AE-2ESC和20T电机提供动力，AS-3金属齿 轮伺服系统安全且精确的控制。移动机器人平台底座见相关参数如下表：

规格	数值
		轴距(mm)	307
长度(mm)	467.4
		宽度(mm)	295.3
高度(mm)	238
		离地间隙(mm)	76.2
重量(kg)	1.9

可选的，微软的Kinect 2.0传感器提供了一个基于在板的ToF距离传感器。Kinect 2.0由多个传感器组成，包括RGB数字摄像机、麦克风和3D ToF距离 摄像机传感器，能够提供深度图像，相关参数如下表所示：

规格	数值
		RGB摄像机像素精度	1920×1080
ToF摄像机像素精度	512×424
		帧速率	30FPS
ToF传感器深度距离(m)	0.5-4.5
		传感器尺寸(cm)	25×6.5×6.5

可选的，主控计算机是移动机器人平台视觉定位与地图构建算法以及自主 控制的核心，它主要处理视觉传感器的数据并对整个平台进行控制，要求处理 能力强和运行速度快。龙浩微动研发的高性能、低功耗、低辐射、全功能单板 计算机，其硬件参数和处理运行速度能够满足移动机器人实时运行的要求，且 能完成定位、地图构建、障碍物检测和路径规划等高级任务的计算需求，同时 具有足够的存储空间来记录数据。下表为本文主控计算机的配置硬件参数，主 控计算机通过USB 3.0接口与Kinect 2.0连接，通过USB2.0接口与PIXHAWK 建立连接，在板主控计算机搭载Ubuntu 14.04LTS操作系统、indigo版的机器人操作系统(ROS)和OpenCV(2.4.11)图像处理库。

可选的，Pixhawk控制板控制部分是执行命令和驱动控制的枢纽，主控计 算机接收Kinect 2.0传感器数据，通过VSLAM算法构建环境的3D地图同时 估计机器人平台的当前位姿估计。Pixhawk控制板是一个高性能的自动驾驶模 块，兼容固定翼飞行器、多旋翼飞行器、直升机、车辆和船舶等移动机器人平 台。Pixhawk控制板是一款开源的微控制平台，装配了具有强处理能力的 STM32F427Cortex M4168MHz微处理器，RAM为256KB，flash存储为2MB。 除了主要的CPU外，还有一个STM32F103处理器，避免主处理器失效时机器 人平台的失控。为了拓展自主能力，PIXHAWK控制板提供了多个接口来连接 外部传感器，如图4所示。Pixhawk主要参数如下

本发明实施例提供的机器人控制系统，提取当前帧的特征点；获取参考帧 的特征点，根据当前帧的特征点及参考帧的特征点，得出当前位姿；通过帧间 配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到当 前帧；当前帧符合新增关键帧的条件时，向局部地图插入关键帧。从而能够更 易于实现地图构建。

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限，任何 熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替 换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要 求的保护范围为准。

Claims (5)

1.一种地图构建方法，其特征在于，包括：

提取当前帧的特征点；

获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；

当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧；

所述方法还包括：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点；

其中，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧包括：

根据局部地图优化所述关键帧K_i的位姿，检测新插入的所述关键帧K_i与其它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧K_i和互可见图中相邻的关键帧之间的相似性，记录最低得分s_min，使用同样的方法计算所述关键帧K_i与地图中其它关键帧之间的相似性，忽略所有得分低于s_min的关键帧，在某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作为候选回环关键帧；

所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧，包括：

检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧地图点之间的对应关系，得到一组3D到3D的对应点，采用RANSAC迭代计算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧；

所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，包括：

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，更正当前关键帧的位姿，将更正传递到闭合环路中的所有关键帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成回环，消除回环中的累积误差；

所述方法还包括：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图；

其中，所述根据所述关键帧，生成点云地图包括：

确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：

d＝z·s

其中，f_x、f_y、c_x和c_y是摄像机内部参数，通过校准摄像机校准获得，s表示深度缩放因子，d表示图像深度，得出像素在空间中的位置，采用矩阵的表达方式：

其中，C为摄像机内参矩阵，R和t分别表示摄像机的旋转和位移；

其中，所述根据所述点云地图构建3D环境的八叉树地图，包括：

八叉树中的叶子，用概率的形式表示是否被占据，对于叶子节点n，在

时刻的观测数据为z₁,z₂,L z_t，则该节点记录的信息：

其中，P(n)为0.5，此处引入logit变换：

两边取logit变换，代入P(n)＝0.5，化简后得：

L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)

对于任意空间中任意一点，每次获得新的观测直接加到原来的上面，父节点根据子节点的数值计算其占据概率，有取平均值

和取最大值

两种方式，基于八叉树地图库在生成点云图的过程中，实时构建3D环境的八叉树地图。

2.根据权利要求1所述的方法，其特征在于，所述提取当前帧的特征点包括：

对所述当前帧采用ORB算法检测特征点并计算特征点的方向，通过随机选取特征点邻域内的像素点对进行比较获得所述特征点二进制描述符；

所述根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿包括：

根据运动模型预估所述参考帧的特征点在所述当前帧的区域内利用所述二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，通过帧间匹配计算出所述当前位姿，具体的，当前帧的特征点与参考帧的特征点存在对应关系，根据运动模型预估参考帧的特征点在当前帧的区域，在该区域内利用上一步骤计算的二进制描述符搜索匹配点，并通过方向一致性校验优化初始对应点，获得一系列3D到2D的对应关系，通过帧间匹配计算出摄像机的当前位姿。

3.根据权利要求1或2所述的方法，其特征在于，还包括：所述局部地图构建过程中检测冗余的关键帧并删除；

当前帧符合新增关键帧的条件，则向局部地图插入关键帧并采用视觉词袋模型进行描述；检查新插入的关键帧中的地图点在相关的关键帧中的重投影误差，忽略不能满足极线约束的错误匹配，剔除不符合要求的地图点以提高算法的鲁棒性，采用局部捆集调整BA优化新的局部地图，包括新插入的关键帧及其相关联的关键帧，以及这些关键帧中的全部地图点。

4.一种机器人控制系统，其特征在于，包括：传感器、计算机、控制板、机器人；

所述传感器获取距离参数；

所述控制板用于控制所述机器人；

所述计算机用于执行：

提取当前帧的特征点；

获取参考帧的特征点，根据所述当前帧的特征点及所述参考帧的特征点，得出当前位姿；

通过帧间配准获取的位姿估计存在累积误差，将局部地图中的关键帧的特征点投影到所述当前帧；

当所述前帧符合新增关键帧的条件时，向所述局部地图插入关键帧；

所述计算机用于执行：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点；

其中，所述根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧包括：

根据局部地图优化所述关键帧K_i的位姿，检测新插入的所述关键帧K_i与其它关键帧是否构成回路闭合；基于视觉词袋模型计算所述关键帧K_i和互可见图中相邻的关键帧之间的相似性，记录最低得分s_min，使用同样的方法计算所述关键帧K_i与地图中其它关键帧之间的相似性，忽略所有得分低于s_min的关键帧，在某个关键帧的互可见图中连续检测出3个候选回环关键帧，即确定该关键帧作为候选回环关键帧；

所述计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧，包括：

检测出候选闭环关键帧后，计算新插入的关键帧与闭环关键帧之间的相似变换，所述相似变换即回环中的累积误差，计算当前关键帧和候选回环关键帧地图点之间的对应关系，得到一组3D到3D的对应点，采用RANSAC迭代计算相似变换，达到拥有设定数量的内点，则确定闭环检测和闭环关键帧；

所述根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，包括：

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点，更正当前关键帧的位姿，进而将更正传递到闭合环路中的所有关键帧以及地图点，所有的关键帧都将产生新的边以将重新优化后的关键帧连接成回环，消除回环中的累积误差；

所述计算机用于执行：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图；

其中，所述根据所述关键帧，生成点云地图包括：

确定空间位置[x,y,z]和像素坐标[u,v,d]之间的对应关系：

d＝z·s

其中，f_x、f_y、c_x和c_y是摄像机内部参数，通过校准摄像机校准获得，s表示深度缩放因子，d表示图像深度，得出像素在空间中的位置，采用矩阵的表达方式：

其中，C为摄像机内参矩阵，R和t分别表示摄像机的旋转和位移；

其中，所述根据所述点云地图构建3D环境的八叉树地图，包括：

八叉树中的叶子，用概率的形式表示是否被占据，对于叶子节点n，在2

时刻的观测数据为z₁,z₂,L z_t，则该节点记录的信息：

其中，P(n)为0.5，此处引入logit变换：

两边取logit变换，代入P(n)＝0.5，化简后得：

L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)

对于任意空间中任意一点，每次获得新的观测直接加到原来的上面，父节点根据子节点的数值计算其占据概率，有取平均值

和取最大值

两种方式，基于八叉树地图库在生成点云图的过程中，实时的构建3D环境的八叉树地图。

5.根据权利要求4所述的系统，其特征在于，所述计算机还用于执行：

所述局部地图构建过程中检测冗余的关键帧并删除；

所述计算机还用于执行：

根据所述局部地图优化所述关键帧的位姿，确定所述关键帧作为候选回环关键帧；

计算新插入的关键帧与闭环关键帧之间的相似变换，确定闭环检测和闭环关键帧；

根据确定的闭环关键帧，在互可见图中插入新的边以连接闭环，融合重复的地图点；

所述计算机还用于执行：

根据所述关键帧，生成点云地图；

根据所述点云地图构建3D环境的八叉树地图。

Images