基于八叉图和三维正态分布优化匹配的3D激光SLAM方法及移动机器人技术

本发明专利技术公开的一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，包括如下步骤：S1、基于首个3D激光帧初始化八叉图内体素的占据概率；S2、计算八叉图体素点集的三维正态分布参数；S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿T

【技术实现步骤摘要】
基于八叉图和三维正态分布优化匹配的3D激光SLAM方法及移动机器人


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

技术介绍

[0002]随着移动机器人在无人驾驶和工业仓储物流应用越来越广泛，其定位导航的SLAM技术也越发重要。现有技术主要有磁导航、视觉导航、激光SLAM导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条，一般应用于室内小范围场景。视觉导航轨迹相对自由，成本较低，在室内多使用二维码或特殊图形码等标记，要定期维护。室外视觉导航受光源和雨雪环境影响较大，可靠性较低。激光SLAM导航定位精准，路径灵活多变，适应多种现场环境，在激光雷达技术突破及成熟后，成本已经降下来，现如今成为了移动机器人主流的定位导航方式。
[0003]3D激光SLAM算法通过点云和体素地图匹配建立体素地图，首先进行了体素滤波点云预处理降低点云密度，然后其直接调用现有的3DNDT算法通过前后激光帧匹配计算运动位姿增量，之后以此位姿为初值，再调用ICP迭代最近点算法优化位姿增量。但是该方案存在如下问题：
[0004]1)调用的NDT算法要计算体素内的ND参数，参数的计算依赖点数，一般NDT用于激光帧和地图匹配，使用地图中大量的点去初始化正态分布。该方案使用激光帧和激光帧匹配，并且第一步就进行了体素滤波减少了大量点数，因此计算的正态分布参数和位姿态增量必然不准。
[0005]2)在使用NDT匹配获取初始位姿增量后，再使用ICP优化这个增量，但是不管是NDT还是ICP，都是基于激光帧对激光帧的匹配来获取位姿增量，这些方法会有较大的累计误差，导致环境建模绝对精度不足，进而影响定位精度。

技术实现思路

[0006]本专利技术提供一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，旨在提高定位精度。
[0007]本专利技术是这样实现的，一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，所述方法包括如下步骤：
[0008]S1、基于首个3D激光帧初始化八叉图内体素的占据概率；
[0009]S2、计算八叉图体素点集的三维正态分布参数；
[0010]S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿T
t
；
[0011]S4、基于当前位姿T
t
将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率。
[0012]进一步的，当前位姿T
t
的获取方法具体如下：
[0013]通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(T
t
)，如下所示：
[0014][0015]其中，w1和w2分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w1+w2＝1，T
t
‑1表示上一时刻t
‑
1的世界位姿，T
t
表示当前时刻t的世界位姿，表示激光点p
i
当前时刻t在世界坐标系下的坐标，表示激光点p
i
上一时刻t
‑
1在世界坐标系下的坐标，E表示八叉图中体素内所有点的均值，即期望值；
[0016]目标函数通过经典的高斯牛顿方法迭代优化，得到t时刻的世界位姿T
t
。
[0017]进一步的，八叉图体素的占据概率函数M(p
i
)计算公式如下所示：
[0018][0019]其中，G(p
i
)表示激光点p
i
在八叉图中对应体素的整数类型坐标，logodds(G(p
i
))表示表示激光点p
i
在八叉图中对应体素的占据赢率值，M(p
i
)表示激光点p
i
在八叉图中对应体素的占据概率。
[0020]本专利技术是这样实现的，一种移动机器人，所述移动机器人包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如上所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法。
[0021]本技术方案提出了一种基于八叉图和三维正态分布优化匹配的3d激光SLAM方法，通过八叉体素地图稳定SLAM的绝对精度，避免较大的累计误差，再通过把体素进行三维正态分布化，使得精度不受限于体素尺寸，提高重复定位精度，使得系统能够以更高效率、更高稳定度和精度进行同步建图和定位。
附图说明
[0022]图1为本专利技术实施例提供的基于八叉图和三维正态分布优化匹配的3D激光SLAM方法流程图。
具体实施方式
[0023]下面对照附图，通过对实施例的描述，对本专利技术的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本专利技术的专利技术构思、技术方案有更完整、准确和深入的理解。
[0024]图1为本专利技术实施例提供的基于八叉图和三维正态分布优化匹配的3D激光SLAM方法流程图，该方法具体包括如下步骤：
[0025](1)基于首个3D激光帧初始化八叉图及八叉图中体素的占据概率；
[0026]已知3D激光帧中点p
i
在世界坐标系中的坐标通过函数G计算点p
i
在不同分辨率八叉图下的体素索引(indx,indy,indz)
resi
，这里函数G主要是除以不同分辨率，再在取整得到体素索引，即G(p
i
)表示点p
i
在八叉图中的整数型坐标八叉图中的已知点即为体素，同时初始化对应体素的logodds概率为0。计算体素的占据概率值M(p
i
)时需要将其占据赢率值logodds概率转为占据概率，八叉图体素的占据概率函数M(p
i
)计算公式如下所示：
[0027][0028]其中，G(p
i
)表示激光点p
i
在八叉图中对应体素的整数类型坐标，logodds(G(p
i
))表示表示激光点p
i
在八叉图中对应体素的占据赢率值，M(p
i
)表示激光点p
i
在八叉图中对应体素的占据概率。
[0029](2)计算八叉图体素点集的三维正态分布参数；
[0030]已知八叉图体素内的点集，当内点数量超过3个时，计算其三维正态分布参数ND(E,Cov,f(p))，参数E、Cov、f(p)分别代表期望、方差矩阵和三维正态分布概率函数，八叉图体素p
i
的三维正态分布概率函数f(p
i
)如下：
[0031][0032][00本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，所述方法包括如下步骤：S1、基于首个3D激光帧初始化八叉图内体素的占据概率；S2、计算八叉图体素点集的三维正态分布参数；S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿T
t
；S4、基于当前位姿T
t
将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率。2.如权利要求1所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，当前位姿T
t
的获取方法具体如下：通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(T
t
)，如下所示：其中，w1和w2分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w1+w2＝1，T
t
‑1表示上一时刻t
‑
1的世界位姿，T
t
表示当前时刻t的世界位姿，表示激光点p
i
当前时刻t在世界坐标系下的坐标...

【专利技术属性】
技术研发人员：郝奇，陈智君，郑亮，曹雏清，陈双，
申请(专利权)人：哈尔滨工业大学芜湖机器人产业技术研究院，
类型：发明
国别省市：