JP2022177607A - マニピュレータ - Google Patents

Abstract

【課題】残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができるマニピュレータを提供する。
【解決手段】アームと、アクチュエータ３０と、位置センサ４０と、加速度センサ５０と、コントローラ６０を備える。コントローラ６０は、位置指令に基づいてアクチュエータ３０を駆動して位置センサ４０の出力値をフィードバックしつつエンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに加速度センサ５０の出力値に基づいてエンドエフェクタの振動を抑制する振動抑制制御を行う。コントローラ６０は、振動抑制制御の継続時間を設定する時間設定部７０を有しており、時間設定部７０はエンドエフェクタの振動周波数情報に応じて継続時間を決定する。
【選択図】図２

Description

本発明は、マニピュレータに関するものである。

特許文献１に開示のロボットにおいては、角度センサ及び慣性センサとしての角速度センサを備える。制御指令値の角速度が０になって目的の停止位置に到達した時点から一定時間が経過すると、角速度情報を用いることなく制御指令値及び角度情報を用いた制御を実施する。

特開２０１０－２８４７７０号公報

ところで、ロボットアームの手先の振動を抑制するために加速度（慣性）センサを用いて制御する場合に、動作停止指令から一定時間経過時に加速度センサを用いた振動抑制制御を終了すると、以下の課題がある。一定時間が残留振動収束時間よりも長いと、アクチュエータ駆動のために余分な動力を使うことになり、効率悪化を招く懸念がある。一方、一定時間が残留振動収束時間よりも短いと、振動を収束できない懸念がある。

上記課題を解決するためのマニピュレータは、リンクが関節を介して繋がり、基端がベース部材に固定され、先端に設けたエンドエフェクタが前記関節の動作により目標の位置に移動可能なアームと、前記関節を動作させて前記エンドエフェクタを目標の位置に移動させるためのアクチュエータと、前記アクチュエータの動作を検出する位置センサと、前記アームまたは前記エンドエフェクタのいずれかに設けられた加速度センサと、位置指令に基づいて前記アクチュエータを駆動して前記位置センサの出力値をフィードバックしつつ前記エンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに、前記加速度センサの出力値に基づいて前記エンドエフェクタの振動を抑制する振動抑制制御を行う制御部と、を備えるマニピュレータであって、前記制御部は、前記振動抑制制御の継続時間を設定する時間設定部を有しており、前記時間設定部は前記エンドエフェクタの振動周波数情報に応じて前記継続時間を決定することを要旨とする。

これによれば、制御部の時間設定部が、振動抑制制御の継続時間を設定する際に、エンドエフェクタの振動周波数情報に応じて継続時間を決定することにより、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。

また、マニピュレータにおいて、前記振動周波数情報は、前記アームの姿勢情報であるとよい。
また、マニピュレータにおいて、前記振動周波数情報は、前記エンドエフェクタにかかる負荷情報であるとよい。

また、マニピュレータにおいて、前記振動周波数情報は、前記エンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータであるとよい。
また、マニピュレータにおいて、前記振動周波数情報は、前記加速度センサの検出値から求めた前記エンドエフェクタの振動周波数であるとよい。

本発明によれば、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。

実施形態におけるマニピュレータの概略構成図。 マニピュレータの電気的構成を示すブロック図。 コントローラの機能ブロック図。 マニピュレータの概略構成図。 マップ説明図。 （ａ）は位置指令を示すタイムチャート、（ｂ）は加速度を示すタイムチャート、（ｃ）は実施形態における加速度センサ使用／不使用を示すタイムチャート、（ｄ）は比較例における加速度センサ使用／不使用を示すタイムチャート。 （ａ）は位置指令を示すタイムチャート、（ｂ）は加速度を示すタイムチャート、（ｃ）は実施形態における加速度センサ使用／不使用を示すタイムチャート、（ｄ）は比較例における加速度センサ使用／不使用を示すタイムチャート。 別例を説明するためのマップ説明図。 別例を説明するためのコントローラの機能ブロック図。 別例を説明するためのアームの２慣性モデル図。 （ａ）は別例を説明するための位置指令を示すタイムチャート、（ｂ）は別例を説明するための加速度センサ出力を示すタイムチャート。

