[发明专利]基于八叉图和三维正态分布优化匹配的3D激光SLAM方法及移动机器人

说明书

本发明公开的一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，包括如下步骤：S1、基于首个3D激光帧初始化八叉图内体素的占据概率；S2、计算八叉图体素点集的三维正态分布参数；S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿T_t；S4、基于当前位姿T_t将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率。通过八叉体素地图稳定SLAM的绝对精度，避免较大的累计误差，再通过把体素进行三维正态分布化，使得精度不受限于体素尺寸，提高重复定位精度，使得系统能够以更高效率、更高稳定度和精度进行同步建图和定位。

技术领域

本发明属于机器人技术领域，更具体地，本发明涉及一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法及移动机器人。

背景技术

随着移动机器人在无人驾驶和工业仓储物流应用越来越广泛，其定位导航的SLAM技术也越发重要。现有技术主要有磁导航、视觉导航、激光SLAM导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条，一般应用于室内小范围场景。视觉导航轨迹相对自由，成本较低，在室内多使用二维码或特殊图形码等标记，要定期维护。室外视觉导航受光源和雨雪环境影响较大，可靠性较低。激光SLAM导航定位精准，路径灵活多变，适应多种现场环境，在激光雷达技术突破及成熟后，成本已经降下来，现如今成为了移动机器人主流的定位导航方式。

3D激光SLAM算法通过点云和体素地图匹配建立体素地图，首先进行了体素滤波点云预处理降低点云密度，然后其直接调用现有的3DNDT算法通过前后激光帧匹配计算运动位姿增量，之后以此位姿为初值，再调用ICP迭代最近点算法优化位姿增量。但是该方案存在如下问题：

1)调用的NDT算法要计算体素内的ND参数，参数的计算依赖点数，一般NDT用于激光帧和地图匹配，使用地图中大量的点去初始化正态分布。该方案使用激光帧和激光帧匹配，并且第一步就进行了体素滤波减少了大量点数，因此计算的正态分布参数和位姿态增量必然不准。

2)在使用NDT匹配获取初始位姿增量后，再使用ICP优化这个增量，但是不管是NDT还是ICP，都是基于激光帧对激光帧的匹配来获取位姿增量，这些方法会有较大的累计误差，导致环境建模绝对精度不足，进而影响定位精度。

发明内容

本发明提供一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，旨在提高定位精度。

本发明是这样实现的，一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，所述方法包括如下步骤：

S1、基于首个3D激光帧初始化八叉图内体素的占据概率；

S2、计算八叉图体素点集的三维正态分布参数；

S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿T_t；

S4、基于当前位姿T_t将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率。

进一步的，当前位姿T_t的获取方法具体如下：

通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(T_t)，如下所示：