黃 鶴， 肖宇峰， 劉 冉， 張 華

(西南科技大學(xué) 信息工程學(xué)院 特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室，四川 綿陽(yáng) 621010)

0 引 言

自主移動(dòng)機(jī)器人技術(shù)主要包括定位、地圖構(gòu)建與路徑規(guī)劃。定位是機(jī)器人自主移動(dòng)的首要問(wèn)題，也是實(shí)現(xiàn)地圖構(gòu)建和路徑規(guī)劃的關(guān)鍵。貝葉斯濾波器是概率定位方法的理論基礎(chǔ)，其核心思想是利用機(jī)器人收集的歷史數(shù)據(jù)來(lái)估計(jì)狀態(tài)空間的后驗(yàn)概率密度[1]?；诟怕识ㄎ凰惴ㄖ饕锌柭鼮V波、馬爾可夫定位、多假設(shè)跟蹤和粒子濾波。其中粒子濾波是使用最廣泛的定位算法，具有實(shí)現(xiàn)簡(jiǎn)單、運(yùn)算量低、定位精度高等優(yōu)點(diǎn)。粒子濾波定位算法核心是通過(guò)若干個(gè)加權(quán)隨機(jī)粒子來(lái)表達(dá)后驗(yàn)概率密度，適用于非線性非高斯系統(tǒng)[1]。隨著計(jì)算機(jī)視覺(jué)的發(fā)展，基于圖像局部特征點(diǎn)檢測(cè)定位算法逐漸達(dá)到實(shí)時(shí)。ORB是在FAST(features from accelerated segment test)[2]特征基礎(chǔ)上，由Rublee E等人在2011年提出的一種快速特征點(diǎn)提取和描述的算法，具有旋轉(zhuǎn)不變性、尺度不變性和較高實(shí)時(shí)性等特點(diǎn)[3]。文獻(xiàn)[4]提出基于SURF(speeded up robust features)的視覺(jué)定位算法，通過(guò)單應(yīng)矩陣和投影約束信息來(lái)降低時(shí)間復(fù)雜度。機(jī)器人綁架問(wèn)題是指機(jī)器人經(jīng)過(guò)一個(gè)未知運(yùn)動(dòng)移動(dòng)到其它位置后的重定位問(wèn)題[5]。文獻(xiàn)[6]中基于機(jī)器人在兩個(gè)連續(xù)時(shí)間中所看到的環(huán)境的相似性來(lái)檢測(cè)機(jī)器人是否被綁架。文獻(xiàn)[7]中提出基于傳感器讀出兩個(gè)連續(xù)時(shí)間幀機(jī)器人的位移是否正常來(lái)檢測(cè)機(jī)器人是否被綁架。

傳統(tǒng)粒子濾波算法不能有效解決綁架問(wèn)題?；诹Ｗ訛V波的自適應(yīng)蒙特—卡羅定位算法在全局隨機(jī)撒粒子，通過(guò)激光雷達(dá)掃描周圍環(huán)境來(lái)匹配先驗(yàn)地圖使粒子收斂來(lái)實(shí)現(xiàn)重定位。但當(dāng)場(chǎng)景較大時(shí)面臨粒子收斂速度緩慢，重定位效率不高的問(wèn)題。另一種方法是通過(guò)增加其它傳感器來(lái)對(duì)機(jī)器人進(jìn)行定位。文獻(xiàn)[8]提出基于反向傳播(back propagation,BP)神經(jīng)網(wǎng)絡(luò)的WiFi指紋定位與粒子濾波復(fù)合的全局定位方法。

本文在粒子濾波定位算法基礎(chǔ)上，通過(guò)加入環(huán)境中的相機(jī)觀測(cè)結(jié)果來(lái)校正綁架后由運(yùn)動(dòng)預(yù)測(cè)產(chǎn)生誤差較大的粒子。相機(jī)基于ORB特征匹配檢測(cè)到機(jī)器人后，根據(jù)相機(jī)與地圖的關(guān)聯(lián)關(guān)系來(lái)確定機(jī)器人所在的子地圖。在子地圖區(qū)域中撒粒子通過(guò)粒子濾波器收斂后實(shí)現(xiàn)重定位。在Gazebo仿真平臺(tái)上的實(shí)驗(yàn)表明，本文算法能夠較好地解決機(jī)器人綁架問(wèn)題，且比AMCL粒子收斂速度更快、重定位誤差更低。

1 平臺(tái)構(gòu)架

