[發(fā)明專利]一種基于子圖更新和反光板優(yōu)化的動態(tài)激光SLAM方法

在審

• 申請?zhí)枺?/td>: 202110740698.0
• 申請日: 2021-06-30
• 公開（公告）號: CN113532421A
• 公開（公告）日: 2021-10-22
• 發(fā)明（設(shè)計）人: 陳浩;鄧若愚
• 申請（專利權(quán)）人: 同濟人工智能研究院（蘇州）有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 蘇州創(chuàng)元專利商標事務(wù)所有限公司 32103
• 代理人: 王樺

本發(fā)明涉及一種基于子圖更新和反光板優(yōu)化的動態(tài)激光SLAM方法，包括如下步驟：實時獲取激光數(shù)據(jù)，進行前端掃描匹配，將激光數(shù)據(jù)插入子圖；根據(jù)激光數(shù)據(jù)的強度信息和初步估計位姿，解析并匹配反光板；根據(jù)建立好的約束構(gòu)建后端優(yōu)化目標，優(yōu)化軌跡和地圖；根據(jù)建圖或定位的需求，動態(tài)更新或覆蓋地圖。本發(fā)明基于子圖更新的方式對地圖進行動態(tài)更新，可有效應(yīng)對環(huán)境的動態(tài)變化，提高了激光匹配的魯棒性，提高了定位的精準性，并將反光板約束條件加入SLAM后端優(yōu)化的過程，使得優(yōu)化過程更加快速且準確，獲得了更加精準的狀態(tài)估計結(jié)果。

技術(shù)領(lǐng)域

本發(fā)明屬于移動機器人自主建圖與定位領(lǐng)域，具體涉及一種基于子圖更新和反光板優(yōu)化的動態(tài)激光SLAM方法。

背景技術(shù)

移動機器人的SLAM問題是機器人在未知或已知環(huán)境中穩(wěn)定工作的基石。靜態(tài)場景的SLAM工作研究已經(jīng)相對成熟，而動態(tài)場景多由于其不確定性，對算法的魯棒性要求非常高，目前尚沒有穩(wěn)定成熟的工業(yè)界解決方案。環(huán)境的動態(tài)變化可能導(dǎo)致很多問題，比如激光難以初始化、依賴于固定特征的算法不穩(wěn)定等。貨物搬運工廠的動態(tài)環(huán)境給現(xiàn)有的SLAM算法帶來了很大的挑戰(zhàn)，變化的環(huán)境可能會造成錯誤的激光匹配，從而導(dǎo)致錯誤的定位。工廠環(huán)境里的AGV在面臨工廠結(jié)構(gòu)變化或人員走動等環(huán)境的動態(tài)變化時，容易發(fā)生定位丟失。

移動機器人采用的靜態(tài)地圖與粒子濾波融合的定位方案往往無法應(yīng)對環(huán)境動態(tài)變化問題，而當前尚未存在穩(wěn)定開源的動態(tài)SLAM算法，只在一些對動態(tài)SLAM的理論研究，比如對時間尺度的并列融合方式可以使得機器人通過建立雙地圖的方案來動態(tài)定位，對動態(tài)物體的檢測算法可以使得機器人對位姿圖節(jié)點進行刪除和更新，而這些算法大多需要很大的內(nèi)存消耗和大量計算，且位置難以從錯誤中恢復(fù)。

發(fā)明內(nèi)容

本發(fā)明的目的是提供一種基于子圖更新和反光板優(yōu)化的動態(tài)激光SLAM方法。

為達到上述目的，本發(fā)明采用的技術(shù)方案是：

一種基于子圖更新和反光板優(yōu)化的動態(tài)激光SLAM方法，包括如下步驟：

S1：實時獲取激光數(shù)據(jù)，進行前端掃描匹配，將激光數(shù)據(jù)插入子圖；

S2：根據(jù)激光數(shù)據(jù)的強度信息和初步估計位姿，解析并匹配反光板；

S3：根據(jù)建立好的約束構(gòu)建后端優(yōu)化目標，優(yōu)化軌跡和地圖；

S4：根據(jù)建圖或定位的需求，動態(tài)更新或覆蓋地圖。

優(yōu)選地，在S1中：所述的前端掃描匹配包括如下步驟：

(1)進行粗細粒度的掃描匹配，

(2)進行基于Ceres優(yōu)化的掃描匹配，

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

優(yōu)選地，所述的粗細粒度的掃描匹配的搜索式為所述的基于Ceres優(yōu)化的掃描匹配的搜索式為

式中：

T_ξ為掃描點到地圖坐標系的變換，

h_k為激光束的掃描點集。

優(yōu)選地，在S1中：所述的插入子圖包括如下步驟：

(1)將當前幀的點云分別插入相鄰的兩個子圖，

(2)對已生成的子圖進行激光幀-子圖的掃描匹配，若響應(yīng)達到指定的閾值，則視為找到回環(huán)，

(3)將激光幀與子圖的約束加入后端，進行周期性的全局優(yōu)化。

優(yōu)選地，在S2中：所述的解析并匹配反光板包括如下步驟：