鲁棒的机器人蒙特卡洛定位算法

Key words Robot localization, particle filter, Markov chain Monte Carlo, resample
定位问题一直是机器人要解决的最关键问题之
一[1−2], 定位精度直接影响后续地图构建及路径规
划的准确度. 在未知环境中, 机器人定位的目的是
wki δ(x0:k − xi0:k)
(2)
i=1
权值 wki 的选择依据是重采样原理[11], 得出 wki ∝
p(x
i 0:k
|
z 1:k)/q(xi0:k
|
z 1:k),
其中
q(xi0:k
|
z 1:k)
为粒
子预测分布, 将式 (1) 代入可得ຫໍສະໝຸດ wki =p(z k
|
x
i k
)p(x
i k
|
xik−1)p(xi0:k−1
准确高效地确定自己的当前位置. 在过去, 扩展卡尔
曼滤波 (Extended Kalman filter, EKF) 得到广泛
应用, 但其一阶泰勒近似限制了它在高噪声环境下
的应用, 也不能有效解决滤波器失步及机器人 “绑 架” 等问题[1, 3]. 近年来, 粒子滤波器在机器人定位
领域中得到了广泛的应用, 它作为蒙特卡洛 (Monte Carlo) 方法的一种应用[4−5], 通过使用一组离散的
以有效处理非线性、非高斯系统. Thrun 和 Fox 等 人首先将粒子滤波器引入到机器人定位中, 并且提 出一种 Mixture-MCL 定位算法, 在解决室内机器 人定位问题中取得满意的效果[6]. Fox 考虑到在大 环境下定位需要大量的粒子, 发展了一种基于 KDtree 自适应调节粒子数算法[7]. Fox 和 Burgard 开 展多机器人协同定位与地图构建的研究, 提出了多 机器人协同定位算法[8−9]. Khan 等人发展了基于 Reverse-Jump MCMC 的粒子滤波算法, 用于有效 跟踪多个交互运动目标, 同时引入马尔可夫蒙链特 卡洛 (Markov chain Monte Carlo, MCMC) 重采样 方法以减少粒子匮乏效应[10]. 虽然以上方法比较成 熟, 但为了保证定位精度和滤波器有效收敛, 往往在 初始状态需要大量粒子, 如文献 [6] 使用了 100 000 粒子, 虽然文献 [7] 最终可将粒子数降为 400 左右, 但在实验中发现, 若噪声变大, 粒子滤波器很容易失 步, 造成定位出现误差. 上述现象主要源于以下两点 原因: 1) 如果单纯依靠里程计信息进行预测, 滤波 器收敛与否与里程计噪声高度相关; 2) 普通重采样 算法不具备自适应调节粒子分散度能力, 即在滤波 器有效收敛时, 使粒子细化, 分布在真实值周围; 而 在发散时, 则将粒子打散掉, 向周围扩散, 使下一步 预测更容易收敛到实际位置. 因此, 本文拟主要解决
908
自动化学报
34 卷
两个问题: 降低粒子数和自适应调节粒子分散度. 为 此, 本文安排如下: 首先阐述基于粒子滤波器机器人 定位算法的基础; 然后采用改进粒子分布预测函数, 用以减少所需粒子数; 通过结合 MCMC 方法, 提出 一种自适应调节粒子分散程度的重采样方法, 使滤 波器不仅能够减少粒子匮乏效应, 而且能够自适应 调节粒子分散度; 实验研究验证了该方法的有效性; 最后给出本文的总结.
关键词 机器人定位, 粒子滤波, 马尔可夫 – 蒙特卡洛, 重采样 中图分类号 TP242.6
Robust Robot Monte Carlo Localization
WU Er-Yong1, 2
XIANG Zhi-Yu2
LIU Ji-Lin2
Abstract A robot localization algorithm based on particle filter is presented. Firstly, in order to improve the filtering effect and decrease the number of particles needed, one parallel extended Kalman filter is used as the proposal density of particle filter, thus partial observation information can be infused into the filtering process. Secondly, in order to enhance the particles refining capacity, one improved Markov chain Monte Carlo (MCMC) resampling method with variable boundary of proposal density is put forward. Finally, the robot localization algorithm with the improved MCMC resampling is established, thus the effect of particle impoverishment can be decreased and the localization accuracy can be improved. Experiment results show that this algorithm has the advantages in computational complexity, localization accuracy and robustness.
p(z k | z 1:k−1)
|
z 1:k−1)
×
q(xik
1 | xi0:k−1, z 1:k)q(xi0:k−1
| z 1:k−1)
∝
wki −1
p(z k
|
x
i k
)p(x
i k
q(xik | xi0:k−1
|
x
i k−1
, z 1:k)
)
(3)
若取 q(xk | x0:k−1, z 1:k) = q(xk | xk−1, z k), 则权值 仅与 xk−1 和 zk 有关, 上式可简化为
wki
∝
wki −1
p(zk | x q(xik
i k
)p(x
i k
| xik−1
|
x
i k−1
,zk)
)
(4)
则被估状态的后验概率密度等于
Ns
p(x k | z 1:k) ≈
wki δ(x
k
−
x
i k
)
(5)
i=1
如果粒子总数 Ns 趋近无穷大, 则式 (5) 将近似等于 状态 xk 的真实后验概率密度. 式 (5) 同时也构成了 粒子滤波算法的基础.
K = Pˆk∇fxTPˆzk
xk
=
xˆk
+
K
(z
i k
−
zˆik )
(8)
Pk = Pˆk − K∇fxPˆk
xˆk = drawSample(xk, Pk); Pˆk = [0] (9)
其中 ∇hθ , ∇hx 分别为观测方程 h(xk, θk) 对观测 θk 和状态 xk 的雅可比矩阵. R 和 Q 分别表示观测 传感器噪声的协方差矩阵和运动控制噪声的协方差 矩阵. 当单步同时观测到多个特征时, 需将每个匹配 特征均采用式 (8) 进行一次滤波, 共相当于 N 步扩 展卡尔曼滤波, 使之接近于 p(xk | xk−1)p(zk | xk) 的分布.
1 粒子滤波器的基本原理
机器人定位的目的是为了获得当前机器人状态 xk, 但求解 xk 需递归计算式 (1).
p(xk |z 1:k)
=
p(z k |xk)p(xk |z 1:k−1) p(z k |z 1:k−1)
(1)
对 于 非 高 斯、非 线 性 系 统, 该 式 的 解 析 解 并不 存
在, 或很难求得[4]. 而粒子滤波器利用离散概率推
造成粒子滤波器难以收敛; 而当噪声非常小时, 将导
致滤波器过收敛, 形成粒子滤波器的粒子匮乏现象.
将当前观测信息融入到粒子预测分布函数中, 无疑
将有助于减少此问题的发生. 因此, 本文将分三步实
现对当前观测信息的融入: 1) 根据线性化后的高斯
运动模型及上次状态 xk−1, 预测中间状态 xˆk, 如式 (6) 所示; 2) 将此高斯状态作为一扩展卡尔曼滤波
演的方法求解式 (1). 假设用一组带权随机采样
{x
i k
,
wki
}Ni=s1
表征后验概率密度函数
p(xk
|
z 1:k),
其
中
{x
i k
,
i
=
0, · · ·
, Ns}
是 一 组 支 撑 点,
{wki ,
i
=
0, · · · , Ns} 为其对应权值, 则有[11]
Ns
p(x0:k | z 1:k) ≈
带权粒子模拟被估状态的后验概率, 并逐步通过状
态预测、更新权值和重采样等步骤完成滤波. 它最主
要的特点是不拘泥于线性系统和高斯噪声假设, 可
收稿日期 2007-03-27 收修改稿日期 2007-10-10 Received March 27, 2007; in revised form October 10, 2007 国 家 自 然 科 学 基 金 项 目 (60505017), 国 家 自 然 科 学 基 金 重 点 项 目 (60534007), 浙江省科技计划项目 (2005C14008) 资助 Supported by National Natural Science Foundation of China (60505017), Key Project of National Natural Science Foundation of China (60534007), Science Planning Project of Zhejiang Province of China (2005C14008) 1. 杭州电子科技大学计算机应用技术研究所 杭州 310018 2. 浙江 大学信息与通信工程研究所 杭州 310027 1. Institute of Computer Application Technology, Hangzhou Dianzi University, Hangzhou 310018 2. Institute of Information and Communication Engineering, Zhejiang University, Hangzhou 310027 DOI: 10.3724/SP.J.1004.2008.00907