假设在一维空间中有一辆车,车辆需要通过观察道路上的红色和绿色标记的位置确定自身的位置。
上图就是解决这个定位问题的思路。
初始状态下,小车由于不知道自身位置,也就是所有地方都是等可能的。因此初始化概率为一条直线,在这个离散系统中,值为1/(可能的位置个数)。
当小车遇到第一个绿色标记后,会有一个观测值,此时小车可能在三个绿色标记中的任何一个。因此概率分布出现了三个峰值。
之后小车第二次遇到绿色标记后,又更新了一个观测值,这时,小车就可以确定自己所处的位置在第二个绿色标记处
其中具体的算法下面介绍。
在车辆定位问题中,假设在初始时,车辆没有开启任何传感器,因此它并不知道自身位置,置所有位置的概率为同样的值。同时,在这个简化的模型中,世界环境是已知的。假设有五个格子,代码如下:
p = [0.2, 0.2, 0.2, 0.2, 0.2]
world = ['green', 'red', 'red', 'green', 'green']
n = 5
假设机器人观测到了红色标记,那么很显然,他在红色位置的概率应该上升,而在绿色位置的概率应当下降,为了简化模型,不妨假设概率上升的程度为0.6,概率下降的程度为0.2
但是计算之后,概率和不为1,因此需要一步归一化的步骤,在离散系统中计算非常简单。
具体代码如下:
pHit = 0.6
pMiss = 0.2
# Z代表观测值
def sense(p, Z):
q = []
for i in range(n):
if world[i] == Z:
q.append(p[i]*pHit)
else:
q.append(p[i]*pMiss)
sum_tmp = sum(q)
for j in range(len(q)):
q[j] = q[j]/sum_tmp
return q
基于最开始的一维假设,这五个格子是循环的,也就是当机器人运动到第五个格子时,再向右走就会走回到第一个格子。由于机器人的运动存在一些不可知性,也就是说他们未必会精确地如设定的那样走,因此,不妨假设如果机器人向右行走2格,那么他正好走2格的概率为0.8,走一格的概率为0.1,走三格的概率为0.1,那么运动之后的概率变化如下所示
代码如下:
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
# U代表运动方向,1表示向右走一格
def move(p, U):
q = []
p_size = len(p)
# U = U % p_size
# q = p[-U:] + p[:-U]
for i in range(len(p)):
q.append(p[(i - U) % p_size]*pExact
+ p[(i - 1 - U) % p_size]*pOvershoot
+ p[(i + 1 - U) % p_size]*pUndershoot)
return q
以上就完成了机器人在一维空间里的定位问题的代码,让我们来运行它,假设机器人向右连续走两次,并且看到的都是红色。
measurements = ['red', 'red']
motions = [1, 1]
for i in range(len(motions)):
p = sense(p, measurements[i])
p = move(p, motions[i])
print(p)
'''
Result1:
[0.07882352941176471, 0.07529411764705884, 0.22470588235294123, 0.4329411764705882, 0.18823529411764706]
'''
代码中是先执行观测,再进行运动,那么在观测到第二个红色后再走一步就走到了第四个格子,与程序运行结果一致,在第四个格子上概率最大。
for i in range(len(motions)):
p = move(p, motions[i])
p = sense(p, measurements[i])
print(p)
'''
Result2:
[0.05882352941176471, 0.2117647058823529, 0.4941176470588235, 0.16470588235294117, 0.07058823529411765]
'''
若是先运动再观测,那么此时程序运行下来机器人处在第三个格子,与实际一致。