1.問題描述：
        通過將kinect深度圖像采集傳感器安裝在移動機器人上對未知室內環境進行三維探索和平面地圖創建，通過裝載kinect深度圖采集傳感器的移動機器人采集環境的彩色信息和深度信息，通過模擬激光掃描方法獲得對應的點云數據，然后通過SIFT算法進行特征點的提取，并對相鄰兩幀進行匹配，然后通過GTM算法剔除錯誤的匹配點，最后通過SLAM算法構建空間的二維平面地圖。

  

目前SLAM方法大致可分為兩類：一類為基于概率模型的方法，另一類為非概率模型方法。許多基于卡爾曼濾波的SLAM方法如完全SLAM、壓縮濾波、FastSLAM就屬于概率模型方法。非概率模型方法有SM-SLAM、掃描匹配、數據融合(data association)、基于模糊邏輯等。

1 基于卡爾曼濾波器的實現方法
從統計學的觀點看，SLAM 是一個濾波問題，也就是根據系統的初始狀態和從0到t時刻的觀測信息與控制信息(里程計的讀數) 估計系統的當前狀態。在SLAM中，系統的狀態xt =rtmT ，由機器人的位姿r和地圖信息m組成(包含各特征標志的位置信息。假設系統的運動模型和觀測模型是帶高斯噪聲的線性模型，系統的狀態xt 服從高斯分布，那SLAM 可以采用卡爾曼濾波器來實現。基于卡爾曼濾波器的SLAM 包括系統狀態預測和更新兩步，同時還需要進行地圖信息的管理，如：新特征標志的加入與特征標志的刪除等。卡爾曼濾波器假設系統是線性系統，但是實際中機器人的運動模型與觀測模型是非線性的。因此通常采用擴展卡爾曼濾波器( Extended Kalman Fil2ter)，擴展卡爾曼濾波器通過一階泰勒展開來近似表示非線性模型。另一種適用于非線性模型的卡爾曼濾波器是UKF(Unscented Kalman Filter) ，UKF 采用條件高斯分布來近似后驗概率分布，與EKF 相比UKF的線性化精度更高，而且不需要計算雅可比矩陣。卡爾曼濾波器已經成為實現SLAM 的基本方法。其協方差矩陣包含了機器人的位置和地圖的不確定信息。當機器人連續地觀測環境中的特征標志時，協方差矩陣的任何子矩陣的行列式呈單調遞減。從理論上講,當觀測次數趨向于無窮時，每個特征標志的協方差只與機器人起始位置的協方差有關。卡爾曼濾波器的時間復雜度是O(n3 )，由于每一時刻機器人只能觀測到少數的幾個特征標志，基于卡爾曼濾波器的SLAM 的時間復雜度可以優化為O(n2 ) ，n表示地圖中的特征標志數。

2 局部子地圖法
局部子地圖法從空間的角度將SLAM 分解為一些較小的子問題。子地圖法中主要需要考慮以下幾個問題：如何劃分子地圖，如何表示子地圖間的相互關系，如何將子地圖的信息傳遞給全局地圖以及能否保證全局地圖的一致性。最簡單局部子地圖方法是不考慮各子地圖之間的相互關系，將全局地圖劃分為包括固定特征標志數的獨立子地圖，在各子地圖中分別實現SLAM，這種方法的時間復雜度為O(1) 。但是，由于丟失了表示不同子地圖之間相關關系的有用信息，這種方法不能保證地圖的全局一致性。對此，Leonard 等人提出了DSM(Decoupled Stochastic Mapping) 方法，DSM中各子地圖分別保存自己的機器人位置估計，當機器人從一個子地圖A 進入另一個子地圖B時，采用基于EKF 的方法來將子地圖A 中的信息傳送給子地圖B；B.Williams 等人提出了一種基于CLSF (Constrained Local Submap Filter) 的SLAM 方法，CLSF在地圖中創建全局坐標已知的子地圖，機器人前進過程中只利用觀測信息更新機器人和局部子地圖中的特征標志的位置，并且按一定的時間間隔把局部子地圖信息傳送給全局地圖。 雖然實驗表明這兩種算法具有很好的性能，但是沒有從理論上證明它們能夠保持地圖的一致性。 J . Guivant 等人提出了一種沒有任何信息丟失的SLAM 優化算法CEKF (Compressed Extended Kalman Filter) 。CEKF 將已經觀測到的特征標志分為A 與B 兩部分，A 表示與機器人當前位置相鄰的區域，被稱為活動子地圖。當機器人在活動子地圖A 中運動時，利用觀測信息實時更新機器人的位置與子地圖A ，并采用遞歸的方法記錄觀測信息對子地圖B 的影響；當機器人離開活動子地圖A 時，將觀測信息無損失地傳送給子地圖B ，一次性地實現子地圖B 的更新，同時創建新的活動子地圖。該方法的計算時間由兩部分組成：活動子地圖中的SLAM，其時間復雜度為O(na2 )， na 是活動子地圖A中特征標志的數目；子地圖B 的更新，其時間復雜度為O (na×nb2 ) ， nb  是地圖B中特征標志的數目。當子地圖合并的時間間隔較大時，CEKF 能有效減少SLAM的計算量。

3去相關法
降低SLAM復雜度的另一種方法是將表示相關關系的協方差矩陣中一些取值較小的元素忽略掉，使其變為一個稀疏矩陣。 然而這也會因信息的丟失而使地圖失去一致性。 但是，如果能改變協方差矩陣的表示方式，使其中的很多的元素接近于零或等于零，那么就可以將其安全地忽略了。基于擴展信息濾波器EIF ( Extended Information Filter)的SLAM 就是出于這一思想。EIF 是EKF 的基于信息的表達形式，它們的區別在于表示信息的形式不一樣。EIF 采用協方差矩陣的逆矩陣來表征SLAM中的不確定信息，并稱之為信息矩陣。兩個不相關的信息矩陣的融合可以簡單地表示為兩個矩陣相加。 信息矩陣中每個非對角線上的元素表示機器人與特征標志之間或特征標志與特征標志之間的一種約束關系，這些約束關系可以通過系統狀態的信關系進行局部更新。這種局部更新使得信息矩陣近似于稀疏矩陣，對其進行稀疏化產生的誤差很小。根據這一點，S. Thrun 等人提出了一種基于稀疏信息濾波器SEIF (Sparse Extended Information Filter) 的SLAM方法，并證明利用稀疏的信息矩陣實現SLAM 的時間復雜度是O(1)。雖然EIF 可以有效降低SLAM的時間復雜度，但是在地圖信息的表示和管理方面還存在一些問題。首先，在常數時間內只能近似算得系統狀態的均值；其次，在基于EIF 的SLAM 方法中，特征標志的增刪不方便。

4 分解法（FastSLAM）
M.Montemerlo 等人提出了一種基于粒子濾波器(Particle Filter) FastSLAM 方法。FastSLAM 將SLAM分解為機器人定位和特征標志的位置估計兩個過程。 粒子濾波器中的每個粒子代表機器人的一條可能運動路徑，利用觀測信息計算每個粒子的權重，以評價每條路徑的好壞。對于每個粒子來說，機器人的運動路徑是確定的，因此特征標志之間相互獨立，特征標志的觀測信息只與機器人的位姿有關，每個粒子可以采用n 個卡爾曼濾波器分別估計地圖中n 個特征的位置。 假設需要k個粒子實現SLAM、FastSLAM，總共有kn個卡爾曼濾波器。FastSLAM 的時間復雜度為O(kn)，通過利用樹型的數據結構進行優化， 其時間復雜度可以達到O( klog n)。 Fast2SLAM方法的另一個主要優點是通過采用粒子濾波器估計機器人的位姿，可以很好地表示機器人的非線性、非高斯運動模型。


————————————————
2.部分程序：
 
function func_slam(xf,yf,zf,RR,CC);

Times = 2000;
 
figure; 
imagesc(zf); 
axis([1,CC,1,RR]); 
title('Landmarks');
hold on;
load Landmarks.mat
plot(pp(1,:),pp(2,:),'r*')
hold off;

 

figure;

imagesc(zf);
axis([1,CC,1,RR]); 
title('trajectory around landmarks');
hold on;
plot(pp(1,:),pp(2,:),'r*');
hold on;
load trajectory.mat
dist = 0;
for i = 2:length(t)
    plot(t(1,i-1:i),t(2,i-1:i),'y-o')
    dist = dist + norm(t(:,i) - t(:,i-1));
end
hold on;

 


%獲得路徑
[tt,vv] = func_lujing(t,dist,Times);  
v       = vv;  
x       = tt;   
clear vv;
p(1:2:2*length(pp)) = pp(1,:);
p(2:2:2*length(pp)) = pp(2,:);
 
%定義噪聲
%地標噪聲
%位置噪聲
%速度噪聲
%過程噪聲
N1 = (15)^2;  
N2 = (1.5)^2;  
N3 = (0.05)^2; 
N4 = (0.001)^2;   

%標志矩陣
H = [diag(ones(2+2,1)) zeros(2+2,2*size(pp,2))];
for i=1:2:2*size(pp,2)
    H = [H;-1 0 0 0 zeros(1,i-1) 1 zeros(1,2*size(pp,2)-i)];
    H = [H;0 -1 0 0 zeros(1,i) 1 zeros(1,2*size(pp,2)-i-1)];
end
%狀態矩陣%SLAM控制矢量%協方差矩陣
[Fk,Uk,Pk]   = func_FUP(pp,N1,N2,N3,N4);
%測量方差
[Q,R,xhat,N] = func_variance(x,v,p,pp,N1,N2,N3,N4);

%卡爾曼濾波
[x,xhat]=func_kalman(zf,Times,H,x,p,v,pp,RR,CC,Fk,Uk,Pk,Q,R,xhat,N,N1,N2,N3,N4);


X = 1:20:RR;
Y = 1:20:CC;

%最終地圖輸出：
figure;
subplot(121)
load Landmarks.mat
plot(pp(1,:),pp(2,:),'k-o');
hold on
load result.mat
plot(x(1,:),x(2,:),'r.','MarkerSize',1)
hold on
grid minor;
 
subplot(122)
load Landmarks.mat
plot(pp(1,:),pp(2,:),'kx');
hold on
load result.mat 
plot(x(1,:),x(2,:),'.','Color',1.0-0.6*(1.0-[0,0,0]),'EraseMode','xor','linewidth',5)
grid minor;
 
 
————————————————

聯系：highspeedlogic

QQ ：1224848052

微信：HuangL1121

郵箱：1224848052@qq.com

網站：http://www.mat7lab.com/

網站：http://www.hslogic.com/

微信掃一掃：