1、轨迹规划基础

对于规划问题，本质上就是在（x,y,t）这个三维空间内搜索出一条可行的序列，能够满足舒适性及安全性。Apollo中是将三维空间坐标转换到Frenet坐标系中，即规划问题解耦合成路径规划（SL）和速度规划(ST)两个一维的优化问题，对其分别进行求解。

两者思路都是先将障碍物映射到S-L/S-T二维空间，先用动态规划等方法搜索可行使的凸空间，再用二次规划或者非线性优化方法进行优化。最后将速度曲线和路径轨迹进行融合得到每一时刻的轨迹点，输入到控制模块中。

2、Frenet坐标系

给定一条平滑的参考线s（一般为道路中心线），车辆在地图框架中的运动被解耦为两个独立的一维运动，即在Frenet框架中沿引导线的纵向运动s和与引导线正交的横向运动l。
在这里插入图片描述
对于道路的每个点上，横轴和纵轴都是垂直的，纵坐标表示在道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

为什么要用Frenet坐标系？
笛卡尔坐标系描述世界坐标系下坐标（x,y），难以利用道路结构（不知道路在哪，车辆行驶距离、是否偏离中心线等）。大部分的道路并不是笔直的，而是具有一定弯曲度的弧线。

Frenet坐标系下，车辆只需知道所在车道位置以及与周边障碍物的横纵向距离。将优化问题（x,y,t）解耦成两个一维优化问题（SL图进行路径规划和ST图进行速度规划）。
笛卡尔坐标系和Frenet坐标系的转换过程？
详细推到公式
Apollo坐标系研究

3、路径规划和速度规划

Apollo中轨迹规划解耦为路径规划（处理静态环境：道路、静止或低速障碍物））与速度规划（处理动态环境：高速障碍物）。两部分。每个规划过程分为决策与优化（DP+QP过程）。
（1）路径规划：
路段分割成若干单元格，对每个单元格随机采样产生候选路径，重复生成多个候选路径。设置合理的cost函数（偏离车道中心距离、障碍物距离、速度和曲率变化程度等）来选择cost最小的路径。

目标是找到S（l）函数（为什么不是l（s）：横向运动是由纵向运动引起的）。
在这里插入图片描述

（2）速度规划：
在ST图上进行，对路程和时间进行采样，将st图离散多个单元格，更具预测和一些路径规划的信息和障碍物的决策等，将障碍物、限速等投影在ST图上。利利用全局搜索方法DP算法得到决策，在利用优化方法（二次规划和非线性规划）进行速度规划。

st图上斜率越大，速度越大。
在这里插入图片描述

采样： 速度规划在ST图进行采样，在t tt的方向上以固定的间隔进行采样，在s ss方向上以先密后疏的方式进行采样（离主车越近，所需规划的精度就需更高；离主车越远，牺牲采样精度，提升采样效率）

设计状态转移方程：
主要考虑不能与障碍物发生碰撞：障碍物安全距离之内，则cost设为无穷；如果在安全距离之外，则cost设为0；如果在安全距离与障碍物之间，则按按之间的距离计算。

4、轨迹优化（QP过程）

主要采用二次规划算法实现轨迹的平滑和拟合，平滑后的路径曲线和速度曲线进行融合生成三维度轨迹点。二次规划标准形式：
在这里插入图片描述
x是要优化的变量，是n维的向量。p是二次项系数，是正定矩阵。q是一次项系数，是n维向量。A是一个mxn的矩阵，A为约束函数的一次项系数，m为约束函数的个数。l和u分别为约束函数的下边界和上边界。Apollo中使用OSQP库对二次规划进行求解。
在这里插入图片描述

