基于協方差矩陣自適應進化策略的機器人手眼標定算法

趙云濤，謝萬琪，李維剛，胡佳明

基于協方差矩陣自適應進化策略的機器人手眼標定算法

趙云濤1，2，謝萬琪2*，李維剛1，2，胡佳明2

（1.冶金自動化與檢測技術教育部工程研究中心（武漢科技大學），武漢 430081； 2.武漢科技大學 信息科學與工程學院， 武漢 430081）（ ? 通信作者電子郵箱atolrop@163.com）

針對視覺傳感器標定和機器人運動學求解過程中存在噪聲干擾，導致傳統的手眼標定算法求解誤差較大的問題，提出一種基于協方差矩陣自適應進化策略（CMAES）的機器人手眼標定算法。首先，采用對偶四元數（DQ）對旋轉和平移分別建立目標函數和幾何約束，簡化求解模型；其次，采用懲罰函數法將約束問題轉化成無約束優化問題；最后，使用CMAES算法逼近手眼標定旋轉和平移方程的全局最優解。搭建機器人、相機實測實驗平臺，將所提算法與Tsai兩步法、非線性優化算法INRIA、DQ算法進行對比。實驗結果表明：所提算法在旋轉和平移上的求解誤差和方差均小于傳統算法；與Tsai算法相比，所提算法的旋轉精度提升了4.58%，平移精度提升了10.54%。可見在存在噪聲干擾的實際手眼標定過程中，所提算法具有更好的求解精度與穩定性。

機器人；協方差矩陣自適應進化策略；對偶四元數；手眼標定；約束優化

0 引言

手眼標定用于獲取機器人坐標系和視覺傳感器坐標系之間的齊次變換矩陣，將相機拍攝到物體在世界坐標系的位姿信息轉換成在機器人基坐標系下的空間坐標，從而實現機器人對目標的抓取、點焊等操作。

此后，Tsai等［2］提出了手眼標定算法，給出了該方法下手眼標定數據集的篩選原則，并采取線性的方法分兩步先求解出旋轉矩陣，再根據旋轉矩陣求解出平移向量；Park等［3］提出Navy手眼標定算法，通過使用李群理論的知識求解手眼標定經典方程；Daniilidis等［4］提出了對偶四元數（Dual Quaternion， DQ）方法。

以上方法雖然采用了不同的數學工具，但都有相同的特點：旋轉和平移的解是用線性代數的方法估計的。這些方法在理想的仿真環境下能獲得高精度的求解結果，但在實際的手眼標定過程中，相機標定和機器人運動學求解過程會存在一些噪聲，允許線性化旋轉方程的幾何性質在有噪聲的情況下不成立，需要解決的線性問題是病態的，最后導致了方程求解的非線性和不穩定性。

Dornaika等［5］提出INRIA算法，用非線性優化方法同時求解旋轉和平移矩陣，相較于以往的算法精度有所提升；王君臣等［6］也采用了非線性的方法，將手眼標定問題轉化為一個非線性優化問題。但是手眼校準本質上是非凸的，因此任何封閉形式的解都只是一個近似的答案，求解過程中可能落入局部最優，對求解時的初值有較高要求。

在近期的手眼標定算法研究中，一些方法采用了全局優化的思想：文獻［7］中首次采用了全局思想研究手眼標定問題；文獻［8］在文獻［7］的基礎上采用了最小二乘法進行全局迭代；文獻［9-10］中采用凸松弛優化方法解決手眼標定的問題，通過gloptipoly3工具箱將非凸問題轉換為凸優化問題，相比傳統解法具有更高的求解精度和穩定性。但這類方法依賴于問題參數模型的選取，對于大規模問題通常不是很實用，因為凸松弛的計算代價是解決單個凸問題的倍數［11］。

協方差矩陣自適應進化策略（Covariance Matrix Adaptation Evolutionary Strategy， CMAES）是在進化策略（Evolutionary Strategy， ES）算法的基礎上發展起來的一種新型無約束的全局優化方法，文獻［12］中將它成功應用在標定領域，驗證了該算法有很好的搜索性能。與遺傳算法（Genetic Algorithm， GA）、粒子群優化（Particle Swarm Optimization， PSO）算法相比，它的收斂更快且搜索精度更高。本文在上述研究的基礎上將CMAES算法應用于對偶四元數手眼標定方程的求解，主要有以下改進：

1）使用對偶四元數分別建立旋轉和平移的目標函數和幾何約束，簡化求解問題模型，用CMAES算法替代傳統對偶四元數中的奇異值分解算法，減小旋轉和平移誤差。

2）采用CMAES算法求解手眼標定方程，不對手眼標定的非凸問題進行松弛處理而是直接解決，相較于文獻［9-10］中的算法，求解更快。

