雷建和，丁 磊，孫合勝，陳 健

(青島理工大學信息與控制工程學院，山東 青島 266000)

1 引言

同步定位與地圖構建(Simultan eous Localization And Mapping，SLAM)在機器人領域方面有著重要的作用。SLAM主要解決的是移動機器人在未知環(huán)境中移動時根據(jù)自身的傳感器信息進行地圖構建的問題和由于自身在運動過程中產(chǎn)生的誤差所引起的定位問題，這兩個問題是關聯(lián)度很高的問題。因此SLAM主要解決的是構建地圖和同步定位這兩大問題。這兩個問題又分成三種情況。1)地圖已知的情況下，解決定位的問題。2)在定位已知的情況下，進行地圖構建。3)在未知環(huán)境中進行地圖構建以及定位問題?，F(xiàn)在大部分的SLAM問題都是在這三種情況下研究的。

SLAM的算法主要是以概率機器人學為主。最初是卡爾曼根據(jù)貝葉斯濾波器提出的卡爾曼濾波算法(Kalman filter，KF)，KF可以計算出線性條件下連續(xù)狀態(tài)的概率。之后Chessseman和Smith等人在此基礎上提出了擴展卡爾曼濾波算法(extended Kalman filter，EKF)，EKF可以計算在非線性條件下的概率。在此基礎上，Montemerlo和Thrun等人根據(jù)粒子濾波器(Rao-Blackwellized Particle Filter，RBPF)提出了FastSLAM算法，之后又在此基礎上提出了FastSLAM2.0算法。Alex等人以FastSLAM為前端EKF為后端，提出了HybridSLAM算法。該算法不需要對運動進行線性化處理，通過子地圖融合避免了粒子退化的缺點，可以快速的建圖。但在粒子多樣性方面改進并不大。

針對多樣性的問題，很多學者都在對其改進。張廷軍等通過權重將粒子分為穩(wěn)定和不穩(wěn)定粒子，對不穩(wěn)定粒子進行改進，但該算法將大權值粒子歸入了不穩(wěn)定粒子，沒有將計算傾向于大權值粒子。張亞楠等在重采樣過程中引入了粒子權重平衡的方法，但該方法具有一定的局限性。劉剛等通過將遺傳算法放入粒子濾波中進行重采樣以改進粒子的多樣性，但其沒有考慮自適應的問題，有一定的局限性。諶劍等通過將粒子權值與權值均值線性組合的方式改進粒子的權重的方式提出了權值優(yōu)化組合粒子濾波算法。該算法雖然提高了精度，但減小了大權值粒子的權重。

由于遺傳算法主要運用的是進化的思想，而粒子濾波中粒子退化問題是主要問題。因此可以用自適應遺傳算法解決粒子退化的問題。為了讓不同粒子進行不同程度的改變，最終進化整個種群，因此將自適應遺傳算法進行改進，以便在粒子濾波中更合適的應用。該方法通過對粒子自適應的進行交叉和變異，保證了粒子的多樣性，提高了數(shù)據(jù)準確性和全局地圖的精度。

2 HybridSLAM算法和自適應遺傳算法

2.1 HybridSLAM算法

HybridSLAM算法以FastSLAM2.0為前端，EKF-SLAM為后端，將兩種濾波器的優(yōu)點結合，規(guī)避了兩者的缺點，使其性能優(yōu)于兩者。FastSLAM2.0作為前端，發(fā)揮其構建地圖精確的優(yōu)點，主要對移動機器人的位姿進行精確定位以及構建地圖。EKF-SLAM作為后端，主要用來記錄機器人的路徑以及外界特征，可以很好的記錄其隨時的位置變化。規(guī)避了FastSLAM2.0可能出現(xiàn)離子性匱乏的缺點。

假設前端共有

M

個粒子。第

p

個粒子在

t

時可以表示為

(1)

將粒子的權重歸一化

(2)

則機器人的當前位姿和已觀測到的路標位置

x

和協(xié)方差

P

可表示為

(3)

(4)

式(4)中，方括號內(nèi)的第一項是包含傳感器誤差的單個粒子構建地圖的協(xié)方差，第二項為包含機器人位姿誤差的各粒子所建地圖之間的協(xié)方差。

