考虑非可忽略操作臂动力学的四足机器人移动操作非线性MPC框架

面向非可忽略操作臂动力学的四足机器人操作-移动非线性MPC框架

论文：A Nonlinear MPC Framework for Loco-Manipulation of Quadrupedal Robots with Non-Negligible Manipulator Dynamics
作者：Ruturaj Sambhus, Kapi Ketan Mehta, Ali Mir Mohammad Sadeghi, Basit Muhammad Imran, Jeeseop Kim, Taizoon Chunawala, Vittorio Pastore, Sujith Vijayan, Kaveh Akbari Hamed
机构：弗吉尼亚理工大学机械工程系；加州理工学院；弗吉尼亚理工大学神经科学学院
链接：arXiv:2507.22042

一句话总结

本文提出一种计算高效的分层非线性MPC（NMPC）框架，通过分解策略将单刚体（SRB）移动模板模型与操作臂全阶动力学模型经刚性完整约束耦合，在60Hz实时求解轨迹优化，低层全身控制器（WBC）以500Hz跟踪移动轨迹，操作臂力矩直接施加。在15kg Unitree Go2搭载4.4kg 4自由度Kinova臂的硬件平台上验证，臂-基座质量比达29%、高度比达216%，在粗糙地形、外扰和载荷变化下实现鲁棒的开门、推拉重物、拾取放置等多种操作-移动任务，相比基线kino-centroidal NMPC成功率高出58%。

研究背景与动机

模型预测控制（MPC）结合降阶模板模型已成为动态足式移动轨迹优化的强大工具。降阶模型如线性倒立摆（LIP）、弹簧加载倒立摆（SLIP）和单刚体（SRB）模型等提供了复杂非线性移动系统的低维抽象。然而，带操作臂的足式机器人执行操作-移动任务引入了新的复杂层——需要不仅计算高效，还能在动态约束下协调高自由度运动的MPC算法。

现有全身规划算法通常利用质心动力学和全身运动学支持动态操作-移动，但许多方法近似或忽略操作臂本身的全阶动力学。这种简化在臂动力学不可忽略时变得局限——例如将4.4kg、4自由度Kinova臂集成到15kg Unitree Go2四足上，臂-基座质量比和高度比分别达29%和216%。Go2标称8kg静态载荷能力未考虑Kinova臂这类动态、高耸的操作器。据作者所知，此前没有基于模型的方法解决小型四足上全阶、力矩可控、较重臂的动态操作-移动问题。

本文旨在回答一个基本问题：如何将全阶操作臂动力学与移动模板模型系统整合，以实现实时、高效且鲁棒的NMPC算法用于操作-移动？核心思路是采用分解策略——保留SRB等移动模板的有效性用于动态四足移动，同时系统整合臂动力学用于操作任务，两者通过刚性完整约束耦合。

图1：分层控制框架概览。高层NMPC基于分解方法同时优化移动模板模型和全阶臂模型；低层非线性WBC跟踪全阶移动模型。

动力学模型

移动模板——SRB模型。采用简化SRB动力学作为移动模板模型，状态包括质心位置、欧拉角及其速度。净力和净力矩由支撑足地面反力（GRF）、臂-基座交互力 wrench $\lambda^{int} = \text{col}(f^{int}, \tau^{int}) \in \mathbb{R}^6$ 及其诱导力矩组成：

其中 $\mathcal{C}$ 为支撑足集合，$f^\ell$ 为足 $\ell$ 的GRF，$r^\ell$ 为足到质心的向量，$\mathbb{S}(\cdot)$ 为反对称矩阵算子。GRF作为SRB动力学的控制输入 $u^{SRB}$，须满足摩擦锥约束。离散化后得到非线性状态空间方程：

操作模型——浮基臂动力学。为在NMPC中实现臂的力矩级控制，将臂的浮基动力学纳入最优控制公式。浮基坐标 $q^{arm} = \text{col}(q^b, q^s) \in \mathbb{R}^{10}$，其中 $q^b \in \mathbb{R}^6$ 为基坐标（位置+欧拉角），$q^s \in \mathbb{R}^4$ 为形状变量。臂运动方程通过Euler-Lagrange形式推导：

