本文是学习自动驾驶控制算法第十二讲 横纵向综合控制的学习笔记。
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
