專利名稱：基于運(yùn)動(dòng)選擇的機(jī)器人在線手眼標(biāo)定方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種基于運(yùn)動(dòng)選擇的機(jī)器人在線手眼標(biāo)定方法，可廣泛應(yīng)用于機(jī)器人三維視覺測(cè)量、視覺伺服和觸覺感知等方面。屬于先進(jìn)制造與自動(dòng)化領(lǐng)域。
背景技術(shù)：
在計(jì)算機(jī)視覺應(yīng)用于機(jī)器人時(shí)，經(jīng)常將攝像機(jī)固定在機(jī)器人手臂的末端執(zhí)行器(機(jī)器人手爪)上。機(jī)器人手眼標(biāo)定就是測(cè)量相機(jī)和機(jī)器人手爪之間的相對(duì)位置和方向關(guān)系，它是機(jī)器人學(xué)中的一個(gè)基本問題。在解決這類問題時(shí)，以前大部分方法都是通過迭代優(yōu)化的方式求解齊次變換方程AX＝XB(Y.C.Shiu andS.Ahmad，″Calibration of wrist-mounted robotic sensors by solving homogeneoustransform equations of the form AX＝XB，″IEEE Trans.Robot.Automat.，vol.5，pp.16-29，F(xiàn)eb.1989.)來進(jìn)行標(biāo)定的，這里A代表機(jī)器人手爪的運(yùn)動(dòng)，B代表相應(yīng)的攝像機(jī)運(yùn)動(dòng)，X為所要標(biāo)定的攝像機(jī)和機(jī)器人手爪之間的空間變換關(guān)系。由于迭代優(yōu)化計(jì)算過程是非實(shí)時(shí)的，所以這種情況下手眼標(biāo)定只能脫機(jī)進(jìn)行。Angeles等(J.Angeles，G.Soucy and F.P.Ferrie，“The online solution of the hand-eyeproblem”，IEEE Trans.Robot.Automat.，vol.16，pp.720-731，Dec.2000.)和Andreff等(N.Andreff，R.Horaud，and B.Espiau，“Robot hand-eye calibration usingstructure-from-motion”，Int.J.Robot.Res.，20(3)228-248，2001.)幾乎同時(shí)提出了手眼標(biāo)定的在線實(shí)現(xiàn)技術(shù)，這樣就克服了傳統(tǒng)方法不能進(jìn)行實(shí)時(shí)標(biāo)定的缺點(diǎn)。Angeles的方法是基于旋轉(zhuǎn)矩陣的線性不變量，采用遞歸的線性最小二乘實(shí)現(xiàn)的。Andreff則受Sylvester方程的啟發(fā)，直接將手眼運(yùn)動(dòng)方程化成線性形式來求解。
無論采用何種方法進(jìn)行手眼標(biāo)定，手眼關(guān)系的計(jì)算都要求機(jī)器人至少獨(dú)立運(yùn)動(dòng)兩次，而且兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸不得平行。因此，當(dāng)采集到的手眼運(yùn)動(dòng)數(shù)據(jù)存在退化情況時(shí)，如純旋轉(zhuǎn)或純平移等，就無法得到手眼變換關(guān)系的完整解?，F(xiàn)有方法的不足之處在于機(jī)器人在工作過程當(dāng)中進(jìn)行在線手眼標(biāo)定時(shí)，機(jī)器人的運(yùn)動(dòng)是由具體應(yīng)用確定的而并非為手眼標(biāo)定而設(shè)計(jì)，所以此時(shí)采集到的用于手眼標(biāo)定的運(yùn)動(dòng)數(shù)據(jù)就很有可能存在退化情形。另外，采樣運(yùn)動(dòng)中旋轉(zhuǎn)角很小或者平移比較大，或者兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間的夾角很小時(shí)，會(huì)產(chǎn)生比較大的標(biāo)定誤差。以上情形的出現(xiàn)都會(huì)使現(xiàn)有算法不能正常工作或失效。

