改進PROSAC算法的并聯機器人末端位姿檢測方法

高國琴 韓 瀅

(江蘇大學電氣信息工程學院 江蘇 鎮江 212013)

0 引 言

隨著現代農業的發展，機器人自動分揀水果成為發展趨勢。并聯機構具有結構穩定、精度高、動態性能好等優點，滿足機器人自動分揀水果對穩定性的要求。基于此，研制一種水果分揀并聯機器人。當并聯機器人自動分揀水果時，實現對末端執行器位置和姿態的實時檢測是成功抓取并分揀水果的前提。同時，水果分揀并聯機器人末端執行器的位姿反映了機器人的運動狀態，是分析機器人末端執行器動態特性以及進一步實現并聯機器人實時閉環控制的重要參數[1]。

目前，并聯機器人末端位姿檢測方法可分為接觸式檢測和非接觸式檢測。非接觸式檢測方法可避免測量力對機器人的受力干擾，適用于末端位姿靜態和動態檢測。非接觸式檢測包括超聲檢測、激光檢測、機器視覺檢測等。基于機器視覺的末端位姿檢測方法相對于其他方法具有非接觸、適用性強、高性價比等優點，尤其適用于具有多運動自由度、運動軌跡復雜和難以直接檢測的并聯機器人末端位姿檢測[2]。機器視覺檢測方法可分為單目、雙目和多目。與單目相比，雙目視覺無須添加幾何約束，可獲取視野范圍內任意一點的三維信息，為機器人的運動控制提供更準確的末端位姿參數[3]；與多目相比，雙目視覺所要求匹配的圖像信息少，減小了立體匹配難度，在保證一定檢測準確率的條件下具有更快的檢測速度[4-5]。因此，本文研究水果分揀并聯機器人的雙目視覺末端位姿檢測方法。

在雙目視覺末端位姿檢測過程中，需要對相機采集到的一對圖像進行立體匹配，并將立體匹配結果代入雙目視覺模型求出末端位姿，故而立體匹配的準確率和實時性是直接影響雙目視覺末端位姿檢測精度和實時性的主要因素[6]。采用雙目視覺對水果分揀并聯機器人進行末端位姿檢測時，光照、噪聲干擾等不確定因素造成立體匹配中包含較多的錯誤匹配，因此，為提高末端位姿檢測的精度，需要對立體匹配結果進行提純。

現有提純算法包括RANSAC算法和PROSAC算法[7-8]。PROSAC算法的魯棒性優于RANSAC算法，主要原因是RANSAC算法將所有采樣點列為相同等級，而PROSAC算法首先對采樣點進行排序，然后從較高等級的采樣點進行迭代，再通過假設和驗證獲得最優解[9]。該算法提出后得到了廣泛應用，有研究結合ORB和PROSAC算法進行實時全景圖像拼接[10]；還有研究提出一種結合極線幾何約束的PROSAC算法以提高立體匹配的準確率[11]。但上述方法應用于水果分揀并聯機器人雙目視覺末端位姿檢測時，不能保證選取采樣點的分散性導致模型參數估計易陷入局部極值，從而模型參數估計精度不高；同時該方法在得到候選模型后將其余所有采樣點檢驗一遍，會耗費時間驗證錯誤候選模型，從而末端位姿檢測的實時性降低?？紤]到水果分揀并聯機器人末端位姿檢測對精度和實時性要求較高，本文通過穿插取點和預檢驗候選模型的方法對PROSAC算法進行改進，以提高模型參數估計的精度和速度，并進一步將改進的PROSAC算法應用于末端位姿檢測中的立體匹配提純，兼顧立體匹配的實時性和準確率，進而提高水果分揀并聯機器人雙目視覺末端位姿的檢測精度。

1 改進的PROSAC算法設計

根據PROSAC基本思想，首先按Hamming距離相似度對匹配點對進行排序，然后抽樣計算兩幅圖像中匹配點對的目標模型H，最后利用目標模型驗證剩余匹配點對并去除誤匹配點對。

