面向手眼標(biāo)定的改進(jìn)灰狼優(yōu)化方法

李晟堯，肖世德，2，賴煥杰，胡鍇灃

（1.西南交通大學(xué)機(jī)械工程學(xué)院，四川 成都 610031；2.軌道交通運維技術(shù)與裝備四川省重點實驗室，四川 成都 610031）

1 引言

目前越來越多的制造業(yè)企業(yè)開始使用基于機(jī)器視覺技術(shù)的智能機(jī)器人以提高產(chǎn)能。英國的羅孚汽車公司［1］利用帶有機(jī)器視覺的智能機(jī)器人提高了車身輪廓尺寸的檢測精度和速度。瑞士ABB公司的Flex Picker機(jī)器人［2］，使用機(jī)器視覺技術(shù)，引導(dǎo)機(jī)器人進(jìn)行工件的快速準(zhǔn)確分揀。機(jī)器視覺技術(shù)可以使機(jī)器人獲得更豐富的環(huán)境信息，提高機(jī)器人在愈加復(fù)雜的工作環(huán)境中完成預(yù)定工作的能力。為了能利用機(jī)器視覺技術(shù)引導(dǎo)機(jī)器人運動，需要對機(jī)器視覺智能機(jī)器人進(jìn)行手眼標(biāo)定，從而獲得攝像機(jī)坐標(biāo)系與機(jī)器人末端執(zhí)行機(jī)構(gòu)坐標(biāo)系之間的位姿變換矩陣。

目前，專家學(xué)者已經(jīng)提出了很多用于手眼標(biāo)定矩陣求解的解析方法。其中，較為經(jīng)典的有文獻(xiàn)［3］提出的基于幾何關(guān)系的算法，文獻(xiàn)［4］提出的基于李群和李代數(shù)的算法，文獻(xiàn)［5］提出的基于單位四元數(shù)的算法以及文獻(xiàn)［6］提出的基于對偶四元數(shù)的算法。但是，以上算法都只是從數(shù)學(xué)分析的角度出發(fā)，求解手眼標(biāo)定矩陣，算法的魯棒性不強(qiáng)，如果拍攝的標(biāo)定板圖像質(zhì)量不佳，或者機(jī)器人位姿讀取結(jié)果不準(zhǔn)確，求解的手眼標(biāo)定矩陣可能不準(zhǔn)確。為此，文獻(xiàn)［7］在歐氏群下定義了一種手眼標(biāo)定誤差的衡量標(biāo)準(zhǔn)，從而可以將求解手眼標(biāo)定矩陣轉(zhuǎn)換成非線性優(yōu)化求解問題，但算法過分依賴初始值，容易陷入局部最優(yōu)解。文獻(xiàn)［8］提出一種利用初始標(biāo)定結(jié)果重新生成機(jī)械手拍照位姿的手眼標(biāo)定方法，該方法需要利用k-means 算法反復(fù)聚類重算拍照位姿，較為繁瑣。文獻(xiàn)［9］提出了一種基于差分進(jìn)化算法的手眼標(biāo)定方法，能夠很好地避免局部最優(yōu)，但算法迭代次數(shù)較多，速度較慢。

這里針對眼在手的智能機(jī)器人視覺系統(tǒng)，通過結(jié)合維度學(xué)習(xí)策略和差分進(jìn)化算法，提出了一種改進(jìn)灰狼算法的手眼標(biāo)定方法。通過分析誤差公式，設(shè)計用于降低手眼標(biāo)定誤差的機(jī)器人拍照位姿生成方案，利用傳統(tǒng)手眼標(biāo)定算法得出解析解后，利用改進(jìn)灰狼算法進(jìn)行非線性優(yōu)化，避免了陷入局部最優(yōu)等問題。該方法簡單，容易實現(xiàn)，而且優(yōu)化迭代速度較快，優(yōu)化結(jié)果較好。

2 手眼標(biāo)定的數(shù)學(xué)模型

