《无人驾驶车辆模型预测控制》（第2版）第五章主动转向详细学习——算法部分（非线性系统线性化）
一、状态轨迹的线性化方法
1.理解：
第四章中的期望轨迹包含了路径上的各个关键点的状态信息（如状态变量与控制变量）。这类轨迹具有一定的规律性（如直线运动与圆周运动）。由此可见，在这种情况下能够获得相应的控制参数较为便捷。针对那些无法全面提供所有状态点信息的情况，则建议采用以下所述的方法。
2.算法详细推导
第五章采用的算法是通过对系统输入持续不变的控制量得到状态轨迹。
目录
①前言
②车辆的运动学方程
③线性化过程
④离散化
⑤推导下一时刻状态
⑥迭代推导
⑦目标函数优化
⑧约束条件
①前言
本文主要参考了下面的博客
基于线性时变系统的模型预测控制研究_Jeossirey的博客-博客_线性时变系统的模型预测控制以自行车系统为例，在推导过程中使用了subscript r（reference）来表示参考信息的标记。
以及：龚建伟等.《无人驾驶车辆模型预测控制》（第2版）
同时在进行具体的代码实现过程中涉及的推导细节进行了更加细致地说明了这一部分内容也可以说是对自己学习过程的一个总结或记录好了准备开始正式的学习和实践了
模型预测控制的核心思想是通过构建基于状态量的偏差变量的状态空间模型来推导控制规律。第四章主要基于车辆运动学方程展开讨论，并运用了非线性系统线性化方法中的第一种技术：即在存在参考速度反馈条件下的动态补偿线性化方法。
②车辆的运动学方程
车辆的运动学方程推导过程略，结果如下：
即：
·····························（1.2 ）
选取系统的状态量为
,控制量
基本思路：基于设定为参考轨迹，在所有时刻系统状态变量与控制变量都符合要求
·····························（1.3 ）
其中，
，
。
**ps①: 1.**
_是状态转移函数，就理解为数学中表示函数的符号就可以了；_
**2.**
_为车辆后轴中心坐标， _
_为车辆的航向角， _
_为前轮偏角， _
_为车辆后轴中心 的速度；_
_3.在该车辆模型中，状态量个数是3个，用 _
_表示；控制量个数是2个，用 _
_表示，在之后的推导过程的说明中，就直接用 _
_来表示~~~_
③线性化过程
上面的运动学方程
是非线性的，所以需要对其进行线性化。对
在参考轨迹点采用泰勒级数展开并忽略高阶项，只保留一阶项：
将（1.4）式减去（1.3）式，得：
记：
，
，
。
**ps②:**
_、 _
_、 _
_矩阵的大小均为： _
_， _
_、 _
_、 _
_矩阵的大小均为： _
_。_
因此上式可以写成：
其中：
ps③:此运动学模型因为较为简单，故其雅克比矩阵手算其实更为方便，但当遇到比较复杂 的动力学模型时，需借助matlab进行求解，具体可见《无人驾驶车辆模型预测控制》 第五章— > 。
因而线性化后的模型可以写成：
④离散化
对上式采用向前欧拉法进行离散化：
其中，
为系统采样时间。
然后得：
**ps④:**
_矩阵的大小： _
_； _
_矩阵的大小： _
_。_
构建一个新的状态量（将原有的状态量偏差与控制量偏差进行融合）
_ps⑤:新构造的状态量 _
_矩阵的大小为： _
_； _
_分别是状态量个数和控制量个 数。_
⑤推导下一时刻状态
_ps⑥: 1.上面 _
_矩阵中的 _
_矩阵的大小为： _
_；_
**2.**
_矩阵的大小为： _
_，在第四章中即为 _
_；_
**3.**
_矩阵的大小为： _
_，在第四章中即为 _
_；_
**4.**
_矩阵的大小为： _
__。
输出为：
_ps⑦: 1. 上面的 _
_矩阵的大小并不一定是 _
_，这要看输出状态量的个数，我们将 输出量的个数定义为 _
_。请注意：此处分析的是与代码中使用的一个特定初始化函数产生的输出数量不相同！ _
_指的是“状态量”中的输出个数，也就是说 _
_。而在第四章中 _
_。_
_2. 所以上面 _
_矩阵中的 _
_矩阵的大小为： _
_，写成 _
_；那么 _
___矩阵的大小为： _
_；写成 _
_； _
_矩阵的大小为： _(N__{y},1)_ ，写成 _
_。为了方便书写我们在之后的部分直接用 _
_代替 _
_。如若要深入了解后续各矩阵的具体尺寸，则需深入研究后续章节的相关内容：————————————
⑥迭代推导
控制时域
，预测时域
，且
。
新的状态量：
输出：
_ps⑧:因为 _
_矩阵的大小为： _
,
_矩阵的大小为： _
,
_矩阵的大小为： _
_。所以 _
_矩阵的大小为： _
_， _
_矩阵 的大小为： _
_， _
_矩阵的大小为： _
_。_
总结规律可得输出方程为：
其中，
**ps⑨:1.以下带"**
_"结尾的矩阵指的是代码中的元胞；_
**2.**
_矩阵大小为： _
_， _
_矩阵大小为： _
_， _
_矩阵 的大小为 _
_， _
_矩阵的大小为： _
_。 _
_矩阵大小为： _
_，又因 _
_矩阵的大小为： _
_，所以 _
_矩阵的大小为： _
_,且其中每个 _
_矩阵的大小为： _
_，(同 _
_一样)， _
_矩阵的大小为： _
_， _
_矩阵的大小为： _
_。_
⑦目标函数优化
选择目标函数为：
并定义系统输出量的参考值为：
令
，将式（1.14）代入目标函数式（1.15）中，有：
上面方框中的部分与
无关，属于常量，可不加入优化。
_ps⑩: 上方蓝色方框中的蓝色字体部分的之所以可以这么合并，是因为， _
_矩阵和 _
_矩阵是同型矩阵， _
_和 _
_都是常数，（根据前面推导的 _
_、 _
_、 _
_矩阵的大小即可推导）因而可以合并。_
故目标函数简化为：
ps⑪:同理，上面第一个方框中的两项也是常数，故可以合并！
故性能评价函数为：
_ps⑫: 1.以上的推导过程中，因为： _
_矩阵的大小为： _
_， _
_矩阵的大小为 _
_； _
_矩阵的大小为： _
_， _
_矩阵的大小为： _
_。所以 _
_元胞的大小为： _
_， _
_矩阵大小为： _
_； _
_元胞的大小为： _
_， _
_矩阵的大小为 _
_。_
2.Matlab中二次规划quadprog的标准形式为：
将性能评价函数式(1.16)进行配凑得到：
_ps⑬: 1.因为 _
_矩阵大小为： _
_， _
_矩阵的大小为： _
_， _
_元胞的大小为： _
_， _
_矩阵大小为： _
_。所以 _
_矩阵的大小为： _
_。 _
_矩阵中右上角的 _
_矩阵大小为： _
_，左下角的 _
_矩阵的大小为： _
_。_
⑧约束条件
ps⑭: 1.
元胞的大小为：
，
矩阵的大小为：
；
元胞的大小为：
，
矩阵的大小为：
；
元胞的大小为：
，
矩阵的大小为：
。
**ps⑮:1.**
_元胞的大小为： _
_， _
_矩阵的大小为： _
;
_同！_
_ps⑯:上面两个 _
_元胞的大小为： _
_， _
_矩阵的大小为： _
_。_
⑨上下限约束
**ps⑰:**
_同 _
_~~~_