基于Sylvester方程變形的荔枝采摘機(jī)器人手眼標(biāo)定方法

莫宇達(dá)，鄒湘軍，葉 敏，司徒偉明，羅少鋒，王成琳，羅陸鋒

?

基于Sylvester方程變形的荔枝采摘機(jī)器人手眼標(biāo)定方法

莫宇達(dá)，鄒湘軍※，葉 敏，司徒偉明，羅少鋒，王成琳，羅陸鋒

（華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣州 510642）

針對視覺荔枝采摘機(jī)器人的Eye-in-Hand視覺與機(jī)器人關(guān)聯(lián)方式的手眼標(biāo)定問題，該文提出一種基于優(yōu)化的求解齊次變換矩陣方程的手眼標(biāo)定方法。該方法通過機(jī)器人帶動(dòng)其臂上的雙目相機(jī)從多個(gè)位置觀測標(biāo)定板，使用Sylvester方程變形對手眼標(biāo)定近似方程線性化，再對簡單的初值進(jìn)行優(yōu)化計(jì)算，最終得到精確的標(biāo)定結(jié)果。該方法的軟件用C++/OpenCV開發(fā)實(shí)現(xiàn)，并進(jìn)行了多個(gè)試驗(yàn)。試驗(yàn)結(jié)果表明，視覺與機(jī)器人關(guān)聯(lián)后，定位誤差與機(jī)器人運(yùn)動(dòng)次數(shù)相關(guān)，當(dāng)距目標(biāo)1 m左右，靜態(tài)時(shí)的視覺系統(tǒng)誤差均值為0.55 mm；動(dòng)態(tài)工作時(shí)，視覺關(guān)聯(lián)機(jī)器人重復(fù)定位誤差的均值為2.93 mm，標(biāo)準(zhǔn)差為0.45 mm，符合具有容錯(cuò)功能的視覺荔枝采摘機(jī)器人的實(shí)際使用需求。使用基于Sylvester方程變形的手眼標(biāo)定方法標(biāo)定的視覺荔枝采摘機(jī)器人，在野外環(huán)境下，總體采摘成功率達(dá)到76.5%，視覺系統(tǒng)成功識(shí)別、定位采摘點(diǎn)的情況下，采摘成功率達(dá)92.3%。

機(jī)器人；標(biāo)定；視覺；荔枝；采摘；立體

0 引 言

山地果園荔枝的收獲方式主要為人工采摘，每年都有果農(nóng)因采摘受傷。同時(shí)由于農(nóng)村勞動(dòng)力的減少，使用智能機(jī)器人采摘荔枝將是果園未來的發(fā)展趨勢。國內(nèi)外，機(jī)器人收獲農(nóng)作物的視覺感知系統(tǒng)研究較多[1-6]。其中華南農(nóng)業(yè)大學(xué)多年來對荔枝與荔枝采摘點(diǎn)的視覺識(shí)別與機(jī)器人采摘進(jìn)行了深入的研究[7-11]。各種對采摘機(jī)器人的研究[1,3-7,8-11]表明，視覺與機(jī)器人的關(guān)聯(lián)及精確定位，是實(shí)現(xiàn)采摘機(jī)器人主動(dòng)視覺和自動(dòng)采摘的關(guān)鍵技術(shù)與前提。

視覺機(jī)器人的視覺與機(jī)器人主要關(guān)聯(lián)方式有2種：Eye-to-Hand和Eye-in-Hand。對于Eye-to-Hand的視覺與機(jī)器人關(guān)聯(lián)方式，有著視覺系統(tǒng)與機(jī)器人坐標(biāo)系關(guān)聯(lián)簡單、安裝方便等優(yōu)點(diǎn)。同時(shí)因相機(jī)的安裝位置固定，靜置使視覺系統(tǒng)的精度高，一般用于固定區(qū)域的目標(biāo)定位與識(shí)別[12-13]。但對于機(jī)器人的大范圍搜索未知目標(biāo)、目標(biāo)跟蹤和實(shí)時(shí)定位等功能，Eye-in-Hand的手眼系統(tǒng)是機(jī)器人實(shí)現(xiàn)主動(dòng)視覺的主要途徑[14]，比Eye-to-Hand更具優(yōu)勢。Eye-in-Hand系統(tǒng)在工業(yè)機(jī)器人中應(yīng)用比較廣泛，隨著機(jī)械臂接近目標(biāo)，攝像機(jī)與目標(biāo)的距離也會(huì)變小，攝像機(jī)測量的絕對誤差會(huì)隨之降低[15]。該優(yōu)點(diǎn)特別適合野外環(huán)境下的采摘機(jī)器人作業(yè)，原因在于野外環(huán)境下存在地面不平、風(fēng)擾動(dòng)、光照變化等不確定因素，目標(biāo)的真實(shí)采摘點(diǎn)不會(huì)長時(shí)間固定在視覺系統(tǒng)采集圖像瞬間的定位坐標(biāo)上，受到擾動(dòng)后的實(shí)際坐標(biāo)偏離了原來的位置。Eye-in-Hand的視覺與機(jī)器人關(guān)聯(lián)方式允許采摘機(jī)器人使用主動(dòng)視覺，具有大范圍搜索采摘目標(biāo)，并在一定距離內(nèi)實(shí)時(shí)測量采摘點(diǎn)相對機(jī)器人的位置關(guān)系的優(yōu)勢。

Eye-in-Hand的視覺與機(jī)器人關(guān)聯(lián)方式需要確定視覺系統(tǒng)坐標(biāo)系與機(jī)械臂關(guān)節(jié)坐標(biāo)系的轉(zhuǎn)換關(guān)系：手眼標(biāo)定。早在20世紀(jì)80年代，美國Tsai等[16-17]就提出了經(jīng)典的手眼標(biāo)定方法，并將其數(shù)學(xué)模型歸結(jié)為求解=矩陣方程。之后，各種更高效的手眼標(biāo)定算法與數(shù)學(xué)模型相繼提出[18-25]。