3）通過懲罰函數法［13］對手眼標定的約束條件進行迭代，將約束優化問題轉化為無約束優化問題，從而通過CMAES算法逼近手眼標定方程的全局最優值。

最后，搭建機器人、相機實測實驗平臺，將本文算法與三種傳統算法（Tsai、INRIA、DQ）進行對比，驗證了本文算法在存在噪聲干擾的實際手眼標定過程中，旋轉和平移的求解誤差和方差均小于其他算法，具有更好的求解精度與穩定性。

1 手眼標定問題

手眼標定的目的是求取機器人執行器末端到相機傳感器的齊次變換矩陣，通過多次變換機器人末端位置，拍攝不同角度的標定板圖片，作為手眼標定的參數。其中一次手眼標定如圖1所示。

如圖1所示，本文的手眼標定系統采用手在眼上（Eye-in-hand）的安裝方式，用表示兩次機器人運動末端相對位置的轉換矩陣；表示兩次相機相對位置的轉換矩陣；是機器人末端到相機的轉換矩陣。手眼標定的數學模型是：

將式（2）的齊次變換矩陣代入式（1）展開，可以得到手眼標定問題的初始數學模型：

由于的求解需要兩組觀測值，所以機器人至少得變換3個不同位置進行相機拍攝。在實際求解中，通過對機器人進行次位姿變換，得到組和，求解矩陣方程組：

其中：，、為兩次不同運動的機器人末端坐標位置，可以通過機器人運動學正解求得；，、則是標定板的兩次運動視覺傳感器所對應的外參矩陣，可以通過張正友標定法得到。

2 AX=XB方程的對偶四元數模型

2.1　對偶四元數的性質

對偶四元數（DQ）是對偶數和四元數在多維空間中的結合，本文采用DQ表示空間的任意旋轉和平移，可以避免萬向節死鎖的問題。DQ的數學形式如下：

2.2　手眼標定的對偶四元數模型

其中1表示手眼標定進行的姿態變換次數。

以式（15）建立DQ平移部分的優化方程，并以單位DQ的性質式（10）建立約束條件，建立平移問題模型：

根據四元數乘法的性質（式（12）），可以簡化式（17）的平移方程：

列出簡化后平移部分的優化問題模型2：

3 基于CMAES的手眼標定算法

3.1　CMAES

CMAES采用擇優截斷選擇策略，能防止手眼標定問題模型的種群過早收斂；在有噪聲的實際標定環境中，能減少因噪聲干擾導致求解模型陷入局部最優的情況［14］。本文采用CMAES逼近手眼標定旋轉和平移方程的全局最優解，對目標進行采樣并更新迭代進化路線，直到搜索到滿意的解或達到最大迭代次數［15］。

1）進行采樣與最優子群計算。

2）更新均值。

3）協方差矩陣自適應調整。

協方差矩陣的更新路徑和協方差更新公式分別如式（22）～（23）所示：

4）步長調整。

步長進化路徑的公式為：

3.2　懲罰函數法轉換約束條件

由于CMAES算法適用于無約束優化，為了方便優化模型的求解，本文引入了懲罰函數法。利用適當定義的復合函數再在原目標函數上建立懲罰函數法，把有約束的優化問題轉換為無約束優化問題再加以解決，并通過定義可達性指數為手眼標定方程優化問題模型中的停止判斷條件：

針對本文的約束問題：

構造輔助函數如下：

根據式（27），將式（16）的約束條件進行轉換，構造新的旋轉優化模型如下：

求解旋轉部分的優化方程后，再根據式（18）、式（27）構造DQ平移部分手眼標定優化模型：

3.3　基于CMAES的手眼標定算法設計

步驟3 建立優化模型，見式（28），使用采樣的候選解計算優化問題函數值；并按式（20）排序，組成當前最優子群。

當滿足停止條件并成功獲取旋轉模型的解后，將步驟3的優化模型（式（28））替換成平移優化模型（式（29）），重復步驟1～7，獲取平移模型的解，最后將DQ轉換為手眼標定齊次變換矩陣進行輸出。

4 實驗與結果分析

實驗采用Eye-in-hand手眼標定系統與HM06型號機器人，重復精度在0.05 mm以內，搭載Azure Kinect相機傳感器，RGB相機的分辨率為3 840×2 160，標定板采用8×11的棋盤格標準，單個棋盤格的邊長為15 mm，實驗場景如圖2所示。

實驗環境為Windows 11 64位操作系統，Intel Core i5-8300H CPU 2.30 GHz，RAM 8.0 GB。

