機(jī)器人裝置及機(jī)器人控制方法
【專利摘要】本發(fā)明提供一種機(jī)器人裝置及機(jī)器人控制方法。機(jī)器人裝置(1)包括多關(guān)節(jié)機(jī)器人(2)以及基于輸入的運(yùn)動(dòng)命令對(duì)多關(guān)節(jié)機(jī)器人(2)進(jìn)行驅(qū)動(dòng)控制的控制器(3)?？刂破?3)包括計(jì)算用于基于所述運(yùn)動(dòng)命令驅(qū)動(dòng)多關(guān)節(jié)機(jī)器人(2)的各關(guān)節(jié)角命令的關(guān)節(jié)角計(jì)算單元(32)、通過基于由所述關(guān)節(jié)角計(jì)算單元(32)計(jì)算出的所述關(guān)節(jié)角命令對(duì)各旋轉(zhuǎn)關(guān)節(jié)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)來移動(dòng)多關(guān)節(jié)機(jī)器人(2)的伺服控制裝置(30)、計(jì)算多關(guān)節(jié)機(jī)器人(2)和多關(guān)節(jié)機(jī)器人(2)的奇異點(diǎn)之間的距離的奇異點(diǎn)計(jì)算單元(51)、以及在所述奇異點(diǎn)距離變?yōu)樾∮陬A(yù)定值的情況下限制基于奇異點(diǎn)類型預(yù)先指定的旋轉(zhuǎn)關(guān)節(jié)的最大旋轉(zhuǎn)速度的最大關(guān)節(jié)角偏差調(diào)整單元(52)。
【專利說明】機(jī)器人裝置及機(jī)器人控制方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種包括多關(guān)節(jié)機(jī)器人的機(jī)器人裝置和多關(guān)節(jié)機(jī)器人的控制方法。

【背景技術(shù)】
[0002] 近年來，使多關(guān)節(jié)機(jī)器人進(jìn)行諸如裝配工件的工作的機(jī)器人裝置日益發(fā)展。這種 機(jī)器人裝置的最終目的在于使多關(guān)節(jié)機(jī)器人像人手一樣移動(dòng)，并且使多關(guān)節(jié)機(jī)器人高速進(jìn) 行諸如裝配復(fù)雜工件的工作。
[0003] 令人遺憾地，多關(guān)節(jié)機(jī)器人具有多個(gè)姿態(tài)（即所謂的奇異點(diǎn)（singular point))， 在所述多個(gè)姿態(tài)中在三維空間中指定的一個(gè)位置和姿態(tài)無法被確定為關(guān)節(jié)空間中的一個(gè) 位置和姿態(tài)，而這些奇異點(diǎn)阻礙了多關(guān)節(jié)機(jī)器人以高速進(jìn)行復(fù)雜的移動(dòng)。因此，在機(jī)器人裝 置的開發(fā)中對(duì)這些奇異點(diǎn)進(jìn)行適當(dāng)?shù)奶幚硎侵匾摹?br> [0004] 在圖13中示出了對(duì)多關(guān)節(jié)機(jī)器人進(jìn)行控制的一般控制裝置的框圖。在圖13中， 當(dāng)前位置C i表示在三維空間中代表的機(jī)械手的手端梢的位置和姿態(tài)，偏差命令Vi表示手端 梢的位置和姿態(tài)針對(duì)預(yù)先決定的各控制周期（例如2ms)的偏差。因?yàn)榭刂浦芷谑呛愣ǖ模?所以偏差命令V i代表速度，并且使用示教器等針對(duì)各控制周期生成偏差命令
[0005] 在這種配置中，位置命令計(jì)算單元150將偏差命令Vi與當(dāng)前位置Ci相加，以獲得 下一位置C i+1，并且逆向運(yùn)動(dòng)學(xué)計(jì)算單元155對(duì)下一位置Ci+1進(jìn)行逆向運(yùn)動(dòng)學(xué)計(jì)算，以獲得 下一關(guān)節(jié)角命令q i+1。然后，將其發(fā)送給對(duì)多關(guān)節(jié)機(jī)器人的關(guān)節(jié)進(jìn)行控制的伺服控制裝置 130,由此對(duì)多關(guān)節(jié)機(jī)器人進(jìn)行驅(qū)動(dòng)控制。然而，如果多關(guān)節(jié)機(jī)器人位于奇異點(diǎn)附近，則關(guān)節(jié) 角命令q i+1從前一關(guān)節(jié)角命令qi顯著改變，因而多關(guān)節(jié)機(jī)器人可能會(huì)急劇移動(dòng)。
[0006] 為了應(yīng)對(duì)上述問題，能夠想到限制關(guān)節(jié)角命令速度的方法。然而，如果關(guān)節(jié)角命令 速度受限，則多關(guān)節(jié)機(jī)器人可能從手端梢的原始目標(biāo)位置顯著偏離，并且一旦多關(guān)節(jié)機(jī)器 人從其偏離，返回到原始目標(biāo)位置就變得很難。根據(jù)公開的應(yīng)對(duì)該問題的技術(shù)，如果關(guān)節(jié)角 速度過大，則針對(duì)減速進(jìn)行速度限制，由此防止多關(guān)節(jié)機(jī)器人在奇異點(diǎn)附近急劇移動(dòng)（日 本特開2003-300183號(hào)公報(bào)）。
[0007] 令人遺憾地，在日本特開2003-300183號(hào)中公開的技術(shù)中，總是限制關(guān)節(jié)角速度， 因此多關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)在奇異點(diǎn)之外的位置和姿態(tài)也變慢。


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

