《无人驾驶车辆模型预测控制》（第二版）第四章详细学习——算法部分

①前言

②车辆的运动学方程

③线性化过程

①前言

本文主要参考了下面的博客线性时变模型预测控制推导_Jeossirey的博客-CSDN博客_线性时变系统的模型预测控制以自行车模型为例，在推导过程中下角标 r （reference）,即参考的意思。https://blog.csdn.net/m0_50888394/article/details/115556185

以及：龚建伟等.《无人驾驶车辆模型预测控制》（第2版）

同时对自己在代码实现时需要用到的推导细节给出了更加详细的说明，以帮助自己更好的理解~算是自己学习过程的记录。好了，下面就正式开始吧！

模型预测控制的核心是通过建立基于状态量的误差量的状态空间方程来推导。第四章通过车辆的运动学方程，采用了非线性系统线性化方法中的第一种：存在参考系统的线性化方法。

②车辆的运动学方程

车辆的运动学方程推导过程略，结果如下：

即：

$\dot{\chi }=f(\chi ,u)$ ·····························（1.2）

$f_{1}=\dot{\chi }_{1}=f_{1}(\chi ,u)$

$f_{2}=\dot{\chi }_{2}=f_{2}(\chi ,u)$

$f_{3}=\dot{\chi }_{3}=f_{3}(\chi ,u)$

选取系统的状态量为 $\chi =[x,y,\varphi ]^{T}$ ,控制量 $u=[v,\delta ]^{T}$

主要思想：假设参考轨迹给定，且参考轨迹在任意时刻其状态量和控制量均满足：

$\dot{\chi }_{r}=f(\chi_{r} ,u_{r})$ ·····························（1.3）

其中， $\chi_{r} =[x_{r},y_{r},\varphi _{r}]^{T}$ ， $u_{r}=[v_{r},\delta_{r} ]^{T}$ 。

ps①: 1. $f$ 是状态转移函数，就理解为数学中表示函数的符号就可以了；
2. $(x,y)$ 为车辆后轴中心坐标， $\varphi$ 为车辆的航向角， $\delta$ 为前轮偏角， $v$ 为车辆后轴中心的速度；
3.在该车辆模型中，状态量个数是3个，用 $N_{x}=3$ 表示；控制量个数是2个，用 $N_{u}=2$ 表示，在之后的推导过程的说明中，就直接用 $N_{x},N_{u}$ 来表示~~~

③线性化过程

上面的运动学方程 $\dot{\chi }=f(\chi ,u)$ 是非线性的，所以需要对其进行线性化。对 $\dot{\chi }=f(\chi ,u)$ 在参考轨迹点采用泰勒级数展开并忽略高阶项，只保留一阶项：

将（1.4）式减去（1.3）式，得：

记： $\dot{\tilde{\chi }}=\dot{\chi }-\dot{\chi _{r}}$ ， $\tilde{\chi }=\chi -\chi _{r}$ ， $\tilde{u}=u-u_{r}$ 。

ps②: $\dot{\tilde{\chi }}$ 、 $\tilde{\chi }$ 、 $\dot{\chi _{r}}$ 矩阵的大小均为： $(N_{x},1)$ ， $\tilde{u}$ 、 $u$ 、 $u_{r}$ 矩阵的大小均为： $(N_{u},1)$ 。

因此上式可以写成：

其中：

ps③:此运动学模型因为较为简单，故其雅克比矩阵手算其实更为方便，但当遇到比较复杂的动力学模型时，需借助matlab进行求解，具体可见《无人驾驶车辆模型预测控制》 第五章—> 。

因而线性化后的模型可以写成：

④离散化

对上式采用向前欧拉法进行离散化：

其中， $T$ 为系统采样时间。

然后得：

ps④: $a$ 矩阵的大小： $(N_{x},N_{x})$ ； $b$ 矩阵的大小： $(N_{x},N_{u})$ 。

接下来构造一个新的状态量（把原来的状态量偏差和控制量偏差整合到一起）：

ps⑤:新构造的状态量 $\xi$ 矩阵的大小为： $(N_{x},N_{u})$ ； $N_{x},N_{u}$ 分别是状态量个数和控制量个数。

⑤推导下一时刻状态

ps⑥: 1.上面 $A$ 矩阵中的 $0$ 矩阵的大小为： $(N_{u},N_{x})$ ；
2. $A$ 矩阵的大小为： $(N_{x}+N_{u},N_{x}+N_{u})$ ，在第四章中即为 $(5, 5)$ ；
3. $B$ 矩阵的大小为： $(N_{x}+N_{u},N_{u})$ ，在第四章中即为 $(5,2)$ ；
4. $\Delta \tilde{u}(k)$ 矩阵的大小为： $(N_{u},1)$ 。

输出为：

ps⑦: 1. 上面的 $I_{Nx}$ 矩阵的大小并不一定是 $(N_{x},N_{x})$ ，这要看输出状态量的个数，我们将输出量的个数定义为 $N_{y}$ 。注意：此处的输出量的个数有别于代码中的初始化子函数中的输出量个数！ $N_{y}$ 指的是“状态量”中的输出个数，也就是说 $N_{y}\leq N_{x}$ 。而在第四章中 $N_{y}= N_{x}$ 。
2. 所以上面 $C$ 矩阵中的 $0$ 矩阵的大小为： $(N_{y},N_{u})$ ，写成 $(N_{x},N_{u})$ ；那么 $C$ 矩阵的大小为： $(N_{y},N_{x}+N_{u})$ ；写成 $(N_{x},N_{x}+N_{u})$ ； $\eta$ 矩阵的大小为： $(N__{y},1)$ ，写成 $(N_{x},1)$ 。为了方便书写我们在之后的部分直接用 $N_{x}$ 代替 $N_{y}$ 。如果想详细知道后面部分各个矩阵的具体大小，可参考我第五章的学习内容：————————————

