基于动态种群分布的双变异优化FastSLAM算法的改进

梁雪慧 张瑞杰 赵 菲 程云泽

（天津理工大学电气电子工程学院）

无人驾驶车辆最重要的功能之一是建立周围环境的地图并且通过地图进行导航，这通常被称为SLAM （同时定位与建图）［1］。 然而， 在实施SLAM时同时让无人车进行定位和建立地图存在一定的困难，难点在于如果要使无人车知道其真实位置， 必须在定位之前构建非常精确的地图；但是在建立精确的地图之前，车辆必须准确对自身定位，否则会导致误差不断累积从而影响结果的精确性。 车辆在行驶过程中的任何摩擦、控制损失或者小障碍都往往是导致错误测距的原因，从而对真实位置估计不当。

SLAM算法最早由Smith R等提出［2］，它利用扩展卡尔曼滤波估计机器人的位姿、路标特征和联合分布，但由于在实际环境中噪声与环境状态多为非线性参量，且计算量会随着迭代次数的增加急剧上升， 因此Martinez-Cantin R和Castellanos J A又提出了基于无迹卡尔曼滤波的SLAM 算法［3］，虽然具有一定的调节能力，但是算法只能用于高斯环境。为了不局限于高斯环境，Doucet A等提出了基于粒子滤波器 （Rao-Blackwellized）的SLAM算法［4］，从而打破了线性和高斯环境的限制。另一方面，在此基础上Montemerlo M等又提出了针对特征地图的FastSLAM算法［5］，把SLAM分解为两个“独立”的问题，并分别采用不同的滤波方式使算法能够在大规模复杂的环境中运用。 但是在计算过程中， 随着粒子种群不断更新权重，这其中有大部分的粒子权重会非常小，甚至权重会集中在种群中的几个甚至单一粒子上，最终出现“粒子退化”的问题。 粒子滤波中的重采样虽然可以降低粒子的退化程度， 提升有效粒子数目，但是会出现一些问题，一方面使得粒子的多样性有所减弱，另一方面会带来粒子的衰竭，计算后只有权重最大的粒子生存下来， 影响结果的精度。Sim R等提出的粒子群优化算法（PSO）虽然已经广泛应用于各种群智能问题［6］，但仍然很难保证粒子的多样性和结果的精确性。 为了解决这些问题，宁国忠提出了一种自适应的粒子群算法（APSO）［7］， 该算法能够根据运算过程中的粒子种群收敛情况自适应调节全局概率，加强全局最优粒子的局部搜索性能， 从而避免粒子陷入局部最优，提高计算精度。

目前已经有学者指出APSO算法不一定能得出问题的最优解［7］，一方面是粒子种群是固定不变的，假若粒子种群规模较大，则会很大程度上增加计算量； 另一方面是粒子群陷入局部最优时，随着计算的不断进行，搜索范围会越来越小，这两方面的问题往往会导致结果不理想甚至失败。 笔者采用种群动态分布策略，并且引入双变异算子，将它应用到无人车的FastSLAM中，提出一种基于种群动态分布双变异优化的FastSLAM算法。 在计算过程中对粒子种群进行判定，如果种群收敛，则通过动态种群策略的调整对粒子种群进行增加或删减粒子来保证粒子多样性，并且对种群中适应度最差和最优的粒子分别进行高斯变异，从而加强在其附近搜索的能力，使算法得到理想的最优解。 最后，将改进后的FastSLAM算法应用于无人驾驶车辆，并进行仿真验证。

1 FastSLAM算法

1.1 FastSLAM算法的流程

基于粒子滤波器和扩展卡尔曼滤波的Fast-SLAM算法步骤如下。

k时刻粒子集sk由该时刻所有粒子

预测机器人位姿， 根据k-1时刻粒子集中的每个粒子及其运动模型来计算k时刻的粒子集sk。

计算权重，k时刻第i个粒子的权值由k-1时刻第i个粒子的权值和k时刻第i个粒子本身的值所估计的k时刻粒子位姿zk得出，具体计算式为：

根据观测模型计算粒子集sk-1对应的特征点坐标，并将它与环境陆标集中的某一陆标mk进行关联。

对粒子集权值进行更新。

重采样。

更新粒子集权重。

更新环境地图。

更新粒子k时刻的位姿，若无人车继续运行，则转到预测机器人位姿步骤，否则结束定位。

1.2 FastSLAM算法的缺陷