發(fā)明內(nèi)容
本發(fā)明的目的在于針對(duì)現(xiàn)有技術(shù)的不足和缺點(diǎn)，充分利用現(xiàn)有采樣到的手眼運(yùn)動(dòng)數(shù)據(jù)，提出了一種基于運(yùn)動(dòng)選擇的在線手眼標(biāo)定方法，以去除退化手眼運(yùn)動(dòng)對(duì)手眼變換關(guān)系正常計(jì)算的影響，同時(shí)還可以通過避免小角度旋轉(zhuǎn)等措施來減小計(jì)算誤差，提高機(jī)器人手眼標(biāo)定精度。
本發(fā)明的技術(shù)方案首先設(shè)定運(yùn)動(dòng)選擇的三個(gè)閾值，即機(jī)器人手爪前后兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間夾角的最小閾值α、機(jī)器人手爪每次運(yùn)動(dòng)的旋轉(zhuǎn)角的最小閾值β和機(jī)器人手爪每次運(yùn)動(dòng)的平移分量的模的最大閾值d。然后，從第一次手眼采樣運(yùn)動(dòng)開始，依次選擇出兩個(gè)符合要求的手眼運(yùn)動(dòng)對(duì)，最后以選擇出的兩個(gè)手眼運(yùn)動(dòng)對(duì)利用Andreff線性算法計(jì)算得到手眼變換關(guān)系。繼續(xù)下一次標(biāo)定時(shí)，可將上一次用于標(biāo)定的第二個(gè)運(yùn)動(dòng)對(duì)作為下一次標(biāo)定的第一個(gè)運(yùn)動(dòng)對(duì)，并從后續(xù)采樣運(yùn)動(dòng)數(shù)據(jù)中搜索選擇另一個(gè)運(yùn)動(dòng)對(duì)，然后進(jìn)行新的手眼標(biāo)定計(jì)算。如此循環(huán)往復(fù)，可連續(xù)不斷地進(jìn)行機(jī)器人的在線手眼標(biāo)定操作。
本發(fā)明的在線手眼標(biāo)定方法主要包括以下幾個(gè)步驟1.設(shè)定運(yùn)動(dòng)選擇的三個(gè)閾值機(jī)器人手爪前后兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間夾角的最小閾值α、機(jī)器人手爪每次運(yùn)動(dòng)的旋轉(zhuǎn)角的最小閾值β和機(jī)器人手爪每次運(yùn)動(dòng)的平移分量的模的最大閾值d。
2.搜索選擇第一個(gè)滿足閾值條件的手眼運(yùn)動(dòng)對(duì)(A′，B′)。首先由第一次和第二次采樣到的手眼姿態(tài)可以計(jì)算得到一個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)，如果A′的旋轉(zhuǎn)角大于等于β且A′的平移分量的模小于等于d，則得到第一個(gè)滿足閾值條件的運(yùn)動(dòng)對(duì)(A′，B′)。否則，再由第一次和第三次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)對(duì)(A′，B′)并判斷A′的旋轉(zhuǎn)角和平移分量的模是否滿足設(shè)定條件。如此循環(huán)往復(fù)，直至找到符合條件的第一個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)，假設(shè)該運(yùn)動(dòng)對(duì)是由第一次和第i+1次采樣的手眼姿態(tài)計(jì)算得到的。
3.搜索選擇第二個(gè)滿足閾值條件的手眼運(yùn)動(dòng)對(duì)(A″，B″)。首先由第i+1次和第i+2次采樣到的手眼姿態(tài)可以計(jì)算得到一個(gè)手眼運(yùn)動(dòng)對(duì)(A″，B″)，如果A″的旋轉(zhuǎn)角大于等于β、A″的平移分量的模小于等于d和A′與A″的旋轉(zhuǎn)軸之間的夾角大于等于α，則得到第二個(gè)滿足閾值條件的運(yùn)動(dòng)對(duì)(A″，B″)。否則，再由第i+1次和第i+3次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)(A″，B″)并判斷A″的旋轉(zhuǎn)角、A″的平移分量的模以及A′與A″的旋轉(zhuǎn)軸之間的夾角是否滿足設(shè)定條件。如此循環(huán)往復(fù)，直至找到符合條件的第二個(gè)手眼運(yùn)動(dòng)對(duì)(A″，B″)。
4.利用兩個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)和(A″，B″)進(jìn)行手眼標(biāo)定。即利用手眼運(yùn)動(dòng)對(duì)(A′，B′)和(A″，B″)列出Andreff的線性方程，求解該線性方程得到手眼變換關(guān)系矩陣，完成一次手眼標(biāo)定。
5.若繼續(xù)下一次手眼標(biāo)定時(shí)，可將上次用于標(biāo)定的手眼運(yùn)動(dòng)對(duì)(A″，B″)作為新的手眼運(yùn)動(dòng)對(duì)(A′，B′)，重復(fù)步驟3從后續(xù)采樣數(shù)據(jù)中繼續(xù)搜索選擇新的手眼運(yùn)動(dòng)對(duì)(A″，B″)，重復(fù)步驟4進(jìn)行新的手眼標(biāo)定。
實(shí)際應(yīng)用時(shí)，根據(jù)事先設(shè)定的三個(gè)閾值，由軟件自動(dòng)進(jìn)行運(yùn)動(dòng)選擇和手眼標(biāo)定計(jì)算。本發(fā)明所提出的手眼標(biāo)定方法，不但可以防止采樣數(shù)據(jù)中出現(xiàn)的退化運(yùn)動(dòng)影響手眼標(biāo)定的正常求解，而且還可以避免小旋轉(zhuǎn)角的運(yùn)動(dòng)引起較大的標(biāo)定誤差，因而提高標(biāo)定精度。
本發(fā)明提出的在線手眼標(biāo)定方法，可廣泛應(yīng)用于機(jī)器人三維視覺測(cè)量、視覺伺服和觸覺感知等方面，具有相當(dāng)?shù)膶?shí)用價(jià)值。


