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

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

關鍵詞：全景視覺；機器人；粒子群優化；SLAM算法

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

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

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

1 FastSLAM算法基本原理和粒子群優化的原理

1.1 FastSLAM算法原理介紹

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

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

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

（3）

根據Bayes規則和Markov假設可得，

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

1.2 粒子群優化算法

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

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

（5）

（6）

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

2 基于全景視覺機器人的粒子群優化FastSLAM算法

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

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

式中，Rand1（）和Rand2（）為對角線矩陣，矩陣對角線上均為符合標準正態分布的隨機正數；c1和c2表示學習因子。XgBest表示全景視覺移動機器人位姿預測值的全局最優解，XpBest表示全景視覺移動機器人位姿預測值的局部最優解。由式（7）和式（8）進行更新，全景視覺移動機器人的位姿向量由Xt變為X。

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

（9）

式中Rk為全景視覺移動機器人的測量噪聲協方差矩陣；ZtPred為t時刻全景視覺移動機器人對環境中路標的預測觀測值。ZtPred由已經創建的地圖mt-1和全景視覺移動機器人的預測位姿值X計算得到，引入如下函數[5]：

（10）

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

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

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

（2）粒子群優化：

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

b.通過式（7）和式（8）求取并更新粒子的速度，得到經過優化后的預估粒子集st→s；

c.通過式（9）和式（10）計算出環境中路標觀測的預測值和適應度函數值Fitness；

d.當Fitness<？啄，記錄新環境中路標觀測值zt和更新后的地圖，X→xt，返回b；

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

XgBest，粒子群優化結束。

（3）權重計算：粒子的權重由下式計算出

（11）

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

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

3 實驗及分析

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

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

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

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

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

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

4 結束語

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

參考文獻

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

[2]吳葉斌.基于全景視覺的移動機器人SLAM方法研究[D].哈爾濱：哈爾濱工程大學，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]陳白帆.動態環境下移動機器人同時定位與建圖研究[D].中南大學，2009.