單目相機物體位姿估計方法研究

邢加偉，田海峰，王 芳

(航天科工智能機器人有限責任公司，北京100074)

0 引言

計算機視覺技術在目標識別和目標位姿求取方面具有強大的優勢。人類獲取的信息有70%都是由視覺獲取。視覺具有信息量大、精度高、非接觸和響應快等特點。

目前，用于工業流水線的視覺伺服基本都是基于輪廓和角點等特征在背景單一的結構化環境下完成檢測等功能。一旦脫離結構化環境，在復雜環境背景下，輪廓和角點等特征魯棒性會大幅下降，基本失去作用。例如，工業裝配流水線的視覺抓取系統[1],以單目相機作為傳感器，利用邊緣輪廓的幾何外形作為特征，需要被抓物體離散放置，相鄰物體有間隙，每個物體放置朝向需要一致。這種解決方案在流水線上效率非常高，但是，其限制條件決定了其只能在工業流水線等結構化環境中使用。基于輪廓的定位由于其本身的特點，對于位姿的確定存在一定的缺失，通常不能完全確定6個自由度的信息。

對于應用場景為復雜環境下的機器人，例如特種機器人、排爆機器人和服務機器人等，技術仍未成熟。不同于工業機器人在結構化環境下對工件的抓取，排爆機器人等在復雜環境下的智能抓取面臨著諸多挑戰，例如動態化環境、光照變化、幾十乃至上百種目標物體、復雜背景、物體間的相互遮擋等。對于機器人按照用戶命令抓取指定物體的應用情境，一個完整的抓取過程一般包括物體識別、位姿計算、抓取姿態生成、運動規劃與執行等多個環節。其中，對目標物體的識別與定位是抓取成功的前提條件。本文針對抓取中的核心問題——位姿確定，提出了一種基于特征點匹配和透視n點定位(Perspective-n-point, PNP)技術的方法,可以使機器人適合非結構化環境，拓展其任務范圍。特征點匹配在視覺導航領域已經廣泛應用[2-5]，證明了其可靠性與實用性。對于表面存在紋理的物體，不論形狀規則與否，在利用深度相機或者雙目相機進行建模后，對消費級相機進行張正友標定[6]，即可進行位姿估計，魯棒性和實時性均滿足物體抓取的需求。

1 算法原理

1.1 算法總述

本文提出的位姿識別算法，需要在物體上固連一個假想的坐標系(下文簡稱物體坐標系)，通過圍繞物體掃描一周，將掃描得到的物體表面進行尺度不變特征變換(Scale-Invariant Feature Transform，SIFT)特征點提取，利用雙目相機計算出特征點在相機坐標系下的坐標，并根據相機在掃描物體時相對于物體的位姿，將特征點在相機坐標系下的坐標轉換為其在物體坐標系下的坐標,得到不同視角下物體可視面的SIFT特征點稀疏點云(后文簡稱模型點云)。在算法進行位姿估計時，拍攝一張物體照片，找到與其最接近的視角下的模型點云。通過SIFT特征匹配，得到相對應的物體坐標系3D坐標隊列和圖像坐標系2D坐標隊列。利用PNP算法和RANSAC算法，求解出物體坐標系相對于相機坐標系的位姿。

1.2 SIFT特征點

SIFT[7]全稱為尺度不變特征轉換 。它用來偵測與描述影像中的局部性特征,具有旋轉不變性(拍攝角度)、尺度不變性(拍攝距離)等優勢，而且對于視角傾斜、光照、噪音影響，目標遮擋和光照變化具有一定的魯棒性。通過旋轉照相機拍攝同一物體不同角度的多張圖像時，這些圖像的對應點的SIFT描述符具有很高的相似度。基于這些特性，因為它們是高度顯著而且相對容易擷取，在母數龐大的特征數據庫中，很容易辨識物體而且鮮有誤認。經過優化和GPU加速的SIFT特征點提取匹配可以達到實時的效果。而且擴展性好，可以方便地與其他的特征進行聯合，例如ORB特征[8]、SURF特征[9]、Harris角點[10]等。

SIFT使用流程分解為如下五步：

1)尺度空間極值檢測

