资讯详情

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

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

5
深蓝具身智能
深蓝学院-具身君
2026-11-16
四足机器人MPC控制与浮动基座动力学建模——用 60 个公式进行完整推导这篇文章围绕两个问题展开:一是 MPC 如何用模型预测、用优化决定控制?二是被用来预测的模型:四足机器人的浮动基座动力学,是如何从牛顿定律一步步推出来?内容整理自深蓝学院《四足机器人:从动力学建模到强化学习》课程学员笔记。MPC:用模型预测,用优化决定控制MPC 在每个采样时刻,利用当前估计状态和系统模型,计算未来一段控制输入,使预测输出尽量跟随参考,并满足约束。控制器只执行这段输入的第一项,再根据新的反馈重新计算。预测时域与控制时域
图片

四足机器人MPC控制与浮动基座动力学建模

——用 60 个公式进行完整推导

这篇文章围绕两个问题展开:

一是 MPC 如何用模型预测、用优化决定控制?

二是被用来预测的模型:四足机器人的浮动基座动力学,是如何从牛顿定律一步步推出来?

内容整理自深蓝学院《四足机器人:从动力学建模到强化学习》课程学员笔记。

图片

MPC:用模型预测,用优化决定控制

MPC 在每个采样时刻,利用当前估计状态和系统模型,计算未来一段控制输入,使预测输出尽量跟随参考,并满足约束。

控制器只执行这段输入的第一项,再根据新的反馈重新计算。

预测时域与控制时域,分别表示什么?

预测时域 p:向前预测多少步输出;

控制时域 m:允许独立调整多少步控制输入,通常 m ≤ p。

采用常见的设置时 ,m 步之后的输入保持不变,但输出仍预测到第 p 步。每步间隔为采样周期 T s 。

图片

参考轨迹是希望达到的输出;预测输出则由模型根据待求控制序列算出。优化器调整控制序列,进而改变预测输出。

图中橙色首项用于当前执行,其余部分用于评估未来。

优化器怎样选择这一段控制?

ΔU 表示待求的控制增量序列。第一项衡量预测输出 ŷ 与参考 r 的偏差;第二项衡量相邻控制输入的变化 Δu。

两个权重分别决定对跟踪精度与控制平滑性的重视程度。

优化还必须遵守模型关系,以及输入、输入变化率和输出的边界。

这一基本形式在采用线性模型和线性约束时,可由二次规划(QP)求解;不需要预先枚举所有动作序列。 

为什么每次只执行第一项?

经过一个采样周期后,实际输出可能因扰动和模型误差偏离预测。此时更新状态估计,将预测时域向前移动一步,再计算新的最优序列。

这种“预测—优化—执行首项—反馈更新”的重复过程,就是滚动时域控制。

MPC 位于反馈控制器内部,动力学模型用于预测施加控制后的运动。后文推导的浮动基座动力学,为建立机器人的预测模型提供基础。

图片

从牛顿定律到浮动基座动力学

先看单摆:一个摆角的运动方程

无质量杆带摆球绕固定轴转动,转轴无摩擦;

惯性力矩+重力项=电机力矩:

  •   m为球质量(kg),l为杆长(m);g为重力加速度大小,约9.81 m/s²。

  •    θ为摆角,从竖直向下起算,向右为正;两点表示角加速度。

  •    电机沿摆角增加方向施加的力矩;本式各项均为N·m。

再看浮动基座:机身和关节一起运动

机身没有固定在地面,平移、转动与关节转动统一写成:

  •    q记录机身位置、姿态和关节角;v由平移速度、机身角速度和关节速度组成;一点 表示对时间求导。

  •   M是质量矩阵;C乘以整机速度v后,给出科氏、离心等速度相关项。

  •   按整机坐标表示、放在左侧的重力项,对应课件的g(q)。它是向量,与单摆式中的加速度g不同。

  •   τ在这里是各关节电机力矩的向量;S是关节坐标选择矩阵,S转置后把力矩填入整机对应位置。

  •   f是地面对第i个接触点的力;J是位置雅克比,将整机坐标变化换成该点位移。J转置乘f,把接触力换算到整机坐标。

  •   i编号各接触点;求和号将各点作用相加;上标T表示矩阵转置。

从每个材料块的牛顿方程开始

下标编号材料块;是质量,是位置,是加速度,是合力。

后文对刚体各材料块求和,保留平动和转动惯性。

单摆的位置、速度与加速度

将支点作为原点,在摆动平面内采用水平 、竖直 坐标。摆球的位置由摆角唯一确定:

图片

对时间求一次导数,使用复合函数求导法则。

把位置对角度的导数记为,它把角速度换算为摆球的线速度:

速度不仅取决于摆角变化的快慢,还取决于当前位置。

因此再次求导时,与两部分都需要求导:

定义沿杆向外的单位向量和沿角度增加方向的切向单位向量:

第一项沿切线,来自角速度大小的变化;第二项沿杆指向支点,来自速度方向的变化。

 分别是径向、切向单位向量,彼此垂直。

