CN111680747A - 用于占据栅格子图的闭环检测的方法和装置 - Google Patents

用于占据栅格子图的闭环检测的方法和装置 Download PDF

Info

Publication number
CN111680747A
CN111680747A CN202010513248.3A CN202010513248A CN111680747A CN 111680747 A CN111680747 A CN 111680747A CN 202010513248 A CN202010513248 A CN 202010513248A CN 111680747 A CN111680747 A CN 111680747A
Authority
CN
China
Prior art keywords
pose
grid
node
resolution
graph
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.)
Granted
Application number
CN202010513248.3A
Other languages
English (en)
Other versions
CN111680747B (zh
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 Baidu Netcom Science and Technology Co Ltd
Original Assignee
Beijing Baidu Netcom Science and Technology 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 Baidu Netcom Science and Technology Co Ltd filed Critical Beijing Baidu Netcom Science and Technology Co Ltd
Priority to CN202010513248.3A priority Critical patent/CN111680747B/zh
Publication of CN111680747A publication Critical patent/CN111680747A/zh
Application granted granted Critical
Publication of CN111680747B publication Critical patent/CN111680747B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本公开的实施例公开了用于占据栅格子图的闭环检测方法和装置，涉及自动驾驶技术领域。该方法的一具体实施方式包括：获取当前扫描的激光点云数据和已建立的占据栅格子图；将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；根据至少一种分辨率的占据栅格子图构建树形结构，其中，树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。该实施方式能够有效减少闭环检测中搜索时间，进而减小累计误差。

Description

用于占据栅格子图的闭环检测的方法和装置
技术领域
本公开的实施例涉及自动驾驶技术领域，具体涉及用于占据栅格子图的闭环检测的方法和装置。
背景技术
在传统的激光SLAM(simultaneous localization and mapping，同步定位与建图)系统中，当前帧的位姿估计是依赖于之前帧的位姿估计，整个定位过程中误差会不断积累。这些误差在小尺度的运动过程中并不明显，但是随着运动距离的加长，累积的误差很快会达到一个较大的量级。因此通过闭合回环约束定位轨迹，有效消除累计误差。
在激光SLAM系统中，每传入一帧激光雷达扫描的数据，与最近建立的子图(Submap)去进行匹配，匹配方法是采用高斯牛顿解最小二乘优化问题，使这一帧的激光雷达扫描数据插入到当前子图上最优的位置，这个过程叫做扫描匹配。随着持续插入新的激光雷达扫描数据，子图得到了更新，当子图达到一定数量的激光雷达扫描数据后，不再有新的激光雷达扫描数据插入，则此子图已经建立完成。接着去创建下一个子图。通过创建大量的子图来实现大规模场景地图建立。通过扫描匹配得到子图的位姿估计在短时间内是准确可靠的，但长时间会存在累积误差，需要通过闭环检测(也称回环检测)来优化所有子图的位姿，减小累计误差。闭环检测每隔一段时间进行，所有创建完成的子图以及当前帧激光雷达扫描数据都会用作闭环检测的扫描匹配。一般来说，闭环检测的扫描匹配策略是根据当前帧激光雷达扫描数据，设定其附近子图邻域作为搜索空间。在搜索空间内寻找最优的匹配位姿，使得当前帧激光雷达扫描数据在该位姿下的得分最高(即与子图环境最相似)，此时找到闭环，这个匹配结果就会被用来作为位姿优化问题的闭环检测约束条件。然而，此闭环检测的扫描匹配策略是一种暴力的解法，即在当前激光扫描与其附近搜索空间范围内中的每一帧进行匹配计算得分值找出其中最大值作为匹配结果。这种匹配搜索效率低，耗时和计算量大。
发明内容
本公开的实施例提出了用于占据栅格子图的闭环检测的方法和装置。
第一方面，本公开的实施例提供了一种用于占据栅格子图的闭环检测的方法，包括：获取当前扫描的激光点云数据和已建立的占据栅格子图；将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；根据至少一种分辨率的占据栅格子图构建树形结构，其中，树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
在一些实施例中，该方法还包括：基于最佳位姿进行激光点云数据和已建立的占据栅格子图的拼接，得到优化后的位姿。
在一些实施例中，该方法还包括：获取与优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿；将至少一个候选位姿分别与激光点云数据进行一致性校验。
在一些实施例中，该方法还包括：根据校验成功的位姿对已建立的占据栅格子图加入图优化。
在一些实施例中，该方法还包括：获取当前车辆的GPS信息；根据GPS信息对已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。
在一些实施例中，逐层计算各节点表征的位姿与激光点云数据的匹配度，包括：对于每个低分辨率栅格，将组成该低分辨率栅格的至少一个高分辨率栅格的占据栅格概率的最大值作为该低分辨率栅格的占据栅格概率；基于各分辨率占据栅格子图的占据栅格概率采用分支定界法和深度优选搜索法逐层计算各节点表征的位姿与激光点云数据的匹配度。
第二方面，本公开的实施例提供了一种用于占据栅格子图的闭环检测的装置，包括：获取单元，被配置成获取当前扫描的激光点云数据和已建立的占据栅格子图；转换单元，被配置成将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；构建单元，被配置成根据至少一种分辨率的占据栅格子图构建树形结构，其中，树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；搜索单元，被配置成从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
在一些实施例中，该装置还包括位姿优化单元，被配置成：基于最佳位姿进行激光点云数据和已建立的占据栅格子图的拼接，得到优化后的位姿。
在一些实施例中，该装置还包括校验单元，被配置成：获取与优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿；将至少一个候选位姿分别与激光点云数据进行一致性校验。
在一些实施例中，该装置还包括图优化单元，被配置成：根据校验成功的位姿对已建立的占据栅格子图加入图优化。
在一些实施例中，该装置还包括裁剪单元，被配置成：获取当前车辆的GPS信息；根据GPS信息对已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。
在一些实施例中，搜索单元进一步被配置成：对于每个低分辨率栅格，将组成该低分辨率栅格的至少一个高分辨率栅格的占据栅格概率的最大值作为该低分辨率栅格的占据栅格概率；基于各分辨率占据栅格子图的占据栅格概率采用分支定界法和深度优选搜索法逐层计算各节点表征的位姿与激光点云数据的匹配度。
第三方面，本公开的实施例提供了一种用于占据栅格子图的闭环检测的电子设备，包括：一个或多个处理器；存储装置，其上存储有一个或多个程序，当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器实现如第一方面中任一的方法。
第四方面，本公开的实施例提供了一种计算机可读介质，其上存储有计算机程序，其中，程序被处理器执行时实现如第一方面中任一的方法。
本公开的实施例提供的用于占据栅格子图的闭环检测的方法和装置，通过设置多分辨率的占据栅格子图，并根据不同分辨率下的搜索空间建立树形结构。不再进行暴力检测，而是沿着树的节点采用分支定界搜索算法和深度优先搜索算法，可以有效地剪树，从而有效加快闭环检测中搜索的速度，进而减小累计误差，实现大规模场景地图的建立。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本公开的其它特征、目的和优点将会变得更明显：
图1是本公开的一个实施例可以应用于其中的示例性系统架构图；
图2是根据本公开的用于占据栅格子图的闭环检测的方法的一个实施例的流程图；
图3a、3b是根据本公开的用于占据栅格子图的闭环检测的方法的一个应用场景的示意图；
图4是根据本公开的用于占据栅格子图的闭环检测的方法的又一个实施例的流程图；
图5是根据本公开的用于占据栅格子图的闭环检测的装置的一个实施例的结构示意图；
图6是适于用来实现本公开的实施例的电子设备的计算机系统的结构示意图。
具体实施方式
下面结合附图和实施例对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
图1示出了可以应用本申请的用于占据栅格子图的闭环检测的方法或用于占据栅格子图的闭环检测的装置的实施例的示例性系统架构100。
如图1所示，系统架构100可以包括无人车101。
无人车101中可以安装有驾驶控制设备1011、网络1012和激光雷达1013。网络1012用以在驾驶控制设备1011和激光雷达1013之间提供通信链路的介质。网络1012可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
驾驶控制设备(又称为车载大脑)1011负责无人车101的智能控制。驾驶控制设备1011可以是单独设置的控制器，例如可编程逻辑控制器(Programmable LogicController，PLC)、单片机、工业控制机等；也可以是由其他具有输入/输出端口，并具有运算控制功能的电子器件组成的设备；还可以是安装有车辆驾驶控制类应用的计算机设备。
需要说明的是，实践中无人车101中还可以安装有至少一个传感器，例如，摄像机、重力传感器、轮速传感器等。某些情况下，无人车101中还可以安装有GNSS(GlobalNavigation Satellite System，全球导航卫星系统)设备和SINS(Strap-down InertialNavigation System，捷联惯性导航系统)等等。
需要说明的是，本申请实施例所提供的用于占据栅格子图的闭环检测的方法一般由驾驶控制设备1011执行，相应地，用于占据栅格子图的闭环检测的装置一般设置于驾驶控制设备1011中。
应该理解，图1中的驾驶控制设备、网络和激光雷达的数目仅仅是示意性的。根据实现需要，可以具有任意数目的驾驶控制设备、网络和激光雷达。
继续参考图2，其示出了根据本申请的用于占据栅格子图的闭环检测的方法的一个实施例的流程200。该用于占据栅格子图的闭环检测的方法，包括以下步骤：
步骤201，获取当前扫描的激光点云数据和已建立的占据栅格子图。
在本实施例中，在无人车行驶过程中，无人车上安装的各种车载传感器(例如，激光雷达、照相机、毫米波雷达等)可以实时采集外界环境信息，激光雷达生成激光点云数据传输给用于占据栅格子图的闭环检测的方法运行于其上的电子设备(例如，图1所示的驾驶控制设备)。上述电子设备可以对所接收的各种传感器数据进行分析处理，以对车辆周围环境进行感知，来构建子图。通过创建大量的子图来实现大规模场景地图建立,通过扫描匹配得到子图的位姿估计在短时间内是准确可靠的，但长时间会存在累积误差，需要通过闭环检测来优化所有子图的位姿，减小累计误差。
占据栅格子图(occupied grid submap)是一种栅格地图，栅格中的值通常为概率值，用于表征栅格被障碍物占据的概率。每个栅格是一个正方体，栅格的边长与分辨率有关，分辨率越高则边长越小。
步骤202，将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图。
在本实施例中，原始的已建立的占据栅格子图的分辨率最高，例如，边长是5cm。将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图。实际是生成多种低分辨率的占据栅格子图。例如，将原始占据栅格子图边长为5cm的栅格转换为10cm的栅格，则降低了分辨率。每种分辨率对应一种搜索窗(包括平移距离、旋转角度)，分辨率越低，则每次搜索时移动的范围越大。例如，对于边长为5cm的栅格进行搜索时，每次平移5cm或旋转1度，对于边长为10cm的栅格进行搜索时，每次平移10cm或旋转2度。如图3a所示，为了便于理解，仅给出鸟瞰栅格的示例。301代表原始分辨率的占据栅格子图。302为原始栅格边长扩大一倍(分辨率降低一倍)后的占据栅格子图。303为原始栅格边长扩大两倍(分辨率降低两倍)后的占据栅格子图。304为原始栅格边长扩大三倍(分辨率降低三倍)后的占据栅格子图。
步骤203，根据至少一种分辨率的占据栅格子图构建树形结构。
在本实施例中，在SLAM中，所谓的位姿其实指的是相机在世界坐标系中的位姿。位姿包括两方面：位置和姿势，即三维坐标和朝向。树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿，即坐标(x,y,z)和角度的离散搜索空间的解。图3b所示为局部占据栅格子图的树形结构。每个叶子节点表示原始分辨率下的一种位姿。叶子节点这一层对应于图3a所示的301。根节点对应于图3a所示的304。
已建立的占据栅格子图包括了原始分辨率下每个栅格的占据概率。转换成低分辨率的占据栅格子图后，低分辨率的占据栅格子图中每个栅格的占据概率是多个子分辨率栅格概率值中的最大值。例如，栅格302由4个原始分辨率的栅格301组成，占据栅格的概率分别为0.4，0.6，0.8，0.2。则占据栅格302的概率为0.8。可根据原始分辨率预计算出各种分辨率下占据栅格的概率。
可选地，获取当前车辆的GPS信息；根据GPS信息对已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。仅保留已建立的占据栅格子图中在GPS信息预定范围内的子图。
步骤204，从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
在本实施例中，构建好树形结构后，从根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度。匹配度是通过当前扫描的激光点云数据投影到栅格地图中，将所有命中点的所在栅格概率值相加得到的。从根节点开始向下搜索，同一层中如果某个节点的匹配度高，则优先从该节点进入下一层节点的搜索。如图3b所示，303层有4个节点，分别按照A、B、C、D，4个节点对应的位姿将当前扫描的激光点云数据投影到已建立的占据栅格子图中，命中的栅格的概率值相加即为该位姿对应的节点的匹配度，即得分。假设A节点的匹配度比B节点、C节点、D节点都高。则从A节点的子节点开始下一层(302层)的搜索。分别按照A1、A2、A3、A4，4节点对应的位姿将当前扫描的激光点云数据投影到已建立的占据栅格子图中，命中的栅格的概率值相加即为该位姿对应的节点的匹配度，即得分。假设A1节点的匹配度比A节点的其它子节点都高。则从A1节点的子节点开始下一层(301层)的搜索。在301层的叶子节点中找到匹配度最高的叶子节点，该叶子节点对应位姿即为最佳位姿。
为了加快搜索速度，可对树形结构进行剪枝。例如，图3a所示，304可分为四块。可尝试用粗粒度的位姿，例如4种xy值不同的粗粒度的位姿，使得用这4个位姿调整激光点云数据后，投影分别位于这四块区域。如果其中某块区域的匹配度最高，则在该区域对应的位姿范围内进一步搜索细粒度的位姿，直到搜到该区域对应的位姿的叶子节点，得到最佳分数，再对另三块区域的位姿搜索，如果另三块区域的匹配度均小于最佳分数，则放弃另三块区域的查找。否则，继续沿高于最佳分数的区域对应的子树查找大于所述最佳分数的节点，直到找到所有节点中匹配度最高的叶子节点。从而提高了搜索速度。
可采用分支定界法+深度优先搜索去寻找最优位姿，具体如下：
(a)初始化best_score＝score_threshold(一般根据经验值设定，或者直接设置为一个非常小的值。只有在最开始根节点才设置)；初始化一个栈C，栈C存储当前层树形解空间的初始节点和上一层未处理的节点，栈C中初始节点存储顺序按照得分(匹配度)值排序，得分值最大的排在栈顶。后续节点的选择采用深度优先搜索确定。
(b)如果栈C不为空，弹出当前栈顶节点X，计算节点X得分值score(X)(投影后命中的栅格的概率之和),如果score(X)<best_score,则回到步骤(b),否则进行如下判断：
(i)如果当前节点X为叶子结点，存储当前的得分值和节点，即best_score＝score(X)，match＝X,则回到步骤(b)。
(ii)如果当前节点X不是叶子结点，则分裂当前节点X获得子节点，并压入栈，入栈规则同步骤(a)(子节点按得分排序放在栈顶),则回到步骤(b)。
(c)如果栈C为空，则退出算法，返回best_score(最佳位姿的得分)和match(最佳位姿)。
进一步参考图4，其示出了用于占据栅格子图的闭环检测的方法的又一个实施例的流程400。该用于占据栅格子图的闭环检测的方法的流程400，包括以下步骤：
步骤401，获取当前扫描的激光点云数据和已建立的占据栅格子图。
步骤402，将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图。
步骤403，根据至少一种分辨率的占据栅格子图构建树形结构。
步骤404，从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
步骤401-404与步骤201-204基本相同，因此不再赘述。
步骤405，基于最佳位姿进行激光点云数据和已建立的占据栅格子图的拼接，得到优化后的位姿。
在本实施例中，可通过现有的拼接算法例如ICP(Iterative Closest Point，最近点搜索法)进行激光点云数据和已建立的占据栅格子图的拼接，将搜索最佳位姿作为测量点，搜索其对应的就近点点集合，实现曲线拟合，从而将离散的最佳位姿转换成优化后的连续的位姿。
步骤406，获取与优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿，将至少一个候选位姿分别与激光点云数据进行一致性校验。
在本实施例中，闭环检测结果一般具有一致性，即当前候选周围存在多个能够匹配上闭环结果，利用该特性对闭环检测结果进行检查。例如，将优化后的位姿稍微旋转一定角度作为候选位姿，再投影到已建立的占据栅格子图，计算匹配度，如果匹配度与最佳位姿的匹配度之差小于预定值，则认为候选位姿合格，如果合格的候选位姿的数量超过预定值，则该优化后的位姿通过一致性校验，可用于图优化。否则，需要重新搜索最佳位姿。可移动车辆后重新扫描激光点云数据，重新进行闭环检测。也可以加入匹配到submap某一位置附近的几帧点云进行拼接，做一致性校验。
步骤407，根据校验成功的位姿对已建立的占据栅格子图加入图优化。
在本实施例中，当发现了闭环信息(校验成功的位姿)后，SLAM可使用BundleAdjuestment(BA)等算法对原先的位姿拓扑地图进行修正(即加入图优化)，从而能有效的进行闭环后地图的修正，实现更加可靠的环境建图。
进一步参考图5，作为对上述各图所示方法的实现，本公开提供了一种用于占据栅格子图的闭环检测的装置的一个实施例，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
如图5所示，本实施例的用于占据栅格子图的闭环检测的装置500包括：获取单元501、转换单元502、构建单元503、搜索单元504。其中，获取单元501，被配置成获取当前扫描的激光点云数据和已建立的占据栅格子图；转换单元502，被配置成将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；构建单元503，被配置成根据至少一种分辨率的占据栅格子图构建树形结构，其中，树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；搜索单元504，被配置成从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
在本实施例中，用于占据栅格子图的闭环检测的装置500的获取单元501、转换单元502、构建单元503、搜索单元504的具体处理可以参考图2对应实施例中的步骤201、步骤202、步骤203、步骤204。
在本实施例的一些可选的实现方式中，装置500还包括位姿优化单元(附图中未示出)，被配置成：基于最佳位姿进行激光点云数据和已建立的占据栅格子图的拼接，得到优化后的位姿。
在本实施例的一些可选的实现方式中，装置500还包括校验单元(附图中未示出)，被配置成：获取与优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿；将至少一个候选位姿分别与激光点云数据进行一致性校验。
在本实施例的一些可选的实现方式中，装置500还包括图优化单元(附图中未示出)，被配置成：根据校验成功的位姿对已建立的占据栅格子图加入图优化。
在本实施例的一些可选的实现方式中，装置500还包括裁剪单元(附图中未示出)，被配置成：获取当前车辆的GPS信息；根据GPS信息对已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。
在本实施例的一些可选的实现方式中，搜索单元504进一步被配置成：对于每个低分辨率栅格，将组成该低分辨率栅格的至少一个高分辨率栅格的占据栅格概率的最大值作为该低分辨率栅格的占据栅格概率；基于各分辨率占据栅格子图的占据栅格概率采用分支定界法和深度优选搜索法逐层计算各节点表征的位姿与激光点云数据的匹配度。
下面参考图6，其示出了适于用来实现本公开的实施例的电子设备(例如图1中的驾驶控制设备)600的结构示意图。图6示出的驾驶控制设备仅仅是一个示例，不应对本公开的实施例的功能和使用范围带来任何限制。
如图6所示，电子设备600可以包括处理装置(例如中央处理器、图形处理器等)601，其可以根据存储在只读存储器(ROM)602中的程序或者从存储装置608加载到随机访问存储器(RAM)603中的程序而执行各种适当的动作和处理。在RAM 603中，还存储有电子设备600操作所需的各种程序和数据。处理装置601、ROM 602以及RAM 603通过总线604彼此相连。输入/输出(I/O)接口605也连接至总线604。
通常，以下装置可以连接至I/O接口605：包括例如触摸屏、触摸板、键盘、鼠标、摄像头、麦克风、加速度计、陀螺仪等的输入装置606；包括例如液晶显示器(LCD)、扬声器、振动器等的输出装置607；包括例如磁带、硬盘等的存储装置608；以及通信装置609。通信装置609可以允许电子设备600与其他设备进行无线或有线通信以交换数据。虽然图6示出了具有各种装置的电子设备600，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。图6中示出的每个方框可以代表一个装置，也可以根据需要代表多个装置。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置609从网络上被下载和安装，或者从存储装置608被安装，或者从ROM 602被安装。在该计算机程序被处理装置601执行时，执行本公开的实施例的方法中限定的上述功能。需要说明的是，本公开的实施例所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开的实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开的实施例中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：该方法的一具体实施方式包括：获取当前扫描的激光点云数据和已建立的占据栅格子图；将已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；根据至少一种分辨率的占据栅格子图构建树形结构，其中，树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；从树形结构的根节点开始，逐层计算各节点表征的位姿与激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
可以以一种或多种程序设计语言或其组合来编写用于执行本公开的实施例的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开的实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元也可以设置在处理器中，例如，可以描述为：一种处理器包括获取单元、转换单元、构建单元、搜索单元。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定，例如，获取单元还可以被描述为“获取当前扫描的激光点云数据和已建立的占据栅格子图的单元”。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (14)

1.一种用于占据栅格子图的闭环检测的方法，包括：
获取当前扫描的激光点云数据和已建立的占据栅格子图；
将所述已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；
根据所述至少一种分辨率的占据栅格子图构建树形结构，其中，所述树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；
从所述树形结构的根节点开始，逐层计算各节点表征的位姿与所述激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
2.根据权利要求1所述的方法，其中，所述方法还包括：
基于所述最佳位姿进行所述激光点云数据和所述已建立的占据栅格子图的拼接，得到优化后的位姿。
3.根据权利要求2所述的方法，其中，所述方法还包括：
获取与所述优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿；
将所述至少一个候选位姿分别与所述激光点云数据进行一致性校验。
4.根据权利要求3所述的方法，其中，所述方法还包括：
根据校验成功的位姿对所述已建立的占据栅格子图加入图优化。
5.根据权利要求1所述的方法，其中，所述方法还包括：
获取当前车辆的GPS信息；
根据所述GPS信息对所述已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。
6.根据权利要求1-5之一所述的方法，其中，所述逐层计算各节点表征的位姿与所述激光点云数据的匹配度，包括：
对于每个低分辨率栅格，将组成该低分辨率栅格的至少一个高分辨率栅格的占据栅格概率的最大值作为该低分辨率栅格的占据栅格概率；
基于各分辨率占据栅格子图的占据栅格概率采用分支定界法和深度优选搜索法逐层计算各节点表征的位姿与所述激光点云数据的匹配度。
7.一种用于占据栅格子图的闭环检测的装置，包括：
获取单元，被配置成获取当前扫描的激光点云数据和已建立的占据栅格子图；
转换单元，被配置成将所述已建立的占据栅格子图转换成至少一种分辨率的占据栅格子图；
构建单元，被配置成根据所述至少一种分辨率的占据栅格子图构建树形结构，其中，所述树形结构中的每层节点表征一种分辨率下的位姿搜索空间，分辨率从根节点向下递增，每个节点表征一种位姿；
搜索单元，被配置成从所述树形结构的根节点开始，逐层计算各节点表征的位姿与所述激光点云数据的匹配度，匹配度高的节点优先进入下一层节点继续计算匹配度，直到在叶子节点匹配得到最佳位姿。
8.根据权利要求7所述的装置，其中，所述装置还包括位姿优化单元，被配置成：
基于所述最佳位姿进行所述激光点云数据和所述已建立的占据栅格子图的拼接，得到优化后的位姿。
9.根据权利要求8所述的装置，其中，所述装置还包括校验单元，被配置成：
获取与所述优化后的位姿在位姿搜索空间中相邻的至少一个候选位姿；
将所述至少一个候选位姿分别与所述激光点云数据进行一致性校验。
10.根据权利要求9所述的装置，其中，所述装置还包括图优化单元，被配置成：
根据校验成功的位姿对所述已建立的占据栅格子图加入图优化。
11.根据权利要求7所述的装置，其中，所述装置还包括裁剪单元，被配置成：
获取当前车辆的GPS信息；
根据所述GPS信息对所述已建立的占据栅格子图进行裁剪，以缩小位姿搜索空间。
12.根据权利要求7-11之一所述的装置，其中，所述搜索单元进一步被配置成：
对于每个低分辨率栅格，将组成该低分辨率栅格的至少一个高分辨率栅格的占据栅格概率的最大值作为该低分辨率栅格的占据栅格概率；
基于各分辨率占据栅格子图的占据栅格概率采用分支定界法和深度优选搜索法逐层计算各节点表征的位姿与所述激光点云数据的匹配度。
13.一种用于占据栅格子图的闭环检测的电子设备，包括：
一个或多个处理器；
存储装置，其上存储有一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的方法。
14.一种计算机可读介质，其上存储有计算机程序，其中，所述程序被处理器执行时实现如权利要求1-6中任一所述的方法。
CN202010513248.3A 2020-06-08 2020-06-08 用于占据栅格子图的闭环检测的方法和装置 Active CN111680747B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010513248.3A CN111680747B (zh) 2020-06-08 2020-06-08 用于占据栅格子图的闭环检测的方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010513248.3A CN111680747B (zh) 2020-06-08 2020-06-08 用于占据栅格子图的闭环检测的方法和装置

Publications (2)

Publication Number Publication Date
CN111680747A true CN111680747A (zh) 2020-09-18
CN111680747B CN111680747B (zh) 2023-09-01

Family

ID=72454026

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010513248.3A Active CN111680747B (zh) 2020-06-08 2020-06-08 用于占据栅格子图的闭环检测的方法和装置

Country Status (1)

Country Link
CN (1) CN111680747B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112700497A (zh) * 2021-01-04 2021-04-23 歌尔股份有限公司 回环检测方法、装置、设备及计算机可读存储介质
CN112883290A (zh) * 2021-01-12 2021-06-01 中山大学 一种基于分支定界法的自动切图方法
CN113421306A (zh) * 2021-08-20 2021-09-21 北京智行者科技有限公司 定位初始化方法、系统及移动工具
CN113763551A (zh) * 2021-09-08 2021-12-07 北京易航远智科技有限公司 一种基于点云的大规模建图场景的快速重定位方法
CN114593737A (zh) * 2022-03-11 2022-06-07 美智纵横科技有限责任公司 控制方法、装置、机器人及存储介质
CN115685223A (zh) * 2022-12-15 2023-02-03 深圳市智绘科技有限公司 位置识别方法、装置、电子设备及可读存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102855663A (zh) * 2012-05-04 2013-01-02 北京建筑工程学院 一种根据激光雷达栅格点云构建csg模型的方法
CN106921978A (zh) * 2015-12-25 2017-07-04 中国移动通信集团北京有限公司 一种位置分布确定方法及装置
CN106991143A (zh) * 2017-03-22 2017-07-28 苏州中科图新网络科技有限公司 多层级图像文件、生成方法及装置、读取方法及装置
CN107038749A (zh) * 2016-02-03 2017-08-11 北京八亿时空信息工程有限公司 多分辨率三维建模方法及建模装置
CN207373179U (zh) * 2017-10-26 2018-05-18 常熟理工学院 一种用于slam及导航的机器人控制系统
CN110244716A (zh) * 2019-05-28 2019-09-17 苏州大学 一种基于波前算法的机器人探索的方法
CN110515382A (zh) * 2019-08-28 2019-11-29 锐捷网络股份有限公司 一种智能设备及其定位方法
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN110907947A (zh) * 2019-12-04 2020-03-24 同济人工智能研究院(苏州)有限公司 一种移动机器人slam问题中的实时回环检测方法
CN110930452A (zh) * 2019-10-23 2020-03-27 同济大学 一种基于自监督学习与模板匹配的物体位姿估计方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102855663A (zh) * 2012-05-04 2013-01-02 北京建筑工程学院 一种根据激光雷达栅格点云构建csg模型的方法
CN106921978A (zh) * 2015-12-25 2017-07-04 中国移动通信集团北京有限公司 一种位置分布确定方法及装置
CN107038749A (zh) * 2016-02-03 2017-08-11 北京八亿时空信息工程有限公司 多分辨率三维建模方法及建模装置
CN106991143A (zh) * 2017-03-22 2017-07-28 苏州中科图新网络科技有限公司 多层级图像文件、生成方法及装置、读取方法及装置
CN207373179U (zh) * 2017-10-26 2018-05-18 常熟理工学院 一种用于slam及导航的机器人控制系统
CN110244716A (zh) * 2019-05-28 2019-09-17 苏州大学 一种基于波前算法的机器人探索的方法
CN110530368A (zh) * 2019-08-22 2019-12-03 浙江大华技术股份有限公司 一种机器人定位方法及设备
CN110515382A (zh) * 2019-08-28 2019-11-29 锐捷网络股份有限公司 一种智能设备及其定位方法
CN110930452A (zh) * 2019-10-23 2020-03-27 同济大学 一种基于自监督学习与模板匹配的物体位姿估计方法
CN110907947A (zh) * 2019-12-04 2020-03-24 同济人工智能研究院(苏州)有限公司 一种移动机器人slam问题中的实时回环检测方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
BING-JUI HO等: "Virtual Occupancy Grid Map for Submap-based Pose Graph SLAM and Planning in 3D Environments", 《2018 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 *
刘清旺等: "机载激光雷达和高光谱组合系统的亚热带森林估测遥感试验", 《高技术通讯》 *
华剑锋等: "基于变分辨率栅格模型的启发式有向搜索最优路径算法", 《浙江大学学报(理学版)》 *
杨晓宇等: "用四叉树算法建立自主系统环境模型", 《计算机仿真》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112700497A (zh) * 2021-01-04 2021-04-23 歌尔股份有限公司 回环检测方法、装置、设备及计算机可读存储介质
WO2022142744A1 (zh) * 2021-01-04 2022-07-07 歌尔股份有限公司 回环检测方法、装置、设备及计算机可读存储介质
CN112883290A (zh) * 2021-01-12 2021-06-01 中山大学 一种基于分支定界法的自动切图方法
CN112883290B (zh) * 2021-01-12 2023-06-27 中山大学 一种基于分支定界法的自动切图方法
CN113421306A (zh) * 2021-08-20 2021-09-21 北京智行者科技有限公司 定位初始化方法、系统及移动工具
CN113763551A (zh) * 2021-09-08 2021-12-07 北京易航远智科技有限公司 一种基于点云的大规模建图场景的快速重定位方法
CN113763551B (zh) * 2021-09-08 2023-10-27 北京易航远智科技有限公司 一种基于点云的大规模建图场景的快速重定位方法
CN114593737A (zh) * 2022-03-11 2022-06-07 美智纵横科技有限责任公司 控制方法、装置、机器人及存储介质
CN115685223A (zh) * 2022-12-15 2023-02-03 深圳市智绘科技有限公司 位置识别方法、装置、电子设备及可读存储介质

Also Published As

Publication number Publication date
CN111680747B (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN111680747B (zh) 用于占据栅格子图的闭环检测的方法和装置
CN109214248B (zh) 用于识别无人驾驶车辆的激光点云数据的方法和装置
US11320836B2 (en) Algorithm and infrastructure for robust and efficient vehicle localization
JP6771059B2 (ja) ３次元シーンを再構成するための方法、装置、機器及びコンピュータ可読記憶媒体
US11594011B2 (en) Deep learning-based feature extraction for LiDAR localization of autonomous driving vehicles
US10613227B2 (en) Method and apparatus for positioning vehicle
US11531110B2 (en) LiDAR localization using 3D CNN network for solution inference in autonomous driving vehicles
CN110954113B (zh) 一种车辆位姿的修正方法和装置
EP3361278B1 (en) Autonomous vehicle localization based on walsh kernel projection technique
US11364931B2 (en) Lidar localization using RNN and LSTM for temporal smoothness in autonomous driving vehicles
CN108280866B (zh) 道路点云数据处理方法及系统
CN111857160A (zh) 一种无人车路径规划方法和装置
CN109410735B (zh) 反射值地图构建方法和装置
CN112086010B (zh) 地图生成方法、装置、设备及存储介质
CN109407073A (zh) 反射值地图构建方法和装置
US20230042968A1 (en) High-definition map creation method and device, and electronic device
CN114459471B (zh) 定位信息确定方法、装置、电子设备及存储介质
KR102118357B1 (ko) 모바일 맵핑 또는 자율 주행용 플랫폼과 관측 데이터의 구조화를 위한 시스템
CN115540896A (zh) 路径规划方法、装置、电子设备和计算机可读介质
CN113325389A (zh) 一种无人车激光雷达定位方法、系统及存储介质
CN111507161B (zh) 利用合并网络进行异质传感器融合的方法和装置
CN112622923B (zh) 用于控制车辆的方法和装置
CN111461980B (zh) 点云拼接算法的性能估计方法和装置
CN111461982B (zh) 用于拼接点云的方法和装置
CN116385825B (zh) 模型联合训练方法、装置及车辆

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
GR01 Patent grant
GR01 Patent grant