人形機器人動態行走層級控制框架

地形感知的人形機器人動態行走層級控制框架

Terrain-Aware Hierarchical Control Framework for Dynamic Locomotion of Humanoid Robots

https://www.mdpi.com/2079-9292/14/7/1264

摘要：

在復雜地形上實現動態行走能力，是人形機器人在工業制造和應急響應等應用中的關鍵需求。為應對非結構化環境中地形感知、欠驅動動力學規劃與落足點控制等根本性挑戰，本文提出了一種融合地形感知的分層規劃與控制框架。該框架首先對地形進行分割，生成表征地形特征的凸多邊形約束；隨后，基于非線性模型預測控制構建優化模型，將欠驅動動力學與地形約束整合為一個多目標最優控制問題，并采用序列二次規劃實現實時求解。此外，采用了一種分層全身控制方法，通過優先級任務分配與二次規劃，在多重任務與約束下實現協調的全身控制。我們在MuJoCo仿真平臺上開展了前向行走、外力擾動以及復雜地形行走的仿真實驗，驗證了所提方法的有效性。仿真結果表明，機器人可實現1 m/s的穩定行走速度，分別具備60 Ns的前向抗擾能力和30 Ns的側向抗擾能力，并能穩定跨越高度差為0.16 m的臺階及隨機地形。這些結果驗證了所提方法在動態性能、魯棒性與地形適應性方面的優勢。

關鍵詞：人形機器人；非線性模型預測控制；基于視覺的行走；全身控制；落足點控制

1. 引言

足式機器人的終極目標是在復雜、非結構化且未知的環境中實現自主移動與作業，這對機器人的感知、規劃與控制算法提出了嚴苛要求 [1–4]。人形機器人憑借其類人的結構與運動模式，在非結構化環境中可通過自適應落足點選擇、步態調整和速度調節，有效降低顛簸、打滑和卡滯等風險。其在復雜環境中的強大移動能力，使其在災難救援、工業自動化和太空探索等場景中具有廣泛應用前景。然而，人形機器人的運動規劃與控制因其高自由度而本質上十分復雜，而在此類環境中對精確地形感知的依賴進一步加劇了其研究與實際部署的挑戰。

當前基于模型的人形機器人行走方法主要集中在兩個方面：(a) 利用靜態步態的感知型行走 [5–9]；(b) 假設地形平坦的盲視動態行走 [10–12]。盡管近年來基于學習的控制框架取得了顯著進展，展現出強大的泛化能力，能夠在無需顯式地形模型的情況下實現對多種復雜地形的魯棒行走 [13–18]，但針對人形機器人的視覺引導強化學習研究仍較為有限 [19,20]。盡管如此，在崎嶇地形上實現精確落足控制的地形感知模塊集成仍是一個重大挑戰。這一集成挑戰具體表現為三個瓶頸：計算高效的地形表征、非結構化環境中的最優落足點選擇，以及欠驅動系統動力學的協同規劃。

在落足點選擇方面，已有若干研究將感知信息融入落足點選擇算法 [21–23]，采用一種分層框架，其中落足位置與軀干運動被獨立優化。在此框架中，高層模塊結合地形信息選擇落足點，低層模塊則基于機器人動力學模型優化軀干運動。盡管這種分層方法降低了計算復雜度，但需要人工對齊這兩個模塊。此外，落足點與軀干運動的獨立優化往往難以充分考慮軀干與腿部之間的運動學約束，以及腿部與地形的碰撞規避問題。

相比之下，一些研究提出了落足位置與軀干運動的聯合優化方法，在軌跡優化過程中同時考慮軀干與腿部的運動，取得了良好效果 [24–26]。該方法無需人工處理腿與軀干之間的耦合運動，并允許將更全面的地形信息整合到優化過程中。然而，由于模型的高維特性，特別是當引入感知信息時，軀干與腿部運動的聯合優化會帶來顯著的計算復雜度，可能導致求解時間過長，難以實現實時部署。

