基于粒子滤波器的移动机器人轨迹预测应用

i k i k
的关系推导出权重的更新为：
p ( Z k | X ) p ( X | X ki 1 ) i q( X ki | X 0: k 1 , Z1:k )
q( X k | X 0:k 1 , Z1:k ) q( X k | X k 1 , Z k ) X ki
（11） ，重要性分布变成只与
（ 4）
并进行了标准化
p ( Z k | Z1:k 1 ) p ( Z k | X k ) p ( X k | Z1:k 1 )dX k
这个量取决于由测量值模型（2）和已知统计 在更新步骤 （4） 中， 测量值
（ 5）
nk 确定的概率 p ( X k | Z k ) 。
Z k 用来调整先验分布， 以得到当前状态的后验分布。（3）

声， z ，
n
nn 分别是状态和过程噪声的维数。 Z {Z i , i 1,..., k} 是估计的基础。 测量值数组 1:k p ( X k | Z1:k ) ，并假设
在贝叶斯方法中，根据直到时刻 k 的各个测量值 Z，迭代计算时刻 k 的状态 X 的可能 性估计值，这样，需要建立后验密度分布（posterior density function） 它的初始值
i p( X k | Z1:k ) wk ( X k X ki ) i 1
（13）
当
N s 时，式（13）就逼近真实的后验概率。
2.2 重采样问题
粒子滤波器的常见问题是退化现象，也就是说经过若干轮滤波后，所有的粒子的权重 都为负。这是因为各粒子权重之间的相关性随时间增强，不可避免会发生粒子退化现象。退 化意味着大量的更新计算对逼近
p( X k | X k 1 ) 由系统模型（1）和已知统计 Vk 1 决定。在每个时刻 k， 测量值 Z k Z 可得，
p ( X k | Z1:k ) p ( Z k | X k ) p ( X k | Z1:k 1 ) p ( Z k | Z1:k 1 )
并用贝叶斯定律来跟新先验密度分布，
进一步的，如果
X k 1 和 Z k 有
关。这样只需要储存
i i wk wk 1
的信息，而不需要了解状态轨迹和观测值记录。权重还可以表达为：
p ( Z k | X ki ) p ( X ki | X ki 1 ) q( X ki | X ki 1 , Z k )
Ns
（12）
后验概率分布可以近似为：
粒子滤波器是基于序列蒙特卡罗方法的迭代贝叶斯滤波方法， 基本做法是由有权重点随 机采样粒子集代表后验分布， 并由此计算所需的系统估计值。 为描述算法， 由 示一个后验分布为
i {wk , i 1,..., N s ) } i 0:k i i Ns { X 0: k , wk }i 1
表
p ( X 0:k | Z1:k ) 的随机测量值，这里 { X , i 1,..., N s ) } 是权重粒子集合，
p( X 0 | Z 0 ) p( X 0 ) 是已知先验的。Z 0 是空的测量值的初始值， p ( X k | Z1:k ) 就
可以通过预测和更新进行迭代计算取得。
假设后验密度分布
p( X k 1 | Z1:k 1 ) 在 k-1 时 刻 已 知 ， 预 测 阶 段 由 系 统 模 型 通 过
2. 通用粒子滤波器
本节将以跟踪问题为例进一步介绍粒子滤波器算法。考虑目标的状态序列
{ X k , k N } ，变化方式是 X k f k ( X k 1 , Vk 1 )
其中 （1） 通常是状态
f k : R nx R nv R nx
X k 1 的非线性函数， {Vk 1 , k N } 是独立均匀分
2 x i:l
k
i xk
的第 i 个变量。 决定取样区的大小，这个值是由
有效样本数
N eff
和状态相关性
来确定的


2 i :l xk
N eff
（21）
当有效样本数
N eff
2
减小时，取样区域会扩展，包含更加分散的粒子。方差
2
是状态
的跟踪精度，大的 意味着更大的搜索范围和取样区域，反之亦然。
可以从
i i X 0: k 1 q( X 0:k 1 | Z1:k 1 ) 得 到 样 本 X 0:k q ( X 0:k | Z1:k ) 。 从 p ( X 0:k 1 | Z1:k 1 ) ，
p( X k | Z1:k )
i i wk wk 1
和
p ( X k | X k 1 )

