多速率非线性MPC实现四足机器人靠墙双足运动

多速率非线性模型预测控制用于四足机器人的墙壁支撑双足运动

论文：Multi-Rate Nonlinear Model Predictive Control for Wall-Supported Bipedal Locomotion of Quadrupedal Robots
作者：Taizoon Chunawala, Jeeseop Kim, Kaveh Akbari Hamed（Virginia Tech / UTEP）
链接：arXiv:2607.01574　|　代码：未公开　|　平台：Unitree A1（RaiSim 仿真）

一句话总结：把"落脚点规划"和"力控制"放进同一个多速率 NMPC 优化框架里，让四足机器人在墙壁支撑双足姿态下，于粗糙地形和外部扰动中实现 2.9 倍于 Raibert 启发式的成功率。

研究背景与动机

四足机器人在直立双足姿态下可以够到更高的物体、翻越四足步态无法通过的障碍、执行需要躯干抬升的操作任务。但在狭窄走廊、废墟、工业管线等受限环境中，仅靠两条腿维持动态平衡几乎不可能——机器人的形态设计决定了双足站立时的支撑面很小、重心偏高、可控裕度极窄。本文研究的核心场景是"墙壁支撑双足运动"：机器人用前腿接触垂直壁面获取额外支撑力，后腿在地面上行走，形成一种多接触的混合运动模式。

这一场景带来的挑战是多层次的。首先，直立姿态下四足机器人的动力学是混合的、不稳定的、高维的：肢体与环境的交互力在毫秒级快速变化，而落脚点和接触切换则在步态周期尺度上缓慢变化。这种"快慢并存"的多速率结构是标准 NMPC 难以处理的——标准 NMPC 假设所有输入以统一速率更新。其次，受限环境要求运动规划与几何约束紧密耦合：安全包络、墙壁接触面的可达性、足端运动学极限都需要在同一优化中被考虑。第三，欠驱动、单侧接触约束、动力学切换性质使得轨迹优化问题高度非凸。

现有方法通常采用分层解耦策略：高层用启发式（如 Raibert 落脚点公式）决定落脚位置，低层用 MPC 跟踪轨迹。这种解耦的问题在于：启发式无法预见未来的接触配置变化，在高速和粗糙地形下，每步覆盖距离增大，反应式反馈不足以管理耦合的基座-步态动力学。本文的出发点是：能不能把落脚点规划从"启发式外部输入"变成"优化框架内部的动力学状态"，让控制器直接推理落脚点与质心轨迹的耦合关系？

核心方法：多速率 NMPC 框架

1. 多速率系统建模

本文首先定义了一个通用的多速率离散非线性系统。状态向量 $x\in\mathbb{R}^{n_x}$ 包含质心位置、速度、欧拉角和角速度。控制输入分为两类：快输入 $u(t)$（环境反作用力 ERFs，每个时间步可更新）和慢输入 $v(t)$（落脚点增量，仅在每个步态周期起始更新）。系统动力学写作：

$$x(t+1)=f\bigl(x(t),u(t),v(t)\bigr)$$

慢输入的分段常数性质用指示函数描述：

$$v(t+1)=v(t)+\delta_{\mathcal{T}}(t+1)\,\Delta v(t)$$

其中 $\delta_{\mathcal{T}}(t+1)$ 是切换时刻集合 $\mathcal{T}$ 的指示函数——仅在步态周期边界处取 1，其余时刻为 0。这个公式精确编码了"落脚点只在步态切换时更新"这一物理约束，是整个多速率框架的核心。

2. 单刚体（SRB）动力学模板

在高层规划中，机器人被简化为单刚体模型。状态定义为：

$$x:=\mathrm{col}(p,\dot{p},\theta,\omega)\in\mathbb{R}^{12}$$

其中 $p\in\mathbb{R}^{3}$ 是质心位置，$\theta\in\mathbb{R}^{3}$ 是欧拉角，$\omega\in\mathbb{R}^{3}$ 是体坐标系角速度。环境反作用力（ERFs）同时包括地面接触力和墙壁接触力——这是与传统 GRF 的关键区别。SRB 动力学由牛顿-欧拉方程描述：

$$\Sigma^{\mathrm{SRB}}:\begin{cases}\ddot{p}=\dfrac{f^{\mathrm{net}}}{m}-g_{0}\\[6pt]\dot{\theta}=A(\theta)\,\omega\\[6pt]\dot{\omega}=I^{-1}\bigl(R^{\top}(\theta)\,\tau^{\mathrm{net}}-\mathbb{S}(\omega)\,I\,\omega\bigr)\end{cases}$$

