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

Description

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

ロボットアーム制御において、移動～停止後に残留振動が生じることがある。その際、振動が落ち着くのを待ってから次の動作へ移るため、残留振動が大きいと動作の遅れにつながる。そのため、特許文献１に開示の成形品取出機のチャック制振方法においては、チャックの移動停止時に出力される停止指示信号に基づいて加速度センサからの信号を制御に用いることで、停止後の残留振動を低減している。

特開２００７－１３６９７５号公報

ところが、停止指示信号を受けた瞬間、即ち、位置指令の動作停止となった瞬間に加速度センサの出力値をいきなり制御に反映させると、制御の出力が急峻に変化するため、振動を誘発する虞がある。

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

これによれば、位置指令の動作停止タイミングを検知した後の加速度センサの出力値が基準レベルとクロスするタイミングで振動抑制制御を有効化することにより、位置指令の動作停止となった瞬間に加速度センサの出力値を制御に反映させる場合に比べ、振動を適切に抑制することができる。

また、マニピュレータにおいて、前記制御部は、重力によるオフセット分を差し引いた前記加速度センサの出力値を元に算出した検出振動が、前記基準レベルとしてのゼロレベルとゼロクロスするタイミングで前記振動抑制制御を有効化するとよい。

また、マニピュレータにおいて、前記制御部は、前記加速度センサの出力値を元に算出した検出振動が、前記基準レベルとしてのゼロレベルとゼロクロスするタイミングで前記振動抑制制御を有効化するとよい。

また、マニピュレータにおいて、前記制御部は、前記加速度センサの出力値を元に算出した検出振動が、ゼロレベルに重力によるオフセット分を加算した前記基準レベルとクロスするタイミングで前記振動抑制制御を有効化するとよい。

また、マニピュレータにおいて、前記制御部は、前記クロスするタイミングから一定時間後に前記振動抑制制御を無効化するとよい。

本発明によれば、振動を適切に抑制することができる。

実施形態におけるマニピュレータの概略構成図。 マニピュレータの電気的構成を示すブロック図。 コントローラの機能ブロック図。 （ａ）～（ｄ）は位置指令、加速度、加速度センサ使用／不使用を示すタイムチャート。 マニピュレータの概略構成図。 （ａ）～（ｄ）は位置指令、加速度、加速度センサ使用／不使用を示すタイムチャート。 （ａ）～（ｄ）は位置指令、加速度、加速度センサ使用／不使用を示すタイムチャート。

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

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

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

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

位置センサ４０は、アクチュエータ３０の動作を検出する。位置センサ４０により、例えば、アクチュエータ３０の動作としてモータの回転角を検出でき、これにより関節角度等を検知することが可能となる。

加速度センサ５０は、エンドエフェクタ２３に設けられている。加速度センサ５０は、アーム２０の先端部の振動を検出するためのものである。
図１に示すように、エンドエフェクタ２３に設けられた加速度センサ５０は、取付面に対して平行方向（ｘ´，ｙ´方向）と垂直方向（ｚ´方向）の３方向の加速度を検出する。よって、図１のリンク２１ｆが斜め下方に延びる状態では、地面に対して水平方向（ｘ，ｙ方向）と加速度検出方向とはずれている。これにより、加速度センサ５０の検出値には、重力によるオフセット分が含まれている。

コントローラ６０は、マイクロコンピュータを中心に構成されている。マイクロコンピュータは、命令を解読しデータ処理を実行するＣＰＵ、プログラムやデータを保存するメモリ、外部のセンサ、アクチュエータとのデータの送受信を制御する入出力部等から構成されている。

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

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

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

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

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

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

振動抑制制御部６６において、検出振動が入力され、制御信号ωａｃｃが出力される。
速度推定部６９において、アクチュエータないしはアームの速度ωｍが出力される。
減算部６２において、制御信号ωｒｅｆと、制御信号ωａｃｃと、速度ωｍが入力される。減算部６２において、制御信号ωｒｅｆに対する制御信号ωａｃｃと、速度ωｍの差が出力される。

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

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

コントローラ６０は、アクチュエータへの駆動指令値あるいは位置センサ４０の実測値からアーム２０の姿勢を検知する。コントローラ６０は、アーム２０の姿勢から加速度センサ５０の姿勢を検知する。コントローラ６０は、加速度センサ５０の姿勢に基づいて重力によるオフセット分ＯＳｇ（図４（ｂ）参照）を算出する。つまり、コントローラ６０は、アーム２０の姿勢が分かることにより加速度センサ５０の出力値のうちの重力によるオフセット分ＯＳｇを検知することができる。

