一種高精度的機器人手眼標定算法*

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

（1.沈陽航空航天大學航空航天工程學部，沈陽 110136；2.沈陽航空航天大學自動化學院，沈陽 110136）

0 引言

排爆機器人在軍事、反恐等領域具有重要的作用價值，該類機器人往往在機械手上安裝一臺相機用來對可疑物進行觀察，并為機械手進行定位。相機與機械手的位置轉換關系直接影響機器人的性能，因此，機器人手眼標定的研究具有重要意義。

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

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

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

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

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

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

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

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

在eye-in-hand系統(tǒng)中攝像機固定在機械手平臺上，跟隨機器人一起運動。圖1是平臺移動前后各個坐標系相對位置關系圖。

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

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

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

2 基于矩陣直積的手眼標定改進算法

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

2.1 矩陣直積與線性算子

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

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

首先定義線性算子vec并且利用線性算子將矩陣進行拉直運算，即將矩陣A的元素以長為mn的列向量表示：

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

當A或B是單位矩陣時，有：

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

2.2 Moore-penrose逆

且通解為：

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

2.3 基于矩陣直積手眼標定算法

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

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

為使手眼標定線性化，所以將式（11）、式（12）進行線性化，如式（14）所示。

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

2.4 基于矩陣直積的改進算法

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

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

針對式（16）、式（17）作如下變換，本處變化將不同于一般的矩陣直積，同時也是本改進算法的特點：

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

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

3 仿真實驗結果及數(shù)據(jù)分析

實驗平臺主要由：MOTOMAN機器人、機器人控制器、CM3-U3-13Y3M-CS工業(yè)相機、棋盤格標定靶、一臺計算機等組成。如圖2所示。

3.1 實驗數(shù)據(jù)

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

通過實驗可知，圖 3 中（a）、（b）、（c）3 個位置點所對應的機械手末端的位置和姿態(tài)信息分別是：

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

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

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

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

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

3.2 誤差分析

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

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

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

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

為保證求解的唯一性則需要機器人做兩次平臺運動，從而存在兩組攝像機坐標系以及機器人運動平臺坐標系的相對位置關系，所以在圖4、圖5中都分別會有兩組誤差分析圖。圖4中（a）、（b）是手眼矩陣中的旋轉矩陣未正交化的誤差分析圖。

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

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

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

表3 平均誤差分析

4 結論

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