子地圖構建時，每個粒子都要進行預測和更新兩個過程。

預測：第

P

個粒子在

t

時刻預測的機器人位姿

(5)

其中

u

為機器人在

t

時刻的控制輸入。路標更新：當檢測到特征路標時，假設特征關聯(lián)已知，如果檢測到的特征

j

是未被觀測特征時。則粒子權重

ω

設為初始值

ω

=1

/M

。

由觀測方程的倒數(shù)得到該特征的位置

(6)

其中

z

是機器人在

t

時刻的觀測值，

h

為觀測方程。如果檢測到的特征

j

為已經(jīng)觀測過的，則每個粒子都沿用EKF-SLAM算法的更新方式進行狀態(tài)估計更新。由觀測方程計算出第

p

個粒子的預測觀測值

(7)

算出對應的卡爾曼增益

(8)

位姿更新為

(9)

相應的協(xié)方差更新為

(10)

(11)

2.2 自適應遺傳粒子濾波

自適應遺傳粒子濾波算法(Adaptive genetic algorithm-Particle Filter，AGA-PF)以遺傳粒子濾波算法為基礎，主要解決粒子多樣性的問題。該算法簡單、魯棒性好、而且可以自適應的根據(jù)不同粒子調(diào)節(jié)交叉和變異的概率。

自適應遺傳濾波以濾波器產(chǎn)生的粒子作為初始種群，根據(jù)粒子的特性可以將粒子權重看作對應個體的適應度。之后通過輪盤賭的方式選擇出新的種群；然后對選擇出的種群進行兩兩配對，根據(jù)個體的適應度算出配對個體的交叉概率，并按此概率直接對實數(shù)進行交叉操作；再通過每個個體的適應度算出各自的變異概率，同樣按此概率對實數(shù)進行變異操作，最終產(chǎn)生新的種群。根據(jù)進化代數(shù)與設定值的比較判斷是否終止，若不終止則繼續(xù)以新種群進行選擇、交叉和變異迭代。

在交叉和變異過程中，兩個體之間以(12)計算交叉概率，每個個體以式(13)計算變異概率。

(12)

(13)

個體對應的交叉操作如式(14)所示

(14)

個體對應的變異操作如式(15)所示

(15)

3 iAGA-HybridSLAM

3.1 改進的AGA-PF

自適應遺傳算法在粒子濾波中應用時，要結合粒子濾波的情況進行改進。在粒子濾波中，權值相近的粒子，交叉后對預估值影響較小，權值越大的粒子，越接近真實值。因此針對交叉概率，既要保證適應度高的個體交叉率高，又要確保權值相近的粒子交叉率低。就要將式(12)改為式(16)。設定

f

為大適應度，

f

為小適應度，其它與之前相同。

(16)

同樣，為保證交叉操作對高權重粒子影響較小，以及解決兩個小適應度個體交叉后改變不大的問題。式(14)也要進行相應的改變。

(17)

(18)

交叉后相應的適應度也要改變，如式(19)所示，

α

和

β

如式(18)所示

(19)

在變異過程中為保證較大適應度的個體也有極小的變異概率，將式(13)進行改進

(20)

迭代的結束，通過設定迭代總次數(shù)決定。在選擇階段，通過判斷粒子群的有效粒子數(shù)Neff值與設定閾值T的比較，決定是否進行交叉和變異階段。

3.2 iAGA-HybridSLAM

HybridSLAM中簡單重采樣造成的粒子退化是不可避免地。為了減少這種粒子退化造成的粒子多樣性損失，用iAGA算法替代原來的重采樣。算法偽代碼如下：

3) 通過第二步的數(shù)據(jù)進行位姿和權值的更新。

4) 計算更新后的Neff值，判斷是否進行重采樣。若進行重采樣，則通過權值得到每個粒子的適應度，以此進行選擇、交叉和變異，進行優(yōu)化。最終迭代出新的粒子群，進行歸一化處理，完成重采樣。

5) 將新的數(shù)據(jù)融入高斯分布中，計算出

K

時刻的估計值。6) 判斷算法是否完成。若未完成則

K

=

K