[0008] 鑒于上述情況，本發(fā)明的目的是提供能夠?qū)ζ娈慄c(diǎn)進(jìn)行適當(dāng)處理并且能夠提高多 關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)速度的機(jī)器人裝置及機(jī)器人控制方法。
[0009] 根據(jù)實(shí)施例的一個(gè)方面，提供一種機(jī)器人裝置，所述機(jī)器人裝置包括：多關(guān)節(jié)機(jī)器 人，其包括多個(gè)旋轉(zhuǎn)關(guān)節(jié)；以及控制裝置，其基于輸入的運(yùn)動(dòng)命令，對(duì)所述多關(guān)節(jié)機(jī)器人進(jìn) 行驅(qū)動(dòng)控制，其中，所述控制裝置包括：關(guān)節(jié)角計(jì)算單元，其計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各 個(gè)的關(guān)節(jié)角命令，所述關(guān)節(jié)角命令用于基于所述運(yùn)動(dòng)命令驅(qū)動(dòng)所述多關(guān)節(jié)機(jī)器人；關(guān)節(jié)驅(qū) 動(dòng)控制單元，其通過基于由所述關(guān)節(jié)角計(jì)算單元計(jì)算出的所述關(guān)節(jié)角命令對(duì)所述多個(gè)旋轉(zhuǎn) 關(guān)節(jié)中的各個(gè)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)，來移動(dòng)所述多關(guān)節(jié)機(jī)器人；奇異點(diǎn)計(jì)算單元，其針對(duì)所述多 關(guān)節(jié)機(jī)器人的奇異點(diǎn)，計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)的奇異點(diǎn)角度和奇異點(diǎn)距離中的一 個(gè)；以及關(guān)節(jié)速度限制單元，其在由所述奇異點(diǎn)計(jì)算單元計(jì)算出的所述奇異點(diǎn)角度和所述 奇異點(diǎn)距離中的一個(gè)變?yōu)樾∮陬A(yù)定值的情況下，限制基于所述奇異點(diǎn)的類型預(yù)先指定的旋 轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)速度。
[0010] 根據(jù)實(shí)施例的另一方面，提供一種機(jī)器人控制方法，所述機(jī)器人控制方法用于基 于輸入的運(yùn)動(dòng)命令對(duì)包括多個(gè)旋轉(zhuǎn)關(guān)節(jié)的多關(guān)節(jié)機(jī)器人進(jìn)行驅(qū)動(dòng)控制，所述機(jī)器人控制方 法包括：控制單元計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)的關(guān)節(jié)角命令，所述關(guān)節(jié)角命令用于基 于所述運(yùn)動(dòng)命令驅(qū)動(dòng)所述多關(guān)節(jié)機(jī)器人；所述控制單元通過基于在計(jì)算所述關(guān)節(jié)角命令中 計(jì)算出的所述關(guān)節(jié)角命令對(duì)所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)，來移動(dòng)所述多關(guān)節(jié) 機(jī)器人；所述控制單元針對(duì)所述多關(guān)節(jié)機(jī)器人的奇異點(diǎn)，計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè) 的奇異點(diǎn)角度和奇異點(diǎn)距離中的一個(gè)；以及在計(jì)算所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的 一個(gè)中計(jì)算出的所述奇異點(diǎn)角度和所計(jì)算出的奇異點(diǎn)距離中的一個(gè)變?yōu)樾∮陬A(yù)定值的情 況下，所述控制單元限制基于所述奇異點(diǎn)的類型預(yù)先指定的旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)速度。
[0011] 本發(fā)明能夠提供一種機(jī)器人裝置和機(jī)器人控制方法，其能夠?qū)ζ娈慄c(diǎn)進(jìn)行適當(dāng)處 理，并且能夠提1?多關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)速度。
[0012] 通過以下參照附圖對(duì)示例性實(shí)施例的描述，本發(fā)明的其他特征將變得清楚。

【專利附圖】

【附圖說明】
[0013] 圖1是示意性地示出根據(jù)本發(fā)明的第一實(shí)施例的機(jī)器人裝置的總體結(jié)構(gòu)的立體 圖。
[0014] 圖2是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂的第一奇異點(diǎn)的說明圖。
[0015] 圖3是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂的第二奇異點(diǎn)的說明圖。
[0016] 圖4是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂的第三奇異點(diǎn)的說明圖。
[0017] 圖5是示出根據(jù)第一實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0018] 圖6是用于描述計(jì)算最大關(guān)節(jié)角偏差的函數(shù)的說明圖。
[0019] 圖7是用于描述根據(jù)各個(gè)奇異點(diǎn)類型要進(jìn)行速度限制的關(guān)節(jié)軸的說明圖。
[0020] 圖8是示出根據(jù)第二實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0021] 圖9是示出根據(jù)第三實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0022] 圖10是示出根據(jù)第四實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0023] 圖11是示出根據(jù)第五實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0024] 圖12是示出根據(jù)第六實(shí)施例的關(guān)節(jié)控制裝置的配置的框圖。
[0025] 圖13是根據(jù)傳統(tǒng)技術(shù)控制多關(guān)節(jié)機(jī)器人的控制裝置的框圖。

