基于人工離線特征庫(kù)的室內(nèi)機(jī)器人雙目定位

張智， 張磊， 蘇麗， 郭文縣

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

2000年以來(lái)，基于視覺傳感器的SLAM (simultaneous localization and mapping)研究逐漸興起[1-2]，許多國(guó)內(nèi)外學(xué)者借助SIFT(scale-invariant feature transform)或其他圖像不變特征，開展了視覺SLAM問(wèn)題研究[3-4]。目前研究SLAM的方法主要有擴(kuò)展卡爾曼濾波[5]、信息濾波[6]、壓縮濾波[7]、FastSLAM[8]方法。其中EKF-SLAM[9]定位效果好，但隨路標(biāo)過(guò)多時(shí)計(jì)算量較大;基于信息濾波算法的即時(shí)定位與地圖構(gòu)建算法雖然穩(wěn)定準(zhǔn)確，但不能進(jìn)行數(shù)據(jù)關(guān)聯(lián)；FastSLAM算法只能在短時(shí)間內(nèi)滿足一致性要求，但不適用于解決大范圍的SLAM問(wèn)題。

EKF-SLAM方法是一種最優(yōu)循環(huán)迭代估計(jì)，在處理信息不確定性方面有獨(dú)到之處，但針對(duì)其計(jì)算量隨路標(biāo)個(gè)數(shù)增多而增長(zhǎng)較快的問(wèn)題，常需改進(jìn)處理，然而在室內(nèi)獨(dú)立小環(huán)境中，以子圖劃分等思想為基礎(chǔ)的快速方法難以適用，此外，EKF-SLAM還存在一個(gè)明顯的缺點(diǎn)便是數(shù)據(jù)關(guān)聯(lián)脆弱問(wèn)題，即一旦數(shù)據(jù)關(guān)聯(lián)出現(xiàn)錯(cuò)誤，會(huì)將誤差代入估計(jì)結(jié)果中，而且不可恢復(fù)，即通常說(shuō)的EKF(extended Kalman filter)缺乏自恢復(fù)能力。

本文以EKF-SLAM算法為基礎(chǔ)，并采用了人工離線建立特征庫(kù)的方法，克服傳統(tǒng)方法存在的計(jì)算量過(guò)大、關(guān)聯(lián)脆弱等問(wèn)題，實(shí)現(xiàn)了室內(nèi)復(fù)雜環(huán)境下實(shí)時(shí)、可靠地定位。此外，在人工建立離線特征庫(kù)時(shí)，解算過(guò)程考慮眾多特征的有效融合、特征密度控制、優(yōu)質(zhì)特征篩選等問(wèn)題，為此，本算法引入了特征稀疏性策略、穩(wěn)健特征點(diǎn)選取、誤匹配去除等方法。算法在定位過(guò)程中需固化特征庫(kù)，但系統(tǒng)實(shí)際工作環(huán)境難免會(huì)發(fā)生一定變化，為彌補(bǔ)算法適應(yīng)動(dòng)態(tài)環(huán)境能力的不足，算法引入了特征庫(kù)定期更新策略，保證了算法長(zhǎng)期有效性。

1 人工地圖庫(kù)建立

1.1 特征點(diǎn)提取及匹配

1.1.1 SIFT特征點(diǎn)檢測(cè)算法

選取SIFT算法[10]提取圖像特征，其原理如下：SIFT算法的尺度空間由DOG(different of Gaussian)金字塔與不同尺度的圖片卷積得到。對(duì)于已經(jīng)檢測(cè)到的特征點(diǎn)，已知特征點(diǎn)的尺度值σ，根據(jù)這一尺度，得到最接近這一尺度的高斯圖像。SIFT算法先進(jìn)行非極大值抑制，去除低對(duì)比度的點(diǎn)，用直方圖統(tǒng)計(jì)鄰域像素的梯度幅值和方向，直方圖的峰值代表了該特征點(diǎn)處鄰域內(nèi)圖像梯度的主方向，由于可能存在多個(gè)最高點(diǎn)的情況，因此特征點(diǎn)方向可能存在多個(gè)。最后每個(gè)關(guān)鍵點(diǎn)都有三個(gè)特征信息：位置(x,y)、特征尺度σ和特征方向θ。SIFT算法將特征點(diǎn)方形區(qū)域劃分為4×4個(gè)部分，統(tǒng)計(jì)每個(gè)部分內(nèi)的梯度方向和特征值為8 bin的直方圖，得到128維的向量作為特征描述子。

1.1.2 特征點(diǎn)匹配

