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

Abstract

【課題】ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減する。【解決手段】各モータの回転角度を各角度指令値に追従させてロボットアームを動作させる（Ｓ１）。ロボットアームを動作させたときに各関節角度値に基づき、ロボットアームの先端の実際の軌道を求める（Ｓ２）。ロボットアームの先端の目標の軌道に対する実際の軌道の軌道誤差が許容範囲を超えているか否かを判断する（Ｓ４）。ステップＳ４で軌道誤差が許容範囲を超えていると判断した場合、各関節の追従遅れ時間を求める（Ｓ５）。各関節の追従遅れ時間の分、初期の各角度指令値を補正した角度指令値を生成する（Ｓ６）。補正した各角度指令値を用いて同様の処理を再度実行し（Ｓ７〜Ｓ９）、再度軌道誤差が許容範囲を超えていると判断した場合（Ｓ１０：Ｙｅｓ）、基準時間を変更して再度、初期の各角度指令値を補正する（Ｓ１３）。【選択図】図６

Description

本発明は、各関節のモータの回転角度が各角度指令値に近づくようにロボットアームの動作を制御するロボット装置の制御方法、ロボット装置、プログラム及び記録媒体に関する。

精密機器の自動組立において、高精度かつ高速に組付けを行う産業用のロボットが用いられている。ロボットは、多関節のロボットアームと、ロボットアームの先端に取り付けられたロボットハンドとを有する。ロボットアームは、各関節を駆動する複数の駆動部を有する。各駆動部は、モータと、モータの回転軸の回転を減速する減速機とを有する。

一般に、モータの回転軸の回転角度が角度指令値に近づくように制御するフィードバック制御が行われている。角度指令値は、コンピュータ上のシミュレーションにより作成される。そのため、ロボット自体の製造公差や撓み等により、複数の駆動部を連動させた際に、角度指令値に基づくロボットアームの先端の目標の軌道と運転時のロボットアームの先端の実際の軌道とに差分（以下、軌道誤差という）が生じる。このように軌道誤差が生じると、組立製品の嵌め合い公差幅を超えることがあり、組み付けができなくなることがあった。

この軌道誤差は、ロボットアームの共振振動に起因するものと、ロボットアームの各関節の関節角度が目標の関節角度（関節角度指令値）に達するまでの時間差（以下、追従遅れ時間という）のばらつきに起因するものとがある。

軌道誤差を低減する方法については、特許文献１で提案されている。特許文献１では、追従遅れ時間に相当するサーボ遅れ時間を関節軸毎に求め、これらサーボ遅れ時間の中で最も短いサーボ遅れ時間又は平均の時間を基準時間として、該基準時間に基づいて補償トルクを計算する。そして、補償トルクを反映した指令値を各軸のサーボに出力し、サーボ遅れ時間が全ての軸で一致するように制御している。

特許第４９９２７０２号公報

しかしながら、特許文献１では、モータの出力軸の回転を検出するロータリエンコーダから各関節の回転角度を検出しており、ロボットアームの関節に撓みが生じた場合には、実際の関節角度とは異なる値となる。そのため、ロボットアームの軌道誤差が解消しない場合もあった。

また、特許文献１では、各関節のサーボ遅れ時間を基準時間に合わせることでサーボ遅れ時間のばらつきを解消することができたとしても、決定される基準時間によってはロボットアームの共振振動による軌道誤差が許容範囲を超える場合があった。このような場合、特許文献１では、ロボットアームの動作速度を低減させていたため、ロボットによる組立作業に要する時間が長くなり、組立製品の生産性が低下していた。

そこで、本発明は、ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減することを目的とする。

本発明は、複数の関節をそれぞれ駆動する複数のモータと、前記複数の関節の角度をそれぞれ検出する複数の関節角度検出部と、を有するロボットアームと、前記ロボットアームの動作を制御する制御部と、を備えたロボット装置の制御方法であって、前記制御部が、前記各モータの回転角度を各角度指令値に追従させて前記ロボットアームを動作させる動作工程と、前記制御部が、前記動作工程にて前記ロボットアームを動作させたときに前記各関節角度検出部により検出された関節角度値に基づき、前記ロボットアームの先端の実際の軌道を求める軌道算出工程と、前記制御部が、前記角度指令値に基づく前記ロボットアームの先端の目標の軌道に対する、前記軌道算出工程で求めた実際の軌道の軌道誤差が、許容範囲を超えているか否かを判断する判断工程と、前記制御部が、初期の前記各角度指令値を関節の角度に換算した各関節角度指令値に対する前記各関節角度値の追従遅れ時間を、関節ごとに求める追従遅れ時間算出工程と、前記制御部が、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、前記各関節の追従遅れ時間のうちの最長の追従遅れ時間以下の基準時間に対する前記各関節の追従遅れ時間のずれ時間の分、初期の前記各角度指令値を補正する補正工程と、を備え、前記制御部が、前記補正工程にて補正した前記各角度指令値を用いて前記動作工程、前記軌道算出工程及び前記判断工程を再度実行し、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合に前記補正工程にて前記最長の追従遅れ時間以下の範囲で前記基準時間を変更して再度、初期の前記各角度指令値を補正することを特徴とする。

本発明によれば、ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減することができる。

本発明の実施形態に係るロボット装置を示す斜視図である。 ロボットアームの第２関節を示す部分分解斜視図である。 ロボット装置の制御系を示すブロック図である。 ロボット装置の制御装置の上位コントローラの構成を示すブロック図である。 ロボット装置の制御系を示す機能ブロック図である。 ロボット装置の制御系による制御動作を示すフローチャートである。 ロボットアームの先端位置の変位を経過時間毎に表わしたグラフである。 経過時間に対する第２関節及び第３関節の関節角度指令値及び関節角度値を示すグラフである。 経過時間に対する第２関節及び第３関節の追従遅れ時間を示すグラフである。 経過時間に対する第２関節及び第３関節の初期の角度指令値及び補正後の角度指令値を示すグラフである。 ロボットアームの先端位置の変位を経過時間毎に表わしたグラフである。 ロボットアームの先端の軌道の周波数特性を示すグラフである。 ロボットアームの各関節の追従遅れ時間を示すグラフである。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、本発明の実施形態に係るロボット装置を示す斜視図である。図１に示すロボット装置５００は、把持したワークＷを別のワークに組付ける組立作業を行うロボット１００と、ロボット１００の動作を制御する制御装置２００と、教示装置であるティーチングペンダント３００と、を備えている。

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

ロボットアーム１０１は、作業台に固定されるベース部（基端リンク）１１１と、変位や力を伝達する複数のリンク１１２〜１１７と、を有している。ベース部１１１及び複数のリンク１１２〜１１７は、複数の関節Ｊ_１〜Ｊ_６で旋回又は回転可能に互いに連結されている。

