基于特征點密度峰值的視覺伺服目標選擇方法

朱宏輝，王嘉豪，朱 軼

(武漢理工大學 物流工程學院，湖北 武漢 430063)

0 引 言

機器人視覺伺服是機器人研究領域的重要方向之一，通過利用圖像信息作為反饋，構成機器人或者機械臂的閉環控制系統[1]。利用無標定視覺伺服技術能夠提高機器人環境適應能力，實現移動機器人在室內環境的實際應用[2-4]。在室內環境下，機器人能夠正確識別和選擇視覺伺服目標是實現機器人視覺伺服的關鍵。

快速進行目標識別是實現移動機器人視覺伺服的前提，采用特征點匹配的方式能夠提高目標識別的速度。目前，常用的特征點匹配算法有：SIFT算法、SURF算法和ORB算法[5-7]，其中ORB算法因運算速度快、抗噪能力強，被廣泛用于各種實時特征檢測和目標識別場景中。通過特征匹配方式識別目標物體后，利用圖像信息作為反饋控制機器人抓取目標物體，從而完成機器人視覺伺服。王博等[8]利用動態擬牛頓算法實現機器人無標定視覺伺服，避免了標定技術對工業機器人視覺伺服系統的影響，提高了機器人視覺伺服系統的魯棒性。梁喜鳳等[9]通過自適應無跡卡爾曼濾波算法估計圖像雅可比矩陣，成功實現了七自由度采摘機械手的視覺伺服。但是這些方法都只能實現對單一目標物體的視覺伺服，而移動機器人應用的實際場景中往往會出現多個同樣目標物體。為此，張璐等[10]通過無標定視覺伺服方法完成采摘機器人的采摘作業，并以果蔬成熟特性作為采摘目標選擇依據，成功實現采摘機器人的選擇性采摘作業，但是卻不適合用于室內移動機器人視覺伺服。

視覺伺服過程中，機器人視野中可能存在多個相同目標物體，機器人容易因無法選擇目標而導致視覺伺服失敗。為此，提出一種基于特征點密度峰值的視覺伺服目標選擇方法，通過該方法機器人能夠選擇唯一視覺伺服目標，提高機器人視覺伺服的成功率。通過ORB算法對所有目標物體進行識別，然后采用特征點密度峰值聚類算法[11]將圖像中不同目標物體特征點進行分離，最后通過位置優先決策方法在多個相同目標物體中選擇一個最佳的抓取目標。最后，通過實驗驗證該方法的正確性和可行性。

1 特征點匹配與分離

1.1 特征提取與匹配

機器人視覺伺服首先需要識別目標物體，采用ORB算法通過特征點提取與匹配能夠實現對目標物體的識別與定位。ORB算法建立在FAST特征點檢測算法和BRIEF特征點描述算法理論基礎上，并針對原算法進行了一些改進及優化。保留了算法的旋轉、尺度不變性，同時極大提高了特征提取與匹配的速度。

ORB算法首先通過FAST算法檢測圖像尺度空間中的特征點，且ORB算法通過灰度質心法為FAST特征點定義了主方向，以增加FAST特征點的旋轉不變性。

為計算特征點方向，ORB定義領域矩，領域矩的定義式見式(1)

(1)

其中，(x,y)∈[-r,r]表示相對于FAST特征點的位置，r表示鄰域半徑；I(x,y)表示該位置的灰度值，鄰域質心計算方法如下

(2)

其中，m00為零階矩，m01和m10為一階矩。

ORB定義質心與特征點連線方向即為FAST特征點的方向，角度計算方法如式(3)

θ=atan2(m01,m10)

(3)

ORB使用BRIEF算法計算特征點的描述子，BRIEF描述子是一種二進制特征描述子。BRIEF在特征點鄰域內隨機選取n對像素點，然后利用一個τ測試比較兩者的灰度值，τ測試定義如式(4)。最后，將所有測試結果形成一個由0和1組成的字符串，也就是特征點的描述子，BRIEF描述子表示方法見式(5)

(4)

(5)

然而，BRIEF描述子同樣不具備旋轉不變性，ORB利用特征點檢測得到的主方向作為描述子的主方向，以增強BRIEF對于旋轉變化的不變性。利用BRIEF中所有點對定義一個2×n的矩陣，如下

(6)

由特征點主方向θ可以得到旋轉矩陣Rθ，由此能夠利用Rθ計算得到改進后的BRIEF特征描述子，計算過程如下

(7)

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

(8)