（1）路径规划优化： 优化变量 x为横向距离和其一阶、二阶导数： $l$ , $l^{'}$ , $l^{''}$ 。目标函数的设计主要考虑：安全、靠近车道中心线、舒适度、尽可能降低加速度、接近终点。约束主要考虑不能与障碍物碰撞、加速度满足车辆运动学限制。目标函数如下：
在这里插入图片描述
Apollo中使用OSQP库对二次规划进行求解，具体包括：设定OSQP求解参数、计算QP系数矩阵、构造OSQP求解器、获取优化结果。

详细原理参考：基于二次规划的路径规划。

速度规划优化： 优化变量x为横向距离和其一阶、二阶导数： $s$ , $s^{'}$ , $s^{''}$ 。目标函数的设计主要考虑：转弯时减速行驶, 曲率越大，速度越小; 舒适度、尽可能降低加速度; 决策时制定的st曲线; 接近参考速度。约束主要考虑不能与障碍物碰撞、加速度满足车辆运动学限制。目标函数如下：
在这里插入图片描述
详细原理参考：Em planer算法解析

5、规划流程(lane follow场景为例)

1、规划模块流程

让我们回顾一下，规划模块运行流程如下：
入口函数PlanningComponent::Proc() -> 主要逻辑函数OnLanePlanning::RunOnce() ->
选择规划模式OnLanePlanning::Plan(）-> 选择规划器PublicRoadPlanner::Plan() ->
分scenario处理Scenario::Process() ->分stage处理 LaneFollowStage::Process() ->顺序执行stage中的任务

2、Apollo中各场景

Apollo默认从lane follow场景开始判断，而且场景之间的跳转必须经过lanefollow这个场景。

switch (current_scenario_->scenario_type()) {
      case ScenarioConfig::LANE_FOLLOW: // 车道线保持
      case ScenarioConfig::PULL_OVER: // 靠边停车
        break;
      case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED: //无保护十字路口
      case ScenarioConfig::EMERGENCY_PULL_OVER: // 紧急靠边
      case ScenarioConfig::PARK_AND_GO: // 停车和启动
      case ScenarioConfig::STOP_SIGN_PROTECTED: // 停止标志
      case ScenarioConfig::STOP_SIGN_UNPROTECTED:// 停止
      case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED: // 信号灯
      case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN: //信号灯无保护左转
      case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
      case ScenarioConfig::VALET_PARKING: // 代客泊车
      case ScenarioConfig::DEADEND_TURNAROUND: //Apollo7.0中新增 路头调头

3、Lane_follow场景中task

Lane_follow场景中task以实际的场景为例，执行具体的路径规划算法和速度规划算法，各task功能如下：

 stage_type: LANE_FOLLOW_DEFAULT_STAGE
 enabled: true

// 路径规划
 task_type: LANE_CHANGE_DECIDER // 判断当前是否进行变道，以及变道的状态
  task_type: PATH_REUSE_DECIDER  // 换道时：
								// 根据横纵向跟踪偏差，来决策是否需要重新规划轨迹；
								// 如果横纵向跟踪偏差，则根据上一时刻的轨迹生成当前周期的轨迹，以尽量保持轨迹的一致性
  task_type: PATH_LANE_BORROW_DECIDER // 只是判断是否满足借道条件，具体的轨迹是否借道，是由后面的task决定
  task_type: PATH_BOUNDS_DECIDER // 根据借道信息、道路宽度生成FallbackPathBound
								 // 根据借道信息、道路宽度生成、障碍物边界生成PullOverPathBound、LaneChangePathBound、		
					             RegularPathBound中的一种
					             // 将车道线和障碍物转为上图中的边界
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER // 根据之前decider决策的reference line和path bound,
                                // 以及横向约束，将最优路径求解问题转化为二次型规划问题;
                                 // 调用osqp库求解最优路径;
  task_type: PATH_ASSESSMENT_DECIDER  //选出之前规划的备选路径中排序最靠前的路径;
								// 添加一些必要信息到路径中
  task_type: PATH_DECIDER	    // 在上一个任务中获得了最优的路径，
                             // PathDecider的功能是根据静态障碍物做出自车的决策，对于前方的静态障碍物是忽略、stop还是nudge
  task_type: RULE_BASED_STOP_DECIDER    // 根据一些规则设定停止标志
  
  //速度规划
  task_type: ST_BOUNDS_DECIDER 
  task_type: SPEED_BOUNDS_PRIORI_DECIDER  // 将规划路径上障碍物的st bounds加载到路径对应的st图上
										  // 计算并生成路径上的限速信息
  task_type: SPEED_HEURISTIC_OPTIMIZER  // 使用DP求解一条最优路径
  task_type: SPEED_DECIDER //
  task_type: SPEED_BOUNDS_FINAL_DECIDER //
  #task_type: PIECEWISE_JERK_SPEED_OPTIMIZER //
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER //
  task_type: RSS_DECIDER //

本文初步了解了规划模块的原理和流程，后续将详细讲解相关代码。

参考链接：
星火计划路径规划学习笔记
星火计划速度规划学习笔记

速度规划论文：
[1] Qian X , Altche F , Bender P , et al. Optimal Trajectory Planning for Autonomous Driving Integrating Logical Constraints: An MIQP Perspective[C]// IEEE International Conference on Intelligent Transportation Systems. IEEE, 2016.