基于全景視覺機器人的粒子群優(yōu)化FastSLAM算法研究

摘 要：針對FastSLAM算法具有粒子退化及重采樣帶來的貧化問題，文章提出了基于全景視覺機器人的粒子群優(yōu)化FastSLAM算法。首先，利用粒子群優(yōu)化算法更新預(yù)估粒子，實現(xiàn)全景視覺機器人在預(yù)估位置時能夠利用觀測到的最新的路標(biāo)信息，粒子群中粒子的相互作用降低了所需粒子的數(shù)量，使FastSLAM算法避免了粒子耗盡。實驗證明，當(dāng)粒子數(shù)相同時，文章采用的改進FastSLAM算法的全景視覺機器人定位精度明顯優(yōu)于傳統(tǒng)的FastSLAM算法。

關(guān)鍵詞：全景視覺；機器人；粒子群優(yōu)化；SLAM算法

全景視覺傳感器以其360°的感知范圍獲得更多的圖像信息進而更完整地表達機器人所在的環(huán)境，增強了機器人的對視覺路標(biāo)的持續(xù)觀測能力和地圖創(chuàng)建能力，提高了機器人對地圖的利用率，有利于系統(tǒng)不確定性的收斂等特點在未知環(huán)境的SLAM（Simultaneous location and mapping）[1]問題中被廣泛應(yīng)用且取得了很好的效果[2]。

傳統(tǒng)解決SLAM問題的方法通常依據(jù)卡爾曼濾波[3]，其最大的優(yōu)點是意義明確、簡潔易實現(xiàn)，但比較適合小尺度的環(huán)境，對于大尺度的室外環(huán)境，算法的運算量過大，無法滿足應(yīng)用需求。

Montemerlo等將Rao-Blackwellised粒子濾波融合到EKF算法中，給出了FastSLAM算法的解決方案，相比基于卡爾曼濾波的SLAM算法計算量大大降低。但FastSLAM算法的粒子退化問題仍難以解決，為此利用重采樣的方式進行改進，雖然減輕了粒子退化，但卻帶來了粒子貧化問題。為此，文章引入了粒子群優(yōu)化算法，很好的解決了粒子的貧化問題。

1 FastSLAM算法基本原理和粒子群優(yōu)化的原理

1.1 FastSLAM算法原理介紹

FastSLAM算法的運動模型和觀測模型一般用下面的形式進行描述：

式中，v（k）和？棕（k）為Gaussian白噪聲，其協(xié)方差分別記為R（k）和Q（k）；f和h為非線性函數(shù)；Xr（k）為k時刻全景視覺移動機器人位姿；xn（k）為n（k）的路標(biāo)的信息；Z（k）、u（k）為k時刻全景視覺移動機器人的觀測值和運動值。

使用FastSLAM算法進行同時定位與地圖創(chuàng)建的核心問題是用后驗概率分布來估計全景視覺移動機器人的運動軌跡X和創(chuàng)建的地圖Xn（k），表示為：

（3）

根據(jù)Bayes規(guī)則和Markov假設(shè)可得，

式（4）的采用Murphy and Russell[4]在1999年提出的因式分解法。

1.2 粒子群優(yōu)化算法

在維數(shù)為n的空間里，隨機一個粒子群，其中的任一粒子表示我們所要解決的問題的可能的答案，共m個。V表示第i個粒子在t時間在搜索空間里的移動速度，V表示該粒子在t時間的位置。所有粒子都根據(jù)該時刻粒子群里的最優(yōu)粒子，來進行迭代搜索。

迭代過程中，每個粒子在不同時刻的位置和速度都會根據(jù)下面兩個極值得到：粒子群體最好位置為gBest（即整個粒子群的最優(yōu)解），粒子自身的最好位置為pBest（即粒子自己的最優(yōu)解）。若粒子群中總共包含n個粒子，每個粒子的位置和速度可以由下面的方程（5）和（6）得到。

（5）

（6）

式中w為慣性權(quán)重，通常w是伴隨著迭代次數(shù)的增加而逐漸變小，w越小算法的搜索能力越小，在值小到一定程度時，只能具備在局部進行搜索的能力，w越大算法的搜索能力越強，能實現(xiàn)全局搜索；c1為粒子自身的加速權(quán)重系數(shù)，c2為粒子群體的加速權(quán)重系數(shù)；Rand1為c1相關(guān)的隨機初值，Rand2為c2相關(guān)的隨時初值，且在區(qū)間（0，1）范圍內(nèi)；在整個搜索過程中，gBest為整個粒子群體的最優(yōu)解，pBest為粒子自身的最優(yōu)解，也就是最佳位置。

2 基于全景視覺機器人的粒子群優(yōu)化FastSLAM算法

