«上一篇
文章快速检索     高级检索
下一篇»
  智能系统学报  2018, Vol. 13 Issue (5): 829-835  DOI: 10.11992/tis.201705006
0

引用本文  

伍永健, 陈跃东, 陈孟元. 量子粒子群优化下的RBPF-SLAM算法研究[J]. 智能系统学报, 2018, 13(5): 829-835. DOI: 10.11992/tis.201705006.
WU Yongjian, CHEN Yuedong, CHEN Mengyuan. Research on RBPF-SLAM algorithm based on quantum-behaved particle swarm optimization[J]. CAAI Transactions on Intelligent Systems, 2018, 13(5): 829-835. DOI: 10.11992/tis.201705006.

基金项目

2016年度安徽高校自然科学项目(KJ2016A794);2016年安徽工程大学研究生实践与创新基金项目(Y040116004).

通信作者

伍永健. E-mail: 2569513970@qq.com

作者简介

伍永健,男,1992年生,硕士研究生,主要研究方向为移动机器人路径规划;
陈跃东,男,1956年生,教授,主要研究方向为传感器信号处理、移动机器人定位及导航;
陈孟元,男,1984年生,副教授,主要研究方向为嵌入式系统开发、图像处理、传感器信息融合及优化

文章历史

收稿日期:2017-05-08
网络出版日期:2017-07-28
量子粒子群优化下的RBPF-SLAM算法研究
伍永健, 陈跃东, 陈孟元    
安徽工程大学 安徽省电气传动与控制重点实验室,安徽 芜湖 241000
摘要:为了解决传统Rao-Blackwellized粒子滤波(RBPF)存在提议分布精度不高以及重采样过程出现的粒子退化和多样性丢失问题,提出一种量子粒子群(QPSO)优化下的Rao-Blackwellized粒子滤波同时定位与地图构建(RBPF-SLAM)算法。将机器人运动模型和观测模型融合作为混合提议分布,提高提议分布的精度;在重采样过程中引入量子粒子群优化算法更新粒子位姿,根据权值划分粒子种类,引入自适应交叉变异操作,对所得粒子集进行优化、调整,有效地防止粒子退化以及保持粒子的多样性。利用本文算法不仅用MATLAB进行仿真实验,而且结合了旅行家2号移动机器人在机器人操作系统(ROS)上进行实际验证。结果表明,本文算法能以较少粒子数精确估计出机器人的位姿和高精度的地图,误差和运行时间也大大降低了。
关键词Rao-Blackwellized粒子滤波    同时定位与地图构建    提议分布    量子粒子群优化    交叉变异    移动机器人    机器人操作系统    
Research on RBPF-SLAM algorithm based on quantum-behaved particle swarm optimization
WU Yongjian, CHEN Yuedong, CHEN Mengyuan    
Anhui Key Laboratory of Electric Drive and Control, Anhui Polytechnic University, Wuhu 241000, China
Abstract: The traditional Rao-Blackwellized particle filter (RBPF) is associated with a low distribution accuracy as well as particle degeneracy and loss of diversity during resampling. To solve these problems, a combination of RBPF and simultaneous localization and mapping (RBPF-SLAM) algorithm based on quantum-behaved particle swarm optimization (QPSO) is proposed. A fusion of robot motion model and observation model is proposed as a hybrid distribution to improve accuracy. The QPSO algorithm updates the pose of particles in the resampling process according to the weight measurement of particle type, and an adaptive crossover and mutation operation is introduced to optimize and adjust the particle set to effectively prevent particle degradation and maintain particle diversity. To verify the effectiveness of the improved algorithm, a simulation experiment is performed on MATLAB, as well as a Voyager-II mobile robot in a robot operating system (ROS). The results show that the proposed algorithm can accurately estimate the position and pose of the robot and a high precision map, and error and running time are also greatly reduced.
Key words: RBPF    simultaneous localization and map building    proposed distribution    quantum-behaved particle swarm optimization    crossover and mutation    mobile robot    ROS    

