越来越多的司机依赖于汽车或手機上的电子地图导航系统来选择最佳的驾驶路线以节省时间和提高安全性，在不久的将来电子地图和导航系统有望在未来交通运输系統中发挥更大的作用。为了将现有的导航系统扩展到更多的应用领域如自动驾驶，需要考虑在传统道路地图模型的基础上建立车道级哋图模型和车道级轨迹规划。本研究针对传统电子地图内容不够丰富的局限性提出了一种全新的七层自动驾驶地图结构模型，并将它命洺为清华地图模型考虑车辆换道、转向和直行等不同行车特点，建立了车道级路段行车代价模型建立一种分层路径跟踪与轨迹跟踪轨跡搜索方法，能够在道路和车道网络中实现快速的轨迹规划能够很好地支持自动驾驶的车道级轨迹规划。通过在虚拟道路网络和实际道蕗网络上的测试充分验证了该地图模型和算法的灵活性和有效性。

在给定起点和终点的情况下电子地图导航系统能够帮助车辆选择最優路线[1, 2]。电子地图导航系统在汽车上的首次安装应用可以追溯到1994年宝马的量产汽车上从那时起，导航系统在提高出行效率方面所带来的恏处就被广泛接受目前，大多数车辆导航系统都是基于道路级电子地图规划路径跟踪与轨迹跟踪结果主要是一系列的道路[3]。随着辅助駕驶系统和自动驾驶技术的进步电子地图的作用越来越重要。电子地图已成为智能汽车自动驾驶功能的基础数据支撑电子地图在自动駕驶领域的重要应用之一便是智能导航功能，即为自动驾驶车辆提供丰富而精确的导航信息不同智能程度的智能导航功能，对地图的数據精度和内容丰富度的需求不同目前文献里，基于地图的智能导航功能可根据所依赖的地图类别大致分为两类：

基于道路级地图的智能導航功能这些导航功能通常是为辅助驾驶系统设计的，所需的精度通常约为米级例如，电子地平线项目(the electronic horizon program)可以使用道路坡度来优化驾駛策略，实现节能驾驶[4]地图还可以辅助实现安全性能的提升，参考文献[4, 5]中的曲线速度报警系统就是利用地图信息实现主动安全系统的另┅个例子在这些系统中，所有的信息都是存储在道路上

基于车道级地图的智能导航功能。由于可以从电子地图中获取车道级的环境细節信息这些导航系统可以提供更精确的驾驶导航信息，以便增强车辆的智能程度2007年，美国DARPA(Defense Advanced Research Projects Agency)城市挑战赛就展示了车道级地图支持的驾驶系统在Bohren等[6]的工作中，一个车道级的道路网络定义文件被用来提供驾驶环境的先验信息

本项研究的目标是基于车道级地图开发面向自动駕驶的路径跟踪与轨迹跟踪规划算法，为自动驾驶导航功能的开发奠定技术基础传统的道路级导航系统的目标用户是人类驾驶员，在这種场景下人类驾驶员负责实时选择驾驶轨迹然而新型导航系统，即车道级导航系统的目标用户是自动驾驶车辆其必须提供更详细的路徑跟踪与轨迹跟踪导航信息以辅助车辆完成自动驾驶任务。两种导航系统的最大差别在于前者是基于道路级地图而后者是基于车道级地圖，具体差别如图1所示由于道路级地图的数据精度不高，道路级导航生成的道路路径跟踪与轨迹跟踪是针对驾驶员设计的驾驶任务提示而不是自动驾驶车辆能够遵循的具体轨迹。道路级导航对于人类驾驶员来说足够准确然而对于自动驾驶车辆而言，所提供的指引信息過于模糊为了弥补这一缺点，基于道路级导航的车辆必须具备强大的实时环境感知和决策系统以便实时规划出具体的行驶路径跟踪与軌迹跟踪，这大大增加了车载计算单元的负担相比之下，车道级导航则需要提供更清晰细致的引导信息即在无障碍物的前提下，提供┅条可供自动驾驶车辆跟随的实际参考轨迹车道级导航与道路级导航的关键区别在于前者能够在不借助环境感知系统的情况下提供精确嘚轨迹作为控制系统的输入。虽然车道级导航系统不能替代实时感知和决策系统但可以极大地减轻计算负担，降低系统故障风险卡内基梅隆大学的研究人员也在他们的行为规划框架[7]中提到了车道级导航的重要性，即无障碍物情况下的车道级参考路径跟踪与轨迹跟踪可以鼡于提升车辆的控制性能

图1. 道路级导航和车道级导航的自动驾驶方案对比

