[發(fā)明專利]一種在高動態(tài)環(huán)境下的激光SLAM方法、系統(tǒng)設(shè)備及存儲介質(zhì)

在審

• 申請?zhí)枺?/td>: 202110872830.3
• 申請日: 2021-07-30
• 公開（公告）號: CN113740871A
• 公開（公告）日: 2021-12-03
• 發(fā)明（設(shè)計）人: 孫宏濱;錢成龍;張旭翀;張敬敏;楊彥龍
• 申請（專利權(quán)）人: 西安交通大學(xué)
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01S17/89;G01S17/931;G01C21/16
• 代理公司: 西安通大專利代理有限責(zé)任公司 61200
• 代理人: 李紅霖

1.一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，包括如下過程：

初始狀態(tài)估計：使用慣性測量單元的預(yù)積分里程計來獲得SLAM系統(tǒng)的初始狀態(tài)估計；

距離圖像獲取：分別將當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖按照當(dāng)前SLAM系統(tǒng)的定位誤差所動態(tài)確定的分辨率投影為距離圖像；

去除代表移動物體的點(diǎn)：比較這兩幅距離圖像，去除當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖中代表移動物體的點(diǎn)；

點(diǎn)云匹配：將去除了代表移動物體的點(diǎn)的當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖進(jìn)行點(diǎn)云匹配，獲得SLAM系統(tǒng)的狀態(tài)估計；

收斂性判斷：判斷點(diǎn)云匹配是否收斂；

確定最終的SLAM系統(tǒng)的狀態(tài)估計：若點(diǎn)云匹配收斂，則將獲得的SLAM系統(tǒng)的狀態(tài)估計作為最終的SLAM系統(tǒng)的狀態(tài)估計；若點(diǎn)云匹配不收斂，則利用去除了代表移動物體的點(diǎn)的當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖重復(fù)進(jìn)行所述距離圖像獲取、去除代表移動物體的點(diǎn)、點(diǎn)云匹配和收斂性判斷的過程，直至點(diǎn)云匹配收斂，得到最終的SLAM系統(tǒng)的狀態(tài)估計。

2.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，首次進(jìn)行點(diǎn)云匹配前的SLAM系統(tǒng)的定位誤差為緊耦合的慣性測量單元的預(yù)積分里程計和激光雷達(dá)里程計之間的定位誤差，該定位誤差通過下式計算：

δX_k＝δX_k-1+δX_k-1Δt

其中，δX_k表示當(dāng)前激光點(diǎn)云時刻通過慣性測量單元的預(yù)積分里程計獲得的SLAM系統(tǒng)定位誤差，δX_k-1表示上一激光點(diǎn)云時刻通過慣性測量單元的預(yù)積分里程計獲得的SLAM系統(tǒng)定位誤差，表示上一時刻的SLAM系統(tǒng)的定位誤差對時間t的導(dǎo)數(shù)，Δt表示當(dāng)前激光點(diǎn)云時刻和上一激光點(diǎn)云時刻之間的時間間隔；

經(jīng)過收斂性判斷且點(diǎn)云匹配不收斂、圖重復(fù)進(jìn)行所述距離圖像獲取時，SLAM系統(tǒng)的定位誤差由當(dāng)前激光點(diǎn)云匹配的誤差得到。

3.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，若點(diǎn)云匹配收斂，利用去除了代表移動物體的點(diǎn)的當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖再進(jìn)行一次所述距離圖像獲取以及去除代表移動物體的點(diǎn)過程，得到最終的SLAM系統(tǒng)的狀態(tài)估計。

4.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，SLAM系統(tǒng)的定位誤差所確定的分辨率r如下：

r＝αδp+δθ

其中，α為縮放系數(shù)，δp表示位置誤差，δθ表示位姿誤差。

5.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，將當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖按照當(dāng)前SLAM系統(tǒng)的定位誤差所動態(tài)確定的分辨率投影為距離圖像時：

距離圖像中的每個像素對應(yīng)的是當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖球面坐標(biāo)的角度范圍大小，像素值是球心到球面扇形區(qū)域中最近激光點(diǎn)的距離。

6.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，比較當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖對應(yīng)的兩幅距離圖像，去除當(dāng)前激光點(diǎn)云和局部點(diǎn)云地圖中代表移動物體的點(diǎn)的過程包括：

將兩幅距離圖像逐像素相減，得到這兩幅距離圖像的視差圖像；

當(dāng)視差圖像中某一像素的像素值大于閾值時，對應(yīng)的當(dāng)前激光點(diǎn)云的距離圖像的像素對應(yīng)的點(diǎn)云為動態(tài)點(diǎn)，將確定出的動態(tài)點(diǎn)從當(dāng)前激光點(diǎn)云中去除；

當(dāng)視差圖像中某一像素的像素值小于閾值的相反數(shù)時，局部點(diǎn)云地圖的距離圖像的像素對應(yīng)的點(diǎn)云為動態(tài)點(diǎn)，將確定出的動態(tài)點(diǎn)從局部點(diǎn)云地圖中去除。

7.根據(jù)權(quán)利要求1所述的一種在高動態(tài)環(huán)境下的激光SLAM方法，其特征在于，使用緊耦合的慣性測量單元的預(yù)積分里程計來獲得SLAM系統(tǒng)的初始狀態(tài)估計的過程包括：

通過對慣性測量單元的數(shù)據(jù)進(jìn)行預(yù)積分得到慣性測量單元預(yù)積分里程計，使用慣性測量單元預(yù)積分里程計補(bǔ)償激光雷達(dá)的運(yùn)動畸變，獲得SLAM系統(tǒng)的初始狀態(tài)估計。