自動駕駛定位技術(shù)-粒子濾波實踐案例解析

Particle Filter - Kidnapped vehicle project

1. Definition of Particle Filter

粒子濾波器是貝葉斯濾波器或馬爾可夫定位濾波器的實現(xiàn)。粒子過濾器基于“適者生存的原理”主要用于解決定位問題。粒子濾波的優(yōu)勢在于易于編程并且靈活。 三種濾波器的性能對比:

?

? 正如你在上面的圖片中看到的，紅點是對機器人可能位置的離散猜測。每個紅點都有 x 坐標(biāo)、 y 坐標(biāo)和方向。粒子濾波器是由幾千個這樣的猜測組成的機器人后驗信度表示。一開始，粒子是均勻分布的，但過濾器使他們生存的比例正比于粒子與傳感器測量的一致性。

權(quán)重(Weights)：

粒子濾波器通常攜帶離散數(shù)量的粒子。每個粒子都是一個包含 x 坐標(biāo)、 y 坐標(biāo)和方向的矢量。顆粒的存活取決于它們與傳感器測量結(jié)果的一致性。一致性是基于實際測量和預(yù)測測量之間的匹配度來衡量的，這種匹配度稱為權(quán)重。 ?

? 權(quán)重意味著粒子的實際測量與預(yù)測測量的接近程度。在粒子濾波器中，粒子權(quán)重越大，生存概率越高。換句話說，每個粒子的生存概率與權(quán)重成正比。 ?

重采樣(Resampling)

重采樣技術(shù)是用于從舊粒子中隨機抽取N個新粒子，并根據(jù)重要權(quán)重按比例進行置換。重采樣后，權(quán)重較大的粒子可能會停留下來，其他粒子可能會消失。粒子聚集在后驗概率比較高的區(qū)域。 為了進行重采樣，采用了重采樣輪技術(shù). ?

?

? 原理：每個粒子被選中的概率都和這個粒子輪所占的周長成正比，權(quán)重大的粒子有更多的機會被選中。 初始index為6，假設(shè)隨機的beta= 0 + 隨機權(quán)重> w6, 則index +1，beta=beta-w6. 此時beta < w7, 7號粒子被選中添加到倉庫中。之后進行下一輪循環(huán)，此時beta 和 index 仍然保留前一輪循環(huán)的值, beta= beta + 隨機權(quán)重 > w7 + w8, 因此index遞增兩次，到達index=1，此時w1 > beta, w1被選中放入倉庫中, 隨后進行下一輪循環(huán)。 重采樣的代碼：

p3 = []
index= int(random.random()*N)
beta=0.0
mw=max(w)
for i in range(N):
    beta +=random.random()*2.0*mw
    while beta>w[index]:
        beta-=w[index]
        index=(index+1)%N
    p3.append(p[index])
p=p3

?2. Particle Filters implementation

? 粒子過濾器有四個主要步驟:

初始化步驟: 我們從 GPS 輸入估計我們的位置。在這個過程中的后續(xù)步驟將完善這個估計，以定位我們的車輛

預(yù)測步驟: 在預(yù)測步驟中，我們添加了所有粒子的控制輸入(偏航速度和速度)

粒子權(quán)重更新步驟: 在更新步驟中，我們使用地圖地標(biāo)位置和特征的測量更新粒子權(quán)重

重采樣步驟: 在重采樣期間，我們將重采樣 m 次(m 是0到 length_of_particleArray的范圍)繪制粒子 i (i 是粒子index)與其權(quán)重成正比。這一步使用了重采樣輪技術(shù)。

新的粒子代表了貝葉斯濾波后驗概率。我們現(xiàn)在有一個基于輸入證明的車輛位置的精確估計。

偽代碼：

1. 初始化步驟:

