第5章：模型预测控制（MPC）

唔希迪西

已于 2024-06-11 22:25:40 修改

阅读量3.9k

点赞数 36

分类专栏：自动驾驶文章标签：人工智能

于 2024-06-11 20:50:45 首次发布

本文链接：https://blog.csdn.net/weixin_44063383/article/details/139474971

版权

自动驾驶专栏收录该内容

13 篇文章 17 订阅

订阅专栏

5.1 模型预测控制（Model Predictive Controller -- MPC）

注：MPC 更像是一种控制策略的框架，它使用了最优的控制思想在里面，如下的几种控制策略其实都是 MPC 的变体；

MHC（Moving Horizon Control）：滚动时域控制
RHC（Receding Horizon Control）：后退时域控制
DMC（Dynamical Matrix Control）：动态矩阵控制
GPC（Generalized Predictive Control）：广义预测控制

5.1.1 MPC 的基本原理

a

已知：系统的【误差型】【离散型】状态空间方程

$x(k+1)=Ax(k)+Bu(k)$
$y(k)=Cx(k)+Du(k)$

a

注意：

如果在 $k-1$ 时刻已知控制量为 $u_{k-1}$ ，那么从 $k-1$ 时刻开始一直到 $k$ 时刻，该控制量一直不变，将持续一个时间步；
将控制量输入系统则可以得到系统的输出量 $y_{k}$ 和系统的状态量 $e_{k}$ ；
因此，已知前一个时刻的控制量 $u_{k-1}$ ，即可得知当前时刻的输出量 $y_{k}$ 和状态量 $e_{k}$ ；

MPC 的设计理念：

假定当前系统处于时刻 k，则系统关于时刻 k 之前的所有信息都已知
- 已知 $u_{0}\sim u_{k-1}$
- 已知 $e_{0}\sim e_{k-1}$ 和 $e_{k}$
- 已知 $y_{0}\sim y_{k-1}$ 和 $y_{k}$
而当前时刻 k 我们只知道系统的输出 $y_{k}$ 和状态量 $e_{k}$ ，而并不知道在时刻 k 处该采取什么控制策略，即我们并不知道该给 $u_{k}$ 赋什么值，这需要 MPC 控制器来给出

MPC 控制器的基本原理：

直接在当前时刻 k 处预测出未来一个时间段内（假设含 N 个时间步，从时刻 k 到时刻 k+N-1）所有的控制动作 $u_{k}\sim u_{k+N-1}$ ，且要保证这 N 个控制动作对当前的系统是最优的；
然后选择这 N 个控制动作中 k 时刻的控制动作（即第一个控制动作）作为当前时刻 k 的控制动作；然后当前时刻将变为时刻 k+1，再进行相同的操作；
之所以只使用第一个控制动作，而舍弃其他的控制动作，是因为环境是有扰动的，在做完第一个动作后系统并不会像预估的那样完美的转换到下一个系统状态；
这样的操作可以保证系统不是只看到了当前时刻的系统误差，从而只对当前时刻的误差进行控制；这样是很短视的行为；MPC控制器不仅考虑了当前的系统误差 $e_{k}$ ，还考虑到了未来时间步内所有的系统误差 $e_{k+1}\sim e_{k+N}$ ，这样的控制效果更好；
如何判断这 N 个控制动作对当前系统是最优的：
- 建立代价函数 $J=\sum_{k}^{k+N-1}[e_{k}^{T}Qe_{k}+u_{k}^{T}Ru_{k}]+e_{N}^{T}Fe_{N}$ ，我们的目的是，求出当前时刻 k 时，在未来 N 个时间步上采用怎么样的控制动作 $u_{k}\sim u_{k+N-1}$ ，可以将系统的代价函数值最小；
- Q和F矩阵代表对系统误差的重视程度；R矩阵代表对系统能耗的重视程度；
- 因为在时刻 k 处我们预估了动作 $u_{k}\sim u_{k+N-1}$ ，且时刻 k 处本来就已知 $y_{k}$ 和 $e_{k}$ ，因此在时刻 k 到 k+N-1 这N个时刻， $u_{k}\sim u_{k+N-1}$ ， $e_{k}\sim e_{k+N}$ ， $y_{k}\sim y_{k+N}$ 均为已知；
- N是系统往前预测的时间步的数量；
每次输出完一个时刻 k 处的控制命令，就会把控制区间和预测区间向后混动一个时刻，形成一个新时刻下的 MPC 控制问题；

总结：MPC 的工作流程

【1】在采样时刻 K 处获得当前的已知信息 $x_{k}$
【2】在线的求解一个有限时域内的优化问题（对自定义的代价函数 J 求最优解的问题），得到这个有限时域的控制序列 $u_{K}\sim u_{K+N-1}$
【3】将获得的控制序列的第一个值 $u_{K}$ 作用到被控对象并持续一个时间步，将系统往后整体推一个时间步
【4】在下一个采样时刻 K+1 重复上述过程

