邢加偉，田海峰，王 芳

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

0 引言

計(jì)算機(jī)視覺(jué)技術(shù)在目標(biāo)識(shí)別和目標(biāo)位姿求取方面具有強(qiáng)大的優(yōu)勢(shì)。人類(lèi)獲取的信息有70%都是由視覺(jué)獲取。視覺(jué)具有信息量大、精度高、非接觸和響應(yīng)快等特點(diǎn)。

目前，用于工業(yè)流水線(xiàn)的視覺(jué)伺服基本都是基于輪廓和角點(diǎn)等特征在背景單一的結(jié)構(gòu)化環(huán)境下完成檢測(cè)等功能。一旦脫離結(jié)構(gòu)化環(huán)境，在復(fù)雜環(huán)境背景下，輪廓和角點(diǎn)等特征魯棒性會(huì)大幅下降，基本失去作用。例如，工業(yè)裝配流水線(xiàn)的視覺(jué)抓取系統(tǒng)[1],以單目相機(jī)作為傳感器，利用邊緣輪廓的幾何外形作為特征，需要被抓物體離散放置，相鄰物體有間隙，每個(gè)物體放置朝向需要一致。這種解決方案在流水線(xiàn)上效率非常高，但是，其限制條件決定了其只能在工業(yè)流水線(xiàn)等結(jié)構(gòu)化環(huán)境中使用?；谳喞亩ㄎ挥捎谄浔旧淼奶攸c(diǎn)，對(duì)于位姿的確定存在一定的缺失，通常不能完全確定6個(gè)自由度的信息。

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

1 算法原理

1.1 算法總述

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

1.2 SIFT特征點(diǎn)

SIFT[7]全稱(chēng)為尺度不變特征轉(zhuǎn)換 。它用來(lái)偵測(cè)與描述影像中的局部性特征,具有旋轉(zhuǎn)不變性(拍攝角度)、尺度不變性(拍攝距離)等優(yōu)勢(shì)，而且對(duì)于視角傾斜、光照、噪音影響，目標(biāo)遮擋和光照變化具有一定的魯棒性。通過(guò)旋轉(zhuǎn)照相機(jī)拍攝同一物體不同角度的多張圖像時(shí)，這些圖像的對(duì)應(yīng)點(diǎn)的SIFT描述符具有很高的相似度?；谶@些特性，因?yàn)樗鼈兪歉叨蕊@著而且相對(duì)容易擷取，在母數(shù)龐大的特征數(shù)據(jù)庫(kù)中，很容易辨識(shí)物體而且鮮有誤認(rèn)。經(jīng)過(guò)優(yōu)化和GPU加速的SIFT特征點(diǎn)提取匹配可以達(dá)到實(shí)時(shí)的效果。而且擴(kuò)展性好，可以方便地與其他的特征進(jìn)行聯(lián)合，例如ORB特征[8]、SURF特征[9]、Harris角點(diǎn)[10]等。

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

1)尺度空間極值檢測(cè)

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

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

2)關(guān)鍵點(diǎn)定位

因?yàn)槠湓诮鹱炙械淖鴺?biāo)為離散值，可以通過(guò)泰勒二階展開(kāi)三維函數(shù)得到精細(xì)化的(x，y,δ)即為SIFT特征點(diǎn)的坐標(biāo)，其中x、y為在圖像中的位置，δ為當(dāng)前所在的尺度值。因?yàn)閷O值點(diǎn)所在的尺度值δ作為變量求出，所以具有尺度不變性。

3)方向確定

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

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為關(guān)鍵點(diǎn)所在尺度空間的灰度分布，m(x,y)為梯度模值，θ(x,y)為梯度的方向。使用直方圖統(tǒng)計(jì)鄰域內(nèi)像素的梯度和方向。梯度直方圖將0°～360°的方向范圍分為36個(gè)柱(bins)，其中每個(gè)柱10°，每個(gè)柱高度為這個(gè)方向的模值之和。直方圖的峰值方向代表了關(guān)鍵點(diǎn)的主方向。

4)關(guān)鍵點(diǎn)描述

以主方向?yàn)閤軸，建立坐標(biāo)系，在其尺度空間上將其鄰域劃分為4×4個(gè)小區(qū)域。每個(gè)小區(qū)域內(nèi)若干像素，統(tǒng)計(jì)每個(gè)像素的梯度，將360°分為8個(gè)方向，統(tǒng)計(jì)每個(gè)方向梯度值之和做分布直方圖，可得到一個(gè)4×4×8=128維的向量，就是此處SIFT點(diǎn)的描述子。

