席曉亮，楊 洋，程 勇，陸 坤

(1.中國科學(xué)院等離子體物理研究所，合肥 230031；2.中國科學(xué)技術(shù)大學(xué)，合肥 230026)

0 引言

隨著機(jī)器人在工業(yè)范圍的普及，一系列具有機(jī)器視覺感知功能的智能機(jī)器人把工業(yè)自動化帶到了一個新的高度，視覺技術(shù)已廣泛應(yīng)用于機(jī)器人領(lǐng)域研究，如實(shí)時定位[1]，視覺伺服[2]，控制[3]和校準(zhǔn)[4]。在這類機(jī)器視覺應(yīng)用到機(jī)器人過程中，視覺傳感器相對于機(jī)械臂的手眼關(guān)系的標(biāo)定十分重要，即手眼關(guān)系的標(biāo)定是實(shí)現(xiàn)機(jī)械臂視覺引導(dǎo)和物體抓取的重要基礎(chǔ)。

Tsai和Lenz提出了兩步法的手眼關(guān)系標(biāo)定法，將旋轉(zhuǎn)矩陣和平移向量進(jìn)行分步求解[5]；Ma提出了共同標(biāo)定法，但系統(tǒng)的精度無法得到保證[6]；Ruda提出了基于四元數(shù)工具用非線性優(yōu)化的工具求解手眼關(guān)系[7]；也有文獻(xiàn)中提出了基于最大似然估計的非線性手眼標(biāo)定方法[8]。非線性手眼標(biāo)定法相對復(fù)雜，實(shí)用性較弱。

在傳統(tǒng)的手眼標(biāo)定中，一般人工給機(jī)器人擺位姿，然后人工判斷當(dāng)前位姿下攝像機(jī)采集的圖像是否有效，如果有效抓取當(dāng)前位姿下的圖像，然后再去擺下一個位姿。一旦發(fā)生手眼關(guān)系意外變動的情況，如連接件松動、鏡頭損壞、焦距調(diào)整等，就需要重新人工標(biāo)定手眼關(guān)系，效率較低。采用人工拖拽法，過程枯燥繁復(fù)效率低[9]；手眼關(guān)系自動化標(biāo)定的實(shí)現(xiàn)原型，通常結(jié)果誤差較大[10]；而發(fā)展標(biāo)定姿態(tài)自動生成的算法，會帶來的各種噪聲很大程度影響了標(biāo)定結(jié)果[11]。因此，研究一種可靠、精準(zhǔn)的手眼關(guān)系自動化標(biāo)定算法很有必要。

在本文中，對Eye-in-Hand系統(tǒng)提出一種手眼標(biāo)定算法。在人工拖拽獲得初始手眼關(guān)系的基礎(chǔ)上，建立空間姿態(tài)約束模型，生成等分空間的位姿點(diǎn)，引入誤差評估機(jī)制，對異常樣本進(jìn)行剔除。實(shí)驗(yàn)表明，引入異常值剔除后，能夠有效減少收斂所需的樣本數(shù)量和提升標(biāo)定精度：平移誤差穩(wěn)定在0.4mm，旋轉(zhuǎn)角度誤差最終穩(wěn)定在0.0018rad。

1 手眼關(guān)系標(biāo)定過程

1.1 標(biāo)定算法實(shí)現(xiàn)過程

