多關節(jié)型機器人的控制裝置、控制方法以及控制程序的制作方法
【專利摘要】本發(fā)明提供多關節(jié)型機器人的控制裝置及方法，旨在維持多關節(jié)型機器人的作業(yè)部的位置以及移動速度的同時使第1關節(jié)驅動系統(tǒng)的驅動軸各自的角度可靠地到達作業(yè)結束點的角度。在例外條件未滿足的情況下，基于步驟(S5)中所計算的插補點來驅動第1以及第2關節(jié)驅動系統(tǒng)的驅動軸的每一個軸(S64)。在例外條件滿足后(S61的“是”側)，至作業(yè)部到達作業(yè)結束點為止的期間(S12的“否”側)，計算用于以作業(yè)結束點的角度為目標使第1關節(jié)驅動系統(tǒng)的驅動軸各自線性地變化的該第1關節(jié)驅動系統(tǒng)的驅動軸各自的角度(S8)，并基于該計算出的第1關節(jié)驅動系統(tǒng)的驅動軸各自的角度和步驟(S5)中計算出的插補點處的作業(yè)部的位置來計算第2關節(jié)驅動系統(tǒng)的驅動軸各自的角度(S9)，且遵照該計算結果來使第1以及第2關節(jié)驅動系統(tǒng)的驅動軸進行驅動(S11)。
【專利說明】多關節(jié)型機器人的控制裝置、控制方法以及控制程序
【技術領域】
[0001]本發(fā)明涉及具備第I關節(jié)驅動系統(tǒng)和第2關節(jié)驅動系統(tǒng)而成的多關節(jié)型機器人的控制技術，第I關節(jié)驅動系統(tǒng)具有使設置于前端的作業(yè)部的姿勢進行變化的3個驅動軸，第2關節(jié)驅動系統(tǒng)具有使所述第I關節(jié)驅動系統(tǒng)的位置進行變化的至少3個驅動軸。
【背景技術】
[0002]作為圖23所示的多關節(jié)型機器人的一種的6軸機械手具備:第I關節(jié)驅動系統(tǒng)，其具有使設置于前端的末端執(zhí)行器(作業(yè)部)的姿勢進行變化的J4軸、J5軸、J6軸；以及第2關節(jié)驅動系統(tǒng)，其具有使該第I關節(jié)驅動系統(tǒng)的位置進行變化的Jl軸、J2軸、J3軸。而且，6軸機械手的控制裝置基于預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢來控制Jl軸?J6軸，從而使末端執(zhí)行器的位置以及姿勢進行變化。
[0003]更具體而言，計算對連結作業(yè)開始點與作業(yè)結束點的示教路徑進行插補的多個插補點，并遵照該插補點使Jl軸?J6軸驅動。此外，在給出了末端執(zhí)行器的位置座標X、Y、Z以及姿勢角α、β、Υ時，通過求取逆運動學問題的解，能求取Jl軸?J 6軸的各關節(jié)角Θ1?Θ6。但此時作為逆運動學問題的解而得到的關節(jié)角Θ1?Θ 6存在兩組解。具體地，兩組解中的J4軸的角度Θ4，Θ ' 4、J5軸的角度Θ5，Θ ' 5、J6軸的角度Θ 6，Θ ' 6處于 Θ4-Θ' 4 = ±180 度，θ 5+Θ ' 5 = 0度，06-0' 6= ±180 度的關系。
[0004]而若在6軸機械手處J5軸的角度變?yōu)镺度，則將取J4軸、J6軸的速度急劇變化的所謂的奇異姿勢(奇異點)。故而，一般通過以與作業(yè)開始點處的解相同的符號直接使J5軸的角度移轉，從而避開了奇異姿勢。但在此情況下，在作業(yè)開始點以及作業(yè)結束點處的J5軸的角度的符號為不同符號時，不能使該J5軸的角度到達作業(yè)結束點處的角度。
[0005]與此相對，例如在專利文獻I中公開了:在檢測到處于奇異姿勢附近的情況下，轉移到逆運動學問題的另一解。根據(jù)這樣的構成，即使在作業(yè)開始點以及作業(yè)結束點處的J5軸的角度的符號為不同符號的情況下，也能使該J5軸的角度到達作業(yè)結束點處的角度。此夕卜，奇異姿勢的檢測一般是以J5軸的角度到達O度附近或J4軸以及J6軸的角度急劇變化為條件來進行的。
[0006]先行技術文獻
[0007]專利文獻
[0008]專利文獻1:日本特開平02-218569號公報
[0009]發(fā)明要解決的課題
[0010]然而，在所述專利文獻I所涉及的構成中，由于到達了奇異姿勢附近是向逆運動學問題的另一解的轉移條件，因此在未接近至奇異點姿勢而進行動作的情況下，不能使驅動軸到達作業(yè)結束點的正確的角度。
[0011]具體地，圖24示出了盡管作業(yè)開始點以及作業(yè)結束點處的J5軸的角度的符號是不同符號、但J5軸的角度未到達O度附近的前提下J4軸和J6軸的角度均未急劇變化而進行動作的情況下的軌跡。在此情況下，不能檢測接近至奇異姿勢附近。故而，J5軸的角度以作業(yè)開始點處的符號直接移動，因此不能使J4軸?J6軸的角度到達作業(yè)結束點的正確的角度。故而，例如有在朝著作業(yè)結束點的下一示教點進行動作的情況下J4軸?J6軸的角度會到達工作范圍外的風險。

【發(fā)明內容】

