[发明专利]定位初始化方法、系统及移动工具

权利要求书

1.一种定位初始化方法，其特征在于，包括：

获取车辆的初始位姿点和地图；

接收车辆上激光雷达采集的单帧激光点云；

在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；

根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；

确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；

针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；

确定所述单帧激光点云分别与每个待选位姿点的匹配度；

根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。

2.根据权利要求1所述的方法，其特征在于，所述确定所述单帧激光点云分别与每个估计位姿点的匹配度，具体包括：

针对每个估计位姿点，执行以下步骤：

根据估计位姿点将所述单帧激光点云投影到所述地图中，得到激光点云中各激光点在所述地图中的投影点；

计算每个投影点与最近障碍物的距离值；

根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度。

3.根据权利要求2所述的方法，其特征在于，根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度，具体包括：

计算各投影点的距离值的和值或平均值，根据所述和值或平均值确定出对应的匹配度；和值或平均值越大对应的匹配度越小。

4.根据权利要求1所述的方法，其特征在于，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：

按照匹配度从高到低的顺序，选取预置数量的估计位姿点作为候选位姿点。

5.根据权利要求1所述的方法，其特征在于，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：

对所述多个估计位姿点进行聚类，得到多个位姿簇；

针对每个位姿簇，按照匹配度从高到低的顺序，从该位姿簇中的估计位姿点选取多个估计位姿点作为候选位姿点。

6.根据权利要求1所述的方法，其特征在于，根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点，具体包括：

将匹配度最高的待选位姿点作为所述目标位姿点；

或者，将匹配度较高的多个待选位姿点中距离所述初始位姿点最近的待选位姿点作为目标位姿点；

或者，将匹配度较高的多个待选位姿点的中心位姿点作为目标位姿点。

7.根据权利要求1所述的方法，其特征在于，在接收到所述单帧激光点云之后，还包括：

按照预置的第一有效性判断条件，判断所述单帧激光点云是否有效；

若有效，则执行所述在所述地图中搜索出以初始位姿点为中心的第一区域的步骤。

8.根据权利要求7所述的方法，其特征在于，所述第一有效性判断条件为以下任意一条或多条：

所述单帧激光点云中，距离值在所述激光雷达测距范围内的激光点云的数量大于或等于预置的第一数量阈值；

所述第一数量与单帧激光点云中所有激光点云总数的比值大于或等于预置的第一比值阈值。

9.根据权利要求1所述的方法，其特征在于，根据所述多个待选位姿点确定所述车辆的目标位姿点之后，还包括：

判断所述目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。