圖3（a）和表1反映了在10組測試實驗中，本文算法在求解手眼標定旋轉部分的誤差平均值最小，旋轉誤差最大值為1.128 5°，最小為0.552 0°。在各組算法中，本文算法的旋轉誤差的方差最小，說明它的優化結果更穩定。與經典手眼標定算法相比，本文算法的精度均有提升，旋轉精度比Tsai提升了4.58%，最高提升了10.60%。

圖3　四種算法的旋轉誤差與平移誤差

表1　旋轉誤差數據對比

通過圖3（b）、表2可以看出，本文算法在4種算法里方差最小，說明全局優化算法在尋優過程中更穩定，旋轉精度和平移精度都有所提升。由于實際實驗的噪聲干擾，傳統手眼標定的線性方法在平移矩陣的求解過程受到的影響更大，因此本文算法相較于其他傳統算法有了更大的提升，與Tsai相比，平移精度提高了10.54%。不足的是，傳統算法的求解時間均在0.2 s內，而本文算法采用擇優截斷選擇策略，犧牲了一部分求解效率，10次手眼標定平均求解時間為0.593 s，比傳統算法稍長。

表2　平移誤差數據對比

5 結語

本文提出基于CMAES的手眼標定算法，將協方差矩陣自適應進化策略應用于全局優化過程，在實際手眼標定過程中減少了噪聲的影響，更大程度避免了齊次方程=的求解落入局部最優的問題。在求解過程中采用數學工具對偶四元數，使用懲罰函數法轉化約束問題，方便手眼標定方程的求解，最后使用CMAES分別求解旋轉和平移部分。實驗結果表明，本文算法的平移誤差和旋轉誤差的方差最小，驗證了算法尋優效果的穩定性，并且精度較傳統算法有所提高，在機器人視覺系統的應用中具有一定的潛力。

[1] SHIU Y C， AHMAD S. Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form AX=XB［J］. IEEE Transactions on Robotics and Automation， 1989， 5（1）： 16-29.

[2] 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.

[3] PARK F C， MARTIN B J. Robot sensor calibration： solving AX=XB on the Euclidean group［J］. IEEE Transactions on Robotics and Automation， 1994， 10（5）： 717-721.

[4] DANIILIDIS K， BAYRO-CORROCHANO E. The dual quaternion approach to hand-eye calibration［C］// Proceedings of 13th International Conference on Pattern Recognition — Volume 1. Piscataway： IEEE， 1996： 318-322.

[5] DORNAIKA F， HORAUD R. Simultaneous robot-world and hand-eye calibration［J］. IEEE Transactions on Robotics and Automation， 1998， 14（4）： 617-622.

[6] 王君臣，王田苗，楊艷，等. 非線性最優機器人手眼標定［J］. 西安交通大學學報， 2011， 45（9）： 15-20， 89.（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， 89.）

[7] HELLER J， HENRION D， PAJDLA T. Hand-eye and robot-world calibration by global polynomial optimization［C］// Proceedings of the 2014 IEEE International Conference on Robotics and Automation. Piscataway： IEEE， 2014： 3157-3164.

[8] 王一凡，段鎖林，高仁洲，等. 基于對偶四元數的機器人手眼標定算法研究［J］. 機電工程， 2019， 36（7）： 755-760， 765.（WANG Y F， DUAN S L， GAO R Z， et al. Study on robot hand-eye calibration using dual quaternion［J］. Journal of Mechanical and Electrical Engineering， 2019， 36（7）： 755-760， 765.）

[9] 李巍，呂乃光，董明利，等. 凸松弛全局優化機器人手眼標定［J］. 計算機應用， 2017， 37（5）： 1451-1455.（LI W， LYU N G， DONG M L， et al. Robot hand-eye calibration by convex relaxation global optimization［J］. Journal of Computer Applications， 2017， 37（5）： 1451-1455.）

[10] 王龍，閔華松. 基于LMI優化的對偶四元數手眼標定算法［J］. 機床與液壓， 2021， 49（21）：8-14.（WANG L， MIN H S. Dual quaternion hand-eye calibration algorithm based on LMI optimization［J］. Machine Tools and Hydraulics， 2021， 49（21）： 8-14.）

[11] GONG P H， ZHANG C S， LU Z S， et al. A general iterative shrinkage and thresholding algorithm for non-convex regularized optimization problems［C］// Proceedings of the 30th International Conference on Machine Learning. New York： JMLR.org， 2013： 37-45.

[12] 仲志丹，張瑋琪，杜慧穎. 基于CMAES算法的加速度計多位置新型標定方法［J］. 智能計算機與應用， 2018， 8（2）： 30-34.（ZHONG Z D， ZHANG W Q， DU H Y. New multi-position calibration method for accelerometer based on CMAES algorithm［J］. Intelligent Computer and Applications， 2018， 8（2）： 30-34.）

