JP2014128843A - ロボットアーム教示システム及びロボットアーム教示方法 - Google Patents

Abstract

【課題】教示者によるロボットアームの操作感を向上すること。
【解決手段】本発明に係るロボットアーム教示システムは、複数のアームを含むロボットアームに対する教示を行うためのものである。アーム間の関節部に設けられ、関節部を駆動するモータと、アーム間の関節部に設けられ、関節部の角度を検出するエンコーダと、アーム間の関節部に設けられ、関節部のトルクを検出するトルクセンサと、モータを制御する制御部を備える。制御部は、エンコーダが検出した角度に基づいて算出した角速度が第１の速度閾値以上であると判定した場合、又は、トルクセンサが検出したトルクが第１のトルク閾値以上であると判定した場合、モータに対してトルク制御を行う第１の状態に遷移し、エンコーダが検出した角度に基づいて算出した角速度が第２の速度閾値未満であると判定した場合、モータに対してトルク制御に加えて位置制御を行う第２の状態に遷移する。
【選択図】図１

Description

本発明は、ロボットアーム教示システム及びロボットアーム教示方法に関する。

特許文献１〜３には、ロボットに対してダイレクトティーチング（直接教示）を行う技術が開示されている。

特許文献１には、ダイレクトティーチング時の操作性向上のための改良に関する技術が開示されている。特許文献１に開示の技術は、アームの手先部に設置された操作装置の力センサにより検出された操作力が第１のスレッショルド値以上となる条件、及び、手先部の速度が第２のスレッショルド値以上となる条件のうち、少なくとも１つの条件を満たす場合に、ダイレクトティーチングモードに切り替わる。また、力センサにより検出された操作力が第１のスレッショルド値よりも小さく、かつ手先部の速度が第２のスレッショルド値よりも低い場合に、ダイレクトティーチングモードから通常モードに切り替わる。

特許文献２には、誘導力を加える教示作業者の意図を理解して教示作業中に力制御の操作感を自動的に変更することで、教示作業の能率と位置決め精度を向上するための技術が開示されている。特許文献２に開示のロボットの直接教示装置は、テーブルに示す遷移条件に基づいて、エンドエフェクタの移動速度のサンプリング時間当たりの変化と、操作ハンドルの力センサにより検出された誘導力のサンプリング時間当たりの変化から、ロボット操作の状態遷移を行う。

特許文献３には、スイッチなどを用いることなしに、操作感を容易に変更することができるようにするための技術が開示されている。特許文献３に開示のロボット制御装置は、操作部を操作することで操作部に取り付けられた力センサに作用した力に基づいて、ダイレクトティーチ時の動作特定を設定する。

しかしながら、特許文献１〜３に開示の技術は、モード（状態、動作特性）によらず、常に、同一のサーボ制御方式（速度制御、位置速度制御、又は力制御）を実施しているため、教示者からのアームに対する柔軟な制御が困難であり、操作感が悪いという問題がある。また、特許文献１〜３に開示の技術は、直接教示をする際に、操作装置（操作ハンドル、操作部）のみに対してしか教示操作を行うことができず、教示者からのアームに対する柔軟な制御が困難であり、操作感が悪いという問題もある。

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

本発明は、上述した知見に基づいてなされたものであって、教示者によるロボットアームの操作感を向上することができるロボットアーム教示システム及びロボットアーム教示方法を提供することを目的とする。

本発明の第１の態様に係るロボットアーム教示システムは、複数のアームを含むロボットアームに対する教示を行うためのロボットアーム教示システムであって、前記アーム間の関節部に設けられ、当該関節部を駆動するモータと、前記アーム間の関節部に設けられ、当該関節部の角度を検出するエンコーダと、前記アーム間の関節部に設けられ、当該関節部のトルクを検出するトルクセンサと、前記エンコーダが検出した角度及び前記トルクセンサが検出したトルクの少なくとも１つに基づいて状態を遷移するとともに、遷移後の状態に応じて前記モータを制御する制御部と、を備え、前記制御部は、前記エンコーダが検出した角度に基づいて算出した角速度が第１の速度閾値以上であると判定した場合、又は、前記トルクセンサが検出したトルクが第１のトルク閾値以上であると判定した場合、前記モータに対してトルク制御を行う第１の状態に遷移し、前記エンコーダが検出した角度に基づいて算出した角速度が第２の速度閾値未満であると判定した場合、前記モータに対して前記トルク制御に加えて位置制御を行う第２の状態に遷移するものである。

本発明の第２の態様に係るロボットアーム教示方法は、複数のアームを含むロボットアームに対する教示を行うためのロボットアーム教示方法であって、前記アーム間の関節部の角度、及び、前記アーム間の関節部のトルクを検出するステップと、前記検出した角度に基づいて算出した角速度が第１の速度閾値以上であると判定した場合、又は、前記検出したトルクが第１のトルク閾値以上であると判定した場合、前記アーム間の関節部に設けられたモータに対してトルク制御を行う第１の状態に遷移し、前記検出した角度に基づいて算出した角速度が第２の速度閾値未満であると判定した場合、前記モータに対して前記トルク制御に加えて位置制御を行う第２の状態に遷移するステップと、遷移後の状態に応じて前記モータを制御するステップと、を備えたものである。

上述した本発明の各態様によれば、教示者によるロボットアームの操作感を向上することができるロボットアーム教示システム及びロボットアーム教示方法を提供することができる。

実施の形態に係るロボットアーム教示システムの構成図である。 実施の形態に係るロボットアームの制御の概要を説明するための図である。 実施の形態に係るロボットアームの制御の概要を説明するための図である。 実施の形態に係るロボットアームの制御の概要を説明するための図である。 実施の形態に係る制御装置の構成図である。 実施の形態に係るロボットアーム教示システムの状態遷移図である。 実施の形態に係るロボットアーム教示システムの状態遷移処理を示すフローチャートである。 状態遷移処理における状態遷移条件を示す図である。 ロボットアーム教示システムの状態と制御方式との関係を示す図である。 状態遷移条件において使用される閾値を説明する図である。 状態遷移条件において使用される閾値の大小関係を示す図である。 データの入出力に対する指令トルク算出処理の流れを示すブロック図である。 指令トルク算出処理における各データ及び各処理を説明するための図である。

以下に図面を参照しながら、本発明の好適な実施の形態について説明する。以下の実施の形態に示す具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、それに限定されるものではない。また、以下の記載及び図面では、説明の明確化のため、当業者にとって自明な事項等については、適宜、省略及び簡略化がなされている。

