基于凸松弛優(yōu)化算法的機(jī)器人手眼標(biāo)定

張亞如，杜建賓,2

（1.廊坊師范學(xué)院，河北 廊坊 065000；2.天津大學(xué)，天津 300072）

0 引言

1962 年世界上第一臺MArobotUNIMATE 誕生以來，隨著科學(xué)技術(shù)的迅猛發(fā)展，機(jī)器人已經(jīng)逐步進(jìn)入我們的社會生活中，其中研制機(jī)器人成為國家或地區(qū)科技水平體現(xiàn)的重要方面［1-2］。機(jī)器人視覺處理的一項基本目標(biāo)就是手眼標(biāo)定，目的是估計變換矩陣，這個矩陣是指機(jī)器人末端執(zhí)行器上搭載的相機(jī)坐標(biāo)系到執(zhí)行坐標(biāo)系，而且是許多高級機(jī)器人的基本任務(wù)，因此手眼標(biāo)定的研究非常重要。

自1989 年Tsai［3］和Shiu［4］首次提出手眼標(biāo)定問題以來，國內(nèi)外研究學(xué)者對此問題做了大量研究，根據(jù)標(biāo)定矩陣的求解順序可以將手眼標(biāo)定問題的求解算法分為兩大類。第一類是先解決旋轉(zhuǎn)矩陣再找出平移向量：如1991 年，Chou［5］提出一種基于單位四元數(shù)的閉環(huán)線性解法；1994年，Park［6］結(jié)合歐氏運(yùn)動群SO（3），提出一種改進(jìn)的方程式求解算法；2008 年，Malti［7］針對醫(yī)用內(nèi)窺鏡輔助治療中的手眼標(biāo)定問題，提出一種基于對偶四元數(shù)理論的手眼標(biāo)定矩陣求解方法；2015年，王金橋［8］提出利用遺傳算法解決關(guān)節(jié)臂視覺檢測系統(tǒng)中的手眼標(biāo)定問題。這類算法適用于旋轉(zhuǎn)矩陣估計精度較高的情況。第二類是旋轉(zhuǎn)矩陣和平移向量同時求解：如1999年，Daniilidis［9］提出基于對偶四元數(shù)和螺旋理論的手眼標(biāo)定方程求解算法；Andreff［10］針對標(biāo)定小范圍移動的測量場景，提出一種基于矩陣直積的手眼標(biāo)定方程閉環(huán)解法；2011 年，Zhao［11］提出一種基于∞-范數(shù)優(yōu)化模型的手眼標(biāo)定算法；李巍［12］結(jié)合RANSAC 算法對夾角預(yù)定數(shù)據(jù)進(jìn)行預(yù)篩選，利用凸松弛全局優(yōu)化算法來解決這個問題。這類算法基于四元數(shù)理論由于旋轉(zhuǎn)矩陣和平移向量一起解耦計算，其中誤差直接影響旋轉(zhuǎn)矩陣的求解精度，因此適用于平移向量誤差較小的情況。

以上算法通過不同的軸角變換、單位四元數(shù)等來尋找矩陣，標(biāo)定矩陣是通過最小化代數(shù)誤差找到的。但是由于敏感的代數(shù)誤差（相對測量噪聲），在誤差較大的情況下容易收斂到局部最優(yōu)解，影響轉(zhuǎn)換矩陣的具體精度。

針對上述問題，本文提出針對對偶四元數(shù)模型的手眼標(biāo)定凸松弛優(yōu)化算法，該算法將旋轉(zhuǎn)矩陣和平移向量分開求解，并利用以目的為導(dǎo)向的誤差評價方法，提高手眼標(biāo)定關(guān)系的標(biāo)定精度和可靠性。

1 機(jī)器人手眼標(biāo)定模型

參照Tsai［3］的手眼標(biāo)定數(shù)學(xué)模型，設(shè)P1，P2代表變換矩陣（靶標(biāo)世界坐標(biāo)系到兩個不同姿態(tài)的攝像機(jī)坐標(biāo)系），Q1，Q2代表為兩次不同的變換矩陣（機(jī)械手末端執(zhí)行器坐標(biāo)系到機(jī)械手基坐標(biāo)系），X代表攝像機(jī)坐標(biāo)系到機(jī)械手末端執(zhí)行器坐標(biāo)系，則手眼關(guān)系可以表示為式（1）：

其中，A、B、X都為4×4的矩陣，將上式展開可以解耦表示成只含有旋轉(zhuǎn)矩陣R和平移矩陣T的方程形式為：

