机器人概率定位学习笔记第三篇_使用python建立机器人运动函数（move fuction）

本笔记学习于优达学城 Artificial Intelligence for Robotics课程

1，假设机器人处在了环形世界，如下图，机器人在各个方块的概率如下，那么机器人向右移动之后，机器人处在各个方块的概率是多少？

answer：1/9，1/9，1/3，1/3，1/9

2，下面我们用一个函数实现这个功能,即move function 运动之后的概率方程。

#Program a function that returns a new distribution 
#q, shifted to the right by U units. If U=0, q should 
#be the same as p.

p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2

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)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

def move(p, U):
    U = U % len(p)
    q = p[-U:] + p[:-U]
    return q
#test the function
print move(p, 1)
print move(p, 2)
print move(p, 3)
print move(p, 4)
print move(p, 5)
print move(p, -1)

3，接下来我们讨论inaccurate robot motion 不准确的机器人移动。

请看下图，假设第一次机器人在第二个方格，向右一次移动了两个方格，但是机器人有可能少移动了一点，或者多移动了一点，那么他在第四个方格的概率是0.8，第三个方格的概率是0.1，第五个方格概率是0.1。

4，请看下图，机器人不在确定的两个方格里，概率各位0.5，现在机器人向右移动两个方格，求移动后的各个方格的概率，记住我们的世界是环形的.

answer:0.4,0.05,0.05,0.4,0.1

第二个问题，假设机器人在各个方格里都有概率，即每个方格的概率都是0.2，那么向右移动两格之后的概率是多少？

答案很明显，依旧是5个0.2.

5,实现不准确的机器人运动方程，

#Write code that makes the robot move twice and then prints 
#out the resulting distribution, starting with the initial 
#distribution p = [0, 1, 0, 0, 0]

p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

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)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

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
p=move(p,1)
p=move(p,1)

print p

6,让机器人向右移动两次

answer：

在上面的代码最后添加两行

p=move(p,1)

p=move(p,1)

print p

查看结果

7,让机器人向右移动1000次

answer：

for i in range(1000):
 p=move(p,1)
print p

查看结果

[0.20000000000000365, 0.20000000000000373, 0.20000000000000365, 0.2000000000000035, 0.2000000000000035]

至此你已经做完机器人运动概率方程，下一篇就是将感知方程和运动方程连接起来了。