本文經(jīng)3D視覺之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。

解決垂直方向漂移

定位在自動駕駛背景下至關(guān)重要。它是安全高效導(dǎo)航的基礎(chǔ)模塊，使車輛能夠準(zhǔn)確地確定其在環(huán)境中的位置。對于不平整地形的導(dǎo)航，車輛在穿越崎嶇地形時必然會出現(xiàn)波動。剛性安裝在地面車輛上的傳感器特別容易受到這些運(yùn)動的影響，使得越野定位成為一個極具挑戰(zhàn)性的任務(wù)。

SLAM技術(shù)能夠同時對傳感器的自身位姿進(jìn)行定位并提供環(huán)境地圖。這種方法為在未知且不平整的環(huán)境中的定位提供了有效的解決方案。

本文介紹的重點(diǎn)在于利用基于 LiDAR 的 SLAM 方法進(jìn)行不平整地形導(dǎo)航。LiDAR 方法通常利用連續(xù)的激光掃描進(jìn)行點(diǎn)云的局部與全局配準(zhǔn)，從而估計傳感器的精確自運(yùn)動。這些方法在不平整的室外場景中表現(xiàn)出對環(huán)境條件的不敏感性、遠(yuǎn)距離感知能力以及低測量噪聲的優(yōu)勢。

現(xiàn)成的基于 LiDAR 的 SLAM 通常包含兩個模塊：前端的 LiDAR 里程計和后端的建圖優(yōu)化。前端通過幀間配準(zhǔn)提供初始位姿估計，而后端則通過全局對齊和優(yōu)化方法精細(xì)調(diào)整位姿估計并重建周圍環(huán)境。這種方法使框架能夠?qū)崿F(xiàn)從粗到細(xì)的定位，適用于諸如城市平坦道路等常規(guī)場景。然而，在不平整的地形中，應(yīng)用于地面車輛的基于 LiDAR 的 SLAM 方法會出現(xiàn)不可忽略的定位漂移，從而導(dǎo)致失真或傾斜的地圖。其主要原因在于垂直方向的位姿分量因車輛在地形表面運(yùn)動而發(fā)生顯著變化。這些變化直接導(dǎo)致幀間匹配時的共識集合減少，使得前端無法提供正確的初始位姿估計。盡管已有大量研究在此方面取得了顯著改進(jìn)，這一課題依然充滿挑戰(zhàn)并需要進(jìn)一步優(yōu)化的解決方案。

項目鏈接：https://github.com/sdwyc/ROLO

針對這一問題，提出了ROLO-SLAM【1】（旋轉(zhuǎn)優(yōu)化激光雷達(dá)專用 SLAM）：一種旨在減少垂直方向位姿漂移并精確估計地面車輛位姿的 LiDAR 專用 SLAM 框架。我們基于在崎嶇地形下位姿估計漂移的觀察，將前端劃分為三個獨(dú)立模塊。在前端，開發(fā)了一種前向位置預(yù)測，用于粗略的平移估計，以實(shí)現(xiàn)旋轉(zhuǎn)與平移的解耦。隨后，通過體素化匹配和旋轉(zhuǎn)配準(zhǔn)來獨(dú)立估計兩次連續(xù)掃描之間的精確旋轉(zhuǎn)。基于連續(xù)時間的平移估計方法進(jìn)一步優(yōu)化掃描的平移精度。最終，將該方法整合到一個高效的 SLAM 框架中，結(jié)合掃描到子地圖的對齊和全局因子圖作為后端。

主要貢獻(xiàn)：

• 提出了一種前向位置預(yù)測方法，實(shí)現(xiàn)了旋轉(zhuǎn)估計與平移估計之間的柔性解耦，使得能夠獨(dú)立估計旋轉(zhuǎn)和平移。
• 在前端提出了一種雙階段旋轉(zhuǎn)和平移估計范式，利用球面對齊和連續(xù)時間優(yōu)化提供了后端優(yōu)化所需的精確初始位姿。
• 建立了一個緊湊的 LiDAR SLAM 框架，集成了掃描到子地圖的對齊和全局因子圖優(yōu)化，從而支持地面車輛在不平整地形中的定位。

