CN112711012B - 一种激光雷达定位***的全局位置初始化方法及*** - Google Patents
一种激光雷达定位***的全局位置初始化方法及*** Download PDFInfo
- Publication number
- CN112711012B CN112711012B CN202011511527.2A CN202011511527A CN112711012B CN 112711012 B CN112711012 B CN 112711012B CN 202011511527 A CN202011511527 A CN 202011511527A CN 112711012 B CN112711012 B CN 112711012B
- Authority
- CN
- China
- Prior art keywords
- map
- global
- initial
- conversion matrix
- initialization
- 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
Images
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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- 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/46—Indirect determination of position data
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Radar Systems Or Details Thereof (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种激光雷达定位***的全局位置初始化方法及***,包括:加载全局地图及相关参数;根据相关参数,在全局地图中,切分局部网格子地图;在切分的每一个局部网格子地图中,生成随机估计的n个初始估计位置;对于每一个初始估计位置,生成移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;根据匹配程度得分和位置转换矩阵,对全局位置初始化进行确定;判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位。同时提供了一种相应的终端及存储介质。本发明自动进行全局位置初始化,避免了人工输入初始位置的繁琐操作;当定位丢失时,自动进行全局位置重定位,避免人工介入;实现无人干预的自主移动底盘。
Description
技术领域
本发明涉及智能机器人技术领域,具体地,涉及一种激光雷达定位***的全局位置初始化方法及***,同时提供了一种相应的终端及存储介质。
背景技术
随着自主移动底盘技术发展,将自主移动底盘放入环境地图已知的房间,从已知位置开始移动成为了当下常见的应用场景。
基于激光雷达的定位算法都会遇到一个问题,就是如何建立自主移动底盘初始位置与环境地图的联系。目前的解决方法,是通过人为干预,手动输入自主移动底盘的初始位置估计,之后自主移动底盘才可以开始进行自动定位及导航活动。
此外,如果在定位过程中受到了其他因素干扰发生了定位漂移的情况,往往需要人为二次干预,对***重新进行初始化全局位置,恢复移动底盘的定位及导航功能。
因此,目前的定位导航***,由于存在不能进行全局位置初始化这一问题,全局位置初始化这一过程严重依赖操作人的参与,导致了自主移动底盘常常需要人为的干预及调试,增加了自主移动底盘的使用及维护成本。
经过检索发现:
授权公告号为CN103412565B,授权公告日为2016年01月27日的中国发明专利《一种具有全局位置快速估计能力的机器人及其定位方法》,其定位过程为:事先对环境空间进行3D扫描得到完整的全局地图,将环境空间栅格化,计算每个栅格到与该栅格最近的障碍物点的距离值;启动所述3D扫描装置完成一帧3D扫描帧,同时采用粒子滤波框架模拟一定量的粒子按照所述机器人平台的动力学模型运动,将3D扫描帧按照各粒子的位置和姿态对环境地图进行模拟扫描得到各粒子的激光扫描点,利用栅格图计算得到激光扫描点与机器人激光扫描点最近的粒子,该粒子位置即为机器人全局位置最终的估计结果。该方法存在如下缺陷:
1、该方法中涉及的栅格地图为二维地图,无需使用三维传感器进行扫描及匹配;
2、该方法中利用粒子滤波框架计算机器人全局位置时需要前置条件,即,在***初始化时有一个对于机器人初始位置的良好估计,这一部分工作仍需要人工手动输入***。
综上所述,包括上述专利文献在内的现有技术仍无法避免人为的参与,目前没有发现同本发明类似技术的说明或报道,也尚未收集到国内外类似的资料。
发明内容
本发明针对现有技术中存在的上述不足,提供了一种激光雷达定位***的全局位置初始化方法及***,同时提供了一种相应的终端及存储介质。
根据本发明的一个方面,提供了一种激光雷达定位***的全局位置初始化方法,包括:
加载全局地图及相关参数;
根据所述相关参数,在所述全局地图中,切分局部网格子地图;
在切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置;
对于每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
根据所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定;
判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位。
优选地,所述全局地图,是指:包含全局坐标系的已知环境地图。
优选地,所述全局地图的地图类型包括:pcd、ply、obj、stl以及pgm。
优选地,所述相关参数,包括:网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。
优选地,所述根据所述相关参数,在所述全局地图中,切分局部网格子地图,包括:
根据所述相关参数中的网格子地图切分尺度参数,计算网格横纵个数及网格中心点坐标;
根据所述网格中心点坐标,将所述全局地图分割成多个局部网格子地图。
优选地,将得到的多个所述局部网格子地图存储在容器中。
优选地,所述在切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置,包括:
遍历每一个所述局部网格子地图,以所述局部网格子地图的中心点、x轴和y轴为范围,生成n个***随机估计的初始估计位置,得到对应的集合为Pguess{Pguess_1,Pguess_2,…,Pguess_n}。
优选地,集合Pguess中的每一个所述初始估计位置,均包括x轴位置、y轴位置以及偏航角θ,记作P(x,y,θ)。
优选地,x,y∈submap,θ∈(3.14,3.14]。
优选地,所述对于每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵,包括:
遍历移动底盘的所有可能初始位置,在每一个所述可能初始位置处,以所述初始估计位置为计算初值,利用迭代最近点算法,计算所述可能初始位置处的匹配程度得分及其位置转换矩阵,分别得到对应的集合S{S1,S2,…,Sn}和Pcorrect{Pcorrect_1,Pcorrect_2,…,Pcorrect_n}。
优选地,所述根据所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定,包括:
遍历所述匹配程度得分,获取最小得分所对应的位置转换矩阵中的可能初始位置,若最小得分小于设定阈值a,则视为全局位置初始化成功,雷达定位***直接开始定位。
优选地,设定阈值a为:0.01。
优选地,所述判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位,包括:
对一个线程或一个进程进行实时监控定位,当发生如下任意一种或任意多种异常情况时,判断为再次触发全局位置初始化情况,进行重定位:
-两次定位结果异常跳变;
-两次定位结果与里程计或惯性测量单元数据相差过大;
-当前位置定位匹配程度得分大于设定阈值b。
优选地,所述设定阈值b为:1.0。
根据本发明的另一个方面,提供了一种激光雷达定位***的全局位置初始化***,包括:
加载模块,该模块用于加载全局地图及相关参数;
网格子地图切分模块,该模块根据加载模块加载的所述相关参数,在加载模块加载的所述全局地图中,切分局部网格子地图;
初始估计位置生成模块,该模块在网格子地图切分模块切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置;
匹配得分及位置转换矩阵生成模块:该模块对于初始估计位置生成模块生成的每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
全局位置初始化模块,该模块根据匹配得分及位置转换矩阵生成模块生成的所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定;
重定位模块,该模块用于判断再次触发全局位置初始化情况,进行重定位。
根据本发明的第三个方面,提供了一种终端,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时可用于执行上述任一项所述的方法。
根据本发明的第四个方面,提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时可用于执行上述任一项所述的方法。
由于采用了上述技术方案,本发明与现有技术相比,具有如下至少一项的有益效果:
1、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,自动进行全局位置初始化,避免了人工输入初始位置的繁琐操作。
2、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,定位丢失时,自动进行全局位置重定位,避免人工介入。
3、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,实现无人干预的自主移动底盘,以减少人工成本。
4、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,去除了移动底盘在开始作业之前,根据环境地图,人工推测初始位置并设置这一环节,在简化了工作流程的基础上,能够带来更为精准的定位效果。
5、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,实现移动底盘的无人操作自启动。
6、本发明提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,实现移动底盘的自动丢失重定位。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:
图1为本发明一实施例中激光雷达定位***的全局位置初始化方法流程图;
图2为本发明一优选实施例中激光雷达定位***的全局位置初始化方法流程图;
图3为本发明一优选实施例中全局地图示意图;
图4为本发明一优选实施例中切分局部网格子地图示意图;
图5为本发明一优选实施例中随机生成的初始估计位置(以第一格为例)示意图;
图6为本发明一优选实施例中局部网格子地图中可能初始位置和初始估计位置示意图;
图7为本发明一优选实施例中全局位置初始化成功示意图;
图8为本发明一具体应用实例中采用webots软件搭建的环境地图(即全局地图)示意图;
图9为本发明一具体应用实例中的可视化界面示意图;
图10为本发明一实施例中激光雷达定位***的全局位置初始化***的组成模块示意图。
具体实施方式
下面对本发明的实施例作详细说明:本实施例在以本发明技术方案为前提下进行实施,给出了详细的实施方式和具体的操作过程。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。
图1为本发明一实施例中激光雷达定位***的全局位置初始化方法流程图。
如图1所示,该实施例提供的激光雷达定位***的全局位置初始化方法,可以包括如下步骤:
S100,加载全局地图及相关参数;
S200,根据相关参数,在全局地图中,切分局部网格子地图;
S300,在切分的每一个局部网格子地图中,生成随机估计的n个初始估计位置;
S400,对于每一个初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
S500,根据匹配程度得分和位置转换矩阵,对全局位置初始化进行确定;
S600,判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位。
在该实施例的S100中,作为一优选实施例,全局地图,可以为:包含全局坐标系的已知环境地图。
在该实施例的S100中,作为一优选实施例,全局地图的地图类型可以包括:pcd、ply、obj、stl以及pgm。
在该实施例的S100中,作为一优选实施例,相关参数,可以包括:网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。
在该实施例的S200中,作为一优选实施例,根据相关参数,在全局地图中,切分局部网格子地图,可以包括如下步骤:
S201,根据相关参数中的网格子地图切分尺度参数,计算网格横纵个数及网格中心点坐标;
S202,根据网格中心点坐标,将全局地图分割成多个局部网格子地图。
在该实施例的S202中,作为一优选实施例,将得到的多个局部网格子地图存储在容器中。
在该实施例的S300中,作为一优选实施例,在切分的每一个局部网格子地图中,生成随机估计的n个初始估计位置,可以包括如下步骤:
遍历每一个局部网格子地图,以局部网格子地图的中心点、x轴和y轴为范围,生成n个***随机估计的初始估计位置,得到对应的集合为Pguess{Pguess_1,Pguess_2,…,Pguess_n}。
在该实施例的S300中,作为一优选实施例,集合Pguess中得每一个初始估计位置,均可以包括x轴位置、y轴位置以及偏航角θ,记作P(x,y,θ)。
在该实施例的一具体应用实例中,x,y∈submap,θ∈(3.14,3.14]。
在该实施例的S400中,作为一优选实施例,对于每一个初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵,可以包括如下步骤:
遍历移动底盘的所有可能初始位置,在每一个可能初始位置处,以初始估计位置为计算初值,利用迭代最近点算法,计算可能初始位置处的匹配程度得分及其位置转换矩阵,分别得到对应的集合S{S1,S2,…,Sn}和Pcorrect{Pcorrect_1,Pcorrect_2,…,Pcorrect_n}。
在该实施例的S500中,作为一优选实施例,根据匹配程度得分和位置转换矩阵,对全局位置初始化进行确定,可以包括如下步骤:
遍历匹配程度得分,获取最小得分Scorrect_min所对应的集合Pcorrect中的可能初始位置Pcorrect_min,若最小得分Scorrect_min小于设定阈值a,则视为全局位置初始化成功,雷达定位***直接开始定位。
在该实施例的一具体应用实例中,设定阈值a可以为:0.01
在该实施例的S600中,判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位,可以包括如下步骤:
对一个线程或一个进程进行实时监控定位,当发生如下任意一种或任意多种异常情况时,判断为再次触发全局位置初始化情况,进行重定位:
-两次定位结果异常跳变;
-两次定位结果与里程计或惯性测量单元数据相差过大;
-当前位置定位匹配程度得分大于设定阈值b。
在该实施例的一具体应用实例中,设定阈值b为:1.0
图2为本发明一优选实施例提供的激光雷达定位***的全局位置初始化方法流程图。
如图2所示,该优选实施例提供的激光雷达定位***的全局位置初始化方法,可以包括如下步骤:
步骤1:加载全局地图及相关参数;
步骤2:基于加载的全局地图及相关参数,切分局部网格子地图;
步骤3:在切分的每一个局部网格子地图中,生成随机估计的初始估计位置,得到该位置的集合Pguess{Pguess_1,Pguess_2,…,Pguess_n};
步骤4:对于每一个初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵,得到相应的匹配程度得分集合S和转换后的初始估计位置集合Pcorrect;
步骤5:根据匹配程度得分和位置转换矩阵,进行全局位置最优估计;
步骤6:根据定位情况监控,自动重定位。
作为一优选实施例,步骤1包括如下步骤:
步骤1.1:加载全局地图,支持的地图类型包括pcd、ply、obj、stl、pgm等,为包含全局坐标系的已知环境地图,如图3所示;
步骤1.2:加载相关参数,包括:网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数、重定位触发相关参数。
作为一优选实施例,如图4所示,步骤2包括以下步骤:
步骤2.1:根据网格子地图切分尺度参数计算网格横纵个数及网格中心点坐标。
步骤2.2:根据网格中心坐标,将全局地图分割成局部网格子地图,并储存在容器中。
作为一优选实施例,如图5所示,步骤3包括如下步骤:
遍历各局部网格子地图,以网格的中心点及网格的x、y轴为范围,生成n个***随机估计的初始估计位置,得到相应的集合Pguess{Pguess_1,Pguess_2,…,Pguess_n}。
作为一优选实施例,对于集合Pguess中的每个初始估计位置,由三部分组成,包括:x轴位置、y轴位置及偏航角(yaw)θ,可以记作P(x,y,θ)。
图5中,随机初始位置估计即为随机估计的初始估计位置。
进一步地,x,y∈submap,θ∈(3.14,3.14]。
作为一优选实施例,如图6所示,步骤4包括如下步骤:
遍历移动底盘的所有可能初始位置,在每一个可能初始位置处,以初始估计位置为计算初值,利用迭代最近点算法,计算可能初始位置处的匹配程度得分及其位置转换矩阵,分别得到对应的集合S{S1,S2,…,Sn}和Pcorrect{Pcorrect_1,Pcorrect_2,…,Pcorrect_n}。
图6中,随机初始位置估计即为随机估计的初始估计位置。
作为一优选实施例,如图7所示,步骤5包括如下步骤:
遍历集合S,寻找匹配程度最大、得分最小Scorrect_min对应的初始估计位置Pcorrect_min,其中,得分物理含义为匹配点云数据之间的偏差值之和,分数越小匹配程度越好,也就是说,得分代表最近邻点之间的距离,距离越小,概率越大;若Scorrect_min小于设定阈值a,视为全局位置初始化成功,雷达定位***可以直接开始定位。
进一步地,设定阈值a为:0.01。
图7中,随机初始位置估计即为随机估计的初始估计位置。
作为一优选实施例,步骤6包括如下步骤:
保证一个线程或进程实时监控定位结果,当发生以下任意一种或任意多种异常情况时,判断为再次触发全局位置初始化,进行***的重定位。
异常情况包括:
-两次定位结果异常跳变;
-两次定位结果与里程计或惯性测量单元数据相差过大;
-当前位置定位匹配程度得分(概率过低)大于设定阈值b。
进一步地,设定阈值b为:1.0。
下面结合附图及一具体应用实例,对本发明上述实施例所提供的技术方案进行进一步的详细说明。
在该具体应用实例中,利用C++中std标准容器存储根据参数切割生成的局部网格子地图、随机初始估计位置、匹配程度得分等数据结构。
使用webots、gazebo等机器人仿真软件,模仿雷达数据及环境地图,对本发明上述实施例所提供的方法进行验证,可使用pcl点云库自带的可视化类进行动态的结果显示,方法实例如图8所示。
图9为该具体应用实例中可视化界面示意图,如图9所示,包括雷达原始数据、环境地图、当前子地图以及匹配结果这些方法关键要素的可视化示意。
本发明另一实施例,提供了一种激光雷达定位***的全局位置初始化***,如图10所示,可以包括如下模块:
加载模块、网格子地图切分模块、初始估计位置生成模块、匹配得分及位置转换矩阵生成模块、全局位置初始化模块以及重定位模块。
其中:
加载模块,该模块用于加载全局地图及相关参数;
网格子地图切分模块,该模块根据加载模块加载的相关参数,在加载模块加载的全局地图中,切分局部网格子地图;
初始估计位置生成模块,该模块在网格子地图切分模块切分的每一个局部网格子地图中,生成随机估计的n个初始估计位置;
匹配得分及位置转换矩阵生成模块:该模块对于初始估计位置生成模块生成的每一个初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
全局位置初始化模块,该模块根据匹配得分及位置转换矩阵生成模块生成的匹配程度得分和位置转换矩阵,对全局位置初始化进行确定;
重定位模块,该模块用于判断再次触发全局位置初始化情况,进行重定位。
本发明第三个实施例提供了一种终端,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,处理器执行程序时可用于执行本发明上述实施例中任一项的方法。
可选地,存储器,用于存储程序;存储器,可以包括易失性存储器(英文:volatilememory),例如随机存取存储器(英文:random-access memory,缩写:RAM),如静态随机存取存储器(英文:static random-access memory,缩写:SRAM),双倍数据率同步动态随机存取存储器(英文:Double Data Rate Synchronous Dynamic Random Access Memory,缩写:DDR SDRAM)等;存储器也可以包括非易失性存储器(英文:non-volatile memory),例如快闪存储器(英文:flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程序、功能模块等)、计算机指令等,上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
处理器,用于执行存储器存储的计算机程序,以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。
处理器和存储器可以是独立结构,也可以是集成在一起的集成结构。当处理器和存储器是独立结构时,存储器、处理器可以通过总线耦合连接。
本发明第四个实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时可用于执行本发明上述实施例中任一项的方法。
本发明上述实施例提供的激光雷达定位***的全局位置初始化方法及***、终端、存储介质,自动进行全局位置初始化,避免了人工输入初始位置的繁琐操作;当定位丢失时,自动进行全局位置重定位,避免人工介入;实现无人干预的自主移动底盘,以减少人工成本;去除了移动底盘在开始作业之前,根据环境地图,人工推测初始位置并设置这一环节,在简化了工作流程的基础上,能够带来更为精准的定位效果;实现移动底盘的无人操作自启动;实现移动底盘的自动丢失重定位。
需要说明的是,本发明提供的方法中的步骤,可以利用***中对应的模块、装置、单元等予以实现,本领域技术人员可以参照方法的技术方案实现***的组成,即,方法中的实施例可理解为构建***的优选例,在此不予赘述。
本领域技术人员知道,除了以纯计算机可读程序代码方式实现本发明提供的***及其各个装置以外,完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以,本发明提供的***及其各项装置可以被认为是一种硬件部件,而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构;也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变形或修改,这并不影响本发明的实质内容。
Claims (7)
1.一种激光雷达定位***的全局位置初始化方法,其特征在于,包括如下步骤:
加载全局点云地图及相关参数;
根据所述相关参数,在所述全局点云地图中,切分局部网格子地图;
在切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置;
对于每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
根据所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定;
判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位;
所述判断再次触发全局位置初始化情况,重新执行上述步骤,进行重定位,包括如下步骤:
对一个线程或一个进程进行实时监控定位,当发生如下任意一种或任意多种异常情况时,判断为再次触发全局位置初始化情况,进行重定位:
-两次定位结果异常跳变;
-两次定位结果与里程计或惯性测量单元数据相差过大;
-当前位置定位匹配程度得分大于设定阈值b;
在切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置,包括如下步骤:
遍历每一个所述局部网格子地图,以所述局部网格子地图的中心点、x轴和y轴为范围,生成n个***随机估计的初始估计位置,得到对应的集合为Pguess{Pguess_1,Pguess_2,…,Pguess_n};其中:
集合Pguess中的每一个所述初始估计位置,均包括x轴位置、y轴位置以及偏航角θ,记作P(x,y,θ);
对于每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵,包括如下步骤:
遍历移动底盘的所有可能初始位置,在每一个所述可能初始位置处,以对应的所述初始估计位置为计算初值,利用迭代最近点算法,计算所述可能初始位置处的匹配程度得分及其位置转换矩阵,分别得到对应的集合S{S1,S2,…,Sn}和Pcorrect{Pcorrect_1,Pcorrect_2,…,Pcorrect_n}。
2.根据权利要求1所述的激光雷达定位***的全局位置初始化方法,其特征在于,所述全局点云地图,是指:包含全局坐标系的已知环境地图;和/或
所述相关参数,包括:网格子地图切分尺度参数、每张子地图中生成的随机位置个数、得分阈值参数以及重定位触发相关参数。
3.根据权利要求1所述的激光雷达定位***的全局位置初始化方法,其特征在于,所述根据所述相关参数,在所述全局点云地图中,切分局部网格子地图,包括如下步骤:
根据所述相关参数中的网格子地图切分尺度参数,计算网格横纵个数及网格中心点坐标;
根据所述网格中心点坐标,将所述全局点云地图分割成多个局部网格子地图。
4.根据权利要求1所述的激光雷达定位***的全局位置初始化方法,其特征在于,所述根据所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定,包括如下步骤:
遍历所述匹配程度得分,获取最小得分所对应的位置转换矩阵中的可能初始位置,若最小得分小于设定阈值a,则视为全局位置初始化成功,雷达定位***直接开始定位。
5.一种激光雷达定位***的全局位置初始化***,其特征在于,包括:
加载模块,该模块用于加载全局点云地图及相关参数;
网格子地图切分模块,该模块根据加载模块加载的所述相关参数,在加载模块加载的所述全局点云地图中,切分局部网格子地图;
初始估计位置生成模块,该模块在网格子地图切分模块切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置;
匹配得分及位置转换矩阵生成模块:该模块对于初始估计位置生成模块生成的每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵;
全局位置初始化模块,该模块根据匹配得分及位置转换矩阵生成模块生成的所述匹配程度得分和所述位置转换矩阵,对全局位置初始化进行确定;
重定位模块,该模块用于判断再次触发全局位置初始化情况,进行重定位;其中,对一个线程或一个进程进行实时监控定位,当发生如下任意一种或任意多种异常情况时,判断为再次触发全局位置初始化情况,进行重定位:
-两次定位结果异常跳变;
-两次定位结果与里程计或惯性测量单元数据相差过大;
-当前位置定位匹配程度得分大于设定阈值b;
其中:
在切分的每一个所述局部网格子地图中,生成随机估计的n个初始估计位置,包括如下步骤:
遍历每一个所述局部网格子地图,以所述局部网格子地图的中心点、x轴和y轴为范围,生成n个***随机估计的初始估计位置,得到对应的集合为Pguess{Pguess_1,Pguess_2,…,Pguess_n};其中:
集合Pguess中的每一个所述初始估计位置,均包括x轴位置、y轴位置以及偏航角θ,记作P(x,y,θ);
对于每一个所述初始估计位置,生成对应的移动底盘可能初始位置处的匹配程度得分及其位置转换矩阵,包括如下步骤:
遍历移动底盘的所有可能初始位置,在每一个所述可能初始位置处,以对应的所述初始估计位置为计算初值,利用迭代最近点算法,计算所述可能初始位置处的匹配程度得分及其位置转换矩阵,分别得到对应的集合S{S1,S2,…,Sn}和Pcorrect{Pcorrect_1,Pcorrect_2,…,Pcorrect_n}。
6.一种终端,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时可用于执行权利要求1-4中任一项所述的方法。
7.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时可用于执行权利要求1-4中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011511527.2A CN112711012B (zh) | 2020-12-18 | 2020-12-18 | 一种激光雷达定位***的全局位置初始化方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011511527.2A CN112711012B (zh) | 2020-12-18 | 2020-12-18 | 一种激光雷达定位***的全局位置初始化方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112711012A CN112711012A (zh) | 2021-04-27 |
CN112711012B true CN112711012B (zh) | 2022-10-11 |
Family
ID=75544595
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011511527.2A Active CN112711012B (zh) | 2020-12-18 | 2020-12-18 | 一种激光雷达定位***的全局位置初始化方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112711012B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116840820B (zh) * | 2023-08-29 | 2023-11-24 | 上海仙工智能科技有限公司 | 一种检测2d激光定位丢失的方法及***、存储介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107395433A (zh) * | 2017-08-18 | 2017-11-24 | 中南大学 | 一种基于烟花算法的无线传感器节点部署方法 |
WO2018006082A2 (en) * | 2016-07-01 | 2018-01-04 | Uber Technologies, Inc. | Autonomous vehicle control using submaps |
CN107908185A (zh) * | 2017-10-14 | 2018-04-13 | 北醒(北京)光子科技有限公司 | 一种机器人自主全局重定位方法及机器人 |
CN110686677A (zh) * | 2019-10-10 | 2020-01-14 | 东北大学 | 一种基于几何信息的全局定位方法 |
CN111429520A (zh) * | 2020-03-02 | 2020-07-17 | 广州视源电子科技股份有限公司 | 负障碍物检测方法、装置、终端设备和存储介质 |
CN111538918A (zh) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | 一种推荐方法、装置、电子设备及存储介质 |
CN111928862A (zh) * | 2020-08-10 | 2020-11-13 | 廊坊和易生活网络科技股份有限公司 | 利用激光雷达和视觉传感器融合在线构建语义地图的方法 |
CN111947672A (zh) * | 2020-03-02 | 2020-11-17 | 北京百度网讯科技有限公司 | 用于检测环境变化的方法、装置、设备和介质 |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB201116959D0 (en) * | 2011-09-30 | 2011-11-16 | Bae Systems Plc | Vehicle localisation with 2d laser scanner and 3d prior scans |
EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
CN107167788B (zh) * | 2017-03-21 | 2020-01-21 | 深圳市速腾聚创科技有限公司 | 获取激光雷达校准参数、激光雷达校准的方法及*** |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
US10891769B2 (en) * | 2019-02-14 | 2021-01-12 | Faro Technologies, Inc | System and method of scanning two dimensional floorplans using multiple scanners concurrently |
CN110031009A (zh) * | 2019-04-04 | 2019-07-19 | 肖卫国 | 一种初始定位方法、计算机可读存储介质及*** |
CN111060888B (zh) * | 2019-12-31 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合icp和似然域模型的移动机器人重定位方法 |
CN111678516B (zh) * | 2020-05-08 | 2021-11-23 | 中山大学 | 一种基于激光雷达的有界区域快速全局定位方法 |
CN111578946A (zh) * | 2020-05-27 | 2020-08-25 | 杭州蓝芯科技有限公司 | 一种基于深度学习的激光导航agv重定位方法和装置 |
CN111737395B (zh) * | 2020-08-19 | 2020-12-22 | 浙江欣奕华智能科技有限公司 | 一种占据栅格地图生成方法、装置及机器人*** |
CN112012450B (zh) * | 2020-09-18 | 2022-04-12 | 上海蔚建科技有限公司 | 一种自动抹灰机 |
-
2020
- 2020-12-18 CN CN202011511527.2A patent/CN112711012B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018006082A2 (en) * | 2016-07-01 | 2018-01-04 | Uber Technologies, Inc. | Autonomous vehicle control using submaps |
CN107395433A (zh) * | 2017-08-18 | 2017-11-24 | 中南大学 | 一种基于烟花算法的无线传感器节点部署方法 |
CN107908185A (zh) * | 2017-10-14 | 2018-04-13 | 北醒(北京)光子科技有限公司 | 一种机器人自主全局重定位方法及机器人 |
CN110686677A (zh) * | 2019-10-10 | 2020-01-14 | 东北大学 | 一种基于几何信息的全局定位方法 |
CN111429520A (zh) * | 2020-03-02 | 2020-07-17 | 广州视源电子科技股份有限公司 | 负障碍物检测方法、装置、终端设备和存储介质 |
CN111947672A (zh) * | 2020-03-02 | 2020-11-17 | 北京百度网讯科技有限公司 | 用于检测环境变化的方法、装置、设备和介质 |
CN111538918A (zh) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | 一种推荐方法、装置、电子设备及存储介质 |
CN111928862A (zh) * | 2020-08-10 | 2020-11-13 | 廊坊和易生活网络科技股份有限公司 | 利用激光雷达和视觉传感器融合在线构建语义地图的方法 |
Non-Patent Citations (4)
Title |
---|
New Monte Carlo Localization Using Deep Initialization: A Three-Dimensional LiDAR and a Camera Fusion Approach;HYUNGGI JO;《IEEE Access》;20200817;74485-74496 * |
基于典型栅格地图的轮式移动机器人路径规划与跟踪;王启宇等;《西南科技大学学报》;20171230(第04期);57-59、91 * |
移动机器人视觉-惯性组合定位及建图算法研究;张婉莹;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200215;全文 * |
空间记忆与类脑导航研究进展;斯白露等;《人工智能》;20200210(第01期);17-32 * |
Also Published As
Publication number | Publication date |
---|---|
CN112711012A (zh) | 2021-04-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112884894B (zh) | 场景重建数据采集方法、装置、计算机设备和存储介质 | |
CN111429574A (zh) | 基于三维点云和视觉融合的移动机器人定位方法和*** | |
US20140025203A1 (en) | Collision detection system, collision detection data generator, and robot | |
Tuna et al. | Evaluations of different simultaneous localization and mapping (SLAM) algorithms | |
CN111210475B (zh) | 一种地图更新方法及装置 | |
Moura et al. | Bim-based localization and mapping for mobile robots in construction | |
CN107622530B (zh) | 一种高效鲁棒的三角网切割方法 | |
CN112835064B (zh) | 一种建图定位方法、***、终端及介质 | |
CN112090078B (zh) | 游戏角色移动控制方法、装置、设备和介质 | |
CN112241989A (zh) | 外参标定方法、装置、计算机设备和存储介质 | |
CN112904369B (zh) | 机器人重定位方法、装置、机器人和计算机可读存储介质 | |
CN113119112B (zh) | 适用于六自由度机器人视觉测量的运动规划方法及*** | |
US20230015214A1 (en) | Planar contour recognition method and apparatus, computer device, and storage medium | |
CN112711012B (zh) | 一种激光雷达定位***的全局位置初始化方法及*** | |
CN113211495A (zh) | 一种机械臂碰撞检测方法、***、存储介质和机械臂 | |
CN111161424B (zh) | 一种三维地图的确定方法及确定装置 | |
CN115326051A (zh) | 一种基于动态场景的定位方法、装置、机器人及介质 | |
CN115752474A (zh) | 非平坦地面环境下的机器人导航规划方法、装置及机器人 | |
US20150103080A1 (en) | Computing device and method for simulating point clouds | |
CN113324537A (zh) | 车辆位姿获取方法、车辆定位方法和装置、设备和介质 | |
CN114580044A (zh) | 建筑物外表面数据获取方法、装置、计算机设备和介质 | |
Neves et al. | An environment to support structural testing of autonomous vehicles | |
CN116921932A (zh) | 焊接轨迹识别方法、装置、设备及存储介质 | |
Birr et al. | Oriented surface reachability maps for robot placement | |
CN113658247B (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 |