(57)【要約】【課題】角速度および加速度センサーを用いて、アームの移動ベクトルに対応して角速度もしくは加速度センサーのどちらかの検出値を選択し、生成された制御信号によりアームの移動停止時に発生する振動を短時間に収束させ、アームの速い作動速度と高い停止位置精度を有するロボット装置とその制御方法を提供する。【解決手段】ロボット装置であって、アームに角速度センサーと加速度センサーとを備え、前記角速度センサーと前記加速度センサーとが配置された部位が、直線運動もしくは曲線運動であるかを判定する動作判定部と、前記動作判定部の結果に基づき前記角速度もしくは加速度センサーの検出値から前記アームの制御信号を生成する制御信号生成部と、前記制御信号に基づき基体連結装置とアーム連結装置とを制御する制御部とを備える。【選択図】図１
2011020188.tif 000002
【特許請求の範囲】
【請求項１】
アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、
複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた少なくとも１以上の角速度センサーおよび加速度センサーと、
前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定部と、
前記動作判定部の演算結果に基づき、前記角速度センサーの検出値または前記加速度センサーの検出値から、前記基体連結装置の前記アクチュエーターと前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成部と、
前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御部とを、
備えることを特徴とするロボット装置。
【請求項２】
請求項１に記載のロボット装置において、
前記アーム体にはワーク保持装置が含まれ、
前記ワーク保持装置に角速度センサーと加速度センサーとが備えられている、
ことを特徴とするロボット装置。
【請求項３】
アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、
複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体とを備えるロボット装置の制御方法であって、
少なくとも１以上の前記アームに角速度センサーおよび加速度センサーを備え、
前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定工程と、
前記角速度センサーと前記加速度センサーとが配置された前記アームが直線運動と判定された場合には、前記加速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成し、
または、前記角速度センサーと前記加速度センサーとが配置された前記アームが曲線運動と判定された場合には、前記角速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成工程と、
前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御工程とを、
有することを特徴とするロボット装置の制御方法。
【請求項４】
請求項３に記載のロボット装置の制御方法において、
前記動作判定工程は、前記アームの軌道生成情報に基づき直線運動もしくは曲線運動を判定する、
ことを特徴とするロボット装置の制御方法。
【請求項５】
請求項３に記載のロボット装置の制御方法において、
前記動作判定工程は、前記角速度センサーおよび前記加速度センサーの出力値に基づき直線運動もしくは曲線運動を判定する、
ことを特徴とするロボット装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボット装置およびロボット装置の制御方法に関する。
【背景技術】
【０００２】
ＩＣハンドラーや組立装置の一部として多く使われている多関節構造を有するロボット装置は、様々な産業現場の中で多用されてきている。故に、ロボット装置には今まで以上に、求められる位置にいかに早く且つ正確にアームを移動させることができるかが重要な性能仕様、品質になってきている。
【０００３】
一般的にアームを高速に且つ正確に移動させるには、アームに掛かる慣性力を小さくし、駆動用のアクチュエーターの負荷を大きくさせないことにある。アームに掛かる慣性力を小さくするには、アームの軽量化が最も効果的な手法として用いられている。しかし、アームを軽量化することによりアーム剛性の低下を招き、アーム停止時に生じるアームの振動を抑制することが困難になり、アーム先端部を目的の位置で停止させても、アーム自体の振動の振幅分の位置ズレが生じてしまい、振動が減衰する時間まで次の動作を開始することができないという問題があった。
【０００４】
この問題に対して、アーム先端に加速度センサーを設置し加速度信号を元にアームを作動させ振動を抑制する方法（例えば、特許文献１）、アーム先端およびアームに角速度センサーを設置し、角速度信号を元にアーム動作を制御する方法（例えば、特許文献２）、アーム先端に設置したチャック部近傍にジャイロ（角速度センサー）と加速度計からなる慣性センサーを設置し、該慣性センサーの信号に基づき駆動体を駆動する方法（例えば、特許文献３）、などが提示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開２００５−２４２７９４号公報
【特許文献３】特開平７−９３７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上述の従来のロボット装置の制御方法にあっては、角速度センサーもしくは加速度センサーのどちらかを用いて、振動を抑制する制御信号を生成しているため、アーム先端に発生している振動成分によっては大きな誤差を生じてしまう場合があった。
【０００７】
例えば、角速度センサーのみを用いる場合には、アーム先端部が直線運動であると角速度センサーは回転運動成分を検出するため、直線方向の動作に対して誤差が大きく出てしまう。また加速度センサーのみを用いる場合には、アームが曲線運動であると加速度センサーの直交する２方向の加速度データから、回転運動としての角速度データを生成するため、曲線運動に対して誤差が大きく出てしまう。従って、取得データの誤差が大きいことにより、生成される制御信号にも誤差を生じさせてしまう。その結果、ロボットアームに発生する振動を、短時間に収束できるロボットが望まれていた。
【課題を解決するための手段】
【０００８】
本発明は、少なくとも上述の課題の一つを解決するように、下記の形態または適用例として実現され得る。
【０００９】
〔適用例１〕
本適用例のロボット装置は、アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた少なくとも１以上の角速度センサーおよび加速度センサーと、前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定部と、前記動作判定部の演算結果に基づき、前記角速度センサーの検出値または前記加速度センサーの検出値から、前記基体連結装置の前記アクチュエーターと前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成部と、前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御部とを、備えることを特徴とする。
【００１０】
本適用例のロボット装置によれば、基体に連結されたアームの動作状態が、直線運動かもしくは曲線運動であるかを動作判定部により判定し、その判定結果に基づき角速度センサーもしくは加速度センサーの出力のどちらかを制御信号生成部へ送出する。制御信号生成部では、アーム動作時に発生する振動を制御する制御信号を生成し、制御部へ送出する。制御部は、基体連結装置およびアーム連結装置のアクチュエーターの制御データを生成、制御することによってアームの振動を短時間に収束させることができる。
【００１１】
〔適用例２〕
上述の適用例に係るロボット装置において、前記アーム体にはワーク保持装置が含まれ、前記ワーク保持装置に角速度センサーと加速度センサーとが備えられていることを特徴とする。
【００１２】
本適用例のロボット装置によれば、加工用ツールもしくは被加工物を保持するワーク保持装置を備えた場合でも、適用例１に記載の角度センサーおよび加速度センサーをワーク保持装置に備えることで、正確にロボット装置の作業部位でセンサー検出値を得ることが可能となる。
【００１３】
〔適用例３〕
本適用例のロボット装置の制御方法は、アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体とを備えるロボット装置の制御方法であって、少なくとも１以上の前記アームに角速度センサーおよび加速度センサーを備え、前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定工程と、前記角速度センサーと前記加速度センサーとが配置された前記アームが直線運動と判定された場合には、前記加速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成し、または、前記角速度センサーと前記加速度センサーとが配置された前記アームが曲線運動と判定された場合には、前記角速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成工程と、前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御工程とを、有することを特徴とする。
【００１４】
本適用例のロボット装置の制御方法によれば、基体に連結されたアームの動作状態が、直線運動もしくは曲線運動であるかを判定する動作判定工程において判定し、その判定結果に基づき角速度センサーもしくは加速度センサーの出力のどちらかを使い、アクチュエーターを制御する信号を制御信号生成工程において生成する。生成された制御信号に基づき、アーム動作時に発生する振動を収束させる、基体連結装置およびアーム連結装置のアクチュエーターの制御データを生成し、アクチュエーターへ制御データを送出する制御工程によってアームの振動を短時間に収束させることができ、ロボット装置の作動時間の大幅な短縮を実現できる。
【００１５】
〔適用例４〕
上述の適用例に係るロボット装置の制御方法において、前記動作判定工程は、前記アームの軌道生成情報に基づき直線運動もしくは曲線運動を判定することを特徴とする。
【００１６】
この適用例によれば、軌道生成情報からアームの動作情報を得るため、精度良く直線運動か曲線運動かを判断することができる。
【００１７】
〔適用例５〕
上述の適用例に係るロボット装置の制御方法において、前記動作判定工程は、前記角速度センサーおよび前記加速度センサーの出力値に基づき直線運動もしくは曲線運動を判定することを特徴とする。
【００１８】
この適用例によれば、加速度センサーおよび角速度センサーの実動作に基づく出力信号による制御信号生成となる。従って、アームが状況に応じて異なる動作を行なう場合でも、連結装置へより正確な制御信号を送出することができる。
【図面の簡単な説明】
【００１９】
【図１】第１実施形態におけるロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図。
【図２】第１実施形態における制御ブロック図。
【図３】第１実施形態における制御フローチャート。
【図４】第１実施形態における制御方法説明図。
【図５】第２実施形態における制御フローチャート。
【図６】第３実施形態におけるロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図。
【図７】第３実施形態における制御ブロック図。
【図８】第３実施形態における制御フローチャート。
【図９】第３実施形態における制御方法説明図。
【図１０】第４実施形態における制御フローチャート。
【発明を実施するための形態】
【００２０】
以下、図面を参照して、本発明の実施形態を説明する。
【００２１】
（第１実施形態）
図１は、本発明に係る第１実施形態のロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図である。本実施形態のロボット装置は、水平に回転可能に２本のアームが連結された、いわゆる２軸水平多関節ロボット１０（以下、ロボット１０という）である。
【００２２】
第１アーム１１と第２アーム１２とは、アーム連結装置１５により回転可能に直列的に連結され、アーム体１６が構成されている。アーム連結装置１５は、アクチュエーター１３とアクチュエーター１３のトルクを所定の減速比で伝達するトルク伝達装置１４とを含む。アーム体１６の一方の端部は、ベースに固定された基体１７に、基体連結装置１８により回転可能に連結されている。基体連結装置１８は、アクチュエーター１３とアクチュエーター１３のトルクを所定の減速比で伝達するトルク伝達装置１４とを含む。また、アーム連結装置１５と基体連結装置１８とは、回転角度を検出する角度センサー２０を含む。
【００２３】
また、アーム体１６の基体１７との連結部側反対方向の一方の端部側の第２アーム１２には、加工用ツールもしくは被加工物を保持するワーク保持装置１９が固定されている。第１アーム１１が基体１７に対して回転運動のみであることから、アーム体１６のアーム連結装置１５には、第１角速度センサー２１が配置されている。ワーク保持装置１９には第２角速度センサー２２と加速度センサー２３とが配置されている。
【００２４】
なおアーム連結装置１５および基体連結装置１８のアクチュエーター１３は特に限定されないが、例えば直流モーターを用いることができる。他にもアクチュエーター１３には、ステッピングモーターを使用しても良い。その場合、角度センサー２０は備えなくても良い。
【００２５】
ロボット１０は、基体連結装置１８とアーム連結装置１５とに備えたアクチュエーター１３を制御作動させることで、ワーク保持装置１９を所定の位置まで移動させ、ワーク保持装置１９の動作により所定の作業をさせる装置である。
【００２６】
図２に第１実施形態に係るロボット１０の制御部の構成を示す。なお図１と共通する構成には同じ符号を付してある。ロボット１０の制御部３０は、軌道情報生成部３１と動作判定部３２と制御信号生成部３３とアクチュエーター制御部３４とにより構成されている。
【００２７】
アクチュエーター制御部３４は、軌道情報生成部３１からの軌道情報に基づきアクチュエーター１３を作動制御する。動作判定部３２は、アーム体１６の動作情報を生成する軌道情報生成部３１から得られる、アーム体１６の移動停止時における軌道情報により、アーム連結装置１５位置およびワーク保持装置１９位置の停止直前の動作が、直線運動か曲線運動かを判定する。
【００２８】
制御信号生成部３３は、動作判定部３２における判定結果に基づき、第１角速度センサー２１からの出力値と、第２角速度センサー２２または加速度センサー２３からの出力値とを使い、制御信号を生成する。前記制御信号に基づきアクチュエーター制御部３４は、アーム体１６の停止時に発生する振動を収束させるアクチュエーター１３の動作制御データを送出する。
【００２９】
図３は、第１実施形態に係るロボット１０の制御方法を示すフローチャートである。ロボット１０のアクチュエーター１３を駆動し、ワーク保持装置１９を作業位置まで動作させる軌道情報入力４０が実行される（Ｓｔｅｐ１）。この軌道情報入力４０は予めプログラムされた位置情報に基づき生成される場合、あるいは図示されない画像処理装置などによって得られた位置情報から生成される場合など、入力方法は限定されない。
【００３０】
上述のＳｔｅｐ１で入力された軌道情報に基づき、ワーク保持装置１９位置が停止直前において直線運動か曲線運動かのワーク保持装置部動作判定４１を行なう。なお、アーム連結装置１５位置では、第１アーム１１が基体１７に対して回転運動のみの連結であるので、動作判定のＳｔｅｐは要しない（Ｓｔｅｐ２）。
【００３１】
Ｓｔｅｐ２のワーク保持装置１９位置のワーク保持装置部動作判定４１での動作判定の結果、直線運動と判定された場合には加速度センサー検出値４２が演算部４５へ送出される。あるいは、曲線運動と判定された場合には第２角速度センサー検出値４３が演算部４５へ送出される。このように、制御信号生成に使うセンサー検出値は、ワーク保持装置１９位置の動作状況に最適なセンサー検出値に切換えて、使用する（Ｓｔｅｐ３）。
【００３２】
入手した検出値を使い演算部４５において、基体連結装置１８のアクチュエーター１３の制御信号生成４５ａおよびアーム連結装置１５のアクチュエーター１３の制御信号生成４５ｂにより、停止時に発生する振動を収束させる制御信号を生成する（Ｓｔｅｐ４）。
【００３３】
Ｓｔｅｐ４において生成された制御信号に基づき、基体連結装置アクチュエーター制御４６により基体連結装置アクチュエーター動作制御データを、アーム連結装置アクチュエーター制御４７により、アーム連結装置アクチュエーター動作制御データを生成する（Ｓｔｅｐ５）。
【００３４】
Ｓｔｅｐ５の制御データにより、基体連結装置１８とアーム連結装置１５とのアクチュエーター１３に、アームの振動を収束させる動作をさせて終了となる（Ｓｔｅｐ６、７）。
【００３５】
動作判定部３２での動作判定方法について、図４に基づき説明する。
アーム体１６のワーク保持装置１９位置における速度成分は式１および式２で表される。
ｖαx2≒ω１×ｒ１×ｓｉｎθm1＋（ω２−ω１）×ｒ２×ｓｉｎ（θm2−θm1）  （式１）
ｖαy2≒ω１×ｒ１×ｃｏｓθm1＋（ω２−ω１）×ｒ２×ｃｏｓ（θm2−θm1）  （式２）
【００３６】
式１、式２におけるｖαx2、ｖαy2はワーク保持装置１９における速度成分である。ｒ１は第１アーム１１の長さ、ｒ２は第２アーム１２の長さである。ω１は第１アーム１１の基体１７に対する角速度、ω２は第２アーム１２の基体１７に対する角速度である。またロボット１０に備えた加速度センサー２３の検出方向を、図示した通り直交するＸＹ方向とした場合に、θm1は第１アーム１１のＸ方向に対する角度、θm2は第１アーム１１と第２アーム１２との開き角度である。
【００３７】
この時、第１アーム１１および第２アーム１２を所定の位置に作動させる軌道生成情報から、基体１７に対する第１アーム１１の角速度、および第１アーム１１に対する第２アーム１２の角速度は決定されているので、ワーク保持装置１９のＸＹ方向の動作速度予測値ｖpx2およびｖpy2は、式３および式４により求められる。
ｖpx2＝ωm1×ｒ１×ｓｉｎθm1＋ωm2×ｒ２×ｓｉｎ（θm2−θm1）  （式３）
ｖpy2＝ωm1×ｒ１×ｃｏｓθm1＋ωm2×ｒ２×ｃｏｓ（θm2−θm1）  （式４）
(ωm1:基体１７に対する第１アーム１１の角速度、ωm2：第１アーム１１に対する第２アーム１２の角速度）
【００３８】
求められた動作速度予測値において、直線運動とはｖpy2およびｖpx2が一定値をとることである。すなわち、
ｖpy2／ｖpx2＝Ｋ1
とした時、Ｋ1が０を含む一定値もしくは無限大であればワーク保持装置１９は直線運動と判定する。Ｋ1が変動する値をとった場合には、ワーク保持装置１９は曲線運動と判定する。
【００３９】
次に、第１角速度センサー２１、第２角速度センサー２２、加速度センサー２３の検出値を使ってアクチュエーター１３の制御信号を生成する方法について詳述する。
上述のワーク保持装置１９が直線運動と判定された場合、アーム連結装置１５のアクチュエーター１３の制御信号生成（Ｓｔｅｐ４）の工程では、ワーク保持装置１９に設けた加速度センサー２３の検出方向ＸおよびＹの検出値である加速度成分αx2、αy2から、次の手順により制御信号を生成できる角速度成分に変換する。
【００４０】
＜手順１＞
加速度成分αx2、αy2を積分し、速度成分ｖαx2、ｖαy2へ変換する。
ｖαx2＝∫αx2ｄｔ  （式５）
ｖαy2＝∫αy2ｄｔ  （式６）
【００４１】
＜手順２＞
式５、式６により求められるｖαx2、ｖαy2を比較する。
【００４２】
＜手順３＞
ｖαx2≧ｖαy2の場合には、ｖαx2すなわちαx2を使って制御信号生成に使う角速度データを演算する。またｖαx2<ｖαy2の場合には、ｖαy2すなわちαy2を使って制御信号生成に使う角速度データを演算する。
【００４３】
＜手順４＞
制御信号生成に使う角速度データは、ｖαx2、ｖαy2と角速度成分の関係を表す
ｖαx2＝ωj1×ｒ１×ｓｉｎθm1＋（ωαx2−ωj1）×ｒ２×ｓｉｎ（θm2−θm1）  （式７）
ｖαy2＝ωj1×ｒ１×ｃｏｓθm1＋（ωαy2−ωj1）×ｒ２×ｃｏｓ（θm2−θm1）  （式８）
式７および式８を変形すると、
（ωαx2−ωj1）＝｛ｖαx2−ωj1×ｒ１×ｓｉｎθm1｝／ｒ２×ｓｉｎ（θm2−θm1）  （式９）
（ωαy2−ωj1）＝｛ｖαy2−ωj1×ｒ１×ｃｏｓθm1｝／ｒ２×ｃｏｓ（θm2−θm1）  （式１０）
となる。式９の左辺（ωαx2−ωj1）は、Ｘ方向の加速度成分から求められる第２アーム１２のアーム連結装置１５を中心とする角速度である。式１０の左辺（ωαy2−ωj1）は、Ｙ方向の加速度成分から求められる第２アーム１２のアーム連結装置１５を中心とする角速度である。この角速度値を第１アーム１１に対する第２アーム１２の制御信号、すなわちアーム連結装置１５のアクチュエーター１３の制御信号に使う。
【００４４】
ここで、ωj1はアーム連結装置１５に備えた第１角速度センサー２１の検出値である。ωαx2およびωαy2は、加速度センサー２３の検出加速度αx2、αy2から計算されるワーク保持装置１９の角速度成分を表す。
【００４５】
次に、上述のワーク保持装置１９が曲線運動と判定された場合の、第２アーム１２の制御信号生成について説明する。第２角速度センサー２２の検出値ωj2は、基体１７に対する第２アーム１２の角速度検出値である。従って、第１アーム１１に対する第２アーム１２の角速度、すなわちアーム連結装置１５に対する第２アーム１２の角速度は（ωj2−ωj1）となる。この角速度値（ωj2−ωj1）を第１アーム１１に対する第２アーム１２の制御信号、すなわちアーム連結装置１５のアクチュエーター１３の制御信号の生成に使う。
【００４６】
第１実施形態によれば、停止直前のワーク保持装置１９振動収束のための制御信号生成には、動作形態に応じてワーク保持装置１９位置に配置した第２角速度センサー２２もしくは加速度センサー２３の検出値のどちらかに切換えて使う。
【００４７】
直線運動では加速度センサーを用いた方が検出値にノイズが少なく、曲線運動では角速度センサーを用いた方が検出値にノイズが少ない。すなわち、実動作の形態に最も適したセンサーから、誤差やノイズの少ないセンサー検出値が得られ、正確なアームの制御をすることが可能となる。また、アーム作動速度をより高速にしても、アーム停止時のアームの振動を短時間で収束することができる。このことは、停止時の振動収束の為の待ち時間を短縮することになり、特に多くの部品を組み込む電子部品組立などにおける作業時間の大幅な短縮となる。
また、アームの剛性が低くても本発明の振動制御方法によれば振動を収束させることができる。このことは、アームに剛性の低い軽量素材の採用、あるいは薄肉設計の採用が可能となり、ロボット装置の軽量化が図られ、より設置場所の自由度が広がり、様々な用途へ適用が可能となる。
【００４８】
（第２実施形態）
ロボット１０の第２実施形態について説明する。
図５に第２実施形態に係るフローチャートを示す。第２実施形態は、第１実施形態における、動作判定工程への入力情報（Ｓｔｅｐ１）が異なるだけであって、その他は第１実施形態と同じである。
【００４９】
ワーク保持装置部動作判定５０における入力情報は、第２角速度センサー検出値および加速度センサー検出値５１である。
【００５０】
ワーク保持装置１９に備えた第２角速度センサー２２の角速度検出値ωj2から、次式により速度成分へ変換する。
ｖωj2＝ωj2×（ｒ１＋ｒ２）  （式１１）
ここでｒ１は第１アーム１１の長さ、ｒ２は第２アーム１２の長さである。求められるｖωj2は、ワーク保持装置１９が基体１７から最も離れた位置、すなわち第１アーム１１と第２アーム１２が直線的に連結された状態として、ωj2が取りうる最大速度値が計算される。
【００５１】
また、加速度センサー２３の加速度検出値αx2、αy2を、次式により速度成分に変換すると、
ｖαx2＝∫αx2ｄｔ            （式１２）
ｖαy2＝∫αy2ｄｔ            （式１３）
となる。
【００５２】
このときｖωj2が、ｖαx2またはｖαy2のいずれか大きい値と同じか、またはそれ以上の大きい速度値であれば曲線運動と判定する。またｖωj2が、ｖαx2またはｖαy2のいずれか大きい値より小さい速度値であれば直線運動と判定する（Ｓｔｅｐ２）。動作判定工程後のＳｔｅｐ３からは、上述の第１実施形態における手順１から手順４と同じである。
【００５３】
第２実施形態によれば、ワーク保持装置１９に備えた第２角速度センサー２２および加速度センサー２３の検出値を使い、実動作情報に基づく動作判定を行うため、なお一層の正確な制御信号の生成が可能になる。また、アームの軌道が未定の場合にも判定できる。
【００５４】
（第３実施形態）
第３実施形態は、アーム体１６が２本のアームと１個のアーム連結装置で構成されている第１実施形態に対して、アーム体が３本のアームを２個のアーム連結装置によって連結される構成となっている。
【００５５】
図６は、第３実施形態に従ったロボット装置を示し、（Ａ）は概略断面図、（Ｂ）が概略平面図である。第３実施形態のロボット装置は、水平に回転可能に３本のアームが連結された、いわゆる３軸水平多関節ロボット６０（以下、ロボット６０という）である。
【００５６】
第１アーム６１と第２アーム６２と第３アーム６３とは、第１アーム連結装置６６と第２アーム連結装置６７とにより、回転可能に直列的に連結されアーム体６８が構成されている。第１アーム連結装置６６と第２アーム連結装置６７とは、アクチュエーター６４とアクチュエーター６４のトルクを所定の減速比で伝達するトルク伝達装置６５とを含む。
【００５７】
アーム体６８の一方の端部はベースに固定された基体６９に、基体連結装置７０により回転可能に連結されている。基体連結装置７０にはアクチュエーター６４とアクチュエーター６４のトルクを所定の減速比で伝達するトルク伝達装置６５とを含む。また、第１アーム連結装置６６と第２アーム連結装置６７と基体連結装置７０とは、回転角度を検出する角度センサー７１を含む。また、アーム体６８の基体６９との連結部側反対方向の一方の端部側には、加工用ツールもしくは被加工物を保持するワーク保持装置７２が、第３アーム６３に固定されている。
【００５８】
アーム体６８の第１アーム連結装置６６には、第１角速度センサー７３が配置されている。第２アーム連結装置６７には、第２角速度センサー７４と第１加速度センサー７５とが配置されている。ワーク保持装置７２には、第３角速度センサー７６と第２加速度センサー７７とが配置されている。
【００５９】
なお第１実施形態同様、第３実施形態においても、第１アーム連結装置６６、第２アーム連結装置６７、基体連結装置７０のアクチュエーター６４は特に限定されないが、例えば直流モーターを用いることができる。他にもアクチュエーター６４には、ステッピングモーターを使用しても良い。その場合、角度センサー７１は備えなくても良い。
【００６０】
また図７に第３実施形態に係るロボット６０の制御部の構成を示す。なお図６と共通する構成には同じ符号を付してある。ロボット６０の制御部８０は、軌道情報生成部８１と動作判定部８２と制御信号生成部８３とアクチュエーター制御部８４とにより構成されている。
【００６１】
アクチュエーター制御部８４は、軌道情報生成部８１からの軌道情報に基づきアクチュエーター６４を作動制御する。動作判定部８２は、アーム体６８の動作情報を生成する軌道情報生成部８１から得られる、アーム体６８の移動停止時における軌道情報により、第１アーム６１、第２アーム６２および第３アーム６３の停止直前の動作が、直線運動か曲線運動かを判定する。または、動作判定部８２は、第１角速度センサー７３と第２角速度センサー７４と第３角速度センサー７６とからの出力値、および第１加速度センサー７５と第２加速度センサー７７とからの出力値に基づき、第１アーム６１、第２アーム６２あるいは第３アーム６３の停止直前の動作が、直線運動か曲線運動かを判定する。
【００６２】
制御信号生成部８３は、動作判定部８２における判定結果に基づき、第１角速度センサー７３からの出力値と、第２角速度センサー７４または第１加速度センサー７５からの出力値と、第３角速度センサー７６または第２加速度センサー７７からの出力値とを使い、制御信号を生成する。前記制御信号によりアクチュエーター制御部８４は、アーム体６８の停止時に発生する振動を収束させるアクチュエーター６４の動作制御データを送出する。
【００６３】
図８は、第３実施形態に係るロボット６０の制御方法を示すフローチャートである。
第３実施形態における制御方法は、上述の第１実施形態における図３に示す制御方法のフローチャートに対して、第２アーム連結装置６７位置の動作判定工程が加わったものであり、各Ｓｔｅｐにおける動作は第１実施形態と同じであるため、詳細説明は省略する。
【００６４】
動作判定部８２での動作判定方法について図９に基づき説明する。
基体６９に対する第１アーム６１ならびに第２アーム６２の動作判定方法は、上述した第１実施形態における動作判定部３２での第１アーム１１および第２アーム１２の動作判定方法と同じである。従って、以下では第３アーム６３の動作判定方法について詳細に説明する。
【００６５】
アーム体６８のワーク保持装置７２位置における速度成分は式１４および式１５で表される。
ｖαx3≒ｖαx2＋（ω３−ω２）×ｒ３×ｓｉｎ（θm3＋θm2−θm1）  （式１４）
ｖαy3≒ｖαy2＋（ω３−ω２）×ｒ３×ｃｏｓ（θm3＋θm2−θm1）  （式１５）
【００６６】
式１４、式１５におけるｖαx3、ｖαy3はワーク保持装置７２位置における速度成分である。ｖαx2、ｖαy2は第２アーム連結装置６７位置における速度成分である。ｒ１は第１アーム６１の長さ、ｒ２は第２アーム６２の長さ、ｒ３は第３アーム６３の長さである。ω１は第１アーム６１の基体６９に対する角速度、ω２は第２アーム６２の基体６９に対する角速度、ω３は第３アーム６３の基体６９に対する角速度である。またロボット６０に備えた第１加速度センサー７５および第２加速度センサー７７の検出方向を、図示した直交するＸＹ方向とした場合に、θm1は第１アーム６１のＸ方向に対する角度、θm2は第１アーム６１と第２アーム６２との開き角度、θm3は第２アーム６２と第３アーム６３との開き角度である。
【００６７】
ここで、第１実施形態の説明で記載したｖαx2、ｖαy2を求める式１および式２を式１４および式１５に代入すると、
ｖαx3≒ω１×ｒ１×ｓｉｎθm1＋（ω２−ω１）×ｒ２×ｓｉｎ（θm2−θm1）＋（ω３−ω２）×ｒ３×ｓｉｎ（θm3＋θm2−θm1）  （式１６）
ｖαy3≒ω１×ｒ１×ｃｏｓθm1＋（ω２−ω１）×ｒ２×ｃｏｓ（θm2−θm1）＋（ω３−ω２）×ｒ３×ｃｏｓ（θm3＋θm2−θm1）  （式１７）
となる。
【００６８】
この時、第１アーム６１、第２アーム６２および第３アーム６３を所定の位置に作動させる軌道生成情報から、基体６９に対する第１アーム６１の角速度と第１アーム６１に対する第２アーム６２の角速度と第２アーム６２に対する第３アーム６３の角速度とは決定されているので、ワーク保持装置７２のＸＹ方向の動作速度予測値ｖpx3およびｖpy3は式１８および式１９により求められる。
【００６９】
ｖpx3＝ωm1×ｒ１×ｓｉｎθm1＋ωm2×ｒ２×ｓｉｎ（θm2−θm1）＋ωm3×ｒ３×ｓｉｎ（θm3＋θm2−θm1）  （式１８）
ｖpy3＝ωm1×ｒ１×ｃｏｓθm1＋ωm2×ｒ２×ｃｏｓ（θm2−θm1）＋ωm3×ｒ３×ｃｏｓ（θm3＋θm2−θm1）  （式１９）
（ωm1：基体６９に対する第１アーム６１の角速度、ωm2：第１アーム６１に対する第２アーム６２の角速度、ωm3：第２アーム６２に対する第３アーム６３の角速度）
【００７０】
求められた動作速度予測値において、直線運動とはｖpy3とｖpx3とが一定値をとることである。すなわち、
ｖpy3／ｖpx3＝Ｋ2
とした時、Ｋ2が０を含む一定値もしくは無限大であればワーク保持装置７２位置は直線運動と判定する。Ｋ2が変動する値をとった場合には、ワーク保持装置７２位置は曲線運動と判定する。
【００７１】
上述のワーク保持装置７２位置が直線運動と判定された場合、第２加速度センサー７７の検出値を使い、第２アーム連結装置６７のアクチュエーター６４の制御信号生成（Ｓｔｅｐ４）の工程において、ワーク保持装置７２位置に設けた第２加速度センサー７７の検出方向ＸＹのそれぞれの検出値である加速度成分αx3、αy3から、次の手順により制御信号を生成できる角速度成分へ変換する。
【００７２】
＜手順１＞
加速度成分αx3、αy3を積分し、速度成分ｖαx3、ｖαy3へ変換する
ｖαx3＝∫αx3ｄｔ  （式２０）
ｖαy3＝∫αy3ｄｔ  （式２１）
【００７３】
＜手順２＞
式２０、式２１により求められるｖαx3、ｖαy3を比較する。
【００７４】
＜手順３＞
ｖαx3≧ｖαy3の場合にはｖαx3すなわちαx3を使って制御信号生成に使う角速度情報を演算する。またｖαx3<ｖαy3の場合にはｖαy3すなわちαy3を使って制御信号生成に使う角速度情報を演算する。
【００７５】
＜手順４＞
制御信号生成に使う角速度情報は、ｖαx3、ｖαy3と角速度成分の関係を表す、
ｖαx3＝ωj1×ｒ１×ｓｉｎθm1＋ωk2×ｒ２×ｓｉｎ（θm2−θm1）＋（ωαx3−ωj2）×ｒ３×ｓｉｎ（θm3＋θm2−θm1）  （式２２）
ｖαy3＝ωj1×ｒ１×ｃｏｓθm1＋ωk2×ｒ２×ｃｏｓ（θm2−θm1）＋（ωαy3−ωj2）×ｒ３×ｃｏｓ（θm3＋θm2−θm1）  （式２３）
より導き出す。
【００７６】
式２２、式２３におけるωk2は、第３実施形態の第２アーム連結装置６７位置の角速度である。すなわち上述の第１実施形態におけるワーク保持装置１９位置の動作判定に基づき算出される、第１アーム１１に対する第２アーム１２の角速度に相当する。従って、第２アーム連結装置６７の動作判定結果が直線運動と判定された場合にはωk2＝（ωαx2−ωj1）であり、曲線運動と判定された場合にはωk2＝（ωj2−ωj1）の値を使う。
【００７７】
式２２および式２３を変形し
（ωαx3−ωj2）＝｛ｖαx3−ωj1×ｒ１×ｓｉｎθm1＋ωk2×ｒ２×ｓｉｎ（θm2−θm1）｝／ｒ３×ｓｉｎ（θm3＋θm2−θm1）  （式２４）
（ωαy3−ωj2）＝｛ｖαy3−ωj1×ｒ１×ｃｏｓθm1＋ωk2×ｒ２×ｃｏｓ（θm2−θm1）｝／ｒ３×ｃｏｓ（θm3＋θm2−θm1）  （式２５）
となる。式２４の左辺（ωαx3−ωj2）はＸ方向の加速度成分から求められる第３アーム６３の第２アーム連結装置６７を中心とする角速度である。式２５の左辺（ωαy3−ωj2）はＹ方向の加速度成分から求められる第３アーム６３の第２アーム連結装置６７を中心とする角速度である。
【００７８】
式２４もしくは式２５から求められる角速度値の（ωαx3−ωj2）もしくは（ωαy3−ωj2）は、第２アーム６２に対する第３アーム６３の制御信号、すなわち第２アーム連結装置６７のアクチュエーター６４の制御信号の生成に使う。
【００７９】
次に、上述のワーク保持装置７２が曲線運動と判定された場合の第３アーム６３の制御信号生成に使う角速度を演算する。第３角速度センサー７６の検出値ωj3は、基体６９に対する第３アーム６３の角速度検出値である。従って、第２アーム６２に対する第３アーム６３の角速度、すなわち第２アーム連結装置６７に対する第３アーム６３の角速度は、（ωj3−ωk2）となる。この角速度値（ωj3−ωk2）を第２アーム６２に対する第３アーム６３の制御信号、すなわち第２アーム連結装置６７のアクチュエーター６４の制御信号の生成に使う。
【００８０】
第３実施形態によれば、各々のアームを作動させるアクチュエーターへ送出する振動収束のために制御信号が、各々の連結装置の動作状態に最適となる角速度センサーもしくは加速度センサーのどちらかの検出値を選択して生成されるため、誤差やノイズの少ない検出値によって制御することが可能となる。従って、アーム作動速度をより高速動作させても、アーム停止時のアームの振動を短時間で収束することができる。このことは、停止時の振動収束の為の待ち時間を短縮することになり、特に多くの部品を組み込む電子部品組立工程などにおいては作業時間の大幅な短縮となる。
【００８１】
（第４実施形態）
本発明の第４実施形態について説明する。第４実施形態は図１０に示す通り、第３実施形態に対して、動作判定工程における取得情報が異なる。なお、第３実施形態と同様の点は、説明を省略する。
【００８２】
第４実施形態における動作判定工程における入力情報は、第２角速度センサー７４および第１加速度センサー７５ならびに第３角速度センサー７６および第２加速度センサー７７の検出値である（Ｓｔｅｐ１）。
【００８３】
第２角速度センサー７４と第１加速度センサー７５との検出値、ならびに第３角速度センサー７６と第２加速度センサー７７との検出値を使い、動作判定工程において以下の演算により直線運動か曲線運動かの判定を実行する（Ｓｔｅｐ２）。
【００８４】
第２角速度センサー７４と第１加速度センサー７５との検出値を使い、第２アーム６２を動作させる第１アーム連結装置６６のアクチュエーター６４の振動収束のための制御信号生成方法は、第２実施形態と同じであるため説明は省略する。
【００８５】
第３アーム６３の制御信号生成方法について説明する。
ワーク保持装置７２に備えた第３角速度センサー７６の角速度検出値ωj3から、次式により速度成分へ変換する。
ｖωj3＝ωj3×（ｒ１＋ｒ２＋ｒ３）  （式２６）
ここでｒ１は第１アーム６１の長さ、ｒ２は第２アーム６２、ｒ３は第３アーム６３の長さである。
【００８６】
求められるｖωj3は、ワーク保持装置７２が基体６９から最も離れた位置、すなわち第１アーム６１と第２アーム６２と第３アーム６３とが直線的に連結された状態として、ωj3が取りうる最大速度値が計算される。
【００８７】
第２加速度センサー７７の加速度検出値αx3、αy3とを次式により速度成分に変換すると、
ｖαx3＝∫αx3ｄｔ                  （式２７）
ｖαy3＝∫αy3ｄｔ                  （式２８）
となる。
【００８８】
このときｖωj3が、ｖαx3またはｖαy3のいずれかの大きい値と同じか、またはそれ以上の大きい速度値であれば、曲線運動と判定する。また、ｖωj3がｖαx3またはｖαy3のいずれか大きい値より小さい速度値であれば、直線運動と判定する。
動作判定工程後は、上述の第３実施形態と同じ制御信号生成方法により制御信号が生成される。
【００８９】
第４実施形態によれば、各連結部およびワーク保持装置部に備えた角速度センサーおよび加速度センサーの検出値を使うことで、実動作情報に基づく動作判定を行うため、なお一層の正確な制御信号の生成が可能になる。上述の第３実施形態および第４実施形態では、連結されるアームを３本として説明したが、更にアームを増設した場合であっても、第３実施形態および第４実施形態同様に、連結された各アーム毎に直線運動もしくは曲線運動であるかを判定し、適合した角速度センサーもしくは加速度センサーの検出値を使い、制御信号を生成することで、上述の実施形態同様の効果を得られるものである。
【００９０】
なお、上述の実施形態ではアーム連結装置部の全てに角度センサーおよび加速度センサーを配置した形態を説明したが、３本以上のアームを連結する場合には、少なくとも一つのアーム連結装置部に角速度センサーおよび加速度センサーを備えることで同様の効果を得ることができる。
【００９１】
（変形例）
なお上述の実施形態は、以下の態様に変更してもよい。上述の実施形態では水平多関節ロボットとしたが、これに限定されるものではなく、垂直多関節ロボット、直交ロボットあるいはパラレルリンクロボットなどにも適用できるものである。
上述の実施形態では、アーム停止直後の振動を収束さる制御であるが、アームの動作中に発生する振動を収束、減衰さる制御であってもよい。
上述の第２実施形態および第４実施形態では、動作判定工程において角速度センサーの検出値を速度成分に変換したが、センサーの検出値と判定値とを比較して、角速度センサーもしくは加速度センサーのどちらを用いるかを判定してもよい。
上述の実施形態で、Ｋ1、Ｋ2が０を含む一定値もしくは無限大の時に直線運動と判定するものとしたが、判定値との比較によって直線運動か曲線運動かを判定してもよい。
上述の実施形態では、ワーク保持装置に角速度センサーと加速度センサーを備えるものとしたが、第１実施形態の第２アームあるいは第３実施形態の第３アームに備えてもよく、その場合にはワーク保持装置などの作業装置が備えられるアームであることが望ましい。上述の第３実施形態で総てのアームに角速度センサーおよび加速度センサーを配置したが、制御上不要な場合には配置しなくてもよい。
【符号の説明】
【００９２】
１０…ロボット、１１…第１アーム、１２…第２アーム、１３…アクチュエーター、１４…トルク伝達装置、１５…アーム連結装置、１６…アーム体、１７…基体、１８…基体連結装置、１９…ワーク保持装置、２０…角度センサー、２１…第１角速度センサー、２２…第２角速度センサー、２３…加速度センサー。
【図１】
2011020188.tif 000003
【図２】
2011020188.tif 000004
【図３】
2011020188.tif 000005
【図４】
2011020188.tif 000006
【図５】
2011020188.tif 000007
【図６】
2011020188.tif 000008
【図７】
2011020188.tif 000009
【図８】
2011020188.tif 000010
【図９】
2011020188.tif 000011
【図１０】
2011020188.tif 000012