CN112862894A - 一种机器人三维点云地图构建与扩充方法 - Google Patents
一种机器人三维点云地图构建与扩充方法 Download PDFInfo
- Publication number
- CN112862894A CN112862894A CN202110389259.XA CN202110389259A CN112862894A CN 112862894 A CN112862894 A CN 112862894A CN 202110389259 A CN202110389259 A CN 202110389259A CN 112862894 A CN112862894 A CN 112862894A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- map
- dimensional point
- pose
- key frame
- 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
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- 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/10028—Range image; Depth image; 3D point clouds
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Computer Graphics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Traffic Control Systems (AREA)
- Image Processing (AREA)
Abstract
本发明公开一种三维点云地图构建与扩充方法,包括:步1,通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真,同时确定先验位姿;步2,从三维点云数据提取特征关键帧,将特征关键帧与生成的局部点云地图扫描匹配,根据匹配结果修正先验位姿获得激光雷达后验位姿,通过后验位姿构建实时三维点云地图;步3,建立离线地图姿态图;当前时刻接收到继续提取的特征关键帧时,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个状态节点,添加到离线地图姿态图中,对离线三维点云地图进行扩充。能实现在少特征环境下进行三维点云地图的构建与扩充。
Description
技术领域
本发明涉及移动机器人领域,尤其涉及一种基于激光雷达绘制地图的机器人三维点云地图构建与扩充方法。
背景技术
在智能移动机器人领域中,建图和定位是自主导航规划的基本前提。基于视觉与激光雷达进行同步定位与建图(SLAM)方法也是现今热门的研究内容。因为激光雷达传感器对季节天气与光照变化不是很敏感并且能够远距离感知环境的细节,所以它是该研究领域所使用到的主要传感器之一。
然而,仅使用激光雷达传感器支持整个建图与定位系统是不可靠的。因为在一些实际场景中存在激光雷达退化的情况,比如在广场、公园等开阔区域,即使是多线激光雷达,也只能接收地面点云进行匹配,这很可能会造成状态估计在水平面上随机移动;或者在单侧墙、走廊等环境中,沿着建筑方向移动获取的激光点云是一样的,这使得匹配算法无法正确估计这个方向上的运动。另外,三维激光点云具有稀疏特性,但对其计算处理上又是昂贵的,故基于激光雷达的状态估计频率往往比较低。现今有许多方法结合IMU或里程计等传感器辅助激光雷达来解决上述问题,但若缺乏绝对测量信息,机器人在长期运动导航过程中会累计误差,出现漂移现象。
发明内容
基于现有技术所存在的问题,本发明的目的是提供一种机器人三维点云地图构建与扩充方法,结合多传感器辅助激光雷达来解决基于激光雷达的退化情况、状态估计频率较低等问题;使用绝对测量信息来消除机器人在长期运动导航过程中的累计误差,减少漂移现象。
本发明的目的是通过以下技术方案实现的:
本发明实施方式提供一种机器人三维点云地图构建与扩充方法,包括:
步骤1,通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真,同时确定激光雷达扫描匹配前的先验位姿;
步骤2,从去除运动失真后的三维点云数据提取特征关键帧,将所述特征关键帧与生成的局部点云地图进行扫描匹配,根据匹配结果对所述先验位姿进行修正获得激光雷达后验位姿,通过所述激光雷达后验位姿构建得到实时三维点云地图,保存所述实时三维点云地图作为离线三维点云地图,同时保存特征关键帧与对应的后验位姿;
步骤3,通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图;在当前时刻接收到继续提取的特征关键帧时,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
由上述本发明提供的技术方案可以看出,本发明实施例提供的机器人三维点云地图构建与扩充方法,其有益效果为:
通过结合相对测量信息与绝对测量信息,使用扩展卡尔曼滤波来辅助激光雷达建图与定位,支持广泛的多传感器输入。此外,还使建立好的离线三维点云地图与当前激光雷达特征关键帧合并到一幅姿态图中进行整体优化,加入多种姿态空间约束,能够提高机器人在离线地图中的定位精度,可以在少特征环境下对离线三维点云地图进行扩充与完善。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域的普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他附图。
图1为本发明实施例提供的机器人三维点云地图构建与扩充方法的一个实施例的处理流程图;
图2为本发明实施例提供的机器人三维点云地图构建与扩充方法的一个实施例的具体处理流程图;
图3为本发明一实施例提供的机器人三维点云地图构建实验测试结果示意图;
图4为本发明一实施例提供的机器人三维点云地图构建实验测试场景的CAD地图;
图5为本发明一实施例提供的机器人三维点云地图构建与扩充方法中结合离线地图的后端姿态图优化结构图;
图6a、6b分别为本发明实施例提供的机器人三维点云地图构建与扩充方法中的三维点云地图扩充实验测试结果示意图。
具体实施方式
下面结合本发明的具体内容,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明的保护范围。本发明实施例中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
如图1所示,本发明实施例提供一种机器人三维点云地图构建与扩充方法,是一种在少特征环境下对三维点云地图构建与扩充的方法,包括:
步骤1,通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真,同时确定激光雷达扫描匹配前的先验位姿;
步骤2,从去除运动失真后的三维点云数据提取特征关键帧,将所述特征关键帧与生成的局部点云地图进行扫描匹配,根据匹配结果对所述先验位姿进行修正获得激光雷达后验位姿,通过所述激光雷达后验位姿构建得到实时三维点云地图,保存所述实时三维点云地图作为离线三维点云地图,同时保存特征关键帧与对应的后验位姿;
步骤3,通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图;在当前时刻接收到继续提取的特征关键帧时,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
上述步骤3中,按步骤1、2的方式处理对当前时刻接收到的原始三维点云数据进行处理,提取特征关键帧。如通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真,同时确定激光雷达扫描匹配前的先验位姿;再从去除运动失真后的三维点云数据提取特征关键帧。
上述方法中,所述多传感器信息包括:
GPS、轮速计和IMU的测量信息。
上述方法的步骤1中,利用卡尔曼滤波融合多传感器信息,获取所述机器人的状态信息X:
X=[pT,qT,vT]T (1);
所述式(1)中,p为机器人位姿的三维位置向量;q为单位四元数,能生成旋转矩阵R∈SO(3);v为三维速度向量;
将结合所述三维位置向量p与所述旋转矩阵R∈SO(3)得到的位姿估计变换T=<R|p>∈SE(3)通过线性插值方式去除原始三维点云数据的运动失真;
通过所述位姿估计变换T将所述机器人的当前坐标系转换到地图坐标系下,得出激光雷达扫描匹配前的先验位姿T0。
上述方法中,所述方法的去除原始三维点云数据的运动失真中,先通过以下公式(2)的线性插值方式获取tj与tj+1之间任意t时刻的位姿估计变换Tt:
所述式(2)中,Tj和Tj+1为两次连续的位姿估计变换;tj与tj+1分别为Tj和Tj+1对应的时间戳;
然后利用所述位姿估计变换通过以下公式(3)将激光雷达一次扫描周期[ti,ti+1]中原始三维点云数据的所有点pt转换到扫描初始时刻下,从而消除运动失真的影响:
上述方法的步骤2中,从当前去除运动失真后的三维点云数据的扫描帧提取平面特征点Fp与边缘特征点Fe,提取的平面特征点Fp与边缘特征点Fe组成特征帧,当特征帧相对应的位姿估计变换超过位置1米和旋转角度10°的变化阈值时,提取该特征帧作为特征关键帧F={Fp,Fe},并舍弃未超过所述变化阈值的全部扫描帧;
若提取的固定数量的最近n+1个特征关键帧为{Fi-n,…,Fi},利用已知的后验位姿将{Fi-n,…,Fi}转换到世界坐标系下生成的局部点云地图(4),所述式(4)中,且是通过对应后验位姿转换到世界坐标系下的结果;
上述方法的步骤3中,通过以下方式通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图:
将所述步骤2中已保存的每个特征关键帧作为一个状态节点x′,利用各特征关键帧对应的后验位姿建立状态节点之间的姿态空间约束形成所述离线地图姿态图T′i-1,i=(T′i-1)-1T′i(9);所述式(9)中,T′i-1,i表示第i-1个特征关键帧作为的状态节点x′i-1到第i个特征关键帧作为的状态节点x′i的姿态空间约束;T′i-1与T′i分别为离线地图姿态图中第i-1与i个特征关键帧对应的后验位姿;
在当前时刻接收到继续提取的特征关键帧时,使用所述步骤2中扫描匹配方法建立该特征关键帧的里程计姿态约束;利用基于欧式距离的KNN搜索算法使该特征关键帧与所述离线三维点云地图进行扫描匹配建立回环姿态约束;以建立里程计姿态约束和回环姿态约束的特征关键帧作为一个状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
上述方法中,所述里程计姿态约束的建立方式为:用当前接收的特征关键帧对应的后验位姿建立相邻状态节点之间的姿态空间约束,其中,所述离线地图姿态图的最后一个状态节点x′n到当前继续建图的第一个状态节点x0之间的空间约束Pn,0由所述机器人在离线地图坐标系中的初始位置确定;所述初始位置通过以下方式确定:在需要扩充三维点云地图时,加载离线三维点云地图后,在激光雷达第一次定位或扫描之前,根据已保存的所述离线三维点云地图原点的绝对测量信息,确定机器人在该离线三维点云地图坐标系中的初始位置;
所述回环姿态约束的建立方式为:用基于欧式距离的KNN搜索算法在离线地图姿态图各状态节点中搜索并提取与当前状态节点最近的一个特征关键帧;当判断该特征关键帧与当前状态节点在位姿上距离小于15米阈值时,提取该特征关键帧前后m个特征关键帧生成局部离线点云地图,与该特征关键帧进行扫描匹配得到该特征关键帧与当前状态节点之间的变换关系,在所述离线地图姿态图中添加回环姿态约束Cn,i。回环姿态约束表示,当判断该特征关键帧与当前状态节点在位姿上距离小于预定阈值时,视为存在回环即回到了原来所经过的地方,则会建立回环姿态约束。
上述方法中,所述预定阈值为10~15米。
上述方法中,所述m个特征关键帧为12~15个特征关键帧。
本发明方法能实现在少特征环境下进行三维点云地图的构建与扩充,结合扩展卡尔曼滤波与实时三维建图,同时,保存特征关键帧与相应位姿变换作为离线地图,能够与再次继续建图的关键扫描帧进行匹配联合优化,从而达到对该离线点云地图进行完善扩充的目的。此外,采用类似回环的机制来优化修正当前机器人位姿,使机器人在离线地图中的定位与现实场景中真实位置有很好的对应关系,可以更好的应用于导航规划,由于加入多种姿态空间约束,能够提高机器人在离线地图中的定位精度。
下面对本发明实施例具体作进一步地详细描述。
如图1所示,本发明提供的针对少特征环境下的机器人三维点云地图构建与扩充的方法,包括以下步骤:
步骤1,使用扩展卡尔曼滤波融合GPS、轮速计、IMU等传感器测量得到当前机器人位姿估计变换来去除原始三维点云数据的运动失真,并同时可确定激光雷达扫描匹配前的先验位姿;
步骤2,在去除运动失真后的三维点云数据中提取特征关键帧,将提取的特征关键帧与生成的局部点云地图进行扫描匹配,根据匹配结果对先验位姿进行修正获取激光雷达后验位姿,通过所述激光雷达后验位姿构建得到实时三维点云地图,保存所述实时三维点云地图作为离线三维点云地图,同时保存特征关键帧与对应的后验位姿;
步骤3,通过步骤2已保存特征关键帧与对应的后验位姿建立离线地图姿态图,在当前时刻接收到从经步骤1、2方式处理的后续三维点云数据中提取的特征关键帧时,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
具体地,上述方法步骤1中,使用扩展卡尔曼滤波融合GPS、轮速计、IMU等传感器测量得到当前机器人位姿估计变换来去除原始三维点云数据的运动失真,并同时可确定激光雷达扫描匹配前的先验位姿与接收融合扫描匹配修正后的后验位姿,包括:
利用卡尔曼滤波融合多传感器信息,关注获取以下机器人状态信息:
X=[pT,qT,vT]T (1);
所述式(1)中,中p为机器人位姿的三维位置向量,q为单位四元数,可生成旋转矩阵R∈SO(3),v为三维速度向量;
结合三维位置与旋转矩阵得到的位姿估计变换T=<R|p>∈SE(3)经过线性插值方法可以为原始三维点云数据去除畸变,消除激光雷达运动失真的影响;
位姿估计变换T可以把当前机器人坐标系转换到地图坐标系下,为激光雷达扫描匹配提供先验位姿T0,同时卡尔曼滤波接收扫描匹配修正的后验位姿T*作为其测量信息之一。正如图2所示,这使得卡尔曼滤波与三维点云地图构建紧密结合在一起,提高了建图的鲁棒性。
优选地,上述位姿估计变换T经过线性插值方法可以为原始三维点云数据去除畸变,包括:
令Tj和Tj+1为两次连续的位姿估计变换,其对应的时间戳分别为tj与tj+1,利用如下的线性插值方法获取tj与tj+1之间任意t时刻的位姿变换:
然后利用该位姿变换把一次激光雷达扫描周期[ti,ti+1]中原始点云数据的所有点pt转换到扫描初始时刻下,从而消除运动失真的影响:
具体地,上述方法步骤2中,所述对去除运动失真后的三维点云数据进行特征关键帧提取,并与生成的局部点云地图进行扫描匹配,根据匹配结果对先验位姿进行修正获取激光雷达后验位姿,通过所述激光雷达后验位姿构建得到实时三维点云地图,保存所述实时三维点云地图作为离线三维点云地图,同时保存特征关键帧与对应的后验位姿,如图2系统框架图所示,包括:
对当前去除运动失真后的三维点云数据的扫描帧提取平面特征点Fp与边缘特征点Fe,得到特征帧,特征帧相对应的位姿估计变换超过位置1米和旋转角度10°的变化阈值时,抽取该特征帧作为特征关键帧F={Fp,Fe},对未超过变化阈值的扫描帧全部舍弃,这样能减小内存消耗;
假设接收处理到第i+1个特征关键帧Fi+1时,提取固定数量的最近n+1个特征关键帧{Fi-n,…,Fi}生成局部点云地图,即利用已知对应的后验位姿把{Fi-n,…,Fi}转换到世界坐标系下生成局部点云地图
为实现当前接收到的特征关键帧Fi+1与局部点云地图Mi的匹配,首先利用上述卡尔曼滤波融合所得到的i+1时刻先验位姿把转换到世界坐标系下获得计算中每个特征点到Mi中对应平面片和边缘线的距离dp,k与de,k:
三维点云地图构建的实施测试结果如图3所示,其中在圆框区域中没有任何其他建筑设施,激光雷达只能接收到地面点云数据,缺乏能够提取的特征信息数据,即会发生少特征环境下激光雷达退化情况。然而对比如图4已知的CAD平面地图可以看出,本发明的方法能在少特征环境下保证构建三维点云地图的鲁棒性。
具体地,上述方法步骤3中,通过步骤2已保存特征关键帧与对应的后验位姿建立离线地图姿态图,在当前时刻接收到后续的原始三维点云数据,经步骤1、2方式处理后提取特征关键帧,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充,包括:
如图5的后端优化姿态图结构图所示,首先使用离线存储的每个特征关键帧与对应的后验位姿建立离线地图姿态图;将存储的每个特征关键帧作为一个状态节点x′,然后利用对应的后验位姿建立节点间的姿态空间约束,状态节点x′i-1到状态节点x′i′的姿态空间约束定义为:T′i-1,i=(T′i-1)-1T′i (9);
所述式(9)中,T′i-1与T′i分别为离线地图姿态图中第i-1与i个特征关键帧对应的后验位姿;
在后续继续构建三维点云地图过程中,以上述同样的方式在离线地图姿态图中继续添加建立了里程计姿态约束和回环姿态约束的状态节点,当继续提取到一个特征关键帧时,便会在离线地图姿态图中继续添加一个状态节点,并在姿态节点间同时建立里程计姿态约束和回环姿态约束这两种姿态空间约束;
其中,所述里程计姿态约束的建立方式为:继续构建三维点云地图过程中,使用特征关键帧对应的后验位姿建立相邻状态节点间的姿态空间约束,不同的是,由离线地图姿态图的最后一个姿态节点x′n到当前继续建图的第一个状态节点x0之间的姿态空间约束Pn,0是由机器人在离线地图坐标系中的初始位置确定的;
所述回环姿态约束的建立方式为:使用KNN搜索算法在离线地图姿态图的各姿态节点中搜索并提取与当前状态节点最近的一个特征关键帧,当判断该特征关键帧与当前状态节点在位姿上距离小于15米阈值时,提取该特征关键帧前后m个特征关键帧生成局部离线点云地图,与该特征关键帧进行扫描匹配得到该特征关键帧与当前状态节点之间的变换关系,在所述离线地图姿态图中添加回环姿态约束Cn,i。
本发明方法对三维点云地图扩充的实施测试结果如图6所示,其中图6a是扩充前加载的离线三维点云地图,图6b是对保存的离线点云地图扩充的结果。两者对比可以明显看出对原有离线三维点云地图的右下角进行了良好的扩充,同时这也为地图的拼接提供了很好的思路。
本发明针对在少特征环境下激光雷达退化情况,提出了一种三维点云地图构建与扩充的方法,结合经典的扩展卡尔曼滤波算法与后端姿态图优化进行三维实时建图,利用多传感器的互补优势,增强了建图的鲁棒性,并且能够实现对保存的离线三维点云地图的完善与扩充。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明披露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求书的保护范围为准。
Claims (9)
1.一种机器人三维点云地图构建与扩充方法,其特征在于,包括:
步骤1,通过扩展卡尔曼滤波融合多传感器信息测量得到当前机器人的位姿估计变换去除原始三维点云数据的运动失真,同时确定激光雷达扫描匹配前的先验位姿;
步骤2,从去除运动失真后的三维点云数据提取特征关键帧,将所述特征关键帧与生成的局部点云地图进行扫描匹配,根据匹配结果对所述先验位姿进行修正获得激光雷达后验位姿,通过所述激光雷达后验位姿构建得到实时三维点云地图,保存所述实时三维点云地图作为离线三维点云地图,同时保存特征关键帧与对应的后验位姿;
步骤3,通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图;在当前时刻接收到继续提取的特征关键帧时,对该特征关键帧建立里程计姿态约束和回环姿态约束,将该特征关键帧作为一个带有里程计姿态约束和回环姿态约束的状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
2.根据权利要求1所述的机器人三维点云地图构建与扩充方法,其特征在于,所述多传感器信息包括:
GPS、轮速计和IMU的测量信息。
3.根据权利要求1或2所述的机器人三维点云地图构建与扩充方法,其特征在于,所述步骤1中,利用卡尔曼滤波融合多传感器信息,获取所述机器人的状态信息X:
X=[pT,qT,vT]T (1);
所述式(1)中,p为机器人位姿的三维位置向量;q为单位四元数,能生成旋转矩阵R∈SO(3);v为三维速度向量;
将结合所述三维位置向量p与所述旋转矩阵R∈SO(3)得到的位姿估计变换T=<R|p>∈SE(3)通过线性插值方式去除原始三维点云数据的运动失真;
通过所述位姿估计变换T将所述机器人的当前坐标系转换到地图坐标系下,得出激光雷达扫描匹配前的先验位姿T0。
5.根据权利要求1或2所述的机器人三维点云地图构建与扩充方法,其特征在于,所述步骤2中,从当前去除运动失真后的三维点云数据的扫描帧提取平面特征点Fp与边缘特征点Fe,提取的平面特征点Fp与边缘特征点Fe组成特征帧,当特征帧相对应的位姿估计变换超过位置1米和旋转角度10°的变化阈值时,提取该特征帧作为特征关键帧F={Fp,Fe},并舍弃未超过所述变化阈值的全部扫描帧;
若提取的固定数量的最近n+1个特征关键帧为{Fi-n,…,Fi},利用已知的后验位姿将{Fi-n,…,Fi}转换到世界坐标系下生成的局部点云地图(4),所述式(4)中,且是通过对应后验位姿转换到世界坐标系下的结果;
6.根据权利要求1或2所述的机器人三维点云地图构建与扩充方法,其特征在于,所述步骤3中,通过以下方式通过所述步骤2中已保存的特征关键帧与对应的后验位姿建立离线地图姿态图:
将所述步骤2中已保存的每个特征关键帧作为一个状态节点x′,利用各特征关键帧对应的后验位姿建立状态节点之间的姿态空间约束形成所述离线地图姿态图T′i-1,i=(T′i-1)-1T′i (9);所述式(9)中,T′i-1,i表示第i-1个特征关键帧作为的状态节点x′i-1到第i个特征关键帧作为的状态节点x′i的姿态空间约束;T′i-1与T′i分别为离线地图姿态图中第i-1与i个特征关键帧对应的后验位姿;
在当前时刻接收到继续提取的特征关键帧时,使用所述步骤2中扫描匹配方法建立该特征关键帧的里程计姿态约束;利用基于欧式距离的KNN搜索算法使该特征关键帧与所述离线三维点云地图进行扫描匹配建立回环姿态约束;以建立里程计姿态约束和回环姿态约束的特征关键帧作为一个状态节点,添加到所述离线地图姿态图中,对所述离线三维点云地图进行扩充。
7.根据权利要求6所述的机器人三维点云地图构建与扩充方法,其特征在于,所述里程计姿态约束的建立方式为:用当前接收的特征关键帧对应的后验位姿建立相邻状态节点之间的姿态空间约束,其中,所述离线地图姿态图的最后一个状态节点x′n到当前继续建图的第一个状态节点x0之间的空间约束Pn,0由所述机器人在离线地图坐标系中的初始位置确定;所述初始位置通过以下方式确定:在需要扩充三维点云地图时,加载离线三维点云地图后,在激光雷达第一次定位或扫描之前,根据已保存的所述离线三维点云地图原点的绝对测量信息,确定机器人在该离线三维点云地图坐标系中的初始位置;
所述回环姿态约束的建立方式为:用基于欧式距离的KNN搜索算法在离线地图姿态图各状态节点中搜索并提取与当前状态节点最近的一个特征关键帧;当判断该特征关键帧与当前状态节点在位姿上距离小于预定阈值时,提取该特征关键帧前后m个特征关键帧生成局部离线三维点云地图,与该特征关键帧进行扫描匹配得到该特征关键帧与当前状态节点之间的变换关系,在所述离线地图姿态图中添加回环姿态约束Cn,i。
8.根据权利要求7所述的机器人三维点云地图构建与扩充方法,其特征在于,所述预定阈值为10~15米。
9.根据权利要求7所述的机器人三维点云地图构建与扩充方法,其特征在于,所述m个特征关键帧为12~15个特征关键帧。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110389259.XA CN112862894B (zh) | 2021-04-12 | 2021-04-12 | 一种机器人三维点云地图构建与扩充方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110389259.XA CN112862894B (zh) | 2021-04-12 | 2021-04-12 | 一种机器人三维点云地图构建与扩充方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112862894A true CN112862894A (zh) | 2021-05-28 |
CN112862894B CN112862894B (zh) | 2022-09-06 |
Family
ID=75992472
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110389259.XA Active CN112862894B (zh) | 2021-04-12 | 2021-04-12 | 一种机器人三维点云地图构建与扩充方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112862894B (zh) |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113447026A (zh) * | 2021-07-14 | 2021-09-28 | 深圳亿嘉和科技研发有限公司 | 适应动态环境变化的amcl定位方法 |
CN113624239A (zh) * | 2021-08-11 | 2021-11-09 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
CN113674399A (zh) * | 2021-08-16 | 2021-11-19 | 杭州图灵视频科技有限公司 | 一种移动机器人室内三维点云地图构建方法及系统 |
CN113720324A (zh) * | 2021-08-30 | 2021-11-30 | 上海大学 | 一种八叉树地图构建方法及系统 |
CN113959437A (zh) * | 2021-10-14 | 2022-01-21 | 重庆数字城市科技有限公司 | 一种用于移动测量设备的测量方法及系统 |
CN113985436A (zh) * | 2021-11-04 | 2022-01-28 | 广州中科云图智能科技有限公司 | 基于slam的无人机三维地图构建与定位方法及装置 |
CN114115263A (zh) * | 2021-11-19 | 2022-03-01 | 武汉万集光电技术有限公司 | 用于agv的自主建图方法、装置、移动机器人及介质 |
CN114119744A (zh) * | 2021-11-08 | 2022-03-01 | 国汽(北京)智能网联汽车研究院有限公司 | 一种构建点云地图的方法、装置、设备及存储介质 |
CN114170280A (zh) * | 2021-12-09 | 2022-03-11 | 北京能创科技有限公司 | 基于双窗口的激光里程计方法、系统、装置 |
CN114322994A (zh) * | 2022-03-10 | 2022-04-12 | 之江实验室 | 一种基于离线全局优化的多点云地图融合方法和装置 |
CN114353780A (zh) * | 2021-12-31 | 2022-04-15 | 高德软件有限公司 | 姿态优化方法及设备 |
CN114413881A (zh) * | 2022-01-07 | 2022-04-29 | 中国第一汽车股份有限公司 | 高精矢量地图的构建方法、装置及存储介质 |
CN114569011A (zh) * | 2022-03-25 | 2022-06-03 | 微思机器人(深圳)有限公司 | 一种沿墙行走方法、装置、扫地机器人及存储介质 |
CN115031718A (zh) * | 2022-05-25 | 2022-09-09 | 安徽中科合鼎科技发展有限公司 | 一种多传感器融合的无人船同步定位与建图方法(slam)及系统 |
CN115060276A (zh) * | 2022-06-10 | 2022-09-16 | 江苏集萃清联智控科技有限公司 | 一种多环境适应性自动驾驶车辆定位设备、系统及方法 |
CN115100377A (zh) * | 2022-07-15 | 2022-09-23 | 小米汽车科技有限公司 | 地图构建方法、装置、车辆、可读存储介质及芯片 |
CN115774265A (zh) * | 2023-02-15 | 2023-03-10 | 江苏集萃清联智控科技有限公司 | 用于工业机器人的二维码和激光雷达融合定位方法与装置 |
CN117291984A (zh) * | 2023-11-22 | 2023-12-26 | 武汉理工大学 | 一种基于位姿约束的多帧描述符匹配重定位方法及系统 |
CN113624239B (zh) * | 2021-08-11 | 2024-05-31 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180211399A1 (en) * | 2017-01-26 | 2018-07-26 | Samsung Electronics Co., Ltd. | Modeling method and apparatus using three-dimensional (3d) point cloud |
CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
CN110243370A (zh) * | 2019-05-16 | 2019-09-17 | 西安理工大学 | 一种基于深度学习的室内环境三维语义地图构建方法 |
CN110849374A (zh) * | 2019-12-03 | 2020-02-28 | 中南大学 | 地下环境定位方法、装置、设备及存储介质 |
CN112183171A (zh) * | 2019-07-05 | 2021-01-05 | 杭州海康机器人技术有限公司 | 一种基于视觉信标建立信标地图方法、装置 |
-
2021
- 2021-04-12 CN CN202110389259.XA patent/CN112862894B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180211399A1 (en) * | 2017-01-26 | 2018-07-26 | Samsung Electronics Co., Ltd. | Modeling method and apparatus using three-dimensional (3d) point cloud |
CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
CN110243370A (zh) * | 2019-05-16 | 2019-09-17 | 西安理工大学 | 一种基于深度学习的室内环境三维语义地图构建方法 |
CN112183171A (zh) * | 2019-07-05 | 2021-01-05 | 杭州海康机器人技术有限公司 | 一种基于视觉信标建立信标地图方法、装置 |
CN110849374A (zh) * | 2019-12-03 | 2020-02-28 | 中南大学 | 地下环境定位方法、装置、设备及存储介质 |
Non-Patent Citations (2)
Title |
---|
LIYANG YU等: "Detecting Duplicated Frames by Mapping Frames to 3D Skeletons", 《2015 INTERNATIONAL CONFERENCE ON INTELLIGENT INFORMATION HIDING AND MULTIMEDIA SIGNAL PROCESSING (IIH-MSP)》 * |
付光耀: "基于闭环检测的三维点云地图创建", 《基于闭环检测的三维点云地图构建》 * |
Cited By (30)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113447026A (zh) * | 2021-07-14 | 2021-09-28 | 深圳亿嘉和科技研发有限公司 | 适应动态环境变化的amcl定位方法 |
CN113624239A (zh) * | 2021-08-11 | 2021-11-09 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
CN113624239B (zh) * | 2021-08-11 | 2024-05-31 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
CN113674399A (zh) * | 2021-08-16 | 2021-11-19 | 杭州图灵视频科技有限公司 | 一种移动机器人室内三维点云地图构建方法及系统 |
CN113720324A (zh) * | 2021-08-30 | 2021-11-30 | 上海大学 | 一种八叉树地图构建方法及系统 |
CN113959437A (zh) * | 2021-10-14 | 2022-01-21 | 重庆数字城市科技有限公司 | 一种用于移动测量设备的测量方法及系统 |
CN113985436A (zh) * | 2021-11-04 | 2022-01-28 | 广州中科云图智能科技有限公司 | 基于slam的无人机三维地图构建与定位方法及装置 |
CN114119744A (zh) * | 2021-11-08 | 2022-03-01 | 国汽(北京)智能网联汽车研究院有限公司 | 一种构建点云地图的方法、装置、设备及存储介质 |
CN114119744B (zh) * | 2021-11-08 | 2024-05-14 | 国汽(北京)智能网联汽车研究院有限公司 | 一种构建点云地图的方法、装置、设备及存储介质 |
CN114115263A (zh) * | 2021-11-19 | 2022-03-01 | 武汉万集光电技术有限公司 | 用于agv的自主建图方法、装置、移动机器人及介质 |
CN114115263B (zh) * | 2021-11-19 | 2024-04-09 | 武汉万集光电技术有限公司 | 用于agv的自主建图方法、装置、移动机器人及介质 |
CN114170280A (zh) * | 2021-12-09 | 2022-03-11 | 北京能创科技有限公司 | 基于双窗口的激光里程计方法、系统、装置 |
CN114170280B (zh) * | 2021-12-09 | 2023-11-28 | 北京能创科技有限公司 | 基于双窗口的激光里程计方法、系统、装置 |
CN114353780A (zh) * | 2021-12-31 | 2022-04-15 | 高德软件有限公司 | 姿态优化方法及设备 |
CN114353780B (zh) * | 2021-12-31 | 2024-04-02 | 高德软件有限公司 | 姿态优化方法及设备 |
CN114413881A (zh) * | 2022-01-07 | 2022-04-29 | 中国第一汽车股份有限公司 | 高精矢量地图的构建方法、装置及存储介质 |
CN114413881B (zh) * | 2022-01-07 | 2023-09-01 | 中国第一汽车股份有限公司 | 高精矢量地图的构建方法、装置及存储介质 |
CN114322994B (zh) * | 2022-03-10 | 2022-07-01 | 之江实验室 | 一种基于离线全局优化的多点云地图融合方法和装置 |
CN114322994A (zh) * | 2022-03-10 | 2022-04-12 | 之江实验室 | 一种基于离线全局优化的多点云地图融合方法和装置 |
CN114569011A (zh) * | 2022-03-25 | 2022-06-03 | 微思机器人(深圳)有限公司 | 一种沿墙行走方法、装置、扫地机器人及存储介质 |
CN114569011B (zh) * | 2022-03-25 | 2023-09-05 | 微思机器人(深圳)有限公司 | 一种沿墙行走方法、装置、扫地机器人及存储介质 |
CN115031718A (zh) * | 2022-05-25 | 2022-09-09 | 安徽中科合鼎科技发展有限公司 | 一种多传感器融合的无人船同步定位与建图方法(slam)及系统 |
CN115031718B (zh) * | 2022-05-25 | 2023-10-31 | 合肥恒淏智能科技合伙企业(有限合伙) | 一种多传感器融合的无人船同步定位与建图方法(slam)及系统 |
CN115060276B (zh) * | 2022-06-10 | 2023-05-12 | 江苏集萃清联智控科技有限公司 | 一种多环境适应性自动驾驶车辆定位设备、系统及方法 |
CN115060276A (zh) * | 2022-06-10 | 2022-09-16 | 江苏集萃清联智控科技有限公司 | 一种多环境适应性自动驾驶车辆定位设备、系统及方法 |
CN115100377A (zh) * | 2022-07-15 | 2022-09-23 | 小米汽车科技有限公司 | 地图构建方法、装置、车辆、可读存储介质及芯片 |
CN115100377B (zh) * | 2022-07-15 | 2024-06-11 | 小米汽车科技有限公司 | 地图构建方法、装置、车辆、可读存储介质及芯片 |
CN115774265A (zh) * | 2023-02-15 | 2023-03-10 | 江苏集萃清联智控科技有限公司 | 用于工业机器人的二维码和激光雷达融合定位方法与装置 |
CN117291984A (zh) * | 2023-11-22 | 2023-12-26 | 武汉理工大学 | 一种基于位姿约束的多帧描述符匹配重定位方法及系统 |
CN117291984B (zh) * | 2023-11-22 | 2024-02-09 | 武汉理工大学 | 一种基于位姿约束的多帧描述符匹配重定位方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN112862894B (zh) | 2022-09-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112862894B (zh) | 一种机器人三维点云地图构建与扩充方法 | |
CN111561923B (zh) | 基于多传感器融合的slam制图方法、系统 | |
CN108717710B (zh) | 室内环境下的定位方法、装置及系统 | |
CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及系统 | |
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
CN111665512B (zh) | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 | |
CN112965063B (zh) | 一种机器人建图定位方法 | |
CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法 | |
CN110763239B (zh) | 滤波组合激光slam建图方法及装置 | |
CN110926485B (zh) | 一种基于直线特征的移动机器人定位方法及系统 | |
CN114013449B (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN115880364A (zh) | 基于激光点云和视觉slam的机器人位姿估计方法 | |
CN113674412A (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
CN111578959A (zh) | 一种基于改进Hector SLAM算法的未知环境自主定位方法 | |
CN116429084A (zh) | 一种动态环境3d同步定位与建图方法 | |
CN116429116A (zh) | 一种机器人定位方法及设备 | |
CN117367412B (zh) | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 | |
Ko et al. | Real-time building of a thinning-based topological map with metric features | |
WO2023226154A1 (zh) | 自主定位方法、装置、设备及计算机可读存储介质 | |
CN115984463A (zh) | 一种适用于狭窄巷道的三维重建方法和系统 | |
CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
CN115507842A (zh) | 一种基于面元的轻量化无人机地图构建方法 | |
CN115235455A (zh) | 一种基于智能手机pdr与视觉修正的行人定位方法 | |
CN115512054A (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 |