JP3698770B2

JP3698770B2 - Load weight estimation method

Info

Publication number: JP3698770B2
Application number: JP26627895A
Authority: JP
Inventors: 哲朗加藤; 一郎管野
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1995-09-20
Filing date: 1995-09-20
Publication date: 2005-09-21
Anticipated expiration: 2015-09-20
Also published as: JPH0991004A

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットや工作機械のようなサーボモータで駆動される自動機械の制御のために負荷重量を推定する技術に関する。本発明の方法で推定された負荷重量を表わすパラメータは、例えばフィードフォワード制御時にフィードフォワード項の計算に利用される。
【０００２】
【従来技術】
昨今、ロボット等を制御するためのディジタルサーボ制御技術においては、ＤＳＰ（ディジタルシグナルプロセッサ）の処理能力や制御対象の特性（イナーシャが大きいなど）から、フィードバックゲインの更なるハイゲイン化を図ることは困難な状況にある。従って、目に見える制御性能の違いを与える要因は、フィードフォワードをうまく活用出来るか否かにかかって来る。
【０００３】
フィードフォワードは、サーボモータを用いてロボットのアームや工作機械の送り軸などを駆動する場合にサーボ系の追従遅れを低減させる制御方法として、広く用いられている手法で、例えば、位置ループと速度ループに関してフィードフォワードを適用する場合には、位置・速度の処理周期毎に位置指令（移動指令）を微分し、この微分値を位置ループ処理で計算された速度指令値に加算して速度指令値を求める一方、イナーシャに関するパラメータに基づいて定められるフィードフォワード項を、速度ループ処理で計算されたトルク指令値に加算し、フィードフォワード制御時のトルク指令とする処理を行う。
【０００４】
フィードフォワード制御を適正に行う為には、ロボットの動特性を正しく把握して、フィードフォワード項の計算を正しく行なわれなければならない。フィードフォワード項は、制御対象とする系自体に変更があれば当然変化する。そのため、従来は、ユーザがロボット等の自動機械に付加的な物体（例えば、ハンド、加工ツールなど）を取り付けた際に、その物体の質量、重心位置などのパラメータをユーザが入力し、そのパラメータをフィードフォワード項の計算に取り入れられる方式が採用されていた。
【０００５】
しかし、ユーザが市販のハンドやツール等を自動機械に装着する場合、その質量や重心位置が簡単に判らないことも多く、それを知る為の調査がユーザの負担となっていた。また、ユーザが負荷重量値を入力しない場合には、最大許容値が自動設定されることが通例であり、無用なサイクルタイムの遅延や、フィードフォワードが不正確さによる振動を生ずるおそれがあった。
【０００６】
このような状況から、ロボット等の製造時には未確定である負荷重量を、負荷が実際に取り付けられる現場で簡便且つ正確に推定する方法が求められていた。負荷重量を推定する一つの手法として、重力モーメントの釣合を表わす方程式を解く方法が知られている。この手法を例えば６軸ロボットに適用する場合には、次のような方程式を解くことになる。
【０００７】
τi ＝Ｍi ＋Ｐ＊Ｌi （Ｘ，Ｙ，Ｚ）＋Ｆi ・・・（１）
ここで、
τi ；ロボットの静止時に第ｉ軸にかかるトルク
Ｐ；負荷重量
Ｆi ；第ｉ軸の静摩擦
Ｍi ；負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ，Ｙ，Ｚ）；負荷の重心位置で、最終軸（６軸ロボットであれば、第６軸）に固定された座標系上の座標値で、後述するように、（ｘ6g，ｙ6g，ｚ6g）など表記される。
【０００８】
Ｌi ；第ｉ軸について、負荷の重力モーメントの腕の長さ。負荷重量Ｐとの積は、第ｉ軸にかかる負荷の重力モーメントを表わす。
【０００９】
Ｍi 及びＬi は、同一のロボットであれば、負荷を取り付けた後は、ロボットの姿勢のみの関数となる。静止時には、負荷のモーメントを表わすデータをロボットから得ることは出来ないので、上記（１）式を解いて求めるパラメータは、Ｐi ，Ｘ，Ｙ，Ｚの４つになる。原理的には、負荷による重力モーメントがかかる４つ（以上）の軸を選んでトルクτi を測定し、上記（１）式を連立させて解けば、負荷重量の推定値と重心位置を表わす４つのパラメータＰi ，Ｘ，Ｙ，Ｚが求められる。
【００１０】
しかし、この方法を実際に適用しようとした場合、静摩擦Ｆi の個体差やユーザの使用状況によるロボットリンクの重量の変化（例えば、ケーブルや溶接機などがロボットアームに装着されることによって変化する）の影響を補正することが困難なので、負荷重量の推定精度を上げることが出来なかった。
【００１１】
【発明が解決しようとする課題】
そこで、本発明の目的は、サーボモータで駆動される制御対象（自動機械）に付加的に取り付けられる物体に関して、その物体の負荷重量に関するパラメータ（質量、重心位置を表わすパラメータ）を簡便且つ正確に推定することにある。また、本発明は、そのことを通して、ユーザがハンド、ツール等の物体をロボットや工作機械に取り付けて作業を行なう場合の作業負担を軽減するとともに、フィードフォワード制御の性能の向上に寄与しようとするものである。
【００１２】
【課題を解決するための手段】
本発明は、上記技術課題を解決する為に、ソフトウェア処理能力を有する制御装置によって制御され、サーボモータで駆動される複数の軸を有する制御対象について、静止中の各軸のバランスを表わす下記のバランス式、
τi ＝Ｍi ＋Ｐ＊Ｌi （Ｘ，Ｙ，Ｚ）＋Ｆi
（但し、τi ；制御対象の静止時に第ｉ軸にかかるトルク
Ｐ；負荷の質量
Ｆi ；第ｉ軸の静摩擦
Ｍi ；負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ，Ｙ，Ｚ）；負荷の重心位置
Ｌi ；第ｉ軸について、負荷の重力モーメントの腕の長さ）
の右辺に影響を与える変量について、既知な条件を含む複数の互いに異なる条件を制御対象に与え、各条件の下で所要数の軸についてトルクを測定し、それを前記式（１）の左辺に代入して得られる所要数の方程式を連立させ、未知の負荷重量のパラメータを求めることによって、前記付加的に取り付けられた物体の負荷重量を表わすパラメータを算出するようにしたものである。ここで、負荷重量のパラメータ（負荷重量パラメータ）には、少なくとも負荷の質量と重心位置を表わすものが含まれている。
【００１３】
バランス式の右辺に影響を与える変量としては、「負荷の質量」と「姿勢」が有用である。前者を採用した場合には、共通した静止姿勢の下で、既知な第１の負荷重量条件と未知の第２の負荷重量条件を前記制御対象に与え、各負荷重量条件の下で、少なくとも４つの軸についてトルクを前記制御装置内部で測定し、上記バランス式の左辺に代入し、両負荷重量条件の間でバランス式の差分をとる形式で作成される連立方程式を解く。これによって、未知の第２の負荷重量条件を表わす未知パラメータが求められる。
【００１４】
バランス式の右辺に影響を与える変量として、「姿勢」を採用した場合には、相互に異なる所定数の静止姿勢条件の下で、未知の負荷重量条件を制御対象に与え、各静止姿勢条件の下で、１つ以上の軸についてトルクを制御装置内部で測定する。そして、これらの測定されたトルクを前記のバランス式に代入して得られる方程式を連立させ、負荷以外の重量から発生し、第ｉ軸にかかる重力モーメントＭi を記述するリンクパラメータと第ｉ軸の静摩擦Ｆi を先に求め、更に、それに基づいて未知の負荷重量条件を表わす未知パラメータを求めることにより、未知の負荷重量が推定される。
【００１５】
制御対象としては、６軸ロボットや工作機械が考えられる。また、求められた負荷重量を表わすパラメータは、例えば、フィードフォワード制御を実行する際のフィードフォワード項の計算に利用される。
【００１６】
【作用】
本発明は、制御対象の静止中の各軸のバランスを表わす前出のバランス式、
τi ＝Ｍi ＋Ｐ＊Ｌi （Ｘ，Ｙ，Ｚ）＋Ｆi
（但し、τi ；制御対象の静止時に第ｉ軸にかかるトルク
Ｐ；負荷の質量
Ｆi ；第ｉ軸の静摩擦
Ｍi ；負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ，Ｙ，Ｚ）；負荷の重心位置
Ｌi ；第ｉ軸について、負荷の重力モーメントの腕の長さ）
の右辺に影響を与える変量について、既知な条件を含む複数の互いに異なる条件を制御対象に与え、各条件の下で所要数の軸についてトルクを測定し、それを前記式（１）の左辺に代入することによって、複数の独立した方程式が得られる点に着目したものである。即ち、求めるべき未知数の数以上の独立した方程式が得られれば、これらを連立させて、直接または間接に負荷重量を表わすパラメータが求められる。
【００１７】
バランス式の右辺に影響を与える変量として、「負荷の質量」を採用した場合には、共通した静止姿勢の下で、既知な第１の負荷重量条件と未知の第２の負荷重量条件を前記制御対象に与え、各負荷重量条件の下で、少なくとも４つの軸についてトルクを前記制御装置内部で測定する。そして、それらを上記バランス式の左辺に代入し、両負荷重量条件の間でバランス式の差分をとる形式で作成される連立方程式を解けば、未知の第２の負荷重量条件を表わすパラメータが求められる。
【００１８】
バランス式の右辺に影響を与える変量として制御対象の「姿勢」を採用した場合には、相互に異なる所定数の静止姿勢条件の下で、未知の負荷重量条件を制御対象に与え、各静止姿勢条件の下で、１つ以上の軸についてトルクを制御装置内部で測定する。そして、これらの測定されたトルクを前記のバランス式に代入して得られる方程式を連立させる。
【００１９】
この場合には、未知の負荷重量条件を表わすパラメータを直接求めずに、先ず負荷以外の重量から発生し、第ｉ軸にかかる重力モーメントＭi を記述するリンクパラメータと第ｉ軸の静摩擦Ｆi を先に求め、次いで、それに基づいて未知の負荷重量条件を表わすパラメータを求める手順を踏む。
【００２０】
このようにして求められた負荷重量のパラメータを、ロボットや工作機械のフィードフォワード制御時のフィードフォワード項の計算に利用すれば、フィードフォワード項の計算値がユーザの使用実態を反映したものとなり、制御性能の向上が期待出来る。
【００２１】
【発明の実施の形態】
本発明の一つの実施形態として、ここでは、図１に示したような「平行リンクなし」の軸（Ｊ５軸）が存在する６軸構成のロボット１の手先にツール２が装着された場合に、ツール２によって生じた負荷重量を推定する方法について説明する。図１で使用されている各符号の意味は次の通りである。
ｔ1 〜ｔ6 ；ロボット１のＪ1 軸〜Ｊ6 軸の変位量を表わす変数
ｍ1 〜ｍ5 ；各々Ｊ1 軸〜Ｊ5 軸のアームの質量
ｍ6 ；負荷（ツール）２の質量
ｘ0 ，ｙ0 ，ｚ0 ；ロボットのベースに固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ1 軸の回転中心にとる。
ｘ1 ，ｙ1 ，ｚ1 ；ロボットのＪ1 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ2 軸の回転中心にとる。
ｘ2 ，ｙ2 ，ｚ2 ；ロボットのＪ2 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ3 軸の回転中心にとる。
ｘ3 ，ｙ3 ，ｚ3 ；ロボットのＪ3 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ4 軸の回転中心にとる。
ｘ4 ，ｙ4 ，ｚ4 ；ロボットのＪ4 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ5 軸の回転中心にとる。
ｘ5 ，ｙ5 ，ｚ6 ；ロボットのＪ5 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ5 軸の回転中心にとる。
ｘ6 ，ｙ6 ，ｚ6 ；ロボットのＪ6 軸に固定された座標系のｘ、ｙ，ｚ軸座標系の原点はＪ6 軸の回転中心にとる。なお、これらｘj ，ｙj ，ｚj 軸（ｊ＝１〜６）を持つ座標系を座標系ｊと呼ぶことにする。
【００２２】
ｌ1x；Ｊ1 軸の回転中心とＪ2 軸の回転中心の間の距離のｘ1 成分
ｌ1y；Ｊ1 軸の回転中心とＪ2 軸の回転中心の間の距離のｙ1 成分
ｌ2x；Ｊ2 軸の回転中心とＪ3 軸の回転中心の間の距離のｘ2 成分
ｌ3x；Ｊ3 軸の回転中心とＪ4 軸の回転中心の間の距離のｘ3 成分
ｌ4y；Ｊ4 軸の回転中心とＪ5 軸の回転中心の間の距離のｙ4 成分
ｌ6z；Ｊ5 軸の回転中心とＪ6 軸の回転中心の間の距離のｚ6 成分
隣合う座標系の関係を表わす一般的な方法としてＤ−Ｈパラメータ（θ，ｄ，ａ，α）による記述法が周知である。図１に示した軸構成を有するロボットのＤ−Ｈパラメータは、図２の通りに与えられる。良く知られているように、Ｄ−Ｈパラメータが定まれば、各座標系間の関係を表わす同次変換行列が決まる。従って、上記ｌ1x，ｌ1y・・・ｌ6zを教示しておけば、座標変換に必要なデータが教示されたことになる。
【００２３】
以下、図中に示した質量ｍ1 〜ｍ6 の重心位置をそれぞれ座標系ｊ上で表わしたものを（ｘjg，ｙjg，ｚjg）で表記する。すると、質量ｍ6 を有する負荷重量の重心位置は、座標系６上で（ｘ6g，ｙ6g，ｚ6g）となる。即ち、推定されるべき負荷重量のパラメータは、質量ｍ6 とその重心位置（ｘ6g，ｙ6g，ｚ6g）となる。
【００２４】
ロボットに対して本発明の方法を実施するには、従来より使用されている通常のロボット制御システムを利用することが出来る。図３は、その基本的なシステム構成を記したものである。同図において、符号１０はシステム全体を統括制御するロボット制御装置のホストＣＰＵである。再生運転時には、ホストＣＰＵ１０が動作プログラムを読み込み、軌道計画を立て、３次元空間上の補間点を作成する。ホストＣＰＵ１０は更に、逆変換処理により、各軸の移動指令を作成して共有ＲＡＭ１１に書き込む。
【００２５】
サーボＤＳＰ１２は、共有ＲＡＭ１１に書き込まれた移動指令と、モータ１４の位置検出器（パルスコーダ）からのフィードバック信号に基づき、速度指令、トルク指令を作成する。そして、トルク指令に応じた電流が、アンプ１３から各軸のモータ１４に供給される。
【００２６】
本発明を実施する際には、ロボットに負荷重量推定のための静止姿勢をとらせた状態で、各軸のトルクを知る必要がある。そこで、ロボットに静止姿勢をとらせた状態でホストＣＰＵ１０からサーボＤＳＰ１２にトルク／各軸値書込指令を送り、その時点におけるトルク指令と、各軸値を共有ＲＡＭ１１に書き込ませる。各軸値はフィードバック信号から知ることが出来る。
【００２７】
ホストＣＰＵ１０は、各軸値（ｔ1 〜ｔ6 ）とホストＣＰＵ１０に実装された不揮発性メモリ１５に予め格納されているリンクパラメータを使ってＤ−Ｈパラメータを求め、ロボットの姿勢を計算する。ホストＣＰＵ１０は、更に、共有ＲＡＭ１１に書き込まれたトルク指令値を読み出し、不揮発性メモリ１５に予め格納されているトルク定数等から実際のトルクτ1 〜τ6 （一部または全部）を推定する。
【００２８】
以下、このようにして推定されたトルクと、ロボットの姿勢、リンクパラメータ等を用いて、負荷重量のパラメータ（ｍ6 ，ｘ6g，ｙ6g，ｚ6g）を推定する計算が、ホストＣＰＵ１０によって実行される。以下、その計算内容について説明する。
【００２９】
本発明に従って負荷重量のパラメータを推定するには、静止中のロボットのバランスを表わす前記式（１）の右辺に影響を与える変量について、既知な条件を含む複数の互いに異なる条件をロボットに与え、各条件の下で所要数の軸についてトルクを測定し、それを前記式（１）の左辺に代入して得られる所要数の方程式を連立させ、未知の負荷重量のパラメータを求めることが必要である。既知条件を与えることが容易で、前記式（１）の右辺に影響を与える変量として、「負荷重量」と「ロボット姿勢」がある。先ず、前者に該当する実施形態について説明する。
【００３０】
［負荷重量の条件を変えてバランス式を立てる実施形態］
この実施形態を採用する場合には、任意の共通のロボット姿勢の下で、未知の負荷重量がロボットに作用する条件と、既知な負荷重量がロボットに作用する条件下を与え、前記（１）式に関する差分をとることで、連立方程式を解く。
【００３１】
ここでは、既知な負荷重量として最も明解な「無負荷（ツール２をロボットからはずした状態）]を採用する。作業と処理の手順は、次の通りである。
１．ツール２を装着せずに（無負荷）、ロボット１に適当な静止姿勢をとらせる。静止姿勢は一般にはほぼ任意であるが、バランス式のトルク項に採用しようとする軸に、トルクがかかり難い姿勢は好ましくない。
【００３２】
２．ロボット制御装置の教示操作盤等を用いた指令入力により、ホストＣＰＵ１０からサーボＤＳＰ１２にトルク／各軸値書込指令を送る。すると、上記したように、ロボット制御装置内部でトルク推定値とロボットの姿勢が求められ、記憶される。この無負荷条件（一般には、既知な第１の負荷重量条件）で、第ｉ軸について得られたトルク推定値をＴ１i とする。
【００３３】
３．ツール２をロボット１に装着する。
【００３４】
４．上記１．で定めたロボット１の静止姿勢を維持したまま、ないしは、予め姿勢を教示しておいて、負荷取付け後ロボットをその点に移動させてから、再度、ロボット制御装置の教示操作盤等を用いた指令入力により、ホストＣＰＵ１０からサーボＤＳＰ１２にトルク／各軸値書込指令を送り、トルク推定値を算出・記憶させる。この未知の負荷条件（ｍ6 ；ｘ6g，ｙ6g，ｚ6g）で、第ｉ軸について得られたトルク推定値をＴ２i とする。
【００３５】
５．４つ以上の軸について、測定されたＴ１i ，Ｔ２i を用いて前出のバランス式（１）の差分をとる形で、連立方程式を立ててこれを解き、未知のパラメータｍ6 ；ｘ6g，ｙ6g，ｚ6gを求める。その計算手順は次の通りである。
【００３６】
各軸についてのバランスの式は、
Ｔ１i ＝Ｍ１i ＋Ｐ１＊Ｌ１i （Ｘ１，Ｙ１，Ｚ１）＋Ｆ１i ・・・（２）
Ｔ２i ＝Ｍ２i ＋Ｐ２＊Ｌ２i （Ｘ２，Ｙ２，Ｚ２）＋Ｆ２i ・・・（３）
となる。ここで、
Ｐ１；既知な第１の負荷重量（負荷の質量）。ここでは、Ｐ１＝０
Ｆ１i ；既知な第１の負荷重量条件の下での第ｉ軸の静摩擦
Ｍ１i ；既知な第１の負荷重量条件の下で、負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ１，Ｙ１，Ｚ１）；既知な第１の負荷Ｐ１の重心位置。ここでは、Ｐ１＝０であるから、実際には不要なデータである。形式的には、例えば、Ｘ２＝Ｙ２＝Ｚ２として良い。
【００３７】
Ｌ１i ；既知な第１の負荷重量条件の下で第ｉ軸にかかる負荷の重力モーメントの腕の長さ
Ｐ２；第１の未知の負荷重量（負荷の質量）。ここでは、Ｐ２＝ｍ6 である。
Ｆ２i ；未知な第２の負荷重量条件の下での第ｉ軸の静摩擦
Ｍ２i ；未知な第２の負荷重量条件の下で、負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ２，Ｙ２，Ｚ２）；未知な第２の負荷Ｐ２の重心位置。ここでは、
（ｘ6g，ｙ6g，ｚ6g）である。
【００３８】
Ｍ１i とＭ２i は、同一のロボットに同一の姿勢をとらせた場合は、等しい値を持つ。また、静摩擦Ｆ１i とＦ２i についても、同一のロボットに同一の姿勢をとらせた場合はほぼ等しい値とみなすことが出来る。従って、ｉ軸に関するバランス式の差分として、上記（３）−（２）を採用すれば、次の（４）式が得られる。
【００３９】
Ｔ２i −Ｔ２i ＝ｍ6 ＊Ｌ２i （ｘ6g，ｙ6g，ｚ6g）・・・（４）
ここで、Ｌ２i （ｘ6g，ｙ6g，ｚ6g）は、第ｉ軸にかかる負荷ｍ6 による重力モーメントの腕の長さである。従って、（４）式を未知のパラメータｍ6 ，ｘ6g，ｙ6g，ｚ6gについて解くには、座標系ｉにおける負荷ｍ2 の位置（ｘig，ｙig，ｚig）を、ｘ6g，ｙ6g，ｚ6gを使って表わす必要がある。
【００４０】
上述したように、各座標系間の座標変換の式は、Ｄ−Ｈパラメータから計算可能である。そこで、既知なロボット姿勢（各軸値ｔ1 〜ｔ6 ）と設定済みのリンクパラメータで記述されるＤ−Ｈパラメータを用いて（図２参照）、Ｌ２i が未知のパラメータｘ6g，ｙ6g，ｚ6gに関する式で求められる。
【００４１】
このようにして、少なくとも４軸（例えば、ｉ＝２，３，４，５）について、（４）式を計算すれば、４個の未知パラメータｍ6 ，ｘ6g，ｙ6g，ｚ6gに関する４個以上の独立した方程式が得られる。そこで、ロボット制御装置内でホストＣＰＵ１０を利用したソフトウェア処理によって、これら方程式を連立させて解くことで、負荷重量を表わすパラメータｍ6 ，ｘ6g，ｙ6g，ｚ6gを求めることが出来る。独立した方程式の数が５以上である場合には、最小２乗計算等によって最適解を求めれば良い。
【００４２】
［ロボットの姿勢の条件を変えてバランス式を立てる実施形態］
この実施形態を採用する場合には、未知の負荷重量を付けたままロボットに所定数の異なるロボット姿勢条件を与え、各後見下で得られる前記（１）式を連立させて、リンクパラメータを求め、それから未知の負荷重量のパラメータｍ6 ，ｘ6g，ｙ6g，ｚ6gを推定する。作業と処理の手順は、次の通りである。
【００４３】
１．ツール２をロボット１に装着する。
２．ロボット１に第１の静止姿勢をとらせる。
３．第１の静止姿勢を維持したまま、ロボット制御装置の教示操作盤等を用いた指令入力により、ホストＣＰＵ１０からサーボＤＳＰ１２にトルク／各軸値書込指令を送り、トルク推定値を算出・記憶させる。第１の姿勢条件下で、第ｉ軸について得られたトルク推定値Ｔ(1)iを記憶する。
【００４４】
４．ロボットをジョグ操作して、ロボット１に第１の静止姿勢とは異なる第２の静止姿勢をとらせる。この静止姿勢で測定されたトルク推定値Ｔ(2)iを記憶する。
５．以下、必要な組数Ｋのデータが得られるまで、ロボット１に第１、第２・・・第ｋ−１の静止姿勢とは異なる第ｋ（ｋ＝３，４・・・Ｋ）の静止姿勢を順次とらせ、各静止姿勢で得られたトルク推定値Ｔ(k)iを順次記憶する。
６．各姿勢における各軸についてのバランスの式は、
Ｔ(k)i＝Ｍ(k)i ＋Ｐ＊Ｌ(k)i （Ｘ，Ｙ，Ｚ）＋Ｆ(k)i ・・・（５）
となる。ここで、
Ｐ；未知の負荷重量（負荷の質量）。ここでは、Ｐ＝ｍ6
Ｌ(k)i；第ｋの静止姿勢条件の下で、第ｉ軸にかかる負荷の重力モーメントの腕の長さ
Ｆ(k)i ；第ｋの静止姿勢条件の下での第ｉ軸の静摩擦
Ｍ(k)i ；第ｋの静止姿勢条件の下で、負荷以外の重量から発生し、第ｉ軸にかかる重力モーメント
（Ｘ，Ｙ，Ｚ）；未知の負荷の重心位置で、ここでは、（ｘ6g，ｙ6g，ｚ6g ）である。
【００４５】
式（５）におけるＬ(k)iは、静止姿勢ｋにおいて第ｉ軸にかかる負荷ｍ6 による重力モーメントの腕の長さである。従って、上述の差分をとるケースと同様に、ｋ番目の既知なロボット姿勢における各軸値（ｔ1 〜ｔ6 ）と設定済みのリンクパラメータで記述されるＤ−Ｈパラメータを用いて（図２参照）、Ｌ(k)iが、未知のパラメータｘ6g，ｙ6g，ｚ6gに関する式としてロボット制御装置内部で求められる。
【００４６】
Ｍ(k)i については、６軸ロボットの場合、次式（６）で与えられる。
【００４７】
【数１】

