專利名稱：加減速控制方法及裝置及加減速控制方法的程序的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及例如具有關(guān)節(jié)的機械的關(guān)節(jié)驅(qū)動電機的加減速控制方法、裝置等。特別是涉及用于控制例如在垂直多關(guān)節(jié)機器人等中受到重力的影響的關(guān)節(jié)驅(qū)動電機的加減速控制方法、裝置等。
背景技術(shù)：
在例如多關(guān)節(jié)型的機器人(以下，稱為機械手)中，存在如下所述問題關(guān)節(jié)驅(qū)動電機(以下，稱為電機)的扭矩因重力作用于關(guān)節(jié)部上的重力扭矩而消耗，從而產(chǎn)生扭矩飽和，或者在定位時殘留有振動。為了解決該問題，必須考慮作用于多關(guān)節(jié)型機械手的各關(guān)節(jié)上的重力的影響，調(diào)整加速以及/或者減速(以下，稱為加減速，稱其速度為加減速度)。
一直以來，提出了如下方法對應(yīng)于機械手的姿勢來實時地計算重力扭矩，從而前饋地消除重力的影響(參照專利文獻1)。該方法具體如下述從機械手的姿勢信息來計算作用于關(guān)節(jié)上的重力扭矩，然后將其從電機的驅(qū)動扭矩指令中減去，以此來消除重力的影響。又，也提出了如下方法利用機械手的指示點的位置、姿勢或者各軸的驅(qū)動方向、驅(qū)動速度，基于由運動方程式導出的計算式，來調(diào)整加減速時的常數(shù)(參照專利文獻2)。
專利文獻1特開平10-244482號公報專利文獻2特開平7-261822號公報但是，在上述前饋地消除重力的影響這一方法中，因為只將基于姿勢而算出的重力扭矩減去從而消除重力的影響，所以并不進行對應(yīng)于驅(qū)動方向、狀況的消除。特別是，即使要一律地消除重力扭矩的影響，也不可能分別相對于例如受到重力扭矩的影響的扭矩飽和、殘留振動而進行調(diào)整，以達到消除的目的。又，就調(diào)整加減速時常數(shù)的方法而言，用于求解運動方程式的計算量多，且為了進行實時的處理必須使用處理能力(CPU能力)高的運算機構(gòu)。

