高精度地图领域的研究与应用进度（基于高精地图的路径规划探索）

概述：

目前以Tesla为首的高速领航功能让大家更加明确自动驾驶的前进方向和主机厂的迫切需求。而在高速领航功能中，又分为纯视觉方案和高精地图 视觉 雷达的方案。目前国内车企基本采用后者。本文主要讲述基于高精地图下的路径规划。

1. 根据道路级导航路线，实现车道级指引
2. 根据目标车道，寻找可供轨迹规划的参考线

1. 基于高精地图ADASIS V3 路网结构链表关系的求解
2. 基于高精地图道路几何形点的求解

基于高精地图ADASIS V3 路网结构链表关系的求解是目前普遍流行的做法。但是此方案强依赖图商生成的车道中心线。本人目前接触到的高精地图，均只保证车道中心线处于左右两侧Lane Marking的中值，其实并非是符合车辆驾驶的参考路径。

为此，本文提出一种基于高精地图道路几何形点的求解。

高精地图的道路形点一般为WGS84坐标系，可以理解为球形坐标，无法更好地满足车辆路径规划的使用，为此我们需要构建平面坐标系，UTM坐标系。

UTM坐标系原理图

由于UTM坐标系为绝对坐标系，而车辆自动驾驶其实更关注车辆前进方向的道路信息，故我们可以在规划的开始，基于此时车辆信息（Heading 经度 维度）作为原点，建立VCS坐标系。此时形点纵坐标数值大于0的数值，将大体为车辆接下来要行驶的道路。

需要注意的是：WGS84里的绝对Heading值，也需要在UTM坐标系中进行矫正，如下所示。

基于VCS坐标系的车道线形点，根据车道线属性可以得到如下的前方600m的道路模型。

栅格单元为：1m *1m

根据道路模型的Points，动态分段多项式原理拟合出所有曲线的曲线方程，求解出栅格地图。

基于上述栅格地图，我们就可以运用主流的搜索算法(Dijkstra A* ... )进行路径寻优。

由于车辆属于刚体，且车辆行驶在高速上是存在一定的规则，故我们需要对传统的搜索算法进行改进，那么就需要我们制定进一步的规则：

1. 车轮减少压线
2. 车辆减少变道
3. 车辆无法后退
4. 路径最短

那么在搜索算法扩展节点时，如不符合上述的规则，我们将会处于一定的惩罚，即增加此扩展节点的cost值。

以此保证搜索算法求出基于此规则下的最优粗略解。

粗略路径拟合

粗略路径并不车辆运动学的平滑考虑，故很可能出现较大曲率的情况，那么车辆是无法根据此路径进行跟踪的。

为此，我们需要做进一步的平滑处理。

1.宏观曲线平滑：

由于粗略路径是符合道路几何形状下的参考线。

往往人类在驾驶车辆时，并不会严格按照车辆中心线进行驾驶，那么此时我们需要按照人类的思维进行进一步的路径规划。

举个例子：

对于下匝道的场景，如果道路修建的几何形状是一个非常急的S弯，那么粗略路径也同样会生成非常急的S弯，此时我们仅仅对道路形点进行多项式拟合或者类似EM Planning中的QP二次规划，也需要非常多次的迭代求解，效果也并非理想。那么此时需要进行平滑甚至重新规划路线。

宏观曲线平滑规则：

直线处满足车辆处于车道中心

变道处满足路线曲率极小值较大

满足起点和终点接点处的G1连续

车辆安全距离 -- 车轮不能压到Border及Guardrail，并尽可能远离。

宏观曲线平滑效果：

2.微观曲线平滑：

图商提供的原始点，在节点处有时会存在存在突兀点，我们也需要进行微观平滑，保证车辆在控制时不存在抖动。

微观曲线平滑效果：

路径规划参考线的好坏，将直接影响后续决策规划效果，所以不考虑高精地图道路模型的连接关系，直接通过图搜索的路径规划求解，有更大的自由度和决策规划进行配合，该方法也未尝不是一个好办法。