次に、作用について説明する。
図４（ａ）には位置指令を示す。図４（ｂ）には検出振動算出部６５の出力値を示す。図４（ｃ）には開閉部６３の開閉、即ち、検出振動算出部６５の出力値を制御に使用する、あるいは、不使用とする指令を示す。図４（ｄ）には開閉部６３が開くことによる振動抑制制御部６６の入力信号ａｃｃを示す。

図４（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。
図１、図４（ｂ）に示すように、加速度センサ５０の加速度検出方向と重力の方向が直交しない場合、アーム２０の姿勢に応じて加速度センサ５０の出力値に重力によるオフセット分ＯＳｇが生じる。即ち、加速度センサ５０の出力値には重力によるオフセット分ＯＳｇが含まれている。コントローラ６０の開閉制御部６４は、加速度センサ５０の出力値から姿勢による重力オフセット分（ＯＳｇ）を差し引いて検出振動を得る。つまり、アクチュエータへの駆動指令値あるいは位置センサ４０の実測値からアーム２０の姿勢が分かるので加速度センサ５０の姿勢が分かり、加速度センサ５０の姿勢に基づいて重力によるオフセット分ＯＳｇを計算することができる。

そして、コントローラ６０の開閉制御部６４は、図４（ａ）～図４（ｄ）に示すように、検出振動と基準レベルとしてのゼロレベルとを比較して検出振動が基準レベルとしてのゼロレベルとゼロクロスするｔ３のタイミングで開閉部６３を閉じる。これにより、減算部６２において、減算部６１の出力に対し加速度センサ５０の出力値が減算されてアクチュエータ３０の駆動指令となる。

このように、コントローラ６０は、位置指令の動作停止タイミングを検知した後の重力によるオフセット分ＯＳｇを差し引いた加速度センサ５０の出力値を元に算出した検出振動が、基準レベルとしてのゼロレベルとゼロクロスするタイミングで加速度センサ５０を用いた振動抑制制御を有効化する。広義には、コントローラ６０は、位置指令の動作停止タイミングを検知した後の加速度センサ５０の出力値が基準レベルとクロスするｔ３のタイミングで振動抑制制御を有効化する。

図４（ｃ）に示すように、コントローラ６０の開閉制御部６４は、位置指令の動作停止タイミングを検知した後の一定時間Ｔ２が経過した後のｔ４のタイミングで開閉部６３を開く。

このように、コントローラ６０は、図４（ｃ）に示すように、クロスするタイミングから一定時間Ｔ２後のｔ４のタイミングで開閉部６３を開いて振動抑制制御を無効化する。
その結果、図４（ｄ）に示すように、ｔ３～ｔ４の期間における加速度センサ５０の出力値が振動抑制制御に反映される。

図５に示すように、リンク２１ｆが水平方向に延びる状態からエンドエフェクタ２３を水平方向に移動させる場合には、リンク２１ｆでは、水平方向（ｘ，ｙ方向）と加速度検出方向とが一致している。これにより、加速度センサ５０の検出値には、重力によるオフセット分が含まれない。即ち、水平方向にしか動かない場合には重力によるオフセット分を考慮しなくてもよい。

重力に直交する水平方向へ移動する場合、即ち、加速度センサ５０の加速度検出方向と重力の方向が直交する場合、加速度センサ５０の出力値に重力によるオフセット分が生じない。この場合には、次のようになる。

図６（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。
図５、図６（ｂ）に示すように、加速度センサ５０の出力値には重力によるオフセット分が含まれていない。よって、加速度センサ５０の出力値が検出振動となる。コントローラ６０の開閉制御部６４は、加速度センサ５０の出力値を元に算出した検出振動と基準レベルとしてのゼロレベルとを比較して加速度センサ５０の出力値を元に算出した検出振動が基準レベルとしてのゼロレベルとゼロクロスするｔ１０のタイミングで開閉部６３を閉じる。これにより、減算部６２において、減算部６１の出力に対し加速度センサ５０の出力値が減算されてアクチュエータ３０の駆動指令となる。このように、コントローラ６０は、加速度センサ５０の出力値を元に算出した検出振動が基準レベルとしてのゼロレベルとゼロクロスするｔ１０のタイミングで加速度センサ５０の出力値を用いた振動抑制制御を有効化する。

図６（ｃ）に示すように、コントローラ６０の開閉制御部６４は、位置指令の動作停止タイミングを検知した後の一定時間Ｔ２が経過した後のｔ１１のタイミングで開閉部６３を開く。このように、コントローラ６０は、図６（ｃ）に示すように、クロスするタイミングから一定時間Ｔ２後のｔ１１のタイミングで開閉部６３を開いて振動抑制制御を無効化する。

その結果、図６（ｄ）に示すように、ｔ１０～ｔ１１の期間における加速度センサ５０の出力値が振動抑制制御に反映される。
以下、詳しく説明する。

