Junior无人车框架-DARPA挑战赛

最新推荐文章于 2024-09-27 18:31:22 发布

qyshen815

最新推荐文章于 2024-09-27 18:31:22 发布

阅读量3k

点赞数 3

分类专栏：无人车论文阅读文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_35503971/article/details/104693677

版权

无人车同时被 2 个专栏收录

5 篇文章 5 订阅

订阅专栏

论文阅读

3 篇文章 3 订阅

订阅专栏

Abstract

本文展示了能够在城市环境下自动驾驶的轮式机器人Junior的工程结构。本车能够自主选定路线，感知并与其他交通工具进行交互，能够执行不同的驾驶技巧，包括换道、U型掉头、停车、汇流等，Junior取得了DARPA城市挑战赛亚军。

1 Introduction

目前交通条件下，事故频发，造成人员伤亡惨重，因此无人车应运而生，旨在降低事故率，提高交通安全。DAPRA挑战赛力图引导自动驾驶汽车的发展，先后举办了“DARPA大挑战”和“DARPA城市挑战赛”。

图1：Junior传感设备

城市挑战赛提供数字道路地图，即一种道路路网描述文件（RNDF），包括了道路标志线、停止线、停车位、和特殊检查点。同时参赛队还获得了高精度区域地图，使得他们能够在比赛前自主修改RNDF。比赛的任务是通过一系列的检查点给出的，所有的车辆都在同一环境下，同一时间，不同的速度限制下进行的，因此这些车辆在遭遇时，也必须遵守交通规则。特别的比赛项目有：通过静止或低速行驶的车辆、带有停止牌的情形处理、当道路完全封闭时的U型掉头。比赛中车辆速度限制在30mph，有些项目的时速要求更低。共有11辆车完成了比赛，Junior是其中之一。

Junior装备了5个测距仪（IBEO，Riegl，Sick和Velodyne生产）、一个GPS辅助惯性导航系统（Applanix）、5个雷达（BOSCH）、两台英特尔四核电脑、大众提供的线控驱动。最大探测范围120m，最高时速30mph。Junior通过分布式的软件架构实现车辆的决策。

图2：所有的计算设备和电源设备均放置在车辆后备箱

2 Vehicle

Junior是配有4缸柴油发动机的2006版帕萨特改装车。这辆140马力的汽车配备了一个有限扭矩转向电机，一个电子制动助力器，电子油门，换档器，停车制动，和转向信号。定制接口提供了所有这些功能的电子控制。通过一台大电流交流发电机向Junior的计算系统提供电力，并由装配电池的电力控制系统提供支持。为了开发调试，机舱配备了开关，使调试员能够随意使用各种电子接口组件。例如，开发人员可以让计算机控制方向盘和转向信号，同时保持手动控制油门和车辆刹车。但在实际比赛中，任何人都不允许进入车辆。

对于惯性导航，Applanix POS LV420系统提供多个双频GPS接收机的实时集成，其中包括GPS方位角测量子系统、高性能惯性测量单元，通过距离测量单元(DMI)的车轮里程测量，以及基于Omnistar卫星的虚拟基站服务。该系统的实时位置和朝向误差通常分别低于100厘米和0.1度。
两个面向侧面的SiCK LMS291-S14传感器和一个前端的RIEGL LMS-Q120激光传感器提供了临近路面三维结构和红外反射率测量，用于车道线检测和精确定位。

用于障碍物和移动车辆检测的Velodyne HDL- 64E安装在车辆的车顶上。Velodyne包含64个激光二极管和旋转频率最高15Hz，能够产生密集的范围数据覆盖360°水平面和30°垂直面。该Velodyne由安装在车辆后部的两个SiCK LDLRS传感器和安装在前保险杠上的两个IBEO ALASCA XT激光雷达作为补充。五个安装在前出风口的BOSCH 长范围雷达提供运动车辆的额外信息。

Junior计算机系统由两个英特尔四核服务器组成。这两台电脑都运行Linux，而且在千兆以太网网络上的交互。

