Coursera自动驾驶课程第7讲：Vehicle Lateral Control

在上一讲《Coursera自动驾驶课程第6讲：Vehicle Longitudinal Control》中我们了解了如何使用PID算法进行汽车纵向控制。

本讲我们继续学习新的模块：汽车横向控制。具体地，我们将学习三种控制算法：Pure pursuit，Stanley，MPC。

B站视频链接：https://www.bilibili.com/video/BV1WE411D74g?p=1

文章目录

- - 1. Introduction to Lateral Control
  - - 1.1 Overview
    - 1.2 Two Types of Control Design
    - 1.3 Driving Controller
  - 2. Geometric Steering Control - Pure Pursuit
  - - 2.1 Geometric path tracking
    - 2.2 Pure pursuit
  - 3. Geometric Steering Control - Stanley Controller
  - - 3.1 Stanley Controller Approach
    - 3.2 Heading control & Combined steering law
    - 3.3 Case Study
  - 4. Advanced Vehicle Control Methods （难点）
  - - 4.1 Overview
    - 4.2 Receding Horizon Control
    - 4.3 MPC Structure
    - 4.4 Vehicle Lateral Control

1. Introduction to Lateral Control

1.1 Overview

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

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

下面介绍三种参考路径定义：

直线段序列。
一系列紧密间隔的航点。
一系列连续的参数化曲线来定义路径。

在上述三种路径跟踪情况下，控制器都试图消除车辆与预期路径之间的偏差，并使车辆航向与路径方向对齐。

1.2 Two Types of Control Design

下面介绍两种类型控制器：

第一类控制器是几何控制器（Geometric Controllers），常见的有：Pure pursuit和Stanley。
另一类控制器称为动态控制器（Dynamic Controllers），常见的有：MPC 控制。

后面我们以自行车模型为基础来讨论汽车横向控制。

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

请注意，根据我们的控制器设计，我们可以在后轴中心，前轴中心或重心处放置车辆参考系。
在这里插入图片描述

1.3 Driving Controller

如上一节所述，我们将介绍两种类型误差：航向误差和轨迹偏移误差。

航向误差为沿路径参考点的路径方向与车辆实际航向之差。航向误差变化率有助于我们了解航向误差如何随时间变化，并且可以从自行车模型方程中进行计算。

这里，我们介绍相对于前轴的航向误差率。对于直线段，所需的航向变化率为零，可以将其删除。这是因为参考航向对于一条直线不是随时间变化的，并且实际上等于零。最终我们得到航向误差变化率为：
$ψ˙(t)=−vf(t)sin⁡δ(t)L\dot{\psi}(t)=\frac{-v_{f}(t) \sin \delta(t)}{L}$

另一种类型的误差是轨迹偏移误差。定义为车辆上的参考点与所需路径上的最接近点之间的距离。其公式为：
$e˙(t)=vf(t)sin⁡(ψ(t)−δ(t))\dot{e}(t)=v_{f}(t) \sin (\psi(t)-\delta(t))$

从该方程中，我们可以看到，随着速度的增加，误差变化更快。

2. Geometric Steering Control - Pure Pursuit

2.1 Geometric path tracking

几何路径跟踪控制器使用车辆运动学的几何形状和参考路径来跟踪参考路径。

几何路径跟踪控制器依赖于沿所需路径的参考点，该参考点可以是用于计算航向和轨迹偏移误差的参考点，也可以是沿路径在车辆前方一定距离处的超前点，一此处以红色显示。

在这里插入图片描述

2.2 Pure pursuit

下面我们介绍Pure pursuit。其核心思想是可以将参考点放置在路径上距车辆前方固定距离的位置，并且可以使用恒定的转向角计算与该点相交所需的转向命令。

在这种方法中，后轴的中心为车辆参考点，我们将后轴中心与目标参考点之间距离定义为 $l_d$ 。车辆的车身方向与连接线之间的角度为 $α\alpha$ 。
在这里插入图片描述
为了构造Pure pursuit控制器，我们再次引入瞬时旋转中心的概念。轨迹上的目标点，后轴中心和瞬时旋转中心形成一个三角形，其半径长度为 $R$ ，目标点和后轴中心长度为 $l_d$ 。 从几何关系以及正弦定律我们可以得出路径曲率：
$κ=1R=2sin⁡αld\\\kappa=\frac{1}{R}=\frac{2 \sin \alpha}{l_{d}}$

现在，让我们看一下自行车模型，以计算跟踪此弧线所需的转向角：
$δ=tan⁡−1(2Lsin⁡αld)\delta=\tan ^{-1}\left(\frac{2 L \sin \alpha}{l_{d}}\right)$

