機器人線結構光視覺引導系統的手眼標定方法

吳慶華，萬 偲*，李子奇，蔡瓊捷思

（1.湖北工業大學 機械工程學院，武漢 430068；2.現代制造質量工程湖北省重點實驗室，武漢 430068）

0 引言

隨著工業機器人自動化水平的提高，工業機器人參與的自動化加工成為未來工業發展的主導[1]。通過視覺引導機器人進行抓取、搬運等任務作業，對于提高生產線的自動化水平，拓寬機器人的應用范圍都有十分重要的意義[2]。由于線結構光視覺傳感器具有非接觸、抗干擾能力強、精度高、實時性強、主動受控等優點[3，4]，常常用于三維散放零件的視覺引導抓取。而線結構光視覺傳感器與機器人之間的位姿關系的標定，是視覺引導中的研究熱點[5，6]。其難點在于尋找一個合適的靶標，分別建立起靶標與視覺傳感器和機器人之間的聯系。

國內外很多學者對線結構光視覺傳感器與機器人之間的手眼關系標定展開研究，手眼關系的標定靶標可分為：平面靶標、場景點、立體靶標。第一類靶標是平面靶標，Sung[7]等控制機器人帶動相機進行純平移和旋轉運動，拍攝固定的棋盤格靶標，同時完成了相機的標定和手眼標定；解則曉[8]等控制機器人帶動棋盤格在相機視野內進行數次姿態變換，采用兩步法[9]依次求取手眼矩陣中的R和t，該方法棋盤格圖像數據獲取過程繁雜，數據處理時間長，不適用于現場手眼標定，且該手眼系統對較遠距離（約700mm～1100mm）目標的三維定位誤差在5mm左右，精度低。第二類靶標是場景點，Qi[10]等提出一種基于線結構光條紋直線特征的手眼標定方法，標定過程中攝像機捕捉兩條線結構光條紋相交的交點作為標定點，但實際中線結構光條紋相交的特征點提取過程精度難以保證；Hu[11]等提出一種基于平面特征的機器人手眼標定方法，以投射到平面上的光斑為標定點，但是同樣存在平面特征精度無法保證的限制。第三類是立體靶標，劉蘇宜[12]等設計了鉆有圓孔的小方塊作為靶標，提取了經靶標調制后的線條紋圖像特征點作為標定參考點，但其算法標定點數量少，且圖像特征點提取精度不高，導致其標定精度較低；Ren Y［13］等采用標定球進行手眼標定，通過改變機器人的位姿變化得到多組數據進行矩陣方程直接求解得到手眼關系，但沒有對整個標定過程進行誤差分析；李劍峰[14]等控制機器人帶動標定球做出復雜的旋轉平移，該方法對機器人重復定位精度要求高，而機器人定位精度誤差會累積到手眼矩陣的精度誤差，且該方法耗時長，不適用于現場標定。

不同坐標系之間位姿關系的求解也是視覺引導系統標定中的一個關鍵問題，以待求解數據類型的不同分為2D-2D問題[15]、3D-2D問題[16]和3D-3D問題[17]。三維視覺手眼關系求解是典型的3D-3D問題，即已知n對特征點在不同坐標系下的三維坐標，求解兩個不同坐標系之間的位姿關系。Besl和McKay[18]提出的經典的最近點迭代算法在數據初值良好的情況下，可以給出較為準確的旋轉平移矩陣，常用于解決3D-3D的位姿求解問題。然而，經典的最近點迭代算法對點云初值位置要求高，進行迭代計算時容易陷入局部最優解[19]。Serafin J[20]提出的基于法向量的改進最近點迭代算法，在最近點匹配的過程中考慮了點云曲面的法向量，并以此為評價指標剔除一些明顯的錯誤匹配點對，減少了計算量并且提高了配準的精度。

本文針對Eye-to-Hand線結構光視覺傳感器的機器人引導系統的現場手眼問題，采用標準球作為標定靶標，機器人只需平動，以解決靶標特征點數量少且難以提取、標定結果受機器人重復定位精度影響大的問題；引入改進的最近點迭代算法，以提高手眼標定的精度。

