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/

微信扫一扫：