自适应遗传算法在HybridSLAM中的应用

雷建和，丁 磊，孙合胜，陈 健

(青岛理工大学信息与控制工程学院，山东 青岛 266000)

1 引言

同步定位与地图构建(Simultan eous Localization And Mapping，SLAM)在机器人领域方面有着重要的作用。SLAM主要解决的是移动机器人在未知环境中移动时根据自身的传感器信息进行地图构建的问题和由于自身在运动过程中产生的误差所引起的定位问题，这两个问题是关联度很高的问题。因此SLAM主要解决的是构建地图和同步定位这两大问题。这两个问题又分成三种情况。1)地图已知的情况下，解决定位的问题。2)在定位已知的情况下，进行地图构建。3)在未知环境中进行地图构建以及定位问题。现在大部分的SLAM问题都是在这三种情况下研究的。

SLAM的算法主要是以概率机器人学为主。最初是卡尔曼根据贝叶斯滤波器提出的卡尔曼滤波算法(Kalman filter，KF)，KF可以计算出线性条件下连续状态的概率。之后Chessseman和Smith等人在此基础上提出了扩展卡尔曼滤波算法(extended Kalman filter，EKF)，EKF可以计算在非线性条件下的概率。在此基础上，Montemerlo和Thrun等人根据粒子滤波器(Rao-Blackwellized Particle Filter，RBPF)提出了FastSLAM算法，之后又在此基础上提出了FastSLAM2.0算法。Alex等人以FastSLAM为前端EKF为后端，提出了HybridSLAM算法。该算法不需要对运动进行线性化处理，通过子地图融合避免了粒子退化的缺点，可以快速的建图。但在粒子多样性方面改进并不大。

针对多样性的问题，很多学者都在对其改进。张廷军等通过权重将粒子分为稳定和不稳定粒子，对不稳定粒子进行改进，但该算法将大权值粒子归入了不稳定粒子，没有将计算倾向于大权值粒子。张亚楠等在重采样过程中引入了粒子权重平衡的方法，但该方法具有一定的局限性。刘刚等通过将遗传算法放入粒子滤波中进行重采样以改进粒子的多样性，但其没有考虑自适应的问题，有一定的局限性。谌剑等通过将粒子权值与权值均值线性组合的方式改进粒子的权重的方式提出了权值优化组合粒子滤波算法。该算法虽然提高了精度，但减小了大权值粒子的权重。

由于遗传算法主要运用的是进化的思想，而粒子滤波中粒子退化问题是主要问题。因此可以用自适应遗传算法解决粒子退化的问题。为了让不同粒子进行不同程度的改变，最终进化整个种群，因此将自适应遗传算法进行改进，以便在粒子滤波中更合适的应用。该方法通过对粒子自适应的进行交叉和变异，保证了粒子的多样性，提高了数据准确性和全局地图的精度。

2 HybridSLAM算法和自适应遗传算法

2.1 HybridSLAM算法

HybridSLAM算法以FastSLAM2.0为前端，EKF-SLAM为后端，将两种滤波器的优点结合，规避了两者的缺点，使其性能优于两者。FastSLAM2.0作为前端，发挥其构建地图精确的优点，主要对移动机器人的位姿进行精确定位以及构建地图。EKF-SLAM作为后端，主要用来记录机器人的路径以及外界特征，可以很好的记录其随时的位置变化。规避了FastSLAM2.0可能出现离子性匮乏的缺点。

假设前端共有

M

个粒子。第

p

个粒子在

t

时可以表示为

(1)

将粒子的权重归一化

(2)

则机器人的当前位姿和已观测到的路标位置

x

和协方差

P

可表示为

(3)

(4)

式(4)中，方括号内的第一项是包含传感器误差的单个粒子构建地图的协方差，第二项为包含机器人位姿误差的各粒子所建地图之间的协方差。

子地图构建时，每个粒子都要进行预测和更新两个过程。

预测：第

P

个粒子在

t

时刻预测的机器人位姿

(5)

其中

u

为机器人在

t

时刻的控制输入。路标更新：当检测到特征路标时，假设特征关联已知，如果检测到的特征

j

是未被观测特征时。则粒子权重

ω

设为初始值

ω

=1

/M

。

由观测方程的倒数得到该特征的位置

(6)

其中

z

是机器人在

t

时刻的观测值，

h

为观测方程。如果检测到的特征

j

为已经观测过的，则每个粒子都沿用EKF-SLAM算法的更新方式进行状态估计更新。由观测方程计算出第

p

个粒子的预测观测值

(7)

算出对应的卡尔曼增益

(8)

位姿更新为

(9)

相应的协方差更新为

(10)

(11)

2.2 自适应遗传粒子滤波

自适应遗传粒子滤波算法(Adaptive genetic algorithm-Particle Filter，AGA-PF)以遗传粒子滤波算法为基础，主要解决粒子多样性的问题。该算法简单、鲁棒性好、而且可以自适应的根据不同粒子调节交叉和变异的概率。

自适应遗传滤波以滤波器产生的粒子作为初始种群，根据粒子的特性可以将粒子权重看作对应个体的适应度。之后通过轮盘赌的方式选择出新的种群；然后对选择出的种群进行两两配对，根据个体的适应度算出配对个体的交叉概率，并按此概率直接对实数进行交叉操作；再通过每个个体的适应度算出各自的变异概率，同样按此概率对实数进行变异操作，最终产生新的种群。根据进化代数与设定值的比较判断是否终止，若不终止则继续以新种群进行选择、交叉和变异迭代。

在交叉和变异过程中，两个体之间以(12)计算交叉概率，每个个体以式(13)计算变异概率。

(12)

(13)

