基于四元數的機器人手眼標定算法*

王昌云， 李立君

(中南林業科技大學 機電工程學院，湖南 長沙 410004)

0 引 言

手眼標定的目的是求解機器人末端執行器坐標系到相機坐標系的變換矩陣，它是機器人完成手眼協調、自主導航等工作的基礎。Tsai R Y等人[1]、Shiu Y C等人[2]將手眼標定的方程歸結為AX=XB，并給出了其線性閉環解法，兩種方法復雜且推導繁瑣；王君臣等人[3]基于非線性優化解手眼標定方程，該方法精度高，但算法復雜且易陷入局部最優得不到最優解；文獻[4～9]用矩陣直積等數學工具求解手眼標定方程，算法結構簡單，精度有待提高。

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

1 四元數的概念

1.1 四元數乘法

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

(1)

(2)

(3)

令

(4)

則式(3)可簡寫為

γ=α+β=β-α

(5)

1.2 四元數與旋轉

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

(6)

(7)

(8)

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

pij=poj?pio

(9)

式(9)為四元數連續旋轉的表達形式。

2 基于四元數的手眼標定求解

手眼標定方程AX=XB可解耦為旋轉和平移2個部分。

RARX=RXRB

(RA-I)tx=RXtB-tA

(10)

式中R為3×3的旋轉矩陣，t為3×1的平移向量。

2.1 手眼標定旋轉矩陣求取

根據單位四元數表達連續旋轉的特性，可將式(10)的等式一改寫為

pA?pX=pX?pB

(11)

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

(12)

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

e0=cos(θ/2)

(13)

e=sin(θ/2)u

(14)

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

(15)

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

(16)

(17)

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

2.2 手眼標定平移向量求取

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

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

(18)

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

(19)

由最小二乘法可得

tx=(CTC)-1CTD

(20)

3 實驗與結果分析

3.1 數值模擬實驗

該實驗的硬件平臺為CPU主頻為2.7 GHz,內存為8 G的計算機，軟件平臺為MATLAB 2016a。

數值模擬分析的步驟如下：

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

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

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

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

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

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

(21)

(22)

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

圖1 兩種誤差與噪聲水平的關系

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

3.2 機器人手眼標定實驗

為了驗證算法的有效性和精度，對機器人進行手眼標定實驗。試驗用機器人為自主研發的六自由度串聯機器人，相機為維視MV-VEM120SC工業相機。

圖2 機器人手眼標定實驗場景

實驗過程如下：機械臂移動N次，通過攝像機標定獲取標定板與攝像機的N+1次位姿關系，通過機器人的控制系統獲取機械臂末端和機器人基坐標的N+1次位姿關系。通過相鄰兩次位姿關系獲取矩陣Ai和Bi。

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

(23)

實驗取N=10，機器人手眼標定的誤差結果如下：本文算法的手眼標定誤差為0.123 8，基于矩陣直積的手眼標定誤差為0.133 9。結果可得本文提出的基于四元數的手眼標定算法比基于矩陣直積的標定手眼算法誤差低，標定結果更精確。

4 結 論

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

2)數值模擬結果表明:本文算法相較KCM對噪聲的敏感度低，穩定性高；機器人實驗結果表明，本文算法有較高的精度，可滿足機器人的應用需求。