基于差分進化算法的手眼標定方法

金 軻，俞桂英，丁 燁，鄭建明

(1.上海交通大學機械與動力工程學院，上海 200240；2.浙江萬豐科技開發股份有限公司，浙江 嵊州 312400)

0 引言

機器人視覺系統廣泛地應用于各種特種機器人中，如飛行機器人[1-3]、輪式機器人[4]和水下機器人[5]等。在復雜的工作環境中，視覺系統能夠為機器人提供豐富的環境信息，指導機器人進行正確的決策，完成預定的動作。為了在統一的坐標框架下對視覺系統捕捉的圖像數據和機器人工作空間中的三維運動數據進行分析與融合，需要對機器人視覺系統進行手眼標定，以獲得視覺傳感器坐標系和機器人末端執行器坐標系之間的變換關系。

傳統的手眼標定方法[6-8]需要使用已知尺寸的標記物作為參考，分別測定標記物圖像數據和機器人末端位姿數據，通過三維空間內的幾何約束進行手眼標定求解。Park等[9]提出將手眼標定分為旋轉標定和平移標定2個部分，先通過最小二乘法進行旋轉標定，再使用旋轉標定的結果進行平移標定，但旋轉標定的誤差會傳遞到平移標定中。Daniilidis[10]基于對偶四元數理論設計求解算法，通過奇異值分解獲取標定結果，實現了旋轉部分和平移部分的同時標定，但算法的魯棒性不強，在噪聲的干擾下，計算可能不穩定。Strobl等[11]針對手眼標定優化問題提出了一種基于特殊歐氏群SE(3)的標定誤差度量方法，將手眼標定問題轉化為一個非線性優化問題，但求解過程中可能落入局部最優，對求解時的初值有較高的要求。Zhao[12]提出了一種基于凸優化的手眼標定方法，將非線性的優化求解問題轉化為L∞范數上的凸優化問題進行求解，有效地避免了局部最優問題，但所得的齊次變換矩陣可能不落在特殊歐氏群SE(3)上。Heller等[13]基于多元多項式重新構建了優化目標，并通過迭代進行求解，在考慮了SE(3)約束的同時，保證了所得解的全局最優性，但計算復雜度高。智向陽等[14]提出了一種實現簡單的凸優化手眼標定方法，為了解決最優解不滿足SE(3)約束的問題，通過QR分解對最優解的旋轉矩陣部分進行了正交化，但沒有嚴格地證明正交化后的結果具有全局最優性。

本文針對眼在手機器人視覺系統，提出了一種基于差分進化算法[15]的手眼標定方法。通過李群李代數理論，構建系統手眼標定誤差函數。通過差分進化算法對優化問題進行求解，避免了局部最優問題。該方法計算簡單，易于實現，在考慮了SE(3)約束的同時，能夠保證所得解的全局最優性。

1 手眼標定問題描述

在典型的眼在手機器人視覺系統中，相機固定于機器人末端并隨機器人運動。為了統一地分析機器人工作空間中的三維運動數據與圖像空間中的圖像特征數據，需要通過手眼標定確定相機坐標系到機器人末端坐標系之間的變換矩陣。一般的手眼標定方法需要在世界坐標系中固定一個標定板，如圖1所示。將機器人末端執行器移動到不同位置處，采集機器人末端位姿以及對應位置處相機捕捉到的標定板圖片，進而計算從相機坐標系{C}到機器人末端坐標系{E}的變換矩陣。

圖1 手眼標定示意

(1)

(2)

AX=XB

(3)

手眼標定的目的是在已知A和B的條件下求解齊次變換矩陣X。由于實際工程應用中采集到的機器人末端位姿數據和標定板圖片數據中存在噪聲，求解時使用的齊次變換矩陣A和B并不準確。為了獲得較為精確的求解結果，一般采集多組數據進行標定。設標定過程中共采集了n組齊次變換矩陣Ai和Bi(i=1,2，…,n)，選擇合適的定義于SE(3)上的誤差函數d(·,·)，可將手眼標定過程轉化為求解如下優化問題：

(4)

2 基于李群李代數的手眼標定求解

利用位姿變換矩陣的性質，式(3)可改寫為

(5)

RA，RB和RX分別為齊次變換矩陣A，B和X對應的旋轉矩陣；bA，bB和bX分別為A，B和X對應的平移向量。對式(5)進行推導可得：

RARX=RXRB

(6)

RAbX+bA=RXbB+bX

(7)

式(6)和式(7)分別表示式(3)的旋轉部分和平移部分，求解式(3)中的矩陣X等價于求解旋轉矩陣RX和平移向量bX。對于采集了多組齊次變換矩陣的手眼標定問題，需要尋求有效的誤差函數，以獲取最優的RX和bX。

首先考慮通過式(6)獲取旋轉分量的誤差函數，在式(6)中，矩陣RA，RB和RX均為特殊正交群SO(3)上的元素。利用旋轉矩陣的性質對式(6)進行推導可得

RA=RXRBRXT

(8)

由于RA和RXRBRXT位于SO(3)中，對其進行對數運算可以獲得對應的李代數，結合矩陣對數的性質可以推導得

1n(RA)=RX1n(RB)RXT

(9)

由旋轉矩陣的性質可知，1n(RA)和ln(RB)均為反對稱矩陣。對任意的三維向量ω，將其對應的反對稱矩陣記為[ω]×，代入式(9)可得