轨迹偏移误差 $e$ 在这里定义为航向矢量和目标点之间的横向距离，因此我们可以得出曲率与误差之间关系：
$κ=2ld2e\kappa=\frac{2}{l_{d}^{2}} e$

随着误差的增加，曲率也随之增加，使车辆更快速地返回至参考路径。

该式也表明，Pure pursuit控制器以类似于比例控制的方式工作，以使用路径曲率作为控制器的输出来校正轨迹偏移误差。比例增益为 $2ld2\frac{2}{l_{d}^{2}}$ 。因此，随着 $l_d$ 的增加，比例增益减小。

我们也可以对Pure pursuit制器进行修改。我们可以根据车辆的速度改变距离 $l_d$ 。使其与车辆前进速度成正比：
$l_{d}=K_{d d} v_{f}$

综合以上式子，我们得到转向角和车速之间的关系：
$δ=tan⁡−1(2Lsin⁡αKddvf)\delta=\tan ^{-1}\left(\frac{2 L \sin \alpha}{K_{d d} v_{f}}\right)$

3. Geometric Steering Control - Stanley Controller

3.1 Stanley Controller Approach

Stanley Controller 是一种几何路径跟踪控制器。最初是由斯坦福大学的Gabe Hoffman开发的，并以斯坦利车名字命名。其主要特点有：

将车辆参考点切换到前轴中心。
同时考虑航向对齐和轨迹偏移误差。
Stanley控制器为其输出设置了上限，使输出落在最大转向角范围内。

3.2 Heading control & Combined steering law

我们先介绍下如何进行航向控制的。

在左图中可以看到轨迹偏移误差是相对于前轴测量的。 Stanley控制器通常会有以下三个需求：

为了消除相对于路径的航向误差，转向角直接等于航向角： $δ(t)=ψ(t)\delta(t) = \psi(t)$
为了消除轨迹偏移误差，使用了比例控制，其增益 $k$ 满足以下公式： $δ(t)=tan−1(ke(t)vf(t))\delta(t) = tan^{-1}(\frac{ke(t)}{v_f(t)})$ 。与误差基本成正比，与速度成反比，控制函数为反正切函数。
最后，转向角会限制在一个取值范围内： $δ(t)∈[δmin,δmax]\delta(t)\in[\delta_{min},\delta_{max}]$ 。

为了消除航向误差和轨迹偏移误差，最终得到的汽车转向角为：
$δ(t)=ψ(t)+tan−1(ke(t)vf(t)),δ(t)∈[δmin,δmax]\delta(t) = \psi(t) + tan^{-1}(\frac{ke(t)}{v_f(t)}), \delta(t)\in[\delta_{min},\delta_{max}]$

现在让我们看一下对于不同的误差系统要求的汽车转向角是多少。

对于航向误差，转向命令指向与航向误差相反的方向，从而使车辆转弯以纠正与路径未对准的情况。例如，对于较大的航向误差，如果航向误差超过最大转向角，则控制器将会发出最大转向指令。

对于较大的轨迹偏移误差，此时反正切角将会接近 $π2\frac{\pi}2$ 。因此，我们可以将Stanley控制器输出为航向误差加上 $π2\frac{\pi}2$ 。
$tan⁡−1(ke(t)vf(t))≈π2→δ(t)≈ψ(t)+π2\tan ^{-1}\left(\frac{k e(t)}{v_{f}(t)}\right) \approx \frac{\pi}{2} \rightarrow \delta(t) \approx \psi(t)+\frac{\pi}{2}$

我们讨论以下误差的变化情况。=我们代入上式，可以得到误差变化率为：
$e˙(t)=−vf(t)sin⁡(ψ(t)−δ(t))=−vf(t)sin⁡(tan⁡−1(ke(t)vf(t)))=−ke(t)1+(ke(t)vf)2\begin{aligned} \dot{e}(t)=-v_{f}(t) \sin (\psi(t)-\delta(t)) &=-v_{f}(t) \sin \left(\tan ^{-1}\left(\frac{k e(t)}{v_{f}(t)}\right)\right) \\ &=\frac{-k e(t)}{\sqrt{1+\left(\frac{k e(t)}{v_{f}}\right)^{2}}} \end{aligned}$

当跨轨偏移误差较小时，上式可以简化为：

$e˙(t)≈−ke(t)\dot{e}(t)\approx-ke(t)$

上式最有趣的是误差变化率与速度完全无关。

3.3 Case Study

下面我们来看两个仿真例子。

