JP5906825B2

JP5906825B2 - ロボット

Info

Publication number: JP5906825B2
Application number: JP2012051320A
Authority: JP
Inventors: 知洋笠川; 正樹元▲吉▼
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2012-03-08
Filing date: 2012-03-08
Publication date: 2016-04-20
Anticipated expiration: 2032-03-08
Also published as: JP2013184253A

Description

本発明は、ロボットに関する。

従来から、アームを回動等させることによってアームの先端に取り付けられた端末装置を所望の位置に移動させて、当該位置において端末装置を稼働させるロボット等の装置が知られている。例えば、把持端末を備え加工装置に対して被加工部材を給除材する給除材装置や、塗装端末を備えた塗装ロボットや、溶接端末を備えた溶接ロボット等が知られている。

ロボットを駆動させる際には、ロボットのアームを駆動するモーター等の駆動源の回転角度を測定し、測定した角度情報に基づいてアームの先端位置等を制御する制御方法が用いられている。しかし、駆動源からアームに駆動力を伝達する伝達機構やアームは剛体ではない。このことに起因して伝達機構やアームが変形するので、角度情報に基づいて位置制御されたアームの先端側の位置が必ずしも目標とする位置と一致しない場合があった。また、動作時に伝達機構やアームが変形することによって、振動を生ずるという問題があった。

これらの問題に対し、アームの先端に慣性センサーを取り付けて先端の動きを測定し、得られた慣性センサーによる角速度情報を制御に用いる方法が提案されている。特許文献１には、慣性センサーの出力信号によってアームの動作を制御する多関節ロボットの制御方法及び多関節ロボットが開示されている。慣性センサーを用いることによって、剛性が低い場合でも高精度の位置決めができると共に、振れによる精度低下を防止することができる。

しかし、慣性センサーの出力からアームの回動角度を求めるためには、慣性センサーの出力を積分する必要がある。積分を繰り返すと慣性センサーの基準電位のドリフトの影響を受けやすくなり、制御装置が情報を誤認識する可能性が高くなるという課題があった。

特許文献２には、制御時に制御切替決定部で動作状況に応じて慣性力情報の重み付け乗数を決定するロボットの制御装置及びロボットの制御方法が開示されている。慣性力情報の重み付け乗数を決定することにより、ノイズの影響や基準電位のドリフト等の誤差要因を排除している。しかし、重み付け乗数を切り替えた際に駆動源への入力の急激な変化が発生し振動を生じたり、駆動源や減速機への機械的負荷を与えるたりすることがある。これにより機械寿命に影響を与える可能性が高くなるという課題があった。

重み付け乗数を変更しない方法として、慣性センサーにデジタルフィルターによるフィルター処理を行う方法が考えられる。特許文献３には、デジタルフィルターによるフィルター処理を行い慣性センサーのノイズ成分を除去するジャイロ装置が開示されている。これによると、デジタルフィルターは低域通過フィルターとしての特性を有する。車両が停止している期間には、デジタルフィルターの遮断周波数は低く設定され、車両が走行している期間には、遮断周波数は高く設定される。これにより、停止期間にはノイズが十分に低減され、走行期間には高速応答性が確保された。

特開平７−９３７４号公報特開２０１０−２８４７７０号公報特開平６−１９４１７０号公報

アームの動作には、移動（加速、等速移動、減速、停止）、整定、待機のステップがある。特許文献３では移動と待機に対しては有効であるが、整定時に対しては有効な方法とはいい難い方法であった。そこで、アームの整定時にアームを品質良く制振させるロボットが求められていた。

本発明は、上記課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。

［適用例１］
本適用例にかかるロボットは、一端を回動可能に支持されたアームと、前記アームを回動させる駆動源と、前記駆動源の回動角度を検出し前記駆動源の回動角度情報を出力する角度センサーと、前記アームに取り付けられ前記アームが回動する角速度を検出して前記アームの角速度情報を出力する慣性センサーと、前記駆動源を制御する制御部と、を備え、前記制御部は、前記アームの回動動作を指示する制御指令を出力する制御指令発生部と、前記角速度情報の高周波成分を除去するフィルター部と、前記高周波成分の遮断周波数を制御するフィルター制御部と、を備えることを特徴とする。

本適用例にかかるロボットによれば、制御指令発生部がアームの回動動作を指示し、駆動源がアームを回動させる。角度センサーが駆動源の回転角度を検出するので、アームの回転角度を検出することができる。そして、慣性センサーがアームの角速度を検出する。アームは移動に伴い振動するので慣性センサーはアームの振動を含んだ角速度を検出する。

