在前面，我们介绍了基于一维网格地图的直方图滤波算法，今天我们将它扩展到二维网格地图，以进一步理解算法。正如前面所提到的那样，整个滤波的过程包含两个部分，测量更新和运动更新最后得到网格地图的概率分布，以表示机器人目前处于的位置。
下面让我们讲解基于2维地图的直方图滤波算法的实现。
首先，我们需要初始化各个网格的初始概率：
初始化
# initializes p to a uniform distribution over a grid of the same dimensions as colors
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))]
其中 colors应该定义为下面的形式以表示每一网格的颜色信息
colors = [['R', 'G'],
['R', 'R'],
['G', 'R'],
['R', 'G'],
['G', 'G']]
sense function
然后是我们的 sense 函数：
Nr=len(colors)
Nc=len(colors[0])
def sense(p, Z):
q=[[p[i][j]*sensor_right if colors[i][j]==Z else p[i][j]*(1-sensor_right) for j in range(Nc) ] for i in range(Nr)]
sumq=sum([sum(e) for e in q])
q=[[x/sumq for x in e] for e in q]
return q
与一维的网格地图一样，分别遍历每一个网格，并且根据测量值（Z）是否与地图值（colors[i][j]）相同以分别乘以相应的概率，然后进行求和，整个过程为一个卷积操作，然后进行归一化，得到由测量引起的概率分布。
move function
motions = [[0, 0], [-1, 0], [0, 1], [0, -1], [0, 1], [1, 0]]
def move(p, U):
q=[[ p_move*p[(i-U[0])%Nr][(j-U[1])%Nc]+(1-p_move)*p[i][j] for j in range(Nc) ] for i in range(Nr)]
return q
这里为了简化情况，只考虑了一个 p_move 即机器人是否移动的概率，然后与sense 函数一样，对所有网格进行遍历，根据定义的运动模型，分别乘上相应的概率，最后得到运动更新之后的概率分布。
输出
我们根据定义的观测值，计算最后的概率分布
colors = [['R', 'G'],
['R', 'R'],
['G', 'R'],
['R', 'G'],
['G', 'G']]
measurements = ['R', 'R', 'G', 'G', 'G', 'R']
motions = [[0, 0], [-1, 0], [0, 1], [0, -1], [0, 1], [1, 0]]
p = localize(colors,measurements,motions,sensor_right=0.99, p_move=0.97)
print('Estimated probability at each location:')
show(p)
最终得到的输出如下：
Estimated probability at each location:
[[0.07876,0.00793],
[0.02465,0.85350],
[0.00001,0.00004],
[0.03447,0.00002],
[0.00003,0.00058]]
参考资料
- 概率机器人
不足之处，敬请斧正; 若你觉得文章还不错，请关注微信公众号“SLAM 技术交流”继续支持我们，笔芯：D。