JP4552328B2 - ロボットの制御装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットの制御装置に関し、特に、作業座標系と関節座標系の制御モードを、連続性を保ちながらスムーズに切換えることができるロボットの制御装置に関するものである。
【０００２】
【従来の技術】
従来のロボットを用いた接触作業においては、作業座標系の特定の方向に力制御を行うことが有効な場合がある。例えば、研磨作業において、ツールの研磨方向にはロボットを柔らかく動作させて、研磨方向以外には堅く動作して位置指令通りに移動することで、力センサやクラッチを用いずに研磨作業を行うことが可能になる。作業座標系での力制御の実現手段として、例えば、特開平１１−２３１９４０号に開示の「ロボットの制御装置」が知られている。図７はそのロボットの制御装置のブロック図である。図７に示すように、関節座標系（第１のフィードバック制御系）において、角度指令が入力されるとトルク指令が導出され、それに第２トルク指令が加算されて、サーボアンプを経てサーボモータが駆動される。位置検出器による位置変位・関節角度情報は関節座標系に負帰還され、他方では、順変換器を経て作業座標系（第２のフィードバック系）へ与えられ、その出力をヤコビアン演算によりトルク指令値に変換し、第１のフィードバック系のトルク指令値に加算し修正トルク値を得て、ロボットの姿勢制御を柔軟に行う。以上のような関節座標系からのトルク指令と、作業座標系からのトルク指令を加算する方式を用いて、作業座標系の位置速度制御ループでは、特定の制御軸を柔軟に制御するようにして、ロボットを作業座標の特定の軸方向にのみ柔らかく動作させたり堅く動作させる力制御を行うことを可能にするものである。その他、作業座標系での力制御モード及び位置速度制御モードと、関節座標系での位置速度制御モードを組合わせて用いる場合も多く用いられる。例えば、一定の位置まで関節座標系の位置速度制御モードで移動させ、移動位置において作業座標系の力制御モードで接触作業を行い、再び関節作業系の位置速度制御モードによって移動を行う作業等の場合である。このような作業を行う場合には、動作させる座標系の切り換えを行っている。
【０００３】
この座標系の切り換えにおいて、関節座標系の位置速度制御モードから作業座標系の位置速度制御モード又は力制御モードへ制御モードを切り換える時点で、トルクの不連続によりロボットにショックが発生する。このショックは通常の関節座標系の位置速度制御モードでは、重力等の静的な負荷を位置速度制御系の速度積分項で補償しているが、作業座標系に移行した際に、関節座標系の速度積分項で補償していた重力トルク分が急に零になるために起こる。但し、一定時間が経過すれば、作業座標系の速度積分項に重力トルク分が溜まるため、元の位置に戻るようになる。同様に、作業座標系の位置速度制御モード又は力制御モードの制御モードから関節座標系の位置速度制御モードへ制御モードを切り換える時にも、ロボットにショックが発生する。また、重力トルクを別途演算により補償した場合でも、演算された重力トルクのズレや摩擦の影響で速度積分項には少なからず値が溜まっているため、やはり制御モードの切り換え時にはロボットにショックが発生する。このショックは短時間で僅かな動作であるため、ロボットが対象物と接触していない状態では特に問題にならないが、ロボットが対象物と接触したままで制御モードを切り換える場合には、ロボットや対象物の破損の危険性がある。このようなロボットを動作させる制御系の切り換えの問題に対しては、例えば、特開平９−６９０１３号に開示の提案がある。この提案では、図８に示すようにスイッチ８５により、関節座標系のトルク制御モードから位置速度制御モードに切り換える時点で、トルク制御モードのトルク指令Ｔｃｍｄ１を位置速度制御モードの速度積分値８３に書き換えている。また、特開平６−１２１５７１号に開示されている提案では、図９に示すように、関節座標系での位置速度制御モードからトルク制御モードへの切換え時に、位置速度制御の後段に設けられた積分器の時定数をトルク制御モード用に変化させるトルクフィルタ時定数切り換え器９１を設けて、スイッチ９５により時定数を切り換える方法がある。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記従来例では、特開平９−６９０１３号の場合は、作業座標系に拡張して作業座標系の速度積分値を関節座標系の速度積分値に書き換えようとすると、座標系間で単位系が違うために座標系間の対応関係式であるヤコビ行列の逆行列及び転置行列を解く必要があり、これらの演算は複雑で実時間で解くのは非常に難しいという問題があった。また、特開平６−１２１５７１では、トルク制御モードにはトルク指令のフィルタ時定数を変化させているため制御の応答性が悪く、更に、重力などが作用する環境下では、位置速度制御の速度積分項で補償していた重力トルク分をトルク制御側で補償できないため、トルク指令の連続性を保つことができないという問題があった。そこで、本発明は、ロボットの柔軟制御において、関節座標系と作業座標系間におけるトルク制御と位置速度制御の移行時にショック無くスムーズに切り換えできるロボットの制御装置を提供することを目的としている。
【０００５】
【課題を解決するための手段】
上記目的を達成するため、請求項１に記載の発明は、ロボットの関節を駆動するモータの制御回路と関節角度を計測する位置検出器を備え、前記関節角度と前記ロボットの関節座標系での角度指令を基に第１の位置、速度の状態フィードバック制御を行う制御モードと、前記関節角度と前記ロボットの作業座標系での位置指令を基に第２の位置、速度の状態フィードバック制御を行う制御モードを有するロボットの制御装置において、前記第１の制御モードと前記第２の制御モードを切り換える制御モード切換手段を有し、前記制御モード切換手段は、制御モードの切り換え直後から所定時間を数える時間カウンタと、前記時間カウンタ始動後、前記所定時間の間に前記第１の状態フィードバック制御の速度積分値を０にまで減少させながら出力する第１減算手段と、前記時間カウンタ始動後、前記所定時間の間に前記第２の状態フィードバック制御の速度積分値を０にまで減少させながら出力する第２減算手段と、前記第１の状態フィードバック制御の関節角トルク指令値に、前記第２の減算手段の出力を前記関節座標系での関節角トルク指令値に変換した値を加算して出力する第１加算手段と、前記第２の状態フィードバック制御の関節角トルク指令値を前記関節座標系での関節角トルク指令値に変換した値に、前記第１減算手段の出力を加算して出力する第２加算手段と、前記第１加算手段の出力または前記第２加算手段の出力の一方を出力するトルク指令切換手段を有することを特徴としている。
このロボットの制御装置によれば、関節座標系と作業座標系間で制御モードを切り換える場合に、切換え時点の補償を行うことによって、連続性を保ってスムーズに切り換えることができる。
また、関節座標系と作業座標系間で制御モードを切り換える際に、一定時間の時間カウンタをスタートさせ時間カウンタの時間内まで、減算手段により関節座標系の速度積分値を漸減させて出力トルク値を作業座標系の速度積分値に加算して制御し、一方、作業座標系から関節座標系へ制御モードを切り換える際には、時間カウンタの時間内まで作業座標系の速度積分値を漸減させ出力トルク値を関節座標系の速度積分値に加算して制御することで、制御モードの切り換え時のショックを無くすことができる。
また、請求項２に記載の発明は、前記制御モード切換手段は、前記ロボットの各関節に作用する重力トルクを算出する重力トルク算出手段と、前記トルク指令切換手段の出力と前記重力トルク算出手段の出力との和を出力する重力トルク補償手段を有することを特徴としている。
このロボットの制御装置によれば、重力分を別途補償することによって、速度積分項に溜まる値を小さくして、時間カウンタの収束時間を短縮できる。
また、請求項３に記載の発明は、前記制御モード切換手段は、前記第１の状態フィードバック制御から前記第２の状態フィードバック制御へ制御モードを切り換える際に、前記トルク指令切換手段を前記第２加算手段側に切り換えると共に前記第１減算手段について前記時間カウンタを始動させ、前記第１減算手段により前記第１の状態フィードバック制御の速度積分値を前記所定時間の間に０にまで減少させながら、前記第１減算手段の出力と前記第２の状態フィードバック制御の関節角トルク値とを前記第２加算手段により加算して前記トルク指令切換手段に出力することを特徴としている。
このロボットの制御装置によれば、関節座標系から作業座標系へ制御モードを切り換える際に、関節座標系の速度積分値を減少させながら作業座標系の関節角トルク値に加算し補償することで、切換え時ショックを無くすことができる。
【０００６】
また、請求項４に記載の発明は、前記制御モード切換手段は、前記第２の状態フィードバック制御から前記第１の状態フィードバック制御へ制御モードを切り換える際に、前記トルク指令切換手段を前記第１加算手段側に切り換えると共に前記第２減算手段について前記時間カウンタを始動させ、前記第２減算手段により前記第２の状態フィードバック制御の速度積分値を前記所定時間の間に０にまで減少させながら、前記第２減算手段の出力を前記関節座標系での関節角トルク指令値に変換した値と前記第１の状態フィードバック制御の関節角トルク指令値とを前記第１加算手段により加算して前記トルク指令切換手段に出力することを特徴としている。
このロボットの制御装置によれば、作業座標系から関節座標系へ制御モードを切り換える際に、作業座標系の速度積分値を減少させながら関節座標系の関節角トルク値に加算し補償することで、切換え時ショックを無くすことができる。
また、請求項５に記載の発明は、前記第１減算手段および前記第２減算手段は、速度積分の後段に前記時間カウンタの所定時間で０に減少するリミットを設けることによって実現することを特徴としている。
このロボットの制御装置によれば、時間カウンタの時間ｔ内に速度積分値が零に収束するようなリミットによる減算手段により、制御モード切換え時にショックが発生しないように補償することができる。
また、請求項６に記載の発明は、前記第２の制御モードが上位からの力指令制限値およびトルク指令制限値に応じて前記作業座標系の位置速度系の出力部分に、前記作業座標系の軸方向ごとに力・トルクリミットを設けて力およびトルク指令を制限するトルク制御モードを第３の制御モードとして備え、
前記制御モード切換手段が、前記第１の状態フィードバック制御から前記第２の状態フィードバック制御または前記トルク制御へ制御モードを切り換える際に、前記作業座標系の特定の軸方向のみ前記第３の制御モードを適用し、前記作業座標系のその他の軸方向には前記第２の制御モードを適用し、前記第１減算手段により前記関節座標系の速度積分値を徐々に減少させながら出力して、前記時間カウンタの所定時間経過後に、前記トルク制御モードにて制御される方向については力・トルクリミットにより前記作業座標系でのトルク制御モードを実行し、その他の方向については前記作業座標系での位置、速度の状態フィードバック制御を実行することを特徴としている。
このロボットの制御装置によれば、制御モードの切換えの他、制御モードの変更もできるので、作業座標系の特定の軸のみトルク制御モードにして、他の軸は位置制御モードにして、ロボットが物体に接触した状態でも制御モードの切換えをスムーズに行うことができる。
【０００７】
【発明の実施の形態】
以下、本発明の第１の実施の形態について図を参照して説明する。図１は本発明の第１の実施の形態に係るロボットの制御装置の制御ブロック図である。図１は、本発明の概念的な構成を示したものである。図１において、関節座標系位置速度ループ（第１のフィードバック制御系）１に、角度指令が与えられると、減算器において帰還値と偏差が算出され位置ゲインで速度変換され、減算器で帰還値角度ＦＢを微分器で時間微分した帰還速度との速度偏差が求められ、更に速度ゲインが係数されて積分器と並列接続された比例増幅器との出力を加算して、制御モード切換手段３からアンプを経て、駆動出力としてモータ５を駆動する。他方、作業座標系ループでは、位置検出器６からの関節角度情報を順変換７に加え、そこから、作業座標系での位置フィードバック（位置ＦＢ）を求め、作業座標系の位置速度ループ（第２のフィードバック制御系）２を構成する。この作業座標系の出力をトルク変換手段４によりヤコビアン行列演算８を用いて関節座標系のトルク指令に変換して、制御モード切換手段３により、関節座標系の制御モードと作業座標系の制御モードを切換え駆動する。
【０００８】
つぎに動作について説明する。先ず、関節座標系での位置速度制御系（第１のフィードバック制御系）１と、作業座標系での位置速度制御系（第２のフィードバック制御系）２の構成は、モータ５の位置検出器６で計測された関節角度フィードバック（角度ＦＢ）から、一般的に順変換又は順運動学と呼ばれる関節変位とロボットの作業位置の関係式を用いて、作業座標系での位置フィードバック（位置ＦＢ）を求める。この位置ＦＢと作業座標系の位置指令を元に、作業座標系での位置制御ループおよび速度制御ループを組むことで、作業座標系の位置速度制御系（第２のフィードバック制御系）２を構成する。この作業座標系の制御系の出力は力指令及びトルク指令であるため、ロボットの関節軸毎に配置されたモータ５を駆動するためには、関節座標系のトルク指令に変換する必要がある。この変換には一般的にヤコビアンと呼ばれる行列式の転置行列である関節座標系と作業座標系の静力学対応関係式８を用いることで、作業座標系の力指令及びトルク指令を関節座標系におけるトルク指令に、トルク変換手段４により変換する。これによって、関節座標系の位置速度ループ１の出力であるトルク指令と、作業座標系の位置速度ループ２から求められたトルク指令を制御モード切換手段３により切換えて、作業座標系での位置速度制御モードと、関節座標系での位置速度制御モードを併用した、スムーズな切換えが可能になる。
【０００９】
次に、本発明の第２の実施の形態について図を参照して説明する。図２は本発明の第２の実施の形態に係るロボットの制御装置の制御ブロック図である。図２は前実施の形態の図１に示した制御モード切換手段の具体構成を示す図であり、実際に後段へ出力するトルク指令を切換えるトルク指令切換手段２３と、トルク指令切換手段２３によって制御モードが関節座標系から作業座標系に切換わった場合と、作業座標系から関節座標系に切換わった場合に予め決められた一定時間ｔを数える時間カウンタ１０を設けている。更に、関節座標系、作業座標系それぞれの位置速度ループにある速度積分項１５、１６の後段に、時間カウンタ１０の時間ｔ内に速度積分値を零にする減算手段１１、１２を設けている。作業座標系の制御モード時には、関節座標系の速度積分項１５の後段にある減算手段１１からの出力は、加算手段１４によって作業座標系からのトルク指令と加算される。又、関節座標系の制御モード時は、作業座標系の速度積分項１６の後段の減算手段１２からの出力は、加算手段１３により関節座標系の出力であるトルク指令に加算される。トルク指令切換手段２３は、加算手段１４の出力、加算手段１３の出力のいずれか一方のトルク指令を選択して出力する。更に、重力トルク算出手段１８と、重力トルク補償手段１７をトルク指令切換手段２３の後段に設けている。なお、その他の図１と同一構成の部分には同一符号を付して重複する説明は省略する。
【００１０】
つぎに動作について説明する。先ず、図２のような関節座標系で構成された位置速度制御ループ１でトルク指令を演算して、高速な位置決め動作を行う場合に、重力トルク演算手段１８、重力トルク補償手段１７により重力分を別途補償することによって、速度積分項１５に溜まる値を小さくして、速度積分値を減少させる時間カウンタ１０の収束時間を短くするように制御する。このまま、関節座標系の位置速度制御ループ１から作業座標系の位置速度制御ループ２に制御モードを切り換えると、重力補償によるトルクの釣り合い状態が崩れてショックが発生するので、制御モードの切り換え直後から時間カウンタ１０をスタートさせて時間ｔ内だけ、減算手段１１により速度積分値を徐々に小さくしてショックを無くすように制御する。同様に、作業座標系の位置制御モードから関節座標系の位置制御モードへ制御モードを切り換える際には、時間カウンタ１０の時間ｔ内だけ速度積分項１６の後段の減算手段１２により、作業座標系の速度積分値を漸減してショックが発生しないように補償する。
【００１１】
次に、本発明の第３の実施の形態について図を参照して説明する。図３は本発明の第３の実施の形態に係るロボットの制御回路の制御ブロック図である。図３において、速度積分項（１／ｓ）１５の後段に設ける減算手段は、速度積分リミットを用いた減算方式又は係数乗算方式の減算手段２０を用いている。８は図２と同様なヤコビアン演算、４はトルク変換手段である。なお、図３の各ブロックは関数、係数による記号表示を用いているが機能的には図２と同一構成であり、同一構成については同一符号を付して重複説明は省略する。なお図３は関節座標系から作業座標系に制御モードを切り換える例であって、この場合、図２のうち減算手段１２は機能しないので図３から省略している。またトルク指令切換手段２３は加算手段１４側へ切り換わっており加算手段１３の出力は無視されるので同様に図３から省略している。つぎに動作について説明する。先ず、ロボットの通常の移動には、関節座標系で構成された位置速度制御系１でトルク指令を演算して、高速な位置決め動作を行う。その際、重力トルク算出手段１８と重力トルク補償手段１７により、重力トルク分の補償を行う。この重力トルク算出手段１８としては、例えば、ロボットの各リンクの重量と重心位置と各関節の角度から求めてトルク指令に加算する。こうして重力分を別途補償することによって、速度積分項１５に溜まる値を小さくすることができ、速度積分値を減少させるための時間カウンタ１０の収束時間を短くすることができる。このように重力トルク分を補償した場合でも、重力補償のズレや摩擦の影響により、ロボットは指令された位置に正確に位置決めされた状態で速度積分項にはある値（定常偏差）が溜まっているが、ロボットの各関節に働く外力とモータの発生するトルクが釣り合った状態にある。このまま作業座標系の位置速度制御系２に制御モードを切り換えると、関節座標系の速度積分項で補償されていた重力補償のズレや、摩擦分のトルクが無くなり、釣り合った状態が崩れる。一定時間経過すれば、作業座標系の速度積分項１６に重力補償のズレや摩擦分のトルクが溜まって釣り合った状態に戻るが、その前にモータ各軸にショックが発生する。そこで、制御モードの切換えを行った直後から、時間カウンタ１０がカウントを開始し、時間カウンタの時間ｔ内だけ、関節座標系の速度積分項１５の後段にある減算手段２０により、関節座標系の速度積分値を徐々に小さくしながら出力して作業座標系のトルク指令に加算する。これによって、作業座標系の速度積分項１６に溜まるまでの補償を行うことで、ショックが無く安定した制御モードの切換えが可能になる。この時は作業座標系の速度積分項１６の後段の減算手段は作用しないようにする。また、速度積分値を徐々に小さくする手段としては、時間カウンタ１０の時間ｔ内に零に達するように、前制御周期までの速度積分値に１より小さい係数を乗ずる方法や、時間カウンタ１０の時間内にゼロになる速度積分リミットを設ける方式がある。本実施の形態では、後者の速度積分リミットを設けた方式を示す。この関節座標系の速度積分リミットは、制御モード切り換え後に徐々に小さく絞られ、一定時間ｔ後に零になるため、作業座標系での位置速度制御系に悪影響を及ぼすことはない。
【００１２】
次に、本発明の第４の実施の形態について図を参照して説明する。図４は本発明の第４の実施の形態に係るロボットの制御装置の制御ブロック図である。図４において、２１は作業座標系の速度積分項１６の後段に設けた減算手段である。その他の図３と同一構成には同一符号を付して重複説明は省略する。なお図４に示す第４の実施の形態は、第３の実施の形態とは逆に、作業座標系から関節座標系へ制御モードを切り換える例を示したものであって、この場合、図２のうち減算手段１１は機能しないので図４から省略している。またトルク指令切換手段２３は加算手段１３側へ切り換わっており加算手段１４の出力は無視されるので同様に図４から省略している。つぎに動作について説明する。先ず、作業座標系から関節座標系へ制御モードを切り換えた直後から、時間カウンタ１０がカウントを開始し、時間カウンタの時間内ｔだけ、作業座標系の速度積分項１６の後段にある減算手段２１により、作業座標系の速度積分値を徐々に小さくしながら出力して関節座標系のトルク指令に加算する。これによって、関節座標系の速度積分項１５にデータが溜まるまでの補助を行うことでショックが無くて安定した制御モードの切り換えが可能になる。
【００１３】
次に、本発明の第５の実施の形態について図を参照して説明する。
図５は本発明の第５の実施の形態に係るロボットの制御装置の制御ブロック図である。図６は図５に示すロボットの制御装置の速度積分値の状態変化図である。図５において、２２は上位からの力指令・トルク指令に応じて作業座標系の位置速度系の出力部分に設けた、トルク指令のリミットである。その他の図３と同一構成には同一符号を付して重複説明は省略する。また図３と同様、図２のうち減算手段１２や加算手段１３は本実施例では機能しないので図５から省略している。つぎに動作について説明する。図５の構成は作業座標系の制御モードを位置速度制御モードではなく、トルク制御モードで行うものである。そのトルク制御モードの方法としては、上位からの力指令及びトルク指令に応じて作業座標系の位置速度制御系出力部分にリミット２２を設けて、力及びトルク指令を制限する。関節座標系の位置速度制御系から作業座標系のトルク制御モードに切り換えると、作業座標系の速度積分項に溜まるまでの間、関節座標系の速度積分値を徐々に絞りながら出力する。一定時間ｔ経過後は、力・トルクリミットにより作業座標系でのトルク制御モードを実行できる。
従って、多軸ロボットでは、作業座標系における特定の軸のみトルク制御モードにして、他の軸は位置速度制御モードにすることも可能であり、特定の方向には力の制御を行って、他の方向には指令通りの位置制御を行うということが可能になる。
これはロボットが物体に接触した状態の制御モード等に応用できる。この場合の制御モードの切換え状態を図６の状態変化図から説明すると、図６は１軸同士の場合を示しているが、例えば、６軸ロボットの場合は、関節座標系の制御系が６軸分あって、作業座標系の制御系も直交３軸と回転３軸がそれぞれあり、直接的に制御系の軸同士が１対１の対応にはならない。即ち、関節座標系と作業座標系における特定の軸同士の速度積分値自体が制御系間を移動するのではなく、関節座標系の速度積分値の６軸分が合成されたものが、作業座標系の直交軸と回転軸の速度積分値にそれぞれ分配されて移行する。
図のように、関節座標系の位置速度制御モードでロボットを動作させている間は、速度積分リミット内で速度積分値が変動している。制御モードを作業座標系のトルク制御モードに切り換えると、一定時間ｔで零になるように関節座標系の速度積分リミット（上限リミットと下限リミット）が絞られる。
これにより、関節座標系の速度積分値を補うように作業座標系の速度積分値に溜まり始める。同様に、制御モードを作業座標系のトルク制御モードから、関節座標系の位置速度制御モードに切り換えると、作業座標系の速度積分リミットが絞られるが、関節座標系の速度積分値が溜まり始めるというように互いに補償してスムーズな切換えが行われる。
【００１４】
【発明の効果】
以上説明したように、本発明によれば、ロボットの制御系を関節座標系の位置速度制御ループと、作業座標系の位置速度制御ループでそれぞれ構成して座標系間で制御系の移行を行う際に、新しい制御系に制御モードが切り換わった後も元の制御系の速度積分値を時間カウンタの時間内に徐々に絞りながら出力し、新しい制御系のトルク指令に加算して、新しい制御系の速度積分値に溜まるまでの重力補償のズレや摩擦の影響の補償を行うことで、制御系を切り換える際のトルク指令の不連続によるショックを減少させることが可能となる。これには複雑な演算を行う必要がなく、様々な作業においても実時間での動作が可能になる。また、制御モード切換えから時間カウンタの時間後には完全に新しい制御系に移行するため、元の制御系の処理が影響することがない。更に、ロボットが物体に接触した状態での制御モードの切換えも可能になるため、制御モードを切り換えるための待機点も必要なくなり、タクト時間も短縮できる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係るロボットの制御装置の制御ブロック図である。
【図２】本発明の第２の実施の形態に係るロボットの制御装置の制御ブロック図である。
【図３】本発明の第３の実施の形態に係るロボットの制御装置の制御ブロック図である。
【図４】本発明の第４の実施の形態に係るロボットの制御装置の制御ブロック図である。
【図５】本発明の第５の実施の形態に係るロボットの制御装置の制御ブロック図である。
【図６】図５に示すロボットの制御装置の速度積分値の状態変化図である。
【図７】従来のロボットの制御装置の制御ブロック図である。
【図８】従来の制御モード切換え方式のブロック図である。
【図９】従来のモータドライブ装置の積分器の詳細図である。
【符号の説明】
１ 関節座標系位置速度ループ
２ 作業座標系位置速度ループ
３ 制御モード切換手段
４ トルク変換手段
５ モータ
６ 位置検出器
７ 順変換
８ 座標系間の静力学対応関係の演算手段
１０ 時間カウンタ
１１、２０ 第１減算手段
１２、２１ 第２減算手段
１３ 第１加算手段
１４ 第２加算手段
１５、１６ 速度積分項
１７ 重力トルク補償手段
１８ 重力トルク算出手段
２２ リミット
２３ トルク指令切換手段

