一種改進的EKF-SLAM算法

許柏楊，王冬青

（青島大學(xué) 自動化與電氣工程學(xué)院，青島 266071）

0 引言

隨著現(xiàn)代物流的快速發(fā)展，企業(yè)對物流系統(tǒng)的自動化與靈活性有著很高的要求，自動導(dǎo)引車（AGV）是一種常見的運輸工具。傳統(tǒng)的AGV依賴于既定的靜態(tài)結(jié)構(gòu)化工作環(huán)境，它的智能化與靈活性有限，于是人們提出了AGV的同時定位與地圖構(gòu)建（SLAM）來克服這些缺點。目前，解決SLAM問題中的EKF-SLAM，PF-SLAM，F(xiàn)astSLAM及UKF-SLAM算法中，EKFSLAM算法最先被提出、最為流行。張超等人使用改進的Shi-Tomas特征點檢測算法進行特征點提取，從而提高SLAM中的地圖構(gòu)建的準確性，但計算復(fù)雜度高；賀利樂等人針對移動機器人在未知環(huán)境下自主導(dǎo)航時，僅利用單一傳感器出現(xiàn)的不能及時定位、構(gòu)建的地圖不精確、魯棒性不強等問題，提出一種雙目視覺與激光雷達信息融合的組合導(dǎo)航方法，但效果并不理想；吳勇等在無跡卡爾曼濾波的基礎(chǔ)上提出了一種收縮無跡卡爾曼濾波器，并應(yīng)于于SLAM問題中，降低了SLAM中的大規(guī)模的復(fù)雜程度，但并沒有解決生成特征圖的準確問題；白云漢 將卷積神經(jīng)網(wǎng)絡(luò)的算法與SLAM算法融合，提高了系統(tǒng)的定位精度，但計算的復(fù)雜度較高。然而，在解決移動機器人的定位與地圖構(gòu)建問題的過程中，傳統(tǒng)擴展卡爾曼濾波的計算復(fù)雜度高，生成的特征圖準確性不足，并且通常與導(dǎo)航分離，基于此，在本文中提出了IEKF-SLAM算法并且使其與D*導(dǎo)航算法結(jié)合，通過將機器人的系統(tǒng)參數(shù)附加到EKF SLAM的狀態(tài)向量中，在執(zhí)行預(yù)測和更新EKF SLAM的狀態(tài)時，對附加系統(tǒng)參數(shù)的狀態(tài)向量進行估計，消除內(nèi)部傳感器引起的誤差，從而提高特征圖的準確性，實現(xiàn)AGV的完整狀態(tài)估計，并且針對EKF-SLAM算法計算復(fù)雜度高的問題，提出了相關(guān)的算法，使AGV在動態(tài)和非結(jié)構(gòu)化的環(huán)境中能夠自動定位并準確跟蹤路徑，提高了系統(tǒng)的工作效率和系統(tǒng)靈活性，降低施工成本。

1 運動學(xué)模型

如圖1所示AGV運動模型，具有不同速度的四輪結(jié)構(gòu)。兩個驅(qū)動輪位于車身對稱的位置，兩個從動輪安裝在垂直于車身兩側(cè)的位置，該車輛由兩個驅(qū)動輪驅(qū)動。

假定質(zhì)心與所述AGV的幾何中心點相同。在一個采樣周期中，AGV的運動可以被近似為一個弧。在相鄰的采樣時間的位置矢量用x，y，θ和x(k+1)，y（k+1），θ（k+1）表示，如圖1所示。

當采樣周期為Ts，則AGV運動學(xué)方程如下：

其中符號說明如下：

圖1 AGV的運動模型

v：AGV的質(zhì)心速度

ω：AGV質(zhì)心的角速度

b：兩個驅(qū)動輪之間的距離

vl，vr：左右驅(qū)動輪的速度

rl，rr：左右車輪的標稱直徑

rl*，rr*：左右車輪的實際直徑

?s：AGV在一次取樣中的位移

?θ：AGV的偏轉(zhuǎn)角度

T：采樣周期。

（xv（i），yv（i），θ（i））T：第i時刻的AGV位置向量

設(shè)δl，δr為左右車輪直徑的系統(tǒng)參數(shù)，δb為輪距的系統(tǒng)參數(shù)，則左右車輪的實際直徑為：

兩個驅(qū)動輪間的實際距離為：

因此，實際左右驅(qū)動輪的速度為：

因此，AGV的實際速度為：

AGV位移和角度表示為：

AGV位置方程為：

2 IEKF SLAM

基于EKF-SLAM的方法對系統(tǒng)的運動模型和傳感器的噪聲模型是非常敏感的，因此，我們需要一個準確的模型，但是，在真實的機器人系統(tǒng)中很難找到準確的運動模型和噪聲模型，除此之外，由于EKF通常有一個假設(shè)的高斯白噪聲，如果有一個有色噪聲或系統(tǒng)偏差，那么無法保證特征提取的準確性，例如，如果車輛左右車輪的距離或車輪的半徑不精確，將會造成系統(tǒng)的估計誤差不斷增加，從而準確性大大降低。