5.1.2 无约束线性系统的MPC

注：无约束线性系统，是一定有解析解的；

无约束线性系统的模型预测控制：

线性：指系统的误差型离散型状态空间模型为线性的
$x_{k+1}=Ax_{k}+Bu_{k}$
$y_{k}=Cx_{k}$
无约束：即系统是没有约束的
这两个条件【无约束】【线性】都是针对系统的

计算控制量 $u_{0}\sim u_{0+N-1}$ 实际值的过程：方法1

当前时刻为 k=0
已知 $x_0$ 和 $y_0$ ；（系统初始状态，系统初始输出）
未知 $u_{0}\sim u_{0+N-1}$ ， $x_{1}\sim x_{N}$ 和 $y_{1}\sim y_{N}$ ；
设定时域预测长度为N，预测时刻 0 ~ N-1 处的控制量

a

Step1：建立代价函数 J

a

a

Step2：求解可以使得代价函数 J 的值最小的控制量 z

我们发现，针对线性无约束的系统使用 MPC 控制器，该系统的最优控制策略是线性的

计算控制量 $u_{0}\sim u_{0+N-1}$ 实际值的过程：方法2

问题：

我们已经得到直接计算控制量 $u_{0}\sim u_{0+N-1}$ 的公式 $z=-H^{-1}F^{T}x_{0}$
但是如果我们直接拿这个公式去计算控制量的值，那计算机的计算开销会非常的大，因为这个公式中最主要的就是对矩阵 H 的逆的计算，但是矩阵 H 是一个稠密矩阵（即非零元素占矩阵所有元素的比例比较大），而计算机不擅长对稠密矩阵的相关计算，因此我们不能直接求解

解决方案：使用动态规划（DP）的方法求解向量 z 的值

a

a

Step0：动态规划的理论基础是最优性原理

介绍：最优性原理是 bellman 等人提出的理论，可以将一个多阶段决策问题，变成一系列相互联系的单阶段问题，然后一个个解决；
最优性原理：不管系统的初始状态 / 初始策略是什么，余下的控制策略一定符合最优控制策略；
解释：就是说不管系统从哪一点出发，他的最优的控制策略最后一定会趋同；
举例：比如我们现在的目的是从时刻 0 开始（系统初始状态为 $x_{0}$ ），求解系统在时刻 k 时到达最优状态 $x_{k}$ 所需要的所有的控制量 $u_{0}\sim u_{k-1}$ ；而如果我们不从时刻 0 出发开始求，而是在时刻 3 且系统处于当前时刻的最优状态 $x_{3}$ 的时候开始求解，求解系统在时刻 k 时到达最优状态 $x_{k}$ 所需要的所有的控制量 $u_{3}\sim u_{k-1}$ ；我们会发现，不管我们的起点选在哪里，我们最终得到的最优控制量中 $u_{3}\sim u_{k-1}$ 这一段，都是一模一样的；即不管我们的起点选在哪里，最后的最优性策略一定是趋于一致的；

a

a

Step1：使用最优性原理将求解最优控制量 $u_{0}\sim u_{N-1}$ 的问题拆分为一个个子问题

也就是说无论前面的 0 ~ k-1 步是如何达到时刻 k 时的状态 $x_{k}$ 的，我们只需要从当前时刻 k 的状态 $x_{k}$ 开始往后看这个子问题，那么后面的 $u_{k}\sim u_{N-1}$ 仍然是这个子问题的最优解；

a

a

Step2：使用动态规划求解向量 z 的值

注意1：对于方法2，可以很明显看出 LQR 其实是线性无约束 MPC 的一种特殊情况

都是通过调用里卡提方程求解矩阵 P
$P=A^TPA+Q-A^TPB(B^TPB+R)^{-1}B^TPA$
都是通过同一公式求解矩阵 K
$K=-(B^{T}PB+R)^{-1}B^{T}PA$

a

解释：

线性无约束 MPC 求解的代价函数为：
$J=\sum_{k}^{k+N-1}[e_{k}^{T}Qe_{k}+u_{k}^{T}Ru_{k}]+e_{N}^{T}Fe_{N}$
LQR 是针对无限时域空间的优化，是通过从无限远时刻一直通过里卡提仿真迭代，迭代到当前时刻 k 为止，获得一个 P 矩阵的稳定解；
然后将求得的当前时刻 k 的稳定 P 矩阵作为以后所有时刻的共用 P 矩阵来用；
LQR 求解的代价函数为：
$J=\sum_{k}^{\infty }[e_{k}^{T}Qe_{k}+u_{k}^{T}Ru_{k}]$
MPC 是针对有限时域空间的优化，是在有限时域空间的最末端（时刻 k+N-1）一直通过里卡提方程迭代到当前时刻 k 获得一个 P 矩阵的稳定解；
但是这个 P 矩阵并不是共用的，而是每往前走一个时间步，都会重新求解一个 P 矩阵的稳定解；

