基于人工離線特征庫的室內機器人雙目定位

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

(哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001)

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

EKF-SLAM方法是一種最優循環迭代估計，在處理信息不確定性方面有獨到之處，但針對其計算量隨路標個數增多而增長較快的問題，常需改進處理，然而在室內獨立小環境中，以子圖劃分等思想為基礎的快速方法難以適用，此外，EKF-SLAM還存在一個明顯的缺點便是數據關聯脆弱問題，即一旦數據關聯出現錯誤，會將誤差代入估計結果中，而且不可恢復，即通常說的EKF(extended Kalman filter)缺乏自恢復能力。

本文以EKF-SLAM算法為基礎，并采用了人工離線建立特征庫的方法，克服傳統方法存在的計算量過大、關聯脆弱等問題，實現了室內復雜環境下實時、可靠地定位。此外，在人工建立離線特征庫時，解算過程考慮眾多特征的有效融合、特征密度控制、優質特征篩選等問題，為此，本算法引入了特征稀疏性策略、穩健特征點選取、誤匹配去除等方法。算法在定位過程中需固化特征庫，但系統實際工作環境難免會發生一定變化，為彌補算法適應動態環境能力的不足，算法引入了特征庫定期更新策略，保證了算法長期有效性。

1 人工地圖庫建立

1.1 特征點提取及匹配

1.1.1 SIFT特征點檢測算法

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

1.1.2 特征點匹配

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

1)極線約束：圖像經過標定校正后，左右特征點垂直方向上小于1個像素。

2)視差約束：水平方向上，左圖特征點坐標大于右圖坐標，且差值小于20個像素。

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

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

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

使用歐氏距離作為特征點間的相似性度量，兩個特征點在特征空間上矢量距離公式為

(1)

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

表1統計了離線建圖過程圖像匹配約束結果，也可以從圖1發現經過匹配約束后，能夠有效地去除誤匹配的特征點，誤匹配的特征點顯著減少。

表1 匹配約束結果Table 1 The result of match constraint

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

1.2 特征點拍攝及位置解算

1.2.1 圖片拍攝過程

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

1.2.2 特征點位置解算

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

(2)

(3)

(4)

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

圖2 相機系統Fig.2 The system of camera

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

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

ui=tjx+zicosθj-xisinθj

(5)

wi=tjy+yi

(6)

vi=tjz+zisinθj+xicosθj

(7)

1.3 數據關聯

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

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

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

1.4 路標的稀疏性策略

考慮到過于密集的特征點未必能進一步提高定位精度，而且會增加計算量，所以為了合理控制路標的數量和密度均勻性，引入稀疏性限制。

在圖4給出的建圖結果中，每次提取一個新特征點時，如未關聯成功，則直接作為新增特征點插入特征庫，本節對未關聯成功的特征點先進行稀疏性檢驗，滿足條件的特征才進行插入操作，具體稀疏性檢驗策略如下：

如圖5，圖中綠色的標記代表地圖庫中已有的路標，對于地圖庫中的任一路標LDi，設其對應的兩個空間鄰域分別為ΩA和ΩB(其中ΩA?ΩB)，ΩB以外的區域定義位為ΩC，則路標的稀疏性限制條件為：

1)在路標LDi的ΩA區域內，不允許存在任何其他路標；

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

圖5 特征點稀疏性約束Fig.5 The sparse strategy of feature point

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

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

LVj∈ΩC

(8)

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

(9)

式中：i=1,2,…,N,N為地圖庫中路標個數；df0為閾值常量。在建圖過程中，只要合理分配空間鄰域和選擇特征空間閾值df0，就可以同時控制地圖中路標點在位置空間和特征空間的分布密度。

表2統計了離線建圖過程圖特征點稀疏性約束結果，圖6為路標經稀疏性策略約束后(取df0=2.5)，地圖庫中路標二維平面分布圖。其定位實驗數據見表3稀疏策略后實驗，相比較全部特征點實驗，在定位精度和實時性上有提升。

表2 特征庫稀疏性約束結果Table 2 The result of sparse strategy in feature-database

圖7為建圖過程的雙目圖像及特征匹配過程，抽取相機左目拍攝的4幀圖像。圖中“○”表示當前幀圖像中新加入全局特征庫的特征點(未關聯成功的特征)，“┌”表示當前幀圖像觀測到與全局特征庫數據關聯成功的特征點，“×”表示當前幀圖像中被稀疏掉的特征點。

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

圖7 數據關聯實驗圖片Fig.7 The experiment figure of data relation

1.5 穩健特征點篩選策略

在識別場景目標時，人類的視覺系統有一個對目標的顯著特征區域進行定位的過程。在計算機視覺研究中，穩健局部不變特征[13-14]選擇技術類似于人類的這種視覺顯著區域定位功能，是指在不同位置和角度拍攝時，將被反復觀測次數較多的特征點稱為“穩健特征點”(或稱“穩健點”)。穩健點表征了圖像局部的顯著特性，選擇其進行特征匹配，將進一步提高定位可靠性和精度。本節將引入穩健點篩選機制，建立基于穩健點的全局特征庫。

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

圖8 穩健點特征庫平面分布圖Fig.8 The flat figure of salient feature-database