＜発明の実施の形態＞
図１を参照して、実施の形態に係るロボットアーム教示システム１の構成について説明する。図１は、実施の形態に係るロボットアーム教示システム１の構成図である。

ロボットアーム教示システム１は、ロボットアーム２及び制御装置３を有する。本実施の形態では、ロボットアーム２が３関節４自由度モデルである場合について例示するが、これに限られない。ロボットアーム２及び制御装置３は、任意の情報を送受信可能とする信号線によって相互に接続されている。

ロボットアーム２は、手先部１０、関節部１１〜１３、肩部１４、本体部１５、及びアーム部２０〜２４を有する。

アーム部２０の一端には、手先部１０が連結されており、アーム部２０の他端には、関節部１１が連結されている。アーム部２１の一端には、関節部１１が連結されており、アーム部２１の他端には、関節部１２が連結されている。アーム部２２の一端には、関節部１２が連結されており、アーム部２２の他端には関節部１３が連結されている。アーム部２３の一端には、関節部１３が連結されており、アーム部２３の他端には、肩部１４が連結されている。アーム部２４の一端には、肩部１４が連結されており、アーム部２４の他端には本体部１５が連結されている。本体部１５は、上述した信号線によって制御装置３と接続されている。

アーム部２０、２１は、関節部１１を軸として回動可能となっている。アーム部２１、２２は、関節部１２を軸として回動可能となっている。アーム部２２、２３は、関節部１３を軸として回動可能となっている。アーム部２３、２４は、肩部１４に対して鉛直軸周りに回動可能となっている。関節部１１〜１３のそれぞれは、アーム部２０〜２３の回動軸が相互に平行するようにアーム部２０〜２３を介して連結されている。

このような構成により、ロボットアーム２は、関節部１４を中心として鉛直軸周りに回動するとともに、アーム部２０〜２３のそれぞれと直交し、かつ相互に平行する軸周りに関節部１１〜１３を中心として回動するように動作する。

ここで、関節部１１〜１３のそれぞれは、エンコーダ３０、トルクセンサ４０、モータ５０、及び減速機６０を含んでいる。

モータ５０は、制御装置３から供給される駆動電流によって駆動される。前腕６１及び上腕６２は、モータ５０が駆動されることで、減速機６０によって減速された回転速度で関節部を軸として回動する。ここで、前腕６１は、関節部よりも手先部１０寄りのアーム部を示しており、上腕６２は、関節部よりも肩部１４寄りのアーム部を示している。例えば、関節部１２に関しては、前腕６１はアーム部２１に相当し、上腕６２はアーム部２２に相当する。

エンコーダ３０は、モータ５０の回転角度（関節部の関節軸の関節角度）を検出し、検出した回転角度（関節角度）を示す角度信号を生成して本体部１５に出力する。エンコーダ３０から出力された角度信号は、本体部１５を介して制御装置３に送信される。

トルクセンサ４０は、関節部の関節軸にかかるトルクを検出し、検出したトルクを示すトルク信号を生成して本体部１５に出力する。トルクセンサ４０から出力されたトルク信号は、本体部１５を介して制御装置３に送信される。

なお、肩部１４は、ロボットアーム２が関節部１４を中心として鉛直軸周りに回動可能となるように、エンコーダ３０、トルクセンサ４０、モータ５０、及び減速機６０が鉛直方向に並んでいる点が関節部１１〜１３と異なる。しかし、エンコーダ３０、トルクセンサ４０、モータ５０、及び減速機６０のそれぞれの動作は同様であるため、詳細な説明は省略する。

このような構成により、ロボットアーム２は、教示者によってロボットアーム２に対して直接教示が行われた際における関節角度及びトルクを制御装置３に通知することができる。そして、制御装置３は、ロボットアーム２から角度信号及びトルク信号によって通知された関節角度及びトルクを教示データとして蓄積するとともに、その関節角度及びトルクに基づいて教示者による直接教示を援けるように関節部１１〜１３及び肩部１４のモータ５０を駆動する。これによって、教示者によるロボットアーム２の直接教示における操作感を向上するようにしている。なお、教示データとして、関節角度及びトルクのうち、いずれか一方を蓄積するようにしてもよい。

続いて、図２Ａ〜図２Ｃを参照して、実施の形態に係るロボットアーム２の制御の概要について説明する。図２Ａ〜図２Ｃは、実施の形態に係るロボットアーム２の制御の概要を説明するための図である。

まず、図２Ａに示すように、教示者が静止したロボットアーム２の任意の手押しをした場合、制御装置３は、ロボットアーム２に対して、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御（トルク制御）を実施することにより、教示者が関節部等の位置を抵抗感が無く移動できるようにする。これにより、教示者が小さな力で軽快にロボットアーム２を所望の姿勢にすることが可能となる。

次に、図２Ｂに示すように、教示者がロボットアーム２の任意の手押しを止めて、所定の時間の間、ロボットアーム２の位置をキープした場合、制御装置３は、上述のトルクサーボ制御と組み合わせて、さらに位置制御も実施することにより、ロボットアーム２を停止位置で維持できるようにする。

そして、図２Ｃに示すように、教示者がロボットアーム２から手を離した場合、制御装置３は、上述のトルクサーボ制御及び位置制御を継続して実施することにより、ロボットアーム２が単独で停止位置を維持できるようにする。これにより、教示者によるロボットアーム２の位置決めを容易にすることが可能となる。

このように、制御装置３は、ロボットアーム２を制御する状態として、図２Ａに示す状態「Ｓ１：移動中」、図２Ｂに示す状態「Ｓ２：整定途中」、図２Ｃに示す状態「Ｓ３：整定」の３つの状態を管理している。そして、制御装置３は、教示者からのロボットアーム２に対する操作状況に応じて、状態を遷移させて遷移後の状態に応じたロボットアーム２の制御を実施する。これによって、教示者がロボットアーム２に対する操作感の良い柔軟な制御（柔軟な姿勢の教示）を行うことを可能とする。なお、ロボットアーム２を制御する状態は、後述するように、ロボットアーム２単位で管理するようにしてもよく、関節部（関節軸）１１〜１４単位で独立して管理するようにしてもよい。

続いて、図３を参照して、実施の形態に係る制御装置３の構成について説明する。図３は、実施の形態に係る制御装置３の構成図である。