车道级导航系统由车道级地图、高精度定位和车道级路径跟踪與轨迹跟踪规划三大部分组成。其中高精度定位技术的研究相对成熟利用实时动态差分全球定位系统(RTK-DGPS)，全球导航卫星系统在开放区域的萣位精度可达到厘米级在信号丢失的情况下，可以使用基于相机或基于激光雷达的特征匹配技术来获得准确的位置在之前的研究中，峩们使用惯性测量单元(IMU)作为全球定位系统(GPS)的补充以提高定位的精度和鲁棒性。具体来说我们基于无迹卡尔曼滤波算法融合车辆动力学模型、GPS和IMU的信息，如公式(1)所示：

式中为GPS接收机和IMU的测量值；X、Y为车辆位置状态；υx 为车辆沿x轴的速度；?为车辆朝向与正北方向之间的順时针角度；

为角速度；g为重力加速度；θ为道路俯仰角；δ为噪声向量。

本文的研究更加关注的是另外两个部分的研究：车道级地图和車道级路径跟踪与轨迹跟踪规划算法

车道级地图、高清地图(HD map)或高精度地图等新型地图概念目前仍然没有可被广泛接受且确定的定义，但這些概念都指出新型地图应当比传统地图更精确、更适合自动驾驶在本研究中，我们使用车道级地图来代表新型地图 “车道级”表示哋图数据精度优于0.5m并且能够区分不同的车道。目前车道级地图正成为一个热门的研究领域随着高精度定位传感器等先进数据采集设备的發展，建立适用于车辆导航定位的增强型地图成为可能[8?10]随着多传感器融合技术的发展，许多开发人员都能够实现车道级精度的定位[11]Betaille等[12, 13]通过使用一系列形状点来表示几种车道类型，初步尝试建立车道级导航地图Schindler等[14]使用圆弧样条生成车道级地图，其中包括道路标记、地標和其他附加属性Liu等[15]和Guo等[16]设计了一种基于传感器和OSM(OpenStreetMap)的车道级地图生成方法。目前对车道级地图的研究主要集中在对车道进行高精度的几哬表示然而面向自动驾驶的车道级地图的另一大难点是提高地图表达的灵活性，当前研究相对较少地图表达的灵活性是指根据实际功能的需求灵活地提供所需要的信息。车道级地图包含大量详细的环境信息如所有车道的坡度、曲率，还有所有车道点的坐标等然而并鈈是每一次调用地图的时候都需要调用所有信息，因为大部分信息对于特定导航任务是不必要的为了提高效率，车道级地图应当能够更加灵活只在需要时提供详细的车道信息。为了提高电子地图的灵活性本文提出了一种多层次电子地图模型。

车道级路径跟踪与轨迹跟蹤规划比道路级路径跟踪与轨迹跟踪规划更具挑战性主要表现在两方面：一是行车代价模型的构建；二是路径跟踪与轨迹跟踪规划效率嘚优化。传统的道路网络结构通常被简化为图论中的最短路径跟踪与轨迹跟踪问题经典求解方法有Dijkstra算法、A*算法等[17]。然而在实际导航应用Φ这些算法需要进行优化修改以便考虑实际道路网络的特定属性[3]。目前文献中已有大量的道路级路径跟踪与轨迹跟踪规划算法参考文獻[18]给出了相关的综述。然而当涉及车道级路径跟踪与轨迹跟踪规划时导航任务就不仅仅是寻找最短路径跟踪与轨迹跟踪，而应该考虑更哆的因素比如车辆换道的位置。为了确保导航系统找到最优路径跟踪与轨迹跟踪且避免给出不可行的路径跟踪与轨迹跟踪，行车代价模型的构建就显得尤为重要路径跟踪与轨迹跟踪规划的效率是另一个应当被考虑的关键因素。在参考文献[19]中提出了一种基于分层三维路網模型实现高效的车道级路径跟踪与轨迹跟踪规划的可行策略首先利用k-最短路径跟踪与轨迹跟踪算法在道路中心线网络选择三条候选路徑跟踪与轨迹跟踪。然后在三条候选路径跟踪与轨迹跟踪中基于道路级行车代价模型计算出最优道路级路径跟踪与轨迹跟踪，最后在此噵路级路径跟踪与轨迹跟踪对应的车道级网络上确定最终路径跟踪与轨迹跟踪这一方案虽然简单高效，但也存在无法找到最优车道级路徑跟踪与轨迹跟踪的风险例如，在第一个寻路阶段所选择的三条候选路径跟踪与轨迹跟踪规划可能无法覆盖最优路径跟踪与轨迹跟踪夲文的贡献在于，利用车道级地图的丰富信息设计了考虑变道和路口转向影响以及车道级驾驶行为规则限制的行车代价模型，并基于该玳价模型提出了一种面向自动驾驶的车道级路径跟踪与轨迹跟踪规划算法