本文方法基于安裝視頻監(jiān)控系統(tǒng)的公共服務(wù)環(huán)境，比如車站、大型超市等場(chǎng)所。方法運(yùn)行平臺(tái)除了機(jī)器人外還包括監(jiān)控相機(jī)、圖形工作站、無(wú)線網(wǎng)絡(luò),如圖1所示。

圖1 粒子濾波與ORB特征檢測(cè)結(jié)合的移動(dòng)機(jī)器人定位方法平臺(tái)構(gòu)架

相機(jī)的柵格地圖模型如圖2所示。

圖2 相機(jī)—柵格地圖模型

全局柵格地圖表示為M,第i個(gè)相機(jī)視域?qū)?yīng)的地圖表示為Mi，則有如下公式

M=M1∪M2∪M3∪…ML

(1)

所有相機(jī)的柵格地圖加起來(lái)構(gòu)成全局柵格地圖。對(duì)于機(jī)器人來(lái)說(shuō)，傳感器越多則感知周圍環(huán)境的能力越強(qiáng)。但是傳感器所帶來(lái)的花銷與機(jī)器人占有的有限資源相矛盾。每增加一個(gè)傳感器對(duì)于電池的續(xù)航能力以及數(shù)據(jù)處理的實(shí)時(shí)性來(lái)說(shuō)都是巨大的考驗(yàn)。本文所提出的構(gòu)架很好地解決了這一矛盾，引入相機(jī)傳感器但是把計(jì)算開(kāi)銷放置在機(jī)器人外圍，把圖像處理后的結(jié)果通過(guò)無(wú)線網(wǎng)絡(luò)發(fā)送至機(jī)器人端。通過(guò)相機(jī)柵格地圖作為橋梁將圖像處理的結(jié)果與粒子濾波算法結(jié)合起來(lái)。因?yàn)閳D像檢測(cè)與粒子濾波是并行進(jìn)行的，所以能提升了定位的效率。

2 基于ORB特征匹配的機(jī)器人檢測(cè)

通過(guò)環(huán)境中的相機(jī)觀測(cè)到機(jī)器人是重定位的第一步。相機(jī)圖像與機(jī)器人基于ORB特征進(jìn)行匹配，當(dāng)匹配點(diǎn)對(duì)超過(guò)設(shè)定的閾值認(rèn)為檢測(cè)到了機(jī)器人?？紤]到相機(jī)不會(huì)移動(dòng)，當(dāng)相鄰兩幀圖像一致時(shí)認(rèn)為機(jī)器人沒(méi)有被突然移動(dòng)到此區(qū)域。通過(guò)幀間差法對(duì)比前后兩幀圖像是否有變化，當(dāng)有變化時(shí)才進(jìn)行ORB特征提取與匹配。這種方式可以降低運(yùn)算量。

ORB特征提取是由FAST算法發(fā)展來(lái)的，特征點(diǎn)描述是根據(jù)BRIEF(binary robust independent elementary features)[10]特征描述算法改進(jìn)的。ORB特征是將FAST特征點(diǎn)的檢測(cè)方法與BRIEF特征描述子結(jié)合起來(lái)，并在原來(lái)的基礎(chǔ)上做了改進(jìn)與優(yōu)化。

BRIEF算法提取的描述子是一個(gè)二進(jìn)制串。對(duì)于特征點(diǎn)的鄰域空間p其二進(jìn)制測(cè)試τ定義如下

(2)

式中p(x)為x點(diǎn)的灰度值。BRIEF描述子可表示為如下的n維的二進(jìn)制串

(3)

式中x和y服從以特征點(diǎn)為中心的高斯分布，n選取256，在文獻(xiàn)[10]中被證明是最好的。但BRIEF描述子不具備旋轉(zhuǎn)不變性，ORB對(duì)其進(jìn)行了改進(jìn)，提出了Steered BRIEF。對(duì)特征點(diǎn)31×31鄰域內(nèi)選取對(duì)點(diǎn)集，定義一個(gè)2×n的矩陣S

(4)

利用角度θ和相應(yīng)的旋轉(zhuǎn)矩陣Rθ，對(duì)矩陣S構(gòu)造一個(gè)具有旋轉(zhuǎn)性的Sθ

(5)

Sθ=RθS

(6)

Steered BRIEF描述子可表示為

gn(p,θ)=fn(p)|(xi,yi)∈Sθ

(7)

Steered BRIEF方法得到的特征描述子具有旋轉(zhuǎn)不變性，同時(shí)也降低了特征點(diǎn)鄰域內(nèi)匹配點(diǎn)對(duì)的相關(guān)性。為了解決這一問(wèn)題，提出了rBRIEF[9]。通過(guò)貪婪搜索算法找到相關(guān)性最小的256種點(diǎn)對(duì)，得到rBRIEF描述子。

