基于動態種群分布的雙變異優化FastSLAM算法的改進

梁雪慧 張瑞杰 趙 菲 程云澤

（天津理工大學電氣電子工程學院）

無人駕駛車輛最重要的功能之一是建立周圍環境的地圖并且通過地圖進行導航，這通常被稱為SLAM （同時定位與建圖）［1］。 然而， 在實施SLAM時同時讓無人車進行定位和建立地圖存在一定的困難，難點在于如果要使無人車知道其真實位置， 必須在定位之前構建非常精確的地圖；但是在建立精確的地圖之前，車輛必須準確對自身定位，否則會導致誤差不斷累積從而影響結果的精確性。 車輛在行駛過程中的任何摩擦、控制損失或者小障礙都往往是導致錯誤測距的原因，從而對真實位置估計不當。

SLAM算法最早由Smith R等提出［2］，它利用擴展卡爾曼濾波估計機器人的位姿、路標特征和聯合分布，但由于在實際環境中噪聲與環境狀態多為非線性參量，且計算量會隨著迭代次數的增加急劇上升， 因此Martinez-Cantin R和Castellanos J A又提出了基于無跡卡爾曼濾波的SLAM 算法［3］，雖然具有一定的調節能力，但是算法只能用于高斯環境。為了不局限于高斯環境，Doucet A等提出了基于粒子濾波器 （Rao-Blackwellized）的SLAM算法［4］，從而打破了線性和高斯環境的限制。另一方面，在此基礎上Montemerlo M等又提出了針對特征地圖的FastSLAM算法［5］，把SLAM分解為兩個“獨立”的問題，并分別采用不同的濾波方式使算法能夠在大規模復雜的環境中運用。 但是在計算過程中， 隨著粒子種群不斷更新權重，這其中有大部分的粒子權重會非常小，甚至權重會集中在種群中的幾個甚至單一粒子上，最終出現“粒子退化”的問題。 粒子濾波中的重采樣雖然可以降低粒子的退化程度， 提升有效粒子數目，但是會出現一些問題，一方面使得粒子的多樣性有所減弱，另一方面會帶來粒子的衰竭，計算后只有權重最大的粒子生存下來， 影響結果的精度。Sim R等提出的粒子群優化算法（PSO）雖然已經廣泛應用于各種群智能問題［6］，但仍然很難保證粒子的多樣性和結果的精確性。 為了解決這些問題，寧國忠提出了一種自適應的粒子群算法（APSO）［7］， 該算法能夠根據運算過程中的粒子種群收斂情況自適應調節全局概率，加強全局最優粒子的局部搜索性能， 從而避免粒子陷入局部最優，提高計算精度。

目前已經有學者指出APSO算法不一定能得出問題的最優解［7］，一方面是粒子種群是固定不變的，假若粒子種群規模較大，則會很大程度上增加計算量； 另一方面是粒子群陷入局部最優時，隨著計算的不斷進行，搜索范圍會越來越小，這兩方面的問題往往會導致結果不理想甚至失敗。 筆者采用種群動態分布策略，并且引入雙變異算子，將它應用到無人車的FastSLAM中，提出一種基于種群動態分布雙變異優化的FastSLAM算法。 在計算過程中對粒子種群進行判定，如果種群收斂，則通過動態種群策略的調整對粒子種群進行增加或刪減粒子來保證粒子多樣性，并且對種群中適應度最差和最優的粒子分別進行高斯變異，從而加強在其附近搜索的能力，使算法得到理想的最優解。 最后，將改進后的FastSLAM算法應用于無人駕駛車輛，并進行仿真驗證。

1 FastSLAM算法

1.1 FastSLAM算法的流程

基于粒子濾波器和擴展卡爾曼濾波的Fast-SLAM算法步驟如下。

k時刻粒子集sk由該時刻所有粒子

預測機器人位姿， 根據k-1時刻粒子集中的每個粒子及其運動模型來計算k時刻的粒子集sk。

計算權重，k時刻第i個粒子的權值由k-1時刻第i個粒子的權值和k時刻第i個粒子本身的值所估計的k時刻粒子位姿zk得出，具體計算式為：

根據觀測模型計算粒子集sk-1對應的特征點坐標，并將它與環境陸標集中的某一陸標mk進行關聯。

對粒子集權值進行更新。

重采樣。

更新粒子集權重。

更新環境地圖。

更新粒子k時刻的位姿，若無人車繼續運行，則轉到預測機器人位姿步驟，否則結束定位。

1.2 FastSLAM算法的缺陷