首先構建差分高斯金字塔(圖1)，高斯差分金字塔由高斯金字塔差分獲得，高斯金字塔模型是將原始圖像不斷降階采樣和高斯濾波，得到一系列大小不一的圖像，由大到小、從下到上構成的塔狀模型。原圖像為金子塔的第一層，每次降采樣所得到的新圖像為金字塔的一層，每層的一張圖像使用不同參數做高斯模糊，使得金字塔的每層含有多張高斯模糊圖像。將金字塔每層多張圖像合稱為一組(Octave)，金字塔每層只有一組圖像，組數和金字塔層數相等。金字塔的層數根據圖像的原始大小和塔頂圖像的大小共同決定。最后，通過搜索差分金字塔，將每一個點與其上下左右共26個點作比較，找到極值點。

圖1 高斯差分金字塔示意圖Fig.1 Gaussian difference pyramid diagram

2)關鍵點定位

因為其在金字塔中的坐標為離散值，可以通過泰勒二階展開三維函數得到精細化的(x，y,δ)即為SIFT特征點的坐標，其中x、y為在圖像中的位置，δ為當前所在的尺度值。因為將極值點所在的尺度值δ作為變量求出，所以具有尺度不變性。

3)方向確定

為了使描述符具有旋轉不變性，需要利用圖像的局部特征給每一個關鍵點分配一個基準方向。采集其所在高斯金字塔圖像3δ鄰域窗口內像素的梯度和方向分布特征。

m(x,y)=

(1)

θ(x,y)=tan-1((L(x,y+1)-L(x,y-1))/

(L(x+1,y)-L(x-1,y)))

(2)

其中，L為關鍵點所在尺度空間的灰度分布，m(x,y)為梯度模值，θ(x,y)為梯度的方向。使用直方圖統計鄰域內像素的梯度和方向。梯度直方圖將0°～360°的方向范圍分為36個柱(bins)，其中每個柱10°，每個柱高度為這個方向的模值之和。直方圖的峰值方向代表了關鍵點的主方向。

4)關鍵點描述

以主方向為x軸，建立坐標系，在其尺度空間上將其鄰域劃分為4×4個小區域。每個小區域內若干像素，統計每個像素的梯度，將360°分為8個方向，統計每個方向梯度值之和做分布直方圖，可得到一個4×4×8=128維的向量，就是此處SIFT點的描述子。

5)SIFT特征點匹配

2個SIFT特征點描述隊列，對于第一個隊列中的某一個特征點，在第二個隊列中找到與其歐氏距離最近的2個SIFT特征點，如果最近的點距離/次近的點的距離小于某個比例值(通常取0.4～0.6)，則認為最近的那個點是匹配點。

1.3 PNP算法

PNP是求解3D到2D點對運動的方法，描述了當知道n個3D空間點及其2D投影位置時，如何估計相機和空間的相對位姿。

PNP問題有很多種求解方法，例如，只有3對點估計位姿的P3P[11]、直接線性變換(Direct Linear Transformation，DLT)、 高效PNP[12](Efficient PNP，EPNP)、直接最小二乘法[13](Direct Least-Squares，DLS)等。

其中，DLT最少需要6對特征點可實現變換矩陣的求解。但是將變換矩陣看成了12個未知數，忽略了他們之間的聯系，因為旋轉矩陣R∈SO(3)，用直接線性變換求出來的解未必滿足該約束條件。

P3P只需要3對匹配點即可求解出相對位姿，但是匹配點通常多于3對，P3P并不能有效利用，而且P3P算法當受到噪聲影響時算法失效，魯棒性較差。

EPNP算法的主要思想是選取世界坐標下的4個控制點坐標為Cw=[0,0,0,1]T, [1,0,0,1]T, [0,1,0,1]T, [0,0,1,1]T；通過n個3D點在相機平面的投影關系，以及與這4個控制點的權重關系，構建一個12×12方陣，求得其零空間特征向量，可以得到虛擬控制點的相機平面坐標，然后使用POSIT算法即可求出相機位姿。

DLS算法的主要思想是對方向向量而不是位置向量進行最小二乘求解，同時加入了噪聲的考慮。對比EPNP算法，DLS算法在匹配點對較少的情況下表現更優。

綜上所述，因為匹配點通常遠遠大于4對，但偶爾存在匹配較少的情況，所以本文選擇DLS算法。

1.4 隨機抽樣一致性算法

隨機抽樣檢驗一致性[14](Random Sample Consensus，RANSAC)算法，可以從一組包含局外點的觀測數據集中，通過迭代方式估計數學模型的參數。它是一種不確定的算法——它有一定的概率得出一個合理的結果；為了提高概率必須提高迭代次數。

