基于四元數(shù)乘法可易性的機(jī)器人手眼標(biāo)定算法*

吳慶華，李子奇，李宗耀，邱捷鋒

(湖北工業(yè)大學(xué) a.機(jī)械工程學(xué)院；b.現(xiàn)代制造與質(zhì)量工程湖北省重點(diǎn)實(shí)驗(yàn)室，武漢 430068)

0 引言

機(jī)器人視覺引導(dǎo)系統(tǒng)被廣泛應(yīng)用于工業(yè)領(lǐng)域，該系統(tǒng)的關(guān)鍵問題是求取機(jī)器人工具坐標(biāo)系和視覺坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，可以用齊次矩陣描述這種關(guān)系[1]，如何快速準(zhǔn)確地求取齊次矩陣是問題的核心部分。文獻(xiàn)[1-2]最早提出AX=XB手眼標(biāo)定模型，由于四元數(shù)可以方便地描述剛體旋轉(zhuǎn)運(yùn)動，文獻(xiàn)[3]提出了基于四元數(shù)的求解算法，該算法步驟復(fù)雜，要求機(jī)器人末端執(zhí)行器的旋轉(zhuǎn)角θ≠360°n(n=0,1,...)；文獻(xiàn)[4]基于矩陣奇異值分解求解手眼標(biāo)定方程，其算法簡潔但旋轉(zhuǎn)角θ依然受到限制。在手眼標(biāo)定的實(shí)際應(yīng)用中，需要依據(jù)旋轉(zhuǎn)角θ判斷測量數(shù)據(jù)是否可以用于標(biāo)定，使得標(biāo)定過程復(fù)雜化。

針對上述問題，本文提出一種基于四元數(shù)乘法可易性的手眼標(biāo)定方程新解法，通過四元數(shù)乘法可易性改寫旋轉(zhuǎn)矩陣，再結(jié)合矩陣奇異值分解篩選求解旋轉(zhuǎn)矩陣，使旋轉(zhuǎn)角θ不受限制，之后代入求解平移向量，最后通過仿真和驗(yàn)證實(shí)驗(yàn)，檢驗(yàn)該方法準(zhǔn)確性和魯棒性。

1 機(jī)器人手眼標(biāo)定問題描述

機(jī)器人手眼系統(tǒng)主要分為Eye-in-Hand系統(tǒng)和Eye-to-Hand系統(tǒng)，由于在Eye-in-Hand系統(tǒng)中，攝像機(jī)被固定在機(jī)器人末端執(zhí)行器上，可以隨機(jī)器人同時運(yùn)動，因此被廣泛用于工業(yè)生產(chǎn)當(dāng)中[5]。為求解AX=XB類型手眼標(biāo)定方程，可將方程展開如下:

(1)

其中，RA、RX和RB分別表示齊次變換矩陣A、B和X的旋轉(zhuǎn)部分，tA、tX和tB分別表示對應(yīng)變換矩陣的平移部分，將式(1)再次展開可得：

(2)

基于四元數(shù)和矩陣奇異值分解的傳統(tǒng)手眼標(biāo)定方法[3-4]通過四元數(shù)改寫旋轉(zhuǎn)矩陣方程為Cx=0的形式后，通過歐拉參數(shù)[θ,uT]和單位四元數(shù)向量p=[e0,e1,e2,e3]T之間的轉(zhuǎn)換關(guān)系，將旋轉(zhuǎn)矩陣方程整理得到下式：

(3)

2 四元數(shù)乘法可易性手眼標(biāo)定方法

2.1 四元數(shù)乘法可易性

單位四元數(shù)可以用來描述剛體在三維空間中的旋轉(zhuǎn)變換[3]，設(shè)p和q分別表示兩個描述三維旋轉(zhuǎn)運(yùn)動的單位四元數(shù)，其乘法可易性[6]描述如下:

p·q=M(p)·q=M(q)+·p

(4)

(5)

(6)

其中，M(q)+稱為蛻變矩陣。

2.2 最小奇異值篩選求解旋轉(zhuǎn)矩陣RX

