机器人控制算法 MPC 的预测模型从哪来?完整公式推导

四足机器人MPC控制与浮动基座动力学建模
——用 60 个公式进行完整推导
目录
这篇文章围绕两个问题展开:
一是 MPC 如何用模型预测、用优化决定控制?
二是被用来预测的模型:四足机器人的浮动基座动力学,是如何从牛顿定律一步步推出来?
内容整理自深蓝学院《四足机器人:从动力学建模到强化学习》课程学员笔记。
01 MPC:用模型预测,用优化决定控制
MPC 在每个采样时刻,利用当前估计状态和系统模型,计算未来一段控制输入,使预测输出尽量跟随参考,并满足约束。
控制器只执行这段输入的第一项,再根据新的反馈重新计算。
预测时域与控制时域,分别表示什么?
预测时域 p:向前预测多少步输出;
控制时域 m:允许独立调整多少步控制输入,通常 m ≤ p。
采用常见的设置时 ,m 步之后的输入保持不变,但输出仍预测到第 p 步。每步间隔为采样周期 T s 。

参考轨迹是希望达到的输出;预测输出则由模型根据待求控制序列算出。优化器调整控制序列,进而改变预测输出。
图中橙色首项用于当前执行,其余部分用于评估未来。
优化器怎样选择这一段控制?
ΔU 表示待求的控制增量序列。第一项衡量预测输出 ŷ 与参考 r 的偏差;第二项衡量相邻控制输入的变化 Δu。
两个权重分别决定对跟踪精度与控制平滑性的重视程度。
优化还必须遵守模型关系,以及输入、输入变化率和输出的边界。
这一基本形式在采用线性模型和线性约束时,可由二次规划(QP)求解;不需要预先枚举所有动作序列。
为什么每次只执行第一项?
经过一个采样周期后,实际输出可能因扰动和模型误差偏离预测。此时更新状态估计,将预测时域向前移动一步,再计算新的最优序列。
这种“预测—优化—执行首项—反馈更新”的重复过程,就是滚动时域控制。
MPC 位于反馈控制器内部,动力学模型用于预测施加控制后的运动。后文推导的浮动基座动力学,为建立机器人的预测模型提供基础。
02 从牛顿定律到浮动基座动力学
先看单摆:一个摆角的运动方程
无质量杆带摆球绕固定轴转动,转轴无摩擦;
惯性力矩+重力项=电机力矩:
(1)
m为球质量(kg),l为杆长(m);g为重力加速度大小,约9.81 m/s²。
θ为摆角,从竖直向下起算,向右为正;两点表示角加速度。
电机沿摆角增加方向施加的力矩;本式各项均为N·m。
再看浮动基座:机身和关节一起运动
机身没有固定在地面,平移、转动与关节转动统一写成:
(2)
q记录机身位置、姿态和关节角;v由平移速度、机身角速度和关节速度组成;一点 表示对时间求导。
M是质量矩阵;C乘以整机速度v后,给出科氏、离心等速度相关项。
按整机坐标表示、放在左侧的重力项,对应课件的g(q)。它是向量,与单摆式中的加速度g不同。
τ在这里是各关节电机力矩的向量;S是关节坐标选择矩阵,S转置后把力矩填入整机对应位置。
f是地面对第i个接触点的力;J是位置雅克比,将整机坐标变化换成该点位移。J转置乘f,把接触力换算到整机坐标。
i编号各接触点;求和号将各点作用相加;上标T表示矩阵转置。
从每个材料块的牛顿方程开始
(3)
下标编号材料块;
是质量,
是位置,
是加速度,
是合力。
后文对刚体各材料块求和,保留平动和转动惯性。
单摆的位置、速度与加速度
将支点作为原点,在摆动平面内采用水平 、竖直 坐标。摆球的位置由摆角唯一确定:

(4)
对时间求一次导数,使用复合函数求导法则。
把位置对角度的导数记为,它把角速度换算为摆球的线速度:
(5)
速度不仅取决于摆角变化的快慢,还取决于当前位置。
因此再次求导时,与
两部分都需要求导:
(6)
(7)
定义沿杆向外的单位向量和沿角度增加方向的切向单位向量
:
(8)
(9)
第一项沿切线,来自角速度大小的变化;第二项沿杆指向支点,来自速度方向的变化。
分别是径向、切向单位向量,彼此垂直。
将单摆的牛顿方程投影到摆角方向
沿杆的被动轴向力记为,重力向下。电机力矩在杆端等效为切向力
,因为“切向力乘杆长”等于力矩。
摆球的牛顿方程是:

