四足机器人 MPC 的数学建模与 QP 推导

很多关于四足机器人 MPC 的文章，要么停留在“模型预测控制很适合动态 locomotion”的概念层面，要么直接从论文里搬出一组矩阵，读者知道符号长什么样，却不知道这些矩阵为什么会这样出现。真正做开发时，最难的部分并不是把现成公式抄进代码，而是要把几个问题真正想清楚：为什么整机建模时通常从单刚体模型开始，而不是直接从全身动力学开始；为什么优化变量通常选足端接触力，而不是关节力矩；连续时间动力学怎样走到离散时间模型；离散预测模型怎样进一步变成一个标准的二次规划问题；摩擦锥、摆动腿零力和法向力上下限这些约束又是怎样写进 QP 的。

这篇文章就只做一件事：把四足机器人 MPC 里最核心的数学链条，从建模一路推到 QP。它不是一篇 broad overview，也不是一篇以工程经验为主的调参手册，而是偏理论和可实现之间的那一层。写完这一篇之后，你至少应该能够回答下面几个关键问题。第一，MPC 在四足机器人中控制的对象到底是什么。第二，单刚体模型的状态变量和输入变量该怎样选。第三，为什么预测控制问题最终会落成一个 QP，而不是一堆看不出结构的优化式。第四，代码里真正需要构造的矩阵到底有哪些。

已有工程笔记里其实已经把这条路线概括得很清楚：四足控制通常先从 FK、IK、雅可比、轨迹和 PD 入门，进入高动态阶段后再走向单刚体模型、直接优化法和 MPC；同时，作者也明确强调，MPC 的关键优势并不只是“求一个当前时刻最优解”，而是基于当前状态得到未来一段时间内的最优解序列。fileciteturn20file1 四足机器人开发里最常见的高动态 MPC 参考框架，则来自 MIT Cheetah 3 的 convex MPC 路线：将机器人建模为一个受到若干接触力作用的 single rigid body，在有限时域内规划地面反作用力，并通过 (\tau_i = J_i^T R_i^T f_i) 把接触力映射成关节扭矩。fileciteturn21file8

一、为什么四足机器人中的 MPC 几乎总是从单刚体模型开始

如果只从机器人学完整性出发，最“正确”的做法当然是直接使用四足机器人的全身动力学模型，把躯干、四条腿、各个关节、接触切换和执行器全部放到一个统一预测模型里。但只要真的尝试过，你就会很快发现这在工程上很难承受。模型维度高，非线性强，接触是混合系统，参数辨识也不轻松，在线求解负担会迅速增长。对大多数四足平台来说，这条路线在理论上很完整，在实时控制上却往往不够友好。

因此，四足 MPC 最常见的做法并不是“追求最完整的模型”，而是先找一个既能抓住整机主要行为、又能被实时求解的简化模型。《四足机器人随笔》里对这一点说得非常直白：直接用四足全身动力学模型做控制过于复杂，而且还存在动力学参数辨识问题，因此常见做法是先基于简化模型做控制，而 lumped mass，也就是单刚体模型，是一种非常常用的四足简化模型。fileciteturn21file14

MIT Cheetah 3 的 convex MPC 论文采取的正是这一路线。论文明确写道，预测控制器把机器人建模为一个 single rigid body subject to forces at the contact patches，并指出这种忽略腿部动力学的简化对于 Cheetah 3 是合理的，因为腿部质量只占总质量的大约 10%。在这种近似下，系统的核心动力学变成：质心平动由各个接触力的和决定，刚体角动量变化由各接触点相对质心的力矩和决定，姿态演化则由角速度驱动。fileciteturn21file8

这一步的真正意义在于，它改变了控制问题的视角。四足机器人上层整机控制关心的已经不再是“每个关节下一拍转多少角度”，而是“在未来一小段时间里，每条支撑腿该对地施加怎样的力，才能让质心和姿态按期望演化”。只要这个视角切换成功，MPC 的输入变量、代价函数和约束结构就会自然出现。