發(fā)明內(nèi)容
因此，本發(fā)明的目的在于提供一種以簡單的順序、對應(yīng)于驅(qū)動狀態(tài)(特別是電機的驅(qū)動方向以及加減速等的狀況)地決定電機的加減速度的加減速控制方法、裝置等。
本發(fā)明的加減速控制方法，具有如下工序算出作用于使具有關(guān)節(jié)的機械動作的關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序、與基于預先設(shè)定的多個重力補償系數(shù)中基于由重力所影響的驅(qū)動狀態(tài)而選擇的重力補償系數(shù)與重力扭矩來算出加減速度修正系數(shù)的工序、與基于加減速度修正系數(shù)和設(shè)定的基本加減速度來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
根據(jù)本發(fā)明，因為基于算出的重力扭矩與從預先設(shè)定的多個重力補償系數(shù)中選擇的重力補償系數(shù)來算出加減速度修正系數(shù)，基于加減速度修正系數(shù)與設(shè)定的基本加減速度來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度，所以可以在運算重力扭矩之后，利用簡單的順序來算出與因驅(qū)動狀態(tài)而不同的重力的影響(例如扭矩飽和、殘留振動等)適當?shù)貙?yīng)的加減速度。以此，就不需要高速的運算處理，從而不使用高價的處理機構(gòu)亦可。
又，本發(fā)明的加減速控制方法，為了關(guān)節(jié)驅(qū)動電機的扭矩飽和防止以及殘留振動抑制，基于關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同的驅(qū)動狀態(tài)來設(shè)定重力補償系數(shù)。
根據(jù)本發(fā)明，因為為了對動作影響較大的扭矩飽和防止以及殘留振動抑制，基于關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同的驅(qū)動狀態(tài)來設(shè)定重力補償系數(shù)，所以可以利用基于因在與重力作用方向相反方向上加速時的扭矩飽和、進行伴隨著減速的定位時的殘留振動而導致的影響分別容易產(chǎn)生這一狀態(tài)的重力補償系數(shù)來算出加速度、減速度，從而可以有效地實現(xiàn)扭矩飽和防止以及殘留振動抑制。
又，本發(fā)明的加減速控制方法，具有如下工序基于與具有關(guān)節(jié)的機械的動作開始以及動作結(jié)束有關(guān)的數(shù)據(jù)、和與機械的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢的、作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序、與基于由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)、重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出動作中加速以及/或者減速時的加減速度修正系數(shù)的工序、與將加減速度修正系數(shù)與設(shè)定的基本加減速度相乘，從而算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
根據(jù)本發(fā)明，因為基于動作開始以及結(jié)束時的姿勢的重力扭矩、為了扭矩飽和防止以及殘留振動抑制而由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)以及輸出扭矩，來算出加減速度修正系數(shù)，并將該加減速度修正系數(shù)與基本加減速度相乘從而算出加速度以及/或者減速度，所以可以利用簡單的順序來算出與較大地受到重力的影響的扭矩飽和、殘留振動適當?shù)貙?yīng)的加減速度。以此，就不需要高速的運算處理，從而不使用高價的處理機構(gòu)亦可。
又，本發(fā)明的加減速控制方法，還具有如下工序設(shè)定加減速度修正系數(shù)的最大值以及/或者最小值，若判斷為算出的加減速度修正系數(shù)在范圍外，則將加減速度修正系數(shù)限制為最大值或者最小值。
根據(jù)本發(fā)明，因為限制加減速度修正系數(shù)，所以可以實現(xiàn)動作的安全。
又，在本發(fā)明的加減速控制方法中，機械是多關(guān)節(jié)型機械手。
上述的發(fā)明，可以相對于必須采取多樣的姿勢、進行更細致的加減速控制、定位的多關(guān)節(jié)型機械手而發(fā)揮最大的效果。
又，本發(fā)明的加減速控制裝置，包括如下機構(gòu)算出作用于使多關(guān)節(jié)型機械手動作的多關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的重力扭矩運算機構(gòu)、與基于預先設(shè)定的多個重力補償系數(shù)中基于由重力所影響的驅(qū)動狀態(tài)而選擇的重力補償系數(shù)與重力扭矩來算出加減速度修正系數(shù)的加減速度修正系數(shù)運算機構(gòu)、與基于加減速度修正系數(shù)和設(shè)定的基本加減速度來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的加減速度運算機構(gòu)。
根據(jù)本發(fā)明，因為加減速度修正系數(shù)運算機構(gòu)，基于重力扭矩運算機構(gòu)算出的重力扭矩與從預先設(shè)定的多個重力補償系數(shù)中例如基于驅(qū)動狀態(tài)來選擇的重力補償系數(shù)，來算出加減速度修正系數(shù)，加減速度運算機構(gòu)，基于加減速度修正系數(shù)與設(shè)定的基本加減速度，來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度，所以各機構(gòu)并不進行復雜的計算，可以以簡單的順序算出與因驅(qū)動狀態(tài)而不同的重力的影響(扭矩飽和、殘留振動等)適當?shù)貙?yīng)的加減速度。以此，裝置的成本也可以降低。
又，本發(fā)明的加減速控制裝置，具有如下機構(gòu)基于具有多關(guān)節(jié)型機械手的動作開始以及動作結(jié)束時的數(shù)據(jù)和與多關(guān)節(jié)型機械手的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢的、作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的重力扭矩運算機構(gòu)、與基于由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)、重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出動作中加速以及/或者減速時的加減速度修正系數(shù)的加減速度修正系數(shù)運算機構(gòu)、與將加減速度修正系數(shù)與由關(guān)節(jié)所設(shè)定的基本加減速度相乘從而算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的加減速度運算機構(gòu)。
根據(jù)本發(fā)明，因為加減速度修正系數(shù)運算機構(gòu)基于重力扭矩運算機構(gòu)所算出的動作開始以及結(jié)束時的姿勢的重力扭矩、為了扭矩飽和防止以及殘留振動抑制而由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)以及輸出扭矩，來算出加減速度修正系數(shù)，加減速度運算機構(gòu)將加減速度修正系數(shù)與基本加減速度相乘從而算出加速度以及/或者減速度，所以各機構(gòu)并不進行復雜的計算，可以以簡單的順序算出與較大地受到重力的影響的扭矩飽和、殘留振動等適當?shù)貙?yīng)的加減速度。以此，裝置的成本也可以降低。
又，本發(fā)明的加減速控制裝置還具有將加減速度修正系數(shù)運算機構(gòu)基于由加減速度修正系數(shù)所設(shè)定的最大值以及/或者最小值而算出的加減速度修正系數(shù)限制為最大值或者最小值的系數(shù)校驗機構(gòu)。
根據(jù)本發(fā)明，因為系數(shù)校驗機構(gòu)校驗加減速度修正系數(shù)，限制其最大值、最小值，所以可以實現(xiàn)動作的安全。
又，本發(fā)明的加減速控制裝置還具有如下機構(gòu)基于各關(guān)節(jié)的移動時間來判斷最大移動時間的關(guān)節(jié)的判斷機構(gòu)、與對最大移動時間的關(guān)節(jié)以外的關(guān)節(jié)，再計算對應(yīng)于最大移動時間的加速度以及/或者減速度的加減速度調(diào)整機構(gòu)。
根據(jù)本發(fā)明，因為判斷機構(gòu)基于各關(guān)節(jié)的移動時間來判斷最大移動時間的關(guān)節(jié)，然后加減速度調(diào)整機構(gòu)再計算最大移動時間的關(guān)節(jié)以外的關(guān)節(jié)，并對應(yīng)于最大移動時間地調(diào)整加速度以及/或者減速度，所以對于最大移動時間的關(guān)節(jié)以外的關(guān)節(jié)，可以生成加減速度的余量，從而可以進一步實現(xiàn)重力的影響的降低。
又，本發(fā)明的加減速控制方法的程序，使在計算機中進行如下工序算出作用于使具有關(guān)節(jié)的機械動作的關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序、與基于從設(shè)定的多個重力補償系數(shù)中基于驅(qū)動狀態(tài)而選擇的重力補償系數(shù)與重力扭矩來算出加減速度修正系數(shù)的工序、與基于設(shè)定的基本加減速度與加減速度修正系數(shù)來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
根據(jù)本發(fā)明，因為基于算出的重力扭矩與從預先設(shè)定的多個重力補償系數(shù)中選擇的重力補償系數(shù)來算出加減速度修正系數(shù)，基于加減速度修正系數(shù)與設(shè)定的基本加減速度來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度，所以可以利用簡單的順序算出與因驅(qū)動狀態(tài)而不同的重力的影響(例如扭矩飽和、殘留振動)適當?shù)貙?yīng)的加減速度。以此，即使是不能進行高速的運算處理的計算機也可以應(yīng)對所述運算。
又，本發(fā)明的加減速控制方法的程序，使在計算機中進行如下工序基于與具有關(guān)節(jié)的機械的動作開始以及動作結(jié)束有關(guān)的數(shù)據(jù)、和與機械的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢的、作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序、與基于由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)、重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出動作中加速以及/或者減速時的加減速度修正系數(shù)的工序、與將加減速度修正系數(shù)與由關(guān)節(jié)所設(shè)定的基本加減速度相乘，從而算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
根據(jù)本發(fā)明，因為包含與機械的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)在內(nèi)基于動作開始以及結(jié)束時的姿勢的重力扭矩、為了扭矩飽和防止以及殘留振動抑制而由關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同所設(shè)定的重力補償系數(shù)以及輸出扭矩，來算出加減速度修正系數(shù)，并將該加減速度修正系數(shù)與基本加減速度相乘從而算出加速度以及/或者減速度，所以可以利用簡單的順序來算出與較大地受到重力的影響的扭矩飽和、殘留振動適當?shù)貙?yīng)的加減速度。以此，即使是不能進行高速的運算處理的計算機也可以應(yīng)對所述運算。
又，本發(fā)明的加減速控制方法的程序，還在計算機中進行如下工序基于算出的加減速度修正系數(shù)來限制加減速度修正系數(shù)的最大值以及/或者最小值。
根據(jù)本發(fā)明，因為限制加減速度修正系數(shù)，所以可以在計算機中進行機械的安全控制。


