JP4281696B2

JP4281696B2 - 加減速制御方法及び装置並びに加減速制御方法のプログラム

Info

Publication number: JP4281696B2
Application number: JP2005070555A
Authority: JP
Inventors: 友一平林
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2005-03-14
Filing date: 2005-03-14
Publication date: 2009-06-17
Anticipated expiration: 2025-03-14
Also published as: CN1833830A; US20060206284A1; KR100749984B1; TW200700948A; JP2006252383A; EP1702726A2; CN100396452C; US7328120B2; TWI296358B; KR20060100935A

Description

本発明は、例えば、関節を有する機械の関節駆動モータの加減速制御方法、装置等に関するものである。特に例えば垂直多関節ロボット等において重力の影響を受ける関節駆動モータを制御するためのものである。

例えば、多関節型のロボット（以下、マニピュレータという）においては、重力によって関節部に働く重力トルクにより、関節駆動モータ（以下、モータという）のトルクが消費されてトルク飽和が発生したり、位置決めの際に振動が残留したりする問題がある。この問題を解決するためには、多関節型マニピュレータの各関節に作用する重力の影響を考慮して加速及び／又は減速（以下、加減速といい、その速度を加減速度という）を調整しなければならない。

従来、マニピュレータの姿勢に応じてリアルタイムに重力トルクを計算し、フィードフォーワード的に重力の影響をキャンセルする方法が提案されている（特許文献１参照）。この方法は、マニピュレータの姿勢情報から関節に働く重力トルクを計算し、これをモータの駆動トルク指令から差し引くことによって重力の影響をキャンセルするというものである。また、マニピュレータの教示点の位置、姿勢や各軸の駆動方向、駆動速度より運動方程式から導いた計算式に基づいて加減速時定数を調整する方法も提案されている（特許文献２参照）。
特開平１０−２４４４８２号公報特開平７−２６１８２２号公報

しかしながら、上述したフィードフォーワード的に重力の影響をキャンセルする方法では、姿勢に基づいて算出した重力トルクをそのまま差し引いてキャンセルしようとするため、駆動方向、状況に応じたキャンセルが行われていない。特に重力トルクの影響を一律にキャンセルしようとしており、例えば重力トルクの影響を受けるトルク飽和、残留振動に対し、それぞれを目的に応じて別々に調整することは不可能である。また、加減速時定数を調整する方法については、運動方程式を解くための計算量が多く、リアルタイムでの処理を行うためには、処理能力（ＣＰＵパワー）が高い演算手段を用いなければならない。

そこで、簡単な手順で、駆動態様（特にモータの駆動方向及び加減速等の状況）に応じてモータの加減速度を決定する加減速制御方法、装置等を提供することを目的とする。

本発明に係る加減速制御方法は、関節を有する機械を動作させる関節駆動モータの駆動に作用する重力トルクを算出する工程と、あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する工程と、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する工程とを有する。
本発明によれば、算出した重力トルクとあらかじめ設定した複数の重力補償係数の中から選択した重力補償係数に基づいて加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（例えばトルク飽和、残留振動等）に適切に対応した加減速度を、重力トルクを演算した後は、簡単な手順により算出することができる。これにより、高速な演算処理を必要とせず、高価な処理手段を用いなくてもよい。

また、本発明に係る加減速制御方法は、関節駆動モータのトルク飽和防止及び残留振動抑制のために、関節駆動モータの駆動方向別及び加速・減速別の駆動態様に基づいて重力補償係数を設定する。
本発明によれば、動作に大きく影響するトルク飽和防止及び残留振動抑制のために、関節駆動モータの駆動方向別及び加速・減速別に重力補償係数を設定するようにしたので、重力作用方向と逆方向に加速する際のトルク飽和、減速を伴う位置決めを行う際の残留振動による影響がそれぞれ生じやすい態様に基づいた重力補償係数により加速度、減速度を算出することができ、トルク飽和防止及び残留振動抑制を有効に図ることができる。

また、本発明に係る加減速制御方法は、関節を有する機械の動作開始及び動作終了に関するデータ及び機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、加減速度補正係数と設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する工程と有する。
本発明によれば、動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて加減速度補正係数を算出し、その加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理を必要とせず、高価な処理手段を用いなくてもよい。

