如果你接触过腿足机器人控制，那一定绕不开 Cassie 这个名字。这台外形像鸵鸟的双足平台能跑、能跳、能上下楼梯，还能在户外草地上稳定行走。2018 年那篇关于它反馈控制的论文，是很多人入门双足机器人模型预测控制（MPC）绕不开的经典。我也把这篇论文来回读了好几遍，结合平时调试倒立摆模型和 MPC 的小经验，整理了一份更偏“工程视角”的阅读笔记。这里不打算逐句翻译论文，而是想把“作者为什么这么设计、哪些地方值得深挖、我自己照着复现时踩过的坑”讲透。
这篇笔记适合正在入门双足机器人控制、准备读论文但又怕被公式劝退，或者已经在实验室里养过一段时间机器人、想换一种控制思路做参考的人。读完你至少能弄清楚一件事：Cassie 这种没有大脚板、没有明显踝关节位置反馈的机器人，到底是怎么把“稳住质心”这件事拆成一个可在嵌入式环境里跑起来的优化问题的。
1. 先说结论：这篇论文到底“神”在哪里
1.1 Cassie 是谁，为什么值得单独写一篇阅读笔记
Cassie 是一台双足机器人平台，身高一米出头，整机不重，两条腿的构型很特殊：每条腿有髋部三个方向的自由度、膝关节和踝关节，膝盖上方还带了一套四连杆机构，把驱动电机抬高，降低小腿惯量。脚踝位置其实没有传统意义上的大平板脚掌，而是类似“脚尖点地”的小支撑区域，所以整机看起来像一只鸵鸟。
这种结构有两个直接后果。第一，它非常适合研究“动态行走”：重心位置高、支撑面小，必须靠连续迈步来维持平衡，而不是靠站桩。第二，它的脚踝不能像人形机器人那样主动把整只脚压平，控制模型里“ZMP 点在支撑多边形内部”这种经典约束变得不那么好用。所以 2018 年的这篇反馈控制论文，本质上是在探索一条新路：不再死守零力矩点（ZMP），而是直接用简化的质心动力学做在线预测，让落脚点去主动“接住”质心。
我读这篇论文前，自己手上的实验平台还停留在“静态稳定”的控制思路，就是必须保证零力矩点稳稳落在支撑区域中间，走一步停几秒。看完 Cassie 这套方案后，我对“动态稳定”有了完全不一样的感受：原来平衡不是哪个控制周期瞬间锁出来的，而是让整个身体在每一步之间主动“倒向前方”，再用下一步接住自己。
1.2 2018 年这篇论文的核心诉求是什么
如果用一句话概括，这篇论文想做的是：让 Cassie 在不知道精确地形、不知道怎么建模每一步接触力的情况下，依然能在多种不平整地面上稳定行走。
作者团队并没有把控制做成“每走一步前查表”，而是把控制任务拆成了三层：先用一个非常轻量的简化模型预测机器人质心在未来一秒左右会怎么运动，然后在线优化出接下来几步的落足点，最后再由底层控制器把“质心该有的加速度”映射成各关节的力矩。这样拆的好处是：模型重量轻，优化可以在线跑；对地形的依赖降到最低，因为落脚点是按回收质心的逻辑产生的，而不是按预设轨迹走的。
所以读这篇论文时，不要只盯着 MPC 三个字母看。真正关键的是“落脚点如何进入动力学”，以及“简化模型丢掉了哪些东西、保住了哪些东西”。前者决定了控制能不能闭环，后者决定了这个控制到底稳不稳。
1.3 开读之前，最好先有这几个基础概念
我给准备读原文的朋友整理了一个预习清单，能省很多时间：
- 线性倒立摆模型（LIPM）：把机器人简化为一个质心和一个可移动的支撑点，支撑点在地面移动，质心高度近似恒定。
- FPE（Foot Placement Estimator）：给定当前质心位置和速度，估算一个合适的落脚点，使质心动量归零。
- MPC 的基本思路：在每个控制周期，基于当前状态滚动求解未来一小段时间的最优输入，只执行第一步，然后重新求解。
- 操作空间控制（OSC）：把任务空间（如质心加速度）映射到关节力矩，需要用到动力学和雅可比矩阵。
如果之前完全没接触过这些，建议先花半天时间把倒立摆模型推一遍，再来看论文会舒服很多。论文里的公式本身不算难，难的是你要理解每一步“为什么用这个模型、为什么丢那个约束”，这个我会在后面展开。
2. 控制方案的整体设计：分层级“减负”，用简化模型做预测
2.1 完整多体动力学模型为什么不能直接上 MPC
一提到机器人控制，工程本能会想：能不能把 Cassie 几十个自由度的完整动力学全放进预测模型里，然后每个控制周期内做一次最优控制？理论上没问题，实践上非常痛苦。
首先，完整模型是非线性的，求解一次最优控制本身就慢。步态频率一高，控制周期可能只有 20 到 50 毫秒，普通嵌入式处理器根本跑不完一个包含躯干、双腿、四连杆机构的非线性优化。其次，完整模型对参数误差很敏感，关节摩擦、弹簧刚度、质心位置偏差都会让模型失配，预测得越“精细”，一旦参数不对，控制反而乱得更快。
还有一个容易被忽略的问题：接触力约束。双足机器人在行走时，脚与地面之间只有单边接触力，脚掌不能“拉住”地面，也不能穿透地面。这种不等式约束放进非线性优化里，会让问题难度进一步增大。作者团队选择“丢弃”完整模型，不是偷懒，而是在算力、稳定性和实时性之间做取舍。
2.2 简化模型：倒立摆加角动量的“留与舍”
论文里用的核心模型可以看作 3D-LIPM 的扩展版：把机器人看成两个主要质点——一个身体质心，一个摆动腿质心，同时保留了质心处角动量的演化。为什么这么拆？
Cassie 上半身（躯干和髋部）的重量占整机很大比例，可以近似为一个有质量的刚体；腿部因为结构设计得比较轻，对整体质心位置的影响相对有限。控制目标自然就变成：控制躯干姿态、控制整体质心高度、控制质心水平运动。至于两条腿具体摆成什么角度、膝关节弯曲多少，这些是底层关节控制器该管的事，不用全部塞进预测模型。
模型“留”下来的状态包括：质心三维位置、质心三维速度、角动量。输入则是落脚点位置和支撑时间。另一个重要的简化是：把落脚当作“瞬间改变支撑点位置”的离散事件来处理，而不是把脚从抬起到落下整个过程都建模进去。这样每个控制周期里，系统状态跳变点是清楚的，优化问题也变得可解。
我的体会是，这套“留与舍”的思路非常值得借鉴。很多人做 MPC 时喜欢往模型里拼命加细节，觉得越接近真实系统越好，结果优化负担重、参数又难调。Cassie 这篇论文告诉我们：模型的首要目标是抓住“平衡和移动”的主要矛盾，而不是复刻整机的每一个动力学细节。
2.3 三步走的控制管线：预测、优化、执行
论文整体控制结构可以归纳为一条流水线：
- 状态估计：通过本体传感器估计质心位置、速度和姿态，作为 MPC 的输入。
- 落脚点优化：MPC 基于当前状态，预测未来约 1 秒的质心运动，同时优化出接下来若干步的落脚点序列。
- 底层控制：把 MPC 给出的期望质心轨迹转换成地面对质心的期望力，再通过操作空间控制映射到关节力矩，最后由电机执行。
这条管线最经典的地方在于：MPC 并没有直接算关节角度，而是算落脚点。落脚点是一个低维、物理含义明确的变量。工程师可以直观地约束它：落脚点不能太远、不能太近、不能落在身体侧面太夸张的位置。这样既保证了稳定性，又方便在实际系统里加入各种工程限制。
我在实验室复现这条管线时，最大的感受是“分工明确”。上层 MPC 不用关心两条腿怎么协调迈步，底层控制器也不用去理解平衡逻辑，只管把想要的力跟踪好。出问题时也很好排查：如果走不稳，先看底层是否跟踪住了；如果跟踪住了还是稳不住，再去调 MPC 的代价权重。这个解耦思路，比传统“生成一整段步态轨迹再单点跟踪”的方案要抗干扰得多。
3. MPC 与落足点规划的核心机制
3.1 我眼中最关键的设置：落脚点如何进入动力学公式
很多人第一次读这篇论文会觉得困惑：MPC 不是控制机器人关节吗？为什么最后出来的是落脚点坐标？
关键在倒立摆模型的离散化处理。想象机器人处在单腿支撑阶段，质心位置和速度已知。如果我们忽略支撑腿产生的额外角动量，那么质心加速度主要由支撑点的位置决定——支撑点相对质心越靠前，质心就会因为倒立摆效应被“顶”得越向后加速；支撑点越靠后，质心就越容易向前倒下。
于是落脚点就变成了控制输入，模型可以写成：
x_{k+1} = f(x_k, u_k)其中状态 x 包含质心位置和速度（以及角动量），控制量 u 就是下一步落脚点相对质心的位置。MPC 在每个控制周期里，会预测从当前时刻开始未来若干步的状态轨迹，同时求解一组落脚点序列，使得预测的状态尽量接近期望状态。这里“接近”不只是位置接近，还包括速度，因为双足动态行走时，质心水平速度不能降为零，必须保持一个“向前倒”的节奏，下一步才能继续迈出。
我建议读到这里时，动手把倒立摆方程推一遍。哪怕只在一维平面上推，你也会发现：质心加速度和支撑点位置之间是反直觉的。支撑点放在身体前方，质心不是向前跑，而是因为重力线相对支撑点产生了一个向后倒的力矩。理解了这一点，后面再看 FPE 和落脚点约束就顺了。
3.2 代价函数里每一项的“物理含义”
论文中 MPC 的目标函数不会写得像教科书那么啰嗦，核心可以拆成几项：
- 质心位置偏离期望位置的代价：保证机器人整体不会一路漂移，每步走完之后仍然在预定路线上。
- 质心速度偏离期望速度的代价：动态行走必须维持一个目标速度，不能让机器人越走越慢最后停下来。
- 落脚点的代价：让落脚点不要过度偏离参考步态，防止步幅忽大忽小。
- 控制输入变化量的代价：比如相邻两步的落脚点不要突变，避免身体重心被“瞬移”到奇怪的位置。
每一项的权重矩阵 Q 和 R 决定了控制系统“关心什么”。实测中我发现，Q 里位置权重调大，机器人会表现得特别“急着回中”，每一步都死死逼近参考位置，结果步幅变化剧烈；但如果位置权重太小，机器人会变得懒散，质心一路漂移。对于 Cassie 这种高动态平台，速度权重往往比位置权重更敏感，因为在连续行走中，质心速度的连续性比瞬时位置更重要。
另外要注意，MPC 解出来的落脚点是一个在优化时域内“未来所有步”的序列，但执行时只取第一步，然后滚动重新求解。这就是滚动时域控制（RHC）的意义：每控制周期都能拿到最新的状态估计，把误差实时修正掉，也让模型预测偏差不至于累积。
3.3 稳定性边界如何落成约束
MPC 的约束设计在这篇论文里很见功力。落脚点不是随便选的，至少要满足以下几点：
- 在地面接触允许的范围内，不能悬空、不能伸进地形内部。
- 支撑脚不能离身体质心的投影太远，否则关节力矩根本撑不住。
- 落脚点变化率要有限制，不能上一步在左边，下一步瞬间跳到右边。
除此之外，论文里还隐含了一个非常关键的稳定性思想：落脚点应该让质心在支撑时间结束后的状态尽量“归一化”，也就是让质心速度与位置构成一个稳定的不变关系。这个思想其实可以用 FPE 来理解。
所谓 FPE，是指给定一个倒立摆的当前状态，存在一个理想的支撑点位置，使得当质心到达某设定高度时，它的水平动能恰好被重力势能和摆动过程完全转化，质心运动终止在期望状态上。如果在每个支撑相结束前，MPC 选择的落足点都恰好落在 FPE 附近，那么每一步结束时，质心的状态就天然处于一个“可控”的区域内。这样即使地形给予了意想不到的扰动，系统也总能通过调整下一步落脚点把状态拉回安全范围。
我在自己搭的双足仿真里试过：直接把 FPE 当成唯一落脚点规则，不做 MPC，也能实现短时间的步行；但一旦遇到斜坡或者冲击，几步之后就会失衡。原因很简单，FPE 是“单步视角”的稳定策略，它只保证这一步结束时的动量状态合理，却不考虑未来几步之间的关联。MPC 的价值恰恰在于把这个“单步合理”扩展成了“多步最优”，你可以让控制器为了三步之后的稳定状态，主动牺牲当前这一步的位置精度。
3.4 对参考轨迹和地形信息的依赖到底有多小
之前看速报时很多人问：Cassie 是不是靠预存地形图走路的？答案是否定的。2018 年这套方案的精妙之处在于，它不需要精确地形模型，只需要知道脚与地面接触后会不会滑、落脚高度差大致什么范围这些粗略信息。
用简化模型做预测带来的一个好处，就是 MPC 不会去预测“脚碰地面瞬间的冲击动力学”。脚落地时那一瞬间的高频振动，是由底层阻抗控制和脚踝弹簧结构与实际地面环境共同吸收的。这反而是一种工程上的友善设计：把不确定的部分留给机械结构和底层控制，让上层决策只用面对不敏感的主干动力学。
不过如果你也打算复现，请注意：不需要地形模型不等于不需要状态估计。MPC 对质心位置、质心速度的估计精度要求其实很高，特别是质心速度，直接决定了倒立摆模型的短期预测准不准。论文中 Cassie 的状态估计用了滤波器融合，我自己的经验是加速度计和关节编码器之间的时间同步一旦有问题，MPC 马上就“发疯”，比参数失配更致命。
4. 从质心轨迹到关节力矩：底层执行与整机映射
4.1 MPC 输出之后，底层控制器在做什么
MPC 给出的是落脚点序列，但电机需要的是关节力矩，这中间还有一大截路。底层控制部分必须先做两件事：
第一，根据期望的落脚点和当前支撑脚位置，生成一条“摆动腿轨迹”。通俗说就是决定腿要从当前位置抬起来、经过什么样的曲线、在多长时间内落到目标点。这条轨迹不需要在整个预测时域提前算好，只需要在当前控制周期内满足平滑性和末端位置即可。
第二，把 MPC 给出的期望落脚点转换成一个“质心期望受力”。这一步是操作空间控制的核心。思路是先设定一个质心的期望加速度，然后利用 Cassie 的简化多体动力学，算出生成这个加速度需要地面给机器人多大的反作用力和力矩。由于机器人只有支撑脚与地面接触，地面反作用力也就等于我们需要的控制力。
如果你写过机器人逆动力学，会发现这里有个微妙的地方：Cassie 的脚踝没有大脚掌，所以支撑脚能提供的抗力矩范围相对有限。操作空间控制器必须保证“期望力矩不能超过支撑脚能提供的极限”，否则脚会打滑。所以在落地控制中，我会额外加一个“脚底压力估计”或“接触力矩饱和”的约束，让期望力矩不超过实际摩擦锥和脚踝结构允许的范围。论文在实现细节里也对反作用力做了约束处理，这一点很值得学习。
4.2 落地冲击、弹簧与阻抗控制
Cassie 脚踝结构里有串联弹性元件，这在机械层面给系统增加了很多“容错空间”。控制上，底层每个关节都带有类似阻抗控制的效果：期望位置和期望速度通过弹簧-阻尼模型换算成力矩，而不是用高增益位置 PID 去强行盯死角度。
这种设计的优势在落地瞬间体现得特别明显。刚性位置控制让脚触地时冲击力很大，甚至会把机器人“弹”起来；阻抗控制则让关节在触地瞬间轻微退让，把冲击动能先吸收一部分。MPC 上层按倒立摆模型假设质心高度大致恒定，这个假设只有在脚落地冲击能迅速被吸收时才能成立。所以你会发现：Cassie 走得稳，不只是控制器算得好，机械结构也为这个控制架构做了贡献。
实际调试中，我习惯把脚踝的等效刚度调低一点，让落地冲击先作用在弹簧上，而不是直接传到膝关节。但刚度低了也容易出问题：支撑相中脚踝会被压得变形太多，导致质心高度明显下降，倒立摆模型的“恒高假设”失效。更好的办法是给阻抗控制加重力前馈，在支撑相主动补偿一部分弹簧压缩产生的力，这样落地柔，站定时又不至于塌下去。
4.3 从论文到仿真复现：我常用的三段式调试路径
如果你想把这套方案搬到自己的仿真环境里，我建议按以下路径来，能少走很多弯路：
- 第一段：固定落脚点，先只调底层阻抗控制和操作空间控制，让机器人能单腿站稳。这个阶段的目标不是走，而是验证“质心期望力”到“关节力矩”的映射关系是对的。哪怕机器人走路前都会摔倒，只要站着不倒、轻微扰动后能回稳，底层就算过关。
- 第二段：把 MPC 换成单步 FPE 逻辑，只规划当前这一步的落脚点，让机器人尝试走两三步。这个阶段主要调 FPE 公式里的高度和时间参数。
- 第三段：再把 MPC 接上，并把预测时域拉长，观察落脚点序列是否平滑、质心速度是否接近期望值。
我在第二段到第三段切换时遇到过一个问题：只用 FPE 时机器人能走两步，一旦接上 MPC，反而容易一步迈得特别大。排查了半天发现是 MPC 里落脚点的位置权重没调，MPC 认为“远离参考步态”无代价，于是倾向于用大步幅去稳速度。把这一项权重加上之后，步幅立刻恢复正常。这类问题论文里不会写，只有实际操作才能体会到。
5. 常见问题排查与参数调优实战
5.1 我在复现中遇到的三个高频问题
问题一：机器人原地打转，明明 MPC 输出了迈步方向，身体却不向前移动。
这个多半是质心速度估计有偏差。双足机器人行走时，身体会在迈步间隙出现轻微前后振荡，如果滤波器对速度估计有延迟，MPC 会认为质心“太快了”，每一步都拼命把落脚点往前放，结果变成原地踏步。解决思路：增加状态估计器中加速度计数据的权重，或者减小 MPC 速度代价的权重，让控制器容忍一点速度波动。
问题二：落脚点频繁抖动，看起来像每一步都在随机重规划。
MPC 解不稳定通常是因为相邻控制周期之间优化起点差异太大，或者每一步落脚点的代价权重太小。我建议给 MPC 的目标函数加一个“落脚点变化量”惩罚项，尽可能让两次滚动求解出来的同一落脚点位置连续。另一个小技巧是把优化问题的初值设置为上一周期的最优解，这样即使噪声存在，解也更稳定。
问题三：机器人能走，但越走越低，最后蹲在地上。
这是质心高度控制出了问题。底层控制器把地面反作用力的垂直分量设定为“维持当前高度”，但如果 MPC 和其他扰动让质心速度向下，高度误差会累积。Cassie 的模型在垂直方向同样可以用倒立摆描述，我建议在 MPC 状态里显式加入垂直高度和垂直速度，并且对垂直速度设置一个较小的负反馈，把它当“有反弹”的弹簧来处理。
这里把常见问题汇总成一个速查表：
| 现象 | 可能原因 | 排查顺序 |
|---|---|---|
| 原地打转 | 质心速度估计延迟、MPC 速度权重过大 | 先查滤波器，再调 MPC 权重 |
| 落脚点抖动 | 优化起点跳跃、缺少增量代价 | 给优化加初值热启动，再加步幅平滑项 |
| 越走越低 | 垂直速度累积误差、阻抗刚度不足 | 先看脚踝等效刚度，再检查高度反馈 |
| 迈步过大 | 落脚点参考权重过低 | 调高 R 矩阵中参考步幅的惩罚 |
| 身体前后摆幅过大 | 角动量项没参与预测 | 确认角动量状态是否进了 MPC，调角动量权重 |
5.2 调参顺序与入门配置经验
MPC 权重参数看似很多，其实可以先按“三组”来理解，不要一个权重一个权重地调试：
- 第一组：质心轨迹组，控制位置和速度误差。先固定速度权重，调位置权重；在仿真里给机器人一个横向速度扰动，观察它几步能回到稳定状态。
- 第二组：落脚点组，控制步长和步宽。典型值可以先用一个很小的参考步宽，然后看机器人是否能维持步态。
- 第三组：时序组，包括支撑时间和过渡时间。MPC 预测时域内有多少步，在很大程度上依赖支撑时间参数，我见过的现象是：支撑时间太短，MPC 不敢迈大步，机器人会“碎步”；太长，优化时域跨度变大，计算量上升，同时稳定性变差。
我个人建议入门时先把仿真步长固定，把所有权重归一化到同一量级，比如位置用米、速度用米每秒、角度用弧度，避免出现“位置误差权重是速度误差权重的一万倍”这种量级灾难。Cassie 论文展示的成果看似轻松，但如果你也像我一样从读公式到自己调通，大概会发现困难不在数学推导，而在“如何让每个模块在实时系统里不互相打架”。
5.3 准备动手前我需要提醒你的一件事
不管是在仿真环境还是实体平台复现，一定要保证 MPC 的运行时间远小于控制周期。如果 MPC 求解耗时接近控制周期，整个系统会进入“优化还没完、底层控制已经在等”的恶性循环，这在双足平台上特别容易表现为髋关节颤抖。解决办法不是单纯换更快的电脑，而是合理裁剪预测时域、减少决策变量数量，甚至把落脚点坐标先参数化，再用曲线拟合去缩小求解规模。
还有一点很实际：论文里的仿真结果和实体结果永远有差距，重点不是复现一模一样的数据，而是复现“控制和稳定逻辑”。把论文核心思想搬到自己的平台上时，质心位置、腿部杆长、电机力矩这些参数全部都不一样，公式里的增益系数不能照抄，需要按你的模型重新标定。我见过不少新手把论文中用米每秒为单位的权重直接抄到自己毫米级单位仿真里，结果优化直接无解。单位统一这件事，越早做越省钱。
6. 顺着这篇论文，还能往哪个方向走
读完整篇论文，我最大的收获并不是 MPC 本身，而是它展示了一种控制设计的方法论：用最便宜的模型算决策，把不确定性留给机械和底层。这套思路后来在很多腿足方案中都能看到影子，比如动态跳跃的落脚点优化、四足机器人的重心轨迹规划。如果你未来想研究更强壮的足式运动，比如越障、跳跃、连续毫秒级冲击下的平衡，这篇论文的打底逻辑依然适用——只是你需要把落足点之外的姿态变化也加入优化变量，同时想办法让 MPC 的预测模型在接触瞬间的切换更平滑。
我自己读完后的下一步计划是：试着把角动量项真正用起来。因为地面反作用力除了改变质心平移，还会对躯干产生力矩，Cassie 在转折和变速时，躯干姿态的轻微摆动其实影响很大。论文里对这块有处理，但还不够多，我的后续实验会从“如何让 MPC 主动利用角动量去辅助转弯”入手，看看能不能把转弯稳定性再提一档。
最后，给你一个最实际的建议：读这类论文，不要指望一遍看懂。第一次读，把控制流程图画出来，标清楚每一级的输入输出是什么；第二次读，去推最核心的那一两个简化公式；第三次读，再回到仿真里修改权重和约束，把每一步错误一一踩一遍。这个过程给我的启发远远大于论文本身。毕竟，反馈控制的价值，永远是在系统偏离预期时才真正显露出来的。
