胡 為，劉 沖，傅 莉，陳新禹

（1.沈陽航空航天大學(xué)航空航天工程學(xué)部，沈陽 110136；2.沈陽航空航天大學(xué)自動(dòng)化學(xué)院，沈陽 110136）

0 引言

排爆機(jī)器人在軍事、反恐等領(lǐng)域具有重要的作用價(jià)值，該類機(jī)器人往往在機(jī)械手上安裝一臺(tái)相機(jī)用來對(duì)可疑物進(jìn)行觀察，并為機(jī)械手進(jìn)行定位。相機(jī)與機(jī)械手的位置轉(zhuǎn)換關(guān)系直接影響機(jī)器人的性能，因此，機(jī)器人手眼標(biāo)定的研究具有重要意義。

本文以軍用排爆機(jī)器人為研究對(duì)象，此類機(jī)器人將相機(jī)固定安裝于其機(jī)械手末端用以觀察可疑物，所構(gòu)成的手眼系統(tǒng)為系統(tǒng)[1]。通過研究可知，該類手眼系統(tǒng)手眼標(biāo)定的求解過程就是求解齊次矩陣方程的過程。目前來看求解該方程的方法主要分為兩大類：線性方法和非線性方法。其中線性方法為：

傳統(tǒng)兩步法[2-3]：將旋轉(zhuǎn)矩陣和平移向量進(jìn)行解耦，該方法需要已知攝像機(jī)的內(nèi)部參數(shù)。該算法的缺點(diǎn)是旋轉(zhuǎn)矩陣正交化后再求平移部分，而平移解不再滿足手眼矩陣。

共同標(biāo)定法：同時(shí)進(jìn)行攝像機(jī)標(biāo)定和手眼標(biāo)定。但是該方法忽略了攝像機(jī)的畸變，系統(tǒng)的精度將無法保證。例如Ma[4]提出的無標(biāo)定物的主動(dòng)視覺算法，就是通過機(jī)械手末端平臺(tái)帶動(dòng)攝像機(jī)運(yùn)動(dòng)獲取數(shù)據(jù)參數(shù)，利用極點(diǎn)建立約束方程組，求出攝像機(jī)的內(nèi)部參數(shù)以及手眼標(biāo)定矩陣。

數(shù)學(xué)法[5-6]：通過使用特殊的數(shù)學(xué)工具如四元數(shù)、矩陣直積、拉直運(yùn)算等數(shù)學(xué)工具來求解手眼關(guān)系。該方法的優(yōu)點(diǎn)就是不用對(duì)旋轉(zhuǎn)平移進(jìn)行解耦，相對(duì)兩步法精度要高、魯棒性強(qiáng)等優(yōu)點(diǎn)。又如另外一種算法是利用直積將手眼矩陣轉(zhuǎn)換成線性方程組的形式，但是在求解手眼矩陣過程中將會(huì)舍去一個(gè)方程，使得該算法在測(cè)量噪聲明顯的情況下存在不穩(wěn)定的情況[7-9]；

運(yùn)動(dòng)限制法：該算法就是限制機(jī)器人做相對(duì)正交與平移的運(yùn)動(dòng)來實(shí)現(xiàn)求解手眼矩陣的目的。在實(shí)現(xiàn)方面相對(duì)比較困難，需要機(jī)器人本身的運(yùn)動(dòng)精度高且重復(fù)精度也高，同時(shí)可以實(shí)現(xiàn)一些特殊運(yùn)動(dòng)才能采用該方法[10-13]。

非線性方法[14]：Ruda采用數(shù)學(xué)工具四元數(shù)求出初值，然后利用非線性優(yōu)化得出最終解。王君臣提出一種基于最大似然估計(jì)的非線性最優(yōu)手眼標(biāo)定算法等[15]。由于本類方法要求較高，實(shí)現(xiàn)相對(duì)復(fù)雜、實(shí)用性相對(duì)較弱的特點(diǎn)，很少引起學(xué)者的注意，在實(shí)際應(yīng)用中并不多見。

鑒于上文提出的線性方法以及非線性方法存在的不足之處，本文提出了一種基于矩陣直積的手眼標(biāo)定改進(jìn)算法，該算法可以實(shí)現(xiàn)一步化求解、避免已有算法存在的誤差傳遞等問題。本算法將對(duì)求取的旋轉(zhuǎn)矩陣進(jìn)行正交單位化處理從而消除外部噪聲的干擾。實(shí)驗(yàn)結(jié)果表明：改進(jìn)算法較另外兩種現(xiàn)有算法有著更強(qiáng)的魯棒性和更小的標(biāo)定誤差，滿足工程需求，應(yīng)用廣泛。

