【机器人状态估计】粒子滤波算法介绍

最新推荐文章于 2024-07-25 17:54:26 发布

CS_Zero

最新推荐文章于 2024-07-25 17:54:26 发布

阅读量1.2k

点赞数 24

文章标签：机器人算法

本文链接：https://blog.csdn.net/CSUzhangpeike/article/details/136140510

版权

本文介绍了在自动驾驶中如何运用概率定位算法，特别是粒子滤波方法，结合贝叶斯滤波框架，通过运动模型预测、传感器测量更新权重、重采样等步骤实现多传感器融合定位。着重讲解了权重更新机制和重采样过程以逼近后验分布。

摘要由CSDN通过智能技术生成

概率定位算法

问题分类：位姿追踪、局部定位、全局定位；静态、动态环境定位；单一机器人定位、多机器人定位。

贝叶斯滤波框架：
定位置信度与运动模型卷积，两次独立估计值的整合比单一估计值使系统状态确定性更高。

粒子滤波

基本思路

随机产生M个粒子（如M=1000），每个粒子表示状态变量的（随机）数值，如位置、航向等，粒子有权重，即重要性因子，初始时每个粒子的权重都相等
预测（运动方程）每个粒子下一时刻的”位置“
根据测量更新粒子的权重，与测量更接近的粒子具有更高的权重
重采样粒子，去除权重低的粒子，并用高权重的粒子去复制替代，粒子的权重被重新均分
统计粒子集合的加权均值和方差以得到系统的状态估计

粒子滤波实现自动驾驶多传感器融合定位

使用运动方程从上一时刻完成更新的粒子预测状态
传感器测量与从预测的状态估计的测量做差, 称为innovation, 重新确定第i个粒子的权重
$w^{i}_t = p(z_t|x^{i}_t, m)$

$\delta z = h(x,m) - z \\ w^{i}_t = w^{i}_{t-1} * N(0, \sigma, \delta z)$
$\sigma, \delta z)$ 指正态分布取 $\delta z$ 的数值
更新状态
$x_t = \frac{\Sigma_i(w^{i}_tx^{i}_t)}{\Sigma_i(w^{i}_t)}$

计算状态协方差

第j行第k列元素的值为：

$X_{j,k}=\frac{1}{1-\Sigma^N_{i=1}(w^i)^2}\Sigma^N_{i=1}w^i(x^i_j-u_j)(x^i_k-u_k)$

$x^i_j$ 是第i个粒子的第j维值， $x^i_k$ 是第i个粒子的第k维值， $u_j$ 是估计的状态第j维值

#计算协方差
def calc_covariance(x_est, px, pw):
    """
    calculate covariance matrix
    see ipynb doc
    """
    cov = np.zeros((3, 3))
    n_particle = px.shape[1]
    for i in range(n_particle):
        dx = (px[:, i:i + 1] - x_est)[0:3]
        cov += pw[0, i] * dx @ dx.T
    cov *= 1.0 / (1.0 - pw @ pw.T)
    return cov

工程与参考《概率机器人》中有一处差异，粒子的权重更新，参考为似然函数值，参考代码将先前权重与函数值相乘。

重采样的实现

经过重采样，粒子的分布近似后验，重复高权重的粒子，低权重的粒子更有可能被替换掉

def re_sampling(px, pw):
    """
    low variance re-sampling
    """

    w_cum = np.cumsum(pw)
    base = np.arange(0.0, 1.0, 1 / NP)
    re_sample_id = base + np.random.uniform(0, 1 / NP)
    indexes = []
    ind = 0
    for ip in range(NP):
        while re_sample_id[ip] > w_cum[ind]:
            ind += 1
        indexes.append(ind)

    px = px[:, indexes]
    pw = np.zeros((1, NP)) + 1.0 / NP  # init weight
    return px, pw