目錄

一、理論基礎

二、核心程序

三、仿真測試結果

---

一、理論基礎

? ? ? ?移動機器人的定位和地圖創建是機器人領域的熱點研究問題，也是導航中重要環節。 對于已知環境中的機器人自主定位和已知機器人位置的地圖創建已經有了一些實用的解決方法。 然而在很多環境中機器人不能利用全局定位系統進行定位,而且事先獲取機器人工作環境的地圖很困難，甚至是不可能的。這時機器人需要在自身位置不確定的條件下，在完全未知環境中，創建地圖同時利用該地圖進行自主定位和導航。 這就是移動機器人的同時定位與地圖創建(SLAM) 問題，也稱為CML (Concurrent Mapping and Localization) 。該研究領域的代表人物有Smith，Self 和Cheeseman。由于SLAM算法具有重要的理論與應用價值，被很多學者認為是實現真正全自主移動機器人的關鍵。

? ? ? ?SLAM算法有很多重要的屬性，影響著地圖特征和機器人位置估計中的不確定性。包括：狀態估計的收斂性，估計過程的一致性，狀態協方差矩陣更新的計算復雜度。

? ? ? ?收斂性：SLAM算法具有三個重要的收斂性，這三個關鍵的收斂結果是：A.地圖協方差矩陣的任意子矩陣地行列式隨著每一次觀測單調下降；B.在極限情況下，隨著觀測數量的增加，特征估計變得完全相關。C.地圖的精度與第一個特征被觀測時機器人的位置精度有關。這些結果表明：隨著觀測數量的增加，地圖估計的不確定性將降低到有限的誤差范圍內，地圖特征的關系將是完全確定的。

? ? ? ?一致性：為了維護SLAM算法估計的一致性，對狀態協方差矩陣進行更新維護是必要的。既然對環境的觀測是相對機器人的，所以機器人估計中的任何誤差和地圖估計中的誤差使絕對相關的。在沒有其它外部的關于特征和機器人位置信息情況下，為了使系統狀態估計的誤差在有限的范圍內，保持狀態估計之間的一致性是很必要的。所以就必須維護機器人狀態與環境特征之間的協方差矩陣。

? ? ? 計算復雜度：SLAM算法應用到大規模環境時的一個重要局限性是計算環境特征之間，特征與機器人之間的相關信息時的計算負擔。由于特征數量很大，協方差矩陣的更新維護導致了SLAM算法的計算復雜性。對于那些包含上萬個特征的環境，計算負擔使得系統協方差的更新變的難以執行。所以需要一個有效的方法來提高算法的計算效率。

根據所依據的理論基礎的不同，SLAM可以分為以下幾種：　

（1）基于擴展卡爾曼濾波（EKF）的CML/ SLAM

? ? ? ? 這是最常用的一種SLAM方法，適合解決非線性系統的估計問題。該方法用平面坐標表示機器人和環境特征的位置，將機器人運動與環境特征的關系描述為兩個非線性模型：機器人運動模型和觀測模型.通過這兩個模型，運用擴展卡爾曼理論的思想來實現。主要包括預測與更新兩個階段。

? （2）基于概率的CML/ SLAM

? ? ? ?盡管不如EKF 那樣流行，但由于用概率表達機器人定位問題的不確定性非常自然合理，基于概率的CML 也吸引了很多人的目光。其中，較流行是最大相似性(Maximum Likelihood Estimation ，MLE)方法。一種非常有效的最大相似性估計算法稱為Baum-Welch (或α-β) 算法[30]，基于這種方法的機器人制圖與定位問題可看作是機器人位置與環境特征位置的最大相似性估計問題。該算法包括兩步：E-Step (expectation) 和M-Step (maximization)。

? （3）基于粒子濾波器( particle filter) 的CML/SLAM

? ? ? 粒子濾波器定位也稱為Monte Carlo 定位，其基本思想是用一組濾波器來估計機器人的可能位置(處于該位置的概率)，每個濾波器對應一個位置，利用觀測對每個濾波器進行加權傳播，從而使最有可能的位置的概率越來越高。

? （4）基于空間擴展信息濾波器的SLAM

? ? ?該方法由Sebastian Thrun 等人提出，是對EKF 算法的改進。它不再用協方差矩陣表示空間信息的相關性，取而代之用空間信息矩陣來表示空間信息間的內在固有的關系，并且使用網狀數據結構僅維護鄰近的環境特征(地圖)。

? （5）基于集合理論估計的SLAM

? ? ? 基于集合理論的方法不假設噪聲服從某種分布，而只假設噪聲是有界的。 具體方法是：定義一個可行狀態集合(feasible state set) 和一個觀測集合(measurement set) ，前者表示機器人和環境特征的狀態估計，后者表示符合條件(觀測誤差小于邊界)的狀態集合。算法首先根據機器人本身的狀態方程計算可行狀態集合，然后根據觀測值計算觀測集合，最后取兩個集合的交集作為某時刻的經估計校正后的機器人與環境特征的狀態集合(地圖)。

此外根據所制地圖的不同，SLAM又可以分為：基于柵格的SLAM；基于特征的SLAM以及基于拓撲結構的SLAM。