1 Eye-to-hand機器人線結構光視覺引導系統手眼標定數學模型

Eye-to-hand機器人線結構光視覺引導系統由視覺掃描系統和機器人系統組成，結構示意圖如圖1所示。

圖1 機器人線結構光視覺引導系統示意圖

本文求解的手眼關系即機器人的基坐標系OR-XRYRZR與線結構光視覺傳感器坐標系OS-XSYSZS之間的旋轉平移矩陣。

標定時，設在n個靶標特征點在線結構光視覺傳感器坐標系OS-XSYSZS下的坐標點集為P={Pi|Pi∈R3，i=1，2，...，n}，對應的n個靶標特征點在機器人的基坐標系OR-XRYRZR下的坐標點集為Q={Qi|Qi∈R3，i=1，2，...，n}。兩組數據的旋轉矩陣R和平移矩陣t即為手眼關系。計算兩組數據的變換矩陣實質上是3D-3D的位姿估計問題，最終手眼矩陣[R，t]的求解問題轉變為式(1)的求解問題。

2 手眼關系求解

手眼關系求解的整個流程分為三步，第一步是靶標特征點在線結構光視覺傳感器坐標系下數據的獲取，第二步是靶標特征點在機器人基座標系下數據的獲取，第三步是引入改進的最近點迭代算法求解手眼關系。

2.1 靶標特征點在線結構光視覺傳感器坐標系下數據的獲取

獲取靶標特征點在線結構光視覺傳感器下的坐標數據，即求標準球心在線結構光視覺傳感器坐標系下的坐標。標定時，線結構光傳感器投出的光平面與標準球相交，采集得到的是一段空間點云數據。擬合此點云即可恢復球心在線結構光傳感器坐標系下的坐標。

空間圓點云數據是一段由線結構光視覺傳感器坐標系下的眾多點組成的圓弧，且此圓弧存在于線結構光視覺傳感器坐標系下的光平面既XSOSZS平面內，其所有點的Y坐標為0。采用最小二乘法擬合擬合此圓弧，最終可得到擬合圓圓心在XSOSZS平面內的坐標（xC，yC）以及擬合圓的半徑r。

求得空間圓在線結構光視覺傳感器坐標系下的圓心坐標（xC，yC）和半徑r，結合球體特殊的立體結構即可獲的靶標球心在線結構光視覺傳感器坐標系下的坐標。如圖2所示，此剖切面的圓心為OC（xC，0，zC），靶標的球心坐標為OB（xB，yB，zB），在線結構光視覺傳感器坐標系中剖切面圓心OC與標準球靶標球心OB連線OCOB垂直于XSOSZS平面。剖切面的圓心坐標與靶標球心坐標存在如下坐標轉換關系：

圖2 標準球靶標與光平面相交

式中：R1和r分別為標準球的半徑與空間擬合圓的半徑。yB的正負號可以通過線結構光視覺傳感器掃描標準球的部位確定。

求得空間圓的圓心坐標（xC，yC）和半徑r，通過式(2)即可求取靶標球心在結構光傳感器坐標系下的的空間球心坐標OB（xB，yB，zB）。

機器人平動改變標準球位置，即可得到一系列標準球心在線結構光視覺傳感器下的坐標點集P。

2.2 靶標特征點在機器人基座標系下數據的獲取

求靶標特征點在機器人基坐標系下的坐標數據，即求標準球心在機器人基座標系下的坐標。從機器人控制器中可以獲取機器人TCP（Tool Center Point，即機器人末端工具坐標系原點）在機器人基座標系下的坐標值，只需建立起機器人基座標系下靶標球心坐標值跟機器人TCP坐標值之間的固定的聯系即可獲取靶標特征點在線結構光視覺傳感器下的坐標數據。