制御部は、制御指令、回動角度情報、フィルター処理を施した角速度情報を用いてアームの動作を制御する。このとき、制御部は回動角度情報と角速度情報とを用いてアームが移動中か整定中か待機中かを判断する。尚、整定は振動が減衰して静止する状態を示す。そして、各状況に応じた制御を行う。

フィルター制御部がアームの動作状況に応じてフィルター部のフィルター特性を制御する。フィルター部が、角速度情報に対して高周波成分を除去するフィルター処理を施す。これにより、角速度情報では除去する高周波成分の周波数範囲が制御される。フィルター制御部は角速度情報を用いることによる効果及び角速度情報の誤差が及ぼす影響を総合的に考慮してフィルター特性を決定する。これにより、角速度情報の誤差が及ぼす影響が小さくなる。従って、制御部は適切な周波数成分を含んだ角速度情報を用いてロボットの制御を実施することができる。その結果、ロボットはアームの整定時にアームを制振させることができる。

［適用例２］
上記適用例にかかるロボットにおいて、前記フィルター部は低域フィルターであり、前記遮断周波数を可変制御されることが好ましい。

本適用例によれば、フィルター部は低域通過フィルターとしてのフィルター特性を有している。そして、遮断周波数が可変制御される。角速度情報の高速応答性が求められる場合にはアーム動作周波数より遮断周波数を高く設定する。これにより、アームの振動を検出し易くすることができる。

角速度情報の誤差、ノイズの影響を小さくしたい場合にはアーム動作周波数より遮断周波数を高くする。これにより、角速度情報に最適なフィルター処理を施すことができる。従って、状況に応じて角速度情報の誤差と高周波成分の影響とを調整することができる。

［適用例３］
上記適用例にかかるロボットにおいて、前記フィルター制御部は、前記制御指令によって、前記アームの動作が移動、整定、待機の状態を判断し、前記アームの動作が移動のときには前記フィルター部の遮断周波数を前記アームの固有振動数より高く設定し、前記アームの動作が待機のときには前記フィルター部の遮断周波数を前記アームの固有振動数より低く設定し、前記アームの動作が整定のときには前記角速度情報によって前記遮断周波数を制御することが好ましい。

本適用例によれば、フィルター制御部は制御指令によって、移動、整定、待機の状態を判断する。そして、フィルター制御部は各状態に応じて遮断周波数を制御する。フィルター部はその判断に従って、角速度情報にフィルター処理を施す。フィルター制御部が整定と判断した場合、角速度情報からアームの整定状態を検知し、アームの整定状態に応じて遮断周波数を制御する。アームの振動が大きい場合は、遮断周波数を高く設定することで、高速応答性を高め、振動が収束した場合は、遮断周波数を低く設定し、角速度情報の誤差、ノイズの影響を小さくする。これにより、フィルター部はアームの整定状態に応じて有用な角速度情報を出力することができる。

アームの動作が移動のとき、アームの移動中の挙動に対する応答性を高めるために遮断周波数を高く設定し、遮断周波数を細かく制御する必要はない。これにより、フィルター制御部が整定と判断した場合と比較して、制御の負荷を軽くすることができる。フィルター制御部が待機と判断した場合、角速度情報の誤差、ノイズの影響を小さくするために遮断周波数を低く設定し、遮断周波数を細かく制御する必要はない。これにより、フィルター制御部が整定と判断した場合と比較して、制御の負荷を軽くすることができる。これにより、アームの動作状態に応じて、角速度情報に最適なフィルター処理を施すことができる。

［適用例４］
上記適用例にかかるロボットにおいて、前記角速度情報を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部が前記角速度情報を前記判定値と比較して前記遮断周波数を制御することが好ましい。

本適用例によれば、角速度情報を判定値と比較して、角速度情報に対する遮断周波数が制御される。このため、角速度情報が判定値を上回るとき遮断周波数が高く設定される。これによって、応答性が高い角速度情報が得られる。角速度情報が判定値を下回るとき遮断周波数が低く設定されることになる。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。角速度情報を判定値と比較して、角速度情報に対してフィルター処理を施すことにより、角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響が小さくなり、制振効果が最大限得られるロボットの制御を実施することができる。

［適用例５］
上記適用例にかかるロボットにおいて、前記回動角度情報を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部が前記回動角度情報を前記判定値と比較して、前記遮断周波数を制御することが好ましい。

本適用例によれば、回動角度情報を判定値と比較して、角速度情報に対する遮断周波数が制御される。このため、回動角度が判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。回動角度情報を判定値と比較して、角速度情報にフィルター処理を施す。回動角度情報が判定値を上回ると、応答性が高い角速度情報が得られる。回動角度情報が判定値を下回ると角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。

