凸松弛全局優化機器人手眼標定

李 巍，呂乃光，董明利，婁小平

(1.北京郵電大學 信息光子學與光通信研究院，北京 100876； 2.光電測試技術北京市重點實驗室(北京信息科技大學)，北京100192)

凸松弛全局優化機器人手眼標定

李 巍1*，呂乃光1,2，董明利2，婁小平2

(1.北京郵電大學 信息光子學與光通信研究院，北京 100876； 2.光電測試技術北京市重點實驗室(北京信息科技大學)，北京100192)

(*通信作者電子郵箱liweikilary@163.com)

針對機器人運動學正解及相機的外參數標定存在偏差時，基于非線性最優化的手眼標定算法無法確保目標函數收斂到全局極小值的問題，提出基于四元數理論的凸松弛全局最優化手眼標定算法。考慮到機械手末端相對運動旋轉軸之間的夾角對標定方程求解精度的影響，首先利用隨機抽樣一致性(RANSAC)算法對標定數據中旋轉軸之間的夾角進行預篩選，再利用四元數參數化旋轉矩陣，建立多項式幾何誤差目標函數和約束，采用基于線性矩陣不等式(LMI)凸松弛全局優化算法求解全局最優手眼變換矩陣。實測結果表明，該算法可以求得全局最優解，手眼變換矩陣幾何誤差平均值不大于1.4 mm，標準差小于0.16 mm，結果稍優于四元數非線性最優化算法。

機器人；手眼標定；四元數；全局優化；隨機抽樣一致性；線性矩陣不等式

0 引言

近年來，隨著科學技術的迅猛發展，機器人已經逐步進入我們的社會生活中，如在柔性裝配、智能服務、自主導航、逆向工程、焊接工程等領域得到了廣泛應用[1-2]。機器人手眼標定作為機器人視覺的關鍵技術之一，一直以來都是機器視覺領域中的研究熱點，為了提高手眼標定的精度，從而使機器人更好地為人類服務，國內外研究學者作了大量工作。

1989年，Tsai等[3]和Shiu等[4]首次提出手眼標定問題，并提出了基于軸角變換的手眼標定閉環線性解法，以后關于手眼標定的研究多是以此數學模型為基礎。受他們二人的啟發，Horaud等[5]、Daniilidis[6]、Andreff等[7]在Tsai和Shiu的研究基礎上分別提出了基于單位四元數、對偶四元數、矩陣直積的線性閉環解法，但這些參數化方法在通常情況下并不滿足標定矩陣的正交和單位特性，需要對其再加入正交和單位約束，從而影響了閉環求解精度的穩定性。2001年，文獻[8]以機器人視覺伺服系統為背景，提出利用運動恢復結構(Structure from Motion，SfM)算法求解相機的方位信息，雖然擺脫了手眼標定過程中對靶標的依賴，擴展了手眼標定的應用場景，但其標定精度并不高。2008年，考慮到標定數據的選擇同樣會影響手眼變換矩陣的求解精度，Schmidt等[9]在Tsai建立的標定誤差數學模型基礎上，提出利用矢量化編碼技術構建手眼標定數據篩選機制，提高了手眼變換矩陣的求解精度的穩定性，但求解效率較低。此后，文獻[10-13]又提出了采用分支定界法和L2范數理論來解決手眼標定過程中的全局優化問題，在一定程度上提高了計算結果的準確性，但在實際應用中需要調整許多參數，并且需要有針對性地對算法進行設計，才能提高算法的執行效率。在國內，2011年，王君臣等[14]提出一種基于最大似然估計的手眼標定非線性優化算法。2015年，王金橋等[15]提出利用遺傳算法解決關節臂視覺檢測系統中的手眼標定問題，然而，當誤差存在的情況下這些優化算法都容易出現過早收斂的問題，影響求解精度的穩定性。綜上，不管是國內和國外，綜合考慮標定方程的求解算法及誤差影響因素的研究還比較少，而事實上，標定數據的篩選和標定方程的求解算法都會直接影響手眼變換矩陣的求解精度，因此有必要綜合在一起進行研究。

