基于差分進(jìn)化算法的手眼標(biāo)定方法

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

(1.上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院，上海 200240；2.浙江萬(wàn)豐科技開(kāi)發(fā)股份有限公司，浙江 嵊州 312400)

0 引言

機(jī)器人視覺(jué)系統(tǒng)廣泛地應(yīng)用于各種特種機(jī)器人中，如飛行機(jī)器人[1-3]、輪式機(jī)器人[4]和水下機(jī)器人[5]等。在復(fù)雜的工作環(huán)境中，視覺(jué)系統(tǒng)能夠?yàn)闄C(jī)器人提供豐富的環(huán)境信息，指導(dǎo)機(jī)器人進(jìn)行正確的決策，完成預(yù)定的動(dòng)作。為了在統(tǒng)一的坐標(biāo)框架下對(duì)視覺(jué)系統(tǒng)捕捉的圖像數(shù)據(jù)和機(jī)器人工作空間中的三維運(yùn)動(dòng)數(shù)據(jù)進(jìn)行分析與融合，需要對(duì)機(jī)器人視覺(jué)系統(tǒng)進(jìn)行手眼標(biāo)定，以獲得視覺(jué)傳感器坐標(biāo)系和機(jī)器人末端執(zhí)行器坐標(biāo)系之間的變換關(guān)系。

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

本文針對(duì)眼在手機(jī)器人視覺(jué)系統(tǒng)，提出了一種基于差分進(jìn)化算法[15]的手眼標(biāo)定方法。通過(guò)李群李代數(shù)理論，構(gòu)建系統(tǒng)手眼標(biāo)定誤差函數(shù)。通過(guò)差分進(jìn)化算法對(duì)優(yōu)化問(wèn)題進(jìn)行求解，避免了局部最優(yōu)問(wèn)題。該方法計(jì)算簡(jiǎn)單，易于實(shí)現(xiàn)，在考慮了SE(3)約束的同時(shí)，能夠保證所得解的全局最優(yōu)性。

1 手眼標(biāo)定問(wèn)題描述

在典型的眼在手機(jī)器人視覺(jué)系統(tǒng)中，相機(jī)固定于機(jī)器人末端并隨機(jī)器人運(yùn)動(dòng)。為了統(tǒng)一地分析機(jī)器人工作空間中的三維運(yùn)動(dòng)數(shù)據(jù)與圖像空間中的圖像特征數(shù)據(jù)，需要通過(guò)手眼標(biāo)定確定相機(jī)坐標(biāo)系到機(jī)器人末端坐標(biāo)系之間的變換矩陣。一般的手眼標(biāo)定方法需要在世界坐標(biāo)系中固定一個(gè)標(biāo)定板，如圖1所示。將機(jī)器人末端執(zhí)行器移動(dòng)到不同位置處，采集機(jī)器人末端位姿以及對(duì)應(yīng)位置處相機(jī)捕捉到的標(biāo)定板圖片，進(jìn)而計(jì)算從相機(jī)坐標(biāo)系{C}到機(jī)器人末端坐標(biāo)系{E}的變換矩陣。

圖1 手眼標(biāo)定示意

(1)

(2)

AX=XB

(3)

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

(4)

2 基于李群李代數(shù)的手眼標(biāo)定求解

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

(5)

RA，RB和RX分別為齊次變換矩陣A，B和X對(duì)應(yīng)的旋轉(zhuǎn)矩陣；bA，bB和bX分別為A，B和X對(duì)應(yīng)的平移向量。對(duì)式(5)進(jìn)行推導(dǎo)可得：

RARX=RXRB

(6)

RAbX+bA=RXbB+bX

(7)

式(6)和式(7)分別表示式(3)的旋轉(zhuǎn)部分和平移部分，求解式(3)中的矩陣X等價(jià)于求解旋轉(zhuǎn)矩陣RX和平移向量bX。對(duì)于采集了多組齊次變換矩陣的手眼標(biāo)定問(wèn)題，需要尋求有效的誤差函數(shù)，以獲取最優(yōu)的RX和bX。

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

RA=RXRBRXT

(8)

由于RA和RXRBRXT位于SO(3)中，對(duì)其進(jìn)行對(duì)數(shù)運(yùn)算可以獲得對(duì)應(yīng)的李代數(shù)，結(jié)合矩陣對(duì)數(shù)的性質(zhì)可以推導(dǎo)得