通過設計一個特殊的載物架來建立起靶標球心坐標跟機器人TCP坐標之間固定的坐標轉換關系。結合機器人法蘭盤上的四個螺孔和法蘭凸緣的尺寸，設計了如圖3所示的載物架。載物架左邊有一個用來與法蘭凸緣配合給載物架定位的通孔，其四周是四個用來配合螺紋鏈接的孔位；載物架右邊是一個直徑為φ的通孔，標定時標準球放置在上面。

如圖3所示，在機器人法蘭凸緣的定位下，用四個螺釘通過螺紋連接把載物架緊固在EPSON機器人的末端法蘭上。載物架的上端面與機器人末端法蘭貼合。由幾何關系可得，結合載物架具體尺寸，可得靶標球心在機器人末端工具坐標系下的坐標為。讀出機器人TCP坐標，即可獲取靶標球心在機器人基坐標系下的坐標數據。

圖3 載物架與法蘭盤的裝配示意圖

在標準球與線結構光傳感器的光平面處于相交狀態的范圍內，機器人平動改變標定位置即可得到一系列標準球心在機器人基坐標系下的坐標點集Q。

2.3 改進的最近點迭代算法求解手眼關系

求解手眼關系，即求解式(1)中的［R，t］矩陣，這是一個典型的3D-3D問題，即已知n對特征點在不同坐標系下的三維坐標，求兩個不同坐標系之間的位姿關系。最近點迭代算法常用于解決3D-3D的位姿求解問題。經典的最近點迭代算法主要分為搜索最近點、求解變換關系(R，t)、應用變換關系(R，t)得到新點集、重復迭代計算這幾個關鍵步驟。然而，經典的最近點迭代算法只是利用點對點之間的歐氏距離作為搜索最近點的唯一判據，這種方法得到的點云初值位置不良，可能導致迭代計算陷入局部最優解。

本文中的點云初值數據是空間中n對特征點分別在傳感器坐標系和機器人基坐標系下的坐標點集，其坐標數據在獲取的過程中受傳感器測量誤差和機器人重復定位誤差的影響，故點云初值不夠準確；且本文點云數據分散，其點云法向量、點云曲率等特征提取困難。故引入向心向量的概念，定義向心向量（如圖4所示）為點集中任意一點到點集中心點的向量。首先，以點和點的向心向量之間歐氏距離為誤差項，進行最近點的匹配；然后，利用最近點對的向心向量夾角對最近點對進行提純，剔除了較差初值的最近點對，有效避免迭代計算陷入局部最優解；最后，對較差初值的最近點對剔除之后的兩組點云進行配準迭代計算，求出兩組點云之間的旋轉平移矩陣。算法具體流程如下：

圖4 向心向量示意圖

第一步，根據點和點的向心向量之間歐氏距離進行最近點的匹配。

定義2 個已知點集：源點集P={Pi|Pi∈R3，i=1，2，...，N}為靶標球心在線結構光視覺傳感器坐標系下的坐標點集，目標點集Q={Qj|Qj∈R3，j=1，2，...，N}為靶標球心在機器人基座標系下的坐標點集。定義兩組點集的中心點分別為；則點集P中特征點的向心向量為，點集Q中特征點的向心向量為。選取源點集P中一個特征點，計算其向心向量與目標點集Q中所有點的向心向量之間的歐氏距離，并相互比較選取歐氏距離最小的兩個點，如果最小距離與次最小距離相比小于預先設定的某個閾值G1（G1＞0），則目標點集Q中與源點集P中特征點向心向量歐氏距離最近點為一對最近點對。

向心向量之間歐氏距離的求解公式如式(3)所示。

第二步，利用最近點對的向心向量夾角對最近點對進行提純。

考慮到源點集P與目標點集Q雖然處于不同的空間坐標系，但其空間拓撲關系應該保持一致，最近點對的向心向量之間的夾角應滿足一定的數學關系。

最近點對的向心向量之間的差別越大，其夾角的余弦值越小，即的值就越小。根據式(4)對向心向量之間的夾角余弦值設置閾值G2，將對應最近點對的向心向量的乘積小于G2的點對視為較差初值的特征點對，并將其剔除。

