第4章：车辆的横向优化控制

4.1 车辆动力学模型

注1：运动学模型和动力学模型最大的不同点在于

运动学模型是在我们不考虑车辆的受力情况下建立的（回顾我们推导出运动学模型的过程，我们没有使用到任何车辆所受的外力作为公式中的已知量，而是直接通过 “ 车速 + 横摆角 + 前后轮转角 ” 再辅以几何作图而得到的运动学模型）；
动力学模型是在我们考虑了车辆在运动过程中所受外力（主要是轮胎侧向力）的情况下建立的；

注2：为何运动学模型可以不考虑外力对车辆的影响，而动力学模型必须考虑呢？

运动学模型的构建有一个很重要的前提，就是车辆是在 “ 低速 ” 状态下行驶的；而对于在低速状态下行驶的车辆，可以认为车辆和路面之间只有纵向的静摩擦，而没有侧向的动摩擦，即车辆与地面之间的侧向力很小，几乎可以忽略不记，因此不会造成轮胎的形变，进而可以认为轮胎朝向和轮胎速度方向是一致的；
动力学模型则是适用于高速行车条件，高速行车时车辆轮胎与地面之间不仅仅会有很大的纵向的滑动摩擦，还会有横向的滑动摩擦（高速行车但凡打一下方向盘，车轮胎就会因为变向而和地面产生巨大的横向滑动摩擦，导致轮胎强烈的形变），因此会导致轮胎速度方向和轮胎朝向并不相同，那么我们就要针对这种具有不可忽视的外力影响的行车情况再建立一个可以使用的模型，即车辆动力学模型；

4.1.1 车辆的横向动力学模型

注：我们的目的是控制车辆沿着我们期望的轨迹行驶，保证行车轨迹路线满足车辆动力学特性；

前置假设：

车辆行车过程中转角比较小，这样可以保证车辆两前轮 / 两后轮的转角相差不大（由阿克曼几何推出），因此可以将两前轮 / 两后轮合并为一个轮胎来对待，这样我们就可以将车辆模型视为两轮自行车模型；
我们只考虑轮胎所受到的外力；而对于一些非线性的效应，如悬架结构 / 道路倾斜 / 空气动力学所造成的外力，我们选择忽略，虽然这些效应对于作用于轮胎的外力会有一定的影响，但是这样可以大大简化我们的建模任务；
忽略轮胎力的横纵向耦合关系；轮胎的横向力和纵向力是相互作用的，但是我们通常会假设轮胎力的横纵向是解耦过的（即互不影响，各自只影响各自方向上的运动），这样就可以将车辆的动力学模型分为横向动力学模型 + 纵向动力学模型，就可以更加简单清楚的描述车辆动力学特性；

a

a

注意：在本小节中推导横向动力学模型时，使用的是车身坐标系；但是车身坐标系无法真正意义上实现横纵向控制解耦，所以我们还需要把在车身坐标系中建立的横向动力学模型转换到自然坐标系中去，只有在 Frenet 才能真正实现横纵向控制解耦；

横向动力学模型推导过程：

注1：在车身坐标系中进行分析，车身坐标系是一个右手系；

注2：以车辆质心作为参考点（坐标系原点）；

a

a

a

a

第1步：车辆的横向（侧向）动力学方程组

注：针对车辆整体进行受力分析，建立整车的横向动力学模型；

横向动力学方程组

注：我们可以发现，这个动力学模型是由两部分组成的，沿着y轴方向的平移运动，和沿着z轴方向的横摆运动；因此这个模型具有两个自由度，所以我们也称这个横向动力学模型是一个二自由度模型；

a

a

a

a

第2步：求出公式中的未知量 ${F_{yf}}'$ 和 ${F_{yr}}'$

注1：接下来要完善横向动力学方程组，即将里面的未知量 ${F_{yf}}'$ 和 ${F_{yr}}'$ 消去，要计算这两个外力，就要针对轮胎的受力情况进行分析；这个时候我们就需要精细化的建模，不能再在整车上进行受力分析了，而是要从自行车模型上的前后轮胎上进行受力分析；

