JP3835641B2 - ロボットの負荷パラメータ推定方法 - Google Patents
ロボットの負荷パラメータ推定方法 Download PDFInfo
- Publication number
- JP3835641B2 JP3835641B2 JP21097197A JP21097197A JP3835641B2 JP 3835641 B2 JP3835641 B2 JP 3835641B2 JP 21097197 A JP21097197 A JP 21097197A JP 21097197 A JP21097197 A JP 21097197A JP 3835641 B2 JP3835641 B2 JP 3835641B2
- Authority
- JP
- Japan
- Prior art keywords
- load
- axis
- center
- robot
- gravity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Manipulator (AREA)
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’は水平から第２軸２の姿勢角度θ2傾いたときの重力トルクであるので、重力モーメントＴg2のcos(θ2)倍したものになっている。
Ｔg2’=Ｔg2＊cos(θ2) ・・・(３)
よって重力モーメントＴg2は、
Ｔg2＝Ｔg2’／cos(θ2) ・・・(４)
で計算される。(ＳＴＥＰ３)
【０００９】
(ＳＴＥＰ４)では、先端の負荷７の重量Ｍgを算出する。
ここで、第３軸３が平行リンク機構を有するため、第６軸６先端に取り付けた負荷７の重量分のトルクは、重心位置(ｅx，ｅy，ｅz)の値に関係なく、第２軸２と第３軸３の結合部に重力トルクとしてのみかかってくる。よって、先端の重量Ｍgは、
Ｍg＝(Ｔg2−Ｔg2 ini)／Ｌ2 ・・・(５)
の計算式のみで求まる。Ｌ2は２軸目アーム長である。Ｔg2 iniは予め、先端に負荷がついていないときの第２軸２にかかる重力モーメントを計算もしくは計測しておいた値である。
以上、第２軸２だけを使用して先端負荷７の重量Ｍgが求まつた。
【００１０】
(ＳＴＥＰ５)では、重心位置(ｅx，ｅy，ｅz)を求める。
本実施例では、第五の実施態様で示される、重心位置の推定計算方法の一実施例を説明する。以下、図３のフローチャートを用いて説明する。
(ＳＴＥＰ５−１) まず、第６軸６の回転軸方向が鉛直方向でないことを確認する。もし、鉛直方向を向いていた場合は、適当な角度まで(実施例では４５°以上)第４軸４と第５軸５を動作させ調整する。
(ＳＴＥＰ５−２) 次に、第６軸６を正方向に９０°以上、低速で動作させる。低速で動作させるのは、加減速時のトルクを抑えるためである。このときのモータトルク指令値を監視し、ピーク値(最大または最小値)をとる時の第６軸６座標のＸ軸からのなす角θp(この方向に負荷の重心がある)を記憶しておく。(図６)(ＳＴＥＰ５−３) 次に、第６軸６を動作させ第５軸５の回転方向に負荷の重心位置がくるようにする。ここで第５軸５を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θg5を求める。(図７)
(ＳＴＥＰ５−４) 次に、第５軸５を動作させ、第４軸４と第５軸５が一直線上にのるようにする。ここで第３軸３を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θg3を求める。(図８)
【００１１】
先端負荷７の重心(ｅx，ｅy，ｅz)は、第３軸３のアーム長Ｌ3、第５軸アーム長Ｌ5、θp、θg3、θg5を使って図８の幾何学問題を解くことによって、以下のように求めることができる。
図８のＬ6について、２つの式が成り立つ。
Ｌ6＝(Ｌ3＋Ｌ5＋ｅz)＊tan(θg3) ・・・(６)
Ｌ6＝(Ｌ5＋ｅz)＊tan(θg5) ・・・(７)
(６)(７)式をｅzについて解くと、(８)式のようになる。
ｅz＝｛(Ｌ3＋Ｌ5)＊tan(θg3)−Ｌ5＊tan(θg5)｝
／｛tan(θg5)−tan(θg3)｝ ・ ・・(８)
また、ｅx，ｅyはＬ6とθpから、以下のように求めることができる。
ｅx＝Ｌ6＊cos(θp)＝｛(Ｌ5＋ｅz)＊tan(θg5)｝＊cos(θp) ・・・(９)
ｅy＝Ｌ6＊sin(θp)＝｛(Ｌ5＋ｅz)＊tan(θg5)｝＊sin(θp)・・・(１０)
以上が、先端負荷７の重心位置を推定計算する方法である。
【００１２】
また、本実施例では、(ｅx，ｅy，ｅz)の推定計算方法として、すべてを幾何学的に求めたが、θpとθg5を算出した後、第４軸４または、第５軸５を動作させ、その時のモータトルク指令値Ｔg4と、先で算出した先端負荷７の重量Ｍgからモーメント長Ｌg4(図７)を求め、
Ｌg4＝Ｔg4／Ｍg ・・・(１１)
それらを使って、重心位置(ｅx，ｅy，ｅz)を求めることも可能である。以下に計算結果のみを示す。
ｅz＝Ｌg4＊cos(θg5)−Ｌ5 ・・・(１２)
ｅx＝｛Ｌg4＊sin(θg5)｝＊cos(θp) ・・・(１３)
ｅy＝｛Ｌg4＊sin(θg5)｝＊sin(θp) ・・・(１４)
この場合も、従来の方法と違い、第４軸、５軸、６軸の各軸モータトルク指令値の誤差が蓄積されないので、精度良く重心位置を求めることができる。
【００１３】
最後に第四の実施態様の方法について説明する。
ロボットに動作を教示し、実行するためには動作プログラムが作成される。動作プログラムは、特定のプログラム言語によって記述されている。第四の実施形態の方法は、そのプログラム言語の命令の一つとして、前記ＳＴＥＰ１からＳＴＥＰ５までの処理を実行させる命令を持っておき、その命令の入ったプログラムを実行するだけで、上記各軸動作ならびに各ＳＴＥＰでの一連の処理を自動的に実行するものである。
【００１４】
【発明の効果】
以上述べたように、この発明によれば、平行リンク機構を有する軸の回転中心を含む軸のモータトルク指令値を使用して、先端負荷の重心位置に関係なく、負荷重量の値だけを算出できるという、平行リンク機構の特性を有効に利用し、先端の負荷の重量を計算するため、計算が非常に簡単であり、各軸の誤差が蓄積されることが無く、結果として、精度良く先端負荷重量Ｍgを算出することができるという効果がある。
また、負荷重心位置の計算にモータトルク指令値のピーク時のアーム位置から幾何学的に解く方法を採用すれば、モータトルク指令値や先に計算した負荷の重量Ｍｇに誤差を含む場合も、それらの誤差の影響を受けずに、精度良く重心位置を推定計算することができるという効果がある。
また、第四の実施態様の方法を使用すれば、オペレータはロボットアーム先端に負荷を取り付け、プログラムを実行するだけで、あとはすべて制御のソフト上で自動的に動作および計算をするため、負荷が変わった場合も簡単に精度良く負荷パラメータを推定でき、オペレータの労力と作業時間を最小にすることができるという効果がある。
【図面の簡単な説明】
【図１】 この発明の方法を実施するためのロボット制御装置の構成を示すブロック図である。
【図２】 この発明の方法を示すフローチャートである。
【図３】 この発明の負荷重心位置の推定計算方法を示すフローチャートである。
【図４】 重力分トルクの値を求める式の説明図である。
【図５】 この発明の実施例を示すロボットの構成図である。
【図６】 この発明の実施例のＳＴＥＰ５−２を説明する図である。
【図７】 この発明の実施例のＳＴＥＰ５−３を説明する図である。
【図８】 この発明の実施例のＳＴＥＰ５−４を説明する図である。
【符号の説明】
１ 第１軸、２ 第２軸、３ 第３軸、４ 第４軸、５ 第５軸、６ 第６軸、７ 負荷、３０ ロボット制御装置、３１ プロセッサボード、３１ａ プロセッサ、３１ｂ ＲＯＭ、３１ｃ ＲＡＭ、３２ ディジタルサーボ制御回路、３３ サーボアンプ、３４ シリアルポート、３５ａ、３５ｂ 不揮発性メモリ、３６ Ｉ／Ｏ、３７ バス、５１〜５６ サーボモータ、５７ 教示操作盤、５８ ＲＳ２３２Ｃ機器、１００ ロボット
【発明の属する技術分野】
この発明は、平行リンク機構を含む多軸のロボットを駆動するサーボモータの制御に関し、特に、ロボットアームの先端に取り付けた負荷の重量および重心位置等の負荷パラメータを推定する方法に関する。
【０００２】
【従来の技術】
複数のサーボモータにより多軸の機械系を制御する場合、制御部に実機モデルを有する制御を適用することによって振動や応答を改善する、または、所望の動作を実現するといった方法がよく用いられる。それらの制御手法の効果を十分に得るためには、軸先端に取り付けた負荷のパラメータ、すなわち、重量Ｍ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’は水平から第２軸２の姿勢角度θ2傾いたときの重力トルクであるので、重力モーメントＴg2のcos(θ2)倍したものになっている。
Ｔg2’=Ｔg2＊cos(θ2) ・・・(３)
よって重力モーメントＴg2は、
Ｔg2＝Ｔg2’／cos(θ2) ・・・(４)
で計算される。(ＳＴＥＰ３)
【０００９】
(ＳＴＥＰ４)では、先端の負荷７の重量Ｍgを算出する。
ここで、第３軸３が平行リンク機構を有するため、第６軸６先端に取り付けた負荷７の重量分のトルクは、重心位置(ｅx，ｅy，ｅz)の値に関係なく、第２軸２と第３軸３の結合部に重力トルクとしてのみかかってくる。よって、先端の重量Ｍgは、
Ｍg＝(Ｔg2−Ｔg2 ini)／Ｌ2 ・・・(５)
の計算式のみで求まる。Ｌ2は２軸目アーム長である。Ｔg2 iniは予め、先端に負荷がついていないときの第２軸２にかかる重力モーメントを計算もしくは計測しておいた値である。
以上、第２軸２だけを使用して先端負荷７の重量Ｍgが求まつた。
【００１０】
(ＳＴＥＰ５)では、重心位置(ｅx，ｅy，ｅz)を求める。
本実施例では、第五の実施態様で示される、重心位置の推定計算方法の一実施例を説明する。以下、図３のフローチャートを用いて説明する。
(ＳＴＥＰ５−１) まず、第６軸６の回転軸方向が鉛直方向でないことを確認する。もし、鉛直方向を向いていた場合は、適当な角度まで(実施例では４５°以上)第４軸４と第５軸５を動作させ調整する。
(ＳＴＥＰ５−２) 次に、第６軸６を正方向に９０°以上、低速で動作させる。低速で動作させるのは、加減速時のトルクを抑えるためである。このときのモータトルク指令値を監視し、ピーク値(最大または最小値)をとる時の第６軸６座標のＸ軸からのなす角θp(この方向に負荷の重心がある)を記憶しておく。(図６)(ＳＴＥＰ５−３) 次に、第６軸６を動作させ第５軸５の回転方向に負荷の重心位置がくるようにする。ここで第５軸５を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θg5を求める。(図７)
(ＳＴＥＰ５−４) 次に、第５軸５を動作させ、第４軸４と第５軸５が一直線上にのるようにする。ここで第３軸３を低速で９０°以上動作させ、モータトルク指令値がピーク値をとる時のモータ位置から、θg3を求める。(図８)
【００１１】
先端負荷７の重心(ｅx，ｅy，ｅz)は、第３軸３のアーム長Ｌ3、第５軸アーム長Ｌ5、θp、θg3、θg5を使って図８の幾何学問題を解くことによって、以下のように求めることができる。
図８のＬ6について、２つの式が成り立つ。
Ｌ6＝(Ｌ3＋Ｌ5＋ｅz)＊tan(θg3) ・・・(６)
Ｌ6＝(Ｌ5＋ｅz)＊tan(θg5) ・・・(７)
(６)(７)式をｅzについて解くと、(８)式のようになる。
ｅz＝｛(Ｌ3＋Ｌ5)＊tan(θg3)−Ｌ5＊tan(θg5)｝
／｛tan(θg5)−tan(θg3)｝ ・ ・・(８)
また、ｅx，ｅyはＬ6とθpから、以下のように求めることができる。
ｅx＝Ｌ6＊cos(θp)＝｛(Ｌ5＋ｅz)＊tan(θg5)｝＊cos(θp) ・・・(９)
ｅy＝Ｌ6＊sin(θp)＝｛(Ｌ5＋ｅz)＊tan(θg5)｝＊sin(θp)・・・(１０)
以上が、先端負荷７の重心位置を推定計算する方法である。
【００１２】
また、本実施例では、(ｅx，ｅy，ｅz)の推定計算方法として、すべてを幾何学的に求めたが、θpとθg5を算出した後、第４軸４または、第５軸５を動作させ、その時のモータトルク指令値Ｔg4と、先で算出した先端負荷７の重量Ｍgからモーメント長Ｌg4(図７)を求め、
Ｌg4＝Ｔg4／Ｍg ・・・(１１)
それらを使って、重心位置(ｅx，ｅy，ｅz)を求めることも可能である。以下に計算結果のみを示す。
ｅz＝Ｌg4＊cos(θg5)−Ｌ5 ・・・(１２)
ｅx＝｛Ｌg4＊sin(θg5)｝＊cos(θp) ・・・(１３)
ｅy＝｛Ｌg4＊sin(θg5)｝＊sin(θp) ・・・(１４)
この場合も、従来の方法と違い、第４軸、５軸、６軸の各軸モータトルク指令値の誤差が蓄積されないので、精度良く重心位置を求めることができる。
【００１３】
最後に第四の実施態様の方法について説明する。
ロボットに動作を教示し、実行するためには動作プログラムが作成される。動作プログラムは、特定のプログラム言語によって記述されている。第四の実施形態の方法は、そのプログラム言語の命令の一つとして、前記ＳＴＥＰ１からＳＴＥＰ５までの処理を実行させる命令を持っておき、その命令の入ったプログラムを実行するだけで、上記各軸動作ならびに各ＳＴＥＰでの一連の処理を自動的に実行するものである。
【００１４】
【発明の効果】
以上述べたように、この発明によれば、平行リンク機構を有する軸の回転中心を含む軸のモータトルク指令値を使用して、先端負荷の重心位置に関係なく、負荷重量の値だけを算出できるという、平行リンク機構の特性を有効に利用し、先端の負荷の重量を計算するため、計算が非常に簡単であり、各軸の誤差が蓄積されることが無く、結果として、精度良く先端負荷重量Ｍgを算出することができるという効果がある。
また、負荷重心位置の計算にモータトルク指令値のピーク時のアーム位置から幾何学的に解く方法を採用すれば、モータトルク指令値や先に計算した負荷の重量Ｍｇに誤差を含む場合も、それらの誤差の影響を受けずに、精度良く重心位置を推定計算することができるという効果がある。
また、第四の実施態様の方法を使用すれば、オペレータはロボットアーム先端に負荷を取り付け、プログラムを実行するだけで、あとはすべて制御のソフト上で自動的に動作および計算をするため、負荷が変わった場合も簡単に精度良く負荷パラメータを推定でき、オペレータの労力と作業時間を最小にすることができるという効果がある。
【図面の簡単な説明】
【図１】 この発明の方法を実施するためのロボット制御装置の構成を示すブロック図である。
【図２】 この発明の方法を示すフローチャートである。
【図３】 この発明の負荷重心位置の推定計算方法を示すフローチャートである。
【図４】 重力分トルクの値を求める式の説明図である。
【図５】 この発明の実施例を示すロボットの構成図である。
【図６】 この発明の実施例のＳＴＥＰ５−２を説明する図である。
【図７】 この発明の実施例のＳＴＥＰ５−３を説明する図である。
【図８】 この発明の実施例のＳＴＥＰ５−４を説明する図である。
【符号の説明】
１ 第１軸、２ 第２軸、３ 第３軸、４ 第４軸、５ 第５軸、６ 第６軸、７ 負荷、３０ ロボット制御装置、３１ プロセッサボード、３１ａ プロセッサ、３１ｂ ＲＯＭ、３１ｃ ＲＡＭ、３２ ディジタルサーボ制御回路、３３ サーボアンプ、３４ シリアルポート、３５ａ、３５ｂ 不揮発性メモリ、３６ Ｉ／Ｏ、３７ バス、５１〜５６ サーボモータ、５７ 教示操作盤、５８ ＲＳ２３２Ｃ機器、１００ ロボット
Claims (1)
- 平行リンク機構を備えた６軸ロボットのアーム先端軸に取り付けた負荷の重心位置を計算するステップを含むロボットの負荷パラメータ推定方法において、
前記負荷の重心位置を計算するステップは、
前記先端軸のモータトルク値がピークとなるときの前記先端軸の位置を求めるステップと、
前記先端軸の位置および前記アームの長さに基づいて前記負荷の重心位置を求めるステップと、を含むことを特徴とするロボットの負荷パラメータ推定方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP21097197A JP3835641B2 (ja) | 1997-08-05 | 1997-08-05 | ロボットの負荷パラメータ推定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP21097197A JP3835641B2 (ja) | 1997-08-05 | 1997-08-05 | ロボットの負荷パラメータ推定方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH1148181A JPH1148181A (ja) | 1999-02-23 |
| JP3835641B2 true JP3835641B2 (ja) | 2006-10-18 |
Family
ID=16598169
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP21097197A Expired - Fee Related JP3835641B2 (ja) | 1997-08-05 | 1997-08-05 | ロボットの負荷パラメータ推定方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3835641B2 (ja) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5245840B2 (ja) * | 2009-01-09 | 2013-07-24 | トヨタ自動車株式会社 | 移乗装置、バランス状態評価装置、バランス状態評価方法、及びプログラム |
| JP5360254B2 (ja) * | 2012-03-21 | 2013-12-04 | トヨタ自動車株式会社 | トルク検出方法及びアーム装置 |
| WO2018109968A1 (ja) * | 2016-12-16 | 2018-06-21 | パナソニックＩｐマネジメント株式会社 | ロボットの制御方法 |
-
1997
- 1997-08-05 JP JP21097197A patent/JP3835641B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH1148181A (ja) | 1999-02-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4759660B2 (ja) | ロボットアーム制御用の装置、方法、プログラム及び集積電子回路、並びに、組立ロボット | |
| US7912584B2 (en) | Power consumption estimation apparatus | |
| JP6174654B2 (ja) | センサの位置と向きを算出する機能を備えたロボットシステム | |
| CN109849025B (zh) | 振动抑制装置 | |
| JP2019166626A (ja) | 制御装置及び機械学習装置 | |
| US20070030271A1 (en) | Robot controller and robot control method using spline interpolation | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| US10406689B2 (en) | Robot simulation apparatus that calculates swept space | |
| JP6248544B2 (ja) | ロボット、制御装置、ロボットシステム | |
| JP2017124455A (ja) | ロボット装置、ロボット制御方法、プログラム及び記録媒体 | |
| JP3766484B2 (ja) | 多関節ロボットの負荷重量及び負荷重心位置の自動算出方法 | |
| JP3349652B2 (ja) | オフラインティーチング方法 | |
| EP1886771B1 (en) | Rotation center point calculating method, rotation axis calculating method, program creating method, operation method, and robot apparatus | |
| JP3927454B2 (ja) | 多関節ロボットの負荷重量及び負荷重心位置の自動算出方法 | |
| JP3698770B2 (ja) | 負荷重量の推定方法 | |
| JP3835641B2 (ja) | ロボットの負荷パラメータ推定方法 | |
| JP4016305B2 (ja) | ロボットの制御装置 | |
| JP2004348250A (ja) | ロボット制御装置 | |
| JP3577124B2 (ja) | 力制御ロボットを用いて嵌合データを取得する方法 | |
| JPH09123077A (ja) | ロボットの剛性同定方法及びその装置 | |
| JP5708091B2 (ja) | ロボットの制御方法およびロボットの制御装置 | |
| CN114800523B (zh) | 机械臂轨迹修正方法、系统、计算机及可读存储介质 | |
| JP7250940B2 (ja) | 共通システムの複数の可動モジュールの位置及び配向を状態推定するための方法 | |
| JPH08190433A (ja) | 負荷重量推定方法 | |
| JPH06332535A (ja) | ロボットの制御装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20030909 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20051215 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20051226 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060217 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20060519 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060614 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20060707 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20060720 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090804 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100804 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110804 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120804 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130804 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140804 Year of fee payment: 8 |
|
| LAPS | Cancellation because of no payment of annual fees |