,, ,

(1.中國(guó)科學(xué)院 沈陽(yáng)自動(dòng)化研究所， 沈陽(yáng) 110016; 2.中國(guó)科學(xué)院大學(xué)，北京 100049;3.沈陽(yáng)新松機(jī)器人自動(dòng)化股份有限公司，沈陽(yáng) 110168)

0 引言

隨著智能感知和智能控制技術(shù)的發(fā)展，機(jī)器人已經(jīng)逐漸融入到人們的日常生活和工作中。在機(jī)器人技術(shù)的應(yīng)用中，機(jī)器人的視覺抓取是一個(gè)重要的研究課題。在機(jī)器人物體抓取中，物體位置檢測(cè)的精度直接依賴于機(jī)器人手眼標(biāo)定的精度。精確的手眼標(biāo)定對(duì)機(jī)器人物體抓取作業(yè)的完成起著至關(guān)重要的作用。因此，提高機(jī)器人手眼標(biāo)定的精度成為機(jī)器人領(lǐng)域的研究熱點(diǎn)。為了解決服務(wù)機(jī)器人的手眼標(biāo)定問題，可以采用高精度測(cè)量?jī)x器測(cè)量的方法。然而，由于這些測(cè)量?jī)x器價(jià)格高昂、操作復(fù)雜，直接測(cè)量方法未能被廣泛應(yīng)用。另一個(gè)可行的方法是根據(jù)機(jī)器人坐標(biāo)系下和相機(jī)坐標(biāo)系下相同位置對(duì)應(yīng)的坐標(biāo)對(duì)，估計(jì)出坐標(biāo)的對(duì)應(yīng)關(guān)系。

在手眼標(biāo)定模型方面，Tasi和Lenz[1]采用了一種基于軸角變換的手眼標(biāo)定模型。Lee[2]提出了一種基于運(yùn)動(dòng)估計(jì)的簡(jiǎn)化的自標(biāo)定方法。Daniilidis[3]引入了一種基于對(duì)偶四元數(shù)的旋轉(zhuǎn)和平移矩陣表示和計(jì)算方法。Chou和Kamel[4]提出了一種基于SVD分解的旋轉(zhuǎn)矩陣計(jì)算方法。Horaud等[5]提出了通過非線性求解最小化誤差模型的手眼標(biāo)定方法。Fassi等人[6]則給出了一種基于幾何解釋的手眼標(biāo)定方法，采用最小二乘方法求解。Schmidt等[7]提出了一種基于向量編碼的有效數(shù)據(jù)選擇方法，剔除部分無(wú)效數(shù)據(jù)，用以提高手眼標(biāo)定的精度。然而，該算法的數(shù)據(jù)剔除策略較為復(fù)雜。Zhuang等[8]為機(jī)器人的手眼系統(tǒng)和末端執(zhí)行器構(gòu)造了一種整體模型。該模型的目標(biāo)是最小化標(biāo)定矩陣的Frobenius范數(shù)。Wei[9]則介紹了一種基于最小化代數(shù)距離的自動(dòng)標(biāo)定方法。王金橋等[10]和鄒勁松等[11]分別采用遺傳算法和自適應(yīng)差異進(jìn)化算法解決手眼標(biāo)定求解過程中過早收斂的問題，在一定程度上提高了求解過程的可靠性。李巍等[12]在四元數(shù)理論的基礎(chǔ)上，提出了矩陣不等式凸松弛全局最優(yōu)化標(biāo)定方法。為了減小數(shù)據(jù)粗差對(duì)標(biāo)定精度的影響，提高機(jī)器人手眼標(biāo)定精度，提出了一種基于誤差分布估計(jì)加權(quán)最小二乘手眼標(biāo)定方法。同時(shí)，提出的迭代求解策略可以進(jìn)一步提高標(biāo)定結(jié)果的置信度。

1 機(jī)器人手眼標(biāo)定模型

在機(jī)器人系統(tǒng)中，相機(jī)的安裝方式通常有兩種，分別為Eye-In-Hand方式和Eye-To-Hand方式。Eye-In-Hand的方式是指相機(jī)安裝在機(jī)器人手臂的末端位置，相機(jī)跟隨機(jī)械臂運(yùn)動(dòng)；而Eye-To-Hand是指相機(jī)固定安裝，機(jī)械臂的運(yùn)動(dòng)不會(huì)影響相機(jī)與機(jī)器人底座的相對(duì)位置關(guān)系。在服務(wù)機(jī)器人系統(tǒng)中，相機(jī)通常采用Eye-To-Hand的安裝方式，如圖1所示。Eye-To-Hand的相機(jī)安裝方式可以使機(jī)器人能夠獲得更全面的場(chǎng)景信息。

