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

阅读量：

①前言

②车辆的运动学方程

③线性化过程

④离散化

⑤推导下一时刻状态

⑥迭代推导

⑦目标函数优化

⑧约束条件

⑨上下限约束

⑩学习遗留问题

①前言

本文主要参考了下面的博客

基于线性时变系统的模型预测控制研究中，在推导过程部分以自行车模型为例进行说明：其中下标 r 表示 reference（参考）意义。

该文详细阐述了基于线性时变系统的模型预测控制策略推导过程，并深入分析了其理论基础与应用前景

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

在编写代码时所涉及的推导细节上给予了更为具体的说明，并以此帮助自己更深入地理解相关内容~这算是对自己学习过程的一种记录。好的,下面正式开始吧!

模型预测控制的核心体现在通过构建基于状态误差的状态空间模型来进行推导。在第4章中,主要采用的是将非线性系统线性化的方法中的一种:即引入参考系统进行处理。

②车辆的运动学方程

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

即：

$\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}$ 来表示~~~

③线性化过程

上面的运动学方程是非线性的因此需要对其进行线性化处理对于在参考轨迹上的各个点而言在这些点处展开泰勒级数并且忽略了高次项从而仅保留了一阶近似

将（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$ 矩阵的大小为：；分别是状态量个数和控制量个数。

⑤推导下一时刻状态

ps⑥: 1. 在矩阵A中观察到一个零矩阵其维度大小为 $(\textit{Nu}, \textit{Nx})$

这个矩阵的维度结构是 $(\textit{Nx} + \textit{Nu}, \textit{Nx} + \textit{Nu})$ , 具体来说，在第四章分析时我们得到结果是 $(5,5)$

B矩阵的具体维度安排是 $(\textit{Nx} + \textit{Nu}, \textit{Nu})$ , 在第四章分析时我们发现这里对应的是

；**_

_**4.

矩阵的大小为：**_ 。

输出为：

ps⑦: 1. 上面所述 $I_{Nx}$ 矩阵其规模可能并非固定值（取决于输出状态量的数量），因此我们定义输出状态量的数量为 $N_{y}$ 。特别需要注意的是，在本节中提到的"输出数量"与代码中初始化子函数所定义的不同——这里的"输出数量"特指状态量方面的输出数量，并满足关系式 $N_{y}\leq N_{x}$ 。而在第四章中我们采用的是等式关系即 $N_{y}= N_{x}$ 。

2. 在上文中提到的C 矩阵尺寸为 $(N_{y}, N_{u})$ " ，可表示为 $C \in \mathbb{R}^{(N_{y}, N_{u})}$ ；那么 $\psi(\cdot)$ 矩阵尺寸则变为 $(N_{y}, N_{x}+N_{u})$ " ，即 $\psi(\cdot) \in \mathbb{R}^{(N_{x}, N_{x}+N_{u})}$ ；而 $\eta$ " 矩阵尺寸则进一步确定为此处使用的特定参数空间维度 $d_\eta= (d_x + d_u)^2 + (d_x + d_u) d_z + d_z^2 + 1$ .

采用这种方式进行表示。为了简化书写，在后续讨论中我们将直接使用符号 $N_{x}$ 来表示相关概念。如需进一步了解后续章节中各矩阵的具体定义和性质，请参阅本论文的第五章。

⑥迭代推导

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

新的状态量：

输出：

_**ps⑧:因为矩阵的大小为： ,矩阵的大小为：, 矩阵的大小为：。所以

矩阵的大小为：， $CA^{n}$ 矩阵的大小为：， $CA^{n}B$ 矩阵的大小为：。**_

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

其中，

ps⑨:1.以下标有" $\_cell$ "标记的矩阵均指代代码中所使用的元胞单元；

_**2. $\Psi\_cell$ 类型的矩阵具有维度 $(N_p, 1)$ ；而总的 $\Psi$ 矩阵则具有维度 $(N_p \times N_x, N_x \times N_u)$ 。相应地， $\Psi\_cell^{\top}$ 的维度同样也是 $(N_p, 1)$ 。基于此可推导出 $\Theta\_cell^{\top}$ 矩阵具有维度 $(n_c, n_u)$ ；进一步分析可知其具体形式与 $Y^{\top}$ 相关联并最终确定其最终维度应为 $(n_x \times n_p, n_c \times n_u)$ 。各相关子矩阵的具体尺寸分别为 $(...,...)$ 和 $(...,...)$ 。