針對上述機器人手眼標定問題中存在的關鍵問題，本文首先利用標定方程中旋轉矩陣的誤差模型，結合隨機抽樣一致性(Random Sample Consensus, RANSAC)算法對標定數據中旋轉軸之間的夾角進行預篩選；然后，鑒于目前非線性優化算法的局限性，提出一種基于線性矩陣不等式(Linear Matrix Inequality, LMI)松弛技術的凸松弛全局優化手眼標定算法；最后，通過實驗分析，驗證了本文算法較之非線性優化算法的優越性。

1 基于四元數的手眼標定問題描述

1.1 標定方程四元數解法

參照Tsai等[3]的定義方式，A1、A2表示靶標世界坐標系到兩個不同姿態的攝像機坐標系的變換矩陣，B1、B2表示為兩次不同姿態的機械手末端執行器坐標系到機械手基坐標系的變換矩陣，X表示攝像機坐標系到機械手末端執行器坐標系的變換矩陣，則手眼關系可以表示為式(1)：

AX=XB

(1)

其中：A、B、X都為4×4的矩陣，可以表示為：

將上式代入式(1)展開，表示成只含有旋轉矩陣和平移向量的方程形式為：

RARX=RXRB

(2)

(RA-I3)tX=RXtB-tA

(3)

根據式(2)和(3)可知，至少需要兩次旋轉軸非平行的相對位姿變換才可以唯一確定手眼關系X。文獻[5]給出上述標定方程的四元數解法，求解步驟如下：

(4)

然后，利用Levenberg-Marquardt非線性優化算法求解式(4)，即可得到手眼變換矩陣X中的旋轉平移關系Rx和tx。為了保證參量qx的單位性質，求解過程中取λ1=λ2=1，λ=106。

1.2 標定方程的誤差模型

Tsai等[3]利用一系列引理給出標定方程中旋轉矩陣的誤差模型，并對影響手眼標定方程的求解精度關鍵因素進行了仿真實驗。假定機械手和攝像機兩次相對運動變換矩陣A，B中R的標定誤差為ΔR，表示為式(5)：

(5)

將式(5)代入式(1)，則可以得到手眼變換矩陣X中旋轉矩陣R的均方根誤差，表示為σR，以三位姿情況為例，旋轉矩陣的均方根誤差σRAB可以表示為：

(6)

2 LMI凸松弛全局優化算法

2.1 全局優化算法問題描述

設W(x)為x=(x1,x2,…,xn)∈Cn上的標量多元多項式，則多元多項式的優化問題通常可以描述為：

minW0(x)

(7)

s.t.Wi(x)≥0，i=1,2,…,k

其中x=(x1,x2,…,xn)T∈Cn

由于上述非凸多項式優化問題的極值點不唯一，因此極小值并不一定是最小值，而通過初等微積分數學概念尋找上述多元多項式優化問題的全局最優解是一個NP問題。為了很好地解決此類問題，Lasserre利用Putinar定理[16]，把原多項式優化問題松弛為半正定規劃問題求解。通過LMI對非凸函數在可行域Cn內進行凸松弛，經過多次松弛逼近多元多項式函數全局最優解，使得多極值非凸優化問題松弛為凸函數優化問題。雖然蒙特卡羅采樣法、神經網絡、禁忌搜索以及模擬退火等都是具有通用性的啟發式全局優化算法，但在實際應用中需要調整許多參數，并且需要有針對性地對算法進行設計，才能提高算法的執行效率，而基于LMI松弛技術的優化方法是專門針對多項式優化問題的方法，從理論上講，LMI方法與其他確定性全局優化算法相比更具有可靠性，可以最大限度地保證計算結果優化到全局最優值。

定義vt(x)為t次多項式W(x)的典范基，t∈N，設s(2t)是vt(x)的維數，表示為：

(8)

即vt(x)為x中的元素xi互乘得到的次數不高于t的所有單項式和常量1構成的集合。

定義標量多元多項式W(x)表示為：

