[發(fā)明專利]一種6R型工業(yè)機(jī)器人負(fù)載識別方法

有效

• 申請?zhí)枺?/td>: 201310456167.4
• 申請日: 2013-09-29
• 公開（公告）號: CN103495977A
• 公開（公告）日: 2014-01-08
• 發(fā)明（設(shè)計）人: 陳友東;谷平平;季旭東
• 申請（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/00
• 代理公司: 北京永創(chuàng)新實專利事務(wù)所 11121
• 代理人: 周長琪

1.一種6R型工業(yè)機(jī)器人負(fù)載識別方法，其特征在于：通過下述步驟實現(xiàn)：

步驟1：根據(jù)D-H方法，建立6R型工業(yè)機(jī)器人的連桿坐標(biāo)系；

6R型工業(yè)機(jī)器人是指全部關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)的6自由度工業(yè)機(jī)器人；6R型工業(yè)機(jī)器人可視為由連桿和關(guān)節(jié)組成。在6R型工業(yè)機(jī)器人的連桿中，將固定基座設(shè)為連桿0，然后由基座一側(cè)到末端執(zhí)行器一側(cè)的各個可動連桿按照1、2、3、4、5、6的順序依次編號，由基座一側(cè)到末端執(zhí)行器一側(cè)的各個關(guān)節(jié)也按照1、2、3、4、5、6的順序進(jìn)行編號。為了描述每個連桿與相鄰連桿之間的相對位置關(guān)系，需要在每個連桿上固定一個連桿坐標(biāo)系，根據(jù)連桿坐標(biāo)系所在的連桿的編號對連桿坐標(biāo)系命名，因此固定在連桿i上的連桿坐標(biāo)系為坐標(biāo)系{i}，坐標(biāo)系{i}為O_i（x_i,y_i,z_i），i=0、1、2、3、4、5、6；則根據(jù)D-H方法建立連桿坐標(biāo)系的具體方法如下：

A、找出各個關(guān)節(jié)的關(guān)節(jié)軸，以相鄰兩個關(guān)節(jié)軸i與i+1的交點，或關(guān)節(jié)軸i與i+1的公垂線與關(guān)節(jié)軸i的交點作為連桿坐標(biāo)系{i}的原點O_i，i=1、2、3、4、5；連桿坐標(biāo)系{i}的z_i軸沿關(guān)節(jié)軸i的指向；當(dāng)關(guān)節(jié)軸i與i+1的公垂線與關(guān)節(jié)軸i的交點作為連桿坐標(biāo)系{i}的原點O_i時，連桿坐標(biāo)系{i}的x_i軸沿關(guān)節(jié)軸i與i+1公垂線的指向；當(dāng)關(guān)節(jié)軸i與i+1的交點與關(guān)節(jié)軸i的交點作為連桿坐標(biāo)系{i}的原點O_i時，連桿坐標(biāo)系{i}的x_i軸垂直于關(guān)節(jié)軸i與i+1所在的平面；連桿坐標(biāo)系{i}的y_i軸則可按照右手原則確定，使y_i＝z_i×x_i；而連桿6作為6R型工業(yè)機(jī)器人的末端連桿，連桿坐標(biāo)系{6}的原點O₆以及x₆軸的方向可任意選取；連桿坐標(biāo)系{6}的z₆軸同樣沿關(guān)節(jié)軸6的指向；連桿坐標(biāo)系{6}的y₆軸是按右手原則確定。

B、建立固定于6R型工業(yè)機(jī)器人基座上的連桿坐標(biāo)系{0}；

當(dāng)6R型工業(yè)機(jī)器人中關(guān)節(jié)1的關(guān)節(jié)變量為0時，規(guī)定連桿坐標(biāo)系{0}與連桿坐標(biāo)系{1}重合；

步驟2：獲得6R型工業(yè)機(jī)器人中相鄰連桿間連桿坐標(biāo)系{i}相對于連桿坐標(biāo)系{i-1}的齊次變換矩陣；

連桿坐標(biāo)系{i}相對于連桿坐標(biāo)系{i-1}的變換，是由繞x_i-1旋轉(zhuǎn)α_i-1角、再沿著旋轉(zhuǎn)后坐標(biāo)系的x_i-1軸平移a_i-1、再繞平移后的坐標(biāo)系的Z_i-1軸旋轉(zhuǎn)θ_i角、最后再沿旋轉(zhuǎn)后的坐標(biāo)系的Z_i-1軸平移d_i合成的，i=1、2、3、4、5、6；根據(jù)步驟1中建立的6R型工業(yè)機(jī)器人的連桿坐標(biāo)系，得到6R型工業(yè)機(jī)器人中相鄰連桿坐標(biāo)系間的齊次變換矩陣為：

Tii-1=Rotx(αi-1)Transx(αi-1)Rotz(θi)Transz(di)=cosθi-sinθi0ai-1sinθicosαi-1cosθicosαi-1-sinαi-1-sinαi-1disinθisinαi-1cosθisinαi-1cosαi-1cosi-1di0001=Rii-1Pii-10001---(1)]]>