第一个例子，对于较大的初始轨迹偏移误差，让我们假设初始误差为5米，最大允许转向角为25度，前进速度为5m/s。
仿真结果显示了Stanley控制器如何纠正较大的轨迹偏移误差并收敛到所需路径。较大的初始误差会导致较大的转向指令，从而使车辆迅速转向行驶路径。然后，航向误差和跨轨误差项达到平衡，车辆沿直线继续朝向路径行驶。

我们还可以在不同的速度下运行相同的模拟。因此，让我们尝试2m/s，5m/s和10m/s的速度。结果显示了Stanley控制器的主要特征。在所有情况下，都可以看到朝向路径的转弯，直线前进以及路径的指数衰减。速度越高，汽车在到达路径之前行驶的距离就越远。

现在让我们看看第二个仿真例子，初始时车辆的航向误差很大。
仿真结果表明航向误差已由Stanley控制器校正。首先，在纠正航向误差时，转向指令已超过其极限。然后，随着轨迹偏移误差开始增加，转向命令将继续校正汽车的行驶方向，使其与路径对齐。最后，汽车像进入指数收敛段。实际上，它具有全局稳定性功能，这意味着无论初始条件如何，控制器都将引导汽车返回原路。

然而在实际情况中，需要对Stanley控制器进行一些调整。

由于速度项位于反正切内的分数的分母中，因此低速往往会导致转向命令放大。这导致方向盘剧烈晃动，这对于驾驶员的舒适性而言是不希望的。因此，为了消除此问题并提高解决方案在低速下的稳定性，可以添加了一个常数，该常数可以在现场进行调整。
在较高的速度下，我们存在一个问题，即转向命令需要缓慢变化以确保横向力不会过大。即使在速度上如此缩放，斯坦利在高速下的反应仍然过于激进，因此还增加了航向阻尼项。这实质上将航向误差控制部分转换为PD控制器。
最后，对于具有高曲率的弯曲路径，控制器无法很好地跟踪它们，因为在几何控制器的推导中未考虑动力学。正如我们在纵向控制中看到的，通过向控制器添加前馈项，可以提高系统性能并将误差更快收敛为0。

4. Advanced Vehicle Control Methods （难点）

4.1 Overview

MPC指的是一种控制设计方法，可以在每个时间步长上以数值方式优化和解决问题。

用于自动驾驶汽车的纵向和横向控制是MPC的一个常见应用。模型预测控制通常也可以称为Receding horizon approach，因为控制器会在每个时间步长基于固定的有限长度生成一个执行器信号。MPC的主要优势有：

MPC控制器的公式简单，只需定义目标函数和相关约束，然后使用已知的求解器对其进行优化。
由于许多汽车子系统具有严格的执行器约束和多样化的性能目标，因此MPC已成为车辆控制的主要工具。 **控制器可以明确地应用于车辆及其子系统的线性或非线性模型，这意味着即使模型随时间变化或改进，也可以使用相同的方法。

当然MPC与其它控制方法相比，其缺点则是需要更多的计算资源。

在这里插入图片描述

4.2 Receding Horizon Control

在开始设计MPC控制器之前，让我们仔细研究一下Receding Horizon的概念。Receding Horizon Control解决了每个时间步长上的固定大小优化问题，该问题根据目标约束和车辆的当前状态确定从当前时间到horizon结束要应用的最佳控制输入。然而，在实现中出现的一个问题是，由于优化可能需要花费一段时间，因此开始优化时的车辆状态将不同于优化完成时的车辆状态。

因此，我们必须在优化中使用预测状态来做为实际控制输入。现在我们定义一些术语符号：

首先，我们定义receding horizon长度 $T$ 。
然后，我们将优化初始状态设置为在 $t$ 时刻的 $x$ 优化结束时的预测状态。
接下来，我们使用上次优化中确定的控制输入 $u_{t-1}$ ，在车辆从时间 $t$ 的当前状态 $x_{t-1}$ 移动到 $t$ 时刻。
最后，我们应用receding horizon优化的第一时间步骤中的控制信号，并为下一个时间步骤重复该过程。

4.3 MPC Structure

我们可以使用下方左图来可视化MPC结构。

我们有两个主要模块，一个是优化器模块，它是核心MPC组件，另一个是动态模型。该模型从时间 $t$ 减去一开始获取过去的输入和状态，并获取优化器的输出，该输出是地平线上每个时间步长的输入 $U$ 的当前序列。然后，该模型在下一个时间步输出预测状态，将其与参考轨迹进行比较，并作为预测误差传递到优化器中。