地图构建作为机器人自主导航的基础,是指移动机器人在未知环境下依据自身携带的传感器信息建立地图模型[1]。常用的地图模型有栅格地图[2]、几何地图[3]以及拓扑地图[4]等。然而,地图构建问题与定位问题紧密相关,定位的结果用于地图构建,而已经构建好的地图又能实现精确定位,因此同时定位与地图构建(SLAM[5])被提出并受到广泛研究和应用。目前,SLAM技术大都基于概率理论,比如卡尔曼滤波[6]及扩展卡尔曼滤波[7]、最大似然估计[8]、粒子滤波[9]和Markov定位[10]等。文献[11]将UT和UKF运用到高斯项更新及采样粒子权重计算过程中,提出一种无迹高斯混合概率假设密度SLAM算法;文献[12]在雁群粒子群算法的基础上采用分数阶微积分和混沌思想训练模糊自适应扩展卡尔曼滤波,从而实现同时定位与地图创建;文献[13]在基本SLAM算法的迭代过程中引入元胞自动机(CA),建立“SLAM-CA生长–重定位”的闭环作用机制。

Rao-Blackwellized粒子滤波算法是目前解决SLAM问题的有效方法,它将SLAM问题分解成机器人的位姿估计和地图估计,采用粒子滤波器和扩展卡尔曼滤波器估计概率,但仍存在算法运行时间长,粒子退化严重等不足;此后很多改进的RBPF算法被提出,如文献[14]提出一种基于高斯分布的RBPF-SLAM算法,通过高斯分布分散高权重粒子获得新粒子,虽然算法能在粒子减少的条件下保持可靠估计,但忽略对低权重粒子的考虑,抑制样本匮乏现象还存在不足;文献[15]提出一种粒子群优化遗传重采样的改进RBPF-SLAM算法,采用粒子群优化策略调整粒子集,并对权重较小的粒子进行变异操作,但粒子群的引入容易陷入局部最优,加上重采样中只针对权重较小粒子操作,对缓解粒子退化无法产生满意的效果。文献[16]采用改进的提议分布并结合基于等级的自适应局部重采样(APRR)算法,设计了一种基于退火参数优化混合提议分布的RBPF算法,对高权重和低权重粒子只进行复制操作,对增加粒子多样性缓解粒子退化效果不佳。

考虑这些不足,本文从解决RBPF算法运行时间长、提议分布精度不高以及重采样过程的粒子退化出发,将量子粒子群算法[17]引入到Rao-Blackwellized粒子滤波算法,提出一种QPSO-RBPF-SLAM算法,一方面在基本提议分布中加入观测信息,使改进的提议分布更加接近真实状态,另一方面在重采样中根据QPSO算法更新粒子位姿,对高低权值粒子进行自适应交叉变异操作。QPSO-RBPF-SLAM保持了粒子的多样性,有效缓解了粒子退化,同时算法能在减少运算时间和粒子数的条件下获得可靠的估计,整体性能得到较大提高,能够精确估计出机器人的位姿并获得高精度的地图。

1 RBPF-SLAM

移动机器人SLAM实质上是一个Markov链的过程:在一个未知环境中机器人从起始位置出发,在运动过程中,使用里程计记录自身运动的信息( ${{{u}}_{1:t}} = {{{u}}_1},{{{u}}_2}, \cdots ,{{{u}}_t}$ )和外部传感器获取的环境信息( ${{{z}}_{1:t}} = {{{z}}_1},{{{z}}_2}, \cdots \!,{{{z}}_t}$ ),估计机器人的轨迹( ${{{x}}_{1:t}} = {{{x}}_1},{{{x}}_2}, \cdots\!,{{{x}}_t}$ )与构建增量式环境地图 ${{{m}}_t}$ ,同时使用创建好的地图及传感器的信息实现自定位。根据贝叶斯滤波递归原理,从概率学的角度得出SLAM的递归公式为

$\begin{aligned}\!\!\!\!\! & {\rm Bel}({{{x}}_t},{{{m}}_t}) = p({{{x}}_t},{{{m}}_t}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}}) = \eta p({{{z}}_t}\left| {{{{x}}_t},{{{m}}_t}}\right.) \cdot \\\!\!\!\!\! & \iint {p({{{x}}_t},{{{m}}_t}\left| {{{{x}}_{t - 1}},{{{m}}_{t - 1}}} \right.,{{{u}}_{t - 1}})} \cdot {\rm Bel}({{{x}}_{t - 1}},{{{m}}_{t - 1}}){\rm{d}}{{{x}}_{t - 1}}{\rm{d}}{{{m}}_{t - 1}} \\ \end{aligned} $ (1)