また、本発明に係る加減速制御方法は、加減速度補正係数の最大値及び／又は最小値を設定し、算出した加減速度補正係数が範囲外であると判断すると、加減速度補正係数を最大値又は最小値に制限する工程をさらに有する。
本発明によれば、加減速度補正係数を制限するようにしたので動作の安全を図ることができる。

また、本発明に係る加減速制御方法において機械は多関節型マニピュレータである。
上記の発明は、多様な姿勢をとり、より細かな加減速制御、位置決めを必要とする多関節型マニピュレータに対して最も大きな効果を発揮することができる。

また、本発明に係る加減速制御装置は、多関節型マニピュレータを動作させる関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する加減速度補正係数演算手段と、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段とを備える。
本発明によれば、重力トルク演算手段が算出した重力トルクと、あらかじめ設定した複数の重力補償係数の中から例えば、駆動態様によって選択した重力補償係数に基づいて加減速度補正係数演算手段が加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、加減速度演算手段が関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（トルク飽和、残留振動等）に適切に対応した加減速度を、各手段は複雑な計算を行うことなく簡単な手順で算出することができる。これにより、装置のコストも低減させることができる。

また、本発明に係る加減速制御装置は、多関節型マニピュレータの動作開始時及び終了時のデータ及び多関節型マニピュレータの機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する加減速度補正係数演算手段と、加減速度補正係数と関節に設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段とを備える。
本発明によれば、重力トルク演算手段が算出した動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて、加減速度補正係数演算手段が加減速度補正係数を算出し、加減速度演算手段が、加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を、各手段は複雑な計算を行うことなく簡単な手順で算出することができる。これにより、装置のコストも低減させることができる。

また、本発明に係る加減速制御装置は、加減速度補正係数に設定した最大値及び／又は最小値に基づいて加減速度補正係数演算手段が算出した加減速度補正係数を最大値又は最小値に制限する係数チェック手段をさらに備える。
本発明によれば、係数チェック手段が、加減速度補正係数をチェックし、その最大値、最小値を制限するようにしたので動作の安全を図ることができる。

また、本発明に係る加減速制御装置は、各関節の移動時間に基づいて最大移動時間の関節を判断する判断手段と、最大移動時間の関節以外の関節について、最大移動時間に合わせた加速度及び／又は減速度を再計算する加減速度調整手段とをさらに備える。
本発明によれば、判断手段が、各関節の移動時間に基づいて最大移動時間の関節を判断し、最大移動時間の関節以外の関節について加減速度調整手段が再計算して最大移動時間に合わせて加速度及び／又は減速度を調整するようにしたので、最大移動時間の関節以外の関節については、加減速度の余裕ができ、重力の影響の低減をさらに図ることができる。

また、本発明に係る加減速制御方法のプログラムは、関節を有する機械を動作させる関節駆動モータの駆動に作用する重力トルクを算出する工程と、駆動態様に基づいて複数設定された中から選択した重力補償係数と重力トルクとに基づいて加減速度補正係数を算出する工程と、設定した基本加減速度と加減速度補正係数とに基づいて、関節駆動モータの加速度及び／又は減速度を算出する工程とをコンピュータに行わせる。
本発明によれば、算出した重力トルクとあらかじめ設定した複数の重力補償係数の中から選択した重力補償係数に基づいて加減速度補正係数を算出し、加減速度補正係数と設定した基本加減速度とに基づいて、関節駆動モータの加速度及び／又は減速度を算出するようにしたので、駆動態様によって異なる重力の影響（例えばトルク飽和、残留振動等）に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理が行えないコンピュータでも対応できる。

また、本発明に係る加減速制御方法のプログラムは、関節を有する機械の動作開始及び動作終了に関するデータ及び機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、重力トルク及び関節駆動モータの出力トルクに基づいて、動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、加減速度補正係数と関節に設定した基本加減速度とを乗じて関節駆動モータの加速度及び／又は減速度を算出する工程とをコンピュータに行わせる。
本発明によれば、機械の機構パラメータ並びに／又はペイロードに関するデータを含めて動作開始及び終了時の姿勢における重力トルク、トルク飽和防止及び残留振動抑制のために関節駆動モータの駆動方向別及び加速・減速別に設定した重力補償係数及び出力トルクに基づいて加減速度補正係数を算出し、その加減速度補正係数を基本加減速度に乗じて加速度及び／又は減速度を算出するようにしたので、重力の影響を大きく受けるトルク飽和、残留振動に適切に対応した加減速度を簡単な手順により算出することができる。これにより、高速な演算処理が行えないコンピュータでも対応できる。