二、连续时间单刚体动力学的建立

在单刚体模型中，把机器人看成一个质量为 (m) 的刚体躯干，其质心位置为 (p\in\mathbb{R}^3)，姿态由旋转矩阵 (R\in SO(3)) 或欧拉角 (\Theta=[\phi,\theta,\psi]^T) 表示，角速度为 (\omega\in\mathbb{R}^3)。若第 (i) 个接触点相对质心的位置向量为 (r_i)，地面对该足端施加的反作用力为 (f_i\in\mathbb{R}^3)，则连续时间刚体动力学可以写成

[
m\ddot p=\sum_{i=1}^{n_c} f_i + mg,
]

[
\frac{d}{dt}(I\omega)=\sum_{i=1}^{n_c} r_i \times f_i,
]

[
\dot R = [\omega]_\times R.
]

这里 (n_c) 是当前或未来某时刻处于接触状态的足端数目，(I) 是躯干关于质心的惯量矩阵，([\omega]_\times) 是由角速度构造的反对称矩阵。MIT Cheetah 3 的论文就是从这一组方程出发建立预测模型，并明确指出，非线性的角动量方程和姿态方程正是后续必须做近似和线性化的原因。fileciteturn21file8

如果只从开发角度看，这组方程至少告诉我们三件事。第一，MPC 的自然输入不是关节扭矩，而是足端接触力。第二，质心线加速度由所有接触力的总和直接决定。第三，姿态和角速度的演化与接触点相对质心的位置紧密相关，因此未来足端位置本身也是预测模型的一部分。

为了便于后续推导，一般会把刚体状态写成一个低维向量。比较常见的一种写法是

[
x=
\begin{bmatrix}
\Theta \
p \
\omega \
\dot p \
g
\end{bmatrix},
\qquad
u=
\begin{bmatrix}
f_1 \
f_2 \
f_3 \
f_4
\end{bmatrix}.
]

这里将重力项 (g) 作为附加常量引入，是为了让平动动力学在状态空间表示里更整齐。真正的控制输入就是四条腿在未来时域上的接触力拼接向量。到了这一步，一个很重要的认识也就成立了：MPC 并不是在直接优化“关节控制量”，它优化的是一段未来时域内的“接触力序列”。

三、姿态动力学为什么要近似和线性化

单刚体模型虽然比全身模型简单很多，但它仍然不是一个天然线性的系统。真正麻烦的地方主要集中在姿态与角速度部分。一方面，旋转矩阵 (R) 属于 (SO(3))，本身是一个非线性流形变量；另一方面，角动量项 (\frac{d}{dt}(I\omega)) 在世界坐标系和身体坐标系切换时会带来明显的非线性耦合。

因此，四足机器人里的线性 MPC 通常都不会直接使用原始的旋转矩阵动力学，而是进一步做近似。MIT Cheetah 3 的论文采用的是非常经典的处理方式：把机身姿态表示为 Z-Y-X 欧拉角 (\Theta=[\phi,\theta,\psi]^T)，其中 (\psi) 是 yaw，(\theta) 是 pitch，(\phi) 是 roll；然后在小 roll、小 pitch 近似下，将角速度到欧拉角导数的映射写成近似线性形式，并把系统整理成一个只依赖于 yaw 和足端位置的线性时变模型。fileciteturn21file8

从数学上看，这一步的核心目的并不是“求更精确的姿态”，而是为了避免把整个预测控制问题变成一个实时难以求解的非凸非线性优化。工程上通常会接受这样的折中：只要当前 roll 和 pitch 不太大，线性近似足够支持几十毫秒到几百毫秒尺度上的预测控制，而 MPC 又会在下一个周期重新求解，因此整个闭环依然是稳定有效的。