3 Software Architecture

Junior的软件架构设计为数据驱动的管道，每个模块异步地处理信息。该软件架构之前在Stanley也成功部署过。每个模块间通过发布/订阅的通信协议（IPC 1998）进行交流。

通过模块的异步地发布和订阅实现了数据的一直处理，传感器数据到管道，再到车辆执行操作大概延迟是300ms，软件大致由一下5个模块组成：

sensor interfaces：
传感器接口分别与每个传感器进行交互，并使得其他软件模块能够获取传感器的数据。
perception模块：
感知模块将环境数据分割成运动车辆和静止障碍物，同时也提供相对于数字地图环境下的准确定位。
navigation模块：
该模块决定了车辆的行为，该模块整合了多个轨迹规划器、一个结构完备的有限状态机以解决和调用不同的行为模式，并且能够预防死锁。
线控接口：
通过线控发布控制指令，使得上层软件能够控制车辆的油门、刹车、转向、换挡、转弯信号灯、紧急刹车。
全局服务：
包括了一些系统级的模块，为车辆提供记录、时间戳处理、消息交互、看门狗等功能，确保软件运行的可靠性。

表1列出了比赛期间计算机上的进程，图3展示了模块间的数据流总览

进程名	计算	描述
PROCESS-CONTROL	1	开始和重新开始进程，通过IPC增加进程控制
APPLANIX	1	Apllanix interface（通过IPC）
LDLRS1 & LDLRS2	1	Sick LDLRS 激光雷达（通过IPC）
IBEO	1	IBEO 激光雷达（通过IPC）
SICK1 & SICK2	1	Sick LMS激光雷达（通过IPC）
RIEDL	1	Riegl 激光雷达（通过IPC）
VELODYNE	1	Velodyne激光雷达（通过IPC和公用内存），并通过Applanix 姿态信息投影3D点云
CAN	1	CAN接线
RADAR1-RADAR5	1	雷达（通过IPC）
PERCEPTION	1	Velodyne数据、障碍物检测、动态跟踪、扫描差分的IPC和内存公用
RNDF_LOCALIZE	1	RNDF的ID定位
HEALTHMON	1	记录计算机信息（温度、进程、CPU、内存使用情况）
PROCESS-CONTROL	2	通过IPC的进程管理
CENTRAL	2	IPC服务器
PARAM_SERVER	2	所有参数的中心服务器
EST0P	2	DARPA E-stop的IPC和序列接口
HEALTHOM	2	监控所有模块情况
POWER	2	电源管理服务器的IPC和序列接口（继电器卡）
PASSAT	2	车辆接口面板的IPC和序列接口
CONTROLLER	2	车辆motion控制器
PLANNER	2	路径规划器和混合A*规划器

图3：数据流图

4 Environment Perception

Junior的感知模块解决了一系列的障碍物检测和跟踪的问题，图4a显示了Velodyne的扫描结果，图4b显示了IBEO激光雷达的扫描结果，LDLRS激光雷达用于补充Velodyne的盲区。雷达系统用于十字路口环境中的运动物体的预警系统。

图4：a图是64线Velodyne激光雷达10Hz旋转下的扫描结果，每次扫描能够探测60m的距离，b图是IBEO扫描出的四条主要地面平行线，其特点正是能够探测大的垂直物体，比如车辆和指示牌。

4.1 Laser Obstacle Detection

在城市环境下，车辆会面临大量的静止和运动的障碍物，小的障碍物如路沿也会阻碍车辆的行驶，因此探测小的障碍物对于无人驾驶来说非常重要。悬崖和树远远看起来可能是大的障碍物，但车辆却能够在他们下面通行，因此障碍物的检测也必须考虑3D几何结构。图5展示了城区环境下的典型障碍物探测结果，红色的物体对应障碍物，右下角的相机图用于环境参考。

图5：检测结果如a图所示，b图是Velodyne的点云数据，两幅图都检测到了路沿

