开云优惠体育网页版入口

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

专利类型：发明专利

专利申请号：CN202111521801.9

专利申请（专利权）人：长三角哈特机器人产业技术研究院
权利人地址：安徽省芜湖市鸠江经济技术开发区神州路17号,国家工业机器人产品质量监督检验中心园区办公楼

专利发明（设计）人：郝奇,陈智君,郑亮,曹雏清,陈双

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

主权利要求：
1.一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，所述方法包括如下步骤：S1、基于首个3D激光帧初始化八叉图内体素的占据概率；
S2、计算八叉图体素点集的三维正态分布参数；
S3、基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，进行非线性优化，获取3D激光帧的当前位姿Tt；
S4、基于当前位姿Tt将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率；
当前位姿Tt的获取方法具体如下：
通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(Tt)，如下所示：其中，w1和w2分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w1+w2＝1，Tt?1表示上一时刻t?1的世界位姿，Tt表示当前时刻t的世界位姿， 表示激光点pi当前时刻t在世界坐标系下的坐标， 表示激光点pi上一时刻t?1在世界坐标系下的坐标，E表示八叉图中体素内所有点的均值，即期望值；
目标函数通过经典的高斯牛顿方法迭代优化，得到t时刻的世界位姿Tt。
2.如权利要求1所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，八叉图体素的占据概率函数M(pi)计算公式如下所示：其中，G(pi)表示激光点pi在八叉图中对应体素的整数类型坐标，logodds(G(pi))表示表示激光点pi在八叉图中对应体素的占据赢率值，M(pi)表示激光点pi在八叉图中对应体素的占据概率。
3.一种移动机器人，其特征在于，所述移动机器人包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1?2任一权利要求所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法。 说明书 : 基于八叉图和三维正态分布优化匹配的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激光帧的当前位姿Tt；[0011] S4、基于当前位姿Tt将激光帧投影到八叉图，将激光帧中的点插入八叉图的体素点集，更新体素点集的三维正态分布参数和体素占据概率。[0012] 进一步的，当前位姿Tt的获取方法具体如下：[0013] 通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(Tt)，如下所示：[0014][0015] 其中，w1和w2分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w1+w2＝1，Tt?1表示上一时刻t?1的世界位姿，Tt表示当前时刻t的世界位姿， 表示激光点pi当前时刻t在世界坐标系下的坐标， 表示激光点pi上一时刻t?1在世界坐标系下的坐标，E表示八叉图中体素内所有点的均值，即期望值；[0016] 目标函数通过经典的高斯牛顿方法迭代优化，得到t时刻的世界位姿Tt。[0017] 进一步的，八叉图体素的占据概率函数M(pi)计算公式如下所示：[0018][0019] 其中，G(pi)表示激光点pi在八叉图中对应体素的整数类型坐标，logodds(G(pi))表示表示激光点pi在八叉图中对应体素的占据赢率值，M(pi)表示激光点pi在八叉图中对应体素的占据概率。[0020] 本发明是这样实现的，一种移动机器人，所述移动机器人包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如上所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法。[0021] 本技术方案提出了一种基于八叉图和三维正态分布优化匹配的3d激光SLAM方法，通过八叉体素地图稳定SLAM的绝对精度，避免较大的累计误差，再通过把体素进行三维正态分布化，使得精度不受限于体素尺寸，提高重复定位精度，使得系统能够以更高效率、更高稳定度和精度进行同步建图和定位。附图说明[0022] 图1为本发明实施例提供的基于八叉图和三维正态分布优化匹配的3D激光SLAM方法流程图。具体实施方式[0023] 下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。[0024] 图1为本发明实施例提供的基于八叉图和三维正态分布优化匹配的3D激光SLAM方法流程图，该方法具体包括如下步骤：[0025] (1)基于首个3D激光帧初始化八叉图及八叉图中体素的占据概率；[0026] 已知3D激光帧中点pi在世界坐标系中的坐标 通过函数G计算点pi在不同分辨率八叉图下的体素索引(indx,indy,indz)resi，这里函数G主要是除以不同分辨率，再在取整得到体素索引，即G(pi)表示点pi在八叉图中的整数型坐标 八叉图中的已知点即为体素，同时初始化对应体素的logodds概率为0。计算体素的占据概率值M(pi)时需要将其占据赢率值logodds概率转为占据概率，八叉图体素的占据概率函数M(pi)计算公式如下所示：[0027][0028] 其中，G(pi)表示激光点pi在八叉图中对应体素的整数类型坐标，logodds(G(pi))表示表示激光点pi在八叉图中对应体素的占据赢率值，M(pi)表示激光点pi在八叉图中对应体素的占据概率。[0029] (2)计算八叉图体素点集的三维正态分布参数；[0030] 已知八叉图体素内的点集，当内点数量超过3个时，计算其三维正态分布参数ND(E,Cov,f(p))，参数E、Cov、f(p)分别代表期望、方差矩阵和三维正态分布概率函数，八叉图体素pi的三维正态分布概率函数f(pi)如下：[0031][0032][0033][0034] 其中，n表示八叉图中的体素数量， 表示体素pi在八叉图中的浮点类坐标。[0035] (3)基于当前激光帧构建八叉图中的体素占据概率及三维正态分布概率残差，非线性优化后获得激光帧的当前位姿；[0036] 将当前激光帧投影到八叉图地图中，计算每个激光点的体素坐标 通过八叉图体素占据概率构建一个占据概率残差，再通过三维正态分布函数构建第二个概率残差，通过双残差构建非线性优化目标函数F(Tt)，如下所示：[0037][0038] 其中：w1和w2分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w1+w2＝1，Tt?1表示上一时刻t?1的世界位姿，Tt表示当前时刻t的世界位姿， 表示激光点pi当前时刻t在世界坐标系下的坐标， 表示激光点pi上一时刻t?1在世界坐标系下的坐标，E表示八叉图中体素内所有点的均值，即期望值。[0039] 已知上一时刻的世界位姿Tt?1，并以此为初值，目标函数通过经典的高斯牛顿方法迭代优化，得到t时刻的世界位姿Tt，实现slam中基于地图的定位。[0040] (4)基于当前位姿将激光帧投影到八叉图，更新体素点集、体素概率和三维正态分布参数，待下次匹配[0041] 通过位姿Tt将当前激光帧投影到八叉图，将激光帧中的点插入对应的体素点集，通重新计算八叉图的三维正态分布参数ND(E,Cov,f(p))。[0042] 八叉图的体素占据赢率值使用logodds(G(p))表示，体素初始化后logodds值为0，设置更新占据概率pocc∈(0.5,1.0)，自由概率pfree∈(0,0.5)，那么更新后的logodds如下:[0043] 在占据概率pocc∈(0.5,1.0)时，则基于如下公式(6)计算logodds(G(p))：[0044][0045] 在自由概率pfree∈(0,0.5)，则基于如下公式(7)计算logodds(G(p))：[0046][0047] 更新后的体素logodds通过公式(1)可以解算出八叉图坐标的体素占据概率M，这就得到了更新后的八叉图，每个体素都包含了三维正态参数ND(E,Cov,f(p))和体素占据概率M，上述过程即建图过程。[0048] 本申请的实施例还提供了一种移动机器人，移动机器人包括:处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法。[0049] 上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

专利地区：安徽

专利申请日期：2021-12-13

专利公开日期：2024-11-29

专利公告号：CN114202586B