基于凸松弛優化算法的機器人手眼標定

張亞如，杜建賓,2

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

0 引言

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

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

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

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

1 機器人手眼標定模型

參照Tsai［3］的手眼標定數學模型，設P1，P2代表變換矩陣（靶標世界坐標系到兩個不同姿態的攝像機坐標系），Q1，Q2代表為兩次不同的變換矩陣（機械手末端執行器坐標系到機械手基坐標系），X代表攝像機坐標系到機械手末端執行器坐標系，則手眼關系可以表示為式（1）：

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

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

1.1 手眼變換矩陣中旋轉矩陣R的求解

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

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

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

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

將（7）轉化為線性方程組：

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

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

2 凸松弛多項式優化方法

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

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

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

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

3 實驗結果與分析

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

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

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

圖1 實測實驗旋轉和平移絕對誤差比較

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

4 結論

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