? ? ? ?在基于柵格的SLAM算法中，環境表示為一組具有特定分辨度的柵格的組合。每個柵格中有特定概率值來表示其被占據的可能性。使用基于柵格SLAM算法的時候，假設各個柵格的狀態變量是相互獨立的，而沒有考慮柵格之間的關聯性。另外，當機器人采用該種SLAM算法運行較長一段路程后，很難再回到出發點。這是因為該方法中僅在局部坐標中考慮了估計的不確定性，卻沒考慮局部坐標與全局坐標中的相互聯系。總的說來，基于柵格的SLAM方法可以提供較為豐富的環境描述，對于局部環境下的規劃與導航有很大幫助，但是通過該方法無法獲得一致性較強的全局地圖。

? ? ? 基于特征地圖的SLAM算法是將環境表示為一組組參數化的特征值，比如說點，線，角等。這里的特征指的是環境中那些突顯于背景的、易于傳感器分辨檢測的且可以通過參數描述的東西。使用這種方法的時候，必須對環境中不同類型的特征分別建立測量模型以便準確提取。基于特征的SLAM算法是最為流行的一種，盡管在環境地圖描述中僅僅用到特征的位置值，但是還可以通過大小，顏色等信息進行匹配等。這種SLAM算法常常是基于卡爾曼濾波的。使用這種方法時，狀態向量不僅包括機器人的位姿信息而且還有特征的位置信息。隨著新特征的不斷提取與確認，狀態向量將不斷的變大。因為用于描述環境的特征值的測量都是相對機器人的，所以對環境特征測量的不確定性是與機器人位姿估計的不確定性息息相關的。可以在理論上證明，隨著時間的不斷推移、測量的不斷進行，地圖中的特征將是完全相關的，也就是說此時隨意給定一個特征的絕對坐標值，將會得到一個精準的地圖。在環境特征容易識別的場合下，該算法運用很好，但是對于特性不是太明顯的非結構化環境中，算法的運用遇到難題。

? ? ? ?在基于拓撲結構的SLAM算法中使用“圖”來描述環境。具體說是通過節點和弧線來描述環境。每個節點表示環境中突顯的地方，邊用來表示相鄰節點間的相對位置。對于純粹的拓撲地圖來說，無須知道每個節點的具體位置，它只用來表示節點間的連通性。但是該方法也有很大的弊端，比如當環境稍微復雜一些時，其對環境的識別能力會明顯下降且無法識別相近的環境。

? ? ? ?盡管SLAM算法的理論基礎已經被很好的研究，但是要將其更好的運用于實際，特別是大型的非結構化環境，仍有大量理論和實際的問題需要解決包括：計算效率、地圖管理、局部地圖與全局地圖之間的協調融合、數據匹配以及傳感器管理等]。

? ? ? ?基于特征SLAM算法架構

二、核心程序

clc; clear; close all; warning off; addpath 'func\' addpath 'func\toolbox\'IF = 0; %導入kirect數據，并獲得點云圖 N = 50;%數據量非常大，只導入100個進行處理 MAP = zeros(600,1800); IMAGES = cell(1,N); indx = 0;if IF == 1for i = 100:1:100+Nindx = indx + 1;if i < 10NAME1 = ['KinectDepthFrames\','kD[00',num2str(i),'].mat'];NAME2 = ['KinectColorFrames\','rgb[00',num2str(i),'].jpg'];endif i < 100 & i >= 10NAME1 = ['KinectDepthFrames\','kD[0',num2str(i),'].mat'];NAME2 = ['KinectColorFrames\','rgb[0',num2str(i),'].jpg'];end if i >= 100NAME1 = ['KinectDepthFrames\','kD[',num2str(i),'].mat'];NAME2 = ['KinectColorFrames\','rgb[',num2str(i),'].jpg'];end load(NAME1);%RGBI0 = imresize(imread(NAME2),1);I = I0(4:end-3,4:end-6,:);%Depth,縮小圖像像素，加快仿真速度，data0 = (imresize(Frame.bin,1));data = data0(4:end-3,4:end-6);%數據濾波Y = func_depath_filter(data);%首先進行拼接if indx == 1 mosaic1 = I; mosaic2 = Y; endif indx > 2[tmps,up,M3,left,N3,Yoffset,Xoffset,M1,N1,tform] = imMosaic(I,mosaic1,1);mosaic1 = tmps(4:end-3,4:end-6,:);tmp2 = zeros(size(tmps,1),size(tmps,2));tmp2(up:up+M3-1,left:left+N3-1) = imtransform(mosaic2,tform); tmp2(Yoffset+1:Yoffset+M1,Xoffset+1:Xoffset+N1) = Y;mosaic2 = tmp2(4:end-3,4:end-6,:);endfigure(1);subplot(2,2,1);imshow(I,[]);title('RGB圖');subplot(2,2,2);imshow(Y,[]);title('深度圖');if i > 1subplot(2,2,3);imshow(mosaic1,[])title('拼接'); subplot(2,2,4);imshow(mosaic2,[])title('深度圖拼接'); enddrawnow;end save R.mat mosaic1 mosaic2 elseload R.matfigure(1);subplot(1,3,1);imshow(mosaic1,[])title('拼接'); subplot(1,3,2);imshow(mosaic2,[])title('深度圖拼接'); [X,Y,Z] = func_realword(mosaic2);figure(1);subplot(1,3,3);map2ds = func_map_gen(X,Y,Z);%產生地圖imshow(map2ds,[]);title('地圖'); end

三、仿真測試結果

?

?

?A23-24

總結

以上是生活随笔為你收集整理的基于Kinect深度图像采集和SLAM室内地图创建算法的matlab仿真的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。