一种改进的无人机FastSLAM1.0算法

沈永福, 王希彬

(1海军军训器材研究所,北京　102301; 2海军航空工程学院控制工程系,山东 烟台　264001)



一种改进的无人机FastSLAM1.0算法

沈永福1, 王希彬2

(1海军军训器材研究所,北京102301; 2海军航空工程学院控制工程系,山东 烟台264001)

摘要：无人机FastSLAM1.0算法粒子滤波的预测过程没有考虑地标的观测值,并不能利用新的测量数据更新无人机的位姿,使得粒子集偏离无人机真实位姿,需要大量粒子改进粒子滤波的精度,增加了算法的计算复杂性。为此,提出采用模拟退火算法更新粒子的预测值,调整提议分布,使得粒子集中在无人机真实位姿的周围,并利用分层重采样解决粒子退化问题。在无人机环境下对该算法进行仿真研究,结果表明其估计精度明显优于FastSLAM2.0算法,验证了算法的有效性和可行性。

关键词：快速同时定位与地图创建; 无人机; 粒子滤波; 模拟退火; 分层重采样

1引言

FastSLAM是由Montemerlo[1-3]最早提出的,采用粒子滤波的方法,把SLAM分解为定位部分和条件地图部分,很好地克服了EKFSLAM的缺陷,能够应用在大规模的室外环境。但是FastS粒子滤波中大多数粒子经多次迭代后,只有很小的权值,产生“粒子退化”现象。重采样可以抑制“粒子退化”,但重采样选择较大权值的粒子使得粒子的多样性缺失,带来“粒子贫乏”现象,使得估计精度降低。针对存在的这些问题,目前许多学者提出了改进的FastSLAM的粒子滤波算法。Yin[4]利用遗传算法的选择、交叉和变异算子进行粒子优化;Li[5]把SLAM问题转化为基于免疫算法的多目标优化问题;Chen[6]提出基于粒子群优化的方法解决移动机器人的SLAM问题。

FastSLAM1.0算法粒子滤波的预测过程没有考虑地标的观测值,并不能利用新的测量数据更新无人机的位姿,当无人机运动模型噪声大于观测模型噪声时,粒子集分布在较低测量兼容性的范围内,偏离无人机的真实位姿,导致SLAM算法的失败。为解决此问题,传统的方法就是增加粒子数,但由此又增加了算法的计算复杂度。

Metropolis[7]于1953年首先提出模拟退火算法,但是并没有引起很大的反响,直到1983年Kirkpatrick[8]才真正创建了现代的模拟退火算法,应用于组合优化问题,可以克服初值的依赖性,避免局部极值。因此,考虑引入模拟退火过程解决FastSLAM1.0中粒子滤波需要大量粒子的问题,使粒子分布在无人机的真实轨迹附近,同时使得粒子数最小。

2FastSLAM1.0算法

绝大多数SLAM算法的目标就是计算后验值:

为计算此后验值,以条件概率分布的形式,利用具有独立高斯噪声的非线性函数,重新建立运动和测量模型:

FastSLAM是基于后验值分解的思想提出的:

表明已知无人机的路径,地标的位置可以独立估计出来。而实际上无人机的路径是未知的,但这种独立性使得后验值的分解成为可能:首先估计无人机的路径;其次以无人机路径为条件估计N个地标的位置。

与FastSLAM1.0算法相比,FastSLAM2.0算法主要发生了两个变化:首先,从新的提议分布中采样;其次,粒子权值的更新反映了提议分布的变化。显然,FastSLAM2.0要明显优于FastSLAM1.0算法。

3无人机FastSLAM算法的数学模型

SLAM算法从本质上说是一个估计问题,给定无人机的运动方程,利用机载传感器(惯导与摄像机)的测量值,估计无人机和地标的状态。

将惯导方程作为无人机的运动方程:xv(k)=fv(xv(k-1),u(k))+w(k)

惯导位置、速度和姿态方程分别为:

其中,xv(k)为无人机的状态向量,包含无人机的位置Pn(k)、速度vn(k)和姿态Ψn(k),将惯导陀螺输出ωb(k)和加速度计输出fb(k)作为控制量u(k),w(k)为零均值白噪声,方差为Q(k)。

