想象這樣一個(gè)場(chǎng)景：你的人形機(jī)器人正在彎腰拾起一個(gè)玻璃杯。如果關(guān)節(jié)太“硬”，手一碰到杯子就急停，可能打翻它；如果太“軟”，又會(huì)陷進(jìn)去，無(wú)法穩(wěn)定抓取。理想的執(zhí)行器，應(yīng)該像人的手臂——既能穩(wěn)穩(wěn)托住物體，又能順應(yīng)外力微調(diào)姿態(tài)。

這正是力位混合控制（Hybrid Force-Position Control）要解決的核心問(wèn)題。

在人形機(jī)器人這種帶減速器的高動(dòng)態(tài)關(guān)節(jié)中，開(kāi)發(fā)者常面臨兩難：

• 純位置控制太“硬”：接觸瞬間產(chǎn)生電流尖峰，易觸發(fā)過(guò)流保護(hù)，機(jī)械沖擊大；
• 純力/電流控制太“軟”：難以維持期望姿態(tài)，容易漂移，缺乏“支撐感”。

為平衡這兩者，我們?cè)?HPM MCL v2 電機(jī)控制庫(kù)中集成了輕量級(jí)力位混合控制器。它不改變現(xiàn)有 FOC 電流環(huán)架構(gòu)，僅在上層增加一個(gè)外環(huán)，即可讓關(guān)節(jié)具備可調(diào)的剛度與阻尼，在精確跟蹤與環(huán)境順應(yīng)之間找到最佳平衡點(diǎn)。

更關(guān)鍵的是：在“抓取/接觸”這類任務(wù)里，關(guān)節(jié)并不存在唯一的最佳剛度。

• 接觸與對(duì)齊階段更需要柔順（低剛度）來(lái)降低沖擊、避免打滑或卡死；
• 抓穩(wěn)與支撐階段更需要穩(wěn)定（高剛度）來(lái)維持姿態(tài)、承載負(fù)載。

力位混合控制讓執(zhí)行單元具備這種“剛度可調(diào)”的能力：不是在硬/軟之間二選一，而是按任務(wù)階段切換到合適的狀態(tài)。

它解決的是執(zhí)行單元的真實(shí)工程痛點(diǎn)

當(dāng)關(guān)節(jié)需要與外界接觸（地面、桌面、人體、裝配件等），若只追求位置剛性，系統(tǒng)往往會(huì)出現(xiàn)：

1. 接觸瞬間的力矩/電流尖峰，帶來(lái)熱應(yīng)力與保護(hù)風(fēng)險(xiǎn)；
2. 因阻尼不足或速度噪聲引發(fā)振蕩、“彈跳”；
3. 接觸后位置難以收斂，要么抖動(dòng)，要么持續(xù)偏移。

力位混合控制的價(jià)值在于：在不改動(dòng)底層驅(qū)動(dòng)的前提下，為執(zhí)行單元增加一層可控的“阻抗行為”。無(wú)論外部擾動(dòng)如何變化，關(guān)節(jié)都能按預(yù)設(shè)的剛度和阻尼響應(yīng)，使接觸過(guò)程更平滑、更可預(yù)測(cè)。

在“大小腦”架構(gòu)中的定位：屬于執(zhí)行單元側(cè)

在典型的人形機(jī)器人分層控制架構(gòu)中：

• 大腦（任務(wù)層） 負(fù)責(zé)感知與決策，如“抓杯子”“邁步上臺(tái)階”；
• 小腦（運(yùn)動(dòng)規(guī)劃層） 將任務(wù)轉(zhuǎn)化為關(guān)節(jié)軌跡、末端力目標(biāo)或全身優(yōu)化指令；
• 執(zhí)行單元（伺服驅(qū)動(dòng)層） 則負(fù)責(zé)將這些目標(biāo)高速、穩(wěn)定地轉(zhuǎn)化為電機(jī)電流。

