胡章芳 曾林全 羅元 羅鑫 趙立明

摘 要：蒙特卡洛定位（MCL）算法存在計(jì)算量大、定位精度差的問題，由于二維碼具有攜帶信息的多樣性、二維碼識(shí)別的方便性與易用性的特點(diǎn)，提出一種融入二維碼信息的自適應(yīng)蒙特卡洛定位算法。首先，利用二維碼提供的絕對(duì)位置信息修正里程計(jì)模型的累計(jì)誤差后進(jìn)行采樣;然后，采用激光傳感器提供的觀測(cè)模型確定粒子的重要性權(quán)重;最后，因?yàn)橹夭蓸硬糠植捎霉潭颖炯瘯?huì)導(dǎo)致大計(jì)算量，所以利用Kullback-Leibler距離（KLD）進(jìn)行重采樣，根據(jù)粒子在狀態(tài)空間的分布情況自適應(yīng)調(diào)整下一次迭代所需粒子數(shù)，從而減小計(jì)算量?；谝苿?dòng)機(jī)器人進(jìn)行的實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法與傳統(tǒng)蒙特卡洛算法相比定位精度提高了15.09%，時(shí)間縮短了15.28%。

關(guān)鍵詞：蒙特卡洛定位;里程計(jì)運(yùn)動(dòng)模型;觀測(cè)模型;二維碼;Kullback-Leibler距離采樣

中圖分類號(hào)：TP242.6

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1001-9081（2019）04-0989-05

Abstract： Monte Carlo Localization （MCL） algorithm has many problems such as large computation and poor positioning accuracy. Because of the diversity of information carried by two-dimensional code and usability and convenience of two-dimensional code recognition， an adaptive MCL algorithm integrated with two-dimensional code information was proposed. Firstly， the cumulative error of odometer model was corrected by absolute position information provided by two-dimensional code and then sampling was performed. Sencondly， the measurement model provided by laser sensor was used to determine the importance weights of the particles. Finally， as fixed sample set used in the resampling part caused large computation， Kullback-Leibler Distance （KLD） was utilized in resampling to reduce the computation by adaptively adjusting the number of particles required for the next iteration according to the distribution of particles in state space. Experimental result on the mobile robot show that the proposed algorithm improves the localization accuracy by 15.09% and reduces the localization time by 15.28% compared to traditional Monte-Carlo algorithm.

Key words： Monte-Carlo Localization （MCL）; odometer motion model; measurement model; two-dimensional coding; Kullback-Leibler Distance （KLD） sampling

0?引言

在環(huán)境地圖已知的前提下，移動(dòng)機(jī)器人根據(jù)環(huán)境感知和自身運(yùn)動(dòng)確定其在環(huán)境中的位姿問題稱為定位問題。蒙特卡洛定位（Monte Carlo Localization， MCL）算法[1]以運(yùn)動(dòng)模型采樣，并結(jié)合觀測(cè)模型評(píng)估每個(gè)粒子的重要性權(quán)重，得到系統(tǒng)狀態(tài)的后驗(yàn)信度分布。