基于粒子滤波器的移动机器人轨迹预测应用1
刘馨，裴海龙，李坚强
华南理工大学自动化学院，广州 (510640)
E-mail：Liuxin@
摘 要：对移动物体的有效预测可以帮助成功的移动控制，本文中粒子滤波器被用于预测机 器人的位置和速度并在实物实验中验证。 关键词：粒子滤波器；移动机器人
i i i wk wk 1 p ( Z k | X k )
（16）
（17）
在各种不同的重要性分布函数中，基于 果最优的是[14] 中提到
X ki 1
和
Zk
的推算，且减少粒子间相关性
*i wk
效
q( X k | X ki 1 , Z k )opt
p( Z k | X k ) p( X k | X ki 1 ) p ( Z k | X ki 1 )
p ( X k | Z1:k ) 几乎起不到作用。退化程度可以用有效粒子数
N eff
来表示：
N eff
Ns *i 1 Var ( wk )
（14）
*i wk
其中
p ( X ki | Z1:k ) ˆ q( X ki | X ki 1 , Z k ) 被当作真实权重值。 N eff 的估计值 N eff 可以这样得到
Chapman-Kolmogorov 方程计算得到 k 时刻的先验密度分布，
p ( X k | Z1:k ) p ( X k | X k 1 ) p ( X k 1 | Z1:k 1 )dX k 1
这里假定 布
。Leabharlann （3）p ( X k | X k 1 , Z1:k 1 ) p ( X k | X k 1 ) ，因为系统模型是马尔可夫过程。概率分
布的噪声序列， 估计测量值
nx ， nv 分别是状态和过程噪声的维数。 N 是自然数。 跟踪的目的是迭代 Z k hk ( X k , nk )
（2）
其中
1
hk : R nz R nn R nz
通常是非线性函数，
{nk , k N } 是独立均匀分布的测量值噪
本课题得到国家自然科学基金(60374036) (60574004)，高校博士点基金(20040561031)资助。 -1-
-2-

是自然数序列，q( )是自定的重要性分布函数。概率可以用以下表示
p( x) wi ( x x i )
i 1
Ns
（7）
wi
这里的第 i 个例子的权重
i wk i p ( X 0: k | Z1:k ) i q( X 0:k | Z1:k )
是权重，
X 0:k { X j , j 0,..., k}
是直到 k 时刻的所有状态集，权重进行
了标准化
w
i 1
N
i k
=1。这样，k 时刻的分布密度可以近似为
p( X 0:k | Z1:k )
w (X
i 1 i k
Ns
0:k
i X 0: k)
（ 6）
i
权重由以下规则来确定：假设概率 p ( x ) ( x ) ， ( x ) 是可以被评估的。让 x q ( x) ，i
-4-

1 p ( X 0 ) 17 分布的样本 X ，权重是均匀分布 N , i 1,..., N 。目标 1） 初始化：取按照初始
i 0
初始位置和速度带入预测模型。 2） 预测：每个样本粒子经过系统模型得到预测值
3.
轨迹预测
本文中，一个随机运动目标的速度和位置用粒子滤波器来预测。实际应用中，预测位
置受到物理条件限制，使用了合理的预测窗口进行预测。 粒子滤波器用作预测移动机器人的位置，在 2-D 空间中被预测的物体位置由质心位置 （x, y）表示，并从已知的过往值推算得到预测位置。x, y 分别是 2-D 坐标系的沿 x, y 轴的 坐标值。按照物理条件，物体的移动速度和范围是有限制的。预测值输出是目标下次采样的 位置速度，并作为机器人的路径规划的输入。 如图 1 中描述的，预测轨迹有四个阶段：
和（4）的迭代关系可以解决递归计算准确后验分布的问题，从而构成这种优化贝叶斯方法 的基础。这种递归计算后验分布还只是概念上的，并不能用解析法说明。优化贝叶斯方法已 经出现在一些应用里面，例如卡尔曼滤波和网格滤波法，还有次优算法，例如扩展卡尔曼滤 波，近似网格算法，粒子滤波器等近似贝叶斯方法。
2.1 粒子滤波方法
1. 引言
在机器人移动规划中轨迹预测是个重要问题，存在着不同的预测方法。本文提出了基 于粒子滤波的方法，并运用到多机器人系统中。通过预测算法，控制系统可以预测目标的位 置，速度和加速度，在此基础上完成目标截取或避免障碍物。 贝叶斯框架在系统状态估计中得到广泛应用。 在线性和高斯系统中卡尔曼滤波常用于 传感器信号融合[1] ，但卡尔曼滤波在非线性系统和非高斯系统中的作用很有限，即使扩展 卡尔曼滤波（EKF）也要求对系统状态的进行高斯后验分布线性化处理。这种近似处理对真 实结构整体存在扭曲，可能导致滤波器发散。 粒子滤波器是一种在贝叶斯框架内的在线学习的序列蒙特卡罗方法， 能够运用到各种状 态模型[2]。在粒子滤波中，目标分布由一组带有权重的样本粒子代表， 而每个权重都随时 间迭代。这类的滤波器也被称为贝叶斯步进（Bayesian bootstrap）[3]，凝聚（condensation） [4]，蒙特卡罗法（Monte Carlo）[5]，和ＭＨ权重采样法（Metropolis-Hasting）[6]。由于它 处理非线性和非高斯系统有优越性， 粒子滤波器在轨迹跟踪应用中广泛[7] [8] [9]。在[10] [11]中粒子滤波和吉布斯取样（Gibbs sampler）一起应用于跟踪多移动目标，目标群设定以 几乎固定的速度移动并具有一维可观测变量方位角。 通过粒子滤波器预测目标的状态包括加速度、速度和位置。本文中建构了基于粒子滤波器的 目标预测方法，并在实物实验中应用取得了效果。