CN115905449B

CN115905449B - 语义地图构建方法及具有熟路模式的自动驾驶系统

Info

Publication number: CN115905449B
Application number: CN202211722050.1A
Authority: CN
Inventors: 李雪; 范圣印; 孙文昭
Original assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Current assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Priority date: 2022-12-30
Filing date: 2022-12-30
Publication date: 2023-07-21
Anticipated expiration: 2042-12-30
Also published as: CN116821266B; CN115905449A; CN116821266A

Abstract

本公开提供了语义地图构建方法，包括：获取本车当前行驶过程的多传感器数据，多传感器数据包括本车定位数据、本车位姿数据及路况图像数据；基于本车定位数据及本车位姿数据获取本车当前行驶过程的本车当前行驶轨迹，基于路况图像数据获取本车当前行驶过程的语义对象信息；基于本车当前行驶轨迹及语义对象信息为本车当前行驶过程建立新语义地图；基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；以及对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图。本公开还提供了语义地图构建装置、具有熟路模式的自动驾驶系统、电子设备、可读存储介质及程序产品。

Description

语义地图构建方法及具有熟路模式的自动驾驶系统

技术领域

本公开涉及自动驾驶技术领域，本公开尤其涉及一种语义地图构建方法、语义地图构建装置、具有熟路模式的自动驾驶系统、电子设备、存储介质及程序产品。

背景技术

现有的自动驾驶对高精地图提出了高实时性、高精度等要求，而高精地图的获取需要昂贵的专业采集车和人工编辑，难以实现地图的快速更新且费用较高。但大多数车主在实际开车过程中多是每天走重复的路线，即通勤。因此在自动驾驶还并不成熟，尤其是众包建图还不被允许的当前阶段，能够简单快速适用于大多数行驶时间的熟路模式具有重要的理论意义和实际应用价值。

以下为现有技术的一些技术方案：

技术方案1：公开号为CN111382370A的《线路推荐方法、装置、车载设备和存储介质》，涉及数字数据信息检索、卫星无线电信标定位系统等技术领域。具体实现方案为：1)根据当前位置信息及当前时间信息，确定预测目的地；2)根据当前位置信息及预测目的地确定目标熟路组，目标熟路组包括至少一条熟路，熟路包括起点信息、终点信息及熟路轨迹；3)熟路对应的轨迹次数大于预设值；4)当熟路的起点信息与当前位置信息不属于同一路段时，将目标熟路组中的预设熟路轨迹确定为目标熟路轨迹；5)根据当前位置信息对目标熟路轨迹进行补足，得到补足轨迹；6)根据目标熟路轨迹及补足轨迹确定推荐线路。该方法通过计算机技术、智能交通等技术领域的融合，借助熟路模式减少了行驶时间，提高了导航效率。但该方法在数据融合过程中仅借助车载导航信息来进行轨迹辨认，在用户不使用导航的情况下有较大的局限性。

技术方案2：公开号为CN111982145A的《行驶路径推荐方法、装置、设备及存储介质》，涉及智能交通和自动驾驶领域，具体地涉及行驶路径规划领域。该方法包括：1)从多个车辆接收多个行驶信息，行驶信息包括车辆的行驶路径、行驶耗时和行驶路况信息；2)针对具有相同行驶起点和行驶终点的每一个行驶路径，根据车辆的行驶耗时和行驶路况信息从多个车辆中筛选出目标车辆；3)将目标车辆的行驶路径作为候选熟路路径存储在熟路数据集中；4)以及根据熟路数据集进行行驶路径推荐。该方法通过智能交通和自动驾驶结合的方式，借助多车传输的信息，获取目标车辆的熟路数据，实现基于熟路模式的自动驾驶。但该方法需要借助同起点终点行驶的其他车辆上传的信息，在不允许使用众包建图的环境下难以立即使用。

技术方案3：公开号为CN113762583A的《一种路线规划方法、装置、设备及计算机可读存储介质》，涉及地图、导航、自动驾驶、智慧交通等多个领域。该方法包括：1)响应于目标对象针对展示的路线规划界面中起终点输入区域的操作，获取路线起点和路线终点；2)将路线起点和路线终点携带在路线规划请求中，发送给服务器；3)接收服务器针对路线规划请求返回的包含目标对象的至少一个目标熟路路线的规划路线，其中，规划路线是依据至少一个目标熟路路线的各个骨架点、路线起点和路线终点确定出的；4)将规划路线展示在路线规划界面的路线展示区域中。该方法在熟路模式的基础上支持用户自行选择熟路路线，增加了熟路路线的泛化能力，提高了用户的熟路路线的利用率。但该方法需要用户参与输入起点终点来获取路线起点终点信息，需要用户有意识的进行操作，给用户的体验带来了负担。

技术方案1融合了数字数据信息检索、卫星无线电信标定位系统等技术领域，提出了当前环境下允许实现的熟路模式的路线推荐算法，但方案1在数据融合过程中仅借助车载导航信息来进行轨迹辨认，在用户不使用导航的情况下有较大的局限性。技术方案2融合了智能交通、自动驾驶等领域，提出了适用性范围广且精确的熟路模式算法，但方案2的主要技术涉及众包建图。技术方案3融合了地图、导航、自动驾驶、智慧交通等多个领域，该方法在熟路模式的基础上支持用户自行选择熟路路线，增加了熟路路线的泛化能力，提高了用户的熟路路线的利用率。但该方法需要用户参与输入起点终点来获取路线起点终点信息，需要用户有意识的进行操作，给用户的体验带来了负担。

发明内容

为了解决上述技术问题中的至少一个，本公开提供了语义地图构建方法、语义地图构建装置、具有熟路模式的自动驾驶系统、电子设备、存储介质及程序产品。

根据本公开的一个方面，提供一种语义地图构建方法，包括：

获取本车当前行驶过程的多传感器数据，所述多传感器数据至少包括本车定位数据、本车位姿数据及路况图像数据；

基于本车定位数据及本车位姿数据获取本车当前行驶过程的本车当前行驶轨迹，基于所述路况图像数据获取本车当前行驶过程的语义对象信息；

基于所述本车当前行驶轨迹及所述语义对象信息为本车当前行驶过程建立新语义地图；

基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；

对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图。

根据本公开的至少一个实施方式的语义地图构建方法，还包括：

如果本车当前行驶过程与已存储在现有轨迹组的某个轨迹匹配，则基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对所述现有轨迹组的唯一语义地图进行验证和/或更新。

根据本公开的至少一个实施方式的语义地图构建方法，本车定位数据基于RTK系统和/或GNSS系统获取；本车位姿数据基于IMU数据及轮速数据获取。

根据本公开的至少一个实施方式的语义地图构建方法，基于车载视觉传感器获取本车当前行驶过程中的路况图像数据。

根据本公开的至少一个实施方式的语义地图构建方法，基于所述本车当前行驶轨迹及所述语义对象信息为本车当前行驶过程建立新语义地图，包括：

基于所述本车当前行驶轨迹及所述语义对象信息为本车当前行驶过程建立完整的新语义地图；

所述语义地图包括轨迹层及语义层；

所述轨迹层至少包括轨迹起点、轨迹终点及行驶路径；

所述语义层至少包括空中语义对象及地面语义对象。

根据本公开的至少一个实施方式的语义地图构建方法，基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，包括：

基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组；

其中，所述轨迹编码包括轨迹起点编号、轨迹终点编号及行驶路径编号。

根据本公开的至少一个实施方式的语义地图构建方法，基于轨迹重合程度，将同一个轨迹组内的行驶轨迹划分为不同的轨迹小组。

根据本公开的至少一个实施方式的语义地图构建方法，获得更新的轨迹组，包括：对该轨迹组的各个轨迹小组的权重进行更新，对该轨迹组的每个轨迹小组的各个行驶轨迹的权重进行更新。

根据本公开的至少一个实施方式的语义地图构建方法，各个轨迹小组的权重以及每个轨迹小组的各个行驶轨迹的权重均具有更新时间戳，以基于更新时间戳对行驶轨迹进行更新。

