CN113470089B - 一种基于三维点云的跨域协同定位和建图方法及系统 - Google Patents

一种基于三维点云的跨域协同定位和建图方法及系统 Download PDF

Info

Publication number
CN113470089B
CN113470089B CN202110834968.4A CN202110834968A CN113470089B CN 113470089 B CN113470089 B CN 113470089B CN 202110834968 A CN202110834968 A CN 202110834968A CN 113470089 B CN113470089 B CN 113470089B
Authority
CN
China
Prior art keywords
point cloud
map
pose
global
rigid body
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
Application number
CN202110834968.4A
Other languages
English (en)
Other versions
CN113470089A (zh
Inventor
杨绍武
蓝龙
李雪琼
吴家安
赵钰玉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110834968.4A priority Critical patent/CN113470089B/zh
Publication of CN113470089A publication Critical patent/CN113470089A/zh
Application granted granted Critical
Publication of CN113470089B publication Critical patent/CN113470089B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/11—Region-based segmentation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • 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/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Abstract

本发明公开了一种基于三维点云的跨域协同定位和建图方法及系统。方法包括S1、生成局部配准的点云流、帧间里程计因子和帧间配准因子；S2、构建局部点云片段地图，对局部地图中的点云片段进行特征提取；S3、带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；S4、构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化；S5、对受影响轨迹中每个位姿节点相关联的全局点云片段进行位姿更新；S6、将局部地图中的点云片段进行位姿更新；S7、计算轨迹中最新位姿节点优化前后的变换关系，对当前位姿进行更新。本发明提高跨域地点识别的召回率、精确率和定位精度。

Description

一种基于三维点云的跨域协同定位和建图方法及系统
技术领域
本发明涉及多机器人协同即时定位与地图构建技术领域，更具体地说，特别涉及一种基于三维点云的跨域协同定位和建图方法及系统。
背景技术
无人机具有飞行速度快、较大的观测视场、通讯能力受环境影响小等优点，但载荷有限、续航时间短，相对而言，无人车的优劣特性与无人机正好相反。空-地机器人跨域协同能够实现无人机和无人车在运动特点、载荷能力和通讯等方面的互补，增进机器人对复杂环境的适应性，拓宽机器人执行任务的灵活性，近年来，有越来越多的研究者在空-地跨域多机器人群体的基础上展开研究。
跨域协同定位和建图是空-地跨域多机器人群体协同完成更复杂任务的基础和关键环节。然而，由于群体中无人机与无人车之间具有较为极端的大视角差异，使其“看到”的信息差异巨大，难以对同一环境形成一致的理解，这就为空-地跨域协同定位与建图带来极大挑战。
发明内容
本发明的目的在于提供一种基于三维点云的跨域协同定位和建图方法及系统，以克服现有技术所存在的缺陷。
为了达到上述目的，本发明采用的技术方案如下：
一种基于三维点云的跨域协同定位和建图方法，该方法基于多架空中无人机和多辆地面无人车，以及一台中央服务器实现，每台所述空中无人机和地面无人车内均配置有激光里程计系统和机器端空-地跨域全局定位和建图系统，所述中央服务器内配置有控制端空-地跨域全局定位和建图系统和位姿因子图增量优化器，该方法包括以下步骤：
S1、通过激光里程计系统实现连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成局部配准的点云流、帧间里程计因子和帧间配准因子；
S2、通过机器端空-地跨域全局定位和建图系统接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取；
S3、通过控制端空-地跨域全局定位和建图系统接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；
S4、通过位姿因子图增量优化器接收各激光里程计系统生成的帧间里程计因子和帧间配准因子以及控制端空-地跨域全局定位和建图系统生成的地点识别因子，构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化；
S5、控制端空-地跨域全局定位和建图系统接收优化后的空中无人机和地面无人车位姿轨迹后，对受影响轨迹中每个位姿节点相关联的全局点云片段进行位姿更新；
S6、空中无人机和地面无人车接收到优化后的位姿轨迹后，将局部地图中的点云片段进行位姿更新；
S7、计算轨迹中最新位姿节点优化前后的变换关系，对空中无人机和地面无人车的当前位姿进行更新。
进一步地，所述步骤S1中的帧间里程计因子是通过扩展卡尔曼滤波方法将轮编码器和IMU数据融合后得出的。
进一步地，所述步骤S2具体包括：
S20、接收激光里程计系统已局部配准的点云流，累积保存在动态体素网格中，同时更新每个受影响体素的地图点个数信息和中心点信息，其中体素的中心点为该体素内所有地图点位置的平均值，更新后若某个体素内部的地图点个数由小于阈值变为大于阈值，则将该体素加入到活动集U中；
S21、以活动集中每个体素的中心点代表体素中的所有地图点进行下采样，将下采样后得到的点作为改进的启发式增量点云分割算法的种子点集，并依据点的高度信息对种子点集进行从下往上排序；
S22、按照顺序从种子点集中获得种子点，并基于欧几里得距离进行区域增长得到点簇，当点簇大小满足设定要求时即成功分割得到点云片段，当点云片段大小达到阈值时，结束增长并构建新的点簇；
S23、对每一个点云片段进行描述子提取，输出带描述子的点云片段地图。
进一步地，所述步骤S23通过基于深度学习的点云描述子提取模型实现，该模型训练损失函数的公式如下：
Figure BDA0003173514360000021
其中，y是One-Hot编码的类标签向量，N是数据集中点云片段的类别数，l是网络预测输出。
进一步地，所述步骤S3具体包括以下步骤：
S31、针对无人车在线局部地图中的每个点云片段，从无人机全局地图中找到K个描述子最相似的点云片段，并用中心点代表每个点云片段建立候选匹配，得到候选匹配集合；
S32、以候选匹配作为图的顶点，通过判断两候选匹配是否一致来建立图的边，在构建好图后通过最大团算法获得最大的、一致的匹配集，若最大团的顶点数满足设定要求则继续执行；
S33、基于最大一致匹配集中已建立匹配关系的两片中心点点云，通过最小二乘法计算出局部地图与全局地图之间粗略的刚体变换关系；
S34、通过保存最近一定窗口大小的刚体变换关系，基于Z-score评分构建异常检测模型，判断当前估计的变换关系是否异常，在所有轴的Z-score评分小于阈值时则成功进行了一次定位；
S35、通过异常检测后，以基于中心点估计的粗刚体变换关系为初始值，使用片段的完整点云进行双ICP配准，得到无人车地图与无人机全局地图之间的精确变换关系。
进一步地，所述步骤S32中判断两候选匹配是否一致的具体为：若两候选匹配的一致性距离小于阈值且在全局地图的中心点的距离小于K倍阀值则为一致。
进一步地，所述步骤S34具体包括：
S341、对于第一次地点识别，无法判断其刚体变换关系是否异常，于是直接保存到窗口W＝{T1}内，并计算该窗口内所有刚体变换关系的均值Tmean＝T1和标准差Tstd＝0；
S342、假设某一时刻窗口内的历史刚体变换关系为W＝{T1，T2，...，Tm}，其中，1≤m≤41，对于之后到来的新的刚体变换关系Q，通过如下公式计算新的均值Tmean_new、标准差Tstd_new和新的刚体变换关系Q的Z-score评分：
Figure BDA0003173514360000031
Figure BDA0003173514360000032
Figure BDA0003173514360000033
然后把新的刚体变换关系Q添加到历史刚体变换关系W中，同时将Tmean和Tstd更新为Tmean_new和Tstd_new，如果新的刚体变换关系Q的Z-score评分中每个元素都小于阈值，则判定这个新的刚体变换关系Q通过了检测。
进一步地，在窗口W内的历史刚体变换关系数量到达42时，首先基于滑动窗口根据如下公式将W最早的刚体变换关系B剔除，将Tmean和Tstd进行更新：
Figure BDA0003173514360000041
Figure BDA0003173514360000042
然后判断新的刚体变换关系Q是否通过了检测，若通过则将新的刚体变换关系Q添加到窗口W中，同时将Tmean和Tstd更新，否则将B重新添加进窗口W内，同时将Tmean和Tstd还原。
进一步地，所述步骤S35具体包括：
S351、以基于中心点估计的粗刚体变换关系为初始值，使用PCL已经实现的基于RANSAC的ICP配准方法对匹配的点云片段进行ICP配准，获得无人车局部点云地图与无人机全局点云地图之间更精确的刚体变换关系Tgl；
S352、基于第一次ICP配准结果Tgl，根据如下公式计算出当前机器人在全局地图中的位姿Pg，Pg＝TglPl，其中P1为当前机器人在局部地图中的位姿；
S353、抽取全局地图中位置Pg半径R米范围内的所有点云片段与局部点云片段地图进行第二次ICP配准，得到精确刚体变换关系；
S354、基于该刚体变换关系，将局部点云片段地图融合进全局点云片段地图中，且该刚体变化关系会作为地点识别因子传输给位姿因子图增量优化器。
进一步地，所述步骤S5具体包括：
S51、计算受影响轨迹中位姿节点优化前后的变换关系；
S52、应用该变换关系，将全局地图中与该位姿节点相关联的点云片段进行刚体变换。
本发明还提供一种用于实现上述的基于三维点云的跨域协同定位和建图方法的系统，包括：多架空中无人机和多辆地面无人车，以及一台中央服务器实现；每台所述空中无人机和地面无人车内均设有激光里程计系统和机器端空-地跨域全局定位和建图系统；所述激光里程计系统用于连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成帧间里程计因子和帧间配准因子；所述机器端空-地跨域全局定位和建图系统用于接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取；所述中央服务器内配置有控制端空-地跨域全局定位和建图系统和位姿因子图增量优化器；所述控制端空-地跨域全局定位和建图系统用于接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；所述位姿因子图增量优化器用于接收各激光里程计系统生成的帧间里程计因子和帧间配准因子、控制端空-地跨域全局定位和建图系统生成的地点识别因子，构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化。
与现有技术相比，本发明的优点在于：本发明来提高跨域地点识别的召回率、精确率和定位精度，并成功实现了跨域地图融合，仿真实验证实了方法的有效性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明基于三维点云的跨域协同定位和建图系统的系统框架。
图2是本发明点云片段描述子提取网络的架构。
图3是本发明点云片段描述子提取网络训练架构。
具体实施方式
下面结合附图对本发明的优选实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。
参阅图1-图3所示，本实施例公开了一种基于三维点云的跨域协同定位和建图系统，包括多架空中无人机和多辆地面无人车，以及一台中央服务器实现，每台空中无人机和地面无人车内均配置有激光里程计系统和机器端空-地跨域全局定位和建图系统，中央服务器内配置有控制端空-地跨域全局定位和建图系统和位姿因子图增量优化器。其中，所述的激光里程计系统用于连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成帧间里程计因子和帧间配准因子；机器端空-地跨域全局定位和建图系统用于接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取；控制端空-地跨域全局定位和建图系统用于接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；位姿因子图增量优化器用于接收各激光里程计系统生成的帧间里程计因子和帧间配准因子、控制端空-地跨域全局定位和建图系统生成的地点识别。
所述的机器端空-地跨域全局定位和建图系统内包括改进的启发式增量点云分割模块(算法)和基于深度学习的点云描述子提取模型；所述的控制端空-地跨域全局定位和建图系统包括K-最近邻描述子匹配模块(算法)、改进的基于图的集合一致性验证模块(算法)、粗位姿估计模块、基于滑动窗口的位姿过滤模块、基于ICP的精确位姿估计模块。
本实施例中，所述的激光里程计系统可以为基于激光雷达和IMU的laser_slam或LeGO-LOAM激光里程计系统。
本实施例提供的基于三维点云的跨域协同定位和建图方法包括以下步骤：
步骤S1、通过激光里程计系统实现连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成局部配准的点云流、帧间配准因子和通过扩展卡尔曼滤波方法将轮编码器和IMU数据融合后得出的帧间里程计因子。
步骤S2、通过机器端空-地跨域全局定位和建图系统接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取，具体包括以下步骤：
步骤S20、采用改进的启发式增量点云分割模块接收激光里程计系统已局部配准的点云流，累积保存在动态体素网格中，同时更新每个受影响体素的地图点个数信息和中心点信息，其中体素的中心点为该体素内所有地图点位置的平均值，更新后若某个体素内部的地图点个数由小于阈值变为大于阈值，则将该体素加入到活动集U中。
步骤S21、以活动集中每个体素的中心点代表体素中的所有地图点进行下采样，将下采样后得到的点作为改进的启发式增量点云分割算法的种子点集，并依据点的高度信息(Z轴坐标)对种子点集进行从下往上排序。
步骤S22、按照顺序从种子点集中获得种子点，并基于欧几里得距离进行区域增长得到点簇，当点簇大小满足设定要求时即成功分割得到点云片段，当点云片段大小达到阈值时，需要结束增长并构建新的点簇。
S23、基于深度学习的点云描述子提取模型对每一个点云片段进行描述子提取，输出带描述子的点云片段地图。其中点云描述子提取网络模型训练损失函数如下：
Figure BDA0003173514360000061
损失函数中y是One-Hot编码的类标签向量，N是数据集中点云片段的类别数，l是网络预测输出。
步骤S3、通过控制端空-地跨域全局定位和建图系统接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子，该步骤具体包括：
步骤S31、K-最近邻描述子匹配模块(算法)基于描述子的欧几得距离，针对无人车在线局部地图中的每个点云片段，从无人机全局地图中找到K个描述子最相似的点云片段，并用中心点代表每个点云片段建立候选匹配(即匹配是一个包含两个三维点的数据结构)，得到候选匹配集合。
步骤S32、以候选匹配作为图的顶点，通过判断两候选匹配是否一致来建立图的边；判断准测为：两候选匹配
Figure BDA0003173514360000071
和
Figure BDA0003173514360000072
若两者的一致性距离小于阈值resolution_threshold，且在全局地图的中心点的距离
Figure BDA0003173514360000073
小于k倍resolution_threshold，则说两者是一致的。其中两匹配的一致性距离计算公式如下：
Figure BDA0003173514360000074
其中上标1和g分别表示中心点在局部和全局地图上。
在构建好图后通过最大团算法获得最大的、一致的匹配集，若最大团的顶点数满足设定要求则表示成功进行了一次地点识别，此时继续执行步骤S33。
步骤S33、粗位姿估计模块基于最大一致匹配集中已建立匹配关系的两片中心点点云，通过最小二乘法计算出局部地图与全局地图之间粗略的刚体变换关系。
步骤S34、基于滑动窗口的位姿过滤模块通过保存最近一定窗口大小的刚体变换关系，基于Z-score评分构建异常检测模型，判断当前估计的变换关系是否异常，在所有轴的Z-score评分小于阈值时则成功进行了一次定位，具体来说该步骤包括：
步骤S341、对于第一次地点识别，无法判断其刚体变换关系是否异常，于是直接保存到窗口W＝{T1}内，并计算该窗口内所有刚体变换关系的均值Tmean＝T1和标准差Tstd＝0。
步骤S342、假设某一时刻窗口内的历史刚体变换关系为W＝{T1，T2...，Tm}，其中，1≤m≤41，对于之后到来的新的刚体变换关系Q，通过如下公式计算新的均值Tmean_new、标准差Tstd_new和新的刚体变换关系Q的Z-score评分：
Figure BDA0003173514360000081
Figure BDA0003173514360000082
Figure BDA0003173514360000083
然后把新的刚体变换关系Q添加到历史刚体变换关系W中，同时将Tmean和Tstd更新为Tmean_new和Tstd_new，如果新的刚体变换关系Q的Z-score评分中每个元素都小于阈值，则判定这个新的刚体变换关系Q通过了检测，可传到下一模块继续进行处理。为了让异常检测模型更加鲁棒，根据经验可把窗口W的大小设置为42，当窗口内刚体变换关系数量不大于8个时，阈值Z-score_threshold固定为2.5，之后窗口内刚体变换关系数量从8增长到42的过程中，阈值Z-score_threshold也线性的从2.5增长到4。
当窗口W内的历史刚体变换关系数量到达42时，首先基于滑动窗口根据如下公式将W最早的刚体变换关系B剔除，将Tmean和Tstd进行更新：
Figure BDA0003173514360000084
Figure BDA0003173514360000085
然后判断新的刚体变换关系Q是否通过了检测，若通过则将新的刚体变换关系Q添加到窗口W中，同时将Tmean和Tstd更新，否则将B重新添加进窗口W内，同时将Tmean和Tstd还原。
步骤S35、通过异常检测后，基于ICP的精确位姿估计模块以基于中心点估计的粗刚体变换关系为初始值，使用片段的完整点云进行双ICP配准，得到无人车地图与无人机全局地图之间的精确变换关系，具体该步骤包括：
步骤S351、以基于中心点估计的粗刚体变换关系为初始值，使用PCL已经实现的基于RANSAC的ICP配准方法对匹配的点云片段进行ICP配准，获得无人车局部点云地图与无人机全局点云地图之间更精确的刚体变换关系Tgl。
步骤S352、基于第一次ICP配准结果Tgl，根据如下公式计算出当前机器人在全局地图中的位姿Pg，Pg＝TglPl，其中P1为当前机器人在局部地图中的位姿。
步骤S353、抽取全局地图中位置Pg半径R米范围内的所有点云片段与局部点云片段地图进行第二次ICP配准，得到精确刚体变换关系。
步骤S354、基于该刚体变换关系，将局部点云片段地图融合进全局点云片段地图中，当多个机器人多次访问同一地点时，Master端空-地跨域全局定位和建图系统会收到多份重复的点云数据，本系统在融合源地图时，通过计算所有点云片段中心点距离，来移除重复度过高的点云片段，节省计算和存储开销。
步骤S355、同时将该刚体变化关系作为地点识别因子传输给位姿因子图增量优化器。
步骤S4、通过位姿因子图增量优化器接收各激光里程计系统生成的帧间里程计因子和帧间配准因子以及控制端空-地跨域全局定位和建图系统生成的地点识别因子，构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化。
步骤S5、控制端空-地跨域全局定位和建图系统接收优化后的空中无人机和地面无人车位姿轨迹后，对受影响轨迹中每个位姿节点相关联的全局点云片段进行位姿更新，具体步骤为：
步骤S51、计算受影响轨迹中位姿节点优化前后的变换关系；
步骤S52、应用该变换关系，将全局地图中与该位姿节点相关联的点云片段进行刚体变换。
步骤S6、空中无人机和地面无人车接收到优化后的位姿轨迹后，同步步骤S51和S52，将局部地图中的点云片段进行位姿更新。
步骤S7、计算轨迹中最新位姿节点优化前后的变换关系，对空中无人机和地面无人车的当前位姿进行更新。
虽然结合附图描述了本发明的实施方式，但是专利所有者可以在所附权利要求的范围之内做出各种变形或修改，只要不超过本发明的权利要求所描述的保护范围，都应当在本发明的保护范围之内。

Claims (9)

1.一种基于三维点云的跨域协同定位和建图方法，该方法基于多架空中无人机和多辆地面无人车，以及一台中央服务器实现，其特征在于，每台所述空中无人机和地面无人车内均配置有激光里程计系统和机器端空-地跨域全局定位和建图系统，所述中央服务器内配置有控制端空-地跨域全局定位和建图系统和位姿因子图增量优化器，该方法包括以下步骤，
S1、通过激光里程计系统实现连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成局部配准的点云流、帧间里程计因子和帧间配准因子；
S2、通过机器端空-地跨域全局定位和建图系统接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取；
S3、通过控制端空-地跨域全局定位和建图系统接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；
S4、通过位姿因子图增量优化器接收各激光里程计系统生成的帧间里程计因子和帧间配准因子以及控制端空-地跨域全局定位和建图系统生成的地点识别因子，构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化；
S5、控制端空-地跨域全局定位和建图系统接收优化后的空中无人机和地面无人车位姿轨迹后，对受影响轨迹中每个位姿节点相关联的全局点云片段进行位姿更新；
S6、空中无人机和地面无人车接收到优化后的位姿轨迹后，将局部地图中的点云片段进行位姿更新；
S7、计算轨迹中最新位姿节点优化前后的变换关系，对空中无人机和地面无人车的当前位姿进行更新；
所述步骤S3具体包括以下步骤：
S31、针对无人车在线局部地图中的每个点云片段，从无人机全局地图中找到K个描述子最相似的点云片段，并用中心点代表每个点云片段建立候选匹配，得到候选匹配集合；
S32、以候选匹配作为图的顶点，通过判断两候选匹配是否一致来建立图的边，在构建好图后通过最大团算法获得最大的、一致的匹配集，若最大团的顶点数满足设定要求则继续执行；
S33、基于最大一致匹配集中已建立匹配关系的两片中心点点云，通过最小二乘法计算出局部地图与全局地图之间粗略的刚体变换关系；
S34、通过保存最近一定窗口大小的刚体变换关系，基于Z-score评分构建异常检测模型，判断当前估计的变换关系是否异常，在所有轴的Z-score评分小于阈值时则成功进行了一次定位；
S35、通过异常检测后，以基于中心点估计的粗刚体变换关系为初始值，使用片段的完整点云进行双ICP配准，得到无人车地图与无人机全局地图之间的精确变换关系；
所述步骤S35具体包括：
S351、以基于中心点估计的粗刚体变换关系为初始值，使用PCL已经实现的基于RANSAC的ICP配准方法对匹配的点云片段进行ICP配准，获得无人车局部点云地图与无人机全局点云地图之间更精确的刚体变换关系Tgl；
S352、基于第一次ICP配准结果Tgl，根据如下公式计算出当前机器人在全局地图中的位姿Pg，Pg＝TglPl，其中Pl为当前机器人在局部地图中的位姿；
S353、抽取全局地图中位置Pg半径R米范围内的所有点云片段与局部点云片段地图进行第二次ICP配准，得到精确刚体变换关系；
S354、基于该刚体变换关系，将局部点云片段地图融合进全局点云片段地图中，且该刚体变化关系会作为地点识别因子传输给位姿因子图增量优化器。
2.根据权利要求1所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S1中的帧间里程计因子是通过扩展卡尔曼滤波方法将轮编码器和IMU数据融合后得出的。
3.根据权利要求1所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S2具体包括：
S20、接收激光里程计系统已局部配准的点云流，累积保存在动态体素网格中，同时更新每个受影响体素的地图点个数信息和中心点信息，其中体素的中心点为该体素内所有地图点位置的平均值，更新后若某个体素内部的地图点个数由小于阈值变为大于阈值，则将该体素加入到活动集U中；
S21、以活动集中每个体素的中心点代表体素中的所有地图点进行下采样，将下采样后得到的点作为改进的启发式增量点云分割算法的种子点集，并依据点的高度信息对种子点集进行从下往上排序；
S22、按照顺序从种子点集中获得种子点，并基于欧几里得距离进行区域增长得到点簇，当点簇大小满足设定要求时即成功分割得到点云片段，当点云片段大小达到阈值时，结束增长并构建新的点簇；
S23、对每一个点云片段进行描述子提取，输出带描述子的点云片段地图。
4.根据权利要求3所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S23通过基于深度学习的点云描述子提取模型实现，该模型训练损失函数的公式如下：
Figure FDA0003548797440000031
其中，y是One-Hot编码的类标签向量，N是数据集中点云片段的类别数，l是网络预测输出。
5.根据权利要求1所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S32中判断两候选匹配是否一致的具体为：若两候选匹配的一致性距离小于阈值且在全局地图的中心点的距离小于K倍阈值则为一致。
6.根据权利要求1所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S34具体包括：
S341、对于第一次地点识别，无法判断其刚体变换关系是否异常，于是直接保存到窗口W＝{T1}内，并计算该窗口内所有刚体变换关系的均值Tmean＝T1和标准差Tstd＝0；
S342、假设某一时刻窗口内的历史刚体变换关系为W＝{T1，T2，...，Tm}，其中，1≤m≤41，对于之后到来的新的刚体变换关系Q，通过如下公式计算新的均值Tmean_new、标准差Tstd_new和新的刚体变换关系Q的Z-score评分：
Figure FDA0003548797440000032
Figure FDA0003548797440000033
Figure FDA0003548797440000034
然后把新的刚体变换关系Q添加到历史刚体变换关系W中，同时将Tmean和Tstd更新为Tmean_new和Tstd_new，如果新的刚体变换关系Q的Z-score评分中每个元素都小于阈值，则判定这个新的刚体变换关系Q通过了检测。
7.根据权利要求5所述的基于三维点云的跨域协同定位和建图方法，其特征在于，在窗口W内的历史刚体变换关系数量到达42时，首先基于滑动窗口根据如下公式将W最早的刚体变换关系B剔除，将Tmean和Tstd进行更新：
Figure FDA0003548797440000041
Figure FDA0003548797440000042
然后判断新的刚体变换关系Q是否通过了检测，若通过则将新的刚体变换关系Q添加到窗口W中，同时将Tmean和Tstd更新，否则将B重新添加进窗口W内，同时将Tmean和Tstd还原。
8.根据权利要求1所述的基于三维点云的跨域协同定位和建图方法，其特征在于，所述步骤S5具体包括：
S51、计算受影响轨迹中位姿节点优化前后的变换关系；
S52、应用该变换关系，将全局地图中与该位姿节点相关联的点云片段进行刚体变换。
9.一种用于实现权利要求1-8任意一项所述的基于三维点云的跨域协同定位和建图方法的系统，其特征在于，包括：多架空中无人机和多辆地面无人车，以及一台中央服务器实现；每台所述空中无人机和地面无人车内均设有激光里程计系统和机器端空-地跨域全局定位和建图系统；所述激光里程计系统用于连续点云配准，并结合轮编码器和IMU信息估计自身运动，生成帧间里程计因子和帧间配准因子；所述机器端空-地跨域全局定位和建图系统用于接收经过激光里程计系统配准的点云流并构建局部点云片段地图，再使用基于深度学习的三维点云特征提取模型对局部地图中的点云片段进行特征提取；所述中央服务器内配置有控制端空-地跨域全局定位和建图系统和位姿因子图增量优化器；所述控制端空-地跨域全局定位和建图系统用于接收各机器端空-地跨域全局定位和建图系统构建的带描述子的局部点云片段地图，再与带描述子的全局点云片段地图进行匹配关联并定位，构建融合的全局点云地图，并生成地点识别因子；所述位姿因子图增量优化器用于接收各激光里程计系统生成的帧间里程计因子和帧间配准因子、控制端空-地跨域全局定位和建图系统生成的地点识别因子，构建并维护一个全局位姿因子图，对所有空中无人机和地面无人车的位姿轨迹进行全局优化。
CN202110834968.4A 2021-07-21 2021-07-21 一种基于三维点云的跨域协同定位和建图方法及系统 Active CN113470089B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110834968.4A CN113470089B (zh) 2021-07-21 2021-07-21 一种基于三维点云的跨域协同定位和建图方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110834968.4A CN113470089B (zh) 2021-07-21 2021-07-21 一种基于三维点云的跨域协同定位和建图方法及系统

Publications (2)

Publication Number Publication Date
CN113470089A CN113470089A (zh) 2021-10-01
CN113470089B true CN113470089B (zh) 2022-05-03

Family

ID=77882175

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110834968.4A Active CN113470089B (zh) 2021-07-21 2021-07-21 一种基于三维点云的跨域协同定位和建图方法及系统

Country Status (1)

Country Link
CN (1) CN113470089B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN115601519B (zh) * 2022-11-07 2024-04-05 南京理工大学 面向低通信带宽下远程操控的操控端建图方法
CN116030134B (zh) * 2023-02-14 2023-07-18 长沙智能驾驶研究院有限公司 定位方法、装置、设备、可读存储介质及程序产品
CN117213470B (zh) * 2023-11-07 2024-01-23 武汉大学 一种多机碎片地图聚合更新方法及系统
CN117191005B (zh) * 2023-11-08 2024-01-30 中国人民解放军国防科技大学 一种空地异构协同建图方法、装置、设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110261877A (zh) * 2019-06-26 2019-09-20 南京航空航天大学 一种基于改进图优化slam的地空协同视觉导航方法及装置
CN111951397A (zh) * 2020-08-07 2020-11-17 清华大学 一种多机协同构建三维点云地图的方法、装置和存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法
CN112669358A (zh) * 2020-12-30 2021-04-16 中山大学 一种适用于多平台协同感知的地图融合方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110261877A (zh) * 2019-06-26 2019-09-20 南京航空航天大学 一种基于改进图优化slam的地空协同视觉导航方法及装置
CN111951397A (zh) * 2020-08-07 2020-11-17 清华大学 一种多机协同构建三维点云地图的方法、装置和存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法
CN112669358A (zh) * 2020-12-30 2021-04-16 中山大学 一种适用于多平台协同感知的地图融合方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Segment-based Cross-domain Localization between Aerial and Ground Robots;Wu, Jiaan,et.al;《2020 Chinese Automation Congress (CAC)》;20200129;全文 *
基于激光雷达的移动机器人三维建图与定位;殷江等;《福建工程学院学报》;20200825(第04期);全文 *

Also Published As

Publication number Publication date
CN113470089A (zh) 2021-10-01

Similar Documents

Publication Publication Date Title
CN113470089B (zh) 一种基于三维点云的跨域协同定位和建图方法及系统
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
WO2021135554A1 (zh) 一种无人车全局路径规划方法和装置
CN109798896B (zh) 一种室内机器人定位与建图方法及装置
Carle et al. Long‐range rover localization by matching LIDAR scans to orbital elevation maps
Ros et al. Visual slam for driverless cars: A brief survey
CN104517275A (zh) 对象检测方法和系统
CN111915675B (zh) 基于粒子漂移的粒子滤波点云定位方法及其装置和系统
CN108426582B (zh) 行人室内三维地图匹配方法
CN111273312A (zh) 一种智能车辆定位与回环检测方法
CN110763245A (zh) 一种基于流式计算的地图创建方法及其系统
CN112965063A (zh) 一种机器人建图定位方法
Bryson et al. Co-operative localisation and mapping for multiple UAVs in unknown environments
Jungnickel et al. Object tracking and dynamic estimation on evidential grids
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
CN115861968A (zh) 一种基于实时点云数据的动态障碍物剔除方法
Peng et al. Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization
Louati Cloud-assisted collaborative estimation for next-generation automobile sensing
Elisha et al. Active online visual-inertial navigation and sensor calibration via belief space planning and factor graph based incremental smoothing
Rozsypálek et al. Multidimensional particle filter for long-term visual teach and repeat in changing environments
Tian et al. Novel hybrid of strong tracking Kalman filter and improved radial basis function neural network for GPS/INS integrated navagation
Weyers et al. Improving occupancy grid FastSLAM by integrating navigation sensors
Bacha et al. A new robust cooperative-reactive Filter for vehicle localization: The Extended Kalman Particle Swarm ‘EKPS’
Yi et al. Lidar Odometry and Mapping Optimized by the Theory of Functional Systems in the Parking Lot
Gan et al. Filtering approach to online estimate the position of high-speed train

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