注意2：LQR 控制器与 MPC 控制器之间的区别

线性系统：
LQR 控制器只能针对线性系统进行调节；
MPC 不仅可以对线性系统进行调节，还可以对非线性系统进行调节；（只是这一节讲的就是线性无约束 MPC，并没有体现这个区别）
约束条件：
LQR 控制器不能对带有约束条件的系统进行控制；
MPC 不仅可以对不含约束条件的系统进行控制，也可以对带有约束条件的系统进行控制；
灵活性：
LQR 只在当前时刻 k 处求出一个 P 矩阵，后面的控制策略全部使用这个 P 矩阵；
MPC 的 P 矩阵在每个时间步都会重新求解，针对不同的情况进行相应的调整；
是否在线：
LQR 是离线控制策略，因为 P 矩阵是早已计算好的，所有时刻的控制策略都使用这同一个矩阵求解；
MPC 是在线控制策略，在每个时间步处都要重新根据当前时刻的系统状态值，重新计算矩阵 P 的，所以他必须是在线的；

注意3：通过上述两种方法，我们都可以得到一个结论，即针对线性无约束 MPC 控制器，控制策略为线性的（ $u=kx$ ）；

5.1.3 MPC控制器 VS 经典控制器

经典控制器（PID）：

离线输出控制策略（不灵活）：一直使用同一个控制策略，如 $u=-Kx$ ，其中的矩阵 K 是不变的；
无法同时控制多个变量：PID 控制器使用的是传递函数，应用场景最多就是单入单出系统，即一个输入对应一个输出；
系统不能有约束：PID 天然的局限性，对于系统约束的应对手段很少且不系统（限幅 ...），没有一套可以应对这种约束条件的统一解决方法；

MPC 控制器：

在线输出控制策略（灵活）：控制策略随系统的变化而变化，如线性无约束 MPC，使用的控制策略 $u=-Kx$ 中的 K 矩阵是随着系统状态的更新而不断变化的；
可以同时控制多个变量：因为 MPC 控制器和 LQR 控制器都一样，是使用状态空间方程进行推导的，因此可以同时控制多个变量，也可以同时观测多个系统状态量；
系统可以有约束：MPC 支持带约束条件的系统；

a约束分为：

系统的状态空间方程：因为系统的状态空间方程就是车辆的动力学方程，车辆的行驶是一定要满足动力学方程的；
物理约束：如电机转速最快是 1000，因此控制量不能是让电机转速达到 2000 转；
安全约束：比如在一些环境中不允许变量调节超过这个环境所能承受的阈值（汽车在高速行驶时，若前轮转角大于某一阈值，会导致车辆侧翻），因此必须限制控制量的值；
性能约束：我们对系统性能有一定的要求；

MPC 控制器的缺点：

求解过于费时，对算力要求过高；

举例：

代价函数：
- $minJ(x_{k},U)$
约束条件：
- $x(k+1)=Ax(k)+Bu(k)$
- $y(k)=Cx(k)+Du(k)$
- $u_{min}\leq u_{k|k+i}\leq u_{max}(i=0,1, ... ,p-1)$
- $y_{min}\leq y_{k|k+i}\leq y_{max}(i=0,1, ... ,p)$
最优解：
- $U_{k}^*=argmin_{U_{k}}J(x_{k},U_{k})$
- $U_{K}^*=\left \{ u_{k|k}^*,u_{k+1|k}^*, ...,u_{k+N-1|k}^* \right \}$

5.1.4 有约束线性系统的MPC

不管是【线性系统有线性约束MPC】还是【线性系统有非线性约束MPC】，对问题的求解最重要的是我们如何将问题构造成一个二次型函数最优化的问题；

这样我们就可以通过调用现成的二次规划（QP）求解库进行求解；

常见的二次规划求解库：

OOQP
OSQP
qpOASES
ECOS（SOCP）
GUROBI
MOSEK（LPs , QPs , SOCPs , MIPs ...）

已知量：

系统的线性状态空间方程：
线性状态空间方程指的是，方程中涉及到的未知量 $x(k)$ 和 $u(k)$ 都是一阶的
当用来描述该系统的状态空间方程是线性的，那么就说这个系统是线性的
$x(k+1)=Ax(k)+Bu(k)$
$y(k)=Cx(k)+Du(k)$
约束条件：
约束条件分为线性约束和非线性约束，非线性约束就是指带有高次方项 / 三角函数项的约束条件；
$m_k(x(k),u(k))\leq 0$
$n_{k}(x(k),u(k))= 0$
$x(k)\in \chi$
$u(k)\in U$
人为指定的代价函数：
$J=\sum_{k}^{k+N-1}[x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}]+x_{N}^{T}Fx_{N}$

a

a

推导出：