式（1）中，i=1、2、3、4、5、6；α_i-1、a_i-1、θ_i、d_i為6R型工業(yè)機(jī)器人的連桿參數(shù)，α_i-1為連桿轉(zhuǎn)角，表示繞x_i-1軸，從z_i-1軸旋轉(zhuǎn)到z_i軸的角度；a_i-1為連桿長度，表示沿x_i-1軸，從z_i-1軸移動到z_i軸的距離；θ_i為關(guān)節(jié)角，表示繞z_i軸，從x_i-1軸旋轉(zhuǎn)到x_i軸的角度；d_i為連桿偏距，表示沿z_i軸，從x_i-1軸移動到x_i軸的距離；在以上定義的4個連桿參數(shù)中，對于旋轉(zhuǎn)關(guān)節(jié)，θ_i表示關(guān)節(jié)動作的關(guān)節(jié)變量；為連桿坐標(biāo)系{i}相對于連桿坐標(biāo)系{i-1}的旋轉(zhuǎn)矩陣，^i-1P_i為連桿坐標(biāo)系{i}相對于坐標(biāo)系{i-1}的平移矩陣；

對6R型工業(yè)機(jī)器人進(jìn)行正向運(yùn)動學(xué)求解可以得到機(jī)器人末端連桿坐標(biāo)系相對于基座連桿坐標(biāo)系的齊次變換矩陣：

⁰T₆＝⁰T₁¹T₂²T₃³T₄⁴T₅⁵T₆??????????????（2）

步驟3：建立6R型工業(yè)機(jī)器人中各連桿的力和力矩平衡方程；

以6R型工業(yè)機(jī)器人中一個連桿i為對象進(jìn)行靜力分析，當(dāng)連桿i處于平衡狀態(tài)時，所受合力為零，連桿i的力和力矩平衡方程為：

fii-fi+1i+miig=0Mii-Mi+1i-Pi+1i×fi+1i+Pcii×miig=0---(3)]]>

其中，i=1、2、3、4、5；f_i為連桿i-1施加在連桿i上的力，ⁱf_i為f_i在坐標(biāo)系{i}中的矢量；M_i為連桿i-1施加在連桿i上的力矩，ⁱM_i為M_i在坐標(biāo)系{i}中的矢量；m_ig為連桿i的重力，ⁱm_ig為坐標(biāo)系{i}中連桿i的重力矢量，g為重力加速度；P_ci為連桿i的質(zhì)心位置，ⁱP_ci為連桿i的質(zhì)心在坐標(biāo)系{i}中的位置矢量；ⁱP_i+1為坐標(biāo)系{i+1}相對于坐標(biāo)系{i}的平移矩陣；

為便于從高編號連桿向低編號連桿進(jìn)行迭代求解，用坐標(biāo)系{i+1}和坐標(biāo)系{0}相對于坐標(biāo)系{i}的旋轉(zhuǎn)矩陣進(jìn)行變換，對式（3）進(jìn)行整理，得到相鄰連桿間的靜力“傳遞”表達(dá)式：

fii=fi+1i-miig=Ri+1i+1ifi+1-Rmi0igMii=Ri+1i+1iMi+1+Pi+1i×(Ri+1i+1ifi+1)-Pcii×(Rmi0ig)---(4)]]>

其中，為坐標(biāo)系{i+1}相對于坐標(biāo)系{i}的旋轉(zhuǎn)矩陣；為坐標(biāo)系{0}相對于坐標(biāo)系{i}的旋轉(zhuǎn)矩陣；

對于連桿6來說，連桿6的重力為m₆g，包含末端執(zhí)行器的重力；設(shè)末端執(zhí)行器所抓持的負(fù)載為mg；通過坐標(biāo)系{0}與坐標(biāo)系{6}之間的坐標(biāo)變換矩陣，得到連桿6在坐標(biāo)系{6}中的重力矢量為末端執(zhí)行器所抓持的負(fù)載在坐標(biāo)系{6}中的重力矢量為即為坐標(biāo)系{6}相對于坐標(biāo)系{0}的旋轉(zhuǎn)矩陣；設(shè)末端執(zhí)行器所抓持的負(fù)載在坐標(biāo)系{6}中的質(zhì)心偏移量為l，則連桿6的力和力矩平衡方程為：

f66+(Rmg60)+(Rm660g)=0M66+((Rmg60)×l(Rm660g)×Pc66)=0---(5)]]>

其中，⁶f₆為在坐標(biāo)系{6}中連桿5施加在連桿6上的力；⁶M₆為在坐標(biāo)系{6}中連桿5施加在連桿6上的力矩；⁶P_c6為連桿6的質(zhì)心在坐標(biāo)系{6}中的位置矢量；

將式（5）代入式（4），從連桿6坐標(biāo)系{6}到坐標(biāo)系{1}依次進(jìn)行迭代計算，得到作用于各個連桿上的力和力矩；

步驟4：得到6R型工業(yè)機(jī)器人中保持各連桿平衡的關(guān)節(jié)驅(qū)動力矩；

