清華大學(xué)李升波教授團(tuán)隊(duì)在IEEE Robotics and Automation Letters發(fā)表論文《Robust State Estimation for Legged Robots With Dual Beta Kalman Filter》。研究提出雙β-卡爾曼濾波器（Dual β-KF），用于解決足式機(jī)器人狀態(tài)估計(jì)中腳部打滑與腿長(zhǎng)變化帶來(lái)的誤差問(wèn)題。
在真實(shí)實(shí)驗(yàn)驗(yàn)證階段，NOKOV度量動(dòng)作捕捉系統(tǒng)提供足式機(jī)器人真實(shí)位姿數(shù)據(jù)，用于精度對(duì)比與魯棒性驗(yàn)證。
研究背景：足式機(jī)器人狀態(tài)估計(jì)為何需要魯棒算法？
現(xiàn)有依賴本體感知傳感器的足式機(jī)器人狀態(tài)估計(jì)算法，常因忽略腳部打滑和腿部變形而產(chǎn)生較大誤差。為消除這一限制，研究團(tuán)隊(duì)提出雙重魯棒估計(jì)框架Dual β-KF：
獨(dú)立估計(jì)腿長(zhǎng)參數(shù)的靜力學(xué)參數(shù)濾波器
屏蔽腳部打滑離群數(shù)據(jù)的β-KF狀態(tài)濾波器
該方法面向足式機(jī)器人狀態(tài)估計(jì)核心難題展開。
方法創(chuàng)新：雙β卡爾曼濾波器（Dual β-KF）如何提升足式機(jī)器人狀態(tài)估計(jì)精度？
雙重估計(jì)框架：參數(shù)濾波器 + 狀態(tài)濾波器
研究團(tuán)隊(duì)提出雙重估計(jì)框架：
參數(shù)濾波器估計(jì)腿長(zhǎng)
狀態(tài)濾波器估計(jì)機(jī)器人位姿
圖1：Dual β-KF框架概覽。內(nèi)部感知傳感器數(shù)據(jù)輸入模型后，與狀態(tài)無(wú)關(guān)的參數(shù)濾波器首先估計(jì)腿長(zhǎng)參數(shù)，估計(jì)結(jié)果隨后被魯棒狀態(tài)濾波器用于確定機(jī)器人的狀態(tài)。
為避免誤差累積，參數(shù)濾波器構(gòu)建腿部靜力學(xué)測(cè)量模型，僅通過(guò)關(guān)節(jié)扭矩和腳部接觸力估計(jì)腿長(zhǎng)，不受狀態(tài)信息干擾。
該設(shè)計(jì)增強(qiáng)了足式機(jī)器人狀態(tài)估計(jì)魯棒性。
β-卡爾曼濾波器如何抑制腳部打滑誤差
研究團(tuán)隊(duì)將腳部打滑視為測(cè)量離群值。
將卡爾曼濾波器重新解釋為基于KL散度的MAP估計(jì)問(wèn)題，并使用更魯棒的β-散度重新定義損失函數(shù)，開發(fā)出β-卡爾曼濾波器（β-KF）。
該算法有效降低腳部打滑對(duì)四足機(jī)器人位姿獲取精度的影響。
最終形成雙β卡爾曼濾波器（Dual β-KF）。
仿真與真實(shí)實(shí)驗(yàn)均驗(yàn)證其優(yōu)于最先進(jìn)本體感知狀態(tài)估計(jì)算法。
實(shí)驗(yàn)設(shè)計(jì)：仿真與真實(shí)機(jī)器人位姿驗(yàn)證方法
Gazebo仿真驗(yàn)證
研究團(tuán)隊(duì)首先在Gazebo仿真環(huán)境中驗(yàn)證算法有效性。
為驗(yàn)證狀態(tài)濾波器β-KF，選?。篞EKF，UKFOR，IEKF，Weight-KF
為驗(yàn)證腿部靜力學(xué)參數(shù)濾波器，對(duì)比：
QEKF
QEKF with [11]
Dual QEKF
Dual β-KF
宇樹GO2真實(shí)實(shí)驗(yàn)與NOKOV度量動(dòng)作捕捉位姿對(duì)比
真實(shí)實(shí)驗(yàn)在宇樹GO2機(jī)器人上進(jìn)行。
機(jī)器人在平坦地面小跑，采集數(shù)據(jù)包括：
1個(gè)IMU
12個(gè)關(guān)節(jié)編碼器與扭矩傳感器
4個(gè)腳部接觸力傳感器
圖2：用于真實(shí)實(shí)驗(yàn)的足式機(jī)器人，推翻了不產(chǎn)生腳部打滑和腿長(zhǎng)恒定的假設(shè)。
圖3：NOKOV度量動(dòng)作捕捉系統(tǒng)采集足式機(jī)器人實(shí)時(shí)位姿數(shù)據(jù)，與算法預(yù)測(cè)結(jié)果進(jìn)行對(duì)比。
NOKOV度量動(dòng)作捕捉系統(tǒng)提供機(jī)器人真實(shí)位姿數(shù)據(jù)，用于與算法預(yù)測(cè)結(jié)果進(jìn)行對(duì)比，完成足式機(jī)器人狀態(tài)估計(jì)精度驗(yàn)證。
在真實(shí)實(shí)驗(yàn)階段，NOKOV度量動(dòng)作捕捉作為高精度位姿基準(zhǔn)系統(tǒng)，驗(yàn)證Dual β-KF算法在真實(shí)環(huán)境下的魯棒性與精度提升效果。
實(shí)驗(yàn)結(jié)果：Dual β-KF相比主流狀態(tài)估計(jì)算法提升多少？
狀態(tài)濾波器驗(yàn)證結(jié)果：
QEKF誤差最大
UKF-OR在某些時(shí)刻忽略測(cè)量更新產(chǎn)生誤差
Dual β-KF顯著優(yōu)于IEKF和Weight-KF
腿部參數(shù)濾波驗(yàn)證結(jié)果：
Dual QEKF、Dual β-KF優(yōu)于QEKF
優(yōu)于動(dòng)態(tài)批量校準(zhǔn)QEKF
Dual β-KF精度最高
與基線QEKF相比，Dual β-KF精度提升超過(guò)40%。
仿真與真實(shí)實(shí)驗(yàn)軌跡對(duì)比中，Dual β-KF估計(jì)軌跡更接近真實(shí)軌跡。
表1：不同環(huán)境下的實(shí)驗(yàn)結(jié)果
圖4：仿真（左）和真實(shí)實(shí)驗(yàn)（右）中不同算法的位置估計(jì)結(jié)果。圖中僅顯示Dual β-KF算法和兩個(gè)基線濾波器（QEKF、UKF-OR）的曲線。可以看出，Dual β-KF算法的估計(jì)軌跡更接近真實(shí)軌跡。
真實(shí)軌跡由NOKOV度量動(dòng)作捕捉系統(tǒng)提供高精度位姿基準(zhǔn)。
NOKOV度量動(dòng)作捕捉在足式機(jī)器人狀態(tài)估計(jì)驗(yàn)證中的作用
在本研究中，NOKOV度量動(dòng)作捕捉系統(tǒng)承擔(dān)核心角色：
提供真實(shí)實(shí)驗(yàn)位姿基準(zhǔn)
作為魯棒狀態(tài)估計(jì)算法驗(yàn)證標(biāo)準(zhǔn)
支持仿真與真實(shí)場(chǎng)景對(duì)比
對(duì)于從事足式機(jī)器人狀態(tài)估計(jì)、四足機(jī)器人位姿驗(yàn)證、魯棒卡爾曼濾波算法研究的科研團(tuán)隊(duì)而言，動(dòng)作捕捉系統(tǒng)是算法精度驗(yàn)證的重要基礎(chǔ)設(shè)施。
FAQ：足式機(jī)器人狀態(tài)估計(jì)與實(shí)驗(yàn)驗(yàn)證關(guān)鍵問(wèn)題解析
Q1：足式機(jī)器人狀態(tài)估計(jì)為什么容易出現(xiàn)誤差？
足式機(jī)器人狀態(tài)估計(jì)依賴IMU、關(guān)節(jié)編碼器、扭矩傳感器與足端接觸力等本體傳感器數(shù)據(jù)。但傳統(tǒng)算法通?；趦蓚€(gè)隱含假設(shè)：
足端不打滑
腿長(zhǎng)保持恒定
真實(shí)運(yùn)動(dòng)過(guò)程中，腳部打滑會(huì)產(chǎn)生測(cè)量離群值，腿部結(jié)構(gòu)微變形會(huì)導(dǎo)致模型參數(shù)變化，從而破壞濾波器的狀態(tài)觀測(cè)模型，導(dǎo)致位姿漂移。
因此，魯棒狀態(tài)估計(jì)成為足式機(jī)器人控制與導(dǎo)航的關(guān)鍵問(wèn)題。
Q2：Dual β-KF如何提升足式機(jī)器人狀態(tài)估計(jì)魯棒性？
Dual β-KF通過(guò)“雙重估計(jì)框架”解決誤差來(lái)源問(wèn)題：
參數(shù)濾波器：實(shí)時(shí)估計(jì)腿長(zhǎng)變化
狀態(tài)濾波器（β-KF）：抑制腳部打滑引起的離群測(cè)量
其中β-卡爾曼濾波器通過(guò)引入β-散度替代傳統(tǒng)KL散度，使濾波器對(duì)異常值不敏感。
實(shí)驗(yàn)結(jié)果表明，在仿真與真實(shí)場(chǎng)景中，Dual β-KF相比QEKF精度提升超過(guò)40%。
Q3：對(duì)于研究足式機(jī)器人狀態(tài)估計(jì)的團(tuán)隊(duì)來(lái)說(shuō)，動(dòng)作捕捉系統(tǒng)的價(jià)值是什么？
對(duì)于開展以下研究方向的團(tuán)隊(duì)：
足式機(jī)器人狀態(tài)估計(jì)
四足機(jī)器人位姿獲取
魯棒卡爾曼濾波算法
具身智能機(jī)器人控制
高精度動(dòng)作捕捉系統(tǒng)不僅是驗(yàn)證工具，更是算法研發(fā)階段的“外部參考真值平臺(tái)”。
它可以：
發(fā)現(xiàn)模型假設(shè)失效場(chǎng)景
定位誤差來(lái)源
評(píng)估不同濾波結(jié)構(gòu)差異
在本文研究中，在Dual β-KF真實(shí)實(shí)驗(yàn)驗(yàn)證階段，NOKOV度量動(dòng)作捕捉系統(tǒng)提供高精度位姿真值，構(gòu)成算法性能評(píng)估的基準(zhǔn)標(biāo)準(zhǔn)。正是實(shí)現(xiàn)這一驗(yàn)證閉環(huán)的關(guān)鍵基礎(chǔ)設(shè)施。
引用格式與作者簡(jiǎn)介
引用格式
T. Zhang, W. Cao, C. Liu, T. Zhang, J. Li and S. E. Li, "Robust State Estimation for Legged Robots With Dual Beta Kalman Filter," in IEEE Robotics and Automation Letters, vol. 10, no. 8, pp. 7955-7962, Aug. 2025, doi: 10.1109/LRA.2025.3579619.
作者簡(jiǎn)介
張?zhí)煲?，清華大學(xué)車輛與運(yùn)載學(xué)院博士生，主要研究方向：機(jī)器人狀態(tài)估計(jì)與控制，強(qiáng)化學(xué)習(xí)等。
曹文涵，新加坡國(guó)立大學(xué)博士后，主要研究方向：狀態(tài)估計(jì)，機(jī)器人學(xué)習(xí)等。
劉暢，北京大學(xué)先機(jī)制造與機(jī)器人學(xué)院助理教授，博導(dǎo)。主要研究方向：具身智能，多機(jī)器人協(xié)同。
張濤，光象科技SunRisingAI Ltd CEO，主要研究方向：具身智能，機(jī)器人系統(tǒng)。
李江濤，光象科技SunRisingAI Ltd 首席科學(xué)家，主要研究方向：具身智能，機(jī)器人系統(tǒng)。
李升波（通訊作者），清華大學(xué)車輛與運(yùn)載學(xué)院、人工智能學(xué)院教授，博導(dǎo)。主要研究方向：自動(dòng)駕駛汽車、具身智能機(jī)器人、深度強(qiáng)化學(xué)習(xí)、神經(jīng)網(wǎng)絡(luò)設(shè)計(jì)、最優(yōu)控制與估計(jì)等。