根据本公开的至少一个实施方式的语义地图构建方法，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，包括：

基于当前行驶轨迹的轨迹起点/轨迹终点与现有轨迹组的起点终点信息进行空间判定，以判定当前行驶轨迹是否归属于某个现有轨迹组；

若通过空间判定，则对当前行驶轨迹赋予轨迹编码以划分至现有轨迹组；

若未通过空间判定，则为当前行驶轨迹新建一个轨迹组，并将当前行驶轨迹的轨迹起点及轨迹终点作为该轨迹组的起点终点信息。

根据本公开的至少一个实施方式的语义地图构建方法，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，还包括：

若当前行驶轨迹为新建轨迹组的第一条轨迹，则将其起点终点时间戳作为该新建轨迹组的起点终点时间信息保存，以使得通过空间判定的后续行驶轨迹基于其起点终点时间戳及现有轨迹组的起点终点时间信息进行时间关联判定，通过空间判定及时间关联判定的轨迹划分至同一个轨迹组。

根据本公开的至少一个实施方式的语义地图构建方法，基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，还包括：

基于当前行驶轨迹的新语义地图对当前行驶轨迹的所在轨迹组的轨迹进行聚类，以更新所在轨迹组的各个轨迹的权重。

根据本公开的至少一个实施方式的语义地图构建方法，对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图，包括：

基于本车定位数据和语义对象信息中的空中语义对象将同轨迹组的各个语义地图转换至同一个坐标系之下，完成各个语义地图之间的粗粒度对齐；

基于语义对象信息中的地面语义对象获得地面语义对象之间的关联关系，以完成各个语义地图之间的细粒度对齐；

基于紧耦合的方式获得同轨迹组的唯一语义地图。

根据本公开的至少一个实施方式的语义地图构建方法，基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对所述现有轨迹组的唯一语义地图进行验证和/或更新，包括：

语义对象的验证与更新；

轨迹定位精准性的验证与更新；

轨迹预测精准性的验证与更新。

根据本公开的至少一个实施方式的语义地图构建方法，语义对象的验证与更新，包括：

若当前行驶轨迹上检测到了语义对象，且在所述唯一语义地图中能够找到对应坐标的语义对象，则语义对象验证成功；

若出现了验证不成功的语义对象，则进行唯一语义地图的语义对象更新。

根据本公开的至少一个实施方式的语义地图构建方法，轨迹定位精准性的验证与更新，包括：

基于所述唯一语义地图为本车当前行驶过程生成融合语义对象信息及本车位姿数据的轨迹Trajectory_Semantic；

为本车当前行驶过程生成融合本车定位数据及本车位姿数据的轨迹Trajectory_RTK；

计算轨迹Trajectory_Semantic和轨迹Trajectory_RTK的相对位姿误差或绝对轨迹误差，若两者(轨迹Trajectory_Semantic和轨迹Trajectory_RTK)的平均精度达到预设阈值，则判定该唯一语义地图的轨迹定位有效。

根据本公开的至少一个实施方式的语义地图构建方法，轨迹预测精准性的验证与更新，包括：

至少基于本车当前行驶过程的所述轨迹Trajectory_Semantic与本车自动规划的规划轨迹的重合度判断所述自动规划是否成功；

如果成功，该轨迹Trajectory_Semantic匹配的轨迹组的唯一语义地图验证成功，记录成功次数，以能够用于具有熟路模式的自动驾驶系统。

根据本公开的至少一个实施方式的语义地图构建方法，本车自动规划的规划轨迹包括规划避障轨迹。

根据本公开的另一个方面，提供了语义地图构建装置，包括：

行驶轨迹生成模块，所述行驶轨迹生成模块基于本车定位数据及本车位姿数据获取本车当前行驶过程的本车当前行驶轨迹；

语义对象信息获取模块，所述语义对象信息获取模块基于获取的路况图像数据获取本车当前行驶过程的语义对象信息；

语义地图生成模块，所述语义地图生成模块基于所述本车当前行驶轨迹及所述语义对象信息为本车当前行驶过程建立新语义地图；

轨迹组获取模块，所述轨迹组获取模块基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；

语义地图处理模块，所述语义地图处理模块对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图。

根据本公开的至少一个实施方式的语义地图构建装置，还包括：

验证更新模块，如果本车当前行驶过程与已存储在现有轨迹组的某个轨迹匹配，则所述验证更新模块基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对所述现有轨迹组的唯一语义地图进行验证和/或更新。

根据本公开的又一个方面，提供一种具有熟路模式的自动驾驶系统，包括：

存储模块，所述存储模块存储有基于本公开任一个实施方式的语义地图构建装置或本公开任一个实施方式的语义地图构建方法获取的轨迹组及各个轨迹组的唯一语义地图；

定位匹配模块，所述定位匹配模块对本车当前定位数据与所述存储模块存储的轨迹组起点定位数据进行匹配；

局部语义地图获取模块，所述局部语义地图获取模块基于所述定位匹配模块的定位匹配结果从匹配的轨迹组的唯一语义地图中得到局部语义地图；

语义定位模块，所述语义定位模块基于当前路况图像数据中的语义对象信息及所述局部语义地图中的语义对象信息在所述局部语义地图中进行轨迹起点语义定位；

轨迹组确认模块，所述轨迹组确认模块基于所述语义定位模块的轨迹起点语义定位结果输出至少一个待确认轨迹组；

自动驾驶规划模块，所述自动驾驶规划模块基于被用户操作指令确认的轨迹组进行熟路模式下的自动驾驶规划。

根据本公开的至少一个实施方式的具有熟路模式的自动驾驶系统，如果本车当前定位数据存在大于预设数量的匹配轨迹组，所述轨迹组确认模块先不输出待确认轨迹组，所述定位匹配模块在后台进行轨迹组匹配，直至匹配的轨迹组的数量小于或等于所述预设数量。

根据本公开的至少一个实施方式的具有熟路模式的自动驾驶系统，所述轨迹组确认模块输出的待确认轨迹组包括轨迹组的权重信息。

根据本公开的至少一个实施方式的具有熟路模式的自动驾驶系统，还包括：

轨迹小组确认模块，所述轨迹小组确认模块输出被确认的轨迹组中的轨迹小组以被用户操作指令确认。

根据本公开的至少一个实施方式的具有熟路模式的自动驾驶系统，轨迹确认模块，所述轨迹确认模块输出被确认的轨迹小组中的轨迹以被用户操作指令确认。

根据本公开的又一个方面，提供了电子设备，包括：

存储器，所述存储器存储执行指令；

处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行本公开任一个实施方式的语义地图构建方法。

根据本公开的又一个方面，提供了可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现本公开任一个实施方式的语义地图构建方法。

根据本公开的再一个方面，提供了计算机程序产品，包括计算机程序/指令，所述计算机程序/指令被处理器执行时实现本公开任一个实施方式的语义地图构建方法。

附图说明

附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。

图1是本公开的一个实施方式的语义地图构建方法的流程示意图。

图2是本公开的又一个实施方式的语义地图构建方法的流程示意图。

图3是本公开的一个实施方式的轨迹分组示例图。

图4是本公开的一个实施方式的车辆行驶过程中生成的轨迹Trajectory_Semantic与符合要求的轨迹Trajectory_RTK。

图5是本公开的一个实施方式的planning具体决策示意图。

图6是本公开的一个实施方式的planning具体决策的验证情况示意图。

图7是本公开的一个实施方式的采用处理系统的硬件实现方式的语义地图构建装置的结构示意框图。

图8是本公开的一个实施方式的采用处理系统的硬件实现方式的具有熟路模式的自动驾驶系统的结构示意框图。

图9是本公开的一个实施方式的自动驾驶系统的存在多目的地时控制面板的显示情况示意图。

图10是本公开的一个实施方式的自动驾驶系统在目的地选定后控制面板显示轨迹的示意图。