1 手眼標(biāo)定傳統(tǒng)算法

在eye-in-hand系統(tǒng)中攝像機(jī)固定在機(jī)械手平臺(tái)上，跟隨機(jī)器人一起運(yùn)動(dòng)。圖1是平臺(tái)移動(dòng)前后各個(gè)坐標(biāo)系相對(duì)位置關(guān)系圖。

Cobj是物體坐標(biāo)系，Cc1、Cc2是攝像機(jī)運(yùn)動(dòng)前后對(duì)應(yīng)的坐標(biāo)系，Ce1、Ce2是機(jī)械手平臺(tái)運(yùn)動(dòng)前后對(duì)應(yīng)的坐標(biāo)系，攝像機(jī)外參數(shù)標(biāo)定可以求得攝像機(jī)坐標(biāo)系與物體坐標(biāo)系之間的關(guān)系，用Ra、ta表示。攝像機(jī)坐標(biāo)系與機(jī)器手平臺(tái)坐標(biāo)系之間的關(guān)系用t、R表示，即CX=XD中的X。求取X的過程就是機(jī)器人手眼標(biāo)定的過程。圖1中的相對(duì)位置關(guān)系A(chǔ)、B、C、D、X都是4×4齊次矩陣，如：

將CX=XD方程中的各4×4矩陣分別用相應(yīng)的旋轉(zhuǎn)矩陣以及平移向量寫出，并展開得到式（1）：

為確保式（1）的解唯一則至少需要兩組數(shù)據(jù)，所以將式（1）進(jìn)行兩次變換處理得到式（2）：

2 基于矩陣直積的手眼標(biāo)定改進(jìn)算法

本節(jié)將介紹基于矩陣直積的手眼標(biāo)定算法及其改進(jìn)算法，該類方法將借用矩陣直積和矩陣的特征向量等數(shù)學(xué)工具對(duì)式（1）進(jìn)行變換求解。

2.1 矩陣直積與線性算子

為了對(duì)手眼矩陣進(jìn)行線性化，將采用矩陣直積以及線性算子對(duì)式（2）進(jìn)行變換求解。如果存在m×n矩陣A和p×q矩陣B：

則A和B的直積A?B為mp×nq矩陣：

首先定義線性算子vec并且利用線性算子將矩陣進(jìn)行拉直運(yùn)算，即將矩陣A的元素以長(zhǎng)為mn的列向量表示：

設(shè)A是m×n的矩陣，B是n×n的矩陣，X是m×n的矩陣，由矩陣直積和線性算子vec的定義可知：

當(dāng)A或B是單位矩陣時(shí)，有：

設(shè)a和b為常數(shù)，則有：

2.2 Moore-penrose逆

且通解為：

由式（8）可知，如果線性方程組Ax=b有解，則當(dāng)且僅當(dāng)A+A=I。在實(shí)際問題中，需求出線性方程組的無窮多個(gè)解中范數(shù)最小的解，即，稱x0為線性方程組Ax=b的極小范數(shù)解。

2.3 基于矩陣直積手眼標(biāo)定算法

用矩陣直積和線性算子vec對(duì)式（2）進(jìn)行的變換求解可得到式（11）、式（12）。

根據(jù)式（11）可知，該式系數(shù)矩陣存在特征值為零的特征向量，該向量與齊次方程的解相差一個(gè)比例因子。令特征值零所對(duì)應(yīng)的特征向量為γ（γ是9×1的列矩陣），比例因子為常量β，將特征向量與比例因子引入式（12）中，vec（R）、t所對(duì)應(yīng)的系數(shù)如式（13）所示。

為使手眼標(biāo)定線性化，所以將式（11）、式（12）進(jìn)行線性化，如式（14）所示。

從以上兩種算法可知，現(xiàn)有矩陣直積手眼標(biāo)定算法與傳統(tǒng)兩步法存在相似之處。在一定意義上都是兩步法，所以仍將產(chǎn)生誤差傳遞，從而導(dǎo)致手眼標(biāo)定精度降低。

2.4 基于矩陣直積的改進(jìn)算法

接下來將提出手眼標(biāo)定改進(jìn)算法，利用矩陣直積和線性算子vec對(duì)式（1）進(jìn)行變換求解。如式（15）所示：