高程圖（elevation maps）在地形感知中起著關鍵作用，被廣泛應用于足式機器人的導航與控制 [27–29]。基于搜索或采樣技術的算法可利用高程圖提供的地面高度信息識別穩定的落足位置。在基于優化的控制算法中，高程圖通常作為輸入，與其他約束條件結合，用于優化機器人的運動或路徑規劃。例如，Winkler 等人 [25] 利用高程圖進行落足點選擇和碰撞規避，通過等式約束將擺動腿軌跡的起點和終點限制在地形表面，并通過不等式約束防止擺動腿與地形發生碰撞。然而，該方法未考慮地形的不連續性與非凸性，可能導致優化問題收斂至局部極小值，從而限制其在復雜環境中的有效性。

在軌跡優化領域，諸如 SNOPT [30] 和 IPOPT [31] 等大規模非線性求解器被廣泛采用 [32,33]。這些求解器能夠處理非線性優化問題中固有的復雜約束和目標函數，但通常需要大量迭代計算才能找到最優解。作為替代方案，一些專用于軌跡優化的求解器也被提出，例如迭代線性二次調節器（iLQR）[34,35] 和序列線性二次（SLQ）方法 [36]。這些方法是微分動態規劃（DDP）[37] 的變體，能夠在相對較短的時間內高效計算近似最優解，特別適用于高維問題。然而，這些方法仍需對系統進行線性化，可能在計算過程中引入近似誤差，從而潛在地影響解的精度。

為應對上述挑戰，本文提出了一種融合地形感知的分層規劃與控制框架。如圖1所示，該框架包含三層架構：感知層、規劃層和控制層。

• 感知層：從三維點云數據構建高精度高程圖，通過地形分割提取可行區域，并生成凸多邊形形式的地形約束。
• 運動規劃層：基于非線性模型預測控制（MPC），構建一個融合欠驅動動力學約束與地形約束的多目標優化問題，并采用序列二次規劃（SQP）算法在10至20毫秒內實時生成步態與軌跡。
• 全身控制（WBC）層：采用分層優化策略，利用零空間投影與二次規劃協調多重任務與約束，實現動態平衡與軌跡跟蹤的協同優化。

這三層架構通過閉環反饋機制構成一個集成系統。本文的主要貢獻如下：? 基于非線性 MPC 的實時軌跡規劃：我們采用非線性 MPC 實現人形機器人的軌跡規劃，并通過 SQP 算法實現實時步態與軌跡生成。? “感知–規劃–控制”三重耦合的閉環架構：構建了具備三重耦合特性的閉環架構，實現了地形感知與欠驅動系統規劃及控制的深度融合。? 高性能行走能力：基于所提方法的仿真實驗表明，人形機器人具備高性能移動能力，包括 1 m/s 的高速行走、前向最大 60 Ns 與側向最大 30 Ns 的抗擾能力，以及跨越 0.16 m 高障礙物的能力，在復雜地形上的行走性能顯著提升。

1. 地形感知

為在復雜環境中建立地形的幾何模型，本節利用從RGB-D傳感器獲取的點云數據，構建一個融合了傳感器不確定性和機器人位姿不確定性的2.5D高程圖。該高程圖隨后被進一步分割，以提取關鍵的地形特征。

2.1 高程圖構建

構建以機器人為中心的局部高程圖需要將傳感器獲取的深度圖像投影到網格地圖上。與在慣性坐標系中定義的全局地圖不同，局部地圖隨著機器人運動持續更新，無需依賴絕對定位。為應對非結構化地形中深度測量和機器人運動引起的位姿變化所帶來的不確定性，我們采用文獻[38]提出的高程圖更新算法。

2.2 平面分割

地形分割的目標是基于地形參數識別可通行區域及其邊界。坡度與粗糙度作為表征地形的關鍵指標，分別反映地形的傾斜程度與不規則性。可行性分類通過局部表面傾角和局部粗糙度進行，其中后者通過標準差估算 [39]。對于大小為 N N 的鄰域，這些量按如下方式計算：

為更精確地定義平面，標準差閾值放寬至2.5厘米，以允許一定程度的地形起伏。同時，坡度約束收緊至15°，確保僅接受滿足這一更嚴格坡度要求的平面。最終的連通區域需經過腐蝕操作處理。通過提取邊緣，獲得各平面的二維非凸輪廓，并沿z軸投影到相應平面上，以定義平面內的邊界。

1. 運動規劃

