反应式控制

• ：把位置拉回去。 比如无人机比目标位置偏右，就增加向左的推力分量。

• ：纠正速度，抑制振荡。 比如向右飞得太快，就施加向左的修正。

• 后两项仍然负责抵消重力、提供轨迹要求的加速度。

• 参数需要调节：位置和速度的纠偏力度需要匹配。

• 复杂系统更难设计：例如四足机器人涉及多关节、接触和身体平衡。

• 普通独立PID不直接处理耦合和约束：例如汽车的车速会影响转弯表现，油门和转向不能完全分开考虑。

• 缺少对未来的主动规划：即使使用期望加速度作为前馈，也没有显式预测、优化未来一段时间的动作。例如前方即将急转弯，它本身不会据此规划提前减速。

• 重力补偿 ：已知重力会把无人机向下拉，就直接提供相应的向上推力。

• 加速度前馈 ：目标轨迹要求怎样的加速度，就根据牛顿第二定律，提供对应的力。

最优控制

• ：实际预测状态与目标状态的差距。

• ：相邻两次控制指令的变化。

• ：决定多重视动作平滑；越大，就越不愿意突然改变控制指令。

• 运动规律：每一步必须满足车辆模型，车辆不能瞬间移动到任意位置。

• 能力限制：加速度、转向角和速度必须处于允许范围，例如.

• 避障限制：车辆不能与障碍物发生碰撞。

第一步采用怎样的加速度和转角，第二步怎样调整，后续什么时候回正……

• 模型不够准确，预测误差逐渐累积。

• 执行器存在延迟或偏差，指令不能被完全准确地执行。

• 风、路面变化等外部扰动。

• 规划时间太长，优化问题计算量过大。

Model Predictive Control

1. 获取当前状态：测量或估计汽车的位置、速度、朝向。

2. 预测并优化：利用车辆模型，计算未来2秒内怎样加速、转向更合适。

3. 只执行第一步：仅执行接下来0.1秒的控制指令。

4. 重新规划：0.1秒后获取新状态，再规划此后2秒的动作。

• ：模型预测的输出，例如车辆位置。

• ：对应时刻的目标输出。

• ：控制指令的变化。

• N：预测步数；：对动作平滑程度的重视程度。

• 能考虑未来：例如发现前方将要急转弯，可以提前减速。

• 能修正偏差：每次重新获取状态，调整后续计划。

• 控制单次问题规模：每次只规划有限时间，不必一次算完整个任务。

• 可利用上次结果加速求解：把上一轮剩余的计划作为新一轮的初始猜测，称为“热启动”。

MPC Design

• 上图：尽量贴近参考轨迹。 偏离蓝色虚线越远，受到的惩罚越大。

• 下图：在赛道边界内尽量向前推进。 车辆可以选择更有利的行驶路线，不必始终贴着参考线。

当前能找到满足约束的控制方案；执行第一步之后，下一次仍然能找到满足约束的方案。

Linear MPC – Unconstrained Case

1. 先把车辆模型写成线性形式。

• ：当前状态对下一时刻的影响。

• ：控制输入对下一时刻的影响。

• ：从状态计算输出。

2. 连续预测，把未来状态全部用控制输入表示。

3. 用二次型代价衡量控制效果。

4. 把状态代入代价函数，直接求最优控制。

MPC and Linear Quadratic Regulator (LQR)

1. 为什么要换一种求解方法？

• 先看：只剩最后一步时，应该怎样控制？

• 再看：还剩两步时，应该怎样控制？

• 依次反推，直到算出当前的 。

2. Bellman最优性原理是什么意思？

3. 先解决最后一步。

4. 为什么可以继续往前算？

5. 它为什么更高效？MPC和LQR有什么关系？

如果MPC每次求解的问题恰好采用线性模型、二次代价，而且没有状态和输入限制，那么这一次优化，就是一个有限时域LQR问题，可以用Riccati递推求解。