(10)
先只看切线方向。轴向力没有切向分量,切向加速度为,所以:
(11)
这行两边都是N。两边同乘,再把重力项移到左边,所有项就都变为
:
(12)
将这个投影写成多连杆也能使用的形式。
式(12)已经给出单摆的动力学方程。
接下来要推广到整机的多个坐标,因此把“取切向分量、乘杆长”写成统一形式,便于对各连杆的材料块求和。求和时,理想被动关节连接力的虚功会成对相消,便不必逐一求这些内部未知力。
这就是引入 “虚位移” 与 “虚功” 的目的。
固定当前时刻,想象摆角作一个任意微小变化,杆长保持不变。摆球位置的一阶变化记为
,称为虚位移。它不要求真实发生,也不指定经过多长时间。
下面先从位置关系求出它。
用虚位移表达同一次受力投影
先对摆球的两个位置分量求一阶变化。微小转角引起的位置变化,等于相应变化率乘这个转角:

(13)
把这两个变化率排成一列,记为位置雅可比,便得到:
(14)
与前面速度式中的相同。因为
,
作用在力上,就相当于先取切向分量、再乘杆长。
任一力对这次假想位移的功记为
,称为虚功。把牛顿方程移到同一侧,再与
点乘,得到:
(15)
代入。杆力沿杆,与虚位移垂直,虚功为0;余下电机、重力和惯性三项:
(16)
可以任取,所以括号必须为0,恰好回到式(12)。零来自牛顿定律,实际力与惯性项始终一起投影。
下一步把一个摆角换成整机坐标:先求各材料块的位置与雅克比,再用同一组坐标变化投影并求和。
后面的浮动基座推导就沿这条路展开。
把材料块的位置写成机身与关节的函数
机身、腿都由材料组成。
把它们分成小质量块,对每块使用牛顿定律。腿上的材料块相对机身的位置,随关节角改变。

第块质量记为
,机身坐标下的位置记为
。
是机身原点的世界位置;
把机身坐标分量转到世界系。材料块的世界位置因此是:
(17)
对时间求导,两份相乘的因子都要求导:
(18)
相对位置通过12个关节角随时间变化。先逐项求导,再把各个系数排成一张表:
(19)
(20)
有3行、12列;每列是某一个关节角前面的三维系数。不带动该材料块的关节,对应列为0。
把机身转动也写成速度系数
位置求导后,关节部分已经写成K乘关节速度。现在处理机身转动产生的那一项。

机身角速度记为,用世界坐标表示。只考虑机身转动时,点的线速度就是“角速度叉乘相对位置”:
(21)
把世界系中的相对位置记为。将刚才的关节速度关系一起代回位置导数:
(22)
三项依次来自机身平移、机身转动、关节运动。始终是从机身原点指向材料块的世界系向量。
(23)
记录机身位置、姿态和关节角。
的中间三项直接使用机身角速度。后面的
表示对应18个运动方向的微小变化,其中机身转动用世界系微小转角表示。
把三组速度系数合成雅克比
先把叉乘写成矩阵乘法。下面的矩阵只由s的三个分量组成:

(24)
交换叉乘的顺序要变号,所以机身转动项的系数是负的叉乘矩阵:
(25)
是三维单位矩阵。把平移、转动、关节速度前面的系数,按
的相同顺序排在一行:
(26)
这就是雅可比:把整机18维速度换算为该材料块的3维速度。它来自刚才三个速度项的系数合并。
再求一次时间导数,两份因子都要求导:
(27)
展开雅克比的导数,分清加速度项与速度项
J也随运动变化。单位矩阵导数为0;另外两组系数逐项求导:

(28)
先展开$s$的导数,再展开$K$的各个元素的导数:
(29)
(30)
将它们代入,再用刚才的去乘,就得到:
(31)
系数2来自两次乘积求导中相同的一项。上式只含当前速度,不含待求加速度;加速度都在前一项里。
把各材料块的牛顿方程投影到同一组方向
每块都满足牛顿定律。将已经求出的加速度直接代入:

