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

  编辑 | Natalie

我们已经拉开了全自动无人驾驶的序幕,本文选自 《第一本无人驾驶技术书(第 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”,当出现需要紧急避让,或者周围交通情况导致无人车无法执行当前的路由寻径结果时,无人车会按照安全第一的原则继续行驶,并且发起重新路由寻径的请求。

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

扫描下方二维码,回复“无人驾驶“参与抽奖

此次活动我们将共计送出八本《第一本无人驾驶技术书(第二版)》

活动时间:文章发出至 10 月 20 日 18:00 截止

中奖联系方式: 中奖用户,填写邮寄地址后,小编会在一周内将实体书包邮寄到你的手中~

另附购买地址,请戳「阅读原文」

雪球转发:0回复:0喜欢:0