附图标记说明

1000 语义地图构建装置

1002 行驶轨迹生成模块

1004 语义对象信息获取模块

1006 语义地图生成模块

1008 轨迹组获取模块

1010 语义地图处理模块

1012 验证更新模块

1100 总线

1200 处理器

1300 存储器

1400 其他电路

2000 自动驾驶系统

2002 定位匹配模块

2004 局部语义地图获取模块

2006 语义定位模块

2008 轨迹组确认模块

2010 自动驾驶规划模块

2300 存储器。

具体实施方式

下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。

需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。

除非另有说明，否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此，除非另有说明，否则在不脱离本公开的技术构思的情况下，各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。

在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如

此，除非说明，否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、5材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外，在附图中，为了清楚和/或描述性的目的，可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时，可以以不同于所描述的顺序来执行具体的工艺顺序。例如，可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。此外，同样的附图标记表示同样的部件。

0当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另

一部件时，该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件，或者可以存在中间部件。然而，当部件被称作“直接在”另一部件“上”、“直接连接到”或“直接结合到”另一部件时，不存在中间部件。为此，术语“连接”可以指物理连接、电气连接等，并且具有或不具有中间部件。

5本文使用的术语是为了描述具体实施例的目的，而不意图是限制性的。如这里所

使用的，除非上下文另外清楚地指出，否则单数形式“一个(种、者)”和“所述(该)”

也意图包括复数形式。此外，当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时，说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们

的组，但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件0和/或它们的组。还要注意的是，如这里使用的，术语“基本上”、“大约”和其它类

似的术语被用作近似术语而不用作程度术语，如此，它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。

在本公开中，可以基于车辆定位用RTK、IMU和轮速及相关状态数据，激光雷达、相机等车辆视觉感知传感器数据，完成单车单次对于熟路轻量级语义地图的构建；对5单次构建的语义地图基于时间、RTK与导航信息(若有)进行同起点终点的分组；对

于单车多次构建同轨迹组的语义地图进行粗对齐，粗对齐后的地图进行一次细粒度的地图优化，生成发布优化后的语义地图以完成熟路模式下语义地图构建；对于构建完成的地图，进行低频率高精度绝对定位与高频率高精度的相对定位融合的方式进行语

义定位的验证与更新，对于其中发现的新增轨迹进行再次的地图优化；对于验证成功0的熟路模式再次经过相关路段时通过系统主动告知的方式帮助用户使用熟路模式的自动驾驶。最终形成一套准确性高、适应范围广、用户无负担的基于熟路模式的自动驾驶系统。

下文结合图1至图10对本公开的语义地图构建方法、语义地图构建装置、自动驾驶系统等进行详细说明。

5图1是本公开的一个实施方式的语义地图构建方法的流程示意图。

参考图1，在本公开的一些实施方式中，本公开的语义地图构建方法S100，包括：

S110、获取本车当前行驶过程的多传感器数据，多传感器数据至少包括本车

定位数据、本车位姿数据及路况图像数据；

0S120、基于本车定位数据及本车位姿数据获取本车当前行驶过程的本车当前

行驶轨迹，基于路况图像数据获取本车当前行驶过程的语义对象信息；

S130、基于本车当前行驶轨迹及语义对象信息为本车当前行驶过程建立新语义地图；

S140、基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；

S150、对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图。

在本公开的一些实施方式中，用户驾驶过程中多传感器数据获取，包括：

(1)获取车辆当前行驶路径的导航信息(若有)，包含起点、终点。

(2)基于RTK/GNSS获取当前行驶过程中的车辆轨迹信息。该步骤是为了获取当前行驶过程中低频率高精度的绝对定位，定位更新频率可以为1hz。RTK获取的车辆位置坐标点可以位于车辆后轴中心，RTK坐标系为世界坐标系。

(3)获取当前行驶过程中的IMU+轮速信息。该步骤是为了获取当前行驶过程中高频率高精度的相对定位，定位更新频率为50hz。其中包括高精度IMU(6自由度、100hz)的获取与轮速里程计(50hz)的获取。

轮速为车辆左右后轮的轮齿脉冲数以及方向盘转角及挡位状态，可以通过Wheel(nl,nr,wheel_ang)来表征，IMU为三个方向的加速度以及三个方向的角速度，可以通过IMU(acc_x,acc_y,acc_z,v_ang_x,v_ang_y,v_ang_z)来表示。IMU的坐标系与车体坐标系一致，以车辆后轴中心为原点，车辆前进方向为正前方，x轴指向正前方，y轴指向正左方，z轴指向正上方。

本公开支持低频率高精度绝对定位与高频率高精度的相对定位融合的定位方式，融合方式如下：

在本公开的一些实施方式中，在两个RTK信号间的时间段ΔT，以IMU坐标系y轴方向为初始方向，基于左右轮编码器Wheel(nl,nr)的变化，计算ΔT时间内里程的变化Sr和Sl，二者取均值即可得到车辆ΔT时间内里程变化S＝(Sr+Sl)/2。假设某次RTK信号的获取时刻为T_S,RTK，下一个RTK信号获取的时刻为T_E,RTK。找到最接近且早于该段数据起始时间的第一个IMU数据，将其对应的时间作为起点T_S,IMU，将最接近且刚好晚于该段数据终止时间所对应的IMU数据时间作为终点T_E,IMU。并以RTK对应的时间戳作为初始时刻T_S,RTK，RTK采集的车辆位置作为当前世界坐标系原点。

基于轮速数据以及IMU数据确定车辆在T_S,RTK到T_E,RTK时间段内的位姿信息，具体计算方式如下：

考虑到IMU低速状态下的噪声影响较大，仅使用IMU角速度数据IMU(v_ang_x,v_ang_y,v_ang_z)在ΔT时间内进行积分得到三个方向的角度变化量IMU(Roll,Pitch,Yaw)。车辆在地面2D空间内运动，可确定车辆的位置及姿态变化为Vehicle(S*sin(Yaw),S*cos(Yaw),Yaw)。

车辆任意一个时刻t的位姿：

Vehicle_t＝Vehicle(S*sin(Yaw),S*cos(Yaw),Yaw)+Vehicle_t-1，即上一个时刻的位姿加上ΔT时间内的位姿变化。

具体地，轮速里程计在某个时间变化段ΔT，基于Wheel(nl,nr,wheel_ang)即左右轮编码器和方向盘转角变化，计算ΔT时间内的车辆位姿变化odometry(x,y,yaw)及其协方差odometry_cov；IMU在ΔT时间内，基于角速度数据积分得到三个方向的角度变化量IMU(Roll,Pitch,Yaw)及其协方差imu_cov；基于扩展卡尔曼滤波，采用松耦合方法融合轮式里程计和IMU的计算输出，得到变化的3D位置及3D姿态。

由于车辆RTK与IMU的坐标系原点均位于车辆后轴中心，因此使用RTK或IMU获取车辆世界坐标系下的位姿时不需进行RTK与IMU的坐标系间的对齐。

基于上述过程，可以得到车辆从T_S,RTK到T_E,RTK内任意时刻的经纬度和车辆位姿信息(精度、维度、高度)。

(4)借助车辆感知系统，如视觉传感器获取车辆当前行驶过程中的路况图像信息，并借助BEVDet、BEVDet4D、BEVFormer等进行语义对象检测与跟踪，检测、跟踪对象主要包括杆、牌、信号灯、地面的车道线、地面箭头标识、斑马线、停止线等。

参考图2，在本公开的一些实施方式中，本公开的语义地图构建方法S100，还包括：

S160、如果本车当前行驶过程与已存储在现有轨迹组的某个轨迹匹配，则基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对现有轨迹组的唯一语义地图进行验证和/或更新。

本公开中，唯一语义地图的验证与更新过程在某起点、终点的路况地图(语义地图)优化完成后开始，在累积三次(可调整)验证后结束，即每个轨迹组的唯一语义地图经过三次验证后结束。