个体对应的交叉操作如式(14)所示

(14)

个体对应的变异操作如式(15)所示

(15)

3 iAGA-HybridSLAM

3.1 改进的AGA-PF

自适应遗传算法在粒子滤波中应用时，要结合粒子滤波的情况进行改进。在粒子滤波中，权值相近的粒子，交叉后对预估值影响较小，权值越大的粒子，越接近真实值。因此针对交叉概率，既要保证适应度高的个体交叉率高，又要确保权值相近的粒子交叉率低。就要将式(12)改为式(16)。设定

f

为大适应度，

f

为小适应度，其它与之前相同。

(16)

同样，为保证交叉操作对高权重粒子影响较小，以及解决两个小适应度个体交叉后改变不大的问题。式(14)也要进行相应的改变。

(17)

(18)

交叉后相应的适应度也要改变，如式(19)所示，

α

和

β

如式(18)所示

(19)

在变异过程中为保证较大适应度的个体也有极小的变异概率，将式(13)进行改进

(20)

迭代的结束，通过设定迭代总次数决定。在选择阶段，通过判断粒子群的有效粒子数Neff值与设定阈值T的比较，决定是否进行交叉和变异阶段。

3.2 iAGA-HybridSLAM

HybridSLAM中简单重采样造成的粒子退化是不可避免地。为了减少这种粒子退化造成的粒子多样性损失，用iAGA算法替代原来的重采样。算法伪代码如下：

3) 通过第二步的数据进行位姿和权值的更新。

4) 计算更新后的Neff值，判断是否进行重采样。若进行重采样，则通过权值得到每个粒子的适应度，以此进行选择、交叉和变异，进行优化。最终迭代出新的粒子群，进行归一化处理，完成重采样。

5) 将新的数据融入高斯分布中，计算出

K

时刻的估计值。6) 判断算法是否完成。若未完成则

K

=

K

+1，返回第2)步，继续迭代。

4 仿真结果

该仿真以MATLAB为平台，使用的是悉尼大学发布的SLAM仿真器进行仿真。仿真器界面为200m×160m设定小车移动速度为3m/s，车轮最大转角为30°，最大转速为20°，控制噪声为±0.3m和3°，观测噪声为±0.1m和1°，激光雷达观测角度和观测范围分别为360°和30m。‘+’表示设定的特征路标，‘×’表示观测特征路标的后验观测位置，‘○’为设定移动轨迹点，‘▲’为小车模型(实际和预估)。细线为小车预设路线，粗线为每个时间步对小车实际位置的预估点形成的路径。

在上述条件下，机器人分别使用HybridSLAM算法，WOC-HybridSLAM(基于权值优化组合的HybridSLAM算法)算法，AGA-HybridSLAM算法以及iAGA-HybridSLAM算法移动一圈绘制地图， 使用的粒子数设为100，根据文献[16]可知，权值优化组合中的T设置在2～12合适，因此设定T为10。AGA中设定Neff小于75时进行重采样，重采样中Neff大于阈值85时选择全部粒子否则进行交叉和变异。迭代次数为50，由文献[17]可知，

k

=

k

=0

.

5，

k

=0

.

005、

k

=0

.

05。结果如图1～4所示。

图1 HybridSLAM绘制的地图

图2 WOC-HybridSLAM绘制的地图

图3 AGA-HybridSLAM绘制的地图

图4 iAGA-HybridSLAM绘制的地图

为了判断地图精确性和定位准确性，将4种SLAM算法分别运行50次。图5为每种算法观测到的特征路标误差取平均值，绘制的曲线。图6为4种算法在每个时间步时根据小车位姿误差取平均值绘制的曲线。

图5 四种算法的特征路标误差

从图5可以看出HybridSLAM算法的特征误差最大；而AGA-HybridSLAM和WOC-HybridSLAM的误差都比HybridSLAM算法的小；iAGA-HybridSLAM检测到的特征误差最小，构建的地图最精确。这说明对AGA的改进是有作用的。

从图6中可以看出HybridSLAM算法的误差最大。虽然算法在中间时刻矫正了一些误差，但依然很大。AGA-HybridSLAM的误差相对较小；WOC-HybridSLAM的变化误差幅度略小，但整体值很大；iWOC-HybridSLAM的误差值最小，虽然在短时间内突变幅值较大，但整体中值变化不大，这说明该算法定位更准确。

图6 四种算法之间的位姿误差

为了准确的对算法精准性进行更准确的评估，需要用最小均方根RMSE来进行评估。

(21)

将每种算法每次绘制地图1次，作为1个RMSE值，计算每种算法的50个RMSE的平均RMSE值，结果如表1所示。

表1 四种算法的RMSE比较

由表1可以看出，前三个算法中HybridSLAM的两个RMSE值比WOC-HybridSLAM和AGA-HybridSLAM的都要高；而AGA-HybridSLAM在定位上比WOC-HybridSLAM要精确，在地图构建上要差一些。iWOC-HybridSLAM算法相比于WOCHybridSLAM、AGA-HybridSLAM以及HybridSLAM算法，两个平均RMSE值都要低。由此可以看出iAGA-HybridSLAM算法在地图构建和自身定位的精确性上更高。

5 结论

本文在HybridSLAM算法的基础上，添加了自适应遗传算法并根据粒子滤波的特性对其进行了改进，提出了iAGA-HybridSLAM算法。将自适应遗传算法替代原来的简单重采样，根据其进化的理念，解决了粒子退化问题，保证了粒子的多样性。根据仿真可以看出，在相同的实验条件下，iAGA-HybridSLAM能够有效减缓粒子耗散的同时，提高了地图的精度。该算法在解决粒子多样性减小的问题上提供了参考方法。