换句话说，四足机器人里的线性 MPC 从来不是在假装世界是线性的，而是在利用“高频滚动重算”来抵消模型近似误差。开发时必须记住这一点：你不需要一个在 500 毫秒后仍然完全精确的模型，你需要的是一个在当前附近足够可信、并且能被快速更新的模型。

四、状态空间模型的整理

完成姿态近似之后，连续时间系统通常可以整理成标准的线性时变形式

[
\dot x(t)=A_c(t)x(t)+B_c(t)u(t).
]

这里 (A_c(t)) 和 (B_c(t)) 的时间变化，通常来自两个方面。第一，yaw 会影响姿态相关的映射矩阵。第二，未来各足端相对质心的位置 (r_i) 会直接影响接触力到角动量的映射关系。MIT Cheetah 3 论文特别强调，它们的连续时间状态空间形式主要依赖于 yaw 和未来足端位置，因此只要这两者可以在短时预测范围内给定，就可以得到一组适用于 convex MPC 的近似线性时变模型。fileciteturn21file8

这一层在代码里一般对应两个关键模块。第一个模块是未来接触序列和未来足端位置的生成。也就是说，步态状态机和落脚点规划器必须先给出未来 (N) 个 MPC 步长内每条腿是支撑还是摆动，以及各支撑足相对质心的位置估计。第二个模块才是基于这些信息构造 (A_c(k)) 和 (B_c(k))。如果没有前面的 gait schedule 和 foothold preview，后面的状态空间模型其实是无从谈起的。

因此，一篇讲数学建模的文章也应该明确指出：MPC 不是一个孤立的优化器，它的数学模型天然依赖步态规划和未来足端位置信息。很多读者误以为“有了模型就能做 MPC”，其实模型里最关键的一部分恰恰来自 gait planner。

五、离散化：从连续系统走向预测控制

连续时间状态空间模型建立之后，下一步就是离散化。因为预测控制实际处理的是离散步长上的状态演化，而不是连续时间微分方程本身。

设 MPC 的离散步长为 (\Delta t)，在零阶保持假设下，连续时间系统

[
\dot x(t)=A_c(k)x(t)+B_c(k)u(t)
]

可以离散成

[
x_{k+1}=A_k x_k + B_k u_k.
]

这里 (A_k) 和 (B_k) 可通过矩阵指数或数值近似得到。在 MIT Cheetah 3 的论文中，作者采用的是一种非常工程化的处理方式：在参考轨迹上近似 (A_c) 为常值，在每个离散节点上用未来足端位置和参考 yaw 构造对应的 (B_c(k))，再通过扩展状态转移矩阵求得离散系统。论文还特别说明，这种离散模型只需要在当前轨迹附近足够准确即可，因为 MPC 会在下一个周期重新基于新状态求解。fileciteturn21file8

对开发者来说，这里最重要的不是某一种离散公式，而是两个实用判断。第一个判断是，离散步长不能太大，否则模型误差会迅速累积，优化器会因为预测过粗而变得短视。第二个判断是，离散步长也不能太小，否则预测时域内的决策变量数会急剧增长，QP 规模扩大，求解速度下降。MIT Cheetah 3 实际采用的预测时域是 0.3 到 0.5 秒左右，并将其离散成 10 到 16 个节点，这个量级在工程上非常具有参考价值。fileciteturn18file1

因此，离散化并不是一个单纯数学步骤，它直接决定了预测范围、QP 规模和实时性，是建模和计算之间最重要的桥梁之一。

六、有限时域预测模型的堆叠

一旦得到了离散系统

[
x_{k+1}=A_kx_k+B_ku_k,
]

下一步就是把未来 (N) 个步长的状态演化统一写出来。设当前时刻状态为 (x_0)，未来控制输入序列为

[
U=
\begin{bmatrix}
u_0 \
u_1 \
\vdots \
u_{N-1}
\end{bmatrix},
]

则整个预测时域内的状态序列可以堆叠成

