迎賓機器人的快速同時定位與地圖生成(FastSLAM)：機器人定位和特征標(biāo)志的位置估計

為了克服基于擴展卡爾曼濾波器 SLAM方法的缺點，Montemerlo等人提出了一種基 于 Rao -Blackwellized 粒子濾波器的快速 SLAM 算法，并稱為 FastSLAM68][69] 。 FastSLAM 將 SLAM分解為機器人定位和特征標(biāo)志的位置估計兩個過程。粒子濾波器中 的每個粒子代表機器人的一條可能運動路徑，利用觀測信息計算每個粒子的權(quán)重，以評價 每條路徑的好壞。對于每個粒子來說，機器人的運 動路徑是確定的，因此特征標(biāo)志之間相互d立，特 征標(biāo)志的觀測信息只與機器人的位姿有關(guān)，每個粒 子可以采用N 個卡爾曼濾波器分別估計地圖中N 個特征的位置。假設(shè)需要k 個粒子實現(xiàn) SLAM, FastSLAM總共有kn 個卡爾曼濾波器。 FastSLAM 的時間復(fù)雜度為0(kn), 通過利用樹型的數(shù)據(jù)結(jié)構(gòu) 進行優(yōu)化，其時間復(fù)雜度可以達到0(klog₂n)。

FastSLAM方法的另一個主要優(yōu)點是通過采用粒子濾波器估計機器人的位姿，可以很好地表示機器人的非線性、非高斯運動模型。圖1.57給出了在 FastSLAM 實 驗 中 生 成 的 地 圖 。

由于粒子濾波器的采樣實際上是從運動模型決定的提議分布中抽取的，如果提議分 布和實際的后驗分布的形狀相似，那么根據(jù)提議分布抽取的采樣在利用權(quán)值進行補償后 離散的采樣能夠很好地表示后驗分布。但是，如果提議分布與實際后驗分布相差加大，權(quán) 重函數(shù)將位于提議分布密度的尾端，這將會導(dǎo)致在權(quán)重取值較大的區(qū)域采樣很少，此時離 散采樣所表示的概率分布與實際的后驗分布存在著較大的差別，從而導(dǎo)致粒子濾波器的 精度很低，需要大量的采樣才能較好地表示后驗分布。

粒子濾波器的另一個問題是早熟現(xiàn)象，即經(jīng)過若干次迭代之后，大多數(shù)的采樣權(quán)重都 趨于零，從而只有少數(shù)的采樣真正對系統(tǒng)的狀態(tài)估計起作用。雖然通過重新采樣可以在 一定程度上避免這種現(xiàn)象，但是由于權(quán)重大的采樣會被多次復(fù)制，權(quán)重小的采樣會被忽 略，這樣經(jīng)過若干次迭代后采樣會集中在某一個較小的區(qū)域。例如，在對稱性較高的結(jié)構(gòu) 化環(huán)境中，完成機器人的定位，需要長時間跟蹤多個機器人的位姿假設(shè)，產(chǎn)生錯誤的定位 結(jié)果。

針對粒子濾波器存在的缺陷，一些研究者提出了改進的方法。為了使采樣能夠更好 地表示系統(tǒng)的后驗分布，Thrun等人將似然函數(shù)也作為提議分布的一部分，提出了混合形 式的提議分布，按照這種提議分布抽取的采樣能夠融合當(dāng)前的觀測信息，因此能夠更好地 表示系統(tǒng)的后驗分布，但是這種方法使采樣階段的計算量大大增加。為了減少粒子濾波 器的計算量，提高效率，F(xiàn)ox 等人提出了自適應(yīng)粒子濾波器，這種基于Kullback-Leibler

距離(KLD) 取樣的粒子濾波器能夠根據(jù)系統(tǒng)狀態(tài)的不確定性自適應(yīng)調(diào)整采樣數(shù)的多 少。蔣正偉[70]等人采用連續(xù)窗口濾波法根據(jù)估計狀態(tài)不確定性刷新采樣粒子數(shù)，通過仿 真實驗成功地解決了機器人定位非線性、非高斯分布的狀態(tài)估計問題。Hahnel71 等人通 過掃描匹配算法修正了里程計的讀數(shù)，改進了機器人的運動模型，獲得了較好的提議分 布，有效地完成了大范圍環(huán)境下的地圖創(chuàng)建。Grisetti[72]等人根據(jù)激光測距傳感器感知 環(huán)境時具有較高的峰值特點，假定感知模型和運動模型混合形成新的自適應(yīng)提議分布仍 然為高斯分布，不僅融合了當(dāng)前的觀測值，而且減少粒子濾波器的計算量，采用了自適應(yīng) 的運動模型來決定提議分布，根據(jù)提議分布和后驗分布的差別，收縮或者擴張?zhí)嶙h分布， 也較好地估計了后驗分布。

SLAM算法的提出和應(yīng)用得益于機器人硬件設(shè)備的推廣。目前較為流行的 FastSLAM 同樣離不開如激光、聲納等非視覺傳感器獲取的信息。從地圖創(chuàng)建的角度考慮 FastSLAM,地圖信息可能是記錄的路標(biāo)位置，也有可能是某一柵格存在障礙物的可能 值。在大規(guī)模未知環(huán)境尤其是擁擠的非結(jié)構(gòu)化環(huán)境中，通常不存在人工路標(biāo)供機器人提 取和匹配，由機器人提取自然特征作為地圖路標(biāo)是要的選擇。從激光傳感器信息中提 取的線段、曲線或拐角特征卻難以區(qū)別于其他地點獲取的同樣特征，這就為FastSLAM 在 地圖創(chuàng)建中的數(shù)據(jù)關(guān)聯(lián)提出了難題。從實際應(yīng)用的角度來看，地圖創(chuàng)建的目的是為了定 位和路徑規(guī)劃，當(dāng)機器人被放置于已知的大規(guī)模環(huán)境中而初始位姿未知時，根據(jù)激光傳感 器獲取的直線或拐角特征很難讓機器人在地圖中找到相應(yīng)的坐標(biāo)。所以，在FastSLAM 創(chuàng)建的地圖中實現(xiàn)機器人的“誘拐恢復(fù)”是一個極富挑戰(zhàn)性的問題。