ここで、Ａij，Ｂij（ｉ＝１，２，３・・・６；ｊ＝１，２，３・・・５）は、リンクパラメータで記述される係数である。また、Ｊ(j) はｊ軸の角度で、各軸値（ｔ1 〜ｔ6 ）と同じものである。本実施形態では、所要数の姿勢に関する上記（５）式におけるＭ(k)iついて上記（６）式を代入し、得られた連立方程式を係数Ａij，Ｂij 並びに静摩擦Ｆ(k)i について解く。独立した方程式の数が未知数の数以上である場合には、最小２乗計算等によって最適解を求めれば良い。
【００４８】
そして、最後に、求められたＭ(k)i とＦ(k)i を（５）式に再代入して、負荷重量の未知パラメータｍ6 ，ｘ6g，ｙ6g，ｚ6gが求められる。以上の計算は、すべて、ロボット制御装置内部で、ホストＣＰＵ１０を利用したソフトウェア処理によって実行される。
【００４９】
こうして、上記いずれかの実施形態で求められたツール（物体）２の負荷重量に関するパラメータは、フィードフォワード制御時にフィードフォワード項の算出の為に使用される。以下、図４に示されたような簡単なケースについてフィードフォワード制御の概略を説明する。
【００５０】
図４は、モータ３（例えば、第１軸のモータ）で駆動される制御対象が、図示した関係でアーム４の手先にツール（物体）２を取り付けた系であると見なせる場合を表わしている。同図に併記したように、ここでは各パラメータＪm ，ｍ1 ，ｓ1 ，Ｉ1z，ｌ1 ，ｍ6 ，ｓ6 の意味は次の通りである。
【００５１】
Ｊm ；モータ３のロータイナーシャ
ｍ1 ；アームの質量
ｓ1 ；アーム４の重心位置
Ｉ1z，；ｚ軸回りに関するアーム４の重心回りのイナーシャ（重心回りのイナーシャのｚ成分）
ｌ1 ；アームの長さ
ｍ6 ；ツール２の質量
ｓ6 ；アーム４の先端からツール２の重心位置までの距離
ここで、Ｊm は予めロボットに設定されるパラメータ、ｍ1 ，ｓ1 ，Ｉ1z並びにｌ1 はロボットの姿勢から計算されるパラメータである。また、ｍ6 は上記処理で求められた値がそのまま使用される。ｓ6 の値は、上記処理で求められた値ｘ6g，ｙ6g，ｚ6gとロボット姿勢から計算される。
【００５２】
このケースにおけるフィードフォワード項は次のようにして定められる。先ず、制御対象の運動方程式を立てる。モータ３の出力トルク及び位置を各々τ，ｔとおけば、運動方程式は次式（７）で与えられる。ここで、Ｊ＝Ｊm ＋ｍ1 ・ｓ1²＋Ｉ1z，Ｌ＝（ｓ1 ＋ｌ1 ＋ｓ6 ）² とおけば、運動方程式（７）は式（８）で表わされる。但し、ツール２の重心まわりのイナーシャは無いものとする。
【００５３】
【数２】

