概要：本文主要学习基于MPC的车辆控制和轨迹规划

文章目录

前言
一、MPC概念
二、MPC formulation
总结

前言

本文将对基于MPC的车辆控制和轨迹规划进行研究。

一、MPC概念

模型预测控制（Model Predictive Control, MPC）
滚动时域控制（Moving Horizon Control，MHC）
后退时域控制（Receding Horizon Control，RHC）
动态矩阵控制（Dynamic Matrix Control，DMC）
广义预测控制（Generalized Predictive Control，GPC）

工作机理：
在这里插入图片描述
如图所示，在每一个采样时刻K，获得当前的测量信息，并且在线求解一个有限时域horizon下的优化问题，并将得到的控制序列的第一个值作用于被控对象，在下一个采样时刻，重复上述过程，用新的测量值刷新优化问题并重新求解。
在线不断迭代求解的优化问题获得优化序列是MPC模型预测控制和传统控制方法的主要区别，因为传统的控制方法通常是通过离线求解一个反馈控制率，并将反馈控制律一直作用于系统。

MPC起源于工业界，主要为了解决PID控制多变量、有约束的优化控制问题。

1.1 控制约束

在现实控制系统中存在各种各样的约束，比如：

执行器的饱和约束，基于机械结构的最大执行效率；
性能约束，环境变量不能超过阈值；
安全约束，温度、压力等不能超过阈值；

研究发现，最优的控制量往往接近约束边界，所以为了处理约束：

经典控制理论：
（1）添加临时约束Ad hoc限制临界值在某一范围内；
（2）设置远离约束的足够的点；
（3）次优化方法，Suaboptimal plant operation。
经典控制理论没有系统化地解决问题，而MPC则是处理基于约束的控制系统最有效的方法。
MPC
（1）约束包含在设计中；
（2）Set point optimal；
（3）Optimal plant operation。

例：
在这里插入图片描述
目标：最小化单圈时间，也就是越快越好；
约束：
（1）不能撞到车辆，Avoid other cars；
（2）在路面上；
（3）不能打滑；
（4）加速度限制。

引入MPC，在一段时域内，通过观测到的环境信息（车辆状态、周围环境）来求解优化，并得到一个控制序列，将控制序列的第一个元素应用到被控系统，进行不断迭代和优化。

1.2 基本原理

在这里插入图片描述
如图所示：
在K时刻的测量量是y(k)，与此同时，还有预测系统未来状态的模型，假设用状态空间模型为例：
其中，xk+1为状态量，uk为输入量、yk为输出量，和状态量息息相关，线性系统中yk=cxk。
这样就可以基于模型预测出起始于yk未来一段时间的输出yp，p为预测时域，可以表示为：

在这里插入图片描述
也就是在当前时刻k预测出k+1，k+2时刻的输出，以此类推，可以构建出需要优化的控制量Uk：

跟踪的参考输入rk：

构建的目标函数就是输出量和期望跟踪的参考量很接近：

如图所示，就是预测输出量和参考量的面积越小越好，这样就可以用累积误差定义一个最简单的优化目标函数。
在这里插入图片描述

在uk时刻，已经得到了rk和yk，需要根据其计算出uk，所有会有延迟的过程，uk往往是最后计算出来，是延续到k+1时刻才会才会基于rk+1和yk+1进行更新。

1.3 MPC解析

寻找最佳控制输入的问题就可以描述成给定uk，基于yk和U最小化目标函数J的问题。
在这里插入图片描述
满足系统动力学方程：

控制和输出约束：

其中，y代表了可以测量的量。

可以把优化解写成当前k时刻下每个时刻的优化解：

在这里插入图片描述
也可以写成和测量值yk相关的函数：

然而测量值yk即反馈信息往往会受到外部的干扰，它会比状态控制中测量的值更加精准，所以运用测量值会能够捕捉到外部干扰和模型误差的一些信息，如果用一次反馈得到的优化解全部应用到系统里面，那么外部干扰和模型误差的一些信息得不到有效利用，会导致系统的性能变坏甚至系统失稳；而且预测的时域是有限的，只会在p时域中进行控制，无法支持控制系统的长期运作的。
所以，控制输入只在对应时刻进行作用，在下一时刻会进行更新迭代，如uk只在k时刻内进行作用，而在k+1时刻会利用该时刻的测量值yk+1进行更新迭代重新计算uk+1，并且一直滚动迭代优化也就是滚动时域控制的由来。

1.3 MPC模型算法步骤

预测系统未来的动态：
求解优化问题的解Uk:
将优化的第一个元素应用到系统中：

1.4 算法描述

可以用控制框图来描述MPC核心算法：
在这里插入图片描述
如图所示，该框图分为模型和优化两个模块：
（1）模型：上一时刻的输入状态uk-1带入模型得到yk，yk+2…yk+p；
（2）优化：将y作为反馈项，和参考项r进行比较，得到损失函数的误差值；
（3）利用损失函数和约束进行优化，计算得到优化的u*，并且将u的第一个控制元素uk|k应用到模型中进行控制；
（4）不断反复迭代更新。

二、MPC formulation

2.1 线性MPC无约束

线性系统空间方程表达式：

根据状态方程可以推出：

可以写成矩阵的形式：
目标函数