制御装置３は、制御部７０及び駆動回路８０を有する。制御部７０は、ＣＰＵ（Central Processing Unit）７１、ＲＡＭ（Random Access Memory）７２、ＲＯＭ（Read Only Memory）７３、及びＩ／Ｏ７４を有する。制御部７０は、制御部７０として処理を実行するためのプログラムをＣＰＵ７１によって実行することによって、本実施の形態において説明するようにロボットアーム２を制御する。ＲＡＭ７２は、ＣＰＵ７１によってプログラムを実行するためにＲＯＭ７３からプログラムがロードされるとともに、プログラムの実行に際して演算されるデータが一時的に格納される。ＲＯＭ７３は、ＣＰＵ７１によって実行させるプログラム、及び、ロボットアーム２の制御演算に利用する各種パラメータ等が格納されている。Ｉ／Ｏ７４は、例えば、Ｉ／Ｏ(Input/Output)ポートである。

駆動回路８０は、ロボットアーム２を制御する際に制御部７０からＩ／Ｏ７４を介して出力される、第１乃至第４のモータ５１〜５４のそれぞれに対するトルク指令値に基づいて、第１乃至第４のモータ５１〜５４のそれぞれを駆動するための駆動電流を生成し、第１乃至第４のモータ５１〜５４のそれぞれに供給する。

ここで、第１のモータ５１は、関節部１１に含まれるモータ５０を示しており、第２のモータ５２は、関節部１２に含まれるモータ５０を示しており、第３のモータ５３は、関節部１３に含まれるモータ５０を示しており、第４のモータ５４は、関節部１４に含まれるモータ５０を示している。

また、第１のエンコーダ３１は、関節部１１に含まれるエンコーダ３０を示しており、第２のエンコーダ３２は、関節部１２に含まれるエンコーダ３０を示しており、第３のエンコーダ３３は、関節部１３に含まれるエンコーダ３０を示しており、第４のエンコーダ３４は、関節部１４に含まれるエンコーダ３０を示している。

また、第１のトルクセンサ４１は、関節部１１に含まれるトルクセンサ４０を示しており、第２のトルクセンサ４２は、関節部１２に含まれるトルクセンサ４０を示しており、第３のトルクセンサ４３は、関節部１３に含まれるトルクセンサ４０を示しており、第４のトルクセンサ４４は、関節部１４に含まれるトルクセンサ４０を示している。

制御部７０は、第１乃至第４のエンコーダ３１〜３４から出力される角度信号が示す関節角度、及び、第１乃至第４のトルクセンサ４１〜４４から出力されるトルク信号が示すトルクに基づいて、状態を遷移するか否かを判定し、判定結果に応じた状態に、ロボットアーム２を制御する状態を遷移させる。

制御部７０は、第１乃至第４のエンコーダ３１〜３４から出力される角度信号が示す関節角度、及び、第１乃至第４のトルクセンサ４１〜４４から出力されるトルク信号が示すトルクに基づいて、状態に応じたロボットアーム２の制御演算を行い、第１乃至第４のモータ５１〜５４のそれぞれに対するトルク指令値を算出する。

これによって、制御部７０は、上述したように、教示者がロボットアーム２に対して操作感の良い柔軟な制御を行うことが可能となるようにトルク指令値を生成してロボットアーム２を制御する。

続いて、図４〜図７を参照して、実施の形態に係るロボットアーム教示システム１の状態遷移処理について説明する。図４は、実施の形態に係るロボットアーム教示システム１の状態遷移図である。図５は、実施の形態に係るロボットアーム教示システム１の状態遷移処理を示すフローチャートである。図６は、実施の形態に係る状態遷移処理における状態遷移条件を示す図である。図７は、実施の形態に係るロボットアーム教示システム１の状態と制御方式との関係を示す図である。

制御部７０は、所定の一定時間間隔（制御周期時間）毎に、第１乃至第４のエンコーダ３１〜３４から送信された角度信号、及び、第１乃至第４のトルクセンサ４１〜４４から送信されたトルク信号をＩ／Ｏ７４を介して取得する。制御部７０は、角度信号のそれぞれが示す関節角度を、関節部１１〜１４の関節軸のそれぞれの関節角度として取得し、トルク信号のそれぞれが示すトルクを、関節部１１〜１４の関節軸のそれぞれのトルクとして取得する（Ｓ１０）。ここで、関節軸毎に独立して状態を管理する場合には、ステップＳ１１以降の処理は、関節軸毎に独立して実施される。

制御部７０は、前回状態（現在の状態）が、「Ｓ１：移動中」、「Ｓ２：整定途中」、「Ｓ３：整定」のうち、いずれの状態であるかを判定する（Ｓ１１）。

前回状態が「Ｓ３：整定」である場合（Ｓ１１で「Ｓ３：整定」）、制御部７０は、遷移条件Ｃ１が成立するか否かを判定する（Ｓ１２）。具体的には、遷移条件Ｃ１として、各関節軸の速度とトルク出力を判定し、遷移条件Ｃ１に付随する遷移条件Ｃ１−１、Ｃ１−２のうち、何れかの遷移条件を満たすと判定した場合、遷移条件Ｃ３が成立すると判定する。

遷移条件Ｃ１−１では、教示者によるロボットアーム２の姿勢変更が開始され、ロボットアーム２が素早く動かされたことを検出するために、制御部７０は、各関節軸の速度を判定する。具体的には、制御部７０は、今回にステップＳ１０で取得した関節部１１〜１３及び肩部１４のそれぞれの関節角度と、前回にステップＳ１０で取得した関節部１１〜１３及び肩部１４のそれぞれの関節角度とに基づいて、関節部１１〜１３及び肩部１４の関節軸のそれぞれの速度（角速度）を算出する。

具体的には、制御部７０は、次式（１）に示すように、前回の関節角度と今回の関節角度の差分を、前回の関節角度を取得してから今回の関節角度を取得するまでの時間で割ることで速度を算出する。

角速度[rad/sec] ＝
(今回関節角度[rad]−前回関節角度[rad])／制御周期時間[sec] ・・・ （１）

制御部７０は、各関節軸の速度が、僅かな速度である「速度閾値(1)」以上となったか否かを判定する。そして、制御部７０は、関節軸の速度が「速度閾値(1)」以上となったと判定した場合、遷移条件Ｃ１−１を満たすと判定し、関節軸の速度が「速度閾値(1)」以上となっていないと判定した場合、遷移条件Ｃ１−１を満たさないと判定する。制御部７０は、例えば、少なくとも１つの関節軸の速度が「速度閾値(1)」以上となったと判定した場合、遷移条件Ｃ１−１を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(1)」以上となったか否かを判定するようにすればよい。