假設匹配點對的數量為N，由于Hamming距離相似度越高則匹配點對的質量越高，故而將Hamming距離相似度作為匹配質量的定量表示[12]。將匹配點對按Hamming距離相似度降序排列后的集合定義為UN，從UN中抽取n組較高質量的匹配點對組成集合序列M，通過采樣計算得到的候選模型為F，設置內點誤差閾值W、最大內點數N和最大迭代次數K。算法具體步驟如下[13]：

(1) 按照Hamming距離相似度對匹配點對做降序排列，得到匹配對集UN。

(2) 從UN中選取前n組較高質量的匹配點對，組成匹配對子集M。

(3) 從M中隨機選取m組匹配點對組成樣本集，求解候選模型F：設(x,y,1)和(x′,y′,1)分別為立體圖像對中一對正確匹配點對a(x,y)和a′(x′,y′)的齊次坐標，將m組匹配點對代入式(1)求解F矩陣參數[14]。

(1) (4) 通過候選模型F和誤差閾值W檢測剩余匹配點對，得到滿足候選模型F的匹配點對和滿足候選模型F的匹配點對數(內點數d)。

(5) 根據終止條件判斷是否結束迭代過程，否則重復步驟(2)-步驟(5)。終止條件：當內點數超過所設定的最大內點數或者k次采樣后的內點數與k-1次采樣后的內點數相比沒有增加。

針對未改進的PROSAC算法應用于水果分揀并聯機器人雙目視覺末端位姿檢測時存在的模型參數估計精度不高以及驗證錯誤候選模型耗時問題，通過穿插取點和預檢驗候選模型的方法對PROSAC算法進行改進，以提高模型參數估計的精度和速度。首先為解決隨機采樣生成的樣本集點間距很近導致模型參數估計精度不高的問題，將排序后得到的高質量匹配對集M等分成t(t>m)份，從中抽取m+1份并從每份中抽取一組匹配點對構成樣本集S，然后從樣本集S中抽取m組匹配點對求出候選模型F。為解決驗證錯誤候選模型耗時的問題，在得到候選模型F后驗證第m+1組匹配點對是否滿足該模型，若是，則繼續尋找滿足候選模型F的匹配點對，否則重新選取樣本集M并抽樣計算候選模型F。

改進PROSAC算法的流程如圖1所示，其中：最大迭代次數K=lg(1-Pc)/lg(1-em)；Pc為置信率，一般取值為0.95；e為抽樣后計算得到的內點概率且每次迭代后進行更新，設置初始值為0.5，即假設所有匹配點對中約有一半為正確匹配點對；m為抽樣個數，取值為8；內點誤差閾值取值為5。

圖1 改進的PROSAC算法流程

當內點數超過所設定的最大內點數或者k次采樣后的內點數與k-1次采樣后的內點數相比沒有增加時，返回包含內點數最多的候選模型參數矩陣F以及滿足該模型的匹配對集。最后利用匹配對集內所有匹配點對通過最小二乘法進一步優化候選模型參數矩陣F得到目標模型參數矩陣H。

2 水果分揀并聯機器人末端位姿檢測

將所提出的改進PROSAC算法應用于本課題組研制的新型4-DOF水果分揀并聯機器人末端位姿檢測，以驗證其正確性和有效性。

圖2 水果分揀并聯機器人

水果分揀并聯機器人機構原型如圖2所示，該機構通過四組支鏈連接動、靜平臺，支鏈的主動臂和靜平臺上的減速器由漲緊套固定連接，通過伺服電機實現主動臂的轉動，進而實現末端執行器的三維空間平動和繞豎直軸的轉動[15]。