該算法成功應(yīng)用于移動(dòng)機(jī)器人領(lǐng)域，適用于局部定位和全局定位兩類問題。里程計(jì)運(yùn)動(dòng)模型通過整合車輪上的光電編碼器信息，進(jìn)而獲得機(jī)器人相對(duì)于上一采樣時(shí)刻位姿的相對(duì)差，在固定的時(shí)間間隔可進(jìn)行位姿估計(jì)。但是由于漂移或打滑等因素的影響，導(dǎo)致運(yùn)動(dòng)模型的精度隨時(shí)間的增大而下降，MCL算法的先驗(yàn)分布依賴于里程計(jì)運(yùn)動(dòng)模型的精度，若預(yù)測(cè)粒子分布與實(shí)際位姿相差太大易導(dǎo)致定位定位誤差增大;另外，重采樣后粒子會(huì)產(chǎn)生退化效應(yīng)，粒子多樣性降低，固定大樣本粒子會(huì)導(dǎo)致計(jì)算資源浪費(fèi)，因此學(xué)者們一直在研究如何解決這兩類問題。Borenstein等[2]在20世紀(jì)90年代就將里程計(jì)誤差分為系統(tǒng)誤差和非系統(tǒng)誤差兩個(gè)部分，并提出了一種里程計(jì)標(biāo)定方法UMBmark消除系統(tǒng)誤差對(duì)里程計(jì)精度影響，機(jī)器人按預(yù)定軌跡運(yùn)動(dòng)幾次便可標(biāo)定出差動(dòng)輪模型參數(shù)。Yap等[3]利用最大期望（Expectation Maximization， EM）算法并結(jié)合環(huán)境建圖來同時(shí)計(jì)算里程計(jì)運(yùn)動(dòng)模型和激光器觀測(cè)模型的參數(shù)，最后實(shí)現(xiàn)了在線自適應(yīng)標(biāo)定。Alhashimi等[4]改進(jìn)蒙特卡洛算法的觀測(cè)模型，通過設(shè)置的閾值來確定粒子樣本集合的大小，有效減小了計(jì)算量。黃露等[5]設(shè)計(jì)人工路標(biāo)并建立了路標(biāo)庫來修正里程計(jì)的累計(jì)誤差;

但是由于人工路標(biāo)設(shè)計(jì)復(fù)雜且路標(biāo)庫建立需要大量數(shù)據(jù)導(dǎo)致定位精度低、計(jì)算量大，而且準(zhǔn)確性也無法滿足要求。

因此，本文提出一種融入二維碼信息的自適應(yīng)蒙特卡洛定位算法（Adaptive MCL algorithm Integrated with two-dimensional code information， I-AMCL），在采樣過程中融入二維碼攜帶的絕對(duì)位置信息，激光傳感器信息用于更新粒子權(quán)重，并采用Kullback-Leibler距離（Kullback-Leibler Distance， KLD）[6]重采樣，根據(jù)采樣在狀態(tài)空間的近似分布的統(tǒng)計(jì)界限來確定粒子數(shù)。最后，通過實(shí)驗(yàn)驗(yàn)證在保證最優(yōu)位姿估計(jì)和穩(wěn)定性不受影響的情況下，本文算法減小計(jì)算量，并提高定位精度。

假設(shè)當(dāng)前的粒子群St只依賴于上一時(shí)刻的粒子群St-1，初始置信度bel（x0）由先驗(yàn)分布 p（x0）隨機(jī)產(chǎn)生的m個(gè)這樣的粒子得到，并為每個(gè)粒子分配相同的重要性因子m-1。該算法第3）行利用運(yùn)動(dòng)模型進(jìn)行采樣，以當(dāng)前置信度為起點(diǎn)分配粒子;第4）行觀測(cè)模型對(duì)每個(gè)粒子的權(quán)重進(jìn)行更新;第5）～11）行根據(jù)粒子權(quán)重重采樣得到新的粒子群，并不斷迭代。

2?本文算法

將視覺傳感器提供的二維碼的絕對(duì)位置信息融入到里程計(jì)運(yùn)動(dòng)模型中，可修正里程計(jì)的累計(jì)誤差;而每次迭代過程中的重采樣會(huì)帶來繁重的計(jì)算量，

因此本文提出一種融入二維碼信息的自適應(yīng)蒙特卡洛定位算法（I-AMCL），以提高定位精度，減少計(jì)算量。

2.1?運(yùn)動(dòng)模型采樣

2.1.1?里程計(jì)運(yùn)動(dòng)模型

里程計(jì)使用的機(jī)器人內(nèi)部坐標(biāo)和全局世界坐標(biāo)之間不存在固定的坐標(biāo)變換，因此里程計(jì)模型的建立需要機(jī)器人內(nèi)部編碼器觀測(cè)的相對(duì)運(yùn)動(dòng)信息。在時(shí)間間隔（t-1，t]內(nèi)，給定運(yùn)動(dòng)信息ut為：

這就是里程計(jì)誤差，在機(jī)器人定位過程中容易導(dǎo)致定位誤差。

2.1.2?二維碼信息