遷移条件Ｃ１−２では、教示者によりロボットアーム２に外力が加えられたことを検出するために、制御部７０は、各関節軸のトルク出力を判定する。具体的には、制御部７０は、ステップＳ１０で取得した関節部１１〜１３及び肩部１４のそれぞれのトルクが「トルク閾値(1)」以上となったか否かを判定する。そして、制御部７０は、トルクが「トルク閾値(1)」以上となったと判定した場合、遷移条件Ｃ１−２を満たすと判定し、トルクが「トルク閾値(1)」以上となっていないと判定した場合、遷移条件Ｃ１−２を満たさないと判定する。制御部７０は、例えば、少なくとも１つの関節軸のトルクが「トルク閾値(1)」以上となったと判定した場合、遷移条件Ｃ１−２を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸のトルクを算出し、算出したトルクが「トルク閾値(1)」以上となったか否かを判定するようにすればよい。

ここで、「トルク閾値(1)」は、ロボットアーム２の姿勢に応じて変動させるようにしてもよい。関節部に対して大きな重力トルクが発生する姿勢であるほど、自重によって大きなトルクが検出されてしまうため、「トルク閾値(1)」も大きくする方が好ましいからである。ここで、重力トルクは常に下げる方向に強く作用しているため、教示者が関節部を手押しする際には、ロボットアーム２を上げる方向に手押ししているか、ロボットアーム２を下げる方向に手押ししているかに応じて、「トルク閾値(1)」を変化させるとよい。すなわち、制御部７０は、教示者がロボットアーム２を下げる方向に手押ししていると判定される場合には、大きな閾値を「トルク閾値(1)」として使用し、教示者がロボットアーム２を上げる方向に手押ししていると判定される場合には、それよりも小さな閾値を「トルク閾値(1)」として使用する。

なお、教示者が、ロボットアーム２を下げる方向に手押ししているか、ロボットアーム２を上げる方向に手押ししているかは、任意に予め定めた判定基準に従って判定するようにしてよい。例えば、ステップＳ１０で取得した各関節軸の関節角度に基づいて算出した各関節軸の位置と、過去（例えば、前回）にステップＳ１０で取得した各関節軸の関節角度に基づいて算出した過去の各関節軸の位置とに基づいて、所定数の関節軸が過去の関節軸の位置よりも今回の関節軸の位置の方が下がっている場合、あるいはステップＳ１０で取得した各関節軸のトルクに基づいて、所定数の関節軸のトルクが重力トルクと同等の向きのロボットアーム２を下げる方向に加えられている場合には、教示者がロボットアーム２を下げる方向に手押ししていると判定し、所定数の関節軸が過去の関節軸の位置よりも今回の関節軸の位置の方が上がっている場合、あるいは所定数の関節軸のトルクが重力トルクと逆の向きのロボットアーム２を上げる方向に加えられている場合には、教示者がロボットアーム２を上げる方向に手押ししていると判定するようにしてもよい。また、重力補償において算出される重力補償トルクの変化（今回に算出した重力補償トルクと前回に算出した重力補償トルクとの差）に基づいて、教示者がロボットアーム２を上げる方向に手押ししているか又は下げる方向に手押ししているかを判定してもよい。例えば、所定数の関節軸の重力補償トルクが増加している場合、教示者がロボットアーム２を上げる方向に手押ししていると判定し、所定数の重力補償トルクが減少している場合、教示者がロボットアーム２を下げる方向に手押ししていると判定するようにすればよい。

ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸（又はそれよりも手先部１０側の関節軸）の位置が下がっているか又は上がっているか、対象の関節軸のトルクが重力トルクと同等の向きに加えられているか又は逆の向きに加えられているか、もしくは、対象の関節軸の重力補償トルクが増加しているか又は減少しているかによって、同様の判定するようにすればよい。

遷移条件Ｃ１が成立すると判定した場合（Ｓ１２でＹｅｓ）、制御部７０は、「Ｓ１：移動中」への状態遷移を行う（Ｓ１３）。遷移条件Ｃ１が成立しないと判定した場合（Ｓ１２でＮｏ）、制御部７０は、「Ｓ３：整定」の状態を維持する。

一方、前回状態が「Ｓ１：移動中」である場合（Ｓ１１で「Ｓ１：移動中」）、制御部７０は、遷移条件Ｃ２が成立するか否かを判定する（Ｓ１４）。具体的には、遷移条件Ｃ２として、各関節軸の速度と継続時間を判定し、遷移条件Ｃ２に付随する遷移条件Ｃ２−１、Ｃ２−２のうち、全ての遷移条件を満たすと判定した場合、遷移条件Ｃ２が成立すると判定する。

遷移条件Ｃ２−１では、教示者によるロボットアーム２の姿勢変更が完了に近い状況となり、ロボットアーム２の位置が停止しようとしていることを検出するために、制御部７０は、各関節軸の速度を判定する。具体的には、制御部７０は、各関節軸の速度が、停止し始めていることを示す「速度閾値(2)」未満であるか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部７０は、算出した各関節軸の速度が「速度閾値(2)」未満であると判定した場合、遷移条件Ｃ２−１を満たすと判定し、算出した各関節軸の速度が「速度閾値(2)」未満でないと判定した場合、遷移条件Ｃ２−１を満たさないと判定する。制御部７０は、例えば、全ての関節軸の速度が「速度閾値(2)」未満であると判定した場合、遷移条件Ｃ２−１を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(2)」以上となったか否かを判定するようにすればよい。

遷移条件Ｃ２−２では、教示者によるロボットアーム２の姿勢変更が完了に近い状況が継続しており、ロボットアーム２の位置が停止想定位置（目標停止位置に近い位置）に留まり続けていることを検出するために、制御部７０は、各関節軸の速度と、速度が継続する時間とを判定する。言い換えると、各関節軸の速度の低下が瞬間的なものではなく、整定に向かっているか否かを判定する。具体的には、制御部７０は、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続しているか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部７０は、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していると判定した場合、遷移条件Ｃ２−２を満たすと判定し、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していないと判定した場合、遷移条件Ｃ２−２を満たさないと判定する。制御部７０は、例えば、全ての関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していると判定した場合、遷移条件Ｃ２−２を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続しているか否かを判定するようにすればよい。

