技术背景

经典的轨迹规划方法分为两类：第一类是基于曲线插值的方法，如直线和圆弧段、多项式曲线、回旋曲线和样条曲线等，其主要通过确定参数直接生成目标轨迹；第二类是基于采样的搜索方法，如 Dijkstra算法、A*算法、人工势场法等方法解决轨迹规划问题。

第一类基于曲线插值的方法：直线和圆弧段实现起来简单、低计算量，但曲率不连续、节点片段连接；多项式曲线具有低计算量、曲率连续的优点，但曲率连续需要四阶以上，关键参数的确定比较困难；回旋曲线适用于局部规划，但积分导致耗时，曲率连续但是不平滑，依赖轨迹点；样条曲线也具有低计算量、曲率连续的优点，但结果可能不是最优解。

第二类基于采样搜索的方法：Dijkstra算法适用于结构化环境下找到最短路径，但不满足实时性要求，结果不连续，计算量较大；而A*算法其优点是具有启发性，能够较快获得全局最优轨迹，然而搜索步长难以确定，在复杂并且范围较大的规划环境中效率较低。

• 道路场景

轨迹规划层应该生成对应于每个行为结果的可执行轨迹。实际上，轨迹规划是一个约束优化问题。约束条件包括车辆的当前状态行为和目标车辆动力学，它们构成最终的轨迹优化。满足这些条件的轨迹可以作为最终输出轨迹。

• 候选轨迹生成

当无人车获得车道的HD地图时，在一定的车道范围内进行“分散点”，即车辆可能的行驶位置，可以将HD地图上车辆的车道轨迹规划问题抽象为有向加权的最短路径搜索问题。如图所示，通过连接随机生成的采样点生成候选轨迹。

• 成本函数模型

考虑到车辆的转向能力，在随机分布采样点时，不仅需要考虑最短的欧几里得距离分布，而且期望轨迹的平滑度，即头部转角尽可能小。基于以上分析，本发明选择了一个更合理的成本函数Cost模型。下图表示轨迹规划成本函数的节点示意图

• Dijkstra轨迹筛选

设计了一种改进的Dijkstra算法。首先，读取采样点数据集合，集合中采样点个数为N，并获取车辆的起点和终点，将起点标记为Start，终点标记为End，采样点编号标记i（1≤i≤N）。然后，从起点Start出发寻找采样点集合中Cost最小的点作为下一个点，初始Cost标记为0。接着，依次检查剩下所有的采样节点，并将其相邻节点添加到要检查的节点列表中。

从起点Start到每个节点遍历后，若节点i的Cost更小，则更新i++。判断采样节点是否被访问，每次不会重复原来的轨迹；当未达到终点End时，计算未标记点j之间的成本函数，选择最小成本点，并标记最佳点，直到最后一个点(目标点)End结束。

• 轨迹平滑

为了实现汽车从初始点到最终点的云动，需要对初始点和目标点的状态进行规划，从而生成更平滑的曲线。

针对换道场景，基于改进的Dijkstra算法考虑车辆运动约束生成候选轨迹。其中，车辆在起点和目标点作为一个约束条件，车头朝向都应该都是水平的。

• 结果计算

通过上述成本函数模型，从候选轨迹中生成最优的曲线。其中，需要考虑轨迹的航向角和曲率。Angle表示车辆的方向，起点和终点的航向角应相同，Curvature表示曲率。

从图8中看出，航向角从0度到最大航向角18度然后回至到0度，这与图5中规划的车辆运动轨迹一致；从图8中看出，轨迹的曲率是连续的，最大曲率0.0053/m（小于0.08/m的极限）。