各個連桿對應(yīng)的關(guān)節(jié)驅(qū)動力矩τ_i為：

τ_i=ⁱM_i·ⁱz_i=ⁱM_i^Tiz_i???????????（6）

式（6）中，i=1、2、3、4、5、6；ⁱz_i為坐標(biāo)系{i}中關(guān)節(jié)軸i的z_i矢量；

則由式（1）和式（6）可得：

τ_i＝X_img+Y_i??????????????????（7）

其中，符號X_i為關(guān)節(jié)i的驅(qū)動力矩τ_i中與負(fù)載mg有關(guān)的系數(shù)，X_i與6R型工業(yè)機(jī)器人的D-H參數(shù)及位姿有關(guān)；Y_i表示τ_i中與各連桿重力有關(guān)、與負(fù)載mg無關(guān)的項；

當(dāng)6R型工業(yè)機(jī)器人在某一位姿下空載時，根據(jù)式（7）可得到機(jī)器人末端執(zhí)行器空載時關(guān)節(jié)i的驅(qū)動力矩τ_i空為：

τ_i空＝Y(jié)_i??????????????????????（8）

在6R型工業(yè)機(jī)器人所處的位姿不改變的情況下，當(dāng)在機(jī)器人的末端執(zhí)行器上附加負(fù)載mg時，根據(jù)式（7）可得到機(jī)器人附加負(fù)載時關(guān)節(jié)i的驅(qū)動力矩τ_i載為：

τ_i載=X_img+Y_i??????????????????????（9）

根據(jù)式（8）和（9）得到6R型工業(yè)機(jī)器人在末端執(zhí)行器附加有負(fù)載mg時與機(jī)器人末端執(zhí)行器空載時關(guān)節(jié)i的驅(qū)動力矩的差值Δτ_i為：

Δτ_i=τ_i載-τ_i空=X_img??????????????????（10）

則根據(jù)式（10）可以得到負(fù)載質(zhì)量m為：

m=ΔτiXig---(11)]]>

從式（11）可以看出，6R型工業(yè)機(jī)器人負(fù)載的質(zhì)量與連桿的質(zhì)量、質(zhì)心等慣性參數(shù)無關(guān)；

上述Δτ_i通過下述方法得到：

6R型工業(yè)機(jī)器人的每個關(guān)節(jié)分別由一個伺服電機(jī)來驅(qū)動；設(shè)驅(qū)動電機(jī)的輸出轉(zhuǎn)矩為τ_M，減速比為i_M，傳動效率為η_M；設(shè)關(guān)節(jié)i處的驅(qū)動電機(jī)的輸出轉(zhuǎn)矩為τ_Mi，減速比為i_Mi，傳動效率為η_Mi，i=1、2、3、4、5、6；則根據(jù)電機(jī)拖動原理，機(jī)器人關(guān)節(jié)i的驅(qū)動力矩τ_i折算到電機(jī)軸上的輸出轉(zhuǎn)矩τ_Mi為：則有：τ_i=τ_Mii_Miη_Mi，由式（10）可得到：

Δτ_i=τ_Mi載i_Miη_Mi-τ_Mi空i_Miη_Mi=Δτ_Mii_Miη_Mi???????（12）

其中，Δτ_Mi表示機(jī)器人在末端執(zhí)行器附加有負(fù)載mg時與機(jī)器人末端執(zhí)行器空載時關(guān)節(jié)i的驅(qū)動電機(jī)的輸出轉(zhuǎn)矩的差值；τ_Mi載為6R型工業(yè)機(jī)器人的末端執(zhí)行器上附加負(fù)載mg時，關(guān)節(jié)i的驅(qū)動電機(jī)的輸出轉(zhuǎn)矩；τ_Mi空為6R型工業(yè)機(jī)器人的末端執(zhí)行器空載時，關(guān)節(jié)i的驅(qū)動電機(jī)的輸出轉(zhuǎn)矩；

由式（11）和（12），得到負(fù)載的質(zhì)量為：

m=ΔτMiiMiηMiXig---(13)]]>

其中，驅(qū)動電機(jī)的輸出轉(zhuǎn)矩可以從電機(jī)的控制器上直接得到，無需附加額外的力和力矩傳感器來測量，減速比與傳動效率均可從設(shè)備型號上得知，系數(shù)X_i可由機(jī)器人D-H參數(shù)及機(jī)器人位姿計算得到；

由此通過式（13）可分別得到6個負(fù)載質(zhì)量值；因此針對不同型號不同參數(shù)的6R型工業(yè)機(jī)器人，通過驗證已知負(fù)載的實際質(zhì)量與通過上述方法識別得到的6個負(fù)載質(zhì)量值進(jìn)行對比，選取最接近實際負(fù)載質(zhì)量值所對應(yīng)的關(guān)節(jié)的數(shù)據(jù)作為當(dāng)前型號6R型工業(yè)機(jī)器人后續(xù)再次進(jìn)行未知負(fù)載識別運(yùn)算時所應(yīng)用的數(shù)據(jù)。