有延迟的MPC

有约束的线性MPC

• z：未来一串控制指令。

• ：当前状态。

• 不等式：把加速度、转向角、速度等限制统一写在一起。

什么是QP

• z：我们要选择的量，例如未来的加速度、转向角。

• 目标函数：评价这个选择有多好。

• 约束：这个选择必须满足哪些限制。

• “二次”：目标中可以包含变量的平方、两个变量相乘等项。

• ：希望 u 接近3。

• ：希望 u 不要太大。

1. 很多工程目标天然就是“平方和”。
例如跟踪误差：
平方能够避免正负误差相互抵消，还会更重地惩罚大误差。把多个误差、控制输入的平方加权相加，就很容易得到二次目标函数。
再加上速度、输入等线性限制，问题就能写成QP。

2. 常见的凸QP具有很好的求解性质。
当 H 半正定时，目标函数可以想象成一个向上开口的“碗”，配上线性约束，就得到凸QP。
它的一个关键优点是：局部最优解也是全局最优解，不会出现找到一个小谷底，却错过另一个更低谷底的情况。若 H 正定且最优解存在，最优解还是唯一的。
不过，并非所有QP都是凸的，还要看目标函数的曲率，也就是矩阵 H 的性质。

3. 有成熟的求解算法，适合反复计算。
对MPC来说，优化通常需要每隔几十或几百毫秒重新进行一次。能够把问题写成有结构的凸QP，就可以利用专门算法加快计算。
更复杂的非线性优化，也常采用“每次构造并求解一个近似QP”的方法，逐步改进结果。

显示MPC Explicit MPC

1. 从“现场解题”变成“提前推导好规则”。
普通MPC每次都要：
显式MPC则先离线分析给定范围内的状态，划分区域，并为每个区域求出控制公式。运行时：
这里不是逐个枚举无限多个状态，而是利用数学方法，求出对整片状态区域都适用的公式。这就是课件中的“多参数二次规划（mpQP）”。

2. 用一个简单例子说明分区是怎样出现的。
问题是：
其中，x 是已知参数，z 是需要选择的量。
目标 希望 z 尽量靠近零，但允许的范围随 x 改变。
• 当 时，右边 ，因此可以直接选择最理想的 z=0。
• 当 时，右边是负数，z=0 不满足约束。允许范围内最接近零的位置，就是边界 z=1+3x。
所以提前就能写出：
例如，测得 ，直接得到 ；测得 x=0，直接得到 。
这就是“分段仿射”：不同区域使用不同的“系数乘状态，再加常数”的公式。 图中红线 是区域边界，实线是最优解，虚线是最小代价。



图中的KKT条件是推导最优解的数学条件。这里可以先抓住两种情况：约束允许取零，或者约束迫使最优解落在边界上。

3. 为什么显式MPC不能普遍替代在线求解？
多维状态下，每个区域通常对应一个控制公式：
彩色图展示的是状态空间的分区，每种颜色代表一套适用的控制规则。
当状态维数、约束数量或预测长度增加时，区域数量可能迅速增长，带来两个问题：
• 存储量大：需要保存许多区域边界和控制公式。
• 查找耗时：运行时要判断当前状态究竟落在哪个区域。
因此，显式MPC特别适合一些规模较小、要求快速响应、运行设备计算能力有限的控制问题；问题规模较大时，在线求解QP往往更实用。

线性时变模型预测控制LTV-MPC和非线性模型预测控制Nonlinear MPC

• 为什么需要“线性时变模型”？（Linear Time-Varying MPC）
前面使用的模型是：
A、B固定，意味着一直使用同一套运动关系。但车辆在低速、高速、直行和转弯时，对控制指令的响应会有所不同。
因此，可以采用：
每一步允许使用不同的 ，以适应不同工况。这里 是一个已知的偏置项。
每轮优化前先计算这些系数，求解时把它们视为已知量，模型对待优化变量仍然是线性的或仿射的。