最后篩選出相關性較低的256個像素點對計算ORB特征描述符。然后，利用Hamming距離等相似性度量方法進行特征點匹配。特征匹配中出現的誤匹配點會降低目標定位精度，同時會導致機器人視覺伺服控制精度降低甚至失敗，為了提高匹配精度，本文采用PROSAC算法剔除誤匹配點[12]，圖1為剔除誤匹配點后的特征點匹配結果。

圖1 剔除誤匹配點后特征匹配結果

1.2 特征點分離

移動機器人視覺伺服過程中出現多個相同目標物體的時候，通過特征匹配方法將識別圖像中所有的目標物體，而機器人視覺伺服需要選擇唯一目標物體作為抓取目標。為此，通過引入特征點密度峰值聚類算法將匹配特征點進行分離，以實現對多個相同目標物體的分離，進而選擇最佳視覺伺服目標。

匹配特征點分離流程如下：

(1)目標物體中心點估計

第一步需要尋找期望圖像上目標物體的中心點，使用期望圖像目標物體的幾何中心點作為模板中心位置，記為C。期望圖像中ORB特征點集合為T，當前圖像上的特征點集合為S，(t,s)表示一個特征匹配對，滿足t∈T且s∈S。期望圖像特征點到中心位置的向量如式(9)所示

ri(ti)=p(C)-p(ti)

(9)

其中，p(C)表示期望圖像上目標物體的中心點位置，p(ti)是期望圖像上第i個特征點的圖像坐標。

(10)

式中：θi=θs-θt，θs是當前圖像上特征點的方向，θt是在期望圖像上對應特征點的方向。

(2)特征點分離

(11)

ρq1≥ρq2≥…≥ρqN

(12)

(13)

圖2 估計中心點分類

由不同中心點集中的估計中心點尋找與之對應的圖像特征點，得到圖像中每一個目標物體的圖像特征點集。如此圖像中不同目標物體上的特征點被分離，特征點分離的結果如圖3所示。

圖3 分離匹配特征點

2 視覺伺服目標選擇

2.1 位置優先目標決策方法

通過特征點密度峰值聚類算法分離圖像中各個目標物體之后，需在多個目標物體之中選擇一個最佳視覺伺服目標。為此，本文提出一種基于位置優先的目標決策方法，利用機器人手眼關系計算各目標物體與機械臂末端的空間位置關系，以各目標物體位置作為視覺伺服目標的選擇依據，為機器人視覺伺服選擇最佳抓取目標。

攝像機模型反映圖像坐標系與笛卡爾空間坐標系之間的關系，利用攝像機模型能夠根據圖像信息計算目標物體的空間位置，從而實現對目標物體的空間定位。攝像機模型本質上是像素坐標系、圖像坐標系、攝像機坐標系和世界坐標系之間的轉換關系，P(Xc,Yc,Zc)表示空間中的一點，其在圖像坐標系的成像點為p(x,y)，f表示攝像機焦距，攝像機坐標系和像素坐標系的轉換關系如式(14)所示

(14)

通過深度相機獲取的RGB-D圖像的深度信息和攝像機模型能夠計算出目標物體在攝像機坐標系中的位置，然后通過機器人手眼關系將目標物體在攝像機坐標系下的位置信息轉化為目標物體在機械臂末端坐標系下的位置信息，從而便于根據目標物體位置信息選擇最佳目標物體。

手眼關系計算原理如圖4所示，將標定板固定在機械臂末端上，控制機械臂運動到不同位置，通過機器人全局攝像機獲取多組圖像。其中，攝像機和標定板之間的外參矩陣Ci由攝像機標定實驗獲取。根據機器人機械臂機械結構，使用D-H參數法構建機械臂連桿坐標系，通過機械臂運動學分析[13,14]得到機械臂基坐標系與機械臂末端坐標系的位姿轉換矩陣Ai。

圖4 手眼關系計算原理

根據攝像機坐標系與機械臂基坐標系相對位姿不變條件可得到式(15)

A1·X·C1=A2·X·C2

(15)

由于標定板與機械臂末端固定在一起，所以式中X為恒定值，經過變換之后可以得到如下關系

(16)

將式(16)按照矩陣形式展開，利用等式左右元素相等，可以得到如下方程

RA·RX=RX·RC(RA-I)·tX=RX·tC-tA

(17)

(18)