其中pα為多項式W(x)以式(8)vt(x)為空間基的系數向量。

定義s(2t)維向量y={yα}，且y0,0,…,0=1，則半定矩陣Mt(y)可以表示成分塊矩陣{Mi, j(y)}0≤i, j≤2t形式，其行列系數由基vt(x)順序確定，表示為：

(9)

例如，當n=2，t=2時，式(9)可以表示為：

最后，根據文獻[17]中4.3節給出的矩陣秩相等條件確定最優解，如果定義y*為式(7)的最優解，則存在：

Mt(y*)≥0,Mt-d(Wiy*)≥0,j=1,2,…,k,且

rank(Mt(y*))=rank(Mt-d(y*))

(10)

其中t≥d。

綜上，多項式LMI優化方法一般可以歸納為如下3個步驟：

2)添加半正定矩陣約束。按照t次多項式的基vt(x)的排列順序，添加半正定矩陣約束Mt(y)≥0，Mt(Wy)≥0。

3)將凸松弛多項式優化問題轉化為半正定規劃問題求解。使用對偶內點算法求解由前兩步組成的新半正定規劃問題，利用式(10)秩相等的條件來判別是否收斂到全局最優解。

2.2 基于全局優化算法的手眼變換矩陣估計

首先，將手眼變換矩陣X用四元數參數化為如下形式：

(11)

其中，旋轉矩陣R(qx)可以表示為：

R(qx)=

然后以最小化標定方程式(1)為幾何誤差目標函數，以單位四元數的性質為約束條件，建立關于式(11)的多元多項式優化問題：

最后，利用Lasserre非凸函數LMI松弛技術把上述多項式函數優化問題松弛為半正定規劃問題求解，圖1是LMI凸松弛優化算法流程。

圖1 LMI凸松弛優化算法流程Fig. 1 Flow diagram of LMI convex relaxation optimization

經計算得知，目標函數f為4次7元的多項式函數，由85個不同的單項式組成，由于求解過程中至少會得到2個全局最優解，因此需要增加約束條件qx≥0，為了提高求解的數值穩定性，可以先將Ai，Bi中平移向量歸一化，再根據實際要求對平移向量tx加入線性約束，將平移向量的模限制在有限的空間內(例如，可設‖tx‖2≤1)，從而確保半正定規劃的內點法可以高效地進行解算。

具體的計算過程可以采用Henrion發布的GloptiPoly3軟件包[18]。Henrion等[18]率先把LMI優化方法引入到計算機視覺中解決多視圖幾何下的三維重建問題。本文采用此方法計算手眼標定矩陣，LMI全局優化算法的關鍵在于如何將需要優化的目標函數轉化為半正定規劃問題求解，基于全局優化算法求解手眼變換矩陣X的部分程序如下所示：

3 實測結果與分析

為了進一步驗證本文提出的算法精度和魯棒性，設計實測實驗，在Inter Core i5-4590 CPU 3.3 GHz、4 GB內存的PC機上，用Matlab R2014a編程實現手眼標定，并比較分析實驗結果。實驗中選用日本電裝公司DENSO機械手VS-6577GM，X、Y、Z各方向的重復定位精度為±0.02 mm，選用凱視佳公司UD274M/C型號的CCD工業相機，分辨率為1 628×1 236 pixel相機，像元尺寸為4.4 μm，選用COMPUTAR 12 mm鏡頭對11 mm×11 mm棋盤格平面靶標進行相機參數標定，實驗前需要先將攝像機固定在機械手末端執行器法蘭盤上，搭建的實測實驗平臺如圖2所示。

圖2 手眼標定實測實驗平臺Fig. 2 Experimental platform for hand-eye calibration

考慮到機械手末端執行器相對運動旋轉軸之間的夾角對標定方程求解精度的影響，為了提高算法的魯棒性，參照標定方程的誤差模型見式(6)，首先利用自適應RANSAC算法預先對標定數據進行角度篩選，然后再用全局優化算法求解手眼變換矩陣。定義rij和rkl分別表示機械手末端執行器從位姿i到位姿j及從位姿k到位姿l的單位旋轉軸，θij,kl表示兩次相對運動的單位旋轉軸的夾角，如下式所示，當θij,kl接近90°或者θt接近0°時，旋轉矩陣的誤差最小：