【具體實(shí)施方式】
[0026] 現(xiàn)在，將根據(jù)附圖詳細(xì)描述本發(fā)明的優(yōu)選實(shí)施例。
[0027] 〈第一實(shí)施例〉
[0028] 下文中，參照?qǐng)D1至圖7描述根據(jù)本發(fā)明的第一實(shí)施例的機(jī)器人裝置。首先，參照 圖1描述根據(jù)第一實(shí)施例的整個(gè)機(jī)器人裝置1的示意性配置。圖1是示意性地示出根據(jù)本 發(fā)明的第一實(shí)施例的機(jī)器人裝置1的總體結(jié)構(gòu)的圖。
[0029] 如圖1所示，機(jī)器人裝置1包括：進(jìn)行工件裝配的6軸垂直多關(guān)節(jié)機(jī)器人（下文中 稱為"多關(guān)節(jié)機(jī)器人")2、對(duì)多關(guān)節(jié)機(jī)器人2進(jìn)行控制的控制器3、以及能夠用于操作多關(guān)節(jié) 機(jī)器人2的示教器4。
[0030] 多關(guān)節(jié)機(jī)器人（多關(guān)節(jié)機(jī)械手）2包括6軸多關(guān)節(jié)機(jī)器人臂20和連接到機(jī)器人臂 20的遠(yuǎn)端的末端執(zhí)行器21。機(jī)器人臂20包括6個(gè)旋轉(zhuǎn)關(guān)節(jié)（Jl至J6)以及分別關(guān)于關(guān)節(jié) 軸（Jl軸至J6軸）旋轉(zhuǎn)驅(qū)動(dòng)的6個(gè)致動(dòng)器（在本實(shí)施例中為伺服機(jī)構(gòu)）。機(jī)器人臂20選 擇性地驅(qū)動(dòng)各關(guān)節(jié)的伺服機(jī)構(gòu)，由此將末端執(zhí)行器21移動(dòng)到期望的三維位置。末端執(zhí)行器 21可拆卸地附裝到機(jī)器人臂20的遠(yuǎn)端，并且基于工作的內(nèi)容是可替換的。在本實(shí)施例中使 用的末端執(zhí)行器是能夠用3個(gè)手指抓握工件的所謂的手。
[0031] 控制器（控制裝置）3包括：對(duì)機(jī)器人臂20的各關(guān)節(jié)的伺服機(jī)構(gòu)進(jìn)行驅(qū)動(dòng)控制的 伺服控制裝置（參見稍后要描述的圖5) 30、以及對(duì)機(jī)器人臂20的關(guān)節(jié)進(jìn)行控制的關(guān)節(jié)控制 裝置（參見稍后要描述的圖5)31。此外，控制器3包括對(duì)末端執(zhí)行器21進(jìn)行控制的末端執(zhí) 行器控制裝置、存儲(chǔ)單元、記錄介質(zhì)讀取裝置以及通信裝置。
[0032] 關(guān)節(jié)控制裝置31對(duì)各旋轉(zhuǎn)關(guān)節(jié)進(jìn)行控制，使得當(dāng)機(jī)器人臂20接近奇異點(diǎn)時(shí)，機(jī)器 人臂20在避免在特定旋轉(zhuǎn)關(guān)節(jié)處急劇移動(dòng)的同時(shí)移動(dòng)。注意，稍后詳細(xì)描述關(guān)節(jié)控制裝置 31。伺服控制裝置（關(guān)節(jié)驅(qū)動(dòng)控制單元）30基于由關(guān)節(jié)控制裝置31計(jì)算出的關(guān)節(jié)角命令 (稍后要描述），對(duì)各旋轉(zhuǎn)關(guān)節(jié)的伺服機(jī)構(gòu)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)，由此移動(dòng)機(jī)器人臂20。
[0033] 存儲(chǔ)單元在其中存儲(chǔ)用于執(zhí)行機(jī)器人控制方法（稍后要描述）的程序（機(jī)器人控 制程序）以及諸如機(jī)器人臂20的奇異點(diǎn)和用戶預(yù)先設(shè)置的初始教示點(diǎn)的數(shù)據(jù)。記錄介質(zhì) 讀取裝置用于讀取在其中記錄諸如機(jī)器人控制程序的各種程序的計(jì)算機(jī)可讀記錄介質(zhì)的 內(nèi)容，并且將記錄在記錄介質(zhì)中的程序和數(shù)據(jù)存儲(chǔ)到存儲(chǔ)單元中。通信裝置例如用于在不 使用上述記錄介質(zhì)的情況下，通過通信裝置下載經(jīng)由因特網(wǎng)分發(fā)的更新程序。
[0034] 示教器（操作單元）4是針對(duì)用戶的人機(jī)接口，并且由用戶用來操作多關(guān)節(jié)機(jī)器人 2。此外，示教器4具有稱為點(diǎn)動(dòng)操作的功能。在點(diǎn)動(dòng)操作中，例如，能夠在用戶按下按鈕的 同時(shí)，在三維坐標(biāo)中線性地移動(dòng)多關(guān)節(jié)機(jī)器人2的手端梢位置。因此，多關(guān)節(jié)機(jī)器人2可以 在點(diǎn)動(dòng)操作期間接近奇異點(diǎn)，并且如果多關(guān)節(jié)機(jī)器人2接近奇異點(diǎn)，則特定關(guān)節(jié)可能急劇 移動(dòng)。在本實(shí)施例中，即使在多關(guān)節(jié)機(jī)器人2接近奇異點(diǎn)的情況下，關(guān)節(jié)控制裝置31也使 得能夠在避免特定關(guān)節(jié)的急劇移動(dòng)的同時(shí)，驅(qū)動(dòng)機(jī)器人臂20。
[0035] 接下來，參照?qǐng)D2至圖7具體描述關(guān)節(jié)控制裝置31。首先，參照?qǐng)D2至圖4描述機(jī) 器人臂20的奇異點(diǎn)和奇異點(diǎn)距離。圖2是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂20的第一 奇異點(diǎn)的說明圖。圖3是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂20的第二奇異點(diǎn)的說明圖。 圖4是用于描述根據(jù)第一實(shí)施例的機(jī)器人臂20的第三奇異點(diǎn)的說明圖。
[0036] 通常，多關(guān)節(jié)機(jī)器人（多關(guān)節(jié)機(jī)械手）具有針對(duì)特定手端梢位置無法計(jì)算關(guān)節(jié)角 的大量奇異點(diǎn)。例如，已知6軸多關(guān)節(jié)機(jī)器人2具有3個(gè)奇異點(diǎn)（第一奇異點(diǎn)至第三奇異 點(diǎn)），并且多關(guān)節(jié)機(jī)器人2可以同時(shí)接近這3個(gè)奇異點(diǎn)。下文中，描述多關(guān)節(jié)機(jī)器人2的這 3個(gè)奇異點(diǎn)。
[0037] 注意，下面，為了便于描述，將Jl軸方向稱為垂直方向，并且將與Jl軸方向正交的 方向稱為水平方向。此外，一般的工業(yè)機(jī)器人被設(shè)計(jì)為使得Jl軸方向與J2軸方向正交，并 且J2軸方向與J3軸方向平行，因此基于這種條件給出描述。也可以采用J2軸相對(duì)于Jl 軸沿水平方向偏移的配置，但是可以以類似的方式對(duì)這種配置進(jìn)行討論，因此這里省略其 描述。此外，通常，在許多情況下，J4軸、J5軸和J6軸被設(shè)計(jì)為在一個(gè)點(diǎn)（圖2至圖4所示 的點(diǎn)P)處彼此相交，以便于進(jìn)行逆向運(yùn)動(dòng)學(xué)（inverse kinematics)計(jì)算，因此基于這種條 件給出描述。
[0038] 如圖2所示，第一奇異點(diǎn)對(duì)應(yīng)于J4軸、J5軸和J6軸彼此相交的點(diǎn)（即點(diǎn)P位于 Jl軸上的情況）。在第一奇異點(diǎn)處，Jl軸、J4軸、J5軸和J6軸在一個(gè)點(diǎn)（即點(diǎn)P)處彼此 相交，因此4個(gè)旋轉(zhuǎn)關(guān)節(jié)Jl以及J4至J6的旋轉(zhuǎn)角度不確定。這是因?yàn)樾D(zhuǎn)角度由三維空 間中的3種類型的方向（變量）確定，而在第一奇異點(diǎn)處，變量的數(shù)量是4,其比3大1。在 第一奇異點(diǎn)附近，手端梢的位置和姿態(tài)的微小改變導(dǎo)致旋轉(zhuǎn)關(guān)節(jié)Jl以及J4至J6的角度的 顯著改變。
[0039] 此外，假設(shè)將點(diǎn)P定位到Jl軸上所需的旋轉(zhuǎn)關(guān)節(jié)J2的旋轉(zhuǎn)角度是δ 1，旋轉(zhuǎn)角度 S1是示出到第一奇異點(diǎn)的距離的代表指標(biāo)。因此，在本實(shí)施例中，特定旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)角 度（下文中稱為"奇異點(diǎn)角度"）被用作奇異點(diǎn)距離。根據(jù)經(jīng)驗(yàn)，如果旋轉(zhuǎn)關(guān)節(jié)J2的旋轉(zhuǎn)角 度（奇異點(diǎn)角度）S 1落到15度以下，則手端梢的位置和姿態(tài)的微小改變使旋轉(zhuǎn)關(guān)節(jié)Jl以 及J4至J6的旋轉(zhuǎn)角度顯著改變，這需要關(guān)注。
[0040] 如圖3所示，第二奇異點(diǎn)對(duì)應(yīng)于J4軸和J6軸位于同一軸上的情況。在第二奇異 點(diǎn)處，J4軸和J6軸彼此一致，因此旋轉(zhuǎn)關(guān)節(jié)J4和J6的旋轉(zhuǎn)角度無法被確定為一個(gè)。在第 二奇異點(diǎn)附近，手端梢的位置和姿態(tài)的微小改變導(dǎo)致旋轉(zhuǎn)關(guān)節(jié)J4和J6的角度的顯著改變。 此外，假設(shè)將J4軸和J6軸定位到同一軸上所需的旋轉(zhuǎn)關(guān)節(jié)J5的旋轉(zhuǎn)角度是δ 2,根據(jù)經(jīng) 驗(yàn)，如果旋轉(zhuǎn)關(guān)節(jié)J2的旋轉(zhuǎn)角度（奇異點(diǎn)角度）δ 2落到15度以下，則手端梢的位置和姿 態(tài)的微小改變使旋轉(zhuǎn)關(guān)節(jié)J4和J6的旋轉(zhuǎn)角度顯著改變，這需要關(guān)注。
[0041] 如圖4所示，第三奇異點(diǎn)對(duì)應(yīng)于J2軸和J3軸位于J4軸上的情況。在第三奇異點(diǎn) 處，因?yàn)镴2軸和J3軸位于J4軸上，因此無法確定旋轉(zhuǎn)關(guān)節(jié)J2和J3的旋轉(zhuǎn)角度。在第三奇 異點(diǎn)附近，手端梢的位置和姿態(tài)的微小改變導(dǎo)致旋轉(zhuǎn)關(guān)節(jié)J2和J3的角度的顯著改變。此 夕卜，假設(shè)將J2軸和J3軸定位到J4軸上所需的旋轉(zhuǎn)關(guān)節(jié)J3的旋轉(zhuǎn)角度是δ 3,根據(jù)經(jīng)驗(yàn)，如 果旋轉(zhuǎn)關(guān)節(jié)J2的旋轉(zhuǎn)角度（奇異點(diǎn)角度）δ 3落到15度以下，則手端梢的位置和姿態(tài)的微 小改變使旋轉(zhuǎn)關(guān)節(jié)J2和J3的旋轉(zhuǎn)角度顯著改變，這需要關(guān)注。
[0042] 接下來，參照?qǐng)D5,描述當(dāng)機(jī)器人臂20接近這3個(gè)奇異點(diǎn)時(shí)，能夠在抑制機(jī)器人臂 20的急劇移動(dòng)的同時(shí)，對(duì)機(jī)器人臂20進(jìn)行驅(qū)動(dòng)控制的關(guān)節(jié)控制裝置31。圖5是示出根據(jù) 第一實(shí)施例的關(guān)節(jié)控制裝置31的配置的框圖。
[0043] 首先，描述關(guān)節(jié)控制裝置31的配置。如圖5所示，關(guān)節(jié)控制裝置31包括：基于運(yùn) 動(dòng)命令計(jì)算用于驅(qū)動(dòng)機(jī)器人臂20的各旋轉(zhuǎn)關(guān)節(jié)的關(guān)節(jié)角命令的關(guān)節(jié)角計(jì)算單元32、以及 計(jì)算各旋轉(zhuǎn)關(guān)節(jié)對(duì)各奇異點(diǎn)的奇異點(diǎn)角度的奇異點(diǎn)計(jì)算單元51。關(guān)節(jié)控制裝置31還包括 ： 進(jìn)行根據(jù)奇異點(diǎn)角度使針對(duì)各旋轉(zhuǎn)關(guān)節(jié)預(yù)先給定的最大關(guān)節(jié)角偏差（最大旋轉(zhuǎn)速度）的極 限值更小的處理的最大關(guān)節(jié)角偏差調(diào)整單元52。
[0044] 關(guān)節(jié)角計(jì)算單元32包括位置命令計(jì)算單元50、校正水平調(diào)整單元53、位置 命令校正單元54、逆向運(yùn)動(dòng)學(xué)計(jì)算單元55、關(guān)節(jié)角校正單元56、正向運(yùn)動(dòng)學(xué)（forward kinematics)計(jì)算單元57以及校正量計(jì)算單元58。
[0045] 位置命令計(jì)算單元50將機(jī)器人臂20的手端梢的偏差命令與由三維坐標(biāo)表示的其 手端梢的當(dāng)前位置相加，由此計(jì)算位置命令。校正水平調(diào)整單元53進(jìn)行根據(jù)奇異點(diǎn)角度來 進(jìn)行計(jì)算校正水平（其是校正的比率，并且下文中將其稱為"校正比率"）的處理。具體來 說，校正水平調(diào)整單元53進(jìn)行隨著奇異點(diǎn)角度越小使校正比率越小的處理。位置命令校正 單元54將前一校正量和校正比率的積，與由位置命令計(jì)算單元50計(jì)算出的位置命令相加， 由此計(jì)算校正后的位置命令。逆向運(yùn)動(dòng)學(xué)計(jì)算單元55將由位置命令校正單元54校正后的 位置命令變換為臨時(shí)（tentative)關(guān)節(jié)角命令。關(guān)節(jié)角校正單元56根據(jù)由逆向運(yùn)動(dòng)學(xué)計(jì) 算單元55計(jì)算出的臨時(shí)關(guān)節(jié)角命令和前一關(guān)節(jié)角命令，來計(jì)算關(guān)節(jié)角偏差，并且使臨時(shí)關(guān) 節(jié)角命令更小，使得關(guān)節(jié)角偏差不超過由最大關(guān)節(jié)角偏差調(diào)整單元52計(jì)算出的最大關(guān)節(jié) 角偏差，由此計(jì)算下一關(guān)節(jié)角命令。正向運(yùn)動(dòng)學(xué)計(jì)算單元57根據(jù)修正后的關(guān)節(jié)角命令，計(jì) 算下一關(guān)節(jié)角命令和下一位置。校正量計(jì)算單元58將校正量與由正向運(yùn)動(dòng)學(xué)計(jì)算單元57 計(jì)算出的下一位置和位置命令之差相加，由此計(jì)算下一校正量。
[0046] 如果基于奇異點(diǎn)類型預(yù)先指定的旋轉(zhuǎn)關(guān)節(jié)的奇異點(diǎn)角度變?yōu)樾∮陬A(yù)定值，則最大 關(guān)節(jié)角偏差調(diào)整單元（關(guān)節(jié)速度限制單元）52使最大關(guān)節(jié)角偏差（最大旋轉(zhuǎn)速度）的極限 值小于預(yù)先設(shè)置的最大關(guān)節(jié)角偏差的極限值。
[0047] 接下來，除了圖5,還參照?qǐng)D6和圖7,描述如上所述構(gòu)成的關(guān)節(jié)控制裝置31進(jìn)行 的對(duì)機(jī)器人臂20的驅(qū)動(dòng)控制（機(jī)器人控制方法）。注意，下面給出的各標(biāo)記的下標(biāo)i代表 計(jì)算的次數(shù)，并且每次一系列計(jì)算結(jié)束時(shí)將下標(biāo)i遞增1。此外，針對(duì)例如2ms的各控制周 期進(jìn)行該計(jì)算。圖6是用于描述計(jì)算最大關(guān)節(jié)角偏差的函數(shù)的說明圖。圖7是用于描述根 據(jù)各奇異點(diǎn)類型要進(jìn)行速度限制的關(guān)節(jié)軸的說明圖。
[0048] 如果向示教器4輸入運(yùn)動(dòng)命令，則基于輸入的運(yùn)動(dòng)命令，從示教器4向關(guān)節(jié)控制裝 置31輸入偏差命令當(dāng)前位置C i、前一校正量Ei和前一關(guān)節(jié)角命令qi。因此，關(guān)節(jié)控制 裝置31基于輸入的數(shù)據(jù)，計(jì)算下一關(guān)節(jié)角命令q i+1、下一位置Ci+1和下一校正量Ei+1，并且向 伺服控制裝置30輸出計(jì)算結(jié)果。伺服控制裝置30基于輸入的數(shù)據(jù)對(duì)旋轉(zhuǎn)關(guān)節(jié)進(jìn)行旋轉(zhuǎn)驅(qū) 動(dòng)，由此移動(dòng)機(jī)器人臂20。下文中，具體描述關(guān)節(jié)控制裝置31計(jì)算下一關(guān)節(jié)角命令q i+1、下 一位置Ci+1和下一校正量Ei+1的方法。
[0049] 如果從示教器4輸入了偏差命令當(dāng)前位置Ci、前一校正量Ei和前一關(guān)節(jié)角命 令1，則首先，關(guān)節(jié)控制裝置31將手端梢的偏差命令V i和手端梢的當(dāng)前位置Ci相加，由此 計(jì)算位置命令Di (位置命令計(jì)算步驟）。這里，當(dāng)前位置數(shù)據(jù)和偏差命令數(shù)據(jù)是具有由X、Y 和Z方向上的運(yùn)動(dòng)和旋轉(zhuǎn)構(gòu)成的6個(gè)分量的矢量，并代表位置和姿態(tài)。代表該位置和姿態(tài) 的其他方法涉及使用4X4齊次坐標(biāo)變換矩陣。在這種方法中，通過矩陣乘法表示矢量的總 和。這僅在如何表示位置和姿態(tài)中不同，而與本發(fā)明的要旨無關(guān)。因此，在本實(shí)施例中，使 用容易直觀地理解的矢量表達(dá)式用于表示位置和姿態(tài)。
[0050] 隨后，關(guān)節(jié)控制裝置31計(jì)算奇異點(diǎn)角度，并且確定機(jī)器人臂20是否在奇異點(diǎn)附近 (奇異點(diǎn)計(jì)算步驟）。在本實(shí)施例中，關(guān)節(jié)控制裝置31根據(jù)三種類型的奇異點(diǎn)，來計(jì)算相應(yīng) 旋轉(zhuǎn)關(guān)節(jié)的奇異點(diǎn)角度。注意，可以使用當(dāng)前關(guān)節(jié)角作為相應(yīng)旋轉(zhuǎn)關(guān)節(jié)的奇異點(diǎn)角度的代 替。這是因?yàn)楦鶕?jù)本實(shí)施例的多關(guān)節(jié)機(jī)器人2的控制周期為非常短時(shí)間的2ms，并且關(guān)節(jié)角 的改變也小。
[0051] 在計(jì)算奇異點(diǎn)角度之后，關(guān)節(jié)控制裝置31然后進(jìn)行根據(jù)奇異點(diǎn)角度，使針對(duì)各旋 轉(zhuǎn)關(guān)節(jié)預(yù)先給出的最大關(guān)節(jié)角偏差（最大旋轉(zhuǎn)速度）的極限值更小的處理（關(guān)節(jié)速度調(diào)整 步驟）。這里，基于電機(jī)產(chǎn)生的轉(zhuǎn)矩和編碼器能夠處置的最大旋轉(zhuǎn)速度的限制，針對(duì)各旋轉(zhuǎn) 關(guān)節(jié)確定最大旋轉(zhuǎn)速度（即最大關(guān)節(jié)角偏差的極限值）。在這種情況下，根據(jù)奇異點(diǎn)角度， 使其極限值更小。
[0052] 例如，如圖6所示，假設(shè)針對(duì)各旋轉(zhuǎn)關(guān)節(jié)給出的最大旋轉(zhuǎn)角度偏差（最大旋轉(zhuǎn)速 度）是V max，奇異點(diǎn)角度為零時(shí)的旋轉(zhuǎn)角度偏差是Vmin，并且確定奇異點(diǎn)附近的角度是0S。 根據(jù)下面的表達(dá)式計(jì)算針對(duì)奇異點(diǎn)角度Θ的減速的最大旋轉(zhuǎn)角度偏差V lim。
[0053] [表達(dá)式1]

