(57)【要約】【課題】測定したフィードバック電流値の測定誤差の影響を受けることなく、多関節ロボットのアーム先端の負荷の重量及び重心位置を自動的に算出する方法を提供する。【解決手段】第１段階では、多関節ロボット１のアーム先端に重量及び重心位置が既知である負荷２を取り付け、動作パターンプログラムに従って多関節ロボット１を動作させ、このとき検出された駆動電流と、既知である負荷２の重量及び重心位置と質点モデルとから算出した理想電流とから、検出された駆動電流を補正するための補正係数を算出する。第２段階では、多関節ロボット１のアーム先端に重量及び重心位置が未知である負荷２を取り付け、動作パターンプログラムに従って多関節ロボット１を動作させ、このとき検出された駆動電流を前述した補正係数により補正することにより測定誤差を排除し、この補正された駆動電流に基づいて未知である負荷２の重量及び重心位置を算出する。【選択図】　図１
2004025387.tif 000002
【特許請求の範囲】
【請求項１】
サーボモータによって駆動される複数の駆動軸を備えた多関節ロボットのアーム先端に位置する負荷の重量及び重心位置を算出する方法において、
多関節ロボットのアーム先端に重量及び重心位置が既知である負荷を取り付け（ステップ２１）、
教示装置により基準姿勢を教示し（ステップ２２）、
該基準姿勢に基づいて動作パターンプログラムが作成されるようにし（ステップ２３）、
該動作パターンプログラムに従って多関節ロボットを動作させ（ステップ２４）、
該動作中に各駆動軸を駆動するサーボモータの駆動電流を予め設定されたスキャンタイム毎に検出し、各駆動軸のそれぞれについて前記検出された駆動電流の平均値を算出し（ステップ２５）、
各駆動軸のそれぞれについて前記既知である負荷の重量及び重心位置と質点モデルとから理想電流を算出し、該理想電流と前記駆動電流の平均値との比から補正係数を算出し（ステップ２６）、
多関節ロボットのアーム先端に重量及び重心位置が未知である負荷を取り付け（ステップ１１）、
教示装置により基準姿勢を教示し（ステップ１２）、
該ステップ１２において教示された基準姿勢に基づいて動作パターンプログラムが作成されるようにし（ステップ１３）、
該ステップ１３において作成された動作パターンプログラムに従って多関節ロボットを動作させ（ステップ１４）、
該ステップ１４における動作中に各駆動軸を駆動するサーボモータの駆動電流を予め設定されたスキャンタイム毎に検出し、各駆動軸のそれぞれについて検出された駆動電流の平均値を算出し（ステップ１５）、
該ステップ１５において算出した駆動電流の平均値を前記ステップ２６において算出した補正係数により補正し（ステップ１６）、
各駆動軸のそれぞれについて、前記ステップ１６において補正された駆動電流の平均値に基づいて前記ステップ１４における動作パターンプログラムの実行中に各駆動軸にかかったトルクの平均値を算出し、該各駆動軸にかかったトルクの平均値は各駆動軸にかかるアンバランストルクと等価であるとすることによりアンバランストルクに未知数として含まれる負荷の重量及び重心位置を算出する（ステップ１７）ようにしたことを特徴とする多関節ロボットの負荷重量及び負荷重心位置の自動算出方法。
【請求項２】
前記教示装置により教示された基準姿勢を記憶装置に記憶させるようにしたことを特徴とする請求項１に記載の多関節ロボットの負荷重量及び負荷重心位置の自動算出方法。
【請求項３】
前記基準姿勢を複数個教示し、これにより前記動作パターンプログラムを複数個作成するようにしたことを特徴とする請求項１または２に記載の多関節ロボットの負荷重量及び負荷重心位置の自動算出方法。
【請求項４】
前記動作パターンプログラムでは、重力に反して負荷を持ち上げる動作即ち上昇動作と、該上昇動作と同一の経路について負荷を持ち下げる動作即ち下降動作とを行わせるようにしたことを特徴とする請求項１乃至３のいずれかに記載の多関節ロボットの負荷重量及び負荷重心位置の自動算出方法。
【請求項５】
前記動作パターンプログラムでは、各駆動軸を個別に動作させるようにしたことを特徴とする請求項１乃至４のいずれかに記載の多関節ロボットの負荷重量及び負荷重心位置の自動算出方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
サーボモータにより駆動し、その姿勢を記憶、再生できる多関節ロボットにおいて、アーム先端に取り付けられたツール等の負荷の重量及び重心位置を算出する方法に関する。
【０００２】
【従来の技術】
多関節ロボットの動作制御では、アーム先端に位置する負荷の重量及び重心位置の影響を考慮する必要があることは広く知られている。具体的に言うと、多関節ロボットの動作制御のうち、駆動軸に使用されている減速機にかかるトルクを制御する加減速制御においては、ロボット自身の質点情報とともにアーム先端に位置する負荷の質点情報すなわち重量及び重心位置のパラメータが必要不可欠である。例えば、溶接ロボットの加減速制御においては、アーム先端に取り付けられた溶接ガンの重量及び重心位置を予め設定しておく必要がある。また、ハンドリングロボットの加減速制御においては、アーム先端に取り付けられた把持部材単独の重量及び重心位置と、把持部材により被搬送物を把持した状態におけるアーム先端の負荷全体の重量及び重心位置とを予め設定しておく必要がある。そして、被搬送物を把持しない状態における加減速制御においては前者の重量及び重心位置を考慮し、一方、被搬送物を把持した状態における加減速制御においては後者の重量及び重心位置を考慮し制御するようにしている。
【０００３】
さらに、グラインダがけやバリ取りのような切削加工ロボットでは、加減速制御の他に力制御においても、切削ツールが被加工物から受ける切削反力を算出する際に、切削ツール自身の重量及び重心位置を基に、切削ツール自身が受ける重力等の影響を算出しこれを排除するようにしている。このようにアーム先端に位置する負荷の重量及び重心位置は、多くの多関節ロボットの動作制御において重要な要素となっており、これを正確に算出することは動作制御を高精度に行う上では必須の条件となっている。
【０００４】
ハンドリングロボットにおける負荷の重量及び重心位置を算出する方法は、特開平３−５５１８９号公報等に開示されている。この特開平３−５５１８９号公報には、ロボットのアーム先端に力センサを介して把持部材を取り付け、その把持部材に被搬送物を持たせた状態で、予め設定された手順に従ってロボットアームの姿勢をロボット制御装置からの指令値によって種々変化させ、ロボットアームの各姿勢毎の姿勢データと力センサからの出力値とを測定し、姿勢データを基に力センサ自身、把持部材、及び被搬送物等のアーム先端の負荷の重量及びその重心位置を算出する方法が開示されている。
【０００５】
また、切削加工ロボットにおける負荷としての切削ツールの重量及び重心位置を算出する方法は、特開平６−３１５８８２号公報等に開示されている。この特開平６−３１５８８２号公報においても前述の特開平３−５５１８９号と同様に、ロボットアームに複数の姿勢をとらせ、各姿勢における姿勢データと力センサからの出力値を基にアーム先端の負荷すなわち切削ツールの重量及びその重心位置を算出する方法が開示されている。ただし、力センサにおいて検出される力データは、切削ツールが力作業を行っていないとき即ち切削加工を行っていないときには切削ツール自身の重量であり、被加工物に接触して力作業を行っているとき即ち切削加工を行っているときには被加工物から切削ツールに加わる力と切削ツール自身の重量とが複合されたものであり、この点がハンドリングロボットにはない切削加工ロボット特有のものとなっている。
【０００６】
しかし、これらの従来技術はいずれも、負荷の重量及び重心位置の算出過程において、アーム先端部に取り付けた力センサからの出力値を必要としていた。切削加工ロボットにおいて行われる力制御においては、切削加工時に切削ツールにかかる切削反力を測定するために、一般に力センサの取り付けは必要不可欠なものとなっているが、ハンドリングロボットや溶接ロボットにおいて行われる加減速制御においては、負荷の重量及び重心位置の算出過程以外では一般に力センサは必要ではない。負荷の重量及び重心位置の算出時期は、ハンドリングロボットにおいては把持部材や被搬送物が変更になったときのみでよく、使用形態にもよるが一般にその頻度は少ない場合が多く、また溶接ロボットにいたっては負荷である溶接ガンを交換することはほとんどないことを考慮すると、初期導入時やメンテナンス終了時のみでよいことになり、負荷の重量及び重心位置の算出のみのために力センサ及びその周辺機器を設置することはコストの面で問題があった。
【０００７】
そこで、係る問題を解決するものとして、特開平９−９１００４号公報や特開平１０−１３８１８７号公報においては、力センサを使用することなく多関節ロボットのアーム先端の負荷の重量及び重心位置を自動的に算出する方法が開示されている。すなわち、これらの公報に開示されている方法では、ロボットを制御しているサーボモータの電流フィードバックを基に各軸にかかるトルクを推定し、このトルク推定値からロボットのリンクパラメータを求めた上で負荷重量を推定するようにしている。さらに、特開平９−９１００４号では、負荷重量の推定精度を向上させるために、ロボットに複数の姿勢をとらせ、各姿勢において得られたデータを基に複数の条件式を連立させるようにしている。また、特開平１０−１３８１８７号では、ロボットに複数の姿勢をとらせる点は特開平９−９１００４号と同様であるが、減速機の効率や摺動抵抗による誤差を補償するために、ロボットを低速で移動させ、この間の平均電流を基に各軸にかかるトルクを推定するようにしている。
【０００８】
【発明が解決しようとする課題】
しかし、サーボモータのフィードバック電流値を測定する前述した特開平９−９１００４号公報や特開平１０−１３８１８７号公報に開示されている方法では、測定したフィードバック電流値の測定誤差が、そのまま負荷の重量及び重心位置の推定誤差となって現れてくるという問題がある。また、特開平１０−１３８１８７号では、前述したように減速機の効率や摺動抵抗による誤差を補償するようにしているが、係る手段を講じても測定される絶対的な誤差は避けられなかった。
【０００９】
本発明は、前述した従来技術の課題を解決するためになされたものであり、測定したフィードバック電流値の測定誤差の影響を受けることなく、多関節ロボットのアーム先端に位置する負荷の重量及び重心位置を自動的に算出する方法を提供することを目的とする。
【００１０】
【課題を解決するための手段】
前述の課題を解決するために、本発明では、サーボモータによって駆動される複数の駆動軸を備えた多関節ロボットのアーム先端に位置する負荷の重量及び重心位置を算出する方法において、負荷重量及び負荷重心位置の算出過程を以下の２つの段階に分けることにした。すなわち、第１段階においては、多関節ロボットのアーム先端に重量及び重心位置が既知である負荷を取り付け、動作パターンプログラムに従って多関節ロボットを動作させ、このとき検出された駆動電流と、既知である負荷の重量及び重心位置と質点モデルとから算出した理想電流とから、検出された駆動電流を補正するための補正係数を算出する。次いで、第２段階においては、多関節ロボットのアーム先端に重量及び重心位置が未知である負荷を取り付け、動作パターンプログラムに従って多関節ロボットを動作させ、このとき検出された駆動電流を前述した補正係数により補正し、この補正された駆動電流に基づいて動作パターンプログラムの実行中に各駆動軸にかかったトルクを算出し、この各駆動軸にかかったトルクは各駆動軸にかかるアンバランストルクと等価であるとすることにより、アンバランストルクに未知数として含まれる負荷の重量及び重心位置を算出するようにした。
【００１１】
すなわち、請求項１にかかる発明では、前述した第１段階の過程として、多関節ロボットのアーム先端に重量及び重心位置が既知である負荷を取り付け（ステップ２１）、教示装置により基準姿勢を教示し（ステップ２２）、この基準姿勢に基づいて動作パターンプログラムが作成されるようにし（ステップ２３）、この動作パターンプログラムに従って多関節ロボットを動作させ（ステップ２４）、この動作中に各駆動軸を駆動するサーボモータの駆動電流を予め設定されたスキャンタイム毎に検出し、各駆動軸のそれぞれについて前記検出された駆動電流の平均値を算出し（ステップ２５）、各駆動軸のそれぞれについて前記既知である負荷の重量及び重心位置と質点モデルとから理想電流を算出し、この理想電流と前記駆動電流の平均値との比から補正係数を算出する（ステップ２６）。
【００１２】
次いで、前述した第２段階の過程として、多関節ロボットのアーム先端に重量及び重心位置が未知である負荷を取り付け（ステップ１１）、教示装置により基準姿勢を教示し（ステップ１２）、この基準姿勢に基づいて動作パターンプログラムが作成されるようにし（ステップ１３）、この動作パターンプログラムに従って多関節ロボットを動作させ（ステップ１４）、この動作中に各駆動軸を駆動するサーボモータの駆動電流を予め設定されたスキャンタイム毎に検出し、各駆動軸のそれぞれについて検出された駆動電流の平均値を算出し（ステップ１５）、このステップ１５において算出した駆動電流の平均値を前記ステップ２６において算出した補正係数により補正し（ステップ１６）、各駆動軸のそれぞれについて、前記ステップ１６において補正された駆動電流の平均値に基づいて前記ステップ１４における動作パターンプログラムの実行中に各駆動軸にかかったトルクの平均値を算出し、この各駆動軸にかかったトルクの平均値は各駆動軸にかかるアンバランストルクと等価であるとすることにより、アンバランストルクに未知数として含まれる負荷の重量及び重心位置を算出するようにした（ステップ１７）。なお、本課題を解決するための手段の欄及び前記特許請求の範囲の欄に記載のステップ番号は、図１に記載のステップ番号と同一である。
【００１３】
かかる構成としたことにより、重量及び重心位置が既知である負荷を使用することにより測定したフィードバック電流値の測定誤差を推定し、これを補正係数として重量及び重心位置が未知である負荷における重量及び重心位置の算出に利用することにしたので、測定したフィードバック電流値の測定誤差の影響が排除されることになる。
【００１４】
ところで、前述の構成は以下に述べる理論に基づいている。理論の根幹は、駆動軸に実際にかかるトルクすなわちモータ駆動トルクはロボットの質点モデルから力学的に求められるトルクすなわち負荷トルクと等価であるとするものである。すなわち、アーム先端に負荷を取り付けた状態でロボットを動作させたときの各駆動軸モータに流れる駆動電流を測定し、この駆動電流よりモータ駆動トルクを求める。一方、ロボットを質点モデルとして考えたとき、この質点モデルに負荷の重量及び重心位置のパラメータを含ませるようにすれば、負荷トルクはこのロボットの質点モデルから力学的に求めることができる。ここで、モータ駆動トルクと負荷トルクが等価であると仮定すれば、ロボットの質点モデルに未知数として含まれている負荷の重量及び重心位置を計算により算出することができる。
【００１５】
さらに詳述すれば、モータ駆動トルクを算出する際に必要となる駆動電流データは、駆動軸モータであるサーボモータとこれを制御するサーボアンプとの間に電流検出器を介在させるなどすることにより容易に得ることができる。しかし、負荷トルクを算出する際に必要となる駆動軸の角度、角速度、及び角加速度の各データについて考えると、このうち角度データについては駆動軸に付属の角度検出器により容易に得ることができるが、角速度データ及び角加速度データについては計算処理により算出する必要があり、これを算出するようにすると計算処理が煩雑になり、ロボット制御装置に多大の負担をかけることになるという問題が生ずる。
【００１６】
そこで、計算処理を軽減するために、本発明では、駆動軸を低速かつ一定速度にて動作させる分については、ロボットの質点モデルのうち、角速度データに起因する遠心力及びコリオリ力によるトルクの和は極めて小さく、また角加速度データに起因するイナーシャによるトルクは無視してもよいことに着目し、動作パターンプログラムの実行においては駆動軸を低速かつ一定速度にて動作させ、これによりロボットの質点モデルから求められる負荷トルクについては角度データに起因するアンバランストルクのみを考慮することとした。さらに、モータ駆動トルクを算出する際に必要となる電流データは、動作パターンプログラムの実行中にスキャンタイム毎に検出される電流値の平均値を使用し、また、負荷トルク（アンバランストルク）を算出する際に必要となる角度データについては、動作パターンプログラムの実行中に変化した軸角度の平均値を使用することとしたので、負荷の重量及び重心位置の算出処理は動作パターンプログラムの実行後に１回だけ行えば済むことになる。
【００１７】
なお、この計算処理においては負荷の重量及び重心位置を未知数とする連立方程式を解くことになるが、これらの未知数を精度良く算出するために、好ましくは実行中に大きなアンバランストルクが駆動軸にかかるように動作パターンプログラムが作成されるようにする。具体的には、負荷の動作において重力の影響を大きく受けるような、すなわち負荷が上下に揺動するような動作を行わせることにより、駆動軸に大きなアンバランストルクがかかるようにする必要があり、このためにはロボットの基準姿勢を良好な位置に選定する必要がある。
【００１８】
請求項２にかかる発明では、請求項１にかかる発明において、教示装置により教示された基準姿勢を記憶装置に記憶させるようにした。前述のように、ロボットの基準姿勢の選定に際しては、動作パターンプログラムの実行中に大きなアンバランストルクが駆動軸にかかるように注意しなければならないが、この選定作業はロボット技術に精通した作業者であれば比較的容易であるが、ロボット技術に精通しない多くの作業者にとっては容易なことではない。また、基準姿勢の設定に際しては、作業者が教示装置を逐一操作してロボットの各駆動軸を位置決めさせる必要があり、溶接ロボットのように負荷である溶接ガンの交換がほとんどないものについてはともかく、ハンドリングロボットのように負荷である被搬送物が頻繁に変更されるものについては、負荷が変更になる度毎にこの作業を行うのは大変煩雑となる。そこで、教示装置により教示された基準姿勢のデータは制御装置内の記憶装置に記憶するようにしておき、負荷の重量及び重心位置の算出の際には記憶された基準姿勢のデータが記憶装置より呼び出され、このデータに基づき動作パターンプログラムが作成されるようにした。
【００１９】
請求項３にかかる発明では、請求項１または２にかかる発明において、基準姿勢を複数個教示し、これにより動作パターンプログラムが複数個作成されるようにした。負荷の重量及び重心位置算出する上記の計算処理においては、その未知数は負荷の重量ｍ及び負荷の重心位置即ち負荷の重心座標（ｘ，ｙ，ｚ）の計４つであり、これら４つの未知数を算出するためには連立方程式は最低でも４元必要となる。駆動軸が４つ以上の場合は１つの基準姿勢によって作成される１つの動作パターンプログラムを実行させるだけで４元以上の連立方程式を作成することができるが、駆動軸が３つ以下の場合は１つの動作パターンプログラムの実行のみでは４元以上の連立方程式は作成できない。
【００２０】
これに関連して、請求項３にかかる発明でも前述の請求項１にかかる発明と同様に、ロボットの質点モデルは角度データに起因するアンバランストルクのみを考慮することとしているが、このアンバランストルクは多関節ロボットを構成する全ての駆動軸に関与されるわけではない。例えば、図３に示す６軸の垂直多関節ロボットにおいては、上腕の揺動軸であるＶ軸、手首部分の揺動軸であるＢ軸、及び負荷が接続されるアーム先端のＲ１軸の合計３軸がアンバランストルクに大きく関与することになるが、その他の駆動軸はアンバランストルクにほとんど関与しない。したがって、この場合、１回の動作パターンプログラムの実施により得られる連立方程式の有効な元数は３つしかなく、最低必要となる４つには足りないことになる。
【００２１】
そこで、アンバランストルクの算出に有効な駆動軸が３つ以下の場合においては、基準姿勢を複数個教示し、各基準姿勢に対応して動作パターンプログラムが複数個作成されるようにしこれを実行するようにし、これにより４元以上の連立方程式が作成されるようにした。また、有効な駆動軸が４つ以上の場合においても、基準姿勢を複数個教示し、各基準姿勢に対応して動作パターンプログラムが複数個作成されるようにしこれを実行するようにすれば、多くの元数を有する連立方程式が作成されることとなり、これを解くことにより未知数である負荷の重量及び重心位置を高精度に算出することができるようになる。
【００２２】
請求項４にかかる発明では、請求項１乃至３のいずれかにかかる発明において、動作パターンプログラムでは、重力に反して負荷を持ち上げる動作即ち上昇動作と、この上昇動作と同一の経路について負荷を持ち下げる動作即ち下降動作とを行わせるようにした。一般に、サーボ系より取り込まれるサーボモータの駆動電流には、減速機の効率及び摺動抵抗による誤差が含まれている。そのため、負荷の重量及び重心位置を高精度に算出することを考えた場合、駆動電流よりモータ駆動トルクを算出する際は、減速機の効率及び摺動抵抗による誤差を補償することが好ましいことになる。しかし、減速機の効率及び摺動抵抗は駆動軸個々に固有な値であるとともにロボットの動作形態によっても異なる値をとる数値であるので、これを一義的に設定することは一般に困難である。
【００２３】
そこで、重力に反して負荷を持ち上げる動作即ち上昇動作と、この上昇動作と同一の経路について負荷を持ち下げる動作即ち下降動作とでは、減速機の効率及び摺動抵抗によるトルクのロスの値は正負が逆となるだけで絶対値は等しくなるということに着目し、モータ駆動トルクと負荷トルクとの関係式において、上昇動作により定義付けられる関係式と下降動作により定義付けられる関係式の２つを加え合わせることにより、減速機の効率及び摺動抵抗によるトルクのロスを相殺した関係式を導出するようにした。この導出された関係式を適用することにより、減速機の効率及び摺動抵抗の誤差の影響を受けることなく、負荷の重量及び重心位置を高精度に算出することができるようになる。
【００２４】
請求項５にかかる発明では、請求項１乃至４のいずれかにかかる発明において、動作パターンプログラムでは各駆動軸を個別に動作させるようにした。多関節ロボットにおいて、１つの駆動軸の動作が他の駆動軸にかかるトルクに影響を与えることはよく知られるところである。前述のように、本発明では、質点モデルにおいてアンバランストルクのみを考慮するようにしているために、動作パターンプログラムの実行中は駆動軸を低速かつ一定速度にて動作させるようにしており、１つの駆動軸の動作が他の駆動軸のトルクに与える影響は通常の動作に比してかなり少ないと言える。しかし、より高精度に負荷の重量及び重心位置を算出するためには、１つの駆動軸の動作が他の駆動軸のトルクに与える影響を完全に排除することが好ましく、そのために動作パターンプログラムでは各駆動軸を個別に動作させるようにした。
【００２５】
【発明の実施の形態】
以下、本発明の一実施形態について図面を参照して説明する。図２は、本発明が適用される多関節ロボットの制御装置の一例を示したものである。教示装置としての軸操作装置１０は、作業者がこれを使用してロボット１を操作するためのティーチングデータを教示する装置である。指令値生成装置５は、軸操作装置１０を介して作業者が与えたティーチングデータや記憶装置６に記憶されている動作プログラムを入力し、ロボット１の各駆動軸を動作させるための指令値を作成し、これをサーボアンプ４へ出力する。また、ここで作成された指令値は記憶装置６へ転送し一時的に記憶される。サーボアンプ４は指令値生成装置５からの指令値を入力し、ロボット１の各駆動軸毎に設けられた駆動軸モータであるサーボモータ３に対して電流指令を出力する。この電流指令によって各駆動軸が所定の角度に設定されることにより、ロボット１のアーム先端の負荷２を位置決めすることができる。
【００２６】
記憶装置６には、動作プログラムなどのプログラムデータ、及び角度データやティーチングデータ等の数値データが記憶される。プログラム作成装置７では、記憶装置６に記憶されているティーチングデータを基に、各駆動軸にかかるトルクすなわちモータ駆動トルクを算出するために必要な電流データを採取するための動作パターンプログラムを自動的に作成し、これを記憶装置６へ転送する。この動作パターンプログラムの自動作成作業は、記憶装置６に予め入力されている動作パターンプログラムの自動作成プログラムにより自動的に行われる。電流取り込み装置９では、サーボアンプ４とサーボモータ３との間に設置された電流検出器９ａにおいて検出された電流データ（駆動電流）を取り込み、これを重量・重心演算装置８へ転送する。重量・重心演算装置８では、電流取り込み装置９から入力された電流データに基づいて、負荷の重量及び重心位置を算出する。
【００２７】
ここで、検出したサーボモータの駆動電流を補正するための補正係数の算出方法について説明する。電流検出器９ａにより検出された任意の駆動軸ｊの駆動電流をＩｊ　とすると、このとき駆動軸ｊに実際にかかるトルクすなわちモータ駆動トルクＴＲｊは式（１）で表される。
【００２８】
【数１】
2004025387.tif 000003
【００２９】
式（１）において、Ｒｊ　は駆動軸ｊの減速比、Ｋｔｊ　は駆動軸ｊのトルク定数であり、双方とも駆動軸に固有な定数である。一方、ロボットの質点モデルから力学的に求められるトルクすなわち負荷トルクＴＭｊは式（２）で表される。
【００３０】
【数２】
2004025387.tif 000004
【００３１】
式（２）において、ｍは負荷の重量、ｒ（ｘ，ｙ，ｚ）は負荷の重心位置、θｊ　は軸ｊの軸角度、ωｊ　は軸ｊの角速度、αｊ　は軸ｊの角加速度、Ｊθ，ｊ　は軸角度θｊ　のとき軸ｊにおけるイナーシャを表す関数（加速度項）、Ｐθ，ｊ　は軸角度θｊ　のとき軸ｊにおけるアンバランストルクを表す関数（重力項）、Ｃθ，ｊ　は軸角度θｊ　のとき軸ｊにおける遠心力およびコリオリ力によるトルクの和を表す関数（速度項）をそれぞれ示す。なお、負荷の重心位置（ｘ，ｙ，ｚ）は手首座標系における座標であり、ロボットの姿勢によって変化するものではない。
【００３２】
本発明では、式（１）で表されるモータ駆動トルクＴＲｊと式（２）で表される負荷トルクＴＭｊとは理論的には等価であると考えることにする。ここで、ロボット１には重量及び重心位置が既知である負荷２が取り付けられているものとすると、式（３）に示す方程式を解くことにより、既知である負荷２が取り付けられている場合のモータの駆動電流Ｉｊ　を逆算することができる。
【００３３】
【数３】
2004025387.tif 000005
【００３４】
しかし、前述の変数データのうち、各駆動軸の電流データである電流値Ｉｊ　と角度データである軸角度θｊ　はそれぞれ電流検出器９ａ及びエンコーダより容易に得ることができるが、角速度ωｊ　（角速度データ）及び角加速度αｊ　（角加速度データ）については、軸角度θｊ　や移動時間等のデータに基づいて計算処理により算出する必要があり、計算処理が煩雑になる。そこで、本発明では、駆動軸を低速かつ一定速度にて動作させる分については、ロボットの質点モデルのうち、角速度ωｊ　に起因する遠心力及びコリオリ力によるトルクの和Ｃθ，ｊ　は極めて小さく、また角加速度データに起因するイナーシャによるトルクＪθ，ｊ　は無視してもよいことに着目し、動作パターンプログラムの実行においては駆動軸を低速かつ一定速度にて動作させ、これによりロボットの質点モデルは角度データに起因するアンバランストルクＰθ，ｊ　のみを考慮することとし、これにより計算処理が軽減されるようにした。すなわち、本発明では、式（４）に示す方程式を解くことにより、既知である負荷２が取り付けられている場合のモータの駆動電流Ｉｊ　を算出するようにした。
【００３５】
【数４】
2004025387.tif 000006
【００３６】
一方、既知である負荷２が取り付けられている場合の理想電流Ｉｒｊ　は、既知である負荷２の重量及び重心位置と質点モデルとから、式（５）により算出される。
【００３７】
【数５】
2004025387.tif 000007
【００３８】
そして、式（４）から算出した既知である負荷２が取り付けられている場合のモータの駆動電流Ｉｊ　と、式（５）から算出した既知である負荷２が取り付けられている場合の理想電流Ｉｒｊ　とから、検出したサーボモータの駆動電流を補正するための補正係数Ｃｊ　を、式（６）により算出することにする。
【００３９】
【数６】
2004025387.tif 000008
【００４０】
次に、式（６）により算出した補正係数Ｃｊ　を用い、負荷の重量ｍ及び負荷の重心位置（ｘ，ｙ，ｚ）を推定する。ここで、ロボット１には重量及び重心位置が未知である負荷２が取り付けられているものとする。前述したように、本発明では、式（１）で表されるモータ駆動トルクＴＲｊと式（２）で表される負荷トルクＴＭｊとは理論的には等価であると考えることにしているので、前述した式（４）に示す方程式に対して式（６）により算出した補正係数Ｃｊ　を付加した方程式、すなわち式（７）を解くことにより、未知である負荷２が取り付けられている場合のモータの駆動電流Ｉｊ　を算出する。
【００４１】
【数７】
2004025387.tif 000009
【００４２】
具体的には、式（７）においては未知数は負荷の重量ｍ及び負荷の重心位置（ｘ，ｙ，ｚ）の４つであるので、動作パターンプログラムでは、ロボットの駆動軸を４つ以上設定し、各駆動軸のそれぞれについて、軸操作装置１０によりティーチングデータとして与えた基準姿勢から各駆動軸を所定量（所定角度α）動作させ、この動作の実行中にスキャンタイム毎に各駆動軸の駆動電流Ｉｊ　を得る。動作の終了後に、検出された駆動電流Ｉｊ　の積算値をスキャン回数で割ることにより、動作の実行中に検出された駆動電流Ｉｊ　の平均値が得られ、この値より式（７）の左辺であるモータ駆動トルクＴＲｊ（＝Ｒｊ　・Ｋｔｊ　・Ｃｊ　・Ｉｊ　）が求められる。一方、式（７）の右辺であるアンバランストルクＰθ，ｊ　は各駆動軸の軸角度に起因しているが、ここでは基準姿勢における軸角度に移動動作の所定量αの半分の値すなわちα／２を加えた値を、動作の実行中の軸角度の平均値として設定することとした。
【００４３】
これにより得られた式（７）についての４元以上の連立方程式を解くことにより、アンバランストルクＰθ，ｊ　に含まれる負荷の重量ｍ及び負荷の重心位置（ｘ，ｙ，ｚ）を算出する。なお、所定角度αは動作パターンプログラムにおいて予め固定値として設定しておいてもよいし、パラメータの一つとして任意の設定値が与えられるようにしておいてもよい。
【００４４】
ここで、前述した一連の計算処理の手順を、図１に示すフローチャートを参照して説明する。図１において、（ｂ）図は補正係数Ｃｊ　を算出するための手順を示したフローチャートであり、（ａ）図は算出した補正係数Ｃｊ　を用いて負荷重量及び負荷重心位置を算出するための手順を示したフローチャートである。
【００４５】
まず、図１（ｂ）を参照して、補正係数Ｃｊ　を算出するための手順を説明する。
【００４６】
まず、ロボット１に重量及び重心位置が既知である負荷２を取り付ける（ステップ２１）。
【００４７】
次に、軸操作装置１０を操作して、ロボットの初期姿勢としてのロボット１の基準姿勢を教示する（ステップ２２）。教示の際は、各駆動軸毎に軸操作装置１０にある寸動ボタンを操作するなどして、基準姿勢として設定する姿勢をとらせる。この基準姿勢の設定の際に注意すべきことは、動作パターンプログラムの実行中に大きなアンバランストルクが駆動軸にかかるように基準姿勢を設定することである。
【００４８】
すなわち、多関節ロボットは、その構造上、各駆動軸の可動範囲は限定されているので、基準姿勢によっては大きなアンバランストルクが駆動軸にかかるような動作パターンプログラムが作成されないことがある。例えば、図３に示す本実施形態の多関節ロボットの場合、図４に示すように負荷がロボット本体ベース付近にくるように基準姿勢を設定するようにすると、Ｖ軸を駆動させた際、大きなアンバランストルクは作用せず、この結果負荷の重量及び重心位置は高精度に算出されなくなる。したがって、基準姿勢の選定にはこの旨十分注意をはらう必要がある。本実施形態の多関節ロボットにおいては、図５に示す位置に基準姿勢を設定するようにすれば、Ｖ軸、Ｂ軸、及びＲ１軸のいずれにも大きなアンバランストルクが作用する動作パターンプログラムが作成されることになる。
【００４９】
次に、記憶装置６に予め記憶されている動作パターンプログラムの自動作成プログラムにより、基準姿勢のデータを基に動作パターンプログラムが自動的に作成される（ステップ２３）。本実施形態においては、ロボット１を基準姿勢に対して所定量移動させ、移動の間のサーボモータ３の駆動電流を電流検出器９ａにより検出するわけであるから、動作パターンプログラムの自動作成のために必要なデータとしては、ロボット１の基準姿勢、基準姿勢からの移動量、駆動軸の選定、及び移動速度などである。このうちロボット１の基準姿勢についてはステップ２２により設定されるが、残りのデータについてはパラメータとして予め与えておけばよい。
【００５０】
次に、ステップ２３において作成された動作パターンプログラムに従って、ロボット１を動作させる（ステップ２４）。
【００５１】
次に、ステップ２４の実行中における各駆動軸のサーボモータ３の駆動電流を検出する（ステップ２５）。駆動電流の検出はスキャンタイム毎に行い、このスキャンタイム毎に検出された駆動電流は電流取り込み装置９内に一時的に記憶される。そして、この動作パターンプログラムの実行中にスキャンタイム毎に検出された駆動電流について、その平均値を算出する。具体的には、スキャンタイム毎に検出された駆動電流を合計し、これをスキャン回数で割ることにより、駆動電流の平均値を算出する。
【００５２】
最後に、ステップ２５において検出された駆動電流の平均値と、理想電流との比から、補正係数Ｃｊ　を求める（ステップ２６）。
【００５３】
次に、図１（ａ）を参照して、算出した補正係数Ｃｊ　を用いて負荷重量及び負荷重心位置を算出するための手順を説明する。
【００５４】
まず、ロボット１に重量及び重心位置が未知である負荷２を取り付ける（ステップ１１）。
【００５５】
次に、前述したステップ２２と同様に、軸操作装置１０を操作して、ロボットの初期姿勢としてのロボット１の基準姿勢を教示する（ステップ１２）。教示の際は、各駆動軸毎に軸操作装置１０にある寸動ボタンを操作するなどして、基準姿勢として設定する姿勢をとらせる。この基準姿勢の設定の際に注意すべきことは、前述したステップ２２と同様に、動作パターンプログラムの実行中に大きなアンバランストルクが駆動軸にかかるように基準姿勢を設定することである。
【００５６】
次に、前述したステップ２３と同様に、記憶装置６に予め記憶されている動作パターンプログラムの自動作成プログラムにより、基準姿勢のデータを基に動作パターンプログラムが自動的に作成される（ステップ１３）。負荷重量及び負荷重心位置の算出に際しては、ロボット１を基準姿勢に対して所定量移動させ、移動の間のサーボモータ３の駆動電流を電流検出器９ａにより検出するわけであるから、動作パターンプログラムの自動作成のために必要なデータとしては、ロボット１の基準姿勢、基準姿勢からの移動量、駆動軸の選定、及び移動速度などである。このうちロボット１の基準姿勢についてはステップ１２により設定されるが、残りのデータについてはパラメータとして予め与えておけばよい。
【００５７】
次に、前述したステップ２４と同様に、ステップ１３において作成された動作パターンプログラムに従って、ロボット１を動作させる（ステップ１４）。
【００５８】
次に、前述したステップ２５と同様に、ステップ１４の実行中における各駆動軸のサーボモータ３の駆動電流を検出する（ステップ１５）。駆動電流の検出はスキャンタイム毎に行い、このスキャンタイム毎に検出された駆動電流は電流取り込み装置９内に一時的に記憶される。そして、この動作パターンプログラムの実行中にスキャンタイム毎に検出された駆動電流について、その平均値を算出する。具体的には、スキャンタイム毎に検出された駆動電流を合計し、これをスキャン回数で割ることにより、駆動電流の平均値を算出する。
【００５９】
次に、ステップ１５において検出された駆動電流の平均値を、ステップ２６において求めた補正係数Ｃｊ　により補正する（ステップ１６）。具体的には、ステップ１５において検出された駆動電流の平均値に、ステップ２６において求めた補正係数Ｃｊ　を乗ずれば、補正された駆動電流が求まる。
【００６０】
最後に、ステップ１６において補正された各駆動軸のサーボモータの駆動電流に基づいて、式（７）についての連立方程式を解くことにより、負荷の重量及び重心位置を算出する（ステップ１７）。ここで、式（７）の右辺であるアンバランストルクＰθ，ｊ　は各駆動軸の軸角度に起因しているが、ここでは基準姿勢における軸角度に動作の所定量αの半分の値をすなわちα／２を加えた値を、動作の実行中の軸角度の平均値として設定することとした。駆動軸が４つ以上選定されていれば、４元以上の連立方程式ができあがることとなり、この連立方程式を解くことにより、アンバランストルクＰθ，ｊ　に含まれている負荷の重量及び重心位置が算出されることとなる。
【００６１】
ところで、ロボット１の基準姿勢の選定に際しては、動作パターンプログラムの実行中に大きなアンバランストルクが駆動軸にかかるように注意しなければならないが、この選定作業はロボット技術に精通した作業者であれば比較的容易であるが、ロボット技術に精通しない多くの作業者にとっては容易なことではない。また、基準姿勢の設定に際しては、作業者が教示装置を逐一操作してロボット１の各駆動軸を位置決めさせる必要があり、溶接ロボットのように負荷である溶接ガンの交換がほとんどないものについてはともかく、ハンドリングロボットのように負荷である被搬送物が頻繁に変更されるものについては、負荷が変更になる度毎にこの作業を行うのは大変煩雑となる。そこで、教示装置により教示された基準姿勢のデータは制御装置内の記憶装置に記憶するようにしておき、負荷の重量及び重心位置の算出の際には記憶された基準姿勢のデータが記憶装置より呼び出され、このデータに基づき動作パターンプログラムが作成されるようにしてもよい。
【００６２】
また、ロボットの質点モデルは角度データに起因するアンバランストルクのみを考慮することとしているが、このアンバランストルクは多関節ロボットを構成する全ての駆動軸に関与されるわけではない。例えば、図３に示す６軸の垂直多関節ロボットにおいては、上腕の揺動軸であるＶ軸、手首部分の揺動軸であるＢ軸、及び負荷が接続されるアーム先端のＲ１軸の合計３軸がアンバランストルクに大きく関与することになるが、その他の駆動軸はアンバランストルクにほとんど関与しない。したがって、この場合、１回の動作パターンプログラムの実施により得られる連立方程式の有効な元数は３つしかなく、最低必要となる４つには足りないことになる。
【００６３】
そこで、アンバランストルクの算出に有効な駆動軸が３つ以下の場合においては、基準姿勢を複数個教示し、各基準姿勢に対応して動作パターンプログラムが複数個作成されるようにしこれを実行するようにし、これにより４元以上の連立方程式が作成されるようにすればよい。また、有効な駆動軸が４つ以上の場合においても、基準姿勢を複数個教示し、各基準姿勢に対応して動作パターンプログラムが複数個作成されるようにしこれを実行するようにすれば、多くの元数を有する連立方程式が作成されることとなり、これを解くことにより未知数である負荷の重量及び負荷の重心位置を高精度に算出することができるようになる。
【００６４】
ところで、一般に、サーボ系より取り込まれるサーボモータ３の駆動電流には、減速機の効率及び摺動抵抗による誤差が含まれている。そのため、負荷の重量及び重心位置を高精度に算出することを考えると、駆動電流よりモータ駆動トルクを算出する際は、減速機の効率及び摺動抵抗による誤差を補償することが好ましいことになる。しかし、減速機の効率及び摺動抵抗は駆動軸個々に固有な値であるとともにロボット１の動作形態によっても異なる値をとる数値であるので、これを一義的に設定することは一般に困難である。
【００６５】
そこで、重力に反して負荷を持ち上げる動作即ち上昇動作と、この上昇動作と同一の経路について負荷を持ち下げる動作即ち下降動作とでは、減速機の効率及び摺動抵抗によるトルクのロスの値は正負が逆となるだけで絶対値は等しくなるということに着目することにした。すなわち、駆動軸ｊにおける減速機の効率及び摺動抵抗によるトルクのロスをτｊ　とおけば、上昇動作と下降動作におけるモータ駆動トルクと負荷トルクとの関係式はそれぞれ式（８）及び式（９）で表される。
【００６６】
【数８】
2004025387.tif 000010
【００６７】
式（８）におけるＩｕｐｊ　は上昇動作時の駆動電流であり、式（９）におけるＩｄｏｗｎｊ　は下降動作時の駆動電流である。ここで式（８）と式（９）を加え合わせることにより、減速機の効率及び摺動抵抗によるトルクのロスτｊ　が相殺された式（１０）が導き出される。
【００６８】
【数９】
2004025387.tif 000011
【００６９】
減速機の効率及び摺動抵抗の誤差の影響を排除した高精度な算出結果が要求される場合には、式（１０）を適用することにより負荷の重量及び重心位置を算出すればよい。
【００７０】
ところで、多関節ロボットにおいて、１つの駆動軸の動作が他の駆動軸にかかるトルクに影響を与えることはよく知られているところである。前述のように、本発明では、質点モデルにおいてアンバランストルクのみを考慮するようにしているために、動作パターンプログラムの実行中は駆動軸を低速かつ一定速度にて動作させるようにしており、１つの駆動軸の動作が他の駆動軸のトルクに与える影響は通常の動作に比してかなり少ないと言える。しかし、より高精度に負荷の重量及び重心位置を算出するためには、１つの駆動軸の動作が他の駆動軸にかかるトルクに与える影響を完全に排除することが好ましい。これに対処するためには、基準姿勢データに基づいて各駆動軸が個別に動作されるような動作パターンプログラムが作成されるように、動作パターンプログラムを自動作成する動作パターンプログラムの自動作成プログラムをプログラミングすればよい。
【００７１】
【発明の効果】
請求項１にかかる発明によれば、サーボモータによって駆動される複数の駆動軸を備えた多関節ロボットのアーム先端に位置する負荷の重量及び重心位置を算出する方法において、負荷重量及び負荷重心位置の算出過程を以下の２つの段階に分けることにした。すなわち、第１段階において、多関節ロボットのアーム先端に重量及び重心位置が既知である負荷を取り付け、動作パターンプログラムに従って多関節ロボットを動作させ、このとき検出された駆動電流と、既知である負荷の重量及び重心位置と質点モデルとから算出した理想電流とから、検出された駆動電流を補正するための補正係数を算出する。次いで、第２段階において、多関節ロボットのアーム先端に重量及び重心位置が未知である負荷を取り付け、動作パターンプログラムに従って多関節ロボットを動作させ、このとき検出された駆動電流を前述した補正係数により補正し、この補正された駆動電流に基づいて動作パターンプログラムの実行中に各駆動軸にかかったトルクを算出し、この各駆動軸にかかったトルクは各駆動軸にかかるアンバランストルクと等価であるとすることによりアンバランストルクに未知数として含まれる負荷の重量及び重心位置を算出するようにした。
【００７２】
これにより、重量及び重心位置が既知である負荷を使用することにより測定したフィードバック電流値の測定誤差を推定し、これを補正係数として重量及び重心位置が未知である負荷における重量及び重心位置の算出に利用することとなったので、測定したフィードバック電流値の測定誤差の影響が排除され、その結果、多関節ロボットのアーム先端における負荷重量及び負荷重心位置の算出精度が高くなった。
【００７３】
請求項２にかかる発明によれば、請求項１にかかる発明において、教示装置により教示された基準姿勢を記憶装置に記憶させるようにした。そのため、教示装置により教示された基準姿勢のデータは制御装置内の記憶装置に記憶されるようになり、負荷の重量及び重心位置の算出の際には記憶された基準姿勢のデータが記憶装置より呼び出され、このデータに基づき動作パターンプログラムが作成されるようになったので、基準姿勢の設定をその都度行う必要はなくなった。これにより、例えば、基準姿勢のデータ入力はロボット技術に精通したメーカーの担当者が行い、ユーザー側の作業者における基準姿勢のデータ入力を不要とすることも可能となった。
【００７４】
請求項３にかかる発明によれば、請求項１または２にかかる発明において、基準姿勢を複数個教示し、これにより動作パターンプログラムが複数個作成されるようにした。そのため、アンバランストルクの算出に有効な駆動軸が３つ以下の場合においては、基準姿勢を複数個教示することにより、各基準姿勢に対応する動作パターンプログラムが複数個作成されるようにしこれを実行するようにし、これにより４元以上の連立方程式の作成を可能にした。また、有効な駆動軸が４つ以上の場合においても、同様にして多くの元数を有する連立方程式が作成されるようにし、これを解くことにより負荷の重量及び重心位置を高精度に算出することが可能となった。
【００７５】
請求項４にかかる発明によれば、請求項１乃至３のいずれかにかかる発明において、動作パターンプログラムでは、重力に反して負荷を持ち上げる動作即ち上昇動作と、この上昇動作と同一の経路について負荷を持ち下げる動作即ち下降動作とを行わせるようにした。そして、この２つの動作により導出される関係式すなわち上昇動作と下降動作のそれぞれの、駆動電流より算出されるモータ駆動トルクとロボットの質点モデルより算出される負荷トルクとの関係式において、これら２つの関係式を加え合わせることにより、減速機の効率及び摺動抵抗によるトルクのロスを相殺した関係式を導出するようにした。そのため、この導出された関係式を適用することにより、減速機の効率及び摺動抵抗の誤差の影響を受けることなく、負荷の重量及び重心位置を高精度に算出することが可能となった。
【００７６】
請求項５にかかる発明によれば、請求項１乃至４のいずれかにかかる発明において、動作パターンプログラムでは各駆動軸を個別に動作させるようにした。そのため、１つの駆動軸の動作が他の駆動軸のトルクに与える影響を完全に排除することができ、より高精度に負荷の重量及び重心位置を算出することが可能となった。
【図面の簡単な説明】
【図１】本発明の一実施形態における計算処理の手順を示すフローチャートであり、（ｂ）図は補正係数Ｃｊ　を算出するための手順を示したフローチャート、（ａ）図は算出した補正係数Ｃｊ　を用いて負荷重量及び負荷重心位置を算出するための手順を示したフローチャートである。
【図２】本発明が適用される多関節ロボット１の制御装置の一例を示すブロック図である。
【図３】本発明が適用される多関節ロボット１の一例を示す外観図である。
【図４】大きなアンバランスがかからないロボット１の基準姿勢を示す図である。
【図５】大きなアンバランスがかかるロボット１の基準姿勢を示す図である。
【符号の説明】
１　多関節ロボット
２　負荷
３　サーボモータ
４　サーボアンプ
５　指令値生成装置
６　記憶装置
７　プログラム作成装置
８　重量・重心演算装置
９　電流取り込み装置
１０　教示装置（軸操作装置）
【図１】
2004025387.tif 000012
【図２】
2004025387.tif 000013
【図３】
2004025387.tif 000014
【図４】
2004025387.tif 000015
【図５】
2004025387.tif 000016