由于直接將視覺傳感器識(shí)別的離散坐標(biāo)融入里程計(jì)模型誤差較大，所以應(yīng)提供機(jī)器人位置的直接觀測(cè)信息，因此需要計(jì)算出二維碼中心點(diǎn)在車體坐標(biāo)系中的精確位置[7-9]。

為了計(jì)算三維空間中物體的幾何信息，需要對(duì)攝像機(jī)進(jìn)行標(biāo)定，由此得出攝像機(jī)的內(nèi)參數(shù)矩陣A和外參數(shù)矩陣R，確定攝像機(jī)相對(duì)于世界坐標(biāo)系的方位。

由于攝像頭焦距不為0，有矩陣A和R可逆?？赏ㄟ^下式計(jì)算拍攝照片中的像素點(diǎn)在車體坐標(biāo)系中的實(shí)際坐標(biāo)。

當(dāng)知道二維碼中心在圖像中的坐標(biāo)（uc，vc），就可以計(jì)算出其在車體坐標(biāo)系中的坐標(biāo)（Xc，Yc，Zc），因此本文使用一個(gè)簡(jiǎn)單易用的二維碼識(shí)別庫（Zbar庫），獲取DM碼三個(gè)位置探測(cè)圖形在圖像中的坐標(biāo)（u1，v1）、（u2，v2）、（u3，v3）。

二維碼中心位于對(duì)角探測(cè)圖形連線中心上。由于已知探測(cè)圖形的位置，可計(jì)算二維碼的中心（uc，vc）在圖形中的坐標(biāo)為：（uc，vc）=（u1+u32，v1+v32）（14）

將（uc，vc）代入就可以獲取二維碼在車體坐標(biāo)系中的坐標(biāo)xc=（Xc，Yc，Zc）。

2.1.3?融入二維碼信息的采樣

運(yùn)動(dòng)模型采樣將初始姿態(tài)xt-1、里程計(jì)讀數(shù)ut和相機(jī)讀數(shù)xc作為輸入，輸出一個(gè)隨機(jī)xt服從p（xtut，xt-1）的分布。機(jī)器人在運(yùn)動(dòng)過程中沒有掃描到二維碼時(shí)，首先由式（2）～（8）推測(cè)一個(gè)位姿xt，當(dāng)相機(jī)獲取二維碼信息，與此時(shí)里程計(jì)采樣輸出的位姿xt進(jìn)行比較，兩者誤差大于臨界值τ，則采樣算法輸出的值為二維碼坐標(biāo)的絕對(duì)值xc，并令當(dāng)前時(shí)刻的位姿xt為二維碼信息提供的位姿，繼續(xù)進(jìn)行采樣直到掃描下一個(gè)二維碼。

2.2?激光傳感器的似然域

其中：xt是機(jī)器人的位姿;zt是t時(shí)刻的觀測(cè);M是環(huán)境地圖。假設(shè)每個(gè)觀測(cè)束噪聲之間獨(dú)立。

機(jī)器人在定位時(shí)需將傳感器掃描的終點(diǎn)zt映射到地圖的全局坐標(biāo)空間。令xt=（x， y，θ）T表示機(jī)器人在t時(shí)刻的位姿。用（xk，sens， yk，sens）T表示安裝在機(jī)器人上的激光傳感器局部坐標(biāo)系統(tǒng)的位置，用θk，sens表示激光光束相對(duì)于機(jī)器人航向的角度。觀測(cè)zkt的終點(diǎn)經(jīng)過三角變換映射到全局坐標(biāo)系：

傳感器檢測(cè)到障礙物時(shí)這些坐標(biāo)才具有意義，因此最大距離讀數(shù)將被丟棄。

三個(gè)參數(shù)zhit、zrand和zmax進(jìn)行加權(quán)平均混合，并且zhit+zrand+zmax=1。

2.3?基于KLD的重采樣

用于表示置信度的采樣集的大小對(duì)粒子濾波器的效率影響程度很大。為了使移動(dòng)機(jī)器人能夠處理全局定位和位置跟蹤問題，必須選擇大樣本集合來避免采樣消耗引起的發(fā)散，若采用固定大小的樣本集合，會(huì)導(dǎo)致非常大的計(jì)算量。KLD采樣能隨時(shí)間自適應(yīng)改變粒子數(shù)。