FastSLAM由于算法本身的估計誤差會隨著時間的推移而累積［8］，因此產生了廣泛存在的權值集中在少數粒子、粒子的多樣性無法得到保證等問題。 粒子退化意味著計算效率在大部分對于計算結果僅有微小作用的粒子影響下會不斷降低，且由于誤差的累積而導致結果精度也受到影響。 但是每一個粒子所包含的位姿信息是所有時刻的位姿信息，然而，算法每一次的迭代過程只需要當即時刻的位姿信息， 進行重采樣之后，雖然淘汰了大多數權重小的粒子，但同時也刪除了這些粒子中所包含的過去時刻的信息， 并且，權重大的粒子被多次復制，那么復制出的粒子也許都繼承的同一個原始粒子的信息，這樣雖然保證了一定數量權值大的粒子參與計算但是大量的粒子位姿信息被刪除，從而會大幅削弱地圖估計的精確性，引起粒子缺乏多樣性的問題。 這使得FastSLAM算法存在粒子多樣性嚴重退化的顯著缺點［9］。

另一個方面，在FastSLAM算法中粒子種群數量是固定不變的，如果種群數量過大，則會很大程度上影響運算效率； 相反如果種群數量過小，則會導致運算得出的最優解不是理想結果，無人車實際路徑與預設路徑偏差過大。

2 基于種群動態分布的雙變異優化FastSLAM算法

傳統的FastSLAM算法中由于需要對粒子進行重采樣，會導致種群中只有少數幾個粒子甚至單一高權重粒子參與到后續計算中［10］，為了改善粒子集的多樣性，同時避免自適應粒子群優化算法中容易出現的局部最優現象，使無人車的狀態估計更加準確，筆者將種群動態分布策略和動態雙變異粒子群算法引入到FastSLAM算法中，在保證計算效率和結果精度的前提下得到更加優秀的粒子集［11］。

2.1 動態種群分布策略

針對傳統FastSLAM算法中粒子種群固定不變的特性，引入了動態種群分布策略來提高優化性能。 加入動態種群分布策略之后，種群規模、慣性權重和學習因子都在搜索過程中能自適應地改變數值大小來進一步優化算法。 在計算過程中， 提出一種新的實時種群分布估計（Particle Swarm Distribution Estimation，PSDE）， 根據粒子種群的分布情況進行判定，共有兩個狀態——收斂和分散，按照動態種群分布策略的設計，如果當前粒子種群的分布狀態為收斂，那么種群則增加一個粒子來增加粒子群的多樣性；如果判定粒子種群此時刻的狀態為分散，那么種群減少一個粒子，這樣在保證采樣多樣性的同時也減少了運算時間，提高了運算效率；如果判定粒子種群為不收斂，則種群減少一個粒子來降低計算次數和運算時間。

根據PSDE，計算每一代粒子到全局適應度最優粒子的距離時，將它與全局適應度最差粒子到最優粒子之間的距離進行比較，假如距離大于最優與最差粒子距離的粒子數量大于粒子總數的60%，則種群此時的狀態為分散，那么種群將隨機刪除一個除全局最優粒子之外的粒子； 反之，假如距離小于最優與最差粒子距離的粒子數量大于粒子總數的60%， 那么種群此時的狀態判定為收斂， 且極有可能已經陷入局部最優的狀態，此時將增加一個粒子，增加的粒子由下式決定：

其中，newPos為增加粒子的位置，Xmax和Xmin分別是搜索空間的上、 下限，gbest為全局最優粒子，Gaussian是一個均值為μ（μ=0）、標準差為σ的高斯分布隨機數。

2.2 動態雙變異優化

在PSDE-FastSLAM算法中針對粒子種群的分布做出了動態調整， 但為了避免出現如圖1所示的粒子分布（狀態估計判定為分散，位姿信息全部集中在某一個或者少數幾個粒子上）， 加入了兩個變異算子（Dynamic Dual Mutation，DD）［12］，通過對適應度最優的粒子和適應度最差的粒子進行高斯變異來控制算法， 提高算法的搜索能力，同時避免動態種群分布策略產生的過度分散的情況。

圖1 粒子種群過度分散

加入的第1個變異算子是最差適應度變異算子，對適應度最差的粒子在迭代的每一代都進行較大概率的高斯變異， 用來增加粒子的搜索空間。 在該算法中，對最差適應度粒子以變異概率pm1進 行 變 異，pm1值 根 據 經 驗 確 定， 一 般 在［0.08，0.50］取值，粒子變異過程為：

其中，xi為變異后的新粒子，gworsti為變異前的最差適應度粒子，ζ為服從高斯分布的隨機量。

第2個變異算子是最優適應度變異算子，隨著計算的不斷進行，粒子種群的分布逐漸趨于收斂，運算后期算法出現局部早熟收斂甚至最優值不再進行更新，于是對最優適應度粒子以變異概率pm2進行變異，pm2值也根據經驗確定， 一般在［0.01，0.06］取值，粒子變異過程為：

其中，gbesti為變異前的最優適應度粒子，γ為服從高斯分布的隨機量。

2.3 PSDE-FastSLAM-DD算法的實現

PSDE-FastSLAM-DD算法的實現步驟如下：

