武汉科技大学本科毕业设计
3 模型建立
3.1 运动模型
在SLAM中有两个系统模型：观测模型和运动模型。运动模型描述了t时刻机器人位姿的概率分布。机器人运动模型如图3.1所示[7]：
图3.1 机器人运动模型
机器人的运动模型见式?3.1?[8]：
i?i??xti???xt????t??1??t?vt?1cos1?wt?1?t?wx??i????i??i??t???w?t?w ?yt???yt?1??t?vt?1sin 1t?1y? （3.1）?i???t?i?????t?1??t?wt?1?w?????i??i?其中：xt???t?-i1?分别为t?1时刻第i个粒子所表征的机器人质心坐标和方向角；?t1、yt?1、
????为t?1时刻到t时刻的时间间隔；v、w分别是机器人的速度和角速度；wx、wy、w?分别为相应的噪声项。
17
武汉科技大学本科毕业设计
3.2 观测模型
观测模型即p?zt|xt?描述了当机器人位姿为xt时，观测到的信息为zt的概率。其中：
ut?1??vt?1,wt?1?为t?1时刻里程计给出的控制信息。vt?1和wt?1分别表示t?1时刻机器人
的速度和角速度。
机器人利用自身配备的距离/方向传感器探测陆标，得到陆标的距离和方向角。其观测模型见式?3.2?[9]：
??x?x?2??y?y?2?w?cicir?z???R????? ?i???? ?yc?yi?? （3.2）z?arctan???w????????????x?xci??i其中：?xc,yc?分别是机器人的位置；?是机器人的方向角；w?、w?分别是对应于测量距离和方向的噪声。机器人观测模型如图3.2所示：
图3.2 机器人观测模型
18
武汉科技大学本科毕业设计
4 FastSLAM算法步骤
4.1 FastSLAM算法步骤
基于粒子滤波器和EKF的FastSLAM算法如下：
?i?① 根据初始坐标，随机生成Num个粒子s?oi?、粒子权值?o组成的粒子集，见式
?4.1? 、?4.2?、?4.3?：
?i??i?so?{so,wo}i?1,?,Num （4.1）
?i??i??i?,yo,?o? （4.2） s?oi???xo
?i??i? sk??sk（4.3） wk
i?1Num?② 预测机器人位姿[10]，根据k?1时刻粒子集sk?1中的每个粒子s?ki?1及运动模型?i??i??i?psk|uk,sk?1，计算k时刻粒子sk。
??③ 计算权重，重要性权值计算方法见式?4.4?
?i?p(XkR|Z0:k,u0:k,n0:k) ?? （4.4） RR?i??i??Xk|X0:k?1,Z0:k,ukik???i??计算粒子s?ki?对应的特征点坐标，并将其与环境陆标集④ 根据观测模型p?zk|ski中的某一陆标mk进行关联。
?i?⑤ 对粒子权值wk进行更新，见式?4.5?[11]:
?i??i??i??i??????pz|s??kk?1kkk?1GM?,?????K?GM?,?????K? （4.5）
22式中，函数G为高斯概率计算公式，可根据zk对粒子集权值进行更新。
⑥ 归一化见式?4.6?：
N?k??/??k?i?iki?12?i?Num （4.6）
⑦ 计算有效粒子数Neff?1/???ki?，设定一个阈值?，若有效粒子数小于这个阈值，
i?1?i?根据粒子权值?k对粒子sk进行重采样[12]，去除权值小的粒子，复制权值大的粒子。
⑧ 计算权重。
⑨ 环境地图更新，利用p?mi|sk,zk,uk,nk?对mi进行更新。
i⑩ 通过公式Xk???kixk计算粒子k时刻的位姿，若还要计算下一时刻的位姿，
i?1N则转到步骤?；否则结束定位。
19
武汉科技大学本科毕业设计
4.2 新位姿采样
粒子集st是递增更新的，是根据t?1时刻的粒子st-1和路标观测量zt，控制量ut得来的。机器人的新位置st,?m?是由上一个位置st?1,?m?和控制输入ut推算得出的，然后结合新的观测值zt来更新路标的位置。FastSLAM对上一时刻的路径st-1和最近的控制量ut进行采样,t时刻st?1中的每个粒子st,?m?生成由可能性运动模型取样中获得的一个对机器人姿态估计的可能性估计，见式?4.7?[13]：
s?m?m?~pst|u1,st?? 1 （4.7）
?? 然后将这种估计连同路径st?1,?m?加入到一个临时粒子组中，假设st-1中的粒子组根据
pst?1|zt?1,ut?1,nt?1来分配，则新的粒子也根据pst|zt?1,ut,nt?1分配。通过这种方法生产
????M个粒子后，通过对临时粒子组取样获得新组st。每个粒子st,?m?通过权重因数?i?m?更新获得。
4.3 环境特征估计的更新
环境特征估计是以机器人的位姿为条件的，N个EKF加到集合st的每个粒子上。第n个环境特征的后验概率很容易得到。它的计算是要看是否n?nt，也就是，是否环境特征在t时刻被观察到。对于被观察到的环境特征，根据通常用贝耶斯定理的分解后验概率的程序。 p?nt|s?t,?m?,z,u,nttttt?????p?z|?mtBabes?m?t?1tt?m?t?1tt ,s,z,u,np?|snttntt,z,u,n （4.8）
??? p?nt|s?t，?m?,z,u,nt?Markov ???m?pzt|?nt,st?m?,ntp?nt|st?1,?m?,zt?1,ut?1,nt?1 （4.9）
??????m?为第m个粒子的观测量概率函数的倒数。 若nt?n，则简单地使用高斯公式不变[14]：
p?nt|st,zt,ut,nt?p?nt|st?1,zt?1,ut?1,nt?1 （4.10）
t时刻的均值和协方差矩阵更新：
?t?gst?m?,?nt,t?1 （4.11） z
??????
G?nt???ntg?st,?nt?|s?s?m?;?tt?m?nt??nt,t?1 （4.12）
?TG?Rt （4.13） zn,t?G?nt??nm ,t?1?tnt?T?m??1G?Qt （4.14） Kt?m????nm t,t?1?m??m??m??m???? （4.15）???Kz?z ?n ,tn,t?1ttttt
??m?nt,t?1???I?Kt?m?G????nm （4.16）,tt?1
20
武汉科技大学本科毕业设计
5 算法流程图和代码
5.1 FastSLAM算法伪代码
FastSLAM算法伪代码如下[15]： for i=1 to M do
得到在t时刻的观测值，进行数据融合 //处理nt个观测到的路标 for j=1 to ntdo if 这是一个新的特征
then 把这个特征加入到地图中 else
1.为第i个粒子采样一个新的位姿，然后计算其采样权值 2.更新路标位置估计 end if end for
处理未观测到的路标 end for
重新获取一组新的M个粒子
5.2 FastSLAM算法流程
Fastslam算法流程如下：
St?Saux??
for m=1 to M //在所有的粒子中循环
?m??m??m??m??m??m? 对第m个粒子的St-1采样st?1,Nt?1,?1,t?1,?1,t?1,?,?N?m?,t?1,?N?m?,t?1
t?1t?1m? s?m?~p?st|u1,st??1? // 采样新位姿
? for m=1 to Nt?-m1 //在可能的数据融合中循环
G?，n???ng?st,?n?|s?s?i?;?tt?i?nt??nt,t?1
?t?gst?m?,?nt,t?1 z?m?T zn,t?G?nt?nt,t?1G?nt?Rt
21
??