CN117031481A - 一种基于投影3d激光点云的移动机器人重定位方法及系统 - Google Patents
一种基于投影3d激光点云的移动机器人重定位方法及系统 Download PDFInfo
- Publication number
- CN117031481A CN117031481A CN202311017158.5A CN202311017158A CN117031481A CN 117031481 A CN117031481 A CN 117031481A CN 202311017158 A CN202311017158 A CN 202311017158A CN 117031481 A CN117031481 A CN 117031481A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- map
- grid map
- projection
- robot
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000013138 pruning Methods 0.000 claims description 11
- 238000012216 screening Methods 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000005516 engineering process Methods 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- DMSMPAJRVJJAGA-UHFFFAOYSA-N benzo[d]isothiazol-3-one Chemical compound C1=CC=C2C(=O)NSC2=C1 DMSMPAJRVJJAGA-UHFFFAOYSA-N 0.000 claims description 4
- 238000012545 processing Methods 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 13
- 238000004422 calculation algorithm Methods 0.000 description 10
- 230000007613 environmental effect Effects 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 239000002245 particle Substances 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Classifications
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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/20—Instruments for performing navigational calculations
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明公开了一种基于投影3D激光点云的移动机器人重定位方法及系统，实现机器人在上电初始化或者发生“劫持”需要进行定位时，仅依靠自身搭载的3D激光雷达传感器，在预先建立的环境点云地图中，不需要人工干预，全自动快速地完成机器人的重定位；通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，获取的当前激光雷达数据进行投影，对2D投影图和多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果。
Description
技术领域
本发明涉及移动机器人领域，尤其涉及一种基于投影3D激光点云的移动机器人重定位方法及系统。
背景技术
基于视觉SLAM的重定位技术：利用视觉图像数据建立历史关键帧数据库，使用基于词袋或者深度学习描述子的方式，在历史帧中搜索和当前图像最相似的图像，然后利用PnP计算当前机器人在地图中的位姿，实现机器人的重定位；
基于激光雷达粒子滤波的重定位技术：预先建立场景的点云地图，利用粒子滤波算法，在地图中搜索和当前激光点云帧最相似匹配的位姿，完成机器人的重定位；
基于全局定位传感器的重定位技术：利用GNSS、UWB等全局定位传感器，在机器人上或者实际场景中增加额外的传感器，获取机器人在运行场景中的绝对位姿，将其作为机器人位姿搜索的初始值，实现机器人的重定位。
现有技术的缺点
基于视觉的方式，图像易受光照变化、低纹理等环境因素干扰，导致重定位的鲁棒性较差；同时图像的处理对硬件处理器的要求较高，消耗较大的运算资源，在低算力的设备上运行压力较大；
基于粒子滤波的技术，一般需要人工手动指定机器人在地图中的大致的位姿，提供较好的搜索初始值，不然算法的收敛性较差；同时在大场景中，算法的耗时较长；
基于全局定位传感器的重定位技术，增加额外的传感器将会提高机器人的成本，并且可能会存在传感器的结构安装限制，运行现场部署全局定位传感器困难；同时GNSS、UWB等全局定位传感器在室内易受遮挡物等干扰，导致传感器信号弱，定位精度差。。
发明内容
本发明的目的在于提供一种基于投影3D激光点云的移动机器人重定位方法及系统，解决了现有技术中指出的上述技术问题。
本发明提供了一种基于投影3D激光点云的移动机器人重定位方法，包括如下操作步骤：
初始化获取多分辨率栅格地图；
在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿。
较佳的，所述初始化获取多分辨率栅格地图，包括如下操作步骤：
通过机器人对当前实际场景进行扫描，并利用SLAM技术获取当前所述实际场景对应的全局3D点云地图；
对所述全局3D点云地图通过点云投影获取多分辨率栅格地图。
较佳的，所述全局3D点云地图包括多个点云数据、各个所述点云数据对应的点云坐标以及各个所述点云数据对应的机器人位姿。
较佳的，所述对所述全局3D点云地图通过点云投影获取多分辨率栅格地图，包括如下操作步骤：
根据机器人高度范围，从所述全局3D点云地图中截取获得多个滤波后的点云数据；
建立初始多分辨率栅格地图；获取所述初始多分辨率栅格地图的所有分辨率resolution；根据各个所述滤波后的点云数据对应的点云坐标，计算获取各个所述分辨率下的栅格地图的大小image(width,height)；
将各个所述滤波后的点云数据Pk投影到所述初始多分辨率栅格地图中得到多分辨率栅格地图，分别对应获取投影点；并根据所述初始对分辨率栅格地图的多个分辨率resolution分别计算获取各个所述投影点的投影坐标P；
判断所述投影点的投影坐标是否有相同；若是，则根据统计获取相同投影坐标的数量Q及预设的相同投影数量最大阈值λ进行筛选获取多个目标投影点，并将所述目标投影点的灰度值设置为0。
较佳的，所述栅格地图的大小image(width,height)的计算方式为：
image(width,height)＝[(xmax-xmin),(ymax-ymin)]/resolution；
式中，(xmax,xmin,ymax,ymin)表示滤波后的点云投影到多分辨率栅格地图上的边界大小；
所述投影坐标P的计算方式为：
式中，为投影坐标；resolution为投影点的分辨率；
x，y，z为滤波后的点云数据对应的点云坐标；xmin为滤波后的点云投影到多分辨率栅格地图上的边界大小；ymax为滤波后的点云投影到多分辨率栅格地图上的边界大小。
较佳的，所述通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿，包括如下操作步骤：
将所述3D点云数据投影到所述多分辨率栅格地图中，得到点云栅格图；基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'；
基于所有所述候选位姿T'，从所述多分辨率栅格地图中通过分枝界定搜索操作获取初始位姿T”；
将所述初始位姿T”恢复为三维位姿T；将所述三维位姿T通过IPC点云配准操作得到配准位姿。
较佳的，所述基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'，包括如下操作步骤：
获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图；从所述目标点云栅格图中提取多个第一线特征；并从所述目标分辨率栅格地图中提取多个第二线特征；
对所述第一线特征构建LBD线段描述符，得到第一LBD线段描述符；对所述第二线特征构建LBD线段描述符，得到第二LBD线段描述符；
将各个所述第一LBD线段描述符与各个所述第二LBD线段描述符分别进行匹配得到多个待确定匹配对；计算获取各个所述待确定匹配对中的第一LBD线段描述符与第二LBD线段描述符的线段描述符相似度K；通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对；
分别根据各个所述目标匹配对进行计算获取各个所述目标匹配对分别对应的候选位姿T'。
较佳的，所述通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对，包括如下操作步骤：
预设线段描述符相似度最高阈值K1；
遍历所有所述线段描述符相似度K，判断所述线段描述符相似度K是否大于或等于所述线段描述符相似度最高阈值K1；若是，则确定所述线段描述符对应的待确定匹配对为目标匹配对；
较佳的，所述基于所述点云栅格图及所述多分辨率栅格图，获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图，包括如下操作步骤：
确定目标分辨率；
基于所述目标分辨率，从所述点云栅格图中获取目标点云栅格图；
基于所述目标分辨率，从所述多分辨率栅格地图中获取目标分辨率栅格地图。
较佳的，所述基于所有所述候选位姿T'，通过分枝界定搜索操作获取初始位姿T”，包括如下操作步骤：
根据所有所述候选位姿T'，获取搜索空间W；
根据候选位姿T'，获取各个所述3D点云数据的初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)；
建立搜索树并获取所述搜索树的所有搜索节点；依次遍历所有所述候选位姿T'；将所述候选位姿T'作为所述搜索树的第一层的第一节点；基于所述初始灰度值Imap(x,y)及投影灰度值Icurrent(x,y)，计算获取所述第一节点的评分score；
对所述第一节点进行展开，得到多个第二节点；并计算各个所述第二节点对应的评分score；并将所有所述第二节点中最后一个第二节点的评分score作为初始下边界；重复上述操作，直至所述节点达到第N节点；
根据各个所述搜索节点及所述搜索节点对应的评分score，对所述搜索树进行剪枝操作，得到多个待确定目标节点；
根据各个所述待确定目标节点对应的评分score，选择其中评分最高的一个待确定目标节点作为目标节点；并确定所述目标节点对应的候选位姿为初始位姿。
较佳的，所述搜索空间W为所有候选位姿在所述多分辨率栅格地图中共同构成的搜索范围；
所述初始灰度值Imap(x,y)为所述3D点云数据采集获取时的灰度值；所述投影灰度值Icurrent(x,y)为所述3D点云数据投影后的灰度值。
较佳的，所述第一节点的评分score的计算方式为：
式中，T'＝[x y yaw]T'为候选位姿；Imap(x1,y1)为初始灰度值；Icurrent(x2,y2)为投影灰度值；W为搜索空间；x，y为候选位姿中的坐标；u，v为搜索空间中的像素坐标；
相应地，本发明还提出了一种基于投影3D激光点云的移动机器人重定位系统，包括建图模块、重定位模块；
其中，所述建图模块，用于初始化获取多分辨率栅格地图；
所述重定位模块，用于在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿。
与现有技术相比，本发明实施例至少存在如下方面的技术优势：
分析本发明提供的上述一种基于投影3D激光点云的移动机器人重定位方法及系统可知，在具体应用时，通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，获取的当前激光雷达数据进行投影，对2D投影图和多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果；
该系统算法消耗的硬件算力小，由于3D激光雷达数据受环境的干扰较小，该方法对环境的适应性较强，同时算法鲁棒性强、重定位精度高、效率高。不需要在机器人上搭载额外的全局定位传感器，部署成本低。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法的整体操作步骤示意图；
图2为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法的整体操作流程模拟示意图；
图3为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中获取多分辨率栅格地图的操作步骤示意图；
图4为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中多分辨率栅格地图的详细操作步骤示意图；
图5为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中获取配准位姿的操作步骤示意图；
图6为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中获取多个候选位姿的操作步骤示意图；
图7为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中得到多个目标匹配对的操作步骤示意图；
图8为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中获取目标点云栅格图及目标分辨率栅格地图的操作步骤示意图；
图9为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中获取初始位姿的操作步骤示意图；
图10为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位方法中剪枝操作的模拟示意图；
图11为本发明实施例一提供的一种基于投影3D激光点云的移动机器人重定位系统的整体架构示意图。
附图标记：建图模块10；重定位模块20；点云地图建图模块11；投影模块12；候选位姿获取模块21；初始位姿获取模块22；配准位姿获取模块23。
具体实施方式
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面通过具体的实施例并结合附图对本发明做进一步的详细描述。
实施例一
如图1所示，本发明提出了一种基于投影3D激光点云的移动机器人重定位方法，包括如下操作步骤：
步骤S10：初始化获取多分辨率栅格地图；
步骤S20：在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿；
解释说明：控制机器人在当前实际场景中运动，并实时判断当前所述机器人是否需要重定位，上述判断当前机器人是否需要重定位的原则是若当前机器人处于电初始化或者发生“劫持”状态时，则确定当前机器人需要重定位操作。
如图2所示，本发明实施例首先通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，获取的当前激光雷达数据进行投影得到点云栅格图(点云栅格图即为2D投影图)，对2D投影图和多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果。
具体地，如图3所示，在步骤S10中，初始化获取多分辨率栅格地图，包括如下操作步骤：
步骤S11：通过机器人对当前实际场景(即对当前机器人周围实际空间上的物体)进行扫描，并利用SLAM技术获取当前所述实际场景对应的全局3D点云地图；
步骤S12：对所述全局3D点云地图通过点云投影获取多分辨率栅格地图；
所述全局3D点云地图包括多个点云数据、各个所述点云数据对应的点云坐标以及各个所述点云数据对应的机器人位姿(位姿即位置及方向)；
需要说明的是，上述本申请实施例中的多分辨率栅格地图是机器人重定位操作之前的初始操作，上述SLAM技术是现有技术，是首先利用机器人对实际场景得到的原始点云数据，然后对原始点云数据通过SLAM技术进行建图得到实际场景对应的全局3D点云地图；其中，原始点云数据指的是通过控制机器人在实际场景中按照固定的路线运动，通过激光雷达传感器获取环境数据(即原始点云数据)；然后对激光雷达的原始点云数据进行处理，包括：点云畸变去除、点云滤波、点云特征提取操作；在构建全局3D点云地图之后，进而就可以通过激光雷达里程计(即激光雷达里程计是识别分析前、后时刻的点云数据)对每一时刻获取的激光雷达点云数据和已经建立的地图进行匹配(匹配指的是初始建立地图时将第一时刻获取得到的激光雷达点云数据放入地图中，随后使用第二时刻获取得到的激光雷达点云数据与第一时刻的激光雷达点云数据进行匹配)，获得各个时刻点云数据对应的机器人位姿；进而进行点云地图构建，根据计算的机器人位姿，将每一时刻的点云数据进行拼接，获得全局3D点云地图。
通过全局3D点云地图可建立获取多分辨率栅格地图，保障机器人在后续需要进行重定位操作时可以不需要人工设置，即可自动完成重定位操作，提高工作效率。
具体地，如图4所示，在步骤S12中，对所述全局3D点云地图通过点云投影获取多分辨率栅格地图，包括如下操作步骤：
步骤S121：获取机器人高度范围；根据所述机器人高度范围，通过点云直通滤波器从所述全局3D点云地图中截取获得多个滤波后的点云数据；
需要说明的是，上述滤波后的点云指的是将机器人的高度范围输入到点云直通滤波器中，进行截取获取得到的多个点云数据(即上述滤波后的点云数据)；
步骤S122：建立初始多分辨率栅格地图；获取所述初始多分辨率栅格地图的所有分辨率resolution；根据各个所述滤波后的点云数据对应的点云坐标，计算获取各个所述分辨率下的栅格地图的大小image(width,height)；
所述栅格地图的大小image(width,height)的计算方式为：
image(width,height)＝[(xmax-xmin),(ymax-ymin)]/resolution；
式中，(xmax,xmin,ymax,ymin)表示滤波后的点云投影到多分辨率栅格地图上的边界大小；
需要说明的是，上述本申请实施例中的初始多分辨率栅格地图是指在栅格地图中使用不同的分辨率来表示环境信息；在多分辨率栅格地图中，地图可以被分成多个区域或者层级，每个区域或者层级具有不同的分辨率；通常会根据环境的特性和需要来确定每个区域的分辨率；其中，高分辨率区域通常用于描述重要的细节和复杂的目标，这些区域可能需要更精确的信息以支持机器人的决策和导航；而低分辨率区域则用于描述相对简单的区域或者远离机器人感兴趣的区域，这样可以降低计算复杂度并提高效率；
在初始多分辨率栅格地图中，每个像素代表实际距离，如0.05、0.1、0.2、0.4(单位m)；以0.05m为例，若初始多分辨率栅格地图中的分辨率为0.05m/像素，那么每个像素就代表实际世界中的0.05米。如果一个物体在地图上占据了1个像素，那么它在实际世界中的大小就是0.05m；这种映射关系使得我们可以在栅格地图中以像素为基本单位来描述和处理环境信息；
步骤S123：将各个所述滤波后的点云数据Pk投影到所述初始多分辨率栅格地图中得到多分辨率栅格地图；获取所述多分辨率栅格地图上所有所述滤波后的点云数据分别对应的所有投影点；并根据所述初始对分辨率栅格地图的多个分辨率resolution分别计算获取各个所述投影点的投影坐标P；
所述投影坐标P的计算方式为：
式中，为投影坐标(即上述P)；resolution为投影点的分辨率；
x，y，z为滤波后的点云数据对应的点云坐标；xmin为滤波后的点云投影到多分辨率栅格地图上的边界大小(即为x轴最小值)；ymax为滤波后的点云投影到多分辨率栅格地图上的边界大小(即为y轴最大值)；
步骤S124：判断所述投影点的投影坐标是否有相同；若是，则获取相同投影坐标的数量Q；
步骤S125：预设相同投影数量最大阈值λ；判断所述相同投影坐标的数量Q是否大于或等于所述相同投影数量最大阈值λ；若是，确定所述投影点为目标投影点；将所述目标投影点的灰度值设置为0(并将所述多分辨率地图中除目标投影点之外的所有像素点的灰度值设置为255)；
需要说明的是，上述相同投影数量最大阈值λ通常为5，是根据经验值设置的，如果这个阈值设置过大将会导致点云投影下来保留的环境信息不够充分，如果设置过小将会导致投影后的栅格地图中会存在较多的噪声；
上述将目标投影点的灰度值设置为0指的是将目标投影点对应的像素点的灰度值设置为0，即为设置为黑色；将多分辨率地图中除目标投影点之外的所有像素点的灰度值设置为255，即为设置为白色。
具体地，如图5所示，在步骤S20中，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿，包括如下操作步骤：
步骤S21：获取当前机器人的当前帧3D点云数据；将所述3D点云数据投影到所述多分辨率栅格地图中，得到点云栅格图(即2D投影图)；基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'；
步骤S22：基于所有所述候选位姿T'，从所述多分辨率栅格地图中通过分枝界定搜索操作获取初始位姿T”；
步骤S23：将所述初始位姿T”恢复为三维位姿T；将所述三维位姿T通过IPC点云配准操作得到配准位姿。
需要说明的是，上述将初始位姿恢复为三维位姿的方式是通过进行计算获取得到的；
其中，zlidar表示雷达在机器人上的安装高度；yaw为初始位姿T”；x1，y1为初始位姿T”对应的3D点云数据的坐标；
上述将三维位姿T通过IPC点云配准操作得到配准位姿是通过将三维位姿T作为IPC点云配准的初始值，进行迭代优化，获得当前3D点云帧和全局点云地图最佳的配准位姿T*＝R*|t*]，R*＝VUT，其中/>分别为全局3D点云地图和当前3D点云帧中的三维点坐标，U,V分别为对矩阵/>进行SVD分解的左右正交矩阵。通过ICP点云配准的结果就是最终的机器人重定位结果；上述IPC点云配准为现有技术，本申请实施例不再是赘述。
上述本申请实施例所采用的技术方案，该系统方案可以实现机器人在上电初始化或者发生“劫持”需要进行定位时，仅依靠自身搭载的3D激光雷达传感器，在预先建立的环境点云地图中，不需要人工干预，全自动快速地完成机器人的重定位；通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，获取的当前激光雷达数据进行投影，对2D投影图和多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果；
该本申请实施例所采用的算法消耗的硬件算力小，由于3D激光雷达数据受环境的干扰较小，该方法对环境的适应性较强，同时算法鲁棒性强、重定位精度高、效率高；不需要在机器人上搭载额外的全局定位传感器，部署成本低。
具体地，如图6所示，在步骤S21中，基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'，包括如下操作步骤：
步骤S211：基于所述点云栅格图及所述多分辨率栅格图，获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图；从所述目标点云栅格图中提取多个第一线特征；并从所述目标分辨率栅格地图中提取多个第二线特征；
需要说明的是，通过提取点云栅格图与多分辨率栅格图的线特征，可以实现机器人在地图中的精确定位；例如，将机器人当前感知到的线特征与预先构建的地图线特征进行匹配，可以推断机器人在地图中的位置和方向；这有助于提高机器人的定位精度，并为导航和路径规划等任务提供准确的参考。
步骤S212：对所述第一线特征构建LBD线段描述符，得到第一LBD线段描述符；对所述第二线特征构建LBD线段描述符，得到第二LBD线段描述符；
需要说明的是，上述LBD描述符是一种紧凑的二进制编码，其中BDi为一个条带描述符，把提取的线段的所有条带描述符组合到一起就构建出这条线段的LBD描述符；具有较小的存储空间和计算复杂度；对第一线特征与第二线特征进行构建LBD线段描述符，具有较强的特征表达和鉴别能力；通过计算两个线特征的LBD描述符之间的相似度，可以实现线特征的匹配；这有助于在不同图像或栅格地图中找到相对应的线特征，从而进行地图匹配、目标识别、姿态估计等任务；并且可使匹配计算效果具有更强的鲁棒性。
步骤S213：将各个所述第一LBD线段描述符与各个所述第二LBD线段描述符分别进行匹配得到多个待确定匹配对；计算获取各个所述待确定匹配对中的第一LBD线段描述符与第二LBD线段描述符的线段描述符相似度K；通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对；
步骤S214：分别根据各个所述目标匹配对进行计算获取各个所述目标匹配对分别对应的候选位姿T'(上述候选位姿即指的是点云栅格图在多分辨率栅格地图上的位姿)。
需要说明的是，上述本申请实施例中计算获取各个目标匹配对对应的候选位姿是通过计算线段之间的相对位移和旋转角度；进而基于匹配线段对之间的相对位移和旋转角度，可以采用一些算法(如最小二乘法、ICP等)来估计整体的位姿变换；最后，根据估计的位姿变换，得到机器人空间中的位姿(即候选位姿)；上述计算获取候选位姿的方式为现有技术，本申请实施例不再赘述。
上述本申请实施例中，因为机器人在实际场景的运动过程中发送重定位操作时，会因为实际场景中的各个区域的场景可能相似，进而导致发生重定位操作时当前帧获取得到的3D点云数据与初始建图时的多帧原始点云数据相同；因此，在机器人依靠多分辨率栅格地图进行重定位时，机器人的当前位姿可能会是多个；则此时，需要根据机器人当前帧的3D点云数据投影得到点云栅格图后与多分辨率栅格地图进行匹配得到当前机器人可能存在的多种位姿(即上述多个候选位姿)；
上述通过图像线特征匹配操作有助于提高机器人的定位精度；通过进行线特征匹配，机器人可以更快进行重定位操作，实现精确定位和鲁棒的导航。
具体地，如图7所示，在步骤S213中，所述通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对，包括如下操作步骤：
步骤S2131：预设线段描述符相似度最高阈值K1；
步骤S2132：遍历所有所述线段描述符相似度K，判断所述线段描述符相似度K是否大于或等于所述线段描述符相似度最高阈值K1；若是，则确定所述线段描述符对应的待确定匹配对为目标匹配对(重复上述步骤S231-步骤S232，获取多个目标匹配对，直至所有线段描述符相似度K被遍历结束)；
具体地，如图8所示，在步骤S211中，基于所述点云栅格图及所述多分辨率栅格图，获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图，包括如下操作步骤：
步骤S2111：确定目标分辨率；
步骤S2112：基于所述目标分辨率，从所述点云栅格图中获取目标点云栅格图；
步骤S2113：基于所述目标分辨率，从所述多分辨率栅格地图中获取目标分辨率栅格地图；
具体地，如图9所示，在步骤S22中，基于所有所述候选位姿T'，通过分枝界定搜索操作获取初始位姿T”，包括如下操作步骤：
步骤S221：根据所有所述候选位姿T'，获取搜索空间W(所述搜索空间W为所有候选位姿在所述多分辨率栅格地图中共同构成的搜索范围)；
步骤S222：根据候选位姿T'，获取各个所述3D点云数据的初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)；
所述初始灰度值Imap(x1,y1)为所述3D点云数据采集获取时的灰度值(即为3D点云数据投影到多分辨率栅格地图得到点云栅格图之前的灰度值)；所述投影灰度值Icurrent(x2,y2)为所述3D点云数据投影后的灰度值(即为3D点云数据投影到多分辨率栅格地图得到点云栅格图之后的灰度值)；
步骤S223：建立搜索树并获取所述搜索树的所有搜索节点(所述搜索树包括多层，即第一层、第二层、......、第N层；所述搜索节点包括第一节点、第二节点、......、第N节点)；依次遍历所有所述候选位姿T'；将所述候选位姿T'作为所述搜索树的第一层的第一节点；基于所述初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)，计算获取所述第一节点的评分score；
所述第一节点的评分score的计算方式为：
式中，T'＝[xyyaw]T'为候选位姿；Imap(x1,y1)为初始灰度值；Icurrent(x2,y2)为投影灰度值；W为搜索空间；x，y为候选位姿中的坐标；u，v为搜索空间中的像素坐标(或称多分辨率栅格地图中的像素坐标)；
需要说明的是，上述本申请实施例中，为了加速初始位姿的搜索，采用分支定界的搜索过程。通过图像线特征匹配获得在最低分辨率栅格地图的搜索空间，依次遍历求解所有的可行解(所有候选位姿)，构成搜索树第一层的所有第一节点，然后使用公式计算获取各个第一节点的评分；进而根据各个第一节点的评分进行分支后继续进行搜索最终的初始位姿的操作，具体操作如下所示。
步骤S224：对所述第一节点进行展开，得到多个第二节点；并计算各个所述第二节点对应的评分score；并将所有所述第二节点中最后一个第二节点的评分score作为初始下边界(初始下边界在后续最优解搜索中可以进行剪枝，加快搜索速度)；重复上述操作，直至所述节点达到第N节点(第N节点即为搜索树的最后一层的所有节点)；
所述第二节点为3D点云数据在多分辨率栅格地图中较高分辨率中的候选位姿；
需要说明的是，上述初始下边界为第二层对应的第二节点的下边界，用于对第二节点进行剪枝操作，随后还会有第二下边界、第三下边界等。上述本申请实施例是对每个节点依次展开，求解其子节点(即第二节点)即在下层较高分辨率地图展开的可行解，每个分支展开到最后一层，得到在最高分辨率栅格地图上的位姿Tξ及评分score，此时将这个分支最后一层解的评分作为初始下边界(有了下边界，在后续最优解搜索中可以进行剪枝，加快搜索速度)；然后逐层返回展开求解并打分，如果当前解的评分小于下边界，将此节点及下层分支进行剪枝。如图10所示，其中◇为被剪枝的节点。如果其他分支能展开到最后一层，并且解的评分大于下边界则更新下边界。通过这样的迭代剪枝方法，遍历所有的叶子节点，以较高效率完成在高分辨率栅格地图上的初始位姿搜索。
步骤S225：根据各个所述搜索节点及所述搜索节点对应的评分score，对所述搜索树进行剪枝操作(剪枝操作指的是若评分小于当前层对应的下边界，则对该评分对应的节点进行筛除操作)，得到多个待确定目标节点；
步骤S226：根据各个所述待确定目标节点对应的评分score，选择其中评分最高的一个待确定目标节点作为目标节点；并确定所述目标节点对应的候选位姿为初始位姿。
需要说明的是，上述本申请实施例所采用的技术方案，利用分支定界通过特定的评分方式，并通过迭代剪枝方法，遍历所有的叶子节点，以较高效率完成在高分辨率栅格地图上的初始位姿搜索。
综上，本发明提供的上述一种基于投影3D激光点云的移动机器人重定位方法，通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，对当前帧的3D点云数据进行投影得到点云栅格图；进而通过点云栅格图与多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该初始位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果；可以实现机器人在上电初始化或者发生“劫持”需要进行定位时，仅依靠机器人自身搭载的3D激光雷达传感器，在预先建立的环境点云地图中，不需要人工干预，全自动快速地完成机器人的重定位。
实施例二
如图11所示，相应的，本发明还提出了一种基于投影3D激光点云的移动机器人重定位系统，包括建图模块10、重定位模块20；
其中，所述建图模块10，用于初始化获取多分辨率栅格地图；
所述重定位模块20，用于在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿。
较佳的，所述建图模块10，包括点云地图建图模块11、投影模块12；
其中，所述点云地图建图模块11，用于通过机器人对当前实际场景进行扫描，并利用SLAM技术获取当前所述实际场景对应的全局3D点云地图；
所述投影模块12，用于对所述全局3D点云地图通过点云投影获取多分辨率栅格地图。
较佳的，所述全局3D点云地图包括多个点云数据、各个所述点云数据对应的点云坐标以及各个所述点云数据对应的机器人位姿；
较佳的，所述投影模块，具体用于获取机器人高度范围；根据所述机器人高度范围，通过点云直通滤波器从所述全局3D点云地图中截取获得多个滤波后的点云数据；
建立初始多分辨率栅格地图；获取所述初始多分辨率栅格地图的所有分辨率resolution；根据各个所述滤波后的点云数据对应的点云坐标，计算获取各个所述分辨率下的栅格地图的大小image(width,height)；
所述栅格地图的大小image(width,height)的计算方式为：
image(width,height)＝[(xmax-xmin),(ymax-ymin)]/resolution；
式中，(xmax,xmin,ymax,ymin)表示滤波后的点云投影到多分辨率栅格地图上的边界大小；
将各个所述滤波后的点云数据Pk投影到所述初始多分辨率栅格地图中得到多分辨率栅格地图；获取所述多分辨率栅格地图上所有所述滤波后的点云数据分别对应的所有投影点；并根据所述初始对分辨率栅格地图的多个分辨率resolution分别计算获取各个所述投影点的投影坐标P；
所述投影坐标P的计算方式为：
式中为投影坐标(即上述P)；resolution为投影点的分辨率；
x，y，z为滤波后的点云数据对应的点云坐标；xmin为滤波后的点云投影到多分辨率栅格地图上的边界大小(即为x轴最小值)；ymax为滤波后的点云投影到多分辨率栅格地图上的边界大小(即为y轴最大值)；
判断所述投影点的投影坐标是否有相同；若是，则获取相同投影坐标的数量Q；
预设相同投影数量最大阈值λ；判断所述相同投影坐标的数量Q是否大于或等于所述相同投影数量最大阈值λ；若是，确定所述投影点为目标投影点；将所述目标投影点的灰度值设置为0。
较佳的，所述重定位模块20，包括候选位姿获取模块21、初始位姿获取模块22、配准位姿获取模块23；
其中，所述候选位姿获取模块21，用于获取当前机器人的当前帧3D点云数据；将所述3D点云数据投影到所述多分辨率栅格地图中，得到点云栅格图；基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'；
所述初始位姿获取模块22，用于基于所有所述候选位姿T'，从所述多分辨率栅格地图中通过分枝界定搜索操作获取初始位姿T”；
所述配准位姿获取模块23，用于将所述初始位姿T”恢复为三维位姿T；将所述三维位姿T通过IPC点云配准操作得到配准位姿。
较佳的，所述候选位姿获取模块21，具体用于基于所述点云栅格图及所述多分辨率栅格图，获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图；从所述目标点云栅格图中提取多个第一线特征；并从所述目标分辨率栅格地图中提取多个第二线特征；
对所述第一线特征构建LBD线段描述符，得到第一LBD线段描述符；对所述第二线特征构建LBD线段描述符，得到第二LBD线段描述符；
将各个所述第一LBD线段描述符与各个所述第二LBD线段描述符分别进行匹配得到多个待确定匹配对；计算获取各个所述待确定匹配对中的第一LBD线段描述符与第二LBD线段描述符的线段描述符相似度K；通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对；
分别根据各个所述目标匹配对进行计算获取各个所述目标匹配对分别对应的候选位姿T'。
较佳的，所述候选位姿获取模块21，在具体实施过程中，还用于预设线段描述符相似度最高阈值K1；
遍历所有所述线段描述符相似度K，判断所述线段描述符相似度K是否大于或等于所述线段描述符相似度最高阈值K1；若是，则确定所述线段描述符对应的待确定匹配对为目标匹配对；
较佳的，所述候选位姿获取模块21，在具体实施过程中，还用于确定目标分辨率；
基于所述目标分辨率，从所述点云栅格图中获取目标点云栅格图；
基于所述目标分辨率，从所述多分辨率栅格地图中获取目标分辨率栅格地图；
较佳的，所述初始位姿获取模块22，在具体实施过程中，用于根据所有所述候选位姿T'，获取搜索空间W(所述搜索空间W为所有候选位姿在所述多分辨率栅格地图中共同构成的搜索范围)；
根据候选位姿T'，获取各个所述3D点云数据的初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)；
所述初始灰度值Imap(x1,y1)为所述3D点云数据采集获取时的灰度值(即为3D点云数据投影到多分辨率栅格地图得到点云栅格图之前的灰度值)；所述投影灰度值Icurrent(x2,y2)为所述3D点云数据投影后的灰度值(即为3D点云数据投影到多分辨率栅格地图得到点云栅格图之后的灰度值)；
建立搜索树并获取所述搜索树的所有搜索节点；依次遍历所有所述候选位姿T'；将所述候选位姿T'作为所述搜索树的第一层的第一节点；基于所述初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)，计算获取所述第一节点的评分score；
所述第一节点的评分score的计算方式为：
式中，T'＝[x y yaw]T'为候选位姿；Imap(x1,y1)为初始灰度值；Icurrent(x2,y2)为投影灰度值；W为搜索空间；x，y为候选位姿中的坐标；u，v为搜索空间中的像素坐标(或称多分辨率栅格地图中的像素坐标)；
对所述第一节点进行展开，得到多个第二节点；并计算各个所述第二节点对应的评分score；并将所有所述第二节点中最后一个第二节点的评分score作为初始下边界(初始下边界在后续最优解搜索中可以进行剪枝，加快搜索速度)；重复上述操作，直至所述节点达到第N节点(第N节点即为搜索树的最后一层的所有节点)；
所述第二节点为3D点云数据在多分辨率栅格地图中较高分辨率中的候选位姿；
根据各个所述搜索节点及所述搜索节点对应的评分score，对所述搜索树进行剪枝操作(剪枝操作指的是若评分小于当前层对应的下边界，则对该评分对应的节点进行筛除操作)，得到多个待确定目标节点；
根据各个所述待确定目标节点对应的评分score，选择其中评分最高的一个待确定目标节点作为目标节点；并确定所述目标节点对应的候选位姿为初始位姿。
综上所述，本发明实例提出的一种基于投影3D激光点云的移动机器人重定位方法及系统，通过激光SLAM预先建立环境的3D点云地图，对其进行投影处理得到多分辨率栅格地图；在机器人需要进行重定位时，对当前帧的3D点云数据进行投影得到点云栅格图；进而通过点云栅格图与多分辨率栅格地图进行线特征匹配和分支定界搜索，以较高的效率完成机器人的初始位姿搜索；然后将该初始位姿作为ICP点云配准的初始值，提高配准的精度和速度，获取最终的机器人重定位结果；可以实现机器人在上电初始化或者发生“劫持”需要进行定位时，仅依靠机器人自身搭载的3D激光雷达传感器，在预先建立的环境点云地图中，不需要人工干预，全自动快速地完成机器人的重定位。
该系统算法消耗的硬件算力小，由于3D激光雷达数据受环境的干扰较小，该方法对环境的适应性较强，同时算法鲁棒性强、重定位精度高、效率高。不需要在机器人上搭载额外的全局定位传感器，部署成本低。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；本领域的普通技术人员可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (10)
1.一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，包括如下操作步骤：
初始化获取多分辨率栅格地图；
在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿。
2.根据权利要求1所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述初始化获取多分辨率栅格地图，包括如下操作步骤：
通过机器人对当前实际场景进行扫描，并利用SLAM技术获取当前所述实际场景对应的全局3D点云地图；
对所述全局3D点云地图通过点云投影获取多分辨率栅格地图；
所述全局3D点云地图包括多个点云数据、各个所述点云数据对应的点云坐标以及各个所述点云数据对应的机器人位姿。
3.根据权利要求2所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述对所述全局3D点云地图通过点云投影获取多分辨率栅格地图，包括如下操作步骤：
根据机器人高度范围，从所述全局3D点云地图中截取获得多个滤波后的点云数据；
建立初始多分辨率栅格地图；获取所述初始多分辨率栅格地图的所有分辨率resolution；根据各个所述滤波后的点云数据对应的点云坐标，计算获取各个所述分辨率下的栅格地图的大小image(width,height)；
将各个所述滤波后的点云数据Pk投影到所述初始多分辨率栅格地图中得到多分辨率栅格地图，分别对应获取投影点；并根据所述初始对分辨率栅格地图的多个分辨率resolution分别计算获取各个所述投影点的投影坐标P；
判断所述投影点的投影坐标是否有相同；若是，则根据统计获取相同投影坐标的数量Q及预设的相同投影数量最大阈值λ进行筛选获取多个目标投影点，并将所述目标投影点的灰度值设置为0；
所述栅格地图的大小image(width,height)的计算方式为：
image(width,height)＝[(xmax-xmin),(ymax-ymin)]/resolution；
式中，(xmax,xmin,ymax,ymin)表示滤波后的点云投影到多分辨率栅格地图上的边界大小；
所述投影坐标P的计算方式为：
式中，为投影坐标；resolution为投影点的分辨率；x，y，z为滤波后的点云数据对应的点云坐标；xmin为滤波后的点云投影到多分辨率栅格地图上的边界大小；ymax为滤波后的点云投影到多分辨率栅格地图上的边界大小。
4.根据权利要求3所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿，包括如下操作步骤：
将所述3D点云数据投影到所述多分辨率栅格地图中，得到点云栅格图；基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'；
基于所有所述候选位姿T'，从所述多分辨率栅格地图中通过分枝界定搜索操作获取初始位姿T”；
将所述初始位姿T”恢复为三维位姿T；将所述三维位姿T通过IPC点云配准操作得到配准位姿。
5.根据权利要求4所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述基于所述点云栅格图及所述多分辨率栅格地图通过图像线特征匹配操作获取多个候选位姿T'，包括如下操作步骤：
获取相同分辨率下的目标点云栅格图及目标分辨率栅格地图；从所述目标点云栅格图中提取多个第一线特征；并从所述目标分辨率栅格地图中提取多个第二线特征；
对所述第一线特征构建LBD线段描述符，得到第一LBD线段描述符；对所述第二线特征构建LBD线段描述符，得到第二LBD线段描述符；
将各个所述第一LBD线段描述符与各个所述第二LBD线段描述符分别进行匹配得到多个待确定匹配对；计算获取各个所述待确定匹配对中的第一LBD线段描述符与第二LBD线段描述符的线段描述符相似度K；通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对；
分别根据各个所述目标匹配对进行计算获取各个所述目标匹配对分别对应的候选位姿T'。
6.根据权利要求5所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述通过所述线段描述符相似度K对所述待确定匹配对进行筛选得到多个目标匹配对，包括如下操作步骤：
预设线段描述符相似度最高阈值K1；
遍历所有所述线段描述符相似度K，判断所述线段描述符相似度K是否大于或等于所述线段描述符相似度最高阈值K1；若是，则确定所述线段描述符对应的待确定匹配对为目标匹配对。
7.根据权利要求6所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述基于所有所述候选位姿T'，通过分枝界定搜索操作获取初始位姿T”，包括如下操作步骤：
根据所有所述候选位姿T'，获取搜索空间W；
根据候选位姿T'，获取各个所述3D点云数据的初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)；
建立搜索树并获取所述搜索树的所有搜索节点；依次遍历所有所述候选位姿T'；将所述候选位姿T'作为所述搜索树的第一层的第一节点；基于所述初始灰度值Imap(x1,y1)及投影灰度值Icurrent(x2,y2)，计算获取所述第一节点的评分score；
对所述第一节点进行展开，得到多个第二节点；并计算各个所述第二节点对应的评分score；并将所有所述第二节点中最后一个第二节点的评分score作为初始下边界；重复上述操作，直至所述节点达到第N节点；
根据各个所述搜索节点及所述搜索节点对应的评分score，对所述搜索树进行剪枝操作，得到多个待确定目标节点；
根据各个所述待确定目标节点对应的评分score，选择其中评分最高的一个待确定目标节点作为目标节点；并确定所述目标节点对应的候选位姿为初始位姿。
8.根据权利要求7所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述搜索空间W为所有候选位姿在所述多分辨率栅格地图中共同构成的搜索范围；
所述初始灰度值Imap(x1,y1)为所述3D点云数据采集获取时的灰度值；所述投影灰度值Icurrent(x2,y2)为所述3D点云数据投影后的灰度值。
9.根据权利要求8所述的一种基于投影3D激光点云的移动机器人重定位方法，其特征在于，所述第一节点的评分score的计算方式为：
式中，T'＝[x y yaw]T'为候选位姿；Imap(x1,y1)为初始灰度值；Icurrent(x2,y2)为投影灰度值；W为搜索空间；x，y为候选位姿中的坐标；u，v为搜索空间中的像素坐标。
10.一种基于投影3D激光点云的移动机器人重定位系统，其特征在于，包括建图模块、重定位模块；
其中，所述建图模块，用于初始化获取多分辨率栅格地图；
所述重定位模块，用于在机器人需要进行重定位时，通过获取当前所述机器人当前帧的3D点云数据及所述多分辨率栅格地图，进行分析获取所述机器人的配准位姿。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311017158.5A CN117031481A (zh) | 2023-08-14 | 2023-08-14 | 一种基于投影3d激光点云的移动机器人重定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311017158.5A CN117031481A (zh) | 2023-08-14 | 2023-08-14 | 一种基于投影3d激光点云的移动机器人重定位方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117031481A true CN117031481A (zh) | 2023-11-10 |
Family
ID=88644403
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311017158.5A Pending CN117031481A (zh) | 2023-08-14 | 2023-08-14 | 一种基于投影3d激光点云的移动机器人重定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117031481A (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111765884A (zh) * | 2020-06-18 | 2020-10-13 | 北京海益同展信息科技有限公司 | 机器人重定位方法、装置、电子设备及存储介质 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112200869A (zh) * | 2020-10-09 | 2021-01-08 | 浙江大学 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
| CN113551677A (zh) * | 2021-08-16 | 2021-10-26 | 河南牧原智能科技有限公司 | 对机器人进行重定位的方法和相关产品 |
| CN113587933A (zh) * | 2021-07-29 | 2021-11-02 | 山东山速机器人科技有限公司 | 一种基于分支定界算法的室内移动机器人定位方法 |
| CN113776533A (zh) * | 2021-07-29 | 2021-12-10 | 北京旷视科技有限公司 | 可移动设备的重定位方法及装置 |
| CN115375869A (zh) * | 2022-10-25 | 2022-11-22 | 杭州华橙软件技术有限公司 | 一种机器人重定位方法、机器人和计算机可读存储介质 |
| CN116148808A (zh) * | 2023-04-04 | 2023-05-23 | 江苏集萃清联智控科技有限公司 | 一种基于点云描述符的自动驾驶激光重定位方法和系统 |
-
2023
- 2023-08-14 CN CN202311017158.5A patent/CN117031481A/zh active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111765884A (zh) * | 2020-06-18 | 2020-10-13 | 北京海益同展信息科技有限公司 | 机器人重定位方法、装置、电子设备及存储介质 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
| CN112200869A (zh) * | 2020-10-09 | 2021-01-08 | 浙江大学 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
| CN113587933A (zh) * | 2021-07-29 | 2021-11-02 | 山东山速机器人科技有限公司 | 一种基于分支定界算法的室内移动机器人定位方法 |
| CN113776533A (zh) * | 2021-07-29 | 2021-12-10 | 北京旷视科技有限公司 | 可移动设备的重定位方法及装置 |
| CN113551677A (zh) * | 2021-08-16 | 2021-10-26 | 河南牧原智能科技有限公司 | 对机器人进行重定位的方法和相关产品 |
| CN115375869A (zh) * | 2022-10-25 | 2022-11-22 | 杭州华橙软件技术有限公司 | 一种机器人重定位方法、机器人和计算机可读存储介质 |
| CN116148808A (zh) * | 2023-04-04 | 2023-05-23 | 江苏集萃清联智控科技有限公司 | 一种基于点云描述符的自动驾驶激光重定位方法和系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| US9990736B2 (en) | Robust anytime tracking combining 3D shape, color, and motion with annealed dynamic histograms | |
| CN111665842B (zh) | 一种基于语义信息融合的室内slam建图方法及系统 | |
| CN107980150B (zh) | 对三维空间建模 | |
| CN111201451A (zh) | 基于场景的激光数据和雷达数据进行场景中的对象检测的方法及装置 | |
| CN113506318B (zh) | 一种车载边缘场景下的三维目标感知方法 | |
| Mondragón et al. | Visual model feature tracking for UAV control | |
| Qian et al. | Robust visual-lidar simultaneous localization and mapping system for UAV | |
| CN110908374A (zh) | 一种基于ros平台的山地果园避障系统及方法 | |
| Wang et al. | Map-enhanced ego-lane detection in the missing feature scenarios | |
| Lin et al. | CNN-based classification for point cloud object with bearing angle image | |
| Ruhnke et al. | Unsupervised learning of compact 3d models based on the detection of recurrent structures | |
| Wang et al. | 3D-LIDAR based branch estimation and intersection location for autonomous vehicles | |
| CN113269147A (zh) | 基于空间和形状的三维检测方法、系统、存储及处理装置 | |
| CN116664851A (zh) | 一种基于人工智能的自动驾驶数据提取方法 | |
| CN116643291A (zh) | 一种视觉与激光雷达联合剔除动态目标的slam方法 | |
| CN117031481A (zh) | 一种基于投影3d激光点云的移动机器人重定位方法及系统 | |
| Treible et al. | Learning dense stereo matching for digital surface models from satellite imagery | |
| Endo et al. | High definition map aided object detection for autonomous driving in urban areas | |
| Xu et al. | Real-time road detection and description for robot navigation in an unstructured campus environment | |
| Zhao et al. | Self-localization using point cloud matching at the object level in outdoor environment | |
| Hernández-García et al. | 3d city models: Mapping approach using lidar technology | |
| Wang et al. | A novel real-time semantic-assisted LiDAR odometry and mapping system | |
| Mateos et al. | Expanding irregular graph pyramid for an approaching object | |
| Park et al. | Quadtree sampling-based superpixels for 3d range data |
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 |