优化器还接收更新的约束和要使用的成本函数，这些约束可以预先确定，也可以根据运行模式的变化而变化。优化器解决其优化问题，然后重复该过程。

现在，让我们看一下线性MPC。

我们使用状态空间来表示离散形式的运动模型。未来状态 $x_{t+1}$ 与当前状态 $x_t$ 和执行器信号 $u_t$ 线性相关。注意， $A$ 和 $B$ 是系数矩阵，并且被假定为是不变的。
$x_{t+1} = Ax_t + Bu_t$
MPC试图找到输入的控制策略 $U$ 。
$U = \{u_{t|t},u_{t+1|t},u_{t+2|t},···\}$
目标函数可以定义为：
$\sum_{j=t}^{t+T-1}x^{T}_{j|t}Qx_{j|t}+u^T_{j|t}Ru_{j|t}$

在从零到非零的控制输入的状态偏差上具有二次误差，类似于最优控制理论的优化问题，并在控制性能和输入积极性之间进行权衡。注意，矩阵Q和R称为权重矩阵，可以进行适当选择以实现特定类型的响应。

相反，如果我们需要跟踪参考信号（例如所需轨迹），则可以修改公式以包括相对于所需状态的误差增量 $x$ 。
$δxj∣t=xj∣t,des−xj∣tJ(x(t),U)=∑j=tt+T−1xj∣tTQxj∣t+uj∣tTRuj∣t\delta x_{j|t}=x_{j|t,des}-x_{j|t} \\ J(x(t),U) = \sum_{j=t}^{t+T-1}x^{T}_{j|t}Qx_{j|t}+u^T_{j|t}Ru_{j|t}$

下面我们给出其对应的优化公式，是一个具有封闭形式的解决方案，即线性二次调节器或LQR。封闭式解决方案使用完整状态反馈，这意味着在控制响应中使用了所有状态。 LQR解决方案定义了一个控制增益矩阵 $K$ ，该矩阵可以根据状态空间模型的 $A$ 和 $B$ 矩阵以及成本函数的 $Q$ 和 $R$ 矩阵进行计算。
$min⁡U≜{ut∣tut+1∣t,…}J(x(t),U)=xt+T∣tTQfxt+T∣t+∑j=tt+T−1xj∣tTQxj∣t+uj∣tTRuj∣ts.t.xj+1∣t=Axt∣t+But∣t,t≤j≤t+T−1\min _{U \triangleq\left\{u_{t \mid t} u_{t+1 \mid t, \ldots\}}\right.} J(x(t), U)=x_{t+T \mid t}^{\mathrm{T}} Q_{f} x_{t+T \mid t}+\sum_{j=t}^{t+T-1} x_{j \mid t}^{\mathrm{T}} Q x_{j \mid t}+u_{j \mid t}^{\mathrm{T}} R u_{j \mid t}\\\quad s.t. \quad x_{j+1 \mid t}=A x_{t \mid t}+B u_{t \mid t}, \quad t \leq j \leq t+T-1$

现在让我们看一下非线性MPC：

在更一般的情况下，其目标函数是状态和receding horizon输入的任何可微的非线性函数。对于这样的一般优化问题，不存在封闭形式的解决方案。因此，我们必须依靠数值优化来找到解决方案。
在这里插入图片描述

4.4 Vehicle Lateral Control

现在，让我们看一下MPC控制器在自动驾驶汽车上进行轨迹跟踪的效果。

MPC与之前介绍的反馈结构相同，但我们将油门，制动和转向命令作为回路中的低级控制器输出。 MPC控制器的输入，包括参考路径和速度以及每个时间步的车辆状态。 MPC控制器的输出是遵循所需轨迹所需的横向和纵向力。之后低级控制器转换为相应的油门，断开和转向命令，最后，在每个时间步长上将操纵信号施加到车辆上，并通过反馈回路获得新的车辆状态。

在这里我们定义了跟踪所需轨迹的成本函数，包括最小化与所需轨迹的偏差和控制命令幅度。接下来，我们根据先前视频中开发的横向和纵向模型定义车辆的运动约束。我们还对轮胎力施加最大限制，以将其限制在线性轮胎区域内。

现在让我们看一下控制效果。

对双车道变更场景进行仿真，右图显示了带有MPC控制的模拟操纵结果，参考轨迹为蓝色，实际车辆轨迹为红色。我们可以看到，MPC控制器的跟踪性能非常好，略有滞后，但没有超调或振荡。可以看到模型预测控制器的输出（横向和纵向力）平滑变化。

同样，绘制了在双车道变更操纵期间的车辆偏航率。