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

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

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

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

📋📋📋本文目录如下：🎁🎁🎁

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文章

---

 ⛳️赠与读者

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

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

💥1 概述

文献来源：

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

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

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

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

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

📚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);

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

🌈4 Matlab代码、数据、文章

资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取