一種基于空間兩點(diǎn)投影的協(xié)同機(jī)器人基座標(biāo)系標(biāo)定方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及了一種機(jī)器人標(biāo)定方法，尤其是涉及了一種基于空間兩點(diǎn)投影的協(xié)同 機(jī)器人基座標(biāo)系標(biāo)定方法，涉及工業(yè)生產(chǎn)中的焊接，搬運(yùn)，雕刻以及噴涂等諸多工業(yè)機(jī)器人 復(fù)雜柔性化系統(tǒng)制造領(lǐng)域。
【背景技術(shù)】
[0002] 隨著工業(yè)4. 0時(shí)代的到來(lái)，許多領(lǐng)域諸如大型復(fù)雜零部件的加工，裝配和搬運(yùn)，無(wú) 夾具系統(tǒng)的焊接，噴涂以及雕刻等，傳統(tǒng)的單個(gè)機(jī)器人已經(jīng)不能很好的滿足其加工要求，因 此研究具有協(xié)同作業(yè)關(guān)系的多機(jī)器人系統(tǒng)將成為工業(yè)發(fā)展的必然趨勢(shì)。多機(jī)器人系統(tǒng)在協(xié) 同作業(yè)時(shí)需要預(yù)先對(duì)其每個(gè)機(jī)器人的相對(duì)位置進(jìn)行標(biāo)定確認(rèn)，即對(duì)于具有協(xié)同作業(yè)關(guān)系的 機(jī)器人之間需要精確的對(duì)其基座標(biāo)系進(jìn)行標(biāo)定，從而準(zhǔn)確的知道每個(gè)機(jī)器人基座標(biāo)系之間 的相對(duì)位姿關(guān)系，然后才能對(duì)其運(yùn)行的軌跡進(jìn)行準(zhǔn)確規(guī)劃。
[0003] 當(dāng)前工業(yè)生產(chǎn)中，針對(duì)多機(jī)器人系統(tǒng)的基座標(biāo)系標(biāo)定依然缺乏較為有效的方法， 主要分為兩大類：接觸式標(biāo)定方法和非接觸式標(biāo)定方法，非接觸式標(biāo)定方法中一般都需要 增加外部傳感器和測(cè)量?jī)x等裝置，標(biāo)定步驟繁瑣以及精度也較為有限。現(xiàn)有的接觸式標(biāo)定 方法中，已經(jīng)提出有通過(guò)插孔、法蘭盤接觸等方法，標(biāo)定步驟較非接觸式標(biāo)定方法也有了一 定優(yōu)化，但插孔、法蘭盤重合等方式中示教的難度較大，需要選定的標(biāo)定點(diǎn)數(shù)量較多并且選 擇規(guī)則復(fù)雜。

【發(fā)明內(nèi)容】