また、本発明に係る加減速制御方法のプログラムは、算出した加減速度補正係数に基づいて、加減速度補正係数の最大値及び／又は最小値を制限する工程をさらにコンピュータに行わせる。
本発明によれば、加減速度補正係数を制限するようにしたので、機械の安全制御をコンピュータに行わせることができる。

実施の形態１．
図１は本発明の実施の形態１に係るマニピュレータ制御装置を中心とするシステムを表す図である。図１において、本実施の形態のマニピュレータ制御装置１０は、加減速制御手段１、補間演算手段２、信号出力手段３及びデータ記憶手段４で構成されている。マニピュレータ制御装置１０は、駆動信号を出力してマニピュレータ１００の動作を制御する。ここで本実施の形態では、主にマニピュレータ１００の各関節（軸）におけるモータの軸回転駆動の加減速を含む速度に関する制御について説明する（基本的には１つの動作に加速、減速が少なくともそれぞれ１回ずつ含まれる）。

加減速制御手段１は、さらに重力トルク演算部１Ａ、加減速度補正係数演算部１Ｂ、補正係数チェック部１Ｃ、加減速度演算部１Ｄ、主軸判断部１Ｅ及び加減速調整部１Ｆで構成される。重力トルク演算部１Ａは、重力の影響によって関節に発生する重力トルクを、マニピュレータ１００の各関節について算出する。また、加減速度補正係数演算部１Ｂは、重力トルクに基づいて各関節の加減速度補正係数を算出する。そして、補正係数チェック部１Ｃは、例えば、加減速度補正係数の効果を制限するため、加減速度補正係数に対して、定められた最大値及び最小値の範囲内であるかどうかを判断する。範囲内でないと判断すると、加減速度補正係数を範囲内になるように制限する。加減速度演算部１Ｄは、加減速度補正係数（補正係数チェック部１Ｃが制限した加減速度補正係数も含む）とあらかじめ各関節に設定された基本加減速度に基づいて、各関節の加減速度を算出する。

主軸判断部１Ｅは、補間演算手段２が補間演算により導き出した各関節の移動時間（回転駆動時間）に基づいて、移動時間が最大（最長）となる関節（これを主軸とする）を判断する。また、加減速度調整部１Ｆは、主軸以外の関節について加減速度を再計算し、各関節の駆動（マニピュレータ１００全体の動作）を調整する。

補間演算手段２は、加減速度演算部１Ｄが算出した加減速度等に基づいて、動作開始から終了までのマニピュレータ１００の動作について、各関節の駆動軌跡、速度等を補間演算する。さらに信号出力手段３は、補間演算手段２を経て算出した各関節の加減速度を含む駆動信号をマニピュレータ１００に出力する。

ここで、本実施の形態では、例えばＣＰＵを中心とする制御処理装置（コンピュータ）により加減速制御手段１、補正演算手段２及び信号出力手段３を実現するものとし、上述した各手段及び手段を構成する各部が行う処理を制御処理装置が実行し、その演算結果をデータ等処理するものとする。この場合、例えば、各部、各手段が処理を行う手順がプログラムとしてデータ記憶手段４に記憶されている。

データ記憶手段４は、加減速制御手段１、補正演算手段２及び信号出力手段３が処理を行うために必要となるデータが記憶されている。例えば、重力トルク演算部１Ａが重力トルクを算出するために必要となるペイロード２００の態様（質量、重心位置）、マニピュレータ１００の機構パラメータ（例えば各アーム（リンク）の質量、アーム長、重心位置）等のデータ及び動作開始点、終了点等の教示点のデータが記憶されている。また、加速度補正係数を演算するための重力補償係数のデータが記憶されている。さらに補正係数チェック部１Ｃが加減速度補正係数のチェックをするための最大値及び最小値のデータ、加減速度演算部１Ｄが加減速度を演算する際の基本加減速度のデータが記憶されている。

図２はマニピュレータ１００の一例を表す図である。ここでは、マニピュレータ１００として６軸垂直多関節ロボット等の多関節マニピュレータを例にして説明するが、重力の影響を考慮しなければならない環境下で動作するマニピュレータであれば、軸数等、その種類については特に限定しない。また、マニピュレータ以外にも、動作に際して重力の影響を考慮しなければならない関節（軸）を有する機械について適用することができる。ペイロード２００は例えばマニピュレータ１００が把持等する搬送物等である。

