激光導航AGV的馬爾可夫定位方法研究

李昊，葉文華，滿增光

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

0 引言

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

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

1 Markov定位算法

對全局定位問題而言，已知AGV的歷史觀測數據、運動控制輸入和環境地圖，k時刻AGV位姿估計可表示為：

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

(1)

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

(2)

(3)

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

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

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

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

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

圖1 柵格信度計算的降維處理

2.2 基于空間域-頻域變換的預測模型

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

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

(4)

其中，f和F分別表示空間域與頻域圖像；M和N分別表示x與y方向的柵格劃分數目。

空間域與頻域中的平移關系可表示為：

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

(5)

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

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

圖2 基于傅里葉變換的預測模型

2.3 綜合算法實現流程

基于降維和傅里葉變換處理預測模型的AGV馬爾可夫全局定位綜合算法實現如下：

k=0 ；

End

While TRUE

k=k+1；

w=0；

End

End

End

3 仿真分析

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

圖3 相似復合環境下所設置的環境特征地圖

表1 相似復合環境中的AGV全局定位參數設置

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

圖4 相似復合環境下AGV直線行走仿真不同時刻信度分布圖像

圖5 AGV位置估計誤差

4 結語

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