圖1是以實施方式一的機械手控制裝置10為中心的圖；圖2是表示機械手100的一例的圖；圖3是以流程圖來表示設(shè)定了重力補償系數(shù)Kg的處理的圖。
符號說明1-加減速控制機構(gòu)，1A-重力扭矩運算部，1B-加減速度修正系數(shù)運算部，1C-修正系數(shù)校驗部，1D-加減速度運算部，1E-主軸判斷部，1F-加減速度調(diào)整部，2-插補運算機構(gòu)，3-信號輸出機構(gòu)，4-數(shù)據(jù)存儲機構(gòu)，10-機械手控制裝置，100-機械手，200-有效負載。
具體實施例方式
實施方式一圖1是表示以本發(fā)明的實施方式一的機械手控制裝置為中心的系統(tǒng)的圖。在圖1中，本實施方式的機械手控制裝置10以加減速控制機構(gòu)1、插補運算機構(gòu)2、信號輸出機構(gòu)3以及數(shù)據(jù)存儲機構(gòu)4構(gòu)成。機械手控制裝置10輸出驅(qū)動信號來控制機械手100的動作。這里在本實施方式中，主要說明與將機械手100的各關(guān)節(jié)(軸)中的電機的軸旋轉(zhuǎn)驅(qū)動的加減速包含的速度有關(guān)的控制(基本地，在1個動作中至少分別包含有1次加速、減速)。
加減速控制機構(gòu)1進而以重力扭矩運算部1A、加減速度修正系數(shù)運算部1B、修正系數(shù)校驗部1C、加減速度運算部1D、主軸判斷部1E以及加減速度調(diào)整部1F構(gòu)成。重力扭矩運算部1A對機械手100的各關(guān)節(jié)算出因重力的影響而在關(guān)節(jié)上產(chǎn)生的重力扭矩。又，加減速度修正系數(shù)運算部1B基于重力扭矩來算出各關(guān)節(jié)的加減速度修正系數(shù)。而且，修正系數(shù)校驗部1C，例如為了限制加減速度修正系數(shù)的效果，相對于加減速度修正系數(shù)來判斷是否在設(shè)定的最大值以及最小值的范圍內(nèi)。若判斷為不在范圍內(nèi)，則限制使加減速度修正系數(shù)變?yōu)樵诜秶鷥?nèi)。加減速度運算部1D基于加減速度修正系數(shù)(也包含有修正系數(shù)校驗部1C所限制的加減速度修正系數(shù))與預先由各關(guān)節(jié)所設(shè)定的基本加減速度，來算出各關(guān)節(jié)的加減速度。
主軸判斷部1E基于插補運算機構(gòu)2利用插補運算而導出的各關(guān)節(jié)的移動時間(旋轉(zhuǎn)驅(qū)動時間)來判斷移動時間最大(最長)的關(guān)節(jié)(將其作為主軸)。又，加減速度調(diào)整部1F對主軸以外的關(guān)節(jié)再計算加減速度，調(diào)整各關(guān)節(jié)的驅(qū)動(機械手100整體的動作)。
插補運算機構(gòu)2基于加減速度運算部1D所算出的加減速度等，對從動作開始至結(jié)束的機械手100的動作，進行各關(guān)節(jié)的驅(qū)動軌跡、速度等的插補運算。進而，信號輸出機構(gòu)3將包含有經(jīng)由插補運算機構(gòu)2而算出的各關(guān)節(jié)的加減速度的驅(qū)動信號輸出到機械手100上。
在此，在本實施方式中，利用例如以CPU為中心的控制處理裝置(計算機)來實現(xiàn)加減速控制機構(gòu)1、插補運算機構(gòu)2以及信號輸出機構(gòu)3，控制處理裝置實施上述各機構(gòu)以及構(gòu)成各機構(gòu)的各部所進行的處理，并對其運算結(jié)果進行數(shù)據(jù)處理。該情況下，例如，各部、各機構(gòu)進行處理的順序作為程序而存儲于數(shù)據(jù)存儲機構(gòu)4中。
數(shù)據(jù)存儲機構(gòu)4存儲為了加減速控制機構(gòu)1、插補運算機構(gòu)2以及信號輸出機構(gòu)3進行處理而成為必要的數(shù)據(jù)。例如，存儲為了重力扭矩運算部1A算出重力扭矩而成為必要的有效負載200的狀態(tài)(質(zhì)量、重心位置)、機械手100的機構(gòu)參數(shù)(例如各臂(連桿)的質(zhì)量、臂長、重心位置)等的數(shù)據(jù)以及動作開始點、結(jié)束點等的指示點的數(shù)據(jù)。又，存儲用于運算加速度修正系數(shù)的重力補償系數(shù)的數(shù)據(jù)。進而，存儲用于修正系數(shù)校驗部1C校驗加減速度修正系數(shù)的最大值以及最小值的數(shù)據(jù)、加減速度運算部1D運算加減速度時的基本加減速度的數(shù)據(jù)。
圖2是表示機械手100的一例的圖。在此，作為機械手100以6軸垂直多關(guān)節(jié)機器人等的多關(guān)節(jié)機械手為例進行說明，不過若是在必須考慮重力的影響的環(huán)境下動作的機械手，則對軸數(shù)等、其種類并不特別地限定。又，除機械手以外，也可以對動作時必須考慮重力的影響的具有關(guān)節(jié)(軸)的機械適用。有效負載200是例如機械手100所把持等的輸送物等。
接著以加減速控制機構(gòu)1為中心說明本實施方式的處理。在重力扭矩運算部1A，基于動作開始時以及動作結(jié)束時的機械手100的姿勢來算出在各關(guān)節(jié)上產(chǎn)生的重力扭矩Tg。就重力扭矩Tg的算出方法而言，并不特別地限定。根據(jù)有效負載200的條件(質(zhì)量、重心位置)、機械手100的機構(gòu)參數(shù)等機械手的種類，以及臂等的傾斜情況(水平距離)而不同。在此，在機械手100的基部以與豎直方向垂直的方式設(shè)置的情況下，就在圖2中進行J1的旋轉(zhuǎn)的關(guān)節(jié)而言，重力扭矩Tg為0。
在此，可以利用到機械手100的加減速中的扭矩是電機的最大輸出扭矩Tp以及重力扭矩Tg。因此通過下式(1)來算出可以利用到機械手100的關(guān)節(jié)的加減速(驅(qū)動)中的加減速扭矩Tac。而且，若以電機的最大輸出扭矩Tp將加減速扭矩Tac標準化，則如下式(2)所示。在此，稱Tg/Tp為標準化重力扭矩值(量綱為1)。另外，(1)、(2)式基于可以最大利用到機械手100的加減速中的最大輸出扭矩Tp，不過即使不是最大也可以應(yīng)用。
Tac＝Tp+Tg(1)Tac/Tp＝(Tp+Tg)/Tp＝1+Tg/Tp (2)加減速度修正系數(shù)運算部1B在算出各關(guān)節(jié)的加減速度時算出與基本加減速度相乘的加減速度修正系數(shù)K。用于算出加減速度修正系數(shù)K的基本式以下式(3)表示。在式(3)中，使用Tg的絕對值是為了不利用Tg的符號變化而利用重力補償系數(shù)Kg的符號設(shè)定來擴大加減速調(diào)整的自由度。在中括弧內(nèi)減去1是為了使基準為0(重力扭矩Tg＝0時為0，從而重力補償系數(shù)Kg并不對加減速度修正系數(shù)K產(chǎn)生影響)。中括弧內(nèi)最終成為與所述標準化重力扭矩值相同的值。另一方面，在標準化重力扭矩值與重力補償系數(shù)Kg的積加上1是為了在使加減速度修正系數(shù)K的基準為1(重力扭矩Tg＝0)時K＝1。
K＝[(Tp+|Tg|)/Tp-1]Kg+1＝(|Tg|·Kg+Tp)/Tp＝(|Tg|/Tp)Kg+1 (3)(3)式的重力補償系數(shù)Kg是為了利用重力扭矩Tg的大小來調(diào)整加減速修正系數(shù)K的效果而設(shè)定的系數(shù)。若Kg＞0，則加減速度修正系數(shù)K比1大，加減速增加。若Kg＜0，則加減速度修正系數(shù)K比1小，抑制加減速。若Kg＝0，則重力扭矩并不對加減速的效果起作用。在此，就重力補償系數(shù)Kg而言，在本實施方式中如表1所示，設(shè)定為按關(guān)節(jié)(旋轉(zhuǎn))驅(qū)動方向Θ’(與重力作用方向相同的方向/相反的方向)不同以及加速/減速不同區(qū)分的4種類型的Kg。一般地，扭矩飽和在與重力作用方向相反的方向上加速時成為問題，殘留振動在進行定位時(伴隨著減速)成為問題。因此，依據(jù)驅(qū)動方向不同以及加速/減速不同而區(qū)分為不同類型，設(shè)定各自的重力補償系數(shù)Kg(Kgaf、Kgdf、Kgar、Kgdr)，以此可以設(shè)定分別考慮了因重力而導致的扭矩飽和、殘留振動這些問題的加減速度。
表1