この右辺をそのままフィードフォワード項として採用して、速度指令値に加算する。
図５に、位置ループにこのフィードフォワード項を加算するフィードフォワード制御のブロック図を示した。同図において明らかなように、ロボット制御装置の位置作成部（工作機械であれば、ＣＮＣ）から所定周期で出力される位置指令は、モータの位置検出器（パルスコーダ）と比較されその偏差が計算される。計算された位置偏差に基づいてゲインＫｐで位置ループ処理が実行され、フィードフォワード項加算以前の速度指令が作成される。
【００５４】
一方、位置・速度の処理周期毎に位置指令（移動指令）が微分され、この微分値がゲインＫｐで実行される位置ループの処理得られた速度指令に加算される。
更に、ゲインＫｖで実行される速度ループ処理で計算されるトルク指令値に関しては、上記（２）式の右辺、即ち（Ｊ＋ｍ6 ・Ｌ）に位置指令の２階微分を乗じたものをフィードフォワード項としてトルク指令値に加算する。こうして計算されたトルク指令に基づいてサーボアンプからモータ３に電流が供給され、モータ３が駆動される。なお、１／（Ｊ＋ｍ6 ・Ｌ）は、モータ３にかかる負荷を表わす伝達関数であり、１／ｓは、モータ速度を積分したものがモータの回転位置であることを表わしている。
【００５５】
以上、６軸ロボットを例にとって、本発明を説明したが、制御対象が軸数の異なるロボット（例えば、４軸ロボット）や工作機械（例えば３軸構成）に変わっても、上記説明した考え方を適用して付加的に取り付けられた物体に負荷重量を計算し得ることは明らかである。
【００５６】
【発明の効果】
本発明の負荷重量推定方法によれば、ロボット等の自動機械の製造時には未確定な各ユーザの使用態様に即した負荷重量が設定出来るので、動力学の補正がより正確に行えるようになり、制御性能の向上が期待出来る。例えば、フィードフォワード制御時のフィードフォワード項の計算だ正しく行なわれるようになる。
【図面の簡単な説明】
【図１】実施形態において、本発明の方法が適用されるロボットの軸構成を表わした図である。
【図２】図１に示した軸構成を有するロボットのＤ−Ｈパラメータを記した図である。
【図３】ロボット制御システムの基本的なシステム構成を記したものである。
【図４】実施形態で求めた負荷重量のパラメータが使用されるフィードフォワード制御の適用事例を説明する図である。
【図５】図４に示したケースにおけるフィードフォワード制御の概要を説明する為のブロック図である。
【符号の説明】
１ロボット
２ツール（付加的に取り付けられた物体）
３，１４モータ
４アーム
１０ホストＣＰＵ
１１共有ＲＡＭ
１２サーボＤＳＰ
１３アンプ
１５不揮発性メモリ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a technique for estimating a load weight for control of an automatic machine driven by a servomotor such as a robot or a machine tool. The parameter representing the load weight estimated by the method of the present invention is used for calculation of the feedforward term, for example, during feedforward control.
[0002]
[Prior art]
In recent years, in digital servo control technology for controlling robots and the like, it is difficult to further increase the feedback gain due to the processing capability of the DSP (digital signal processor) and the characteristics of the control target (such as large inertia). It is in the situation. Therefore, a factor that gives a visible difference in control performance depends on whether or not feedforward can be used effectively.
[0003]
Feedforward is a widely used control method that reduces the tracking delay of the servo system when a servo motor is used to drive a robot arm, a machine tool feed shaft, etc. When feedforward is applied to the loop, the position command (movement command) is differentiated at each position / velocity processing cycle, and this differential value is added to the speed command value calculated by the position loop process. On the other hand, the feedforward term determined based on the parameter relating to the inertia is added to the torque command value calculated by the speed loop processing, and the torque command at the time of feedforward control is performed.
[0004]
In order to perform the feedforward control properly, it is necessary to correctly grasp the dynamic characteristics of the robot and correctly calculate the feedforward term. The feedforward term will naturally change if there is a change in the system itself to be controlled. Therefore, conventionally, when a user attaches an additional object (for example, a hand, a processing tool, etc.) to an automatic machine such as a robot, the user inputs parameters such as the mass of the object and the position of the center of gravity. Was adopted in the calculation of the feedforward term.
[0005]
However, when a user attaches a commercially available hand, tool, or the like to an automatic machine, the mass and the position of the center of gravity are often not easily known, and a survey for knowing this has been a burden on the user. In addition, when the user does not input the load weight value, the maximum allowable value is usually set automatically, and there is a possibility that unnecessary cycle time delay and vibration due to inaccuracy of feedforward may occur. .
[0006]
Under such circumstances, there has been a demand for a method for simply and accurately estimating a load weight that is uncertain at the time of manufacturing a robot or the like at a site where the load is actually attached. As one method for estimating the load weight, a method for solving an equation representing a balance of gravitational moments is known. When this method is applied to, for example, a six-axis robot, the following equation is solved.
[0007]
.tau.i = Mi + P * Li (X, Y, Z) + Fi (1)
here,
τi: Torque applied to the i-axis when the robot is stationary
P: Load weight
Fi: Static friction of the i-th axis
Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
(X, Y, Z): The coordinate value on the coordinate system fixed to the final axis (sixth axis in the case of a six-axis robot) at the center of gravity of the load, as will be described later, (x6g, y6g, z6g).
[0008]
Li: The length of the arm of the gravity moment of the load with respect to the i-th axis. The product with the load weight P represents the gravity moment of the load applied to the i-th axis.
[0009]
For the same robot, Mi and Li are functions only of the robot's posture after the load is attached. Since data representing the moment of load cannot be obtained from the robot at rest, there are four parameters Pi, X, Y, and Z obtained by solving the above equation (1). In principle, if four (or more) axes on which a gravitational moment due to a load is selected and the torque τi is measured, and the above equation (1) is solved simultaneously, the estimated value of the load weight and the position of the center of gravity 4 Two parameters Pi, X, Y and Z are determined.
[0010]
However, when this method is actually applied, changes in the weight of the robot link due to individual differences in the static friction Fi and usage conditions of the user (for example, changes when a cable or a welder is attached to the robot arm). Because it is difficult to correct the influence of the load weight, it was not possible to improve the estimation accuracy of the load weight.
[0011]
[Problems to be solved by the invention]
Therefore, an object of the present invention is to easily and accurately set parameters (mass and center of gravity position parameters) relating to the load weight of an object attached to a control target (automatic machine) driven by a servo motor. There is to estimate. In addition, through this, the present invention reduces the work burden when the user attaches an object such as a hand or tool to a robot or machine tool, and contributes to the improvement of the performance of feedforward control. Is.
[0012]
[Means for Solving the Problems]
  In order to solve the above technical problem, the present invention provides a control object having a plurality of axes controlled by a control device having software processing capability and driven by a servo motor. Balance type,
    .tau.i = Mi + P * Li (X, Y, Z) + Fi
  (However, τi;Control targetTorque applied to the i-axis during stationary
          P: LoadQuality ofamount
          Fi: Static friction of the i-th axis
          Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
          (X, Y, Z): Center of gravity of load
    Li: length of arm of gravity moment of load with respect to the i-th axis)
  For variables that affect the right side of theControl targetThe torque is measured for the required number of axes under each condition, and the required number of equations obtained by substituting the torque into the left side of the equation (1) is determined to obtain the unknown load weight parameter. Thus, a parameter representing the load weight of the additionally attached object is calculated.Here, the load weight parameter (load weight parameter) includes at least the load mass and the center of gravity position.