我們可以通過估計系統(tǒng)參數(shù)δl，δr和δb的方法來減少機器人內(nèi)部里程計的誤差。在IEKF-SLAM中，系統(tǒng)參數(shù)將增加到系統(tǒng)的狀態(tài)向量中作為其元素，然后系統(tǒng)參數(shù)，機器人的狀態(tài)以及觀察特征的位置將會通過卡爾曼濾波算法的步驟被更新，更新后的系統(tǒng)參數(shù)將會減少在預(yù)測過程中的里程計的測量誤差，從而使EKFSLAM方法的特征地圖更加精確。具體的過程如下。

車輛在K時刻的整體狀態(tài)x（k）由車輛在K時刻的位置狀態(tài)Xv（k）=[xv（k），yv（k），θv（k）]T和地標的位置Xm（k）=（x1，y1，x2，y2，...，xm，ym）T，以及車輛的系統(tǒng)誤差的參數(shù)向量Xω=[δl，δr，δb]T組成，（x，y）是第j個地標的位置。即：

系統(tǒng)相應(yīng)的協(xié)方差矩陣為：

AGV運動過程模型如圖2所示，暗區(qū)是估計的信息，而淺色區(qū)域是實際信息；xk是AGV在k時刻的狀態(tài)，每個時刻狀態(tài)的數(shù)據(jù)構(gòu)成了AGV的軌跡；Zk是在k時刻觀察到第j個地標；uk是在（k-1）時刻的控制輸入。

AGV從不確定的初始位置移動逐步獲得估計的環(huán)境地圖，并估算AGV自身的位姿。同步定位和創(chuàng)建地圖可以分為四個步驟。

圖2 移動過程中AGV的SLAM模型

2.1 預(yù)測階段

在k時刻，設(shè)具有系統(tǒng)參數(shù)誤差的狀態(tài)向量為Xw（k）=[δl，δr，δb]T，系統(tǒng)整體狀態(tài)如下：

基于EKF-SLAM方法描述車輛的運動模型為：

其中，W（k）表示過程噪聲，在此我們忽略掉除系統(tǒng)參數(shù)誤差外的其他噪聲。

因此，車輛在 k 時刻的預(yù)測運動學(xué)方程為：

其中，考慮到系統(tǒng)參數(shù)的AGV運動模型為：

協(xié)方差預(yù)測為：

其中，Q是表示不確定性的協(xié)方差控制向量；▽f為車輛狀態(tài)(13)的雅可比矩陣：

2.2 數(shù)據(jù)關(guān)聯(lián)

在移動機器人SLAM中，數(shù)據(jù)關(guān)聯(lián)用來建立觀測量與地圖中地標的關(guān)系，并且每一個觀測量對應(yīng)一個地圖中的地標，在（k+1）次，AGV獲得新的地標觀測量，對于觀測到的地標，車輛將更新地標位置（步驟3）；否則進行狀態(tài)擴充（步驟4）。目前有一些廣泛使用的數(shù)據(jù)關(guān)聯(lián)算法，例如最近鄰方法（NNM），概率數(shù)據(jù)關(guān)聯(lián)（PDA）算法等。

2.3 更新階段

在k時刻，根據(jù)數(shù)據(jù)關(guān)聯(lián)算法AGV獲得觀察數(shù)據(jù)，與觀察數(shù)據(jù)匹配的地標j的估計位置是。因此，第j個地標的觀測量是：

其中，R表示地標的距離，j是觀察到的地標相對于AGV的角度，Zj表示第j個地標的觀測量。

對測量值線性化并獲得雅可比矩陣：

Rk是觀察誤差的協(xié)方差矩陣：

根據(jù)擴展卡爾曼濾波我們可以獲得卡爾曼增益矩陣：

殘差協(xié)方差：

然后通過式(21)、式(22)分別獲得機器人更新的狀態(tài)向量和協(xié)方差：

2.4 狀態(tài)擴充

隨著車輛的移動，通過數(shù)據(jù)關(guān)聯(lián)，新的地標將出現(xiàn)在AGV的觀察范圍內(nèi)。因此，它有必要通過在地圖中添加新的地標來更新（k+1）時刻的狀態(tài)。

更新后，系統(tǒng)狀態(tài)向量為：

至于擴充的協(xié)方差矩陣，首先，分別計算在新的地標下的機器人位姿的雅可比矩陣和觀測量的雅克比矩陣：

然后通過在上一時刻的協(xié)方差矩陣和新地標的雅可比矩陣得到擴充協(xié)方差矩陣：

2.5 計算的復(fù)雜性

