【定位問題】基於matlab GUI SLAM模擬地圖構建和定位【含Matlab原始碼 1120期】

語言: CN / TW / HK

一、簡介

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必將在相互競爭和融合中發展,使機器人從實驗室和展廳中走出來,做到真正的服務於人類。

二、原始碼

```c 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 ```

三、執行結果

在這裡插入圖片描述

四、備註

版本:2014a

「其他文章」