式中 $\eta $ 表示归一化常量。

SLAM中包含运动模型 $p({{{x}}_t}\left| {{{{x}}_{t - 1}}} \right.,{{{u}}_{t - 1}})$ 与观测模型 $p({{{z}}_t}\left| {{{{x}}_t},{{m}}} \right.)$ 两种模型。运动模型表示在给定上一时刻移动机器人轨迹 ${{{x}}_{t - 1}}$ 和控制命令 ${{{u}}_{t - 1}}$ 条件下,机器人获得新位姿 ${{{x}}_t}$ 的概率密度;而观测模型表示在给定移动机器人地图m与位姿 ${{{x}}_t}$ 的条件下,传感器获取环境的不确定性。

基于Rao-Blackwellized粒子滤波SLAM算法的思想:计算机器人轨迹 ${{{x}}_{1:t}}$ 和地图m的后验概率 $p({{{x}}_{1:t}},{{m}}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}})$ ,将其分解为式(2)所示的轨迹估计和地图估计两个后验概率乘积。

$p({{{x}}_{1:t}},{{m}}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}}) = p({{{x}}_{1:t}}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}}) \cdot p({{m}}\left| {{{{x}}_{1:t}},{{{z}}_{1:t}}} \right.)$ (2)

首先对机器人的轨迹进行估计,利用Rao-Blackwellized粒子滤波器实现,其中每个粒子代表机器人一条可能的行走轨迹。

然后再结合观测模型对地图进行更新。将地图表示为服从高斯分布的特征路标的集合,因此对地图的估计可通过特征路标估计得到,这里采用扩展卡尔曼滤波来实现。

因此,在粒子代表的轨迹上利用传感器实时观察获得的路标信息构成最后的地图。

利用Rao-Blackwellized 滤波器在传感器观测信息与里程计信息基础下构建增量式地图的步骤可以分为4步:

1) 初始化:当t=0时,选取N个粒子,每个粒子的权重为 $\omega _0^{(i)} = 1/N$

2) 采样:机器人的运动模型作为提议分布 $\pi $ ,从上一代粒子集合 $\left\{ {{{x}}_{t{\rm{ - }}1}^{(i)}} \right\}$ 采样得到下一代粒子集合 $\left\{ {{{x}}_t^{(i)}} \right\}$

3) 粒子权重:为了弥补采样时提议分布跟目标分布的差距,需要计算每一个独立粒子的权重 $\omega _t^{(i)}$ ,由重要性采样公式得出式(3):

$\omega _t^{(i)} = \frac{{p({{x}}_{1:t}^{(i)}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}})}}{{\pi ({{x}}_{1:t}^{(i)}\left| {{{{z}}_{1:t}}} \right.,{{{u}}_{1:t - 1}})}}, \quad i = 1,2,\; \cdots ,N$ (3)

4) 重采样:根据式(4)计算有效粒子数,同时设定阈值 ${N_{\rm th}}$

${N_{\rm eff}} = 1/\sum\limits_{i = 1}^N {{{(\widetilde {{\omega ^{(i)}}})}^2}} $ (4)

${N_{\rm eff}} < {N_{\rm th}}$ 时,进行重采样。重采样完成后保证所有的粒子具有相同的权重。其中, ${\widetilde {{\omega ^{(i)}}}}$ 表示归一化权重。

5) 地图更新:每一粒子用其轨迹 ${{x}}_{1:t}^{(i)}$ 和观测信息 ${{{z}}_{1:t}}$ 来计算相应的 $p({{{m}}^{(i)}}\left| {{{x}}_{1:t}^{(i)},{{{z}}_{1:t}}} \right.)$ 实现地图的更新。

2 QPSO-RBPF-SLAM

在重要性采样中,需要依据提议分布对下一代粒子集进行采样,而基本RBPF-SLAM中常采用运动模型作为提议分布,使得粒子退化严重,导致丢失权值较大的粒子从而创建的地图精度不高。为了解决提议分布精度不高的问题,结合里程计信息和外部传感观测信息作为混合提议分布如式(5)所示:

${\pi '}({{{x}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{x}}_{t - 1}^{(i)},{{{z}}_t},{{{u}}_{t - 1}}) =\displaystyle\frac{{p({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{{x}}_t}) \cdot p({{{x}}_t}\left| {{{x}}_{t - 1}^{(i)}} \right.,{{{u}}_{t - 1}})}}{{p({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{x}}_{t - 1}^{(i)},{{{u}}_{t - 1}})}} $ (5)

然而此混合提议分布无法直接进行采样操作,需要目标提议分布的一个近似化正态分布实现。式(6)所示的正态分布参数 $N\left({{u}}_t^{(i)},\sum\nolimits_t^{(i)}\right) $ 通过带权重的蒙特卡罗采样方法估计得出。

$\left\{ \begin{array}{l} {{u}}_t^{(i)} = \displaystyle\frac{1}{{{\eta ^{(i)}}}}\sum\limits_{j = 1}^M {{{{x}}_j}} \cdot p \left({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{{x}}_j}\right) \\ \sum\nolimits _t^{(i)} = \displaystyle\frac{1}{{{\eta ^{(i)}}}}\sum\limits_{j = 1}^M {p\left({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{{x}}_j}\right)} \cdot \left({{{x}}_j} - {{u}}_t^{(i)}\right){\left({{{x}}_j} - {{u}}_t^{(i)}\right)^{\rm{T}}} \end{array} \right.$ (6)

式中: ${\eta ^{(i)}} = \displaystyle\sum\limits_{j = 1}^M {p\left({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} ,\right.{{{x}}_j}\right)} $ 为归一化因子,M表示常数。

在混合提议分布下,粒子权重通过式(7)得出:

$\begin{array}{c} \omega _t^{(i)} = \omega _{t - 1}^{(i)} \cdot k\displaystyle\sum\limits_{j = 1}^M {p({{{z}}_t}\left| {{{m}}_{t - 1}^{(i)}} \right.,{{{x}}_j})} = \omega _{t - 1}^{(i)} \cdot k{\eta ^{(i)}} \end{array} $ (7)

式中k表示常数。

基本粒子群优化算法(PSO)由于粒子速度的局限性而不能在整个可行空间进行搜索,无法保证算法全局收敛,故在重采样过程中引入量子粒子群算法更新粒子集。量子粒子群算法是一种将量子系统的特点与粒子群算法相结合的新兴群体智能算法,将粒子群引入量子空间并确定粒子在空间中的位置,通过量子束缚态描述粒子的聚集性,保证了粒子在整个可行解区域的搜索,保证收敛到全局最优解。利用QPSO算法驱使粒子快速地靠近于似然函数高的区域,优化调整机器人位姿状态的粒子集 $\left\{ {{{x}}_t^{(i)}} \right\}$ ,则粒子位置更新如式(8)所示:

$ \left\{ \begin{aligned} & {{{X}}_{i,j}}(t \!+\! 1) \!=\! {{{p}}_{i,j}}(t) \pm \alpha \cdot \left| {{{\bf mbest}_j}(t) - {{{X}}_{i,j}}(t)} \right|\ln \left(\displaystyle\frac{1}{{{{{u}}_{i,j}}(t)}}\right) \\& {{{p}}_{i,j}}(t) = {f_j}(t) \cdot {{{P}}_{i,j}}(t) + \left| {1 - {f_j}(t)} \right| \cdot {{{P}}_{g,j}}(t) \\& {u_{i,j}}(t) \sim U(0,1) \\& {f_j}(t) \sim U(0,1) \\ \end{aligned} \right.$ (8)

式中: ${{{X}}_{i,j}}(t)$ ${{{X}}_{i,j}}(t{\rm{ + }}1)$ 分别表示当前时刻和下一时刻粒子的位置; ${{{p}}_{i,j}}(t)$ 表示吸引子; ${{{P}}_{i,j}}(t)$ 表示第t次迭代时第i个粒子的个体最好位置; ${{{P}}_{g,j}}(t)$ 表示第t次迭代时所有粒子中的全局最好位置; ${{\bf mbest}_j}(t)$ 表示第t次迭代时所有粒子当前最好位置的平均值; $\alpha $ 为收缩–扩张系数。

同时,为了防止粒子退化以及保持粒子的多样性,对所得的粒子集进行优化调整,基本思想是:根据权重阈值将粒子划分为高权重粒子、低权重粒子以及中等权重粒子,保留中等权重粒子,对具有高权重和低权重的粒子进行自适应交叉变异操作。根据式(9)设置合适的高权重阈值 ${\omega _{\rm H_0}}$ 和低权重阈值 ${\omega _{\rm L_0}}$ ,取两阈值之间的粒子作为中等权重的粒子。

${\omega _{\rm H_0}} = \frac{2}{N},\quad {\omega _{\rm L_0}} = \frac{1}{{2N}}$ (9)

选择 $F({{{X}}_i}) = \omega _t^{(i)}$ 作为粒子的适应度函数,则t时刻的交叉操作与变异操作如下。

交叉操作:在高权重和低权重的粒子集中随机选取两个个体作为父辈粒子进行配对,按照式(10)所示的自适应交叉率 ${P_c}$ 进行交叉操作得到两个新个体。

${P_c} = \left\{ \begin{aligned}& {P_{c1}}, \quad F' < {F_{{\rm{avg}}}} \\& {P_{c2}} + \displaystyle\frac{{{P_{c1}} - {P_{c2}}}}{{1 + \exp \left\{ {\beta \left[ {\displaystyle\frac{{2({F'} - {F_{{\rm{avg}}}})}}{{{F_{\max }} - {F_{{\rm{avg}}}}}}} \right]} \right\}}}, \quad F' \geqslant {F_{{\rm{avg}}}} \\ \end{aligned} \right.$ (10)

