最近科技圈值得關(guān)注的一條新聞和中國機(jī)器人創(chuàng)下的百米紀(jì)錄有關(guān)。很多人看到的是“機(jī)器人能跑了”但運(yùn)動(dòng)控制工程師眼中的東西不太一樣這更像一次全行業(yè)能力的整體攀峰把高維、強(qiáng)耦合、時(shí)變的物理系統(tǒng)穩(wěn)定控制到“跑起來”的程度中間涉及的硬件、算法、仿真和調(diào)試每一條都值得單獨(dú)寫一篇技術(shù)博客。更值得注意的是相關(guān)話題在技術(shù)社區(qū)里的搜索熱詞明顯升溫比如人形機(jī)器人、四足機(jī)器人、宇樹機(jī)器人電路板拆解、ROS2機(jī)器人開發(fā)、ABB/KUKA工業(yè)機(jī)器人調(diào)試等。這說明很多開發(fā)者已經(jīng)不滿足于圍觀新聞而是想搞清楚“跑步”背后到底有什么技術(shù)以及自己能不能快速入門。這篇文章不以復(fù)述新聞為目的只講技術(shù)鏈路。讀完你會(huì)掌握機(jī)器人跑步到底難在物理原理的哪一層跑步控制常用的 ZMP、LIPM、MPC、WBC、強(qiáng)化學(xué)習(xí)分別在干什么如何從零搭建一個(gè)機(jī)器人運(yùn)動(dòng)仿真環(huán)境并用 Python 寫一個(gè)可運(yùn)行的最小步態(tài)控制 Demo從仿真到真實(shí)機(jī)器人指標(biāo)驗(yàn)證、安全邊界和工程協(xié)作怎么設(shè)計(jì)。1. 機(jī)器人創(chuàng)下百米紀(jì)錄技術(shù)含量到底在哪1.1 跑步不是走路的進(jìn)階而是控制體系的遷移很多非技術(shù)背景的人覺得跑步就是走路的加速版。對(duì)機(jī)器人來說這個(gè)認(rèn)知偏差非常致命。走路時(shí)機(jī)器人幾乎全程保持至少一條腿與地面接觸地面反作用力持續(xù)支撐身體控制器的壓力相對(duì)小。跑步則完全不同它是一種“騰空相”與“觸地相”交替出現(xiàn)的周期運(yùn)動(dòng)。騰空時(shí)機(jī)器人雙腳離地沒有任何地面支撐只能依靠慣性和預(yù)期的落點(diǎn)規(guī)劃來維持狀態(tài)觸地時(shí)整個(gè)身體質(zhì)量產(chǎn)生的沖擊力又會(huì)在幾十毫秒內(nèi)通過關(guān)節(jié)吸收并轉(zhuǎn)化為下一步的推進(jìn)力。這意味著控制器的執(zhí)行周期必須足夠快通常要跑到 1kHz 甚至更高才能在短瞬內(nèi)完成狀態(tài)估計(jì)、力學(xué)解算和關(guān)節(jié)力矩輸出。對(duì)比工業(yè)機(jī)器人更容易理解這一點(diǎn)。在工廠場(chǎng)景里ABB、KUKA 這類工業(yè)機(jī)械臂的軌跡通常是確定且可重復(fù)的工作區(qū)域內(nèi)的人和障礙物也被嚴(yán)格控制。它們的核心是位置精度和重復(fù)定位精度而不是動(dòng)態(tài)平衡。而跑步機(jī)器人面對(duì)的環(huán)境充滿擾動(dòng)地面材質(zhì)、重心偏移、電池電壓下降都會(huì)直接影響控制效果。它更像在操控一個(gè)“隨時(shí)可能從自行車上摔下來的人”而不是在操作一臺(tái)精密機(jī)床。1.2 從“敢跑”到“跑得穩(wěn)”每個(gè)環(huán)節(jié)都是變量如果你看過機(jī)器人跑步的測(cè)試錄像會(huì)發(fā)現(xiàn)不少 Demo 并不是一帆風(fēng)順地跑完全程而是頻繁出現(xiàn)上半身大幅擺動(dòng)、落點(diǎn)偏移、觸地瞬間重心歪斜甚至摔倒在終點(diǎn)線之前。這背后的本質(zhì)原因只有一個(gè)跑步機(jī)器人是一個(gè)高維、強(qiáng)耦合的時(shí)變系統(tǒng)基于簡(jiǎn)單模型的控制策略無法覆蓋所有真實(shí)狀態(tài)邊界。工業(yè)界早年解決雙足行走用的是 ZMP 判據(jù)強(qiáng)調(diào)“壓力中心始終落在支撐多邊形內(nèi)”。這個(gè)方法在低速行走時(shí)很有效但在跑步這種強(qiáng)動(dòng)態(tài)工況下ZMP 方法很難直接滿足約束。于是研究者轉(zhuǎn)向了“模型預(yù)測(cè)控制 全身控制”的組合再后來加入強(qiáng)化學(xué)習(xí)讓策略本身學(xué)會(huì)在仿真中試錯(cuò)從而逼近更接近人類跑步的動(dòng)態(tài)動(dòng)作。這些技術(shù)研究最終共同構(gòu)成了“機(jī)器人跑步”這件事的底層支撐。百米紀(jì)錄的意義不是速度本身而是把高維動(dòng)態(tài)系統(tǒng)穩(wěn)定控制住的能力。這正是運(yùn)動(dòng)控制領(lǐng)域一次實(shí)打?qū)嵉墓こ掏黄啤?. 機(jī)器人跑步的核心概念與原理在繼續(xù)實(shí)操之前先把幾個(gè)繞不開的概念講清楚。每個(gè)術(shù)語我會(huì)先給通俗解釋再給出技術(shù)定義和它在跑步場(chǎng)景里的作用。2.1 自由度自由度是指機(jī)器人身上可以獨(dú)立運(yùn)動(dòng)的關(guān)節(jié)數(shù)量。人形機(jī)器人通常有十幾個(gè)到幾十個(gè)自由度四足機(jī)器人雖然腿少但每條腿也有多個(gè)主動(dòng)關(guān)節(jié)。通俗理解你手腕能轉(zhuǎn)、肘能彎、肩膀能抬這些可以獨(dú)立控制的旋轉(zhuǎn)/平移動(dòng)作就是自由度。跑步時(shí)機(jī)器人的自由度不僅“多”而且“耦合”——膝關(guān)節(jié)彎曲會(huì)改變質(zhì)心高度髖關(guān)節(jié)旋轉(zhuǎn)會(huì)影響前進(jìn)方向一個(gè)關(guān)節(jié)的力矩異常會(huì)通過動(dòng)態(tài)鏈傳遞到全身。自由度越多理論上動(dòng)作表達(dá)能力越強(qiáng)控制難度也越大。2.2 動(dòng)力學(xué)方程動(dòng)力學(xué)方程描述的是“關(guān)節(jié)力矩與運(yùn)動(dòng)狀態(tài)之間的數(shù)學(xué)關(guān)系”。簡(jiǎn)單說就是你給出每個(gè)關(guān)節(jié)應(yīng)該出多大的力方程會(huì)告訴你機(jī)器人會(huì)怎么動(dòng)。跑步控制里的動(dòng)力學(xué)方程比工業(yè)機(jī)械臂復(fù)雜得多。機(jī)械臂基座固定末端軌跡可以提前規(guī)劃跑步機(jī)器人基座是懸浮的地面反作用力、慣性力、科里奧利力全都要在毫秒級(jí)時(shí)間內(nèi)計(jì)算。實(shí)際工程項(xiàng)目中很少有人手推完整動(dòng)力學(xué)方程而是依賴物理引擎或自動(dòng)求導(dǎo)工具來生成運(yùn)動(dòng)方程。但核心思想沒有變控制器知道了“當(dāng)前狀態(tài)”需要算出“下一步該輸出多大關(guān)節(jié)力矩”才能讓機(jī)器人朝期望方向運(yùn)動(dòng)。2.3 ZMP 零力矩點(diǎn)ZMP 是雙足/四足機(jī)器人控制里最經(jīng)典的概念之一。它定義的是地面反作用力與重力合力的作用點(diǎn)這個(gè)點(diǎn)如果落在支撐多邊形之外機(jī)器人就會(huì)失去平衡開始翻轉(zhuǎn)。你可以把 ZMP 理解成“你站在滑板上的壓力中心”?；屐o止時(shí)你的重心垂直投影必須落在滑板范圍內(nèi)滑板移動(dòng)時(shí)你要不斷調(diào)整腳底壓力分布讓壓力中心始終留在可控區(qū)域內(nèi)。跑步時(shí)ZMP 約束從“全程有效”變成“只在支撐相有效”騰空相沒有任何支撐多邊形可用。因此現(xiàn)代跑步控制器還需要引入另一套模型來處理騰空階段。2.4 LIPM 線性倒立擺模型LIPM 是跑步控制里最常用的一種簡(jiǎn)化模型。它把機(jī)器人等效為一個(gè)“質(zhì)心固定高度”的倒立擺支撐點(diǎn)看作腳底接觸點(diǎn)。這個(gè)模型的價(jià)值在于把復(fù)雜的全身動(dòng)力學(xué)濃縮成幾個(gè)關(guān)鍵變量質(zhì)心位置、質(zhì)心速度和支撐點(diǎn)位置。跑步控制器的第一層規(guī)劃往往就是先根據(jù) LIPM 算出質(zhì)心軌跡再根據(jù)軌跡反推每個(gè)關(guān)節(jié)的目標(biāo)角度和力矩。LIPM 雖然簡(jiǎn)化了很多但它抓住了運(yùn)動(dòng)控制的主干尤其適合用來快速驗(yàn)證算法邏輯是很多雙足機(jī)器人項(xiàng)目的“第一步模型”。2.5 MPC 與 WBCMPCModel Predictive Control模型預(yù)測(cè)控制是一種在每一控制周期內(nèi)用當(dāng)前狀態(tài)與動(dòng)力學(xué)模型預(yù)測(cè)未來一段時(shí)間內(nèi)的系統(tǒng)行為并求取最優(yōu)控制序列的方法。它非常適合處理跑步中的約束條件比如關(guān)節(jié)角度限制、力矩上限和地面反作用力范圍。WBCWhole-Body Control全身控制則是把任務(wù)落實(shí)到每個(gè)關(guān)節(jié)的優(yōu)化框架。MPC 可以算出“質(zhì)心應(yīng)該以什么軌跡運(yùn)動(dòng)”WBC 負(fù)責(zé)把這些高層任務(wù)映射到“每個(gè)關(guān)節(jié)該輸出多少力矩”同時(shí)兼顧平衡、接觸力、執(zhí)行器限幅等多個(gè)目標(biāo)。簡(jiǎn)單說MPC 負(fù)責(zé)“策略層”WBC 負(fù)責(zé)“執(zhí)行層”。兩者配合才可能讓機(jī)器人在跑步這種強(qiáng)動(dòng)態(tài)場(chǎng)景下既“有目標(biāo)”又“能落地”。2.6 強(qiáng)化學(xué)習(xí)強(qiáng)化學(xué)習(xí)在機(jī)器人跑步領(lǐng)域越來越常見。核心思路是讓機(jī)器人策略在一個(gè)仿真環(huán)境里不斷與環(huán)境交互收到獎(jiǎng)勵(lì)或懲罰信號(hào)最終學(xué)到在復(fù)雜狀態(tài)下如何決策動(dòng)作。與 MPC 相比強(qiáng)化學(xué)習(xí)不依賴精確的動(dòng)力學(xué)模型更適合處理難以手工建模的接觸摩擦、落地沖擊和關(guān)節(jié)非線性。它的問題是訓(xùn)練成本高、仿真到實(shí)機(jī)遷移困難。目前很多團(tuán)隊(duì)的做法是先用仿真訓(xùn)練策略再用少量真實(shí)機(jī)器人數(shù)據(jù)微調(diào)或者把強(qiáng)化學(xué)習(xí)策略與 MPC/WBC 結(jié)合成混合控制器。2.7 跑步步態(tài)的周期結(jié)構(gòu)|The assistant|跑步步態(tài)與行走步態(tài)的最大差異是存在明顯騰空相。以雙足人形機(jī)器人為例一個(gè)完整跑步周期通常分為支撐相、騰空相和觸地相。階段狀態(tài)控制任務(wù)支撐相單腿或雙腿著地吸收沖擊、調(diào)整質(zhì)心、蓄力向前騰空相雙腳離地收縮腿部、調(diào)整姿態(tài)、準(zhǔn)備落地觸地相腳剛接觸地面緩沖沖擊、防止速度損失、快速過渡到支撐相四足機(jī)器人原理類似但因?yàn)樗臈l腿提供了更大的支撐多邊形平衡難度略低所以很多四足項(xiàng)目可以更早展示高速奔跑、跳躍甚至翻滾動(dòng)作。這也是為什么在熱搜詞里四足機(jī)器人的關(guān)注度一直不低的原因之一。3. 跑步機(jī)器人的硬件選型與系統(tǒng)架構(gòu)硬件是機(jī)器人跑步的基礎(chǔ)。沒有足夠強(qiáng)的執(zhí)行器、傳感器和計(jì)算單元任何高級(jí)算法都無從談起。這一節(jié)主要整理“要讓機(jī)器人跑起來需要哪幾類核心硬件”。3.1 關(guān)節(jié)執(zhí)行器決定跑步能力的上限跑步對(duì)關(guān)節(jié)執(zhí)行器的要求非常高。常見方案有三類方案優(yōu)勢(shì)局限無框力矩電機(jī) 諧波減速器扭矩密度高、精度高成本高、散熱難、減速器柔性強(qiáng)盤式電機(jī) 行星減速器響應(yīng)快、結(jié)構(gòu)緊湊制造工藝要求高、批量一致性難保證直驅(qū)電機(jī)外轉(zhuǎn)子力控精確、無齒隙體積重量大、峰值扭矩相對(duì)有限跑步機(jī)器人的關(guān)節(jié)不僅要能在靜止時(shí)輸出大扭矩還要在高速旋轉(zhuǎn)時(shí)提供足夠功率。每次觸地沖擊都會(huì)反饋到電機(jī)輸出端如果減速器剛度不足電機(jī)和控制器的“判斷”就會(huì)受到遲滯影響表現(xiàn)就是落地瞬間抖動(dòng)或失控。從成本與工程難度看無框力矩電機(jī) 諧波減速器是目前人形/高動(dòng)態(tài)四足機(jī)器人最主流的選擇。但電機(jī)選型之外還要考慮驅(qū)動(dòng)器帶寬與散熱。長時(shí)間跑步測(cè)試時(shí)關(guān)節(jié)溫度常常上升得比預(yù)期快一旦驅(qū)動(dòng)器過熱降額力矩不足就會(huì)直接導(dǎo)致摔倒。3.2 傳感與狀態(tài)估計(jì)跑步控制必須具備高帶寬、低延遲的狀態(tài)反饋部件主要包括IMU慣性測(cè)量單元測(cè)量三軸加速度和角速度是機(jī)身姿態(tài)估計(jì)的核心傳感器。關(guān)節(jié)編碼器測(cè)量每個(gè)關(guān)節(jié)的角度和角速度用于位置環(huán)和速度環(huán)控制。六維力/力矩傳感器安裝在腳底或腕部用于檢測(cè)地面反作用力進(jìn)而計(jì)算 ZMP 和接觸狀態(tài)。在實(shí)際系統(tǒng)中這些傳感器數(shù)據(jù)不能單獨(dú)使用。IMU 有漂移編碼器有量化誤差力傳感器有噪聲。控制循環(huán)里通常需要用擴(kuò)展卡爾曼濾波EKF或互補(bǔ)濾波把多源數(shù)據(jù)融合起來得到更準(zhǔn)確的機(jī)身狀態(tài)估計(jì)。3.3 計(jì)算與通信架構(gòu)跑步控制頻率通常要達(dá)到 1kHz 甚至更高。一條典型的控制鏈路是IMU / 編碼器 / 力傳感器 ↓ 高頻采集 MCU或機(jī)載計(jì)算機(jī) ↓ 控制解算 關(guān)節(jié)驅(qū)動(dòng)器 ↓ 電機(jī)輸出通信協(xié)議常見的有 EtherCAT、CAN FD 或私有高速總線。若使用 ROS2則需要把所有傳感器數(shù)據(jù)封裝成 Topic供上層算法訂閱。這里要特別提醒ROS2 是上層軟件生態(tài)底層關(guān)節(jié)伺服往往仍由 MCU 實(shí)時(shí)控制。把 ROS2 直接用于 1kHz 關(guān)節(jié)力矩環(huán)延遲和抖動(dòng)通常難以接受。因此穩(wěn)定架構(gòu)通常是“底層 MCU 負(fù)責(zé)實(shí)時(shí)控制上層工控機(jī)或開發(fā)板負(fù)責(zé) AI 策略和感知ROS2 只做跨節(jié)點(diǎn)通信”。4. 從零搭建機(jī)器人運(yùn)動(dòng)仿真環(huán)境對(duì)沒有實(shí)驗(yàn)條件的開發(fā)者來說仿真就是最好的“第二基地”。你可以在電腦上先跑通控制算法再?zèng)Q定是否遷移到實(shí)機(jī)。4.1 為什么先做仿真仿真可以無限次摔倒不燒電機(jī)不出安全事故還能反復(fù)調(diào)整物理參數(shù)。更重要的是當(dāng)前很多開源的機(jī)器人模型和算法生態(tài)都圍繞仿真構(gòu)建從仿真起步的技術(shù)路徑最平滑。主流仿真平臺(tái)有MuJoCo物理引擎開源且效率高支持接觸力求解、柔體建模研究雙足/四足機(jī)器人非常合適。PyBulletPython 接口方便安裝簡(jiǎn)單適合快速原型。Gazebo ROS2適合需要與導(dǎo)航、SLAM、多傳感器融合結(jié)合的機(jī)器人項(xiàng)目。Isaac Sim / Isaac Lab適合大規(guī)模強(qiáng)化學(xué)習(xí)訓(xùn)練和合成數(shù)據(jù)生成。如果你剛開始接觸建議從 MuJoCo 或 PyBullet 入手它們輕量、容易上手可以在自己的電腦上快速跑通最小 Demo。4.2 環(huán)境安裝示例以 Python 3.10 為例在命令行中執(zhí)行# 創(chuàng)建虛擬環(huán)境 conda create -n robot_sim python3.10 -y conda activate robot_sim # 安裝核心依賴 pip install numpy scipy matplotlib mujoco # 如果喜歡 PyBullet也可以安裝 pip install pybullet # 驗(yàn)證 MuJoCo 版本 python -c import mujoco; print(mujoco.__version__)安裝完成后可以嘗試加載 MuJoCo 自帶的人形機(jī)器人模型。不同版本的 MuJoCo 自帶模型路徑略有差異請(qǐng)以你本機(jī)安裝后的文件結(jié)構(gòu)為準(zhǔn)。4.3 跑通一個(gè)最小仿真模型下面這段代碼演示如何用 MuJoCo 加載一個(gè)人形機(jī)器人模型并前進(jìn)若干仿真步。它本身不會(huì)產(chǎn)生什么有效運(yùn)動(dòng)但能驗(yàn)證仿真環(huán)境是否可用。import mujoco # 請(qǐng)將路徑替換為你本機(jī)的實(shí)際模型路徑 model_path /path/to/humanoid.xml model mujoco.MjModel.from_xml_path(model_path) data mujoco.MjData(model) # 前進(jìn) 1000 個(gè)仿真步 for i in range(1000): mujoco.mj_step(model, data) # 輸出機(jī)器人前向位置 print(仿真完成機(jī)器人前向位置, data.qpos[0])如果這段代碼能正常輸出位置說明 MuJoCo 安裝正確仿真環(huán)境可用。PyBullet 的加載方式更輕量一些默認(rèn)安裝包自帶四足機(jī)器人和地面模型示例代碼如下import pybullet as p import pybullet_data import time p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 加載地面 p.loadURDF(plane.urdf) # 加載四足機(jī)器人模型 robot_id p.loadURDF(quadruped/quadruped.urdf) # 設(shè)置重力 p.setGravity(0, 0, -9.81) # 簡(jiǎn)單示例遍歷前 240 個(gè)仿真步設(shè)置關(guān)節(jié)目標(biāo)位置 num_joints p.getNumJoints(robot_id) for _ in range(240): target_positions [0.5] * num_joints p.setJointMotorControlArray( robot_id, jointIndicesrange(num_joints), controlModep.POSITION_CONTROL, targetPositionstarget_positions, ) p.stepSimulation() time.sleep(1.0 / 240.0)這里把每個(gè)關(guān)節(jié)目標(biāo)設(shè)成 0.5只是為了演示 API 用法。真正的步態(tài)控制必須結(jié)合逆運(yùn)動(dòng)學(xué)、步態(tài)相位和動(dòng)力學(xué)約束來規(guī)劃目標(biāo)位置。5. 最小步態(tài)控制實(shí)現(xiàn)LIPM ZMP 示例下面用 Python 實(shí)現(xiàn)一個(gè)跑步控制中很常用的線性倒立擺模型LIPM并在此基礎(chǔ)上演示如何計(jì)算落腳點(diǎn)和 ZMP 穩(wěn)定性判斷。代碼不依賴實(shí)體機(jī)器人可以在仿真環(huán)境里繼續(xù)擴(kuò)展。5.1 LIPM 模型 Python 實(shí)現(xiàn)import numpy as np class LIPM: def __init__(self, z_h, g9.81): self.z_h z_h self.g g self.omega np.sqrt(g / z_h) def predict(self, state, foothold_x, T): 狀態(tài): [x, vx] 質(zhì)心位置與水平速度 foothold_x: 支撐腳的水平位置 T: 預(yù)測(cè)時(shí)間 x, vx state A np.array([ [np.cosh(self.omega * T), np.sinh(self.omega * T) / self.omega], [self.omega * np.sinh(self.omega * T), np.cosh(self.omega * T)] ]) B np.array([ [1 - np.cosh(self.omega * T)], [-self.omega * np.sinh(self.omega * T)] ]) next_state A np.array([x, vx]) B.flatten() * foothold_x return next_state # 初始化跑步場(chǎng)景質(zhì)心高度 0.9m runner LIPM(z_h0.9) # 當(dāng)前質(zhì)心位置 0速度 1.5m/s state np.array([0.0, 1.5]) # 假設(shè)支撐腳位置為 0 foothold 0.0 # 預(yù)測(cè)未來 0.2 秒的質(zhì)心狀態(tài) T_step 0.2 next_state runner.predict(state, foothold, T_step) print(下一步質(zhì)心狀態(tài), next_state)代碼邏輯解釋state表示當(dāng)前質(zhì)心的位置和水平速度。predict方法用 LIPM 的解析解快速推算出未來某個(gè)時(shí)刻的質(zhì)心狀態(tài)。輸出結(jié)果是一個(gè)二維數(shù)組分別代表未來的位置和速度。在實(shí)際跑步控制器中我們可以用這個(gè)預(yù)測(cè)結(jié)果來決定“下一步腳落點(diǎn)到哪里”從而維持目標(biāo)速度。5.2 規(guī)劃落腳點(diǎn)的簡(jiǎn)易閉環(huán)下面的函數(shù)基于目標(biāo)速度用 LIPM 模型計(jì)算出期望落腳點(diǎn)并輸出預(yù)測(cè)后的質(zhì)心狀態(tài)def step_control(state, target_speed, z_h0.9, T_step0.2): model LIPM(z_h) x, vx state # 期望落腳點(diǎn) 當(dāng)前質(zhì)心位置 目標(biāo)速度 × 單步時(shí)間 target_foothold x target_speed * T_step # 預(yù)測(cè)下一步質(zhì)心狀態(tài) next_state model.predict(state, target_foothold, T_step) return target_foothold, next_state # 測(cè)試目標(biāo)速度 2.0 m/s state np.array([0.0, 1.5]) foothold, next_state step_control(state, target_speed2.0) print(目標(biāo)落腳點(diǎn), foothold) print(預(yù)測(cè)下一步狀態(tài), next_state)在跑步控制里這個(gè)“目標(biāo)落腳點(diǎn)”會(huì)被進(jìn)一步轉(zhuǎn)換為髖關(guān)節(jié)和膝關(guān)節(jié)的目標(biāo)角度帶動(dòng)整條腿邁到預(yù)定落點(diǎn)。速度越高落腳點(diǎn)就離當(dāng)前質(zhì)心越遠(yuǎn)對(duì)腿部擺動(dòng)速度和關(guān)節(jié)驅(qū)動(dòng)能力的要求也會(huì)增加。5.3 ZMP 穩(wěn)定性判斷示例ZMP 是否落在支撐多邊形內(nèi)是判斷機(jī)器人是否穩(wěn)定的重要依據(jù)。下面給出一個(gè)簡(jiǎn)化示例def check_zmp_in_polygon(zmp_x, zmp_y, polygon): 檢查 ZMP 是否落在支撐多邊形內(nèi) polygon: [(x1, y1), (x2, y2), ...] 頂點(diǎn)列表 這里簡(jiǎn)化為矩形邊界判斷 min_x min(p[0] for p in polygon) max_x max(p[0] for p in polygon) min_y min(p[1] for p in polygon) max_y max(p[1] for p in polygon) return min_x zmp_x max_x and min_y zmp_y max_y # 示例支撐腳落在 x0, y0 附近支撐區(qū)域約為 0.2m × 0.2m support_polygon [(-0.1, -0.1), (0.1, -0.1), (0.1, 0.1), (-0.1, 0.1)] # 當(dāng)前 ZMP 位置 zmp_x, zmp_y 0.02, -0.03 print(ZMP 是否合規(guī), check_zmp_in_polygon(zmp_x, zmp_y, support_polygon))如果 ZMP 靠近支撐多邊形邊緣說明穩(wěn)定裕度不足下一步控制器就需要調(diào)整姿態(tài)或落點(diǎn)把 ZMP 重新拉回中心區(qū)域。5.4 強(qiáng)化學(xué)習(xí)策略接入控制循環(huán)的框架如果你選擇走強(qiáng)化學(xué)習(xí)路線控制循環(huán)大致如下。這里用偽代碼展示結(jié)構(gòu)重點(diǎn)在于說明 RL 策略與底層仿真之間的交互方式。# 偽代碼RL 步態(tài)控制循環(huán) def get_observation(data): # 狀態(tài)觀測(cè)質(zhì)心位置、速度、關(guān)節(jié)角度、關(guān)節(jié)角速度、上一動(dòng)作 return np.concatenate([ data.qpos[:3], # 機(jī)身位置 data.qvel[:3], # 機(jī)身速度 data.qpos[7:], # 關(guān)節(jié)角度 data.qvel[6:], # 關(guān)節(jié)角速度 ]) def compute_action(obs, policy): # 將 obs 輸入策略網(wǎng)絡(luò)得到動(dòng)作向量 return policy(obs) for step in range(max_steps): obs get_observation(data) action compute_action(obs, policy) # 將 action 映射為關(guān)節(jié)目標(biāo)位置或力矩增量 set_joint_action(action) mujoco.mj_step(model, data)實(shí)際工程中set_joint_action會(huì)根據(jù)策略輸出是“目標(biāo)位置”還是“力矩增量”來做不同處理。如果是位置控制模式策略輸出經(jīng)過 PD 控制器變成力矩如果是力矩控制模式策略輸出直接作為關(guān)節(jié)力矩目標(biāo)。高頻循環(huán)保證了策略的實(shí)時(shí)性同時(shí)需要把狀態(tài)估計(jì)放在get_observation中完成確保策略輸入的是融合后的準(zhǔn)確狀態(tài)。6. 運(yùn)行結(jié)果與效果驗(yàn)證寫完控制代碼后不能只滿足于“機(jī)器人沒倒”。你需要有明確指標(biāo)來判斷是否真的“跑起來了”。6.1 主要驗(yàn)證指標(biāo)指標(biāo)說明靠譜的參考范圍質(zhì)心