假設(shè)機(jī)器人坐標(biāo)系、攝像機(jī)坐標(biāo)系和標(biāo)定板坐標(biāo)系之間的關(guān)系，如圖1所示。其中，{W}坐標(biāo)系是世界坐標(biāo)系，{E}坐標(biāo)系是機(jī)器人末端執(zhí)行機(jī)構(gòu)坐標(biāo)系，{C}坐標(biāo)系是攝像機(jī)坐標(biāo)系，{G}坐標(biāo)系是標(biāo)定板坐標(biāo)系。

圖1 機(jī)器人坐標(biāo)系示意圖Fig.1 Schematic Diagram of Robot Coordinate System

定義T6為坐標(biāo)系{W}到{E}的變換矩陣，Tm為坐標(biāo)系{E}到{C}的變換矩陣，Tc為坐標(biāo)系{C}到{G}的變換矩陣，Tg為坐標(biāo)系{W}到{G}的變換矩陣。

由坐標(biāo)系之間的變換關(guān)系，可得：

固定標(biāo)定板的位姿，然后操作機(jī)器人，在不同的機(jī)器人末端位姿下，標(biāo)定攝像機(jī)相對于標(biāo)定板的外參數(shù)Tc。對于第i次和第i-1次標(biāo)定，由于Tg保持不變，由式（1）得：

式中：T6i—第i次標(biāo)定時坐標(biāo)系{W}到{E}的變換矩陣T6；Tci—第i次標(biāo)定時攝像機(jī)的坐標(biāo)系{C}相對于標(biāo)定板坐標(biāo)系{G}的外參數(shù)Tc

式（2）經(jīng)過整理，可以改寫為：

3 機(jī)器人拍照位姿生成方案

Tsai不僅提出了基于幾何關(guān)系的手眼標(biāo)定算法，同時還以拍攝三張標(biāo)定板圖像為例，給出了誤差公式：

式中：σR—R的等效旋轉(zhuǎn)軸k的均方根誤差；Err(R)—R的等效旋轉(zhuǎn)軸k的最大誤差幅值；σP—P的均方根誤差；Err(P)—P的最大誤差幅值；cond(A)—‖A‖‖A-1‖。

其中，A滿足：

由式（6）可以看出影響手眼標(biāo)定誤差的因素有：

（1）相鄰的拍照位姿之間的等效旋轉(zhuǎn)軸之間的角度，例如∠(kg12，kg23)；

（2）相鄰拍照位姿之間的等效旋轉(zhuǎn)角度θgij(=θcij)；

（3）攝像機(jī)坐標(biāo)系的原點到標(biāo)定板坐標(biāo)系的原點之間的距離|Pci|；

（4）相鄰拍照位姿機(jī)器人的末端執(zhí)行機(jī)構(gòu)的坐標(biāo)系原點之間的距離|Pg1-Pg2|；

（5）攝像機(jī)坐標(biāo)系相對于標(biāo)定板坐標(biāo)系的外參數(shù)矩陣標(biāo)定誤差σRci，σPci；

（6）機(jī)器人末端執(zhí)行機(jī)構(gòu)坐標(biāo)系相對于世界坐標(biāo)系的求取誤差σRgi，σPgi。

因此，根據(jù)Tsai提出的誤差影響公式，設(shè)計了如下的拍照位姿生成方案，如圖2所示。以拍攝五張標(biāo)定板圖像為例，首先，在標(biāo)定板幾何中心的正上方 |Pc|距離處，放置一個參考坐標(biāo)系{C0}。并使得{C0}的z軸指向標(biāo)定板的幾何中心。然后，在{C0}坐標(biāo)系的xy軸平面，虛構(gòu)一個外接圓半徑為r的正五角星，正五角星的五個頂點即為拍照位置。至于五個拍照姿態(tài)，以如下方式設(shè)置：首先五個拍照點即是攝像機(jī)坐標(biāo)系的原點，然后，令每個位置時，攝像機(jī)坐標(biāo)系的z軸都指向標(biāo)定板的幾何中心；令第一個攝像機(jī)坐標(biāo)系的x軸沿外接圓的切向方向，即圖示的坐標(biāo)系1，然后，虛構(gòu)一個基準(zhǔn)面，其定義為過正五角星一邊，且垂直于{C0}的xy軸平面，即圖示的基準(zhǔn)面1，從而虛構(gòu)一根旋轉(zhuǎn)軸，其定義為過標(biāo)定板幾何中心，垂直于該基準(zhǔn)面，即圖示的基準(zhǔn)軸1，由此，第二個攝像機(jī)坐標(biāo)系設(shè)置為第一個坐標(biāo)系沿該基準(zhǔn)軸旋轉(zhuǎn)到正五角星的一點，即圖示的坐標(biāo)系2。后續(xù)坐標(biāo)系設(shè)置與此類似，就不贅述。

