(57)【特許請求の範囲】
【請求項１】
ソフトウェア処理能力を有する制御装置によって制御され、サーボモータで駆動される複数の軸を有する制御対象に付加的に取り付けられた物体の負荷重量パラメータを推定する負荷重量推定方法であって、
前記負荷重量パラメータは、少なくとも前記物体による負荷の質量及び重心位置を表わしており、
静止中の制御対象について各軸のバランスを表わす下記のバランス式、
τ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 ＝Ｍ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 については、６軸ロボットの場合、次式（６）で与えられる。
【００４７】
【数１】
0003698770.tif 000002
ここで、Ａ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 ・ｓ12＋Ｉ1z，Ｌ＝（ｓ1 ＋ｌ1 ＋ｓ6 ）2 とおけば、運動方程式（７）は式（８）で表わされる。但し、ツール２の重心まわりのイナーシャは無いものとする。
【００５３】
【数２】
0003698770.tif 000003
この右辺をそのままフィードフォワード項として採用して、速度指令値に加算する。
図５に、位置ループにこのフィードフォワード項を加算するフィードフォワード制御のブロック図を示した。同図において明らかなように、ロボット制御装置の位置作成部（工作機械であれば、ＣＮＣ）から所定周期で出力される位置指令は、モータの位置検出器（パルスコーダ）と比較されその偏差が計算される。計算された位置偏差に基づいてゲインＫｐで位置ループ処理が実行され、フィードフォワード項加算以前の速度指令が作成される。
【００５４】
一方、位置・速度の処理周期毎に位置指令（移動指令）が微分され、この微分値がゲインＫｐで実行される位置ループの処理得られた速度指令に加算される。
更に、ゲインＫｖで実行される速度ループ処理で計算されるトルク指令値に関しては、上記（２）式の右辺、即ち（Ｊ＋ｍ6 ・Ｌ）に位置指令の２階微分を乗じたものをフィードフォワード項としてトルク指令値に加算する。こうして計算されたトルク指令に基づいてサーボアンプからモータ３に電流が供給され、モータ３が駆動される。なお、１／（Ｊ＋ｍ6 ・Ｌ）は、モータ３にかかる負荷を表わす伝達関数であり、１／ｓは、モータ速度を積分したものがモータの回転位置であることを表わしている。
【００５５】
以上、６軸ロボットを例にとって、本発明を説明したが、制御対象が軸数の異なるロボット（例えば、４軸ロボット）や工作機械（例えば３軸構成）に変わっても、上記説明した考え方を適用して付加的に取り付けられた物体に負荷重量を計算し得ることは明らかである。
【００５６】
【発明の効果】
本発明の負荷重量推定方法によれば、ロボット等の自動機械の製造時には未確定な各ユーザの使用態様に即した負荷重量が設定出来るので、動力学の補正がより正確に行えるようになり、制御性能の向上が期待出来る。例えば、フィードフォワード制御時のフィードフォワード項の計算だ正しく行なわれるようになる。
【図面の簡単な説明】
【図１】実施形態において、本発明の方法が適用されるロボットの軸構成を表わした図である。
【図２】図１に示した軸構成を有するロボットのＤ−Ｈパラメータを記した図である。
【図３】ロボット制御システムの基本的なシステム構成を記したものである。
【図４】実施形態で求めた負荷重量のパラメータが使用されるフィードフォワード制御の適用事例を説明する図である。
【図５】図４に示したケースにおけるフィードフォワード制御の概要を説明する為のブロック図である。
【符号の説明】
１  ロボット
２  ツール（付加的に取り付けられた物体）
３，１４  モータ
４  アーム
１０  ホストＣＰＵ
１１  共有ＲＡＭ
１２  サーボＤＳＰ
１３  アンプ
１５  不揮発性メモリ
【図１】
0003698770.tif 000004
【図２】
0003698770.tif 000005
【図３】
0003698770.tif 000006
【図４】
0003698770.tif 000007
【図５】
0003698770.tif 000008