為確保手眼矩陣解的唯一性則至少需要旋轉(zhuǎn)軸做兩組不平行的運(yùn)動(dòng)，因此，在做兩次運(yùn)動(dòng)的過程中將會(huì)得到兩個(gè)形如式（15）的兩個(gè)等式。根據(jù)得到的兩組方程式分別進(jìn)行聯(lián)合：

針對(duì)式（16）、式（17）作如下變換，本處變化將不同于一般的矩陣直積，同時(shí)也是本改進(jìn)算法的特點(diǎn)：

式（11）～ 式（18）中 o9×3為 9×3的全零矩陣、o9是9×1的全零矩陣、I3為3×3的單位矩陣。

式（18）是形如Ax=b的方程組，根據(jù)矛盾方程組的性質(zhì)，該方程組左邊的系數(shù)矩陣為不可逆矩陣，在求解該矛盾方程組的解時(shí)將用到廣義逆的思想。所以需要求取方程組的極小范數(shù)最小二乘解，則該方程組的極小范數(shù)最小二乘解X就等于A+b。將旋轉(zhuǎn)矩陣與平移向量進(jìn)行耦合，將會(huì)在計(jì)算過程中減少系統(tǒng)噪聲帶來的誤差影響。對(duì)極小范數(shù)最小二乘解進(jìn)行重排，將得到手眼標(biāo)定矩陣中的旋轉(zhuǎn)矩陣以及平移向量。最后對(duì)旋轉(zhuǎn)矩陣進(jìn)行斯密特正交單位化處理，標(biāo)定結(jié)果滿足標(biāo)準(zhǔn)正交化。

3 仿真實(shí)驗(yàn)結(jié)果及數(shù)據(jù)分析

實(shí)驗(yàn)平臺(tái)主要由：MOTOMAN機(jī)器人、機(jī)器人控制器、CM3-U3-13Y3M-CS工業(yè)相機(jī)、棋盤格標(biāo)定靶、一臺(tái)計(jì)算機(jī)等組成。如圖2所示。

3.1 實(shí)驗(yàn)數(shù)據(jù)

在內(nèi)部參數(shù)已知的情況下，求出攝像機(jī)兩次運(yùn)動(dòng)所拍攝圖片的3個(gè)相應(yīng)外部參數(shù)。圖3所示為攝像機(jī)運(yùn)動(dòng)兩次在3個(gè)不同位置點(diǎn)拍攝的靶標(biāo)圖像，根據(jù)該組圖像求出相應(yīng)的攝像機(jī)外部參數(shù)和機(jī)械手平臺(tái)在該處的位置與姿態(tài)。

通過實(shí)驗(yàn)可知，圖 3 中（a）、（b）、（c）3 個(gè)位置點(diǎn)所對(duì)應(yīng)的機(jī)械手末端的位置和姿態(tài)信息分別是：

pos0、pos1以及pos2位置姿態(tài)信息的矩陣形式可通過旋轉(zhuǎn)角度與旋轉(zhuǎn)矩陣之間的變換關(guān)系求得，如式（22）～式（25）所示。

式（22）～ 式（24）中 q、w、r分別表示 pos0、pos1以及pos2中各旋轉(zhuǎn)軸的旋轉(zhuǎn)弧度，Rx、Ry以及Rz代表笛卡爾空間各旋轉(zhuǎn)軸的旋轉(zhuǎn)矩陣，R代表旋轉(zhuǎn)向量所對(duì)應(yīng)的旋轉(zhuǎn)矩陣。根據(jù)攝像機(jī)在位置點(diǎn)（a）、（b）以及（c）處所拍攝3張圖片信息，可利用攝像機(jī)標(biāo)定來求得外部參數(shù)。

由圖3中3張圖片所對(duì)應(yīng)的外部參數(shù)以及機(jī)械手姿態(tài)信息進(jìn)行手眼標(biāo)定。根據(jù)傳統(tǒng)兩步法求出的未正交化處理的手眼矩陣Xtwo-step以及正交單位化處理之后的機(jī)器人手眼矩陣Xtwo-step1，如式（26）、式（27）所示。

同理，基于矩陣直積算法求取的未正交化處理機(jī)器人手眼矩陣為Xkronecker以及正交單位化的手眼矩陣Xkronecker1。本文提出的改進(jìn)算法所對(duì)應(yīng)的未正交化手眼矩陣與正交單位化之后的手眼矩陣分別為Xchange、Xchange1，如式（28）～ 式（31）所示。

