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

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

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

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

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

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

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

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