遷移条件Ｃ２が成立すると判定した場合（Ｓ１４でＹｅｓ）、制御部７０は、「Ｓ２：整定途中」への状態遷移を行う（Ｓ１５）。遷移条件Ｃ２が成立しないと判定した場合（Ｓ１４でＮｏ）、制御部７０は、「Ｓ１：移動中」の状態を維持する。

一方、前回状態が「Ｓ２：整定途中」である場合（Ｓ１１で「Ｓ２：整定途中」）、制御部７０は、遷移条件Ｃ３が成立するか否かを判定する（Ｓ１６）。具体的には、遷移条件Ｃ３として、各関節軸の速度とトルク出力を判定し、遷移条件Ｃ３に付随する遷移条件Ｃ３−１、Ｃ３−２のうち、何れかの遷移条件を満たすと判定した場合、遷移条件Ｃ３が成立すると判定する。なお、状態不安定中であるため、遷移条件Ｃ１よりも厳しい遷移条件となる。

遷移条件Ｃ３−１では、教示者によるロボットアーム２の姿勢変更が再開され、ロボットアーム２が素早く動かされたことを検出するために、制御部７０は、各関節軸の速度を判定する。具体的には、制御部７０は、各関節軸の速度が、小さな速度である「速度閾値(3)」以上となったか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部７０は、各関節軸の速度が「速度閾値(3)」以上となったと判定した場合、遷移条件Ｃ３−１を満たすと判定し、各関節軸の速度が「速度閾値(3)」以上となっていないと判定した場合、遷移条件Ｃ３−１を満たさないと判定する。制御部７０は、例えば、少なくとも１つの関節軸の速度が「速度閾値(3)」以上となったと判定した場合、遷移条件Ｃ３−１を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(3)」以上となったか否かを判定するようにすればよい。

遷移条件Ｃ３−２では、教示者によるロボットアーム２に外力が加えられたことを検出するために、制御部７０は、各関節軸のトルク出力を判定する。具体的には、制御部７０は、ステップＳ１０で取得した関節部１１〜１３及び肩部１４のそれぞれのトルクが「トルク閾値(2)」以上となったか否かを判定する。そして、制御部７０は、トルクが「トルク閾値(2)」以上となったと判定した場合、遷移条件Ｃ３−２を満たすと判定し、トルクが「トルク閾値(2)」以上となっていないと判定した場合、遷移条件Ｃ３−２を満たさないと判定する。制御部７０は、例えば、少なくとも１つの関節軸のトルクが「トルク閾値(2)」以上となったと判定した場合、遷移条件Ｃ３−２を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸のトルクを算出し、算出したトルクが「トルク閾値(2)」以上となったか否かを判定するようにすればよい。

ここで、「トルク閾値(2)」は、遷移条件Ｃ１−２と同様に、ロボットアーム２の姿勢に応じて変動させるようにしてもよい。ただし、好ましくは、遷移条件Ｃ３−２では、未整定状態であるため、遷移条件Ｃ１−２での閾値よりも所定値だけ大きい閾値を使用するようにするとよい。

遷移条件Ｃ３が成立すると判定した場合（Ｓ１６でＹｅｓ）、制御部７０は、「Ｓ１：移動中」への状態遷移を行う（Ｓ１７）。遷移条件Ｃ３が成立しないと判定した場合（Ｓ１６でＮｏ）、制御部７０は、遷移条件Ｃ４が成立するか否かを判定する（Ｓ１８）。具体的には、遷移条件Ｃ４として、各関節軸の現在位置と整定目標位置の差異と継続時間を判定し、遷移条件Ｃ４に付随する遷移条件Ｃ４−１、Ｃ４−２のうち、全ての遷移条件を満たすと判定した場合、遷移条件Ｃ４が成立すると判定する。

遷移条件Ｃ４−１では、教示者によるロボットアーム２の姿勢変更が完了しており、ロボットアーム２の位置が目標停止位置に留まっていることを検出するために、制御部７０は、各関節軸の位置を判定する。具体的には、制御部７０は、ステップＳ１０で取得した関節部１１〜１３及び肩部１４のそれぞれの関節角度に基づいて、関節部１１〜１３の関節軸のそれぞれの位置を算出する。制御部７０は、算出した関節軸の位置のそれぞれと、関節軸のそれぞれに対応する整定目標位置のそれぞれとの位置差が、安定していることを示す「位置差閾値(1)」未満であることを判定する。そして、制御部７０は、各位置差が「位置差閾値(1)」未満であると判定した場合、遷移条件Ｃ４−１を満たすと判定し、各位置差が「位置差閾値(1)」未満でないと判定した場合、遷移条件Ｃ４−１を満たさないと判定する。制御部７０は、例えば、全ての関節軸の位置差が「位置差閾値(1)」未満であると判定した場合、遷移条件Ｃ４−１を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の位置を算出し、算出した位置と対象の関節軸の整定目標位置との位置差が「位置差閾値(1)」未満であるか否かを判定するようにすればよい。

ここで、関節部１１〜１３の関節軸のそれぞれに対応する整定目標位置は、遷移条件Ｃ２が成立して状態「Ｓ２：整定途中」に遷移し、整定処理が開始された際に決定され、制御装置３において保持される。例えば、制御部７０は、決定した整定目標位置を示す情報をＲＡＭ７２に格納し、その情報が示す整定目標位置と、関節軸の現在位置とを比較して、関節軸が整定目標位置に向かうようにモータ５１〜５４を制御する。

遷移条件Ｃ４−２では、教示者によるロボットアーム２の姿勢変更の完了が継続しており、ロボットアーム２の位置が目標停止位置に留まり続けていることを検出するために、制御部７０は、各関節軸の位置と、位置が継続する時間とを判定する。言い換えると、制御部７０は、各関節軸の位置の安定が瞬間的なものではなく、整定に向かっているか否かを判定する。具体的には、制御部７０は、算出した関節軸の位置のそれぞれと、関節軸のそれぞれに対応する整定目標位置のそれぞれとの位置差が、安定していることを示す「位置差閾値(1)」未満で「継続時間(2)」以上継続しているか否かを判定する。そして、制御部７０は、各位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していると判定した場合、遷移条件Ｃ４−２を満たすと判定し、各位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していないと判定した場合、遷移条件Ｃ４−２を満たさないと判定する。制御部７０は、例えば、全ての関節軸の位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していると判定した場合、遷移条件Ｃ４−２を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の位置と対象の関節軸の整定目標位置との位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続しているか否かを判定するようにすればよい。

