基于誤差分布估計的機器人手眼標定方法研究

,, ,

(1.中國科學院 沈陽自動化研究所， 沈陽 110016; 2.中國科學院大學，北京 100049;3.沈陽新松機器人自動化股份有限公司，沈陽 110168)

0 引言

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

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

1 機器人手眼標定模型

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

1.1 基于最小二乘的手眼標定模型

(1)

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

RTR=I

(2)

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

(3)

令:

(4)

上述F對T求偏導數(shù)，有：

(5)

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

(6)

式中，

(7)

(8)

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

(9)

手眼標定參數(shù)中，旋轉矩陣應滿足如下公式：

(10)

令:

(11)

(12)

考慮到RTR=I，有：

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

(13)

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

(14)

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

svd(S) =U∑VT

(15)

此時：

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

(16)

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

(17)

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

(18)

1.2 基于誤差分布估計的手眼標定算法

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

(19)

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

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

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

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

(20)

(21)

(22)

相似地，對GWHT進行SVD分解，有：

svd(GWHT) =U∑VT

(23)

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

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

圖2 可擴展手眼標定算法流程圖

2 實驗驗證

2.1 實驗設計

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

(24)

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

2.2 實驗結果比較與分析

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

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

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

2.2.2 不同標定算法對比

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

表1 不同算法標定結果對比

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

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

3 結論

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

參考文獻：

[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ā)階，汪潤. 精確標定關節(jié)臂視覺檢測系統(tǒng)手眼關系[J]. 計算機工程與應用，2015，51(21):225-229.

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

[12] 李 巍，呂乃光，董明利，等. 凸松弛全局優(yōu)化機器人手眼標定[J]. 計算機應用. 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.