JP2011020188A - ロボット装置およびロボット装置の制御方法 - Google Patents

Abstract

【課題】角速度および加速度センサーを用いて、アームの移動ベクトルに対応して角速度もしくは加速度センサーのどちらかの検出値を選択し、生成された制御信号によりアームの移動停止時に発生する振動を短時間に収束させ、アームの速い作動速度と高い停止位置精度を有するロボット装置とその制御方法を提供する。
【解決手段】ロボット装置であって、アームに角速度センサーと加速度センサーとを備え、前記角速度センサーと前記加速度センサーとが配置された部位が、直線運動もしくは曲線運動であるかを判定する動作判定部と、前記動作判定部の結果に基づき前記角速度もしくは加速度センサーの検出値から前記アームの制御信号を生成する制御信号生成部と、前記制御信号に基づき基体連結装置とアーム連結装置とを制御する制御部とを備える。
【選択図】図１

Description

本発明はロボット装置およびロボット装置の制御方法に関する。

ＩＣハンドラーや組立装置の一部として多く使われている多関節構造を有するロボット装置は、様々な産業現場の中で多用されてきている。故に、ロボット装置には今まで以上に、求められる位置にいかに早く且つ正確にアームを移動させることができるかが重要な性能仕様、品質になってきている。

一般的にアームを高速に且つ正確に移動させるには、アームに掛かる慣性力を小さくし、駆動用のアクチュエーターの負荷を大きくさせないことにある。アームに掛かる慣性力を小さくするには、アームの軽量化が最も効果的な手法として用いられている。しかし、アームを軽量化することによりアーム剛性の低下を招き、アーム停止時に生じるアームの振動を抑制することが困難になり、アーム先端部を目的の位置で停止させても、アーム自体の振動の振幅分の位置ズレが生じてしまい、振動が減衰する時間まで次の動作を開始することができないという問題があった。

この問題に対して、アーム先端に加速度センサーを設置し加速度信号を元にアームを作動させ振動を抑制する方法（例えば、特許文献１）、アーム先端およびアームに角速度センサーを設置し、角速度信号を元にアーム動作を制御する方法（例えば、特許文献２）、アーム先端に設置したチャック部近傍にジャイロ（角速度センサー）と加速度計からなる慣性センサーを設置し、該慣性センサーの信号に基づき駆動体を駆動する方法（例えば、特許文献３）、などが提示されている。

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

しかしながら、上述の従来のロボット装置の制御方法にあっては、角速度センサーもしくは加速度センサーのどちらかを用いて、振動を抑制する制御信号を生成しているため、アーム先端に発生している振動成分によっては大きな誤差を生じてしまう場合があった。

例えば、角速度センサーのみを用いる場合には、アーム先端部が直線運動であると角速度センサーは回転運動成分を検出するため、直線方向の動作に対して誤差が大きく出てしまう。また加速度センサーのみを用いる場合には、アームが曲線運動であると加速度センサーの直交する２方向の加速度データから、回転運動としての角速度データを生成するため、曲線運動に対して誤差が大きく出てしまう。従って、取得データの誤差が大きいことにより、生成される制御信号にも誤差を生じさせてしまう。その結果、ロボットアームに発生する振動を、短時間に収束できるロボットが望まれていた。

本発明は、少なくとも上述の課題の一つを解決するように、下記の形態または適用例として実現され得る。

〔適用例１〕
本適用例のロボット装置は、アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた少なくとも１以上の角速度センサーおよび加速度センサーと、前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定部と、前記動作判定部の演算結果に基づき、前記角速度センサーの検出値または前記加速度センサーの検出値から、前記基体連結装置の前記アクチュエーターと前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成部と、前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御部とを、備えることを特徴とする。

本適用例のロボット装置によれば、基体に連結されたアームの動作状態が、直線運動かもしくは曲線運動であるかを動作判定部により判定し、その判定結果に基づき角速度センサーもしくは加速度センサーの出力のどちらかを制御信号生成部へ送出する。制御信号生成部では、アーム動作時に発生する振動を制御する制御信号を生成し、制御部へ送出する。制御部は、基体連結装置およびアーム連結装置のアクチュエーターの制御データを生成、制御することによってアームの振動を短時間に収束させることができる。

〔適用例２〕
上述の適用例に係るロボット装置において、前記アーム体にはワーク保持装置が含まれ、前記ワーク保持装置に角速度センサーと加速度センサーとが備えられていることを特徴とする。

本適用例のロボット装置によれば、加工用ツールもしくは被加工物を保持するワーク保持装置を備えた場合でも、適用例１に記載の角度センサーおよび加速度センサーをワーク保持装置に備えることで、正確にロボット装置の作業部位でセンサー検出値を得ることが可能となる。