[0013]
  Variables that affect the right side of the balance formula include “loadQuality ofQuantity and posture are useful. When the former is adopted, a known first load weight condition and an unknown second load weight condition are given to the control object under a common static posture, and at least 4 under each load weight condition. Torque is measured for the two axes inside the control device, and is substituted into the left side of the balance equation to solve the simultaneous equations created in the form of taking the balance equation difference between the two load weight conditions. Thereby, an unknown parameter representing an unknown second load weight condition is obtained.
[0014]
When `` posture '' is adopted as a variable that affects the right side of the balance equation, an unknown load weight condition is given to the controlled object under a predetermined number of different static posture conditions, and each static posture condition is Below, the torque is measured inside the controller for one or more axes. Then, an equation obtained by substituting these measured torques into the balance equation is made simultaneous, and a link parameter describing the gravitational moment Mi applied to the i-axis generated from a weight other than the load and the i-axis The unknown load weight is estimated by first obtaining the static friction Fi and further obtaining an unknown parameter representing the unknown load weight condition based on the static friction Fi.
[0015]
As a control target, a 6-axis robot or a machine tool can be considered. Further, the parameter representing the obtained load weight is used for, for example, calculation of a feedforward term when executing feedforward control.
[0016]
[Action]
  The present invention is the balance formula described above that represents the balance of each axis during the stationary period of the controlled object.
    .tau.i = Mi + P * Li (X, Y, Z) + Fi
  (However, τi;Control targetTorque applied to the i-axis during stationary
          P: LoadQuality ofamount
          Fi: Static friction of the i-th axis
          Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
          (X, Y, Z): Center of gravity of load
    Li: length of arm of gravity moment of load with respect to the i-th axis)
  For variables that affect the right side of theControl targetIt is noted that a plurality of independent equations can be obtained by measuring the torque for the required number of axes under each condition and substituting it for the left side of the equation (1). That is, if an independent equation equal to or greater than the number of unknowns to be obtained is obtained, these parameters are combined to obtain a parameter representing the load weight directly or indirectly.