FastSLAM由于算法本身的估计误差会随着时间的推移而累积［8］，因此产生了广泛存在的权值集中在少数粒子、粒子的多样性无法得到保证等问题。 粒子退化意味着计算效率在大部分对于计算结果仅有微小作用的粒子影响下会不断降低，且由于误差的累积而导致结果精度也受到影响。 但是每一个粒子所包含的位姿信息是所有时刻的位姿信息，然而，算法每一次的迭代过程只需要当即时刻的位姿信息， 进行重采样之后，虽然淘汰了大多数权重小的粒子，但同时也删除了这些粒子中所包含的过去时刻的信息， 并且，权重大的粒子被多次复制，那么复制出的粒子也许都继承的同一个原始粒子的信息，这样虽然保证了一定数量权值大的粒子参与计算但是大量的粒子位姿信息被删除，从而会大幅削弱地图估计的精确性，引起粒子缺乏多样性的问题。 这使得FastSLAM算法存在粒子多样性严重退化的显著缺点［9］。

另一个方面，在FastSLAM算法中粒子种群数量是固定不变的，如果种群数量过大，则会很大程度上影响运算效率； 相反如果种群数量过小，则会导致运算得出的最优解不是理想结果，无人车实际路径与预设路径偏差过大。

2 基于种群动态分布的双变异优化FastSLAM算法

传统的FastSLAM算法中由于需要对粒子进行重采样，会导致种群中只有少数几个粒子甚至单一高权重粒子参与到后续计算中［10］，为了改善粒子集的多样性，同时避免自适应粒子群优化算法中容易出现的局部最优现象，使无人车的状态估计更加准确，笔者将种群动态分布策略和动态双变异粒子群算法引入到FastSLAM算法中，在保证计算效率和结果精度的前提下得到更加优秀的粒子集［11］。

2.1 动态种群分布策略

针对传统FastSLAM算法中粒子种群固定不变的特性，引入了动态种群分布策略来提高优化性能。 加入动态种群分布策略之后，种群规模、惯性权重和学习因子都在搜索过程中能自适应地改变数值大小来进一步优化算法。 在计算过程中， 提出一种新的实时种群分布估计（Particle Swarm Distribution Estimation，PSDE）， 根据粒子种群的分布情况进行判定，共有两个状态——收敛和分散，按照动态种群分布策略的设计，如果当前粒子种群的分布状态为收敛，那么种群则增加一个粒子来增加粒子群的多样性；如果判定粒子种群此时刻的状态为分散，那么种群减少一个粒子，这样在保证采样多样性的同时也减少了运算时间，提高了运算效率；如果判定粒子种群为不收敛，则种群减少一个粒子来降低计算次数和运算时间。

根据PSDE，计算每一代粒子到全局适应度最优粒子的距离时，将它与全局适应度最差粒子到最优粒子之间的距离进行比较，假如距离大于最优与最差粒子距离的粒子数量大于粒子总数的60%，则种群此时的状态为分散，那么种群将随机删除一个除全局最优粒子之外的粒子； 反之，假如距离小于最优与最差粒子距离的粒子数量大于粒子总数的60%， 那么种群此时的状态判定为收敛， 且极有可能已经陷入局部最优的状态，此时将增加一个粒子，增加的粒子由下式决定：

其中，newPos为增加粒子的位置，Xmax和Xmin分别是搜索空间的上、 下限，gbest为全局最优粒子，Gaussian是一个均值为μ（μ=0）、标准差为σ的高斯分布随机数。

2.2 动态双变异优化

在PSDE-FastSLAM算法中针对粒子种群的分布做出了动态调整， 但为了避免出现如图1所示的粒子分布（状态估计判定为分散，位姿信息全部集中在某一个或者少数几个粒子上）， 加入了两个变异算子（Dynamic Dual Mutation，DD）［12］，通过对适应度最优的粒子和适应度最差的粒子进行高斯变异来控制算法， 提高算法的搜索能力，同时避免动态种群分布策略产生的过度分散的情况。

图1 粒子种群过度分散

加入的第1个变异算子是最差适应度变异算子，对适应度最差的粒子在迭代的每一代都进行较大概率的高斯变异， 用来增加粒子的搜索空间。 在该算法中，对最差适应度粒子以变异概率pm1进 行 变 异，pm1值 根 据 经 验 确 定， 一 般 在［0.08，0.50］取值，粒子变异过程为：

其中，xi为变异后的新粒子，gworsti为变异前的最差适应度粒子，ζ为服从高斯分布的随机量。

第2个变异算子是最优适应度变异算子，随着计算的不断进行，粒子种群的分布逐渐趋于收敛，运算后期算法出现局部早熟收敛甚至最优值不再进行更新，于是对最优适应度粒子以变异概率pm2进行变异，pm2值也根据经验确定， 一般在［0.01，0.06］取值，粒子变异过程为：

其中，gbesti为变异前的最优适应度粒子，γ为服从高斯分布的随机量。

2.3 PSDE-FastSLAM-DD算法的实现

PSDE-FastSLAM-DD算法的实现步骤如下：

