JP6512790B2 - ロボット制御方法、ロボット装置、プログラム、記録媒体及び物品の製造方法 - Google Patents

Description

本発明は、多関節のロボットを制御するロボット制御方法、ロボット装置、プログラム、記録媒体及び物品の製造方法に関する。

従来、工場等において、様々なロボット装置が使用されており、昨今においては、より複雑な動作を行わせるために多関節のロボットを備えたロボット装置が広く普及している。

生産装置として多関節のロボットを用いる場合、作業効率を高めるためには、ロボットを、可能な限り高速に動作させる必要がある。そのためには、ロボットが動作時に指令位置へ確実に到達したことの検出、すなわち位置決め完了検出を速やかに行う必要がある。

ロボットの位置決め完了検出は、各関節のモータに連結されたエンコーダにより、モータ軸、すなわち減速機入力側の位置を検出し、検出した位置と位置指令値との差が、予め定めた判定値を下回ることにより判断される。

このときロボットの先端では、ロボットの剛性の低さに起因する振動が発生するので、従来、ロボットの位置決め完了検出に対しては、以下のような技術が提案されている。例えば、ロボットの先端位置をモータの位置、速度、トルク指令値から推定し、この値が予め設定した先端位置の許容値以内となった場合に位置決め完了と判定する方法が提案されている（特許文献１参照）。

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

しかしながら、特許文献１に記載の技術は、ロボットの先端位置を、モータの回転軸の回転角度を検出するエンコーダによる検出角度に基づいて、計算により推定している。即ち、検出しているのは減速機の入力側の角度である。減速機の入力側からロボットの先端までには、剛性が低く振動を起こしやすい減速機が介在しており、減速機の撓みやヒステリシスによる誤差を減速機入力側の値で正確に求めることは困難である。したがって、実際にはロボットの先端の振動が収束していない場合もあり、組み付け作業の精度が低く、信頼性の面で問題があった。

また、次の動作に移る前に、予め設定した所定時間、ロボットの動作を停止させることも考えられる。この停止時間を相当の余裕をもって設定した場合、ロボットの振動を所定の許容範囲内に収束させることは可能であるが、同一の組み付け動作を繰り返し実施した場合、余裕を見た分の時間（マージン）が累積され、ロボットによる作業効率が低下していた。逆に、停止時間を短く設定した場合は、ロボットの先端の振動が収束しないまま次動作に入る可能性があった。この場合、次動作の精度が低下し、組付の失敗やロボットの衝突などのエラーの原因となるおそれがあった。

本発明は、多関節のロボットの動作精度を確保しつつ、ロボットの作業効率を向上させることを目的とする。

本発明は、制御手段が、各関節を駆動するモータと、前記各関節の角度を検出する関節角度検出器とを有する多関節のロボットの動作を制御するロボット制御方法であって、前記制御手段が、前記各モータの回転角度が目標角度となるように前記各モータの回転を制御して、前記各モータの回転角度が前記目標角度に達した後に前記各モータの回転動作を一時停止させる第１動作工程と、前記制御手段が、前記各関節角度検出器により検出された関節角度に基づき、前記ロボットの所定箇所の位置を算出し、前記ロボットの所定箇所の位置の時間変化に基づき、前記ロボットの所定箇所における第１方向の振動と、前記第１方向とは異なる方向である第２方向の振動の各々が許容範囲内に収束するまでの収束時間を各々推定する推定工程と、前記制御手段が、前記第１方向および前記第２方向の振動の各々が前記収束時間に達した後に、前記モータの回転を制御して前記ロボットを動作させる第２動作工程と、を備えたことを特徴とする。

本発明によれば、ロボットの先端の位置を各関節角度検出器により実測された関節角度に基づいて求め、その時間変化によりロボットの先端の振動の収束時間を推定し、少なくとも収束時間の分、ロボットを一時停止させている。これにより、ロボットの動作精度を確保しつつ短時間でロボットを動作させることが可能となり、ロボットの作業効率が向上する。

第１実施形態に係るロボット装置を示す斜視図である。 図１に示すロボットアームの第２関節を示す部分断面図である。 第１実施形態に係るロボット装置の制御装置の構成を示すブロック図である。 第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。 第１実施形態に係るロボット装置におけるロボット制御方法の各工程を示すフローチャートである。 第１実施形態のワールド座標系における各軸方向におけるロボットの先端の振動状況を説明するためのグラフである。 第１実施形態に係るロボット制御方法における推定工程を示すフローチャートである。 第１実施形態に係るロボット制御方法における推定工程を説明するための概念図である。 第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。 第２実施形態に係るロボット装置の判定処理部における経時変化のデータの処理を説明するための図である。 第２実施形態に係るロボット装置におけるロボット制御方法の各工程を示すフローチャートである。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置を示す斜視図である。図１に示すロボット装置５００は、産業用ロボットである。ロボット装置５００は、把持した第１部品Ｗ１を第２部品Ｗ２に組付けて組立部品を製造するロボット１００と、ロボット１００を制御する制御手段としての制御装置２００と、制御装置２００に接続されたティーチングペンダント３００と、を備えている。

ロボット１００は、多関節のロボットである。ロボット１００は、垂直多関節型のロボットアーム１０１と、ロボットアーム１０１の先端に接続されたエンドエフェクタであるロボットハンド１０２と、を備えている。

ロボット１００の先端（ロボットハンド１０２又はロボットアーム１０１の先端）には、ツールセンターポイント（ＴＣＰ）が設定されている。ロボット１００の教示点がタスク空間で指定される場合、その教示点は、ロボット１００のワールド座標系ΣにおけるＴＣＰの位置及び姿勢を示すパラメータで表現される。教示点がコンフィグレーション空間（関節空間）で指定される場合、その教示点は、ロボット１００の各関節の関節角度を示すパラメータで表現される。なお、コンフィグレーション空間で指定された教示点は、順運動学計算により、タスク空間に変換することができ、タスク空間で指定された教示点は、逆運動学計算により、コンフィグレーション空間に変換することができる。ここで、ワールド座標系Σは、ロボット１００の基端に原点が設定され、水平方向に互いに直交する２軸（ｘ軸及びｙ軸）と２軸に対して直交する１軸（ｚ軸）とからなる互いに直交する３軸で表される。