[
X=
\begin{bmatrix}
x_1 \
x_2 \
\vdots \
x_N
\end{bmatrix}

A_{qp}x_0+B_{qp}U.
]

其中 (A_{qp}) 是由各个 (A_k) 连乘构成的块矩阵，(B_{qp}) 则由各步状态转移和输入矩阵递推堆叠而成。例如，

[
x_1=A_0x_0+B_0u_0,
]

[
x_2=A_1A_0x_0+A_1B_0u_0+B_1u_1,
]

以此类推。把所有这些关系按块矩阵形式写在一起，就得到了预测控制最核心的线性关系。MIT Cheetah 3 的论文将这一步写成 condensed formulation 的基础：先用系统动力学把全部未来状态显式消掉，最终把优化变量只保留为 (U)。fileciteturn21file8

这一步在理论上看起来只是矩阵堆叠，但在工程实现里非常关键。因为一旦 (A_{qp}) 和 (B_{qp}) 构造完成，后面的目标函数和约束函数就都可以直接写成只关于 (U) 的表达式。也就是说，真正的 QP 结构就是从这里开始显现出来的。

七、代价函数：为什么最后一定会变成一个二次型

MPC 的目标并不是“找到一组任意可行的接触力”，而是找到一组既满足约束、又尽可能让整机状态贴近期望轨迹的接触力。最常见的代价函数由两部分组成。第一部分是状态跟踪误差，第二部分是控制输入惩罚。写成数学形式，可以表示为

[
J=\sum_{k=0}^{N-1}(x_{k+1}-x^{ref}{k+1})^TQ_k(x{k+1}-x^{ref}{k+1})+\sum{k=0}^{N-1}u_k^TR_ku_k.
]

这里 (Q_k) 用来权衡姿态、质心位置、角速度和线速度的跟踪误差，(R_k) 用来抑制过大的接触力或过于激进的力分配。

将前面得到的预测关系 (X=A_{qp}x_0+B_{qp}U) 代入，就可以把代价函数全部写成关于 (U) 的形式：

[
J(U)= (A_{qp}x_0+B_{qp}U-X^{ref})^T L (A_{qp}x_0+B_{qp}U-X^{ref}) + U^T K U,
]

其中 (L) 是预测时域内所有 (Q_k) 的块对角堆叠，(K) 是所有 (R_k) 的块对角堆叠。展开后就得到标准二次型：

[
J(U)=\frac12 U^T H U + g^TU + \text{const}.
]

MIT Cheetah 3 的论文给出了这一形式下的标准表达，并将 Hessian 和线性项写成

[
H = 2(B_{qp}^TLB_{qp}+K),
\qquad
g = 2B_{qp}^TL(A_{qp}x_0-Y^{ref}).
]

这一步的意义非常大，因为它说明：只要动力学是线性或线性化的，代价函数采用二次型，那么 MPC 的目标函数天然就是一个 QP 的目标函数。fileciteturn21file8

如果你愿意在文章里做一个很好的衔接，还可以顺带指出，直接优化法在当前时刻上的力分配问题，本质上已经是一个“小型 QP”。《四足机器人随笔》在讲直接优化法时，就明确把目标函数整理成 qpOASES 可接受的 (H) 和 (g) 矩阵，并指出约束包括力上下限、摆动腿末端力为零以及摩擦力约束。MPC 与它的区别，并不是“突然换了一种数学工具”，而是把单时刻 QP 推广成了有限时域上的序列 QP。fileciteturn21file3

八、接触约束的写法：为什么这才是四足 MPC 有物理意义的关键

只看目标函数，优化器完全可能给出一组“数学上很好看、物理上根本实现不了”的接触力。四足 MPC 真正的物理意义来自约束。通常至少需要考虑三类约束。

第一类是摆动腿零力约束。任何不在接触状态的腿，都不允许给系统提供地面反作用力。若第 (i) 条腿在第 (k) 个离散步长内处于摆动相，则必须有

[
f_{i,k}=0.
]

