聊聊无人驾驶技术中的路由寻径

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

---

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

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

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

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

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

例如Lane二、Lane五、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 之间是沿着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”，当出现须要紧急避让，或者周围交通状况致使无人车没法执行当前的路由寻径结果时，无人车会按照安全第一的原则继续行驶，而且发起从新路由寻径的请求。

无人驾驶是一个复杂的系统，涉及的技术点种类多且跨度大，入门者经常不知从何入手。本书首先宏观地呈现了无人驾驶的总体技术架构，概述了无人驾驶涉及的各个技术点。在读者对无人驾驶技术有了宏观认识后，本书深刻浅出地讲解了无人驾驶定位导航、感知、决策与控制等算法，以及深度学习在无人驾驶中的应用等多个主要技术点。本书的做者都是无人驾驶行业的从业者与研究人员，有着多年无人驾驶及人工智能技术的实战经验。

了解本书详情：京东、当当