采用基于Kd-樹[11]的BBF(best bin first)算法[12]進(jìn)行快速特征點(diǎn)匹配，該算法的優(yōu)點(diǎn)在于其能夠優(yōu)先查找與查詢數(shù)據(jù)點(diǎn)更為接近的數(shù)據(jù)空間。在匹配過(guò)程中，引入在極線、視差、方向、尺度上的約束限制，從而去除誤匹配：

1)極線約束：圖像經(jīng)過(guò)標(biāo)定校正后，左右特征點(diǎn)垂直方向上小于1個(gè)像素。

2)視差約束：水平方向上，左圖特征點(diǎn)坐標(biāo)大于右圖坐標(biāo)，且差值小于20個(gè)像素。

3)方向約束：左右特征點(diǎn)方向差值小于20°。

4)尺度約束：左右特征點(diǎn)尺度差值小于1.5。

5)唯一匹配約束：如果一個(gè)特征點(diǎn)有多個(gè)特征點(diǎn)滿足約束條件與之相匹配，則該特征點(diǎn)失效，將其丟棄。

使用歐氏距離作為特征點(diǎn)間的相似性度量，兩個(gè)特征點(diǎn)在特征空間上矢量距離公式為

(1)

式中x=(x1,x2,…,x128)、y=(y1,y2,…,y128)分別為特征點(diǎn)x和特征點(diǎn)y的128維矢量描述符。

表1統(tǒng)計(jì)了離線建圖過(guò)程圖像匹配約束結(jié)果，也可以從圖1發(fā)現(xiàn)經(jīng)過(guò)匹配約束后，能夠有效地去除誤匹配的特征點(diǎn)，誤匹配的特征點(diǎn)顯著減少。

表1 匹配約束結(jié)果Table 1 The result of match constraint

圖1 第1幀圖像匹配Fig.1 The frame of first frame image

1.2 特征點(diǎn)拍攝及位置解算

1.2.1 圖片拍攝過(guò)程

如圖2所示，相機(jī)系統(tǒng)主要由以下幾個(gè)部分組成(自下往上的順序)：三腳架、旋轉(zhuǎn)云臺(tái)、雙目相機(jī)、指南針。在室內(nèi)環(huán)境下建立世界坐標(biāo)系，并選取若干具有代表性的拍攝位置，每個(gè)位置在世界坐標(biāo)系的坐標(biāo)通過(guò)測(cè)量獲得。在每個(gè)位置處，將雙目相機(jī)固定在三腳架上的旋轉(zhuǎn)云臺(tái)上，建立以相機(jī)左目為原點(diǎn)的機(jī)器人坐標(biāo)系，借助三腳架水平泡和云臺(tái)水平泡確保旋轉(zhuǎn)云臺(tái)系統(tǒng)水平轉(zhuǎn)動(dòng)(即轉(zhuǎn)動(dòng)過(guò)程中相機(jī)的俯仰角始終為0°)，并在多個(gè)角度拍攝，云臺(tái)帶有角度刻度，可直接讀取拍攝角度，拍攝時(shí)借助指南針來(lái)保證不同位置的角度零位一致。如圖3所示，本實(shí)驗(yàn)共選取了5個(gè)位置，每個(gè)位置選取12個(gè)拍攝方向(在360°方向均勻分布)，采集的圖像為1 024×768的模式，每次拍攝獲得左右兩幅圖像。

1.2.2 特征點(diǎn)位置解算

利用SIFT特征點(diǎn)檢測(cè)算法提取并保存特征點(diǎn)的像素坐標(biāo)，已知雙目相機(jī)的視差d=12 cm，焦距f=839.478 943。特征點(diǎn)位置以左目坐標(biāo)系為基準(zhǔn)，在右目中尋找與左目相匹配的特征點(diǎn)。已知左目特征點(diǎn)的像素坐標(biāo)(xl,yl)，與左目匹配的右目特征點(diǎn)的像素坐標(biāo)(xr,yr)，根據(jù)雙目幾何計(jì)算公式可以解算出左目特征點(diǎn)在攝像機(jī)坐標(biāo)系下的三維坐標(biāo)(xi,yi,zi)。雙目幾何計(jì)算公式為

(2)

(3)

(4)

其中：Δx=xl-xr，Δy=yl-yr。

圖2 相機(jī)系統(tǒng)Fig.2 The system of camera

圖3 相機(jī)拍攝位置及方向示意Fig.3 The position and direction of camera

由特征點(diǎn)在機(jī)器人坐標(biāo)系下的坐標(biāo)與相機(jī)拍攝位置在世界坐標(biāo)系下的位置姿態(tài)tj=(tjx,tjy,tjz,θj)(j=1,2,3,4,5)，可以解算出該左目特征點(diǎn)在世界坐標(biāo)系下的三維坐標(biāo)(ui,wi,vi):

