(57)【特許請求の範囲】
【請求項１】
複数の関節を有するロボット機構部の動作を制御するロボットシステムの制御方法であって、
上記ロボット機構部を動作させるときのロボット機構部の各関節の開始位置と、目標位置と、目標速度とを教示データとして第１の記憶手段に格納する第１の記憶ステップと、
上記ロボット機構部の各関節の速度パターンを予測演算するときに用いる各関節の加速度と減速度の情報とを第２の記憶手段に格納する第２の記憶ステップと、
上記第１の記憶手段に格納された教示データと、上記第２の記憶手段に格納された各関節の加速度及び減速度とに基づいて、当該各関節の開始位置と目標位置と目標速度である教示データから当該格納された各関節の加速度及び減速度で動作するように、各関節についての時間に対する速度のパターンであって、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターン、もしくは一定の加速度で動作し一定の減速度で動作する三角形状の速度パターンを計算する第１の速度パターン計算方法を用いて、上記ロボット機構部の各関節の加速度予測値と、動作時間予測値を演算し、上記動作時間予測値の中から、その最大値を最長動作時間として演算する第１の演算ステップと、
上記動作時間予測値の中から動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間とし、各関節について上記動作時間予測値が上記最長動作時間と同じになり、加速時の距離が再計算前後で同じでかつ減速時の距離が再計算前後で同じになり、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターンとなるように動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値として再計算する速度パターン再計算方法を用いて、上記ロボット機構部の各関節について、上記第１の演算ステップによって演算された動作時間予測値が、上記第１の演算ステップによって演算された最長動作時間と同じになるように、速度並びに加速度及び減速度として再計算した値を、速度予測値、加速度予測値及び減速度予測値とする第２の演算ステップとを含むロボットシステムの制御方法において、
上記ロボット機構部の各関節に連結される各リンクの慣性テンソル、質量及び重心位置とを含むロボットダイナミクスベースパラメータを第３の記憶手段に格納する第３の記憶ステップと、
上記第１の記憶手段に格納された各関節の開始位置及び目標位置と、上記第２の演算ステップによって演算された速度予測値と、上記第３の記憶手段に格納されたロボットダイナミクスベースパラメータとに基づいて、
（ａ）上記ロボット機構部の各関節についての、加速度により関節自身に発生するトルク、並びに他関節の加速度により発生する干渉トルクを示す慣性行列と、
（ｂ）上記ロボット機構部の各関節についての、遠心力及びコリオリ力により発生するトルクを示す粘性行列と、
（ｃ）上記ロボット機構部の各関節の重力モーメントにより発生するトルクを示す重力行列と
を含む上記開始位置及び目標位置におけるロボットダイナミクスモデルを計算する第３の演算ステップと、
上記第３の演算ステップによって演算されたロボットダイナミクスモデルと、上記第２の演算ステップによって演算された各関節の加速度予測値及び減速度予測値とに基づいて、当該各関節の加速度予測値が互いに比例して加速時に加速度予測値の各関節間の比例関係が保持されるという第１の比例関係と、当該各関節の減速度予測値が互いに比例して減速時に減速度予測値の各関節間の比例関係が保持されるという第２の比例関係とが保持される第１の条件を満足し、かつ各関節の許容ピークトルクから、加速度及び減速度に関係の無い重力行列から求められる重量項の値、並びに加速度及び減速度に関係の無い粘性行列から求められる粘性項の値を減算した減算結果値が、各関節が上記加速度予測値及び減速度予測値で動作した場合に各関節で発生するトルク以上となるという第２の条件を満足するような最適な加速度及び最適な減速度を各関節毎に演算する第４の演算ステップと、
上記第１の演算ステップにおいて最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、上記第４の演算ステップによって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された第ｋの関節の開始位置、目標位置及び目標速度を含む教示データとから、上記第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間とする第５の演算ステップと、
上記第ｋの関節以外のすべての関節について、上記第５の演算ステップによって演算された基準動作時間と、上記第４の演算ステップによって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された開始位置及び目標位置を含む教示データとに基づいて、最長動作時間である基準動作時間となった関節についてのみ、当該最適な加速度及び当該最適な減速度を用いて上記第１の速度パターン計算方法により基準動作時間を求め、その他の関節については、当該最適な加速度と当該最適な減速度を保持し、かつ動作時間が上記基準動作時間となるように各関節の台形状の速度パターンを計算する第２の速度パターン計算方法を用いて、動作時間が上記基準動作時間となり、上記最適な加速度及び最適な減速度で動作するような速度パターンを生成する第６の演算ステップとを含むことを特徴とするロボットシステムの制御方法。
【請求項２】
上記粘性行列及び上記重力行列において、加速度に関係しないトルクとして、上記各関節で発生する摩擦抵抗を考慮することを特徴とする請求項１記載のロボットシステムの制御方法。
【請求項３】
各関節の慣性モーメントからなる慣性行列の対角成分に、上記各関節の減速機を含むモータのロータの慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正するステップとをさらに含むことを特徴とする請求項１又は２記載のロボットシステムの制御方法。
【請求項４】
複数の関節を有するロボット機構部の動作を制御するロボットシステムの制御装置であって、
上記ロボット機構部を動作させるときのロボット機構部の各関節の開始位置と、目標位置と、目標速度とを教示データとして格納する第１の記憶手段と、
上記ロボット機構部の各関節の速度パターンを予測演算するときに用いる各関節の加速度と減速度の情報とを格納する第２の記憶手段と、
上記第１の記憶手段に格納された教示データと、上記第２の記憶手段に格納された各関節の加速度及び減速度とに基づいて、当該各関節の開始位置と目標位置と目標速度である教示データから当該格納された各関節の加速度及び減速度で動作するように、各関節についての時間に対する速度のパターンであって、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターン、もしくは一定の加速度で動作し一定の減速度で動作する三角形状の速度パターンを計算する第１の速度パターン計算方法を用いて、上記ロボット機構部の各関節の加速度予測値と、動作時間予測値を演算し、上記動作時間予測値の中から、その最大値を最長動作時間として演算する第１の演算手段と、
上記動作時間予測値の中から動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間とし、各関節について上記動作時間予測値が上記最長動作時間と同じになり、加速時の距離が再計算前後で同じでかつ減速時の距離が再計算前後で同じになり、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターンとなるように動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値として再計算する速度パターン再計算方法を用いて、上記ロボット機構部の各関節について、上記第１の演算手段によって演算された動作時間予測値が、上記第１の演算手段によって演算された最長動作時間と同じになるように、速度並びに加速度及び減速度として再計算した値を、速度予測値、加速度予測値及び減速度予測値とする第２の演算手段とを備えたロボットシステムの制御装置において、
上記ロボット機構部の各関節に連結される各リンクの慣性テンソル、質量及び重心位置とを含むロボットダイナミクスベースパラメータを格納する第３の記憶手段と、
上記第１の記憶手段に格納された各関節の開始位置及び目標位置と、上記第２の演算手段によって演算された速度予測値と、上記第３の記憶手段に格納されたロボットダイナミクスベースパラメータとに基づいて、
（ａ）上記ロボット機構部の各関節についての、加速度により関節自身に発生するトルク、並びに他関節の加速度により発生する干渉トルクを示す慣性行列と、
（ｂ）上記ロボット機構部の各関節についての、遠心力及びコリオリ力により発生するトルクを示す粘性行列と、
（ｃ）上記ロボット機構部の各関節の重力モーメントにより発生するトルクを示す重力行列と
を含む上記開始位置及び目標位置におけるロボットダイナミクスモデルを計算する第３の演算手段と、
上記第３の演算手段によって演算されたロボットダイナミクスモデルと、上記第２の演算手段によって演算された各関節の加速度予測値及び減速度予測値とに基づいて、当該各関節の加速度予測値が互いに比例して加速時に加速度予測値の各関節間の比例関係が保持されるという第１の比例関係と、当該各関節の減速度予測値が互いに比例して減速時に減速度予測値の各関節間の比例関係が保持されるという第２の比例関係とが保持される第１の条件を満足し、かつ各関節の許容ピークトルクから、加速度及び減速度に関係の無い重力行列から求められる重量項の値、並びに加速度及び減速度に関係の無い粘性行列から求められる粘性項の値を減算した減算結果値が、各関節が上記加速度予測値及び減速度予測値で動作した場合に各関節で発生するトルク以上となるという第２の条件を満足するような最適な加速度及び最適な減速度を各関節毎に演算する第４の演算手段と、
上記第１の演算手段において最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、上記第４の演算手段によって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された第ｋの関節の開始位置、目標位置及び目標速度を含む教示データとから、上記第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間とする第５の演算手段と、
上記第ｋの関節以外のすべての関節について、上記第５の演算手段によって演算された基準動作時間と、上記第４の演算手段によって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された開始位置及び目標位置を含む教示データとに基づいて、最長動作時間である基準動作時間となった関節についてのみ、当該最適な加速度及び当該最適な減速度を用いて上記第１の速度パターン計算方法により基準動作時間を求め、その他の関節については、当該最適な加速度と当該最適な減速度を保持し、かつ動作時間が上記基準動作時間となるように各関節の台形状の速度パターンを計算する第２の速度パターン計算方法を用いて、動作時間が上記基準動作時間となり、上記最適な加速度及び最適な減速度で動作するような速度パターンを生成する第６の演算手段とを備えたことを特徴とするロボットシステムの制御装置。
【請求項５】
上記粘性行列及び上記重力行列において、加速度に関係しないトルクとして、上記各関節で発生する摩擦抵抗を考慮することを特徴とする請求項４記載のロボットシステムの制御装置。
【請求項６】
各関節の慣性モーメントからなる慣性行列の対角成分に、上記各関節の減速機を含むモータのロータの慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正する手段とをさらに備えたことを特徴とする請求項４又は５記載のロボットシステムの制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットシステムの制御方法及び制御装置に関する。
【０００２】
【従来の技術】
従来、複数の関節を有するロボットシステムの制御装置では、動作速度指令に加減速度（以下、加速度と減速度を意味する。また、加速と減速とを含み、加減速という。）を設け、なめらかな動作を実現している。ロボットの関節を、同じ速度パターンで動作させても、関節に発生するトルクは、ロボットの姿勢に依存して異なる。重力や他の関節から受ける干渉力などの負荷トルクが、ロボットの関節位置すなわちロボット姿勢により変わってしまうためである。負荷トルクが最大となるロボット姿勢で、速度パターンを固定すると、負荷トルクの影響が小さい姿勢での動作時間が必要以上に延びてしまう。一方、最小の姿勢で固定すると、負荷トルクの大きな姿勢で、モータが許容できる以上のトルクが必要となり、モータ制御に異常が発生して問題となる。
【０００３】
図１０は、第１の従来例の位置決め動作処理を示すフローチャートである。図１０において、まず、ステップＳ１０１で関節番号ｉを０にリセットした後、関節番号ｉが最終の関節ｉmaxとなるまで、各関節番号ｉ毎にステップＳ１０２からステップＳ１０４までの処理を繰り返す。ここで、ステップＳ１０２では、開始位置、目標位置及び目標速度をセットし、ステップＳ１０３で移動量の計算処理を実行した後、ステップＳ１０４で加速度及び減速度のプリセット情報に基づいて速度パターンの計算を行う。さらに、ステップＳ１０７では、最長動作時間の関節を抽出し、ステップＳ１０８で関節番号ｉを０にリセットした後、関節番号ｉが最終の関節ｉmaxとなるまで、各関節番号ｉ毎にステップＳ１０９における速度パターンの再計算処理である終了時間の同期処理を繰り返す。
【０００４】
図１１は、図１０の終了時間の同期処理の一例を説明するための各関節の速度パターンを示すグラフである。図１０のステップＳ１０７では、第２の関節が最長動作時間ｔmaxを有するとして抽出され、第１の関節と第３の関節の速度パターンは、それらの動作時間が第２の関節が最長動作時間ｔmaxとなり、
（ａ）面積Ｓａが面積Ｓａｐに等しくなり、
（ｂ）面積Ｓｂが面積Ｓｂｐに等しくなり、
（ｃ）面積Ｓｃが面積Ｓｃｐに等しくなり、
（ｄ）面積Ｓｄが面積Ｓｄｐに等しくなる
ように決定される。なお、図１０のグラフの積分値である面積は距離を表す。
【０００５】
この第１の従来例において、ステップＳ１０４での速度パターンの生成に使われる、加速度及び減速度の情報は、関節毎にプリセットされており、ロボットの姿勢に応じて変動する負荷トルクの影響度は考慮されていない。変動する負荷トルクとは、重力及び負荷慣性モーメント並びに他の関節から受ける干渉力等である。そのため、プリセットされる加速度及び減速度情報は、負荷トルクが最大となるロボット姿勢において、モータ制御に異常が発生しない値とするのが一般的である。
【０００６】
【発明が解決しようとする課題】
しかしながら、第１の従来例の方法では以下の問題がある。
（ａ）負荷トルクが最大でないロボット姿勢における位置決め動作では、モータの能力が最大限に使用されていない。
（ｂ）ロボット設計の際、負荷トルクが最大のロボット姿勢において、モータ制御に異常が発生しないようにモータを選定しなければならず、必然的にオーバースペックとなる。
【０００７】
この問題を解決するために、特開平７−２６１８２２号公報（以下、第２の従来例という。）では、重力モーメント、加速度による慣性、他の関節の動作による干渉トルクを受ける駆動関節に対して、各関節の質量並びに重心位置から発生する負荷トルク成分を計算し、加速度の影響を受ける負荷トルク成分が、駆動関節の許容ピークトルクから加減速度の影響を受けない負荷トルク成分の値を差し引いた値となるように、加減速時間を求める方法がとられていた。しかしながら、この方法にも、以下の問題がある。
【０００８】
（ａ）ロボットのすべての関節の加減速時間が同じという前提条件のもとで、加減速時間が演算されており、関節毎の移動量に大きな差があるような位置決め動作を行う場合、駆動関節の許容トルクに余裕があるにもかかわらず、動作時間を短縮できない。以下、第１の課題という。
（ｂ）ロボットのリンク自体の慣性テンソルによる負荷トルク成分を考慮していないため、関節の回転中心から重心位置までの距離が短いロボットでは厳密な負荷トルク成分の計算ができない。以下、第２の課題という。
【０００９】
本発明の目的は以上の問題点を解決し、ロボット機構部の各関節の許容ピークトルク（許容値）を越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求めることができ、しかも、動作時間を短縮できるロボットシステムの制御方法及び制御装置を提供することにある。
【００１０】
【課題を解決するための手段】
本発明に係るロボットシステムの制御方法は、複数の関節を有するロボット機構部の動作を制御するロボットシステムの制御方法であって、
上記ロボット機構部を動作させるときのロボット機構部の各関節の開始位置と、目標位置と、目標速度とを教示データとして第１の記憶手段に格納する第１の記憶ステップと、
上記ロボット機構部の各関節の速度パターンを予測演算するときに用いる各関節の加速度と減速度の情報とを第２の記憶手段に格納する第２の記憶ステップと、
上記第１の記憶手段に格納された教示データと、上記第２の記憶手段に格納された各関節の加速度及び減速度とに基づいて、当該各関節の開始位置と目標位置と目標速度である教示データから当該格納された各関節の加速度及び減速度で動作するように、各関節についての時間に対する速度のパターンであって、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターン、もしくは一定の加速度で動作し一定の減速度で動作する三角形状の速度パターンを計算する第１の速度パターン計算方法を用いて、上記ロボット機構部の各関節の加速度予測値と、動作時間予測値を演算し、上記動作時間予測値の中から、その最大値を最長動作時間として演算する第１の演算ステップと、
上記動作時間予測値の中から動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間とし、各関節について上記動作時間予測値が上記最長動作時間と同じになり、加速時の距離が再計算前後で同じでかつ減速時の距離が再計算前後で同じになり、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターンとなるように動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値として再計算する速度パターン再計算方法を用いて、上記ロボット機構部の各関節について、上記第１の演算ステップによって演算された動作時間予測値が、上記第１の演算ステップによって演算された最長動作時間と同じになるように、速度並びに加速度及び減速度として再計算した値を、速度予測値、加速度予測値及び減速度予測値とする第２の演算ステップとを含むロボットシステムの制御方法において、
上記ロボット機構部の各関節に連結される各リンクの慣性テンソル、質量及び重心位置とを含むロボットダイナミクスベースパラメータを第３の記憶手段に格納する第３の記憶ステップと、
上記第１の記憶手段に格納された各関節の開始位置及び目標位置と、上記第２の演算ステップによって演算された速度予測値と、上記第３の記憶手段に格納されたロボットダイナミクスベースパラメータとに基づいて、
（ａ）上記ロボット機構部の各関節についての、加速度により関節自身に発生するトルク、並びに他関節の加速度により発生する干渉トルクを示す慣性行列と、
（ｂ）上記ロボット機構部の各関節についての、遠心力及びコリオリ力により発生するトルクを示す粘性行列と、
（ｃ）上記ロボット機構部の各関節の重力モーメントにより発生するトルクを示す重力行列と
を含む上記開始位置及び目標位置におけるロボットダイナミクスモデルを計算する第３の演算ステップと、
上記第３の演算ステップによって演算されたロボットダイナミクスモデルと、上記第２の演算ステップによって演算された各関節の加速度予測値及び減速度予測値とに基づいて、当該各関節の加速度予測値が互いに比例して加速時に加速度予測値の各関節間の比例関係が保持されるという第１の比例関係と、当該各関節の減速度予測値が互いに比例して減速時に減速度予測値の各関節間の比例関係が保持されるという第２の比例関係とが保持される第１の条件を満足し、かつ各関節の許容ピークトルクから、加速度及び減速度に関係の無い重力行列から求められる重量項の値、並びに加速度及び減速度に関係の無い粘性行列から求められる粘性項の値を減算した減算結果値が、各関節が上記加速度予測値及び減速度予測値で動作した場合に各関節で発生するトルク以上となるという第２の条件を満足するような最適な加速度及び最適な減速度を各関節毎に演算する第４の演算ステップと、
上記第１の演算ステップにおいて最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、上記第４の演算ステップによって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された第ｋの関節の開始位置、目標位置及び目標速度を含む教示データとから、上記第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間とする第５の演算ステップと、
上記第ｋの関節以外のすべての関節について、上記第５の演算ステップによって演算された基準動作時間と、上記第４の演算ステップによって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された開始位置及び目標位置を含む教示データとに基づいて、最長動作時間である基準動作時間となった関節についてのみ、当該最適な加速度及び当該最適な減速度を用いて上記第１の速度パターン計算方法により基準動作時間を求め、その他の関節については、当該最適な加速度と当該最適な減速度を保持し、かつ動作時間が上記基準動作時間となるように各関節の台形状の速度パターンを計算する第２の速度パターン計算方法を用いて、動作時間が上記基準動作時間となり、上記最適な加速度及び最適な減速度で動作するような速度パターンを生成する第６の演算ステップとを含むことを特徴とする。
【００１１】
また、上記ロボットシステムの制御方法において、好ましくは、上記粘性行列及び上記重力行列において、加速度に関係しないトルクとして、上記粘性行列及び重力行列に加算することにより、上記各関節で発生する摩擦抵抗を考慮することを特徴とする。
【００１２】
さらに、上記ロボットシステムの制御方法において、好ましくは、各関節の慣性モーメントからなる慣性行列の対角成分に、上記各関節の減速機を含むモータのロータの慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正するステップとをさらに含むことを特徴とする。
【００１３】
本発明に係るロボットシステムの制御装置は、複数の関節を有するロボット機構部の動作を制御するロボットシステムの制御装置であって、
上記ロボット機構部を動作させるときのロボット機構部の各関節の開始位置と、目標位置と、目標速度とを教示データとして格納する第１の記憶手段と、
上記ロボット機構部の各関節の速度パターンを予測演算するときに用いる各関節の加速度と減速度の情報とを格納する第２の記憶手段と、
上記第１の記憶手段に格納された教示データと、上記第２の記憶手段に格納された各関節の加速度及び減速度とに基づいて、当該各関節の開始位置と目標位置と目標速度である教示データから当該格納された各関節の加速度及び減速度で動作するように、各関節についての時間に対する速度のパターンであって、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターン、もしくは一定の加速度で動作し一定の減速度で動作する三角形状の速度パターンを計算する第１の速度パターン計算方法を用いて、上記ロボット機構部の各関節の加速度予測値と、動作時間予測値を演算し、上記動作時間予測値の中から、その最大値を最長動作時間として演算する第１の演算手段と、
上記動作時間予測値の中から動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間とし、各関節について上記動作時間予測値が上記最長動作時間と同じになり、加速時の距離が再計算前後で同じでかつ減速時の距離が再計算前後で同じになり、一定の加速度で動作し一定の速度で動作し一定の減速度で動作する台形状の速度パターンとなるように動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値として再計算する速度パターン再計算方法を用いて、上記ロボット機構部の各関節について、上記第１の演算手段によって演算された動作時間予測値が、上記第１の演算手段によって演算された最長動作時間と同じになるように、速度並びに加速度及び減速度として再計算した値を、速度予測値、加速度予測値及び減速度予測値とする第２の演算手段とを備えたロボットシステムの制御装置において、
上記ロボット機構部の各関節に連結される各リンクの慣性テンソル、質量及び重心位置とを含むロボットダイナミクスベースパラメータを格納する第３の記憶手段と、
上記第１の記憶手段に格納された各関節の開始位置及び目標位置と、上記第２の演算手段によって演算された速度予測値と、上記第３の記憶手段に格納されたロボットダイナミクスベースパラメータとに基づいて、
（ａ）上記ロボット機構部の各関節についての、加速度により関節自身に発生するトルク、並びに他関節の加速度により発生する干渉トルクを示す慣性行列と、
（ｂ）上記ロボット機構部の各関節についての、遠心力及びコリオリ力により発生するトルクを示す粘性行列と、
（ｃ）上記ロボット機構部の各関節の重力モーメントにより発生するトルクを示す重力行列と
を含む上記開始位置及び目標位置におけるロボットダイナミクスモデルを計算する第３の演算手段と、
上記第３の演算手段によって演算されたロボットダイナミクスモデルと、上記第２の演算手段によって演算された各関節の加速度予測値及び減速度予測値とに基づいて、当該各関節の加速度予測値が互いに比例して加速時に加速度予測値の各関節間の比例関係が保持されるという第１の比例関係と、当該各関節の減速度予測値が互いに比例して減速時に減速度予測値の各関節間の比例関係が保持されるという第２の比例関係とが保持される第１の条件を満足し、かつ各関節の許容ピークトルクから、加速度及び減速度に関係の無い重力行列から求められる重量項の値、並びに加速度及び減速度に関係の無い粘性行列から求められる粘性項の値を減算した減算結果値が、各関節が上記加速度予測値及び減速度予測値で動作した場合に各関節で発生するトルク以上となるという第２の条件を満足するような最適な加速度及び最適な減速度を各関節毎に演算する第４の演算手段と、
上記第１の演算手段において最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、上記第４の演算手段によって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された第ｋの関節の開始位置、目標位置及び目標速度を含む教示データとから、上記第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間とする第５の演算手段と、
上記第ｋの関節以外のすべての関節について、上記第５の演算手段によって演算された基準動作時間と、上記第４の演算手段によって演算された最適な加速度及び最適な減速度と、上記第１の記憶手段に格納された開始位置及び目標位置を含む教示データとに基づいて、最長動作時間である基準動作時間となった関節についてのみ、当該最適な加速度及び当該最適な減速度を用いて上記第１の速度パターン計算方法により基準動作時間を求め、その他の関節については、当該最適な加速度と当該最適な減速度を保持し、かつ動作時間が上記基準動作時間となるように各関節の台形状の速度パターンを計算する第２の速度パターン計算方法を用いて、動作時間が上記基準動作時間となり、上記最適な加速度及び最適な減速度で動作するような速度パターンを生成する第６の演算手段とを備えたことを特徴とする。
【００１４】
また、上記ロボットシステムの制御装置において、好ましくは、上記粘性行列及び上記重力行列において、加速度に関係しないトルクとして、上記各関節で発生する摩擦抵抗を考慮することを特徴とする。
【００１５】
さらに、上記ロボットシステムの制御装置において、好ましくは、各関節の慣性モーメントからなる慣性行列の対角成分に、上記各関節の減速機を含むモータのロータの慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正する手段とをさらに備えたことを特徴とする。
【００１６】
【発明の実施の形態】
以下、図面を参照して本発明に係る実施形態について説明する。
【００１７】
図１は、本発明に係る一実施形態であるロボットシステムの構成を示すブロック図であり、図２は、図１のロボット機構部１の構成を示すスケルトン図である。図１において、この実施形態に係るロボットシステムは、例えば３個である複数の関節ＲＪ１乃至ＲＪ３を有するロボット機構部１と、ロボット機構部１の動作を制御するための制御装置１０と、教示データなどのデータを入力するための入力手段であるティーチペンダント２０とを備えて構成される。ここで、ロボット機構部１は、図２に示すように、ベースＢＡ１からリンクＬ１を介して関節ＲＪ１に連結され、次いで、関節ＲＪ１はリンクＬ２を介して関節ＲＪ２に連結され、そして、関節ＲＪ２はリンクＬ３を介して関節ＲＪ３に連結され、さらに、関節ＲＪ３にはリンクＬ４が連結され、そのリンクＬ４の端部は例えば溶接作業などの所定の操作を行う操作端ＬＥとなっている。ここで、各関節ＲＪ１乃至ＲＪ３は回転可能に構成されている。
【００１８】
この実施形態に係るロボットシステムの制御装置１０は、概略説明すれば、ロボット機構部１の各関節ＲＪ１乃至ＲＪ３を駆動するモータの性能を効率よく使用することで、タクトタイム（工程時間）の短縮、並びにマニピュレータ開発時のモータ選定裕度の向上を行い、具体的には、関節ＲＪ１乃至ＲＪ３に発生させることができる許容ピークトルクを指定しておき、ロボット機構部１の各関節ＲＪ１乃至ＲＪ３の開始位置、目標位置及び目標速度と、各関節に連結されるロボットリンク（リンクアームともいうが、本実施形態では、リンクＬ１乃至Ｌ４をいう。）の重心位置、質量及び慣性テンソルから成るダイナミクスベースパラメータから、各関節の許容ピークトルクを越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求める。この場合、ロボット機構部１の関節ＲＪ１乃至ＲＪ３毎の移動量に大きな差がある位置決め動作を行う場合でも、駆動関節の許容ピークトルクを最大限有効に使用した加減速度を演算し、動作時間の短縮を可能にするとともに、関節に連結されるリンク自体の慣性テンソルによる負荷トルク成分を考慮し、関節の回転中心からリンクの重心位置までの距離が短いロボットや、ロボットが特異な姿勢となっても、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。また、この実施形態に係るロボットシステムの制御装置１０は、モータＭＯ１乃至ＭＯ３の軸受けや減速機の摩擦抵抗を考慮し、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。さらに、この実施形態に係るロボットシステムの制御装置１０は、減速機を含むモータＭＯ１乃至ＭＯ３のロータの慣性モーメントにより発生するトルクを考慮し、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。
【００１９】
まず、この実施形態に係る制御方法の動作原理について以下に詳述する。ロボット機構部１の各関節に連結されるリンクの重心位置、質量、リンク長さ、慣性テンソル並びに、各関節の位置（角度）、速度、加速度から、各関節に発生するトルクＴが、次式により演算できることが知られており、ロボットダイナミクスモデルという。
【００２０】
【数１】
Ｔ＝Ｈ（θ）・θｄｄ＋Ｃ（θ，θｄ）＋Ｇ（θ）・ｇ
【００２１】
この数１は、例えば、従来技術文献１「広瀬茂男，”ロボット工学−機械システムのベクトル解析−”，裳華房，２版，１９８９年２月１０日」の１１．３．［４］項に、剛体の運動解析手法の１つであるニュートンオイラー法を用いた逆動力学演算の手順が詳しく説明されており、最終的に逆動力学の演算式は、当該従来技術文献１の式（１１．２８）となる。ここで、式（１１．２８）中のＪTＫは、ロボット先端に加えるべき外力とモーメントで、本発明に係る実施形態では考慮する必要の無い項となり、上記数１を得ることができる。ここで、上記ロボットダイナミクスモデルは、慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇから構成され、慣性行列Ｈはロボット機構部の各関節の加速度により関節自身に発生するトルク並びに他関節の加速度により発生する干渉トルクを示し、粘性行列Ｃはロボット機構部の各関節の遠心力及びコリオリ力により発生するトルクを示し、重力行列Ｇは、ロボット機構部の各関節の重力モーメントにより発生するトルクを示す。
【００２２】
慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇは、ロボット機構部１の各関節に連結されるリンクの重心位置、質量、慣性テンソル、及び各関節の位置、速度、加速度から計算できる。ここで、ロボット各関節に連結されるリンクの重心位置、質量、慣性テンソル、リンク長さは、ロボットの機構やリンク形状及び材質から予め求めておくことが可能な既知の固定値であるから、上記数１を使用すると、各関節の位置、速度、加速度の３つから、各関節に発生するトルクが演算できる。上記数１の各項目は以下の意味を持つ。なお、実施例では、各関節の数ｎ＝３である。
【００２３】
【表１】
0004358979.tif 000002
【００２４】
次いで、この実施形態で用いる各項目の単位及び個数を以下に示す。
【００２５】
【表２】
―――――――――――――――――――――――――――――――――――
記号        項目                      単位            個数
―――――――――――――――――――――――――――――――――――
Ｔ          関節に発生するトルク      Ｎ・ｍ          ｎ×１行列
θ          関節位置（角度）          ｒａｄ          ｎ×１行列
θｄ        関節速度                  ｒａｄ／ｓｅｃ  ｎ×１行列
θｄｄ      関節加速度                ｒａｄ／ｓｅｃ2  ｎ×１行列
Ｈ          慣性行列                  ｋｇ・ｍ2        ｎ×ｎ行列
Ｃ          粘性行列                  Ｎ・ｍ          ｎ×１行列
Ｇ          重力行列                  ｋｇ・ｍ        ｎ×３行列
ｇ          重力加速度ベクトル        ｍ／ｓ2          ３×１行列
Ｔｐｅａｋ  関節の許容ピークトルク    Ｎ・ｍ          ｎ×１行列
θｓ        開始位置                  ｒａｄ          ｎ×１
θｅ        目標位置                  ｒａｄ          ｎ×１
Ｖ          目標速度                  ｒａｄ／ｓｅｃ  ｎ×１
Ａｓｍｐ    仮の加速度                ｒａｄ／ｓｅｃ2  ｎ×１
Ｄｓｍｐ    仮の減速度                ｒａｄ／ｓｅｃ2  ｎ×１
Ｖｅｓｔ    加速度予測値              ｒａｄ／ｓｅｃ    ｎ×１
Ｔｅｓｔ    動作時間予測値            ｓｅｃ            ｎ×１
Ｖｒｅａ    速度予測値                ｒａｄ／ｓｅｃ    ｎ×１
Ａｒｅａ    加速度予測値              ｒａｄ／ｓｅｃ2  ｎ×１
Ｄｒｅａ    減速度予測値              ｒａｄ／ｓｅｃ2  ｎ×１
Ｔｍａｘ    最長動作時間              ｓｅｃ            １
―――――――――――――――――――――――――――――――――――
Ｉｘｉ，Ｉｘｙｉ，Ｉｘｚｉ  リンクの慣性テンソル  ｋｇ・ｍ2  ３×３
Ｉｘｙｉ，Ｉｙｉ，Ｉｙｚｉ
Ｉｘｚｉ，Ｉｙｚｉ，Ｉｚｉ
―――――――――――――――――――――――――――――――――――
ｍｉ        リンク質量                ｋｇ              １×ｎ
ｍｘｉ，ｍｙｉ，ｍｚｉ  リンク重心位置ベクトル  ｍ      ３×ｎ
記号なし    重力加速度                ｍ／ｓｅｃ2      １
Ｃｍ        動粘性係数                Ｎ・ｍ・ｓｅｃ    ｎ×１行列
θｄｍ      モータ速度                ｒａｄ／ｓｅｃ    ｎ×１行列
Ｆｃｏｕ    クーロン摩擦トルク        Ｎ・ｍ            ｎ×１行列
―――――――――――――――――――――――――――――――――――
Ｉｍ        減速機を含むモータロータの  ｋｇ・ｍ2        １×ｎ
慣性モーメント
―――――――――――――――――――――――――――――――――――
Ｔａｄ      加減速で使用できるトルク  Ｎ・ｍ            ｎ×１行列
Ａｏｐｔ    最適な加速度              ｒａｄ／ｓｅｃ2    ｎ×１行列
Ｄｏｐｔ    最適な減速度              ｒａｄ／ｓｅｃ2    ｎ×１行列
Ｔｂａｓｅ  基準動作時間              ｓｅｃ              １
Ｓａｉ      第ｉの関節の移動量        ｒａｄ              １
Ｖｏｐｔｉ  第ｉの関節の最適な速度    ｒａｄ／ｓｅｃ      １
Ｔａｉ      第ｉの関節の加速時間      ｓｅｃ              １
Ｔｃｉ      第ｉの関節の定速時間      ｓｅｃ              １
Ｔｄｉ      第ｉの関節の減速時間      ｓｅｃ              １
ξｉ        第ｉの関節の減速比        無次元              １
―――――――――――――――――――――――――――――――――――
【００２６】
次いで、実施例（ｎ＝３）におけるロボットダイナミクスのベースパラメータについて説明する。質量ｍｉは、第ｉの関節（ここで、ｉは自然数であり、１から関節の最大個数までとる。）の動作により移動しかつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの質量を示す。また、重心位置ｍｘｉ、ｍｙｉ、ｍｚｉは、第ｉの関節の動作により移動し、かつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの重心位置を示す。また、第ｉの関節の慣性テンソルＩｘｉ乃至Ｉｚｉは、第ｉの関節の動作により移動しかつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの慣性テンソルを示し、例えば、従来技術文献１の節３．８に、演算手順が詳しく説明されており、最終的に慣性テンソルの演算式は、当該従来技術文献１の式（３．３８）となる。
【００２７】
ここで、重心位置及び慣性テンソルを演算する上での基準をリンク座標系とする。リンク座標系は、例えば、従来技術文献２「吉川恒夫訳，”ロボット・マニピュレータ”，株式会社コロナ社，初版，１９８９年１２月１５日」の節２．９に各関節毎に割り当てるリンク座標系の定義手順が詳細に説明されているが、ここでは、基本的なリンク座標系の定義について説明する。
【００２８】
まず、慣性テンソルＩは、物体の回転し難さを示すのに一般的に使用され、公知の通り次式の３×３の対称行列で表され、その対角成分はロボット機構部の各関節の慣性モーメントを、その他の成分は慣性乗積の負の値を表す。
【００２９】
【数２】
0004358979.tif 000003
【００３０】
また、慣性モーメントは次式より求められ、物体（ロボットリンク）の回り難さ及び止め難さを表す。
【００３１】
【数３】
Ｉｘ＝∫（ｙ2＋ｚ2）ｄｍ
【数４】
Ｉｙ＝∫（ｚ2＋ｘ2）ｄｍ
【数５】
Ｉｚ＝∫（ｘ2＋ｙ2）ｄｍ
【００３２】
次いで、慣性乗積は、次式より求められ、物体（ロボットリンク）が回転運動を行っているとき、その運動を乱すモーメントを表す。
【００３３】
【数６】
Ｉｘｙ＝∫ｘｙｄｍ
【数７】
Ｉｙｚ＝∫ｙｚｄｍ
【数８】
Ｉｚｘ＝∫ｚｘｄｍ
【００３４】
ここで、式中のｘ，ｙ，ｚは、所定の慣性テンソルの基準座標で表され、ｄｍは微小体積の質量を示す。ここで、基準座標をリンク座標系とする。例えば、第２の関節から第３の関節に伸張するリンクＬ３において、第２の関節の回転中心を原点として、第２の関節に割り当てられたリンク座標が定められ、第２の関節のリンク座標におけるｘ方向をリンク長さの方向とし、ｚ方向を回転軸方向とし、ｙ方向を右手座標系の方向とする。ここで、第２の関節に連結されたリンクＬ３の慣性テンソルの基準座標は、第２の関節に割り当てられたリンク座標系をリンクＬ３の重心位置に移動したときの座標系とする。他の関節についても同様に定義する。
【００３５】
次いで、慣性、粘性及び重力の行列値の演算方法について説明する。慣性行列Ｈ、粘性行列Ｃ、及び重力行列Ｇは、次式を用いて数値計算して求める。なお、第１の関節の位置をθ1とし、第２の関節の位置をθ2とし、第３の関節の位置をθ3とする。また、第１の関節の速度をθｄ1とし、第２の関節の速度をθｄ2とし、第３の関節の速度をθｄ3とする。
【００３６】
【数９】
0004358979.tif 000004
【００３７】
ここで、
【数１０】
H11
=mx12・m1+[2・a1・mx2・cos(θ2)+(1/2)・mx22{1+cos(2・θ2)}+a12+mz22]m2
+(a12+2{cos(θ2)・a2-my3・sin(θ2+θ3)+mx3・cos(θ2+θ3)}a1
+[-my3{sin(2・θ2+θ3)+sin(θ3)}+mx3{cos(2・θ2+θ3)+cos(θ3)}]a2
-mx3・my3・sin(2・θ3+2・θ2)+(1/2)a22{1+cos(2・θ2)}
-(1/2)my32{cos(2・θ3+2・θ2)-1}
+(1/2)mx32{cos(2・θ3+2・θ2)+1)}m3+(1/2)Ix2{1-cos(2・θ2)}
+(1/2)Iy2{1+cos(2・θ2)}
+(1/2)Ix3{1-cos(2・θ3+2・θ2)}+(1/2)Iy3{cos(2・θ3+2・θ2)+1}
-Ixy3・sin(2・θ3+2・θ2)+Iz1
【数１１】
H12=-m2・mz2・mx2・sin(θ2)
【数１２】
H13=0
【００３８】
【数１３】
H21=H12
【数１４】
H22
=m2・mx22+[a22+2{mx3・cos(θ3)-my3・sin(θ3)}a2+my32+mx32]m3+Iz3+Iz2
【数１５】
H23=[{mx3・cos(θ3)-my3・sin(θ3)}a2+my32+mx32]m3+Iz3
【００３９】
【数１６】
H31=H13
【数１７】
H32=H23
【数１８】
H33=(my32+mx32)m3+Iz3
【００４０】
【数１９】
Ｃ＝［Ｃ１    Ｃ２    Ｃ３］T
【００４１】
ここで、
【数２０】
C1
=[{-mx2・m2(mx2・sin(2・θ2)+2・a1・sin(θ2))
+(-2・a1(mx3・sin(θ2+θ3)+my3・cos(θ2+θ3)+a2・sin(θ2))
-2・a2(mx3・sin(2・θ2+θ3)+my3・cos(2・θ2+θ3))-2・my3・mx3・cos(2・θ3+2・θ2)
+(my32-mx32)・sin(2・θ3+2・θ2)-a22・sin(2・θ2))m3
+(Ix3-Iy3)sin(2・θ3+2・θ2)
+(Ix2-Iy2)sin(2・θ2)-2・Ixy3・cos(2・θ3+2・θ2)}θd2
+{m3(-2・a1(my3・cos(θ2+θ3)+mx3・sin(θ2+θ3))
-2・my3・mx3・cos(2・θ3+2・θ2)
+(my32-mx32)sin(2・θ3+2・θ2)
-a2(mx3(sin(2・θ2+θ3)+sin(θ3))+my3(cos(2・θ2+θ3)+cos(θ3))}]
-2・Ixy3・cos(2・θ3+2・θ2)-(Iy3-Ix3)sin(2・θ3+2・θ2))θd3)θd1
-m2・mz2・mx2・cos(θ2)・θd22
【数２１】
C2
=[(1/2)mx2・m2(mx2・sin(2・θ2)+2・a1・sin(θ2))
+(1/2)(Iy3-Ix3)sin(2・θ3+2・θ2)
+(1/2)sin(2・θ2)(Iy2-Ix2)+Ixy3・cos(2・θ3+2・θ2)
+{a1(my3・cos(θ2+θ3)+mx3・sin(θ2+θ3))
+a2(mx3・sin(2・θ2+θ3)+my3・cos(2・θ2+θ3))
+(1/2)a22・sin(2・θ2)+(1/2)(mx32-my32)sin(2・θ3+2・θ2)
+my3・mx3・cos(2・θ3+2・θ2)
+a1・a2・sin(θ2)}m3]θd12-2・m3・a2(mx3・sin(θ3)+my3・cos(θ3))θd2・θd3
-m3・a2(mx3・sin(θ3)+my3・cos(θ3))θd32
【数２２】
C3=[{a1(my3・cos(θ2+θ3)+mx3・sin(θ2+θ3))
+(1/2)a2{mx3(sin(2・θ2+θ3)+sin(θ3))+my3(cos(2・θ2+θ3)+cos(θ3))}
+(1/2)(mx32-my32)sin(2・θ3+2・θ2)+my3・mx3・cos(2・θ3+2・θ2)}m3
+(1/2)(Iy3-Ix3)sin(2・θ3+2・θ2)+Ixy3・cos(2・θ3+2・θ2)]θd12
+m3・a2(mx3・sin(θ3)+my3・cos(θ3))θd22
【００４２】
【数２３】
0004358979.tif 000005
【００４３】
ここで、
【数２４】
G11
=-sin(θ1)・mx1・m1
+{cos(θ1)・mz2-sin(θ1)・a1-(1/2)mx2(sin(θ1+θ2)-sin(-θ1+θ2))}m2
-sin(θ1)・a1・m3-(1/2)m3((sin(θ1+θ2)-sin(-θ1+θ2))a2
+mx3(-sin(-θ1+θ2+θ3)+sin(θ1+θ2+θ3))
+my3(cos(θ1+θ2+θ3)-cos(-θ1+θ2+θ3)))
【数２５】
G12
=cos(θ1)・mx1・m1
+{sin(θ1)・mz2+cos(θ1)・a1+(1/2)mx2(cos(-θ1+θ2)+cos(θ1+θ2))}m2
+cos(θ1)・a1・m3+(1/2)m3{(cos(-θ1+θ2)+cos(θ1+θ2))a2
+mx3(cos(-θ1+θ2+θ3)+cos(θ1+θ2+θ3))
+my3(-sin(-θ1+θ2+θ3)-sin(θ1+θ2+θ3))}
【数２６】
G13=0
【００４４】
【数２７】
G31
=(1/2)m3・{mx3(-sin(-θ1+θ2+θ3)-sin(θ1+θ2+θ3))
-my3(cos(-θ1+θ2+θ3)+cos(θ1+θ2+θ3))}
【数２８】
G32
=(1/2)m3・{mx3(cos(θ1+θ2+θ3)-cos(-θ1+θ2+θ3))
-my3(-sin(-θ1+θ2+θ3)+sin(θ1+θ2+θ3))}
【数２９】
G33=m3(-my3・sin(θ2+θ3)+mx3・cos(θ2+θ3))
【００４５】
【数３０】
G21
=(1/2)mx2・m2(-sin(θ1+θ2)-sin(-θ1+θ2))
+(1/2)m3・a2(-sin(θ1+θ2)-sin(-θ1+θ2))
+(1/2)m3{mx3(-sin(-θ1+θ2+θ3)-sin(θ1+θ2+θ3))-my3(cos(-θ1+θ2+θ3)
+cos(θ1+θ2+θ3))}
【数３１】
G22
=(1/2)mx2・m2(-cos(-θ1+θ2)+cos(θ1+θ2))
+(1/2)m3・a2(-cos(-θ1+θ2)+cos(θ1+θ2))
+(1/2)m3(mx3(cos(θ1+θ2+θ3)-cos(-θ1+θ2+θ3))
-my3(-sin(-θ1+θ2+θ3)+sin(θ1+θ2+θ3)))
【数３２】
G23
=m2・mx2・cos(θ2)+cos(θ2)m3・a2+m3(-my3・sin(θ2+θ3)+mx3・cos(θ2+θ3))
【００４６】
上記数９乃至数３２は、表２に示すパラメータ、図２に示すリンク長さａ１、ａ２、並びに関節位置θ、関節速度θｄのみから構成され、ここで、θ、θｄ以外はすべて既知の値である。なお、ａ１はリンクＬ２のリンク長さであり、ａ２はリンクＬ３のリンク長さである。従って、開始位置θｓと速度予測値Ｖｒｅａを入力すれば、開始位置θｓにおけるロボットダイナミクスモデルを計算することができ、同様に、目標位置θｅと速度予測値Ｖｒｅａを入力すれば、目標位置におけるロボットダイナミクスモデルが計算できる。具体的には、以下の式に示す通りである。速度予測値Ｖｒｅａの演算方法については、後述するのでここでの説明は省略する。
【００４７】
【数３３】
開始位置の慣性行列：Ｈｓ＝Ｈ（θｓ）
【数３４】
開始位置の粘性行列：Ｃｓ＝Ｃ（θｓ，Ｖｒｅａ）
【数３５】
開始位置の重力行列：Ｇｓ＝Ｇ（θｓ）
【数３６】
目標位置の慣性行列：Ｈｅ＝Ｈ（θｅ）
【数３７】
目標位置の粘性行列：Ｃｅ＝Ｃ（θｅ，Ｖｒｅａ）
【数３８】
目標位置の重力行列：Ｇｅ＝Ｇ（θｅ）
【００４８】
ところで、ロボットの各関節に発生させることができるトルクは、モータや機械の設計仕様から制限される既知の値であり、これをＴｐｅａｋとすると、上述の数１の左辺を次式のように置き換えられる。
【００４９】
【数３９】
Ｔｐｅａｋ≧Ｈ（θ）・θｄｄ＋Ｃ（θ，θｄ）＋Ｇ（θ）・ｇ
【００５０】
ここで、Ｔｐｅａｋは、関節の許容ピークトルクｎ×１行列である。上記数３９を変形して次式の数４０の形式に変換する。数４０の左辺は、関節の許容ピークトルクＴｐｅａｋから、加減速度に関係の無い重力モーメントや遠心力、コリオリ力等の外乱トルクを差し引いた値であり、加減速に使用できるトルクを表す。数４０の右辺は、ロボット各関節が加減速度θｄｄで動作した場合に各関節で発生するトルクを表す。つまり、数４０は、第１の課題を満足させる加減速度を演算する上での条件式の１つとなる。
【００５１】
【数４０】
Ｔｐｅａｋ−（Ｃ(θ,θｄ)＋Ｇ(θ)・ｇ）≧Ｈ(θ)・θｄｄ
【００５２】
数４０において、Ｔｐｅａｋ及びｇは既知の固定値であり、また、Ｈ、Ｃ、Ｇは、ロボット各関節に連結されるリンクの重心位置、質量、慣性テンソル、リンク長さ、及び関節位置θ、関節速度θｄから演算されるロボットのダイナミクスモデルである。従って、上記数４０において、関節加速度θｄｄを除くと、未知の値は関節速度θｄのみとなる。なぜなら、ロボット各関節に連結されるリンクの重心位置、質量、慣性テンソル、リンク長さは、ロボットの機構やリンク形状及び材質等から予め求めておくことが可能な既知の固定値で、また関節位置θは、予め教示された開始位置あるいは目標位置を使用できるからである。
【００５３】
そのため、本実施形態では、数４０における関節速度θｄを予測する手段を持つ。ここでことわっておくが、θｄとして教示された目標速度を使用することはできない。すべての関節に対して最高速度での位置決め動作を要求しても、各関節ごとに移動量が違うことが一般的であり、目標位置へ到達する時間すなわち動作時間に差が発生する。そのため、すべての関節で動作時間を同じにすると、移動量の少ない関節は、目標速度より低い速度で動作する。同様の理由で、移動量の少ない関節は加減速度も抑えられるので、この考慮をしなければ、上述の第１の課題を解決する最適な値を演算することはできない。加減速度が抑えられる関節は、他の関節へ与える干渉力の影響を抑えることができる。そのため、本発明に係る実施形態では、関節毎の加減速度についても予測する手段を持つ。
【００５４】
関節毎の速度、加減速度の予測は、以下の手順で行う。まず、ロボットのいかなる姿勢でも正常に動作が可能な加速度及び減速度（それぞれ以下、仮の加速度、仮の減速度という。）を予め記憶しておく。次に、予め教示される開始位置θs、目標位置θe、及び目標速度Ｖから、上記した仮の加減速度が保持されるように、すべての関節の速度パターンを計算する。本発明では、この速度パターンの計算方法を、第１の速度パターン計算方法という。
【００５５】
この第１の速度パターン計算方法は、後述する図４の処理フローにおいて、ステップＳ２、Ｓ６及びＳ９において用いるものであり、その方法について以下に説明する。
【００５６】
ロボットの第ｉの関節における、第１の速度パターン計算方法は、第ｉの関節の教示される教示データの開始位置θｓｉ、目標位置θeｉ、目標速度Ｖｉから、目標加速度Ａｉ、目標減速度Ｄｉで動作するように、各関節の速度パターンを生成する公知の計算方法である。ここで、各関節の速度パターンとは、図７に示す台形状の速度パターン、又は図８に示す三角形状の速度パターンのことであり、第ｉの関節の加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作速度Ｖｒｉがこの第１の速度パターン計算方法により演算される。また、第ｉの関節の加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉを加算した値が速度パターンにおける動作時間Ｔｒｉとなる。第ｉの関節の場合、以下の式を使用することで、第ｉの関節の開始位置θsｉ、目標位置θeｉ、目標速度Ｖｉ、目標加速度Ａｉ、目標減速度Ｄｉから、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作時間Ｔｒｉ、動作速度Ｖｒｉを計算できる。
【００５７】
【数４１】
Ｔａｉ＝Ｖｉ／Ａｉ
【数４２】
Ｔｄｉ＝Ｖｉ／Ｄｉ
【数４３】
Ｔｃｉ＝｛θｅｉ−θｓｉ−Ｖｉ（Ｔａｉ＋Ｔｄｉ）／２｝／Ｖｉ
【数４４】
Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数４５】
Ｖｒｉ＝Ｖｉ
【００５８】
ここで、上記数４３により求められた定速時間Ｔｃｉが０未満となったときは、図８に示す三角形状の速度パターンとなるケースで、この場合は、以下の式により、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作時間Ｔｒｉ、及び動作速度Ｖｒｉを計算できる。
【００５９】
【数４６】
0004358979.tif 000006
【数４７】
Ｔａｉ＝Ｖｉ／Ａｉ・ｓｆ
【数４８】
Ｔｄｉ＝Ｖｉ／Ｄｉ・ｓｆ
【数４９】
Ｔｃｉ＝０
【数５０】
Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数５１】
Ｖｒｉ＝Ｖｉ・ｓｆ
【００６０】
次に、すべての関節の速度パターンの中から、動作時間が最大となる関節（以下、最大動作関節という。）を抽出し、すべての関節において速度パターンの動作時間が、最大動作関節の動作時間と同じになるように速度パターンの再生成を行う。この処理を、終了時間の同期処理という。終了時間の同期処理については、公知の技術であるので説明は省略する。以上の処理の一例を図１０及び図１１に示す。図１０及び図１１からも判るように、第１の速度パターン計算を行い、終了時間の同期処理を行うことで、速度予測値Ｖｒｅａ、加速度予測値Ａｒｅａ、減速度予測値Ｄｒｅａを演算できる。
【００６１】
速度予測値Ｖｒｅａを上記数４０の関節速度θｄに代入することにより、上記数４０は関節の加速度又は減速度であるθｄｄ以外はすべて既知の値となる。また、他の関節の動作により干渉トルクを受ける駆動関節の許容ピークトルクを最適に配分するために、演算された加速度予測値Ａｒｅａ及び減速度予測値Ｄｒｅａの各関節間の比例関係を求め、この比例関係と上記数４０とを同時に満足する加減速度を求める。つまり、加速時においては、予測された加速度の各関節間の比例関係が保たれ、かつ上記数４０を満足させる加速度が、前述の第１の課題を解決する最適な加速度Ａｏｐｔとなり、減速時においては、予測された減速度の各関節間の比例関係が保たれ、かつ上記数４０を満足させる減速度が、前述の第１の課題を解決する最適な減速度Ｄｏｐｔとなる。
【００６２】
以上の結果、最適な加速度及び減速度が求まるがこれだけでは最適な速度パターンが計算できたとは言えない。なぜなら、最適な加速度Ａｏｐｔと最適な減速度Ｄｏｐｔを使用しても、速度パターンの生成手段が従来と同じであれば、関節毎に動作時間が変わり、図１０及び図１１に示すような従来と同様の終了時間の同期処理を施さねばならず、これにより最適な加減速度が保持されないためである。そのため、本発明に係る実施形態では、計算の結果求められた最適な加減速度が保持されるような、速度パターン生成処理を含む。以下に、その手順を説明する。
【００６３】
前述した終了時間の同期処理において、最長動作時間となった関節についてのみ、既に求めた最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを用いて、前述した第１の速度パターン計算方法により基準動作時間Ｔｂａｓｅを求める（図４のステップＳ６）。その他の関節については、最適な加速度Ａｏｐｔと最適な減速度Ｄｏｐｔを保持し、かつ動作時間が上記基準動作時間Ｔｂａｓｅとなるように、第２の速度パターン計算方法を用いた速度パターン生成処理を実行する（図４のステップＳ７）。これにより、すべての関節の動作時間が最長動作関節と同じになるので、終了時間の同期処理を施す必要が無くなり、先に計算された最適な加減速度が保持される。
【００６４】
先に求められた最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを保持し、かつ動作時間が上記基準動作時間Ｔｂａｓｅとなるような速度パターンが実現できない関節が存在した場合は（図４のステップＳ８でＮＯ）、その関節のみ第１の速度パターン計算を行い、すべての関節の速度パターン計算が終了した（図４のステップＳ９）後に、前述の終了時間の同期処理（図４のステップＳ１０）を施せばよい。このようなことが起こるのは、加減速度を最適化したことで、最適化前とは最大動作関節が変わってしまうケースである。終了時間の同期処理を施すことで、加減速度の最適度合いが若干低下するが、加減速に使用できるトルクを越えることは無い。
【００６５】
以上のように演算される速度パターンで各関節を動作させることにより、第１の課題を解決できる。第２の課題については、上記数４０の慣性行列Ｈ及び粘性行列Ｃが関節に連結されるリンクの慣性テンソルを含んだものになるため、必然的に達成可能となる。
【００６６】
また、別の実施形態において、各関節内のモータの軸受けや減速機の摩擦抵抗を考慮した場合について説明する。上記数１で表されるロボットダイナミクスモデルを、次式のように変更することにより、各関節内のモータの軸受けや減速機の摩擦抵抗を考慮できる。
【００６７】
【数５２】
0004358979.tif 000007
【００６８】
ここで、Ｃｍはモータの軸受けや減速機の動粘性係数ｎ×１行列であり、動粘性係数とは、速度と動摩擦トルクとの比例関係を表す定数である。また、動摩擦トルクとは、速度に比例した大きさを持つトルク成分である。さらに、θｄｍはモータの速度ｎ×１行列であり、Ｆｃｏｕはクーロン摩擦トルクである。クーロン摩擦トルクとは、速度に無関係な大きさを持ち、常に運動と逆の方向に働くトルク成分である。上記数５２において、ｓｇｎ（θｄｍ）はモータの速度ｎ×１行列に基づいて次式のように決定される符号関数である。
【００６９】
【数５３】
ｓｇｎ（θｄｍ）
＝１；θｄｍ＞０のとき
＝０；θｄｍ＝０のとき
＝−１；θｄｍ＜０のとき
【００７０】
また、第ｉの関節の速度とモータの回転速度の関係はその減速比ξｉから次式のように表される。
【００７１】
【数５４】
θｄｍｉ＝θｄｉ・ξｉ
【００７２】
さらに、上記数５２のθｄｍは、次式で表される。
【００７３】
【数５５】
θｄｍ＝［θｄ1・ξ1，θｄ2・ξ2，θｄ3・ξ3］T
【００７４】
ここで、ξｉは、第ｉの関節内の減速機の減速比である。ここで、パラメータＣｍ、Ｆｃｏｕ、ξも、予め設定可能な既知の値であるから、上記数５２であっても、上述と同様に最適な加減速度の演算が可能であるので、詳細については省略する。
【００７５】
さらに、そのほかの別の実施形態においては、上述の数９に示す慣性行列Ｈを以下のように補正することにより、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクの考慮が可能になる。
【００７６】
【数５６】
0004358979.tif 000008
【００７７】
ここで、Ｉｍｉは減速機を含むモータロータの慣性モーメントである。連動して動く回転体の慣性モーメントは、その減速比の２乗分の１倍されて入力軸の慣性モーメントに影響を与える。従って、入力軸の慣性モーメントが出力軸に与える影響はその逆であるため、減速比の２乗倍されなければならない。減速機を含むモータロータの慣性モーメントが減速比の２乗倍されて加算されているのはそのためである。
【００７８】
慣性行列Ｈの対角要素は自分自身に与えるトルクの影響を示し、他の要素は他の関節に与える干渉トルクの影響を示している。減速機を含むモータロータの慣性モーメントが他の関節に干渉することは無いので、対角要素のみ補正すればよい。従って、Ｉｍｉ、ξｉも、予め設定可能な既知の値であるから、上記数５６であっても、上述と同様に最適な加減速度の演算が可能であるので、詳細については省略する。
【００７９】
次いで、図１を参照して、本実施形態に係るロボットシステムの構成について説明する。図１に示すように、制御装置１０は、
（ａ）クロック発生器１１０によって発生されるクロック信号と当該クロック信号を所定の分周比で分周する分周器１１１によって発生される同期信号ＳＹＮＣに基づいてＲＯＭ１０１に格納されたシステムプログラムに従って、ロボット機構部１の動作を制御するＣＰＵ１００と、
（ｂ）ロボット機構部１の動作を制御するための位置決め動作処理を含むプログラムと、当該プログラムを実行するために必要なデータを格納するＲＯＭ１０１と、
（ｃ）ＣＰＵ１００のワーキングエリアとして用いられ、計算途中のデータを一時的に格納するＲＡＭ１０２と、
（ｄ）上記位置決め動作処理を実行するために必要なデータを予め格納する記憶装置であって、教示データメモリ１０６ａと、仮の加減速度情報メモリ１０６ｂと、ロボットダイナミクスベースパラメータメモリ１０６ｃとを含むハードディスクメモリ１０６と、
（ｅ）ティーチペンダント２０が接続されたインターフェース１０３と、
（ｆ）各モータＭＯ１乃至ＭＯ３に対する駆動制御値であるモータ指令位置のデータを格納するデュアルポートＲＡＭ１０４とを備える。
これらの各回路１００乃至１０３及び１０６とデュアルポートＲＡＭ１０４の第１のポートがバス１０９を介して接続される。なお、ティーチペンダント２０についての処理はＣＰＵ１００の割り込み処理によって実行される。
【００８０】
ハードディスクメモリ１０６内の教示データメモリ１０６ａは、ロボット機構部１を動作させるときのロボット機構部１の各関節の開始位置と、目標位置と、目標速度とを、教示データとして予めティーチペンダント２０を用いて入力して格納する。また、仮の加減速度情報メモリ１０６ｂは、速度パターンを予測演算するときに用いる各関節の仮の加速度と仮の減速度の情報を予めティーチペンダント２０を用いて入力して格納する。さらに、ロボットダイナミクスベースパラメータメモリ１０６ｃは、ロボット機構部１の各関節に連結されるリンクの慣性テンソルと、質量と、重心位置とを含むロボットダイナミクスベースパラメータをティーチペンダント２０を用いて入力して格納する。
【００８１】
デュアルポートＲＡＭ１０４の第２のポートはラッチ回路１０５に接続され、ラッチ回路１０５は同期信号ＳＹＮＣに同期して、デュアルポートＲＡＭ１０４に格納された各モータＭＯ１乃至ＭＯ３に対する３個のモータ指令位置のデータを同時にラッチした後、ロボット機構部１の関節制御に関するモータ指令位置のデータをサーボ制御回路１０７を介してモータＭＯ１乃至ＭＯ３に出力して駆動する。
【００８２】
次いで、ロボット機構部１の各関節ＲＪｋ（実施例では、ｋ＝１，２，３）の構造とサーボ制御回路１０７の構成について図３を参照して説明する。図３に示すように、モータＭＯｋの回転軸は減速機ＲＥｋの入力軸に連結され、減速機ＲＥｋはモータＭＯｋの回転速度を所定の減速比λｋだけ減速することによって、モータＭＯｋによって発生されるトルクを上記減速比の逆数倍だけ増大させて、減速機ＲＥｋの出力軸に連結されたリンクＬｋに出力する。ここで、モータＭＯｋの回転軸にはセンサＳＮｋの軸が連結され、当該センサＳＮｋからの出力信号がサーボ制御回路１０７内の加算器１２０の減算入力端子に入力される。一方、ラッチ回路１０５からのモータ指令位置ＤＰｋのデータが加算器１２０の加算入力端子に入力される。加算器１２０から出力されるデータは、Ｄ／Ａ変換器（図示せず。）によってアナログ電圧信号に変換された後、所定の増幅度を有する増幅器１２１を介してモータＭＯｋの駆動端子に印加される。以上のように構成されたサーボ制御のフィードバック系においては、回転角度がモータ指令位置ＤＰｋとなるように制御され、これによって、モータと減速機を介して連結されるリンクＬｋが、ＤＰｋを減速比で除した分である関節変数θｋだけ回転される。
【００８３】
図４は、図１のＣＰＵ１００によって実行される位置決め動作処理を示すフローチャートである。
【００８４】
図４において、まず、ステップＳ１において、教示データメモリ１０６ａ内の教示データに基づいて、ロボット機構部１の各関節の開始位置、目標位置及び目標速度を設定し、ステップＳ２において、上記ステップＳ１で設定されたデータと、仮の加減速度情報メモリ１０６ｂ内の仮の加減速度情報とに基づいて、第１の速度パターン計算を行い、ロボット機構部１の各関節の動作時間と加速度予測値とを演算する。この処理は、図１０の従来例のステップＳ１０３及びＳ１０４と同様の処理である。具体的には、第１の速度パターン計算方法における目標加速度Ａとして仮の加速度Ａｓｍｐを、目標減速度Ｄとして仮の減速度Ｄｓｍｐを使用する。また、第１の速度パターン計算方法により演算される動作速度Ｖｒを加速度予測値Ｖｅｓｔとし、動作時間Ｔｒを動作時間予測値Ｔｅｓｔとする。
【００８５】
さらに、ステップＳ３において、ステップＳ２で演算された全ての関節の速度パターンに対して終了時間の同期処理を実行する。この終了時間の同期処理は、図１０の従来例のステップＳ１０９乃至Ｓ１１１の処理（以下、従来例の速度パターン再計算処理という。）と同様の処理であり、上記動作時間予測値の中から、動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間Ｔｍａｘとする。そして、各関節について、上記動作時間予測値が上記最長動作時間と同じになるように、動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値としてＲＡＭ１０２に一時的に格納する。
【００８６】
そして、ステップＳ４においてロボットダイナミクスベースパラメータメモリ１０６ｃ内のロボットダイナミクスベースパラメータに基づいて、開始位置及び目標位置のロボットダイナミクスモデルの演算処理を実行する。この処理では、上記開始位置及び目標位置と上記速度予測値と上記ロボットダイナミクスベースパラメータから、数９乃至数３８を用いて、開始位置及び目標位置における慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇを含むロボットダイナミクスモデルを演算してロボットダイナミクスベースパラメータメモリ１０６ｃに格納する。さらに、ステップＳ５において最適な加減速度の演算を実行する。この処理では、上記演算されたロボットダイナミクスモデルに基づいて、上記数４０と、上記加速度予測及び減速度予測における各関節間の比例関係を同時に満足する加減速度を、最適な加減速度として求める。つまり、加速時においては、加速度予測値Ａｒｅａの各関節間の比例関係（後述する数７６）が保たれ、かつ数４０を満足するような最適な加速度Ａｏｐｔを演算する一方、減速時においては、減速度予測値Ｄｒｅａの各関節間の比例関係（後述する数８９）が保たれ、かつ数４０を満足するような最適な減速度Ｄｏｐｔを演算し、演算された最適な加速度Ａｏｐｔ及び最適な減速度ＤｏｐｔをＲＡＭ１０２内に格納する。
【００８７】
次いで、ステップＳ６では、ステップＳ３の処理結果に基づいて、最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、ステップＳ５で演算された最適な加速度Ａｏｐｔｋと、最適な減速度Ｄｏｐｔｋと、第ｋ関節の教示データ（開始位置、目標位置、目標速度）とから、上述の第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間Ｔｂａｓｅとし、演算結果をＲＡＭ１０２に格納する。さらに、ステップＳ７では、上記の第ｋの関節以外のすべての関節について、第２の速度パターン計算方法を用いて、基準動作時間Ｔｂａｓｅと、最適な加速度Ａｏｐｔと、最適な減速度Ｄｏｐｔと、教示データ（開始位置、目標位置）とから、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンを生成してＲＡＭ１０２に格納する。そして、ステップＳ８では、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンがすべての関節で存在したか否かが判断され、ＹＥＳのときは当該位置決め動作処理を終了するが、ＮＯのときはステップＳ９に進む。
【００８８】
ステップＳ９では、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンが存在しなかったすべての関節について、第１の速度パターン計算方法を用いて、最適な加速度Ａｏｐｔと、最適な減速度Ｄｏｐｔと、教示データ（開始位置、目標位置、目標速度）から速度パターンを演算してＲＡＭ１０２に格納する。最後に、ステップＳ１０では、ステップＳ６、Ｓ７及びＳ９で演算された速度パターンに基づいて、すべての関節の速度パターンの中から最長動作時間を抽出し、従来例の速度パターン再計算処理と同様に、すべての関節について、最長動作時間と一致するように、速度パターンを再計算して計算結果をＲＡＭ１０２に格納する。
【００８９】
この位置決め動作処理の後、生成された速度パターンに基づいて、公知の通り、モータ指令位置ＤＰｋのデータを計算してデュアルポートＲＡＭ１０４、ラッチ回路１０５及びサーボ制御回路１０７を介してロボット機構部１に出力することにより、最適に位置決め動作処理がなされたデータに基づいて、ロボット機構部１の動作を制御することができる。
【００９０】
なお、以上の実施形態において、上記数５２に示すように、加速度に関係しないトルク成分として、粘性行列Ｃ及び重力行列Ｇに加え、モータＭＯ１乃至ＭＯ３の軸受けや減速機ＲＥ１乃至ＲＥ３から発生する摩擦抵抗を考慮することで、加速に使用することのできるトルクの演算精度を向上させてもよい。また、上記数５６に示すように、慣性行列Ｈの対角成分に、減速機ＲＥ１乃至ＲＥ３を含むモータＭＯ１乃至ＭＯ３のロータの慣性モーメントを加算することで、自分自身の加減速で発生するトルク演算値を補正してもよい。
【００９１】
図４のステップＳ８において、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターン存在しない関節があったとき（ステップＳ８でＮＯ）、その関節を最長動作時間の関節の番号をｋとし、ステップＳ６からの処理を再度行ってもよい。
【００９２】
【実施例】
図２に示すロボット機構部１（ｎ＝３）における、位置決め動作のための最適な加減速度の演算例を図４のフローチャートを参照して説明する。
【００９３】
ステップＳ１では、ロボット機構部１の各関節について、位置決め動作の開始位置、目標位置及び目標速度を、教示データメモリ１０６ａ内の教示データから読み出して以下のようにセットする。
【００９４】
【数５７】
開始位置：θｓ＝［θｓ１，θｓ２，θｓ３］T
【数５８】
目標位置：θｅ＝［θｅ１，θｅ２，θｅ３］T
【数５９】
目標速度：Ｖ＝［Ｖ１，Ｖ２，Ｖ３］T
【００９５】
一方、仮の加減速度情報メモリ１０６ｂには、ロボットのすべての姿勢で動作が可能な仮の加減速度情報の値が以下のように予めセットされる。
【００９６】
【数６０】
仮の加速度：Ａｓｍｐ＝［Ａｓｍｐ１，Ａｓｍｐ２，Ａｓｍｐ３］T
【数６１】
仮の減速度：Ｄｓｍｐ＝［Ｄｓｍｐ１，Ｄｓｍｐ２，Ｄｓｍｐ３］T
【００９７】
そして、ステップＳ２では、上記セットされた開始位置、目標位置、目標速度、仮の加速度及び仮の減速度に基づいて、各関節毎に前述の第１の速度パターン計算を行う。第１の速度パターンの計算は、従来例と同様である。第１の速度パターン計算により、以下の値が予測される。その概念を図５に示す。図５の例では、第１の関節及び第２の関節は、目標速度に達するに必要な移動量があったが、第３の関節は移動量が少なく、目標速度に達していない。
【００９８】
【数６２】
加速度予測値：Ｖｅｓｔ＝［Ｖｅｓｔ１，Ｖｅｓｔ２，Ｖｅｓｔ３］T
【数６３】
動作時間予測値：Ｔｅｓｔ＝［Ｔｅｓｔ１，Ｔｅｓｔ２，Ｔｅｓｔ３］T
【００９９】
さらに、ステップＳ３においては、前述の終了時間の同期処理後におけるロボット各関節の速度及び加減速度の値を以下のように再予測する。
【０１００】
【数６４】
速度予測値：Ｖｒｅａ＝［Ｖｒｅａ１，Ｖｒｅａ２，Ｖｒｅａ３］T
【数６５】
加速度予測値：Ａｒｅａ＝［Ａｒｅａ１，Ａｒｅａ２，Ａｒｅａ３］T
【数６６】
減速度予測値：Ｄｒｅａ＝［Ｄｒｅａ１，Ｄｒｅａ２，Ｄｒｅａ３］T
【０１０１】
上記動作時間予測値における最長動作時間をＴｍａｘとおくと、上記の値は、以下の式から演算できる。終了時間の同期処理の概念を図６に示す。図６の予測例では、移動量の少ない第２の関節及び第３の関節の速度及び加減速度が低下しているのがわかる。
【０１０２】
【数６７】
Ｖｒｅａｉ＝Ｖｅｓｔｉ・Ｔｅｓｔｉ／Ｔｍａｘ
【数６８】
Ａｒｅａｉ＝Ａｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）2
【数６９】
Ｄｒｅａｉ＝Ｄｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）2
【０１０３】
なお、ロボットダイナミクスベースパラメータメモリ１０６ｃは、上記数１で表されるロボットダイナミクスモデルを生成するための、ロボットダイナミクスベースパラメータを予め格納する。ロボットダイナミクスベースパラメータの一覧を以下の表に示す。
【０１０４】
【表３】
0004358979.tif 000009
【０１０５】
次いで、ステップＳ４では、上記ロボットダイナミクスベースパラメータと、ロボット機構部の各関節の位置及び速度から、上記数１で表現されるロボットダイナミクスモデルを生成する。ロボットダイナミクスモデルは、慣性行列Ｈと、粘性行列Ｃと、重力行列Ｇから構成される。図２の機構を持つロボット機構部１におけるダイナミクスモデルを、ニュートン及びオイラー法に基づき計算したものが数９乃至数３８に示されている。これら数９乃至数３８に示されたモデル式において、未知の値は、関節の位置及び速度のみであり、これに開始位置θｓと、速度予測値Ｖｒｅａとを入力すれば、開始位置におけるロボットダイナミクスモデルを計算できる。同様に、目標位置θｅと、速度予測値Ｖｒｅａを入力すれば、目標位置におけるロボットダイナミクスモデルを計算できる。従って、ステップＳ４では、以下のロボットダイナミクスモデルが計算される。
【０１０６】
【表４】
0004358979.tif 000010
【０１０７】
次いで、ステップＳ５では、最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを、以下の手順で演算する。最初は、最適な加速度Ａｏｐｔを求める。そして、Ｈｓ、Ｇｓ、Ｃｓをそれぞれ数４０のＨ、Ｇ、Ｃに代入し、最適な加速度Ａｏｐｔ（又は最適な減速度Ｄｏｐｔ）を数４０のθｄｄとする。また、簡略化のため数４０の左辺を１つにまとめると次式のように表すことができる。
【０１０８】
【数７０】
Ｔａｄ≧Ｈｓ（θｓ)・Ａｏｐｔ
【０１０９】
数７０の左辺のＴａｄは、次式で表され、加速時においては加速に使用できるトルクであり、減速時においては減速に使用できるトルクとなる。ここでは、加速度を求めようとしているので、Ｔａｄは加速に使用できるトルクを表す。加速時と減速時とでは、使用するトルクの符号が異なるので、前述した終了時間の同期処理後における各関節加速度の予測値Ａｒｅａの符号を考慮する。
【０１１０】
【数７１】
Ｔａｄ
＝ｓｇｎ２（Ａｒｅａ）・Ｔｐｅａｋ−(Ｃｓ(θｓ，Ｖｒｅａ)
＋Ｇ(θｓ)・ｇ）
【０１１１】
ここで、符号関数ｓｇｎ２は次式で表される。
【０１１２】
【数７２】
ｓｇｎ２（Ａｒｅａ）
＝１；Ａｒｅａ≧０のとき
＝−１；Ａｒｅａ＜０のとき
【０１１３】
上記数７０を要素別に展開すると、次式のように表される。Ｔａｄは、加速に使用できるトルクを表しているから、次式が加速に使用できるトルクを最大限利用するための加速度を求める条件式となる。
【０１１４】
【数７３】
Ｔａｄ１
≧Ｈｓ１１・Ａｏｐｔ１＋Ｈｓ１２・Ａｏｐｔ２＋Ｈｓ１３・Ａｏｐｔ３
【数７４】
Ｔａｄ２
≧Ｈｓ２１・Ａｏｐｔ１＋Ｈｓ２２・Ａｏｐｔ２＋Ｈｓ２３・Ａｏｐｔ３
【数７５】
Ｔａｄ３
≧Ｈｓ３１・Ａｏｐｔ１＋Ｈｓ３２・Ａｏｐｔ２＋Ｈｓ３３・Ａｏｐｔ３
【０１１５】
前述した終了時間の同期処理後における各関節加速度の予測値はＡｒｅａであった。各関節の最適な加速度Ａｏｐｔも、加速度予測値Ａｒｅａと同じ比例関係を保つものとすると、次式の関係を満たさなければならない。
【０１１６】
【数７６】
Ａｏｐｔ１：Ａｏｐｔ２：Ａｏｐｔ３＝Ａｒｅａ１：Ａｒｅａ２：Ａｒｅａ３
【０１１７】
上記数７３乃至７５及び数７６の関係を満たす加速度Ａｏｐｔが、加速度予測値Ａｒｅａの各関節間の比例関係を保ち、かつ加速に使用できるトルクを最大限利用できる加速度であり、本実施形態において、最適となる値となる。基準関節は、動作する関節であればどれでも良いが、本実施例では、Ａｒｅａの中で絶対値が最大となる関節を求め、これを基準関節ｊとする。基準関節ｊをベースに上記数７６を次式のように変形する。
【０１１８】
【数７７】
Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・Ａｏｐｔｊ
【数７８】
Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・Ａｏｐｔｊ
【数７９】
Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・Ａｏｐｔｊ
【０１１９】
ここで、Ａｒｅａｊは基準関節ｊの予測加速度であり、Ａｏｐｔｊは基準関節ｊの最適な加速度である。次に、上記数７７乃至７９を上記数７３乃至７５に代入して展開すると、次の３個の不等式になる。
【０１２０】
【数８０】
Tad1
≧H11・(Area1/Areaj)・Aoptj+H12・(Area2/Areaj)・Aoptj
+H13・(Area3/Areaj)・Aoptj
【数８１】
Tad2
≧H21・(Area1/Areaj)・Aoptj+H22・(Area2/Areaj)・Aoptj
+H23・(Area3/Areaj)・Aoptj
【数８２】
Tad3
≧H31・(Area1/Areaj)・Aoptj+H32・(Area2/Areaj)・Aoptj
+Hn3・(Area3/Areaj)・Aoptj
【０１２１】
上記数８０乃至８２の３個の不等式において、基準関節ｊにおける最適な加速度Ａｏｐｔｊ以外はすべて既知の値であるから、上記数８０乃至８２の３個の不等式から、基準関節ｊにおける最適な加速度が３通り計算できる。このときに計算される３個の加速度をそれぞれを、Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３とすると、次式を得る。
【０１２２】
【数８３】
Aoptj_1=Tad1・Areaj/(H11・Area1+H12・Area2+H13・Area3)
【数８４】
Aoptj_2=Tad2・Areaj/(H21・Area1+H22・Area2+H23・Area3)
【数８５】
Aoptj_3=Tad3・Areaj/(H31・Area1+H32・Area2+H33・Area3)
【０１２３】
上記数８３乃至８５に示す加速度Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３から、絶対値が最小の値を基準関節ｊの最適な加速度としなければならない。なぜなら、絶対値が最小の値のみが、上記数８０乃至数８２の３個の不等式を同時に満たすことができるからである。加速度Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３の中で最小の値を最小の加速度Ａｏｐｔｊ＿ｍｉｎとし、これを上記数７７乃至数７９に代入すると次式となり、これによりすべての関節の最適な加速度を求めることができる。
【０１２４】
【数８６】
Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【数８７】
Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【数８８】
Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【０１２５】
このようにして求められた加速度が、第１の課題を満たす最適な加速度となる。最適な減速度Ｄｏｐｔについても、全く同様の手順で求めることができる。具体的には、上記数７０乃至数８８において、パラメータθｓ、Ｈｓ、Ｃｓ、Ｇｓ、Ａｏｐｔ、Ａｒｅａの代わりにそれぞれ、パラメータθｅ、Ｈｅ、Ｃｅ、Ｇｅ、Ｄｏｐｔ、Ｄｒｅａを使用すればよい。ここで、加速度に関する数７６に対応する減速度に関する式は次式となる。
【０１２６】
【数８９】
Ｄｏｐｔ１：Ｄｏｐｔ２：Ｄｏｐｔ３＝Ｄｒｅａ１：Ｄｒｅａ２：Ｄｒｅａ３
【０１２７】
次いで、ステップＳ６では、以上の計算の結果求められた最適な加減速度が保持されるような、速度パターンの演算処理を行う。以下に、その手順を説明する。ステップＳ３の終了時間の同期処理において、最長動作時間となった第ｋ関節についてのみ、最適な加速度Ａｏｐｔｋ、最適な減速度Ｄｏｐｔｋを用いて、前述した第１の速度パターン計算により基準動作時間Ｔｂａｓｅを求める。次いで、ステップＳ７では、第ｋ関節以外の関節について、加速度が最適な加速度Ａｏｐｔとなり、減速度が最適な減速度Ｄｏｐｔとなり、かつ動作時間が基準動作時間Ｔｂａｓｅとなるように、第２の速度パターン計算を行う。ステップＳ６、Ｓ７により、すべての関節の速度パターンにおける動作時間が基準動作時間Ｔｂａｓｅとなるので、終了時間の同期処理を施す必要が無くなり、先に計算された最適な加減速度が保持される。
【０１２８】
ロボットの第ｉの関節における、第２の速度パターン計算方法について図９を交えて具体的に説明する。図９から判るように、既知の値である基準動作時間Ｔｂａｓｅ、第ｉの関節の最適な加速度Ａｏｐｔｉ、第ｉの関節の最適な減速度Ｄｏｐｔｉ、第ｉの関節の移動量Ｓａｉは、第ｉの関節の最適な速度Ｖｏｐｔｉ、第ｉの関節の加速時間Ｔａｉ、第ｉの関節の定速度時間Ｔｃｉ、第ｉの関節の減速時間Ｔｄｉにより以下の式で表される。
【０１２９】
【数９０】
Ｔｂａｓｅ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数９１】
Ｓａｉ＝Ｖｏｐｔｉ・（Ｔｃｉ＋（Ｔａｉ＋Ｔｄｉ）／２）
【数９２】
Ｖｏｐｔｉ＝Ｔａｉ・Ａｏｐｔｉ
【数９３】
Ｖｏｐｔｉ＝Ｔｄｉ・Ｄｏｐｔｉ
【０１３０】
上記数９０乃至９３の４つの式からなる連立方程式を解くと、時間Ｔｃｉ、Ｖｏｐｔｉ、Ｔａｉ、Ｔｄｉを、以下の式から演算できる。
【０１３１】
【数９４】
0004358979.tif 000011
【０１３２】
【数９５】
Ｖｏｐｔｉ
＝(Ｔｂａｓｅ−Ｔｃｉ)・Ａｏｐｔｉ・Ｄｏｐｔｉ／(Ｄｏｐｔｉ＋Ａｏｐｔｉ)
【数９６】
Ｔａｉ＝Ｖｏｐｔｉ／Ａｏｐｔｉ
【数９７】
Ｔｄｉ＝Ｖｏｐｔｉ／Ｄｏｐｔｉ
【０１３３】
ただし、加速度が最適な加速度Ａｏｐｔｉとなり、減速度が最適な減速度Ｄｏｐｔｉとなり、かつ動作時間が基準動作時間Ｔｂａｓｅとなるような、速度パターン計算が求まらないケースがある。それは、次の２つのケースである。
（ａ）上記数９４において、定速時間Ｔｃｉが虚数として求まる場合、及び
（ｂ）上記数９４において、最適な速度Ｖｏｐｔｉが教示速度Ｖｉを越える値として求まる場合。
【０１３４】
ステップＳ８では、上記の２つのケースに該当する関節が存在した場合をＮＯと判定する。ステップＳ９では、ステップＳ８でＮＯと判定された全ての関節について、従来と同様の第１の速度パターン計算を実施する。次いでステップＳ１０では、すべての関節の速度パターンについて、前述の終了時間の同期処理を施す。ステップＳ８でＮＯと判定されるのは、加減速度を最適化したことで、最適化前とは最大動作関節が変わってしまうケースであるあるが、ステップＳ９、Ｓ１０の処理を実施することで、加減速度の最適度合いが若干低下するが、加減速に使用できるトルクを越えることは無い。
【０１３５】
最後に、上記数７１におけるｇは重力加速度ベクトルであり、重力の働く方向を定義するものである。一般的に、ロボット座標において重力が−ｚ方向に働くよう座標を定義するので、ｇは次式のようになる。なお、重力加速度は、９．８ｍ／ｓｅｃ2である。
【０１３６】
【数９８】
ｇ＝［０，０，−１×（重力加速度）］T
【０１３７】
以上説明したように、本発明に係る実施形態によれば、開始位置及び目標位置と速度予測値とロボットダイナミクスベースパラメータから、上記数９乃至数３８を用いて、開始位置及び目標位置における慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇを含むロボットダイナミクスモデルを計算し（ステップＳ４）、計算されたロボットダイナミクスモデルに基づいて、加速度予測値の各関節間の比例関係と各関節の許容ピークトルクの条件を満足する最適な加速度及び減速度を演算した（ステップＳ５）後、すべての関節で動作時間を一致させる同期処理を実行する（ステップＳ６、Ｓ７）。従って、ロボット機構部１の各関節毎の移動量に大きな差がある位置決め動作を行う場合でも、駆動関節の許容トルクを最大限有効に使用した加減速度を演算し、動作時間の短縮を可能にし、関節に連結されるリンク自体の慣性による負荷トルク成分を考慮し、関節回転中心から関節に連結されるリンクの重心位置までの距離が短いロボットや、ロボットが特異な姿勢となっても、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。これにより、ロボット機構部１の各関節の許容ピークトルクを越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求めることができる。
【０１３８】
また、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンがすべての関節で存在しないときは、ステップＳ９で第１の速度パターン計算方法を用いて速度パターンを演算した後、ステップＳ１０で従来例の速度パターン再計算処理で同期処理を実行しているので、すべての場合について、最適な速度パターンを演算できる。
【０１３９】
さらに、本実施形態によれば、モータの軸受けや減速機の摩擦抵抗を考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４０】
またさらに、本実施形態によれば、減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４１】
＜変形例＞
以上の実施形態において、ロボット機構部１は、３個の関節ＲＪ１乃至ＲＪ３を備えているが、本発明はこれに限らず、少なくとも２個の関節を備えていればよい。
【０１４２】
以上の実施形態においては、各関節ＲＪ１乃至ＲＪ３は回転可能に構成されているが、本発明はこれに限らず、２つの関節以上では少なくとも１つは、並進運動を行う直動であってもよい。
【０１４３】
【発明の効果】
以上詳述したように本発明に係るロボットシステムの制御方法又は制御装置によれば、計算されたロボットダイナミクスモデルに基づいて、加速度予測値の各関節間の比例関係と各関節の許容ピークトルクの条件を満足する最適な加速度及び減速度を演算した後、すべての関節で動作時間を一致させる同期処理を実行する。従って、ロボット機構部の各関節毎の移動量に大きな差がある位置決め動作を行う場合でも、駆動関節の許容トルクを最大限有効に使用した加減速度を演算し、動作時間の短縮を可能にし、関節自体の慣性による負荷トルク成分を考慮し、関節回転中心から重心位置までの距離が短いロボットや、ロボットが特異な姿勢となっても、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。これにより、ロボット機構部の各関節の許容ピークトルクを越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求めることができる。
【０１４４】
また、上記ロボットシステムの制御方法又は制御装置において、好ましくは、上記加速度に関係しないトルク成分を、上記粘性行列及び重力行列に加算することにより、上記各関節で発生する摩擦抵抗を考慮する。従って、上記各関節の摩擦抵抗を考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４５】
さらに、上記ロボットシステムの制御方法又は制御装置において、好ましくは、上記慣性行列の対角成分に、上記各関節の慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正する。従って、上記各関節の慣性モーメントにより発生するトルクを考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【図面の簡単な説明】
【図１】  本発明に係る一実施形態であるロボットシステムの構成を示すブロック図である。
【図２】  図１のロボット機構部１の構成を示すスケルトン図である。
【図３】  図１のロボット機構部１の各関節ＲＪｋ（ｋ＝１，２，３）の構成を示す斜視図、並びに図１の制御装置１０内のサーボ制御回路１０７の構成を示すブロック図である。
【図４】  図１のＣＰＵ１００によって実行される位置決め動作処理を示すフローチャートである。
【図５】  図４の速度パターンの予測演算処理（ステップＳ２）の概念を示すグラフであって、（ａ）は第１の関節の予想速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の予想速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の予想速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【図６】  図４の速度パターンの終了時間の同期処理（ステップＳ３）の概念を示すグラフであって、（ａ）は第１の関節の予想速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の予想速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の予想速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【図７】  台形状の速度パターンのときの本実施形態で用いる第１の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図８】  三角形状の速度パターンのときの本実施形態で用いる第１の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図９】  本実施形態で用いる第２の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図１０】  第１の従来例の位置決め動作処理を示すフローチャートである。
【図１１】  図１０の終了時間の同期処理の一例を説明するための各関節の速度パターンを示すグラフであって、（ａ）は第１の関節の速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【符号の説明】
１…ロボット機構部、
１０…制御装置、
２０…ティーチペンダント、
１００…ＣＰＵ、
１０１…ＲＯＭ、
１０２…ＲＡＭ、
１０３…インターフェース、
１０４…デュアルポートＲＡＭ、
１０５…ラッチ回路、
１０６…ハードディスクメモリ、
１０６ａ…教示データメモリ、
１０６ｂ…仮の加減速度情報メモリ、
１０６ｃ…ロボットダイナミクスベースパラメータメモリ、
１０７…サーボ制御回路、
１１０…クロック発生器、
１１１…分周器、
ＲＪ１乃至ＲＪ３……関節、
Ｌ１乃至Ｌ４…リンク、
ＭＯ１乃至Ｍ３…モータ、
ＲＥ１乃至ＲＥ３…減速機、
ＳＮ１乃至ＳＮ３…センサ。
【図１】
0004358979.tif 000012
【図２】
0004358979.tif 000013
【図３】
0004358979.tif 000014
【図４】
0004358979.tif 000015
【図５】
0004358979.tif 000016
【図６】
0004358979.tif 000017
【図７】
0004358979.tif 000018
【図８】
0004358979.tif 000019
【図９】
0004358979.tif 000020
【図１０】
0004358979.tif 000021
【図１１】
0004358979.tif 000022