无人机的观测方程为:

其中,mi(k)为静止的第i个地标的状态,vi为零均值白噪声,方差为R。

4基于模拟退火算法的粒子优化

4.1模拟退火算法

模拟退火的主要思想来源于热力学的退火过程。物理退火过程包括加温、等温和冷却过程。1953年,Metropolis等提出了一种重要性重采样法,即以概率来接受新状态。具体而言,在温度T,由当前状态i产生新状态j,两者的能量分别为Ei和Ej,若Ei>Ej,则接受新状态为当前状态;否则以一定的概率pr=exp[-(Ej-Ei)/kBT]来接受新状态,其中kB为Boltzmann常数。当这种过程多次重复,即经过大量迁移后,系统将趋于能量较低的平衡态,各状态的概率分布将趋于一定的正则分布。这种接受新状态的方法被称为Metropolis准则,它能够大大减少采样的计算量

模拟退火算法(SA)就是一个将退火过程中系统熵值类比为目标函数值F,来模拟这个退火系统的算法。在SA中,优化问题中的一个解xi及其目标函数c(xi)分别可以看成物理退火中的物体的一个状态和能量函数,而最优解x*就是最低能量的状态。而设定一个初始高温、基于Metropolis准则的搜索和控制温度参数t的下降分别相当于物理退火的加温等温和冷却过程的状态。Kirkpatrick等人利用金属的退火过程模拟组合优化问题,将Metropolis准则作为搜索策略,以避免陷入局部最优,并最终趋于问题的全局最优解。

4.2基于模拟退火的FastSLAM

FastSLAM1.0中粒子滤波算法通过连续更新粒子的位置和权值,近似真实的后验概率分布。将模拟退火引入粒子滤波的主要思想就是考虑测量值带来的影响,在不增加粒子数的情形下,使预测值近似真实系统的状态分布。

(1)

(2)

4.3算法步骤

基于模拟退火的无人机FastSLAM是一个连续迭代过程,主要包含预测、模拟退火优化、权值计算和分层重采样四个步骤。

(1) 预测

根据提议分布从当前粒子集中采样预测值,获得下一时刻的粒子st～p(st|st-1,ut)

(2) 模拟退火优化过程

其中,r是0～1之间的随机数;g是邻域调整因子。

(b) 利用(1)、(2)式计算地标预测观测值与目标函数

(3) 权值计算

(4) 分层重采样

利用模拟退火算法,在权值计算之前,粒子集更加接近于无人机的真实置,权值计算使得粒子的分布和重采样过程更加有效。因此,粒子集加速收敛,并在下一时刻,赋予无人机位姿一个更好的初值。

5仿真试验与分析

在仿真过程中,无人机作半径为20m的圆周运动,并从无人机的周围随机选择12个特征作为地标。无人机的真实初始位置作为滤波的初值:

系统噪声与测量噪声为:

模拟退火算法中,初始温度t0=1°,循环迭代次数L=300,温度下降速率η=0.8。

图1～4给出了基于模拟退火的FastSLAM1.0(简写为SAFastSLAM1.0)与FastSLAM2.0估计的误差曲线的对比,其中红色实线表示基于模拟退火的FastSLAM1.0的估计误差曲线,蓝色虚线表示FastSLAM2.0的估计误差曲线。从仿真结果看,与FastSLAM2.0相比,不论是无人机的位置、速度和姿态误差,还是地标的位置误差,基于模拟退火的FastSLAM1.0获得了更加精确的结果。

图1　 SAFastSLAM1.0与FastSLAM2.0估计的 无人机的位置误差比较Fig.1　 The comparison of estimated UAV position error between SAFastSLAM1.0 and FastSLAM2.0

图2　 SAFastSLAM1.0与FastSLAM2.0估计的 无人机的速度误差比较Fig.2　 The comparison of estimated UAV velocity error between SAFastSLAM1.0 and FastSLAM2.0

6结论

