CN114255324A - 一种多激光雷达协同的地图构建方法及其系统 - Google Patents

一种多激光雷达协同的地图构建方法及其系统 Download PDF

Info

Publication number
CN114255324A
CN114255324A CN202111648187.2A CN202111648187A CN114255324A CN 114255324 A CN114255324 A CN 114255324A CN 202111648187 A CN202111648187 A CN 202111648187A CN 114255324 A CN114255324 A CN 114255324A
Authority
CN
China
Prior art keywords
pose
point cloud
plane
cloud data
global optimization
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
Application number
CN202111648187.2A
Other languages
English (en)
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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202111648187.2A priority Critical patent/CN114255324A/zh
Publication of CN114255324A publication Critical patent/CN114255324A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/187—Segmentation; Edge detection involving region growing; involving region merging; involving connected component labelling
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration
    • 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/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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/20112—Image segmentation details
    • G06T2207/20128—Atlas-based segmentation
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明属于地图构建技术领域，更具体地，涉及一种多激光雷达协同的地图构建方法及其系统，方法包括以下步骤：多个激光雷达分别获取同一建图区域的原始点云数据，并根据原始点云数据求解得到多个激光雷达之间的外参初始值；对原始点云数据进行特征提取，得到特征g′；根据特征g′对原始点云数据进行特征匹配，得到初始化的前端位姿；对前端位姿进行优化，得到全局优化位姿以及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定输出优化外参；根据优化外参更新外参初始值。本发明提高了多激光雷达协同建图的外参标定精准度，同时还降低了系统的成本。

Description