1.1 基于最小二乘的手眼標(biāo)定模型

(1)

在上述公式中，R和T分別表示旋轉(zhuǎn)矩陣和平移矩陣。旋轉(zhuǎn)矩陣R具有如下性質(zhì)，即：

RTR=I

(2)

式中，I表示單位陣。手眼標(biāo)定的目的是估計(jì)出旋轉(zhuǎn)矩陣R和平移矩陣T。最直接的估計(jì)方法是采用最小二乘估計(jì)。針對(duì)手眼標(biāo)定問題，采用最小二乘估計(jì)的優(yōu)化目標(biāo)可以表示為:

(3)

令:

(4)

上述F對(duì)T求偏導(dǎo)數(shù)，有：

(5)

式(4)取得極值的條件是使F對(duì)T的偏導(dǎo)數(shù)為零。令式(5)等于零，則可以計(jì)算出平移矩陣T，即有：

(6)

式中，

(7)

(8)

將式(6)代入式(4)，則目標(biāo)函數(shù)可以表示為:

(9)

手眼標(biāo)定參數(shù)中，旋轉(zhuǎn)矩陣應(yīng)滿足如下公式：

(10)

令:

(11)

(12)

考慮到RTR=I，有：

(Rgi-hi)T(Rgi-hi)=

(13)

因此，R可以由如下公式估計(jì)得出:

(14)

式中,tr(·)為求解矩陣的跡。令S=GHT，對(duì)S進(jìn)行SVD分解，有：

svd(S) =U∑VT

(15)

此時(shí)：

tr(RGHT) = tr(RU∑VT) = tr(∑VTRU)

(16)

式中，U,V和R均為正交陣。因此，VTRU也是正交陣，如果tr(∑VTRU)取最大值，則:

(17)

那么，由式(6)可解得:

(18)

1.2 基于誤差分布估計(jì)的手眼標(biāo)定算法

在機(jī)器人的手眼標(biāo)定中，通過相機(jī)和機(jī)器人運(yùn)動(dòng)學(xué)求解兩種方式獲得的空間中點(diǎn)的坐標(biāo)通常存在誤差。根據(jù)最小二乘估計(jì)原理，當(dāng)誤差服從正態(tài)分布時(shí)，最小二乘估計(jì)即是最優(yōu)估計(jì)。在考慮系統(tǒng)存在的誤差時(shí)，公式(1)改寫為如下公式：

(19)

在上述公式(19)中，ε=[εxi,εyi,εzi]T是系統(tǒng)的誤差向量?；谧钚《斯烙?jì)理論，只有在誤差ε服從正態(tài)分布時(shí), 變換矩陣R和T可以用最小二乘估計(jì)方法合理計(jì)算得到。

然而，當(dāng)系統(tǒng)誤差不滿足服從正態(tài)分布時(shí)，尤其是粗差存在時(shí)，傳統(tǒng)的最小二乘估計(jì)方法的可靠性會(huì)大大下降。為了解決最小二乘估計(jì)魯棒性低的問題，多種魯棒估計(jì)方法(如L估計(jì)、M-估計(jì)、R估計(jì))[13-15]被提出來(lái)。盡管這些方法能夠部分解決魯棒性低的問題，但這些估計(jì)方法的迭代過程會(huì)增加計(jì)算的時(shí)間開銷。

降低最小二乘估計(jì)誤差的一個(gè)直觀方法是將帶有粗差的數(shù)據(jù)從數(shù)據(jù)集中剔除掉。然而，判斷某個(gè)數(shù)據(jù)是否帶有粗差非常困難。在采用最小二乘估計(jì)方法對(duì)服務(wù)機(jī)器人進(jìn)行手眼標(biāo)定時(shí)，大部分測(cè)量數(shù)據(jù)包含大小相似似的誤差。而數(shù)據(jù)集中帶有粗差的數(shù)據(jù)的數(shù)量通常較少，同時(shí)粗差值通常遠(yuǎn)離有效數(shù)據(jù)的誤差分布區(qū)間。若能根據(jù)誤差的分布情況對(duì)數(shù)據(jù)提前進(jìn)行甄別，則可以提高手眼標(biāo)定的精度。

