本文是学习自动驾驶控制算法第十二讲 横纵向综合控制的学习笔记。
1. 概览
仿真模型图如下图所示，主要有四个模块：
模块1是Carsim的车辆模型，输入是油门、制动、转向，输出是车辆位置、速度、横摆角、横摆角速度等；
模块2是用于计算仿真的轨迹，输出轨迹信息，包括位置、速度、加速度、曲率等；
模块3是用于计算横向控制的转角以及纵向控制所需的位置误差 e s e_s es和速度 s ˙ \dot{s} s˙，输入是车辆运动状态和规划的轨迹信息；
模块4是用于计算纵向控制的油门和刹车，输入主要是位置误差 e s e_s es和速度 s ˙ \dot{s} s˙、规划的速度和加速度；
2. 模块2-仿真轨迹
给定起点和终点的位置坐标和车辆运动状态，使用五次多项式计算起点到终点的轨迹，如下图所示：
已知车辆起点状态
x s t a r t = x ( 0 ) , x ˙ ( 0 ) , x ¨ ( 0 ) \begin{equation} x_{start}=x(0),\\dot{x}(0),\\ddot{x}(0) \end{equation} xstart=x(0),x˙(0),x¨(0)
y s t a r t = y ( 0 ) , y ˙ ( 0 ) , y ¨ ( 0 ) \begin{equation} y_{start}=y(0),\\dot{y}(0),\\ddot{y}(0) \end{equation} ystart=y(0),y˙(0),y¨(0)
和终点状态
x e n d = x ( T ) , x ˙ ( T ) , x ¨ ( T ) \begin{equation} x_{end}=x(T),\\dot{x}(T),\\ddot{x}(T) \end{equation} xend=x(T),x˙(T),x¨(T)
y e n d = y ( T ) , y ˙ ( T ) , y ¨ ( T ) \begin{equation} y_{end}=y(T),\\dot{y}(T),\\ddot{y}(T) \end{equation} yend=y(T),y˙(T),y¨(T)
其中 T T T表示仿真周期，起点到终点的轨迹方程满足：
x ( t ) = a 5 t 5 + a 4 t 4 + a 3 t 3 + a 2 t 2 + a 1 t + a 0 \begin{equation} x(t)=a_5t^5+a_4t^4+a_3t^3+a_2t^2+a_1t+a_0 \end{equation} x(t)=a5t5+a4t4+a3t3+a2t2+a1t+a0
y ( t ) = b 5 t 5 + b 4 t 4 + b 3 t 3 + b 2 t 2 + b 1 t + b 0 \begin{equation} y(t)=b_5t^5+b_4t^4+b_3t^3+b_2t^2+b_1t+b_0 \end{equation} y(t)=b5t5+b4t4+b3t3+b2t2+b1t+b0
将1-4代入5~6可求解得到系数 a i ( i = 0 ... 5 ) a_i\ (i=0\dots5) ai (i=0...5)和 b i ( i = 0 ... 5 ) b_i\ (i=0\dots5) bi (i=0...5)的值，有了 x ( t ) x(t) x(t)和 y ( t ) y(t) y(t)，也就能求得轨迹上的速度、加速度、曲率等信息。
3. 模块3
横向控制详细介绍参考前面章节介绍。
其输出项中的 e s e_s es、 e d e_d ed和 s ˙ \dot{s} s˙如下图所示，其中蓝色点表示车辆当前位置在轨迹上的匹配点（仿真中直接使用模块2的输出）：
考虑到转向不足，加入了 e d e_d ed来控制补偿转向。
4. 模块4
模块4内部就是上一节所说的双PID纵向控制，这里输入的车速 v v v和发动机（电机）转速 r p m rpm rpm仅用于车辆模型的功率仿真计算。
5. 仿真结果
5.1 case1
x s t a r t = 0 , 0 , 0 x_{start}=0,0,0 xstart=0,0,0， x e n d = 100 , 0 , 0 x_{end}=100,0,0 xend=100,0,0
y s t a r t = 0 , 0 , 0 y_{start}=0,0,0 ystart=0,0,0， y e n d = 10 , 0 , 0 y_{end}=10,0,0 yend=10,0,0
T = 20 s T=20s T=20s
如下图所示，自车位置 x x x和 y y y与规划的轨迹几乎完全重合：
5.2 case2
在case1的基础上，将时间减少， T = 10 s T=10s T=10s，如下图所示，对轨迹的跟踪偏差较大，这是因为时间变短，PID输出的加速度要求太大，超出了设计的最大值
5.3 case3
也可以仿真速度比较高的场景
x s t a r t = 0 , 20 , 0 x_{start}=0,20,0 xstart=0,20,0， x e n d = 100 , 0 , 0 x_{end}=100,0,0 xend=100,0,0
y s t a r t = 0 , 0 , 0 y_{start}=0,0,0 ystart=0,0,0， y e n d = 10 , 0 , 0 y_{end}=10,0,0 yend=10,0,0
T = 10 s T=10s T=10s
