自動駕駛使用粒子濾波定位

前幾章介紹了卡爾曼濾波是用來對其他車輛追蹤和定位的，本章介紹的粒子濾波是用來定位自身的。通常我們用手機地圖的gps功能已經能大致定位出自身位置，但是gps存在10m左右的偏差，這麼大的偏差在自動駕駛時是無法承受的。本章介紹的粒子濾波是一種高精度定位自身位置的演算法。

粒子濾波的原理概括來說是通過地圖得到一些路標的座標，假設自身車輛以某種概率分布在一定範圍內，在這個範圍內按概率分布假設一些粒子代表車輛位置，根據車輛運動情況**每個粒子δ

t\delta t

δt以後的位置，車輛每隔δ

t\delta t

δt的時間還會用感測器感知路標相對自己的距離，用每個粒子周圍路標的位置和感測器感知的路標位置做比較，感測器感知的路標和探測到的路標越接近則該粒子越有可能代表車輛位置。從**到探測更新迴圈迭代，確認車輛的實際位置。

粒子濾波可以抽象為以下幾步：

初始化粒子

**探測更新

重新分布粒子，重複2， 3步

下圖是自動駕駛車輛使用粒子濾波的一般流程

粒子濾波是一種比卡爾曼濾波更加簡單的演算法，下面對每一步進行更詳細的說明並給出原始碼實現加深理解。

第一步是撒粒子，但是粒子應該撒到什麼位置呢？又該按什麼規律撒呢？和其他感測器一樣，gps資料應該也滿足正態分佈。所以先通過gps得到乙個位置不精確的位置座標(x,

y)(x, y)

(x,y

)，用正態分佈的引擎以該位置為均值，以gps裝置雜訊為方差隨機生成若干粒子。粒子數量是精度和計算時間的均衡，本次專案共生成300個粒子。對每個粒子依次編號，初始權重為1。

void particlefilter::init(double x, double y, double theta, double std) 
num_particles = particles.size();
is_initialized = true;
}

由於是定位自己的車輛，所以速度，方向都是已知量，根據狀態方程計算δ

t\delta t

δt後每個粒子的位置即可，需要注意的是即使是自身速度和方向的值，也可能存在一定誤差，所以每個粒子的**值依然要考慮雜訊影響。

void particlefilter::prediction(double delta_t, double std_pos, double velocity, double yaw_rate) 
return;
}

我們將能代表特定座標的物體稱為路標（map_landmark），並且這些路標在地圖中都是標明的，即座標已知且精確。自身車輛通過感測器探測路標能得到一組變數，代表了某個路標和車輛的距離，我們逐個選取**以後的粒子，以該粒子為座標中心，就可以將距離變數對映到地圖座標中。對映以後的座標和路標都在地圖座標系中可以相互比較，將距離最近感測器座標和路標關聯起來。

對每個粒子計算其探測到的路標和真實路標的位置差異的逆作為該粒子的權重。粒子的座標位置和真實車輛的座標位置越接近，則探測到的路標和真實路標位置也越接近，該粒子的權重就越大，反之相差越大權重越小。

對所有粒子權重歸一化處理，使所有粒子權重值和為1.

double particlefilter::calculateparticleweight(double sensor_range, vectorobservations, map map_landmarks, double sigma_x_2sq, double sigma_y_2sq, double outer_term, particle& particle)
double sum = 0;
for (int i = 0; i < weights.size(); i++)
sum+=weights[i];
for (int i = 0; i < weights.size(); i++)
weights[i] = weights[i]/sum;
return particle.weight;
}void particlefilter::dataassociation(std::vectorpredicted, std::vector&observations) }}
return ;
}void particlefilter::updateweights(double sensor_range, double std_landmark,
vector&observations, const map &map_landmarks)

通過為每個粒子分配權重我們知道有的粒子更可能接近真實的車輛位置，但是由於**和更新都存在一定誤差，我們也不想直接去掉權重小的粒子，於是使用了一中根據權重大小的分布來選中重取樣粒子的方法。我們將所有粒子組成乙個餅狀圖，權重越大，其所佔餅的面積就越大，然後在以某一步幅在餅狀圖上迴圈取樣，落在哪個權重區間就將其代表的粒子作為重取樣粒子。這樣權重最大的粒子就可能多次被選中作為重取樣粒子。由於**會引入高斯雜訊，所以這些重取樣粒子又會在該粒子周圍呈高斯分布。

通過以上步驟迴圈濾波，最終越接近真實位置的粒子分布越密集，從而有效定位出車輛位置。重取樣**如下：

void particlefilter::resample() 
particles_temp.push_back(particles[index]);
}particles.clear();
for( int i = 0; i< num_particles; i++) 
return;
}

自動駕駛使用粒子濾波定位

自動駕駛汽車與自動駕駛網路

自動駕駛相關

自動駕駛相關

自動駕駛 使用粒子濾波定位

自動駕駛汽車與自動駕駛網路

自動駕駛相關

自動駕駛相關

相關推薦

自動駕駛使用粒子濾波定位