李 昊，葉文華，滿增光

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

0 引言

自動導引車(Automated Guided Vehicle, AGV)正越來越多地應(yīng)用于智能物流與智能制造系統(tǒng)中，定位問題是AGV導航運動中的基本問題[1]。Markov定位算法即是一種常用于初始位姿未知情況下的AGV全局定位方法[2]，該方法基于概率狀態(tài)分布，將對AGV位姿的估計看作多階Markov過程，離散化的空間位姿即代表其中的狀態(tài)變量?；诟怕实亩ㄎ环椒苡行枋龆ㄎ贿^程中的不確定性，其定位魯棒性較好[3]，例如Markov定位方法不僅可以解決AGV被移動到新位置而產(chǎn)生的所謂“拐騙”問題[4-5]，還可以處理多模及非高斯分布的概率模型。在實際應(yīng)用中，Markov定位方法可以基于任意一種形式的地圖實現(xiàn)對AGV的全局定位[6]。對Markov定位的研究最早出現(xiàn)在基于拓撲地圖的定位中[7]，其后被廣泛應(yīng)用于基于柵格地圖的定位[8-9]。拓撲地圖環(huán)境表示緊湊，便于實現(xiàn)快速搜索和路徑規(guī)劃，但無法表達拓撲節(jié)點以外的環(huán)境信息；柵格地圖直觀、易維護，但應(yīng)用Markov定位時，其計算量與空間大小和分辨率有關(guān)，在空間范圍大時其計算量通常較大，限制了地圖精度的提高。

特征地圖也是AGV常用的一種地圖[10]，它以環(huán)境典型特征(如點、線等)作為地圖表達形式，具有創(chuàng)建形式多樣、空間信息表示高層次且簡潔、對空間大小不敏感的特點。由于特征(路標)分布常具有稀疏性，在使用Markov方法對AGV定位時需要將觀測數(shù)據(jù)與地圖特征數(shù)據(jù)進行關(guān)聯(lián)來計算觀測模型[11]，而實際上，AGV自身攜帶的傳感器探測環(huán)境時所得到的某一觀測數(shù)據(jù)與地圖特征的匹配常常不唯一，導致錯誤的數(shù)據(jù)關(guān)聯(lián)[12]，使全局定位失敗，因此Markov定位方法在特征地圖中的研究應(yīng)用較少，而針對特征地圖目前也沒有成熟有效的基于概率的全局定位方法[13]。

為提高Markov定位方法在基于特征地圖時對AGV進行定位的有效性和實用性，使其能運用在實際運輸作業(yè)中，本文提出一種基于高斯核函數(shù)擬合致密曲線的觀測模型計算方法。因為特征在AGV極坐標系下的分布具有稀疏性，若直接運算，得到的則為二進制的離散值，限制了貝葉斯估計的作用，所以需要進行數(shù)據(jù)關(guān)聯(lián)[14]；受數(shù)據(jù)關(guān)聯(lián)方法使用的必要性啟發(fā)，本文通過高斯核函數(shù)擬合AGV探測到的稀疏分散特征，得到包含這些特征的致密平滑曲線，使特征在AGV極坐標系下的分布由稀疏補為致密。高斯核函數(shù)是單值函數(shù)，具有旋轉(zhuǎn)對稱性、濾波器平滑程度可調(diào)等優(yōu)點[15]，通過高斯核函數(shù)擬合特征點后觀測模型的似然計算結(jié)果為連續(xù)值，對觀測模型的計算即轉(zhuǎn)變?yōu)閷GV實際觀測與算法預測觀測所擬合出的兩條曲線相似度的計算，避免了特征地圖中AGV觀測與地圖路標間的數(shù)據(jù)關(guān)聯(lián)問題。將該計算方法用于Markov定位算法，可以在特征地圖中實現(xiàn)對AGV的位姿估計與全局定位。

1 特征地圖中的Markov定位算法

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

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

(1)

(2)

(3)

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

2 基于高斯核函數(shù)平滑的觀測模型及算法

2.1 高斯核函數(shù)平滑方法

高斯核函數(shù)平滑原理如圖1所示。以AGV在環(huán)境中觀測到的3個特征為例，設(shè)為l1，l2和l3，分別對應(yīng)極坐標系下的位置(ρ1,θ1)，(ρ2,θ2)，(ρ3,θ3)。若θ為自變量，ρ為θ的函數(shù)，則該函數(shù)為一個非連續(xù)、非平滑的函數(shù)。

高斯核函數(shù)平滑方法即利用高斯核函數(shù)使ρ與θ間的關(guān)系呈現(xiàn)光滑連續(xù)性，可表示為