［適用例６］
上記適用例にかかるロボットにおいて、前記角速度情報の積分値を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部は、前記角速度情報の一回以上の積分値を前記判定値と比較して、前記遮断周波数を制御することが好ましい。

本適用例によれば、角速度情報の一回以上の積分値を判定値と比較して、角速度情報に対する遮断周波数が制御されるため、判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。

角速度情報の一回以上の積分値を判定値と比較して、角速度情報にフィルター処理を施す。判定値を上回るとき応答性が高い角速度情報が得られる。判定値を下回るとき角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。角速度情報の一回以上の積分値を判定値と比較することは、角速度情報を判定値と比較する場合と同様の判断基準とすることができる。角速度情報の一回以上の積分値を扱うことで、回動角度情報等と単位を共通にして扱いやすくすることができる。

［適用例７］
上記適用例にかかるロボットにおいて、前記回動角度情報の微分値を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部は前記回動角度情報の一回以上の微分値を前記判定値と比較して、前記遮断周波数を制御することが好ましい。

本適用例によれば、回動角度情報の一回以上の微分値を判定値と比較して、角速度情報に対する遮断周波数が制御されるため、判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。

回動角度情報の一回以上の微分値を判定値と比較して、角速度情報にフィルター処理を施すことにより、判定値を上回ると、応答性が高い角速度情報が得られ、判定値を下回ると角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。回動角度情報の一回以上の微分値を判定値と比較することは、回動角度情報を判定値と比較する場合と同様の判断基準とすることができる。回動角度情報の一回以上の微分値を扱うことで、角速度情報等と単位を共通にして扱いやすくすることができる。

［適用例８］
上記適用例にかかるロボットにおいて、前記制御指令が示す角速度と前記角速度情報との差分を判断する判定値を予め設定し、前記アームの動作が待機のときには前記フィルター制御部は前記制御指令が示す角速度と前記角速度情報との差分を前記判定値と比較して、前記フィルター制御部は角速度情報の値を使用するかしないかを決定することが好ましい。

本適用例によれば、制御指令が示す角速度と角速度情報との差分を判定値と比較して、角速度情報を用いるかが決定される。制御指令が示す角速度と角速度情報との差分が判定値を上回るとき角速度情報は用いない。角速度情報を用いた制御は、制御指令が示す角速度と角速度情報との差分が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、動作が不安定になる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。

第１の実施形態にかかわる給除材装置の構成を示す概略斜視図。ロボット機構を駆動する機能の構成を示すブロック図。給除材アームを回動させる工程を示すフローチャート。制御を切替える基準とする角速度の判定値を説明するための図。第２の実施形態にかかわるロボットシステムの模式正面図。

以下、搬送装置としてのロボット及び慣性センサーを用いたロボットの制御方法の一実施形態について、図面を参照して説明する。尚、以下の説明において参照する図面では、構成部材をわかり易く表示するために、部材または部分の縦横の縮尺や部分ごとの縮尺を実際のものとは異なるように表す場合がある。

（第１の実施形態）
搬送装置としてのロボット及び慣性センサーを用いた制御方法の一実施形態について、説明する。本実施形態は、搬送装置の一例である給除材装置を例にして説明する。本実施形態の給除材装置は、例えば、半導体装置の製造工程において半導体装置を構成する複数の半導体チップが区画形成されたウェハーを取扱う給除材装置である。

＜給除材装置＞
最初に、給除材装置１０の機械的構成について、図１を参照して説明する。図１は、給除材装置の構成を示す概略斜視図である。図１に示すように、給除材装置１０は、保持ハンド１２と、ロボット機構２０と、給除材装置制御部３０と、角速度センサー３２と、角度センサー３４（図２参照）と、を備えている。

ロボット機構２０は、ハンド保持機構２４と、給除材アーム２１と、アーム軸部２６と、機台２８と、を備えている。機台２８は、内蔵する軸受機構（図示省略）を介して、アーム軸部２６をアーム軸部２６の回動軸回りに回動自在に、且つ精密に位置決め固定可能に支持している。アーム軸部２６は、機台２８に内蔵されたアーム駆動モーター２２（図２参照）と、アーム駆動機構２３を介して接続されており、アーム駆動モーター２２によって回動させられる。アーム駆動モーター２２には角度センサー３４が接続されており、角度センサー３４によってアーム駆動モーター２２の回動角度が測定される。

アーム軸部２６の機台２８に支持された側と反対側の端には、給除材アーム２１の一端が固定されている。給除材アーム２１は、アーム駆動モーター２２によって、アーム軸部２６の回動軸を中心に回動させられる。給除材アーム２１の回動角度は、角度センサー３４によってアーム駆動モーター２２の回動角度を測定することによって近似的に測定される。

