激光導(dǎo)航AGV的馬爾可夫定位方法研究

李昊，葉文華，滿增光

(南京航空航天大學(xué) 機(jī)電學(xué)院，江蘇 南京 210016)

0 引言

自動導(dǎo)引車(automated guided vehicle，AGV)作為高端智能物流運(yùn)輸裝備正越來越多地應(yīng)用于自動化生產(chǎn)與制造中，定位問題是AGV導(dǎo)航運(yùn)動中的基本問題。馬爾可夫(Markov)定位算法是一種常用于初始位姿未知情況下的AGV全局定位方法[1]。該方法利用概率狀態(tài)分布，將對AGV位姿的估計(jì)看作多階Markov過程，離散化的空間位姿即代表其中的狀態(tài)變量。Markov定位方法不僅可以解決AGV被移動到新位置而已知初始位姿錯(cuò)誤的拐騙問題，而且可以處理多模及非高斯分布的概率模型。應(yīng)用上，Markov定位方法可以基于任意一種形式的地圖實(shí)現(xiàn)對AGV的全局定位[2]。對Markov定位的研究最早出現(xiàn)在基于拓?fù)涞貓D的定位中[3]，其后被廣泛應(yīng)用于基于柵格地圖的定位。Markov定位方法最大的問題在于對移動機(jī)器人位姿的離散化處理導(dǎo)致的龐大計(jì)算量。在以往的研究中，Burgard等[4]利用圖像處理中的八叉樹模型使?fàn)顟B(tài)空間可變分辨率表示；吳慶祥等[5]通過對Markov定位的仿真研究，使用電子羅盤解決了對稱環(huán)境中的定位問題。但針對計(jì)算量龐大限制Markov定位方法應(yīng)用的問題，始終沒有完善的簡化改進(jìn)方法。

Markov定位方法的龐大計(jì)算量主要體現(xiàn)在兩方面[6]：離散化后的AGV位姿是三維變量，計(jì)算時(shí)離散化柵格數(shù)量多；計(jì)算某一時(shí)刻單一柵格信度時(shí)涉及到前一時(shí)刻所有柵格的信度。針對這兩方面，本文采用高精度電子羅盤的方向信息作為AGV的姿態(tài)信息，從而將三維空間信息計(jì)算簡化為二維平面信息計(jì)算；同時(shí)在計(jì)算Markov算法中的預(yù)測模型時(shí)通過傅里葉變換將空間域轉(zhuǎn)換至頻域，將AGV的兩個(gè)平面位置變量視為一個(gè)整體處理，從而不必對相鄰時(shí)刻的柵格信度重新依次計(jì)算，減少了計(jì)算量。將此簡化預(yù)測模型計(jì)算方法結(jié)合觀測更新模型構(gòu)成Markov定位算法，從而實(shí)現(xiàn)對AGV的位姿估計(jì)與全局定位。

1 Markov定位算法

對全局定位問題而言，已知AGV的歷史觀測數(shù)據(jù)、運(yùn)動控制輸入和環(huán)境地圖，k時(shí)刻AGV位姿估計(jì)可表示為：

p(Xk|Z0:k,U0:k,M)

(1)

其中,Xk表示k時(shí)刻AGV的位姿；Z0:k表示從0到k時(shí)刻AGV對環(huán)境特征的觀測；U0:k表示從0到k時(shí)刻AGV所有運(yùn)動控制輸入；M表示環(huán)境地圖，對于不同形式的地圖有不同的表達(dá)形式。

(2)

(3)

其中，η=1/p(zk|Z0:k-1,U0:k)為歸一化常數(shù)，通過式(2)、式(3)的遞歸預(yù)測和校正遞推計(jì)算可以對初始位姿未知情況下的AGV估計(jì)當(dāng)前位姿，實(shí)現(xiàn)AGV全局定位。

2 基于降維和傅里葉變換的預(yù)測模型

由于觀測校正更新式(3)的計(jì)算方法與地圖形式息息相關(guān)且計(jì)算量在整個(gè)算法中不占主要構(gòu)成，本文主要對預(yù)測更新式(2)進(jìn)行簡化計(jì)算量處理，主要包括對空間信息的降維及相鄰時(shí)刻柵格的整體平移計(jì)算。

2.1 基于電子羅盤信息的降維處理

通過在AGV上安裝高精度的電子羅盤，姿態(tài)分量φ可由方向信息直接得到，繼而在計(jì)算中只考慮離散化后的AGV位置分量(x,y)的二維柵格信度，柵格信度計(jì)算的降維處理過程如圖1所示。

圖1 柵格信度計(jì)算的降維處理

2.2 基于空間域-頻域變換的預(yù)測模型

若把二維的柵格信度平面稱為信度圖像，將任一時(shí)刻的信度圖像看作整體實(shí)現(xiàn)更新計(jì)算。即在得到運(yùn)動輸入uk后，同時(shí)考慮其不確定性，通過信度模糊化處理把k-1時(shí)刻的信度圖像看作整體平移形成k時(shí)刻信度圖像。

實(shí)際上在進(jìn)行信度圖像整體平移操作時(shí)，由于AGV運(yùn)動的連續(xù)性，為避免導(dǎo)致誤差的累積，需要實(shí)現(xiàn)信度圖像的非整數(shù)倍單位柵格平移。本文采用基于空間域-頻域轉(zhuǎn)換的預(yù)測更新模型，同時(shí)也在頻域中完成信度圖像的模糊化操作。為此需要通過傅里葉變換轉(zhuǎn)換信度圖像，二維離散傅里葉變換公式可表示為：

