導讀
本文提出了一種名為Leg-KILO（Kinematic-Inertial-Lidar Odometry）的多傳感器融合框架，專為動態足式機器人設計。為解決高動態運動導致的IMU漂移和LiDAR失真問題，該方法緊密耦合腿部里程計、慣性測量單元（IMU）和LiDAR數據，并結合圖優化實現回環檢測。通過提出基于誤差狀態卡爾曼濾波器的腿部運動學慣性里程計、高動態自適應掃描分割方法和機器人中心增量建圖，顯著提高了高度和位置估計的精度。實驗表明，Leg-KILO在室內外環境中的漂移顯著小于現有的LiDAR方法，尤其在高動態運動中表現出優越的魯棒性。研究還公開了相關數據集和代碼以供社區使用。
論文信息
- 標題：Leg-KILO: Robust Kinematic-Inertial-Lidar Odometry for Dynamic Legged Robots
- 作者：Guangjun Ou , Dong Li , and Hanmin Li
- 論文鏈接：https://ieeexplore.ieee.org/document/10631676
- 項目地址：https://github.com/ouguangjun/Leg-KILO
動機(Motivation)
本文的研究動機源于動態足式機器人在高動態運動中狀態估計面臨的挑戰，以及現有方法的局限性：
-
高動態運動中的問題：
- 動態足式機器人（如四足機器人）在高動態運動（如跑步或快步）時，足部沖擊頻繁，導致慣性測量單元（IMU）數據退化，尤其是加速度計的漂移。
- LiDAR掃描會受到運動失真影響，進一步導致基于LiDAR的SLAM系統難以穩定運行。
- IMU與LiDAR在高動態環境中容易累積漂移，尤其是在高度（Z軸）方向。
-
現有方法的不足：
- 單純依賴IMU或LiDAR的狀態估計方法在高動態運動中誤差較大，不能滿足動態足式機器人導航的需求。
- 許多現有方法側重于中低速運動場景，而忽視了在高速動態環境下狀態估計的精度和魯棒性。
- 一些僅基于腿部運動學和慣性測量的估計方法容易在長時間運動中累積顯著漂移，且在發生足部滑動等突發狀況時可能失效。
-
解決這一問題的必要性：
- 提升足式機器人在動態、高速場景中的狀態估計精度和魯棒性是實現其在復雜環境中自主導航和控制的關鍵。
- 需要整合多傳感器（如腿部運動學、IMU、LiDAR）的信息，發揮各自優勢，彌補單一傳感器的局限性。
基于以上背景，本文提出了Leg-KILO框架，旨在通過多傳感器融合和高效的優化方法，解決動態足式機器人在高動態運動中的狀態估計問題，推動相關領域的發展。
創新點
-
腿部運動學-慣性里程計的改進：提出了一種基于誤差狀態卡爾曼濾波器（ESKF）的腿部運動學-慣性里程計方法，結合接觸高度檢測約束，有效減少了由足部沖擊引起的高度波動。
-
自適應掃描切片與拼接：針對高動態運動帶來的LiDAR掃描失真問題，提出了一種自適應掃描切片與拼接方法，根據機器人運動速度動態調整掃描角度，提高了輸入頻率的同時降低了運動失真。
-
機器人中心增量地圖：設計了一種機器人中心的增量式局部地圖維護方法，通過增量kd-tree技術高效處理點云的添加和刪除，減少了地圖維護的計算開銷。
-
多傳感器緊密耦合與回環優化：通過圖優化框架將腿部里程計、LiDAR里程計以及回環檢測緊密耦合，從而提升了系統的全局定位精度。
-
全面的實驗驗證與數據共享：在室內外高動態環境下驗證了方法的魯棒性和精確性，結果優于其他LiDAR慣性里程計方法，同時公開了包含腿部運動學數據的LiDAR慣性數據集和代碼，促進了社區研究。
本文核心算法
System overview.
本文的核心算法圍繞Leg-KILO框架，融合腿部運動學、慣性測量單元（IMU）和LiDAR數據，通過創新的算法模塊提升動態足式機器人在高動態環境中的狀態估計能力。以下是算法的主要組成：
1. 腿部運動學-慣性里程計
結合機器人腿部運動學和IMU數據，提出了一種基于誤差狀態卡爾曼濾波器（ESKF）的腿部里程計算法：
- 狀態預測：利用運動學模型預測機器人在空間中的位置、速度和姿態。
- 狀態更新：融合足端速度、足端位置和接觸高度檢測等多種觀測，減少由于足部沖擊引起的高度波動和漂移。
- 接觸高度檢測：通過機器人中心的增量地圖計算足端接觸點的高度，有效降低動態運動中高度方向的累積誤差。
2. LiDAR里程計
為了適應動態運動帶來的點云失真問題，提出了以下創新方法：
- 自適應掃描切片與拼接：根據機器人運動速度動態調整LiDAR掃描切片的角度，拼接當前和歷史掃描切片以生成高頻的去畸變點云。
- 點云匹配與優化：使用點到線和點到面的匹配算法對點云進行優化，提升位姿估計的精度。
- 增量式地圖維護：采用增量kd-tree技術動態管理局部地圖，通過高效添加與移除點云，實現對大規模點云的實時維護。
3. 因子圖優化
通過因子圖緊密耦合腿部里程計、LiDAR里程計和回環檢測：
- 因子圖結構：整合腿部里程計的相對位姿、LiDAR里程計的相對位姿以及回環檢測的匹配信息，形成優化目標。
- 回環檢測：基于距離約束的回環檢測方法，選擇匹配幀對進行閉環優化，進一步提升全局定位精度。
4. 系統流程
- 使用腿部運動學和IMU數據進行預測和初步狀態估計。
- 通過自適應掃描與拼接生成高頻點云，優化點云匹配結果。
- 將腿部里程計、LiDAR里程計及回環檢測結果融合到因子圖中，實現全局優化。
- 維護增量式局部地圖，確保實時性和計算效率。
實驗仿真結果
本文提出的方法與其他方法的相對位姿誤差（RPE）的比較
Comparison of trajectory for various methods on parking.
在平地上跑步時，不同方法對高度變化的比較。跑步時，車身高度會略有下降，不會超過圖中的灰色區域。
untime comparison on sequence parking.
總結 & 展望
本文提出了一種基于運動學-慣性-LiDAR的里程計方法——Leg-KILO。通過引入腿部運動學約束，增強了基于LiDAR里程計的穩定性。此外，提出了一種自適應掃描切片與拼接方法，以緩解動態運動的影響。同時，采用一種以機器人為中心的增量式局部地圖維護方法，顯著降低了地圖維護的時間成本。通過因子圖優化，將腿部里程計、LiDAR里程計和回環檢測因子進行整合，以提高定位精度。
本文的方法在多種高動態運動環境下（采用快步運動方式）進行了廣泛的測試，結果表明，與其他先進的LiDAR方法相比，Leg-KILO更適合動態四足機器人的運動場景。以機器人為中心的局部地圖維護對接觸高度檢測的精度有重要影響。未來，我們將更加關注地圖一致性的改進，以進一步提升接觸高度檢測和點云配準的精度。
本文僅做學術分享，如有侵權，請聯系刪文！