ロボットアーム１０１は、作業台に固定されるベース部（基端リンク）１０３と、変位や力を伝達する複数のリンク１２１〜１２６と、を有している。ベース部１０３及び複数のリンク１２１〜１２６は、複数の関節Ｊ１〜Ｊ６で旋回又は回転可能に互いに連結されている。また、ロボットアーム１０１は、各関節Ｊ１〜Ｊ６に設けられた、関節を駆動する関節駆動装置１１０を備えている。各関節Ｊ１〜Ｊ６に配置された関節駆動装置１１０は、必要なトルクの大きさに合わせて適切な出力のものが用いられる。

ロボットハンド１０２は、部品Ｗ１を把持する複数の把持爪（フィンガ）１０４と、複数の把持爪１０４を駆動する不図示の駆動部と、駆動部の回転角度を検出する不図示のエンコーダと、回転を把持動作に変換する不図示の機構とを有している。この不図示の機構は、カム機構やリンク機構などで必要な把持動作に合わせて設計される。なお、ロボットハンド１０２に用いる駆動部に必要なトルクは、ロボットアーム１０１の関節用と異なるが、基本構成は同じである。また、ロボットハンド１０２は、把持爪１０４等に作用する力（反力）やモーメントを検出可能な不図示の力覚センサを有している。

ティーチングペンダント３００は、制御装置２００に接続可能に構成され、制御装置２００に接続された際に、ロボットアーム１０１やロボットハンド１０２を駆動制御する指令を制御装置２００に送信可能に構成されている。

以下、関節Ｊ２における関節駆動装置１１０を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６の関節駆動装置１１０については、サイズや性能が異なる場合もあるが、同様の構成であるため、説明を省略する。

図２は、ロボットアーム１０１の関節Ｊ２を示す部分断面図である。関節駆動装置１１０は、関節Ｊ２を駆動する駆動源としての電動モータであるサーボモータ（以下、「モータ」という）１と、モータ１の回転軸２の回転を減速して出力する減速機１１と、を有している。

また、関節駆動装置１１０は、モータ１の回転軸２の回転角度（出力角度）を検出するモータ角度検出器である入力側エンコーダ１０を有している。また、関節駆動装置１１０は、関節Ｊ２の関節角度、即ち減速機１１の出力軸の回転角度（出力角度）を検出する関節角度検出器である出力側エンコーダ１６を有している。

モータ１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ１は、回転軸２とロータマグネット３とで構成された回転部４と、モータハウジング５と、回転軸２を回転自在に支持する軸受６，７と、回転部４を回転させるステータコイル８と、を備えている。軸受６，７はモータハウジング５に設けられ、ステータコイル８はモータハウジング５に取り付けられている。また、モータ１はモータカバー９で囲われている。

入力側エンコーダ１０は、光学式或いは磁気式のロータリエンコーダであり、モータ１の回転軸２の一端に設けられ、モータ１の回転軸２の回転に伴ってパルス信号を生成し、生成したパルス信号を制御装置２００に出力する。なお、入力側エンコーダ１０は、回転軸２に取り付けられているが、減速機１１の入力軸に取り付けてもよい。

出力側エンコーダ１６は、光学式或いは磁気式のロータリエンコーダであり、減速機１１の出力角度、第１実施形態では、ベース部１０３とリンク１２１、或いは隣り合う２つのリンク間の相対角度を検出する。関節Ｊ２においては、出力側エンコーダ１６は、リンク１２１とリンク１２２との間の相対角度を検出する。具体的には、出力側エンコーダ１６は、関節Ｊ２の駆動（リンク１２１とリンク１２２との相対移動）に伴ってパルス信号を生成し、制御装置２００に生成したパルス信号を出力する。

リンク１２１とリンク１２２とは、クロスローラベアリング１５を介して回転自在に結合されている。モータ１と入力側エンコーダ１０との間には、必要に応じて、電源オフ時にロボットアーム１０１の姿勢を保持するためのブレーキユニットを設けてもよい。

減速機１１は、第１実施形態では、小型軽量で減速比の大きい波動歯車減速機である。減速機１１は、モータ１の回転軸２に結合された、入力軸を有するウェブジェネレータ１２と、リンク１２２に固定された、出力軸を有するサーキュラスプライン１３と、を備えている。なお、サーキュラスプライン１３は、リンク１２２に直結されているが、リンク１２２に一体に形成されていてもよい。

また、減速機１１は、ウェブジェネレータ１２とサーキュラスプライン１３との間に配置され、リンク１２１に固定されたフレクスプライン１４を備えている。フレクスプライン１４は、ウェブジェネレータ１２の回転に対して減速比Ｎで減速され、サーキュラスプライン１３に対して相対的に回転する。従って、モータ１の回転軸２の回転数が減速機１１で１／Ｎに減速され、フレクスプライン１４が固定されたリンク１２１に対してサーキュラスプライン１３が固定されたリンク１２２を相対的に回転運動させ、関節Ｊ２を屈曲（回転）させる。このときの減速機１１の出力側の回転角度が、実出力角度、即ち関節Ｊ２の角度（関節角度）となる。

ロボット１００の振動は、ロボット１００の剛性の低さに起因するものであるが、その要因として支配的である箇所は本実施形態では減速機１１である。よって、減速機１１の振動状況が分かれば収束検出の精度を向上させることができる。