次に本実施の形態の処理について、加減速制御手段１を中心に説明する。重力トルク演算部１Ａでは、動作開始時及び動作終了時のマニピュレータ１００の姿勢に基づいて、各関節に発生する重力トルクＴｇを算出する。重力トルクＴｇの算出方法については、特に限定しないが、ペイロード２００の条件（質量、重心位置）、マニピュレータ１００の機構パラメータ等マニピュレータの種類によって、また、アーム等の傾き具合（水平距離）によって異なる。ここで、マニピュレータ１００の基部が鉛直方向に垂直となるように設けられている場合、図２においてＪ１の回転を行う関節については重力トルクＴｇが０となる。

ここで、マニピュレータ１００の加減速に利用できるトルクは、モータの最大出力トルクＴｐ及び重力トルクＴｇである。そこでマニピュレータ１００の関節の加減速（駆動）に利用できる加減速トルクＴａｃを次式（１）により算出する。そして、加減速トルクＴａｃをモータの最大出力トルクＴｐで正規化すると、次式（２）のようになる。ここでＴｇ／Ｔｐを正規化重力トルク値（無次元）ということにする。なお、（１）、（２）式はマニピュレータ１００の加減速に最大利用できるトルク最大出力トルクＴｐに基づいているが最大でなくても適用できる。
Ｔａｃ＝Ｔｐ＋Ｔｇ …（１）
Ｔａｃ／Ｔｐ＝（Ｔｐ＋Ｔｇ）／Ｔｐ
＝１＋Ｔｇ／Ｔｐ …（２）

加減速度補正係数演算部１Ｂは、各関節の加減速度を算出する際に基本加減速度にかけ合わせる加減速補正係数Ｋを算出する。加減速補正係数Ｋを算出するための基本的な式は次式（３）で表される。中括弧内で１を引いているのは基準を０にする（重力トルクＴｇ＝０のときに０となるようにして重力補償係数Ｋｇが加減速補正係数Ｋに影響しないようにする）ためである。中括弧内は前述した正規化重力トルク値と最終的に同じ値になる。一方、正規化重力トルク値と重力補償係数Ｋｇとの積に１を足しているのは加減速補正係数Ｋの基準を１にする（重力トルクＴｇ＝０）ときにＫ＝１とするためである。
Ｋ＝｛（Ｔｐ＋Ｔｇ）／Ｔｐ−１｝Ｋｇ＋１
＝（Ｔｇ・Ｋｇ＋Ｔｐ）／Ｔｐ
＝（Ｔｇ／Ｔｐ）Ｋｇ＋１ …（３）

（３）式の重力補償係数Ｋｇは、重力トルクＴｇの大きさによって、加減速補正係数Ｋの効果を調整するために設定した係数である。Ｋｇ＞０であれば、加減速補正係数Ｋが１より大きくなり、加減速が増加する。Ｋｇ＜０であれば、加減速補正係数が１より小さくなり、加減速が抑えられる。Ｋｇ＝０であれば、加減速における効果に重力トルクＴｇを作用させない。ここで、重力補償係数Ｋｇについて、本実施の形態では表１に示すように関節（回転）駆動方向Θ’（重力作用方向と同方向・逆方向）別及び加速・減速別に分けた４タイプのＫｇを設定するものとする。一般的には、トルク飽和は重力作用方向と逆方向に加速する際に問題となり、残留振動は位置決めを行う際（減速が伴う）に問題となる。そこで、駆動方向別及び加速・減速別にタイプを分け、それぞれの重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）を設定することで、重力により生じるトルク飽和、残留振動の問題をそれぞれ考慮した加減速度を設定できるようにする。

図３は重力補償係数Ｋｇを設定するまでの処理をフローチャートで表した図である。まず、Ｋｇ＝０（このときＫ＝１となる）と設定する（Ｓ１）。そして、例えば、重力作用方向と逆方向に加速した場合に、関節にトルク飽和が発生しないかどうかを判断する（Ｓ２）。さらに残留振動が許容範囲内にあるかどうかを判断する（Ｓ３）。このトルク飽和、残留振動の測定は、例えば実測、計算シミュレーション等により行う。トルク飽和又は残留振動が発生しなければ、Ｋｇ≧０となるような重力補償係数Ｋｇの設定を決定する（Ｓ４）。一方、発生すればＫｇ＜０となるようなＫｇの設定を決定する（Ｓ５）。