アクチュエータ（モータ）駆動開始直後から加速度センサ５０による振動抑制制御を行うと、駆動のための加速度を振動による加速度と捉えてしまい加速度を加わりにくくする方向にトルクが出るようにしてしまい移動速度が低下する懸念がある。振動にはアーム先端部が動いている時の振動と停止した時の振動がある。本実施形態では、アーム先端部が動いている時の振動、及び、位置指令の動作停止直後の振動は制御に反映させない。これにより、移動速度が低下することを防止することができる。

また、特許文献１に開示の技術のように停止指示信号を入力した瞬間に振動抑制制御を行うと、加速度センサ出力値を入力した途端に大きな外乱を入力することにより振動が誘発されることが懸念される。アクチュエータ（モータ）停止後の残留振動について、本実施形態では、検出振動がゼロとなるタイミングから制御を開始することにより、ゼロでなければアクチュエータ（モータ）への指令が急峻に変化して振動を誘発するのに対し、振動の誘発を防ぐことができる。具体的には、図４（ａ）～図４（ｄ）でのｔ２～ｔ３の時間Ｔ１における加速度センサ出力値（ステップ状の外乱）を無効化する。

このようにして、アーム２０の移動（アクチュエータ３０の駆動）速度を低下させることなく、かつアーム先端部の振動を抑制することができる。
また、目標指令値はＡ→Ｂ→Ｃというように変わっていくので、次にアクチュエータ（モータ）が駆動する際に加速度センサ５０を用いた制御が有効化していると次の動き出しにおいて動作に遅れが生じる。本実施形態では、加速度センサ５０を用いた制御を有効化してから一定時間Ｔ２が経過したら、加速度センサ５０を用いた制御を再度無効化する。よって、加速度センサ５０の出力値の内の所定時期の出力値を排除することにより、アクチュエータ（モータ）への指令に対して遅れなく追従することができる。

上記実施形態によれば、以下のような効果を得ることができる。
（１）マニピュレータ１０の構成として、リンク２１ａ，２１ｂ，２１ｃ，２１ｄ，２１ｅ，２１ｆが関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅを介して繋がり、基端がベース部材２４に固定され、先端に設けたエンドエフェクタ２３が関節２２ａ，２２ｂ，２２ｃ，２２ｄ，２２ｅの動作により目標の位置に移動可能なアーム２０を備える。アーム２０の関節を動作させてエンドエフェクタ２３を目標の位置に移動させるためのアクチュエータ３０と、アクチュエータ３０の動作を検出する位置センサ４０を備える。エンドエフェクタ２３に設けられた加速度センサ５０を備える。位置指令に基づいてアクチュエータ３０を駆動して位置センサ４０の出力値をフィードバックしつつエンドエフェクタ２３を目標の位置に移動させるフィードバック制御を行うとともに、加速度センサ５０の出力値に基づいて振動抑制制御を行う制御部としてのコントローラ６０を備える。コントローラ６０は、位置指令の動作停止タイミングを検知した後の加速度センサ５０の出力値が基準レベルとクロスするタイミングで振動抑制制御を有効化する。

よって、位置指令の動作停止タイミングを検知した後の加速度センサ５０の出力値が基準レベルとクロスするタイミングで振動抑制制御を有効化することにより、位置指令の動作停止となった瞬間に加速度センサの出力値をいきなり制御に反映させる場合に比べ、図４（ｂ）に示すように所定の時間Ｔ１だけ遅らせて振動を適切に抑制することができる。

（２）図４（ａ）～図４（ｄ）を用いて説明したように、コントローラ６０は、重力によるオフセット分ＯＳｇを差し引いた加速度センサ５０の出力値を元に算出した検出振動が、基準レベルとしてのゼロレベルとゼロクロスするタイミングで振動抑制制御を有効化する。よって、加速度センサ５０において重力を加味して振動を適切に抑制することができる。

（３）図６（ａ）～図６（ｄ）を用いて説明したように、コントローラ６０は、加速度センサ５０の出力値を元に算出した検出振動が、基準レベルとしてのゼロレベルとゼロクロスするタイミングで振動抑制制御を有効化する。よって、加速度センサ５０において重力を加味することなく加速度検出方向のみ考慮すればよい場合において振動を適切に抑制することができる。

（４）コントローラ６０は、クロスするタイミングから一定時間後に振動抑制制御を無効化する。よって、次の位置指令に対して動作に遅れが生じるのを抑制することができる。