給除材アーム２１においてアーム軸部２６の反対側の端には、ハンド保持機構２４が固定されている。ハンド保持機構２４は、給除材アーム２１に固定された保持軸受２４ａと、保持軸受２４ａに支持された保持機構軸２４ｂとを備えている。保持機構軸２４ｂは、図示省略した上下駆動源によって、保持軸受２４ａに対して保持機構軸２４ｂの軸方向に摺動可能となっている。保持機構軸２４ｂの軸方向は、アーム軸部２６の軸方向と略平行である。そして、保持機構軸２４ｂは軸方向に精密に位置決めして固定することが可能となっている。

保持機構軸２４ｂの図中下側の端には保持ハンド１２が取り付けられている。給除材アーム２１を回動させることによって、保持ハンド１２を搬送対象物に臨む位置に位置させることができる。保持軸受２４ａに対して保持機構軸２４ｂを摺動させることによって、保持ハンド１２を搬送対象物に離接させると共に、保持ハンド１２によって保持した搬送対象物を載置場所から持ち上げたり、載置場所に接近させたりすることが可能となっている。

保持ハンド１２が取り付けられているハンド保持機構２４には、保持ハンド１２と反対側に角速度センサー３２が固定されている。すなわち、角速度センサー３２は給除材アーム２１の先端に固定されており、給除材アーム２１が回動させられる角速度を測定可能である。

給除材装置制御部３０は、情報入出力装置（図示省略）を介して予め入力された制御プログラムに基づいて、給除材装置１０の各部の動作を統括制御する。例えば、角度センサー３４及び角速度センサー３２が出力する信号を入力し、アーム駆動モーター２２を駆動する。

＜ロボット機構駆動の機能的構成＞
次に、ロボット機構２０を駆動させる機能の構成について、図２を参照して説明する。図２は、ロボット機構を駆動する機能の構成を示すブロック図である。上述したように、給除材アーム２１を回動させるために、給除材装置１０は、アーム駆動モーター２２と、アーム駆動機構２３と、角速度センサー３２と、角度センサー３４と、給除材装置制御部３０とを備えている。

角速度センサー３２には、例えば、ジャイロセンサーを用いることができる。角度センサー３４には、例えば、エンコーダーを用いることができる。給除材アーム２１がアームに相当し、ロボット機構２０がロボットに相当し、アーム駆動モーター２２が駆動源に相当し、角速度センサー３２が慣性センサーに相当する。

給除材装置制御部３０は、アーム駆動モーター２２を制御するための制御部としてのロボット制御部３１を備えている。ロボット制御部３１は、制御指令発生部３６と、フィルター制御部３７と、フィルター部３５と、アーム動作制御部３８と、モータードライバー３９とを備えている。

制御指令発生部３６は、給材または除材の稼動指令に基づくロボット機構２０の動作指令を演算する。そして、給除材アーム２１の動作指令をフィルター制御部３７及びアーム動作制御部３８に出力する。稼動指令は、操作者により図示省略した入力装置から給除材装置１０に入力される。稼動指令に基づくロボット機構２０の動作指令は、給除材装置制御部３０が備える統括制御部（図示省略）から制御指令発生部３６に出力される。制御指令発生部３６が出力する給除材アーム２１の動作指令は、例えば、給除材アーム２１の先端の軌道が稼動指令となるように、給除材アーム２１が移動する角度の推移として指令される。

アーム動作制御部３８はアーム駆動モーター２２を制御する信号をモータードライバー３９に出力する。これにより、制御指令発生部３６が出力する給除材アーム２１の動作指令が実行される。アーム動作制御部３８は、角度情報及び角速度情報使用制御部３８ａを有している。角度情報及び角速度情報使用制御部３８ａは、角度センサー３４からの角度情報、及び角速度センサー３２からの角速度情報を入力する。そして、角度情報及び角速度情報使用制御部３８ａは給除材アーム２１の動作指令を実行するのに最適なアーム駆動モーター２２の制御信号を生成して出力する。

フィルター部３５は角速度センサー３２から角速度情報を入力する。そして、フィルター部３５は角速度情報に対して高周波成分を除去するローパスフィルターを適用するフィルター処理を施して角度情報及び角速度情報使用制御部３８ａに出力する。そして、角度情報及び角速度情報使用制御部３８ａはアーム駆動モーター２２の制御信号を生成する際に角度情報及びフィルター処理された角速度情報を用いる。

フィルター部３５が用いるフィルター特性の制御はフィルター制御部３７によって行われる。フィルター特性は遮断周波数に相当する。アーム動作制御部３８またはアーム動作制御部３８が有する角度情報及び角速度情報使用制御部３８ａがアーム動作制御部に相当する。