僅通過就近原則選擇最佳目標物體無法保證目標選擇的唯一性，特殊情況下場景中可能存在多個目標物體同時被選擇作為抓取對象。當根據距離最近方法同時選擇多個視覺伺服目標時，則表示這些目標物體都處于以機械臂末端為球心的空間球面與桌面相交形成的圓上，此時需要根據輔助決策方法選擇唯一的視覺伺服目標。圖5為機器人結構俯視圖，圖中全局攝像頭位于機器人頂端，機器人前方安裝有置物臺用于臨時放置抓取目標物體。圖中A、B、C表示根據距離就近原則同時選中的目標物體，3個目標物體與機器人機械臂末端的距離相同。通過機械臂正運動學分析與實驗數據分析知，機械臂末端沿YH軸方向的運動由兩個關節控制，沿XH方向的運動由3個關節控制，故相同距離下，物體距離軸YH越近，則機械臂的控制效率越高，所以物體A、C相比物體B更適合作為最佳視覺伺服目標。抓取目標之后，機器人需將物體放置于安裝在機器人上的置物臺上，所以選擇距離置物臺更近的物體A作為最佳視覺伺服目標。通過上述輔助決策方法和距離最近原則可選擇唯一的視覺伺服目標，解決多目標視覺伺服中的目標選擇問題。

圖5 機器人結構俯視圖

2.2 移動機器人視覺伺服

由于目標物體可能處于機器人機械臂抓取范圍之外，所以移動機器人的視覺伺服過程分為兩步。初始的時候，全局攝像機視角范圍大，能夠拍攝到當前視角下所有的目標物體。但是由于距離目標物體較遠，獲取的圖像特征信息受環境影響較大，無法進行精確的視覺伺服控制，因此采用基于位置的視覺伺服方法，以保證不丟失目標。首先，通過全局攝像機獲取圖像，使用視覺伺服目標選擇方法選定抓取目標。然后，根據目標物體與機械臂末端的位置信息計算得到機器人旋轉和移動控制量，從而控制機器人靠近目標物體，并控制機械臂運動到期望位姿，使得目標物體處于局部攝像機視場之中。

當機器人靠近目標物體后，使用機械臂末端的局部攝像機獲取圖像，能夠獲得精確的特征點信息。視覺伺服中圖像雅可比矩陣反映了圖像特征變化與機械臂末端運動之間的映射關系，通過圖像雅可比矩陣能將當前圖像與目標圖像的特征偏差轉化為機械臂的控制量，進而實現視覺伺服。采用基于圖像雅可比矩陣在線估計的無標定視覺伺服方法對機器人機械臂進行運動控制，能夠有效避免攝像機標定和機器人模型的誤差，控制機械臂精確抓取目標物體。無標定視覺伺服方法通過粒子濾波算法對當前時刻的圖像雅可比矩陣進行在線估計，視覺伺服控制器根據當前圖像與期望圖像的特征偏差圖像雅可比矩陣估計值計算得到機械臂各關節控制量。然后，機械臂關節控制器控制機械臂運動，使得圖像特征誤差不斷減小，直到趨近于零，從而控制機械臂運動到期望位姿，實現對目標物體的準確抓取，這一階段視覺伺服控制方法結構如圖6所示。

圖6 無標定視覺伺服方法結構

完整的視覺伺服目標選擇流程如圖7所示，通過ORB特征提取與匹配、誤匹配點剔除、特征點分離和位置優先目標決策等方法，可從圖像中多個相同目標物體中選擇唯一的視覺伺服目標。

圖7 視覺伺服目標選擇流程

3 多目標抓取決策實驗

本文搭建移動機器人作為實驗平臺，如圖8所示。機器人視覺系統包括固定在頭部的全局攝像機和固定在機械臂末端的局部攝像機，全局攝像機為奧比中光AstraPro深度攝像頭，局部攝像機是普通USB攝像頭。機器人基座為兩輪差速驅動的移動平臺，機器人右臂為使用六個數字舵機和一個機械手爪構建的六自由度機械臂，機器人上通過嵌入式計算機運行程序，實現移動機器人視覺伺服。實驗在Windows7平臺下編寫程序，程序開發環境為Visual Studio2013，使用計算機視覺庫OpenCV3.4.1實現目標識別和目標選擇，并借助視覺伺服庫VISP完成機器人視覺伺服控制，最終根據視覺信息完成對靜態目標物體的正確抓取。

圖8 機器人實驗平臺

3.1 目標識別實驗

機器人在不同視點下使用全局攝像機獲取5組圖像，分別使用SIFT、SURF和ORB算法對獲取圖像進行特征點提取與匹配實驗。實驗中，ORB算法特征提取與匹配平均耗時為423 ms，SIFT算法平均耗時約為ORB算法的4.34倍，SURF算法平均耗時約為ORB算法的4倍，實驗數據見表1所示。實驗結果表明使用ORB算法特征提取與匹配耗時遠遠小于SIFT算法和SURF算法，在保證能夠快速、準確識別目標物體的同時，更能夠確保視覺伺服中目標識別的實時性，更適合用于移動機器人視覺伺服。

