CN113421306A - 定位初始化方法、系统及移动工具 - Google Patents

定位初始化方法、系统及移动工具 Download PDF

Info

Publication number
CN113421306A
CN113421306A CN202110962408.7A CN202110962408A CN113421306A CN 113421306 A CN113421306 A CN 113421306A CN 202110962408 A CN202110962408 A CN 202110962408A CN 113421306 A CN113421306 A CN 113421306A
Authority
CN
China
Prior art keywords
pose
point
points
determining
map
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110962408.7A
Other languages
English (en)
Inventor
顾浩
曹菊宁
王苏南
高翔
张放
张德兆
王肖
李晓飞
霍舒豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Idriverplus Technologies Co Ltd
Original Assignee
Beijing Idriverplus Technologies Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Idriverplus Technologies Co Ltd filed Critical Beijing Idriverplus Technologies Co Ltd
Priority to CN202110962408.7A priority Critical patent/CN113421306A/zh
Publication of CN113421306A publication Critical patent/CN113421306A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Remote Sensing (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明提供了一种定位初始化方法、系统及移动工具，方法包括：获取车辆的初始位姿点和地图；接收单帧激光点云；在地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对第一区域进行栅格化处理得到第一栅格地图；确定出多个估计位姿点及其分别与单帧激光点云的匹配度，从多个估计位姿点中选取多个候选位姿点；在地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；确定单帧激光点云分别与每个待选位姿点的匹配度；根据多个待选位姿点的匹配度确定车辆的目标位姿点。

Description

定位初始化方法、系统及移动工具
技术领域
本发明涉及自动驾驶技术领域，尤其涉及一种定位初始化方法、系统及移动工具。
背景技术
随着科技水平的快速发展，无人驾驶技术越发受到重视。无人驾驶可高效利用交通资源，减少碳排放，并能有效应对劳动力的成本上升，减少人们繁重的重复性体力劳动，具有广泛的应用市场。无论是低速商用机器人或是高速乘用车，实现无人驾驶功能均需要依赖高精度定位技术。
目前室内低速商用移动机器人定位主要采用多传感器融合技术，探测周围环境主要采用激光或摄像头，而单线激光由于其稳定可靠价格低廉的优势，在该领域具有广泛的应用市场。采用单线激光与地图匹配方案进行定位主要分为定位初始化，实时融合定位两部分，因此定位初始化是单线激光无人车定位的基础。室内无人车初始化时由于缺少全球卫星导航系统（Global Navigation Satellite System，GNSS）提供的绝对姿态，相对室外乘用车定位初始化较为不同。
目前针对激光定位初始化方法主要有两种：1.基于GNSS绝对位置的初始化方案；2.基于固定启动点的初始化方案。
基于GNSS绝对位置的初始化方案主要应用于室外GNSS信号良好的区域，当GNSS信号较好时可以给定较好的车辆初始位置。该方案要求卫星信号接收良好，但是室内环境无法满足，因此该方案的适应环境有限。
基于固定启动点的初始化方案受环境干扰较小，在室内同样适用，但要求车辆在固定地点进行初始化。该方案对于车辆停放的准确性要求较高，无法满足车位被占用等情形，因此该方案对于室内快速激光定位挑战较大。
发明内容
本发明实施例的目的是提供一种定位初始化方法，能够稳定有效、快速且准确进行定位初始化，以快速得到精准的目标位姿。
本发明第一方面提供了一种定位初始化方法，所述定位初始化方法包括：
获取车辆的初始位姿点和地图；
接收车辆上激光雷达采集的单帧激光点云；
在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；
针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
确定所述单帧激光点云分别与每个待选位姿点的匹配度；
根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。
在一种可能的实现方式中，所述确定所述单帧激光点云分别与每个估计位姿点的匹配度，具体包括：
针对每个估计位姿点，执行以下步骤：
根据估计位姿点将所述单帧激光点云投影到所述地图中，得到激光点云中各激光点在所述地图中的投影点；
计算每个投影点与最近障碍物的距离值；
根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度。
在一种可能的实现方式中，根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度，具体包括：
计算各投影点的距离值的和值或平均值，根据所述和值或平均值确定出对应的匹配度；和值或平均值越大对应的匹配度越小。
在一种可能的实现方式中，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：
按照匹配度从高到低的顺序，选取预置数量的估计位姿点作为候选位姿点。
在一种可能的实现方式中，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：
对所述多个估计位姿点进行聚类，得到多个位姿簇；
针对每个位姿簇，按照匹配度从高到低的顺序，从该位姿簇中的估计位姿点选取多个估计位姿点作为候选位姿点。
在一种可能的实现方式中，根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点，具体包括：
将匹配度最高的待选位姿点作为所述目标位姿点；
或者，将匹配度较高的多个待选位姿点中距离所述初始位姿点最近的待选位姿点作为目标位姿点；
或者，将匹配度较高的多个待选位姿点的中心位姿点作为目标位姿点。
在一种可能的实现方式中，在接收到所述单帧激光点云之后，还包括：
按照预置的第一有效性判断条件，判断所述单帧激光点云是否有效；
若有效，则执行所述在所述地图中搜索出以初始位姿点为中心的第一区域的步骤。
在一种可能的实现方式中，所述第一有效性判断条件为以下任意一条或多条：
所述单帧激光点云中，距离值在所述激光雷达测距范围内的激光点云的数量大于或等于预置的第一数量阈值；
所述第一数量与单帧激光点云中所有激光点云总数的比值大于或等于预置的第一比值阈值。
在一种可能的实现方式中，根据所述多个待选位姿点确定所述车辆的目标位姿点之后，还包括：
判断所述目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。
本发明第二方面提供了一种计算机服务器，包括：存储器、处理器和收发器；
所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现如第一方面所述的定位初始化方法；
所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。
本发明第三方面提供了一种芯片系统，包括处理器，所述处理器与存储器的耦合，所述存储器存储有程序指令，当所述存储器存储的程序指令被所述处理器执行时实现如第一方面所述的定位初始化方法。
本发明第四方面提供了一种计算机系统，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如第一方面所述的定位初始化方法。
本发明第五方面提供了一种移动工具，包括上述第三方面所述的计算机服务器。
本发明第六方面提供了一种定位初始化装置，包括：
获取模块，用于获取车辆的初始位姿点和地图；
接收模块，用于接收车辆上激光雷达采集的单帧激光点云；
第一地图生成模块，用于在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
估计位姿点确定模块，用于根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
候选位姿点确定模块，用于确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；
待选位姿点确定模块，用于针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
目标位姿点确定模块，用于确定所述单帧激光点云分别与每个待选位姿点的匹配度，并根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。通过应用本发明实施例提供的定位初始化方法，一方面，通过采用全范围位置角度搜索及多尺度栅格激光匹配的方式，保证了搜索范围全面性和搜索结果准确有效性，搜索速度符合预期；另一方面，通过采用非线性优化方式精匹配激光点云及栅格地图，可提供更为准确的位姿，提高后续预测递推稳定性。因此，采用本发明技术方案，在不依赖于GNSS信号以及车辆精准停放的情况下，能够稳定有效、快速且准确进行定位初始化，以快速得到精准的目标位姿。另外，本申请不需要依赖昂贵的多线激光传感器，价格低廉，室内环境下稳定可靠，具有良好的产品化能力。
附图说明
图1为本发明实施例一提供的定位初始化方法流程示意图之一；
图2为图1步骤150的一个实现方式示意图；
图3为本发明实施例一提供的定位初始化方法流程示意图之二；
图4为本发明实施例一提供的定位初始化方法流程示意图之三；
图5为本发明实施例二提供的定位初始化装置的结构示意图之一；
图6为本发明实施例二提供的定位初始化装置的结构示意图之二；
图7为本发明实施例二提供的定位初始化装置的结构示意图之三。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
实施例一
图1为本发明实施例一提供定位初始化方法流程示意图之一，该初始化方法适用于安装有单线激光雷达的在室内的无人驾驶车辆。该方法的执行主体为处理器等具有运算能力的设备，比如，无人驾驶车辆的自动处理单元。如图1所示，本方法包括以下步骤：
步骤110，获取车辆的初始位姿点和地图；
具体的，所述地图可以是车辆当前已加载的地图，初始位姿点可以是用户通过车载终端输入得到。例如，车载终端可以包含触摸屏，触摸屏在对地图进行显示后，用户可以通过在触摸屏上进行操作输入初始位姿点。初始位姿点可以包括车辆位置信息和朝向信息。在一些实施例中，例如还可以是在用户终端（例如智能手机、PAD、遥控设备等）上输入初始位姿点，并将初始位姿点发送给车辆；例如，还可以是从云端服务器获取车辆的初始位姿点和加载地图。本申请不做严格限定。
步骤120，接收车辆上激光雷达采集的单帧激光点云；
具体的，激光雷达可以实时的获取激光点云，本申请可以针对激光点云进行逐帧处理。激光雷达可以是低线束激光雷达，例如单线激光雷达、四线激光雷达等。
步骤130，在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
具体的，可以以初始位姿点为中心，按照预置的尺寸范围，搜索出第一区域，例如以初始位姿点为中心点，按照设置的长方形尺寸或正方形尺寸得到以所述初始位姿点为中心点的长方形或正方形形状的第一区域；或者，例如，以初始位置点为圆心，按照预置的半径得到圆形形状的第一区域。当然第一区域的几何形状还可以是其他形状，本申请不做严格限定。例如，设定第一区域为长宽均为10米的正方形区域。假设，第一位置搜索分辨率是对第一区域进行搜索的分辨率，可以以0.2米为第一位置搜索分辨率对第一区域进行栅格化处理，从而，将第一区域划分为（
Figure 984965DEST_PATH_IMAGE001
）2,即2500个栅格。
步骤140，根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
具体的，为防止激光点云由于角度搜索不同反复进行三角函数的计算，可以采用激光预先旋转的方式，将激光预先旋转多种角度，后续可以直接将单帧激光点云和旋转后的角度进行匹配即可，大大减少了运算量。
继续接上例，可以进行全角度搜索，即以正负180°，第一搜索角度分辨率为0.5°进行搜索，得到
Figure 574078DEST_PATH_IMAGE002
=720个角度，共计得到720*2500=1800000个估计位姿点。
步骤150，确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点。
具体的，接续上例，将单帧激光点云分别与上述的1800000个估计位姿点进行匹配，并且可以计算单帧激光点云与每个估计位姿点的匹配度。
在一个示例中，如图2所示，所述确定所述单帧激光点云分别与每个估计位姿点的匹配度，具体包括：
针对每个估计位姿点，执行以下步骤1501~步骤1503，其中：
步骤1501，根据估计位姿点将所述单帧激光点云投影到所述地图中，得到激光点云中各激光点在所述地图中的投影点；
步骤1502，计算每个投影点与最近障碍物的距离值；
步骤1503，根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度。
其中，步骤1503具体包括：计算各投影点的距离值的和值或平均值，根据所述和值或平均值确定出对应的匹配度；和值或平均值越大对应的匹配度越小。
在一个具体实例中，此处的投影点，可以看作是高斯投影，通过高斯投影可以求取各投影点距离最近障碍物点的距离值。
示例而非限定，所述步骤150中，可以通过但不仅限于以下方式选取多个候选位姿点：
在一个示例中，按照匹配度从高到低的顺序，选取预置数量的估计位姿点作为候选位姿点。例如选取匹配度最高的前N个作为候选位姿点。
在另一个示例中，对所述多个估计位姿点进行聚类，得到多个位姿簇；针对每个位姿簇，按照匹配度从高到低的顺序，从该位姿簇中的估计位姿点选取多个估计位姿点作为候选位姿点。例如，包含M个位姿簇，从每个位姿簇中选取K个匹配度最高的候选位姿点，以得到候选位姿点。
此处的聚类，可以是通过聚类算法进行聚类，聚类算法包括但不限于K均值聚类、均值漂移聚类、基于密度的聚类、用高斯混合模型的最大期望聚类、凝聚层次聚类和图团体检测聚类，本申请不对聚类算法做严格限定。
在一个可选的实施例中，前述每个位姿簇具有匹配度，可以选取匹配度最高的几个位姿簇，比如，按照匹配度由大到小的顺序选前三个位姿簇，再分别从前三个位姿簇中选取5个估计位姿点作为候选位姿点，总计得到3*5=15个候选位姿点。此处通过聚类可减少误匹配导致的错误高分值集中问题。
步骤160，针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
具体的，针对每个候选位姿点，以该候选位姿点为中心，按照与步骤130类似的方法，搜索得到第二区域。此处的第二区域小于第一区域。例如，第二区域可以是0.2m*0.2m的区域。第二位置搜索分辨率可以是0.05m,从而得到的第二栅格地图可以是4*4=16的栅格，第二搜索角度分辨率为2度。继续接上例，候选位姿点的数量为15，则得到的待选位姿点的数量为16*（2/0.5）*15=960个。
步骤170，确定所述单帧激光点云分别与每个待选位姿点的匹配度；
具体的，可以与步骤150类似，计算单帧激光点云分别与每个待选位姿点的匹配度，在此不再赘述。
步骤180，根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。
在一个具体的实施例中，步骤180中，根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点，可以按照但不仅限于以下任意一种方式实现：
在一个实施例中，将匹配度最高的待选位姿点作为所述目标位姿点；
在另一个实施例中，将匹配度较高的多个待选位姿点中距离所述初始位姿点最近的待选位姿点作为目标位姿点；
在另一个实施例中，将匹配度较高的多个待选位姿点的中心位姿点作为目标位姿点。
可以理解的是，为了更进一步的提高定位初始化的准确度，还可以继续以小于第二栅格地图比例尺的第三栅格地图、第三区域、第三位置搜索分辨率和第三角度搜索分辨率执行步骤160-步骤180，当然，当后续继续执行时，所选取的区域面积逐次递减，以实现精度更高的初始化定位。本领域技术人员可以根据实际需求灵活选择多层不同比例尺的栅格地图进行操作。
进一步的优选地，在本申请一个优选的实现方案中，可以在执行步骤120之后，对单帧激光点云进行有效性判断，以减少初始化定位时的误定位的概率。
具体的，在接收到所述单帧激光点云之后，如图3所示，还包括：
步骤210，按照预置的第一有效性判断条件，判断所述单帧激光点云是否有效；
步骤220，若有效，则执行所述在所述地图中搜索出以初始位姿点为中心的第一区域的步骤。若无效则：重新选取下一帧激光点云执行步骤210；或者，从其他激光雷达选取一帧激光点云执行步骤210；或者，结束流程。
在一些可选的实施例中，所述第一有效性判断条件为以下任意一条或多条：
所述单帧激光点云中，距离值在所述激光雷达测距范围内的激光点云的数量大于或等于预置的第一数量阈值；
所述第一数量与单帧激光点云中所有激光点云总数的比值大于或等于预置的第一比值阈值。
其中，第一数量阈值和第一比值阈值都是多次实验的经验值，比如第一数量阈值可以是20，第一比值阈值可以是15%。具体数值，可以根据实际需求灵活设置，本申请不做严格限定。
进一步的，在本申请一个优选的实现方案中，根据所述多个待选位姿点确定所述车辆的目标位姿点之后，如图4所示，还包括：
步骤310，判断所述目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。
在一个可选的实施例中，步骤310中，确定目标位姿与单帧激光雷达点云的匹配度的方式，具体可采用前述步骤150中确定单帧激光点云分别与每个估计位姿点的匹配度的方式，在此不再赘述。
其中，匹配度阈值也是一个多次实验的经验值。
通过应用本发明实施例提供的定位初始化方法，一方面，通过采用全范围位置角度搜索及多尺度栅格激光匹配的方式，保证了搜索范围全面性和搜索结果准确有效性，搜索速度符合预期；另一方面，通过采用非线性优化方式精匹配激光点云及栅格地图，可提供更为准确的位姿，提高后续预测递推稳定性。因此，采用本发明技术方案，在不依赖于GNSS信号以及车辆精准停放的情况下，能够稳定有效、快速且准确进行定位初始化，以快速得到精准的目标位姿。另外，本申请不需要依赖昂贵的多线激光传感器，价格低廉，室内环境下稳定可靠，具有良好的产品化能力。
实施例二
本发明实施例二提供一种定位初始化装置，如图5所示，包括：
获取模块51，用于获取车辆的初始位姿点和地图；
接收模块52，用于接收车辆上激光雷达采集的单帧激光点云；
第一地图生成模块53，用于在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
估计位姿点确定模块54，用于根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
候选位姿点确定模块55，用于确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；
待选位姿点确定模块56，用于针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
目标位姿点确定模块57，用于确定所述单帧激光点云分别与每个待选位姿点的匹配度，并根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。
获取模块51与实施例一步骤110实现相同的功能，具体实现请参见实施例一对步骤110的相关描述，在此不再赘述。
第一地图生成模块53与实施例一步骤130实现相同功能，具体实现请参见实施例一对步骤130的相关描述，在此不再赘述。
估计位姿点确定模块54与实施例一步骤140实现相同功能，具体实现请参见实施例一对步骤140的相关描述，在此不再赘述。
候选位姿点确定模块55确定所述单帧激光点云分别与每个估计位姿点的匹配度，具体包括：针对每个估计位姿点，执行以下步骤：根据估计位姿点将所述单帧激光点云投影到所述地图中，得到激光点云中各激光点在所述地图中的投影点；计算每个投影点与最近障碍物的距离值；根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度。
前述根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度，具体包括：计算各投影点的距离值的和值或平均值，根据所述和值或平均值确定出对应的匹配度；和值或平均值越大对应的匹配度越小。
具体可参见实施例一中的步骤150，在此不再赘述。
候选位姿点确定模块55根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：
按照匹配度从高到低的顺序，选取预置数量的估计位姿点作为候选位姿点；
或者，
对所述多个估计位姿点进行聚类，得到多个位姿簇；针对每个位姿簇，按照匹配度从高到低的顺序，从该位姿簇中的估计位姿点选取多个估计位姿点作为候选位姿点。
在一个可选的实施例中，目标位姿点确定模块57根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点，具体包括：
将匹配度最高的待选位姿点作为所述目标位姿点；
或者，将匹配度较高的多个待选位姿点中距离所述初始位姿点最近的待选位姿点作为目标位姿点；
或者，将匹配度较高的多个待选位姿点的中心位姿点作为目标位姿点。
在一个可选的实施例中，前述图5所示的定位初始化装置，还可进一步包括有效性验证模块50，如图6所示：
有效性验证模块50，用于按照预置的第一有效性判断条件，判断所述接收模块接收到的单帧激光点云是否有效；若有效，则触发第一地图生成模块53。
有效性验证模块50具体实现可参见实施例一中的步骤210~步骤220，在此不再赘述。
在一个可选的实施例中，前述图5或图6所示的定位初始化装置，还可进一步包括有效性判断模块58，如图7所示为在图6所示的定位初始化装置中还可包括有限性判断模块58，其中：
有效性判断模块58，用于判断所述目标位姿确定模块确定的目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。
有效性判断模块58具体实现可参见实施例一中步骤310，在此不再赘述。
实施例三
本发明实施例三供了一种芯片系统，包括处理器，处理器与存储器的耦合，存储器存储有程序指令，当存储器存储的程序指令被处理器执行时实现如实施例一提供的任意一种定位初始化方法。
实施例四
本发明提供了一种计算机服务器，包括：存储器、处理器和收发器；
处理器用于与存储器耦合，读取并执行存储器中的指令，以实现上述实施例一提供的任意一种定位初始化方法；
收发器与处理器耦合，由处理器控制收发器进行消息收发。
实施例五
本发明实施例五提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现如实施例一提供的任意一种定位初始化方法。
实施例六
实施例六提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如实施例一提供的任意一种定位初始化方法。
实施例七
本发明实施例七提供了一种移动工具，包括上述的计算机服务器。
移动工具可以是任何可以移动的工具，例如车辆（例如吸尘车、清扫车、洗地车、物流小车、乘用车、环卫车、公交车、大巴车、厢式货车、卡车、载重车、挂车、甩挂车、吊车、挖掘机、铲土机、公路列车、扫地车、洒水车、垃圾车、工程车、救援车、AGV（Automated GuidedVehicle，自动导引运输车）等）、摩托车、自行车、三轮车、手推车、机器人、扫地机、平衡车等，本申请对于移动工具的类型不做严格限定，在此不再穷举。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器（RAM）、内存、只读存储器（ROM）、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (17)

1.一种定位初始化方法，其特征在于，包括：
获取车辆的初始位姿点和地图；
接收车辆上激光雷达采集的单帧激光点云；
在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；
针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
确定所述单帧激光点云分别与每个待选位姿点的匹配度；
根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。
2.根据权利要求1所述的方法，其特征在于，所述确定所述单帧激光点云分别与每个估计位姿点的匹配度，具体包括：
针对每个估计位姿点，执行以下步骤：
根据估计位姿点将所述单帧激光点云投影到所述地图中，得到激光点云中各激光点在所述地图中的投影点；
计算每个投影点与最近障碍物的距离值；
根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度。
3.根据权利要求2所述的方法，其特征在于，根据各投影点的距离值，确定出所述单帧激光点云与所述估计位姿点的匹配度，具体包括：
计算各投影点的距离值的和值或平均值，根据所述和值或平均值确定出对应的匹配度；和值或平均值越大对应的匹配度越小。
4.根据权利要求1所述的方法，其特征在于，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：
按照匹配度从高到低的顺序，选取预置数量的估计位姿点作为候选位姿点。
5.根据权利要求1所述的方法，其特征在于，根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点，具体包括：
对所述多个估计位姿点进行聚类，得到多个位姿簇；
针对每个位姿簇，按照匹配度从高到低的顺序，从该位姿簇中的估计位姿点选取多个估计位姿点作为候选位姿点。
6.根据权利要求1所述的方法，其特征在于，根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点，具体包括：
将匹配度最高的待选位姿点作为所述目标位姿点；
或者，将匹配度较高的多个待选位姿点中距离所述初始位姿点最近的待选位姿点作为目标位姿点；
或者，将匹配度较高的多个待选位姿点的中心位姿点作为目标位姿点。
7.根据权利要求1所述的方法，其特征在于，在接收到所述单帧激光点云之后，还包括：
按照预置的第一有效性判断条件，判断所述单帧激光点云是否有效；
若有效，则执行所述在所述地图中搜索出以初始位姿点为中心的第一区域的步骤。
8.根据权利要求7所述的方法，其特征在于，所述第一有效性判断条件为以下任意一条或多条：
所述单帧激光点云中，距离值在所述激光雷达测距范围内的激光点云的数量大于或等于预置的第一数量阈值；
所述第一数量与单帧激光点云中所有激光点云总数的比值大于或等于预置的第一比值阈值。
9.根据权利要求1所述的方法，其特征在于，根据所述多个待选位姿点确定所述车辆的目标位姿点之后，还包括：
判断所述目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。
10.一种定位初始化装置，其特征在于，包括：
获取模块，用于获取车辆的初始位姿点和地图；
接收模块，用于接收车辆上激光雷达采集的单帧激光点云；
第一地图生成模块，用于在所述地图中搜索出以初始位姿点为中心的第一区域，并按照第一位置搜索分辨率对所述第一区域进行栅格化处理得到第一栅格地图；
估计位姿点确定模块，用于根据第一栅格地图中各栅格的位置信息和预置的第一搜索角度分辨率，确定出多个估计位姿点；
候选位姿点确定模块，用于确定所述单帧激光点云分别与每个估计位姿点的匹配度；根据所述匹配度从所述多个估计位姿点中选取多个候选位姿点；
待选位姿点确定模块，用于针对每个候选位姿点，在所述地图中搜索出以候选位姿点为中心的第二区域，并按照第二位置搜索分辨率对所述第二区域进行栅格化处理得到第二栅格地图；根据第二栅格地图中各栅格的位置信息和预置的第二搜索角度分辨率，确定出多个待选位姿点；所述第二位置搜索分辨率小于第一位置搜索分辨率；
目标位姿点确定模块，用于确定所述单帧激光点云分别与每个待选位姿点的匹配度，并根据所述多个待选位姿点的匹配度确定所述车辆的目标位姿点。
11.根据权利要求10所述的装置，其特征在于，还包括：
有效性验证模块，用于按照预置的第一有效性判断条件，判断所述接收模块接收到的单帧激光点云是否有效；若有效，则触发第一地图生成模块。
12.根据权利要求10或11所述的装置，其特征在于，还包括：
有效性判断模块，用于判断所述目标位姿点确定模块确定的目标位姿与单帧激光雷达点云的匹配度是否大于预置的匹配度阈值，若是则确定所述目标位姿有效；若否则确定所述目标位姿无效。
13.一种计算机服务器，其特征在于，包括：存储器、处理器和收发器；
所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现权利要求1-9任一项所述的定位初始化方法；
所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。
14.一种芯片系统，其特征在于，包括处理器，所述处理器与存储器的耦合，所述存储器存储有程序指令，当所述存储器存储的程序指令被所述处理器执行时实现权利要求1-9任一项所述的定位初始化方法。
15.一种计算机系统，其特征在于，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如权利要求1-9任一项所述的定位初始化方法。
16.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行权利要求1-9任意一项所述的定位初始化方法。
17.一种移动工具，其特征在于，包括上述权利要求13所述的计算机服务器。
CN202110962408.7A 2021-08-20 2021-08-20 定位初始化方法、系统及移动工具 Pending CN113421306A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110962408.7A CN113421306A (zh) 2021-08-20 2021-08-20 定位初始化方法、系统及移动工具

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110962408.7A CN113421306A (zh) 2021-08-20 2021-08-20 定位初始化方法、系统及移动工具

Publications (1)

Publication Number Publication Date
CN113421306A true CN113421306A (zh) 2021-09-21

Family

ID=77719772

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110962408.7A Pending CN113421306A (zh) 2021-08-20 2021-08-20 定位初始化方法、系统及移动工具

Country Status (1)

Country Link
CN (1) CN113421306A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772894A (zh) * 2023-08-23 2023-09-19 小米汽车科技有限公司 定位初始化方法、装置、电子设备、车辆和存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180306922A1 (en) * 2017-04-20 2018-10-25 Baidu Online Network Technology (Beijing) Co., Ltd Method and apparatus for positioning vehicle
CN111596298A (zh) * 2020-05-13 2020-08-28 北京百度网讯科技有限公司 目标对象的定位方法、装置、设备及存储介质
CN111680747A (zh) * 2020-06-08 2020-09-18 北京百度网讯科技有限公司 用于占据栅格子图的闭环检测的方法和装置
CN112697169A (zh) * 2020-12-07 2021-04-23 北京小马智行科技有限公司 车辆定位的初始化方法、装置、处理器和车辆

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180306922A1 (en) * 2017-04-20 2018-10-25 Baidu Online Network Technology (Beijing) Co., Ltd Method and apparatus for positioning vehicle
CN111596298A (zh) * 2020-05-13 2020-08-28 北京百度网讯科技有限公司 目标对象的定位方法、装置、设备及存储介质
CN111680747A (zh) * 2020-06-08 2020-09-18 北京百度网讯科技有限公司 用于占据栅格子图的闭环检测的方法和装置
CN112697169A (zh) * 2020-12-07 2021-04-23 北京小马智行科技有限公司 车辆定位的初始化方法、装置、处理器和车辆

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116772894A (zh) * 2023-08-23 2023-09-19 小米汽车科技有限公司 定位初始化方法、装置、电子设备、车辆和存储介质
CN116772894B (zh) * 2023-08-23 2023-11-14 小米汽车科技有限公司 定位初始化方法、装置、电子设备、车辆和存储介质

Similar Documents

Publication Publication Date Title
US11651553B2 (en) Methods and systems for constructing map data using poisson surface reconstruction
US9298992B2 (en) Geographic feature-based localization with feature weighting
CN111192295B (zh) 目标检测与跟踪方法、设备和计算机可读存储介质
CN110632617B (zh) 一种激光雷达点云数据处理的方法及装置
CN110674705B (zh) 基于多线激光雷达的小型障碍物检测方法及装置
CN110889808B (zh) 一种定位的方法、装置、设备及存储介质
RU2720140C1 (ru) Способ оценки собственной позиции и устройство оценки собственной позиции
Li et al. High Resolution Radar-based Occupancy Grid Mapping and Free Space Detection.
US11668573B2 (en) Map selection for vehicle pose system
CN112505671B (zh) Gnss信号缺失环境下毫米波雷达目标定位方法及装置
CN112781599A (zh) 确定车辆的位置的方法
CN115461262A (zh) 利用面元地图的自主驾驶
CN114485698A (zh) 一种交叉路口引导线生成方法及系统
Slutsky et al. Dual inverse sensor model for radar occupancy grids
CN113421306A (zh) 定位初始化方法、系统及移动工具
Rabe et al. Ego-lane estimation for downtown lane-level navigation
CN112558035B (zh) 用于估计地面的方法和装置
CN114419573A (zh) 动态占据栅格估计方法及装置
US11747454B2 (en) Granularity-flexible existence-based object detection
CN115359332A (zh) 基于车路协同的数据融合方法、装置、电子设备及系统
CN112823353A (zh) 使用机器学习进行对象定位
US10532750B2 (en) Method, device and system for wrong-way driver detection
CN114694106A (zh) 道路检测区域的提取方法、装置、计算机设备和存储介质
JP2023533140A (ja) 自動運転車の位置推定精度を向上させる方法
US10876843B2 (en) Method, device and system for wrong-way driver detection

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096

Applicant after: Beijing Idriverplus Technology Co.,Ltd.

Address before: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096

Applicant before: Beijing Idriverplus Technology Co.,Ltd.

CB02 Change of applicant information