特征地圖中基于高斯核函數(shù)的自動(dòng)導(dǎo)引車Markov定位算法

李 昊，葉文華，滿增光

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

0 引言

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

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

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

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

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

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

(1)

(2)

(3)

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

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

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

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

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

(4)

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

2.2 觀測似然計(jì)算

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

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

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

k=0；

End

While TRUE

k=k+1；

w=0；

End

End

End

3 仿真分析

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

3.1 相似環(huán)境下AGV運(yùn)動(dòng)定位的仿真

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

5 結(jié)束語

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

參考文獻(xiàn):

[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].計(jì)算機(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).[劉 瑜,朱世強(qiáng),金 波.移動(dòng)機(jī)器人Markov定位算法的研究——方向傳感器建模新方法[J].浙江大學(xué)學(xué)報(bào):工學(xué)版,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).[高 巍,邵曉東,劉煥玲.基于粒子濾波的自動(dòng)裝配定位方法[J].計(jì)算機(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].計(jì)算機(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.可移動(dòng)機(jī)器人的馬爾可夫自定位算法研究[J].自動(dòng)化學(xué)報(bào),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)境下移動(dòng)機(jī)器人特征地圖創(chuàng)建研究[J].計(jì)算機(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).[滿增光.基于激光雷達(dá)的室內(nèi)AGV地圖創(chuàng)建與定位方法研究[D].南京：南京航空航天大學(xué),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).[陳冠中,陳慶新,毛 寧,等.具有隨機(jī)路徑AGV的制造系統(tǒng)排隊(duì)網(wǎng)建模與分析[J].計(jì)算機(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].微型機(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)各向異性高斯核的散亂點(diǎn)云保特征去噪[J].計(jì)算機(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)洪,杜彥斌,等.基于改進(jìn)ICP算法的損傷零部件精確配準(zhǔn)方法[J].計(jì)算機(jī)集成制造系統(tǒng),2016,22(4):1021-1028.]