一种多激光雷达协同的地图构建方法及其系统
技术领域
本发明属于地图构建技术领域，更具体地，涉及一种多激光雷达协同的地图构建方法及其系统。
背景技术
即时定位与地图构建(SLAM)技术一般指机器人或自主运动设备在未知环境下运动，利用其搭载的传感器对周围环境进行观测，得到自身的位姿和周围环境信息并构建出其周围环境的地图。随着无人驾驶以及机器人行业的高速发展，SLAM的应用频率越来越高，应用场景也越来越丰富。
当前，SLAM技术最常用的传感器为激光雷达，激光雷达可以获取环境的点云信息，点云信息包含激光点的三维空间坐标、反射强度和激光获取时间；传统的SLAM一般只使用一个激光雷达，其只能通过增加激光雷达的线数以提高点云的密度和建图精度，这样单个的激光雷达容易受到天气环境的影响或发生其他故障，造成无法建图，因此其鲁棒性不够高、建图效果并不稳定，且存在视野盲区，另外高线数的激光雷达成本较高。
为解决上述问题，现有技术方案中也有采用多个小型激光雷达协同建图以提高系统对环境的感知能力以及系统的稳定性，但是多个激光雷达需要进行有效的外参标定才能完成有效的数据融合，从而提高建图的精准度。然而，当前多个激光雷达的外参标定不够准确，且无法在建图过程中实时对外参进行优化。
发明内容
本发明为克服上述现有技术中的至少一个缺陷，提供一种多激光雷达协同的地图构建方法及其系统，其可以提高多激光雷达外参标定的精准度。
为解决上述技术问题，本发明采用的技术方案是：
提供一种多激光雷达协同的地图构建方法，包括以下步骤：
S1：多个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到多个激光雷达之间的外参初始值；
S2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征g′；
S3：基于关键帧和滑动窗口，利用点面特征g′对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；
S4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤S1中更新外参初始值；
S5：重复执行步骤S1至步骤S4，持续输出多个全局优化地图，根据多个全局优化地图构建SLAM地图。
本方案中先通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度。
优选地，上述的步骤S1中具体包括以下步骤：
S11：对原始点云数据进行预处理；
S12：利用手眼标定法对经步骤S11预处理后的点云数据求解，得到多个激光雷达之间的外参初始值。
优选地，上述的步骤S2中对原始点云数据进行特征提取具体包括以下步骤：
S21：通过计算预处理后的点云数据内的平面光滑度，提取出边缘特征
Figure BDA0003444277850000021
与平面特征g；
S22：对平面特征g进行二次平面提取，得到点面特征g′。
优选地，上述的步骤S22中对平面特征g进行二次平面提取，得到点面特征g′具体包括以下步骤：
S221：计算得到平面特征g的点云法向量，记为N；
S222：应用区域生长算法对点云法向量N进行计算，得到多个平面聚类的点集D，其中D＝{d1，...，dn}；
S223：遍历点集D中的所有元素；
S224：设当前平面聚类的法向量为
Figure BDA0003444277850000022
拟合的平面方程为
Figure BDA0003444277850000023
其中，p为原始点云，d为平面聚类中的点，T表示转置；
S225：求解平面方程与聚类中的每个点构建的超定方程
Figure BDA0003444277850000024
得到n与d，其中||n||＝1；
S225：遍历平面聚类D中的所有元素，并代入平面方程计算得到
Figure BDA0003444277850000031
S226：判断y是否小于y’，若是，则令g′＝g′∪pi，并输出点面特征g′，若否则返回步骤S221。
优选地，上述的步骤S3中基于关键帧和滑动窗口，利用点面特征g′对预处理后的点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：
S31：提取预处理后的点云数据的当前帧并标记为下一帧Ti+1；
S32：遍历预处理后的点云数据的所有关键帧；
S33：利用下一帧Ti+1将边缘特征
Figure BDA0003444277850000032
与平面特征gi+1转换至世界坐标系下，得到
Figure BDA0003444277850000033
与g′i+1；
S34：利用
Figure BDA0003444277850000034
计算边缘特征
Figure BDA0003444277850000035
中的有效特征点pl到滑动窗口最近直线的残差redge；利用g’i+1计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；
S35：求解最小化目标函数T*i+1＝argminTi+1{∑redge+∑rplane}；
S36：输出匹配后的关键帧T*i+1，即得到前端位姿。
优选地，上述的步骤S4具体包括以下步骤：
S41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；
S42：采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；
S43：利用全局优化位姿对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。
优选地，上述的步骤S43具体包括以下步骤：
S431：获取多个激光雷达的关键帧集合；
S432：利用关键帧集合构建当前时刻下的局部地图；
S433：计算统计关键帧集合中所有特征点的残差；
S434：计算并统计关键帧集合中所有特征点的有效平面残差；
S435：计算子目标函数fi＝∑redge+∑rplane首次线性化时的特征值；
S436：根据特征值更新子目标函数为fT；
S437：根据更新后的子目标函数fT对目标函数T*AB＝arg minTABfT进行优化，最终输出优化后的T*AB，即为优化外参。
优选地，上述的步骤S41中利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿具体包括以下步骤：
S411：通过体素划分对预处理后的点云数据的特征进行结构化；
S412：设置滑动窗口大小为m，k＝3；
S413：将经步骤S411结构化的特征放入体素中；
S414：计算体素内特征的协方差；
S415：判断k是否大于m，若是，则进入步骤42，若否，则待滑动窗口容量饱满后返回步骤S413。
优选地，上述的步骤S42采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图具体包括以下步骤：
S421：求解滑动窗口内的最优位姿，获取关键帧集合；
S422：遍历滑动窗口内的位姿并更新至关键帧集合；
S423：构建因子图；
S424：将步骤422中的关键帧集合中部分关键帧边缘化并发布至因子图中，构建里程计因子以及进行回环检测；
S425：利用剩余的关键帧构建当前滑动窗口副本；
S426：在体素中边缘化关键帧对应的体素与特征，得到全局优化位姿以及全局优化地图。
本方案中还提供一种多激光雷达协同的地图构建系统，包括数据采集系统、前端里程计以及后端优化系统、在线外参标定系统；
数据采集系统用于获取同一建图区域的原始点云数据并发送至前端里程计；
前端里程计用于利用原始点云数据求解得到多个激光雷达之间的外参初始值，并对原始电源数据进行特征提取，得到点面特征g′，根据点面特征g′对原始点云数据进行特征匹配，得到初始化的前端位姿并输入至后端优化系统；
后端优化系统用于对前端位姿进行优化，得到全局优化位姿以及全局优化地图，并输出SLAM地图；
在线外参标定系统用于根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至前端里程计中。
与现有技术相比，有益效果是：
本发明一方面通过多激光雷达协同进行地图构建，可以增强建图的鲁棒性，且避免了单个稠密激光雷达的视野盲区问题，提高了建图效果，且多个稀疏激光雷达相较于单个稠密激光雷达成本更低；另一方面，通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度。
附图说明
图1是本发明实施例1多激光雷达协同的地图构建方法的整体流程示意框图；
图2是本发明实施例2多激光雷达协同的地图构建方法的在线外参标定优化流程示意框图；
图3是本发明实施例3多激光雷达协同的地图构建系统的整体连接示意框图。
具体实施方式
附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”“长”“短”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：
实施例1：
如图1所示为一种多激光雷达协同的地图构建方法的第一实施例，包括以下步骤：
S1：两个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到两个激光雷达之间的外参初始值；
S2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征g′；
S3：基于关键帧和滑动窗口，利用点面特征g′对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；
S4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤S1中更新外参初始值；
S5：重复执行步骤S1至步骤S4，持续输出多个全局优化地图，根据多个全局优化地图构建SLAM地图。
本实施例中的两个激光雷达包括一个主激光雷达以及一个辅助激光雷达，这样便于后续的计算过程中对两个激光雷达的数据进行判别。
本实施例中的步骤S1中具体包括以下步骤：
S11：对原始点云数据进行预处理；具体地，预处理包括对原始点云数据进行过滤以去除部分噪声点，以避免噪声点对特征提取等操作的影响；同时对原始点云数据进行降采样，减少后续处理过程中的计算量，从而提高地图构建的速度；
S12：利用手眼标定法对经步骤S11预处理后的点云数据求解，得到两个激光雷达之间的外参初始值。本实施例中采用基于欧式变换的线性插值方法对两个激光雷达进行运动对齐，得到运动对齐后的多组位姿序列及式，使得两个激光雷达的关键帧位姿上时间戳连贯。
本实施例中的步骤S2中对原始点云数据进行特征提取具体包括以下步骤：
S21：通过计算预处理后的点云数据内的平面光滑度，提取出边缘特征
Figure BDA0003444277850000061
与平面特征g；
S22：对平面特征g进行二次平面提取，得到点面特征g′。进行二次平面提取可以挑选出底面、曲率较低的曲面或者平面等强平面特征以进行汇聚得到同样为强平面特征的特征g′，其记录了有效的平面约束，降低平面特征点云数量，利用其进行关键帧间匹配可以有效提高关键帧匹配的精度以及匹配效率；而对于树叶等不平整的弱平面特征，其会用于后续的滑动窗口的特征匹配。
本实施例中的步骤S22中对平面特征g进行二次平面提取，得到特征g′具体包括以下步骤：
S221：计算得到平面特征g的点云法向量，记为N；
S222：应用区域生长算法对点云法向量N进行计算，得到两个平面聚类的点集D，其中D＝{d1，...，dn}；
S223：遍历点集D中的所有元素；
S224：设当前平面聚类的法向量为
Figure BDA0003444277850000075
拟合的平面方程为
Figure BDA0003444277850000076
其中，p为原始点云，d为平面聚类中的点，T表示转置；
S225：求解平面方程与聚类中的每个点构建的超定方程
Figure BDA0003444277850000077
得到n与d，其中||n||＝1；
S225：遍历平面聚类D中的所有元素，并代入平面方程计算得到
Figure BDA0003444277850000078
S226：判断y是否小于y’，若是，则令g′＝g′Upi，并输出点面特征g′，若否则返回步骤S221。
本实施例中的步骤S3中基于关键帧和滑动窗口，利用点面特征g′对原始点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：
S31：提取预处理后的点云数据的当前帧并标记为下一帧Ti+1；
S32：遍历预处理后的点云数据的所有关键帧；
S33：利用下一帧Ti+1将边缘特征
Figure BDA0003444277850000071
i+1与平面特征gi+1转换至世界坐标系下，得到
Figure BDA0003444277850000072
’i+1与g’i+1；
S34：利用
Figure BDA0003444277850000073
’i+1计算边缘特征
Figure BDA0003444277850000074
i+1中的有效特征点pl到滑动窗口最近直线的残差redge；利用g’i+1计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；
S35：求解最小化目标函数T*i+1＝argminTi+1{∑redge+∑rplane}；
S36：输出匹配后的关键帧T*i+1，即得到前端位姿。
本实施例中的步骤S4具体包括以下步骤：
S41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；
S42：采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；
S43：利用全局优化位姿以及局部地图对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。
本实施例中的步骤S43具体包括以下步骤：
S431：获取两个激光雷达的关键帧集合；
S432：利用关键帧集合构建当前时刻下的局部地图；
S433：计算统计关键帧集合中所有特征点的残差；
S434：计算并统计关键帧集合中所有特征点的有效平面残差；
S435：计算子目标函数fi＝∑redge+∑rplane首次线性化时的特征值；
S436：根据特征值更新子目标函数为fT；
S437：根据更新后的子目标函数fT对目标函数T*AB＝arg minTABfT进行优化，最终输出优化后的T*AB，即为优化外参。
其中fi=∑redge+∑rplane，为辅助激光雷达的每一帧特征点云到局部地图的子目标函数。
这样是由于初始化的外参标定过程中的运动估计可能会存在漂移误差，两个激光雷达之间可能会出现刚体变化而影响采集结果。本实施例中的在线外参标定方法无需借助其他辅助传感器，直接通过多组固定的激光雷达，利用得到的局部地图对外参初值进行优化，多次迭代后得到一个可靠稳定且准确的外参标定结果，其能够精准地表示多个雷达之间的位置关系，同时，准确的外参标定结果也可以反馈至数据采集以及建图过程中，以缩小数据采集时可能产生的误差，进而提高建图的准确性。
本实施例中的步骤S41中利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿具体包括以下步骤：
S411：通过体素划分对预处理后的点云数据的特征进行结构化；
S412：设置滑动窗口大小为m，k＝3；
S413：将经步骤S411结构化的特征放入体素中；
S414：计算体素内特征的协方差；
S415：判断k是否大于m，若是，则进入步骤42，若否，则待滑动窗口容量饱满后返回步骤S413。
本实施例中的步骤S42采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图具体包括以下步骤：
S421：求解滑动窗口内的最优位姿，获取关键帧集合；
S422：遍历滑动窗口内的位姿并更新至关键帧集合；
S423：构建因子图；
S424：将步骤422中的关键帧集合中部分关键帧边缘化并发布至因子图中，构建里程计因子以及进行回环检测；
S425：利用剩余的关键帧构建当前滑动窗口副本；
S426：在体素中边缘化关键帧对应的体素与特征，得到全局优化位姿以及全局优化地图。
需要说明的是，本实施例中激光雷达的数量为两个仅为参考，不能理解为对本方案的限定，具体实施过程中，当然可以根据实际情况增加激光雷达的数量，其建图过程与本实施例相同，这里不再详述。
本实施例中一方面通过多激光雷达协同进行地图构建，可以增强建图的鲁棒性，且避免了单个稠密激光雷达的视野盲区问题，提高了建图效果，且两个稀疏激光雷达相较于单个稠密激光雷达成本更低；另一方面，通过原始点云数据求解出外参初始值，然后再对前端位姿优化完成后，再利用外参初始值进行优化标定，从而输出优化外参，以实现在线外参标定，输出稳定精准的外参结果，提高多激光雷达融合建图的精准度，避免了多个激光雷达融合建图出现的外参标定结果不够准确、后续无法进行优化的问题。
实施例2：
如图2所示为一种多激光雷达协同的地图构建方法的第二实施例，本实施例与实施例1的区别仅在于，本实施例步骤S2中对原始点云数据进行特征提取还会根据原始点云数据内的平面光滑度的大小将特征点云分为强特征与弱特征，本实施例中先提取得到四种强弱特征，即强边缘特征、弱边缘特征、强平面特征与弱平面特征，然后将强边缘特征与弱边缘特征合成为边缘特征，将强平面特征与弱平面特征合成为平面特征，后续对平面特征g进行二次提取，得到特征g′。本实施例中的弱平面特征会加入到滑动窗口中进行特征匹配，以增加特征匹配的精准度。
实施例3：
如图3所示为用于实现上述实施例1或实施例2的一种多激光雷达协同的地图构建系统的实施例，包括数据采集系统、前端里程计以及后端优化系统、在线外参标定系统；数据采集系统与前端里程计通信电连接，前端里程计与后端优化系统通信电连接，后端优化系统与在线外参标定系统通信连接，在线外参标定系统还与数据采集系统通信连接。
数据采集系统用于获取同一建图区域的原始点云数据并发送至前端里程计；
前端里程计用于利用原始点云数据求解得到多个激光雷达之间的外参初始值，并对原始电源数据进行特征提取，得到点面特征g′，根据点面特征g′对原始点云数据进行特征匹配，得到初始化的前端位姿并输入至后端优化系统；
后端优化系统用于对前端位姿进行优化，得到全局优化位姿以及全局优化地图，并输出SLAM地图；
在线外参标定系统用于根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至前端里程计中；本实施例中优化外参同时返回至数据采集系统中，以提高数据采集的精准度。
本发明是参照本申请实施例的方法、设备(系统)、和计算机程序产品的流程图或方框图来描述的，应理解可由计算机程序指令实现流程图或方框图中的每一流程或方框、以及流程图或方框图中的流程或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种多激光雷达协同的地图构建方法，其特征在于，包括以下步骤：
S1：多个激光雷达分别获取同一建图区域的原始点云数据，对原始点云数据进行预处理，基于预处理后的点云数据求解得到多个激光雷达之间的外参初始值；
S2：对预处理后的点云数据进行特征提取，得到边缘特征、平面特征，并对平面特征进行二次提取得到点面特征
Figure FDA0003444277840000011
S3：基于关键帧和滑动窗口，利用所述点面特征
Figure FDA0003444277840000012
对预处理后的点云数据进行特征匹配，得到初始化的前端位姿；
S4：基于集束调整对前端位姿进行局部优化，得到局部优化位姿；对全局位姿进行优化得到全局优化位姿及全局优化地图；根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至步骤S1中更新外参初始值；
S5：重复执行步骤S1至步骤S4，持续输出多个全局优化地图，根据多个全局优化地图构建SLAM地图。
2.根据权利要求1所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S1中具体包括以下步骤：
S11：对原始点云数据进行预处理；
S12：利用手眼标定法对经步骤S11预处理后的点云数据求解，得到多个激光雷达之间的外参初始值。
3.根据权利要求2所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S2中所述对原始点云数据进行特征提取具体包括以下步骤：
S21：通过计算预处理后的点云数据的平面光滑度，提取出边缘特征
Figure FDA0003444277840000019
与平面特征
Figure FDA0003444277840000013
S22：对平面特征
Figure FDA0003444277840000014
进行二次平面提取，得到点面特征
Figure FDA0003444277840000015
4.根据权利要求3所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S22中所述对平面特征
Figure FDA0003444277840000016
进行二次平面提取，得到点面特征
Figure FDA0003444277840000017
具体包括以下步骤：
S221：计算得到平面特征
Figure FDA0003444277840000018
的点云法向量，记为N；
S222：应用区域生长算法对点云法向量N进行计算，得到多个平面聚类的点集D，其中D＝{d1,...,dn}；
S223：遍历点集D中的所有元素；
S224：设当前平面聚类的法向量为
Figure FDA0003444277840000021
拟合的平面方程为
Figure FDA0003444277840000022
其中，p为原始点云，d为平面聚类中的点，T表示转置；
S225：求解平面方程与聚类中的每个点构建的超定方程
Figure FDA0003444277840000023
得到n与d，其中‖n‖＝1；
S225：遍历平面聚类D中的所有元素，并代入平面方程计算得到
Figure FDA0003444277840000024
S226：判断y是否小于y’，若是，则令
Figure FDA0003444277840000025
并输出点面特征
Figure FDA0003444277840000026
若否则返回步骤S221。
5.根据权利要求4所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S3中所述基于关键帧和滑动窗口，利用所述点面特征
Figure FDA0003444277840000027
对预处理后的点云数据进行特征匹配，得到初始化的前端位姿具体包括以下步骤：
S31：提取预处理后的点云数据的当前帧并标记为下一帧Ti+1；
S32：遍历预处理后的点云数据的所有关键帧；
S33：利用下一帧Ti+1将边缘特征
Figure FDA0003444277840000028
与平面特征
Figure FDA0003444277840000029
转换至世界坐标系下，得到
Figure FDA00034442778400000210
与
Figure FDA00034442778400000211
S34：利用
Figure FDA00034442778400000212
计算边缘特征
Figure FDA00034442778400000213
中的有效特征点pl到滑动窗口最近直线的残差redge；利用
Figure FDA00034442778400000214
计算平面特征中有效特征点pl到滑动窗口最近平面的残差redge；
S35：求解最小化目标函数T*i+1＝argminTi+1{∑redge+∑rplane}；
S36：输出匹配后的关键帧T*i+1，即得到前端位姿。
6.根据权利要求1至5任一项所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S4具体包括以下步骤：
S41：利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿；
S42：采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图；
S43：利用全局优化位姿对初始外参进行进一步优化标定，得到优化外参，若标定结果没有收敛，则保持外参初始值不变，若标定结果收敛，则将优化外参替换外参初始值。
7.根据权利要求6所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S41中所述利用集束调整对前端位姿进行局部优化，并利用滑动窗口进行调整，得到局部优化位姿具体包括以下步骤：
S411：通过体素划分对预处理后的点云数据的特征进行结构化；
S412：设置滑动窗口大小为m，k＝3；
S413：将经步骤S411结构化的特征放入体素中；
S414：计算体素内特征的协方差；
S415：判断k是否大于m，若是，则进入步骤42，若否，则待滑动窗口容量饱满后返回步骤S413。
8.根据权利要求7所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S42采用ISAM2方法，基于因子图对全局位姿进行优化，得到全局优化位姿以及全局优化地图具体包括以下步骤：
S421：求解滑动窗口内的最优位姿，获取关键帧集合；
S422：遍历滑动窗口内的位姿并更新至关键帧集合；
S423：构建因子图；
S424：将步骤422中的关键帧集合中部分关键帧边缘化并发布至因子图中，构建里程计因子以及进行回环检测；
S425：利用剩余的关键帧构建当前滑动窗口副本；
S426：在体素中边缘化关键帧对应的体素与特征，得到全局优化位姿以及全局优化地图。
9.根据权利要求8所述的一种多激光雷达协同的地图构建方法，其特征在于，步骤S43具体包括以下步骤：
S431：获取多个激光雷达的关键帧集合；
S432：利用所述关键帧集合构建当前时刻下的局部地图；
S433：计算统计所述关键帧集合中所有特征点的残差；
S434：计算并统计所述关键帧集合中所有特征点的有效平面残差；
S435：计算子目标函数fi＝∑redge+∑rplane首次线性化时的特征值；
S436：根据所述特征值更新子目标函数为fT；
S437：根据更新后的子目标函数fT对目标函数T*AB＝arg minTABfT进行优化，最终输出优化后的T*AB，即为优化外参。
10.一种用于实现权利要求1至9任一项所述的多激光雷达协同的地图构建方法的系统，其特征在于，
包括数据采集系统、前端里程计以及后端优化系统、在线外参标定系统；
所述数据采集系统用于获取同一建图区域的原始点云数据并发送至前端里程计；所述前端里程计用于利用所述原始点云数据求解得到多个激光雷达之间的外参初始值，并对原始电源数据进行特征提取，得到点面特征
Figure FDA0003444277840000041
根据点面特征
Figure FDA0003444277840000042
对原始点云数据进行特征匹配，得到初始化的前端位姿并输入至后端优化系统；所述后端优化系统用于对所述前端位姿进行优化，得到全局优化位姿以及全局优化地图，并输出SLAM地图；
所述在线外参标定系统用于根据全局优化位姿与外参初始值对多个激光雷达的外参进行优化标定，输出优化外参，并将优化外参返回至前端里程计中。
CN202111648187.2A 2021-12-29 2021-12-29 一种多激光雷达协同的地图构建方法及其系统 Pending CN114255324A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111648187.2A CN114255324A (zh) 2021-12-29 2021-12-29 一种多激光雷达协同的地图构建方法及其系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111648187.2A CN114255324A (zh) 2021-12-29 2021-12-29 一种多激光雷达协同的地图构建方法及其系统

