自动驾驶定位算法-直方图滤波定位

1、直方图滤波(Histogram Filter)的算法思想

直方图滤波的算法思想在于：它把整个状态空间dom(x(t))切分为互不相交的部分、、，使得：

然后定义一个新的状态空间，当且仅当时，,由于是一个离散的状态空间，我们就可以采用离散贝叶斯算法计算。是对的近似，它给出x(t)在每一个的概率，划分的越精细，计算的结果就越精确，当然精确的代价是计算成本的上升。

2、1D直方图滤波在自动驾驶定位的应用

如下图所示，无人驾驶汽车在一维的宽度为5m的世界重复循环，因为世界是循环的，所以如果无人驾驶汽车到了最右侧，再往前走一步，它就又回到了最左侧的位置。

自动驾驶汽车上安装有Sensor可以检测车辆当前所在位置的颜色，但Sensor本身的存在一定检测错误率，即Sensor对颜色的检测不是100%准确的；

无人驾驶汽车以自认为1m/step的恒定速度向右运动，车辆运动本身也存在误差，即向车辆发出的控制命令是向右移动2m，而实际的车辆运动结果可能是待在原地不动，可能向右移动1m，也可能向右移动3m。

2.1 数学模型

从数学的语言来描述这个问题：机器人只有一个状态变量：位置，。应用直方图滤波(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*0.125m、0.025m*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_beliefs

3.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直方图滤波在自动驾驶定位中的应用(二)

车辆状态的定义如下：

$$\begin{aligned}\mathbf{{x}}_{t}&=\begin{bmatrix}\text{x} \\\text{y} \\\theta \\\text{v}\end{bmatrix}\\\end{aligned}$$

其中，(x,y)是车辆的位置，是车辆的朝向，v是车辆的运动速度，我们假设车辆是匀速运动的，是车辆运动的角速度。

车辆运动方程如下，其实就是:

$x_{t+1} = x_t + v * cos(\theta) * \Delta t$

$y_{t+1} = y_t + v * sin(\theta) * \Delta t$

$\theta_{t+1} = \theta_t + \omega * \Delta t$

写成矩阵形式:

$$x_{t+1} =\begin{bmatrix}1.0 & 0.0 & 0.0 & 0.0 \\0.0 & 1.0 & 0.0 & 0.0 \\0.0 & 0.0 & 1.0 & 0.0 \\0.0 & 0.0 & 0.0 & 0.0 \\\end{bmatrix} *\begin{bmatrix}\text{x} \\\text{y} \\\theta \\\text{v}\end{bmatrix}+ \begin{bmatrix}\Delta t cos(\theta) & 0.0 \\ \Delta t sin(\theta) & 0.0 \\0.0 & \Delta t \\1.0 & 0.0 \\\end{bmatrix} *\begin{bmatrix}v \\\omega \\\end{bmatrix}$$

车辆的运动模型代码如下:

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 x

4.1 运动更新

运动更新的过程与前面谈到的车辆运动模型一致，车辆运动有不确定性，所以增加了Gaussian Filter用来处理不确定性。还有一个细节，就是车辆运动距离和直方图滤波的分块地图之间的转换关系：

x_shift = $\Delta$ x / map_x_resolution

y_shift = $\Delta$ y / map_y_resolution

代码如下:

# 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_map

4.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 gmap

4.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/robo...
2.https://github.com/AtsushiSakai/PythonRobotics