[發明專利]一種6R型工業機器人負載識別方法

有效

• 申請號: 201310456167.4
• 申請日: 2013-09-29
• 公開（公告）號: CN103495977A
• 公開（公告）日: 2014-01-08
• 發明（設計）人: 陳友東;谷平平;季旭東
• 申請（專利權）人: 北京航空航天大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/00
• 代理公司: 北京永創新實專利事務所 11121
• 代理人: 周長琪

1.一種6R型工業機器人負載識別方法，其特征在于：通過下述步驟實現：

步驟1：根據D-H方法，建立6R型工業機器人的連桿坐標系；

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

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

B、建立固定于6R型工業機器人基座上的連桿坐標系{0}；

當6R型工業機器人中關節1的關節變量為0時，規定連桿坐標系{0}與連桿坐標系{1}重合；

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

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

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型工業機器人的連桿參數，α_i-1為連桿轉角，表示繞x_i-1軸，從z_i-1軸旋轉到z_i軸的角度；a_i-1為連桿長度，表示沿x_i-1軸，從z_i-1軸移動到z_i軸的距離；θ_i為關節角，表示繞z_i軸，從x_i-1軸旋轉到x_i軸的角度；d_i為連桿偏距，表示沿z_i軸，從x_i-1軸移動到x_i軸的距離；在以上定義的4個連桿參數中，對于旋轉關節，θ_i表示關節動作的關節變量；為連桿坐標系{i}相對于連桿坐標系{i-1}的旋轉矩陣，^i-1P_i為連桿坐標系{i}相對于坐標系{i-1}的平移矩陣；

對6R型工業機器人進行正向運動學求解可以得到機器人末端連桿坐標系相對于基座連桿坐標系的齊次變換矩陣：

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

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

以6R型工業機器人中一個連桿i為對象進行靜力分析，當連桿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在坐標系{i}中的矢量；M_i為連桿i-1施加在連桿i上的力矩，ⁱM_i為M_i在坐標系{i}中的矢量；m_ig為連桿i的重力，ⁱm_ig為坐標系{i}中連桿i的重力矢量，g為重力加速度；P_ci為連桿i的質心位置，ⁱP_ci為連桿i的質心在坐標系{i}中的位置矢量；ⁱP_i+1為坐標系{i+1}相對于坐標系{i}的平移矩陣；

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

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

其中，為坐標系{i+1}相對于坐標系{i}的旋轉矩陣；為坐標系{0}相對于坐標系{i}的旋轉矩陣；

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

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

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

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

步驟4：得到6R型工業機器人中保持各連桿平衡的關節驅動力矩；

各個連桿對應的關節驅動力矩τ_i為：

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

式（6）中，i=1、2、3、4、5、6；ⁱz_i為坐標系{i}中關節軸i的z_i矢量；

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

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

其中，符號X_i為關節i的驅動力矩τ_i中與負載mg有關的系數，X_i與6R型工業機器人的D-H參數及位姿有關；Y_i表示τ_i中與各連桿重力有關、與負載mg無關的項；

當6R型工業機器人在某一位姿下空載時，根據式（7）可得到機器人末端執行器空載時關節i的驅動力矩τ_i空為：

τ_i空＝Y_i??????????????????????（8）

在6R型工業機器人所處的位姿不改變的情況下，當在機器人的末端執行器上附加負載mg時，根據式（7）可得到機器人附加負載時關節i的驅動力矩τ_i載為：

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

根據式（8）和（9）得到6R型工業機器人在末端執行器附加有負載mg時與機器人末端執行器空載時關節i的驅動力矩的差值Δτ_i為：

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

則根據式（10）可以得到負載質量m為：

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

從式（11）可以看出，6R型工業機器人負載的質量與連桿的質量、質心等慣性參數無關；

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

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

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

其中，Δτ_Mi表示機器人在末端執行器附加有負載mg時與機器人末端執行器空載時關節i的驅動電機的輸出轉矩的差值；τ_Mi載為6R型工業機器人的末端執行器上附加負載mg時，關節i的驅動電機的輸出轉矩；τ_Mi空為6R型工業機器人的末端執行器空載時，關節i的驅動電機的輸出轉矩；

由式（11）和（12），得到負載的質量為：

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

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

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