[α]×=RX[β]×RXT

(10)

α和β分別為1n(RA)和1n(RB)對應的三維向量。結合反對稱矩陣的性質可得

[α]×=[RXβ]×

(11)

對于任意的反對稱矩陣，其對應的三維向量是唯一的。因此式(11)可改寫為

α=RXβ

(12)

式(11)與式(12)等價，描述了手眼標定求解過程中旋轉分量上的約束關系。當使用多組齊次變換矩陣Ai和Bi(i=1,2,…,n)進行手眼標定時，定義手眼標定在旋轉分量上的誤差函數為

(13)

αi和βi∈R3,分別為齊次變換矩陣Ai和Bi的旋轉矩陣所對應的李代數。

對于式(7)所示的手眼標定求解的平移分量，定義手眼標定在平移分量上的誤差函數為

(14)

RAi∈SO(3)，表示齊次變換矩陣Ai對應的旋轉矩陣；bAi,bBi∈R3，分別表示齊次變換矩陣Ai和Bi對應的平移向量。記γ∈R3為旋轉矩陣RX對應的李代數，定義手眼標定問題的決策向量x∈R6為

(15)

定義手眼標定問題對應的目標函數為

f(x)=Er(x)+λEt(x)

(16)

λ為權重系數。則手眼標定問題的數學模型可以表示為

(17)

對于任意的三維向量，在SO(3)上都有唯一的元素與其對應。當計算得到最優解x*時，對應的旋轉矩陣RX和平移向量bX也唯一確定。該優化問題的構造形式保證了通過求解式能夠獲得位于SE(3)上的齊次變換矩陣X，而不需要附加額外的約束條件。

3 差分進化算法

為了求解式(17)所示的優化問題，采用差分進化算法計算最優的齊次變換矩陣X。差分進化算法是一種隨機啟發式搜索方法，擁有較強的全局搜索能力和魯棒性，能夠避免在搜索最優解的過程中落入局部最優。差分進化算法的基本思想是模擬生物進化規律進行搜索迭代，算法實現包含如下步驟。

a.隨機生成初代種群。在6維空間中，設定各個維度的上界和下界，隨機產生M個符合約束條件的6維向量xi(i=1,2,…,M)。

b.變異操作。設當前種群處于第t代，對于下一代的變異個體hi(t+1)，其產生的方式為

hi(t+1)=xp1(t)+F(xp2(t)-xp3(t))

(18)

c.交叉操作。對變異個體hi(t+1)進行交叉，生成新的待選個體vi(t+1)，具體的操作為

(19)

vij(t+1)和xij(t)分別為vi(t+1)和xi(t)的第j個分量(j=1,2,…,6);r為區間[0,1]內的隨機數;CR∈[0,1]為交叉因子。

d.選擇操作。設置算法的評價函數為手眼標定問題的目標函數f(x)，通過評價函數確定差分進化方向，生成第t+1代個體：

(20)

定義第t代種群中對應評價函數值最小的個體為第t代的最優個體。在產生新一代種群后，若迭代達到最大迭代次數，則停止迭代，否則重新執行步驟b到步驟d。迭代結束后，末代的最優個體即為差分進化算法所獲得的優化問題最優解。

4 實驗驗證

為了驗證本文所提出的基于差分進化算法的手眼標定方法(以下簡稱為差分進化算法)的有效性，搭建了機器人相機實驗平臺進行手眼標定實驗驗證，如圖2所示。實驗使用的機器人為UR10六自由度工業機器人，使用的相機為大恒MER-131-75GM/C單目工業相機，相機通過連接件固連于機器人末端執行器。采用的標定板為8×7柵格的棋盤格標定板，每個棋盤格的邊長為25 mm，如圖3所示。

圖2 實驗平臺 圖3 采樣的標定板圖像

(21)

使用前18組齊次變換矩陣AN和BN(N=1,2,…,18)進行手眼標定，確定相機坐標系到機器人末端坐標系之間的變換矩陣X。使用后6組齊次變換矩陣AN和BN(N=19,20,…,24)對算法求得的變換矩陣X進行驗證。在確定手眼標定目標函數時，設定權重系數λ=1。在使用差分進化算法求解時，根據推薦的參數選取方式[15]，設定個體數量M=30，變異因子F=0.5，交叉因子CR=0.9，最大迭代次數為100。根據先驗知識，設定種群的上界向量xU和下界向量xL為

(22)

計算得到的齊次變換矩陣X為

(23)

為了驗證該方法的有效性，以Zhao[12]提出的正交矩陣形式凸優化方法(以下簡稱為凸優化方法)進行對比。定義變換矩陣AN(N=19,20,…,24)的估計值為

(24)

相應地，定義估計AN時的平移誤差和旋轉誤差為:

(25)

(26)

圖4 不同樣本的平移誤差 圖5 不同樣本的旋轉誤差

5 結束語

利用李群李代數理論，將手眼標定問題轉化為針對誤差函數的優化問題，通過對目標函數使用差分進化算法，實現優化問題的求解。借助李代數與李群的對應特性，該方法能夠保證求解得到的齊次變換矩陣X落在SE(3)上。利用差分進化算法，避免了該方法在求解過程中落入局部最優。最后搭建手眼標定實驗平臺，與其他方法進行對比實驗。實驗結果表明，本文所述方法在平移誤差估計和旋轉誤差估計上均能達到較高的精度，驗證了本文所述方法的可行性和有效性。