Coursera自动驾驶1.5——纵向控制和横向控制

一、横向控制

1.PID控制

（1）PID控制介绍

PID控制包含三项：

• 与误差e ee成比例的比例项 $K_P$
• 与误差积分成比例的积分项$K_I$
• 与误差导数成比例的导数项$K_D$

其时间域公式为：
$$u(t)=K_{P}e(t)+K_I{\int }_0^{1}e(t)dt+K_D\dot{e}(t)$$
对其进行拉普拉斯变换可得：
$$U(s)=G_c(s)E(s)=(K_P+\frac{K_I}{s}+K_{D}s)E(s)=(\frac{K_D{s}^2+K_{P}s+K_I}{s})E(s)$$
对于系统响应，我们定义几个评价指标：

• Rise Time （上升时间）：达到所需参考值90％所需要的时间。
• Overshoot：系统输出超出此参考的最大百分比。
• Settling Time（稳定时间）：系统输出稳定在参考值范围的5％之内所需时间，
• Steady State Error（稳态误差）：稳态下输出与参考值之间的误差。

下图展示了P、I、D分别对如上指标的影响

（2）例子：二阶弹簧质量阻尼模型

系统受到输入力F的作用，模型的输出为物体位移x。 质量块M通过具有常数K的弹簧和具有阻尼系数b的阻尼器连接墙面。

其状态转移函数如下图

令m=1，b=10，k=20，F=1。当使用单位阶跃输入来激励系统。 此时系统响应为开环响应，没有控制器应用于系统。系统响应以及状态如下图

当添加控制器到系统中时，此时为闭环响应，其系统响应和状态函数变为

2.PID纵向控制

汽车纵向控制中最常用的控制应用就是巡航控制。通常巡航控制系统主要功能就是：通过节气门或制动命令维持车辆速度。

通常控制器可以分为两个级别：高级控制器和低级控制器。

1. 高级控制器：高级控制器根据车辆参考速度和实际速度之差，生成所需的加速度以减小速度差。其输入是速度差，输出是车辆的期望加速度。
   根据PID原理我们所需的加速度可以写成：
   

2. 低级控制器：低级控制器通过增加或减小发动机产生的扭矩来产生期望的加速度。将期望的加速度转换为扭矩需求，然后将扭矩需求转换为节气门角度指令。
   

3.前馈速度控制

（1）介绍

前馈图是一个典型的闭环结构，将输出与参考信号做比较。 并将两者之差输入到反馈控制器中。反馈图是一个开环结构，其中参考信号直接馈入前馈控制器。两者结构如下图：

在许多应用中，通常前馈回路和反馈回路会结合使用以提高控制器性能。主要原因是前馈控制器在产生参考输出以实现特定跟踪响应时会提供预测响应，尤其是在所需输入为非零时。反馈控制器会修正响应，从而消除由于干扰而引起的控制误差。

（2）汽车纵向速度控制

我们使用反馈和前馈控制器用于汽车纵向速度控制。结构如下：

参考速度是前馈控制器的输入，速度差是反馈控制器的输入。 两个控制器均产生两个车辆控制信号，即节气门和制动命令。其具体步骤如下：

• 根据车辆速度和车轮角速度之间的运动关系计算车轮角速度。同时通过建模模块中定义的运动关系来计算与所需车轮角速度相对应的发动机RPM。
• 使用车辆的当前状态来计算负载扭矩。
• 将负载扭矩与RPM中的当前发动机运行速度结合起来，求出所需扭矩对应的节气门位置。
  

二、横向控制

1.介绍

设计汽车横向控制器时，通常需要进行以下三步：

1. 定义车辆所处位置和预期路径之间的偏差。
2. 选择一种控制设计策略，将偏差降至零，并使车辆航向与路径方向对齐。同时仍满足输入限制（如输入转向角限制等）。
3. 考虑车辆本身的动态限制和所需的行驶特性。

车辆的参考路径可以有如下几种：直线段序列，一系列紧密间隔的航点，一系列连续的参数化曲线

2.控制器设计

有两种类型控制器：

• 几何控制器（Geometric Controllers）：使用车辆运动学的几何形状和参考路径来跟踪参考路径。常见的有：Pure pursuit和Stanley。
• 动态控制器（Dynamic Controllers）：常见的有：MPC 控制。

如下图所示，我们将使用线段作为参考路径，在图中以实心黑线显示。还可以看到一条平行于路径但穿过前轴中心的黑色虚线。为了进行横向控制，我们重新定义了相对于当前路径线段的航向δ和ψ。

我们可以在后轴中心，前轴中心或重心处放置车辆参考系。

3.类型误差

两种类型误差：航向误差和轨迹偏移误差。

• 航向误差：沿路径参考点的路径方向与车辆实际航向之差。 航向误差变化率有助于我们了解航向误差如何随时间变化。航向误差变化率为：
  $$\dot{\Psi (t)}=\frac{-v_f(t)\sin \delta (t)}{L}$$
• 轨迹偏移误差:车辆上的参考点与所需路径上的最接近点之间的距离。其公式为：
  $$\dot{e(t)}=v_f(t)\sin (\Psi (t)-\delta (t))$$

通过公式我们可以看出随着速度的增加，误差变化更快。

2.常用的横向控制算法

Pure Persuit（纯跟踪）: 横向控制 | Pure Persuit（纯跟踪）算法
Stanley算法：横向控制 | Stanley算法
MPC算法：自动驾驶控制算法实例之模型预测控制（MPC）–从模型推导到代码实现（以Autoware为例）
三种算法比较：横向自动控制方法：Purepursuit, Stanley, MPC对比