基于動(dòng)態(tài)種群分布的雙變異優(yōu)化FastSLAM算法的改進(jìn)

梁雪慧 張瑞杰 趙 菲 程云澤

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

無(wú)人駕駛車(chē)輛最重要的功能之一是建立周?chē)h(huán)境的地圖并且通過(guò)地圖進(jìn)行導(dǎo)航，這通常被稱(chēng)為SLAM （同時(shí)定位與建圖）［1］。 然而， 在實(shí)施SLAM時(shí)同時(shí)讓無(wú)人車(chē)進(jìn)行定位和建立地圖存在一定的困難，難點(diǎn)在于如果要使無(wú)人車(chē)知道其真實(shí)位置， 必須在定位之前構(gòu)建非常精確的地圖；但是在建立精確的地圖之前，車(chē)輛必須準(zhǔn)確對(duì)自身定位，否則會(huì)導(dǎo)致誤差不斷累積從而影響結(jié)果的精確性。 車(chē)輛在行駛過(guò)程中的任何摩擦、控制損失或者小障礙都往往是導(dǎo)致錯(cuò)誤測(cè)距的原因，從而對(duì)真實(shí)位置估計(jì)不當(dāng)。

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

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

1 FastSLAM算法

1.1 FastSLAM算法的流程

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

k時(shí)刻粒子集sk由該時(shí)刻所有粒子

預(yù)測(cè)機(jī)器人位姿， 根據(jù)k-1時(shí)刻粒子集中的每個(gè)粒子及其運(yùn)動(dòng)模型來(lái)計(jì)算k時(shí)刻的粒子集sk。

計(jì)算權(quán)重，k時(shí)刻第i個(gè)粒子的權(quán)值由k-1時(shí)刻第i個(gè)粒子的權(quán)值和k時(shí)刻第i個(gè)粒子本身的值所估計(jì)的k時(shí)刻粒子位姿zk得出，具體計(jì)算式為：

根據(jù)觀測(cè)模型計(jì)算粒子集sk-1對(duì)應(yīng)的特征點(diǎn)坐標(biāo)，并將它與環(huán)境陸標(biāo)集中的某一陸標(biāo)mk進(jìn)行關(guān)聯(lián)。

對(duì)粒子集權(quán)值進(jìn)行更新。

重采樣。

更新粒子集權(quán)重。

更新環(huán)境地圖。

更新粒子k時(shí)刻的位姿，若無(wú)人車(chē)?yán)^續(xù)運(yùn)行，則轉(zhuǎn)到預(yù)測(cè)機(jī)器人位姿步驟，否則結(jié)束定位。

1.2 FastSLAM算法的缺陷

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

另一個(gè)方面，在FastSLAM算法中粒子種群數(shù)量是固定不變的，如果種群數(shù)量過(guò)大，則會(huì)很大程度上影響運(yùn)算效率； 相反如果種群數(shù)量過(guò)小，則會(huì)導(dǎo)致運(yùn)算得出的最優(yōu)解不是理想結(jié)果，無(wú)人車(chē)實(shí)際路徑與預(yù)設(shè)路徑偏差過(guò)大。

2 基于種群動(dòng)態(tài)分布的雙變異優(yōu)化FastSLAM算法

傳統(tǒng)的FastSLAM算法中由于需要對(duì)粒子進(jìn)行重采樣，會(huì)導(dǎo)致種群中只有少數(shù)幾個(gè)粒子甚至單一高權(quán)重粒子參與到后續(xù)計(jì)算中［10］，為了改善粒子集的多樣性，同時(shí)避免自適應(yīng)粒子群優(yōu)化算法中容易出現(xiàn)的局部最優(yōu)現(xiàn)象，使無(wú)人車(chē)的狀態(tài)估計(jì)更加準(zhǔn)確，筆者將種群動(dòng)態(tài)分布策略和動(dòng)態(tài)雙變異粒子群算法引入到FastSLAM算法中，在保證計(jì)算效率和結(jié)果精度的前提下得到更加優(yōu)秀的粒子集［11］。

2.1 動(dòng)態(tài)種群分布策略

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

