TWI325361B - - Google Patents

Description

1325361 (1) -九、發明說明 【發明所屬之技術領域】 本發明係關於一種以互相干涉之複數軸所構成的機器 人，即使在干涉力作用之時，亦可進行控制而使各個軸依 照指令方式而動作之機器人之控制裝置。 【先前技術】 先前技術之機器人控制裝置，具有模式控制器，在模 φ 式控制器內部中具有擬似模式、擬似模式用之控制部、算 出可抵消干涉扭力用之模式補償扭力之修正量算出部，將 被加算模式補償扭力後之模式前授指令做爲前授訊號而輸 出之時，可進行非干涉化（例如，參照日本特許文獻1 ) 〇 第5圖係顯示揭示於日本特許文獻1中之先前技術之 一實施形態的機器人控制裝置的構成之控制方塊圖，第6 圖係顯示第5圖之模式控制器之構成的控制方塊圖。在第 φ 6圖中，54U' 54L係修正量演算部，60U、60L係對回授 控制系統之輸出訊號。在第5圖中，顯示有在各個軸上近 似於2慣性系（電動機+減速機等之彈簧+臂）之2軸的 機器人控制系統之基本構成。在第5圖及第6圖中，將2 軸做爲L軸、ϋ軸’在有關於L軸之指令或量後面將 '、_L〃附加於詞尾’並在有關於U軸之指令或量後面將 、、—U 〃附加於詞尾。 ’ 該機器人之控制裝置’係根據各個軸之電動機位置指 — -5 - (2) (2)1325361 令二，Xref_u而進行L軸及U軸之控制者，其具有2 軸之回授控制系統（回授控制器）5 3 L，5 3 U及控制對象 動機、減速機、機器人手臂）之擬似模式之模式控制 @ 5 1 °模式控制器5 1將電動機位置指令Xref_L，Xref_u

’考慮機器人之動態而計算各個軸之前授指令UFF_L ’ UpF-U ’因而對各個軸之電動機的加速度項進行前授補 同時算出爲規範之模式中各個軸之電動機位置指令 0 ’ 0 Mm_u、電動機速度 籲

[數式1 J

. e^J 扭轉角0 Ms_L，0 Ms_u、扭轉角速度 [數式2] VL · 並輸出。 以下，在爲模式控制器5 1之輸出的各要素中，附加 φ 接頭語「模式」。 另—方面，在回授控制系統（回授控制器）5 3 L， 53u之中’各個軸之位置增益爲KP_L，Kp_u、速度增益爲 Kv-L ’ Kv_u、扭力常數爲Kt_L，Κ,_υ，賦予電動機之最終 的加速度指令分別爲Uref_L，Uref_u。並且，各個軸之電 動機的慣性力矩分別爲、電流指令爲Iref_L，

Ifef_U。電動機之實際的位置爲0m_L，6>m_u、電動機之實 - 際的速度爲 -6 - (3) (3)1325361 [數式3] ®'n^L ；,； 電動機之實際的加速度爲 [數式4] “ ·'· 足 nJL :，,ϊη_]Ι 又’減速機之減速比爲N_L，N_u，減速機之彈簧常 數爲KC_L ’ Ke_u ’臂之慣性力矩Jl l，Jl —^，臂之實際的 位置爲Θ L_L ’ 0 L_u、臂之加速度（負荷之加速度）係以 [數式5]

Kl . θυ 表不。然後’爲了表不兩軸間之干涉，而做成：由機器人 臂之質量或兩軸間之角度所決定的mlu/m。2之値，被乘 到L軸之負荷扭力，而作用到U軸之負荷加速度上，间 樣地，MLU/M。2之値被乘到U軸之負荷扭力，而作用到 L軸之負荷加速度上之構成。 該機器人控制系統，係分別在L軸、U軸上設置有. 將對電動機之最終的加速度指令Urei L，Uref_u及電動機 之實際的位置爲0m_L，0m_U做爲輸入，而推定實際之扭 轉角0 S_L，0 s_U及其角速度 [數式6] ^s_L / 的狀態觀測器52L，52U。而，在該回授控制器中，對於^ 從模式控制器5]輸出之電動機位置θΜιη_1_，與實 (4) (4)1325361 際之電動機位置0 m_L，0 m_U的偏差，乘以位置增益爲 Kp_L，Kp_U，而分別做爲各個軸之速度指令。 在依此方式獲得的各速度指令上，將模式電動機速度 [數式7] 與實際之電動機速度 [數式8]