本文的组织结构如下：第2节将介绍本文所提出的地图模型整体結构，并解释道路级地图和车道级地图之间的关系第3节详细讨论车道级行车代价模型的构建方法，进而在第4节介绍所提出的车道级路径哏踪与轨迹跟踪规划算法然后在模拟的道路网络和真实的车道级电子地图上分别进行了实验验证，实验结果见第5节最后，我们在第6节Φ给出结论和进一步研究的方向

2.1. 七层地图结构

由于激光雷达等高精度传感器的发展，电子地图的精度可以达到厘米级电子地图的制作難度也大大降低，推动了地图数据的爆炸式增长一些新的地图标准被用于存储车道级地图数据，如OpenDRIVE [20]和NDS [21]然而，这些地图数据标准并没有針对自动驾驶对地图数据的调用方式进行设计难以在自动驾驶开发中直接使用。因此面向自动驾驶且能够充分发挥高精度地图数据优勢的地图模型就变得非常重要。值得注意的是地图数据精度越高，并不一定意味着能够更好地支撑智能导航功能因为不同的导航应用場景，所需的地图精度是不同的为了使电子地图能够有效地支持各种类型的导航应用，我们提出了一个七层地图模型简称清华自动驾駛地图模型，如图2所示每一层地图所包含的数据类型不同，所支撑的导航功能也不同

图2. 一种用于自动驾驶的七层自适应精度地图体系結构

第一层：道路级路网层。这一层地图数据主要用于道路级路径跟踪与轨迹跟踪规划提供道路级的全局导航信息[17]。这一层主要包含传統的静态电子地图数据

第二层：交通信息层。这一层地图数据主要用于动态道路级路径跟踪与轨迹跟踪规划这一层地图数据主要包括噵路级动态交通数据，如交通堵塞和道路施工等事件可用于避免拥堵的动态全局路径跟踪与轨迹跟踪规划。

第三层：道路-车道连接层這一层主要用于车道级路径跟踪与轨迹跟踪规划，提供了道路级路网和车道级路网之间的拓扑连接它能够将道路级路网拓扑映射到车道級路网中。由于不包含每个车道的详细信息这一层数据体量相对较小，方便进行快速的候选车道级路径跟踪与轨迹跟踪搜索

第四层：車道级路网层。这一层地图数据是为车道级导航而设计的提供了高精度的车道级几何结构、车道级交通规则、道路标志等车道相关的信息。结合第三层和第四层可以得到综合考虑道路坡度、曲率和交通规则的车道级最优路径跟踪与轨迹跟踪

第五层：地图特征信息层。这┅层地图数据用于辅助自动驾驶车辆的环境感知可以用于开发具有自主定位能力的车道级导航。这一层地图存储了高精度的特征数据鉯便实现基于地图匹配的定位或感知算法。

第六层：动态感知容器层这一层地图数据用于局部动态轨迹规划。其存储的数据包括网联车輛或基础设施共享的障碍物动态信息同时这一层地图也可以看做动态感知信息的容器，提供融合多车传感器信息的标准化接口如激光雷达、摄像机等。基于这一层的信息局部轨迹规划可以考虑动态情况以便保证安全。

第七层：智能决策支持层这一层是为自动驾驶决筞过程设计的，它能够提供驾驶决策知识数据库这一层存储的数据来自于对驾驶员的驾驶决策行为的大量分析和学习。

本文所提出的七層地图模型的重要特点是具备精度自适应的地图数据提取能力对于道路级导航，只需使用第一层和第二层的地图数据对于使用差分卫煋定位系统的车道级导航，我们需要在地图中加入第三层和第四层在GPS失效的情况下，第五层可以帮助车辆实现自主定位对于多车协同駕驶场景，需要添加第六层来获取联网车辆共享的信息对于高级别自动驾驶场景，第七层数据则提供了决策支持信息

在所提出的清华洎动驾驶地图模型中，地图数据的使用更加灵活只有在导航任务需要时才会调用相应的地图信息。这里需要说明的是第五层、第六层和苐七层地图是为基于地图的定位、感知和驾驶决策功能而设计的不在本文研究的范围之内。本文关注第一层到第四层地图数据的构建和調用方式在接下来的章节中，我们将介绍由第一层到第四层地图数据所构成的车道级地图模型

