腿足机器人之五- 粒子滤波

腿足机器人之五粒子滤波

上一篇博客使用的是高斯分布结合贝叶斯准则来估计机器人状态,本篇是基于直方图和粒子滤波器这两种无参滤波器估计机器人状态。

直方图方法将状态空间分解成有限多个区域,并用直方图表示后验概率。直方图为每个区域分配一个单独的累积概率;可以将其视为对连续密度函数的逐段常数近似。第二种技术通过有限多个样本来表示后验概率。由此产生的滤波器被称为粒子滤波器,在某些机器人问题中获得了极大的流行。

直方图滤波器和粒子滤波器,都不对后验密度做强烈的参数假设。因而非常适合表示复杂的多模态信念(后验状态概率估计)。因此,当机器人需要应对全局不确定性的阶段,以及面对产生多个独立假设的困难数据关联问题时,它们通常是首选方法。

直方图滤波

直方图滤波器的离散形式称为离散贝叶斯滤波器,其算法流程如下:

其中 u t u_t ut是控制信息, z t z_t zt是传感器观测值, { P k , t − 1 } \{P_{k,t-1}\} {Pk,t−1},其中的大括号表示这个概率是一个集合, t − 1 t-1 t−1表示的是时刻,k表示的是状态 x x x处在k时, p k , t − 1 p_{k,{t-1}} pk,t−1表示t-1时刻,机器人处于状态k的概率,然后大扩号表示这是要遍历机器人所有可能状态的集合。

第3行根据t时刻控制信息 u t u_t ut和前一时刻状态 x i x_i xi估计t时刻处于状态k的概率 p ‾ k , t \overline p_{k,t} pk,t,这是一个先验概率,第4行根据传感器观测值修正先验概率,得到状态的后验估计 p k , t p_{k,t} pk,t

上面的离散的可能状态如何求得?分为静态和动态法,静态分解技术依赖于一个预先选定的固定分解方案,这种方案在分解过程开始前就已经确定,且不考虑被近似的后验概率分布的具体形状。动态分解技术会根据后验分布的具体形状来适应性地调整分解结构。这类技术能更有效地针对状态空间中后验概率较大的区域提供更高的分辨率,而在概率较低的区域采用更粗糙的分解,从而节省计算资源并提高效率。递归地分解状态空间并调整分解的精细度的密度树就是一种动态法。

粒子滤波

在粒子滤波方法中,后验概率是一些离散的点,称为粒子(particles),其可以表示为:
X t : = x t [ 1 ] , x t [ 2 ] , ⋯   , x t [ M ] \mathcal X_t := x_t^{[1]}, x_t^{[2]}, \cdots, x_t^{[M]} Xt:=xt[1],xt[2],⋯,xt[M]

每一个粒子 x t [ m ] x_t^{[m]} xt[m]都是t时刻状态的一个可能分布。通常粒子的状态总数M是一个很大的数,如M=1000,在一些实现中M也是时间或者和后验概率有联系的其他变量的函数。

粒子滤波器的核心思想是用一组粒子 ( X t ) ( {\mathcal X_t} ) (Xt)近似后验概率分布 ( bel ( x t ) ) ( \text{bel}(x_t) ) (bel(xt))。理想情况下,某个状态假设 ( x t ) ( x_t ) (xt) 被包含在粒子集 ( X t ) ( \mathcal X_t ) (Xt) 中的概率应与其在贝叶斯滤波后验中的概率成正比:
x t [ m ] ∼ p ( x t ∣ z 1 : t , u 1 : t ) x_t^{[m]}∼p(x_t∣z_{1:t},u_{1:t}) xt[m]∼p(xt∣z1:t,u1:t)

粒子滤波的算法流程如下:

第2行~6行创建一个临时的粒子集 ( X t ‾ ) (\overline {\mathcal X_t}) (Xt),这个集合中的粒子是从先前粒子集 X t − 1 \mathcal X_{t-1} Xt−1对称处理得到。

第3行要遍历M个粒子;

第4行采样,即对于每一个粒子 ( x t [ m ] ) ( x_t^{[m]} ) (xt[m]),首先基于过渡模型 ( p ( x t ∣ u t , x t − 1 [ m ] ) ) ( p(x_t | u_t, x_{t-1}^{[m]}) ) (p(xt∣ut,xt−1[m]))从前一状态 ( x t − 1 [ m ] ) ( x_{t-1}^{[m]} ) (xt−1[m]) 生成一个新的粒子样本。这表示根据控制输入 ( u t u_t ut ) 和之前的状态,预测当前可能的状态。

第5行权重更新,每个新采样的粒子 ( x t [ m ] ) ( x_t^{[m]} ) (xt[m]) 的权重 ( w t [ m ] ) ( w_t^{[m]} ) (wt[m]) 由观测模型 ( p ( z t ∣ x t [ m ] ) ) ( p(z_t | x_t^{[m]}) ) (p(zt∣xt[m])) 计算得出。这个权重反映了给定当前观察 ( z t ) ( z_t ) (zt) 下,粒子 ( x t [ m ] ) ( x_t^{[m]} ) (xt[m]) 的合理性或"拟合度"。

第8~11行是粒子滤波的重采样部分,这一过程在算法实现中非常关键,用于更新粒子集以更好地代表后验概率分布。每个粒子的重要性权重决定了其被重新抽取的概率。这些权重是根据每个粒子相对于观测数据的似然性来分配的,即反映了每个粒子如何适应观测数据的程度。

相关推荐
遨博学院10 小时前
第一个机器人程序
机器人
陈苏同学11 小时前
[论文梳理] 足式机器人规划&控制流程 - 接触&碰撞的控制 - 模型误差 - 自动驾驶车的安全&合规(4个课堂讨论问题)
人工智能·安全·机器学习·机器人·自动驾驶·汽车
三阶码叟12 小时前
sicar 标准功能块 RobotService (机器人服务程序)
机器人·service·sicar
信奥洪老师13 小时前
2025-03 机器人等级考试三级理论真题 3级
算法·青少年编程·机器人·arduino
cnbestec13 小时前
Weiss Robotics的WPG与WSG系列紧凑型机器人夹爪,精准、灵活、高效
人工智能·机器人·weiss·伺服电爪·机器人电爪
yintele15 小时前
机器人手臂控制器:EMC电磁兼容解决(一)
机器人
Mountain and sea1 天前
松下机器人快速入门指南(2025年更新版)
机器人
Blossom.1181 天前
智能机器人在物流行业的应用:效率提升与未来展望
大数据·人工智能·神经网络·安全·机器学习·计算机视觉·机器人
信奥洪老师1 天前
2025-03 机器人等级考试四级理论真题 4级
算法·青少年编程·机器人
望获linux1 天前
望获实时Linux系统荣获人形机器人技术突破奖
linux·运维·机器人·操作系统·开源软件·rtos