[发明专利]三维激光建图方法及系统

说明书

本发明涉及一种三维激光建图方法及系统,包括：利用置放在机器人上的传感器获取数据，并滤除不合格的数据；利用由粗到精的匹配算法计算所述机器人的位姿；根据所述机器人的位姿构建新的图像，然后根据环路闭合检测结果修改地图。本发明可以生成一致性较高的场景地图，算法简单，精确度高。

技术领域

本发明涉及计算机图像处理的技术领域，尤其是指一种三维激光建图方法。

背景技术

在大规模室外环境下，采用激光测距传感器创建环境地图相对于室内更为复杂。一般来说，室外环境由于范围大而特征少，因此传感器能够感知的信息有限，这对机器人自主建图提出了新的挑战。现阶段，一般采用的是三维激光SLAM技术，该技术通常采用点云地图作为存储格式，然而随着场景范围的扩大，点云地图势必导致内存的激增，所以需要寻找更为合适的数据结构；另外，当机器人经过漫长的路径重新回到以前经过的地点时，由于数据关联的累积误差不可避免，势必会产生不一致的地图。当场景范围增大，以及数据量激增时，对闭环检测和位姿优化都提出了新的挑战。

发明内容

为此，本发明所要解决的技术问题在于克服现有技术中定位精度低，方法复杂的问题，从而提供一种定位精度高，方法简单的三维激光建图方法。

为解决上述技术问题，本发明的一种三维激光建图方法，包括如下步骤：利用置放在机器人上的传感器获取数据，并滤除不合格的数据；利用由粗到精的匹配算法计算所述机器人的位姿，所述由粗到精的匹配算法包括：对Q_k采用K-D树描述，对中的每个激光点寻找在Q_k中的最近点，建立配对；建立非线性方程，目标使得所有配对的距离最小；利用L-M法求解非线性优化问题，求解T_opt＝(R,t)；则k+1时刻的机器人位姿为其中Q_k表示k时刻建立的点云地图，表示k+1时刻当前帧转换到世界坐标系下的点云地图，表示k时刻机器人在世界坐标系下的位姿，表示k+1时刻机器人的位姿增量，t是平移位置，R是旋转角度；根据所述机器人的位姿构建新的图像，然后根据环路闭合检测结果修改地图。

在本发明的一个实施例中，所述根据环路闭合检测结果修改地图的方法为：先增加新的图像，再判断环路是否闭合，若闭合，则对地图进行优化；否则，更新地图。

在本发明的一个实施例中，所述对地图进行优化后，将地图添加到全局地图中，直至全局地图建立完成。

在本发明的一个实施例中，所述对地图进行优化的方法为：首先根据所述机器人行驶路径的长度进行离散化；对所述机器人的初始数据进行初始化，利用由粗到精的匹配算法计算并输出所述机器人的位姿；对所述机器人的位姿、关键序列帧差值、两帧对应的机器人的位姿的差值、地图环境变化的匹配度分别进行判定，若满足条件，则认为检测到正确的闭环，并对两帧之间所有的点云地图进行调整，将地图添加到全局地图中；若不满足条件，则更新地图。

在本发明的一个实施例中，所述对机器人的位姿、当前关键帧和检索到的关键帧的序列号的差值、两帧对应的机器人的位姿的差值、地图环境变化的匹配度进行判定的方法为：计算当前距离，如果当前距离大于第一阀值，则继续下一步，否则，下一时刻继续检测；判断当前关键帧和检索到的关键帧的序列号的差值是否满足预设的第二阈值，若成立，则继续计算下一步，否则更新地图；判断两次经过同一点时所述机器人位姿的差值是否满足预设的第三阈值，若成立则转至继续下一步，否则更新地图；将观测到的地图环境与检测到的地图环境进行匹配，若匹配满足得分，则认为检测到闭环，若否，下一时刻继续检测。

在本发明的一个实施例中，根据所述机器人的位姿构建新的图像时，采用增量式优化的方法，通过获取上一时刻所述机器人的位姿、控制量以及当前时刻的观测量计算所述计算机新的位姿和地图。

在本发明的一个实施例中，所述机器人新的位姿和地图的计算公式为：