根據(jù)公式(17)和公式(18)，可以通過最小二乘估計(jì)方法計(jì)算得到粗糙的標(biāo)定矩陣的結(jié)果R和T。之后，根據(jù)已經(jīng)計(jì)算得到的R和T，對(duì)標(biāo)定數(shù)據(jù)進(jìn)行重構(gòu)，可以計(jì)算數(shù)據(jù)的重構(gòu)誤差。對(duì)應(yīng)于較大重構(gòu)誤差的數(shù)據(jù)視作帶粗差數(shù)據(jù)。然而，由于有效數(shù)據(jù)與無(wú)效數(shù)據(jù)對(duì)應(yīng)誤差的閾值難以確定，因此仍然難以將帶有粗差的數(shù)據(jù)從數(shù)據(jù)集中剔除。為此，提出了一種基于誤差分布估計(jì)的加權(quán)最小二乘估計(jì)方法。通過調(diào)整數(shù)據(jù)集中數(shù)據(jù)的權(quán)重系數(shù)，降低粗差數(shù)據(jù)的影響，實(shí)現(xiàn)高精度的機(jī)器人手眼標(biāo)定。如前所述，帶有粗差的數(shù)據(jù)的數(shù)量較少，且粗差數(shù)據(jù)的重建誤差遠(yuǎn)離有效數(shù)據(jù)重建誤差的分布區(qū)域，因此考慮將誤差分布的概率密度估計(jì)值作為原始數(shù)據(jù)的權(quán)值，從而可以使帶粗差的數(shù)據(jù)的權(quán)值較小，而有效數(shù)據(jù)的權(quán)值較大。數(shù)據(jù)重建誤差的概率密度分布可以通過如下公式計(jì)算得到。

(20)

(21)

(22)

相似地，對(duì)GWHT進(jìn)行SVD分解，有：

svd(GWHT) =U∑VT

(23)

類似的，根據(jù)式(17)和式(18)可以計(jì)算出標(biāo)定參數(shù)中的旋轉(zhuǎn)矩陣R和平移矩陣T。