HPM MCL v2 的力位混合控制明確歸屬于執(zhí)行單元側(cè)。它不參與任務(wù)規(guī)劃，也不決定“該施加多大的力”，而是接收上層給出的期望位置、速度（以及可選的前饋力矩），在電機(jī)側(cè)實(shí)時(shí)合成一個(gè)符合設(shè)定剛度/阻尼特性的力矩指令，并通過(guò) FOC 電流環(huán)精準(zhǔn)執(zhí)行。

簡(jiǎn)言之：上層決定“想要什么”，我們負(fù)責(zé)把它穩(wěn)定、安全、可控地做出來(lái)。

注：雖然“阻抗控制”與“導(dǎo)納控制”在理論層面常被區(qū)分，但在實(shí)際系統(tǒng)中，只要采樣率與帶寬匹配，二者可通過(guò)數(shù)學(xué)變換等效。對(duì)執(zhí)行單元而言，最終落地需要一個(gè)高帶寬、帶限幅與濾波的力矩執(zhí)行鏈路——這正是本方案的定位。

核心思想：讓關(guān)節(jié)“可軟可硬”，且行為一致

力位混合控制的本質(zhì)，是將位置誤差和速度誤差映射為力矩輸出

輸出力矩 = 剛度(kp) × 位置誤差 + 阻尼(kd) × 速度誤差 + 前饋力矩(tau_ff)

其中：

• Kp 決定剛度：值越大，抵抗外力變形的能力越強(qiáng)；
• Kd 決定阻尼：值越大，運(yùn)動(dòng)越平穩(wěn)，抑制振蕩；
• tau_ff 為可選前饋力矩，用于補(bǔ)償重力或慣性項(xiàng)。

執(zhí)行單元將輸出力矩除以電機(jī)轉(zhuǎn)矩常數(shù) Kt，得到 q 軸電流指令，交由 FOC 電流環(huán)執(zhí)行。整個(gè)過(guò)程可在微秒級(jí)完成，確保阻抗行為實(shí)時(shí)響應(yīng)。

這里要強(qiáng)調(diào)的是：低剛度與高剛度都是正常系統(tǒng)狀態(tài)。

• 低剛度適合“觸碰/對(duì)齊/人機(jī)交互”等需要順應(yīng)的階段；
• 高剛度適合“抓穩(wěn)/定位/支撐”等需要穩(wěn)態(tài)保持的階段。

力位混合控制的價(jià)值在于讓這種行為“可調(diào)且一致”，并在執(zhí)行層用限幅/濾波把它做得可控、可實(shí)現(xiàn)。

為什么 HPM 芯片能高效支持這一功能？

力位混合控制雖邏輯簡(jiǎn)潔，但對(duì)計(jì)算實(shí)時(shí)性與控制帶寬要求高。先楫高性能 RISC-V MCU 為此提供了關(guān)鍵硬件支撐：

• 主頻高達(dá) 800MHz 以上，確保外環(huán)控制周期可短至 1μs；
• 內(nèi)置硬件加速 FOC 單元，減輕 CPU 負(fù)擔(dān)；
• 高精度同步 ADC 與 PWM 觸發(fā)機(jī)制，保障電流環(huán)與位置環(huán)的嚴(yán)格時(shí)序?qū)R。

得益于此，開(kāi)發(fā)者無(wú)需犧牲現(xiàn)有 FOC 架構(gòu)，僅需調(diào)用一個(gè)函數(shù)，即可啟用可調(diào)阻抗行為。

在 HPM SDK 中如何快速集成？

我們已在 hpm_sdk_extra 倉(cāng)庫(kù)中提供完整的力位混合控制示例，集成過(guò)程極為簡(jiǎn)潔，僅需四步：