回顾：LQR
在这里插入图片描述
它是解决从0到无穷大的优化问题。

MPC是在有限的时域内进行优化，引入了后退机制，即在x1时相对应的是u0：
在这里插入图片描述
由图可知，该目标函数为两部分，一部分是新引入的XN，一部分是二次型（范围有限制在0，N-1），而这两部分都是对称正定矩阵保证可以求出解。
将上述目标函数展开：

目标就变成寻找最优序列使其目标函数最小化。

状态量和输入的完整表达式：
在这里插入图片描述
目标函数完整表达式：（寻找u和x的关系）

结合1和2式：

其中，H项为二次型，F项为一次项，E项为常数项（X为常数），

将其目标函数进行求导令其梯度等于0，
在这里插入图片描述
通过上式可以发现，最优的控制量和初始状态成线性关系，所以，无约束的线性MPC就是一个线性反馈：

2.2 最优解求解方法

上述求解由于H矩阵是稠密矩阵，不好解，所以需要将H矩阵稀疏化：

将未来的输入和状态拼成一个列：
在这里插入图片描述

这样就可以将目标函数转换成二次型的对角矩阵。这样求解比稠密矩阵较容易。但是这种解法会引入新的约束，对于在无约束的MPC控制不友好，所以这种方法在无约束的MPC中是不会使用的。在有约束的MPC控制中却使用较广泛。

最优化原理（principle of optimality）
最优策略的子策略也是最优的，它是动态规划的基础。

在这里插入图片描述

无论Uk是如何达到的，从Uk以后仍然是最优解，可以从后往前推。
（1）利用动态规划的思想将问题拆解成一系列子问题来进行递归求解
第N个时刻：

第N-1时刻的cost就是N-1时刻的cost加上N时刻的cost：

代入上式可知：在这里插入图片描述
其中，第一个部分是N-1时刻的损失函数，第二部分是N时刻也就是到达cost。
上式代入得：

假设XN-1是已知的，则前一部分是常量，后一部分只与UN-1有关，则JN-1只与UN-1有关，所以可以对上式求导得到最优解：
在这里插入图片描述

将UN-1代入：

可得N-1如下的二次型：

N-2时刻：

离散的raccti方程

不断更新调整P值，也就是从N时刻开始不断往后倒退，倒退到K时刻计算完之后，和初始状态量相乘得到第一步的最优控制。
在这里插入图片描述

2.3 总结

动态规划的思想比稠密矩阵求解速度快；

在这里插入图片描述

（1）LQR其实是一种特殊的MPC控制方法，LQR计算的是一个P的最优解，是针对一个无限时域进行的优化控制，没有约束，线性二次调节器；
（2）MPC是不断更新P值最后得到u0，它是针对有限时域进行优化的问题，有约束，处理线性和非线性系统，支持不同种类的目标函数；
在这里插入图片描述

2.4 线性MPC有约束

实际情况中，系统的输入和状态都是有合理的范围，可以把线性不等式的约束问题转换成QP（Quadratic Programming）二次规划问题（凸优化方法，针对线性问题进行优化）。

QP形式如下：
在这里插入图片描述
一般来说，Z是一阶变量，适用于避障和状态量约束。
QP方法：

2.5 实现流程

线性MPC有约束实现流程如下：
在这里插入图片描述

初始化：首先将模型中的状态和输入进行初始化；
构建优化问题：基于参考轨迹、约束和测量反馈构建优化问题；
问题转换：将优化问题转变为QP二次规划问题；
问题求解：求解QP问题；
求解判断：
（1）如果能够找到解，则得到输入信号的增益，计算得到应用到系统的最优输入量，并且不断迭代重复该过程；
（2）如果无解，控制器就会报错，引入工程方法如启用另一套系统/控制器来保证的安全性。

2.6 总结

有约束的模型预测控制的算法其实是在线求解有不等式约束的二次规划问题，也就是QP问题。QP问题是典型的数值优化问题，它的优化目标就是二次型函数并且带有线性约束；其常见求解方法如下：

积极集法（Active-set methods）

适用于不等式约束的二次规划问题，；
主要原理是沿着约束边缘向下搜索，直到下方没有可行的下降方向为止。所以，在可行区域要有一个初始可行点。
积极集法，变动比较频繁，迭代步数不是很稳定，求解过程相对比较复杂，计算量非常大。
2. 内点法（Interior-point methods）
在这里插入图片描述

适用于任何形式的二次规划问题，（等式约束、不等式约束、变量上下值的约束）
主要原理是把所有满足约束的集合设置一个可行域，可行域的点叫做内点，核心思想就是从一个内点出发找到一个可行的下降方向，使内点沿着方向移动，能够使得目标函数下降，不断重复上述过程，满足迭代终止条件为止，得到的点就是最优点。
总结：内点法是优化问题中很核心的算法，现实中非常实用，计算复杂度低，有着基于内点法的很多变种，惩罚函数内点算法等。

尽管线性化模型广泛应用在控制系统设计中，但大部分实际系统存在着非线性的特性，随着对于控制精度的提高，需要了解解决对于非线性的控制跟踪，目前业内有成熟的非线性模型，不需要求解但是需要知道如何构建目标函数，如何选取模型，如何设置约束条件，如何合理控制状态和输入。