手眼關(guān)系的自動化標(biāo)定涉及到圖像位姿檢測、機(jī)械臂運(yùn)動等諸多功能模塊，具體的標(biāo)定框架過程如圖1所示。UR5驅(qū)動、相機(jī)驅(qū)動以及場景描述文件事先準(zhǔn)備完成，相機(jī)內(nèi)參標(biāo)定采用張正友單平面棋盤格的相機(jī)標(biāo)定法。人工拖拽機(jī)械臂四次，確保在機(jī)械臂對應(yīng)位姿情況下，能夠從相機(jī)獲取的圖像中計算出marker相對于相機(jī)的位姿信息，通過以上四個標(biāo)定樣本計算得到手眼的初始關(guān)系，從而得到marker相對于機(jī)械臂基座的初始位姿關(guān)系。在初始手眼關(guān)系的基礎(chǔ)上，結(jié)合空間約束條件，生成相機(jī)將要跟隨的等分空間位姿點(diǎn)，然后再轉(zhuǎn)化為機(jī)械臂末端在機(jī)器人基座坐標(biāo)系下要跟隨的位姿點(diǎn)，通過RobWork進(jìn)行路徑規(guī)劃，當(dāng)機(jī)械臂到達(dá)相應(yīng)位姿點(diǎn)后，判斷當(dāng)前相機(jī)視野是否可以計算出marker的位姿，如果可以，就將這組marker位姿和對應(yīng)的機(jī)器人關(guān)節(jié)位姿存入標(biāo)定樣本集中去計算，否則繼續(xù)下一個位姿點(diǎn)的跟隨。最終通過循環(huán)計算，得出標(biāo)定結(jié)果。

圖1 算法實(shí)現(xiàn)過程

1.2 等分空間約束的位姿生成

手眼關(guān)系的自動化標(biāo)定，離不開可行位姿點(diǎn)的生成。文獻(xiàn)[5]中指出提高手眼標(biāo)定精度的四個重要準(zhǔn)則：

1）盡可能增大相對運(yùn)動旋轉(zhuǎn)軸之間的夾角；

2）盡可能增大相對運(yùn)動的旋轉(zhuǎn)角度；

3）盡可能減小標(biāo)定板到相機(jī)光軸中心的距離；

4）盡可能減小末端在不同姿態(tài)之間的距離。

基于這些準(zhǔn)則，以marker的中心為原點(diǎn)建立球坐標(biāo)系，合理約束相機(jī)坐標(biāo)系原點(diǎn)到球坐標(biāo)系原點(diǎn)的距離r，以及相機(jī)光軸與z軸夾角θ。設(shè)定相機(jī)坐標(biāo)系原點(diǎn)到marker中心最小距離rmin=0.2m最大距離rmax=0.38m，夾角θmin=0，θmmax=π/6，在xoy平面與x軸的夾角αmin=0，αmax=2π。

為了保證采樣點(diǎn)在空間中的均勻分布，在采樣過程中在約束的范圍內(nèi)對α,θ,r進(jìn)行等分采樣。為了確定(α,θ,r)位置處相機(jī)的姿態(tài)，定義相機(jī)的光軸指向球坐標(biāo)原點(diǎn)的徑向方向，相機(jī)光軸作為在該位置點(diǎn)處的zc軸，然后定義相機(jī)的xc與yc，具體如如圖2所示。xc垂直于面OcOF，從而確定在該位置點(diǎn)處相機(jī)的方向信息，最終作為相機(jī)將跟隨的一個位姿點(diǎn)。依據(jù)圖2的示意圖，可以得到位姿點(diǎn)處的笛卡爾坐標(biāo)系相對于以marker中心為坐標(biāo)系的笛卡爾坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣分別為：

圖2 空間約束模型原理圖

[R1丨t1]構(gòu)成了生成位姿點(diǎn)的位置和姿態(tài)，基于手眼初始關(guān)系和[R1丨t1]可以得到生成的采樣點(diǎn)在機(jī)械臂基座坐標(biāo)系下的位姿，從而下一步基于RobWork對機(jī)器人進(jìn)行采樣點(diǎn)跟隨的運(yùn)動，使得機(jī)器人末端的相機(jī)到達(dá)預(yù)期位姿。

2 手眼關(guān)系標(biāo)定原理

2.1 Tsai-Lenz 標(biāo)定計算

