JPH11277468A - ロボットの制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 モータの能力を最大限利用して関節を動作さ
せることができ、また、ロボット先端の軌跡のずれを生
ずることなく関節の動作を制御する。 【解決手段】 速度パターンに基づいて加算時間量ＳＴ
k の経過毎に、次の加算時間量ＳＴk の経過時点での関
節の角度を仮関節角度指令値として演算し、この仮関節
角度指令値に動作させるに必要な駆動トルクが限界値以
下の場合には、仮関節角度指令値を角度指令値として関
節の動作を制御する。駆動トルクが限界値を越えた場合
には、限界値を越えないようにするための速度変更比率
Ｒを求め、この速度変更比率Ｒに基づいて加算時間量Ｓ
Ｔk を短く修正し、再度仮関節角度指令値を演算し、こ
れを角度指令値として関節の動作を制御する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明はモータを駆動源とし
て動作する関節を備え、その関節をモータの持つ能力を
最大限利用して動作させることができるようにしたロボ
ット制御装置に関する。

【０００２】

【発明が解決しようとする課題】多関節型の産業用ロボ
ットは、複数の可動部を有し、それら可動部が関節結合
された構造となっている。そして、各可動部（関節）
は、駆動源である直流サーボモータなどのモータから適
宜の伝動機構を介して駆動トルクを受けて回転動作を行
うようになっており、それぞれの可動部の総合動作の結
果としてロボット先端がプログラムされた通りの動作を
行う。

【０００３】ここで、関節の回転動作の制御を一つの関
節を例にして説明するに、まず、ロボットでは、モータ
のロータリエンコーダから与えられる信号により関節の
現在の角度（位置）を検出し、この現在の角度をフィー
ドバック信号として、現在角度が指令角度に一致するよ
うにモータを制御するという、いわゆるフィードバック
制御によって関節の動作を制御するようにしている。

【０００４】このフィードバック制御を行うために、制
御装置は、動作プログラム中から読み出したパラメータ
に基づいて速度パターンを決定し、次いで、その速度パ
ターンから所定のサンプリング時間が経過する毎に、そ
の時点を現在として、現在からサンプリング時間経過後
の角度を算出し、これを指令角度とする。そして、関節
の現在位置と指令角度とを比較し、サンプリング時間と
同じ長さに定められた単位制御時間後に、関節が指令さ
れた角度まで動作するようにモータの速度を制御する。
この制御により、関節は最初に動作プログラムが定める
速度パターンと同様の速度パターンにて動作するように
なり、このようなことが各関節の全体について行われる
ことから、ロボット先端がプログラムされた通りの動作
を行うのである。

【０００５】ところで、ロボットでは、モータの負荷ト
ルクが限界値、例えば最大許容トルクを越えると、モー
タ保護の観点から、動作を停止させるように構成されて
いる。このため、ロボットの動作プログラムを作成する
ティーチング時には、モータが過負荷とならないよう
に、関節の動作速度などを設定するようにしている。し
かしながら、そのような負荷トルクの限界を各関節につ
いて考えながらティーチング作業を行うことは、大変に
煩わしく、非常に多くの手間を要するもので、ティーチ
ング作業の能率が悪いという問題があった。しかも、モ
ータが過負荷とならないように関節の動作速度を設定す
るため、必然的に、ほとんどの動作においてモータの許
容最大トルクを越えることはなくなるが、現実には、許
容最大トルクよりもかなり低いレベルでモータを使用す
ることとなり、この結果、関節の動作速度が遅くなっ
て、ロボット作業のタクトタイムが長くなるという問題
を生じていた。

【０００６】このような問題を解決するものとして、例
えば、特開平５−１３８５６３号公報に開示された技術
がある。これは、一般にロボットでは、移動動作におい
ては、加速完了付近、減速開始付近で最も大きな負荷が
かかるので、その加速完了位置と減速開始位置の負荷状
態に合わせて、両位置でモータの負荷トルクが限界値を
越えないように加速度および減速度を決定するというも
のである。

