1. 项目概述当全身模型预测控制遇上“关节锁定”在足式机器人尤其是人形机器人的控制领域模型预测控制Model Predictive Control, MPC一直被视为实现动态、复杂行为的“圣杯”。它的魅力在于能够在一个统一的框架内前瞻性地规划出未来一段时间内所有关节的运动序列从而优雅地协调平衡、行走、操作等多种任务。然而这份优雅背后是沉重的计算代价。一个完整的人形机器人模型动辄拥有30个以上的自由度Degrees-of-Freedom, DoF每一次MPC求解都意味着在一个高维、非线性的状态-动作空间中进行搜索计算复杂度呈指数级增长。这使得实时例如100Hz以上的全身模型预测控制在硬件上几乎成为不可能完成的任务长期以来研究者们不得不在模型精度和计算速度之间做出痛苦的折衷。传统的简化思路是使用降阶模型比如经典的线性倒立摆模型。这就像为了快速抵达目的地你选择只规划躯干质心的轨迹而暂时忽略手臂和头部的具体摆动细节。这种方法计算飞快但代价是牺牲了模型的保真度忽略了肢体惯性等不可忽略的动力学效应在需要精细全身协调或应对突发扰动时性能会大打折扣。另一种思路是启发式地、固定地减少自由度比如在行走任务中只考虑腿部和躯干关节完全锁死手臂。这虽然简化了问题但丧失了MPC的核心优势之一——任务自适应性。当机器人需要从行走突然切换到挥手或抓取时被“遗忘”的手臂关节无法被快速重新激活。VDoF-MPC的核心思想正是为了打破这一僵局。它不再进行“一刀切”的模型简化而是引入了一种任务自适应的、动态可变自由度的机制。简单来说这套系统会在每个控制周期通常是几毫秒内像一个经验丰富的指挥官一样快速评估当前任务下每个关节的“重要性得分”。对于得分低、即对完成当前核心任务如保持平衡、跟踪末端轨迹贡献微乎其微的关节例如在直线行走时的手指关节、在静立时的远端手臂关节系统会将其动态“锁定”——在优化问题中将它们暂时视为刚体忽略其动力学计算和梯度更新。而对于核心关节则保持其完全的活动能力进行精细优化。这带来了两个立竿见影的好处第一计算加速。被锁定的关节无需进行耗时的前向动力学和梯度计算直接降低了优化问题的维度加快了求解速度。第二可能提升求解性能。高维优化问题的解空间异常复杂充斥着大量局部最优解。通过剔除无关变量的干扰优化算法实际上是在一个更干净、更聚焦的搜索空间中寻找解有时反而能更快、更好地收敛到全局更优的解。而当任务切换时比如从跟踪左手末端切换到右手系统又能根据新的任务目标动态地解锁相关关节、锁定变得无关的关节实现了真正的全身模型自适应能力。2. 核心原理如何量化关节的“任务相关性”VDoF-MPC的巧妙之处在于它利用MPC求解过程本身产生的信息来量化每个关节的“贡献度”而不是依赖预设的、死板的规则。其核心评估指标被称为“贡献度分数”。2.1 贡献度分数的计算在每一次MPC循环中求解器如论文中使用的Box-DDP都会输出一条在预测时域内的最优状态轨迹 (X_{opt}) 和最优控制输入轨迹 (T_{opt})。基于这些轨迹我们可以计算每个关节对整个代价函数 (\ell) 的贡献。贡献度分数 (s_i) 对于第 (i) 个关节的计算公式如下[ s_{i} \sum_{kt}^{tH-1} \gamma^{k-t} \left( \left| \frac{\partial \ell}{\partial x_{j,i,k}} \right| \left| \frac{\partial \ell}{\partial \tau_{i,k}} \right| \right) \gamma^H \left| \frac{\partial \ell}{\partial x_{j,i,tH}} \right| ]我们来拆解这个公式的物理意义梯度范数(| \partial \ell / \partial x |) 和 (| \partial \ell / \partial \tau |) 分别衡量了关节状态位置、速度和关节扭矩的微小变化会对总代价函数产生多大影响。梯度越大说明改变这个关节对降低任务代价如跟踪误差越有效即该关节越“重要”。时间衰减因子 (\gamma)这是一个介于0和1之间的权重因子。它使得近期时间步的梯度贡献更大远期的贡献更小。这是符合直觉的因为MPC是滚动优化的当前控制动作对近期状态的影响最为直接和关键。求和与终端项将整个预测时域内所有时间步的加权梯度范数求和并加上终端代价的梯度最终得到一个标量分数 (s_i)。注意这里计算的是代价函数关于关节状态和扭矩的梯度而不是关节速度。速度项被有意排除在贡献度计算之外因为我们的目标是评估关节对完成任务目标的贡献而不是其本身的运动剧烈程度。一个快速摆动但对任务无益的关节其重要性应该很低。2.2 从分数到动作惩罚、候选与锁定计算出所有关节的贡献度分数后系统需要决定哪些关节应该被锁定。这个过程分为三步标准化与惩罚分配首先将所有活动关节的贡献度分数进行标准化处理。系统设定了一个关键的超参数 (n_u)它表示必须保持活动状态的最小关节数量。找到分数排名在第 (n_u) 位的关节的分数 (s_{n_u})。然后根据以下规则将分数转换为速度惩罚权重 (p_i) [ p_i \leftarrow \text{clip}\left( p_i - \beta \frac{s_i - s_{n_u}}{\sigma_s}, \quad \underline{p}, \quad \overline{p} \right) ] 其中(\sigma_s) 是分数的标准差(\beta) 是缩放因子(\underline{p}) 和 (\overline{p}) 是惩罚权重的上下限。核心逻辑是对于分数低于 (s_{n_u}) 的关节其惩罚权重 (p_i) 会被显著增加因为 (s_i - s_{n_u}) 为负。这意味着在下一个MPC循环的代价函数中这些关节的运动速度项将被严重惩罚从而鼓励优化器让它们保持静止。锁定候选集更新那些贡献度分数低于第 (n_u) 名分数的关节将被加入“锁定候选集” (F_{cand})。公式表示为(F_{cand} \leftarrow { \zeta_i \in F_{act} | s_i s_{n_u} })。执行锁定在下一个MPC周期开始时系统会检查候选集中的关节。如果一个关节在预测时域内的平均角速度低于一个很小的阈值 (\epsilon)例如0.01 rad/s则认为它确实处于准静止状态便将其正式移入“锁定集” (F_{lock})。锁定在模型内部是通过给该关节施加一个极大的摩擦损耗来模拟的使其在动力学计算中如同被焊死。2.3 关节的解锁随机性与温启动被锁定的关节并非被永久“封印”。由于任务可能切换之前不重要的关节可能变得关键。然而计算被锁定关节的贡献度分数需要重新对其进行梯度计算这会抵消锁带来的计算收益。因此VDoF-MPC采用了一种轻量且巧妙的概率性解锁机制。每个被锁定的关节 (i)依据其最后一次被锁定前的贡献度分数 (s_i)以及当前活动关节中的最低分数 (s_{n_u})按以下概率进行解锁尝试 [ P(\text{unlock}i) \frac{s_i}{s{n_u} s_i} ] 这个概率计得很直观如果一个关节历史重要性分数很高接近当前活动关节的最低水平那么它被解锁的概率就接近50%如果历史分数极低则解锁概率也极低。一旦关节被解锁为了平稳过渡需要为其提供一个优化的“温启动”初值。论文中的做法是使用一组较低比例的PD比例-微分反馈增益根据关节的当前位置误差和速度误差生成一个初始控制序列作为优化器向前传递的起点。这避免了关节从完全静止状态突然被纳入优化时可能产生的剧烈抖动和动力学失配。3. 系统实现与工程细节将VDoF-MPC从理论公式落地到真实的机器人控制系统需要解决一系列工程挑战。论文以Unitree G1人形机器人为平台详细阐述了其实现方案。3.1 整体控制架构由于MPC的计算频率论文中为66Hz远低于底层电机驱动的控制频率500Hz直接使用MPC输出的扭矩指令是不现实的。因此系统采用了分层控制架构高层VDoF-MPC66Hz运行核心算法根据当前状态和任务目标求解出最优的关节扭矩序列 (\tau_{cmd})。同时根据贡献度分数动态决定关节的锁定与解锁状态。中层状态估计500Hz使用基于扩展卡尔曼滤波的位姿估计器仅依靠本体感知传感器IMU、关节编码器实时估计机器人基座躯干在空间中的位置和姿态。这是浮动基座机器人控制的基础。底层局部反馈扭矩控制500Hz将MPC计算出的前馈扭矩 (\tau_{cmd}) 与基于状态误差的高增益PD反馈扭矩相结合生成最终的关节指令 (\tau_{local})。 [ \tau_{local} \tau_{cmd} K \delta x ] 其中 (K) 是反馈增益矩阵(\delta x) 是状态误差。这层高频率的反馈环用于抵抗模型误差、外部扰动和执行器动态。最终输出混合根据VDoF-MPC给出的关节状态活动/锁定最终发送给执行器的扭矩 (\tau_{out}) 是不同的对于活动关节(\tau_{out,i} \tau_{local,i} k_{p,low,i} \delta q_i k_{d,low,i} \delta \dot{q}_i)。这里额外添加了一组低增益PD项目的是在关节刚被解锁时提供柔和的恢复力。对于锁定关节(\tau_{out,i} k_{p,i} \delta q_i - k_{d,i} \dot{q}_i)。这里使用一组高增益的PD控制器强行将关节稳定在锁定时的位置并阻尼其速度模拟“刚性锁定”的效果。3.2 摩擦补偿与执行器处理真实的机器人关节存在显著的摩擦尤其是采用高减速比齿轮箱的关节。G1机器人没有关节扭矩传感器其前馈扭矩 (\tau_{cmd}) 是基于电机电流估计的。对于腕部、脚踝等输出扭矩较小的远端关节齿轮摩擦的影响尤为突出。为了补偿摩擦论文采用了Stribeck摩擦模型对远端关节进行建模和补偿 [ \tau_{fric} \left[ F_c (F_s - F_c) \exp\left(-\left(\frac{|\dot{q}|}{v_s}\right)^2\right) \right] \text{sgn}(\dot{q}) ] 其中(F_c) 是库伦摩擦(F_s) 是静摩擦(v_s) 是Stribeck速度。通过对实验数据进行非线性最小二乘拟合可以估计出这些参数。补偿后的最终底层指令变为 [ \tau_{local} \tau_{cmd} K\delta x \tau_{fric} ] 这项补偿对于提高锁定关节的控制精度、减少稳态误差至关重要。3.3 动力学计算加速的具体实现关节锁定带来的计算加速主要体现在梯度计算的省略上。在基于梯度的优化器如DDP中需要反复计算动力学模型 (f(x, \tau)) 关于状态 (x) 和控制输入 (\tau) 的雅可比矩阵导数。通常这需要通过有限差分法进行数值近似需要对每个控制输入维度都进行一次前向动力学计算。当关节 (j) 被锁定后在动力学计算中该关节的角度 (q_j) 被固定为锁定时的值 (q_{j,lock})。该关节的速度 (\dot{q}_j) 被设为零。在计算导数时关于被锁定关节扭矩的导数被直接设为零 [ \frac{\partial f(x_t, \tau_t)}{\partial \tau_{j,lock}} 0, \quad \frac{\partial f(x_t, \tau_t){j}}{\partial \tau} 0 ] 这意味着在有限差分计算中完全不需要为了这个被锁定的关节而额外调用前向动力学函数。对于一个有 (n_u) 个活动关节、(n{lock}) 个锁定关节的系统梯度计算量从 (O(n_u n_{lock})) 降低到了 (O(n_u))。在预测时域较长、关节总数较多时这种节省是相当可观的。4. 实验验证与性能分析论文通过仿真和实物实验从多个维度验证了VDoF-MPC的有效性。实验平台包括Franka Emika Panda机械臂带Allegro手和Unitree G1人形机器人。4.1 Panda机械臂指尖跟踪任务这是一个相对简单的任务旨在验证方法的基本特性。机器人需要控制其末端指尖跟踪一个给定的空间轨迹。对比方法Box使用完整模型23 DoF的基准Box-DDP方法。Reduced使用固定简化模型仅7个手臂关节手指关节被完全移除的方法。MH模型分层方法在预测时域的前半段使用完整模型后半段使用简化模型。结果分析计算速度VDoF-MPC的计算时间显著低于完整的Box方法与MH方法相当但远慢于固定的7-DoF简化模型。这符合预期因为VDoF仍需维护和评估完整模型的结构。跟踪精度令人惊讶的是VDoF-MPC的跟踪误差低于完整的Box方法。论文分析认为对于指尖跟踪这个任务手指关节本就是冗余的。完整模型的优化器在优化手指关节的冗余运动上“浪费”了搜索精力反而使代价函数地形更复杂阻碍了进一步优化。VDoF通过锁定这些无关关节简化了问题空间使优化器能更专注于对手臂关节的优化从而找到了更好的解。自由度变化如图3所示无论任务代价函数是跟踪整条轨迹还是仅跟踪终点VDoF-MPC锁定的关节模式几乎一致手指关节被大量锁定而完成跟踪所必需的手臂关节始终保持活动。这证明了贡献度评分机制的有效性。超参数 (n_u) 的影响如图4所示将 (n_u) 设得过大如15或23接近总自由度性能会下降因为过多的关节被施加了速度惩罚。而将 (n_u) 设得过小如3或0性能也有轻微下降。这表明存在一个合适的 (n_u) 范围在本任务中约为7既能充分简化问题又不会因过度惩罚而限制必要运动。4.2 G1人形机器人双臂交替跟踪任务这个任务更具挑战性要求机器人在保持全身平衡的同时交替用左手和右手末端执行器跟踪空间轨迹。任务在运行中会动态切换目标手臂。结果分析动态适应性如图5所示VDoF-MPC成功展示了其核心优势——任务自适应的自由度切换。当跟踪目标从左臂切换到右臂时系统自动减少了当前不活跃手臂右臂的自由度同时释放了即将活跃的手臂左臂的自由度。整个过程平滑机器人成功完成了任务切换。性能权衡与完整模型相比VDoF-MPC的跟踪误差略有增加表3。这是因为动态锁定和解锁机制并非完美会引入微小的瞬态误差。然而其计算速度更快表2。在实时控制中计算延迟本身就是一种性损失。更快的求解意味着更低的指令延迟这对于动态任务至关重要。4.3 G1人形机器人行走任务行走是一个复杂的周期性任务需要协调全身多个关节以维持动态平衡和前进动力。结果分析必要性验证固定使用12-DoF简化模型仅腿部的方法其行走性能如质心速度跟踪明显差于完整模型和VDoF-MPC。这表明对于行走这种复杂任务手臂等上身关节的惯性对于保持动态平衡是有贡献的纯粹的腿部模型不足以胜任。VDoF的智能选择如图6所示VDoF-MPC在行走过程中主要锁定了手臂的远端关节肘部、腕部而肩关节等近端关节则根据平衡需要保持活动。这既保留了上身对平衡的辅助作用又通过锁定远端不必要运动的关节实现了计算加速。计算优势VDoF-MPC在保持了与完整模型相近的行走性能的同时计算速度更快表2。这证明了其在复杂任务上的实用价值。4.4 硬件实验与实时性挑战在真实的G1机器人上进行双臂交替跟踪实验凸显了计算速度的极端重要性。延迟与振动使用完整模型29 DoF的MPC其计算几乎无法在66Hz的控制周期内完成导致了持续的指令延迟。这种延迟不仅增大了跟踪误差更严重的是引起了关节的低频振动图7的功率谱密度图显示完整模型在低频段能量更高。振动会恶化控制性能、干扰状态估计甚至触发机器人的软安全保护机制导致实验中断。VDoF的稳定性相比之下VDoF-MPC基本能跟上控制周期。虽然跟踪误差与仿真中一样略高于无延迟的理想完整模型但其质心位置误差更小整体运行更平稳没有引发有害振动。这清晰地表明在实时性约束下一个稍欠精确但及时的控制律远胜于一个更精确但延迟的控制律。VDoF-MPC通过牺牲对无关关节的毫厘级优化换来了整个系统稳定运行的基石——实时性。5. 讨论、局限与未来方向尽管VDoF-MPC表现出了巨大潜力但论文也坦诚地指出了其当前的局限性和未来的改进空间。5.1 关节锁定机制的评估与改进贡献度分数基于代价函数的梯度这本质上依赖于任务代价的设计。在论文使用的末端跟踪和质心动量最小化任务中雅可比矩阵较小的远端关节自然得分低。但如果任务代价直接是某个非冗余手臂的SE(3)位姿误差那么远端关节可能变得重要。此外当机械臂构型接近奇异点时即使近端关节的梯度也可能变小导致被误锁。未来的工作可以探索更鲁棒、更能反映关节动力学重要性的评估指标当然这些指标必须保持计算上的轻量。5.2 解锁机制的启发式与潜在改进当前的概率性解锁机制是基于历史信息的启发式方法。理想情况下应在每个控制周期都评估所有关节包括被锁定的的重要性但这会重新引入计算开销。论文采用的折中方案在实践中表现良好。一个更有前景的方向是引入一个轻量级的神经网络预测器在线预测被锁定关节的“潜在重要性”从而做出更智能的解锁决策前提是该预测器的延迟必须极低。5.3 超参数调优与自适应关键超参数 (n_u)最小活动关节数需要手动设置。虽然实验表明其鲁棒性较强但一个能根据任务复杂度自动调整 (n_u) 的机制将更具实用性。此外惩罚权重 (p) 的更新规则也可以优化例如采用类似异常值检测的方法只对贡献度极低的关节施加惩罚从而减少对参数 (\beta) 和 (p) 的敏感度。5.4 动力学近似的准确性目前的关节锁定是通过在模型中施加高摩擦实现的并非从动力学方程中彻底移除该关节。这意味着除了梯度计算被省略外其他计算如前向动力学仍基于完整模型。因此计算加速主要来自梯度计算的节省而非模型维度的彻底降低。在受到剧烈外部冲击如足部撞击时这种“软锁定”可能被破坏。要实现更彻底的加速需要在每个仿真步动态生成不同维度的模型实例这在工程实现上更具挑战。5.5 抗扰动性验证附录中的实验表明当对行走中的G1躯干施加一个侧向冲击时VDoF-MPC能够迅速激活上身关节图10来帮助恢复平衡其抗扰动性能优于固定的简化模型。这证明了保留完整模型框架的价值——当需要时全身的潜力可以被随时调用。6. 实操启示与工程考量基于论文内容和工程实践对于想要在真实机器人上尝试或借鉴VDoF-MPC思路的研究者和工程师以下几点至关重要状态估计的可靠性是基石任何基于模型的控制方法都极度依赖准确的状态反馈。对于人形机器人一个仅基于本体感知IMU、关节编码器的、鲁棒的基座位姿估计器是必不可少的。任何状态估计的延迟或漂移都会直接被控制器放大导致性能下降甚至失稳。分层控制架构是必选项不要试图让MPC直接输出500Hz或更高频率的扭矩指令。必须采用“MPC中低频前馈规划 高增益局部PD反馈高频纠偏”的分层架构。局部反馈环是抵御模型不确定性、摩擦和外部扰动的最后防线。摩擦补偿尤其是对于远端关节在扭矩控制模式下齿轮摩擦是性能的主要杀手。特别是对于需要精细力控或经常被锁定的腕部、手部、踝部关节建立一个简单的摩擦模型如库伦粘滞摩擦并进行在线或离线补偿能显著提升锁定精度和整体控制性能。超参数初始化建议如果对任务所需的必要自由度数量没有先验知识建议从较小的 (n_u) 值开始尝试。因为过大的 (n_u) 会导致不必要的关节被惩罚可能限制运动能力而过小的 (n_u) 虽然可能轻微影响性能但通常不会导致任务失败同时能带来最大的计算收益。参数 (\gamma) 建议设置在0.8-0.95之间以平衡近期和远期的影响。锁定/解锁的平滑过渡至关重要突然将关节扭矩设为零或最大值会导致冲击。论文中采用的方法值得借鉴对锁定关节使用高增益PD控制来“钳位”对刚解锁的关节在MPC的前馈指令上叠加一组低增益PD进行“温启动”使其平滑地重新融入优化过程。监控计算耗时与抖动实时性不仅是性能指标更是安全指标。必须严格监控MPC单步求解时间是否超过控制周期。如果经常超时应考虑进一步降低 (n_u) 或缩短预测时域。同时监控关节速度的功率谱密度低频抖动是指令延迟的典型标志需要及时调整控制器参数或简化模型。
VDoF-MPC：动态可变自由度模型预测控制，加速机器人实时全身控制
发布时间：2026/5/27 17:13:13
1. 项目概述当全身模型预测控制遇上“关节锁定”在足式机器人尤其是人形机器人的控制领域模型预测控制Model Predictive Control, MPC一直被视为实现动态、复杂行为的“圣杯”。它的魅力在于能够在一个统一的框架内前瞻性地规划出未来一段时间内所有关节的运动序列从而优雅地协调平衡、行走、操作等多种任务。然而这份优雅背后是沉重的计算代价。一个完整的人形机器人模型动辄拥有30个以上的自由度Degrees-of-Freedom, DoF每一次MPC求解都意味着在一个高维、非线性的状态-动作空间中进行搜索计算复杂度呈指数级增长。这使得实时例如100Hz以上的全身模型预测控制在硬件上几乎成为不可能完成的任务长期以来研究者们不得不在模型精度和计算速度之间做出痛苦的折衷。传统的简化思路是使用降阶模型比如经典的线性倒立摆模型。这就像为了快速抵达目的地你选择只规划躯干质心的轨迹而暂时忽略手臂和头部的具体摆动细节。这种方法计算飞快但代价是牺牲了模型的保真度忽略了肢体惯性等不可忽略的动力学效应在需要精细全身协调或应对突发扰动时性能会大打折扣。另一种思路是启发式地、固定地减少自由度比如在行走任务中只考虑腿部和躯干关节完全锁死手臂。这虽然简化了问题但丧失了MPC的核心优势之一——任务自适应性。当机器人需要从行走突然切换到挥手或抓取时被“遗忘”的手臂关节无法被快速重新激活。VDoF-MPC的核心思想正是为了打破这一僵局。它不再进行“一刀切”的模型简化而是引入了一种任务自适应的、动态可变自由度的机制。简单来说这套系统会在每个控制周期通常是几毫秒内像一个经验丰富的指挥官一样快速评估当前任务下每个关节的“重要性得分”。对于得分低、即对完成当前核心任务如保持平衡、跟踪末端轨迹贡献微乎其微的关节例如在直线行走时的手指关节、在静立时的远端手臂关节系统会将其动态“锁定”——在优化问题中将它们暂时视为刚体忽略其动力学计算和梯度更新。而对于核心关节则保持其完全的活动能力进行精细优化。这带来了两个立竿见影的好处第一计算加速。被锁定的关节无需进行耗时的前向动力学和梯度计算直接降低了优化问题的维度加快了求解速度。第二可能提升求解性能。高维优化问题的解空间异常复杂充斥着大量局部最优解。通过剔除无关变量的干扰优化算法实际上是在一个更干净、更聚焦的搜索空间中寻找解有时反而能更快、更好地收敛到全局更优的解。而当任务切换时比如从跟踪左手末端切换到右手系统又能根据新的任务目标动态地解锁相关关节、锁定变得无关的关节实现了真正的全身模型自适应能力。2. 核心原理如何量化关节的“任务相关性”VDoF-MPC的巧妙之处在于它利用MPC求解过程本身产生的信息来量化每个关节的“贡献度”而不是依赖预设的、死板的规则。其核心评估指标被称为“贡献度分数”。2.1 贡献度分数的计算在每一次MPC循环中求解器如论文中使用的Box-DDP都会输出一条在预测时域内的最优状态轨迹 (X_{opt}) 和最优控制输入轨迹 (T_{opt})。基于这些轨迹我们可以计算每个关节对整个代价函数 (\ell) 的贡献。贡献度分数 (s_i) 对于第 (i) 个关节的计算公式如下[ s_{i} \sum_{kt}^{tH-1} \gamma^{k-t} \left( \left| \frac{\partial \ell}{\partial x_{j,i,k}} \right| \left| \frac{\partial \ell}{\partial \tau_{i,k}} \right| \right) \gamma^H \left| \frac{\partial \ell}{\partial x_{j,i,tH}} \right| ]我们来拆解这个公式的物理意义梯度范数(| \partial \ell / \partial x |) 和 (| \partial \ell / \partial \tau |) 分别衡量了关节状态位置、速度和关节扭矩的微小变化会对总代价函数产生多大影响。梯度越大说明改变这个关节对降低任务代价如跟踪误差越有效即该关节越“重要”。时间衰减因子 (\gamma)这是一个介于0和1之间的权重因子。它使得近期时间步的梯度贡献更大远期的贡献更小。这是符合直觉的因为MPC是滚动优化的当前控制动作对近期状态的影响最为直接和关键。求和与终端项将整个预测时域内所有时间步的加权梯度范数求和并加上终端代价的梯度最终得到一个标量分数 (s_i)。注意这里计算的是代价函数关于关节状态和扭矩的梯度而不是关节速度。速度项被有意排除在贡献度计算之外因为我们的目标是评估关节对完成任务目标的贡献而不是其本身的运动剧烈程度。一个快速摆动但对任务无益的关节其重要性应该很低。2.2 从分数到动作惩罚、候选与锁定计算出所有关节的贡献度分数后系统需要决定哪些关节应该被锁定。这个过程分为三步标准化与惩罚分配首先将所有活动关节的贡献度分数进行标准化处理。系统设定了一个关键的超参数 (n_u)它表示必须保持活动状态的最小关节数量。找到分数排名在第 (n_u) 位的关节的分数 (s_{n_u})。然后根据以下规则将分数转换为速度惩罚权重 (p_i) [ p_i \leftarrow \text{clip}\left( p_i - \beta \frac{s_i - s_{n_u}}{\sigma_s}, \quad \underline{p}, \quad \overline{p} \right) ] 其中(\sigma_s) 是分数的标准差(\beta) 是缩放因子(\underline{p}) 和 (\overline{p}) 是惩罚权重的上下限。核心逻辑是对于分数低于 (s_{n_u}) 的关节其惩罚权重 (p_i) 会被显著增加因为 (s_i - s_{n_u}) 为负。这意味着在下一个MPC循环的代价函数中这些关节的运动速度项将被严重惩罚从而鼓励优化器让它们保持静止。锁定候选集更新那些贡献度分数低于第 (n_u) 名分数的关节将被加入“锁定候选集” (F_{cand})。公式表示为(F_{cand} \leftarrow { \zeta_i \in F_{act} | s_i s_{n_u} })。执行锁定在下一个MPC周期开始时系统会检查候选集中的关节。如果一个关节在预测时域内的平均角速度低于一个很小的阈值 (\epsilon)例如0.01 rad/s则认为它确实处于准静止状态便将其正式移入“锁定集” (F_{lock})。锁定在模型内部是通过给该关节施加一个极大的摩擦损耗来模拟的使其在动力学计算中如同被焊死。2.3 关节的解锁随机性与温启动被锁定的关节并非被永久“封印”。由于任务可能切换之前不重要的关节可能变得关键。然而计算被锁定关节的贡献度分数需要重新对其进行梯度计算这会抵消锁带来的计算收益。因此VDoF-MPC采用了一种轻量且巧妙的概率性解锁机制。每个被锁定的关节 (i)依据其最后一次被锁定前的贡献度分数 (s_i)以及当前活动关节中的最低分数 (s_{n_u})按以下概率进行解锁尝试 [ P(\text{unlock}i) \frac{s_i}{s{n_u} s_i} ] 这个概率计得很直观如果一个关节历史重要性分数很高接近当前活动关节的最低水平那么它被解锁的概率就接近50%如果历史分数极低则解锁概率也极低。一旦关节被解锁为了平稳过渡需要为其提供一个优化的“温启动”初值。论文中的做法是使用一组较低比例的PD比例-微分反馈增益根据关节的当前位置误差和速度误差生成一个初始控制序列作为优化器向前传递的起点。这避免了关节从完全静止状态突然被纳入优化时可能产生的剧烈抖动和动力学失配。3. 系统实现与工程细节将VDoF-MPC从理论公式落地到真实的机器人控制系统需要解决一系列工程挑战。论文以Unitree G1人形机器人为平台详细阐述了其实现方案。3.1 整体控制架构由于MPC的计算频率论文中为66Hz远低于底层电机驱动的控制频率500Hz直接使用MPC输出的扭矩指令是不现实的。因此系统采用了分层控制架构高层VDoF-MPC66Hz运行核心算法根据当前状态和任务目标求解出最优的关节扭矩序列 (\tau_{cmd})。同时根据贡献度分数动态决定关节的锁定与解锁状态。中层状态估计500Hz使用基于扩展卡尔曼滤波的位姿估计器仅依靠本体感知传感器IMU、关节编码器实时估计机器人基座躯干在空间中的位置和姿态。这是浮动基座机器人控制的基础。底层局部反馈扭矩控制500Hz将MPC计算出的前馈扭矩 (\tau_{cmd}) 与基于状态误差的高增益PD反馈扭矩相结合生成最终的关节指令 (\tau_{local})。 [ \tau_{local} \tau_{cmd} K \delta x ] 其中 (K) 是反馈增益矩阵(\delta x) 是状态误差。这层高频率的反馈环用于抵抗模型误差、外部扰动和执行器动态。最终输出混合根据VDoF-MPC给出的关节状态活动/锁定最终发送给执行器的扭矩 (\tau_{out}) 是不同的对于活动关节(\tau_{out,i} \tau_{local,i} k_{p,low,i} \delta q_i k_{d,low,i} \delta \dot{q}_i)。这里额外添加了一组低增益PD项目的是在关节刚被解锁时提供柔和的恢复力。对于锁定关节(\tau_{out,i} k_{p,i} \delta q_i - k_{d,i} \dot{q}_i)。这里使用一组高增益的PD控制器强行将关节稳定在锁定时的位置并阻尼其速度模拟“刚性锁定”的效果。3.2 摩擦补偿与执行器处理真实的机器人关节存在显著的摩擦尤其是采用高减速比齿轮箱的关节。G1机器人没有关节扭矩传感器其前馈扭矩 (\tau_{cmd}) 是基于电机电流估计的。对于腕部、脚踝等输出扭矩较小的远端关节齿轮摩擦的影响尤为突出。为了补偿摩擦论文采用了Stribeck摩擦模型对远端关节进行建模和补偿 [ \tau_{fric} \left[ F_c (F_s - F_c) \exp\left(-\left(\frac{|\dot{q}|}{v_s}\right)^2\right) \right] \text{sgn}(\dot{q}) ] 其中(F_c) 是库伦摩擦(F_s) 是静摩擦(v_s) 是Stribeck速度。通过对实验数据进行非线性最小二乘拟合可以估计出这些参数。补偿后的最终底层指令变为 [ \tau_{local} \tau_{cmd} K\delta x \tau_{fric} ] 这项补偿对于提高锁定关节的控制精度、减少稳态误差至关重要。3.3 动力学计算加速的具体实现关节锁定带来的计算加速主要体现在梯度计算的省略上。在基于梯度的优化器如DDP中需要反复计算动力学模型 (f(x, \tau)) 关于状态 (x) 和控制输入 (\tau) 的雅可比矩阵导数。通常这需要通过有限差分法进行数值近似需要对每个控制输入维度都进行一次前向动力学计算。当关节 (j) 被锁定后在动力学计算中该关节的角度 (q_j) 被固定为锁定时的值 (q_{j,lock})。该关节的速度 (\dot{q}_j) 被设为零。在计算导数时关于被锁定关节扭矩的导数被直接设为零 [ \frac{\partial f(x_t, \tau_t)}{\partial \tau_{j,lock}} 0, \quad \frac{\partial f(x_t, \tau_t){j}}{\partial \tau} 0 ] 这意味着在有限差分计算中完全不需要为了这个被锁定的关节而额外调用前向动力学函数。对于一个有 (n_u) 个活动关节、(n{lock}) 个锁定关节的系统梯度计算量从 (O(n_u n_{lock})) 降低到了 (O(n_u))。在预测时域较长、关节总数较多时这种节省是相当可观的。4. 实验验证与性能分析论文通过仿真和实物实验从多个维度验证了VDoF-MPC的有效性。实验平台包括Franka Emika Panda机械臂带Allegro手和Unitree G1人形机器人。4.1 Panda机械臂指尖跟踪任务这是一个相对简单的任务旨在验证方法的基本特性。机器人需要控制其末端指尖跟踪一个给定的空间轨迹。对比方法Box使用完整模型23 DoF的基准Box-DDP方法。Reduced使用固定简化模型仅7个手臂关节手指关节被完全移除的方法。MH模型分层方法在预测时域的前半段使用完整模型后半段使用简化模型。结果分析计算速度VDoF-MPC的计算时间显著低于完整的Box方法与MH方法相当但远慢于固定的7-DoF简化模型。这符合预期因为VDoF仍需维护和评估完整模型的结构。跟踪精度令人惊讶的是VDoF-MPC的跟踪误差低于完整的Box方法。论文分析认为对于指尖跟踪这个任务手指关节本就是冗余的。完整模型的优化器在优化手指关节的冗余运动上“浪费”了搜索精力反而使代价函数地形更复杂阻碍了进一步优化。VDoF通过锁定这些无关关节简化了问题空间使优化器能更专注于对手臂关节的优化从而找到了更好的解。自由度变化如图3所示无论任务代价函数是跟踪整条轨迹还是仅跟踪终点VDoF-MPC锁定的关节模式几乎一致手指关节被大量锁定而完成跟踪所必需的手臂关节始终保持活动。这证明了贡献度评分机制的有效性。超参数 (n_u) 的影响如图4所示将 (n_u) 设得过大如15或23接近总自由度性能会下降因为过多的关节被施加了速度惩罚。而将 (n_u) 设得过小如3或0性能也有轻微下降。这表明存在一个合适的 (n_u) 范围在本任务中约为7既能充分简化问题又不会因过度惩罚而限制必要运动。4.2 G1人形机器人双臂交替跟踪任务这个任务更具挑战性要求机器人在保持全身平衡的同时交替用左手和右手末端执行器跟踪空间轨迹。任务在运行中会动态切换目标手臂。结果分析动态适应性如图5所示VDoF-MPC成功展示了其核心优势——任务自适应的自由度切换。当跟踪目标从左臂切换到右臂时系统自动减少了当前不活跃手臂右臂的自由度同时释放了即将活跃的手臂左臂的自由度。整个过程平滑机器人成功完成了任务切换。性能权衡与完整模型相比VDoF-MPC的跟踪误差略有增加表3。这是因为动态锁定和解锁机制并非完美会引入微小的瞬态误差。然而其计算速度更快表2。在实时控制中计算延迟本身就是一种性损失。更快的求解意味着更低的指令延迟这对于动态任务至关重要。4.3 G1人形机器人行走任务行走是一个复杂的周期性任务需要协调全身多个关节以维持动态平衡和前进动力。结果分析必要性验证固定使用12-DoF简化模型仅腿部的方法其行走性能如质心速度跟踪明显差于完整模型和VDoF-MPC。这表明对于行走这种复杂任务手臂等上身关节的惯性对于保持动态平衡是有贡献的纯粹的腿部模型不足以胜任。VDoF的智能选择如图6所示VDoF-MPC在行走过程中主要锁定了手臂的远端关节肘部、腕部而肩关节等近端关节则根据平衡需要保持活动。这既保留了上身对平衡的辅助作用又通过锁定远端不必要运动的关节实现了计算加速。计算优势VDoF-MPC在保持了与完整模型相近的行走性能的同时计算速度更快表2。这证明了其在复杂任务上的实用价值。4.4 硬件实验与实时性挑战在真实的G1机器人上进行双臂交替跟踪实验凸显了计算速度的极端重要性。延迟与振动使用完整模型29 DoF的MPC其计算几乎无法在66Hz的控制周期内完成导致了持续的指令延迟。这种延迟不仅增大了跟踪误差更严重的是引起了关节的低频振动图7的功率谱密度图显示完整模型在低频段能量更高。振动会恶化控制性能、干扰状态估计甚至触发机器人的软安全保护机制导致实验中断。VDoF的稳定性相比之下VDoF-MPC基本能跟上控制周期。虽然跟踪误差与仿真中一样略高于无延迟的理想完整模型但其质心位置误差更小整体运行更平稳没有引发有害振动。这清晰地表明在实时性约束下一个稍欠精确但及时的控制律远胜于一个更精确但延迟的控制律。VDoF-MPC通过牺牲对无关关节的毫厘级优化换来了整个系统稳定运行的基石——实时性。5. 讨论、局限与未来方向尽管VDoF-MPC表现出了巨大潜力但论文也坦诚地指出了其当前的局限性和未来的改进空间。5.1 关节锁定机制的评估与改进贡献度分数基于代价函数的梯度这本质上依赖于任务代价的设计。在论文使用的末端跟踪和质心动量最小化任务中雅可比矩阵较小的远端关节自然得分低。但如果任务代价直接是某个非冗余手臂的SE(3)位姿误差那么远端关节可能变得重要。此外当机械臂构型接近奇异点时即使近端关节的梯度也可能变小导致被误锁。未来的工作可以探索更鲁棒、更能反映关节动力学重要性的评估指标当然这些指标必须保持计算上的轻量。5.2 解锁机制的启发式与潜在改进当前的概率性解锁机制是基于历史信息的启发式方法。理想情况下应在每个控制周期都评估所有关节包括被锁定的的重要性但这会重新引入计算开销。论文采用的折中方案在实践中表现良好。一个更有前景的方向是引入一个轻量级的神经网络预测器在线预测被锁定关节的“潜在重要性”从而做出更智能的解锁决策前提是该预测器的延迟必须极低。5.3 超参数调优与自适应关键超参数 (n_u)最小活动关节数需要手动设置。虽然实验表明其鲁棒性较强但一个能根据任务复杂度自动调整 (n_u) 的机制将更具实用性。此外惩罚权重 (p) 的更新规则也可以优化例如采用类似异常值检测的方法只对贡献度极低的关节施加惩罚从而减少对参数 (\beta) 和 (p) 的敏感度。5.4 动力学近似的准确性目前的关节锁定是通过在模型中施加高摩擦实现的并非从动力学方程中彻底移除该关节。这意味着除了梯度计算被省略外其他计算如前向动力学仍基于完整模型。因此计算加速主要来自梯度计算的节省而非模型维度的彻底降低。在受到剧烈外部冲击如足部撞击时这种“软锁定”可能被破坏。要实现更彻底的加速需要在每个仿真步动态生成不同维度的模型实例这在工程实现上更具挑战。5.5 抗扰动性验证附录中的实验表明当对行走中的G1躯干施加一个侧向冲击时VDoF-MPC能够迅速激活上身关节图10来帮助恢复平衡其抗扰动性能优于固定的简化模型。这证明了保留完整模型框架的价值——当需要时全身的潜力可以被随时调用。6. 实操启示与工程考量基于论文内容和工程实践对于想要在真实机器人上尝试或借鉴VDoF-MPC思路的研究者和工程师以下几点至关重要状态估计的可靠性是基石任何基于模型的控制方法都极度依赖准确的状态反馈。对于人形机器人一个仅基于本体感知IMU、关节编码器的、鲁棒的基座位姿估计器是必不可少的。任何状态估计的延迟或漂移都会直接被控制器放大导致性能下降甚至失稳。分层控制架构是必选项不要试图让MPC直接输出500Hz或更高频率的扭矩指令。必须采用“MPC中低频前馈规划 高增益局部PD反馈高频纠偏”的分层架构。局部反馈环是抵御模型不确定性、摩擦和外部扰动的最后防线。摩擦补偿尤其是对于远端关节在扭矩控制模式下齿轮摩擦是性能的主要杀手。特别是对于需要精细力控或经常被锁定的腕部、手部、踝部关节建立一个简单的摩擦模型如库伦粘滞摩擦并进行在线或离线补偿能显著提升锁定精度和整体控制性能。超参数初始化建议如果对任务所需的必要自由度数量没有先验知识建议从较小的 (n_u) 值开始尝试。因为过大的 (n_u) 会导致不必要的关节被惩罚可能限制运动能力而过小的 (n_u) 虽然可能轻微影响性能但通常不会导致任务失败同时能带来最大的计算收益。参数 (\gamma) 建议设置在0.8-0.95之间以平衡近期和远期的影响。锁定/解锁的平滑过渡至关重要突然将关节扭矩设为零或最大值会导致冲击。论文中采用的方法值得借鉴对锁定关节使用高增益PD控制来“钳位”对刚解锁的关节在MPC的前馈指令上叠加一组低增益PD进行“温启动”使其平滑地重新融入优化过程。监控计算耗时与抖动实时性不仅是性能指标更是安全指标。必须严格监控MPC单步求解时间是否超过控制周期。如果经常超时应考虑进一步降低 (n_u) 或缩短预测时域。同时监控关节速度的功率谱密度低频抖动是指令延迟的典型标志需要及时调整控制器参数或简化模型。
