(57)【要約】【課題】  簡単なアルゴリズムで、例えばモータの駆動方向及び加減速等に応じてモータの加減速度を決定する加減速制御方法、装置等を提供する。【解決手段】  関節を有する機械であるマニピュレータ１００を動作させる関節駆動モータに作用する重力トルクを重力トルク演算部１Ａが算出し、あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動条件に基づいて選択した重力補償係数と重力トルクとに基づいて加減速度補正係数演算部１Ｂが算出した加減速度補正係数と設定した基本加減速度とに基づいて、加減速度演算部１Ｄが関節駆動モータの加速度及び／又は減速度を算出する。【選択図】        図１
2006252383.tif 000002
【特許請求の範囲】
【請求項１】
関節を有する機械を動作させる関節駆動モータに作用する重力トルクを算出する工程と、
あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と前記重力トルクとに基づいて加減速度補正係数を算出する工程と、
該加減速度補正係数と設定した基本加減速度とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する工程と
を有することを特徴とする加減速制御方法。
【請求項２】
前記関節駆動モータのトルク飽和防止及び残留振動抑制のために、前記関節駆動モータの駆動方向別及び加速・減速別の前記駆動態様に基づいて前記重力補償係数を設定することを特徴とする請求項１に記載の加減速制御方法。
【請求項３】
関節を有する機械の動作開始及び動作終了に関するデータ及び前記機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、
前記加減速度補正係数と設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する工程と
を有することを特徴とする加減速制御方法。
【請求項４】
前記加減速度補正係数は、前記重力トルクの絶対値と前記関節駆動モータの出力トルクとの和を前記出力トルクで割った値から１を引いた値と、前記重力補償係数とを乗し、その値に１を加えた値であることを特徴とする請求項１〜３のいずれかに記載の加減速制御方法。
【請求項５】
前記加減速度補正係数の最大値及び／又は最小値を設定し、算出した前記加減速度補正係数が範囲外であると判断すると、前記加減速度補正係数を最大値又は最小値に制限する工程をさらに有することを特徴とする請求項３又は４に記載の加減速制御方法。
【請求項６】
前記機械は多関節型マニピュレータであることを特徴とする請求項１〜５のいずれかに記載の加減速制御方法。
【請求項７】
多関節型マニピュレータを動作させる関節駆動モータに作用する重力トルクを算出する重力トルク演算手段と、
あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と前記重力トルクとに基づいて加減速度補正係数を算出する加減速度補正係数演算手段と、
該加減速度補正係数と設定した基本加減速度とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段と
を備えることを特徴とする加減速制御装置。
【請求項８】
多関節型マニピュレータの動作開始時及び終了時のデータ及び前記多関節型マニピュレータの機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する加減速度補正係数演算手段と、
前記加減速度補正係数と前記関節に設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段と
を備えることを特徴とする加減速制御装置。
【請求項９】
前記加減速度補正係数に設定した最大値及び／又は最小値に基づいて前記加減速度補正係数演算手段が算出した前記加減速度補正係数を最大値又は最小値に制限する係数チェック手段をさらに備えることを特徴とする請求項７又は８に記載の加減速制御装置。
【請求項１０】
各関節の移動時間に基づいて最大移動時間の関節を判断する判断手段と、
前記最大移動時間の関節以外の関節について、前記最大移動時間に合わせた加速度及び／又は減速度を再計算する加減速度調整手段と
をさらに備えることを特徴とする請求項７〜９のいずれかに記載の加減速制御装置。
【請求項１１】
関節を有する機械を動作させる関節駆動モータに作用する重力トルクを算出する工程と、
駆動態様に基づいて複数設定された中から選択した重力補償係数と該重力トルクとに基づいて加減速度補正係数を算出する工程と、
設定した基本加減速度と前記加減速度補正係数とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する工程と
をコンピュータに行わせることを特徴とする加減速制御方法のプログラム。
【請求項１２】
関節を有する機械の動作開始及び動作終了に関するデータ及び前記機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、
前記加減速度補正係数と前記関節に設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する工程と
をコンピュータに行わせることを特徴とする加減速制御方法のプログラム。
【請求項１３】
算出した前記加減速度補正係数に基づいて、前記加減速度補正係数の最大値及び／又は最小値を制限する工程をさらにコンピュータに行わせることを特徴とする請求項２に記載の加減速制御方法のプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、関節を有する機械の関節駆動モータの加減速制御方法、装置等に関するものである。特に例えば垂直多関節ロボット等において重力の影響を受ける関節駆動モータを制御するためのものである。
【背景技術】
【０００２】
例えば、多関節型のロボット（以下、マニピュレータという）においては、重力によって関節部に働く重力トルクにより、関節駆動モータ（以下、モータという）のトルクが消費されてトルク飽和が発生したり、位置決めの際に振動が残留したりする問題がある。この問題を解決するためには、多関節型マニピュレータの各関節に作用する重力の影響を考慮して加速及び／又は減速（以下、加減速といい、その速度を加減速度という）を調整しなければならない。
【０００３】
従来、マニピュレータの姿勢に応じてリアルタイムに重力トルクを計算し、フィードフォーワード的に重力の影響をキャンセルする方法が提案されている（特許文献１参照）。この方法は、マニピュレータの姿勢情報から関節に働く重力トルクを計算し、これをモータの駆動トルク指令から差し引くことによって重力の影響をキャンセルするというものである。また、マニピュレータの教示点の位置、姿勢や各軸の駆動方向、駆動速度より運動方程式から導いた計算式に基づいて加減速時定数を調整する方法も提案されている（特許文献２参照）。
【特許文献１】特開平１０−２４４４８２号公報
【特許文献２】特開平７−２６１８２２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述したフィードフォーワード的に重力の影響をキャンセルする方法では、姿勢に基づいて算出した重力トルクをそのまま差し引いてキャンセルしようとするため、駆動方向、状況に応じたキャンセルが行われていない。特に重力トルクの影響を一律にキャンセルしようとしており、例えば重力トルクの影響を受けるトルク飽和、残留振動に対し、それぞれを目的に応じて別々に調整することは不可能である。また、加減速時定数を調整する方法については、運動方程式を解くための計算量が多く、リアルタイムでの処理を行うためには、処理能力（ＣＰＵパワー）が高い演算手段を用いなければならない。
【０００５】
そこで、簡単な手順で、駆動態様（特にモータの駆動方向及び加減速等の状況）に応じてモータの加減速度を決定する加減速制御方法、装置等を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明に係る加減速制御方法は、関節を有する機械を動作させる関節駆動モータの駆動に作用する重力トルクを算出する工程と、あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する工程と、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する工程とを有する。
本発明によれば、算出した重力トルクとあらかじめ設定した複数の重力補償係数の中から選択した重力補償係数に基づいて加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（例えばトルク飽和、残留振動等）に適切に対応した加減速度を、重力トルクを演算した後は、簡単な手順により算出することができる。これにより、高速な演算処理を必要とせず、高価な処理手段を用いなくてもよい。
【０００７】
また、本発明に係る加減速制御方法は、関節駆動モータのトルク飽和防止及び残留振動抑制のために、関節駆動モータの駆動方向別及び加速・減速別の駆動態様に基づいて重力補償係数を設定する。
本発明によれば、動作に大きく影響するトルク飽和防止及び残留振動抑制のために、関節駆動モータの駆動方向別及び加速・減速別に重力補償係数を設定するようにしたので、重力作用方向と逆方向に加速する際のトルク飽和、減速を伴う位置決めを行う際の残留振動による影響がそれぞれ生じやすい態様に基づいた重力補償係数により加速度、減速度を算出することができ、トルク飽和防止及び残留振動抑制を有効に図ることができる。
【０００８】
また、本発明に係る加減速制御方法は、関節を有する機械の動作開始及び動作終了に関するデータ及び機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、加減速度補正係数と設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する工程と有する。
本発明によれば、動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて加減速度補正係数を算出し、その加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理を必要とせず、高価な処理手段を用いなくてもよい。
【０００９】
また、本発明に係る加減速制御方法は、加減速度補正係数の最大値及び／又は最小値を設定し、算出した加減速度補正係数が範囲外であると判断すると、加減速度補正係数を最大値又は最小値に制限する工程をさらに有する。
本発明によれば、加減速度補正係数を制限するようにしたので動作の安全を図ることができる。
【００１０】
また、本発明に係る加減速制御方法において機械は多関節型マニピュレータである。
上記の発明は、多様な姿勢をとり、より細かな加減速制御、位置決めを必要とする多関節型マニピュレータに対して最も大きな効果を発揮することができる。
【００１１】
また、本発明に係る加減速制御装置は、多関節型マニピュレータを動作させる関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する加減速度補正係数演算手段と、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段とを備える。
本発明によれば、重力トルク演算手段が算出した重力トルクと、あらかじめ設定した複数の重力補償係数の中から例えば、駆動態様によって選択した重力補償係数に基づいて加減速度補正係数演算手段が加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、加減速度演算手段が関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（トルク飽和、残留振動等）に適切に対応した加減速度を、各手段は複雑な計算を行うことなく簡単な手順で算出することができる。これにより、装置のコストも低減させることができる。
【００１２】
また、本発明に係る加減速制御装置は、多関節型マニピュレータの動作開始時及び終了時のデータ及び多関節型マニピュレータの機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する加減速度補正係数演算手段と、加減速度補正係数と関節に設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段とを備える。
本発明によれば、重力トルク演算手段が算出した動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて、加減速度補正係数演算手段が加減速度補正係数を算出し、加減速度演算手段が、加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を、各手段は複雑な計算を行うことなく簡単な手順で算出することができる。これにより、装置のコストも低減させることができる。
【００１３】
また、本発明に係る加減速制御装置は、加減速度補正係数に設定した最大値及び／又は最小値に基づいて加減速度補正係数演算手段が算出した加減速度補正係数を最大値又は最小値に制限する係数チェック手段をさらに備える。
本発明によれば、係数チェック手段が、加減速度補正係数をチェックし、その最大値、最小値を制限するようにしたので動作の安全を図ることができる。
【００１４】
また、本発明に係る加減速制御装置は、各関節の移動時間に基づいて最大移動時間の関節を判断する判断手段と、最大移動時間の関節以外の関節について、最大移動時間に合わせた加速度及び／又は減速度を再計算する加減速度調整手段とをさらに備える。
本発明によれば、判断手段が、各関節の移動時間に基づいて最大移動時間の関節を判断し、最大移動時間の関節以外の関節について加減速度調整手段が再計算して最大移動時間に合わせて加速度及び／又は減速度を調整するようにしたので、最大移動時間の関節以外の関節については、加減速度の余裕ができ、重力の影響の低減をさらに図ることができる。
【００１５】
また、本発明に係る加減速制御方法のプログラムは、関節を有する機械を動作させる関節駆動モータの駆動に作用する重力トルクを算出する工程と、駆動態様に基づいて複数設定された中から選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する工程と、設定した基本加減速度と加減速度補正係数とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する工程とをコンピュータに行わせる。
本発明によれば、算出した重力トルクとあらかじめ設定した複数の重力補償係数の中から選択した重力補償係数に基づいて加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（例えばトルク飽和、残留振動等）に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理が行えないコンピュータでも対応できる。
【００１６】
また、本発明に係る加減速制御方法のプログラムは、関節を有する機械の動作開始及び動作終了に関するデータ及び機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、加減速度補正係数と関節に設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する工程とをコンピュータに行わせる。
本発明によれば、機械の機構パラメータ並びに／又はペイロードに関するデータを含めて動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて加減速度補正係数を算出し、その加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理が行えないコンピュータでも対応できる。
【００１７】
また、本発明に係る加減速制御方法のプログラムは、算出した加減速度補正係数に基づいて、加減速度補正係数の最大値及び／又は最小値を制限する工程をさらにコンピュータに行わせる。
本発明によれば、加減速度補正係数を制限するようにしたので、機械の安全制御をコンピュータに行わせることができる。
【発明を実施するための最良の形態】
【００１８】
実施の形態１．
図１は本発明の実施の形態１に係るマニピュレータ制御装置を中心とするシステムを表す図である。図１において、本実施の形態のマニピュレータ制御装置１０は、加減速制御手段１、補間演算手段２、信号出力手段３及びデータ記憶手段４で構成されている。マニピュレータ制御装置１０は、駆動信号を出力してマニピュレータ１００の動作を制御する。ここで本実施の形態では、主にマニピュレータ１００の各関節（軸）におけるモータの軸回転駆動の加減速を含む速度に関する制御について説明する（基本的には１つの動作に加速、減速が少なくともそれぞれ１回ずつ含まれる）。
【００１９】
加減速制御手段１は、さらに重力トルク演算部１Ａ、加減速度補正係数演算部１Ｂ、補正係数チェック部１Ｃ、加減速度演算部１Ｄ、主軸判断部１Ｅ及び加減速調整部１Ｆで構成される。重力トルク演算部１Ａは、重力の影響によって関節に発生する重力トルクを、マニピュレータ１００の各関節について算出する。また、加減速度補正係数演算部１Ｂは、重力トルクに基づいて各関節の加減速度補正係数を算出する。そして、補正係数チェック部１Ｃは、例えば、加減速度補正係数の効果を制限するため、加減速度補正係数に対して、定められた最大値及び最小値の範囲内であるかどうかを判断する。範囲内でないと判断すると、加減速度補正係数を範囲内になるように制限する。加減速度演算部１Ｄは、加減速度補正係数（補正係数チェック部１Ｃが制限した加減速度補正係数も含む）とあらかじめ各関節に設定された基本加減速度に基づいて、各関節の加減速度を算出する。
【００２０】
主軸判断部１Ｅは、補間演算手段２が補間演算により導き出した各関節の移動時間（回転駆動時間）に基づいて、移動時間が最大（最長）となる関節（これを主軸とする）を判断する。また、加減速度調整部１Ｆは、主軸以外の関節について加減速度を再計算し、各関節の駆動（マニピュレータ１００全体の動作）を調整する。
【００２１】
補間演算手段２は、加減速度演算部１Ｄが算出した加減速度等に基づいて、動作開始から終了までのマニピュレータ１００の動作について、各関節の駆動軌跡、速度等を補間演算する。さらに信号出力手段３は、補間演算手段２を経て算出した各関節の加減速度を含む駆動信号をマニピュレータ１００に出力する。
【００２２】
ここで、本実施の形態では、例えばＣＰＵを中心とする制御処理装置（コンピュータ）により加減速制御手段１、補正演算手段２及び信号出力手段３を実現するものとし、上述した各手段及び手段を構成する各部が行う処理を制御処理装置が実行し、その演算結果をデータ等処理するものとする。この場合、例えば、各部、各手段が処理を行う手順がプログラムとしてデータ記憶手段４に記憶されている。
【００２３】
データ記憶手段４は、加減速制御手段１、補正演算手段２及び信号出力手段３が処理を行うために必要となるデータが記憶されている。例えば、重力トルク演算部１Ａが重力トルクを算出するために必要となるペイロード２００の態様（質量、重心位置）、マニピュレータ１００の機構パラメータ（例えば各アーム（リンク）の質量、アーム長、重心位置）等のデータ及び動作開始点、終了点等の教示点のデータが記憶されている。また、加速度補正係数を演算するための重力補償係数のデータが記憶されている。さらに補正係数チェック部１Ｃが加減速度補正係数のチェックをするための最大値及び最小値のデータ、加減速度演算部１Ｄが加減速度を演算する際の基本加減速度のデータが記憶されている。
【００２４】
図２はマニピュレータ１００の一例を表す図である。ここでは、マニピュレータ１００として６軸垂直多関節ロボット等の多関節マニピュレータを例にして説明するが、重力の影響を考慮しなければならない環境下で動作するマニピュレータであれば、軸数等、その種類については特に限定しない。また、マニピュレータ以外にも、動作に際して重力の影響を考慮しなければならない関節（軸）を有する機械について適用することができる。ペイロード２００は例えばマニピュレータ１００が把持等する搬送物等である。
【００２５】
次に本実施の形態の処理について、加減速制御手段１を中心に説明する。重力トルク演算部１Ａでは、動作開始時及び動作終了時のマニピュレータ１００の姿勢に基づいて、各関節に発生する重力トルクＴｇを算出する。重力トルクＴｇの算出方法については、特に限定しないが、ペイロード２００の条件（質量、重心位置）、マニピュレータ１００の機構パラメータ等マニピュレータの種類によって、また、アーム等の傾き具合（水平距離）によって異なる。ここで、マニピュレータ１００の基部が鉛直方向に垂直となるように設けられている場合、図２においてＪ１の回転を行う関節については重力トルクＴｇが０となる。
【００２６】
ここで、マニピュレータ１００の加減速に利用できるトルクは、モータの最大出力トルクＴｐ及び重力トルクＴｇである。そこでマニピュレータ１００の関節の加減速（駆動）に利用できる加減速トルクＴａｃを次式（１）により算出する。そして、加減速トルクＴａｃをモータの最大出力トルクＴｐで正規化すると、次式（２）のようになる。ここでＴｇ／Ｔｐを正規化重力トルク値（無次元）ということにする。なお、（１）、（２）式はマニピュレータ１００の加減速に最大利用できるトルク最大出力トルクＴｐに基づいているが最大でなくても適用できる。
Ｔａｃ＝Ｔｐ＋Ｔｇ                                                  …（１）
Ｔａｃ／Ｔｐ＝（Ｔｐ＋Ｔｇ）／Ｔｐ
＝１＋Ｔｇ／Ｔｐ                                        …（２）
【００２７】
加減速度補正係数演算部１Ｂは、各関節の加減速度を算出する際に基本加減速度にかけ合わせる加減速補正係数Ｋを算出する。加減速補正係数Ｋを算出するための基本的な式は次式（３）で表される。（３）式において、Ｔｇの絶対値を使用するのは、Ｔｇの符号変化に代わり、重力補償係数Ｋｇの符号設定によって加減速調整の自由度を拡大するためである。中括弧内で１を引いているのは基準を０にする（重力トルクＴｇ＝０のときに０となるようにして重力補償係数Ｋｇが加減速補正係数Ｋに影響しないようにする）ためである。中括弧内は前述した正規化重力トルク値と最終的に同じ値になる。一方、正規化重力トルク値と重力補償係数Ｋｇとの積に１を足しているのは加減速補正係数Ｋの基準を１にする（重力トルクＴｇ＝０）ときにＫ＝１とするためである。
Ｋ＝（（Ｔｐ＋|Ｔｇ|）／Ｔｐ−１）Ｋｇ＋１
＝（|Ｔｇ|・Ｋｇ＋Ｔｐ）／Ｔｐ
＝（|Ｔｇ|／Ｔｐ）Ｋｇ＋１                                          …（３）
【００２８】
（３）式の重力補償係数Ｋｇは、重力トルクＴｇの大きさによって、加減速補正係数Ｋの効果を調整するために設定した係数である。Ｋｇ＞０であれば、加減速補正係数Ｋが１より大きくなり、加減速が増加する。Ｋｇ＜０であれば、加減速補正係数が１より小さくなり、加減速が抑えられる。Ｋｇ＝０であれば、加減速における効果に重力トルクＴｇを作用させない。ここで、重力補償係数Ｋｇについて、本実施の形態では表１に示すように関節（回転）駆動方向Θ’（重力作用方向と同方向・逆方向）別及び加速・減速別に分けた４タイプのＫｇを設定するものとする。一般的には、トルク飽和は重力作用方向と逆方向に加速する際に問題となり、残留振動は位置決めを行う際（減速が伴う）に問題となる。そこで、駆動方向別及び加速・減速別にタイプを分け、それぞれの重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）を設定することで、重力により生じるトルク飽和、残留振動の問題をそれぞれ考慮した加減速度を設定できるようにする。
【００２９】
【表１】
2006252383.tif 000003
【００３０】
図３は重力補償係数Ｋｇを設定するまでの処理をフローチャートで表した図である。まず、Ｋｇ＝０（このときＫ＝１となる）と設定する（Ｓ１）。そして、例えば、重力作用方向と逆方向に加速した場合に、関節にトルク飽和が発生しないかどうかを判断する（Ｓ２）。さらに残留振動が許容範囲内にあるかどうかを判断する（Ｓ３）。このトルク飽和、残留振動の測定は、例えば実測、計算シミュレーション等により行う。トルク飽和又は残留振動が発生しなければ、Ｋｇ≧０となるような重力補償係数Ｋｇの設定を決定する（Ｓ４）。一方、発生すればＫｇ＜０となるようなＫｇの設定を決定する（Ｓ５）。
【００３１】
Ｓ４においてＫｇ≧０の決定をすると、｜Ｋｇ｜を増加（正の方向）させ（Ｓ６）、残留振動が増加したかどうかを判断する（Ｓ７）。増加していないと判断するとＳ６に戻ってさらに｜Ｋｇ｜を増加させる。一方、増加したと判断すると、トルク飽和及び残留振動が問題にならない重力補償係数Ｋｇ（例えば、１段階前のステップにおいて増加前のＫｇの値）を設定する（Ｓ８）。ここではＳ７において残留振動が増加したと判断すると、Ｓ８においてすぐに重力補償係数Ｋｇを設定しているが、例えば残留振動が許容範囲を越えるまで｜Ｋｇ｜を増加させた後、重力補償係数Ｋｇを設定してもよい。なお、Ｓ６において｜Ｋｇ｜の増加幅については特に限定しないが、｜Ｋｇ｜の増加幅を小さくすることによって、よりきめ細かな重力補償係数Ｋｇの設定を行うことができる（後述するＳ９においても同様である）。
【００３２】
Ｓ５においてＫｇ＜０の決定をすると、｜Ｋｇ｜を増加（負の方向）させ（Ｓ９）、トルク飽和がなくなったかどうかを判断する（Ｓ１０）。トルク飽和がなくなっていなければ、Ｓ９に戻り、さらに｜Ｋｇ｜を増加させる。トルク飽和がなくなれば、さらに残留振動が許容範囲内にあるかどうかを判断する（Ｓ１１）。許容範囲内になければＳ１０に戻ってさらに｜Ｋｇ｜を増加させる。許容範囲内にあれば、そのときの値を重力補償係数Ｋｇとして設定する（Ｓ１２）。
【００３３】
以上のようにして４つのタイプのそれぞれに設定した重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）をそれぞれ（３）式に当てはめて算出した加減速補正係数Ｋ（Ｋａｆ、Ｋｄｆ、Ｋａｒ、Ｋｄｒ）は、次式（４）〜（７）となる。
Ｋａｆ＝（｜Ｔｇ｜／Ｔｐ）Ｋｇａｆ＋１                                …（４）
Ｋｄｆ＝（｜Ｔｇ｜／Ｔｐ）Ｋｇｄｆ＋１                                …（５）
Ｋａｒ＝（｜Ｔｇ｜／Ｔｐ）Ｋｇａｒ＋１                                …（６）
Ｋｄｒ＝（｜Ｔｇ｜／Ｔｐ）Ｋｇｄｒ＋１                                …（７）
【００３４】
補正係数チェック部１Ｃは、加減速度補正係数演算部１Ｂが算出した各関節の加減速補正係数Ｋに対して、次式（８）に示すように、最小値Ｋｍｉｎ以上最大値Ｋｍａｘ以下の範囲内にあるかどうかをチェックする。例えば傾きが大きい等、マニピュレータ１００の姿勢によって加減速度を制限したい場合に加減速度補正係数の効果を制限する。Ｋｍａｘ＞Ｋと判断すればＫ＝Ｋｍａｘとし、Ｋｍｉｎ＜Ｋと判断すればＫ＝Ｋｍｉｎとする。なお、本実施の形態では最小値Ｋｍｉｎ及び最大値Ｋｍａｘは各関節により異なる値とするが、これに限定されるものではなく、関節に関係なく一律の値を定めてもよい。
Ｋｍｉｎ≦Ｋ≦Ｋｍａｘ                                              …（８）
【００３５】
補正係数チェック部１Ｃでチェックされ、確定した加減速補正係数Ｋに基づいて、加減速度演算部１Ｄは、各関節の加減速度を算出する。ここで、加速を行う場合は、各関節に設定された基本加速度Ａａｂに、加減速補正係数Ｋａｆ又はＫａｒを乗する。また、減速を行う場合は、基本減速度Ａｄｂに、加減速補正係数Ｋｄｆ又はＫｄｒを乗する。ここで、基本加速度Ａａｂ、基本減速度Ａｄｂは、例えば、アームが伸びた状態の姿勢、縮んだ状態の姿勢（基本姿勢）等から加速又は基本姿勢等に減速する際の基本の加速度、減速度である。関節（回転）駆動方向が重力作用方向と同方向（Ｔｇ×Θ’＞０）の加速度Ａａｃ、減速度Ａｄｃは、それぞれ次式（９）、（１０）で表される。
Ａａｃ＝Ｋａｆ×Ａａｂ                                              …（９）
Ａｄｃ＝Ｋｄｆ×Ａｄｂ                                            …（１０）
【００３６】
また、関節（回転）駆動方向が重力作用方向と逆方向（Ｔｇ×Θ’＜０）の加速度Ａａｃ、減速度Ａｄｃは、それぞれ次式（１１）、（１２）で表される。
Ａａｃ＝Ｋａｒ×Ａａｂ                                            …（１１）
Ａｄｃ＝Ｋｄｒ×Ａｄｂ                                            …（１２）
【００３７】
補間演算手段２は、加減速度演算部１Ｄが算出した加速度で関節が加速駆動してマニピュレータ１００の動作が開始し、減速度で減速駆動してマニピュレータ１００の動作が終了するように、各関節について動作開始から終了までの軌跡（位置）、速度等を補間する演算を行う。
【００３８】
主軸判断部１Ｅは、各関節の加減速度等に基づいて各関節の移動時間（回転駆動時間）を算出し、その中での移動時間が最大となる関節を判断し、これを主軸とする。この最大の移動時間がマニピュレータ１００全体の動作時間となる。したがって、主軸以外の各関節は最大の移動時間になるように調整すればよいので、算出した移動時間よりも余裕ができることになる（例えば、主軸以外の関節は最大の出力トルクで加減速を行わなずにすむ）。そこで、加減速度調整部１Ｆは、最大移動時間の関節を主軸とし、主軸以外の関節について加減速の再計算を行い、各関節について最終的に加減速度を決定する。ここで、再計算の方法、手順については特に限定するものではない。例えば、急な減速によって残留振動が生じやすいので、最大移動時間の範囲内で加減速度を抑制するように調整すれば、残留振動の発生を抑えることができる。調整した加減速度に基づいて、補間演算手段２はさらに補間演算を行う。
【００３９】
信号出力手段３は、各関節の加減速度等に応じて補間演算手段２が演算した軌跡、速度等に基づいて、マニピュレータ１００を駆動させるための駆動信号をマニピュレータ１００に出力する。マニピュレータ１００の各関節には駆動信号に基づいた電流が供給され、関節駆動モータが（回転）駆動してマニピュレータ１００が動作する。
【００４０】
以上のように実施の形態１によれば、マニピュレータ１００の各関節に対し、重力トルク演算部１Ａが重力トルクＴｇを算出し、駆動態様（モータの駆動方向別及び加速・減速別）に基づいて複数設定された中から選択した重力補償係数Ｋｇにより、加速度補正係数演算部１Ｂが正規化重力トルク値（重力トルク）を調整した上で加速度補正係数Ｋを決定し、加減速演算部１Ｄが加速度補正係数Ｋを基本加減速度に乗じて各関節の加減速を算出するようにしたので、駆動の態様に応じ、重力の影響に適切に対応した加減速度を簡単な計算（アルゴリズム）により算出することができる。特にモータの駆動方向別及び加速・減速別に重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）を設定しているので、重力作用方向と同方向に加速する際のトルク飽和、減速を伴う位置決めを行う際の残留振動による影響がそれぞれ生じやすい態様に基づいて調整した上で加減速度を算出することができ、トルク飽和防止及び残留振動抑制を有効に図ることができる。
【００４１】
さらに、加減速度補正係数Ｋの最大値最小値を設定し、補正係数チェック部１Ｃにより、加減速度補正係数を最大値又は最小値に制限するようにしたので、例えば、傾きが大きい場合の加減速度を制限することができ、マニピュレータの動作の安全を図ることができる。また、主軸判断部１Ｅが、各関節の移動時間に基づいて主軸を判断し、その最大移動時間（マニピュレータ１００全体の動作時間）に合わせて加減速度調整部１Ｆが再計算して加減速度を調整するようにしたので、主軸以外の関節については、加減速度の余裕ができ、トルク飽和防止及び残留振動抑制をさらに図ることができる。
【図面の簡単な説明】
【００４２】
【図１】実施の形態１に係るマニピュレータ制御装置１０を中心とする図である。
【図２】マニピュレータ１００の一例を表す図である。
【図３】重力補償係数Ｋｇを設定する処理をフローチャートで表した図である。
【符号の説明】
【００４３】
１  加減速制御手段、１Ａ  重力トルク演算部、１Ｂ  加減速度補正係数演算部、１Ｃ  補正係数チェック部、１Ｄ  加減速度演算部、１Ｅ  主軸判断部、１Ｆ  加減速度調整部、２  補間演算手段、３  信号出力手段、４  データ記憶手段、１０  マニピュレータ制御装置、１００  マニピュレータ、２００  ペイロード。
【図１】
2006252383.tif 000004
【図２】
2006252383.tif 000005
【図３】
2006252383.tif 000006
【手続補正書】
【提出日】平成18年2月9日(2006.2.9)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】請求項１３
【補正方法】変更
【補正の内容】
【請求項１３】
算出した前記加減速度補正係数に基づいて、前記加減速度補正係数の最大値及び／又は最小値を制限する工程をさらにコンピュータに行わせることを特徴とする請求項１２に記載の加減速制御方法のプログラム。