1. 腿部運動學-慣性里程計

結合機器人腿部運動學和IMU數據，提出了一種基于誤差狀態卡爾曼濾波器（ESKF）的腿部里程計算法：

• 狀態預測：利用運動學模型預測機器人在空間中的位置、速度和姿態。
• 狀態更新：融合足端速度、足端位置和接觸高度檢測等多種觀測，減少由于足部沖擊引起的高度波動和漂移。
• 接觸高度檢測：通過機器人中心的增量地圖計算足端接觸點的高度，有效降低動態運動中高度方向的累積誤差。

2. LiDAR里程計

為了適應動態運動帶來的點云失真問題，提出了以下創新方法：

• 自適應掃描切片與拼接：根據機器人運動速度動態調整LiDAR掃描切片的角度，拼接當前和歷史掃描切片以生成高頻的去畸變點云。
• 點云匹配與優化：使用點到線和點到面的匹配算法對點云進行優化，提升位姿估計的精度。
• 增量式地圖維護：采用增量kd-tree技術動態管理局部地圖，通過高效添加與移除點云，實現對大規模點云的實時維護。

3. 因子圖優化

通過因子圖緊密耦合腿部里程計、LiDAR里程計和回環檢測：

• 因子圖結構：整合腿部里程計的相對位姿、LiDAR里程計的相對位姿以及回環檢測的匹配信息，形成優化目標。
• 回環檢測：基于距離約束的回環檢測方法，選擇匹配幀對進行閉環優化，進一步提升全局定位精度。

4. 系統流程

1. 使用腿部運動學和IMU數據進行預測和初步狀態估計。
2. 通過自適應掃描與拼接生成高頻點云，優化點云匹配結果。
3. 將腿部里程計、LiDAR里程計及回環檢測結果融合到因子圖中，實現全局優化。
4. 維護增量式局部地圖，確保實時性和計算效率。

實驗仿真結果

本文提出的方法與其他方法的相對位姿誤差（RPE）的比較

Comparison of trajectory for various methods on parking.

在平地上跑步時，不同方法對高度變化的比較。跑步時，車身高度會略有下降，不會超過圖中的灰色區域。

untime comparison on sequence parking.

總結 & 展望

本文提出了一種基于運動學-慣性-LiDAR的里程計方法——Leg-KILO。通過引入腿部運動學約束，增強了基于LiDAR里程計的穩定性。此外，提出了一種自適應掃描切片與拼接方法，以緩解動態運動的影響。同時，采用一種以機器人為中心的增量式局部地圖維護方法，顯著降低了地圖維護的時間成本。通過因子圖優化，將腿部里程計、LiDAR里程計和回環檢測因子進行整合，以提高定位精度。

本文的方法在多種高動態運動環境下（采用快步運動方式）進行了廣泛的測試，結果表明，與其他先進的LiDAR方法相比，Leg-KILO更適合動態四足機器人的運動場景。以機器人為中心的局部地圖維護對接觸高度檢測的精度有重要影響。未來，我們將更加關注地圖一致性的改進，以進一步提升接觸高度檢測和點云配準的精度。

本文僅做學術分享，如有侵權，請聯系刪文！