粒子過濾器的第一件事就是初始化所有的粒子。在這一步，我們必須決定要使用多少粒子。一般來說，我們必須拿出一個好的數(shù)字，因為如果不會太小，將容易出錯，如果太多會拖慢貝葉斯濾波器的速度。傳統(tǒng)的粒子初始化方式是把狀態(tài)空間劃分成一個網(wǎng)格，并在每個單元格中放置一個粒子，但這種方式只能適合小狀態(tài)空間，如果狀態(tài)空間是地球，這是不合適的。因此用 GPS位置輸入來初始估計我們的粒子分布是最實用的。值得注意的是，所有傳感器的測量結(jié)果必然伴隨著噪聲，為了模擬真實的噪聲不可控情況，應(yīng)考慮給本項目的初始 GPS 位置和航向添加高斯噪聲。 項目的最終初始化步驟代碼：
void ParticleFilter::init(double x, double y, double theta, double std[]) {

    /**
     * TODO: Set the number of particles. Initialize all particles to
     *   first position (based on estimates of x, y, theta and their uncertainties
     *   from GPS) and all weights to 1.
     * TODO: Add random Gaussian noise to each particle.
     * NOTE: Consult particle_filter.h for more information about this method
     *   (and others in this file).
     */

    if (is_initialized) {
        return;
    }
    num_particles = 100;  // TODO: Set the number of particle

    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // Normal distributions
    normal_distribution dist_x(x, std_x);
    normal_distribution dist_y(y, std_y);
    normal_distribution dist_theta(theta, std_theta);

    // Generate particles with normal distribution with mean on GPS values.
    for (int i = 0; i < num_particles; ++i) {
        Particle pe;
        pe.id = i;
        pe.x = dist_x(gen);
        pe.y = dist_y(gen);
        pe.theta = dist_theta(gen);
        pe.weight = 1.0;
        particles.push_back(pe);
        
    }
    is_initialized = true;
}

? 2. 預(yù)測步驟： 現(xiàn)在我們已經(jīng)初始化了粒子，是時候預(yù)測車輛的位置了。在這里，我們將使用下面的公式來預(yù)測車輛將在下一個時間步驟，通過基于偏航速度和速度的更新，同時考慮高斯傳感器噪聲。 ?

? 項目的最終預(yù)測步驟代碼:

for (int i = 0; i < num_particles; i++) { if (fabs(yaw_rate) >= 0.00001) { particles[i].x += (velocity / yaw_rate) * (sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta)); particles[i].y += (velocity / yaw_rate) * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t)); particles[i].theta += yaw_rate * delta_t; } else { particles[i].x += velocity * delta_t * cos(particles[i].theta); particles[i].y += velocity * delta_t * sin(particles[i].theta); } // Add noise particles[i].x += disX(gen); particles[i].y += disY(gen); particles[i].theta += angle_theta(gen); }

3. 更新步驟:

現(xiàn)在，我們已經(jīng)將速度和偏航率測量輸入納入到我們的過濾器中，我們必須更新基于激光雷達和雷達地標(biāo)讀數(shù)的粒子權(quán)重。 更新步驟有三個主要步驟:

Transformation

Association

Update Weights

轉(zhuǎn)換 (Transformation) 我們首先需要將汽車的測量數(shù)據(jù)從當(dāng)?shù)氐钠囎鴺?biāo)系轉(zhuǎn)換為地圖上的坐標(biāo)系。

通過傳遞車輛觀測坐標(biāo)(xc 和 yc)、地圖粒子坐標(biāo)(xp 和 yp)和我們的旋轉(zhuǎn)角度(- 90度) ，通過齊次變換矩陣，車輛坐標(biāo)系中的觀測值可以轉(zhuǎn)換為地圖坐標(biāo)(xm 和 ym) 。這個齊次的變換矩陣，如下所示，執(zhí)行旋轉(zhuǎn)和平移。

矩陣乘法的結(jié)果是:

代碼

 double x_part, y_part, x_obs, y_obs, theta;
  double x_map;
 x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
  double y_map;
 y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);

