来自硅谷的无人驾驶一线技术

我们已经拉开了全自动无人驾驶的序幕，本文选自《第一本无人驾驶技术书（第2版）》，本书来自硅谷无人驾驶一线技术团队的实践经验，为无数读者揭开无人驾驶技术的神秘面纱 。

作为一个复杂的软、硬件结合系统，无人车的安全可靠运行需要车载硬件、传感器集成、感知预测，以及控制规划等多个模块的协同配合工作。

本文介绍的路由寻径模块（也称为寻径模块），其作用可以简单理解为实现无人驾驶软件系统内部的导航功能，即在宏观层面上指导无人驾驶软件系统的控制规划模块按照什么样的道路行驶，从而实现从起始点到目的地点的目的。

（值得注意的是，这里的路由寻径虽然在一定程度上与传统的导航类似，但其在细节上紧密依赖于专门为无人车导航绘制的高精地图，所以和传统的导航有本质不同。）

普通的谷歌或者百度导航解决的是从A点到B 点的道路层面的路由寻径问题。普通导航的底层导航元素最小可以具体到某一条路的某一个车道。这些道路和车道都是符合自然的道路划分和标识的。无人车路径规划的寻径问题，虽然也是要解决从A 点到B 点的路由问题，但由于其输出结果并不以为实际的驾驶员所使用为目的，而是给下游的行为决策和动作规划等模块作为输入的，其路径规划的层次要更加深入到无人车使用的高精地图的车道级别。

无人车路由寻径模块的高精地图道路级别路由寻径

上图的箭头线段代表高精地图级别的道路划分和方向。Lane1,Lane2,…,Lane8 构成了一条路由导航输出的路由片段序列。可以看到，无人车地图级别的Lane 划分并非和实际的自然道路划分对应。

例如Lane2、Lane5、Lane7 都代表了由地图定义绘制的“虚拟”转向Lane。类似地，一条较长的自然道路也可能被划分为若干个Lane（例如Lane3 和Lane4）。

路由寻径模块的输出严格依赖无人车高精地图的绘制。在高精地图定义的路网（Road Graph）划分的基础上，以及在一定的最优策略定义下，路由寻径模块需要解决的问题是计算出一个从起点到终点的最佳道路行驶序列：

{(lane,start_position,end_position)}

我们将 (lane,start_position,end_position)i 称作一个路由片段 （Routing Segment），所在的道路由Lane 标识，start_position 和end_position 分别代表在这条路由上的起始纵向距离和结束纵向距离。

和普通的谷歌或者百度导航不同，无人车路由寻径所考虑的不仅是路径的长短、拥塞情况等，还需要考虑无人车执行某些特定行驶动作的难易程度。

例如，无人车路由寻径可能会尽量避免在短距离内进行换道，出于安全考虑，短距离内需要的换道空间可能比正常的驾驶距离所需要的换道空间更大。从安全第一的原则出发，无人车路由寻径模块可能会给“换道”路径赋予更高的权重（cost）。

我们可以把无人车在高精地图的Lane 级别寻径问题，抽象成一个在有向带权图上的最短路径搜索问题。路由寻径模块首先会基于Lane 级别的高精地图，在一定范围内所有可能经过的Lane 上进行分散“撒点”，我们称这些点为“Lane Point”。这些点代表了对无人车可能经过的Lane 上的位置的抽样。这些点与点之间，由有向带权的边进行连接。

①换道场景中Lane Point 间cost 的设置

②无人车寻径基于Lane Point 的有向带权图上的

最短路径问题抽象