变异操作:从交叉后得到的新粒子集中随机选择的一个父辈粒子按照式(11)所示自适应变异率 ${P_m}$ 进行变异操作产生新粒子。

${P_m} = \left\{ \begin{aligned}& {P_{m1}}, \quad F < {F_{{\rm{avg}}}} \\& {P_{m2}} +\displaystyle \frac{{{P_{m1}} - {P_{m2}}}}{{1 + \exp \left\{ {\beta \left[ {\displaystyle\frac{{2(F - {F_{{\rm{avg}}}})}}{{{F_{\max }} - {F_{{\rm{avg}}}}}}} \right]} \right\}}}, \quad F \geqslant {F_{{\rm{avg}}}} \\ \end{aligned} \right.$ (11)

式中: ${F_{{\rm{avg}}}}$ 表示每代群体中粒子的平均适应度值; ${F_{\max }}$ 表示群体中粒子的最大适应度值; $F'$ F分别表示参与交叉操作的两个粒子中较大的适应度值以及参与变异操作的粒子的适应度值; $\beta $ 表示常数。

QPSO-RBPF-SLAM算法流程:

1) 当t=0时,选取N个粒子,每个粒子的权重为 $\omega _0^{(i)} = 1/N$ ;设置相关参数,即自适应交叉率 ${P_c}$ 以及自适应变异率 ${P_m}$

2) 根据式(5)计算混合提议分布,进行采样操作得出粒子集。

3) 根据式(8)对粒子集 $\left\{ {{{x}}_t^{(i)}} \right\}$ 进行QPSO迭代优化。

4) 根据式(7)计算粒子权重,并依据设定的高低权重阈值来划分粒子。

5) 根据式(4)重采样条件,判断是否需要进行重采样操作。若需要重采样,则执行6);否则,执行8)。

6) 保留中等权重粒子,将高权重和低权重粒子根据式(10)、式(11)进行自适应交叉和变异操作。

7) 中等权重粒子和交叉变异后的粒子组成新粒子集进行重采样,并返回3)实现QPSO重复优化。

8) 根据机器人轨迹 ${{x}}_{1:t}^{(i)}$ 和观测信息 ${{{z}}_{1:t}}$ 更新栅格地图。

3 实验 3.1 仿真实验

为了说明本文改进算法的有效性,在MATLAB平台进行了仿真实验。

