CN115127559A - 一种定位方法、装置、设备和存储介质 - Google Patents
一种定位方法、装置、设备和存储介质 Download PDFInfo
- Publication number
- CN115127559A CN115127559A CN202210743402.5A CN202210743402A CN115127559A CN 115127559 A CN115127559 A CN 115127559A CN 202210743402 A CN202210743402 A CN 202210743402A CN 115127559 A CN115127559 A CN 115127559A
- Authority
- CN
- China
- Prior art keywords
- current
- robot
- pose
- map
- point cloud
- 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 61
- 239000002245 particle Substances 0.000 claims abstract description 82
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 238000005457 optimization Methods 0.000 claims abstract description 22
- 239000011159 matrix material Substances 0.000 claims description 31
- 230000009466 transformation Effects 0.000 claims description 31
- 238000001514 detection method Methods 0.000 claims description 14
- 238000007476 Maximum Likelihood Methods 0.000 claims description 4
- 238000012952 Resampling Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 6
- 230000003068 static effect Effects 0.000 description 5
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- 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
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Abstract
本申请公开了一种定位方法、装置、设备和存储介质,定位方法包括:根据机器人在全局地图中的初始位置进行初始位置重定位;基于全局地图,对机器人上的激光雷达采集的当前点云数据进行匹配,获取机器人的当前初步位姿;对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到机器人的当前最终位姿,改善了现有技术存在定位精度低,导致定位漂移,甚至定位丢失的技术问题。
Description
技术领域
本申请涉及定位技术领域,尤其涉及一种定位方法、装置、设备和存储介质。
背景技术
机器人基于视觉传感器所提供的视觉信息并结合其他移动传感器所提供的移动数据,一方面能够构建机器人所在场地的地图数据,另一方面,还可基于所构建的地图数据提供路线规划、路线规划调整及导航服务,这使得移动机器人的移动效率更高。然而,在实际应用中,由于定位技术不够完善,导致定位精度低,经常出定位漂移,甚至定位丢失的问题。
发明内容
本申请提供了一种定位方法、装置、设备和存储介质,用于改善现有技术存在定位精度低,导致定位漂移,甚至定位丢失的技术问题。
有鉴于此,本申请第一方面提供了一种定位方法,包括:
根据机器人在全局地图中的初始位置进行初始位置重定位;
基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿;
对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿。
可选的,所述根据机器人在全局地图中的初始位置进行初始位置重定位,包括:
将全局地图中的障碍物进行二维点云化,所述全局地图为栅格地图;
在所述机器人移动到所述全局地图中的初始位置后进行初始位置重定位。
可选的,所述基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿,包括:
根据所述机器人的当前位置,从所述全局地图中提取对应的当前局部地图;
对所述机器人上的激光雷达采集的当前点云数据与所述当前局部地图进行匹配,获取所述机器人的当前初步位姿。
可选的,所述根据所述机器人的当前位置,从所述全局地图中提取对应的当前局部地图,包括:
以所述机器人的当前位置为中心,根据所述机器人上的激光雷达的最大探测距离确定该激光雷达的当前最大探测区域;
从所述全局地图中提取所述激光雷达的当前最大探测区域对应的数据,得到当前局部地图。
可选的,所述对所述机器人上的激光雷达采集的当前点云数据与所述当前局部地图进行匹配,获取所述机器人的当前初步位姿,包括:
对所述机器人上的激光雷达采集的当前点云数据和所述当前局部地图进行多重分辨率降采样,得到多个分辨率的当前点云数据和当前局部地图;
在第i=1轮匹配时,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵;
设置i=i+1,基于上一轮匹配得到的变换矩阵,将次低分辨率的当前点云数据和次低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵,直至对原分辨率的当前点云数据和原分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的最终变换矩阵;
基于所述激光雷达到所述当前局部地图的最终变换矩阵获取所述机器人的当前初步位姿。
可选的,所述将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵,包括:
对最低分辨率的当前点云数据和最低分辨率的当前局部地图分别进行角点特征和线性特征提取,得到对应的点云特征和地图特征;
对所述点云特征和所述地图特征进行点对线迭代最近点匹配,得到所述激光雷达到所述当前局部地图的变换矩阵。
可选的,所述对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿,包括:
在所述机器人的当前局部地图范围内随机生成粒子簇,粒子簇中的每个粒子代表所述机器人的位姿;
根据所述机器人的全向运动模型,结合所述机器人的当前初步位姿对当前粒子簇进行更新;
对当前粒子簇进行自适应重采样,并采用高斯牛顿法对下一时刻的粒子簇进行极大似然估计;
通过对当前粒子簇中的每个粒子进行多次迭代优化,直至粒子簇收敛,得到最终的预测粒子簇;
计算最终预测粒子簇的位姿加权平均值作为所述机器人的当前最终位姿。
本申请第二方面提供了一种定位装置,包括:
重定位单元,用于根据机器人在全局地图中的初始位置进行初始位置重定位;
匹配单元,用于基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿;
位姿优化单元,用于对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿。
本申请第三方面提供了一种定位设备,所述设备包括处理器以及存储器;
所述存储器用于存储程序代码,并将所述程序代码传输给所述处理器;
所述处理器用于根据所述程序代码中的指令执行第一方面任一种所述的定位方法。
本申请第四方面提供了一种计算机可读存储介质,所述计算机可读存储介质用于存储程序代码,所述程序代码被处理器执行时实现第一方面任一种所述的定位方法。
从以上技术方案可以看出,本申请具有以下优点:
本申请提供了一种定位方法,包括:根据机器人在全局地图中的初始位置进行初始位置重定位;基于全局地图,对机器人上的激光雷达采集的当前点云数据进行匹配,获取机器人的当前初步位姿;对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到机器人的当前最终位姿。
本申请中,根据机器人在全局地图中的初始位置进行初始位置重定位,以保证机器人地图坐标与实际位置基本相对应,有助于提高后续定位的准确性;根据全局地图,对激光雷达当前采集的点云数据进行匹配,获取机器人的当前初步位姿,并且进一步对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,以提高定位精度和鲁棒性,从而改善了现有技术存在定位精度低,导致定位漂移,甚至定位丢失的技术问题。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其它的附图。
图1为本申请实施例提供的一种定位方法的一个流程示意图;
图2为本申请实施例提供的一种定位装置的一个结构示意图;
图3为本申请实施例提供的一种定位设备的一个结构示意图。
具体实施方式
本申请提供了一种定位方法、装置、设备和存储介质,用于改善现有技术存在的定位精度低,导致定位漂移,甚至定位丢失的技术问题。
为了使本技术领域的人员更好地理解本申请方案,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
为了便于理解,请参阅图1,本申请实施例提供了一种定位方法,包括:
步骤101、根据机器人在全局地图中的初始位置进行初始位置重定位。
导入机器人所处环境的全局地图,该全局地图为预先构建好的栅格地图。将全局地图中的障碍物进行二维点云化,以构造全局地图匹配数据,用于后续与采集的点云数据进行匹配,有助于提高匹配速度和精度;在机器人移动到全局地图中的初始位置后进行初始位置重定位,保证机器人地图坐标与实际位置基本相对应。
步骤102、基于全局地图,对机器人上的激光雷达采集的当前点云数据进行匹配,获取机器人的当前初步位姿。
在一种实施例中,可以直接将全局地图与机器人上的激光雷达采集的当前点云数据进行匹配,获取机器人的当前初步位姿。但是在全局地图较大时,直接将全局地图与点云数据进行匹配,计算量较大,匹配时收敛速度慢,导致定位速度较慢。
在另一种实施例中,可以根据机器人的当前位置,从全局地图中提取对应的当前局部地图;对机器人上的激光雷达采集的当前点云数据与当前局部地图进行匹配,获取机器人的当前初步位姿。具体的,可以以机器人的当前位置为中心,根据机器人上的激光雷达的最大探测距离确定该激光雷达的当前最大探测区域;从全局地图中提取激光雷达的当前最大探测区域对应的数据,得到当前局部地图。例如,当机器人的当前位置为初始位置时,可以以机器人的初始位置为中心,以机器人上的激光雷达的最大探测距离为半径,在全局地图中以初始位置为圆心确定一个圆形区域,可以将该圆形区域作为机器人的当前局部地图,也可以将该圆形区域的外接正方形区域作为机器人的当前局部区域。通过根据机器人的当前位置和激光雷达的最大探测距离来确定机器人的当前局部地图,以与当前采集的点云数据进行匹配,可以减少计算量,匹配时,由于地图数据大大减少,有助于提高收敛速度,从而提高定位速度。随着机器人的移动,机器人的当前局部地图也会随着当前位置的变化而进行更新。
考虑到机器人所处环境会临时增加静态障碍物的情况,当临时增加的静态障碍物占据传感器视野30%,而预先构建好的全局地图中又没有该障碍物信息,在对激光雷达采集的当前点云数据与当前局部地图进行匹配时,会出现静态障碍物与地图不匹配的情况,导致定位失准,会出现明显定位漂移,甚至导致定位丢失。为了改善该问题,本申请实施例对当前点云数据和当前局部地图进行多重分辨率的变采样匹配搜索,丢弃匹配失败的结果(如临时增加的障碍物),以提高地图匹配精度和定位鲁棒性。
具体的,对机器人上的激光雷达采集的当前点云数据和当前局部地图进行多重分辨率降采样,得到多个分辨率的当前点云数据和当前局部地图;
在第i=1轮匹配时,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的变换矩阵;
设置i=i+1,基于上一轮匹配得到的变换矩阵,将次低分辨率的当前点云数据和次低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的变换矩阵,直至对原分辨率的当前点云数据和原分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的最终变换矩阵;
基于激光雷达到当前局部地图的最终变换矩阵获取机器人的当前初步位姿。
对当前点云数据和当前局部地图进行了n个分辨率的降采样,可以得到降采样后的n个分辨率的当前点云数据和当前局部地图,以及原始分辨率的当前点云数据和当前局部地图(即降采样前的当前点云数据和当前局部地图),即最终有n+1个分辨率的当前点云数据和当前局部地图,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的粗粒度的变换矩阵,然后,基于上一轮匹配获取的变换矩阵,将次低分辨率的当前点云数据和次低分辨率的当前局部地图进行匹配,以获取细粒度的变换矩阵,重复该过程,最终对原分辨率的当前点云数据和原分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的最终变换矩阵,实现从低分辨率到高分辨率对当前点云数据和当前局部地图匹配,以获取高精度位姿。
进一步,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的变换矩阵,包括:
对最低分辨率的当前点云数据和最低分辨率的当前局部地图分别进行角点特征和线性特征提取,得到对应的点云特征和地图特征;
对点云特征和地图特征进行点对线迭代最近点匹配,得到激光雷达到当前局部地图的变换矩阵。
本申请实施例根据机器人的当前位置从全局地图中提取当前局部地图,进行地图线性特征提取和地图多重分辨率的变采样匹配搜索,丢弃匹配失败的结果,提高了地图匹配精度,并提高了在超大型地图(4000平方以上)中进行定位的定位速度和定位鲁棒性;进一步,采用点对线迭代最近点匹配,通过提取满足一定条件的线性特征地图数据与经过线性拟合的点云数据进行匹配,而不仅仅是采用角点特征进行匹配,有助于提高匹配精度,从而提高定位准确性。
步骤103、对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到机器人的当前最终位姿。
本申请实施例,在匹配得到当前初步位姿后,采用粒子滤波进行位姿跟踪,在机器人的当前局部地图范围内随机生成粒子簇,粒子簇中的每个粒子代表机器人的位姿,可以采用高斯分布随机生成粒子簇,也可以使粒子均匀分布在当前局部地图范围内;根据机器人的全向运动模型,结合机器人的当前初步位姿对当前粒子簇进行更新;每次更新,均对当前粒子簇进行自适应重采样,在定位准确性比较高时,随机生成较少的粒子,在定位准确性不高时,随机生成较多的粒子;若在超过时间阈值后,当前粒子簇仍难以收敛到一点附件,则在当前局部地图范围内随机产生一些粒子;采用高斯牛顿法对下一时刻的粒子簇进行极大似然估计;通过对当前粒子簇中的每个粒子进行多次迭代优化,直至粒子簇收敛,得到最终的预测粒子簇;计算最终预测粒子簇的位姿加权平均值作为机器人的当前最终位姿。本申请实施例还取消了运动滤波器限制,即在机器人静态情况下,也进行粒子滤波采样,并限制重采样粒子簇大小,不再限制旋转/平移一段距离时才进行粒子采样,增强了其静态全局定位鲁棒性;本申请实施例采用高斯牛顿法对粒子滤波器产生的位姿进行非线性优化,可以对现实误差及机器人运动模型进行数学建模升级,从而增强模型的可靠性,以提高定位精度和鲁棒性。相比于现有技术采用自适应蒙特卡洛粒子滤波和点对点的迭代最近点匹配对机器人进行运动追踪和全局地图匹配,得到的每秒10HZ的全局定位输出频率,本申请实施例中的定位方法可以优化至20HZ以上,可以确保机器人在1.5m/s内的运动速度,均能被良好追踪,并为路径导航控制程序提供了足够的定位采样频率,提高了整体导航和定位的精度。
本申请实施例中,根据机器人在全局地图中的初始位置进行初始位置重定位,以保证机器人地图坐标与实际位置基本相对应,有助于提高后续定位的准确性;根据全局地图,对激光雷达当前采集的点云数据进行匹配,获取机器人的当前初步位姿,并且进一步对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,以提高定位精度和鲁棒性,从而改善了现有技术存在定位精度低,导致定位漂移,甚至定位丢失的技术问题。
以上为本申请提供的一种定位方法的一个实施例,以下为本申请提供的一种定位装置的一个实施例。
请参考图2,本申请实施例提供的一种定位装置,包括:
重定位单元201,用于根据机器人在全局地图中的初始位置进行初始位置重定位;
匹配单元202,用于基于全局地图,对机器人上的激光雷达采集的当前点云数据进行匹配,获取机器人的当前初步位姿;
位姿优化单元203,用于对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到机器人的当前最终位姿。
作为进一步地改进,重定位单元201具体用于:
将全局地图中的障碍物进行二维点云化,全局地图为栅格地图;
在机器人移动到全局地图中的初始位置后进行初始位置重定位。
作为进一步地改进,匹配单元202具体用于:
根据机器人的当前位置,从全局地图中提取对应的当前局部地图;具体的,以机器人的当前位置为中心,根据机器人上的激光雷达的最大探测距离确定该激光雷达的当前最大探测区域;从全局地图中提取激光雷达的当前最大探测区域对应的数据,得到当前局部地图;
对机器人上的激光雷达采集的当前点云数据与当前局部地图进行匹配,获取机器人的当前初步位姿;具体的,对机器人上的激光雷达采集的当前点云数据和当前局部地图进行多重分辨率降采样,得到多个分辨率的当前点云数据和当前局部地图;
在第i=1轮匹配时,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的变换矩阵;
设置i=i+1,基于上一轮匹配得到的变换矩阵,将次低分辨率的当前点云数据和次低分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的变换矩阵,直至对原分辨率的当前点云数据和原分辨率的当前局部地图进行匹配,得到激光雷达到当前局部地图的最终变换矩阵;
基于激光雷达到当前局部地图的最终变换矩阵获取机器人的当前初步位姿。
作为进一步地改进,位姿优化单元203具体用于:
在机器人的当前局部地图范围内随机生成粒子簇,粒子簇中的每个粒子代表机器人的位姿;
根据机器人的全向运动模型,结合机器人的当前初步位姿对当前粒子簇进行更新;
对当前粒子簇进行自适应重采样,并采用高斯牛顿法对下一时刻的粒子簇进行极大似然估计;
通过对当前粒子簇中的每个粒子进行多次迭代优化,直至粒子簇收敛,得到最终的预测粒子簇;
计算最终预测粒子簇的位姿加权平均值作为机器人的当前最终位姿。
本申请实施例中,根据机器人在全局地图中的初始位置进行初始位置重定位,以保证机器人地图坐标与实际位置基本相对应,有助于提高后续定位的准确性;根据全局地图,对激光雷达当前采集的点云数据进行匹配,获取机器人的当前初步位姿,并且进一步对机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,以提高定位精度和鲁棒性,从而改善了现有技术存在定位精度低,导致定位漂移,甚至定位丢失的技术问题。
请参考图3,本申请实施例还提供了一种定位设备,设备包括处理器301以及存储器302;
存储器302用于存储程序代码,并将程序代码传输给处理器301;
处理器301用于根据程序代码中的指令执行前述方法实施例中的定位方法。
本申请实施例还提供了一种计算机可读存储介质,计算机可读存储介质用于存储程序代码,程序代码被处理器执行时实现前述方法实施例中的定位方法。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本申请的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
应当理解,在本申请中,“至少一个(项)”是指一个或者多个,“多个”是指两个或两个以上。“和/或”,用于描述关联对象的关联关系,表示可以存在三种关系,例如,“A和/或B”可以表示:只存在A,只存在B以及同时存在A和B三种情况,其中A,B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达,是指这些项中的任意组合,包括单项(个)或复数项(个)的任意组合。例如,a,b或c中的至少一项(个),可以表示:a,b,c,“a和b”,“a和c”,“b和c”,或“a和b和c”,其中a,b,c可以是单个,也可以是多个。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本申请各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以通过一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(英文全称:Read-OnlyMemory,英文缩写:ROM)、随机存取存储器(英文全称:Random Access Memory,英文缩写:RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述,以上实施例仅用以说明本申请的技术方案,而非对其限制;尽管参照前述实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
Claims (10)
1.一种定位方法,其特征在于,包括:
根据机器人在全局地图中的初始位置进行初始位置重定位;
基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿;
对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿。
2.根据权利要求1所述的定位方法,其特征在于,所述根据机器人在全局地图中的初始位置进行初始位置重定位,包括:
将全局地图中的障碍物进行二维点云化,所述全局地图为栅格地图;
在所述机器人移动到所述全局地图中的初始位置后进行初始位置重定位。
3.根据权利要求1所述的定位方法,其特征在于,所述基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿,包括:
根据所述机器人的当前位置,从所述全局地图中提取对应的当前局部地图;
对所述机器人上的激光雷达采集的当前点云数据与所述当前局部地图进行匹配,获取所述机器人的当前初步位姿。
4.根据权利要求3所述的定位方法,其特征在于,所述根据所述机器人的当前位置,从所述全局地图中提取对应的当前局部地图,包括:
以所述机器人的当前位置为中心,根据所述机器人上的激光雷达的最大探测距离确定该激光雷达的当前最大探测区域;
从所述全局地图中提取所述激光雷达的当前最大探测区域对应的数据,得到当前局部地图。
5.根据权利要求1所述的定位方法,其特征在于,所述对所述机器人上的激光雷达采集的当前点云数据与所述当前局部地图进行匹配,获取所述机器人的当前初步位姿,包括:
对所述机器人上的激光雷达采集的当前点云数据和所述当前局部地图进行多重分辨率降采样,得到多个分辨率的当前点云数据和当前局部地图;
在第i=1轮匹配时,将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵;
设置i=i+1,基于上一轮匹配得到的变换矩阵,将次低分辨率的当前点云数据和次低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵,直至对原分辨率的当前点云数据和原分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的最终变换矩阵;
基于所述激光雷达到所述当前局部地图的最终变换矩阵获取所述机器人的当前初步位姿。
6.根据权利要求5所述的定位方法,其特征在于,所述将最低分辨率的当前点云数据和最低分辨率的当前局部地图进行匹配,得到所述激光雷达到所述当前局部地图的变换矩阵,包括:
对最低分辨率的当前点云数据和最低分辨率的当前局部地图分别进行角点特征和线性特征提取,得到对应的点云特征和地图特征;
对所述点云特征和所述地图特征进行点对线迭代最近点匹配,得到所述激光雷达到所述当前局部地图的变换矩阵。
7.根据权利要求1所述的定位方法,其特征在于,所述对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿,包括:
在所述机器人的当前局部地图范围内随机生成粒子簇,粒子簇中的每个粒子代表所述机器人的位姿;
根据所述机器人的全向运动模型,结合所述机器人的当前初步位姿对当前粒子簇进行更新;
对当前粒子簇进行自适应重采样,并采用高斯牛顿法对下一时刻的粒子簇进行极大似然估计;
通过对当前粒子簇中的每个粒子进行多次迭代优化,直至粒子簇收敛,得到最终的预测粒子簇;
计算最终预测粒子簇的位姿加权平均值作为所述机器人的当前最终位姿。
8.一种定位装置,其特征在于,包括:
重定位单元,用于根据机器人在全局地图中的初始位置进行初始位置重定位;
匹配单元,用于基于所述全局地图,对所述机器人上的激光雷达采集的当前点云数据进行匹配,获取所述机器人的当前初步位姿;
位姿优化单元,用于对所述机器人的当前初步位姿进行粒子滤波,并采用高斯牛顿法对粒子滤波得到的位姿进行非线性优化,得到所述机器人的当前最终位姿。
9.一种定位设备,其特征在于,所述设备包括处理器以及存储器;
所述存储器用于存储程序代码,并将所述程序代码传输给所述处理器;
所述处理器用于根据所述程序代码中的指令执行权利要求1-7任一项所述的定位方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质用于存储程序代码,所述程序代码被处理器执行时实现权利要求1-7任一项所述的定位方法。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210743402.5A CN115127559A (zh) | 2022-06-28 | 2022-06-28 | 一种定位方法、装置、设备和存储介质 |
PCT/CN2022/140041 WO2024001083A1 (zh) | 2022-06-28 | 2022-12-19 | 一种定位方法、装置、设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210743402.5A CN115127559A (zh) | 2022-06-28 | 2022-06-28 | 一种定位方法、装置、设备和存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115127559A true CN115127559A (zh) | 2022-09-30 |
Family
ID=83379865
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210743402.5A Pending CN115127559A (zh) | 2022-06-28 | 2022-06-28 | 一种定位方法、装置、设备和存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN115127559A (zh) |
WO (1) | WO2024001083A1 (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024001083A1 (zh) * | 2022-06-28 | 2024-01-04 | 广东利元亨智能装备股份有限公司 | 一种定位方法、装置、设备和存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180074209A1 (en) * | 2016-09-14 | 2018-03-15 | Uber Technologies, Inc. | Localization and Tracking Using Location, Signal Strength, and Pseudorange Data |
CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
CN111580530A (zh) * | 2020-06-16 | 2020-08-25 | 福勤智能科技(昆山)有限公司 | 一种定位方法、装置、自主移动设备及介质 |
US20220026920A1 (en) * | 2020-06-10 | 2022-01-27 | AI Incorporated | Light weight and real time slam for robots |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102017111351A1 (de) * | 2017-05-24 | 2018-11-29 | Jena-Optronik Gmbh | Verfahren zur Erfassung und autonomer Verfolgung eines Zielobjekts mittels eines LIDAR-Sensors |
CN112014857B (zh) * | 2020-08-31 | 2023-04-07 | 上海宇航系统工程研究所 | 用于智能巡检的三维激光雷达定位导航方法及巡检机器人 |
CN112764053B (zh) * | 2020-12-29 | 2022-07-15 | 深圳市普渡科技有限公司 | 一种融合定位方法、装置、设备和计算机可读存储介质 |
CN112904369B (zh) * | 2021-01-14 | 2024-03-19 | 深圳市杉川致行科技有限公司 | 机器人重定位方法、装置、机器人和计算机可读存储介质 |
CN115127559A (zh) * | 2022-06-28 | 2022-09-30 | 广东利元亨智能装备股份有限公司 | 一种定位方法、装置、设备和存储介质 |
-
2022
- 2022-06-28 CN CN202210743402.5A patent/CN115127559A/zh active Pending
- 2022-12-19 WO PCT/CN2022/140041 patent/WO2024001083A1/zh unknown
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180074209A1 (en) * | 2016-09-14 | 2018-03-15 | Uber Technologies, Inc. | Localization and Tracking Using Location, Signal Strength, and Pseudorange Data |
CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
US20220026920A1 (en) * | 2020-06-10 | 2022-01-27 | AI Incorporated | Light weight and real time slam for robots |
CN111580530A (zh) * | 2020-06-16 | 2020-08-25 | 福勤智能科技(昆山)有限公司 | 一种定位方法、装置、自主移动设备及介质 |
CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
Non-Patent Citations (2)
Title |
---|
祝继华等: "基于ICP算法和粒子滤波的未知环境地图创建", 《自动化学报》 * |
郝亚东等: "基于高斯牛顿的局部优化SLAM系统", 《传感器世界》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024001083A1 (zh) * | 2022-06-28 | 2024-01-04 | 广东利元亨智能装备股份有限公司 | 一种定位方法、装置、设备和存储介质 |
Also Published As
Publication number | Publication date |
---|---|
WO2024001083A1 (zh) | 2024-01-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110631554B (zh) | 机器人位姿的确定方法、装置、机器人和可读存储介质 | |
CN106940704B (zh) | 一种基于栅格地图的定位方法及装置 | |
CN109544615B (zh) | 基于图像的重定位方法、装置、终端及存储介质 | |
CN109509210B (zh) | 障碍物跟踪方法和装置 | |
CN111486842B (zh) | 重定位方法及装置、机器人 | |
CN111540005B (zh) | 基于二维栅格地图的回环检测方法 | |
CN110260856A (zh) | 一种建图方法和装置 | |
US9111172B2 (en) | Information processing device, information processing method, and program | |
CN113048980B (zh) | 位姿优化方法、装置、电子设备及存储介质 | |
CN109806585B (zh) | 游戏的显示控制方法、装置、设备和存储介质 | |
CN113050116A (zh) | 机器人定位方法、装置、机器人和可读存储介质 | |
CN115127559A (zh) | 一种定位方法、装置、设备和存储介质 | |
CN111113422A (zh) | 机器人定位方法、装置、计算机可读存储介质及机器人 | |
CN112330815A (zh) | 基于障碍物融合的三维点云数据处理方法、装置和设备 | |
CN116088503A (zh) | 动态障碍物检测方法和机器人 | |
Han et al. | DiLO: Direct light detection and ranging odometry based on spherical range images for autonomous driving | |
CN113313765B (zh) | 定位方法、装置、电子设备和存储介质 | |
CN114091515A (zh) | 障碍物检测方法、装置、电子设备和存储介质 | |
CN109816726B (zh) | 一种基于深度滤波器的视觉里程计地图更新方法和系统 | |
CN112308917A (zh) | 一种基于视觉的移动机器人定位方法 | |
CN111376249B (zh) | 移动设备定位系统、方法、装置及移动设备 | |
JP7231036B2 (ja) | メッシュ構造設備検出装置、メッシュ構造設備検出方法、及びプログラム | |
CN113808196A (zh) | 平面融合定位方法、装置、电子设备及存储介质 | |
Ma et al. | Research on robot SLAM of RBPF improved with weight accumulation | |
CN113534848B (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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220930 |