一般来说，在不考虑倒车情况时，Lane Point 之间是沿着Lane 行进方向单向可达的关系。连接Lane Point 之间边的权重，代表了无人车从一个Lane Point 行驶到另一个点的潜在代价。Lane Point 的采样频率需要保证即使是地图上被分割比较短的Lane，也能得到充分的采样点。Lane Point 之间的连接具有局部性。同一条Lane 上面的点是前后连接的，值得注意的是，不同Lane 之间的Lane Point 也有相互连接的关系。一个明显的例子是，在转弯时，转弯Lane 的第一个Lane Point 和其前驱Lane 的最后一个Lane Point 自然连接在一起。另外，两条相邻的平行Lane，在可以合法进行换道的位置（比如虚线位置），其对应位置的Lane Point 也可能互相连接。

图① 给出了换道场景中Lane Point 间cost 的设置：在任何一个Lane 的内部采样点Lane Point 之间，我们把cost设置为1；考虑到右转的代价低于左转，我们把直行接右转的cost 设置为5，直行接左转的cost 设置为8，右转Lane 内部Lane Point 连接cost 设置为2，左转Lane 内部LanePoint 连接cost 设置为3。在图①的换道场景中，两条平行可以换道的Lane，每条Lane 内部的连接cost 依然为1，但为了突出换道的代价，我们把相邻Lane 之间的连接权重设置为10。

按照图①设置的cost，在图②的一个路网（Road Graph）下，对比从A 到B两个可能不同的路由路径Route 1 和Route 2。其中Route 1 对应从L1 出发，在左下角的路口处直行接L4，之后右转（L5），再继续直行经过L10 和L11，最后直行经过L12 到达目的地；Route 2 对应同样从A 出发的L1，但在左下角的第一个路口处右转接L2，然后直行并且从L3 换道至L6，在右下角路口处经过L7 左转接直行（L8），最后在右上角的路口处右转（L9）进入最后目的地B 所在的L12。即使Route 2 的实际物理长度小于Route 1，按照图①设置的cost，无人车路由寻径也会偏向于选择总cost 较小的Route 2

（假设属于不同Lane 的Lane Point 之间的连接cost 除了图①所示外均为1，读者可以验证Route 1 的总cost 为22，Route 2 的总cost 为44）。

针对上文的无人车路由寻径有向带权图的最短路径问题，我们这里介绍一种常见的无人车路由寻径算法：Dijkstra 算法。

Dijkstra 算法是一种常见的图论中的最短路径算法，由Edsger W. Dijkstra 在1959 年发表。给定一个图中的源节点（Source Node），Dijkstra 算法会寻找该源节点到所有其他节点的最短路径。结合无人车路由的Lane Point 场景，算法的描述如下。

（1）从高精地图的路网数据接口中读取一定范围的地图Lane 连接数据，按照上文中所述进行Lane Point 抽样并构建Lane Point Graph。无人车主车（也称作Master Vehicle）所在Lane 上最接近无人车主车的Lane Point 为源节点，目的地所在Lane 上最接近目的地的Lane Point 为目的节点。设置源节点到其他节点（包括目的节点）的距离为无穷大（inf），源节点到自身的距离为0。

（2）当前节点设置为源Lane Point，设置其他所有Lane Point 为unvisited（未访问）并将它们放到一个集合中（Unvisited Set），同时维护一个前驱节点的映射prev_map，保存每一个visited 的Lane Point 到其前驱Lane Point 的映射。

（3）从当前Lane Point 节点出发，考虑相邻能够到达的所有未访问的Lane Point，计算可能的距离（Tentative Distance）。例如，假设当前Lane Point X 被标记的距离为3，LanePoint X 到Lane Point Y 的距离为5，那么可能的距离为3+5=8。比较该Tentative Distance和Lane Point Y 的当前标记距离。如果Lane Point Y 的当前标记距离较小，那么保存Lane Point Y 的当前标记距离不变，否则更新Lane Point Y 的当前标记距离为这个新的Tentative Distance 并且更新prev_map。

（4）对当前Lane Point 的所有连接的unvisited Lane Point 重复步骤（3）的操作，当所有相连接的Lane Point 均被操作过之后，标记当前的Lane Point 为visited，从unvisited的集合中去除。被visited 的Lane Point 的标记距离将不再被更新。

