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

直方图滤波的算法思想在于:它把整个状态空间dom(x(t))切分为互不相交的部分\(b_1、b_2、...,b_{n-1}\),使得:

然后定义一个新的状态空间\(y_t \in \{0,…,n−1\}\),当且仅当\(x(t)∈b_i\)时,\(y_t=i\),由于\(y_t\)是一个离散的状态空间,我们就可以采用离散贝叶斯算法计算\(bel(y_t)\)。\(bel(y_t)\)是对\(bel(x_t)\)的近似,它给出x(t)在每一个\(b_i\)的概率,\(b_i\)划分的越精细,计算的结果就越精确,当然精确的代价是计算成本的上升。

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

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

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

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

2.1 数学模型

从数学的语言来描述这个问题:机器人只有一个状态变量:位置,\(pos(t) \in [0,5)\)。应用直方图滤波(Histogram Filter),对状态空间做如下分解:

\[b_0 \in [0,1), b_1 \in [1,2), b_2 \in [2,3), b_3 \in [3,4), b_4 \in [4,5)\]

于是得到一个新的状态空间\(y(t) \in {0,...,4}\),它是对连续状态空间的近似,在某一时刻车辆只能在这些离散状态中的一个。

虽然车辆自认为在向右运动,每一步运动1m,我们假设存在5%的概率,无人驾驶汽车仍待在原地没动;存在90%的概率车辆在向右移动1m;存在5%的概率无人驾驶汽车在向右运动2m。

\[\begin{aligned}\text{P}(y_t &= (x+2) mod 5 | y_{t-1} = x) = 0.05 \\\text{P}(y_t &= (x+1) mod 5 | y_{t-1} = x = 0.9 \\\text{P}(y_t &= x|y_{t-1} = x)=0.05 \\\end{aligned}\]

无人驾驶汽车的Sensor假设90%的概率感知结果是正确的,还有10%的情况下它感知的结果是错误的。

\[\begin{aligned}\text{P}(\text{MeasuredColor}_t = \text{Blue} | y_t = 0,2,3) &= 0.9 \\\text{P}(\text{MeasuredColor}_t = \text{Orange} | y_t = 0,2,3) &= 0.1 \\\text{P}(\text{MeasuredColor}_t = \text{Blue} | y_t = 1,4) &= 0.1 \\\text{P}(\text{MeasuredColor}_t = \text{Orange} | y_t = 1,4) &= 0.9 \\\end{aligned}\]

2.2 利用直方图滤波(Histogram Filter)进行车辆定位的过程

我们用一个5维的向量来描述t时刻,无人驾驶汽车位于第1个格子、第2个格子、第3个格子、第4个格子、第5个格子的概率。

\[\text{bel}(y_t) = (\text{bel}_{t,1}, \text{bel}_{t,2}, \text{bel}_{t,3}, \text{bel}_{t,4}, \text{bel}_{t,5})\]

无人驾驶汽车对自己所在位置一无所知,假设它连续三次【运动-向右走一步】-【观测】,三次观测结果分别是orange、blue、orange,我们一步步无人驾驶汽车是如何通过【运动】-【观测】过程逐步确认自己的位置的。

t=0时刻

没有任何先验知识,无人车不知道自己在哪里,所以在各个位置概率相等:

\[\text{bel}(y_0) = (0.2, 0.2, 0.2, 0.2, 0.2)\]

t=1时刻

首先向右走1m,用运动模型进行位置预测

\[\begin{aligned}\overline{\text{bel}}(y_1) &= \sum_{y_0} \text{P}(y_1 | y_0) P(y_0) \\&=(0.05,0.9,0.05,0,0) * 0.2+(0,0.05,0.9,0.05,0) * 0.2 \\&+(0,0.05,0.9,0.05) * 0.2+(0.05,0,0,0.05,0.9) * 0.2 \\ &+(0.9,0.05,0,0,0.05) * 0.2 \\&=(0.2,0.2,0.2,0.2,0.2)\end{aligned}\]

可以看出无人车虽然向前运动一步,但它仍然对自己所在位置一无所知。这也和我们的认知相同,刚开始完全不知道自己在哪里,走了一步自然也完全不知道自己在哪里。

再用更新模型通过Sensor感知环境,更新当前位置的置信度。

\[
\begin{aligned}
\text{bel}(y_1) &= \eta \text{P}(\text {MeasuredColor}_1 = \text{orange} | {y_1}) \overline {\text {bel}} (y_1) \\& = \eta (0.1,0.9,0.1,0.1,0.9) {*} (0.2,0.2,0.2,0.2,0.2) \\& = \eta (0.02,0.18,0.02,0.02,0.18) \\ & = (0.04762,0.42857,0.04762,0.04761,0.42857)
\end{aligned}
\]

orange的颜色感知信息使得传感器认为自己很可能位于第二个和第五个方格中。

t=2时刻

运动模型-向右走1m

\[\begin{aligned}\overline{\text{bel}}(y_2) &= \sum_{y_1} \text{P}(y_2 | y_1) \text{P}(y_1) \\&= (0.39048,0.08571,0.39048,0.06667,0.06667) \\\end{aligned}\]

更新模型-sensor环境感知

\[
\begin{aligned}
\text{bel}(y_2) &= \eta \text{P}(\text{MeasuredColor}_2 = \text{blue} | y_2) \overline{\text{bel}}(y_2) \\ &=\eta (0.1,0.9,0.1,0.1,0.9) {*} (0.39048,0.08571,0.39048,0.06667,0.06667) \\&=(0.45165,0.01102,0.45165,0.07711,0.00857) \\
\end{aligned}
\]

t=3时刻

运动模型-向右走1m

\[\begin{aligned}\overline{\text{bel}}(y_3) &= \sum_{y_2} \text{P}(y_3 | y_2) \text{P}(y_2) \\&= =(0.03415,0.40747,0.05508,0.41089,0.09241) \\\end{aligned}\]

感知更新模型-sensor环境感知

\[
\begin{aligned}
\text{bel}(y_3) &= \eta \text{P}(\text{MeasuredColor}_3 = \text{orange} | y_3) \overline{\text{bel}}(y_3) \\
&= \eta(0.1,0.9,0.1,0.1,0.9){\times} (0.03415,0.40747,0.05508,0.41089,0.09241) \\
&= (0.00683,0.73358,0.01102,0.08219,0.16637) \\
\end{aligned}
\]

可以看到经过三次的运动和观测后,无人驾驶汽车已经有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)是车辆的位置,\(\theta\)是车辆的朝向,v是车辆的运动速度,我们假设车辆是匀速运动的,\(\omega\)是车辆运动的角速度。

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