圖2 拍照位姿生成方案Fig.2 Pose Generation Scheme

4 改進(jìn)灰狼非線性優(yōu)化算法

由Horaud在其論文中所述，由解析法得到的手眼標(biāo)定矩陣，還可以通過以解析解為初始解，利用非線性優(yōu)化算法進(jìn)一步提高精度。這里提出的用于手眼標(biāo)定的拍照位姿生成方案已使誤差下降得很小了，利用傳統(tǒng)的非線性優(yōu)化算法容易陷入局部最小值而難以進(jìn)一步優(yōu)化。為此，這里提出一種用于手眼標(biāo)定的DLHGWO-DE非線性優(yōu)化算法。

灰狼算法是Mirjalili提出的一種通過模擬灰狼的社會等級和狩獵行為的智能種群優(yōu)化算法［10］。該算法具有結(jié)構(gòu)簡單、參數(shù)設(shè)置少的優(yōu)點，因而被廣泛地用于尋優(yōu)領(lǐng)域。但是該算法也和其他智能算法一樣，存在收斂速度慢，容易陷入局部最優(yōu)解等缺陷。為此，Mohammad于2020年提出一種結(jié)合維度學(xué)習(xí)的狩獵搜索策略的IGWO算法［11］，有效提高了灰狼算法的全局搜索能力，但仍會出現(xiàn)較早收斂等缺陷。

為改進(jìn)算法的尋優(yōu)性能，這里將差分進(jìn)化（DE）算法［12］和維度學(xué)習(xí)策略結(jié)合起來，提出一種更適合手眼標(biāo)定矩陣優(yōu)化的算法（DLH-GWO-DE），算法流程，如圖3所示。

圖3 DLH-GWO-DE算法流程圖Fig.3 Flow Chart of DLH-GWO-DE Algorithm

設(shè)置目標(biāo)函數(shù)，即適應(yīng)度為：

DLH-GWO-DE算法包含初始化，移動，變異，交叉，選擇，更新六個階段。

4.1 初始化階段

如果利用單位四元數(shù)替代旋轉(zhuǎn)矩陣，則手眼標(biāo)定矩陣可以變換成1×7的向量：

參考傳統(tǒng)算法得出的解析解，設(shè)置搜索空間下界lb，上界ub。并產(chǎn)生N×7初始狼群：

最后用傳統(tǒng)算法得出的解析解替代部分初始狼群。

4.2 移動階段

移動階段包含群體狩獵和維度學(xué)習(xí)。

群體狩獵即原始GWO搜索策略：前三個適應(yīng)度最好的狼被認(rèn)為是α，β，δ。然后利用公式計算新狼群：

維度學(xué)習(xí)即DLH搜索策略：新狼群由其鄰域?qū)W習(xí)決定。首先，根據(jù)下式計算當(dāng)前位置和候選位置之間的歐氏距離，以其作為半徑Ri(t)：

然后，由下式根據(jù)半徑Ri(t)構(gòu)造Xi(t)的領(lǐng)域Ni(t)：

式中：Di—Xi(t)和Xj(t)之間的歐氏距離。

由此，DLH策略產(chǎn)生的新狼群為：

4.3 變異階段

分別對原始策略產(chǎn)生的Xi-GWO狼群和DLH 策略產(chǎn)生的Xi-DLH狼群進(jìn)行變異，變異階段采用DE/best/1變異策略，即：

式中：Xp1，Xp2—從當(dāng)前狼群中隨機(jī)選擇的兩只不相同的灰狼，并且p1≠p2≠i；F—縮放比例因子，F(xiàn)∈[ 0，1]。

4.4 交叉階段