遷移条件Ｃ４が成立すると判定した場合（Ｓ１８でＹｅｓ）、制御部７０は、「Ｓ３：整定」への状態遷移を行う（Ｓ１９）。ステップＳ１２、Ｓ１４、Ｓ１８で遷移条件が成立しなかったと判定した場合（ステップＳ１２、Ｓ１４、Ｓ１８でＮｏ）、及び、ステップＳ１３、Ｓ１５、Ｓ１７、Ｓ１９の実行後、制御部７０は、最新の状態に応じてロボットアーム２を制御する。

最新の状態が「Ｓ１：移動中」である場合（Ｓ２０で「Ｓ１：移動中」）、制御部７０は、サーボ制御方式「制御(1)」によってロボットアーム２を制御する（Ｓ２１）。具体的には、制御部７０は、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御を実施する。

最新の状態が「Ｓ２：整定途中」又は「Ｓ３：整定」である場合（Ｓ２０で「Ｓ２：整定途中」、「Ｓ３：整定」）、制御部７０は、サーボ制御方式「制御(2)」によってロボットアーム２を制御する（Ｓ２２）。具体的には、制御部７０は、上述のトルクサーボ制御と組み合わせて位置制御を実施する。ここで、関節軸毎に独立して状態を管理する場合には、関節軸のそれぞれの状態に応じて、各関節軸を独立してサーボ制御方式「制御(1)」又はサーボ制御方式「制御(2)」で制御する。

以上に説明した処理によれば、図７に示すように、教示者からのロボットアーム２に対する直接教示の状況に対して適切なサーボ制御方式でロボットアーム２を制御することが可能となる。すなわち、教示者がロボットアーム２の姿勢を変更している状態（Ｓ１：移動中）である場合には、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御を実施することで、教示者が小さな力で軽快にロボットアーム２の教示操作を行うことを可能としている。また、教示者がロボットアーム２の姿勢の変更を完了しようとしている、又は、ロボットアーム２の姿勢の変更を完了した状態（Ｓ２：整定途中、Ｓ３：整定）である場合には、さらに位置制御も実施することで、教示者が容易にロボットアーム２の位置決めをすることを可能とている。すなわち、本実施の形態によれば、教示者によるロボットアームの操作感を向上することができる。ここで、この位置制御として、後述するように弱いレベルの位置制御を行うことで操作性の向上を図っている。動摩擦に打ち勝つようにするために強い位置制御を実施すると、教示者がロボットアーム２を教示する際に固く、状態遷移が遅れがちとなってしまい、操作感が悪くなってしまう。それに対して、本実施の形態のように弱いレベルの位置制御を行うことで操作感を向上することができる。

このように本実施の形態では、ロボットアーム２を制御する状態は大きく分けると、サーボ制御方式が「制御(1)」となる状態（第１の状態）と、サーボ制御方式が「制御(2)」となる状態（第２の状態）との間で遷移する。第１の状態では、ロボットアーム２を制御する状態として、「Ｓ１：移動中」をとる。また、第２の状態では、ロボットアーム２を制御する状態として、「Ｓ２：整定途中」（第１のサブ状態）と、「Ｓ３：整定」（第２のサブ状態）のいずれかをとる。

続いて、図８及び図９を参照して、上述した閾値の関係について説明する。図８は、状態遷移条件において使用される閾値を説明する図である。図９は、状態遷移条件において使用される閾値の大小関係を示す図である。

まず、速度に関係する閾値として、遷移条件Ｃ１−１で使用される「速度閾値(1)」、遷移条件Ｃ２−１で使用される「速度閾値(2)」、及び、遷移条件Ｃ３−１で使用される「速度閾値(3)」がある。

「速度閾値(2)」は、移動中（Ｓ１：移動中）に、一定速度未満で停止し始めていることを判断するための閾値であるのに対し、「速度閾値(3)」は、整定に向かう途中（Ｓ２：整定途中）で、小さな速度変化の発生を判断するための閾値であるため、「速度閾値(2)」は「速度閾値(3)」以上の大きさで設定される。

「速度閾値(3)」は、整定に向かう途中（Ｓ２：整定途中）で、小さな速度変化の発生を判断するための閾値であるのに対し、「速度閾値(1)」は、完全に整定した後（Ｓ３：整定）の僅かな速度変化の発生を判断するための閾値であるため、「速度閾値(3)」は「速度閾値(1)」よりも大きく設定される。

次に、トルクに関係する閾値として、遷移条件Ｃ１−２で使用される「トルク閾値(1)」、及び、遷移条件Ｃ３−２で使用される「トルク閾値(2)」がある。

「トルク閾値(2)」は、整定に向かう途中（Ｓ２：整定途中）で、小さなトルク変化の発生を判断するための閾値であるのに対し、「トルク閾値(1)」は、完全に整定した後（Ｓ３：整定）の僅かなトルク変化の発生を判断するための閾値であるため、「トルク閾値(2)」は「トルク閾値(1)」よりも大きく設定される。

次に、位置差に関する閾値として、遷移条件４−１で使用される「位置差閾値(1)」がある。「位置差閾値(1)」は、整定に向かう途中（Ｓ２：整定途中）で、位置差が安定していると扱ってもよい整定許容範囲内であることを判断するための閾値である。

次に、継続時間に関する閾値として、遷移条件２−２で使用される「継続時間(1)」、及び、遷移条件Ｃ４−２で使用される「継続時間(2)」がある。

「継続時間(2)」は、移動中（Ｓ１：移動中）に、一定速度未満を継続し続けていることを判断するための閾値であるのに対し、「継続時間(1)」は、整定に向かう途中（Ｓ２：整定途中）で、位置差が整定許容範囲内を継続し続けていることを判断するための閾値であるため、「継続時間(2)」は「継続時間(1)」よりも大きく設定される。

制御部７０による、これらの閾値を用いた判定は、例えば、これらの閾値を示す閾値情報をＲＯＭ７２に予め格納しておき、制御部７０（ＣＰＵ７１）が、この閾値情報が示す閾値と比較演算することで実現される。

続いて、図１０及び図１１を参照して、実施の形態に係る制御装置３の指令トルク算出処理について説明する。図１０は、データの入出力に対する指令トルク算出処理の流れを示すブロック図である。図１１は、指令トルク算出処理における各データ及び各処理を説明するための図である。

図１１では、「Ｃ」は、ＣＯＮＳＴ（固定データ）を示しており、「Ｄ」は、Ｄａｔａ（変数データ）を示しており、「Ｐ」は、Ｐｒｏｃｅｓｓ（処理ブロック）を示している。