5)SIFT特征點(diǎn)匹配

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

1.3 PNP算法

PNP是求解3D到2D點(diǎn)對(duì)運(yùn)動(dòng)的方法，描述了當(dāng)知道n個(gè)3D空間點(diǎn)及其2D投影位置時(shí)，如何估計(jì)相機(jī)和空間的相對(duì)位姿。

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

其中，DLT最少需要6對(duì)特征點(diǎn)可實(shí)現(xiàn)變換矩陣的求解。但是將變換矩陣看成了12個(gè)未知數(shù)，忽略了他們之間的聯(lián)系，因?yàn)樾D(zhuǎn)矩陣R∈SO(3)，用直接線(xiàn)性變換求出來(lái)的解未必滿(mǎn)足該約束條件。

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

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

DLS算法的主要思想是對(duì)方向向量而不是位置向量進(jìn)行最小二乘求解，同時(shí)加入了噪聲的考慮。對(duì)比EPNP算法，DLS算法在匹配點(diǎn)對(duì)較少的情況下表現(xiàn)更優(yōu)。

綜上所述，因?yàn)槠ヅ潼c(diǎn)通常遠(yuǎn)遠(yuǎn)大于4對(duì)，但偶爾存在匹配較少的情況，所以本文選擇DLS算法。

1.4 隨機(jī)抽樣一致性算法

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

RANSAC的基本假設(shè)是：

1)數(shù)據(jù)由局內(nèi)點(diǎn)組成，例如：數(shù)據(jù)的分布可以用一些模型參數(shù)來(lái)解釋?zhuān)?/p>

2)局外點(diǎn)是不能適應(yīng)該模型的數(shù)據(jù)；

3)除此之外的數(shù)據(jù)屬于噪聲。

其中局外點(diǎn)產(chǎn)生的原因有：噪聲的極值、錯(cuò)誤的測(cè)量方法、對(duì)數(shù)據(jù)的錯(cuò)誤假設(shè)等。

PNP算法與RAMSAC算法結(jié)合的工作流程為：

1)隨機(jī)從樣本中選擇5個(gè)點(diǎn)對(duì)，利用PNP算法算出外參數(shù)矩陣。

2)用1)中的外參數(shù)矩陣去測(cè)試樣本中的其他點(diǎn)對(duì)，即用3D點(diǎn)經(jīng)過(guò)外參矩陣計(jì)算投影出的2D坐標(biāo)和實(shí)際的2D坐標(biāo)比較，根據(jù)其是否小于閾值，確定點(diǎn)對(duì)為局內(nèi)點(diǎn)還是局外點(diǎn)。

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

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

2 位姿算法實(shí)現(xiàn)

2.1 建立模型

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

O=log2min(M,N)-2

(3)

其中，M、N分別為圖片在橫向和縱向2個(gè)方向的像素?cái)?shù)。

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

(4)

將每一幀的左相機(jī)拍攝的圖片進(jìn)行SIFT特征點(diǎn)提取，因?yàn)槊恳粠瑘D片能夠提取的SIFT特征點(diǎn)均為多個(gè)，所以把這些特征點(diǎn)存入堆棧之中，提取的描述子和坐標(biāo)轉(zhuǎn)換后的坐標(biāo)同樣存入堆棧之中，最后存入模型庫(kù)。對(duì)應(yīng)轉(zhuǎn)動(dòng)40次，模型庫(kù)共包括40包數(shù)據(jù)，分別為data1，data2，…，data40。每包數(shù)據(jù)主要包括SIFT特征點(diǎn)堆棧(vector)、SIFT描述子堆棧(vector)和特征點(diǎn)在物體坐標(biāo)系中的坐標(biāo)堆棧(vector)這3個(gè)數(shù)組。至此，模型建立完畢。

2.2 與模型匹配

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

2.3 求取位姿

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

3 實(shí)驗(yàn)

3.1 算法實(shí)現(xiàn)平臺(tái)

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

圖2 試驗(yàn)驗(yàn)證平臺(tái)Fig.2 Experiment platform