[0004] 為了解決多機(jī)器人協(xié)同作業(yè)系統(tǒng)的基座標(biāo)系標(biāo)定問(wèn)題，本發(fā)明的目的是提出了一 種基于空間兩點(diǎn)投影的協(xié)同機(jī)器人基座標(biāo)系標(biāo)定方法，針對(duì)基座標(biāo)系z(mì)軸垂直于同一平面 的協(xié)同機(jī)器人系統(tǒng)，不需要增加外部的精密傳感儀器以及其他測(cè)量工具，更簡(jiǎn)單，高效，可 行性更強(qiáng)，通過(guò)讓具有協(xié)同作業(yè)關(guān)系的兩機(jī)器人以任意姿態(tài)握手到達(dá)空間的兩個(gè)標(biāo)定點(diǎn)， 基于空間投影的幾何約束關(guān)系，從而快速，精確的對(duì)其機(jī)器人基座標(biāo)系進(jìn)行相對(duì)位姿的標(biāo) 定。
[0005] 本發(fā)明采用的技術(shù)方案是采用以下步驟：
[0006] 第一步：將所有η臺(tái)機(jī)器人以相互之間具有協(xié)同關(guān)系的任意兩臺(tái)機(jī)器人為一個(gè)小 組作為最小標(biāo)定單位進(jìn)行劃分；
[0007] 所述第一步中所有η臺(tái)機(jī)器人劃分后至少共劃分有η-1個(gè)小組。
[0008] 第二步：在最小標(biāo)定單位的兩臺(tái)機(jī)器人R1, &中，建立兩臺(tái)機(jī)器人各自的基座標(biāo)系 F1, Fj，在兩臺(tái)機(jī)器人各自基座標(biāo)系原點(diǎn)O1, Oj連線的兩側(cè)各任意確定一個(gè)標(biāo)定點(diǎn)，作為公共 工作空間中的兩個(gè)標(biāo)定點(diǎn)Pl和Ρ2 ;
[0009] 第三步：將該兩臺(tái)機(jī)器人的末端點(diǎn)分別在標(biāo)定點(diǎn)Pl和標(biāo)定點(diǎn)Ρ2處以握手方式接 觸，獲得以下各自機(jī)器人的關(guān)節(jié)轉(zhuǎn)角信息；
[0010] 1R1 (1 Θ 丄，1 Θ 2, 1 Θ 3".1 Θ n) jR1 (1 Θ 丄，1 Θ 2, 1 Θ 3".1 θ J
[0011] 1R2C Θ p 1 Θ 2, 1 Θ 3"·1 Θ n) jR2C Θ p 1 Θ 2, 1 Θ 3…1 Θ n)
[0012] 其中，1R1 C θ ρ 1 θ 2, 1 θ 3···1 θ η)表示機(jī)器人R1在第一標(biāo)定點(diǎn)Pl處各關(guān)節(jié)轉(zhuǎn)角值， 1R2Cei, 12, 表示機(jī)器人R1在第二標(biāo)定點(diǎn)Ρ2處各關(guān)節(jié)轉(zhuǎn)角值，iR 1Cei, 2， 1Qf1Gn)表示機(jī)器人Rj在第一標(biāo)定點(diǎn)Pl處各關(guān)節(jié)轉(zhuǎn)角值； iR2Cei, 2, 表 示機(jī)器人Rj在第二標(biāo)定點(diǎn)Ρ2處各關(guān)節(jié)轉(zhuǎn)角值。
[0013] 第四步：分別建立該兩臺(tái)機(jī)器人的D-H坐標(biāo)系，利用關(guān)節(jié)轉(zhuǎn)角信息計(jì)算出以下各 自的末端姿態(tài)矩陣'0Tq和末端姿態(tài)矩陣h^1Thi ::
[0015] 其中，k = 1或2，k = 1分別表示機(jī)器人在標(biāo)定點(diǎn)Pl處，k = 2分別表示機(jī)器人在 標(biāo)定點(diǎn)P2處，
：分別表示該兩機(jī)器人在標(biāo)定點(diǎn)Pl，P2處末 端點(diǎn)姿態(tài)相對(duì)于各基座標(biāo)系的旋轉(zhuǎn)矩陣，iiu感^、Jtit分別表示機(jī)器人民的末端點(diǎn) 在基座標(biāo)系F1中的X，y，z坐標(biāo)值；#&、、/?么分別表示機(jī)器人&的末端點(diǎn)在基座 標(biāo)系Fj中的X，y，z坐標(biāo)值；再計(jì)算出各末端對(duì)應(yīng)的RPY轉(zhuǎn)角中繞基座標(biāo)系z(mì)軸旋轉(zhuǎn)角γ。
[0016] 第五步：將該兩臺(tái)機(jī)器人以及兩個(gè)標(biāo)定點(diǎn)Pl和Ρ2進(jìn)行投影，利用其平面幾何約束 關(guān)系，計(jì)算得到相對(duì)水平平移矩陣丨Γ、相對(duì)水平旋轉(zhuǎn)矩陣:沐和相對(duì)垂直平移矩陣T ;
[0017] 所述的第五步具體為：
[0018] 將該兩臺(tái)機(jī)器人以及兩個(gè)標(biāo)定點(diǎn)Pl和Ρ2向水平面投影，利用其平面幾何約束關(guān) 系，計(jì)算出水平面上該兩臺(tái)機(jī)器人基座標(biāo)F1, F,之間的相對(duì)水平平移矩陣貧和相對(duì)水平旋 轉(zhuǎn)矩陣
[0019] 將兩臺(tái)協(xié)同作業(yè)的機(jī)器人以及兩個(gè)標(biāo)定點(diǎn)向垂直面投影，利用其平面幾何約束關(guān) 系，計(jì)算出垂直面上兩臺(tái)協(xié)作機(jī)器人基座標(biāo)F1, F,之間的相對(duì)垂直平移矩陣。
[0020] 所述相對(duì)水平平移矩陣，和相對(duì)水平旋轉(zhuǎn)矩陣iS具體采用以下方式得到：
[0021] 建立XYZ坐標(biāo)系，z軸方向豎直向上，將該兩臺(tái)機(jī)器人以及標(biāo)定點(diǎn)Pl和標(biāo)定點(diǎn)P2 向水平的X-Y平面投影，利用X-Y平面投影的幾何約束關(guān)系以及RPY角中的繞z軸旋轉(zhuǎn)角 γ，計(jì)算出其中一機(jī)器人R,相對(duì)于另一機(jī)器人Ri的基座標(biāo)系在X-Y平面上的X方向偏移量 PdP y方向偏移量P y以及相對(duì)于繞z軸的旋轉(zhuǎn)量θ ratz，由X方向偏移量Pjp y方向偏移 量Py組合得到相對(duì)水平平移矩陣由旋轉(zhuǎn)量Sratz得到相對(duì)水平旋轉(zhuǎn)矩陣丨況。
[0022] 如附圖4所示，P/，為標(biāo)定點(diǎn)Pl和標(biāo)定點(diǎn)P2在x-y平面上的兩個(gè)投影點(diǎn)， Al (K-l, PU )，(P.U , )和 P7I (<I，K-I ), Pp (k- 2，K 2 )分別表示兩臺(tái)機(jī)器人 R1，R j末端 點(diǎn)處于標(biāo)定點(diǎn)Pl和標(biāo)定點(diǎn)P2時(shí)在x-y平面上的投影，O1'，0/分別為兩臺(tái)協(xié)同機(jī)器人的 基座標(biāo)系原點(diǎn)在x-y平面的投影點(diǎn)，A i，Γι 2分別表示機(jī)器人R 1末端點(diǎn)在Pl和P2處位姿 的RPY轉(zhuǎn)角γ i i，γ i 2在x-y平面的投影，r j i，rj 2分別表示機(jī)器人R ;末端點(diǎn)在Pl和Ρ2處 位姿的RPY轉(zhuǎn)角γ P γ i 2在χ-y平面的投影。
[0023] 在x-y平面確定兩協(xié)同關(guān)系的機(jī)器人基座標(biāo)系的相對(duì)位姿，采用以下公式先求解 得到標(biāo)定點(diǎn)與基座標(biāo)系原點(diǎn)之間的距離，再進(jìn)一步求解出兩基座標(biāo)系之間在x-y平面上投 影的旋轉(zhuǎn)角Θ rotz和平移偏量（P X，Py):
[0026] 其中，IV lPll I，|〇' lP' i I表示機(jī)器人R1的末端點(diǎn)在Pl處與基坐標(biāo)系F1原點(diǎn) (^之間的連線長(zhǎng)度的水平投影，IV _]P]1|，|c/ 」表示機(jī)器人R,的末端點(diǎn)在標(biāo)定點(diǎn)Pl 處與基坐標(biāo)系F]原點(diǎn)0]之間的連線長(zhǎng)度的水平投影，lo' lPl2|，lo' lP' 2|表示機(jī)器人 R1的末端點(diǎn)在標(biāo)定點(diǎn)P2處與基坐標(biāo)系F1原點(diǎn)1之間的連線長(zhǎng)度的水平投影，|〇' ]P]2|， 〇' _]P' 2|表示機(jī)器人R,的末端點(diǎn)在標(biāo)定點(diǎn)P2處與基坐標(biāo)系F ,原點(diǎn)0 ,之間的連線長(zhǎng)度 的水平投影。
[0027] 附圖4所示的三角形Vo' lP' lP' 2中，已知第一機(jī)器人R1在兩個(gè)標(biāo)定點(diǎn)之間的 旋轉(zhuǎn)角L= |r ; ^ri 2|和第二機(jī)器人Rj在兩個(gè)標(biāo)定點(diǎn)之間的旋轉(zhuǎn)角r _j= |r ^rj 2|利用 余弦定理，可計(jì)算出Zi以及|p/ P2' |，其中|p' iP' 2|表示兩標(biāo)定點(diǎn)Pi和P2連線在 x-y水平面的投影長(zhǎng)度，Z 1表示lo' lPl2|與|p' lP' 2|之間的夾角。
[0028] 同理在Vo' .jP' iP' 2中，可計(jì)算出Z 2,在Vo' iAp' 2，Vo' .jBp' 2中采用以下 公式可依次計(jì)算出：
[0029] Z 3 = π - ( Z Ι+lr, 2|)
[0030] Z 4 = π - ( Z 2+1 T j 21)
[0031] Z 5 = Z 1+ Z 2
[0032] 其中，Z 3表示機(jī)器人R1的基坐標(biāo)系F沖X軸與|p， lP，21之間的夾角，Z 4 表示機(jī)器人Rj的基坐標(biāo)系F ,中X軸與|p' lP' 2|之間的夾角，Z5表示lo' lP' 2|與 c/ y 2|之間的夾角；
[0033] 在三角形Vo' # .j中，利