ui=tjx+zicosθj-xisinθj

(5)

wi=tjy+yi

(6)

vi=tjz+zisinθj+xicosθj

(7)

1.3 數(shù)據(jù)關(guān)聯(lián)

建圖過(guò)程需要將多次拍攝的特征融合，所以每提取新的一幀雙目圖像中的特征時(shí)，需首先將每個(gè)特征與已記入庫(kù)中的特征進(jìn)行匹配，以排除重復(fù)特征，即本節(jié)所指的數(shù)據(jù)關(guān)聯(lián)問(wèn)題，另外，機(jī)器人在實(shí)時(shí)定位過(guò)程中也需進(jìn)行數(shù)據(jù)關(guān)聯(lián)。數(shù)據(jù)關(guān)聯(lián)的正確性直接關(guān)系到定位和地圖創(chuàng)建的精度，為提高關(guān)聯(lián)準(zhǔn)確性，采用雙向匹配方法。具體做法如下：設(shè)LV1為當(dāng)前幀圖像觀測(cè)到的特征點(diǎn)之一，遍歷搜索地圖庫(kù)中與該點(diǎn)特征相似性最高的特征點(diǎn)，將該點(diǎn)設(shè)為L(zhǎng)D1，計(jì)算公式見式(1)，此為正向匹配；然后在當(dāng)前幀觀測(cè)特征點(diǎn)集中尋找與LD1特征最相似的點(diǎn)，將該點(diǎn)設(shè)為L(zhǎng)V2，此為逆向匹配，然后判斷LV1和LV2是否為同一點(diǎn)，是則本次關(guān)聯(lián)成功，匹配點(diǎn)對(duì)為L(zhǎng)V1(LV2)和LD1，否則本次關(guān)聯(lián)失敗。

引入數(shù)據(jù)關(guān)聯(lián)方法后，可初步建立特征地圖庫(kù)，直接將拍攝到所有雙目圖像中的特征解算位置后匯總，并利用數(shù)據(jù)關(guān)聯(lián)方法去除重復(fù)點(diǎn)即可，建立后的特征庫(kù)平面分布如圖4所示，其定位實(shí)驗(yàn)數(shù)據(jù)見第4章定位綜合實(shí)驗(yàn)中表3全部特征點(diǎn)實(shí)驗(yàn)。

圖4 特征庫(kù)平面分布圖Fig.4 The flat figure of feature-database

1.4 路標(biāo)的稀疏性策略

考慮到過(guò)于密集的特征點(diǎn)未必能進(jìn)一步提高定位精度，而且會(huì)增加計(jì)算量，所以為了合理控制路標(biāo)的數(shù)量和密度均勻性，引入稀疏性限制。

在圖4給出的建圖結(jié)果中，每次提取一個(gè)新特征點(diǎn)時(shí)，如未關(guān)聯(lián)成功，則直接作為新增特征點(diǎn)插入特征庫(kù)，本節(jié)對(duì)未關(guān)聯(lián)成功的特征點(diǎn)先進(jìn)行稀疏性檢驗(yàn)，滿足條件的特征才進(jìn)行插入操作，具體稀疏性檢驗(yàn)策略如下：

如圖5，圖中綠色的標(biāo)記代表地圖庫(kù)中已有的路標(biāo)，對(duì)于地圖庫(kù)中的任一路標(biāo)LDi，設(shè)其對(duì)應(yīng)的兩個(gè)空間鄰域分別為ΩA和ΩB(其中ΩA?ΩB)，ΩB以外的區(qū)域定義位為ΩC，則路標(biāo)的稀疏性限制條件為：

1)在路標(biāo)LDi的ΩA區(qū)域內(nèi)，不允許存在任何其他路標(biāo)；

2)在路標(biāo)LDi的ΩB內(nèi)ΩA外的區(qū)域中，不允許存在與其相似性較高的路標(biāo)(為便于說(shuō)明，圖中用圓形標(biāo)記代表與LDi相似性較高的路標(biāo)，用三角形標(biāo)記代表與LDi相似性較低的路標(biāo))。

圖5 特征點(diǎn)稀疏性約束Fig.5 The sparse strategy of feature point

其中第1個(gè)限制條件可以控制路標(biāo)位置空間分布密度，第2個(gè)限制條件可以控制路標(biāo)的特征相似性分布，保證空間距離較近的路標(biāo)都在特征上有明顯的差別，可有效防止路標(biāo)匹配混淆。

