目录
💥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.
部分理论引用网络文献,若有侵权联系博主删除。