問題定義

大多數(shù)基于 LiDAR 的 SLAM 方法在地面車輛經(jīng)過不平整地形時會產(chǎn)生明顯的垂直方向位姿漂移。導(dǎo)致這一問題的主要原因包括以下兩個方面：

1. 不平整地形表面：車輛的姿態(tài)，尤其是滾轉(zhuǎn)角（roll）和俯仰角（pitch），隨著地形表面的起伏而發(fā)生變化。在這種情況下，LiDAR 傳感器在垂直方向上會產(chǎn)生較大的角位移。然而，由于 LiDAR 在垂直方向上的分辨率有限，這些位姿誤差會逐漸累積。

本研究的目標(biāo)是針對不平整地形中的車輛定位問題，如何利用單一 LiDAR 傳感器提高車輛定位精度。為此，我們重構(gòu)了整個前端并將其劃分為多個模塊以優(yōu)化位姿估計。隨后，我們引入了掃描到子地圖的對齊和因子圖，以在地圖內(nèi)優(yōu)化車輛的位姿。

具體方法

ROLO-SLAM 系統(tǒng)流程

ROLO-SLAM 的體系架構(gòu)如圖 3 所示。該框架由兩個主要組件組成：前端 LiDAR 里程計模塊和后端建圖模塊。

首先，利用后端的里程計數(shù)據(jù)對 LiDAR 掃描數(shù)據(jù)進(jìn)行矯正，以消除運(yùn)動畸變。在前端，基于光滑度指標(biāo)提取邊緣和平面特征。隨后，開發(fā)了一種前向位置預(yù)測，用于快速對 LiDAR 平移進(jìn)行初步估計，從而實(shí)現(xiàn)旋轉(zhuǎn)和平移的松散解耦。在此基礎(chǔ)上，通過開發(fā)的體素化方法確定點(diǎn)云的對應(yīng)關(guān)系。旋轉(zhuǎn)和平移分別獨(dú)立估計，其中旋轉(zhuǎn)通過球面對齊模型注冊，平移通過連續(xù)時間優(yōu)化獲得。

此外，后端通過關(guān)鍵幀的聚合來構(gòu)建子地圖，并通過掃描到子地圖的對齊進(jìn)行優(yōu)化。最后，利用因子圖進(jìn)一步優(yōu)化 LiDAR 的全局位姿和點(diǎn)云地圖。

前向位置預(yù)測

在前端，我們通過前向位置預(yù)測實(shí)現(xiàn)了連續(xù)掃描之間旋轉(zhuǎn)和平移估計的解耦。這是通過消除掃描之間的平移差異來實(shí)現(xiàn)的。

傳統(tǒng)的配準(zhǔn)方法通常將旋轉(zhuǎn)和平移估計結(jié)合起來，使得各自的獨(dú)立挑戰(zhàn)被掩蓋，從而可能導(dǎo)致車輛姿態(tài)和位置估計的潛在不準(zhǔn)確。通過引入前向位置預(yù)測，我們對連續(xù) LiDAR 掃描之間的平移進(jìn)行粗略估計，從而在前端實(shí)現(xiàn)了旋轉(zhuǎn)和平移估計的解耦。這為掃描位置建立了一致的基準(zhǔn)，預(yù)計能夠提高車輛旋轉(zhuǎn)估計的準(zhǔn)確性。

體素化匹配與旋轉(zhuǎn)配準(zhǔn)

上述過程詳細(xì)描述于 算法 1 的第 8-12 行。

基于體素化的匹配

旋轉(zhuǎn)配準(zhǔn)