a. 根据初始坐标，生成具有Num个粒子的粒子集s0和相应的权值ω0，如前文式（1）～（3）；

b. 计算粒子适应度值，根据适应度来选择局部历史最优适应度粒子和全局最优适应度粒子；

c. 进行PSDE步骤；

d. 对最差适应度粒子进行高斯变异，帮助粒子种群进行收敛，避免过度分散；

e. 更新粒子适应度以及局部历史最优适应度粒子、全局最优适应度粒子；

f. 对最优适应度粒子进行高斯变异， 帮助粒子种群跳出局部收敛状态，保证运算结果的精确性；

g. 更新环境地图；

h. 计算粒子k时刻的位姿， 若无人车继续运行，则转到步骤b，否则结束更新。

3 仿真结果及实验分析

3.1 仿真模型

无人车的里程计数学模型如下［13，14］：

其中，Xk+1表示无人车在k+1时刻的位姿状态，uk+1表示无人车在k+1时刻里程计的控制输入，θk为k时刻无人车转过的航向角，xk和yk分别为k时刻无人车的坐标值，D为无人车的车长，Δdk+1为单位时间内无人车走过的路径长度，Δθk+1为单位时间内无人车的偏转角度。

观测模型为：

其中，r和θ分别表示检测到的环境特征与无人车之间的距离和运动航向角，xl和yl表示里程计接收的路标信息，ψk表示观测噪声。

3.2 MATLAB结果分析

3.2.1 MATLAB仿真环境

为了验证PSDE-FastSLAM-DD算法的实际有效性，在实验中，针对天津理工大学实际地图来创建仿真环境， 选取学校东门作为路径起点，无人车车辆途径体育馆， 最后到达机械工程学院，对此创建了如图2所示的一个基于路标点和导航点的仿真环境， 环境中设置34个路标，24个导航点。

图2 仿真路径

仿真中时间ΔT=0.025s， 运动过程噪声为0.3m/s，仿真过程中取20个粒子，仿真结果如图3所示。

图3 仿真结果

在图3中，黑色方块为仿真对象无人车，绿色星号点为路标点，红色圆圈点为导航点，红色细实线为预设路径，蓝色实线为无人车的实际行驶路径， 黄色显示为无人车在该点监测到的路标点，从仿真结果可以看出，无人车基本按照预设路径完成了行驶过程。

3.2.2 仿真参数的选取

笔者在MATLAB仿真平台上采用独立实验分别 对 传 统 的FastSLAM 算 法、 基 于APSO 的FastSLAM算法和改进后的PSDE-FastSLAM-DD算法这3种不同的算法进行仿真对比分析来验证改进后算法在实际应用中的有效性。

采用一个样本空间， 粒子种群规模设定为30，算法迭代500次结束，选取Rastrigrin为测试函数，针对算法的均最小值、均耗时两个方面进行测试，从测试结果中得到最优的参数。

由测试结果表1可得，选取最差适应度粒子的变异概率pm1为0.20， 最优适应度粒子的变异概率pm2为0.03， 其他变异概率的取值都不利于算法的收敛。

表1 变异概率对算法性能影响的测试结果

3.2.3 3种算法对比

为了验证算法的优越性， 采用一个样本空间， 对比了传统的FastSLAM算法、 基于APSO的FastSLAM算法和PSDE-FastSLAM-DD算法的无人车实际运动路径与预设路径之间的偏差，结果如图4所示，结果显示PSDE-FastSLAM-DD算法的无人车的运动路径与真实值相比偏差最小，说明它具有较高的计算精度。

图4 3种算法路径偏差的对比

实验又对比了3种算法的估计精度和运行时间（表2）。 由表2可知，改进后的PSDE-FastSLAMDD算法不仅估计精度比其他两种算法有所提高，运行效率也有了提升。

表2 3种算法估计精度和运行时间

4 结束语

在传统的FastSLAM算法中， 对粒子集进行重采样会引起“粒子耗尽”问题，而单一的PSO算法会随着运算而发生“局部最优”的情况，若采用APSO算法来替代FastSLAM算法中的粒子滤波同样难以保证粒子种群不陷入 “局部收敛” 的情况。 针对这两种算法的特点， 改进了FastSLAM算法， 在算法初期运用动态种群分布策略根据粒子群的实时进化状态来改变其规模， 并且在运算中加入针对最优适应度粒子和最差适应度粒子的变异算子， 增强这两种粒子附近的搜索范围，既避免陷入了“局部最优”，又避免了粒子群过度分散、 粒子信息单一的缺陷， 保证了粒子的多样性。 理论分析和实验表明，改进后的PSDE-FastSLAM-DD算法在估计精度和运行效率方面的性能都较为突出， 保障了无人车的实时精准运行。