Kj. , ^ 的偏差加算到其上，並將速度增益爲KV_L，KV_U乘算到 其上，而分別做爲加速度指令。 對於如此的加速度指令，將（a )來自模式控制器5 1 之前授指令UFF L，Uffj、及（b )模式扭轉角0 Ms_L， 0 Ms_u與從狀態觀測器52L，52U輸出的扭轉角0 s_l， 0 s_u之偏差乘以回授增益K〗_L，所獲得者、及（c ) 模式扭轉角速度 [數式9] * ·

Slis_L , 0Ms_D 與從狀態觀測器52L，52U輸出的扭轉角速度 [數式1 0]

之偏差乘以回授增益K2_L，K2 U所獲得者進行加算，而 分別對各個軸做爲最終的電動機加速度指令， U re f_ u。 在該機器人之控制系統中’藉由如此獲得的最終的電 -8- (5) (5)1325361 動機加速度指令Uref_L，Uref_u，而驅動各個軸之電動機 ，藉由減速比分別爲N_L，N_u之減速機’而驅動各個軸 之臂。此時，如上所述，軸間之千涉力作用在各個軸之負 荷加速度上。 其次，將說明模式控制器5]之構成。第6圖顯示， 由回授控制系統或電動機、減速機、表示機器人臂之擬似 模式所形成的模式控制器51之構成的詳細圖。在擬似模 式中，各個軸之位置增益及速度增益，分別爲模式位置增 · 益KpM L，KpM_u及模式速度增益KvM L，KvM U。同樣地 ，模式電動機慣性力矩JmM_L，、模式臂之慣性力矩

Jlm_l，JLM_U、模式減速比Nm_l ’ NM_u '模式減速機之彈 簧常數KeM_L，KeM_u，被設定做爲模式參數。 模式控制器51將各個軸之電動機位置指令Xref_L，

Xref_u輸入，將位置增益KpM_L，ΚρΜ_υ乘以該電動機位置 指令Xref_L，Xref_u與電動機位置XMm_L，XMm_U之偏差， 而做爲各個軸之模式速度指令。然後，從這些模式速度指 鲁 令減去各個軸之模式電動機速度 [數式1 1 ] • « 後之値，乘以模式速度增益KvM L，KvM U，而分別做爲 各個軸之模式加速度指令。 在該模式加速度指令中，將模式回授增益K1M_L， K1M_u乘以從模式減速機所獲得的模式扭轉角0 Ms_b， β 而獲得的値，減去將模式扭轉角0 Ms_L，0 Ms ^微 -9- (6) · (6) ·1325361 分而獲得模式扭轉角速度 [數式1 2] ^Ks_L . 乘以模式回授增益K2M_L，K2M_u所獲得的値，而做爲模 式加速度指令 UMref_L’ U]vlref_U。該模式力□速度指令 UMref_L，UMref_u被輸入模式電動機中。 從而，模式加速度指令UMref L，Uwrefj可表示爲 [數式13] ·

^Mi*ei_L ~ * -^vW_L ^^ret.L — ^lVini..L^ — ^vM_L * ^ Min_L _ 馬网丄‘合版^ ⑴ w' υϊνίτεί Jj = %K〇j 表剛 <爲rfiJ _ %m_U) - .石 Μπχ_ϋ -Km_T % jij - ^Ms_u (2) 但是，在此推定的2軸機器人中，由於干涉，由機器 人手臂之質量或兩軸間之角度所決定之Mlum/Mw2之値 被乘到L軸之負荷扭力，而作用到U軸之負荷加速度上 | 。同樣地，將Mlum/ McM2之値乘到U軸之負荷扭力，而 作用到L軸之負荷加速度上。 將如此之干涉做爲作用於臂側之外亂而抓住之時’關 於L軸方面，將下式所顯示之模式修正扭力Te()mp_L加算 到模式電動機加速度指令UMref_L中的話’可減輕由於干 渉造成的臂之振動。 -10- (7)1325361 [數式14] ·