在本公开的一些实施方式中，验证与更新的过程如下：

(1)若车辆当前行驶过程中，能够获取导航起点、终点数据，则借助导航数据匹配符合时间空间要求的已存储的轨迹组信息，若匹配成功，则对该轨迹组的唯一语义地图进行验证与更新。

若无法获取车辆当前行驶过程中导航的起点、终点数据，或能够获取但与已存储轨迹组均不匹配，则借助RTK数据进行匹配。

当车辆行驶到已存储的某轨迹组的RTK允许偏移范围内且时间范围符合要求时，验证开始，借助此后200m(可调整)的局部轨迹与同组中的不同轨迹进行聚合，当与现存某条轨迹聚合成功时，则认为当前路径属于现有轨迹，则对该轨迹组的唯一语义地图进行验证与更新。

若匹配不成功，则当前行驶结束后，根据起点终点情况，进行同轨迹组不同轨迹的添加或新轨迹组的建立。

本公开的语义地图构建方法中，本车定位数据可以基于RTK系统和/或GNSS系统获取；本车位姿数据可以基于IMU数据及轮速数据获取；基于车载视觉传感器获取本车当前行驶过程中的路况图像数据。

对于本公开的语义地图构建方法S100，优选地，S130、基于本车当前行驶轨迹及语义对象信息为本车当前行驶过程建立新语义地图，包括：

基于本车当前行驶轨迹及语义对象信息为本车当前行驶过程建立完整的新语义地图；

语义地图包括轨迹层及语义层；

轨迹层至少包括轨迹起点、轨迹终点及行驶路径；

语义层至少包括空中语义对象(例如空中交通标识)及地面语义对象(例如地面交通标识)。

在本公开的一些实施方式中，语义地图包括：

轨迹层(trajectory layer)：包含建图时车辆的行驶路径、轨迹起点、轨迹终点、轨迹权重，轨迹起点和轨迹终点可以是轨迹上的一点，也可以是轨迹上的一段轨迹。

语义层(semantic layer)：在语义层中基于车辆的多传感器数据进行空中交通标识(杆、牌、信号灯等)、地面交通标识(地面车道线、地面箭头标识、斑马线、停止线等)的界定。

为了提高建图的精准性，本公开在分析与建图过程中，无论当前行驶轨迹是否已经存在，均对该轨迹建立完整的路况地图即完整的新路况地图(新语义地图)，而非直接在已有路况地图上叠加轨迹。

在本公开的一些实施方式中，本公开的语义地图构建方法S100中，S140、基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，包括：

其中，轨迹编码包括轨迹起点编号、轨迹终点编号及行驶路径编号。

本公开的语义地图构建方法中，对于新完成建图的行驶轨迹进行分组，将同起点、同终点的行驶轨迹归属为同一组，同组内存储同起点、同终点但不同行驶路径(包括大路径不同与小分支不同)的轨迹信息，不同轨迹可以8位数字的编号表示，如“00100111”，该编号1-3位代表起点，对于本车获取到的第1个起点信息，其编号为001，之后对于不同的地点(起点)以1为增量递增编号；4-6位代表终点，编号规则与1-3位相同；第7、8位均代表路径，对于同起点同终点的行驶轨迹，可能存在选择几乎完全不同的道路行驶的情况(即行驶路径完全不同)，也可能存在仅在短暂的小分支上选择不同的情况。

图3是本公开的一个实施方式的轨迹分组示例图。

本领域技术人员在本公开技术方案的启示下，也可以对编号规则进行调整，均落入本公开的保护范围。

在本公开的一些实施方式中，本公开的语义地图构建方法S100中，基于轨迹重合程度，将同一个轨迹组内的行驶轨迹划分为不同的轨迹小组。

示例性地，本公开可以进行以下设定，若两条轨迹之间在允许偏移范围内(例如15m)完全重合，则认为两条轨迹为同一条轨迹，即两条轨迹的8位编号完全相同。

若两条轨迹之间仅有小于三分之一的部分不重合，则认为两条轨迹同属于一条“大路径”，仅“小分支”不同，即两条轨迹的前7位编号相同，仅第八位编号不同。

若两条轨迹之间有超过三分之一的部分不重合，则认为两条轨迹选择了不同的行驶路径从同起点到达同终点，即两条轨迹仅前6位编号相同，第七、八位编号均不同。

在本公开的一些实施方式中，对于前6位编号相同的所有轨迹，称为同一轨迹组；对于前7位编号相同第8位不同的所有轨迹，称为同一轨迹组内的不同小组，即轨迹小组。

例如，对于已知的某起点某终点，本车获取的第一条行驶轨迹的第7-8位编号为11。之后对于同起点同终点的大路径选择不同的情况(即当前轨迹与已存所有轨迹均有超过三分之一的部分不重合)，以10为增量递增编号，即21、31…；对于小分支选择不同的情况(即当前轨迹与已存某条轨迹间仅有小于三分之一的部分不重合)，以该条轨迹所在轨迹小组的最大编号为基础，以01为增量递增编号，如，12、13…。

对于上述各个实施方式的语义地图构建方法S100，优选地，获得更新的轨迹组，包括：对该轨迹组的各个轨迹小组的权重进行更新，对该轨迹组的每个轨迹小组的各个行驶轨迹的权重进行更新。

在本公开的一些实施方式中，对于同一个轨迹组内的行驶轨迹，起点终点相同，即同一个轨迹组内的各个轨迹的编号前六位相同。对于同一个轨迹组的编号，即前六位编号相同的编号，内置了权重ω_n，n代表第n个轨迹小组，n即上文描述的第7位编号，权重自带更新时间戳，更新时间戳为最近一次更新权重的时间，权重下标与其对应的第7位编号相同，其具体数值为

所有前六位编号相同、第7位不同编号的权重之和为1(仅看前7位)，即同一个轨迹组内所有轨迹小组的权重之和为1。

对于同组轨迹中的不同轨迹，即前7位相同第8位不同的编号，内置了权重ω_n,m，n代表轨迹组即第7位编号，m代表轨迹组n下的不同轨迹，权重自带更新时间戳，更新时间戳为最近一次更新权重的时间，权重下标n与其对应的第7位编号相同，m与其对应的第8位编号相同，其具体数值为所有前7位编号相同的第8位不同编号的权重之和为1。

在本公开的一些实施方式中，优选地，各个轨迹小组的权重以及每个轨迹小组的各个行驶轨迹的权重均具有更新时间戳，以基于更新时间戳对行驶轨迹进行更新。

在本公开的一些实施方式中，特别地，若同起点、同终点的不同组或小组内轨迹数量达到9，当第十条出现时，采用“最近最少用”的原则进行更新，使第十条轨迹信息替换已存储的更新时间戳最早的轨迹信息。

对于上述各个实施方式的语义地图构建方法S100，优选地，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，包括：

对于上述各个实施方式的语义地图构建方法S100，优选地，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，还包括：

在本公开的一些实施方式中，对于是否为同一个轨迹组的确认(即起点终点是否相同)借助时间与空间相结合的方式，即当两个起点\终点的RTK采集时间同时满足要求时，才认为这两个点是同一个点，判定方式如下：

(1)空间判定：当两个点的RTK位置间的距离≤150m(可调整)时，认为空间判定通过。特别地，对于同一个轨迹组而言，将当前点与轨迹组内所有点进

行判定较为繁琐且意义不大，因此，本公开在每个轨迹组形成时，以第一条被存5储的轨迹起点终点相关信息为该轨迹组的起点终点信息，在进行当前轨迹的空间

判定时，当前轨迹的起点终点信息仅与轨迹组的起点终点信息进行判定，不与组内的每条轨迹的起点终点信息进行判定。

(2)空间与时间判定结合：对于通过空间判定的RTK点，结合时间进一步

进行判定。本公开优选地将时间分为工作日时间(周一到周五)与非工作日时间0(周末时间、节假日时间)。