RANSAC的基本假設是：

1)數據由局內點組成，例如：數據的分布可以用一些模型參數來解釋；

2)局外點是不能適應該模型的數據；

3)除此之外的數據屬于噪聲。

其中局外點產生的原因有：噪聲的極值、錯誤的測量方法、對數據的錯誤假設等。

PNP算法與RAMSAC算法結合的工作流程為：

1)隨機從樣本中選擇5個點對，利用PNP算法算出外參數矩陣。

2)用1)中的外參數矩陣去測試樣本中的其他點對，即用3D點經過外參矩陣計算投影出的2D坐標和實際的2D坐標比較，根據其是否小于閾值，確定點對為局內點還是局外點。

3)如果局內點足夠多，并且局內點多于原有最佳外參矩陣的局內點，那么將這次迭代的外參矩陣設為最佳外參矩陣。

4)重復1)～3)步直到找到最佳外參矩陣。

2 位姿算法實現

2.1 建立模型

建立模型時，為了使物體在雙目相機視場的重疊面積盡可能大而且便于計算，首先將物體坐標系和雙目左相機坐標系重合，將物體坐標系原點沿物體坐標系X軸正方向平移到雙目相機基線(雙目相機2個光心距連線)中點，平移距離為b/2；再沿物體坐標系Z軸正方向移動到照片相機可以拍攝到物體全貌,距離記為d。為了體現算法的通用性，模型選擇為普通食品盒。通過將物體沿物體坐標系Y軸旋轉拍攝來模擬相機繞物體旋轉拍攝，并利用雙目相機拍攝物體，得到左右2張圖片。若每次旋轉弧度過大，則SIFT特征點匹配成功率會嚴重下降;若每次旋轉弧度過小，則需要拍攝的圖片數量過多，增加計算量，影響實時性。因此，經過實驗，選擇每次正向轉動π/20弧度，共拍攝40張照片。對每張照片加入包圍盒，以僅利用照片中物體表面圖像的SIFT特征點，將其他的特征點舍棄。將左右2張圖片進行SIFT特征點提取并計算其描述子。SIFT提取時，選擇層數根據如下公式

O=log2min(M,N)-2

(3)

其中，M、N分別為圖片在橫向和縱向2個方向的像素數。

攝像機輸出的圖像分辨率為1024×768，即M=1024，N=768，計算得到層數應為7.5，四舍五入選擇8層。高斯模糊系數初始值取1.6，其余參數采用OpenCV默認值。匹配2幅圖中SIFT特征點，將匹配的點對利用雙目視差法[15]算出特征點在雙目相機左相機坐標系下的坐標。根據雙目相機左相機坐標系和物體坐標系的位姿關系，將SIFT特征點坐標從雙目相機左相機坐標系轉換到物體坐標系下，轉換公式如下

(4)

將每一幀的左相機拍攝的圖片進行SIFT特征點提取，因為每一幀圖片能夠提取的SIFT特征點均為多個，所以把這些特征點存入堆棧之中，提取的描述子和坐標轉換后的坐標同樣存入堆棧之中，最后存入模型庫。對應轉動40次，模型庫共包括40包數據，分別為data1，data2，…，data40。每包數據主要包括SIFT特征點堆棧(vector)、SIFT描述子堆棧(vector)和特征點在物體坐標系中的坐標堆棧(vector)這3個數組。至此，模型建立完畢。

2.2 與模型匹配

拍攝一張包含物體的照片(下文中簡稱實驗照片)并測量此時相機相對于物體的位姿，對實驗照片提取SIFT特征點和計算描述子，并與模型庫中每包數據進行特征點匹配。因為每個特征點的描述子為一個128維向量，所以每一個匹配點對的暴力匹配的結果為一個描述子的點乘，即一個歐氏距離。每一個匹配結果為一個歐式距離(distance)堆棧(vector<>)，共得到40個匹配結果即vector1, vector2，…，vector40。根據實驗，即使待測照片和模型照片重合度非常高，也會出現誤匹配的情況，因為匹配對歐氏距離越大，誤匹配幾率越大。為了增強算法實時性，消除誤匹配的干擾，因此，將每個匹配結果的歐式距離堆棧從小到大排列，得到40個經過排序的歐式距離序列,取其前80%以盡可能消除誤匹配的影響，并計算剩余歐氏距離的平均值，平均值最小的模型包就是匹配最好的模型包。

2.3 求取位姿

