【定位问题】基于matlab GUI SLAM模拟地图构建和定位【含Matlab源码 1120期】

一、简介

SLAM是Simultaneous localization and mapping缩写，意为“同步定位与建图”，主要用于解决机器人在未知环境运动时的定位与地图构建问题，为了让大家更多的了解SLAM，以下将从SLAM的应用领域、SLAM框架、SLAM分类（基于传感器的SLAM分类）来进行全面阐述，本文仅对没有接触过SLAM的新人进行的科普。

1 SLAM的典型应用领域

机器人定位导航领域：地图建模。SLAM可以辅助机器人执行路径规划、自主探索、导航等任务。国内的科沃斯、塔米以及最新面世的岚豹扫地机器人都可以通过用SLAM算法结合激光雷达或者摄像头的方法，让扫地机高效绘制室内地图，智能分析和规划扫地环境，从而成功让自己步入了智能导航的阵列。国内思岚科技(SLAMTEC)为这方面技术的主要提供商，SLAMTEC的命名就是取自SLAM的谐音，其主要业务就是研究服务机器人自主定位导航的解决方案。目前思岚科技已经让关键的二维激光雷达部件售价降至百元，这在一定程度上无疑进一步拓展了SLAM技术的应用前景。

VR/AR方面：辅助增强视觉效果。SLAM技术能够构建视觉效果更为真实的地图，从而针对当前视角渲染虚拟物体的叠加效果，使之更真实没有违和感。VR/AR代表性产品中微软Hololens、谷歌ProjectTango以及MagicLeap都应用了SLAM作为视觉增强手段。

无人机领域：地图建模。SLAM可以快速构建局部3D地图，并与地理信息系统（GIS）、视觉对象识别技术相结合，可以辅助无人机识别路障并自动避障规划路径，曾经刷爆美国朋友圈的Hovercamera无人机，就应用到了SLAM技术。

无人驾驶领域：视觉里程计。SLAM技术可以提供视觉里程计功能，并与GPS等其他定位方式相融合，从而满足无人驾驶精准定位的需求。例如，应用了基于激光雷达技术Google无人驾驶车以及牛津大学MobileRoboticsGroup11年改装的无人驾驶汽车野猫(Wildcat)均已成功路测。

2 SLAM框架

SLAM系统框架如图所示，一般分为五个模块，包括传感器数据、视觉里程计、后端、建图及回环检测。

传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。

视觉里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。

后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。

建图：用于三维地图构建。

回环检测：主要用于空间累积误差消除

其工作流程大致为：

传感器读取数据后，视觉里程计估计两个时刻的相对运动（Ego-motion），后端处理视觉里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。

3 SLAM分类（基于传感器的SLAM分类）

目前用在SLAM上的传感器主要分为这两类，一种是基于激光雷达的激光SLAM(Lidar SLAM)和基于视觉的VSLAM(Visual SLAM)。

3.1 激光SLAM

激光SLAM采用2D或3D激光雷达（也叫单线或多线激光雷达），2D激光雷达一般用于室内机器人上（如扫地机器人），而3D激光雷达一般使用于无人驾驶领域。激光雷达的出现和普及使得测量更快更准，信息更丰富。激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。

激光雷达测距比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。激光SLAM理论研究也相对成熟，落地产品更丰富。

3.2 视觉SLAM

眼睛是人类获取外界信息的主要来源。视觉SLAM也具有类似特点，它可以从环境中获取海量的、富于冗余的纹理信息，拥有超强的场景辨识能力。早期的视觉SLAM基于滤波理论，其非线性的误差模型和巨大的计算量成为了它实用落地的障碍。近年来，随着具有稀疏性的非线性优化理论(Bundle Adjustment)以及相机技术、计算性能的进步，实时运行的视觉SLAM已经不再是梦想。

视觉SLAM的优点是它所利用的丰富纹理信息。例如两块尺寸相同内容却不同的广告牌，基于点云的激光SLAM算法无法区别他们，而视觉则可以轻易分辨。这带来了重定位、场景分类上无可比拟的巨大优势。同时，视觉信息可以较为容易的被用来跟踪和预测场景中的动态目标，如行人、车辆等，对于在复杂动态场景中的应用这是至关重要的。