本文的研究對象是水果分揀并聯機器人末端執行器，在實際工業測量中，復雜的圖像背景不利于直接定位末端執行器的中心點。為減少工業環境對末端位姿檢測的影響，在該機構末端執行器的正上方安裝點陣圓形標定板，利用點陣圓形標定板的漫反射特性提高雙目視覺成像中特征點與背景圖像的灰度對比度，從而便于識別和提取特征點，適用于對實時性要求高的并聯機器人末端位姿檢測。因此，本文將并聯機器人末端位姿檢測問題轉換為點陣圓形標定板的中心點位姿檢測問題，在得到點陣圓形標定板中心點位姿后經過仿射變換求得末端執行器位姿。

對于該水果分揀并聯機器人，首先通過雙目相機采集機構末端圖片，并基于ORB算法進行特征點檢測和立體預匹配。然后采用改進的PROSAC算法對立體預匹配結果進行提純，以提高立體匹配的準確率。最后將提純后的匹配點對代入雙目視覺模型，求出并聯機器人末端位姿參數。

2.1 圖像采集和特征點檢測

2.1.1圖像采集和處理

通過Dalsa CamExpert圖像采集軟件進行圖像采集，兩個G3-M2420工業相機固定安裝在末端執行器的上方，可實現直徑D=400 mm、高度H=100 mm的圓柱體空間定位。鏡頭SV-0614H通過C-mount接口安裝到相機上，相機通過GigE Vision連接到圖像采集卡，從而連接到計算機上。圖3為同一時刻左右兩個相機采集到的點陣圓形標定板原始圖像。

圖3 相機采集的原始圖像

由于所采集立體圖像對中的特征點對不在同一掃描線上，因此采用極線校正法[16]對立體圖像對進行投影變換使其滿足掃描線特性，可大大減少立體匹配的搜索空間，提高立體匹配速度。校正結果如圖4所示。

圖4 極線校正后的圖像

2.1.2特征點檢測

為保證所研究并聯機器人末端位姿檢測的實時性，采用簡單快速的ORB算法進行特征點提取和描述。ORB算法由O-FAST特征提取算子和rBRIEF特征描述算子組成，實時性比傳統的SIFT、SURF算法有數量級的提高[17-18]，能夠滿足水果分揀并聯機器人雙目視覺末端位姿檢測的實時性要求。

2.2 立體匹配及三維坐標求解

2.2.1基于ORB算法的立體預匹配

ORB算法中的rBRIEF特征描述子以二進制形式存在，因此采用Hamming距離對特征點匹配與否進行判斷，即通過對兩個特征點的二進制串進行異或處理，篩選出正確的匹配點對[19]。Hamming距離可表示如下：

(2)

2.2.2基于改進PROSAC算法的立體預匹配結果提純

本文采用穿插取點和預檢驗候選模型的方法對PROSAC算法進行改進，并將改進的PROSAC算法應用到立體匹配中，通過對預匹配結果進行提純以提高立體匹配的準確率，進而末端位姿檢測的精度得以提高。

2.2.3相機模型參數求解

采用針孔模型對相機成像模型進行分析和建立。如圖5所示，定義四個坐標系：(1) 世界坐標系{W}={OW-XWYWZW}，描述物體和相機的空間位置；(2) 相機坐標系{C}={OC-XCYCZC}，原點設置在相機光心上，ZC軸與光軸重合并垂直于成像平面；(3)
圖像像素坐標系{O}={o-uv}，圖像右上方為原點；(4)
圖像物理坐標系{I}={OI-XIYI}，光軸與圖像平面的交點設為原點[20]。

圖5 針孔成像模型

根據針孔成像模型，空間中任一點M(XW,YW,ZW)在圖像上的像素投影點為m(u,v)，兩者的關系為:

(3)

式中：ZC為焦點到像平面的距離；M1、M2為相機內外參數矩陣。通過將標定板中各標志點的像素坐標和世界坐標代入式(3)，求解出相機的內外參數。

2.2.4特征點三維坐標求解

假設點P的三維空間坐標為PW(XW,YW,ZW)，在左相機拍攝圖像中的像素坐標為pl(ul,vl)，在右相機拍攝圖像中的像素坐標為pr(ur,vr)。則根據投影定理有：

(4)

