導(dǎo)讀

本文提出了一種名為Leg-KILO（Kinematic-Inertial-Lidar Odometry）的多傳感器融合框架，專為動(dòng)態(tài)足式機(jī)器人設(shè)計(jì)。為解決高動(dòng)態(tài)運(yùn)動(dòng)導(dǎo)致的IMU漂移和LiDAR失真問題，該方法緊密耦合腿部里程計(jì)、慣性測(cè)量單元（IMU）和LiDAR數(shù)據(jù)，并結(jié)合圖優(yōu)化實(shí)現(xiàn)回環(huán)檢測(cè)。通過提出基于誤差狀態(tài)卡爾曼濾波器的腿部運(yùn)動(dòng)學(xué)慣性里程計(jì)、高動(dòng)態(tài)自適應(yīng)掃描分割方法和機(jī)器人中心增量建圖，顯著提高了高度和位置估計(jì)的精度。實(shí)驗(yàn)表明，Leg-KILO在室內(nèi)外環(huán)境中的漂移顯著小于現(xiàn)有的LiDAR方法，尤其在高動(dòng)態(tài)運(yùn)動(dòng)中表現(xiàn)出優(yōu)越的魯棒性。研究還公開了相關(guān)數(shù)據(jù)集和代碼以供社區(qū)使用。

論文信息

• 標(biāo)題：Leg-KILO: Robust Kinematic-Inertial-Lidar Odometry for Dynamic Legged Robots
• 作者：Guangjun Ou , Dong Li , and Hanmin Li
• 論文鏈接：https://ieeexplore.ieee.org/document/10631676
• 項(xiàng)目地址：https://github.com/ouguangjun/Leg-KILO

動(dòng)機(jī)(Motivation)

本文的研究動(dòng)機(jī)源于動(dòng)態(tài)足式機(jī)器人在高動(dòng)態(tài)運(yùn)動(dòng)中狀態(tài)估計(jì)面臨的挑戰(zhàn)，以及現(xiàn)有方法的局限性：

1. 高動(dòng)態(tài)運(yùn)動(dòng)中的問題：

• 動(dòng)態(tài)足式機(jī)器人（如四足機(jī)器人）在高動(dòng)態(tài)運(yùn)動(dòng)（如跑步或快步）時(shí)，足部沖擊頻繁，導(dǎo)致慣性測(cè)量單元（IMU）數(shù)據(jù)退化，尤其是加速度計(jì)的漂移。
• LiDAR掃描會(huì)受到運(yùn)動(dòng)失真影響，進(jìn)一步導(dǎo)致基于LiDAR的SLAM系統(tǒng)難以穩(wěn)定運(yùn)行。
• IMU與LiDAR在高動(dòng)態(tài)環(huán)境中容易累積漂移，尤其是在高度（Z軸）方向。

2. 現(xiàn)有方法的不足：

• 單純依賴IMU或LiDAR的狀態(tài)估計(jì)方法在高動(dòng)態(tài)運(yùn)動(dòng)中誤差較大，不能滿足動(dòng)態(tài)足式機(jī)器人導(dǎo)航的需求。
• 許多現(xiàn)有方法側(cè)重于中低速運(yùn)動(dòng)場(chǎng)景，而忽視了在高速動(dòng)態(tài)環(huán)境下狀態(tài)估計(jì)的精度和魯棒性。
• 一些僅基于腿部運(yùn)動(dòng)學(xué)和慣性測(cè)量的估計(jì)方法容易在長時(shí)間運(yùn)動(dòng)中累積顯著漂移，且在發(fā)生足部滑動(dòng)等突發(fā)狀況時(shí)可能失效。

3. 解決這一問題的必要性：

• 提升足式機(jī)器人在動(dòng)態(tài)、高速場(chǎng)景中的狀態(tài)估計(jì)精度和魯棒性是實(shí)現(xiàn)其在復(fù)雜環(huán)境中自主導(dǎo)航和控制的關(guān)鍵。
• 需要整合多傳感器（如腿部運(yùn)動(dòng)學(xué)、IMU、LiDAR）的信息，發(fā)揮各自優(yōu)勢(shì)，彌補(bǔ)單一傳感器的局限性。