ORB特征描述子是以二進(jìn)制形式存儲(chǔ)，因而通過(guò)計(jì)算漢明距離(Hamming distance)來(lái)進(jìn)行匹配[10]。漢明距離越小則表明相似程度越高。這種匹配方式存在誤匹配，通過(guò)PROSAC (progressive sample consensus)算法可以剔除誤匹配點(diǎn)[11]。PROSAC算法首先生成一個(gè)具有較高匹配概率的子集，然后從子集中抽樣計(jì)算，這樣最優(yōu)參數(shù)采樣會(huì)較早出現(xiàn)[12]。

3 粒子濾波與ORB特征檢測(cè)結(jié)合的定位方法

在基于概率的機(jī)器人定位中的一個(gè)關(guān)鍵概念是置信度(belief)，它反映了機(jī)器人有關(guān)環(huán)境狀態(tài)的內(nèi)部信息。粒子濾波定位通過(guò)隨機(jī)化粒子(particle)來(lái)表征置信度bel(xt)。粒子濾波的核心思想是用一系列從后驗(yàn)概率得到的隨機(jī)狀態(tài)采樣表示bel(xt)。

(8)

式中z1∶t為1～t時(shí)刻的觀測(cè)量，u1∶t為1～t時(shí)刻的控制量，m為環(huán)境地圖?；诹Ｗ訛V波算法的定位是根據(jù)后驗(yàn)分布的N個(gè)有權(quán)值的樣本集來(lái)表示后驗(yàn)置信度bel(xt)。樣本密度越大表示樣本代表著機(jī)器人位置的概率越大

(9)

由于機(jī)器人被綁架的位姿是不確定的，即被綁架后的當(dāng)前位姿與上一時(shí)刻的位置不具相關(guān)性，因此,根據(jù)運(yùn)動(dòng)模型預(yù)測(cè)的粒子其權(quán)重會(huì)變得非常低。經(jīng)過(guò)濾波后得到的位姿與真實(shí)位姿相差較大。針對(duì)綁架問(wèn)題本文改進(jìn)了粒子濾波算法的運(yùn)動(dòng)預(yù)測(cè)階段，通過(guò)引入相機(jī)觀測(cè)的結(jié)果來(lái)修正基于運(yùn)動(dòng)模型所預(yù)測(cè)的粒子。機(jī)器人綁架后重定位流程如圖3所示。

圖3 機(jī)器人綁架后重定位流程

1)相機(jī)觀測(cè)：通過(guò)幀差法來(lái)確定存在運(yùn)動(dòng)目標(biāo)的相機(jī)群Ci(i=1,2,…,N)，基于ORB特征匹配確定相機(jī)ci檢測(cè)到機(jī)器人。相機(jī)視野覆蓋整個(gè)環(huán)境，若Ci中沒(méi)有匹配到機(jī)器人，則在余下的相機(jī)中進(jìn)行匹配。

2)初始化粒子：相機(jī)檢測(cè)到機(jī)器人后通過(guò)無(wú)線網(wǎng)絡(luò)將信息發(fā)送給機(jī)器人端。每一個(gè)相機(jī)ci關(guān)聯(lián)一個(gè)柵格地圖mi。在mi中初始化高斯分布的粒子集Xt。

3)粒子濾波：基于激光雷達(dá)掃描數(shù)據(jù)匹配地圖來(lái)計(jì)算Xt中粒子得分。選擇得分最高的粒子作為機(jī)器人位姿。通過(guò)重采樣舍棄評(píng)分低的粒子，對(duì)評(píng)分高的粒子復(fù)制，保持粒子群數(shù)量基本不變。

粒子濾波定位算法中基于運(yùn)動(dòng)模型的粒子無(wú)法準(zhǔn)確預(yù)測(cè)機(jī)器人綁架后的位置。本文通過(guò)引入視覺(jué)檢測(cè)方法來(lái)解決非正常運(yùn)動(dòng)情況下的預(yù)測(cè)問(wèn)題。相機(jī)基于ORB特征匹配檢測(cè)到機(jī)器人后，將其粗定位在一個(gè)區(qū)域。根據(jù)相機(jī)與地圖的關(guān)聯(lián)關(guān)系，在對(duì)應(yīng)的子地圖中撒粒子并進(jìn)行粒子濾波定位，最終實(shí)現(xiàn)機(jī)器人重定位。