在復雜地形中，足式機器人的運動規劃需要同時考慮動力學約束與物理約束。通過動態調整落足點和身體姿態，機器人可在不同地形條件下維持穩定性。本節描述如何將人形機器人的運動規劃問題轉化為一個最優控制問題，并將地形信息、動力學模型及其他物理約束整合到建模與求解過程中。

3.1 機器人定義

人形機器人的結構示意圖如圖3所示。每條腿配置六個自由度，每只手臂配置四個自由度。定義慣性坐標系 I：其 x 軸沿機器人前進方向，z 軸垂直向上，y 軸按右手定則確定。浮動基座坐標系 B 固定于機器人本體上，原點位于本體上的某一特定點；初始時，B 的朝向與慣性坐標系 I 對齊。質心坐標系 G 的原點與機器人質心重合，其朝向始終與慣性坐標系 I 保持一致。

3.2 動力學模型

機器人的狀態 x x 和輸入 u u 定義如下：

根據質心動量動力學，作用于機器人的外力等效于質心動量的變化率，其表達式如下：

3.3 軌跡規劃
3.3.1 浮動基座軌跡規劃

3.3.2 落足點規劃

若不考慮地形因素，名義落足點可通過整合步態信息與浮動基座軌跡來確定。基于 Raibert 提出的啟發式落足點理論 [40]，名義落足點可表示如下：

3.4 非線性模型預測控制

在本研究中，人形機器人的控制問題被表述為一個最優控制問題，表達如下：

3.4.2 約束條件

對于支撐腿的四個接觸點，每個接觸點必須位于由凸多邊形所圍成的三維空間內。該空間由 N 個邊界曲面圍成，每個邊界曲面均垂直于凸多邊形所在平面，并穿過凸多邊形的一條邊，如圖 4a 所示。接觸點同時位于這 N 個邊界曲面正側的條件可表示為如下向量形式：

在擺動相期間，為防止擺動足與環境發生碰撞，對足部的每個接觸點施加一個一維非線性不等式約束，表示如下：

1. 數值優化

方程(19)中的標準MPC源自連續時間公式，需要轉換為非線性優化問題以便進行數值求解。在本節中，采用直接多重打靶法將連續最優控制問題離散化為有限維非線性優化問題，隨后使用序列二次規劃在實時中求解。

4.1 離散化

4.2 順序二次規劃

對于非線性約束優化問題，SQP 方法將原問題分解為一系列二次規劃（QP）問題，并通過迭代方式求解。在每一步迭代中，求解一個 QP 問題，其中目標函數是拉格朗日函數的二階近似，而約束條件是原始約束的線性近似。

對應于方程 (31) 的拉格朗日函數由以下給出：

1. 整體運動控制

高自由度的人形機器人在執行過程中必須同時滿足多任務的需求。為了解決這些挑戰，本研究采用了一種基于分層優化的整體運動控制方法。該方法根據任務優先級將控制問題分解為多個層次，通過優化方法解決任務沖突和約束沖突，實現整體協調控制，提高機器人執行多任務的能力。

5.1 任務構建

完整的任務被分為兩層，如表1中詳細說明。動力學方程，提供了機器人運動和力之間關系的基本描述，被賦予最高優先級。為了防止因扭矩飽和而對硬件造成物理損壞，關節扭矩限制也被視為最高優先級任務。對于欠驅動系統，運動所需的外力完全來源于腳與地面之間的接觸力。保持腳與地面的接觸是所有其他任務的前提，因此腳部接觸任務也被賦予最高優先級。當腳與地面接觸時，必須考慮摩擦錐任務，而對于擺動的腳，其接觸力任務必須被解決。

第二層任務的主要目標是在滿足機器人動力學和物理約束的前提下，跟蹤由MPC規劃模塊生成的軌跡（包括動量、關節空間角度、工作空間位姿以及接觸力軌跡）。在確保第一層任務完成的前提下，需調整機器人的質心位置和浮動基座朝向，以實現期望的運動，即線性動量任務和浮動基座朝向任務。當機器人行走時，必須控制擺動腿的足端位姿以跟蹤優化后的軌跡。此外，為防止行走過程中雙腿發生自碰撞，引入了自碰撞避免任務；為避免運動過程中超出關節位置限制，還加入了關節限位任務。接觸力正則化任務旨在保證接觸力的合理分布，而手臂關節角度任務則用于跟蹤手臂關節的參考軌跡。