对于工作日时间内，出现的第一条轨迹，将其起点、终点对应的时间戳信息作为该轨迹形成的轨迹组的起点终点相关信息(即起点终点时间信息)保存；对于之后出现的通过某轨迹组空间判断的RTK起点、终点，若该RTK起/终点与轨

迹组保存的起/终点的时间戳偏差在2h(可调整)内，则认为通过时间判定，该5RTK起/终与相应的轨迹组起/终点为同一起/终点；特别地，对于工作日时间的中

午(例如11：00-15：00)时间范围内，若存在通过空间判定的点，可忽略偏差2h内的时间判定要求，直接认为该RTK起/终与相应的轨迹组起/终点为同一起/

终点；其他情况下通过空间判定而未通过时间判定的点，将其作为新的点进行新

增编号(即新增轨迹组)，在后续自动驾驶的路径选择中，若有多个范围相近终0点的终点满足要求，则将当时时间作为第一优先级进行路径选择。

为了避免终点并非同一个但两终点间的距离＜150m(可调整)的情况给分析过程带来的额外负担，在本公开的一些实施方式中，对于周末时间的常规性日程，即使空间判定为同一起点终点，仍当作新点进行新增编号，在后续自动驾驶的路

径选择中，若有多个范围相近终点的终点满足要求，则将当时时间作为第一优先5级进行路径选择。

周末时间的判定方式可以与工作日时间的判定方式相同，为了避免终点并非同一个但两终点间的距离＜150m的情况给分析过程带来的额外负担，周末时间与工作日时间作为两套时间分开判定。

本公开中，优选地，行驶轨迹的起点终点判定完成后的具体分组可以采用以0下方式：

(1)若当前行驶轨迹存在导航起点、终点，且系统中存在已存储的同时间范围内同起点终点的轨迹组信息，则将当前行驶轨迹判定至对应的轨迹组中。

实际应用过程中可能会出现当前行驶轨迹的导航起点终点与系统中已存储

的某轨迹组同起点终点，但非同时间范围的情况(例如，一个在工作日，一个在5周末)，尽管由于导航信息的存在，可以认定当前行驶轨迹与该轨迹组必定的同

起点终点，不存在终点相近但不同的情况，本公开为了降低处理程序的复杂度，不做特殊情况处理，仍沿用基于时间范围的时间空间判定相结合的判定方式，不将当前轨迹判定为上述轨迹组中的轨迹。

(2)若当前行驶轨迹存在导航起点、终点，而系统中不存在已存储的同时0间范围内同起点终点的轨迹组信息，则借助当前行驶轨迹起点与终点的RTK位置

数据与系统中已存储的所有轨迹组起点终点的RTK位置数据进行比较，若当前行驶轨迹的起点、终点与某一轨迹组的起点、终点的RTK位置距离差均在150m(可调整)范围内，则将当前行驶轨迹判定至对应轨迹组中，并添加轨迹组的起点终点信息。

若查找不到距离差允许或者同时间空间范围内的轨迹组，即不存在当前行驶轨迹的同组轨迹，则按照上文描述的编号编码规则递增起点、终点、路径编号，组合成一个新的编号，并添加轨迹组的起点、终点信息。

(3)若当前行驶轨迹不存在导航起点、终点，则借助当前行驶轨迹起点与终点的RTK位置数据与系统中已存储的同时间范围内所有轨迹组起点终点的RTK位置数据进行比较，若当前行驶轨迹的起点、终点与某一轨迹组的起点、终点的时间空间范围均在允许偏差范围内，则将当前行驶轨迹判定至对应轨迹组中。

若查找不到时间空间允许范围内的轨迹组，则按照编号编码规则组合一个新的编号。

对于本公开的语义地图构建方法S100，优选地，S140、基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，还包括：

在本公开的一些实施方式中，基于当前行驶轨迹的新语义地图的轨迹层进行聚类。基于轨迹层进行聚类，优选地包括：借助当前行驶轨迹的RTK位置(当经过高架桥等信号不稳路段时可以借助IMU+轮速数据进行补充)与同组中已存储的轨迹RTK信息进行聚类，若除起点、终点的150m允许偏差范围外，当前行驶轨迹与同组已存在的某轨迹聚类成功(两轨迹随时间的变化方向同向，且两轨迹间的聚类允许偏差在15m以内则认为聚类成功，聚类标准可调整)，则认为当前轨迹已存在，不进行新增编号，增加经过某轨迹的次数，更新同轨迹组的轨迹权重；否则认为当前轨迹未存储，新增编号并更新存储相关信息，并更新同轨迹组的轨迹权重。

在本公开的一些实施方式中，当某轨迹组内轨迹的累积行驶次数达到7次后(可更改)，认为完成该轨迹组的轨迹聚类过程，允许进行后续的地图融合过程。

对于本公开的语义地图构建方法S100，优选地，S150、对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图，包括：

基于紧耦合的方式获得同轨迹组的唯一语义地图。

其中，该唯一语义地图存在同起点同终点的多条不同行驶轨迹。即本公开结合聚类结果对同轨迹组的多个路况地图(语义地图)进行了图的融合优化：

优化时对多个完整的路况地图(语义地图)进行优化，包括轨迹层与语义层，语义层进行融合，轨迹层进行叠加。

在本公开的一些实施方式中，可以首先基于RTK/GNSS和空中的对象(例如，路标指示牌、红绿灯等)进行图与图间的粗粒度对齐。对齐方法可以包括基于RTK/GNSS轨迹点的地图对齐方法、基于空间物体的方法进行数据关联。每种方法都能给出地图间偏移量及其不确定度。选取不确定度低的偏移量作为其对齐结果，从而将多个第一地图调整至同一个坐标系下，完成图与图间的粗粒度对齐。

之后，基于地面标识(例如，人行道、转向指示等)进行细粒度的地图优化：对线性物体和其他不便于使用上述数据关联方法进行数据关联的物体进行数据关联，如对于车道线，可以利用已确定关联关系的物体，确定的次序确定关联关系。具体的方法可以为：根据两个地图中已经确定关联关系的路旁标志牌，计算该标志牌到已检测出的车道线的垂线的方向和长度，确定车道线的次序，从而进行地面标识间关联关系的确认，即完成细粒度的地图优化。

基于图优化，本公开优选地采用紧耦合的方式得到优化后的该组的唯一语义地图，该唯一语义地图存在同起点同终点的多条不同轨迹(含权重)。

在本公开的一些实施方式中，对于同起点终点的地图融合完成后的手动驾驶新增轨迹的情况，在唯一地图的基础上，将该次新建立的路况地图(语义地图)与唯一地图进行统计与分析，生成新的唯一地图。

对于本公开的语义地图构建方法S100，优选地，S160中，基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对现有轨迹组的唯一语义地图进行验证和/或更新，包括：

语义对象的验证与更新；

轨迹定位精准性的验证与更新；

轨迹预测精准性的验证与更新。

在本公开的一些实施方式中，上文描述的语义对象的验证与更新，包括：

若当前行驶轨迹上检测到了语义对象，且在唯一语义地图中能够找到对应坐标的语义对象，则语义对象验证成功；

在本公开的一些实施方式中，基于车载相机传感器、激光雷达等视觉传感器进行语义对象检测，检测对象包括杆、牌、信号灯、地面的车道线、地面箭头标识、斑马线、停止线等空中检测对象和地面检测对象，基于语义对象的检测结果、GNSS/RTK给出的本车初始位姿从语义地图中得到局部语义地图。

对于检测结果出现问题的情况：

若出现同物体间坐标出现偏差，即对齐问题，则借助上文描述方法进行粗粒度与细粒度的对齐。

若出现同坐标出现了不同物体的情况，假设原物体为A，新观测的同坐标物体为B，对匹配不上的物体A进行次数的累积，同时保留同坐标的新观测的物体B的观测数据，当连续三次匹配物体A失败时，则认为之前存储的A发生了变化，将A进行替换为新观测到的B(替换时的物体B至少存在连续三次的观测数据，需借助上文描述的方法进行粗粒度和细粒度的对齐之后再进行替换)。特别地，在本公开的一些实施方式中，物体A或B允许为空，如指示牌增加/拆除等情况。

