机器人定位 - 蒙特卡罗定位（直方滤波算法）

---

wiki上的介绍 https://en.wikipedia.org/wiki/Monte_Carlo_localization

Udacity上的课程 https://cn.udacity.com/course/artificial-intelligence-for-robotics–cs373/
下面的例子来自优达学城

---

原理

假设一个机器人在一个一维的世界里，具体假设场景[R,R,G,R,R]。其中R代表红色，G代表绿色。
机器人有一个全局的分布表。

1.首先机器人为所有特征点给一个平均概率，代表最混乱的情况。

• 1/5 = 0.2， 所以机器人现在全局表是[0.2, 0.2, 0.2, 0.2, 0,2]，代表机器人有0.2的概率处在对应的位置。

2.当机器人感知到对应特征的时候，会添加一个滤波器

• 机器人每感知到一个特征就会为所有的位置添加一个滤波器
• 例如 p=0.8， (1-p)=0.2，滤波器自己定义
  原始状态是[0.2, 0.2, 0.2, 0.2, 0.2]，当感知到R特征的时候，就为所有R的位置添加0.8的滤波器. 0.2*0.8 = 0.16, 0.2 * 0.2 = 0.04
• 然后进行归一化的处理。这里涉及到贝叶斯公式的原理。
• 最后的结果大家自己算吧。

3.然后机器人向前移动

因为机器人很难确定自己具体行进过程，所以当机器人前进的时候，需要进行卷积运算。
- 例如 p0=0.1 p1=0.8 p2=0.1,代表机器人有0.1的概念走到目标点的后面，有0.8的概率刚好到目标点，0.1的概率会超过目标点。
- 假设现在是[0.6, 0, 0, 0],机器人向前走两步，则 0.1 * 0.6 = 0.06， 0.8 * 0.6 = 0.48,结果是[0, 0.06, 0.48, 0.06]，最后归一化处理。
- 需要注意的是，移动是对于全局表中所有概率都进行运算，一移动整个表都会移动的。移动完再进行归一化。这里涉及到全概率公式。

4.最后结果取最大值就是机器人的位置。

代码

这里选的是Udacity的习题，我的结果对了，但是就是提交失败，这是仅提供给大家参考

# show：# 输入：#       概率# 输出：#       格式化输出def show(p):    rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]    print('[' + ',\n '.join(rows) + ']')# sense:#   输入：#       probs 当前概率分布#       term  观察到的特征#   输出：#       p     添加滤波后的概率分布def sense(probs, term, sensor_right) :    s = 0    for i in range(len(probs)) :        for j in range(len(probs[0])) :            # 判断是不是目标特征            # 方便为后面添加不同的滤波器            hit = (colors[i][j] == term)            probs[i][j] *= (hit * sensor_right + (1 - hit) * (1 - sensor_right))            s += probs[i][j]    for i in range(len(probs)) :        for j in range(len(probs[0])) :            probs[i][j] /= s     return probs# move:#   输入：#       probs  当前概率分布#       motion 移动指令#   输出：#       p      卷积后的概率分布def move(probs, motion, p_move) :    q = []    x = len(probs)    y = len(probs[0])    #竖着移动    if motion[0] != 0 :        for i in range(x) :            tmp = []            for j in range(y) :                s = p_move * probs[(i - motion[0]) % x][j]                s += (1 - p_move) * probs[(i - motion[0] + 1) % x][j]                tmp.append(s)            q.append(tmp)    #横着移动    else :        for i in range(x) :            tmp = []            for j in range(y) :                s = p_move * probs[i][(j - motion[1]) % y]                s += (1 - p_move) * probs[i][(j - motion[1] + 1) % y]                tmp.append(s)            q.append(tmp)    return q #localize:#   输入：#       colors          二维空间#       measurements    观察到的特征#       motions         移动#       sensor_right    滤波器概率#       p_move          移动卷积概率#   输出：#       probs           概率分布def localize(colors, measurements, motions, sensor_right, p_move) :    pinit = 1.0 / float(len(colors)) / float(len(colors[0]))    p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]    for i in range(len(motions)) :        p = move(p, motions[i], p_move)        p = sense(p, measurements[i], sensor_right)    return p# colors:#   二维空间特征colors = [['R','G','G','R','R'],          ['R','R','G','R','R'],          ['R','R','G','G','R'],          ['R','R','R','R','R']]# measurements:#   观察到的特征            measurements = ['G','G','G','G','G']# motions:#   移动轨迹motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]############################################################## 对于一下的例子，应该输出：# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],#  [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],#  [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],#  [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]# ( +/- 0.001 的误差)p = localize(colors, measurements, motions, sensor_right = 0.7, p_move = 0.8)show(p)