其中 $D(q^{arm}) \in \mathbb{R}^{10 \times 10}$ 为正定质量惯性矩阵，$H$ 包含科氏力、离心力和重力效应，$B \in \mathbb{R}^{10 \times 4}$ 为输入分布矩阵，$u^{arm} \in \mathbb{R}^4$ 为关节力矩，$J$ 为约束雅可比矩阵。交互 wrench $\lambda^{int}$ 通过 $-J^\top \lambda^{int}$ 项作用于臂——牛顿第三定律确保SRB和臂上的交互 wrench 大小相等方向相反。

完整约束与交互 wrench 计算

SRB和臂通过刚性完整约束耦合——臂基座位置和朝向须与SRB上对应点一致：

其中 $d$ 为SRB质心到交互点的固定偏移向量。紧凑形式为 $\varphi(p, \theta, q^{arm}) = 0$。对时间二次求导并代入SRB和臂动力学，得到动力学一致性条件：

该条件作为NMPC的等式约束，使优化器隐式求解交互 wrench，同时SRB和臂动力学保持各自的局部状态和控制变量，仅通过完整约束耦合。这种策略避免了解析消元 $\lambda^{int}$ 引入的额外非线性，降低了最优控制问题的复杂性。

统一NMPC公式

提出实时统一NMPC公式，决策变量为SRB和臂的状态-控制轨迹及交互 wrench 轨迹 $(\xi^{SRB}(\cdot), \xi^{arm}(\cdot), \lambda^{int}(\cdot))$，目标函数为各子系统二次代价之和加交互 wrench 惩罚：

约束包括：SRB和臂的离散动力学等式约束、完整约束 $\ddot{\varphi} = 0$、GRF可用性约束（摆动相足的GRF为零）、摩擦锥和力矩容许不等式约束。各子系统局部代价为标准二次型：

交互 wrench 代价 $J^{int} = \sum_{k=0}^{N-1} \|\lambda^{int}_{t+k|t}\|_{R^{int}}^2$ 惩罚过大的臂-基座交互力。NMPC以60Hz滚动时域实时求解，初始条件每步更新为当前测量状态。