〔適用例３〕
本適用例のロボット装置の制御方法は、アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体とを備えるロボット装置の制御方法であって、少なくとも１以上の前記アームに角速度センサーおよび加速度センサーを備え、前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定工程と、前記角速度センサーと前記加速度センサーとが配置された前記アームが直線運動と判定された場合には、前記加速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成し、または、前記角速度センサーと前記加速度センサーとが配置された前記アームが曲線運動と判定された場合には、前記角速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成工程と、前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御工程とを、有することを特徴とする。

本適用例のロボット装置の制御方法によれば、基体に連結されたアームの動作状態が、直線運動もしくは曲線運動であるかを判定する動作判定工程において判定し、その判定結果に基づき角速度センサーもしくは加速度センサーの出力のどちらかを使い、アクチュエーターを制御する信号を制御信号生成工程において生成する。生成された制御信号に基づき、アーム動作時に発生する振動を収束させる、基体連結装置およびアーム連結装置のアクチュエーターの制御データを生成し、アクチュエーターへ制御データを送出する制御工程によってアームの振動を短時間に収束させることができ、ロボット装置の作動時間の大幅な短縮を実現できる。

〔適用例４〕
上述の適用例に係るロボット装置の制御方法において、前記動作判定工程は、前記アームの軌道生成情報に基づき直線運動もしくは曲線運動を判定することを特徴とする。

この適用例によれば、軌道生成情報からアームの動作情報を得るため、精度良く直線運動か曲線運動かを判断することができる。

〔適用例５〕
上述の適用例に係るロボット装置の制御方法において、前記動作判定工程は、前記角速度センサーおよび前記加速度センサーの出力値に基づき直線運動もしくは曲線運動を判定することを特徴とする。

この適用例によれば、加速度センサーおよび角速度センサーの実動作に基づく出力信号による制御信号生成となる。従って、アームが状況に応じて異なる動作を行なう場合でも、連結装置へより正確な制御信号を送出することができる。

第１実施形態におけるロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図。 第１実施形態における制御ブロック図。 第１実施形態における制御フローチャート。 第１実施形態における制御方法説明図。 第２実施形態における制御フローチャート。 第３実施形態におけるロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図。 第３実施形態における制御ブロック図。 第３実施形態における制御フローチャート。 第３実施形態における制御方法説明図。 第４実施形態における制御フローチャート。

以下、図面を参照して、本発明の実施形態を説明する。

（第１実施形態）
図１は、本発明に係る第１実施形態のロボット装置を示し、（Ａ）は概略断面図、（Ｂ）は概略平面図である。本実施形態のロボット装置は、水平に回転可能に２本のアームが連結された、いわゆる２軸水平多関節ロボット１０（以下、ロボット１０という）である。

第１アーム１１と第２アーム１２とは、アーム連結装置１５により回転可能に直列的に連結され、アーム体１６が構成されている。アーム連結装置１５は、アクチュエーター１３とアクチュエーター１３のトルクを所定の減速比で伝達するトルク伝達装置１４とを含む。アーム体１６の一方の端部は、ベースに固定された基体１７に、基体連結装置１８により回転可能に連結されている。基体連結装置１８は、アクチュエーター１３とアクチュエーター１３のトルクを所定の減速比で伝達するトルク伝達装置１４とを含む。また、アーム連結装置１５と基体連結装置１８とは、回転角度を検出する角度センサー２０を含む。

また、アーム体１６の基体１７との連結部側反対方向の一方の端部側の第２アーム１２には、加工用ツールもしくは被加工物を保持するワーク保持装置１９が固定されている。第１アーム１１が基体１７に対して回転運動のみであることから、アーム体１６のアーム連結装置１５には、第１角速度センサー２１が配置されている。ワーク保持装置１９には第２角速度センサー２２と加速度センサー２３とが配置されている。

なおアーム連結装置１５および基体連結装置１８のアクチュエーター１３は特に限定されないが、例えば直流モーターを用いることができる。他にもアクチュエーター１３には、ステッピングモーターを使用しても良い。その場合、角度センサー２０は備えなくても良い。

ロボット１０は、基体連結装置１８とアーム連結装置１５とに備えたアクチュエーター１３を制御作動させることで、ワーク保持装置１９を所定の位置まで移動させ、ワーク保持装置１９の動作により所定の作業をさせる装置である。

図２に第１実施形態に係るロボット１０の制御部の構成を示す。なお図１と共通する構成には同じ符号を付してある。ロボット１０の制御部３０は、軌道情報生成部３１と動作判定部３２と制御信号生成部３３とアクチュエーター制御部３４とにより構成されている。