【權(quán)利要求】
1. 一種機(jī)器人裝置，所述機(jī)器人裝置包括： 多關(guān)節(jié)機(jī)器人，其包括多個(gè)旋轉(zhuǎn)關(guān)節(jié)；以及 控制裝置，其基于輸入的運(yùn)動(dòng)命令，對(duì)所述多關(guān)節(jié)機(jī)器人進(jìn)行驅(qū)動(dòng)控制， 其中，所述控制裝置包括： 關(guān)節(jié)角計(jì)算單元，其計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)的關(guān)節(jié)角命令，所述關(guān)節(jié)角命令 用于基于所述運(yùn)動(dòng)命令驅(qū)動(dòng)所述多關(guān)節(jié)機(jī)器人； 關(guān)節(jié)驅(qū)動(dòng)控制單元，其通過基于由所述關(guān)節(jié)角計(jì)算單元計(jì)算出的所述關(guān)節(jié)角命令對(duì)所 述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)，來使所述多關(guān)節(jié)機(jī)器人移動(dòng)； 奇異點(diǎn)計(jì)算單元，其針對(duì)所述多關(guān)節(jié)機(jī)器人的奇異點(diǎn)計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè) 的奇異點(diǎn)角度和奇異點(diǎn)距離中的一個(gè)；以及 關(guān)節(jié)速度限制單元，其在由所述奇異點(diǎn)計(jì)算單元計(jì)算出的所述奇異點(diǎn)角度和所述奇異 點(diǎn)距離中的所述一個(gè)變?yōu)樾∮陬A(yù)定值的情況下，限制基于所述奇異點(diǎn)的類型預(yù)先指定的旋 轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)速度。
2. 根據(jù)權(quán)利要求1所述的機(jī)器人裝置，其中，在所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中 的所述一個(gè)變?yōu)樾∮谒鲱A(yù)定值的情況下，所述關(guān)節(jié)速度限制單元根據(jù)所述奇異點(diǎn)角度和 所述奇異點(diǎn)距離中的所述一個(gè)，使用于限制所指定的旋轉(zhuǎn)關(guān)節(jié)的所述旋轉(zhuǎn)速度的極限值更 小。
3. 根據(jù)權(quán)利要求1所述的機(jī)器人裝置，其中，所述關(guān)節(jié)角計(jì)算單元包括： 位置命令計(jì)算單元，其基于所述運(yùn)動(dòng)命令計(jì)算位置命令； 校正水平調(diào)整單元，其計(jì)算校正比率，所述校正比率是根據(jù)由所述奇異點(diǎn)計(jì)算單元計(jì) 算出的所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的所述一個(gè)來進(jìn)行校正的比率； 位置命令校正單元，其通過將前一校正量和由所述校正水平調(diào)整單元計(jì)算出的所述校 正比率的乘積與所述位置命令相加，來對(duì)所述位置命令進(jìn)行校正； 逆向運(yùn)動(dòng)學(xué)計(jì)算單元，其將由所述位置命令校正單元校正的所述位置命令變換為臨時(shí) 關(guān)節(jié)角命令； 關(guān)節(jié)角校正單元，其通過根據(jù)所述臨時(shí)關(guān)節(jié)角命令和前一關(guān)節(jié)角命令計(jì)算關(guān)節(jié)角偏 差，并且以所述關(guān)節(jié)角偏差不超過所述旋轉(zhuǎn)速度的方式使所述臨時(shí)關(guān)節(jié)角命令更小，來計(jì) 算下一關(guān)節(jié)角命令； 正向運(yùn)動(dòng)學(xué)計(jì)算單元，其根據(jù)由所述關(guān)節(jié)角校正單元計(jì)算出的所述下一關(guān)節(jié)角命令， 來計(jì)算下一位置；以及 校正量計(jì)算單元，其通過將所述前一校正量與所述下一位置和所述位置命令之差相 力口，來計(jì)算下一校正量。
4. 一種機(jī)器人控制方法，所述機(jī)器人控制方法用于基于輸入的運(yùn)動(dòng)命令對(duì)包括多個(gè)旋 轉(zhuǎn)關(guān)節(jié)的多關(guān)節(jié)機(jī)器人進(jìn)行驅(qū)動(dòng)控制，所述機(jī)器人控制方法包括以下步驟： 控制單元計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)的關(guān)節(jié)角命令，所述關(guān)節(jié)角命令用于基于所 述運(yùn)動(dòng)命令驅(qū)動(dòng)所述多關(guān)節(jié)機(jī)器人； 所述控制單元通過基于在計(jì)算所述關(guān)節(jié)角命令中計(jì)算出的所述關(guān)節(jié)角命令對(duì)所述多 個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)進(jìn)行旋轉(zhuǎn)驅(qū)動(dòng)，來使所述多關(guān)節(jié)機(jī)器人移動(dòng)； 所述控制單元針對(duì)所述多關(guān)節(jié)機(jī)器人的奇異點(diǎn)計(jì)算所述多個(gè)旋轉(zhuǎn)關(guān)節(jié)中的各個(gè)的奇 異點(diǎn)角度和奇異點(diǎn)距離中的一個(gè)；以及 在計(jì)算所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的一個(gè)中計(jì)算出的所述奇異點(diǎn)角度和所 計(jì)算出的奇異點(diǎn)距離中的所述一個(gè)變?yōu)樾∮陬A(yù)定值的情況下，所述控制單元限制基于所述 奇異點(diǎn)的類型預(yù)先指定的旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)速度。
5. 根據(jù)權(quán)利要求4所述的機(jī)器人控制方法，其中， 限制所述旋轉(zhuǎn)速度包括：在所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的所述一個(gè)變?yōu)樾∮?所述預(yù)定值的情況下，根據(jù)所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的所述一個(gè)，使用于限制 所指定的旋轉(zhuǎn)關(guān)節(jié)的所述旋轉(zhuǎn)速度的極限值更小。
6. 根據(jù)權(quán)利要求4所述的機(jī)器人控制方法，其中，計(jì)算所述關(guān)節(jié)角命令包括： 基于所述運(yùn)動(dòng)命令計(jì)算位置命令； 計(jì)算校正比率，所述校正比率是根據(jù)在計(jì)算所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的一 個(gè)中計(jì)算出的所述奇異點(diǎn)角度和所述奇異點(diǎn)距離中的一個(gè)來進(jìn)行校正的比率； 通過將前一校正量和在計(jì)算所述校正比率中計(jì)算出的所述校正比率的乘積與所述位 置命令相加，來對(duì)所述位置命令進(jìn)行校正； 通過將在校正所述位置命令中校正的所述位置命令變換為臨時(shí)關(guān)節(jié)角命令，來進(jìn)行逆 向運(yùn)動(dòng)學(xué)計(jì)算； 通過根據(jù)所述臨時(shí)關(guān)節(jié)角命令和前一關(guān)節(jié)角命令計(jì)算關(guān)節(jié)角偏差，并且以所述關(guān)節(jié)角 偏差不超過所述旋轉(zhuǎn)速度的方式使所述臨時(shí)關(guān)節(jié)角命令更小，來計(jì)算下一關(guān)節(jié)角命令； 通過根據(jù)在計(jì)算所述下一關(guān)節(jié)角命令中計(jì)算出的所述下一關(guān)節(jié)角偏差計(jì)算下一位置， 來進(jìn)行正向運(yùn)動(dòng)學(xué)計(jì)算；以及 通過將所述前一校正量與所述下一位置和所述位置命令之差相加，來計(jì)算下一校正 量。
7. 根據(jù)權(quán)利要求6所述的機(jī)器人控制方法，其中，進(jìn)行所述正向運(yùn)動(dòng)學(xué)計(jì)算包括通過 如下步驟來計(jì)算所述下一位置：獲得雅克比矩陣；將所述雅克比矩陣乘以所述前一關(guān)節(jié)角 命令和在計(jì)算所述下一關(guān)節(jié)角命令中計(jì)算出的所述下一關(guān)節(jié)角命令之差；以及將當(dāng)前位置 與獲得的積相加。
8. 根據(jù)權(quán)利要求6所述的機(jī)器人控制方法，所述機(jī)器人控制方法還包括： 從計(jì)算所述下一校正量的計(jì)算結(jié)果中去除偏差命令的方向分量。
9. 根據(jù)權(quán)利要求6所述的機(jī)器人控制方法，其中， 限制所述旋轉(zhuǎn)速度包括：除計(jì)算所述旋轉(zhuǎn)速度之外，還計(jì)算最大旋轉(zhuǎn)速度改變，以及 計(jì)算所述下一關(guān)節(jié)角命令包括通過以下步驟來計(jì)算所述下一關(guān)節(jié)角命令：根據(jù)所述臨 時(shí)關(guān)節(jié)角命令、所述前一關(guān)節(jié)角命令以及所述前一關(guān)節(jié)角命令之前的關(guān)節(jié)角命令，計(jì)算關(guān) 節(jié)角偏差改變；以所述關(guān)節(jié)角偏差改變不超過最大關(guān)節(jié)角偏差改變的方式使所述臨時(shí)關(guān)節(jié) 角命令更小；根據(jù)所述臨時(shí)關(guān)節(jié)角命令和所述前一關(guān)節(jié)角命令，進(jìn)一步計(jì)算所述關(guān)節(jié)角偏 差；以及以所述關(guān)節(jié)角偏差不超過最大關(guān)節(jié)角偏差的方式使所述臨時(shí)關(guān)節(jié)角命令更小。
10. 根據(jù)權(quán)利要求6所述的機(jī)器人控制方法，所述機(jī)器人控制方法還包括： 以預(yù)定比率對(duì)計(jì)算所述下一校正量的計(jì)算結(jié)果進(jìn)行衰減計(jì)算。
【文檔編號(hào)】G05B19/416GK104339349SQ201410379795
【公開日】2015年2月11日 申請(qǐng)日期:2014年8月4日 優(yōu)先權(quán)日:2013年8月6日
【發(fā)明者】根岸真人 申請(qǐng)人:佳能株式會(huì)社