式中：sl、sr表示比例因子；Ml=Al[RlTl]和Mr=Ar[RrTr]分別為左右投影矩陣，Al、Ar、Rl、Rr、Tl、Tr分別為左右相機內參矩陣、左右相機坐標系相對于世界坐標系的旋轉矩陣和平移矩陣。將式(4)展開，則有：

(5)

(6)

聯立式(5)和式(6)，可得：

A×PW=B

(7)

因此：

PW=(ATA)-1ATB

(8)

將特征點的左右像素坐標代入式(8)，即可求得特征點的三維坐標。

2.3 水果分揀并聯機器人位姿參數求解

2.3.1并聯機器人坐標系定義

水果分揀并聯機器人坐標系如圖6所示，建立世界坐標系{W}={OW-XWYWZW}、左相機坐標系{C}={OC-XCYCZC}和目標坐標系{P}={OP-XPYPZP}，其中：世界坐標系原點OW位于并聯機器人靜平臺的中心點，ZW軸垂直于靜平臺平面；左相機坐標系原點OC為左相機平面中心點，XC軸和YC軸平行于左相機平面；目標坐標系原點OP位于P2點，則OP的位姿可表示并聯機器人的末端位姿。因此，水果分揀并聯機器人的位姿信息包括點OP在世界坐標系下的三維空間坐標和并聯機器人末端執行器的三個姿態角(俯仰角α、翻滾角β，航向角γ)。根據水果分揀并聯機器人的運動特點，其運動過程中α和β相對于定坐標系不變，只有γ在變化，故該并聯機器人的末端位姿參數可表示為(x,y,z,γ)。

圖6 水果分揀并聯機器人坐標系

2.3.2位置參數計算

將標定板的中心圓形標志點設為目標標志點P，將點P的左右像素坐標代入式(8)求出其世界坐標P(xW,yW,zW)。已知標定板與末端執行器的相對位置保持不變，通過仿射變換即可求得末端執行器的位置參數。假設仿射變換中的旋轉矩陣為：

(9)

平移矩陣為：

N=[NxNyNz]T

(10)

則末端執行機構的位置可表示為：

(11)

2.3.3姿態參數計算

一般并聯機器人包括三個姿態角α、β、γ，其中：α為繞X軸旋轉的俯仰角，β為繞Y軸旋轉的翻滾角，γ為繞Z軸旋轉的航向角。假設目標坐標系{P}繞坐標軸旋轉變換之后與世界坐標系重合，則旋轉矩陣表示為：

(12)

則俯仰角、翻滾角和航向角分別表示為：

(13)

已知末端執行器上兩點M1和M2在目標坐標系下的坐標M1=(x1,y1,z1)T、M2=(x2,y2,z2)T，在世界坐標系下的坐標MW1=(xW1,yW1,zW1)T、MW2=(xW2,yW2,zW2)T，則有：

(14)

(15)

由式(14)、式(15)得：

MW1-MW2=R(M1-M2)

(16)

求出的R即為旋轉矩陣，將R代入式(13)可得到并聯機器人的末端姿態參數γ。

3 實驗與結果分析

實驗平臺包括硬件平臺和軟件平臺。硬件平臺：操作系統Windows 7，處理器 Intel( R) Core(TM) i7-3770，主頻3.40 GHz，內存8 GB，Teledyne Dalsa面陣相機(Nano-M2420)，VST鏡頭(SV-0614H)。軟件平臺：VC++，OpenCV庫，MATLAB。

實驗方法：為驗證本文所提出改進PROSAC算法的水果分揀并聯機器人雙目視覺末端位姿檢測方法相比于未改進PROSAC算法的檢測方法的有效性，首先在立體匹配階段進行三組對比實驗。實驗一為原始立體圖像對，實驗二為光照變化后的立體圖像對，實驗三為加入方差0.01的高斯噪聲后的立體圖像對，分別對每組立體圖像對采用未改進的PROSAC算法和改進的PROSAC算法提純ORB立體預匹配結果，并依據立體匹配的準確率和時耗對兩種立體匹配方法進行比較分析。