Publications (1)

Publication Number Publication Date
CN114255324A true CN114255324A (zh) 2022-03-29

Family

ID=80795769

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111648187.2A Pending CN114255324A (zh) 2021-12-29 2021-12-29 一种多激光雷达协同的地图构建方法及其系统

Country Status (1)

Country Link
CN (1) CN114255324A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115236644A (zh) * 2022-07-26 2022-10-25 广州文远知行科技有限公司 一种激光雷达外参标定方法、装置、设备和存储介质
CN117289298A (zh) * 2023-10-19 2023-12-26 广州行深智能科技有限公司 基于激光雷达的多机协同在线建图方法、系统及终端设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115236644A (zh) * 2022-07-26 2022-10-25 广州文远知行科技有限公司 一种激光雷达外参标定方法、装置、设备和存储介质
CN117289298A (zh) * 2023-10-19 2023-12-26 广州行深智能科技有限公司 基于激光雷达的多机协同在线建图方法、系统及终端设备

Similar Documents

Publication Publication Date Title
Rozenberszki et al. LOL: Lidar-only Odometry and Localization in 3D point cloud maps
CN107301654B (zh) 一种多传感器的高精度即时定位与建图方法
CN114255324A (zh) 一种多激光雷达协同的地图构建方法及其系统
CN111798475A (zh) 一种基于点云深度学习的室内环境3d语义地图构建方法
CN101877148B (zh) 基于全局结构的三维网格模型修复方法
CN110223298A (zh) 基于点云局部结构的语义分割改进算法
CN113538486B (zh) 一种提高汽车钣金工件的识别与定位精度的方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN115880364B (zh) 基于激光点云和视觉slam的机器人位姿估计方法
CN112904358A (zh) 基于几何信息的激光定位方法
WO2022021661A1 (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
CN113985435A (zh) 一种融合多激光雷达的建图方法及系统
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法
CN113077497A (zh) 基于3d ndt-icp算法的掘进巷道点云配准方法
CN114998276A (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
CN110688440B (zh) 一种适用于子地图重叠部分较少的地图融合方法
CN116429116A (zh) 一种机器人定位方法及设备
CN115439621A (zh) 一种煤矿井下巡检机器人三维地图重建及目标检测方法
CN115280960A (zh) 一种基于田间视觉slam的联合收获机转向控制方法
CN115143958A (zh) 一种基于gpu加速的多传感器融合的slam方法
CN110568437A (zh) 一种基于雷达辅助的精确环境建模方法
CN117169942A (zh) 一种基于LiDAR/GPS/IMU融合的无人车重定位方法
CN115034987B (zh) 一种基于slam和调度系统的地图双向更新适配装置
CN115050010B (zh) 一种针对三维物体检测器的迁移学习方法
CN114202586A (zh) 基于八叉图和三维正态分布优化匹配的3d激光slam方法及移动机器人

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