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

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

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

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