對(duì)于當(dāng)前幀圖像發(fā)現(xiàn)的每個(gè)路標(biāo)，首先判斷該路標(biāo)與地圖庫(kù)中已存在路標(biāo)的相對(duì)位置關(guān)系以及特征相似性，以LDi為例，假設(shè)新增路標(biāo)LVm與地圖庫(kù)路標(biāo)LDi相似性較低，則該路標(biāo)位置位于LDi的ΩA鄰域之外時(shí)才符合插入條件，而對(duì)于新增路標(biāo)LVn與LDi相似性較高時(shí)，則其位置必須位于LDi的ΩB鄰域以外才符合插入條件。需要說(shuō)明的是，圖5僅以路標(biāo)點(diǎn)LDi為例來(lái)說(shuō)明插入判斷條件，實(shí)際操作時(shí)必須遍歷地圖庫(kù)中路標(biāo)點(diǎn)，對(duì)于所有路標(biāo)均符合插入條件時(shí)，該點(diǎn)才允許被插入，即對(duì)于任何新發(fā)現(xiàn)路標(biāo)LVj，需滿足式(8)、(9)其中之一：

LVj∈ΩC

(8)

LVj∈ΩB，LVj?ΩA,D(LDi,LVj)>df0

(9)

式中：i=1,2,…,N,N為地圖庫(kù)中路標(biāo)個(gè)數(shù)；df0為閾值常量。在建圖過(guò)程中，只要合理分配空間鄰域和選擇特征空間閾值df0，就可以同時(shí)控制地圖中路標(biāo)點(diǎn)在位置空間和特征空間的分布密度。

表2統(tǒng)計(jì)了離線建圖過(guò)程圖特征點(diǎn)稀疏性約束結(jié)果，圖6為路標(biāo)經(jīng)稀疏性策略約束后(取df0=2.5)，地圖庫(kù)中路標(biāo)二維平面分布圖。其定位實(shí)驗(yàn)數(shù)據(jù)見表3稀疏策略后實(shí)驗(yàn)，相比較全部特征點(diǎn)實(shí)驗(yàn)，在定位精度和實(shí)時(shí)性上有提升。

表2 特征庫(kù)稀疏性約束結(jié)果Table 2 The result of sparse strategy in feature-database

圖7為建圖過(guò)程的雙目圖像及特征匹配過(guò)程，抽取相機(jī)左目拍攝的4幀圖像。圖中“○”表示當(dāng)前幀圖像中新加入全局特征庫(kù)的特征點(diǎn)(未關(guān)聯(lián)成功的特征)，“┌”表示當(dāng)前幀圖像觀測(cè)到與全局特征庫(kù)數(shù)據(jù)關(guān)聯(lián)成功的特征點(diǎn)，“×”表示當(dāng)前幀圖像中被稀疏掉的特征點(diǎn)。

圖6 稀疏后特征庫(kù)平面分布圖Fig.6 The flat figure of sparse strategy in feature-database

圖7 數(shù)據(jù)關(guān)聯(lián)實(shí)驗(yàn)圖片F(xiàn)ig.7 The experiment figure of data relation

1.5 穩(wěn)健特征點(diǎn)篩選策略

在識(shí)別場(chǎng)景目標(biāo)時(shí)，人類的視覺系統(tǒng)有一個(gè)對(duì)目標(biāo)的顯著特征區(qū)域進(jìn)行定位的過(guò)程。在計(jì)算機(jī)視覺研究中，穩(wěn)健局部不變特征[13-14]選擇技術(shù)類似于人類的這種視覺顯著區(qū)域定位功能，是指在不同位置和角度拍攝時(shí)，將被反復(fù)觀測(cè)次數(shù)較多的特征點(diǎn)稱為“穩(wěn)健特征點(diǎn)”(或稱“穩(wěn)健點(diǎn)”)。穩(wěn)健點(diǎn)表征了圖像局部的顯著特性，選擇其進(jìn)行特征匹配，將進(jìn)一步提高定位可靠性和精度。本節(jié)將引入穩(wěn)健點(diǎn)篩選機(jī)制，建立基于穩(wěn)健點(diǎn)的全局特征庫(kù)。

在人工建圖過(guò)程中，記錄每個(gè)特征點(diǎn)在整個(gè)人工建圖過(guò)程中被雙目相機(jī)觀望到次數(shù)，選取觀望次數(shù)不小于3次的特征點(diǎn)視為特征點(diǎn)中的“穩(wěn)健特征點(diǎn)”；而觀望次數(shù)小于3次的特征點(diǎn)視為特征點(diǎn)中的“一般特征點(diǎn)”(或稱“非穩(wěn)健特征點(diǎn)”)。將所有的穩(wěn)健特征點(diǎn)數(shù)據(jù)信息保存到全局特征庫(kù)。在某次拍攝中在稀疏處理基礎(chǔ)上，進(jìn)一步采用穩(wěn)健點(diǎn)約束處理后的特征點(diǎn)數(shù)量，圖8為特征點(diǎn)平面分布圖，最后將去除537個(gè)一般特征點(diǎn)，最終僅采用307個(gè)穩(wěn)健特征點(diǎn)定位，這不僅大大簡(jiǎn)化了計(jì)算量，同時(shí)也提高了定位精度，其定位實(shí)驗(yàn)見表3穩(wěn)健點(diǎn)篩選后實(shí)驗(yàn)。