\(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的位置都是已知的。

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)

定位效果如下:

参考链接

1.http://www.deepideas.net/robot-localization-histogram-filter/

2.https://github.com/AtsushiSakai/PythonRobotics

个人博客网站地址: http://www.banbeichadexiaojiubei.com

自动驾驶定位算法-直方图滤波(Histogram Filter)定位的更多相关文章

  1. zz自动驾驶复杂环境下高精度定位技术

    今天为大家分享下,自动驾驶在复杂环境下的高精度定位技术. 定位/导航负责实时提供载体的运动信息,包括载体的:位置.速度.姿态.加速度.角速度等信息. 自动驾驶对定位系统的基本要求: 1. 高精度:达到 ...

  2. [CLPR] 定位算法探幽 - 边缘和形态学

    一. 引言 如何从一副图片中找到车牌? 这是机器视觉的一个应用. 理所当然地, 思考的角度是从车牌本身的信息入手, 为了讨论方便, 下面均以长窄型蓝白车牌为例. 下图就是这样一张车牌的基本信息. 一眼 ...

  3. L3级自动驾驶

    L3级自动驾驶 2020年开年 3月9日,工信部在其官网公示了<汽车驾驶自动化分级>推荐性国家标准报批稿,并拟于2021年1月1日开始实施. 按照中国自身标准制定的自动驾驶分级标准,在千呼 ...

  4. Mobileye 自动驾驶策略(二)

    Mobileye 自动驾驶策略(二) 与多方都成功进行了合作,其中比较大型的合作包括法雷奥.百度和中国 ITS. 法雷奥是最近的的 Tier 1 合作伙伴,法雷奥和 Mobileye 签署协议,表示未 ...

  5. 关于APIT定位算法的讨论

    关于APIT定位算法的讨论 [摘要]   无线传感器网络节点定位机制的研究中,基于距离无关的定位技术得到快速发展,其中基于重叠区域的APIT定位技术在实际环境中的定位精度高,被广泛研究和应用. [关键 ...

  6. zz高精地图和定位在自动驾驶的应用

    本次分享聚焦于高精地图在自动驾驶中的应用,主要分为以下两部分: 1. 高精地图 High Definition Map 拓扑地图 Topological Map / Road Graph 3D栅格地图 ...

  7. <转载> 从算法上解读自动驾驶是如何实现的?

     科技新闻小鹏汽车2016-03-28 10:42 [摘要]车辆路径规划问题中路网模型.路径规划算法和交通信息的智能预测为关键点. 由于驾驶员的驾驶工作繁重,同时随着汽车拥有量的增加,非职业驾驶员的数 ...

  8. 2020云栖大会智慧出行专场:聚焦高精地图/算法、智能模型、自动驾驶、AR导航

    2020云栖大会将于9月17日-18日在线举行,届时将通过官网为全球科技人带来前沿科技.技术产品.产业应用等领域的系列重要分享.   阿里巴巴高德地图携手合作伙伴精心筹备了“智慧出行”专场.我们将为大 ...

  9. zz自动驾驶中轨迹规划的探索和挑战

    大家好,今天我们主要介绍一下轨迹规划的探索和挑战,我主要从四个方面介绍: 轨迹规划的概念 决策 横向规划 纵向规划 轨迹规划的概念: 轨迹规划的核心就是要解决车辆该怎么走的问题.比如我们知道了附近有行 ...

随机推荐

  1. 工作流Activity框架入门(一)

    Activity工作流入门 1. 工作流概念 工作流(Workflow),就是"业务过程的部分或整体在计算机应用环境下的自动化",它主要解决的是"使在多个参与者之间按照某 ...

  2. react中,路由的使用。import {BrowserRouter,Switch,Route} from "react-router-dom";

      import React from "react"; import ReactDom  from "react-dom"; import {BrowserR ...

  3. js中函数this的指向

    this 在面试中,js指向也常常被问到,在开发过程中也是一个需要注意的问题,严格模式下的this指向undefined,这里就不讨论. 普通函数 记住一句话哪个对象调用函数,该函数的this就指向该 ...

  4. C语言系列之实验楼笔记(一)

    创建C程序的几个过程: 1.编辑:创建和修改C程序的源代码 2.编译:编译器可以将源代码转成机器语言.linux 这些文件扩展名.o 3.链接:通过一次完成编译和链接 4.执行;运行程序 打开xfce ...

  5. python+selenium爬取百度文库不能下载的word文档

    有些时候我们需要用到百度文库的某些文章时,却发现需要会员才能下载,很难受,其实我们可以通过爬虫的方式来获取到我们所需要的文本. 工具:python3.7+selenium+任意一款编辑器 前期准备:可 ...

  6. Linux用户在第一次登录时强制更改初始密码

    迫使用户更改密码 如果你想迫使用户更改其密码,请使用下面这个命令. $ sudo chage -d0 <user-name>   最初,“-d <N>”选项应该被设成密码的“有 ...

  7. CLion在项目里编译不同文件

    1. 在完整的项目下找到CMakeList.txt(项目配置文件) 2. Build 和 Run的目标在add_executable参数中 3. 将其修改为如下格式:add_executable(pr ...

  8. Codeforces_729_E

    http://codeforces.com/problemset/problem/729/E 存在的数必须连续,若不连续,从最后选人来补,注意根不是0和其他点是0的情况. #include<io ...

  9. 《C# 爬虫 破境之道》:第二境 爬虫应用 — 第七节:并发控制与策略

    我们在第五节中提到一个问题,任务队列增长速度太快,与之对应的采集.分析.处理速度远远跟不上,造成内存快速增长,带宽占用过高,CPU使用率过高,这样是极度有害系统健康的. 我们在开发采集程序的时候,总是 ...

  10. pytorch之 RNN 参数解释

    上次通过pytorch实现了RNN模型,简易的完成了使用RNN完成mnist的手写数字识别,但是里面的参数有点不了解,所以对问题进行总结归纳来解决. 总述:第一次看到这个函数时,脑袋有点懵,总结了下总 ...