（5）不断从unvisited 的Lane Point 集合中选取Lane Point 作为当前节点并重复步骤（4），直到我们的目标Lane Point 从unvisited 集合中被去除；或者在一定范围内的Lane Point 均无法到达（unvisited 集合中最小的Tentative Distance 为无穷大，代表从源Lane Point 无法到达剩下的所有unvisited Lane Point）。此时，需要返回给下游模块没有可达路径（寻径失败），或者重新读入更大范围的地图路网数据，重新开始寻径的过程。

（6）当找到从A 到B 的最短路径后，根据prev_map 进行Lane 序列重构。

基于Dijkstra 算法的Lane Point 有向带权图上的路由寻径算法的伪码如下。

其中第2~16 行是典型的用Dijkstra 算法构建每个源Lane Point 到其他Lane Point 的最小距离表。从第 17~22 行，根据得到的每个节点标记的最小距离映射，通过不断查找前驱的prev_map 映射重建最短路径。注意这里的最短路径是一个Lane Point 的序列，在第23 行，我们对Lane Point 按照Lane 进行聚类合并最终生成如{(lane,start_position, end_position)i}格式的路由寻径输出。

假设根据上文的Lane Point 有向带权图生成方法的图有V 个节点和E 条边。在使用最小优先队列（minimum priority queue）来优化第10 行的最小距离查找的情况下，Dijkstra 的路由寻径算法复杂度可以达到O(丨E丨+丨V丨log丨V丨)。

其他：A*算法

另外，还有一种在无人车路由寻径中常用的算法是 A*算法。A*算法是一种启发式的搜索算法。A*算法在某种程度上和广度优先搜索（BFS）、深度优先搜索（DFS）类似，都是按照一定的原则确定如何展开需要搜索的节点树状结构。A*可以认为是一种基于“优点”（best first/merit based）的搜索算法。在《第一本无人驾驶技术书（第2版）》中会对此进行详细介绍。

在实际的无人车路由寻径计算问题中，更重要的往往不是算法的选择，而是cost 的设置策略。

上文描述的cost 调整是整个路由寻径策略的精髓，而具体的算法实现（Dijkstra 或者A*）并不是最重要的。例如，从地图信息我们得知某一条道路的某一条Lane非常拥堵，就可以把进入这条Lane 上的Lane Point 之间的连接权重cost 提高；类似地，如果某条Lane 被交通管制不能通行，我们可以相应地把这条Lane 上的Lane Point 设置为互相不可达，从而使得算法不会去选择某条特定的Lane。路由寻径的Lane Point 之间的cost 可以根据不同策略实时灵活调整，为无人车路由寻径提供支持。考虑到实际的路网数据往往较大，基于Lane Point 有向带权图的最短路径往往是在提前预先加载（preload）的部分地图路网数据上进行的。如果出现在较小范围内不可达的情况，则可能需要重新读入更大的路网和地图数据重新进行路由寻径。

对路由寻径模块产生路由计算的请求，有两种情况：一种情况是当无人车开始行驶时，由用户来设置起点和终点，从而触发路由寻径请求；另一种情况是，请求是由下游模块发起的。这里我们讨论“强Routing”和“弱Routing”两种系统设计思想。“强Routing”指的是下游模块（如行为决策及动作规划）严格遵守路由寻径模块的输出。例如，路由寻径模块要求按照某条Lane X 行驶，但感知发现Lane X 上有一辆行驶非常慢的障碍车，在强路由的设计下，无人车会严格执行在Lane X 上行驶；但在“弱Routing”的设计下，无人车可能会短暂跨越到相邻的Lane，超过障碍车辆，再回到Lane X 继续行驶。无论是“强Routing”还是“弱Routing”，当出现需要紧急避让，或者周围交通情况导致无人车无法执行当前的路由寻径结果时，无人车会按照安全第一的原则继续行驶，并且发起重新路由寻径的请求。