θt=‖90-θij,kl‖；θij,kl=∠(rij,rkl)

具體的實驗操作步驟如下：

首先，利用機械手帶動攝像機每次選取Q個不同位姿對平面靶標拍照成像，兩兩進行組合可以得到S=Q(Q-1)/2組手眼標定數據集，由于至少需要兩組旋轉軸非平行的標定數據就可以唯一確定手眼變換矩陣，所以最少數據點mn=2，設定滿足角度閾值要求的內點比例初值w0=0.1，K次抽樣中所有樣本均為壞樣本的概率z=0.02，角度閾值初值θ0=5°，終止RANSAC抽樣的條件為滿足角度閾值的標定數據集CS≥15，采用自適應算法抽樣并更新w0和θ0，直到標定數據集CS≥15，記下此時的角度閾值θt，終止抽樣。

為了證明算法的可靠性，每次機械手變換Q=16個不同的位置，進行10組完全獨立的重復標定實驗，利用Matlab Camera Calibration Toolbox工具箱[19]標定攝像機的內外參數，內參數標定的最大誤差均在0.1 pixel內，隨機選取10組實驗中一組相機標定內參數矩陣T和鏡頭的徑向、切向畸變系數kC為：

kC=[-0.072 958,0.094 598,-0.001 835,-0.001 131,0]

以此內參數矩陣求解相機的外參數，在同樣的標定數據下，利用本文GO算法和OL算法得到的手眼變換矩陣分別為：

10組完全獨立的實測實驗全局優化算法(GO)與非線性優化算法(OL)的手眼標定幾何誤差的平均值和標準差見圖3和表1所示，其中圖3為表1中數據的誤差棒圖。

表1 GO算法與OL算法手眼標定幾何誤差平均值及標準差δTab. 1 Geometric average error and standard deviation δ of hand-eye calibration by GO and OL algorithms

圖3 實測實驗手眼標定幾何誤差棒圖Fig. 3 Comparison of geometric error bars for hand-eye calibration experiment

由圖3可知，從手眼標定幾何誤差的精度和穩定性上看，GO算法的每組觀測誤差值均低于OL算法。10組測量數據中：GO算法的手眼變換矩陣誤差平均值最大為1.4 mm，標準差小于0.16 mm；而OL算法的手眼變換矩陣誤差平均值最大已超過1.6 mm，標準差接近0.2 mm。篩選后標定數據集CS中的最大夾角閾值θt如表1所示，由表1可知，針對10組不同的觀測數據，經過RANSAC算法篩選后參與計算變換矩陣X的標定數據中兩次相對運動的單位旋轉軸的夾角θij,kl均接近90°，在83°和97°之間，由式(6)可知，此時標定方程中旋轉矩陣誤差σR接近最小，即標定數據的誤差對標定方程求解精度的影響最小。此外，為了比較兩種算法的運行效率，將兩種算法的運行時間在Matlab R2014a測試平臺下進行比較分析，如表2所示。

表2 GO算法和OL算法的運行時間比較Tab. 2 Comparison of calculation time for GO and OL algorithms

由表2可知，從計算時間上看，GO算法計算手眼變換矩陣的平均時間為0.51 s，OL算法計算手眼變換矩陣的平均時間為0.025 s，與OL算法相比，GO算法計算耗時較大，因此有必要利用GPU加速技術提升算法的運行速度。

4 結語

