[发明专利]一种基于子图更新和反光板优化的动态激光SLAM方法

说明书

本发明涉及一种基于子图更新和反光板优化的动态激光SLAM方法，包括如下步骤：实时获取激光数据，进行前端扫描匹配，将激光数据插入子图；根据激光数据的强度信息和初步估计位姿，解析并匹配反光板；根据建立好的约束构建后端优化目标，优化轨迹和地图；根据建图或定位的需求，动态更新或覆盖地图。本发明基于子图更新的方式对地图进行动态更新，可有效应对环境的动态变化，提高了激光匹配的鲁棒性，提高了定位的精准性，并将反光板约束条件加入SLAM后端优化的过程，使得优化过程更加快速且准确，获得了更加精准的状态估计结果。

技术领域

本发明属于移动机器人自主建图与定位领域，具体涉及一种基于子图更新和反光板优化的动态激光SLAM方法。

背景技术

移动机器人的SLAM问题是机器人在未知或已知环境中稳定工作的基石。静态场景的SLAM工作研究已经相对成熟，而动态场景多由于其不确定性，对算法的鲁棒性要求非常高，目前尚没有稳定成熟的工业界解决方案。环境的动态变化可能导致很多问题，比如激光难以初始化、依赖于固定特征的算法不稳定等。货物搬运工厂的动态环境给现有的SLAM算法带来了很大的挑战，变化的环境可能会造成错误的激光匹配，从而导致错误的定位。工厂环境里的AGV在面临工厂结构变化或人员走动等环境的动态变化时，容易发生定位丢失。

移动机器人采用的静态地图与粒子滤波融合的定位方案往往无法应对环境动态变化问题，而当前尚未存在稳定开源的动态SLAM算法，只在一些对动态SLAM的理论研究，比如对时间尺度的并列融合方式可以使得机器人通过建立双地图的方案来动态定位，对动态物体的检测算法可以使得机器人对位姿图节点进行删除和更新，而这些算法大多需要很大的内存消耗和大量计算，且位置难以从错误中恢复。

发明内容

本发明的目的是提供一种基于子图更新和反光板优化的动态激光SLAM方法。

为达到上述目的，本发明采用的技术方案是：

一种基于子图更新和反光板优化的动态激光SLAM方法，包括如下步骤：

S1：实时获取激光数据，进行前端扫描匹配，将激光数据插入子图；

S2：根据激光数据的强度信息和初步估计位姿，解析并匹配反光板；

S3：根据建立好的约束构建后端优化目标，优化轨迹和地图；

S4：根据建图或定位的需求，动态更新或覆盖地图。

优选地，在S1中：所述的前端扫描匹配包括如下步骤：

(1)进行粗细粒度的扫描匹配，

(2)进行基于Ceres优化的扫描匹配，

(3)得到激光帧插入位姿。

优选地，所述的粗细粒度的扫描匹配的搜索式为所述的基于Ceres优化的扫描匹配的搜索式为

式中：

T_ξ为扫描点到地图坐标系的变换，

h_k为激光束的扫描点集。

优选地，在S1中：所述的插入子图包括如下步骤：

(1)将当前帧的点云分别插入相邻的两个子图，

(2)对已生成的子图进行激光帧-子图的扫描匹配，若响应达到指定的阈值，则视为找到回环，

(3)将激光帧与子图的约束加入后端，进行周期性的全局优化。

优选地，在S2中：所述的解析并匹配反光板包括如下步骤：