(4)

其中，f和F分別表示空間域與頻域圖像；M和N分別表示x與y方向的柵格劃分?jǐn)?shù)目。

空間域與頻域中的平移關(guān)系可表示為：

f(x-x0,y-y0)?F(u,v)e-i2π(ux0/M+vy0/N)

(5)

其中，x0和y0分別表示空間域中信度圖像的平移量，其可以是任意實(shí)數(shù)。

為實(shí)現(xiàn)信度圖像的非整數(shù)倍柵格平移，基于式(4)和式(5)，先將空間域中的f(x,y)轉(zhuǎn)換至頻域，得到F(u,v)，利用傅里葉變換的周期性中心化F(u,v)，利用卷積定理通過高斯低通濾波實(shí)現(xiàn)對柵格信度圖像的模糊化處理，平移后的F(u,v)e-i2π(ux0/M+vy0/N)作為頻域中新的信度圖像再轉(zhuǎn)換回空間域。

圖2 基于傅里葉變換的預(yù)測模型

2.3 綜合算法實(shí)現(xiàn)流程

基于降維和傅里葉變換處理預(yù)測模型的AGV馬爾可夫全局定位綜合算法實(shí)現(xiàn)如下：

k=0 ；

End

While TRUE

k=k+1；

w=0；

End

End

End

3 仿真分析

為驗(yàn)證本文提出的基于降維和傅里葉變換的預(yù)測模型計(jì)算方法的有效性和準(zhǔn)確性，基于特征地圖進(jìn)行了復(fù)合環(huán)境中AGV運(yùn)動的仿真。該仿真環(huán)境如圖3所示，長寬均為30 m，設(shè)置50個(gè)特征點(diǎn)，其中49個(gè)特征點(diǎn)以均勻分布在環(huán)境中，構(gòu)成相似性環(huán)境，以此驗(yàn)證算法的可靠性；第50個(gè)特征點(diǎn)使AGV在短暫的運(yùn)動探測中能收斂到唯一位置。設(shè)置5個(gè)導(dǎo)航點(diǎn)A(0，-4)，B(7，-4)，C(7, 7)，D(2.5, 7.5)，E(-2.5, 5)，AGV初始時(shí)刻位于第1個(gè)導(dǎo)航點(diǎn)(0，-4)，初始朝向與初始運(yùn)動方向均平行于x軸，最大線速度為1m/s，運(yùn)動軌跡如圖3所示。傳感器及算法相關(guān)參數(shù)如表1所示。

圖3 相似復(fù)合環(huán)境下所設(shè)置的環(huán)境特征地圖

表1 相似復(fù)合環(huán)境中的AGV全局定位參數(shù)設(shè)置

仿真所得信度分布結(jié)果如圖4所示。從圖4(a)中可知，在初始k=0s時(shí)刻，AGV實(shí)際位置在(0, -4)，經(jīng)過計(jì)算各柵格被占有概率，得到了眾多信度大值坐標(biāo)點(diǎn)。由圖4(b)可知，在k=5s時(shí)，AGV運(yùn)動在AB段，得到相似環(huán)境中的若干對稱位置。在k=12s時(shí)，AGV實(shí)際運(yùn)動接近于導(dǎo)航點(diǎn)B，此時(shí)經(jīng)過計(jì)算各柵格被占有概率，存在信度極大值的范圍位置變?yōu)?個(gè)，AGV此刻的位置估計(jì)在這9個(gè)坐標(biāo)附近。圖4(c)-圖4(f)顯示了之后不同時(shí)刻AGV在概率柵格中的位姿信度極值分布和變化情況，在k=76s時(shí)，AGV探測到了第50個(gè)特征點(diǎn)，信度峰值極大值坐標(biāo)收斂至一個(gè)，其他位置的概率信度均為0，即已經(jīng)正確估計(jì)出了AGV位姿。從上述位姿信度的變化可知，隨著AGV不斷運(yùn)動，感知到的特征信息不斷增加，AGV從算法最初估計(jì)的多個(gè)坐標(biāo)位置逐漸確定為唯一真實(shí)坐標(biāo)。AGV位置估計(jì)與真實(shí)位姿的誤差如圖5所示，均控制在一個(gè)柵格大小以內(nèi)。以上結(jié)果表明了AGV在相似復(fù)合環(huán)境中運(yùn)動時(shí)，本文所提的基于高斯核函數(shù)的Markov全局定位方法在特征地圖中是有效且準(zhǔn)確的。

圖4 相似復(fù)合環(huán)境下AGV直線行走仿真不同時(shí)刻信度分布圖像

圖5 AGV位置估計(jì)誤差

4 結(jié)語

利用電子羅盤信息降低了求解AGV位姿信息的維度，且在計(jì)算Markov定位算法中的預(yù)測模型時(shí)利用頻域變換將柵格信度視為整體計(jì)算，簡化了信度更新時(shí)對相鄰時(shí)刻柵格信息的依賴。加入預(yù)測模型新計(jì)算方法的Markov定位算法能有效運(yùn)用在特征地圖中對AGV的全局定位，不僅提高了Markov定位在特征地圖中的實(shí)用性，也對其他類型的移動機(jī)器人和其他地圖形式的定位需求提供了參考。