備注: 黑色方框是一個粒子，我們需要更新他的權(quán)重，(4,5) 是它在地圖坐標(biāo)中的位置，它的航向是(-90度)，由于傳感器對路標(biāo)的測量結(jié)果是基于車輛本身坐標(biāo)，因此我們要把車輛的觀察數(shù)據(jù)轉(zhuǎn)換為地圖坐標(biāo)。如L1路標(biāo)的真實地圖坐標(biāo)是(5,3)，車輛傳感器測得的OBS2的車輛坐標(biāo)為(2,2), 經(jīng)過齊次矩陣轉(zhuǎn)換后的地圖坐標(biāo)是(6,3), 現(xiàn)在我們就可以將測量結(jié)果與真實結(jié)果聯(lián)系起來，匹配現(xiàn)實世界中的地標(biāo), 從而更新黑色方框粒子的權(quán)重。
?聯(lián)系 (Association )

聯(lián)系問題是在現(xiàn)實世界中地標(biāo)測量與物體匹配的問題，如地圖地標(biāo). 我們的最終目標(biāo)是為每個粒子找到一個權(quán)重參數(shù)，這個權(quán)重參數(shù)代表這個粒子與實際汽車在同一位置的匹配程度。

現(xiàn)在已經(jīng)將觀測值轉(zhuǎn)換為地圖的坐標(biāo)空間，下一步是將每個轉(zhuǎn)換后的觀測值與一個地標(biāo)識符關(guān)聯(lián)起來。在上面的地圖練習(xí)中，我們總共有5個地標(biāo)，每個都被確定為 L1，L2，L3，L4，L5，每個都有一個已知的地圖位置。我們需要將每個轉(zhuǎn)換觀察 TOBS1，TOBS2，TOBS3與這5個識別符之一聯(lián)系起來。為了做到這一點，我們必須將最接近的地標(biāo)與每一個轉(zhuǎn)化的觀察聯(lián)系起來。
TOBS1?= (6,3),?TOBS2?= (2,2) and?TOBS3?= (0,5). OBS1匹配L1，OBS2匹配L2，OBS3匹配L2或者L5(距離相同)。
下面的例子來解釋關(guān)于數(shù)據(jù)關(guān)聯(lián)的問題。

在這種情況下，我們有兩個激光雷達測量巖石。我們需要找出這兩個測量值中，哪一個與巖石相對應(yīng)。如果我們估計，任何測量是真實的，汽車的位置將根據(jù)我們選擇的測量是不同的。也就是說，根據(jù)路標(biāo)選擇的不同, 最終確定的車輛位置也會不同。
由于我們有多個測量的地標(biāo)，我們可以使用最近鄰技術(shù)找到正確的一個。

在這種方法中，我們將最接近的測量作為正確的測量。

最緊鄰法的優(yōu)缺點:

更新權(quán)重 (update weights)
現(xiàn)在我們已經(jīng)完成了測量轉(zhuǎn)換和關(guān)聯(lián)，我們有了計算粒子最終權(quán)重所需的所有部分。粒子的最終權(quán)重將計算為每個測量的多元-高斯概率密度的乘積。
多元-高斯概率密度有兩個維度，x 和 y。多元高斯分布的均值是測量的相關(guān)地標(biāo)位置，多元高斯分布的標(biāo)準差是由我們在 x 和 y 范圍內(nèi)的初始不確定度來描述的。多元-高斯的評估基于轉(zhuǎn)換后的測量位置。多元高斯分布的公式如下。

備注：x, y 是地圖坐標(biāo)系的觀測值，μx,?μy是最近的路標(biāo)的地圖坐標(biāo)。如對于OBS2 (x,y) =(2,2), (μx,?μy)= (2,1)
誤差計算公式：

?

其中x, y, theta代表最好的粒子位置，xmeas, ymeas, thetameas代表真實值

?

?

?

3. Project Demo

編輯：黃飛

?