圖8 穩(wěn)健點(diǎn)特征庫(kù)平面分布圖Fig.8 The flat figure of salient feature-database

2 基于特征庫(kù)的機(jī)器人定位

如圖9，采用履帶式移動(dòng)機(jī)器人平臺(tái)，并采用雙目攝像機(jī)作為視覺傳感器，安裝在移動(dòng)機(jī)器人前端，機(jī)器人左右履帶差動(dòng)運(yùn)動(dòng)，可原地旋轉(zhuǎn)，利用左右輪的編碼器進(jìn)行里程測(cè)量，很容易估算出機(jī)器人的中心速度和角速度，因此后文設(shè)計(jì)算法時(shí)，直接假設(shè)可獲得機(jī)器人的速度和角速度里程信息。在機(jī)器人定位過(guò)程中，認(rèn)為特征庫(kù)已經(jīng)固定，定位時(shí)只需基于特征庫(kù)動(dòng)態(tài)推算機(jī)器人當(dāng)前位置和姿態(tài)即可，本節(jié)首先建立機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型，然后基于擴(kuò)展卡爾曼濾波方法實(shí)現(xiàn)機(jī)器人位置估計(jì)。

表3 機(jī)器人每幀估計(jì)位姿和實(shí)測(cè)數(shù)據(jù)Table 3 The measured data of estimated pose by every frame figure of robot

圖9 移動(dòng)機(jī)器人Fig.9 The figure of mobile robot

2.1 移動(dòng)機(jī)器人運(yùn)動(dòng)模型

通常情況下機(jī)器人運(yùn)動(dòng)模型都是非線性的：

Xr(k+1)=f(Xr(k),u(k))+?r(k)

(10)

(11)

式中：ΔT為時(shí)間間隔，?r(k)為高斯噪聲，其協(xié)方差矩陣為Q(k)。

圖10 移動(dòng)機(jī)器人運(yùn)動(dòng)及觀測(cè)模型Fig.10 The model of process and observation

2.2 移動(dòng)機(jī)器人觀測(cè)模型

觀測(cè)值z(mì)(k)采用極坐標(biāo)表示，即在視覺坐標(biāo)系下，環(huán)境中某一特征點(diǎn)距離傳感器的距離r和方向角β，如圖10所示，極坐標(biāo)表示形式為

(12)

式中：(xi,yi)為觀測(cè)到第i個(gè)路標(biāo)的世界坐標(biāo)，γh為觀測(cè)噪聲，其協(xié)方差矩陣為R(k)。

2.3 基于擴(kuò)展卡爾曼位置估計(jì)

EKF算法將機(jī)器人的位姿隨機(jī)器人的運(yùn)動(dòng)不斷更新，Xr(k)為k時(shí)刻機(jī)器人的位姿，其狀態(tài)向量為：Xr(k)=[x(k)y(k)θ(k)]T。人工地圖建成后，特征庫(kù)中路標(biāo)的坐標(biāo)信息寫入一個(gè)狀態(tài)向量Xl(k)=[u1v1…unvn]T，在定位過(guò)程中Xl(k)不再更新。算法步驟如下：

1)根據(jù)機(jī)器人的里程計(jì)信息，可以獲得第k步機(jī)器人的速度和角速度，根據(jù)第k步機(jī)器人位姿可以推算出第k+1步的機(jī)器人位姿估計(jì)，利用擴(kuò)展卡爾曼誤差協(xié)方差估計(jì)公式解得誤差協(xié)方差矩陣估計(jì)P(k+1)-。

(13)

P(k+1)-=F(k)P(k)FT(k)+Q(k)

(14)

式中F(k)為f對(duì)X(k)的偏導(dǎo)。

2)提取當(dāng)前幀雙目圖像特征，將觀測(cè)到的每個(gè)特征點(diǎn)利用1.2節(jié)位置解算公式解算出該特征點(diǎn)在世界坐標(biāo)系下實(shí)際三維坐標(biāo)，并與全局特征庫(kù)路標(biāo)進(jìn)行數(shù)據(jù)關(guān)聯(lián)，采用1.3節(jié)的數(shù)據(jù)關(guān)聯(lián)方法，對(duì)于每次關(guān)聯(lián)成功的特征點(diǎn)，均采用步驟3和步驟4修正機(jī)器人的位置和姿態(tài)信息。

