自动驾驶定位算法(九)-直方图滤波定位
1、直方图滤波(Histogram Filter)的算法思想
直方图滤波的算法思想在于：它把整个状态空间 dom(x(t))切分为互不相交的部分
，使得：
然后定义一个新的状态空间
，当且仅当
时，
由于
是一个离散的状态空间，我们就可以采用离散贝叶斯算法计算
。
是对
的近似，它给出 x(t)在每一个
的概率，
划分的越精细，计算的结果就越精确，当然精确的代价是计算成本的上升。
2、1D 直方图滤波在自动驾驶定位的应用
如下图所示，无人驾驶汽车在一维的宽度为 5m 的世界重复循环，因为世界是循环的，所以如果无人驾驶汽车到了最右侧，再往前走一步，它就又回到了最左侧的位置。
1D直方图滤波Histogram Filter
自动驾驶汽车上安装有 Sensor 可以检测车辆当前所在位置的颜色，但 Sensor 本身的存在一定检测错误率，即 Sensor 对颜色的检测不是 100%准确的；
无人驾驶汽车以自认为 1m/step 的恒定速度向右运动，车辆运动本身也存在误差，即向车辆发出的控制命令是向右移动 2m，而实际的车辆运动结果可能是待在原地不动，可能向右移动 1m，也可能向右移动 3m。
数学模型
从数学的语言来描述这个问题：机器人只有一个状态变量：位置，
。应用直方图滤波(Histogram Filter)，对状态空间做如下分解:
于是得到一个新的状态空间
，它是对连续状态空间的近似，在某一时刻车辆只能在这些离散状态中的一个。
虽然车辆自认为在向右运动，每一步运动 1m，我们假设存在 5%的概率，无人驾驶汽车仍待在原地没动；存在 90%的概率车辆在向右移动 1m；存在 5%的概率无人驾驶汽车在向右运动 2m。
无人驾驶汽车的 Sensor 假设 90%的概率感知结果是正确的，还有 10%的情况下它感知的结果是错误的。
2.2 利用直方图滤波(Histogram Filter)进行车辆定位的过程
我们用一个 5 维的向量来描述 t 时刻，无人驾驶汽车位于第 1 个格子、第 2 个格子、第 3 个格子、第 4 个格子、第 5 个格子的概率。
无人驾驶汽车对自己所在位置一无所知，假设它连续三次【运动-向右走一步】-【观测】,三次观测结果分别是 orange、blue、orange，我们一步步无人驾驶汽车是如何通过【运动】-【观测】过程逐步确认自己的位置的。
t=0 时刻
没有任何先验知识，无人车不知道自己在哪里,所以在各个位置概率相等：
t=1 时刻
首先向右走 1m，用运动模型进行位置预测
可以看出无人车虽然向前运动一步，但它仍然对自己所在位置一无所知。这也和我们的认知相同，刚开始完全不知道自己在哪里，走了一步自然也完全不知道自己在哪里。
再用更新模型通过 Sensor 感知环境，更新当前位置的置信度。
orange 的颜色感知信息使得传感器认为自己很可能位于第二个和第五个方格中。
t=2 时刻
运动模型-向右走 1m
更新模型-sensor 环境感知
t=3 时刻
运动模型-向右走 1m
感知更新模型-sensor 环境感知
可以看到经过三次的运动和观测后，无人驾驶汽车已经有 73%的概率确认自己位于第二个网格中，事实再经过三次的运动观测，无人驾驶汽车可以有 94%的概率确认自己的位置。
3、2D直方图滤波在自动驾驶定位中的应用(一)
1D 的直方图滤波可以很好的帮助我们理解直方图滤波的原理以及在如何应用在自动驾驶的定位过程中。但是 1D 的直方图滤波在实际应用中几乎是不存在的，所以我们从更偏向应用的角度，看看 2D 直方图滤波在自动驾驶定位中是如何工作的。
3.1 定义二维地图
首先定义一张二维地图，R 和 G 代表地图块的颜色：R 为红色，G 为绿色。每个地图块的大小根据实际应用而定，比如 0.0125m x 0.125m、0.025m x 0.025m 等。地图块越小，定位精度越高，但是地图数据量和计算量也就越大；反之，地图块越大，定位精度越低，但数据量和计算量也相应较低。
grid = [
[R,G,G,G,R,R,R],
[G,G,R,G,R,G,R],
[G,R,G,G,G,G,R],
[R,R,G,R,G,G,G],
[R,G,R,G,R,R,R],
[G,R,R,R,G,R,G],
[R,R,R,G,R,G,G],
]t=0 时刻，车辆不知道自己处于地图中的具体位置，转化为数学表述，就是车辆在各个地图块的置信度相同，代码如下:
def initialize_beliefs(grid):
height = len(grid)
width = len(grid[0])
area = height * width
belief_per_cell = 1.0 / area
beliefs = []
for i in range(height):
row = []
for j in range(width):
row.append(belief_per_cell)
beliefs.append(row)
return beliefs初始置信度如下:
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020
0.020 0.020 0.020 0.020 0.020 0.020 0.020置信度的可视化如下，红色星星位置为车辆的真实初始实际位置，蓝色圈大小代表置信度的高低，蓝色圈越大，置信度越高，蓝色圈越小，置信度越低。t=0 时刻，车辆不确定自己的位置，所以各个位置的置信度相等。
3.2 运动更新
车辆运动模型简化为 x、y 两个方向的运动，同时由于运动的不确定性，需要对运动后的位置增加概率性信息。
代码如下:
def move(dy, dx, beliefs, blurring):
height = len(beliefs)
width = len(beliefs[0])
new_G = [[0.0 for i in range(width)] for j in range(height)]
for i, row in enumerate(beliefs):
for j, cell in enumerate(row):
new_i = (i + dy ) % height
new_j = (j + dx ) % width
new_G[int(new_i)][int(new_j)] = cell
return blur(new_G, blurring)3.3 观测更新
观测更新的过程中，当观测的 Color 等于地图块的 Color 时，hit=1， bel=beliefs[i][j] * p_hit；当观测到的 Color 不等于地图块的 Color 时，hit=0， bel=beliefs[i][j] * p_miss。
代码如下:
def sense(color, grid, beliefs, p_hit, p_miss):
new_beliefs = []
height = len(grid)
width = len(grid[0])
# loop through all grid cells
for i in range(height):
row = []
for j in range(width):
hit = (color == grid[i][j])
row.append(beliefs[i][j] * (hit * p_hit + (1-hit) * p_miss))
new_beliefs.append(row)
s = sum(map(sum, new_beliefs))
for i in range(height):
for j in range(width):
new_beliefs[i][j] = new_beliefs[i][j] / s
return new_beliefs3.4 运行定位流程
单次直方图滤波定位过程中，先进行观测更新，再进行运动更新。
def run(self, num_steps=1):
for i in range(num_steps):
self.sense()
dy, dx = self.random_move()
self.move(dy,dx)设置运动更新的不确定度为 0.1，观测更新的错误率：每隔 100 次观测出现一次观测错误，车辆的真实初始位置为(3,3),注意，这个真实位置车辆自己并不知道，我们只是为了仿真而设置的值。
blur = 0.1
p_hit = 100.0
init_pos = (3,3)
simulation = sim.Simulation(grid, blur, p_hit, init_pos)
simulation.run(1)
simulation.show_beliefs()
show_rounded_beliefs(simulation.beliefs)经过一次直方图滤波定位之后，各个位置的置信度已经发生了变化。
0.003 0.002 0.036 0.002 0.037 0.003 0.038
0.003 0.037 0.002 0.002 0.001 0.002 0.037
0.038 0.038 0.003 0.036 0.002 0.002 0.003
0.038 0.004 0.038 0.003 0.037 0.038 0.038
0.003 0.038 0.039 0.038 0.003 0.037 0.003
0.038 0.038 0.038 0.003 0.037 0.003 0.003
0.038 0.003 0.002 0.002 0.038 0.038 0.038置信度的可视化效果如下。可以看到，车辆已经对自己的置信度有了一定的认知，但是还是有大量的可能位置需要进一步确认。
连续执行直方图滤波 100 次，各个位置置信度的数值如下:
0.008 0.000 0.000 0.000 0.000 0.016 0.016
0.032 0.001 0.000 0.000 0.001 0.032 0.833
0.016 0.000 0.000 0.000 0.000 0.025 0.017
0.001 0.000 0.000 0.000 0.000 0.000 0.000
0.000 0.000 0.000 0.000 0.000 0.000 0.000
0.000 0.000 0.000 0.000 0.000 0.000 0.000
0.000 0.000 0.000 0.000 0.000 0.000 0.000置信度的可视化效果如下,可以看到，车辆已经 83.3%的概率可以确定自己所处的位置了。
4、2D直方图滤波在自动驾驶定位中的应用(二)
车辆状态的定义如下：
其中，(x,y)是车辆的位置，
是车辆的朝向，v 是车辆的运动速度，我们假设车辆是匀速运动的，
是车辆运动的角速度。
车辆运动方程如下，其实就是:
写成矩阵形式:
车辆的运动模型代码如下:
def motion_model(x, u):
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
[0, 0, 0, 0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT],
[1.0, 0.0]])
x = F @ x + B @ u
return x4.1 运动更新
运动更新的过程与前面谈到的车辆运动模型一致，车辆运动有不确定性，所以增加了 Gaussian Filter 用来处理不确定性。还有一个细节，就是车辆运动距离和直方图滤波的分块地图之间的转换关系：
代码如下:
# grid_map是网格地图，u=(v,w)是车辆运动的控制参数，yaw是车辆朝向
def motion_update(grid_map, u, yaw):
# DT是时间间隔
grid_map.dx += DT * math.cos(yaw) * u[0]
grid_map.dy += DT * math.sin(yaw) * u[0]
# grid_map.xy_reso是地图分辨率
x_shift = grid_map.dx // grid_map.xy_reso
y_shift = grid_map.dy // grid_map.xy_reso
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
grid_map.dx -= x_shift * grid_map.xy_reso
grid_map.dy -= y_shift * grid_map.xy_reso
# MOTION_STD是车辆运动不确定性的标准差
grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
return grid_map4.2 观测更新
这个例子中通过测量车辆到 LandMark 的距离来确定自身的位置，LandMark 的位置都是已知的。
2D 直方图滤波定位算法
def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
# predicted range
x = ix * gmap.xy_reso + gmap.minx
y = iy * gmap.xy_reso + gmap.miny
d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2)
# likelihood
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
return pdf
#z=[(车辆到Landmark的测量距离，Landmark的x坐标，Landmark的y坐标),...]，z是所有Landmark测量距离和位置的集合，std是测量误差的标准差
def observation_update(gmap, z, std):
for iz in range(z.shape[0]):
for ix in range(gmap.xw):
for iy in range(gmap.yw):
gmap.data[ix][iy] *= calc_gaussian_observation_pdf(
gmap, z, iz, ix, iy, std)
# 概率归一化
gmap = normalize_probability(gmap)
return gmap4.3 运行定位流程
设置地图和测量相关参数:
DT = 0.1 # time tick [s]
MAX_RANGE = 10.0 # maximum observation range
MOTION_STD = 1.0 # standard deviation for motion gaussian distribution
RANGE_STD = 3.0 # standard deviation for observation gaussian distribution
# grid map param
XY_RESO = 0.5 # xy grid resolution
MINX = -15.0
MINY = -5.0
MAXX = 15.0
MAXY = 25.0
# Landmark Position
RF_ID = np.array([[10.0, 0.0],
[10.0, 10.0],
[0.0, 15.0],
[-5.0, 20.0]])
# 车辆的初始位置(for simulation)
xTrue = np.zeros((4, 1))通过 Observation 模拟自动驾驶车辆对各个 LandMark 的观测结果和车辆速度的误差。
def observation(xTrue, u, RFID):
xTrue = motion_model(xTrue, u)
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])):
dx = xTrue[0, 0] - RFID[i, 0]
dy = xTrue[1, 0] - RFID[i, 1]
d = math.sqrt(dx ** 2 + dy ** 2)
if d <= MAX_RANGE:
# add noise to range observation
dn = d + np.random.randn() * NOISE_RANGE
zi = np.array([dn, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to speed
ud = u[:, :]
ud[0] += np.random.randn() * NOISE_SPEED
return xTrue, z, ud执行车辆定位流程:
while SIM_TIME >= time:
time += DT
print("Time:", time)
u = calc_input()
yaw = xTrue[2, 0] # Orientation is known
xTrue, z, ud = observation(xTrue, u, RF_ID)
grid_map = histogram_filter_localization(grid_map, u, z, yaw)定位效果如下:
图片来源:https://github.com/AtsushiSakai/PythonRobotics
参考链接
1.http://www.deepideas.net/robot-localization-histogram-filter 2.https://github.com/AtsushiSakai/PythonRobotics
文中完整代码路径:
第3节中的代码github路径:
https://github.com/iamshnoo/localization
第4节中的代码的github路径:
https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/histogram_filter/histogram_filter.py
相关文章: