基于自適應(yīng)粒子群優(yōu)化FastSLAM算法的改進

梁雪慧，張瑞杰，趙 菲，程云澤

（天津理工大學(xué)電氣電子工程學(xué)院，天津300384）

無人駕駛車輛最明顯的功能之一是建立周圍環(huán)境的地圖并且通過地圖進行導(dǎo)航，這通常被稱為SLAM（同時定位與建圖）[1].然而，在實施SLAM時同時讓無人車進行定位和建立地圖存在一定的困難，難點在于如果要使無人車知道其真實位置，必須在定位之前構(gòu)建非常精確的地圖[2]；但是在建立精確的地圖之前，車輛也同樣必須準確對其自身定位，否則會導(dǎo)致誤差的不斷累積從而很大程度上影響結(jié)果的精確性.車輛在行駛過程中的任何摩擦，控制損失或者小障礙都往往是導(dǎo)致錯誤測距的原因，從而對真實位置的估計不良[3-4].

針對SLAM[5-6]存在的上述問題，Montemerlo等最早提出了FastSLAM算法，采用粒子濾波（particle filtering，PF）的方法，把SLAM分解為兩個“獨立”的問題，并分別采用不同的濾波方式使之能夠在大規(guī)模復(fù)雜的環(huán)境中運用.但是在計算過程中，隨著粒子種群不斷更新權(quán)值，這其中有大部分的粒子權(quán)重會非常小，甚至權(quán)重會集中在種群中的幾個甚至單一粒子上，最終會出現(xiàn)“粒子退化”的問題.為了解決這些問題，許多學(xué)者提出了諸多改進的算法.文獻[7]中提出了用粒子群優(yōu)化來代替粒子濾波的重采樣過程，雖然改善了在求解復(fù)雜的非線性函數(shù)優(yōu)化問題時狀態(tài)估計的精度，但是粒子群優(yōu)化算法本身容易發(fā)生粒子種群早熟收斂的現(xiàn)象.文獻[8]中，提出了基于遺傳算法的FastSLAM2.0算法，雖然一定程度上解決了粒子退化的問題，使得參與計算的大權(quán)值粒子數(shù)目有所提高，但由于FastSLAM2.0算法和遺傳算法本身計算的復(fù)雜性，算法的實時性上有所欠缺.文獻[9]中利用免疫學(xué)的原理理論，結(jié)合了人工免疫算法，把目標模版的特征點看作抗原，利用變異的方式淘汰對抗原親和力小的抗體，由此讓結(jié)果更快速的接近最準確，但這個算法復(fù)雜、耗時且利用率較低.

綜合這些算法目前存在的問題，本文提出一種基于自適應(yīng)粒子群優(yōu)化算法的FastSLAM算法.先利用粒子的后驗位姿提議分布構(gòu)建粒子的篩選區(qū)間[10]，區(qū)間內(nèi)的粒子得以生存，同時使區(qū)間外的粒子向高似然概率區(qū)間移動，改善算法初期粒子的分布，然后通過自適應(yīng)粒子群優(yōu)化算法來更新粒子的全局最優(yōu)位置以及粒子歷史最優(yōu)，提高粒子的優(yōu)越性，進一步通過交叉變異操作步驟，克服了原始粒子群算法容易出現(xiàn)的“局部最優(yōu)”問題.最后，將改進后的算法應(yīng)用于無人駕駛車輛的FastSLAM，并進行仿真結(jié)果驗證.

1 無人車FastSLAM的算法模型

FastSLAM所示的誤差會隨著時間的推移而累積[12]，因此產(chǎn)生了廣泛存在的權(quán)值集中在少數(shù)粒子的問題.粒子退化問題意味著計算效率在大部分對于計算結(jié)果僅有微小作用的粒子影響下會不斷降低，且由于誤差的累積而導(dǎo)致結(jié)果精度也受到影響.通過計算粒子種群中的有效粒子數(shù)Neff，Neff的值越小則說明粒子退化現(xiàn)象越嚴重，Neff可由式（1）近似計算：

