一种改进FastSLAM的移动机器人同时定位与建图算法

1 引言
SLAM指在室内[1]，地下[2]，水下[3]等环境下，移动机器人通过携带的传感器获得控制信息与观测信息，完成对环境地图构建，并确定机器人在地图中位置的过程[4]。其中，扩展卡尔曼滤波(ExtendKalman Filter，EKF)[5]是常用的解决SLAM的滤波类算法，然而当环境中路标过多时，EKF算法存在计算量过大的问题。为了减少SLAM算法的运行时间，文献[6]提出FastSLAM算法。FastSLAM基于Rao-Blackwellized粒子滤波算法，以及路径已知情况下路标估计相互独立的条件，结合粒子滤波器[7,8]和EKF求解SLAM后端问题。FastSLAM算法在计算速度方面相比EKF算法有明显的优势，但存在粒子退化，粒子多样性损失[9]等问题。为提高FastSLAM算法的精确度，FastSLAM2.0算法[10]在预测步骤加入观测信息， UFastSLAM[11] 结合无迹滤波器(Unscented Kalman Filter，UKF)与FastSLAM给出更加鲁棒的求解算法；此外，通过增加粒子的多样性，例如结合遗传算法[12]，粒子群优化算法[13]随机改变粒子的信息，文献[14]在UFastSLAM的基础上应用模糊控制器提高了粒子对路径估计的精度。文献[15]应用自适应的渐消无迹卡尔曼滤波器替代EKF，依据自适应渐消滤波理论提出一种可自适应调节参数的提议分布。文献[16]应用自适应粒子群优化代替粒子滤波，并且引入了粒子筛选区间。需要指出的是，上述文献均没有解决FastSLAM陷入局部最优解的问题，即当多数粒子包含的信息为局部最优解时会导致FastSLAM算法的误差逐渐加大。

给定SLAM优化问题的目标函数，应用拉格朗日对偶理论[17]可求得其最小值的下界。文献[18]应用弦距离[19]表示角度之间的偏差并求解其拉格朗日对偶问题，并应用拉格朗日对偶理论验证了SLAM问题所得解的最优性。文献[20]在目标函数中引用复数形式，并进一步完善文献[18]得出的验证算法。文献[21]给出三维情况下的最优解验证方法。然而，随着状态向量维度的增加，对偶问题的求解复杂度大大增加。

本文提出一种结合FastSLAM与拉格朗日对偶理论的改进SLAM算法。首先，通过拉格朗日对偶理论获取FastSLAM算法所有粒子对应的目标函数值与对偶问题最优值的差值，进而消除陷入局部最优解的粒子。其次，提出一种判断粒子是否陷入局部最优的方法。仿真结果证明改进算法在精确度上优于传统FastSLAM算法。
2 移动机器人 SLAM 问题描述
SLAM 问题指移动机器人根据各个时刻采集到的控制信息和观测信息，完成对自身路径以及地图中观测到的路标位置的估计，如图 1 所示：