(4)

式中：K即高斯核函數(shù)；c為觀測到的特征索引集合；σi為核帶寬，σi=λ/ρi；λ為擬合平滑函數(shù)參數(shù)，通常大于0，其對擬合曲線的影響如圖2所示，λ越大，曲線越平滑。

2.2 觀測似然計算

通過上述高斯核函數(shù)平滑方法處理后，由傳感器觀測與算法預測的觀測得到兩條平滑函數(shù)曲線，再通過兩者的離散化逐值比較來計算其相似度。轉(zhuǎn)換的觀測似然計算公式為

2.3 算法實現(xiàn)流程

基于高斯核函數(shù)的AGV Markov全局定位算法如下：

k=0；

End

While TRUE

k=k+1；

w=0；

End

End

End

3 仿真分析

為驗證本文所提基于高斯核函數(shù)的觀測似然計算構(gòu)成的Markov定位方法的有效性，在MATLAB平臺下進行了仿真分析，測距傳感器為激光雷達傳感器。按相似環(huán)境和非相似環(huán)境兩種情況進行仿真，在相似環(huán)境下通過AGV直線行走的仿真來驗證所提方法對AGV位姿估計的有效性；在非相似環(huán)境下通過AGV靜止的仿真來驗證所提方法對AGV位姿估計不但有效，而且高效。

3.1 相似環(huán)境下AGV運動定位的仿真

該仿真環(huán)境如圖3所示，長寬均為20 m，設(shè)置4個特征點，特征點以5 m等間距分列在y=-2的直線上，設(shè)AGV初始時刻位于坐標原點，朝向與運動方向均平行于x軸，運動的線速度為1 m/s。傳感器及算法相關(guān)參數(shù)如表1所示。

表1 傳感器及算法參數(shù)設(shè)置

參數(shù)設(shè)定值激光測距傳感器最遠探測距離/m11激光測距傳感器最大探測角度/(°)180離散化柵格大小/m20.2×0.2高斯核函數(shù)參數(shù)100

仿真所得信度分布結(jié)果如圖4所示。從圖4a可知，在初始k=0時刻，AGV的實際位置在(0,0)，計算各柵格被占有概率，得到(0,0)，(5,0)，(10,0)3個較大和(15,0)1個次大4個信度峰值坐標點，即AGV初始位置估計在這3個坐標附近。圖4b～圖4d分別為k=5，10，15時AGV在概率柵格中的位姿信度極值分布和變化情況。當k=15時，AGV的實際位姿為(15,0,0)，而信度柵格圖像中只有(15,0)坐標存在信度極值，其他位置的概率信度均為0，即此刻已經(jīng)正確估計出了AGV的位姿。從上述位姿信度的變化可知，隨著AGV不斷運動，感知到的特征信息不斷增加，AGV從算法最初估計的多個坐標位置逐漸確定為唯一的真實坐標。以上結(jié)果表明AGV在直線行進時，本文所提的基于高斯核函數(shù)的Markov全局定位方法在特征地圖中是有效的。

3.2 非相似環(huán)境下AGV靜止定位的仿真

該仿真環(huán)境(如圖5)的長寬均為10 m，設(shè)置40個隨機分布的特征點。以a(2,2)，b(2,-2)，c(-2,2)，d(-2,-2)4個柵格整點作為AGV的初始放置坐標點，朝向如圖5所示。傳感器及算法相關(guān)參數(shù)如表2所示。

表2 傳感器及算法參數(shù)設(shè)置

參數(shù)設(shè)定值激光測距傳感器最遠探測距離/m11激光測距傳感器最大探測角度/(°)180離散化柵格大小/m20.1×0.1高斯核函數(shù)參數(shù)100

仿真所得信度分布結(jié)果如圖6所示。圖6a可知，AGV靜止處于a位置時，位置信度圖像中僅(2,2)坐標位置存在信度極值，即AGV只經(jīng)過一次定位計算就將位置估計確定到真實坐標位置。圖6b～圖6d所示為AGV處于其他不同位置b，c，d時的仿真結(jié)果，與a位置結(jié)果相同，該仿真只經(jīng)過一次定位計算即使AGV位置估計分別確定到真實坐標位置(2,-2)，(-2,2)和(-2,-2)。這說明在非相似環(huán)境中，將經(jīng)過高斯核函數(shù)處理的Markov定位方法運用在特征地圖中具有有效性和高效性。

4 實驗驗證

