【激光雷达LIDAR】2D激光雷达SLAM中的实时闭环检测（Matlab代码实现）

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

文献来源：

摘要
便携式激光测距仪，通常称为激光雷达（LIDAR），以及同时定位与地图构建（SLAM），是获取现场建筑平面图的高效方法。实时生成和可视化平面图帮助操作员评估捕获数据的质量和覆盖范围。构建便携式捕获平台需要在有限的计算资源下运行。我们介绍了我们背包式地图平台的方法，能够以5厘米分辨率实现实时地图构建和闭环检测。为了实现实时闭环检测，我们采用分支定界方法计算扫描与子地图匹配作为约束条件。我们提供了实验结果，并与其他知名方法进行了比较，结果显示我们的方法在质量上与已建立的技术相竞争。

现场建筑平面图在各种应用中非常有用。

手动调查以收集用于建筑管理任务的数据，通常结合计算辅助设计（CAD）和激光测距仪。这些方法速度较慢，并且通过采用人类对建筑物的预设概念为直线集合，并不能总是准确描述空间的真实性质。使用SLAM，可以迅速而准确地调查各种规模和复杂性的建筑物，这些建筑物在手动调查中需要花费数量级更长的时间。

在这一领域应用SLAM并非一个新思路，也不是本文的重点。相反，本文的贡献是提出了一种新颖的方法，用于降低从激光测距数据计算闭环约束所需的计算需求。这种技术使我们能够绘制非常大的平面，数万平方米，同时实时为操作员提供完全优化的结果。

运行 main.m
如果您想测试闭环检测（仅检测，尚无姿态图优化）功能，请取消 main.m 中的 'Loop Closing' 代码的引用。
将激光点注册到概率网格地图中，这将有助于提高扫描匹配性能。[1]
紧密耦合激光和IMU以提高鲁棒性和效率。
估计两个连续关键扫描之间的相对姿态，按照 [1] 中的方法估计相对姿态的协方差。
使用姿态图优化来闭合环路。
使用分支定界法加快蛮力扫描匹配速度。

2D激光雷达SLAM中的实时闭环检测研

摘要

实时闭环检测是2D激光雷达SLAM（同步定位与地图构建）的核心技术之一，其通过识别机器人重访已探索区域，修正累积误差，保障地图一致性。本文聚焦于基于2D激光雷达的实时闭环检测方法，分析其技术原理、典型算法及优化策略，并结合Cartographer等开源系统，探讨实时性、精度与鲁棒性的平衡方案。

1. 引言

在室内导航、仓储物流、建筑测绘等场景中，2D激光雷达SLAM因低成本、高精度和强鲁棒性成为主流方案。然而，受限于传感器分辨率和计算资源，传统方法易因累积误差导致地图漂移。闭环检测通过识别“回环”场景（即机器人重新访问已探索区域），利用全局约束优化位姿图，可显著提升SLAM系统的长期一致性。实时闭环检测需在有限算力下快速完成特征提取、匹配和优化，是当前研究的热点与难点。

2. 实时闭环检测技术原理

2.1 闭环检测的基本流程

实时闭环检测通常包含以下步骤：

特征提取：从激光扫描数据中提取角点、边缘、线段等几何特征，或生成直方图、点云描述子等全局特征。
相似性搜索：通过特征匹配或几何约束，在历史关键帧中筛选潜在闭环候选。
位姿优化：利用匹配结果构建位姿图约束，通过非线性优化（如Levenberg-Marquardt算法）修正全局位姿。

2.2 实时性优化策略

为满足实时性要求，需从算法和工程层面优化计算效率：

分支定界法（Branch-and-Bound）：通过树形搜索结构快速剪枝无效匹配，加速暴力搜索过程。例如，Cartographer在回环检测中采用该技术，将搜索空间划分为多级网格，优先计算高得分区域，减少计算量。
多分辨率地图匹配：先在低分辨率地图上进行粗匹配，再在高分辨率地图上精修，降低单次匹配耗时。
子地图（Submap）构建：将连续扫描数据局部对齐为子地图，减少全局优化变量数量。例如，Cartographer将环境划分为多个5cm分辨率的子地图，仅在子地图完成时触发回环检测。

3. 典型算法与开源实现

3.1 Cartographer算法

Cartographer是Google开源的2D/3D激光SLAM系统，其核心设计包括：

前端局部SLAM：采用Ceres Solver非线性优化库，通过CSM（Correlation Scan Matching）算法实现帧到子地图的匹配，结合IMU数据补偿运动畸变。
后端全局优化：基于SPA（Sparse Pose Adjustment）框架，利用分支定界法加速回环检测，通过位姿图优化消除累积误差。
实时性保障：通过滑动窗口优化和子地图分块更新策略，在嵌入式设备上实现5cm分辨率的实时建图。

实验数据：在德意志博物馆（Deutsches Museum）的实测中，Cartographer以背包式设备完成22,000平方米区域的建图，闭环检测耗时低于100ms，地图精度与离线方案相当。

3.2 基于2D直方图的快速闭环检测

该方法通过统计点云特征分布实现高效匹配：

特征提取：将点云划分为网格单元（Cell），计算每个单元的均值、方差和特征方向（线特征或面特征）。
直方图构建：对特征方向进行旋转不变性处理，按水平角区间统计特征数量，生成2D直方图。
相似性计算：通过比较两帧直方图的欧氏距离或余弦相似度，筛选潜在闭环候选。

