武汉科技大学本科毕业设计
st?{s1,s2,?,st} （2.3）
在平面环境中包含三个参数机器人的坐标?x,y?和方向角?， t时刻以前所有路标的观测量：
zt??z?,z?
2.4（）1,t本文假设每个观测量提供的信息是一个环境特征?n相对于目前位置st的确切位置。变量n代表了某个被观察到的环境特征。实际上，并不会观察到环境特征的唯一性，因为很多环境特征看起来是一样的。对应观测量zt的唯一的环境特征写做nt，相关性变量：
nt??n?,n?1,t （2.5）
1,?,N?表示在时刻t，表示所有数据关联的集合，确定nt的过程叫数据关联，nt??第nt个路标是否被观测到。路径信息st和地图信息?两个量都依赖于观测量zt，必须把构建地图和定位过程一起考虑。机器人依靠传感器得到的观测量zt、自身的控制量ut以及相关性变量nt来得到路径信息st和地图信息?。解决SLAM问题的核心是在于应该把定位和地图构建视为整体。实际上，这是因为路径信息st和地图信息?两个量都依赖于观测量zt。所以要想找到解决方法，必须把构建地图和定位过程一起考虑。在实际应用中，这些点的环境特征可以对应传感器提取特征的位置。
SLAM问题的实质就是根据已记录的从时刻1到t的观测量zt、控制信息ut以及初始状态信息来决定所有路标位置集合?和机器人位姿st。若在t时刻所测得的路标数已知，那么SLAM问题将变得简单得多，概率分布描述成在时刻t的路标位置和小车状态的联合后验密度函数，即SLAM被定义为一个关于路径和路标的后验估计：
p?st,?|zt,ut,nt? (2.6) 如果数据关联错误！未找到引用源。的集合也已知，那么SLAM后验概率简化为： p?st,?|zt,ut,nt? (2.7) SLAM在t时刻的后验概率可以用t?1时刻后验概率函数来递归计算，这个递归的更新算法，称为SLAM的贝叶斯滤波器。
下面的递归公式，也就是贝耶斯滤波器，可以用于已知在t?1的后验概率情况下，计算时刻的后验概率：
p?st,?|zt,ut,nt????p?zt|st,?,nt??p?st|st?1,ut?p?st?1,?|zt?1,ut?1,nt?1?dst?1 ( 2.8) ?是代表概率分布的归一化因子。一般式中的积分是无法计算的，可以假设后验概率分布为一个特定形式来计算。机器人在环境移动时，采集了它自身运动的相关信息，这个信息可以用机器人轮子上带的里程计、内部的导航单元或通过观测机器人执行的控
12
武汉科技大学本科毕业设计
制命令得到。
FastSLAM是RBPF(Rao-Blackwellised Particle Fitering)滤波器的一个实例[6] ，Murphy等人对动态贝叶斯网络（Dynamic Bayesian Networks）进行分析得出结论：若机器人的路径估计已知，则陆标估计（?i,?j,i?j）之间相互独立。由贝叶斯公式及特征估计之间的独立性假设，对后验概率分布p?st,?|zt,ut,nt?分解：
p?st,?|zt,ut,nt??p?st|zt,ut,nt?p??|st,zt,ut,nt? (2.9) 其中右边第一部分用粒子滤波器估计，第二部分用扩展卡尔曼滤波器估计，每个粒子都有自己单独的地图，在每个粒子中，如果环境特征的个数是已知的，那么每个环境特征的位置都可以用一次EKF独立的进行估计。
假设需要k个粒子实现SLAM，FastSLAM总共有kn个卡尔曼滤波器，粒子滤波器的每个粒子代表机器人的一条可能运动路径，利用观测信息计算每个粒子的权重：
p?|z,u,n???p??i|st,zt,ut,nt? (2.10)
ttti?1N FastSLAM 的EKF与传统的EKF相似，它近似线性高斯函数测量模型，线性高斯测量模型得到的分布 p??n|st,zt,ut,nt? 是高斯分布的，即使动作模型不是线性的，这也是对机器人姿态使用采样来近似分布的结果。
2.4.4 SLAM公式推导
SLAM分解可以从SLAM的路径后验概率中直接推导。用条件概率的定义，SLAM后验概率可以写成：
ttttttttttttps,?|z,u,n?ps|z,u,np?|s,z,u,n (2.11)
???t???因此，要得到分解的后验概率，对所有的非负值t需要以下方程：
p(?s,z,u,n)??p(?nst,zt,ut,nt)tn?1ttN (2.12)
这个论断的证据可以从归纳中得到。要得到这个结果，需要两个中间结果．第一个量是观测到的环境特征?nt的概率。这个量可以用贝耶斯定理写成：
p(?nts,z,u,n)?ttttp(zt?nt,st,zt?1,ut,nt)p(ztst,zt?1,ut,nt)p(?ntst,zt?1,ut,nt) (2.13)
目前的观测量错误！未找到引用源。仅仅与机器人的状态和被观测到的环境特征有关。在（2.13）最右边的项，在没有新的观测量错误！未找到引用源。的情况下，当前的位姿错误！未找到引用源。、当前的控制量错误！未找到引用源。、当前的数据关联错误！未找到引用源。对错误！未找到引用源。没有影响。因此，所有这些变量可以省去
13
武汉科技大学本科毕业设计
得到：
p(ztst,zt?1,ut,nt)ttttp(?nts,z,u,n)?p(,,,) (2.14) ?sunzntttp(zt?nt,s,n)t?1t?1t?1t?1tttt需要的第二个中间结果是p(?n?nts,z,u,n)，没有观测到的任意环境特征的概率这很简
单，如果没有观测到环境特征，环境特征后验概率不变。因此，环境特征的后验概率在
t时刻等于t?1时刻。首先，在t?1时刻做归纳假设：
Np(?st?1,zt?1,ut?1,nt?1)??p(?nst?1,zt?1,ut?1,nt?1) (2.15)
n?1归纳的开始情况t?0,没有加入观测量。因此，t?0，分解因式是对的。在t?0,再一次用贝耶斯定理来扩展（2.12）的左边可以得到：
p(?st,zt,ut,nt)?p(?ntst,zt,ut,nt)?p(?tst,zt,ut,nt) (2.16)
n?ntN等于单独的环境特征后验概率的乘积。
p(?s,z,u,n)??p(?nst,zt,ut,nt) (2.17)
ttttn?1N2.4.5 有效的数据关联
在实际的SLAM应用中，很少能观测到数据关联nt。然而，如果环境特征位置的不确定性相对于环境特征间的平均距离低，确定正确的约束关联的简单提示很见效。数据关联的最普通的方法是用最大似然定理来给每个观测量指派。换句话说，每个观测量是被指派给最可能的产生它的环境特征。如果最大的概率仍比固定的限值低，就被考虑加入一个新的环境特征。
?的差值的函数。在EKF的情况下，观测量的概率可以写成观测量zt和期望观测量z这个差值被称为修正值：
?t?argmaxp(ztnt,st,zt?1,ut,n?t?1)n1?1? (2.18) T?1?argmaxexp??(zt?z?t)??t)zt(zt?z?2?2?zt数据关联的提示经常用负的对数似然方程表示，如下：
?) (2.19) ?)z?1(z?z?t?argminlnz?(z?znT方程的第二部分就是Mahalanobis距离：通过观测量和环境特征估计的协方差米制规格化的距离。由于这个原因，米制的数据关联通常被称为最邻近的数据关联。最大似然数据关联主要用在数据关联比不正确的数据关联显著合适的时候。然而，如果环境特征的位置很不确定，多个数据关联的概率很高。如果选取的数据关联是错误地。对于结果地图的精确性会有影响。如果机器人的传感器噪声严重的话，很容易产生这种数据关联
14
武汉科技大学本科毕业设计
的不确定性。
解决这个问题的一个方法是只加入那些得到明确数据关联的观测量。然而，如果SLAM环境是有噪声的，大多数的观测量得不到处理。而且，没有加入观测量会导致高估环境特征的协方差，使以后的数据关联变得更加不确定。
2.4.6 FastSLAM的粒子表示形式
在FastSLAM中，路标估计利用独立的EKF实现，一个路标对应一个EKF滤波器，地图是一组N个2维的高斯模型，而不是一个单独的联合2*N维高斯模型。共有M个粒子滤波器用来估计路径，每个粒子含有N个独立的EKF滤波器，用来估计N个路标，第i个粒子表示：
i?m?sti?{si,?m?,m1i,t,s1?,it??,mN,s,tN,t} （2.20）
其中包含一个路径st,?m?和基于该路径上N个路标高斯估计，第N个路标的高斯表征
?m??m?的均值和方差分别是?N和?N,t，如图2-3所示： ，t机器人路标粒子1路标1路标2路标N粒子2粒子M
x y ??? ? ?11?x y ? ?? ?11……………………
图2-3 FastSLAM的粒子表示
15
武汉科技大学本科毕业设计
2.4.7 FastSLAM的计算时间复杂度
FastSLAM有机地将粒子滤波器与卡尔曼滤波器集成在一起，鲁棒性地解决数据关联和多目标跟踪问题，FastSLAM算法将状态估计分解成了采样部分和解析部分。由于这种分解，其计算复杂度仅为O?MN?，其中N,M分别为FastSLAM算法所用的粒子数和未知环境中的特征个数,每个粒子中的高斯组用稳定二叉树表示，高斯参数u1,t和?1,tii在树的叶子上，访问每个高斯模型所需要的时间与K成对数增长关系。在复制粒子时，所有高斯模型树中只有一个路径必须被修改，为了完成这个树，对于所有与之分开的枝干，所有的相关点从生成的粒子复制过来。所有与之分开的枝干就指向了与生成树同样的子树，生成一个这样的不完全的书只用了K对数时间，此外，由于需要用来达到叶子的步骤数与路径的长度相等，访问高斯模型也需要K对时间。在每个更新步骤需要创建M个新粒子，所以更新总共需要0?logk?时间。
FastSLAM算法是基于粒子滤波的，它对于机器人位姿和数据融合采样，在每个粒子的基础上计算路标的位置，用一些平衡的二叉树来表示路标，使得更新路标只需要路标数的对数次，每个粒子对位姿的不确定性是来自数据融合中，能更清楚的在噪声环境中区分出来路标，甚至能在很简单的数据融合条件下进行更新路标。
k?4?Tk?2?FFk?3?TTF??m?3,?3?m?k?4?T Fk?2?TFTk?2?Fk?1?TFT?m?2k?3?F?m?3k?5?TF?m?k?7?TF?
?m?1,?1?m??,?2?m??,?3?m???m?4,?4?m???m?5,?5??m?6,?6?m???m?7,?7?m??,?8?m??m?8
图2-4 从上一时刻复制和更新树的过程图
16