目标场景区域的车辆定位方法、装置、设备和存储介质与流程

1.本技术涉及定位技术领域，特别是涉及一种目标场景区域的车辆定位方法、 装置、计算机设备和存储介质。


背景技术：

2.智能驾驶车辆是依靠人工智能技术，基于视觉，激光雷达，毫米波雷达， gnss(global navigation satellite system，全球卫星导航系统)，imu(inertialmeasurement unit，惯性测量单元)等多传感器协同合作，让电脑可以自主地进行 感知，定位，规划，控制，从而在特定条件下不同程度地实现安全自主地操控 车辆。智能驾驶技术是人工智能邻域最为热门和关键的技术之一。智能驾驶技 术的研究能够让驾驶员从常规的、持久且疲劳驾驶活动中解放出来，有效改善 驾驶员的驾驶体验，达到减少，甚至最终消除人为驾驶活动的目的，同时还能 有效地改善智能驾驶车辆的安全性和舒适性，具有广泛的应用价值和前景。
3.相关技术中的自动驾驶技术中，在交叉路口、分叉路口等众多没有车道线 的区域以及许多车道线不清晰或无法准确检测的路段，需要依靠rtk(real
-ꢀ
time kinematic，实时动态)技术或者高精度地图定位等方法实现车辆定位；然 而rtk技术存在电磁干扰、效应等不利因素的影响而导致无法满足自动驾驶的 高精度定位需求，而高精度地图存在地图数据采集困难，制作周期长，成本高 等缺点。因此需提供一种低成本且高效的实现已知路径的车辆自动化行驶的方 法。


技术实现要素：