以下、本発明を具体化した一実施形態を図面に従って説明する。
図１、図２に示すように、マニピュレータ１０は、アーム２０と、関節を動作させるための複数のアクチュエータ３０と、アクチュエータ３０の動作を検出するための複数の位置センサ４０と、加速度センサ５０と、制御部としての複数のコントローラ６０と、記憶部８０と、を備える。図１において、水平方向をｘ，ｙ方向で示すとともに、上下方向をｚ方向としている。

図１に示すように、アーム２０は、リンク２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ，２１ｆが関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを介して繋がっている。詳しくは、基端側の棒状のリンク２１ａの先端側には関節２２ａが設けられ、関節２２ａと関節２２ｂとの間が棒状のリンク２１ｂで連結されている。関節２２ｂと関節２２ｃとの間が棒状のリンク２１ｃで連結されている。関節２２ｃと関節２２ｄとの間が棒状のリンク２１ｄで連結されている。関節２２ｄと関節２２ｅとの間が棒状のリンク２１ｅで連結されている。関節２２ｅには先端側の棒状のリンク２１ｆが繋がっている。

リンク２１ａの基端がベース部材２４に固定されている。これにより、アーム２０は、基端がベース部材２４に固定されている。リンク２１ｆの先端にはエンドエフェクタ（手先効果器）２３が取り付けられている。これにより、アーム２０は、先端に設けたエンドエフェクタ２３が関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅの動作により目標の位置に移動可能である。

関節（２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅ）毎に、アクチュエータ３０、位置センサ４０及びコントローラ６０が設けられている。具体的には、関節２２ａに、アクチュエータ３０ａ、位置センサ４０ａ及びコントローラ６０ａが設けられている。関節２２ｂに、アクチュエータ３０ｂ、位置センサ４０ｂ及びコントローラ６０ｂが設けられている。関節２２ｃに、アクチュエータ３０ｃ、位置センサ４０ｃ及びコントローラ６０ｃが設けられている。関節２２ｄに、アクチュエータ３０ｄ、位置センサ４０ｄ及びコントローラ６０ｄが設けられている。関節２２ｅに、アクチュエータ３０ｅ、位置センサ４０ｅ及びコントローラ６０ｅが設けられている。図２においては、アクチュエータ３０については２つのアクチュエータ３０ａ，３０ｅのみを示し、位置センサ４０については２つの位置センサ４０ａ，４０ｅのみを示し、コントローラ６０については２つのコントローラ６０ａ，６０ｅのみを示す。

アーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅに設けられた各アクチュエータ３０は、アーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させるためのものである。各アクチュエータ３０は、例えば、関節モータで構成されている。エンドエフェクタ２３は、例えば、把持機器であり、ワークＷを把持して搬送できる。

各位置センサ４０は、対応するアクチュエータ３０の動作を検出する。各位置センサ４０により、例えば、アクチュエータ３０の動作としてモータの回転角を検出でき、これにより関節角度を検知することが可能となる。図１におけるｘ－ｚ平面において関節角度θ１～θ５を例示している。

加速度センサ５０は、エンドエフェクタ２３に設けられている。加速度センサ５０は、エンドエフェクタ２３の振動を検出するためのものである。
各コントローラ６０は、それぞれ、マイクロコンピュータで構成されている。マイクロコンピュータは、命令を解読しデータ処理を実行するＣＰＵ、プログラムやデータを保存するメモリ、外部のセンサ、対応するアクチュエータとのデータの送受信を制御する入出力部等から構成されている。

図２に示すように、各コントローラ６０には上位機器であるホストコンピュータ１００が接続されている。ホストコンピュータ１００から各コントローラ６０に位置指令が送られてくる。なお、位置指令とは、ホストコンピュータ１００から送られてくる位置指令そのものだけでなくフィルタを介した位置指令も含む。各コントローラ６０には、対応するアクチュエータ３０が接続されている。各コントローラ６０は、対応するアクチュエータ３０に指令を出してアーム２０の関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させる。

各コントローラ６０には、対応する位置センサ４０が接続されている。各コントローラ６０には、対応する位置センサ４０の出力値が入力され、対応するアクチュエータ３０の動作を検知することができる。加速度センサ５０が各コントローラ６０に接続されている。各コントローラ６０には加速度センサ５０の出力値が入力されてエンドエフェクタ２３の振動を検知することができる。

各コントローラ６０は、位置指令に基づいて、対応するアクチュエータ３０を駆動して、対応する位置センサ４０の出力値をフィードバックしつつエンドエフェクタ２３を目標の位置に移動させるフィードバック制御を行う。各コントローラ６０は、エンドエフェクタ２３の振動を抑制すべく加速度センサ５０の出力値に基づいて振動抑制制御を行う。

