实例详解自动驾驶中的最优路径规划

实例详解自动驾驶中的最优路径规划

附赠自动驾驶学习资料和量产经验：链接

自动驾驶中的路径规划是一种顶层导航问题的上层模块，主要通过一系列车道级引导规划，解决车辆从起点位置移动至终点位置。一般的路径规划与控制模块外围是交通预测模块，其输入是感知到的环境信息，包含如车道线、道路曲率、道路障碍物、交通标志牌等环境静态信息，亦或是道路移动车辆信息（如速度、加速度、距离等）；而路径预测得到的输出信息包含时间信息以及空间信息，预测可以优化感知模块输出，或者对决策/规划模块输入到物体进行预处理。

1、路径规划技术原理

1、建立轨迹规划坐标系

首先，是对自车建立可靠的坐标系，一般情况下有两种坐标系建立方式。1）比较简单且常用的坐标系是X-Y-Z三向，该坐标系采用美国机动车工程师学会(SAE)的设定，以自车车辆中心为原点，以车辆行驶方向的前后为 x 轴，且向前为正，以车辆行驶方向的两侧为 y 轴，且向右为正，以车辆行驶方向的上下方向为 z 轴，且向上为正。

2）针对规划控制比较好理解的坐标建立方式Frenet坐标系，该坐标系使用道路中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系，Frenet–Serret公式用于描述粒子在三维欧氏空间R3内沿一条连续可微曲线的运动学特征，该坐标系在规划模块中广泛使用。

2、建立初始规划轨迹

路径规划的过程是包含了两个层面，其一是基于行车自动驾驶初始激活状态建立相应的初始运动轨迹规划，它包含如下几个步骤进行。

1） 将车辆位置及参考线参照如上定义并选择的坐标系进行车辆坐标系转化；

2） 按设定规则固定采样选取目标点；

3） 利用曲线插值或拟合方法生成备选轨迹；

4） 将备选轨迹进行膨胀计算剔除可能不合理的轨迹；

5） 基于控制量平滑或偏差最小等代价函数选择最优轨迹；

如下将对上面的轨迹规划过程进行一一介绍。

（1） 图搜索基础路径：

传感器识别的连续环境信息一般属于模拟信号，需要首先采用一定的图搜索路径规划算法将其转换为适应于所选路径规划算法的离散图，然后运用一定搜索算法得到基础路径。一般针对视频图的搜索算法包含如下：

**Dijkstra算法：**设置初始节点集，从目标车所在的初始点开始，迭代的检查结点集中的所有结点，并把和该结点最靠近的尚未检查的结点加入待检查结点集。该结点集从初始结点向外扩展，直到到达目标结点。

**A*算法：**设置代价函数g(n)表示为从初始结点到结点n的代价，h(n)表示从结点n到目标点的启发式评估代价。结合靠近初始点的结点和靠近目标点的结点的信息块。当从初始点向目标点移动时， A*权衡这两者得到最优路径。

（2）采样点生成路径

采样点生成实际是针对自车在某个设定构型空间生成样本点，寻找满足任务需求的样本点序列作为规划结果；采样方法分为随机采样和固定采样两种。随机采样是指在构型空间中生成随机性的样本点的方法，该方法在规划空间内随机选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个可行路径。固定采样则是按照明确给定的规则生成一系列待选样本点，并通过筛选选定其中质量最佳的样本。典型的应用是采用触须算法规划车辆局部轨迹，触须过程即是试探性的尝试多种方向控制方案，通过建立综合考虑运动可行性、平滑程度、安全性能的评价指标来选拔理想的行驶路径，摒除危险或不可用路径。

3. 轨迹曲线插值

一般情况下，针对规则型道路（即道路宽度或曲率等因素不存在突变），无论针对对中控制还是换道，车辆轨迹曲线往往与道路轨迹曲线是保持一致的运动规律，如下图表示了适合用轨迹曲线描述的车道类型。利用车道转弯半径所对应的圆弧确定行驶路径可以完全适配于车辆行驶路径，将行驶路径显式地表述为数学函数，通过确定函数的参数来决定行驶路径的具体形态。

ü 曲线元素组合方法：

（1） Dubins曲线；

（2） Reeds-Shepp曲线

（3） Clothoid 曲线 ( 即回旋曲线 ) 与圆弧段结合方式；

以Rees-Shepp曲线为例，该曲线由几段半径固定的圆弧表示汽车转弯的最小弯道半径和一段直线表示直行车道的线拼接组成；假设车辆以固定的半径转向，且车辆能够前进和后退，那么Reeds-Sheep曲线就是车辆在上述条件下从起点道终点的最短路径，该曲线不仅能够表示所能到达的终点，而且能够保证车辆的角度在终点所能到达的预期角度， Dubins曲线与Reeds-Sheep曲线设置方式相似，但是，Dubins多了一个约束条件从而导致汽车只能朝前开，不能后退（即不能挂倒挡）。