flowchart TD
  subgraph High["高层 NMPC 60Hz"]
    SRB["SRB 模板模型
状态: 质心位置/欧拉角/速度
输入: 地面反力 GRF"]
    ARM["全阶臂动力学
状态: 浮基坐标
输入: 关节力矩"]
    SRB <-->|交互 wrench λ_int| ARM
    HC["完整约束 φ̈=0"]
    SRB --> HC
    ARM --> HC
  end
  SRB -->|最优状态+GRF| WBC["低层 WBC 500Hz
QP 虚拟约束"]
  WBC -->|关节力矩| LEGS["腿部执行"]
  ARM -->|最优力矩| ARMEXE["臂直接执行"]

低层非线性WBC

S RB最优状态和GRF轨迹传递给低层非线性WBC（500Hz），基于二次规划（QP）和虚拟约束合成关节级力矩。WBC的优化目标为：

约束包括：输出动力学 $\ddot{y} + K_D \dot{y} + K_P y = \delta$（虚拟约束编码位置跟踪，$y = y^a - y^{des}$包含质心位置、欧拉角和摆动足笛卡尔位置）；无滑移条件 $\ddot{r}^{st} = 0$；关节力矩和GRF的可行性约束。$f^{des}(t)$ 由高层NMPC提供，$\delta$ 为缺陷变量确保QP可行。摆动足期望轨迹使用Bézier多项式在当前和下一个落脚点间插值（Raibert启发式）。臂的最优力矩命令则直接施加，无需低层WBC。

实验设置

硬件平台为 Unitree Go2（15.0kg，站立高度0.28m，12个驱动关节）搭载 Kinova Gen2 4自由度臂（4.4kg，力矩控制，三指夹爪）。Sony DualShock 4 手柄提供实时参考命令（期望基座速度、朝向和臂构型）。NMPC在i9-12900F CPU + 64GB RAM台式机上板外多线程执行，通过以太网LAN通信。数值仿真使用RaiSim物理引擎。

NMPC超参数：SRB权重 $Q_p^{SRB} = \text{diag}\{1e8, 1e8, 8e8\}$，$Q_\theta^{SRB} = \text{diag}\{1e8, 1e8, 5e8\}$，终端代价 $P^{SRB} = 20 Q^{SRB}$，控制惩罚 $R^{SRB} = 100 \mathbb{I}_{3 \times 3}$。臂权重按基座位置/朝向/形状变量/速度分块对角设置。使用CasADi非线性优化框架和IPOPT求解器求解NMPC，qpSWIFT求解WBC的QP。

实验结果

通过物理引擎仿真和硬件实验全面验证，展示多种操作-移动任务：

图2：分解NMPC算法集成低层WBC在多种操作-移动任务中的实验快照：开冰箱门取瓶、木块行走载荷、推货车、拉行李箱、拾取放置、开门通过、推扰恢复、开抽屉取物、碎石草地行走。

定量对比。在200个随机生成地形上进行数值仿真，与基线 kino-centroidal NMPC（来自Sleiman等人的统一MPC框架）对比。本文方法在鲁棒移动成功率上比基线高出58%。基线方法忽略臂的全阶动力学，在臂-基座质量比较大的场景下鲁棒性和稳定性显著下降。

图3：Go2+Kinova臂鲁棒操作-移动。(a)携带1kg载荷在木块上trot，(b)承受外部推扰。

技术要点分析

分解策略的核心优势。分解策略的关键在于：SRB和臂各自保持局部状态和控制变量，仅通过完整约束 $\ddot{\varphi} = 0$ 耦合。这使得NMPC可以分别优化移动和操作子目标，同时通过交互 wrench 惩罚 $J^{int}$ 显式调节两者间的力交互。相比将全阶臂动力学消元后嵌入SRB模型的方法，分解策略避免了额外的系统非线性，使最优控制问题更易求解，实现60Hz实时性能。

力矩直接施加的意义。臂的最优力矩命令直接施加而不经过低层WBC，这是因为臂的浮基动力学已在NMPC中全阶建模，力矩级控制可以直接实现。而腿部需要WBC将SRB级别的GRF分配到各关节力矩——因为SRB模型不含关节级信息，WBC通过QP和虚拟约束完成这一映射。这种差异化处理体现了分层控制的精确设计。

臂-基座质量比29%的挑战。Kinova臂相对Go2基座的质量比达29%、高度比达216%，臂的运动会对基座产生显著的动态扰动。传统忽略臂动力学的方法在此场景下会因模型失配导致不稳定。本文的全阶臂动力学建模通过交互 wrench $\lambda^{int}$ 准确捕捉臂对基座的力和力矩影响，使SRB控制器能预补偿臂运动引起的扰动。

完整约束的数学推导。完整约束 $arphi(p, heta, q^{arm}) = 0$ 要求臂基座位姿与SRB上对应点一致。一阶导数为 $\dot{p} + rac{\partial}{\partial heta}(R d) A \omega = \dot{p}^b$ 和 $A \omega = \dot{ heta}^b$，其中 $A( heta)$ 将世界系角速度映射到欧拉角导数。二阶导数代入SRB和臂的连续时间动力学后，得到动力学一致性条件 $\ddot{arphi} = 0$，它是控制输入和交互 wrench 的函数。该条件作为NMPC等式约束使优化器隐式求解 $\lambda^{int}$——而非解析消元，避免了将 $\lambda^{int}$ 代入状态方程引入的额外非线性，是保持计算效率的关键设计选择。

交互 wrench 的物理意义。$\lambda^{int} = ext{col}(f^{int}, au^{int})$ 表示SRB和臂在连接点处的力和力矩交互。当臂运动时，其惯性力通过 $\lambda^{int}$ 反作用到SRB基座，引起基座姿态扰动；反之，基座移动时臂的浮基动力学也受 $\lambda^{int}$ 影响。NMPC同时优化 $\lambda^{int}$ 并通过惩罚项 $J^{int} = \sum \|\lambda^{int}\|_{R^{int}}^2$ 抑制过大的交互力，实现移动和操作的协调。这种显式建模使得控制器能预补偿臂快速运动对基座平衡的影响——例如推货车时臂施力对基座产生的反作用力矩可被SRB的GRF分配提前抵消。

虚拟约束与Bézier轨迹。WBC的虚拟约束 $y = y^a - y^{des}$ 编码位置跟踪，受控变量 $y^a$ 包括质心位置、欧拉角和摆动足笛卡尔位置。期望轨迹 $y^{des}(t)$ 包含NMPC给出的质心位置和欧拉角，摆动足期望轨迹使用Bézier多项式在当前和下一个落脚点间插值，落脚点按Raibert启发式计算。期望输出动力学 $\ddot{y} + K_D \dot{y} + K_P y = \delta$ 通过PD增益 $K_P, K_D$ 施加，缺陷变量 $\delta$ 确保QP在物理约束下可行。无滑移条件 $\ddot{r}^{st} = 0$ 确保支撑足不滑动，摩擦锥约束确保GRF在可行方向内。

与学习方法的对比。与RL-based方法（如DeepWBC、RoboDuet等）相比，本文的基于模型方法具有可解释性和安全保证优势——NMPC显式处理约束（摩擦锥、力矩限制），可证明地满足物理可行性。RL方法虽然训练后推理快，但缺乏形式化安全保证，且sim-to-real迁移存在不确定性。然而，基于模型方法对模型精度敏感——臂动力学参数（质量、惯性）的误差会影响控制性能，而RL方法通过域随机化可更鲁棒地处理模型不确定性。本文通过全阶建模将臂动力学不确定性降至最低，在125%质量不确定性下仍保持鲁棒，展示了基于模型方法在精心建模下的潜力。

NMPC求解的计算效率。本文NMPC能在60Hz实时求解的关键在于分解策略降低了问题规模。决策变量包括SRB状态-控制轨迹、臂状态-控制轨迹和交互wrench轨迹。若将臂全阶动力学消元嵌入SRB，非线性维度将急剧增加。分解策略使SRB和臂各自的动力学保持局部线性结构，仅通过完整约束耦合，CasADi的自动微分和IPOPT内点法能高效处理这种稀疏结构。低层WBC的QP问题规模更小，仅涉及浮基全阶移动模型的关节力矩和GRF，500Hz运行毫无压力。

推拉重物125%质量不确定性的意义。推货车实验中货车加载荷总质量达24.4kg，相当于Go2基座质量的125%。这种大质量不确定性对控制器鲁棒性构成严峻考验。臂施力推车时反作用力矩几乎可以使机器人翻倒。本文NMPC通过交互wrench的显式建模和优化使控制器能预补偿推力对基座平衡的影响，同时SRB的GRF分配动态调整以维持稳定。这正是全阶臂动力学建模的核心价值，忽略臂动力学的kino-centroidal方法在此场景下会因模型失配而失败。

开冰箱门和开抽屉任务展示了操作臂在动态接触下的精确力控能力。冰箱门有弹簧反弹力，抽屉有摩擦阻力，这些未建模的动态力对控制器的鲁棒性提出额外要求。NMPC通过力矩级直接控制臂关节，能够实时调整施力大小和方向以克服这些阻力，同时保持基座稳定。实验中所有开门和开抽屉任务均成功完成，验证了力矩级控制相比位置控制在动态接触任务中的优势。

局限性与未来方向

局限一：板外计算依赖。 NMPC在台式PC上板外执行，通过以太网通信。实际部署中通信延迟和带宽限制可能影响实时性，板载计算资源可能不足以支撑60Hz的NMPC求解。

局限二：操作臂自由度有限。 Kinova Gen2仅4自由度，操作灵巧性受限。更高自由度臂会增加NMPC决策变量维度，对实时求解构成更大挑战。

局限三：任务依赖手动参考命令。 当前需要人类操作员通过手柄提供实时参考命令（基座速度、朝向、臂构型），缺乏自主任务规划和感知能力。

未来工作方向包括：板载化计算、集成感知实现自主操作、扩展到更高自由度臂、以及结合学习方法提升鲁棒性。

总结

本文提出了首个面向小型四足机器人搭载非可忽略动力学操作臂的动态操作-移动的基于模型的控制算法。核心创新在于分解策略——将SRB移动模板与全阶臂浮基动力学通过刚性完整约束耦合，使NMPC能同时优化移动和操作子目标并显式调节交互 wrench，在60Hz实时求解。低层WBC以500Hz跟踪移动轨迹，臂力矩直接施加。在Go2+Kinova（臂-基座质量比29%）平台上验证了开门、推拉重物（125%质量不确定性）、拾取放置、推扰恢复等多样任务，相比忽略臂全阶动力学的基线方法成功率高出58%。该工作使动态操作-移动在小四足平台的物理极限附近成为可能，为带臂四足机器人的鲁棒控制奠定了模型基础。