台湾 || 语言: 大陆简体港澳繁體台灣正體

粒子濾波SLAM

雪花台灣 2019-05-12 17:30

一、解決什麼問題

SLAM問題要解決的是在未知環境中，建立環境地圖同時在地圖中確定自身的位置。用概率的方法可以將這個問題寫為：

$p(x_{1:t},m|z_{1:t},u_{1:t})$

其中，x表示軌跡；m表示地圖，由於通常假設環境是靜態的不隨時間發生變化所以地圖m無下標1:t；z表示每個時刻對環境的觀測，在使用測距感測器(lidar,radar,深度相機)時，觀測指返回的障礙物距離和角度信息；u表示對平台施加的控制，通常來自輪速記，里程計，慣導等。

在整個問題的處理中，將運動、觀測過程假設為Markov過程，即當前時刻的觀測至於當前時刻的位置和m有關，當前時刻的位置只與上一時刻的位置和控制有關，常用貝葉斯網路來表示這一過程。

二、R-B 分解

用概率相乘的方式先將(1)式分解為：

$p(x_{1:t},m|z_{1:t},u_{1:t})=p(m|z_{1:t},u_{1:t},x_{1:t})p(x_{1:t}|z_{1:t},u_{1:t})=p(m|z_{1:t},x_{1:t})p(x_{1:t}|z_{1:t},u_{1:t})$

這一分解成為Rao-Blackwellized分解，將同時對軌跡和地圖的估計問題轉化為對地圖和對軌跡分別估計。

粒子濾波被用於解決在已知觀測和控制信息情況下對軌跡的估計，有了這個估計後，剩下的部分變成一個已經位置的建圖問題即下式。

當前空間的維數與時間t相關，是一個高維空間，很難利用粒子有效採樣。這個問題通過序貫採樣(SIR,sampling importance resamling)解決。

三、SIR

序貫採樣我理解為把整個需要採樣的式子寫為遞推的形式，即

$p(x_{1:t}|z_{1:t},u_{1:t})$

$=p(x_{t}|x_{1:t-1},z_{1:t},u_{1:t})p(x_{1:t-1}|z_{1:t},u_{1:t})$

$=frac{p(z_{t}|x_{t},x_{1:t-1},u_{1:t},z_{1:t-1})p(x_{t}|x_{1:t-1},u_{1:t},z_{1:t-1})}{p(x_{1:t-1},u_{1:t},z_{1:t})/p(x_{t},x_{1:t-1},u_{1:t},z_{1:t-1})}p(x_{1:t-1}|z_{1:t-1},u_{1:t-1})$

$=eta p(z_{t}|x_{t},m)p(x_{t}|x_{t-1},u_{t})p(x_{1:t-1}|z_{1:t-1},u_{1:t-1})$

第一個等號利用概率的乘法分解出t-1時刻的遞推形式

第二個等號利用貝葉斯定理，得到觀測模型和運動模型的形式

第三個等號利用Markov過程去掉不相關的條件

四、粒子濾波的採樣和權重計算

粒子濾波就是用有限個隨機採樣的粒子集來表示概率分布的一種方法。經過上述序貫採樣的分解，我們只需要對 $p(z_{t}|x_{t},m)p(x_{t}|x_{t-1},u_{t})$ 進行採樣即可。

一種基本的方法是，通過運動模型進行採樣，即乘積中後一個式子。在得到的不同粒子中用觀測模型計算每個粒子的權重，加權和即可得到平台位置的估計。

1.運動模型

在不同的車輛模型中不同，常見的模型通過控制不同車輪速度進行轉向等。

在本文中為了方便進行模擬，在採樣中，直接假設存在控制向量u

通過某一時刻的控制則可採樣得到下一時刻的位置分布，假設控制雜訊是零均值的高斯雜訊則控制模型可以寫成：

$x_{t} sim N(x_{t-1} + u_{t-1} , sigma^2 )$

如圖所示：

相关文章