4.基于此，有必要针对上述技术问题，提供一种低成本且高效的实现已知路 径的车辆自动化行驶的目标场景区域的车辆定位方法、装置、设备和存储介质。
5.一种目标场景区域的车辆定位方法，所述方法包括：
6.获取车辆的当前位置以及上一时刻实时位置姿态；
7.根据所述当前位置确定当前时刻所处的道路区域，所述道路区域包括邻接 的过渡区域和目标场景区域；
8.根据所述道路区域确定对应的实时位置姿态确定方式，基于所述上一时刻 实时位置姿态和所述实时位置姿态确定方式确定所述车辆的当前时刻实时位置 姿态；
9.其中，所述道路区域为过渡区域，所述实时位置姿态确定方式包括：基于 预设栅格地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时刻实时 位置姿态确定所述车辆的当前时刻实时位置姿态；
10.所述道路区域为目标场景区域，所述实时位置姿态确定方式包括：基于预 设激光雷达点云地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时 刻实时位置姿态确定所述车辆的当前时刻实时位置姿态。
11.一种目标场景区域的车辆定位装置，所述装置包括：
12.获取模块，用于获取车辆的当前位置以及上一时刻实时位置姿态；
13.道路区域确定模块，用于根据所述当前位置确定当前时刻所处的道路区域， 所述道路区域包括邻接的过渡区域和目标场景区域；
14.定位模块，用于根据所述道路区域确定对应的实时位置姿态确定方式，基 于所述上一时刻实时位置姿态和所述实时位置姿态确定方式确定所述车辆的当 前时刻实时位置姿态；
15.其中，所述道路区域为过渡区域，所述实时位置姿态确定方式包括：基于 预设栅格地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时刻实时 位置姿态确定所述车辆的当前时刻实时位置姿态；
16.所述道路区域为目标场景区域，所述实时位置姿态确定方式包括：基于预 设激光雷达点云地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时 刻实时位置姿态确定所述车辆的当前时刻实时位置姿态。
17.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序， 所述处理器执行所述计算机程序时实现上述目标场景区域的车辆定位方法的步 骤。
18.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处 理器执行时实现上述目标场景区域的车辆定位方法的步骤。
19.上述目标场景区域的车辆定位方法、装置、设备和存储介质，获取车辆的 当前位置和上一时刻车辆的实时位置姿态，根据当前位置确定车辆所处的道路 区域，其中，道路区域包括邻接的过渡区域和目标场景区域；根据道路区域确 定对应的实时位置姿态确定方式，基于上一时刻实时位置姿态和确定的实时位 置姿态确定方式确定车辆的当前实时位置姿态。其中，若道路区域为过渡区域， 基于预设栅格地图当前时刻采集的激光雷达实时单帧点云以及所述上一时刻实 时位置姿态确定所述车辆的当前时刻实时位置姿态；若道路区域为目标场景区 域，基于预设激光雷达点云地图、当前时刻采集的激光雷达实时单帧点云以及 所述上一时刻实时位置姿态确定所述车辆的当前时刻实时位置姿态。上述方法 车辆在过渡区域时，利用预设栅格地图和激光雷达实时单帧点云确定实时位置 姿态，可以提升在过渡区域的定位精度，从而车辆在进入目标场景区域时，具 备高精度的定位初值，车辆在目标场景区域中以该定位初值为基础进行计算实 时位置姿态，可保障车辆在目标场景区域中实现快速准确的高精度定位，实现 了车辆位置的高精度输出，满足了在无车道线等复杂路况下的车辆定位需求， 且仅需构建部分区域的栅格地图和激光雷达地图，成本低且制作周期短。
附图说明
20.图1为一个实施例中目标场景区域的车辆定位方法的应用环境图；
21.图2为一个实施例中目标场景区域的车辆定位方法的流程示意图；
22.图3(1)为一个具体实施例中路口范围点的示意图；
23.图3(2)为另一个具体实施例中路口范围点的示意图；
24.图4为一个实施例中基于预设激光雷达点云地图、当前时刻采集的激光雷 达实时单帧点云和上一时刻的实时位置姿态计算当前时刻实时位置姿态的流程 示意图；
25.图5为一个实施例中基于当前时刻基础位置姿态、预设栅格地图以及当前 时刻采
集的激光雷达实时单帧点云确定车辆的当前时刻位置姿态的流程示意图；
26.图6为一个实施例中基于当前时刻基础位置姿态在预设栅格地图中启发式 搜索与激光雷达实时单帧点云匹配的三个中间位置姿态的流程示意图；
27.图7为另一个实施例中目标场景区域的车辆定位方法的流程示意图；
28.图8为一个具体实施例中目标场景区域的车辆定位方法的完整流程示意图；
29.图9为一个具体实施例中基于基础位置姿态和预设栅格地图确定车辆在过 渡区域的第二实时位置姿态的流程示意图；
30.图10为一个实施例中目标场景区域的车辆定位装置的结构框图；
31.图11为一个实施例中计算机设备的内部结构图。
具体实施方式
32.为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实 施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅 用以解释本技术，并不用于限定本技术。
33.本技术提供的目标场景区域的车辆定位方法，在一些实施例中可以应用于 如图1所示的应用环境中。其中，终端102通过网络与服务器104进行通信。 终端102获取车辆的当前位置和上一时刻车辆的实时位置姿态，根据当前位置 确定车辆所处的道路区域，其中，道路区域包括邻接的过渡区域和目标场景区 域；根据道路区域确定对应的实时位置姿态确定方式，基于上一时刻实时位置 姿态和确定的实时位置姿态确定方式确定车辆的当前实时位置姿态。其中，若 道路区域为过渡区域，基于预设栅格地图当前时刻采集的激光雷达实时单帧点 云以及所述上一时刻实时位置姿态确定所述车辆的当前时刻实时位置姿态；若 道路区域为目标场景区域，基于预设激光雷达点云地图、当前时刻采集的激光 雷达实时单帧点云以及所述上一时刻实时位置姿态确定所述车辆的当前时刻实 时位置姿态。；其中，预设栅格地图、预设激光雷达点云地图可从服务器104中 获取。其中，终端102可以但不限于是各种车载终端，或者与车辆通信的终端， 服务器104可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
34.在一个实施例中，如图2所示，提供了一种目标场景区域的车辆定位方法， 以该方法应用于图1中的终端为例进行说明，包括步骤s210至步骤s230。
35.步骤s210，获取车辆的当前位置以及上一时刻实时位置姿态。
36.其中，当前位置可通过任意一种方式获取。上一时刻实时位置姿态是指当 前时刻的上一时刻所计算得到的实时位置姿态。车辆的位置姿态是指车辆所在 的位置点和车辆当前的姿态；其中，车辆的位置(vrp,vehicle reference point)， 一般选取一个车辆的基准点在世界坐标系的位置作为车辆位置。车辆的姿态则 包括车辆的航行角度、方位等信息。
37.在一个实施例中，若当前时刻为车辆启动时刻，获取车辆的当前位置以及 车辆的启动时刻位置姿态；根据道路区域确定对应的实时位置姿态确定方式， 基于启动时刻位置姿态和实时位置姿态确定方式确定车辆的当前时刻实时位置 姿态。
38.其中，车辆的启动时刻位置姿态可以以任意一种方式获取。后续在启动时 刻位置姿态的基础上进行计算获得车辆的实时位置姿态。若当前时刻为车辆的 启动时刻，则以获
取的车辆启动时刻位置姿态为基础计算后续的实时位置姿态。
39.在另一个实施例中，若当前时刻非车辆启动时刻，进入获取车辆的当前位 置以及上一时刻实时位置姿态的步骤。
40.当前时刻若非车辆的启动时刻，则车辆的上一时刻对应有一个实时位置姿 态，此时获取上一时刻的实时位置姿态，并在上一时刻的位置姿态为基础计算 当前时刻车辆的实时位置姿态。
41.步骤s220，根据当前位置确定当前时刻所处的道路区域。
42.其中，道路区域是指根据以目标场景区域为基准将道路划分的区域，在一 个实施例中，道路区域包括邻接的过渡区域和目标场景区域。在一个具体实施 例中，目标场景区域即是指路口，进而根据距离路口的远近可以将道路划分为 所处位置为目标场景区域(路口内)、所处位置为距离路口较近的过渡区域(路 口外)，以及所处位置为距离路口较远的远场区域(路口外)。在一个具体实施 例中对于远场区域、过渡区域和目标场景区域的划分依据的距离可根据实际情 况进行设置；远场区域、过渡区域和目标场景区域依次相邻，且远场区域、过 渡区域和目标场景区域共同组成道路路径；在一个实施例中，道路区域是本申 请的实施例中预先生成的，根据采集的道路数据进行道路区域划分，将不存在 车道线或者车道线清晰度不够的区域(路口)划分为目标场景区域，将包含车 道线、且距离目标场景区域在第一阈值内的区域划分为过渡区域，将包含车道 线、且距离目标场景区域大于第一阈值小于第二阈值内的区域划分为远场区域， 可以理解地划分道路区域时，需结合道路的方向划分，将进入该路口之前的道 路以该路口为基础进行道路区域划分。通常来说，在目标场景区域采用高精度 定位方法，而在包含清晰车道线的远场区域和过渡区域，通常采用非高精度定 位的方法计算车辆的实时位置姿态。
43.在一个实施例中，在确定车辆本次行驶的导航路径后，可获得导航路径中 依次需经过的路口，其中，每一个路口对应的区域依次包括远场区域、过渡区 域和目标场景区域。
44.进一步地，在一个实施例中，在根据当前位置确定车辆所处的道路区域之 前，还包括：获取车辆所选择的预设导航路径，读取预设导航路径中的所有路 口；进一步地，在本实施例中，根据当前位置确定车辆所处的道路区域包括： 根据当前位置确定车辆在预设导航路径中行驶方向上距离最近的下一个路口， 记为目标路口；获取目标路口的目标路口信息，根据目标路口信息获取目标路 口的路口范围点(其中，路口范围点包括三个以上，将路口范围完整包含在内)； 分别计算车辆的当前位置与目标路口的各路口范围点的距离，根据当前位置与 目标路口的各路口范围点的距离确定车辆所处的道路区域。更进一步地，在一 个实施例中，通过计算当前车辆的位置与各路口范围点围成的多边形的位置关 系，来确定车辆当前所处的道路区域；在一个具体实施例中，当前位置与目标 路口的位置关系检测可通过以下方式实现：
45.若
46.其中，si＝(x
i-x
vehicle
)
·
(y
i 1-y
vehicle
)-(x
i 1-x
vehicle
)
·
(y
i-y
vehicle
)， p
vehicle
(x
vehicle
,y
vehicle
)为车辆的当前位置，pi(xi,yi)表示目标路口的路口范围点， p
i 1
(x
i 1
,y
i 1
)为顺时针顺序的下一个路口范围点，当i＝n时，令p
i 1
＝p1。
47.当通过上述方法判断基础位置所处的道路区域为路口外时，可通过以下方 法确
定基础位置所处的道路区域是否处于过渡区域：首先计算当前位置对应的 点pi与邻近路口对应的范围点的水平距离因此，当前位置对应 的点与路口区域的距离可由如下公式近似：
[0048][0049]
其中，和为距离当前位置对应的点pi最近的两个路口范围点的距离。 当d
xy
小于设定阈值d时，则车辆当前已进入过渡区域。
[0050]
在其中的一个实施例中，根据当前位置确定车辆在预设导航路径中行驶方 向上距离最近的下一个路口，确定为目标路口包括：从预设导航路径中读取各 路口的索引号，根据当前位置读取下一路口的索引号，下一索引号对应的路口 为目标路口。在本实施例中，在生成预设导航路径时，对于依次需要经过的路 口依次设置了索引号，根据车辆的当前位置可确定车辆下一个需经过路口的路 口索引号。在其中的另一个实施例中，根据目标路口信息获取目标路口的路口 范围点包括：获取目标路口对应的点云数据，从点云数据中选取符合预设要求 的点作为路口范围点，可以理解地，其中路口范围点需满足的预设条件为包括 三个以上，路口范围点共同将这一路口范围完整包含在内。如图3(1)所示为 一个具体实施例中十字路口的路口范围点示意图，如图3(2)所示为一个具体 实施例中t字型路口的路口范围点示意图，其中，作为路口区域的范围，每个 路口范围至少包括以下信息：(路口索引号，点1(x1,y1)，点2(x2,y2)，点3(x3,y3)，点 4(x4,y4))。
[0051]
步骤s230，根据道路区域确定对应的实时位置姿态确定方式，基于上一时 刻实时位置姿态和实时位置姿态确定方式确定车辆的当前时刻实时位置姿态。
[0052]
本实施例中，根据车辆当前所处的道路区域的不同，根据对应的实时位置 姿态确定方式结合上一时刻实时位置姿态来确定当前时刻的实时位置姿态。
[0053]
进一步地，道路区域为过渡区域时，实时位置姿态确定方式包括：基于预 设栅格地图、当前时刻采集的激光雷达实时单帧点云以及上一时刻实时位置姿 态确定车辆的当前时刻实时位置姿态；道路区域为目标场景区域时，实时位置 姿态确定方式包括：基于预设激光雷达点云地图、当前时刻采集的激光雷达实 时单帧点云以及上一时刻实时位置姿态确定车辆的当前时刻实时位置姿态。
[0054]
点云数据(point cloud data)是指扫描资料以点的形式记录，每一个点包含 有三维坐标，有些可能含有颜色信息(rgb)或反射强度信息(intensity)；在 一个实施例中，当前时刻车辆的激光雷达实时单帧点云，是指车辆在当前时刻 所在的位置采集的激光雷达实时单帧点云数据。栅格地图是把环境划分成一系 列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率；本实施例 中，将预先生成的栅格地图记为预设栅格地图。
[0055]
在一个实施例中，预设栅格地图的生成包括步骤：依据激光雷达采集的单 帧点云的时间戳，插值该时刻对应的位姿，结合组合导航设备与激光雷达的外 参(相对位置关系)，可将过渡区域范围内的所有单帧点云全部进行坐标变换，合 并生成路口区域的点云数据；将过渡区域的点云地图基于垂直度过滤，并进行 空间栅格划分，根据每一个点的水平坐标计算对应栅格，若有点落入则记为占 用，否则记为空闲，生成预设栅格地图，用于车辆处于过渡区域中时的定位。
[0056]
本实施例中，若当前位置确定车辆所处的道路区域为过渡区域，则在上一 位置姿态的基础上，结合预设栅格地图和当前时刻采集的激光雷达实时点云确 定当前车辆的实时位置姿态，实时位置姿态的位置精度更高，进而使得车辆在 驶入目标场景区域时，具备更高精度的定位初值，有助于计算车辆在目标场景 区域中的实时位置。
[0057]
在一个实施例中，基于预设栅格地图和当前时刻采集的激光雷达实时点云 确定当前车辆的实时位置姿态包括：将激光雷达实时点云与预设栅格地图中进 行匹配得到多个目标匹配点，并将匹配分数最高的目标匹配点对应的位置姿态 确定为实时位置姿态。在本实施例中，通过多个目标匹配点进行综合比较和计 算，将匹配分数最高的目标匹配点对应的位置姿态确定为当前时刻实时估计位 置姿态，进而根据当前时刻实时估计位置姿态校正当前时刻基础位置姿态可以 得到更加稳定可靠的实时位置姿态。
[0058]
进一步地在本实施例中，从车辆处于过渡区域的第二个时刻开始，每一时 刻的实时位置姿态均以上一时刻的实时位置姿态为基础位置姿态，结合预设栅 格地图和当前时刻采集的激光雷达单帧点云计算当前时刻的实时位置姿态。
[0059]
在一个具体实施例中，目标场景区域是指道路中的路口等区域，如十字路 口、t字型路口等，该区域中通常没有车道线或者车道线不清晰，无法实现高精 度定位的区域。在一个实施例中，由于道路区域包括依次相邻的过渡区域和目 标场景区域，当根据当前位置确定车辆所处的道路区域为目标场景区域时，判 定车辆从过渡区域驶入目标场景区域。进一步地，以车辆在过渡区域的实时位 置姿态为车辆在目标场景区域的初始定位基础，计算车辆在目标场景区域的实 时位置姿态。
[0060]
其中，预设激光雷达点云地图是预先依据激光雷达采集的单帧点云的时间 戳，插值该时刻对应的位置姿态，结合导航设备与激光雷达的外参(相对位置 关系)，将路口单帧点云经去噪、下采样、特征提取等处理后生成目标场景区域 对应的点云地图，用于目标场景区域的定位。
[0061]
上一时刻车辆在过渡区域的实时位置姿态是指车辆在过渡区域中最后一个 时刻对应的实时位置姿态，在根据当前位置确定车辆从过渡区域驶入目标场景 区域后，在目标场景区域的第一个时刻需以车辆在过渡区域最后一个时刻的实 时位置姿态为定位初值，计算车辆在目标场景区域的实时位置姿态。
[0062]
在一个实施例中，如图4所示，基于预设激光雷达点云地图、当前时刻采 集的激光雷达实时单帧点云和上一时刻的实时位置姿态计算当前时刻实时位置 姿态，包括步骤s410至步骤s430。
[0063]
步骤s410，获取车辆的惯性测量单元数据。
[0064]
惯性测量单元数据，即imu数据(inertial measurement unit，惯性测量单元)， 惯性测量单元是测量物体三轴姿态角及加速度的装置。
[0065]
步骤s420，根据惯性测量单元数据确定车辆当时时刻相对上一时刻的位置 姿态变化信息，并根据位置姿态变化信息对上一时刻至当时时刻的时间段内的 激光雷达实时单帧点云进行补偿。
[0066]
步骤s430，以上一时刻实时位置姿态为初始值，在预设激光雷达点云地图 中根据补偿后的激光雷达实时单帧点云计算当前时刻实时位置姿态。
[0067]
在本实施例中，当车辆进入目标场景区域范围，随即开始目标场景区域的 定位；
在一个具体实施例中，在目标场景区域中利用卡尔曼滤波融合激光雷达 和imu数据来实现车辆位置的估计。其中imu推算用于高频预测车辆状态、基 于激光雷达的地图匹配用于高精度地更新车辆状态，实现高频高精度的定位输 出。
[0068]
进一步地，在一个实施例中，为保证地图匹配精度，需提前根据激光雷达 采集时间段的运动状态变化量对激光雷达单帧数据进行运动补偿；为提升匹配 效率，可适当去除距离传感器过近或过远的无效点数据，同时利用点云下采样 的方式降低数据冗余，尤其是地面点的数据冗余。在此基础上，构建单帧点云 与路口地图的对应关系，求解最优匹配参数r,t，实现单帧点云与地图点云间的 误差函数最小：
[0069][0070]
其中，n为有效点对数量，pi为在线点云中一点，qi为地图点云中与pi对应点， r为旋转矩阵，t为平移向量。
[0071]
可以理解地，以过渡区域最后一个时刻的实时位置姿态为基础计算目标场 景区域的当前时刻实时位置姿态发生在车辆驶入目标场景区域的第一个时刻。 进一步地在本实施例中，从车辆驶入目标场景区域的第二个时刻开始，每一时 刻的实时位置姿态均以上一时刻的实时位置姿态为基础，结合预设激光雷达点 云地图计算当前时刻实时位置姿态。
[0072]
本实施例中，在目标场景区域以上一时刻在过渡区域确定的车辆的实时位 置姿态为基础，结合预设激光雷达点云数据计算该区域的实时位置姿态，由于 在过渡区域采用预设栅格地图和实时采集的激光雷达实时单帧点云对上一时刻 实时位置姿态进行航位推算后得到的当前时刻基础位置姿态进行了校正，可得 到精确度较高的实时位置姿态，进而在车辆进入目标场景区域时，依据该实时 位置姿态为定位初值可计算得到较为精准的在目标场景区域中的实时位置姿态。
[0073]
上述目标场景区域的车辆定位方法，获取车辆的当前位置和上一时刻车辆 的实时位置姿态，根据当前位置确定车辆所处的道路区域，其中，道路区域包 括邻接的过渡区域和目标场景区域；根据道路区域确定对应的实时位置姿态确 定方式，基于上一时刻实时位置姿态和确定的实时位置姿态确定方式确定车辆 的当前实时位置姿态。其中，若道路区域为过渡区域，基于预设栅格地图当前 时刻采集的激光雷达实时单帧点云以及所述上一时刻实时位置姿态确定所述车 辆的当前时刻实时位置姿态；若道路区域为目标场景区域，基于预设激光雷达 点云地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时刻实时位置 姿态确定所述车辆的当前时刻实时位置姿态。上述方法车辆在过渡区域时，利 用预设栅格地图和激光雷达实时单帧点云确定实时位置姿态，可以提升在过渡 区域的定位精度，从而车辆在进入目标场景区域时，具备高精度的定位初值， 车辆在目标场景区域中以该定位初值为基础进行计算实时位置姿态，可保障车 辆在目标场景区域中实现快速准确的高精度定位，实现了车辆位置的高精度输 出，满足了在无车道线等复杂路况下的车辆定位需求，且仅需构建部分区域的 栅格地图和激光雷达地图，成本低且制作周期短。
[0074]
进一步地，在一个实施例中，车辆所处的道路区域为过渡区域，基于预设 栅格地图、当前时刻采集的激光雷达实时单帧点云以及上一时刻实时位置姿态 确定车辆的当前时刻实时位置姿态，包括：基于上一时刻实时位置姿态进行航 位推算，得到当前时刻基础位置姿态；基于当前时刻基础位置姿态、预设栅格 地图以及当前时刻采集的激光雷达实时
单帧点云确定车辆的当前时刻实时位置 姿态。
[0075]
在另一个实施例中，车辆所处的道路区域为目标场景区域，基于预设激光 雷达点云地图、当前时刻采集的激光雷达实时单帧点云以及上一时刻实时位置 姿态确定车辆的当前时刻实时位置姿态，包括：基于上一时刻实时位置姿态进 行航位推算，得到当前时刻基础位置姿态；基于当前时刻基础位置姿态、预设 激光雷达点云地图以及当前时刻采集的激光雷达实时单帧点云确定所述车辆的 当前时刻实时位置姿态。
[0076]
航位推算包括以下步骤：利用实时激光雷达数据辅助imu、轮速信息等其 它传感器数据从起始位置(非目标场景区域)、驶出路口、视觉定位校正位置开 始进行位置估计，包括里程计、激光实时建图与定位两个关键步骤。在一个具 体实施例中，可采用loam等开源激光slam算法；其中，slam(simultaneouslocalization and mapping)，也称为cml(concurrent mapping and localization)， 即时定位与地图构建，或并发建图与定位。
[0077]
其中的里程计是通过激光雷达数据、imu数据、轮速信息中的一种或几种 数据，在上一时刻位置的基础上，对当前时刻位置进行推估，并不断迭代这个 过程。其中，激光雷达主要通过前后两帧点云间的匹配，计算出变换关系；imu 数据，主要通过加速度计和角速度计数据的积分，计算出两个时刻间的位姿变 换量；轮速信息，主要通过将车辆各轮子的速度信息带入运动模型，利用轮速 和轮速差，估计车辆的运动量。
[0078]
进一步地，在一个实施例中，基于当前时刻基础位置姿态、预设栅格地图 以及当前时刻采集的激光雷达实时单帧点云确定车辆的当前时刻实时位置姿态， 包括：根据航位推算的结果获得当时时刻相对上一时刻的位置姿态的变化信息 对当前时刻采集的激光雷达实时单帧点云进行补偿，获得补偿后的单帧点云； 基于预设栅格地图和补偿后的实时单帧点云求解获得当前时刻实时位置姿态。
[0079]
在另一个实施例中，基于预设激光雷达点云地图、当前时刻采集的激光雷 达实时单帧点云以及上一时刻实时位置姿态确定车辆的当前时刻实时位置姿态， 包括：根据航位推算的结果获得当时时刻相对上一时刻的位置姿态的变化信息 对当前时刻采集的激光雷达实时单帧点云进行补偿，获得补偿后的单帧点云； 基于预设栅格地图和补偿后的实时单帧点云求解获得当前时刻实时位置姿态。
[0080]
本实施例中对于采集的车辆当前的每一帧实时点云数据都需执行处理。其 中的激光实时建图与定位首先利用里程计的结果单帧激光雷达数据对进行运动 补偿；接着利用补偿后的激光雷达数据与实时生成的激光地图进行匹配，获得 激光雷达时刻的车辆位置姿态；通过将单帧点云进行坐标变换，结合现有激光 地图，不断累加生成局部地图。
[0081]
本实施例中，对于在上一时刻的实时位置姿态的基础上进行航位推算后获 得的位置姿态，进一步对该位置姿态进行运动补偿，可获得更接近当前实际位 置的单帧点云，在后续计算实时位置姿态中，可以得到更高精度的位置姿态。 在一个实施例中，若采用激光雷达实时单帧点云进行航位推算，则对激光雷达 实时单帧点云数据进行补偿，并利用补偿后的激光雷达实时单帧点云数据做后 续的计算。
[0082]
更进一步地，在一个实施例中，如图5所示，基于当前时刻基础位置姿态、 预设栅格地图以及当前时刻采集的激光雷达实时单帧点云确定车辆的当前时刻 位置姿态，包括步骤s510至步骤s530。
[0083]
步骤s510，基于当前时刻基础位置姿态在预设栅格地图中启发式搜索与激 光雷
达实时单帧点云匹配的三个中间位置姿态。
[0084]
启发式搜索(heuristically search)又称为有信息搜索(informed search)，它是 利用问题拥有的启发信息来引导搜索，达到减少搜索范围、降低问题复杂度的 目的，这种利用启发信息的搜索过程称为启发式搜索。启发式搜索根据启发式 的策略来进行搜索，以达到减少搜索范围、降低问题复杂度的目的，并得到令 人能接受的解(通常并不一定是最佳解)。
[0085]
在一个实施例中，基于当前时刻基础位置姿态在预设栅格地图中启发式搜 索与激光雷达实时单帧点云匹配的三个中间位置姿态，包括：基于当前时刻基 础位置姿态在预设栅格地图中进行启发式搜索获得激光雷达实时单帧点云匹配 的点，根据搜索获得的匹配点对应的位置姿态确定三个中间位置姿态。基于当 前时刻基础位置姿态在预设栅格地图中进行启发式搜索与激光雷达实时单帧点 云匹配的位置姿态，可能获得多个匹配的位置姿态，本实施例中筛选其中前三 个最优的位置姿态匹配结果，记为本实施例中与激光雷达实时单帧点云匹配的 三个中间位置姿态。
[0086]
在一个实施例中，基于当前时刻基础位置姿态在预设栅格地图中进行启发 式搜索与激光雷达实时单帧点云匹配的位置姿态，具体是以当前时刻基础位置 姿态为初始值，采用一定值的搜索步长在预设栅格地图中搜索匹配的位置姿态； 其中，搜索步长可包括水平搜索步长和角度搜索步长。
[0087]
进一步地，在另一个实施例中，在以预设搜索步长搜索获得匹配的搜索结 果后，可对于设置的预设搜索步长可进行调整后再次搜索，分别按照不同的步 长搜索匹配结果，从所有搜索到的匹配结果中取匹配分数最高的三个匹配结果 对应的位置姿态确定为三个中间位置姿态。
[0088]
步骤s520，根据三个中间位置姿态输出车辆的当前时刻实时估计位置姿态。
[0089]
在一个实施例中，根据三个中间位置姿态输出车辆的当前时刻实时估计位 置姿态，包括：计算第一中间位置姿态、第二中间位置姿态与第三中间位置姿 态两两之间的差异值，其中，三个中间位置姿态包括第一中间位置姿态、第二 中间位置姿态和第三中间位置姿态。进一步地，在一个实施例中，计算第一中 间位置姿态、第二中间位置姿态与第三中间位置姿态两两之间的差异实际包括 三个中间位置姿态两两之间的坐标和角度差异。
[0090]
在一个实施例中，若第一中间位置姿态与第二中间位置姿态的差异值、以 及第一中间位置姿态与第三中间位置姿态之间的差异值均大于预设阈值，输出 第一中间位置姿态为当前时刻实时估计位置姿态。其中，预设阈值可以根据实 际情况自行设定。
[0091]
在另一个实施例中，若第一中间位置姿态与第二中间位置姿态之间的差异 值小于预设阈值，且第一中间位置姿态与第三中间位置姿态之间的差异值、以 及第二中间位置姿态与第三中间位置姿态之间的差异值均大于预设阈值，基于 第一中间位置姿态、第二中间位置姿态和第一预设权重系数输出当前时刻实时 估计位置姿态。在一个具体实施例中，用p
optimal
表示当前时刻实时估计位置姿态， pa、pb、pc分别表示第一中间位置姿态、第二中间位置姿态和第三中间位置姿态， 在本实施例中，pa与pb、pc的差异均大于阈值，则p
optimal
＝pa。
[0092]
其中，第一预设权重系数包括第一中间位置姿态的权重系数和第二中间位 置姿态的权重系数；在一个具体实施例中，第一中间位置姿态的权重系数和第 二中间位置姿态
的权重系数可以用α和β表示，其中，α与β的和为1，α和β 之间的比例即为第一中间位置姿态和第二中间位置姿态的匹配分数的比例；在 一个具体实施例中，用p
optimal
表示当前时刻实时估计位置姿态，pa、pb、pc分别 表示第一中间位置姿态、第二中间位置姿态和第三中间位置姿态，在本实施例 中，若pa与pb的差异小于阈值，pa、pb与pc的差异大于阈值，则p
optimal
＝αpa βpb； α β＝1，α、β的比例等于pa、pb匹配分数间的比例。
[0093]
在另一个实施例中，若第一中间位置姿态与第二中间位置姿态之间的差异 值、以及第一中间位置姿态与第三中间位置姿态之间的差异值均小于预设阈值， 或者，第一中间位置姿态与第二中间位置姿态之间的差异值、以及第二中间位 置姿态与第三中间位置姿态之间的差异值均小于预设阈值，基于第一中间位置 姿态、第二中间位置姿态、第三中间位置姿态和第二预设权重系数输出当前时 刻实时估计位置姿态。
[0094]
其中，第二预设权重系数包括第一中间位置姿态的权重系数、第二中间位 置姿态的权重系数和第三中间位置姿态的权重系数；在一个具体实施例中，第 二预设权重系数包括第一中间位置姿态的权重系数、第二中间位置姿态的权重 系数和第三中间位置姿态的权重系数分别用α、β和γ表示，其中，α、β和γ的和 为1，α、β和γ之间的比例即为第一中间位置姿态、第二中间位置姿态和第三中 间位置姿态对应的匹配分数的比例。在一个具体实施例中，用p
optimal
表示当前时 刻实时估计位置姿态，pa、pb、pc分别表示第一中间位置姿态、第二中间位置姿 态和第三中间位置姿态，在本实施例中，若pa与pb、pc的差异均小于阈值，或若pa与pb、pb与pc的差异小于阈值，则p
optimal
＝αpa βpb γpc；α β γ＝1，α、β、γ的 比例等于pa、pb、pc的匹配分数间的比例。
[0095]
步骤s530，基于当前时刻实时估计位置姿态对当前时刻基础位置姿态进行 校正，得到当前时刻实时位置姿态。
[0096]
在一个实施例中，以当前时刻实时估计位置姿态替换当前时刻基础位置姿 态得到当前时刻实时位置姿态。
[0097]
在本实施例中，以当前时刻基础位置姿态为基础在预设栅格地图中进行搜 索，获得三个中间位置姿态，并从中确定出一个作为当前时刻实时估计位置姿 态，然后以该当前时刻实时估计位置姿态对当前时刻基础位置姿态进行校正， 得到当前时刻实时位置姿态，相比直接输出匹配分数的位置，根据多个结果综 合比较、计算，输出的当前时刻实时位置状态更加稳定可靠。
[0098]
在一个实施例中，如图6所示，基于当前时刻基础位置姿态在预设栅格地 图中启发式搜索与激光雷达实时单帧点云匹配的三个中间位置姿态包括步骤 s610至步骤s690。
[0099]
步骤s610，根据当前时刻基础位置姿态将激光雷达实时单帧点云映射至预 设定位坐标系，获得激光雷达实时单帧点云对应的位置坐标。
[0100]
在一个实施例中，预设定位坐标系为车辆当前所处位置对应的世界坐标系； 世界坐标系是系统的绝对坐标系，在没有建立用户坐标系之前画面上所有点的 坐标都是以该坐标系的原点来确定各自的位置的；在一个实施例中，路口的各 范围点是以预设定位坐标系进行存储的，根据当前时刻基础位置姿态将激光雷 达实时单帧点云映射至预设定位坐标系后，便于后面筛选点云数据。在一个具 体实施例中，世界坐标系可采用utm(universal transverse mercator grid system, 通用横墨卡托格网系统)世界坐标系。在一个实施例中，基于基础位置姿态将激 光雷达实时单帧点云映射至预设定位坐标系，包括：
[0101]
在一个实施例中，若以上一时刻的位置姿态进行航位推算获得当前时刻的 基础位置姿态，则在对激光雷达实时单帧点云数据进行补偿后，根据当前时刻 基础位置姿态将补偿后的单点云映射到预设坐标系中，得到激光雷达预设点云 地图对应的位置坐标。
[0102]
步骤s620，基于激光雷达实时单帧点云对应的位置坐标在激光雷达实时单 帧点云中筛选在水平方向内处于预设范围内，且垂直度大于预设垂直度阈值的 目标点云数据。
[0103]
其中，对激光雷达实时单帧点云中筛选获得目标点云数据，可先进行水平 方向的筛选，然后根据垂直度进行筛选；或者也可以先进行垂直度的筛选然后 进行水平方向的筛选。
[0104]
在一个实施例中，先根据垂直度对点云数据进行过滤，然后进行水平方向 的点云筛选。根据垂直度对激光雷达实时单帧点云进行过滤包括：在激光雷达 实时单帧点云中，筛选出垂直度大于预设垂直度阈值的点云数据，记为第一点 云数据。
[0105]
其中，垂直度是用于描述局部垂直程度的三维描述子。在一个具体实施例 中，通过点周围邻近区域包含的点集共同描述该点的垂直度，其计算公式如下：
[0106][0107]
其中是由该点领域点集生成的三维协方差m
cov
矩阵求解的最小 特征值对应的特征向量。
[0108][0109]
其中，n为该点p的领域点集中点的数量，对每个点，其领域点的 获取方式主要有按空间区域划分和按空间距离搜索两种。最终，依据垂直度值， 仅保留垂直度大于阈值的点。主要是街道场景中垂向特征明显的地物，包括树 木、路灯、路牌、建筑等，这些对象的点，更加有益于车辆的定位。
[0110]
进一步地，在水平方向进行点云数据的筛选包括步骤：利用当前时刻基础 位置姿态将激光雷达实时单帧点云映射至预设定位坐标系中得到激光雷达实时 单帧点云对应的位置坐标，利用映射得到的位置坐标与路口范围点进行比较， 从上述垂直度过滤得到的第一点云数据中筛选出水平方向距离车辆当前实际的 位置处于预设范围内的点云数据，即为上述目标点云数据。
[0111]
在一个具体实施例中，基于激光雷达实时单帧点云对应位置的坐标位置在 激光雷达实时单帧点云中筛选在水平方向内处于预设范围内且满足以下条件的 点云数据：激光雷达实时单帧点云中坐标点位于路口范围内的点云数据，或者， 激光雷达实时单帧点云中坐标点与路口的水平距离小于阈值的点云数据。其中， 判断坐标点是否处于路口范围内以及坐标点与路口的水平距离可通过点云数据 与该路口的路口范围点之间的关系进行判断。
[0112]
本实施例中，由于后续匹配过程中通常是在距离车辆当前实际的位置较近 的点云数据中进行匹配，因此在本实施例中对于采集的单帧点云数据按照一定 的条件(包括水平方向的距离远近和垂直度大小)进行筛选，筛选出水平方向 在预设范围内、且垂直度大于预设垂直度阈值的目标点云数据，可以减少数据 量，从而加快后续的匹配处理过程，提高处理效率。
[0113]
步骤s630，将预设搜索步长设置为当前搜索步长，将当前时刻基础位置姿 态确定为搜索起始位置姿态。
[0114]
在一个实施例中，预设搜索步长包括预设角度搜索步长和预设水平搜索步 长。在搜索的最开始时刻，以当前时刻基础位置姿态作为起始点开始搜索，在 本实施例中，将某一次搜索的起始点记为搜索起始位置姿态。
[0115]
步骤s640，在搜索起始位置姿态的基础上，按照当前搜索步长进行搜索获 得搜索匹配位置姿态。
[0116]
步骤s650，分别基于各搜索匹配位置姿态对目标点云数据进行变换，并计 算各搜索匹配位置姿态对应变换后的目标点云数据与预设栅格地图的搜索匹配 分数。
[0117]
在搜索起始位置姿态的基础上，按照当前搜索步长进行搜索获得搜索匹配 位置姿态，包括：在搜索起始位置姿态的基础上增加或者减少当前搜索步长的 数值，搜索得到搜索匹配位置姿态。本实施例中将对各搜索匹配位置姿态计算 与预设栅格地图得到的匹配分数记为搜索匹配分数。
[0118]
在搜索得到搜索匹配位置姿态之后，可根据搜索匹配位置姿态结合车辆与 激光传感器的相对位置关系，将目标点云数据转换到与预设栅格地图一致的坐 标系中，得到变换后的目标点云数据，而后可以计算变换后的目标点云数据与 预设栅格地图的搜索匹配分数。在一个具体实施例中，预设栅格地图的坐标系 为utm世界坐标系，即根据搜索匹配位置姿态将目标点云数据转换到utm世 界坐标系中，得到变换后的目标点云数据。
[0119]
进一步地，在一个实施例中，计算搜索匹配位置姿态对应变换后目标点云 数据与预设栅格地图的搜索匹配分数，包括：对于变化后的目标点云数据中的 每一个点，若该点落入预设栅格地图的占用栅格时，对应数值记为1，若该点未 落入预设栅格地图的占用栅格(落入空闲栅格)，则对应数值记为0，计算该变 换后的目标点云数据中所有点对应的数值的和值，确定为该变换后的目标点云 数据与预设栅格地图的搜索匹配分数。
[0120]
分别计算各搜索匹配位置姿态对应变换后的目标点云数据与预设栅格地图 的搜索匹配分数，可以理解地，搜索匹配位置姿态有多少个，则对应多少个搜 索匹配分数。
[0121]
在一个具体实施例中，上述计算搜索匹配分数可以通过以下方式实现：当 前时刻基础位置姿态pi(xi,yi,yawi)设置水平坐标搜索步长为l，角度搜索步长为ε；
[0122]
计算当前时刻基础位置姿态pj(xj,yj,yawj)的匹配分数acore；
[0123]
score＝∑f(pi)；
[0124]
其中，对于目标点云数据中的每个点pi，
[0125]
由此可以获得搜索匹配到的各搜索匹配位置姿态对应的搜索匹配分数。
[0126]
步骤s660，选取分数值大于搜索起始匹配分数的搜索匹配分数对应的搜索 匹配位置姿态，确定为新的搜索起始位置姿态，返回在搜索起始位置姿态的基 础上，按照当前搜索步长进行搜索获得搜索匹配位置姿态的步骤，直至以当前 搜索步长进行搜索的次数达到预设次数阈值。
[0127]
其中，搜索起始匹配分数为搜索起始位置姿态对应变换后的目标点云数据 与预设栅格地图的匹配分数。在第一次搜索时，以当前时刻基础位置姿态为搜 索起始位置姿态，先以当前时刻基础位置姿态对目标点云数据进行变换后获得 变换后的目标点云数据，
然后计算变换后的目标点云数据与预设栅格地图的匹 配分数记为第一次搜索时搜索起始匹配分数。
[0128]
其中，预设次数阈值可根据实际情况进行设置；在一个具体实施例中，预 设次数阈值设置为2，即在以当前时刻基础位置姿态为基础搜索得到的各搜索匹 配位置姿态中，根据对应匹配分数选择大于搜索起始匹配分数的各搜索匹配位 置姿态，为当前搜索步长(第一次搜索为预设搜索步长)下的第一轮搜索；进 一步地，将选取的各搜索匹配位置姿态为基础以当前搜索步长再次进行搜索并 计算匹配分数，为当前搜索步长下的第二轮搜索；此时当前搜索步长的搜索次 数已达到预设次数阈值2次，即不再以该当前搜索步长进行进一步的搜索。
[0129]
步骤s670，从当前搜索步长搜索得到的所有搜索匹配分数中选取分数值最 大的三个搜索匹配分数对应的位置姿态，确定为三个中间搜索位置姿态。
[0130]
本实施例中，在以当前搜索步长进行搜索得到的所有位置姿态中，取匹配 分数值最大的三个位置姿态，记为三个中间搜索位置姿态。
[0131]
步骤s680，缩小当前搜索步长得到新的当前搜索步长，并分别以三个中间 搜索位置姿态为新的搜索起始位置姿态，返回在搜索起始位置姿态的基础上， 按照当前搜索步长进行搜索获得搜索匹配位置姿态的步骤，直至当前搜索步长 小于预设搜索步长阈值，停止搜索。
[0132]
在一个实施例中，缩小当前搜索步长得到新的当前搜索步长，包括：以当 前搜索步长的三分之一作为新的当前搜索步长，返回继续以新的当前搜索步长 在目标匹配分数对应的位置姿态的基础上进行搜索；在一个实施例中，在前面 的当前搜索步长的搜索中已经搜索过的点可以不再进行匹配分数的计算。
[0133]
直到当前搜索步长小于预设搜索步长阈值时，不再进行下一步的搜索；其 中，预设搜索步长阈值可以根据实际情况进行设置。
[0134]
步骤s690，获取停止搜索时依次选取的分数值最大的三个搜索匹配分数对 应的位置姿态，确定为三个中间位置姿态。
[0135]
在停止搜索时得到的所有目标匹配位置姿态中筛选匹配分数最大的三个中 间位置姿态。
[0136]
上述实施例中，通过从当前时刻基础位置姿态开始，按照搜索步长进行搜 索得到搜索匹配位置姿态，并且依次调整搜索步长后再进行搜索，从搜索获得 的搜索匹配位置姿态通过与预设栅格地图进行匹配，获得匹配分数，逐步选取 当次搜索的各匹配分数中最大的三个搜索匹配分数，也就是说通过迭代地启发 式搜索确定三个中间位置姿态，可以从多个匹配结果中确定中间位置姿态，从 而获得更优的中间位置姿态。
[0137]
在另一个实施例中，如图7所示，道路区域包括依次邻接的远场区域、过 渡区域和目标场景区域；在本实施例中，还包括步骤s710：道路区域为远场区 域，实时位置姿态确定方式包括：基于预设视觉地图、摄像头获取的实时图像 数据以及上一时刻实时位置姿态确定车辆的当前时刻实时位置姿态。
[0138]
其中，远场区域为距离目标场景区域较远的区域，在本实施例中，远场区 域、过渡区域和目标场景区域依次邻接，且远场区域、过渡区域和目标场景区 域构成道路路径；将道路路径划按照距离目标场景区域的远近程度依次划分为： 远场区域(距离目标场景区域
较远)、过渡区域(距离目标场景区域较近)以及 目标场景区域。若当前位置确定车辆所在的道路区域处于远场区域时，根据预 设视觉地图和安装在车身上的摄像头获取的实时图像数据，以及上一时刻实时 位置姿态确定车辆在远场区域的实时位置姿态。
[0139]
其中，预设视觉地图是预先构建的，包括关键帧图像的特征及其位姿信息。 其中关键帧是指通过图像形态学处理等方法筛选出特定地标在图像中的位置形 态符合要求的图像帧；图像特征包括但不限于orb(oriented fast and rotatedbrief，一种快速特征点提取和描述的算法)特征；图像数据对应的位置姿态信 息，根据关键帧图像时间戳，利用组合导航设备采集的位姿数据插值生成。
[0140]
在一个实施例中，基于预设视觉地图、摄像头获取的实时图像数据以及上 一时刻实时位置姿态确定车辆的当前时刻实时位置姿态，包括：在远场区域中 通过摄像头实时采集车辆的实时图像数据；在预设视觉地图中查找与实时图像 数据匹配的位置点；将车辆的基础位置姿态替换为实时图像数据匹配的位置点 对应的位置姿态，获得车辆在远场区域的实时位置姿态。
[0141]
进一步地，在一个实施例中，在预设视觉地图中查找与实时图像数据匹配 的位置点，包括：在预设视觉地图中提取当前场景的场景地图(预设视觉地图 中包括多个场景地图，每个场景地图对应一个位置姿态)；计算实时图像数据与 场景地图中路侧参照标识物的特征匹配分数；若特征匹配分数高于预设阈值， 确定场景地图对应的位置点为与实时图像数据匹配的位置点。
[0142]
本实施例中，在车辆所处道路区域为远场区域时，通过获取车辆周围的实 时图像数据，结合预设视觉地图确定车辆当前的初始世界坐标位置，进而基于 该初始世界坐标位置结合航位推算车辆在远场区域中的实时位置姿态。
[0143]
在一个具体实施例中，在车辆的行驶过程中，若车辆所处的道路区域为远 场区域，通过摄像头采集车辆的实时图像数据，并利用和生成预设视觉地图过 程一致的方式检测特定的地标，同时结合车辆当前所在的大致位置和预设导航 路径的途径地标顺序，快速从预设视觉地图中提取出当前所处位置的场景地图， 通过实时图像数据与地图中的地标间的特征匹配程度，结合预设阈值判断是否 抵达地图所在位置点。若符合，则利用地图对应位置姿态替代车辆的基础位置 姿态；否则，则继续下一帧图像处理。通过一个提取的视觉地图位置点后，则 依据途径地图点顺序向后继续进行视觉定位。需要说明的是，结合预设视觉地 图进行校正车辆位置姿态的过程在目标场景区域中不执行。
[0144]
进一步地，在一个实施例中，从车辆处于远场区域的第二个时刻开始，每 一时刻的实时位置姿态均以上一时刻的实时位置姿态为基础，结合预设视觉地 图与摄像头获取的实时图像数据计算当前时刻实时位置姿态。
[0145]
请继续参照图7，更进一步地，当根据当前位置确定车辆从远场区域驶入过 渡区域时，还包括：获取上一时刻车辆在远场区域的实时位置姿态，进行航位 推算后得到车辆的当前时刻基础位置姿态，结合预设栅格地图和当前时刻采集 的激光雷达实时单帧点云确定车辆在过渡区域的当前时刻实时位置姿态。当车 辆从过渡区域驶入目标场景区域时，还包括：获取上一时刻车辆在过渡区域的 实时位置姿态，基于预设激光雷达点云地图、当前时刻采集的激光雷达实时单 帧点云以及上一时刻实时位置姿态确定车辆的当前时刻实时位置姿态。
[0146]
在本实施例中，由于已知车辆是从远场区域驶入过渡区域，因此将远场区 域最后一个时刻对应的实时位置姿态为基础，进行航位推算后获得的位置姿态 作为在过渡区域的第一个时刻的基础位置姿态，结合预设栅格地图和采集的激 光雷达实时单帧点云确定车辆在过渡区域的当前时刻实时；在过渡区域的第二 个时刻开始以上一个时刻的实时位置姿态作为基础，确定车辆在过渡区域的实 时位置姿态。当车辆从过渡区域驶入目标场景区域则与上述实施例中从过渡区 域驶入目标场景区域中的定位方法相同。
[0147]
本实施例中，实际对应的是车辆从某一路口的远场区域开始依次途径远场 区域、过渡区域和远场区域的行驶过程。步骤可概括为：
①
车辆启动时刻，获 取启动时刻位置姿态，车辆在远场区域：先基于预设视觉地图和摄像头采集的 实时图像数据进行匹配，而后根据匹配结果校正启动时刻位置姿态，获得车辆 在远场区域的实时位置姿态，非启动时刻则以上一时刻的实时位置姿态进行航 位推算获得当前时刻的基础位置姿态，基于基础位置姿态进行匹配，从而确定 非启动时刻在远场区域的当前时刻实时位置姿态。
②
当车辆从远场区域驶入过 渡区域时，获取上一时刻的实时位置姿态进行航位推算后获得的当前时刻的基 础位置姿态，在过渡区域：以预设栅格地图和激光雷达实时单帧点云确定当前 时刻实时位置姿态。
③
当车辆从过渡区域驶入过渡区域后，获取上一时刻的实 时位置姿态进行航位推算后获得的当前时刻的基础位置姿态，在目标场景区域： 基于预设激光雷达点云地图和基础位置姿态确定目标场景区域的当前时刻实时 位置姿态。
[0148]
本实施例中在远场区域、过渡区域和目标场景区域分别采用不同的方法确 定车辆的实时位置姿态，而每个时刻的实时位置姿态均以上一时刻计算得到的 实时位置姿态为基础计算确定，在过渡区域中结合预设栅格地图可获得较高精 度的位置姿态，从而车辆进入目标场景区域时，可以得到精度较高的位置姿态 作为定位初值，使得在目标场景区域的定位更为准确。
[0149]
如图8所示为一个具体实施例中，目标场景区域的车辆定位方法的完整流 程示意图，以车辆从启动时刻，处于远场区域为例，包括以下步骤：获取当前 位置和上一时刻实时位置姿态，或者获取当前位置和启动时刻位置姿态，根据 当前位置确定当前所处的道路区域，若启动时刻当前所处道路区域为远场区域， 执行远场区域的定位方法：基于预设视觉地图和摄像头采集的实时图像数据进 行匹配，而后根据匹配结果校正启动时刻位置姿态，得到车辆在远场区域的实 时位置姿态；若非启动时刻则以上一时刻实时位置姿态进行航位推算获得当前 时刻基础位置姿态，基于当前时刻基础位置姿态进行匹配，从而确定非启动时 刻在远场区域的实时位置姿态。
[0150]
当车辆从远场区域驶入过渡区域，则基于预设栅格地图和激光雷达实时单 帧点云确定车辆在过渡区域的当前时刻实时位置姿态。进一步地，在一个具体 实施例中，基于上一时刻实时位置姿态和预设栅格地图确定车辆在过渡区域的 当前时刻实时位置姿态如图9所示，包括以下步骤：首先，进行单帧点云数据 裁剪：通过激光雷达采集车辆的激光雷达实时单帧点云，结合车辆与激光传感 器的相对位置关系以及当前时刻基础位置姿态，将激光雷达实时单帧点云映射 到与当前位置对应的定位坐标系中，获得定位坐标；通过以下条件进行筛选： 坐标点位于路口范围内，或者，坐标点与路口的水平距离小于阈值，筛选获得 第一点云数据，单帧点云数据裁剪完成。其次，在第一点云数据的基础上进行 垂直度提取，分别计算第一点云数据的垂直度，保留垂直度值大于预设垂直度 阈值的点，获得目标
点云数据，垂直度提取完成。然后，在目标点云数据中， 以当前时刻基础位置姿态为基础，在预设栅格地图中进行迭代启发式搜索，具 体采用预设步长搜索得到与预设栅格地图匹配分数最大的三个匹配结果，然后 缩小预设步长得到新的搜索步长，基于新的搜索步长再次进行搜索得到与预设 栅格地图匹配分数最大的三个匹配结果，直至搜索步长小于预设搜索步长阈值 l
min
时停止搜索，停止搜索时的三个匹配结果对应位置姿态中确定为三个中间位 置姿态。进一步地，基于该三个中间位置姿态两两之间的差异从中确定当前时 刻实时位置姿态。在一个具体实施例中，基于该三个中间位置姿态两两之间的 差异从中确定当前时刻实时位置姿态可参见上述实施例中的描述。
[0151]
进一步地，当车辆从过渡区域驶入目标场景区域(路口)，则以过渡区域输 出的实时位置姿态作为目标场景区域的定位初值基础，计算车辆在目标场景区 域的实时位置姿态。在一个具体实施例中，在目标场景区域计算实时位置包括 步骤：利用卡尔曼滤波融合激光雷达和imu数据来实现车辆位置的估计。其中 imu推算用于高频预测车辆状态、基于激光雷达的地图匹配用于高精度地更新 车辆状态，实现高频高精度的定位输出。
[0152]
特别的，为保证在目标场景区域定位时的地图匹配精度，需提前根据激光 雷达采集时间段的运动状态变化量对激光雷达单帧数据进行运动补偿；为提升 匹配效率，可适当去除距离传感器过近或过远的无效点数据，同时利用点云下 采样的方式降低数据冗余，尤其是地面点的数据冗余。在此基础上，构建单帧 点云与路口地图的对应关系，求解最优匹配参数，实现单帧点云与地图点云间 的误差函数最小。
[0153]
本实施例中，所采用的航位推算方式主要包括基于轮速/imu的推算和激光 实时建图与定位两种。因此，需根据所处区域的不同需求，采用合适的航位推 算方式。例如在远场区域通常采用激光实时建图与定位进行位置姿态估计，能 够有效控制定位的累计误差；特别地，每当驶出路口或视觉定位校正位置后， 地图将重新开始累加生成。在一个实施例中，车辆在远场区域时可采用任意一 种方法实现激光实时建图与定位；在过渡区域和目标场景区域，通常采用计算 量更小的轮速和imu结合的方式实现高效高频的航位推算，可达到高频输出定 位状态的效果。
[0154]
进一步地，本实施例中需预先进行地图数据的采集和构建视觉语义地图和 激光雷达点云地图。具体包括以下过程：
[0155]
地图数据采集：通过搭载激光雷达、摄像头、高精度组合导航设备等传感 器的车辆作为地图数据采集车，采集地图构建所需数据。包括组合导航设备采 集的位姿数据、路口范围内激光雷达采集的点云数据、摄像头采集的路口前区 域包括视觉地标的图像数据等。
[0156]
视觉语义地图构建：视觉地图包括关键帧图像的特征及其位姿信息。其中 关键帧是指通过图像形态学处理等方法筛选出特定地标在图像中的位置形态符 合要求的图像帧；图像特征包括但不限于orb特征；图像位姿信息，根据关键 帧图像时间戳，利用组合导航设备采集的位姿数据插值生成。
[0157]
激光定位地图构建：依据激光雷达采集的单帧点云的时间戳，插值该时刻 对应的位姿，结合组合导航设备与激光雷达的外参(相对位置关系)，可将路口范 围的所有单帧点云全部进行坐标变换，合并生成路口区域的点云数据。将路口 点云经去噪、下采样、特征提取等处理后生成路口点云地图，用于路口定位； 将路口点云地图进一步基于垂直度过滤，
并进行空间栅格划分，根据每一个点 的水平坐标计算对应栅格，若有点落入则记为占用，否则记为空闲，生成占用 栅格地图，用于过渡区定位。
[0158]
对于需执行目标场景区域定位的车辆，在确定起始点、途经点、终止点后， 其导航路径随即确定，即预设导航路径。将预设导航路径进行间隔采样可获得 离散的位置点，通过顺序逐个位置点计算与所有路口范围点围成的多边形的位 置关系，可以获取得到导航路径沿途经过的路口索引及其顺序。对预设导航路 径中顺序获取的位置点(x,y)首先计算出距离最近的路口，若该距离大于阈值，则 直接跳过；否则，则需进一步计算各位置点与最近路口范围点围成的多边形的 位置关系。通过遍历所有位置点，则获取了预设导航路径中沿途所有的路口索 引及其顺序。在一个具体实施例中，路口索引提取包括以下步骤：
[0159]
(1)将所有的路口范围点合并生成一个点云数据，每个点都包含位置和索 引信息pk(xk,yk,indexk)；
[0160]
(2)根据导航路径采样生成车辆顺序经过的离散位置点(p0,p1,p2,
…
,pn)；
[0161]
(3)从路口范围点云数据中心，搜索与位置点pi距离最近dm的路口范围点pm；
[0162]
(4)若该距离dm小于阈值d，计算位置点pi与路口indexm的相对位置关系；
[0163]
(5)若位置点pi位于路口范围内，且位置点p
i-1
不在路口范围内，则记录该 路口的索引；
[0164]
(6)令i＝i 1，重复(3)～(5)直到所有位置点完成搜索。
[0165]
(7)顺序记录的路口索引，即车辆即将经过的路口顺序；
[0166]
本过程仅在导航路径发生改变时需要执行，若导航路径不发生改变则仅需 在导航路径生成时同时执行一次。
[0167]
进一步地，地图预加载：目的是提前加载路口地图，消除地图加载耗时导 致的定位延时。地图预加载主要在两种情况下执行：(1)启动时提前加载下一个 路口的地图；(2)驶出当前路口，提前加载下一个路口的地图。
[0168]
更进一步地，在一个实施例中，还提供一种应用场景，该应用场景中上述 目标场景区域的车辆定位方法应用于无人驾驶车辆。
[0169]
本实施例中的目标场景区域的车辆定位方法中，可适用于车辆智能驾驶中 在目标区域感知和高精度定位，关键是邻近目标场景的过渡区域的定位方法， 包括在远场区域中利用里程计/航位推算不断推估当前位置信息同时使用摄像头 对地标物的探测和匹配以校正当前车辆位置姿态；在目标场景前的过渡区域， 利用基于占用栅格的激光定位算法，逐步提升定位精度；最终使得车辆在进入 目标场景时，具备较高精度的定位初值，从而保障车辆在目标场景中实现快速 准确的高精度定位。通过结合使用高频实时imu数据，实现车辆位姿的高精度、 高频率输出，满足了无gnss(global navigation satellite system，全球卫星导 航系统)辅助下的目标区域定位需求。通过上述方法只需要构建有限区域的激 光雷达和视觉定位地图，通过与车道保持技术相结合，即可实现全区域的自动 驾驶。相比传统的利用rtk(real-time kinematic，实时动态)进行定位的方法， 不会受到多径效应，通讯信号被干扰等因素的影响。相比现有利用全局高精度 地图的方法，具有采集相对简单，制作周期短，更新速度快，存储空间小等诸 多优势，可节省大量人力物力财力。
[0170]
应该理解的是，虽然图2-9的流程图中的各个步骤按照箭头的指示依次显示， 但
是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的 说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执 行。而且，图2-9中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步 骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这 些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其 它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
[0171]
在一个实施例中，如图10所示，提供了一种目标场景区域的车辆定位装置， 包括：获取模块1010、道路区域确定模块1020和定位模块1030，其中：
[0172]
获取模块1010，用于获取车辆的当前位置以及上一时刻实时位置姿态；
[0173]
道路区域确定模块1020，用于根据当前位置确定当前时刻所处的道路区域， 道路区域包括邻接的过渡区域和目标场景区域；
[0174]
定位模块1030，用于根据道路区域确定对应的实时位置姿态确定方式，基 于上一时刻实时位置姿态和实时位置姿态确定方式确定车辆的当前时刻实时位 置姿态。其中，定位模块1030包括过渡区域定位模块1031，用于在道路区域为 过渡区域时，基于预设栅格地图、当前时刻采集的激光雷达实时单帧点云以及 上一时刻实时位置姿态确定车辆的当前时刻实时位置姿态；定位模块1030还包 括目标场景区域定位模块1032，用于在道路区域为目标场景区域时，基于预设 激光雷达点云地图、当前时刻采集的激光雷达实时单帧点云以及上一时刻实时 位置姿态确定车辆的当前时刻实时位置姿态。
[0175]
上述目标场景区域的车辆定位装置，获取车辆的当前位置和上一时刻车辆 的实时位置姿态，根据当前位置确定车辆所处的道路区域，其中，道路区域包 括邻接的过渡区域和目标场景区域；根据道路区域确定对应的实时位置姿态确 定方式，基于上一时刻实时位置姿态和确定的实时位置姿态确定方式确定车辆 的当前实时位置姿态。其中，若道路区域为过渡区域，基于预设栅格地图当前 时刻采集的激光雷达实时单帧点云以及所述上一时刻实时位置姿态确定所述车 辆的当前时刻实时位置姿态；若道路区域为目标场景区域，基于预设激光雷达 点云地图、当前时刻采集的激光雷达实时单帧点云以及所述上一时刻实时位置 姿态确定所述车辆的当前时刻实时位置姿态。上述方法车辆在过渡区域时，利 用预设栅格地图和激光雷达实时单帧点云确定实时位置姿态，可以提升在过渡 区域的定位精度，从而车辆在进入目标场景区域时，具备高精度的定位初值， 车辆在目标场景区域中以该定位初值为基础进行计算实时位置姿态，可保障车 辆在目标场景区域中实现快速准确的高精度定位，实现了车辆位置的高精度输 出，满足了在无车道线等复杂路况下的车辆定位需求，且仅需构建部分区域的 栅格地图和激光雷达地图，成本低且制作周期短。
[0176]
在一个实施例中，若当前时刻非车辆启动时刻，进入获取模块1010获取车 辆的当前位置以及上一时刻实时位置姿态的步骤；若当前时刻为车辆启动时刻， 获取模块1010用于获取车辆的当前位置以及车辆的启动时刻位置姿态；根据道 路区域确定对应的实时位置姿态确定方式，基于启动时刻位置姿态和实时位置 姿态确定方式确定车辆的当前时刻实时位置姿态。
[0177]
在一个实施例中，上述过渡区域定位模块1031具体用于：基于上一时刻实 时位置姿态进行航位推算，得到当前时刻基础位置姿态；基于当前时刻基础位 置姿态、预设栅格地图以及当前时刻采集的激光雷达实时单帧点云确定车辆的 当前时刻实时位置姿态；上
述目标场景区域定位模块1032具体用于：基于上一 时刻实时位置姿态进行航位推算，得到当前时刻基础位置姿态；基于当前时刻 基础位置姿态、预设激光雷达点云地图以及当前时刻采集的激光雷达实时单帧 点云确定车辆的当前时刻实时位置姿态。
[0178]
在一个实施例中，上述过渡区域定位模块1031具体用于：根据所述航位推 算的结果获得所述当时时刻相对上一时刻的位置姿态的变化信息对当前时刻采 集的激光雷达实时单帧点云进行补偿，获得补偿后的单帧点云；基于预设栅格 地图和补偿后的实时单帧点云求解获得当前时刻实时位置姿态；上述目标场景 区域定位模块1032具体用于：根据航位推算的结果获得当时时刻相对上一时刻 的位置姿态的变化信息对当前时刻采集的激光雷达实时单帧点云进行补偿，获 得补偿后的单帧点云；基于预设栅格地图和补偿后的实时单帧点云求解获得当 前时刻实时位置姿态。
[0179]
在一个实施例中，上述过渡区域定位单元1011包括：搜索单元，用于基于 当前时刻基础位置姿态在预设栅格地图中启发式搜索与激光雷达实时单帧点云 匹配的三个中间位置姿态；实时估计位置姿态确定单元，用于根据三个中间位 置姿态之间的位置差异输出车辆的当前时刻实时估计位置姿态；过渡区域定位 单元1011具体用于：基于当前时刻实时估计位置姿态对当前时刻基础位置姿态 进行校正，得到当前时刻实时位置姿态。
[0180]
进一步地，在一个实施例中，上述搜索单元包括：位置映射子单元，用于 根据当前时刻基础位置姿态将激光雷达实时单帧点云映射至预设定位坐标系， 获得激光雷达实时单帧点云对应的位置坐标；
[0181]
点云筛选子单元，用于基于激光雷达实时单帧点云对应的位置坐标在激光 雷达实时单帧点云中筛选在水平方向内处于预设范围内，且垂直度大于预设垂 直度阈值的目标点云数据；
[0182]
设置子单元，用于将预设搜索步长确定为当前搜索步长，搜索起始位置姿 态确定子单元，用于将当前时刻基础位置姿态确定为搜索起始位置姿态；
[0183]
搜索子单元，用于在搜索起始位置姿态的基础上，按照当前搜索步长进行 搜索获得搜索匹配位置姿态；
[0184]
匹配分数计算子单元，用于分别基于各搜索匹配位置姿态对目标点云数据 进行变换，并计算各搜索匹配位置姿态对应变换后的目标点云数据与预设栅格 地图的搜索匹配分数；
[0185]
第一返回子单元，用于选取分数值大于搜索起始匹配分数的搜索匹配分数 对应的搜索匹配位置姿态，确定为新的搜索起始位置姿态，返回在搜索起始位 置姿态的基础上，按照当前搜索步长进行搜索获得搜索匹配位置姿态的步骤， 直至以当前搜索步长进行搜索的次数达到预设次数阈值；其中，搜索起始匹配 分数为搜索起始位置姿态对应变换后的目标点云数据与预设栅格地图的匹配分 数。
[0186]
中间搜索位置姿态确定子单元，用于从当前搜索步长搜索得到的所有搜索 匹配分数中选取分数值最大的三个搜索匹配分数对应的位置姿态，确定为三个 中间搜索位置姿态。
[0187]
第二返回子单元，用于缩小预设搜索步长获得新的当前搜索步长，并分别 以三个中间搜索位置姿态确定为新的搜索起始位置姿态，返回在搜索起始位置 姿态的基础上，按照当前搜索步长进行搜索获得搜索匹配位置姿态的步骤，直 至搜索步长小于预设搜索步
长阈值，停止搜索；
[0188]
中间位姿姿态确定子单元，用于获取停止搜索时依次选取的分数值最大的 三个搜索匹配分数对应的位置姿态，确定为三个中间位置姿态。
[0189]
在一个实施例中，上述装置的实时估计位置姿态确定单元包括：位置姿态 差异计算子单元，用于计算第一中间位置姿态、第二中间位置姿态与第三中间 位置姿态两两之间的差异值，其中，三个中间位置姿态包括第一中间位置姿态、 第二中间位置姿态和第三中间位置姿态；当前时刻实时位置姿态确定子单元， 用于若第一中间位置姿态与第二中间位置姿态的差异值、以及第一中间位置姿 态与第三中间位置姿态之间的差异值均大于预设阈值，输出第一中间位置姿态 为当前时刻实时估计位置姿态；若第一中间位置姿态与第二中间位置姿态之间 的差异值小于预设阈值，且第一中间位置姿态与第三中间位置姿态之间的差异 值、以及第二中间位置姿态与第三中间位置姿态之间的差异值均大于预设阈值， 基于第一中间位置姿态、第二中间位置姿态和第一预设权重系数输出当前时刻 实时估计位置姿态；若第一中间位置姿态与第二中间位置姿态之间的差异值、 以及第一中间位置姿态与第三中间位置姿态之间的差异值均小于预设阈值，或 者，第一中间位置姿态与第二中间位置姿态之间的差异值、以及第二中间位置 姿态与第三中间位置姿态之间的差异值均小于预设阈值，基于第一中间位置姿 态、第二中间位置姿态、第三中间位置姿态和第二预设权重系数输出当前时刻 实时估计位置姿态。
[0190]
在一个实施例中，道路区域包括依次邻接的远场区域、过渡区域和目标场 景区域；在本实施例中，上述定位模块1030还用于若道路区域为远场区域，基 于预设视觉地图、摄像头获取的实时图像数据以及上一时刻实时位置姿态确定 车辆的当前时刻实时位置姿态。
[0191]
进一步地，在一个实施例中，上述装置的远场区域定位模块1013包括：位 置点匹配单元，在预设视觉地图中查找与实时图像数据匹配的位置点；航位推 算单元，用于对上一时刻实时位置姿态进行航位推算，得到当前时刻基础位置 姿态；位置校正单元，用于基于匹配的位置点对应的位置姿态对当前时刻基础 位置姿态进行校正，得到当前时刻实时位置姿态。
[0192]
关于目标场景区域的车辆定位装置的具体限定可以参见上文中对于目标场 景区域的车辆定位方法的限定，在此不再赘述。上述目标场景区域的车辆定位 装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块 可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存 储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操 作。
[0193]
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其 内部结构图可以如图11所示。该计算机设备包括通过系统总线连接的处理器、 存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提 供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。 该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存 储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口 用于与外部的终端进行有线或无线方式的通信，无线方式可通过wifi、运营商 网络、nfc(近场通信)或其他技术实现。该计算机程序被处理器执行时以实 现一种目标场景区域的车辆定位方法。该计算机设备的显示屏可以是液晶显示 屏或者电
子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸 层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接 的键盘、触控板或鼠标等。
[0194]
本领域技术人员可以理解，图11中示出的结构，仅仅是与本技术方案相关 的部分结构的框图，并不构成对本技术方案所应用于其上的计算机设备的限定， 具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件， 或者具有不同的部件布置。
[0195]
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器 中存储有计算机程序，该处理器执行计算机程序时实现上述任意一个实施例中 的目标场景区域的车辆定位方法的步骤。
[0196]
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程 序，计算机程序被处理器执行时实现上述任意一个实施例中的目标场景区域的 车辆定位方法的步骤。
[0197]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程， 是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于 一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述 各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、 存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的 至少一种。非易失性存储器可包括只读存储器(read-only memory，rom)、磁 带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(randomaccess memory，ram)或外部高速缓冲存储器。作为说明而非局限，ram可 以是多种形式，比如静态随机存取存储器(static random access memory，sram) 或动态随机存取存储器(dynamic random access memory，dram)等。
[0198]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述 实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特 征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0199]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细， 但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的 普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改 进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权 利要求为准。