基于協(xié)方差矩陣自適應(yīng)進化策略的機器人手眼標定算法

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

基于協(xié)方差矩陣自適應(yīng)進化策略的機器人手眼標定算法

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

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

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

機器人；協(xié)方差矩陣自適應(yīng)進化策略；對偶四元數(shù)；手眼標定；約束優(yōu)化

0 引言

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

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

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

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

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

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

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

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

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

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

1 手眼標定問題

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

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

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

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

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

2 AX=XB方程的對偶四元數(shù)模型

2.1　對偶四元數(shù)的性質(zhì)

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

2.2　手眼標定的對偶四元數(shù)模型

其中1表示手眼標定進行的姿態(tài)變換次數(shù)。

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

根據(jù)四元數(shù)乘法的性質(zhì)（式（12）），可以簡化式（17）的平移方程：

列出簡化后平移部分的優(yōu)化問題模型2：

3 基于CMAES的手眼標定算法

3.1　CMAES

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

1）進行采樣與最優(yōu)子群計算。

2）更新均值。

3）協(xié)方差矩陣自適應(yīng)調(diào)整。

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

4）步長調(diào)整。

步長進化路徑的公式為：

3.2　懲罰函數(shù)法轉(zhuǎn)換約束條件

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

針對本文的約束問題：

構(gòu)造輔助函數(shù)如下：

根據(jù)式（27），將式（16）的約束條件進行轉(zhuǎn)換，構(gòu)造新的旋轉(zhuǎn)優(yōu)化模型如下：

求解旋轉(zhuǎn)部分的優(yōu)化方程后，再根據(jù)式（18）、式（27）構(gòu)造DQ平移部分手眼標定優(yōu)化模型：

3.3　基于CMAES的手眼標定算法設(shè)計

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

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

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

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

實驗環(huán)境為Windows 11 64位操作系統(tǒng)，Intel Core i5-8300H CPU 2.30 GHz，RAM 8.0 GB。

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

圖3　四種算法的旋轉(zhuǎn)誤差與平移誤差

表1　旋轉(zhuǎn)誤差數(shù)據(jù)對比

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

表2　平移誤差數(shù)據(jù)對比

5 結(jié)語

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

[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] 王君臣，王田苗，楊艷，等. 非線性最優(yōu)機器人手眼標定［J］. 西安交通大學(xué)學(xué)報， 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] 王一凡，段鎖林，高仁洲，等. 基于對偶四元數(shù)的機器人手眼標定算法研究［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] 李巍，呂乃光，董明利，等. 凸松弛全局優(yōu)化機器人手眼標定［J］. 計算機應(yīng)用， 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優(yōu)化的對偶四元數(shù)手眼標定算法［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］. 智能計算機與應(yīng)用， 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] 曾繁琦，袁曉靜，王旭平，等. 基于荷電狀態(tài)懲罰函數(shù)的能量管理策略優(yōu)化方法［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] 黃亞飛，梁昔明，陳義雄. 求解全局優(yōu)化問題的正交協(xié)方差矩陣自適應(yīng)進化策略算法［J］. 計算機應(yīng)用， 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集成學(xué)習(xí)方法的地表水質(zhì)分類［J］. 計算機科學(xué)與探索， 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；

湖北省教育廳科學(xué)技術(shù)研究計劃項目（B2020012）。

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

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.