由于協(xié)方差矩陣的更新，EKF-SLAM的計算復(fù)雜度為O（N2），其中N是環(huán)境中的地標數(shù)量。這限制了在大規(guī)模環(huán)境等特征數(shù)量大的場合中使用EKF。因此，很多不同的改進算法被提出，其中，主要是利用分區(qū)更新和子映射的方法降低復(fù)雜性。分區(qū)更新的方法是把傳感器的更新速率限制在一個小的局部區(qū)域，使它以一個更低的頻率更新整個地圖，從而減少計算的負擔。目前，分區(qū)更新的改進方法有很多，例如壓縮EKF（CEKF）[13]，延遲更新算法[14]等。子映射方法是將總區(qū)域劃分為多個子區(qū)域，每個子區(qū)域都有和全局坐標相關(guān)聯(lián)的局部坐標，這樣預(yù)測和更新的過程就會在子圖中較少的地標執(zhí)行，減少了計算負擔。目前，子映射的改進方法也有很多，例如解耦隨機映射（DSM）算法[15]，局部地圖排序算法[16]。

3 D＊算法

在本文中，由于Dijstra算法不適用于在動態(tài)環(huán)境下，因此采用D*算法用于AGV的最短路徑跟蹤。其具體的方法是：

1）先用Dijstra算法從目標節(jié)點G向起始節(jié)點搜索。儲存路網(wǎng)中目標點到各個節(jié)點的最短路和該位置到目標點的實際值h，k（k為所有變化h之中最小的值）

2）機器人沿最短路開始移動，在移動的下一節(jié)點沒有變化時，無需計算，利用上一步Dijstra計算出的最短路信息從出發(fā)點向后追述即可，當在Y點探測到下一節(jié)點X狀態(tài)發(fā)生改變，如堵塞。機器人首先調(diào)整自己在當前位置Y到目標點G的實際值h（Y），h（Y）=X到Y(jié)的新權(quán)值c（X，Y）+X的原實際值h（X）X為下一節(jié)點（到目標點方向Y-＞X-＞G），Y是當前點。k值取h值變化前后的最小。

3）用A*或其他算法計算Y的字節(jié)a的值，這里假設(shè)用A*算法，遍歷Y的子節(jié)點，點放入CLOSE，調(diào)整Y的子節(jié)點a的h值，h（a）=h（Y）+Y到子節(jié)點a的權(quán)重C（Y，a），比較a點是否存在于OPEN和CLOSE中。如果a的值小于OPEN以及CLOSE表中的a值，則更新OPEN和CLOSE表中的a值。

4）利用第一步Dijkstra算法計算出a點到目標點的最短路徑，重復(fù)上述操作，完成最短路徑的跟蹤。

4 仿真與結(jié)果分析

為了驗證具有系統(tǒng)參數(shù)估計的IEKF-SLAM具有更好的操作性能，我們進行了仿真實驗，仿真實驗室在MatlabR2014a平臺上進行的，首先X軸坐標為-10m～20m、Y軸坐標為-25m～20m，范圍內(nèi)產(chǎn)生若干個環(huán)境特征，假設(shè)機器人和目標均為理想模型，目標的實際運行軌跡是不規(guī)則的，仿真實驗總體如圖3所示。

圖3 IEKF-SLAM仿真效果圖

圖3的結(jié)果表明，增強的擴展卡爾曼濾波算法使AGV在跟蹤路徑上有更好的效果。

圖4 在X軸上的路標位置誤差

圖4和圖5顯示了機器人的實際路標位置和測量路標位置的均方根誤差，從仿真結(jié)果中我們得出車輛在X軸上的路標位置均方根誤差為22.5m，在Y軸上的路標位置均方根誤差為6m。

圖5 在Y軸上的路標位置誤差

圖6 機器人在X軸上的位置誤差

圖7 機器人在Y軸上的位置誤差

圖6和圖7分別表明機器人在X軸和Y軸的位置均方根誤差。從仿真結(jié)果得，機器人在X軸上的位置均方根誤差為-4m，在Y軸上的位置均方根誤差為0.9m。

5 結(jié)論

在本文中，我們針對擴展卡爾曼濾波算法在大范圍環(huán)境等特征數(shù)量較大的場合中計算復(fù)雜度高，并且生成的特征圖準確性不足的問題，提出了IEKF-SLAM方法，在IEKF-SLAM中，我們將機器人的系統(tǒng)參數(shù)附加到EKF-SLAM的狀態(tài)向量中，在執(zhí)行預(yù)測和更新EKF SLAM的狀態(tài)時，對附加系統(tǒng)參數(shù)的狀態(tài)向量進行估計，提高了EKF-SLAM的生成特征圖的準確性，并提出了一些減少計算復(fù)雜性的方法，除此之外，結(jié)合了D*導(dǎo)航算法。通過仿真的實驗結(jié)果我們可以看出，采用對系統(tǒng)參數(shù)估計的增強擴展卡爾曼濾波的機器人在生成的特征圖方面有更好的準確性，這種方法提高了工作效率，增強了系統(tǒng)的靈活性。