注2：在轮胎上力也分为纵向力和横向力；纵向力实现的是驱动和制动，而横向力则用来给车辆提供转向力并且保证行驶的稳定性；

注3：横向力一般是由于路面的侧向倾斜 / 侧向风 / 曲线行驶时产生的离心力而产生的；

注4：使用字母 $r$ 代替角速度 $\dot{\varphi }$ （ $r$ 并不代表任何半径，就是一个字母符号）

a

$\delta$ ：前轮(朝向)转角（没有后轮转角，是因为认为后轮转角方向与车身纵轴方向一致，因为使用前轮控制转向而不是使用后轮，所以后轮转角为0）
$\theta _{vf}$ ：前轮速度转角（因为高速行车时轮胎速度和轮胎朝向之间有夹角，方向不一致）
$\theta _{vr}$ ：后轮速度转角
$\alpha _{f}$ ：前轮侧偏角
$F_{xf}$ ：前轮纵向力
$F_{yf}$ ：前轮横向力
$F_{xr}$ ：后轮纵向力
$F_{yr}$ ：后轮横向力
$v$ ：车辆(质心)速度
$v_f$ ：前轮速度
$v_r$ ：后轮速度

即有横向动力学方程组表达式变为：

a

a

a

a

第3步：求出公式中的未知量 $F_{yf}$ 和 $F_{yr}$ 表达式

以前轮为例，要求施加在前轮上的横向力 $F_{yf}$ 的表达式，则通过上图（通过实验测出的，横轴为前轮侧偏角 $\alpha _f$ ，纵轴为前轮横向力 $F_{yf}$ ）可知，当前轮侧偏角很小的时候，前轮横向力 $F_{yf}$ 的大小与前轮侧偏角 $\alpha _f$ 之间的关系近似于正比例函数，而在车辆正常行驶的过程中车辆的前轮侧偏角是很小的，所以我们可以认为：

$c_{f} / c_{r}$ ：前轮 / 后轮侧偏刚度（取图中轮胎侧偏角为0时曲线的斜率）
注意1：后轮转角（即后轮朝向角）与车辆纵轴方向相同，因此在车辆坐标系中 $\delta _r = 0$
注意2：有其他的博客推导出的公式是乘上了2，是因为前轮 / 后轮都是两个，所以求的时候两个轮胎的受力都要算进去；但是我们的这个分析中的力 $F_{yf}$ 和 $F_{yr}$ 指的是两前轮 / 两后轮的侧向力的合力，所以不用乘2；

因此当前公式中的未知量为 $\theta _{vf}$ 和 $\theta _{vr}$ ；我们需要将他们表示出来；

a

a

a

a

第4步：求出公式中的未知量 $\theta _{vf}$ 和 $\theta _{vr}$ 表达式

注意1：这是在车身坐标系（右手系）下进行的分析；车辆朝向方向为x轴；

注意2：图中涉及到的速度都是标量（不带方向，即不带正负号，只有大小）；

注意3： $r=\dot{\varphi }$ ，即 $r$ 代表的是车辆的角速度；

a

a

a

a

a

第5步：将【 $\theta _{vf}$ 和 $\theta _{vr}$ 】的表达式代回【 $F_{yf}$ 和 $F_{yr}$ 】的表达式

a

a

a

a

第6步：将【 $F_{yf}$ 和 $F_{yr}$ 】的表达式代回【 ${F_{yf}}'$ 和 ${F_{yr}}'$ 】的表达式

注： ${F_{yf}}'$ 和 ${F_{yr}}'$ 的表达式即为车辆的横向动力学方程组

该方程组即为横向动力学方程组

代入后获得真正的横向动力学模型

a

a

a

a

第7步：化简横向动力学模型

a

（1）得到非线性横向动力学方程组：

a

（2）由于公式中包含了很多非线性的参数（三角函数部分），所以将公式中的三角函数部分进行化简，得到线性的横向动力学方程组：

注意：其中的未知量 $F_{xf}$ 会在这一步消去，所以在上面就根本不用求这个量的表达式；