(32)
为了把每块的三维方程统一起来,固定当前时刻,给机身和关节一组微小变化。机身微小转角也用世界系表示:
(33)
像单摆一样,两边与虚位移点乘,再对全部材料块求和:
(34)
(35)
理想被动连接力在同一点反向作用,两侧虚位移相同,总虚功相消。重力、电机和地面作用保留,它们对这18个方向的总作用记为。
(36)
这里先保留未知地面力,不限制测试变化的接触方向;固定脚的条件最后联立加入。
因此可逐个比较18个方向的系数。
直接从牛顿惯性项收集出M和C
把加速度的两部分代入式(32)、(36)。各材料块使用同一个整机加速度和速度,分别提取出来:

(37)
把加速度前面的矩阵系数记为;把第二组括号记为
:
(38)
(39)
和
直接来自上面的牛顿方程:
收集加速度系数,
收集全部速度相关项。
可以有不同写法,运动方程使用其乘积
。
单摆只有一个坐标,的一列是
,因此同一步会给出:
(40)
整机只是把这一列扩成18列,并对所有材料块相加。
把重力换算到整机坐标
世界竖直方向向上;g是正的重力加速度大小。每块的实际重力直接写成向下的三维力:

(41)
用刚才虚功关系得到的同一个力的换算系数,实际重力贡献为:
(42)
把它移到动力学左侧,记成:
(43)
(44)
此处大写是18维的重力项;小写
始终是加速度数值。后两项分别表示电机与接触作用,接着逐项求出。
由关节电机的虚功得到选择矩阵
一个电机对关节两侧施加反向力矩。沿关节轴的两侧微小转动之差,就是关节相对转角变化:

(45)
的后12项就是关节角变化。定义
取出这部分:
(46)
代入电机虚功,把前面的系数收集起来:
(47)
(48)
前6项填零,后12项填入关节力矩。转置只安排位置;电机力矩的数值和单位保持原样。
机身仍可通过与腿的惯性和接触耦合运动。
加入接触力,得到完整的速度形式
脚点位置与材料块位置形式相同,因此沿用已经完成的求导过程。Kc是脚点相对位置对12个关节角的系数表:

(49)
地面力乘以脚点虚位移,再按收集系数:
(50)
(51)
固定支撑时,脚点真实速度为0。再求导一次,就得到与受力方程联立的接触条件:
(52)
将各只脚的雅可比和地面力按相同顺序竖着叠起来,统一记为、
:
把动力学中的接触力移到左侧,与式 (52) 的加速度约束一起排成矩阵:
上半行是受力方程,下半行是固定接触约束。给定q、v和电机力矩τ,联立求出整机加速度与地面接触力。
先看清位移向量,再写材料块速度
以下均用世界系表示。
是第$b$根连杆的质心位置,com是质心(center of mass)的缩写。材料块
相对质心的位置是:
有方向,
才是长度,单位为
。同一刚体上,这个长度不变,方向会随杆转动;它也不同于计算力矩时到力作用线的垂直力臂。

点号表示对时间求导。用、
分别收集质心线速度、连杆角速度对整机速度
的系数:
(53)
同一刚体内,随杆转动,所以
。对
求导,材料块速度就是:
(54)
式(54)与前面的描述同一个点。比较
前的系数,得到:
把这份代入式(38),先只对连杆
内的材料块求和:
把一根杆内的材料块合并,得到式(57)
接下来只整理式(55)的求和。
总质量为;质心和惯量的定义给出:

(56)
叉乘矩阵的每个元素都与位移分量成正比。因此,加权位移之和为0,也使加权叉乘矩阵之和为0:
将式(55)的两个括号展开成四项。同一根杆的和
对所有材料块相同,可以提到求和号外;再用式(56)整理:
后两项为0,留下质心平移项和绕质心转动项。再对所有连杆求和:
(57)
式(55)使用的就是原来式(38)的材料块求和。式(57)只是先把同一根杆内的材料块合并,再对各杆求和,所以得到同一个M。
下面再把两种写法的矩阵直接对齐。
先明确三段位移与两组速度系数
先区分三段位移:从机身原点到材料块,
从机身原点到连杆质心,
从质心到材料块。