【０００７】しかしながら、このものでは、加速完了位
置、減速開始位置という代表的な動作状態で移動動作の
加速度および減速度を計算するため、必要とする全ての
時点でモータの最大能力を発揮できる動作が実現される
というものではなく、逆に、加速完了位置、減速開始位
置以外の他の動作位置では、モータの最大能力を越える
おそれもある。また、各関節の移動動作の加速度および
減速度は、各関節毎に加速完了位置と減速開始位置の動
作状態に基づいて決定するため、各関節毎に速度パター
ンを設定する動作（ＰＴＰ動作）の場合には良いが、ロ
ボット先端の軌道を設定する場合には適用できず、適用
した場合には、ロボット先端の軌跡がプログラムされた
軌跡から外れてしまうという問題を生ずる。

【０００８】本発明は上記の事情に鑑みてなされたもの
で、その目的は、必要なときには、駆動源の能力を最大
限利用して関節を動作させることができ、また、ロボッ
ト先端の軌跡のずれを生ずることなく関節の動作制御を
行うことができるロボットの制御装置を提供するにあ
る。

【０００９】

【課題を解決するための手段】本発明は、速度パターン
に基づき、サンプリング時間が経過する度に、次のサン
プリング時間経過時点における関節の位置を演算する位
置演算手段と、前記関節を前記位置演算手段により演算
された位置に動作させるに必要な駆動トルクを演算する
トルク演算手段と、このトルク演算手段により演算され
た駆動トルクが予め設定された限界値を越えるとき、駆
動トルクが前記限界値を越えないようにするための速度
変更比率を演算し、この速度変更比率に基づいて通常の
サンプリング時間よりも短い修正サンプリング時間を演
算するサンプリング時間変更手段とを備え、前記トルク
演算手段により演算された駆動トルクが前記限界値以下
のとき、前記位置演算手段により演算された位置を指令
位置として、前記関節が所定の単位制御時間後にその指
令位置まで動作するように制御し、前記トルク演算手段
により演算された駆動トルクが前記限界値を越えたと
き、前記サンプリング時間変更手段により演算された前
記修正サンプリング時間を前記次のサンプリング時間と
して前記位置演算手段により再度関節の位置を演算し、
その位置を指令位置として、前記関節が前記所定の単位
制御時間後にその指令位置まで動作するように制御する
構成としたことを特徴とするものである（請求項１）。

【００１０】この構成によれば、サンプリング時間が経
過する毎に、そのサンプリング時間経過時点から次のサ
ンプリング時間経過後における関節の位置が演算され、
その位置を仮の位置として当該仮位置に動作させるに必
要な駆動トルクが演算される。そして、演算された駆動
トルクが限界値以下の場合には、上記の仮位置を指令位
置とし、関節が所定の単位制御時間（例えばサンプリン
グ時間と同じ長さ）でその指令位置まで動作するように
関節を制御する。しかしながら、仮位置に動作させるに
要する駆動トルクが限界値を越える場合には、通常のサ
ンプリング時間よりも短い修正サンプリング時間が設定
され、そして、修正サンプリング時間の経過後の位置を
指令位置として再度演算し、関節を所定の単位制御時間
で指令位置に動作するように制御する。このため、最初
に演算された仮位置までの距離よりも修正後の指令位置
までの方が短いため、単位制御時間の長さを変えること
なく、修正後の仮位置を指令位置として関節を動作させ
れば、駆動トルクが限界値を越えないようにすることが
できる。

【００１１】このようにサンプリング時間が経過する毎
に、仮位置を設定して当該仮位置に動作させるに必要な
駆動トルクが限界値を越えるか否かを判断し、越えたと
きには、越えないように関節の指令位置を修正するの
で、ティーチングの際には、モータの駆動トルクが限界
値を越えないように考慮しながら作業する必要がなくな
り、容易にティーチング作業を行うことができる。ま
た、駆動トルクが限界値を越える場合には、限界値と同
等かやや下回る駆動トルクとなるように指令位置が修正
されるので、ティーチングの際には駆動トルクが限界値
を越えるように、速度や加速度を大きく設定しておけ
ば、モータなどの駆動源の能力を最大限利用して動作さ
せることができ、タクトタイムを短縮することができ
る。

