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

腿足机器人之五粒子滤波

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

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

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

直方图滤波

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

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

相关推荐
一颗小树x5 小时前
【机器人】复现 Aether 世界模型 | 几何感知统一 ICCV 2025
人工智能·机器人·世界模型·aether
Black_Rock_br5 小时前
语音交互新纪元:Hugging Face LeRobot如何让机器人真正“懂你”
人工智能·计算机视觉·机器人
hans汉斯9 小时前
【人工智能与机器人研究】基于力传感器坐标系预标定的重力补偿算法
人工智能·算法·机器人·信号处理·深度神经网络
想要成为计算机高手12 小时前
4. isaac sim4.2 教程-Core API-Hello robot
人工智能·python·机器人·英伟达·isaac sim·仿真环境
YFJ_mily13 小时前
2025第二届机电一体化、机器人与控制系统国际会议(MRCS2025)即将来袭
大数据·人工智能·机器人·机电一体化
weixin_446260851 天前
Isaac Lab:让机器人学习更简单的开源框架
学习·机器人
Mr.Winter`1 天前
轨迹优化 | 基于激光雷达的欧氏距离场ESDF地图构建(附ROS C++仿真)
c++·人工智能·机器人·自动驾驶·ros·ros2·具身智能
Blossom.1182 天前
机器学习在智能制造业中的应用:质量检测与设备故障预测
人工智能·深度学习·神经网络·机器学习·机器人·tensorflow·sklearn
MidJourney中文版2 天前
深度报告:中老年AI陪伴机器人需求分析
人工智能·机器人
沫儿笙3 天前
ABB焊接机器人智能节气仪
人工智能·机器人