通过对比我们发现，激光SLAM和视觉SLAM各擅胜场，单独使用都有其局限性，而融合使用则可能具有巨大的取长补短的潜力。例如，视觉在纹理丰富的动态环境中稳定工作，并能为激光SLAM提供非常准确的点云匹配，而激光雷达提供的精确方向和距离信息在正确匹配的点云上会发挥更大的威力。而在光照严重不足或纹理缺失的环境中，激光SLAM的定位工作使得视觉可以借助不多的信息进行场景记录。

近年来，SLAM导航技术已取得了很大的发展，它将赋予机器人和其他智能体前所未有的行动能力，而激光SLAM与视觉SLAM必将在相互竞争和融合中发展，使机器人从实验室和展厅中走出来，做到真正的服务于人类。

二、源代码

function varargout = mapMakerGUI(varargin)
% mapMakerGUI MATLAB code for mapMakerGUI.fig
%      mapMakerGUI, by itself, creates a new mapMakerGUI or raises the existing
%      singleton*.
%
%      H = mapMakerGUI returns the handle to a new mapMakerGUI or the handle to
%      the existing singleton*.
%
%      mapMakerGUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in mapMakerGUI.M with the given input arguments.
%
%      mapMakerGUI('Property','Value',...) creates a new mapMakerGUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before mapMakerGUI_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to mapMakerGUI_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help mapMakerGUI

% Last Modified by GUIDE v2.5 21-Mar-2021 08:07:51

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
'gui_Singleton',  gui_Singleton, ...
'gui_OpeningFcn', @mapMakerGUI_OpeningFcn, ...
'gui_OutputFcn',  @mapMakerGUI_OutputFcn, ...
'gui_LayoutFcn',  [] , ...
'gui_Callback',   []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before mapMakerGUI is made visible.
function mapMakerGUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to mapMakerGUI (see VARARGIN)

% Choose default command line output for mapMakerGUI
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

global Lmks LmkGraphics Wpts WptGraphics DefaultText AxisDim RunTime ...
Obstacles ObstaclesMidpoint World

World = [];

Lmks = [];

LmkGraphics = line(...
'parent',handles.mainAxes, ...
'linestyle','none', ...
'marker','+', ...
'color','b', ...
'xdata',[], ...
'ydata',[]);

Wpts = [];

WptGraphics = line(...
'parent',handles.mainAxes, ...
'marker','o', ...
'color','r', ...
'xdata',[], ...
'ydata',[]);

Obstacles = [];
ObstaclesMidpoint = [];

DefaultText = 'Select a command...';

AxisDim = 10;
%set(handles.mainAxes, 'XLim', [-AxisDim,AxisDim], 'YLim', [-AxisDim,AxisDim]);
axes(handles.mainAxes)
axis([-AxisDim AxisDim -AxisDim AxisDim])
axis square

set(handles.AxisDimVar, 'String', AxisDim)

RunTime = 400;
set(handles.RunTimeVar, 'String', RunTime)

obsVertices = 3;
set(handles.obsVertices, 'String', obsVertices)

obsVelX = 0; obsVelY = 0;
set(handles.obsVelX, 'String', obsVelX)
set(handles.obsVelY, 'String', obsVelY)

% imagesc([], 'parent', handles.map1);
set(handles.map1, 'XTick', [], 'YTick', [])
% imagesc([], 'parent', handles.map2);
set(handles.map2, 'XTick', [], 'YTick', [])

% UIWAIT makes mapMakerGUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = mapMakerGUI_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;