(15)

(16)

(17)

P(k+1)=(I-K(k+1)Jh(k+1))P(k+1)-

(18)

3 特征庫(kù)的定期更新

本文采用的離線建立特征庫(kù)建立策略，保證了在線定位的實(shí)時(shí)性、精度和穩(wěn)定性，但缺點(diǎn)是地圖固化后無(wú)法適應(yīng)環(huán)境的后期變化，為了提高算法實(shí)用性，本節(jié)引入特征庫(kù)的定期更新策略進(jìn)行彌補(bǔ)。

如圖11，圖中的全局特征庫(kù)代表已經(jīng)離線建立的特征庫(kù)，也是機(jī)器人實(shí)時(shí)定位的基礎(chǔ)，在此基礎(chǔ)上增加一個(gè)動(dòng)態(tài)特征庫(kù)，記錄機(jī)器人近期一段時(shí)間觀測(cè)獲得的特征，動(dòng)態(tài)特征庫(kù)不斷從機(jī)器人當(dāng)前觀測(cè)特征中獲取數(shù)據(jù)，區(qū)別只是此時(shí)機(jī)器人位置姿態(tài)不再通過(guò)人工測(cè)量，而是實(shí)時(shí)通過(guò)定位算法獲取。動(dòng)態(tài)庫(kù)更新策略與第1節(jié)人工建圖過(guò)程一致，但主要經(jīng)過(guò)特征提取、數(shù)據(jù)關(guān)聯(lián)、稀疏性限制等步驟，暫不進(jìn)行穩(wěn)健點(diǎn)篩選。

動(dòng)態(tài)特征庫(kù)會(huì)定期地更新至全局特征庫(kù)，更新過(guò)程主要經(jīng)過(guò)數(shù)據(jù)關(guān)聯(lián)(去除重復(fù)點(diǎn))、稀疏性限制和特征的穩(wěn)健點(diǎn)篩選步驟，更新完成后即可清除動(dòng)態(tài)特征庫(kù)，后續(xù)定位時(shí)再重新記錄。該更新過(guò)程不需要實(shí)時(shí)進(jìn)行，可間隔一段時(shí)間更新一次，或者室內(nèi)環(huán)境發(fā)生明顯變化時(shí)再更新，這樣可以保證用來(lái)定位的全局特征庫(kù)不會(huì)發(fā)生快速變化，保證定位過(guò)程的穩(wěn)定性，同時(shí)也便于在動(dòng)態(tài)特征庫(kù)中不斷統(tǒng)計(jì)特征點(diǎn)的觀測(cè)次數(shù)，以便作出正確的穩(wěn)健點(diǎn)評(píng)價(jià)后再更新至全局特征庫(kù)。

圖11 特征庫(kù)定期更新機(jī)制Fig.11 The periodically updated strategy of feature-database

為防止全局特征庫(kù)中特征持續(xù)上升，可定期對(duì)其進(jìn)行特征刪除操作，刪除條件可檢測(cè)理論上應(yīng)位于機(jī)器人觀測(cè)視場(chǎng)之內(nèi)，當(dāng)實(shí)際觀測(cè)時(shí)并未檢測(cè)到該特征的情況，設(shè)機(jī)器人k時(shí)刻的位姿[x(k)y(k)θ(k)]T和機(jī)器人的觀測(cè)角度范圍(-β0,β0)、距離范圍(rmin,rmax)，遍歷全局特征庫(kù)中的每一個(gè)路標(biāo)[uivi]T，并利用式(12)計(jì)算該路標(biāo)在機(jī)器人觀測(cè)坐標(biāo)系下的極坐標(biāo)[ri,βi]T，如果位于有效觀測(cè)范圍內(nèi)，但數(shù)據(jù)關(guān)聯(lián)時(shí)未匹配該特征，則認(rèn)為本特征觀測(cè)失敗，為防止誤操作，可對(duì)特征庫(kù)中特征觀測(cè)失敗次數(shù)進(jìn)行計(jì)數(shù)，達(dá)到一定次數(shù)時(shí)再進(jìn)行刪除。

通過(guò)上述特征更新策略，即可維持特征庫(kù)數(shù)量的平衡，又可定期換入新的特征，從而不斷適應(yīng)環(huán)境的變化，達(dá)到長(zhǎng)期有效的定位。

4 定位綜合實(shí)驗(yàn)

定位實(shí)驗(yàn)選取的是1 024×768的圖像，機(jī)器人直線速度約為0.07 m/s，轉(zhuǎn)彎的角速度約為0.23 rad/s，機(jī)器人的初始位置(2.36 m，6.79 m)。