在下節(jié)將針對(duì)本組實(shí)驗(yàn)數(shù)據(jù)以及其余13組數(shù)據(jù)進(jìn)行誤差分析，根據(jù)分析結(jié)果來驗(yàn)證基于矩陣直積改進(jìn)算法的標(biāo)定精度。

3.2 誤差分析

本節(jié)將根據(jù)實(shí)驗(yàn)數(shù)據(jù)進(jìn)行標(biāo)定精度分析并采用范數(shù)來檢驗(yàn)重復(fù)誤差，即將3種算法求取的手眼矩陣代入進(jìn)行誤差分析。3種算法的誤差比較如表1、表2所示。

表1 第1組未正交化手眼矩陣誤差分析

表2 第1組正交單位化手眼矩陣誤差分析

表1 中 Xtwo-step、Xkronecker、Xchange分別代表兩步法、矩陣直積法以及改進(jìn)算法的手眼矩陣未正交化時(shí)的誤差。Xtwo-step1、Xkronecker1、Xchange1分別是對(duì)應(yīng)的3種算法手眼矩陣正交單位化之后的誤差。通過表1、表2可知，基于矩陣直積改進(jìn)算法同現(xiàn)有的另外兩種算法相較而言標(biāo)定精度最高。為準(zhǔn)確驗(yàn)證改進(jìn)算法的標(biāo)定精度，本節(jié)將給出多組實(shí)驗(yàn)結(jié)果并作分析。

為保證求解的唯一性則需要機(jī)器人做兩次平臺(tái)運(yùn)動(dòng)，從而存在兩組攝像機(jī)坐標(biāo)系以及機(jī)器人運(yùn)動(dòng)平臺(tái)坐標(biāo)系的相對(duì)位置關(guān)系，所以在圖4、圖5中都分別會(huì)有兩組誤差分析圖。圖4中（a）、（b）是手眼矩陣中的旋轉(zhuǎn)矩陣未正交化的誤差分析圖。

通過圖4可知，傳統(tǒng)兩步法和一般性矩陣直積手眼標(biāo)定算法魯棒性較差。未對(duì)手眼矩陣中的旋轉(zhuǎn)矩陣正交之前，改進(jìn)算法表現(xiàn)出很高的魯棒性且誤差結(jié)果趨近于零。對(duì)手眼矩陣進(jìn)行正交單位化處理并加以誤差分析，如圖5所示。

圖5中基于矩陣直積算法表現(xiàn)出極不穩(wěn)定的狀態(tài)，其次是傳統(tǒng)的兩步法。結(jié)合圖4、圖5可知，未對(duì)手眼標(biāo)定結(jié)果歸一化之前改進(jìn)算法的標(biāo)定精度非常高且接近于零。在正交單位化之后，改進(jìn)算法的標(biāo)定誤差結(jié)果依然是最理想且滿足手眼標(biāo)定精度要求。下頁表3將給出更加直觀的平均誤差的分析數(shù)據(jù)，標(biāo)定誤差結(jié)果如表3所示。

表3中Xerror、X正交單位化error分別表示手眼矩陣中的旋轉(zhuǎn)矩陣未進(jìn)行正交化和正交單位化后的平均誤差。通過表3中的平均誤差分析可知，改進(jìn)的機(jī)器人手眼標(biāo)定算法誤差最小且精度最高。

表3 平均誤差分析

4 結(jié)論

通過多組實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證了基于矩陣直積改進(jìn)算法的魯棒性很高。從兩步法和數(shù)學(xué)法這個(gè)角度分析，數(shù)學(xué)法在手眼標(biāo)定方面精度要高于傳統(tǒng)的兩步法。其次，改進(jìn)算法采用的線性化處理的計(jì)算精度要高于非線性的算法。非線性化的兩步法誤差較大的原因很大方面來自于旋轉(zhuǎn)矩陣的誤差傳遞。一般性的基于矩陣直積算法并未實(shí)現(xiàn)真正的一步法，通過本文介紹的一般性矩陣直積算法可知，求解比例因子以及特征向量都會(huì)產(chǎn)生誤差傳遞。實(shí)驗(yàn)證明：本文提出的一步化求解的改進(jìn)算法在標(biāo)定過程中具有誤差幅度小，魯棒性強(qiáng)等特點(diǎn)。