図３に示すように、各コントローラ６０は、減算部６１と減算部６２と開閉部６３と開閉制御部６４と検出振動算出部６５と振動抑制制御部６６と位置制御部６７と速度制御部６８と速度推定部６９を有する。また、本例では位置制御－速度制御のＰ－ＰＩカスケード構造を示しているが、速度推定部や速度制御部を有さないＰＩＤ制御やＩ－ＰＤ制御も同様の効果を期待できる。

位置指令であるアクチュエータへの駆動指令値が減算部６１に入力される。位置センサ４０の出力値が減算部６１に入力される。減算部６１において、位置指令（アクチュエータへの駆動指令値）と位置センサ４０の出力値である実位置の差が出力される。

位置制御部６７において、減算部６１の出力が入力され、制御信号ωｒｅｆが出力される。
検出振動算出部６５において、加速度センサ５０の出力値である実加速度が入力され、姿勢に応じた座標変換処理がなされ、検出振動が出力される。ここで算出される検出振動は、振動抑制制御を行うアクチュエータの回転軸から見た接線方向の振動である。図１において、関節２２ｃ内のアクチュエータで振動抑制制御を実施する際の検出振動の方向は、関節２２ｃ内のアクチュエータの回転軸の接線方向となる。

図３の振動抑制制御部６６において、検出振動が入力され、制御信号ωａｃｃが出力される。
速度推定部６９において、位置センサ４０の出力値が入力されて、アクチュエータないしはアームの速度ωｍが出力される。

減算部６２において、制御信号ωｒｅｆと、制御信号ωａｃｃと、速度ωｍが入力される。減算部６２において、制御信号ωｒｅｆに対する制御信号ωａｃｃと、速度ωｍの差が出力される。

減算部６２の出力が速度制御部６８に入力され、速度制御部６８は駆動指令を出力する。
速度制御部６８の出力がアクチュエータ（関節モータ）３０に駆動指令として送られ、この駆動指令に基づいてアクチュエータ（関節モータ）３０が駆動する。

検出振動算出部６５から振動抑制制御部６６への信号伝達ライン上には開閉部６３が設けられている。開閉部６３は開閉制御部６４により開閉される。開閉制御部６４には、位置指令が入力されるとともに検出振動算出部６５の出力値が入力される。開閉制御部６４は、位置指令と検出振動算出部６５の出力値に基づいて開閉部６３を開閉する。

各コントローラ６０は、アクチュエータへの駆動指令値あるいは位置センサ４０の実測値から関節角度を検知する。コントローラ６０同士は相互通信により各関節角度を検知する。各コントローラ６０は、各関節角度からアーム２０の姿勢情報（後述するアーム２０の伸び量）を検知することができるようになっている。

図２に示すように、各コントローラ６０は、振動抑制制御の継続時間Ｔａｃｃを設定する時間設定部７０を有する。時間設定部７０は、エンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定することができるようになっている。

振動周波数情報は、各関節角度から求められるアーム２０の姿勢情報である。具体的には、アーム２０の姿勢情報とは、アーム２０の伸び量である。例えば図１がアーム２０を比較的縮めた縮小状態であり、図４がアーム２０を最大に伸ばした最大伸長状態である。図２の記憶部８０には図５に示すマップが記憶されている。図５において縦軸にはアーム姿勢情報、すなわちアーム２０の伸び量をとり、横軸には振動抑制制御の継続時間Ｔａｃｃをとっている。図５においてマップ特性線Ｌｍ１が設定されており、マップ特性線Ｌｍ１はアーム２０の伸び量が増えるほど継続時間Ｔａｃｃが長くなる特性線である。コントローラ６０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、アーム姿勢情報（アーム２０の伸び量）に対応する継続時間Ｔａｃｃを算出することができるようになっている。なお、アーム２０の姿勢情報としては、アーム２０の伸び量に限らず、エンドエフェクタ２３の振動に影響する任意の姿勢情報を採用し得る。