[0017]
  As a variable that affects the right side of the balance equation,Quality ofIn the case of adopting “amount”, a known first load weight condition and an unknown second load weight condition are given to the control object under a common stationary posture, and at least under each load weight condition, Torque is measured inside the control unit for four axes. Then, by substituting them into the left side of the balance equation and solving the simultaneous equations created in the form of taking the difference of the balance equation between the two load weight conditions, a parameter representing the unknown second load weight condition is obtained. It is done.
[0018]
When the `` posture '' of the control target is adopted as a variable that affects the right side of the balance formula, an unknown load weight condition is given to the control target under a predetermined number of different static posture conditions, and each static posture Under conditions, the torque is measured inside the controller for one or more axes. Then, an equation obtained by substituting these measured torques into the balance equation is made simultaneous.
[0019]
In this case, the parameter representing the unknown load weight condition is not directly obtained, but first, the link parameter describing the gravitational moment Mi applied to the i-axis and the static friction Fi of the i-axis is first generated from the weight other than the load. Then, a procedure for obtaining a parameter representing an unknown load weight condition is taken.
[0020]
If the load weight parameter obtained in this way is used for the calculation of the feedforward term in the feedforward control of the robot or machine tool, the calculated value of the feedforward term reflects the actual usage of the user, Improvement in control performance can be expected.
[0021]
DETAILED DESCRIPTION OF THE INVENTION
As an embodiment of the present invention, here, when the tool 2 is mounted on the hand of a robot 1 having a 6-axis configuration in which a “no parallel link” axis (J5 axis) as shown in FIG. A method for estimating the load weight generated by the tool 2 will be described. The meaning of each symbol used in FIG. 1 is as follows.
t1 to t6: Variable representing the displacement of the robot 1 from the J1 axis to the J6 axis
m1 to m5; mass of arm of J1 axis to J5 axis respectively
m6: Mass of load (tool) 2
x0, y0, z0: x, y, z axes of the coordinate system fixed to the base of the robot The origin of the coordinate system is taken as the center of rotation of the J1 axis.
x1, y1, z1; x, y, z axis of the coordinate system fixed to the J1 axis of the robot The origin of the coordinate system is taken as the center of rotation of the J2 axis.
x2, y2, z2; x, y, z axis of the coordinate system fixed to the J2 axis of the robot The origin of the coordinate system is taken as the center of rotation of the J3 axis.
x3, y3, z3: x, y, z axis of the coordinate system fixed to the J3 axis of the robot The origin of the coordinate system is taken as the center of rotation of the J4 axis.
x4, y4, z4: x, y, z axis of the coordinate system fixed to the J4 axis of the robot The origin of the coordinate system is taken as the center of rotation of the J5 axis.
x5, y5, z6: x, y, z axis of the coordinate system fixed to the J5 axis of the robot The origin of the coordinate system is taken as the center of rotation of the J5 axis.
x6, y6, z6: x, y, z axis of the coordinate system fixed to the J6 axis of the robot The origin of the coordinate system is taken as the rotation center of the J6 axis. A coordinate system having these xj, yj, and zj axes (j = 1 to 6) will be referred to as a coordinate system j.
[0022]
l1x: x1 component of the distance between the center of rotation of the J1 axis and the center of rotation of the J2 axis
l1y: y1 component of the distance between the center of rotation of the J1 axis and the center of rotation of the J2 axis
l2x: x2 component of the distance between the center of rotation of the J2 axis and the center of rotation of the J3 axis
l3x: x3 component of the distance between the rotation center of the J3 axis and the rotation center of the J4 axis
l4y; y4 component of the distance between the center of rotation of the J4 axis and the center of rotation of the J5 axis
l6z: z6 component of the distance between the center of rotation of the J5 axis and the center of rotation of the J6 axis
A description method using DH parameters (θ, d, a, α) is well known as a general method for expressing the relationship between adjacent coordinate systems. The DH parameter of the robot having the axis configuration shown in FIG. 1 is given as shown in FIG. As is well known, when the DH parameter is determined, a homogeneous transformation matrix representing the relationship between the coordinate systems is determined. Therefore, if the above l1x, l1y... L6z are taught, data necessary for coordinate conversion is taught.
[0023]
In the following, the positions of the center of gravity of the masses m1 to m6 shown in the figure are expressed on the coordinate system j as (xjg, yjg, zjg). Then, the center of gravity position of the load weight having the mass m6 is (x6g, y6g, z6g) on the coordinate system 6. That is, the parameters of the load weight to be estimated are the mass m6 and the center of gravity (x6g, y6g, z6g).
[0024]
In order to carry out the method of the present invention for a robot, a conventional robot control system conventionally used can be used. FIG. 3 shows the basic system configuration. In the figure, reference numeral 10 denotes a host CPU of a robot control apparatus that performs overall control of the entire system. During the reproduction operation, the host CPU 10 reads an operation program, makes a trajectory plan, and creates an interpolation point in a three-dimensional space. Further, the host CPU 10 creates a movement command for each axis by inverse conversion processing and writes it to the shared RAM 11.
[0025]
The servo DSP 12 creates a speed command and a torque command based on the movement command written in the shared RAM 11 and a feedback signal from the position detector (pulse coder) of the motor 14. Then, a current corresponding to the torque command is supplied from the amplifier 13 to the motor 14 of each axis.
[0026]
When carrying out the present invention, it is necessary to know the torque of each axis in a state where the robot is in a stationary posture for load weight estimation. Therefore, a torque / each axis value write command is sent from the host CPU 10 to the servo DSP 12 in a state where the robot is in a stationary posture, and the torque command and each axis value at that time are written in the shared RAM 11. Each axis value can be known from the feedback signal.
[0027]
The host CPU 10 obtains a DH parameter using each axis value (t1 to t6) and a link parameter stored in advance in the nonvolatile memory 15 mounted on the host CPU 10, and calculates the posture of the robot. Further, the host CPU 10 reads the torque command value written in the shared RAM 11 and estimates actual torques τ1 to τ6 (part or all) from torque constants or the like stored in advance in the nonvolatile memory 15.
[0028]
Hereinafter, the host CPU 10 executes a calculation for estimating the load weight parameters (m6, x6g, y6g, z6g) using the torque estimated in this way, the posture of the robot, link parameters, and the like. Hereinafter, the calculation content will be described.
[0029]
In order to estimate the load weight parameter according to the present invention, the variable affecting the right side of the equation (1) representing the balance of the stationary robot is given to the robot a plurality of different conditions including known conditions, It is necessary to measure the torque for the required number of axes under each condition, and to calculate the parameters of the unknown load weight by combining the required number of equations obtained by substituting it for the left side of the equation (1). is there. Variables that are easy to give known conditions and affect the right side of the equation (1) include “load weight” and “robot posture”. First, an embodiment corresponding to the former will be described.
[0030]
[Embodiment in which a balance formula is established by changing the load weight condition]
In the case of adopting this embodiment, under any common robot posture, a condition under which an unknown load weight acts on the robot and a condition under which the known load weight acts on the robot are given. Solve the simultaneous equations by taking the difference for the equation.
[0031]
Here, the most obvious “no load (a state where the tool 2 is removed from the robot)” is adopted as the known load weight. The procedure of the work and the process is as follows.
1. Without attaching the tool 2 (no load), the robot 1 is allowed to take an appropriate stationary posture. In general, the static posture is almost arbitrary, but a posture in which torque is hardly applied to the shaft to be adopted for the balanced torque term is not preferable.
[0032]
2. A torque / each axis value write command is sent from the host CPU 10 to the servo DSP 12 by a command input using a teaching operation panel or the like of the robot controller. Then, as described above, the estimated torque value and the posture of the robot are obtained and stored in the robot controller. Under this no-load condition (generally, the first known load weight condition), the estimated torque value obtained for the i-th axis is defined as T1i.
[0033]
3. The tool 2 is attached to the robot 1.
[0034]
4). Above 1. While maintaining the static posture of the robot 1 defined in the above, or by teaching the posture in advance and moving the robot to that point after attaching the load, the teaching operation panel of the robot controller was used again. In response to the command input, a torque / each axis value writing command is sent from the host CPU 10 to the servo DSP 12, and the estimated torque value is calculated and stored. Under this unknown load condition (m6; x6g, y6g, z6g), the estimated torque value obtained for the i-th axis is T2i.
[0035]
5. For four or more axes, use the measured T1i and T2i to take the difference of the balance equation (1) above and solve the simultaneous equation to solve the unknown parameter m6; x6g, y6g , Z6g is obtained. The calculation procedure is as follows.
[0036]
  The balance equation for each axis is
    T1i = M1i + P1 * L1i (X1, Y1, Z1) + F1i (2)
    T2i = M2i + P2 * L2i (X2, Y2, Z2) + F2i (3)
  It becomes. here,
    P1; known first load weight(Load mass). Here, P1 = 0
    F1i: Static friction of the i-th axis under a known first load weight condition
    M1i: gravity moment applied to the i-th axis generated from a weight other than the load under a known first load weight condition
    (X1, Y1, Z1); known center of gravity position of the first load P1. Here, since P1 = 0, it is actually unnecessary data. For example, X2 = Y2 = Z2 may be used.
