以下に図面を参照しながら、本発明の好適な実施の形態について説明する。以下の実施の形態に示す具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、それに限定されるものではない。また、以下の記載及び図面では、説明の明確化のため、当業者にとって自明な事項等については、適宜、省略及び簡略化がなされている。
<発明の実施の形態>
図1を参照して、実施の形態に係るロボットアーム教示システム1の構成について説明する。図1は、実施の形態に係るロボットアーム教示システム1の構成図である。
ロボットアーム教示システム1は、ロボットアーム2及び制御装置3を有する。本実施の形態では、ロボットアーム2が3関節4自由度モデルである場合について例示するが、これに限られない。ロボットアーム2及び制御装置3は、任意の情報を送受信可能とする信号線によって相互に接続されている。
ロボットアーム2は、手先部10、関節部11〜13、肩部14、本体部15、及びアーム部20〜24を有する。
アーム部20の一端には、手先部10が連結されており、アーム部20の他端には、関節部11が連結されている。アーム部21の一端には、関節部11が連結されており、アーム部21の他端には、関節部12が連結されている。アーム部22の一端には、関節部12が連結されており、アーム部22の他端には関節部13が連結されている。アーム部23の一端には、関節部13が連結されており、アーム部23の他端には、肩部14が連結されている。アーム部24の一端には、肩部14が連結されており、アーム部24の他端には本体部15が連結されている。本体部15は、上述した信号線によって制御装置3と接続されている。
アーム部20、21は、関節部11を軸として回動可能となっている。アーム部21、22は、関節部12を軸として回動可能となっている。アーム部22、23は、関節部13を軸として回動可能となっている。アーム部23、24は、肩部14に対して鉛直軸周りに回動可能となっている。関節部11〜13のそれぞれは、アーム部20〜23の回動軸が相互に平行するようにアーム部20〜23を介して連結されている。
このような構成により、ロボットアーム2は、関節部14を中心として鉛直軸周りに回動するとともに、アーム部20〜23のそれぞれと直交し、かつ相互に平行する軸周りに関節部11〜13を中心として回動するように動作する。
ここで、関節部11〜13のそれぞれは、エンコーダ30、トルクセンサ40、モータ50、及び減速機60を含んでいる。
モータ50は、制御装置3から供給される駆動電流によって駆動される。前腕61及び上腕62は、モータ50が駆動されることで、減速機60によって減速された回転速度で関節部を軸として回動する。ここで、前腕61は、関節部よりも手先部10寄りのアーム部を示しており、上腕62は、関節部よりも肩部14寄りのアーム部を示している。例えば、関節部12に関しては、前腕61はアーム部21に相当し、上腕62はアーム部22に相当する。
エンコーダ30は、モータ50の回転角度(関節部の関節軸の関節角度)を検出し、検出した回転角度(関節角度)を示す角度信号を生成して本体部15に出力する。エンコーダ30から出力された角度信号は、本体部15を介して制御装置3に送信される。
トルクセンサ40は、関節部の関節軸にかかるトルクを検出し、検出したトルクを示すトルク信号を生成して本体部15に出力する。トルクセンサ40から出力されたトルク信号は、本体部15を介して制御装置3に送信される。
なお、肩部14は、ロボットアーム2が関節部14を中心として鉛直軸周りに回動可能となるように、エンコーダ30、トルクセンサ40、モータ50、及び減速機60が鉛直方向に並んでいる点が関節部11〜13と異なる。しかし、エンコーダ30、トルクセンサ40、モータ50、及び減速機60のそれぞれの動作は同様であるため、詳細な説明は省略する。
このような構成により、ロボットアーム2は、教示者によってロボットアーム2に対して直接教示が行われた際における関節角度及びトルクを制御装置3に通知することができる。そして、制御装置3は、ロボットアーム2から角度信号及びトルク信号によって通知された関節角度及びトルクを教示データとして蓄積するとともに、その関節角度及びトルクに基づいて教示者による直接教示を援けるように関節部11〜13及び肩部14のモータ50を駆動する。これによって、教示者によるロボットアーム2の直接教示における操作感を向上するようにしている。なお、教示データとして、関節角度及びトルクのうち、いずれか一方を蓄積するようにしてもよい。
続いて、図2A〜図2Cを参照して、実施の形態に係るロボットアーム2の制御の概要について説明する。図2A〜図2Cは、実施の形態に係るロボットアーム2の制御の概要を説明するための図である。
まず、図2Aに示すように、教示者が静止したロボットアーム2の任意の手押しをした場合、制御装置3は、ロボットアーム2に対して、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御(トルク制御)を実施することにより、教示者が関節部等の位置を抵抗感が無く移動できるようにする。これにより、教示者が小さな力で軽快にロボットアーム2を所望の姿勢にすることが可能となる。
次に、図2Bに示すように、教示者がロボットアーム2の任意の手押しを止めて、所定の時間の間、ロボットアーム2の位置をキープした場合、制御装置3は、上述のトルクサーボ制御と組み合わせて、さらに位置制御も実施することにより、ロボットアーム2を停止位置で維持できるようにする。
そして、図2Cに示すように、教示者がロボットアーム2から手を離した場合、制御装置3は、上述のトルクサーボ制御及び位置制御を継続して実施することにより、ロボットアーム2が単独で停止位置を維持できるようにする。これにより、教示者によるロボットアーム2の位置決めを容易にすることが可能となる。
このように、制御装置3は、ロボットアーム2を制御する状態として、図2Aに示す状態「S1:移動中」、図2Bに示す状態「S2:整定途中」、図2Cに示す状態「S3:整定」の3つの状態を管理している。そして、制御装置3は、教示者からのロボットアーム2に対する操作状況に応じて、状態を遷移させて遷移後の状態に応じたロボットアーム2の制御を実施する。これによって、教示者がロボットアーム2に対する操作感の良い柔軟な制御(柔軟な姿勢の教示)を行うことを可能とする。なお、ロボットアーム2を制御する状態は、後述するように、ロボットアーム2単位で管理するようにしてもよく、関節部(関節軸)11〜14単位で独立して管理するようにしてもよい。
続いて、図3を参照して、実施の形態に係る制御装置3の構成について説明する。図3は、実施の形態に係る制御装置3の構成図である。
制御装置3は、制御部70及び駆動回路80を有する。制御部70は、CPU(Central Processing Unit)71、RAM(Random Access Memory)72、ROM(Read Only Memory)73、及びI/O74を有する。制御部70は、制御部70として処理を実行するためのプログラムをCPU71によって実行することによって、本実施の形態において説明するようにロボットアーム2を制御する。RAM72は、CPU71によってプログラムを実行するためにROM73からプログラムがロードされるとともに、プログラムの実行に際して演算されるデータが一時的に格納される。ROM73は、CPU71によって実行させるプログラム、及び、ロボットアーム2の制御演算に利用する各種パラメータ等が格納されている。I/O74は、例えば、I/O(Input/Output)ポートである。
駆動回路80は、ロボットアーム2を制御する際に制御部70からI/O74を介して出力される、第1乃至第4のモータ51〜54のそれぞれに対するトルク指令値に基づいて、第1乃至第4のモータ51〜54のそれぞれを駆動するための駆動電流を生成し、第1乃至第4のモータ51〜54のそれぞれに供給する。
ここで、第1のモータ51は、関節部11に含まれるモータ50を示しており、第2のモータ52は、関節部12に含まれるモータ50を示しており、第3のモータ53は、関節部13に含まれるモータ50を示しており、第4のモータ54は、関節部14に含まれるモータ50を示している。
また、第1のエンコーダ31は、関節部11に含まれるエンコーダ30を示しており、第2のエンコーダ32は、関節部12に含まれるエンコーダ30を示しており、第3のエンコーダ33は、関節部13に含まれるエンコーダ30を示しており、第4のエンコーダ34は、関節部14に含まれるエンコーダ30を示している。
また、第1のトルクセンサ41は、関節部11に含まれるトルクセンサ40を示しており、第2のトルクセンサ42は、関節部12に含まれるトルクセンサ40を示しており、第3のトルクセンサ43は、関節部13に含まれるトルクセンサ40を示しており、第4のトルクセンサ44は、関節部14に含まれるトルクセンサ40を示している。
制御部70は、第1乃至第4のエンコーダ31〜34から出力される角度信号が示す関節角度、及び、第1乃至第4のトルクセンサ41〜44から出力されるトルク信号が示すトルクに基づいて、状態を遷移するか否かを判定し、判定結果に応じた状態に、ロボットアーム2を制御する状態を遷移させる。
制御部70は、第1乃至第4のエンコーダ31〜34から出力される角度信号が示す関節角度、及び、第1乃至第4のトルクセンサ41〜44から出力されるトルク信号が示すトルクに基づいて、状態に応じたロボットアーム2の制御演算を行い、第1乃至第4のモータ51〜54のそれぞれに対するトルク指令値を算出する。
これによって、制御部70は、上述したように、教示者がロボットアーム2に対して操作感の良い柔軟な制御を行うことが可能となるようにトルク指令値を生成してロボットアーム2を制御する。
続いて、図4〜図7を参照して、実施の形態に係るロボットアーム教示システム1の状態遷移処理について説明する。図4は、実施の形態に係るロボットアーム教示システム1の状態遷移図である。図5は、実施の形態に係るロボットアーム教示システム1の状態遷移処理を示すフローチャートである。図6は、実施の形態に係る状態遷移処理における状態遷移条件を示す図である。図7は、実施の形態に係るロボットアーム教示システム1の状態と制御方式との関係を示す図である。
制御部70は、所定の一定時間間隔(制御周期時間)毎に、第1乃至第4のエンコーダ31〜34から送信された角度信号、及び、第1乃至第4のトルクセンサ41〜44から送信されたトルク信号をI/O74を介して取得する。制御部70は、角度信号のそれぞれが示す関節角度を、関節部11〜14の関節軸のそれぞれの関節角度として取得し、トルク信号のそれぞれが示すトルクを、関節部11〜14の関節軸のそれぞれのトルクとして取得する(S10)。ここで、関節軸毎に独立して状態を管理する場合には、ステップS11以降の処理は、関節軸毎に独立して実施される。
制御部70は、前回状態(現在の状態)が、「S1:移動中」、「S2:整定途中」、「S3:整定」のうち、いずれの状態であるかを判定する(S11)。
前回状態が「S3:整定」である場合(S11で「S3:整定」)、制御部70は、遷移条件C1が成立するか否かを判定する(S12)。具体的には、遷移条件C1として、各関節軸の速度とトルク出力を判定し、遷移条件C1に付随する遷移条件C1−1、C1−2のうち、何れかの遷移条件を満たすと判定した場合、遷移条件C3が成立すると判定する。
遷移条件C1−1では、教示者によるロボットアーム2の姿勢変更が開始され、ロボットアーム2が素早く動かされたことを検出するために、制御部70は、各関節軸の速度を判定する。具体的には、制御部70は、今回にステップS10で取得した関節部11〜13及び肩部14のそれぞれの関節角度と、前回にステップS10で取得した関節部11〜13及び肩部14のそれぞれの関節角度とに基づいて、関節部11〜13及び肩部14の関節軸のそれぞれの速度(角速度)を算出する。
具体的には、制御部70は、次式(1)に示すように、前回の関節角度と今回の関節角度の差分を、前回の関節角度を取得してから今回の関節角度を取得するまでの時間で割ることで速度を算出する。
角速度[rad/sec] =
(今回関節角度[rad]−前回関節角度[rad])/制御周期時間[sec] ・・・ (1)
制御部70は、各関節軸の速度が、僅かな速度である「速度閾値(1)」以上となったか否かを判定する。そして、制御部70は、関節軸の速度が「速度閾値(1)」以上となったと判定した場合、遷移条件C1−1を満たすと判定し、関節軸の速度が「速度閾値(1)」以上となっていないと判定した場合、遷移条件C1−1を満たさないと判定する。制御部70は、例えば、少なくとも1つの関節軸の速度が「速度閾値(1)」以上となったと判定した場合、遷移条件C1−1を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(1)」以上となったか否かを判定するようにすればよい。
遷移条件C1−2では、教示者によりロボットアーム2に外力が加えられたことを検出するために、制御部70は、各関節軸のトルク出力を判定する。具体的には、制御部70は、ステップS10で取得した関節部11〜13及び肩部14のそれぞれのトルクが「トルク閾値(1)」以上となったか否かを判定する。そして、制御部70は、トルクが「トルク閾値(1)」以上となったと判定した場合、遷移条件C1−2を満たすと判定し、トルクが「トルク閾値(1)」以上となっていないと判定した場合、遷移条件C1−2を満たさないと判定する。制御部70は、例えば、少なくとも1つの関節軸のトルクが「トルク閾値(1)」以上となったと判定した場合、遷移条件C1−2を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸のトルクを算出し、算出したトルクが「トルク閾値(1)」以上となったか否かを判定するようにすればよい。
ここで、「トルク閾値(1)」は、ロボットアーム2の姿勢に応じて変動させるようにしてもよい。関節部に対して大きな重力トルクが発生する姿勢であるほど、自重によって大きなトルクが検出されてしまうため、「トルク閾値(1)」も大きくする方が好ましいからである。ここで、重力トルクは常に下げる方向に強く作用しているため、教示者が関節部を手押しする際には、ロボットアーム2を上げる方向に手押ししているか、ロボットアーム2を下げる方向に手押ししているかに応じて、「トルク閾値(1)」を変化させるとよい。すなわち、制御部70は、教示者がロボットアーム2を下げる方向に手押ししていると判定される場合には、大きな閾値を「トルク閾値(1)」として使用し、教示者がロボットアーム2を上げる方向に手押ししていると判定される場合には、それよりも小さな閾値を「トルク閾値(1)」として使用する。
なお、教示者が、ロボットアーム2を下げる方向に手押ししているか、ロボットアーム2を上げる方向に手押ししているかは、任意に予め定めた判定基準に従って判定するようにしてよい。例えば、ステップS10で取得した各関節軸の関節角度に基づいて算出した各関節軸の位置と、過去(例えば、前回)にステップS10で取得した各関節軸の関節角度に基づいて算出した過去の各関節軸の位置とに基づいて、所定数の関節軸が過去の関節軸の位置よりも今回の関節軸の位置の方が下がっている場合、あるいはステップS10で取得した各関節軸のトルクに基づいて、所定数の関節軸のトルクが重力トルクと同等の向きのロボットアーム2を下げる方向に加えられている場合には、教示者がロボットアーム2を下げる方向に手押ししていると判定し、所定数の関節軸が過去の関節軸の位置よりも今回の関節軸の位置の方が上がっている場合、あるいは所定数の関節軸のトルクが重力トルクと逆の向きのロボットアーム2を上げる方向に加えられている場合には、教示者がロボットアーム2を上げる方向に手押ししていると判定するようにしてもよい。また、重力補償において算出される重力補償トルクの変化(今回に算出した重力補償トルクと前回に算出した重力補償トルクとの差)に基づいて、教示者がロボットアーム2を上げる方向に手押ししているか又は下げる方向に手押ししているかを判定してもよい。例えば、所定数の関節軸の重力補償トルクが増加している場合、教示者がロボットアーム2を上げる方向に手押ししていると判定し、所定数の重力補償トルクが減少している場合、教示者がロボットアーム2を下げる方向に手押ししていると判定するようにすればよい。
ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸(又はそれよりも手先部10側の関節軸)の位置が下がっているか又は上がっているか、対象の関節軸のトルクが重力トルクと同等の向きに加えられているか又は逆の向きに加えられているか、もしくは、対象の関節軸の重力補償トルクが増加しているか又は減少しているかによって、同様の判定するようにすればよい。
遷移条件C1が成立すると判定した場合(S12でYes)、制御部70は、「S1:移動中」への状態遷移を行う(S13)。遷移条件C1が成立しないと判定した場合(S12でNo)、制御部70は、「S3:整定」の状態を維持する。
一方、前回状態が「S1:移動中」である場合(S11で「S1:移動中」)、制御部70は、遷移条件C2が成立するか否かを判定する(S14)。具体的には、遷移条件C2として、各関節軸の速度と継続時間を判定し、遷移条件C2に付随する遷移条件C2−1、C2−2のうち、全ての遷移条件を満たすと判定した場合、遷移条件C2が成立すると判定する。
遷移条件C2−1では、教示者によるロボットアーム2の姿勢変更が完了に近い状況となり、ロボットアーム2の位置が停止しようとしていることを検出するために、制御部70は、各関節軸の速度を判定する。具体的には、制御部70は、各関節軸の速度が、停止し始めていることを示す「速度閾値(2)」未満であるか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部70は、算出した各関節軸の速度が「速度閾値(2)」未満であると判定した場合、遷移条件C2−1を満たすと判定し、算出した各関節軸の速度が「速度閾値(2)」未満でないと判定した場合、遷移条件C2−1を満たさないと判定する。制御部70は、例えば、全ての関節軸の速度が「速度閾値(2)」未満であると判定した場合、遷移条件C2−1を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(2)」以上となったか否かを判定するようにすればよい。
遷移条件C2−2では、教示者によるロボットアーム2の姿勢変更が完了に近い状況が継続しており、ロボットアーム2の位置が停止想定位置(目標停止位置に近い位置)に留まり続けていることを検出するために、制御部70は、各関節軸の速度と、速度が継続する時間とを判定する。言い換えると、各関節軸の速度の低下が瞬間的なものではなく、整定に向かっているか否かを判定する。具体的には、制御部70は、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続しているか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部70は、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していると判定した場合、遷移条件C2−2を満たすと判定し、各関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していないと判定した場合、遷移条件C2−2を満たさないと判定する。制御部70は、例えば、全ての関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続していると判定した場合、遷移条件C2−2を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度が「速度閾値(2)」未満で「継続時間(1)」以上継続しているか否かを判定するようにすればよい。
遷移条件C2が成立すると判定した場合(S14でYes)、制御部70は、「S2:整定途中」への状態遷移を行う(S15)。遷移条件C2が成立しないと判定した場合(S14でNo)、制御部70は、「S1:移動中」の状態を維持する。
一方、前回状態が「S2:整定途中」である場合(S11で「S2:整定途中」)、制御部70は、遷移条件C3が成立するか否かを判定する(S16)。具体的には、遷移条件C3として、各関節軸の速度とトルク出力を判定し、遷移条件C3に付随する遷移条件C3−1、C3−2のうち、何れかの遷移条件を満たすと判定した場合、遷移条件C3が成立すると判定する。なお、状態不安定中であるため、遷移条件C1よりも厳しい遷移条件となる。
遷移条件C3−1では、教示者によるロボットアーム2の姿勢変更が再開され、ロボットアーム2が素早く動かされたことを検出するために、制御部70は、各関節軸の速度を判定する。具体的には、制御部70は、各関節軸の速度が、小さな速度である「速度閾値(3)」以上となったか否かを判定する。なお、各関節軸の速度の算出方法は、上述と同様である。そして、制御部70は、各関節軸の速度が「速度閾値(3)」以上となったと判定した場合、遷移条件C3−1を満たすと判定し、各関節軸の速度が「速度閾値(3)」以上となっていないと判定した場合、遷移条件C3−1を満たさないと判定する。制御部70は、例えば、少なくとも1つの関節軸の速度が「速度閾値(3)」以上となったと判定した場合、遷移条件C3−1を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の速度を算出し、算出した速度が「速度閾値(3)」以上となったか否かを判定するようにすればよい。
遷移条件C3−2では、教示者によるロボットアーム2に外力が加えられたことを検出するために、制御部70は、各関節軸のトルク出力を判定する。具体的には、制御部70は、ステップS10で取得した関節部11〜13及び肩部14のそれぞれのトルクが「トルク閾値(2)」以上となったか否かを判定する。そして、制御部70は、トルクが「トルク閾値(2)」以上となったと判定した場合、遷移条件C3−2を満たすと判定し、トルクが「トルク閾値(2)」以上となっていないと判定した場合、遷移条件C3−2を満たさないと判定する。制御部70は、例えば、少なくとも1つの関節軸のトルクが「トルク閾値(2)」以上となったと判定した場合、遷移条件C3−2を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸のトルクを算出し、算出したトルクが「トルク閾値(2)」以上となったか否かを判定するようにすればよい。
ここで、「トルク閾値(2)」は、遷移条件C1−2と同様に、ロボットアーム2の姿勢に応じて変動させるようにしてもよい。ただし、好ましくは、遷移条件C3−2では、未整定状態であるため、遷移条件C1−2での閾値よりも所定値だけ大きい閾値を使用するようにするとよい。
遷移条件C3が成立すると判定した場合(S16でYes)、制御部70は、「S1:移動中」への状態遷移を行う(S17)。遷移条件C3が成立しないと判定した場合(S16でNo)、制御部70は、遷移条件C4が成立するか否かを判定する(S18)。具体的には、遷移条件C4として、各関節軸の現在位置と整定目標位置の差異と継続時間を判定し、遷移条件C4に付随する遷移条件C4−1、C4−2のうち、全ての遷移条件を満たすと判定した場合、遷移条件C4が成立すると判定する。
遷移条件C4−1では、教示者によるロボットアーム2の姿勢変更が完了しており、ロボットアーム2の位置が目標停止位置に留まっていることを検出するために、制御部70は、各関節軸の位置を判定する。具体的には、制御部70は、ステップS10で取得した関節部11〜13及び肩部14のそれぞれの関節角度に基づいて、関節部11〜13の関節軸のそれぞれの位置を算出する。制御部70は、算出した関節軸の位置のそれぞれと、関節軸のそれぞれに対応する整定目標位置のそれぞれとの位置差が、安定していることを示す「位置差閾値(1)」未満であることを判定する。そして、制御部70は、各位置差が「位置差閾値(1)」未満であると判定した場合、遷移条件C4−1を満たすと判定し、各位置差が「位置差閾値(1)」未満でないと判定した場合、遷移条件C4−1を満たさないと判定する。制御部70は、例えば、全ての関節軸の位置差が「位置差閾値(1)」未満であると判定した場合、遷移条件C4−1を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の位置を算出し、算出した位置と対象の関節軸の整定目標位置との位置差が「位置差閾値(1)」未満であるか否かを判定するようにすればよい。
ここで、関節部11〜13の関節軸のそれぞれに対応する整定目標位置は、遷移条件C2が成立して状態「S2:整定途中」に遷移し、整定処理が開始された際に決定され、制御装置3において保持される。例えば、制御部70は、決定した整定目標位置を示す情報をRAM72に格納し、その情報が示す整定目標位置と、関節軸の現在位置とを比較して、関節軸が整定目標位置に向かうようにモータ51〜54を制御する。
遷移条件C4−2では、教示者によるロボットアーム2の姿勢変更の完了が継続しており、ロボットアーム2の位置が目標停止位置に留まり続けていることを検出するために、制御部70は、各関節軸の位置と、位置が継続する時間とを判定する。言い換えると、制御部70は、各関節軸の位置の安定が瞬間的なものではなく、整定に向かっているか否かを判定する。具体的には、制御部70は、算出した関節軸の位置のそれぞれと、関節軸のそれぞれに対応する整定目標位置のそれぞれとの位置差が、安定していることを示す「位置差閾値(1)」未満で「継続時間(2)」以上継続しているか否かを判定する。そして、制御部70は、各位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していると判定した場合、遷移条件C4−2を満たすと判定し、各位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していないと判定した場合、遷移条件C4−2を満たさないと判定する。制御部70は、例えば、全ての関節軸の位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続していると判定した場合、遷移条件C4−2を満たすと判定する。ここで、関節軸毎に独立して状態を管理する場合には、対象の関節軸の位置と対象の関節軸の整定目標位置との位置差が「位置差閾値(1)」未満で「継続時間(2)」以上継続しているか否かを判定するようにすればよい。
遷移条件C4が成立すると判定した場合(S18でYes)、制御部70は、「S3:整定」への状態遷移を行う(S19)。ステップS12、S14、S18で遷移条件が成立しなかったと判定した場合(ステップS12、S14、S18でNo)、及び、ステップS13、S15、S17、S19の実行後、制御部70は、最新の状態に応じてロボットアーム2を制御する。
最新の状態が「S1:移動中」である場合(S20で「S1:移動中」)、制御部70は、サーボ制御方式「制御(1)」によってロボットアーム2を制御する(S21)。具体的には、制御部70は、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御を実施する。
最新の状態が「S2:整定途中」又は「S3:整定」である場合(S20で「S2:整定途中」、「S3:整定」)、制御部70は、サーボ制御方式「制御(2)」によってロボットアーム2を制御する(S22)。具体的には、制御部70は、上述のトルクサーボ制御と組み合わせて位置制御を実施する。ここで、関節軸毎に独立して状態を管理する場合には、関節軸のそれぞれの状態に応じて、各関節軸を独立してサーボ制御方式「制御(1)」又はサーボ制御方式「制御(2)」で制御する。
以上に説明した処理によれば、図7に示すように、教示者からのロボットアーム2に対する直接教示の状況に対して適切なサーボ制御方式でロボットアーム2を制御することが可能となる。すなわち、教示者がロボットアーム2の姿勢を変更している状態(S1:移動中)である場合には、トルクフィードバックと、重力補償及び摩擦補償とを組み合わせたトルクサーボ制御を実施することで、教示者が小さな力で軽快にロボットアーム2の教示操作を行うことを可能としている。また、教示者がロボットアーム2の姿勢の変更を完了しようとしている、又は、ロボットアーム2の姿勢の変更を完了した状態(S2:整定途中、S3:整定)である場合には、さらに位置制御も実施することで、教示者が容易にロボットアーム2の位置決めをすることを可能とている。すなわち、本実施の形態によれば、教示者によるロボットアームの操作感を向上することができる。ここで、この位置制御として、後述するように弱いレベルの位置制御を行うことで操作性の向上を図っている。動摩擦に打ち勝つようにするために強い位置制御を実施すると、教示者がロボットアーム2を教示する際に固く、状態遷移が遅れがちとなってしまい、操作感が悪くなってしまう。それに対して、本実施の形態のように弱いレベルの位置制御を行うことで操作感を向上することができる。
このように本実施の形態では、ロボットアーム2を制御する状態は大きく分けると、サーボ制御方式が「制御(1)」となる状態(第1の状態)と、サーボ制御方式が「制御(2)」となる状態(第2の状態)との間で遷移する。第1の状態では、ロボットアーム2を制御する状態として、「S1:移動中」をとる。また、第2の状態では、ロボットアーム2を制御する状態として、「S2:整定途中」(第1のサブ状態)と、「S3:整定」(第2のサブ状態)のいずれかをとる。
続いて、図8及び図9を参照して、上述した閾値の関係について説明する。図8は、状態遷移条件において使用される閾値を説明する図である。図9は、状態遷移条件において使用される閾値の大小関係を示す図である。
まず、速度に関係する閾値として、遷移条件C1−1で使用される「速度閾値(1)」、遷移条件C2−1で使用される「速度閾値(2)」、及び、遷移条件C3−1で使用される「速度閾値(3)」がある。
「速度閾値(2)」は、移動中(S1:移動中)に、一定速度未満で停止し始めていることを判断するための閾値であるのに対し、「速度閾値(3)」は、整定に向かう途中(S2:整定途中)で、小さな速度変化の発生を判断するための閾値であるため、「速度閾値(2)」は「速度閾値(3)」以上の大きさで設定される。
「速度閾値(3)」は、整定に向かう途中(S2:整定途中)で、小さな速度変化の発生を判断するための閾値であるのに対し、「速度閾値(1)」は、完全に整定した後(S3:整定)の僅かな速度変化の発生を判断するための閾値であるため、「速度閾値(3)」は「速度閾値(1)」よりも大きく設定される。
次に、トルクに関係する閾値として、遷移条件C1−2で使用される「トルク閾値(1)」、及び、遷移条件C3−2で使用される「トルク閾値(2)」がある。
「トルク閾値(2)」は、整定に向かう途中(S2:整定途中)で、小さなトルク変化の発生を判断するための閾値であるのに対し、「トルク閾値(1)」は、完全に整定した後(S3:整定)の僅かなトルク変化の発生を判断するための閾値であるため、「トルク閾値(2)」は「トルク閾値(1)」よりも大きく設定される。
次に、位置差に関する閾値として、遷移条件4−1で使用される「位置差閾値(1)」がある。「位置差閾値(1)」は、整定に向かう途中(S2:整定途中)で、位置差が安定していると扱ってもよい整定許容範囲内であることを判断するための閾値である。
次に、継続時間に関する閾値として、遷移条件2−2で使用される「継続時間(1)」、及び、遷移条件C4−2で使用される「継続時間(2)」がある。
「継続時間(2)」は、移動中(S1:移動中)に、一定速度未満を継続し続けていることを判断するための閾値であるのに対し、「継続時間(1)」は、整定に向かう途中(S2:整定途中)で、位置差が整定許容範囲内を継続し続けていることを判断するための閾値であるため、「継続時間(2)」は「継続時間(1)」よりも大きく設定される。
制御部70による、これらの閾値を用いた判定は、例えば、これらの閾値を示す閾値情報をROM72に予め格納しておき、制御部70(CPU71)が、この閾値情報が示す閾値と比較演算することで実現される。
続いて、図10及び図11を参照して、実施の形態に係る制御装置3の指令トルク算出処理について説明する。図10は、データの入出力に対する指令トルク算出処理の流れを示すブロック図である。図11は、指令トルク算出処理における各データ及び各処理を説明するための図である。
図11では、「C」は、CONST(固定データ)を示しており、「D」は、Data(変数データ)を示しており、「P」は、Process(処理ブロック)を示している。
「C1」は、各関節軸に対する各アーム部の質量を示しており、「C2」は、各関節軸に対する各アーム部の重心位置を示している。質量「C1」及び重心「C2」は、固定値として用意されている。すなわち、質量「C1」及び重心「C2」は、例えば、それぞれを示す情報をROM72に予め格納しておき、制御部70(CPU71)が、それらの情報を参照することで取得する。
「D1」は、各関節軸の関節角度を示しており、「D2」は、各関節軸のトルクを示しめしている。すなわち、関節角度「D1」は、上述のステップS10で取得された各関節軸の関節角度を示しており、トルク「D2」は、上述のステップS10で取得された各関節軸のトルクを示している。目標停止位置「D3」は、P4で使用される目標停止位置(各関節軸の整定目標位置)であり、P4で入力された状態が「S2:整定途中」となった段階で保持される。
以下、P1〜P6の処理について説明する。P1〜P6の処理によって各関節軸(各関節部11〜13及び肩部14のモータ51〜54)に対するトルク指令値を算出する。
P1では、制御部70は、各関節軸に対する各アーム部の質量「C1」、各関節軸に対する各アーム部の重心位置「C2」、及び、各関節軸の関節角度「D1」に基づいて、各関節軸の重力補償トルクを算出する。ここで、具体的な重力補償トルクの算出方法については、一般的な逆運動学の手法を用いればよいため、詳細な説明は省略する。
P2では、制御部70は、各関節軸の関節角度「D1」に基づいて、各関節軸の速度(角速度)を算出する。具体的には、過去(例えば、前回)の各関節軸の関節角度「D1」からの変更量に基づいて、各関節軸の速度を算出する。例えば、上述した式(1)によって算出するようにしてもよい。
P3では、制御部70は、P2で算出された各関節軸の速度と、各関節軸のトルク「D2」とに基づいて、図5のステップS11〜S19で説明したように状態遷移を判定する。
P4では、制御部70は、各関節軸の関節角度「D1」と、P3で判定された最新の状態と、目標停止位置「D3」とに基づいて、ロボットアーム2の目標停止位置に対する位置制御トルクを、各関節軸について算出する。ここで、本処理P4は、P3で判定した状態が「S1:移動中」である場合には実施されず、P3で判定した状態が「S2:整定途中」又は「S3:整定」である場合に実施される。具体的には、各関節軸の位置が、それぞれの整定目標位置に位置するように、弱いゲインによる位置制御を実施する。すなわち、このゲインは、教示者が各関節部の関節軸の角度を容易に変更可能となる程度のトルクがモータ51〜54に掛かるように予め定められる。重力補償のみだと、教示者がロボットアーム2を操作した場合に無重力空間のようなフンワリとした動作となり、期待する整定目標位置に留まるのが困難となってしまう。それに対して、弱いゲインの位置制御を併用することで、整定目標位置に合わせたモータ制御が可能となる。このとき、位置制御の算出結果をトルク値に変換することで、他のトルク(重力補償トルクや摩擦補償トルク)の算出結果と足し合わせてトルク指令値を作成できるようにする。これによって、同一系統のトルク指令値によるトルクサーボ制御を可能とする。ここで、関節軸毎に独立して状態を管理する場合には、状態が「S2:整定途中」又は「S3:整定」である関節軸についてのみ、位置制御を実施して位置制御トルクを算出する。
P5では、制御部70は、外乱オブザーバによって、P2で算出された各関節軸の速度と、P1で算出された各関節軸の重力補償トルクとP4で算出された各関節軸の位置制御トルクを加算し各関節軸のトルク「D2」を減算しゲインを乗じたトルクと、P5で前回に算出した各関節軸の摩擦補償トルクとに基づいて、各関節軸の摩擦補償トルクを各関節軸について算出する。ここで、外乱オブザーバにおける、モータトルク(重力補償トルクと位置制御トルクを加算しトルク「D2」を減算したトルクと、前回の摩擦補償トルクの合計)と、モータ速度(各関節軸の速度)から外乱(摩擦補償トルク)を推定する構成については一般的であるため、詳細な説明は省略する。なお、状態が「S1:移動中」である場合には、位置制御トルクは算出されておらず、加算されないことになる。
P6では、制御部70は、上述の重力補償トルクと位置制御トルクとトルク「D2」を合成しゲインを乗じた各関節軸のトルクと、P5で算出された各関節軸の摩擦補償トルクとを加算したトルクを、各関節軸について算出する。これによって算出された各関節軸のトルクは、各関節軸(各関節部11〜13及び肩部14のモータ51〜54)に対する指令トルクとなる。制御部70は、モータ51〜54のそれぞれについて、指令トルクに対応するトルク指令値を生成して駆動回路80に出力する。これによって、駆動回路80によって、トルク指令値に対応する指令トルクでモータを駆動するための駆動電流が生成され、モータ51〜54のそれぞれに供給される。
以上に説明したように、本実施の形態では、エンコーダ30が検出した角度に基づいて算出した角速度が第1の速度閾値(例えば、速度閾値(1)に相当)以上であると判定した場合、又は、トルクセンサ40が検出したトルクが第1のトルク閾値(例えば、トルク閾値(1)に相当)以上であると判定した場合、モータ50に対してトルク制御を行う第1の状態に遷移し、エンコーダ30が検出した角度に基づいて算出した角速度が第2の速度閾値(例えば、速度閾値(2)に相当)未満であると判定した場合、モータ50に対して前記トルク制御に加えて位置制御を行う第2の状態に遷移するようにしている。
これによれば、特に位置制御又は速度制御がベースとなった直接教示方法と比較して、トルクサーボ制御(トルク制御)によるイナーシャスケーリングによって、教示者がロボットアーム2に対して操作感の良い柔軟な制御が可能となる。例えば、位置制御又は速度制御を行う場合、ゲインを抑えてロボットアーム2の姿勢変更時の抵抗感を低減する手法も考えられるが、抵抗感が各関節部の摩擦抵抗以下になることはない。それに対して、本実施の形態によれば、トルクサーボ制御によって摩擦補償やトルクフィードバックを行うことができ、見かけ上の慣性を小さくすること(イナーシャスケーリング)が可能である。また、教示者がロボットアーム2の姿勢の変更を完了することを検出した場合に遷移する第2の状態では、さらに位置制御も実施するようにしているため、教示者が容易にロボットアーム2の位置決めをすることができる。この点においても、教示者がロボットアーム2に対して操作感の良い柔軟な制御が可能となる。
また、各関節部にエンコーダ及びトルクセンサが組み込まれており、各関節部で検出した関節角度及びトルクに基づいて状態遷移を行うようにしている。すなわち、教示者がロボットアーム2に対する教示を行う場合に、操作装置(操作ハンドル、操作部)のみに限定されず、各関節部に対して直接的に教示操作を行うことができる。そのため、教示者がロボットアーム2に対して操作感の良い柔軟な制御が可能となる。すなわち、本実施の形態によれば、教示者によるロボットアームの操作感を向上することができる。
さらに、本実施の形態では、位置制御は、整定用に弱いゲインで行うのみであるため、整定時に教示者がロボットアーム2を操作しても抵抗感を感じることはなく「S1:移動中」に遷移することができる。
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した遷移条件C1−1〜C4−1は、常に、全ての遷移条件を判定するように構成しなくてもよい。上述した遷移条件C1−1〜C4−1のうち、いくつかの遷移条件を省略した形態で実施するようにしてもよい。