【００１２】また、本発明では、速度パターンに基づ
き、サンプリング時間が経過する度に、次のサンプリン
グ時間経過時点における関節の位置を演算する位置演算
手段と、前記関節を前記位置演算手段により演算された
位置に動作させるに必要な駆動トルクを演算するトルク
演算手段と、このトルク演算手段により演算された駆動
トルクが予め設定された限界値を越えるとき、駆動トル
クが前記限界値を越えないようにするための速度変更比
率を演算する速度変更比率演算手段とを備え、前記トル
ク演算手段により演算された駆動トルクが前記限界値以
下のとき、前記位置演算手段により演算された位置を指
令位置として、前記関節が単位制御時間後にその指令位
置まで移動するように制御し、前記トルク演算手段によ
り演算された駆動トルクが前記限界値を越えたとき、前
記速度変更比率演算手段により演算された速度変更比率
に基づいて前記単位制御時間を通常の単位制御時間より
も長く設定し、前記位置演算手段により演算された位置
を指令位置として、前記関節が前記長く設定された単位
制御時間後にその指令位置に動作するように制御する構
成とすることができる（請求項２）。

【００１３】この構成によれば、駆動トルクが限界値を
越えた場合、指令位置を変えることなく、単位制御時間
を長くするので、駆動トルクの限界値と同等の駆動トル
クかそれよりやや下回る駆動トルクとなる程度に関節の
速度が抑制される。このため、前述したと同様に、ティ
ーチングの際に駆動トルクが限界値を越えないように考
慮する必要がなく、作業を楽に行うことができると共
に、ティーチングの際に、駆動トルクが限界値を越える
ように、速度や加速度を大きく設定しておくことによ
り、モータの能力を最大限利用して動作させることがで
きる。

【００１４】

【発明の実施の形態】以下、本発明をロボットに適用し
た一実施例につき図面に基づいて説明する。ロボット
は、図４に示すように、ロボット本体１と、このロボッ
ト本体１を制御する制御装置２と、ティーチングペンダ
ント３を備えている。この実施例において、制御装置２
の制御対象であるロボット本体１は、例えば垂直多関節
型の組み立て用ロボットとして構成され、ベース４と、
このベース４に水平方向に旋回可能に設けられたショル
ダ部５と、このショルダ部５に上下方向に旋回可能に設
けられた下アーム６と、この下アーム６に上下方向に旋
回可能に且つ捻り回転可能に設けられた上アーム７と、
この上アーム７に上下方向に旋回可能に設けられた手首
８とを備えており、手首８は先端部に捻り回転可能なフ
ランジ９を備えている。なお、図示はしないが、ワーク
を把持するハンド（図示せず）は、フランジ９に取り付
けられるようになっている。

【００１５】そして、ショルダ部５の水平方向の旋回動
作、下アーム６の上下方向の旋回動作、上アーム７の上
下方向の旋回動作、上アーム７の回転動作、手首８の上
下方向の旋回動作、フランジ９の回転動作は、それぞれ
関節が、駆動源である例えば直流サーボモータからなる
モータ１０（図３参照）により図示しない適宜の伝動機
構を介して回転駆動されることにより行われる。なお、
図３では、ショルダ部５、下アーム６、上アーム７、手
首８およびフランジ９を可動部として一つのブロックで
示し、これに応じてそれらの関節の駆動源であるモータ
１０も一台だけ示した。

【００１６】一方、図３に示すように、制御装置２は、
制御部としてのＣＰＵ１１、各関節のモータ１０を駆動
する駆動手段としての駆動回路１２、位置検出手段とし
ての位置検出回路１３などを備えている。そして、ＣＰ
Ｕ１１には、ロボット全体のシステムプログラム等を記
憶したＲＯＭ１４、ロボット本体１の動作プログラム等
を記憶したＲＡＭ１５および前記ティーチングペンダン
ト３を接続するためのインターフェース１６が接続され
ている。

【００１７】上記位置検出回路１３は、各関節の現在位
置（回転角度）を検出するためのもので、この位置検出
回路１３には、各関節を駆動するモータ１０に設けられ
た位置検出センサとしてのロータリエンコーダ１７が接
続されている。ロータリエンコーダ１７は、各モータ１
０の回転角度に応じたパルス信号を出力し、そのパルス
信号は位置検出回路１３に与えられる。位置検出回路１
３は、各ロータリエンコーダ１７からのパルス信号に基
づいて各モータ１０ひいては各関節の現在の角度位置を
検出し、その位置情報は、各モータ１０の駆動回路１２
に与えられると共に、ＣＰＵ１１に与えられるようにな
っている。