實驗所用的AGV平臺(如圖7)長100 cm、寬60 cm、高40 cm。平臺配備的傳感器包括兩個記錄AGV運動信息的編碼器和西克公司的LMS291-S05激光雷達測距傳感器。真實環(huán)境如圖8所示。為驗證本文算法的有效性，首先通過對某一時刻的全局定位實驗來驗證算法對AGV位姿估計的準確性；然后采用ICP(iterated closest points)方法[16]估計的AGV位姿作為真實值，通過比較本文改進的Markov定位方法與常規(guī)Markov定位方法所得到的AGV運動軌跡來說明本文方法的優(yōu)越性。實驗中的特征路標采用從傳感器數(shù)據(jù)中提取的環(huán)境角點與斷點表示。

實驗中AGV的行走路程為14.3 m，速度設(shè)置為0.1 m/s，運動時長280 s，使用AGV運動的前10 s傳感器數(shù)據(jù)作為全局定位實驗，離散化柵格尺寸為0.1 m×0.1 m。運動中AGV在環(huán)境中的位姿從(0,0,0)移至(0.604 7,0.020 4,-0.005 9)，算法運算時間為1.1 s。

第10 s的AGV位置信度分布如圖9所示，可見對AGV位姿的估計確定到了唯一一個信度極值點。10 s時間內(nèi)的位置估計誤差如圖10所示，AGV在x方向上的位置估計誤差不超過一個柵格邊長0.1 m，在y方向上的位置估計誤差接近于0且波動較小，證明本文方法在實際應(yīng)用中是有效且準確的。

采用本文所提基于高斯核函數(shù)的Markov定位方法和常規(guī)使用的數(shù)據(jù)關(guān)聯(lián)的Markov方法對AGV進行位姿跟蹤，兩者估計出的AGV運動軌跡如圖11所示。從全局路徑可以看出，本文所提定位方法對AGV位姿和軌跡的估計更接近真實軌跡，且大部分位置軌跡幾乎重合，偏差很??；而常規(guī)方法估計出的AGV運動軌跡相比真實軌跡有一定偏移且軌跡偏移量不均勻。兩種方法對AGV位姿估計的誤差如圖12所示，具體對比各時刻估計出的AGV位姿誤差，顯然本文方法對AGV在各方向上估計的位姿誤差較小，相對真實位姿誤差波動平緩；常規(guī)方法得到的位姿估計誤差及誤差波動相對更大。由此可見，本文方法對AGV在各方向上估計的位姿誤差明顯小于常規(guī)Markov方法，因此定位精度更高。

5 結(jié)束語

特征地圖是AGV導航時常用的一種地圖，其特征分布具有稀疏性，使用Markov方法定位時通常需要進行觀測數(shù)據(jù)與地圖間特征數(shù)據(jù)的關(guān)聯(lián)，但常因觀測與地圖間匹配不唯一而導致錯誤的數(shù)據(jù)關(guān)聯(lián)。本文提出的基于高斯核函數(shù)平滑和加密稀疏特征建立Markov全局定位觀測模型及其計算方法，從本質(zhì)上避免了在觀測模型計算時對AGV觀測與地圖路標間進行的數(shù)據(jù)關(guān)聯(lián)，提高了定位效率，且加入觀測模型新計算方法的Markov定位算法能有效運用于特征地圖中對AGV的全局定位，不僅提升了Markov定位在特征地圖中的實用性，還對求解其他類型移動機器人在特征地圖中的初始位姿有參考意義。同時，如何兼顧Markov定位方法的實時性與精度，使其有效應(yīng)用于大規(guī)模環(huán)境，是下一步研究的內(nèi)容。

參考文獻:

[1] CUI Kangji, SUN Yu, ZHANG Xinning, et al. The research of accurate orientation technology for AGV[J]. Computer Integrated Manufacturing Systems,1995,1(4):39-41(in Chinese).[崔康吉,孫 宇,張新寧,等.AGV精確定位技術(shù)的研究[J].計算機集成制造系統(tǒng)，1995,1(4):39-41.]

[2] LIU Yu, ZHU Shiqiang, JIN Bo. Study on Markov localization algorithm for mobile robots:new modeling method for orientation sensor[J]. Journal of Zhejiang University:Engineering Science,2005,39(3):339-341(in Chinese).[劉 瑜,朱世強,金 波.移動機器人Markov定位算法的研究——方向傳感器建模新方法[J].浙江大學學報:工學版,2005,39(3):339-341.]

[3] GAO Wei, SHAO Xiaodong, LIU Huanling. Automatic assembly location metion based on particle filter[J]. Computer Integrated Manufacturing Systems,2014,20(7):1615-1624(in Chinese).[高 巍,邵曉東,劉煥玲.基于粒子濾波的自動裝配定位方法[J].計算機集成制造系統(tǒng),2014,20(7):1615-1624.]