Ｓ４においてＫｇ≧０の決定をすると、｜Ｋｇ｜を増加（正の方向）させ（Ｓ６）、残留振動が増加したかどうかを判断する（Ｓ７）。残留振動が増加していないと判断するとＳ６に戻ってさらに｜Ｋｇ｜を増加させる。一方、残留振動が増加したと判断すると、トルク飽和及び残留振動が問題にならない重力補償係数Ｋｇ（例えば、１段階前のステップにおいて増加前のＫｇの値）を設定する（Ｓ８）。ここではＳ７において残留振動が増加したと判断すると、Ｓ８においてすぐに重力補償係数Ｋｇを設定しているが、例えば残留振動が許容範囲を越えるまで｜Ｋｇ｜を増加させた後、重力補償係数Ｋｇを設定してもよい。なお、Ｓ６において｜Ｋｇ｜の増加幅については特に限定しないが、｜Ｋｇ｜の増加幅を小さくすることによって、よりきめ細かな重力補償係数Ｋｇの設定を行うことができる（後述するＳ９においても同様である）。

Ｓ５においてＫｇ＜０の決定をすると、｜Ｋｇ｜を増加（負の方向）させ（Ｓ９）、トルク飽和がなくなったかどうかを判断する（Ｓ１０）。トルク飽和がなくなっていなければ、Ｓ９に戻り、さらに｜Ｋｇ｜を増加させる。トルク飽和がなくなれば、さらに残留振動が許容範囲内にあるかどうかを判断する（Ｓ１１）。許容範囲内になければＳ１０に戻ってさらに｜Ｋｇ｜を増加させる。許容範囲内にあれば、そのときの値を重力補償係数Ｋｇとして設定する（Ｓ１２）。

以上のようにして４つのタイプのそれぞれに設定した重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）をそれぞれ（３）式に当てはめて算出した加減速補正係数Ｋ（Ｋａｆ、Ｋｄｆ、Ｋａｒ、Ｋｄｒ）は、次式（４）〜（７）となる。
Ｋａｆ＝（Ｔｇ／Ｔｐ）Ｋｇａｆ＋１ …（４）
Ｋｄｆ＝（Ｔｇ／Ｔｐ）Ｋｇｄｆ＋１ …（５）
Ｋａｒ＝（Ｔｇ／Ｔｐ）Ｋｇａｒ＋１ …（６）
Ｋｄｒ＝（Ｔｇ／Ｔｐ）Ｋｇｄｒ＋１ …（７）

補正係数チェック部１Ｃは、加減速度補正係数演算部１Ｂが算出した各関節の加減速補正係数Ｋに対して、次式（８）に示すように、最小値Ｋｍｉｎ以上最大値Ｋｍａｘ以下の範囲内にあるかどうかをチェックする。例えば傾きが大きい等、マニピュレータ１００の姿勢によって加減速度を制限したい場合に加減速度補正係数の効果を制限する。Ｋｍａｘ＜Ｋと判断すればＫ＝Ｋｍａｘとし、Ｋｍｉｎ＞Ｋと判断すればＫ＝Ｋｍｉｎとする。なお、本実施の形態では最小値Ｋｍｉｎ及び最大値Ｋｍａｘは各関節により異なる値とするが、これに限定されるものではなく、関節に関係なく一律の値を定めてもよい。
Ｋｍｉｎ≦Ｋ≦Ｋｍａｘ …（８）

補正係数チェック部１Ｃでチェックされ、確定した加減速補正係数Ｋに基づいて、加減速度演算部１Ｄは、各関節の加減速度を算出する。ここで、加速を行う場合は、各関節に設定された基本加速度Ａａｂに、加減速補正係数Ｋａｆ又はＫａｒを乗する。また、減速を行う場合は、基本減速度Ａｄｂに、加減速補正係数Ｋｄｆ又はＫｄｒを乗する。ここで、基本加速度Ａａｂ、基本減速度Ａｄｂは、例えば、アームが伸びた状態の姿勢、縮んだ状態の姿勢（基本姿勢）等から加速又は基本姿勢等に減速する際の基本の加速度、減速度である。関節（回転）駆動方向が重力作用方向と同方向（Ｔｇ×Θ’≧０）の加速度Ａａｃ、減速度Ａｄｃは、それぞれ次式（９）、（１０）で表される。
Ａａｃ＝Ｋａｆ×Ａａｂ …（９）
Ａｄｃ＝Ｋｄｆ×Ａｄｂ …（１０）