將對偶四元數(shù)aˉ,bˉ,qˉ展開為實部和對偶部四元數(shù)的形式，應(yīng)用四元數(shù)乘法可以將（2）、（3）中的旋轉(zhuǎn)平移部分用對偶四元數(shù)的實部和對偶部分別表示為：

1.1 手眼變換矩陣中旋轉(zhuǎn)矩陣R的求解

先考慮旋轉(zhuǎn)方程a·q=q·b 中的實部q 求解，其中a，b，q分別表示旋轉(zhuǎn)矩陣RA,RB,RX所對應(yīng)的單位四元數(shù)，將（5）式代入上述旋轉(zhuǎn)方程并利用四元數(shù)乘法合并整理可得:

其中，I3表示為3×3單位矩陣，K（a，b）表示由單位四元數(shù)對（a，b）所確定的4×4的矩陣。假設(shè)n（n≥2）次位姿變換測量得到K（ai，bi），構(gòu)建4n×4 矩陣L=［K（a1，b1），K（a2，b2），…，K（an，bn）］，則旋轉(zhuǎn)矩陣R 對應(yīng)的四元數(shù)q 可以由矩陣L 奇異值分解求得。

1.2 手眼變換矩陣中平移矩陣T的求解

再考慮平移方程a′·q+a·q′=q·b′+q′·b中的對偶部q′求解，利用四元數(shù)的乘法性質(zhì)以及式（6）的表示形式，關(guān)于對偶四元數(shù)的對偶部q′的標(biāo)定方程可以化簡表示為：

將（7）轉(zhuǎn)化為線性方程組：

對偶四元數(shù)的正交性，將方程組（8）變換成帶有約束條件的最小二乘問題：

其中，||.||2表示矩陣的2 范數(shù)，利用Levenberg-Marquardt 非線性優(yōu)化算法對上式求解即可得到對偶部q′，再利用對偶四元數(shù)中對偶部與實部的轉(zhuǎn)換關(guān)系式（8）即可得到手眼變換矩陣X 中的平移向量tx，從而得到表示手眼關(guān)系的變換矩陣X。

2 凸松弛多項式優(yōu)化方法

利用文獻(xiàn)［7］中的非線性優(yōu)化算法式（9）中最小二乘問題能較快地收斂到最優(yōu)解，這個問題自帶約束性。但準(zhǔn)確性取決于對偶部q′初值的選擇，且存在收斂到局部最優(yōu)解，不能保證每次全局最優(yōu)解都能收斂找到。啟發(fā)式的全局優(yōu)化算法例如蒙特卡羅采樣法、神經(jīng)網(wǎng)絡(luò)等基本通用，但具體案例中需要規(guī)劃眾多參數(shù)和算法設(shè)計以提高執(zhí)行效率，而基于LMI 松弛技術(shù)的優(yōu)化專門針對特性的凸松弛多項式優(yōu)化問題，不需要初值估計，從理論上講，LMI 方法是其中最可靠的選擇，可以最大限度地保證計算尋優(yōu)到全局最優(yōu)值［14］。因此本文將手眼變換矩陣中的優(yōu)化問題轉(zhuǎn)化為凸松弛多項式優(yōu)化問題從而去求最優(yōu)解。

首先，利用奇異值分解式對偶四元數(shù)的實部q進(jìn)行運(yùn)算，然后，以單位對偶四元數(shù)的正交性為約束限制，建立關(guān)于式（9）的多元多項式優(yōu)化問題：

至此求解多項式函數(shù)優(yōu)化問題的等效問題，就是半正定規(guī)劃，那么采用Lasserre 非凸函數(shù)LMI 求解，經(jīng)計算得知，由70個單項式組成的2元4次目標(biāo)函數(shù)f 為多項式函數(shù)，增加約束條件q′0≥0 是為了避免求解過程中出現(xiàn)2 個全局最優(yōu)解的情況，增加約束條件q′Tq0=0 是為了保證對偶四元數(shù)求解結(jié)果的正交性，確保半正定規(guī)劃限制內(nèi)的點求解的數(shù)值穩(wěn)定性。