図３は、ロボット装置５００の制御装置２００の構成を示すブロック図である。制御手段としての制御装置２００は、制御部（演算部）としてのＣＰＵ（Central Processing Unit）２０１を備えている。また、制御装置２００は、記憶手段として、ＲＯＭ（Read Only Memory）２０２、ＲＡＭ（Random Access Memory）２０３、ＨＤＤ（Hard Disk Drive）２０４を備えている。また、制御装置２００は、記録ディスクドライブ２０５、計時手段であるタイマ２０６及び各種のインタフェース２１１〜２１６を備えている。また、制御装置２００は、インタフェース２１６に接続されたサーボ制御装置２３０を備えている。

ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５、タイマ２０６及び各種のインタフェース２１１〜２１６が、バス２２０を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。

ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ２０１に、後述する各種演算処理を実行させるためのプログラム２４０を記録するものである。ＣＰＵ２０１は、ＨＤＤ２０４に記録（格納）されたプログラム２４０に基づいてロボット制御方法の各工程を実行する。

記録ディスクドライブ２０５は、記録ディスク２４１に記録された各種データやプログラム等を読み出すことができる。タイマ２０６は、時間をカウントし、その時間のデータをＣＰＵ２０１に出力する。なお、タイマ２０６の機能をＣＰＵ２０１が備えていてもよい。

インタフェース２１１には、ユーザが操作可能なティーチングペンダント３００が接続されている。ティーチングペンダント３００は、入力された教示点のデータ（コンフィグレーション空間で指定された場合は各関節Ｊ１〜Ｊ６の目標角度）をインタフェース２１１及びバス２２０を介してＣＰＵ２０１に出力する。ＣＰＵ２０１は、ティーチングペンダント３００から入力を受けた教示点のデータをＨＤＤ２０４等の記憶部に設定する。

インタフェース２１２，２１３には、各種画像が表示されるモニタ３１１や書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３１２が接続されている。

インタフェース２１４，２１５には、入力側エンコーダ１０及び出力側エンコーダ１６がそれぞれ接続されている。入力側エンコーダ１０及び出力側エンコーダ１６は、前述したパルス信号をインタフェース２１４，２１５及びバス２２０を介してＣＰＵ２０１に出力する。

インタフェース２１６には、サーボ制御装置２３０が接続されている。ＣＰＵ２０１は、教示点間を繋ぐ軌道を生成して、モータ１の回転軸２の回転角度の制御量を示す駆動指令のデータを所定の時間間隔でバス２２０及びインタフェース２１６を介してサーボ制御装置２３０に出力する。

サーボ制御装置２３０は、ＣＰＵ２０１から入力を受けた駆動指令に基づき、フィードバック制御によるモータ１への電流の出力量を演算し、モータ１へ電流を供給して、ロボットアーム１０１の関節Ｊ１〜Ｊ６の関節角度制御を行う。即ち、ＣＰＵ２０１は、サーボ制御装置２３０を介して、関節Ｊ１〜Ｊ６の角度が目標角度となるように、モータ１による関節Ｊ１〜Ｊ６の駆動を制御する。

なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム２４０が格納される場合について説明するが、これに限定するものではない。プログラム２４０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム２４０を供給するための記録媒体としては、図３に示すＲＯＭ２０２や外部記憶装置３１２、記録ディスク２４１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。

ここで、後述する収束検出プログラムを実行する際に、ＣＰＵ２０１及びＨＤＤ２０４が有する機能について、図４を参照しながら説明する。図４は、第１実施形態に係るロボット装置５００の制御系を示す機能ブロック図である。ここで、図３に示す制御部（演算部）としてのＣＰＵ２０１は、プログラム２４０を実行することにより、図４に示す各演算部４０１，４０２，４０３，４０５として機能する。また、記憶手段としてのＨＤＤ２０４は、記憶部４０４として機能する。以下、各部について説明する。

理論出力角演算部４０１は、各入力側エンコーダ１０のパルス信号をカウントしてモータ１の回転角度θ_ｍ１〜θ_ｍ６を求める。つまり、理論出力角演算部４０１は、各入力側エンコーダ１０により検出されたモータ１の回転角度θ_ｍ１〜θ_ｍ６のデータを取得する。また、理論出力角演算部４０１は、減速比Ｎから減速機１１の出力角度、即ち関節Ｊ１〜Ｊ６の関節角度に換算した理論出力角度θ_１〜θ_６を演算する。具体的には、理論出力角演算部４０１は、θ（θ_１〜θ_６）＝θ_ｍ（θ_ｍ１〜θ_ｍ６）÷Ｎを演算する。この値θ_１〜θ_６を、サーボ制御装置２３０を介してフィードバックすることで、関節Ｊ１〜Ｊ６の角度が、ティーチングペンダント３００より入力された各関節Ｊ１〜Ｊ６の目標関節角度となるように制御される。

実出力角演算部４０２は、各出力側エンコーダ１６からパルス信号の入力を受け、各関節Ｊ１〜Ｊ６の実際の関節角度θ_Ｊ１〜θ_Ｊ６を求める。つまり、実出力角演算部４０２は、各出力側エンコーダ１６から各関節Ｊ１〜Ｊ６の実際の関節角度θ_Ｊ１〜θ_Ｊ６のデータを取得する。

先に述べた理論出力角度θ_１〜θ_６が、モータ１（減速機１１の入力軸）の回転角度θ_ｍ１〜θ_ｍ６を基に推定した関節Ｊ１〜Ｊ６（減速機１１の出力軸）の回転角度の理論値である。これに対し、関節角度θ_Ｊ１〜θ_Ｊ６は、各出力側エンコーダ１６が発するパルス信号を基に計算された（即ち各出力側エンコーダ１６により検出された）実測値であり、減速機１１の撓みによる振動を含んだ値となる。

先端位置演算部４０３は、実出力角演算部４０２により算出した関節角度θ_Ｊ１〜θ_Ｊ６に基づき、順運動学により、ロボット１００の先端（ＴＣＰ）の位置を演算する。

収束判定値記憶部４０４は、ロボット１００の先端位置における、位置決め完了位置として許容できるずれ量の幅（即ち、振動が収束したと見做せる許容範囲、収束幅）を記憶する。