• 线性化，就是用局部简单关系近似复杂关系。
例如，车辆模型中有：
其中包含三角函数以及变量之间的乘积，因此是非线性的。
线性化可以想象成：在曲线的某一点附近，用切线近似曲线。 选定一组已知状态和输入 ，就有：
其中， 由偏导数构成，描述“状态或输入稍微变化时，系统变化速度会怎样改变”。
整理后就是：





图中的大矩阵，就是把车辆的四条运动方程分别这样处理，再合并起来。离展开点越远，这种近似通常越不准确。

• 离散化，是把连续变化变成逐步预测。
计算机按采样时刻运行，需要把 转成下一步状态。课件使用最简单的前向欧拉方法：
因此：
把这些系数分别记成 ，就得到前面的离散预测模型。其中 是采样周期，I 是单位矩阵。
线性化处理“关系太复杂”，离散化处理“计算机如何一步一步预测”。

• 目标仍然是跟踪准确、动作平滑，并满足限制。
代价为：
前两项要求车辆贴近目标位置，后两项避免突然改变加速度和方向盘转角。
为了方便优化控制变化量，可以把上一时刻的输入也放进状态中：
这就是“增广模型”的基本意思：额外记住之前用了多大的控制量，方便计算这次应该调整多少。
当预测模型已经线性化，代价是二次型，速度、加速度和转角限制也是线性的，就可以构成一个约束QP问题。

• 但应该在哪条轨迹附近线性化？
要算最优轨迹，需要先线性化；但要线性化，又需要先有一条用来展开模型的轨迹。
常见的处理方式是：
• 根据参考轨迹选取展开点。
• 把上一轮的最优预测轨迹向前移动，作为这一轮的初始猜测，也就是热启动。
于是形成：
实际实现中，可以根据计算时间预算决定是否继续重复这个过程。

• 直接使用非线性模型，就是NMPC。
NMPC在原优化问题中保留非线性运动关系，通常形成非线性规划问题（NLP）。

方法	模型处理方式	主要计算特点
LTV MPC	使用随工况更新的局部线性近似	配合二次代价和线性约束，可求解QP
NMPC	在原问题中保留非线性模型	通常更难求解，可能存在多个局部最优解

NMPC也可以在求解过程中使用QP。例如，序列二次规划（SQP）就是不断构造、求解近似QP，再更新结果，逐步逼近原非线性问题的解。

Tube MPC

1. 为什么需要“管束”？



真实无人机会受到风、模型误差等影响，因此实际运动很难始终与预测完全一致。
第图中：



• 红线：名义轨迹，即用不考虑扰动的模型规划出的轨迹。
• 蓝线：真实系统的运动轨迹。
• 灰色管道：围绕名义轨迹的允许误差范围。
“保持在管内”表示误差受到控制。有持续扰动时，实际状态不一定与名义状态精确重合，但可以保持在一定范围内。

2. MPC和反馈控制器怎样配合？
可以把控制指令理解为：
• ：MPC根据名义模型算出的计划动作。
• ：根据实际状态与名义状态的偏差，追加的纠偏动作。
例如，无人机计划直线飞行，风把它吹向右侧，反馈控制器就增加向左的修正。
Tube MPC还需要在规划时留出误差空间。 假设允许的横向位置是 [-1,1] 米，且能保证跟踪误差不超过 0.2 米，那么名义轨迹就应限制在：
这样，即使出现允许范围内的偏差，实际位置仍能满足限制。这叫约束收紧。

3. 图的内外环和大矩阵是什么意思？



外环负责规划：优化器利用简单的名义模型，预测无人机的位置、速度和加速度，生成计划。
内环负责跟踪：图中用PID控制器示意，根据真实系统的反馈调整动作，让无人机跟随计划，并抵抗扰动。
左边采用的是“三阶积分模型”：
其中 分别是位置、速度、加速度，j 是加速度的变化率，也叫 jerk。
这个矩阵表达的是：给定当前的位置、速度、加速度，以及接下来施加的 j，就能预测下一步的状态。 是采样周期。