a. 根據初始坐標，生成具有Num個粒子的粒子集s0和相應的權值ω0，如前文式（1）～（3）；

b. 計算粒子適應度值，根據適應度來選擇局部歷史最優適應度粒子和全局最優適應度粒子；

c. 進行PSDE步驟；

d. 對最差適應度粒子進行高斯變異，幫助粒子種群進行收斂，避免過度分散；

e. 更新粒子適應度以及局部歷史最優適應度粒子、全局最優適應度粒子；

f. 對最優適應度粒子進行高斯變異， 幫助粒子種群跳出局部收斂狀態，保證運算結果的精確性；

g. 更新環境地圖；

h. 計算粒子k時刻的位姿， 若無人車繼續運行，則轉到步驟b，否則結束更新。

3 仿真結果及實驗分析

3.1 仿真模型

無人車的里程計數學模型如下［13，14］：

其中，Xk+1表示無人車在k+1時刻的位姿狀態，uk+1表示無人車在k+1時刻里程計的控制輸入，θk為k時刻無人車轉過的航向角，xk和yk分別為k時刻無人車的坐標值，D為無人車的車長，Δdk+1為單位時間內無人車走過的路徑長度，Δθk+1為單位時間內無人車的偏轉角度。

觀測模型為：

其中，r和θ分別表示檢測到的環境特征與無人車之間的距離和運動航向角，xl和yl表示里程計接收的路標信息，ψk表示觀測噪聲。

3.2 MATLAB結果分析

3.2.1 MATLAB仿真環境

為了驗證PSDE-FastSLAM-DD算法的實際有效性，在實驗中，針對天津理工大學實際地圖來創建仿真環境， 選取學校東門作為路徑起點，無人車車輛途徑體育館， 最后到達機械工程學院，對此創建了如圖2所示的一個基于路標點和導航點的仿真環境， 環境中設置34個路標，24個導航點。

圖2 仿真路徑

仿真中時間ΔT=0.025s， 運動過程噪聲為0.3m/s，仿真過程中取20個粒子，仿真結果如圖3所示。

圖3 仿真結果

在圖3中，黑色方塊為仿真對象無人車，綠色星號點為路標點，紅色圓圈點為導航點，紅色細實線為預設路徑，藍色實線為無人車的實際行駛路徑， 黃色顯示為無人車在該點監測到的路標點，從仿真結果可以看出，無人車基本按照預設路徑完成了行駛過程。

3.2.2 仿真參數的選取

筆者在MATLAB仿真平臺上采用獨立實驗分別 對 傳 統 的FastSLAM 算 法、 基 于APSO 的FastSLAM算法和改進后的PSDE-FastSLAM-DD算法這3種不同的算法進行仿真對比分析來驗證改進后算法在實際應用中的有效性。

采用一個樣本空間， 粒子種群規模設定為30，算法迭代500次結束，選取Rastrigrin為測試函數，針對算法的均最小值、均耗時兩個方面進行測試，從測試結果中得到最優的參數。

由測試結果表1可得，選取最差適應度粒子的變異概率pm1為0.20， 最優適應度粒子的變異概率pm2為0.03， 其他變異概率的取值都不利于算法的收斂。

表1 變異概率對算法性能影響的測試結果

3.2.3 3種算法對比

為了驗證算法的優越性， 采用一個樣本空間， 對比了傳統的FastSLAM算法、 基于APSO的FastSLAM算法和PSDE-FastSLAM-DD算法的無人車實際運動路徑與預設路徑之間的偏差，結果如圖4所示，結果顯示PSDE-FastSLAM-DD算法的無人車的運動路徑與真實值相比偏差最小，說明它具有較高的計算精度。

圖4 3種算法路徑偏差的對比

實驗又對比了3種算法的估計精度和運行時間（表2）。 由表2可知，改進后的PSDE-FastSLAMDD算法不僅估計精度比其他兩種算法有所提高，運行效率也有了提升。

表2 3種算法估計精度和運行時間

4 結束語

在傳統的FastSLAM算法中， 對粒子集進行重采樣會引起“粒子耗盡”問題，而單一的PSO算法會隨著運算而發生“局部最優”的情況，若采用APSO算法來替代FastSLAM算法中的粒子濾波同樣難以保證粒子種群不陷入 “局部收斂” 的情況。 針對這兩種算法的特點， 改進了FastSLAM算法， 在算法初期運用動態種群分布策略根據粒子群的實時進化狀態來改變其規模， 并且在運算中加入針對最優適應度粒子和最差適應度粒子的變異算子， 增強這兩種粒子附近的搜索范圍，既避免陷入了“局部最優”，又避免了粒子群過度分散、 粒子信息單一的缺陷， 保證了粒子的多樣性。 理論分析和實驗表明，改進后的PSDE-FastSLAM-DD算法在估計精度和運行效率方面的性能都較為突出， 保障了無人車的實時精準運行。