収束判定部４０５は、先端位置演算部４０３および収束判定値記憶部４０４の値に基づき、ロボット１００の先端の収束（位置決め完了）を判定し、次動作へ移行する指令をサーボ制御装置２３０に与える。

ここで、図１に示すように、ロボット１００の先端（ＴＣＰ）を教示点Ｐ１，Ｐ２，Ｐ３の順に動作させて部品Ｗ１を部品Ｗ２に組付けて組付部品を製造する場合について説明する。図１において、教示点Ｐ１から教示点Ｐ２に向かう動作を第１動作、教示点Ｐ２から教示点Ｐ３に向かう動作を第２動作とする。制御装置２００は、ロボット１００を第１動作から次の第２動作に移行させる際に、ロボット１００の振動を収束させる目的で教示点Ｐ２の位置でロボット１００を一時的に停止させる。

つまり、制御装置２００は、第１動作では、各入力側エンコーダ１０により検出された検出角度が、コンフィグレーション空間で指定された教示点Ｐ２（つまり、各関節Ｊ１〜Ｊ６の目標角度）となるようにフィードバック制御する。制御装置２００は、入力側エンコーダ１０の検出結果に基づいてフィードバック制御するので、出力側エンコーダ１６の検出結果に基づいてフィードバック制御する際よりも速く教示点Ｐ２に到達する。しかし、ロボット１００は、第１動作に伴う慣性力の影響を受け、教示点Ｐ２に到達したときにロボット１００が減速機１１の撓み等により振動する。この減速機１１による振動は、モータ１の角度（入力側エンコーダ１０による検出角度）では正確に求めることは困難である。ロボット１００の振動が収束していない状態で第２動作、即ち部品Ｗ１を部品Ｗ２に組付けて組立部品を製造する動作を行うと、組み付けが失敗することがある。

そこで、ロボットの振動を実測し、振動が所定の収束幅（許容値）以内に収束したか否かを許容値判定することも考えられる。しかし、仮に、許容値判定を行う場合、ロボットの振動が許容値以下となる瞬間があっても、次の振動のピークで許容値を超えることもあるため、少なくとも振動の１周期以上の時間、待機する必要がある。

よって、第１実施形態では、ロボット１００に第１動作を行わせて教示点Ｐ２に到達した時点から、ロボット１００の先端の振動の収束時間を推定して、推定した収束時間、一時停止させた後に、次の第２動作を実行するようにしている。この収束時間は、前述した収束判定部４０５が推定する。

図５は、本発明の第１実施形態に係るロボット装置におけるロボット制御方法（収束時間検出方法）の各工程を示すフローチャートである。まず、サーボ制御装置２３０は、ＣＰＵ２０１の指令によりロボット１００に第１動作（検出対象動作）を実行させる（Ｓ１：ロボット動作工程）。サーボ制御装置２３０は、ＣＰＵ２０１からの指令に基づき、入力側エンコーダ１０からの位置情報をフィードバックして制御する。即ちサーボ制御装置２３０は、ＣＰＵ２０１の制御の下、各入力側エンコーダ１０により検出された回転角度θ_ｍ１〜θ_ｍ６が、ロボット１００の先端が第１動作の動作完了位置に移動する目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊となるよう、各モータ１の回転を制御する。これにより、ロボット１００（ロボットアーム１０１）の先端（ＴＣＰ）が教示点Ｐ２に近づけられる。

なお、回転角度θ_ｍ１〜θ_ｍ６を目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊に制御することは、回転角度θ_ｍ１〜θ_ｍ６を減速比Ｎで割った角度θ_１〜θ_６を、目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊を減速比Ｎで割った目標角度θ_１ ^＊〜θ_６ ^＊に制御することと同義である。目標角度θ_１ ^＊〜θ_６ ^＊は、目標関節角度である。また、第１動作の「動作完了位置」とは、タスク空間で指定された教示点Ｐ２のワールド座標系Σにおける位置座標ｘ_０，ｙ_０，ｚ_０であり、以下、「目標位置」ともいう。

収束判定部４０５は、収束判定値記憶部４０４より、対象動作の収束判定値（許容範囲の上下限値を示す許容値）を読み出す（Ｓ２）。収束判定部４０５は、この値を、検出対象動作におけるロボット１００の先端の目標位置の各ｘ，ｙ，ｚ軸方向の位置ｘ_０，ｙ_０，ｚ_０を中心値として分布させることで、収束幅（許容範囲）を設定する。

図６は、ワールド座標系Σにおけるｘ，ｙ，ｚ軸方向の各方向におけるロボット１００の先端の振動状況を説明するためのグラフである。収束判定値は各ｘ，ｙ，ｚ軸方向に、必要とされる精度に応じてユーザが事前に設定する（記憶部であるＨＤＤ２０４に記憶させる）。

図６における各方向のグラフは、上からワールド座標系におけるロボット１００（即ちロボットアーム１０１）の先端のｘ軸方向、ｙ軸方向、ｚ軸方向の位置について表しており、縦軸は位置、横軸は経過時間ｔである。横軸（時間軸）の起点０は、サーボ制御装置２３０からモータ１への指令が完了した時点、即ち各モータ１の回転角度θ_ｍ１〜θ_ｍ６を目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊となるように制御した時点である。

ｘ軸方向については、目標位置（動作完了位置）がｘ_０であり、その値ｘ_０を中心として、収束幅（許容範囲）−ｘｌｉｍ〜＋ｘｌｉｍが設定されている。即ち、許容範囲は、［ｘ_０−ｘｌｉｍ，ｘ_０＋ｘｌｉｍ］である。ロボット１００の先端の位置が収束幅（許容範囲）内に収束し、収束したと判定される収束時間がｔ_ｘである。

ｙ軸方向については、目標位置（動作完了位置）がｙ_０であり、その値ｙ_０を中心として、収束幅（許容範囲）−ｙｌｉｍ〜＋ｙｌｉｍが設定されている。即ち、許容範囲は、［ｙ_０−ｙｌｉｍ，ｙ_０＋ｙｌｉｍ］である。ロボット１００の先端の位置が収束幅（許容範囲）内に収束し、収束したと判定される収束時間がｔ_ｙである。

