无人驾驶中的简单定位1

最近在学习Udacity的无人驾驶课程，简单做一些总结

问题描述

假设在一维空间中有一辆车，车辆需要通过观察道路上的红色和绿色标记的位置确定自身的位置。

上图就是解决这个定位问题的思路。
初始状态下，小车由于不知道自身位置，也就是所有地方都是等可能的。因此初始化概率为一条直线，在这个离散系统中，值为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]
'''

若是先运动再观测，那么此时程序运行下来机器人处在第三个格子，与实际一致。