5.2 分層優化

上述每一項分層任務均可描述為如下標準形式：

1. 仿真與結果

為驗證所提出框架的有效性，本研究利用MuJoCo仿真平臺評估一臺機器人（身高：1.5米，總質量：48公斤）的運動能力。我們的實驗方案旨在量化三個關鍵性能指標：動態穩定性、在擾動場景下的魯棒性，以及在非結構化地形中的適應能力。所開展的具體實驗包括以下內容：

(a) 前向行走：本實驗旨在評估機器人的動態穩定性和運動效率。機器人被要求在平坦地面上向前行走，其步態模式、平衡維持能力和速度一致性將被分析，以評估其動態性能。

(b) 外力擾動：本實驗旨在測試機器人對外部擾動的魯棒性。在機器人運動過程中，對其不同部位施加外力，并觀察其恢復平衡并維持穩定運動的能力。該測試突出了控制框架應對意外擾動的能力。

(c) 復雜地形行走：本實驗旨在評估機器人對不平整且具有挑戰性地形的適應能力。機器人需穿越具有不同坡度、臺階和障礙物的表面。通過分析其調整步態、維持穩定性和避免碰撞的能力，展示其對真實世界環境的適應性。

這些仿真實驗共同提供了對機器人運動控制框架的全面評估，確保其在動態、魯棒和自適應場景中的有效性。仿真視頻包含在補充材料中。

6.1 前向行走

全身運動控制算法與狀態估計以500 Hz的頻率運行。MPC算法以50 Hz的頻率運行，預測時域為1秒，離散時間步長為0.015秒。采用基于HPIPM [45] 的SQP求解器 [44]，其收斂容差設為1×10??，且每個優化周期最多執行一次迭代。為確保計算效率，為SQP計算分配了三個線程。仿真時間步長設為0.002秒。在無外部擾動且處于平坦地面的情況下，機器人在X方向上的期望速度被設定為從0 m/s逐步加速至1 m/s，隨后再減速回0 m/s。

圖5展示了機器人浮動基座的速度跟蹤曲線。機器人成功跟隨了指令的加速、勻速和減速過程。在1 m/s時的最大速度跟蹤誤差為0.07 m/s，而Y方向的速度波動保持在0.05 m/s以內。圖6顯示了機器人行走過程中關節力矩與速度的關系曲線，可作為電機選型的依據。在整個過程中，髖關節俯仰（Hip Pitch）關節表現出最大力矩，約為200 Nm。關節速度通常較低，均低于4 rad/s。

6.2 外部擾動

在展示基本行走能力之后，進一步評估機器人在外部力擾動下的穩定性。在仿真環境中，機器人被指令執行原地踏步動作，同時通過施加脈沖力來模擬外部擾動。脈沖力的幅值設定為120 N，持續時間范圍從0.2秒至0.5秒。

圖7a展示了機器人浮動基座的速度曲線。前三個脈沖力的持續時間為0.2秒，后三個脈沖力的持續時間為0.5秒。觀察發現，在經歷24 Ns的沖量后，機器人的速度出現短暫波動，峰值達到0.6 m/s；當承受60 Ns的沖量時，機器人的速度波動更為顯著，峰值達到1.1 m/s。在此情況下，機器人通過連續向前邁出兩步來調整其速度，如圖7b中的相應時序圖所示。該抗擾能力（前向推力沖量：60 Ns，機器人質量：48 kg）與在iCub機器人上實現的基于學習的方法性能相當（前向推力沖量：40 Ns，機器人質量：33 kg）[46]。

圖7c描繪了在側向脈沖力作用下的速度曲線。鑒于側向抗擾能力弱于前向抗擾能力，三個側向脈沖力的幅值設定為60 N，每個持續時間為0.5秒。可以看出，在經歷側向沖量后，機器人的側向速度出現顯著波動，峰值達到1 m/s。機器人通過調整側向足端落點位置來調節其側向速度，如圖7d中的相應時序圖所示。值得注意的是，機器人在側向擾動后未出現腿部交叉現象，驗證了全身控制框架中自碰撞避免約束的有效性。