采用誤差分布概率密度估計(jì)的方法可以對(duì)具有不同誤差的原始數(shù)據(jù)賦以不同的權(quán)重，從而克服部分粗差對(duì)手眼標(biāo)定精度的影響。然而，在上述步驟中，數(shù)據(jù)重建誤差分布情況與最小二乘估計(jì)標(biāo)定的結(jié)果相關(guān)。因此，經(jīng)初次標(biāo)定后，數(shù)據(jù)重建后誤差的分布略偏離真實(shí)值。為了進(jìn)一步提高數(shù)據(jù)重建誤差的準(zhǔn)確度，提出了一種基于迭代策略的手眼標(biāo)定算法，即重復(fù)進(jìn)行基于數(shù)據(jù)重建和誤差分布估計(jì)的加權(quán)最小二乘手眼標(biāo)定過程。標(biāo)定結(jié)果隨著迭代過程逐漸收斂到最優(yōu)標(biāo)定結(jié)果?；谡`差分布估計(jì)加權(quán)最小二乘迭代策略的手眼標(biāo)定算法的流程如圖2所示。

圖2 可擴(kuò)展手眼標(biāo)定算法流程圖

2 實(shí)驗(yàn)驗(yàn)證

2.1 實(shí)驗(yàn)設(shè)計(jì)

在服務(wù)機(jī)器人抓取應(yīng)用中，為了能夠提高物體抓取的可靠性，需要精確的手眼標(biāo)定。為了驗(yàn)證所提算法對(duì)機(jī)器人物體抓取應(yīng)用的適用性和可靠性，設(shè)計(jì)了相應(yīng)的服務(wù)機(jī)器人手眼標(biāo)定實(shí)驗(yàn)。在實(shí)驗(yàn)前，已經(jīng)對(duì)相機(jī)和機(jī)器人的參數(shù)進(jìn)行了標(biāo)定。由于可以直接獲得環(huán)境的深度信息，RGB-D相機(jī)成為機(jī)器人應(yīng)用中較受關(guān)注的視覺傳感器之一，并廣泛應(yīng)用于服務(wù)機(jī)器人的物體抓取任務(wù)。在服務(wù)機(jī)器人手眼標(biāo)定實(shí)驗(yàn)中，在機(jī)器人末端執(zhí)行器上設(shè)定視覺參考標(biāo)志。示教末端執(zhí)行器運(yùn)動(dòng)至不同的空間位置。通過RGB-D視覺傳感器測(cè)量參考標(biāo)志在相機(jī)坐標(biāo)系下的三維坐標(biāo)，同時(shí)通過運(yùn)動(dòng)學(xué)求解機(jī)器人末端執(zhí)行器在機(jī)器人坐標(biāo)系下的位置坐標(biāo)。此時(shí)，可以獲得一系列空間中點(diǎn)在相機(jī)坐標(biāo)系和機(jī)器人坐標(biāo)系下的坐標(biāo)對(duì)。在標(biāo)定實(shí)驗(yàn)中，共獲得150對(duì)測(cè)試坐標(biāo)對(duì)，其中110對(duì)坐標(biāo)用于計(jì)算手眼變換矩陣，另外4組每組10對(duì)坐標(biāo)數(shù)據(jù)對(duì)用于對(duì)標(biāo)定結(jié)果進(jìn)行驗(yàn)證。實(shí)驗(yàn)結(jié)果采用坐標(biāo)重建結(jié)果誤差的平均值，即重建坐標(biāo)距離的平均值作為標(biāo)定精度的衡量指標(biāo)。坐標(biāo)重建誤差均值定義如下：

(24)

為了有效評(píng)估算法性能，設(shè)計(jì)了兩個(gè)實(shí)驗(yàn)。首先，通過實(shí)驗(yàn)評(píng)估了迭代次數(shù)對(duì)坐標(biāo)重建誤差均值的影響。其次，比較了算法與現(xiàn)有手眼標(biāo)定算法的精度。

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

2.2.1 迭代次數(shù)對(duì)坐標(biāo)重建誤差均值的影響

由于所提算法中迭代次數(shù)影響標(biāo)定結(jié)果的精度，因此實(shí)驗(yàn)首先討論了不同迭代次數(shù)對(duì)標(biāo)定結(jié)果的影響。實(shí)驗(yàn)中，最小迭代次數(shù)為0次，最大迭代次數(shù)為10次。當(dāng)?shù)螖?shù)為0次時(shí)，算法退化為最小二乘算法。不同迭代次數(shù)在測(cè)試集上的實(shí)驗(yàn)結(jié)果如圖3所示。由圖3可知，隨著迭代次數(shù)的增加，坐標(biāo)數(shù)據(jù)重建誤差逐漸減小。數(shù)據(jù)重建平均誤差逐漸減小是因?yàn)殡S著迭代次數(shù)的增加，算法對(duì)數(shù)據(jù)重建誤差的分布情況的估計(jì)越來(lái)越準(zhǔn)確，標(biāo)定結(jié)果也越來(lái)越接近真實(shí)值。良好的標(biāo)定結(jié)果使得數(shù)據(jù)重建后也越來(lái)越接近真實(shí)值。另外，隨著迭代次數(shù)的增加，誤差減小的速率逐漸降低。當(dāng)?shù)螖?shù)大于等于3次后，誤差均值不再發(fā)生明顯改變?？梢哉J(rèn)為，手眼標(biāo)定中，誤差分布估計(jì)加權(quán)最小二乘方法手眼標(biāo)定的合理迭代次數(shù)為3次。過小的迭代次數(shù)會(huì)使標(biāo)定結(jié)果誤差較大，而迭代次數(shù)超過3次則會(huì)提高標(biāo)定的時(shí)間開銷。

圖3 粗標(biāo)定數(shù)據(jù)重建誤差的分布情況

2.2.2 不同標(biāo)定算法對(duì)比

在實(shí)驗(yàn)中，分別采用最小二乘估計(jì)方法、凸松弛全局優(yōu)化算法和基于誤差分布估計(jì)加權(quán)最小二乘算法對(duì)服務(wù)機(jī)器人進(jìn)行了手眼標(biāo)定實(shí)驗(yàn)。其中基于誤差分布估計(jì)加權(quán)最小二乘算法的迭代次數(shù)為3次。不同算法的標(biāo)定結(jié)果如表1所示，坐標(biāo)數(shù)據(jù)重建誤差如表2所示。

表1 不同算法標(biāo)定結(jié)果對(duì)比

表2 不同算法測(cè)試集數(shù)據(jù)重建誤差均值

由表2可知，由所提算法計(jì)算得到的標(biāo)定結(jié)果對(duì)4組測(cè)試集的坐標(biāo)重建誤差均值比采用最小二乘估計(jì)算法和凸松弛全局優(yōu)化算法的重建誤差小。此實(shí)驗(yàn)結(jié)果證明所提算法的有效性。在服務(wù)機(jī)器人物體抓取應(yīng)用中，由所提標(biāo)定算法的標(biāo)定精度能夠滿足物體抓取作業(yè)的要求。

3 結(jié)論

為適應(yīng)服務(wù)機(jī)器人物體抓取任務(wù)的需求，在搭建的服務(wù)機(jī)器人物體抓取平臺(tái)的基礎(chǔ)上，對(duì)Eye-To-Hand相機(jī)安裝方式手眼標(biāo)定算法進(jìn)行了研究。為了減小數(shù)據(jù)粗差對(duì)標(biāo)定精度的影響，提高機(jī)器人手眼標(biāo)定精度，提出了一種基于誤差分布概率密度估計(jì)的加權(quán)最小二乘估計(jì)算法。通過粗標(biāo)定結(jié)果計(jì)算原始數(shù)據(jù)的重建誤差，并根據(jù)重建誤差的概率密度分布為原始數(shù)據(jù)賦以相應(yīng)權(quán)重，重新計(jì)算標(biāo)定結(jié)果，從而有效降低帶粗差數(shù)據(jù)對(duì)標(biāo)定結(jié)果的影響。另外，為了解決初次標(biāo)定后，數(shù)據(jù)重建后誤差的分布偏離真實(shí)值的情況，采用迭代求解標(biāo)定策略進(jìn)一步提高手眼標(biāo)定的精度。實(shí)驗(yàn)結(jié)果表明，所提標(biāo)定算法精度高、易操作，能夠滿足服務(wù)機(jī)器人物體抓取的需求。

參考文獻(xiàn)：

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

[2] Lee S, Ro S. A self-calibration model for hand-eye systems with motion estimation [J]. Mathematical and Computer Modelling, 1996, 24(5-6): 49-77.

[3] Daniilidis K. Hand-eye calibration using dual quaternions [J]. The International Journal of Robotics Research, 1999,18(3):286-298.

[4] Chou J C K, Kamel M. Finding the position and orientation of a sensor on a robot manipulator using quaternions [J]. The International Journal of Robotics Research, 1991, 10(3): 240-254.

[5] Horaud R, Dornaika F. Hand-eye calibration [J]. The International Journal of Robotics Research, 1995, 14(3): 195-210.

[6] Fassi I, Legnani G. Hand to sensor calibration: A geometrical interpretation of the matrix equation AX=XB [J]. Journal of Field Robotics, 2005, 22(9): 497-506.

[7] Schmidt J, Heinrich N. Data selection for hand-eye calibration: a vector quantization approach [J]. The International Journal of Robotics Research,2008, 28(9):1027-1053.

[8] Zhuang H, Roth Z S, Shiu Y C, et al. Comments on" Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form AX= XB"[with reply][J]. IEEE Transactions on Robotics and Automation, 1991, 7(6): 877-878.

[9] Wei G Q, Arbter K, Hirzinger 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.

[10] 王金橋，段發(fā)階，汪潤(rùn). 精確標(biāo)定關(guān)節(jié)臂視覺檢測(cè)系統(tǒng)手眼關(guān)系[J]. 計(jì)算機(jī)工程與應(yīng)用，2015，51(21):225-229.

[11] 鄒勁松，黃凱鋒. 一種新的三維測(cè)量機(jī)器人手眼標(biāo)定方法[J]. 計(jì)算機(jī)測(cè)量與控制，2015, 23(7): 2270-2273.

[12] 李 巍，呂乃光，董明利，等. 凸松弛全局優(yōu)化機(jī)器人手眼標(biāo)定[J]. 計(jì)算機(jī)應(yīng)用. 2017, 37(5): 1451-1455.

[13] Jiang R, Lo K M. Optimal adaptive controller for stochastic systems based on weighted least-squares algorithm [J]. ACTA Automatica Sinica, 2006, 42: 140-147.

[14] Chang L C, Jones D K, Pierpaoli C. RESTORE: robust estimation of tensors by outlier rejection [J]. Magnetic resonance in medicine, 2005, 53(5): 1088-1095.

[15] Loh P L, Wainwright M J. Regularized M-estimators with nonconvexity: Statistical and algorithmic theory for local optima[A].Advances in Neural Information Processing Systems[C]. 2013: 476-484.