武汉科技大学本科毕业设计
2 基于粒子滤波器的SLAM算法
2.1 SLAM的通用框架和理论模型
地图编码器细调特征参数细调特征参数增加可信度增加可信度加入新的特征扩充地图消除讨厌的特征减小可信度测量和位置预测预测的特征观测用确定的地图估计匹配的 是 预测和观测 非期望的观测是不可预测的观测否匹配非期望的观测否 原始传感器数据感知在机传感器观测 图2-1 SLAM通用框架
SLAM问题是把定位和地图构建结合在一起的，下图为SLAM的理论模型：
7
武汉科技大学本科毕业设计
mkzt,kxxt?1uxt?2t?2t?1uxttut?1zt?1,jmj路标
图2-2 SLAM的理论模型
2.2 粒子滤波器定位的基本原理
粒子滤波（PF）定位方法用粒子集描述概率分布，而不用概率分布函数本身表示[4]。卡尔曼滤波器（KF）和扩展卡尔曼滤波器（EKF）利用参数化模型(一个高维的高斯函数)来表示概率密度分布；粒子滤波器采用一系列有限的采样粒子代表概率密度分布，高概率区域含有高概率的粒子，反之亦然。粒子滤波以样本集合的方法逼近概率分布，如果粒子数趋于无穷大，那么这种非参数化的粒子分布能够逼近任何复杂的概率分布。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布。它采用一组带有权值的随机样本来描述概率分布。在概率高的区域，粒子的密度就大，相反，在概率低的区域，粒子的密度就小。
粒子滤波定位方法优点很突出：能够处理多峰分布；能够很大地降低内存耗费；能够更精确地定位，且便于实现；不受限于高斯分布，在很多定位问题中，如位姿跟踪、全局定位甚至绑架问题，都取得较好的应用。粒子滤波方法作为一种基于贝叶斯估计的非线性滤波算法，在处理非高斯非线性时变系统的参数估计和状态滤波问题方面具有独特的优势。
EKF只适用于高斯噪声模型，对于非线性的模型表现非常差。而粒子滤波则适用于任意噪声模型。利用EKF只能在滤波误差和预测误差很小的情况下通过对系统的非线性进行近似以获得令人满意的估计精度，否则滤波初期估计协方差下降太快会导致滤波不稳定甚至发散。此外EKF还要求噪声必须服从高斯分布，大大限制它的应用范围。
8
武汉科技大学本科毕业设计
而PF在测量的基础上，通过调节个粒子的权值大小和样本的位置，来近似实际的概率分布，并以样本的均值作为系统的估计值。因此，PF不受线性化误差和高斯噪声假定的限制，适用于任何状态转换和观测模型，有效克服了EKF的缺点。
PF也有缺点：相对于EKF，粒子滤波的计算了较大，特别是在采用非常多的粒子数目时。随着计算机处理能力的不断增强，以及各种改进的实时粒子滤波算法不断涌现，PF算法计算了大等障碍正在逐渐消失；在采用PF算法时，在粒子滤波中，普遍存在的问题可能是存在的退化现象[5]。因为PF抽取N个具有权值的粒子后，系统如接收到新的观测数据，则实时更新每个粒子的权值。重要性权值的分布会变得越来越倾斜，这时有可能出现粒子匮乏现象。即经过几次迭代后，少数样本会占去总体样本的绝大部分权重，除了一个粒子以外，其余粒子均只有微小的权值，可忽略不计，这样使得样本集不能体现所要表达的后验概率密度，同样使得大量计算浪费在几乎不起任何作用的粒子的更新上。
2.3 扩展卡尔曼滤波器算法
卡尔曼滤波器是由Kalman在20世纪60年代提出的一种递推滤波器算法，它不需要保留使用过的测量数据，当系统检测到新的测量数据后，可以利用递推公式获得新的系统状态估计。扩展卡尔曼滤波器是在卡尔曼滤波器的基础上针对非线性系统输出估计提出的一种改进方法。在扩展卡尔曼滤波器中，对非线性系统首先采用一级泰勒展开进行线性化近似，然后利用最小均方误差准则对非线性系统状态方程和测量方程的输出进行估计。在基于扩展卡尔曼滤波器的机器人同步定位与地图重建（Extended Kalman Filter SLAM，EKF-SLAM）算法中，系统噪声均被默认为符合高斯分布，因此利用系统状态的均值和协方差进行系统状态输出的描述。在EKF-SLAM中，系统状态中包括机器人的位姿和已检测到所有路标的位置，因此，协方差矩阵的大小随着路标数量成平方比而增大。在EKF-SLAM算法中，根据机器人的运动状态方程，并结合机器人当前位姿和运动控制输入利用扩展卡尔曼滤波器对机器人下一时刻的位姿进行预测估计。当机器人传感器检测到外部路标时，根据传感器测量方程，结合机器人当前位姿、路标位置以及传感器的测量信息采用扩展卡尔曼滤波器进行系统状态的更新。
传统的EKF-SLAM算法存在如下两个典型问题，一是计算复杂度，由于在EKF-SLAM中维护着系统状态的协方差，并且在每次接受到传感器测量数据时需要进行系统状态协方差的更新，因此EKF-SLAM算法的计算复杂度为O(N3)（其中N为已检测到的路标数目）。二是系统状态估计的一致性，由于EKF-SLAM算法中利用扩展卡尔曼滤波器对机器人运动方程和传感器测量方程的输出进行估计，然而在扩展卡尔曼滤波器中，对系统输出采用的是一级泰勒公式展开近似，因此当系统的非线性非常严重时，采用扩展卡尔曼滤波器对系统输出估计的精确性会严重降低，甚至导致输出估计的发散。针对EKF-SLAM算法计算复杂度大的问题，Newman等人提出了一种基于局部子地图序
9
武汉科技大学本科毕业设计
列的SLAM算法，它的核心是重建一系列相互独立而且具有固定大小的局部地图，然后再利用扩展卡尔曼滤波器将各个局部地图合并为一个完整的全局地图，从而保证SLAM过程中的耗时不会随着检测到路标数目的增大而无限增大。Lina等人从机器人位姿估计一致性最大化角度对局部地图的大小取值进行了分析。Chong等人同样提出利用子地图进行环境地图描述的方法，他的做法是对子地图之间的相对位置进行单独估计，当机器人回到先前的子地图区域时，采用搜索算法对机器人进行重新定位。子地图分割法可以有效降低计算的复杂度，但它带来的最大问题是数据关联的确认上有一定的困难。
2.3 粒子重采样
通过增加采样点数可以有效地克服退化现象，采样点数越小退化现象越严重，然而采样点数大时又影响计算的实时性。可见通过增加采样点数来解决退化问题是不现实的。
目前消除退化问题的两个关键技术分别是：选取适当的重要密度函数和进行再采样。粒子滤波的收敛精度和收敛速度直接关系到它能否实现应用及应用的范围，是另一个值得重点研究的关键内容。为了避免粒子匮乏，Gorden等人提出了重采样方案，其主要思想史除去那些权值小的粒子，并复制权值大的粒子，重采样通过过滤样本集中权值较低的样本。保留高权重样本，使权重集中在权重大的样本上，来达到解决退化问题，一般根据权重对后验概率密度进行离散近似处理。
重采样原理：t?1时刻的先验概率可由带有权值的粒子近似表示。经过系统观测并重新计算权值，那些权值大的粒子可以分裂出若干新的粒子，而权值很小的粒子将被丢弃，如此即可得到一组新的粒子。这些新的粒子加入随机量后即可预测在t时刻的状态，即系统状态转移过程。最后再次进入系统观测过程，并预测下一时刻的状态。重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化也是应该考虑的问题之一。
2.4 移动机器人SLAM问题描述
2.4.1 SLAM计算复杂度
SLAM解法在实际应用中需要满足实时性要求，即传感器的数据采集周期应大于SLAM算法处理一帧传感器数据的时间。全状态SLAM算法如EKF-SLAM的计算复杂度与状态向量的维数成指数关系，在路标数目较大时全状态SLAM算法不能满足实时性要求，因而不适合大范围环境中的SLAM问题，Guivant等人提出了CEKF-SLAM（Compressed EKF）算法，采用近似的EKF全方差计算方法，将大范围环境分成若干局部子区域，在大部分时间仅用EKF更新状态向量中的局部估计，在机器人从一个局部区域转向另一个局部区域时对状态向量的均值和方差进行全局更新，从而大大提高了计算
10
武汉科技大学本科毕业设计
效率。此外，Leonard等将整个地图分解为一系列子地图，然后对每个子地图进行滤波估计，降低了状态向量方差更新过程中的计算复杂度。扩展信息滤波器（Extended Information Filter，EIF）是EKF的对称形式，它保持了EKF数学严谨、估计准确的特点，并且，其信息矩阵几乎稀疏，特别适合SLAM问题，Eustice等从稀疏信息矩阵的角度提出了SEIF-SLAM算法，估计精度也得到了保证。Montemerlo等人采用Rao-Blackweuise粒子滤波器解决SLAM问题，并提出了FastSLAM算、法它的计算量与某一时刻观测到的地图特征数目成线性关系，尤其适用于大范围环境中的SLAM问题。
2.4.2 SLAM的联合估计
机器人运动误差如何干扰传感器误差的先后不确定会导致定位和构建地图之间先后的关系的不确定。机器人运动，位姿估计被运动误差干扰。环境特征在全中的位置也基于粒子滤波器的同时定位与地圈构件的方法研究被观测误差和位姿估计误差所干扰。然而，与观测噪声不同的是，位姿误差系统性地作用于全局地图。这个作用也就是，路径的误差与地图的误差是相互关联的。因此，没有正确路径估计的条件下无法估计地图的误差。尽管，路径误差和地图误差的关系使SLAM问题在变得更加复杂和困难，下面就要讲怎样把这个关系分解，把SLAM问题转换成一些小的问题的乘积，然后可以用更有效的方法解决这些问题。
2.4.3 SLAM的后验估计表示
在机器人的控制过程中会有噪声干扰，可以把每个控制量或者观测量以及一些噪声模型当成一个概率约束，每个控制量概率地约束两个连续的机器人位姿，观测量约束机器人和目标的相对位置，新的观测量会更新地图特征和机器人位姿以及以前所观测的地图特征，开始，这些约束可能很不确定。然而，随着地图的特征被反复的观测，约束越来越确定。在无限次的观测和控制后，所有的地图特征的位置就完全相关。
可以通过估计算法估计最可能的机器人位姿和地图。但是这些方法需要处理的是一组变化无常的观测量和控制量，不适合在线操作，这些算法一般不能估计地图的不同部分的确定性。最流行的在线解决SLAM问题的方法是：已知传感器读数，对所有可能的地图和路径估计后验概率分布。
SLAM算法的一些基本参数：地图含有N个特征，记为：
????1,?,?N? （2.1）
在t时刻，加入的控制量：
ut??u1,?ut? （2.2） 机器人轨迹：
11