CN111123279B - 一种融合nd和icp匹配的移动机器人重定位方法 - Google Patents
一种融合nd和icp匹配的移动机器人重定位方法 Download PDFInfo
- Publication number
- CN111123279B CN111123279B CN201911408092.6A CN201911408092A CN111123279B CN 111123279 B CN111123279 B CN 111123279B CN 201911408092 A CN201911408092 A CN 201911408092A CN 111123279 B CN111123279 B CN 111123279B
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- matrix
- laser
- scattering array
- 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
- 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
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明适用于机器人定位技术领域,提供了一种融合ND和IPC匹配的移动机器人重定位方法,包括:对激光帧初始点云进行滤波,获得激光帧点云S;基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分,提取前m个最高ND概率得分的撒阵列粒子;基于m个撒阵列粒子构建矩阵集T2,基于各矩阵将激光帧点云S换到栅格地图,得到m个激光帧点云S″;将m个激光帧点云S″和分别地图点云M'进行ICP匹配,输出前n个误差较小的撒阵列粒子及对应的变换矩阵Te;计算n个撒阵列粒子的ND概率得分,输出最高得分撒阵列粒子对应的变换矩阵Te,基于该变化矩阵Te来计算机器人的当前位姿。仅通过用激光雷达进行定位,降低定位的成本的同时,实现了高精度的重定位。
Description
技术领域
本发明属于机器人定位技术领域,提供了一种融合ND和ICP匹配的移动机器人重定位方法。
背景技术
随着移动机器人在服务业和仓储物流领域有着越来越广泛的应用,其自主定位导航技术也越发重要。现主要的技术有磁导航、视觉导航和激光导航,其中磁导航工作轨迹固定,路径需要铺设电磁导线或磁条。视觉导航轨迹较为自由,但是环境中多使用二维码或特殊图形码等标记物,还需要定期维护。激光导航定位精准,路径灵活多变,适应多种现场环境,因此成为了移动机器人主流的定位导航方式,但是也显露出一系列问题,如机器人的重启或关机、打滑漂移和人为移动等,导致机器人绑架和定位失败问题,此时重定位技术就显得越发重要。
为了解决上述问题,现有的方案主要包含如下三种:3)AMCL定位。在地图中生成随机粒子,计算粒子权重,重采样剔除权重低的粒子,保存和复制权重高的粒子,多次迭代后粒子群收敛于真实机器人位姿。2)机器人回到初始位置。定位失败后,人为将机器人推到初始位置,然后重新开机。3)人为标记机器人新位姿,根据导航地图和真实环境,人为标记机器人的大致位置。这些定位方法都需要人工参与,无法保证重定位的精度。
发明内容
本发明实施例提供一种融合ND和ICP匹配的移动机器人重定位方法,仅通过激光雷达来实现了高精度的重定位。
本发明是这样实现的,一种融合ND和ICP匹配的移动机器人重定位方法,所述方法具体包括如下步骤:
S1、获取激光帧的初始点云,并对激光帧初始点云进行滤波,获得激光帧点云S;
S2、获取栅格导航地图的撒阵列粒子集,基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分,提取前m个最高ND概率得分的撒阵列粒子;
S3、基于m个撒阵列粒子构建矩阵集T2,矩阵集T2中包括m个矩阵,基于各矩阵将激光帧点云S换到栅格地图,得到变换后的m个激光帧点云S″;
S4、将m个激光帧点云S”和分别地图点云M'进行ICP匹配,输出前n个误差较小的撒阵列粒子及对应的变换矩阵Te;
S5、计算n个撒阵列粒子的ND概率得分,输出最高得分撒阵列粒子对应的变换矩阵Te,基于该变化矩阵Te来计算机器人的当前位姿。
进一步的,撒阵列粒子的ND概率得分获取方法具体如下:
基于撒阵列粒子集来构建矩阵集T1,矩阵集T1中包括K个矩阵,基于各矩阵将激光帧点云S换到栅格导航地图上,形成K个激光帧点云S';
基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分,即为帧激光帧点云S'对应撒阵列粒子的ND概率得分。
进一步的,撒阵列粒子集的获取方法具体如下:
遍历栅格导航地图中的像素点,若当前像素值≥254,即且所述像素点的周围距离阈值内的像素值均≥254,将所述像素坐标作为撒阵列粒子保存到撒阵列粒子集。
进一步的,地图点云的ND模型的构建方法具体包括如下步骤:
S31、将栅格导航地图转化为点云地图M,即获取障碍物在地图中的坐标,并对点云地图M进行滤波,生成滤波后的点云地图M';
S32、针对点云地图M'来构建地图点云的ND模型,即二维正态分布概率模型。
进一步的,障碍物在地图中的坐标的获取过程具体如下:
遍历栅格导航地图像素,将像素值为0的像素点通过公式(1)和公式(2)进行坐标变换,即将像素坐标(r,c)转化为地图坐标(x,y),把点添加到点云地图M,坐标变换过程具体如下:
x=c*res+origin_x
y=(height-r-1)*res+origin_y
其中,res为地图分辨率,height为像素表示的方形地图宽度,(origin_x,origin_y)为地图坐标系的偏移值。
本发明提供的融合ND和ICP匹配的移动机器人重定位方法具有如下有益效果:仅通过用激光雷达进行定位,不依赖其它传感器和安装环境,降低定位的成本的同时,实现了高精度的重定位。
附图说明
图1为本发明实施例提供的融合ND和ICP匹配的移动机器人重定位方法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
本发明要解决的是机器人出现打滑漂移、人为移动、重启或关机导致的定位失败问题,实现移动机器人的高精度定位。
图1为本发明实施例提供的融合ND和ICP匹配的移动机器人重定位方法流程图,该方法具体包括如下步骤:
S1、获取激光帧的初始点云,并对激光帧初始点云进行滤波,获得激光帧点云S;
获取当前原始激光帧,使用统计概率滤波器和体素滤波器剔除点云中的离群点,降低点云密度,得到激光帧点云S。
S2、获取栅格导航地图的撒阵列粒子集,撒阵列粒子集存在K个撒阵列粒子,基于撒阵列粒子集来构建矩阵集T1,矩阵集T1中包括K个矩阵,基于各矩阵将激光帧点云S换到栅格导航地图上,形成K个激光帧点云S';
在本发明实施例中,撒阵列粒子集的获取方法具体如下:撒阵列粒子集中的撒阵列粒子是机器人在地图中的假设位置,机器人在导航地图中应处于白色的自由区域,并且周围0.3米范围内没有黑色障碍物;该步骤遍历栅格导航地图中的像素点,步进值为预设的阵列点距离值,如果当前像素值≥254,即且该像素点的周围距离阈值内(0.3米)的像素值均≥254,将此点像素坐标保存到撒阵列粒子集中。
S3、基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分,提取前m个最高ND概率得分对应的撒阵列粒子,点云地图中标注了障碍物的地图坐标;
在本发明实施例中,地图点云的ND模型的构建方法具体包括如下步骤:
S31、将栅格导航地图转化为点云地图M,即获取障碍物在地图中的坐标,并对点云地图M进行滤波,生成滤波后的点云地图M';
将栅格导航地图转化为点云地图M:遍历栅格导航地图像素,栅格导航地图为8位单通道灰度图像,根据像素值分为三种状态:0代表的黑色障碍物区域,像素值>=254代表的白色自由区域,像素值小于254并大于0代表的灰色空闲区域。如果像素值为0,使用如下公式进行坐标变换,将像素坐标(r,c)转化为地图坐标(x,y),把点添加到点云地图M,坐标变换过程具体如下:
x=c*res+origin_x
y=(height-r-1)*res+origin_y
其中,res为地图分辨率,height为像素表示的方形地图宽度,(origin_x,origin_y)为地图坐标系的偏移值。
地图点云滤波:地图点云M的点数多、密度大,还存在明显的一些离群点,容易造成误匹配和匹配效率低,因此使用统计概率滤波器和体素滤波提,剔除点云地图M中的离群点,并且在保证点云微观特征的基础上降低点云的密度,提高匹配效率,最终得到点云地图M'。
S32、针对点云地图M'来构建地图点云的ND模型,即二维正态分布概率模型,其构建方法具体如下:
将点云地图M'栅格化,计算每个栅格内点集qi的均值μi和协方差Σi。
其中,i为栅格点的数量。基于均值μi和协方差Σi得到栅格q的二维正态分布概率密度函数Pi,二维正态分布概率密度函数Pi的计算公式具体如下:
在本发明实施例中,各激光帧点云S'的ND概率得分计算过程具体如下:
遍历激光帧点云S'的点集,判断每个激光点所在的栅格,计算各栅格的二维正态分布概率值,各栅格的二维正态分布概率值累乘得到激光帧点云S'与点云地图M'的匹配总得分,匹配总得分采用似然函数来计算,似然函数值最大的m个撒阵列粒子作为初步最优位姿,具体似然函数公式如下:
S4、最优的m个撒阵列粒子构建矩阵集T2,矩阵集T2中包括m个矩阵,基于各矩阵将激光帧点云S换到栅格地图,得到变换后的m个激光帧点云S″;
S5、将m个激光帧点云S”和分别地图点云M'进行ICP匹配,输出前n个误差较小的撒阵列粒子、对应的对齐点云AL及变换矩阵Te;
将m个激光帧点云S”分别与地图点云M'进行ICP匹配,单个激光帧点云S”与地图点云M'的ICP匹配过程具体如下:
首先分别计算激光帧点云S”及地图点云M'点云的重心,然后去重心化得到激光帧点云S1和地图点云M1,在地图点云M1分别查找离激光帧点云S1各点最近的点,所有的点构成地图点云M2,将地图点云M2和激光帧点云S1中的点分别定义为m2i和si,i∈[0,N],激光帧点云S1和地图点云M2中都存在N个点,地图点云M2和激光帧点云S1中所有点的均值分别为m2mean,smean;
构建最小二乘目标函数使用SVD分解求解最优变换R,t=m2mean-Rsmean,由R和t构建变换矩阵Te,将激光帧点云S1通过矩阵Te变换为AL对齐点云,最后计算激光帧点云S1和地图点云M1的匹配误差如果误差不满足阈值要求,重新进行上述匹配过程,如果满足阈值,停止迭代,返回AL对齐点云、误差error和变换矩阵Te。
S6、计算n个撒阵列粒子的ND概率得分,输出最优位姿撒阵列粒子对应的变换矩阵Te,基于该变化矩阵Te来计算机器人的当前位姿:
基于n个撒阵列粒子来构建矩阵集T3,阵集T3中含有n个矩阵,基于各矩阵将激光帧点云S换到栅格导航地图上,形成n个激光帧点云S″′,遍历激光帧点云S″′的点集,计算各激光帧点云S″′的ND概率得分,输出最高ND概率得分中对应的撒阵列粒子,及其对应的变换矩阵Te,最优的位姿矩阵即为Te*T2,转化为向量得到机器人的位姿(x,y,theta)。
本发明提供的融合ND和ICP匹配的移动机器人重定位方法具有如下有益效果:仅通过用激光雷达进行定位,不依赖其它传感器和安装环境,降低定位的成本的同时,实现了高精度的重定位。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (4)
1.一种融合ND和ICP匹配的移动机器人重定位方法,其特征在于,所述方法具体包括如下步骤:
S1、获取激光帧的初始点云,并对激光帧初始点云进行滤波,获得激光帧点云S;
S2、获取栅格导航地图的撒阵列粒子集,基于地图点云的ND模型计算撒阵列粒子集中各撒阵列粒子的ND概率得分,提取前m个最高ND概率得分的撒阵列粒子;
S3、基于m个撒阵列粒子构建矩阵集T2,基于各矩阵将激光帧点云S换到栅格地图,得到变换后的m个激光帧点云S″;
S4、将m个激光帧点云S”和点云地图M'进行ICP匹配,输出前n个误差较小的撒阵列粒子、对应的变换矩阵Te;
S5、计算n个撒阵列粒子的ND概率得分,输出最高得分撒阵列粒子对应的变换矩阵Te,基于该变化矩阵Te来计算机器人的当前位姿;
撒阵列粒子集的获取方法具体如下:
遍历栅格导航地图中的像素点,若当前像素值≥254,即且所述像素点的周围距离阈值内的像素值均≥254,将所述像素坐标作为撒阵列粒子保存到撒阵列粒子集。
2.如权利要求1所述融合ND和ICP匹配的移动机器人重定位方法,其特征在于,撒阵列粒子的ND概率得分获取方法具体如下:
基于撒阵列粒子集来构建矩阵集T1,矩阵集T1中包括K个矩阵,基于各矩阵将激光帧点云S换到栅格导航地图上,形成K个激光帧点云S';
基于地图点云的ND模型来计算各帧激光帧点云S'的ND概率得分,即为帧激光帧点云S'对应撒阵列粒子的ND概率得分。
3.如权利要求1或2所述融合ND和ICP匹配的移动机器人重定位方法,其特征在于,地图点云的ND模型的构建方法具体包括如下步骤:
S31、将栅格导航地图转化为点云地图M,即获取障碍物在地图中的坐标,并对点云地图M进行滤波,生成滤波后的点云地图M';
S32、针对点云地图M'来构建地图点云的ND模型,即二维正态分布概率模型。
4.如权利要求1或2所述融合ND和ICP匹配的移动机器人重定位方法,其特征在于,障碍物在地图中的坐标的获取过程具体如下:
遍历栅格导航地图像素,将像素值为0的像素点通过公式(1)和公式(2)进行坐标变换,即将像素坐标(r,c)转化为地图坐标(x,y),把点添加到点云地图M,坐标变换过程具体如下:
x=c*res+origin_x (1)
y=(height-r-1)*res+origin_y (2)
其中,res为地图分辨率,height为像素表示的方形地图宽度,(origin_x,origin_y)为地图坐标系的偏移值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911408092.6A CN111123279B (zh) | 2019-12-31 | 2019-12-31 | 一种融合nd和icp匹配的移动机器人重定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911408092.6A CN111123279B (zh) | 2019-12-31 | 2019-12-31 | 一种融合nd和icp匹配的移动机器人重定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111123279A CN111123279A (zh) | 2020-05-08 |
CN111123279B true CN111123279B (zh) | 2022-05-27 |
Family
ID=70506202
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911408092.6A Active CN111123279B (zh) | 2019-12-31 | 2019-12-31 | 一种融合nd和icp匹配的移动机器人重定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111123279B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112904369B (zh) * | 2021-01-14 | 2024-03-19 | 深圳市杉川致行科技有限公司 | 机器人重定位方法、装置、机器人和计算机可读存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105258702A (zh) * | 2015-10-06 | 2016-01-20 | 深圳力子机器人有限公司 | 一种基于slam导航移动机器人的全局定位方法 |
JP5847352B2 (ja) * | 2013-03-04 | 2016-01-20 | 三井金属鉱業株式会社 | リチウム金属複合酸化物粉体 |
CN106092104A (zh) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | 一种室内机器人的重定位方法及装置 |
CN106323273A (zh) * | 2016-08-26 | 2017-01-11 | 深圳微服机器人科技有限公司 | 一种机器人重定位方法及装置 |
CN107765694A (zh) * | 2017-11-06 | 2018-03-06 | 深圳市杉川机器人有限公司 | 一种重定位方法、装置及计算机可读取存储介质 |
CN109460267A (zh) * | 2018-11-05 | 2019-03-12 | 贵州大学 | 移动机器人离线地图保存与实时重定位方法 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及*** |
-
2019
- 2019-12-31 CN CN201911408092.6A patent/CN111123279B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5847352B2 (ja) * | 2013-03-04 | 2016-01-20 | 三井金属鉱業株式会社 | リチウム金属複合酸化物粉体 |
CN105258702A (zh) * | 2015-10-06 | 2016-01-20 | 深圳力子机器人有限公司 | 一种基于slam导航移动机器人的全局定位方法 |
CN106092104A (zh) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | 一种室内机器人的重定位方法及装置 |
CN106323273A (zh) * | 2016-08-26 | 2017-01-11 | 深圳微服机器人科技有限公司 | 一种机器人重定位方法及装置 |
CN107765694A (zh) * | 2017-11-06 | 2018-03-06 | 深圳市杉川机器人有限公司 | 一种重定位方法、装置及计算机可读取存储介质 |
CN109460267A (zh) * | 2018-11-05 | 2019-03-12 | 贵州大学 | 移动机器人离线地图保存与实时重定位方法 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN111123279A (zh) | 2020-05-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111060888B (zh) | 一种融合icp和似然域模型的移动机器人重定位方法 | |
CN112859859B (zh) | 一种基于三维障碍物体素对象映射的动态栅格地图更新方法 | |
US11145073B2 (en) | Computer vision systems and methods for detecting and modeling features of structures in images | |
CN111539994B (zh) | 一种基于语义似然估计的粒子滤波重定位方法 | |
CN113345008B (zh) | 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法 | |
CN109255302A (zh) | 目标物识别方法及终端、移动装置控制方法及终端 | |
CN106056643A (zh) | 一种基于点云的室内动态场景slam方法及*** | |
CN111699410A (zh) | 点云的处理方法、设备和计算机可读存储介质 | |
CN111721279A (zh) | 一种适用于输电巡检工作的末端路径导航方法 | |
CN113112491A (zh) | 一种悬崖检测方法、装置、机器人及存储介质 | |
CN111123279B (zh) | 一种融合nd和icp匹配的移动机器人重定位方法 | |
CN112381873B (zh) | 一种数据标注方法及装置 | |
Deng et al. | Research on target recognition and path planning for EOD robot | |
CN117029870A (zh) | 一种基于路面点云的激光里程计 | |
CN117029817A (zh) | 一种二维栅格地图融合方法及*** | |
Cai et al. | Deep representation and stereo vision based vehicle detection | |
CN114353779B (zh) | 一种采用点云投影快速更新机器人局部代价地图的方法 | |
CN115060268A (zh) | 一种机房融合定位方法、***、设备及存储介质 | |
Zhao et al. | The construction method of the digital operation environment for bridge cranes | |
Dong et al. | Semantic lidar odometry and mapping for mobile robots using rangeNet++ | |
CN117537803B (zh) | 机器人巡检语义-拓扑地图构建方法、***、设备及介质 | |
US20230377307A1 (en) | Method for detecting an object based on monocular camera, electronic device, and non-transitory storage medium storing the method | |
Zhao et al. | Enhanced Indoor Localization Technique Based on Point Cloud Conversion Image Matching. | |
Xie et al. | Precise LiDAR SLAM in Structured Scene Using Finite Plane and Prior Constraint | |
CN118411507A (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 |