また、ロボットアーム１０１は、各関節Ｊ_１〜Ｊ_６に設けられた、関節Ｊ_１〜Ｊ_６をそれぞれ駆動する複数（本実施形態では６つ）の駆動部１２０を有している。各関節Ｊ_１〜Ｊ_６に配置された駆動部１２０は、必要なトルクの大きさに合わせて適切な出力のものが用いられる。

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

ロボットアーム１０１の先端には、ツールセンターポイント（ＴＣＰ）が設定されており、教示点がタスク空間で規定される場合、その教示点は、ＴＣＰの位置及び姿勢を示す６つのパラメータで表現される。具体的には、教示点は、タスク空間で表すと、３つの位置パラメータ（Ｘ，Ｙ，Ｚ）と、３つの姿勢パラメータ（ｔＸ，ｔＹ，ｔＺ）とからなる６つのパラメータで表現される。教示点がコンフィグレーション空間（関節空間）で規定される場合、その教示点は、ロボット１００の各関節Ｊ_１〜Ｊ_６の関節位置（関節角度）を示すパラメータ（θ_１，θ_２，θ_３，θ_４，θ_５，θ_６）で表現される。

なお、コンフィグレーション空間で規定された教示点は、順運動学計算により、タスク空間に変換することができ、タスク空間で規定された教示点は、逆運動学計算により、コンフィグレーション空間に変換することができる。

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

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

図２は、ロボットアーム１０１の第２関節である関節Ｊ_２を示す部分分解斜視図である。駆動部１２０は、サーボモータであるモータ１２１と、モータ１２１のトルクを増大させるためにモータ１２１の回転を減速比Ｎで減速する減速機１２３と、減速機１２３にモータ１２１の回転駆動を伝達するベルト１２４と、を有している。モータ１２１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。

モータ１２１の回転軸１２１Ａには、モータ１２１の回転軸１２１Ａの回転角度を検出するモータ角度検出部であるロータリエンコーダ（以下、「エンコーダ」という）１２２が取り付けられている。エンコーダ１２２は、モータ１２１の回転軸１２１Ａの一端に設けられている。回転軸１２１Ａの他端には、駆動プーリ１２６が設けられている。エンコーダ１２２は、モータ１２１の回転軸１２１Ａの回転に伴って、パルス信号を発生して、図１の制御装置２００に出力する。エンコーダ１２２は、インクリメンタル型のロータリエンコーダでもよいが、モータ１２１の所定の回転位置を原点とした絶対角度が検出できるアブソリュート型のロータリエンコーダであるのが望ましい。なお、エンコーダ１２２は、回転軸１２１Ａに取り付けられているが、減速機１２３の入力軸１２３Ｂに取り付けてもよい。

減速機１２３の入力軸１２３Ｂには、従動プーリ１２７が取り付けられ、駆動プーリ１２６と従動プーリ１２７とに無端状のベルト１２４が架け渡されている。これにより、駆動プーリ１２６の回転駆動力は減速機１２３の入力軸１２３Ｂに取り付けられた従動プーリ１２７へベルト２０４を介して伝達される。組立作業等を行うロボットでは正確な位置決めが求められるので、ベルト１２４は歯付ベルトであることが望ましい。

減速機１２３には、小型軽量で減速比Ｎの大きい波動歯車減速機を用いるのが望ましい。減速機１２３は、ベルト１２４を介して伝達されたモータ１２１の回転を減速する。減速機１２３の筐体は、リンク１１３に固定され、減速機１２３の出力軸１２３Ａは、リンク１１２（図１）に接続される。ベルト１２４による減速或いは増速がない場合、モータ１２１の回転軸１２１Ａの回転は、減速機１２３により１／Ｎに減速される。本実施形態ではベルト１２４による減速や増速が無い場合について説明する。

また、関節Ｊ_２には、リンク１１２に対するリンク１１３の相対角度、具体的には減速機１２３の出力軸１２３Ａの角度、即ち関節Ｊ_２の関節角度を検出する関節角度検出部であるロータリエンコーダ（以下、「エンコーダ」という。）１２５が設けられている。エンコーダ１２５は、エンコーダ１２２に対してＮ（減速機１２３の減速比）倍の分解能を持つことが望ましい。エンコーダ１２５は、関節Ｊ_２の駆動に伴ってパルス信号を生成し、制御装置２００（図１）に出力する。エンコーダ１２５は、光学式ロータリエンコーダや磁気式ロータリエンコーダ等のいずれの方式でもよい。

図３は、ロボット装置５００の制御系を示すブロック図である。制御装置２００は、上位コントローラ２５０と、上位コントローラ２５０に接続された、関節Ｊ_１〜Ｊ_６の数と同じ数の複数（本実施形態では６つ）のサーボ制御装置２１０_１〜２１０_６と、を有する。また、上位コントローラ２５０には、各関節Ｊ_１〜Ｊ_６のエンコーダ１２５_１〜１２５_６が接続されている。各サーボ制御装置２１０_１〜２１０_６は、各関節Ｊ_１〜Ｊ_６の駆動部１２０_１〜１２０_６及びエンコーダ１２２_１〜１２２_６に接続されている。サーボ制御装置２１０_１〜２１０_６は、本実施形態では、制御装置２００の筐体に内部に配置されているが、これに限定するものではなく、ロボットアーム１０１に配置されていてもよい。

上位コントローラ２５０は、各駆動部１２０_１〜１２０_６のモータ１２１_１〜１２１_６の目標角度である角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊を、各サーボ制御装置２１０_１〜２１０_６に出力する。各角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊は、各関節Ｊ_１〜Ｊ_６の軌道データを示す各関節Ｊ_１〜Ｊ_６の関節角度指令値θ_ｒ１ ^＊〜θ_ｒ６ ^＊をＮ倍したものである。

各サーボ制御装置２１０_１〜２１０_６は、各エンコーダ１２５_１〜１２５_６により検出された回転角度値θ_ｍ１〜θ_ｍ６が、入力を受けた各角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊に近づくように、各モータ１２１_１〜１２１_６の回転をフィードバック制御する。上位コントローラ２５０は、各エンコーダ１２５_１〜１２５_６から各関節Ｊ_１〜Ｊ_６の関節角度値θ_ｒ１〜θ_ｒ６を取得する。

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

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

ＨＤＤ２５４は、ＣＰＵ２５１の演算処理結果である各種のデータ等を記憶する記憶装置であると共に、ＣＰＵ２５１に、後述する各種演算処理（工程）を実行させるためのプログラム２７０を記録するものである。ＣＰＵ２５１は、ＨＤＤ２５４に記録（格納）されたプログラム２７０に基づいて各種演算処理を実行する。

記録ディスクドライブ２５５は、記録ディスク２７１に記録された各種データやプログラム等を読み出すことができる。

