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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

1 手眼標(biāo)定問題

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

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

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

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

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

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

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

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

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

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

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

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

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

3 基于CMAES的手眼標(biāo)定算法

3.1　CMAES

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

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

2）更新均值。

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

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

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

步長進(jìn)化路徑的公式為：

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

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

針對本文的約束問題：

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

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

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

3.3　基于CMAES的手眼標(biāo)定算法設(shè)計(jì)

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

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

4 實(shí)驗(yàn)與結(jié)果分析

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

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

圖3（a）和表1反映了在10組測試實(shí)驗(yàn)中，本文算法在求解手眼標(biāo)定旋轉(zhuǎn)部分的誤差平均值最小，旋轉(zhuǎn)誤差最大值為1.128 5°，最小為0.552 0°。在各組算法中，本文算法的旋轉(zhuǎn)誤差的方差最小，說明它的優(yōu)化結(jié)果更穩(wěn)定。與經(jīng)典手眼標(biāo)定算法相比，本文算法的精度均有提升，旋轉(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)精度和平移精度都有所提升。由于實(shí)際實(shí)驗(yàn)的噪聲干擾，傳統(tǒng)手眼標(biāo)定的線性方法在平移矩陣的求解過程受到的影響更大，因此本文算法相較于其他傳統(tǒng)算法有了更大的提升，與Tsai相比，平移精度提高了10.54%。不足的是，傳統(tǒng)算法的求解時(shí)間均在0.2 s內(nèi)，而本文算法采用擇優(yōu)截?cái)噙x擇策略，犧牲了一部分求解效率，10次手眼標(biāo)定平均求解時(shí)間為0.593 s，比傳統(tǒng)算法稍長。

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

5 結(jié)語

本文提出基于CMAES的手眼標(biāo)定算法，將協(xié)方差矩陣自適應(yīng)進(jìn)化策略應(yīng)用于全局優(yōu)化過程，在實(shí)際手眼標(biāo)定過程中減少了噪聲的影響，更大程度避免了齊次方程=的求解落入局部最優(yōu)的問題。在求解過程中采用數(shù)學(xué)工具對偶四元數(shù)，使用懲罰函數(shù)法轉(zhuǎn)化約束問題，方便手眼標(biāo)定方程的求解，最后使用CMAES分別求解旋轉(zhuǎn)和平移部分。實(shí)驗(yàn)結(jié)果表明，本文算法的平移誤差和旋轉(zhuǎn)誤差的方差最小，驗(yàn)證了算法尋優(yōu)效果的穩(wěn)定性，并且精度較傳統(tǒng)算法有所提高，在機(jī)器人視覺系統(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ī)器人手眼標(biāo)定［J］. 西安交通大學(xué)學(xué)報(bào)， 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ī)器人手眼標(biāo)定算法研究［J］. 機(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ī)器人手眼標(biāo)定［J］. 計(jì)算機(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ù)手眼標(biāo)定算法［J］. 機(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ì)多位置新型標(biāo)定方法［J］. 智能計(jì)算機(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］. 中國機(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ìn)化策略算法［J］. 計(jì)算機(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] 陳興國，徐修穎，陳康揚(yáng)，等. 基于CMAES集成學(xué)習(xí)方法的地表水質(zhì)分類［J］. 計(jì)算機(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ù)研究計(jì)劃項(xiàng)目（B2020012）。

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

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.