粒子过滤器实例——使用粒子滤波器跟踪类车机器人

给老板画饼

已于 2023-08-20 11:20:52 修改

阅读量212

点赞数 2

文章标签：机器人算法人工智能

于 2023-08-19 21:18:50 首次发布

本文链接：https://blog.csdn.net/weixin_48334986/article/details/132361303

版权

使用粒子滤波器跟踪类车机器人

粒子滤波器是一种基于采样的递归贝叶斯估计算法，在 stateEstimatorPF 对象中实现。在本例中，一个类似遥控汽车的机器人正在室外环境中被跟踪。机器人的姿态测量由车载 GPS 提供，但 GPS 存在噪声。有已知的运动指令发送给机器人，但由于机械松弛或模型不精确，机器人不会执行精确的指令运动。本示例将展示如何使用 stateEstimatorPF 来减少测量数据中的噪声影响，并获得更准确的机器人姿态估计值。类似汽车的机器人运动模型由以下非线性系统描述。粒子滤波器非常适合估计这类系统的状态，因为它可以处理固有的非线性问题。

$\begin{array} {cl} \dot{x} &= v \cos(\theta) \\ \dot{y} &= v \sin(\theta)\\ \dot{\theta} &= \frac{v}{L}\tan{\phi}\\ \dot{\phi} &= \omega \end{array}$

场景：类似汽车的机器人在行驶过程中不断改变速度和转向角。机器人的姿态由一些噪声较大的外部系统测量，例如 GPS 或 Vicon 系统。在行驶过程中，机器人会穿过一个无法进行测量的屋顶区域。

输入：
机器人局部姿态的噪声测量值( $x$ , $y$ , $\theta$ )请注意，这不是完整的状态测量。无法测量前轮方向( $\phi$ )和所有速度（ $\dot{x}$ , $\dot{y}$ , $\dot{\theta}$ , $\dot{\phi}$ ）。
发送给机器人的线速度和角速度指令（ $v_c$ , $\omega_c$ ）。请注意，指令运动与机器人的实际运动之间会有一些差异。

目标：估算仿车机器人的局部姿态( $x$ , $y$ , $\theta$ )。请再次注意，车轮方向( $\phi$ )并不包含在估算中。从观察者的角度来看，汽车的完整状态只有 ( $x$ , $y$ , $\theta$ , $\dot{x}$ , $\dot{y}$ , $\dot{\theta}$ )。

方法：使用 stateEstimatorPF 处理两个噪声输入（两个输入本身都不可靠），并对当前（部分）姿势做出最佳估计。

在预测阶段，我们用一个简化的、类似独轮车的机器人模型来更新粒子的状态，如下图所示。请注意，用于状态估计的系统模型并不完全代表实际系统。只要系统噪声（由粒子群表示）能很好地捕捉到模型差异，这种情况是可以接受的。更多详情，请参阅predict。

$\begin{array} {cl} \dot{x} &= v \cos(\theta) \\ \dot{y} &= v \sin(\theta) \\ \dot{\theta} &= \omega \end{array}$

在纠正阶段，粒子的重要性权重（可能性）由当前测量( $\sqrt{(\Delta x)^2 + (\Delta y)^2 + (\Delta\theta)^2}$ )的误差规范决定，因为我们只对这三个部分进行测量。更多详情，请参见correct。

初始化类似汽车的机器人

设置粒子过滤器

本节使用 5000 个粒子对粒子滤波器进行配置。最初，所有粒子都是从正态分布中随机抽取的，正态分布的均值为初始状态，协方差为单位。每个粒子包含 6 个状态变量（, , , , , ）。请注意，第三个变量被标记为圆形，因为它是汽车的方向。

最低0.47元/天解锁文章

给老板画饼

关注

2
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
粒子过滤器实例——使用粒子滤波器跟踪类车机器人

粒子滤波器是一种基于采样的递归贝叶斯估计算法，在 stateEstimatorPF 对象中实现。在本例中，一个类似遥控汽车的机器人正在室外环境中被跟踪。机器人的姿态测量由车载 GPS 提供，但 GPS 存在噪声。有已知的运动指令发送给机器人，但由于机械松弛或模型不精确，机器人不会执行精确的指令运动。本示例将展示如何使用 stateEstimatorPF 来减少测量数据中的噪声影响，并获得更准确的机器人姿态估计值。类似汽车的机器人运动模型由以下非线性系统描述。
复制链接

扫一扫