1. 從編碼器讀取當(dāng)前關(guān)節(jié)位置 q 與速度 dq；
2. 調(diào)用mcl_hybrid_ctrl_step()，傳入期望位置/速度、剛度 Kp、阻尼 Kd（以及可選前饋力矩），即可獲得目標(biāo)力矩tau_cmd；
3. 根據(jù)電機(jī)轉(zhuǎn)矩常數(shù) Kt，計(jì)算 q 軸電流指令：iq_cmd = tau_cmd / Kt；
4. 調(diào)用hpm_mcl_loop_set_current_q(iq_cmd)，交由底層 FOC 電流環(huán)執(zhí)行。

整個(gè)外環(huán)邏輯不到十行代碼，卻能讓原本“非硬即剛”的伺服系統(tǒng)，具備按需調(diào)節(jié)的柔順交互能力——無(wú)需改動(dòng)現(xiàn)有驅(qū)動(dòng)架構(gòu)，開(kāi)箱即用。

實(shí)際效果一：面對(duì)“穿墻指令”，誰(shuí)更聰明？

為了直觀展示力位混合的價(jià)值，我們?cè)O(shè)計(jì)了一個(gè)典型場(chǎng)景：上層控制器給出一個(gè)“穿過(guò)物理限位”的目標(biāo)位置（例如指令要求轉(zhuǎn)到 1.2 rad，但機(jī)械限位在 1.0 rad）。這在抓取、裝配或足式行走中非常常見(jiàn)。

我們并排對(duì)比三種策略：

• 左：傳統(tǒng)位置控制（固定高增益）
• 中：力位混合 + 低剛度（適用于接觸、對(duì)齊階段）
• 右：力位混合 + 高剛度（適用于抓穩(wěn)、支撐階段）

圖中關(guān)鍵信息已標(biāo)注：

• 灰色粗線：物理限位（無(wú)法越過(guò)）
• 紅色虛線：上層給出的“穿墻”目標(biāo)位置
• 底部數(shù)字：頂墻后關(guān)節(jié)穩(wěn)定輸出的力矩值（單位：N·m）

可以看到：

• 傳統(tǒng)位置控制持續(xù)輸出接近限幅的力矩（約 0.60 N·m），相當(dāng)于“死命頂墻”，既浪費(fèi)能量，又增加電流與發(fā)熱風(fēng)險(xiǎn)；
• 力位混合控制則根據(jù)設(shè)定剛度，自動(dòng)收斂到合理的穩(wěn)態(tài)力矩：
  • 低剛度模式僅輸出約 0.20 N·m，輕柔貼合；
  • 高剛度模式輸出約 0.50 N·m，提供強(qiáng)支撐。

這意味著：同一個(gè)執(zhí)行單元，可在不同任務(wù)階段動(dòng)態(tài)切換“手感”——接觸時(shí)柔順，抓持時(shí)穩(wěn)固，全程不超限、不失穩(wěn)。

實(shí)際效果二：突加外力沖擊，誰(shuí)更穩(wěn)健？

再看一個(gè)更貼近真實(shí)世界的場(chǎng)景：在穩(wěn)定運(yùn)行中，關(guān)節(jié)突然受到外部擾動(dòng)（例如人手推一下，或機(jī)器人腳踩到石子），我們模擬為 +0.5 N·m 的階躍力矩，持續(xù) 100ms。

對(duì)比結(jié)果如下：

表面看，低剛度偏轉(zhuǎn)更大，但這恰恰是主動(dòng)順應(yīng)的表現(xiàn)：它通過(guò)允許可控的微小位移，顯著降低了力矩峰值和電流沖擊。而傳統(tǒng)位置控制因“拒絕任何偏移”，反而被迫輸出最大力矩對(duì)抗擾動(dòng)，極易觸發(fā)過(guò)流保護(hù)。

在實(shí)際應(yīng)用中，你完全可以：

• 接觸/探索階段：?jiǎn)⒂玫蛣偠?，提升安全性與適應(yīng)性；
• 作業(yè)/支撐階段：切換至高剛度，保證精度與剛性。

這種“按需調(diào)節(jié)”的能力，正是力位混合控制的核心優(yōu)勢(shì)。