フィルター制御部３７は、角度センサー３４からの角度情報、角速度センサー３２からの角速度情報、制御指令発生部３６からの給除材アーム２１の動作指令等を入力する。そして、フィルター特性を決定してフィルター部３５に出力する。具体的にはフィルター制御部３７はフィルター部３５の遮断周波数を制御する。

＜給除材アームの回動制御手順＞
次に、アーム駆動モーター２２の駆動を制御することによって給除材アーム２１を回動させて、給除材アーム２１の先端に配設された保持ハンド１２を所定の位置に位置させる工程について、図３及び図４を参照して説明する。図３は、給除材アームを回動させる工程を示すフローチャートである。図４は、制御を切替える基準とする角速度の判定値を説明するための図である。図４の上段は、給除材アームを回動させる間の時間経過と角速度の関係及び角速度の判定値の例を示す図である。図４の下段は、給除材アームが停止する間際の時間経過と角速度の関係及び角速度の判定値の例を示す図である。

図３において最初のステップＳ２１は、「モーター停止指令あり」の有無を判定するステップである。モーター停止指令は、アーム駆動モーター２２を停止させ、制御を終了する指令である。モーター停止指令があった場合（ステップＳ２１でＹＥＳ）には、アーム駆動モーター２２の駆動を制御することによって給除材アーム２１を回動させて給除材アーム２１の先端に配設された保持ハンド１２を所定の位置に位置させる工程を終了する。モーター停止指令がない場合（ステップＳ２１でＮＯ）には、ステップＳ２２に進む。

次に、ステップＳ２２は、「制御指令値、角度情報、角速度情報を取得」するステップである。詳細には、制御指令発生部３６が出力する制御指令値がアーム動作制御部３８及びフィルター制御部３７に入力される。そして、角度センサー３４はアーム駆動モーター２２の回動角度を検出し、給除材アーム２１の回動角度の角度情報を算出する。次に角度センサー３４は給除材アーム２１の回動角度の角度情報をフィルター制御部３７及びアーム動作制御部３８に出力する。

角速度センサー３２は給除材アーム２１の先端近くにおける給除材アーム２１の角速度を検出する。そして、角速度センサー３２は給除材アーム２１の角速度の角速度情報をフィルター制御部３７と角度情報及び角速度情報使用制御部３８ａとに出力する。

次に、ステップＳ２３は、「アーム動作パラメーター、移動、整定、待機」のステップである。このステップでは、取得した制御指令値からアームの動作状態を決定する。詳細には、制御指令発生部３６は制御指令値に動作パラメーターを入れてから出力する。フィルター制御部３７は動作パラメーターから、給除材アーム２１の状態が移動、整定、待機のどの状態かを判断する。動作パラメーターを動作パラメーターＰとする。

動作パラメーターＰが移動の場合には、ステップＳ２４に進む。ステップＳ２４は、「高い遮断周波数を設定」するステップである。このステップでは、フィルター制御部３７がフィルター部３５の遮断周波数を高く設定する。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含む情報となる。次に、ステップＳ２９に進む。

動作パラメーターＰが待機の場合には、ステップＳ２５に進む。ステップＳ２５は、「低い遮断周波数を設定」するステップである。このステップではフィルター制御部３７が、フィルター部３５の遮断周波数を低く設定する。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含まない情報となる。次に、ステップＳ２９に進む。

動作パラメーターＰが整定の場合には、ステップＳ２６に進む。ステップＳ２６は、「角速度情報判定値超え」を判定するステップである。このステップでは、フィルター制御部３７が、角速度センサー３２から出力された角速度情報から給除材アーム２１の角速度が所定の判定値を超えたか否かを判定する。所定の判定値を判定値Ｓと表記する。そして、判定値Ｓの角速度の反対方向で同じ値の判定値を判定値−Ｓと表記する。

ステップＳ２６を行う前に予め判定値Ｓが設定されている。判定値Ｓは予め実験を行い適切な値が設定されている。給除材アーム２１の角速度の絶対値が所定の判定値Ｓを超えた（ステップＳ２６でＹＥＳ）場合にはステップＳ２７に進む。ステップＳ２７は、「高い遮断周波数を設定」するステップである。このステップではフィルター制御部３７がフィルター部３５の遮断周波数を高く設定する。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含む情報となる。次に、ステップＳ２９に進む。

ステップＳ２６において給除材アーム２１の角速度の絶対値が所定の判定値Ｓより小さい（ステップＳ２６でＮＯ）場合には、ステップＳ２８に進む。ステップＳ２８は、「低い遮断周波数を設定」するステップである。このステップではフィルター制御部３７がフィルター部３５の遮断周波数を低く設定する。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含まない情報となる。次に、ステップＳ２９に進む。