根據(jù)PSDE，計(jì)算每一代粒子到全局適應(yīng)度最優(yōu)粒子的距離時(shí)，將它與全局適應(yīng)度最差粒子到最優(yōu)粒子之間的距離進(jìn)行比較，假如距離大于最優(yōu)與最差粒子距離的粒子數(shù)量大于粒子總數(shù)的60%，則種群此時(shí)的狀態(tài)為分散，那么種群將隨機(jī)刪除一個(gè)除全局最優(yōu)粒子之外的粒子； 反之，假如距離小于最優(yōu)與最差粒子距離的粒子數(shù)量大于粒子總數(shù)的60%， 那么種群此時(shí)的狀態(tài)判定為收斂， 且極有可能已經(jīng)陷入局部最優(yōu)的狀態(tài)，此時(shí)將增加一個(gè)粒子，增加的粒子由下式?jīng)Q定：

其中，newPos為增加粒子的位置，Xmax和Xmin分別是搜索空間的上、 下限，gbest為全局最優(yōu)粒子，Gaussian是一個(gè)均值為μ（μ=0）、標(biāo)準(zhǔn)差為σ的高斯分布隨機(jī)數(shù)。

2.2 動(dòng)態(tài)雙變異優(yōu)化

在PSDE-FastSLAM算法中針對(duì)粒子種群的分布做出了動(dòng)態(tài)調(diào)整， 但為了避免出現(xiàn)如圖1所示的粒子分布（狀態(tài)估計(jì)判定為分散，位姿信息全部集中在某一個(gè)或者少數(shù)幾個(gè)粒子上）， 加入了兩個(gè)變異算子（Dynamic Dual Mutation，DD）［12］，通過(guò)對(duì)適應(yīng)度最優(yōu)的粒子和適應(yīng)度最差的粒子進(jìn)行高斯變異來(lái)控制算法， 提高算法的搜索能力，同時(shí)避免動(dòng)態(tài)種群分布策略產(chǎn)生的過(guò)度分散的情況。

圖1 粒子種群過(guò)度分散

加入的第1個(gè)變異算子是最差適應(yīng)度變異算子，對(duì)適應(yīng)度最差的粒子在迭代的每一代都進(jìn)行較大概率的高斯變異， 用來(lái)增加粒子的搜索空間。 在該算法中，對(duì)最差適應(yīng)度粒子以變異概率pm1進(jìn) 行 變 異，pm1值 根 據(jù) 經(jīng) 驗(yàn) 確 定， 一 般 在［0.08，0.50］取值，粒子變異過(guò)程為：

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

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

其中，gbesti為變異前的最優(yōu)適應(yīng)度粒子，γ為服從高斯分布的隨機(jī)量。

2.3 PSDE-FastSLAM-DD算法的實(shí)現(xiàn)

PSDE-FastSLAM-DD算法的實(shí)現(xiàn)步驟如下：

a. 根據(jù)初始坐標(biāo)，生成具有Num個(gè)粒子的粒子集s0和相應(yīng)的權(quán)值ω0，如前文式（1）～（3）；

b. 計(jì)算粒子適應(yīng)度值，根據(jù)適應(yīng)度來(lái)選擇局部歷史最優(yōu)適應(yīng)度粒子和全局最優(yōu)適應(yīng)度粒子；

c. 進(jìn)行PSDE步驟；

d. 對(duì)最差適應(yīng)度粒子進(jìn)行高斯變異，幫助粒子種群進(jìn)行收斂，避免過(guò)度分散；

e. 更新粒子適應(yīng)度以及局部歷史最優(yōu)適應(yīng)度粒子、全局最優(yōu)適應(yīng)度粒子；

f. 對(duì)最優(yōu)適應(yīng)度粒子進(jìn)行高斯變異， 幫助粒子種群跳出局部收斂狀態(tài)，保證運(yùn)算結(jié)果的精確性；

g. 更新環(huán)境地圖；

h. 計(jì)算粒子k時(shí)刻的位姿， 若無(wú)人車(chē)?yán)^續(xù)運(yùn)行，則轉(zhuǎn)到步驟b，否則結(jié)束更新。

3 仿真結(jié)果及實(shí)驗(yàn)分析

3.1 仿真模型

無(wú)人車(chē)的里程計(jì)數(shù)學(xué)模型如下［13，14］：

其中，Xk+1表示無(wú)人車(chē)在k+1時(shí)刻的位姿狀態(tài)，uk+1表示無(wú)人車(chē)在k+1時(shí)刻里程計(jì)的控制輸入，θk為k時(shí)刻無(wú)人車(chē)轉(zhuǎn)過(guò)的航向角，xk和yk分別為k時(shí)刻無(wú)人車(chē)的坐標(biāo)值，D為無(wú)人車(chē)的車(chē)長(zhǎng)，Δdk+1為單位時(shí)間內(nèi)無(wú)人車(chē)走過(guò)的路徑長(zhǎng)度，Δθk+1為單位時(shí)間內(nèi)無(wú)人車(chē)的偏轉(zhuǎn)角度。