本文將闡述一種求解手眼標(biāo)定歸結(jié)方程的方法。該方法使用Sylvester方程變形，把實(shí)際情況下的歸結(jié)方程線性化，配合拉格朗日乘子法與共軛梯度法，對傳統(tǒng)的2步法所求結(jié)果進(jìn)行優(yōu)化，得到更準(zhǔn)確的手眼標(biāo)定結(jié)果。該方法的標(biāo)定軟件最終使用C++/OpenCV實(shí)現(xiàn)，視覺和末端采摘執(zhí)行器安裝在一個(gè)國產(chǎn)的小型六軸機(jī)器人上，進(jìn)行視覺系統(tǒng)誤差的動(dòng)態(tài)試驗(yàn)以及視覺關(guān)聯(lián)機(jī)器人重復(fù)定位精度試驗(yàn)，統(tǒng)計(jì)分析平均定位誤差與誤差離散情況。最后，在野外環(huán)境下的山地果園進(jìn)行了荔枝采摘試驗(yàn)，驗(yàn)證其標(biāo)定方法與結(jié)果的可靠性。

1 手眼標(biāo)定原理與過程

手眼標(biāo)定需要使用輔助標(biāo)定物——棋盤格標(biāo)定板。手眼標(biāo)定涉及標(biāo)定板、相機(jī)系統(tǒng)、機(jī)器人、機(jī)器人的機(jī)械臂4個(gè)對象的空間位置關(guān)系。因此，首先要對這4個(gè)對象各自的坐標(biāo)系進(jìn)行設(shè)定，然后是4個(gè)坐標(biāo)系之間空間位置關(guān)系的求解。

1.1 標(biāo)定板與雙目相機(jī)的空間位置求解

對雙目相機(jī)進(jìn)行單目與立體標(biāo)定后，設(shè)此時(shí)的雙目相機(jī)坐標(biāo)系為{}、標(biāo)定板坐標(biāo)系為{}。以標(biāo)定板左上角的角點(diǎn)為標(biāo)定板坐標(biāo)系{}的原點(diǎn)，標(biāo)定板水平方向?yàn)樽鴺?biāo)系{}的軸方向，標(biāo)定板垂直方向?yàn)樽鴺?biāo)系{}的軸方向。當(dāng)標(biāo)定板置于左右相機(jī)的視場范圍內(nèi)時(shí)，可通過角點(diǎn)檢測算法來確定左右圖上每個(gè)角點(diǎn)的像素位置。標(biāo)定板坐標(biāo)系{}設(shè)定及標(biāo)定板角點(diǎn)檢測見圖1所示。

通過左右圖像角點(diǎn)位置與對應(yīng)關(guān)系，確定每個(gè)角點(diǎn)的圖像視差（disparity）d。根據(jù)雙目視覺的定位原理，即方程（1）和方程組（2），可求出每個(gè)角點(diǎn)相對于坐標(biāo)系{}的空間三維坐標(biāo)P。

方程（1）與方程組（2）中c與￠分別為左右相機(jī)的圖像中心與光心軸（水平）方向偏移量。其中c為左相機(jī)的圖像中心與光心軸（垂直）方向的偏移量。T為左右相機(jī)坐標(biāo)系的水平方向距離、為左右相機(jī)的鏡頭焦距參數(shù)。以上參數(shù)，均可由雙目相機(jī)的單目、立體標(biāo)定得到。、、分別為空間某角點(diǎn)相對相機(jī)坐標(biāo)系的真實(shí)三維坐標(biāo)，、分別為該角點(diǎn)在左圖像二維平面的二維坐標(biāo)，為該角點(diǎn)的圖像視差。

根據(jù)坐標(biāo)變換描述與坐標(biāo)點(diǎn)關(guān)系，某角點(diǎn)相對于坐標(biāo)系{}的空間三維坐標(biāo)P=[X，Y，Z，1]T與相對于坐標(biāo)系{}的空間三維坐標(biāo)P的關(guān)系可用下列矩陣方程（3）表示。

由于標(biāo)定板棋盤格的邊長已知，角點(diǎn)相對于坐標(biāo)系{}的空間坐標(biāo)P均可直接得到。根據(jù)方程（3），可利用最小二乘法求解坐標(biāo)系{}與坐標(biāo)系{}的空間位置轉(zhuǎn)換關(guān)系矩陣。

1.2 傳統(tǒng)的手眼標(biāo)定

經(jīng)典的2步法由Tsai 等[16]提出，至今仍有很高的實(shí)用價(jià)值。之后相繼提出的在線標(biāo)定過程[18-23]與經(jīng)典的2步法基本一致。

標(biāo)定的基本設(shè)備如圖2所示，標(biāo)定板3固定在機(jī)器人2前方的水平位置，相對機(jī)器人靜止，雙目相機(jī)1鏡頭朝向標(biāo)定板3。

圖3為手眼標(biāo)定涉及的坐標(biāo)系定義的示意圖。小型六軸機(jī)器人以簡單的線條表示。

設(shè)機(jī)器人自身坐標(biāo)系為{}，即{}表示機(jī)器人基坐標(biāo)系，機(jī)械臂末端的坐標(biāo)系為{}，即{}表示機(jī)械臂末端的空間位姿。手眼標(biāo)定時(shí)，標(biāo)定板相對機(jī)器人靜止，標(biāo)定板坐標(biāo)系{}與機(jī)器人坐標(biāo)系{}之間的位置轉(zhuǎn)換關(guān)系矩陣恒定。雙目相機(jī)使用穩(wěn)定的機(jī)構(gòu)固定在機(jī)械臂末端上，相機(jī)坐標(biāo)系{}與機(jī)械臂末端坐標(biāo)系{}的位置轉(zhuǎn)換關(guān)系矩陣亦恒定。

機(jī)器人控制機(jī)械臂移動(dòng)雙目相機(jī)，使標(biāo)定板完整出現(xiàn)在左右相機(jī)圖像中，進(jìn)行次圖像采集。理想狀態(tài)下，第次時(shí)各坐標(biāo)系的位置轉(zhuǎn)換關(guān)系可由方程（4）表述。