図３の開閉制御部６４の出力である開閉部６３のオフ条件は、閉じた後の振動抑制制御の継続時間Ｔａｃｃであり、振動抑制制御の継続時間Ｔａｃｃの決定方法として、アーム２０の姿勢情報を入力としたマップにより決定する。残留振動の周波数が低ければ収束までに時間がかかり、周波数が高ければ収束までの時間は短くなる。残留振動の周波数はアーム２０の姿勢（アーム２０の伸び量）に依存するため、これを元に加速度センサによる制振制御を終了するまでの時間を決定する。具体的には、アーム２０が縮むほど残留振動の周波数が高くなるので、振動抑制制御の継続時間Ｔａｃｃが短くされる。一方、アーム２０が伸びるほど残留振動の周波数が低くなるので、振動抑制制御の継続時間Ｔａｃｃが長くされる。

＜作用＞
次に、作用について説明する。
図６（ａ）には位置指令を示す。図６（ｂ）には検出振動算出部６５の出力値を示す。図６（ｃ）には開閉部６３の開閉、即ち、検出振動算出部６５の出力値を制御に使用する、あるいは、不使用とする指令を示す。図６（ｄ）には比較例における開閉部６３の開閉、即ち、検出振動算出部６５の出力値を制御に使用する、あるいは、不使用とする指令を示す。

図６（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。
関節毎に設けられたコントローラ６０のうちのいずれかのコントローラ６０における時間設定部７０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、動作停止タイミングｔ２におけるアーム姿勢情報（アーム２０の伸び量）に対応する継続時間Ｔａｃｃを算出する。よって、継続時間Ｔａｃｃは、アーム姿勢に応じて変わる変数である。

コントローラ６０は、動作停止タイミングｔ２のタイミングで振動抑制制御を開始する。そして、動作停止タイミングｔ２から、継続時間Ｔａｃｃが経過したｔ３のタイミングにおいて振動抑制制御を終了する。

図６（ｄ）に示す比較例においては、継続時間は固定値であり、例えば動作停止指令から一定時間経過時に加速度センサを用いた振動抑制制御を終了する。
アーム２０の姿勢によって停止後の残留振動が収まるまでの時間が異なる。図６（ｄ）に示す比較例のように残留振動の収束までに最も時間のかかる条件で一定時間（固定値）を決定した場合、その他の条件では残留振動が収まった後も加速度センサを用いた制振制御を実施することになる。そのため、アクチュエータ駆動のために余分なモータ電流を出力し、電力効率の悪化につながるおそれがある。

本実施形態では例えば図１に示すようにアーム２０を比較的縮めた状態において図６（ｃ）に示すように、継続時間Ｔａｃｃは、アーム姿勢に応じて変わり、図６（ｄ）の比較例よりも短くされる。そのため、電力効率の悪化を防止することができる。

アーム２０の姿勢が図４に示すようにアーム２０を最大に伸ばした状態である場合には、図７（ａ）、図７（ｂ）、図７（ｃ）のようになる。
図７（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。

関節毎に設けられたコントローラ６０のうちのいずれかのコントローラ６０における時間設定部７０は、図５のマップ（マップ特性線Ｌｍ１）を用いて、アーム姿勢に対応する継続時間Ｔａｃｃを算出する。

コントローラ６０は、図７（ａ）～図７（ｃ）に示すようにｔ２のタイミングで振動抑制制御を開始する。そして、動作停止タイミングｔ２から、継続時間Ｔａｃｃが経過したｔ１０のタイミングにおいて振動抑制制御を終了する。

図７（ｄ）に示す比較例においては、図６（ｄ）と同様に継続時間は固定値である。
図７（ｄ）に示す比較例において、継続時間を決定する条件が残留振動の収束までに最も時間のかかる条件でなかった場合、動作条件によっては振動抑制制御終了（継続時間の経過）後、振動を収束できずに振動を制振しきれないおそれがある。

図７（ｃ）に示す実施形態においては、継続時間Ｔａｃｃは、アーム姿勢に応じて変わり、図７（ｄ）の比較例よりも長くされる。そのため、停止時の残留振動を抑制することができる。

このようにして、図６（ｄ）の比較例では、制振制御の終了が遅すぎる場合が生じ得るが、図６（ｃ）の本実施形態では、制振完了後、速やかに加速度センサによる制振制御を終了するため、無駄なモータ電流を出力しない。これにより、電力効率悪化を防止できる。

また、図７（ｄ）の比較例では、制振制御の終了が速すぎる場合においては振動を制振しきれないおそれがある。図７（ｃ）の本実施形態では、制振完了まで加速度センサによる制振制御を行っているので、制振目標加速度範囲内への収束が速い。