Velodyne是主要的障碍物检测传感器，一个简单的检测算法是找到相似x-y坐标的点，并且他们的垂直平面坐标之差超过一定阈值。该算法可以用于检测行人、标志牌、和车辆。然而，该该传感器的检测范围和校准误差较高，使得垂直平面的阈值不能设置太低，否则在工程实践中，将会误检大量的路沿大小的障碍物。

比较垂直差的另一种办法是比较相邻射线返回的范围，相邻程度衡量的是射线间的夹角。64条激光中的每一条都有相对于车辆坐标系的俯仰角，因此当传感器旋转的时候，会扫描出一个固定半径的圆圈。斜坡会局部压缩这些圆环，导致他们之间的距离比在平地扫出来的圆圈更小。举个极端的例子，垂直的障碍物甚至会使得传感器扫出来的距离相同。正是由于扫描地面的激光都处于一个较低的俯仰角，因此圆环之间的距离差能够比垂直平面的坐标差更精确地。所以通过找到那些产生圆环差与预期不同的点，就可以探测出在垂直平面坐标差不能明显被判断的障碍物。

除了地面斜坡影响，车辆的俯仰、滚转角度的改变，都会导致这些圆环发生压缩或者扩展。如果不考虑这个影响，当车辆向右滚转的时候，可能会把右侧的平地当做障碍物处理。该问题的解决办法是，把下一次圆环的期望距离当做距离的函数，因此当车向右侧发生滚转时，离车越近的激光射线的期望距离差将会越低，如此，小的障碍物也能在车辆发生滚转和俯仰时检测到。

在城市环境下，有两个问题需要强调：第一个是树和其他悬挂不影响车辆安全行驶的物体不应该被当做障碍物，悬挂的物体可以通过与简单地面模型进行比较而滤除，也就是落在特定xy平面的最低高度检测点，超过了同一栅格中最低高度的阈值（车辆的高度加上安全缓冲区间）就被当做是安全的悬挂物体。第二个是Velodyne的车后检测盲区，这是由于传感器的几何形状和安装位置导致的，进一步来说，就是由于车辆的自我阻挡，使得传感器不能立即检测车辆附近的小障碍物比如路沿等。这里使用IBEO和SICK LDLRS传感器作为补充，两种传感器本质上是二维的，因此不能从垂直平面区分障碍物，也只能在非常短的距离下探测到障碍物。当两个传感器的其中之一在比较近的范围内探测到障碍物（LDLRS为15m，IBEO为5m），那么检测到的物体就标记为障碍物。近距离的2D和远距离3D组合带来了高可靠性。我们注意到尽管设计距离是100+m的IBEO用5m的的距离来判断看起来非常悲观，然而在非平坦的路段、泥巴路该传感器会带来非常多的误检。

我们的检测方法意外的好，在比赛中我们没有碰撞过障碍物，更没有压过路沿。误检的障碍物也非常的小，在这种程度下，可以说我们的的静态障碍物检测完美无瑕。

4.2 Static Mapping

在许多场景下，静态环境地图生产必须使用多种不同时间段的测量结果整合。比如在停车场，遮挡或者距离限制都会导致漏检。在车辆附近的盲区检测也需要使用多种测量结果的整合，特别地，路沿超过了Velodyne激光雷达探测的最小范围。为了解决这些问题，Junior将所有传感器结果缓存至局部地图，图六显示了随时间累计的多个传感器结果生成的局部地图，不同的颜色区分不同的障碍物类型，完整的地图更新规则依赖于累积证据的标准贝叶斯框架（Moravec，1988），该方法能够滤除仅在少出测量结果中出现的小障碍物。

图6：停车场地图，黄色为高障碍物，棕色为路沿，绿色为安全的悬挂障碍物（如树枝）

随着时间的累积，静态地图中逐渐消失的就是运动的物体，比如一个通道可能会被阻碍一段时间，随后又可能变为可通行了。为了适应这种情形，软件系统进行了局部可视化计算，在每个极性方向，从最近被检测到的物体到车辆之间的范围被视为自由区域，当然除了第一次检测到的障碍物，不能说该遮挡下没有进一步的障碍物。地图只会在该自由区域内更新，但在工程经验中发现，该地图更新机制在实际中运用效果很好，比如在停车场也能提供很好的导航效果。图7展示了Velodyne传感器扫描下的自由区域。