[0037]
    L1i: Length of the arm of the gravity moment of the load on the i-th axis under the known first load weight condition
    P2: first unknown load weight(Load mass). Here, P2 = m6.
    F2i: Static friction of the i-th axis under unknown second load weight condition
    M2i: Gravitational moment on the i-th axis generated from a weight other than the load under an unknown second load weight condition
    (X2, Y2, Z2); the position of the center of gravity of the unknown second load P2. here,
                          (X6g, y6g, z6g).
[0038]
M1i and M2i have the same value when the same robot has the same posture. Also, the static frictions F1i and F2i can be regarded as substantially equal values when the same robot is in the same posture. Therefore, if the above (3)-(2) is adopted as the difference of the balance equation regarding the i-axis, the following equation (4) is obtained.
[0039]
T2i-T2i = m6 * L2i (x6g, y6g, z6g) (4)
Here, L2i (x6g, y6g, z6g) is the length of the arm of the gravitational moment due to the load m6 applied to the i-th axis. Therefore, in order to solve the equation (4) for the unknown parameters m6, x6g, y6g, z6g, it is necessary to express the position (xig, yig, zig) of the load m2 in the coordinate system i using x6g, y6g, z6g. is there.
[0040]
As described above, the coordinate conversion formula between the coordinate systems can be calculated from the DH parameter. Therefore, using the DH parameter described with the known robot posture (each axis value t1 to t6) and the set link parameter (see FIG. 2), the equation for the parameters x6g, y6g, z6g with unknown L2i Desired.
[0041]
In this way, if equation (4) is calculated for at least four axes (for example, i = 2, 3, 4, 5), four or more independent parameters related to the four unknown parameters m6, x6g, y6g, z6g. The obtained equation is obtained. Therefore, parameters m6, x6g, y6g, z6g representing the load weight can be obtained by solving these equations simultaneously by software processing using the host CPU 10 in the robot controller. If the number of independent equations is 5 or more, an optimal solution may be obtained by least square calculation or the like.
[0042]
[Embodiment in which a balance expression is established by changing the robot posture conditions]
When this embodiment is adopted, a predetermined number of different robot posture conditions are given to the robot with an unknown load weight, and the above equation (1) obtained under each guard is combined to obtain the link parameter. Then, the unknown load weight parameters m6, x6g, y6g, z6g are estimated. The procedure of work and processing is as follows.
[0043]
1. The tool 2 is attached to the robot 1.
2. The robot 1 is caused to take the first stationary posture.
3. While the first stationary posture is maintained, a torque / each axis value writing command is sent from the host CPU 10 to the servo DSP 12 by a command input using a teaching operation panel or the like of the robot controller, and a torque estimated value is calculated and stored. . The estimated torque value T (1) i obtained for the i-th axis under the first posture condition is stored.
[0044]
  4). The robot is jog-operated to cause the robot 1 to take a second static posture different from the first static posture. The estimated torque value T (2) i measured in this static posture is stored.
  5. Hereinafter, until the required number of sets K of data is obtained, the robot 1 is stopped at kth (k = 3, 4... K) different from the first, second,. The posture is taken sequentially, and the estimated torque value T (k) i obtained in each stationary posture is sequentially stored.
  6). The balance equation for each axis in each posture is
      T (k) i = M (k) i + P * L (k) i (X, Y, Z) + F (k) i (5)
  It becomes. here,
    P: Unknown load weight(Load mass). Here, P = m6
    L (k) i: Length of arm of gravity moment of load applied to the i-th axis under the k-th stationary posture condition
    F (k) i: static friction of the i-th axis under the k-th stationary posture condition
    M (k) i: Gravitational moment applied to the i-th axis generated from a weight other than the load under the k-th stationary posture condition
    (X, Y, Z): The position of the center of gravity of the unknown load, here (x6g, y6g, z6g).
