自动驾驶控制算法-横纵向控制仿真

本文是学习自动驾驶控制算法第十二讲 横纵向综合控制的学习笔记。

1. 概览

仿真模型图如下图所示，主要有四个模块：
模块1是Carsim的车辆模型，输入是油门、制动、转向，输出是车辆位置、速度、横摆角、横摆角速度等；
模块2是用于计算仿真的轨迹，输出轨迹信息，包括位置、速度、加速度、曲率等；
模块3是用于计算横向控制的转角以及纵向控制所需的位置误差$e_s$和速度$\dot{s}$，输入是车辆运动状态和规划的轨迹信息；
模块4是用于计算纵向控制的油门和刹车，输入主要是位置误差$e_s$和速度$\dot{s}$、规划的速度和加速度；

2. 模块2-仿真轨迹

给定起点和终点的位置坐标和车辆运动状态，使用五次多项式计算起点到终点的轨迹，如下图所示：

已知车辆起点状态
$$\begin{array}{c}{x}_{start}=[x(0),\dot{x}(0),\ddot{x}(0)]\end{array}$$
$$\begin{array}{c}{y}_{start}=[y(0),\dot{y}(0),\ddot{y}(0)]\end{array}$$
和终点状态

$$\begin{array}{c}{x}_{end}=[x(T),\dot{x}(T),\ddot{x}(T)]\end{array}$$
$$\begin{array}{c}{y}_{end}=[y(T),\dot{y}(T),\ddot{y}(T)]\end{array}$$
其中$T$表示仿真周期，起点到终点的轨迹方程满足：
$$\begin{array}{c}x(t)=a_5{t}^5+a_4{t}^4+a_3{t}^3+a_2{t}^2+a_{1}t+a_0\end{array}$$
$$\begin{array}{c}y(t)=b_5{t}^5+b_4{t}^4+b_3{t}^3+b_2{t}^2+b_{1}t+b_0\end{array}$$
将1-4代入5~6可求解得到系数$a_i(i=0\dots 5)$和$b_i(i=0\dots 5)$的值，有了$x(t)$和$y(t)$，也就能求得轨迹上的速度、加速度、曲率等信息。

3. 模块3

横向控制详细介绍参考前面章节介绍。
其输出项中的$e_s$、$e_d$和$\dot{s}$如下图所示，其中蓝色点表示车辆当前位置在轨迹上的匹配点（仿真中直接使用模块2的输出）：

考虑到转向不足，加入了$e_d$来控制补偿转向。

4. 模块4

模块4内部就是上一节所说的双PID纵向控制，这里输入的车速$$v$$和发动机（电机）转速$$rpm$$仅用于车辆模型的功率仿真计算。

5. 仿真结果

5.1 case1

$x_{start}=[0,0,0]$，$x_{end}=[100,0,0]$
$y_{start}=[0,0,0]$，$y_{end}=[10,0,0]$
$T=20s$
如下图所示，自车位置$x$和$y$与规划的轨迹几乎完全重合：

5.2 case2

在case1的基础上，将时间减少，$T=10s$，如下图所示，对轨迹的跟踪偏差较大，这是因为时间变短，PID输出的加速度要求太大，超出了设计的最大值

5.3 case3

也可以仿真速度比较高的场景
$x_{start}=[0,20,0]$，$x_{end}=[100,0,0]$
$y_{start}=[0,0,0]$，$y_{end}=[10,0,0]$
$T=10s$