KLD采樣對(duì)每次粒子濾波迭代都以概率1-δ確定樣本數(shù)，使得真實(shí)的后驗(yàn)分布與基于采樣的近似分布之間的誤差小于ε，由此確定重采樣樣本集合的大小[10-11]。當(dāng)粒子數(shù)m滿足一定值時(shí)，可以保證概率的真實(shí)值與估計(jì)值之間的K-L距離小于閾值ε，此時(shí)m的值為：

2.4?融入二維碼信息的自適應(yīng)MCL算法

與蒙特卡洛算法相比，本文算法I-AMCL除了將t-1時(shí)刻的采樣集合St-1、地圖M和最新的控制ut及觀測(cè)zkt作為輸入，KLD采樣還把一個(gè)加權(quán)后采樣集合和二維碼提供的全局坐標(biāo)信息xc與臨界值τ作為輸入，且要求輸入統(tǒng)計(jì)誤差界限ε和δ。

算法I-AMCL在滿足第27）行統(tǒng)計(jì)界限之前，KLD采樣將一直產(chǎn)生粒子。這個(gè)界限基于粒子所覆蓋的狀態(tài)空間的體積，粒子覆蓋的體積由覆蓋在三維狀態(tài)空間的位來衡量，直方圖H的位有兩種狀態(tài)：空或被一個(gè)粒子占據(jù)（第3）～5）行），這個(gè)粒子是從St-1樣本集合中提取出來的（第7）行）?；谶@個(gè)粒子，一個(gè)新粒子被預(yù)測(cè)、加權(quán)，并插入到新采樣集合，這里預(yù)測(cè)采用的是二維碼修正過的里程計(jì)模型（第8）～14）行），加權(quán)采用的激光傳感器的似然域（第15）～21）行）。h是至少填充了一個(gè)粒子的直方圖的位數(shù)，如果新產(chǎn)生的粒子落入直方圖的一個(gè)空位中，那么非空位數(shù)目h是增加的，并且該位被標(biāo)記為非空（第22～30）行）。本文算法I-AMCL如算法2所示。

3?實(shí)驗(yàn)與結(jié)果分析

3.1?仿真研究

首先通過仿真實(shí)驗(yàn)進(jìn)行研究。使用Matlab 2012a在計(jì)算機(jī)上進(jìn)行仿真，I-AMCL參數(shù)設(shè)置如下：c=0.7，ε=0.1，τ=0.01，δ=0.01，實(shí)驗(yàn)所用初始粒子數(shù)為1000。從圖1可看出，I-AMCL與真實(shí)狀態(tài)值更接近，有相近的收斂速度，由此說明本文算法估計(jì)精度更高。

3.2?系統(tǒng)實(shí)現(xiàn)

3.2.1?實(shí)驗(yàn)平臺(tái)

為驗(yàn)證本文算法I-AMCL的有效性，采用如圖2所示的室內(nèi)移動(dòng)機(jī)器人來進(jìn)行。

在倉庫物流場(chǎng)景中，機(jī)器人根據(jù)訂單任務(wù)在粘貼有二維碼的位置進(jìn)行裝貨卸貨，該機(jī)器人采用光電編碼器進(jìn)行相對(duì)位置觀測(cè)，并配有CPU為1.86GHz運(yùn)行內(nèi)存為2GB的筆記本電腦，機(jī)器人的運(yùn)動(dòng)速度為V=0.2m/s，采用灰度攝像頭，在1280×1024的分辨率下圖像傳輸速度達(dá)100fps。為保證旋轉(zhuǎn)過程中二維碼始終位于攝像頭視野之內(nèi)，并給攝像頭提供穩(wěn)定的照明，本文將攝像頭安裝在驅(qū)動(dòng)輪之間，使攝像頭的光心和機(jī)器人的旋轉(zhuǎn)中心重合。運(yùn)行時(shí)，安裝LED陣列在車輛底部，可以提供穩(wěn)定照明。