图7：Velodyne扫描结果的自由区域分析，绿色线覆盖的区域代表观测下的空旷区域，这一结果被纳入到静态地图中，以蓝色和黑色显示。

4.3 Dynamic Object Detection and Tracting

城区驾驶的关键挑战是其他交通工具的检测，Junior的软件框架提供了一个基于粒子滤波的可靠运动障碍物检测和预测。

运动物体检测是合成2D环境扫描结果下进行的，这种扫描是从各种激光传感器合成的，通过沿着均匀间隔的合成距离传感器阵列将范围提取到最近检测到的障碍物，这种合成扫描的方法带来了许多原始传感器数据不可比拟的优势。首先，其紧凑性运行高效的计算，其次该方法可以应用于三种距离的传感器（Velodyne、IBEO、SICK LDLRS）中的一个，或者他们的组合。第二种优势是由于三个传感器都能够轻松地投影合成为2D，从而使扫描表示结果和特定传感器无关。这种合成方法提供了一种统一的探测、跟踪、预测运动物理的方法。图8a显示了该合成扫描结果。

图8：a：Velodyne数据的合成2D扫描结果；b：扫描差异提供了区域的变化信息，这里用绿色和红色表示；c：其他车辆的跟踪；d：对应的相机图像

运动障碍物的跟踪主要有两个阶段，首先是标记变化的区域，通过比较短暂时间间隔的两幅合成结果，如果一次扫描中障碍物落入了另一次扫描的自由区域，那么可以认为该障碍物是运动的，图8b展示了这种场景，红色展示的是新出现的障碍物，绿色表示在先前已经被检测出的障碍物。当这种初步找到这种运动的物体后，跟踪器初始化一系列可能是该物体的粒子。这些粒子以不同的速度和位置代表了不同尺寸的矩形，然后用一种粒子滤波算法来跟踪这些运动的物体，通常在三次以内的观察，改滤波器就能锁定并可靠地跟踪运动目标了。

图8c展示了跟踪结果，图8d显示了同一场景下的相机图像。该跟踪器能够估计物体的位置、航偏角、速度和大小。

5 Precision Localization

Junior的另一个感知类关键模块是定位。之前提到，比赛时提供了RNDF文件，然而RNDF文件所有数据都是在GPS坐标系下，Applanix系统的解算结果没有足够的精确度来使得车辆在没有传感器反馈下的道路保持。并且RNDF本身有些小错误，如果使用Applanix的姿态解算并参考RNDF，可能会带来更大的误差。因此Junior使用本地传感器测量结果与RNDF进行局部对准，换句话说，就是使用传感器来获取参考于RNDF的定位。

细粒度的定位主要使用了两类信息：道路的反射率和路沿状的障碍物。反射率使用指向地面的RIEGL LMS-Q120和SICK LMS传感器获得。图9展示的是侧面安装的SICK传感器获取的反射率信息随着时间不断整合的结果。图中显示了车道线的红外反射率变化。

图9：侧面传感器提供的强度信息用来与RNDF进行概率匹配从而获得精确定位

定位滤波器是一个1D直方滤波器，用来估计车辆相对于RNDF的横向偏差。该滤波器估计的是基于反射率和可见路沿的横向误差的后验分布。其会以概率形式“奖励”那些和RNDF对准的车道线或者路沿偏差，“惩罚”进入RNDF安全走廊的路沿的横向偏差。因此，在任何时间点，车辆都会估计相对于基于GPS的惯导系统测量结果的细粒度偏差。

图10展示了测试时获取的相对于RNDF的定位信息，图中绿色的曲线显示了两种激光雷达传感器的车道线较为可能的定位信息，黄色的曲线显示的是横向上的后验分布。该后验分布相对于Applanix的解算结果误差大约80cm，如果不考虑该误差，那么在定位中，车辆后轮将会在中心先上。比赛过程中，1m的定位误差非常普遍，没有定位这一步，Junior很有可能驾越中心线或撞上路沿。