+1，返回第2)步，繼續(xù)迭代。

4 仿真結果

該仿真以MATLAB為平臺，使用的是悉尼大學發(fā)布的SLAM仿真器進行仿真。仿真器界面為200m×160m設定小車移動速度為3m/s，車輪最大轉(zhuǎn)角為30°，最大轉(zhuǎn)速為20°，控制噪聲為±0.3m和3°，觀測噪聲為±0.1m和1°，激光雷達觀測角度和觀測范圍分別為360°和30m?！?’表示設定的特征路標，‘×’表示觀測特征路標的后驗觀測位置，‘○’為設定移動軌跡點，‘▲’為小車模型(實際和預估)。細線為小車預設路線，粗線為每個時間步對小車實際位置的預估點形成的路徑。

在上述條件下，機器人分別使用HybridSLAM算法，WOC-HybridSLAM(基于權值優(yōu)化組合的HybridSLAM算法)算法，AGA-HybridSLAM算法以及iAGA-HybridSLAM算法移動一圈繪制地圖， 使用的粒子數(shù)設為100，根據(jù)文獻[16]可知，權值優(yōu)化組合中的T設置在2～12合適，因此設定T為10。AGA中設定Neff小于75時進行重采樣，重采樣中Neff大于閾值85時選擇全部粒子否則進行交叉和變異。迭代次數(shù)為50，由文獻[17]可知，

k

=

k

=0

.

5，

k

=0

.

005、

k

=0

.

05。結果如圖1～4所示。

圖1 HybridSLAM繪制的地圖

圖2 WOC-HybridSLAM繪制的地圖

圖3 AGA-HybridSLAM繪制的地圖

圖4 iAGA-HybridSLAM繪制的地圖

為了判斷地圖精確性和定位準確性，將4種SLAM算法分別運行50次。圖5為每種算法觀測到的特征路標誤差取平均值，繪制的曲線。圖6為4種算法在每個時間步時根據(jù)小車位姿誤差取平均值繪制的曲線。

圖5 四種算法的特征路標誤差

從圖5可以看出HybridSLAM算法的特征誤差最大；而AGA-HybridSLAM和WOC-HybridSLAM的誤差都比HybridSLAM算法的?。籭AGA-HybridSLAM檢測到的特征誤差最小，構建的地圖最精確。這說明對AGA的改進是有作用的。

從圖6中可以看出HybridSLAM算法的誤差最大。雖然算法在中間時刻矯正了一些誤差，但依然很大。AGA-HybridSLAM的誤差相對較?。籛OC-HybridSLAM的變化誤差幅度略小，但整體值很大；iWOC-HybridSLAM的誤差值最小，雖然在短時間內(nèi)突變幅值較大，但整體中值變化不大，這說明該算法定位更準確。

圖6 四種算法之間的位姿誤差

為了準確的對算法精準性進行更準確的評估，需要用最小均方根RMSE來進行評估。

(21)

將每種算法每次繪制地圖1次，作為1個RMSE值，計算每種算法的50個RMSE的平均RMSE值，結果如表1所示。

表1 四種算法的RMSE比較

由表1可以看出，前三個算法中HybridSLAM的兩個RMSE值比WOC-HybridSLAM和AGA-HybridSLAM的都要高；而AGA-HybridSLAM在定位上比WOC-HybridSLAM要精確，在地圖構建上要差一些。iWOC-HybridSLAM算法相比于WOCHybridSLAM、AGA-HybridSLAM以及HybridSLAM算法，兩個平均RMSE值都要低。由此可以看出iAGA-HybridSLAM算法在地圖構建和自身定位的精確性上更高。

5 結論

本文在HybridSLAM算法的基礎上，添加了自適應遺傳算法并根據(jù)粒子濾波的特性對其進行了改進，提出了iAGA-HybridSLAM算法。將自適應遺傳算法替代原來的簡單重采樣，根據(jù)其進化的理念，解決了粒子退化問題，保證了粒子的多樣性。根據(jù)仿真可以看出，在相同的實驗條件下，iAGA-HybridSLAM能夠有效減緩粒子耗散的同時，提高了地圖的精度。該算法在解決粒子多樣性減小的問題上提供了參考方法。