Claims (6)

1. ロボットの関節を駆動するモータの制御回路と関節角度を計測する位置検出器を備え、前記関節角度と前記ロボットの関節座標系での角度指令を基に第１の位置、速度の状態フィードバック制御を行う制御モードと、前記関節角度と前記ロボットの作業座標系での位置指令を基に第２の位置、速度の状態フィードバック制御を行う制御モードを有するロボットの制御装置において、
   前記第１の制御モードと前記第２の制御モードを切り換える制御モード切換手段を有し、
   前記制御モード切換手段は、制御モードの切り換え直後から所定時間を数える時間カウンタと、
   前記時間カウンタ始動後、前記所定時間の間に前記第１の状態フィードバック制御の速度積分値を０にまで減少させながら出力する第１減算手段と、
   前記時間カウンタ始動後、前記所定時間の間に前記第２の状態フィードバック制御の速度積分値を０にまで減少させながら出力する第２減算手段と、
   前記第１の状態フィードバック制御の関節角トルク指令値に、前記第２の減算手段の出力を前記関節座標系での関節角トルク指令値に変換した値を加算して出力する第１加算手段と、
   前記第２の状態フィードバック制御の関節角トルク指令値を前記関節座標系での関節角トルク指令値に変換した値に、前記第１減算手段の出力を加算して出力する第２加算手段と、
   前記第１加算手段の出力または前記第２加算手段の出力の一方を出力するトルク指令切換手段を有することを特徴とするロボットの制御装置。