よって、停止時の残留振動を抑制し、かつ、アクチュエータの駆動電力効率の悪化を防ぐことができる。
＜実施形態の効果＞
上記実施形態によれば、以下のような効果を得ることができる。

（１）マニピュレータ１０の構成として、リンク２１ａ～２１ｆが関節２２ａ～２２ｅを介して繋がり、基端がベース部材２４に固定され、先端に設けたエンドエフェクタ２３が関節２２ａ～２２ｅの動作により目標の位置に移動可能なアーム２０を備える。マニピュレータ１０は、関節２２ａ～２２ｅを動作させてエンドエフェクタ２３を目標の位置に移動させるためのアクチュエータ３０を備える。マニピュレータ１０は、アクチュエータ３０の動作を検出する位置センサ４０と、エンドエフェクタ２３に設けられた加速度センサ５０を備える。マニピュレータ１０は、制御部としてのコントローラ６０を備える。コントローラ６０は位置指令に基づいてアクチュエータ３０を駆動して位置センサ４０の出力値をフィードバックしつつエンドエフェクタ２３を目標の位置に移動させるフィードバック制御を行うとともに加速度センサ５０の出力値に基づいてエンドエフェクタ２３の振動を抑制する振動抑制制御を行う。コントローラ６０は、振動抑制制御の継続時間Ｔａｃｃを設定する時間設定部７０を有しており、時間設定部７０はエンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定する。よって、コントローラ６０の時間設定部７０は、振動抑制制御の継続時間Ｔａｃｃを設定する際に、エンドエフェクタ２３の振動周波数情報に応じて継続時間Ｔａｃｃを決定することにより、残留振動を抑制しつつアクチュエータの駆動効率悪化を防止することができる。

（２）振動周波数情報は、アーム２０の姿勢情報であるので、実用的である。
実施形態は前記に限定されるものではなく、例えば、次のように具体化してもよい。
・振動周波数情報は、エンドエフェクタ２３にかかる負荷情報であってもよい。具体的には、各コントローラ６０はホストコンピュータ１００との通信によりワークＷの情報を取得して、エンドエフェクタ２３にかかる負荷情報としてワーク質量ｍを検知する。記憶部８０には図８に示すマップが記憶されている。図８において縦軸にはワーク質量ｍをとり、横軸には振動抑制制御の継続時間Ｔａｃｃをとっている。図８においてマップ特性線Ｌｍ２が設定されており、マップ特性線Ｌｍ２はワーク質量ｍが大きいほど継続時間Ｔａｃｃが長くなる特性線である。コントローラ６０は、図８のマップ（マップ特性線Ｌｍ２）を用いて、ワーク質量ｍに対応する継続時間Ｔａｃｃを算出することができる。

振動抑制制御の継続時間Ｔａｃｃの決定方法として、エンドエフェクタ２３にかかる負荷情報を入力としたマップにより、継続時間Ｔａｃｃを決定する。残留振動の周波数が低ければ収束までに時間がかかり、周波数が高ければ収束までの時間は短くなる。残留振動の周波数は手先の負荷であるエンドエフェクタ２３にかかる負荷により決定されるため、これを元に制振制御を終了するまでの時間を決定する。エンドエフェクタ２３にかかる負荷によって停止後の残留振動が収まるまでの時間が異なる。図６（ｄ）に示す比較例のように残留振動の収束までに最も時間のかかる条件で一定時間（固定値）を決定した場合、その他の条件では残留振動が収まった後も加速度センサを用いた制振制御を実施することになる。そのため、余分なモータ電流を出力し、電力効率の悪化につながるおそれがある。本例においては、継続時間Ｔａｃｃは、エンドエフェクタ２３にかかる負荷に応じて変わるため、図６（ｃ）のように電力効率の悪化を防止することができる。

図７（ｄ）に示す比較例のように継続時間を決定する条件が残留振動の収束までに最も時間のかかる条件でなかった場合、動作条件によっては制振制御終了（継続時間の経過）後、振動を制振しきれないおそれがある。本例においては継続時間Ｔａｃｃは、エンドエフェクタ２３にかかる負荷に応じて変わる。そのため、図７（ｃ）のように停止時の残留振動を抑制することができる。

