移动机器人原理与技术 课件 第六章 移动机器人同时定位与建图.ppt

6.6移动机器人粒子滤波SLAM算法6.6.1蒙特卡罗采样对于要解决的问题，蒙特卡罗方法首先建立一个概率模型，使概率模型的参数等于问题的解，所求参数的统计特征通过对模型的观察或抽样试验来计算，最后给出所求解的近似值。蒙特卡罗方法通过随机试验求解积分问题，将服从分布密度函数p(r)的随机变量g(r)的数学期望E(g(r))作为所要求解的积分：6.6.1蒙特卡罗采样从分布密度函数p(r)中通过某种试验获得采样N个观测值和N个相应的随机变量值g(ri)i=1,...,N的算术平均值作为积分的估计值:当采样点N足够大时，根据大数定律，趋于数学期望E(g)。6.6.2重要性采样蒙特卡罗采样需要从后验概率分布p(r)中采样N个点，对于移动机器人系统，移动机器人位置估计E(g(X0:k))可以表示为：在移动机器人系统中，系统的后验分布密度函数无法直接获得，对系统状态的估计无法通过蒙特卡罗方法采样后验分布密度函数而进行。但可以对系统中一个已知的并且容易采样的概率分布函数进行采样，该概率分布函数被称为重要性采样分布或者重要性函数。通过对由重要性函数的采样点进行加权来逼近：6.6.2重要性采样具体推导如下：6.6.2重要性采样称为未归一化的重要性权值。另外，可以表示为：6.6.2重要性采样如果能够从重要性函数式采样到样本点，则通过蒙特卡罗采样后的表示为：称为归一化权值。6.6.3序列重要性采样重要性函数可以分解为：实际动态系统服从一阶马尔可夫过程并且满足系统观测独立，其数学表达式可以分别描述为：p(X0)表示该动态系统的初始先验密度。6.6.3序列重要性采样如果重要性分布函数只依赖于前一状态Xk-1和当前的观测值Zk，即上式的重要性采样函数采样得到的样本，样本的最优的选择方法是：当前时刻的粒子由前一时刻粒子Xk-1和当前的观测值Zk采样得到，这种采样方法称为序列重要性采样。将一定的权值wik赋予给每一个样本Xik，则带权重的粒子集表示Xk的概率分布，形式如下：6.6.3序列重要性采样可以近似地表达为：式中，是狄拉克函数。6.6.4退化问题与重采样序列重要性采样算法中，当前时刻的粒子权值是由上一时刻的粒子权值递推得到，存在误差的权值的误差会随着时间的传播而进一步的积累，导致只有少数粒子的权值比较大，而大多数的粒子因权值太小以至于最终被忽略不计，即序列重要性采样存在“粒子退化”现象。6.6.4退化问题与重采样“粒子退化”用指标Neff来衡量，Neff值小于阈值则表明“粒子退化”现象严重，需要采取措施改善退化现象。Neff由下式近似计算：通过两种方案来改善退化现象：选择好的建议分布或进行重采样。在通过序列重要性函数采样得到个样本的基础上，再对序列重要性函数进行次采样，剔除权值较小的样本，保留权值较大的样本。也就是权值较小的样本被复制的权值较大的样本所代替，得到一组新的样本集合。6.6.5基于粒子滤波的SLAM算法标准PF是在SIS方法上加入重采样策略，也称为SIR粒子滤波，从序列重要性函数采样得到的样本就是粒子滤波中的粒子。Montemerlo首先将粒子滤波器用于SLAM领域，提出了FastSLAM算法。其核心思想是用Rao-Blackwellise分解将SLAM问题分离为线性状态的地图特征估计与非线性状态的路径估计，应用SIR粒子滤波器估移动计机器人的路径，地图则采用扩展卡尔曼滤波器更新，FastSLAM算法也称为RBPF-SLAM算法。移动机器人技术原理与应用第六章移动机器人同时定位与建图基于滤波的移动机器人SLAM原理基于滤波的移动机器人SLAM滤波算法框架6.1基于滤波的移动机器人SLAM简介6.26.3基于滤波的移动机器人及环境的模型6.4移动机器人卡尔曼滤波SLAM算法移动机器人粒子滤波SLAM算法6.56.6基于图优化的移动机器人SLAM算法6.76.1基于滤波的移动机器人SLAM简介SLAM主要研究在对机器人位姿和其环境信息都不具备先验知识的情况下，如何应用合理的表征方法对环境建模（也就是构建地图）并同时确定移动机器人自身位姿。在应用随机概率方法解决同时定位与地图创建问题时，首先要建立概率表示的移动机器人运动模型和观测模型