頂刊JFR最新ROLO-SLAM開源：專為復雜地形下地面車輛的位姿漂移問題進設計
本文經3D視覺之心公眾號授權轉載，轉載請聯系出處。
解決垂直方向漂移
定位在自動駕駛背景下至關重要。它是安全高效導航的基礎模塊，使車輛能夠準確地確定其在環境中的位置。對于不平整地形的導航，車輛在穿越崎嶇地形時必然會出現波動。剛性安裝在地面車輛上的傳感器特別容易受到這些運動的影響，使得越野定位成為一個極具挑戰性的任務。
SLAM技術能夠同時對傳感器的自身位姿進行定位并提供環境地圖。這種方法為在未知且不平整的環境中的定位提供了有效的解決方案。
本文介紹的重點在于利用基于 LiDAR 的 SLAM 方法進行不平整地形導航。LiDAR 方法通常利用連續的激光掃描進行點云的局部與全局配準，從而估計傳感器的精確自運動。這些方法在不平整的室外場景中表現出對環境條件的不敏感性、遠距離感知能力以及低測量噪聲的優勢。
現成的基于 LiDAR 的 SLAM 通常包含兩個模塊：前端的 LiDAR 里程計和后端的建圖優化。前端通過幀間配準提供初始位姿估計，而后端則通過全局對齊和優化方法精細調整位姿估計并重建周圍環境。這種方法使框架能夠實現從粗到細的定位，適用于諸如城市平坦道路等常規場景。然而，在不平整的地形中，應用于地面車輛的基于 LiDAR 的 SLAM 方法會出現不可忽略的定位漂移，從而導致失真或傾斜的地圖。其主要原因在于垂直方向的位姿分量因車輛在地形表面運動而發生顯著變化。這些變化直接導致幀間匹配時的共識集合減少，使得前端無法提供正確的初始位姿估計。盡管已有大量研究在此方面取得了顯著改進，這一課題依然充滿挑戰并需要進一步優化的解決方案。
項目鏈接：https://github.com/sdwyc/ROLO
針對這一問題，提出了ROLO-SLAM【1】（旋轉優化激光雷達專用 SLAM）：一種旨在減少垂直方向位姿漂移并精確估計地面車輛位姿的 LiDAR 專用 SLAM 框架。我們基于在崎嶇地形下位姿估計漂移的觀察，將前端劃分為三個獨立模塊。在前端，開發了一種前向位置預測，用于粗略的平移估計，以實現旋轉與平移的解耦。隨后，通過體素化匹配和旋轉配準來獨立估計兩次連續掃描之間的精確旋轉。基于連續時間的平移估計方法進一步優化掃描的平移精度。最終，將該方法整合到一個高效的 SLAM 框架中，結合掃描到子地圖的對齊和全局因子圖作為后端。
主要貢獻：
- 提出了一種前向位置預測方法，實現了旋轉估計與平移估計之間的柔性解耦，使得能夠獨立估計旋轉和平移。
- 在前端提出了一種雙階段旋轉和平移估計范式，利用球面對齊和連續時間優化提供了后端優化所需的精確初始位姿。
- 建立了一個緊湊的 LiDAR SLAM 框架，集成了掃描到子地圖的對齊和全局因子圖優化，從而支持地面車輛在不平整地形中的定位。
問題定義
大多數基于 LiDAR 的 SLAM 方法在地面車輛經過不平整地形時會產生明顯的垂直方向位姿漂移。導致這一問題的主要原因包括以下兩個方面：
- 不平整地形表面：車輛的姿態，尤其是滾轉角（roll）和俯仰角（pitch），隨著地形表面的起伏而發生變化。在這種情況下，LiDAR 傳感器在垂直方向上會產生較大的角位移。然而，由于 LiDAR 在垂直方向上的分辨率有限，這些位姿誤差會逐漸累積。
本研究的目標是針對不平整地形中的車輛定位問題，如何利用單一 LiDAR 傳感器提高車輛定位精度。為此，我們重構了整個前端并將其劃分為多個模塊以優化位姿估計。隨后，我們引入了掃描到子地圖的對齊和因子圖，以在地圖內優化車輛的位姿。
具體方法
ROLO-SLAM 系統流程
ROLO-SLAM 的體系架構如圖 3 所示。該框架由兩個主要組件組成：前端 LiDAR 里程計模塊和后端建圖模塊。
首先，利用后端的里程計數據對 LiDAR 掃描數據進行矯正，以消除運動畸變。在前端，基于光滑度指標提取邊緣和平面特征。隨后，開發了一種前向位置預測，用于快速對 LiDAR 平移進行初步估計，從而實現旋轉和平移的松散解耦。在此基礎上，通過開發的體素化方法確定點云的對應關系。旋轉和平移分別獨立估計，其中旋轉通過球面對齊模型注冊，平移通過連續時間優化獲得。
此外，后端通過關鍵幀的聚合來構建子地圖，并通過掃描到子地圖的對齊進行優化。最后，利用因子圖進一步優化 LiDAR 的全局位姿和點云地圖。
前向位置預測
在前端，我們通過前向位置預測實現了連續掃描之間旋轉和平移估計的解耦。這是通過消除掃描之間的平移差異來實現的。
傳統的配準方法通常將旋轉和平移估計結合起來，使得各自的獨立挑戰被掩蓋，從而可能導致車輛姿態和位置估計的潛在不準確。通過引入前向位置預測，我們對連續 LiDAR 掃描之間的平移進行粗略估計，從而在前端實現了旋轉和平移估計的解耦。這為掃描位置建立了一致的基準，預計能夠提高車輛旋轉估計的準確性。
體素化匹配與旋轉配準
上述過程詳細描述于 算法 1 的第 8-12 行。
基于體素化的匹配
旋轉配準
通過匹配過程，我們得到了源點和目標體素之間的對應關系C。接下來，通過將源點云中的點與目標體素中的高斯分布均值Pk對齊，計算掃描之間的旋轉。旋轉配準模型如 圖 5 所示。
最終，該優化問題可通過高斯-牛頓（GN）或 Levenberg-Marquardt（LM）算法迭代求解。
基于連續時間的平移優化
在前向位置預測之后，車輛的平移分量tk已經得到了初步估計。然而，由于崎嶇地形對位姿的影響，直接使用初步預測的平移可能會導致估計誤差累積。因此，我們提出了一種基于連續時間的平移優化方法，以進一步提高位姿估計的準確性。
平移模型
連續時間優化
后端建圖與回環檢測
為了減少累積誤差并進一步優化位姿估計，我們在 ROLO-SLAM 的后端設計了一個建圖與回環檢測模塊。該模塊結合局部子地圖構建和全局因子圖優化，確保車輛的定位在長期運行中保持精確和一致。
局部子地圖構建
在后端，我們基于關鍵幀聚合構建局部子地圖。關鍵幀是經過挑選的 LiDAR 掃描幀，能夠充分代表車輛在環境中的運動狀態。在生成局部子地圖時，關鍵幀間的重疊率需要達到預設閾值，以確保每個子地圖能夠包含足夠的環境特征。
局部子地圖的構建能夠顯著減少處理點云的計算量，同時為全局對齊和回環檢測提供可靠的局部環境描述。
回環檢測與因子圖優化
回環檢測
回環檢測的目的是識別車輛在某一時刻重新訪問了先前經過的地點。這一過程通過當前掃描與全局地圖的匹配來實現。當檢測到回環時，我們將生成一個回環約束以減少累積誤差。
回環檢測的步驟包括：
- 從全局地圖中提取候選關鍵幀；
- 通過快速點云描述子（如 FPFH 或 ISS 特征）進行粗匹配；
- 利用點對點的精確 ICP 算法進行精細配準，計算當前掃描與歷史關鍵幀之間的相對位姿。
因子圖優化通常通過非線性最小二乘方法（如 Ceres 或 GTSAM）求解，以同時校正所有關鍵幀的全局位姿。
全局地圖生成
全局地圖不僅能夠準確描述環境，還可以用于導航、路徑規劃以及后續的定位任務。
實驗效果
總結一下
ROLO-SLAM是一種新穎的 LiDAR 專用 SLAM 框架，針對復雜地形下地面車輛的位姿漂移問題進行了優化。我們的框架通過前端和后端的協同設計，能夠有效減少垂直方向的漂移，提高定位和建圖精度。在實驗中，ROLO-SLAM 在多種場景下展示了卓越的性能，尤其是在地形崎嶇環境中的精度和魯棒性。未來，我們計劃將多傳感器融合引入系統中，以進一步提升在極端場景下的魯棒性和實時性。此外，開發更加高效的算法以進一步優化計算性能也是未來的研究方向。