式（1）中，ωi為第i個粒子的權(quán)重值；k是當即時刻.如果通過判定得出參與到后續(xù)計算的粒子數(shù)目小于有效粒子數(shù)（一般情況下，有效粒子數(shù)=0.8×粒子總數(shù)），就對不同權(quán)重的粒子進行重新采樣，使權(quán)重大的粒子的數(shù)量增多.通過重采樣步驟，雖然可以一定程度上減弱粒子的退化現(xiàn)象，但是每一個粒子所包含的位姿信息是所有時刻的位姿信息，然而，算法每一次的迭代過程只需要當即時刻的位姿信息，進行重新采樣之后，雖然淘汰了大多數(shù)權(quán)重小的粒子，但同時也刪除了這些粒子中所包含的過去時刻的信息，并且，權(quán)重大的粒子被多次復(fù)制，那么復(fù)制出的粒子也許都繼承的同一個原始粒子的信息，這樣雖然保證了一定數(shù)量權(quán)值大的粒子參與計算但是大量的粒子位姿信息被淘汰，從而會大大削弱地圖估計的精確性，引起粒子缺乏多樣性的問題.

2 基于自適應(yīng)粒子群優(yōu)化算法的FastSLAM算法

傳統(tǒng)的FastSLAM算法中由于需要對粒子進行重采樣，會導(dǎo)致種群中只有少數(shù)幾個粒子甚至單一高權(quán)重粒子參與到后續(xù)計算中[13-14]，為了改善粒子集的多樣性，同時避免粒子群優(yōu)化算法中容易出現(xiàn)的局部最優(yōu)現(xiàn)象，引入自適應(yīng)粒子群優(yōu)化算法來優(yōu)化粒子集.

粒子群優(yōu)化算法[15]容易出現(xiàn)的局部最優(yōu)（也稱為早熟收斂）問題是指計算過程中粒子種群進化迭代到一個不是全局最優(yōu)狀態(tài)，但是粒子群的進一步迭代已不可能產(chǎn)生更好的可行解，反映在粒子適應(yīng)度值上就表示為：已獲得的當前最優(yōu)粒子的適應(yīng)度值小于解決方案的最優(yōu)值且適應(yīng)度值不再更新.自適應(yīng)粒子群優(yōu)化算法[16]是一種基于生物群體智能且加入了交叉變異的自適應(yīng)策略的全局優(yōu)化方法，在多目標優(yōu)化、動態(tài)系統(tǒng)的跟蹤和優(yōu)化以及模糊分類等方面都發(fā)揮了重要的作用.而在算法初期采用篩選粒子[12]的方式使得粒子分布更為合理，再用自適應(yīng)粒子群的優(yōu)化過程代替粒子重采樣過程，這樣既克服了粒子濾波算法帶來的粒子貧乏和粒子多樣性減弱等問題，又克服了普通粒子群優(yōu)化算法中粒子群的早熟收斂問題.本文將這兩種方法結(jié)合并引入FastSLAM算法中，在保證計算效率和結(jié)果精度的前提下得到更加優(yōu)秀的粒子集.

2.1 自適應(yīng)粒子群優(yōu)化算法

本文采用的自適應(yīng)策略為：交叉變異操作[16-17].根據(jù)算法的收斂情況自適應(yīng)的確定進行交叉變異操作的概率，并且根據(jù)每個粒子與全局最優(yōu)粒子的歐氏距離，將交叉算子引入群體中高度聚集的粒子，然后引入變異算子以增強全局最優(yōu)粒子的局部搜索，并用備份好的原全局最優(yōu)粒子替換掉當前種群的適應(yīng)度最差的粒子.

在改進后的算法中，用粒子的適應(yīng)度值的大小來衡量粒子的位置，適應(yīng)度越高，粒子的位置就越優(yōu)秀，粒子與現(xiàn)實的匹配度就越高，可以看出此時適應(yīng)度與傳統(tǒng)FastSLAM算法中權(quán)值的定義是一樣的，因此用式（2）來定義粒子的適應(yīng)度為粒子權(quán)重：

對于每個粒子個體用一維向量X表示其空間位置，用速度向量V來表示粒子在空間中的移動方向和距離.種群中第i個粒子在第t次迭代時，粒子的位置向量可以表示為式（3）：

速度向量可以表示為式（4）：

截止到第t代，粒子i搜索到的最優(yōu)位置記為

稱為局部歷史最優(yōu)位置，記為Pbest（t）.群體中所有粒子經(jīng)歷過的全局最優(yōu)位置記為Gbest（t），Gbest（t）可以通過公式（6）計算得到.

在計算過程中，粒子群一旦陷入局部最優(yōu)，往往很難自拔，從演化計算中陷入局部最優(yōu)時變異策略的經(jīng)驗學(xué)習(xí)到：通過計算自適應(yīng)的概率來引進交叉變異操作，自適應(yīng)的概率定義為：