方程（5）中為4×4的零矩陣。設(shè)

則可把方程（5）簡單表示為矩陣方程（7）。

方程（7）中，A和B分別是第和（+1）次圖像采集過程中相機(jī)的位姿變化和機(jī)械臂末端的位姿變化矩陣。圖像采集次數(shù)越多，則方程（7）的組數(shù)越多，聯(lián)合計(jì)算的結(jié)果會(huì)更精確。把方程（7）中的4×4的齊次矩陣分解，左上角3×3元素組成旋轉(zhuǎn)矩陣，右上角3×1元素組成平移矩陣，如把A分解為R與P，得方程（8）。

分解方程（8）得到矩陣方程組（9）。

根據(jù)方程組（9）中第1式變換，可以得到矩陣方程（10）。

由方程（10）可知旋轉(zhuǎn)矩陣R與R相似，旋轉(zhuǎn)角相同。如果用旋轉(zhuǎn)軸（3×1的空間向量l、l）和旋轉(zhuǎn)角表示R與R，有以下關(guān)系

方程（11）（12）中，旋轉(zhuǎn)軸l、l以及旋轉(zhuǎn)角可使用羅德里格斯（Rodrigues）變換求得。假設(shè)機(jī)器人控制相機(jī)運(yùn)動(dòng)2次，相機(jī)采集3組圖像數(shù)據(jù)后，可以得到以下4個(gè)方程，組成方程組（13）。

參照公式（12），根據(jù)方程組（13）的第1、第3式，可得到矩陣方程組（14）和矩陣方程（15）。

方程（15）中“×”表示向量（一維矩陣）的叉乘。解方程（15）得到R后，代入方程組(13)的第2和第4式即可解得P，最終解得手眼關(guān)系。

2 基于Sylvester方程變形的手眼標(biāo)定原理

2.1 Sylvester方程變形

設(shè)有矩陣方程（16）如下

方程（16）中，∈（×）∈（×）∈（×），（×）是行列的矩陣。方程（16）被稱為Sylvester方程。根據(jù)文獻(xiàn)[26]，方程（16）可變形為矩陣方程（17）。

I是階單位矩陣，I是階的單位矩陣，是Kronecker乘積。其中與分別為與按行序列重新排列成的×1的矩陣。

2.2 基于Sylvester方程變形的手眼標(biāo)定

使用Sylvester方程變換進(jìn)行歸結(jié)方程求解早在20世紀(jì)末已被提出。研究者Andreff等[21]就利用Sylvester方程變換把方程組（13）進(jìn)行線性變換，從而用線性方法簡單求出理想狀態(tài)下的手眼標(biāo)定歸結(jié)方程（7）。但此類方法忽略了實(shí)際數(shù)據(jù)采集時(shí)產(chǎn)生的誤差。當(dāng)相機(jī)對標(biāo)定板進(jìn)行第次觀測時(shí)，考慮到相機(jī)采集圖像的誤差、雙目相機(jī)定位的誤差、機(jī)器人各運(yùn)動(dòng)軸伺服電機(jī)的誤差、實(shí)際環(huán)境擾動(dòng)對機(jī)構(gòu)影響等干擾因素，實(shí)際上位置轉(zhuǎn)換關(guān)系應(yīng)為方程（4）的近似，見式（18）。

因此，把與+1次數(shù)據(jù)相減后，變換所得到的歸結(jié)方程，并不如方程（7）那樣完全相等。實(shí)際情況下，2次觀測的數(shù)據(jù)所得到的手眼方程應(yīng)為Sylvester方程的形式，見矩陣方程（19）。

方程（19）中C是元素接近零的矩陣。

受變形方程（17）的啟發(fā)，結(jié)合實(shí)際情況，手眼標(biāo)定方程求解變?yōu)榫仃嚪匠蹋?0）的求解。

組數(shù)據(jù)可組成形式簡單的矩陣方程（23）。

方程組（24）中，x（=1,2,3,…,16）為的16個(gè)元素。通過拉格朗日乘子法構(gòu)建無約束優(yōu)化方程，并使用共軛梯度法[27]對無約束方程進(jìn)行最優(yōu)化計(jì)算，可快速解出的最優(yōu)值，得到手眼關(guān)系。

3 試驗(yàn)與分析

3.1 試驗(yàn)設(shè)備

圖像采集硬件采用USB 2.0接口的MicroVision MV-VD120SC型號(hào)工業(yè)相機(jī)，組成雙目視覺系統(tǒng)。相機(jī)的分辨率為1 280×960像素，幀率為15 fps。相機(jī)的鏡頭型號(hào)為Computar M0814-MP的8 mm定焦鏡頭。使用AFT-MCT-OV系列高精度棋盤格型金屬標(biāo)定板，格邊長30 mm×30 mm，打印精度為0.01 mm，尺寸400 mm× 300 mm×5 mm。使用GSK RB-03機(jī)器人，末端負(fù)載3 kg，各運(yùn)動(dòng)軸重復(fù)定位精度±0.05 mm，使用自制的有限通用輕量化的末端執(zhí)行器[11]。自主開發(fā)的視覺軟件運(yùn)行在工業(yè)計(jì)算機(jī)中，操作系統(tǒng)：Windows 7，CPU: Intel core i5-4590 3.3 GHz，內(nèi)存：4G 1333 MHz。

3.2 機(jī)械臂運(yùn)動(dòng)對視覺系統(tǒng)定位誤差影響