針對機器人運動學正解及相機的外參數標定偏差對手眼標定方程求解精度的影響，本文利用標定方程中旋轉矩陣的誤差模型，通過單位旋轉軸的夾角建立標定數據篩選機制，減小了標定數據的選擇對標定方程求解精度的影響。此外，為了最大限度避免優化結果過早陷入局部最小值，在現有的四元數手眼標定算法的基礎上，提出一種基于凸松弛全局優化技術的手眼標定算法，在不需要初值估計的情況下，保證了求解的最優性。實測結果表明，本文提出的全局優化算法較非線性優化手眼標定算法具有較高的精度和魯棒性，在高精度的機器人視覺系統中具有一定的應用潛力，但計算較為耗時，需要利用GPU并行計算進行加速。下一步將針對全局優化算法的計算耗時問題，應用統一計算設備架構(Compute Unified Device Architecture, CUDA)并行架構理念，將本文提出的手眼標定算法中具有并行性的部分應用CUDA架構實現并行處理，提高算法的處理速度，擴展算法的應用范圍。

References)

[1] 陳錫愛,徐方. 基于手眼立體視覺的機器人定位系統[J]. 計算機應用,2005,25(S1):302-304.(CHEN X A, XU F. Robot positioning system based on hand-eye stereo vision[J]. Journal of Computer Applications, 2005,25(S1):302-304.)

[2] 戰茜,屠大維. 移動機器人自主抓取作業[J]. 計算機應用,2016,36(S1):95-98.(ZHAN X, TU D W. Research on autonomous grasping of mobile robot[J]. Journal of Computer Applications, 2016,36(S1):95-98.)

[3] TSAI R Y, LENZ R K. A new technique for fully autonomous and efficient 3D robotics hand/eye calibration[J]. IEEE Transactions on Robotics and Automation, 1989, 5(3): 345-358.

[4] SHIU Y C, AHMAD S. Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the formAX=XB[J]. IEEE Transactions on Robotics and Automation, 1989, 5(1): 16-29.

[5] HORAUD R, DORNAIKA F. Hand-eye calibration[J]. The International Journal of Robotics Research, 1995, 14(3): 195-210.

[6] DANIILIDIS K. Hand-eye calibration using dual quaternions[J]. The International Journal of Robotics Research, 1999, 18(3): 286-298.

[7] ANDREFF N, HORAUD R, ESPIAU B. On-line hand-eye calibration[C]// Proceedings of the 2nd International Conference on 3-D Digital Imaging and Modeling. Washington, DC: IEEE Computer Society, 1999: 430-436.

[8] ANDREFF N, HORAUD R, ESPIAU B. Robot hand-eye calibration using structure-from-motion[J]. The International Journal of Robotics Research, 2001, 20(3): 228-248.

[9] SCHMIDT J, NIEMANN H. Data selection for hand-eye calibration: a vector quantization approach[J]. The International Journal of Robotics Research, 2008, 27(9): 1027-1053.

[10] ZHAO Z. Hand-eye calibration using convex optimization[C]// Proceedings of the 2011 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2011: 2947-2952.

[11] HELLER J, HAVLENA M, PAJDLA T. A branch-and-bound algorithm for globally optimal hand-eye calibration[C]// Proceedings of the 2012 IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, NJ: IEEE, 2012: 1608-1615.

[12] RULAND T, PAJDLA T, KRüGER L. Globally optimal hand-eye calibration[C]// Proceedings of the 2012 IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, NJ: IEEE, 2012: 1035-1042.

[13] 馬騰, 陳庶樵, 張校輝, 等. 基于規則集劃分的多決策樹報文分類算法[J]. 計算機應用, 2013, 33(9): 2450-2454.(MA T, CHEN S Q, ZHANG X H, et al. Multiple decision-tree packet classification algorithm based on rule set partitioning[J]. Journal of Computer Applications, 2013, 33(9): 2450-2454.)

[14] 王君臣, 王田苗, 楊艷, 等. 非線性最優機器人手眼標定[J]. 西安交通大學學報, 2011, 45(9): 15-20.(WANG J C, WANG T M, YANG Y, et al. Nonlinear optimal robot hand-eye calibration[J]. Journal of Xi’an Jiaotong University, 2011, 45(9): 15-20.)

[15] 王金橋, 段發階, 汪潤. 精確標定關節臂視覺檢測系統手眼關系[J]. 計算機工程與應用, 2015, 51(21): 225-229.(WANG J Q, DUAN F J, WANG R. Accurate calibration of AACMM visual detection system hand-eye relationship[J]. Computer Engineering and Applications, 2015, 51(21): 225-229.)

