移动机器人的改进无迹粒子滤波蒙特卡罗定位算法

2.1 改进无迹粒子滤波
在重要性函数 q (x k |x k−1 , z k , u k−1 ) 的设计中融 入当前环境观测信息 z k , 使采样粒子集中地分布于 高观测似然区域是减低粒子集退化、提高粒子滤波 器性能的有效途径[8] . 本文利用迭代 Sigma 点卡尔 曼滤波实现提议分布设计中的观测信息融合, 提出 IUPF 算法, 其重要性函数表达为高斯分布
[m]
[m]
(4)
粒子 x k
[m]
的重要性权重 ωk
计算为
ωk
[m]
= ωk
[m] [m] [m−1] , u k−1 ) [m−1] p(z k |x k )p(x k |x k [m] [m] q (x k |x k−1 , z k , u k−1 )
(5)
1 移动机器人蒙特卡罗定位
1.1 贝叶斯信度传播
数 q (x k |x k−1 , z k , u k−1 ) 中采集的索引为 m 的粒子. [m] ωk 为该粒子的非负重要性权重, 满足归一化条件 N [m] = 1. N 为粒子集包括的粒子总数. 则后 m=1 ωk 验信度估计
N
Bel(x k ) ≈
m=1
ωk δ (x k − x k )
[m]
由式 (2) 和式 (3) 知, 贝叶斯信度传播由信度预 测和信度更新两个步骤组成, 其中涉及到三个重要 的概率密度模型: p(x k |x k−1 , u k−1 ) 用于描述系统动 态, 称为系统模型; p(z k |x k ) 称为感知模型或观测似 然; Bel(x 0 ) 为状态的初始信度分布.
第 36 卷 第 6 期
2010 年 6 月
自 动 化 学 报
ACTA AUTOMATICA SINICA
Vol. 36, No. 6 June, 2010
移动机器人的改进无迹粒子滤波蒙特卡罗定位算法
宋 宇 1, 2 孙富春 1 李庆玲 3
摘 要 粒子滤波是移动机器人蒙特卡罗定位 (Monte Carlo localization, MCL) 的核心环节. 首先, 针对粒子滤波过程的 粒子退化问题, 利用迭代 Sigma 点卡尔曼滤波来精确设计粒子滤波器的提议分布, 以迭代更新方式将当前观测信息融入顺序 重要性采样过程, 提出 IUPF (Improved unscented particle filter) 算法. 然后, 将 IUPF 与移动机器人 MCL 相结合, 给出 IUPF-MCL 定位算法的实现细节. 仿真结果表明, IUPF-MCL 是一种精确鲁棒的移动机器人定位算法. 关键词
贝叶斯信度传播根据传感器观测数据和系统动 态模型来递归估计动态系统状态的后验信度分布 Bel(x k ) = p(x k |z 0:k , u 1:k−1 ). 其中, x k 为时刻 k 的 动态系统状态. z 0:k 表示历史环境观测数据. u 1:k−1 表示历史机器人控制数据. 根据贝叶斯规则有
收稿日期 2009-01-04 录用日期 2009-11-12 Manuscript received January 4, 2009; accepted November 12, 2009 国家高科术研究发展计划 (863 计划) (2007AA04Z232), 国家自然科 学基金 (60909055, 90820304), 机器人技术与系统国家重点实验室开 放研究项目 (SKLRS-2009-ZD-04), 中国博士后科学基金 (20080440 382) 资助 Supported by National High Technology Research and Development Program of China (863 Program) (2007AA04Z232), National Natural Science Foundation of China (60909055, 90820304), State Key Laboratory of Robotics and System (SKLRS-2009-ZD-04), and Postdoctoral Science Foundation of China (20080440382) 1. 清华大学智能技术与系统国家重点实验室 北京 100084 2. 哈尔 滨工业大学机器人技术与系统国家重点实验室 哈尔滨 150080 3. 中 国科学院自动化研究所 北京 100190 1. State Key Laboratory of Intelligent Technology and System, Tsinghua University, Beijing 100084 2. State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150080 3. Institute of Automation, Chinese Academy of Sciences, Beijing 100190
q (x k |x k−1 , z k , u k−1 ) = N (x k , Pk )
x k = f (x k−1 , u k−1 + δ uk−1 ) z k = h(x k , n) + δ z k
(7)
(2) 结合马尔可夫观测独立性假设, 则动态系统状 态的信度更新过程表示为 Bel(x k ) ∝ p(z k |x k )Bel(x k )+ (3)
其中, x k = [xk , yk , φk ]T 为移动机器人的二维笛卡 尔坐标和航向角构成的状态向量. z k = [ρk , θk ]T 为 环境特征 n 在移动机器人传感器坐标系内的极坐标 观测向量. f (·) 和 h(·) 为非线性系统函数与观测函 数. u k−1 为在时间段 [k − 1, k ] 内施加的控制作用. δ uk−1 , δ z k 分别为观测噪声和控制噪声
报
36 卷
相似性的二次更新算法[11] , 增强了定位算法的鲁棒 性; 武二永等将扩展卡尔曼滤波用于设计粒子滤波 器的提议分布, 并结合变密度边界 MCMC 实现了 MCL 中粒子集的优化与细化[12] . 在简要回顾粒子滤波技术的基础上, 提出基于 改进无迹粒子滤波 (Improved unscented particle filter, IUPF) 的移动机器人蒙特卡罗定位算法: 首 先, 针对粒子集退化问题, 利用统计学迭代 Sigma 点卡尔曼滤波来精确设计粒子滤波器的重要性函数, 提出了 IUPF 算法; 进而, 将 IUPF 引入移动机器人 定位问题, 给出 IUPF-MCL 定位算法并通过仿真实 验验证其性能; 最后, 给出全文总结.
蒙 特 卡 罗 定 位 (Monte Carlo localization, MCL) 作为粒子滤波[1] 在移动机器人领域的成功应 用, 最早由 Dellaertt 等在发表于 1999 年的 IEEE ICRA 大会的论文上提出[2] . 理论上, 粒子滤波期 望通过对系统状态的后验概率密度函数直接进行蒙 特卡罗随机采样来逼近真实状态的概率分布, 因而 适用于任意系统动态、任意噪声分布系统的后验信 度估计. 但基于粒子滤波的 MCL 仍然存在一些理 论缺陷: 一方面, MCL 通过从先验分布中采样来 代替从后验分布中采样, 并利用观测似然评估采样
重要性, 由于先验粒子在传播时没有考虑当前观测 信息, 预测粒子集有可能分布于观测似然函数的尾 部, 导致粒子集退化; 再者, 如激光等精确的环境 观测传感器会导致观测似然函数的形状尖锐, 导致 滤波器采集不到有效粒子. 为解决 MCL 的上述问 题, 学者们作了大量工作, 代表性的有: Thrun 等 提出的 Mixture-MCL 算法[3] , 在解决室内移动机 器人定位的问题上取得了满意的成果; Fox 提出了 基于 Kullback-Leiber 距离采样的自适应粒子密度 算法[4] , 该方法采用描述概率分布差异的 KLD 来 衡量真实后验概率分布与粒子集分布之间的误差, 实现在线调整粒子数目; Lenser 等提出了 Sensor resetting localization 算法[5] 来解决 RoboCup 机 器人比赛时相互碰撞导致的机器人绑架问题, 并由 Ueda 对该方法进行了扩展[6] ; Khan 等将马尔可夫 蒙特卡罗采样引入粒子滤波, 解决了粒子滤波在高 维空间中采样效率低下的问题, 并一定程度上避免 了粒子集的退化[7] , 实现了对多交互目标的视觉跟 踪; van der Merwe 等将无迹卡尔曼滤波引入粒子 滤波器的提议分布设计, 提出了 Unscented particle filter (UPF) 算法, 使粒子更加集中于高观测似然区 域[8] ; Wu 等将随机噪声转化为模糊噪声, 提出了模 糊粒子滤波器[9] ; Pfaff 等通过平滑观测斯然函数使 得有效粒子增加[10] ; 方正等提出几何相似性和距离
MCL 中采用的重要性函数为移动机器人的系 统动态, 即
[m] [m−1] q (x k |x k−1 , z k , u k−1 ) = p(x k |x k , u k−1 )
(6)
2 IUPF-MCL 算法
定义移动机器人系统的含噪声运动学方程和观 测方程为
Bel(x k )∝ p(z k |x k , u 1:k−1 , z 0:k−1 )p(x k |u 1:k−1 , z 0:k−1 ) (1) 其 中, p(x k |u 1:k−1 , z 0:k−1 ) 为 信 度 预 测, 记 为 Bel(x k )+ , 结合一阶马尔可夫过程假设, 有 Bel(x k )+ = p(x k |x k−1 , u k−1 )Bel(x k−1 )dx k−1
DOI
移动机器人, 蒙特卡罗定位, 粒子滤波, 无迹卡尔曼滤波
10.3724/SP.J.1004.2010.00851