このようにして、図６（ｄ）の比較例では、制振制御の終了が遅すぎる場合が生じ得るが、図６（ｃ）の本例では、制振完了後、速やかに加速度センサによる制振制御を終了するため、無駄なモータ電流を出力しない。これにより、電力効率悪化を防止できる。また、図７（ｄ）の比較例では、制振制御の終了が速すぎる場合においては振動を制振しきれないおそれがある。図７（ｃ）の本例では、制振完了まで制振制御を行っているので、制振目標加速度範囲内への収束が速い。よって、停止時の残留振動を抑制し、かつ、アクチュエータの駆動電力効率の悪化を防ぐことができる。

なお、振動周波数情報として、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報を用いて、継続時間Ｔａｃｃを決定してもよく、要は、振動周波数情報は、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報の少なくとも一方であればよい。具体的には、アーム２０の姿勢及びエンドエフェクタ２３にかかる負荷情報の少なくとも一方を入力としたマップにより継続時間Ｔａｃｃを決定する。

・上述したようにアーム２０の姿勢、エンドエフェクタ２３にかかる負荷情報に基づいたマップで振動抑制制御の継続時間を算出したが、エンドエフェクタ２３における振動の周波数を推定して振動抑制制御の継続時間を算出してもよい。

予めエンドエフェクタ２３における残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係をマップ化しておき、下記の［その１］または［その２］のように残留振動の周波数を推定する。

［その１］
振動周波数情報は、エンドエフェクタ２３を目標の位置に移動させる際のフィードフォワード制御に用いるパラメータである。図９、図１０を用いて具体的に説明する。

図３に代わる図９において、コントローラ６０は、フィードフォワード制御部９０と加算部９１を有する。フィードフォワード制御部９０は、位置指令を入力する。フィードフォワード制御部９０は、モデル式を用いて駆動指令を出力する。モデル式は、位置指令に応じてアーム２０を動かそうとした場合にアーム２０が理想の剛体であると仮定したときにおいて外乱が無い状態で必要トルクを求めるための式である。加算部９１は、速度制御部６８の出力である駆動指令と、フィードフォワード制御部９０の出力である駆動指令とを加算して出力する。加算部９１の出力がアクチュエータ（関節モータ）３０に駆動指令として送られ、この駆動指令に基づいてアクチュエータ（関節モータ）３０が駆動する。

ここで、フィードフォワード制御に使用されるモデル式の一例を、図１０に示す。モデル式として、２慣性モデルを用いている。
２慣性モデルは、アームの関節の振動を物理モデルで表したものであり、特に、バネ定数、慣性モーメント、粘性係数を用いている。モデル式はモータの出力トルクから実際のアームの手先の位置までの物理的な材料力学の式であり、トルクから手先の位置までの式を、物理的に運動方程式を作って計算している。

図１０において、θｍがモータ角度、θｌが負荷側角度、Ａｃｃｌが加速度、τｍが実トルク、Ｉｒｍｓ＊が電流指令値である。Ｊｍがモータ慣性モーメント、Ｊｌが負荷側慣性モーメント、Ｎｇが減速比、Ｋ０がモータ軸受のバネ定数、Ｄ０がモータ軸受の粘性係数、Ｋ１がギアのバネ定数、Ｄ１がギアの粘性係数、Ｋｔがトルク定数、Ｌ１がリンク長である。

このように、アームの制御には一般的に応答性向上のため、フィードフォワード制御が用いられる。フィードフォワード制御は、予め制御対象をモデル化してパラメータを記憶しておくことで、振動を抑制しつつ応答性を向上させることができる。

このフィードフォワード制御に用いているパラメータを元に、残留振動の周波数を推定する。例えば、下記式（１）により周波数ｆ、即ち、共振周波数を算出する。

上記式（１）において、Ｋ１、Ｊｍ、Ｊｌは、以下のとおりである。

Ｋ１：ギアのバネ定数
Ｊｍ：モータ慣性モーメント
Ｊｌ：負荷側慣性モーメント
このようにして残留振動の周波数ｆを取得し、予めマップ化しておいた残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係を参照することで、振動抑制制御を終了するまでの継続時間を決定する。

このように、振動周波数情報は、エンドエフェクタ２３を目標の位置に移動させる際のフィードフォワード制御に用いるパラメータであるギアのバネ定数Ｋ１、モータ慣性モーメントＪｍ、負荷側慣性モーメントＪｌである。

［その２］
振動周波数情報は、加速度センサ５０の検出値から求めたエンドエフェクタ２３の振動周波数ｆである。