図４において横軸は時間の経過を示している。上段の縦軸は角速度を示し、角速度推移線４２は角速度センサー３２が検出する角速度の推移を示している。下段の縦軸は遮断周波数を示し、遮断周波数推移線４３はフィルター制御部３７がフィルター部３５に出力する遮断周波数の推移を示している。アーム動作周波数指示線４４はアーム動作周波数を示す線である。アーム動作周波数は給除材アーム２１の固有振動数であり、給除材アーム２１が振動し易い振動数を示している。

給除材アーム２１の動作は移動、整定、待機の３つの状態の繰り返しである。Ｐ３に示す待機の区間では遮断周波数推移線４３はアーム動作周波数指示線４４より低い第１周波数４３ａに設定されている。そして、給除材アーム２１が回動を始めると、Ｐ１に示す区間ではアーム動作パラメーターが移動に変更される。そして、角速度推移線４２が上昇する。そして、遮断周波数推移線４３が示すように遮断周波数はアーム動作周波数指示線４４より高い第２周波数４３ｂに設定される。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含む情報となる。

指令値が設定した目的位置に達するとＰ２に示す区間に移行する。そして、アーム動作パラメーターは整定に変わり、角速度が減衰する。そして、角速度が判定値Ｓ〜判定値−Ｓ以内に収まるとき、遮断周波数推移線４３に示すように遮断周波数はアーム動作周波数指示線４４より低い第１周波数４３ａに設定される。これによりフィルター処理された角速度情報は高い周波数の振動の角速度を含まない情報となる。予め設定された整定時間を過ぎるとＰ３に示す区間に移行する。そして、アーム動作パラメーターは待機に変更され、遮断周波数はアーム動作周波数指示線４４より低い第１周波数４３ａに設定される。

図３に戻って、ステップＳ２４、ステップＳ２５、ステップＳ２７またはステップＳ２８の次に、ステップＳ２９に移行する。ステップＳ２９は、「制御指令値、角度情報、フィルター処理を施した角速度情報からトルク指令を値演算」するステップである。このステップでは角度情報及び角速度情報使用制御部３８ａが、制御指令値、角度情報、を用いて角速度情報にフィルター処理を施す。そして、フィルター処理した角速度情報及び角度情報を用いてアーム動作制御部３８がトルク指令を演算する。次に、ステップＳ３０に進む。

次に、ステップＳ３０は、「モータードライバーにトルク指令値を入力」するステップである。このステップではアーム動作制御部３８によって演算されて求められたトルク指令値を、モータードライバー３９に入力する。次に、ステップＳ３１に進む。

ステップＳ３１は、「アーム駆動モーターのトルクを発生」するステップである。このステップではモータードライバー３９によってトルク指令値に対応する電力がアーム駆動モーター２２に供給される。アーム駆動モーター２２では供給された電力に対応するトルクが発生する。次に、ステップＳ３２に進む。

ステップＳ３２は、「アーム機構部作動」のステップである。このステップではアーム駆動モーター２２が発生したトルクによってアーム駆動モーター２２に接続されたアーム駆動機構２３が作動する。そして、アーム駆動モーター２２が発生したトルクによって給除材アーム２１の角速度が加速または減速される。

ステップＳ３２の次に、ステップＳ２１に戻り、ステップＳ２１においてモーター停止指令があった場合（ステップＳ２１でＹＥＳ）に、アーム駆動モーター２２の駆動を制御することによって給除材アーム２１を回動させて給除材アーム２１の先端に配設された保持ハンド１２を所定の位置に位置させる工程を終了する。

（第２の実施形態）
次に、ロボットの一実施形態について図５のロボットシステムの模式正面図を用いて説明する。本実施形態が第１の実施形態と異なるところは、ロボットが双腕ロボットである点にある。尚、第１の実施形態と同じ点については説明を省略する。

すなわち、本実施形態では図５に示すように、ロボットシステム５０は、ロボット本体５１と、撮影装置５２と、把持部５３と、ロボット制御装置５４とを含んで構成される。ロボット本体５１は、具体的には、地面に対して可動に設置された本体５１ａと、旋回可能に本体５１ａに連結された首部５１ｂと、首部５１ｂに対して固定された頭部５１ｃと、を備えている。さらに、ロボット本体５１は旋回可能及び屈伸可能に頭部５１ｃに連結された第１アーム部５１ｄと、旋回可能及び屈伸可能に頭部５１ｃに連結された第２アーム部５１ｅと、を備えている。さらに、ロボット本体５１はロボット本体５１の設置面に対してロボット本体５１を移動可能に本体５１ａに取り付けられた搬送部５１ｆとを含んで構成される。

