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

王昌云， 李立君

(中南林業(yè)科技大學(xué) 機(jī)電工程學(xué)院，湖南 長沙 410004)

0 引 言

手眼標(biāo)定的目的是求解機(jī)器人末端執(zhí)行器坐標(biāo)系到相機(jī)坐標(biāo)系的變換矩陣，它是機(jī)器人完成手眼協(xié)調(diào)、自主導(dǎo)航等工作的基礎(chǔ)。Tsai R Y等人[1]、Shiu Y C等人[2]將手眼標(biāo)定的方程歸結(jié)為AX=XB，并給出了其線性閉環(huán)解法，兩種方法復(fù)雜且推導(dǎo)繁瑣；王君臣等人[3]基于非線性優(yōu)化解手眼標(biāo)定方程，該方法精度高，但算法復(fù)雜且易陷入局部最優(yōu)得不到最優(yōu)解；文獻(xiàn)[4～9]用矩陣直積等數(shù)學(xué)工具求解手眼標(biāo)定方程，算法結(jié)構(gòu)簡單，精度有待提高。

本文利用四元數(shù)基于奇異值分解(singular value decomposition,SVD)手眼標(biāo)定旋轉(zhuǎn)部分，之后代入旋轉(zhuǎn)部分的解求得手眼標(biāo)定的平移部分完成標(biāo)定。最后，將四元數(shù)的手眼標(biāo)定方法(quaternion hand-eye calibration method,QCM)與文獻(xiàn)[9]中的矩陣直積(matrix product)的標(biāo)定方法進(jìn)行對比實(shí)驗(yàn)，評價本文算法的優(yōu)劣。

1 四元數(shù)的概念

1.1 四元數(shù)乘法

四元數(shù)向量可用來描述剛體的坐標(biāo)旋轉(zhuǎn)，設(shè)3個四元數(shù)向量分別為α=[a0,aT]T，β=[b0,bT]T和γ=[c0,cT]T，則兩四元數(shù)相乘寫為矩陣形式如下(乘法符號用“?”表示)

(1)

(2)

(3)

令

(4)

則式(3)可簡寫為

γ=α+β=β-α

(5)

1.2 四元數(shù)與旋轉(zhuǎn)

單位四元數(shù)可定義任意兩個坐標(biāo)系之間的旋轉(zhuǎn)變換關(guān)系，設(shè)任一空間向量r在坐標(biāo)系Co，Ci和Cj中的表示為ro，ri和rj。任意兩坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系用四元數(shù)向量可表示為pio，poj和pij，則有

(6)

(7)

(8)

由式(6)～式(8)可得

pij=poj?pio

(9)

式(9)為四元數(shù)連續(xù)旋轉(zhuǎn)的表達(dá)形式。

2 基于四元數(shù)的手眼標(biāo)定求解

手眼標(biāo)定方程AX=XB可解耦為旋轉(zhuǎn)和平移2個部分。

RARX=RXRB

(RA-I)tx=RXtB-tA

(10)

式中R為3×3的旋轉(zhuǎn)矩陣，t為3×1的平移向量。

2.1 手眼標(biāo)定旋轉(zhuǎn)矩陣求取

根據(jù)單位四元數(shù)表達(dá)連續(xù)旋轉(zhuǎn)的特性，可將式(10)的等式一改寫為

pA?pX=pX?pB

(11)

式中pA，pX，pB分別為旋轉(zhuǎn)矩陣RA，RX，RB的四元數(shù)形式。結(jié)合上節(jié)所述式(11)可改寫為

(12)

單位四元數(shù)向量p=[e0,eT]T和歐拉參數(shù)[θ,uT]的關(guān)系可表示為

e0=cos(θ/2)

(13)

e=sin(θ/2)u

(14)

綜合式(13)、式(14)將式(12)改寫成矩陣形式

(15)

式中θ為機(jī)械臂末端移動旋轉(zhuǎn)分量的旋轉(zhuǎn)角度，可人為控制。uA,uB為旋轉(zhuǎn)矩陣RA和RB的旋轉(zhuǎn)軸向量

(16)

(17)

由式(17)即可求得pX特解。

2.2 手眼標(biāo)定平移向量求取

將四元數(shù)pX轉(zhuǎn)換成旋轉(zhuǎn)矩陣RX并代入式(10)中，且機(jī)械臂存在2次移動則

(RA1-I)tX=RXtB1-tA1,(RA2-I)tX=RXtB2-tA2

(18)

式(18)中僅有tX為未知量，則令

(19)

由最小二乘法可得

tx=(CTC)-1CTD

(20)

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

3.1 數(shù)值模擬實(shí)驗(yàn)

