写在前面：本博客仅作记录学习之用，部分图片来自网络，如需引用请注明出处，同时如有侵犯您的权益，请联系删除！

---

前言

基于卫星定位方式通常具有以下缺点：定位信息遮挡（地下或隧道）、多径效应、电磁干扰、基准站覆盖外（差分定位）等问题。换言之卫星定位通常在空旷区域定位效果较好。

惯导系统（遮挡）、SLAM技术在一定程度上缓解的上述弊端，对于学习激光SLAM来说，仿真是一种免费仿真，减少购买激光雷达的费用。MATLAB为SLAM算法提供了高效仿真平台。本文将通过简单案例，展示如何利用MATLAB实现激光SLAM仿真。

---

MATLAB函数

所有函数依赖于Navigation Toolbox，安装时选上该工具箱即可。注意一般需要MATLAB 2023往上版本才行。

创建激光SLAM实例

slamAlg = lidarSLAM(mapResolution, maxLidarRange);

参数说明：

• maxLidarRange：激光雷达的最大距离
• mapResolution：网格地图分辨率为每米mapResolution格.
• slamAlg.PoseGraph：连接扫描的底层姿态图，添加扫描会更新该姿态图。

扫描与构建

addScan(scanMapObj,currScan)：将指定的二维激光雷达扫描添加到最近的扫描中对象。

[scans，poses] = scansAndPoses(slamObj)：返回lidarSLAM扫描数据，以及其从底层姿态图中关联的姿态。

栅格占用

map = buildMap(scans,poses, mapResolution,maxRange)：在给定处插入激光雷达创建占用Map地图。

---

MATLAB仿真

代码

本部分代码参考与官方示例。离线数据下载offlineSlamData.mat，免费。

load('offlineSlamData.mat');

maxLidarRange = 8;
mapResolution = 30;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);
slamAlg.LoopClosureThreshold = 210;
slamAlg.LoopClosureSearchRadius = 8;

filename = '激光SLAM的栅格占用示意图.gif';
figure;
for i=1:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % 以下保存为动图
    map=getframe(gcf);
    A=frame2im(map);
    [A,map]=rgb2ind(A,256);
    fps=2;
    if i<=2 % 第一帧是全灰，故跳过
        if i==2
            imwrite(A, map, filename, 'gif', 'LoopCount', Inf, 'DelayTime', 1/fps);
        end
    else
        imwrite(A, map, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 1/fps);
    end
	  % 以上保存为动图

    [scans_, optimizedPoses]  = scansAndPoses(slamAlg);
    map = buildMap(scans_, optimizedPoses, mapResolution, maxLidarRange);
    show(map);
    hold on
    show(slamAlg.PoseGraph, 'IDs', 'off');
    hold off
    title('激光SLAM的栅格占用示意图');
    drawnow % 刷新
end

load('offlineSlamData.mat');

maxLidarRange = 8;
mapResolution = 30;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);

slamAlg.LoopClosureThreshold = 210;
slamAlg.LoopClosureSearchRadius = 8;

filename = '激光SLAM重建环境示意图.gif';

figure;
for i=1:length(scans)

    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % 以下保存为动图
    map=getframe(gcf);
    A=frame2im(map);
    [A,map]=rgb2ind(A,256);
    fps=2;
    if i<=2
        if i==2
            imwrite(A, map, filename, 'gif', 'LoopCount', Inf, 'DelayTime', 1/fps);
        end
    else
        imwrite(A, map, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 1/fps);
    end
    % 以上保存为动图

    show(slamAlg, 'Poses', 'off');
    hold on;
    show(slamAlg.PoseGraph, 'IDs', 'off');
    hold off;
    firstTimeLCDetected = true;
    title('激光SLAM重建环境示意图');
    drawnow
end

---

效果

总结

总结： 本文介绍了利用MATLAB实现激光SLAM仿真的方法，讲解了MATLAB Navigation Toolbox中的关键函数，包括创建激光SLAM实例、扫描与构建地图等操作。代码展示了完整的仿真流程，并生成两种可视化效果图。

---

致谢

欲尽善本文，因所视短浅，怎奈所书皆是瞽言蒭议。行文至此，诚向予助与余者致以谢意。

---

参考

[1] Navigation Toolbox 使用说明