这两天在优达学城上学习了谷歌无人驾驶汽车最基本的定位原理，编写了一个简单的python程序，理解了带有传感器的汽车，如何在事先已知的地图中运动时确定自身的位置。虽然只是很简单的数学原理，但是人嘛，活着总是需要成就感的，非常开心，用到了一些简单的概率知识，没有上一篇神经网络识别阿拉伯数字的矩阵知识那么难，但是都熟练了python的使用，感受到了数学的魅力，实现了很有实际意义的功能。
我们来看实际的例子解析，首先我们现在有一张地图，是一个4行5列的矩阵colors1，一共20个格子，如图，G代表green绿色，R代表red红色，现在汽车在地图中哪一个格子都是可能的，每一个格子都是1/20的概率，所以我们初始化一个4*5的矩阵p，每一个元素都为1/20，现在我们让小车先向右走一格，用[0,1]来表示，再让小车向下走一格，用[1,0]来表示，我们给小车发布了五个命令，用motions1来表示， [[0,0]，[0,1]，[1,0]，[1,0]，[0，1]]，这意思是，第一次不动，然后右走一格，下走两格，再右走一格。这个就叫做里程计了。小车每执行一次操作，都会得到一个测量信息，这个就是传感器信息，程序里就是measurements1，[‘G’，’G’，’G’，’G’，’G’]。意思是每次执行完命令，摄像头都看到自身所在的格子是绿色的。
image
现在我们有地图colors1，里程计信息motions1，传感器信息measurements1，和一个初始化的矩阵p，如果我们看着地图，我们应该知道我们肯定在这个蓝色位置了，概率为1，其他位置概率为0。这只是在理想的情况下。
image
现实都不那么理想，第一，我们的运动命令可能没能成功执行，比如地面比较坑，我们指令走一格，油没够，真实没有走动，我们设置了一个参数p_move=0.8，表示指令成功执行的概率，第二传感器信息，摄像头在当前位置看错了，可能出现光强等影响，我们设置sensor_right=0.7表示摄像头信息正确的概率，通过我们的程序，可以把每个格子都为1/20的p矩阵，变为一个小车在走完五步以后，自己处于每个格子概率大小的矩阵，就是我们想要的定位了，当然如果p_move=1.0,sensor_right=1.0,那么我们得到的理想位置蓝色的1，而在不理想情况下我们得到的是这样的，这个蓝色位置的概率必须是最大的而且远远大于其他位置，而且20个格子概率之和为1。
image
下面就看看程序如何实现的
# -*- coding: utf-8 -*-
"""
Created on Sun Nov 26 21:15:23 2017
@author: leisure
"""
import numpy as np #引用numpy库，以下都用np表示，这个库可以方便进行矩阵数据的处理
def localize(colors,measurements,motions): #这是一个实现定位的主程序
pinit = 1.0 / float(len(colors)) / float(len(colors[0])) #pinit的大小由格子的总个数决定，例子里是4*5=20，每一格都是1/20。
p = np.full(np.shape(colors),pinit) #这句话构造一个和地图一样的大小的矩阵，用pinit填满。
for k in range(len(measurements)): #这里开始对每一次指令循环，共五次
p = move(p,motions[k]) #调用一次move函数，输入为初始概率p和第一次指令，具体功能看子函数
p = sense(p,colors, measurements[k]) #调用一次sense函数，输入为上一次的p和地图，还有本次测量的颜色，具体功能看子函数
return p #五次循环后得到最终的概率分布，返回
def sense(p, colors, measurement): #定义函数sense，和需要的三个输入参数
aux = np.zeros(np.shape(colors)) #构造一个和地图一样4*5的矩阵aux，全部元素都为暂时为0，
s= 0.0 #s=0,为了方便后面概率归一化
for i in range(len(p)): #行循环
for j in range(len(p[i])): #列循环，为了遍历每一个数据
hit =(measurement == colors[i][j]) #如果测量颜色和遍历到的这一格颜色一样，hit为1，不一样hit为0
aux[i][j] = p[i][j]*(hit*sensor_right +(1-hit)*(1-sensor_right)) #如果颜色一样的话，这个位置的概率由初始的1/20乘以0.7，如果不一样的话，就乘以0.3，这样颜色一样的位置的概率就远大于颜色不一样的位置了
s += aux[i][j] #因为这样操作，所有格子的概率之和不为1，那就把所有的格子的概率累加，作归一化处理
for i in range(len(aux)): #得到上面的概率后，再遍历一遍20个格子
for j in range(len(p[i])):
aux[i][j] /=s #每个格子的概率除以总和来表示这个格子的概率，那处理之后的概率总和就为1了，归一化处理
return aux #函数返回的这个aux就是我们通过摄像头信息处理后的概率了，颜色正确的位置的概率大于不正确的位置，但是也有可能是摄像头出错了，所以其他位置概率也有，但很小
def move(p,motion): #定义move函数，输入为上一次的概率p，和行动指令motion
aux = np.zeros(np.shape(p)) #和上面的sense函数一样全为0的4*5矩阵，等会要往里面填概率
for i in range(len(p)):
for j in range(len(p[i])): #遍历每一格格子
aux[i][j]=(p_move*p[(i-motion[0])%len(p)][j-motion[1]%len(p[i])])+((1-p_move)*p[i][j]) #这是关键所在，用到了贝叶斯法则的全概率公式，地图是循环的也就是说，走出右边就回到左边，走出下边就回到上边，
#拿向右走一格举例，小车可能是从左边走过来的，也可能是向右走失败停在这个位置的，假设没执行前，小车在这个位置的概率为0.3，在左边的概率为0.1，那么执行后小车在这个这个位置的概率是0.8*0.1+（1-0.8）*0.3
#全概率公式，可能从左边过来，也可能本来就在这儿，因为走失败了，还停留在这儿
return aux #返回的aux就是执行命令之后的概率分布了
p_move=1.0 #假设行动理想
sensor_right=1.0 #假设摄像头没问题
colors = np.array([['G','G','G'], #这是3*3的地图
['G','R','G'],
['G','G','G']])
measurements = ['R'] #测量一次为红色
motions = [[0,0]] #一次命令，不走
colors1 = np.array([['R','G','G','R','R'], #4*5的地图，就是例子所给的情况
['R','R','G','R','R'],
['R','R','G','G','R'],
['R','R','R','R','R']])
measurements1 = ['G','G','G','G','G'] #五次测量信息，全为绿色
motions1 = [[0,0],[0,1],[1,0],[1,0],[0,1]] #五次移动指令，停，右，下，下，右
p = localize(colors,measurements,motions) #执行3*3，只有一次命令的地图定位主程序，得到最终概率p
print(p) # displays your answer
p_move=0.8 #更改成功率
sensor_right=0.7 #更改摄像头准确率
p1 = localize(colors1,measurements1,motions1) #执行4*5地图，移动五次的地图定位程序，得到最终执行完五步后小车所在位置的概率分布
print(p1) #输出结果p1
以上的每一句都有很详细的注释，如果熟悉python的应该很轻松实现这个功能，最关键的数学知识在于move函数里贝叶斯法则的全概率公式那里，还有sense函数里概率归一化的处理。地图和里程计和传感器信息可以任意更改，得到最后的概率分布。
在这个核心的原理上进行接下来的卡尔曼滤波和粒子滤波消除测量信息等噪声的误差影响，丰富程序，slam的实现，继续学习后面的课程。
网友评论