該實(shí)驗(yàn)的硬件平臺為CPU主頻為2.7 GHz,內(nèi)存為8 G的計算機(jī)，軟件平臺為MATLAB 2016a。

數(shù)值模擬分析的步驟如下：

1)隨機(jī)生成3×3正交矩陣RX和3×1向量tx，作為手眼矩陣X的真實(shí)值；

2)隨機(jī)生成正交矩陣RAi和tAi，i=1,2…N；

3)基于AX=XB求解RBi和tBi，i=1,2…N；

4)加入高斯噪聲，模擬實(shí)際測量數(shù)據(jù)，考慮到旋轉(zhuǎn)分量和平移分量的誤差水平不同，參考王君臣在文獻(xiàn)[3]的數(shù)據(jù)，對RAi和RBi加入均值為0，標(biāo)準(zhǔn)差為0.01d的高斯噪聲，其中d為噪聲水平；對tAi和tBi加入均值為0標(biāo)準(zhǔn)差為0.2d的高斯噪聲向量；

5)對包含噪聲數(shù)據(jù)的[RAitAi]和[RBitBi]分別使用文獻(xiàn)[9]中的基于矩陣直積的手眼標(biāo)定方法(matrix Kronecker-product hand-eye calibration method,KCM)和本文提出的QCM分別進(jìn)行手眼標(biāo)定。

為更好模擬實(shí)測數(shù)據(jù)，將上述標(biāo)定過程重復(fù)1 000次(取N=20)，每次加入同分布但不同數(shù)據(jù)的噪聲，標(biāo)定結(jié)果用相對旋轉(zhuǎn)誤差和相對平移誤差2個指標(biāo)評價,即

(21)

(22)

至此，完成噪聲水平為d的2種算法的相對誤差估計。令噪聲水平d=1,2,…,10，對每種噪聲水平重復(fù)上述步驟，可求出兩種算法在不同噪聲水平的估計誤差分布。模擬結(jié)果如圖1所示。

圖1 兩種誤差與噪聲水平的關(guān)系

圖1(a)和圖1(b)分別為兩種標(biāo)定算法的相對旋轉(zhuǎn)誤差和相對平移誤差隨噪聲水平變化的關(guān)系圖，從圖中可得，隨著噪聲水平的增大，兩種算法的相對平移誤差和相對旋轉(zhuǎn)誤差都隨之增大；在同等噪聲水平下，本文算法的誤差增長較低，穩(wěn)定性更好。

3.2 機(jī)器人手眼標(biāo)定實(shí)驗(yàn)

為了驗(yàn)證算法的有效性和精度，對機(jī)器人進(jìn)行手眼標(biāo)定實(shí)驗(yàn)。試驗(yàn)用機(jī)器人為自主研發(fā)的六自由度串聯(lián)機(jī)器人，相機(jī)為維視MV-VEM120SC工業(yè)相機(jī)。

圖2 機(jī)器人手眼標(biāo)定實(shí)驗(yàn)場景

實(shí)驗(yàn)過程如下：機(jī)械臂移動N次，通過攝像機(jī)標(biāo)定獲取標(biāo)定板與攝像機(jī)的N+1次位姿關(guān)系，通過機(jī)器人的控制系統(tǒng)獲取機(jī)械臂末端和機(jī)器人基坐標(biāo)的N+1次位姿關(guān)系。通過相鄰兩次位姿關(guān)系獲取矩陣Ai和Bi。

通過文獻(xiàn)[9]的KCM和本文算法(QCM)求取手眼矩陣X。誤差定義為

(23)

實(shí)驗(yàn)取N=10，機(jī)器人手眼標(biāo)定的誤差結(jié)果如下：本文算法的手眼標(biāo)定誤差為0.123 8，基于矩陣直積的手眼標(biāo)定誤差為0.133 9。結(jié)果可得本文提出的基于四元數(shù)的手眼標(biāo)定算法比基于矩陣直積的標(biāo)定手眼算法誤差低，標(biāo)定結(jié)果更精確。

4 結(jié) 論

1)本文利用四元數(shù)表示坐標(biāo)系在空間的旋轉(zhuǎn)運(yùn)動，基于SVD的線性優(yōu)化算法和單位四元數(shù)特性求解手眼關(guān)系的旋轉(zhuǎn)部分，代入平移部分等式基于最小二乘法求解手眼標(biāo)定的平移部分。

2)數(shù)值模擬結(jié)果表明:本文算法相較KCM對噪聲的敏感度低，穩(wěn)定性高；機(jī)器人實(shí)驗(yàn)結(jié)果表明，本文算法有較高的精度，可滿足機(jī)器人的應(yīng)用需求。