また、関節（回転）駆動方向が重力作用方向と逆方向（Ｔｇ×Θ’＜０）の加速度Ａａｃ、減速度Ａｄｃは、それぞれ次式（１１）、（１２）で表される。
Ａａｃ＝Ｋａｒ×Ａａｂ …（１１）
Ａｄｃ＝Ｋｄｒ×Ａｄｂ …（１２）

補間演算手段２は、加減速度演算部１Ｄが算出した加速度で関節が加速駆動してマニピュレータ１００の動作が開始し、減速度で減速駆動してマニピュレータ１００の動作が終了するように、各関節について動作開始から終了までの軌跡（位置）、速度等を補間する演算を行う。

主軸判断部１Ｅは、各関節の加減速度等に基づいて各関節の移動時間（回転駆動時間）を算出し、その中での移動時間が最大となる関節を判断し、これを主軸とする。この最大の移動時間がマニピュレータ１００全体の動作時間となる。したがって、主軸以外の各関節は最大の移動時間になるように調整すればよいので、算出した移動時間よりも余裕ができることになる（例えば、主軸以外の関節は最大の出力トルクで加減速を行わなずにすむ）。そこで、加減速度調整部１Ｆは、最大移動時間の関節を主軸とし、主軸以外の関節について加減速の再計算を行い、各関節について最終的に加減速度を決定する。ここで、再計算の方法、手順については特に限定するものではない。例えば、急な減速を行うことによって残留振動が生じやすいので、最大移動時間の範囲内で加減速度を抑制するように調整すれば、残留振動の発生を抑えることができる。調整した加減速度に基づいて、補間演算手段２はさらに補間演算を行う。

信号出力手段３は、各関節の加減速度等に応じて補間演算手段２が演算した軌跡、速度等に基づいて、マニピュレータ１００を駆動させるための駆動信号をマニピュレータ１００に出力する。マニピュレータ１００の各関節には駆動信号に基づいた電流が供給され、関節駆動モータが（回転）駆動してマニピュレータ１００が動作する。

以上のように本実施の形態によれば、マニピュレータ１００の各関節に対し、重力トルク演算部１Ａが重力トルクＴｇを算出し、駆動態様（モータの駆動方向別及び加速・減速別）に基づいて複数設定された中から選択した重力補償係数Ｋｇにより、加速度補正係数演算部１Ｂが正規化重力トルク値（重力トルク）を調整した上で加速度補正係数Ｋを決定し、加減速演算部１Ｄが、加速度補正係数Ｋを基本加減速度に乗じて、各関節の加減速を算出するようにしたので、駆動の態様に応じ、重力の影響に適切に対応した加減速度を簡単な計算（アルゴリズム）により算出することができる。特にモータの駆動方向別及び加速・減速別に重力補償係数Ｋｇ（Ｋｇａｆ、Ｋｇｄｆ、Ｋｇａｒ、Ｋｇｄｒ）を設定しているので、重力作用方向と同方向に加速する際のトルク飽和、減速を伴う位置決めを行う際の残留振動による影響がそれぞれ生じやすい態様に基づいて調整した上で加減速度を算出することができ、トルク飽和防止及び残留振動抑制を有効に図ることができる。

さらに、加減速度補正係数Ｋの最大値、最小値を設定し、補正係数チェック部１Ｃにより、加減速度補正係数を最大値又は最小値に制限するようにしたので、例えば、傾きが大きい場合の加減速度を制限することができ、マニピュレータの動作の安全を図ることができる。また、主軸判断部１Ｅが、各関節の移動時間に基づいて主軸を判断し、その最大移動時間（マニピュレータ１００全体の動作時間）に合わせて加減速度調整部１Ｆが再計算して加減速度を調整するようにしたので、主軸以外の関節については、加減速度の余裕ができ、トルク飽和防止及び残留振動抑制をさらに図ることができる。

実施の形態１に係るマニピュレータ制御装置１０を中心とする図である。マニピュレータ１００の一例を表す図である。重力補償係数Ｋｇを設定する処理をフローチャートで表した図である。

符号の説明