在语义对象的验证过程中，若当前轨迹上检测到了语义对象，且在语义地图中能够找到对应坐标的语义对象，则认为语义对象部分验证成功；若出现了验证不成功的语义对象，则按上述方式进行语义地图的更新，语义对象验证的失败与否不直接影响整个语义地图的验证结果，该步骤仅作为不断更新语义地图与对应局部语义地图用，语义对象作为影响轨迹定位与预测的重要因素，在接下来的检测步骤中间接的影响整个语义地图的验证结果。

在本公开的一些实施方式中，上文描述的轨迹定位精准性的验证与更新，包括：

基于唯一语义地图为本车当前行驶过程生成融合语义对象信息及本车位姿数据的轨迹Trajectory_Semantic；

基于语义对象的检测结果与当前行驶轨迹中的GNSS/RTK给出的初始位姿，从语义地图中得到对应的局部语义地图；基于图匹配的方法进行语义定位，融合IMU+轮速进行位姿的轨迹生成，生成的轨迹为Trajectory_Semantic；同时获取当前行驶过程中RTK和IMU融合后的轨迹Trajectory_RTK，但由于行驶过程中RTK会受如高大建筑物遮挡等的影响，导致RTK置信度变低，因此对于获取的Trajectory_RTK轨迹，仅使用RTK置信度高的部分及RTK置信度由高变低后短距离内(100m)的DR推演部分。因为DR推演在短距离内非常准确，但会累计误差，一段距离后会变的并不准确，因此若RTK置信度由高变低，短距离内的一段轨迹仍可用来验证。

对于验证与更新过程中的每次获取了Trajectory_Semantic和Trajectory_RTK的驾驶轨迹，计算每条轨迹获取的RTK置信度比较高部分或者RTK置信度变低后一段距离内DR推演部分的Trajectory_Semantic和Trajectory_RTK的RPE(相对位姿误差)或者ATE(绝对轨迹误差)，若平均精度达到设定的阈值，就认为该组建立的语义地图的定位部分有效，验证成功。

若平均精度没能达到设定的阈值，则认为该段地图定位不够准确，若当前段语义地图中的关联对象较少，则优先考虑场景变化导致的精度较低，将该次验证过程中的信息仍然作为当前行驶路径的又一次行驶信息，补充入该段地图中；若当前段语义地图中的关联对象较多，则不认为是某处场景变化导致的精度较低，考虑是建图出现问题，需要重新构建该段地图。对于需要重新构建地图的情况，为了避免验证时出现问题而非建图问题，第一次判定需要重建时保留验证所获的相关信息但暂不进行重建，当连续两次验证判定为需要重建时，则删除该段路径原有的建图信息，借助两次验证所获信息分别进行该段路径的地图重建(重建地图的过程严格按照上述新建地图的步骤，仍然需要完成7次建图累积后，进行优化融合等过程后，再验证)。

特别地，该重建属于小段路径重建，仅对于精度不够准确的某段路径进行重新，参考图4(图4是本公开的一个实施方式的车辆行驶过程中生成的轨迹Trajectory_Semantic与符合要求的轨迹Trajectory_RTK)，每段红黑线重合的线段为一段。当存在某段路径验证失败时，认为整条路径存在问题，不增加累积成功次数，也不继续进行后续planning的验证。

在本公开的一些实施方式中，上文描述的轨迹预测精准性的验证与更新，包括：

至少基于本车当前行驶过程的轨迹Trajectory_Semantic与本车自动规划的规划轨迹的重合度判断自动规划是否成功；

在本公开的一些实施方式中，本车自动规划的规划轨迹包括规划避障轨迹。

由于本公开的语义地图的构建的目的是为了自动驾驶，因此除了轨迹定位的精准性之外，车辆在驾驶过程中能否自行进行正确有效的判断也非常重要。因此除了上文描述的Semantic部分的验证外，还需要planning部分(自动驾驶规划模块)的验证。

当前使用的planning(自动驾驶规划模块)主要分为三个层次：决策模块分为全局路径规划层(Route Planning)、行为决策层(Behavioral Layer)、运动规划层(MotionPlanning)三个层次。特别地，由于处于验证过程中，全程仍由用户手动驾驶，因此planning仅作预测决策并不向自动驾驶模块传输信息从而对实际驾驶产生影响。

在验证过程中，由于从同起点出发到达同终点可能有不同的已存储的行驶轨迹，因此允许Route Planning开始时保留基于当前行驶路径的所有符合要求的行驶轨迹，即允许Route Planning结果在行驶过程中不唯一，随着行驶过程不断剪枝，最后仅验证成功最多一条轨迹。在行驶过程中，仅当车辆的行驶轨迹与基于当前路径的所有符合要求的行驶轨迹都存在允许范围外的偏差时，Route Planning(全局路径规划层)才会失败。

Behavioral Layer基于行驶路径与当前路况(包括其他车辆与行人，障碍物，以及道路上的交通规则信息)做出未来max(10，车速×时间)米的具体行为决策(例如，前方有正在减速刹车车辆时选择例如选择变道超车还是跟随刹车)，并随着周围情况而不断动态变化。其中，时间默认为3s，可在2-5s内调整；动态变化的频率默认为1s一次，可在每秒1-10次内调整。

图5是本公开的一个实施方式的planning具体决策示意图。图6是本公开的一个实施方式的planning具体决策的验证情况示意图。

当行驶过程中无障碍物、交通信息等情况影响时，计算用户手动驾驶车辆的实际行驶路径与Behavioral Layer规划路径的偏差，偏差计算借助局部语义地图中的语义对象与RTK信息，允许偏差范围为1m。

对于存在障碍物、交通信息等情况影响的planning部分：

(1)当车辆的具体决策与当前用户手动驾驶时的决策相同时(如，均是从右侧超车)，允许偏差范围阈值设为1.5m(可在1.5-2m范围内调整)，若车辆的决策与用户手动驾驶实际路径的偏差在1.5m内，且未违反交通规则，则认为避障成功，累积避障评判成功次数。

(2)当车辆的具体决策与当前用户手动驾驶时的决策不同时，进行planing决策是否有效的验证：

首先判断是否有碰撞风险或是否违反交通规则，若有碰撞风险或会违反交通规则，则验证失败，认为存在安全隐患，该次验证作废(失败)；若无有碰撞风险且不违反交通规则，则跳过对避障路段的验证，且认为避障路程的规划轨迹与实际驾驶轨迹的偏差均在允许范围内，不进行决策正确与否的评判。未避免决策不同次数过多导致实际避障评判次数太少，因此设置避障成功次数设下限为5次，当此条路径累次验证成功次数达到允许开启熟路模式的次数时，若避障评判成功次数小于5次，则并不开启熟路模式功能，直到避障评判成功次数达到5次或以上。

当整条轨迹验证完成时，计算Behavioral Layer规划路径与用户实际驾驶路径的重合度，若重合度＞95％(可调)，且全程没有违反交通规则，则认为planning验证成功；否则验证失败。每次验证失败时，清除对应轨迹的验证成功次数，即将对应轨迹的验证成功次数置为0，若累积验证失败三次，则认为对应轨迹不适合自动驾驶，清除对应轨迹存储的语义地图但不封禁对应地图的自动驾驶功能，可由下次经过时重新开始存储建图与验证。

在连续三次验证成功且避障评判成功次数达到要求后，则认为建立的地图有效，对应轨迹的语义地图验证完成，允许启用熟路模式，对于无导航终点信息的熟路模式已开启的轨迹，通过联网自动获取终点RTK范围内被大众搜索最多次的地标信息作为终点的导航信息。特别地，只启用完成验证部分的语义地图，未完成验证的部分等待用户手动驾驶完成验证后才能开启。另外，每次验证成功或新增轨迹时，同样的会增加对应轨迹的权重。

