用概率假设密度滤波实现同步定位与地图创建

Implementation of SLAM by probability hypothesis density filter

针对杂波环境中传统同步定位与地图创建(SLAM)算法无法有效表达传感器多种信息以及容易发生错误数据关联的问题,提出一种基于概率假设密度滤波的SLAM算法。该算法将每一时刻传感器的观测信息和环境地图表示为随机有限集,建立联合目标状态变量;通过概率假设密度(PHD)滤波对机器人位姿和环境地图状态进行同时估计,并利用粒子滤波实现PHD滤波。在进行目标状态提取时,为避免聚类算法引入的误差,对粒子集进行时滞输出。提出的SLAM算法能准确表达观测的不确定性、漏检以及杂波引起的虚警等多种传感器信息,且避免了数据关联过程,使系统状态估计更接近真实值。仿真实验结果表明:与传统SLAM算法相比,新算法的机器人定位及环境构图精度提高了50%以上,为杂波环境下SLAM问题的研究提供了新的途径。

Traditional Simultaneous Localization and Mapping（SLAM） algorithm is lack of the ability to describe multiple sensor information accurately in a clutter environment,and it is prone to false data association.Therefore,this paper proposes a SLAM algorithm based on Probability Hypothesis Density（PHD） filter to deal with these problems.By taking the sensor observation and environmental map as random finite sets in every time step,a joint target state variable is constructed.Then,with the Probability Hypothesis Density（PHD） filtering,the poses and environmental map of the robot are estimated simultaneously and the PHD filter is realized by a particle filter.To avoid the error caused by cluster,a time-delay particle set outputting approach is proposed for joint target state extracting.The new algorithm can describe the observation uncertainty,loss detecting,false alarm due to a clutter and other sensor information accurately,and also can avoid the data association,by which the system state estimation is closer to real values.The simulation results show that the accuracy of the new algorithm in the vehicle localization and mapping is improved by more than 50% as compared with that of traditional SLAM algorithm.It provides a new solution for SLAM problems in the clutter environment.