使用2D激光雷达的SLAM（Matlab代码实现）

     目录

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码

💥1 概述

SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边逐步描绘出此环境完全的地图，同时一边决定机器人应该往哪个方向行进。

例如扫地机器人就是一个很典型的SLAM问题，所谓完全的地图（a consistent map）是指不受障碍行进到房间可进入的每个角落。

SLAM最早由Smith、Self和Cheeseman于1988年提出。

由于其重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。

当你来到一个陌生的环境时，为了迅速熟悉环境并完成自己的任务（比如找饭馆，找旅馆），你应当做以下事情：

a.用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）

b.在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）

c.当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）

d.根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）

e.当无意中走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）。实际这一步可有可无。

以上五步是同时进行的，因此是simultaneous localization and mapping。

📚2 运行结果

主函数部分代码：

clear; close all; clc;


%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);
    %----------------------------------------------------------------------
    
    
    % Execute a mini update, if the robot has moved a certain distance
    dp = abs(DiffPose(map.keyscans(end).pose, pose));
    if dp(1)>miniUpdateDT || dp(2)>miniUpdateDT || dp(3)>miniUpdateDR
        miniUpdated = true;
        [map, pose] = AddAKeyScan(map, gridMap2, scan, pose, hits,...
                        pixelSize, bruteResolution, 0.1, deg2rad(3));
    else
        miniUpdated = false;
    end    
    path = [path, pose];      
    
    
    % ===== Loop Closing =========================================
%     if miniUpdated
%         if TryLoopOrNot(map)
%             map.keyscans(end).loopTried = true;
%             map = DetectLoopClosure(map, scan, hits, 4, pi/6, pixelSize);
%         end
%     end
    
    %----------------------------------------------------------------------
    
    % Plot
    if mod(scanIdx, 30) == 0
        PlotMap(cfig, map, path, scan, scanIdx);
    end
    
end

🎉3 参考文献

[1]赵一鸣,李艳华,商雅楠,李静,于勇,李凉海.激光雷达的应用及发展趋势[J].遥测遥控,2014,35(05):4-22.

部分理论引用网络文献，若有侵权联系博主删除。