雙目視覺定位精度與視覺系統(tǒng)機(jī)構(gòu)的穩(wěn)定性相關(guān)[2]，而雙目視覺定位與相機(jī)標(biāo)定結(jié)果成為重要影響因素。只有當(dāng)標(biāo)定結(jié)果準(zhǔn)確反映相機(jī)鏡頭畸變、光心距離、相機(jī)間關(guān)系等結(jié)構(gòu)的實(shí)際情況時(shí)，雙目視覺系統(tǒng)定位才會(huì)精確。但視覺機(jī)器人手眼標(biāo)定過程是動(dòng)態(tài)的，需要機(jī)械臂帶動(dòng)雙目相機(jī)進(jìn)行視覺采集，因此，動(dòng)態(tài)的在線手眼標(biāo)定過程必然會(huì)影響相機(jī)標(biāo)定結(jié)果的準(zhǔn)確性。有必要分析機(jī)械臂運(yùn)動(dòng)對雙目視覺定位系統(tǒng)誤差的動(dòng)態(tài)影響。

相機(jī)坐標(biāo)系是無法使用物理手段直接測量的，這導(dǎo)致了無法直接測量視覺定位的系統(tǒng)誤差。然而標(biāo)定板精準(zhǔn)的打印精度提供了間接測量的方法：標(biāo)定板上任意2個(gè)相鄰角點(diǎn)之間的距離均為(30±0.01) mm，而雙目視覺所測算出的角點(diǎn)間距離與視覺定位對角點(diǎn)的定位精度有關(guān)，定位精度越高，視覺測算出的角點(diǎn)間距離越接近真實(shí)距離。這里，雙目視覺所測算出的角點(diǎn)間距離與真實(shí)距離的差距，稱為“測距誤差”。測距誤差能直觀反映雙目視覺定位的精度。

當(dāng)雙目相機(jī)安裝到機(jī)械臂末端后，保持機(jī)械臂靜止，進(jìn)行雙目相機(jī)的單目與立體標(biāo)定。標(biāo)定后，把標(biāo)定板放置在距離相機(jī)1 m左右的位置，使用雙目相機(jī)對標(biāo)定板上每個(gè)角點(diǎn)進(jìn)行三維定位，計(jì)算所有相鄰角點(diǎn)間的距離，統(tǒng)計(jì)測距誤差集1。在機(jī)械臂改變雙目相機(jī)的空間位置后，對標(biāo)定板的每個(gè)角點(diǎn)進(jìn)行三維定位，統(tǒng)計(jì)測距誤差集2。如此類推，連續(xù)進(jìn)行了組測距誤差的測量，得到數(shù)據(jù)集{S,=1, 2, 3, …,}。

3.3 視覺關(guān)聯(lián)機(jī)器人重復(fù)定位精度

Eye-in-Hand視覺機(jī)器人對某點(diǎn)的聯(lián)合定位過程如下：通過雙目視覺定位，計(jì)算空間中某點(diǎn)相對于相機(jī)坐標(biāo)系{}的坐標(biāo)值，再根據(jù)坐標(biāo)系{}與機(jī)械臂末端坐標(biāo)系{}的位置轉(zhuǎn)換關(guān)系矩陣計(jì)算相對坐標(biāo)系{T}的坐標(biāo)值p，最后根據(jù)坐標(biāo)系{}與機(jī)器人基坐標(biāo)系{}的位置轉(zhuǎn)換關(guān)系矩陣計(jì)算相對機(jī)器人坐標(biāo)系{}的坐標(biāo)值p。

視覺定位數(shù)據(jù)轉(zhuǎn)換過程見圖4。

根據(jù)聯(lián)合定位過程，視覺關(guān)聯(lián)機(jī)器人的聯(lián)合定位精度主要取決于3點(diǎn)：1）雙目視覺定位準(zhǔn)確性；2）手眼關(guān)系矩陣準(zhǔn)確性（由手眼標(biāo)定所得）；3）機(jī)械臂末端坐標(biāo)系與機(jī)器人自身坐標(biāo)系轉(zhuǎn)換關(guān)系矩陣（由機(jī)器人運(yùn)動(dòng)學(xué)正解所得）。

以上坐標(biāo)系{}、{}與{}是無法用物理手段直接測量的，這導(dǎo)致了視覺關(guān)聯(lián)機(jī)器人的定位精度亦無法直接測量。此外，雙目定位精度、轉(zhuǎn)換關(guān)系矩陣，兩者從手眼標(biāo)定過程就開始影響手眼標(biāo)定的結(jié)果，而手眼標(biāo)定過程的機(jī)器人運(yùn)動(dòng)也會(huì)影響雙目視覺定位的精度。綜上，視覺關(guān)聯(lián)機(jī)器人的聯(lián)合定位誤差，是由上述三者復(fù)雜耦合產(chǎn)生的，極難找到合適的數(shù)學(xué)模型去定量分析每個(gè)環(huán)節(jié)與誤差產(chǎn)生的關(guān)系。

盡管如此，視覺關(guān)聯(lián)機(jī)器人的聯(lián)合定位精度是可以間接測量的。假設(shè)視覺機(jī)器人以不同姿態(tài)對空間某一固定點(diǎn)進(jìn)行多次聯(lián)合定位，所有定位結(jié)果都會(huì)在空間中接近該點(diǎn)的真實(shí)位置。

視覺機(jī)器人以不同姿態(tài)對相對機(jī)器人靜置的標(biāo)定板上的每個(gè)角點(diǎn)進(jìn)行次聯(lián)合定位，每次對同一角點(diǎn)的定位結(jié)果都會(huì)不同，但必然會(huì)隨機(jī)分布在該角點(diǎn)真實(shí)位置的附近。角點(diǎn)的三維定位數(shù)據(jù)集的質(zhì)心與數(shù)據(jù)集中的每個(gè)點(diǎn)的空間距離的平均值，可以看作視覺關(guān)聯(lián)機(jī)器人的在該點(diǎn)的重復(fù)定位誤差。而不同點(diǎn)的重復(fù)定位誤差，其集合的標(biāo)準(zhǔn)差，可以反映重復(fù)定位誤差的浮動(dòng)情況。具體流程見圖5。

3.4 戶外采摘