本公开还提供了语义地图构建装置1000，包括：

行驶轨迹生成模块1002，行驶轨迹生成模块1002基于本车定位数据及本车位姿数据获取本车当前行驶过程的本车当前行驶轨迹；

语义对象信息获取模块1004，语义对象信息获取模块1004基于获取的路况图像数据获取本车当前行驶过程的语义对象信息；

语义地图生成模块1006，语义地图生成模块1006基于本车当前行驶轨迹及语义对象信息为本车当前行驶过程建立新语义地图；

轨迹组获取模块1008，轨迹组获取模块1008基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；

语义地图处理模块1010，语义地图处理模块1010对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图。

在本公开的一些实施方式中，本公开的语义地图构建装置1000，还包括：

验证更新模块1012，如果本车当前行驶过程与已存储在现有轨迹组的某个轨迹匹配，则验证更新模块1012基于本车当前行驶过程的本车定位数据、本车位姿数据及路况图像数据对现有轨迹组的唯一语义地图进行验证和/或更新。

其中，本公开的语义地图构建装置1000可以基于软件程序架构的方式实现。

该语义地图构建装置1000还可以包括执行上述流程图中各个或几个步骤的相应模块。因此，可以由相应模块执行上述流程图中的每个步骤或几个步骤，并且该装置可以包括这些模块中的一个或多个模块。模块可以是专门被配置为执行相应步骤的一个或多个硬件模块、或者由被配置为执行相应步骤的处理器来实现、或者存储在计算机可读介质内用于由处理器来实现、或者通过某种组合来实现。

该硬件结构可以利用总线架构来实现。总线架构可以包括任何数量的互连总线和桥接器，这取决于硬件的特定应用和总体设计约束。总线1100将包括一个或多个处理器1200、存储器1300和/或硬件模块的各种电路连接到一起。总线1100还可以将诸如外围设备、电压调节器、功率管理电路、外部天线等的各种其他电路1400连接。

总线1100可以是工业标准体系结构(ISA，Industry Standard Architecture)总线、外部设备互连(PCI，Peripheral Component)总线或扩展工业标准体系结构(EISA，Extended Industry Standard Component)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，该图中仅用一条连接线表示，但并不表示仅有一根总线或一种类型的总线。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本公开的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如，本公开中的方法实施方式可以被实现为软件程序，其被有形地包含于机器可读介质，例如存储器。在一些实施方式中，软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时，可以执行上文描述的方法中的一个或多个步骤。备选地，在其他实施方式中，处理器可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述方法之一。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，可以具体实现在任何可读存储介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。

本公开还提供了具有熟路模式的自动驾驶系统2000，包括：

存储模块(例如存储器2300)，存储模块存储有基于本公开任一个实施方式的语义地图构建装置或本公开任一个实施方式的语义地图构建方法获取的轨迹组及各个轨迹组的唯一语义地图；

定位匹配模块2002，定位匹配模块2002对本车当前定位数据(GNSS/RTK数据)与存储模块存储的轨迹组起点定位数据进行匹配；

局部语义地图获取模块2004，局部语义地图获取模块2004基于定位匹配模块2002的定位匹配结果从匹配的轨迹组的唯一语义地图中得到局部语义地图；

语义定位模块2006，语义定位模块2006基于当前路况图像数据中的语义对象信息及局部语义地图中的语义对象信息在局部语义地图中进行轨迹起点语义定位；

轨迹组确认模块2008，轨迹组确认模块2008基于语义定位模块2006的轨迹起点语义定位结果输出至少一个待确认轨迹组；

自动驾驶规划模块2010，自动驾驶规划模块2010基于被用户操作指令确认的轨迹组进行熟路模式下的自动驾驶规划。

本公开的具有熟路模式的自动驾驶系统2000可以基于软件程序架构的方式实现。

参考图8，本公开的自动驾驶系统2000包括可以包括本公开任一个实施方式的语义地图构建装置1000。

在本公开的一些实施方式的具有熟路模式的自动驾驶系统2000中，如果本车当前定位数据(例如RTK数据)存在大于预设数量(例如3)的匹配轨迹组，轨迹组确认模块2008先不输出待确认轨迹组，定位匹配模块2002在后台进行轨迹组匹配，直至匹配的轨迹组的数量小于或等于预设数量。

在本公开的一些实施方式的具有熟路模式的自动驾驶系统2000中，轨迹组确认模块2008输出的待确认轨迹组包括轨迹组的权重信息。

本公开构建的语义地图能够具有熟路模式的自动驾驶系统，熟路模式可被用户确认与使用。

在本公开的一些实施方式中，当用户驾驶车辆到达验证成功轨迹组的RTK起点允许偏移范围内时，熟路模式启动。

借助车辆当前GNSS/RTK位置与系统中已存储的轨迹组起点RTK位置与当前时间进行匹配；基于感知进行语义对象检测，检测对象主要包括杆、牌、信号灯、地面的车道线、地面箭头标识、斑马线、停止线等。基于语义对象的检测结果、GNSS/RTK给出的初始位姿从语义地图中得到局部语义地图；基于图匹配的方法进行语义定位，由于允许偏移的情况，轨迹起点是轨迹上一点或一段轨迹，因此当且仅当车辆行驶通过起点路段，进入参与聚合的轨迹路段时，才有可能语义定位成功，定位时系统先进行轨迹组的确认(仅空间范围的确认)，若当前RTK范围内存在多个轨迹大组(即以当前RTK为起点存在多个终点)时，为便于用户选择，若存在三个以上的终点，则先不主动询问用户，先进行当前行驶路径与多轨迹组的匹配(借助语义地图与RTK)，等待用户手动驾驶至可匹配的轨迹大组仅存三组或以下时，系统以时间范围为主结合权重对所剩目的地进行排序，按排列序号从第一位开始，主动询问用户“是否开启终点为xxx的自动驾驶模式”。当用户仍想手动驾驶时，可在控制面板上点击“退出”，则退出熟路驾驶模式；用户若想开启自动驾驶但需要去其他终点时，可以在控制面板上点击“切换”，则系统询问下一个编号对应的终点地址。若用户点击“切换”时无下一个编号，则重新询问排名第一位编号对应的终点地址，直至用户确认或退出。若用户退出，则当前轨迹作为手动驾驶的轨迹进行轨迹组信息的更新补充。

重定位(轨迹组确认)成功后，若基于当前路径同轨迹组中存在多条匹配成功的轨迹(因用户可能自动向前行驶了一段距离，所以达到某目的地的所有路径中，可能有些已经匹配不成功)，则选择其中权重最高的轨迹作为目标轨迹(先比较大路径权重，即第7位轨迹编号不同对应的权重；选择大路径权重最高的轨迹小组，比较小组内权重最高的小分支权重。即，取权重最高的轨迹小组内权重最高的小分支作为默认目标轨迹)，并显示终点目标和轨迹线。轨迹线包括权重最高的轨迹实线与其余匹配成功的轨迹虚线，如图9所示(图9是本公开的一个实施方式的自动驾驶系统的存在多目的地时控制面板的显示情况示意图)。同时，由于堵车等临时性道路问题的存在，支持用户通过控制面板自主从匹配成功的轨迹中选择自动驾驶的路径，选择时可通过一次点击切换至同轨迹小组内的不同分支或不同轨迹小组内的权重最高的默认分支(若有)，也可通过两次点击切换至不同轨迹小组内的非默认分支(若有)。

在本公开的一些实施方式中，本公开的具有熟路模式的自动驾驶系统2000，还包括：

轨迹小组确认模块，轨迹小组确认模块输出被确认的轨迹组中的轨迹小组以被用户操作指令确认。

轨迹确认模块，轨迹确认模块输出被确认的轨迹小组中的轨迹以被用户操作指令确认。