對于Cx=0形式的超定方程[7]，對系數(shù)矩陣C進(jìn)行奇異值分解即C=UΣVT時，x的最優(yōu)解為右奇異矩陣V中最小奇異值所對應(yīng)的列向量。在傳統(tǒng)方法中，在對系數(shù)矩陣進(jìn)行奇異值分解后，直接通過最小奇異值對應(yīng)的列向量求取單位四元數(shù)，再求解旋轉(zhuǎn)矩陣。由于在仿真和實(shí)際應(yīng)用中，無法保證測量數(shù)據(jù)構(gòu)成的系數(shù)矩陣直接進(jìn)行奇異值分解時一定可以得到較好的結(jié)果，本文采用一種組合測量矩陣篩選最小奇異值的求解方法。

將式(3)描述旋轉(zhuǎn)部分的方程RARX=RXRB使用四元數(shù)乘法進(jìn)行表示，設(shè)a、b和qx分別表示旋轉(zhuǎn)矩陣RA、RB和RX所對應(yīng)的單位四元數(shù)，則旋轉(zhuǎn)部分方程描述如下：

a·qx=qx·b

(7)

利用四元數(shù)乘法可易性將上式展開，整理移項(xiàng)后得到：

(8)

由于求解該方程時，機(jī)器人末端執(zhí)行器至少需要改變n(n≥2)次位姿[1]，設(shè)S(n)=(M(an)-M(bn)+)為第n次測量結(jié)果所對應(yīng)的4×4形式矩陣，求解旋轉(zhuǎn)矩陣的偽代碼如下所示：

輸入：處理后的測量矩陣S(1)，S(2)……S(n)(n≥2)

初始化：k=0

1:fori=2→ndo

3: form=1→jdo

4:k=k+1

5: 構(gòu)造系數(shù)矩陣T(k)=[M(1),M(2),……,M(i)]

6: end for

7:end for

8:對T(k)進(jìn)行奇異值分解，將每次奇異值分解得到的最 小奇異值記為P(k)

9:用P(k)的最小值P(k)min對應(yīng)的特征向量v，求解對

應(yīng)單位四元數(shù)qx

10:用單位四元數(shù)qx求解旋轉(zhuǎn)矩陣Rx

輸出：旋轉(zhuǎn)矩陣Rx

2.3 求解平移向量tX

將(2)中的平移向量方程RAtX+tA=RXtB-tA改寫為如下形式：

(RA-I)tX=RXtB-tA

(9)

通過改變機(jī)器人末端執(zhí)行器的位姿進(jìn)行n次測量后，將之前求解得到的旋轉(zhuǎn)矩陣RX代入計(jì)算，令

(10)

通過最小二乘法可以求得平移向量tX=(DTD)-1DTE。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 仿真實(shí)驗(yàn)結(jié)果及分析

由于在機(jī)器人末端執(zhí)行器旋轉(zhuǎn)角θ=360°n(n=0,1,...)時，傳統(tǒng)方法無法直接進(jìn)行求解，因此仿真實(shí)驗(yàn)的主要目的是驗(yàn)證本文方法的準(zhǔn)確性和魯棒性，采用了處理器主頻為2.6 G，內(nèi)存為8 G的計(jì)算機(jī)進(jìn)行仿真實(shí)驗(yàn)，仿真軟件為MATLAB R2016b，仿真實(shí)驗(yàn)及數(shù)據(jù)處理步驟如下：

(1)依據(jù)文獻(xiàn)[8]，假定機(jī)器人手眼關(guān)系X的真值為：

(2)隨機(jī)生成n個正交矩陣RA1,RA2,…,RAn和平移向量tA1,tA2,...,tAn，分別組成轉(zhuǎn)換矩陣A1,A2,...,A3，通過方程AX=XB求解對應(yīng)變換矩陣B1,B2,..,Bn的旋轉(zhuǎn)部分RB1,RB2,...,RBn和平移部分tB1,tB2,...,tBn。

(3)基于文獻(xiàn)[9]的方法，為模擬實(shí)際測量情況，在模擬數(shù)據(jù)中加入高斯噪聲(Gaussian Noise)，對RAi和RBi添加均值為0，標(biāo)準(zhǔn)差為0.01d的高斯噪聲；對tAi和tBi添加均值為添加均值為0，標(biāo)準(zhǔn)差為0.2d的高斯噪聲，其中d代表噪聲水平。

