[发明专利]一种基于三维点云的机器人闭环检测方法

说明书

本发明公开了一种基于三维点云的机器人闭环检测方法，该方法使机器人具备识别曾经到达过场景的能力。首先针对三维点云中点云数据量大、分布较为密集的特点，进行点云体素滤波对点云进行降采样，在保持原有点云分布特征不变情况下，降低点云数量，提高算法运行效率。同时对降采样后点云进行PCA算法处理，确定主轴方向，解决机器人旋转不确定性对闭环检测造成影响问题。然后对PCA算法处理后点云进行降维处理，从笛卡尔坐标系转化为极坐标系，并进行栅格化处理为二维图像。最后利用数据结构Kdtree加速搜索候选帧，为减少二维图像受离散化和噪声的影响，采取在主轴方向进行局部列移动，提高闭环检测效率的同时，保证闭环检测准确。

技术领域

本发明属于机器人自主导航领域，尤其涉及一种机器人三维点云的闭环检测方法。

背景技术

闭环检测又称为回环检测，是指机器人识别曾经到达的场景，使地图闭环的能力，也就是机器人在建图定位的过程中，把当前帧的扫描和曾经构建的地图做匹配。如果回环检测成功，可以减少机器人构图累计位姿误差，错误检测会对后续后端优化构成严重影响。三维点云闭环检测比视觉闭环检测相比，稳定性更高，不受季节、时间、光照条件等因素的影响。但由于缺乏纹理和颜色信息，点云数量较多，激光闭环检测同样存在一定困难。同时即使机器人回到相同场景，由于首向旋转不确定性的影响，导致获取的三维观测点云在激光传感器坐标系下坐标值不同，也会对闭环检测带来一定难度。

发明内容

本发明的目的在于针对现有技术的不足，提出一种基于三维点云的机器人闭环检测方法。本发明通过对原始点云体素滤波进行点云降采样，在保证点云分布特征不变的基础上，减少需要处理的点云量。同时通过PCA算法对采样后点云进行处理，从而确定主轴方向，根据主轴方向进行点云变换，保证相同场景点云分布相同。对降维栅格化后的点云用数据结构Kdtree进行组织，加快候选帧的搜寻。为避免降维后的数据受噪声和离散化的影响，在主轴方向进行局部列移动，最后用移动后的距离图像与候选帧进行相似度检测。

本发明的目的是通过以下技术方案来实现的：一种基于三维点云的机器人闭环检测方法，具体为：

（1）获取原始输入点云，利用体素滤波对原始点云体素滤波进行降采样，得到降采样后的点云；所述体素滤波根据输入原始点云的范围确定三维体素网格，在该三维体素网格中的每一个体素中选取一个点作为该体素所有点云的代表；

（2）对步骤（1）得到的经过降采样后的点云通过PCA算法处理确定主轴方向，根据主轴方向对降采样后点云进行变换得到变换后的点云数据；

（3）对变换后的点云进行投影,将投影后的点云从笛卡尔坐标系转化为极坐标系，对并对坐标转换后的点云进行栅格化，得到点云栅格占据情况；根据点云栅格占据情况，生成点云降维二维图像和向量ringKey；

（4）判断累计帧数是否大于阈值；若小于阈值，则重复步骤（1）~步骤（3）直至累计帧数大于阈值；

（5）根据距离当前帧数大于阈值的所有点云帧的ringKey向量集合生成数据结构Kdtree；

（6）当前帧利用自身的向量ringKey在数据结构Kdtree中通过最近邻搜寻算法查找候选帧；

（7）将当前帧的二维图像在主轴方向进行局部列移动，将局部列移动后得到的帧与未进行局部列移动的候选帧进行相似度比较，若距离相似度小于闭环检测阈值，即为闭环。

进一步地，所述步骤（2）包括以下子步骤：

（2.1）对步骤（1）得到的经过降采样后的点云通过PCA算法处理确定主轴方向；

（2.2）将步骤（1）输出的降采样后的点云作为输入样本集，其中，降采样后的点云规模为，m为点云的个数，i表示第i个点云，对所有的输入样本中心化：

（2.3）计算输入样本集的协方差矩阵：