a

（3）将横向动力学方程组转化为状态方程，选取状态 $x = \binom{v_y}{\dot{\varphi }} = \binom{v_y}{r}$ ：

a

a

a

a

第8步：总结

注意：这样我们就在车身坐标系中建立起来了以【车辆横向速度 $v_y$ 】和【车辆角速度（横摆角速度） $\dot{\varphi }$ 】为状态，以【前轮转向角 $\delta$ 】为输入的线性横向动力学模型的状态空间方程描述；

a

a

a

a

补充：还可以选择其他的状态x，建立另一种形式的横向动力学模型

注意：基于全局坐标系

$y$ ：车辆在Y轴上的坐标
$\varphi$ ：车辆横摆角（车辆航向）

4.1.2 横向动力学模型中一些参数的估算方法

关于质心位置的估算：（即估算 $l_f$ 和 $l_r$ 的值）

a

注：在车辆的四个轮子下面放四个秤，分别测量车辆在行驶过程中每个轮子上的压力值；

$m_{fl}$ ：左前轮压力
$m_{fr}$ ：右前轮压力
$m_{f}=m_{fl}+m_{fr}$ ：前轮总压力
$m_{rl}$ ：左后轮压力
$m_{rr}$ ：右后轮压力
$m_r=m_{rl}+m_{rr}$ ：后轮总压力
$m = m_{fl} + m_{fr} + m_{rl} + m_{rr}$ ：车身总压力（总重量）
$L$ ：纵向轴距

关于车辆的转动惯量 $I_z$ 的估算：

a

关于侧偏刚度 $c_{f}$ 和 $c_{r}$ 的估算：

a

a

（1）如果可以直接从车厂拿到轮胎横向力 $F_{yf} / F_{yr}$ 和轮胎侧偏角 $\alpha _{f} / \alpha _{r}$ 的数据：

注：使用该侧偏刚度代表大多数行车情况下的侧偏刚度；

a

a

a

（2）如果无法从车厂拿到原始数据，则需要我们通过公式估算：

a

横向动力学模型：

a

将横向动力学模型修改为以侧偏刚度为变量的方程，这样通过方程中其他的已知量就可以通过解二元方程组求出侧偏刚度 $c_f$ 和 $c_r$ ：

a

但是实际上车辆上的传感器是无法直接测量到 $\dot{v_{y}}$ 和 $\dot{r}$ 的，只能直接测量到 $v$ 和 $r=\dot{\varphi }$ ，所以我们使用欧拉定理将连续的方程离散化，这样所有的量就都是已知的了：

比如转角 $\delta$ 的值，本来这个是状态空间方程的输入，即是一个未知量，但是因为我们当前是在 $k+1$ 时刻，所以 $k$ 时刻的转角 $\delta (k)$ 其实是已知的，所以转化为离散型之后，方程中所有的量都是已知的了；

a

这样我们就把问题转化为了线性最小二乘法问题，我们可以通过做多组实验（实验条件应该符合一般的行车情况，这样求出的侧偏刚度才是最符合普通情况下行车的实际情况的）来得到一个线性方程组，通过最小二乘法求解：

4.2 线性二次调节器（LQR -- Linear Quadratic Regulator）

4.2.1 举例说明LQR控制器的设计思路 -- 1

当前的情况是：我们要选择一个代价最小的从起点到终点的出行方式，对于一个出行而言，选择不同的出行方式会花费不同的时间和不同的金钱；

对于不同的人，他们有的更看重花费少的时间付出更多的金钱，也有的人看重花费少的钱但是对时间的消耗无所谓；所以我们建立一个代价函数J，包含两个参数Q和R，分别代表时间在代价中占据的权重，和金钱在代价中占据的权重；

我们的目的是求出可以使得代价函数J的值尽可能小的出行方式是什么；

如果一个人认为时间更宝贵，想要花费更少的时间，而对金钱的花费没有要求，那么我们可以加大时间的权重Q，比如设置Q=30和R=1，这样我们求取使得代价值最小的解为直升机；