在本公开的基于熟路模式的自动驾驶过程中，上述基于轨迹组和权重匹配或用户自己选定的轨迹作为车辆驾驶的目标轨迹，图10是本公开的一个实施方式的自动驾驶系统在目的地选定后控制面板显示轨迹的示意图，自动驾驶系统基于语义定位和IMU的融合以及RTK的融合，提供实时的定位信息；同时基于感知模块(多传感器)得到车辆周围的障碍物的类型、尺寸、位置和速度相关信息，进行障碍物的轨迹预测；将障碍物信息、障碍物和自车的轨迹预测结果、对应的局部语义地图给局部决策和规划模块进行轨迹规划；最后基于规划的目标轨迹，车辆进行基于熟路模式的自动驾驶。

在自动驾驶的过程中，若用户想手动驾驶，可在准备好接管方向盘后通过手动点击控制面板的“退出自动驾驶”等待5s(可调)自行手动驾驶；或当遇紧急情况时通过转动方向盘或踩刹车退出自动驾驶。

当车辆自动驾驶至接近轨迹组终点100m(可调)时，自动驾驶系统主动告知用户“前方100m即将到达自动驾驶终点，请用户做好准备”，以便于用户进行自动驾驶结束后的处理。

基于本公开的上述描述，可见，本公开提供了高准确性、低负担性的当前可用的基于熟路模式的自动驾驶方法，尤其适用于城区自动驾驶。

针对目前熟路模式多需要借助导航信息与用户手动输入的问题，本公开提出了以时间空间判定相结合的方式，借助一定的时间与RTK允许偏移范围来确定是否同起点终点，并以导航信息为辅的轨迹起点终点确定方法，在降低用户负担的同时提高了适用性。

针对当前熟路模式中常用的众包技术无法在短时间内投产的问题，本公开提出了低频率高精度绝对定位与高频率高精度的相对定位相融合的基于本车轨迹的熟路模式，在保证精确度的同时能够支持尽快投产。

本公开的提供的语义地图构建方法及具有熟路模式的自动驾驶系统，无需高精地图，可仅借助导航地图(若有)完成整个自车熟路模式的构建；支持以时间范围、RTK范围为主、导航信息为辅的起点终点确认与同起点终点的分组，无需过度依赖导航地图或依赖用户手动输入，且运用了时间空间相结合的判定方法，减少了建图、验证过程中的干扰信息，提高了实际运用过程中轨迹组确认的效率；通过基于RTK的单车语义建图，借助单车多次地图的粗对齐、新增轨迹情况下的多次地图的优化以及优化后的图生成发布，保证了单车语义建图的准确性，无需使用多车众包建图来保证准确性；验证过程中支持低频率高精度绝对定位与高频率高精度的相对定位相融合的方式进行语义地图的验证与更新，进一步保障了建图的准确性；使用Semantic(语义定位)与planning(预测规划)相结合的验证方式，既保证了轨迹的精确性，又保证了实际自动驾驶过程中的安全性，同时，每次验证失败重新累积成功次数，三次验证失败则清除轨迹相应信息的验证方式，在允许车辆学习的同时保证了安全性；熟路模式训练及使用全程，用户全程无训练感，保证了良好的用户体验，同时还保留了权重较低的熟路轨迹，支持用户自主选择自动驾驶轨迹。

本公开还提供了电子设备，包括：存储器，存储器存储执行指令；以及处理器，处理器执行存储器存储的执行指令，使得处理器执行本公开任一个实施方式的语义地图构建方法。

本公开还提供了可读存储介质，可读存储介质中存储有执行指令，执行指令被处理器执行时用于实现本公开任一个实施方式的语义地图构建方法。

本公开还提供了计算机程序产品，包括计算机程序/指令，计算机程序/指令5被处理器执行时实现本公开任一个实施方式的语义地图构建方法。

就本说明书而言，“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个

布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，0只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式只读存储器(CDROM)。另外，可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在存储器中。

5应当理解，本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施

方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门

电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列0(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成，程序可以存储于一种可读存储介质中，该程序在执行时，包括方法实施方式的步骤之一或其组合。

此外，在本公开各个实施方式中的各功能单元可以集成在一个处理模块中，也可5以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集

成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个可读存储介质中。存储介质可以是只读存储器，磁盘或光盘等。

Claims

1.一种语义地图构建方法，其特征在于，包括：

基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；以及

对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图；

其中，基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，包括：

基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组；所述轨迹编码包括轨迹起点编号、轨迹终点编号及行驶路径编号；

其中，基于轨迹重合程度，将同一个轨迹组内的行驶轨迹划分为不同的轨迹小组；

其中，获得更新的轨迹组，包括：对该轨迹组的各个轨迹小组的权重进行更新，对该轨迹组的每个轨迹小组的各个行驶轨迹的权重进行更新；

其中，各个轨迹小组的权重以及每个轨迹小组的各个行驶轨迹的权重均具有更新时间戳，以基于更新时间戳对行驶轨迹进行更新；

其中，基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组，还包括：

2.根据权利要求1所述的语义地图构建方法，其特征在于，还包括：

3.根据权利要求1或2所述的语义地图构建方法，其特征在于，本车定位数据基于RTK系统和/或GNSS系统获取；本车位姿数据基于IMU数据及轮速数据获取。

4.根据权利要求1或2所述的语义地图构建方法，其特征在于，基于车载视觉传感器获取本车当前行驶过程中的路况图像数据。

5.根据权利要求1或2所述的语义地图构建方法，其特征在于，基于所述本车当前行驶轨迹及所述语义对象信息为本车当前行驶过程建立新语义地图，包括：

所述语义地图包括轨迹层及语义层；

所述轨迹层至少包括轨迹起点、轨迹终点及行驶路径；

所述语义层至少包括空中语义对象及地面语义对象。

6.根据权利要求1所述的语义地图构建方法，其特征在于，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，包括：

7.根据权利要求6所述的语义地图构建方法，其特征在于，基于轨迹起点、轨迹终点及行驶路径，对本车当前行驶轨迹赋予轨迹编码并划分至现有轨迹组，或新建一个轨迹组，还包括：

8.一种语义地图构建装置，其特征在于，包括：

轨迹组获取模块，所述轨迹组获取模块基于轨迹起点、轨迹终点及行驶路径对建立新语义地图的本车当前行驶轨迹进行聚类分组，获得更新的轨迹组；以及

语义地图处理模块，所述语义地图处理模块对于同轨迹组的各个语义地图进行轨迹层叠加及语义层融合，获得同轨迹组的唯一语义地图；

9.根据权利要求8所述的语义地图构建装置，其特征在于，还包括：

10.一种具有熟路模式的自动驾驶系统，其特征在于，包括：

存储模块，所述存储模块存储有基于权利要求8或9所述的语义地图构建装置或权利要求1至7中任一项所述的语义地图构建方法获取的轨迹组及各个轨迹组的唯一语义地图；

轨迹组确认模块，所述轨迹组确认模块基于所述语义定位模块的轨迹起点语义定位结果输出至少一个待确认轨迹组；以及

11.根据权利要求10所述的具有熟路模式的自动驾驶系统，其特征在于，如果本车当前定位数据存在大于预设数量的匹配轨迹组，所述轨迹组确认模块先不输出待确认轨迹组，所述定位匹配模块在后台进行轨迹组匹配，直至匹配的轨迹组的数量小于或等于所述预设数量。

12.根据权利要求10所述的具有熟路模式的自动驾驶系统，其特征在于，所述轨迹组确认模块输出的待确认轨迹组包括轨迹组的权重信息。

13.根据权利要求10所述的具有熟路模式的自动驾驶系统，其特征在于，还包括：

14.根据权利要求10所述的具有熟路模式的自动驾驶系统，其特征在于，轨迹确认模块，所述轨迹确认模块输出被确认的轨迹小组中的轨迹以被用户操作指令确认。

15.一种电子设备，其特征在于，包括：

存储器，所述存储器存储执行指令；以及

处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行权利要求1至7中任一项所述的语义地图构建方法。

16.一种可读存储介质，其特征在于，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现权利要求1至7中任一项所述的语义地图构建方法。