二次型目标函数：（推导过程在上面）
$J(z,x_{k})=\frac{1}{2}z^THz+x_{0}^TFz+\frac{1}{2}x_{0}^TEx_{0}$
$z^T=\left [ u(k),u(k+1),.....,u(k+N-1) \right ]$

（1）有线性约束线性系统的MPC

求解过程：

将问题化为带线性约束的二次规划问题（凸优化问题）
$min_{z}J(z,x_{k})=\frac{1}{2}z^THz+x_0^TFz+\frac{1}{2}x_{0}^THx_{0}$
$Gz\leq W+Sx_{k}$
调用二次规划库求解向量 $z$ 的值；
如果求解器发现无法求解当前的这个凸优化问题（即无解）时，那么就会启用另一套备用的控制器来对当前时间步进行控制；

常见的 QP 求解方法：

（2）有非线性约束线性系统的MPC

注：和上面的【有线性约束线性系统的MPC】的求解过程一模一样，只是其中的约束条件换成非线性的罢了；思路都是将原问题变成一个二次规划问题，再调用现成的二次规划求解库计算；

5.1.5 有约束非线性系统的MPC

注：其实车辆并不是一个线性的系统，只是我们在推导车辆横向动力学模型的过程中，将系统中的非线性项（三角函数）全部使用假设化作了线性项（比如假设高速行驶条件，车辆转角很小，从而视 $tan(\theta) \approx \theta$ ），从而使得我们得到的非线性系统的非线性状态空间方程转化为了线性状态空间方程；

注：其实车辆是一个【有非线性约束的】【非线性的】系统

目标函数：
$J=\sum_{k}^{k+N-1}[x_{k}^{T}Qx_{k}+u_{k}^{T}Ru_{k}]+x_{N}^{T}Fx_{N}$
约束条件：
非线性系统的状态空间方程：
$x(k+1)=f(x(k),u(k))$
$y(k+1)=g(x(k),u(k))$
$m_k(x(k),u(k))\leq 0$
$n_{k}(x(k),u(k))= 0$
状态量取值范围约束： $x(k)\in \chi$
输入量取值范围约束： $u(k)\in U$

求解过程：

已知带非线性约束的非线性系统
构建模型，代价函数，约束条件
调用市面上已有的成熟的求解库求解代价函数的极小值点

注：市面上已有很多成熟的用来计算【非线性代价函数 + 非线性约束】问题的求解库，所以我们的主要任务就是如何针对带非线性约束的非线性系统

选取模型
建立代价函数
构建非线性约束条件
给出合理的约束范围

5.2 使用 MPC 控制车辆的运动

架构图：

注意：使用 MPC 控制器的系统架构和使用其他控制器（如 PID，LQR）的系统架构对比，可以发现多了一个【底层控制器（low level controller）】；

因为 PID / Stanely / LQR 这些控制器求解得到控制量的复杂度很低，也就是说求解控制量不会消耗很长的时间，因此通过控制器得到的控制量可以直接输出给车辆作为控制命令；
但是 MPC 控制器的时间复杂度取决于对车辆系统的建模的复杂度，他每一轮都要求解 N 个未来时刻的动作，并且如果车辆的状态空间模型是非线性的话，二次规划的求解将更加难以收敛，更加缓慢，也就是说求解控制量会消耗很长的时间；
所以可能出现这样的情况：时刻 k 时我们将反馈的状态量输给 MPC 控制器，但是 MPC 控制器计算出来时刻 k 应该采取的控制动作，需要三个时间步的时长，即一直到时刻 k+3 才能知道时刻 k 时应该采取什么动作，而我们必须在 k 时刻就知道应该采用什么样的控制动作；
因此很难保证控制器输出控制信息的连续性和实时性，因此我们通过将 MPC 控制器视为上层控制器，再给系统加一个底层控制器，让 MPC 控制器还没有求解出来 k 时刻应该使用的控制量时，使用底层控制器（PID ... ）代为操控，使得车辆的控制命令更为连续和高频，从而保证车辆的一个较好的抗干扰性能；

使用 MPC 控制器的系统架构有以下几个重要的方面：

精确的车辆系统建模
代价函数
约束条件
底层控制器

5.2.1 车辆运动模型构建 -- Curvilinear Coordinates Vehicle M $n$ odel

注1：一个精准的模型可以使得在使用 MPC 求解最优控制量时更精确，但是也会耗费更长时间，所以在实际任务中，我们需要在【模型的简化】和【控制命令的准确度】之间进行一个权衡；

注2：主流使用的3种车辆模型

车辆横向运动学模型（Kinematic Bicycle Model） + 车辆纵向动力学模型
车辆横向动力学模型（Dynamic Bicycle Model） + 车辆纵向动力学模型
Curvilinear Coordinates Vehicle Model

（1）指定状态量 x 和输入量 u

