【文章內(nèi)容簡介】 機(jī)器人定位中。 貝葉斯濾波 （ Bayesian filter） BF(Bayesian Filter)是概率定位方法的理論基礎(chǔ)， BF 用傳感器測量數(shù)據(jù)去估計一個動態(tài)系統(tǒng)的未知狀態(tài)，其核心思想是：以當(dāng)前為止所收集的數(shù)據(jù)為條件，遞歸估計 狀態(tài)空間后驗(yàn)概率密度。 移動機(jī)器人定位的 系統(tǒng)狀態(tài)是機(jī)器人位姿（ yx, ,? ） ,其中（ yx, ）是機(jī)器人在二維 環(huán)境坐標(biāo)系中的坐標(biāo)， ? 是正對方向。往往稱 后驗(yàn)概率為置信度， 系統(tǒng)狀態(tài) xt 的置信度標(biāo)記為 ? ?xtBel =p( dxtt ?,0),其中 dt?,0是時刻 0 到時刻 t 收集的數(shù)據(jù) ， 表示 機(jī)器人在每一個位置 xt ?? （ ? 是機(jī)器人的所有位置 的 集合） 的概率分布。定位的目標(biāo)是使位置概率分布盡可能的接近實(shí)際分布，實(shí)際分布是一個以 實(shí)際位置為平均值的單峰高斯分布，如果實(shí)現(xiàn)了該目標(biāo)， 就實(shí)現(xiàn)了準(zhǔn)確定位。 移動機(jī)器人把數(shù)據(jù)通常分為兩類：感知數(shù)據(jù)，如聲納、激光等傳感器測距 9 數(shù)據(jù) ；里程計數(shù)據(jù)，它攜帶著機(jī)器人的移動信息 。這兩類數(shù)據(jù)分別用符號標(biāo)記為? 和 a 。為了不失一般性，假設(shè)機(jī)器人移動過程中這兩類數(shù)據(jù) 交替出現(xiàn)，則置信度 ? ?xtBel可表示為 [5]： ? ?xtBel =P? ?aaax ttttt 00211 ,, ??? ???? (1) 根據(jù)馬爾可 夫假設(shè)，如果當(dāng)前狀態(tài)已知，過去和未來測量獲得的數(shù)據(jù) 是條件獨(dú)立的，即： P? ?aaaxttttt 00211 ,, ??? ????=P（ axxttt 11, ??）（ 2） 應(yīng)用 以下兩個更新規(guī)則可以有效確定后驗(yàn)概率：當(dāng) 輸入一個新控制數(shù)據(jù) at1?時，可以根據(jù)式 （ 3）預(yù)測： ??????*xtBel ? ?? ??? axx kttP 11 , ? ?dx xBel tt 11 ??（ 3） 當(dāng) 收集到新感知數(shù)據(jù) ?t時，則根據(jù)公式（ 4） 正確估計： ? ?xtBel ? ?xo ttP?? ?????? *xtBel（ 4） 其中， ? 是一個標(biāo)準(zhǔn)化常數(shù)，以確保整個狀態(tài)空間置信度和為 1。 為了實(shí)現(xiàn)上述公式，必須知道兩個條件概率密度：概率 P? ?axxttt 1,1 ??表示系統(tǒng)的動態(tài)性，即輸入相應(yīng)控制信息，系統(tǒng)狀態(tài)怎樣變化，稱 為 運(yùn)動模型；概率 P? ?xtt?表示當(dāng)前狀態(tài) xt能感知數(shù)據(jù) ?t的可能性，稱為感知模型。其中隨機(jī)變量 x,? ,a 可以是高維向量。 為了實(shí)現(xiàn) BF，還需要詳細(xì)定義感知模型 P? ?xtt?和 P? ?axxttt 1,1 ??，以及合理表示置信度 ? ?xtBel。 卡爾曼濾波 卡爾曼濾波（ Kalman Filter,KF） 是估計線性動態(tài)系統(tǒng)狀態(tài)的遞歸數(shù)據(jù)處 理算法。 KF 不管獲得的 數(shù)據(jù)是否準(zhǔn)確，盡可能的處理所有 數(shù)據(jù) 獲得一個全局最優(yōu)估計 ，因此 KF 是最優(yōu)化的。假設(shè) KF可以用線性動態(tài)模型 描述系統(tǒng)狀態(tài)和測量數(shù)據(jù)： 10 wxx ttt 11 ?? ??? vxz ttt ??? （ 5） 其中， ix nt?為系統(tǒng)狀態(tài)，向量 iw nt ??1為系統(tǒng)噪聲， zt為測量數(shù)據(jù)， iv mt? 為測量噪聲 ， A、 H 分別是 n n? 和 nm? 矩陣。 KF 假設(shè) wt1?和 vt是條件獨(dú)立的 0均值高斯分布，系統(tǒng)初始 狀態(tài) x0是條件獨(dú)立的單峰高斯分布。 KF 通過預(yù)測和修正計算置信度 。在預(yù)測階段，先驗(yàn)狀態(tài)估 計 ^xt? 和先驗(yàn)誤差協(xié)方矩陣 pt?組成先驗(yàn)置信度 ???????xtBel,可以按照高斯分布表示 [6]，其式為： ???????xtBel =N( ^xt? ,pt? ) ^xt? =AE? ?zzxtt 111 , ?? ?+E? ?wt 1? pt?=E??????????????????? ? ???????? ?? ^^ xxxxttTtt(6) 在 修 正 階段 ， KF 跟 據(jù) 測量 數(shù) 據(jù)對 預(yù) 測階 段 的先 驗(yàn) 狀態(tài) 估 計證信度進(jìn)行修正，形成了后驗(yàn)x t ： ???????xtBel=N( pxtt??,^ ) ? ?vPHHpKPKPPxzKxxttHTttttttttttt?? ?????????????????1^^^)((7) 盡管有許多限制條件，但 KF 已經(jīng)取得了 很大的成功。它可以有效跟蹤機(jī)器人的位置。 KF 的一個關(guān)鍵優(yōu)點(diǎn)就是它的有效性，它的復(fù)雜度 是狀態(tài)空間和感知數(shù)據(jù) 維數(shù)的多項(xiàng)式函數(shù)。而且可以解決 SLAM 問題。 然而，許多動態(tài)系統(tǒng)和傳感器模型是非線性的，例如 全局定位的多峰值概率密度估計，線性 KF 是不能處理的。對于非線性估計問題，可以通過線性近似去解決 ，因而不是最優(yōu)的。相應(yīng)的方法有 PKF、 EKF 等。 11 馬爾可夫定位 概述 為了克服 KF 中單峰高斯分布假設(shè)，又提出了許多不同方法表示狀態(tài)的不確定性。其中馬爾可夫 定位能夠表示復(fù)雜的多峰值概率分布，實(shí)現(xiàn)全局定位。 它為 以觀測和動作為基礎(chǔ)的移動機(jī)器人的全局定位提供了一種通用的概率結(jié)構(gòu)，是對部分可 觀察馬爾可夫決策過程 POMDP 的直接應(yīng)用。 POMDP 概念來自運(yùn)籌學(xué)領(lǐng)域，主要用于解決感知和動作都具有不確定性的最優(yōu)決策問題。當(dāng)應(yīng)用POMDP 進(jìn)行規(guī)劃時，機(jī)器人活動環(huán)境的狀態(tài)通常用一個或幾個隨機(jī)變量表示，決策的性能用環(huán)境狀態(tài)的效用函數(shù)進(jìn)行評價。效用函數(shù)可以事先定義，例如可用機(jī)器人離目標(biāo)點(diǎn)的距離描述。決策的一個重要前提條件之一是機(jī)器人具有關(guān)于環(huán)境狀態(tài)的知識；但在實(shí)際的環(huán)境中，感興趣的環(huán)境狀態(tài)并不能直接觀察到，只能根據(jù)傳感器信息進(jìn)行評估。因此 POPDP 中一個重要內(nèi)容是狀態(tài)估計，它負(fù)責(zé)根據(jù)傳感器輸入數(shù)據(jù)和機(jī)器人所采 取的動作估計位于環(huán)境中的狀態(tài)。馬爾可夫定位既是對這種狀態(tài)估計器的應(yīng)用特例， 又 是通過計算環(huán)境空間中所有可能存在位置的概率分布函數(shù)來估計機(jī)器人位置的一種概率分析法。 馬爾可夫定位中，環(huán)境狀態(tài) 是機(jī)器人在環(huán)境中的位置；通常環(huán)境狀態(tài)用離散變量表示，如拓?fù)涔?jié)點(diǎn)或位置柵格。因此環(huán)境模型通常為拓?fù)鋱D或柵格圖。設(shè) Rk為機(jī)器人在 k 時刻的位置，是一隨機(jī)變量。 ? ?RKBel表示機(jī)器人在 k時刻位于 RK處的可信度，該可信 度表示了機(jī)器人位于整個環(huán)境空間 M的概率分布情況。此方法中用可信度概念而不直接用概率表示的目的是為了強(qiáng)調(diào)這是對機(jī)器人狀態(tài)的評估。方法中 Rk 可以是 ? ??,yx T 空間中的任一位置或拓?fù)淇臻g中的 某一節(jié)點(diǎn)。當(dāng)機(jī)器人運(yùn)動或接收到新的傳感器輸入數(shù)據(jù)時，狀態(tài) Rk 將被更新。為不失一般性，假設(shè)在每一個離散的時刻 k 機(jī)器人收到一次測量數(shù)據(jù) mk ,并給出一次動作命令 ak ，該動作完成后由里程計測量得到的運(yùn)動信息為 U? 。馬爾可夫定位的任務(wù)是隨著機(jī)器人的運(yùn)動，不斷地更新機(jī)器人位于位置 Rk 的可信度 12 ? ?RKBel ,并以可信度最大的位置作為對機(jī)器人實(shí)際的估計。 機(jī)器人在 k 時刻所處狀態(tài)的可信度以當(dāng)前時刻所有有效地測量和動作數(shù)據(jù)為基礎(chǔ)，因此可定義 ? ?RKBel=? ( RRK 1,… .,RK1?。m1,… .,mk1? 。a1,… ..ak1? )此式表示在已知 Rk， m?和 ak前面的 所有測量和動作數(shù)據(jù)時，機(jī)器人位于 Rk的可信度。這種條件概率的計算復(fù)雜度與條件變量個數(shù)成指數(shù)增長關(guān)系。在應(yīng)用Markov 定位方法時，系統(tǒng)必須滿足 Markov 條件假設(shè)。在介紹 Markov 定位算法之前，論文將詳細(xì)描述這兩個重要的 Markov 獨(dú)立性條件假設(shè)。這是應(yīng)用貝葉斯公式更新狀態(tài)變量的 R 概率估計的基礎(chǔ)。 馬爾可夫獨(dú)立性假設(shè) 馬爾可夫定位的關(guān)鍵是過程的無后效性假設(shè)，即馬爾可夫假設(shè)。這包括動作的獨(dú)立性假設(shè)和環(huán)境感知的獨(dú)立性假設(shè)兩方面。 1 動作的獨(dú)立性 動 作的獨(dú)立性是指機(jī)器人當(dāng)前時刻的狀態(tài) Rk僅與前一時刻的狀態(tài) Rk1?，以及動作 ak1?有關(guān)，即一旦 Rk1?狀態(tài)已知時 k1 時刻以前的所有動作和狀態(tài)對 k 時刻的狀態(tài)無影響。這一條件用概率表示可描述如下： ? ( RRK 1 ,? ,RK1? 。m1 ,? ,mk1? 。a1 ,? ak1? )=? ( RR KK 1? ,aK1? ) (41)這就表明，一旦已知機(jī)器人在 k1 時刻的位姿和在 k1 時刻執(zhí)行的運(yùn)動命令就足以預(yù)測機(jī)器人在 k 時刻的機(jī)器人位置。這一假設(shè)也是合理的，因?yàn)?k1時刻之前的動作和位姿狀態(tài)并沒有為 k時刻的機(jī)器人位姿提供任何附加的信息。 2 感知的獨(dú)立性 感知的獨(dú)立性是指機(jī)器人當(dāng)前狀態(tài)下的傳感器測量數(shù)據(jù) mk 僅依賴于當(dāng)前時刻 k的機(jī)器人狀態(tài)，與過去狀態(tài)下的測量無關(guān)，即過去的測量并不會向當(dāng)前的測量傳遞任何相關(guān)的信息。這樣一旦知道機(jī)器人在 k時刻的位置，其它的所有測量，即以前的狀態(tài)和動作都對 mk 條件概率的預(yù)測無影響。這一條件可用下式描述： ? ( Rmk 1 ,… ,RK 。m1 ,… .,mk1? 。a1 ,… ..ak1? )=? ( Rm Kk )(42) 13 這表明，所有的感知 數(shù)據(jù)與機(jī)器人在環(huán)境中的位置有關(guān)。例如，當(dāng)給定機(jī)器人的位置時，由距離傳感器測得的距離數(shù)據(jù)只與機(jī)器人到障礙物的距離有關(guān)，而與另一傳感器測得的距離無關(guān)。除定位外，這一假設(shè)也在移動機(jī)器人的地圖創(chuàng)建中得到廣泛應(yīng)用。 根據(jù)動作和感知獨(dú)立性假設(shè)，與定位相關(guān)的各個問題方面都可用狀態(tài)變量Rk 、條件概率 ? ( aRR KKK 11, ?? )和 ? ( Rm Kk )進(jìn)行建模。 馬爾可夫定位原理 馬爾可夫定位是根據(jù)機(jī)器人傳感器的感知數(shù)據(jù) mk和所采取的動作 ak確定機(jī)器人位于環(huán)境空間位置的可能性大小 ? ?RKBel。 ? ?RKBel越大，機(jī)器人實(shí)際位于 Rk處的可能性越大。條件概率 ? ?RKBel的計算是一個遞推的貝葉斯估計 過程，應(yīng)用貝葉斯公式，機(jī)器人在 k時刻位于 Rk的可信度可用式 43表示。 ? ?RKBel =? ( amR kkK .~~,1.~~,1 , ) =)。()。()。(1~~,11~~~,11~~~,11~~,11~~~,11~~,1 amm amRamRmKKkKKKKKKK??????? ?? (43) 式中， Rk表示在 k時刻機(jī)器人的位姿狀態(tài)， mk表示外部傳感器感知數(shù)據(jù)， ak是機(jī)器人在 k 時刻采取的控制動作，有時也可認(rèn)為是里程計測量數(shù)據(jù)而直接用UK 代替，因?yàn)闄C(jī)器人執(zhí)行動作命令后的運(yùn)動信息是靠里程計提供的， ak 也可認(rèn)為是反映了 k1 時刻執(zhí)行控制動作后由里程計在 [k,k1]時間段內(nèi)由里程計讀入的信息。 式 43對機(jī)器人狀態(tài)可信度的估計是遞推進(jìn)行的，系統(tǒng)初始狀態(tài)的可信度知識體現(xiàn)了定位的特點(diǎn)。如果初始位置是確定的，即 ? ?RBel 0 =1 時，為位姿跟蹤過程。如果初始時刻 機(jī)器人對其所處位置一無所知，那么初始狀態(tài)的概率分布函數(shù)可用均勻分布表示，此時相應(yīng)于全局定位問題。式 43 中分母相對于 Rk 來說是一常數(shù)，可理解為歸一化因子，其作用是確保在所有可能位置處的概率之和為 1。為了簡化書寫，此常數(shù)常用 ? 表示。 14 ?1? =? ( amm kKk 1,11,