ｚ軸方向については、目標位置（動作完了位置）がｚ_０であり、その値ｚ_０を中心として、収束幅（許容範囲）−ｚｌｉｍ〜＋ｚｌｉｍが設定されている。即ち、許容範囲は、［ｚ_０−ｚｌｉｍ，ｚ_０＋ｚｌｉｍ］である。ロボット１００の先端の位置が収束幅（許容範囲）内に収束し、収束したと判定される収束時間がｔ_ｚである。

ここで例えば、第１動作の次の第２動作において、ｘ軸方向のみに高い精度が必要とされる場合、第１動作完了時におけるｘ軸方向の振動量は、次の第２動作の精度に強く影響する。この場合、ｘ軸方向の収束判定値＋ｘｌｉｍ，−ｘｌｉｍの絶対値は小さく設定する。残りのｙ，ｚ軸方向については、次の第２動作の精度にあまり影響しないため、なるべく早く次の第２動作に移行させるように収束判定値の絶対値を大きく設定する。

以上、第１実施形態では、サーボ制御装置２３０に目標位置ｘ_０，ｙ_０，ｚ_０（具体的には各モータ１の目標角度又は該目標角度を減速比Ｎで割った角度）が指令されたとき、目標位置ｘ_０，ｙ_０，ｚ_０を中心として図６のように収束幅が設定される。この収束幅の大きさについてはユーザが任意で設定可能である。収束幅は、動作精度が要求される場合には小さく、動作速度が要求される場合には大きく設定すればよい。

図６に示すように、ｙ，ｚ軸方向の振動が大きいものの、次の第２動作に高い精度が必要とされないのであれば、収束判定値を大きく設定することで収束時間ｔ_ｙ，ｔ_ｚが短くなり、次の第２動作に早く移行することができる。よってロボット１００を効率的に動作させることができる。収束判定値は、別途実験で求めた値を設定してもよいし、事前に数段階用意した値からユーザが選択できるようにしてもよい。ここで、収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚは、後のステップＳ６で推定により求める。

次に、収束判定部４０５は、各モータ１の回転角度（入力側エンコーダ１０の検出角度）θ_ｍ１〜θ_ｍ６が目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊となった時点で、タイマ２０６に計時を開始させる。また、収束判定部４０５は、各モータ１の回転角度θ_ｍ１〜θ_ｍ６が目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊となった時点で、次の第２動作に移行しないよう、ロボット１００の各モータ１の回転動作（駆動）を一時停止させる（Ｓ３）。

次に、実出力角演算部４０２は、各関節Ｊ１〜Ｊ６の関節角度θ_Ｊ１〜θ_Ｊ６を、各出力側エンコーダ１６からのパルス信号から求める。即ち、実出力角演算部４０２は、各出力側エンコーダ１６により検出された各関節Ｊ１〜Ｊ６の関節角度θ_Ｊ１〜θ_Ｊ６を取得する（Ｓ４）。

次に、先端位置演算部４０３は、各出力側エンコーダ１６により検出された関節角度θ_Ｊ１〜θ_Ｊ６に基づきロボット１００（ロボットアーム１０１）の先端（ＴＣＰ）の位置を求める（Ｓ５：先端位置算出工程）。以上のステップＳ４，Ｓ５は、第１動作におけるモータ１への動作指令が完了した段階でスタートする。このとき、ロボット１００の各モータ１の動作は停止しているが、減速機１１から先、即ちロボット１００の先端が振動している状態である。ロボット１００の先端の位置は、各関節の関節角度θ_Ｊ１〜θ_Ｊ６を順運動学で計算し得られるものである。

次に、収束判定部４０５は、ロボット１００の先端の位置が収束幅以内（許容範囲内）に収束する収束時間を推定する（Ｓ６：推定工程）。ここで、各モータ１の回転角度θ_ｍ１〜θ_ｍ６を目標角度θ_ｍ１ ^＊〜θ_ｍ６ ^＊に制御した時点をｔ＝０とする。ロボット１００の先端における動作完了位置に対する振れ幅が許容範囲内に収束する時点をｔ＝ｔ_ｘ，ｔ_ｙ，ｔ_ｚとする。よって、収束時間はｔ_ｘ，ｔ_ｙ，ｔ_ｚである。収束判定部４０５は、ステップＳ６において、収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚを、ステップＳ５にて算出したロボット１００の先端の位置の時間変化に基づき推定する推定処理を実行する。

ここで、ステップＳ６の推定工程について具体的に説明する。図７は、本発明の第１実施形態に係るロボット制御方法における推定工程を示すフローチャートである。図８は、推定工程を説明するための概念図である。図８のグラフにおいて、横軸は時間ｔ、縦軸は先端位置ｘ，ｙ，ｚの３軸方向のうち１軸方向（ｘ軸方向）の先端位置ｘを示している。なお、他の軸（ｙ，ｚ軸）についても同様であるので、説明を省略する。

図８に示すように、ロボット１００（ロボットアーム１０１）の先端位置は、目標位置ｘ_０を中心とする収束幅（２本の破線の間の許容範囲）の内外を往復しながら徐々に減衰して目標位置ｘ_０に収束する。よって、第１実施形態では、ロボット１００の振動で収束幅を逸脱しない時間ｔ_ｘを推定する。

具体的に説明すると、まず、ＣＰＵ２０１は、ステップＳ５にて算出したロボット１００の先端の位置ｘの時間変化によりロボット１００の先端の振動の少なくとも２つ、本実施形態では２つのピーク値ｘ_１，ｘ_２を算出する（Ｓ６１）。具体的に説明すると、ＣＰＵ２０１は、所定の時間間隔で各出力側エンコーダ１６から関節角度θ_Ｊ１〜θ_Ｊ６を取得して先端位置を求めており、先端位置が各ピーク値ｘ_１，ｘ_２を過ぎてｘ_０に近づいた時点で各ピーク値ｘ_１，ｘ_２を割り出す。即ち、ＣＰＵ２０１は、時系列で先に割り出した少なくとも２つのピーク値ｘ_１，ｘ_２を求める。