KpM L' ΚνΜ,ί' NM.L'D^.UL KvM_L，NM.L'Djs.UL Pjs.UL

Tc〇mp_L KcM_L

.UL

s.UL

hLL 在此處，Dis_UL係從U軸作用於L軸之干涉 ⑶ 扭力，可 表示爲

[數式1 5] Dis_UL=JlM.L * KcM _U

⑷

因此，最終的模式電動機加速度指令uff_l被做成 U =U +T (5) FF丄 Mref丄 compj- 而對電動機之加速度項進行前授補償。在該模式控制器 51中設置有，將模式扭轉角0Ms_U做爲輸入，根據前述 之數式（3) ，（4)算出模式修正扭力Te()mp_L之修正量

演算部54L，來自於修正量演算部54L之模式修正扭力 TCC)mp_L被加算到電動機加速度指令uref_L中。 同樣地’關於U軸方面，模式修正扭力Te()mp_L亦由

[數式1 6J T, c〇mp_U! ^is^LU Nm.u'%.lu ：_LU: ^^lM^U/PjsJLU KcMJJC6) 而決定。在此處，Dis_LU係從L軸作用於u軸之干涉扭力 ’其係爲 -11 - (8)1325361 [數式17]

Mlum

*^iW5_L m 設置有將模式扭轉角0Ms_L做爲輸入’根據前述之數 式（6) ， （7)算出模式修正扭力Te<JmpU之修正量演算 部5 4U。其結果，最終的模式電動機加速度指令成 爲 U =U +T (8)