「Ｃ１」は、各関節軸に対する各アーム部の質量を示しており、「Ｃ２」は、各関節軸に対する各アーム部の重心位置を示している。質量「Ｃ１」及び重心「Ｃ２」は、固定値として用意されている。すなわち、質量「Ｃ１」及び重心「Ｃ２」は、例えば、それぞれを示す情報をＲＯＭ７２に予め格納しておき、制御部７０（ＣＰＵ７１）が、それらの情報を参照することで取得する。

「Ｄ１」は、各関節軸の関節角度を示しており、「Ｄ２」は、各関節軸のトルクを示しめしている。すなわち、関節角度「Ｄ１」は、上述のステップＳ１０で取得された各関節軸の関節角度を示しており、トルク「Ｄ２」は、上述のステップＳ１０で取得された各関節軸のトルクを示している。目標停止位置「Ｄ３」は、Ｐ４で使用される目標停止位置（各関節軸の整定目標位置）であり、Ｐ４で入力された状態が「Ｓ２：整定途中」となった段階で保持される。

以下、Ｐ１〜Ｐ６の処理について説明する。Ｐ１〜Ｐ６の処理によって各関節軸（各関節部１１〜１３及び肩部１４のモータ５１〜５４）に対するトルク指令値を算出する。

Ｐ１では、制御部７０は、各関節軸に対する各アーム部の質量「Ｃ１」、各関節軸に対する各アーム部の重心位置「Ｃ２」、及び、各関節軸の関節角度「Ｄ１」に基づいて、各関節軸の重力補償トルクを算出する。ここで、具体的な重力補償トルクの算出方法については、一般的な逆運動学の手法を用いればよいため、詳細な説明は省略する。

Ｐ２では、制御部７０は、各関節軸の関節角度「Ｄ１」に基づいて、各関節軸の速度（角速度）を算出する。具体的には、過去（例えば、前回）の各関節軸の関節角度「Ｄ１」からの変更量に基づいて、各関節軸の速度を算出する。例えば、上述した式（１）によって算出するようにしてもよい。

Ｐ３では、制御部７０は、Ｐ２で算出された各関節軸の速度と、各関節軸のトルク「Ｄ２」とに基づいて、図５のステップＳ１１〜Ｓ１９で説明したように状態遷移を判定する。

Ｐ４では、制御部７０は、各関節軸の関節角度「Ｄ１」と、Ｐ３で判定された最新の状態と、目標停止位置「Ｄ３」とに基づいて、ロボットアーム２の目標停止位置に対する位置制御トルクを、各関節軸について算出する。ここで、本処理Ｐ４は、Ｐ３で判定した状態が「Ｓ１：移動中」である場合には実施されず、Ｐ３で判定した状態が「Ｓ２：整定途中」又は「Ｓ３：整定」である場合に実施される。具体的には、各関節軸の位置が、それぞれの整定目標位置に位置するように、弱いゲインによる位置制御を実施する。すなわち、このゲインは、教示者が各関節部の関節軸の角度を容易に変更可能となる程度のトルクがモータ５１〜５４に掛かるように予め定められる。重力補償のみだと、教示者がロボットアーム２を操作した場合に無重力空間のようなフンワリとした動作となり、期待する整定目標位置に留まるのが困難となってしまう。それに対して、弱いゲインの位置制御を併用することで、整定目標位置に合わせたモータ制御が可能となる。このとき、位置制御の算出結果をトルク値に変換することで、他のトルク（重力補償トルクや摩擦補償トルク）の算出結果と足し合わせてトルク指令値を作成できるようにする。これによって、同一系統のトルク指令値によるトルクサーボ制御を可能とする。ここで、関節軸毎に独立して状態を管理する場合には、状態が「Ｓ２：整定途中」又は「Ｓ３：整定」である関節軸についてのみ、位置制御を実施して位置制御トルクを算出する。

Ｐ５では、制御部７０は、外乱オブザーバによって、Ｐ２で算出された各関節軸の速度と、Ｐ１で算出された各関節軸の重力補償トルクとＰ４で算出された各関節軸の位置制御トルクを加算し各関節軸のトルク「Ｄ２」を減算しゲインを乗じたトルクと、Ｐ５で前回に算出した各関節軸の摩擦補償トルクとに基づいて、各関節軸の摩擦補償トルクを各関節軸について算出する。ここで、外乱オブザーバにおける、モータトルク（重力補償トルクと位置制御トルクを加算しトルク「Ｄ２」を減算したトルクと、前回の摩擦補償トルクの合計）と、モータ速度（各関節軸の速度）から外乱（摩擦補償トルク）を推定する構成については一般的であるため、詳細な説明は省略する。なお、状態が「Ｓ１：移動中」である場合には、位置制御トルクは算出されておらず、加算されないことになる。

Ｐ６では、制御部７０は、上述の重力補償トルクと位置制御トルクとトルク「Ｄ２」を合成しゲインを乗じた各関節軸のトルクと、Ｐ５で算出された各関節軸の摩擦補償トルクとを加算したトルクを、各関節軸について算出する。これによって算出された各関節軸のトルクは、各関節軸（各関節部１１〜１３及び肩部１４のモータ５１〜５４）に対する指令トルクとなる。制御部７０は、モータ５１〜５４のそれぞれについて、指令トルクに対応するトルク指令値を生成して駆動回路８０に出力する。これによって、駆動回路８０によって、トルク指令値に対応する指令トルクでモータを駆動するための駆動電流が生成され、モータ５１〜５４のそれぞれに供給される。

以上に説明したように、本実施の形態では、エンコーダ３０が検出した角度に基づいて算出した角速度が第１の速度閾値（例えば、速度閾値(1)に相当）以上であると判定した場合、又は、トルクセンサ４０が検出したトルクが第１のトルク閾値（例えば、トルク閾値(1)に相当）以上であると判定した場合、モータ５０に対してトルク制御を行う第１の状態に遷移し、エンコーダ３０が検出した角度に基づいて算出した角速度が第２の速度閾値（例えば、速度閾値(2)に相当）未満であると判定した場合、モータ５０に対して前記トルク制御に加えて位置制御を行う第２の状態に遷移するようにしている。

