第5期 伍永健，等：量子粒子群优化下的RBPF-SLAM算法研究 ·831· 标分布的差距，需要计算每一个独立粒子的权重 量子系统的特点与粒子群算法相结合的新兴群体 ”,由重要性采样公式得出式(3)： 智能算法，将粒子群引入量子空间并确定粒子在 w9=Pr2lz,4-） 空间中的位置，通过量子束缚态描述粒子的聚集 i=1,2,…,N (3) π(x9lz1u,u-i） 性，保证了粒子在整个可行解区域的搜索，保证 4)重采样：根据式(4)计算有效粒子数，同时 收敛到全局最优解。利用QPSO算法驱使粒子快 设定阈值Nh 速地靠近于似然函数高的区域，优化调整机器人 Na-u> 位姿状态的粒子集x,则粒子位置更新如式(8) (4) 所示： 当Nea<N时，进行重采样。重采样完成后保 X(+1)=p/()+a.mbest()-X.()In1 证所有的粒子具有相同的权重。其中，心表示归 ·化权重。 p)=ft0)P0+1-ft)·P) (8) 5)地图更新：每一粒子用其轨迹x和观 4()U0,1) ft)~U0,1) 测信息zu来计算相应的p(mox,z)实现地图的 式中：X)、Xt+1)分别表示当前时刻和下一时 更新。 刻粒子的位置；p0)表示吸引子；P0表示第1次 2 QPSO-RBPF-SLAM 迭代时第i个粒子的个体最好位置；P①表示第 1次迭代时所有粒子中的全局最好位置；mbest,() 在重要性采样中，需要依据提议分布对下一 表示第1次迭代时所有粒子当前最好位置的平均 代粒子集进行采样，而基本RBPF-SLAM中常采 值；α为收缩-扩张系数。 用运动模型作为提议分布，使得粒子退化严重， 同时，为了防止粒子退化以及保持粒子的多 导致丢失权值较大的粒子从而创建的地图精度不 样性，对所得的粒子集进行优化调整，基本思想 高。为了解决提议分布精度不高的问题，结合里 是：根据权重阈值将粒子划分为高权重粒子、低 程计信息和外部传感观测信息作为混合提议分布 权重粒子以及中等权重粒子，保留中等权重粒 如式(5)所示： 子，对具有高权重和低权重的粒子进行自适应交 txmg,2za-=pempxk2u-） 叉变异操作。根据式(9)设置合适的高权重阈值 p(zm21,x2,u-) w:,和低权重阈值L,取两阈值之间的粒子作为中 (5) 等权重的粒子。 然而此混合提议分布无法直接进行采样操 2 1 作，需要目标提议分布的一个近似化正态分布实 =N'=2N (9) 现。式(6)所示的正态分布参数(，∑)通过带 选择F(X)=作为粒子的适应度函数，则 权重的蒙特卡罗采样方法估计得出。 1时刻的交叉操作与变异操作如下。 交叉操作：在高权重和低权重的粒子集中随 =∑pkmg,s) 1 机选取两个个体作为父辈粒子进行配对，按照式(10) 所示的自适应交叉率P进行交叉操作得到两个新 个体。 (6) (Pa, F'< 式中-之62为归-化因不，M表示 Pa-Pe2 P= P2+ I2F'-Fw） F'≥Fawg(10) 1+expB 常数。 Fmax -Favg 在混合提议分布下，粒子权重通过式(7)得出： 变异操作：从交叉后得到的新粒子集中随机 选择的一个父辈粒子按照式(11)所示自适应变异 u=9k∑pzm2,x)=k (7) 率Pm进行变异操作产生新粒子。 Pm: F<Fag 式中k表示常数。 Pm -Pm2 基本粒子群优化算法(PSO)由于粒子速度的 J2(F-Faw) F≥Fawg(11) 局限性而不能在整个可行空间进行搜索，无法保 1+exp Fa3x-FaVg 证算法全局收敛，故在重采样过程中引人量子粒 式中：F表示每代群体中粒子的平均适应度值； 子群算法更新粒子集。量子粒子群算法是一种将 Fm表示群体中粒子的最大适应度值；F'、F分别ω (i) t 标分布的差距，需要计算每一个独立粒子的权重 ，由重要性采样公式得出式 (3)： ω (i) t = p(x (i) 1:t |z1:t ,u1:t−1) π(x (i) 1:t |z1:t ,u1:t−1) , i = 1,2, ··· ,N (3) Nth 4) 重采样：根据式 (4) 计算有效粒子数，同时 设定阈值 。 Neff = 1/ ∑N i=1 (ωf(i) ) 2 (4) Neff < Nth ωf(i) 当 时，进行重采样。重采样完成后保 证所有的粒子具有相同的权重。其中， 表示归 一化权重。 x (i) 1:t z1:t p(m(i)   x (i) 1:t ,z1:t ) 5) 地图更新：每一粒子用其轨迹 和 观 测信息 来计算相应的 实现地图的 更新。 2 QPSO-RBPF-SLAM 在重要性采样中，需要依据提议分布对下一 代粒子集进行采样，而基本 RBPF-SLAM 中常采 用运动模型作为提议分布，使得粒子退化严重， 导致丢失权值较大的粒子从而创建的地图精度不 高。为了解决提议分布精度不高的问题，结合里 程计信息和外部传感观测信息作为混合提议分布 如式 (5) 所示： π ′ (xt   m (i) t−1 , x (i) t−1 ,zt ,ut−1) = p(zt   m (i) t−1 , xt)· p(xt   x (i) t−1 ,ut−1) p(zt   m (i) t−1 , x (i) t−1 ,ut−1) (5) N ( u (i) t , ∑(i) t ) 然而此混合提议分布无法直接进行采样操 作，需要目标提议分布的一个近似化正态分布实 现。式 (6) 所示的正态分布参数 通过带 权重的蒙特卡罗采样方法估计得出。    u (i) t = 1 η (i) ∑M j=1 xj · p ( zt   m (i) t−1 , xj ) ∑(i) t = 1 η (i) ∑M j=1 p ( zt   m (i) t−1 , xj ) · ( xj −u (i) t ) (xj −u (i) t )T (6) η (i) = ∑M j=1 p ( zt   m (i) t−1 , xj ) 式中： 为归一化因子，M 表示 常数。 在混合提议分布下，粒子权重通过式 (7) 得出： ω (i) t = ω (i) t−1 · k ∑M j=1 p(zt   m (i) t−1 , xj) = ω (i) t−1 · kη (i) (7) 式中 k 表示常数。 基本粒子群优化算法 (PSO) 由于粒子速度的 局限性而不能在整个可行空间进行搜索，无法保 证算法全局收敛，故在重采样过程中引入量子粒 子群算法更新粒子集。量子粒子群算法是一种将 { x (i) t } 量子系统的特点与粒子群算法相结合的新兴群体 智能算法，将粒子群引入量子空间并确定粒子在 空间中的位置，通过量子束缚态描述粒子的聚集 性，保证了粒子在整个可行解区域的搜索，保证 收敛到全局最优解。利用 QPSO 算法驱使粒子快 速地靠近于似然函数高的区域，优化调整机器人 位姿状态的粒子集 ，则粒子位置更新如式 (8) 所示：    Xi, j(t+1)= pi, j(t)±α·   mbestj(t)− Xi, j(t)   ln( 1 ui, j(t) ) pi, j(t) = fj(t)· Pi, j(t)+   1− fj(t)    · Pg, j(t) ui, j(t) ∼ U(0,1) fj(t) ∼ U(0,1) (8) Xi, j(t) Xi, j(t+1) pi, j(t) Pi, j(t) Pg, j(t) mbestj(t) α 式中： 、 分别表示当前时刻和下一时 刻粒子的位置； 表示吸引子； 表示第 t 次 迭代时第 i 个粒子的个体最好位置； 表示第 t 次迭代时所有粒子中的全局最好位置； 表示第 t 次迭代时所有粒子当前最好位置的平均 值; 为收缩–扩张系数。 ωH0 ωL0 同时，为了防止粒子退化以及保持粒子的多 样性，对所得的粒子集进行优化调整，基本思想 是：根据权重阈值将粒子划分为高权重粒子、低 权重粒子以及中等权重粒子，保留中等权重粒 子，对具有高权重和低权重的粒子进行自适应交 叉变异操作。根据式 (9) 设置合适的高权重阈值 和低权重阈值 ，取两阈值之间的粒子作为中 等权重的粒子。 ωH0 = 2 N , ωL0 = 1 2N (9) F(Xi) = ω (i) 选择 t 作为粒子的适应度函数，则 t 时刻的交叉操作与变异操作如下。 Pc 交叉操作：在高权重和低权重的粒子集中随 机选取两个个体作为父辈粒子进行配对，按照式 (10) 所示的自适应交叉率 进行交叉操作得到两个新 个体。 Pc =    Pc1 , F ′ < Favg Pc2 + Pc1 − Pc2 1+exp{ β [ 2(F ′ − Favg) Fmax − Favg ]}, F ′ ⩾ Favg (10) Pm 变异操作：从交叉后得到的新粒子集中随机 选择的一个父辈粒子按照式 (11) 所示自适应变异 率 进行变异操作产生新粒子。 Pm =    Pm1, F < Favg Pm2 + Pm1 − Pm2 1+exp{ β [ 2(F − Favg) Fmax − Favg ]}, F ⩾ Favg (11) Favg Fmax F ′ 式中： 表示每代群体中粒子的平均适应度值； 表示群体中粒子的最大适应度值； 、F 分别 第 5 期 伍永健，等：量子粒子群优化下的 RBPF-SLAM 算法研究 ·831·