第１アーム部５１ｄの開放端であるハンドには、把持部５３が取り付けられている。また、第２アーム部５１ｅの開放端であるハンドには、撮影装置５２が取り付けられている。第１アーム部５１ｄ及び第２アーム部５１ｅは頭部５１ｃと接続する肩関節部５５を備えている。肩関節部５５と接続して上腕アーム５６が設置され、上腕アーム５６と接続して肘関節５７が設置されている。さらに、肘関節５７と接続して下腕アーム５８が設置されている。

そして、第１アーム部５１ｄでは下腕アーム５８と接続して把持部５３が設置されている。第２アーム部５１ｅでは下腕アーム５８と接続して撮影装置５２が設置されている。肩関節部５５及び肘関節５７にはアーム駆動モーター２２、アーム駆動機構２３、角度センサー３４が設置されている。そして、上腕アーム５６の肘関節５７側には角速度センサー３２が設置されている。さらに、第１アーム部５１ｄでは下腕アーム５８の把持部５３側に角速度センサー３２が設置されている。第２アーム部５１ｅでは下腕アーム５８の撮影装置５２側に角速度センサー３２が設置されている。

搬送部５１ｆは、ロボット本体５１の設置面に対して、ロボット本体５１を一定方向または方向自在に移動可能に支持する。搬送部５１ｆは、四組の車輪、四組のキャスター、一対の無限軌道等により実現される。

ロボット本体５１は、例えば、２系統のアームを備えた垂直多関節ロボット（双腕ロボット）である。ロボット本体５１は、ロボット制御装置５４から供給されるロボット制御命令を取り込み、このロボット制御命令による駆動制御によって、撮影装置５２及び把持部５３それぞれの位置及び姿勢を三次元空間内で所望に変更し、また把持部５３の爪部を開閉させる。

撮影装置５２は、被写体を撮影して静止画像または動画像である撮影画像を取得し、この撮影画像をロボット制御装置５４に供給する。撮影装置５２は、例えば、デジタルカメラ装置、デジタルビデオカメラ装置により実現される。把持部５３は、物体を把持または挟持可能な爪部を備える。尚、図中では把持部５３は機能を示すために模式的に示してある。

ロボット制御装置５４は、ロボット本体５１の首部５１ｂと頭部５１ｃと第２アーム部５１ｅとの動作を制御し、撮影装置５２の位置及び姿勢を変更させる。ロボット制御装置５４の内部にはロボット制御部３１が設置されている。

ロボット制御部３１は第１の実施形態と同様に、角度センサー３４が出力する角度情報と角速度センサー３２が出力する角速度情報を入力する。そして、フィルター制御部３７がフィルター部３５の遮断周波数を制御し、フィルター部３５が角速度情報のフィルター処理を行なっている。従って、ロボットシステム５０は第１アーム部５１ｄまたは第２アーム部５１ｅの整定時に第１アーム部５１ｄまたは第２アーム部５１ｅを短時間に制振させることができる。

尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記第１の実施形態では、角速度センサー３２が出力する角速度を判定値Ｓ及び判定値−Ｓと比較してフィルター制御部３７が遮断周波数を制御した。フィルター制御部３７は角度センサー３４が出力する回動角度情報を用いて遮断周波数を制御しても良い。回動角度情報を判断する判定値を予め設定する。回動角度が判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。回動角度情報を判定値と比較して、角速度情報にフィルター処理を施す。回動角度情報が判定値を上回ると、応答性が高い角速度情報が得られる。回動角度情報が判定値を下回ると角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。

（変形例２）
前記第１の実施形態では、角速度センサー３２が出力する角速度を判定値Ｓ及び判定値−Ｓと比較してフィルター制御部３７が遮断周波数を制御した。フィルター制御部３７は角速度センサー３２が出力する角速度情報の積分値を用いて遮断周波数を制御しても良い。角速度情報の積分値を判断する判定値を予め設定する。給除材アーム２１の動作が整定のときにはフィルター制御部３７は、角速度情報の一回以上の積分値を判定値と比較して、フィルター部３５の遮断周波数を制御する。

角速度情報の一回以上の積分値が、判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。

（変形例３）
前記第１の実施形態では、角速度センサー３２が出力する角速度を判定値Ｓ及び判定値−Ｓと比較してフィルター制御部３７が遮断周波数を制御した。フィルター制御部３７は角度センサー３４が出力する回動角度情報の微分値を用いて遮断周波数を制御しても良い。回動角度情報の微分値を判断する判定値を予め設定する。そして、給除材アーム２１の動作が整定のときにはフィルター制御部３７は回動角度情報の一回以上の微分値を判定値と比較して、フィルター部３５の遮断周波数を制御する。