FF.U MreCU comp.U 如上所述，在揭示於特許文獻1中之先前技術之中， 依照前述藉由模式控制器51將干涉扭力進行修正之後， 將成爲規範之各狀態量做爲指令而輸入回授控制系統中， 因此各機器人臂不會受到來自他軸之干涉所影響。 因而，先前技術之機器人控制裝置，將從他軸接受之 干涉力以模式控制器修正，以此爲基準將成爲規範之各狀 態量輸入回授控制器時，可消除雜訊成分重疊到修正扭力 上，機器人手臂不會接受由干涉所造成之振動，因而可使 φ 前端之軌跡精度提高。 特許文獻1:曰本特開平10-329063號公報（第2-5頁，第〗圖，第2圖） 【發明內容】 發明欲解決的課題 但是，先前技術之機器人之控制裝置，爲了實現該控 制器’在回授控制部之演算以外，在模式控制器5 1內， 每個控制周期中，必須進行扭轉角之計算等之擬似模式之 -12 - (9) (9)1325361 計算，因而有各個軸之計算量龐大的問題。 並且，在模式控制器51內，在回授控制部之演算以 外，每個控制周期中，必須進行控制擬似模式用之模式用 的回授控制之計算，因而有各個軸之計算量變成更龐大的 問題。 並且，非干涉化僅在扭力之修正上進行，因此在模式 修正扭力τ。。^^之計算上需要模式位置增益κΡηι或模式 速度增益κνΜ之値，因而有使計算變成更複雜的問題。 秦 然後，結果各個軸之計算量變成龐大，控制演算時間 變長，因而爲了實現本控制裝置而需要有高速CPU之問 題。 本發明係鑑於前述問題點而進行開發者，目的在提供 一種機器人之控制裝置，係針對由互相干涉的複數軸所構 成之機器人，其係具備有將電動機中藉由減速機等之彈簧 元件（彈簧常數）而結合的臂、及檢測電動機之位置之電 動機位置檢測器所構成的各個軸，依照各個軸之指令方式 · 而動作用之位置控制器及速度控制器之機器人之控制裝置 ，其中即使有來自於它軸作用之干涉力之時，亦可使電動 機動作而使自軸依照指令方式而動作，在不受到干涉之影 響之下，可使機器人手臂動作，因而可使計算量大幅地降 低，且減輕CPU之負擔。 並且，即使來自於複數個軸之干涉力對自軸作用之時 ，亦不需要考慮複數個軸之關係而做複雜的計算，僅須計 ^ 算從各個軸對自軸作用之干涉力的合計値而進行處理，隨 · -13 - (10) (10)1325361 後僅進行與2軸間之非干涉化同樣的處理，即可簡單地達 成複數個軸間之非干涉化的目的。 並且’過去未考慮到之軸間的干涉被追加之時，亦幾 乎不需要修正程式，僅追加計算從各個軸對自軸作用之干 涉力的合計値而進行處理’即可簡單地達成所有軸間之非 干涉化的目的。 解決課題之手段 φ 爲了解決前述問題’本發明之第1構成，係針對由互 相干涉的複數軸所構成之機器人，其係具備有由電動機、 前述電動機中藉由減速機等之彈簧元件而結合的臂、及檢 測前述電動機之位置之電動機位置檢測器所構成的各個軸 ’依照各個軸之指令方式而動作用之位置控制器及速度控 制is之機器人之控制裝置’其特徵爲：具備有：計算而求 出從自軸之指令而作用於它軸之干涉力之干涉力計算部、 在有來自於它軸作用之干涉力之情況時，亦可使電動機動 · 作而使自軸依照指令方式而動作的電動機扭力指令訊號， 從自軸之指令及由從它軸作用之干涉力的計算値求出之非 干涉扭力訊號作成部、及在有來自於它軸作用之干涉力之 情況時’亦可使電動機動作而使自軸依照指令方式而動作 的電動機位置訊號’從自軸之指令及由從它軸作用之干涉 力的計算値求出之非干涉位置訊號作成部。 在該第1之構成中，具有干涉力計算部及非干涉扭力 ^ 訊號作成部及非干涉位置訊號作成部，不必進行擬似模式 ' -14 - (12) (12)1325361 s :拉普拉斯運算子 d is 1 :從第1軸作用到第2軸之干涉力 dis2 :從第2軸作用到第1軸之干涉力 ben :第1軸位置指令 :第2軸位置指令 在該第2構成中，計算干涉力 '非干涉扭力訊號、及 非千涉位置訊號之數式係特定。 並且’本發明之第3構成，其特徵係針對在前述非干 鲁 涉扭力訊號作成部及非干涉位置訊號作成部之處理所使用 ’從它軸作用之干涉力的計算値，在軸爲超過2軸之情況 ’係爲從各個軸對自軸作用的干涉力計算値之總和。 在該第3構成中，干涉力作用於複數個軸間之情況時 ’自軸亦受到來自於複數個軸之干涉力作用。此時，藉由 計算2軸間之干涉力之總合，可求出作用於自軸之干涉力 發明之效果 依照本發明之第1構成時，即使有來自於它軸作用之 +涉力之時’亦可使電動機動作而使自軸依照指令方式而 動作’結果不受到干涉的影響，而可依照指令方式使機器 A手臂動作。並且，可使實現非干涉化之計算非常少。其 '結果’可使CPU之計算量大幅地降低，因此可使用價格 便宜的CPU » 依照本發明之第2構成時，由於計算干涉力、非干涉 -16- (13) (13)1325361 扭力訊號、及非干涉位置訊號之數式係特定，可使機器人 控制被具體化。 並且，依照第3構成時，即使自軸亦受到來自於複數 個軸之干涉力作用時，並不考慮複數軸的關係而做複雜的 計算，而僅計算從各個軸對自軸作用之干涉力的合計値而 進行處理，其後僅進行與2軸間之非干涉化同樣的處理即 可，因而可簡單地達成複數軸間之非干涉化。 【實施方式】 以下，將參照附圖而說明本發明之實施形態。 實施例1 第1圖係說明本發明之機器人之控制裝置的全體構成 圖。在圖中，符號1 0 0係第1軸之機器人之控制裝置，符 號2 0 0係第2軸機器人之控制裝置。 第1軸之機器人之控制裝置100，將第1軸位置指令 鲁 Xren及第1軸電動機位置檢測値ΧΠΗ及從第2軸作用於第 1軸之干涉力計算値dis2輸入，而進行控制演算以使控制 對象依照指令動作，因而將第1軸扭力指令trefI輸出。 符號8係第1軸電動機，其係藉由從第1軸之機器人之控 制裝置1 00輸出之第1軸扭力指令trefl而動作。符號9 係第1軸臂’其係與電動機8藉由減速機等而結合。符號 係檢測第1軸電動機8之位置xfbi的第1軸電動機位 置檢測器。 · -17 - (14) (14)1325361 第2軸之機器人之控制裝置200，將第2軸位置指令 xref2及第2軸電動機位置檢測値xfb2及從第1軸作用於第 2軸之干涉力計算値d i s,輸入，而進行控制演算以使控制 對象依照指令動作，因而將第2軸扭力指令tref2輸出。 符號18係第2軸電動機，其係藉由從第2軸之機器人之 控制裝置200輸出之第2軸扭力指令tref2而動作。符號 19係第2軸臂，其藉由電動機18及減速機等而結合。符 號20係檢測第2軸電動機18之位置xfb2的第2軸電動機 · 位置檢測器。 並且，雖然通常兩軸所輸出的扭力指令値tref變換成 電流指令値，通過電流放大器而使電動機動作，但是其響 應可忽略而十分地快速，並且在本發明中並無特別關係， 故予以省略。 在此處，將電動機8，18及臂9，19結合的減速機， 具有彈簧元件之故’機器人之控制裝置100及機器人之控 制裝置2 0 0的控制對象，均係考慮電動機慣性力矩及臂慣 鲁 性力矩在彈簧元件之下結合的2慣性系。 本實施例係考量，將每個單軸之控制對象做成2慣性 系’而以考量彼此之軸間的干涉之第4圖顯示之傳遞函數 表示之構成。 桌4圖中’各記號之意義係如以下所示。 j m !:第1軸電動機慣性扭矩 JL1 :第1軸臂慣性扭矩 : K1:第1軸減速機彈簧常數 胃 -18 - (15) (15)1325361