【００１８】そして、各駆動回路１２は、ＣＰＵ１１か
ら与えられる指令角度と位置検出回路１３から与えられ
る現在角度とを比較し、その偏差に応じた電流を各モー
タ１０に供給してそれらを駆動する。これにより、ロボ
ット先端であるフランジ９の中心部が動作プログラムに
より定められた通りの軌跡を辿って動作し、部品の組み
立て作業などを行うものである。

【００１９】さて、動作プログラムには、一つの動作毎
に動作終了位置、速度係数、加減速度係数などのパラメ
ータが記録されている。このうち、速度係数および加減
速度係数とは、動作の最高速度および加減速度をモータ
１０の許容最大速度および許容最大加減速度に対する割
合で定めたもので、許容最大速度および許容最大加減速
度は、モータ１０の負荷トルクが例えば許容最大トルク
を越えることのないように、各モータ１０毎にその性能
を考慮して設定されている。

【００２０】ＣＰＵ１１は、上記の動作プログラムに記
録されたパラメータから速度パターンを例えば台形パタ
ーンに当て嵌めて決定し、その速度パターンに基づいて
各関節について経過時間毎の角度を演算し、これを角度
指令値として駆動回路１２に与えるようになっている。
すなわち、台形速度パターンは、図２（ａ）に示すよう
に、加速過程、最高速度での等速過程、減速過程からな
るが、この台形の速度パターンとなるような動作角度の
パターンは図２（ｂ）に示すようになる。

【００２１】図２（ｂ）に示す角度パターンにおいて、
ｔn 時点での関節の角度をθn 、このｔn 時点からΔｔ
時間後のｔ(n+1) 時点での角度をθ(n+1) とすると、次
式で示すように、Δｔ時間での角度の変化がｔ(n+1) 時
点での関節の角速度Ｖ(n+1)となる。 Ｖ(n+1) ・Δｔ＝θ(n+1) −θn

【００２２】上の式から、θ(n+1) ＝θn ＋Ｖ(n+1) ・
Δｔ となるので、ｔn 時点からΔｔ時間後の関節の角
度θ(n+1) は、ｔn 時点の角度θn に、速度パターンか
ら得られるｔ(n+1) 時点の角速度Ｖ(n+1) にΔｔを乗じ
た値を加算することによって求めることができる。従っ
て、速度パターンに基づいて、動作開始時点から所定の
サンプリング時間（Δｔ）経過毎に、その経過時点を現
在として次のサンプリング時点（Δｔ経過後に相当する
時点）の速度を算出し、これにサンプリング時間を乗じ
た値を順次加算してゆけば、動作開始時点から動作終了
時点までの間、サンプリング時間経過毎の関節の角度を
順次求めることができ、それを角度指令値として駆動回
路１２に与えれば速度パターン通りの動作を関節に行わ
せることができるものである。

【００２３】この実施例のロボットは、ロボット先端の
動作軌跡を動作プログラムに設定し、ロボット先端を設
定された軌跡に従って動作させるように各関節を制御す
るようにしている。そして、ＣＰＵ１１は、一つの動作
を制御する場合、動作プログラムに記録されているパラ
メータに基づいて速度パターンを決定し、この速度パタ
ーンに基づいてサンプリング時点毎に次のサンプリング
時点におけるロボット先端の座標位置を求め、次いで、
ロボット先端の座標位置から各関節の角度を求めるよう
にしている。

【００２４】以下、制御の内容を図１のフローチャート
を参照しながら説明する。ここで、以下の説明では、サ
ンプリング時間（Δｔ）を加算時間量ＳＴk 、動作開始
時点から各サンプリング時点までの経過時間を指令動作
時間ｔｉとし、関節（モータ１０）を現在の角度から角
度指令値まで動作させる制御を行うための時間を単位制
御時間ｔu ということとする。上記加算時間量ＳＴk
は、通常は一定の時間に定められているが、後述のよう
に、仮に定められた角度指令値に動作させるために必要
とする駆動トルクが、例えばモータ１０の最大許容トル
クに等しく設定された限界値を越えると予測される場合
には、通常時よりも短い時間に修正されるようになって
いる。これに対し、単位制御時間ｔu は、常に一定で変
化することはない。