[0045]
In equation (5), L (k) i is the length of the arm of the gravitational moment due to the load m6 applied to the i-th axis in the stationary posture k. Accordingly, as in the case of taking the difference described above, each axis value (t1 to t6) in the kth known robot posture and the DH parameter described by the set link parameters are used (see FIG. 2). , L (k) i is obtained inside the robot controller as an expression relating to unknown parameters x6g, y6g, z6g.
[0046]
M (k) i is given by the following equation (6) in the case of a 6-axis robot.
[0047]
[Expression 1]

Here, Aij, Bij (i = 1, 2, 3... 6; j = 1, 2, 3... 5) are coefficients described by link parameters. J (j) is the angle of the j axis and is the same as each axis value (t1 to t6). In this embodiment, the above equation (6) is substituted for M (k) i in the above equation (5) regarding the required number of postures, and the obtained simultaneous equations are solved for the coefficients Aij and Bij and the static friction F (k) i. . When the number of independent equations is greater than or equal to the number of unknowns, an optimal solution may be obtained by least square calculation or the like.
[0048]
Finally, the obtained M (k) i and F (k) i are re-assigned to the equation (5), and the unknown parameters m6, x6g, y6g and z6g of the load weight are obtained. All of the above calculations are executed by software processing using the host CPU 10 inside the robot controller.
[0049]
Thus, the parameter relating to the load weight of the tool (object) 2 obtained in any one of the above embodiments is used for calculating the feedforward term during the feedforward control. Hereinafter, the outline of the feedforward control will be described for a simple case as shown in FIG.
[0050]
FIG. 4 shows a case where the control target driven by the motor 3 (for example, the first axis motor) can be regarded as a system in which the tool (object) 2 is attached to the hand of the arm 4 in the illustrated relationship. . As shown in the figure, the meanings of the parameters Jm, m1, s1, I1z, l1, m6, s6 are as follows.
[0051]
Jm: Rotor inertia of motor 3
m1: Arm mass
s1; the center of gravity of arm 4
I1z,; Inertia around the center of gravity of the arm 4 around the z axis (z component of the inertia around the center of gravity)
l1; arm length
m6: Tool 2 mass
s6: Distance from the tip of arm 4 to the center of gravity of tool 2
Here, Jm is a parameter set in advance for the robot, and m1, s1, I1z and l1 are parameters calculated from the posture of the robot. As m6, the value obtained in the above process is used as it is. The value of s6 is calculated from the values x6g, y6g, z6g obtained by the above processing and the robot posture.
[0052]
The feedforward term in this case is determined as follows. First, the equation of motion of the controlled object is established. If the output torque and position of the motor 3 are set to τ and t, respectively, the equation of motion is given by the following equation (7). Where J = Jm + m1 · s1²+ I1z, L = (s1 + l1 + s6)² Then, the equation of motion (7) is expressed by equation (8). However, there is no inertia around the center of gravity of the tool 2.
[0053]
[Expression 2]