Jm2 :第2軸電動機慣性扭矩 J L 2 :第2軸臂慣性扭矩 K2:第2軸減速機彈簧常數 A ’ B:從2軸之構成、形成角度 '幾何學的關係求 出的係數 s :拉普拉斯運算子 d i s!:從第1軸作用到第2軸之干涉力 dis2:從第2軸作用到第I軸之干涉力 φ X1 :臂1之位置 :臂2之位置 並且，前述A， B可以力學計算而求得。 例如，考慮2個軸如第3圖所示的關係之情況。第] 軸與第2軸所成的角度以0，第1軸與第2軸之軸間距離 以L表示，從各個軸、旋轉中心到質量中心之距離以l 1 ’ L2表示’分別之質量爲Ml，M2之時，A，B爲如數式 (9)者。 鲁 A = B = M2 * L2 氺 cos ( 0 ) + M2 * L22 …（9) 以下，將說明實際上第1軸之機器人之控制裝置1〇〇 內部之構成。 在第1圖中，符號1係第1軸之位置控制部，其使第 1軸之非干涉位置訊號xref_m與第1軸位置檢測値Xfbl成 一致地’將兩者的偏差之第1軸位置偏差errl輸入，進 行控制演算，而將第1軸速度指令v ren輸出。 : 雖然如何進行在位置控制部1內之處理均可，例如數 · -19- (16) (16)1325361 式（1 〇 )所示使用第1軸位置環路比例增益Kpl而進行比 例控制時較佳。 v ref 1 =Kpi * err 1 ... ( 1 Ο ) 符號2係第1軸速度控制器，其使第1軸之非干涉速 度訊號vref_ffl與第1軸速度檢測値vfbl成一致地，以數 式（11)將兩者的偏差之第1軸速度偏差verM輸入，進 行控制演算，而將第1軸扭力指令t ref fbl輸出》

