CN112509053A - 机器人位姿的获取方法、装置及电子设备 - Google Patents
机器人位姿的获取方法、装置及电子设备 Download PDFInfo
- Publication number
- CN112509053A CN112509053A CN202110167562.5A CN202110167562A CN112509053A CN 112509053 A CN112509053 A CN 112509053A CN 202110167562 A CN202110167562 A CN 202110167562A CN 112509053 A CN112509053 A CN 112509053A
- Authority
- CN
- China
- Prior art keywords
- key frame
- point
- determining
- descriptor
- current
- 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.)
- Granted
Links
Images
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/40—Scenes; Scene-specific elements in video content
- G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Image Analysis (AREA)
Abstract
本申请涉及一种机器人位姿的获取方法、装置及电子设备,所述方法包括:首先获取当前关键帧的第一扫描描述子,然后根据预先构建的场景地图中每个分离式关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度,并将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧,最后根据目标历史关键帧的位姿确定机器人当前的位姿。如此,便可以根据预先构建的场景地图中的历史关键帧与当前关键帧确定机器人当前的位姿,避开了现有技术中的限制,可以全天候获取实时位姿。
Description
技术领域
本申请涉及机器人技术领域,尤其涉及一种机器人位姿的获取方法、装置及电子设备。
背景技术
机器人在自主导航应用中,机器人的位置识别和重定位的应用是自主导航的关键,位置识别和重定位的最终目的往往是获取自身的实时位置和实时姿态(即实时位姿)。
目前,获取自身的实时位姿可以利用以下方式:比如利用全球导航卫星系统(Global Navigation Satellite System,GNSS)来获取机器人的位置,但是在卫星信号较弱的区域,就会难以与卫星实现准确交互,从而无法准确获取机器人的位置;或者可以基于WiFi、蓝牙或者UWB等进行有源定位,但是利用该方式进行定位的前提是需要假设大量的信号发射器(WiFi发射器、蓝牙发射器或者UWB发射器),在面对大范围场景时,需要较高的成本;或者基于视觉进行位置识别以及重定位,由于视觉传感器本身的缺陷,在弱光、逆光等情况下无法使用,使用限制较多,无法全天候获取实时位姿。
发明内容
为克服相关技术中存在的使用限制较多,无法全天候获取实时位姿问题,本申请提供一种机器人位姿的获取方法、装置及电子设备。
根据本申请的第一方面,提供一种机器人位姿的获取方法,所述方法包括:
获取当前关键帧的第一扫描描述子;
根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度;
将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
根据所述目标历史关键帧的位姿确定所述机器人当前的位姿。
在一个可选的实施方式中,所述获取当前关键帧的第一扫描描述子,包括:
获取当前关键帧的局部特征点云数据;
根据所述局部特征点云数据中点的原始坐标在预设坐标系中确定图形区域,所述预设坐标系设置有第一坐标维度和第二坐标维度;
将所述图形区域同时在第一坐标维度和第二坐标维度进行均分,得到多个子区域,所述每个子区域在第一坐标维度上具有第一范围,在第二坐标维度上具有第二范围;
根据所述局部特征点云数据中所有点的原始坐标以及所述第一范围和第二范围确定各子区域的特征值;
根据各子区域的特征值确定所述当前关键帧的第一扫描描述子。
在一个可选的实施方式中,所述根据所述局部特征点云数据中所有点的原始坐标以及所述第一范围和第二范围确定各子区域的特征值,包括:
根据所述局部特征点云数据中点的原始坐标以及第一范围和第二范围确定各子区域中包含的点;
根据每个子区域中的点的原始坐标以及每个子区域中点的数量分别对每个子区域进行特征映射,对应得到每个子区域的特征值。
在一个可选的实施方式中,所述根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度,包括:
根据所述第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定所述当前关键帧与所述每个历史关键帧的视角偏移量;
基于所述视角偏移量以及第一扫描描述子、所述第二扫描描述子,对所述当前关键帧与所述历史关键帧进行相似度计算,得到每个所述历史关键帧与所述当前关键帧的相似度。
在一个可选的实施方式中,所述根据所述第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定所述当前关键帧与所述每个历史关键帧的视角偏移量,包括:
构建第一扫描描述子以及F个第二扫描描述子各自的二值矩阵;
按照预设的移动策略对第f个第二扫描描述子的二值矩阵进行列移动,其中,f=1,2,3,……,F,F为正整数;
在每次进行列移动后,计算进行列移动后的第f个第二扫描描述子的二值矩阵和第一扫描描述子的二值矩阵的几何相似度;
根据计算得到的几何相似度确定满足预设条件的进行列移动后的第f个第二扫描描述子的二值矩阵为目标二值矩阵;
根据所述目标二值矩阵与所述第f个第二扫描描述子的未进行列移动时的二值矩阵确定所述当前关键帧与第f个第二扫描描述子对应的历史关键帧的视角偏移量。
在一个可选的实施方式中,所述基于所述视角偏移量以及第一扫描描述子、所述第二扫描描述子,对所述当前关键帧与所述历史关键帧进行相似度计算,得到每个所述历史关键帧与所述当前关键帧的相似度,包括:
根据第f个第二扫描描述子与第一扫描描述子的视角偏移量调整所述第f个第二扫描描述子中的特征值顺序,f=1,2,3,……,F,F为正整数;
计算调整后的第f个第二扫描描述子与第一扫描描述子的相似度,并将计算得到的相似度作为第f个历史关键帧与当前关键帧的相似度。
在一个可选的实施方式中,所述根据所述目标历史关键帧的位姿确定所述机器人当前的位姿,包括:
利用最近邻搜索算法根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧与所述目标历史关键帧中具有对应关系的点对;
根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿;
将满足预设条件的传播位姿确定为所述机器人当前的位姿。
在一个可选的实施方式中,所述利用最近邻搜索算法根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧与所述目标历史关键帧中具有对应关系的点对,包括:
根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧的点云数据中的每个点的初始坐标;
根据所述初始坐标确定所述当前关键帧中每个点的第一描述子矩阵;
根据预获取的所述相似关键帧的全局位姿确定所述相似关键帧中每个点的全局坐标;
根据所述全局坐标确定所述相似关键帧中每个点的第二描述子矩阵;
利用所述当前关键帧中每个点的第一描述子矩阵和所述相似关键帧中每个点的第二描述子矩阵进行最近邻搜索,确定搜索当前关键帧中的点与所述相似关键帧中的点之间的对应关系;
将所述当前关键帧与所述相似关键帧中具有所述对应关系的两个点确定为一组点对。
在一个可选的实施方式中,所述根据所述初始坐标确定所述当前关键帧中每个点的第一描述子矩阵,包括:
以所述当前关键帧的第ic个点为中心、第jc个预设邻域半径为邻域半径,构建第jc个坐标范围,其中ic=1,2,3,……,Nc,jc=1,2,3,……,Jc,Nc、Jc为正整数;
根据所述每个点的初始坐标将处于所述第jc个坐标范围内的所有点对应确定为第jc个目标集合;
根据第jc个目标集合确定第ic个点的第jc个协方差矩阵;
对第ic个点的第jc个协方差矩阵进行奇异值分解,得到第ic个点的第jc个特征组;
根据第ic个点的所有特征组,确定第ic个点的第一描述子矩阵。
在一个可选的实施方式中,根据所述全局坐标确定所述相似关键帧中每个点的第二描述子矩阵,包括:
以所述目标历史关键帧的的第iq个点为中心、第jq个预设邻域半径为邻域半径,构建第jq个坐标范围,其中iq=1,2,3,……,Nq,jq=1,2,3,……,Jq,Nq、Jq为正整数;
根据所述每个点的初始坐标将处于所述第jq个坐标范围内的所有点对应确定为第jq个目标集合;
根据第jq个目标集合确定第iq个点的第jq个协方差矩阵;
对第iq个点的第jq个协方差矩阵进行奇异值分解,得到第iq个点的第jq个特征组;
根据第iq个点的所有特征组,确定第iq个点的第二描述子矩阵。
在一个可选的实施方式中,所述根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿,包括:
计算第m个点对中属于所述当前关键帧的点与当前关键帧其他点中的第ic个点的第一距离以及第一夹角,m=1,2,……,M,ic=1,2,3,……,Nc,M、Nc为正整数;
以第m个对应点对中属于所述目标历史关键帧的点为中心,所述第一距离为邻域半径进行搜索,得到所述目标历史关键帧中的至少一个目标历史点;
计算第m个对应点对中属于所述目标历史关键帧的点与每个目标历史点的第二距离以及第二夹角;
将满足预设搜索条件的所述第二距离和所述第二夹角对应的目标历史点确定为所述当前关键帧中第ic个点的对应点;
将所有对应点的三维坐标以及具有对应点的当前关键帧中的点的三维坐标输入到描述一致传播算法中,得到所述第m个对应点对的传播位姿。
在一个可选的实施方式中,所述将满足预设条件的传播位姿确定为所述机器人当前的位姿,包括:
根据第m个点对的传播位姿将当前关键帧中每个点的原始坐标转换到全局坐标系中,得到当前关键帧中每个点的全局坐标;
根据当前关键帧中每个点的全局坐标确定当前关键帧中每个点与目标历史关键帧中最邻近点的距离;
根据当前关键帧中每个点与目标历史关键帧中最邻近点的距离确定第m个点对的传播位姿下的匹配误差;
将满足预设条件的匹配误差对应的传播位姿确定为所述机器人当前的位姿。
根据本申请的第二方面,提供一种机器人位姿的获取装置,所述装置包括:
获取模块,用于获取当前关键帧的第一扫描描述子;
第一确定模块,用于根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度;
第二确定模块,用于将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
第三确定模块,用于根据所述目标历史关键帧的位姿确定所述机器人当前的位姿。
根据本申请的第三方面,提供一种电子设备,包括:至少一个处理器和存储器;
所述处理器用于执行所述存储器中存储的机器人位姿的获取程序,以实现本申请第一方面所述的机器人位姿的获取方法。
本申请提供的技术方案可以包括以下有益效果:首先获取当前关键帧的第一扫描描述子,然后根据预先构建的场景地图中每个分离式关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度,并将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧,最后根据目标历史关键帧的位姿确定机器人当前的位姿。如此,便可以根据预先构建的场景地图中的历史关键帧与当前关键帧确定机器人当前的位姿,避开了现有技术中的限制,可以全天候获取实时位姿。
应当理解的是,以上的一般描述和后文的细节描述仅是示例性和解释性的,并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本申请的实施例,并与说明书一起用于解释本申请的原理。
图1是本申请的一个实施例提供的一种机器人位姿的获取方法的流程示意图;
图2是本申请的一个实施例提供的确定当前关键帧的第一扫描描述子的流程示意图;
图3为本申请的一个实施例中图形区域与子区域的示意图;
图4是本申请的一个实施例确定视角偏移量的流程示意图;
图5是本申请的一个实施例提供的确定历史关键帧与当前关键帧的相似度的流程示意图;
图6是本申请的一个实施例提供的确定机器人当前的位姿的流程示意图;
图7为本申请的另一实施例提供的一种机器人位姿的获取装置的结构示意图;
图8是本申请的另一实施例提供的一种电子设备的结构示意图。
具体实施方式
这里将详细地对示例性实施例进行说明,其示例表示在附图中。下面的描述涉及附图时,除非另有表示,不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反,它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
请参阅图1,图1是本申请的一个实施例提供的一种机器人位姿的获取方法的流程示意图。
如图1所示,本实施例提供的机器人位姿的获取方法可以包括:
步骤S101、获取当前关键帧的第一扫描描述子。
本步骤中,当前关键帧是机器人利用三维激光雷达进行实时扫描得到的一个扫描帧,该当前关键帧中包含了利用三维激光雷达到描得到的点云数据,利用当前关键帧中的点云数据得到当前关键帧的第一扫描描述子的过程可以参阅图2,图2是本申请的一个实施例提供的确定当前关键帧的第一扫描描述子的流程示意图。
如图2所示,本实施例提供的确定当前关键帧的第一扫描描述子的过程可以包括:
步骤S201、获取当前关键帧的局部特征点云数据。
当前关键帧的局部点云数据可以包含属于当前关键帧中的所有点的原始坐标,原始坐标一般是以三维坐标的形式体现的,比如某个点的原始坐标为(x,y,z)。
步骤S202、根据局部特征点云数据中点的原始坐标在预设坐标系中确定图形区域,预设坐标系设置有第一坐标维度和第二坐标维度。
需要说明的是,预设坐标系可以是平面坐标,也可以是极坐标,也可以是具有两个坐标维度的其他坐标,为了将图形区域与实际的区域表达的更为相近,本实施例可以采用极坐标的形式,按照如下公式将原始坐标转换为极坐标:
极坐标中的第一坐标维度可以是表示距离的坐标维度,第二坐标维度可以是表示角度的坐标维度,为了将当前关键帧中的所有点都表示到图形区域中,本实施例可以以极坐标原点为圆心,当前关键帧中点的最大的值为半径生成圆形,如图3所示,图3为本申请的一个实施例中图形区域与子区域的示意图。
步骤S203、将图形区域同时在第一坐标维度和第二坐标维度进行均分,得到多个子区域,每个子区域在第一坐标维度上具有第一范围,在第二坐标维度上具有第二范围。
由于第一坐标维度是表示距离的坐标维度,第二坐标维度是表示角度的坐标维度,那么本步骤中便可以是以离圆形等距离递增的生成同心圆,实心在第一坐标维度进行均分,然后再将生成的圆形等角度划分为预设数量个扇形,扇形和同心圆就会构成多个子区域,其中,每个子区域在距离上具有第一范围,在角度上具有第二范围。
如图3所示的子区域Sij的第一范围即为图中的第i个圆环到第i+1个圆环,第二范围则为第j个扇形到第j+1个扇形。
步骤S204、根据局部特征点云数据中所有点的原始坐标以及第一范围和第二范围确定各子区域的特征值。
首先,根据局部特征点云数据中点的原始坐标以及第一范围和第二范围确定各子区域中包含的点,具体的,根据前述关系,设极坐标系中距离最大值为Dmax,为圆环序号,为扇形序号,则子区域Sij表示序号为i的圆环和序号为j的扇形的相交区域,子区域Sij中的点可表示为:
接着,根据每个子区域中的点的原始坐标以及每个子区域中点的数量分别对每个子区域进行特征映射,对应得到每个子区域的特征值。
由于点云不可避免存在噪声点,利用高度值z,通过距离聚类方法对子区域中的点进行聚类并滤波离群点,计算可得到该子区域中点云的高度最大值和点数目,通过比较所有子区域的高度最大值和点数目,便可以得到全局高度最大值Zmin和全局高度最小值Zmax,以及全局点数目最大值Nmin和全局点数目最小值Nmax。则每个子区域的特征值便可以是:
步骤S205、根据各子区域的特征值确定当前关键帧的第一扫描描述子。
步骤S102、根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度。
需要说明的是,预先构建的场景地图中每个历史关键帧对应的第二扫描描述子的确定过程可以参考前述图2示例的确定第一扫描描述子的过程,其区别是历史关键帧的局部特征点云数据需要包含历史关键帧前后预设数量个关键帧的点云数据。
由于采用不同三维激光雷达,获得的点云数据的疏密程度不同,比如16线的三维激光雷达获得的点云数据必然要比32线的三维激光雷达稀疏,为了降低成本,本申请可以将历史关键帧对应的点云数据以及历史关键帧之前预设数量个关键帧的点云数据共同构建为点云数据集,作为历史关键帧的局部特征点云数据,以弥补因三维激光雷达采集的数据过于稀疏所带来的弊端,因此,利用本申请的方法,可以使用16线的三维激光雷达。
一般,每个关键帧都会对应有一个位姿,以第p个历史关键帧为例,按照时间顺序,将第p个历史关键帧以及第p个历史关键帧前后预设数量个历史关键帧构建为关键帧集合,其中,p=1,2,……,P,P为正整数。
具体的,关键帧集合可以为,其中,q=1,2,3,……,Q,Q为正整数,对于Kq,其对应的位姿为,第p个关键帧的位姿为,设关键帧集合中的第Kq个关键帧中的点的局部三维坐标为,那么的全局三维坐标为:。这样,就可以将其他关键帧中的点的坐标映射到第p个历史关键帧的坐标系中了。
在对关键帧集合中的所有关键帧做了如上的坐标转换后,便可以得到第p个关键帧的局部特征点云数据。
在确定每个历史关键帧与当前关键帧的相似度时,需要先根据第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定当前关键帧与每个历史关键帧的视角偏移量,具体可以参阅图4,图4是本申请的一个实施例确定视角偏移量的流程示意图。
如图4所示,本实施例提供的确定视角偏移量的过程可以包括:
步骤S401、构建第一扫描描述子以及F个第二扫描描述子各自的二值矩阵。
步骤S402、按照预设的移动策略对第f个第二扫描描述子的二值矩阵进行列移动,其中,f=1,2,3,……,F,F为正整数。
对于同一个位置,当移动机器人发生纯旋转时,由于三维激光雷达360度的扫描特性,此时对于二维特征矩阵而言,列向量的向量值不变,但是会出现偏移;而行向量中元素的顺序会发生改变,但是行向量不会发生偏移。因此,可以对第二扫描描述子的二值矩阵中的列进行移动,比如,移动策略可以是每次将第一列移动到最后一列的位置,第二列变成第一列,以此类推。
步骤S403、在每次进行列移动后,计算进行列移动后的第f个第二扫描描述子的二值矩阵和第一扫描描述子的二值矩阵的几何相似度。
步骤S404、根据计算得到的几何相似度确定满足预设条件的进行列移动后的第f个第二扫描描述子的二值矩阵为目标二值矩阵。
由于二值矩阵的列表示方位角,因此,激光的旋转可通过矩阵的列移动进行反映,所以为了检测视角变化并进行视角对其,需通过列移动以获取最大几何相似度,最大几何相似度对应的二值矩阵即为目标二值矩阵。
步骤S405、根据目标二值矩阵与第f个第二扫描描述子的未进行列移动时的二值矩阵确定当前关键帧与第f个第二扫描描述子对应的历史关键帧的视角偏移量。
在得到视角偏移量后,便可以基于视角偏移量以及第一扫描描述子、历史关键帧对应的第二扫描描述子对当前关键帧与历史关键帧进行相似度计算,得到每个历史关键帧与当前关键帧的相似度,具体可以参阅图5,图5是本申请的一个实施例提供的确定历史关键帧与当前关键帧的相似度的流程示意图。
如图5所示,确定历史关键帧与当前关键帧的相似度的过程可以包括:
步骤S501、根据第f个第二扫描描述子与第一扫描描述子的视角偏移量调整第f个第二扫描描述子中的特征值顺序,f=1,2,3,……,F,F为正整数。
步骤S502、计算调整后的第f个第二扫描描述子与第一扫描描述子的相似度,并将计算得到的相似度作为第f个历史关键帧与当前关键帧的相似度。
按照上述过程,便可以得到所有历史关键帧与当前关键帧的相似度。
步骤S103、将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧。
本步骤中,可以将相似度最大的历史关键帧确定为目标历史关键帧,当然,有时会出现历史关键帧与当前关键帧均差距很大的情况,为了避免后期的多余计算,此处也可以设置一个阈值,将超过阈值的相似度中,最大的一个相似度对应的历史关键帧确定为目标历史关键帧。
步骤S104、根据目标历史关键帧的位姿确定机器人当前的位姿。
本步骤中,具体确定机器人当前的位姿的过程可以参阅图6,图6是本申请的一个实施例提供的确定机器人当前的位姿的流程示意图。
如图6所示,确定机器人当前的位姿的过程可以包括:
步骤S601、利用最近邻搜索算法根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧与目标历史关键帧中具有对应关系的点对。
然后根据初始坐标确定当前关键帧中每个点的第一描述子矩阵,具体的,先以当前关键帧的第ic个点为中心、第jc个预设邻域半径为邻域半径,构建第jc个坐标范围,其中ic=1,2,3,……,Nc,jc=1,2,3,……,Jc,Nc、Jc为正整数,然后跟据每个点的初始坐标将处于第jc个坐标范围内的所有点对应确定为第jc个目标集合,此处可以记为。
然后根据第jc个目标集合确定第ic个点的第jc个协方差矩阵,即:
由于点云数据在树冠、杂草等具备体状的地物处容易产生噪声,不利于特征匹配,故需要滤除,则特征值需满足以下条件:
其中,||为逻辑或运算,&&为逻辑与运算,该条件表明当前点需为线状或面状特征点。
然后根据预获取的相似关键帧的全局位姿确定相似关键帧中每个点的全局坐标,再利用上述同样的方式根据全局坐标确定相似关键帧中每个点的第二描述子矩阵。
构建第二描述子矩阵的过程具体可以有:以所述目标历史关键帧的的第iq个点为中心、第jq个预设邻域半径为邻域半径,构建第jq个坐标范围,其中iq=1,2,3,……,Nq,jq=1,2,3,……,Jq,Nq、Jq为正整数;根据所述每个点的初始坐标将处于所述第jq个坐标范围内的所有点对应确定为第jq个目标集合;根据第jq个目标集合确定第iq个点的第jq个协方差矩阵;对第iq个点的第jq个协方差矩阵进行奇异值分解,得到第iq个点的第jq个特征组;根据第iq个点的所有特征组,确定第iq个点的第二描述子矩阵。由于上述过程与构建第一描述子矩阵的过程相似,可以参考第一描述子矩阵的过程,此处不再赘述。
在得到第一描述子矩阵和第二描述子矩阵后,利用当前关键帧中每个点的第一描述子矩阵和相似关键帧中每个点的第二描述子矩阵进行最近邻搜索,确定搜索当前关键帧中的点与相似关键帧中的点之间的对应关系,再将当前关键帧与相似关键帧中具有对应关系的两个点确定为一组点对,即,其中,Nc为当前关键帧中点的数目。
步骤S602、根据点对以及预设的描述一致传播算法确定每组点对的传播位姿。
本步骤中,首先计算第m个点对中属于当前关键帧的点与当前关键帧其他点中的第ic个点的第一距离以及第一夹角,m=1,2,……,M,ic=1,2,3,……,Nc,M、Nc为正整数,其中,第一距离可以是,第一夹角可以是。其中,,为Nc当前关键帧中点的数目。
再以第m个对应点对中属于目标历史关键帧的点为中心,第一距离为邻域半径进行搜索,得到目标历史关键帧中的至少一个目标历史点,然后计算第m个对应点对中属于目标历史关键帧的点与每个目标历史点的第二距离以及第二夹角,需要说明的是,第二距离和第二夹角的计算可以参阅第一距离和第一夹角的计算,此处不再赘述。
将满足预设搜索条件的第二距离和第二夹角对应的目标历史点确定为当前关键帧中第ic个点的对应点,其中,预设搜索条件可以为:
最后,将所有对应点的三维坐标以及具有对应点的当前关键帧中的点的三维坐标输入到描述一致传播算法(RANSAC)中,得到第m个对应点对的传播位姿。
步骤S603、将满足预设条件的传播位姿确定为机器人当前的位姿。
本步骤中,首先根据第m个点对的传播位姿将当前关键帧中每个点的原始坐标转换到全局坐标系中,得到当前关键帧中每个点的全局坐标。
然后根据当前关键帧中每个点的全局坐标确定当前关键帧中每个点与目标历史关键帧中最邻近点的距离;再根据当前关键帧中每个点与目标历史关键帧中最邻近点的距离确定第m个点对的传播位姿下的匹配误差,即,其中,为目标历史关键帧中的点,当点与目标历史关键帧中最邻近点的距离小于阈值时,,否则。
请参阅图7,图7为本申请的另一实施例提供的一种机器人位姿的获取装置的结构示意图。
如图7所示,本实施例提供的机器人位姿的获取装置包括:
获取模块701,用于获取当前关键帧的第一扫描描述子;
第一确定模块702,用于根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度;
第二确定模块703,用于将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
第三确定模块704,用于根据目标历史关键帧的位姿确定机器人当前的位姿。
请参阅图8,图8是本申请的另一实施例提供的一种电子设备的结构示意图。
如图8所示,本实施例提供的电子设备800包括:至少一个处理器801、存储器802、至少一个网络接口803和其他用户接口804。电子设备800中的各个组件通过总线系统805耦合在一起。可理解,总线系统805用于实现这些组件之间的连接通信。总线系统805除包括数据总线之外,还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见,在图8中将各种总线都标为总线系统805。
其中,用户接口804可以包括显示器、键盘或者点击设备(例如,鼠标,轨迹球(trackball)、触感板或者触摸屏等。
可以理解,本发明实施例中的存储器802可以是易失性存储器或非易失性存储器,或可包括易失性和非易失性存储器两者。其中,非易失性存储器可以是只读存储器(Read-Only Memory,ROM)、可编程只读存储器 (Programmable ROM,PROM)、可擦除可编程只读存储器(Erasable PROM,EPROM)、电可擦除可编程只读存储器(Electrically EPROM,EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory,RAM),其用作外部高速缓存。通过示例性但不是限制性说明,许多形式的RAM可用,例如静态随机存取存储器(Static RAM,SRAM)、动态随机存取存储器 (Dynamic RAM,DRAM)、同步动态随机存取存储器(Synchronous DRAM,SDRAM)、双倍数据速率同步动态随机存取存储器(Double DataRate SDRAM,DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM,ESDRAM)、同步连接动态随机存取存储器(Synch link DRAM,SLDRAM)和直接内存总线随机存取存储器(Direct Rambus RAM,DRRAM)。本文描述的存储器802旨在包括但不限于这些和任意其它适合类型的存储器。
在一些实施方式中,存储器802存储了如下的元素,可执行单元或者数据结构,或者他们的子集,或者他们的扩展集:操作系统8021和第二应用程序8022。
其中,操作系统8021,包含各种系统程序,例如框架层、核心库层、驱动层等,用于实现各种基础业务以及处理基于硬件的任务。第二应用程序8022,包含各种第二应用程序,例如媒体播放器(Media Player)、浏览器(Browser)等,用于实现各种应用业务。实现本发明实施例方法的程序可以包含在第二应用程序8022中。
在本发明实施例中,通过调用存储器802存储的程序或指令,具体的,可以是第二应用程序8022中存储的程序或指令,处理器801用于执行各方法实施例所提供的方法步骤,例如包括:
获取当前关键帧的第一扫描描述子;
根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度;
将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
根据目标历史关键帧的位姿确定机器人当前的位姿。
在一个可选的实施方式中,获取当前关键帧的第一扫描描述子,包括:
获取当前关键帧的局部特征点云数据;
根据局部特征点云数据中点的原始坐标在预设坐标系中确定图形区域,预设坐标系设置有第一坐标维度和第二坐标维度;
将图形区域同时在第一坐标维度和第二坐标维度进行均分,得到多个子区域,每个子区域在第一坐标维度上具有第一范围,在第二坐标维度上具有第二范围;
根据局部特征点云数据中所有点的原始坐标以及第一范围和第二范围确定各子区域的特征值;
根据各子区域的特征值确定当前关键帧的第一扫描描述子。
在一个可选的实施方式中,根据局部特征点云数据中所有点的原始坐标以及第一范围和第二范围确定各子区域的特征值,包括:
根据局部特征点云数据中点的原始坐标以及第一范围和第二范围确定各子区域中包含的点;
根据每个子区域中的点的原始坐标以及每个子区域中点的数量分别对每个子区域进行特征映射,对应得到每个子区域的特征值。
在一个可选的实施方式中,根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度,包括:
根据第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定当前关键帧与每个历史关键帧的视角偏移量;
基于视角偏移量以及第一扫描描述子、第二扫描描述子,对当前关键帧与历史关键帧进行相似度计算,得到每个历史关键帧与当前关键帧的相似度。
在一个可选的实施方式中,根据第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定当前关键帧与每个历史关键帧的视角偏移量,包括:
构建第一扫描描述子以及F个第二扫描描述子各自的二值矩阵;
按照预设的移动策略对第f个第二扫描描述子的二值矩阵进行列移动,其中,f=1,2,3,……,F,F为正整数;
在每次进行列移动后,计算进行列移动后的第f个第二扫描描述子的二值矩阵和第一扫描描述子的二值矩阵的几何相似度;
根据计算得到的几何相似度确定满足预设条件的进行列移动后的第f个第二扫描描述子的二值矩阵为目标二值矩阵;
根据目标二值矩阵与第f个第二扫描描述子的未进行列移动时的二值矩阵确定当前关键帧与第f个第二扫描描述子对应的历史关键帧的视角偏移量。
在一个可选的实施方式中,基于视角偏移量以及第一扫描描述子、第二扫描描述子,对当前关键帧与历史关键帧进行相似度计算,得到每个历史关键帧与当前关键帧的相似度,包括:
根据第f个第二扫描描述子与第一扫描描述子的视角偏移量调整第f个第二扫描描述子中的特征值顺序,f=1,2,3,……,F,F为正整数;
计算调整后的第f个第二扫描描述子与第一扫描描述子的相似度,并将计算得到的相似度作为第f个历史关键帧与当前关键帧的相似度。
在一个可选的实施方式中,根据目标历史关键帧的位姿确定机器人当前的位姿,包括:
利用最近邻搜索算法根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧与目标历史关键帧中具有对应关系的点对;
根据点对以及预设的描述一致传播算法确定每组点对的传播位姿;
将满足预设条件的传播位姿确定为机器人当前的位姿。
在一个可选的实施方式中,利用最近邻搜索算法根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧与目标历史关键帧中具有对应关系的点对,包括:
根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧的点云数据中的每个点的初始坐标;
根据初始坐标确定当前关键帧中每个点的第一描述子矩阵;
根据预获取的相似关键帧的全局位姿确定相似关键帧中每个点的全局坐标;
根据全局坐标确定相似关键帧中每个点的第二描述子矩阵;
利用当前关键帧中每个点的第一描述子矩阵和相似关键帧中每个点的第二描述子矩阵进行最近邻搜索,确定搜索当前关键帧中的点与相似关键帧中的点之间的对应关系;
将当前关键帧与相似关键帧中具有对应关系的两个点确定为一组点对。
在一个可选的实施方式中,根据初始坐标确定当前关键帧中每个点的第一描述子矩阵,包括:
以所述当前关键帧的第ic个点为中心、第jc个预设邻域半径为邻域半径,构建第jc个坐标范围,其中ic=1,2,3,……,Nc,jc=1,2,3,……,Jc,Nc、Jc为正整数;
根据所述每个点的初始坐标将处于所述第jc个坐标范围内的所有点对应确定为第jc个目标集合;
根据第jc个目标集合确定第ic个点的第jc个协方差矩阵;
对第ic个点的第jc个协方差矩阵进行奇异值分解,得到第ic个点的第jc个特征组;
根据第ic个点的所有特征组,确定第ic个点的第一描述子矩阵。
在一个可选的实施方式中,根据全局坐标确定相似关键帧中每个点的第二描述子矩阵,包括:
以所述目标历史关键帧的的第iq个点为中心、第jq个预设邻域半径为邻域半径,构建第jq个坐标范围,其中iq=1,2,3,……,Nq,jq=1,2,3,……,Jq,Nq、Jq为正整数;
根据所述每个点的初始坐标将处于所述第jq个坐标范围内的所有点对应确定为第jq个目标集合;
根据第jq个目标集合确定第iq个点的第jq个协方差矩阵;
对第iq个点的第jq个协方差矩阵进行奇异值分解,得到第iq个点的第jq个特征组;
根据第iq个点的所有特征组,确定第iq个点的第二描述子矩阵。
在一个可选的实施方式中,所述根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿,包括:
计算第m个点对中属于所述当前关键帧的点与当前关键帧其他点中的第ic个点的第一距离以及第一夹角,m=1,2,……,M,ic=1,2,3,……,Nc,M、Nc为正整数;
以第m个对应点对中属于所述目标历史关键帧的点为中心,所述第一距离为邻域半径进行搜索,得到所述目标历史关键帧中的至少一个目标历史点;
计算第m个对应点对中属于所述目标历史关键帧的点与每个目标历史点的第二距离以及第二夹角;
将满足预设搜索条件的所述第二距离和所述第二夹角对应的目标历史点确定为所述当前关键帧中第ic个点的对应点;
将所有对应点的三维坐标以及具有对应点的当前关键帧中的点的三维坐标输入到描述一致传播算法中,得到所述第m个对应点对的传播位姿。在一个可选的实施方式中,将满足预设条件的传播位姿确定为机器人当前的位姿,包括:
根据第m个点对的传播位姿将当前关键帧中每个点的原始坐标转换到全局坐标系中,得到当前关键帧中每个点的全局坐标;
根据当前关键帧中每个点的全局坐标确定当前关键帧中每个点与目标历史关键帧中最邻近点的距离;
根据当前关键帧中每个点与目标历史关键帧中最邻近点的距离确定第m个点对的传播位姿下的匹配误差;
将满足预设条件的匹配误差对应的传播位姿确定为机器人当前的位姿。
上述本发明实施例揭示的方法可以应用于处理器801中,或者由处理器801实现。处理器801可能是一种集成电路芯片,具有信号的处理能力。在实现过程中,上述方法的各步骤可以通过处理器801中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器801可以是通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现成可编程门阵列(FieldProgrammable Gate Array, FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成,或者用译码处理器中的硬件及软件单元组合执行完成。软件单元可以位于随机存储器,闪存、只读存储器,可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器802,处理器801读取存储器802中的信息,结合其硬件完成上述方法的步骤。
可以理解的是,本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现,处理单元可以实现在一个或多个专用集成电路(ApplicationSpecific Integrated Circuits,ASIC)、数字信号处理器(Digital Signal Processing,DSP)、数字信号处理设备(DSPDevice,DSPD)、可编程逻辑设备(Programmable LogicDevice,PLD)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请功能的其它电子单元或其组合中。
对于软件实现,可通过执行本文功能的单元来实现本文的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
本发明实施例还提供了一种存储介质(计算机可读存储介质)。这里的存储介质存储有一个或者多个程序。其中,存储介质可以包括易失性存储器,例如随机存取存储器;存储器也可以包括非易失性存储器,例如只读存储器、快闪存储器、硬盘或固态硬盘;存储器还可以包括上述种类的存储器的组合。
当存储介质中一个或者多个程序可被一个或者多个处理器执行,以实现上述在机器人位姿的获取设备侧执行的机器人位姿的获取方法。
处理器用于执行存储器中存储的机器人位姿的获取程序,以实现以下在机器人位姿的获取设备侧执行的机器人位姿的获取方法的步骤:
获取当前关键帧的第一扫描描述子;
根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度;
将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
根据目标历史关键帧的位姿确定机器人当前的位姿。
在一个可选的实施方式中,获取当前关键帧的第一扫描描述子,包括:
获取当前关键帧的局部特征点云数据;
根据局部特征点云数据中点的原始坐标在预设坐标系中确定图形区域,预设坐标系设置有第一坐标维度和第二坐标维度;
将图形区域同时在第一坐标维度和第二坐标维度进行均分,得到多个子区域,每个子区域在第一坐标维度上具有第一范围,在第二坐标维度上具有第二范围;
根据局部特征点云数据中所有点的原始坐标以及第一范围和第二范围确定各子区域的特征值;
根据各子区域的特征值确定当前关键帧的第一扫描描述子。
在一个可选的实施方式中,根据局部特征点云数据中所有点的原始坐标以及第一范围和第二范围确定各子区域的特征值,包括:
根据局部特征点云数据中点的原始坐标以及第一范围和第二范围确定各子区域中包含的点;
根据每个子区域中的点的原始坐标以及每个子区域中点的数量分别对每个子区域进行特征映射,对应得到每个子区域的特征值。
在一个可选的实施方式中,根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与第一扫描描述子确定每个历史关键帧与当前关键帧的相似度,包括:
根据第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定当前关键帧与每个历史关键帧的视角偏移量;
基于视角偏移量以及第一扫描描述子、第二扫描描述子,对当前关键帧与历史关键帧进行相似度计算,得到每个历史关键帧与当前关键帧的相似度。
在一个可选的实施方式中,根据第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定当前关键帧与每个历史关键帧的视角偏移量,包括:
构建第一扫描描述子以及F个第二扫描描述子各自的二值矩阵;
按照预设的移动策略对第f个第二扫描描述子的二值矩阵进行列移动,其中,f=1,2,3,……,F,F为正整数;
在每次进行列移动后,计算进行列移动后的第f个第二扫描描述子的二值矩阵和第一扫描描述子的二值矩阵的几何相似度;
根据计算得到的几何相似度确定满足预设条件的进行列移动后的第f个第二扫描描述子的二值矩阵为目标二值矩阵;
根据目标二值矩阵与第f个第二扫描描述子的未进行列移动时的二值矩阵确定当前关键帧与第f个第二扫描描述子对应的历史关键帧的视角偏移量。
在一个可选的实施方式中,基于视角偏移量以及第一扫描描述子、第二扫描描述子,对当前关键帧与历史关键帧进行相似度计算,得到每个历史关键帧与当前关键帧的相似度,包括:
根据第f个第二扫描描述子与第一扫描描述子的视角偏移量调整第f个第二扫描描述子中的特征值顺序,f=1,2,3,……,F,F为正整数;
计算调整后的第f个第二扫描描述子与第一扫描描述子的相似度,并将计算得到的相似度作为第f个历史关键帧与当前关键帧的相似度。
在一个可选的实施方式中,根据目标历史关键帧的位姿确定机器人当前的位姿,包括:
利用最近邻搜索算法根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧与目标历史关键帧中具有对应关系的点对;
根据点对以及预设的描述一致传播算法确定每组点对的传播位姿;
将满足预设条件的传播位姿确定为机器人当前的位姿。
在一个可选的实施方式中,利用最近邻搜索算法根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧与目标历史关键帧中具有对应关系的点对,包括:
根据当前关键帧与目标历史关键帧的视角偏移量确定当前关键帧的点云数据中的每个点的初始坐标;
根据初始坐标确定当前关键帧中每个点的第一描述子矩阵;
根据预获取的相似关键帧的全局位姿确定相似关键帧中每个点的全局坐标;
根据全局坐标确定相似关键帧中每个点的第二描述子矩阵;
利用当前关键帧中每个点的第一描述子矩阵和相似关键帧中每个点的第二描述子矩阵进行最近邻搜索,确定搜索当前关键帧中的点与相似关键帧中的点之间的对应关系;
将当前关键帧与相似关键帧中具有对应关系的两个点确定为一组点对。
在一个可选的实施方式中,根据初始坐标确定当前关键帧中每个点的第一描述子矩阵,包括:
以所述当前关键帧的第ic个点为中心、第jc个预设邻域半径为邻域半径,构建第jc个坐标范围,其中ic=1,2,3,……,Nc,jc=1,2,3,……,Jc,Nc、Jc为正整数;
根据所述每个点的初始坐标将处于所述第jc个坐标范围内的所有点对应确定为第jc个目标集合;
根据第jc个目标集合确定第ic个点的第jc个协方差矩阵;
对第ic个点的第jc个协方差矩阵进行奇异值分解,得到第ic个点的第jc个特征组;
根据第ic个点的所有特征组,确定第ic个点的第一描述子矩阵。
在一个可选的实施方式中,根据所述全局坐标确定所述相似关键帧中每个点的第二描述子矩阵,包括:
以所述目标历史关键帧的的第iq个点为中心、第jq个预设邻域半径为邻域半径,构建第jq个坐标范围,其中iq=1,2,3,……,Nq,jq=1,2,3,……,Jq,Nq、Jq为正整数;
根据所述每个点的初始坐标将处于所述第jq个坐标范围内的所有点对应确定为第jq个目标集合;
根据第jq个目标集合确定第iq个点的第jq个协方差矩阵;
对第iq个点的第jq个协方差矩阵进行奇异值分解,得到第iq个点的第jq个特征组;
根据第iq个点的所有特征组,确定第iq个点的第二描述子矩阵。
在一个可选的实施方式中,所述根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿,包括:
计算第m个点对中属于所述当前关键帧的点与当前关键帧其他点中的第ic个点的第一距离以及第一夹角,m=1,2,……,M,ic=1,2,3,……,Nc,M、Nc为正整数;
以第m个对应点对中属于所述目标历史关键帧的点为中心,所述第一距离为邻域半径进行搜索,得到所述目标历史关键帧中的至少一个目标历史点;
计算第m个对应点对中属于所述目标历史关键帧的点与每个目标历史点的第二距离以及第二夹角;
将满足预设搜索条件的所述第二距离和所述第二夹角对应的目标历史点确定为所述当前关键帧中第ic个点的对应点;
将所有对应点的三维坐标以及具有对应点的当前关键帧中的点的三维坐标输入到描述一致传播算法中,得到所述第m个对应点对的传播位姿。
在一个可选的实施方式中,将满足预设条件的传播位姿确定为机器人当前的位姿,包括:
根据第m个点对的传播位姿将当前关键帧中每个点的原始坐标转换到全局坐标系中,得到当前关键帧中每个点的全局坐标;
根据当前关键帧中每个点的全局坐标确定当前关键帧中每个点与目标历史关键帧中最邻近点的距离;
根据当前关键帧中每个点与目标历史关键帧中最邻近点的距离确定第m个点对的传播位姿下的匹配误差;
将满足预设条件的匹配误差对应的传播位姿确定为机器人当前的位姿。
关于上述实施例中的装置,其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述,此处将不做详细阐述说明。
可以理解的是,上述各实施例中相同或相似部分可以相互参考,在一些实施例中未详细说明的内容可以参见其他实施例中相同或相似的内容。
需要说明的是,在本申请的描述中,术语“第一”、“第二”等仅用于描述目的,而不能理解为指示或暗示相对重要性。此外,在本申请的描述中,除非另有说明,“多个”的含义是指至少两个。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
应当理解,本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本申请各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器,磁盘或光盘等。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本申请的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本申请的限制,本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (13)
1.一种机器人位姿的获取方法,其特征在于,所述方法包括:
获取当前关键帧的第一扫描描述子;
根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度;
将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
根据所述目标历史关键帧的位姿确定所述机器人当前的位姿。
2.根据权利要求1所述的方法,其特征在于,所述获取当前关键帧的第一扫描描述子,包括:
获取当前关键帧的局部特征点云数据;
根据所述局部特征点云数据中点的原始坐标在预设坐标系中确定图形区域,所述预设坐标系设置有第一坐标维度和第二坐标维度;
将所述图形区域同时在第一坐标维度和第二坐标维度进行均分,得到多个子区域,所述每个子区域在第一坐标维度上具有第一范围,在第二坐标维度上具有第二范围;
根据所述局部特征点云数据中所有点的原始坐标以及所述第一范围和第二范围确定各子区域的特征值;
根据各子区域的特征值确定所述当前关键帧的第一扫描描述子。
3.根据权利要求2所述的方法,其特征在于,所述根据所述局部特征点云数据中所有点的原始坐标以及所述第一范围和第二范围确定各子区域的特征值,包括:
根据所述局部特征点云数据中点的原始坐标以及第一范围和第二范围确定各子区域中包含的点;
根据每个子区域中的点的原始坐标以及每个子区域中点的数量分别对每个子区域进行特征映射,对应得到每个子区域的特征值。
4.根据权利要求1所述的方法,其特征在于,所述根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度,包括:
根据所述第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定所述当前关键帧与所述每个历史关键帧的视角偏移量;
基于所述视角偏移量以及第一扫描描述子、所述第二扫描描述子,对所述当前关键帧与所述历史关键帧进行相似度计算,得到每个所述历史关键帧与所述当前关键帧的相似度。
5.根据权利要求4所述的方法,其特征在于,所述根据所述第一扫描描述子与预先构建的场景地图中每个历史关键帧对应的第二扫描描述子确定所述当前关键帧与所述每个历史关键帧的视角偏移量,包括:
构建第一扫描描述子以及F个第二扫描描述子各自的二值矩阵;
按照预设的移动策略对第f个第二扫描描述子的二值矩阵进行列移动,其中,f=1,2,3,……,F,F为正整数;
在每次进行列移动后,计算进行列移动后的第f个第二扫描描述子的二值矩阵和第一扫描描述子的二值矩阵的几何相似度;
根据计算得到的几何相似度确定满足预设条件的进行列移动后的第f个第二扫描描述子的二值矩阵为目标二值矩阵;
根据所述目标二值矩阵与所述第f个第二扫描描述子的未进行列移动时的二值矩阵确定所述当前关键帧与第f个第二扫描描述子对应的历史关键帧的视角偏移量。
6.根据权利要求5所述的方法,其特征在于,所述基于所述视角偏移量以及第一扫描描述子、所述第二扫描描述子,对所述当前关键帧与所述历史关键帧进行相似度计算,得到每个所述历史关键帧与所述当前关键帧的相似度,包括:
根据第f个第二扫描描述子与第一扫描描述子的视角偏移量调整所述第f个第二扫描描述子中的特征值顺序,f=1,2,3,……,F,F为正整数;
计算调整后的第f个第二扫描描述子与第一扫描描述子的相似度,并将计算得到的相似度作为第f个历史关键帧与当前关键帧的相似度。
7.根据权利要求4所述的方法,其特征在于,所述根据所述目标历史关键帧的位姿确定所述机器人当前的位姿,包括:
利用最近邻搜索算法根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧与所述目标历史关键帧中具有对应关系的点对;
根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿;
将满足预设条件的传播位姿确定为所述机器人当前的位姿。
8.根据权利要求7所述的方法,其特征在于,所述利用最近邻搜索算法根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧与所述目标历史关键帧中具有对应关系的点对,包括:
根据所述当前关键帧与所述目标历史关键帧的视角偏移量确定所述当前关键帧的点云数据中的每个点的初始坐标;
根据所述初始坐标确定所述当前关键帧中每个点的第一描述子矩阵;
根据预获取的所述相似关键帧的全局位姿确定所述相似关键帧中每个点的全局坐标;
根据所述全局坐标确定所述相似关键帧中每个点的第二描述子矩阵;
利用所述当前关键帧中每个点的第一描述子矩阵和所述相似关键帧中每个点的第二描述子矩阵进行最近邻搜索,确定搜索当前关键帧中的点与所述相似关键帧中的点之间的对应关系;
将所述当前关键帧与所述相似关键帧中具有所述对应关系的两个点确定为一组点对。
9.根据权利要求8所述的方法,其特征在于,所述根据所述初始坐标确定所述当前关键帧中每个点的第一描述子矩阵,包括:
以所述当前关键帧的第ic个点为中心、第jc个预设邻域半径为邻域半径,构建第jc个坐标范围,其中ic=1,2,3,……,Nc,jc=1,2,3,……,Jc,Nc、Jc为正整数;
根据所述每个点的初始坐标将处于所述第jc个坐标范围内的所有点对应确定为第jc个目标集合;
根据第jc个目标集合确定第ic个点的第jc个协方差矩阵;
对第ic个点的第jc个协方差矩阵进行奇异值分解,得到第ic个点的第jc个特征组;
根据第ic个点的所有特征组,确定第ic个点的第一描述子矩阵。
10.根据权利要求8所述的方法,其特征在于,所述根据所述点对以及预设的描述一致传播算法确定每组点对的传播位姿,包括:
计算第m个点对中属于所述当前关键帧的点与当前关键帧其他点中的第ic个点的第一距离以及第一夹角,m=1,2,……,M,ic=1,2,3,……,Nc,M、Nc为正整数;
以第m个对应点对中属于所述目标历史关键帧的点为中心,所述第一距离为邻域半径进行搜索,得到所述目标历史关键帧中的至少一个目标历史点;
计算第m个对应点对中属于所述目标历史关键帧的点与每个目标历史点的第二距离以及第二夹角;
将满足预设搜索条件的所述第二距离和所述第二夹角对应的目标历史点确定为所述当前关键帧中第ic个点的对应点;
将所有对应点的三维坐标以及具有对应点的当前关键帧中的点的三维坐标输入到描述一致传播算法中,得到所述第m个对应点对的传播位姿。
11.根据权利要求8所述的方法,其特征在于,所述将满足预设条件的传播位姿确定为所述机器人当前的位姿,包括:
根据第m个点对的传播位姿将当前关键帧中每个点的原始坐标转换到全局坐标系中,得到当前关键帧中每个点的全局坐标;
根据当前关键帧中每个点的全局坐标确定当前关键帧中每个点与目标历史关键帧中最邻近点的距离;
根据当前关键帧中每个点与目标历史关键帧中最邻近点的距离确定第m个点对的传播位姿下的匹配误差;
将满足预设条件的匹配误差对应的传播位姿确定为所述机器人当前的位姿。
12.一种机器人位姿的获取装置,其特征在于,所述装置包括:
获取模块,用于获取当前关键帧的第一扫描描述子;
第一确定模块,用于根据预先构建的场景地图中每个历史关键帧对应的第二扫描描述子与所述第一扫描描述子确定每个历史关键帧与所述当前关键帧的相似度;
第二确定模块,用于将满足预设条件的相似度对应的历史关键帧确定为目标历史关键帧;
第三确定模块,用于根据所述目标历史关键帧的位姿确定所述机器人当前的位姿。
13.一种电子设备,其特征在于,包括:至少一个处理器和存储器;
所述处理器用于执行所述存储器中存储的机器人位姿的获取程序,以实现权利要求1-11任一项所述的机器人位姿的获取方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110167562.5A CN112509053B (zh) | 2021-02-07 | 2021-02-07 | 机器人位姿的获取方法、装置及电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110167562.5A CN112509053B (zh) | 2021-02-07 | 2021-02-07 | 机器人位姿的获取方法、装置及电子设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112509053A true CN112509053A (zh) | 2021-03-16 |
CN112509053B CN112509053B (zh) | 2021-06-04 |
Family
ID=74953203
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110167562.5A Active CN112509053B (zh) | 2021-02-07 | 2021-02-07 | 机器人位姿的获取方法、装置及电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112509053B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113008274A (zh) * | 2021-03-19 | 2021-06-22 | 奥特酷智能科技(南京)有限公司 | 车辆初始化定位方法、系统及计算机可读介质 |
CN113792699A (zh) * | 2021-09-24 | 2021-12-14 | 北京易航远智科技有限公司 | 一种基于语义点云的对象级快速场景识别方法 |
WO2022247045A1 (zh) * | 2021-05-28 | 2022-12-01 | 浙江大学 | 一种基于激光雷达信息的移动机器人位置重识别方法 |
CN115685223A (zh) * | 2022-12-15 | 2023-02-03 | 深圳市智绘科技有限公司 | 位置识别方法、装置、电子设备及可读存储介质 |
CN116883502A (zh) * | 2023-09-05 | 2023-10-13 | 深圳市智绘科技有限公司 | 相机位姿和路标点位置的确定方法、装置、介质及设备 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109425365A (zh) * | 2017-08-23 | 2019-03-05 | 腾讯科技(深圳)有限公司 | 激光扫描设备标定的方法、装置、设备及存储介质 |
CN110308459A (zh) * | 2019-06-30 | 2019-10-08 | 南京理工大学 | 不依赖模型的非合作卫星相对位姿测量方法 |
CN110645986A (zh) * | 2019-09-27 | 2020-01-03 | Oppo广东移动通信有限公司 | 定位方法及装置、终端、存储介质 |
CN111311588A (zh) * | 2020-02-28 | 2020-06-19 | 浙江商汤科技开发有限公司 | 重定位方法及装置、电子设备和存储介质 |
US20200226782A1 (en) * | 2018-05-18 | 2020-07-16 | Boe Technology Group Co., Ltd. | Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database |
CN111429517A (zh) * | 2020-03-23 | 2020-07-17 | Oppo广东移动通信有限公司 | 重定位方法、重定位装置、存储介质与电子设备 |
CN111784776A (zh) * | 2020-08-03 | 2020-10-16 | Oppo广东移动通信有限公司 | 视觉定位方法及装置、计算机可读介质和电子设备 |
-
2021
- 2021-02-07 CN CN202110167562.5A patent/CN112509053B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109425365A (zh) * | 2017-08-23 | 2019-03-05 | 腾讯科技(深圳)有限公司 | 激光扫描设备标定的方法、装置、设备及存储介质 |
US20200226782A1 (en) * | 2018-05-18 | 2020-07-16 | Boe Technology Group Co., Ltd. | Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database |
CN110308459A (zh) * | 2019-06-30 | 2019-10-08 | 南京理工大学 | 不依赖模型的非合作卫星相对位姿测量方法 |
CN110645986A (zh) * | 2019-09-27 | 2020-01-03 | Oppo广东移动通信有限公司 | 定位方法及装置、终端、存储介质 |
CN111311588A (zh) * | 2020-02-28 | 2020-06-19 | 浙江商汤科技开发有限公司 | 重定位方法及装置、电子设备和存储介质 |
CN111429517A (zh) * | 2020-03-23 | 2020-07-17 | Oppo广东移动通信有限公司 | 重定位方法、重定位装置、存储介质与电子设备 |
CN111784776A (zh) * | 2020-08-03 | 2020-10-16 | Oppo广东移动通信有限公司 | 视觉定位方法及装置、计算机可读介质和电子设备 |
Non-Patent Citations (2)
Title |
---|
李莹莹: "图像局部特征描述子的构建研究", 《中国博士学位论文全文数据库 信息科技辑》 * |
王洪姣: "图像质量评价方法的研究及实现", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113008274A (zh) * | 2021-03-19 | 2021-06-22 | 奥特酷智能科技(南京)有限公司 | 车辆初始化定位方法、系统及计算机可读介质 |
WO2022247045A1 (zh) * | 2021-05-28 | 2022-12-01 | 浙江大学 | 一种基于激光雷达信息的移动机器人位置重识别方法 |
CN113792699A (zh) * | 2021-09-24 | 2021-12-14 | 北京易航远智科技有限公司 | 一种基于语义点云的对象级快速场景识别方法 |
CN113792699B (zh) * | 2021-09-24 | 2024-03-12 | 北京易航远智科技有限公司 | 一种基于语义点云的对象级快速场景识别方法 |
CN115685223A (zh) * | 2022-12-15 | 2023-02-03 | 深圳市智绘科技有限公司 | 位置识别方法、装置、电子设备及可读存储介质 |
CN116883502A (zh) * | 2023-09-05 | 2023-10-13 | 深圳市智绘科技有限公司 | 相机位姿和路标点位置的确定方法、装置、介质及设备 |
Also Published As
Publication number | Publication date |
---|---|
CN112509053B (zh) | 2021-06-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112509053B (zh) | 机器人位姿的获取方法、装置及电子设备 | |
CN111627065B (zh) | 一种视觉定位方法及装置、存储介质 | |
Yuan et al. | An improved Otsu threshold segmentation method for underwater simultaneous localization and mapping-based navigation | |
CN110663060B (zh) | 一种用于表示环境元素的方法、装置、系统、以及车辆/机器人 | |
CN111750820A (zh) | 影像定位方法及其系统 | |
CN113865580A (zh) | 构建地图的方法、装置、电子设备及计算机可读存储介质 | |
CN111380510B (zh) | 重定位方法及装置、机器人 | |
Lee et al. | SpherePHD: Applying CNNs on 360${}^\circ $∘ Images With Non-Euclidean Spherical PolyHeDron Representation | |
CN111429344B (zh) | 基于感知哈希的激光slam闭环检测方法及系统 | |
Zhou et al. | Review on millimeter-wave radar and camera fusion technology | |
CN107851196A (zh) | 一种图像模式匹配的方法及装置 | |
US20220277581A1 (en) | Hand pose estimation method, device and storage medium | |
CN114255197B (zh) | 一种红外与可见光图像自适应融合对齐方法及系统 | |
CN113284163A (zh) | 基于车载激光雷达点云的三维目标自适应检测方法及系统 | |
Yin et al. | SLAM-based self-calibration of a binocular stereo vision rig in real-time | |
CN112989469A (zh) | 建筑物屋顶模型的构建方法、装置、电子设备及存储介质 | |
Yin et al. | CoMask: Corresponding mask-based end-to-end extrinsic calibration of the camera and LiDAR | |
Li et al. | Learn then match: A fast coarse-to-fine depth image-based indoor localization framework for dark environments via deep learning and keypoint-based geometry alignment | |
Kang et al. | Detecting maritime obstacles using camera images | |
JP2022080303A (ja) | オプティカルフローを用いたライダー位置推定 | |
Li et al. | Subpixel image registration algorithm based on pyramid phase correlation and upsampling | |
CN116071404A (zh) | 图像配准方法、装置、计算机设备和存储介质 | |
Du et al. | Topology adaptive water boundary extraction based on a modified balloon snake: using GF-1 satellite images as an example | |
CN115630185B (zh) | 一种重定位方法、水面航行器及存储介质 | |
Deng et al. | Two-Step Matching Approach to Obtain More Control Points for SIFT-like Very-High-Resolution SAR Image Registration |
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 | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: Method, device and electronic equipment for obtaining robot pose Effective date of registration: 20210721 Granted publication date: 20210604 Pledgee: Shenzhen hi tech investment small loan Co.,Ltd. Pledgor: Shenzhen Smart Mapping Tech. Co.,Ltd. Registration number: Y2021980006554 |
|
PE01 | Entry into force of the registration of the contract for pledge of patent right |