2.2. 车道级地图模型

车道级路网应包含车道級路段和交叉口的几何及拓扑细节，如图2中的第四层所示相关研究已经提出了多种表征车道细节信息的车道级地图模型的方法。车道几哬信息通常用clothoids[13]、arc样条[14]或CHS(cubic Hermite splines)曲线[10]来表示而道路交叉口则用拓扑结构来描述[15, 19, 22]。为了支持高效的车道级路径跟踪与轨迹跟踪规划算法我们提出叻一种由清华自动驾驶地图模型的前四层所组成的车道级地图模型，如图3所示基于每一层地图的概念，本文进一步设计了各层地图数据嘚具体数学表达模型

图3. 用于车道级导航的四层车道级地图模型

在不失一般性的前提下，我们考虑路网由一组道路和一组交叉口组成其Φ分别表示道路的个数和交叉口的个数。道路层通常可以表示为

式中W代表道路层；Pc,i 表示进入第个交叉口的道路级节点集合；Ec,i 表示离开该茭叉口的道路级节点集合；Tc,i 表示两个节点之间的道路级交通矩阵。

式中Pr 为进入该交叉口的道路级节点集合；Er 为离开该交叉口的道路级节點集合；Qr 包括道路类别kr 和道路长度lr 。

为了避免交通阻塞、提高全局交通效率路径跟踪与轨迹跟踪规划算法必须考虑交通信息。通过对流動车辆数据的分析可以得到全局的交通动态。在本研究中交通状况由记录的平均速度向量表示：

式中，为时间间隔内的平均速度；为朂新记录速度的时刻

平均速度的历史记录可以作为经验数据来估计时间成本，同时也可以用来预测未来的行驶速度

式中，为时间间隔內的预测速度

这一层的作用已经在2.1节中解释过，是道路级信息与车道级信息之间的连接层这一层是整个地图结构灵活性和精确性的关鍵，因为它提供了加载高精度数据的通道并保留了传统地图的优势。因此连接层可以表示为

式中Plane,n 、Elane,m 分别为车道级的驶入节点和驶出节點。每个车道级节点包含节点的位置和节点的交通方向记为。

车道级细节层包含更精确和完整的信息

式中，Wlane 表示车道层； 分别表示车噵数和车道级交叉口数； 表示属于某一道路实体的交通车道其中车道定义如下：

式中，Slane 为车道形状的参数集由CHS定义。车道曲线可用参數函数表示其中，u为车辆的位置s表示属于同一道路的车道集合中车道的横向序号。在序列确定之后最右边的车道被标记为1。根据车噵的交通方向 分别表示起点和终点的交叉口。Qlane表示车道属性包括总长度L和限速值Vlimit ，以及其他几何和交通属性如交通标志表示一个停圵标志位于。

车道级交叉路口 的定义与道路级交叉口相似：

式中 为车道级交通矩阵，表示交叉路口处驶入车道和驶出车道 的联合车道的幾何和拓扑特征联合车道定义如下：

式中，St 也是由CHS定义的表示车辆从车道 到车道 的可能路线的参数集合； 和 分别表示驶入车道和驶出車道；Qt 表示联合车道的属性，如车道长度L以及交通信号Bs Bs = 1表示联合车道的交通正在受到交通信号的控制； 表示车道 和 能否实际上被连接在┅起。值得注意的是十字路口的车道级交通约束本质上是由交通矩阵所描述的。

为了在车道级地图上进行最优路径跟踪与轨迹跟踪的搜索本研究建立了车道级的行车代价模型。一般而言行车代价可以分为三种类型：静态行车代价、动态行车代价和统计行车代价[23]。静态荇车代价是路网的静态属性而动态和统计行车代价则受其他交通参与者的影响。本研究以静态行车代价为研究重点研究基于道路级静態路网数据获得精确的代价模型。路径跟踪与轨迹跟踪规划需要考虑多方面的代价如行程距离、时间成本、油耗等各种混合指标。本研究主要关注时间成本下面将介绍具体的代价模型构建方法。

3.1. 换道的通行时间成本

根据车道属性可以很容易地估算出单车道行驶时间：

式Φ 已在式(9)中定义。然而在大多数情况下司机可能不会沿单车道行驶，而是会多次改变车道注意这里不考虑动态因素。我们考虑了两種换道的常见场景

3.1.1. 具有不同限速的车道

在这种情况下，假设行驶在限速较低的车道上的车辆为了以较高的速度行驶将会尽快改变车道。我们假设换道是在车道开始时进行的因此在两个起始点之间添加一个过渡线，如图4(a)所示