这类约束在矩阵形式中通常写成一个选择矩阵对 (U) 的线性等式约束。MIT Cheetah 3 的论文正是通过这种方式，把 swing leg 的接触力显式约束为零。fileciteturn21file8

第二类是法向力上下限约束。支撑腿的法向力必须非负，而且不能超过执行器和结构允许范围。通常写成

[
f_{z,\min}\le f_{z,i,k}\le f_{z,\max}.
]

其中 (f_{z,\min}) 有时会取一个略大于零的数，用来避免优化器在支撑相中频繁让接触力接近零，从而导致支撑不稳定。MIT Cheetah 3 的论文参数表中就给出了法向力上下限的具体数值。fileciteturn18file1

第三类是摩擦锥约束。真实的 Coulomb 摩擦锥是二阶锥约束，但为了提高求解速度，实时控制里通常会使用线性近似。最常见的是方锥形式：

[
-\mu f_{z,i,k}\le f_{x,i,k}\le \mu f_{z,i,k},
\qquad
-\mu f_{z,i,k}\le f_{y,i,k}\le \mu f_{z,i,k}.
]

《四足机器人随笔》在讲 QP 化时也专门指出，常见的摩擦约束写法有多种，而工程里为了计算方便，最常用的是线性摩擦力约束。fileciteturn21file3

这一层非常值得在文章里明确强调：MPC 的“预测未来”固然重要，但它真正区别于普通轨迹跟踪器的地方，不是预测本身，而是在预测过程中始终满足接触物理约束。没有这些约束，QP 的解虽然可能让代价函数最小，却不一定让机器人真的站住。

九、标准 QP 形式的最终写法

到这里为止，目标函数已经整理成二次型，约束也已经写成了线性形式。于是整个四足机器人 MPC 问题就可以统一写成

[
\min_U \frac12 U^T H U + g^TU
]

[
\text{s.t. } C U \le d,\qquad C_{eq}U = d_{eq}.
]

在很多实现里，摆动腿零力约束既可以写成等式约束，也可以通过在不等式矩阵里上下界都设为零来统一处理。MIT Cheetah 3 的论文给出了相同本质的写法，只不过用的是统一不等式加若干线性等式的形式，然后把问题交给实时 QP 求解器。fileciteturn21file8

从软件实现角度看，这个最终形式意味着你在每个 MPC 周期里真正需要准备的对象很有限。你要更新的主要是：当前状态 (x_0)，参考轨迹 (X^{ref})，预测时域内的 (A_k,B_k)，以及由 gait schedule 决定的约束矩阵。其余很多量，例如块对角权重矩阵的结构、选择矩阵的模板，往往在初始化时就能固定。

也正因为如此，一旦你把 (A_{qp},B_{qp},H,g,C,d) 这几类矩阵的构造逻辑真正理顺，四足 MPC 在代码里就不再神秘。它归根结底只是一个“每拍更新模型和约束，再求一次 QP”的循环。

十、为什么只执行最优序列的第一拍

这是预测控制里最经典的问题之一，但在四足机器人里尤为关键。每个 MPC 周期求解出来的其实是一整段未来时域上的最优接触力序列。既然这一整段都算出来了，为什么不干脆按顺序执行完，而是只执行第一拍，然后立刻重新求解？

原因就在于模型不可能完全准确，接触也不可能完全按预测发生。四足机器人在真实运行时，会持续受到状态估计误差、地面摩擦变化、提前触地、延后触地和小外部扰动的影响。若你机械地执行整段最优序列，那么越往后的输入就越建立在“旧状态”之上，可信度会迅速下降。

《四足机器人随笔》对这一点给出了非常工程化的解释：整个最优解序列都是由当前初始状态 (x_0) 计算得到的，而四足机器人在实际运行中存在模型误差和外部扰动，因此越往后的解越不可靠，故需要尽可能提高 MPC 频率，并在受到扰动后及时用新的状态重新求解。fileciteturn17file13