[13] 曾繁琦，袁曉靜，王旭平，等. 基于荷電狀態懲罰函數的能量管理策略優化方法［J］. 中國機械工程， 2022， 33（7）： 852-587， 871.（ZENG F Q， YUAN X J， WANG X P， et al. Energy management strategy optimization method based on SOC penalty function［J］. China Mechanical Engineering， 2022， 33（7）： 852-587， 871.）

[14] 黃亞飛，梁昔明，陳義雄. 求解全局優化問題的正交協方差矩陣自適應進化策略算法［J］. 計算機應用， 2012， 32（4）：981-985.（HUANG Y F， LIANG X M， CHEN Y X. Hybrid orthogonal CMAES for solving global optimization problems［J］. Journal of Computer Applications， 2012， 32（4）： 981-985.）

[15] WANG T C， TING C K. Fitness inheritance assisted MOEA/D-CMAES for complex multi-objective optimization problems［C］// Proceedings of the 2018 IEEE Congress on Evolutionary Computation. Piscataway： IEEE， 2018： 1-8.

[16] 陳興國，徐修穎，陳康揚，等. 基于CMAES集成學習方法的地表水質分類［J］. 計算機科學與探索， 2020， 14（3）：426-436.（CHEN X G， XU X Y， CHEN K Y， et al. Surface water quality classification via CMAES ensemble method［J］. Journal of Frontiers of Computer Science and Technology， 2020， 14（3）： 426-436.）

[17] ENEBUSE I， FOO M， IBRAHIM B S K K， et al. A comparative review of hand-eye calibration techniques for vision guided robots［J］. IEEE Access， 2021， 9： 113143-113155.

[18] DEKEL A， H?RENSTAM-NIELSEN L， CACCAMO S. Optimal least-squares solution to the hand-eye calibration problem［C］// Proceedings of the 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway： IEEE， 2020： 13595-13603.

Robot hand-eye calibration algorithm based on covariance matrix adaptation evolutionary strategy

ZHAO Yuntao1，2， XIE Wanqi2*， LI Weigang1，2， HU Jiaming2

（1，（），430081，；2，，430081，）

To solve the problem that the traditional hand-eye calibration algorithms have large solution errors due to the noise interference in the processes of vision sensor calibration and robot kinematics solution， a robot hand-eye calibration algorithm based on Covariance Matrix Adaptation Evolutionary Strategy （CMAES） was proposed. Firstly， the mathematical tool Dual Quaternion （DQ） was used to establish the objective functions and geometric constraints for both rotation and translation， and the solution model was simplified. Then， the penalty function method was used to transform the constrained problem into an unconstrained optimization problem. Finally， CMAES algorithm was used to approximate the global optimal solution of hand-eye calibration rotation and translation equations. An experimental platform of robot and camera measurement was built， and the proposed algorithm was compared with two-step Tsai algorithm， the nonlinear optimization algorithm INRIA， and the DQ algorithm. Experimental results show that the solution error and variance of the proposed algorithm are smaller than those of traditional algorithms for both rotation and translation. Compared with Tsai algorithm， the proposed algorithm has the rotation accuracy improved by 4.58%， and the translation accuracy improved by 10.54%. It can be seen that the proposed algorithm has better solution accuracy and stability in the actual hand-eye calibration process with noise interference.

robot; Covariance Matrix Adaptation Evolutionary Strategy (CMAES); Dual Quaternion (DQ); hand-eye calibration; constrained optimization

1001-9081（2023）10-3225-05

10.11772/j.issn.1001-9081.2022081282

2022?09?19；

2022?11?10；

湖北省教育廳科學技術研究計劃項目（B2020012）。

趙云濤（1982—），男，內蒙古赤峰人，副教授，博士，主要研究方向：機器視覺、機器人應用； 謝萬琪（1998—），男，湖北武漢人，碩士研究生，主要研究方向：機器人與視覺、點云； 李維剛（1977—），男，湖北咸寧人，教授，博士，主要研究方向：智能優化、數據挖掘； 胡佳明（1998—），男，湖北鄂州人，碩士研究生，主要研究方向：智能優化、機器人控制。

TP242

A

2022?11?25。

This work is partially supported by Science and Technology Research Program of Department of Education of Hubei Province （B2020012）.

ZHAO Yuntao， born in 1982， Ph. D.， associate professor. His research interests include machine vision， robot application.

XIE Wanqi， born in 1998， M. S. candidate. His research interests include robot and vision， point cloud.

LI Weigang， born in 1977， Ph. D.， professor. His research interests include intelligent optimization， data mining.

HU Jiaming， born in 1998， M. S. candidate. His research interests include intelligent optimization， robot control.