次に、ＣＰＵ２０１は、ピーク値ｘ_１，ｘ_２に基づき、収束時間ｔ_ｘを推定する。具体的に説明すると、まず、ＣＰＵ２０１は、ピーク値ｘ_２を割り出した後、２つのピーク値ｘ_１，ｘ_２から対数減衰率を算出する（Ｓ６２）。対数減衰率は、隣り合う振幅の比の自然対数で表される。図８のロボットアーム先端位置の波形においては、対数減衰率σは以下の式で表される。

次いで、ＣＰＵ２０１は、算出した対数減衰率から収束時間ｔ_ｘを算出する（Ｓ６３）。

以上のステップＳ６１〜Ｓ６３により、収束時間ｔ_ｘが推定される。収束時間ｔ_ｙ，ｔ_ｚについても同様に推定する。なお、推定方法として対数減衰率を求める場合について説明したが、これに限定するものではない。先端位置の複数のサンプリングデータから減衰波形の関数を求めて、収束時間を推定してもよい。また、２つのピーク値から次以降のピーク値及びそのタイミングを推定して、収束時間を推定してもよい。

このように、第１実施形態では、ＣＰＵ２０１は、ワールド座標系Σにおける各ｘ，ｙ，ｚ軸方向について各々収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚを推定する。

次に、図５においてステップＳ６で収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚの推定を終えた後、収束判定部４０５は、収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚが経過したか否かを判断する（Ｓ７）。具体的には、収束判定部４０５は、収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚのうち、最も長い収束時間（第１実施形態では収束時間ｔ_ｘ）が経過したか否かを判断する。

収束判定部４０５は、タイマ２０６による計時が収束時間ｔ_ｘを経過していなければ（Ｓ７：Ｎｏ）、そのままロボット１００の各モータ１の一時停止を続行する。収束判定部４０５は、タイマ２０６による計時が収束時間ｔ_ｘに達したと判断した場合には（Ｓ７：Ｙｅｓ）、次の第２動作をロボット１００に行わせる（Ｓ８）。

第２動作は、ステップＳ７で肯定判断をしたタイミングでロボット１００に行わせるのが好ましいが、これに限定するものではない。つまり、ステップＳ７で肯定判断した場合は、次の第２動作が可能な状態となったものであり、実際に第２動作を行うのは、肯定判断した後であればどのタイミングでもよい。第２動作は、肯定判断した後に次の振動のピークを迎える前に開始するのが好ましい。

以上、ステップＳ３及びステップＳ７にて、ＣＰＵ２０１は、ロボットに第２動作を行わせる前に、少なくとも各モータ１の回転角度を目標角度に制御した時点から推定した収束時間ｔ_ｘに達する時点まで、各モータ１の回転動作を一時停止させる。つまり、ＣＰＵ２０１は、ステップＳ３及びステップＳ７にて一時停止工程を実施する。

第１実施形態によれば、少なくともステップＳ６で推定した収束時間の分、各モータ１の回転動作を一時停止させるので、予め定めた所定時間の分、各モータ１の回転動作を一時停止させるよりも一時停止させておく時間を短縮できる。そして、少なくとも収束時間の分、ロボットの各モータの回転動作を停止させておけば、ロボット１００の先端の振動は許容範囲に収束するので、次の第２動作におけるロボット１００の動作精度を確保することができる。そして、所定時間とする場合に比べて余分な停止時間を削減することができるので、第１動作及び第２動作の一連のロボット１００の動作が短時間となり、ロボット１００による組立部品の組立作業の効率が向上する。

また、第１実施形態では、ロボット１００の先端の振動のピーク値を少なくとも２つ割り出して収束時間を推定しているので、次以降のピーク値を減衰特性に基づいて容易に求めることができ、推定される収束時間の精度がより向上する。特に、対数減衰率を計算することで、２つのピーク値だけで済み、より迅速に収束時間を求めることができる。

また、３軸の収束時間ｔ_ｘ，ｔ_ｙ，ｔ_ｚのうち最も長い収束時間ｔ_ｘにタイマ２０６のカウントが達するまでロボット１００の各モータ１を停止させておくので、収束時間ｔ_ｙ，ｔ_ｚは経過しており、全ての軸方向で振動が許容範囲に収束していることとなる。これにより、ロボット１００の次の第２動作の動作精度が向上する。

更に、第１実施形態によれば、各モータ（減速機の入力側）の回転角度に基づいてロボットの先端の位置を算出する場合と比較して、ロボット１００の先端の位置を正確に求めることができる。よって、推定の収束時間を正確に求めることができ、これにより、ロボット１００の作業効率が向上する。

また、許容範囲は、ワールド座標系Σの各軸方向で個別に設定可能であるので、動作精度を要求されない方向の収束判定値（許容範囲）を大きく設定することができ、収束時間を短縮でき、ロボット１００の作業効率が向上する。

なお、第１実施形態では、３軸全てについて収束時間を推定する場合について説明したが、これに限定するものではなく、例えばロボット１００の特性上、収束時間が最も長くなる軸方向が分かっていれば、その方向の収束時間のみ推定してもよい。この場合、推定した収束時間のみを用いてロボット１００の停止時間を設定すればよい。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置におけるロボット制御方法について説明する。図９は、本発明の第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、ロボット装置の全体構成は、第１実施形態のロボット装置と同様であるため説明を省略する。第２実施形態では、制御装置２００のＣＰＵ２０１による制御動作、即ちプログラム２４０が上記第１実施形態と異なる。以下、第２実施形態において、上記第１実施形態と異なる点を中心に説明する。