如果一个人认为金钱更宝贵，想要花费更少的金钱，而对时间的花费没有要求，那么我们可以加大金钱的权重R，比如设置Q=1和R=40，这样我们求取使得代价值最小的解为自行车；

对于普通人而言，如果能同时兼顾时间和金钱，使得时间和金钱都尽可能的少，那么可以选择Q=2和R=10，可以求得使得代价函数最小的解为公交车；

当我们取Q=1和R=5的时候，最优解同样还是公交车；因此可以得出结论，当Q和R矩阵保持一致的比例时，Q和R矩阵的取值对该问题的解没有任何影响；因此LQR控制器中重要的是Q和R矩阵之间的比值；

因此有代价函数J的设计理念：

使用Q和R矩阵作为权重，使用J作为代价；
矩阵Q和R各自的取值不重要，重要的是Q和R矩阵的比例关系；

4.2.2 举例说明LQR控制器的设计思路 -- 2

以车辆的横向控制为例：当车辆参考点和车辆当前位置不重合时，那么车辆具有一个横向误差；

车辆系统的线性状态空间方程组为：

假设状态空间方程的状态量 X 为横向误差；
假设状态空间方程的输入量 U 为方向盘转角；

任务背景：当系统的状态X出于某些原因偏离了平衡状态X=0的时候，我们能够在保证车辆平稳行驶的前提下（即尽可能不让方向盘转角变化的过于剧烈，同时也不要急转弯，方向盘转角就是控制器的输入U的变化程度），让系统的状态X尽可能快的回归到平衡状态；

设置代价函数J为二次型函数：令矩阵Q作为系统状态量X的权重，令矩阵R作为输入量U的权重；这样代价函数J就可以同时兼顾状态量X和输入量U的大小；

代价函数的设计理念：

关于作用：
- X代表系统状态，如果系统的状态偏离了平衡状态（指X=0），权重矩阵Q的取值越大，系统回归平衡状态的速度越快；
- U代表系统的输入，输入值是根据系统状态X所决定的（即 U = f (X) ），权重矩阵R的取值越大，系统调节的力度越小，系统状态量X到达平衡状态所需的时间越长；
关于函数的类型为二次型：
- 代价函数J中的被积分项为一个二次型函数，这是为了防止状态量X / 输入量U中存在负数，因此直接使用 $X^{T}X$ 和 $U^{T}U$ 来保证每个量都是正值（因为如果存在负值，那么积分的时候会存在消去的现象）；
- 同时使用二次型函数作为被积分项还有一个优点，即二次型函数一定是凸函数，可以保证代价函数J一定存在极小值；
关于矩阵Q和R的取值：
- Q与R矩阵均为人为给定的；
- 设计 $Q\geq 0$ 和 $R>0$ ；
  - 即Q为半正定阵，R为正定阵；
  - 一般为了方便，都是直接取R为单位阵，Q为对角阵，通过查看控制效果对应的修改Q矩阵的对角线元素；
- 且由于矩阵Q和R各自的取值不重要，重要的是Q和R矩阵的比例关系，所以可以直接取矩阵R为单位阵，人为调参Q矩阵即可；
- 每个对角元素 $q_i$ 都是系统状态量X中某个分量 $x_i$ 的权重值；

LQR控制器 + 车辆系统的结构图：用LQR做控制器，关键就在于负反馈环节的设计（-K）

LQR控制器的设计理念1：LQR控制器控制的系统必须是一个【以误差作为状态量的系统】，这样才能保证状态量x的平衡状态为0，才能符合LQR的控制目标 -- 令系统收敛至0；

LQR控制器的设计理念2：LQR控制器通过使用 $u=-kx$ 来通过系统状态量得到系统输入量；指定控制器中的负反馈关系为U = -K * X的前提下，可否得到一个合适的矩阵K，使得

在U = -K * X时，代价函数J存在极小值；
在U = -K * X时，该系统的状态量X可以达到稳态（即该系统是收敛的）；

已知内容：

矩阵A和B：通过系统的状态空间方程直接给定；
矩阵Q和R：人为调参调出来的，Q和R矩阵之间只有比例关系才会影响控制器的效果，所以可以直接给定矩阵R为单位阵，人为调参Q矩阵即可；