例えば、図１１（ａ）、図１１（ｂ）に示すように、加速度センサ５０による検出振動における振動の中心レベルとクロスするタイミング間の周期Ｔから振動周波数ｆを算出して、振動周波数ｆから継続時間Ｔａｃｃを決定してもよい。加速度センサの検出値を元に推定する際に、エンドエフェクタに設置されている加速度センサの検出値を記録し、残留振動の周波数を直接計測する。そして、残留振動の周波数を取得し、予めマップ化しておいた残留振動の周波数と振動抑制制御の継続時間Ｔａｃｃの関係を参照することで、振動抑制制御を終了するまでの継続時間を決定する。

検出振動の周波数を直接計算して周波数に基づいて継続時間Ｔａｃｃを求めることによるメリットについて言及する。
振動抑制制御の継続時間Ｔａｃｃを決定するために検出振動の振幅を求める場合においては、振幅の最小値と最大値を算出するが、この際、ノイズを検出していまい、例えば無駄な制振を行ってしまう可能性がある。これに対し、［その２］では、振幅を検出せずにクロスタイミング間の時間検出により振動周波数から継続時間を求めることによりノイズの影響を受けにくい。即ち、加速度センサ出力をそのままではなく周波数を計測して周波数に基づいて継続時間Ｔａｃｃを求めることにより振幅を求める際のデメリットを無くすることができる。

このようにして、［その１］または［その２］の手法を元に残留振動の周波数を取得し、予めマップ化しておいた残留振動の周波数と継続時間の関係を参照することで、振動抑制制御を終了するまでの時間を決定する。

広義には、振動周波数情報は、アームの姿勢及びエンドエフェクタにかかる負荷情報及びエンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータ及び加速度センサの検出値から求めた振動周波数の少なくとも一つであればよい。

・加速度センサ５０は、エンドエフェクタ２３に設けたが、加速度センサ５０は、アーム２０の任意の位置に設けられていてもよい。この場合は、アーム２０の任意の位置における加速度からエンドエフェクタ２３の加速度を推定する。

・振動周波数情報と継続時間Ｔａｃｃとの関係を示すマップを用いて振動抑制制御の継続時間Ｔａｃｃを決定したが、これに代わり関数式を用いてもよい。詳しくは、振動周波数情報と継続時間Ｔａｃｃとの関係を示す関係式を用いて振動抑制制御の継続時間Ｔａｃｃを決定してもよい。

１０…マニピュレータ、２０…アーム、２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ，２１ｆ…リンク、２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅ…関節、２３…エンドエフェクタ、２４…ベース部材、３０…アクチュエータ、４０…位置センサ、５０…加速度センサ、６０…コントローラ、７０…時間設定部。

Claims (5)

1. リンクが関節を介して繋がり、基端がベース部材に固定され、先端に設けたエンドエフェクタが前記関節の動作により目標の位置に移動可能なアームと、
   前記関節を動作させて前記エンドエフェクタを目標の位置に移動させるためのアクチュエータと、
   前記アクチュエータの動作を検出する位置センサと、
   前記アームまたは前記エンドエフェクタのいずれかに設けられた加速度センサと、
   位置指令に基づいて前記アクチュエータを駆動して前記位置センサの出力値をフィードバックしつつ前記エンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに、前記加速度センサの出力値に基づいて前記エンドエフェクタの振動を抑制する振動抑制制御を行う制御部と、
   を備えるマニピュレータであって、
   前記制御部は、前記振動抑制制御の継続時間を設定する時間設定部を有しており、前記時間設定部は前記エンドエフェクタの振動周波数情報に応じて前記継続時間を決定する
   ことを特徴とするマニピュレータ。
2. 前記振動周波数情報は、前記アームの姿勢情報である
   ことを特徴とする請求項１に記載のマニピュレータ。
3. 前記振動周波数情報は、前記エンドエフェクタにかかる負荷情報である
   ことを特徴とする請求項１に記載のマニピュレータ。
4. 前記振動周波数情報は、前記エンドエフェクタを目標の位置に移動させる際のフィードフォワード制御に用いるパラメータである
   ことを特徴とする請求項１に記載のマニピュレータ。
5. 前記振動周波数情報は、前記加速度センサの検出値から求めた前記エンドエフェクタの振動周波数である
   ことを特徴とする請求項１に記載のマニピュレータ。

Images