其中 $m$ 是总质量，$I$ 是体坐标系惯量矩阵，$\mathbb{S}(\cdot)$ 是反对称矩阵算子，$A(\theta)$ 将体角速度映射到欧拉角导数。净力和净力矩由所有接触点的 ERF 求和得到：

$$\begin{bmatrix}f^{\mathrm{net}}\\[3pt]\tau^{\mathrm{net}}\end{bmatrix}:=\sum_{\ell\in\mathcal{C}}\begin{bmatrix}f^{\ell}\\[3pt]\mathbb{S}(r^{\ell})\,f^{\ell}\end{bmatrix}$$

这里 $r^{\ell}:=r^{\mathrm{foot},\ell}-p$ 是从质心到第 $\ell$ 个接触足的向量。关键创新在于：作者将足端位置 $r^{\mathrm{foot},\ell}$ 增广到状态向量中，使其通过慢输入 $\Delta v$ 演化：

$$\Delta r^{\mathrm{foot},\ell}=\Delta v^{\ell}(t),\quad \ell\notin\mathcal{C}$$

这个增广使得 NMPC 能够直接推理"落脚点选择如何影响未来的支撑多边形和稳定裕度"，而不是把落脚点当作外生的独立变量。

3. 统一 NMPC 优化问题

基于增广状态空间，作者构造了一个统一的非线性 MPC 优化问题。代价函数包含快状态和慢状态的终端代价与阶段代价：

$$\min \;\mathcal{L}_{\mathrm{terminal},x}(x_{t+N|t})+\mathcal{L}_{\mathrm{terminal},v}(v_{t+N|t})+\sum_{k=0}^{N-1}\Bigl[\mathcal{L}_{\mathrm{stage},x}(x_{t+k|t},u_{t+k|t})+\mathcal{L}_{\mathrm{stage},v}(v_{t+k|t},\Delta v_{t+k|t})\Bigr]$$

约束包括增广动力学方程、慢输入的分段常数演化、以及时间变化的输入可用性约束 $E_u(t+k)\,u_{t+k|t}=0$ 和 $E_{\Delta v}(t+k)\,\Delta v_{t+k|t}=0$。这些二元矩阵 $E_u$ 和 $E_{\Delta v}$ 会在不同步态阶段将不可用的接触力或落脚规划置零——例如在摆动相中，对应腿的接触力约束为零。值得注意的是，框架将 Raibert 启发式计算的步长作为参考值 $\Delta v^{\mathrm{ref}}$，这意味着启发式不是被完全替代，而是被嵌入为优化问题的参考轨迹。

4. 步态设计

墙壁支撑双足运动的周期步态由三个阶段组成：四接触相（50ms）、三接触相（30ms）和双接触相（120ms）。采样时间 $T_s=10$ms，对应 100Hz 控制频率。预测时域 $N=20$ 步，即 200ms 的预测窗口——这意味着优化器能跨越多个步态阶段进行预测。例如在四接触相中期求解时，预测时域覆盖当前四接触相、随后的三接触和双接触相，以及下一个周期的部分四接触相。

graph TD
  A[四接触相
50ms: 前腿触墙+后腿触地] --> B[三接触相
30ms: 一腿摆动]
  B --> C[双接触相
120ms: 两腿支撑]
  C --> D[下一周期四接触相]
  D --> B
  style A fill:#4a90d9,stroke:#2c5f8a,color:#fff
  style B fill:#f5a623,stroke:#b97316,color:#fff
  style C fill:#7ed321,stroke:#4a8a14,color:#fff
  style D fill:#4a90d9,stroke:#2c5f8a,color:#fff

5. 低层全身控制器（WBC）

高层 MR-NMPC 输出的最优质心轨迹、ERF 轨迹和足端轨迹被传递给低层非线性全身控制器。WBC 基于全阶浮基座欧拉-拉格朗日方程：

$$D(q)\,\ddot{q}+H(q,\dot{q})=B\,\tau+\sum_{\ell\in\mathcal{C}}\bigl(J^{\ell}(q)\bigr)^{\top}f^{\ell}$$

其中 $D(q)$ 是质量-惯量矩阵，$H(q,\dot{q})$ 包含科氏力、离心力和重力项，$J^{\ell}(q)$ 是接触雅可比。WBC 通过虚拟约束 $y(t,q):=y_a(q)-y_{\mathrm{des}}(t)$ 桥接降阶模型和全阶模型，并用一个凸二次规划（QP）求解关节力矩：