图10：典型的定位结果：红条显示的是Applanix定位结果，黄色曲线测量的是车辆横向位置的后验结果，绿色的线显示的是车道线检测器的响应，此结果下误差大约80cm

最后图11比赛期间显示了横向误差校正的分布图

图片11：比赛期间平均定位偏差校正值的直方图，由图可知，横向误差偶尔会超过1m

5.1 Smooth Coordinate

在随时间不段整合多种传感器的测量结果过程中，使用惯导系统的姿态解算（Applannix的输出结果）来计算不同测量之间的相对误差可能非常诱人，但任何精确的惯导系统都会频繁出现由于GPS测量造成的位置跳变。这是因为惯导系统提供的当前时间最有可能的位置信息，当新的GPS信息响应时，可能会得到与车辆运动信息不一致的位置信息变化。解决该问题即使得这种位置信息修正发生时，过去的惯导系统测量结果也必须进行修正，以便产生一个连续的地图。该问题在定位估算领域被称为（向后）平滑问题（Jazwinsky，1970）。

为了缓解该问题，Junior维护一个对该跳变鲁棒的内部的光滑坐标系。在该坐标系系统下，机器人的位置被定义为所有递增的速度变化总和即：
$\overline{x} = x_0+\sum _{t} \Delta t \cdot \dot{x_{t}}$
其中 $x_0$ 是第一个惯导坐标， $\dot{x_{t}}$ 是惯导给出的速度估计。在该内部坐标系统下，突然的惯导位置跳变不会带来任何影响，并且传感器的数据始终局部连续。车辆的速度估计值比位置估计值更加稳定，即使GPS信号间歇出现或者不可用时。X和Y轴的速度特别地不容易跳变，因为他们分别使用轮速计测得。

该平滑坐标系方法使得在GPS飘逸情况下也能获得局部连续的地图，但是值得注意的是，随着时间间隔的增加，该方法也会导致不连续，因此只能用于解决局部地图问题，但这并不影响我们的实践效果，因为局部地图只有在navigation过程中才会使用到。

在软件执行过程中，原始的（全局）和平滑度（局部）坐标系之间的映射，只需要通过获取所有估计值飘逸的和，而这种映射在初始值通常为0。然后通过增加实际惯导系统和基于速度测量值的偏差来迭代更新这种校正。

6 Navigation

6.1 Global Path Planning

导航的第一步就是全局路径规划，在每个新的检查点处、永久的道路封锁导致的路网拓扑结构变化都会调用全局路径规划。然而，全局路径不只会规划一条到下一个检查点的路径，而是规划地图所有点到下一个点的路径，从而当车辆偏离最优路径时，就不会丢失方向。

Junior的全局规划器是动态规划（DP）的一个实例（Howard，1960），该算法迭代的计算离散RNDF下每个从当前点到目标点单元的累积代价。代价的迭代更新方程采用的是文献中的标准算法，另 $V (x)$ 为RNDF中离散位置的代价， $V (x) = 0$ ，那么下面的递归方程定义了备份、隐含的累积代价函数：
$\leftarrow {\underset {u}{\operatorname {min}}}c(x,u)+\sum_yp(y|x,u)V(y)$
这里的 $u$ 表示一个动作，比如在特定的路段行驶，大部分情况下，只能允许有一个可执行的动作。然而在路口中，却有许多的动作可以选择，比如直行、左转等等。有多条车道可以选择时，也就会有换道动作。上述公式的最大动作选择提供多个项，其中最小的项可以推算出最短、最快的路径。

在工程实践中，并非所有的动作选择都能成功，比如，换道需要考虑安全性，这是使用迁移概率 $p (y ∣ x, u)$ 所考虑到的。举个例子，Junior评估给定位置的换道成功率低至10%，这样做的好处是惩罚换道行为，并把换道当做最后的动作考虑。事实上，当Junior超过一辆行驶速度缓慢的车辆时，早期会有较高的概率换道，并且较低的概率加速。