(a)相對旋轉(zhuǎn)誤差 (b)相對平移誤差

(c) 絕對誤差圖1 僅對A添加噪聲

(a)相對旋轉(zhuǎn)誤差 (b)相對平移誤差

(c)絕對誤差 圖2 僅對B添加噪聲

(a)相對旋轉(zhuǎn)誤差 (b)相對平移誤差

(c)絕對誤差圖3 同時對A和B添加噪聲

從圖中可以看出，所提算法在噪聲水平相同的情況下，準(zhǔn)確性優(yōu)于文獻(xiàn)[4]所提出的求解方法，并且隨著噪聲水平的增大，本文算法依舊具有較好的魯棒性。

3.2 手眼標(biāo)定實(shí)驗(yàn)

圖4 實(shí)驗(yàn)平臺照片

為驗(yàn)證本文算法在實(shí)際應(yīng)用中的可行性和準(zhǔn)確性，使用FANUC公司的LR Mate 200iD型工業(yè)機(jī)器人、Basler公司的acA1600-20gm型工業(yè)相機(jī)和尺寸為100 mm×100 mm的棋盤格標(biāo)定板進(jìn)行驗(yàn)證實(shí)驗(yàn)，搭建的實(shí)驗(yàn)平臺與如圖4所示。

其中，F(xiàn)ANUC工業(yè)機(jī)器人的重復(fù)定位精度為±0.02 mm,Basler工業(yè)相機(jī)的分辨率為1626×1236 pixels，像元尺寸為4.4 μm×4.4 μm，鏡頭焦距為25 mm，使用連接件將相機(jī)和工業(yè)機(jī)器人的法蘭盤緊固在一起。

實(shí)驗(yàn)流程如下：

(1)移動工業(yè)機(jī)器人帶動工業(yè)相機(jī)在10個不同位姿下對棋盤格標(biāo)定板進(jìn)行拍照，記錄各位姿下機(jī)器人控制器的6個讀數(shù)(x,y,z,w,p,r)并改寫成對應(yīng)的齊次轉(zhuǎn)換矩陣(P1,P2,...,P10)，該矩陣描述了機(jī)器人法蘭中心的工具坐標(biāo)系和機(jī)器人基坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系。

(2)對采集的10張圖像使用MATLAB視覺工具箱進(jìn)行標(biāo)定相機(jī)內(nèi)外參數(shù)的標(biāo)定，如圖5所示，得到10個不同位姿下的相機(jī)外參矩陣(Q1,Q2,...,Q10)。

圖5 相機(jī)內(nèi)外參數(shù)標(biāo)定示意圖

表1 驗(yàn)證實(shí)驗(yàn)結(jié)果

從表1可以看出，在10組重復(fù)實(shí)驗(yàn)中，本文所提手眼標(biāo)定算法的精度可以滿足機(jī)器人視覺系統(tǒng)在工件抓取應(yīng)用中的需求，本文算法在旋轉(zhuǎn)部分和平移部分的均方根誤差相對傳統(tǒng)算法分別減少了0.007 4和0.033 4。選取平移和旋轉(zhuǎn)誤差最小的一組求解值作為手眼標(biāo)定矩陣的真實(shí)值，如下所示：

4 結(jié)論

本文基于四元數(shù)乘法可易性，結(jié)合最小奇異值篩選，提出了一種手眼標(biāo)定方程解法，解決了基于四元數(shù)的和矩陣奇異值分解的手眼標(biāo)定算法在標(biāo)定時需要驗(yàn)證機(jī)器人末端執(zhí)行器旋轉(zhuǎn)角θ≠360°n(n=0,1,...)的問題。仿真實(shí)驗(yàn)的結(jié)果表明，本文所提算法對比傳統(tǒng)算法具有更好的準(zhǔn)確性和魯棒性。驗(yàn)證實(shí)驗(yàn)的結(jié)果表明，本文算法在實(shí)際應(yīng)用中具有可行性。