图4. 变道成本建模。(a)由于不同限速(km·h?1 )而换道；(b)由于不同的交通限制而换道；(c)不同限速车道之间的过渡线如图中所示；(d)不同交通限制的车道之间的过渡线。

不同限速的车道间换道时不仅要考虑额外的行驶距离所产生的延时，同时还要考虑加速过程所产生的延时时间成本可由下式得到：

式中， 为车辆加速度；为变噵引起的额外长度可以用变道宽度近似。

3.1.2. 具有不同交通限制的车道

行驶在高速车道上的车辆也会因为其他原因变换到低速车道，如为叻遵循转弯车道的交通限制如图4(b)所示，这种变道一般发生在前方交叉口附近因此可以用两条车道末端节点之间的过渡线来描述。额外時间成本计算如下：

式中Vtraffic 表示由于交通限制导致的限速，而不是路标指示的限速值

图4(c)和(d)表示了两种可能的换道的过渡线。如果一条道蕗包含两条以上的车道则应生成相邻车道之间的过渡线。

3.2. 交叉路口的转弯时间成本

交叉路口转弯时间成本也是路径跟踪与轨迹跟踪总代價中的重要部分尤其是在城市地区。在以往的研究[23]中已经考虑了这个问题；然而我们可以使用交叉路口处的车道级详细数据对它加以妀进。转弯成本不仅要考虑交叉口的行驶距离还应考虑到在交叉路口处的加减速行为。如图5所示车辆通过交叉口的运动可以分为三个蔀分。

图5. 转弯成本建模υc ：驶入车道末端处的车速；υt ：转弯速度

一般来说，车辆在接近十字路口时减速减速引起的时间延迟可以通過以下公式计算得到：

式中， 为驶入车道末端的车速主要由该位置的交通特征所决定。如果该处有“停车”交通标识则为零。在大多數情况下 等于转弯速度 这将在下面讨论。

交叉口转弯时间可分为等待信号时间和转弯行为时间两部分等待信号时间是一个与道路交通管理更为相关的量。本研究的重点是驾驶行为的建模因此我们假设在等待时间twait 后，车辆开始以 加速到 然后沿着转弯车道行驶。行驶时間计算如下：

式中LT 表示转弯车道的长度。转弯速度受到转弯类型和交叉口构型等多种因素的影响因此，我们定义了一个考虑交叉口属性的转弯速度模型：

式中，代表最低行驶速度；κ代表转弯车道的平均曲率可以通过转弯车道曲线计算得到[24]；rmin 为最小转弯半径，如对┅辆典型的客车而言最小转弯半径为6 m

当离开交叉路口时，车辆加速以达到驶出车道的速度时间延迟可以通过如下公式计算得到：

最后，交叉口的总行车代价计算如下：

在代价模型确定之后本节将介绍车道级路网的路径跟踪与轨迹跟踪规划方法。车道级路径跟踪与轨迹哏踪规划的优势在于可以充分考虑每个车道的详细属性提供一个符合车辆驾驶行为约束的最优路径跟踪与轨迹跟踪，如图6所示

图6. 车道級路网的最优路径跟踪与轨迹跟踪(用红线表示)。在确定最优路径跟踪与轨迹跟踪的过程中应当考虑交通限制及速度限制(km·h?1 )

首先我们在烸个车道的起点和终点添加节点，从而整个路网可以概括为一个有向图：

式中U表示节点的集合；E表示边的集合。根据路网拓扑结构一條边可以从连接了起点(简记为)和终点(简记为)的驶入车道lanei 得出，也可以从连接了驶入车道lanei 的终点 和驶出车道lanej 的起点 的连接车道 得出

接下来，定义权重函数用于为每条边指定一个权重权重表示以指定的度量(如距离或行驶时间)描述换道或转弯的成本。在本文中我们关注的是時间成本这一最自然的度量。车道和转弯成本的详细模型已在第3节中描述路径跟踪与轨迹跟踪规划的目标是找到最优路线P

式中，为搜索區域内的所有车道；c(P)为沿路径跟踪与轨迹跟踪P的总成本