インタフェース２６１には、ティーチングペンダント３００が接続されている。ティーチングペンダント３００は、ユーザが操作するものであり、ユーザの操作により教示点の設定を行うことができる。

インタフェース２６２には、エンコーダ１２５_１〜１２５_６が接続されている。エンコーダ１２５_１〜１２５_６からは、各関節Ｊ_１〜Ｊ_６の関節角度値θ_ｒ１〜θ_ｒ６を示すパルス信号が出力される。ＣＰＵ２５１は、インタフェース２６２及びバス２６０を介してエンコーダ１２５_１〜１２５_６からのパルス信号の入力を受ける。

インタフェース２６３には、サーボ制御装置２１０_１〜２１０_６が接続されている。ＣＰＵ２５１は、バス２６０及びインタフェース２６３を介して各サーボ制御装置２１０_１〜２１０_６に各角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊を、所定の時間間隔（例えば２［ｍｓ］）で出力する。各サーボ制御装置２１０_１〜２１０_６は、ＣＰＵ２５１から入力を受けた角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊と、各エンコーダ１２２_１〜１２２_６から取得した各回転角度値θ_ｍ１〜θ_ｍ６との差分値を求める。各サーボ制御装置２１０_１〜２１０_６は、演算した差分値が小さくなるように各モータ１２１_１〜１２１_６への電流の出力量（電流値）を演算する。各サーボ制御装置２１０_１〜２１０_６は、各モータ１２１_１〜１２１_６へ電流を供給して、ロボットアーム１０１の関節Ｊ_１〜Ｊ_６の関節角度の制御（各モータ１２１_１〜１２１_６の回転角度の制御）を行う。即ち、ＣＰＵ２５１は、各サーボ制御装置２１０_１〜２１０_６を介して各モータ１２１_１〜１２１_６の回転角度を制御することで、ロボットアーム１０１の動作（関節角度）を制御する。

インタフェース２６４には、ユーザに各種情報を通知する通知部としてのモニタ６００が接続されており、モニタ６００には、ＣＰＵ２５１の制御の下、各種画像が表示される。インタフェース２６５には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置８００が接続されている。

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

ここで、実際の生産工程に移る前のロボット１００の教示作業について説明する。上位コントローラ２５０は、不図示の軌道生成装置にて生成された、複数の教示点を所定の補間方法（関節補間や直線補間、円弧補間、スプライン補間等）で補間して得られた各関節Ｊ_１〜Ｊ_６の軌道データを取得する。なお、上位コントローラ２５０が、各関節Ｊ_１〜Ｊ_６の軌道データを生成してもよい。軌道データは、シミュレーションにより仮想ロボットを動作させて作成したものである。

上位コントローラ２５０（ＣＰＵ２５１）は、各関節Ｊ_１〜Ｊ_６の軌道データに基づく初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を生成する。なお、初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊の生成は、不図示の軌道生成装置が行ってもよい。このように角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊は、予めシミュレーション（いわゆるオフライン教示）により作成した、未補正の初期の角度指令値である。角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊は、記憶部（例えばＨＤＤ２５４）に記憶される。角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊は、後述する補正を受けた後、実際に生産工程にて使用する角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊として設定される。

制御部としてのＣＰＵ２５１は、動作モードと動作最適モードとを有しており、ユーザからの指令を受けていずれかのモードに設定を変更する。動作モードでは、設定済みの角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊に従ってロボット１００（ロボットアーム１０１）を動作させるモードである。動作最適モードは、追従遅れ時間に基づいて角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊の補正を行って、角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊を設定するモードである。なお、動作最適モードは、生産作業の前に行う教示作業（いわゆるオンライン教示）におけるモードである。

以下、動作最適モードに設定変更した場合について説明する。図５は、ロボット装置５００の制御系を示す機能ブロック図である。図５では関節Ｊ_１〜Ｊ_６のうちの１つの関節を例示している。なお、他の関節も略同様の構成であるため、図示を省略する。なお、図４中、上位コントローラ２５０のＣＰＵ２５１がプログラム２７０を読み出して実行することにより、図５中、各部４０１，４０２，４０４，４０５，４０６，４０７，４０８として機能する。また、図４中、ＲＯＭ２５２，ＲＡＭ２５３，ＨＤＤ２５４，外部記憶装置８００の少なくとも１つ、例えばＨＤＤ２５４が、図５中、閾値記憶部４０３として機能する。

各エンコーダ１２２（１２２_１〜１２２_６）は、各モータ１２１（１２１_１〜１２１_６）の回転軸１２１Ａの回転角度値θ_ｍ（θ_ｍ１〜θ_ｍ６）を検出し、各回転角度値θ_ｍを各サーボ制御装置２１０（２１０_１〜２１０_６）に出力する。

各サーボ制御装置２１０は、回転角度値θ_ｍ（θ_ｍ１〜θ_ｍ６）と設定前の角度指令値θ_ｍ ^＊（θ_ｍ１ ^＊〜θ_ｍ６ ^＊）との差分に基づき、各モータ１２１への電流の出力量を演算する。そして、各サーボ制御装置２１０は、各モータ１２１へ電流を供給して各モータ１２１の回転角度値θ_ｍが角度指令値θ_ｍ ^＊に近づくようにフィードバック制御する。各モータ１２１は、各サーボ制御装置２１０から電力供給を受けて駆動トルクを発生し、ベルト１２４及び減速機１２３を介して関節を駆動する。なお、動作モードにおける制御指令部４０５及びサーボ制御装置２１０の動作は、設定された角度指令値θ_ｍ ^＊（θ_ｍ１ ^＊〜θ_ｍ６ ^＊）を用いる以外、動作最適モードと同様である。

各減速機１２３の出力軸１２３Ａの回転角度値、即ち各関節の関節角度値θ_ｒ（θ_ｒ１〜θ_ｒ６）は、エンコーダ１２５（１２５_１〜１２５_６）により検出される。各エンコーダ１２５により検出された関節角度値θ_ｒは、軌道誤差算出部４０１及び追従遅れ時間算出部４０６へ出力される。

軌道誤差算出部４０１は、エンコーダ１２５_１〜１２５_６により検出された関節角度値θ_ｒ１〜θ_ｒ６に基づき、順運動学計算によりロボットアーム１０１の先端の実際の軌道を算出する。そして、軌道誤差算出部４０１は、設定前の角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊に基づくロボットアーム１０１の先端の目標の軌道と、算出した実際の軌道との差分を軌道誤差として算出する。ロボットアーム１０１の先端の目標の軌道は、不図示の軌道生成装置が作成して、上位コントローラ２５０のＣＰＵ２５１が取得するようにしてもよいし、ＣＰＵ２５１が作成してもよい。本実施形態では、ＣＰＵ２５１（軌道誤差算出部４０１）が計算するものとして具体的に説明する。

