(57)【特許請求の範囲】
【請求項１】
各軸のサーボモータとアームとの間にバネ要素を含む機構を持つ多軸ロボットの制御装置において、
干渉を与える軸と干渉を受ける軸のサーボモータへの制御入力と各軸減速機に生じるねじり角に基づき、各軸アーム側に発生する他の軸からの干渉力とさらに、前記干渉力により生じる各軸独立のセミクローズドループで構成されたサーボモータの制御入力に含まれる干渉力とを算出する手段を有し、各軸のアーム先端が他の軸からの干渉を生じずに動作するように、他の軸からの干渉力とサーボモータの制御入力に含まれる干渉力に基づいて補正トルクを算出し、前記制御入力に前記補正トルクを加算して出力する手段からなることを特徴とする多軸ロボットの制御装置。
【請求項２】
サーボモータの制御入力と実際のサーボモータの位置により状態量を再現する状態観測手段を各軸に有し、前記状態観測手段により再現されたねじり角を用いて干渉力を算出することを特徴とする請求項１記載の多軸ロボットの制御装置。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、互いの制御軸間に力学的な干渉を有し、且つ各軸にバネ要素を含む、産業用ロボットを代表とする多軸ロボットに関し、特にアーム先端に生じる干渉力を完全に除去する制御装置に関する。
【０００２】
【従来の技術】
一般に電動機により各軸が駆動される産業用ロボットにおいて、電動機の力不足や負荷側からの外乱力の影響を極力小さくするために減速比の大きなハーモニックドライブ等のギアを介して、負荷側のアームを駆動している。したがって、制御軸間の力学的干渉は従来あまり問題とされなかった。
しかし、高速化、高精度化の要求の高まりからＰＩ制御では補償しきれない力学的影響や高減速比駆動であっても無視できない外乱が生じ問題となっている。この問題を解決するために外乱推定オブザーバにより外乱補償する方法やダイレクトドライブ駆動方式のロボットと同様に動力学方程式に基づき予め補償する方法が報告されている（例えば特開昭６３−３１４６０６号公報、特開平６−２７００７９号公報、特開平７−７３０７号公報）。
【０００３】
【発明が解決しようとする課題】
負荷側に生じる外乱が大きい場合、外乱補償オブザーバによる方法では、干渉等に関する動特性は考慮していないため十分な精度の向上は望めない。動力学方程式に基づき予め補償する方法についても、従来の方法ではギアを介した場合の動特性や各軸を動作させるため計算された制御入力の含む干渉力については十分に考慮されておらず、高速動作時には完全に干渉を取り除くことはできない。
本発明が解決すべき課題は、各軸に減速機等のバネ要素を含む産業用ロボットに関して、高速動作時においても互いの軸の干渉を完全に取り除くことにある。
【０００４】
【課題を解決するための手段】
本発明の多軸ロボットの制御装置は、上記の課題を解決するため、各軸のサーボモータとアームとの間にバネ要素を含む機構を持つ多軸ロボットの制御装置において、干渉を与える軸と干渉を受ける軸のサーボモータへの制御入力と各軸減速機に生じるねじり角に基づき、各軸アーム側に発生する他の軸からの干渉力とさらに、前記干渉力により生じる各軸独立のセミクローズドループで構成されたサーボモータの制御入力に含まれる干渉力とを算出する手段を有し、各軸のアーム先端が他の軸からの干渉を生じずに動作するように、他の軸からの干渉力とサーボモータの制御入力に含まれる干渉力に基づいて補正トルクを算出し、前記制御入力に前記補正トルクを加算して出力する手段からなるものである。この装置において、各軸減速機に生じるねじり角は、各軸のサーボモータの制御入力と実際のサーボモータの位置により状態量を再現する状態観測手段により再現された値を用いることもできる。具体的には、各軸のサーボモータとアームとの結合をバネ要素を含む２慣性系で表現することにより動力学方程式を求め、各軸のアーム側に発生する多軸からの干渉力を算出するとともに、セミクローズドループ構成された各軸の制御入力が含む干渉力を算出し、干渉力を完全に打ち消すような補正トルクを制御入力に加算して出力する。
【０００５】
【発明の実施の形態】
説明を容易にするため、２軸ロボットに関して述べる。多軸ロボットにおいては、前記２軸ロボットに関して以下に説明される事項を拡大して適用することが可能である。
図１は、Ｌ軸、Ｕ軸を有する一般的な垂直２軸ロボットの模式図、図２は、平行四辺形リンク構造の垂直２軸ロボットの模式図である。図中ｌＬはＬ軸の長さ、ＭＬはＬ軸の質量、ｌＬｇはＬ軸の重心までの長さ、θｌＬはＬ軸の角度、ＭＵはＵ軸の質量、ｌＵｇはＵ軸の重心までの長さ、θｌＵはＵ軸のＬ軸に対する角度である。図１、２に示す２軸ロボットの運動方程式は、以下に示す数１〜数６により表すことができる。まず、各軸減速機に生じるねじり角を表す関係式が、次式で表される。
【数１】
0003611147.tif 000002
【数２】
0003611147.tif 000003
ここで、θｍＬはＬ軸モータの角度、θｍＵはＵ軸モータの角度、ＮＬはＬ軸減速機の減速比、ＮＵはＵ軸減速機の減速比、θＳＬはＬ軸減速機に生じるねじり角、θＳＵはＵ軸減速機に生じるねじり角である。
【０００６】
次の第３式及び第４式は、ラグランジェの運動方程式より求められる関係式である。
【数３】
0003611147.tif 000004
【数４】
0003611147.tif 000005
ここで、ＫＣＬはＬ軸減速機のバネ定数、ＫＣＵはＵ軸減速機のバネ定数、ｄＬはＬ軸に働くトルク外乱、ｄＵはＵ軸に働くトルク外乱である。
図３に運動方程式の各係数について示す。
また、図４に示すモータとアームの間のトルク伝達機構は次式で表すことができる。図中１はアーム、２は負荷、３はアーム１を駆動するモータ、４はモータ３とアーム１との間に設けられた減速機である。減速機４は、減速機構とバネ要素から成り立っていると考える。
【数５】
0003611147.tif 000006
【数６】
0003611147.tif 000007
ここで、ＪｍＬはＬ軸モータの慣性モーメント、ＪｍＵはＵ軸モータの慣性モーメント、ｕＬｒｅｆはＬ軸の制御入力、ｕＵｒｅｆはＵ軸の制御入力である。ここでの制御入力は加速度指令として表している。
【０００７】
図５は、本発明の制御装置による制御系の構成を２軸について示したブロック図である。図中１０はＬ軸状態観測手段、１１はＵ軸状態観測手段、１２はＬ軸状態コントローラ、１３はＵ軸状態コントローラ、１４は干渉力算出手段である。
数１〜６から、アーム１の位置θｌＬ、θｌＵの４回微分について解くと、次式のように表すことができる。
【数７】
0003611147.tif 000008
【数８】
0003611147.tif 000009
式７、８において、アームの位置の４回微分には、自軸の状態量、制御入力の他に、他軸の状態量、外乱項が含まれている。
制御入力を指令により算出される値、他軸からの干渉による値、干渉以外の外乱による値に分離すると次式のように表すことができる。
【数９】
0003611147.tif 000010
【数１０】
0003611147.tif 000011
ここで、各軸の非干渉化の条件は、次式が成り立つことだと言える。
【数１１】
0003611147.tif 000012
【数１２】
0003611147.tif 000013
したがって、次式が成り立つ必要がある。
【数１３】
0003611147.tif 000014
【数１４】
0003611147.tif 000015
自軸の動作に対する干渉力を取り除くための条件は、第１１式、第１２式が成り立つように制御入力を補償してやることである。つまり、第１３式、第１４式が成り立つことである。
【０００８】
ここで、さらに各軸を動作させるため計算された制御入力の含む干渉力を補償するために数１〜６により求められる次式を導入する。
【数１５】
0003611147.tif 000016
【数１６】
0003611147.tif 000017
数１５、１６を数１３、１４に代入することにより、非干渉化の補償量として次式が得られる。
【数１７】
0003611147.tif 000018
【数１８】
0003611147.tif 000019
【０００９】
ただし、この補償量には、減速機でのねじれ量も含まれているため、制御演算に使用される状態変数に次の補正量を加える。
【数１９】
0003611147.tif 000020
【数２０】
0003611147.tif 000021
【数２１】
0003611147.tif 000022
【数２２】
0003611147.tif 000023
前記の制御装置により、各軸アーム先端の動作によって生じる干渉力と各軸の制御入力が含む干渉力とが非干渉化され、アーム先端の動作に関して、干渉のない応答が得られる。
外乱が無視できる場合あるいは観測できない場合は、数１〜数２２の外乱項を省略して適用することができる。
【００１０】
以下、本発明の実施例の効果について説明する。
図６に示すように、第１軸にはゼロ指令（図６の上の特性）を、第２軸にはステップ型の速度指令（図６の下の特性）を与えた場合、通常のＰＩ制御では、第１軸に図７のような干渉力が生じるが、本発明の制御装置を用いることにより、図８のように干渉力を取り除くことができる。また、先端に水平方向に直線軌道を描くよう指令した場合、通常のＰＩ制御では図９のようにかなり軌跡にずれを生じるが、本発明の制御装置を用いることにより図１０のように改善することができる。
【００１１】
【発明の効果】
以上説明したように本発明によれば、サーボモータとアームとの間に減速機等のバネ要素を含む機構を持つ多軸ロボットにおいても、各軸のアーム側に発生する他の軸からの干渉力と各軸独立のセミクローズドループで構成されたサーボモータの制御入力に含まれる干渉力が算出され、各軸に生じる干渉トルクが補正される。これにより、減速機にねじり振動が生じるような高速動作時においても干渉力を取り除くことができ、あたかも各駆動軸間に干渉がないかのごとく各軸のサーボモータを制御することができる。
【図面の簡単な説明】
【図１】一般的な垂直２軸ロボットの模式図である。
【図２】平行四辺形リンク構造の垂直２軸ロボットの模式図である。
【図３】運動方程式の各係数について示す説明図である。
【図４】モータとアームの間のトルク伝達機構の説明図である。
【図５】本発明の制御装置による制御系の構成を２軸について示したブロック図である。
【図６】第１軸に与えるゼロ指令と、第２軸に与えるステップ型の速度指令の特性図である。
【図７】通常のＰＩ制御において、第１軸に生じる干渉力の説明図である。
【図８】本発明において干渉力が取り除かれた様子を示す説明図である。
【図９】通常のＰＩ制御において生じる軌跡のずれを示す説明図である。
【図１０】本発明において改善された軌跡のずれを示す説明図である。
【符号の説明】
１  アーム、２  負荷、３  モータ、４  減速機、１０  Ｌ軸状態観測手段、１１  Ｕ軸状態観測手段、１２  Ｌ軸状態コントローラ、１３  Ｕ軸状態コントローラ、１４  干渉力算出手段
【図１】
0003611147.tif 000024
【図２】
0003611147.tif 000025
【図３】
0003611147.tif 000026
【図４】
0003611147.tif 000027
【図５】
0003611147.tif 000028
【図６】
0003611147.tif 000029
【図７】
0003611147.tif 000030
【図８】
0003611147.tif 000031
【図９】
0003611147.tif 000032
【図１０】
0003611147.tif 000033