下面把原有质心雅克比和角速度雅克比的后12列,分别简记为和
。用前面已有的
取出这部分:
是关节运动带来的质心线速度,
是关节运动带来的连杆角速度。加上机身运动,便有:
的第
列是关节j的转轴在世界系中的单位方向;该关节不带动这根杆时,该列为0。
是系数矩阵,
才是实际角速度。
按的顺序收集上面两式的系数:
需要对照的旧矩阵,是把式(26)的直接代入式(38)得到的:
展开两种点速度,推导关节列的关系
先看第一种写法:由式(26),直接用材料块的速度系数计算:
再看第二种写法:按式(54),材料块速度等于质心平移速度加上绕质心转动的速度。把上一页的质心速度和连杆角速度代入:
位置关系给出。接着处理最后一项:叉乘交换顺序要变号,所以:
第二种写法因此整理成:
两种写法描述同一个点。减去相同的机身平移项和机身转动项,得到:
这个等式对任意关节速度都成立。
比较系数时,依次让一个关节速度取、其余取0,矩阵乘积取出的就是对应的那一列。对12个关节逐列比较,得到:
这里比较的是每个关节角速度前的系数;仅凭某一次运动的点速度相同,不能得出整个矩阵相同。
代回关节列关系,把两种M写成相同矩阵
现在把上面证明的关系代回式(54)的速度系数,逐列整理:
第三行到第四行用了和刚证明的
,结果正是式(26)。
所以,两条路线从一开始用的就是相同的。
将旧矩阵按连杆分组,再用式(56)合并材料块,得到:
将式(57)中的和
直接按列相乘,也正好得到上面这个矩阵。
为看清如何合并,以右下角(分块矩阵M(q)的第三行第三列) “关节与关节” 分块为例:
第一行用;最后一行展开后,两项交叉项由
消去,平方项按式(56)合成
。
其余分块使用同样的两条关系。
因此,“材料块形式的M”和式(57)的M相等,依据就是:同一套点速度系数,加上同一根杆内的质量求和。
退回单摆,核对全部系数
固定支点,只保留单摆角θ。此时整机速度只剩角速度,选择矩阵为1,没有脚底接触力。

(58)
切向与径向垂直,速度相关项的投影为0。重力与同一雅克比相乘,得到:
(59)
(60)
所以单摆和浮动基座用的是同一条路线:位置求导,整理速度系数,代入牛顿方程,按虚功投影并收集系数。
03 一条被冠军验证过的工程链路
本文出自:深蓝学院联合英国纽卡斯尔大学正教授、智身科技CTO潘为,推出《四足机器人:从动力学建模到强化学习》实战课程
——以IROS 2025四足挑战赛冠军的实战经验为核心,从动力学建模到实机部署,手把手教你掌握四足机器人全栈开发能力。
这是一门:教你理解四足机器人的物理本质,然后用 RL 控制它的工程实践课:
模型 → 辨识 → 仿真 → 训练 → 部署。
课程全程基于 MATRiX 一站式平台(MuJoCo 高精度物理引擎 + UE5 高保真视觉渲染 + GPU 并行训练),支持多种四足机器人型号,内置丰富地形。
从系统辨识到策略训练,从视觉验证到实机部署,全部在同一个平台内完成闭环。
我们不绑定任何一家硬件生态。CPU 能训,GPU 能训,Windows / macOS / Linux 都能跑。

Ref
1. 深蓝学院《四足机器人:从动力学建模到强化学习》-第1章 导论:四足控制的演进-课件.pdf(第4页)。
用于核对目标方程与单摆起点。
2. ETH Zurich, Robot Dynamics Lecture Notes, HS 2017,§2.10、§3.5-3.7。
https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf
用于核对浮动基座运动学、牛顿投影与接触动力学。
3. Kevin M. Lynch and Frank C. Park, Modern Robotics,§8.7 Constrained Dynamics。
用于核对约束力与约束运动的联立形式。
4. MathWorks, Understanding Model Predictive Control:https://www.mathworks.com/videos/understanding-model-predictive-control-part-2-what-is-mpc--1528106359076.html
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐



所有评论(0)