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

概率定位算法

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

贝叶斯滤波框架：

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

粒子滤波

基本思路

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

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

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

δ z = h ( x , m ) − z w t i = w t − 1 i ∗ N ( 0 , σ , δ z ) \delta z = h(x,m) - z \\ w^{i}t = w^{i}{t-1} * N(0, \sigma, \delta z) δz=h(x,m)−zwti=wt−1i∗N(0,σ,δz)
N ( 0 , σ , δ z ) N(0, \sigma, \delta z) N(0,σ,δz)指正态分布取 δ z \delta z δz的数值

更新状态
x t = Σ i ( w t i x t i ) Σ i ( w t i ) x_t = \frac{\Sigma_i(w^{i}_tx^{i}_t)}{\Sigma_i(w^{i}_t)} xt=Σi(wti)Σi(wtixti)

计算状态协方差

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

X j , k = 1 1 − Σ i = 1 N ( w i ) 2 Σ i = 1 N w i ( x j i − u j ) ( x k i − u 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) Xj,k=1−Σi=1N(wi)21Σi=1Nwi(xji−uj)(xki−uk)

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

python 复制代码

#计算协方差
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

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

重采样的实现

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

python 复制代码

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