Tsai-Lenz是一種計算機(jī)械臂末端坐標(biāo)系與相機(jī)坐標(biāo)系間手眼關(guān)系的算法。需要的數(shù)據(jù)有機(jī)械臂末端坐標(biāo)系之間相對位姿變換矩陣、攝像機(jī)坐標(biāo)系間的相對位姿變換矩陣。相機(jī)安裝在機(jī)械臂上，并在相機(jī)視野內(nèi)放置marker，則有以下變換矩陣關(guān)系[12]：

根據(jù)上述的變換矩陣關(guān)系式，可以得到：

在AX=XB求解中，至少需要兩組A、B才能計算出結(jié)果，也就是至少需要采集3個標(biāo)定樣本。實(shí)際標(biāo)定過程，需要采集更多的標(biāo)定樣本，來獲得更好的標(biāo)定精度。

2.2 pnp圖像位姿求解原理

在本實(shí)驗(yàn)中，marker是一個平面，且marker的真實(shí)方塊物理尺寸是已知的，這些角點(diǎn)在相機(jī)中的像素坐標(biāo)也是已知的。對于DLT解法，場景中角點(diǎn)P的齊次坐標(biāo)P=(x,y,z,1)，齊次化表示其在像素平面上對應(yīng)的投影特征p=(u1,v1,1)點(diǎn)，s表示尺度因子。記增廣矩陣[R丨t]是3*4的矩陣，即：

用式(3)最后一行把s消去，得到兩個約束[13]：

每個特征點(diǎn)提供了關(guān)于t(t1,t2,t3)的兩個線性約束，對于3*4的[R丨t]矩陣，6對匹配點(diǎn)即可求得求解相機(jī)與marker間的[R丨t]矩陣，即位姿矩陣。攝相機(jī)在某采樣點(diǎn)拍攝照片如圖3中左圖所示，采用棋盤圖檢測法對左圖進(jìn)行內(nèi)角點(diǎn)檢測并按角點(diǎn)編號順序連接，得到圖3中右圖?；跈z測到的內(nèi)角點(diǎn)，下一步使用上文所述的pnp算法求解。

圖3 棋盤圖在攝像機(jī)坐標(biāo)系中的位姿求解

2.3 誤差評估機(jī)制

手眼標(biāo)定結(jié)果的精度直接取決于所獲取計算數(shù)據(jù)的準(zhǔn)確性。OpenCV棋盤圖檢測時，會存在有些圖像因?yàn)楣饩€等問題給圖像帶來噪聲干擾[14]，導(dǎo)致個別圖像計算出來的位姿出現(xiàn)很大誤差，如果把這種異常樣本放在后續(xù)的計算循環(huán)中，則會導(dǎo)致后面每一次更新計算出來的結(jié)果都會相應(yīng)的產(chǎn)生較大誤差，因此，在每次更新計算前，需要對新的圖像位姿數(shù)據(jù)進(jìn)行誤差評估，如果是非異常值則添加入計算數(shù)據(jù)中進(jìn)行當(dāng)次的更新計算。

對于非異常值的圖像計算序列chessPosem和對應(yīng)的機(jī)械臂位姿計算序列tcpPosem(m=1,2,...,i,...n)，對應(yīng)于前n組計算序列的得到的結(jié)果Rx和tx，按照參考文獻(xiàn)[15]定義，(k=1,2,...,j,...n-1)。從E1到Ej服從正態(tài)分布的特征，求得其正態(tài)分布的均值mean和方差stdev。對于i+1第組的計算樣本，定義允許的誤差區(qū)間[floor,ceiling]：

當(dāng)機(jī)械臂運(yùn)行到下一個有效位姿點(diǎn)時，記錄在這個位姿處的chessPosei+1和tcpPosei+1，結(jié)合tcpPosei和chessPosei，計算得到Ej+1：

如果Ej+1滿足條件(7)則把chessPosei+1和tcpPosei+1放入非異常值計算序列中，然后更新計算序列的誤差區(qū)間[floor,ceiling]。如果Ej+1不滿足條件(8)則認(rèn)為chessPosei+1和tcpPosei+1是異常值，不放入非異常值計算序列中。接下來對下一個位姿點(diǎn)處的圖像位姿數(shù)據(jù)和機(jī)械臂位姿數(shù)據(jù)同樣按照這樣去判定。

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