觀測(cè)模型為：

其中，r和θ分別表示檢測(cè)到的環(huán)境特征與無(wú)人車(chē)之間的距離和運(yùn)動(dòng)航向角，xl和yl表示里程計(jì)接收的路標(biāo)信息，ψk表示觀測(cè)噪聲。

3.2 MATLAB結(jié)果分析

3.2.1 MATLAB仿真環(huán)境

為了驗(yàn)證PSDE-FastSLAM-DD算法的實(shí)際有效性，在實(shí)驗(yàn)中，針對(duì)天津理工大學(xué)實(shí)際地圖來(lái)創(chuàng)建仿真環(huán)境， 選取學(xué)校東門(mén)作為路徑起點(diǎn)，無(wú)人車(chē)車(chē)輛途徑體育館， 最后到達(dá)機(jī)械工程學(xué)院，對(duì)此創(chuàng)建了如圖2所示的一個(gè)基于路標(biāo)點(diǎn)和導(dǎo)航點(diǎn)的仿真環(huán)境， 環(huán)境中設(shè)置34個(gè)路標(biāo)，24個(gè)導(dǎo)航點(diǎn)。

圖2 仿真路徑

仿真中時(shí)間ΔT=0.025s， 運(yùn)動(dòng)過(guò)程噪聲為0.3m/s，仿真過(guò)程中取20個(gè)粒子，仿真結(jié)果如圖3所示。

圖3 仿真結(jié)果

在圖3中，黑色方塊為仿真對(duì)象無(wú)人車(chē)，綠色星號(hào)點(diǎn)為路標(biāo)點(diǎn)，紅色圓圈點(diǎn)為導(dǎo)航點(diǎn)，紅色細(xì)實(shí)線為預(yù)設(shè)路徑，藍(lán)色實(shí)線為無(wú)人車(chē)的實(shí)際行駛路徑， 黃色顯示為無(wú)人車(chē)在該點(diǎn)監(jiān)測(cè)到的路標(biāo)點(diǎn)，從仿真結(jié)果可以看出，無(wú)人車(chē)基本按照預(yù)設(shè)路徑完成了行駛過(guò)程。

3.2.2 仿真參數(shù)的選取

筆者在MATLAB仿真平臺(tái)上采用獨(dú)立實(shí)驗(yàn)分別 對(duì) 傳 統(tǒng) 的FastSLAM 算 法、 基 于APSO 的FastSLAM算法和改進(jìn)后的PSDE-FastSLAM-DD算法這3種不同的算法進(jìn)行仿真對(duì)比分析來(lái)驗(yàn)證改進(jìn)后算法在實(shí)際應(yīng)用中的有效性。

采用一個(gè)樣本空間， 粒子種群規(guī)模設(shè)定為30，算法迭代500次結(jié)束，選取Rastrigrin為測(cè)試函數(shù)，針對(duì)算法的均最小值、均耗時(shí)兩個(gè)方面進(jìn)行測(cè)試，從測(cè)試結(jié)果中得到最優(yōu)的參數(shù)。

由測(cè)試結(jié)果表1可得，選取最差適應(yīng)度粒子的變異概率pm1為0.20， 最優(yōu)適應(yīng)度粒子的變異概率pm2為0.03， 其他變異概率的取值都不利于算法的收斂。

表1 變異概率對(duì)算法性能影響的測(cè)試結(jié)果

3.2.3 3種算法對(duì)比

為了驗(yàn)證算法的優(yōu)越性， 采用一個(gè)樣本空間， 對(duì)比了傳統(tǒng)的FastSLAM算法、 基于APSO的FastSLAM算法和PSDE-FastSLAM-DD算法的無(wú)人車(chē)實(shí)際運(yùn)動(dòng)路徑與預(yù)設(shè)路徑之間的偏差，結(jié)果如圖4所示，結(jié)果顯示PSDE-FastSLAM-DD算法的無(wú)人車(chē)的運(yùn)動(dòng)路徑與真實(shí)值相比偏差最小，說(shuō)明它具有較高的計(jì)算精度。

圖4 3種算法路徑偏差的對(duì)比

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

表2 3種算法估計(jì)精度和運(yùn)行時(shí)間

4 結(jié)束語(yǔ)

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