⑥迭代推导

控制时域 $N_{c}$ ，预测时域 $N_{p}$ ，且 $N_{c}\leqslant N_{p}$ 。

新的状态量：

输出：

ps⑧:因为 $C$ 矩阵的大小为： $(N_{x},N_{x}+N_{u})$ , $A$ 矩阵的大小为： $(N_{x}+N_{u},N_{x}+N_{u})$ , $B$ 矩阵的大小为： $(N_{x}+N_{u},N_{u})$ 。所以 $A^{n}$ 矩阵的大小为： $(N_{x}+N_{u},N_{x}+N_{u})$ ， $CA^{n}$ 矩阵的大小为： $(N_{x},N_{x}+N_{u})$ ， $CA^{n}B$ 矩阵的大小为： $(N_{x},N_{u})$ 。

总结规律可得输出方程为：

其中，

ps⑨:1.以下带" $\_cell$ "结尾的矩阵指的是代码中的元胞；
2. $\Psi \_cell$ 矩阵大小为： $(N_{p},1)$ ， $\Psi$ 矩阵大小为： $(N_{p}*N_{x},N_{x}*N_{u})$ ， $Y\_cell$ 矩阵的大小为 $(N_{p},1)$ ， $Y$ 矩阵的大小为： $(N_{x}*N_{p},1)$ 。 $\Theta\_cell$ 矩阵大小为： $(N_{p},N_{c})$ ，又因 $CA^{n}B$ 矩阵的大小为： $(N_{x},N_{u})$ ，所以 $\Theta$ 矩阵的大小为： $(N_{x}*N_{p},N_{c}*N_{u})$ ,且其中每个 $0$ 矩阵的大小为： $(N_{x},N_{u})$ ，(同 $CA^{n}B$ 一样)， $\Delta U\_cell$ 矩阵的大小为： $(N_{c},1)$ ， $\Delta U$ 矩阵的大小为： $(N_{c}*N_{u},1)$ 。

⑦目标函数优化

选择目标函数为：

并定义系统输出量的参考值为：

令 $E=\Psi \xi (k)$ ，将式（1.14）代入目标函数式（1.15）中，有：

上面方框中的部分与 $\Delta U$ 无关，属于常量，可不加入优化。

ps⑩: 上方蓝色方框中的蓝色字体部分的之所以可以这么合并，是因为， $E$ 矩阵和 $Y_{ref}$ 矩阵是同型矩阵， $E^{T}QY_{ref}$ 和 $Y^{T}_{ref}QE$ 都是常数，（根据前面推导的 $E$ 、 $Y$ 、 $Q$ 矩阵的大小即可推导）因而可以合并。

故目标函数简化为：

ps⑪:同理，上面第一个方框中的两项也是常数，故可以合并！

故性能评价函数为：

ps⑫: 1.以上的推导过程中，因为： $Y$ 矩阵的大小为： $(N_{x}*N_{p},1)$ ， $Y\_cell$ 矩阵的大小为 $(N_{p},1)$ ； $\Delta U$ 矩阵的大小为： $(N_{c}*N_{u},1)$ ， $\Delta U\_cell$ 矩阵的大小为： $(N_{c},1)$ 。所以 $Q\_{cell}$ 元胞的大小为： $(N_{p},N_{p})$ ， $Q$ 矩阵大小为： $(N_{x}* N_{p},N_{x}* N_{p})$ ； $R\_{cell}$ 元胞的大小为： $(N_{c},N_{c})$ ， $R$ 矩阵的大小为 $(N_{c}*N_{u},N_{c}*N_{u})$ 。
2.Matlab中二次规划quadprog的标准形式为：

将性能评价函数式(1.16)进行配凑得到：

ps⑬: 1.因为 $\Theta\_cell$ 矩阵大小为： $(N_{p},N_{c})$ ， $\Theta$ 矩阵的大小为： $(N_{x}*N_{p},N_{c}*N_{u})$ ， $Q\_{cell}$ 元胞的大小为： $(N_{p},N_{p})$ ， $Q$ 矩阵大小为： $(N_{x}* N_{p},N_{x}* N_{p})$ 。所以 $\Theta ^{T}Q\Theta$ 矩阵的大小为： $(N_{c}*N_{u},N_{c}*N_{u})$ 。 $H$ 矩阵中右上角的 $0$ 矩阵大小为： $(N_{c}*N_{u},1)$ ，左下角的 $0$ 矩阵的大小为： $(1,N_{c}*N_{u})$ 。

⑧约束条件

ps⑭: 1. $U_{t}\_{cell}$ 元胞的大小为： $(N_{c},1)$ ， $U_{t}$ 矩阵的大小为： $(N_{c}*N_{u},1)$ ； $A_{I}\_{cell}$ 元胞的大小为： $(N_{c},N_{u})$ ， $A_{I}$ 矩阵的大小为： $(N_{c}*N_{u},N_{c}*N_{u})$ ； $U\_cell$ 元胞的大小为： $(N_{c},1)$ ， $U$ 矩阵的大小为： $(N_{c}*N_{u},1)$ 。