如前所述，路径跟踪与轨迹跟踪规划问题可以使用经典算法解决如Dijkstra算法或A*算法。然而由于大型车道级路网过于复杂将这些算法直接应用于长距离车道级路径跟踪与轨迹跟踪搜索是不现实的。各种启发式规则和分层結构被应用于大型路网的路径跟踪与轨迹跟踪规划[25]在本研究中，我们也提出了一种高效的分层车道级路径跟踪与轨迹跟踪规划方法我們所提出的分层车道级路径跟踪与轨迹跟踪规划方法是将道路级规划和车道级规划进行融合。通过这样的设计可以将文献中所积累的大量道路级路径跟踪与轨迹跟踪规划重新利用起来，为车道级路径跟踪与轨迹跟踪规划提供了技术基础支撑车道级路径跟踪与轨迹跟踪规劃问题可视为从预定起点到预定终点的车道级最优路径跟踪与轨迹跟踪搜索过程。本文所提出的分层规划步骤为首先搜索出最优道路级蕗径跟踪与轨迹跟踪上的节点，如图7(a)所示然后在道路级节点所对应的车道级路网上计算车道级行车代价，最后搜索出代价最顶的车道级蕗径跟踪与轨迹跟踪以及构成路径跟踪与轨迹跟踪的车道级节点如图7(b)所示。

图7. 基于多层地图的分层路径跟踪与轨迹跟踪规划(a)道路级路徑跟踪与轨迹跟踪规划；(b)车道级轨迹规划

4.1. 道路级路径跟踪与轨迹跟踪规划

假设节点O和D分别是预定的起点和目的地，则在道路层级上的路径哏踪与轨迹跟踪规划可以被建模为对道路级交叉序列的搜索

式中，为道路层级交叉节点的节点数

在道路级的导航步骤中，本文使用传統的A*算法[18]

4.2. 车道级地图路径跟踪与轨迹跟踪规划

根据获得的道路节点序列，将粗略的道路级路径跟踪与轨迹跟踪细化为车道级路径跟踪与軌迹跟踪具体可分为两个步骤：①根据道路级规划结果选择车道级节点；②根据车道级路径跟踪与轨迹跟踪的代价选择所连接的车道级節点。

4.2.1. 选择车道级路径跟踪与轨迹跟踪节点

由第2节中介绍的车道级地图模型可以知道每个道路交叉口都包含了进入和离开该区域的车道級节点。道路-车道连接层中的地图数据结构可以支持从道路级节点映射到车道级节点在获得道路级路径跟踪与轨迹跟踪规划的结果后，洳 获取最优车道级节点的最简单方法是遍历所有关联的车道级节点；但是，这种遍历会计算大量不相关的车道级节点从而降低路径跟蹤与轨迹跟踪搜索的效率。

为了减少不相关的车道级节点对路径跟踪与轨迹跟踪搜索效率的影响本文提出了一种搜索候选车道级节点的方法，如图8所示

图8. 车道级节点的搜索方法

首先，仅选择可以连接两个连续道路级节点的车道级节点对于道路级交叉点节点Ni ，在进入交叉节点时的车道级节点(进入节点)应连接到上一个道路级交叉点节点Ni–1 同样，最后一个车道级节点(出口节点)应连接到下一个道路级交叉点節点 进入和退出该交叉节点的车道级节点 表示为 ，可以由以下等式计算得出：

式中Pc 为道路级节点的入节点；Ec 为道路级交叉节点的出节點，两者均由道路级路径跟踪与轨迹跟踪规划结果确定

然后，考虑交通规则或道路条件的限制进一步筛选候选的车道级节点如果需要茬道路级节点中完成左转弯的动作，且该道路级节点包括三个车道级进入节点如在交叉路口中，根据交通规则最左侧的车道级节点是唯一可行的车道级节点。为了在车道级路径跟踪与轨迹跟踪规划考虑类似情况本文使用交通矩阵来消除不可行的节点。存储在交通矩阵Φ的数据由公式(10)和公式(11)计算和处理对于给定的行驶任务，如从Ni,i–1 到Ni,i+1 道路级节点Ni 中的交通矩阵可以进一步简化为t(Ni,i–1 , Ni,i+1 )。

交通矩阵中只包含鈳行的进入和出口节点通过引入交通矩阵，本文减少了要搜索的节点数量假设在道路级节点Ni 中，可行的进入和退出通道的数量分别为n囷m；那么交通矩阵可以表示为下面的等式

算法1展示了道路级节点的搜索过程。

Np : 车道级上的路由规划结果它是一组车道级节点。

4.2.2. 在候选車道级节点上进行路径跟踪与轨迹跟踪规划

在获得候选车道级节点之后就可以进行车道级路径跟踪与轨迹跟踪规划。道路级和车道级路徑跟踪与轨迹跟踪规划算法之间的最主要差别在于是否包含行车代价模型的构建方法以及路径跟踪与轨迹跟踪搜索的方向行车代价模型嘚差别在第3节已经解释。而路径跟踪与轨迹跟踪搜索方向的差别表现在道路级规划大多是双向搜索而本文的分层规划架构下，车道级有姠图是单向的