$s$ ：目标点处的路径长度
$\dot{s}$ ：目标点在参考线上的移动速度（方向为当前目标点的切向）
$\phi _{s}$ ：目标航向角
$\dot{\phi _{s}}$ ：目标点在参考线上的角速度
$R$ ：目标点处的转弯半径
$k$ ：目标点处的曲率（取粗略估算 $k=\frac{1}{R}$ ）
$n$ ：横向误差
$\dot{n}$ ：横向误差变化率（取粗略估算约为 $\dot{n}=v_xsin\mu +v_ycos\mu =vsin(\beta +\mu )$ ）
$v$ ：车辆速度
$a$ ：车辆加速度
$v_{s}$ ：车辆在目标点切向（目标航向）方向上的速度分量
$\beta$ ：质心侧偏角
$\mu$ ：local heading（ $\mu =\varphi -\phi _{s}$ ）
$\varphi$ ：横摆角

$r$ ：车辆角速度
$R_{v}$ ：车辆转弯半径
$\delta _{f}$ ：前轮转角

取状态量 x 为：

取控制量 u 为：

a

a

注意：控制量其实有多种选择，但是为什么选择第三种方式？

【速度 $v$ 】和【前轮转角 $\delta$ 】
【加速度 $a$ 】和【角速度 $\dot{\delta }$ 】
【加加速度 $jerk$ 】和【前轮转角加速度 $\ddot{\delta }$ 】

a

a

解释：选择第三种作为控制量有以下好处

微分没有积分精确；
如果我们得到的控制量是低阶的（如速度 $v$ ），那么得到高阶的性质（如加加速度 $jerk$ ）需要微分，但是微分非常容易收到环境噪声的影响；
而我们直接得到高阶的性质，再通过积分得到低阶的性质，相对而言更加准确；
因为加加速度 $jerk$ 和角加速度 $\ddot{\delta }$ 是和车辆的运行舒适度息息相关的，而我们是很看重舒适度的；
通过对高阶导数进行优化求解，可以使得低阶导数连续性更好，跟踪轨迹越平滑流畅；

（2）构建车辆模型 -- 推导过程

图1

图2

a

a

注意：到目前为止上述公式中只有 $\beta$ 还是未知的，需要计算出来

总结：Curvilinear Coordinates Vehicle Model 模型特点

横纵向控制耦合在一起；
包含了车辆的运动轨迹和参考线之间的相对位置关系；
非线性（包含很多三角函数）；

注：正是由于 Curvilinear Coordinates Vehicle Model 模型是非线性的，并且耦合了横纵向控制，使得模型的精确性相对于之前使用的运动学模型 / 动力学模型有了很大的提升；但是随之，由于模型复杂度高了，对这个模型进行求解的速度会变慢；

5.2.2 目标函数构建

代价函数分 4 部分：

跟踪参考轨迹的代价： $J_{tracking}$
舒适性的代价： $J_{comfort}$
安全性的代价： $J_{safty}$
代价函数无解 / 不收敛的代价： $J_{terminal}$

$J(x_{k},U)=\sum_{k=0}^{N-1}J_{stage}(x_{k},u_{k},\lambda _{k})+J_{terminal}(x_{N})$

$J_{stage}=w_{t}J_{tracking}+w_{c}J_{comfort}+w_{s}J_{safety}$

$U=\left \{ u_{k},u_{k+1},...,u_{k+N-1} \right \}$
代表时间步 i 处的代价函数值，一个 stage 即代表一个时间步
- $x_{i},u_{i}$ ：第 i 个时间步处的状态量和控制量
- $\lambda _{i}$ ：第 i 个时间步处的松弛变量
$w_{t},w_{c},w_{s}$ 为对应轨迹追踪 / 舒适性 / 安全性代价的权重

（1）跟踪参考轨迹的代价

$J_{tracking}=w_{\dot{s}}(\dot{s}-\dot{s}_{ref})^2+w_{n}n^2+w_{\mu }(\mu +\beta )^2$

第 1 项：参考点纵向速度误差
$\dot{s}$ ：车辆在行驶过程中不断在参考线进行匹配的速度；
$\dot{s}_{ref}$ ：参考线上每个参考点自带的目标速度；
第 2 项：横向误差
$n$ ：横向（侧向）误差；
第 3 项：航向误差
$\mu +\beta =\varphi +\beta -\phi$ ：相当于车速方向减去目标航向，即航向误差；

（2）舒适性的代价

$J_{comfort}=w_{j_x}*j_{x}^2+w_{a_x}*a_{x}^2+w_{a_y}*a_{y}^2+w_{\dot{\delta }}*\dot{\delta }^2+w_{\ddot{\delta }}*\ddot{\delta }^2$

第 1 项：加加速度（ jerk ）；
第 2 / 3 项：车辆加速度；
第 4 项：前轮转角角速度；
第 5 项：前轮转角角加速度；

（3）安全性的代价

安全性代价设计思路：

