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

　　（9）
　　式中Rk为全景视觉移动机器人的测量噪声协方差矩阵；ZtPred为t时刻全景视觉移动机器人对环境中路标的预测观测值。ZtPred由已经创建的地图mt-1和全景视觉移动机器人的预测位姿值X计算得到，引入如下函数[5]：
　　（10）
　　如果全景视觉移动机器人在t时刻再一次观测到之前观测过的特征mt-1，就可以在全景视觉移动机器人预测位姿X的基础上，得到这个特征相对于全景视觉移动机器人预测位姿X的预测观测值ZtPred。根据观测值的预测值和实际值差值判断全景视觉移动机器人更新后的位姿X与实际位置是否更接近。文章在此设定一个阈值？啄，当适应度函数值Fitness<？啄时，表明这个预测值X与全景视觉移动机器人的真实位置误差较大，需要再一次进行粒子群优化的寻优过程，直到Fitness？叟？啄为止，表明X已经分布在全景视觉移动机器人的实际位置的小误差范围内。优化所有粒子，使每个粒子都向全景视觉移动机器人的实际位置周围运动。全局最优解XgBest和局部最优解XpBest的初始值为预设值，在FastSLAM运行过程中的每一个周期内，将这个周期的前一个周期里全景视觉移动机器人位姿的估计值作为局部最优值XpBest，再通过式（10）对环境中的路标观测值预测后，利用式（9）计算出适应度，如果全景视觉移动机器人的某个位姿预测值的适应度值最大，则这个值就是全局最优解XgBest。
    基于全景视觉移动机器人的粒子群优化FastSLAM算法流程如下：
　　（1）预测：通过提议分布对当前状态下的粒子群集合进行预测采样，得到下一个时刻的粒子群集合，st～p（st|st-1，ut）。
　　（2）粒子群优化：
　　a.将粒子集st的全景视觉移动机器人位姿作为XpBest；
　　b.通过式（7）和式（8）求取并更新粒子的速度，得到经过优化后的预估粒子集st→s；
　　c.通过式（9）和式（10）计算出环境中路标观测的预测值和适应度函数值Fitness；
　　d.当Fitness<？啄，记录新环境中路标观测值zt和更新后的地图，X→xt，返回b；
　　e.当Fitness？叟？啄，粒子集s的全景视觉移动机器人位姿赋予
　　XgBest，粒子群优化结束。
　　（3）权重计算：粒子的权重由下式计算出
　　（11）
　　（4）重采样：按照权重大小的不同，在临时的粒子集s里抽取一定比例的粒子，并将这些抽取出来的粒子添加到新的粒子集s中。
　　如果粒子集在计算权重之前，粒子集在粒子群优化后能更接近全景视觉移动机器人的实际位置，则权重可以体现粒子的真实分布情况，重采样的效果也就更好，粒子集的收敛的速度也更快，也就会为下一个周期的全景视觉移动机器人的位姿预测提供一个更加准确的初始值。
　　3 实验及分析
　　本小节分别对基于全景视觉移动机器人的传统FastSLAM算法和文章的改进FastSLAM算法的进行仿真实验，进而验证文章所提出全景视觉移动机器人的改进FastSLAM算法的优势。
　　仿真选取的环境为400m×400m的矩形区域，该区域中分布着若干环境特征点，用“*”表示；在实验中提前规划了全景视觉机器人的行驶路线，全景视觉机器人的行驶路线由25个路径点（用“”表示）确定。
　　在上述仿真条件下，选取粒子数N=100，分别对传统的FastSLAM和文章采用的改进的FastSLAM算法进行仿真实验，对全景视觉移动机器人定位精度进行比较分析。
　　图1和图2为传统的FastSLAM和文章采用的改进FastSLAM的仿真结果。全景视觉机器人从（0，0）点出发，沿行驶路线按逆时针方向行驶1周。长虚线是按路径信息设置好的全景视觉机器人运动轨迹；短虚线表示机器人实际运动轨迹；直线表示机器人利用SLAM算法之后得到的估计更新后的轨迹；“×”表示地图中预设的静止路标；“+”表示全景视觉机器人经过SLAM算法得到的环境中路标估计位置；椭圆表示全景视觉机器人对路标误差的估计，椭圆的大小代表了对应路标的不确定程度。位于短虚线的矩形表示全景视觉机器人在控制信息下到达的实际位置，位于直线的矩形表示全景视觉机器人在SLAM算法下所估计的位置。
　　图3和图4为两种SLAM算法获得的全景视觉机器人定位误差对比情况。横轴为SLAM执行过程中的运行时间，纵轴为全景视觉机器人在X和Y方向上的位置偏差，t时刻X方向的位置偏差值是由该时刻的全景视觉机器人位置的实际值和经过SLAM算法得到的估计值的全局坐标的x值相减并取绝对值得到，Y方向的位置偏差同理可得。
　　总体上从曲线中可以看出，两种算法都具有较小的估计误差，且误差具有收敛性，由此表明，这两种SLAM算法能有效实现自主定位。但传统的FastSLAM算法的全景视觉机器人位置估计的X方向的最大误差为5.0469m，Y方向的最大误差为2.7413m，而采用文章提出方法的全景视觉机器人位置估计的X方向的最大误差为1.0576m，Y方向的最大误差为0.9717m。证明文章采用的改进的FastSLAM算法的全景视觉机器人定位精度明显优于传统的FastSLAM算法。仿真实验结果与前文的理论分析一致。
　　4 结束语
　　文章将粒子群优化算法引入到全景视觉移动机器人FastSLAM算法，使粒子获得更接近真实系统状态分布的预测，提高了全景视觉移动机器人FastSLAM预测过程的粒子采样效率。仿真实验证明，全景视觉机器人在完成位姿估计和地图创建时仅需较少的粒子即可实现，提高了SLAM的精度和运行效率。
　　参考文献
　　[1]梁志伟，马旭东，戴先中，等.基于分布式感知的移动机器人同时定位与地图创建[J].机器人，2009，31（1）：33-39.
　　[2]吴叶斌.基于全景视觉的移动机器人SLAM方法研究[D].哈尔滨：哈尔滨工程大学，2011
　　[3]Kurt K.Improved Occupancy Grids for Map Building.Autonomous Robots，1997，4：351-367.
　　[4]K.Murphy. Dynamic Bayesian Networks： Representation， Inference and Learning Phd.Thesis University of Callfornia Berkeley，2002.
　　[5]陈白帆.动态环境下移动机器人同时定位与建图研究[D].中南大学，2009.