Verrl= Vref_ffi+ V ref] - Vfbl "-(11) · 雖然如何進行在速度控制器2內之處理均可，例如數 式（1 2 )所示使用第1軸速度環路比例增益κν|及第1軸 速度環路積分增益Kh而進行比例積分控制時較佳。 tref_fb]=Kvi* (1+Kji/s)氺 Ve『fi "-(12) 符號3及4表示微分器。 符號5係第1軸非干涉位置訊號作成部，其將第1軸 位置指令X ^ η及從第2軸作用於第1軸之干涉力計算値 din輸入’即使干涉力dis2作用之時，亦可依照第1軸位鲁 置指令xrefl方式’將臂9前端χ1動作用之第1軸電動機 位置訊號’使用第4圖所示之2軸的干涉模式之關係而作 成’而做爲第1軸之非干涉位置訊號xref_fn輸出。 xref_ff 1之計算式如數式（1 3 )所示。 xref_fn=(JL1/Kl * " + 1) * Xrefi — 1/K1 * dis2 ...... (13) 符號ό係第1軸非干涉扭力訊號作成部，其將第ι軸 位置指令xref]及從第2軸作用於第1軸之干涉力計算値 · din輸入’即使干涉力dis2之影響作用之時，亦可依照第 - -20 - (17) (17)1325361 1軸位置指令xrefl方式’將臂9前端χΐ動作用之第1軸 扭力指令訊號’使用第4圖所示之2軸的干涉模式之關係 而作成，而做爲第1軸之非干涉扭力訊號trefffl輸出。

tref_fn之計算式如數式（14 )所示。 t =(J *J /K1*S4+(J +J )S2)*X refjn ml U ml U ref) 一（J /K1 氺s2+l)*d ...(14) ml is2 符號7係第1軸干涉力計算部，其將第1軸位置指令 xren輸入’且計算從第1軸作用於第2軸之干涉力disl而 輸出。如第4圖淸楚地顯示，disi係以數式（15)計算》 dis 1 =B * Xrefi * s2 …（1 5 ) 最終地’從第1軸之機器人之控制裝置1 Ο 0向電動機 8輸出之第1軸扭力指令値t refl係以下列之數式（1 6 ) 計算。 trefl=tref_ffi+tref_fb] ... (16) 其次，將說明第2軸之機器人之控制裝置200內部之 構成。 在圖中，符號1 1係第2軸之位置控制部，其使第2 軸之非干涉位置訊號xref_ff2與第2軸位置檢測値xfb2成 一致地，將兩者的偏差之第2軸位置偏差err2輸入，進 行控制演算，而將第2軸速度指令v ref2輸出。 雖然如何進行在位置控制部1 1內之處理均可，例如 數式（1 7 )所示使用第2軸位置環路比例增益Kp2而進行 比例控制時較佳。 ν ref2 = KP2 * err2 …（]7 ) 符號1 2係第2軸速度控制器，其使第2軸之非干涉 -21 - (18) (18)1325361 速度訊號vref_ff2與第2軸速度檢測値vfb2成一致地，以 數式（18)將兩者的偏差之第2軸速度·偏差Ve„2輸入， 進行控制演算，而將第2軸扭力指令t ref_fb2輸出。 verr2= vref_ff2+ V r e f 2 ~ V f b 2 ...(18) 雖然如何進行在速度控制器1 2內之處理均可，例如 數式（19)所示使用第2軸速度環路比例增益Kv2及第2 軸速度環路積分增益Ki2而進行比例積分控制時較佳。 t ref_fb2 = Kv2 * ( 1 + K i 2 / S ) * Verr2 “.（19) 符號13及14表示微分器。 符號1 5係第2軸非干涉位置訊號作成部，其將第2 軸位置指令xref2及從第1軸作用於第2軸之干涉力計算 値dis]輸入’即使干涉力disl作用之時，亦可依照第2軸 位置指令xref2方式’將臂19前端x2動作用之第2軸電 動機位置訊號’使用第4圖所示之2軸的干涉模式之關係 而作成，而做爲第2軸之非干涉位置訊號Xref_ff2輸出。 xref_ff2之計算式如數式（20 )所示。