機(jī)器人手眼標(biāo)定實(shí)驗(yàn)在Ubuntu18.04操作系統(tǒng)下基于ROS搭建，采用OpenCV進(jìn)行圖像處理。RobWork是一個用于仿真模擬控制機(jī)器人的平臺，能夠和ROS很好的集成。

3.1 仿真實(shí)驗(yàn)

基于RobWork搭建仿真實(shí)驗(yàn)平臺，添加虛擬相機(jī)獲取圖像信息。仿真實(shí)驗(yàn)平臺如圖4所示。按照如圖1所示的算法實(shí)現(xiàn)流程開展實(shí)驗(yàn)。

圖4 仿真實(shí)驗(yàn)平臺

基于α,θ,r的范圍約束[16]，采用等空間采樣法，生成采樣位姿點(diǎn)。在循環(huán)更新計算部分，當(dāng)計算序列在4組及以上時開始計算，然后每到一個有效位姿點(diǎn)，就把該位姿處對應(yīng)的圖像位姿和機(jī)械臂位姿數(shù)據(jù)放入計算序列中，和之前的計算序列一起計算更新當(dāng)前位姿下的手眼關(guān)系。tn、Rn定義用n組數(shù)據(jù)更新計算得到的平移矩陣和旋轉(zhuǎn)矩陣。ttrue、Rtrue定義手眼關(guān)系的平移矩陣真值和旋轉(zhuǎn)矩陣真值，可以直接從仿真環(huán)境配置文件中讀出。

位置誤差的定義由更新計算得來的平移向量與真值平移向量做差取模得出[17]：

角度誤差的定義由計算得來的旋轉(zhuǎn)矩陣與真值旋轉(zhuǎn)矩陣的轉(zhuǎn)置相乘后的結(jié)果通過羅德里格斯公式處理為旋轉(zhuǎn)向量，再取模得出：

對應(yīng)的ep_n、er_n曲線分別如圖5、圖6所示。

圖5 仿真實(shí)驗(yàn)位置誤差曲線

圖6 仿真實(shí)驗(yàn)旋轉(zhuǎn)角度誤差曲線

圖5橫軸代表每次計算時所采用的計算樣本數(shù)，縱軸代表對應(yīng)序列數(shù)下所得值與真值的誤差，位置誤差曲線隨著計算序列組迭代更新而整體持續(xù)下降，最終在第22次更新后達(dá)到0.2mm左右的穩(wěn)定收斂狀態(tài)。圖6旋轉(zhuǎn)誤差呈現(xiàn)下降趨勢，最終在第22次的更新后達(dá)到0.0008rad左右的收斂狀態(tài)。

3.2 實(shí)體實(shí)驗(yàn)

本論文的實(shí)體實(shí)驗(yàn)平臺如圖7所示。采用UR5協(xié)作機(jī)器人末端搭載Stingray F504 CCD相機(jī)。通過ROS平臺建立機(jī)器人、攝像機(jī)和標(biāo)定程序之間通訊和控制網(wǎng)絡(luò)。

圖7 實(shí)體實(shí)驗(yàn)平臺

