ロボットの制御装置
【課題】ロボットをより高速で動作させるための制御パターンを取得する制御装置において、その実用性を向上させる。
【解決手段】制御装置２０は、ロボットのアームを第１姿勢と第２姿勢との間で駆動して、各サーボモータ３１において速度、加速度、及び供給される電流の各連続値を検出する。各モータ３１において所定の期間毎に、上限値に対する電流値の比率である電流比を算出する。所定の期間毎に、各モータ３１の電流比のうち最大電流比を選択し、この最大電流比の逆数に基づいて各モータ３１の修正加速度を算出する。修正加速度と各モータ３１の速度とに基づいて、各モータ３１における修正速度を繰り返し算出する。修正速度と各モータ３１の回転位置とに基づいて、各モータ３１における修正回転位置を繰り返し算出する。第１姿勢側から算出された修正速度と、第２姿勢側から算出された修正速度とに基づいて、修正後の速度変動パターンを設定する。
【解決手段】制御装置２０は、ロボットのアームを第１姿勢と第２姿勢との間で駆動して、各サーボモータ３１において速度、加速度、及び供給される電流の各連続値を検出する。各モータ３１において所定の期間毎に、上限値に対する電流値の比率である電流比を算出する。所定の期間毎に、各モータ３１の電流比のうち最大電流比を選択し、この最大電流比の逆数に基づいて各モータ３１の修正加速度を算出する。修正加速度と各モータ３１の速度とに基づいて、各モータ３１における修正速度を繰り返し算出する。修正速度と各モータ３１の回転位置とに基づいて、各モータ３１における修正回転位置を繰り返し算出する。第１姿勢側から算出された修正速度と、第２姿勢側から算出された修正速度とに基づいて、修正後の速度変動パターンを設定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置に関する。
【背景技術】
【０００２】
従来、ロボットの動作によって発生する負荷トルクを、他軸からの干渉トルクも含めて全軸についてラグランジェ方程式を用いて評価して、最適な加減速時間を決定するものがある（例えば、特許文献１参照）。この特許文献１に記載のものでは、ロボットの各軸の質量、重心位置からなるパラメータと、教示された各駆動軸の位置、動作方向、及び動作速度とに基づいて、各軸に発生する負荷トルクを演算している。そして、発生する負荷トルクが許容ピークトルクに等しくなるように加減速時間を決定している。その結果、負荷トルクがピークトルクを越えない範囲で、最短の加減速時間とすることができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−２６１８２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、特許文献１に記載のものでは、加減速時間を決定するために、ロボットの各軸の質量、重心位置等のパラメータが必要となる。このため、これらのパラメータが不明である場合には、加減速時間を決定することができない。また、これらのパラメータを求めることは必ずしも容易ではなく、ロボットの扱うワーク（対象物）によってパラメータが変化するといった問題もある。
【０００５】
本発明は、こうした実情に鑑みてなされたものであり、ロボットをより高速で動作させるための制御パターンを取得する制御装置において、その実用性を向上させることを主たる目的とするものである。
【課題を解決するための手段】
【０００６】
本発明は、上記課題を解決するために、以下の手段を採用した。
【０００７】
第１の発明は、アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定する修正速度変動パターン設定手段と、を備えることを特徴とする。
【０００８】
上記構成によれば、ロボットはアームを駆動するサーボモータを各関節に備えており、アームが第１姿勢と第２姿勢との間で駆動されるとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流（各種パラメータ）の各連続値が検出される。例えば、ロボットのティーチングにおいて、第１姿勢と第２姿勢とがティーチングされ、第１姿勢及び第２姿勢の一方から他方への駆動中に、各サーボモータにおいて上記各種パラメータの各連続値が検出される。
【０００９】
そして、各サーボモータにおいて、検出された電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比が算出される。この電流比の逆数は、電流値を上限値まで大きくする際の倍率であり、各サーボモータでは電流値をこの電流比の逆数倍まで大きくすることができる。ここで、各サーボモータの加速度は、各サーボモータの発生するトルクに比例し、この発生トルクは各サーボモータに供給される電流に比例する。このため、各サーボモータの加速度は各サーボモータに供給される電流に比例することとなり、電流値の倍率に基づいて修正後の加速度を算出することができる。
【００１０】
この点、所定の期間毎に、算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比が選択され、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度が算出される。ここで、最大電流比の逆数は、電流比の逆数のうちで最も小さな値である。このため、各サーボモータの加速度を同じ比率で大きくする際に、各サーボモータにおいて電流値が上限値を超えることを抑制しつつ、各サーボモータの修正加速度を最大限大きくすることができる。なお、一般にロボットの制御では、各関節の駆動が同時に終了するように、各サーボモータの加速度等が制御される。このため、各関節の加速度等を変更する場合には、全ての関節の加速度等を同じ比率で変更する必要がある。
【００１１】
さらに、第１姿勢及び第２姿勢をそれぞれ開始時点として、上記修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける所定の期間後の速度である修正速度が繰り返し算出される。すなわち、第１姿勢を開始時点として各サーボモータにおいて、その時点の修正加速度とその修正加速度に対応する速度とに基づいて、所定の期間後の速度である修正速度が算出され、更に所定の期間後の修正加速度と算出された修正速度とに基づいて、その更に所定の期間後の修正速度が算出されるといったことが繰り返される。また、第２姿勢を開始時点としても、同様に修正速度が繰り返し算出される。このようにして、各サーボモータにおいて、加速度の修正に伴って速度が修正される。
【００１２】
また、同様にして、第１姿勢及び第２姿勢をそれぞれ開始時点として、上記修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける所定の期間後の回転位置である修正回転位置が繰り返し算出される。すなわち、各サーボモータにおいて、速度の修正に伴って回転位置が修正される。そして、第１姿勢を開始時点として繰り返し算出される修正回転位置と、第２姿勢を開始時点として繰り返し算出される修正回転位置とが、やがて同一の回転位置まで到達して重なることとなる。その場合には、第１姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正回転位置と、第２姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正回転位置とが算出されている。また、その場合には同様にして、第１姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正速度，修正加速度と、第２姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正速度，修正加速度とが算出されている。
【００１３】
ここで、例えば第１姿勢を開始時点として第２姿勢まで順に修正速度を算出した場合には、修正速度にて減速を開始する時点についての情報がないため、修正回転位置が第２姿勢まで到達した時に修正速度が０にならない。このため、第２姿勢において未だ速度が出ている状態となり、ロボットのアームが第２姿勢を越えて駆動されるおそれがある。そして、第２姿勢において修正速度を０にするためには、サーボモータに限界を超える大きな電流の供給を要求する必要があり、電流値が上限値を超えることとなったり、修正速度が０になるように作業者が修正を加える必要が生じたりする。これに対して、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度を算出した場合には、第２姿勢に到達した時点で修正速度を０にすることができる。すなわち、第２姿勢で速度が０になる状態を出発点として減速の計算を行うことにより、第２姿勢で必ず速度が０となる修正速度変動パターンを作成することができる。
【００１４】
このとき、第２姿勢を開始時点として第１姿勢へ向けて加速する際の修正速度は、第１姿勢側から第２姿勢で停止する場合に、第２姿勢へ向けて減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で駆動する際に各サーボモータに供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータに供給される電流の大きさと等しいと考えることができる。したがって、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度を算出することにより、電流値が上限値を超えない範囲で適切に修正速度を算出することができる。
【００１５】
以上により、第１姿勢を開始時点として回転位置が重なるまでに繰り返し算出された修正速度と、第２姿勢を開始時点として回転位置が重なるまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを適切に設定することができる。その結果、アームを第１姿勢と第２姿勢との間で駆動することにより、各サーボモータにおける速度、加速度、及び供給される電流の各連続値以外のパラメータを必要とすることなく、ロボットをより高速で動作させるための速度変動パターンを取得することができる。
【００１６】
また、第２の発明では、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度と、前記第２姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度とに基づいて、修正後の加速度変動パターンを設定する修正加速度変動パターン設定手段を備えている。こうした構成によれば、第１の発明と同様にして、ロボットの各軸の質量や重心位置等のパラメータを必要とすることなく、各サーボモータにおける速度、加速度、及び供給される電流の各連続値のみにより、ロボットをより高速で動作させるための加速度変動パターンを取得することができる。
【００１７】
また、第３の発明では、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置とに基づいて、修正後の回転位置変動パターンを設定する修正回転位置変動パターン設定手段を備えている。こうした構成によれば、第１の発明と同様にして、ロボットの各軸の質量や重心位置等のパラメータを必要とすることなく、各サーボモータにおける速度、加速度、及び供給される電流の各連続値のみにより、ロボットをより高速で動作させるための回転位置変動パターンを取得することができる。
【００１８】
第１姿勢及び第２姿勢をそれぞれ開始時点として、サーボモータにおける所定の期間後の修正速度を同じ回数だけ繰り返し算出した場合は、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とに差が生じることがある。この場合には、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点で、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とに差が生じているおそれがある。このため、第１姿勢を開始時点として修正回転位置が重なる時点までに繰り返し算出された修正速度と、第２姿勢を開始時点として修正回転位置が重なる時点までに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定した場合に、その速度変動パターンに段差が生じるおそれがある。
【００１９】
この点、第４の発明では、第１〜第３のいずれかの発明において、前記修正速度算出手段は、前記第１姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度とを比較して、低い方の前記修正速度について優先的に前記所定の期間後の修正速度を算出するといった構成を採用している。
【００２０】
上記構成によれば、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とにおいて、低い方の修正速度について優先的に所定の期間後の修正速度が算出されるため、それらの修正速度の差を縮めることができる。したがって、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点で、第１姿勢を開始時点として算出された修正速度と、第２姿勢を開始時点として算出された修正速度との差を小さくすることができる。その結果、修正後の速度変動パターンに段差が生じることを抑制することができる。
【００２１】
さらに、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度との差が小さい状態を維持しつつ、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点まで修正速度が算出される。このため、必要な部分についてのみ修正速度が算出されるようになり、計算負荷が軽減されることで処理を高速化することができる。
【００２２】
ロボットのアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンが設定された場合であっても、実際のアームの駆動に対してそれらの変動パターンが最適であるとは限らず、変動パターンをより最適化する余地がある。
【００２３】
この点、第５の発明では、前記修正後の速度変動パターン、前記修正後の加速度変動パターン、及び前記修正後の回転位置変動パターンの少なくとも１つが設定された後に、前記連続値検出手段は、その設定された修正後の変動パターンとなるように、再度、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出するといった構成を採用している。
【００２４】
上記構成によれば、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転位置変動パターンの少なくとも１つが設定された後に、連続値検出手段により、再度、アームが第１姿勢と第２姿勢との間で駆動されるとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値が検出される。このため、その各連続地に基づいて、電流比算出手段、修正加速度算出手段、修正速度算出手段、及び修正回転位置算出手段により、再度、電流比、修正加速度、修正速度、及び修正回転位置がそれぞれ算出される。したがって、一度設定された修正後の変動パターンに基づいて、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンを、より最適化することができる。
【００２５】
電流比（上限値に対する電流値の比率）の逆数をサーボモータの対応する加速度に掛けることにより、そのサーボモータでは、その加速度を実現する際に電流値がその上限値に一致することとなる。しかしながら、上述したようにロボットのアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、電流値がその上限値に一致するように各変動パターンを設定すると、アームを実際に駆動する際に電流値が上限値を超えるおそれがある。
【００２６】
この点、第６の発明では、第１〜第５のいずれかの発明において、前記修正加速度算出手段は、前記最大電流比の逆数を各サーボモータの対応する加速度に掛けた値よりも所定度合小さくなるように前記修正加速度を算出するといった構成を採用している。したがって、電流の上限値に相当する修正加速度よりも所定度合小さい修正加速度を算出することができ、アームを実際に駆動する際に電流値が上限値を超えることを抑制することができる。
【００２７】
さらに、第５の発明と第６の発明とを組み合わせることにより、修正後の変動パターンによりアームを実際に駆動する際に、電流値が上限値を超えることを抑制しつつ、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンを、より最適化することができる。
【図面の簡単な説明】
【００２８】
【図１】ロボットの概要を示す正面図。
【図２】ロボットの制御装置及びその周辺構成を示す回路図。
【図３】第１回転部の目標回転角度と速度変動パターンとを示すタイムチャート。
【図４】第２回転部の目標回転角度と速度変動パターンとを示すタイムチャート。
【図５】速度変動パターンの設定態様を示すタイムチャート。
【図６】修正変動パターン設定制御の処理手順を示すフローチャート。
【図７】同じく修正変動パターン設定制御の処理手順を示すフローチャート。
【図８】各種パラメータの連続値と電流比とを示すタイムチャート。
【図９】修正加速度、修正速度及び修正回転角度を算出する態様を示す拡大図。
【図１０】各種パラメータの連続値と電流比とを示すタイムチャート。
【図１１】修正加速度、修正速度及び修正回転角度を算出する態様を示す拡大図。
【図１２】回転角度及び修正回転角度の変動パターンを示すタイムチャート。
【図１３】速度及び修正速度の変動パターンを示すタイムチャート。
【図１４】修正速度変動パターン及び修正回転角度変動パターンを設定する態様を示すタイムチャート。
【図１５】最適化前と最適化後の各種パラメータの変動パターンを示すタイムチャート。
【図１６】電流比が最大となるサーボモータが途中で替わる例を示す模式図。
【図１７】最適化前と最適化後の各種パラメータの変動パターンを示すタイムチャート。
【発明を実施するための形態】
【００２９】
以下、垂直多関節型ロボットに具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。
【００３０】
はじめに、ロボット１０の概要を図１に基づいて説明する。
【００３１】
同図に示すように、ロボット１０は、各関節の回転中心軸線として、第１軸線Ｊ１、第２軸線Ｊ２、第３軸線Ｊ３、第４軸線Ｊ４、第５軸線Ｊ５、第６軸線Ｊ６を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、モータ駆動により原点位置を基準として各部が動作する。なお、各関節には、各軸線を中心として各部を滑らかに回転可能とする軸受け（軸受け手段）が設けられている。
【００３２】
ロボット１０は、床に設置されており、第１軸線Ｊ１が鉛直方向へ延びている。ロボット１０において、基台１１は、床等に固定される固定部１２と、その固定部１２の上方に設けられる回転部１３（第１回転部）とを有しており、回転部１３が第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。
【００３３】
下アーム１５（第２回転部）が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム１５は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。下アーム１５は、基本姿勢として鉛直方向に延びる向きに設けられている。
【００３４】
下アーム１５の上端部には、上アーム１６が、水平方向に延びる第３軸線Ｊ３を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム１６は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、下アーム１５により第３軸線Ｊ３を中心として回転可能に支持されている。上アーム１６は、基本姿勢として水平方向に延びる向きに設けられている。
【００３５】
上アーム１６は、基端側と先端側とで２つのアーム部に分割されて構成されており、基端側は第１上アーム１６Ａ（第３回転部）、先端側は第２上アーム１６Ｂ（第４回転部）となっている。第２上アーム１６Ｂは、上アーム１６の長手方向に延びる第４軸線Ｊ４を回転中心として、第１上アーム１６Ａに対してねじり方向に回転可能になっている。すなわち、第２上アーム１６Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第１上アーム１６Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。
【００３６】
上アーム１６（詳しくは第２上アーム１６Ｂ）の先端部には、手首部１７（第５回転部）が設けられている。手首部１７は、水平方向に延びる第５軸線Ｊ５を回転中心として、第２上アーム１６Ｂに対して回転可能になっている。すなわち、手首部１７は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。
【００３７】
手首部１７の先端部には、ワークやツール等を取り付けるためのハンド部１８（第６回転部）が設けられている。ハンド部１８は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部１８は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、手首部１７により第６軸線Ｊ６を中心として回転可能に支持されている。
【００３８】
図２に、ロボット１０の制御装置２０及びその周辺構成を示す。
【００３９】
制御装置２０は、ロボット１０の動作を制御するものであり、ＣＰＵ２１、ＲＯＭ２２、ＲＡＭ２３、駆動回路２５、及び位置検出回路２６等を備えている。これらは互いに電気的に接続されている。ＲＯＭ２２は、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭ２３は、これらのプログラムを実行する際にパラメータの値等を記憶する。ＣＰＵ２１は、これらのＲＯＭ２２やＲＡＭ２３の記憶内容に基づいて、各種プログラムを実行することにより、ロボット１０の動作を制御する。
【００４０】
ロボット１０において、各軸線Ｊ１〜Ｊ６を回転中心軸とした各関節には、それぞれ前段部側にサーボモータ３１が設けられている。ＣＰＵ２１は、駆動回路２５を通じて、各サーボモータ３１の駆動を行う。これにより、各関節において、前段部に設けられたサーボモータ３１の駆動により、後段部がそれぞれ回転動作する。
【００４１】
各サーボモータ３１には、その出力軸を制動する非励磁作動型の電磁ブレーキ３３と、出力軸の回転角度（回転位置）に応じたパルス信号を出力するエンコーダ３２とがそれぞれ設けられている。
【００４２】
各電磁ブレーキ３３は、ばね等の弾性力に基づきサーボモータ３１の出力軸の制動を行い、励磁コイルへの電力供給に基づき出力軸の制動を解除する。ＣＰＵ２１は、駆動回路２５を通じて、各電磁ブレーキ３３の駆動を行う。ＣＰＵ２１は、各電磁ブレーキ３３による出力軸の制動を解除した状態において各サーボモータ３１の駆動を行う。
【００４３】
各エンコーダ３２は、所定パターンに形成された回転子の回転を磁気的又は光学的に検出する検出素子と、その検出素子の信号を処理するＩＣとを有している。位置検出回路２６には、各エンコーダ３２の検出信号がそれぞれ入力される。位置検出回路２６は、各エンコーダ３２の検出信号に基づいて、回転部１３、下アーム１５、第１上アーム１６Ａ、第２上アーム１６Ｂ、手首部１７、及びハンド部１８の各位置（各角度）を検出する。
【００４４】
具体的には、位置検出回路２６は、固定部１２に対する回転部１３の回転角度（回転位置）、回転部１３に対する下アーム１５の回転角度、下アーム１５に対する第１上アーム１６Ａの回転角度、第１上アーム１６Ａに対する第２上アーム１６Ｂの回転角度、第２上アーム１６Ｂに対する手首部１７の回転角度、手首部１７に対するハンド部１８の回転角度をそれぞれ検出し、それら検出した位置情報をＣＰＵ２１に出力する。位置検出回路２６は、上記の各回転角度を０．１°単位で検出可能である。
【００４５】
そして、ＣＰＵ２１は、動作プログラムを実行することにより、位置検出回路２６から入力される位置情報に基づいて、ロボット１０の上記各部の動作をフィードバック制御する。また、ＣＰＵ２１は、後述する修正変動パターン設定制御を実行する。
（ティーチング）
修正変動パターン設定制御に先立って、ロボット１０に対して動作（アームを移動させる軌跡）のティーチングを行う。このティーチングでは、作業者が、ロボット１０に作業を行わせるべく、ロボット１０のハンド部１８を移動させる各動作点（各移動先）を教示する。さらに、作業者は、ハンド部１８を移動させる際の軌跡の形状、上限速度等を設定する。制御装置２０は、この教示内容及び設定に従って速度変動パターン（各種パラメータ）を作成し、その作成した速度変動パターンによりロボット１０を動作させる。作業者は、このロボット１０の動作結果を評価して、必要に応じて上記設定を変更する。
【００４６】
図３〜５を参照して、ロボットの２つの回転部を例として、各回転部の速度変動パターンを設定する手順について説明する。なお、図３（ａ）は第１回転部の目標回転角度を示しており、図３（ｂ）はそれに対する第１回転部の速度変動パターンを示している。図４（ａ）は第２回転部の目標回転角度を示しており、図４（ｂ）はそれに対する第２回転部の速度変動パターンを示している。図５（ａ）は主回転部の速度変動パターンを示しており、図５（ｂ）はそれ以外の回転部の速度変動パターンを設定する態様を示している。
【００４７】
まず、図３（ａ），図４（ａ）に示すように、各回転部の目標回転角度を設定する。ここでは、各回転部を原点（０）から回転させる場合を考え、第１回転部の目標回転角度を回転角度θ１とし、第２回転部の目標回転角度を回転角度θ２とする。回転角度θ１は、回転角度θ２よりも大きく設定されている（θ１＞θ２）。
【００４８】
ロボット１０では、各回転部の回転角度を変更する場合について、標準的な速度変動パターンが設定されており、この情報が制御装置２０のＲＯＭ２２に記憶されている。このため、ＣＰＵ２１は、この情報に基づいて、各回転部の目標回転角度に対して速度変動パターンをそれぞれ設定する。このようにして設定した各回転部の速度変動パターンを、図３（ｂ），図４（ｂ）にそれぞれ示す。
【００４９】
図３（ｂ）に示すように、第１回転部では、速度Ｖ１まで加速時間Ｔａ１で加速した後、速度Ｖ１で等速時間Ｔｃ１移動し、速度０まで減速時間Ｔｄ１で減速している。また、図４（ｂ）に示すように、第２回転部では、速度Ｖ２まで加速時間Ｔａ2で加速した後、速度Ｖ２で等速時間Ｔｃ２移動し、速度０まで減速時間Ｔｄ２で減速している。第１回転部が回転を開始してから回転を終了するまでの時間（時間Ｔａ１＋時間Ｔｃ１＋時間Ｔｄ１）は、第２回転部が回転を開始してから回転を終了するまでの時間（時間Ｔａ２＋時間Ｔｃ２＋時間Ｔｄ２）よりも長くなっている。
【００５０】
目標回転角度までの回転に最も時間のかかる第１回転部を主回転部に設定し、図５に示すように、この主回転部の速度変動パターンに基づいて、その他の回転部の速度変動パターンを設定する。すなわち、各回転部の回転開始から回転終了までの時間が等しくなるように、主回転部以外の回転部の速度変動パターンを補正する。詳しくは、図５（ｂ）に示すように、第２回転部の回転角度である台形の面積を変化させないようにしつつ、第２回転部の回転開始から回転終了までの時間（時間Ｔａｍ＋時間Ｔｃｍ＋時間Ｔｄｍ）を、第１回転部の回転開始から回転終了までの時間（時間Ｔａ１＋時間Ｔｃ１＋時間Ｔｄ１）に合わせる。なお、各回転部において、加速時間、等速時間、減速時間が互いに等しくなるようにしている（時間Ｔａ１＝時間Ｔａｍ、時間Ｔｃ１＝時間Ｔｃｍ、時間Ｔｄ１＝時間Ｔｄｍ）。
【００５１】
このようにして、教示された複数の動作点に対して、ハンド部１８を各動作点間（アームの第１姿勢と第２姿勢との間）で移動させる際の速度変動パターンを設定する。例えば、第１姿勢を動作開始点とし、第２姿勢を動作終了点とする。そして、制御装置２０は、各動作点間の速度変動パターンを設定した後、それらの速度変動パターンに従ってロボット１０を実際に駆動し、その時の各回転部の回転角度を検出する。詳しくは、ＣＰＵ２１は、各速度変動パターンに基づいて各サーボモータ３１を駆動する電流を算出し、駆動回路２５を通じて各サーボモータ３１の駆動を行う。この駆動中において、所定周期毎に、各エンコーダ３２の検出信号が位置検出回路２６に入力され、位置検出回路２６は各エンコーダ３２の検出信号に基づいて各回転部の回転角度を検出する。ＣＰＵ２１は、位置検出回路２６から各回転部の回転角度を入力して、各回転部の回転角度連続値を検出する。
【００５２】
ＣＰＵ２１は、このように検出された各回転部の回転角度連続値に基づいて、各回転部の速度連続値を算出する。すなわち、回転角度の変化から、その時々の回転速度を算出する。さらに、ＣＰＵ２１は、算出された各回転部の速度連続値に基づいて、各回転部の加速度連続値を算出する。すなわち、回転速度の変化から、その時々の回転加速度を算出する。また、ＣＰＵ２１は、ロボット１０を実際に駆動する際の各サーボモータ３１の電流、すなわち上記のように各速度変動パターンに基づいて算出された各サーボモータ３１の電流を、電流連続値として検出する。そして、検出された各連続値をＲＡＭ２３に記憶させる。なお、各サーボモータ３１において、速度、加速度、及び電流の各連続値を検出する処理が、連続値検出手段としての処理に相当する。
【００５３】
次に、図６のフローチャートを参照して、ロボット１０をより高速で動作させるための変動パターンを設定する制御の処理手順について説明する。この一連の処理は、制御装置２０のＣＰＵ２１によって実行される。
【００５４】
まず、ロボット１０の複数の動作点間のうち最適化する動作点間（最適化区間）について、各種パラメータの連続値を取得する(Ｓ１１)。具体的には、上述したようにロボット１０のティーチングにおいて検出した速度連続値、加速度連続値、及び電流連続値について、最適化区間での連続値をそれぞれ取得する。詳しくは、ＲＡＭ２３からそれらの連続値のデータを読み込む。なお、最適化区間は、ロボット１０の動作の精度がそれほど要求されず、動作の速度が要求される動作区間をユーザが指定することにより決定される。
【００５５】
その後、カウンタ初期化処理として、前側カウンタＣｆｒｔの値を０にするとともに、後側カウンタＣｂａｋの値を最大値Ｃｍａｘにする(Ｓ１２)。最大値Ｃｍａｘは、最適化する動作点間を分割する分割数である。前側カウンタＣｆｒｔは、ロボット１０の第１姿勢(最適化区間の一方の端部)から所定の期間Ｔｓ毎に各種パラメータを算出する際の位置を表すカウンタである。後側カウンタＣｂａｋは、ロボット１０の第２姿勢(最適化区間の他方の端部)から所定の期間Ｔｓ毎に各種パラメータを算出する際の位置を表すカウンタである。
【００５６】
続いて、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、定格の最大電流Ｉｍａｘ（上限値）に対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する(Ｓ１３)。Ｊ番目のサーボモータ３１を添え字Ｊで表し、前側カウンタＣｆｒｔに対応する位置を添え字Ｃｆｒｔで表すと、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜／最大電流Ｉｍａｘ［Ｊ］となる。
【００５７】
図８に示すように、最適化区間において、速度連続値（図８（ａ）参照）、加速度連続値（図８（ｂ）参照）、電流連続値（図８（ｃ）参照）がそれぞれ取得されている。速度連続値では、時間ｔに比例して速度Ｖが上昇し、定格の最大速度Ｖｍａｘ（上限値）に達した時点で最大速度Ｖｍａｘが維持され、その後に時間に比例して速度Ｖが０まで低下している。加速度連続値では、速度Ｖが最大速度Ｖｍａｘに到達するまで一定の加速度に維持され、速度Ｖが最大速度Ｖｍａｘに達した時点で加速度Ａが０にされ、その後に速度Ｖが０に低下するまで一定の加速度（減速度）に維持されている。電流連続値では、所定の正の電流から時間ｔに比例して電流Ｉが増加し、速度Ｖが最大速度Ｖｍａｘに達した時点で電流Ｉが減少され、その後に所定の負の電流から時間ｔに比例して電流Ｉが増加している。そして、図８（ｃ）の枠で囲んだ部分を拡大して図８（ｄ）に示すように、前側カウンタＣｆｒｔに対応する位置において、最大電流Ｉｍａｘに対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する。
【００５８】
図６に戻り、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最大値である最大電流比Ｉｒａｔｍａｘを選択する（Ｓ１４）。
【００５９】
そして、最大電流比Ｉｒａｔｍａｘの逆数に基づいて、各サーボモータ３１の対応する加速度Ａを修正した修正加速度Ａａｍｄを算出する（Ｓ１５）。ここで、電流比Ｉｒａｔの逆数は、電流Ｉを定格の最大電流Ｉｍａｘまで大きくする際の倍率であり、各サーボモータ３１では電流Ｉをこの電流比Ｉｒａｔの逆数倍まで大きくすることができる。上記最大電流比Ｉｒａｔｍａｘは、定格の最大電流Ｉｍａｘまでの余裕が最も少ないサーボモータ３１での電流比Ｉｒａｔとなる。このため、このサーボモータ３１では、電流Ｉに電流比Ｉｒａｔの逆数を掛けると定格の最大電流Ｉｍａｘとなる。
【００６０】
したがって、前側カウンタＣｆｒｔに対応する位置において、各サーボモータ３１の加速度Ａをそれぞれ最大電流比Ｉｒａｔｍａｘで割って修正加速度Ａａｍｄを算出する。すなわち、Ｊ番目のサーボモータ３１を添え字Ｊで表し、前側カウンタＣｆｒｔに対応する位置を添え字Ｃｆｒｔで表すと、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］＝加速度Ａ［Ｊ］［Ｃｆｒｔ］／最大電流比Ｉｒａｔｍａｘとなる。この修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］は、各サーボモータ３１の駆動が同時に終了するように、各サーボモータの加速度Ａ［Ｊ］［Ｃｆｒｔ］を同じ倍率（最大電流比Ｉｒａｔｍａｘの逆数）で大きくしたものである。このとき、各サーボモータ３１の加速度Ａは、各サーボモータ３１を駆動する電流Ｉに概ね比例するとみなしている。なお、最大電流比Ｉｒａｔｍａｘが０である場合には、各サーボモータの加速度Ａ［Ｊ］［Ｃｆｒｔ］を０とする。
【００６１】
続いて、修正加速度Ａａｍｄとその修正加速度Ａａｍｄに対応する各サーボモータ３１の速度に基づいて、各サーボモータ３１における所定の期間Ｔｓ後の速度である修正速度Ｖａｍｄを算出する（Ｓ１６）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ−１］に前回の修正加速度Ａａｍｄ［Ｊ］［ｃｆｒｔ−１］と所定の期間Ｔｓとの積を加えて、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を算出する。すなわち、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］＝修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ−１］＋Ａａｍｄ［Ｊ］［ｃｆｒｔ−１］×期間Ｔｓとなる。なお、前側カウンタＣｆｒｔの値が０の場合には、修正速度Ｖａｍｄ［Ｊ］［０］＝速度Ｖ［Ｊ］［０］とする。
【００６２】
そして、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さくないと判定した場合には（Ｓ１７：ＮＯ）、修正速度Ｖａｍｄを最大速度Ｖｍａｘとする（Ｓ１９）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］が定格の最大速度Ｖｍａｘ［Ｊ］よりも小さくないと判定した場合には、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を最大速度Ｖｍａｘ［Ｊ］とする。
【００６３】
一方、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さいと判定した場合には（Ｓ１７：ＹＥＳ）、修正速度Ｖａｍｄとその修正速度Ｖａｍｄに対応する各サーボモータ３１の回転角度に基づいて、各サーボモータ３１における所定の期間Ｔｓ後の回転角度である修正回転角度θａｍｄを算出する（Ｓ１８）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ−１］に前回の修正速度Ｖａｍｄ［Ｊ］［ｃｆｒｔ−１］と所定の期間Ｔｓとの積を加えて、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］を算出する。すなわち、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］＝修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ−１］＋Ｖａｍｄ［Ｊ］［ｃｆｒｔ−１］×期間Ｔｓとなる。なお、前側カウンタＣｆｒｔの値が０の場合には、修正回転角度θａｍｄ［Ｊ］［０］＝回転角度θ［Ｊ］［０］とする。
【００６４】
図９（ａ）〜（ｃ）に、各サーボモータ３１において、修正加速度Ａａｍｄ、修正速度Ｖａｍｄ、及び修正回転角度θａｍｄを算出する態様を示す。
【００６５】
図９（ａ）に示すように、前側カウンタＣｆｒｔに対応する位置（Ｃｆｒｔ＝０）において、サーボモータ３１の対応する加速度Ａ［０］（破線）に、最大電流比Ｉｒａｔｍａｘの逆数を掛けて、矢印で示すように修正加速度Ａａｍｄ［０］（実線）を算出する。
【００６６】
そして、図９（ｂ）に示すように、修正加速度Ａａｍｄ［０］とそれに対応するサーボモータ３１の速度Ｖ［０］とに基づいて、所定の期間Ｔｓ後の修正速度Ｖａｍｄ［１］を算出する。このため、破線で示す速度Ｖ［１］は、実線で示す修正速度Ｖａｍｄ［１］に修正される。
【００６７】
さらに、図９（ｃ）に示すように、修正速度Ｖａｍｄ［０］とそれに対応するサーボモータ３１の回転角度θ［０］とに基づいて、所定の期間Ｔｓ後の修正回転角度θａｍｄ［１］を算出する。このため、破線で示す回転角度θ［１］は、実線で示す修正回転角度θａｍｄ［１］に修正される。
【００６８】
このようにして、各サーボモータ３１において、第１姿勢側から修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］、及び修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］が繰り返し算出される。
【００６９】
図６に戻り、前側カウンタＣｆｒｔの値を１増加させる（Ｓ２１）。すなわち、前側カウンタＣｆｒｔ＝前側カウンタＣｆｒｔ＋１とする。これにより、前側カウンタＣｆｒｔに対応する位置は、第１姿勢を開始時点として所定の期間Ｔｓ経過後の位置となる。
【００７０】
続いて、ロボット１０の第２姿勢(最適化区間の他方の端部)から、各種パラメータの算出が一度も行われていないか否か判定する（Ｓ２２）。具体的には、後側カウンタＣｂａｋの値が最大値Ｃｍａｘであるか否か判定する。そして、後側カウンタＣｂａｋの値が最大値Ｃｍａｘであると判定した場合には移行先１へ進み（Ｓ２２：ＹＥＳ）、後側カウンタＣｂａｋの値が最大値Ｃｍａｘでないと判定した場合には移行先２へ進む（Ｓ２２：ＮＯ）。
【００７１】
移行先１へ処理を移行した後、後側カウンタＣｂａｋに対応する位置において、Ｓ１３〜Ｓ２１の処理に対応するＳ２３〜Ｓ３１の処理を行う。すなわち、ロボット１０の第２姿勢(最適化区間の他方の端部)を開始時点として、後側カウンタＣｂａｋに対応して所定の期間Ｔｓ毎に各種パラメータを算出する。なお、Ｓ１３〜Ｓ２１の処理と同様の処理については説明を簡略化する。
【００７２】
後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、定格の最大電流Ｉｍａｘ（上限値）に対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する(Ｓ２３)。Ｊ番目のサーボモータ３１を添え字Ｊで表し、後側カウンタＣｂａｋに対応する位置を添え字Ｃｂａｋで表すと、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｂａｋ］｜／最大電流Ｉｍａｘ［Ｊ］となる。
【００７３】
図１０に示すように、最適化区間において、速度連続値（図１０（ａ）参照）、加速度連続値（図１０（ｂ）参照）、電流連続値（図１０（ｃ）参照）がそれぞれ取得されている。速度連続値では、時間ｔの最大値からの減少量（右端の第２姿勢を開始時点とすれば時間ｔの増加量）に比例して速度Ｖが上昇し、定格の最大速度Ｖｍａｘ（上限値）に達した時点で最大速度Ｖｍａｘが維持され、その後に時間の減少量（第２姿勢側からの時間ｔの増加量）に比例して速度Ｖが０まで低下している。加速度連続値では、時間ｔが最大値から減少する（第２姿勢側から時間ｔが増加する）際に、速度Ｖが最大速度Ｖｍａｘに到達するまで一定の負の加速度に維持され、速度Ｖが最大速度Ｖｍａｘに達した時点で加速度Ａが０にされ、その後に速度Ｖが０に低下するまで一定の正の加速度に維持されている。電流連続値では、所定の負の電流から時間ｔの減少量（第２姿勢側からの時間ｔの増加量）に比例して電流Ｉが減少（負側に増大）し、速度Ｖが最大速度Ｖｍａｘに達した時点で電流Ｉが増加され、その後に所定の正の電流から時間ｔの減少量（第２姿勢側からの時間ｔの増加量）に比例して電流Ｉが減少している。そして、図１０（ｃ）の枠で囲んだ部分を拡大して図１０（ｄ）に示すように、後側カウンタＣｂａｋに対応する位置において、最大電流Ｉｍａｘに対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する。
【００７４】
図７に戻り、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最大値である最大電流比Ｉｒａｔｍａｘを選択する（Ｓ２４）。そして、最大電流比Ｉｒａｔｍａｘの逆数に基づいて、各サーボモータ３１の対応する加速度Ａを修正した修正加速度Ａａｍｄを算出する（Ｓ２５）。すなわち、後側カウンタＣｂａｋに対応する位置において、各サーボモータ３１の加速度Ａをそれぞれ最大電流比Ｉｒａｔｍａｘで割って修正加速度Ａａｍｄを算出する。詳しくは、Ｊ番目のサーボモータ３１を添え字Ｊで表し、後側カウンタＣｂａｋに対応する位置を添え字Ｃｂａｋで表すと、修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］＝加速度Ａ［Ｊ］［Ｃｂａｋ］／最大電流比Ｉｒａｔｍａｘとなる。なお、最大電流比Ｉｒａｔｍａｘが０である場合には、各サーボモータの加速度Ａ［Ｊ］［Ｃｂａｋ］を０とする。
【００７５】
続いて、修正加速度Ａａｍｄとその修正加速度Ａａｍｄに対応する各サーボモータ３１の速度に基づいて、各サーボモータ３１における第２姿勢側から所定の期間Ｔｓ後の速度である修正速度Ｖａｍｄを算出する（Ｓ２６）。詳しくは、後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ＋１］に前回の修正加速度Ａａｍｄ［Ｊ］［ｃｆｒｔ＋１］と所定の期間Ｔｓとの積を加えて、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］を算出する。すなわち、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］＝修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ＋１］＋Ａａｍｄ［Ｊ］［ｃｆｒｔ＋１］×期間Ｔｓとなる。なお、後側カウンタＣｂａｋの値が最大値Ｃｍａｘの場合には、修正速度Ｖａｍｄ［Ｊ］［Ｃｍａｘ］＝速度Ｖ［Ｊ］［Ｃｍａｘ］とする。
【００７６】
そして、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さくないと判定した場合には（Ｓ２７：ＮＯ）、修正速度Ｖａｍｄを最大速度Ｖｍａｘとする（Ｓ２９）。
【００７７】
一方、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さいと判定した場合には（Ｓ２７：ＹＥＳ）、修正速度Ｖａｍｄとその修正速度Ｖａｍｄに対応する各サーボモータ３１の回転角度に基づいて、各サーボモータ３１における第２姿勢側から所定の期間Ｔｓ後の回転角度である修正回転角度θａｍｄを算出する（Ｓ２８）。詳しくは、後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ＋１］に前回の修正速度Ｖａｍｄ［Ｊ］［ｃｆｒｔ＋１］と所定の期間Ｔｓとの積を加えて、修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］を算出する。すなわち、修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］＝修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ＋１］＋Ｖａｍｄ［Ｊ］［ｃｆｒｔ＋１］×期間Ｔｓとなる。なお、後側カウンタＣｂａｋの値が最大値Ｃｍａｘの場合には、修正回転角度θａｍｄ［Ｊ］［Ｃｍａｘ］＝回転角度θ［Ｊ］［Ｃｍａｘ］とする。
【００７８】
図１１（ａ）〜（ｃ）に、各サーボモータ３１において、修正加速度Ａａｍｄ、修正速度Ｖａｍｄ、及び修正回転角度θａｍｄを算出する態様を示す。
【００７９】
図１１（ａ）に示すように、後側カウンタＣｂａｋに対応する位置（Ｃｂａｋ＝Ｃｍａｘ）において、サーボモータ３１の対応する加速度Ａ［Ｃｍａｘ］（破線）に、最大電流比Ｉｒａｔｍａｘの逆数を掛けて、矢印で示すように修正加速度Ａａｍｄ［Ｃｍａｘ］（実線）を算出する。
【００８０】
そして、図１１（ｂ）に示すように、修正加速度Ａａｍｄ［Ｃｍａｘ］とそれに対応するサーボモータ３１の速度Ｖ［Ｃｍａｘ］とに基づいて、第２姿勢側から所定の期間Ｔｓ後の修正速度Ｖａｍｄ［Ｃｍａｘ−１］を算出する。このため、破線で示す速度Ｖ［Ｃｍａｘ−１］は、実線で示す修正速度Ｖａｍｄ［Ｃｍａｘ−１］に修正される。
【００８１】
さらに、図１１（ｃ）に示すように、修正速度Ｖａｍｄ［Ｃｍａｘ］とそれに対応するサーボモータ３１の回転角度θ［Ｃｍａｘ］とに基づいて、第２姿勢側から所定の期間Ｔｓ後の修正回転角度θａｍｄ［Ｃｍａｘ−１］を算出する。このため、破線で示す回転角度θ［Ｃｍａｘ−１］は、実線で示す修正回転角度θａｍｄ［Ｃｍａｘ−１］に修正される。
【００８２】
このようにして、各サーボモータ３１において、第２姿勢側から修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］、及び修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］が繰り返し算出される。
【００８３】
図７に戻り、後側カウンタＣｂａｋの値を１減少させる（Ｓ３１）。すなわち、後側カウンタＣｂａｋ＝後側カウンタＣｂａｋ−１とする。これにより、後側カウンタＣｂａｋに対応する位置は、第２姿勢側から所定の期間Ｔｓ経過後の位置となる。
【００８４】
続いて、第１姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と、第２姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっているか否か判定する（Ｓ３２）。具体的には、後側カウンタＣｂａｋに対応する位置における修正回転角度θａｍｄ［Ｃｂａｋ］が、前側カウンタＣｆｒｔに対応する位置における修正回転角度θａｍｄ［Ｃｆｒｔ］よりも小さいか否か判定する。
【００８５】
図１２に示すように、回転角度θの変動パターンにおいて、第１姿勢側（図の左端側）から修正回転角度θａｍｄ［Ｃｆｒｔ］が順次算出されるとともに、第２姿勢側（図の右端側）から修正回転角度θａｍｄ［Ｃｂａｋ］が順次算出される。そして、これらの修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の変動パターンによりロボット１０のサーボモータ３１を駆動する場合には、これらの修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の間がサーボモータ３１を駆動する残り回転角度になる。
【００８６】
図７に戻り、第１姿勢側から算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっていないと判定した場合には（Ｓ３２：ＮＯ）、次回に、第１姿勢側から算出する修正速度Ｖａｍｄ［Ｃｆｒｔ］と、第２姿勢側から算出する修正速度Ｖａｍｄ［Ｃｂａｋ］とのいずれを優先的に算出するか決定する（Ｓ３３）。具体的には、第１姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｆｒｔ］が、第２姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｂａｋ］よりも小さいと判定した場合には（Ｓ３３：ＹＥＳ）、移行先３へ進んで、次回に第１姿勢側から各種パラメータを算出する（Ｓ１３〜Ｓ２１）。一方、第１姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｆｒｔ］が、第２姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｂａｋ］よりも小さくないと判定した場合には（Ｓ３３：ＮＯ）、移行先１へ進んで、次回に第２姿勢側から各種パラメータを算出する（Ｓ２３〜Ｓ３１）。
【００８７】
図１３に示すように、速度Ｖの変動パターンにおいて、第１姿勢側（図の左端側）から修正速度Ｖａｍｄ［Ｃｆｒｔ］が順次算出されるとともに、第２姿勢側（図の右端側）から修正速度Ｖａｍｄ［Ｃｂａｋ］が順次算出される。このとき、修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とは必ずしも一致しない。したがって、仮に修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とを同じ回数だけ繰り返し算出して、修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の間の残り回転角度が０になったとすると、その時点で修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とに差が生じているおそれがある。そこで、上記Ｓ３３の判定を通じて、修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とを比較して、低い方の修正速度Ｖａｍｄについて優先的に次回（所定の期間Ｔｓ後）の修正速度Ｖａｍｄを算出する。
【００８８】
図７に戻り、第１姿勢側から算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっていると判定した場合には（Ｓ３２：ＹＥＳ）、この一連の処理を終了する（ＥＮＤ）。この場合には、第１姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。また、同様にして、第１姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。さらに、同様にして、第１姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。
【００８９】
なお、Ｓ１３，Ｓ２３の処理が電流比算出手段としての処理に相当し、Ｓ１５，Ｓ２５の処理が修正加速度算出手段としての処理に相当し、Ｓ１６，Ｓ２６の処理が修正速度算出手段としての処理に相当し、Ｓ１８，Ｓ２８の処理が修正回転位置算出手段としての処理に相当する。
【００９０】
その後、制御装置２０のＣＰＵ２１は、図１４（ａ），（ｃ）に示すように、第１姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の速度変動パターンを設定する。すなわち、一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正速度変動パターンを設定する。このとき、第２姿勢を開始時点として各サーボモータ３１を加速する際の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］は、第１姿勢側から各サーボモータ３１を第２姿勢で停止する場合に、第２姿勢へ向けて各サーボモータ３１を減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で各サーボモータ３１を駆動する際に各サーボモータ３１に供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータ３１に供給される電流の大きさと等しいと考えることができる。
【００９１】
また、制御装置２０のＣＰＵ２１は、図１４（ｂ），（ｄ）に示すように、第１姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の回転角度変動パターン（回転位置変動パターン）を設定する。すなわち、一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正回転角度変動パターン（修正回転位置変動パターン）を設定する。
【００９２】
同様にして、制御装置２０のＣＰＵ２１は、第１姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の加速度変動パターンを設定する。すなわち、一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正加速度変動パターンを設定する。
【００９３】
図１５に、最適化前の各種パラメータの変動パターン（図１５（ａ１）〜（ｄ１））と、最適化後の各種パラメータの変動パターン（図１５（ａ２）〜（ｄ２））とを示す。ここでは、ロボット１０において、Ｊ１番目のサーボモータ及びＪ２番目のサーボモータの２つのサーボモータを例にして説明する。
【００９４】
図１５（ｄ１）に示すように、Ｊ１番目のサーボモータを駆動する電流は、Ｊ２番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ１番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。
【００９５】
そして、図１５（ｃ１）に示す各サーボモータの加速度Ａ［Ｊ］に、最大電流比Ｉｒａｔｍａｘの逆数が掛けられて、図１５（ｃ２）に示す修正加速度Ａａｍｄ［Ｊ］が算出される。ただし、上述したように、第１姿勢側から算出した修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。
【００９６】
続いて、図１５（ｂ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］とその修正加速度に対応する各サーボモータの修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の速度である修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。このとき、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］と、第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］とを比較して、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後（次回）の修正速度Ｖａｍｄ［Ｊ］を算出する。そして、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。図１５（ｂ２）と図１５（ｂ１）とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
【００９７】
続いて、図１５（ａ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖaｍｄ［Ｊ］［Ｃｂａｋ］とその修正速度に対応する各サーボモータの修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］，θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の回転角度である修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，θａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この修正回転角度θａｍｄ［Ｊ］の算出は、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］とのうち、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後（次回）の修正速度Ｖａｍｄ［Ｊ］を算出することに併せて行う。そして、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。図１５（ａ２）と図１５（ａ１）とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
【００９８】
図１６に、ロボット１０が駆動される途中で、電流比Ｉｒａｔ［Ｊ１］が最大となるサーボモータが替わる場合について説明する。ここでは、Ｊ１番目のサーボモータにより回転される回転部に、Ｊ２番目のサーボモータにより回転される回転部が接続されている構成を例にして説明する。
【００９９】
Ｊ１番目のサーボモータの回転角度が−π／２から＋π／２まで変化し、Ｊ２番目のサーボモータの回転角度が０から１４４π／１８０まで変化する。このとき、図１６（ａ）に示すように、Ｊ１番目のサーボモータとＪ２番目のサーボモータとが、同時に回転を開始する。そして、図１６（ｂ）に示す状態において、Ｊ１番目のサーボモータの回転により、Ｊ２番目のサーボモータにより回転される回転部に遠心力が作用し、Ｊ２番目のサーボモータの負荷が最大となる。このため、Ｊ2番目のサーボモータを駆動する電流が、Ｊ１番目のサーボモータを駆動する電流よりも大きくなり、両者の大小関係が逆転する。その後、図１６（ｃ）に示すように、Ｊ１番目のサーボモータとＪ２番目のサーボモータとは、それぞれの目標回転角度まで回転する。
【０１００】
図１７に、図１６で示す場合について、最適化前の各種パラメータの変動パターン（図１７（ａ１）〜（ｄ１））と、最適化後の各種パラメータの変動パターン（図１７（ａ２）〜（ｄ２））とを示す。
【０１０１】
図１７（ｄ１）に示すように、一点鎖線で囲む範囲のうち左寄りの部分（第１姿勢近傍）では、Ｊ１番目のサーボモータを駆動する電流は、Ｊ２番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ１番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。そして、一点鎖線で囲む範囲のうち右寄りの部分では、Ｊ２番目のサーボモータを駆動する電流は、Ｊ１番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ２番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。したがって、一点差線で囲む範囲において、ロボット１０が駆動される途中で、電流比Ｉｒａｔ［Ｊ］が最大となるサーボモータが替わることとなる。
【０１０２】
その後、図１５に示した場合と同様にして、図１７（ｃ１）に示す各サーボモータの加速度Ａ［Ｊ］に、最大電流比Ｉｒａｔｍａｘの逆数が掛けられて、図１７（ｃ２）に示す修正加速度Ａａｍｄ［Ｊ］が算出される。そして、図１７（ｂ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］とその修正加速度に対応する各サーボモータの修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の速度である修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この場合も、図１７（ｂ２）と図１７（ｂ１）とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
【０１０３】
続いて、図１７（ａ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖaｍｄ［Ｊ］［Ｃｂａｋ］とその修正速度に対応する各サーボモータの修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］，θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の回転角度である修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，θａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この場合も、図１７（ａ２）と図１７（ａ１）とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
【０１０４】
以上詳述した本実施形態は以下の利点を有する。
【０１０５】
・各サーボモータ３１の加速度Ａ［Ｊ］は各サーボモータ３１に供給される電流Ｉに比例することとなり、電流値の倍率に基づいて修正後の加速度を算出することができる。この点、所定の期間毎に、算出された各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最も大きい電流比Ｉｒａｔ［Ｊ］である最大電流比Ｉｒａｔｍａｘ［Ｊ］が選択され、この最大電流比Ｉｒａｔｍａｘ［Ｊ］の逆数に基づいて各サーボモータ３１の対応する加速度を修正した修正加速度Ａａｍｄ［Ｊ］が算出される。ここで、最大電流比Ｉｒａｔｍａｘ［Ｊ］の逆数は、電流比Ｉｒａｔ［Ｊ］の逆数のうちで最も小さな値である。このため、各サーボモータ３１の加速度Ａ［Ｊ］を同じ比率で大きくする際に、各サーボモータ３１において電流値が最大電流Ｉｍａｘを超えることを抑制しつつ、各サーボモータ３１の修正加速度Ａａｍｄ［Ｊ］を最大限大きくすることができる。
【０１０６】
・第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出しているため、第２姿勢に到達した時点で修正速度Ｖａｍｄ［Ｊ］を０にすることができる。このとき、第２姿勢を開始時点として加速する際の修正速度Ｖａｍｄ［Ｊ］は、第１姿勢側から第２姿勢で停止する場合に、第２姿勢へ向けて減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で駆動する際に各サーボモータ３１に供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータ３１に供給される電流の大きさと等しいと考えることができる。したがって、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出することにより、電流値が最大電流Ｉｍａｘを超えない範囲で適切に修正速度Ｖａｍｄ［Ｊ］を算出することができる。
【０１０７】
・第１姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Ｖａｍｄ［Ｊ］と、第２姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Ｖａｍｄ［Ｊ］とに基づいて、修正後の速度変動パターンを適切に設定することができる。その結果、ロボット１０のアームを第１姿勢と第２姿勢との間で駆動することにより、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための速度変動パターンを取得することができる。
【０１０８】
・また、第１姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］とが重なる場合に、第１姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Ａａｍｄ［Ｊ］と、第２姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Ａａｍｄ［Ｊ］とに基づいて、修正加速度変動パターンを設定している。したがって、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための加速度変動パターンを取得することができる。
【０１０９】
・第１姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］とが重なる場合に、第１姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θａｍｄ［Ｊ］とに基づいて、修正後の回転角度変動パターンを設定している。したがって、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための回転角度変動パターンを取得することができる。
【０１１０】
・第１姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］と第２姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］とにおいて、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後の修正速度Ｖａｍｄ［Ｊ］が算出されるため、それらの修正速度Ｖａｍｄ［Ｊ］の差を縮めることができる。したがって、第１姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］とが重なる時点で、第１姿勢を開始時点として算出された修正速度Ｖａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正速度Ｖａｍｄ［Ｊ］との差を小さくすることができる。その結果、修正後の速度変動パターンに段差が生じることを抑制することができる。
【０１１１】
さらに、第１姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］と第２姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］との差が小さい状態を維持しつつ、第１姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］とが重なる時点まで修正速度Ｖａｍｄ［Ｊ］が算出される。このため、必要な部分についてのみ修正速度Ｖａｍｄ［Ｊ］が算出されるようになり、計算負荷が軽減されることで処理を高速化することができる。
【０１１２】
上記実施形態に限定されず、例えば次のように実施することもできる。
【０１１３】
・上記実施形態では、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けて、修正加速度Ａａｍｄ［Ｊ］を算出した。しかしながら、電流比Ｉｒａｔ［Ｊ］の逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けることにより、そのサーボモータ３１では、その修正加速度Ａａｍｄ［Ｊ］を実現する際に電流Ｉ［Ｊ］がその上限値に一致することとなる。ここで、ロボット１０のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、電流値がその定格の最大電流Ｉｍａｘ（上限値）に一致するように修正後の各変動パターンを設定すると、アームを実際に駆動する際に電流値が最大電流Ｉｍａｘを超えるおそれがある。
【０１１４】
この点、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けた値よりも所定度合小さくなるように、修正加速度Ａａｍｄ［Ｊ］を算出するといった構成を採用することが有効である。具体的には、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けたものに、所定の係数ｋ（例えばｋ＝０．９）を掛けて修正加速度Ａａｍｄ［Ｊ］を算出するとよい。こうした構成によれば、最大電流Ｉｍａｘに相当する修正加速度Ａａｍｄ［Ｊ］よりも所定度合小さい修正加速度Ａａｍｄ［Ｊ］を算出することができ、修正後の変動パターンによりアームを実際に駆動する際に電流値が最大電流Ｉｍａｘを超えることを抑制することができる。
【０１１５】
・上記のように、ロボット１０のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンが設定された場合であっても、実際のアームの駆動に対してそれらの変動パターンが最適であるとは限らず、変動パターンをより最適化する余地がある。
【０１１６】
この点、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも１つが設定された後に、その設定された修正後の変動パターンとなるように、再度、アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータ３１において速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉ［Ｊ］の各連続値を検出するといった構成を採用することが有効である。
【０１１７】
上記構成によれば、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも１つが設定された後に、再度、各サーボモータ３１において速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉ［Ｊ］の各連続値が検出される。このため、その各連続値に基づいて、再度、電流比Ｉｒａｔ［Ｊ］、修正加速度Ａａｍｄ［Ｊ］、修正速度Ｖａｍｄ［Ｊ］、及び修正回転角度θａｍｄ［Ｊ］がそれぞれ算出される。したがって、一度設定された修正後の変動パターンに基づいて、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
【０１１８】
さらに、上記構成と、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けた値よりも所定度合小さくなるように、修正加速度Ａａｍｄ［Ｊ］を算出する構成とを組み合わせることにより、修正後の変動パターンによりアームを実際に駆動する際に、電流値が最大電流Ｉｍａｘを超えることを抑制しつつ、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
【０１１９】
・上記実施形態では、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜／最大電流Ｉｍａｘ［Ｊ］としたが、電流比Ｉｒａｔ［Ｊ］＝最大電流Ｉｍａｘ［Ｊ］／｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜とし、それに整合させるように図６のＳ１４，Ｓ１５の計算式を変更してもよい。
【０１２０】
・上記実施形態では、第１姿勢側から算出された修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出された修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを比較して、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に次回の（所定の期間Ｔｓ後）の修正速度Ｖａｍｄ［Ｊ］を算出するようにした。しかしながら、Ｖａｍｄ［Ｊ］を算出する手順はこれに限定されず、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを同一の所定回数算出して、両者の差が所定値よりも大きくなった場合に、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に次回の修正速度Ｖａｍｄ［Ｊ］を算出してもよい。また、第１姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが重なるまで、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを同一回数算出してもよい。
【０１２１】
これらの場合であっても、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］が算出されるため、第１姿勢及び第２姿勢から修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］に基づいて徐々に修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］を大きくすることができる。さらに、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出しているため、第２姿勢に到達した時点で修正速度Ｖａｍｄ［Ｊ］を０にすることができる。これに対して、例えば第１姿勢を開始時点として第２姿勢まで順に修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を算出した場合には、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］が第２姿勢まで到達した時に、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］は０から離れた値となる。このため、第２姿勢において修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を０にするためには、各サーボモータ３１に大きな電流の供給を要求する必要があり、電流値が最大電流Ｉｍａｘを超えることとなる。
【０１２２】
・上記実施形態では、修正速度変動パターン、修正加速度変動パターン、及び修正回転角度変動パターンを設定したが、これらの少なくとも１つを設定するようにしてもよい。
【０１２３】
・上記実施形態では、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］についても、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とのうち、優先的に次回算出される修正速度Ｖａｍｄ［Ｊ］に併せて、対応する電流比Ｉｒａｔ［Ｊ］を算出した。しかしながら、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］を、第１姿勢側から又は第２姿勢側から予め全て算出しておき、修正加速度Ａａｍｄ［Ｊ］を算出する際に対応する電流比Ｉｒａｔ［Ｊ］を読み込んで使用してもよい。
【０１２４】
・垂直多関節型の６軸ロボットに限らず、水平多関節型の４軸ロボット等、それ以外の型式や軸数のロボットを採用することもできる。
【符号の説明】
【０１２５】
１０…ロボット、１５…下アーム、１６…上アーム、２０…制御装置、２１…ＣＰＵ、３１…サーボモータ。
【技術分野】
【０００１】
本発明は、ロボットの制御装置に関する。
【背景技術】
【０００２】
従来、ロボットの動作によって発生する負荷トルクを、他軸からの干渉トルクも含めて全軸についてラグランジェ方程式を用いて評価して、最適な加減速時間を決定するものがある（例えば、特許文献１参照）。この特許文献１に記載のものでは、ロボットの各軸の質量、重心位置からなるパラメータと、教示された各駆動軸の位置、動作方向、及び動作速度とに基づいて、各軸に発生する負荷トルクを演算している。そして、発生する負荷トルクが許容ピークトルクに等しくなるように加減速時間を決定している。その結果、負荷トルクがピークトルクを越えない範囲で、最短の加減速時間とすることができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平７−２６１８２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、特許文献１に記載のものでは、加減速時間を決定するために、ロボットの各軸の質量、重心位置等のパラメータが必要となる。このため、これらのパラメータが不明である場合には、加減速時間を決定することができない。また、これらのパラメータを求めることは必ずしも容易ではなく、ロボットの扱うワーク（対象物）によってパラメータが変化するといった問題もある。
【０００５】
本発明は、こうした実情に鑑みてなされたものであり、ロボットをより高速で動作させるための制御パターンを取得する制御装置において、その実用性を向上させることを主たる目的とするものである。
【課題を解決するための手段】
【０００６】
本発明は、上記課題を解決するために、以下の手段を採用した。
【０００７】
第１の発明は、アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定する修正速度変動パターン設定手段と、を備えることを特徴とする。
【０００８】
上記構成によれば、ロボットはアームを駆動するサーボモータを各関節に備えており、アームが第１姿勢と第２姿勢との間で駆動されるとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流（各種パラメータ）の各連続値が検出される。例えば、ロボットのティーチングにおいて、第１姿勢と第２姿勢とがティーチングされ、第１姿勢及び第２姿勢の一方から他方への駆動中に、各サーボモータにおいて上記各種パラメータの各連続値が検出される。
【０００９】
そして、各サーボモータにおいて、検出された電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比が算出される。この電流比の逆数は、電流値を上限値まで大きくする際の倍率であり、各サーボモータでは電流値をこの電流比の逆数倍まで大きくすることができる。ここで、各サーボモータの加速度は、各サーボモータの発生するトルクに比例し、この発生トルクは各サーボモータに供給される電流に比例する。このため、各サーボモータの加速度は各サーボモータに供給される電流に比例することとなり、電流値の倍率に基づいて修正後の加速度を算出することができる。
【００１０】
この点、所定の期間毎に、算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比が選択され、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度が算出される。ここで、最大電流比の逆数は、電流比の逆数のうちで最も小さな値である。このため、各サーボモータの加速度を同じ比率で大きくする際に、各サーボモータにおいて電流値が上限値を超えることを抑制しつつ、各サーボモータの修正加速度を最大限大きくすることができる。なお、一般にロボットの制御では、各関節の駆動が同時に終了するように、各サーボモータの加速度等が制御される。このため、各関節の加速度等を変更する場合には、全ての関節の加速度等を同じ比率で変更する必要がある。
【００１１】
さらに、第１姿勢及び第２姿勢をそれぞれ開始時点として、上記修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける所定の期間後の速度である修正速度が繰り返し算出される。すなわち、第１姿勢を開始時点として各サーボモータにおいて、その時点の修正加速度とその修正加速度に対応する速度とに基づいて、所定の期間後の速度である修正速度が算出され、更に所定の期間後の修正加速度と算出された修正速度とに基づいて、その更に所定の期間後の修正速度が算出されるといったことが繰り返される。また、第２姿勢を開始時点としても、同様に修正速度が繰り返し算出される。このようにして、各サーボモータにおいて、加速度の修正に伴って速度が修正される。
【００１２】
また、同様にして、第１姿勢及び第２姿勢をそれぞれ開始時点として、上記修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける所定の期間後の回転位置である修正回転位置が繰り返し算出される。すなわち、各サーボモータにおいて、速度の修正に伴って回転位置が修正される。そして、第１姿勢を開始時点として繰り返し算出される修正回転位置と、第２姿勢を開始時点として繰り返し算出される修正回転位置とが、やがて同一の回転位置まで到達して重なることとなる。その場合には、第１姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正回転位置と、第２姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正回転位置とが算出されている。また、その場合には同様にして、第１姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正速度，修正加速度と、第２姿勢を開始時点としてその同一の回転位置へ到達するまでの一連の修正速度，修正加速度とが算出されている。
【００１３】
ここで、例えば第１姿勢を開始時点として第２姿勢まで順に修正速度を算出した場合には、修正速度にて減速を開始する時点についての情報がないため、修正回転位置が第２姿勢まで到達した時に修正速度が０にならない。このため、第２姿勢において未だ速度が出ている状態となり、ロボットのアームが第２姿勢を越えて駆動されるおそれがある。そして、第２姿勢において修正速度を０にするためには、サーボモータに限界を超える大きな電流の供給を要求する必要があり、電流値が上限値を超えることとなったり、修正速度が０になるように作業者が修正を加える必要が生じたりする。これに対して、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度を算出した場合には、第２姿勢に到達した時点で修正速度を０にすることができる。すなわち、第２姿勢で速度が０になる状態を出発点として減速の計算を行うことにより、第２姿勢で必ず速度が０となる修正速度変動パターンを作成することができる。
【００１４】
このとき、第２姿勢を開始時点として第１姿勢へ向けて加速する際の修正速度は、第１姿勢側から第２姿勢で停止する場合に、第２姿勢へ向けて減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で駆動する際に各サーボモータに供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータに供給される電流の大きさと等しいと考えることができる。したがって、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度を算出することにより、電流値が上限値を超えない範囲で適切に修正速度を算出することができる。
【００１５】
以上により、第１姿勢を開始時点として回転位置が重なるまでに繰り返し算出された修正速度と、第２姿勢を開始時点として回転位置が重なるまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを適切に設定することができる。その結果、アームを第１姿勢と第２姿勢との間で駆動することにより、各サーボモータにおける速度、加速度、及び供給される電流の各連続値以外のパラメータを必要とすることなく、ロボットをより高速で動作させるための速度変動パターンを取得することができる。
【００１６】
また、第２の発明では、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度と、前記第２姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度とに基づいて、修正後の加速度変動パターンを設定する修正加速度変動パターン設定手段を備えている。こうした構成によれば、第１の発明と同様にして、ロボットの各軸の質量や重心位置等のパラメータを必要とすることなく、各サーボモータにおける速度、加速度、及び供給される電流の各連続値のみにより、ロボットをより高速で動作させるための加速度変動パターンを取得することができる。
【００１７】
また、第３の発明では、前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置とに基づいて、修正後の回転位置変動パターンを設定する修正回転位置変動パターン設定手段を備えている。こうした構成によれば、第１の発明と同様にして、ロボットの各軸の質量や重心位置等のパラメータを必要とすることなく、各サーボモータにおける速度、加速度、及び供給される電流の各連続値のみにより、ロボットをより高速で動作させるための回転位置変動パターンを取得することができる。
【００１８】
第１姿勢及び第２姿勢をそれぞれ開始時点として、サーボモータにおける所定の期間後の修正速度を同じ回数だけ繰り返し算出した場合は、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とに差が生じることがある。この場合には、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点で、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とに差が生じているおそれがある。このため、第１姿勢を開始時点として修正回転位置が重なる時点までに繰り返し算出された修正速度と、第２姿勢を開始時点として修正回転位置が重なる時点までに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定した場合に、その速度変動パターンに段差が生じるおそれがある。
【００１９】
この点、第４の発明では、第１〜第３のいずれかの発明において、前記修正速度算出手段は、前記第１姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度とを比較して、低い方の前記修正速度について優先的に前記所定の期間後の修正速度を算出するといった構成を採用している。
【００２０】
上記構成によれば、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度とにおいて、低い方の修正速度について優先的に所定の期間後の修正速度が算出されるため、それらの修正速度の差を縮めることができる。したがって、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点で、第１姿勢を開始時点として算出された修正速度と、第２姿勢を開始時点として算出された修正速度との差を小さくすることができる。その結果、修正後の速度変動パターンに段差が生じることを抑制することができる。
【００２１】
さらに、第１姿勢を開始時点とした修正速度と第２姿勢を開始時点とした修正速度との差が小さい状態を維持しつつ、第１姿勢を開始時点として算出された修正回転位置と、第２姿勢を開始時点として算出された修正回転位置とが重なる時点まで修正速度が算出される。このため、必要な部分についてのみ修正速度が算出されるようになり、計算負荷が軽減されることで処理を高速化することができる。
【００２２】
ロボットのアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンが設定された場合であっても、実際のアームの駆動に対してそれらの変動パターンが最適であるとは限らず、変動パターンをより最適化する余地がある。
【００２３】
この点、第５の発明では、前記修正後の速度変動パターン、前記修正後の加速度変動パターン、及び前記修正後の回転位置変動パターンの少なくとも１つが設定された後に、前記連続値検出手段は、その設定された修正後の変動パターンとなるように、再度、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出するといった構成を採用している。
【００２４】
上記構成によれば、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転位置変動パターンの少なくとも１つが設定された後に、連続値検出手段により、再度、アームが第１姿勢と第２姿勢との間で駆動されるとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値が検出される。このため、その各連続地に基づいて、電流比算出手段、修正加速度算出手段、修正速度算出手段、及び修正回転位置算出手段により、再度、電流比、修正加速度、修正速度、及び修正回転位置がそれぞれ算出される。したがって、一度設定された修正後の変動パターンに基づいて、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンを、より最適化することができる。
【００２５】
電流比（上限値に対する電流値の比率）の逆数をサーボモータの対応する加速度に掛けることにより、そのサーボモータでは、その加速度を実現する際に電流値がその上限値に一致することとなる。しかしながら、上述したようにロボットのアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、電流値がその上限値に一致するように各変動パターンを設定すると、アームを実際に駆動する際に電流値が上限値を超えるおそれがある。
【００２６】
この点、第６の発明では、第１〜第５のいずれかの発明において、前記修正加速度算出手段は、前記最大電流比の逆数を各サーボモータの対応する加速度に掛けた値よりも所定度合小さくなるように前記修正加速度を算出するといった構成を採用している。したがって、電流の上限値に相当する修正加速度よりも所定度合小さい修正加速度を算出することができ、アームを実際に駆動する際に電流値が上限値を超えることを抑制することができる。
【００２７】
さらに、第５の発明と第６の発明とを組み合わせることにより、修正後の変動パターンによりアームを実際に駆動する際に、電流値が上限値を超えることを抑制しつつ、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転位置変動パターンを、より最適化することができる。
【図面の簡単な説明】
【００２８】
【図１】ロボットの概要を示す正面図。
【図２】ロボットの制御装置及びその周辺構成を示す回路図。
【図３】第１回転部の目標回転角度と速度変動パターンとを示すタイムチャート。
【図４】第２回転部の目標回転角度と速度変動パターンとを示すタイムチャート。
【図５】速度変動パターンの設定態様を示すタイムチャート。
【図６】修正変動パターン設定制御の処理手順を示すフローチャート。
【図７】同じく修正変動パターン設定制御の処理手順を示すフローチャート。
【図８】各種パラメータの連続値と電流比とを示すタイムチャート。
【図９】修正加速度、修正速度及び修正回転角度を算出する態様を示す拡大図。
【図１０】各種パラメータの連続値と電流比とを示すタイムチャート。
【図１１】修正加速度、修正速度及び修正回転角度を算出する態様を示す拡大図。
【図１２】回転角度及び修正回転角度の変動パターンを示すタイムチャート。
【図１３】速度及び修正速度の変動パターンを示すタイムチャート。
【図１４】修正速度変動パターン及び修正回転角度変動パターンを設定する態様を示すタイムチャート。
【図１５】最適化前と最適化後の各種パラメータの変動パターンを示すタイムチャート。
【図１６】電流比が最大となるサーボモータが途中で替わる例を示す模式図。
【図１７】最適化前と最適化後の各種パラメータの変動パターンを示すタイムチャート。
【発明を実施するための形態】
【００２９】
以下、垂直多関節型ロボットに具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。
【００３０】
はじめに、ロボット１０の概要を図１に基づいて説明する。
【００３１】
同図に示すように、ロボット１０は、各関節の回転中心軸線として、第１軸線Ｊ１、第２軸線Ｊ２、第３軸線Ｊ３、第４軸線Ｊ４、第５軸線Ｊ５、第６軸線Ｊ６を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、モータ駆動により原点位置を基準として各部が動作する。なお、各関節には、各軸線を中心として各部を滑らかに回転可能とする軸受け（軸受け手段）が設けられている。
【００３２】
ロボット１０は、床に設置されており、第１軸線Ｊ１が鉛直方向へ延びている。ロボット１０において、基台１１は、床等に固定される固定部１２と、その固定部１２の上方に設けられる回転部１３（第１回転部）とを有しており、回転部１３が第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。
【００３３】
下アーム１５（第２回転部）が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム１５は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。下アーム１５は、基本姿勢として鉛直方向に延びる向きに設けられている。
【００３４】
下アーム１５の上端部には、上アーム１６が、水平方向に延びる第３軸線Ｊ３を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム１６は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、下アーム１５により第３軸線Ｊ３を中心として回転可能に支持されている。上アーム１６は、基本姿勢として水平方向に延びる向きに設けられている。
【００３５】
上アーム１６は、基端側と先端側とで２つのアーム部に分割されて構成されており、基端側は第１上アーム１６Ａ（第３回転部）、先端側は第２上アーム１６Ｂ（第４回転部）となっている。第２上アーム１６Ｂは、上アーム１６の長手方向に延びる第４軸線Ｊ４を回転中心として、第１上アーム１６Ａに対してねじり方向に回転可能になっている。すなわち、第２上アーム１６Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第１上アーム１６Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。
【００３６】
上アーム１６（詳しくは第２上アーム１６Ｂ）の先端部には、手首部１７（第５回転部）が設けられている。手首部１７は、水平方向に延びる第５軸線Ｊ５を回転中心として、第２上アーム１６Ｂに対して回転可能になっている。すなわち、手首部１７は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。
【００３７】
手首部１７の先端部には、ワークやツール等を取り付けるためのハンド部１８（第６回転部）が設けられている。ハンド部１８は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部１８は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、手首部１７により第６軸線Ｊ６を中心として回転可能に支持されている。
【００３８】
図２に、ロボット１０の制御装置２０及びその周辺構成を示す。
【００３９】
制御装置２０は、ロボット１０の動作を制御するものであり、ＣＰＵ２１、ＲＯＭ２２、ＲＡＭ２３、駆動回路２５、及び位置検出回路２６等を備えている。これらは互いに電気的に接続されている。ＲＯＭ２２は、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭ２３は、これらのプログラムを実行する際にパラメータの値等を記憶する。ＣＰＵ２１は、これらのＲＯＭ２２やＲＡＭ２３の記憶内容に基づいて、各種プログラムを実行することにより、ロボット１０の動作を制御する。
【００４０】
ロボット１０において、各軸線Ｊ１〜Ｊ６を回転中心軸とした各関節には、それぞれ前段部側にサーボモータ３１が設けられている。ＣＰＵ２１は、駆動回路２５を通じて、各サーボモータ３１の駆動を行う。これにより、各関節において、前段部に設けられたサーボモータ３１の駆動により、後段部がそれぞれ回転動作する。
【００４１】
各サーボモータ３１には、その出力軸を制動する非励磁作動型の電磁ブレーキ３３と、出力軸の回転角度（回転位置）に応じたパルス信号を出力するエンコーダ３２とがそれぞれ設けられている。
【００４２】
各電磁ブレーキ３３は、ばね等の弾性力に基づきサーボモータ３１の出力軸の制動を行い、励磁コイルへの電力供給に基づき出力軸の制動を解除する。ＣＰＵ２１は、駆動回路２５を通じて、各電磁ブレーキ３３の駆動を行う。ＣＰＵ２１は、各電磁ブレーキ３３による出力軸の制動を解除した状態において各サーボモータ３１の駆動を行う。
【００４３】
各エンコーダ３２は、所定パターンに形成された回転子の回転を磁気的又は光学的に検出する検出素子と、その検出素子の信号を処理するＩＣとを有している。位置検出回路２６には、各エンコーダ３２の検出信号がそれぞれ入力される。位置検出回路２６は、各エンコーダ３２の検出信号に基づいて、回転部１３、下アーム１５、第１上アーム１６Ａ、第２上アーム１６Ｂ、手首部１７、及びハンド部１８の各位置（各角度）を検出する。
【００４４】
具体的には、位置検出回路２６は、固定部１２に対する回転部１３の回転角度（回転位置）、回転部１３に対する下アーム１５の回転角度、下アーム１５に対する第１上アーム１６Ａの回転角度、第１上アーム１６Ａに対する第２上アーム１６Ｂの回転角度、第２上アーム１６Ｂに対する手首部１７の回転角度、手首部１７に対するハンド部１８の回転角度をそれぞれ検出し、それら検出した位置情報をＣＰＵ２１に出力する。位置検出回路２６は、上記の各回転角度を０．１°単位で検出可能である。
【００４５】
そして、ＣＰＵ２１は、動作プログラムを実行することにより、位置検出回路２６から入力される位置情報に基づいて、ロボット１０の上記各部の動作をフィードバック制御する。また、ＣＰＵ２１は、後述する修正変動パターン設定制御を実行する。
（ティーチング）
修正変動パターン設定制御に先立って、ロボット１０に対して動作（アームを移動させる軌跡）のティーチングを行う。このティーチングでは、作業者が、ロボット１０に作業を行わせるべく、ロボット１０のハンド部１８を移動させる各動作点（各移動先）を教示する。さらに、作業者は、ハンド部１８を移動させる際の軌跡の形状、上限速度等を設定する。制御装置２０は、この教示内容及び設定に従って速度変動パターン（各種パラメータ）を作成し、その作成した速度変動パターンによりロボット１０を動作させる。作業者は、このロボット１０の動作結果を評価して、必要に応じて上記設定を変更する。
【００４６】
図３〜５を参照して、ロボットの２つの回転部を例として、各回転部の速度変動パターンを設定する手順について説明する。なお、図３（ａ）は第１回転部の目標回転角度を示しており、図３（ｂ）はそれに対する第１回転部の速度変動パターンを示している。図４（ａ）は第２回転部の目標回転角度を示しており、図４（ｂ）はそれに対する第２回転部の速度変動パターンを示している。図５（ａ）は主回転部の速度変動パターンを示しており、図５（ｂ）はそれ以外の回転部の速度変動パターンを設定する態様を示している。
【００４７】
まず、図３（ａ），図４（ａ）に示すように、各回転部の目標回転角度を設定する。ここでは、各回転部を原点（０）から回転させる場合を考え、第１回転部の目標回転角度を回転角度θ１とし、第２回転部の目標回転角度を回転角度θ２とする。回転角度θ１は、回転角度θ２よりも大きく設定されている（θ１＞θ２）。
【００４８】
ロボット１０では、各回転部の回転角度を変更する場合について、標準的な速度変動パターンが設定されており、この情報が制御装置２０のＲＯＭ２２に記憶されている。このため、ＣＰＵ２１は、この情報に基づいて、各回転部の目標回転角度に対して速度変動パターンをそれぞれ設定する。このようにして設定した各回転部の速度変動パターンを、図３（ｂ），図４（ｂ）にそれぞれ示す。
【００４９】
図３（ｂ）に示すように、第１回転部では、速度Ｖ１まで加速時間Ｔａ１で加速した後、速度Ｖ１で等速時間Ｔｃ１移動し、速度０まで減速時間Ｔｄ１で減速している。また、図４（ｂ）に示すように、第２回転部では、速度Ｖ２まで加速時間Ｔａ2で加速した後、速度Ｖ２で等速時間Ｔｃ２移動し、速度０まで減速時間Ｔｄ２で減速している。第１回転部が回転を開始してから回転を終了するまでの時間（時間Ｔａ１＋時間Ｔｃ１＋時間Ｔｄ１）は、第２回転部が回転を開始してから回転を終了するまでの時間（時間Ｔａ２＋時間Ｔｃ２＋時間Ｔｄ２）よりも長くなっている。
【００５０】
目標回転角度までの回転に最も時間のかかる第１回転部を主回転部に設定し、図５に示すように、この主回転部の速度変動パターンに基づいて、その他の回転部の速度変動パターンを設定する。すなわち、各回転部の回転開始から回転終了までの時間が等しくなるように、主回転部以外の回転部の速度変動パターンを補正する。詳しくは、図５（ｂ）に示すように、第２回転部の回転角度である台形の面積を変化させないようにしつつ、第２回転部の回転開始から回転終了までの時間（時間Ｔａｍ＋時間Ｔｃｍ＋時間Ｔｄｍ）を、第１回転部の回転開始から回転終了までの時間（時間Ｔａ１＋時間Ｔｃ１＋時間Ｔｄ１）に合わせる。なお、各回転部において、加速時間、等速時間、減速時間が互いに等しくなるようにしている（時間Ｔａ１＝時間Ｔａｍ、時間Ｔｃ１＝時間Ｔｃｍ、時間Ｔｄ１＝時間Ｔｄｍ）。
【００５１】
このようにして、教示された複数の動作点に対して、ハンド部１８を各動作点間（アームの第１姿勢と第２姿勢との間）で移動させる際の速度変動パターンを設定する。例えば、第１姿勢を動作開始点とし、第２姿勢を動作終了点とする。そして、制御装置２０は、各動作点間の速度変動パターンを設定した後、それらの速度変動パターンに従ってロボット１０を実際に駆動し、その時の各回転部の回転角度を検出する。詳しくは、ＣＰＵ２１は、各速度変動パターンに基づいて各サーボモータ３１を駆動する電流を算出し、駆動回路２５を通じて各サーボモータ３１の駆動を行う。この駆動中において、所定周期毎に、各エンコーダ３２の検出信号が位置検出回路２６に入力され、位置検出回路２６は各エンコーダ３２の検出信号に基づいて各回転部の回転角度を検出する。ＣＰＵ２１は、位置検出回路２６から各回転部の回転角度を入力して、各回転部の回転角度連続値を検出する。
【００５２】
ＣＰＵ２１は、このように検出された各回転部の回転角度連続値に基づいて、各回転部の速度連続値を算出する。すなわち、回転角度の変化から、その時々の回転速度を算出する。さらに、ＣＰＵ２１は、算出された各回転部の速度連続値に基づいて、各回転部の加速度連続値を算出する。すなわち、回転速度の変化から、その時々の回転加速度を算出する。また、ＣＰＵ２１は、ロボット１０を実際に駆動する際の各サーボモータ３１の電流、すなわち上記のように各速度変動パターンに基づいて算出された各サーボモータ３１の電流を、電流連続値として検出する。そして、検出された各連続値をＲＡＭ２３に記憶させる。なお、各サーボモータ３１において、速度、加速度、及び電流の各連続値を検出する処理が、連続値検出手段としての処理に相当する。
【００５３】
次に、図６のフローチャートを参照して、ロボット１０をより高速で動作させるための変動パターンを設定する制御の処理手順について説明する。この一連の処理は、制御装置２０のＣＰＵ２１によって実行される。
【００５４】
まず、ロボット１０の複数の動作点間のうち最適化する動作点間（最適化区間）について、各種パラメータの連続値を取得する(Ｓ１１)。具体的には、上述したようにロボット１０のティーチングにおいて検出した速度連続値、加速度連続値、及び電流連続値について、最適化区間での連続値をそれぞれ取得する。詳しくは、ＲＡＭ２３からそれらの連続値のデータを読み込む。なお、最適化区間は、ロボット１０の動作の精度がそれほど要求されず、動作の速度が要求される動作区間をユーザが指定することにより決定される。
【００５５】
その後、カウンタ初期化処理として、前側カウンタＣｆｒｔの値を０にするとともに、後側カウンタＣｂａｋの値を最大値Ｃｍａｘにする(Ｓ１２)。最大値Ｃｍａｘは、最適化する動作点間を分割する分割数である。前側カウンタＣｆｒｔは、ロボット１０の第１姿勢(最適化区間の一方の端部)から所定の期間Ｔｓ毎に各種パラメータを算出する際の位置を表すカウンタである。後側カウンタＣｂａｋは、ロボット１０の第２姿勢(最適化区間の他方の端部)から所定の期間Ｔｓ毎に各種パラメータを算出する際の位置を表すカウンタである。
【００５６】
続いて、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、定格の最大電流Ｉｍａｘ（上限値）に対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する(Ｓ１３)。Ｊ番目のサーボモータ３１を添え字Ｊで表し、前側カウンタＣｆｒｔに対応する位置を添え字Ｃｆｒｔで表すと、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜／最大電流Ｉｍａｘ［Ｊ］となる。
【００５７】
図８に示すように、最適化区間において、速度連続値（図８（ａ）参照）、加速度連続値（図８（ｂ）参照）、電流連続値（図８（ｃ）参照）がそれぞれ取得されている。速度連続値では、時間ｔに比例して速度Ｖが上昇し、定格の最大速度Ｖｍａｘ（上限値）に達した時点で最大速度Ｖｍａｘが維持され、その後に時間に比例して速度Ｖが０まで低下している。加速度連続値では、速度Ｖが最大速度Ｖｍａｘに到達するまで一定の加速度に維持され、速度Ｖが最大速度Ｖｍａｘに達した時点で加速度Ａが０にされ、その後に速度Ｖが０に低下するまで一定の加速度（減速度）に維持されている。電流連続値では、所定の正の電流から時間ｔに比例して電流Ｉが増加し、速度Ｖが最大速度Ｖｍａｘに達した時点で電流Ｉが減少され、その後に所定の負の電流から時間ｔに比例して電流Ｉが増加している。そして、図８（ｃ）の枠で囲んだ部分を拡大して図８（ｄ）に示すように、前側カウンタＣｆｒｔに対応する位置において、最大電流Ｉｍａｘに対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する。
【００５８】
図６に戻り、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最大値である最大電流比Ｉｒａｔｍａｘを選択する（Ｓ１４）。
【００５９】
そして、最大電流比Ｉｒａｔｍａｘの逆数に基づいて、各サーボモータ３１の対応する加速度Ａを修正した修正加速度Ａａｍｄを算出する（Ｓ１５）。ここで、電流比Ｉｒａｔの逆数は、電流Ｉを定格の最大電流Ｉｍａｘまで大きくする際の倍率であり、各サーボモータ３１では電流Ｉをこの電流比Ｉｒａｔの逆数倍まで大きくすることができる。上記最大電流比Ｉｒａｔｍａｘは、定格の最大電流Ｉｍａｘまでの余裕が最も少ないサーボモータ３１での電流比Ｉｒａｔとなる。このため、このサーボモータ３１では、電流Ｉに電流比Ｉｒａｔの逆数を掛けると定格の最大電流Ｉｍａｘとなる。
【００６０】
したがって、前側カウンタＣｆｒｔに対応する位置において、各サーボモータ３１の加速度Ａをそれぞれ最大電流比Ｉｒａｔｍａｘで割って修正加速度Ａａｍｄを算出する。すなわち、Ｊ番目のサーボモータ３１を添え字Ｊで表し、前側カウンタＣｆｒｔに対応する位置を添え字Ｃｆｒｔで表すと、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］＝加速度Ａ［Ｊ］［Ｃｆｒｔ］／最大電流比Ｉｒａｔｍａｘとなる。この修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］は、各サーボモータ３１の駆動が同時に終了するように、各サーボモータの加速度Ａ［Ｊ］［Ｃｆｒｔ］を同じ倍率（最大電流比Ｉｒａｔｍａｘの逆数）で大きくしたものである。このとき、各サーボモータ３１の加速度Ａは、各サーボモータ３１を駆動する電流Ｉに概ね比例するとみなしている。なお、最大電流比Ｉｒａｔｍａｘが０である場合には、各サーボモータの加速度Ａ［Ｊ］［Ｃｆｒｔ］を０とする。
【００６１】
続いて、修正加速度Ａａｍｄとその修正加速度Ａａｍｄに対応する各サーボモータ３１の速度に基づいて、各サーボモータ３１における所定の期間Ｔｓ後の速度である修正速度Ｖａｍｄを算出する（Ｓ１６）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ−１］に前回の修正加速度Ａａｍｄ［Ｊ］［ｃｆｒｔ−１］と所定の期間Ｔｓとの積を加えて、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を算出する。すなわち、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］＝修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ−１］＋Ａａｍｄ［Ｊ］［ｃｆｒｔ−１］×期間Ｔｓとなる。なお、前側カウンタＣｆｒｔの値が０の場合には、修正速度Ｖａｍｄ［Ｊ］［０］＝速度Ｖ［Ｊ］［０］とする。
【００６２】
そして、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さくないと判定した場合には（Ｓ１７：ＮＯ）、修正速度Ｖａｍｄを最大速度Ｖｍａｘとする（Ｓ１９）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］が定格の最大速度Ｖｍａｘ［Ｊ］よりも小さくないと判定した場合には、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を最大速度Ｖｍａｘ［Ｊ］とする。
【００６３】
一方、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さいと判定した場合には（Ｓ１７：ＹＥＳ）、修正速度Ｖａｍｄとその修正速度Ｖａｍｄに対応する各サーボモータ３１の回転角度に基づいて、各サーボモータ３１における所定の期間Ｔｓ後の回転角度である修正回転角度θａｍｄを算出する（Ｓ１８）。詳しくは、前側カウンタＣｆｒｔに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ−１］に前回の修正速度Ｖａｍｄ［Ｊ］［ｃｆｒｔ−１］と所定の期間Ｔｓとの積を加えて、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］を算出する。すなわち、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］＝修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ−１］＋Ｖａｍｄ［Ｊ］［ｃｆｒｔ−１］×期間Ｔｓとなる。なお、前側カウンタＣｆｒｔの値が０の場合には、修正回転角度θａｍｄ［Ｊ］［０］＝回転角度θ［Ｊ］［０］とする。
【００６４】
図９（ａ）〜（ｃ）に、各サーボモータ３１において、修正加速度Ａａｍｄ、修正速度Ｖａｍｄ、及び修正回転角度θａｍｄを算出する態様を示す。
【００６５】
図９（ａ）に示すように、前側カウンタＣｆｒｔに対応する位置（Ｃｆｒｔ＝０）において、サーボモータ３１の対応する加速度Ａ［０］（破線）に、最大電流比Ｉｒａｔｍａｘの逆数を掛けて、矢印で示すように修正加速度Ａａｍｄ［０］（実線）を算出する。
【００６６】
そして、図９（ｂ）に示すように、修正加速度Ａａｍｄ［０］とそれに対応するサーボモータ３１の速度Ｖ［０］とに基づいて、所定の期間Ｔｓ後の修正速度Ｖａｍｄ［１］を算出する。このため、破線で示す速度Ｖ［１］は、実線で示す修正速度Ｖａｍｄ［１］に修正される。
【００６７】
さらに、図９（ｃ）に示すように、修正速度Ｖａｍｄ［０］とそれに対応するサーボモータ３１の回転角度θ［０］とに基づいて、所定の期間Ｔｓ後の修正回転角度θａｍｄ［１］を算出する。このため、破線で示す回転角度θ［１］は、実線で示す修正回転角度θａｍｄ［１］に修正される。
【００６８】
このようにして、各サーボモータ３１において、第１姿勢側から修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］、及び修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］が繰り返し算出される。
【００６９】
図６に戻り、前側カウンタＣｆｒｔの値を１増加させる（Ｓ２１）。すなわち、前側カウンタＣｆｒｔ＝前側カウンタＣｆｒｔ＋１とする。これにより、前側カウンタＣｆｒｔに対応する位置は、第１姿勢を開始時点として所定の期間Ｔｓ経過後の位置となる。
【００７０】
続いて、ロボット１０の第２姿勢(最適化区間の他方の端部)から、各種パラメータの算出が一度も行われていないか否か判定する（Ｓ２２）。具体的には、後側カウンタＣｂａｋの値が最大値Ｃｍａｘであるか否か判定する。そして、後側カウンタＣｂａｋの値が最大値Ｃｍａｘであると判定した場合には移行先１へ進み（Ｓ２２：ＹＥＳ）、後側カウンタＣｂａｋの値が最大値Ｃｍａｘでないと判定した場合には移行先２へ進む（Ｓ２２：ＮＯ）。
【００７１】
移行先１へ処理を移行した後、後側カウンタＣｂａｋに対応する位置において、Ｓ１３〜Ｓ２１の処理に対応するＳ２３〜Ｓ３１の処理を行う。すなわち、ロボット１０の第２姿勢(最適化区間の他方の端部)を開始時点として、後側カウンタＣｂａｋに対応して所定の期間Ｔｓ毎に各種パラメータを算出する。なお、Ｓ１３〜Ｓ２１の処理と同様の処理については説明を簡略化する。
【００７２】
後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、定格の最大電流Ｉｍａｘ（上限値）に対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する(Ｓ２３)。Ｊ番目のサーボモータ３１を添え字Ｊで表し、後側カウンタＣｂａｋに対応する位置を添え字Ｃｂａｋで表すと、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｂａｋ］｜／最大電流Ｉｍａｘ［Ｊ］となる。
【００７３】
図１０に示すように、最適化区間において、速度連続値（図１０（ａ）参照）、加速度連続値（図１０（ｂ）参照）、電流連続値（図１０（ｃ）参照）がそれぞれ取得されている。速度連続値では、時間ｔの最大値からの減少量（右端の第２姿勢を開始時点とすれば時間ｔの増加量）に比例して速度Ｖが上昇し、定格の最大速度Ｖｍａｘ（上限値）に達した時点で最大速度Ｖｍａｘが維持され、その後に時間の減少量（第２姿勢側からの時間ｔの増加量）に比例して速度Ｖが０まで低下している。加速度連続値では、時間ｔが最大値から減少する（第２姿勢側から時間ｔが増加する）際に、速度Ｖが最大速度Ｖｍａｘに到達するまで一定の負の加速度に維持され、速度Ｖが最大速度Ｖｍａｘに達した時点で加速度Ａが０にされ、その後に速度Ｖが０に低下するまで一定の正の加速度に維持されている。電流連続値では、所定の負の電流から時間ｔの減少量（第２姿勢側からの時間ｔの増加量）に比例して電流Ｉが減少（負側に増大）し、速度Ｖが最大速度Ｖｍａｘに達した時点で電流Ｉが増加され、その後に所定の正の電流から時間ｔの減少量（第２姿勢側からの時間ｔの増加量）に比例して電流Ｉが減少している。そして、図１０（ｃ）の枠で囲んだ部分を拡大して図１０（ｄ）に示すように、後側カウンタＣｂａｋに対応する位置において、最大電流Ｉｍａｘに対する駆動時の電流Ｉの電流比Ｉｒａｔを算出する。
【００７４】
図７に戻り、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最大値である最大電流比Ｉｒａｔｍａｘを選択する（Ｓ２４）。そして、最大電流比Ｉｒａｔｍａｘの逆数に基づいて、各サーボモータ３１の対応する加速度Ａを修正した修正加速度Ａａｍｄを算出する（Ｓ２５）。すなわち、後側カウンタＣｂａｋに対応する位置において、各サーボモータ３１の加速度Ａをそれぞれ最大電流比Ｉｒａｔｍａｘで割って修正加速度Ａａｍｄを算出する。詳しくは、Ｊ番目のサーボモータ３１を添え字Ｊで表し、後側カウンタＣｂａｋに対応する位置を添え字Ｃｂａｋで表すと、修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］＝加速度Ａ［Ｊ］［Ｃｂａｋ］／最大電流比Ｉｒａｔｍａｘとなる。なお、最大電流比Ｉｒａｔｍａｘが０である場合には、各サーボモータの加速度Ａ［Ｊ］［Ｃｂａｋ］を０とする。
【００７５】
続いて、修正加速度Ａａｍｄとその修正加速度Ａａｍｄに対応する各サーボモータ３１の速度に基づいて、各サーボモータ３１における第２姿勢側から所定の期間Ｔｓ後の速度である修正速度Ｖａｍｄを算出する（Ｓ２６）。詳しくは、後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ＋１］に前回の修正加速度Ａａｍｄ［Ｊ］［ｃｆｒｔ＋１］と所定の期間Ｔｓとの積を加えて、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］を算出する。すなわち、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］＝修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ＋１］＋Ａａｍｄ［Ｊ］［ｃｆｒｔ＋１］×期間Ｔｓとなる。なお、後側カウンタＣｂａｋの値が最大値Ｃｍａｘの場合には、修正速度Ｖａｍｄ［Ｊ］［Ｃｍａｘ］＝速度Ｖ［Ｊ］［Ｃｍａｘ］とする。
【００７６】
そして、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さくないと判定した場合には（Ｓ２７：ＮＯ）、修正速度Ｖａｍｄを最大速度Ｖｍａｘとする（Ｓ２９）。
【００７７】
一方、修正速度Ｖａｍｄが定格の最大速度Ｖｍａｘよりも小さいと判定した場合には（Ｓ２７：ＹＥＳ）、修正速度Ｖａｍｄとその修正速度Ｖａｍｄに対応する各サーボモータ３１の回転角度に基づいて、各サーボモータ３１における第２姿勢側から所定の期間Ｔｓ後の回転角度である修正回転角度θａｍｄを算出する（Ｓ２８）。詳しくは、後側カウンタＣｂａｋに対応する位置において、ロボット１０の各サーボモータ３１について、前回の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ＋１］に前回の修正速度Ｖａｍｄ［Ｊ］［ｃｆｒｔ＋１］と所定の期間Ｔｓとの積を加えて、修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］を算出する。すなわち、修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］＝修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ＋１］＋Ｖａｍｄ［Ｊ］［ｃｆｒｔ＋１］×期間Ｔｓとなる。なお、後側カウンタＣｂａｋの値が最大値Ｃｍａｘの場合には、修正回転角度θａｍｄ［Ｊ］［Ｃｍａｘ］＝回転角度θ［Ｊ］［Ｃｍａｘ］とする。
【００７８】
図１１（ａ）〜（ｃ）に、各サーボモータ３１において、修正加速度Ａａｍｄ、修正速度Ｖａｍｄ、及び修正回転角度θａｍｄを算出する態様を示す。
【００７９】
図１１（ａ）に示すように、後側カウンタＣｂａｋに対応する位置（Ｃｂａｋ＝Ｃｍａｘ）において、サーボモータ３１の対応する加速度Ａ［Ｃｍａｘ］（破線）に、最大電流比Ｉｒａｔｍａｘの逆数を掛けて、矢印で示すように修正加速度Ａａｍｄ［Ｃｍａｘ］（実線）を算出する。
【００８０】
そして、図１１（ｂ）に示すように、修正加速度Ａａｍｄ［Ｃｍａｘ］とそれに対応するサーボモータ３１の速度Ｖ［Ｃｍａｘ］とに基づいて、第２姿勢側から所定の期間Ｔｓ後の修正速度Ｖａｍｄ［Ｃｍａｘ−１］を算出する。このため、破線で示す速度Ｖ［Ｃｍａｘ−１］は、実線で示す修正速度Ｖａｍｄ［Ｃｍａｘ−１］に修正される。
【００８１】
さらに、図１１（ｃ）に示すように、修正速度Ｖａｍｄ［Ｃｍａｘ］とそれに対応するサーボモータ３１の回転角度θ［Ｃｍａｘ］とに基づいて、第２姿勢側から所定の期間Ｔｓ後の修正回転角度θａｍｄ［Ｃｍａｘ−１］を算出する。このため、破線で示す回転角度θ［Ｃｍａｘ−１］は、実線で示す修正回転角度θａｍｄ［Ｃｍａｘ−１］に修正される。
【００８２】
このようにして、各サーボモータ３１において、第２姿勢側から修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］、修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］、及び修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］が繰り返し算出される。
【００８３】
図７に戻り、後側カウンタＣｂａｋの値を１減少させる（Ｓ３１）。すなわち、後側カウンタＣｂａｋ＝後側カウンタＣｂａｋ−１とする。これにより、後側カウンタＣｂａｋに対応する位置は、第２姿勢側から所定の期間Ｔｓ経過後の位置となる。
【００８４】
続いて、第１姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と、第２姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっているか否か判定する（Ｓ３２）。具体的には、後側カウンタＣｂａｋに対応する位置における修正回転角度θａｍｄ［Ｃｂａｋ］が、前側カウンタＣｆｒｔに対応する位置における修正回転角度θａｍｄ［Ｃｆｒｔ］よりも小さいか否か判定する。
【００８５】
図１２に示すように、回転角度θの変動パターンにおいて、第１姿勢側（図の左端側）から修正回転角度θａｍｄ［Ｃｆｒｔ］が順次算出されるとともに、第２姿勢側（図の右端側）から修正回転角度θａｍｄ［Ｃｂａｋ］が順次算出される。そして、これらの修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の変動パターンによりロボット１０のサーボモータ３１を駆動する場合には、これらの修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の間がサーボモータ３１を駆動する残り回転角度になる。
【００８６】
図７に戻り、第１姿勢側から算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっていないと判定した場合には（Ｓ３２：ＮＯ）、次回に、第１姿勢側から算出する修正速度Ｖａｍｄ［Ｃｆｒｔ］と、第２姿勢側から算出する修正速度Ｖａｍｄ［Ｃｂａｋ］とのいずれを優先的に算出するか決定する（Ｓ３３）。具体的には、第１姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｆｒｔ］が、第２姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｂａｋ］よりも小さいと判定した場合には（Ｓ３３：ＹＥＳ）、移行先３へ進んで、次回に第１姿勢側から各種パラメータを算出する（Ｓ１３〜Ｓ２１）。一方、第１姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｆｒｔ］が、第２姿勢側から繰り返し算出した修正速度Ｖａｍｄ［Ｃｂａｋ］よりも小さくないと判定した場合には（Ｓ３３：ＮＯ）、移行先１へ進んで、次回に第２姿勢側から各種パラメータを算出する（Ｓ２３〜Ｓ３１）。
【００８７】
図１３に示すように、速度Ｖの変動パターンにおいて、第１姿勢側（図の左端側）から修正速度Ｖａｍｄ［Ｃｆｒｔ］が順次算出されるとともに、第２姿勢側（図の右端側）から修正速度Ｖａｍｄ［Ｃｂａｋ］が順次算出される。このとき、修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とは必ずしも一致しない。したがって、仮に修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とを同じ回数だけ繰り返し算出して、修正回転角度θａｍｄ［Ｃｆｒｔ］，θａｍｄ［Ｃｂａｋ］の間の残り回転角度が０になったとすると、その時点で修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とに差が生じているおそれがある。そこで、上記Ｓ３３の判定を通じて、修正速度Ｖａｍｄ［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｃｂａｋ］とを比較して、低い方の修正速度Ｖａｍｄについて優先的に次回（所定の期間Ｔｓ後）の修正速度Ｖａｍｄを算出する。
【００８８】
図７に戻り、第１姿勢側から算出した修正回転角度θａｍｄ［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｃｂａｋ］とが重なっていると判定した場合には（Ｓ３２：ＹＥＳ）、この一連の処理を終了する（ＥＮＤ）。この場合には、第１姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。また、同様にして、第１姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。さらに、同様にして、第１姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とが算出されている。
【００８９】
なお、Ｓ１３，Ｓ２３の処理が電流比算出手段としての処理に相当し、Ｓ１５，Ｓ２５の処理が修正加速度算出手段としての処理に相当し、Ｓ１６，Ｓ２６の処理が修正速度算出手段としての処理に相当し、Ｓ１８，Ｓ２８の処理が修正回転位置算出手段としての処理に相当する。
【００９０】
その後、制御装置２０のＣＰＵ２１は、図１４（ａ），（ｃ）に示すように、第１姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の速度変動パターンを設定する。すなわち、一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正速度変動パターンを設定する。このとき、第２姿勢を開始時点として各サーボモータ３１を加速する際の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］は、第１姿勢側から各サーボモータ３１を第２姿勢で停止する場合に、第２姿勢へ向けて各サーボモータ３１を減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で各サーボモータ３１を駆動する際に各サーボモータ３１に供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータ３１に供給される電流の大きさと等しいと考えることができる。
【００９１】
また、制御装置２０のＣＰＵ２１は、図１４（ｂ），（ｄ）に示すように、第１姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の回転角度変動パターン（回転位置変動パターン）を設定する。すなわち、一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正回転角度変動パターン（修正回転位置変動パターン）を設定する。
【００９２】
同様にして、制御装置２０のＣＰＵ２１は、第１姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢を開始時点とした各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、修正後の加速度変動パターンを設定する。すなわち、一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とを結合することにより、修正加速度変動パターンを設定する。
【００９３】
図１５に、最適化前の各種パラメータの変動パターン（図１５（ａ１）〜（ｄ１））と、最適化後の各種パラメータの変動パターン（図１５（ａ２）〜（ｄ２））とを示す。ここでは、ロボット１０において、Ｊ１番目のサーボモータ及びＪ２番目のサーボモータの２つのサーボモータを例にして説明する。
【００９４】
図１５（ｄ１）に示すように、Ｊ１番目のサーボモータを駆動する電流は、Ｊ２番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ１番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。
【００９５】
そして、図１５（ｃ１）に示す各サーボモータの加速度Ａ［Ｊ］に、最大電流比Ｉｒａｔｍａｘの逆数が掛けられて、図１５（ｃ２）に示す修正加速度Ａａｍｄ［Ｊ］が算出される。ただし、上述したように、第１姿勢側から算出した修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正加速度Ａａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。
【００９６】
続いて、図１５（ｂ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］とその修正加速度に対応する各サーボモータの修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の速度である修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。このとき、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］と、第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］とを比較して、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後（次回）の修正速度Ｖａｍｄ［Ｊ］を算出する。そして、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。図１５（ｂ２）と図１５（ｂ１）とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
【００９７】
続いて、図１５（ａ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖaｍｄ［Ｊ］［Ｃｂａｋ］とその修正速度に対応する各サーボモータの修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］，θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の回転角度である修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，θａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この修正回転角度θａｍｄ［Ｊ］の算出は、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］とのうち、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後（次回）の修正速度Ｖａｍｄ［Ｊ］を算出することに併せて行う。そして、第１姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と第２姿勢側から算出した修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とが重なった時点で、第１姿勢側から算出した各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出した各サーボモータ３１の一連の修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが結合されている。図１５（ａ２）と図１５（ａ１）とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
【００９８】
図１６に、ロボット１０が駆動される途中で、電流比Ｉｒａｔ［Ｊ１］が最大となるサーボモータが替わる場合について説明する。ここでは、Ｊ１番目のサーボモータにより回転される回転部に、Ｊ２番目のサーボモータにより回転される回転部が接続されている構成を例にして説明する。
【００９９】
Ｊ１番目のサーボモータの回転角度が−π／２から＋π／２まで変化し、Ｊ２番目のサーボモータの回転角度が０から１４４π／１８０まで変化する。このとき、図１６（ａ）に示すように、Ｊ１番目のサーボモータとＪ２番目のサーボモータとが、同時に回転を開始する。そして、図１６（ｂ）に示す状態において、Ｊ１番目のサーボモータの回転により、Ｊ２番目のサーボモータにより回転される回転部に遠心力が作用し、Ｊ２番目のサーボモータの負荷が最大となる。このため、Ｊ2番目のサーボモータを駆動する電流が、Ｊ１番目のサーボモータを駆動する電流よりも大きくなり、両者の大小関係が逆転する。その後、図１６（ｃ）に示すように、Ｊ１番目のサーボモータとＪ２番目のサーボモータとは、それぞれの目標回転角度まで回転する。
【０１００】
図１７に、図１６で示す場合について、最適化前の各種パラメータの変動パターン（図１７（ａ１）〜（ｄ１））と、最適化後の各種パラメータの変動パターン（図１７（ａ２）〜（ｄ２））とを示す。
【０１０１】
図１７（ｄ１）に示すように、一点鎖線で囲む範囲のうち左寄りの部分（第１姿勢近傍）では、Ｊ１番目のサーボモータを駆動する電流は、Ｊ２番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ１番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。そして、一点鎖線で囲む範囲のうち右寄りの部分では、Ｊ２番目のサーボモータを駆動する電流は、Ｊ１番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、Ｊ２番目のサーボモータの電流比Ｉｒａｔ［Ｊ１］が、最大電流比Ｉｒａｔｍａｘとして選択される。したがって、一点差線で囲む範囲において、ロボット１０が駆動される途中で、電流比Ｉｒａｔ［Ｊ］が最大となるサーボモータが替わることとなる。
【０１０２】
その後、図１５に示した場合と同様にして、図１７（ｃ１）に示す各サーボモータの加速度Ａ［Ｊ］に、最大電流比Ｉｒａｔｍａｘの逆数が掛けられて、図１７（ｃ２）に示す修正加速度Ａａｍｄ［Ｊ］が算出される。そして、図１７（ｂ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］とその修正加速度に対応する各サーボモータの修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の速度である修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この場合も、図１７（ｂ２）と図１７（ｂ１）とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
【０１０３】
続いて、図１７（ａ２）に示すように、第１姿勢（図の左端）及び第２姿勢（図の右端）をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖaｍｄ［Ｊ］［Ｃｂａｋ］とその修正速度に対応する各サーボモータの修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］，θａｍｄ［Ｊ］［Ｃｂａｋ］とに基づいて、各サーボモータにおける所定の期間後（次回）の回転角度である修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ＋１］，θａｍｄ［Ｊ］［Ｃｂａｋ−１］を算出する。この場合も、図１７（ａ２）と図１７（ａ１）とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
【０１０４】
以上詳述した本実施形態は以下の利点を有する。
【０１０５】
・各サーボモータ３１の加速度Ａ［Ｊ］は各サーボモータ３１に供給される電流Ｉに比例することとなり、電流値の倍率に基づいて修正後の加速度を算出することができる。この点、所定の期間毎に、算出された各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］のうち、最も大きい電流比Ｉｒａｔ［Ｊ］である最大電流比Ｉｒａｔｍａｘ［Ｊ］が選択され、この最大電流比Ｉｒａｔｍａｘ［Ｊ］の逆数に基づいて各サーボモータ３１の対応する加速度を修正した修正加速度Ａａｍｄ［Ｊ］が算出される。ここで、最大電流比Ｉｒａｔｍａｘ［Ｊ］の逆数は、電流比Ｉｒａｔ［Ｊ］の逆数のうちで最も小さな値である。このため、各サーボモータ３１の加速度Ａ［Ｊ］を同じ比率で大きくする際に、各サーボモータ３１において電流値が最大電流Ｉｍａｘを超えることを抑制しつつ、各サーボモータ３１の修正加速度Ａａｍｄ［Ｊ］を最大限大きくすることができる。
【０１０６】
・第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出しているため、第２姿勢に到達した時点で修正速度Ｖａｍｄ［Ｊ］を０にすることができる。このとき、第２姿勢を開始時点として加速する際の修正速度Ｖａｍｄ［Ｊ］は、第１姿勢側から第２姿勢で停止する場合に、第２姿勢へ向けて減速する際の速度と等しいと考えることができる。すなわち、第２姿勢から最も高速で駆動する際に各サーボモータ３１に供給される電流の大きさは、第１姿勢側から第２姿勢まで最大限減速する際に各サーボモータ３１に供給される電流の大きさと等しいと考えることができる。したがって、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出することにより、電流値が最大電流Ｉｍａｘを超えない範囲で適切に修正速度Ｖａｍｄ［Ｊ］を算出することができる。
【０１０７】
・第１姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Ｖａｍｄ［Ｊ］と、第２姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Ｖａｍｄ［Ｊ］とに基づいて、修正後の速度変動パターンを適切に設定することができる。その結果、ロボット１０のアームを第１姿勢と第２姿勢との間で駆動することにより、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための速度変動パターンを取得することができる。
【０１０８】
・また、第１姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］とが重なる場合に、第１姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Ａａｍｄ［Ｊ］と、第２姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Ａａｍｄ［Ｊ］とに基づいて、修正加速度変動パターンを設定している。したがって、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための加速度変動パターンを取得することができる。
【０１０９】
・第１姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として繰り返し算出された修正回転角度θａｍｄ［Ｊ］とが重なる場合に、第１姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θａｍｄ［Ｊ］とに基づいて、修正後の回転角度変動パターンを設定している。したがって、各サーボモータ３１における速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉの各連続値以外のパラメータを必要とすることなく、ロボット１０をより高速で動作させるための回転角度変動パターンを取得することができる。
【０１１０】
・第１姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］と第２姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］とにおいて、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に所定の期間後の修正速度Ｖａｍｄ［Ｊ］が算出されるため、それらの修正速度Ｖａｍｄ［Ｊ］の差を縮めることができる。したがって、第１姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］とが重なる時点で、第１姿勢を開始時点として算出された修正速度Ｖａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正速度Ｖａｍｄ［Ｊ］との差を小さくすることができる。その結果、修正後の速度変動パターンに段差が生じることを抑制することができる。
【０１１１】
さらに、第１姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］と第２姿勢を開始時点とした修正速度Ｖａｍｄ［Ｊ］との差が小さい状態を維持しつつ、第１姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］と、第２姿勢を開始時点として算出された修正回転角度θａｍｄ［Ｊ］とが重なる時点まで修正速度Ｖａｍｄ［Ｊ］が算出される。このため、必要な部分についてのみ修正速度Ｖａｍｄ［Ｊ］が算出されるようになり、計算負荷が軽減されることで処理を高速化することができる。
【０１１２】
上記実施形態に限定されず、例えば次のように実施することもできる。
【０１１３】
・上記実施形態では、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けて、修正加速度Ａａｍｄ［Ｊ］を算出した。しかしながら、電流比Ｉｒａｔ［Ｊ］の逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けることにより、そのサーボモータ３１では、その修正加速度Ａａｍｄ［Ｊ］を実現する際に電流Ｉ［Ｊ］がその上限値に一致することとなる。ここで、ロボット１０のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、電流値がその定格の最大電流Ｉｍａｘ（上限値）に一致するように修正後の各変動パターンを設定すると、アームを実際に駆動する際に電流値が最大電流Ｉｍａｘを超えるおそれがある。
【０１１４】
この点、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けた値よりも所定度合小さくなるように、修正加速度Ａａｍｄ［Ｊ］を算出するといった構成を採用することが有効である。具体的には、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けたものに、所定の係数ｋ（例えばｋ＝０．９）を掛けて修正加速度Ａａｍｄ［Ｊ］を算出するとよい。こうした構成によれば、最大電流Ｉｍａｘに相当する修正加速度Ａａｍｄ［Ｊ］よりも所定度合小さい修正加速度Ａａｍｄ［Ｊ］を算出することができ、修正後の変動パターンによりアームを実際に駆動する際に電流値が最大電流Ｉｍａｘを超えることを抑制することができる。
【０１１５】
・上記のように、ロボット１０のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンが設定された場合であっても、実際のアームの駆動に対してそれらの変動パターンが最適であるとは限らず、変動パターンをより最適化する余地がある。
【０１１６】
この点、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも１つが設定された後に、その設定された修正後の変動パターンとなるように、再度、アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータ３１において速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉ［Ｊ］の各連続値を検出するといった構成を採用することが有効である。
【０１１７】
上記構成によれば、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも１つが設定された後に、再度、各サーボモータ３１において速度Ｖ［Ｊ］、加速度Ａ［Ｊ］、及び供給される電流Ｉ［Ｊ］の各連続値が検出される。このため、その各連続値に基づいて、再度、電流比Ｉｒａｔ［Ｊ］、修正加速度Ａａｍｄ［Ｊ］、修正速度Ｖａｍｄ［Ｊ］、及び修正回転角度θａｍｄ［Ｊ］がそれぞれ算出される。したがって、一度設定された修正後の変動パターンに基づいて、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
【０１１８】
さらに、上記構成と、最大電流比Ｉｒａｔｍａｘの逆数を各サーボモータ３１の対応する加速度Ａ［Ｊ］に掛けた値よりも所定度合小さくなるように、修正加速度Ａａｍｄ［Ｊ］を算出する構成とを組み合わせることにより、修正後の変動パターンによりアームを実際に駆動する際に、電流値が最大電流Ｉｍａｘを超えることを抑制しつつ、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
【０１１９】
・上記実施形態では、電流比Ｉｒａｔ［Ｊ］＝｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜／最大電流Ｉｍａｘ［Ｊ］としたが、電流比Ｉｒａｔ［Ｊ］＝最大電流Ｉｍａｘ［Ｊ］／｜電流Ｉ［Ｊ］［Ｃｆｒｔ］｜とし、それに整合させるように図６のＳ１４，Ｓ１５の計算式を変更してもよい。
【０１２０】
・上記実施形態では、第１姿勢側から算出された修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から算出された修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを比較して、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に次回の（所定の期間Ｔｓ後）の修正速度Ｖａｍｄ［Ｊ］を算出するようにした。しかしながら、Ｖａｍｄ［Ｊ］を算出する手順はこれに限定されず、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを同一の所定回数算出して、両者の差が所定値よりも大きくなった場合に、低い方の修正速度Ｖａｍｄ［Ｊ］について優先的に次回の修正速度Ｖａｍｄ［Ｊ］を算出してもよい。また、第１姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］と、第２姿勢側から繰り返し算出した修正回転角度θａｍｄ［Ｊ］［Ｃｂａｋ］とが重なるまで、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とを同一回数算出してもよい。
【０１２１】
これらの場合であっても、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］が算出されるため、第１姿勢及び第２姿勢から修正加速度Ａａｍｄ［Ｊ］［Ｃｆｒｔ］，Ａａｍｄ［Ｊ］［Ｃｂａｋ］に基づいて徐々に修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］，Ｖａｍｄ［Ｊ］［Ｃｂａｋ］を大きくすることができる。さらに、第１姿勢及び第２姿勢をそれぞれ開始時点として、修正速度Ｖａｍｄ［Ｊ］を算出しているため、第２姿勢に到達した時点で修正速度Ｖａｍｄ［Ｊ］を０にすることができる。これに対して、例えば第１姿勢を開始時点として第２姿勢まで順に修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を算出した場合には、修正回転角度θａｍｄ［Ｊ］［Ｃｆｒｔ］が第２姿勢まで到達した時に、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］は０から離れた値となる。このため、第２姿勢において修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］を０にするためには、各サーボモータ３１に大きな電流の供給を要求する必要があり、電流値が最大電流Ｉｍａｘを超えることとなる。
【０１２２】
・上記実施形態では、修正速度変動パターン、修正加速度変動パターン、及び修正回転角度変動パターンを設定したが、これらの少なくとも１つを設定するようにしてもよい。
【０１２３】
・上記実施形態では、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］についても、修正速度Ｖａｍｄ［Ｊ］［Ｃｆｒｔ］と修正速度Ｖａｍｄ［Ｊ］［Ｃｂａｋ］とのうち、優先的に次回算出される修正速度Ｖａｍｄ［Ｊ］に併せて、対応する電流比Ｉｒａｔ［Ｊ］を算出した。しかしながら、各サーボモータ３１の電流比Ｉｒａｔ［Ｊ］を、第１姿勢側から又は第２姿勢側から予め全て算出しておき、修正加速度Ａａｍｄ［Ｊ］を算出する際に対応する電流比Ｉｒａｔ［Ｊ］を読み込んで使用してもよい。
【０１２４】
・垂直多関節型の６軸ロボットに限らず、水平多関節型の４軸ロボット等、それ以外の型式や軸数のロボットを採用することもできる。
【符号の説明】
【０１２５】
１０…ロボット、１５…下アーム、１６…上アーム、２０…制御装置、２１…ＣＰＵ、３１…サーボモータ。
【特許請求の範囲】
【請求項１】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定する修正速度変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項２】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度と、前記第２姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度とに基づいて、修正後の加速度変動パターンを設定する修正加速度変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項３】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置とに基づいて、修正後の回転位置変動パターンを設定する修正回転位置変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項４】
前記修正速度算出手段は、前記第１姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度とを比較して、低い方の前記修正速度について優先的に前記所定の期間後の修正速度を算出することを特徴とする請求項１〜３のいずれか１項に記載のロボットの制御装置。
【請求項５】
前記修正後の速度変動パターン、前記修正後の加速度変動パターン、及び前記修正後の回転位置変動パターンの少なくとも１つが設定された後に、
前記連続値検出手段は、その設定された修正後の変動パターンとなるように、再度、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出することを特徴とする請求項１〜４のいずれか１項に記載のロボットの制御装置。
【請求項６】
前記修正加速度算出手段は、前記最大電流比の逆数を各サーボモータの対応する加速度に掛けた値よりも所定度合小さくなるように前記修正加速度を算出することを特徴とする請求項１〜５のいずれか１項に記載のロボットの制御装置。
【請求項１】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段によりそれまでに繰り返し算出された修正速度とに基づいて、修正後の速度変動パターンを設定する修正速度変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項２】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度と、前記第２姿勢を開始時点として前記修正加速度算出手段によりそれまでに繰り返し算出された修正加速度とに基づいて、修正後の加速度変動パターンを設定する修正加速度変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項３】
アームを駆動するサーボモータを各関節に備えるロボットの制御装置であって、
前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出する連続値検出手段と、
各サーボモータにおいて、前記連続値検出手段により検出された前記電流の連続値の一方の端部から他方の端部へ所定の期間毎に、上限値に対する電流値の比率である電流比を算出する電流比算出手段と、
前記所定の期間毎に、前記電流比算出手段により算出された各サーボモータの電流比のうち、最も大きい電流比である最大電流比を選択し、この最大電流比の逆数に基づいて各サーボモータの対応する加速度を修正した修正加速度を算出する修正加速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正加速度算出手段により算出された修正加速度とその修正加速度に対応する各サーボモータの速度とに基づいて、各サーボモータにおける前記所定の期間後の速度である修正速度を繰り返し算出する修正速度算出手段と、
前記第１姿勢及び前記第２姿勢をそれぞれ開始時点として、前記修正速度算出手段により算出された修正速度とその修正速度に対応する各サーボモータの回転位置とに基づいて、各サーボモータにおける前記所定の期間後の回転位置である修正回転位置を繰り返し算出する修正回転位置算出手段と、
前記第１姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段により繰り返し算出された修正回転位置とが重なる場合に、前記第１姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置と、前記第２姿勢を開始時点として前記修正回転位置算出手段によりそれまでに繰り返し算出された修正回転位置とに基づいて、修正後の回転位置変動パターンを設定する修正回転位置変動パターン設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項４】
前記修正速度算出手段は、前記第１姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度と、前記第２姿勢を開始時点として前記修正速度算出手段により算出された前記修正速度とを比較して、低い方の前記修正速度について優先的に前記所定の期間後の修正速度を算出することを特徴とする請求項１〜３のいずれか１項に記載のロボットの制御装置。
【請求項５】
前記修正後の速度変動パターン、前記修正後の加速度変動パターン、及び前記修正後の回転位置変動パターンの少なくとも１つが設定された後に、
前記連続値検出手段は、その設定された修正後の変動パターンとなるように、再度、前記アームを第１姿勢と第２姿勢との間で駆動するとともに、その駆動中に各サーボモータにおいて速度、加速度、及び供給される電流の各連続値を検出することを特徴とする請求項１〜４のいずれか１項に記載のロボットの制御装置。
【請求項６】
前記修正加速度算出手段は、前記最大電流比の逆数を各サーボモータの対応する加速度に掛けた値よりも所定度合小さくなるように前記修正加速度を算出することを特徴とする請求項１〜５のいずれか１項に記載のロボットの制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２０１２−６１５６０（Ｐ２０１２−６１５６０Ａ）
【公開日】平成２４年３月２９日（２０１２．３．２９）
【国際特許分類】
【出願番号】特願２０１０−２０７８１０（Ｐ２０１０−２０７８１０）
【出願日】平成２２年９月１６日（２０１０．９．１６）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２４年３月２９日（２０１２．３．２９）
【国際特許分類】
【出願日】平成２２年９月１６日（２０１０．９．１６）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]