圖3是以流程圖來表示設(shè)定了重力補償系數(shù)Kg的處理的圖。首先，設(shè)定為Kg＝0(此時K＝1)(S1)。而且，在例如與重力作用方向相反的方向上加速的情況下，判斷在關(guān)節(jié)上是否產(chǎn)生扭矩飽和(S2)。進而，判斷殘留振動是否在允許范圍內(nèi)(S3)。該扭矩飽和、殘留振動的測定利用例如實測、計算模擬等進行。若沒有產(chǎn)生扭矩飽和或者殘留振動，則決定為Kg≥0那樣的重力補償系數(shù)Kg的設(shè)定(S4)。另一方面，若產(chǎn)生則決定為Kg＜0那樣的Kg的設(shè)定(S5)。
在S4中，若進行Kg≥0的決定，則使|Kg|增加(正的方向)(S6)，判斷殘留振動是否增加(S7)。若判斷為沒有增加，則返回到S6進而使|Kg|增加。另一方面，若判斷為增加，則設(shè)定不使扭矩飽和以及殘留振動成為問題的重力補償系數(shù)Kg(例如，在前一階段的步驟中增加前的Kg的值)(S8)。在此在S7中若判斷為殘留振動增加，則在S8中立即設(shè)定重力補償系數(shù)Kg，不過在增加|Kg|直至振動殘留超過允許范圍之后，設(shè)定重力補償系數(shù)Kg亦可。另外，在S6中就|Kg|的增加幅度而言，并不特別地限定，不過通過使|Kg|的增加幅度較小，就可以進行更加細致的重力補償系數(shù)Kg的設(shè)定(在后述S9中也同樣)。
若在S5中進行Kg＜0的決定，則使|Kg|增加(負的方向)(S9)，判斷扭矩飽和是否消失(S10)。若扭矩飽和沒有消失，則返回到S9，進一步增加|Kg|。若扭矩飽和消失，則進而判斷殘留振動是否在允許范圍內(nèi)(S11)。若不在允許范圍內(nèi)，則返回到S10，進一步增加|Kg|。若在允許范圍內(nèi)，則設(shè)定此時的值為重力補償系數(shù)Kg(S12)。
如上所述，將分別設(shè)定為4種類型的重力補償系數(shù)Kg(Kgaf、Kgdf、Kgar、Kgdr)分別代入到(3)式而算出的加減速度修正系數(shù)K(Kaf、Kdf、Kar、Kdr)如下式(4)～(7)。
Kaf＝(|Tg|/Tp)Kgaf+1 (4)Kdf＝(|Tg|/Tp)Kgdf+1 (5)Kar＝(|Tg|/Tp)Kgar+1 (6)Kdr＝(|Tg|/Tp)Kgdr+1 (7)修正系數(shù)校驗部1C相對于加減速度修正系數(shù)運算部1B所算出的各關(guān)節(jié)的加減速度修正系數(shù)K，如下式(8)所示，校驗是否在最小值Kmin以上、最大值Kmax以下的范圍內(nèi)。例如，在根據(jù)傾斜較大等機械手100的姿勢來限制加減速度的情況下，限制加減速度修正系數(shù)的效果。若判斷為Kmax＞K，則設(shè)定K＝Kmax，若判斷為Kmin＜K，則設(shè)定K＝Kmin。另外，在本實施方式中最小值Kmin以及最大值Kmax設(shè)定為因各關(guān)節(jié)而不同的值，不過并不限定于此，與關(guān)節(jié)無關(guān)地設(shè)定為一律的值亦可。
Kmin≤K≤Kmax (8)加減速度運算部1D基于由修正系數(shù)校驗部1C校驗、確定的加減速度修正系數(shù)K來算出各關(guān)節(jié)的加減速度。在此，在進行加速的情況下，在由各關(guān)節(jié)所設(shè)定的基本加速度Aab上乘以加減速度修正系數(shù)Kaf或者Kar。又，在進行減速的情況下，在基本減速度Adb上乘以加減速度修正系數(shù)Kdf或者Kdr。在此，基本加速度Aab、基本減速度Adb是例如從臂延伸狀態(tài)的姿勢、收縮狀態(tài)的姿勢(基本姿勢)等加速或者減速為基本姿勢等時的基本的加速度、減速度。關(guān)節(jié)(旋轉(zhuǎn))驅(qū)動方向與重力作用方向為相同方向(Tg×Θ’＞0)的加速度Aac、減速度Adc，分別以下式(9)、(10)表示。
Aac＝Kaf×Aab (9)Adc＝Kdf×Adb (10)又，關(guān)節(jié)(旋轉(zhuǎn))驅(qū)動方向與重力作用方向為相反方向的加速度Aac、減速度Adc，分別以下式(11)、(12)表示。
Aac＝Kar×Aab (11)Adc＝Kdr×Adb (12)插補運算機構(gòu)2以加減速度運算部1D算出的加速度來加速驅(qū)動關(guān)節(jié)，從而機械手100的動作開始，以減速度來減速驅(qū)動，從而機械手100的動作結(jié)束的方式，對各關(guān)節(jié)進行插補從動作開始至結(jié)束的軌跡(位置)、速度等的運算。
主軸判斷部1E基于各關(guān)節(jié)的加減速度等來算出各關(guān)節(jié)的移動時間(旋轉(zhuǎn)驅(qū)動時間)，并判斷出其中的移動時間最大的關(guān)節(jié)，將其作為主軸。該最大的移動時間成為機械手100整體的移動時間。因而，主軸以外的各關(guān)節(jié)以成為最大的移動時間的方式調(diào)整即可，所以可以比算出的移動時間有一定的余量(例如，主軸以外的關(guān)節(jié)并不以最大的輸出扭矩進行加減速)。因此，加減速度調(diào)整部1F將最大移動時間的關(guān)節(jié)作為主軸，對主軸以外的關(guān)節(jié)進行加減速的再計算，從而決定各關(guān)節(jié)的最終加減速度。在此，就再計算的方法、順序而言并不特別地限定。例如，因為殘留振動容易因急劇的減速而產(chǎn)生，所以若以在最大移動時間的范圍內(nèi)抑制加減速度的方式調(diào)整，則可以抑制殘留振動的產(chǎn)生。進而，插補運算機構(gòu)2基于調(diào)整的加減速度來進行插補運算。
信號輸出機構(gòu)3基于插補運算機構(gòu)2對應(yīng)于各關(guān)節(jié)的加減速度等運算得到的軌跡、速度等，將用于驅(qū)動機械手100的驅(qū)動信號輸出到機械手100上。在機械手100的各關(guān)節(jié)中供給有基于驅(qū)動信號的電流，關(guān)節(jié)驅(qū)動電機(旋轉(zhuǎn))驅(qū)動，從而機械手100動作。
如上述那樣，根據(jù)實施方式一，相對于機械手100的各關(guān)節(jié)，重力扭矩運算部1A算出重力扭矩Tg，加減速度修正系數(shù)運算部1B利用基于驅(qū)動狀態(tài)(電機的驅(qū)動方向不同以及加速/減速不同)而從設(shè)定的多個重力補償系數(shù)中選擇的重力補償系數(shù)Kg來調(diào)整標準化重力扭矩值(重力扭矩)，然后決定加減速度修正系數(shù)K，加減速度運算部1D在基本加減速度上乘以加減速度修正系數(shù)K，從而算出各關(guān)節(jié)的加速度，所以可以對應(yīng)于驅(qū)動的狀態(tài)，利用簡單的計算(算法)算出與重力的影響適當?shù)貙?yīng)的加減速度。特別是因為根據(jù)電機的驅(qū)動方向不同以及加速/減速不同而設(shè)定重力補償系數(shù)Kg(Kgaf、Kgdf、Kgar、Kgdr)，所以可以在基于因在與重力作用方向相同的方向上加速時的扭矩飽和、進行伴隨著減速的定位時的殘留振動而導致的影響分別容易產(chǎn)生的狀態(tài)而進行調(diào)整之后算出加減速度，從而可以有效地實現(xiàn)扭矩飽和防止以及殘留振動抑制。
進而，因為設(shè)定加減速度修正系數(shù)K的最大值最小值，并利用修正系數(shù)校驗部1C將加減速度修正系數(shù)限制為最大值或者最小值，所以可以限制傾斜較大的情況下的加減速度，從而可以實現(xiàn)機械手的動作的安全。又，因為主軸判斷部1E基于各關(guān)節(jié)的移動時間來判斷主軸，加減速度調(diào)整部1F對應(yīng)于該最大移動時間(機械手100整體的動作時間)地再計算從而調(diào)整加減速度，所以就主軸以外的關(guān)節(jié)而言，可以生成加減速度的余量，從而可以實現(xiàn)扭矩飽和防止以及殘留振動抑制。
權(quán)利要求
1.一種加減速控制方法，其特征在于，包括算出作用于使具有關(guān)節(jié)的機械動作的關(guān)節(jié)驅(qū)動電機上的重力扭矩的工序；根據(jù)預先設(shè)定的多個重力補償系數(shù)中基于受重力影響的驅(qū)動狀態(tài)而選擇的重力補償系數(shù)、和所述重力扭矩，來算出加減速度修正系數(shù)的工序；基于該加減速度修正系數(shù)和設(shè)定的基本加減速度來算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
2.如權(quán)利要求1所述的加減速控制方法，其特征在于，為了所述關(guān)節(jié)驅(qū)動電機的扭矩飽和防止以及殘留振動抑制，基于因所述關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同而構(gòu)成的所述驅(qū)動狀態(tài)來設(shè)定所述重力補償系數(shù)。
3.一種加減速控制方法，其特征在于，包括基于與具有關(guān)節(jié)的機械的動作開始以及動作結(jié)束有關(guān)的數(shù)據(jù)、以及與所述機械的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢下作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序；基于因所述關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同而決定的重力補償系數(shù)、所述重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出所述動作中加速以及/或者減速時的加減速度修正系數(shù)的工序；將所述加減速度修正系數(shù)與設(shè)定的基本加減速度相乘，從而算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
4.如權(quán)利要求1～3中的任意一項所述的加減速控制方法，其特征在于，所述加減速度修正系數(shù)是如下得到的值將所述重力扭矩的絕對值與所述關(guān)節(jié)驅(qū)動電機的輸出扭矩的和除以所述輸出扭矩后再減去1得到的值、與所述重力補償系數(shù)相乘，然后在該值上加上1。
5.如權(quán)利要求3或者4所述的加減速控制方法，其特征在于，還具有設(shè)定所述加減速度修正系數(shù)的最大值以及/或者最小值，若判斷為算出的所述加減速度修正系數(shù)在范圍外，則將所述加減速度修正系數(shù)限制為最大值或者最小值的工序。
6.如權(quán)利要求1～5中的任意一項所述的加減速控制方法，其特征在于，所述機械是多關(guān)節(jié)型機械手。
7.一種加減速控制裝置，其特征在于，包括算出作用于使多關(guān)節(jié)型機械手動作的多關(guān)節(jié)驅(qū)動電機上的重力扭矩的重力扭矩運算機構(gòu)；根據(jù)預先設(shè)定的多個重力補償系數(shù)中基于受重力影響的驅(qū)動狀態(tài)而選擇的重力補償系數(shù)、與所述重力扭矩，來算出加減速度修正系數(shù)的加減速度修正系數(shù)運算機構(gòu)；基于該加減速度修正系數(shù)和設(shè)定的基本加減速度來算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的加減速度運算機構(gòu)。
8.一種加減速控制裝置，其特征在于，包括基于多關(guān)節(jié)型機械手的動作開始以及動作結(jié)束時的數(shù)據(jù)、和與所述多關(guān)節(jié)型機械手的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢下的、作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的重力扭矩運算機構(gòu)；基于因所述關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同而決定的重力補償系數(shù)、所述重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出所述動作中加速以及/或者減速時的加減速度修正系數(shù)的加減速度修正系數(shù)運算機構(gòu)；將所述加減速度修正系數(shù)、與在所述關(guān)節(jié)上設(shè)定的基本加減速度相乘，從而算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的加減速度運算機構(gòu)。
9.如權(quán)利要求7或者8所述的加減速控制裝置，其特征在于，還包括系數(shù)校驗機構(gòu)，該系數(shù)校驗機構(gòu)，基于在所述加減速度修正系數(shù)上設(shè)定的最大值以及/或者最小值將所述加減速度修正系數(shù)運算機構(gòu)算出的所述加減速度修正系數(shù)限制為最大值或者最小值。
10.如權(quán)利要求7～9中的任意一項所述的加減速控制裝置，其特征在于，還包括基于各關(guān)節(jié)的移動時間來判斷最大移動時間的關(guān)節(jié)的判斷機構(gòu)；對所述最大移動時間的關(guān)節(jié)以外的關(guān)節(jié)，再計算與所述最大移動時間相匹配的加速度以及/或者減速度的加減速度調(diào)整機構(gòu)。
11.一種加減速控制方法的程序，其特征在于，使在計算機中進行算出作用于使具有關(guān)節(jié)的機械動作的關(guān)節(jié)驅(qū)動電機上的重力扭矩的工序；根據(jù)從基于驅(qū)動狀態(tài)設(shè)定多個的重力補償系數(shù)中選擇的重力補償系數(shù)、與該重力扭矩來算出加減速度修正系數(shù)的工序；基于設(shè)定的基本加減速度與所述加減速度修正系數(shù)來算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
12.一種加減速控制方法的程序，其特征在于，使在計算機中進行基于與具有關(guān)節(jié)的機械的動作開始以及動作結(jié)束有關(guān)的數(shù)據(jù)、和與所述機械的機構(gòu)參數(shù)以及/或者有效負載有關(guān)的數(shù)據(jù)，來算出動作開始以及動作結(jié)束時的姿勢下的、作用于關(guān)節(jié)驅(qū)動電機的驅(qū)動上的重力扭矩的工序；基于因所述關(guān)節(jié)驅(qū)動電機的驅(qū)動方向不同以及加速/減速不同而決定的重力補償系數(shù)、所述重力扭矩以及關(guān)節(jié)驅(qū)動電機的輸出扭矩，來算出所述動作中加速以及/或者減速時的加減速度修正系數(shù)的工序；將所述加減速度修正系數(shù)與在所述關(guān)節(jié)上設(shè)定的基本加減速度相乘，從而算出所述關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度的工序。
13.如權(quán)利要求12所述的加減速控制方法的程序，其特征在于，使在計算機中還進行基于算出的加減速度修正系數(shù)來限制所述加減速度修正系數(shù)的最大值以及/或者最小值的工序。
全文摘要
本發(fā)明提供一種以簡單的算法、對應(yīng)于例如電機的驅(qū)動方向以及加減速等地決定電機的加減速度的加減速控制方法、裝置等。重力扭矩運算部(1A)算出作用于使具有關(guān)節(jié)的機械即機械手(100)動作的關(guān)節(jié)驅(qū)動電機上的重力扭矩，加減速度修正系數(shù)運算部(1B)利用預先設(shè)定的多個重力補償系數(shù)中根據(jù)由重力所影響的驅(qū)動條件而選擇的重力補償系數(shù)與重力扭矩來算出加減速度修正系數(shù)，從而加減速度運算部(1D)基于加減速度修正系數(shù)與設(shè)定的基本加減速度來算出關(guān)節(jié)驅(qū)動電機的加速度以及/或者減速度。
文檔編號G05B19/416GK1833830SQ20061006783
公開日2006年9月20日 申請日期2006年3月14日 優(yōu)先權(quán)日2005年3月14日
發(fā)明者平林友一 申請人:精工愛普生株式會社