ApolloPlanning决策规划代码详细解析（5）：规划算法流程介
绍
Apollo Planning决策规划系列⽂章：
之前的章节介绍了planning模块的整体框架，经过scenario与stage的选择，便进⼊了具体的task任务，由⼀系列配置好的task组成了具体的规划算法，本章以apollo中的PublicRoadPlanner为例，整体介绍规划算法的流程与思路，在之后的章节会对具体的每个模块进⾏单独的介绍。
码字不易，喜欢的朋友们⿇烦点个关注与赞。
在本⽂你将学到下⾯这些内容：
规划算法整体流程
Frenet坐标系介绍
横向规划算法介绍
纵向规划算法介绍及S-T图
轨迹合成概念
本章正⽂如下：
1、PublicRoadPlanner 的 LaneFollowStage 配置了以下⼏个task 来实现具体的规划逻辑：
scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
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_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: ST_BOUNDS_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
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
}
接下来将会对整个规划流程做的事情进⾏⼀个概述。
2、 轨迹规划需要做什么？
导航给的地图路线只是规划过程中的⼀部分，我们仍需要构建沿这条路线前进的局部轨迹。这意味着要处理⼀些不属于地图的物体：如其他车辆、⾃⾏车或⾏⼈。例如，我们可能需要与试图在我们前⾯掉头的汽车互动，或者我们可能希望超过⼀辆在公路上⾏驶的慢车。这些场景需要更低 级别、更⾼精确度的规划。我们将这⼀级别的规划称为轨迹规划。
轨迹规划的⽬标是⽣成⼀系列路径点所定义的轨迹。我们为每个路径点分配了⼀个时间戳和速度。由于移动的障碍物可能会暂时阻挡部分路段，轨迹中的每个路径点都有时间戳。我们可以将 时间戳与预测模块的输出相结合，以确保我们计划通过时，轨迹上的每个路径点均未被占⽤。这 些时间戳和空间上的两个维度(2D position)共同创建了⼀个三维轨迹(3DTrajectory)。我们还为每个路径点指定了⼀个速度，⽤于确保车辆按时到达每个路径点。
现实世界中的规划⾯临多种约束。⾸先轨迹应能免于碰撞，这意味着必须没有障碍物。其次，要让乘客感到舒适，所以路径点之间的过渡以及速度的任何变化都必须平滑。再者，路径点对车辆应实际可⾏，例如⾼速⾏驶的汽车不能⽴即做180度转弯。我们不能构建包含这种不满⾜控制算法的轨迹。最后，轨迹应合法。我们需要了解每个路径点的交通法律，并确保轨迹遵守这些法律法规。
在道路的任何两点，可能会有多个不会发⽣碰撞、⾏驶舒适、可⾏且合法的轨迹。我们如何选择最佳轨迹呢?答案是使⽤“cost function”。cost function每个轨迹分配了⼀个“成本”，我们选择成 本最低的轨迹。轨迹“成本”由各种犯规处罚组成，例如：偏离道路中⼼，有可能产⽣碰撞，速度限制，轨迹的曲率和加速度让乘客感到不舒服等。
汽车
轨迹成本将所有这些缺陷聚合为单个数值，这使我们能对不同的轨迹按数字⼤⼩进⾏排名。车辆 甚⾄可能在不同的环境中使⽤不同的成本函数。例如，⾼速路的成本函数可能与停车场的不同。
3、Frenet坐标系
我们通常使⽤笛卡尔坐标系描述物体的位置，但笛卡尔坐标系对车辆来说并不是最佳选择。即使给出了车辆位置(x,y)，如果我们不知道道路在哪，就很难知道车辆⾏驶了多远也很难知道车辆是否偏离了道路中⼼。这点很好理解，⽐如我们在⽇常驾驶过程中，我们也是根据与两边车道的距离来调整车⾝，⽽不是靠当前的经纬度，这边是frenet坐标系在规划算法中的意义，不仅拉直了道路减少了道路曲率的⼲扰，⽽且使规划的路线更好理解。
Frenet坐标系描述了汽车相对于道路的位置。 在Frenet框架中，s代表沿道路的距离，也被称为纵坐标。d表⽰与纵向线的位移，也被称为横坐标。在道路的每个点上，横轴和纵轴都是垂直的。纵坐标表⽰道路中的⾏驶距离，横坐标表⽰ 汽车偏离中⼼线的距离。
关于笛卡尔坐标系与frenet坐标系转化的推导公式很复杂，⼤家有兴趣可以⾃⾏学习，或者在使⽤过程中调⽤已经推导好的公式。
4、路径-速度解耦
规划本质上是⼀个搜索问题，为了降低算法的资源消耗，常⽤的处理⽅法是将横纵向的搜索解耦为横向路径规划与纵向的速度规划，这样可以快速逼近问题的最优解，同事节省开销。
路径-速度解耦规划将轨迹规划分为两步：路径规划、速度规划。⾸先在路径规划步骤中⽣成候 选曲线，这是车辆可⾏驶的路径。使⽤成本函数对每条路径进⾏评估，该函数包含平滑度、安全 性、与车道中⼼的偏离以及开发者想要考虑的其他任何因素。然后按成本对路径进⾏排名并选择 成本最低的路径。
下⼀步是确定沿这条路线⾏进的速度。我们可能希望改变在该路径上的速度，所以真正需要选择 的是与路径点相关的⼀系列速度，⽽不是单个速度。我们将该序列称作“速度曲线”。我们可以 使⽤优化功能为路径选择受到各种限制的良好速度曲线。通过将路径和速度曲线相结合可构建车 辆⾏驶轨迹。
5、路径规划
路径规划在Frenet坐标系下进⾏，即在S-L图上进⾏撒点。
为了在路径-速度解耦规划中⽣成候选路径，⾸先将路段分割成单元格。然后对这些单元格中的点进⾏随机采样。通过从每个单元格中取⼀个点并将点连接，我们创建了候选路径。通过重复此 过程可以构建多个候选路径。使⽤成本函数对这些路径进⾏评估并选择成本最低的路径，成本函 数可能考虑以下
因素：与车道中⼼的偏离、与障碍物的距离、速度和曲率的变化、对车辆的压⼒、或希望列⼊的任何其他因素。
6、速度规划
速度规划在S-T图上进⾏，会使⽤预测给的⽬标信息来进⾏规划。在S-T图中，曲线的斜率即为速度，斜率越⼤，即对应的速度越快。
求解最佳速度曲线需要将 ST 图离散为多个单元格。单元格之间的速度有所变化，但在每个单 元格内速度保持不变，该⽅法可简化速度曲线的构建并维持曲线的近似度。在 ST 图中可以将根据预测信息以及横向规划信息，将障碍物绘制为在特定时间段内阻挡道路的某些部分的矩形。例如，假设预测模块预测车辆将在 t0 到 t1 的时间段内驶⼊的车道。由于该车将在此期间占据位置 s0 到 s1，因此在 ST 图上绘制了 ⼀个矩形，它将在时间段 t0 到 t1 期间阻挡位置 s0 到 s1。为避免碰撞，速度曲线不得与此 形相交。既然有了⼀张各种单元格被阻挡的 ST 图，便可以使⽤优化引擎为该图选择最佳的速度曲线。优化算法通过复杂的数学运算来搜索受到各种限制的低成本解决⽅案。这些限制可能包括：交规限制，如速度限制;  距离限制，如与障碍物的距离; 汽车的物理限制，如加速度限制。
7、路径与速度曲线的优化
路径-速度解耦规划在很⼤程度上取决于离散化。路径选择涉及将道路划分为单元格，速度曲线 构建涉及将 ST 图划分为单元格。尽管离散化使这些问题更容易解决，但该解决⽅案⽣成的轨迹 并不平滑。
为 了 将 离 散 解 决 ⽅ 案 转 换 为 平 滑 轨 迹 ， 可 使 ⽤ “ ⼆ 次 规 划 ” 技 术 ( Quadratic Programming)。⼆次规划将平滑的⾮线性曲线与这些分段式线性段拟合。在实际使⽤过程中，我们只
需简单使⽤⼏种不同的优化包中的⼀种，包括⼀种由 Apollo 推出的运⾏⽅案来⽣成平滑的轨迹，⼀旦路径和速度曲线优化完成，便可以⽤其构建三维轨迹。
8、轨迹⽣成
回顾⼀下端到端路径-速度解耦规划。假设我们正在路上⾏驶，感知系统观察到⼀辆缓慢⾏驶的车辆离我们越来越近。⾸先，在这辆车的周围⽣成多条候选路线，使⽤成本函数对这些候选路径进⾏评估并选择成本最低的路径。然后使⽤ ST 图来进⾏速度规划，根据其他车辆随时间变化的位置阻挡了 ST 图
的部分区域。优化引擎可帮助确定该图的最佳速度曲线，该曲线受制于约束和成本函数。 我们可以使⽤⼆次规划让路径和速度曲线变平滑。最后，将路径和速度曲线合并构建轨迹。这⾥的轨迹在速度较快时为红⾊，在速度较慢时为蓝⾊。