まず、軌道誤差算出部４０１は、設定前の角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊を制御指令部４０５から取得する。軌道誤差算出部４０１は、設定前の各角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊に基づき、関節Ｊ_１〜Ｊ_６の関節角度に換算した関節角度指令値θ_ｒ１ ^＊〜θ_ｒ６ ^＊を求める。軌道誤差算出部４０１は、得られた各関節Ｊ_１〜Ｊ_６の関節角度指令値θ_ｒ１ ^＊〜θ_ｒ６ ^＊を用いて、ロボット順運動学により、経過時間毎のロボットアーム１０１の先端の目標の軌道を算出する。

さらに、軌道誤差算出部４０１は、各関節Ｊ_１〜Ｊ_６内のエンコーダ１２５において検出した関節角度値θ_ｒ１〜θ_ｒ６を取得する。軌道誤差算出部４０１は、得られた関節Ｊ_１〜Ｊ_６の関節角度値θ_ｒ１〜θ_ｒ６を用いて、ロボット順運動学により、経過時間毎のロボットアーム１０１の先端の実際の軌道を算出する。そして、軌道誤差算出部４０１は、経過時間毎に目標の軌道と実際の軌道との軌道誤差を算出する。

この軌道誤差が組立に必要な精度を超えた場合、ロボットアームの先端が組付け作業周囲の治工具などと接触を起こし、ロボットもしくはロボットハンドに把持されているワークが破損する可能性がある。そこで、軌道誤差判定部４０２は、軌道誤差が許容範囲を超えているか否かを判断する。本実施形態では、軌道誤差判定部４０２は、軌道誤差算出部４０１で算出した軌道誤差の絶対値とユーザが予め設定した閾値記憶部４０３に記憶された閾値との比較判定を行う。

閾値記憶部４０３には、軌道誤差算出部４０１で算出した軌道誤差との比較に用いる閾値（許容範囲）が記憶されている。この閾値は、ユーザが任意に設定することができ、例えば組付け部品の嵌め合い公差のクリアランスの値や、実験により求めた値とする。

共振振動幅算出部４０４は、軌道誤差算出部４０１で算出したロボットアーム１０１の先端の軌道に対して高速フーリエ変換（ＦＦＴ）を行い、ロボットアーム１０１の先端の振動周波数と振幅を算出する。具体的に説明すると、まず共振振動幅算出部４０４は、軌道誤差算出部４０１からロボットアーム１０１の先端の実際の軌道（動作軌道）を取得する。そして共振振動幅算出部４０４は、取得した動作軌道の振動波形のＦＦＴを計算する。そして共振振動幅算出部４０４は、周波数毎の振幅について算出を行う。

追従遅れ時間算出部４０６は、指令に対する目標軌道と実際の動作軌道との差分時間となる追従遅れ時間を算出する。具体的に説明すると、まず追従遅れ時間算出部４０６は、制御指令部４０５から設定前の各角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊を取得し、これに基づき関節角度指令値θ_ｒ１ ^＊〜θ_ｒ６ ^＊を求める。また、追従遅れ時間算出部４０６は、エンコーダ１２５_１〜１２５_６で検出された関節角度値θ_ｒ１〜θ_ｒ６を取得する。そして追従遅れ時間算出部４０６は、各関節角度指令値θ_ｒ１ ^＊〜θ_ｒ６ ^＊と各関節角度値θ_ｒ１〜θ_ｒ６から各関節Ｊ_１〜Ｊ_６の目標軌道と各関節Ｊ_１〜Ｊ_６の実際の動作軌道との差分時間を、追従遅れ時間として算出する。

補正指令値算出部４０７は、追従遅れ時間算出部４０６で算出された各関節Ｊ_１〜Ｊ_６の追従遅れ時間とゼロ時間を合わせた７つの時間の中から、全関節Ｊ_１〜Ｊ_６で追従遅れ時間が一致するような基準時間を選択する。そして補正指令値算出部４０７は、選択した基準とする追従遅れ時間になるような補正値を全軸分算出し、制御指令部４０５へフィードバックをする。追従遅れ時間の基準時間の選択は、共振振動幅算出部４０４にて求められた共振振動幅の変化から選択され、ゼロ時間を含めた関節Ｊ_１〜Ｊ_６の追従遅れ時間の短い時間順に選ばれる。

通知制御部４０８は、軌道誤差判定部４０２による判定により軌道誤差と閾値との比較結果を、ティーチングペンダント３００やモニタ６００等の通知部に、ユーザに対する通知を行わせる。なお、通知部としては、不図示のランプやスピーカ等でもよく、ランプによる発光やスピーカからの音声でユーザに通知してもよい。

図６は、ロボット装置５００の制御系による制御動作を示すフローチャートである。ＣＰＵ２５１は、動作最適化モードに移行した際に、プログラム２７０を実行することにより、上記各部４０１，４０２，４０４，４０５，４０６，４０７，４０８として機能し、以下の処理（工程）を実行する。以下、各部の動作、即ち、ロボット装置５００の制御方法の各工程についてフローチャートを用いて詳細に説明する。

制御指令部４０５は、空間上の２点間の教示点を移動する所定の組付け動作をロボット１００に行わせる初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊をサーボ制御装置２１０に出力する（Ｓ１：動作工程）。初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊は、上述したようにオフライン教示にて作成したものである。

サーボ制御装置２１０は、入力を受けた角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に基づきモータ１２１に駆動電流を供給して、ロボットアーム１０１の関節Ｊ_１〜Ｊ_６の関節角度制御を行う。このように、制御指令部４０５は、各サーボ制御装置２１０に各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を出力することで、各モータ１２１の回転角度θ_ｍ１〜θ_ｍ６を各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に追従させてロボットアーム１０１を動作させる。

各エンコーダ１２５で検出された関節角度値θ_ｒ１〜θ_ｒ６は、軌道誤差算出部４０１に送られる。軌道誤差算出部４０１は、ステップＳ１にてロボットアーム１０１を動作させたときに各エンコーダ１２５により検出された関節角度値θ_ｒ１〜θ_ｒ６に基づき、順運動学の計算によりロボットアーム１０１の先端の実際の軌道を求める（Ｓ２：軌道算出工程）。

また、軌道誤差算出部４０１は、角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊からロボットアーム１０１の先端の目標の軌道を算出する。具体的には、軌道誤差算出部４０１は、角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を１／Ｎ倍した関節角度指令値^０θ_ｒ１ ^＊〜^０θ_ｒ６ ^＊に基づき、順運動学の計算によりロボットアーム１０１の先端の目標の軌道を求める。この演算結果であるロボットアーム１０１の先端の目標の軌道は、記憶部（ＨＤＤ２５４等）に記憶される。なお、予めロボットアーム１０１の先端の目標の軌道を計算しておき、記憶部（ＨＤＤ２５４等）に記憶させておけば、ステップＳ２でロボットアーム１０１の手先の目標軌道を計算しなくても済む。