控制器的上层，即规划器，他的主要任务就是根据周围的动态环境来建立一个安全可行范围，也就是说在这个范围内车辆的运动就是安全的，反之超出这个范围车辆的运动就是不安全的；
这个安全行驶范围就是一个约束，所以我们可以按照车辆在实际行驶中对这个约束的违反程度来衡量系统的安全性；

a

对车辆系统存在两种约束条件：

软约束（soft constraint）：可以违反的约束就是软约束；
举例：车辆在某个车道内行驶过程时，是具有一个最优边界的，车辆在这个最优边界内行驶是最好的情况，但是超过也没事，我们的目的就是尽量让车在最优的区域内行驶，这就是软约束；
硬约束（hard constraint）：不能违反的约束就是硬约束；
举例：但是车辆在车道内的行驶绝对不能超出车道边界，这就是硬约束；
注意：对于同一个物理量，它可以既有软约束，也可以有硬约束；对于一个物理量我们可以有一定的违反冗余度（软），但是这个违反的量不能超过一定的限制（硬）；

a

如何在代价函数中表示软约束：

引入松弛变量（slack variables） $\lambda _{k}$ ；
松弛变量是给优化器提供了一个可以违反边界值的程度；
而为了保证对软约束的违反程度比较小，将对松弛变量 $\lambda _{k}$ 的惩罚项引入 $J_{safty}$ ，使得优化器在求解时会尽量对软约束的违反程度较轻；

a

引入了松弛因子这一新变量，原始最优化问题改变为：

注意：松弛变量 $\lambda _{k}$ 也是存在约束范围的；

$J_{safety}=\lambda _{soft}^TE\lambda _{soft}+\bar{H}\lambda _{hard}$

$E$ 和 $\bar{H}$ 均为参数矩阵，里面都是权重系数：
$E=\begin{bmatrix} w_{\lambda _{n,soft}} & 0 & 0\\ 0& w_{\lambda _{v,soft}} & 0\\ 0& 0 & w_{\lambda _{a,soft}} \end{bmatrix}$
$\bar{H}=\left [ w_{\lambda _{n}} ,w_{\lambda _{a}} ,w_{\lambda _{s}} \right ]$
注意1：软约束和硬约束条件都是有对应的松弛因子的；
注意2：每个时间步 k 上使用的同一类型松弛因子的值都是不同的；
软约束对应的松弛变量：
$\lambda_{soft}=\begin{bmatrix} \lambda _{n,soft}\\ \lambda _{v,soft}\\ \lambda _{a,soft} \end{bmatrix}$
选择【横向误差 / 速度 / 加速度】作为软约束；
硬约束对应的松弛变量：
$\lambda_{hard}=\begin{bmatrix} \lambda _{n}\\ \lambda _{a}\\ \lambda _{s} \end{bmatrix}$
选择【横向误差 / 加速度 / 路径长度】作为硬约束；
注意：这里将路径长度 $s$ 作为硬约束，是为了应对前 / 后方有车辆的情况，规划的时候为了防止碰撞，车辆的 $s$ 一定不能是软约束；

（4）代价函数无解 / 不收敛的代价

MPC 控制器本身的限制 -- 两种情况：

第1种：代价函数在约束条件下无解，一般都是因为约束条件苛刻或者具有冲突；
第2种：由于 MPC 控制器设定的有限时域长度 N 太小，导致虽然二次求解库可以求解当前代价函数 J 的极小值解，但是这 N 个时间步并不能使得车辆系统的状态量 x 收敛，及车辆系统无法到达稳定状态；

a

解决思路：

对这 N 个步长的有限时域的终端状态 $x_{N}$ 进行约束和优化；
迫使这个系统在 N 个时间步的预测结束后获得的终端状态 $x_{N}$ ，是在一个特定的集合内（吸引力域 -- safe region）取的值，来保证迭代的稳定性和可行性；

a

注意：代价函数本来是有 N 个 $J_{stage}$ 组成的，加入终端约束后，更改为由 N-1 个 $J_{stage}$ 和一个 $J_{terminal}$ 组成；

$J_{terminal}(x_{N})$

5.2.3 不等式约束和松弛因子

约束条件总结：

对状态量 x 和控制量 u 的约束：
- $x_{min}\leq x\leq x_{max}$
- $u_{min}\leq u \leq u_{max}$
对目标点位置 s 的硬约束：
- $c^{station,hard}(x_{k},\lambda _{s})\leq 0$
对车辆速度 v 的软约束
- $c^{vel,soft}(x_{k},\lambda _{v,soft})\leq 0$
对车辆加速度 a 的硬约束和软约束：
- $c^{a,hard}(x_{k},\lambda _{a})\leq 0$
- $c^{a,soft}(x_{k},\lambda _{a,soft})\leq 0$
对车辆方向盘转角角速度的约束：
- $c^{\dot{\delta }}(x_{k})\leq 0$
对车辆安全可行驶范围 tube 的硬约束和软约束：
- $c^{tube,hard}(x_{k},\lambda _{n})\leq 0$
- $c^{tube,soft}(x_{k},\lambda _{n,soft})\leq 0$