Xref_ff2 = (JL2/K2* S2 + 1)* Xref2- 1/K2* dis]…（20) 符號1 6係第2軸非干涉扭力訊號作成部，其將第2 軸位置指令xren及從第1軸作用於第2軸之干涉力計算 値disl輸入，即使干涉力disl之影響作用之時，亦可依照 桌2軸位置指令xfef2方式，將臂19前端x2動作用之第2 軸扭力指令訊號，使用第4圖所示之2軸的干涉模式之關 係而作成，而做爲第2軸之非干涉扭力訊號tfef ff2輸出 。t re f_ff2之計算式如數式（2 1 )所示。 -22 - (19) (19)1325361 t =〇 *j/K2*s4+a +j )s2)*x refj|2 m2 L2 m2 L2 ref2 -CJ /K2*s2+l)*d ".(21) m2 is】 符號1 7係第2軸干涉力計算部，其將第2軸位置指 令xref2輸入，且計算從第2軸作用於第！軸之干涉力dis2 而輸出。如第4圖淸楚地顯示，d; s2係以數式（2 2 )計算 〇 dis 1 = A * xref2 * s2 ... ( 22 ) 最終地’從第2軸之機器人之控制裝置200向電動機 φ 1 8輸出之第2軸扭力指令値t f2係以下列之數式（23 ) 計算。 t ref2= tref_ff2 + tref一fb2 …（23) 依此方式，即使干涉力作用之時，亦作成電動機位置 訊號、及電動機扭力指令訊號，使各個軸臂9，19依照各 個軸指令方式而動作，並進行回授控制，使電動機8，18 依照這些指令方式而動作，因此即使有軸間干涉之時，各 個軸臂9，19可依照自軸之指令方式而動作。 φ 本發明與特許文獻1之相異部分，係爲不具有擬似模 式或模式用之控制器，而具備有非干涉位置訊號作成部及 非干涉扭力訊號作成部，不僅電動機扭力修正訊號，而且 電動機位置訊號亦可考慮干涉而作成，之部分。 實施例2 第2圖係顯示第2實施例之構成之圖。本實施例與實 施例1相異之點，相對於實施例]考慮2軸之干涉’本實 施例係考慮干涉力作用於複數軸間之情形之點。 -23- (20) (20)1325361 如第2圖所示，係考慮關於以η軸構成的機器人。在 此處，雖然係以從第2軸從第η軸之個別對第1軸之干涉 作用之情形的第1軸之非干涉化做爲例子而說明，但是關 於其他之軸間的干涉亦同樣地可實現》 圖中，符號38係第η軸電動機，其係藉由從第η軸 機器人之控制裝置3 00輸出的第η軸扭力指令値trefn而 動作》符號3 9係第η軸臂，其係與電動機3 8藉由減速機 等而結合。符號3 0係檢測第η軸電動機3 8之位置xfbn的 鲁 第η軸電動機位置檢測器。 符號500係干涉力加算器，求出從第2軸作用於第1 軸之干涉力計算値dis2，到從第η軸作用於第1軸之干涉 力5十算値d;sn之干涉力之總和dis_an之處理，係如數式（ 24 )所示而執行。 dis_all= dis2+ djs3+ ··· + disn …（24) 在此處’從dis2到disn，係與在實施例1中使用第3 圖說明者同樣地，係從第1軸到第η軸之各個軸之構成、 鲁 形成的角度、幾何學的關係而求出。 依照此方式’即使干涉作用於複數軸間之情形下，將 從個別之軸對自軸作用的干涉力計算値之合計値輸入，其 後’僅執行與實施例1同樣的處理之時，可使用簡單的構 成而計算量亦少的情況下’而進行所有軸間之非干涉化。 並且，軸增加之情況’或過去未考慮之干涉重新考慮 之情況等’亦不必全部變更各個軸之處理，而僅將計算各 * 個軸間之干涉力計算値的和之部分追加修正即可，因此可 · -24 - (21) (21)1325361 簡單地對應。 產業上利用之可行性 本發明，即使干涉力作用於複數軸間之時，亦可在計 算量少之下，實現機器人之非干涉化，因此軸數多的機器 人之非干涉化控制，或使用便宜的CPU之機器人之控制 裝置等的用途，亦可使用。 【圖式簡單說明】 第1圖係顯示本發明之第1實施例之構成的方塊圖。 第2圖係顯示本發明之第2實施例之構成的方塊圖。 第3圖係求出干涉力之計算上需要的係數A，B之時 的力學模式圖。 第4圖係將單軸做成2慣性系，考慮互相之干涉力後 之控制對象模式圖。 第5圖係顯示先前技術之機器人控制裝置的構成之方 φ 塊圖。 第6圖係顯示先前技術之機器人控制裝置的模式控制 器內部之處理的方塊圖。 【主要元件符號說明】 1…位置控制部 2…速度控制部 3…微分器 -25- (22) (22)1325361 4…微分器 5…非干涉位置訊號作成部 6…非干涉扭力訊號作成部 7···干涉力計算部 8…電動機 9…臂 10…位置檢測器 1 1…位置控制部 · 12…速度控制部 13…微分器 1 4…微分器 15…非干涉位置訊號作成部 1 6…非千涉扭力訊號作成部 17…干涉力計算部 1 8…電動機 1 9…臂 馨 20…位置檢測器 38…電動機 3 9…臂 3 0…位置檢測器 51…模式控制器 52L、52U…狀態觀測器 5 3 L、5 3 U…回授控制系（回授控制器） 5 4L、5 4U…修正量演算部 ’ -26 - (23) (23)1325361 60L、60U…對回授控制系輸出之訊號 100···第1軸機器人之控制裝置 200···第2軸機器人之控制裝置 3 00···第η軸機器人之控制裝置 5 00···干涉力加算器