如果单就比较简单的几个驾驶场景而言，该曲线元素组合的方法完全可以满足拟合要求。但是针对存在障碍物的情况，如上曲线就不再满足要求。目前，针对路径规划障碍物检测的方法中，采用回旋曲线与圆弧段结合的方式是一种比较常规的方法。比如在自动换道中，该方法考虑的轨迹曲线预测中需要首先考虑驾驶场景下的道路曲率，然后再对限定状态下的道路进行对中曲线进行规划，其次，再利用自车道与目标车道的实际情况对换道曲线进行预测。最后将两者进行无缝衔接组合就形成了最终的轨迹曲线。当然实际过程中的圆弧段既可以采用最为简单的抛物线也可以采用比较简单的低阶回旋曲线。如下图分别清晰的表示回旋曲线与圆弧曲线的组合表示方式。除开驾驶路径规划以外，真正的自动驾驶更要考虑到对于车辆实际行驶路径的规划，也就是自动驾驶车辆如何沿着我们所规划的路径向前行进。这就要求我们具备一定的曲线插值拟合方法，如下列举了相应的曲线拟合方式。

（1） 三次B样条曲线

（2） 基于四阶贝塞尔曲线

（3） 采用五次多项式

对于自动驾驶开发而言，如上几种曲线插值拟合的方式，各自有自己的优缺点，但针对不同的场合，需要从适配度、算力等相关因素共同考虑。一般的，针对检测环境信息生成的离散航迹参考点可利用三次B样条曲线进行拟合，自动驾驶车辆的实际可行使轨迹规划可采用四阶贝塞尔曲线进行拟合，复杂路口的转弯行驶路径或正常变道的行驶路径可以采用5次多项式进行拟合。如下将以具体实例讲解完整的基于采样及曲线插值结合的方式规划驾驶路径，其中曲线拟合采用4阶贝塞尔曲线。对于给定离散点t，可分别得到x(y),y(t)，即该车所在位置的横纵坐标，那么利用四阶贝塞尔曲线表示轨迹的参数化表达式表示如下，其中P0表示曲线起始点，P1-P4表示曲线控制点（其中以控制点参数d1，d2，d3来表示），该四个曲线控制点影响形成曲线的扭曲程度（或运动趋势）：

，0表示该离散点已被行驶过，1表示该离散点未经过。

以自动换道轨迹曲线为例，为了求解上述方程式中的所有参数预测过程中需要设置为初始状态和终止状态，作为以上方程的解集之一。其中，初始状态表示为车辆当前位置状态，终止状态则为按照当前状态和实际车道线位置预测的终止点信息。

如上图所示，对于已知点来说只有P0和P4两个点。由此，曲线经过起始点和终止点分别表示为；

曲线端点切向量为：

曲线在任一点的曲率表示为

，那么基于初始点计算的该点曲率就可以表示为

此时，为了求解P0，P1，P2，需要，以自车当前位置建立带有航向角表示的坐标原点P0=（0，0，0，kT），设置曲线控制点参数d1=|P0P1 |

为了求解P3点，则需要设置车辆目标位置约束P4=（xT, yT，ΨT，kT），d4=|P3P4|，同时为了满足目标状态的航向角约束，则第3个控制点的坐标为

以上推导过程已经满足无人车的初始状态及目标状态约束（即四阶贝塞尔曲线起始点、终止点、3个控制点坐标），计算得到四阶贝塞尔曲线，对于其中的自由变量控制参数p=[d1，d4，x2] 进行调整则可以更改生成的4阶贝塞尔曲线走向或称扭曲程度。

2、车辆路径规划跟随原理

当规划形成了车辆路径后，就需要对车辆如何遵循该路径移动达到最终点，这里我们称车辆行驶规划为运动规划，其中包含两个层面的运动规划：

1） 基于横向的路径规划，横向规划上一般是指轨迹形状规划；

当横向规划已经确定后，可以将横向规划转化为QP问题，即对当前给定范围内的约束条件可以输出相应的最优化结果曲线。对于QP问题实际是通过设置在之前已经检测到的已知条件，然后通过一定滤波算法（比如卡尔曼滤波），求解未来的相关参数，而其中可以设置约束条件来不断优化相应的曲线设置。已知条件：x[t-2],x[t-1],x[t]; 求解：x[1],x[2],…约束：x[i]表示的每个点都不能超越左右边界，且当检测到一定障碍物车辆目标时，x[i]与目标的距离应该大于安全距离Dsafe。由此，相应的QP公式及其优化目标设置如下：

2） 基于纵向的路径规划，纵向规划则是对轨迹规划中的速度进行分配；

3、总结

++本文从自动驾驶轨迹规划和行驶运动规划中对相关原理进行了相应的分析，轨迹规划的输入包括拓扑地图、障碍物即障碍物预测轨迹、交通标志状态、定位、导航、车辆状态等其他信息。++轨迹规划本质上来说是一个优化问题，往往需要了解优化约束和优化目标。优化约束包括遵循交通规则，同时该规划在自车上的可实现性。优化的目标则是需要保证自动驾驶与人类司机驾驶无限相似。高级一点的方法是对老司机进行驾驶行为采样，从而为自动驾驶控制产生推荐性方法。