次に、軌道誤差算出部４０１は、各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に基づくロボットアーム１０１の先端の目標の軌道に対する、ステップＳ２で求めた実際の軌道の軌道誤差を計算する（Ｓ３：軌道誤差算出工程）。

図７は、ロボットアーム１０１の先端が２つの教示点間を直線補間で移動した際の進行方向に対して垂直となる座標系で、ロボットアーム１０１の先端位置の変位を経過時間毎に表わしたグラフである。図７には角度指令値に基づくロボットアーム１０１の先端位置の軌道と経過時間のグラフである波形Ｐ_１と、エンコーダ１２５で検出された関節角度値から得られたロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ_２を表わす。ＣＰＵ２５１は、軌道の波形Ｐ_１に対する軌道の波形Ｐ_２の最も大きい差分値を軌道誤差Ｚ_２として算出する。

次に軌道誤差判定部４０２は、軌道誤差算出部４０１で算出した軌道誤差が、許容範囲を超えているか否かを判断する（Ｓ４：判断工程）。本実施形態では、許容範囲の上限は閾値Ｚｒｅｆであり、下限は閾値（−Ｚｒｅｆ）である。即ち、軌道誤差Ｚ_２の絶対値が閾値Ｚｒｅｆを超えたか否かを判断する。なお、本実施形態では、許容範囲の上限の絶対値と下限の絶対値とが同じ値Ｚｒｅｆとしたが、これに限定するものではなく、異なる値であってもよい。

閾値Ｚｒｅｆは、予め閾値記憶部４０３に記憶されている。この閾値Ｚｒｅｆは、ユーザが任意に設定することができ、本実施形態では、ワークＷの組付けに必要な値に設定されている。

軌道誤差判定部４０２は、ステップＳ３で計算した軌道誤差Ｚ_２と、ユーザが予め設定した閾値Ｚｒｅｆとの比較判定を行い、閾値Ｚｒｅｆを超えていなければ（Ｓ４：Ｎｏ）、組付けが問題なく可能であるので、図６のフローは終了する。その際、軌道誤差判定部４０２は、その判定結果を通知制御部４０８に伝達し、通知制御部４０８は、通知部（ティーチングペンダント３００やモニタ６００等）にユーザへの通知を行わせる。そして、初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を後の動作モードにおける生産工程で用いる角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊として設定する。

軌道誤差が許容範囲を超える場合、即ち軌道誤差Ｚ_２の絶対値が閾値Ｚｒｅｆを超える場合（Ｓ４：Ｙｅｓ）、部品が組付かない可能性があるためロボットアーム１０１の動作に補正が必要であるためステップＳ５に進む。

次に、ステップＳ５において、追従遅れ時間算出部４０６は、各関節角度指令値^０θ_ｒ１ ^＊〜^０θ_ｒ６ ^＊に対する各関節角度値θ_ｒ１〜θ_ｒ６の追従遅れ時間を、関節ごとに求める（追従遅れ時間算出工程）。即ち、追従遅れ時間算出部４０６は、各モータ１２１が動作した際に発生する、各関節Ｊ_１〜Ｊ_６への角度指令値に対する追従遅れ時間を算出する。ここで、各関節Ｊ_１〜Ｊ_６の追従遅れ時間をｔ_１〜ｔ_６とする。そして、各関節Ｊ_１〜Ｊ_６において、各経過時間における追従遅れ時間ｔ_１〜ｔ_６のうちピーク値の追従遅れ時間をＴ_１〜Ｔ_６とする。

図８は、経過時間に対する関節Ｊ_２，Ｊ_３の関節角度指令値^０θ_ｒ２ ^＊，^０θ_ｒ３ ^＊及び関節角度値θ_ｒ２，θ_ｒ３を示すグラフである。縦軸は角度、横軸は経過時間を示す。図９は、経過時間に対する関節Ｊ_２，Ｊ_３の追従遅れ時間ｔ_２，ｔ_３を示すグラフである。縦軸は追従遅れ時間、横軸は経過時間を示す。

関節角度指令値^０θ_ｒ２ ^＊に対する関節角度値θ_ｒ２の時間的な差異が追従遅れ時間ｔ_２であり、関節角度指令値^０θ_ｒ３ ^＊に対する関節角度値θ_ｒ３の時間的な差異が追従遅れ時間ｔ_３である。図８及び図９には、関節Ｊ_２，Ｊ_３についてのみ図示しているが、ＣＰＵ２５１は、Ｊ_１，Ｊ_４〜Ｊ_６についても同様に追従遅れ時間ｔ_１，ｔ_４〜ｔ_６を求める。

次に、補正指令値算出部４０７は、各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６のうちの最長の追従遅れ時間以下の基準時間を設定する。ここで、基準時間は、指令値補正後に各関節Ｊ_１〜Ｊ_６で一致させる目標とする追従遅れ時間であり、本実施形態では、ゼロ（遅れなし）及び時間ｔ_１〜ｔ_６の中から設定される。

補正指令値算出部４０７は、基準時間に対する各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６のずれ時間Δｔ_１〜Δｔ_６の分、各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を補正して、新たな各角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を生成する（Ｓ６：補正工程）。このように、補正指令値算出部４０７は、全関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６が１つの基準時間に一致するような補正後の角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を算出する。

本実施形態では、ステップＳ６では基準時間をゼロとする。よって、基準時間に対する各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６のずれ時間Δｔ_１〜Δｔ_６は、ｔ_１〜ｔ_６である。本実施形態では、各関節Ｊ_１〜Ｊ_６の目標軌道に対する各関節Ｊ_１〜Ｊ_６の追従遅れ時間がゼロとなるように各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を補正する。即ち基準時間がゼロであるので、補正指令値算出部４０７は、ステップＳ６では、各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６の分、各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を補正（オフセット）して、新たな複数の角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を生成する。

即ち、ステップＳ６では、基準時間がゼロなので関節Ｊ_１への補正値^１θ_ｍ１ ^＊は初期指令値^０θ_ｍ１ ^＊に対して時間ｔ_１分進んだ値、関節Ｊ_２への補正値^１θ_ｍ２ ^＊は初期指令値^０θ_ｍ２ ^＊に対して時間ｔ_２分進んだ値として算出する。同様に関節Ｊ_３〜Ｊ_６の補正値も追従遅れ時間がゼロになるような補正値を算出する。

なお、基準時間をゼロとした場合、各関節Ｊ_１〜Ｊ_６は、補正前の初期のロボットアーム１０１の動作速度よりもロボットアーム１０１の動作速度が速くなる。

このように、ステップＳ６では、ステップＳ１で動作した際の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に対して追従遅れ時間がゼロになるような補正値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を補正指令値算出部４０７で算出し、制御指令部４０５にフィードバックをする。