為了進(jìn)一步驗(yàn)證在此重復(fù)定位精度下的視覺荔枝采摘機(jī)器人能可靠地進(jìn)行主動(dòng)視覺采摘任務(wù)，機(jī)器人安裝有限通用輕量化的末端執(zhí)行器[11]，配合自主開發(fā)的立體視覺荔枝采摘點(diǎn)定位系統(tǒng)，在山地果園進(jìn)行采摘試驗(yàn)。試驗(yàn)時(shí)間在7月13日早上，天氣晴朗，溫度29 ℃，陽光猛烈，背光陰影處采摘。試驗(yàn)地點(diǎn)在廣東省惠州市鎮(zhèn)隆山頂村荔枝專業(yè)合作社果園，海拔200 m左右。荔枝品種為淮枝。

具體采摘流程：1）相機(jī)標(biāo)定；2）手眼標(biāo)定；3）機(jī)器人主動(dòng)視覺發(fā)現(xiàn)荔枝；4）荔枝采摘點(diǎn)定位；5）定位結(jié)果轉(zhuǎn)換到機(jī)器人坐標(biāo)系下；6）采摘、放置和系統(tǒng)復(fù)位。

3.5 結(jié)果與分析

3.5.1 機(jī)械臂運(yùn)動(dòng)對視覺系統(tǒng)定位誤差影響試驗(yàn)結(jié)果與分析

進(jìn)行了109組測距誤差的測量，得到每個(gè)測距誤差集的平均誤差與機(jī)械臂運(yùn)動(dòng)次數(shù)的關(guān)系，見圖6。

由圖6可知，機(jī)械臂的運(yùn)動(dòng)次數(shù)越多，平均測距誤差值越大。相機(jī)標(biāo)定后，在原始狀態(tài)下，平均測距誤差為0.55 mm，在機(jī)械臂運(yùn)動(dòng)25次后，平均測距誤差約0.8 mm，在機(jī)械臂運(yùn)動(dòng)45次時(shí)更是達(dá)到1 mm以上。

圖6結(jié)果產(chǎn)生的原因在于受野外條件所限，試驗(yàn)時(shí)機(jī)器人固定于非剛性不平整的泥土地面，加上機(jī)械臂的運(yùn)動(dòng)導(dǎo)致了雙目相機(jī)的結(jié)構(gòu)發(fā)生了微小變化，原始標(biāo)定結(jié)果已不能完全準(zhǔn)確地反映相機(jī)鏡頭畸變、光心距離、相機(jī)間關(guān)系等結(jié)構(gòu)的實(shí)際情況。由于試驗(yàn)用相機(jī)為組裝式相機(jī)，即鏡頭與感光元件是分離和獨(dú)立的，因此，機(jī)械臂的運(yùn)動(dòng)會(huì)導(dǎo)致相機(jī)結(jié)構(gòu)的松動(dòng)、變形，從而產(chǎn)生相機(jī)結(jié)構(gòu)變化的情況。

根據(jù)圖6所示趨勢，運(yùn)動(dòng)次數(shù)的增加會(huì)影響相機(jī)立體定位的精度。而立體定位精度會(huì)直接影響手眼標(biāo)定精度，間接影響目標(biāo)重復(fù)定位精度和視覺關(guān)聯(lián)機(jī)器人采摘點(diǎn)定位精度。在相機(jī)標(biāo)定后，手眼標(biāo)定、重復(fù)定位試驗(yàn)和采摘試驗(yàn)均需要機(jī)械臂的運(yùn)動(dòng)。

為了避免機(jī)械臂的運(yùn)動(dòng)次數(shù)過多而導(dǎo)致目標(biāo)重復(fù)定位精度和視覺關(guān)聯(lián)機(jī)器人采摘點(diǎn)定位精度降低，本文對視覺關(guān)聯(lián)機(jī)器人重復(fù)定位精度試驗(yàn)與戶外采摘可靠性試驗(yàn)進(jìn)行以下操作：

1）手眼標(biāo)定過程中的圖像采集次數(shù)，即機(jī)械臂運(yùn)動(dòng)次數(shù)不會(huì)多于15次，但大于10次。2）機(jī)械臂運(yùn)動(dòng)總次數(shù)達(dá)40次時(shí)，對機(jī)器人重新進(jìn)行相機(jī)標(biāo)定與手眼標(biāo)定。

該操作合理分配了用于手眼標(biāo)定和標(biāo)定后應(yīng)用階段的機(jī)械臂運(yùn)動(dòng)次數(shù)，既保證手眼標(biāo)定過程中數(shù)據(jù)采集的正確性、優(yōu)化方法得出結(jié)果的準(zhǔn)確性，也兼顧應(yīng)用階段的視覺機(jī)器人聯(lián)合定位精度的準(zhǔn)確性。

3.5.2 視覺關(guān)聯(lián)機(jī)器人重復(fù)定位精度

完成相機(jī)標(biāo)定與手眼標(biāo)定后，立即進(jìn)行重復(fù)定位精度試驗(yàn)，對傳統(tǒng)手眼標(biāo)定算法與基于Sylvester方程變形的優(yōu)化標(biāo)定算法分別進(jìn)行3組、共6組的重復(fù)定位試驗(yàn)。每組試驗(yàn)前均需重新進(jìn)行單目標(biāo)定、立體標(biāo)定與手眼標(biāo)定，然后機(jī)器人控制相機(jī)以20個(gè)不同位姿對標(biāo)定板進(jìn)行圖像采集。標(biāo)定板有88個(gè)角點(diǎn)，6組試驗(yàn)共對528個(gè)角點(diǎn)進(jìn)行20次重復(fù)定位（即=20），共采集10 560個(gè)三維坐標(biāo)數(shù)據(jù)。統(tǒng)計(jì)傳統(tǒng)手眼標(biāo)定算法與基于Sylvester方程變形的優(yōu)化標(biāo)定算法所導(dǎo)致的視覺關(guān)聯(lián)機(jī)器人重復(fù)定位誤差，見表1。

表1 2種標(biāo)定方法對比