This right side is directly adopted as a feed forward term and added to the speed command value.
FIG. 5 shows a block diagram of feedforward control for adding this feedforward term to the position loop. As is clear from the figure, the position command output from the position creation unit (CNC for machine tools) of the robot controller at a predetermined cycle is compared with the motor position detector (pulse coder) and the deviation is calculated. Is done. Based on the calculated position deviation, a position loop process is executed with a gain Kp, and a speed command before adding the feedforward term is created.
[0054]
On the other hand, the position command (movement command) is differentiated at each position / speed processing cycle, and this differential value is added to the speed command obtained by the position loop processing executed with the gain Kp.
Further, with respect to the torque command value calculated by the speed loop processing executed with the gain Kv, the feedforward term is obtained by multiplying the right side of the above equation (2), that is, (J + m6 · L) by the second order derivative of the position command. Is added to the torque command value. A current is supplied from the servo amplifier to the motor 3 based on the torque command thus calculated, and the motor 3 is driven. Here, 1 / (J + m6 · L) is a transfer function representing the load applied to the

motor

3, and 1 / s represents that the motor speed is integrated to obtain the rotational position of the motor.
[0055]
As described above, the present invention has been described by taking a 6-axis robot as an example. However, even if the control target is changed to a robot with a different number of axes (for example, a 4-axis robot) or a machine tool (for example, a 3-axis configuration), Obviously, the load weight can be calculated on an additionally attached object by application.
[0056]
【The invention's effect】
According to the load weight estimation method of the present invention, it is possible to set the load weight according to each user's use mode that is uncertain at the time of manufacturing an automatic machine such as a robot, so that the dynamics can be corrected more accurately. Improvement in control performance can be expected. For example, the calculation of the feedforward term at the time of feedforward control is correctly performed.
[Brief description of the drawings]
FIG. 1 is a diagram showing an axis configuration of a robot to which a method of the present invention is applied in an embodiment.
FIG. 2 is a diagram showing DH parameters of a robot having the axis configuration shown in FIG.
FIG. 3 shows a basic system configuration of a robot control system.
FIG. 4 is a diagram illustrating an application example of feedforward control in which a load weight parameter obtained in the embodiment is used.
5 is a block diagram for explaining the outline of feedforward control in the case shown in FIG. 4; FIG.
[Explanation of symbols]
1 Robot
2 Tools (additional attached objects)
3,14 motor
4 arms
10 Host CPU
11 Shared RAM
12 Servo DSP
13 Amplifier
15 Nonvolatile memory

Claims

A load weight estimation method for estimating a load weight parameter of an object controlled by a control device having software processing capability and additionally attached to a control target having a plurality of axes driven by a servo motor,
The load weight parameter represents at least a load mass and a gravity center position due to the object,
The following balance formula that represents the balance of each axis for a stationary control object:
.tau.i = Mi + P * Li (X, Y, Z) + Fi (1)
(However, τi; torque applied to the i-th axis when the controlled object is stationary
Quality amount of load; P
Fi: Static friction of the i-th axis
Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
(X, Y, Z); center of gravity position of load Li: length of arm of load gravity moment with respect to i-axis)
For the variable that affects the right side of the control object , a plurality of different conditions including a known condition are given to the control object ,
By measuring the torque for the required number of axes under each condition and substituting it into the left side of the balance equation (1), the required number of equations can be combined to determine the unknown load weight parameter, The load weight estimation method, wherein a parameter representing a load weight of the additionally attached object is calculated.

A load weight estimation method for estimating a load weight parameter of an object controlled by a control device having software processing capability and additionally attached to a control target having a plurality of axes driven by a servo motor,
The load weight parameter represents at least a load mass and a gravity center position due to the object,
Under a common static posture, a known first load weight condition and an unknown second load weight condition are given to the control object,
Under each load weight condition, torque is measured inside the controller for at least four axes;
The following balance formula that represents the balance of each axis for a stationary control object:
.tau.i = Mi + P * Li (X, Y, Z) + Fi
(However, τi; torque applied to the i-th axis when the controlled object is stationary
Quality amount of load; P
Fi: Static friction of the i-th axis
Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
(X, Y, Z); center of gravity position of load Li: length of arm of load gravity moment with respect to i-axis)
For the at least four axes, simultaneous equations created in a form that takes the difference of the balance formula between the known first load weight condition and the unknown second load weight condition, The load weight estimation method, wherein a load weight corresponding to the unknown second load weight condition is estimated by solving for an unknown parameter representing the unknown second load weight condition.

A load weight estimation method for estimating a load weight parameter of an object controlled by a control device having software processing capability and additionally attached to a control target having a plurality of axes driven by a servo motor,
The load weight parameter represents at least a load mass and a gravity center position due to the object,
An unknown load weight condition is given to the control object under a predetermined number of different stationary posture conditions,
Under each of the stationary posture conditions, the torque is measured inside the controller for one or more axes,
The following balance formula that represents the balance of each axis for a stationary control object:
.tau.i = Mi + P * Li (X, Y, Z) + Fi
(However, τi; torque applied to the i-th axis when the controlled object is stationary
Quality amount of load; P
Fi: Static friction of the i-th axis
Mi: Gravitational moment generated from the weight other than the load and applied to the i-axis
(X, Y, Z); center of gravity position of load Li: length of arm of load gravity moment with respect to i-axis)
Equations obtained by substituting into the above are obtained simultaneously, and a link parameter describing the gravitational moment Mi applied to the i-th axis and the static friction Fi of the i-th axis is obtained from the weight other than the load for the one or more axes.
Further, the load weight estimation method, wherein the unknown load weight is estimated by obtaining an unknown parameter representing the unknown load weight condition based on the unknown parameter.

The load weight estimation method according to any one of claims 1 to 3, wherein the control target is a robot.

The load weight estimation method according to any one of claims 1 to 3, wherein the control target is a six-axis robot.

The load according to any one of claims 1 to 3, wherein the parameter representing the obtained load weight is used for calculation of a feedforward term when performing feedforward control on the controlled object. Weight estimation method.