车辆定位初始化方法、装置、设备及存储介质与流程

1.本发明涉及自动驾驶技术领域，尤其涉及一种车辆定位初始化方法、装置、设备及存储介质。


背景技术：

2.对于高阶自动驾驶系统来说，高精度的厘米级定位是决策规划、控制等模块执行的必要条件。目前基于车道线与高精度地图的定位方案依赖于高精度组合惯导(ins)等提供较好的初值来实现初始化，但在非开阔环境下，非常容易受到多遮挡、多路径等因素影响，高精度组合惯导航较难快速的为初始化提供车道级别精度的初始值，会使得车辆定位慢，且在有遮挡存在时，往往难以定位，或出现定位不准确的情况，导致目前车辆定位效果不理想的问题。


技术实现要素：

3.本发明提供一种车辆定位初始化方法、装置、设备及存储介质，用以解决现有技术中通过高精度组合惯导进行车辆车辆定位初始化时的定位慢、效果不理想的问题，实现采用粒子滤波法，实现车道级快速定位，无需依赖高精度组合惯导，结合高精度地图实现高精度全局定位的快速初始化，提高定位的精度，并实现车道内的高精度厘米级位置初始化。
4.本发明提供一种车辆定位初始化方法，所述方法包括：
5.基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；
6.基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；
7.基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；
8.基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
9.根据本发明提供的一种车辆定位初始化方法，所述基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子的步骤，包括：
10.基于所述车辆的周围环境中道路边界标识的感测数据，对所述道路边界标识进行采样得到多个第一观测点；
11.基于预设的高精度地图数据库，获取所述粒子滤波器中粒子对应的局部高精度地图；
12.根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重；
13.根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子。
14.根据本发明提供的一种车辆定位初始化方法，所述根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒
子滤波器中粒子的权重为当前权重的步骤，包括：
15.将所述第一观测点转换至所述局部高精度地图所在的世界坐标系中，得到转换后的第一观测点；
16.计算所述转换后的第一观测点的位置与所述局部高精度地图中所述道路边界标识的位置之间的欧式距离，得到距离数据；
17.基于所述距离数据更新所述粒子滤波器中粒子的权重，得到每个粒子的当前权重。
18.根据本发明提供的一种车辆定位初始化方法，所述根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子的步骤之后，所述方法还包括：
19.当所述更新后粒子满足预设重采样条件时，对所述粒子滤波器中所有所述更新后粒子进行重采样，得到重采样后的粒子；
20.所述基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置的步骤，包括：
21.基于所述车辆的速度相关数据，利用车辆运动模型预测所述重采样后的粒子的下一位置，得到粒子新位置。
22.根据本发明提供的一种车辆定位初始化方法，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤之前，所述方法还包括：
23.判断所述粒子滤波器是否满足收敛条件；
24.若是，则执行基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤；
25.若否，则基于所述粒子新位置，循环执行：基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子。
26.根据本发明提供的一种车辆定位初始化方法，所述判断所述粒子滤波器是否满足收敛条件的步骤，包括：
27.计算所述粒子滤波器中所有粒子当前权重的实际方差，判断所述实际方差是否超过方差阈值；
28.若是，则对所述更新后粒子进行聚类计算，当聚类结果为预设结果时，确定所述粒子滤波器满足收敛条件。
29.根据本发明提供的一种车辆定位初始化方法，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤，包括：
30.基于所述粒子新位置，确定出用于车辆车道级定位中位置转换的目标粒子；
31.计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化。
32.根据本发明提供的一种车辆定位初始化方法，所述计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化的步骤，包括：
33.对车辆所在的车道线标识进行采样得到多个第二观测点；
34.基于所述目标粒子，将所述第二观测点转换至所述目标粒子对应的高精度地图所在的世界坐标系中，得到转换后的第二观测点；
35.获取所述目标粒子对应的高精度地图中所述车道线标识的位置信息；
36.构建所述转换后的第二观测点的位置与所述目标粒子对应的高精度地图中所述车道线标识的位置之间的欧式距离残差，利用优化方法，确定出车辆的目标位置，以完成车辆的定位初始化。
37.本发明还提供一种车辆定位初始化装置，包括：
38.数据获取模块，用于基于车辆全球导航卫星系统定位数据，初始化粒子滤波器；
39.滤波器处理模块，用于基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；
40.位置预测模块，用于基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；
41.位置初始化模块，用于基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
42.本发明还提供一种车辆定位初始化设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述车辆定位初始化方法。
43.本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述车辆定位初始化方法。
44.本发明提供的车辆定位初始化方法、装置、设备及存储介质，基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。也即，采用粒子滤波法，利用高精度地图与道路边界标识信息实现车道级快速定位，无需依赖高精度组合惯导，结合高精度地图与道路边界标识的感测数据实现高精度全局定位的快速初始化，提高定位的精度，并实现车道级的高精度位置初始化。
附图说明
45.为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
46.图1是本发明提供的车辆定位初始化方法的流程示意图之一；
47.图2是本发明提供的车辆定位初始化方法的流程示意图之二；
48.图3是本发明提供的车辆定位初始化方法中粒子滤波器的粒子预测流程示意图；
49.图4是本发明提供的车辆定位初始化设备的结构示意图。
具体实施方式
50.为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
51.下面结合图1-图4描述本发明的车辆定位初始化方法，参照图1、图2，所述车辆定位初始化方法包括：
52.步骤s100，基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；
53.步骤s200，基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；
54.步骤s300，基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；
55.步骤s400，基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
56.本实施例旨在：通过粒子滤波的方法，实现车道级快速定位，不依赖于高精度组合惯导，不需要大量的存储空间与算力即可完成全局高精度位置初始化。并且采取车辆周围环境中道路边界标识并使用粒子滤波器的方法，能够在快速、低算力的条件下，实现高速全场景的车道级位置确定，提高车辆定位初始化的速度。
57.在本实施例中，针对的具体应用场景是：
58.对于高阶自动驾驶系统来说，高精度的厘米级定位是决策规划、控制等模块执行的必要条件。目前基于车道线与高精度地图的定位方案依赖于高精度组合惯导(ins)等提供较好的初值来实现初始化，但在非开阔环境下，非常容易受到多遮挡、多路径等因素影响，高精度组合惯导航较难快速的为初始化提供车道级别精度的初始值，会使得车辆定位慢，且在有遮挡存在时，往往难以定位，或出现定位不准确的情况，导致目前车辆定位效果不理想的问题。
59.作为一种示例，车辆定位初始化方法可以应用于车辆定位初始化系统，所述车辆定位初始化系统应用于车辆定位初始化设备中。
60.作为一种示例，粒子滤波器的粒子指的是一组随机生成的状态向量，在粒子滤波器中，状态向量通常表示系统的状态。每个粒子都有一个权重，用于表示该粒子的重要性，在每个时刻，粒子的权重根据观测数据进行更新，以反映该粒子与实际状态的匹配程度。粒子滤波器中粒子数量越多，估计结果越精确，但计算量也越大。
61.具体步骤如下：
62.步骤s100，基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；
63.作为一种示例，全球导航卫星系统(global navigation satellite system，gnss)包括但不限于全球定位系统(global positioning system，gps)、北斗卫星导航系统、伽利略卫星导航系统、格洛纳斯卫星导航系统等，全球导航卫星系统定位数据是指gnss
信号接收机获取的定位数据，例如gnss定位数据提供较为粗略的位置信息和轨迹信息，满足地图制作的一些基本需求，应用于汽车驾驶领域。
64.需要说明的是，gnss定位数据可以是低成本gnss接收机获取的定位数据，如属于标准精度的低成本gnss接收机，还可以是高精度的gnss接收机。目前基于高精度地图的定位方案将高精度组合惯导作为定位系统的初始化必要传感器，其非常容易受到多遮挡的影响，并且初始化时间往往较长，使得使用高精度组合惯导传感器成本高。鉴于上述原因，在本发明中，可以使用低成本gnss获取的观测数据提高车辆的车道级快速定位，能够降低车辆定位的成本，实现低成本、快速、高精度的定位效果。
65.作为一种示例，获取低成本gnss定位数据，初始化粒子滤波器。
66.作为一种示例，所述基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器的步骤，包括：
67.步骤s110，以gnss观测点位置为中心，在其设定范围内构建预设规格的栅格；
68.步骤s120，取每个栅格的中心点作为粒子滤波器的初始的粒子。
69.作为一种示例，为了实现车道级别的定位初始化，提高算力，避免大量的冗余计算，以gnss观测点位置为中心，在其设定范围内构预设规格的栅格，将栅格中心点作为粒子滤波器的初始的粒子。
70.以gnss观测点位置为中心的设定范围是指基于gnss的精度确定的，栅格的预设规格是根据车道宽度确定的。例如，若gnss的误差在50米左右，车道宽度为3.5米时，以gnss观测点位置为中心，在边长为50米的范围内构建边长为3.5米的栅格。需要说明的是，根据实际gnss精度与车道宽度确定的上述数据还可以是其他数值，根据实际情况进行划分，在此不做具体限定。
71.步骤s200，基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子。
72.作为一种示例，车辆周围环境中道路边界标识是指特定区域内具有稳定特征的地标或标志性物体的标识，其能够在相当长的一段时间内都保持不变，例如，路沿、护栏等，在高精度地图中也同样包含了大量的道路边界标识的指示与位置信息。需要说明的是，道路边界标识可以是沿着道路方向(如路沿、护栏)的道路交通设施的边界标识，也可以是道路的纵向方向的道路交通设施的边界标识，在纵向道路边界标识基础上(主要提升车辆位置的横向约束，在转弯处也可以提升车辆位置的纵向约束)，也可以用横向布置的道路边界标识，例如龙门架，以提升车辆位置的纵向约束。
73.为了便于描述，以下道路边界标识以路沿或护栏为例进行描述，其他物体具体实施方式基本相同，在此不再赘述。
74.作为一种示例，通过对车辆周围环境中的路沿或护栏进行采样，确定出路沿或护栏的多个第一观测点，该第一观测点反映的是路沿或护栏的位置和形状信息，结合粒子滤波器中的粒子对应的高精度地图，可以将基于车辆感知的第一观测点映射于高精度地图中，通过两者之间的距离，得到粒子滤波器中用于确定路沿或护栏位置的粒子，该粒子将感知的路沿或护栏的测量结果匹配到高精度地图中的路沿或护栏中，用于后续实现快速对车辆进行定位，完成全局高精度位置初始化。
75.作为一种示例，所述基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子的步骤，包括：
76.步骤s210，基于所述车辆的周围环境中道路边界标识的感测数据，对所述道路边界标识进行采样得到多个第一观测点；
77.步骤s220，基于预设的高精度地图数据库，获取所述粒子滤波器中粒子对应的局部高精度地图；
78.步骤s230，根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重；
79.步骤s240，根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子。
80.作为一种示例，对车辆周围环境中的路沿或护栏进行采样确定出多个第一观测点，获取粒子滤波器中粒子，根据粒子获取每个粒子对应的高精度地图中的路沿或护栏，得到高精度地图中路沿或护栏对应的第一地图数据。通过第一观测点在高精度地图中的数值与第一地图数据之间的距离，将第一观测点在高精度地图中的观测值与高精度地图中路沿或护栏进行相关联，即更新粒子滤波器中的粒子，得到更新后粒子。
81.作为一种示例，根据检测的路沿或者护栏的三次多项式方程，均匀采样观测点，得到多个第一观测点，其中，三次多项方程式用于描述路沿或护栏的位置和形状，检测和识别路沿或护栏。
82.作为一种示例，检测过程可以通过相机检测、毫米波雷达检测，还是通过相机与毫米波雷达融合的方式检测。
83.作为一种示例，车辆自动驾驶或智能导航中配置有一套高精度地图数据库与引擎，粒子滤波器中每个粒子包含了位置信息、协方差信息等，将粒子的位置信息输入至高精度地图的引擎，可以获取到粒子对应位置附近的局部高精度地图，该地图中包含了护栏或者路沿的形状信息、位置信息等。将第一观测点的数据集成到粒子滤波器中，用于更新粒子滤波器中所有粒子的权重，得到粒子的当前权重，根据粒子的当前权重与粒子位置坐标，更新粒子滤波器中粒子。第一观测点是相对于车辆自身的坐标系统确定的，而局部高精度地图属于世界坐标系中，因此，需要表示路沿或护栏的第一观测点转换至与局部高精度地图处于同一世界坐标系中，再进行粒子权重的更新操作。
84.在本实施例中，根据检测路沿或护栏的三次多项方程式均匀采样得到第一观测点，结合粒子滤波器中粒子对应的局部高精度地图，更新粒子滤波器中粒子的权重，从而达到粒子滤波的目的。
85.作为一种示例，所述根据所述第一观测点在世界坐标中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重的步骤，包括：
86.步骤s231，将所述第一观测点转换至所述局部高精度地图所在的世界坐标系中，得到转换后的第一观测点；
87.步骤s232，计算所述转换后的第一观测点的位置与所述局部高精度地图中所述道路边界标识的位置之间的欧式距离，得到距离数据；
88.步骤s233，基于所述距离数据更新所述粒子滤波器中粒子的权重，得到每个粒子的当前权重。
89.作为一种示例，第一观测点是感知到的路沿或护栏，这种感知到的路沿或护栏是在车体坐标系下确定的，而高精度地图是在世界坐标系下确定的，因此，需要将第一观测点转换到当前粒子对应的局部高精度地图所在的世界坐标系中，得到转换后的第一观测点。
90.作为一种示例，将获取到的第一观测点通过粒子滤波器中每个粒子变换到每个粒子的世界坐标系中。具体的，将在车体坐标系下感知的路沿或护栏的第一观测点为p_b，在世界坐标系下的高精度地图的数据记为p_w，每个粒子是世界坐标系下表示的，记为t_{wp}。因此，需要把p_b变换到世界坐标系下表示，才能与高精度地图做比较。t_{wp}是矩阵，则将第一观测点变换到世界坐标系记为t_{wp}*p_b。
91.可以理解，不同的粒子对相同的路沿或护栏变换后，得到同一个世界坐标系下护栏或者路沿的不同数值的表示，这些不同的表示与局部高精度地图中路沿或护栏比较会得到有不同的误差。
92.作为一种示例，将转换到世界坐标系下的转换后的第一观测点的位置与局部高精度地图中道路边界标识的位置进行欧式距离计算，得到距离数据。基于该距离数据更新粒子滤波器中粒子的权重，得到每个粒子的当前权重。具体的，当感知到a护栏时，基于预设粒子去局部高精度地图中查询当前粒子位置所在的护栏a’，计算感知到的a护栏上的m个第一观测点与局部高精度地图中与其对应的护栏之间的距离，得到距离数据。将各个粒子对应的距离数据进行归一化处理，用于更新粒子滤波器中每个粒子的权重，从而更新粒子滤波器中粒子。
93.作为一种示例，粒子滤波器中有n个粒子，以1个粒子为例进行描述，若感知到a根护栏(最常规的是左右两根)，用当前的这个粒子去局部高精度地图中查询当前粒子位置所在的护栏，查询到b根。若b＝a，则表示查询到的护栏数量与感知到的数量一样，那么以最常规的情况a＝2为例，将感知到的左边护栏上的m个第一观测点，与局部高精度地图中左边护栏计算欧式距离，同理右边，将感知到的右边护栏上的m个第一观测点与局部高精度地图中右边护栏之间计算欧式距离，由此通过当前粒子共计算2*m次，推广到一般情况下，就是a*m次。因此，n个粒子共计算n*a*m次。
94.若b≠a，则表示查询到的护栏数量与感知到的数量不一样，可能由于某些关系、遮挡等，只能感知到1根护栏，即a＝1。此时a＝1，b＝2，计算感知到的1根护栏上m个第一观测点与局部高精度地图上同侧的护栏之间的欧式距离，由此，n个粒子的计算次数是n*a*m次。
95.通过上述计算所述转换后的第一观测点的位置与局部高精度地图中所述道路边界标识的位置之间的欧式距离，得到距离数据，实现了两者之间的关联。
96.作为一种示例，在对各个粒子对应的距离数据进行归一化处理，用于更新粒子滤波器中每个粒子的权重。初始化时，粒子滤波器中所有粒子的权重相同，则对粒子权重进行均匀化，每个粒子的权重为w0＝1/粒子总数，所有粒子的权重和为1。
97.粒子更新时，对于每一个粒子对应的距离数据进行归一化处理之后记为di，则粒子置信度pi＝1-di，在已知每个粒子的初始权重wi的基础上，更新后粒子的当前权重为vi＝wi*pi，i表示第i个粒子。
98.作为一种示例，所述根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波
器中的粒子的步骤之后，所述方法还包括：
99.步骤a11，当所述更新后粒子满足预设重采样条件时，对所述粒子滤波器中所有所述更新后粒子进行重采样，得到重采样后的粒子。
100.在实际过程中，利用权重更新粒子滤波器中粒子时，在几次迭代之后，粒子的权重的分布会极其不均匀，例如出现个别粒子权重很大接近于1，而其他的都为0或接近于0的情况，此时采用重采样策略，即每次权重更新之后，若更新后粒子满足预设重采样条件，则根据当前权重对所有粒子进行重采样，之后将所有权重设定为相同。这样我们用粒子的数量代替了粒子的权重，避免了权重的不均匀。
101.作为一种示例，更新后粒子满足预设重采样条件可以是粒子滤波器处于发散状态时，也可以是粒子的权重迭代更新了一定次数。
102.作为一种示例，粒子滤波器发散：当前粒子的离散程度大于之前粒子的离散程度，离散程度用粒子坐标的方差表示。可以理解，当更新后粒子的位置坐标的方差大于预设的方差值时，对粒子滤波器中所有更新后粒子进行重采样，得到重采样后的粒子以及权重。预设的方差值根据实际训练经验确定。
103.作为一种示例，在迭代更新n次后，对粒子进行重采样，针对粒子进行重新划分。具体的，对每个粒子的当前权重wi进行排序，取粒子当前权重靠后一定比例的粒子进行重新分配(如粒子当前权重后30％的粒子)，这些粒子的位置需要变成当前权重排序中靠前一定比例(前30％)的粒子附近范围内的点，权重越高的粒子有更大的的概率生存下来，权重越小的例子生存下来的概率就越低，从而达到优胜劣汰的目的。需要说明的是，在粒子附近范围内选择点时，范围大小的选择和收敛速度有关，范围越小，粒子滤波器的收敛越快，因此，根据实际情况，设定范围大小，在此不做具体限定。
104.步骤s300，基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置。
105.作为一种示例，在初始化阶段，车辆接收到来自gps的观测数据，并将其用于初始化车辆的位置，gps观测数据包括车辆的位置和行驶朝向，在粒子滤波器中使用gps观测数据初始化这些粒子的位置和朝向，用于后续对车道级定位提供基础。
106.作为一种示例，获取车辆的速度相关数据，如车辆底盘imu(inertial measurement unit，惯性测量单元)数据，通过底盘imu数据能够反映车辆当前的加速度、角度轴等数据。以下车辆的速度相关数据以底盘imu数据为例进行描述。
107.参照图3，在粒子滤波器初始化后，基于车辆的底盘imu数据与车辆运动模型计算gps相对位移，对所有更新后粒子进行位置预测，预测每个更新后粒子下一步出现的位置。基于预测后的粒子的新位置的信息，确定车辆的目标位置，以完成车辆的定位初始化。
108.在本实施例中，通过车辆周围环境中道路边界标识的第一观测点与高精度地图中相应的道路边界标识进行关联，对粒子滤波器进行初始化，更新粒子滤波器中的粒子。基于初始化后的粒子滤波器以及实时检测的车辆的速度相关数据，预测出粒子的下一步的位置信息，进而确定出车辆的目标位置，实现车道级高精度位置初始化。
109.作为一种示例，所述基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置的步骤，包括：
110.步骤b11，基于所述车辆的速度相关数据，利用车辆运动模型预测所述重采样后的
粒子的下一位置，得到粒子新位置。
111.当对粒子滤波器中粒子进行重采样，得到重采样粒子后，预测粒子的下一位置时，预测的是重采样后粒子的下一位置。可以理解，基于车辆的速度相关数据、重采样后粒子的位置坐标以及权重，利用车辆运动模型来预测粒子新位置。
112.步骤s400，基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
113.作为一种示例，确定车辆的目标位置可以是通过计算粒子滤波器中所有粒子的粒子新位置的均值得到，也可以是通过加权平均计算得到，通过上述方式计算得到的坐标数据所对应的位置即为车辆的目标位置，从而完成车辆的定位初始化。
114.作为一种示例，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤之前，所述方法还包括
115.步骤c11，判断所述粒子滤波器是否满足收敛条件；
116.步骤c12，若是，则执行基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤；
117.步骤c13，若否，则基于所述粒子新位置，循环执行基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子的步骤。
118.在基于粒子新位置确定出车辆的位置信息之前，需要判断粒子滤波器是否收敛，当粒子滤波器收敛时，可基于车辆的周围环境中道路边界标识的感测数据计算车道级高精度位置，即执行基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤。若粒子滤波器未收敛，则需要继续对粒子滤波器进行迭代处理，获取新的路沿或护栏的三次多项式方程，均匀采样出第一观测点，根据新的第一观测点更新粒子滤波器中粒子或重采样粒子，继续进行粒子滤波器是否收敛的判断，直至粒子滤波器满足收敛条件而停止。
119.在本实施例中，通过对粒子滤波器的收敛判断与初始化过程，提高了粒子滤波器对车辆定位的精度，且依靠收敛后的粒子滤波器能够实现车道级快速定位方法，无需依赖高精度组合惯导，适用高速全场景的全局位置快速初始化。
120.作为一种示例，所述判断所述粒子滤波器是否满足收敛条件的步骤，包括：
121.步骤c111，计算所述粒子滤波器中所有粒子当前权重的实际方差，判断所述实际方差是否超过方差阈值；
122.步骤c112，若是，则对所述更新后粒子进行聚类计算，当聚类结果为预设结果时，确定所述粒子滤波器满足收敛条件。
123.作为一种示例，计算粒子滤波器中所有粒子当前权重的实际方差，判断实际方差是否大于或等于方差阈值，方差阈值根据实际训练经验确定。若实际方差是否大于方差阈值，则对更新后粒子进行聚类计算，例如k-means聚类，直至聚类结果为预设结果时，确定粒子滤波器收敛。其中，预设结果为聚类类别为一类。若聚类类别大于一类时，重复执行步骤s200-步骤s300的过程，直至粒子滤波器满足收敛条件。
124.若实际方差是否小于方差阈值，则重复执行步骤s200-步骤s300的过程，直至粒子滤波器中所有粒子当前权重的实际方差超过方差阈值。
125.可以理解，判断粒子滤波器的收敛条件是同时满足实际方差超过方差阈值，且对粒子进行聚类计算后聚类结果为一类，即确定当前的粒子滤波器收敛。
126.本发明提供一种车辆定位初始化方法、装置、设备及存储介质，与目前通过高精度组合惯导进行车辆车辆定位初始化时的定位慢、效果不理想相比，在本发明中，基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。也即，采用粒子滤波法，利用高精度地图与道路边界标识信息实现车道级快速定位，无需依赖高精度组合惯导，结合高精度地图与车道线信息实现高精度全局定位的快速初始化，提高定位的精度，并实现车道级的高精度位置初始化。
127.基于上述车辆定位初始化方法的第一实施例，提出车辆定位初始化方法的第二实施例。
128.作为一种示例，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤，包括：
129.步骤s500，基于所述粒子新位置，确定出用于车辆车道级定位中位置转换的目标粒子；
130.步骤s600，计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化。
131.当粒子滤波器收敛时，计算聚类为一类的所有粒子的位置坐标均值，得到该均值对应位置的粒子，即为目标粒子。该目标粒子将感知的车道线标识的测量结果匹配到高精度地图中的车道线标识中，用于后续实现感知的车道线与高精度地图中车道线的关联，进而能够快速对车辆进行定位，完成全局高精度位置初始化。即该目标粒子用于车辆车道内的定位中位置的转换，提高车辆定位的全局位置快速初始化，并且采用点对点的优化方法，实现车道内高精度厘米级位置初始化。
132.作为一种示例，基于车辆坐标系统检测到车辆所在车道线的第二观测点，将第二观测点转换至目标粒子对应的高精度地图所在的世界坐标系中，计算转换后第二观测点的位置与目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，从而快速确定出车辆的位置，完成车辆的定位初始化。
133.在本实施例中，基于护栏或路沿的初步定位过程对粒子滤波器进行收敛，通过收敛后的粒子滤波器利用粒子滤波方法，实现车道级快速定位，且采用点到点的优化方法，实现车道内高精度厘米级位置初始化。
134.作为一种示例，所述计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化的步骤，包括：
135.步骤s610，对车辆所在的车道线标识进行采样得到多个第二观测点；
136.步骤s620，基于所述目标粒子，将所述第二观测点转换至所述目标粒子对应的高精度地图所在的世界坐标系中，得到转换后的第二观测点；
137.步骤s630，获取所述目标粒子对应的高精度地图中所述车道线标识的位置信息；
138.步骤s640，构建所述转换后的第二观测点的位置与所述目标粒子对应的高精度地图中所述车道线标识的位置之间的欧式距离残差，利用优化方法，确定出车辆的目标位置，以完成车辆的定位初始化。
139.作为一种示例，对车辆所在的车道线进行采样，获取车道线的三次多项方程式，根据车道线检测三次多项方程式结果，确定多个第二观测点。由于第二观测点是基于车辆坐标系统下确定的，而高精度地图是在世界坐标系下确定的，因此利用目标粒子的位置信息将采样到的多个第二观测点变换到世界坐标系下，得到转换后的第二观测点。
140.基于转换后的第二观测点与目标粒子对应的高精度地图中车道线标识的位置信息，构建转换后观测点的位置与目标粒子对应的高精度地图中车道线标识的位置之间的欧式距离残差，通过优化方法，快速确定出车辆当前的位置信息，完成车辆定位初始化。
141.作为一种示例，优化的方法可以是levenberg-marquardt(lm)非线性优化的方法。利用lm非线性优化方法求解转换后观测点与第二地图数据之间的欧式距离残差的最优解，得到车辆的位置信息，以此通过点对点的优化方法，实现车道内的高精度厘米级位置初始化。
142.在本实施例中，基于车道级精度初始值，采用车道线与优化的方法，能够快速提供高精度的车道内厘米级位置初始化，提高车辆定位效果。
143.下面对本发明提供的车辆定位初始化装置进行描述，下文描述的车辆定位初始化装置与上文描述的车辆定位初始化方法可相互对应参照。
144.本发明还提供一种车辆定位初始化装置，所述装置包括：
145.数据获取模块，用于基于车辆全球导航卫星系统定位数据，初始化粒子滤波器；
146.滤波器处理模块，用于基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；
147.位置预测模块，用于基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；
148.位置初始化模块，用于基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
149.和/或，所述滤波器处理模块还包括：
150.采样观测子模块，用于基于所述车辆的周围环境中道路边界标识的感测数据，对所述道路边界标识进行采样得到多个第一观测点；
151.数据获取子模块，用于基于预设的高精度地图数据库，获取所述粒子滤波器中粒子对应的局部高精度地图；
152.权重更新子模块，用于根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重；
153.粒子更新子模块，用于根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子。
154.和/或，所述权重更新子模块还包括：
155.第一坐标转换单元，用于将所述第一观测点转换至局部所述高精度地图所在的世界坐标系中，得到转换后的第一观测点；
156.距离计算单元，用于计算所述转换后的第一观测点的位置与所述局部高精度地图中所述道路边界标识的位置之间的欧式距离，得到距离数据；
157.权重更新单元，用于基于所述距离数据更新所述粒子滤波器中粒子的权重，得到每个粒子的当前权重。
158.和/或，所述滤波器处理模块还包括：
159.重采样子模块，用于当所述更新后粒子满足预设重采样条件时，对所述粒子滤波器中所有所述更新后粒子进行重采样，得到重采样后的粒子；
160.和/或，所述位置预测模块还包括：
161.位置预测子模块，用于基于所述车辆的速度相关数据，利用车辆运动模型预测所述重采样后的粒子的下一位置，得到粒子新位置。
162.和/或，所述装置还包括：
163.收敛判断模块，用于判断所述粒子滤波器是否满足收敛条件；
164.第一判断确认模块，用于若是，则执行基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤；
165.第二判断确认模块，用于若否，则基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子。
166.和/或，所述收敛判断模块还包括：
167.收敛判断子模块，用于计算所述粒子滤波器中所有粒子当前权重的实际方差，判断所述实际方差是否超过方差阈值；
168.聚类计算子模块，用于若是，则对所述更新后粒子进行聚类计算，当聚类结果为预设结果时，确定所述粒子滤波器满足收敛条件。
169.和/或，所述位置初始化模块还包括：
170.粒子处理模块，用于基于所述粒子新位置，确定出用于车辆车道级定位中位置转换的目标粒子；
171.第二位置初始化子模块，用于计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化。
172.和/或，所述第二位置初始化子模块还包括：
173.采样观测单元，用于对车辆所在的车道线标识进行采样得到多个第二观测点；
174.第二坐标转换单元，用于基于所述目标粒子，将所述第二观测点转换至所述目标粒子对应的高精度地图所在的世界坐标系中，得到转换后的第二观测点；
175.数据获取单元，用于获取所述目标粒子对应的高精度地图中所述车道线标识相应的位置信息；
176.位置初始化单元，用于构建所述转换后的第二观测点的位置与所述目标粒子对应的高精度地图中所述车道线标识的位置之间的欧式距离残差，利用优化方法，确定出车辆的目标位置，以完成车辆的定位初始化。
177.图4示例了一种车辆定位初始化设备的实体结构示意图，如图4所示，该车辆定位初始化设备可以包括：处理器(processor)810、通信接口(communications interface)820、存储器(memory)830和通信总线840，其中，处理器810，通信接口820，存储器830通过通信总线840完成相互间的通信。处理器810可以调用存储器830中的逻辑指令，以执行车辆定位初始化方法，该方法包括：基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
178.此外，上述的存储器830中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
179.另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的车辆定位初始化方法，该方法包括：基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
180.又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的车辆定位初始化方法，该方法包括：基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。
181.以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
182.通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上
述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如rom/ram、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
183.最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

