粒子濾波在車輛定位中的應用

粒子濾波算是乙個非常經典的演算法了，具體的思想步驟不再詳述，這裡和這裡講的都比較詳細了。

下圖就是演算法的偽**，分為以下幾個步驟：

下圖是粒子濾波車輛定位演算法的流程圖。

在車輛定位問題中，不能將所有點的分布視為平均分布，比較好的辦法就是根據gps的資訊，並考慮到感測器的高斯雜訊來隨機撒點。具體的做法就是將gps讀取的資訊作為均值，感測器高斯雜訊作為方差，讓所有點服從這兩個引數下的正態分佈，**如下：

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

更新需要根據車輛的運動學方程，勻速運動的車不必多說，單位時間內的位置更新就是簡單的速度*夾角正弦或者余弦。如果是變速運動，那麼用如下公式計算：

當然，我們仍然需要疊加上高斯雜訊，具體**如下：

void particlefilter::
prediction
(double delta_t,
double std_pos,
double velocity,
double yaw_rate)
else
// 最後疊加上高斯雜訊
p.x +
=new_x
(gen)
;        p.y +
=new_y
(gen)
;        p.theta +
=new_theta
(gen);}
}

在更新之前，我們還需要解決乙個問題。眾所周知無人駕駛中，我們通常通過lidar或者radar等感測器獲取位置資訊，那麼這些感測器就一定會存在誤差。以lidar為例，如下所示，藍色點表示物體的位置，黃色點表示lidar檢測返回的點雲資料點。那麼我們如何確定哪乙個雷達點返回的是我們定位所需的landmark所反射回來的呢？

乙個很簡單的方法就是用最鄰近點表示，也就是圖中紅色圈出的部分，用檢測到的最鄰近的點表示landmark所在位置。這樣做的好處毫無疑問就是理解簡單，程式設計簡單，並且能解決大部分場景的問題。但是問題也是突出的，那就是魯棒性不夠，在landmark周圍點雲密集時，當感測器存在較大雜訊時等場景都會出現誤差較大的情況。以下是**實現：

void particlefilter::
dataassociation
(vector predicted, 
vector
& observations)}}
}

下面就是更新權重的步驟了，那麼權重如何計算呢，可以利用多元高斯公式，如下：

**的總體思路就是針對每乙個粒子，首先找到距離範圍內可用的landmark，再利用最近點法找到相應的測量值，用測量值和真值利用多元高斯公式計算權重進行更新。其中還要注意的是，測量的座標系是車，也就是行進方向為x，左為y；landmark座標系是地圖座標系，要進行轉換。

void particlefilter::
updateweights
(double sensor_range,
double std_landmark,
const vector
&observations,
const map &map_landmarks));
}}// 車座標系和map座標系轉換
vector observations_map;
double cos_theta =
cos(p.theta)
;double sin_theta =
sin(p.theta)
;for
(const
auto
&obs : observations)
dataassociation
(predictions, observations_map)
;for
(const
auto
&obs_m : observations_map)
weights.
push_back
(p.weight);}
}

最後來到了重取樣這一步，這一步思想上很簡單，那就是更新粒子群的集合，更新的準則就是權重大的有更大的機會被選入。c++中的discrete_distribution可以很好地實現這一目的。**如下：

void particlefilter::
resample()
// assign the resampled_particles to the previous particles
particles = resampled_particles;
// clear the weight vector for the next round
weights.
clear()
;}

可以看到粒子濾波很好地實現了二維平面上小車的定位問題。

粒子濾波在車輛定位中的應用

粒子濾波在目標跟蹤的應用

字元結構知識在車牌識別中的應用

字元結構知識在車牌識別中的應用

粒子濾波在車輛定位中的應用

粒子濾波在目標跟蹤的應用

字元結構知識在車牌識別中的應用

字元結構知識在車牌識別中的應用

相關推薦