6.3 復雜地形行走實驗

為評估機器人對復雜地形的適應能力，仿真環境中集成了RGB-D傳感器和階梯地形。RGB-D傳感器用于獲取地形的深度信息。該階梯由四階臺階組成，每階高度為0.16米，寬度為0.5米。高程地圖配置尺寸為3米×3米，分辨率為0.04米。機器人被指令以0.36米/秒的速度向前移動，步態周期為0.8秒。

圖8a展示了機器人攀爬樓梯的時序圖。當機器人接近樓梯時，它自主調整基座高度并修改足端落點位置，使其與臺階表面對齊。為更直觀地呈現運動細節，圖8b展示了樓梯攀爬過程中足端落點規劃的時序圖。圖中顯示了兩個機器人模型：一個代表當前狀態（高透明度），另一個代表提前1秒的狀態（低透明度）。藍色點表示名義足端落點，綠色點表示投影足端落點。四個接觸點的擺動軌跡由不同顏色的曲線表示，對應投影足端落點的凸多邊形區域則由不同顏色的多邊形表示。如圖8b所示，當名義足端落點接近臺階邊緣時，投影足端落點會自動調整至當前臺階表面或下一階臺階表面。若投影足端落點位于下一階臺階表面，則擺動腿軌跡將從當前臺階表面平滑過渡至下一階臺階表面，確保樓梯攀爬過程無縫銜接。同時，機器人基座高度根據高程地圖動態調整，以與臺階高度保持一致。

圖9比較了考慮與未考慮擺動腿碰撞避免約束的仿真結果。當未考慮擺動腿碰撞避免約束時，由MPC優化得到的擺動足軌跡會與樓梯發生碰撞（如圖9a所示），導致機器人跌倒。相比之下，當引入擺動腿碰撞避免約束后，由MPC優化得到的擺動足軌跡會自動避開樓梯邊緣（如圖9b所示），確保擺動足不會與樓梯結構發生碰撞。

為進一步評估機器人在復雜環境中的地形適應能力，仿真環境中構建了一個20×20的網格地形，如圖10所示。每個網格單元尺寸為0.6米×0.6米，相鄰網格之間隨機分配高度差（最大可達0.16米）。對于較小的高度差，相鄰網格被歸類為共面區域，機器人可直接通行。在此類場景中，機器人的穩定性主要由控制器固有的魯棒性保障。相反，對于顯著的高度變化，足端落點規劃成為成功通行的關鍵決定因素。圖10展示了機器人穿越該隨機化地形的時間序列。結果表明，盡管相鄰網格之間存在隨機的高度變化，機器人仍通過自適應足端選擇與動態控制實現了穩定的運動。

1. 結論

本研究構建了一個全面的、具備地形感知能力的分層框架，有效打通了人形機器人在復雜環境中行走時的感知—規劃—控制閉環。通過對高程地圖進行分割以提取凸多邊形地形特征，構建了一種受地形約束的非線性模型預測控制（MPC）形式，該形式通過凸多邊形約束實現實時足端落點自適應。序列二次規劃（SQP）算法確保以50 Hz的更新頻率實時生成步態與軌跡，而基于分層優化的全身控制策略則有效協調了動態平衡與軌跡跟蹤任務。所提出的框架已在仿真中得到驗證，顯著提升了機器人在運動性、魯棒性以及地形適應性方面的性能，為解決人形機器人在非結構化環境中動態運動控制的挑戰提供了新方案。

本工作中的地形感知基于高程地圖技術，在面對沙地、茂密植被或尖銳巖石等極端地形時可能存在固有局限。對于這些高度不規則或可變形的表面，難以從中提取有效的凸多邊形特征。此外，在真實機器人上的部署還面臨諸多挑戰，包括傳感器噪聲與延遲、執行器非線性特性，以及仿真與物理系統動力學之間的差異。未來的研究需融合多模態感知手段（例如基于學習的方法[47]），以實現更精確且魯棒的地形重建，并通過硬件實驗驗證，彌合仿真與現實部署之間的差距。

原文鏈接： https://www.mdpi.com/2079-9292/14/7/1264