【００２５】しかして、ＣＰＵ１１は、一つの動作に対
する制御を開始すると、まず、動作プログラム中から動
作終了位置の座標、速度および加速度の両係数などのパ
ラメータを読み出し、それらパラメータから速度パター
ンを決定する（ステップＳ１、速度パターン決定手
段）。

【００２６】速度パターンは、動作開始から終了までの
ロボット先端の移動量Ｌ、加速度α、最高速度Ｖmax 、
減速度βに基づいて、図２（ａ）に示す台形パターンに
当て嵌めて決定される。ここで、加速度αと減速度β
は、モータ１０の最大許容加速度および最大許容減速度
に加減速度係数を乗じて求められ、最高速度Ｖmax は、
モータ１０の許容最大速度に速度係数を乗じて求めら
れ、また、移動距離Ｌは、パラメータの一つとして動作
プログラムから得た動作終了位置と位置検出回路１３の
位置情報から求められる現在位置（動作開始時の位置）
から演算される。

【００２７】速度パターンを決定すると、ＣＰＵ１１
は、動作パラメータを演算する（ステップＳ２、動作パ
ラメータ演算手段）。動作パラメータとは、速度パター
ンを形成するパラメータで、加速度過程の所要時間ｔ1
、減速過程の所要時間ｔ2 、動作開始から終了までの
所要時間Ｔおよび最高速度Ｖmax である。なお、最高速
度Ｖmax 以外の動作パラメータは、次式にて求められ
る。

【００２８】ｔ1 ＝Ｖmax ／α ｔ2 ＝Ｖmax ／β Ｔ＝（Ｌ／Ｖmax ）＋Ｖmax ・（１／α＋１／β）／２

【００２９】次に、ＣＰＵ１１は、速度パターンにおい
て、指令動作時間ｔi を初期化し、「ｔｉ＝０」に設定
する（ステップＳ３）。この初期化により、図２（ａ）
の速度パターンの時間軸において、現在時点が原点（動
作開始時点）に設定される。この後、ＣＰＵ１１は、加
算時間量ＳＴk を一定時間ＳＴに設定する初期化を実行
する（ステップＳ４）。

【００３０】次に、ＣＰＵ１１は、ステップＳ１で決定
した速度パターンの動作パラメータに基づき、現在時点
ｔｉから加算時間量ＳＴk 経過後におけるロボット先端
の位置を演算する（ステップＳ５、先端位置演算手
段）。ここで、ロボット先端の動作開始位置を三次元の
直交座標（Ｘｓ，Ｙｓ，Ｚｓ）および直交座標の３軸に
対する傾き角（αｓ，βｓ，γｓ）、動作終了位置を同
じく（Ｘｅ，Ｙｅ，Ｚｅ）および（αｅ，βｅ，γｅ）
で表すと共に、ロボット先端の最高速度Ｖmax を三次元
直交座標の３軸方向の速度成分（Ｖmx，Ｖmy，Ｖmz）お
よび直交座標の３軸を中心とする回転速度成分（Ｖm
α，Ｖmβ，Ｖmγ）で表した場合、ｔ時点でのロボット
先端の位置を、三次元の直交座標（Ｘｔ，Ｙｔ，Ｚｔ）
および三次元の直交座標の３軸に対する傾き角（αｔ，
βｔ，γｔ）で示すと、次の（１−１）式〜（１−３）
式のようになる。なお、（１−１）式〜（１−３）式は
Ｘ座標についてだけを示しているが、Ｙｔ，Ｚｔおよび
αｔ，βｔ，γｔも同様にして求めることができるもの
である。

【００３１】

【数１】

【００３２】ＣＰＵ１１は、このようにして現在時点ｔ
ｉから加算時間量ＳＴk 経過した時点におけるロボット
先端の位置を演算した後、そのロボット先端の位置から
各関節の（ｔｉ＋ＳＴk ）時間における位置を求め、こ
れを各関節についての仮関節角度指令値とする（ステッ
プＳ６、位置演算手段）。次いで、ＣＰＵ１１は、各関
節をＳＴ時間で現在位置から仮関節角度指令値まで動作
させる場合、各関節の動作に必要な駆動トルク（各モー
タ１０の負荷トルク）を仮関節駆動トルクとして演算す
る（ステップＳ７、トルク演算手段）。