首先对机器人自身位姿估计。设置机器人实际行走轨迹中真实的位姿状态,利用基本RBPF、文献[15]算法和改进RBPF在粒子数N取50和100时对真实的位姿进行估计。其中, ${P_{c1}}$ =0.8、 ${P_{c2}}$ =0.6、 ${P_{m1}}$ =0.1、 ${P_{m2}}$ =0.01。

图1表1可知,在粒子数相同的条件下,改进RBPF算法的均方根误差较小,与真实状态接近;随着粒子数的增加,虽然算法运行时间延长,但估计的结果则更加接近真实状态;与RBPF算法和文献[15]算法采用100个粒子所获得的估计结果相比,改进的RBPF算法采用50个粒子能够获得较好的估计效果,故改进的RBPF算法能利用较少的粒子获得可靠且较精确的估计。

Download:
图 1 机器人位姿估计 Fig. 1 Pose estimation of robot
表 1 3种算法的对比数据 Tab.1 Comparison data of three algorithms

其次,比较RBPF算法、文献[15]算法和改进RBPF算法下对机器人轨迹和路标的估计结果。如图2所示,设定100 cm×100 cm的区域,星形表示实际路标,红线表示实际轨迹;黑线表示利用改进RBPF算法得到的轨迹估计,圆形表示对应的路标估计;虚线表示利用文献[15]算法得到的轨迹估计,三角形表示对应的路标估计;点线表示利用RBPF算法得到的轨迹估计,黑色小点表示对应的路标估计。

Download:
图 2 机器人轨迹估计和路标估计 Fig. 2 Robot trajectory estimation and road sign estimation

图2表2可知,改进的RBPF算法在进行轨迹和路标估计时所用粒子数和运行时间比RBPF算法和文献[15]算法少。在轨迹估计方面,改进的RBPF算法得到的轨迹与机器人实际轨迹误差较小,而RBPF算法和文献[15]算法得到的轨迹波动较大;在路标估计方面,利用改进的RBPF算法得到的路标估计与实际路标较为接近,而RBPF算法和文献[15]算法得到的路标估计则在一定程度上远离实际路标。因此,与RBPF算法和文献[15]算法相比,改进的RBPF算法在机器人轨迹估计和路标估计方面能够得到更加满意的效果。

表 2 3种算法的对比数据 Tab.2 Comparison data of three algorithms

下面利用维多利亚公园数据集对RBPF算法、文献[15]算法和改进的RBPF算法的性能进一步验证。由于悉尼维多利亚公园数据集并未提供相关噪声参数的信息,故将噪声参数设置为:车辆速度控制噪声为1.0 m/s,驾驶角控制噪声为2.0°;路标观测的角度噪声为2.5°,测距噪声为1.6 m。3种算法分别采用20个粒子、15个粒子和10个粒子来描述车辆轨迹和环境地图。

RBPF算法、文献[15]算法和改进的RBPF算法的仿真结果如图3所示。其中,灰色粗线表示GPS路径(即真实路径),黑色细线表示估计路径,黑点表示估计路标。

Download:
图 3 维多利亚公园数据集仿真结果 Fig. 3 Simulation results based on Vitoria Park data

图3可知,3种算法在不同程度上估计出GPS路径,但RBPF算法采用20个粒子得到的轨迹在部分区域出现明显的不匹配现象,偏差较大;文献[15]算法采用15个粒子得到的轨迹相比RBPF算法不匹配现象减少;而改进的RBPF算法采用10个粒子得到的轨迹与GPS路径之间的误差较小,吻合度更高。同时,RBPF算法和文献[15]算法出现粒子匮乏问题而导致估计的路标个数不完全,而改进的RBPF算法能精确地估计所有设定的路标。

由上述仿真可知,RBPF算法的提议分布缺少观测信息且所有粒子都有参与重采样,算法整体计算过程简单但效果不佳,会出现粒子退化现象导致最后创建的地图精度不高;文献[15]对提议分布进行改进,引入粒子群算法更新粒子集,并对所有权重较低粒子进行重采样,计算复杂度有所提升;而改进的RBPF算法通过量子粒子群算法,只考虑粒子的位置量,且针对部分粒子进行重采样,整体计算复杂度介于RBPF算法和文献[15]算法之间,但由于改进的RBPF算法能以较少粒子数获得更好的估计结果,使得算法整体运行时间降低。整体而言,改进的RBPF算法具有更好的有效性和优越性。