１加減速制御手段、１Ａ重力トルク演算部、１Ｂ加減速度補正係数演算部、１Ｃ補正係数チェック部、１Ｄ加減速度演算部、１Ｅ主軸判断部、１Ｆ加減速度調整部、２補間演算手段、３信号出力手段、４データ記憶手段、１０マニピュレータ制御装置、１００マニピュレータ、２００ペイロード。

Claims

関節を有する機械を動作させる関節駆動モータに作用する重力トルクを算出する工程と、
あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と前記重力トルクとに基づいて加減速度補正係数を算出する工程と、
該加減速度補正係数と設定した基本加減速度とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する工程と
を有することを特徴とする加減速制御方法。
前記関節駆動モータのトルク飽和防止及び残留振動抑制のために、前記関節駆動モータの駆動方向別及び加速・減速別の前記駆動態様に基づいて前記重力補償係数を設定することを特徴とする請求項１に記載の加減速制御方法。
関節を有する機械の動作開始及び動作終了に関するデータ及び前記機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、
前記加減速度補正係数と設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する工程と
を有することを特徴とする加減速制御方法。
前記加減速度補正係数は、前記重力トルクの絶対値と前記関節駆動モータの出力トルクとの和を前記出力トルクで割った値から１を引いた値と、前記重力補償係数とを乗し、その値に１を加えた値であることを特徴とする請求項１〜３のいずれかに記載の加減速制御方法。
前記加減速度補正係数の最大値及び／又は最小値を設定し、算出した前記加減速度補正係数が範囲外であると判断すると、前記加減速度補正係数を最大値又は最小値に制限する工程をさらに有することを特徴とする請求項３又は４に記載の加減速制御方法。
前記機械は多関節型マニピュレータであることを特徴とする請求項１〜５のいずれかに記載の加減速制御方法。
多関節型マニピュレータを動作させる関節駆動モータに作用する重力トルクを算出する重力トルク演算手段と、
あらかじめ設定した複数の重力補償係数のうち、重力に影響される駆動態様に基づいて選択した重力補償係数と前記重力トルクとに基づいて加減速度補正係数を算出する加減速度補正係数演算手段と、
該加減速度補正係数と設定した基本加減速度とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段と
を備えることを特徴とする加減速制御装置。
多関節型マニピュレータの動作開始時及び終了時のデータ及び前記多関節型マニピュレータの機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する重力トルク演算手段と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する加減速度補正係数演算手段と、
前記加減速度補正係数と前記関節に設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する加減速度演算手段と
を備えることを特徴とする加減速制御装置。
前記加減速度補正係数に設定した最大値及び／又は最小値に基づいて前記加減速度補正係数演算手段が算出した前記加減速度補正係数を最大値又は最小値に制限する係数チェック手段をさらに備えることを特徴とする請求項７又は８に記載の加減速制御装置。
各関節の移動時間に基づいて最大移動時間の関節を判断する判断手段と、
前記最大移動時間の関節以外の関節について、前記最大移動時間に合わせた加速度及び／又は減速度を再計算する加減速度調整手段と
をさらに備えることを特徴とする請求項７〜９のいずれかに記載の加減速制御装置。
関節を有する機械を動作させる関節駆動モータに作用する重力トルクを算出する工程と、
駆動態様に基づいて複数設定された中から選択した重力補償係数と該重力トルクとに基づいて加減速度補正係数を算出する工程と、
設定した基本加減速度と前記加減速度補正係数とに基づいて、前記関節駆動モータの加速度及び／又は減速度を算出する工程と
をコンピュータに行わせることを特徴とする加減速制御方法のプログラム。
関節を有する機械の動作開始及び動作終了に関するデータ及び前記機械の機構パラメータ並びに／又はペイロードに関するデータに基づいて、動作開始及び終了時の姿勢における、関節駆動モータの駆動に作用する重力トルクを算出する工程と、
前記関節駆動モータの駆動方向別及び加速・減速別に定めた重力補償係数、前記重力トルク及び関節駆動モータの出力トルクに基づいて、前記動作において加速及び／又は減速する際の加減速度補正係数を算出する工程と、
前記加減速度補正係数と前記関節に設定した基本加減速度とを乗じて前記関節駆動モータの加速度及び／又は減速度を算出する工程と
をコンピュータに行わせることを特徴とする加減速制御方法のプログラム。
算出した前記加減速度補正係数に基づいて、前記加減速度補正係数の最大値及び／又は最小値を制限する工程をさらにコンピュータに行わせることを特徴とする請求項１２に記載の加減速制御方法のプログラム。