圖1為本發(fā)明的機(jī)器人手眼標(biāo)定模型示意圖。
圖2為本發(fā)明的用于在線手眼標(biāo)定的運(yùn)動(dòng)選擇算法示意圖。
具體實(shí)施例方式
為了更好地講解本發(fā)明的技術(shù)方案，以下結(jié)合附圖和實(shí)施例作進(jìn)一步的詳細(xì)描述。
1.設(shè)定運(yùn)動(dòng)選擇的三個(gè)閾值機(jī)器人手爪前后兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間夾角的最小閾值α、機(jī)器人手爪每次運(yùn)動(dòng)的旋轉(zhuǎn)角的最小閾值β和機(jī)器人手爪每次運(yùn)動(dòng)的平移分量的模的最大閾值d。一般情況下選α＝β＝30°，d＝100。設(shè)第i(i為自然數(shù))次采樣到的攝像機(jī)和手爪姿態(tài)的齊次矩陣分別為Pi、Qi，X為所需要求解的手眼變換關(guān)系齊次矩陣。
2.搜索選擇第一個(gè)滿足條件的手眼運(yùn)動(dòng)對(duì)(A′，B′)。首先從前兩次的手眼姿態(tài)可以求出第一次手爪運(yùn)動(dòng)的齊次矩陣A1和相機(jī)運(yùn)動(dòng)的齊次矩陣B1，如圖1所示，P1、P2分別為攝像機(jī)在第1和第2時(shí)刻相對(duì)于標(biāo)定參照物的姿態(tài)齊次矩陣，Q1、Q2分別為機(jī)器人手爪在第1和第2時(shí)刻在機(jī)器人基坐標(biāo)系下的姿態(tài)齊次矩陣，則有A1=Q1-1Q2,B1=P1-1P2]]>設(shè)(A′，B′)＝(A1，B1)。如果A′的旋轉(zhuǎn)角大于等于β而且A′平移分量的模小于等于d，則當(dāng)前運(yùn)動(dòng)對(duì)(A′，B′)滿足給定閾值條件，否則再由第一次和第三次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)對(duì)(A′，B′)并判斷A′的旋轉(zhuǎn)角和A′的平移的模是否滿足以上條件。如此循環(huán)往復(fù)，直至找到符合條件的手眼運(yùn)動(dòng)對(duì)作為(A′，B′)，設(shè)此時(shí)(A′，B′)由第一次和第i+1次采樣的手眼姿態(tài)(即經(jīng)過了i次手眼運(yùn)動(dòng))計(jì)算得到的。圖2示出了搜索第一個(gè)滿足條件的手眼運(yùn)動(dòng)對(duì)的完整過程。
3.從第i+1次采樣的手眼姿態(tài)開始(即第i次運(yùn)動(dòng)后)搜索選擇第二個(gè)滿足條件的手眼運(yùn)動(dòng)對(duì)(A″，B″)，如圖二所示。首先從第i+1次和第i+2次的手眼姿態(tài)可以求出手爪運(yùn)動(dòng)齊次矩陣Ai+1和相機(jī)運(yùn)動(dòng)齊次矩陣Bi+1，即有Ai+1=Qi+1-1Qi+2,Bi+1=Pi+1-1Pi+2]]>設(shè)(A″，B″)＝(Ai+1，Bi+1)。如果A″的旋轉(zhuǎn)角大于等于β、A″的平移分量的模小于等于d而且A′與A″的旋轉(zhuǎn)軸之間的夾角大于等于α，則當(dāng)前運(yùn)動(dòng)對(duì)(A″，B″)滿足給定閾值條件，否則再由第i+1次和第i+3次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)對(duì)(A″，B″)并判斷A″的旋轉(zhuǎn)角、A″的平移分量的模和A′與A″的旋轉(zhuǎn)軸之間的夾角是否滿足以上條件，如此循環(huán)往復(fù)，直至找到符合條件的手眼運(yùn)動(dòng)對(duì)作為(A″，B″)。
4.利用步驟2、3中選擇出的兩個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)和(A″，B″)進(jìn)行手眼標(biāo)定。根據(jù)手眼運(yùn)動(dòng)對(duì)(A′，B′)和(A″，B″)列出Andreff手眼標(biāo)定線性方程，求解該方程得到手眼變換關(guān)系矩陣X，完成一次手眼標(biāo)定。
5.如果需要繼續(xù)標(biāo)定，可將上次用于標(biāo)定的手眼運(yùn)動(dòng)對(duì)(A″，B″)作為新的手眼運(yùn)動(dòng)對(duì)(A′，B′)，重復(fù)步驟3從后續(xù)手眼姿態(tài)采樣數(shù)據(jù)中開始搜索選擇新的手眼運(yùn)動(dòng)對(duì)(A″，B″)，重復(fù)步驟4進(jìn)行新的手眼標(biāo)定。
權(quán)利要求
1.一種基于運(yùn)動(dòng)選擇的機(jī)器人在線手眼標(biāo)定方法，其特征在于包括如下具體步驟1)設(shè)定運(yùn)動(dòng)選擇的三個(gè)閾值設(shè)定機(jī)器人手爪前后兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間夾角的最小閾值α、機(jī)器人手爪每次運(yùn)動(dòng)的旋轉(zhuǎn)角的最小閾值β和機(jī)器人手爪每次運(yùn)動(dòng)的平移分量的模的最大閾值d；2)搜索選擇第一個(gè)滿足條件的手眼運(yùn)動(dòng)對(duì)(A′，B′)由第一次和第二次采樣到的手眼姿態(tài)計(jì)算得到一個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)，如果A′的旋轉(zhuǎn)角大于等于β且A′的平移分量的模小于等于d，則認(rèn)為(A′，B′)找到，否則，再由第一次和第三次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)對(duì)(A′，B′)并判斷A′的旋轉(zhuǎn)角和平移分量的模是否滿足設(shè)定條件，如此循環(huán)往復(fù)，直至找到符合條件的手眼運(yùn)動(dòng)對(duì)作為(A′，B′)，設(shè)此時(shí)的(A′，B′)由第一次和第i+1次采樣的手眼姿態(tài)計(jì)算得到；3)搜索選擇第二個(gè)滿足條件的手眼運(yùn)動(dòng)對(duì)(A″，B″)由第i+1次和第i+2次采樣到的手眼姿態(tài)計(jì)算得到一個(gè)手眼運(yùn)動(dòng)對(duì)(A″，B″)，如果A″的旋轉(zhuǎn)角大于等于β、A″的平移分量的模小于等于d而且A′與A″的旋轉(zhuǎn)軸之間的夾角大于等于α，則認(rèn)為(A″，B″)找到，否則，再由第i+1次和第i+3次采樣的手眼姿態(tài)計(jì)算手眼運(yùn)動(dòng)對(duì)(A″，B″)并判斷A″的旋轉(zhuǎn)角、A″的平移分量的模和A′與A″的旋轉(zhuǎn)軸之間的夾角是否滿足設(shè)定條件，如此循環(huán)往復(fù)，直至找到符合條件的手眼運(yùn)動(dòng)對(duì)(A″，B″)；4)利用兩個(gè)手眼運(yùn)動(dòng)對(duì)(A′，B′)和(A″，B″)進(jìn)行手眼標(biāo)定由(A′，B′)和(A″，B″)列出Andreff線性方程，求解該方程得到手眼變換關(guān)系矩陣，完成一次手眼標(biāo)定；5)若繼續(xù)標(biāo)定時(shí)，將上次用于標(biāo)定的手眼運(yùn)動(dòng)對(duì)(A″，B″)作為新的手眼運(yùn)動(dòng)對(duì)(A′，B′)，重復(fù)步驟3從后續(xù)采樣數(shù)據(jù)中繼續(xù)搜索選擇新的手眼運(yùn)動(dòng)對(duì)(A″，B″)，重復(fù)步驟4進(jìn)行新的手眼標(biāo)定。
全文摘要
一種基于運(yùn)動(dòng)選擇的機(jī)器人在線手眼標(biāo)定方法，首先設(shè)定機(jī)器人手爪前后兩次運(yùn)動(dòng)的旋轉(zhuǎn)軸之間夾角的最小閾值α、機(jī)器人手爪每次運(yùn)動(dòng)的旋轉(zhuǎn)角的最小閾值β和機(jī)器人手爪每次運(yùn)動(dòng)的平移分量的模的最大閾值d，然后從第一次手眼采樣運(yùn)動(dòng)開始，依次選擇出兩個(gè)符合要求的手眼運(yùn)動(dòng)對(duì)，最后以選擇出的兩個(gè)手眼運(yùn)動(dòng)對(duì)利用Andreff線性算法計(jì)算得到手眼變換關(guān)系矩陣，完成一次手眼標(biāo)定。繼續(xù)下一次標(biāo)定時(shí)，將上次標(biāo)定的第二個(gè)運(yùn)動(dòng)對(duì)作為第一個(gè)運(yùn)動(dòng)對(duì)，并從后續(xù)采樣運(yùn)動(dòng)數(shù)據(jù)中搜索選擇另一個(gè)運(yùn)動(dòng)對(duì)，然后進(jìn)行標(biāo)定計(jì)算。如此循環(huán)往復(fù)，可連續(xù)不斷地進(jìn)行機(jī)器人的在線手眼標(biāo)定操作。本發(fā)明可廣泛應(yīng)用于機(jī)器人三維視覺測(cè)量、視覺伺服和觸覺感知等實(shí)際工作中。
文檔編號(hào)B25J19/00GK1672881SQ20051002525
公開日2005年9月28日 申請(qǐng)日期2005年4月21日 優(yōu)先權(quán)日2005年4月21日
發(fā)明者石繁槐, 王建華, 劉允才 申請(qǐng)人:上海交通大學(xué)