一种显式几何特征匹配的激光雷达SLAM方法

doi:10.3969/j.issn.1004-132X.2025.08.017

摘要/Abstract

摘要：

目前多数LiDAR-SLAM系统采用前端里程计估计初始位姿和后端优化位姿的方法，缺少批量的后端优化方案。针对此问题，提出了一个完整的基于显式几何特征的激光雷达同时定位与建图（SLAM）系统。采用凝聚层次聚类方法实现平面特征点云平面分割并通过计算点云的局部曲率值筛选直线特征点；通过配准点云特征和特征子地图实现激光雷达运动的初始位姿估计；采用基于直线和平面基元的局部状态优化方法，基于因子图模型融合了直线因子和平面因子，通过最小化直线到直线和平面到平面的残差，实现了位姿、直线和平面参数的联合批量优化。实验结果表明，所提SLAM系统在其他场景下也能实现较高精度的定位和地图构建，满足SLAM的实时性要求。

关键词: 同时定位与建图, 激光雷达里程计, 特征提取, 非线性优化

Abstract:

Currently， most LiDAR-SLAM systems utilized front-end odometry to estimate the initial pose and back-end optimization to refine the pose， but they lacked batch back-end optimization approaches. To address these issues， a comprehensive LiDAR-SLAM system was proposed based on explicit geometric features. This system employed agglomerative hierarchical clustering for plane feature point cloud segmentation and employed local curvature computation to filter linear feature points. Furthermore， the initial pose estimation of LiDAR motion was achieved through registering point cloud features and submap features. A local state optimization method was utilized based on linear and planar primitives， where linear and planar factors were merged within a factor graph model. By minimizing residuals between linear-to-linear and plane-to-plane associations， joint batch optimization of pose， linear， and planar parameters was achieved. Experimental results demonstrate that the proposed SLAM system achieves high precision localization and map construction in various scenarios， meeting real-time SLAM requirements.

Key words: simultaneous localization and mapping（SLAM）, LiDAR odometry, feature extraction, nonlinear optimization

中图分类号:

TP242.6

张红彦, 赵昊阳, 赵焕峰, 李念轩, 孙钦政, 黄玲涛. 一种显式几何特征匹配的激光雷达SLAM方法[J]. 中国机械工程, 2025, 36(8): 1824-1831.

Hongyan ZHANG, Haoyang ZHAO, Huanfeng ZHAO, Nianxuan LI, Qinzheng SUN, Lingtao HUANG. An Explicit Geometric Feature Matching LiDAR SLAM Method[J]. China Mechanical Engineering, 2025, 36(8): 1824-1831.

导出引用管理器 EndNote|Ris|BibTeX

链接本文: https://www.cmemo.org.cn/CN/10.3969/j.issn.1004-132X.2025.08.017

https://www.cmemo.org.cn/CN/Y2025/V36/I8/1824

图/表 17

参考文献 21

[1]	GALVEZ-LÓPEZ D， TARDOS J D. Bags of Binary Words for Fast Place Recognition in Image Sequences［J］. IEEE Transactions on Robotics， 2012， 28（5）：1188-1197.
[2]	RIBEIRO M I. Kalman and Extended Kalman Filters： Concept， Derivation and Properties［J］. Institute for Systems and Robotics， 2004， 43（46）：3736-3741.
[3]	MURPHY K， RUSSELL S. Rao-blackwellised Particle Filtering for Dynamic Bayesian Networks［M］∥Sequential Monte Carlo Methods in Practice. New York： Springer New York， 2001：499-515.
[4]	THRUN S. Probabilistic Robotics［J］. Communications of the ACM， 2002， 45（3）：52-57.
[5]	OLSON E B. Real-time Correlative Scan Matching［C］∥2009 IEEE International Conference on Robotics and Automation. Kobe， 2009：4387-4393.
[6]	ZHANG Ji， SINGH S. LOAM：Lidar Odometry and Mapping in Real-time［C］∥Robotics：Science and Systems. Berkeley， 2014：1-9.
[7]	SHAN Tixiao， ENGLOT B. LeGO-LOAM：Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain［C］∥2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Madrid， 2018：4758-4765.
[8]	WANG Han， WANG Chen， CHEN Chunlin， et al. F-LOAM：Fast LiDAR Odometry and Mapping［C］∥2021 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Prague， 2021：4390-4396.
[9]	PAN Yue， XIAO Pengchuan， HE Yujie， et al. MULLS： Versatile LiDAR SLAM via Multi-metric Linear Least Square［C］∥2021 IEEE International Conference on Robotics and Automation （ICRA）. Xi'an， 2021：11633-11640.
[10]	YANG Heng， SHI Jingnan， CARLONE L. TEASER：Fast and Certifiable Point Cloud Registration［J］. IEEE Transactions on Robotics， 2021， 37（2）：314-333.
[11]	SÜNDERHAUF N， PROTZEL P. Towards a Robust Back-end for Pose Graph SLAM［C］∥2012 IEEE International Conference on Robotics and Automation. St Paul， 2012：1254-1261.
[12]	WANG Han， WANG Chen， XIE Lihua. Intensity-SLAM：Intensity Assisted Localization and Mapping for Large Scale Environment［J］. IEEE Robotics and Automation Letters， 2021， 6（2）：1715-1721.
[13]	WOLD S， ESBENSEN K， GELADI P. Principal Component Analysis［J］. Chemometrics and Intelligent Laboratory Systems， 1987， 2（1/2/3）：37-52.
[14]	BEHLEY J， STACHNISS C. Efficient Surfel-based SLAM Using 3D Laser Range Data in Urban Environments［C］∥Robotics： Science and Systems. Pittsburgh， 2018：46954808.
[15]	FENG Chen， TAGUCHI Y， KAMAT V R. Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering［C］∥2014 IEEE International Conference on Robotics and Automation （ICRA）. Hong Kong， 2014：6218-6225.
[16]	GAO Haiming， ZHANG Xuebo， FANG Yongchun， et al. A Line Segment Extraction Algorithm Using Laser Data Based on Seeded Region Growing［J］. International Journal of Advanced Robotic Systems， 2018， 15：1729881418755245.
[17]	ZUO Xingxing， XIE Xiaojia， LIU Yong， et al. Robust Visual SLAM with Point and Line Features［C］∥2017 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Vancouver， 2017：1775-1782.
[18]	GENEVA P， ECKENHOFF K， YANG Yulin， et al. Lips：LiDAR-inertial 3D Plane Slam［C］∥2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Madrid， 2018：123-130.
[19]	ZHANG L， CAMURRI M， WISTH D， et al. Multi-camera LiDAR Inertial Extension to the Newer College Dataset［M］∥arXiv Preprint， 2022.
[20]	LIU Zheng， LI Haotian， YUAN Chongjian， et al. Voxel-SLAM：a Complete， Accurate， and Versatile LiDAR-Inertial SLAM System ［EB/OL］. arXiv：2410.08935， 2024-10-11［2024-10-20］. .
[21]	XU Ziheng， LI Qingfeng， CHEN Chen， et al. GLC-SLAM：Gaussian Splatting SLAM with Efficient Loop Closure ［EB/OL］. arXiv：2409.10982， 2024-09-17［2024-10-20］. .

