(57)【要約】【課題】加減速時の軌道が非直線軌道の速度パターンに基づいてモータを駆動する構成において、パラメータの不適によりロボットに振動が生じてしまうことを未然に防止する。【解決手段】ロボットの制御装置は、速度パターンを決定する際に、加速度変化率を求め、加速度変化率が所定の設定値を上回る場合は、設定値以下となるまで速度パターンの動作時間を延長して動作パラメータを最終的に設定する。これにより、速度パターンがサイクロイド軌道速度パターンにおいて、パラメータとして低速度且つ高加速度に設定した場合であっても、ロボットに振動が生じてしまうことを未然に防止することができる。【選択図】　図１
2004025355.tif 000002
【特許請求の範囲】
【請求項１】
与えられたパラメータに基づいてロボットの関節を駆動するモータの速度パターンを求める速度パターン決定手段と、この速度パターン決定手段が求めた速度パターンを形成する動作パラメータを求める動作パラメータ演算手段とを備え、この動作パラメータ演算手段が求めた動作パラメータに基づいて前記モータを駆動するロボットの制御装置において、
前記速度パターン決定手段が求めた速度パターンに基づいて当該速度パターンの加速度変化率を求める加速度変化率演算手段を備え、
前記速度パターン決定手段は、前記加速度変化率演算手段が求めた加速度変化率が所定の設定値を上回る場合は、設定値以下となるまで速度パターンの動作時間を延長することを特徴とするロボットの制御装置。
【請求項２】
前記速度パターン決定手段は、加減速時の軌道としてサイクロイド軌道を設定することを特徴とする請求項１記載のロボットの制御装置。
【請求項３】
前記速度パターン決定手段は、設定値として加速度変化率の最大値を設定することを特徴とする請求項１または２記載のロボットの制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットの関節を駆動するモータの速度パターンに基づいてモータを駆動するロボットの制御装置に関する。
【０００２】
【発明が解決しようとする課題】
多関節型の産業用ロボットは、複数の可動部を有し、それら可動部が関節結合された構造となっている。そして、各可動部（関節）は、駆動源である直流サーボモータなどのモータから適宜の伝動機構を介して駆動トルクを受けて回転動作を行うようになっており、それぞれの可動部の総合動作の結果としてロボット先端がプログラムされた通りの動作を行う。
【０００３】
ここで、関節の回転動作の制御を一つの関節を例にして説明するに、まず、ロボットでは、モータのロータリエンコーダから与えられる信号により関節の現在の角度（位置）を検出し、この現在の角度をフィードバック信号として、現在角度が指令角度に一致するようにモータを制御するという、いわゆるフィードバック制御によって関節の動作を制御するようにしている。
【０００４】
このフィードバック制御を行うために、制御装置は、動作プログラム中から読み出したパラメータ（駆動時間、最高速度、最高加速度など）に基づいて速度パターンを決定し、次いで、その速度パターンから所定のサンプリング時間が経過する毎に、その時点を現在として、現在からサンプリング時間経過後の角度を算出し、これを指令角度とする。そして、関節の現在位置と指令角度とを比較し、サンプリング時間と同じ長さに定められた単位制御時間後に、関節が指令された角度まで動作するようにモータの速度を制御する。この制御により、関節は最初に動作プログラムが定める速度パターンと同様の速度パターンにて動作するようになり、このようなことが各関節の全体について行われることから、ロボット先端がプログラムされた通りの動作を行うのである。
【０００５】
ところで、速度パターンとして加減速時の軌道が例えばサイクロイド軌道となるサイクロイド軌道速度パターンを決定する構成のものがある。このサイクロイド軌道速度パターンに基づいてモータを駆動することにより、台形速度パターンに比較してロボットを円滑に動作させることができる。
【０００６】
しかしながら、サイクロイド軌道速度パターンを実行する場合において、パラメータとして、最高速度を小さく設定する一方で、最高加速度を大きく設定した場合には、モータの加減速の開始或いは終了時の加速度変化率が極端に大きくなることがある。このような場合、モータに作用する負荷が通常よりも大きくなることから、ロボットに振動を生じるようになり、ロボット先端の軌跡精度の悪化を招来するようになる。
【０００７】
本発明は上記事情に鑑みてなされたもので、その目的は、速度パターンに基づいてモータを駆動する構成において、パラメータの不適によりロボットに振動が生じてしまうことを未然に防止することができるロボットの制御装置を提供することにある。
【０００８】
【課題を解決するための手段】
請求項１の発明によれば、ロボットの制御装置にパラメータを与えると、速度パターン決定手段がパラメータに基づいて速度パターンを演算する。すると、加速度変化率演算手段は、速度パターン決定手段が求めた速度パターンに基づいて当該速度パターンの加速度変化率を求める。
【０００９】
ここで、通常においては、加速度変化率演算手段が求めた変化率は所定の設定値よりも小さいので、速度パターン決定手段が求めた速度パターンに基づいて動作パラメータ演算手段が動作パラメータを求め、その動作パラメータに基づいてモータを駆動することによりロボットを適切に制御することができる。
【００１０】
ところで、パラメータとして低速度且つ高加速度に設定すると、加速度変化率が極端に大きくなり、ロボットに振動が生じることがあるものの、このような場合は、加速度変化率演算手段が求めた加速度変化率が所定の設定値を上回るようになるので、速度パターン決定手段は、設定値以下となるまでモータに対する駆動時間を延長する。これにより、低速度且つ高加速度に設定するにしても、ロボットに振動が生じてしまうことを未然に防止できる。
【００１１】
【発明の実施の形態】
以下、本発明の一実施の形態を図面に基づいて説明する。図４は、ロボットの斜視図である。この図４において、ロボットは、ロボット本体１と、このロボット本体１を制御する制御装置（速度パターン決定手段、動作パラメータ演算手段、加速度変化率演算手段に相当）と、ティーチングペンダント３を備えている。この実施の形態において、制御装置２の制御対象であるロボット本体１は、例えば垂直多関節型の組み立て用ロボットとして構成され、ベース４と、このベース４に水平方向に旋回可能に設けられたショルダ部５と、このショルダ部５に上下方向に旋回可能に設けられた下アーム６と、この下アーム６に上下方向に旋回可能に且つ捻り回転可能に設けられた上アーム７と、この上アーム７に上下方向に旋回可能に設けられた手首８とを備えており、手首８は先端部に捻り回転可能なフランジ９を備えている。なお、図示はしないが、ワークを把持するハンド（図示せず）は、フランジ９に取り付けられるようになっている。
【００１２】
そして、ショルダ部５の水平方向の旋回動作、下アーム６の上下方向の旋回動作、上アーム７の上下方向の旋回動作、上アーム７の回転動作、手首８の上下方向の旋回動作、フランジ９の回転動作は、それぞれ関節が、駆動源である例えば直流サーボモータからなるモータ１０（図３参照）により図示しない適宜の伝動機構を介して回転駆動されることにより行われる。なお、図３では、ショルダ部５、下アーム６、上アーム７、手首８およびフランジ９を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ１０も一台だけ示した。
【００１３】
一方、図３に示すように、制御装置２は、制御部としてのＣＰＵ１１、各関節のモータ１０を駆動する駆動手段としての駆動回路１２、位置検出手段としての位置検出回路１３などを備えている。そして、ＣＰＵ１１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ１４、ロボット本体１の動作プログラム等を記憶したＲＡＭ１５および前記ティーチングペンダント３を接続するためのインターフェース１６が接続されている。
【００１４】
上記位置検出回路１３は、各関節の現在位置（回転角度）を検出するためのもので、この位置検出回路１３には、各関節を駆動するモータ１０に設けられた位置検出センサとしてのロータリエンコーダ１７が接続されている。ロータリエンコーダ１７は、各モータ１０の回転角度に応じたパルス信号を出力し、そのパルス信号は位置検出回路１３に与えられる。位置検出回路１３は、各ロータリエンコーダ１７からのパルス信号に基づいて各モータ１０ひいては各関節の現在の角度位置を検出し、その位置情報は、各モータ１０の駆動回路１２に与えられると共に、ＣＰＵ１１に与えられるようになっている。
【００１５】
そして、各駆動回路１２は、ＣＰＵ１１から与えられる指令角度と位置検出回路１３から与えられる現在角度とを比較し、その偏差に応じた電流を各モータ１０に供給してそれらを駆動する。これにより、ロボット先端であるフランジ９の中心部が動作プログラムにより定められた通りの軌跡を辿って動作し、部品の組み立て作業などを行うものである。
【００１６】
さて、動作プログラムには、一つの動作毎に動作終了位置、速度係数、加減速度係数などのパラメータが記録されている。このうち、速度係数および加減速度係数とは、動作の最高速度および加減速度をモータ１０の許容最大速度および許容最大加減速度に対する割合で定めたもので、許容最大速度および許容最大加減速度は、モータ１０の負荷トルクが例えば許容最大トルクを越えることのないように、各モータ１０毎にその性能を考慮して設定されている。
【００１７】
ＣＰＵ１１は、上記の動作プログラムに記録されたパラメータから速度パターンを例えば台形速度パターンに当て嵌めて決定し、その台形速度パターンに基づいて各関節について経過時間毎の角度を演算し、これを角度指令値として駆動回路１２に与えるようになっている。すなわち、標準的な速度パターンである台形速度パターンは、図２（ａ）に示すように、加速過程、最高速度での等速過程、減速過程からなるが、この台形速度パターンとなるような動作角度のパターンは図２（ｂ）に示すようになる。
【００１８】
図２（ｂ）に示す角度パターンにおいて、ｔｎ時点での関節の角度をθｎ、このｔｎ時点からΔｔ時間後のｔ（ｎ＋１）時点での角度をθ（ｎ＋１）とすると、次式で示すように、Δｔ時間での角度の変化がｔ（ｎ＋１）時点での関節の角速度Ｖ（ｎ＋１）となる。
Ｖ（ｎ＋１）　・Δｔ＝θ（ｎ＋１）−θｎ
【００１９】
上の式から、θ（ｎ＋１）＝θｎ＋Ｖ（ｎ＋１）・Δｔ　となるので、ｔｎ時点からΔｔ時間後の関節の角度θ（ｎ＋１）は、ｔｎ時点の角度θｎに、速度パターンから得られるｔ（ｎ＋１）時点の角速度Ｖ（ｎ＋１）にΔｔを乗じた値を加算することによって求めることができる。従って、速度パターンに基づいて、動作開始時点から所定のサンプリング時間（Δｔ）経過毎に、その経過時点を現在として次のサンプリング時点（Δｔ経過後に相当する時点）の速度を算出し、これにサンプリング時間を乗じた値を順次加算してゆけば、動作開始時点から動作終了時点までの間、サンプリング時間経過毎の関節の角度を順次求めることができ、それを角度指令値として駆動回路１２に与えれば速度パターン通りの動作を関節に行わせることができるものである。
【００２０】
本実施の形態のロボットは、ロボット先端の動作軌跡を動作プログラムに設定し、ロボット先端を設定された軌跡に従って動作させるように各関節を制御するようにしている。そして、ＣＰＵ１１は、一つの動作を制御する場合、動作プログラムに記録されているパラメータに基づいて速度パターンを決定し、この速度パターンに基づいてサンプリング時点毎に次のサンプリング時点におけるロボット先端の座標位置を求め、次いで、ロボット先端の座標位置から各関節の角度を求めるようにしている。
【００２１】
以下、制御の内容を図１のフローチャートを参照しながら説明する。ここで、以下の説明では、サンプリング時間（Δｔ）を加算時間量ＳＴｋ、動作開始時点から各サンプリング時点までの経過時間を指令動作時間ｔｉとし、関節（モータ１０）を現在の角度から角度指令値まで動作させる制御を行うための時間を単位制御時間ｔｕということとする。上記加算時間量ＳＴｋは、通常は一定の時間に定められているが、後述のように、仮に定められた角度指令値に動作させるために必要とする駆動トルクが、例えばモータ１０の最大許容トルクに等しく設定された限界値を越えると予測される場合には、通常時よりも短い時間に修正されるようになっている。これに対し、単位制御時間ｔｕは、常に一定で変化することはない。
【００２２】
しかして、ＣＰＵ１１は、一つの動作に対する制御を開始すると、まず、動作プログラム中から動作終了位置の座標、速度および加速度の両係数などのパラメータを読み出し、それらパラメータから速度パターンを決定する（ステップＳ１、速度パターン決定手段）。
【００２３】
速度パターンは、動作開始から終了までのロボット先端の移動量Ｌ、加速度α、最高速度Ｖｍａｘ　、減速度βに基づいて、図５（ａ）に示すサイクロイド軌道速度パターンに当て嵌めて決定される。このサイクロイド軌道速度パターンは、例えばＶ（ｔ）＝（Ｖｃ／２）・（１−ｃｏｓ（πｔ／ｔ１））という演算式に基づいて設定されており、台形速度パターンに比較して、モータ１０の加減速を円滑に行うことができる。また、加速度αと減速度βは、モータ１０の最大許容加速度および最大許容減速度に加減速度係数を乗じて求められ、最高速度Ｖｍａｘ　は、モータ１０の許容最大速度に速度係数を乗じて求められ、また、移動距離Ｌは、パラメータの一つとして動作プログラムから得た動作終了位置と位置検出回路１３の位置情報から求められる現在位置（動作開始時の位置）から演算される。
【００２４】
ところで、サイクロイド軌道速度パターンにおいて、パラメータとして低速度且つ高加速度に設定したときは、図５（ｃ）に示すように加速度変化率が加速開始時あるいは加速終了時に過大となり、ロボットの動作時に振動が発生する虞がある。
【００２５】
そこで、本実施の形態では、図１に示すように速度パターンに基づいて加速度変化率を求め、その加速度変化率が所定の設定値を上回ると判断したときは（Ｓ３：ＮＯ）、速度パターンの動作所要時間をΔＴだけ延長し（Ｓ１０）、加速度変化率を再度演算する。この場合、所定の設定値としては、ロボットの仕様に応じて異なり、本実施の形態のロボットでは、最高速度Ｖｍａｘ、最高加速度αｍａｘ設定時の加速度変化率の最大値（π２／２）・（αｍａｘ／Ｖｍａｘ）を所定の設定値として設定することにより良好な結果を得ることができた。
そして、速度パターンを形成する動作パラメータのうち動作所要時間を延長した結果、速度パターンの加速度変化率が所定の設定値以下となったときは、そのときの速度パターンを真の速度パターンとして設定する。
【００２６】
速度パターンを決定すると、ＣＰＵ１１は、動作パラメータを演算する（ステップＳ２、動作パラメータ演算手段）。動作パラメータとは、速度パターンを形成するパラメータで、加速度過程の所要時間ｔ１　、減速過程の所要時間ｔ２　、動作開始から終了までの所要時間Ｔおよび最高速度Ｖｍａｘ　である。なお、最高速度Ｖｍａｘ　以外の動作パラメータは、次式にて求められる。
ｔ１＝Ｖｍａｘ／α
ｔ２＝Ｖｍａｘ／β
Ｔ＝（Ｌ／Ｖｍａｘ）＋Ｖｍａｘ・（１／α＋１／β）／２
【００２７】
次に、ＣＰＵ１１は、上述のようにして求めた速度パターンにおいて、指令動作時間ｔｉを初期化し、「ｔｉ＝０」に設定する（ステップＳ３）。この初期化により、図２（ａ）の速度パターンの時間軸において、現在時点が原点（動作開始時点）に設定される。この後、ＣＰＵ１１は、加算時間量ＳＴｋを一定時間ＳＴに設定する初期化を実行する（ステップＳ４）。
【００２８】
次に、ＣＰＵ１１は、ステップＳ１で決定した速度パターンの動作パラメータに基づき、現在時点ｔｉから加算時間量ＳＴｋ経過後におけるロボット先端の位置を演算する（ステップＳ６）。ここで、ロボット先端の動作開始位置を三次元の直交座標（Ｘｓ，Ｙｓ，Ｚｓ）および直交座標の３軸に対する傾き角（αｓ，βｓ，γｓ）、動作終了位置を同じく（Ｘｅ，Ｙｅ，Ｚｅ）および（αｅ，βｅ，γｅ）で表すと共に、ロボット先端の最高速度Ｖｍａｘ　を三次元直交座標の３軸方向の速度成分（Ｖｍｘ，Ｖｍｙ，Ｖｍｚ）および直交座標の３軸を中心とする回転速度成分（Ｖｍα，Ｖｍβ，Ｖｍγ）で表した場合、ｔ時点でのロボット先端の位置を、三次元の直交座標（Ｘｔ，Ｙｔ，Ｚｔ）および三次元の直交座標の３軸に対する傾き角（αｔ，βｔ，γｔ）で示すと、次の（１−１）式〜（１−３）式のようになる。なお、（１−１）式〜（１−３）式はＸ座標についてだけを示しているが、Ｙｔ，Ｚｔおよびαｔ，βｔ，γｔも同様にして求めることができるものである。
【００２９】
【数１】
2004025355.tif 000003
【００３０】
ＣＰＵ１１は、このようにして現在時点ｔｉから加算時間量ＳＴｋ経過した時点におけるロボット先端の位置を演算した後、そのロボット先端の位置から各関節の（ｔｉ＋ＳＴｋ）時間における位置を求め、これを各関節についての仮関節角度指令値とする（ステップＳ７）。
次いで、ＣＰＵ１１は、上記仮関節角度指令値を各関節の角度指令値としてＣＰＵ１１が有するアキュームレータなどのバッファに転送する（ステップＳ８）。
【００３１】
そして、ＣＰＵ１１は、動作完了か否か、すなわち指令動作時間ｔｉがＴ時間以上になったか否かを判断し（ステップＳ９）、動作完了でない場合（ｔｉ＜Ｔ）には、ステップＳ９で「ＮＯ」と判断して次のステップＳ１１でバッファに格納されている各関節の角度指令値を駆動回路１２に送信する。この後、ＣＰＵ１１は、指令動作時間ｔｉにＳＴｋ時間を加算して現在時点をｔｉ時点からＳＴｋ時間だけ進めた時点に更新し（ステップＳ１２）、前述の加算時間量ＳＴｋ　を初期化するステップＳ５に戻る。以下、ＣＰＵ１１は、上述したと同様にして単位制御時間ｔｕの経過毎に、加算時間量ＳＴｋ時間後における各関節の仮関節角度指令値を求め、仮関節駆動トルクが限界値以下ならば、その仮関節角度指令値を角度指令値として駆動回路１２に与える、という動作を繰り返す。
【００３２】
このようにして駆動回路１２には、単位制御時間ｔｕの経過毎に、各関節についての指令角度が与えられる。そして、駆動回路１２は、単位制御時間ｔｕをｎ等分してｔｕ／ｎ毎の指令角度に分割し、ｔｕ／ｎ毎に指令角度と位置検出回路１３から与えられる現在角度とを比較してその差に応じた電流を各モータ１０に供給し、各関節の動作角度を指令角度と一致するように動作させる。このとき、加算時間量ＳＴｋ　と単位制御時間ｔｕは、同じ時間であるので、各関節の角速度、ひいてはロボット先端の角速度はステップＳ１で決定された速度パターンと同等のものとなる。
【００３３】
このように本実施の形態では、サイクロイド軌道速度パターンにおいて、パラメータを低速度且つ高速度に設定した場合であっても、加速度変化率が所定の設定値を上回るようなときは、速度パターンの動作時間を延長するようにしたので、ロボットが振動してしまうことを未然に防止できる。
【００３４】
本発明は、上記実施の形態に限定されるものではなく、本発明を適用する速度パターンとしては、サイクロイド軌道速度パターンに代えてサイン波形軌道速度パターンに適用するようにしてもよい。
【図面の簡単な説明】
【図１】本発明の一実施の形態における制御装置の動作を示すフローチャート
【図２】台形速度パターンの一例を示す図
【図３】制御装置のブロック図
【図４】ロボットの斜視図
【図５】速度パターンの補正結果を示す図
【符号の説明】
１はロボット本体、２は制御装置（速度パターン決定手段、動作パラメータ演算手段、加速度変化率演算手段）である。
【図１】
2004025355.tif 000004
【図２】
2004025355.tif 000005
【図３】
2004025355.tif 000006
【図４】
2004025355.tif 000007
【図５】
2004025355.tif 000008