待求内容：

矩阵K的取值（U = -K * X）

因此有代价函数J的设计理念：

矩阵Q和R均为人为给定的；
矩阵Q为半正定，矩阵R为正定；
- 一般在实际应用时，将R设置为单位阵，将Q设置为对角阵，通过查看控制效果对应的修改Q矩阵的对角线元素；
由于矩阵Q和R的比例关系才会影响代价函数J的极小值点位置，所以可以直接给定矩阵R为单位阵，然后人为调参Q矩阵即可；

因此有LQR控制器的设计理念：

LQR控制器所控制的系统状态量X的平衡状态为 X = 0；
- 所以系统一般都使用误差来作为状态量X，这样可以保证其稳态为0，与LQR控制器的控制目标契合；
- 总的来说：LQR所控制的系统必须是一个以误差作为状态量的系统；
LQR控制器的输入U与系统状态X之间的关系固定为 U = -K * X；
我们的目的是得到一个LQR控制器中的取值合适的K矩阵，这个K矩阵必须使得当前系统能收敛，且代价函数J可以取得极小值；

4.2.3 举例说明LQR控制器的设计思路 -- 3

问题描述：一维系统中的LQR控制

有车辆系统的状态空间方程为： $x=\dot{x}+u$ （A与B矩阵均为一维的）；
状态量X为横向误差，输入量U为方向盘转角；
系统的平衡状态为： $x=0$ ；
系统初始状态为： $x\neq 0$ ；

目的：想使用LQR控制器控制车辆的横向运动，使得车辆可以在兼顾快速性（使状态量X快速收敛至平衡状态）和稳定性（LQR控制器输出的U不会有剧烈的波动）的前提下到达平衡状态；

求解LQR控制器：

1）设计代价函数：（ $q\geq 0 , r> 0$ ）

2）取 $u=-K * x$ ：这就是LQR控制器指定的控制规律，矩阵K就是我们的待求量；

3）取 $r=1$ ：因为q与r的取值不重要，重要的是比例，所以直接指定 $r=1$ ，调整q值即可；

4）推导K矩阵过程：

通过系统状态空间方程可得：

可知如果想让状态量最后收敛到0的话矩阵 K 要满足： $K>1$

将 u = -K * x 代入代价函数 J：

将状态量 x 的表达式代入代价函数：

如果想让代价函数J存在极小值，要保证：

综上，矩阵K需要满足： $K>1$ && $K\geq 1$ && $K=1\pm \sqrt{1+q}$

因此K矩阵取值为： $K=1+ \sqrt{1+q}$

5）LQR矩阵的核心控制原理为： $u=(1+ \sqrt{1+q}) * x$

4.2.4 连续LQR控制器的详细推导

LQR控制器【最优控制策略】+【里卡提（Riccati）方程】的推导方法：

对于连续LQR控制器：
- 关于连续LQR控制器为什么选择使用 u = -K * x 作为最优控制策略，以及里卡提方程的推导过程，使用的方法涉及变分法和泛函分析；
- 详细推导：连续系统的LQR变分法推导 - 知乎
对于离散LQR控制器：
- 关于离散LQR控制器为什么选择使用 u = -K * x 作为最优控制策略，以及里卡提方程的推导过程，使用的方法仅涉及拉格朗日乘子法；
- 详细推导：笔记92：离散LQR控制器详细推导-CSDN博客

注：本小节主讲内容为连续LQR控制器的推到流程，这里仅使用一种不严格且易于理解的推导过程，来大致的讲述一下是如何推导出里卡提方程的，以及是如何计算出控制策略中的K矩阵的；因为如果在推导连续LQR时使用严格的推导过程，那将涉及很多超纲的知识，所以这里仅采用一种通俗易懂，但并不严格的推导流程，前后推导存在过渡生硬，无法解释为什么这样选择的地方，只需大致了解一下即可；

连续LQR控制器的推导流程：

a

a

a

总结：