第三步，對剔除較差初值的最近點對之后的兩組點云進行下一步配準迭代計算求出兩組點云之間的旋轉和平移矩陣。

設旋轉矩陣為R，平移矩陣為t，用f(R，t)來表示點集P在變換矩陣(R，t)下與點集Q之間的誤差函數。則求解最優變換矩陣的問題就可以轉化為求解滿足minf(R，t)的最優解(R，t)的問題。

主要步驟如下：

1）計算旋轉矩陣R和平移矩陣t，使得誤差函數f(R，t)最小；

2）利用獲得的R和t對點集Q進行旋轉平移，得到新的點集Q1；

3）計算新的點集P與點集Q1的平均距離D：

4）如果D小于某一給定的閾值或者迭代次數大于預設的最大迭代次數，則停止迭代計算。否則返回第1步，直到滿足收斂條件為止。

其中誤差函數f(R，t)的求解采用常用的奇異值分解（Singular Value Decomposition，SVD）方法。

3 手眼標定及三維定位實驗

為了驗證本文手眼標定方法的可行性，以及改進的最近點迭代算法求解手眼矩陣的精度，進行了手眼標定實驗，并以三維定位精度為評定依據，與經典的最近點迭代求解方法進行對比分析。

3.1 機器人線結構光視覺引導系統的手眼標定實驗

如圖5所示為Eye-to-hand機器人線結構光視覺引導實驗平臺，主要器材包括EPSON S5-A701S機器人，其重復定位精度為±0.02mm；LMI公司的Gocator2350線結構光視覺傳感器；直徑為20mm（精度0.5μm）的標準球、載物架、一維線性位移平臺、工控機等。用高精度的坐標測量機測得載物架的關鍵尺寸即標準球放置孔的孔徑F為19.993mm、O1O2距離L為149.981mm。其中，Gocator2350線結構光視覺傳感器固定安裝在一維線性位移平臺上，確保EPSON機器人的工作空間位于相機視場范圍內。

圖5 Eye-to-hand機器人線結構光視覺引導系統

第一步，求取靶標球心在線結構光視覺傳感器坐標系和機器人基坐標系下的坐標點集P和Q。