技术特征：
1.一种车辆定位初始化方法，其特征在于，所述方法包括：基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。2.根据权利要求1所述的车辆定位初始化方法，其特征在于，所述基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子的步骤，包括：基于所述车辆的周围环境中道路边界标识的感测数据，对所述道路边界标识进行采样得到多个第一观测点；基于预设的高精度地图数据库，获取所述粒子滤波器中粒子对应的局部高精度地图；根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重；根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子。3.根据权利要求2所述的车辆定位初始化方法，其特征在于，所述根据所述第一观测点在世界坐标系中的位置与所述局部高精度地图中所述道路边界标识的位置之间的距离，更新所述粒子滤波器中粒子的权重为当前权重的步骤，包括：将所述第一观测点转换至所述局部高精度地图所在的世界坐标系中，得到转换后的第一观测点；计算所述转换后的第一观测点的位置与所述局部高精度地图中所述道路边界标识的位置之间的欧式距离，得到距离数据；基于所述距离数据更新所述粒子滤波器中粒子的权重，得到每个粒子的当前权重。4.根据权利要求2所述的车辆定位初始化方法，其特征在于，所述根据所述粒子滤波器中粒子的当前权重，更新所述粒子滤波器中的粒子的步骤之后，所述方法还包括：当所述更新后粒子满足预设重采样条件时，对所述粒子滤波器中所有所述更新后粒子进行重采样，得到重采样后的粒子；所述基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置的步骤，包括：基于所述车辆的速度相关数据，利用车辆运动模型预测所述重采样后的粒子的下一位置，得到粒子新位置。5.根据权利要求1所述的车辆定位初始化方法，其特征在于，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤之前，所述方法还包括：判断所述粒子滤波器是否满足收敛条件；若是，则执行基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤；若否，则基于所述粒子新位置，循环执行：基于所述车辆的周围环境中道路边界标识的
感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子。6.根据权利要求5所述的车辆定位初始化方法，其特征在于，所述判断所述粒子滤波器是否满足收敛条件的步骤，包括：计算所述粒子滤波器中所有粒子当前权重的实际方差，判断所述实际方差是否超过方差阈值；若是，则对所述更新后粒子进行聚类计算，当聚类结果为预设结果时，确定所述粒子滤波器满足收敛条件。7.根据权利要求1所述的车辆定位初始化方法，其特征在于，所述基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化的步骤，包括：基于所述粒子新位置，确定出用于车辆车道级定位中位置转换的目标粒子；计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化。8.根据权利要求7所述的车辆定位初始化方法，其特征在于，所述计算车辆所在车道线标识基于所述目标粒子转换后的多个第二观测点的位置与所述目标粒子对应的高精度地图中相应车道线标识的位置之间的距离，确定出车辆的目标位置，以完成车辆的定位初始化的步骤，包括：对车辆所在的车道线标识进行采样得到多个第二观测点；基于所述目标粒子，将所述第二观测点转换至所述目标粒子对应的高精度地图所在的世界坐标系中，得到转换后的第二观测点；获取所述目标粒子对应的高精度地图中所述车道线标识的位置信息；构建所述转换后的第二观测点的位置与所述目标粒子对应的高精度地图中所述车道线标识的位置之间的欧式距离残差，利用优化方法，确定出车辆的目标位置，以完成车辆的定位初始化。9.一种车辆定位初始化装置，其特征在于，所述装置包括：数据获取模块，用于基于车辆全球导航卫星系统定位数据，初始化粒子滤波器；滤波器处理模块，用于基于所述车辆的周围环境中道路边界标识的感测数据和所述粒子滤波器中粒子对应的局部高精度地图中所述道路边界标识的位置信息，更新所述粒子滤波器中的粒子；位置预测模块，用于基于所述车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；位置初始化模块，用于基于所述粒子新位置，确定所述车辆的目标位置，以完成所述车辆的定位初始化。10.一种车辆定位初始化设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至8任一项所述车辆定位初始化方法。11.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8任一项所述车辆定位初始化方法。

技术总结
本发明提供一种车辆定位初始化方法、装置、设备及存储介质，该方法包括：基于车辆的全球导航卫星系统定位数据，初始化粒子滤波器；基于车辆的周围环境中道路边界标识的感测数据和粒子滤波器中粒子对应的局部高精度地图中道路边界标识的位置信息，更新粒子滤波器中的粒子；基于车辆的速度相关数据，利用车辆运动模型预测更新后粒子的下一位置，得到粒子新位置；基于粒子新位置，确定车辆的目标位置，以完成车辆的定位初始化。即采用粒子滤波法，利用高精度地图与道路边界标识信息实现车道级快速定位，无需依赖高精度组合惯导，结合高精度地图与道路边界标识的感测数据实现高精度全局定位的快速初始化，提高定位的精度。提高定位的精度。提高定位的精度。


技术研发人员：江玲新 叶茂
受保护的技术使用者：嬴彻星创智能科技（上海）有限公司
技术研发日：2023.05.25
技术公布日：2023/9/22