為了提高實時性，PNP解算不利用全部的匹配點，而是利用3.2節中的排序結果，求取匹配最好的模型包對應的歐氏距離序列的最小值distancemin，將大于n*distancemin的進行舍棄，選擇倍數n通過實驗確定。利用PNP算法和RANSAC算法進行誤匹配濾除和位姿解算，RANSAC算法迭代次數選擇為20000(只為了得到最好結果，實際工作遠遠小于這個迭代次數就會得到最佳效果)，通過實驗確定數據是否適用于模型的閾值。算出模型本體坐標系相對于相機坐標系的旋轉向量和平移向量。

3 實驗

3.1 算法實現平臺

本文所采用的視覺硬件為加拿大PointGray公司的BumbleBee BB2雙目相機，雙目僅在建立模型時使用，在位姿確定時僅用左鏡頭圖像。實驗平臺(圖2)搭建在普通實驗室中，光照條件變化較大，可以測試系統對光照的魯棒性。計算設備采用第四代英特爾i7處理器，16G內存。模型搭載平臺為三軸旋轉平臺。

圖2 試驗驗證平臺Fig.2 Experiment platform

軟件平臺為Ubuntu16.04下QT開發環境。利用了Eigen數學計算庫、OpenCV圖形處理庫、BumbleBee自帶的triclops雙目視覺庫。

受實驗條件限制，初始化對齊過程不可避免存在誤差，為了最大限度消除誤差，測量其3個自由度上的旋轉幅值來計算其精確度。每次繞一個軸旋轉10°，通過單目視覺位姿算法測量其轉動角度，相減得到其誤差值。

3.2 實驗設計

通過實驗確定如下參數：

1)輸入PNP的特征點選擇倍數n，通常在2～4之間，本次試驗分別驗證2,2.5,3,3.5的效果；

2)RANSAC算法中數據是否符合模型的偏差閾值，選擇0.5～8之間，分辨率為0.1。

同時，在傳統SIFT特征點參數之外，將SIFT提取時高斯金字塔的層數增加到16，初始高斯模糊參數選擇為1.3，查看實驗效果。最后，針對當前深度學習用于目標檢測效果不斷提升的現狀，通過人工添加目標檢測框，模擬位姿估計算法和目標檢測算法結合使用時的效果。

3.3 實驗結果

在3個自由度上的誤差如圖3～圖5所示，其中，X軸為RANSAC的偏差閾值，Y軸為旋轉10°時，實際測得的轉動角度和10°的差值。

1)繞X軸旋轉結果

如圖3所示，上圖為8層高斯金字塔效果，下圖為16層高斯金字塔效果。

圖3 繞X軸旋轉結果圖Fig.3 Result of rotate around the X axis

2)繞Y軸旋轉結果：

如圖4所示，上圖為8層高斯金字塔效果，下圖為16層高斯金字塔效果。

3)繞Z軸旋轉結果：

如圖5所示，上圖為8層高斯金字塔效果，下圖為16層高斯金字塔效果。

圖4 繞Y軸旋轉效果圖Fig.4 Result of rotate around the Y axis

圖5 繞Z軸旋轉效果圖Fig.5 Result of rotate around the Z axis

3.4 實驗分析

實驗結果表明:

1)RANSAC偏差閾值越大，允許的誤差就越大，不穩定性越高，絕大多數情況下，允許偏差在到達一定值之后，都會使得位姿估計出現錯誤。

2)輸入PNP算法中的SIFT點對選擇倍數n，過小則被誤舍棄的正確匹配點對過多，導致不穩定。

3)提高SIFT特征點的高斯金字塔層數可以明顯增多SIFT特征點的數量，提高算法的穩定性、實時性和精度。

4)偏差閾值選擇2～3之間，SIFT點選擇倍數在3～3.5之間，在標準高斯金字塔的基礎上將層數翻倍，可以取得最好的效果。

4 結論

本文提出了一種基于SIFT特征匹配和PNP算法的單目位姿估計方法，并進行了實驗驗證和參數確定。結果表明:

1)該算法可以通過選擇合適的參數，在穩定性、實時性和精度方面滿足機器人抓取的需求，拓寬了機器人的應用范圍。

2)根據不同的任務需求，可以在穩定性、實時性和精確度方面進行取舍，本文的實驗結果可以作為參考。

3)本文還可以作為點云粗配準的一種方案，配合點云精配準取得更好的結果。