控制機器人帶動標準球靶標移動至線結構光傳感器光平面內，使球面與線結構光相交。控制機器人在線結構光與球面相交的范圍內平動并采集15組空間圓點云數據，使用2.1節中方法得到標準球靶標球心在線結構光傳感器下的坐標點集P={Pi|Pi∈R3，i=1，2，...，15}。同時，從機器人控制器中讀出對應的15組機器人TCP坐標點集為T={Ti|Ti∈R3，T={i=1，2，...，15}。根據2.2節中的坐標轉換公式可求得對應的15組靶標特征點在機器人基坐標系下的坐標點集Q={Qi|Qi∈R3，i=1，2，...，N}。求得坐標點集P和Q如表1所示。

表1 靶標球心分別在視覺傳感器和機器人下的坐標點集（mm）

第二步，使用2.3節改進的最近點迭代算法求解這兩組坐標點集Q和P之間的旋轉平移矩陣[R，t]，根據試驗數據將最近點匹配的閾值G1設定為0.05，將最近點對提純的閾值G1設定為0.8，實驗過程中第7、13組點對被剔除，最終求得的手眼關系為：

3.2 機器人線結構光視覺引導系統的三維定位精度驗證試驗

手眼關系標定無法單獨評估其精度，需要進行機器人線結構光視覺引導系統的三維定位精度驗證實驗，以三維定位精度為指標來檢驗手眼標定精度。

保持機器人和標準球不動，由線結構光視覺傳感器掃描獲得標準球球心坐標值，然后通過手眼矩陣將此坐標值轉換到機器人基坐標系下。控制機器人平動使標準球移動到另外一個位置，再次得到標準球球心在線結構光視覺傳感器下的坐標值，轉換此坐標值到機器人基坐標系下。重復多次移動和測量，得到標準球球心在機器人基坐標系下的坐標集{M}；同時，讀取機器人TCP坐標集，由2.2節中的方法換算出標準球球心在機器人基坐標系下實際坐標值，得到坐標集{N}。根據坐標集{M}、{N}的坐標數據，通過式(7)計算得到三維定位的誤差集{S}。以誤差集{S}中的誤差均值和誤差的標準值為指標來對手眼標定精讀進行評價。

式中：K為三維定位誤差；xM1、yM1、zM1為坐標集{M}中固定點三維坐標值；xN1、yN1、zN1為坐標集{N1}中同一固定點三維坐標值。

完成系統的手眼標定工作后，啟動一維線性位移平臺帶動線結構光視覺傳感器對由載物架固定在機器人TCP上的標準球靶標進行掃描測量，在線結構光視覺傳感器掃描工作范圍內，通過控制機器人平動對10個不同位置的標準球進行掃描測量，測量結果如表2所示。

根據表2統計數據，Eye-to-hand機器人線結構光視覺引導系統的機器人定位精度實驗的定位誤差的均值為1.091mm，定位誤差的標準差為0.399mm，可以滿足大多數機器人線結構光視覺引導系統的定位要求。

表2 三維定位精度分析（mm）

為保證算法的普遍有效性，通過多組數據進行驗證。按4.1節所述方法，完成10次標定數據的采集，分別使用改進的最近點迭代算法和直接使用最近點迭代算法獲取手眼矩陣，使用上述方法分別獲得10次實驗中三維定位誤差均值和誤差的標準值，結果如圖6和圖7所示。10次實驗中，改進的最近點迭代算法獲取的手眼矩陣用于系統三維定位時的定位誤差的均值和定位誤差的標準差都小于用經典最近點迭代算法的實驗組。相比經典的最近點迭代算法，本文方法標定后，10次三維定位實驗中定位誤差均值的平均值由1.96mm降為1.01mm，定位誤差的標準差的平均值由0.444mm降為0.391mm。

圖6 手眼矩陣用于三維定位時的誤差均值

圖7 手眼矩陣用于三維定位時的誤差標準差

3.3 改進的最近點迭代算法性能分析

為分析改進的最近點迭代算法性能，如表3所示，將10次標定實驗中改進的最近點迭代算法和經典最近點迭代算法的迭代次數、迭代配準用時、配準精度進行對比。10次實驗中，改進的最近點迭代算法的平均迭代配準用時由1630ms降到640ms，配準誤差由0.5441mm降為0.1136mm。

表3 算法性能比較

本文改進的最近點迭代算法中，由于點云的向心向量的計算是一次性的，不需要在每次迭代計算中再重新計算，算法每次迭代的時間減少了；經過最近點的匹配和最近點對的提純，算法提供了良好的點云初始配準位置，因此算法迭代次數有所減少，配準精度反而有所提高；且整個算法的迭代配準用時比經典最近點迭代算法少。

4 結語

針對Eye-to-Hand機器人線結構光視覺引導系統的手眼標定問題，提出了一種基于改進的最近點迭代算法的手眼關系標定方法。在標定過程中，只需要機器人做若干組平動的動作即可方便快捷地獲取兩組標定數據，且手眼標定精度受機器人重復定位精度影響較小；在改進的最近點迭代算法中，引入向心向量；以點和點的向心向量之間歐氏距離為誤差項，進行最近點的匹配；利用最近點對的向心向量夾角對最近點對進行提純。由實驗數據知，由于提供了較好的初值位置，改進的最近點迭代算法的迭代次數和迭代時間都進一步縮短，且配準誤差進一步減小了；10次三維定位實驗中用本文方法進行手眼標定后的定位誤差平均值由1.96mm降為1.01mm，定位誤差標準差的平均值由0.444mm降為0.391mm。證明本文中改進的最近點迭代算法對于提高機器人線結構光視覺引導系統的三維定位精度有實際意義。