JP3835641B2

JP3835641B2 - ロボットの負荷パラメータ推定方法

Info

Publication number: JP3835641B2
Application number: JP21097197A
Authority: JP
Inventors: 萩原　　淳; 康之井上
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1997-08-05
Filing date: 1997-08-05
Publication date: 2006-10-18
Anticipated expiration: 2017-08-05
Also published as: JPH1148181A

Description

【０００１】
【発明の属する技術分野】
この発明は、平行リンク機構を含む多軸のロボットを駆動するサーボモータの制御に関し、特に、ロボットアームの先端に取り付けた負荷の重量および重心位置等の負荷パラメータを推定する方法に関する。
【０００２】
【従来の技術】
複数のサーボモータにより多軸の機械系を制御する場合、制御部に実機モデルを有する制御を適用することによって振動や応答を改善する、または、所望の動作を実現するといった方法がよく用いられる。それらの制御手法の効果を十分に得るためには、軸先端に取り付けた負荷のパラメータ、すなわち、重量Ｍ_gと重心位置(ｅ_x，ｅ_y，ｅ_z)が精度良く分かることが必要になる。従来では、これらの値は、負荷取り付け時、あるいは、負荷交換時に、
１．パラメータとしてオペレータが設計値を手入力する方法
２．各軸を動作させてみてその時のトルク値もしくは電流値から推定計算を行う方法
がとられてきた。以下に第２の方法の一例を示す。
各軸の先端負荷に相当するトルク値または電流値の値を
Ｔ(ｉ) (ｉ＝１，２，３．．．：ｉ軸目)、
先端負荷の重量をＭ_g、重心位置を(ｅ_x，ｅ_y，ｅ_z)とすると、Ｔ(ｉ)は
Ｔ(ｉ)＝ｆ(ｉ)(ｅ_x，ｅ_y，ｅ_z，Ｍ_g) ・・・(１)
の形で表される。ここで、ｆ(ｉ)は各軸長さと各軸角度から一意に求まる関数である。未知変数が４つあるので、上記(１)式を４軸分もしくは４姿勢分導いて、４元連立方程式を解くことによって、先端負荷パラメータを推定する。
【０００３】
【発明が解決しようとする課題】
ところが、パラメータとしてオペレータが設計値を手入力する第１の方法では、負荷の形状が複雑で、重心位置が正確に算出できないような時や、オペレータが入力する際の誤入力等により、実機との誤差が大きくなる可能性があり、また、重心位置の正確な計算やパラメータ入力に、時間と労力がかかるという問題があった。
また、第２の方法では、ロボットの姿勢によって、各未知変数(Ｍ_g，ｅ_x，ｅ_y，ｅ_z)が、ある軸のトルク値におよぼす影響の度合いが変化するため、４つの式の選び方が難しい。また、４つの式全てのトルク値に、あまり影響しない変数がある場合などは、トルク値Ｔ(ｉ)が誤差を含む場合は特に、推定計算値の誤差が、非常に大きくなり正確な値を推定できないといった問題があった。
さらに、４つの式を最適に選ぶことができたとしても、各軸のトルク値Ｔ(ｉ)がそれぞれ誤差を含む場合、計算過程で誤差が蓄積された結果として推定計算値の誤差が大きくなるという問題があった。
そこで、この発明は、推定計算値の誤差を小さくし、正確な負荷パラメータを推定することのできる方法を提供することを目的とする。
【０００４】
【課題を解決するための手段】
上記課題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、平行リンク機構を備えた６軸ロボットのアーム先端軸に取り付けた負荷の重心位置を計算するステップを含むロボットの負荷パラメータ推定方法において、前記負荷の重心位置を計算するステップは、前記先端軸のモータトルク値がピークとなるときの前記先端軸の位置を求めるステップと、前記先端軸の位置および前記アームの長さに基づいて前記負荷の重心位置を求めるステップと、を含むことを特徴とするものである。
【０００５】
第一の実施態様として、前記各軸モータトルク指令値として、各軸に正転、逆転の動作をさせた時のモータトルク指令値より、摩擦トルク分と加減速トルク分をキャンセルし、重力トルク分のみを計算した値を使用する。
第二の実施態様として、前記モータトルク指令値の代わりに、ある動作時間内のモータトルク指令値を制御周期毎に積算し、積算値を積算回数で割った値を使用する。
第三の実施態様として、前記モータトルク指令値の代わりに、モータ電流指令値を使用する。
第四の実施態様として、前記負荷の重心位置を推定計算する方法として、アーム動作時の各軸モータトルク指令値の値を監視し、前記モータトルク指令値のピーク時の各軸位置から、各軸座標上での重心方向の角度を算出し、該重心方向の角度とアーム長を使用することにより、前記負荷の重心位置を推定計算する。
第五の実施態様として、前記ロボットの動作プログラム中に、推定動作及び推定計算をする命令を有する。
【０００６】
【発明の実施の形態】
以下、この発明の実施例を図に基づいて説明する。
図１は、本発明の方法を実施するためのロボット制御装置の構成を示すブロック図である。同図において、ロボット制御装置３０にはプロセッサボード３１があり、プロセッサボード３１にはプロセッサ３１ａ、ＲＯＭ３１ｂ、ＲＡＭ３１ｃおよび不揮発性メモリ３５ａ、３５ｂが搭載されている。プロセッサ３１ａは、ＲＯＭ３１ｂに予め設定されたプログラムに基づいて、ロボット制御装置３０全体を制御する。ＲＡＭ３１ｃには各種のデータが格納されている。不揮発性メモリ３５ａ、３５ｂには、ロボット１００の動作プログラムや本発明に係る負荷パラメータ推定のためのプログラムがＲＯＭ３１ｂからロードされている。後述する図２および図３に示されている負荷パラメータ推定のための各動作および各計算は、プロセッサ３１ａが不揮発性メモリ３５ａ、３５ｂ内のプログラムを読みとって実行するソフトウエアによる機能である。プロセッサボード３１はバス３７に結合されている。ディジタルサーボ制御回路３２はバス３７に結合され、プロセッサボード３１からの指令によって、サーボアンプ３３を経由して、サーボモータ５１，５２，５３，５４，５５および５６を駆動する。これらのサーボモータはロボット１００に内蔵され、ロボット１００の各軸を動作させる。シリアルポート３４はバス３７に結合され、教示操作盤５７やその他のＲＳ２３２Ｃ機器５８と接続されている。教示操作盤５７はロボットへの教示点入力に使用される。また、Ｉ／Ｏ３６を経て外部とのデータおよび信号等の入出力が行われる。
【０００７】
図２は、この発明の方法を説明するフローチャートである。本実施例では図５の６軸ロボットの構成を用いて説明する。図５中１〜６はそれぞれ第１軸〜第６軸、７は負荷である。第３軸は５０１と５０２により構成される平行リンク機構を有している。推定計算で必要なモータトルク指令値を計算する方法としては、第二の実施態様の方法を使用する。以下、図２のフローチャートを参照しながら詳細に説明する。
(ＳＴＥＰ１)では、まず、第２軸２を正負方向に動作させ、その時のモータトルク指令値として、正方向Ｔ(＋)と、負方向Ｔ(−)を保存する。
(ＳＴＥＰ２)ではＳＴＥＰ１で保存したモータトルク指令値から、加減速トルク分と摩擦トルク分をキャンセルして重量トルク分Ｔ_g2’のみを算出する。
第２軸目重力トルクＴ_g2’は図４に示すように、正方向と負方向動作時のトルクの和を２で割って求めることができ、
Ｔ_g2’＝｛［Ｔ(＋)］＋［Ｔ(−)］｝／２・・・(２)
となる。
【０００８】
次に、第三の実施態様の方法を説明する。ＳＴＥＰ１で保存するモータトルク指令値正方向Ｔ(＋)と、負方向Ｔ(−)として、第２軸２を正方向に動作させている間のモータトルク指令値を制御周期毎に積算した値をＩＮＴ_T(＋)、負方向に動作させている間のモータトルク指令値を制御周期毎に積算した値をＩＮＴ_T(−)とする。また、積算回数をＮ回とすると、(２)式は、
Ｔ_g2’＝｛［ＩＮＴ_T(＋)］＋［ＩＮＴ_T(−)］｝／Ｎ／２・・・(２’)
となる。
第三の実施態様の方法を使用することによって、モータトルク指令値にリップルやノイズがある場合、それらの誤差の影響を小さくすることができる。
ここで、Ｔ_g2’は水平から第２軸２の姿勢角度θ₂傾いたときの重力トルクであるので、重力モーメントＴ_g2のcos(θ₂)倍したものになっている。
Ｔ_g2’=Ｔ_g2＊cos(θ₂) ・・・(３)
よって重力モーメントＴ_g2は、
Ｔ_g2＝Ｔ_g2’／cos(θ₂) ・・・(４)
で計算される。(ＳＴＥＰ３)
【０００９】
(ＳＴＥＰ４)では、先端の負荷７の重量Ｍ_gを算出する。
ここで、第３軸３が平行リンク機構を有するため、第６軸６先端に取り付けた負荷７の重量分のトルクは、重心位置(ｅ_x，ｅ_y，ｅ_z)の値に関係なく、第２軸２と第３軸３の結合部に重力トルクとしてのみかかってくる。よって、先端の重量Ｍ_gは、
Ｍ_g＝(Ｔ_g2−Ｔ_{g2 ini})／Ｌ₂ ・・・(５)
の計算式のみで求まる。Ｌ₂は２軸目アーム長である。Ｔ_{g2 ini}は予め、先端に負荷がついていないときの第２軸２にかかる重力モーメントを計算もしくは計測しておいた値である。
以上、第２軸２だけを使用して先端負荷７の重量Ｍ_gが求まつた。
【００１０】
(ＳＴＥＰ５)では、重心位置(ｅ_x，ｅ_y，ｅ_z)を求める。
本実施例では、第五の実施態様で示される、重心位置の推定計算方法の一実施例を説明する。以下、図３のフローチャートを用いて説明する。
(ＳＴＥＰ５−１) まず、第６軸６の回転軸方向が鉛直方向でないことを確認する。もし、鉛直方向を向いていた場合は、適当な角度まで(実施例では４５°以上)第４軸４と第５軸５を動作させ調整する。
(ＳＴＥＰ５−２) 次に、第６軸６を正方向に９０°以上、低速で動作させる。低速で動作させるのは、加減速時のトルクを抑えるためである。このときのモータトルク指令値を監視し、ピーク値(最大または最小値)をとる時の第６軸６座標のＸ軸からのなす角θ_p(この方向に負荷の重心がある)を記憶しておく。(図６)(ＳＴＥＰ５−３) 次に、第６軸６を動作させ第５軸５の回転方向に負荷の重心位置がくるようにする。ここで第５軸５を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θ_g5を求める。(図７)
(ＳＴＥＰ５−４) 次に、第５軸５を動作させ、第４軸４と第５軸５が一直線上にのるようにする。ここで第３軸３を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θ_g3を求める。(図８)
【００１１】
先端負荷７の重心(ｅ_x，ｅ_y，ｅ_z)は、第３軸３のアーム長Ｌ₃、第５軸アーム長Ｌ₅、θ_p、θ_g3、θ_g5を使って図８の幾何学問題を解くことによって、以下のように求めることができる。
図８のＬ₆について、２つの式が成り立つ。
Ｌ₆＝(Ｌ₃＋Ｌ₅＋ｅ_z)＊tan(θ_g3) ・・・(６)
Ｌ₆＝(Ｌ₅＋ｅ_z)＊tan(θ_g5) ・・・(７)
(６)(７)式をｅ_zについて解くと、(８)式のようになる。
ｅ_z＝｛(Ｌ₃＋Ｌ₅)＊tan(θ_g3)−Ｌ₅＊tan(θ_g5)｝
／｛tan(θ_g5)−tan(θ_g3)｝・・・(８)
また、ｅ_x，ｅ_yはＬ₆とθ_pから、以下のように求めることができる。
ｅ_x＝Ｌ₆＊cos(θ_p)＝｛(Ｌ₅＋ｅ_z)＊tan(θ_g5)｝＊cos(θ_p) ・・・(９)
ｅ_y＝Ｌ₆＊sin(θ_p)＝｛(Ｌ₅＋ｅ_z)＊tan(θ_g5)｝＊sin(θ_p)・・・(１０)
以上が、先端負荷７の重心位置を推定計算する方法である。
【００１２】
また、本実施例では、(ｅ_x，ｅ_y，ｅ_z)の推定計算方法として、すべてを幾何学的に求めたが、θ_pとθ_g5を算出した後、第４軸４または、第５軸５を動作させ、その時のモータトルク指令値Ｔ_g4と、先で算出した先端負荷７の重量Ｍ_gからモーメント長Ｌ_g4(図７)を求め、
Ｌ_g4＝Ｔ_g4／Ｍ_g ・・・(１１)
それらを使って、重心位置(ｅ_x，ｅ_y，ｅ_z)を求めることも可能である。以下に計算結果のみを示す。
ｅ_z＝Ｌ_g4＊cos(θ_g5)−Ｌ₅ ・・・(１２)
ｅ_x＝｛Ｌ_g4＊sin(θ_g5)｝＊cos(θ_p) ・・・(１３)
ｅ_y＝｛Ｌ_g4＊sin(θ_g5)｝＊sin(θ_p) ・・・(１４)
この場合も、従来の方法と違い、第４軸、５軸、６軸の各軸モータトルク指令値の誤差が蓄積されないので、精度良く重心位置を求めることができる。
【００１３】
最後に第四の実施態様の方法について説明する。
ロボットに動作を教示し、実行するためには動作プログラムが作成される。動作プログラムは、特定のプログラム言語によって記述されている。第四の実施形態の方法は、そのプログラム言語の命令の一つとして、前記ＳＴＥＰ１からＳＴＥＰ５までの処理を実行させる命令を持っておき、その命令の入ったプログラムを実行するだけで、上記各軸動作ならびに各ＳＴＥＰでの一連の処理を自動的に実行するものである。
【００１４】
【発明の効果】
以上述べたように、この発明によれば、平行リンク機構を有する軸の回転中心を含む軸のモータトルク指令値を使用して、先端負荷の重心位置に関係なく、負荷重量の値だけを算出できるという、平行リンク機構の特性を有効に利用し、先端の負荷の重量を計算するため、計算が非常に簡単であり、各軸の誤差が蓄積されることが無く、結果として、精度良く先端負荷重量Ｍ_gを算出することができるという効果がある。
また、負荷重心位置の計算にモータトルク指令値のピーク時のアーム位置から幾何学的に解く方法を採用すれば、モータトルク指令値や先に計算した負荷の重量Ｍｇに誤差を含む場合も、それらの誤差の影響を受けずに、精度良く重心位置を推定計算することができるという効果がある。
また、第四の実施態様の方法を使用すれば、オペレータはロボットアーム先端に負荷を取り付け、プログラムを実行するだけで、あとはすべて制御のソフト上で自動的に動作および計算をするため、負荷が変わった場合も簡単に精度良く負荷パラメータを推定でき、オペレータの労力と作業時間を最小にすることができるという効果がある。
【図面の簡単な説明】
【図１】この発明の方法を実施するためのロボット制御装置の構成を示すブロック図である。
【図２】この発明の方法を示すフローチャートである。
【図３】この発明の負荷重心位置の推定計算方法を示すフローチャートである。
【図４】重力分トルクの値を求める式の説明図である。
【図５】この発明の実施例を示すロボットの構成図である。
【図６】この発明の実施例のＳＴＥＰ５−２を説明する図である。
【図７】この発明の実施例のＳＴＥＰ５−３を説明する図である。
【図８】この発明の実施例のＳＴＥＰ５−４を説明する図である。
【符号の説明】
１第１軸、２第２軸、３第３軸、４第４軸、５第５軸、６第６軸、７負荷、３０ロボット制御装置、３１プロセッサボード、３１ａプロセッサ、３１ｂＲＯＭ、３１ｃＲＡＭ、３２ディジタルサーボ制御回路、３３サーボアンプ、３４シリアルポート、３５ａ、３５ｂ不揮発性メモリ、３６Ｉ／Ｏ、３７バス、５１〜５６サーボモータ、５７教示操作盤、５８ＲＳ２３２Ｃ機器、１００ロボット

Claims

平行リンク機構を備えた６軸ロボットのアーム先端軸に取り付けた負荷の重心位置を計算するステップを含むロボットの負荷パラメータ推定方法において、
前記負荷の重心位置を計算するステップは、
前記先端軸のモータトルク値がピークとなるときの前記先端軸の位置を求めるステップと、
前記先端軸の位置および前記アームの長さに基づいて前記負荷の重心位置を求めるステップと、を含むことを特徴とするロボットの負荷パラメータ推定方法。