ロボット１００（ロボットアーム１０１）を長時間使用すると、減速機１１、タイミングベルト（不図示）など、ロボット１００（ロボットアーム１０１）を構成する各部品の経時変化の影響が現れる。ロボット１００が経時劣化するに連れ、ロボット１００（ロボットアーム１０１）の先端の振動の収束時間も長く変化する。

そこで、第２実施形態では、実測した収束時間の経時変化に基づき、ロボット１００（ロボットアーム１０１）の経時劣化を、ロボット１００を分解することなく検出する方法について述べる。

以下、図９に示す各部について説明する。第２実施形態では、ＣＰＵ２０１は、上記第１実施形態で説明した図４の各演算部４０１，４０２，４０３，４０５として機能するほか、各演算部４０６，４０９として機能する。更に、ＨＤＤ２０４は、上記第１実施形態で説明した図４の記憶部４０４として機能するほか、各記憶部４０７，４０８として機能する。なお、第２実施形態において、上記第１実施形態と共通する各部４０１〜４０５については説明を省略する。図９に示す各演算部４０１，４０２，４０３，４０５，４０６，４０９は、ＣＰＵ２０１が、プログラム２４０を実行することにより機能する。

収束時間計測部４０６は、サーボ制御装置２３０による動作指令完了の時点から、収束判定部４０５による収束判定の時点までの実際の収束時間を計測する。収束時間記憶部４０７は、収束時間計測部４０６により求めた収束時間を、経時変化Ｔ_ｆのデータとして記憶する。ここで、経時変化Ｔ_ｆとは、初期動作時の収束時間を基準とした、現時点での収束時間の時間変化分（又は時間変化分に比例した値）である。初期動作時の収束時間は、最初にロボット１００を動作させたときの収束時間とするのが好ましいが、ユーザが指定した収束時間（例えば、ユーザが指定した回数、ロボット１００を動作させたときの収束時間）としてもよい。

判定値記憶部４０８は、実測した収束時間の経時変化Ｔ_ｆに対する判定値（許容値）を記憶する。第２実施形態では、判定値（許容値）は数段階用意してもよく、具体的には、判定値記憶部４０８は、警告のみをユーザに通知する段階の通知判定値Ｔ_１と、警告に加えてロボット１００の停止が必要な段階の停止判定値Ｔ_２とを記憶（設定）する。ここで、判定値Ｔ_１＜判定値Ｔ_２である。各判定値Ｔ_１，Ｔ_２は、収束時間とロボット１００の劣化の関係を事前に実験的に求めて設定してもよいし、またロボット１００の動作を含めた生産のタクトタイムに対する収束時間の影響度合いにより設定してもよい。

判定処理部４０９は、収束時間記憶部４０７に記憶された収束時間の経時変化Ｔ_ｆと、判定値記憶部４０８に記憶された判定値Ｔ_１，Ｔ_２とを比較して、警告指令及び／又はロボット１００の停止指令を出す。この処理については後に説明する。

収束時間の経時変化Ｔ_ｆの処理方法としては、例えば移動平均を用いるとよい。警告指令はユーザが確認容易な、例えばティーチングペンダント３００やモニタ３１１等の通知手段に表示（通知）させるのがよい。ロボット１００（ロボットアーム１０１）の停止指令はサーボ制御装置２３０に送る。

図１０は、判定処理部４０９における経時変化Ｔ_ｆのデータの処理を説明するための図である。図１０に示す縦軸は収束時間の経時変化Ｔ_ｆ、横軸は対象動作の実行回数である。図１０の例において判定値は２種類設定されている。一つは収束時間の経時変化Ｔ_ｆについてユーザに警告通知を行う通知判定値Ｔ_１、もう一つはユーザへの警告に加え、ロボット１００（ロボットアーム１０１）の停止を行う停止判定値Ｔ_２である。対象動作を実行し、ｎ_１回目において収束時間の経時変化Ｔ_ｆが判定値Ｔ_１を超えたと判断した場合、ユーザへ警告を発する。またｎ_２回目において収束時間の経時変化Ｔ_ｆが判定値Ｔ_２を超えたと判断した場合、ユーザへの警告発報に加えてロボット１００の停止の措置を行う。

図１１は、本発明の第２実施形態に係るロボット装置におけるロボット制御方法の各工程を示すフローチャートである。以下、第２実施形態の処理の流れを説明する。ここでステップＳ２１〜Ｓ２７までは、第１実施形態で説明したステップＳ１〜Ｓ７と同様であるので省略する。

収束時間計測部４０６は、ステップＳ２５で算出したロボット１００の先端の振動が許容範囲内に収束する実際の収束時間（実収束時間）を計算（計測）する（Ｓ２８）。この実収束時間は、サーボ制御装置２３０による動作指令完了の時点から、収束判定部４０５によりロボット１００の振動が実際に許容範囲内に収束したとする収束判定の時点までの時間である。

判定処理部４０９は、判定値記憶部４０８に事前に記憶されている判定値Ｔ_１，Ｔ_２の読み出しを行う（Ｓ２９）。

次に、判定処理部４０９は、経時変化Ｔ_ｆを実測した収束時間を処理して求める（Ｓ３０）。即ち、収束時間計測部４０６は、計測した収束時間を、収束時間記憶部４０７へ記憶させている。判定処理部４０９は、収束時間記憶部４０７から読み出した収束時間の経時変化Ｔ_ｆを、移動平均などを用いて求める。

判定処理部４０９は、経時変化Ｔ_ｆが通知判定値Ｔ_１を超えているか否かを判断する（Ｓ３１：通知判断工程）。

判定処理部４０９は、経時変化Ｔ_ｆが通知判定値Ｔ_１を超えていないと判断した場合（Ｓ３１：Ｎｏ）、上記第１実施形態で説明したステップＳ８と同様、ロボット１００に第２動作を実行させる（Ｓ３２）。

判定処理部４０９は、経時変化Ｔ_ｆが通知判定値Ｔ_１を超えていると判断した場合（Ｓ３１：Ｙｅｓ）、通知手段であるモニタ３１１又はティーチングペンダント３００に警告の通知（表示）を行わせる（Ｓ３３：通知工程）。