里卡提方程： $A^{T}P+PA+Q=PBR^{-1}B^{T}P$
最优控制策略： $u=-K*x=-(R^{-1}B^{T}P)*x$

连续LQR控制器的应用：

已知：

系统状态空间方程 $\dot{x}=Ax+Bu$ ；
代价函数 $J=\int_{0}^{\infty }(x^{T}Qx+u^{T}Ru)dt$ ；

确定使用LQR作为控制器的前提下：

获得系统状态空间方程（要保证状态量x的平衡状态为0）；
使用里卡提方程计算P矩阵；
使用P矩阵计算K矩阵；
获得最优控制策略 u = -K * x；

4.2.5 离散LQR控制器的推导

离散LQR控制器的应用：

已知：

a

a

关键公式：

离散状态空间方程：

$A_{d}=(I-\frac{Adt}{2})^{-1}*(I+\frac{Adt}{2})$
$B_{d}=Bdt$
里卡提方程：

最优控制策略：

a

a

确定使用离散LQR作为控制器的前提下：

获得系统状态空间方程（要保证状态量x的平衡状态为0）；
使用里卡提方程计算P矩阵；
使用P矩阵计算K矩阵；
获得最优控制策略 u = -K * x；

注意：【离散状态空间方程】/【离散LQR的里卡提方程】/【离散LQR的最优控制策略】的计算在如下文章中进行了详细的推导

笔记92：离散LQR控制器详细推导-CSDN博客

4.3 基于车辆横向动力学模型，使用LQR控制横向运动

注1：目前针对车辆纵向运动的控制，使用的都是【PID + 车辆运动学模型】

注2：目前针对车辆横向运动的控制，有两种方案

通过几何关系控制：【PID + 纯追踪法 / 斯坦利控制】
通过模型控制：【LQR + 车辆横向动力学模型】

4.3.1 车辆横向动力学模型的误差形式

在车身坐标系中建立起来的以【车辆横向速度 $v_y$ 】和【车辆角速度（横摆角速度） $\dot{\varphi }$ $v_x(s)$ 】为状态量，以【前轮转向角 $\delta$ 】为输入的【线性】横向动力学模型的状态空间方程描述：

注意： $r=\dot{\varphi }$ ，即 $r$ 代表的是车辆的角速度；

注：我们已经有了以 $x=\binom{\dot{v_y}}{\dot{r}}$ 作为状态量的车辆横向动力学模型，但是这个模型并不能用LQR进行控制；因为LQR控制的系统必须保证其状态量的平衡状态为0，换言之LQR所控制的系统必须是以误差作为状态量的系统；但是当前这俩状态量的平衡状态不为0；所以如果想使用LQR控制器来控制车辆的横向运动，那么必须将当前这个车辆横向动力学模型修改为误差模型；

前导1：我们选择以下4个量作为系统状态量；

横向误差： $e_{cg}$
横向误差变化率： $\dot{e_{cg}}$
航向误差： $e_{\theta }$
航向误差变化率： $\dot{e_{\theta }}$

a

a

前导2：本节推导车辆横向动力学模型的误差表示形式的时候，需要三个假设；

车辆速度 $v$ 的纵向分速度 $v_x$ 是常数（即车辆一直使用同一个纵向速度前进）；
目标点是规划路径中距离车辆质心最近的点，且这个目标点和车辆质心之间的连线垂直于车辆纵轴（这个假设只是为了便于分析，并无实际意义）；
目标点的等效移动速度为 $v(s)$ ，以车辆坐标系为准，将 $v(s)$ 分解为分量 $v_x(s)$ 和 $v_y(s)$ ，并假定 $v_x(s)=v_x$ ；

a

a