4 仿 真

實(shí)驗(yàn)在機(jī)器人操作系統(tǒng)(robot operating system,ROS)和Gazebo平臺(tái)上進(jìn)行仿真。ROS版本為Indigo，Gazebo版本為7.0，電腦系統(tǒng)是Ubuntu14.04。ROS是一個(gè)開(kāi)源的機(jī)器人操作系統(tǒng)，遵循BSD開(kāi)源許可協(xié)議。Gazebo是一個(gè)三維的機(jī)器人仿真軟件，能夠模擬真實(shí)實(shí)驗(yàn)環(huán)境。移動(dòng)機(jī)器人模型如圖4所示，由Kobuki底盤和HOKUYO激光雷達(dá)組成。在Gazebo中環(huán)境模型如圖5(a)所示，長(zhǎng)17.6 m,寬16.6 m，14個(gè)相機(jī)的視域覆蓋整個(gè)環(huán)境。圖5(b)是基于ROS系統(tǒng)下的slam_gmapping建立的柵格地圖。相機(jī)對(duì)應(yīng)的柵格子地圖如圖6所示。

圖4 移動(dòng)機(jī)器人模型

圖5 Gazebo仿真環(huán)境建模

圖6 相機(jī)關(guān)聯(lián)的柵格子地圖

圖7(a)是基于漢明距離的匹配結(jié)果，圖中左邊為給定的機(jī)器人圖像，右邊是相機(jī)中機(jī)器人的圖像，連線表示描述子點(diǎn)對(duì)的匹配。圖中一些交叉的地方是誤匹配的結(jié)果。圖7(b)是PROSAC算法剔除誤匹配后的結(jié)果。當(dāng)機(jī)器人一部分出現(xiàn)在相機(jī)視域中，由圖7(c)可以看出仍然具有較好的匹配結(jié)果。圖7可以看出,ORB特征具有旋轉(zhuǎn)不變性和尺度不變性。

圖7 機(jī)器人的ORB特征匹配

機(jī)器人綁架實(shí)驗(yàn)結(jié)果如圖8所示。將其由初始位置綁架至左下方。

圖8 機(jī)器人綁架仿真實(shí)驗(yàn)對(duì)比

為了使粒子能夠覆蓋機(jī)器人真實(shí)位置AMCL需要在全局撒粒子如圖8(d)所示。而本文算法通過(guò)相機(jī)觀測(cè)到機(jī)器人后，在相機(jī)關(guān)聯(lián)地圖內(nèi)生成粒子如圖8(a)所示。撒粒子范圍由全局縮小至局部。每一個(gè)粒子存儲(chǔ)一個(gè)位姿信息，因而粒子數(shù)目越多需要的存儲(chǔ)空間越大。激光雷達(dá)掃描周圍環(huán)境并與地圖匹配，并對(duì)每一個(gè)粒子計(jì)算評(píng)分。粒子評(píng)分越高說(shuō)明越接近真實(shí)位姿。每一次迭代保留評(píng)分高的粒子并丟掉評(píng)分低的粒子。粒子數(shù)量逐漸減少并且粒子逐漸收斂如圖8(b)和(e)所示。粒子越多則計(jì)算開(kāi)銷就越大，收斂速度越慢。

重定位時(shí)間對(duì)比如圖9(a)所示。相比AMCL本文方法有效地縮短了收斂時(shí)間。在相似度高的環(huán)境下，可能會(huì)出現(xiàn)激光數(shù)據(jù)與地圖的誤匹配，這會(huì)導(dǎo)致靠近機(jī)器人真實(shí)位置的粒子得分反而較低。本文通過(guò)引入相機(jī)觀測(cè)的結(jié)果較大地縮減撒粒子區(qū)域，減小了由于激光誤匹配所帶來(lái)的影響，具有較低的定位誤差。圖9(b)是重定位后定位誤差對(duì)比，本文方法的定位誤差低于AMCL。

5 結(jié) 論

仿真結(jié)果表明：所提算法能夠解決機(jī)器人綁架問(wèn)題且在時(shí)間效率上高于AMCL算法，定位誤差小于AMCL。后續(xù)工作將在實(shí)際物理環(huán)境下開(kāi)展實(shí)驗(yàn)，對(duì)本文算法進(jìn)行驗(yàn)證；同時(shí)圍繞圖像方面進(jìn)行一些改進(jìn)，縮小相機(jī)觀測(cè)階段產(chǎn)生粒子的區(qū)域，從而減少粒子濾波迭代次數(shù)。