ここで、補正後の角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊は、記憶部（ＨＤＤ２５４等）に、初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊と区別して記憶される。

図１０は、経過時間に対する初期の角度指令値^０θ_ｍ２ ^＊，^０θ_ｍ３ ^＊及び補正後の角度指令値^１θ_ｍ２ ^＊，^１θ_ｍ３ ^＊を示すグラフである。図９には、初期の角度指令値^０θ_ｍ２ ^＊，^０θ_ｍ３ ^＊に対し、追従遅れ補正時間（ずれ時間）Δｔ_２＝ｔ_２，Δｔ_３＝ｔ_３分、指令タイミングを進めた補正後の角度指令値^１θ_ｍ２ ^＊，^１θ_ｍ３ ^＊を図示している。ステップＳ６では、全軸の追従遅れ時間がゼロになるように指令タイミングが進められた補正値^１θ_ｍ２ ^＊，^１θ_ｍ３ ^＊が算出される。図１０に不図示の他の関節Ｊ_１，Ｊ_４〜Ｊ_６についても関節Ｊ_２，Ｊ_３と同様にＣＰＵ２５１により算出する。

ＣＰＵ２５１は、ステップＳ６（補正工程）にて補正した各角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を用いて、ステップＳ１〜Ｓ４（動作工程、軌道算出工程、軌道誤差算出工程及び判断工程）と同様の処理を再度実行する。

即ち、制御指令部４０５は、各モータ１２１の回転角度θ_ｍ１〜θ_ｍ６を補正後の各角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊に追従させてロボットアームを再動作させる（Ｓ７：動作工程）。

軌道誤差算出部４０１は、ステップＳ７にてロボットアーム１０１を再動作させたときに各エンコーダ１２５により検出された関節角度値θ_ｒ１〜θ_ｒ６に基づき、ロボットアーム１０１の先端の実際の軌道を求める（Ｓ８：軌道算出工程）。

軌道誤差算出部４０１は、角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に基づくロボットアーム１０１の先端の目標の軌道に対する、ステップＳ８で求めた実際の軌道の軌道誤差Ｚ_３を算出する（Ｓ９：軌道誤差算出工程）。軌道誤差判定部４０２は、軌道誤差Ｚ_３が、許容範囲を超えているか否かを判断する（Ｓ１０：判断工程）。

図１１は、ロボットアーム１０１の先端が２点の教示点間を直線で移動した際の進行方向に対して垂直となる座標系で、ロボットアーム１０１の先端位置の変位を経過時間毎に表わしたグラフである。図１１には、初期の角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊に基づいて動作した際のロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ_２を図示している。また、図１１には、補正指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊に基づいて動作した際のロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ_３を図示している。そして、目標の軌道に対するそれぞれの指令値を基にした軌道の軌道誤差を軌道誤差Ｚ_２、軌道誤差Ｚ_３として表わす。

軌道誤差判定部４０２は、ステップＳ８で計算した軌道誤差Ｚ_３と、ユーザが予め設定した閾値Ｚｒｅｆとの比較判定を行い、閾値Ｚｒｅｆを超えていなければ（Ｓ１０：Ｎｏ）、組付けが問題なく可能であるので、図６のフローは終了する。その際、軌道誤差判定部４０２は、その判定結果を通知制御部４０８に伝達し、通知制御部４０８は、通知部（ティーチングペンダント３００やモニタ６００等）にユーザへの通知を行わせる。そして、補正後の角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を後の動作モードにおける生産工程で用いる角度指令値θ_ｍ１ ^＊〜θ_ｍ６ ^＊として設定する。

図１１の例の場合、Ｚｒｅｆ＜Ｚ_３であるため、次のステップＳ１１に進む。ステップＳ１０で軌道誤差が許容範囲を超えていると判断した場合（Ｓ１０：Ｙｅｓ）、共振振動幅算出部４０４は、最初にステップＳ３で算出した軌道からロボットアーム１０１の共振振動による振幅のピーク値（第１ピーク値）を求める。また、共振振動幅算出部４０４は、再度ステップＳ８で算出した軌道からロボットアーム１０１の共振振動による振幅のピーク値（第２ピーク値）を算出する（Ｓ１１：共振算出工程）。

次に、共振振動幅算出部４０４は、第２ピーク値が第１ピーク値よりも高いか否かを判断する（Ｓ１２：共振判断工程）。

即ち、ステップＳ１２ではステップＳ１０での再動作時の軌道誤差が閾値を超えた原因判断を行う。共振振動幅算出部４０４は、軌道誤差算出部４０１からステップＳ３での初期動作時の軌道誤差波形Ｐ_２とステップＳ９での補正動作時の軌道誤差波形Ｐ_３と取得し、高速フーリエ変換（ＦＦＴ）を行いロボットアーム先端位置の振動周波数と振幅を算出する。共振振動幅算出部４０４は、算出した動作軌道波形の共振振動の振幅変化に基づいて、ステップＳ１０において軌道誤差の絶対値が閾値を超えた原因が、補正により動作加速度が過大になったためか否かを判断する。再動作時に全軸の追従遅れ時間を一致させる基準時間の設定が短すぎた場合、動作加速度が上がり共振振動幅が増加する。つまり共振振動幅の増加変化を検出することで補正時間が短すぎたか否かが判断される。

図１２は、ステップＳ３で算出したロボットアーム１０１の先端位置軌道の波形Ｐ_２をＦＦＴした結果の波形Ｐ_２０と、ステップＳ８で算出したロボットアーム１０１の先端位置軌道の波形Ｐ_３をＦＦＴした結果の波形Ｐ_３０を示すグラフである。波形Ｐ_２０及び波形Ｐ_３０は、周波数ｆ_１と周波数ｆ_１よりも高い周波数ｆ_２にピークを持つ。周波数ｆ_１は追従遅れ時間が関節Ｊ_１〜Ｊ_６間で一致しないことに起因するもので、周波数ｆ_２はロボットアーム１０１の関節Ｊ_１〜Ｊ_６の共振振動に起因するものである。

図１２では、波形Ｐ_２０の周波数ｆ_２での振幅はＷ_２２である。波形Ｐ_３０の周波数ｆ_２での振幅はＷ_２３である。図１２の例では、Ｗ_２２＞Ｗ_２３であり、共振振動幅は増加していないため（Ｓ１２：Ｎｏ）、ステップＳ１３に進む。

ステップＳ１３では、再度ステップＳ１０で軌道誤差が許容範囲を超えていると判断した場合に、補正指令値算出部４０７が、最長の追従遅れ時間以下の範囲で基準時間を変更して再度、初期の各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ６ ^＊を補正する。これにより、補正後の新たな角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ６ ^＊を得る。

