CN113532421B - 一种基于子图更新和反光板优化的动态激光slam方法 - Google Patents

Abstract

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

Description

一种基于子图更新和反光板优化的动态激光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中：所述的解析并匹配反光板包括如下步骤：

(1)对每一个激光帧设定激光强度阈值，在超过阈值的区域进行局部极大值检测，获得反光柱圆心到激光雷达的位置，并获取其相对于激光雷达的直线距离和偏转角度信息，定义激光坐标系下的反光板位置为R_n＝{d′,θ′}，机器人坐标系下的反光板位置为R′_n＝{d,θ}，对激光坐标系和机器人坐标系下的相对位姿变换，

(2)根据当前机器人的位姿对检测到的反光板进行初步判断，根据反光板安装时的位置并按照距离最近原则的匹配标准进行匹配，获取反光板的唯一标识，将反光板位姿转R′_n换为旋转矩阵形式的位姿ξ_jk并加入后端约束。

优选地，所述的激光坐标系和机器人坐标系下的相对位姿变换为：

式中：

Δx和Δy为机器人坐标系和激光坐标系之间的偏移量，

所述的旋转矩阵形式的位姿ξ_jk为：

式中：

优选地，在S3中：所述的后端优化包括如下步骤：

(1)建立激光帧节点位姿和子图位姿间的约束、激光帧位姿和反光板节点位姿间的约束，

(2)融合两项约束，按照动态SLAM的后端全局优化目标进行优化，

(3)基于SPA算法迭代求解优化目标，得到优化后的子图集合Ξ^m，激光点云集合Ξ^s，反光板位姿集合Ξ^r。

优选地，所述的动态SLAM的后端全局优化目标为

其中：

为某一个子图的位姿，/>为某一帧激光点云的位姿，/>为某一个反光板的位姿。

优选地，所述的子图集合Ξ^m、激光点云集合Ξ^s、反光板位姿集合Ξ^r分别为：

优选地，在S4中：所述的动态更新或覆盖地图包括如下步骤：

(1)建立覆盖工作环境的地图，并将轨迹和子图保存，

(2)在下次作业开始时，对原先存储的地图和轨迹进行加载，机器人在场景内建立一条新的轨迹，并产生新的子图，

(3)定位过程中保留新建立的三个子图，并将其以滑动窗口的形式更新原来重叠的子图部分，

(4)在产生新的轨迹后，对整个地图进行更新，使系统中保存的地图始终是经最新动态探索而得到的地图m。

由于上述技术方案运用，本发明与现有技术相比具有下列优点：

1.本发明基于子图更新的方式对地图进行动态更新，可有效应对环境的动态变化，提高了激光匹配的鲁棒性，提高了定位的精准性。

2.本发明将反光板约束条件加入SLAM后端优化的过程，使得优化过程更加快速且准确，并获得了更加精准的状态估计结果。

附图说明

附图1为本发明的动态激光SLAM方法流程图；

附图2为本发明的前端扫描匹配过程示意图；

附图3为本发明的插入子图过程示意图；

附图4为本发明的后端优化过程示意图；

附图5为本发明的动态地图更新过程示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

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

S1：

实时获取激光数据，使用激光雷达作为前端数据来源。

进行前端扫描匹配，如图2所示，包括如下步骤：

进行粗细粒度的扫描匹配；在计算出一个位姿后，进行基于Ceres优化的扫描匹配，对位姿进行更精细粒度的优化求解，基于Ceres的匹配过程可使激光帧在粗细粒度的扫描匹配后在小范围内获取更为精准的解；最后得到激光帧插入位姿。

具体而言：

粗细粒度的扫描匹配的搜索式为

基于Ceres优化的扫描匹配的搜索式为

式中：

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

h_k为激光束的扫描点集。

将激光数据插入子图，如图3所示，包括如下步骤：

为使子图之间连续性更高，将当前帧的点云分别插入相邻的两个子图，子图在达到设定帧数的点云后，会置为完成状态，并开启下一个子图的建立；

同时对已生成的子图进行激光帧-子图的扫描匹配，若响应达到指定的阈值，则视为找到回环；将激光帧与子图的约束加入后端，进行周期性的全局优化。在本实施例中：优化的时机设置为多个子图完成的时刻。

S2：

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

对每一个激光帧设定激光强度阈值，雷达激光束到达反光板和墙面后，所返回的激光强度值不同，反光板面返回的激光强度值远高于普通墙面，且离激光雷达越近的点激光强度值越高。

在超过阈值的区域进行局部极大值检测，获得反光柱圆心到激光雷达的位置，并获取其相对于激光雷达的直线距离和偏转角度信息。

定义激光坐标系下的反光板位置为R_n＝{d′,θ′}，机器人坐标系下的反光板位置为R′_n＝{d,θ}，机器人坐标系和激光雷达坐标系之间的偏移量为{Δx,Δy}，对激光坐标系和机器人坐标系下的相对位姿变换，具体而言：