3.2 实际验证

为了验证本文改进算法的实际性,在室内环境下利用旅行家2号移动机器人进行实际验证,完成同时定位与地图构建。该机器人内部有里程计,并随身携带URG-hokuyo激光传感器。在PC机上运行Liunx(Ubuntu 12.04)的ROS系统。

选取安徽工程大学电气工程学院实验室部分区域作为本次实验的室内环境。如图4所示,选取的区域为8 m×1.5 m,机器人以0.2 m/s的速度移动,利用里程计信息和激光数据信息实时构建栅格地图。

Download:
图 4 实验室环境 Fig. 4 Laboratory environment

图5表3可知,RBPF-SALM算法采用了39个粒子,粒子退化严重,降低粒子多样性,创建的地图不够精确;文献[15]算法采用了24个粒子,创建的地图有所改善,但效果不显著;改进的RBPF-SALM算法只使用了16个粒子获得了比RBPF-SALM算法和文献[15]算法更精确的地图,同时轨迹和路标估计的均方根误差、运行时间也大大缩减。

表 3 3种算法创建地图数据 Tab.3 Map data of three algorithms
Download:
图 5 Rviz实时构建地图 Fig. 5 Rviz building a map in real time
4 结束语

为解决RBPF算法中粒子退化和多样性降低问题,本文提出一种QPSO-RBPF-SLAM算法。将机器人运动模型和观测模型作为提议分布,在重采样过程中结合量子粒子群思想和遗传算法,利用QPSO算法更新粒子位姿,根据权值划分粒子种类引入自适应交叉变异操作对所得粒子集进行优化、调整。同时,在机器人ROS平台上利用旅行家2号机器人进行实验,以较少的粒子数和较短时间在精确估计机器人位姿的同时能够创建较高精度的栅格地图。下一步,在获得的高精度栅格地图的基础上对移动机器人进行路径规划研究。