高速二維碼的識(shí)別對(duì)攝像頭的拍攝幀率、曝光時(shí)間等都有要求。由于攝像頭安裝在車底，距離地面的高度有限制，實(shí)測(cè)其視野的邊長(zhǎng)為0.15m。本文使用的二維碼為DM碼，用于存儲(chǔ)坐標(biāo)信息，坐標(biāo)信息計(jì)算方法為，利用精度為0.1cm的卷尺測(cè)量二維碼之間的距離，并通過幾何關(guān)系求出坐標(biāo)（X，Y），航向角θ值由機(jī)器人正方向和讀取二維碼的方向之間的夾角確定。然后利用二維碼的位置探測(cè)圖形來確認(rèn)機(jī)器人和二維碼間的相對(duì)坐標(biāo)。因此，可利用網(wǎng)頁工具或二維碼批量生成工具等，將實(shí)際的坐標(biāo)信息/坐標(biāo)編號(hào)存入到DM碼中，由此，當(dāng)機(jī)器人行駛到二維碼上方時(shí)，可以通過掃描二維碼來確認(rèn)自身的當(dāng)前坐標(biāo)。二維碼粘貼示例如圖3所示。

3.2.2?實(shí)驗(yàn)結(jié)果分析

利用機(jī)器人操作系統(tǒng)（Robot Operating System， ROS）中的gmapping節(jié)點(diǎn)構(gòu)建室內(nèi)環(huán)境地圖[12]，初始位置已知，MCL和I-AMCL的初始粒子數(shù)都設(shè)置為1000，其相關(guān)參數(shù)設(shè)置同步仿真實(shí)驗(yàn)。完成機(jī)器人在先驗(yàn)地圖中的精確定位。

機(jī)器人從A點(diǎn)到B點(diǎn)的規(guī)劃路徑如圖4（a）所示（其中①～④表示二維碼粘貼位置）。圖4（b）～（e）表示機(jī)器人在運(yùn)動(dòng)過程中通過二維碼粘貼位置時(shí)的粒子變化情況，中心大圓點(diǎn)表示機(jī)器人，周圍的小點(diǎn)表示粒子，箭頭表示機(jī)器人移動(dòng)方向。從圖4可看出移動(dòng)機(jī)器人在利用I-AMCL進(jìn)行定位時(shí)，在初始位置，粒子數(shù)較多并分布在機(jī)器人周圍，隨著機(jī)器人運(yùn)動(dòng)，觀測(cè)值和控制值在不斷矯正機(jī)器人的位姿，其定位所需粒子數(shù)在不斷減少，且機(jī)器人在二維碼粘貼位置的定位精度較高。

圖5為兩種算法的粒子數(shù)隨機(jī)器人運(yùn)動(dòng)距離的變化情況，進(jìn)一步說明MCL采用固定粒子數(shù)，而I-AMCL在機(jī)器人運(yùn)動(dòng)過程中可根據(jù)自由空間狀態(tài)來實(shí)時(shí)調(diào)整定位所需粒子數(shù)，結(jié)合二維碼位姿信息和激光傳感器的觀測(cè)信息，移動(dòng)機(jī)器人定位所需粒子數(shù)為110左右。

如圖6所示的室內(nèi)環(huán)境為重慶郵電大學(xué)自動(dòng)化學(xué)院一樓。為模擬倉庫物流環(huán)境，將二維碼粘貼在①～⑥位置，如圖6（a）所示。實(shí)驗(yàn)過程中6個(gè)二維碼均能被快速識(shí)別，分別記錄機(jī)器人采用兩種算法的運(yùn)動(dòng)軌跡的實(shí)測(cè)值。定位數(shù)據(jù)分析對(duì)比如圖6所示：圖6（b）為MCL算法定位路徑，與規(guī)劃路徑相比誤差明顯;圖6（c）為I-AMCL定位路徑，可以明顯看出定位精度得到了明顯的提高，使得定位路徑明顯趨近于規(guī)劃路徑，整個(gè)導(dǎo)航系統(tǒng)的誤差大幅度降低。

表1對(duì)比了兩種定位算法的性能，可以說明本文算法I-AMCL有更精確的定位，可根據(jù)機(jī)器人的狀態(tài)實(shí)時(shí)調(diào)整采樣粒子數(shù)達(dá)到提高計(jì)算效率的目的，實(shí)現(xiàn)機(jī)器人室內(nèi)導(dǎo)航。