【００３３】ここで、ｎ個の関節を有するロボットにお
いて、関節ｉの駆動トルクτｉは次の（２）式により求
めることができる。

【数２】 ここで、上記（２）式において、右辺第１項は各関節の
角加速度に比例した項、第２項は２つの関節の角速度の
積に比例した項、第３項は自身の関節ｉの角速度に比例
した項、第４項は定数項である。なお、ハンドやハンド
で把持するワークの重量は関節の駆動トルクに影響を及
ぼすが、それらの要素は（２）式に入れて演算すること
は可能であるので、それらの重量が無視できない場合に
は、（２）式にそれらの重量要素を入れることによって
一層正確に駆動トルクを演算することができる。

【００３４】次いで、ＣＰＵ１１は、ステップＳ７で演
算した各関節の仮関節駆動トルクが予め設定されている
限界値以下であるか否かを判断する（ステップＳ８、判
断手段）。各関節の仮関節駆動トルクが各モータ１０の
最大許容トルクの範囲内である場合には、ＣＰＵ１１
は、ステップＳ８で「ＹＥＳ」と判断し、上記仮関節角
度指令値を各関節の角度指令値としてＣＰＵ１１が有す
るアキュームレータなどのバッファに転送する（ステッ
プＳ９）。

【００３５】そして、ＣＰＵ１１は、動作完了か否か、
すなわち指令動作時間ｔｉがＴ時間以上になったか否か
を判断し（ステップＳ１０）、動作完了でない場合（ｔ
ｉ＜Ｔ）には、ステップＳ１０で「ＮＯ」と判断して次
のステップＳ１１でバッファに格納されている各関節の
角度指令値を駆動回路１２に送信する。この後、ＣＰＵ
１１は、指令動作時間ｔｉにＳＴk 時間を加算して現在
時点をｔｉ時点からＳＴk 時間だけ進めた時点に更新し
（ステップＳ１２）、前述の加算時間量ＳＴkを初期化
するステップＳ４に戻る。以下、ＣＰＵ１１は、上述し
たと同様にして単位制御時間ｔu の経過毎に、加算時間
量ＳＴk 時間後における各関節の仮関節角度指令値を求
め、仮関節駆動トルクが限界値以下ならば、その仮関節
角度指令値を角度指令値として駆動回路１２に与える、
という動作を繰り返す。

【００３６】このようにして駆動回路１２には、単位制
御時間ｔu の経過毎に、各関節についての指令角度が与
えられる。そして、駆動回路１２は、単位制御時間ｔu
をｎ等分してｔu ／ｎ毎の指令角度に分割し、ｔu ／ｎ
毎に指令角度と位置検出回路１３から与えられる現在角
度とを比較してその差に応じた電流を各モータ１０に供
給し、各関節の動作角度を指令角度と一致するように動
作させる。このとき、加算時間量ＳＴk と単位制御時間
ｔu は、同じ時間であるので、各関節の角速度、ひいて
はロボット先端の角速度はステップＳ１で決定された速
度パターンと同等のものとなる。

【００３７】さて、今、ステップＳ８において、一つの
関節或いは複数の関節について仮関節駆動トルクが限界
値を越えたとする。すると、ＣＰＵ１１は、ステップＳ
８で「ＮＯ」と判断し、速度変更比率Ｒを演算するステ
ップＳ１３に移行する。このステップＳ１３で、ＣＰＵ
１１は、各関節について、次の（３−１）および（３−
３）式により示された第ｊ関節の修正角速度、および第
ｊ関節の修正角加速度を、上記（２）式の第ｊ関節の角
速度、および第ｊ関節の角加速度に代入し、駆動トルク
が限界値τimaxを越えないような速度変換比率Ｒを求め
る（速度変換比率演算手段）。

【００３８】

【数３】