本文将模拟退火引入无人机FastSLAM1.0的粒子滤波,通过调整粒子的位置分布,使预测采样粒子集中在无人机的真实位姿周围,有效提高SLAM的精度,减少粒子数并降低计算时间复杂度。仿真试验显示,与FastSLAM2.0相比,提出的算法可以获得更加准确的无人机位姿和地标位置的估计值,表明该算法是有效可行的。

图3　 SAFastSLAM1.0与FastSLAM2.0估计的 无人机的姿态误差比较Fig.3　 The comparison of estimated UAV attitude error between SAFastSLAM1.0 and FastSLAM2.0

图4　 SAFastSLAM1.0与FastSLAM2.0估计的 地标的位置误差比较Fig.4　 The comparison of estimated landmark position error between SAFastSLAM1.0 and FastSLAM2.0

参考文献:

[1]Michal Montemerlo.FastSLAM:A factored solution to the simultaneous localization and mapping problem with unknown data association[D].Carnegie Mellon University Pittsbwghers,2003.

[2]M Montemerlo,S Thrun,D Koller,and B Wegbreit.FastSLAM:A Factored Solution to the Simultaneous Localization and Mapping Problem[C]∥Proceedings of the National Conference on Artificial Intelligence.Edmonton:2002,AAAI,593-598.

[3]Montemerlo M,Thrun S.Simultaneous localization and mapping with unknown data association using FastSLAM[C]∥Proceedings of the IEEE International Conference on Robotics and Automation,Karlsruhe Germany,2003,1985-1991.

[4]Yin B,Wei Z O,Zhuang X D.Robust mobile robot localization using an evolutionary particle filter[M]∥Lecture Notes in Computer Science(vo1．3801),Berlin,Germany:Springer-Verlag,2005:279-284.

[5]李枚毅.结合免疫机制的并发定位与建图多目标进化算法[J].湘潭大学自然科学学报,2007,29(2):111-117.

LI Meiyi.Multi-objective evolutionary algorithms for SLAM with immunity[J].Natural Science Journal of Xiangtan University,2007,29(2):111-117.

[6]陈白帆,蔡自兴,袁成.基于粒子群优化的移动机器人SLAM方法[J].机器人,2009,31(6):513-517.

CHEN Baifan,Cai Zixing,Yuan Cheng.Mobile robot SLAM method based on particle swarm optimization[J].Robot,2009,31(6):513-517.

[7]Metroplis N,Rosenbluth A,Rosenbluth M et al.Equation of state calculations by fast computing machines[J].Journal of Cherimal Physics,1953,21(6):1087-1092.

[8]Kirkpatrick S,Gelatt Jr C D,Vecchi M P.Optimized by simulated annealing[J].Science,1983,220(4598):671-680.

沈永福男(1974-),浙江人,硕士,工程师,主要研究方向为导航制导与控制。

王希彬(1985-),山东人,博士,讲师,主要研究方向为航空自动化。

AnImprovedFastSLAM1.0AlgorithmforUAVSHEN Yongfu1,WANGXibin2

(1.NavalMilitaryTrainingResearchInstitute,Bejing,102301,China;

2.DepartmentofControlEngineering,NavalAeronauticalandAstronauticalUniversity,YantaiShandon,264001,China)

Abstract：Landmark observations are not considered to update the predicted UAV pose in the prediction process of particle filtering for fast simultaneous localization and mapping (FastSLAM) of uninhabited aerial vehicle (UAV),so the particle set deviates from the true UAV pose.A great deal of particles are needed to improve the prediction which increases the computation complexity.Simulated annealing (SA) method is proposed to update the particle’s prediction and adjust the proposal distribution,so the particles are concentrated around the true pose.Stratified resampling is used to solve the degeneracy of particles.At last,the proposed algorithm is simulated in the environment of UAV.The results show that the estimation accuracy of the proposed algorithm is superior to the FastSLAM 2.0 algorithm and validate the availability and feasibility of this method.

Key words：fast simultaneous localization and mapping (FastSLAM); uninhabited aerial vehicle (UAV); particle filtering; simulated annealing (SA); stratified resampling

中图分类号：V 24

文献标识码：A