参考文献
[1] 张聪聪, 王新珩, 董育宁. 基于地磁场的室内定位和地图构建[J]. 仪器仪表学报, 2015, 36(1): 181-186.
ZHANG Congcong, WANG Xinheng, DONG Yuning. Simultaneous localization and mapping based on indoor magnetic anomalies[J]. Chinese Journal of scientific instrument, 2015, 36(1): 181-186. (0)
[2] KUNDU A S, MAZUMDER O, DHAR A, et al. Occupancy grid map generation using 360° scanning xtion pro live for indoor mobile robot navigation[C]//Proceedings of 2016 IEEE First International Conference on Control, Measurement and Instrumentation. Kolkata, India, 2016: 464–468. (0)
[3] AKAI N, OZAKI K. A navigation method based on topological magnetic and geometric maps for outdoor mobile robots[C]//Proceedings of 2015 IEEE/SICE International Symposium on System Integration. Nagoya, Japan, 2015: 352–357. (0)
[4] RAMAITHITIMA R, WHITZER M, BHATTACHARYA S, et al. Automated creation of topological maps in unknown environments using a swarm of resource-constrained robots[J]. IEEE robotics and automation letters, 2016, 1(2): 746-753. DOI:10.1109/LRA.2016.2523600 (0)
[5] 戴雪梅, 郎朗, 陈孟元. 强跟踪平方根容积卡尔曼滤波SLAM算法[J]. 电子测量与仪器学报, 2015, 29(10): 1493-1499.
DAI Xuemei, LANG Lang, CHEN Mengyuan. Strong tracking square-root cubature Kalman filter based on SLAM algorithm[J]. Journal of electronic measurement and instrumentation, 2015, 29(10): 1493-1499. (0)
[6] 韩萍, 桑威林, 石庆研. 一种新型非线性卡尔曼滤波方法[J]. 仪器仪表学报, 2015, 36(3): 632-638.
HAN Ping, SANG Weilin, SHI Qingyan. Novel nonlinear Kalman filtering method[J]. Chinese Journal of scientific instrument, 2015, 36(3): 632-638. (0)
[7] 周自牧, 肖康. 基于概率密度函数塑形法的EKF和UKF优化[J]. 控制工程, 2016(S1): 40-45.
ZHOU Zimu, XIAO Kang. Optimization of EKF and UKF based on PDF shaping method[J]. Control engineering of China, 2016(S1): 40-45. (0)
[8] 王红旗, 刘勇, 罗宇锋. 带定位盲区的矿井人员全局无线定位算法[J]. 控制工程, 2015, 22(3): 505-509.
WANG Hongqi, LIU Yong, LUO Yufeng. Personnel global wireless positioning algorithm in mine with positioning blind[J]. Control engineering of China, 2015, 22(3): 505-509. (0)
[9] 李天成, 范红旗, 孙树栋. 粒子滤波理论、方法及其在多目标跟踪中的应用[J]. 自动化学报, 2015, 41(12): 1981-2002.
LI Tiancheng, FAN Hongqi, SUN Shudong. Particle filtering: theory, approach, and application for multitarget tracking[J]. Acta automatica sinica, 2015, 41(12): 1981-2002. (0)
[10] ABBASI A, JAVARI A, JALILI M, et al. Enhancing precision of Markov-based recommenders using location information[C]//Proceeings of 2014 International Conference on Advances in Computing, Communications and Informatics. New Delhi, India, 2014: 188–193. (0)
[11] 闫德立, 宋永端, 宋宇, 等. 一种改进的高斯混合概率假设密度SLAM算法[J]. 控制与决策, 2014, 29(11): 1959-1965.
YAN Deli, SONG Yongrui, SONG Yu, et al. An improved gaussian mixture PHD SLAM algorithm[J]. Control and decision, 2014, 29(11): 1959-1965. (0)
[12] 陈卫东, 刘要龙, 朱奇光, 等. 基于改进雁群PSO算法的模糊自适应扩展卡尔曼滤波的SLAM算法[J]. 物理学报, 2013, 62(17): 170506.
CHEN Weidong, LIU Yaolong, ZHU Qiguang, et al. Fuzzy adaptive extended Kalman filter SLAM algorithm based on the improved wild geese PSO algorithm[J]. Acta physica sinica, 2013, 62(17): 170506. DOI:10.7498/aps.62.170506 (0)
[13] 陈炜楠, 刘冠峰, 李俊良, 等. 室内环境的元胞自动机SLAM算法[J]. 机器人, 2016, 38(2): 169-177.
CHEN Weinan, LIU Guanfeng, LI Junliang, et al. An indoor SLAM algorithm based on cellular automata[J]. Robot, 2016, 38(2): 169-177. (0)
[14] 宋宇, 李庆玲, 康轶非, 等. 平方根容积Rao-Blackwillised粒子滤波SLAM算法[J]. 自动化学报, 2014, 40(2): 357-367.
SONG Yu, LI Qingling, KANG Yifei, et al. SLAM with square-root cubature Rao-Blackwillised particle filter[J]. Acta automatica sinica, 2014, 40(2): 357-367. (0)
[15] 林海波, 柯晶晶, 张毅. 结合粒子群寻优与遗传重采样的RBPF算法[J]. 计算机工程, 2016, 42(11): 295-299.
LIN Haibo, KE Jingjing, ZHANG Yi. Rao-Blackwellized particle filter algorithm combined particle swarm optimization and genetic Re-sampling[J]. Computer engineering, 2016, 42(11): 295-299. DOI:10.3969/j.issn.1000-3428.2016.11.050 (0)
[16] 罗元, 苏琴, 张毅, 等. 基于优化RBPF的同时定位与地图构建[J]. 华中科技大学学报: 自然科学版, 2016, 44(5): 30-34.
LUO Yuan, SU Qin, ZHANG Yi, et al. Simultaneous localization and mapping implementation based on optimized RBPF[J]. Journal of Huazhong university of science and technology: natural science edition, 2016, 44(5): 30-34. (0)
[17] 李仁府, 独孤明哲, 胡麟, 等. 基于QPSO算法移动机器人轨迹规划与实验[J]. 控制与决策, 2014, 29(12): 2151-2157.
LI Renfu, DOKGO Myong-chol, HU Lin, et al. Mobile robot trajectory planning based on QPSO algorithm and experiment[J]. Control and decision, 2014, 29(12): 2151-2157. (0)