実施形態は前記に限定されるものではなく、例えば、次のように具体化してもよい。
・図４（ａ）～図４（ｄ）で説明したように、コントローラ６０は、位置指令の動作停止タイミングを検知した後の加速度センサ５０の出力値から重力によるオフセット分ＯＳｇを差し引いた検出振動が基準レベルとしてのゼロレベルとゼロクロスするタイミングで加速度センサ５０の出力値を用いた振動抑制制御を有効化した。

これに代わり図７（ａ）～図７（ｄ）に示すようにしてもよい。
検出振動を計算せずに、加速度センサ５０の出力値と重力によるオフセット分ＯＳｇを直接比較する。アーム姿勢から重力によるオフセット分ＯＳｇを計算し、ゼロレベルに重力によるオフセット分ＯＳｇを加算したものを基準レベルＬｒｅｆとする。

図７（ａ）においてｔ１が動作開始タイミングであり、ｔ２が動作停止タイミングである。図７（ｂ）に示すように、加速度センサ５０の出力値には重力によるオフセット分ＯＳｇが含まれている。コントローラ６０の開閉制御部６４は、ゼロレベルに重力によるオフセット分ＯＳｇを加算した基準レベルＬｒｅｆと加速度センサ５０の出力値を比較して基準レベル（ゼロレベルに重力オフセット分を加算したもの）Ｌｒｅｆに加速度センサ５０の出力値がクロスするｔ２０のタイミングで開閉部６３を閉じる。これにより、減算部６２において、減算部６１の出力に対し加速度センサ５０の出力値が減算されてアクチュエータ３０の駆動指令となる。よって、アクチュエータ（モータ）への指令が急峻に変化するのを防止できる。

図７（ｃ）に示すように、コントローラ６０の開閉制御部６４は、位置指令の動作停止タイミングを検知した後の一定時間Ｔ２が経過した後のｔ２１のタイミングで開閉部６３を開く。このように、コントローラ６０は、図７（ｃ）に示すように、クロスするタイミングから一定時間Ｔ２後のｔ２１のタイミングで開閉部６３を開いて振動抑制制御を無効化する。その結果、図７（ｄ）に示すように、ｔ２０～ｔ２１の期間における加速度センサ５０の出力値が振動抑制制御に反映される。

このようにして、コントローラ６０は、加速度センサ５０の出力値を元に算出した検出振動が、ゼロレベルに重力によるオフセット分ＯＳｇを加算した基準レベルＬｒｅｆとクロスするタイミングで振動抑制制御を有効化する。よって、加速度センサ５０において重力を加味して振動を適切に抑制することができる。

・加速度センサ５０は、エンドエフェクタ２３に設けたが、加速度センサ５０は、アーム２０に設けられていてもよい。要は、加速度センサ５０は、アーム２０またはエンドエフェクタ２３のいずれかに設けられていればよい。

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

Claims (5)

1. リンクが関節を介して繋がり、基端がベース部材に固定され、先端に設けたエンドエフェクタが前記関節の動作により目標の位置に移動可能なアームと、
   前記アームの関節を動作させて前記エンドエフェクタを目標の位置に移動させるためのアクチュエータと、
   前記アクチュエータの動作を検出する位置センサと、
   前記アームまたは前記エンドエフェクタのいずれかに設けられた加速度センサと、
   位置指令に基づいて前記アクチュエータを駆動して前記位置センサの出力値をフィードバックしつつ前記エンドエフェクタを目標の位置に移動させるフィードバック制御を行うとともに、前記加速度センサの出力値に基づいて振動抑制制御を行う制御部と、
   を備えるマニピュレータであって、
   前記制御部は、位置指令の動作停止タイミングを検知した後の前記加速度センサの出力値が基準レベルとクロスするタイミングで前記振動抑制制御を有効化する
   ことを特徴とするマニピュレータ。
2. 前記制御部は、重力によるオフセット分を差し引いた前記加速度センサの出力値を元に算出した検出振動が、前記基準レベルとしてのゼロレベルとゼロクロスするタイミングで前記振動抑制制御を有効化する
   ことを特徴とする請求項１に記載のマニピュレータ。
3. 前記制御部は、前記加速度センサの出力値を元に算出した検出振動が、前記基準レベルとしてのゼロレベルとゼロクロスするタイミングで前記振動抑制制御を有効化する
   ことを特徴とする請求項１に記載のマニピュレータ。
4. 前記制御部は、前記加速度センサの出力値を元に算出した検出振動が、ゼロレベルに重力によるオフセット分を加算した前記基準レベルとクロスするタイミングで前記振動抑制制御を有効化する
   ことを特徴とする請求項１に記載のマニピュレータ。
5. 前記制御部は、前記クロスするタイミングから一定時間後に前記振動抑制制御を無効化する
   ことを特徴とする請求項１～４のいずれか１項に記載のマニピュレータ。