文章提出了基于全景視覺移動機器人的粒子群優(yōu)化FastSLAM算法。結(jié)合粒子群優(yōu)化算法，通過PSO算法更新預(yù)估的粒子進而改變其提議分布，這就在預(yù)測全景視覺移動機器人位置的時候，還考慮了觀測到的最新的路標(biāo)信息，預(yù)測到的采樣粒子大多集中在全景視覺移動機器人的實際位置周圍。另外，在粒子群預(yù)估時，不僅要考慮單獨的粒子本身，還要考慮整個粒子群的共同作用，進而達到同傳統(tǒng)的FastSLAM相比，在獲得相同的真實系統(tǒng)狀態(tài)分布預(yù)測精度的情況下，需要更少的粒子數(shù)量，這也就解決了前面提到的FastSLAM面臨粒子耗盡問題。

第一，利用粒子群優(yōu)化算法中的位置和速度方程對全景視覺移動機器人的位姿進行預(yù)測更新，方程如下：

式中，Rand1（）和Rand2（）為對角線矩陣，矩陣對角線上均為符合標(biāo)準(zhǔn)正態(tài)分布的隨機正數(shù)；c1和c2表示學(xué)習(xí)因子。XgBest表示全景視覺移動機器人位姿預(yù)測值的全局最優(yōu)解，XpBest表示全景視覺移動機器人位姿預(yù)測值的局部最優(yōu)解。由式（7）和式（8）進行更新，全景視覺移動機器人的位姿向量由Xt變?yōu)閄。

第二，為了評價粒子群優(yōu)化算法對全景視覺移動機器人位姿預(yù)測的改進程度，引入適應(yīng)度函數(shù)，函數(shù)如下：

（9）

式中Rk為全景視覺移動機器人的測量噪聲協(xié)方差矩陣；ZtPred為t時刻全景視覺移動機器人對環(huán)境中路標(biāo)的預(yù)測觀測值。ZtPred由已經(jīng)創(chuàng)建的地圖mt-1和全景視覺移動機器人的預(yù)測位姿值X計算得到，引入如下函數(shù)[5]：

（10）

如果全景視覺移動機器人在t時刻再一次觀測到之前觀測過的特征mt-1，就可以在全景視覺移動機器人預(yù)測位姿X的基礎(chǔ)上，得到這個特征相對于全景視覺移動機器人預(yù)測位姿X的預(yù)測觀測值ZtPred。根據(jù)觀測值的預(yù)測值和實際值差值判斷全景視覺移動機器人更新后的位姿X與實際位置是否更接近。文章在此設(shè)定一個閾值？啄，當(dāng)適應(yīng)度函數(shù)值Fitness<？啄時，表明這個預(yù)測值X與全景視覺移動機器人的真實位置誤差較大，需要再一次進行粒子群優(yōu)化的尋優(yōu)過程，直到Fitness？叟？啄為止，表明X已經(jīng)分布在全景視覺移動機器人的實際位置的小誤差范圍內(nèi)。優(yōu)化所有粒子，使每個粒子都向全景視覺移動機器人的實際位置周圍運動。全局最優(yōu)解XgBest和局部最優(yōu)解XpBest的初始值為預(yù)設(shè)值，在FastSLAM運行過程中的每一個周期內(nèi)，將這個周期的前一個周期里全景視覺移動機器人位姿的估計值作為局部最優(yōu)值XpBest，再通過式（10）對環(huán)境中的路標(biāo)觀測值預(yù)測后，利用式（9）計算出適應(yīng)度，如果全景視覺移動機器人的某個位姿預(yù)測值的適應(yīng)度值最大，則這個值就是全局最優(yōu)解XgBest。

基于全景視覺移動機器人的粒子群優(yōu)化FastSLAM算法流程如下：

（1）預(yù)測：通過提議分布對當(dāng)前狀態(tài)下的粒子群集合進行預(yù)測采樣，得到下一個時刻的粒子群集合，st～p（st|st-1，ut）。

（2）粒子群優(yōu)化：

a.將粒子集st的全景視覺移動機器人位姿作為XpBest；

b.通過式（7）和式（8）求取并更新粒子的速度，得到經(jīng)過優(yōu)化后的預(yù)估粒子集st→s；

c.通過式（9）和式（10）計算出環(huán)境中路標(biāo)觀測的預(yù)測值和適應(yīng)度函數(shù)值Fitness；

d.當(dāng)Fitness<？啄，記錄新環(huán)境中路標(biāo)觀測值zt和更新后的地圖，X→xt，返回b；

e.當(dāng)Fitness？叟？啄，粒子集s的全景視覺移動機器人位姿賦予

XgBest，粒子群優(yōu)化結(jié)束。

（3）權(quán)重計算：粒子的權(quán)重由下式計算出

（11）