本実施形態では、補正指令値算出部４０７は、ステップＳ１３において基準時間を変更する際に、基準時間を、各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６の中から選択して該選択した追従遅れ時間に設定する。その際、基準時間を、各関節Ｊ_１〜Ｊ_６の追従遅れ時間ｔ_１〜ｔ_６のうち未選択の追従遅れ時間の中から選択して該追従遅れ時間に設定する。つまり、１巡目のステップＳ１３では、全ての追従遅れ時間ｔ_１〜ｔ_６が未選択であるので、基準時間は、これら時間ｔ_１〜ｔ_６の中から設定される。

そして、本実施形態では、ステップＳ１３で基準時間を変更する際に、基準時間を、未選択の追従遅れ時間ｔ_１〜ｔ_６のうち最短の追従遅れ時間を選択して該追従遅れ時間に設定する。

図１３は、ステップＳ５で算出した、ロボットアーム１０１の各関節Ｊ_１〜Ｊ_６の追従遅れ時間（ピーク値）Ｔ_１〜Ｔ_６を示した図である。最短の追従遅れ時間が関節Ｊ_６の追従遅れ時間Ｔ_６で、最長の追従遅れ時間が関節Ｊ_２の追従遅れ時間Ｔ_２である。図１３の例で、１巡目のステップＳ１２では、関節Ｊ_１〜Ｊ_６の追従遅れ時間Ｔ_１〜Ｔ_６のうち最短の追従遅れ時間Ｔ_６を選択する。そして、基準時間を、選択した追従遅れ時間Ｔ_６を含む追従遅れ時間ｔ_６とする。このように、本実施形態では、ステップＳ１３の追従遅れ時間の選択について、比較が容易となるピーク値を示す追従遅れ時間Ｔ_１〜Ｔ_６を用いている。

そして、関節Ｊ_１については、補正後の角度指令値^１θ_ｍ１ ^＊は、初期の角度指令値^０θ_ｍ１ ^＊に対してずれ時間Δｔ_１（＝ｔ_１−ｔ_６）進んだ値とする。関節Ｊ_２については、補正後の角度指令値^１θ_ｍ２ ^＊は、初期の角度指令値^０θ_ｍ２ ^＊に対してずれ時間Δｔ_２（＝ｔ_２−ｔ_６）進んだ値とする。残りの関節Ｊ_３〜Ｊ_５についても同様に角度指令値^１θ_ｍ３ ^＊〜^１θ_ｍ５ ^＊を求める。

なお、角度指令値^１θ_ｍ６ ^＊については、基準時間を時間ｔ_６としているので、そのままの値（^０θ_ｍ６ ^＊）となる。このように、選択された追従遅れ時間ｔ_６に対応する関節Ｊ_６の角度指令値^１θ_ｍ６ ^＊は、補正演算をする必要がないので、ＣＰＵ２５１における演算負荷が減る。

ステップＳ１３にて補正した角度指令値を求めた後、再度、ステップＳ７の処理に移行して、ステップＳ１０で軌道誤差が許容範囲に収まるまでステップＳ７〜Ｓ１３の処理を繰り返す。

これにより、ステップＳ１３では、図１３の各関節Ｊ_１〜Ｊ_６の追従遅れ時間Ｔ_１〜Ｔ_６のうち、未選択の追従遅れ時間の中から短い順で選択される。図１３の例では、Ｔ_６、Ｔ_４、Ｔ_１、Ｔ_５、Ｔ_３、Ｔ_２の順で選択される。

例えば、２巡目ではＴ_４が選択されるので、基準時間は追従遅れ時間ｔ_４となる。その際、ずれ時間Δｔ_１〜Δｔ_３、Δｔ_５は正の値であり、各角度指令値^１θ_ｍ１ ^＊〜^１θ_ｍ３ ^＊、^１θ_ｍ５ ^＊は、各角度指令値^０θ_ｍ１ ^＊〜^０θ_ｍ３ ^＊、^０θ_ｍ５ ^＊に対してずれ時間分進んだ値となる。また、ずれ時間Δｔ_４はゼロであり、角度指令値^１θ_ｍ４ ^＊は、角度指令値^０θ_ｍ４ ^＊のままである。また、ずれ時間Δｔ_６は負の値であり、角度指令値^１θ_ｍ６ ^＊は、角度指令値^０θ_ｍ６ ^＊に対してずれ時間分遅れた値となる。このように、２巡目では、関節Ｊ_４の追従遅れ時間ｔ_４に合わせて各関節Ｊ_１〜Ｊ_３、Ｊ_５、Ｊ_６の追従遅れ時間が揃えられる。

なお、６巡目では、Ｔ_２が選択されるので、基準時間は追従遅れ時間ｔ_２となる。その際、ずれ時間Δｔ_１，Δｔ_３〜Δｔ_６は負の値であり、各角度指令値^１θ_ｍ１ ^＊，^１θ_ｍ３ ^＊〜^１θ_ｍ６ ^＊は、各角度指令値^０θ_ｍ１ ^＊，^０θ_ｍ３ ^＊〜^０θ_ｍ６ ^＊に対してずれ時間分遅れた値となる。また、ずれ時間Δｔ_２はゼロであり、角度指令値^１θ_ｍ２ ^＊は、角度指令値^０θ_ｍ２ ^＊のままである。このように、６巡目では、関節Ｊ_２の追従遅れ時間ｔ_２に合わせて各関節Ｊ_１、Ｊ_３〜Ｊ_６の追従遅れ時間が揃えられる。以上、本実施形態では、最長の追従遅れ時間ｔ_２以下で基準時間が設定される。つまり、一番遅い関節Ｊ_２に各関節Ｊ_１、Ｊ_３〜Ｊ_６の追従遅れ時間を揃えたとしても、ロボットアーム１０１の動作速度は低下しない。

共振振動幅算出部４０４が、ステップＳ１２にて第２ピーク値が第１ピーク値よりも高いと判断した場合（Ｓ１２：Ｙｅｓ）、通知制御部４０８の制御に移行する。通知制御部４０８は、許容範囲（即ち閾値Ｚｒｅｆ）の変更を促す情報を、モニタ６００やティーチングペンダント３００等の通知部に通知させる（Ｓ１４：通知制御工程）。このように、ステップＳ１４では通知制御部４０８が、ユーザに対して予め閾値記憶部４０３に設定しておいた軌道誤差閾値Ｚｒｅｆの見直し要求の通知を行わせる。通知制御部４０８は、ステップＳ１２における共振振動幅算出部４０４の判定結果を受け、ユーザへ通知を行った後、フローを終了する。

以上、本実施形態によれば、ロボットアーム１０１の関節Ｊ_１〜Ｊ_６同士の追従遅れ時間を基準時間に一致させ、追従遅れ時間がばらつくのを抑制して、実際の動作軌道が目標軌道から外れるのを抑制している。更に、本実施形態によれば、ロボットアーム１０１の各関節Ｊ_１〜Ｊ_６の追従遅れ時間を揃える基準時間を、ステップＳ５で求めた最長の追従遅れ時間以下の範囲内でロボットアーム１０１の先端の振動が低減するように変更している。これにより、ロボットアーム１０１の動作速度が低減することなく、ロボットアーム１０１の先端の軌道誤差を低減させることができる。