基于以上背景，本文提出了Leg-KILO框架，旨在通過多傳感器融合和高效的優(yōu)化方法，解決動(dòng)態(tài)足式機(jī)器人在高動(dòng)態(tài)運(yùn)動(dòng)中的狀態(tài)估計(jì)問題，推動(dòng)相關(guān)領(lǐng)域的發(fā)展。

創(chuàng)新點(diǎn)

1. 腿部運(yùn)動(dòng)學(xué)-慣性里程計(jì)的改進(jìn)：提出了一種基于誤差狀態(tài)卡爾曼濾波器（ESKF）的腿部運(yùn)動(dòng)學(xué)-慣性里程計(jì)方法，結(jié)合接觸高度檢測(cè)約束，有效減少了由足部沖擊引起的高度波動(dòng)。

2. 自適應(yīng)掃描切片與拼接：針對(duì)高動(dòng)態(tài)運(yùn)動(dòng)帶來的LiDAR掃描失真問題，提出了一種自適應(yīng)掃描切片與拼接方法，根據(jù)機(jī)器人運(yùn)動(dòng)速度動(dòng)態(tài)調(diào)整掃描角度，提高了輸入頻率的同時(shí)降低了運(yùn)動(dòng)失真。

3. 機(jī)器人中心增量地圖：設(shè)計(jì)了一種機(jī)器人中心的增量式局部地圖維護(hù)方法，通過增量kd-tree技術(shù)高效處理點(diǎn)云的添加和刪除，減少了地圖維護(hù)的計(jì)算開銷。

4. 多傳感器緊密耦合與回環(huán)優(yōu)化：通過圖優(yōu)化框架將腿部里程計(jì)、LiDAR里程計(jì)以及回環(huán)檢測(cè)緊密耦合，從而提升了系統(tǒng)的全局定位精度。

5. 全面的實(shí)驗(yàn)驗(yàn)證與數(shù)據(jù)共享：在室內(nèi)外高動(dòng)態(tài)環(huán)境下驗(yàn)證了方法的魯棒性和精確性，結(jié)果優(yōu)于其他LiDAR慣性里程計(jì)方法，同時(shí)公開了包含腿部運(yùn)動(dòng)學(xué)數(shù)據(jù)的LiDAR慣性數(shù)據(jù)集和代碼，促進(jìn)了社區(qū)研究。

本文核心算法

System overview.

本文的核心算法圍繞Leg-KILO框架，融合腿部運(yùn)動(dòng)學(xué)、慣性測(cè)量單元（IMU）和LiDAR數(shù)據(jù)，通過創(chuàng)新的算法模塊提升動(dòng)態(tài)足式機(jī)器人在高動(dòng)態(tài)環(huán)境中的狀態(tài)估計(jì)能力。以下是算法的主要組成：

1. 腿部運(yùn)動(dòng)學(xué)-慣性里程計(jì)

結(jié)合機(jī)器人腿部運(yùn)動(dòng)學(xué)和IMU數(shù)據(jù)，提出了一種基于誤差狀態(tài)卡爾曼濾波器（ESKF）的腿部里程計(jì)算法：

• 狀態(tài)預(yù)測(cè)：利用運(yùn)動(dòng)學(xué)模型預(yù)測(cè)機(jī)器人在空間中的位置、速度和姿態(tài)。
• 狀態(tài)更新：融合足端速度、足端位置和接觸高度檢測(cè)等多種觀測(cè)，減少由于足部沖擊引起的高度波動(dòng)和漂移。
• 接觸高度檢測(cè)：通過機(jī)器人中心的增量地圖計(jì)算足端接觸點(diǎn)的高度，有效降低動(dòng)態(tài)運(yùn)動(dòng)中高度方向的累積誤差。

2. LiDAR里程計(jì)