把528個(gè)角點(diǎn)聯(lián)合定位所測得的三維定位數(shù)據(jù)集的質(zhì)心與數(shù)據(jù)集中的每個(gè)點(diǎn)的空間距離（歐氏距離）的平均值分別列出并制成散點(diǎn)折線圖，可直觀地看出每個(gè)點(diǎn)的重復(fù)定位誤差及標(biāo)定方法的誤差分布情況，見圖7所示。

a. 傳統(tǒng)標(biāo)定方法

a. Tranditional calibration method

根據(jù)圖7所示，對比傳統(tǒng)標(biāo)定方法，基于Sylvester變形方程的最優(yōu)化標(biāo)定方法誤差數(shù)據(jù)均值更低，分布范圍更小，更均勻、穩(wěn)定。其結(jié)果最終定量地表現(xiàn)在表1所示的誤差與標(biāo)準(zhǔn)差中。

根據(jù)圖7數(shù)據(jù)分布及表1的統(tǒng)計(jì)數(shù)據(jù)，使用基于Sylvester變形方程的最優(yōu)化標(biāo)定方法的視覺機(jī)器人，相機(jī)工作距離為1 m左右時(shí)，視覺關(guān)聯(lián)機(jī)器人重復(fù)定位試驗(yàn)的重復(fù)定位誤差均值2.93 mm，標(biāo)準(zhǔn)差0.45 mm，即視覺關(guān)聯(lián)機(jī)器人重復(fù)定位誤差為（2.93±0.45）mm，比傳統(tǒng)標(biāo)定方法的（4.44±0.55）mm精度高、穩(wěn)定性強(qiáng)。

結(jié)合圖4的測距誤差趨勢圖，分析視覺關(guān)聯(lián)機(jī)器人重復(fù)定位誤差產(chǎn)生的主要原因是機(jī)械臂頻繁帶動(dòng)雙目相機(jī)運(yùn)動(dòng)導(dǎo)致了視覺系統(tǒng)原本穩(wěn)定的機(jī)構(gòu)發(fā)生了變化，降低了視覺定位精度。

對于采用了機(jī)構(gòu)與視覺的容錯(cuò)設(shè)計(jì)的末端執(zhí)行器[11]的荔枝采摘機(jī)器人，基于Sylvester變形方程的最優(yōu)化標(biāo)定方法的精度已在理論上滿足荔枝采摘的要求。

3.5.3 戶外采摘可靠性

試驗(yàn)進(jìn)行了51次采摘，機(jī)器人采摘成功見圖8。

采摘試驗(yàn)結(jié)果詳情見表2。

表2 采摘試驗(yàn)詳情

表2數(shù)據(jù)顯示，視覺荔枝采摘機(jī)器人總體采摘成功率達(dá)到了76%以上。在視覺系統(tǒng)準(zhǔn)確地識(shí)別定位荔枝采摘點(diǎn)后的采摘成功率達(dá)到了92.3%。

造成采摘失敗的原因可能有以下2點(diǎn)：1）荔枝采摘點(diǎn)的識(shí)別與定位；2）野外環(huán)境存在各種擾動(dòng)，真實(shí)采摘點(diǎn)在采摘過程中有偏移原來位置的可能。

由于相機(jī)放置于末端執(zhí)行器的正上方，采摘過程中可能會(huì)與采摘點(diǎn)上方的枝葉碰撞，從而也使真實(shí)采摘點(diǎn)偏移。因此，即使視覺準(zhǔn)確識(shí)別定位采摘點(diǎn)后，仍會(huì)出現(xiàn)采摘失敗的情況，采摘率的成功率是符合實(shí)際情況的。在采摘點(diǎn)定位失敗的情況下，采摘失敗，符合實(shí)際情況。

從采摘點(diǎn)識(shí)別定位成功后的采摘成功率92.3%可知，該手眼標(biāo)定方法計(jì)算的手眼標(biāo)定結(jié)果是可靠的，且完全能滿足本文所述視覺荔枝采摘機(jī)器人的實(shí)際采摘要求。同時(shí)，總體采摘成功率76.5%可知，本文所述視覺荔枝采摘機(jī)器人能在野外環(huán)境進(jìn)行荔枝采摘作業(yè)。

4 結(jié) 論

本文闡述了雙目視覺定位和手眼標(biāo)定的基本原理，提出了一種基于Sylvester方程變形的優(yōu)化計(jì)算手眼標(biāo)定關(guān)系的方法，該方法利用變形Sylvester方程使原本的手眼歸結(jié)方程轉(zhuǎn)換成線性方程，更容易進(jìn)行有約束優(yōu)化計(jì)算。重復(fù)定位實(shí)驗(yàn)和野外荔枝采摘試驗(yàn)結(jié)果表明：1）用基于Sylvester方程變形的優(yōu)化計(jì)算手眼標(biāo)定方法進(jìn)行手眼標(biāo)定的視覺荔枝采摘機(jī)器人，重復(fù)定位精度為（2.93±0.45）mm，對比傳統(tǒng)手眼標(biāo)定方法的定位誤差（4.44±0.55）mm，精度高、穩(wěn)定性強(qiáng)；2）基于Sylvester方程變形的優(yōu)化計(jì)算手眼標(biāo)定方法得出了手眼標(biāo)定關(guān)系，總體采摘成功率76.5%，采摘點(diǎn)識(shí)別定位正確的情況下，采摘成功率達(dá)92.3%，可靠性強(qiáng)，能滿足本文所述視覺荔枝采摘機(jī)器人的作業(yè)要求。

[1] Rath T, Kawollek M. Rbotic harvesting of Gerbera Jamesonii based on detection and three-dimensional modeling of cut flower pedicels[J]. Computers and Electronics in Agriculture, 2009, 66(1): 85－92.

[2] Zou X J, Zou H X, Lu J. Virtual manipulator-based binocular stereo vision positioning system and errors modelling[J]. Machine Vision and Applications, 2012, 23(1): 43－63.