そして組付けで重要となるロボットアーム１０１の先端軌道の軌道誤差が小さくなるので、ロボットアーム１０１の動作開始から動作終了までの総合の動作時間が短くなり、ロボット１００による組立製品の生産性が向上する。

また、ステップＳ６において最初は基準時間をゼロに設定して角度指令値を補正しているので、この補正でステップＳ１０にて軌道誤差が許容範囲内であると判断されれば、ロボットアーム１０１の動作速度は格段に向上する。これにより、ロボット１００による組立製品の生産性が格段に向上する。

また、ステップＳ１３で、基準時間を変更する際に、基準時間を、追従遅れ時間ｔ_１〜ｔ_６の中から選択しているので、基準時間を演算する必要がなく、ＣＰＵ２５１による演算負荷が低減する。

また、基準時間を変更する際に、未選択の追従遅れ時間のうち最短の追従遅れ時間を選択して基準時間に設定しているので、軌道誤差が許容範囲を満たすものの中で、最もロボットアーム１０１の動作速度が速くなる角度指令値が設定されることになる。よって、ロボット１００による組立製品の生産性の向上につながる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。

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

また、上記実施形態では、ステップＳ１１にて共振振動幅を算出し、ステップＳ１２にて共振判断を行い、共振振動幅が増加したときステップＳ１４を実行する場合について説明したが、ステップＳ１１，Ｓ１２，Ｓ１４の処理は、必要に応じて省略可能である。

また、上記実施形態では、ステップＳ１３で未選択の追従遅れ時間の中から短い順に選択して基準時間を設定したが、これに限定するものではなく、例えば未選択の追従遅れ時間の中からランダムに追従遅れ時間を選択して基準時間に設定してもよい。

また、上記実施形態では、ステップＳ６で最初に基準時間をゼロに設定したが、これに限定するものではなく、例えば追従遅れ時間ｔ_６に設定してもよい。

また、上記実施形態では、基準時間を、ゼロ及び追従遅れ時間ｔ_１〜ｔ_６の中から選択したが、これに限定するものではない。例えば、時間ｔ_１と時間ｔ_２との中間の時間や時間ｔ_２と時間ｔ_３との中間の時間を求めて、基準時間に設定してもよい。この場合、基準時間を演算する必要があるので、ＣＰＵ２５１による演算負荷が上記実施形態よりも増加することにはなるが、上記実施形態と同様、ロボットアーム１０１の振動を抑制しつつロボットアーム１０１の動作速度を向上させることができる。

上記実施形態では、ロボットアーム１０１が、垂直多関節型（シリアルリンク型）のロボットアームである場合について説明したが、これに限定するものではない。水平多関節型のロボットアームやパラレルリンク型のロボットアームなどであっても本発明は適用可能である。

１０１…ロボットアーム、１２１…モータ、１２２…エンコーダ（モータ角度検出部）、１２３…減速機、１２５…エンコーダ（関節角度検出部）、２５１…ＣＰＵ（制御部）、５００…ロボット装置

Claims (10)

1. 複数の関節をそれぞれ駆動する複数のモータと、前記複数の関節の角度をそれぞれ検出する複数の関節角度検出部と、を有するロボットアームと、
   前記ロボットアームの動作を制御する制御部と、を備えたロボット装置の制御方法であって、
   前記制御部が、前記各モータの回転角度を各角度指令値に追従させて前記ロボットアームを動作させる動作工程と、
   前記制御部が、前記動作工程にて前記ロボットアームを動作させたときに前記各関節角度検出部により検出された関節角度値に基づき、前記ロボットアームの先端の実際の軌道を求める軌道算出工程と、
   前記制御部が、前記角度指令値に基づく前記ロボットアームの先端の目標の軌道に対する、前記軌道算出工程で求めた実際の軌道の軌道誤差が、許容範囲を超えているか否かを判断する判断工程と、
   前記制御部が、初期の前記各角度指令値を関節の角度に換算した各関節角度指令値に対する前記各関節角度値の追従遅れ時間を、関節ごとに求める追従遅れ時間算出工程と、
   前記制御部が、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、前記各関節の追従遅れ時間のうちの最長の追従遅れ時間以下の基準時間に対する前記各関節の追従遅れ時間のずれ時間の分、初期の前記各角度指令値を補正する補正工程と、を備え、
   前記制御部が、前記補正工程にて補正した前記各角度指令値を用いて前記動作工程、前記軌道算出工程及び前記判断工程を再度実行し、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合に前記補正工程にて前記最長の追従遅れ時間以下の範囲で前記基準時間を変更して再度、初期の前記各角度指令値を補正することを特徴とするロボット装置の制御方法。
2. 前記補正工程では、最初は前記基準時間をゼロとすることを特徴とする請求項１に記載のロボット装置の制御方法。
3. 前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記各関節の追従遅れ時間の中から選択して該選択した追従遅れ時間に設定することを特徴とする請求項１又は２に記載のロボット装置の制御方法。
4. 前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記各関節の追従遅れ時間のうち未選択の追従遅れ時間の中から選択して該追従遅れ時間に設定することを特徴とする請求項３に記載のロボット装置の制御方法。
5. 前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記未選択の追従遅れ時間のうち最短の追従遅れ時間を選択して該追従遅れ時間に設定することを特徴とする請求項４に記載のロボット装置の制御方法。
6. 前記制御部が、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、最初に前記軌道算出工程で算出した軌道から前記ロボットアームの共振振動による振幅の第１ピーク値と、再度、前記軌道算出工程で算出した軌道から前記ロボットアームの共振振動による振幅の第２ピーク値とを算出する共振算出工程と、
   前記制御部が、前記第２ピーク値が前記第１ピーク値よりも高いか否かを判断する共振判断工程と、を更に備え、
   前記補正工程は、前記共振判断工程にて前記第２ピーク値が前記第１ピーク値よりも低いと判断した場合に再度実行することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置の制御方法。
7. 前記制御部が、前記共振判断工程にて前記第２ピーク値が前記第１ピーク値よりも高いと判断した場合、前記許容範囲の変更を促す情報を通知部に通知させる通知制御工程を更に備えたことを特徴とする請求項６に記載のロボット装置の制御方法。
8. 請求項１乃至７のいずれか１項に記載のロボット装置の制御方法の各工程を実行する制御部を備えたロボット装置。
9. コンピュータに、請求項１乃至７のいずれか１項に記載のロボット装置の制御方法の各工程を実行させるためのプログラム。
10. 請求項９に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images