通過匹配過程，我們得到了源點(diǎn)和目標(biāo)體素之間的對應(yīng)關(guān)系C。接下來，通過將源點(diǎn)云中的點(diǎn)與目標(biāo)體素中的高斯分布均值Pk對齊，計算掃描之間的旋轉(zhuǎn)。旋轉(zhuǎn)配準(zhǔn)模型如 圖 5 所示。

最終，該優(yōu)化問題可通過高斯-牛頓（GN）或 Levenberg-Marquardt（LM）算法迭代求解。

基于連續(xù)時間的平移優(yōu)化

在前向位置預(yù)測之后，車輛的平移分量tk已經(jīng)得到了初步估計。然而，由于崎嶇地形對位姿的影響，直接使用初步預(yù)測的平移可能會導(dǎo)致估計誤差累積。因此，我們提出了一種基于連續(xù)時間的平移優(yōu)化方法，以進(jìn)一步提高位姿估計的準(zhǔn)確性。

平移模型

連續(xù)時間優(yōu)化

后端建圖與回環(huán)檢測

為了減少累積誤差并進(jìn)一步優(yōu)化位姿估計，我們在 ROLO-SLAM 的后端設(shè)計了一個建圖與回環(huán)檢測模塊。該模塊結(jié)合局部子地圖構(gòu)建和全局因子圖優(yōu)化，確保車輛的定位在長期運(yùn)行中保持精確和一致。

局部子地圖構(gòu)建

在后端，我們基于關(guān)鍵幀聚合構(gòu)建局部子地圖。關(guān)鍵幀是經(jīng)過挑選的 LiDAR 掃描幀，能夠充分代表車輛在環(huán)境中的運(yùn)動狀態(tài)。在生成局部子地圖時，關(guān)鍵幀間的重疊率需要達(dá)到預(yù)設(shè)閾值，以確保每個子地圖能夠包含足夠的環(huán)境特征。

局部子地圖的構(gòu)建能夠顯著減少處理點(diǎn)云的計算量，同時為全局對齊和回環(huán)檢測提供可靠的局部環(huán)境描述。

回環(huán)檢測與因子圖優(yōu)化

回環(huán)檢測

回環(huán)檢測的目的是識別車輛在某一時刻重新訪問了先前經(jīng)過的地點(diǎn)。這一過程通過當(dāng)前掃描與全局地圖的匹配來實(shí)現(xiàn)。當(dāng)檢測到回環(huán)時，我們將生成一個回環(huán)約束以減少累積誤差。

回環(huán)檢測的步驟包括：

1. 從全局地圖中提取候選關(guān)鍵幀；
2. 通過快速點(diǎn)云描述子（如 FPFH 或 ISS 特征）進(jìn)行粗匹配；
3. 利用點(diǎn)對點(diǎn)的精確 ICP 算法進(jìn)行精細(xì)配準(zhǔn)，計算當(dāng)前掃描與歷史關(guān)鍵幀之間的相對位姿。

因子圖優(yōu)化通常通過非線性最小二乘方法（如 Ceres 或 GTSAM）求解，以同時校正所有關(guān)鍵幀的全局位姿。

全局地圖生成

全局地圖不僅能夠準(zhǔn)確描述環(huán)境，還可以用于導(dǎo)航、路徑規(guī)劃以及后續(xù)的定位任務(wù)。

實(shí)驗效果

總結(jié)一下

ROLO-SLAM是一種新穎的 LiDAR 專用 SLAM 框架，針對復(fù)雜地形下地面車輛的位姿漂移問題進(jìn)行了優(yōu)化。我們的框架通過前端和后端的協(xié)同設(shè)計，能夠有效減少垂直方向的漂移，提高定位和建圖精度。在實(shí)驗中，ROLO-SLAM 在多種場景下展示了卓越的性能，尤其是在地形崎嶇環(huán)境中的精度和魯棒性。未來，我們計劃將多傳感器融合引入系統(tǒng)中，以進(jìn)一步提升在極端場景下的魯棒性和實(shí)時性。此外，開發(fā)更加高效的算法以進(jìn)一步優(yōu)化計算性能也是未來的研究方向。