CN112819891B - 激光扫地机的位姿重定位方法、装置、设备及介质 - Google Patents
激光扫地机的位姿重定位方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN112819891B CN112819891B CN202110139564.3A CN202110139564A CN112819891B CN 112819891 B CN112819891 B CN 112819891B CN 202110139564 A CN202110139564 A CN 202110139564A CN 112819891 B CN112819891 B CN 112819891B
- Authority
- CN
- China
- Prior art keywords
- analyzed
- pose
- scattering point
- obstacle distance
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 83
- 241001417527 Pempheridae Species 0.000 title claims abstract description 68
- 238000013507 mapping Methods 0.000 claims abstract description 102
- 238000004364 calculation method Methods 0.000 claims abstract description 68
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 42
- 238000000605 extraction Methods 0.000 claims description 41
- 238000006073 displacement reaction Methods 0.000 claims description 31
- 238000006243 chemical reaction Methods 0.000 claims description 16
- 230000007774 longterm Effects 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 13
- 238000009826 distribution Methods 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 10
- 238000003860 storage Methods 0.000 claims description 10
- 238000010606 normalization Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 6
- 239000002245 particle Substances 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 4
- 230000000295 complement effect Effects 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 7
- 229960002138 anisindione Drugs 0.000 description 4
- 230000000153 supplemental effect Effects 0.000 description 4
- 238000010408 sweeping Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 3
- 210000004027 cell Anatomy 0.000 description 3
- 230000001960 triggered effect Effects 0.000 description 3
- 238000004140 cleaning Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000007480 spreading Effects 0.000 description 2
- 238000003892 spreading Methods 0.000 description 2
- 230000001502 supplementing effect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 210000004725 window cell Anatomy 0.000 description 1
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
- 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
- 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/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
- 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
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- 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/77—Determining position or orientation of objects or cameras using statistical methods
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Geometry (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Data Mining & Analysis (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Computer Graphics (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Image Analysis (AREA)
Abstract
本申请涉及激光定位技术领域,揭示了一种激光扫地机的位姿重定位方法、装置、设备及介质,其中方法包括:根据历史灰度地图得到多个待分析的障碍物距离子地图和各自对应的待分析撒点集合;采用AMCL算法分别根据每个待分析的障碍物距离子地图得到多个待分析候选位姿;基于所有待分析候选位姿分别将待定位的点云数据映射到历史栅格地图上得到多个待计算的映射区域;采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的匹配分数集合;从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值确定位姿重定位结果。从而提高了重定位的速度和准确性。
Description
技术领域
本申请涉及到激光定位技术领域,特别是涉及到一种激光扫地机的位姿重定位方法、装置、设备及介质。
背景技术
随着经济的发展,扫地机得到广泛的应用。相比普通扫地机,激光扫地机由于清扫覆盖更全面、更智能得到广泛认可。现有市场的激光扫地机在清扫过程中被离地抱起后放下、关机状态下移动后再开机、里程计打滑等一种或多种情况发生时需要进行重新定位,因为很多时候清扫面积较大,导致重定位的速度较慢,而且经常导致重定位失败,降低了用户体验。
发明内容
本申请的主要目的为提供一种激光扫地机的位姿重定位方法、装置、设备及介质,旨在解决现有技术的激光扫地机在清扫面积较大时,重定位的速度较慢,经常重定位失败的技术问题。
为了实现上述发明目的,本申请提出一种激光扫地机的位姿重定位方法,所述方法包括:
获取位姿重定位请求;
基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
进一步的,所述根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合的步骤,包括:
根据所述历史栅格地图进行距离每个栅格最近的障碍物距离查找和障碍物距离地图生成,得到待划分的障碍物距离地图;
获取预设撒点区域尺寸,采用所述预设撒点区域尺寸,对所述待划分的障碍物距离地图进行区域划分,得到划分区域的障碍物距离地图;
采用将所述划分区域的障碍物距离地图的每个区域作为九宫格的一个格子、将九宫格作为提取窗口、逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图的提取方法,从所述划分区域的障碍物距离地图中进行子地图提取,得到所述多个待分析的障碍物距离子地图;
获取预设撒点数量,采用所述预设撒点数量,分别在每个所述待分析的障碍物距离子地图对应的所述提取窗口的中心格子进行撒点,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
进一步的,所述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,包括:
从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合;
采用欧式距离,对所述目标撒点集合中的每个撒点进行聚类,得到所述目标撒点集合对应的多个撒点聚类集合;
采用AMCL算法的似然域模型的高斯分布和随机分布,分别对每个所述撒点聚类集合进行每个撒点的似然域概率得分计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的似然域概率得分集合;
分别对每个所述似然域概率得分集合进行归一化处理,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的归一化处理后的概率集合;
分别对每个所述归一化处理后的概率集合进行求和计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的概率总得分;
从所有所述概率总得分中找出最大值,得到目标总得分;
将所述目标总得分对应的所述撒点聚类集合,作为目标撒点聚类集合;
对所述目标撒点聚类集合进行位姿平均值计算,得到所述目标撒点聚类集合对应的待分析的位姿平均值;
将所述待分析的位姿平均值,作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿;
重复执行所述从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合的步骤,直至确定所述多个待分析的障碍物距离子地图各自对应的所述待分析候选位姿。
进一步的,所述采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合的步骤,包括:
从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域;
获取预设切片角度,采用所述预设切片角度和将所述目标映射区域对应的所述待分析候选位姿作为初始位置,对所述目标映射区域进行切片,得到所述目标映射区域对应的多个待分析的映射子区域;
采用暴力匹配的匹配方式,分别对每个所述待分析的映射子区域进行匹配分数计算,得到所述目标映射区域对应的待转换的匹配分数集合;
采用代价函数,对所述待转换的匹配分数集合中每个匹配分数进行转换,得到所述目标映射区域对应的所述待分析的匹配分数集合;
重复执行所述从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域的步骤,直至确定所有所述待计算的映射区域各自对应的所述待分析的匹配分数集合。
进一步的,所述根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果的步骤,包括:
将所述候选匹配分数和所述匹配分数阈值进行比较;
当所述候选匹配分数大于所述匹配分数阈值时,确定所述位姿重定位结果的定位结果为重定位成功,将所述候选匹配分数对应的所述待分析候选位姿作为待优化的候选位姿,采用ceres scan matching算法,对所述待优化的候选位姿进行矫正,得到所述位姿重定位结果的目标位姿;
当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败。
进一步的,所述当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败的步骤包括:
当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为所述重定位失败,并生成位姿重新重定位请求;
基于所述位姿重新重定位请求,获取待处理的点云数据和运动模型发送的所述待处理的点云数据对应的位姿位移数据,其中,所述待处理的点云数据是根据所述激光扫地机的一帧点云数据得到的点云数据,所述待处理的点云数据对应的激光扫描时间晚于所述待定位的点云数据对应的激光扫描时间;
将所述待处理的点云数据作为所述待定位的点云数据;
采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合;
重复执行所述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,直至确定所述位姿重定位结果的定位结果为所述重定位成功,或者位姿重定位的次数满足预设迭代次数。
进一步的,所述采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合的步骤,包括:
获取短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值;
采用粒子过滤算法、所述短期的指数滤波器衰减率阈值和所述长期的指数滤波器衰减率阈值,分别对每个所述待分析撒点集合进行撒点过滤,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的删除撒点集合和保留撒点集合;
基于所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的所述删除撒点集合,分别对每个所述保留撒点集合进行补充撒点,得到所述多个待分析的障碍物距离子地图各自对应的补充撒点后的撒点集合,其中,同一个所述待分析的障碍物距离子地图对应的所述补充撒点后的撒点集合和所述待分析撒点集合的撒点数量相同;
分别将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿与所述位姿位移数据进行相加,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
本申请还提出了一种激光扫地机的位姿重定位装置,所述装置包括:
请求获取模块,用于获取位姿重定位请求;
数据获取模块,用于基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
栅格地图确定模块,用于根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
障碍物距离子地图确定模块,用于根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
候选位姿确定模块,用于采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
点云映射模块,用于基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
匹配分数计算模块,用于采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
位姿重定位结果确定模块,用于获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
本申请还提出了一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现上述任一项所述方法的步骤。
本申请还提出了一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述任一项所述的方法的步骤。
本申请的激光扫地机的位姿重定位方法、装置、设备及介质,通过根据历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和多个待分析的障碍物距离子地图各自对应的待分析撒点集合,采用AMCL算法,针对多个待分析的障碍物距离子地图各自对应的待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿,基于所有待分析候选位姿,分别将待定位的点云数据映射到历史栅格地图上,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿对应的待计算的映射区域,采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的障碍物距离子地图各自对应的待分析的匹配分数集合,获取匹配分数阈值,从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值,确定位姿重定位结果,从而实现了先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
附图说明
图1为本申请一实施例的激光扫地机的位姿重定位方法的流程示意图;
图2为本申请一实施例的激光扫地机的位姿重定位装置的结构示意框图;
图3为本申请一实施例的计算机设备的结构示意框图。
本申请目的实现、功能特点及优点将结合实施例,参照附图做进一步说明。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
为了解决现有技术的激光扫地机在清扫面积较大时,重定位的速度较慢,经常重定位失败的技术问题,本申请提出了激光扫地机的位姿重定位方法,所述方法应用于激光定位技术领域,所述方法还可以应用于采用激光进行定位的设备。比如,激光扫地机、激光定位的驾驶汽车、激光定位的机器人。所述激光扫地机的位姿重定位方法,通过先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
参照图1,本申请实施例中提供一种激光扫地机的位姿重定位方法,所述方法包括:
S1:获取位姿重定位请求;
S2:基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
S3:根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
S4:根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
S5:采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
S6:基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
S7:采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
S8:获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
本实施例通过根据历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和多个待分析的障碍物距离子地图各自对应的待分析撒点集合,采用AMCL算法,针对多个待分析的障碍物距离子地图各自对应的待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿,基于所有待分析候选位姿,分别将待定位的点云数据映射到历史栅格地图上,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿对应的待计算的映射区域,采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的障碍物距离子地图各自对应的待分析的匹配分数集合,获取匹配分数阈值,从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值,确定位姿重定位结果,从而实现了先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
可以理解的是,本申请的激光扫地机的位姿重定位方法可以应用于激光扫地机,还可以应用于其他采用激光进行定位的设备。比如,激光定位的驾驶汽车、激光定位的机器人,在此举例不做具体限定。
激光扫地机,是采用激光进行定位的扫地机。
对于S1,位姿重定位请求可以是用户发送的,还可以是实现本申请的程序文件触发的,还可以是第三方应用系统发送的。
位姿重定位请求,是需要对激光扫地机的当前位姿(二维空间的位姿)进行重新定位的请求。
可选的,所述位姿重定位请求的步骤,包括:获取重定位信号,根据所述重定位信号生成所述位姿重定位请求,其中,所述重定位信号包括:开机信号、开机离地后放回地面信号、轮子打滑信号中的任一种。用户可以在激光扫地机的触摸显示屏操作开机按钮,开机按钮被点击时将自动触发开机信号。用户也可以按下激光扫地机的开机按键,开机按键被按下时将自动触发开机信号。用户还可以在与激光扫地机通信连接的手机APP中操作开机按钮,开机按钮被点击时将自动触发开机信号。用户还可以按下与激光扫地机通信连接的遥控器中的开机按键,开机按键被按下时将自动触发开机信号。激光扫地机在检测到开机状态下离地后再放回地面时,自动触发开机离地后放回地面信号。激光扫地机在检测到轮子打滑时,自动触发轮子打滑信号。其中,激光扫地机的处理器收到重定位信号时,生成位姿重定位请求,并将位姿重定位请求发送给接收请求的软件程序模块。
对于S2,可以从数据库中获取历史灰度地图,还可以从缓存中获取历史灰度地图。
为了减少存储空间,激光扫地机在存储地图时,需要将缓存中的栅格地图转换成灰度图像存放。
历史灰度地图,也就是栅格地图转换成的灰度图像。
激光扫地机的顶端安装有激光传感器,通过激光传感器发射激光和接收障碍物反射的激光,根据发射的激光和反射的激光之间的时间差确定激光扫地机与障碍物之间的距离。
其中,控制激光扫地机的激光传感器对周围环境进行激光扫描一圈,得到一帧激光数据,根据一帧激光数据进行点云格式转换将转换得到的所有点云作为一帧点云数据,将一帧点云数据作为待定位的点云数据。
激光扫地机在激光扫描过程中扫描到的点以距离和角度的方式采集,也就是说,待处理的激光数据中包括的是每个点的距离和角度。
待处理的点云数据表述为(x,y),也就是说待处理的点云数据是二维数据。
待处理的点云数据表述为(x,y)的计算公式为:
x=range*cos(θ)
y=range*sin(θ)
其中,range是待处理的激光数据的点的距离,θ是待处理的激光数据的点的角度。
对于S3,根据所述历史灰度地图进行栅格地图和像素点三值化转换,得到历史栅格地图。
栅格地图,也称光栅图像,是指在空间和亮度上都已经离散化了的图像。我们可以把一幅栅格图像考虑为一个矩阵,矩阵中的任一元素对应于图像中的一个点,而相应的值对应于该点的灰度级,数字矩阵中的元素叫做像素。
根据所述历史灰度地图进行栅格地图转换得到待三值化的地图,待三值化的地图中每个栅格对应一个像素点,待三值化的地图分辨率为x,每个像素点代表x*x(x*x也就是一个栅格的面积),在待三值化的地图中,当栅格被占用时该栅格对应的像素点像素值为254,当栅格没有被占用时该栅格对应的像素点像素值为1,当栅格不确定是否被占用时该栅格对应的像素点像素值为255。
对于S4,采用所述历史栅格地图对应的激光点位于边界时看做最大障碍物距离、不位于边界时取最小障碍物距离、激光点越界最大障碍物距离的方法(也就是似然域模型的激光点映射地图的方法),根据所述历史栅格地图进行障碍物距离地图生成,将生成的地图划分为多个子地图,得到多个待分析的障碍物距离子地图;分别在每个待分析的障碍物距离子地图进行撒点,得到所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合。
其中,分别在每个待分析的障碍物距离子地图进行撒点,可以采用均匀撒点,也可以采用随机撒点。
可选的,将待分析的障碍物距离子地图保存在容器中。
对于S5,采用AMCL算法(adaptive Monte Carlo localization,机器人二维环境下的概率定位算法),分别根据每个待分析的障碍物距离子地图,进行所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点的概率计算,根据每个所述待分析撒点集合的概率计算结果确定一个候选位姿,将确定的每个候选位姿作为一个待分析候选位姿。
对于S6,分别将每个所述待分析候选位姿作为一个参考点,将所述待定位的点云数据映射到所述历史栅格地图上,得到一个待计算的映射区域。也就是说,待计算的映射区域是所述历史栅格地图的部分或者全部。
对于S7,采用实时相关扫描匹配方法(Real Time Correlative Scan Matcher)的二维模式,以所述待计算的映射区域对应的待分析候选位姿作为初始位置,对所述待计算的映射区域进行切片、以初始位置为基础进行搜索、生成候选位置、计算每个候选位置的匹配分数,将每个所述待分析候选位姿对应的所有匹配分数作为一个待分析的匹配分数集合。
对于S8,可以从数据库中获取匹配分数阈值,也可以从第三方应用系统获取匹配分数阈值,还可以将匹配分数阈值写入实现本申请的程序文件中。
从所有所述待分析的匹配分数集合的所有匹配分数中找出最大值,将找出的最大值对应的匹配分数作为候选匹配分数。
当所述候选匹配分数大于所述匹配分数阈值时,确定所述位姿重定位结果的定位结果为重定位成功,根据大于所述匹配分数阈值的所述候选匹配分数,确定所述位姿重定位结果的目标位姿;当所述候选匹配分数小于或等于所述匹配分数阈值时,确定位姿重定位结果为定位失败。
可以理解的是,位姿重定位结果的目标位姿是个二维空间的位姿。
在一个实施例中,上述根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合的步骤,包括:
S41:根据所述历史栅格地图进行距离每个栅格最近的障碍物距离查找和障碍物距离地图生成,得到待划分的障碍物距离地图;
S42:获取预设撒点区域尺寸,采用所述预设撒点区域尺寸,对所述待划分的障碍物距离地图进行区域划分,得到划分区域的障碍物距离地图;
S43:采用将所述划分区域的障碍物距离地图的每个区域作为九宫格的一个格子、将九宫格作为提取窗口、逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图的提取方法,从所述划分区域的障碍物距离地图中进行子地图提取,得到所述多个待分析的障碍物距离子地图;
S44:获取预设撒点数量,采用所述预设撒点数量,分别在每个所述待分析的障碍物距离子地图对应的所述提取窗口的中心格子进行撒点,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
本实施例实现了先采用所述预设撒点区域尺寸对所述待划分的障碍物距离地图进行区域划分,然后采用将所述划分区域的障碍物距离地图的每个区域作为九宫格的一个格子、将九宫格作为提取窗口、逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图的提取方法,从所述划分区域的障碍物距离地图中进行子地图提取,然后在每个所述待分析的障碍物距离子地图对应的所述提取窗口的中心格子进行撒点,从而减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度。
对于S41,采用所述历史栅格地图对应的激光点位于边界时看做最大障碍物距离、不位于边界时取最小障碍物距离、激光点越界最大障碍物距离的方法(也就是似然域模型的激光点映射地图的方法),根据所述历史栅格地图进行距离每个栅格最近的障碍物距离查找,根据查找到的障碍物距离进行障碍物距离地图生成,将生成的地图作为待划分的障碍物距离地图。
对于S42,可以从数据库中获取预设撒点区域尺寸,也可以从第三方应用系统获取预设撒点区域尺寸,还可以将预设撒点区域尺寸写入实现本申请的程序文件中。
预设撒点区域尺寸,是激光扫描半径映射到所述待划分的障碍物距离地图上的区域的长度和宽度。比如,激光扫描半径5米*5米映射到所述待划分的障碍物距离地图上的区域的长度和宽度。
可选的,将2米*2米映射到所述待划分的障碍物距离地图上的区域的长度和宽度,作为预设撒点区域尺寸。也就是说,预设撒点区域尺寸包括长度和宽度。
采用所述预设撒点区域尺寸,对所述待划分的障碍物距离地图进行区域划分,也就是将所述待划分的障碍物距离地图划分为多个长度与预设撒点区域尺寸的长度相同,以及宽度与预设撒点区域尺寸的宽度相同的矩形或正方形区域。
对于S43,采用将所述划分区域的障碍物距离地图的每个区域作为九宫格的一个格子、将九宫格作为提取窗口、逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图的提取方法,从所述划分区域的障碍物距离地图中进行子地图提取,将提取的每个子地图作为一个待分析的障碍物距离子地图。也就是说,待分析的障碍物距离子地图的数量和划分区域的障碍物距离地图中区域的数量相同,待分析的障碍物距离子地图是划分区域的障碍物距离地图的部分区域,待分析的障碍物距离子地图包括小于或等于9个的划分区域的障碍物距离地图中的区域。
其中,九宫格包括3*3共计9个格子。
可以理解的是,逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图,也就是从划分区域的障碍物距离地图中的区域的左上角第一个区域开始逐行逐个区域提取,直至完成划分区域的障碍物距离地图中的区域的左上角最后一个区域提取,每次提取时所述提取窗口的中心格子必须包括划分区域的障碍物距离地图的一个区域。
对于S44,可以从数据库中获取预设撒点数量,也可以从第三方应用系统获取预设撒点数量,还可以将预设撒点数量写入实现本申请的程序文件中。
预设撒点数量,是一个具体数值。可选的,预设撒点数量设置为5000。
采用所述预设撒点数量,在所述提取窗口的中心格子在所述待分析的障碍物距离子地图上进行随机撒点或均匀撒点,在每个所述待分析的障碍物距离子地图上撒点的数量与所述预设撒点数量相同。
在一个实施例中,上述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,包括:
S51:从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合;
S52:采用欧式距离,对所述目标撒点集合中的每个撒点进行聚类,得到所述目标撒点集合对应的多个撒点聚类集合;
S53:采用AMCL算法的似然域模型的高斯分布和随机分布,分别对每个所述撒点聚类集合进行每个撒点的似然域概率得分计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的似然域概率得分集合;
S54:分别对每个所述似然域概率得分集合进行归一化处理,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的归一化处理后的概率集合;
S55:分别对每个所述归一化处理后的概率集合进行求和计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的概率总得分;
S56:从所有所述概率总得分中找出最大值,得到目标总得分;
S57:将所述目标总得分对应的所述撒点聚类集合,作为目标撒点聚类集合;
S58:对所述目标撒点聚类集合进行位姿平均值计算,得到所述目标撒点聚类集合对应的待分析的位姿平均值;
S59:将所述待分析的位姿平均值,作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿;
S510:重复执行所述从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合的步骤,直至确定所述多个待分析的障碍物距离子地图各自对应的所述待分析候选位姿。
在采用AMCL算法进行候选位姿确定时,撒点的数量越多,收敛得越慢,导致处理的效率大幅度降低,本实施例通过先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度。
对于S51,从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,将提取的所述待分析撒点集合作为目标撒点集合。
对于S52,采用欧式距离,对所述目标撒点集合中的每个撒点进行聚类,将每个聚类得到的集合作为一个撒点聚类集合。
采用欧式距离,对所述目标撒点集合中的每个撒点进行聚类的方法可以从现有技术中选择,在此不做赘述。
对于S53,采用AMCL算法的似然域模型的高斯分布和随机分布,分别对每个所述撒点聚类集合进行每个撒点的似然域概率得分计算,将计算得到的每个撒点聚类集合对应的所有似然域概率得分作为一个似然域概率得分集合。
采用AMCL算法的似然域模型的高斯分布和随机分布,分别对每个所述撒点聚类集合进行每个撒点的似然域概率得分计算的方法可以从现有技术中选择,在此不做赘述。
对于S54,对目标似然域概率得分集合进行归一化处理,以使所述似然域概率得分集合中每个似然域概率得分被归一化为大于0并小于1的小数。目标似然域概率得分集合是所述目标撒点集合对应的所述多个撒点聚类集合各自对应的似然域概率得分集合中任一个似然域概率得分集合。
归一化处理的方法可以从现有技术中选择,在此不做赘述。
对于S55,将目标归一化处理后的概率集合中所有的数据进行相加,得到目标归一化处理后的概率集合对应的撒点聚类集合对应的概率总得分。目标归一化处理后的概率集合是所述目标撒点集合对应的所述多个撒点聚类集合各自对应的归一化处理后的概率集合中任一个撒点聚类集合。
对于S56,从所有所述概率总得分中找出最大值,将找到的最大值对应的概率总得分作为目标总得分。
对于S58,将所述目标撒点聚类集合中所有撒点的位姿的x的值(也就是横坐标的值)进行平均值计算,得到x值平均值,将所述目标撒点聚类集合中所有撒点的位姿的y的值(也就是纵坐标的值)进行平均值计算,得到y值平均值,将所述目标撒点聚类集合中所有撒点的位姿的角度值进行平均值计算,得到角度平均值,将x值平均值、y值平均值、角度平均值作为所述目标撒点聚类集合对应的待分析的位姿平均值。
对于S59,将所述待分析的位姿平均值的x值平均值作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿的x的值,将所述待分析的位姿平均值的y值平均值作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿的y的值,将所述待分析的位姿平均值的角度平均值作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿的角度的值。
对于S510,重复执行步骤S51至步骤S510,直至确定所述多个待分析的障碍物距离子地图各自对应的所述待分析候选位姿。
在一个实施例中,上述采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合的步骤,包括:
S71:从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域;
S72:获取预设切片角度,采用所述预设切片角度和将所述目标映射区域对应的所述待分析候选位姿作为初始位置,对所述目标映射区域进行切片,得到所述目标映射区域对应的多个待分析的映射子区域;
S73:采用暴力匹配的匹配方式,分别对每个所述待分析的映射子区域进行匹配分数计算,得到所述目标映射区域对应的待转换的匹配分数集合;
S74:采用代价函数,对所述待转换的匹配分数集合中每个匹配分数进行转换,得到所述目标映射区域对应的所述待分析的匹配分数集合;
S75:重复执行所述从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域的步骤,直至确定所有所述待计算的映射区域各自对应的所述待分析的匹配分数集合。
本实施例实现了采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了位姿重定位的速度和准确性。
对于S71,从所有所述待计算的映射区域提取一个所述待计算的映射区域,将提取的所述待计算的映射区域作为目标映射区域。
对于S72,可以从数据库中获取预设切片角度,也可以从第三方应用系统获取预设切片角度,还可以获取用户输入的预设切片角度,还可以将预设切片角度写入实现本申请的程序文件中。
预设切片角度,是个具体角度。可选的,预设切片角度设置为1°。
采用所述预设切片角度,将所述目标映射区域对应的所述待分析候选位姿作为初始位置,对所述目标映射区域进行切片,将切出的每片地图区域作为所述目标映射区域对应的一个待分析的映射子区域。
采用所述预设切片角度,将所述目标映射区域对应的所述待分析候选位姿作为初始位置,对所述目标映射区域进行切片的方法可以从现有技术中选择,在此不做赘述。
对于S73,采用暴力匹配的匹配方式,分别对每个所述待分析的映射子区域进行匹配分数计算,将计算得到的所有匹配分数作为所述目标映射区域对应的待转换的匹配分数集合。
采用暴力匹配的匹配方式,分别对每个所述待分析的映射子区域进行匹配分数计算的方法可以从现有技术中选择,在此不做赘述。
对于S74,分别将所述待转换的匹配分数集合中每个匹配分数与代价函数进行相乘计算,将相乘计算得到的所有分数作为所述目标映射区域对应的所述待分析的匹配分数集合。
代价函数,距离初始位置越远则概率值越小,距离初始位置的角度越大则概率值越小。
对于S75,重复执行步骤S71至步骤S75,直至确定所有所述待计算的映射区域各自对应的所述待分析的匹配分数集合。
在一个实施例中,上述根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果的步骤,包括:
S81:将所述候选匹配分数和所述匹配分数阈值进行比较;
S82:当所述候选匹配分数大于所述匹配分数阈值时,确定所述位姿重定位结果的定位结果为重定位成功,将所述候选匹配分数对应的所述待分析候选位姿作为待优化的候选位姿,采用ceres scan matching算法,对所述待优化的候选位姿进行矫正,得到所述位姿重定位结果的目标位姿;
S83:当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败。
本实施例实现了根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
对于S82,当所述候选匹配分数大于所述匹配分数阈值时,意味着所述候选匹配分数符合最低分数要求,此时可以确定所述位姿重定位结果的定位结果为重定位成功,并将大于所述匹配分数阈值的所述候选匹配分数对应的所述待分析候选位姿作为待优化的候选位姿,然后采用ceres scan matching算法对所述待优化的候选位姿进行位姿矫正,将矫正后的位姿作为所述位姿重定位结果的目标位姿。
ceres scan matching算法,是实时相关扫描匹配方法的二维模式的匹配算法。
对于S83,当所述候选匹配分数小于或等于所述匹配分数阈值时,意味着所述候选匹配分数不符合最低分数要求,也就是在所有所述待分析候选位姿中没有找到合适的位姿点作为所述位姿重定位结果的目标位姿,此时可以确定所述位姿重定位结果的所述定位结果为重定位失败。
在一个实施例中,上述当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败的步骤包括:
S831:当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为所述重定位失败,并生成位姿重新重定位请求;
S832:基于所述位姿重新重定位请求,获取待处理的点云数据和运动模型发送的所述待处理的点云数据对应的位姿位移数据,其中,所述待处理的点云数据是根据所述激光扫地机的一帧点云数据得到的点云数据,所述待处理的点云数据对应的激光扫描时间晚于所述待定位的点云数据对应的激光扫描时间;
S833:将所述待处理的点云数据作为所述待定位的点云数据;
S834:采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合;
S835:重复执行所述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,直至确定所述位姿重定位结果的定位结果为所述重定位成功,或者位姿重定位的次数满足预设迭代次数。
本实施例实现了在重定位失败时自动进行下一次位姿重定位,提高了自动化,提高了用户体验。
对于S831,位姿重新重定位请求,基于此次位姿重定位对激光扫地机的当前位姿(二维空间的位姿)进行重新定位的请求。
对于S832,运动模型,是指AMCL技术的运动模型,用于检测激光扫地机的运动量。
所述待处理的点云数据,也就是位姿重新重定位请求用于位姿重定位的点云数据。
运动模型发送的所述待处理的点云数据对应的位姿位移数据,是所述待处理的点云数据对应的机器人位姿相对所述待定位的点云数据(所述位姿重定位结果的所述定位结果为重定位失败)的运动量。
位姿位移数据,包括:横坐标位移量、纵坐标位移量、角度位移量。
对于S833,将所述待处理的点云数据作为所述待定位的点云数据,以便用于下一次位姿重新进行重定位。
对于S834,采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点的位姿进行位姿转换,也就是将所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点的位姿的横坐标的值加上所述位姿位移数据的横坐标位移量,将所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点的位姿的纵坐标的值加上所述位姿位移数据的纵坐标位移量,将所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点的位姿的角度的值加上所述位姿位移数据的角度位移量。
对于S835,重复执行步骤S5,直至确定所述位姿重定位结果的定位结果为所述重定位成功,或者位姿重定位的次数满足预设迭代次数。
预设迭代次数,是个大于0的整数。
位姿重定位的次数的初始值是0,执行步骤S8一次,位姿重定位的次数增加1。
在一个实施例中,上述采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合的步骤,包括:
S8341:获取短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值;
S8342:采用粒子过滤算法、所述短期的指数滤波器衰减率阈值和所述长期的指数滤波器衰减率阈值,分别对每个所述待分析撒点集合进行撒点过滤,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的删除撒点集合和保留撒点集合;
S8343:基于所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的所述删除撒点集合,分别对每个所述保留撒点集合进行补充撒点,得到所述多个待分析的障碍物距离子地图各自对应的补充撒点后的撒点集合,其中,同一个所述待分析的障碍物距离子地图对应的所述补充撒点后的撒点集合和所述待分析撒点集合的撒点数量相同;
S8344:分别将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿与所述位姿位移数据进行相加,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
本实施例实现了先过滤不质量不好的撒点,然后再进行补撒点,在保证每个所述待分析撒点集合中的撒点数量不变化的情况下,提高了所述待分析撒点集合中撒点的质量,提高了位姿重新重定位的效率,也提高了位姿重新重定位的准确性。
对于S8341,可以从数据库中获取短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值,也可以从第三方应用系统获取短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值,还可以获取用户输入的短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值,还可以将短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值写入实现本申请的程序文件中。
短期的指数滤波器衰减率阈值,也就是短期的指数滤波器衰减率的阈值。
长期的指数滤波器衰减率阈值,也就是长期的指数滤波器衰减率的阈值。
对于S8342,采用粒子过滤算法、所述短期的指数滤波器衰减率阈值和所述长期的指数滤波器衰减率阈值,分别对每个所述待分析撒点集合进行撒点过滤,将每个所述待分析撒点集合所有被过滤掉的撒点作为一个删除撒点集合,将每个所述待分析撒点集合被过滤后剩余的所有撒点作为一个保留撒点集合。
采用粒子过滤算法、所述短期的指数滤波器衰减率阈值和所述长期的指数滤波器衰减率阈值,分别对每个所述待分析撒点集合进行撒点过滤的方法可以从现有技术中选择,在此不做赘述。
对于S8343,基于目标待分析的障碍物距离子地图对应的所述待分析撒点集合对应的所述删除撒点集合,分别对目标待分析的障碍物距离子地图对应的所述待分析撒点集合对应的保留撒点集合进行补充撒点,补充撒点得到的撒点与目标待分析的障碍物距离子地图对应的所述待分析撒点集合对应的所述删除撒点集合中的撒点不相同,目标待分析的障碍物距离子地图是所述多个待分析的障碍物距离子地图中任一个待分析的障碍物距离子地图。
对于S8344,分别将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿与所述位姿位移数据进行相加,也就是将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿的横坐标的值加上所述位姿位移数据的横坐标位移量,将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿的纵坐标的值加上所述位姿位移数据的纵坐标位移量,将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿的角度的值加上所述位姿位移数据的角度位移量。
参照图2,本申请还提出了一种激光扫地机的位姿重定位装置,所述装置包括:
请求获取模块100,用于获取位姿重定位请求;
数据获取模块200,用于基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
栅格地图确定模块300,用于根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
障碍物距离子地图确定模块400,用于根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
候选位姿确定模块500,用于采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
点云映射模块600,用于基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
匹配分数计算模块700,用于采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
位姿重定位结果确定模块800,用于获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
本实施例通过根据历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和多个待分析的障碍物距离子地图各自对应的待分析撒点集合,采用AMCL算法,针对多个待分析的障碍物距离子地图各自对应的待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿,基于所有待分析候选位姿,分别将待定位的点云数据映射到历史栅格地图上,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿对应的待计算的映射区域,采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的障碍物距离子地图各自对应的待分析的匹配分数集合,获取匹配分数阈值,从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值,确定位姿重定位结果,从而实现了先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
参照图3,本申请实施例中还提供一种计算机设备,该计算机设备可以是服务器,其内部结构可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中,该计算机设计的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于储存激光扫地机的位姿重定位方法等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种激光扫地机的位姿重定位方法。所述激光扫地机的位姿重定位方法,包括:获取位姿重定位请求;基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
本实施例通过根据历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和多个待分析的障碍物距离子地图各自对应的待分析撒点集合,采用AMCL算法,针对多个待分析的障碍物距离子地图各自对应的待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿,基于所有待分析候选位姿,分别将待定位的点云数据映射到历史栅格地图上,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿对应的待计算的映射区域,采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的障碍物距离子地图各自对应的待分析的匹配分数集合,获取匹配分数阈值,从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值,确定位姿重定位结果,从而实现了先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
本申请一实施例还提供一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现一种激光扫地机的位姿重定位方法,包括步骤:获取位姿重定位请求;基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
上述执行的激光扫地机的位姿重定位方法,通过根据历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和多个待分析的障碍物距离子地图各自对应的待分析撒点集合,采用AMCL算法,针对多个待分析的障碍物距离子地图各自对应的待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿,基于所有待分析候选位姿,分别将待定位的点云数据映射到历史栅格地图上,得到多个待分析的障碍物距离子地图各自对应的待分析候选位姿对应的待计算的映射区域,采用实时相关扫描匹配方法的二维模式,分别对每个待计算的映射区域进行匹配分数计算,得到多个待分析的障碍物距离子地图各自对应的待分析的匹配分数集合,获取匹配分数阈值,从所有待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据候选匹配分数和匹配分数阈值,确定位姿重定位结果,从而实现了先进行子地图提取和撒点,减少了每次需要处理的子地图的大小和撒点的数量,有利于提高采用AMCL算法进行候选位姿确定时收敛的速度,从而提高了重定位的速度;而且采用实时相关扫描匹配方法的二维模式进行匹配分数计算,有利于提高匹配分数计算的速度和准确性,从而提高了重定位的速度和准确性。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的和实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双速据率SDRAM(SSRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、装置、物品或者方法不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、装置、物品或者方法所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括该要素的过程、装置、物品或者方法中还存在另外的相同要素。
以上所述仅为本申请的优选实施例,并非因此限制本申请的专利范围,凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本申请的专利保护范围内。
Claims (10)
1.一种激光扫地机的位姿重定位方法,其特征在于,所述方法包括:
获取位姿重定位请求;
基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
2.根据权利要求1所述的激光扫地机的位姿重定位方法,其特征在于,所述根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合的步骤,包括:
根据所述历史栅格地图进行距离每个栅格最近的障碍物距离查找和障碍物距离地图生成,得到待划分的障碍物距离地图;
获取预设撒点区域尺寸,采用所述预设撒点区域尺寸,对所述待划分的障碍物距离地图进行区域划分,得到划分区域的障碍物距离地图;
采用将所述划分区域的障碍物距离地图的每个区域作为九宫格的一个格子、将九宫格作为提取窗口、逐格滑动所述提取窗口和所述提取窗口的中心格子必须包含地图的提取方法,从所述划分区域的障碍物距离地图中进行子地图提取,得到所述多个待分析的障碍物距离子地图;
获取预设撒点数量,采用所述预设撒点数量,分别在每个所述待分析的障碍物距离子地图对应的所述提取窗口的中心格子进行撒点,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
3.根据权利要求1所述的激光扫地机的位姿重定位方法,其特征在于,所述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,包括:
从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合;
采用欧式距离,对所述目标撒点集合中的每个撒点进行聚类,得到所述目标撒点集合对应的多个撒点聚类集合;
采用AMCL算法的似然域模型的高斯分布和随机分布,分别对每个所述撒点聚类集合进行每个撒点的似然域概率得分计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的似然域概率得分集合;
分别对每个所述似然域概率得分集合进行归一化处理,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的归一化处理后的概率集合;
分别对每个所述归一化处理后的概率集合进行求和计算,得到所述目标撒点集合对应的所述多个撒点聚类集合各自对应的概率总得分;
从所有所述概率总得分中找出最大值,得到目标总得分;
将所述目标总得分对应的所述撒点聚类集合,作为目标撒点聚类集合;
对所述目标撒点聚类集合进行位姿平均值计算,得到所述目标撒点聚类集合对应的待分析的位姿平均值;
将所述待分析的位姿平均值,作为所述目标撒点集合对应的所述待分析的障碍物距离子地图对应的所述待分析候选位姿;
重复执行所述从所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中提取一个所述待分析撒点集合,得到目标撒点集合的步骤,直至确定所述多个待分析的障碍物距离子地图各自对应的所述待分析候选位姿。
4.根据权利要求1所述的激光扫地机的位姿重定位方法,其特征在于,所述采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合的步骤,包括:
从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域;
获取预设切片角度,采用所述预设切片角度和将所述目标映射区域对应的所述待分析候选位姿作为初始位置,对所述目标映射区域进行切片,得到所述目标映射区域对应的多个待分析的映射子区域;
采用暴力匹配的匹配方式,分别对每个所述待分析的映射子区域进行匹配分数计算,得到所述目标映射区域对应的待转换的匹配分数集合;
采用代价函数,对所述待转换的匹配分数集合中每个匹配分数进行转换,得到所述目标映射区域对应的所述待分析的匹配分数集合;
重复执行所述从所有所述待计算的映射区域提取一个所述待计算的映射区域,作为目标映射区域的步骤,直至确定所有所述待计算的映射区域各自对应的所述待分析的匹配分数集合。
5.根据权利要求1所述的激光扫地机的位姿重定位方法,其特征在于,所述根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果的步骤,包括:
将所述候选匹配分数和所述匹配分数阈值进行比较;
当所述候选匹配分数大于所述匹配分数阈值时,确定所述位姿重定位结果的定位结果为重定位成功,将所述候选匹配分数对应的所述待分析候选位姿作为待优化的候选位姿,采用ceres scan matching算法,对所述待优化的候选位姿进行矫正,得到所述位姿重定位结果的目标位姿;
当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败。
6.根据权利要求5所述的激光扫地机的位姿重定位方法,其特征在于,所述当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为重定位失败的步骤包括:
当所述候选匹配分数小于或等于所述匹配分数阈值时,确定所述位姿重定位结果的所述定位结果为所述重定位失败,并生成位姿重新重定位请求;
基于所述位姿重新重定位请求,获取待处理的点云数据和运动模型发送的所述待处理的点云数据对应的位姿位移数据,其中,所述待处理的点云数据是根据所述激光扫地机的一帧点云数据得到的点云数据,所述待处理的点云数据对应的激光扫描时间晚于所述待定位的点云数据对应的激光扫描时间;
将所述待处理的点云数据作为所述待定位的点云数据;
采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合;
重复执行所述采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿的步骤,直至确定所述位姿重定位结果的定位结果为所述重定位成功,或者位姿重定位的次数满足预设迭代次数。
7.根据权利要求6所述的激光扫地机的位姿重定位方法,其特征在于,所述采用所述位姿位移数据,对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中每个撒点进行位姿转换,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合的步骤,包括:
获取短期的指数滤波器衰减率阈值和长期的指数滤波器衰减率阈值;
采用粒子过滤算法、所述短期的指数滤波器衰减率阈值和所述长期的指数滤波器衰减率阈值,分别对每个所述待分析撒点集合进行撒点过滤,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的删除撒点集合和保留撒点集合;
基于所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合对应的所述删除撒点集合,分别对每个所述保留撒点集合进行补充撒点,得到所述多个待分析的障碍物距离子地图各自对应的补充撒点后的撒点集合,其中,同一个所述待分析的障碍物距离子地图对应的所述补充撒点后的撒点集合和所述待分析撒点集合的撒点数量相同;
分别将所述多个待分析的障碍物距离子地图各自对应的所述补充撒点后的撒点集合中每个撒点的位姿与所述位姿位移数据进行相加,得到所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合。
8.一种激光扫地机的位姿重定位装置,其特征在于,所述装置包括:
请求获取模块,用于获取位姿重定位请求;
数据获取模块,用于基于所述位姿重定位请求,获取历史灰度地图和待定位的点云数据,所述待定位的点云数据是根据激光扫地机的一帧点云数据得到的点云数据;
栅格地图确定模块,用于根据所述历史灰度地图进行三值化栅格地图转换,得到历史栅格地图;
障碍物距离子地图确定模块,用于根据所述历史栅格地图进行障碍物距离地图生成、子地图提取和撒点,得到多个待分析的障碍物距离子地图和所述多个待分析的障碍物距离子地图各自对应的待分析撒点集合;
候选位姿确定模块,用于采用AMCL算法,针对所述多个待分析的障碍物距离子地图各自对应的所述待分析撒点集合中的每个撒点,分别根据每个待分析的障碍物距离子地图进行每个撒点的概率计算和候选位姿确定,得到所述多个待分析的障碍物距离子地图各自对应的待分析候选位姿;
点云映射模块,用于基于所有所述待分析候选位姿,分别将所述待定位的点云数据映射到所述历史栅格地图上,得到所有所述待分析候选位姿各自对应的待计算的映射区域;
匹配分数计算模块,用于采用实时相关扫描匹配方法的二维模式,分别对每个所述待计算的映射区域进行匹配分数计算,得到所有所述待分析候选位姿各自对应的待分析的匹配分数集合;
位姿重定位结果确定模块,用于获取匹配分数阈值,从所有所述待分析的匹配分数集合中找出最大值,得到候选匹配分数,根据所述候选匹配分数和所述匹配分数阈值,确定位姿重定位结果。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110139564.3A CN112819891B (zh) | 2021-02-01 | 2021-02-01 | 激光扫地机的位姿重定位方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110139564.3A CN112819891B (zh) | 2021-02-01 | 2021-02-01 | 激光扫地机的位姿重定位方法、装置、设备及介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112819891A CN112819891A (zh) | 2021-05-18 |
CN112819891B true CN112819891B (zh) | 2023-12-22 |
Family
ID=75861469
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110139564.3A Active CN112819891B (zh) | 2021-02-01 | 2021-02-01 | 激光扫地机的位姿重定位方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112819891B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113375683A (zh) * | 2021-06-10 | 2021-09-10 | 亿嘉和科技股份有限公司 | 机器人环境地图实时更新方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN110736456A (zh) * | 2019-08-26 | 2020-01-31 | 广东亿嘉和科技有限公司 | 稀疏环境下基于特征提取的二维激光实时定位方法 |
CN111060888A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合icp和似然域模型的移动机器人重定位方法 |
CN111426316A (zh) * | 2020-06-15 | 2020-07-17 | 北京云迹科技有限公司 | 机器人定位方法、装置、机器人及可读存储介质 |
CN111596298A (zh) * | 2020-05-13 | 2020-08-28 | 北京百度网讯科技有限公司 | 目标对象的定位方法、装置、设备及存储介质 |
-
2021
- 2021-02-01 CN CN202110139564.3A patent/CN112819891B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN110736456A (zh) * | 2019-08-26 | 2020-01-31 | 广东亿嘉和科技有限公司 | 稀疏环境下基于特征提取的二维激光实时定位方法 |
CN111060888A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合icp和似然域模型的移动机器人重定位方法 |
CN111596298A (zh) * | 2020-05-13 | 2020-08-28 | 北京百度网讯科技有限公司 | 目标对象的定位方法、装置、设备及存储介质 |
CN111426316A (zh) * | 2020-06-15 | 2020-07-17 | 北京云迹科技有限公司 | 机器人定位方法、装置、机器人及可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112819891A (zh) | 2021-05-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109141437B (zh) | 一种机器人全局重定位方法 | |
CN108297115B (zh) | 一种机器人的自主重定位方法 | |
CN111160302A (zh) | 基于自动驾驶环境的障碍物信息识别方法和装置 | |
KR20180034529A (ko) | 차선 인식 모델링 방법, 장치, 저장 매체 및 기기, 및 인식 방법, 장치, 저장 매체 및 기기 | |
CN112102151A (zh) | 栅格地图的生成方法、装置、移动智慧设备和存储介质 | |
CN112414403B (zh) | 一种机器人的定位定姿方法、设备及存储介质 | |
CN114529837A (zh) | 建筑物轮廓提取方法、系统、计算机设备及存储介质 | |
CN112802103B (zh) | 激光扫地机的位姿重定位方法、装置、设备及介质 | |
CN112819891B (zh) | 激光扫地机的位姿重定位方法、装置、设备及介质 | |
CN111191730B (zh) | 一种面向嵌入式深度学习的超大尺寸图像目标检测方法及系统 | |
CN105262991A (zh) | 一种基于二维码的变电站设备对象识别方法 | |
CN114972968A (zh) | 基于多重神经网络的托盘识别和位姿估计方法 | |
CN110826509A (zh) | 一种基于高分遥感影像的草原围栏信息提取系统及方法 | |
CN115880953B (zh) | 无人机的管控方法及智慧路灯系统 | |
CN110253579B (zh) | 基于圆弧特征提取的机器人定位方法、装置、设备及介质 | |
CN112883853A (zh) | 车道线的检测方法、装置、设备及存储介质 | |
CN109313708B (zh) | 图像匹配方法和视觉系统 | |
CN116363319B (zh) | 一种建筑物屋顶的建模方法、建模装置、设备及介质 | |
CN112258517A (zh) | 激光雷达栅格地图的自动修图方法和装置 | |
CN116664987A (zh) | 目标对象检测模型的训练方法及对象检测方法 | |
CN115909253A (zh) | 一种目标检测、模型训练方法、装置、设备及存储介质 | |
CN111476879A (zh) | 一种点云数据的处理方法、终端及存储介质 | |
CN112348944B (zh) | 三维模型数据更新方法、装置、计算机设备和存储介质 | |
CN112802102A (zh) | 激光扫地机的位姿重定位方法、装置、设备及介质 | |
CN114536326A (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 |