基于全景视觉机器人的粒子群优化FastSLAM算法研究

摘 要：针对FastSLAM算法具有粒子退化及重采样带来的贫化问题，文章提出了基于全景视觉机器人的粒子群优化FastSLAM算法。首先，利用粒子群优化算法更新预估粒子，实现全景视觉机器人在预估位置时能够利用观测到的最新的路标信息，粒子群中粒子的相互作用降低了所需粒子的数量，使FastSLAM算法避免了粒子耗尽。实验证明，当粒子数相同时，文章采用的改进FastSLAM算法的全景视觉机器人定位精度明显优于传统的FastSLAM算法。
　　关键词：全景视觉；机器人；粒子群优化；SLAM算法
　　全景视觉传感器以其360°的感知范围获得更多的图像信息进而更完整地表达机器人所在的环境，增强了机器人的对视觉路标的持续观测能力和地图创建能力，提高了机器人对地图的利用率，有利于系统不确定性的收敛等特点在未知环境的SLAM（Simultaneous location and mapping）[1]问题中被广泛应用且取得了很好的效果[2]。
　　传统解决SLAM问题的方法通常依据卡尔曼滤波[3]，其最大的优点是意义明确、简洁易实现，但比较适合小尺度的环境，对于大尺度的室外环境，算法的运算量过大，无法满足应用需求。
　　Montemerlo等将Rao-Blackwellised粒子滤波融合到EKF算法中，给出了FastSLAM算法的解决方案，相比基于卡尔曼滤波的SLAM算法计算量大大降低。但FastSLAM算法的粒子退化问题仍难以解决，为此利用重采样的方式进行改进，虽然减轻了粒子退化，但却带来了粒子贫化问题。为此，文章引入了粒子群优化算法，很好的解决了粒子的贫化问题。
　　1 FastSLAM算法基本原理和粒子群优化的原理
　　1.1 FastSLAM算法原理介绍
　　FastSLAM算法的运动模型和观测模型一般用下面的形式进行描述：
　　式中，v（k）和？棕（k）为Gaussian白噪声，其协方差分别记为R（k）和Q（k）；f和h为非线性函数；Xr（k）为k时刻全景视觉移动机器人位姿；xn（k）为n（k）的路标的信息；Z（k）、u（k）为k时刻全景视觉移动机器人的观测值和运动值。
　　使用FastSLAM算法进行同时定位与地图创建的核心问题是用后验概率分布来估计全景视觉移动机器人的运动轨迹X和创建的地图Xn（k），表示为：
　　（3）
　　根据Bayes规则和Markov假设可得，
　　式（4）的采用Murphy and Russell[4]在1999年提出的因式分解法。
　　1.2 粒子群优化算法
　　在维数为n的空间里，随机一个粒子群，其中的任一粒子表示我们所要解决的问题的可能的答案，共m个。V表示第i个粒子在t时间在搜索空间里的移动速度，V表示该粒子在t时间的位置。所有粒子都根据该时刻粒子群里的最优粒子，来进行迭代搜索。
　　迭代过程中，每个粒子在不同时刻的位置和速度都会根据下面两个极值得到：粒子群体最好位置为gBest（即整个粒子群的最优解），粒子自身的最好位置为pBest（即粒子自己的最优解）。若粒子群中总共包含n个粒子，每个粒子的位置和速度可以由下面的方程（5）和（6）得到。
　　（5）
　　（6）
　　式中w为惯性权重，通常w是伴随着迭代次数的增加而逐渐变小，w越小算法的搜索能力越小，在值小到一定程度时，只能具备在局部进行搜索的能力，w越大算法的搜索能力越强，能实现全局搜索；c1为粒子自身的加速权重系数，c2为粒子群体的加速权重系数；Rand1为c1相关的随机初值，Rand2为c2相关的随时初值，且在区间（0，1）范围内；在整个搜索过程中，gBest为整个粒子群体的最优解，pBest为粒子自身的最优解，也就是最佳位置。
　　2 基于全景视觉机器人的粒子群优化FastSLAM算法
　　文章提出了基于全景视觉移动机器人的粒子群优化FastSLAM算法。结合粒子群优化算法，通过PSO算法更新预估的粒子进而改变其提议分布，这就在预测全景视觉移动机器人位置的时候，还考虑了观测到的最新的路标信息，预测到的采样粒子大多集中在全景视觉移动机器人的实际位置周围。另外，在粒子群预估时，不仅要考虑单独的粒子本身，还要考虑整个粒子群的共同作用，进而达到同传统的FastSLAM相比，在获得相同的真实系统状态分布预测精度的情况下，需要更少的粒子数量，这也就解决了前面提到的FastSLAM面临粒子耗尽问题。
　　第一，利用粒子群优化算法中的位置和速度方程对全景视觉移动机器人的位姿进行预测更新，方程如下：
　　式中，Rand1（）和Rand2（）为对角线矩阵，矩阵对角线上均为符合标准正态分布的随机正数；c1和c2表示学习因子。XgBest表示全景视觉移动机器人位姿预测值的全局最优解，XpBest表示全景视觉移动机器人位姿预测值的局部最优解。由式（7）和式（8）进行更新，全景视觉移动机器人的位姿向量由Xt变为X。
　　第二，为了评价粒子群优化算法对全景视觉移动机器人位姿预测的改进程度，引入适应度函数，函数如下：