[0012]為此，本發(fā)明鑒于上述事實而提出，其目的在于，提供能在維持作業(yè)部的位置以及移動速度的同時使第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度可靠地到達作業(yè)結束點的角度的多關節(jié)型機器人的控制裝置、控制方法以及控制程序。
[0013]用于解決課題的手段
[0014]為了達成上述目的，本發(fā)明應用于對多關節(jié)型機器人進行控制的多關節(jié)型機器人的控制裝置，并構成為以具備以下的(I)?(4)為特征，其中，該多關節(jié)型機器人具備:第I關節(jié)驅動系統(tǒng)，其具有使設置于前端的作業(yè)部的姿勢進行變化的3個驅動軸；以及第2關節(jié)驅動系統(tǒng)，其具有使所述第I關節(jié)驅動系統(tǒng)的位置進行變化的至少3個驅動軸。
[0015](I)插補點計算單元，其計算多個插補點，所述插補點用于在對預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢進行連結的示教路徑上使所述作業(yè)部的位置以預先規(guī)定的速度進行移動。
[0016](2)驅動控制單元，其遵照由所述插補點計算單元計算出的所述插補點來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動。
[0017](3)例外條件判定單元，其在由所述驅動控制單元對所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行了驅動時，判定與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件是否滿足。
[0018](4)例外動作單元，其在由所述例外條件判定單元判定為所述例外條件滿足后，至所述作業(yè)部到達所述作業(yè)結束點為止的期間，計算用于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自線性地變化的該第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度，并基于該計算出的第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度和由所述插補點計算單元計算出的所述插補點處的所述作業(yè)部的位置來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸各自的角度，且遵照該計算結果來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動。
[0019]由此，能與所述作業(yè)開始點以及所述作業(yè)結束點處的所述第I關節(jié)驅動系統(tǒng)的中央的驅動軸的符號的異同無關地，在維持由所述插補點計算單元計算出的所述插補點處的所述作業(yè)部的位置以及移動速度的同時，使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度可靠地到達所述作業(yè)結束點的角度。
[0020]例如，所述例外條件是:在從下一所述插補點起的所述作業(yè)部的剩余的移動時間或移動距離的期間使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度到達所述作業(yè)結束點處的角度的情況下，所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的速度超過預先設定的容許范圍。由此，能在將所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的速度抑制到所述容許范圍內的同時使該第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度可靠地到達所述作業(yè)結束點的角度。另外，所述例外條件是:所述作業(yè)部的剩余的移動時間或移動距離達到預先設定的給定值以下。
[0021]進而，所述例外條件是:下一所述插補點處的所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的角度偏離基于所述作業(yè)開始點和/或所述作業(yè)結束點而確定的基準角度預先設定的給定角度以上。由此，例如能預防所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度過分偏離所述基準角度從而到達所述作業(yè)結束點的角度的速度變得迅急的情形，從而使該第I關節(jié)驅動系統(tǒng)的驅動軸各自可靠地到達所述作業(yè)結束點。
[0022]另外，所述例外條件可以是:下一所述插補點處的所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的角度超過預先設定的工作范圍。由此，能在不超過所述工作范圍的范圍內使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動而到達所述作業(yè)結束點的角度。
[0023]通過具備檢測所述作業(yè)部與加工對象物的加工線的偏差量的偏差量檢測單元、以及基于由所述偏差量檢測單元檢測出的偏差量使所述作業(yè)部仿形所述加工線進行移動的仿形控制單元在內的構成，所述作業(yè)結束點的位置會變化。
[0024]于是，考慮還具備在所述作業(yè)結束點加上由所述偏差量檢測單元檢測出的偏差量來更新該作業(yè)結束點的作業(yè)結束點更新單元，且所述例外條件判定單元或所述例外動作單元基于由所述作業(yè)結束點更新單元更新后的所述作業(yè)結束點來執(zhí)行處理。由此，即使在由所述仿形控制單元執(zhí)行仿形控制的情況下，所述例外動作單元也能使所述驅動軸各自到達由所述作業(yè)結束點更新單元更新后的作業(yè)結束點。所述例外條件判定單元所執(zhí)行的所述例外條件的判定也能以由所述作業(yè)結束點更新單元更新后的作業(yè)結束點為基準來適當?shù)剡M行。
[0025]進而，還考慮所述例外動作單元具備:角度候補計算單元，其在較之于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自進行線性動作的情況下離該目標更近的角度的范圍內，將該驅動軸各自的角度的候補計算多個；以及角度選擇單元，其從由所述角度候補計算單元計算出的角度的候補之中，選擇對在采用了各該候補時的下一所述插補點的從所述作業(yè)部觀察的作業(yè)坐標系下的3個姿勢角度的分量當中預先設定的一個或二個特定分量的變動而言抑制效果最好的角度的候補。
[0026]由此，能在抑制所述特定分量的變動的同時使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自可靠地到達所述作業(yè)結束點處的角度。
[0027]另外，本發(fā)明還能采用為多關節(jié)型機器人的控制方法或控制程序的發(fā)明。
[0028]S卩，還能體現(xiàn)為具備如下工序的多關節(jié)型機器人的控制方法或使處理器執(zhí)行如下工序的控制程序，所述工序包括:插補點計算工序，計算多個插補點，所述插補點用于在對預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢進行連結的示教路徑上使所述作業(yè)部的位置以預先規(guī)定的速度進行移動；驅動控制工序，遵照由所述插補點計算工序計算出的所述插補點來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動；例外條件判定工序，在由所述驅動控制工序對所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行了驅動時，根據(jù)與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件，來判定至所述第I關節(jié)驅動系統(tǒng)的驅動軸各自到達所述作業(yè)結束點的角度為止的期間該第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的速度或角度是否超過預先設定的給定范圍；以及例外動作工序，在由所述例外條件判定工序判定為所述例外條件滿足后，取代所述驅動控制工序，而使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動，使得所述第I關節(jié)驅動系統(tǒng)的驅動軸各自以所述作業(yè)結束點的角度為目標線性地變化，且所述作業(yè)部的位置以預先規(guī)定的速度進行移動。[0029]發(fā)明效果
[0030]根據(jù)本發(fā)明，能與所述作業(yè)開始點以及所述作業(yè)結束點處的所述第I關節(jié)驅動系統(tǒng)的中央的驅動軸的符號的異同無關地，在維持由所述插補點計算單元計算出的所述插補點處的所述作業(yè)部的位置以及移動速度的同時，使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度可靠地到達所述作業(yè)結束點的角度。
【專利附圖】