2. 前記制御モード切換手段は、前記ロボットの各関節に作用する重力トルクを算出する重力トルク算出手段と、前記トルク指令切換手段の出力と前記重力トルク算出手段の出力との和を出力する重力トルク補償手段を有することを特徴とする請求項１記載のロボットの制御装置。
3. 前記制御モード切換手段は、前記第１の状態フィードバック制御から前記第２の状態フィードバック制御へ制御モードを切り換える際に、
   前記トルク指令切換手段を前記第２加算手段側に切り換えると共に前記第１減算手段について前記時間カウンタを始動させ、
   前記第１減算手段により前記第１の状態フィードバック制御の速度積分値を前記所定時間の間に０にまで減少させながら、
   前記第１減算手段の出力と前記第２の状態フィードバック制御の関節角トルク指令値とを前記第２加算手段により加算して前記トルク指令切換手段に出力することを特徴とする請求項１又は２のいずれか１項記載のロボット制御装置。
4. 前記制御モード切換手段は、前記第２の状態フィードバック制御から前記第１の状態フィードバック制御へ制御モードを切り換える際に、
   前記トルク指令切換手段を前記第１加算手段側に切り換えると共に前記第２減算手段について前記時間カウンタを始動させ、
   前記第２減算手段により前記第２の状態フィードバック制御の速度積分値を前記所定時間の間に０にまで減少させながら、
   前記第２減算手段の出力を前記関節座標系での関節角トルク指令値に変換した値と前記第１の状態フィードバック制御の関節角トルク指令値とを前記第１加算手段により加算して前記トルク指令切換手段に出力することを特徴とする請求項１〜３のいずれか１項記載のロボットの制御装置。
5. 前記第１減算手段および前記第２減算手段は、速度積分の後段に前記時間カウンタの所定時間で０に減少するリミットを設けることによって実現することを特徴とする請求項１〜４のいずれか１項記載のロボットの制御装置。
6. 前記第２の制御モードは、上位からの力指令制限値およびトルク指令制限値に応じて前記作業座標系の位置速度系の出力部分に、前記作業座標系の軸方向ごとに力・トルクリミットを設けて力およびトルク指令を制限するトルク制御モードを第３の制御モードとして備え、
   前記制御モード切換手段は、前記第１の状態フィードバック制御から前記第２の状態フィードバック制御または前記トルク制御へ制御モードを切り換える際に、前記作業座標系の特定の軸方向のみ前記第３の制御モードを適用し、前記作業座標系のその他の軸方向には前記第２の制御モードを適用し、
   前記第１減算手段により前記関節座標系の速度積分値を徐々に減少させながら出力して、前記時間カウンタの所定時間経過後に、前記トルク制御モードにて制御される方向については力・トルクリミットにより前記作業座標系でのトルク制御モードを実行し、その他の方向については前記作業座標系での位置、速度の状態フィードバック制御を実行することを特徴とする請求項１〜５のいずれか１項記載のロボットの制御装置。