该迭代方程的主要组成部分是代价 $c (x, u)$ ，大部分情况下，该代价就是在离散RNDF下相邻两个单元格之间运动的时间。同时，规划过程也考虑了车辆的速度限制，其结果会选择一个到达时间最短的路径。而交通灯下的左转将会用增加时间和扩大风险的方式进行考虑。代价函数将会取得仔细地导航时间和风险的平衡，通过绕道的方式避免左转。其他动作的额外代价可以通过手工设置，也可以通过仿真数据学习。

图12：全局路径规划：在粗糙的离散地图上进行动态规划，图中的颜色代表了每个点到目标点行驶的代价，低代价的表示为绿色，高代价的表示为红色

如图12所示，规划不断累积代价函数，用不同的颜色表征累积代价，全局函数用来评估每个点，并没有考虑到传感器的即时影响。

6.2 RNDF Road Navigation

实际的车辆导航分别按照通常路段的导航和停车场这类的自由空间导航。

图13是通常路段的可视化展示，对于每一条车道，规划器都会规划出一条平行于车道中心线的光滑路径，该光滑路径是直接根据RNDF计算出来的，同时，规划器还会还会规划出横向换道的路径，每一条轨迹都是不同转向参数下的仿真结果。每一条轨迹的评分都会考虑保持该轨迹的时间（当轨迹被障碍物挡住时，该时间是无穷）加上轨迹末尾处的轨迹点的全局路径规划出的累积代价。选择出的轨迹，应该是评分高、代价最低的轨迹。

图13：规划器规划出可供选择的多个路径。a图每条车道都会规划出不同换道结果的轨迹。b图是在进入路口时两种选择，即左转或右转，车道使用DP的代价函数进行了涂色，红色表示高代价，绿色表示低代价

图14展示了低速移动的车辆阻挡了右侧车道的情景的决策过程，即使换道只会带来较低的惩罚，左侧车道所能提供的快速通行条件促使车辆最终做出向左换道的决定。当超车过程结束后，车辆最终会再次驶回右侧车道。

图14：超车场景，在全局最优的路径下，长时间在一个慢速车辆后行驶的代价超过了非全局最优路径的代价，最终会导致车辆做出换道、超车行为。

我们发现，路径规划器在交通规则约束下运行效果显著，并使得车辆沿着无障碍物的路径持续地、平滑地行驶。同时，该规划器也会避免延伸到道路内的障碍物，比如路边停靠的车辆。然而该方法并不能解决封锁的道路或者路口，也不能导航车辆进入停车位。

6.3 Free-Form Navigation

对于像停车场这类的自由形式导航，车辆使用的是第二种规划器，产生于道路结构无关的绝对轨迹。该规划器需要一个全局坐标系和地图，它确定的是一条代价最优的路径。

该自由形式的规划器是A*算法的改进版，我们称之为hybrid A*。Hybrid A*在4D离散栅格图中表示车辆状态。其中两个维度表示在光滑地图坐标系下车辆中心点的x-y坐标。第三个维度是车辆朝向 $\theta$ ，第四个维度表示的是车辆的运动方向，前进或者倒车。

一般的A*算法主要的一个问题是，离散规划结果并不能用于车辆的直接执行，因为世界是连续的，但是A*的状态时离散的。为了解决该问题，hybrid A*算法将连续车辆坐标系分配到每个离散的单元格，这样的连续车辆坐标可以用于车辆的实际执行。

