鲁棒的机器人蒙特卡洛定位算法
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
第 34 卷 第 8 期 2008 年 8 月
自动化学报 ACTA AUTOMATICA SINICA
Vol. 34, No. 8 August, 2008
鲁棒的机器人蒙特卡洛定位算法
武二永 1, 2 项志宇 2 刘济林 2
摘 要 提出一种基于粒子滤波器的机器人定位算法. 首先利用一并行扩展卡尔曼滤波器作为粒子预测分布, 将当前观测 的部分信息融入, 以改善滤波效果, 减小所需粒子数; 然后提出变密度函数边界的马尔可夫链蒙特卡洛 (Markov chain Monte Carlo, MCMC) 重采样方法, 以提高粒子的细化能力; 最后结合普通重采样方法, 提出一种改进的 MCMC 重采样的机器人定 位算法, 减少粒子匮乏效应的同时, 提高了定位精度. 实验结果表明, 该算法较传统方法在计算复杂度、定位精度和鲁棒性方面 都有显著提高.
准确高效地确定自己的当前位置. 在过去, 扩展卡尔
曼滤波 (Extended Kalman filter, EKF) 得到广泛
应用, 但其一阶泰勒近似限制了它在高噪声环境下
的应用, 也不能有效解决滤波器失步及机器人 “绑 架” 等问题[1, 3]. 近年来, 粒子滤波器在机器人定位
领域中得到了广泛的应用, 它作为蒙特卡洛 (Monte Carlo) 方法的一种应用[4−5], 通过使用一组离散的
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) 同时也构成了 粒子滤波算法的基础.
908
自动化学报
34 卷
两个问题: 降低粒子数和自适应调节粒子分散度. 为 此, 本文安排如下: 首先阐述基于粒子滤波器机器人 定位算法的基础; 然后采用改进粒子分布预测函数, 用以减少所需粒子数; 通过结合 MCMC 方法, 提出 一种自适应调节粒子分散程度的重采样方法, 使滤 波器不仅能够减少粒子匮乏效应, 而且能够自适应 调节粒子分散度; 实验研究验证了该方法的有效性; 最后给出本文的总结.
演的方法求解式 (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) ≈
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) 的分布.
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
以有效处理非线性、非高斯系统. 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) 普通重采样 算法不具备自适应调节粒子分散度能力, 即在滤波 器有效收敛时, 使粒子细化, 分布在真实值周围; 而 在发散时, 则将粒子打散掉, 向周围扩散, 使下一步 预测更容易收敛到实际位置. 因此, 本文拟主要解决
Key words Robot localization, particle filter, Markov chain Monte Carlo, resample
定位问题一直是机器人要解决的最关键问题之
一[1−2], 定位精度直接影响后续地图构建及路径规
划的准确度. 在未知环境中, 机器人定位的目的是
带权粒子模拟被估状态的后验概率, 并逐步通过状
态预测、更新权值和重采样等步骤完成滤波. 它最主
要的特点是不拘泥于线性系统和高斯噪声假设, 可
收稿日期 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
2 关键技术
2.1 改进的粒子预测分布
蒙特卡洛定位的目标是利用粒子滤波器计算
当前机器人的位置. 对于二维空间, 为二维笛卡儿
位置坐标及航向角, 用 (x, y, θ) 表示. 当进行预测
时, 常规方法令粒子预测分布 q(xk | xk−1, zk) 等于 p(xk | xk−1), 即通过系统函数 xˆk = fk(xk−1, uk) 演 化, 而忽略当前观测 zk 的影响. 但当运动模型噪声 比较大时, 真实的粒子预测分布将与系统函数失配,
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 有关, 上式可简化为
造成粒子滤波器难以收敛; 而当噪声非常小时, 将导
致滤波器过收敛, 形成粒子滤波器的粒子匮乏现象.
将当前观测信息融入到粒子预测分布函数中, 无疑
将有助于减少此问题的发生. 因此, 本文将分三步实
现对当前观测信息的融入: 1) 根据线性化后的高斯
运动模型及上次状态 xk−1, 预测中间状态 xˆk, 如式 (6) 所示; 2) 将此高斯状态作为一扩展卡尔曼滤波
器的初始预测状态,
对于每个观测特征
z
i k
,
利用扩展
卡尔曼滤波器更新, 如式 (7) 和 (8) 所示; 3) 根据最
终得到的概率后验分布获取新的预测粒子, 如式 (9)
所示.
xˆk = f (xk−1, uk)
Pˆk = ∇fxPˆk−1∇fxT + Q
(6)
zˆik
=
h(xˆk
,
θ
i k
)
Pˆzk ∼= ∇hxPˆk∇hxT + ∇hθ Pˆf ∇hθT + R (7)
关键词 机器人定位, 粒子滤波, 马尔可夫 – 蒙特卡洛, 重采样 中图分类号 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.
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]. 而粒子滤波器利用离散概率推
自动化学报 ACTA AUTOMATICA SINICA
Vol. 34, No. 8 August, 2008
鲁棒的机器人蒙特卡洛定位算法
武二永 1, 2 项志宇 2 刘济林 2
摘 要 提出一种基于粒子滤波器的机器人定位算法. 首先利用一并行扩展卡尔曼滤波器作为粒子预测分布, 将当前观测 的部分信息融入, 以改善滤波效果, 减小所需粒子数; 然后提出变密度函数边界的马尔可夫链蒙特卡洛 (Markov chain Monte Carlo, MCMC) 重采样方法, 以提高粒子的细化能力; 最后结合普通重采样方法, 提出一种改进的 MCMC 重采样的机器人定 位算法, 减少粒子匮乏效应的同时, 提高了定位精度. 实验结果表明, 该算法较传统方法在计算复杂度、定位精度和鲁棒性方面 都有显著提高.
准确高效地确定自己的当前位置. 在过去, 扩展卡尔
曼滤波 (Extended Kalman filter, EKF) 得到广泛
应用, 但其一阶泰勒近似限制了它在高噪声环境下
的应用, 也不能有效解决滤波器失步及机器人 “绑 架” 等问题[1, 3]. 近年来, 粒子滤波器在机器人定位
领域中得到了广泛的应用, 它作为蒙特卡洛 (Monte Carlo) 方法的一种应用[4−5], 通过使用一组离散的
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) 同时也构成了 粒子滤波算法的基础.
908
自动化学报
34 卷
两个问题: 降低粒子数和自适应调节粒子分散度. 为 此, 本文安排如下: 首先阐述基于粒子滤波器机器人 定位算法的基础; 然后采用改进粒子分布预测函数, 用以减少所需粒子数; 通过结合 MCMC 方法, 提出 一种自适应调节粒子分散程度的重采样方法, 使滤 波器不仅能够减少粒子匮乏效应, 而且能够自适应 调节粒子分散度; 实验研究验证了该方法的有效性; 最后给出本文的总结.
演的方法求解式 (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) ≈
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) 的分布.
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
以有效处理非线性、非高斯系统. 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) 普通重采样 算法不具备自适应调节粒子分散度能力, 即在滤波 器有效收敛时, 使粒子细化, 分布在真实值周围; 而 在发散时, 则将粒子打散掉, 向周围扩散, 使下一步 预测更容易收敛到实际位置. 因此, 本文拟主要解决
Key words Robot localization, particle filter, Markov chain Monte Carlo, resample
定位问题一直是机器人要解决的最关键问题之
一[1−2], 定位精度直接影响后续地图构建及路径规
划的准确度. 在未知环境中, 机器人定位的目的是
带权粒子模拟被估状态的后验概率, 并逐步通过状
态预测、更新权值和重采样等步骤完成滤波. 它最主
要的特点是不拘泥于线性系统和高斯噪声假设, 可
收稿日期 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
2 关键技术
2.1 改进的粒子预测分布
蒙特卡洛定位的目标是利用粒子滤波器计算
当前机器人的位置. 对于二维空间, 为二维笛卡儿
位置坐标及航向角, 用 (x, y, θ) 表示. 当进行预测
时, 常规方法令粒子预测分布 q(xk | xk−1, zk) 等于 p(xk | xk−1), 即通过系统函数 xˆk = fk(xk−1, uk) 演 化, 而忽略当前观测 zk 的影响. 但当运动模型噪声 比较大时, 真实的粒子预测分布将与系统函数失配,
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 有关, 上式可简化为
造成粒子滤波器难以收敛; 而当噪声非常小时, 将导
致滤波器过收敛, 形成粒子滤波器的粒子匮乏现象.
将当前观测信息融入到粒子预测分布函数中, 无疑
将有助于减少此问题的发生. 因此, 本文将分三步实
现对当前观测信息的融入: 1) 根据线性化后的高斯
运动模型及上次状态 xk−1, 预测中间状态 xˆk, 如式 (6) 所示; 2) 将此高斯状态作为一扩展卡尔曼滤波
器的初始预测状态,
对于每个观测特征
z
i k
,
利用扩展
卡尔曼滤波器更新, 如式 (7) 和 (8) 所示; 3) 根据最
终得到的概率后验分布获取新的预测粒子, 如式 (9)
所示.
xˆk = f (xk−1, uk)
Pˆk = ∇fxPˆk−1∇fxT + Q
(6)
zˆik
=
h(xˆk
,
θ
i k
)
Pˆzk ∼= ∇hxPˆk∇hxT + ∇hθ Pˆf ∇hθT + R (7)
关键词 机器人定位, 粒子滤波, 马尔可夫 – 蒙特卡洛, 重采样 中图分类号 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.
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]. 而粒子滤波器利用离散概率推