令機(jī)器人在實(shí)驗(yàn)室復(fù)雜室內(nèi)環(huán)境下進(jìn)行定位實(shí)驗(yàn)，結(jié)果如表3，為了對(duì)比前面建圖及定位過(guò)程各項(xiàng)策略的效果，表中按照特征庫(kù)選取全部特征點(diǎn)、稀疏策略后、穩(wěn)健點(diǎn)篩選后以及定位過(guò)程特征庫(kù)定期更

新后分別進(jìn)行實(shí)驗(yàn)，各類實(shí)驗(yàn)均為離線建圖模式，不同實(shí)驗(yàn)最終保留的特征點(diǎn)數(shù)量如表中第2列所示。圖12為穩(wěn)健特征點(diǎn)定位實(shí)驗(yàn)過(guò)程中，拍取幾幀圖像，圖像中的兩種符號(hào)(○型符號(hào)和×型符號(hào))所代表的是該幀圖像所觀測(cè)到的特征點(diǎn)；在定位過(guò)程中，圖像中○型符號(hào)為與特征庫(kù)中路標(biāo)匹配關(guān)聯(lián)成功的特征點(diǎn)，而×型符號(hào)為與特征庫(kù)中路標(biāo)沒(méi)有匹配關(guān)聯(lián)的特征點(diǎn)。圖13為本次穩(wěn)健特征點(diǎn)實(shí)驗(yàn)過(guò)程中，X方向偏差曲線圖、Y方向偏差曲線圖、角度偏差曲線圖，以及在X-Y二維軌跡上偏差曲線圖。表3中分別給出了各實(shí)驗(yàn)過(guò)程x方向偏差、y方向偏差、角度偏差以及實(shí)時(shí)性。對(duì)比可見，在全部特征點(diǎn)試驗(yàn)結(jié)果基礎(chǔ)上，通過(guò)引入稀疏策略、穩(wěn)健點(diǎn)篩選策略，實(shí)時(shí)性不斷提升，但定位精度并未受到明顯影響，且有少量提升。另外進(jìn)行了特征庫(kù)后期更新試驗(yàn)(表中最后一行結(jié)果)，在前期定位實(shí)驗(yàn)一周后再次在相同環(huán)境下定位實(shí)驗(yàn)(環(huán)境已有部分變化)，引入特征庫(kù)定期更新策略后，新增了56個(gè)特征點(diǎn)，刪除掉了42個(gè)特征點(diǎn)，最終實(shí)驗(yàn)結(jié)束后，特征庫(kù)共有321個(gè)特征點(diǎn)，其定位精度及實(shí)時(shí)性與前期實(shí)驗(yàn)結(jié)果接近。

圖12 定位實(shí)驗(yàn)圖片F(xiàn)ig.12 The figures of locative experiment

圖13 穩(wěn)健點(diǎn)篩選后機(jī)器人定位實(shí)驗(yàn)誤差曲線Fig.13 The error curves of locative experiment in salient feature-database

5 結(jié)論

1)針對(duì)移動(dòng)機(jī)器人室內(nèi)環(huán)境的雙目定位問(wèn)題開展研究，以EKF方法為基礎(chǔ)，但初始特征庫(kù)采用人工離線測(cè)量的方法，特征庫(kù)一旦建立，在線定位時(shí)僅對(duì)機(jī)器人位姿進(jìn)行卡爾曼濾波估計(jì)，而不再維護(hù)包含路標(biāo)位置在內(nèi)的大型協(xié)方差矩陣，克服了EKF-SLAM方法計(jì)算量過(guò)大和數(shù)據(jù)關(guān)聯(lián)脆弱的問(wèn)題，提高了定位算法的實(shí)時(shí)性和可靠性。

2)本方法在建立基于雙目視覺系統(tǒng)的特征庫(kù)時(shí)，在房間內(nèi)選擇若干拍攝位置，每個(gè)位置選擇多個(gè)拍攝角度，形成初始圖片庫(kù)，然后根據(jù)拍攝位置、角度信息和雙目視覺測(cè)距公式解算特征點(diǎn)的三維位置等信息，解算并建立離線特征庫(kù)，解算過(guò)程中引入了特征稀疏性策略、穩(wěn)健特征點(diǎn)選取、誤匹配去除等方法，既提高了定位精度和可靠性，又可合理控制特征點(diǎn)數(shù)量。