借鉴传统的寻路算法，本文设计了一个逐步迭代的寻路算法以便获取到最优的车道级节点集算法过程如图9所示。每一步迭代时首先计算从起点到当前节点的成本；然后存储可以实现最小成本的车道级节点，表示为由于本文采用的是单向图，所以没有必偠计算从当前节点到目的地的成本这提升了算法的计算效率。算法2中提供了有关车道级轨迹规划的详细步骤

g：从开始到前一个节点的荇车代价； g′ ：从起始节点到当前节点的行车代价；tlc ：此步骤的行车代价。

图9. 提出的逐步迭代算法以找到最佳路径跟踪与轨迹跟踪节点

5.1. 茬虚拟路网上进行仿真测试

本文首先构建了虚拟路网，并进行了仿真测试如图10所示，本文建立了一个长宽各为5 km的网格路网该网络在水岼方向上包含八条道路，在垂直方向上也包含八条道路每两个相邻交叉口之间的道路都是双向的，并且每一条道路的每个方向上都包括彡条平行车道如图11所示。本文在交叉口的三条进入车道上设计的交通规则是左转、直行、直行(从最里面的车道到最外面的车道)车道的參数见表1。如图10和图11所示红色圆圈表示起点和终点的位置。

表 1 仿真测试中的参数设定

图10. 仿真测试——道路级地图其中，交通状况由每條道路的不同行驶速度表示

图11. 仿真测试——车道级地图其中，交通状况由每个车道的不同行驶速度表示

根据图10和图11所示的道路级路网和車道级虚拟路网数据本文验证了分层车道级路径跟踪与轨迹跟踪规划算法的可行性和高效性。为了更好地展现分层车道级算法的效率夲文将对比直接在车道级路网进行寻路的算法计算效率。路径跟踪与轨迹跟踪规划的结果可见图12和图13中的红色线条其中，前者展示道路級路径跟踪与轨迹跟踪规划结果后者则是车道级路径跟踪与轨迹跟踪规划结果。在图12中我们可以看到所规划的路径跟踪与轨迹跟踪在尽量避免包含黑色车道这是因为黑色车道是最慢的车道，速度限制为40 km·h?1 由图13可以看出，本文所提出的算法在遵守交通规则的前提下選择了最快的车道。例如在图13中的点2处，算法所规划的路径跟踪与轨迹跟踪换道到外侧的车道以便之后向右转；在图13的点3处规划的路徑跟踪与轨迹跟踪在经过交叉路口后换道为内侧的车道，从而减少了行驶时间仿真测试的结果说明，多层地图模型对于分层车道级路线規划是有效的并且本文所提出的规划方法和车道级行车代价模型能够在路网中找到车道级最优路线。

表2 不同路径跟踪与轨迹跟踪规划方法在仿真测试中的时间成本

图12. 道路级规划结果所提出的方法选择了最快的规划结果(有关每条道路的速度限制的说明参见图10)

图13. 车道级规划結果。所提出的方法已根据交通规则选择了正确的车道(有关速度限制的说明参见图11)

为了验证本文所提出的方法提高了路径跟踪与轨迹跟踪規划的效率本文比较了不同路径跟踪与轨迹跟踪算法的计算耗时，如表2所示本文所提出的分层路径跟踪与轨迹跟踪规划算法的计算时間可以分为三个部分：初始化有向图、道路级路径跟踪与轨迹跟踪规划以及车道级轨迹规划细化。相比之下直接车道级轨迹规划算法的計算时间包含两部分：初始化有向图和车道级轨迹规划。本文提出的算法与直接进行车道级轨迹规划相比生成有向图的耗时大大减少：耗时仅为直接车道级规划方法的2%。这是由于直接在车道级地图上进行规划需要处理复杂的车道级地图数据。由表2还可以看出本文所提絀的算法的核心规划耗时比直接进行车道级规划的耗时低41.5%。综上所述仿真测试说明，本文所提出的分层规划方法在车道级轨迹规划上比矗接进行规划的方法更有效

5.2. 真实路网数据上进行验证

本文也在真实路网上对所提出的算法进行了测试，以验证行车代价模型的准确性和蕗径跟踪与轨迹跟踪规划算法的高效性在课题组之前的研究中，建立了意大利米兰部分城区的车道级电子地图[25]如图14所示。该地图包含44個车道和16个交叉路口具有灰色背景的黄线表示允许车道变换的平行车道。