交叉階段可以增加狼群的多樣性，分別對上步的兩種變異狼群進(jìn)行交叉操作，交叉操作如下：

式中：CR—交叉概率，CR∈[0，1]。

4.5 選擇階段

利用適應(yīng)度對四種新狼群中的每個個體進(jìn)行比較：（1）由原始策略產(chǎn)生Xi-GWO狼群；（2）由DLH 策略產(chǎn)生的Xi-DLH狼群；（3）變異交叉后的Vi-GWO狼群；（4）變異交叉后的Vi-DLH狼群。

4.6 更新階段

利用適應(yīng)度對新狼群X(t+1) 中的各個體進(jìn)行比較：

5 實驗分析

5.1 拍照位姿生成方案實驗

實驗1：對比隨機(jī)拍照和根據(jù)拍照位姿生成方案拍照的手眼標(biāo)定結(jié)果，其中方案一是移動機(jī)械臂末端到19個隨機(jī)位置，拍攝標(biāo)定板；方案二是利用拍照位姿生成方案在 |Pc|=380，r=60，N=19拍攝標(biāo)定板。實驗結(jié)果，如表1所示。

表1 實驗1結(jié)果Tab.1 Results of Experiment 1單位：×10-3

由實驗1可知，Tsai算法，Park算法，Horaud算法手眼標(biāo)定結(jié)果相近，優(yōu)于Daniilidis算法，拍照位姿生成方案比隨機(jī)拍照旋轉(zhuǎn)誤差降低約74%，平移誤差降低約92%。

實驗2：控制變量 |Pc|=380，r=60，實驗N對手眼標(biāo)定誤差的影響，實驗結(jié)果，如圖4所示。

圖4 實驗2結(jié)果Fig.4 Results of Experiment 2

由圖4可知，在使用拍照位姿生成方案后，拍照張數(shù)N對誤差的影響不明顯，隨著N的增加，誤差僅少量降低。

實驗3：控制變量 |Pc|=380，N=19，實驗r對手眼標(biāo)定誤差的影響，實驗結(jié)果，如圖5所示。

圖5 實驗3結(jié)果Fig.5 Results of Experiment 3

由圖5可知，隨著r的增加，旋轉(zhuǎn)誤差增加，平移誤差減小。

實驗4：控制變量r=60，N=19，實驗 |Pc|對手眼標(biāo)定誤差的影響，實驗結(jié)果，如圖6所示。

圖6 實驗4結(jié)果Fig.6 Results of Experiment 4

由圖6可知，隨著 |Pc|的增加，旋轉(zhuǎn)誤差變化不明顯，平移誤差增加。

5.2 非線性優(yōu)化算法實驗

以實驗一中方案二經(jīng)Tsai算法計算的手眼標(biāo)定結(jié)果為初始值，利用DLH-GWO-DE算法進(jìn)行非線性優(yōu)化，可以得到的優(yōu)化結(jié)果，如圖7所示。由圖7可知，提出的DLH-GWO-DE的手眼標(biāo)定優(yōu)化算法比IGWO算法尋優(yōu)速度跟快，得到的結(jié)果更優(yōu)。

圖7 優(yōu)化結(jié)果Fig.7 Optimization Results

6 結(jié)語

針對眼在手的智能機(jī)器人視覺系統(tǒng)的手眼標(biāo)定問題，這里提出了面向手眼標(biāo)定的改進(jìn)灰狼優(yōu)化方法，利用拍照位姿生成方案設(shè)計手眼標(biāo)定實驗的攝像機(jī)拍照位姿，然后利用傳統(tǒng)手眼標(biāo)定算法求得手眼標(biāo)定矩陣的解析解，最后，以解析解作為初始解，利用提出的DLH-GWO-DE 非線性優(yōu)化算法迭代尋優(yōu)，進(jìn)一步降低手眼標(biāo)定誤差。通過實驗可以得出該拍照位姿生成方案簡便易操作，相比隨機(jī)拍照，手眼標(biāo)定旋轉(zhuǎn)誤差降低約74%，平移誤差降低約92%。非線性優(yōu)化算法相比傳統(tǒng)算法，手眼標(biāo)定誤差進(jìn)一步降低約19%。