序列名称	场景	帧数	难度	时长/s	长度/m
序列1	方形庭院	1991	简单	198	246.7
序列2	方形庭院	1910	中等	190	260.4
序列3	环形走廊	2788	简单	190	428.8
序列4	数学研究所	2160	简单	216	263.6
序列5	数学研究所	1770	中等	176	176.9
序列6	地下矿洞	1412	简单	141	162.5
序列7	地下矿洞	1487	中等	148	174.1

序列名称	场景	帧数	难度	时长/s	长度/m
序列1	方形庭院	1991	简单	198	246.7
序列2	方形庭院	1910	中等	190	260.4
序列3	环形走廊	2788	简单	190	428.8
序列4	数学研究所	2160	简单	216	263.6
序列5	数学研究所	1770	中等	176	176.9
序列6	地下矿洞	1412	简单	141	162.5
序列7	地下矿洞	1487	中等	148	174.1

方法	序列1	序列2	序列3	序列4	序列5	序列6	序列7	误差均值
A-LOAM	0.0856	0.4034	0.2639	0.0863	0.8257	0.1451	0.1935	0.2862
LeGO-LOAM	0.0929	0.5453	0.1865	0.1303	0.4315	0.1081	0.1311	0.2322
LL-LO	0.1139	0.3569	0.2942	0.1522	0.3162	0.1255	0.1629	0.2174
LP-LO	0.1068	0.3847	0.1742	0.1182	0.2835	0.1138	0.1497	0.1901
LPL-LO	0.0888	0.2912	0.1224	0.0804	0.2617	0.0919	0.1244	0.1515
LPL-SLAM	0.0865	0.2891	0.1027	0.0685	0.2388	0.0683	0.0935	0.1353

方法	序列1	序列2	序列3	序列4	序列5	序列6	序列7	误差均值
A-LOAM	0.0856	0.4034	0.2639	0.0863	0.8257	0.1451	0.1935	0.2862
LeGO-LOAM	0.0929	0.5453	0.1865	0.1303	0.4315	0.1081	0.1311	0.2322
LL-LO	0.1139	0.3569	0.2942	0.1522	0.3162	0.1255	0.1629	0.2174
LP-LO	0.1068	0.3847	0.1742	0.1182	0.2835	0.1138	0.1497	0.1901
LPL-LO	0.0888	0.2912	0.1224	0.0804	0.2617	0.0919	0.1244	0.1515
LPL-SLAM	0.0865	0.2891	0.1027	0.0685	0.2388	0.0683	0.0935	0.1353

方法	序列1	序列2	序列3	序列4	序列5	序列6	序列7	平均误差
A-LOAM	1.3262	2.1683	1.8402	0.6375	3.9587	2.1558	2.6240	2.1015
LeGO-LOAM	1.3705	2.3082	1.9917	0.8802	3.3395	2.3710	2.3344	2.0851
LL-LO	2.1462	2.3851	2.3518	0.9573	3.6842	2.3568	2.5713	2.3504
LP-LO	1.7438	2.4692	2.1566	0.8359	3.1183	2.1366	2.1724	2.0904
LPL-LO	1.3405	2.2011	1.6433	0.5451	2.9053	1.9088	2.1230	1.8096
LPL-SLAM	1.2309	2.1816	1.5943	0.5553	2.8719	1.9140	2.0116	1.7656