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

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

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

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

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

1 無人車FastSLAM的算法模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

具體交叉操作按照式（10）進(jìn)行：

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

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

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

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

圖1所示為自適應(yīng)粒子群優(yōu)化算法的具體步驟，在算法執(zhí)行交叉變異操作之后，更新粒子的適應(yīng)度，用備份的原全局最優(yōu)粒子替換適應(yīng)值最差的粒子；再按照標(biāo)準(zhǔn)的粒子群算法更新種群中每個(gè)粒子的速度和位置，更新全局最優(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算法的實(shí)現(xiàn)過程

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

詳細(xì)實(shí)現(xiàn)過程如下：

步驟1：計(jì)算各個(gè)粒子的后驗(yàn)位姿建議分布.

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

式（12）中，xk為粒子在k時(shí)刻的采樣位姿；xk-1為k-1時(shí)刻粒子的采樣位姿；zk為k時(shí)刻的觀測(cè)信號(hào)；uk為k時(shí)刻的控制信號(hào)，i取1到N.

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

步驟4：計(jì)算粒子的篩選區(qū)間，并對(duì)區(qū)間外的粒子進(jìn)行移動(dòng)篩選區(qū)間：

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

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

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

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

步驟8：地圖估計(jì)與更新.

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

3.1 仿真模型

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

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

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

3.2 仿真結(jié)果分析

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

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

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

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

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

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

圖3 算法運(yùn)動(dòng)偏差對(duì)比Fig.3 Simulation environment

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

表1 估計(jì)方根誤差和算法運(yùn)行時(shí)間表Tab.1 Estimated square root error and algorithm runtime table

4 結(jié)論

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