アクチュエーター制御部３４は、軌道情報生成部３１からの軌道情報に基づきアクチュエーター１３を作動制御する。動作判定部３２は、アーム体１６の動作情報を生成する軌道情報生成部３１から得られる、アーム体１６の移動停止時における軌道情報により、アーム連結装置１５位置およびワーク保持装置１９位置の停止直前の動作が、直線運動か曲線運動かを判定する。

制御信号生成部３３は、動作判定部３２における判定結果に基づき、第１角速度センサー２１からの出力値と、第２角速度センサー２２または加速度センサー２３からの出力値とを使い、制御信号を生成する。前記制御信号に基づきアクチュエーター制御部３４は、アーム体１６の停止時に発生する振動を収束させるアクチュエーター１３の動作制御データを送出する。

図３は、第１実施形態に係るロボット１０の制御方法を示すフローチャートである。ロボット１０のアクチュエーター１３を駆動し、ワーク保持装置１９を作業位置まで動作させる軌道情報入力４０が実行される（Ｓｔｅｐ１）。この軌道情報入力４０は予めプログラムされた位置情報に基づき生成される場合、あるいは図示されない画像処理装置などによって得られた位置情報から生成される場合など、入力方法は限定されない。

上述のＳｔｅｐ１で入力された軌道情報に基づき、ワーク保持装置１９位置が停止直前において直線運動か曲線運動かのワーク保持装置部動作判定４１を行なう。なお、アーム連結装置１５位置では、第１アーム１１が基体１７に対して回転運動のみの連結であるので、動作判定のＳｔｅｐは要しない（Ｓｔｅｐ２）。

Ｓｔｅｐ２のワーク保持装置１９位置のワーク保持装置部動作判定４１での動作判定の結果、直線運動と判定された場合には加速度センサー検出値４２が演算部４５へ送出される。あるいは、曲線運動と判定された場合には第２角速度センサー検出値４３が演算部４５へ送出される。このように、制御信号生成に使うセンサー検出値は、ワーク保持装置１９位置の動作状況に最適なセンサー検出値に切換えて、使用する（Ｓｔｅｐ３）。

入手した検出値を使い演算部４５において、基体連結装置１８のアクチュエーター１３の制御信号生成４５ａおよびアーム連結装置１５のアクチュエーター１３の制御信号生成４５ｂにより、停止時に発生する振動を収束させる制御信号を生成する（Ｓｔｅｐ４）。

Ｓｔｅｐ４において生成された制御信号に基づき、基体連結装置アクチュエーター制御４６により基体連結装置アクチュエーター動作制御データを、アーム連結装置アクチュエーター制御４７により、アーム連結装置アクチュエーター動作制御データを生成する（Ｓｔｅｐ５）。

Ｓｔｅｐ５の制御データにより、基体連結装置１８とアーム連結装置１５とのアクチュエーター１３に、アームの振動を収束させる動作をさせて終了となる（Ｓｔｅｐ６、７）。

動作判定部３２での動作判定方法について、図４に基づき説明する。
アーム体１６のワーク保持装置１９位置における速度成分は式１および式２で表される。
ｖ_α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＝Ｋ₁
とした時、Ｋ₁が０を含む一定値もしくは無限大であればワーク保持装置１９は直線運動と判定する。Ｋ₁が変動する値をとった場合には、ワーク保持装置１９は曲線運動と判定する。

次に、第１角速度センサー２１、第２角速度センサー２２、加速度センサー２３の検出値を使ってアクチュエーター１３の制御信号を生成する方法について詳述する。
上述のワーク保持装置１９が直線運動と判定された場合、アーム連結装置１５のアクチュエーター１３の制御信号生成（Ｓｔｅｐ４）の工程では、ワーク保持装置１９に設けた加速度センサー２３の検出方向ＸおよびＹの検出値である加速度成分α_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＝Ｋ₂
とした時、Ｋ₂が０を含む一定値もしくは無限大であればワーク保持装置７２位置は直線運動と判定する。Ｋ₂が変動する値をとった場合には、ワーク保持装置７２位置は曲線運動と判定する。

上述のワーク保持装置７２位置が直線運動と判定された場合、第２加速度センサー７７の検出値を使い、第２アーム連結装置６７のアクチュエーター６４の制御信号生成（Ｓｔｅｐ４）の工程において、ワーク保持装置７２位置に設けた第２加速度センサー７７の検出方向ＸＹのそれぞれの検出値である加速度成分α_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のいずれか大きい値より小さい速度値であれば、直線運動と判定する。
動作判定工程後は、上述の第３実施形態と同じ制御信号生成方法により制御信号が生成される。