$$\min_{(\tau,f,\delta)}\;\frac{\gamma_1}{2}\|\tau\|^2+\frac{\gamma_2}{2}\|f-f_{\mathrm{des}}\|^2+\frac{\gamma_3}{2}\|\delta\|^2$$

约束包括输出动力学 $\ddot{y}+K_D\dot{y}+K_P y=\delta$、接触足零加速度、力矩和力可行域。QP 的松弛变量 $\delta$ 在不可行时放松输出动力学约束，保证数值稳定性。由于 $\ddot{y}$ 和 $\ddot{r}^{\mathrm{foot},\ell}$ 在拉格朗日动力学下是 $(\tau,f)$ 的仿射函数，该 QP 是凸的，可实时求解。

名义条件下的基线对齐

在平坦地形无外部扰动的名义条件下，MR-NMPC 和 Raibert 启发式实现了几乎相同的参考速度（$v_{\text{des}}=0.8$ m/s）和基座姿态跟踪。这一基线对齐至关重要——它使后续扰动和非平整地形场景中的性能增益可归因于提出的预测性落点规划，而非控制器调参差异。两种控制器在简单条件下表现相当，证明 MR-NMPC 的优势不是过度调优的产物。

非平整地形下的前-后腿解耦

关键对比在于 MR-NMPC 对后肢的处理与 Raibert 启发式的显著分歧。前脚放置（$r_x^f$）在两种方法间大致可比，但 NMPC 主动调节后脚放置（$r_x^r$）以适应起伏表面。通过解耦前后步长，优化器能动态扩展或收缩支撑多边形。这种灵活性使框架能产生 Raibert 启发式固定对称约束下无法实现的校正性恢复力矩，有效"转向"机器人回到参考路径。在 250 个随机生成的粗糙地形上，MR-NMPC 的成功率是 Raibert 启发式的 2.9 倍。

安全包络与约束满足

图 1 的快照对比清晰展示了安全包络的差异——绿色框表示规定的安全包络，红色框表示墙壁支撑运动期间安全包络的违反。MR-NMPC 控制器在整个运动过程中维持安全包络内，而 Raibert 启发式基线在墙壁支撑运动中出现安全包络违反。这种约束满足能力源于 MR-NMPC 在优化中显式编码了力矩限制、运动学可达性和摩擦锥等可行性约束，而启发式方法缺乏这种前瞻性约束推理。

外部扰动鲁棒性

在外部扰动条件下，MR-NMPC 通过预测性落点规划主动应对扰动——优化器在控制地平线内预测扰动影响并提前调整接触点和力分配，而非被动反应。这种预测性鲁棒性是 MR-NMPC 相对启发式的核心优势：启发式方法仅基于当前状态反应式调整步长，无法预见即将到来的扰动或地形变化。多速率框架的快输入（环境反作用力）和慢输入（接触点位置）分层设计，使控制器能同时高频调节力和低频规划落点，匹配了机器人混合动力学的多时间尺度特性。

实验结果

仿真在 RaiSim 物理引擎中进行，使用 Unitree A1 平台。高层 MR-NMPC 使用 CasADi 框架和 IPOPT 内点求解器，在 Intel Core i9-12900F（64GB RAM）上实现平均 8.26ms 求解时间（标准差 0.87ms），保证 100Hz 控制频率。

图1：MR-NMPC 控制器（上）与 Raibert 启发式基线（下）的墙壁支撑运动对比。绿色框为安全包络，红色框表示安全包络被违反。

对比基准

基线是经典 Raibert 启发式，其步长调整为 $\Delta x=\frac{\dot{x}T_s}{2}+k_v(\dot{x}-\dot{x}_{\mathrm{des}})$，其中 $k_v=\sqrt{h/g}$。在平坦地形无扰条件下，两种控制器实现了几乎相同的参考速度（$v_{\mathrm{des}}=0.8$ m/s）和基座姿态跟踪，建立了公平的对比基线。

对比维度	Raibert 启发式	MR-NMPC
平坦地形速度跟踪	0.8 m/s（达标）	0.8 m/s（达标）
粗糙地形成功率	低速尚可，高速急剧下降	高速下 2.9 倍成功率
扰动恢复	50N 扰动后失稳	50N 扰动下维持稳定
后足落脚调节	固定对称约束	动态收缩/扩展支撑多边形

图6：250 次随机粗糙地形上的生存概率对比。Raibert 启发式在高速下成功率急剧下降，MR-NMPC 保持高成功率。