依據(jù)Tsai-Lenz兩步法求解方程AX=XB，至少需要兩組的A和B才能得出一個初始的X結(jié)果。此處和的初始A和B計算樣本數(shù)從4開始，機(jī)械臂每到一個采集點(diǎn)進(jìn)行相機(jī)拍照，如果圖像可以計算出位姿，則把當(dāng)前的圖像位姿信息和機(jī)械臂位姿信息和前面4組對應(yīng)信息一起作為下一次更新計算手眼關(guān)系的數(shù)據(jù)。如果采集點(diǎn)機(jī)械臂不可達(dá)或機(jī)械臂到達(dá)后，相機(jī)圖像不能計算出圖像的位姿，則該位姿點(diǎn)跳過，繼續(xù)進(jìn)入到下一個位姿點(diǎn)處的相關(guān)計算流程。每一個位姿點(diǎn)處的X更新計算完后，計算當(dāng)次計算結(jié)果的位置誤差與旋轉(zhuǎn)誤差。實(shí)體實(shí)驗(yàn)中ep_n、er_n和式(8)、式(9)定義一致，得到對應(yīng)的ep_n、er_n曲線如圖8、圖9所示。對130個標(biāo)定采集樣本，每次隨機(jī)采樣80組用于計算出一個t、R，總共取50次，最后對這50個t、R分別取均值，得出ttrue、Rtrue。

圖8 實(shí)體實(shí)驗(yàn)位置誤差曲線

圖9 實(shí)體實(shí)驗(yàn)旋轉(zhuǎn)角度誤差曲線

實(shí)體實(shí)驗(yàn)使用固定焦距的CCD相機(jī)，相較于仿真環(huán)境，采集到的圖像不可避免會存在瑕疵，前一次迭代樣本中存在的誤差會一直在后續(xù)計算組中去計算。在第65組新后，位置誤差在0.6mm左右，旋轉(zhuǎn)誤差在0.0224rad左右。采集樣本中的異常點(diǎn)，會導(dǎo)致標(biāo)定誤差波動，但隨著樣本數(shù)的增加，誤差的波動范圍降低了。

3.3 引入異常值剔除環(huán)節(jié)

在實(shí)體實(shí)驗(yàn)中引入誤差評估機(jī)制，按照式(6)、式(7)對異常值進(jìn)行剔除。每次非異常值計算序列更新后，計算得到一個對應(yīng)于該組計算序列的手眼標(biāo)定值，并依據(jù)式(8)、式(9)得到ep_n、er_n。得出的位置誤差曲線如圖10、圖11所示。

圖10 異常值剔除后的實(shí)體實(shí)驗(yàn)位置誤差曲線

圖11 異常值剔除后的實(shí)體實(shí)驗(yàn)角度誤差曲線

從圖10所示位置誤差曲線中可以看出，引入異常值剔除環(huán)節(jié)后，當(dāng)計算組更新到45組時已經(jīng)趨于穩(wěn)定，位置誤差最終穩(wěn)定在0.4mm左右，而且位置誤差從始至終在極小的范圍內(nèi)波動（約為0.1mm）。相對于無異常剔除機(jī)制的測試結(jié)果：第65迭代達(dá)到的0.6mm位置誤差，異常值剔除環(huán)節(jié)引入后在收斂速度和誤差精度方面都起到了明顯的優(yōu)化效果，而且誤差穩(wěn)定了，不會出現(xiàn)大幅度波動的情況。

圖11中所示的旋轉(zhuǎn)誤差曲線，計算更新到第45組時已經(jīng)趨于收斂，旋轉(zhuǎn)角度誤差最終穩(wěn)定在0.0018rad左右，引入誤差剔除前的測試結(jié)果：第65組時角度誤差穩(wěn)定在0.0224rad，在計算速度和精度上都有了顯著提升，而且旋轉(zhuǎn)誤差全程的波動范圍在0.0012rad以內(nèi)。

位置誤差和旋轉(zhuǎn)誤差在更新中都存在微小的震蕩，這是因?yàn)楫惓Ｖ蹬卸?biāo)準(zhǔn)當(dāng)中的誤差判定區(qū)間是[mean+2*stdev,mean-2*stdev]，對應(yīng)于2倍標(biāo)準(zhǔn)差的區(qū)間，其置信概率是95.5%，這會導(dǎo)致有些誤差較大但在判定區(qū)間內(nèi)的序列也會被計入計算序列中，從而導(dǎo)致下次的異常值誤差判定區(qū)間的范圍又變大，而在更新計算環(huán)節(jié)中，當(dāng)次加進(jìn)來的計算序列也會在接下來的更新輪次中反復(fù)用于計算。如果把誤差判定區(qū)間設(shè)置的較小，會導(dǎo)致對應(yīng)的置信概率會比較小。