根据当前机器人的位姿对检测到的反光板进行初步判断，由于环境中存在多个反光板，需要进行做唯一性识别，根据反光板安装时的位置并按照距离最近原则的匹配标准进行匹配，获取反光板的唯一标识，将反光板位姿转R′_n换为旋转矩阵形式的位姿ξ_jk并加入后端约束，具体而言：

式中：

S3：

根据建立好的约束构建后端优化目标，优化轨迹和地图，如图4所示，包括如下步骤：

建立激光帧节点位姿和子图位姿间的约束、激光帧位姿和反光板节点位姿间的约束。

融合两项约束，按照动态SLAM的后端全局优化目标进行优化，全局优化的时间在提前设定的周期内以及子图构建完成时，具体而言：

其中：

为某一个子图的位姿，/>为某一帧激光点云的位姿，/>为某一个反光板的位姿。

基于SPA算法迭代求解优化目标，得到优化后的子图集合Ξ^m，激光点云集合Ξ^s，反光板位姿集合Ξ^r，具体而言：

S4：

根据建图或定位的需求，动态更新或覆盖地图，如图5所示，保证了在环境变化时不会因为激光的误匹配而导致定位系统失效，具体而言：

利用前端和后端系统，建立覆盖工作环境的地图，并将轨迹和子图保存。

在下次作业开始时，对原先存储的地图和轨迹进行加载，机器人在场景内建立一条新的轨迹，并产生新的子图。

为避免程序对大量子图匹配和存储带来计算负担，定位过程中保留新建立的三个子图，并将其以滑动窗口的形式更新原来重叠的子图部分。如图5所示，如果当前子图与原始子图存在差异，则原始子图被覆盖，该情况通常发生在定位轨迹与建图轨迹相近的情况下，此时当前子图与原始的子图会存在重叠的情况。

在产生新的轨迹后，对整个地图进行更新，使系统中保存的地图始终是经最新动态探索而得到的地图m。如此反复，在完成一项任务后，将更新后的地图保存，在下次任务开始时，加载更新后的地图。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (3)

1.一种基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：依次包括如下步骤：S1：实时获取激光数据，进行前端扫描匹配，将激光数据插入子图，

所述的前端扫描匹配包括如下步骤：

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

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

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

所述的插入子图包括如下步骤：

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

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

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

S2：根据激光数据的强度信息和初步估计位姿，解析并匹配反光板，所述的解析并匹配反光板包括如下步骤：

(1)对每一个激光帧设定激光强度阈值，在超过阈值的区域进行局部极大值检测，获得反光柱圆心到激光雷达的位置，并获取其相对于激光雷达的直线距离和偏转角度信息，定义激光坐标系下的反光板位置为R_n＝{d′,θ′}，机器人坐标系下的反光板位置为R_n′＝{d,θ}，对激光坐标系和机器人坐标系下的相对位姿变换为：

式中：

Δx和Δy为机器人坐标系和激光坐标系之间的偏移量，

旋转矩阵形式的位姿ξ_jk为：

式中：

(2)根据当前机器人的位姿对检测到的反光板进行初步判断，根据反光板安装时的位置并按照距离最近原则的匹配标准进行匹配，获取反光板的唯一标识，将反光板位姿R_n′转换为旋转矩阵形式的位姿ξ_jk并加入后端约束，

S3：根据建立好的约束构建后端优化目标，优化轨迹和地图，所述的后端优化包括如下步骤：

(1)建立激光帧节点位姿和子图位姿间的约束、激光帧位姿和反光板节点位姿间的约束，

(2)融合两项约束，按照动态SLAM的后端全局优化目标进行优化，

(3)基于SPA算法迭代求解优化目标，得到优化后的子图集合Ξ^m，激光点云集合Ξ^s，反光板位姿集合Ξ^r；

S4：根据建图或定位的需求，动态更新或覆盖地图，所述的动态更新或覆盖地图包括如下步骤：

(1)建立覆盖工作环境的地图，并将轨迹和子图保存，

(2)在下次作业开始时，对原先存储的地图和轨迹进行加载，机器人在场景内建立一条新的轨迹，并产生新的子图，

(3)定位过程中保留新建立的三个子图，并将其以滑动窗口的形式更新原来重叠的子图部分，(4)在产生新的轨迹后，对整个地图进行更新，使系统中保存的地图始终是经最新动态探索而得到的地图m。

2.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：所述的粗细粒度的扫描匹配的搜索式为所述的基于Ceres优化的扫描匹配的搜索式为/>式中：

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

h_k为激光束的扫描点集。

3.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：所述的子图集合Ξ^m、激光点云集合Ξ^s、反光板位姿集合Ξ^r分别为：

为某一个子图的位姿，/>为某一帧激光点云的位姿，/>为某一个反光板的位姿。