第４実施形態によれば、各連結部およびワーク保持装置部に備えた角速度センサーおよび加速度センサーの検出値を使うことで、実動作情報に基づく動作判定を行うため、なお一層の正確な制御信号の生成が可能になる。上述の第３実施形態および第４実施形態では、連結されるアームを３本として説明したが、更にアームを増設した場合であっても、第３実施形態および第４実施形態同様に、連結された各アーム毎に直線運動もしくは曲線運動であるかを判定し、適合した角速度センサーもしくは加速度センサーの検出値を使い、制御信号を生成することで、上述の実施形態同様の効果を得られるものである。

なお、上述の実施形態ではアーム連結装置部の全てに角度センサーおよび加速度センサーを配置した形態を説明したが、３本以上のアームを連結する場合には、少なくとも一つのアーム連結装置部に角速度センサーおよび加速度センサーを備えることで同様の効果を得ることができる。

（変形例）
なお上述の実施形態は、以下の態様に変更してもよい。上述の実施形態では水平多関節ロボットとしたが、これに限定されるものではなく、垂直多関節ロボット、直交ロボットあるいはパラレルリンクロボットなどにも適用できるものである。
上述の実施形態では、アーム停止直後の振動を収束さる制御であるが、アームの動作中に発生する振動を収束、減衰さる制御であってもよい。
上述の第２実施形態および第４実施形態では、動作判定工程において角速度センサーの検出値を速度成分に変換したが、センサーの検出値と判定値とを比較して、角速度センサーもしくは加速度センサーのどちらを用いるかを判定してもよい。
上述の実施形態で、Ｋ₁、Ｋ₂が０を含む一定値もしくは無限大の時に直線運動と判定するものとしたが、判定値との比較によって直線運動か曲線運動かを判定してもよい。
上述の実施形態では、ワーク保持装置に角速度センサーと加速度センサーを備えるものとしたが、第１実施形態の第２アームあるいは第３実施形態の第３アームに備えてもよく、その場合にはワーク保持装置などの作業装置が備えられるアームであることが望ましい。上述の第３実施形態で総てのアームに角速度センサーおよび加速度センサーを配置したが、制御上不要な場合には配置しなくてもよい。

１０…ロボット、１１…第１アーム、１２…第２アーム、１３…アクチュエーター、１４…トルク伝達装置、１５…アーム連結装置、１６…アーム体、１７…基体、１８…基体連結装置、１９…ワーク保持装置、２０…角度センサー、２１…第１角速度センサー、２２…第２角速度センサー、２３…加速度センサー。

Claims (5)

1. アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、
   複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、
   前記アームに取り付けられた少なくとも１以上の角速度センサーおよび加速度センサーと、
   前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定部と、
   前記動作判定部の演算結果に基づき、前記角速度センサーの検出値または前記加速度センサーの検出値から、前記基体連結装置の前記アクチュエーターと前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成部と、
   前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御部とを、
   備えることを特徴とするロボット装置。
2. 請求項１に記載のロボット装置において、
   前記アーム体にはワーク保持装置が含まれ、
   前記ワーク保持装置に角速度センサーと加速度センサーとが備えられている、
   ことを特徴とするロボット装置。
3. アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含むアーム連結装置と、
   複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構とを含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体とを備えるロボット装置の制御方法であって、
   少なくとも１以上の前記アームに角速度センサーおよび加速度センサーを備え、
   前記角速度センサーと前記加速度センサーとが配置された前記アームが、直線運動もしくは曲線運動であるかを判定する動作判定工程と、
   前記角速度センサーと前記加速度センサーとが配置された前記アームが直線運動と判定された場合には、前記加速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成し、
   または、前記角速度センサーと前記加速度センサーとが配置された前記アームが曲線運動と判定された場合には、前記角速度センサーの検出値から、前記基体連結装置と前記アーム連結装置との前記アクチュエーターの制御信号を生成する制御信号生成工程と、
   前記制御信号に基づき前記基体連結装置と前記アーム連結装置との前記アクチュエーターを制御する制御工程とを、
   有することを特徴とするロボット装置の制御方法。
4. 請求項３に記載のロボット装置の制御方法において、
   前記動作判定工程は、前記アームの軌道生成情報に基づき直線運動もしくは曲線運動を判定する、
   ことを特徴とするロボット装置の制御方法。
5. 請求項３に記載のロボット装置の制御方法において、
   前記動作判定工程は、前記角速度センサーおよび前記加速度センサーの出力値に基づき直線運動もしくは曲線運動を判定する、
   ことを特徴とするロボット装置の制御方法。

Images