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

权利要求书

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

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

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

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

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

2.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：在S1中：所述的前端扫描匹配包括如下步骤：

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

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

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

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

式中：

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

h_k为激光束的扫描点集。

4.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：在S1中：所述的插入子图包括如下步骤：

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

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

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

5.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：在S2中：所述的解析并匹配反光板包括如下步骤：

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

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

6.根据权利要求5所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：所述的激光坐标系和机器人坐标系下的相对位姿变换为：

式中：

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

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

式中：

7.根据权利要求1所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：在S3中：所述的后端优化包括如下步骤：

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

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

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

8.根据权利要求7所述的基于子图更新和反光板优化的动态激光SLAM方法，其特征在于：所述的动态SLAM的后端全局优化目标为

其中：

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