軟件平臺(tái)為Ubuntu16.04下QT開(kāi)發(fā)環(huán)境。利用了Eigen數(shù)學(xué)計(jì)算庫(kù)、OpenCV圖形處理庫(kù)、BumbleBee自帶的triclops雙目視覺(jué)庫(kù)。

受實(shí)驗(yàn)條件限制，初始化對(duì)齊過(guò)程不可避免存在誤差，為了最大限度消除誤差，測(cè)量其3個(gè)自由度上的旋轉(zhuǎn)幅值來(lái)計(jì)算其精確度。每次繞一個(gè)軸旋轉(zhuǎn)10°，通過(guò)單目視覺(jué)位姿算法測(cè)量其轉(zhuǎn)動(dòng)角度，相減得到其誤差值。

3.2 實(shí)驗(yàn)設(shè)計(jì)

通過(guò)實(shí)驗(yàn)確定如下參數(shù)：

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

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

同時(shí)，在傳統(tǒng)SIFT特征點(diǎn)參數(shù)之外，將SIFT提取時(shí)高斯金字塔的層數(shù)增加到16，初始高斯模糊參數(shù)選擇為1.3，查看實(shí)驗(yàn)效果。最后，針對(duì)當(dāng)前深度學(xué)習(xí)用于目標(biāo)檢測(cè)效果不斷提升的現(xiàn)狀，通過(guò)人工添加目標(biāo)檢測(cè)框，模擬位姿估計(jì)算法和目標(biāo)檢測(cè)算法結(jié)合使用時(shí)的效果。

3.3 實(shí)驗(yàn)結(jié)果

在3個(gè)自由度上的誤差如圖3～圖5所示，其中，X軸為RANSAC的偏差閾值，Y軸為旋轉(zhuǎn)10°時(shí)，實(shí)際測(cè)得的轉(zhuǎn)動(dòng)角度和10°的差值。

1)繞X軸旋轉(zhuǎn)結(jié)果

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

圖3 繞X軸旋轉(zhuǎn)結(jié)果圖Fig.3 Result of rotate around the X axis

2)繞Y軸旋轉(zhuǎn)結(jié)果：

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

3)繞Z軸旋轉(zhuǎn)結(jié)果：

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

圖4 繞Y軸旋轉(zhuǎn)效果圖Fig.4 Result of rotate around the Y axis

圖5 繞Z軸旋轉(zhuǎn)效果圖Fig.5 Result of rotate around the Z axis

3.4 實(shí)驗(yàn)分析

實(shí)驗(yàn)結(jié)果表明:

1)RANSAC偏差閾值越大，允許的誤差就越大，不穩(wěn)定性越高，絕大多數(shù)情況下，允許偏差在到達(dá)一定值之后，都會(huì)使得位姿估計(jì)出現(xiàn)錯(cuò)誤。

2)輸入PNP算法中的SIFT點(diǎn)對(duì)選擇倍數(shù)n，過(guò)小則被誤舍棄的正確匹配點(diǎn)對(duì)過(guò)多，導(dǎo)致不穩(wěn)定。

3)提高SIFT特征點(diǎn)的高斯金字塔層數(shù)可以明顯增多SIFT特征點(diǎn)的數(shù)量，提高算法的穩(wěn)定性、實(shí)時(shí)性和精度。

4)偏差閾值選擇2～3之間，SIFT點(diǎn)選擇倍數(shù)在3～3.5之間，在標(biāo)準(zhǔn)高斯金字塔的基礎(chǔ)上將層數(shù)翻倍，可以取得最好的效果。

4 結(jié)論

本文提出了一種基于SIFT特征匹配和PNP算法的單目位姿估計(jì)方法，并進(jìn)行了實(shí)驗(yàn)驗(yàn)證和參數(shù)確定。結(jié)果表明:

1)該算法可以通過(guò)選擇合適的參數(shù)，在穩(wěn)定性、實(shí)時(shí)性和精度方面滿(mǎn)足機(jī)器人抓取的需求，拓寬了機(jī)器人的應(yīng)用范圍。

2)根據(jù)不同的任務(wù)需求，可以在穩(wěn)定性、實(shí)時(shí)性和精確度方面進(jìn)行取舍，本文的實(shí)驗(yàn)結(jié)果可以作為參考。

3)本文還可以作為點(diǎn)云粗配準(zhǔn)的一種方案，配合點(diǎn)云精配準(zhǔn)取得更好的結(jié)果。