第6课 路径-速度解耦规划

无人驾驶第一课：从Apollo起步

GitHub：路径-速度解耦规划
   在Apollo中，路径规划基于高精度地图、定位和路径预测结果来构建无人车轨迹。规划的第一步是路径导航，即如何从地图上的A 到B，之后输入地图数据等，输出可行驶路径。之后可以进行最终轨迹规划决策。
  轨迹由一系列点定义，每一个点都有一个关联速度和一个指示何时应到达那个点的时间戳。

1 首先，要进行路线规划。
  输入：
  1）地图数据。包括公路网和实时交通信息；
  2）无人车的位置；
  3）目的地。
  通过搜索查找路线，首先，将地图数据格式化为图形的数据结构，以便于利用在图形中查找路径的快速算法（如A*路径查找算法，考虑从初始节点到候选节点的成本g，还得估计候选节点到目标节点的成本h,最终相加形成f,总是选择尚未访问过且f值最小的节点），最后再转换为地图上的路径。该图形由“节点”和“边缘”构成，节点代表路段，边缘代表路段之间的连接。对从一个节点到另一个节点的成本进行建模，如时间，能耗，安全性等。最终确定一条路线。

  实际候选路径的生成方法：
  首先，将路段分解为单元格；

  其次，对单元格中的点进行随机采样；

再次，从每个单元格中取一个点并将点连接，重复此过程构成多条路径；

最后，采用成本函数对它们进行评估，选择成本最低路径。成本如考虑曲率、加速度、有可能碰撞、超速、偏离道路中心等。

2 其次，延这条路线构建轨迹，即轨迹生成：
  生成由若干路径点形成的轨迹，每个路径点包含时间戳和速度，之后将时间戳与预测模块的输出相结合，以确保在通过时每个点均未被占有。每个路径点由二维坐标和时间构成三维轨迹，此外加一个在此点的指定速度。
生成的轨迹要平滑，且应注意速度与转弯的匹配。
  确定在这条路上的速度，一条轨迹上速度是可变的，因此选择的是一速度序列构成的速度曲线，而不是单个速度。
  在规划中，使用的是Frenet坐标系，其描述了汽车相对于道路的位置，S代表延道路的距离，即纵坐标，L代表与纵向线的位移，即横坐标；两个轴垂直。
  利用ST工具设计和选择速度曲线。S代表纵向位移（纵轴），T代表时间（横轴），ST上的运动曲线说明了车辆在不同时间的位置，曲线的斜率可以推算出速度。为构建最佳的速度曲线，将ST图离散为多个单元格，单元格之间的速度有变化，但单元格内的速度不变。在ST图中，可以将障碍物绘制为在特定的时间段内，阻挡道路的某些部分的矩形，之后就可以使用优化引擎为该图选择最佳的速度曲线，优化算法通过复杂的数学运算来搜索，如考虑与障碍物的距离限制、速度限制、汽车的加速度限制等。

  由于ST图离散为多个单元格，因此速度曲线并不平滑（如上图），因此可使用二次规划技术将分段式线性段拟合为平滑的非线性速度曲线。Apollo包含几种不同的优化包，可选择其中一种。

通过将路径与速度曲线相结合，构成车辆三维行驶轨迹（二维坐标和时间,(s,d,t)），路径-速度解耦规划完成。下面介绍轨迹生成方法Lattice：
  第一步，分离轨迹的横向d和纵向分量s，形成两个二维轨迹，其中一个是ST轨迹（S,T)，另一个是相对于纵向轨迹的横向偏移SL轨迹（L,S)（下左图）；对预选模式中的多个候选最终状态进行采样（见第1点，见下右图），形成多个最终车辆状态。
  
  ST轨迹的终止状态：巡航（完成规划步骤后定速行驶，（V,T）图，V不变）、跟随（要对位置和时间状态进行采样，并尝试在时间t出现在某辆车后面，且要保持安全距离，速度和加速度的修正取决于跟随的车辆）或终止（需要对汽车何时何地停止进行抽样，速度加速度为0）
  LS轨迹的终止状态：与车道中心对齐，即在一个小区域内，对横向终止位置进行采样，即采样的是相邻车道中心线周围的位置（下图左），终止状态应该与中心线一致（下图右）。则SL候选轨迹应该以车辆与车道对其并直线行驶而结束。为达到这种终止状态，车的朝向和位置的一阶和二阶导数都应该为0，这意味着车辆既不是横向移动的（一阶导数），也不是横向加速（二阶导数）。

          
  

  第二步，合并两个轨迹。对每个候选最终状态，构建了一组轨迹，使用成本函数对这些轨迹进行评估，找出最低成本轨迹。之后将确定好的ST和LS轨迹由Frenet坐标系重新转换为笛卡尔坐标系，然后可以将它们结（利用都有的S值进行匹配），构建由二维路径点和一维时间戳组成的三维轨迹。