（4）重采樣：按照權(quán)重大小的不同，在臨時的粒子集s里抽取一定比例的粒子，并將這些抽取出來的粒子添加到新的粒子集s中。

如果粒子集在計算權(quán)重之前，粒子集在粒子群優(yōu)化后能更接近全景視覺移動機器人的實際位置，則權(quán)重可以體現(xiàn)粒子的真實分布情況，重采樣的效果也就更好，粒子集的收斂的速度也更快，也就會為下一個周期的全景視覺移動機器人的位姿預(yù)測提供一個更加準(zhǔn)確的初始值。

3 實驗及分析

本小節(jié)分別對基于全景視覺移動機器人的傳統(tǒng)FastSLAM算法和文章的改進FastSLAM算法的進行仿真實驗，進而驗證文章所提出全景視覺移動機器人的改進FastSLAM算法的優(yōu)勢。

仿真選取的環(huán)境為400m×400m的矩形區(qū)域，該區(qū)域中分布著若干環(huán)境特征點，用“*”表示；在實驗中提前規(guī)劃了全景視覺機器人的行駛路線，全景視覺機器人的行駛路線由25個路徑點（用“”表示）確定。

在上述仿真條件下，選取粒子數(shù)N=100，分別對傳統(tǒng)的FastSLAM和文章采用的改進的FastSLAM算法進行仿真實驗，對全景視覺移動機器人定位精度進行比較分析。

圖1和圖2為傳統(tǒng)的FastSLAM和文章采用的改進FastSLAM的仿真結(jié)果。全景視覺機器人從（0，0）點出發(fā)，沿行駛路線按逆時針方向行駛1周。長虛線是按路徑信息設(shè)置好的全景視覺機器人運動軌跡；短虛線表示機器人實際運動軌跡；直線表示機器人利用SLAM算法之后得到的估計更新后的軌跡；“×”表示地圖中預(yù)設(shè)的靜止路標(biāo)；“+”表示全景視覺機器人經(jīng)過SLAM算法得到的環(huán)境中路標(biāo)估計位置；橢圓表示全景視覺機器人對路標(biāo)誤差的估計，橢圓的大小代表了對應(yīng)路標(biāo)的不確定程度。位于短虛線的矩形表示全景視覺機器人在控制信息下到達的實際位置，位于直線的矩形表示全景視覺機器人在SLAM算法下所估計的位置。

圖3和圖4為兩種SLAM算法獲得的全景視覺機器人定位誤差對比情況。橫軸為SLAM執(zhí)行過程中的運行時間，縱軸為全景視覺機器人在X和Y方向上的位置偏差，t時刻X方向的位置偏差值是由該時刻的全景視覺機器人位置的實際值和經(jīng)過SLAM算法得到的估計值的全局坐標(biāo)的x值相減并取絕對值得到，Y方向的位置偏差同理可得。

總體上從曲線中可以看出，兩種算法都具有較小的估計誤差，且誤差具有收斂性，由此表明，這兩種SLAM算法能有效實現(xiàn)自主定位。但傳統(tǒng)的FastSLAM算法的全景視覺機器人位置估計的X方向的最大誤差為5.0469m，Y方向的最大誤差為2.7413m，而采用文章提出方法的全景視覺機器人位置估計的X方向的最大誤差為1.0576m，Y方向的最大誤差為0.9717m。證明文章采用的改進的FastSLAM算法的全景視覺機器人定位精度明顯優(yōu)于傳統(tǒng)的FastSLAM算法。仿真實驗結(jié)果與前文的理論分析一致。

4 結(jié)束語

文章將粒子群優(yōu)化算法引入到全景視覺移動機器人FastSLAM算法，使粒子獲得更接近真實系統(tǒng)狀態(tài)分布的預(yù)測，提高了全景視覺移動機器人FastSLAM預(yù)測過程的粒子采樣效率。仿真實驗證明，全景視覺機器人在完成位姿估計和地圖創(chuàng)建時僅需較少的粒子即可實現(xiàn)，提高了SLAM的精度和運行效率。

參考文獻

[1]梁志偉，馬旭東，戴先中，等.基于分布式感知的移動機器人同時定位與地圖創(chuàng)建[J].機器人，2009，31（1）：33-39.

[2]吳葉斌.基于全景視覺的移動機器人SLAM方法研究[D].哈爾濱：哈爾濱工程大學(xué)，2011

[3]Kurt K.Improved Occupancy Grids for Map Building.Autonomous Robots，1997，4：351-367.

[4]K.Murphy. Dynamic Bayesian Networks： Representation， Inference and Learning Phd.Thesis University of Callfornia Berkeley，2002.

[5]陳白帆.動態(tài)環(huán)境下移動機器人同時定位與建圖研究[D].中南大學(xué)，2009.