為了適應(yīng)動(dòng)態(tài)運(yùn)動(dòng)帶來的點(diǎn)云失真問題，提出了以下創(chuàng)新方法：

• 自適應(yīng)掃描切片與拼接：根據(jù)機(jī)器人運(yùn)動(dòng)速度動(dòng)態(tài)調(diào)整LiDAR掃描切片的角度，拼接當(dāng)前和歷史掃描切片以生成高頻的去畸變點(diǎn)云。
• 點(diǎn)云匹配與優(yōu)化：使用點(diǎn)到線和點(diǎn)到面的匹配算法對(duì)點(diǎn)云進(jìn)行優(yōu)化，提升位姿估計(jì)的精度。
• 增量式地圖維護(hù)：采用增量kd-tree技術(shù)動(dòng)態(tài)管理局部地圖，通過高效添加與移除點(diǎn)云，實(shí)現(xiàn)對(duì)大規(guī)模點(diǎn)云的實(shí)時(shí)維護(hù)。

3. 因子圖優(yōu)化

通過因子圖緊密耦合腿部里程計(jì)、LiDAR里程計(jì)和回環(huán)檢測(cè)：

• 因子圖結(jié)構(gòu)：整合腿部里程計(jì)的相對(duì)位姿、LiDAR里程計(jì)的相對(duì)位姿以及回環(huán)檢測(cè)的匹配信息，形成優(yōu)化目標(biāo)。
• 回環(huán)檢測(cè)：基于距離約束的回環(huán)檢測(cè)方法，選擇匹配幀對(duì)進(jìn)行閉環(huán)優(yōu)化，進(jìn)一步提升全局定位精度。

4. 系統(tǒng)流程

1. 使用腿部運(yùn)動(dòng)學(xué)和IMU數(shù)據(jù)進(jìn)行預(yù)測(cè)和初步狀態(tài)估計(jì)。
2. 通過自適應(yīng)掃描與拼接生成高頻點(diǎn)云，優(yōu)化點(diǎn)云匹配結(jié)果。
3. 將腿部里程計(jì)、LiDAR里程計(jì)及回環(huán)檢測(cè)結(jié)果融合到因子圖中，實(shí)現(xiàn)全局優(yōu)化。
4. 維護(hù)增量式局部地圖，確保實(shí)時(shí)性和計(jì)算效率。

實(shí)驗(yàn)仿真結(jié)果

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

Comparison of trajectory for various methods on parking.

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

untime comparison on sequence parking.

總結(jié) & 展望

本文提出了一種基于運(yùn)動(dòng)學(xué)-慣性-LiDAR的里程計(jì)方法——Leg-KILO。通過引入腿部運(yùn)動(dòng)學(xué)約束，增強(qiáng)了基于LiDAR里程計(jì)的穩(wěn)定性。此外，提出了一種自適應(yīng)掃描切片與拼接方法，以緩解動(dòng)態(tài)運(yùn)動(dòng)的影響。同時(shí)，采用一種以機(jī)器人為中心的增量式局部地圖維護(hù)方法，顯著降低了地圖維護(hù)的時(shí)間成本。通過因子圖優(yōu)化，將腿部里程計(jì)、LiDAR里程計(jì)和回環(huán)檢測(cè)因子進(jìn)行整合，以提高定位精度。

本文的方法在多種高動(dòng)態(tài)運(yùn)動(dòng)環(huán)境下（采用快步運(yùn)動(dòng)方式）進(jìn)行了廣泛的測(cè)試，結(jié)果表明，與其他先進(jìn)的LiDAR方法相比，Leg-KILO更適合動(dòng)態(tài)四足機(jī)器人的運(yùn)動(dòng)場(chǎng)景。以機(jī)器人為中心的局部地圖維護(hù)對(duì)接觸高度檢測(cè)的精度有重要影響。未來，我們將更加關(guān)注地圖一致性的改進(jìn)，以進(jìn)一步提升接觸高度檢測(cè)和點(diǎn)云配準(zhǔn)的精度。

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文！