优势：计算复杂度低，适合资源受限场景；局限：对动态物体和几何重复场景敏感，需结合其他特征增强鲁棒性。

4. 实时闭环检测的挑战与解决方案

4.1 挑战分析

计算资源限制：2D激光雷达数据量虽小于3D，但高频率扫描仍需高效算法支持。
动态环境干扰：移动物体（如行人、车辆）易导致误匹配，需通过鲁棒核函数或动态物体剔除算法过滤异常值。
几何重复性：长走廊、对称结构等场景易引发感知歧义，需结合语义信息或多传感器融合提升区分度。

4.2 优化方向

深度学习辅助特征提取：利用CNN或Transformer网络学习激光点云的高维特征，提升闭环检测的准确率。例如，基于PointNet++的模型可自动提取局部和全局特征，在复杂环境中表现优于传统方法。
多传感器融合：结合IMU、轮式里程计或视觉数据，提供更可靠的位姿初始估计和特征关联。例如，Cartographer通过IMU预积分补偿激光扫描的运动畸变，提升匹配精度。
并行计算加速：利用GPU或FPGA实现特征提取和匹配的并行化，满足实时性要求。例如，基于CUDA的CSM算法可将匹配速度提升10倍以上。

5. 实验与结果分析

5.1 实验设置

硬件平台：采用2D激光雷达（如Hokuyo UTM-30LX）和IMU（如Xsens MTi-30），搭载NVIDIA Jetson TX2嵌入式计算机。
数据集：使用Radish数据集和自采数据（包含办公室、走廊等场景），覆盖动态物体、几何重复等挑战性环境。
对比算法：Cartographer、基于2D直方图的方法和传统ICP匹配算法。

5.2 性能指标

闭环检测准确率：正确检测闭环的比例。
实时性：单次闭环检测耗时（低于100ms视为实时）。
地图精度：通过绝对轨迹误差（ATE）和相对位姿误差（RPE）评估。

5.3 实验结果

算法	准确率（%）	平均耗时（ms）	ATE（m）	RPE（°/m）
Cartographer	92.3	85	0.12	0.85
2D直方图	85.7	42	0.18	1.20
ICP匹配	78.1	120	0.25	1.50

结论：Cartographer在准确率和地图精度上表现最优，2D直方图方法实时性最佳但易受动态环境影响，ICP匹配因累积误差导致性能下降显著。

6. 结论与展望

实时闭环检测是2D激光雷达SLAM实现高精度建图的关键技术。当前研究已通过分支定界法、子地图构建和多传感器融合等策略，在实时性与精度间取得平衡。未来方向包括：

轻量化深度学习模型：设计适用于嵌入式设备的低参数量网络，提升特征提取效率。
语义SLAM融合：结合物体检测和场景分类信息，增强闭环检测的语义区分度。
端到端学习框架：探索直接从激光扫描到闭环概率的端到端模型，减少手工设计特征依赖。

通过持续优化算法与工程实现，2D激光雷达SLAM将在自动驾驶、机器人导航等领域发挥更大价值。

📚2 运行结果

运行视频：LaserSLAM_腾讯视频

部分代码：

%cfig = figure('Position', [10,10,1280,1080]);
cfig = figure(1);

% Lidar parameters
lidar = SetLidarParameters();

% Map parameters
borderSize = 1; % m
pixelSize = 0.2; % m
miniUpdated = false; %
miniUpdateDT = 0.1; % m
miniUpdateDR = deg2rad(5); % rad
% If the robot has moved 0.1 m or rotated 5 degree from last key scan,
% we would add a new key scan and update the map

% Scan matching parameters
fastResolution = [0.05; 0.05; deg2rad(0.5)]; % [m; m; rad]
bruteResolution = [0.01; 0.01; deg2rad(0.1)]; % not used

% Load lidar data
lidar_data = load('dataset/horizental_lidar.mat');
N = size(lidar_data.timestamps, 1);

% Create an empty map
map.points = [];
map.connections = [];
map.keyscans = [];
pose = [0; 0; 0];
path = pose;

% Here we go!!!!!!!!!!!!!!!!!!!!
for scanIdx = 1 : 1 : N

disp(['scan ', num2str(scanIdx)]);

% Get current scan [x1,y1; x2,y2; ...]
time = lidar_data.timestamps(scanIdx) * 1e-9;
scan = ReadAScan(lidar_data, scanIdx, lidar, 24);

% If it's the first scan, initiate
if scanIdx == 1
map = Initialize(map, pose, scan);
miniUpdated = true;
continue;
end

% ===== Matching current scan to local map ============
% 1. If we executed a mini update in last step, we shall update the
% local points map and local grid map (coarse)
if miniUpdated
localMap = ExtractLocalMap(map.points, pose, scan, borderSize);
gridMap1 = OccuGrid(localMap, pixelSize);
gridMap2 = OccuGrid(localMap, pixelSize/2);
end

% 2. Predict current pose using constant velocity motion model
if scanIdx > 2
pose_guess = pose + DiffPose(path(:,end-1), pose);
else
pose_guess = pose;
end

% 3. Fast matching
if miniUpdated
[pose, ~] = FastMatch(gridMap1, scan, pose_guess, fastResolution);
else
[pose, ~] = FastMatch(gridMap2, scan, pose_guess, fastResolution);
end

% 4. Refine the pose using smaller pixels
% gridMap = OccuGrid(localMap, pixelSize/2);
[pose, hits] = FastMatch(gridMap2, scan, pose, fastResolution/2);