回動角度情報の一回以上の微分値を判定値と比較して、角速度情報に対する遮断周波数が決定される。このため、判定値を上回ると遮断周波数が高く設定され、下回ると遮断周波数が低く設定される。角速度情報を用いた制御は、角速度情報が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、精度が落ちる。

回動角度情報の一回以上の微分値を判定値と比較して、角速度情報にフィルター処理を施す。回動角度情報の一回以上の微分値が判定値を上回ると、応答性が高い角速度情報が得られ、判定値を下回ると角度情報の誤差、ノイズの影響を小さくできる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。回動角度情報の一回以上の微分値を判定値と比較することは、回動角度情報を判定値と比較する場合と同様の判断基準とすることができる。回動角度情報の一回以上の微分値を扱うことで、角速度情報等と単位を共通にして扱いやすくすることができる。

（変形例４）
前記第１の実施形態では、角速度センサー３２が出力する角速度を判定値Ｓ及び判定値−Ｓと比較してフィルター制御部３７が遮断周波数を制御した。フィルター制御部３７は制御指令が示す角速度と角速度情報との差分を用いて遮断周波数を制御しても良い。制御指令発生部３６が示す角速度と角速度情報との差分を判断する判定値を予め設定する。そして、給除材アーム２１の動作が待機のときにはフィルター制御部３７は制御指令発生部３６が示す角速度と角速度情報との差分を判定値と比較する。次に、フィルター制御部３７は角速度情報の値を使用するかしないかを決定する。

制御指令発生部３６が示す角速度と角速度情報との差分を判定値と比較して、角速度情報を用いるかを決定する。角速度と角速度情報との差分が判定値を上回るとき角速度情報は用いない。角速度情報を用いた制御は、制御指令が示す角速度と角速度情報との差分が小さくなると、角速度情報を用いる効果が減少すると共に、ノイズ等による角速度情報の誤差の影響を受け、動作が不安定になる。従って、角速度情報の誤差が及ぼす影響を小さくすることができる。

２０…ロボットとしてのロボット機構、２２…駆動源としてのアーム駆動モーター、３１…制御部としてのロボット制御部、３２…慣性センサーとしての角速度センサー、３４…角度センサー、３５…フィルター部、３６…制御指令発生部、３７…フィルター制御部。

Claims

一端を回動可能に支持されたアームと、
前記アームを回動させる駆動源と、
前記駆動源の回動角度を検出し前記駆動源の回動角度情報を出力する角度センサーと、
前記アームに取り付けられ前記アームが回動する角速度を検出して前記アームの角速度情報を出力する慣性センサーと、
前記駆動源を制御する制御部と、を備え、
前記制御部は、前記アームの回動動作を指示する制御指令を出力する制御指令発生部と、
前記角速度情報の高周波成分を除去するフィルター部と、
前記高周波成分の遮断周波数を制御するフィルター制御部と、を備え、
前記フィルター制御部は、前記制御指令によって、前記アームの動作が移動、整定、待機の状態を判断し、前記アームの動作が移動のときには前記フィルター部の遮断周波数を前記アームの固有振動数より高く設定し、前記アームの動作が待機のときには前記フィルター部の遮断周波数を前記アームの固有振動数より低く設定し、前記アームの動作が整定のときには前記角速度情報によって前記遮断周波数を制御することを特徴とするロボット。
前記フィルター部は低域フィルターであり、前記遮断周波数を可変制御されることを特徴とする請求項１に記載のロボット。
前記角速度情報を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部が前記角速度情報を前記判定値と比較して前記遮断周波数を制御することを特徴とする請求項１または２に記載のロボット。
前記回動角度情報を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部が前記回動角度情報を前記判定値と比較して、前記遮断周波数を制御することを特徴とする請求項１または２に記載のロボット。
前記角速度情報の積分値を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部は、前記角速度情報の一回以上の積分値を前記判定値と比較して、前記遮断周波数を制御することを特徴とする請求項１または２に記載のロボット。
前記回動角度情報の微分値を判断する判定値を予め設定し、前記アームの動作が整定のときには前記フィルター制御部は前記回動角度情報の一回以上の微分値を前記判定値と比較して、前記遮断周波数を制御することを特徴とする請求項１または２に記載のロボット。
前記制御指令が示す角速度と前記角速度情報との差分を判断する判定値を予め設定し、前記アームの動作が待機のときには前記フィルター制御部は前記制御指令が示す角速度と前記角速度情報との差分を前記判定値と比較して、前記フィルター制御部は角速度情報の値を使用するかしないかを決定することを特徴とする請求項１または２に記載のロボット。