SIEMENS四川省德阳市（授权）西门子代理商——西门子西南总代理

粒子滤波估计位置

基于混合滤波的位姿信息融合模型包含多种滤波器对测量数据的处理操作, 下面将介绍如何通过卡尔曼滤波算法进行姿态估计、粒子滤波算法进行位置估计。由于这个模型需要考虑机器人末端位置与姿态之间存在的时空关联性, 因此, 在进行状态估计过程中, 需要考虑状态估计的依赖关系以及如何进行数 据融合操作。通过粒子滤波算法进行位置估计需要依赖于卡尔曼滤波器估计姿态, 在这里主要讨论如何使用粒子滤波算法估计机器人末端位置以及如何进行混合滤波的位姿信息融合。信息融合流程图如图 4-14 所示, 从中可看出卡尔曼滤波算法和粒子滤波算法存在时空关联性。为了实现粒子滤波算法进行状态估计, 定义在  时刻的后验密度为:  (4-104)但在实际的过程中, 对后验密度的取样比较困难, 因此更多的是使用重要性取样的方法来进行取样。重要性取样方法如下: 当任意粒子的目标密度取样困难时, 样本可以通过 已知的标准概率密度  进行求解。首先对所有的粒子进行校正来平衡目标密度和重要密度之间的差异, 因此定义一个权重来表示两个密度之间的比例。直至  时刻的离散后验密度为:  (4-105)目标密度  和重要性密度  与标准化权重有如下关系：  (4-106)其标准化的权重为:  在这种情况下，使用粒子滤波有一个问题：系统中有一个粒子的权重非常 高, 而剩余的粒子权重往往小到可以忽略不计, 这种现象被称为退化。为了防止这样的现象发生, 可以根据粒子的重量来决定权重。这样, 进行重采样后可以增加更多有高权重的样本数据, 减少了低权重样本的数量。重采样后, 所有的粒子具有相同的权重。因此, 在时间  内它们的权重相等  。这 样, 式(4-107) 可以写成:  (4-108)在经过采样时间  后, 式(4-104)可以表示为:  (4-109)为了使用粒子滤波算法估计机器人末端的位置, 定义机器人末端的位置状 态为:  (4-110)可以根据位置粒子的计算值来决定粒子的权重。然而, 在位置测量和计算过程中, 每个粒子状态的差值并不完全地取决于粒子的状态, 还取决于传感器的噪声和误差。因此在实际应用中, 一般不使用粒子的即时差值, 而是使用一段时间  , 其中下标是迭代次数,  位置的累积差值来更新每个粒子的权重。被估算出的第  个粒子的累积位置差值可以用于似然计算：  其中,   为在  时刻的位置状态;  为经过粒子滤波算法处理后 的状态;  为机器人末端在  时刻的速度。对于给定的粒子, 计算出来的  越低, 其概率就越高, 反之亦然。当累积误差Zui小时, 计算出的粒子在正确位置上的可能性Zui高。定义  表示Zui可能的位置值, 由于权值的计算需要  的值, 所以可以将标准化的权值表示为:  (4-112) 3. 粒子滤波估计姿态
上节介绍了如何使用卡尔曼滤波算法估计机器人末端的姿态, 粒子滤波算 法估计位置的混合滤波位姿信息融合模型。反之亦然, 使用卡尔曼滤波算法也可以估算机器人末端的位置, 使用粒子滤波算法也能够估计机器人末端的姿态。这两种滤波在信息融合过程中需要考虑时空关联性, 这里, 使用卡尔曼滤波进行位置估计需要依赖于使用粒子滤波估计位置, 它们之间的信息融合流程图如图 4-14 所示。类似粒子滤波算法估计位置的流程, 定义粒子滤波算法在  时刻的后验密 度为:  (4-113)同样, 使用重要性取样方法, 将粒子滤波算法在  时刻的离散后验密度近 似表示为:  (4-114)重要性密度  和目标密度  与标准化权重的关系为:   (4-115)所以,  (4-116)重要性密度应该选择可以被推导为以下递归方式的函数: 为了满足式(4-117)的条件, 重要性密度函数可以表示为:  式(4-117)可以写成:  为了避免退化问题, 可采用重采样方法。重采样后, 所有的粒子的权重都 是一样的。因此, 在  时刻权重为  。式(4-119)可写成:  在经过采样时间  后, 式(4-113)可表示为: ） 在实际情况中, 由于传感器以及姿态状态存在一定误差, 因此计算所得出的 数据与真正的所测量出的数据往往也会有一定的偏差, 所以姿态的偏差越高的粒子往往和实际的情况更加相似。为了避免这种情况, 可以模仿上述粒子滤波用来 估计位置的方法, 使用一段时间内 (  , 其中下标是迭代次数,   的姿态差之和来表示权重, 而不是通过在  时刻的瞬间姿态差来计算权重。被估计和计算出的第  个粒子的累积姿态差值可以用于似然计算: ） 其中,   代表在  时刻的第  个粒子的状态。   越小表示正确 的方向上的可能性越高。对于每个  , 每个粒子的权重需要重新计算。因此,后验估算值在近似时间  可以表示为:  则在  时刻的后验概率为:  第  次循环迭代的单位化权重可以写成:  根据在前验概率  中重采样和选择重要的密度函数, 单位化权重可表示为:  (4-126)姿态粒子权重的计算是根据  的Zui可能值。  代表   Zui有可 能的值。因为正确姿态的可能性Zui高是由Zui小的姿态误差所得, 因此有:  (4-127)其中,   是对  求标准偏差运算。由于  包含  将近  的值, 所以假设小于  平均值的Zui小值为  。需要注意的一点是,   需要根据传感器的精度来决定, 如果  选择的值太小, 那么姿态上的误差 量可能会因为其值不够大而无法zuihao地识别姿态粒子; 如果  , 选择的值过大, 姿态上的误差量会因为在先验概率中的增大而影响姿态估计的精度。 

4 本章小结

本章主要阐述机器人的三种在线误差测量模型：基于激光跟踪的在线误差 测量模型、基于视觉的在线误差测量模型和基于位姿传感器的在线误差测量模 型。在基于激光跟踪的在线误差测量中, 在机器人末端绑定一个激光发射器, 它发射出的激光映射到固定的挡板上, 从而可以推导出实际与真实的激光位置之间的偏差, 即机器人结构参数引起的偏差。在基于视觉的在线误差测量模型 中, 详细介绍了整个误差测量模型的构建流程, 模型的设计以及如何对摄像机 和手眼相机进行标定。在模型实现中, 在机器人末端绑定一个手眼相机观察机 器人基座的位置, 通过观察和实际的基座位置对比来测量机器人结构参数的误差。在基于位姿传感器的在线误差测量模型中, 主要介绍如何通过混合传感器 和混合滤波算法对机器人末端的位置和姿态进行实时估计, 应用多种滤波算法 来预测机器人运动的状态, 从而获取机器人的实际状态, 通过和真实值对比即可获得机器人结构参数的误差。本章讨论的三种误差测量模型已经通过实验验证, 这三种误差测量模型能 够很好地解决机器人结构参数引起的偏差问题。