これによれば、特に位置制御又は速度制御がベースとなった直接教示方法と比較して、トルクサーボ制御（トルク制御）によるイナーシャスケーリングによって、教示者がロボットアーム２に対して操作感の良い柔軟な制御が可能となる。例えば、位置制御又は速度制御を行う場合、ゲインを抑えてロボットアーム２の姿勢変更時の抵抗感を低減する手法も考えられるが、抵抗感が各関節部の摩擦抵抗以下になることはない。それに対して、本実施の形態によれば、トルクサーボ制御によって摩擦補償やトルクフィードバックを行うことができ、見かけ上の慣性を小さくすること（イナーシャスケーリング）が可能である。また、教示者がロボットアーム２の姿勢の変更を完了することを検出した場合に遷移する第２の状態では、さらに位置制御も実施するようにしているため、教示者が容易にロボットアーム２の位置決めをすることができる。この点においても、教示者がロボットアーム２に対して操作感の良い柔軟な制御が可能となる。

また、各関節部にエンコーダ及びトルクセンサが組み込まれており、各関節部で検出した関節角度及びトルクに基づいて状態遷移を行うようにしている。すなわち、教示者がロボットアーム２に対する教示を行う場合に、操作装置（操作ハンドル、操作部）のみに限定されず、各関節部に対して直接的に教示操作を行うことができる。そのため、教示者がロボットアーム２に対して操作感の良い柔軟な制御が可能となる。すなわち、本実施の形態によれば、教示者によるロボットアームの操作感を向上することができる。

さらに、本実施の形態では、位置制御は、整定用に弱いゲインで行うのみであるため、整定時に教示者がロボットアーム２を操作しても抵抗感を感じることはなく「Ｓ１：移動中」に遷移することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した遷移条件Ｃ１−１〜Ｃ４−１は、常に、全ての遷移条件を判定するように構成しなくてもよい。上述した遷移条件Ｃ１−１〜Ｃ４−１のうち、いくつかの遷移条件を省略した形態で実施するようにしてもよい。

１ ロボットアーム教示システム
２ ロボットアーム
３ 制御装置
１０ 手先部
１１、１２、１３ 関節部
１４ 肩部
１５ 本体部
２０、２１、２２、２３、２４ アーム部
３０、３１、３２、３３、３４ エンコーダ
４０、４１、４２、４３、４４ トルクセンサ
５０、５１、５２、５３、５４ モータ
６０ 減速機
６１ 前腕
６２ 上腕
７０ 制御部
７１ ＣＰＵ
７２ ＲＡＭ
７３ ＲＯＭ
８０ 駆動回路

Claims (8)

1. 複数のアームを含むロボットアームに対する教示を行うためのロボットアーム教示システムであって、
   前記アーム間の関節部に設けられ、当該関節部を駆動するモータと、
   前記アーム間の関節部に設けられ、当該関節部の角度を検出するエンコーダと、
   前記アーム間の関節部に設けられ、当該関節部のトルクを検出するトルクセンサと、
   前記エンコーダが検出した角度及び前記トルクセンサが検出したトルクの少なくとも１つに基づいて状態を遷移するとともに、遷移後の状態に応じて前記モータを制御する制御部と、を備え、
   前記制御部は、
   前記エンコーダが検出した角度に基づいて算出した角速度が第１の速度閾値以上であると判定した場合、又は、前記トルクセンサが検出したトルクが第１のトルク閾値以上であると判定した場合、前記モータに対してトルク制御を行う第１の状態に遷移し、
   前記エンコーダが検出した角度に基づいて算出した角速度が第２の速度閾値未満であると判定した場合、前記モータに対して前記トルク制御に加えて位置制御を行う第２の状態に遷移する、
   ロボットアーム教示システム。
2. 前記第２の状態は、第１のサブ状態と、第２のサブ状態とを含み、
   前記制御部は、
   前記角速度が前記第２の速度閾値未満であると判定した場合、前記第１のサブ状態に遷移し、
   前記第１のサブ状態において、前記エンコーダが検出した角度に基づいて算出した前記関節部の位置と、前記位置制御における前記関節部の整定目標位置との位置差が位置差閾値未満である場合、前記第２のサブ状態に遷移し、
   前記第２のサブ状態において、前記角速度が前記第１の速度閾値以上であると判定した場合、又は、前記トルクが前記第１のトルク閾値以上であると判定した場合、前記第１の状態に遷移する、
   請求項１に記載のロボットアーム教示システム。
3. 前記第２のサブ状態において、前記角速度が前記第１の速度閾値よりも大きい第３のトルク閾値以上であると判定した場合、又は、前記トルクが前記第１のトルク閾値よりも大きい第２のトルク閾値以上であると判定した場合、前記第１の状態に遷移する、
   請求項２に記載のロボットアーム教示システム。
4. 前記第２の速度閾値は、前記第３の速度閾値以上の大きさである、
   請求項３に記載のロボットアーム教示システム。
5. 前記制御部は、
   前記角速度が前記第２の速度閾値未満で第１の継続時間以上継続していると判定した場合、前記第１のサブ状態に遷移し、
   前記関節部の位置と前記関節部の整定目標位置との位置差が位置差閾値未満で第２の継続時間以上継続していると判定した場合、前記第２のサブ状態に遷移する、
   請求項２乃至４のいずれか１項に記載のロボットアーム教示システム。
6. 前記制御部は、
   前記エンコーダが検出した角度に基づいて、前記関節部が下げる方向及び上げる方向のうち、いずれの方向に移動されているかを判定し、前記関節部が下げる方向に移動されていると判定した場合、前記第１の速度閾値を、前記関節部が上げる方向に移動されている場合よりも大きくして判定する、
   請求項１乃至５のいずれか１項に記載のロボットアーム教示システム。
7. 前記トルク制御は、前記トルクセンサが検出したトルクに基づくトルクフィードバックと、前記エンコーダが検出した角度に基づく重力補償及び摩擦補償とを組み合わせたものである、
   請求項１乃至６のいずれか１項に記載のロボットアーム教示システム。
8. 複数のアームを含むロボットアームに対する教示を行うためのロボットアーム教示方法であって、
   前記アーム間の関節部の角度、及び、前記アーム間の関節部のトルクを検出するステップと、
   前記検出した角度に基づいて算出した角速度が第１の速度閾値以上であると判定した場合、又は、前記検出したトルクが第１のトルク閾値以上であると判定した場合、前記アーム間の関節部に設けられたモータに対してトルク制御を行う第１の状態に遷移し、前記検出した角度に基づいて算出した角速度が第２の速度閾値未満であると判定した場合、前記モータに対して前記トルク制御に加えて位置制御を行う第２の状態に遷移するステップと、
   遷移後の状態に応じて前記モータを制御するステップと、
   を備えたロボットアーム教示方法。
   

Images