定時采集36幅圖像，分別采用未改進的PROSAC算法和改進的PROSAC算法對ORB立體預匹配結果進行提純，從提純后的立體匹配結果中提取目標標志點對，將其代入雙目視覺模型獲取水果分揀并聯機器人末端位姿參數，并與激光跟蹤儀Leica AT901和電子羅盤Honeywell-HMR3100測得的末端執行器位姿對比，從而驗證所提出水果分揀并聯機器人末端位姿檢測方法的有效性。

實驗結果及分析：圖7-圖9分別為三組立體匹配實驗結果圖，表1-表3分別為三組立體匹配的準確率和時耗統計。由表1-表3可知，與采用未改進的PROSAC算法進行立體匹配提純相比，采用改進的PROSAC算法將原始立體圖像對的立體匹配準確率從92.5%提高至96.1%，耗時從0.085 s降低至0.062 s；將光照變化后立體圖像對的立體匹配準確率從90.4%提高至94.2%，耗時從0.112 s降低至0.087 s；將增加噪聲后立體圖像對的立體匹配準確率從89.0%提高至93.9%，耗時從0.139 s降低至0.101 s。由此可看出，改進的PROSAC算法降低了運算復雜度，提高了立體匹配的準確率和實時性，且對光照變化和噪聲干擾有良好的魯棒性。

圖7 原始立體圖像對的立體匹配結果

圖8 光照變化后立體圖像對的立體匹配結果

圖9 增加噪聲后立體圖像對的立體匹配結果

表1 原始立體圖像對的立體匹配結果統計

表2 光照變化后立體圖像對的立體匹配結果統計

表3 增加噪聲后立體圖像對的立體匹配結果統計

圖10為水果分揀并聯機器人的末端位姿檢測誤差圖，其中：(a)為未改進PROSAC算法的末端位姿檢測誤差圖，(b)為改進PROSAC算法的末端位姿檢測誤差圖。表4為兩種檢測方法的誤差結果統計。

圖10 水果分揀并聯機器人的末端位姿檢測誤差

表4 末端位姿檢測誤差結果統計

由表4分析可得，與未改進PROSAC算法的末端位姿檢測方法相比，改進PROSAC算法的水果分揀并聯機器人末端位姿檢測方法，其位姿分量x的位置誤差平均絕對值降低了53.9%，標準差降低了53.2%；y的位置誤差平均絕對值降低了65.5%，標準差降低了67%；z的位置誤差平均絕對值降低了66.9%，標準差降低了66.6%；γ的姿態誤差平均絕對值降低了47%，標準差降低了56.6%。

4 結 語

針對一種水果分揀并聯機器人，本文提出一種改進PROSAC算法的水果分揀并聯機器人雙目視覺末端位姿檢測方法。主要結論如下：

(1) 通過穿插取點和預檢驗候選模型的方法對PROSAC算法進行改進，克服了PROSAC算法應用于水果分揀并聯機器人雙目視覺末端位姿檢測時存在的模型參數估計精度不高和驗證錯誤候選模型耗時問題。基于此，進一步提出了一種改進PROSAC算法的水果分揀并聯機器人末端位姿檢測方法，解決了雙目視覺末端位姿檢測中光照、噪聲干擾等外部因素造成的末端位姿檢測精度降低問題。

(2) 將改進PROSAC算法的水果分揀并聯機器人末端位姿檢測方法與未改進PROSAC算法的檢測方法進行實驗對比，實驗結果表明，所提出改進PROSAC算法的并聯機器人末端位姿檢測方法，其位姿分量x、y、z、γ的誤差平均絕對值分別降低了53.9%、65.5%、66.9%、47%，誤差標準差分別降低了53.2%、67%、66.6%、56.6%。因此，所提出的改進PROSAC算法的水果分揀并聯機器人末端位姿檢測方法提高了末端位姿檢測精度。