3.4 與主流方法的比較

現(xiàn)有主流的方法有NAVY算法（Park[18]）、INRIA算法（Horaud[19]）、對偶四元數(shù)算法（Daniilidis[20]）。依據(jù)式(8)、式(9)所定義的ep_n、er_n，采用現(xiàn)有主流方法進(jìn)行計算，得出標(biāo)定的位置誤差和角度誤差。NAVY算法（Park）的位置誤差和角度誤差結(jié)果分別如圖12、圖13所示。

圖12 NAVY算法位置誤差曲線

圖13 NAVY算法角度誤差曲線

從圖1 2 可以看出，在8 5 次更新后，位置誤差在0.529mm左右，角度誤差在0.002114rad左右。對接下來的兩種主流方法分別開展實(shí)驗(yàn)，發(fā)現(xiàn)這兩種方法和INRIA算法得出的誤差曲線趨勢基本一致。

把仿真實(shí)驗(yàn)結(jié)果、實(shí)體實(shí)驗(yàn)結(jié)果、引入異常值剔除環(huán)節(jié)后的實(shí)驗(yàn)結(jié)果、主流方法結(jié)果整理如表1所示。

表1 標(biāo)定仿真和實(shí)體實(shí)驗(yàn)結(jié)果匯總

表1中三種主流方法得到的實(shí)驗(yàn)結(jié)果基本一致。發(fā)現(xiàn)主流方法在位置誤差和角度誤差上優(yōu)于實(shí)體實(shí)驗(yàn)（無異常值剔除）的結(jié)果。加入異常值剔除算法后，位置誤差和角度誤差結(jié)果優(yōu)于主流方法。

在引入異常值剔除環(huán)節(jié)后，在標(biāo)定計算精度和迭代次數(shù)上相對于表一中的其它方法有了較大提升，說明了該算法的有效性與優(yōu)越性。

4 結(jié)語

本文提出了一種機(jī)器人手眼自動化標(biāo)定的算法。首先人工拖拽機(jī)器人4個位姿，得出初始的手眼關(guān)系。在初始手眼關(guān)系的基礎(chǔ)上，建立基于maker的等分空間位姿約束模型。機(jī)械臂對這些位姿進(jìn)行逐一跟隨，采集標(biāo)定樣本，基于Tsai-Lenz方法完成循環(huán)更新計算。在每一次更新計算完成后，計算當(dāng)次的位置誤差和旋轉(zhuǎn)角度誤差。

在仿真實(shí)驗(yàn)中，干擾因素較少，位置誤差在第22組的更新后達(dá)到0.2mm左右的穩(wěn)定收斂狀態(tài)，旋轉(zhuǎn)角度誤差在第22組的更新后達(dá)到0.008rad左右的收斂狀態(tài)。實(shí)體實(shí)驗(yàn)中，位置誤差在第65組的更新后達(dá)到0.6mm左右，轉(zhuǎn)角度誤差在第65組的更新后達(dá)到0.0224rad左右，后續(xù)的樣本中有異常出現(xiàn)，導(dǎo)致標(biāo)定誤差波動。針對這種情況，提出了誤差評估機(jī)制，對異常計算序列進(jìn)行剔除。加入異常值剔除環(huán)節(jié)后，位置誤差在第45組的更新后達(dá)到0.4mm左右的穩(wěn)定收斂狀態(tài)，旋轉(zhuǎn)角度誤差在第45組的更新后達(dá)到0.0024rad左右的收斂狀態(tài)。最后用三種主流方法進(jìn)行實(shí)驗(yàn)，發(fā)現(xiàn)主流方法標(biāo)定的位置誤差和角度誤差結(jié)果介于兩者之間，說明了異常值剔除環(huán)節(jié)的有效性與優(yōu)越性。