式（7）中，μ和σ是變異率的調(diào)節(jié)參數(shù)，在日常操作中μ設(shè)置為0.001，σ設(shè)置為0.005.Re是當最優(yōu)值不再更新或者更新不明顯時開始計次的迭代數(shù).若最優(yōu)值不斷更新，則不對種群進行自適應(yīng)優(yōu)化操作；如種群收斂速度停滯，連續(xù)若干代不更新或者最優(yōu)值更新不明顯，Re將累積增大，對種群進行交叉變異操作的概率也隨之加大.具體調(diào)節(jié)措施如下：

首先，復(fù)制種群中的最優(yōu)粒子，然后將種群中出最優(yōu)之外的每個粒子依次取出，判斷依次取出的粒子與最優(yōu)粒子的變量空間距離是否小于計算得出的閾值，如果小于閾值，則依據(jù)公式（10）交叉兩個粒子.

兩個粒子的變量空間距離用歐幾里得距離來定義：

式（8）中，l為粒子的變量空間距離；D為搜索區(qū)域的面積，x1和x2是種群中進行對比的兩個粒子.閾值定義為：

式（9）中，t代表當代迭代次數(shù)，T表示最大迭代次數(shù)；ub和lb表示搜索區(qū)域的上下限；n是調(diào)節(jié)參數(shù).由式（9）得出，可以根據(jù)種群進化的過程對閾值進行調(diào)整.算法初期，由于種群中粒子具有多樣性，此時不宜對種群進行調(diào)整（閾值設(shè)置較大）；隨著t的不斷增加，種群中粒子逐漸向最優(yōu)粒子靠近，粒子的多樣性逐漸減弱，就可能出現(xiàn)粒子種群的早熟收斂，此時需要對種群進行必要的自適應(yīng)優(yōu)化策略，更新粒子群的狀態(tài).

具體交叉操作按照式（10）進行：

式（10）中，cx1和cx2是交叉操作生成的新粒子；x1和x2是父粒子；e是一個（0，1）區(qū)間的D維隨機數(shù)列.

進行交叉操作后，更新粒子的權(quán)重（即適應(yīng)度）.如父粒子的適應(yīng)度增加，則用更新后的粒子來代替父粒子；如適應(yīng)值惡化或者不變化，則下一步的變異操作，變異后舍棄不夠優(yōu)秀（適應(yīng)度低）的粒子，用兩個粒子中相對適應(yīng)值較高的粒子來代替，具體變異操作如下：

式（11）中，mx1和mx2是經(jīng)過變異操作的粒子，其余參數(shù)含義同上，然后加強全局最優(yōu)粒子局部細搜索.具體操作是在沿著搜索區(qū)域的上下限方向移動全局最優(yōu)粒子一個很小的步長，以獲得一組新的粒子，更新這組粒子的適應(yīng)度，并舍棄適應(yīng)度低的粒子，用適應(yīng)值更佳的粒子來代替.最后整個種群中具有最小適應(yīng)度的粒子被一開始復(fù)制的原始最優(yōu)粒子替換.

2.2 自適應(yīng)粒子群優(yōu)化算法實現(xiàn)步驟

圖1所示為自適應(yīng)粒子群優(yōu)化算法的具體步驟，在算法執(zhí)行交叉變異操作之后，更新粒子的適應(yīng)度，用備份的原全局最優(yōu)粒子替換適應(yīng)值最差的粒子；再按照標準的粒子群算法更新種群中每個粒子的速度和位置，更新全局最優(yōu)粒子Gbest和粒子歷史最優(yōu)Pbest，從而得出最優(yōu)的粒子群.

圖1 自適應(yīng)粒子群優(yōu)化算法流程Fig.1 Self-adapt swarm optimization algorithm flow

2.3 自適應(yīng)粒子群優(yōu)化FastSLAM算法的實現(xiàn)過程

在傳統(tǒng)FastSLAM算法的基礎(chǔ)上做了兩處改進：一是用自適應(yīng)粒子群優(yōu)化過程代替粒子濾波中的重采樣，另一處改進是通過設(shè)置篩選區(qū)間，區(qū)間內(nèi)的粒子得以生存并且區(qū)間外的粒子向中心區(qū)域移動，從而改善粒子在算法初期的分布.

詳細實現(xiàn)過程如下：

步驟1：計算各個粒子的后驗位姿建議分布.

步驟2：從建議分布中采樣

式（12）中，xk為粒子在k時刻的采樣位姿；xk-1為k-1時刻粒子的采樣位姿；zk為k時刻的觀測信號；uk為k時刻的控制信號，i取1到N.

步驟3：計算各個粒子的適應(yīng)度