4?結(jié)語

本文提出了一種融入二維碼信息的自適應(yīng)蒙特卡洛定位（I-AMCL）算法，利用攝像機(jī)提供的二維碼信息修正里程計(jì)模型后再采樣，激光傳感器提供的觀測(cè)模型用以確定粒子的重要性權(quán)重;并采用KLD重采樣調(diào)整所需粒子數(shù)，減小了計(jì)算量同時(shí)提高了定位精度;最后通過仿真和實(shí)驗(yàn)驗(yàn)證了本文算法的可行性。但是，本文人為地改變了部分環(huán)境地圖信息，導(dǎo)致機(jī)器人的起始點(diǎn)必須在二維碼正上方，在一定程度上缺乏靈活性。

參考文獻(xiàn)（References）

[1] THRUN S， FOX D， BURGARD W， et al. Robust Monte Carlo localization for mobile robots[J]. Artificial Intelligence， 2001， 128（1/2）： 99-141.

[2] BORENSTEIN J， FENG L. Measurement and correction of systematic odometry errors in mobile robots[J]. IEEE Transactions on Robotics and Automation， 1997， 12（6）： 869-880.

[3] YAP T N， SHELTON C R. Simultaneous learning of motion and sensor model parameters for mobile robots[C]// Proceedings of the 2008 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2008： 2091-2097.

[4] ALHASHIMIl A W， HOSTETTLER R， GUSTAFSSON T. An improvement in the observation model for Monte Carlo localization[C]// Proceedings of the 2014 11th International Conference on Informatics in Control， Automation and Robotics. Piscataway， NJ： IEEE， 2014： 498-505.

[5] 黃露， 朱明. 單目視覺人工路標(biāo)輔助的移動(dòng)機(jī)器人導(dǎo)航方法[J]. 計(jì)算機(jī)系統(tǒng)應(yīng)用， 2018， 27（1）： 106-112. （HUANG L， ZHU M. Mobile robot navigation algorithm assisted by monocular vision artificial road sign[J]. Computer Systems & Applications， 2018， 27（1）： 106-112.）

[6] ZHOU T， PENG D， XU C， et al. An adaptive particle filter based on Kullback-Leibler distance for underwater terrain aided navigation with multi-beam sonar[J]. IET Radar， Sonar & Navigation， 2018， 12（4）： 433-441.

[7] 尚明超， 楊斌， 張翠芳. 二維碼修正EKF-SLAM定位的室內(nèi)無人駕駛小車[J]. 單片機(jī)與嵌入式系統(tǒng)應(yīng)用， 2017， 17（7）： 63-66. （SHANG M C， YANG B， ZHANG C F. Indoor driverless car with EKF-SLAM localization modified by QR code[J]. Microcontroller and Embedded System Applications， 2017， 17（7）： 63-66.）

[8] LI Z Q， HUANG J D. Study on the use of QR codes as landmarks for indoor positioning： preliminary results[C]// Proceedings of the 2018 IEEE/ION Position， Location and Navigation Symposium. Piscataway， NJ： IEEE， 2018： 1270-1276.

[9] TRUC N T， KIM Y T. Navigation method of the transportation robot using fuzzy line tracking and QR code recognition[J]. International Journal of humanoid Robotics， 2016， 14（2）： 456-460.

[10] WANG F， LIN Y. Improving particle filter with a new sampling strategy[C]// Proceedings of the 2009 4th International Conference on Computer Science and Convergence Information Technology. Piscataway， NJ： IEEE， 2009： 408-412.

[11] WANG F， ZHAO Q， ZHANG Y， et al. Particle filtering with multi proposal distributions[J]. International Journal of Communications Network & System Sciences， 2008， 1（1）： 22-28.

[12] 張建偉， 張立新， 胡穎， 等. 開源機(jī)器人操作系統(tǒng)——ROS[M]. 北京： 科學(xué)出版社， 2012： 1-6. （ZHANG J W， ZHANG L X， HU Y， et al. The Open Source Robot Operating System — ROS[M]. Beijing： Science Press， 2012： 1-6.）