1n(RA)=RX1n(RB)RXT

(9)

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

[α]×=RX[β]×RXT

(10)

α和β分別為1n(RA)和1n(RB)對(duì)應(yīng)的三維向量。結(jié)合反對(duì)稱矩陣的性質(zhì)可得

[α]×=[RXβ]×

(11)

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

α=RXβ

(12)

式(11)與式(12)等價(jià)，描述了手眼標(biāo)定求解過(guò)程中旋轉(zhuǎn)分量上的約束關(guān)系。當(dāng)使用多組齊次變換矩陣Ai和Bi(i=1,2,…,n)進(jìn)行手眼標(biāo)定時(shí)，定義手眼標(biāo)定在旋轉(zhuǎn)分量上的誤差函數(shù)為

(13)

αi和βi∈R3,分別為齊次變換矩陣Ai和Bi的旋轉(zhuǎn)矩陣所對(duì)應(yīng)的李代數(shù)。

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

(14)

RAi∈SO(3)，表示齊次變換矩陣Ai對(duì)應(yīng)的旋轉(zhuǎn)矩陣；bAi,bBi∈R3，分別表示齊次變換矩陣Ai和Bi對(duì)應(yīng)的平移向量。記γ∈R3為旋轉(zhuǎn)矩陣RX對(duì)應(yīng)的李代數(shù)，定義手眼標(biāo)定問(wèn)題的決策向量x∈R6為

(15)

定義手眼標(biāo)定問(wèn)題對(duì)應(yīng)的目標(biāo)函數(shù)為

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

(16)

λ為權(quán)重系數(shù)。則手眼標(biāo)定問(wèn)題的數(shù)學(xué)模型可以表示為

(17)

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

3 差分進(jìn)化算法

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

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

b.變異操作。設(shè)當(dāng)前種群處于第t代，對(duì)于下一代的變異個(gè)體hi(t+1)，其產(chǎn)生的方式為

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

(18)

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

(19)

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

d.選擇操作。設(shè)置算法的評(píng)價(jià)函數(shù)為手眼標(biāo)定問(wèn)題的目標(biāo)函數(shù)f(x)，通過(guò)評(píng)價(jià)函數(shù)確定差分進(jìn)化方向，生成第t+1代個(gè)體：

(20)

定義第t代種群中對(duì)應(yīng)評(píng)價(jià)函數(shù)值最小的個(gè)體為第t代的最優(yōu)個(gè)體。在產(chǎn)生新一代種群后，若迭代達(dá)到最大迭代次數(shù)，則停止迭代，否則重新執(zhí)行步驟b到步驟d。迭代結(jié)束后，末代的最優(yōu)個(gè)體即為差分進(jìn)化算法所獲得的優(yōu)化問(wèn)題最優(yōu)解。

4 實(shí)驗(yàn)驗(yàn)證

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

圖2 實(shí)驗(yàn)平臺(tái) 圖3 采樣的標(biāo)定板圖像

(21)

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

(22)

計(jì)算得到的齊次變換矩陣X為

(23)

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

(24)

相應(yīng)地，定義估計(jì)AN時(shí)的平移誤差和旋轉(zhuǎn)誤差為:

(25)

(26)

圖4 不同樣本的平移誤差 圖5 不同樣本的旋轉(zhuǎn)誤差

5 結(jié)束語(yǔ)

利用李群李代數(shù)理論，將手眼標(biāo)定問(wèn)題轉(zhuǎn)化為針對(duì)誤差函數(shù)的優(yōu)化問(wèn)題，通過(guò)對(duì)目標(biāo)函數(shù)使用差分進(jìn)化算法，實(shí)現(xiàn)優(yōu)化問(wèn)題的求解。借助李代數(shù)與李群的對(duì)應(yīng)特性，該方法能夠保證求解得到的齊次變換矩陣X落在SE(3)上。利用差分進(jìn)化算法，避免了該方法在求解過(guò)程中落入局部最優(yōu)。最后搭建手眼標(biāo)定實(shí)驗(yàn)平臺(tái)，與其他方法進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，本文所述方法在平移誤差估計(jì)和旋轉(zhuǎn)誤差估計(jì)上均能達(dá)到較高的精度，驗(yàn)證了本文所述方法的可行性和有效性。