自動駕駛定位算法-直方圖濾波定位

時間  2020-02-16
標籤 自動 駕駛 定位 算法 直方圖 濾波 欄目 系統網絡 简体版
原文   原文鏈接

一、直方圖濾波(Histogram Filter)的算法思想

直方圖濾波的算法思想在於：它把整個狀態空間dom(x(t))切分爲互不相交的部分、、，使得：python

無人駕駛高精度定位技術(1)-直方圖濾波Histogram Filter

而後定義一個新的狀態空間，當且僅當時，,因爲是一個離散的狀態空間，咱們就能夠採用離散貝葉斯算法計算。是對的近似，它給出x(t)在每個的機率，劃分的越精細，計算的結果就越精確，固然精確的代價是計算成本的上升。git

二、1D直方圖濾波在自動駕駛定位的應用

以下圖所示，無人駕駛汽車在一維的寬度爲5m的世界重複循環，由於世界是循環的，因此若是無人駕駛汽車到了最右側，再往前走一步，它就又回到了最左側的位置。github

自動駕駛汽車上安裝有Sensor能夠檢測車輛當前所在位置的顏色，但Sensor自己的存在必定檢測錯誤率，即Sensor對顏色的檢測不是100%準確的；算法

無人駕駛汽車以自認爲1m/step的恆定速度向右運動，車輛運動自己也存在偏差，即向車輛發出的控制命令是向右移動2m，而實際的車輛運動結果多是待在原地不動，可能向右移動1m，也可能向右移動3m。app

2.1 數學模型

從數學的語言來描述這個問題：機器人只有一個狀態變量：位置，。應用直方圖濾波(Histogram Filter)，對狀態空間作以下分解:dom

無人駕駛高精度定位技術(1)-直方圖濾波Histogram Filter

因而獲得一個新的狀態空間，它是對連續狀態空間的近似，在某一時刻車輛只能在這些離散狀態中的一個。ide

雖然車輛自認爲在向右運動，每一步運動1m，咱們假設存在5%的機率，無人駕駛汽車仍待在原地沒動；存在90%的機率車輛在向右移動1m；存在5%的機率無人駕駛汽車在向右運動2m。oop

無人駕駛汽車的Sensor假設90%的機率感知結果是正確的，還有10%的狀況下它感知的結果是錯誤的。idea

2.2 利用直方圖濾波(Histogram Filter)進行車輛定位的過程

咱們用一個5維的向量來描述t時刻，無人駕駛汽車位於第1個格子、第2個格子、第3個格子、第4個格子、第5個格子的機率。spa

無人駕駛汽車對本身所在位置一無所知，假設它連續三次【運動-向右走一步】-【觀測】,三次觀測結果分別是orange、blue、orange，咱們一步步無人駕駛汽車是如何經過【運動】-【觀測】過程逐步確認本身的位置的。

t=0時刻

沒有任何先驗知識，無人車不知道本身在哪裏,因此在各個位置機率相等：

t=1時刻

首先向右走1m，用運動模型進行位置預測

能夠看出無人車雖然向前運動一步，但它仍然對本身所在位置一無所知。這也和咱們的認知相同，剛開始徹底不知道本身在哪裏，走了一步天然也徹底不知道本身在哪裏。

再用更新模型經過Sensor感知環境，更新當前位置的置信度。

orange的顏色感知信息使得傳感器認爲本身極可能位於第二個和第五個方格中。

t=2時刻

運動模型-向右走1m

更新模型-sensor環境感知

t=3時刻

運動模型-向右走1m

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

能夠看到通過三次的運動和觀測後，無人駕駛汽車已經有73%的機率確認本身位於第二個網格中，事實再通過三次的運動觀測，無人駕駛汽車能夠有94%的機率確認本身的位置。

三、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%的機率能夠肯定本身所處的位置了。

四、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)

定位效果以下:

3

圖片來源:https://github.com/AtsushiSakai/PythonRobotics

參考連接

1.http://www.deepideas.net/robo...
2.https://github.com/AtsushiSakai/PythonRobotics

aHR0cHM6Ly91cGxvYWQtaW1hZ2VzLmppYW5zaHUuaW8vdXBsb2FkX2ltYWdlcy8xNzY1MDk3Mi1mYTNkOGJkZTViZDg4MmIxLnBuZw.jpeg

相關文章
相關標籤/搜索
每日一句
    每一个你不满意的现在，都有一个你没有努力的曾经。
本站公眾號
   歡迎關注本站公眾號,獲取更多信息
聯繫我們 最近搜索 最新文章 沪ICP备13005482号-10 MyBatis教程 SQL 教程 MySQL教程 Java 教程 Thymeleaf 教程 Hibernate教程 Spring教程 Redis教程