【附圖說明】
[0031]圖1是表征本發(fā)明的實施方式所涉及的多關節(jié)型機器人的控制裝置I以及多關節(jié)型機器人2的概略構成的框圖。
[0032]圖2是表示多關節(jié)型機器人2的關節(jié)驅動系統(tǒng)的模型圖。
[0033]圖3是表示由本發(fā)明的實施方式所涉及的多關節(jié)型機器人的控制裝置I執(zhí)行的機器人控制處理的過程的一例的流程圖。
[0034]圖4是表示現(xiàn)有的驅動軸34?36的動作例的圖。
[0035]圖5是表示在執(zhí)行了實施方式所涉及的機器人控制處理時的驅動軸34?36的動作例的圖。
[0036]圖6是表示現(xiàn)有的驅動軸34?36的動作例的圖。
[0037]圖7是表示在執(zhí)行了實施例1所涉及的機器人控制處理時的驅動軸34?36的動作例的圖。
[0038]圖8是用于說明機器人控制處理中的線性動作的變形例的圖。
[0039]圖9是表示實施例2所涉及的機器人控制處理的過程的一例的流程圖。
[0040]圖10是表示在執(zhí)行了實施例2所涉及的機器人控制處理時的驅動軸34?36的動作例的圖。
[0041]圖11是表示在一并執(zhí)行仿形控制以及機器人控制處理時的末端執(zhí)行器2的動作例的圖。
[0042]圖12是表示特定分量抑制處理的過程的一例的流程圖。
[0043]圖13是用于說明焊接線座標系Σ line的圖。
[0044]圖14是用于說明焊接線座標系Σ line的圖。
[0045]圖15是用于說明焊接線座標系Σ line的圖。
[0046]圖16是用于說明焊接線座標系Σ line的圖。
[0047]圖17是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0048]圖18是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0049]圖19是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0050]圖20是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0051]圖21是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0052]圖22是用于說明特定分量抑制處理的執(zhí)行結果的圖。
[0053]圖23是表示一般的多關節(jié)型機器人的關節(jié)驅動系統(tǒng)的模型圖。
[0054]圖24是表示在未接近奇異姿勢的情況下的現(xiàn)有的驅動軸J4軸?J6軸的動作例的圖。
[0055]圖25是用于說明對使奇異姿勢處于避開中進行報知的裝置的圖?！揪唧w實施方式】
[0056]以下，參照附圖來說明本發(fā)明的實施方式，以幫助理解本發(fā)明。此外，以下的實施方式只是使本發(fā)明具體化的一例，并不是為了限定本發(fā)明的技術范圍。
[0057]如圖1所示，本發(fā)明的實施方式所涉及的多關節(jié)型機器人的控制裝置I (以下簡稱為“控制裝置I”)對前端設置有末端執(zhí)行器2的多關節(jié)型機器人3的動作進行控制，具備:運算部11、存儲部12、以及操作部13等。所述末端執(zhí)行器2是對加工對象物施加例如焊接或涂裝等作用的焊接器(焊炬)、涂裝器、工具、捕捉器等。
[0058]所述存儲部12具有:將給定的控制程序或各種處理中所參照的參數(shù)信息等預先進行存儲的ROM或HDD等、以及作為由所述運算部11執(zhí)行各種處理時的作業(yè)區(qū)域(臨時存儲區(qū)域)進行利用的EEPROM等的易失性存儲器。
[0059]所述操作部13是具有板鍵或操作按鈕、操作桿等的用戶界面。所述操作部13將與用戶操作對應的操作信號輸入至所述運算部11。具體地，通過由用戶操作所述操作部13，來設定所述多關節(jié)型機器人3的作業(yè)開始點、作業(yè)結束點、作業(yè)時間、以及移動速度等各種加工參數(shù)。
[0060]所述運算部11是遵照所述存儲部12中所存儲的所述控制程序來執(zhí)行后述的機器人控制處理(參照圖3)等各種處理的CPU等的處理器。此外，本發(fā)明可以體現(xiàn)為用于使所述運算部11執(zhí)行后述的機器人控制處理的控制程序的發(fā)明。
[0061]在此，圖2是表示所述多關節(jié)型機器人3的關節(jié)驅動系統(tǒng)的模型圖。
[0062]如圖2所示，所述多關節(jié)型機器人3具備:第I關節(jié)驅動系統(tǒng)，其具有使所述末端執(zhí)行器2的姿勢進行變化的3個驅動軸34?36 ;以及第2關節(jié)驅動系統(tǒng)，其具有與該第I關節(jié)驅動系統(tǒng)的后端的驅動軸34連結來使該第I關節(jié)驅動系統(tǒng)的位置進行變化的3個驅動軸31?33。所述驅動軸31?36各自由電動機等驅動單元例如在土 180度的范圍內單獨地旋轉驅動。此外，所述第2關節(jié) 驅動系統(tǒng)可以是具有4軸以上的驅動軸的構成。
[0063]所述多關節(jié)型機器人3中的所述末端執(zhí)行器2的位置?姿勢通過該多關節(jié)型機器人3的基本座標系Σ base下的位置座標XYZ和姿勢角α β Y來表征。所述姿勢角α β Υ以歐拉角或輥距偏航角等來表征。在此，所述第2關節(jié)驅動系統(tǒng)能與所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36的動作無關地，自由地生成所述多關節(jié)型機器人3的基本座標系Σ base下的所述末端執(zhí)行器2的位置座標XYZ。
[0064]此外，所述多關節(jié)型機器人3中的所述驅動軸31?36各自的角度Θ1?Θ6能通過從所述多關節(jié)型機器人3的基本座標系Ebase下的位置座標XYZ、姿勢角α β Y求取逆運動學問題的解來進行計算。
[0065]以下，遵照圖3的流程圖，來說明由所述控制裝置I執(zhí)行的機器人控制處理的過程的一例。在此，圖3所示的S1，S2，…表示處理過程(步驟)編號。此外，本發(fā)明可以體現(xiàn)為執(zhí)行以下的處理過程的各工序的多關節(jié)型機器人的控制方法的發(fā)明。
[0066]所述控制裝置I通過執(zhí)行該機器人控制處理，來基于由所述操作部13預先設定的作業(yè)開始點以及作業(yè)結束點對所述多關節(jié)型機器人3進行控制。在此，設通過所述操作部13的操作來預先設定了作業(yè)開始點Ql的位置.姿勢{Xs，Ys, Zs, a s，β s，Y s}、以及作業(yè)結束點Q2的位置.姿勢{Xe，Ye, Ze, a e, β e，Ye}。[0067](步驟SI)
[0068]首先，在步驟SI中，所述控制裝置I求取路徑方程式P (t) = Ql+ΛΡ.t，該路徑方程式用于計算插補對所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2各自的位置以及姿勢進行連結的示教路徑的多個插補點。在此，ΛΡ表示每單位時間(Imsec)的所述末端執(zhí)行器2的位置的移動量，t表示從所述作業(yè)開始點Ql起計時的控制時間[msec]。此外，以所述路徑方程式P (t)所計算的插補點通過所述多關節(jié)型機器人3的基本座標系Σ base下的位置.姿勢ΧΥΖα β y (t)來表征。
[0069]具體地，所述路徑方程式P (t)計算用于使所述末端執(zhí)行器2的位置以預先規(guī)定的速度從所述作業(yè)開始點Ql起移動到所述作業(yè)結束點Q2的多個插補點。具體地，若將所述末端執(zhí)行器2的位置從所述作業(yè)開始點Ql起移動到所述作業(yè)結束點Q2時的總控制時間設為T[msec],將所述末端執(zhí)行器2的位置.姿勢XYZ α β y的每單位時間(Imsec)的變化量設為 Λ X、Λ Y、Λ Ζ、Λ a、Λ β、Λ Y，貝Ij Λ P 通過(Q2-Q1) / T= { Λ X = (Xe-Xs) / Τ,Δ Y = (Ye-Ys) / Τ, Δ Z = (Ze-Zs) / Τ, Δ a = ( a e_ a s) / Τ, Δ β = (β e_ β s) / Τ,Δ Y = (ye-ys) / Τ}來表征。當然，不限于設定所述總控制時間T的情況，還可以將所述末端執(zhí)行器2的移動速度設定為預先規(guī)定的速度。
[0070](步驟S2 ?S3)
[0071]接著，在步驟S2中，所述控制裝置I通過求取所述作業(yè)結束點Q2處的所述末端執(zhí)行器2的位置?姿勢{Xe，Ye，Ze，a e, β e, ye}的逆運動學問題的解，來計算該作業(yè)結束點Q2處的所述驅動軸31?36的角度{ Θ le，Θ 2e，Θ 3e，Θ 4e，Θ 5e，Θ 6e}。此外，在基于所述操作部13的示教時可以直接設定所述作業(yè)結束點Q2處的所述驅動軸31?36的角度 Θ Ie ?Θ 6e。
[0072]另外，在步驟S3中，所述控制裝置I將控制時間t以及控制開始標志F初始設定為O。此外，所述控制開始 標志F是表示后述的例外條件(S61)是否已滿足的信息，在后述的步驟S62中被設定為I。
[0073](步驟S4 ?S6)
[0074]接下來，在步驟S4中，所述控制裝置I更新所述控制時間t(t = t+At)。在此，Δ t是所述控制裝置I的所述多關節(jié)型機器人3的控制周期(采樣周期)，例如是10 [msec]或 100[msec]。
[0075]然后，在步驟S5中，所述控制裝置I基于所述路徑方程式P (t)來計算下一插補點處的所述驅動軸31?36各自的角度Θ l(t)?Θ 6 (t)。在此，執(zhí)行相應的計算處理時的所述控制裝置I相當于插補點計算單元。
[0076]具體地，首先，所述控制裝置I計算下一插補點的位置?姿勢XYZ α β Y (t)={Xs+ Δ X.t, Ys+ Δ Y.t, Zs+ ΔΖ.?, as+Δ a.t, β s+ Δ β.t, Y S+ Δ γ.t} ? 然后，所述控制裝置I通過求取所述位置?姿勢XYZa β Y (t)的逆運動學問題的解，來計算下一插補點處的所述驅動軸31?36各自的角度01(t)、02(t)、03(t)、04(t)、θ 5 (t)、θ 6 (t)。此外，在將所述控制開始標志F設定為I后，不使用所述角度Θ l(t)?Θ 6 (t)，因此該步驟S5的處理可以省略。
[0077]其后，在步驟S6中，所述控制裝置I判斷所述控制開始標志F是否為I。在此，若判斷為所述控制開始標志F是I (S6的“是”側)，則處理轉移至步驟S7，若判斷為所述控制開始標志F是O (S6的“否”側)，則處理轉移至步驟S61。
[0078](步驟S61)
[0079]在步驟S61中，所述控制裝置I判定與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件是否滿足。在此，該步驟S61是在后述的步驟S64中所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸31?36各自基于所述步驟S5中計算出的角度ei(t)?0 6(t)被驅動時來執(zhí)行的。在此，執(zhí)行相應的判定處理時的所述控制裝置I相當于例外條件判定單元。所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件是指，例如所述驅動軸35的角度接近O或所述驅動軸34、36的角度發(fā)生了急劇變化。
[0080]具體地，在本實施方式中，在從下一插補點起的所述末端執(zhí)行器2的剩余的移動時間的期間使所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36各自的角度到達所述作業(yè)結束點Q2處的角度Θ 4e?Θ 6e的情況下，將所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36的至少一個驅動軸的速度超過預先設定的容許范圍預先設定為所述例外條件。即，所述控制裝置I在該步驟S61中，在所述末端執(zhí)行器2的剩余的移動時間的期間，判定是否能在將所述驅動軸34?36各自的速度維持在所述容許范圍內的同時到達所述作業(yè)結束點Q2。
[0081]例如，所述控制裝置I在以下的(11)式?(13)式均成立的情況下，能判定為所述例外條件已滿足。在此，T(t)是下一插補點處的剩余的移動時間，θ 4d、Θ 5d、Q6d是所述驅動軸34?36的速度的容許范圍的上限值。
[0082]I ( Θ 4e- Θ 4(t)) / T (t) | > Θ 4d…(11)
[0083]I ( Θ 5e_ θ 5 (t)) / T (t) | > Θ 5d…(12)
[0084]( Θ 6e- Θ 6(t)) / T (t) | > Θ 6cl...(13)
[0085]此外，不限 于所述末端執(zhí)行器2的剩余的移動時間，還可以在該末端執(zhí)行器2的剩余的移動距離(=剩余的移動時間X移動速度)之間，判定是否能在將所述驅動軸34?36各自的速度維持在容許范圍內的同時，使所述末端執(zhí)行器2到達所述作業(yè)結束點Q2。另夕卜，作為其他的實施例，還考慮僅將所述末端執(zhí)行器2的剩余的移動時間或剩余的移動距離達到預先設定的給定值以下作為所述例外條件進行使用。
[0086]然后，在該步驟S61中若判斷為所述例外條件未滿足(S61的“否”側)，則處理轉移至步驟S64，若判斷為所述例外條件已滿足(S61的“是”側)，則處理轉移至步驟S62。
[0087](步驟SM)
[0088]在步驟S64中，所述控制裝置I基于所述步驟S5中計算出的角度Θ I (t)、Θ 2 (t)、03(t)、04(t)、05(t)、0 6(t)來使所述驅動軸31?36各自進行驅動，使處理轉移至步驟S12。此外，該步驟S64中的處理與現(xiàn)有技術相同，故省略詳細的說明。在此，執(zhí)行相應的驅動處理時的所述控制裝置I相當于驅動控制單元。
[0089](步驟S62 ?S63)
[0090]另一方面，若所述例外條件滿足，則在接下來的步驟S62中，所述控制裝置I將所述控制開始標志F設定為I。由此，今后，在所述步驟S6中，若判斷為所述控制開始標志F為1，則處理不經(jīng)由所述步驟S61?S63而轉移至后述的步驟S7。
[0091]接下來，在步驟S63中，所述控制裝置I將所述驅動軸34?36各自的當前的角度設為 0 4old、0 5old、Θ6_ 而存儲至所述存儲部 12。即，Θ 4old = 04(t-At), Θ 50LD =θ 5 (t- At)，Θ 6old = θ 6 (t- Δ t)。[0092](步驟S7 ?S8)
[0093]然后，在步驟S7中，所述控制裝置I在從當前的插補點起的剩余的移動時間T(t-At)的期間，計算使所述驅動軸34?36各自的角度到達所述作業(yè)結束點Q2的角度所需的所述驅動軸34?36各自的每單位時間(Imsec)的速度V Θ 4、V Θ 5、V θ 6。在此，速度V Θ 4、V Θ 5、V Θ 6是用于使當前的所述驅動軸34?36各自的角度Θ 40LD、Θ 5_、Θ 60LD線性地變化至所述作業(yè)結束點Q2處的角度Θ 4e、Θ 5e、Θ 6e的速度。具體地，速度V Θ 4、V05、V06能通過以下的(21)式?(23)式來進行計算。
[0094]V Θ 4 = ( Θ 4e- Θ 40LD) / T (t_ Λ t)…(21)
[0095]V θ 5 = ( θ 5e_ θ 50LD) / T (t_ Λ t)…(22)
[0096]V θ 6 = ( θ 6e_ θ 60LD) / T (t_ Λ t)…(23) [0097]接下來，在步驟S8中，所述控制裝置I基于所述步驟S7中計算出的速度V Θ 4、V Θ 5、V Θ 6，來計算下一插補點的所述驅動軸34?36的角度Θ 4麗、Θ 5NQW、Θ 6N0ffO在此，角度Θ 4nqw、Θ 5NOff> Θ6νμ能通過以下的(31)式?(33)式來計算。
[0098]Θ 4麗=Θ 4old+V Θ 4.Δ t...(31)
[0099]Θ 5麗=Θ 5old+V θ 5.At...(32)
[0100]Θ 6NOff = θ 6old+V θ 6.At...(33)
[0101](步驟S9 ?S10)
[0102]然后，在步驟S9中，所述控制裝置I基于所述步驟S8中計算出的下一插補點的角度Θ 4now、Θ 5NOff> Θ 6now>以及所述步驟S5中計算出的下一插補點的位置座標XYZ (t),來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸31?33各自的下一插補點處的角度Θ lNOff> θ 2μ、θ 3μ。
[0103]在此所計算的所述驅動軸31?33的角度Θ lNOff> θ 2μ、Θ 3NOff維持了所述步驟S5中計算出的下一插補點的位置座標XYZ(t)，因此所述末端執(zhí)行器2的位置以及移動速度維持了所述步驟S64的驅動控制時的所述末端執(zhí)行器2的位置以及移動速度。
[0104]此外，所述控制裝置I在步驟SlO中為了將所述驅動軸34?36各自的角度Θ 4NQW、Θ 5nqw、Θ 6now在下一控制周期中進行使用而作為Θ 4old、Θ 5old> Θ 6old進行存儲(Θ 4old =9 4now，Θ 5old — Θ 5now，Θ 6old — Θ 6now)。
[0105](步驟Sll ?S12)
[0106]其后，在步驟Sll中，所述控制裝置I基于所述步驟S8中計算出的下一插補點處的所述驅動軸34?36的角度Θ4_、Θ5_、Θ6Ν?、以及所述步驟S9中計算出的下一插補點處的所述驅動軸31?33的角度θ 1Ν0Ι> Θ 2NOff, Θ 3N()W，來使所述驅動軸31?36驅動。如此，在該機器人控制處理中，在所述例外條件已滿足后，取代所述步驟S64中的驅動控制，而執(zhí)行所述步驟Sll中的驅動控制。
[0107]此外，所述控制裝置I在接下來的步驟S12中，判斷所述末端執(zhí)行器2是否已到達所述作業(yè)結束點Q2，并直到該末端執(zhí)行器2到達所述作業(yè)結束點Q2為止(S12的“否”側)，反復執(zhí)行所述步驟S4以后的處理。另一方面，若所述末端執(zhí)行器2到達所述作業(yè)結束點Q2 (S12的“是”側)，則結束該機器人控制處理。
[0108]如以上說明所述，本發(fā)明的實施方式所涉及的所述控制裝置I在判定為所述例外條件已滿足后，直到所述末端執(zhí)行器2到達所述作業(yè)結束點Q2為止的期間，計算用于以所述作業(yè)結束點Q2的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36各自線性地變化的該第I關節(jié)驅動系統(tǒng)的驅動軸34?36各自的角度，并基于該計算出的第I關節(jié)驅動系統(tǒng)的驅動軸34?36各自的角度和通過所述步驟S5計算出的插補點處的所述末端執(zhí)行器2的位置來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸31?33各自的角度，且遵照該計算結果來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動。在此，執(zhí)行相應的處理時的所述控制裝置I相當于例外動作單元。
[0109]因此，能在維持所述步驟S5中計算出的插補點處的所述末端執(zhí)行器2的位置以及移動速度的同時，使所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36各自的角度可靠地到達所述作業(yè)結束點Q2的角度。由此，例如能防止在從所述作業(yè)結束點Q2起朝著下一示教點進行動作的情況下所述驅動軸34?36超過工作范圍的情形。
[0110]所述控制裝置I在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2的所述驅動軸35的角度的符號為不同符號、且若不經(jīng)歷所述第I關節(jié)驅動系統(tǒng)的奇異姿勢(驅動軸35的角度為O度)則不能使所述驅動軸35的角度到達所述作業(yè)結束點Q2的角度的情況下特別適用。以下，參照圖4以及圖5來說明該點。在此，圖4以及圖5示出在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號為不同符號的情況下的動作例，圖4示出現(xiàn)有例，圖5示出所述機器人控制處理(圖3)的執(zhí)行結果。具體地，所述作業(yè)開始點Ql處的所述驅動軸35的角度Θ 5s為+65度，所述作業(yè)結束點Q2處的所述驅動軸35的角度Θ 5e 為-65 度。
[0111]如圖4所示，通常，在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的角度Θ 5s、Θ Se的符號為不同符號的情況下，所述末端執(zhí)行器2以維持了所述作業(yè)開始點Ql處的所述驅動軸35的符號的解直接進行移動，因此所述驅動軸34?36各自不到達所述作業(yè)結束點Q2處的角度。故而，例如在從所述作業(yè)結束點Q2起朝著下一示教點進行動作的情況下，存在所述驅動軸34?36脫離工作范圍的風險。另外，即使是以所述驅動軸35接近O度或所述驅動軸34、36的速度成為給定值以上為條件來檢測所述第I關節(jié)驅動系統(tǒng)的奇異姿勢并轉移至不同的解的構成，如圖4所示，在所述驅動軸35未接近O度、所述驅動軸34、36的速度未成為給定值以上的情況下，也不能檢測所述第I關節(jié)驅動系統(tǒng)的奇異姿勢。
[0112]與此相對，如圖5所示，在執(zhí)行所述機器人控制處理(圖3)的情況下，若判定為所述例外條件已滿足，則從該時間點LI起驅動所述驅動軸31?36以使所述驅動軸34?36朝著所述作業(yè)結束點Q2的角度線性地變化，因此能使該驅動軸34?36各自可靠地到達所述作業(yè)開始點Q2處的角度。
[0113]當然，即使在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號為相同符號的情況下，也同樣地，從所述例外條件已滿足的時間點起，所述驅動軸34?36朝著所述作業(yè)結束點Q2處的角度線性地變化，因此能使該驅動軸34?36各自可靠地到達所述作業(yè)開始點Q2處的角度。
[0114]【實施例1】
[0115]在本實施例1中，針對所述機器人控制處理(參照圖3)中的所述例外條件的其他的例子進行說明。
[0116]具體地，在本實施例1中，在所述步驟S61中，在所述驅動軸34?36的至少一個驅動軸的下一插補點處的角度超過預先設定的工作范圍的情況下，所述控制裝置I判定為所述例外條件已滿足。例如，在預先設定的所述驅動軸34?36各自的正側的工作范圍為Θ 4+L、Θ 5+L、Θ 6+L、且所述驅動軸34?36各自的負側的工作范圍為Θ 4-L、Θ 5-L、Θ 6-L時，所述控制裝置I在以下的(41)?(46)的至少一個成立了的情況下判定為所述例外條件已滿足。此外，所述驅動軸34?36各自的工作范圍例如指所述驅動軸34?36的驅動電動機的容許動作范圍。
[0117]Θ 4(t) > Θ 4+l...(41)
[0118]Θ 4(t) < Θ 4-l...(42)
[0119]θ 5 (t) > Θ 5+l...(43)
[0120]θ 5(t) < θ 5-l...(44)
[0121 ] θ 6 (t) > θ 6+l…(45)
[0122]θ 6(t) < θ (46)
[0123]在此，圖6以及圖7示出在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號為不同符號的情況下的動作例，圖6示出現(xiàn)有例，圖7示出本實施例1所涉及的機器人控制處理的執(zhí)行結果。具體地，所述作業(yè)開始點Ql處的所述驅動軸35的角度Θ 5s為-10度，所述作業(yè)結束點Q2處的所述驅動軸35的角度Θ Se為+10度。此外，所述驅動軸34?36的角度的工作范圍設為-180度?+180度。
[0124]如圖6所示,若與所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號是否為不同符號無關地，所述末端執(zhí)行器2以維持了所述作業(yè)開始點Ql處的所述驅動軸35的符號的解直接進行移動，則存在所述驅動軸34、36的角度超過該驅動軸34，36的工作范圍(±180度)從而異常停止的風險。
[0125]與此相對，如圖7所示，在執(zhí)行本實施例1所涉及的機器人控制處理的情況下，若判定為所述驅動軸34?36的至少一個驅動軸的下一插補點處的角度超過預先設定的工作范圍這樣的所述例外條件已滿足，則驅動所述驅動軸31?36以使從其時間點L2起所述驅動軸34?36朝著所述作業(yè)結束點Q2的角度線性地變化。然后，一般在所述控制裝置I中能將所述作業(yè)結束點Q2設定為僅在所述驅動軸34?36的角度的工作范圍內。故而，能在使所述驅動軸34?36各自在容許范圍內進行驅動的同時使所述作業(yè)開始點Q2的角度可靠地到達。當然，即使在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號為相同符號的情況下也同樣。
[0126]而在圖7所示的動作例中，所述驅動軸34?36各自的角度在比所述作業(yè)結束點Q2稍微提前的時間點到達該作業(yè)結束點Q2的附近，其后，朝著所述作業(yè)結束點Q2處的角度以緩慢的速度進行動作。如此，若一點點地降下所述驅動軸34?36各自的速度來使得緩慢地停止至所述作業(yè)結束點Q2，則能抑制停止時的振動等。
[0127]特別地，此時期望所述控制裝置I對所述驅動軸34?36各自的速度進行調節(jié)來使該驅動軸34?36各自同時到達所述作業(yè)結束位置Q2處的角度而停止。由此，較之于單獨停止所述驅動軸34?36各自的驅動電動機的情況，能抑制在該驅動電動機的停止時產(chǎn)生的振動等。進而，還考慮校正該驅動軸34?36的角度以使所述例外條件滿足的時間點L2上的所述驅動軸34?36各自的線性動作開始時的速度變緩。例如圖8示出了在所述例外條件滿足的前后的所述驅動軸36的角度的變化例。如圖8中實線所示，若在所述驅動軸36的角度增加到180度后突然朝著負方向變化，則在該驅動軸36產(chǎn)生急劇的負方向的加速度，從而成為振動等的原因。于是，如圖8中虛線所示，考慮事先估計到達所述驅動軸36的工作范圍的上限值180，并校正該驅動軸36的角度以使從該時間點起所述驅動軸36的角度緩慢地朝著負方向變化。由此，在所述例外條件滿足后不會立刻產(chǎn)生所述驅動軸36的急劇的加速度，因此能防止振動等。當然，關于所述驅動軸35、36也同樣。
[0128]另外，如此用于防止所述驅動軸34?36的急劇的停止或急劇的加速度的控制手法還能同樣適用于所述實施方式或后述的實施例2等的使用其他的例外條件的情況。
[0129]【實施例2】
[0130]在本實施例2中，說明所述機器人控制處理(參照圖3)中的所述例外條件的又一例。
[0131]在此，圖9是表示本實施例2所涉及的機器人控制處理的過程的一例的流程圖。此外，針對與所述機器人控制處理(參照圖3)同樣的處理過程賦予相同的符號并省略其說明。
[0132]具體地，如圖9所示，在本實施例2所涉及的機器人控制處理中，取代所述步驟S2?S4、S61(參照圖3)而執(zhí)行后述的步驟S21、S31、S41、S611。
[0133](步驟S21、S31、S41)
[0134]在步驟S21中，所述控制裝置I 一并計算所述作業(yè)結束點Q2處的所述驅動軸34?36的角度Θ 4e、Θ 5e、Θ 6e、以及所述作業(yè)開始點Ql處的所述驅動軸34?36的角度Θ 4s、Θ5s、 Θ6s0.[0135]另外，所述控制裝置I在該步驟S21中還計算在使所述驅動軸34?36從所述作業(yè)開始點Ql起到所述作業(yè)結束點Q2為止以一定速度線性地動作時的每單位時間(Imsec)的所述驅動軸34?36的角度變動Λ Θ 4、Λ Θ 5、Λ Θ 6。此時，若將所述作業(yè)開始點Ql處的所述末端執(zhí)行器2的剩余的移動時間設為T(S)，則角度變動Λ Θ 4、Λ Θ 5、Λ Θ 6通過以下的(51)?(53)來求取。
[0136]Λ Θ 4 = ( Θ 4e_ Θ 4s)/T(s)...(51)
[0137]Λ θ 5 = ( Θ 5e_ Θ 5s)/T(s)...(52)
[0138]Λ θ 6 = ( θ 6e_ θ 6s)/T(s)...(53)
[0139]接下來，在步驟S31中，所述控制裝置I不僅將控制時間t以及控制開始標志F初始設定為0，還進行與所述驅動軸34?36對應的基準角度0k(t) = { Θ 4k, 9 5k, 9 6k}的初始設定。所述基準角度9k(t)在后述的步驟S611中在例外條件的判斷中被參照，其初始值 Θ k(0)被設定為{ Θ 4k, Θ 5k，Θ 6k} = { Θ 4s，Θ 5s，Θ 6s}。
[0140]另外，關于所述控制裝置1，在步驟S41中，所述控制裝置I更新所述控制時間t(t = t+Δ t),并更新所述基準角度Θ k(t)。具體地，基準角度Θ k(t)成為:Θ 4k =Θ 4k+ Δ θ 4.At，θ 5k = θ 5k+ Δ θ 5.At，θ 6k = θ 6k+ Δ θ 6.At。
[0141]S卩，所述基準角度θ k(t)是基于所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2而確定的，被隨時更新成對所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2進行連結的直線上的值。此外，作為其他的實施例，還考慮所述基準角度9k(t)為所述作業(yè)開始點Ql或所述作業(yè)結束點Q2處的所述驅動軸34?36的角度(步驟S611)。
[0142]然后，在本實施例2所涉及的機器人控制處理中，在步驟S611的例外條件的判定處理中，所述控制裝置I判斷下一插補點處的所述第I關節(jié)驅動系統(tǒng)的驅動軸34?36的角度Θ 4(t)、Θ 5(t)、Θ 6(t)的至少一個與所述基準角度Θ 4k、Θ 5k、Θ 6k之間的偏離角度是否超過預先設定的給定角度Θ 4w、Θ 5w、Θ 6w。為了對所述驅動軸34?36的角度Θ 4(t)、Θ 5 (t)、Θ 6 (t)過分偏離所述基準角度Θ 4k、Θ 5k、Θ 6k從而為了使該驅動軸34?36到達所述作業(yè)結束點Q2會伴隨該驅動軸34?36的角度的急劇的變化的可能性的情形進行事先估計，預先設定所述給定角度Θ 4w、Θ 5w、Θ 6w。例如,考慮將所述給定角度Θ 4w、Θ 5w、Θ 6w各自設定為90度、180度、90度。
[0143]例如，所述控制裝置I在下述的￠1)式?￠3)式的任一者成立的情況下判定為所述例外條件已滿足。
[0144]I Θ 4(t)- Θ 4k > 9 4w…(61)
[0145]I θ 5 (t) - Θ 5k > θ 5w...(62)
[0146]I θ 6 (t) - θ 6k > θ 6w…(63)
[0147]在此，圖10示出在所述作業(yè)開始點Ql以及所述作業(yè)結束點Q2處的所述驅動軸35的符號為不同符號的情況下的動作例，示出本實施例2所涉及的所述機器人控制處理的執(zhí)行結果。具體地，所述作業(yè)開始點Ql處的所述驅動軸35的角度0 5s為+10度，所述作業(yè)結束點Q2處的所述驅動軸35的角度Θ5θ為-10度。另外，在此以將所述給定角度0 6w設定為90度、且所述驅動軸36的角度0 6(t)偏離所述基準角度Θ 6k(t)90度的情況為例進行說明。當然，關于所述驅動軸34、35也同樣。
[0148]如圖10所示，在執(zhí)行本實施例2所涉及的機器人控制處理的情況下，從判斷為所述驅動軸36的下一插補點的角度06(t)偏離所述基準角度0 6k(t)9O度以上的時間點L3起，驅動所述驅動軸31?36以使所述驅動軸34?36朝著所述作業(yè)結束點Q2的角度線性地變化，因此能使該驅動軸34?36各自可靠地到達所述作業(yè)開始點Q2處的角度。特別地，根據(jù)本實施例2所涉及的機器人控制處理，在所述驅動軸34?36各自的角度過分偏離所述基準角度Θ k(t)前的 早期的階段，能使該驅動軸34?36朝著所述作業(yè)結束點Q2線性動作，從而能將所述驅動軸34?36的急劇的角度變動防范于未然。
[0149]盡管所述控制裝置I只要判斷所述實施方式、所述實施例1、本實施例2中說明的多個所述例外條件的至少一個滿足的有無即可，但在同時監(jiān)視多個所述例外條件的滿足的有無、且任一個例外條件已滿足的情況下，還考慮執(zhí)行所述步驟S7起以后的線性動作處理。由此，能具體實現(xiàn)同時起到所述實施方式、所述實施例1、本實施例2所得到的效果的構成。
[0150]【實施例3】
[0151]在本實施例3中，考慮所述控制裝置I執(zhí)行對所述多關節(jié)型機器人3進行控制的仿形控制以使所述末端執(zhí)行器2仿形加工對象物的加工線進行移動的構成。在此情況下，在所述多關節(jié)型機器人3，設置對所述末端執(zhí)行器2與所述加工線的偏差量ΔΧΥΖα β y (t)進行檢測的傳感器(相當于偏差量檢測單元)。
[0152]在此，在各時間點由所述傳感器檢測的所述偏差量ΛΧΥΖα β y (t)是以通過所述路徑方程式P(t)計算出的該時間點上的插補點為基準而表征的值。然后，所述控制裝置I基于所述傳感器的檢測結果來執(zhí)行使所述末端執(zhí)行器2仿形所述加工線進行移動的所述仿形控制。在此，執(zhí)行相應的仿形控制時的所述控制裝置I相當于仿形控制單元。
[0153]具體地，所述控制裝置I在所述機器人控制處理(參照圖3)的步驟S5中計算下一插補點時，將由當前所述傳感器檢測出的偏差量ΛΧΥΖα β Y (t)相加到所述路徑方程式P(t)。另外，所述控制裝置I通過根據(jù)該相加后的插補點的位置.姿勢VVVβ / Y / (t)求取逆運動學問題的解，來計算下一插補點處的所述驅動
軸31?36各自的角度Θ1' (t)?Θ6' (t)。然后，在所述例外條件未滿足的情況下，所述控制裝置I在所述步驟S64中基于以所述偏差量ΛΧΥΖα β Y (t)修正后的所述角度Θ I'⑴?Θ 6' (t)來驅動所述驅動軸31?36。
[0154]另外，所述控制裝置I在所述步驟S5中將所述作業(yè)結束點Q2更新成對該作業(yè)結束點Q2相加了所述偏差量ΛΧΥΖα β y (t)后的值。此時，將更新后的所述作業(yè)結束點Q2處的所述驅動軸34?36各自的角度設為0 4e(t)、Θ 5e (t)、Θ 6e (t)。在此，執(zhí)行相應的處理時的所述控制裝置I相當于作業(yè)結束點更新單元。
[0155]由此，在所述例外條件的判斷處理中，基于所述更新后的所述作業(yè)結束點Q2來判斷所述例外條件的滿足的有無。例如，作為所述實施方式中的所述步驟S61的例外條件的判定指標的上述(11)式?(13)式成為以下的(11')式?(13')式。
[0156]( Θ 4e(t)- Θ 4(t))/T(t) | > Θ 4d...(11;)
[0157]( θ 5e (t) - θ 5 (t)) /T (t) | > θ 5d— (12;)
[0158]( θ 6e (t) - θ 6 (t)) /T (t) | > θ 6d— (13;)
[0159]另外，在所述步驟S7中用于計算所述速度V θ 4、V Θ 5、V θ 6的上述(21)式?(23)式成為以下的(21')式?(23')式。S卩，在所述步驟Sll中的所述驅動處理中，以所述更新后的所述作業(yè)結束點Q2為目標來進行所述驅動軸34?36的線性動作處理。
[0160]V Θ 4 = ( Θ 4e ⑴-Θ 40LD)/T (t_ Δ t)…(21')
[0161]V Θ 5 = ( Θ 5e(t)_ Θ 50LD)/T(t_At)…(22')
[0162]V Θ 6 = ( Θ 6e(t)_ Θ 60LD)/T(t_At)…(23')
[0163]由此，即使在基于由所述傳感器檢測的偏差量來執(zhí)行所述仿形控制的情況下，也能使所述驅動軸34?36各自的角度可靠地到達以所述仿形控制進行了修正后的作業(yè)結束點Q2。
[0164]具體地，如圖11(a)?(C)所示，所述末端執(zhí)行器2從所述作業(yè)開始點Ql起朝著所述作業(yè)結束點Q2移動，在通過所述仿形控制對所述偏差量ΛΧΥΖα β Y (t)進行修正時，所述作業(yè)結束點Q2相加所述偏差量ΛΧΥΖα β Y (t)而被更新成作業(yè)結束點Q2(t)。由此，能使所述驅動軸34?36各自的角度到達所述作業(yè)結束點Q2 (t)。
[0165]【實施例4】
[0166]而在所述機器人控制處理(參照圖3)中，不希望完全無視所述末端執(zhí)行器2的姿勢。于是，在本實施例4中，針對能抑制所述末端執(zhí)行器2的姿勢當中預先規(guī)定的特定分量的變動的構成進行說明。
[0167]具體地，本實施例4中，所述控制裝置I在所述機器人控制處理(參照圖3)的所述步驟S8與所述步驟S9之間執(zhí)行后述的特定分量抑制處理(參照圖12)。此外，關于所述機器人控制處理，不限于在所述實施方式中說明的處理，采用所述實施例1?3的任一者所記載的內容即可。
[0168]以下，遵照圖12的流程圖，來說明由所述控制裝置I執(zhí)行的特定分量抑制處理的過程的一例。[0169](步驟S81)
[0170]首先，在步驟S81中，所述控制裝置I針對下一插補點，如圖13所示，求取將所述末端執(zhí)行器2移動的方向(行進方向)的軸作為X軸、將通過該X軸與重力方向的外積(X軸X重力方向)所表征的軸作為Y軸、且將通過該X軸與該Y軸的外積(X軸XY軸)所表征的方向的軸作為Z軸的作業(yè)座標系(以下，稱為“焊接線座標系Σ line”)下的姿勢角度。在此，所述焊接線座標系Σ line下的所述末端執(zhí)行器2的姿勢角度如圖14?圖16所示，通過所述X軸的繞軸的旋轉角即焊炬傾斜角Rx、所述Y軸的繞軸的旋轉角即焊炬前進角Ry、以及所述Z軸的繞軸的旋轉角即焊炬旋轉角Rz來表征。
[0171]具體地，所述控制裝置I將所述步驟S5中計算出的下一插補點的所述基本座標系Σ base下的位置?姿勢XYZa β y (t)變換成所述焊接線座標系Σ line下的姿勢角度(焊炬傾斜角Rx，焊炬前進角Ry，焊炬旋轉角Rz)。
[0172]例如，從所述基本座標系Σ base到所述焊接線座標系Σ line的變換遵照以下的
(101)式?(107)式來進行。此外，將所述焊接線座標系Σ line下的X軸、Y軸、Z軸的單位矢量設為Xm、Ym、Zm。
【權利要求】
1.一種多關節(jié)型機器人的控制裝置，對多關節(jié)型機器人進行控制，所述多關節(jié)型機器人具備--第I關節(jié)驅動系統(tǒng)，其具有使設置于前端的作業(yè)部的姿勢進行變化的3個驅動軸；以及第2關節(jié)驅動系統(tǒng)，其具有使所述第I關節(jié)驅動系統(tǒng)的位置進行變化的至少3個驅動軸， 所述多關節(jié)型機器人的控制裝置具備: 插補點計算單元，其計算多個插補點，所述插補點用于在對預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢進行連結的示教路徑上使所述作業(yè)部的位置以預先規(guī)定的速度進行移動； 驅動控制單元，其遵照由所述插補點計算單元計算出的所述插補點來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動； 例外條件判定單元，其在由所述驅動控制單元對所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行了驅動時，判定與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件是否滿足；以及 例外動作單元，其在由所述例外條件判定單元判定為所述例外條件滿足后，至所述作業(yè)部到達所述作業(yè)結束點為止的期間，計算用于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸 各自線性地變化的該第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度，并基于該計算出的第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度和由所述插補點計算單元計算出的所述插補點處的所述作業(yè)部的位置來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸各自的角度，且遵照該計算結果來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動。
2.根據(jù)權利要求1所述的多關節(jié)型機器人的控制裝置，其中， 所述例外條件是:在從下一所述插補點起的所述作業(yè)部的剩余的移動時間或移動距離間使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度到達所述作業(yè)結束點處的角度的情況下，所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的速度超過預先設定的容許范圍。
3.根據(jù)權利要求1或2所述的多關節(jié)型機器人的控制裝置，其中， 所述例外條件是:所述作業(yè)部的剩余的移動時間或移動距離達到預先設定的給定值以下。
4.根據(jù)權利要求1?3中任一項所述的多關節(jié)型機器人的控制裝置，其中， 所述例外條件是:下一所述插補點處的所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的角度偏離基于所述作業(yè)開始點和/或所述作業(yè)結束點而確定的基準角度預先設定的給定角度以上。
5.根據(jù)權利要求1?4中任一項所述的多關節(jié)型機器人的控制裝置，其中， 所述例外條件是:下一所述插補點處的所述第I關節(jié)驅動系統(tǒng)的至少一個驅動軸的角度超過預先設定的工作范圍。
6.根據(jù)權利要求1?5中任一項所述的多關節(jié)型機器人的控制裝置，其中， 所述多關節(jié)型機器人的控制裝置還具備:偏差量檢測單元，其檢測所述作業(yè)部與加工對象物的加工線的偏差量；仿形控制單元，其基于由所述偏差量檢測單元檢測出的偏差量，使所述作業(yè)部仿形所述加工線進行移動；以及作業(yè)結束點更新單元，其在所述作業(yè)結束點加上由所述偏差量檢測單元檢測出的偏差量來更新該作業(yè)結束點，所述例外條件判定單元和/或所述例外動作單元基于由所述作業(yè)結束點更新單元更新后的所述作業(yè)結束點來執(zhí)行處理。
7.根據(jù)權利要求1?6中任一項所述的多關節(jié)型機器人的控制裝置，其中， 所述例外動作單元具備: 角度候補計算單元，其在較之于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自進行線性動作的情況下離該目標更近的角度的范圍內，計算多個該驅動軸各自的角度的候補；以及 角度選擇單元，其從由所述角度候補計算單元計算出的角度的候補之中，選擇將在采用了各該候補時的下一所述插補點的從所述作業(yè)部觀察的作業(yè)坐標系下的3個姿勢角度的分量當中預先設定的一個或兩個特定分量的變動抑制得最大的角度的候補。
8.一種多關節(jié)型機器人的控制方法，對多關節(jié)型機器人進行控制，所述多關節(jié)型機器人具備:第I關節(jié)驅動系統(tǒng)，其具有使設置于前端的作業(yè)部的姿勢進行變化的3個驅動軸；以及第2關節(jié)驅動系統(tǒng)，其具有使所述第I關節(jié)驅動系統(tǒng)的位置進行變化的至少3個驅動軸， 所述多關節(jié)型機器人的控制方法執(zhí)行如下工序: 插補點計算工序，計算多個插補點，所述插補點用于在對預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢進行連結的示教路徑上使所述作業(yè)部的位置以預先規(guī)定的速度進行移動； 驅動控制工序，遵照由所述插補點計算工序計算出的所述插補點來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動； 例外條件判定工序， 在由所述驅動控制工序對所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行了驅動時，判定與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件是否滿足；以及 例外動作工序，在由所述例外條件判定工序判定為所述例外條件滿足后，至所述作業(yè)部到達所述作業(yè)結束點為止的期間，計算用于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自線性地變化的該第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度，并基于該計算出的第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度和由所述插補點計算工序計算出的所述插補點處的所述作業(yè)部的位置來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸各自的角度，且遵照該計算結果來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動。
9.一種多關節(jié)型機器人的控制程序，對多關節(jié)型機器人進行控制，所述多關節(jié)型機器人具備:第I關節(jié)驅動系統(tǒng)，其具有使設置于前端的作業(yè)部的姿勢進行變化的3個驅動軸；以及第2關節(jié)驅動系統(tǒng)，其具有使所述第I關節(jié)驅動系統(tǒng)的位置進行變化的至少3個驅動軸， 所述多關節(jié)型機器人的控制程序使處理器執(zhí)行如下工序: 插補點計算工序，計算多個插補點，所述插補點用于在對預先設定的作業(yè)開始點以及作業(yè)結束點各自的位置以及姿勢進行連結的示教路徑上使所述作業(yè)部的位置以預先規(guī)定的速度進行移動； 驅動控制工序，遵照由所述插補點計算工序計算出的所述插補點來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行驅動； 例外條件判定工序，在由所述驅動控制工序對所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)進行了驅動時，判定與所述第I關節(jié)驅動系統(tǒng)的奇異姿勢的檢測條件不同的預先設定的例外條件是否滿足；以及 例外動作工序，在由所述例外條件判定工序判定為所述例外條件滿足后，至所述作業(yè)部到達所述作業(yè)結束點為止的期間，計算用于以所述作業(yè)結束點的角度為目標使所述第I關節(jié)驅動系統(tǒng)的驅動軸各自線性地變化的該第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度，并基于該計算出的第I關節(jié)驅動系統(tǒng)的驅動軸各自的角度和由所述插補點計算工序計算出的所述插補點處的所述作業(yè)部的位置來計算所述第2關節(jié)驅動系統(tǒng)的驅動軸各自的角度，且遵照該計算結果來使所述第I關節(jié)驅動系統(tǒng)以及所述第2關節(jié)驅動系統(tǒng)的驅動軸各自進行驅動 。
【文檔編號】B23K9/12GK103429397SQ201280011374
【公開日】2013年12月4日 申請日期:2012年3月2日 優(yōu)先權日:2011年3月8日
【發(fā)明者】大根努, 西村利彥, 重吉正之, 小池武 申請人:株式會社神戶制鋼所