（1）对目标点位置 s 的硬约束

意义：在车辆行驶过程中，如果前方车辆插入我车目前行驶的车道，那么必须保证车辆当前所跟踪的目标点的 s 值不能超过前方障碍物车辆在我们的参考线上所匹配的参考点的 s 值；所以为了防止碰撞，这个约束必须为硬约束；

a

公式：

$s_{k}\leq s_{k}^{max}+\lambda _{s,k}$
$s_{k}\geq s_{k}^{min}-\lambda _{s,k}$
$k\in \left [ 0,...,N \right ]$

得到：

$c_{k}^{station,hard}(x_{k},\lambda _{s,k})=\left\{\begin{matrix} s_{k}-s_{k}^{max}- \lambda _{s,k}\leq 0 \\ -s_{k}+s_{k}^{min}- \lambda _{s,k}\leq 0 \end{matrix}\right.$

（2）对车辆速度 v 的软约束

意义：对车辆的速度是一个软约束，因为很可能出现各种各样的情况导致车辆掉速，掉出当前时间步的理想速度范围 $\left [ v_{k}^{min},v_{k}^{max} \right ]$ ；

a

公式：

$v_{k} \leq v_{k}^{max} + \lambda _{v,soft,k}$

得到：

$c^{vel,soft}_{k}(x_{k},\lambda _{v,soft,k})=v_{k}- v_{k}^{max} - \lambda _{v,soft,k} \leq 0$

（3）对车辆加速度 a 的硬约束和软约束

注意：加速度 a 是横向加速度和纵向加速度耦合起来的总加速度；

硬约束意义：保证车辆的安全性；

在行驶过程中，如果纵向加速度很高，那么我们就要限制横向加速度的大小，否则横向加速度过大会导致车辆打滑；
同理如果行驶时横向加速度很大，比如车辆在进行急转弯的时候，那么我们就要限制纵向加速度的大小，否则车辆会产生侧滑；
一旦车辆发生了侧滑，对车辆的控制作用就会变得特别复杂，因为车辆轮胎的运动特征变成了非线性的；
所以横向加速度和纵向加速度总是一大一小的；我们使用椭圆形的边界来约束横纵向加速度的大小；

a

a

绿色区域：加速度 a 的最优区域（椭圆区域，椭圆公式为 $\frac{x^2}{a^2}+\frac{y^2}{b^2}\leq 1$ ）
蓝色区域：加速度 a 的软约束区域，当加速度 a 在蓝色区域和绿色区域时，轮胎的运动特性为线性的；
红色区域：加速度 a 不可进入这个区域，因为在这个区域，轮胎的运动特性是非线性的，会令车辆的运动难以控制和预测；

a

a

公式：

$\sqrt{\frac{a_{x,k}^2}{a_{x,max,k}^2}+\frac{a_{y,k}^2}{a_{y,max,k}^2}}\leq 1+\lambda _{a,k}$
绿色椭圆区域的范围（软约束范围）： $\sqrt{\frac{a_{x,k}^2}{a_{x,max,k}^2}+\frac{a_{y,k}^2}{a_{y,max,k}^2}}\leq 1$
绿色椭圆的长 / 短边： $a_{x,max,k}$ 和 $a_{y,max,k}$
车辆实际的横 / 纵向加速度： $a_{x,k}$ 和 $a_{y,k}$

得到：

$c_{k}^{a,hard}(x_{k},\lambda _{a,k}) =\sqrt{\frac{a_{x,k}^2}{a_{x,max,k}^2}+\frac{a_{y,k}^2}{a_{y,max,k}^2}}-1-\lambda _{a,k}\leq 0$

软约束意义：保证车辆的舒适性；

a

公式：

$a_{k}\leq a_{comfort,k}+\lambda _{a,soft,k}$
$a_{k}\geq -a_{uncomfort,k} - \lambda _{a,soft,k}$
令乘客舒适的加速度范围： $\left [ a_{uncomfort,k} , a_{comfort,k} \right ]$

得到：

$c_{k}^{a,soft}(x_{k},\lambda _{a,soft,k})=\left\{\begin{matrix} a_{k} - a_{comfort,k} - \lambda _{a,soft,k}\leq 0\\ -a_{k} -a_{uncomfort,k} - \lambda _{a,soft,k}\leq 0\end{matrix}\right.$

（4）对车辆安全可行驶范围 tube 的硬约束和软约束

硬约束意义：防止车辆直接开出道路开到马路牙子上；软约束给出的范围是最优行驶空间，而硬约束给出的范围则是道路边界；下面的4个约束分别针对车辆的四个轮子，使四个轮子全都在马路牙子范围内运动；

a

公式：

$n_{k} + d_{car,left,front}(\mu _{k}) \leq d_{road,left,front,k} + \lambda _{n,k}$
$n_{k} - d_{car,right,front}(\mu _{k}) \geq d_{road,right,front,k} - \lambda _{n,k}$
两前轮所在位置马路牙子的范围： $\left [ d_{road,right,front,k} , d_{road,left,front,k} \right ]$
两前轮相对于车辆质心的侧偏量： $d_{car,left,front}(\mu _{k})$ 和 $d_{car,right,front}(\mu _{k})$
$n_{k} + d_{car,left,rear}(\mu _{k}) \leq d_{road,left,rear,k} + \lambda _{n,k}$
$n_{k} - d_{car,right,rear}(\mu _{k}) \geq d_{road,right,rear,k} - \lambda _{n,k}$
两后轮所在位置马路牙子的范围： $\left [ d_{road,right,rear,k} , d_{road,left,rear,k} \right ]$
两后轮相对于车辆质心的侧偏量： $d_{car,left,rear}(\mu _{k})$ 和 $d_{car,right,rear}(\mu _{k})$

a

得到：

$c_{k}^{tube,hard}=\left\{\begin{matrix} n_{k} + d_{car,left,front}(\mu _{k}) - d_{road,left,front,k} - \lambda _{n,k}\leq 0\\ -n_{k} + d_{car,right,front}(\mu _{k}) + d_{road,right,front,k} - \lambda _{n,k}\leq 0\\ n_{k} + d_{car,left,rear}(\mu _{k}) - d_{road,left,rear,k} - \lambda _{n,k}\leq 0\\-n_{k} + d_{car,right,rear}(\mu _{k}) + d_{road,right,rear,k} - \lambda _{n,k}\leq 0 \end{matrix}\right.$

软约束意义：令车辆在最优的安全行驶空间中运行；

5.3 总结

已知：

车辆系统的非线性状态空间模型；
代价函数；
车辆系统的各种约束条件；

得到：我们需要进行二次规划求解的问题

目标函数：

$min_{U=[u_{k},...,u_{k+N-1}]}J(x_{k},U)=\sum_{k=0}^{N-1}J_{stage}(x_{k},u_{k},\lambda _{k})+J_{terminal}(x_{N})$

a

a

限制条件：

状态空间方程： $x_{k+1}=f(x_{k},u_{k})$
不等式约束： $c_{k}(x_{k},u_{k},\lambda _{k})\leq 0$
物理量约束：
$x\in\chi$
$u\in U$
$\lambda \in \varsigma$

5.4 主流二次规划求解器与 MPC 加速

对于不同的系统，有不同的解法：

Linear NonConstraint MPC：一定存在解析解，使用 DP 求解
Linear / NonLinear Constraint MPC：使用 QP 求解
- 常用求解库（Linear）：OSQP，qpOASES，FORCEs
- 常用求解器（NonLinear）：Codegen，acados

5.4.1 显性 MPC（Explicit MPC）

求解 MPC 控制器的问题其实就是一个 QP 问题，但是 QP 问题的求解复杂度会随着【状态量 x 分量的个数】+【约束条件的个数】+【有限时域的长度】的增大而增大；

而在自动驾驶领域，我们要求控制器对车辆输出控制命令的频率要求很高，即车辆系统将状态量返回给控制器状态量的频率很高，对车辆系统状态量的采样时间间隔大概为几毫秒一次，因为要保证控制命令的实时性；

但是车载内存芯片大小有限，因此算力是有限的，如果 QP 问题过于复杂而且，而且每个几毫秒就要求解一次，那么 MPC 控制器这么大的计算量是不现实的；所以我们要减少 MPC 消耗的算力；

解决方法：

简化模型（减小模型阶数，简化模型参数）
减小有限预测时域
减少约束条件
降低精确度要求
Expilct MPC

Expilcit MPC 的原理：

将一部分优化问题提前计算好，然后做成表存储起来，当 MPC 需要计算时，直接查找表里面的最优值即可；就是将 online 的问题变成了 offline 的问题；

具体实现：

制表（offline）：按照初始状态 $x_{k}$ 的范围，细分为不同的小区间，然后对每个区间都提前计算其对应的 QP 问题的解函数 $f(x_{k})$ ，然后做成表格；
查表（online）：当真正使用的时候，车辆系统将当前时间步上测得的状态量 x 输入 MPC 控制器，然后 MPC 控制器去查表，得到应该使用哪个解函数，就可以快速得到对应的最优解；

离线求解方法：KKT 条件

Karush-Kuhn-Tucker （KKT）条件是非线性规划最佳解的必要条件
文章链接：https://zhuanlan.zhihu.com/p/38163970

a

a

非线性规划问题：

$f(x)/g(x)/h(x)$ 都可以是非线性的；

定义 Lagrangian 函数：

$\lambda$ 和 $\mu$ 都是拉格朗日乘子

4 个 KKT 条件：

a

a

关于非线性规划的解的形式：

对于一个严格的多参数凸优化问题，他的解一定是连续的，解的形式一定是线性的；