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

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

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

目录

[01 MPC:用模型预测,用优化决定控制](#01 MPC:用模型预测,用优化决定控制)

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

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

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

[02 从牛顿定律到浮动基座动力学](#02 从牛顿定律到浮动基座动力学)

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

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

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

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

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

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

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

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

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

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

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

把重力换算到整机坐标

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

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

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

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

先明确三段位移与两组速度系数

展开两种点速度,推导关节列的关系

代回关节列关系,把两种M写成相同矩阵

退回单摆,核对全部系数

[03 一条被冠军验证过的工程链路](#03 一条被冠军验证过的工程链路)


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

一是 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

相关推荐
蓝天居士2 小时前
ROS2从入门到精通(13)
机器人·ros·ros 2
蓝天居士3 小时前
JY61P认识、学习与实战(3)
机器人·嵌入式·传感器
feasibility.5 小时前
399美元桌面机器人Doly:可爱电子宠物+树莓派开源平台,但不能像ChatGPT那样聊天
机器人·开源·嵌入式·树莓派·diy·桌宠
民乐团扒谱机17 小时前
Rakazo 智能清洁机器人深度评测报告
机器人
feasibility.18 小时前
英伟达把物理AI的3D模型“应用商店”开放了:2575个SimReady仿真资产,总计约472GB,可让agent装配机器人训练场景
人工智能·3d·机器人·仿真·建模·具身智能·英伟达
wangxiaoxuan2171 天前
机器人软件如何分层:驱动、算法、能力、动作与业务
机器人·科沃斯八界
kyle~1 天前
模型预测控制(MPC)
算法·机器人·控制·传感器
草履虫AI课1 天前
强化学习从零到一 06|同策略与异策略:探路的和学习的,可以不是同一个
强化学习
飞Link1 天前
灵巧手是什么?从机械结构、驱动方式到控制原理,认识机器人的精细操作能力
人工智能·python·机器人