表1 目標識別算法耗時

3.2 多目標視覺伺服抓取實驗

目標物體與機械臂末端相對位置和目標物體姿態都可能影響視覺伺服目標選擇的決策結果，算法以目標物體與機械臂末端相對位置作為主要決策依據，為驗證目標物體位姿對視覺伺服目標選擇結果正確性的影響進行如下實驗。

實驗中，保證其它物體位置不變的前提下，改變同一位置下的單個目標物體的位姿狀態，使用視覺伺服目標選擇方法進行多次實驗，并通過視覺伺服方法控制機器人抓取目標物體。圖9(a)、圖9(b)中方框選中目標分別為改變右邊目標物體位姿前后通過視覺伺服目標選擇方法選定的目標物體，分別進行3組視覺伺服實驗。獲取圖像中右邊目標物體相比左邊的目標物體距離機器人右機械臂更近，多次實驗中均選擇右邊的目標物體作為最佳視覺伺服目標，結果與實際相符合，驗證目標物體位姿變化不會影響視覺伺服目標選擇方法的正確性。

圖9 不同位姿下目標選擇結果

統計實驗中目標物體與機械臂末端距離和抓取目標物體所耗時間，利用機器人視覺伺服耗時變化可以表示機器人視覺伺服受影響大小，見表2。由實驗結果和表2中數據可知，改變視覺伺服目標前后機器人視覺伺服過程耗時變化較小，即驗證視覺伺服目標位姿對機器人視覺伺服影響較小。

表2 機器人抓取不同位姿下的目標物體耗時

移動機器人在實際場景中抓取目標物體的時候，無法確定機器人與目標物體的初始相對位置，為此改變機器人初始位置進行多次視覺伺服實驗。圖10為實驗中通過視覺伺服目標選擇方法選定的抓取目標，由實驗結果可知，機器人與目標物體初始相對位置對視覺伺服目標選擇結果的正確性無影響，在任意初始位置選擇的視覺伺服目標均為位置最優。表3為實驗中通過算法計算得到的目標物體與機械臂的距離及統計獲取的機器人視覺伺服耗時，由實驗數據可知選定的視覺伺服目標為多個相同目標物體中距離機械臂末端最近的，且距離機械臂越遠，視覺伺服耗時越長。

圖10 視覺伺服目標選擇結果

對比表2和表3中數據可知，目標物體與機械臂末端距離變化相比目標物體擺放位姿對移動機器人視覺伺服耗時影響更大，目標物體位姿對視覺伺服幾乎無影響。實驗結果表明目標物體位姿對位置優先目標決策方法的結果正確性無影響，驗證了視覺伺服目標選擇方法的正確性。

表3 視覺伺服實驗數據

當實際應用場景中出現多個相同目標物體時，無法通過視覺伺服方法控制機械臂正確抓取目標物體。采用基于特征點密度峰值的視覺伺服目標選擇方法選定目標物體后，移動機器人視覺伺服成功率在90%以上。圖11為機器人視覺伺服過程，機器人先靠近目標物體，然后通過無標定視覺伺服方法控制機械臂準確抓取目標。

圖11 移動機器人視覺伺服過程

4 結束語

視覺伺服方法在移動機器人實際應用中，多個相同目標同時出現在機器人視野中會導致機器人視覺伺服失敗。針對移動機器人視覺伺服中出現的目標選擇問題，本文提出了一種基于特征點密度峰值的視覺伺服目標選擇方法，該方法可以在多個相同目標物體中選擇一個最佳視覺伺服目標。為保證機器人視覺伺服實時性要求，通過ORB特征點匹配方法識別圖像中的目標物體，并通過PROSAC算法剔除誤匹配點，從而提高視覺伺服精度。將特征點密度峰值聚類算法引入視覺伺服，成功將圖像中不同目標物體特征點分離，然后通過基于位置優先的目標決策方法選擇最佳視覺伺服目標。通過視覺伺服目標選擇方法有效解決了移動機器人視覺伺服中的目標選擇問題，提高了機器人視覺伺服的成功率。最后，通過實驗驗證了視覺伺服目標選擇方法的正確性，實驗結果表明機器人在任意室內場景下任意位置，本文提出的視覺伺服目標選擇方法均能在當前視場中選擇唯一視覺伺服目標，并且不受目標物體位姿影響。本文研究內容對于機器人視覺伺服在實際中的應用具有一定的價值，能夠提高移動機器人視覺伺服對室內場景的適用性。