【００３９】なお、上記（３−１）および（３−３）式
で示される第ｊ関節の修正角速度、第ｊ関節の修正角加
速度を（２）式の第ｊ関節の角速度、第ｊ関節の角加速
度に代入して整理すると、次の（４−１）式のようにな
る。ただし、Ｃ1 、Ｃ2 およびＣ3 は、（４−２）〜
（４−４）式により示す内容のものである。

【００４０】

【数４】

【００４１】上記のステップＳ１３での演算の結果、求
められる速度変換比率Ｒの解は、次の（５）式となる。

【数５】 このＲの解は数学上はプラスの解とマイナスの解とが存
在するが、マイナスの解は速度変換比率としては有り得
ないので、プラスの解を速度変換比率Ｒの解とする。

【００４２】速度変換比率Ｒを求めた後、ＣＰＵ１１
は、通常の加算時間量であるＳＴ時間に速度変換比率Ｒ
を乗じた時間（ＳＴ・Ｒ）を加算時間量ＳＴk （修正サ
ンプリング時間）として新たに設定する（ステップＳ１
４、サンプリング時間変更手段）。次に、ＣＰＵ１１
は、前述のステップＳ５に戻り、ここで、速度パターン
に基づき、現時点から修正後の加算時間量ＳＴk （＝Ｓ
Ｔ・Ｒ）だけ経過した時点でのロボット先端の位置を演
算する。

【００４３】そして、前に述べたと同様にして各関節の
仮関節角度指令値を計算し（ステップＳ６）、次いで各
関節の仮関節駆動トルクを演算し（ステップＳ７）、演
算した仮関節駆動トルクが限界値以下であるか否かを判
断し（ステップＳ８）、限界値以下であった場合には
（ステップＳ８で「ＹＥＳ」）、ステップＳ９およびス
テップＳ１０を経てステップＳ１１に移行して新たに求
めた仮関節角度指令値を指令角度として駆動回路１２に
与える。

【００４４】この結果、駆動回路１２に与えられる角度
指令値としては、加算時間量ＳＴkがＳＴのままのとき
と比べて現在角度に近い値となり、関節の動作角度は小
さくなる。これに対し、駆動回路１２は、単位制御時間
ｔu を短縮することなく、通常と同じ時間（ＳＴ）で小
さく設定し直された角度指令値に関節を動作させるの
で、関節の角速度、或いは角加速度としては低くなり、
関節の駆動トルクが限界値を越えないようにすることが
できる。

【００４５】このように本実施例では、加算時間量ＳＴ
k の経過毎に、仮関節角度指令値を求めて仮関節駆動ト
ルクを演算し、この仮関節駆動トルクが限界値を越える
場合には、限界値以下となるような角度指令値に修正す
るので、ティーチング時に速度係数、加減速度係数を大
きく設定しておいても、実際の関節の速度や加減速度
は、限界値を越えないように制御される。

【００４６】このため、ティーチング時に、モータ１０
が過負荷状態にならないように配慮しながら速度係数や
加減速度係数を設定する必要がなくなり、ティーチング
作業を容易に行うことができる。また、最高速度Ｖmax
を速くしたり、加減速度を大きくしたりするなど、必要
なときにモータ１０の能力を最大限利用できるようにな
るので、ロボット作業のタクトタイムを短縮することが
できる。

【００４７】しかも、仮関節駆動トルクが限界値を越え
る場合、限界値以下となるように加算時間量ＳＴk が修
正され、この結果、実際の関節の速度や加速度が、速度
パターンから求められる速度や加速度より低くなって
も、各関節は同じ割合で速度や加速度を減じて速度パタ
ーンから求められる角度と同じ角度だけ動作するので、
ロボット先端の軌跡はプログラムされた通りのものとな
る。

【００４８】なお、上記実施例では、仮関節駆動トルク
の限界値をモータ１０の許容最大トルクに設定し、仮関
節駆動トルクがその限界値を越えたとき、限界値以下と
なるように加算時間量ＳＴk を短くして角度指令値を修
正するように構成したが、例えばモータ１０の最大許容
トルクを上限値、その最大許容トルクよりもやや低いト
ルクを下限値として設定し、仮関節駆動トルクが下限値
を下回ったときには、加算時間量ＳＴk を長くして角度
指令値を修正するように構成しても良く、このようにす
れば、一層モータ１０の能力を最大限利用でき、タクト
タイムの短縮化を図ることができる。

