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

权利要求书

1.一种基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，所述方法包括如下步骤：

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

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

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

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

2.如权利要求1所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，当前位姿T_t的获取方法具体如下：

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

其中，w₁和w₂分别是八叉图概率匹配权重、三维正态分布概率匹配权重，w₁+w₂＝1，T_t-1表示上一时刻t-1的世界位姿，T_t表示当前时刻t的世界位姿，表示激光点p_i当前时刻t在世界坐标系下的坐标，表示激光点p_i上一时刻t-1在世界坐标系下的坐标，E表示八叉图中体素内所有点的均值，即期望值；

目标函数通过经典的高斯牛顿方法迭代优化，得到t时刻的世界位姿T_t。

3.如权利要求1所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法，其特征在于，八叉图体素的占据概率函数M(p_i)计算公式如下所示：

其中，G(p_i)表示激光点p_i在八叉图中对应体素的整数类型坐标，logodds(G(p_i))表示表示激光点p_i在八叉图中对应体素的占据赢率值，M(p_i)表示激光点p_i在八叉图中对应体素的占据概率。

4.一种移动机器人，其特征在于，所述移动机器人包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1-3任一权利要求所述基于八叉图和三维正态分布优化匹配的3D激光SLAM方法。