(57)【要約】【課題】ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減する。【解決手段】各モータの回転角度を各角度指令値に追従させてロボットアームを動作させる（Ｓ１）。ロボットアームを動作させたときに各関節角度値に基づき、ロボットアームの先端の実際の軌道を求める（Ｓ２）。ロボットアームの先端の目標の軌道に対する実際の軌道の軌道誤差が許容範囲を超えているか否かを判断する（Ｓ４）。ステップＳ４で軌道誤差が許容範囲を超えていると判断した場合、各関節の追従遅れ時間を求める（Ｓ５）。各関節の追従遅れ時間の分、初期の各角度指令値を補正した角度指令値を生成する（Ｓ６）。補正した各角度指令値を用いて同様の処理を再度実行し（Ｓ７〜Ｓ９）、再度軌道誤差が許容範囲を超えていると判断した場合（Ｓ１０：Ｙｅｓ）、基準時間を変更して再度、初期の各角度指令値を補正する（Ｓ１３）。【選択図】図６
2016078149.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節をそれぞれ駆動する複数のモータと、前記複数の関節の角度をそれぞれ検出する複数の関節角度検出部と、を有するロボットアームと、
前記ロボットアームの動作を制御する制御部と、を備えたロボット装置の制御方法であって、
前記制御部が、前記各モータの回転角度を各角度指令値に追従させて前記ロボットアームを動作させる動作工程と、
前記制御部が、前記動作工程にて前記ロボットアームを動作させたときに前記各関節角度検出部により検出された関節角度値に基づき、前記ロボットアームの先端の実際の軌道を求める軌道算出工程と、
前記制御部が、前記角度指令値に基づく前記ロボットアームの先端の目標の軌道に対する、前記軌道算出工程で求めた実際の軌道の軌道誤差が、許容範囲を超えているか否かを判断する判断工程と、
前記制御部が、初期の前記各角度指令値を関節の角度に換算した各関節角度指令値に対する前記各関節角度値の追従遅れ時間を、関節ごとに求める追従遅れ時間算出工程と、
前記制御部が、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、前記各関節の追従遅れ時間のうちの最長の追従遅れ時間以下の基準時間に対する前記各関節の追従遅れ時間のずれ時間の分、初期の前記各角度指令値を補正する補正工程と、を備え、
前記制御部が、前記補正工程にて補正した前記各角度指令値を用いて前記動作工程、前記軌道算出工程及び前記判断工程を再度実行し、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合に前記補正工程にて前記最長の追従遅れ時間以下の範囲で前記基準時間を変更して再度、初期の前記各角度指令値を補正することを特徴とするロボット装置の制御方法。
【請求項２】
前記補正工程では、最初は前記基準時間をゼロとすることを特徴とする請求項１に記載のロボット装置の制御方法。
【請求項３】
前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記各関節の追従遅れ時間の中から選択して該選択した追従遅れ時間に設定することを特徴とする請求項１又は２に記載のロボット装置の制御方法。
【請求項４】
前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記各関節の追従遅れ時間のうち未選択の追従遅れ時間の中から選択して該追従遅れ時間に設定することを特徴とする請求項３に記載のロボット装置の制御方法。
【請求項５】
前記補正工程では、前記基準時間を変更する際に、前記基準時間を、前記未選択の追従遅れ時間のうち最短の追従遅れ時間を選択して該追従遅れ時間に設定することを特徴とする請求項４に記載のロボット装置の制御方法。
【請求項６】
前記制御部が、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、最初に前記軌道算出工程で算出した軌道から前記ロボットアームの共振振動による振幅の第１ピーク値と、再度、前記軌道算出工程で算出した軌道から前記ロボットアームの共振振動による振幅の第２ピーク値とを算出する共振算出工程と、
前記制御部が、前記第２ピーク値が前記第１ピーク値よりも高いか否かを判断する共振判断工程と、を更に備え、
前記補正工程は、前記共振判断工程にて前記第２ピーク値が前記第１ピーク値よりも低いと判断した場合に再度実行することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置の制御方法。
【請求項７】
前記制御部が、前記共振判断工程にて前記第２ピーク値が前記第１ピーク値よりも高いと判断した場合、前記許容範囲の変更を促す情報を通知部に通知させる通知制御工程を更に備えたことを特徴とする請求項６に記載のロボット装置の制御方法。
【請求項８】
請求項１乃至７のいずれか１項に記載のロボット装置の制御方法の各工程を実行する制御部を備えたロボット装置。
【請求項９】
コンピュータに、請求項１乃至７のいずれか１項に記載のロボット装置の制御方法の各工程を実行させるためのプログラム。
【請求項１０】
請求項９に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、各関節のモータの回転角度が各角度指令値に近づくようにロボットアームの動作を制御するロボット装置の制御方法、ロボット装置、プログラム及び記録媒体に関する。
【背景技術】
【０００２】
精密機器の自動組立において、高精度かつ高速に組付けを行う産業用のロボットが用いられている。ロボットは、多関節のロボットアームと、ロボットアームの先端に取り付けられたロボットハンドとを有する。ロボットアームは、各関節を駆動する複数の駆動部を有する。各駆動部は、モータと、モータの回転軸の回転を減速する減速機とを有する。
【０００３】
一般に、モータの回転軸の回転角度が角度指令値に近づくように制御するフィードバック制御が行われている。角度指令値は、コンピュータ上のシミュレーションにより作成される。そのため、ロボット自体の製造公差や撓み等により、複数の駆動部を連動させた際に、角度指令値に基づくロボットアームの先端の目標の軌道と運転時のロボットアームの先端の実際の軌道とに差分（以下、軌道誤差という）が生じる。このように軌道誤差が生じると、組立製品の嵌め合い公差幅を超えることがあり、組み付けができなくなることがあった。
【０００４】
この軌道誤差は、ロボットアームの共振振動に起因するものと、ロボットアームの各関節の関節角度が目標の関節角度（関節角度指令値）に達するまでの時間差（以下、追従遅れ時間という）のばらつきに起因するものとがある。
【０００５】
軌道誤差を低減する方法については、特許文献１で提案されている。特許文献１では、追従遅れ時間に相当するサーボ遅れ時間を関節軸毎に求め、これらサーボ遅れ時間の中で最も短いサーボ遅れ時間又は平均の時間を基準時間として、該基準時間に基づいて補償トルクを計算する。そして、補償トルクを反映した指令値を各軸のサーボに出力し、サーボ遅れ時間が全ての軸で一致するように制御している。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第４９９２７０２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、特許文献１では、モータの出力軸の回転を検出するロータリエンコーダから各関節の回転角度を検出しており、ロボットアームの関節に撓みが生じた場合には、実際の関節角度とは異なる値となる。そのため、ロボットアームの軌道誤差が解消しない場合もあった。
【０００８】
また、特許文献１では、各関節のサーボ遅れ時間を基準時間に合わせることでサーボ遅れ時間のばらつきを解消することができたとしても、決定される基準時間によってはロボットアームの共振振動による軌道誤差が許容範囲を超える場合があった。このような場合、特許文献１では、ロボットアームの動作速度を低減させていたため、ロボットによる組立作業に要する時間が長くなり、組立製品の生産性が低下していた。
【０００９】
そこで、本発明は、ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減することを目的とする。
【課題を解決するための手段】
【００１０】
本発明は、複数の関節をそれぞれ駆動する複数のモータと、前記複数の関節の角度をそれぞれ検出する複数の関節角度検出部と、を有するロボットアームと、前記ロボットアームの動作を制御する制御部と、を備えたロボット装置の制御方法であって、前記制御部が、前記各モータの回転角度を各角度指令値に追従させて前記ロボットアームを動作させる動作工程と、前記制御部が、前記動作工程にて前記ロボットアームを動作させたときに前記各関節角度検出部により検出された関節角度値に基づき、前記ロボットアームの先端の実際の軌道を求める軌道算出工程と、前記制御部が、前記角度指令値に基づく前記ロボットアームの先端の目標の軌道に対する、前記軌道算出工程で求めた実際の軌道の軌道誤差が、許容範囲を超えているか否かを判断する判断工程と、前記制御部が、初期の前記各角度指令値を関節の角度に換算した各関節角度指令値に対する前記各関節角度値の追従遅れ時間を、関節ごとに求める追従遅れ時間算出工程と、前記制御部が、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合、前記各関節の追従遅れ時間のうちの最長の追従遅れ時間以下の基準時間に対する前記各関節の追従遅れ時間のずれ時間の分、初期の前記各角度指令値を補正する補正工程と、を備え、前記制御部が、前記補正工程にて補正した前記各角度指令値を用いて前記動作工程、前記軌道算出工程及び前記判断工程を再度実行し、再度、前記判断工程で軌道誤差が許容範囲を超えていると判断した場合に前記補正工程にて前記最長の追従遅れ時間以下の範囲で前記基準時間を変更して再度、初期の前記各角度指令値を補正することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、ロボットアームの動作速度を低減することなく、ロボットアームの先端の軌道誤差を低減することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施形態に係るロボット装置を示す斜視図である。
【図２】ロボットアームの第２関節を示す部分分解斜視図である。
【図３】ロボット装置の制御系を示すブロック図である。
【図４】ロボット装置の制御装置の上位コントローラの構成を示すブロック図である。
【図５】ロボット装置の制御系を示す機能ブロック図である。
【図６】ロボット装置の制御系による制御動作を示すフローチャートである。
【図７】ロボットアームの先端位置の変位を経過時間毎に表わしたグラフである。
【図８】経過時間に対する第２関節及び第３関節の関節角度指令値及び関節角度値を示すグラフである。
【図９】経過時間に対する第２関節及び第３関節の追従遅れ時間を示すグラフである。
【図１０】経過時間に対する第２関節及び第３関節の初期の角度指令値及び補正後の角度指令値を示すグラフである。
【図１１】ロボットアームの先端位置の変位を経過時間毎に表わしたグラフである。
【図１２】ロボットアームの先端の軌道の周波数特性を示すグラフである。
【図１３】ロボットアームの各関節の追従遅れ時間を示すグラフである。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、本発明の実施形態に係るロボット装置を示す斜視図である。図１に示すロボット装置５００は、把持したワークＷを別のワークに組付ける組立作業を行うロボット１００と、ロボット１００の動作を制御する制御装置２００と、教示装置であるティーチングペンダント３００と、を備えている。
【００１４】
ロボット１００は、多関節のロボットである。ロボット１００は、垂直多関節型のロボットアーム１０１と、ロボットアーム１０１の先端に接続されたエンドエフェクタであるロボットハンド１０２と、を備えている。
【００１５】
ロボットアーム１０１は、作業台に固定されるベース部（基端リンク）１１１と、変位や力を伝達する複数のリンク１１２〜１１７と、を有している。ベース部１１１及び複数のリンク１１２〜１１７は、複数の関節Ｊ１〜Ｊ６で旋回又は回転可能に互いに連結されている。
【００１６】
また、ロボットアーム１０１は、各関節Ｊ１〜Ｊ６に設けられた、関節Ｊ１〜Ｊ６をそれぞれ駆動する複数（本実施形態では６つ）の駆動部１２０を有している。各関節Ｊ１〜Ｊ６に配置された駆動部１２０は、必要なトルクの大きさに合わせて適切な出力のものが用いられる。
【００１７】
ロボットハンド１０２は、ワークＷを把持する複数の把持爪（フィンガ）１０４と、複数の把持爪１０４を駆動する不図示の駆動部と、駆動部の回転角度を検出する不図示のエンコーダと、回転を把持動作に変換する不図示の機構とを有している。この不図示の機構は、カム機構やリンク機構などで必要な把持動作に合わせて設計される。なお、ロボットハンド１０２に用いる駆動部に必要なトルクは、ロボットアーム１０１の関節用と異なるが、基本構成は同じである。また、ロボットハンド１０２は、把持爪１０４等に作用する反力を検出可能な不図示の力覚センサを有している。
【００１８】
ロボットアーム１０１の先端には、ツールセンターポイント（ＴＣＰ）が設定されており、教示点がタスク空間で規定される場合、その教示点は、ＴＣＰの位置及び姿勢を示す６つのパラメータで表現される。具体的には、教示点は、タスク空間で表すと、３つの位置パラメータ（Ｘ，Ｙ，Ｚ）と、３つの姿勢パラメータ（ｔＸ，ｔＹ，ｔＺ）とからなる６つのパラメータで表現される。教示点がコンフィグレーション空間（関節空間）で規定される場合、その教示点は、ロボット１００の各関節Ｊ１〜Ｊ６の関節位置（関節角度）を示すパラメータ（θ１，θ２，θ３，θ４，θ５，θ６）で表現される。
【００１９】
なお、コンフィグレーション空間で規定された教示点は、順運動学計算により、タスク空間に変換することができ、タスク空間で規定された教示点は、逆運動学計算により、コンフィグレーション空間に変換することができる。
【００２０】
ティーチングペンダント３００は、制御装置２００に接続可能に構成され、制御装置２００に接続された際に、ロボットアーム１０１やロボットハンド１０２を駆動制御する指令を制御装置２００に送信可能に構成されている。
【００２１】
以下、関節Ｊ２における駆動部１２０を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６の駆動部１２０については、サイズや性能が異なる場合もあるが、同様の構成であるため、説明を省略する。
【００２２】
図２は、ロボットアーム１０１の第２関節である関節Ｊ２を示す部分分解斜視図である。駆動部１２０は、サーボモータであるモータ１２１と、モータ１２１のトルクを増大させるためにモータ１２１の回転を減速比Ｎで減速する減速機１２３と、減速機１２３にモータ１２１の回転駆動を伝達するベルト１２４と、を有している。モータ１２１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。
【００２３】
モータ１２１の回転軸１２１Ａには、モータ１２１の回転軸１２１Ａの回転角度を検出するモータ角度検出部であるロータリエンコーダ（以下、「エンコーダ」という）１２２が取り付けられている。エンコーダ１２２は、モータ１２１の回転軸１２１Ａの一端に設けられている。回転軸１２１Ａの他端には、駆動プーリ１２６が設けられている。エンコーダ１２２は、モータ１２１の回転軸１２１Ａの回転に伴って、パルス信号を発生して、図１の制御装置２００に出力する。エンコーダ１２２は、インクリメンタル型のロータリエンコーダでもよいが、モータ１２１の所定の回転位置を原点とした絶対角度が検出できるアブソリュート型のロータリエンコーダであるのが望ましい。なお、エンコーダ１２２は、回転軸１２１Ａに取り付けられているが、減速機１２３の入力軸１２３Ｂに取り付けてもよい。
【００２４】
減速機１２３の入力軸１２３Ｂには、従動プーリ１２７が取り付けられ、駆動プーリ１２６と従動プーリ１２７とに無端状のベルト１２４が架け渡されている。これにより、駆動プーリ１２６の回転駆動力は減速機１２３の入力軸１２３Ｂに取り付けられた従動プーリ１２７へベルト２０４を介して伝達される。組立作業等を行うロボットでは正確な位置決めが求められるので、ベルト１２４は歯付ベルトであることが望ましい。
【００２５】
減速機１２３には、小型軽量で減速比Ｎの大きい波動歯車減速機を用いるのが望ましい。減速機１２３は、ベルト１２４を介して伝達されたモータ１２１の回転を減速する。減速機１２３の筐体は、リンク１１３に固定され、減速機１２３の出力軸１２３Ａは、リンク１１２（図１）に接続される。ベルト１２４による減速或いは増速がない場合、モータ１２１の回転軸１２１Ａの回転は、減速機１２３により１／Ｎに減速される。本実施形態ではベルト１２４による減速や増速が無い場合について説明する。
【００２６】
また、関節Ｊ２には、リンク１１２に対するリンク１１３の相対角度、具体的には減速機１２３の出力軸１２３Ａの角度、即ち関節Ｊ２の関節角度を検出する関節角度検出部であるロータリエンコーダ（以下、「エンコーダ」という。）１２５が設けられている。エンコーダ１２５は、エンコーダ１２２に対してＮ（減速機１２３の減速比）倍の分解能を持つことが望ましい。エンコーダ１２５は、関節Ｊ２の駆動に伴ってパルス信号を生成し、制御装置２００（図１）に出力する。エンコーダ１２５は、光学式ロータリエンコーダや磁気式ロータリエンコーダ等のいずれの方式でもよい。
【００２７】
図３は、ロボット装置５００の制御系を示すブロック図である。制御装置２００は、上位コントローラ２５０と、上位コントローラ２５０に接続された、関節Ｊ１〜Ｊ６の数と同じ数の複数（本実施形態では６つ）のサーボ制御装置２１０１〜２１０６と、を有する。また、上位コントローラ２５０には、各関節Ｊ１〜Ｊ６のエンコーダ１２５１〜１２５６が接続されている。各サーボ制御装置２１０１〜２１０６は、各関節Ｊ１〜Ｊ６の駆動部１２０１〜１２０６及びエンコーダ１２２１〜１２２６に接続されている。サーボ制御装置２１０１〜２１０６は、本実施形態では、制御装置２００の筐体に内部に配置されているが、これに限定するものではなく、ロボットアーム１０１に配置されていてもよい。
【００２８】
上位コントローラ２５０は、各駆動部１２０１〜１２０６のモータ１２１１〜１２１６の目標角度である角度指令値θｍ１＊〜θｍ６＊を、各サーボ制御装置２１０１〜２１０６に出力する。各角度指令値θｍ１＊〜θｍ６＊は、各関節Ｊ１〜Ｊ６の軌道データを示す各関節Ｊ１〜Ｊ６の関節角度指令値θｒ１＊〜θｒ６＊をＮ倍したものである。
【００２９】
各サーボ制御装置２１０１〜２１０６は、各エンコーダ１２５１〜１２５６により検出された回転角度値θｍ１〜θｍ６が、入力を受けた各角度指令値θｍ１＊〜θｍ６＊に近づくように、各モータ１２１１〜１２１６の回転をフィードバック制御する。上位コントローラ２５０は、各エンコーダ１２５１〜１２５６から各関節Ｊ１〜Ｊ６の関節角度値θｒ１〜θｒ６を取得する。
【００３０】
図４は、ロボット装置５００の制御装置２００の上位コントローラ２５０の構成を示すブロック図である。上位コントローラ２５０は、コンピュータで構成され、制御部（演算部）としてのＣＰＵ（Central  Processing  Unit）２５１を備えている。上位コントローラ２５０は、記憶部としてＲＯＭ（Read  Only  Memory）２５２、ＲＡＭ（Random  Access  Memory）２５３及びＨＤＤ（Hard  Disk  Drive）２５４を備えている。また、上位コントローラ２５０は、記録ディスクドライブ２５５と、各種のインタフェース２６１〜２６５とを備えている。
【００３１】
ＣＰＵ２５１には、ＲＯＭ２５２、ＲＡＭ２５３、ＨＤＤ２５４、記録ディスクドライブ２５５及び各種のインタフェース２６１〜２６５が、バス２６０を介して接続されている。ＲＯＭ２５２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２５３は、ＣＰＵ２５１の演算処理結果を一時的に記憶する記憶装置である。
【００３２】
ＨＤＤ２５４は、ＣＰＵ２５１の演算処理結果である各種のデータ等を記憶する記憶装置であると共に、ＣＰＵ２５１に、後述する各種演算処理（工程）を実行させるためのプログラム２７０を記録するものである。ＣＰＵ２５１は、ＨＤＤ２５４に記録（格納）されたプログラム２７０に基づいて各種演算処理を実行する。
【００３３】
記録ディスクドライブ２５５は、記録ディスク２７１に記録された各種データやプログラム等を読み出すことができる。
【００３４】
インタフェース２６１には、ティーチングペンダント３００が接続されている。ティーチングペンダント３００は、ユーザが操作するものであり、ユーザの操作により教示点の設定を行うことができる。
【００３５】
インタフェース２６２には、エンコーダ１２５１〜１２５６が接続されている。エンコーダ１２５１〜１２５６からは、各関節Ｊ１〜Ｊ６の関節角度値θｒ１〜θｒ６を示すパルス信号が出力される。ＣＰＵ２５１は、インタフェース２６２及びバス２６０を介してエンコーダ１２５１〜１２５６からのパルス信号の入力を受ける。
【００３６】
インタフェース２６３には、サーボ制御装置２１０１〜２１０６が接続されている。ＣＰＵ２５１は、バス２６０及びインタフェース２６３を介して各サーボ制御装置２１０１〜２１０６に各角度指令値θｍ１＊〜θｍ６＊を、所定の時間間隔（例えば２［ｍｓ］）で出力する。各サーボ制御装置２１０１〜２１０６は、ＣＰＵ２５１から入力を受けた角度指令値θｍ１＊〜θｍ６＊と、各エンコーダ１２２１〜１２２６から取得した各回転角度値θｍ１〜θｍ６との差分値を求める。各サーボ制御装置２１０１〜２１０６は、演算した差分値が小さくなるように各モータ１２１１〜１２１６への電流の出力量（電流値）を演算する。各サーボ制御装置２１０１〜２１０６は、各モータ１２１１〜１２１６へ電流を供給して、ロボットアーム１０１の関節Ｊ１〜Ｊ６の関節角度の制御（各モータ１２１１〜１２１６の回転角度の制御）を行う。即ち、ＣＰＵ２５１は、各サーボ制御装置２１０１〜２１０６を介して各モータ１２１１〜１２１６の回転角度を制御することで、ロボットアーム１０１の動作（関節角度）を制御する。
【００３７】
インタフェース２６４には、ユーザに各種情報を通知する通知部としてのモニタ６００が接続されており、モニタ６００には、ＣＰＵ２５１の制御の下、各種画像が表示される。インタフェース２６５には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置８００が接続されている。
【００３８】
なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２５４であり、ＨＤＤ２５４にプログラム２７０が格納される場合について説明するが、これに限定するものではない。プログラム２７０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム２７０を供給するための記録媒体としては、図４に示すＲＯＭ２５２や外部記憶装置８００、記録ディスク２７１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。
【００３９】
ここで、実際の生産工程に移る前のロボット１００の教示作業について説明する。上位コントローラ２５０は、不図示の軌道生成装置にて生成された、複数の教示点を所定の補間方法（関節補間や直線補間、円弧補間、スプライン補間等）で補間して得られた各関節Ｊ１〜Ｊ６の軌道データを取得する。なお、上位コントローラ２５０が、各関節Ｊ１〜Ｊ６の軌道データを生成してもよい。軌道データは、シミュレーションにより仮想ロボットを動作させて作成したものである。
【００４０】
上位コントローラ２５０（ＣＰＵ２５１）は、各関節Ｊ１〜Ｊ６の軌道データに基づく初期の角度指令値０θｍ１＊〜０θｍ６＊を生成する。なお、初期の角度指令値０θｍ１＊〜０θｍ６＊の生成は、不図示の軌道生成装置が行ってもよい。このように角度指令値０θｍ１＊〜０θｍ６＊は、予めシミュレーション（いわゆるオフライン教示）により作成した、未補正の初期の角度指令値である。角度指令値０θｍ１＊〜０θｍ６＊は、記憶部（例えばＨＤＤ２５４）に記憶される。角度指令値０θｍ１＊〜０θｍ６＊は、後述する補正を受けた後、実際に生産工程にて使用する角度指令値θｍ１＊〜θｍ６＊として設定される。
【００４１】
制御部としてのＣＰＵ２５１は、動作モードと動作最適モードとを有しており、ユーザからの指令を受けていずれかのモードに設定を変更する。動作モードでは、設定済みの角度指令値θｍ１＊〜θｍ６＊に従ってロボット１００（ロボットアーム１０１）を動作させるモードである。動作最適モードは、追従遅れ時間に基づいて角度指令値０θｍ１＊〜０θｍ６＊の補正を行って、角度指令値θｍ１＊〜θｍ６＊を設定するモードである。なお、動作最適モードは、生産作業の前に行う教示作業（いわゆるオンライン教示）におけるモードである。
【００４２】
以下、動作最適モードに設定変更した場合について説明する。図５は、ロボット装置５００の制御系を示す機能ブロック図である。図５では関節Ｊ１〜Ｊ６のうちの１つの関節を例示している。なお、他の関節も略同様の構成であるため、図示を省略する。なお、図４中、上位コントローラ２５０のＣＰＵ２５１がプログラム２７０を読み出して実行することにより、図５中、各部４０１，４０２，４０４，４０５，４０６，４０７，４０８として機能する。また、図４中、ＲＯＭ２５２，ＲＡＭ２５３，ＨＤＤ２５４，外部記憶装置８００の少なくとも１つ、例えばＨＤＤ２５４が、図５中、閾値記憶部４０３として機能する。
【００４３】
各エンコーダ１２２（１２２１〜１２２６）は、各モータ１２１（１２１１〜１２１６）の回転軸１２１Ａの回転角度値θｍ（θｍ１〜θｍ６）を検出し、各回転角度値θｍを各サーボ制御装置２１０（２１０１〜２１０６）に出力する。
【００４４】
各サーボ制御装置２１０は、回転角度値θｍ（θｍ１〜θｍ６）と設定前の角度指令値θｍ＊（θｍ１＊〜θｍ６＊）との差分に基づき、各モータ１２１への電流の出力量を演算する。そして、各サーボ制御装置２１０は、各モータ１２１へ電流を供給して各モータ１２１の回転角度値θｍが角度指令値θｍ＊に近づくようにフィードバック制御する。各モータ１２１は、各サーボ制御装置２１０から電力供給を受けて駆動トルクを発生し、ベルト１２４及び減速機１２３を介して関節を駆動する。なお、動作モードにおける制御指令部４０５及びサーボ制御装置２１０の動作は、設定された角度指令値θｍ＊（θｍ１＊〜θｍ６＊）を用いる以外、動作最適モードと同様である。
【００４５】
各減速機１２３の出力軸１２３Ａの回転角度値、即ち各関節の関節角度値θｒ（θｒ１〜θｒ６）は、エンコーダ１２５（１２５１〜１２５６）により検出される。各エンコーダ１２５により検出された関節角度値θｒは、軌道誤差算出部４０１及び追従遅れ時間算出部４０６へ出力される。
【００４６】
軌道誤差算出部４０１は、エンコーダ１２５１〜１２５６により検出された関節角度値θｒ１〜θｒ６に基づき、順運動学計算によりロボットアーム１０１の先端の実際の軌道を算出する。そして、軌道誤差算出部４０１は、設定前の角度指令値θｍ１＊〜θｍ６＊に基づくロボットアーム１０１の先端の目標の軌道と、算出した実際の軌道との差分を軌道誤差として算出する。ロボットアーム１０１の先端の目標の軌道は、不図示の軌道生成装置が作成して、上位コントローラ２５０のＣＰＵ２５１が取得するようにしてもよいし、ＣＰＵ２５１が作成してもよい。本実施形態では、ＣＰＵ２５１（軌道誤差算出部４０１）が計算するものとして具体的に説明する。
【００４７】
まず、軌道誤差算出部４０１は、設定前の角度指令値θｍ１＊〜θｍ６＊を制御指令部４０５から取得する。軌道誤差算出部４０１は、設定前の各角度指令値θｍ１＊〜θｍ６＊に基づき、関節Ｊ１〜Ｊ６の関節角度に換算した関節角度指令値θｒ１＊〜θｒ６＊を求める。軌道誤差算出部４０１は、得られた各関節Ｊ１〜Ｊ６の関節角度指令値θｒ１＊〜θｒ６＊を用いて、ロボット順運動学により、経過時間毎のロボットアーム１０１の先端の目標の軌道を算出する。
【００４８】
さらに、軌道誤差算出部４０１は、各関節Ｊ１〜Ｊ６内のエンコーダ１２５において検出した関節角度値θｒ１〜θｒ６を取得する。軌道誤差算出部４０１は、得られた関節Ｊ１〜Ｊ６の関節角度値θｒ１〜θｒ６を用いて、ロボット順運動学により、経過時間毎のロボットアーム１０１の先端の実際の軌道を算出する。そして、軌道誤差算出部４０１は、経過時間毎に目標の軌道と実際の軌道との軌道誤差を算出する。
【００４９】
この軌道誤差が組立に必要な精度を超えた場合、ロボットアームの先端が組付け作業周囲の治工具などと接触を起こし、ロボットもしくはロボットハンドに把持されているワークが破損する可能性がある。そこで、軌道誤差判定部４０２は、軌道誤差が許容範囲を超えているか否かを判断する。本実施形態では、軌道誤差判定部４０２は、軌道誤差算出部４０１で算出した軌道誤差の絶対値とユーザが予め設定した閾値記憶部４０３に記憶された閾値との比較判定を行う。
【００５０】
閾値記憶部４０３には、軌道誤差算出部４０１で算出した軌道誤差との比較に用いる閾値（許容範囲）が記憶されている。この閾値は、ユーザが任意に設定することができ、例えば組付け部品の嵌め合い公差のクリアランスの値や、実験により求めた値とする。
【００５１】
共振振動幅算出部４０４は、軌道誤差算出部４０１で算出したロボットアーム１０１の先端の軌道に対して高速フーリエ変換（ＦＦＴ）を行い、ロボットアーム１０１の先端の振動周波数と振幅を算出する。具体的に説明すると、まず共振振動幅算出部４０４は、軌道誤差算出部４０１からロボットアーム１０１の先端の実際の軌道（動作軌道）を取得する。そして共振振動幅算出部４０４は、取得した動作軌道の振動波形のＦＦＴを計算する。そして共振振動幅算出部４０４は、周波数毎の振幅について算出を行う。
【００５２】
追従遅れ時間算出部４０６は、指令に対する目標軌道と実際の動作軌道との差分時間となる追従遅れ時間を算出する。具体的に説明すると、まず追従遅れ時間算出部４０６は、制御指令部４０５から設定前の各角度指令値θｍ１＊〜θｍ６＊を取得し、これに基づき関節角度指令値θｒ１＊〜θｒ６＊を求める。また、追従遅れ時間算出部４０６は、エンコーダ１２５１〜１２５６で検出された関節角度値θｒ１〜θｒ６を取得する。そして追従遅れ時間算出部４０６は、各関節角度指令値θｒ１＊〜θｒ６＊と各関節角度値θｒ１〜θｒ６から各関節Ｊ１〜Ｊ６の目標軌道と各関節Ｊ１〜Ｊ６の実際の動作軌道との差分時間を、追従遅れ時間として算出する。
【００５３】
補正指令値算出部４０７は、追従遅れ時間算出部４０６で算出された各関節Ｊ１〜Ｊ６の追従遅れ時間とゼロ時間を合わせた７つの時間の中から、全関節Ｊ１〜Ｊ６で追従遅れ時間が一致するような基準時間を選択する。そして補正指令値算出部４０７は、選択した基準とする追従遅れ時間になるような補正値を全軸分算出し、制御指令部４０５へフィードバックをする。追従遅れ時間の基準時間の選択は、共振振動幅算出部４０４にて求められた共振振動幅の変化から選択され、ゼロ時間を含めた関節Ｊ１〜Ｊ６の追従遅れ時間の短い時間順に選ばれる。
【００５４】
通知制御部４０８は、軌道誤差判定部４０２による判定により軌道誤差と閾値との比較結果を、ティーチングペンダント３００やモニタ６００等の通知部に、ユーザに対する通知を行わせる。なお、通知部としては、不図示のランプやスピーカ等でもよく、ランプによる発光やスピーカからの音声でユーザに通知してもよい。
【００５５】
図６は、ロボット装置５００の制御系による制御動作を示すフローチャートである。ＣＰＵ２５１は、動作最適化モードに移行した際に、プログラム２７０を実行することにより、上記各部４０１，４０２，４０４，４０５，４０６，４０７，４０８として機能し、以下の処理（工程）を実行する。以下、各部の動作、即ち、ロボット装置５００の制御方法の各工程についてフローチャートを用いて詳細に説明する。
【００５６】
制御指令部４０５は、空間上の２点間の教示点を移動する所定の組付け動作をロボット１００に行わせる初期の角度指令値０θｍ１＊〜０θｍ６＊をサーボ制御装置２１０に出力する（Ｓ１：動作工程）。初期の角度指令値０θｍ１＊〜０θｍ６＊は、上述したようにオフライン教示にて作成したものである。
【００５７】
サーボ制御装置２１０は、入力を受けた角度指令値０θｍ１＊〜０θｍ６＊に基づきモータ１２１に駆動電流を供給して、ロボットアーム１０１の関節Ｊ１〜Ｊ６の関節角度制御を行う。このように、制御指令部４０５は、各サーボ制御装置２１０に各角度指令値０θｍ１＊〜０θｍ６＊を出力することで、各モータ１２１の回転角度θｍ１〜θｍ６を各角度指令値０θｍ１＊〜０θｍ６＊に追従させてロボットアーム１０１を動作させる。
【００５８】
各エンコーダ１２５で検出された関節角度値θｒ１〜θｒ６は、軌道誤差算出部４０１に送られる。軌道誤差算出部４０１は、ステップＳ１にてロボットアーム１０１を動作させたときに各エンコーダ１２５により検出された関節角度値θｒ１〜θｒ６に基づき、順運動学の計算によりロボットアーム１０１の先端の実際の軌道を求める（Ｓ２：軌道算出工程）。
【００５９】
また、軌道誤差算出部４０１は、角度指令値０θｍ１＊〜０θｍ６＊からロボットアーム１０１の先端の目標の軌道を算出する。具体的には、軌道誤差算出部４０１は、角度指令値０θｍ１＊〜０θｍ６＊を１／Ｎ倍した関節角度指令値０θｒ１＊〜０θｒ６＊に基づき、順運動学の計算によりロボットアーム１０１の先端の目標の軌道を求める。この演算結果であるロボットアーム１０１の先端の目標の軌道は、記憶部（ＨＤＤ２５４等）に記憶される。なお、予めロボットアーム１０１の先端の目標の軌道を計算しておき、記憶部（ＨＤＤ２５４等）に記憶させておけば、ステップＳ２でロボットアーム１０１の手先の目標軌道を計算しなくても済む。
【００６０】
次に、軌道誤差算出部４０１は、各角度指令値０θｍ１＊〜０θｍ６＊に基づくロボットアーム１０１の先端の目標の軌道に対する、ステップＳ２で求めた実際の軌道の軌道誤差を計算する（Ｓ３：軌道誤差算出工程）。
【００６１】
図７は、ロボットアーム１０１の先端が２つの教示点間を直線補間で移動した際の進行方向に対して垂直となる座標系で、ロボットアーム１０１の先端位置の変位を経過時間毎に表わしたグラフである。図７には角度指令値に基づくロボットアーム１０１の先端位置の軌道と経過時間のグラフである波形Ｐ１と、エンコーダ１２５で検出された関節角度値から得られたロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ２を表わす。ＣＰＵ２５１は、軌道の波形Ｐ１に対する軌道の波形Ｐ２の最も大きい差分値を軌道誤差Ｚ２として算出する。
【００６２】
次に軌道誤差判定部４０２は、軌道誤差算出部４０１で算出した軌道誤差が、許容範囲を超えているか否かを判断する（Ｓ４：判断工程）。本実施形態では、許容範囲の上限は閾値Ｚｒｅｆであり、下限は閾値（−Ｚｒｅｆ）である。即ち、軌道誤差Ｚ２の絶対値が閾値Ｚｒｅｆを超えたか否かを判断する。なお、本実施形態では、許容範囲の上限の絶対値と下限の絶対値とが同じ値Ｚｒｅｆとしたが、これに限定するものではなく、異なる値であってもよい。
【００６３】
閾値Ｚｒｅｆは、予め閾値記憶部４０３に記憶されている。この閾値Ｚｒｅｆは、ユーザが任意に設定することができ、本実施形態では、ワークＷの組付けに必要な値に設定されている。
【００６４】
軌道誤差判定部４０２は、ステップＳ３で計算した軌道誤差Ｚ２と、ユーザが予め設定した閾値Ｚｒｅｆとの比較判定を行い、閾値Ｚｒｅｆを超えていなければ（Ｓ４：Ｎｏ）、組付けが問題なく可能であるので、図６のフローは終了する。その際、軌道誤差判定部４０２は、その判定結果を通知制御部４０８に伝達し、通知制御部４０８は、通知部（ティーチングペンダント３００やモニタ６００等）にユーザへの通知を行わせる。そして、初期の角度指令値０θｍ１＊〜０θｍ６＊を後の動作モードにおける生産工程で用いる角度指令値θｍ１＊〜θｍ６＊として設定する。
【００６５】
軌道誤差が許容範囲を超える場合、即ち軌道誤差Ｚ２の絶対値が閾値Ｚｒｅｆを超える場合（Ｓ４：Ｙｅｓ）、部品が組付かない可能性があるためロボットアーム１０１の動作に補正が必要であるためステップＳ５に進む。
【００６６】
次に、ステップＳ５において、追従遅れ時間算出部４０６は、各関節角度指令値０θｒ１＊〜０θｒ６＊に対する各関節角度値θｒ１〜θｒ６の追従遅れ時間を、関節ごとに求める（追従遅れ時間算出工程）。即ち、追従遅れ時間算出部４０６は、各モータ１２１が動作した際に発生する、各関節Ｊ１〜Ｊ６への角度指令値に対する追従遅れ時間を算出する。ここで、各関節Ｊ１〜Ｊ６の追従遅れ時間をｔ１〜ｔ６とする。そして、各関節Ｊ１〜Ｊ６において、各経過時間における追従遅れ時間ｔ１〜ｔ６のうちピーク値の追従遅れ時間をＴ１〜Ｔ６とする。
【００６７】
図８は、経過時間に対する関節Ｊ２，Ｊ３の関節角度指令値０θｒ２＊，０θｒ３＊及び関節角度値θｒ２，θｒ３を示すグラフである。縦軸は角度、横軸は経過時間を示す。図９は、経過時間に対する関節Ｊ２，Ｊ３の追従遅れ時間ｔ２，ｔ３を示すグラフである。縦軸は追従遅れ時間、横軸は経過時間を示す。
【００６８】
関節角度指令値０θｒ２＊に対する関節角度値θｒ２の時間的な差異が追従遅れ時間ｔ２であり、関節角度指令値０θｒ３＊に対する関節角度値θｒ３の時間的な差異が追従遅れ時間ｔ３である。図８及び図９には、関節Ｊ２，Ｊ３についてのみ図示しているが、ＣＰＵ２５１は、Ｊ１，Ｊ４〜Ｊ６についても同様に追従遅れ時間ｔ１，ｔ４〜ｔ６を求める。
【００６９】
次に、補正指令値算出部４０７は、各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６のうちの最長の追従遅れ時間以下の基準時間を設定する。ここで、基準時間は、指令値補正後に各関節Ｊ１〜Ｊ６で一致させる目標とする追従遅れ時間であり、本実施形態では、ゼロ（遅れなし）及び時間ｔ１〜ｔ６の中から設定される。
【００７０】
補正指令値算出部４０７は、基準時間に対する各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６のずれ時間Δｔ１〜Δｔ６の分、各角度指令値０θｍ１＊〜０θｍ６＊を補正して、新たな各角度指令値１θｍ１＊〜１θｍ６＊を生成する（Ｓ６：補正工程）。このように、補正指令値算出部４０７は、全関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６が１つの基準時間に一致するような補正後の角度指令値１θｍ１＊〜１θｍ６＊を算出する。
【００７１】
本実施形態では、ステップＳ６では基準時間をゼロとする。よって、基準時間に対する各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６のずれ時間Δｔ１〜Δｔ６は、ｔ１〜ｔ６である。本実施形態では、各関節Ｊ１〜Ｊ６の目標軌道に対する各関節Ｊ１〜Ｊ６の追従遅れ時間がゼロとなるように各角度指令値０θｍ１＊〜０θｍ６＊を補正する。即ち基準時間がゼロであるので、補正指令値算出部４０７は、ステップＳ６では、各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６の分、各角度指令値０θｍ１＊〜０θｍ６＊を補正（オフセット）して、新たな複数の角度指令値１θｍ１＊〜１θｍ６＊を生成する。
【００７２】
即ち、ステップＳ６では、基準時間がゼロなので関節Ｊ１への補正値１θｍ１＊は初期指令値０θｍ１＊に対して時間ｔ１分進んだ値、関節Ｊ２への補正値１θｍ２＊は初期指令値０θｍ２＊に対して時間ｔ２分進んだ値として算出する。同様に関節Ｊ３〜Ｊ６の補正値も追従遅れ時間がゼロになるような補正値を算出する。
【００７３】
なお、基準時間をゼロとした場合、各関節Ｊ１〜Ｊ６は、補正前の初期のロボットアーム１０１の動作速度よりもロボットアーム１０１の動作速度が速くなる。
【００７４】
このように、ステップＳ６では、ステップＳ１で動作した際の角度指令値０θｍ１＊〜０θｍ６＊に対して追従遅れ時間がゼロになるような補正値１θｍ１＊〜１θｍ６＊を補正指令値算出部４０７で算出し、制御指令部４０５にフィードバックをする。
【００７５】
ここで、補正後の角度指令値１θｍ１＊〜１θｍ６＊は、記憶部（ＨＤＤ２５４等）に、初期の角度指令値０θｍ１＊〜０θｍ６＊と区別して記憶される。
【００７６】
図１０は、経過時間に対する初期の角度指令値０θｍ２＊，０θｍ３＊及び補正後の角度指令値１θｍ２＊，１θｍ３＊を示すグラフである。図９には、初期の角度指令値０θｍ２＊，０θｍ３＊に対し、追従遅れ補正時間（ずれ時間）Δｔ２＝ｔ２，Δｔ３＝ｔ３分、指令タイミングを進めた補正後の角度指令値１θｍ２＊，１θｍ３＊を図示している。ステップＳ６では、全軸の追従遅れ時間がゼロになるように指令タイミングが進められた補正値１θｍ２＊，１θｍ３＊が算出される。図１０に不図示の他の関節Ｊ１，Ｊ４〜Ｊ６についても関節Ｊ２，Ｊ３と同様にＣＰＵ２５１により算出する。
【００７７】
ＣＰＵ２５１は、ステップＳ６（補正工程）にて補正した各角度指令値１θｍ１＊〜１θｍ６＊を用いて、ステップＳ１〜Ｓ４（動作工程、軌道算出工程、軌道誤差算出工程及び判断工程）と同様の処理を再度実行する。
【００７８】
即ち、制御指令部４０５は、各モータ１２１の回転角度θｍ１〜θｍ６を補正後の各角度指令値１θｍ１＊〜１θｍ６＊に追従させてロボットアームを再動作させる（Ｓ７：動作工程）。
【００７９】
軌道誤差算出部４０１は、ステップＳ７にてロボットアーム１０１を再動作させたときに各エンコーダ１２５により検出された関節角度値θｒ１〜θｒ６に基づき、ロボットアーム１０１の先端の実際の軌道を求める（Ｓ８：軌道算出工程）。
【００８０】
軌道誤差算出部４０１は、角度指令値０θｍ１＊〜０θｍ６＊に基づくロボットアーム１０１の先端の目標の軌道に対する、ステップＳ８で求めた実際の軌道の軌道誤差Ｚ３を算出する（Ｓ９：軌道誤差算出工程）。軌道誤差判定部４０２は、軌道誤差Ｚ３が、許容範囲を超えているか否かを判断する（Ｓ１０：判断工程）。
【００８１】
図１１は、ロボットアーム１０１の先端が２点の教示点間を直線で移動した際の進行方向に対して垂直となる座標系で、ロボットアーム１０１の先端位置の変位を経過時間毎に表わしたグラフである。図１１には、初期の角度指令値０θｍ１＊〜０θｍ６＊に基づいて動作した際のロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ２を図示している。また、図１１には、補正指令値１θｍ１＊〜１θｍ６＊に基づいて動作した際のロボットアーム１０１の先端位置の軌道の経過時間のグラフである波形Ｐ３を図示している。そして、目標の軌道に対するそれぞれの指令値を基にした軌道の軌道誤差を軌道誤差Ｚ２、軌道誤差Ｚ３として表わす。
【００８２】
軌道誤差判定部４０２は、ステップＳ８で計算した軌道誤差Ｚ３と、ユーザが予め設定した閾値Ｚｒｅｆとの比較判定を行い、閾値Ｚｒｅｆを超えていなければ（Ｓ１０：Ｎｏ）、組付けが問題なく可能であるので、図６のフローは終了する。その際、軌道誤差判定部４０２は、その判定結果を通知制御部４０８に伝達し、通知制御部４０８は、通知部（ティーチングペンダント３００やモニタ６００等）にユーザへの通知を行わせる。そして、補正後の角度指令値１θｍ１＊〜１θｍ６＊を後の動作モードにおける生産工程で用いる角度指令値θｍ１＊〜θｍ６＊として設定する。
【００８３】
図１１の例の場合、Ｚｒｅｆ＜Ｚ３であるため、次のステップＳ１１に進む。ステップＳ１０で軌道誤差が許容範囲を超えていると判断した場合（Ｓ１０：Ｙｅｓ）、共振振動幅算出部４０４は、最初にステップＳ３で算出した軌道からロボットアーム１０１の共振振動による振幅のピーク値（第１ピーク値）を求める。また、共振振動幅算出部４０４は、再度ステップＳ８で算出した軌道からロボットアーム１０１の共振振動による振幅のピーク値（第２ピーク値）を算出する（Ｓ１１：共振算出工程）。
【００８４】
次に、共振振動幅算出部４０４は、第２ピーク値が第１ピーク値よりも高いか否かを判断する（Ｓ１２：共振判断工程）。
【００８５】
即ち、ステップＳ１２ではステップＳ１０での再動作時の軌道誤差が閾値を超えた原因判断を行う。共振振動幅算出部４０４は、軌道誤差算出部４０１からステップＳ３での初期動作時の軌道誤差波形Ｐ２とステップＳ９での補正動作時の軌道誤差波形Ｐ３と取得し、高速フーリエ変換（ＦＦＴ）を行いロボットアーム先端位置の振動周波数と振幅を算出する。共振振動幅算出部４０４は、算出した動作軌道波形の共振振動の振幅変化に基づいて、ステップＳ１０において軌道誤差の絶対値が閾値を超えた原因が、補正により動作加速度が過大になったためか否かを判断する。再動作時に全軸の追従遅れ時間を一致させる基準時間の設定が短すぎた場合、動作加速度が上がり共振振動幅が増加する。つまり共振振動幅の増加変化を検出することで補正時間が短すぎたか否かが判断される。
【００８６】
図１２は、ステップＳ３で算出したロボットアーム１０１の先端位置軌道の波形Ｐ２をＦＦＴした結果の波形Ｐ２０と、ステップＳ８で算出したロボットアーム１０１の先端位置軌道の波形Ｐ３をＦＦＴした結果の波形Ｐ３０を示すグラフである。波形Ｐ２０及び波形Ｐ３０は、周波数ｆ１と周波数ｆ１よりも高い周波数ｆ２にピークを持つ。周波数ｆ１は追従遅れ時間が関節Ｊ１〜Ｊ６間で一致しないことに起因するもので、周波数ｆ２はロボットアーム１０１の関節Ｊ１〜Ｊ６の共振振動に起因するものである。
【００８７】
図１２では、波形Ｐ２０の周波数ｆ２での振幅はＷ２２である。波形Ｐ３０の周波数ｆ２での振幅はＷ２３である。図１２の例では、Ｗ２２＞Ｗ２３であり、共振振動幅は増加していないため（Ｓ１２：Ｎｏ）、ステップＳ１３に進む。
【００８８】
ステップＳ１３では、再度ステップＳ１０で軌道誤差が許容範囲を超えていると判断した場合に、補正指令値算出部４０７が、最長の追従遅れ時間以下の範囲で基準時間を変更して再度、初期の各角度指令値０θｍ１＊〜０θｍ６＊を補正する。これにより、補正後の新たな角度指令値１θｍ１＊〜１θｍ６＊を得る。
【００８９】
本実施形態では、補正指令値算出部４０７は、ステップＳ１３において基準時間を変更する際に、基準時間を、各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６の中から選択して該選択した追従遅れ時間に設定する。その際、基準時間を、各関節Ｊ１〜Ｊ６の追従遅れ時間ｔ１〜ｔ６のうち未選択の追従遅れ時間の中から選択して該追従遅れ時間に設定する。つまり、１巡目のステップＳ１３では、全ての追従遅れ時間ｔ１〜ｔ６が未選択であるので、基準時間は、これら時間ｔ１〜ｔ６の中から設定される。
【００９０】
そして、本実施形態では、ステップＳ１３で基準時間を変更する際に、基準時間を、未選択の追従遅れ時間ｔ１〜ｔ６のうち最短の追従遅れ時間を選択して該追従遅れ時間に設定する。
【００９１】
図１３は、ステップＳ５で算出した、ロボットアーム１０１の各関節Ｊ１〜Ｊ６の追従遅れ時間（ピーク値）Ｔ１〜Ｔ６を示した図である。最短の追従遅れ時間が関節Ｊ６の追従遅れ時間Ｔ６で、最長の追従遅れ時間が関節Ｊ２の追従遅れ時間Ｔ２である。図１３の例で、１巡目のステップＳ１２では、関節Ｊ１〜Ｊ６の追従遅れ時間Ｔ１〜Ｔ６のうち最短の追従遅れ時間Ｔ６を選択する。そして、基準時間を、選択した追従遅れ時間Ｔ６を含む追従遅れ時間ｔ６とする。このように、本実施形態では、ステップＳ１３の追従遅れ時間の選択について、比較が容易となるピーク値を示す追従遅れ時間Ｔ１〜Ｔ６を用いている。
【００９２】
そして、関節Ｊ１については、補正後の角度指令値１θｍ１＊は、初期の角度指令値０θｍ１＊に対してずれ時間Δｔ１（＝ｔ１−ｔ６）進んだ値とする。関節Ｊ２については、補正後の角度指令値１θｍ２＊は、初期の角度指令値０θｍ２＊に対してずれ時間Δｔ２（＝ｔ２−ｔ６）進んだ値とする。残りの関節Ｊ３〜Ｊ５についても同様に角度指令値１θｍ３＊〜１θｍ５＊を求める。
【００９３】
なお、角度指令値１θｍ６＊については、基準時間を時間ｔ６としているので、そのままの値（０θｍ６＊）となる。このように、選択された追従遅れ時間ｔ６に対応する関節Ｊ６の角度指令値１θｍ６＊は、補正演算をする必要がないので、ＣＰＵ２５１における演算負荷が減る。
【００９４】
ステップＳ１３にて補正した角度指令値を求めた後、再度、ステップＳ７の処理に移行して、ステップＳ１０で軌道誤差が許容範囲に収まるまでステップＳ７〜Ｓ１３の処理を繰り返す。
【００９５】
これにより、ステップＳ１３では、図１３の各関節Ｊ１〜Ｊ６の追従遅れ時間Ｔ１〜Ｔ６のうち、未選択の追従遅れ時間の中から短い順で選択される。図１３の例では、Ｔ６、Ｔ４、Ｔ１、Ｔ５、Ｔ３、Ｔ２の順で選択される。
【００９６】
例えば、２巡目ではＴ４が選択されるので、基準時間は追従遅れ時間ｔ４となる。その際、ずれ時間Δｔ１〜Δｔ３、Δｔ５は正の値であり、各角度指令値１θｍ１＊〜１θｍ３＊、１θｍ５＊は、各角度指令値０θｍ１＊〜０θｍ３＊、０θｍ５＊に対してずれ時間分進んだ値となる。また、ずれ時間Δｔ４はゼロであり、角度指令値１θｍ４＊は、角度指令値０θｍ４＊のままである。また、ずれ時間Δｔ６は負の値であり、角度指令値１θｍ６＊は、角度指令値０θｍ６＊に対してずれ時間分遅れた値となる。このように、２巡目では、関節Ｊ４の追従遅れ時間ｔ４に合わせて各関節Ｊ１〜Ｊ３、Ｊ５、Ｊ６の追従遅れ時間が揃えられる。
【００９７】
なお、６巡目では、Ｔ２が選択されるので、基準時間は追従遅れ時間ｔ２となる。その際、ずれ時間Δｔ１，Δｔ３〜Δｔ６は負の値であり、各角度指令値１θｍ１＊，１θｍ３＊〜１θｍ６＊は、各角度指令値０θｍ１＊，０θｍ３＊〜０θｍ６＊に対してずれ時間分遅れた値となる。また、ずれ時間Δｔ２はゼロであり、角度指令値１θｍ２＊は、角度指令値０θｍ２＊のままである。このように、６巡目では、関節Ｊ２の追従遅れ時間ｔ２に合わせて各関節Ｊ１、Ｊ３〜Ｊ６の追従遅れ時間が揃えられる。以上、本実施形態では、最長の追従遅れ時間ｔ２以下で基準時間が設定される。つまり、一番遅い関節Ｊ２に各関節Ｊ１、Ｊ３〜Ｊ６の追従遅れ時間を揃えたとしても、ロボットアーム１０１の動作速度は低下しない。
【００９８】
共振振動幅算出部４０４が、ステップＳ１２にて第２ピーク値が第１ピーク値よりも高いと判断した場合（Ｓ１２：Ｙｅｓ）、通知制御部４０８の制御に移行する。通知制御部４０８は、許容範囲（即ち閾値Ｚｒｅｆ）の変更を促す情報を、モニタ６００やティーチングペンダント３００等の通知部に通知させる（Ｓ１４：通知制御工程）。このように、ステップＳ１４では通知制御部４０８が、ユーザに対して予め閾値記憶部４０３に設定しておいた軌道誤差閾値Ｚｒｅｆの見直し要求の通知を行わせる。通知制御部４０８は、ステップＳ１２における共振振動幅算出部４０４の判定結果を受け、ユーザへ通知を行った後、フローを終了する。
【００９９】
以上、本実施形態によれば、ロボットアーム１０１の関節Ｊ１〜Ｊ６同士の追従遅れ時間を基準時間に一致させ、追従遅れ時間がばらつくのを抑制して、実際の動作軌道が目標軌道から外れるのを抑制している。更に、本実施形態によれば、ロボットアーム１０１の各関節Ｊ１〜Ｊ６の追従遅れ時間を揃える基準時間を、ステップＳ５で求めた最長の追従遅れ時間以下の範囲内でロボットアーム１０１の先端の振動が低減するように変更している。これにより、ロボットアーム１０１の動作速度が低減することなく、ロボットアーム１０１の先端の軌道誤差を低減させることができる。
【０１００】
そして組付けで重要となるロボットアーム１０１の先端軌道の軌道誤差が小さくなるので、ロボットアーム１０１の動作開始から動作終了までの総合の動作時間が短くなり、ロボット１００による組立製品の生産性が向上する。
【０１０１】
また、ステップＳ６において最初は基準時間をゼロに設定して角度指令値を補正しているので、この補正でステップＳ１０にて軌道誤差が許容範囲内であると判断されれば、ロボットアーム１０１の動作速度は格段に向上する。これにより、ロボット１００による組立製品の生産性が格段に向上する。
【０１０２】
また、ステップＳ１３で、基準時間を変更する際に、基準時間を、追従遅れ時間ｔ１〜ｔ６の中から選択しているので、基準時間を演算する必要がなく、ＣＰＵ２５１による演算負荷が低減する。
【０１０３】
また、基準時間を変更する際に、未選択の追従遅れ時間のうち最短の追従遅れ時間を選択して基準時間に設定しているので、軌道誤差が許容範囲を満たすものの中で、最もロボットアーム１０１の動作速度が速くなる角度指令値が設定されることになる。よって、ロボット１００による組立製品の生産性の向上につながる。
【０１０４】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。
【０１０５】
［その他の実施形態］
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１０６】
また、上記実施形態では、ステップＳ１１にて共振振動幅を算出し、ステップＳ１２にて共振判断を行い、共振振動幅が増加したときステップＳ１４を実行する場合について説明したが、ステップＳ１１，Ｓ１２，Ｓ１４の処理は、必要に応じて省略可能である。
【０１０７】
また、上記実施形態では、ステップＳ１３で未選択の追従遅れ時間の中から短い順に選択して基準時間を設定したが、これに限定するものではなく、例えば未選択の追従遅れ時間の中からランダムに追従遅れ時間を選択して基準時間に設定してもよい。
【０１０８】
また、上記実施形態では、ステップＳ６で最初に基準時間をゼロに設定したが、これに限定するものではなく、例えば追従遅れ時間ｔ６に設定してもよい。
【０１０９】
また、上記実施形態では、基準時間を、ゼロ及び追従遅れ時間ｔ１〜ｔ６の中から選択したが、これに限定するものではない。例えば、時間ｔ１と時間ｔ２との中間の時間や時間ｔ２と時間ｔ３との中間の時間を求めて、基準時間に設定してもよい。この場合、基準時間を演算する必要があるので、ＣＰＵ２５１による演算負荷が上記実施形態よりも増加することにはなるが、上記実施形態と同様、ロボットアーム１０１の振動を抑制しつつロボットアーム１０１の動作速度を向上させることができる。
【０１１０】
上記実施形態では、ロボットアーム１０１が、垂直多関節型（シリアルリンク型）のロボットアームである場合について説明したが、これに限定するものではない。水平多関節型のロボットアームやパラレルリンク型のロボットアームなどであっても本発明は適用可能である。
【符号の説明】
【０１１１】
１０１…ロボットアーム、１２１…モータ、１２２…エンコーダ（モータ角度検出部）、１２３…減速機、１２５…エンコーダ（関節角度検出部）、２５１…ＣＰＵ（制御部）、５００…ロボット装置
【図１】
2016078149.tif 000003
【図２】
2016078149.tif 000004
【図３】
2016078149.tif 000005
【図４】
2016078149.tif 000006
【図５】
2016078149.tif 000007
【図６】
2016078149.tif 000008
【図７】
2016078149.tif 000009
【図８】
2016078149.tif 000010
【図９】
2016078149.tif 000011
【図１０】
2016078149.tif 000012
【図１１】
2016078149.tif 000013
【図１２】
2016078149.tif 000014
【図１３】
2016078149.tif 000015