为了展示其工作原理，令 $（x,y,\theta）$ 代表当前车辆的坐标，假设在离散A*的表示中，这些坐标都分布于单元格 $c_{i}$ ，根据定义，和单元格 $c_{i}$ 相关的连续坐标为 $x_{i}=x,y_{i}=y,\theta_{i}=\theta$ ，在该单元格下，预测车辆给定控制输入 $u$ 一段时间后的状态。假设预测结果为 $（x\prime,y\prime,\theta\prime$ ，并且该预测结果落入了不同的单元格，表示为 $c_j$ ，如果该单元格是第一次扩展，那么该单元格状态会被赋值为 $x_j=x\prime,y_j=y\prime,\theta_j=\theta\prime$ ，该赋值结果表明了通过给定的控制输入 $u$ 就能控制车辆移动到该单元格，而在传统的A*算法中，这样的连接是无法实现的。虽然hybrid A*算法可以保证找到的路径是可靠地，但却不能保证每次规划都能找到路径。特别地，越粗糙离散，越容易失败。

图15将hybrid A*、普通A*以及Field D*（Ferguson and Stentz）做了对比。Field D*也考虑了状态空间的连续性。普通的A*的结果很难直接用于车辆执行，即使是更加平滑的Field D*规划结果，也存在难以用于车辆执行的结点。然而，hybrid A*则可以直接用于执行。

图15：搜索算法结果的比较图。左边：A*算法将单元格的中心点和代价挂钩，并且只会考虑栅格的中心。中间：Field D*算法，同样用单元格中心评价代价，但允许任意位置的的单元格间的连线。右边：每个单元格与连续的状态挂钩，用连续的状态评价代价。

A*算法的代价函数使用执行时间作为指标。我们在实现过程中，给倒车分配了比较高的代价值来激励车辆尽可能多地正常行驶。改变行驶方向也会导致额外的代价，并且我们增加与最近障碍物的距离来激励车辆尽可能远离障碍物。

我们的搜索算法由两套启发式函数构成，分别是非完整无障碍物的启发函数和完整地含障碍物的启发。第一套忽略障碍物但考虑车辆非完整的性质，因此可以完全地预先在4D空间内计算，通过给定车辆朝向的方式帮助驶向目标。第二种启发方式是第一种启发方式的对偶形式，忽略车辆的非完整性质，但是计算到目标点的最短距离。该启发函数是在线调用的，使用的是2D（忽略朝向和运动方向）动态规划。两种启发函数都是可行的，因此两个的最大值都能用来规划。

图16：a图：欧氏距离启发函数扩展了21515个节点；b图：非完备不考虑障碍物的启发显著提高了效率，只扩展了1465个节点；c图：非完备不考虑障碍物的启发造成的在死胡同情景下的扩展浪费；d图：通过完备的考虑障碍物的启发函数可以进行结果修正

虽然hybrid A*算法的规划结果已经可以用于车辆的执行，但是少数情况也会出现转向角的突变，其结果是导致过度的转向。后端通过共轭梯度（Conjugate Gradient）平滑器进行优化，修正局部控制和运动点，同时也考虑的是最小的转向轮运动，和最小的弯曲。

图17：共轭梯度（Conjugate Gradient）路径平滑，该平滑器使用车辆模型保证结果可以用于执行，黑色结果是hybrid A*规划的，蓝色是前轴的平滑路径，青色是后轴平滑路径，最优的路径比hybrid A*规划的更加平滑，可以用于车辆的快速执行。

Hybrid A*规划器用于停车场和特殊场景规划，如U型掉头。规划使用计算机单核，最高规划100毫秒。

图18：Junior的hybrid A*规划器使用案例。a-c图是DARPA挑战赛的轨迹，a、b展示的是U-turn场景，c是停车场场景。d是在类似于迷宫地图的仿真结果。注意：当传感器感知的障碍物变化时，需要重新进行规划，这也解释了d情景下不够最优的原因。

6.4 Intersection and Merges

在路口内，需要做出多种选择，不包括在Navigation模块内。Junior处理思路是追踪临界区域，比如在路口会有一个区域用于应对停止标志牌。在汇流时，会有一个区域用于考核是否能够进行汇流。这些区域可以基于RNDF进行标注。

图19：临界区域，a图是四路停车标志，红色表示忙碌的临界区域，绿色表示没有车辆的临界区域。图中表示车辆可以从右侧驶出。b图是汇流情景下的临界区域。