将单摆的牛顿方程投影到摆角方向

沿杆的被动轴向力记为,重力向下。电机力矩在杆端等效为切向力,因为“切向力乘杆长”等于力矩。

摆球的牛顿方程是:

图片

先只看切线方向。轴向力没有切向分量,切向加速度为,所以:

这行两边都是N。两边同乘,再把重力项移到左边,所有项就都变为:

将这个投影写成多连杆也能使用的形式。

式(12)已经给出单摆的动力学方程。

接下来要推广到整机的多个坐标,因此把“取切向分量、乘杆长”写成统一形式,便于对各连杆的材料块求和。求和时,理想被动关节连接力的虚功会成对相消,便不必逐一求这些内部未知力。

这就是引入 “虚位移” 与 “虚功” 的目的。

固定当前时刻,想象摆角作一个任意微小变化,杆长保持不变。摆球位置的一阶变化记为,称为虚位移。它不要求真实发生,也不指定经过多长时间。

下面先从位置关系求出它。

用虚位移表达同一次受力投影

先对摆球的两个位置分量求一阶变化。微小转角引起的位置变化,等于相应变化率乘这个转角:

图片

把这两个变化率排成一列,记为位置雅可比,便得到:

与前面速度式中的相同。因为,作用在力上,就相当于先取切向分量、再乘杆长。

任一力对这次假想位移的功记为,称为虚功。把牛顿方程移到同一侧,再与点乘,得到:

代入。杆力沿杆,与虚位移垂直,虚功为0;余下电机、重力和惯性三项:

可以任取,所以括号必须为0,恰好回到式(12)。零来自牛顿定律,实际力与惯性项始终一起投影。

下一步把一个摆角换成整机坐标:先求各材料块的位置与雅克比,再用同一组坐标变化投影并求和。

后面的浮动基座推导就沿这条路展开。

把材料块的位置写成机身与关节的函数

机身、腿都由材料组成。

把它们分成小质量块,对每块使用牛顿定律。腿上的材料块相对机身的位置,随关节角改变。

图片

第块质量记为,机身坐标下的位置记为。

是机身原点的世界位置;把机身坐标分量转到世界系。材料块的世界位置因此是:

对时间求导,两份相乘的因子都要求导:

相对位置通过12个关节角随时间变化。先逐项求导,再把各个系数排成一张表:

有3行、12列;每列是某一个关节角前面的三维系数。不带动该材料块的关节,对应列为0。

把机身转动也写成速度系数

位置求导后,关节部分已经写成K乘关节速度。现在处理机身转动产生的那一项。

图片

机身角速度记为,用世界坐标表示。只考虑机身转动时,点的线速度就是“角速度叉乘相对位置”:

把世界系中的相对位置记为。将刚才的关节速度关系一起代回位置导数:

三项依次来自机身平移、机身转动、关节运动。始终是从机身原点指向材料块的世界系向量。

记录机身位置、姿态和关节角。的中间三项直接使用机身角速度。后面的表示对应18个运动方向的微小变化,其中机身转动用世界系微小转角表示。

把三组速度系数合成雅克比

先把叉乘写成矩阵乘法。下面的矩阵只由s的三个分量组成:

图片

交换叉乘的顺序要变号,所以机身转动项的系数是负的叉乘矩阵:

是三维单位矩阵。把平移、转动、关节速度前面的系数,按的相同顺序排在一行:

这就是雅可比:把整机18维速度换算为该材料块的3维速度。它来自刚才三个速度项的系数合并。

再求一次时间导数,两份因子都要求导:

展开雅克比的导数,分清加速度项与速度项

J也随运动变化。单位矩阵导数为0;另外两组系数逐项求导:

图片

先展开的导数,再展开的各个元素的导数:

将它们代入,再用刚才的去乘,就得到:

系数2来自两次乘积求导中相同的一项。上式只含当前速度,不含待求加速度;加速度都在前一项里。

相关阅读:聊聊雅可比矩阵,如何统一机器人控制的两条主线?

把各材料块的牛顿方程投影到同一组方向

每块都满足牛顿定律。将已经求出的加速度直接代入:

图片

为了把每块的三维方程统一起来,固定当前时刻,给机身和关节一组微小变化。机身微小转角也用世界系表示:

像单摆一样,两边与虚位移点乘,再对全部材料块求和:

理想被动连接力在同一点反向作用,两侧虚位移相同,总虚功相消。重力、电机和地面作用保留,它们对这18个方向的总作用记为。

这里先保留未知地面力,不限制测试变化的接触方向;固定脚的条件最后联立加入。

因此可逐个比较18个方向的系数。

直接从牛顿惯性项收集出M和C

把加速度的两部分代入式(32)、(36)。各材料块使用同一个整机加速度和速度,分别提取出来:

图片

把加速度前面的矩阵系数记为;把第二组括号记为:

和直接来自上面的牛顿方程:收集加速度系数,收集全部速度相关项。可以有不同写法,运动方程使用其乘积。

单摆只有一个坐标,的一列是,因此同一步会给出:

整机只是把这一列扩成18列,并对所有材料块相加。

把重力换算到整机坐标

世界竖直方向向上;g是正的重力加速度大小。每块的实际重力直接写成向下的三维力:

图片

用刚才虚功关系得到的同一个力的换算系数,实际重力贡献为:

把它移到动力学左侧,记成:

此处大写是18维的重力项;小写始终是加速度数值。后两项分别表示电机与接触作用,接着逐项求出。

由关节电机的虚功得到选择矩阵

一个电机对关节两侧施加反向力矩。沿关节轴的两侧微小转动之差,就是关节相对转角变化:

图片

的后12项就是关节角变化。定义取出这部分:

代入电机虚功,把前面的系数收集起来:

前6项填零,后12项填入关节力矩。转置只安排位置;电机力矩的数值和单位保持原样。

机身仍可通过与腿的惯性和接触耦合运动。

加入接触力,得到完整的速度形式

脚点位置与材料块位置形式相同,因此沿用已经完成的求导过程。Kc是脚点相对位置对12个关节角的系数表:

图片

地面力乘以脚点虚位移,再按收集系数:

固定支撑时,脚点真实速度为0。再求导一次,就得到与受力方程联立的接触条件:

将各只脚的雅可比和地面力按相同顺序竖着叠起来,统一记为、:

把动力学中的接触力移到左侧,与式 (52) 的加速度约束一起排成矩阵:

上半行是受力方程,下半行是固定接触约束。给定q、v和电机力矩τ,联立求出整机加速度与地面接触力。

先看清位移向量,再写材料块速度

以下均用世界系表示。

 是第根连杆的质心位置,com是质心(center of mass)的缩写。材料块相对质心的位置是:

有方向,才是长度,单位为。同一刚体上,这个长度不变,方向会随杆转动;它也不同于计算力矩时到力作用线的垂直力臂。

图片

点号表示对时间求导。用、分别收集质心线速度、连杆角速度对整机速度的系数:

同一刚体内,随杆转动,所以。对求导,材料块速度就是:

式(54)与前面的描述同一个点。比较前的系数,得到:

把这份代入式(38),先只对连杆内的材料块求和:

把一根杆内的材料块合并,得到式(57)

接下来只整理式(55)的求和。

总质量为;质心和惯量的定义给出:

图片

叉乘矩阵的每个元素都与位移分量成正比。因此,加权位移之和为0,也使加权叉乘矩阵之和为0:

将式(55)的两个括号展开成四项。同一根杆的和对所有材料块相同,可以提到求和号外;再用式(56)整理:

后两项为0,留下质心平移项和绕质心转动项。再对所有连杆求和:

式(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,没有脚底接触力。

图片

切向与径向垂直,速度相关项的投影为0。重力与同一雅克比相乘,得到:

所以单摆和浮动基座用的是同一条路线:位置求导,整理速度系数,代入牛顿方程,按虚功投影并收集系数。


一条被冠军验证过的工程链路

本文出自:深蓝学院联合英国纽卡斯尔大学正教授、智身科技CTO潘为,推出《四足机器人:从动力学建模到强化学习》实战课程

——以IROS 2025四足挑战赛冠军的实战经验为核心,从动力学建模到实机部署,手把手教你掌握四足机器人全栈开发能力。

这是一门:教你理解四足机器人的物理本质,然后用 RL 控制它的工程实践课:

模型 → 辨识 → 仿真 → 训练 → 部署。

课程全程基于 MATRiX 一站式平台(MuJoCo 高精度物理引擎 + UE5 高保真视觉渲染 + GPU 并行训练),支持多种四足机器人型号,内置丰富地形。

从系统辨识到策略训练,从视觉验证到实机部署,全部在同一个平台内完成闭环。

我们不绑定任何一家硬件生态。CPU 能训,GPU 能训,Windows / macOS / Linux 都能跑。

图片

目前课程已报满

扫码预约下一期 / 了解课程详情

编辑|xubo

审编|具身君

Ref

1. 深蓝学院《四足机器人:从动力学建模到强化学习》-第1章 导论:四足控制的演进-课件.pdf(第4页)。

用于核对目标方程与单摆起点。

2. ETH Zurich, Robot Dynamics Lecture Notes, HS 2017,§2.10、§3.5-3.7。

用于核对浮动基座运动学、牛顿投影与接触动力学。 

3. Kevin M. Lynch and Frank C. Park, Modern Robotics,§8.7 Constrained Dynamics。

用于核对约束力与约束运动的联立形式。

4. MathWorks, Understanding Model Predictive Control:


来源:深蓝具身智能

分享1
Shopping Cart
0 new
购物车

当前购物车数量由 Destoon 实时读取

Cart Items
0
打开购物车时加载商品清单
Go to shopping cart 我的订单