$\theta$ ：车辆横摆角（即 $\varphi$ ，车辆的纵向车轴指向的方向，同时也是车辆的前进方向）
$\theta _p$ ：目标航向角（即目标点处的切线与大地坐标系中X轴的夹角）
$e_{\theta }=\theta -\theta _p$ ：航向误差
$e_{cg}$ ：横向误差
$r=\dot{\theta }=\dot{\varphi }$ ：车辆角速度
$r(s)$ ：目标点角速度
$v(s)=\frac{v_{x}}{tan(e_{\theta })}$ ：目标点线速度
因为车辆在前进过程中，目标路径上和车辆质心当前位置匹配的目标点也是不断往前变化的，而目标点的变化既包含切向的线速度，也包含角速度；
将其角速度记为 $r(s)$ ；
将其线速度记为 $v(s)=\frac{v_{x}}{tan(e_{\theta })}$ ，方向为目标点的切向；

注：与目标点相关的量全部带 $(s)$ ；

a

a

注意：由于这三个假设的存在，所以本节的推导其实并不严谨，严谨的推导过程是没有这两个假设的，本节只是展示大致的推导过程，详细的推导过程请看下文；

严谨推导：

车辆横向动力学方程的误差形式 -- 推导过程：

因此：我们需要用上述4个新状态量（ $e_{cg}$ ， $\dot{e_{cg}}$ ， $e_{\theta }$ ， $\dot{e_{\theta }}$ ）将原车辆横向动力学模型中的旧状态量（ $v_{y}$ ， $r$ ）表示出来；

a

a

a

1）将 $e_{\theta }$ ， $\dot{e_{\theta }}$ ， $\ddot{e_{\theta }}$ 表示出来：

$e_{\theta }=\theta -\theta _p$
$\dot{e_{\theta }}=r-r(s)$
解释1：通过上图可知，车辆的角速度指向的是目标路径，因此车辆的角速度会导致 $\theta$ 变大，即车辆的角速度 $r$ 就是车辆朝向角 $\theta$ 的变化率；
解释2：通过上图可知，车辆在前进过程中，和车辆当前质心位置匹配的目标点也是沿着 $path$ 不断上移的，而目标点的移动会带来线速度和角速度，目标点的角速度 $r(s)$ 指向的是远离车辆的方向，因此目标点的角速度会使航向角 $\theta _p$ 变大，而 $r(s)$ 即为航向角 $\theta _p$ 的变化率；
解释3： $\dot{e_{\theta }}=\dot{\theta }-\dot{\theta_p}=r-r(s)$ ；
$\ddot{e_{\theta }}=\dot{r}-\dot{r(s)}$

a

a

2）将 $\dot{e_{cg}}$ ， $\ddot{e_{cg}}$ 表示出来：

$\dot{e_{cg}}=v_{y}+v_{x}tan(e_{\theta })\approx v_{y}+v_{x}e_{\theta }$
解释1：横向误差的变化率就是 -- 横向误差的大小 $e_{cg}$ 沿横向误差的方向（质心和目标点的连线）变化的速率；因此横向误差 $e_{cg}$ 的变化率包含两部分，一部分是车辆速度在该方向上的分速度，另一部分是目标点的移动导致的在该方向上的分速度；
解释2：车辆在该方向上的分速度是 $v_{y}$ ；
解释3：目标点在该方向上的分速度是 $v_{x}tan(e_{\theta })\approx v_{x}e_{\theta }$ ；（因为车辆横向动力学模型建立的条件是高速 + 小转角情况，所以转角很小时，可视 $tan(e_{\theta })\approx e_{\theta }$ ）
$\ddot{e_{cg}} =\dot{v_{y}}+v_{x}\dot{e_\theta }$
解释1：

a

a

3）将公式代入原车辆横向误差模型的状态空间方程：

代换公式：

原车辆横向动力学模型：

代入过程：

a

a

4）得到误差型的车辆横向动力学模型的状态空间方程：

4.3.2 使用DLQR，根据横向动力学模型，控制车辆的横向运动

Step0：我们在使用上述的状态空间方程的时候，只考虑矩阵A和B1，而忽略了矩阵B2；因此我们使用的状态空间方程为 $\dot{x}=Ax+B_1\delta$ ；

a

a

Step1：验证误差形式横向动力学模型的能控性；

该矩阵行满秩，则该系统是能控的；

a

a

Step2：获得状态空间方程的离散形式；

a

a