自适应落脚与稳定性

在木块堆积障碍（2cm 高、14cm 宽、最多 3 层、覆盖 70% 跑道）测试中，MR-NMPC 通过定义前足和后足的纵向偏移量来分析稳定性机制：

$$r^{f}_{x}=p^{\mathrm{FR}}_{x}-x_{\mathrm{CoM}},\quad r^{r}_{x}=p^{\mathrm{RR}}_{x}-x_{\mathrm{CoM}}$$

关键发现是：MR-NMPC 与 Raibert 启发式在前足放置上差异不大，但在后足放置上有显著差异。NMPC 会主动调节后足位置以适应起伏地形——通过解耦前后步长，优化器可以动态扩展或收缩支撑多边形，产生 Raibert 启发式固定对称约束下无法获得的修正恢复力矩，有效地将机器人"引导"回参考路径。

图5：(a) Raibert 启发式（蓝）与 MR-NMPC（红）的质心速度对比，目标速度 0.8 m/s。(b) 前后足相对于质心的位置演化。

扰动抑制

在 $t=8$s 到 $9$s 之间施加 50N 正弦力扰动（x 方向）。Raibert 启发式在速度突变后产生过大步长，触发关节限位饱和，随后失稳。MR-NMPC 利用预测时域识别出一系列较小的非对称步长序列，在满足所有安全和运动学约束的同时抑制扰动。

实验场景	扰动/条件	Raibert 结果	MR-NMPC 结果
平坦地形	无扰动	稳定跟踪	稳定跟踪
木块障碍	250 随机地形	高速失败率高	2.9× 成功率
正弦力扰动	50N, 8-9s	关节饱和→失稳	非对称小步长→稳定
硬件实验	初步验证	—	60s+ 稳定站立

从相关工作看，MPC 已成为腿式机器人轨迹优化和反馈控制的强大框架，常与降阶模板模型配对。线性倒立摆（LIP）及其扩展、弹簧加载倒立摆（SLIP）、质心动力学和单刚体（SRB）模型提供了计算高效且物理代表性的描述。基于 QP 的线性化模板 MPC 虽高效但难以捕捉质心动力学和 SRB 的非线性效应。NMPC 被用于动态步态实时轨迹规划，但这些方法主要针对四足步态且未扩展到墙壁支撑双足运动——后者的多速率环境交互引入了根本不同的控制挑战。四足机器人双足步态主要通过 RL 研究，展示了仿真和硬件上的优异性能，但 RL 方法的可解释性和约束保证不如基于模型的 NMPC。MR-NMPC 填补了这一空白：将多速率优化控制扩展到墙壁支撑双足运动，同时规划最优状态轨迹、反作用力和前后腿落点。

局限性

作者自述：初步硬件实验中，速度估计噪声导致机器人前倾俯仰，无法复现仿真中的稳定极限环行为。作者将此归因于状态估计精度不足，而非控制器设计本身的问题。

分析判断：本文仅提供仿真验证，硬件实验为"初步"性质（60 秒静态站立），缺乏完整的硬件运动对比数据。250 次随机地形试验虽然统计量充分，但所有仿真均基于同一物理引擎（RaiSim），sim-to-real 差距未得到充分评估。此外，慢输入仅考虑了纵向（x 方向）的落脚点和步长，横向落脚规划被简化处理，这限制了在需要侧向调整的复杂环境中的适用性。墙壁接触模型假设为理想刚性面，实际墙壁的摩擦和形变特性未被讨论。

总结与展望

本文的核心贡献在于将落脚点规划从启发式外部决策提升为 NMPC 优化框架内部的状态变量。通过指示函数编码的多速率结构，快输入（ERFs）和慢输入（落脚增量）在同一优化问题中被联合求解，使控制器能够预判未来接触配置并主动调整支撑多边形。2.9 倍的成功率提升不是来自更精细的力控制，而是来自"落脚点预见"——这正是 Raibert 启发式在高速下崩溃的根本原因。低层 WBC 的凸 QP 设计保证了全阶动力学的实时可行性。初步硬件结果虽然受限于状态估计精度，但验证了框架的实时部署能力。

未来工作方向包括：Kalman 滤波传感器融合改善状态估计、增大前足接触面积提升墙壁支撑稳定性、感知感知规划处理地形几何和接触条件的不确定性。

把落脚点从"算出来的参数"变成"优化出来的状态"，是让预测控制真正接管多接触运动的关键一步。