• 微信公众号:美女很有趣。 工作之余,放松一下,关注即送10G+美女照片!

定位(粒子滤波)

开发技术 开发技术 3个月前 (03-01) 18次浏览

一、粒子滤波

1.1 介绍

粒子滤波器是一种基于蒙特卡罗的近似解法,由于计算机计算能力的不断提高和易于实现,粒子滤波器在机器人定位领域得到了广泛的应用,其优势在于对复杂问题的求解上,比如一些非线性、非高斯动态系统的状态递推估计或概率推理问题。粒子滤波器的本质是使用一组有限的加权随机样本(粒子)来近似表征任意状态的后验概率密度 (bel(x_t))

1.2 算法

粒子滤波器的算法流程主要是对粒子集(X)的预测和更新。

算法的输入是粒子集(X_{t-1}),以及最新的控制(u_t)和测量(z_t)算法首先构造一个暂时的粒子集 (bar{X}),表示置信度 (bar {bel(x_t)})。这通过系统地处理输入粒子集(X_{t-1})中的每个粒子(x^{[1]}_{t-1})完成。随后它将这些粒子转换为粒子集(X_t),用于近似后验分布 (bel(x_t))。值得注意的是,这里 (M) 代表粒子集(X_t)的粒子数量,(M)的要根据实际情况选取合适的值。(w^{[m]}_t=p(z_t|x^{[m]}_t))是测量(z_t)在粒子(x^{[m]}_t)下的概率,是粒子滤波表征后验概率密度的由来,即通过当前的观测到的数据来预测当前状态的发生概率。还有就是算法的更新部分,采用了重采样(有一个版本是不进行重采样的)。

二、定位

2.1 初始化

假定机器人是在一个二维的世界中,在这个世界中可以被感知到的路标点有4个,分别是(L1(20.0, 20.0)), (L2(80.0, 80.0)), (L3(20.0, 80.0)), (L4(80.0, 20.0)),同时地图的大小是100X100。那么首先我们在地图上随机一个机器人的坐标(x,y)和运动方向orientation,设定粒子集的M为1000,每个粒子都初始化一个和机器人相同数据结构的状态。

N = 1000 #初始粒子个数
p = [] #粒子集
world_size = 100.0 #地图尺寸

for i in range(N):
    x = random.random() * world_size
    y = random.random() * world_size
    orientation = random.random() * 2.0 * pi
    p.append([x,y,orientation])

接着给机器人设定初始噪声参数,分别是前进噪声(forward_{noise}),转向噪声(turn_{noise})和传感器噪声 (sense_{noise})

forward_noise = 5.0
turn_noise = 0.1
sense_noise = 5.0

2.2 预测

对每一个粒子根据机器人的运动状态,进行下一个状态的预测,例如,机器人的运动指令是先顺时针转动pi/2,再前进10米,则所有的粒子都采用相同的运动指令。

myrobot = myrobot.move(-pi/2, 10.0) #机器人先顺时针转动pi/2,再前进10米

p2 = [] #粒子和机器人做出同样的动作
for i in range(N):
    p2.append(p[i].move(-pi/2, 10.0)) 
p = p2

接着运动之后的,机器人感知路标点,获得与4个路标点的测量距离Z,然后那些粒子也都进行一次路标点的距离dist的计算,然后每个粒子通过各自dist和Z的相近程度,分配权值w。dist与Z越接近,权值w越大,即该粒子越可能是当前机器人的所在点。

2.3 重采样

重采样采用Resampling wheel算法。我们将所有粒子摆在轮盘上,并规定粒子权重的大小和其所占的转轮面积成正比。随后确定一个转盘指针的位置index,beta是在当前index的粒子中,可否被采样的指标。初始beta为0,每次转动转盘我们随机增加r给beta。在重采样中会出现两种情况:

  • (beta > w[index])时,此时算没有抽中,我们将index更新为index+1,同时beta更新为beta-w[index]。
  • (beta leqslant w[index])时,此时算抽中, 将编号为index的粒子采样到新的粒子集中,之后转动转盘接着抽,且beta更新为beta+r,直到beta的值大于index所指的权重。
p3 = []
index = int(random.random()*N) # 从N个粒子中随机选择一个粒子的编号index
beta = 0.0
mw = max(w) #获取粒子集中最大一个粒子的权值w
for i in range(N):
    r = random.random() * 2.0 * mw 
    beta += r
    while beta > w[index]:
        beta -= w[index]
        index = (index + 1) % N
    p3.append(p[index])

2.4 结果

最终,经过不断的预测,重采样更新粒子集,把那些和机器人有着相似观测数据的粒子保留了下来。之后我们可能将权值最大的粒子的状态当作此时机器人的状态,完成了定位。粒子滤波定位的演示如下,绿色是机器人,红色是粒子,一开始这些粒子是均匀分布在地图的,随着机器人的不断运动,慢慢只剩下符合机器人状态的粒子。

定位(粒子滤波)
图2. 粒子滤波demo


程序员灯塔
转载请注明原文链接:定位(粒子滤波)
喜欢 (0)