“0公式系列”之粒子滤波算法----逃离迷宫

放在开头,“0公式系列:无公式,无理论,单靠动画,或图片,加上个人理解,来解释算法中的思想。

你醒来在一个迷宫内,不知道自己在这个大迷宫的哪一个位置,你要出去,但是你也不能乱走吧,正当你迷茫的时候,你发现自己手里有一张纸质的迷宫地图,该地图不仅包含了迷宫的形状,还包括了迷宫的尺寸。于是,希望来了,你现在要做的就是用这个地图,应用神奇的“粒子滤波”算法(particle filtering,以下简称pf),完成自身的定位,然后走出去(走出去这个环节与pf无关)。

                                                                                     图1.手中的迷宫地图

pf第一步:假设你在迷宫的任何一个位置(这可能听的有点扯),周围的墙体高大,你爬不上去,如图2。你假设的你所处的位置就叫粒子(红点),现在你前方不远处有一堵墙(等下我再解释看不到墙的情况),这个所谓的“墙”,它可能是个转角处,也可能是个死胡同。你能用你的眼睛,估计你面前这堵墙到你的距离假设是是20m左右吧,注意这个地方,你眼睛看到的叫观测值,观测值是有偏差的。好了,我们有了第一个观测值,重复一下,我们到我们面前这堵墙的距离有大概20m远,你不用管你面朝东南西北,你也不用管你的后面有没有墙。总之啊,你距离一堵眼前的墙有大概20m远,那么就是说,你可以把距离转角处或者死胡同不在20m范围内的粒子(假设的位置点)去掉,这样的话,你可以排除一些最开始假设的位置点。如图3。

 

                                图2.红色的点是你最开始假设的位置,我画的一点也不密集,实际上假设的位置是铺满整个迷宫的

 

                                                      图3.虽然去除了一些点,但是还是有不少假设的位置

pf第二步:现在你要往前走20m,就走到你面前的墙的那个位置。分三种情况:

(1)是个拐角处,那还好,拐过去就行了。

(2)是个“T”型岔口也没关系,规定遇到岔口向左转吧。

(3)是个死胡同就悲剧了,那就回到出发点向后转吧,看看你在出发点的时候后面有没有墙。

pf第三步:假设你的运气不错,不是个死胡同,那么你到了转角处后,转向,然后停!你再看看你的面前没有新的墙。这分两种情况:

(1)有墙,这跟你的第一步一样,估计你目前的位置到这个新墙的距离。不管它是个转角处还是个死胡同都不重要。假设你距离新墙的位置是10m。现在重点来了啊!你捋捋思路,你看到前面有堵墙,起身,往前走了大概20m,转弯后前面又有一堵墙,距离大概10m。也就是说,你转弯后的视野内,必须有一堵墙在你面前10m左右远(定义为条件1)。不满足这个条件的之前的粒子(假设的位置)都要去掉。好了现在我们有了两幅图,一幅是所有粒子往前走20m到了墙跟头,见图4。你得到了很多蓝点,根据条件1,除去不符合条件蓝色点以及之前所有假设的红点。见图5。

(2)没有墙,是个无尽的大道。那就有点悲剧了,只能往前走了。直到看到墙位置,而且你还得记得你走了大概多少米,专业点这叫里程计读到的数据。假设你走了大概100m,终于看到了一个新的墙在你前面50m处,那么这条道就有100+50m左右的距离。接下来跟上面(1)的过程一样,看到墙就好了。

                                                图4.蓝色是红点(也就是你醒来的位置)向前移动大概20m的后的位置

     图5.现在我只保留了符合条件1的蓝点,红色箭头表示你最开始前进的方向,黄色箭头表示你到了墙角转头后的方向

现在很舒服,你已经定位到地图3个可能的位置了,比之前全地图都是假设位置的情况好多了。但是你有没有发现,你在转角处看到新墙的位置,是大概10m,有可能是6m,也可能是14m,所以说,这个3个假设位置也是有不同的可信度。根据图5。例如,对于最上面这个假设位置,看到新墙的距离就比10m小一些,右下角这个位置,看到新墙的距离就很接近10m,左下角的这个位置,看到新墙的距离就比10m大一些。但他们三个都是大概10m, 为了区分,我们就给右下角的可信度大一些,其他两个位置(最上面和左下角)的可信度就小一些。

话说我定义的可信度有什么用呢?

pf第四步:我们基于上面三个点重新假设位置,注意,根据可信度的大小,假设位置的点的数量不同,见图6,就是说我们更相信我们在右下角的位置,但是我们不排除在其他那两个可信度不高的位置的情况。

然后,重复第二步到第四步,重复几次,你就可以定位到你的位置了,很神奇吧。这个重复的图我就不画了,大家自己想想吧。重点就是,目前的位置跟上一时刻的位置是相关的

整个粒子滤波算法的流程我就说完了。这里没有公式(贝叶斯公式巴拉巴拉),专业术语(重采样巴拉巴拉),网上有很多原理的介绍,感兴趣的,可以一边看正规的原理和公式,一边思考我这个迷宫定位的方法,找一找他们之间的联系。

另外推荐一本书《粒子滤波算法及其应用》(朱志宇),上面还有粒子滤波的衍生算法,以及在目标检测,地图定位,FPGA,DSP的应用。

猜你喜欢

转载自blog.csdn.net/qq_34754836/article/details/84557745