利用Henrion 發(fā)布的GloptiPoly3 軟件包進(jìn)行算法計算［15］，Henrion等人率先把LMI加入到計算機(jī)視覺中解決多視圖幾何下的三維重建問題的辦法之中［16］。國內(nèi)，肖永亮［17］及柯豐愷［18］將其引入解決相機(jī)的內(nèi)外參數(shù)標(biāo)定問題。本文也采用此方法解決基于對偶四元數(shù)模型下的手眼標(biāo)定矩陣優(yōu)化問題，LMI難點在于優(yōu)化的目標(biāo)函數(shù)轉(zhuǎn)化為半正定規(guī)劃問題求解的具體操作。

3 實驗結(jié)果與分析

為了驗證所提出算法的可行性與優(yōu)化精度，搭建模擬實驗平臺，利用Matlab Camera Calibration Toolbox 工具箱標(biāo)定相機(jī)得到不同位姿下的相機(jī)外參數(shù)Pi（i=1，…，N），并將其交叉混合計算整合獲得新的M=N（N+1）/2組手眼標(biāo)定數(shù)據(jù)集{Pi+1（Pi）-1，（Qi+1）-1Qi}，最后，分別使用本文提出的凸松弛優(yōu)化估計（Convex Relaxation Optimization，CRO）、文獻(xiàn)［7］中的非線性優(yōu)化分步估計（Nonlinear Optimization Separated，NOS）和文獻(xiàn)［9］中的非線性優(yōu)化同步估計（Nonlinear Optimization Homochromous，NOH）手眼標(biāo)定算法對機(jī)器人的手眼關(guān)系進(jìn)行標(biāo)定，結(jié)果分別記為XCRO，XNOS，XNOH。由于實際標(biāo)定的真值無法得知，因此為了評價和比較3 種估計算法的精度，采用以目的為向?qū)У脑u價模式，即根據(jù)標(biāo)定手眼關(guān)系矩陣X和變換矩陣（運(yùn)動模型為原型）Bi逆求解相機(jī)運(yùn)動變換矩陣估計值?i，并將其值與經(jīng)過相機(jī)標(biāo)定得到的相機(jī)運(yùn)動變換矩陣Ai的值進(jìn)行比較分析。

實驗中，每次移動機(jī)械手篩選出M=16 組標(biāo)定數(shù)據(jù){Aj，Bj}（j=1，…，16），分別使用上述3 種不同的算法（CRO，NOS，NOH）通過前8組標(biāo)定數(shù)據(jù)計算手眼變換矩陣X，在同一標(biāo)定數(shù)據(jù)集下，3 種算法得到的手眼變換矩陣分別為：

10組完全獨立的手眼標(biāo)定實測實驗中3種不同的算法（CRO，NOS，NOH）的手眼標(biāo)定旋轉(zhuǎn)和平移誤差比較見圖1所示。

圖1 實測實驗旋轉(zhuǎn)和平移絕對誤差比較

圖1中，每一種算法采用10個點代表10次手眼標(biāo)定實驗的旋轉(zhuǎn)和平移絕對誤差結(jié)果，距離原點的位置遠(yuǎn)近和誤差大小對應(yīng)，可以看出，從旋轉(zhuǎn)和平移的可靠性穩(wěn)定性精確度看，NOH算法的精度和穩(wěn)定最差；CRO和NOS算法手眼標(biāo)定計算中將旋轉(zhuǎn)矩陣和平移向量分開求解，位置誤差較大的情況下，仍靠近原點，具有高精度和分布集中性。10組重復(fù)測試中（如圖1），CRO 算法的旋轉(zhuǎn)矩陣絕對誤差最大值為0.035mrad，比NOS小0.002mard，平移矩陣絕對誤差最大值為1.315mm，比NOS的小0.019mm，從誤差數(shù)值可以看出CRO算法略優(yōu)于NOS。

4 結(jié)論

鑒于機(jī)器人運(yùn)動學(xué)正解及Camel外參數(shù)標(biāo)定必然出現(xiàn)數(shù)據(jù)的偏向差別，因此局部最優(yōu)解存在手眼標(biāo)定優(yōu)化問題中。本文在對偶四元數(shù)概念之上采用一種基于凸松弛優(yōu)化技術(shù)的手眼標(biāo)定算法。該算法優(yōu)點是初值不需預(yù)估，可以最大限度確保獲得最優(yōu)解，從而規(guī)避尋優(yōu)過早陷入局部解。實測結(jié)果表明，提出的凸松弛優(yōu)化算法（CRO）較傳統(tǒng)的非線性優(yōu)化手眼標(biāo)定算法（NOS）具有較高的精度和穩(wěn)定性，在高精度的機(jī)器人視覺系統(tǒng)中具有一定的應(yīng)用價值。