3)在已建立的離線特征庫(kù)的基礎(chǔ)上，通過(guò)EKF方法實(shí)現(xiàn)機(jī)器人在線定位，且為了適應(yīng)室內(nèi)環(huán)境變化，算法可定期記錄圖片庫(kù)，并實(shí)現(xiàn)特征庫(kù)的定期更新，最終算法可保證機(jī)器人在室內(nèi)環(huán)境下長(zhǎng)期、有效地定位。

4)本方法應(yīng)用過(guò)程需要事先拍攝環(huán)境，僅適用于特定環(huán)境下的定位，不適用于對(duì)未知環(huán)境的探索過(guò)程。

[1] 權(quán)美香, 樸松昊, 李國(guó). 視覺SLAM綜述[J]. 智能系統(tǒng)學(xué)報(bào)， 2016, 11(6): 768-776.

QUAN Meixiang, PIAO Songhao, LI Guo. An overview of visual SLAM[J]. CAAI transactions on intelligent systems, 2016, 11(6): 768-776.

[2] 顧照鵬, 劉宏. 單目視覺同步定位與地圖創(chuàng)建方法綜述[J]. 智能系統(tǒng)學(xué)報(bào), 2015, 10(4): 499-507.

GU Zhaopeng, LIU Hong. A survey of monocular simultaneous localization and mapping[J]. CAAI transactions on intelligent systems, 2015, 10(4): 499-507.

[3] LEMAIRE T, BERGER C, JUNG I, et al. Vision-based SLAM: stereo and monocular approaches[J]. International journal of computer vision, 2007, 74(3): 343-364.

[4] 林睿. 基于圖像特征點(diǎn)的移動(dòng)機(jī)器人立體視覺SLAM研究[D]. 哈爾濱:哈爾濱工業(yè)大學(xué), 2011: 6-11.

LIN Rui. Research on image feature-based mobile robot stereo visual SLAM[D]. Harbin: Harbin Institute of Technology, 2011: 6-11.

[5] WAN E A, RUDOLPH V D M. The Unscented Kalman Filter[J]. Kalman Filtering & Neural Networks, 2009: 221-280.

[6] THRUN S, LIU Y. Multi-robot SLAM with sparse extended information filers[C]//Proceedings of the 11th International Symposium of Robotics Research. 2003: 254-266.

[7] GUIVANT J E, NEBOT E. Solving computational and memory requirements of feature-based simultaneous localization and mapping algorithms[J]. IEEE transactions on robotics and automation, 2003, 19(4): 749-755.

[8] 王玉全. 基于全景視覺的移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué)，2010: 82-94.

WANG Yuquan. A research on simultaneous location and mapping of mobile robot with omnidirectional vision[D]. Harbin: Harbin Engineering University, 2010: 82-94.

[9] 趙鑫. 未知環(huán)境下無(wú)人駕駛汽車同時(shí)定位與地圖創(chuàng)建研究[D]. 成都：西南交通大學(xué), 2017: 19-27.

ZHAO Xin. Research on simultaneous locatization and mapping of self-driving vehicle in unknown environment[D]. Chengdu: Southwest Jiaotong University, 2017: 19-27.

[10] LOW D G. Distinctive image features from scale-invariant keypoints[J]. International journal of computer vision, 2004, 60(2): 91-110.

[11]LIU J, QIU Y. Application of SIFT feature extraction algorithm on the image registration[C]// 2011 10th International Conference on Electronic Measurement & Instruments (ICEMI). 2011:177-180.

[12] 杜振鵬, 李德華. 基于KD-Tree搜索和SURF特征的圖像匹配算法研究[J]. 計(jì)算機(jī)與數(shù)字工程, 2012, 40(2): 96-98.

DU Zhenpeng, LI Dehua. Image matching algorithm research based on KD-Tree search and surf features[J]. Compute & digital engineering, 2012, 40(2): 96-98.

[13] 劉建軍. 基于圖像局部不變特征的類屬超圖構(gòu)建與目標(biāo)識(shí)別技術(shù)研究[D]. 長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué), 2010: 18-24.

LIU Jianjun. Research on local invariant features based class specific hyper graphs learning and object recognition[D]. Changsha: Nation University of Defence Technology, 2010: 18-24.

[14] ELAZARY L, ITTI L. Interesting objects are visually salient [J]. Journal of vision, 2008, 8(3): 1-15.

本文引用格式：

張智， 張磊， 蘇麗， 等. 基于人工離線特征庫(kù)的室內(nèi)機(jī)器人雙目定位[J]. 哈爾濱工程大學(xué)學(xué)報(bào)， 2017， 38(12): 1906 -1914.

ZHANG Zhi, ZHANG Lei, SU Li, et al. Binocular localization of indoor robot based on artificial offline feature-database[J]. Journal of Harbin Engineering University， 2017， 38(12): 1906 -1914.