Hybrid MPC

1. 前两张：什么叫“混合”？
以汽车为例：
• 连续量：速度、转矩、油门和刹车开度，可以取一定范围内的实数。
• 离散模式：倒挡、空挡、1挡、2挡等，只能从有限选项中选择。
不同挡位下，同样的油门可能产生不同的加速效果。因此，模式切换和连续运动相互影响，需要一起考虑。



图中的 0、1 用来表示开关或逻辑状态，多种模式也可以用多个二进制变量编码。这里“离散”指模式选择，与前面“每隔一段时间采样”是不同的概念。

2. 怎样把“如果……那么……”交给优化器？ 课件采用 MLD（混合逻辑动态）模型，把逻辑判断转换成包含连续变量和整数变量的不等式。



例如，小球有两种运动模式：
第一种按重力改变速度；第二种让速度反向，并保留一部分速度幅值。
引入一个模式变量：
再看课件中的一条约束：
• 当 时，两端都是零，强制得到自由落体公式。
• 当 时，这条约束被放宽，同时由另一条约束强制执行反弹公式。
M的作用是放松未被选中的那条规则。 它需要根据变量范围合理选择。
这样，优化器就能用数学约束处理模式切换。课件的小球模型是简化示意，实际碰撞还需要处理触地瞬间等细节。

3. 无人机吊绳也是一个混合系统。
吊绳有两种状态：
• 绷紧：可以传递拉力。
• 松弛：不传递拉力。
课件用下面的关系表达：
其中，T 是绳子拉力， 是两端距离， 是绳长。配合 ，它意味着：
• 若有拉力 T>0，则必须有 ，绳子绷紧。
• 若绳子松弛 ，则必须有 。
这种关系称为互补关系。图中上方的向量方程则描述重物受力与加速度之间的关系。

模型预测轮廓控制

1. 关键变化：路径上的目标点也可以一起优化。
前面按时间跟踪轨迹时，参考点通常是预先指定的，例如“第2秒应该到达这里”。
MPCC用参数 表示沿路径走到了哪里：
控制器同时决定无人机怎么运动，以及 应该推进多快。
这里的 是路径进度，不是偏航角。 遇到急弯或扰动时，可以暂时放慢推进；路况允许时，再加快前进。

2. 目标函数：既要贴近路径，又要向前推进。
课件中的代价可以简写为：
第一项希望无人机靠近路径，第二项鼓励路径进度增加。其中：
• ：预测的无人机位置。
• ：路径参数 的变化率。
• ：决定多重视前进速度。
因为目标是最小化代价，所以前进奖励前面加负号。 越大，这部分代价越小。
图中的红色点是预测位置，蓝色点是路径上对应的参考点。优化器会同时调整这两组点的对应关系。

3. 为什么状态和输入看起来很多？
状态主要分成两部分：
• 无人机在 x、y、z 三个方向的位置、速度和加速度。
• 路径进度 ，以及它的变化率 。
输入中的 是三个方向的加速度变化率，即 jerk； 用来调整虚拟路径进度的变化。
这样，控制器既能规划无人机的运动，也能平滑地调整沿路径推进的节奏。若 不是按路径长度定义的， 就不能直接当作无人机的实际速度。

4. 安全走廊负责限制“可以飞到哪里”。
表示预测位置必须位于允许的走廊内。可以把每一行不等式理解为一面边界，多个边界围成一个可飞行区域。
同时还有限制速度、加速度、输入等的约束。预测末端的速度限制，则避免计划以过高速度结束，为后续制动或转弯留出余量。
走廊来自环境中的可飞区域；前面Tube MPC的“管束”，描述的是实际状态相对名义轨迹的误差范围。