CN114519817A - 机器人重定位方法、装置及机器人 - Google Patents
机器人重定位方法、装置及机器人 Download PDFInfo
- Publication number
- CN114519817A CN114519817A CN202111670051.1A CN202111670051A CN114519817A CN 114519817 A CN114519817 A CN 114519817A CN 202111670051 A CN202111670051 A CN 202111670051A CN 114519817 A CN114519817 A CN 114519817A
- Authority
- CN
- China
- Prior art keywords
- point
- data
- robot
- laser
- 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.)
- Pending
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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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
-
- 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/10016—Video; Image sequence
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种机器人重定位方法、装置及机器人。在上述方法中,接收机器人在当前位置获取的激光点云数据和图像数据;根据视觉词典确定图像数据对应的图像关键帧信息;在视觉地图中获取与图像关键帧信息对应的物理位置信息,将图像数据的特征点信息与图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及图像数据中特征点的像素位置信息进行解算;在解算获取机器人的第一位姿数据的情况下,根据第一位姿数据设定预定区域范围,在预定区域范围内使用激光地图以及激光点云数据进行迭代匹配计算,获取机器人的第二位姿数据。采用上述方案,利用初始位姿数据可以减少激光匹配的迭代范围,加快匹配速度,快速实现机器人重定位。
Description
技术领域
本发明涉及人工智能领域,具体而言,涉及一种机器人重定位方法、装置及机器人。
背景技术
即时定位与地图创建(Simultaneous Localization and Mapping,简称为SLAM)是目前最为广泛应用的机器人定位技术之一。
机器人在使用过程中有可能发生被抱起、脚踢或是打滑等情况,上述情况有可能造成机器人自身定位失效,此时就需要对机器人进行重定位。重定位是机器人智能导航和环境探索的一个重要的基础,也是移动机器人实现真正完全自主的关键技术之一。在SLAM技术中,机器人重定位是重要的一环,在地图复用时起到关键作用。
相关技术中,主要采用激光重定位方案,通过当前激光和已有地图的匹配关系解算机器人的位置和姿态。然而,在实际应用中,针对复杂大场景的环境,需要在全地图查找,采用激光重定位方案,难以快速重定位。
发明内容
本发明的主要目的在于公开了一种机器人重定位方法、装置及机器人,以至少解决相关技术中针对复杂大场景的环境,需要在全地图查找,采用激光重定位方案,难以快速重定位等问题。
根据本发明的一个方面,提供了一种机器人重定位方法。
根据本发明的机器人重定位方法包括:接收机器人在当前位置获取的激光点云数据和图像数据;根据视觉词典确定所述图像数据对应的图像关键帧信息;在视觉地图中获取与所述图像关键帧信息对应的物理位置信息,将所述图像数据的特征点信息与所述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及所述图像数据中特征点的像素位置信息进行解算,在解算获取所述机器人的第一位姿数据的情况下,根据所述第一位姿数据设定预定区域范围,在所述预定区域范围内使用激光地图以及所述激光点云数据进行迭代匹配计算,获取所述机器人的第二位姿数据。
根据本发明的另一方面,提供了一种机器人重定位装置。
根据本发明的机器人重定位装置包括:接收模块,用于接收机器人在当前位置获取的激光点云数据和图像数据;确定模块,用于根据视觉词典确定所述图像数据对应的图像关键帧信息;第一解算模块,用于在视觉地图中获取与所述图像关键帧信息对应的物理位置信息,将所述图像数据的特征点信息与所述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及所述图像数据中特征点的像素位置信息进行解算;第二解算模块,用于在所述第一解算模块解算获取所述机器人的第一位姿数据的情况下,根据所述第一位姿数据设定预定区域范围,在所述预定区域范围内使用激光地图以及所述激光点云数据进行迭代匹配计算,获取所述机器人的第二位姿数据。
根据本发明的又一方面,提供了一种机器人。
根据本发明的机器人包括:存储器及处理器,所述存储器,用于存储计算机执行指令;所述处理器,用于执行所述存储器存储的计算机执行指令,使得所述机器人执行上述任一项所述的方法。
根据本发明,结合视觉词典确定机器人在当前位置获取的图像数据对应的图像关键帧信息,结合视觉地图获取与上述图像关键帧信息对应的物理位置信息,使用上述图像数据的特征点信息与上述图像关键帧信息对应的特征点信息匹配后的视觉地图中特征点的物理位置信息以及上述图像数据中特征点的像素位置信息,解算获取上述机器人的初始位姿数据,根据初始位姿数据设定预定区域范围,在预定区域范围内遍历,使用预先建立的激光地图以及上述激光点云数据进行迭代匹配计算获取上述机器人重定位的位姿数据。尤其针对复杂大场景的环境,无需在全地图查找,缩短了使用激光传感器在大场景范围内搜索的时间,利用初始位姿数据可以减少激光匹配的迭代范围,加快匹配速度,快速实现机器人重定位。
附图说明
图1是根据本发明实施例的机器人重定位方法的流程图;
图2是根据本发明优选实施例的机器人重定位方法的流程图;
图3是根据本发明实施例的机器人重定位装置的结构框图;
图4是根据本发明优选实施例的机器人重定位装置的结构框图;
图5是根据本发明实施例的机器人的结构框图。
具体实施方式
下面结合说明书附图对本发明的具体实现方式做一详细描述。
根据本发明实施例,提供了一种机器人重定位方法。
图1是根据本发明实施例的机器人重定位方法的流程图。如图1所示,该机器人重定位方法包括:
步骤S101:接收机器人在当前位置获取的激光点云数据和图像数据;
步骤S102:根据视觉词典确定上述图像数据对应的图像关键帧信息;
步骤S103:在视觉地图中获取与上述图像关键帧信息对应的物理位置信息,将上述图像数据的特征点信息与上述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及上述图像数据中特征点的像素位置信息进行解算;
步骤S104:在解算获取上述机器人的第一位姿数据的情况下,根据上述第一位姿数据设定预定区域范围,在上述预定区域范围内使用激光地图以及上述激光点云数据进行迭代匹配计算,获取上述机器人的第二位姿数据。
采用图1所示的方法,结合视觉词典确定机器人在当前位置获取的图像数据对应的图像关键帧信息,结合视觉地图获取与上述图像关键帧信息对应的物理位置信息,使用上述图像数据的特征点信息与上述图像关键帧信息对应的特征点信息匹配后的视觉地图中特征点的物理位置信息以及上述图像数据中特征点的像素位置信息,解算获取上述机器人的初始位姿数据,根据初始位姿数据设定预定区域范围,在预定区域范围内遍历,使用预先建立的激光地图以及上述激光点云数据进行迭代匹配计算获取上述机器人重定位的位姿数据。尤其针对复杂大场景的环境,无需在全地图查找,缩短了使用激光传感器在大场景范围内搜索的时间,利用初始位姿数据可以减少激光匹配的迭代范围,加快匹配速度,快速实现机器人重定位。
对于步骤S103中,如果解算未获取到上述机器人的第一位姿数据,则使用激光地图以及上述激光点云数据在全地图范围内进行匹配,获取上述机器人的位姿数据。
优选地,在接收机器人在当前位置获取的激光点云数据和图像数据之前,还可以包括以下处理:建立上述视觉词典、上述视觉地图和上述激光地图,其中,上述视觉词典包括:图像关键帧的特征向量,上述视觉地图包括:图像关键帧的物理位置信息(例如,三维位置坐标)和特征点信息,上述激光地图包括:占用栅格地图。
优选地,上述步骤S102中,根据视觉词典确定上述图像数据对应的图像关键帧信息可以进一步包括:对上述图像数据提取第一特征向量,将上述第一特征向量与视觉词典中图像关键帧的第二特征向量进行匹配,获取匹配度最高的图像关键帧信息(例如,关键帧索引信息)。
视觉词典是图像分类检索等领域的图像建模方法,词典表示将文档描述为词典中关键词出现频率的向量。例如,用SURF(SpeededUp Robust Features)算法提取图像的自然局部视觉特征向量,把相似的SURF自然视觉特征向量划分为相同的自然视觉单词(采用K-means算法对局部视觉特征集合进行聚类,一个聚类中心即为一个视觉单词);自然视觉词典的每一自然视觉单词采用GMM(GaussianMixture Model)方法进行建模自然视觉单词的概率模型,通过概率模型建立了更为精确的局部自然视觉特征与自然视觉单词间的匹配。
在优选实施过程中,从图像数据提取第一特征向量,与视觉词典中图像关键帧的第二特征向量进行匹配,可以利用欧式距离计算公式得到这两个特征向量的距离,当距离最小时,获取到匹配度最高的图像关键帧信息,例如,当欧式距离最小时,与机器人在当前位置的图像数据匹配度最高的图像关键帧是第15帧。然后在视觉地图中获取与第15帧图像对应的三维位置坐标。将上述图像数据的特征点信息与第15帧对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的三维位置坐标以及上述图像数据中特征点的像素位置信息,解算获取上述机器人的初始位姿数据。例如,采用PnP算法通过n个特征点与图像成像中的n个像素点,计算出其投影关系,从而计算出机器人的位姿数据。
PnP算法是在已知n个3D空间点以及它们的投影位置时估计机器人的位姿数据。
假设机器人位于点Oc,P1、P2、P3……为特征点。
场景1:n=1时;
当只有一个特征点P1,假设它就在图像的正中央,向量OcP1就是机器人坐标系中的Z轴,此时机器人永远是面对P1,于是机器人可能的位置在以P1为球心的球面上,此外球的半径也无法确定,有无数个解。
场景2:n=2时;
多了一个约束条件,OcP1P2形成一个三角形,由于P1、P2两点位置确定,三角形的边P1P2确定。再加上向量OcP1和OcP2,从Oc点射向特征点的方向角也能确定。于是能够计算出OcP1的长度=r1,OcP2的长度=r2。这种情况下得到两个球:以P1为球心,半径为r1的球A;以P2为球心,半径为r2的球B。显然,相机位于球A,球B的相交处,依旧是无数个解。
场景3:n=3时;
这次又多了一个以P3为球心的球C,相机这次位于ABC三个球面的相交处,有4个解,其中一个就是机器人位姿。
场景4:n>3时;
n>3后,能够求出正解,为了更快更节省计算机资源地解决问题,可以用3个点计算出4组解获得四个旋转矩阵、平移矩阵。根据公式:
将第4个点的世界坐标代入公式,获得其在图像中的四个投影(一个解对应一个投影),取出其中投影误差最小的解,这是需要的正解。
优选地,步骤S104中,在上述预定区域范围内使用激光地图以及上述激光点云数据进行迭代匹配计算,获取上述机器人的第二位姿数据可以进一步以下包括以下处理:在上述预设区域范围内选取多个位置点,对于每个位置点选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到上述占用栅格地图中,根据投影结果计算该角度对应的占用概率值;在所有获取到的占用概率值中选取最大的占用概率值,将上述最大的占用概率值对应的机器人位姿数据确定为上述第二位姿数据。
优选地,在上述预设区域范围内选取多个位置点,对于每个位置点选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算该角度对应的占用概率值可以进一步包括以下处理:
S1:将上述第一位姿数据中的位置点作为初始点,选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算该角度对应的占用概率值;
S2:按照预定步长和方向,确定多个下一级位置点,选取上述下一级位置点中每个位置点对应的多个角度,将各个角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算各个角度对应的占用概率值,循环执行上述S2,直至遍历完上述预设区域范围。
在优选实施过程中,利用获取的图像数据与预先建立的视觉词典、视觉地图初步解算获取机器人的初始位姿数据,之后再利用获取的激光点云数据与预先建立的激光地图,基于上述初始位姿在预定区域范围内进行精确位姿解算。缩短了激光在大范围地图搜索的时间,并且利用视觉初始值可以减少激光匹配的迭代范围,加快匹配速度。
优选地,上述步骤S2可以进一步包括:
1、从所述初始点出发,按照所述预定步长(例如,5厘米的步长)和预定方向向初始点周边多个方向(例如,东、南、西、北、东北、西北、西南、东南八个方向)扩展;
2、确定所述初始点对应的占用概率值中最大占用概率值对应的角度方向,以该角度方向为中心确定第一角度范围(例如,3°的角度范围),优选地,所述第一角度范围通常小于所述初始点对应的多个角度所构成的角度范围(例如,5°的角度范围);当然,第一角度范围也可以等于或者大于所述初始点对应的多个角度所构成的角度范围。
对于扩展后的下一级位置点中每个位置点,在所述第一角度范围内确定多个角度(例如,每个角度为0.5°),获取各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与所述初始点对应的占用概率值中最大占用概率值进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或者等于时,将该位置点作为可扩展点,将可扩展点对应的所有占用概率值中最大占用概率值设置为当前最优解;
从所述可扩展点出发,按照所述预定步长和预定方向向初始点周边多个方向扩展,确定能够扩展的下一级新位置点,确定所述当前最优解对应的角度方向,以该角度方向为中心确定第二角度范围(例如,2°的角度范围),优选地,第二角度范围通常小于第一角度范围,当然,第二角度范围也可以大于或者等于第一角度范围。对于所述下一级新位置点中每个位置点,在所述第二角度范围内确定多个角度(例如,每个角度为0.5°),获取各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与所述当前最优解进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或等于时,将该位置点设置为所述可扩展点,并将所述可扩展点对应的占用概率值中最大的占用概率值设置为所述当前最优解,循环执行此步骤,直至遍历完所述预设区域范围。
基于机器人的初始位姿数据,在预定区域范围内进行精确位姿解算的过程中,由于搜索解算时考虑舍弃了一部分位置点,每次都以当前最优解对应的角度方向为中心确定的角度范围内进行搜索,而且不断缩小搜索角度的范围,因此进一步缩短了搜索时间,大大提高了位姿解算效率,有效加快了匹配速度。
优选地,可以通过以下方式计算每个位置点上每个角度对应的占用概率值P:
P=(P(x1,y1)+...+P(xi,yi)+...+P(xn,yn))/n
其中,(xi,yi)表示第i个点云数据点在栅格中的坐标,P(xi,yi)表示第i个点云数据点的栅格占用概率,n为扫描点云数据点的个数。
优选地,在获取所述机器人的第二位姿数据之后,还可以包括以下处理:确定所述第一位姿数据对应的视觉三维点以及与所述视觉三维点对应的像素坐标;确定所述第二位姿数据对应的激光数据以及激光地图;根据所述视觉三维点、所述像素坐标、所述激光数据、所述激光地图、以及视觉传感器位姿数据,构建视觉和激光耦合的非线性图;对所述非线性图进行优化,得到接近最或然值的第三位姿数据,作为所述机器人的最终位姿数据。
在优选实施过程中,步骤S104之后,为了获取更精准的机器人位姿数据,可以采用视觉和激光的耦合方式,基于第一位姿数据和第二位姿数据进行联合优化,具体地,采用上述第一位姿数据,获取第一位姿数据对应的视觉三维点以及与所述视觉三维点对应的像素坐标;采用上述第二位姿数据,获取第二位姿数据对应的激光数据和激光地图,构建视觉和激光耦合的非线性图,其中,采用上述视觉三维点、像素坐标、激光数据和激光地图、视觉传感器(例如,相机)位姿数据为非线性图的顶点,基于视觉和激光物理规则(例如,投影关系),建立顶点之间的连接边,最后使用图优化方式(例如,ceres优化方式)联合视觉和激光共同优化,得到接近最或然值的第三位姿数据,作为机器人最终重定位的位姿数据。当然,也可以采用其他的优化方式,例如最小二乘法等。
以下结合图2进一步描述上述优选实施方式。
图2是根据本发明优选实施例的机器人重定位方法的流程图。如图2所示,该机器人重定位方法包括:
步骤S201:在建图时预先建立视觉词典、视觉地图和激光地图,其中,视觉词典保存图像关键帧的特征向量,视觉地图保存图像关键帧的索引标识信息、物理位置信息和特征点信息,激光地图保存采用激光传感器建立的占用栅格地图。
步骤S202:使用S201建立的地图,接收机器人在当前位置获取的激光数据和图像数据。
步骤S203:使用当前位置对应的图像数据与步骤S201建立的视觉词典和视觉地图,对上述图像数据提取特征向量,与视觉词典中的特征向量进行匹配,获取匹配度最高的图像关键帧,在视觉地图中通过索引标识信息(例如,第10帧)获得与当前位置相似度最高的视觉地图关键帧所在的物理位置信息(例如,三维位置坐标)。
步骤S204,根据上述图像数据和视觉地图的特征点,通过计算特征相似度的方法进行匹配,通过视觉地图中特征点的物理位置信息和当前图像特征点的像素位置,采用PnP方法解算获得机器人视觉重定位的位姿数据。
步骤S205:以步骤S204获得的位姿数据作为基础,利用激光点云数据与步骤S201建立的激光地图进行迭代匹配,获取机器人重定位的位姿数据。具体如下:
根据上述第一位姿数据设定预定区域范围,例如,以第一位姿数据的位置为圆心,以预定长度(例如,50厘米)为半径构建圆形区域范围,或者以第一位姿数据的位置为中心点,构建矩形区域范围等,在上述区域范围内,以预定的步长(例如,5厘米)和预定的角度(例如,0.5°)进行变化,计算获取多个占用概率值,在所有获取到的占用概率值中选取最大的占用概率值,将上述最大的占用概率值对应的机器人位姿数据确定为上述第二位姿数据。
具体地,可以通过以下方式计算每个位置点上每个角度对应的占用概率值P:
P=(P(x1,y1)+...+P(xi,yi)+...+P(xn,yn))/n
其中,(xi,yi)表示第i个点云数据点在栅格中的坐标,P(xi,yi)表示第i个点云数据点的栅格占用概率,n为扫描点云数据点的个数。
在优选实施过程中,将上述第一位姿数据中的位置点作为初始点,选取该位置点对应的多个角度(例如,11个角度方向,正前方一个角度方向,以正前方为基准向左偏5个角度,向右偏5个角度,每个角度之间间隔0.5°),对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算每个角度对应的占用概率值,将获取到的占用概率值中最大的占用概率值作为当前最优解,按照5厘米的步长向该初始点四周预定的8个方向扩展,确定初始点对应的占用概率值中最大占用概率值对应的角度方向,以该角度方向为中心确定3°的角度范围,则有7个角度方向,其中一个是最大占用概率值对应的角度方向,以最大占用概率值对应的角度方向为为基准向左偏3个角度,向右偏3个角度,每个角度之间间隔0.5°。
对于扩展后的下一级位置点中每个位置点,获取上述各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与上述初始点对应的占用概率值中最大占用概率值进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或者等于时,将该位置点设置为上述可扩展点,并将上述可扩展点对应的占用概率值中最大的占用概率值设置为上述当前最优解。
从所述可扩展点出发,按照5厘米步长向初始点周边8个方向扩展,确定能够扩展的下一级新位置点,确定所述当前最优解对应的角度方向,以该角度方向为中心确定2°的角度范围,对于所述下一级新位置点中每个位置点,在2°的角度范围内确定多个角度,则有5个角度方向,其中一个是最大占用概率值对应的角度方向,以最大占用概率值对应的角度方向为为基准向左偏2个角度,向右偏2个角度,每个角度之间间隔0.5°。获取各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与所述当前最优解进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或等于时,将该位置点设置为所述可扩展点,并将所述可扩展点对应的占用概率值中最大的占用概率值设置为所述当前最优解,循环执行此步骤,直至遍历完上述预设区域范围。在遍历完上述预设区域范围之后,在所有获取到的占用概率值中选取最大的占用概率值,将上述最大的占用概率值对应的机器人位姿数据确定为上述机器人重定位的位姿数据。
根据本发明实施例,还提供了一种机器人重定位装置。
图3是根据本发明实施例的机器人重定位装置的结构框图。如图3所示,该机器人重定位装置包括:接收模块30,用于接收机器人在当前位置获取的激光点云数据和图像数据;确定模块32,用于根据预先建立的视觉词典确定上述图像数据对应的图像关键帧信息;第一解算模块34,用于在预先建立的视觉地图中获取与上述图像关键帧信息对应的物理位置信息,将上述图像数据的特征点信息与上述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及上述图像数据中特征点的像素位置信息进行解算;第二解算模块36,用于在所述第一解算模块解算获取所述机器人的第一位姿数据的情况下,根据上述第一位姿数据设定预定区域范围,在上述预定区域范围内使用预先建立的激光地图以及上述激光点云数据进行迭代匹配计算,获取上述机器人的第二位姿数据。
在图3所示的装置中,确定模块32结合视觉词典确定机器人在当前位置获取的图像数据对应的图像关键帧信息,第一解算模块34结合视觉地图获取与上述图像关键帧信息对应的物理位置信息,使用上述图像数据的特征点信息与上述图像关键帧信息对应的特征点信息匹配后的视觉地图中特征点的物理位置信息以及上述图像数据中特征点的像素位置信息,解算获取上述机器人的初始位姿数据,第二解算模块36根据初始位姿数据设定预定区域范围,在预定区域范围内遍历,使用预先建立的激光地图以及上述激光点云数据进行迭代匹配计算获取上述机器人重定位的位姿数据。采用上述装置,尤其针对复杂大场景的环境,无需在全地图查找,缩短了使用激光传感器在大场景范围内搜索的时间,利用初始位姿数据可以减少激光匹配的迭代范围,加快匹配速度,快速实现机器人重定位。
优选地,如图4所示,上述第二解算模块36可以进一步包括:计算子模块360,用于在上述预设区域范围内选取多个位置点,对于每个位置点选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算该角度对应的占用概率值;确定子模块362,用于在所有获取到的占用概率值中选取最大的占用概率值,将上述最大的占用概率值对应的机器人位姿数据确定为上述第二位姿数据。
需要说明的是,上述机器人重定位装置中的各模块相互结合的优选实施方式,具体可以参见图1至图2所示的实施例中对应的相关描述和效果进行理解,此处不再赘述。
根据本发明实施例,提供了一种机器人。
图5是根据本发明实施例的机器人的结构框图。如图5所示,根据本发明的机器人包括:存储器50及处理器52,上述存储器50,用于存储计算机执行指令;上述处理器52,用于执行上述存储器存储的计算机执行指令,使得上述机器人执行如上述实施例提供的机器人重定位方法。
处理器52可以为中央处理器(Central Processing Unit,CPU)。处理器52还可以为其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable GateArray,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片,或者上述各类芯片的组合。
存储器50作为一种非暂态计算机可读存储介质,可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块,如本发明实施例中的机器人重定位方法对应的程序指令/模块。处理器通过运行存储在存储器中的非暂态软件程序、指令以及模块,从而执行处理器的各种功能应用以及数据处理。
存储器40可以包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需要的应用程序;存储数据区可存储处理器所创建的数据等。此外,存储器可以包括高速随机存取存储器,还可以包括非暂态存储器,例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中,存储器50可选包括相对于处理器远程设置的存储器,这些远程存储器可以通过网络连接至处理器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
上述一个或者多个模块存储在上述存储器50中,当被上述处理器52执行时,执行如图1和图2所示实施例中的机器人重定位方法。
上述机器人的具体细节可以对应参阅图1和图2所示的实施例中对应的相关描述和效果进行理解,此处不再赘述。
综上所述,借助本发明提供的上述实施方式,利用视觉词典和视觉地图与当前位置获取的图像数据先初步解算初始位姿,在以初始位姿为基础的前提下,在预设的区域范围内,利用当前位置获取的激光点云数据与激光地图,进行精确定位。缩短了激光在大范围地图搜索的时间,并且利用视觉初始值可以减少激光匹配的迭代范围,提高重定位效率。并且,基于机器人的初始位姿数据,在预定区域范围内进行精确位姿解算的过程中,由于搜索解算时考虑舍弃了一部分位置点,因此进一步缩短了搜索时间,大大提高了位姿解算效率,更加有效地加快了匹配速度。
以上公开的仅为本发明的几个具体实施例,但是,本发明并非局限于此,任何本领域的技术人员能思之的变化都应落入本发明的保护范围。
Claims (10)
1.一种机器人重定位方法,其特征在于,包括:
接收机器人在当前位置获取的激光点云数据和图像数据;
根据视觉词典确定所述图像数据对应的图像关键帧信息;
在视觉地图中获取与所述图像关键帧信息对应的物理位置信息,将所述图像数据的特征点信息与所述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及所述图像数据中特征点的像素位置信息进行解算;
在解算获取所述机器人的第一位姿数据的情况下,根据所述第一位姿数据设定预定区域范围,在所述预定区域范围内使用激光地图以及所述激光点云数据进行迭代匹配计算,获取所述机器人的第二位姿数据。
2.根据权利要求1所述的方法,其特征在于,在接收机器人在当前位置获取的激光点云数据和图像数据之前,还包括:
建立所述视觉词典、所述视觉地图和所述激光地图,其中,所述视觉词典包括:图像关键帧的特征向量,所述视觉地图包括:图像关键帧的物理位置信息和特征点信息,所述激光地图包括:占用栅格地图。
3.根据权利要求1所述的方法,其特征在于,根据视觉词典确定所述图像数据对应的图像关键帧信息包括:
对所述图像数据提取第一特征向量,将所述第一特征向量与视觉词典中图像关键帧的第二特征向量进行匹配,获取匹配度最高的图像关键帧信息。
4.根据权利要求1所述的方法,其特征在于,在所述预定区域范围内使用预先建立的激光地图以及所述激光点云数据进行迭代匹配计算,获取所述机器人的第二位姿数据包括:
在所述预设区域范围内选取多个位置点,对于每个位置点选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到所述占用栅格地图中,根据投影结果计算该角度对应的占用概率值;
在所有获取到的占用概率值中选取最大的占用概率值,将所述最大的占用概率值对应的机器人位姿数据确定为所述第二位姿数据。
5.根据权利要求4所述的方法,其特征在于,在所述预设区域范围内选取多个位置点,对于每个位置点选取该位置点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算该角度对应的占用概率值包括:
S1:将所述第一位姿数据中的位置点作为初始点,选取该初始点对应的多个角度,对于每个角度,将该角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算该角度对应的占用概率值;
S2:按照预定步长和方向,确定多个下一级位置点,选取所述下一级位置点中每个位置点对应的多个角度,将各个角度对应的激光点云数据投影到占用栅格地图中,根据投影结果计算各个角度对应的占用概率值,循环执行所述S2,直至遍历完所述预设区域范围。
6.根据权利要求5所述的方法,其特征在于,所述S2进一步包括:
从所述初始点出发,按照所述预定步长向初始点周边多个预定方向扩展;
确定所述初始点对应的占用概率值中最大占用概率值对应的角度方向,以该角度方向为中心确定第一角度范围;
对于扩展后的下一级位置点中每个位置点,在所述第一角度范围内确定多个角度,获取各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与所述初始点对应的占用概率值中最大占用概率值进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或者等于时,将该位置点作为可扩展点,将可扩展点对应的所有占用概率值中最大占用概率值设置为当前最优解;
从所述可扩展点出发,按照所述预定步长向初始点周边多个方向扩展,确定能够扩展的下一级新位置点,确定所述当前最优解对应的角度方向,以该角度方向为中心确定第二角度范围,对于所述下一级新位置点中每个位置点,在所述第二角度范围内确定多个角度,获取各个角度对应的占用概率值,将该位置点对应的占用概率值中最大占用概率值与所述当前最优解进行对比,在对比结果为小于时,将该位置点舍弃,在对比结果为大于或等于时,将该位置点设置为所述可扩展点,并将所述可扩展点对应的占用概率值中最大的占用概率值设置为所述当前最优解,循环执行此步骤,直至遍历完所述预设区域范围。
7.根据权利要求4至6中任一项所述的方法,其特征在于,通过以下方式计算每个位置点上每个角度对应的占用概率值P:
P=(P(x1,y1)+...+P(xi,yi)+...+P(xn,yn))/n
其中,(xi,yi)表示第i个点云数据点在栅格中的坐标,P(xi,yi)表示第i个点云数据点的栅格占用概率,n为扫描点云数据点的个数。
8.根据权利要求1所述的方法,其特征在于,获取所述机器人的第二位姿数据之后,还包括:
确定所述第一位姿数据对应的视觉三维点以及与所述视觉三维点对应的像素坐标;
确定所述第二位姿数据对应的激光数据以及激光地图;
根据所述视觉三维点、所述像素坐标、所述激光数据、所述激光地图、以及视觉传感器位姿数据,构建视觉和激光耦合的非线性图;
对所述非线性图进行优化,得到接近最或然值的第三位姿数据,作为所述机器人的最终位姿数据。
9.一种机器人重定位装置,其特征在于,包括:
接收模块,用于接收机器人在当前位置获取的激光点云数据和图像数据;
确定模块,用于根据视觉词典确定所述图像数据对应的图像关键帧信息;
第一解算模块,用于在视觉地图中获取与所述图像关键帧信息对应的物理位置信息,将所述图像数据的特征点信息与所述图像关键帧信息对应的特征点信息进行匹配,结合匹配后视觉地图中特征点的物理位置信息以及所述图像数据中特征点的像素位置信息进行解算;
第二解算模块,用于在所述第一解算模块解算获取所述机器人的第一位姿数据的情况下,根据所述第一位姿数据设定预定区域范围,在所述预定区域范围内使用激光地图以及所述激光点云数据进行迭代匹配计算,获取所述机器人的第二位姿数据。
10.一种机器人,包括:存储器及处理器,其特征在于,
所述存储器,用于存储计算机执行指令;
所述处理器,用于执行所述存储器存储的计算机执行指令,使得所述机器人执行如权利要求1至7中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111670051.1A CN114519817A (zh) | 2021-12-30 | 2021-12-30 | 机器人重定位方法、装置及机器人 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111670051.1A CN114519817A (zh) | 2021-12-30 | 2021-12-30 | 机器人重定位方法、装置及机器人 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114519817A true CN114519817A (zh) | 2022-05-20 |
Family
ID=81597316
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111670051.1A Pending CN114519817A (zh) | 2021-12-30 | 2021-12-30 | 机器人重定位方法、装置及机器人 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114519817A (zh) |
-
2021
- 2021-12-30 CN CN202111670051.1A patent/CN114519817A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR102607113B1 (ko) | 국지화를 수행하는 데에 사용되기 위한 방법 및 시스템 | |
Svärm et al. | City-scale localization for cameras with known vertical direction | |
US10269147B2 (en) | Real-time camera position estimation with drift mitigation in incremental structure from motion | |
US10269148B2 (en) | Real-time image undistortion for incremental 3D reconstruction | |
Paucher et al. | Location-based augmented reality on mobile phones | |
US20180315232A1 (en) | Real-time incremental 3d reconstruction of sensor data | |
CN111652934A (zh) | 定位方法及地图构建方法、装置、设备、存储介质 | |
CN104040590A (zh) | 用于估计物体的姿态的方法 | |
CN111968177B (zh) | 一种基于固定摄像头视觉的移动机器人定位方法 | |
WO2017107865A1 (zh) | 图像检索系统、服务器、数据库及相关的方法 | |
CN111080692B (zh) | 基于无序点集的处理方法、处理装置及电子设备 | |
Drost et al. | Local hough transform for 3d primitive detection | |
Sadeghi et al. | 2DTriPnP: A robust two-dimensional method for fine visual localization using Google streetview database | |
CN114742894A (zh) | 一种大场景下多相机标定方法、信息处理终端及存储介质 | |
CN111145293B (zh) | 用于重建户型图轮廓的方法、装置及电子设备 | |
Sui et al. | An accurate indoor localization approach using cellphone camera | |
Li et al. | Quantized self-supervised local feature for real-time robot indirect VSLAM | |
Jiang et al. | 3D reconstruction of spherical images: a review of techniques, applications, and prospects | |
CN114519817A (zh) | 机器人重定位方法、装置及机器人 | |
CN116642492A (zh) | 一种移动机器人重定位方法、装置及移动机器人 | |
Huang et al. | Image-based localization for indoor environment using mobile phone | |
CN115330861A (zh) | 基于物体平面共同表示和语义描述符匹配的重定位算法 | |
WO2022252036A1 (zh) | 障碍物信息获取方法、装置、可移动平台及存储介质 | |
Aladem | Robust real-time visual odometry for autonomous ground vehicles | |
Jiang et al. | 3D Reconstruction of Spherical Images based on Incremental Structure from Motion |
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 |