-27 -

Claims (1)

1325361 / 十、申請專利範園 第93 140629號專利申請案 中文申請專利範圍修正本 民國99年2月24日修正 1 · 一種機器人之控制裝置；其係針對由互相干涉的 複數軸所構成之機器人，其具備令由電動機、於前述電動 機藉由減速機等之彈簧元件而結合的臂、與檢測前述電動 機之位置之電動機位置檢測器所構成之各個軸，依照各個 軸之指令而加以動作之位置控制部及速度控制部；其特徵 爲具備有： 計算而求出由自軸之指令而作用於它軸之干涉力之干 涉力計算部； 也於作用有來自它軸之干涉力之場合，將自軸依照指 令而動作的電動機扭力指令訊號，由自軸的指令與作用自 它軸的干涉力之計算値求出之非干涉扭力訊號作成部；和 也於作用有來自它軸的干涉力之場合，將自軸依照指 令而動作的電動機位置訊號，由自軸的指令與作用自它軸 的干涉力之計算値求出之非干涉位置訊號作成部。 2 .如申請專利範圍第1項所記載之機器人之控制裝 置，其中：關於軸爲2軸的場合之干涉；前述干涉力計算 部之中的干涉力，係以下式計算： disl = B * Xrefl * S2 dis2 = A * Xref2 * S2 前述非干涉扭力訊號作成部之中的非干涉扭力訊號， 1325361 係以下式計算： *J /Kl*s4+〇 +J )s2)*x rcW1 ml LI ml LI reil —CJ /K1 氺s2+l)*d ml is2 '娜二(J ,氺J /K2 * s4+(J +J ) s2)氺x ref-K m2 U m2 L2 ref2 -(J /K2*s2+1)氺d m2 isl 前述非干涉位置訊號作成部之中的非干涉位置訊號， 係以下式計算： xref_ffi= ( Jli/ΚΙ 氺 S2 + 1)氺 Xrefl— 1/K1 氺 dis2 xref_ff2= ( JL2/K2 氺 S2 + 1)木 Xref2- 1/K2 氺 djsi 但是，!:第1軸電動機慣性扭矩 JL1 :第1軸臂慣性扭矩 K1:第1軸減速機彈簧常數 Jm2 :第2軸電動機慣性扭矩 Jl2 :第2軸臂慣性扭矩 K2:第2軸減速機彈簧常數 A(=B):由2軸之構成、形成角度、幾何學的關係 而求出的係數 s :拉普拉斯運算子 disl :從第1軸作用到第2軸之干涉力 din :從第2軸作用到第1軸之干涉力 xrefl :第1軸位置指令 Xref2 :第2軸位置指令。 3.如申請專利範圍第】項所記載之機器人之控制裝置 ’其中：使用在前述非干涉扭力訊號作成部及非干涉位置 訊號作成部之處理，作用來自它軸的干涉力的計算値係， -2- 1325361 於軸爲超過2軸之場合，爲從各個軸作用到自軸的干涉力 計算値之總和。