这也解释了为什么 MPC 不是“一次规划，多拍执行”，而是“滚动时域优化”。只执行第一拍，并不是浪费了后面的解，而是承认真实世界永远在变，只有当前这一拍才是最可信的。

十一、从接触力到关节扭矩：QP 结果如何落到机器人上

只做完 QP 推导还不够，因为优化器的输出是足端接触力，而机器人执行器真正能接收的是关节层指令。这个映射关系如果讲不清楚，整篇文章就仍然停留在“数学模型”层，没有真正落回控制器。

对单条支撑腿而言，最经典的映射就是雅可比转置：

[
\tau_i = J_i^T f_i.
]

若接触力 (f_i) 是在世界系或其他参考系下定义，还需要加上对应的坐标变换。MIT Cheetah 3 的论文给出的实际形式为

[
\tau_i = J_i^T R_i^T f_i,
]

其中 (J_i) 是足端雅可比，(R_i) 是从 body 到 world 的旋转关系。论文明确指出，在支撑相，ground force control 产生的期望接触力就是通过这个式子映射为关节扭矩的。fileciteturn21file8

这一步在数学上很简单，在工程上却极关键。因为它告诉我们，四足机器人里的 MPC 上层只需要负责“决定该发什么接触力”，至于如何通过腿部几何关系把这个力实现出来，则交给雅可比映射和更底层的扭矩执行模块。也正是这种清晰分层，让 MPC 能够作为整机上层控制器而存在，而不必直接陷入每个关节的局部细节。

十二、一个最小可运行的数学实现链

如果把前面的推导全部浓缩成一条最小实现链，那么一套最小可运行的四足 MPC 数学流程其实非常清晰。首先，根据当前状态估计和未来步态表，得到预测时域内每个时刻的足端接触模式和足端位置。其次，构造每个时刻的连续时间单刚体模型 (A_c(k),B_c(k))，再离散化为 (A_k,B_k)。然后，将整个预测时域上的状态关系堆叠成 (X=A_{qp}x_0+B_{qp}U)。接着，根据期望机身轨迹构造二次型代价函数，得到 (H) 和 (g)。再之后，根据摆动腿零力、法向力上下限和摩擦锥约束构造 (C,d) 以及必要的等式约束。最后，将求解结果的第一拍接触力通过雅可比映射成关节扭矩，并发给支撑腿执行层。

如果你把这条链真正理解透了，那么四足 MPC 的数学建模其实已经完成了大半。后面再继续往下走，无非就是状态估计、线程组织、求解器选择和参数整定这些工程问题。

十三、总结

四足机器人 MPC 的数学建模，说到底就是把一个高维、多刚体、混合接触系统，在整机层抽象成“刚体状态 + 接触力输入 + 物理约束”的有限时域优化问题。这个过程里最关键的几个步骤，一个是从全身系统退到单刚体模型，一个是把连续时间动力学近似成线性时变离散模型，一个是把未来时域内的状态递推堆叠成 (A_{qp},B_{qp}) 结构，最后再把状态跟踪和输入惩罚整理成标准二次型，把接触约束写成线性不等式或等式。到这一步，一个标准的 QP 其实就已经自然出现了。

因此，四足机器人里的 MPC 从来不是“黑箱算法”，也不是一组只能照抄论文的矩阵。它本质上是一条非常清晰的数学链：单刚体建模、离散化、时域堆叠、代价构造、约束组织、QP 求解、力矩映射。只要把这条链真正理解透，后面的代码实现就不会再像是魔法，而更像是一套逻辑清楚的工程结构。

如果沿着这一篇继续往下写，下一篇最自然的标题其实就是《四足机器人 MPC 的工程实现：状态估计、线程组织与参数整定》。因为到了那里，问题就不再是“矩阵怎么来”，而是“这些矩阵怎样在 30 Hz 里稳定算出来，并和 1 kHz 的腿控闭环拼起来”。