[3] Shigehiko H, Kenta S, Satoshi Y, et al. Evaluation of a strawberry-harvesting robot in a field test[J]. Biosystems Engineering, 2010, 105(2): 160－171.

[4] Kanae T, Tateshi F, Akira A, et al. Cherry-harvesting robot[J]. Computers and Electronics in Agriculture, 2008, 63(1): 65－72.

[5] Mehta S S, Burks T F. Vision-based control of robotic manipulator for citrus harvesting[J]. Computers and Electronics in Agriculture, 2014, 102: 146－158.

[6] 紀(jì)超，馮青春，袁挺，等. 溫室黃瓜采摘機(jī)器人系統(tǒng)研制及性能分析[J]. 機(jī)器人，2011，33(6)：726－730.

Ji Chao, Feng Qingchun, Yuan Ting, et al. Development and performance analysis on cucumber harvesting robot system in greenhouse[J]. Robot, 2011, 33(6): 146－158. (in Chinese with English abstract)

[7] 熊俊濤，鄒湘軍，陳麗娟，等. 采摘機(jī)械臂對擾動(dòng)荔枝的視覺定位[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2012，28(14)：36－41.

Xiong Juntao, Zou Xiangjun, Chen Lijuan, et al. Visual position of picking manipulator for disturbed litchi[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2012, 28(14): 36－41. (in Chinese with English abstract)

[8] 郭艾俠，鄒湘軍，鄒海鑫. 基于二次閾值的荔枝果與結(jié)果母枝圖像識(shí)別[J]. 計(jì)算機(jī)工程與設(shè)計(jì)，2014，35(2)：557－561.

Guo Aixia, Zou Xiangjun, Zou Haixin. Recognition on image of litchi and their main fruit bearing based on twin-threshold method[J]. Computer Engineering and Design, 2014, 35(2): 557－561. (in Chinese with English abstract)

[9] 葉敏，鄒湘軍，羅陸鋒，等. 荔枝采摘機(jī)器人雙目視覺的動(dòng)態(tài)定位誤差分析[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2016，32(5)：50－56.

Ye Min, Zou Xiangjun, Luo Lufeng, et al. Error analysis of dynamic localization tests based on binocular stereo vision on litchi harvesting manipulator[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(5): 50－56. (in Chinese with English abstract)

[10] Wang C L, Zou X J, Tang Y C, et al. Localisation of litchi in an unstructured environment using binocular stereo vision[J]. Biosystems Engineering, 2016, 145(5): 39－51.

[11] Zou, X J, Ye M, Luo C Y, et al. Fault-tolerant design of a limited universal fruit-picking end-effector based on visoin positioning error[J]. Applied Engineering in Agriculture, 2016, 32(1): 5－18.

[12] 孫琦，羅志增. Eye-to-Hand機(jī)器人系統(tǒng)的平面工件識(shí)別研究[J]. 杭州電子科技大學(xué)學(xué)報(bào)，2010，30(1)：46－49.

Sun Qi, Luo Zhizeng. The research of plane part recognition based on the eye-to-hand robot system[J]. Journal of Hangzhou Dianzi University, 2010, 30(1): 46－49. (in Chinese with English abstract)

[13] 解楊敏，劉強(qiáng). 高精度自動(dòng)貼片機(jī)視覺系統(tǒng)定位算法研究[J].光學(xué)技術(shù)，2008，34(3)：449－454.

Xie Yangmin, Liu Qiang. Research on locating algorithm of vision alignment system in automatic high precision chip mounter[J]. Optical Technique, 2008, 34(3): 449－454. (in Chinese with English abstract)

[14] 許海霞，王耀南，萬琴，等. 一種機(jī)器人手眼關(guān)系自標(biāo)定方法[J]. 機(jī)器人，2008，30(4)：373－378. Xu Haixia, Wang Yaonan, Wan Qin, et al. A self-calibration approach to hand-eye relation of robot[J]. Robot, 2008, 30(4): 1－8. (in Chinese with English abstract)

[15] 張?jiān)浦? 工業(yè)機(jī)器人手眼標(biāo)定技術(shù)研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2009.

Zhang Yunzhu. Research on Hand-eye Calibration technology of industrial robot[D]. Harbin: Harbin Engineering University, 2009. (in Chinese with English abstract)

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

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

[18] Chou J C K, Kamel M. Finding the position and orientation of a sensor on a robot manipulator using quatemions[J]. Robot, 1991, 10(3): 240－254.

[19] Hoaud R, Domaika F. Hand-eye calibration[J]. Robot, 1995, 14(3): 195－210.

[20] Wei G, Arbter K, Hizinger G. Active self calibration of robotic eyes and hand-eye relationships with model identification[J]. IEEE Transactions on Robotics and Automation, 1998, 14(1): 158－166.

[21] Nicolas A, Radu H, Bernard E. On-line hand-eye calibration[C]// International Conference on 3-D Digital Imaging and Modeling, 1999. 3DIM 1999. 1999: 430–436..

[22] Angeles J, Soucy G, Ferrie F P. The online solution of the hand-eye problem[J]. IEEE Transactions on Robotics and Automation, 2000, 16(6): 720－731.

[23] Malm H, Heyden A. Simplified intrinsic camera calibration and hand-eye calibration for robot vision[J]. Intelligent Robots and Systems, 2003, 1: 1037－1043.

[24] 楊廣林，孔令富，王潔. 一種新的機(jī)器人手眼關(guān)系標(biāo)定方法[J]. 機(jī)器人，2006，28(4)：400－405.

Yang Guanglin, Kong Lingfu, Wang Jie. A new calibration approach to hand-eye-relation of manipulator[J]. Robot, 2006, 28(4): 400－405. (in Chinese with English abstract)

[25] 王君臣，王田苗，楊艷，等. 非線性最優(yōu)機(jī)器人手眼標(biāo)定[J]. 西安交通大學(xué)學(xué)報(bào)，2011，45(9)：15－21.

Wang Junchen, Wang Tianmiao, Yang Yan, et al. Nonlinear optimal robot hand-eye calibration[J]. Journal of Xi'an Jiaotong University, 2011, 45(9): 15－21. (in Chinese with English abstract)

[26] 馬飛. 矩陣方程AX+XB=C的迭代解法[D]. 廈門：廈門大學(xué)，2002.

Ma Fei. Iteration method of the matrix equation AX+XB=C[D]. Xiamen: Xiamen University, 2002. (in Chinese with English abstract)

[27] Hestenes M R. Methods of conjugate gradients for solving linear systems[J]. Research of the National Bureau of Standards, 1952, 49(6): 409－436.

[28] Fletcher D, Ian P. 3D math primer for graphics and game development[M]. Beijing: Tsinghua University Press, 2005.

[29] Bradki G, Kaehler A. Learning OpenCV[M]. Tsinghua University Press, 2013.

Hand-eye calibration method based on Sylvester equation deformation for lychee harvesting robot

Mo Yuda, Zou Xiangjun※, Ye Min, Situ Weiming, Luo Shaofeng, Wang Chenglin, Luo Lufeng

(,510642,)

Establishing the correlation between vision system and robot is the key and premise for the fruit harvesting robot based on binocular vision to successfully harvest fruit. And the robot with eye-in-hand vision system can use the active vision to search harvesting targets in a great scope and find out the position of picking points related to the robot’s coordinate in real time. For the accurate hand-eye calibration of the lychee harvesting robot with eye-in-hand vision system, a hand-eye calibration method based on optimization solution of the homogeneous matrix equation was proposed. Binocular camera was installed on the end of the robot's manipulator. The calibration board was placed in front of robot and the calibration board was stationary relative to the robot. By controlling the robot’s movement, binocular camera can observe the calibration board from multiple positions. Then the relative position relationship data between binocular camera and calibration board can be collected by stereovision location. Also, position data of manipulator’s end relative to robot coordinate system can be calculated using the robot kinematics. These data above can be used to establish an approximate equation of hand-eye calibration. Sylvester equation deformation was used to transform the approximate equation into a linear equation that made the problem into a linear optimization problem with some constraints. Lagrangian-Multiplier method was applied in changing the constrained optimization problem to a global optimization problem, and the conjugate gradient methods was a good choice for solving this global optimization problem. The result calculated by traditional method was used as the initial value that a more accurate solution can be solved out by iteration. This hand-eye calibration method was a module of our software “Stereo Vision Lychee Picking Point Recognition and Localization System” which had already been implemented using C++ and OpenCV. The software ran in an industrial control computer cooperated with the harvest robot and binocular vision system and were tests for three experiments. The first experiment was the robot’s mechanical arm movement influence on the ranging error of binocular vision system. This experiment showed that when the binocular cameras had been installed on the terminal of robot’s mechanical arm, the mean ranging error of binocular vision was associated with the movement times of robot’s mechanical arm. When the binocular cameras’ working distance was about 1000 mm, the initial mean ranging error of binocular vision would be 0.55 mm. And then, with the increase of movement times of robot’s mechanical arm, the mean ranging error of binocular vision presented a rising tendency. When the movement times of robot’s mechanical arm was more than one hundred, the mean ranging error of binocular vision would be more than 2 mm. Hence, the best movement times of robot’s mechanical arm used to hand-eye calibration can be 15. This experiment also showed the probable cause of repeat joint positioning error of robot with eye-in-hand visual system. The second experiment was the repeat joint positioning error of the robot with eye-in-hand visual system. The mean repeat joint positioning error of the robot generated by our hand-eye calibration method base on Sylvester deformation equation was 2.929 mm, which was better than the 4.442 mm error generated by traditional method . Meanwhile, the standard deviation of repeat joint positioning error of the robot was 0.454 mm, which was better than the 0.554 mm standard deviation of error generated by traditional method. The second experiment showed our hand-eye calibration method was better than the traditional method in the aspect of precision. At last, we used this harvesting robot to harvest lychee in a real lychee orchard. This experiment can show the reliability of our hand-eye calibration method in the natural environment. We tested 51 times, the ratio of successful picking was more than 76%. Moreover, when the software accurately recognized and located the lychee picking point, the ratio of successful picking could reach 92.3%. This experiment showed that the proposed hand-eye calibration method and the transformation result calculated using our method had a strong reliability and could satisfy the actual picking requirements of lychee harvesting robot.

robot; calibration; vision; lychee; picking; stereo

10.11975/j.issn.1002-6819.2017.04.007

TP391

A

1002-6819(2017)-04-0047-08

2016-05-30

2017-02-13

國家自然科學(xué)基金資助項(xiàng)目（31571568）；廣東省科技項(xiàng)目（2015A020209111）；廣州市科技計(jì)劃（201510010140）。

莫宇達(dá)，男（漢族），廣東肇慶人，主要從事機(jī)器視覺、農(nóng)業(yè)采摘機(jī)器人研究。廣州 華南農(nóng)業(yè)大學(xué)工程學(xué)院，510642。 Email：vettdetmore@live.com

鄒湘軍，女（漢族），湖南衡陽人，教授，博士生導(dǎo)師，主要從事農(nóng)業(yè)采摘機(jī)器人、智能設(shè)計(jì)與制造、虛擬現(xiàn)實(shí)等研究。廣州 華南農(nóng)業(yè)大學(xué)工程學(xué)院，510642。Email：xjzou1@163.com

莫宇達(dá)，鄒湘軍，葉 敏，司徒偉明，羅少鋒，王成琳，羅陸鋒. 基于Sylvester方程變形的荔枝采摘機(jī)器人手眼標(biāo)定方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2017，33(4)：47－54. doi：10.11975/j.issn.1002-6819.2017.04.007 http://www.tcsae.org

Mo Yuda, Zou Xiangjun, Ye Min, Situ Weiming, Luo Shaofeng,Wang Chenglin, Luo Lufeng. Hand-eye calibration method based on Sylvester equation deformation for lychee harvesting robot[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 47－54. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.04.007 http://www.tcsae.org