[4] ZHANG L, ZAPATA R, LEPINAY P. Self-adaptive Monte Carlo localization for mobile robots using range finders[J]. Robotica,2012,30(2):229-244.

[5] PENG Yanbin, AI Jieqing. Markov chain Monte Carlo method based bilateral multi-issue negotiation model[J]. Computer Integrated Manufacturing Systems,2011,17(9):2044-2050(in Chinese).[彭艷斌,艾解清.基于馬爾可夫鏈蒙特卡羅方法的雙邊多議題協(xié)商模型[J].計算機集成制造系統(tǒng),2011,17(9):2044-2050.]

[6] WU Qingxiang, Bell D. A study on Markov localization for mobile robots[J]. Acta Automatica Sinica,2003,29(1):154-160(in Chinese).[吳慶祥,D. Bell.可移動機器人的馬爾可夫自定位算法研究[J].自動化學報,2003,29(1):154-160.]

[7] NOURBAKHSH I, POWER R, BIRCHFIELD S. DERVISH an office-navigating robot[J]. AI Magazine,1995,16(2):73-90.

[8] COLLINS T, COLLINS J J, MANSFIELD M, et al. Evaluating techniques for resolving redundant information and specularity in occupancy grids[C]∥Procedings of Australasian Jaint Conference on Artificial Intelligence. Berlin, Germany:Springer-Verlag,2005:235-244.

[9] BURGARD W, DERR A, FOX D, et al. Integrating global position estimation and position tracking for mobile robots:the dynamic Markov localization approach[C]∥Proceedings of the 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems.Washington,D.C., USA:IEEE,1998:730-735.

[10] ZHU Jianguo, GAO Junyao, LI Kejie, et al. Research on feature map building of mobile robot in indoor unknown environment[J]. Computer Measurement & Control,2011,19(12)：3044-3046(in Chinese).[朱建國,高峻峣,李科杰,等.室內(nèi)未知環(huán)境下移動機器人特征地圖創(chuàng)建研究[J].計算機測量與控制,2011,19(12)：3044-3046.]

[11] MAN Zengguang. Research on mapping and localization of indoor AGV based on ladar[D]. Nanjing:Nanjing University of Aeronautics and Astronautics,2014(in Chinese).[滿增光.基于激光雷達的室內(nèi)AGV地圖創(chuàng)建與定位方法研究[D].南京：南京航空航天大學,2014.]

[12] NEIRA J, TARDOS J D. Data association in stochastic mapping using the joint compatibility test[J]. IEEE Transactions on Robotics and Automation,2001,17(6):890-897.

[13] CHEN Guangzhong, CHEN Qingxin, MAO Ning, et al. Modeling and analysis of queuing network in manufacturing system with stochastic path AGV[J]. Computer Integrated Manufacturing Systems,2017,23(1):52-65(in Chinese).[陳冠中,陳慶新,毛 寧,等.具有隨機路徑AGV的制造系統(tǒng)排隊網(wǎng)建模與分析[J].計算機集成制造系統(tǒng),2017,23(1):52-65.]

[14] LI Yanju, XIAO Yufeng, GU Song, et al. Research on data association in SLAM based laser sensor[J]. Microcomputer & Its Applications,2017,36(2):78-82(in Chinese).[李延炬,肖宇峰,古 松,等.基于激光傳感器的SLAM數(shù)據(jù)關(guān)聯(lián)算法的研究[J].微型機與應(yīng)用,2017,36(2):78-82.]

[15] LIN Hongbin, FU Demin, WANG Yinteng. Feature preserving denoising of scattered point cloud based on parametric adaptive and anisotropic Gaussian kernel[J]. Computer Integrated Manufacturing Systems, 2017,23(12):2583-2592(in Chinese).[林洪彬,付德敏,王銀騰.基于參數(shù)自適應(yīng)各向異性高斯核的散亂點云保特征去噪[J].計算機集成制造系統(tǒng), 2017,23(12):2583-2592.]

[16] LI Congbo, XIAO Weihong, DU Yanbin, et al. Fine registration method for defective parts based on improved ICP algorithm[J]. Computer Integrated Manufacturing Systems,2016,22(4):1021-1028(in Chinese).[李聰波,肖衛(wèi)洪,杜彥斌,等.基于改進ICP算法的損傷零部件精確配準方法[J].計算機集成制造系統(tǒng),2016,22(4):1021-1028.]