Step3：人为设定代价函数；取R阵为单位阵，取Q阵为对角阵；

用Q阵中对角线元素的取值确定状态量X中四个分量的重要性；

a

a

Step4：求解LQR控制器的控制策略 $\delta =Kx$ ；

（0）取 $A_d=A$ 和 $B_d=B_{1d}$

（1）里卡提方程求解P矩阵

（2）根据P矩阵求解K矩阵

（3）得到DLQR的控制策略

4.3.3 LQR的弊端

弊端1：LQR的适用范围很小

注意1：LQR的全名可是线性二次调节器；

线性：指的是LQR只能针对线性系统进行调节；
二次型：指的是建立的代价函数是二次型函数；

注意2：正是因为LQR只对线性系统起作用，所以我们在对系统进行建模（建立状态空间方程）的时候，要对非线性系统进行线性化，这个特点就限制了LQR的应用范围；

注意3：而我们在建立车辆横向动力学模型的时候，是假设了【高速 + 小转角】的前提，正是因为这个小转角的假设，让我们可以将横向动力学模型中的非线性部分（三角函数部分）简化为纯角度；所以我们在使用LQR控制器的时候，就限制了使用范围必须为【小转角】的行驶场景，对于目标路径path的曲率很大的场景，使用LQR跟踪轨迹的效果会很差；

弊端2：LQR的跟踪缺少前瞻性（短视）

注意：LQR在跟踪目标轨迹时，目标点选择的是距离车辆当前质心最近的点，这就意味着LQR没有前瞻性，只将当前点作为跟踪对象，即只考虑了当前点的横向误差和航向误差；而没有考虑到未来的路径的变化；因此会导致目标路径出现较大的扰动（如：连续的S弯）时LQR的控制效果很差；

总结：

LQR对于大曲率轨迹的跟踪效果不好，仅适用于小曲率轨迹的跟踪；
LQR缺少前瞻性，对于多弯道型的道路跟踪效果很差；

4.4 基于多点预瞄的LQR控制器

注：该方法为针对LQR的改进，仅提出这种方法作为延伸拓展，有价值的是他的preview的思路；

原论文：https://www.cnki.com.cn/Article/CJFDTotal-QCJS201810003.htm

4.4.1 基于多点预瞄的LQR控制器

注：LQR控制在大曲率弯道处很难有好的控制效果，这是因为LQR控制只专注于当前目标点处的横向/航向误差，而对目标点后面的目标路径的变化毫不关心，因此当目标路径点处的曲率忽然变大，LQR时跟踪不上的；而如果可以让车辆还未进入急转弯道时（即匹配的目标点处还比较平滑时），就让车辆多打一点方向，这样就可以平滑的过渡进入急转弯，这样控制效果就好了；

解决方案：类似于MPC的思路，在当前匹配的目标点处，多往后看几个目标路径上的点，即存在一个固定长度的视窗，多收集一些未来信息；

注：后面的推导过程中涉及的量均以该图为准；

a

a

Step1：

当车辆从当前目标点移动至下一目标点后，当前视窗也应同步变化，排除最旧的目标点，同时为了保证视窗长度一致，应再新加一个目标点进入；

a

a

Step2：

有车辆的原始横向动力学模型（连续）： $\dot{x}=Ax+B\delta$

注意：该模型是基于全局坐标系推导出来的

$y$ ：车辆在Y轴上的坐标
$\varphi=\theta$ ：车辆横摆角（车辆航向）

有车辆的原始横向动力学模型（离散）： $x(k+1)=A_{d}x(k)+B_{d}\delta (k)$

a

a

Step3：

联立【视窗迭代公式】+【原始车辆横向动力学方程】=【新状态空间方程】：

注意1：我们获得的这个新状态空间方程并不是误差形式的，不能直接使用LQR；

注意2：将前瞻信息加入了状态量X中，使得系统模型包含更多的未来信息；

a

a

Step4：

针对新系统（新状态空间方程）建立代价函数 $J$ ，在代价函数中将系统表述为误差型：

a

a

Step5：

使用LQR求解：由于带上了前瞻内容，控制效果变好