代码整体规划

整车自动驾驶的架构

目前已经完成了定位和高精地图模块
还有感知，预测，规划，控制。

PNC的架构

Plan And Control

这既是我们的模块化流程

任务/路径规划，行为规划/决策，行为规划/局部规划，行为控制，车辆行驶

其中局部规划又分为轨迹规划+速度规划。

总览

基于车辆横向动力学模型，使用LQR控制横向运动
注1：目前针对车辆纵向运动的控制，使用的都是【PID + 车辆运动学模型】
注2：目前针对车辆横向运动的控制，有两种方案
通过几何关系控制：【PID + 纯追踪法 / 斯坦利控制】
通过模型控制：【LQR + 车辆横向动力学模型】

纵向控制

需要了解车辆运动学，动力学模型
PID，
纵向控制的输入：油门 / 刹车
纵向控制的输出：纵向速度 / 纵向加速度 / 跟车距离

巡航控制系统

流程为，输入目标速度，
通过上层控制器将需要跟踪的目标速度V_{des}转化为需要跟踪的目标加速度A_{des}；
通过下层控制器得到追踪目标加速度A_{des}所需要的油门/刹车指令throttle / brake

上层PI控制器通过输入量e=距离差或e=速度差得到预期加速度Ades

下层控制器有两种方式，
1是建立车辆动力学模型，通过公式获得目标加速度Ades和油门刹车开度的函数。
2.查表，油门刹车标定表

定速巡航控制系统

输入：预定速度，本车速度，两车距离，两车相对速度，时间间隔
（之所以不指定一个固定的安全距离，是因为在不同的车辆载荷和不同的车速情况下，刹车距离是不同的，无法指定一个固定的万能距离，因此我们使用一个固定的时间间隔来间接计算安全跟车距离，这样可以使得我们的跟车距离和车速存在对应关系）
输出：本车加速度/油门刹车开度

横向控制

需要了解：车辆运动学模型（自行车模型，阿克曼转向几何）
主流的横向控制方法主要有两类，一种是基于几何的方法，另一种是基于模型的方法

纯追踪法（Pure Pursuit）是一种基于几何的追踪方法；

纯追踪法在低速状态下有很好的效果（是因为该方法是基于自行车模型推导出来的，而自行车模型的应用有两个前提 – 低速+小转向角），他是基于自行车模型和阿克曼转向几何推导出的算法；
纯追踪法的使用步骤：

通过几何关系，从起始点到终点的位置沿圆弧行驶：
通过GPS确定车辆的当前位置
找到规划的目标路径path中和车辆当前位置距离最近的点（waypoint）
以该点为圆心，前视距离为半径，找出和目标路径path的交点，作为车辆的目标点G（go_point）
将目标点G转化到车辆坐标系中，方便进行跟踪
通过公式计算车辆的转向角，并操纵车辆的转向运动
隔一段时间更新一次车辆位置和状态

斯坦利控制（Stanley Controller）

注：斯坦利控制（Stanley Controller）和纯追踪法一样，都是一种基于几何的轨迹追踪方法；

车辆的横向动力学模型

注：我们的目的是控制车辆沿着我们期望的轨迹行驶，保证行车轨迹路线满足车辆动力学特性；

优化控制

线性二次调节器（LQR – Linear Quadratic Regulator）

有车辆系统的状态空间方程为：（A与B矩阵均为一维的）；
状态量X为横向误差，输入量U为方向盘转角；
系统的平衡状态为：；
系统初始状态为：；
目的：想使用LQR控制器控制车辆的横向运动，使得车辆可以在兼顾快速性（使状态量X快速收敛至平衡状态）和稳定性（LQR控制器输出的U不会有剧烈的波动）的前提下到达平衡状态；

参考线（reference line）

注意1：这里用来计算得到参考线的算法比较粗糙，使用的方法是【直接把全局路径规划得到的目标路径作为参考线】；
而全局路径的每个路径点只有基础的位置信息；
但是参考线上的每个路径点所需要的不仅仅是坐标，还有其他的几何信息（如曲率，曲率变化率…）；
注意2：因此需要将全局路径信息进行进一步的处理，使得每个路径点都包含以下五种信息
位置坐标（全局坐标系中的坐标）
朝向角
路径长度
曲率
曲率变化率

MPC 轨迹追踪

运动规划（Motion Planner）/ 局部路径规划（Local Planner）

全局路径规划

决策