步驟4：計算粒子的篩選區(qū)間，并對區(qū)間外的粒子進行移動篩選區(qū)間：

式（14）中κ為區(qū)間調(diào)節(jié)系數(shù)，假設(shè)噪聲服從高斯分布；Rv即是高斯噪聲的方差；h-1（yk）為粒子位姿提議分布的期望。區(qū)間內(nèi)的粒子得以生存，對區(qū)間外的粒子沿期望方向移動，且移動方法如下：

步驟5：更新全局最優(yōu)粒子和粒子歷史最優(yōu).

步驟6：備份全局最優(yōu)粒子，對種群進行操作.

步驟7：粒子優(yōu)化.

步驟8：地圖估計與更新.

3 仿真結(jié)果及實驗分析

3.1 仿真模型

無人車的里程計數(shù)學(xué)模型如下：

式（16）中，Xk+1表示無人車k+1時刻的位姿狀態(tài)；uk+1表示無人車在k+1時刻里程計的控制輸入；θk為k時刻的無人車轉(zhuǎn)過的航向角且其值的范圍位于[-180°，+180°]之間；D表示兩驅(qū)動軸的間距.觀測模型為：

式（17）中，r和θ分別表示檢測到的環(huán)境特征與無人車之間的距離和運動航向角；xi和yi表示觀測到的第i個路標位置；ωk表示觀測噪聲.

3.2 仿真結(jié)果分析

為了驗證基于自適應(yīng)粒子群優(yōu)化的FastSLAM算法的實際有效性，本文在MATLAB仿真平臺上通過獨立實驗分別對三種不同的算法進行仿真對比分析.

創(chuàng)建了如圖1所示一個基于路標點和導(dǎo)航點的仿真環(huán)境，環(huán)境中設(shè)置28個路標，30個導(dǎo)航點.無人車的相關(guān)參數(shù)：兩驅(qū)動輪之間間距D=2.7 m；運動速度為0.5 m/s；航向角速度6 rad/s；傳感器采樣時間ΔT=0.025 s，運動過程噪聲0.3 m/s；仿真過程中取20個粒子.

運行結(jié)果如圖2，其中，“*”為導(dǎo)航點，“○”為路標.

圖2 仿真環(huán)境Fig.2 Simulation environment

為了驗證算法的優(yōu)越性，采用一個樣本空間，粒子總數(shù)都為40，對比了傳統(tǒng)FastSLAM算法，基于二階差分粒子濾波的FastSLAM算法以及基于自適應(yīng)粒子群優(yōu)化FastSLAM算法進行仿真.

由圖3可知，基于自適應(yīng)粒子群優(yōu)化算法的FastSLAM算法的無人車實際運行路徑與預(yù)設(shè)路徑相比，偏差隨著時間的推移有所波動，但整體路徑偏差要比其他兩種算法低，這表明自適應(yīng)粒子群優(yōu)化算法的確對于估計精度有一定提高，因此驗證了基于自適應(yīng)粒子群優(yōu)化改進后的FastSLAM算法在無人車運行中的精確性.

圖3 算法運動偏差對比Fig.3 Simulation environment

為了滿足無人車運行的實時性要求，基于三種算法的估計方根誤差和運行時間兩個指標進行了仿真實驗的對比，結(jié)果如表1，結(jié)果表明，改進后的FastSLAM算法不僅在運算效率方面表現(xiàn)優(yōu)異，并且有效的提高了算法的估計精度，完全可以滿足無人車實時性的要求.

表1 估計方根誤差和算法運行時間表Tab.1 Estimated square root error and algorithm runtime table

4 結(jié)論

在傳統(tǒng)的FastSLAM算法中，對粒子集進行重采樣會引起“粒子耗盡”問題，而單一的粒子群優(yōu)化算法會隨著不斷迭代而容易發(fā)生“局部最優(yōu)”的情況，針對這兩種算法的特點，設(shè)計了一種改進的FastSLAM算法，用自適應(yīng)粒子群優(yōu)化來代替重粒子濾波，并且在算法初期通過構(gòu)建區(qū)間來篩選粒子，使得粒子的分布更加合理.種群中的粒子有較多機會參與到后續(xù)的運算中，而不是僅僅被丟棄.另一方面，許多優(yōu)秀的粒子所包含的信息傳遞給了新粒子，而且交叉變異操作的進行，又保證了粒子種群的多樣性.理論分析和實驗表明，基于自適應(yīng)粒子群優(yōu)化的FastSLAM算法在估計精度和計算效率方面的性能都較為突出，滿足了無人車實時并且精準運行的要求.