·832· 智能系统学报 第13卷 表示参与交叉操作的两个粒子中较大的适应度 改进RBPF算法得到的轨迹估计，圆形表示对应 值以及参与变异操作的粒子的适应度值；B表示 的路标估计；虚线表示利用文献[15]算法得到的 常数。 轨迹估计，三角形表示对应的路标估计：点线表 QPSO-RBPF-SLAM算法流程： 示利用RBPF算法得到的轨迹估计，黑色小点表 1)当=0时，选取N个粒子，每个粒子的权重 示对应的路标估计。 为ω。=1N;设置相关参数，即自适应交叉率P以 一真实状态 一文献[15]估计 20 -RBPF估计 改进RBPF估计 及自适应变异率Pm。 15 2)根据式（⑤）计算混合提议分布，进行采样操 10 作得出粒子集。 3)根据式(8)对粒子集{x进行QPSO迭代 优化。 4)根据式(7)计算粒子权重，并依据设定的高 101520253035404550 低权重阈值来划分粒子。 迭代步数 5)根据式(4)重采样条件，判断是否需要进行 (a)W=50 重采样操作。若需要重采样，则执行6)：否则，执 一真实状态 一文献[15]估计 -RBPF估计 …改进RBPF估计 行8) 20 15 6)保留中等权重粒子，将高权重和低权重粒 10 子根据式(10)、式(11)进行自适应交叉和变异 操作。 7)中等权重粒子和交叉变异后的粒子组成 -10 15 新粒子集进行重采样，并返回3)实现QPSO重复 20 5 101520253035404550 优化。 迭代步数 8)根据机器人轨迹x2和观测信息z1更新栅 (b)W=100 格地图。 图1机器人位姿估计 Fig.1 Pose estimation of robot 3实验 表13种算法的对比数据 Table 1 Comparison data of three algorithms 3.1仿真实验 算法 粒子数均方根误差(RMSE) 运行时间/s 为了说明本文改进算法的有效性，在MAT LAB平台进行了仿真实验。 50 2.361 0.343 RBPF 首先对机器人自身位姿估计。设置机器人实 100 2.045 0.561 际行走轨迹中真实的位姿状态，利用基本RBPF 50 2.056 0.396 文献[15] 文献[15]算法和改进RBPF在粒子数N取50和 100 1.472 0.673 100时对真实的位姿进行估计。其中，P=0.8、 50 1.974 0.350 P2=0.6、Pm1=0.1、Pm2=0.01。 改进的RBPF 100 1.386 0.647 由图1和表1可知，在粒子数相同的条件下， 改进RBPF算法的均方根误差较小，与真实状态 实际轨迹 ·实际路标 RBPF轨迹 ·RBPF路标 接近；随着粒子数的增加，虽然算法运行时间延 100 文献[15]轨迹 ·文献5]路标 90 改进RBPF轨迹改进RBPF路标 长，但估计的结果则更加接近真实状态；与RBPF 算法和文献[15]算法采用100个粒子所获得的估 0 60 计结果相比，改进的RBPF算法采用50个粒子能 50 够获得较好的估计效果，故改进的RBP℉算法能 % 30 利用较少的粒子获得可靠且较精确的估计。 2 10 其次，比较RBPF算法、文献[15]算法和改进 RBPF算法下对机器人轨迹和路标的估计结果。 0102030405060708090100 x/cm 如图2所示，设定100cm×100cm的区域，星形表 图2机器人轨迹估计和路标估计 示实际路标，红线表示实际轨迹；黑线表示利用 Fig.2 Robot trajectory estimation and road sign estimationβ 表示参与交叉操作的两个粒子中较大的适应度 值以及参与变异操作的粒子的适应度值； 表示 常数。 QPSO-RBPF-SLAM 算法流程： ω (i) 0 = 1/N Pc Pm 1) 当 t=0 时，选取 N 个粒子，每个粒子的权重 为 ；设置相关参数，即自适应交叉率 以 及自适应变异率 。 2) 根据式 (5) 计算混合提议分布，进行采样操 作得出粒子集。 { x (i) t } 3) 根据式 (8) 对粒子集 进行 QPSO 迭代 优化。 4) 根据式 (7) 计算粒子权重，并依据设定的高 低权重阈值来划分粒子。 5) 根据式 (4) 重采样条件，判断是否需要进行 重采样操作。若需要重采样，则执行 6)；否则，执 行 8)。 6) 保留中等权重粒子，将高权重和低权重粒 子根据式 (10)、式 (11) 进行自适应交叉和变异 操作。 7) 中等权重粒子和交叉变异后的粒子组成 新粒子集进行重采样，并返回 3) 实现 QPSO 重复 优化。 x (i) 1:t 8) 根据机器人轨迹 和观测信息 z1:t更新栅 格地图。 3 实验 3.1 仿真实验 为了说明本文改进算法的有效性，在 MAT￾LAB 平台进行了仿真实验。 Pc1 Pc2 Pm1 Pm2 首先对机器人自身位姿估计。设置机器人实 际行走轨迹中真实的位姿状态，利用基本 RBPF、 文献[15]算法和改进 RBPF 在粒子数 N 取 50 和 100 时对真实的位姿进行估计。其中， =0.8、 =0.6、 =0.1、 =0.01。 由图 1 和表 1 可知，在粒子数相同的条件下， 改进 RBPF 算法的均方根误差较小，与真实状态 接近；随着粒子数的增加，虽然算法运行时间延 长，但估计的结果则更加接近真实状态；与 RBPF 算法和文献[15]算法采用 100 个粒子所获得的估 计结果相比，改进的 RBPF 算法采用 50 个粒子能 够获得较好的估计效果，故改进的 RBPF 算法能 利用较少的粒子获得可靠且较精确的估计。 其次，比较 RBPF 算法、文献[15]算法和改进 RBPF 算法下对机器人轨迹和路标的估计结果。 如图 2 所示，设定 100 cm×100 cm 的区域，星形表 示实际路标，红线表示实际轨迹；黑线表示利用 改进 RBPF 算法得到的轨迹估计，圆形表示对应 的路标估计；虚线表示利用文献[15]算法得到的 轨迹估计，三角形表示对应的路标估计；点线表 示利用 RBPF 算法得到的轨迹估计，黑色小点表 示对应的路标估计。 0 5 10 15 20 25 30 35 40 45 50 −20 −15 −10 −5 0 5 10 15 20 迭代步数 状态 真实状态 RBPF 估计 文献[15]估计 改进 RBPF 估计 真实状态 RBPF 估计 文献[15]估计 改进 RBPF 估计 (a) N=50 0 5 10 15 20 25 30 35 40 45 50 −20 −15 −10 −5 0 5 10 15 20 迭代步数 状态 (b) N=100 图 1 机器人位姿估计 Fig. 1 Pose estimation of robot 表 1 3 种算法的对比数据 Table 1 Comparison data of three algorithms 算法 粒子数 均方根误差 (RMSE) 运行时间/s RBPF 50 2.361 0.343 100 2.045 0.561 文献[15] 50 2.056 0.396 100 1.472 0.673 改进的 RBPF 50 1.974 0.350 100 1.386 0.647 0 20 30 40 50 60 70 80 90 100 10 10 20 30 40 50 60 70 80 90 100 x/cm y/cm 实际轨迹 RBPF 轨迹 文献[15]轨迹 改进 RBPF 轨迹 实际路标 RBPF 路标 文献[15]路标 改进 RBPF 路标 图 2 机器人轨迹估计和路标估计 Fig. 2 Robot trajectory estimation and road sign estimation ·832· 智 能 系 统 学 报 第 13 卷