[發明專利]一種基于子圖更新和反光板優化的動態激光SLAM方法

在審

• 申請號: 202110740698.0
• 申請日: 2021-06-30
• 公開（公告）號: CN113532421A
• 公開（公告）日: 2021-10-22
• 發明（設計）人: 陳浩;鄧若愚
• 申請（專利權）人: 同濟人工智能研究院（蘇州）有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 蘇州創元專利商標事務所有限公司 32103
• 代理人: 王樺

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的后端全局優化目標為

其中：

為某一個子圖的位姿，為某一幀激光點云的位姿，為某一個反光板的位姿。