武汉科技大学本科毕业设计
pn,t?2?zn,t end for
?m? pNt??m1??1,t?p0
?m??12?T?1???1???exp?z?zzz?z?tn,tn,ttn,t?? 2????m?maxpn nt?arg,t //进行数据融合 ?? if nt?Nt?-m1?1 //如果是一个新的特征？
? N?tm??Nt?-m1?1
?m????n?g?1s?m?t?z ?n t,tt,t??
?nt,t?G?T，n?tR?1G?T,n?t??m????1
else //是已知的特征？
? N?tm??Nt?-m1
T?1???Z? ?G Kn?,tn?,nntt,t?1tt,t?m??m??z?z????n???Kn ?n ,tn,tttt,t?1t,t??
??m??nt,t?1?G??I?Kn?,ntt,t?????m?nt,t?1
end if
for n=1 to Nt?m? do //处理未观测到的特征
? if n?nt?m?
?m??m? ??n,t???n,t?1
?????????mmn,t?1n,t?1
end if end for
for m=1 to M //重采样M个新粒子 从Saux集合中采样任意符合概率分布?t?m?的粒子 将得到的粒子集加入到St中 end for return St
22
武汉科技大学本科毕业设计
6 仿真环境建立和仿真结果
6.1 仿真环境介绍
MATLAB是矩阵实验室（Matrix Laboratory）的简称，是美国MathWorks公司出
品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境，主要包括MATLAB和Simulink两大部分。
MATLAB环境是由美国New Mexico大学的C1eveMoler博士于1980年开始开发的，1984年由CleveMoler等人创立的MathWorks公司正式推出了第一个商业版本。MATLAB是一套高性能的数值计算和可视化软件。它集数值分析、矩阵运算、信号处理和图形显示于一体，构成了一个方便的、界面友好的用户环境。这使它成为国际控制领域应用最广的首选软件工具。现在MATLAB软件不但广泛应用于控制领域，也应用于其它的工程和非工程领域。在控制界，很多著名专家和学者为其擅长的领域开发了工具箱，而其中很多工具箱己经成为该领域的标准。
6.2 仿真结果
实验一
仿真图如下：
图6-1 FastSLAM算法仿真图
23
武汉科技大学本科毕业设计
图中黑色小点表示机器人的实际位置，曲线折线为机器人的预测位置，由此可见，FastSLAM算法存在不稳定，但相对误差较小，基本满足了实验环境下的定位精度，解决了粒子重采样时经常出现的粒子退化问题 实验二
先在用户界面上添加35个路标，在添加17个位姿，在用户界面上创建环境特征和运动控制信息。完成以上操作得到仿真图6-2：
图6-2 添加了路标和路径信息的仿真界面
24
武汉科技大学本科毕业设计
图6-3 EKF-SLAM仿真图
图6-4 FastSLAM仿真图
25
武汉科技大学本科毕业设计
星型点和黑点分别表示实际和估计的路标位置。
分析：在控制量噪声一定且相对较小时，同时加入的观测量噪声也相对较小的情况下，EKF-SLAM和FastSLAM都能较好地对路径和路标进行估计，得到良好的SLAM效果，但在X轴和Y轴的误差方面，FastSLAM仍然比EKF-SLAM小一些。
图6-5 EKF-SLAM仿真图
星型点表示实际路标位置，十字点表示估计的路标位置，在其周围的椭圆表示其观测的不确定性。
26