Location直方图滤波算法实现与概率分布可视化

Hello 大家好，今天给大家带来一份关于定位的概述入门性质的文章，看完这篇文章，你将了解Location的意义，以及一个最基本1D直方图滤波的Demo，如果你只是需要源代码，请直接往下翻！

一、Location的意义

在机器人导航任务中，location 可以告诉机器人目前位置，以方便闭环控制或者轨迹规划。一般情况下，Location 可以通过
GPS，WIFI 等方式完成。GPS的定位精度在3.5米左右，WIFI则大于10米。对于机器人、无人汽车而言，这样的精度显然是不可接受的。激光雷达在10m的距离可以达到cm的精度，双目视觉在4m可以达到10cm的精度，与GPS相比有一定优势，此外，这种非在线的定位方式可以在室内使用。

Location 最大的难度来自于测量误差。里程计的计数误差，测量与里程计的不统一等。所以我们需要一种算法，能够较高精度的确定机器人的位姿。

image

如上图所示，那些环境的网格是一个个离散的单元，将连续的环境离散化，面对的是一个离散的概率分布，这样子就意味着机器人可能处于的位置总数是有限的。下面我们将实例讲解一个最基本的基于一维空间的网格Location的直方图滤波算法，以初步定性的理解机器人Location算法。

下面的列子Udacity上的课程 https://cn.udacity.com/course/artificial-intelligence-for-robotics–cs373/

二、直方图滤波

定义一个一维的网格空间

首先我们定义一个一维的网格空间，如下图所示, 机器人就在这里纠结自己处于哪一个格子;

image

初始化概率分布

接着我们需要初始化相应的概率分布：

初始概率


p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']

我们首先初始化没一个网格的概率都占 0.2的均匀分布，定义每一个网格的颜色，这样子这个一维的网格地图就初始化完毕了。

接着我们需要加入传感器的信息，以及机器人运动概率信息。

加入传感器与运动


measurements = ['red', 'red'] 
motions = [1,1]
pHit    = 0.6
pMiss   = 0.2
pExact      = 0.8
pOvershoot  = 0.1
pUndershoot = 0.1

在机器人运动过程中，我们首先定义机器人的运动模式 motions 分别是 -1 和 1,即移动方向左（-）右（+），假定格子是循环的(首尾联通);

image

接着我们定义pHit和pMiss，他们分别代表的是实际值与测量值相同的概率和不同的概率，这个怎么理解喃，我们来看一下下面这张图：

image

如图所示，机器人测量到的信息是red，真实的颜色分别是 ['green', 'red', 'red', 'green', 'green']，那么机器人处于各个位置的概率就应该这样计算，首先如果处于第一个网格，那么表示机器人检测错了，则处于一个网格的值应该为：0.2 * 0.2 = 0.04，那么同理可得，若处于第二个网格，就表明检测对了，则相应的网格值为：0.6 * 0.2 = 0.12。
最终得到下面的数值分布：
['0.04', '0.12', '0.12', '0.04', '0.04', '0.04'];

注意,这里还不是概率分布，因为和不等于1,我们需要进行归一化处理，需要知道具体的代码实现请继续往下看。

然后，我们定义了机器人自己运动正确到达每个位置的概率, 即使我们已经可以预测他到达的位置，但是实际运动总是存在误差，可能是轮子打滑，遇到障碍物，中途没电，等等，因此我们有不足/超调的三种情况：

image

pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

就这样我们定义了所有的概率分布，接着让我们实现具体的算法吧。

算法主体包含两个部分，测量值的更新和运动更新，最终得到各个网格的概率分布。

测量值更新

#  update measurement
def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    # Normalized Sense
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

我们首先需要获取所有的测量值： for i in range(len(p)):，之后分别与实际的world[i]比较，之后乘上相应的pHit / pMiss。
之后通过sum函数获取总和用于皈依化处理，得到测量的概率分布。

运动更新

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q

我们通过len(p)获取运动的次数，之后分别计算正确/超调/不足的概率，并且求和以得到运动之后的概率分布。

可视化：

然后我们需要可视化我们的得到的分布：

def display_map(grid, bar_width=1):

    if(len(grid) > 0):
        x_labels = range(len(grid))
        plt.bar(x_labels, height=grid, width=bar_width, color='b')
        plt.xlabel('Grid Cell')
        plt.ylabel('Probability')
        plt.ylim(0, 1) # range of 0-1 for probability values 
        plt.title('Probability of the robot being at each cell in the grid')
        plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))
        plt.show()
    else:
        print('Grid is empty')

image

通过python的matplotlib库可以轻松的实现可视化，我们可以通过定义如下的main函数，整合上述的工作：

def main():
    global p
    for k in range(len(measurements)):
        p = sense(p, measurements[k])
        
        p = move(p, motions[k])
    
    print (p)   
    display_map(p)   

if __name__ == '__main__':
    main()

我们甚至可以开启matplotlib实时显示功能，观察如果我们运动1000次会有什么结果：

def main():
    global p
    plt.ion()
    for k in range(len(measurements)):
        p = sense(p, measurements[k])
        for i in range(1000):
            p = move(p, motions[k])
            plt.pause(0.05)
            display_map(p, 0.9)
    print (p)

Peek 2018-10-09 04-10.gif

三、总结一下

通过今天的这篇文章，我们介绍了一个最基本用于机器人定位的算法模型，并且使用结合了Python与概率论的只是具体实现了它，别小看这个算法，他可是自动驾驶汽车定位的核心思想！