許柏楊，王冬青

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

0 引言

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

1 運(yùn)動(dòng)學(xué)模型

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

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

當(dāng)采樣周期為Ts，則AGV運(yùn)動(dòng)學(xué)方程如下：

其中符號說明如下：

圖1 AGV的運(yùn)動(dòng)模型

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

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

b：兩個(gè)驅(qū)動(dòng)輪之間的距離

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

rl，rr：左右車輪的標(biāo)稱直徑

rl*，rr*：左右車輪的實(shí)際直徑

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

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

T：采樣周期。

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

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

兩個(gè)驅(qū)動(dòng)輪間的實(shí)際距離為：

因此，實(shí)際左右驅(qū)動(dòng)輪的速度為：

因此，AGV的實(shí)際速度為：

AGV位移和角度表示為：

AGV位置方程為：

2 IEKF SLAM

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

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

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

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

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

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

圖2 移動(dòng)過程中AGV的SLAM模型

2.1 預(yù)測階段

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

基于EKF-SLAM方法描述車輛的運(yùn)動(dòng)模型為：

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

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

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

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

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

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

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

2.3 更新階段

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

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

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

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

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

殘差協(xié)方差：

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

2.4 狀態(tài)擴(kuò)充

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

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

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

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

2.5 計(jì)算的復(fù)雜性

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

3 D＊算法

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

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

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

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

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

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

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

圖3 IEKF-SLAM仿真效果圖

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

圖4 在X軸上的路標(biāo)位置誤差

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

圖5 在Y軸上的路標(biāo)位置誤差

圖6 機(jī)器人在X軸上的位置誤差

圖7 機(jī)器人在Y軸上的位置誤差

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

5 結(jié)論

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