2 基于特征庫的機器人定位

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

表3 機器人每幀估計位姿和實測數據Table 3 The measured data of estimated pose by every frame figure of robot

圖9 移動機器人Fig.9 The figure of mobile robot

2.1 移動機器人運動模型

通常情況下機器人運動模型都是非線性的：

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

(10)

(11)

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

圖10 移動機器人運動及觀測模型Fig.10 The model of process and observation

2.2 移動機器人觀測模型

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

(12)

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

2.3 基于擴展卡爾曼位置估計

EKF算法將機器人的位姿隨機器人的運動不斷更新，Xr(k)為k時刻機器人的位姿，其狀態向量為：Xr(k)=[x(k)y(k)θ(k)]T。人工地圖建成后，特征庫中路標的坐標信息寫入一個狀態向量Xl(k)=[u1v1…unvn]T，在定位過程中Xl(k)不再更新。算法步驟如下：

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

(13)

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

(14)

式中F(k)為f對X(k)的偏導。

2)提取當前幀雙目圖像特征，將觀測到的每個特征點利用1.2節位置解算公式解算出該特征點在世界坐標系下實際三維坐標，并與全局特征庫路標進行數據關聯，采用1.3節的數據關聯方法，對于每次關聯成功的特征點，均采用步驟3和步驟4修正機器人的位置和姿態信息。

(15)

(16)

(17)

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

(18)

3 特征庫的定期更新

本文采用的離線建立特征庫建立策略，保證了在線定位的實時性、精度和穩定性，但缺點是地圖固化后無法適應環境的后期變化，為了提高算法實用性，本節引入特征庫的定期更新策略進行彌補。

如圖11，圖中的全局特征庫代表已經離線建立的特征庫，也是機器人實時定位的基礎，在此基礎上增加一個動態特征庫，記錄機器人近期一段時間觀測獲得的特征，動態特征庫不斷從機器人當前觀測特征中獲取數據，區別只是此時機器人位置姿態不再通過人工測量，而是實時通過定位算法獲取。動態庫更新策略與第1節人工建圖過程一致，但主要經過特征提取、數據關聯、稀疏性限制等步驟，暫不進行穩健點篩選。

動態特征庫會定期地更新至全局特征庫，更新過程主要經過數據關聯(去除重復點)、稀疏性限制和特征的穩健點篩選步驟，更新完成后即可清除動態特征庫，后續定位時再重新記錄。該更新過程不需要實時進行，可間隔一段時間更新一次，或者室內環境發生明顯變化時再更新，這樣可以保證用來定位的全局特征庫不會發生快速變化，保證定位過程的穩定性，同時也便于在動態特征庫中不斷統計特征點的觀測次數，以便作出正確的穩健點評價后再更新至全局特征庫。

圖11 特征庫定期更新機制Fig.11 The periodically updated strategy of feature-database

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

通過上述特征更新策略，即可維持特征庫數量的平衡，又可定期換入新的特征，從而不斷適應環境的變化，達到長期有效的定位。

4 定位綜合實驗

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

令機器人在實驗室復雜室內環境下進行定位實驗，結果如表3，為了對比前面建圖及定位過程各項策略的效果，表中按照特征庫選取全部特征點、稀疏策略后、穩健點篩選后以及定位過程特征庫定期更

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

圖12 定位實驗圖片Fig.12 The figures of locative experiment

圖13 穩健點篩選后機器人定位實驗誤差曲線Fig.13 The error curves of locative experiment in salient feature-database

5 結論

1)針對移動機器人室內環境的雙目定位問題開展研究，以EKF方法為基礎，但初始特征庫采用人工離線測量的方法，特征庫一旦建立，在線定位時僅對機器人位姿進行卡爾曼濾波估計，而不再維護包含路標位置在內的大型協方差矩陣，克服了EKF-SLAM方法計算量過大和數據關聯脆弱的問題，提高了定位算法的實時性和可靠性。

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

3)在已建立的離線特征庫的基礎上，通過EKF方法實現機器人在線定位，且為了適應室內環境變化，算法可定期記錄圖片庫，并實現特征庫的定期更新，最終算法可保證機器人在室內環境下長期、有效地定位。

4)本方法應用過程需要事先拍攝環境，僅適用于特定環境下的定位，不適用于對未知環境的探索過程。

[1] 權美香, 樸松昊, 李國. 視覺SLAM綜述[J]. 智能系統學報， 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] 顧照鵬, 劉宏. 單目視覺同步定位與地圖創建方法綜述[J]. 智能系統學報, 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] 林睿. 基于圖像特征點的移動機器人立體視覺SLAM研究[D]. 哈爾濱:哈爾濱工業大學, 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]. 哈爾濱: 哈爾濱工程大學，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] 趙鑫. 未知環境下無人駕駛汽車同時定位與地圖創建研究[D]. 成都：西南交通大學, 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]. 計算機與數字工程, 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] 劉建軍. 基于圖像局部不變特征的類屬超圖構建與目標識別技術研究[D]. 長沙:國防科學技術大學, 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.

本文引用格式：

張智， 張磊， 蘇麗， 等. 基于人工離線特征庫的室內機器人雙目定位[J]. 哈爾濱工程大學學報， 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.