【００４９】また、上記実施例では、仮関節駆動トルク
が限界値を越えたとき、速度変更比率Ｒ（Ｒ＜１）を求
め、加算時間量ＳＴk をＳＴ・Ｒに短縮することにより
駆動トルクが限界値以下となるようにしたが、加算時間
量ＳＴは常に一定時間とし、そのかわりに、単位制御時
間ｔu を長くすることにより関節の角速度を低下させて
限界値を越えないように構成しても良い。

【００５０】更には、ロボット先端の軌跡をプログラム
するのではなく、各関節毎に速度パターンを設定するＰ
ＴＰ制御の場合には、各関節毎に速度パターンを設定
し、それら速度パターンに基づいて各関節の仮関節角度
指令値を演算すれば良い。このとき、仮関節駆動トルク
が限界値を越える関節が一つでもあれば、その関節につ
いて速度変更比率を演算し、演算した速度変更比率を全
ての関節について適用して、加算時間量或いは単位制御
時間を修正すれば、各関節ひいてはロボット先端の軌跡
を当初プログラムした軌跡を辿るように制御できる。

【図面の簡単な説明】

【図１】本発明の一実施例を示すフローチャート

【図２】速度パターンおよびその速度パターンでの動作
角度を示す図

【図３】電気回路構成を示すブロック図

【図４】ロボットの斜視図

【符号の説明】

図中、１はロボット本体、２は制御装置、１０はモー
タ、１１はＣＰＵ（位置演算手段、トルク演算手段、サ
ンプリング時間変更手段）、１７はロータリエンコーダ
である。

Claims (2)

【特許請求の範囲】
1. 【請求項１】 速度パターンに基づき、サンプリング時
   間が経過する度に、次のサンプリング時間経過時点にお
   ける関節の位置を演算する位置演算手段と、 前記関節を前記位置演算手段により演算された位置に動
   作させるに必要な駆動トルクを演算するトルク演算手段
   と、 このトルク演算手段により演算された駆動トルクが予め
   設定された限界値を越えるとき、駆動トルクが前記限界
   値を越えないようにするための速度変更比率を演算し、
   この速度変更比率に基づいて通常のサンプリング時間よ
   りも短い修正サンプリング時間を演算するサンプリング
   時間変更手段とを備え、 前記トルク演算手段により演算された駆動トルクが前記
   限界値以下のとき、前記位置演算手段により演算された
   位置を指令位置として、前記関節が所定の単位制御時間
   後にその指令位置まで動作するように制御し、 前記トルク演算手段により演算された駆動トルクが前記
   限界値を越えたとき、前記サンプリング時間変更手段に
   より演算された前記修正サンプリング時間を前記次のサ
   ンプリング時間として前記位置演算手段により再度関節
   の位置を演算し、その位置を指令位置として、前記関節
   が前記所定の単位制御時間後にその指令位置まで動作す
   るように制御する構成としたことを特徴とするロボット
   の制御装置。
2. 【請求項２】 速度パターンに基づき、サンプリング時
   間が経過する度に、次のサンプリング時間経過時点にお
   ける関節の位置を演算する位置演算手段と、 前記関節を前記位置演算手段により演算された位置に動
   作させるに必要な駆動トルクを演算するトルク演算手段
   と、 このトルク演算手段により演算された駆動トルクが予め
   設定された限界値を越えるとき、駆動トルクが前記限界
   値を越えないようにするための速度変更比率を演算する
   速度変更比率演算手段とを備え、 前記トルク演算手段により演算された駆動トルクが前記
   限界値以下のとき、前記位置演算手段により演算された
   位置を指令位置として、前記関節が単位制御時間後にそ
   の指令位置まで移動するように制御し、 前記トルク演算手段により演算された駆動トルクが前記
   限界値を越えたとき、前記速度変更比率演算手段により
   演算された速度変更比率に基づいて前記単位制御時間を
   通常の単位制御時間よりも長く設定し、前記位置演算手
   段により演算された位置を指令位置として、前記関節が
   前記長く設定された単位制御時間後にその指令位置に動
   作するように制御することを特徴とするロボットの制御
   装置。