智能驾驶规划控制理论学习08-自动驾驶控制模块（轨迹跟踪）

一、基于几何的轨迹跟踪方法

1、基本思想

2、纯追踪

3、Stanly Method

二、PID控制器

三、LQR（Linear Quadratic Regulator）

1、基本思想

2、LQR解法

3、案例学习

基于LQR的路径跟踪

基于LQR的速度跟踪

4、MPC（Model Predicitive Control）

1、基本思想

2、优化思路

3、MPC vs LQR

4、案例学习

系统模型

生成参考状态

数学优化

代价函数设计

约束设计

一、基于几何的轨迹跟踪方法

1、基本思想

基于几何的方法利用车辆和路径之间的几何关系，得到路径跟踪问题的控制律解。基于几何的方法通常利用前视距离来测量车辆前方的误差。该算法的目标是找出每个时刻的前轮角度。

基于几何的轨迹跟踪方法主要有纯跟踪和斯坦福提出的Stanley Method。在几何方法中使用的依然是在前面规划章节使用的自行车模型。

如上图所示，R是在给定转向角度下，后桥沿着的圆周半径。这个模型较好地近似了汽车在低速和中等转向角时的运动。

2、纯追踪

纯追踪方法的最终目的是计算前轮转角，可以通过当前车辆后轴位置到生成目标路径的前视距离la可以得到一条车辆的运动圆弧曲线和路径上的目标点 $(g_{x},g_{y})$ ，然后通过几何关系计算得到前轮转角。

$\alpha$ 角是车身行驶方向与前向距离的夹角，根据数学上的正弦定理，进行如下计算：

使用简单的自行车模型，转向角可以写成：

$\delta = tan^{-1}(kL)$

将曲率代入，可得到纯追踪控制律如下：

$\delta (t)=tan^{-1}(\frac{2Lsin(\alpha (t))}{l_{d}})$

为了使得控制律更具有直观性，可以引入一个新的变量cross track error $e_{ld}$ （横跨跟踪误差），该误差描述的是车辆行驶方向与目标点之间的横向距离，那么：

$sin(\alpha )=\frac{e_{ld}}{l_{d}}$

$k=\frac{2}{l_{d}^{2}}e_{l_{d}}$

由上式可知，纯追踪方法是一个方向盘角度的比例控制器，它作用在车里前方一些前视距离的横跨追踪误差上，增益为 $\frac{2}{l_{d}^{2}}$ 。

在实践中，增益（前视距离）被独立地调整为在几个恒定的速度下保持稳定，从而使 $l_{d}$ 更好地被指定为车速的函数。

3、Stanly Method

纯追踪方法以车辆后轴为参考点，而Stanly Method以前轴中心为参考点，同时考虑相对于路径最近点的行驶方向和位置误差。

如上图所示， $\theta$ 角是前轮偏转角， $\theta _{p}$ 是在点 $(c_{x},c_{y})$ 处前轮与路径的夹角，那么前轮要再偏转的角度 $\theta _{e}$ 为：

$\theta _{e}=\theta - \theta _{p}$

Stanly Method使用非线性反馈函数，从前轴中心到最近路径点 $(c_{x},c_{y})$ 测量横跨跟踪误差 $e_{fa}$ ,公式如下：

二、PID控制器

PID控制器试图通过调整控制变量u(t)使误差随时间的误差最小，整体的控制函数如下：

$u(t)=K_{p}e(t)+K_{i}\int_{0}^{t}e(\tau)d \tau+K_{d}\frac{de(t)}{dt}$

比例项 $P_{out}=K_{p}e(t)$ 产生一个与当前误差值e(t)成正比的输出值， $K_{p}$ 被称为比例增益常数。上图可以直观反映比例增益的取值对系统的影响，如果比例增益过高，系统就会变的不稳定，而如果比例增益太低，则当系统发生扰动时，控制作用可能太小。

积分项 $I_{out}=K_{i}\int_{0}^{t}e(\tau )d\tau$ 是针对比例控制器工作时存在的稳态误差，稳态误差指的是期望的最终输出与实际输出之间的差值。积分项跟踪累积误差，累计误差乘以积分增益 $K_{i}$ 再添加到控制器输出中。如果积分增益过高，则会导致参考值超调。

为了减小控制的波动性（提高阻尼稳定性），又引入微分项 $D_{out}=K_{d}\frac{de(t)}{dt}$ ，通过确定误差随时间的斜率来计算微分误差，计算该误差的变化率乘以微分增益 $K_{d}$ ，并加上控制器输出。微分项根据当前变化率来估计误差的未来趋势。误差变化越大，阻尼效应越大，微分项能够提高系统的稳定时间和稳定性。