[16] PUTINAR M. Positive polynomials on compact semi-algebraic sets[J]. Indiana University Mathematics Journal, 1993, 42(3): 969-984.

[17] LASSERRE J B. Moments, positive polynomials and their applications[EB/OL].[2016-06-20]. http://www.worldscientific.com/doi/pdf/10.1142/9781848164468_fmatter.

[18] HENRION D, LASSERRE J B, LOFBERG J. GloptiPoly 3: moments, optimization and semidefinite programming[J]. Optimization Methods & Software, 2009, 24(4/5): 761-779.

[19] BOUGUET J Y. Camera calibration toolbox for matlab[EB/OL].[2016-06-20]. http://www.cvg.ethz.ch/teaching/2011spring/3dphoto/Misc/Calib_toolbox_instructions.pdf.

This work is partially supported by the National High Technology Research and Development Program (863 Program) of China (2015AA042308), the National Science Instrument Program (2013YQ22089304), the Open Project of Beijing Key Laboratory of Optoelectronics Measurement Technology (GDKF2016005).

LI Wei, born in 1988, Ph. D. candidate. His research interests include vision measurement, machine vision.

LYU Naiguang, born in 1944, M. S., professor. His research interests include information optics, optoelectronics, optoelectronic measurement.

DONG Mingli, born in 1965, Ph. D., professor. Her research interests include vision measurement, precision measurement and instrument.

LOU Xiaoping, born in 1970, M. S., professor. Her research interests include vision measurement, precision electronic-optical measurement.

Robot hand-eye calibration by convex relaxation global optimization

LI Wei1*, LYU Naiguang1,2, DONG Mingli2, LOU Xiaoping2

(1.InstituteofOpticalCommunication&Optoelectronics,BeijingUniversityofPosts&Telecommunications,Beijing100876,China;2.BeijingKeyLaboratoryofOptoelectronicsMeasurementTechnology(BeijingInformationScience&TechnologyUniversity),Beijing100192,China)

Hand-eye calibration based on nonlinear optimization algorithm can not guarantee the convergence of the objective function to the global minimum, when there are errors in both robot forward kinematics and camera external parameters calibration. To solve this tricky problem, a new hand-eye calibration algorithm based on quaternion theory by convex relaxation global optimization was proposed. The critical factor of the angle between different interstation rotation axes by a manipulator was considered, an optimal set of relative movements from calibration data was selected by Random Sample Consensus (RANSAC) approach. Then, rotation matrix was parameterized by a quaternion, polynomial geometric error objective function and constraints were established based on Linear Matrix Inequality (LMI) convex relaxation global optimization algorithm, and the hand-eye transformation matrix could be solved for global optimum. Experimental validation on real data was provided. Compared with the classical quaternion nonlinear optimization algorithm, the proposed algorithm can get global optimal solution, the geometric mean error of hand-eye transformation matrix is no more than 1.4 mm, and the standard deviation is less than 0.16 mm.

robot; hand-eye calibration; quaternion; global optimization; Random Sample Consensus (RANSAC); Linear Matrix Inequality (LMI)

2016-09-30;

2016-12-22。 基金項目:國家863計劃項目(2015AA042308)；國家重大科學儀器設備開發專項(2013YQ22089304)；光電測試技術北京市重點實驗室開放課題(GDKF2016005)。

李巍(1988—)，男，河北承德人，博士研究生，主要研究方向：視覺測量、機器視覺； 呂乃光(1944—)，男，安徽臨泉人，教授，博士生導師，主要研究方向：信息光學、光電子學、光電測試; 董明利(1965—)，女，遼寧鞍山人，教授，博士生導師，博士，主要研究方向：視覺測量、精密測量與儀器； 婁小平(1970—)，女，河南臨潁人，教授，主要研究方向：視覺測量、精密光電測試。

1001-9081(2017)05-1451-05

10.11772/j.issn.1001-9081.2017.05.1451

TP242.2

A