% --- Executes on button press in AddLmk.
function AddLmk_Callback(hObject, eventdata, handles)
% hObject    handle to AddLmk (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Lmks AxisDim DefaultText
set(handles.helpBox, 'String', 'Click on the axes to place a landmark...')
clicked = 0;
while ~clicked
    [x,y]=ginputax(handles.mainAxes,1);
if abs(x) < AxisDim && abs(y) < AxisDim
Lmks = [Lmks [x;y]];
axes(handles.mainAxes);
%hold on
plotItems(Lmks, 'landmarks');
set(handles.helpBox, 'String', ...
            ['Landmark placed at:' char(10) ...
'(' num2str(x) ', ' num2str(y) ')' char(10) DefaultText])
clicked = 1;
else
set(handles.helpBox, 'String', ...
'Landmark not placed! Click somewhere on the axes.')
end
end

% --- Executes on button press in DelLmk.
function DelLmk_Callback(hObject, eventdata, handles)
% hObject    handle to DelLmk (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Lmks AxisDim DefaultText
set(handles.helpBox, 'String', 'Click a landmark to delete...')
clicked = 0;
while ~clicked
if ~isempty(Lmks)
p = ginputax(handles.mainAxes,1);
if abs(p(1))<AxisDim && abs(p(2))<AxisDim
i = nearestNeighbour(Lmks, p);
% Remove nearest neighbour from Lmks
lmk_deleted = Lmks(:,i);
Lmks(:,i) = [];

axes(handles.mainAxes);
hold on
plotItems(Lmks, 'landmarks');

set(handles.helpBox, 'String', ...
                ['Landmark deleted at:' char(10) ...
'(' num2str(lmk_deleted(1)) ', ' num2str(lmk_deleted(2)) ')' ...
char(10) DefaultText])
clicked = 1;
else
set(handles.helpBox, 'String', ...
'No landmarks deleted! Click somewhere on the axes.')
end
else
set(handles.helpBox, 'String', ...
            ['No landmarks to delete.' char(10) DefaultText])
clicked = 1;
end
end

% --- Executes on button press in doSLAM.
function doSLAM_Callback(hObject, eventdata, handles)
% hObject    handle to doSLAM (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Lmks Wpts DefaultText AxisDim Obstacles World Map1 Map2
if isempty(Lmks) || isempty(Wpts) || isempty(Obstacles)
errordlg(['The map must consist of at least 1 landmark,' ...
'1 waypoint and 1 obstacle'],'BOOM!')
else
set(handles.helpBox, 'String', 'Executing SLAM...')
World = ekfSLAM(handles, AxisDim, Lmks, Wpts, Obstacles);
set(handles.helpBox, 'String', ['SLAM simulation complete!' char(10) ...
DefaultText])
Map1 = World.gridmap_greyscale;
Map2 = World.gridmap;
end

function plotItems(Items, ItemType)
global LmkGraphics WptGraphics
if strcmp(ItemType, 'landmarks')
set(LmkGraphics, 'xdata', Items(1, :), 'ydata', Items(2, :))
elseif strcmp(ItemType, 'waypoints')
set(WptGraphics, 'xdata', Items(1, :), 'ydata', Items(2, :))
end

function i = nearestNeighbour(Items, p)
diff2 = (Items(1,:)-p(1)).^2 + (Items(2,:)-p(2)).^2;
i= find(diff2 == min(diff2));
i= i(1);


% --- Executes on button press in LoadMap.
function LoadMap_Callback(hObject, eventdata, handles)
% hObject    handle to LoadMap (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global Lmks Wpts Obstacles ObstaclesMidpoint DefaultText
clearMap_Callback(hObject, eventdata, handles)
seed = {'*.mat','MAT-files (*.mat)'};
[fn,pn] = uigetfile(seed, 'Load Map');
if fn==0, return, end

fnpn = strrep(fullfile(pn,fn), '''', '''''');
load(fnpn)
Lmks = lmks;
Wpts = wpts;
Obstacles = obs;
if ~isempty(Lmks)
plotItems(Lmks, 'landmarks');
end
if ~ isempty(Wpts)
plotItems(Wpts, 'waypoints');
end
if ~isempty(Obstacles)
for i = 1:length(Obstacles)
Obstacles(i).plot(handles.mainAxes);
ObstaclesMidpoint(:, i) = [mean(Obstacles(i).vertices(1,:)); ...
mean(Obstacles(i).vertices(2,:))];
end
end

三、运行结果