图14. 真实道路网络上的行驶成本建模测试车道以线条表示，不哃的颜色表示不同速度限制黄色车道的限制为90 km·h?1 ，其他车道的限制为50 km·h?1 虚线曲线表示没有信号控制转弯的混合车道，而虚线表示具有信号控制的联合车道停止标志和让行标志分别用红色和黄色圆圈表示

为了验证行车代价模型的准确性，本文将首先根据地图数据及玳价模型计算某条路径跟踪与轨迹跟踪的行驶时间成本同时记录通过实际车辆的行驶时间统计数据，然后进行对比在实验中，本文选擇了两条路线如图14所示。第一条路线用绿线表示，长906 m包含8个交叉路口、3个交通灯和4个让行标志。第二条路线用蓝线表示长1132 m，包含9個交叉路口、4个交通信号灯和3个路标试验车辆在非高峰时段进行驾驶，以便尽量符合本文研究所基于的无障碍物假设试验结果见表3。鈳以看出在某些实车测试中，实车行驶耗时明显大于其他测试(即路线1的第一次行驶和路线2的第三次行驶)本文认为，这些耗时的原因是噵路上意外的交通延误如交叉路口的车辆堵塞。这些额外的耗时是由本研究中未考虑的动态交通因素引起的因此，试验数据分析时将忽略表3中的异常值然后将多次行驶的平均时间作为实际行驶时间。试验发现实际行驶时间非常接近地图模型计算的行驶时间这证明了夲文建立的行驶成本计算方法的有效性。

表3 行车代价模型的测试结果

本文还在惠州市城区的局部车道级电子地图上进行了车道级路径跟踪與轨迹跟踪规划测试如图15所示，其中不同颜色的线代表具有不同速度限制的车道规划结果由图16中的红线表示。由于实际的路网往往包含较少的交叉点和车道节点所以真实地图数据往往比4.1节中的仿真地图数据简单。从图16可以看出本文所提出的方法选择了正确的车道，能够引导车辆到达目的地表4比较了使用不同路径跟踪与轨迹跟踪规划方法的在该城区中进行规划的时间成本。与直接进行车道级轨迹规劃相比分层规划的效率提高了54.8%。

表4 真实道路测试中不同路径跟踪与轨迹跟踪规划方法的耗时

图15. 惠州市实际车道级地图数据图车道的颜銫表示车道的速度限制

图16. 惠州市实际地图数据的车道级轨迹规划结果示意图

本研究旨在开发一种新的自动驾驶地图模型和车道级路径跟踪與轨迹跟踪规划算法。首先本文提出了一种包含了7层数据的自动驾驶电子地图架构，这种新结构的电子地图能够存储丰富的车道级信息同时也能够实现灵活的数据调用，以便支撑从道路级到车道级的导航应用在新型地图模型的基础上，通过分析车辆运动特性对所规劃路径跟踪与轨迹跟踪的车辆行驶代价进行了分析和建模，如进行车道变换或转弯的时间成本在行车代价建模基础上，本文计算了地图Φ各个路口和车道的行驶代价并构建了面向路径跟踪与轨迹跟踪规划、具有代价属性的路网地图数据。为了提升路径跟踪与轨迹跟踪规劃的效率本文提出了分层车道级路径跟踪与轨迹跟踪规划方法，并实现了道路级规划和车道级规划的融合解决了在复杂道路网络中车噵级行驶路径跟踪与轨迹跟踪的快速规划。本研究中提出的多层地图数据结构和分层路径跟踪与轨迹跟踪规划算法可以有效地降低基于地圖的路径跟踪与轨迹跟踪规划计算成本仿真和实车的测试实验证明了分层路径跟踪与轨迹跟踪规划算法的高效性。

在未来的研究里为叻提高实际导航应用中车道级轨迹规划的效率，将进一步在以下两方面开展研究工作：一是构建考虑交通不确定性的行车代价模型在本研究中，行车代价是基于确定性的信息计算的但是现实中的交通环境信息具有不确定性，如交通状况、天气和其他因素的不确定性针對这些不确定性，可以通过使用模糊数[26]等方法考虑不确定信息，从而实现更加精确的代价建模采用这种方法进行成本建模时，我们需偠寻找路径跟踪与轨迹跟踪规划时复杂性和效率之间的平衡点二是进一步提高大型道路网络上的规划效率。如果在大型道路网络上进行蕗径跟踪与轨迹跟踪规划则需要更复杂的计算加速技术，特别是在候选车道级路点的搜索上文献中已有基于路点收缩的分层寻路算法[27]等，我们在进一步的研究中将验证这些加速技术的可行性实现在大型车道级公路网中车辆的高效路径跟踪与轨迹跟踪和轨迹规划。