次に、判定処理部４０９は、経時変化Ｔ_ｆが停止判定値Ｔ_２を超えているか否かを判断する（Ｓ３４：停止判断工程）。

判定処理部４０９は、経時変化Ｔ_ｆが停止判定値Ｔ_２を超えていないと判断した場合（Ｓ３４：Ｎｏ）、上記第１実施形態で説明したステップＳ８と同様、ロボット１００に第２動作を実行させる（Ｓ３２）。

判定処理部４０９は、経時変化Ｔ_ｆが停止判定値Ｔ_２を超えていると判断した場合（Ｓ３４：Ｙｅｓ）、ロボット１００の動作を非常停止させる（Ｓ３５：非常停止工程）。

以上、第２実施形態によれば、収束時間の経時変化Ｔ_ｆのデータについて判定値Ｔ_１で判断することにより、ロボット１００を分解することなくロボット１００の劣化状態をユーザが判断することができる。また、収束時間の経時変化Ｔ_ｆのデータについて判定値Ｔ_２で判断し、劣化したロボット１００を停止することにより、劣化状態にあるロボット１００で組立作業を行うのを回避することができる。以上、第２実施形態によれば、ロボット１００の劣化状態を判定値Ｔ_１及び判定値Ｔ_２により２段階で判断しているため、メンテナンス性が向上する。

なお、第２実施形態では、判定値Ｔ_１，Ｔ_２で判断する場合について説明したが、これに限定するものではなく、判定値Ｔ_１のみ又は判定値Ｔ_２のみで判断する場合であってもよい。即ち、ステップＳ３１，Ｓ３３又はステップＳ３４，Ｓ３５は省略可能である。ステップＳ３１，Ｓ３３を省略した場合、ステップＳ３５にてロボット１００の停止に加えて、ステップＳ３３と同様、警告を通知してもよい。

また、第２実施形態では、経時変化Ｔ_ｆを求める収束時間として、実測した収束時間を用いる場合について説明したが、ステップＳ２６で推定した収束時間を用いてもよい。この場合、収束時間の実測は省略可能となる。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

［その他の実施形態］
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

また、上記実施形態においては、多関節のロボット１００が、垂直多関節のロボットである場合について説明したが、水平多関節のロボット（スカラロボット）、パラレルリンクロボットなどであってもよい。

１…モータ、１６…出力側エンコーダ（関節角度検出器）、１００…ロボット、２００…制御装置（制御手段）、５００…ロボット装置

Claims (12)

1. 制御手段が、各関節を駆動するモータと、前記各関節の角度を検出する関節角度検出器とを有する多関節のロボットの動作を制御するロボット制御方法であって、
   前記制御手段が、前記各モータの回転角度が目標角度となるように前記各モータの回転を制御して、前記各モータの回転角度が前記目標角度に達した後に前記各モータの回転動作を一時停止させる第１動作工程と、
   前記制御手段が、前記各関節角度検出器により検出された関節角度に基づき、前記ロボットの所定箇所の位置を算出し、前記ロボットの所定箇所の位置の時間変化に基づき、前記ロボットの所定箇所における第１方向の振動と、前記第１方向とは異なる方向である第２方向の振動の各々が許容範囲内に収束するまでの収束時間を各々推定する推定工程と、
   前記制御手段が、前記第１方向および前記第２方向の振動の各々が前記収束時間に達した後に、前記モータの回転を制御して前記ロボットを動作させる第２動作工程と、を備えたことを特徴とするロボット制御方法。
2. 前記ロボットの所定箇所は、前記ロボットの先端であることを特徴とする請求項１に記載のロボット制御方法。
3. 前記推定工程では、前記制御手段が、算出した前記ロボットの先端の位置の時間変化により前記ロボットの先端の振動のピーク値を求め、少なくとも２つのピーク値に基づき、前記収束時間を推定することを特徴とする請求項２に記載のロボット制御方法。
4. 前記推定工程では、前記制御手段が、前記少なくとも２つのピーク値から対数減衰率を求め、該対数減衰率から前記収束時間を求めることを特徴とする請求項３に記載のロボット制御方法。
5. 前記制御手段が、前記推定工程にて推定した前記収束時間、又は実際に計測した実収束時間の経時変化が、予め設定した通知判定値を超えているか否かを判断する通知判断工程と、
   前記制御手段が、前記経時変化が前記通知判定値を超えていると判断した場合は、警告を通知する通知工程と、を更に備えたことを特徴とする請求項１乃至４のいずれか１項に記載のロボット制御方法。
6. 前記制御手段が、前記推定工程にて推定した前記収束時間、又は実際に計測した実収束時間の経時変化が予め設定した停止判定値を超えているか否かを判断する停止判断工程と、
   前記制御手段が、前記経時変化が前記停止判定値を超えていると判断した場合は、前記ロボットの動作を停止させる非常停止工程と、を更に備えたことを特徴とする請求項５に記載のロボット制御方法。
7. 前記関節角度検出器はエンコーダである請求項１乃至６のいずれか１項に記載のロボット制御方法。
8. 前記ロボットは、前記モータの回転を減速して出力する減速機を更に備え、前記エンコーダは前記減速機の出力軸側に備えられていることを特徴とする請求項７に記載のロボット制御方法。
9. 各関節を駆動するモータと、前記各関節の角度を検出する関節角度検出器と、を有する多関節のロボットと、
   前記ロボットの前記各モータの回転を制御して前記ロボットの動作を制御する制御手段と、を備え、
   前記制御手段が、請求項１乃至８のいずれか１項に記載のロボット制御方法の各工程を実行することを特徴とするロボット装置。
10. コンピュータに、請求項１乃至８のいずれか１項に記載のロボット制御方法の各工程を実行させるためのプログラム。
11. 請求項１０に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
12. 請求項１乃至８のいずれか１項に記載のロボット制御方法の各工程を実行して前記ロボットにより物品を製造する方法。

Images