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

解決垂直方向漂移

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

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

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

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

項(xiàng)目鏈接：https://github.com/sdwyc/ROLO

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

主要貢獻(xiàn)：

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

問(wèn)題定義

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

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

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

具體方法

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

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

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

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

前向位置預(yù)測(cè)

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

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

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

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

基于體素化的匹配

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

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

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

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

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

平移模型

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

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

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

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

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

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

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

回環(huán)檢測(cè)

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

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

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

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

全局地圖生成

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

實(shí)驗(yàn)效果

總結(jié)一下

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