JP6862604B2 - 垂直多関節ロボットの慣性パラメータ同定システム及び慣性パラメータ同定方法並びに垂直多関節ロボットの制御装置及び制御方法 - Google Patents

Description

本明細書は、垂直多関節ロボットのアーム先端の手首部に交換可能に取り付けたエンドエフェクタを旋回動作させるときの慣性パラメータを同定する垂直多関節ロボットの慣性パラメータ同定システム及び慣性パラメータ同定方法並びに垂直多関節ロボットの制御装置及び制御方法に関する技術を開示したものである。

現在、産業用ロボットとして多用されている垂直多関節ロボットは、アーム先端の手首部に取り付けるエンドエフェクタ（ハンド、グリッパ、溶接機、ねじ締め機等）を付け替えることで、様々な作業に対応できるようになっている。エンドエフェクタはその種類毎に重量や重心位置が異なるため、アーム先端の手首部のエンドエフェクタを付け替えると、アーム先端の手首部に加わる負荷重量や重心位置が変化してエンドエフェクタの旋回動作の位置決め性能に影響を与えるが、エンドエフェクタの重量や重心位置が既知であれば、その既知データに合わせて垂直多関節ロボットの制御装置にセットする制御パラメータを対応させることでエンドエフェクタの旋回動作の位置決め性能を含む垂直多関節ロボットの各種動作での位置決め性能を改善することができる。

しかし、ユーザが使用するエンドエフェクタは、システムインテグレータやユーザにて設計、製作される場合が多い。そのため、ユーザ側でエンドエフェクタの重量や重心位置を計算又は計測して垂直多関節ロボットの制御装置にセットする制御パラメータを変更する必要があり、その作業に多くの手間や工数がかかってしまう。

そこで、特許文献１（特開平１０−１３８１８７号公報）に記載されているように、多関節ロボットのアーム先端の負荷の重量と重心位置を自動的に算出できるようにしたものがある。この自動算出方法は、まず、作業者が教示装置を操作して基準姿勢を教示して、この基準姿勢に基づいて動作パターンプログラムを作成して、その動作パターンプログラムに従ってアーム先端に所定の負荷（エンドエフェクタ）を取り付けた多関節ロボットを動作させ、この動作中に各駆動軸（各関節軸）を駆動するサーボモータの駆動電流を予め設定されたスキャンタイム毎に検出し、各駆動軸のそれぞれについて、検出された駆動電流の平均値に基づいて動作パターンプログラムの実行中に各駆動軸に作用したトルクの平均値を算出し、このトルクの平均値を各駆動軸に作用するアンバランストルクと等価であるとすることにより、アンバランストルクに未知数として含まれる負荷の重量及び重心位置を算出するものである。

特開平１０−１３８１８７号公報

しかし、上記特許文献１に記載されているように、ロボットの基準姿勢の選定に際しては、動作パターンプログラムの実行中に大きなアンバランストルクが駆動軸に作用するように注意しなければならないため、基準姿勢を選定する作業はロボット技術に精通した熟練した作業者が行う必要があり、ロボット技術に精通していない多くの作業者にとっては容易な作業ではない。しかも、基準姿勢の教示に際しては、作業者が教示装置を逐一操作してロボットの各駆動軸を位置決めする必要があり、その教示作業に手間がかかる。

これらの解決策として、上記特許文献１では、事前に教示装置により教示した基準姿勢のデータをロボットの制御装置の記憶装置に記憶しておき、負荷の重量及び重心位置を算出する際に、記憶装置から基準姿勢のデータを読み出して、このデータに基づき動作パターンプログラムを作成することが提案されている。

しかし、前述したように、ユーザが使用するエンドエフェクタは、システムインテグレータやユーザにて設計、製作される場合が多いため、ロボットの製造会社が事前にユーザの使用するエンドエフェクタの仕様を予測して教示装置により基準姿勢を教示しておくことは困難である。

上記課題を解決するために、垂直多関節ロボットのアーム先端の手首部に交換可能に取り付けたエンドエフェクタをモータで駆動される前記手首部の関節軸によって旋回動作させるときの慣性パラメータを同定する垂直多関節ロボットの慣性パラメータ同定システムにおいて、前記エンドエフェクタを前記手首部の関節軸に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させる制御部と、前記エンドエフェクタの往復旋回動作中に前記手首部の関節軸の関節角度を計測して算出する関節角度算出部と、前記エンドエフェクタの往復旋回動作中に各関節角度における前記モータのトルクを計測して算出するトルク算出部と、前記関節角度算出部及び前記トルク算出部で算出した往路の各関節角度における前記モータのトルク（往路トルク）と復路の各関節角度における前記モータのトルク（復路トルク）とに基づいて各関節角度における前記モータのトルクから摩擦トルクを補償した重力トルクを算出する重力トルク算出部と、前記重力トルク算出部で算出した各関節角度における重力トルクに基づいて前記慣性パラメータを同定する慣性パラメータ同定部と、事前に重量が異なる複数の基準重りを用意して、その基準重りを１個ずつ前記アーム先端の手首部に付け替えて前記往復旋回動作させて各関節角度における重力トルクを算出して前記複数の基準重りの各関節角度における重力トルクの算出データを記憶した記憶部とを備え、前記慣性パラメータ同定部は、前記慣性パラメータの１つである前記エンドエフェクタの重量を同定する際に、前記重力トルク算出部で算出した各関節角度における重力トルクを前記記憶部に記憶されている前記複数の基準重りの各関節角度における重力トルクと比較して両者の差が最小となる前記基準重りの重量を前記エンドエフェクタの重量として同定するようにしたものである。

この構成では、アーム先端の手首部に取り付けたエンドエフェクタを重力トルクが０となる関節角度を含む所定区間で往復旋回動作させることで、各関節角度における往路トルクと復路トルクを計測、算出して、それらの算出値に基づいて各関節角度におけるモータのトルクから摩擦トルクを補償した重力トルクを算出し、各関節角度における重力トルクに基づいて慣性パラメータを同定するようにしたので、ユーザが様々なエンドエフェクタをアーム先端の手首部に取り付けても、ユーザ側で簡単にエンドエフェクタの慣性パラメータを同定することができる。

図１は一実施例の５軸垂直多関節ロボットを示す側面図である。 図２は５軸垂直多関節ロボットの構成を関節記号で表示した図（その１）で、慣性パラメータ同定時に往復旋回動作を開始する関節角度を示している。 図３は５軸垂直多関節ロボットの構成を関節記号で表示した図（その２）で、重力トルクが０となる関節角度を示している。 図４は５軸垂直多関節ロボットの構成を関節記号で表示した図（その３）で、慣性パラメータ同定時に往復旋回動作を折り返す関節角度を示している。 図５は５軸垂直多関節ロボットの手首部の関節軸（Ｊ４）の慣性パラメータを同定する動作を制御する構成を示すブロック図である。 図６は慣性パラメータ同定プログラムの処理の流れを示すフローチャートである。 図７は往復旋回動作中に計測して算出した関節角度の時系列データの波形を示すグラフである。 図８は往復旋回動作中に計測して算出したサーボモータのトルクの時系列データの波形を示すグラフである。 図９は往路トルクと復路トルクの算出データの波形と両トルクを平均して求める重力トルクの波形を示すグラフである。 図１０はアーム先端の手首部にエンドエフェクタを取り付けて計測して算出した重力トルクの波形と重量が異なる複数の基準重りを１個ずつ付け替えて計測して算出した重力トルクの波形を示すグラフである。

以下、一実施例を説明する。
まず、図１乃至図４を用いて５軸垂直多関節ロボット１１の構成を説明する。

５軸垂直多関節ロボット１１は、工場フロア１２に設置された固定ベース１３と、この固定ベース１３上に第１関節軸１４（Ｊ１）を中心に回転可能に設けられた第１アーム１５と、この第１アーム１５の先端に第２関節軸１６（Ｊ２）によって旋回可能に設けられた第２アーム１７と、この第２アーム１７の先端に第３関節軸１８（Ｊ３）によって旋回可能に設けられた第３アーム１９と、この第３アーム１９の先端に第４関節軸２０（Ｊ４）によって旋回可能に設けられた手首部２１と、この手首部２１に第５関節軸２２（Ｊ５）を中心に回転可能且つ交換可能に取り付けられたエンドエフェクタ２３とから構成されている。これにより、手首部２１に取り付けたエンドエフェクタ２３は、その手首部２１の関節軸である第４関節軸２０によって旋回動作するようになっている。

この場合、エンドエフェクタ２３は、ハンド、グリッパ、溶接機、ねじ締め機等のいずれであっても良い。また、エンドエフェクタ２３は、５軸垂直多関節ロボット１１の製造会社が作成したものであっても良いし、システムインテグレータやユーザにて設計、製作したものであっても良い。５軸垂直多関節ロボット１１の第１〜第５の各関節軸１４，１６，１８，２０，２２は、それぞれサーボモータ等のアクチュエータ（図５に手首部２１の関節軸である第４関節軸２０を駆動するサーボモータ３１のみ図示）により駆動されるようになっている。

次に、図５を用いて、手首部２１の関節軸である第４関節軸２０によって旋回運動するエンドエフェクタ２３の慣性パラメータを同定する動作を制御する構成を説明する。

５軸垂直多関節ロボット１１の制御装置３２は、エンドエフェクタ２３の旋回動作（第４関節軸２０の回転動作）を制御するコントローラ３３と、このコントローラ３３からのトルク指令に従ってサーボモータ３１を駆動するサーボアンプ３４と、後述する図６の慣性パラメータ同定プログラム等の各種制御プログラムや複数の基準重りの各関節角度における重力トルクの算出データ等の各種データを記憶した記憶部３５等を備えた構成となっている。記憶部３５は、ハードディスク装置、ＲＯＭ、フラッシュメモリ等、電源オフ時でも記憶データを保持する不揮発性の記憶装置により構成されている。

サーボモータ３１には、回転角を検出するエンコーダ３６が設けられ、このエンコーダ３６で検出した回転角の情報がサーボアンプ３４を経由してコントローラ３３にフィードバックされる。これにより、コントローラ３３は、エンコーダ３６で検出したサーボモータ３１の回転角が回転角指令と一致するようにサーボモータ３１をフィードバック制御し、サーボアンプ３４は、コントローラ３３からのトルク指令（駆動電流指令）に従って駆動電流を制御する。

ところで、５軸垂直多関節ロボット１１の手首部２１に取り付けるエンドエフェクタ２３はその種類毎に重量や重心位置が異なるため、エンドエフェクタ２３を付け替えると、エンドエフェクタ２３の旋回動作（第４関節軸２０の回転動作）の慣性パラメータであるエンドエフェクタ２３の重量や重心位置が変化してエンドエフェクタ２３の旋回動作のみならず、５軸垂直多関節ロボット１１の各種動作での位置決め性能に影響を与える。そのため、５軸垂直多関節ロボット１１を使用するユーザは、生産開始前にエンドエフェクタ２３の旋回動作（第４関節軸２０の回転動作）の慣性パラメータであるエンドエフェクタ２３の重量や重心位置を同定して、そのエンドエフェクタ２３の重量や重心位置に応じて５軸垂直多関節ロボット１１の制御パラメータを変更する必要がある。

そこで、コントローラ３３は、後述する図６の慣性パラメータ同定プログラムを実行することで、エンドエフェクタ２３を第４関節軸２０に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させてエンドエフェクタ２３の旋回動作（第４関節軸２０の回転動作）の慣性パラメータであるエンドエフェクタ２３の重量や重心位置を自動的に同定して、エンドエフェクタ２３の重量や重心位置に応じて５軸垂直多関節ロボット１１の制御パラメータを自動的に変更するようにしている。

本実施例では、コントローラ３３は、慣性パラメータを同定する際にエンドエフェクタ２３を第４関節軸２０に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させる制御部４１として機能する。更に、コントローラ３３は、エンドエフェクタ２３の往復旋回動作中に第４関節軸２０の関節角度を計測して算出する関節角度算出部４２として機能すると共に、エンドエフェクタ２３の往復旋回動作中に各関節角度におけるサーボモータ３１のトルクを計測して算出するトルク算出部４３としても機能する。

ここで、関節角度算出部４２は、エンドエフェクタ２３の往復旋回動作中にサーボモータ３１の回転角を検出するエンコーダ３６の検出情報を所定周期で取得して、エンコーダ３６の検出情報から第４関節軸２０の関節角度を所定周期で算出する。図７は往復旋回動作中に計測して算出した関節角度の時系列データの波形を示している。

トルク算出部４３は、サーボモータ３１へのトルク指令である駆動電流指令から各関節角度におけるサーボモータ３１のトルクを算出する。図８は往復旋回動作中に計測して算出したサーボモータ３１のトルクの時系列データの波形を示している。

更に、コントローラ３３は、関節角度算出部４２及びトルク算出部４３で算出した往路の各関節角度におけるサーボモータ３１のトルク（以下「往路トルク」という）と復路の各関節角度におけるサーボモータ３１のトルク（以下「復路トルク」という）とに基づいて各関節角度におけるサーボモータ３１のトルクから摩擦トルクを補償した重力トルクを算出する重力トルク算出部４４として機能すると共に、この重力トルク算出部４４で算出した各関節角度における重力トルクに基づいてエンドエフェクタ２３の旋回動作（第４関節軸２０の回転動作）の慣性パラメータを同定する慣性パラメータ同定部４５としても機能する。図９は往路トルクと復路トルクの時系列データの波形と両トルクから求める重力トルクの時系列データの波形を示している。

往復旋回動作中のサーボモータ３１のトルクを往路トルクと復路トルクに区分する際に、第４関節軸２０の関節角度を時間微分して第４関節軸２０の回転速度を求め、この第４関節軸２０の回転速度が所定の往路閾値（例えば−１００ｒｐｍ）よりも低い区間のトルクを往路トルクに区分し、この第４関節軸２０の回転速度が所定の復路閾値（例えば＋１００ｒｐｍ）よりも高い区間のトルクを復路トルクに区分する。

前述したように、慣性パラメータを同定する際にエンドエフェクタ２３を第４関節軸２０に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させるようにしている。ここで、重力トルクが０となる関節角度とは、第４関節軸２０がサーボモータ３１で駆動されずに回転自由な状態になっているときにエンドエフェクタ２３が重力によって第４関節軸２０を支点にして自然に垂れ下がった状態になっているときの関節角度である。この重力トルクが０となる関節角度は、エンドエフェクタ２３の重心位置に応じて変化するため、エンドエフェクタ２３を重力トルクが０となる関節角度を含む所定区間で往復旋回動作させて重力トルクを計測して算出することで、重力トルクが０となる関節角度を算出できれば、その関節角度から慣性パラメータの１つであるエンドエフェクタ２３の重心位置を同定することができる。

重力トルク算出部４４は、第４関節軸２０によってエンドエフェクタ２３を往復旋回動作させたときの各関節角度におけるサーボモータ３１のトルクから摩擦トルクを補償した重力トルクを算出するようにしている。ここで、摩擦トルクとは、第４関節軸２０の摩擦によって生じるトルクであり、重力トルクとは、重力によって生じるトルクである。第４関節軸２０に作用するトルクとしては、サーボモータ３１のトルクと摩擦トルクと重力トルクの他に、往復旋回動作の加減速によって生じる加減速トルクがあり、これら４種類のトルクは次式で表される関係となる。

サーボモータ３１のトルク＝重力トルク＋摩擦トルク＋加減速トルク
この場合、往復旋回動作の加減速をできるだけ小さくすれば、加減速トルクの影響を無視できる程度にまで小さくすることができる。加減速トルクを小さくするには、旋回速度をできるだけ低速にして、一定速度で旋回する区間をできるだけ長くするようにすれば良い。加減速トルクの影響を無視できれば、サーボモータ３１のトルクと摩擦トルクと重力トルクとの関係は、次式で表される関係となる。

サーボモータ３１のトルク＝重力トルク＋摩擦トルク
ここで、各関節角度における重力トルクは、往復旋回動作の往路と復路で同じ値となる。一方、各関節角度における摩擦トルクは、往復旋回動作の往路と復路で作用方向が反転して符号（±）が反対になるが、各関節角度における摩擦トルクの絶対値は、往路と復路で同じ値となる。従って、各関節角度における往路トルクと復路トルクとの平均値（つまり各関節角度における往路トルクと復路トルクとを足して２で割り算した値）を算出すると、各関節角度におけるサーボモータ３１のトルクから摩擦トルクの影響をキャンセルした重力トルクを求めることができる。この際、上述したように、往復旋回動作中の加減速トルクの影響を無視できるように旋回速度をできるだけ低速にして、一定速度で旋回動作させる区間をできるだけ長くするようにすれば、各関節角度における往路トルクと復路トルクとの平均値を算出することで、各関節角度におけるサーボモータ３１のトルクから摩擦トルクと加減速トルクの両方の影響をキャンセルした重力トルクを求めることができる。

各関節角度における往路トルクと復路トルクとの平均値を算出する際に、往路、復路ともに関節角度毎のトルクが必要である。そこで、往路、復路ともにトルクを線形補間処理して、等ピッチの関節角度毎（例えば３５ｄｅｇから６５ｄｅｇまでの区間で１ｄｅｇ毎）にトルクを算出する。尚、１ｄｅｇより小さい関節角度毎又は１ｄｅｇより大きい関節角度毎にトルクを算出しても良いことは明らかである。

図９の例では、往復旋回動作させる区間を例えば２５ｄｅｇから７５ｄｅｇ（又は２０ｄｅｇから８０ｄｅｇ）までの区間とし、一定速度で旋回動作させる区間を例えば３５ｄｅｇから６５ｄｅｇまでの区間としているが、これ以外の関節角度の区間としても良い。図９の関節角度毎に算出した重力トルクの波形から重力トルクが０となる関節角度を同定することができる。重力トルクが０となる関節角度から慣性パラメータの１つであるエンドエフェクタ２３の重心位置を同定しても良いし、重力トルクが０となる関節角度をそのままエンドエフェクタ２３の重心位置の代用情報として用いるようにしても良い。重力トルクが０となる関節角度を同定する際に、関節角度に対する重力トルクの波形を線形近似して１次関数の係数を算出し、重力トルクが０となる関節角度を同定するようにしても良い。

また、本実施例では、慣性パラメータの１つであるエンドエフェクタ２３の重量を同定するために、事前に重量が異なる複数の基準重りを用意して、その基準重りを１個ずつ手首部２１に付け替えて前記往復旋回動作させて各関節角度における重力トルクを算出して複数の基準重りの各関節角度における重力トルクの算出データを記憶部３５に記憶しておく。この際、手首部２１に基準重りを付けない状態で往復旋回動作させて各関節角度における重力トルクを算出して、基準重り＝０ｋｇのときの重力トルクの算出データとして記憶部３５に記憶しておく。そして、慣性パラメータの１つであるエンドエフェクタ２３の重量を同定する際に、前記重力トルク算出部４４で算出した重力トルクの波形を記憶部３５に記憶されている前記複数の基準重りの重力トルクの波形と比較して両者の差が最小となる基準重りの重量をエンドエフェクタ２３の重量として同定する。

図１０は手首部２１にエンドエフェクタ２３を取り付けて計測して算出した重力トルクの波形と重量が異なる複数の基準重りを１個ずつ付け替えて計測して算出した重力トルクの波形を示している。図１０の例では、エンドエフェクタ２３を取り付けて計測して算出した重力トルクの波形を、重量が異なる４種類の基準重り（０ｋｇ、０．４ｋｇ、１ｋｇ、２ｋｇ）の重力トルクの波形と比較して、両者の差が最小となる基準重りの重量（図１０の例では１ｋｇ）をエンドエフェクタ２３の重量として同定する。

この際、手首部２１にエンドエフェクタ２３を取り付けて計測して算出した重力トルクの波形と重量が異なる複数の基準重りを１個ずつ付け替えて計測して算出した重力トルクの波形について、それぞれの波形を正弦波関数で近似して係数を算出して、それらの係数に基づいて差が最小となる基準重りの重量をエンドエフェクタ２３の重量として同定するようにしても良い。

コントローラ３３は、生産開始前に慣性パラメータ同定部４５で同定した慣性パラメータ（エンドエフェクタ２３の重量と重心位置）に基づいて５軸垂直多関節ロボット１１のサーボモータ３１の制御パラメータを変更する制御パラメータ変更部４６としても機能する。

以上説明した本実施例のエンドエフェクタ２３の慣性パラメータの同定とサーボモータ３１の制御パラメータの変更は、コントローラ３３によって図６の慣性パラメータ同定プログラムに従って実行される。以下、コントローラ３３が実行する図６の慣性パラメータ同定プログラムの処理内容を説明する。

コントローラ３３は、図６の慣性パラメータ同定プログラムを起動すると、まず、ステップ１０１で、同定開始操作が行われるまで待機する。ここで、同定開始操作とは、作業者が同定対象となるエンドエフェクタ２３を５軸垂直多関節ロボット１１の手首部２１に取り付けて制御装置３２の操作部（図示せず）を操作して慣性パラメータの同定を開始させる操作である。

この同定開始操作が行われた時点で、ステップ１０２に進み、エンドエフェクタ２３の往復旋回動作（第４関節軸２０の回転回転動作）を所定速度以下の遅い一定速度で開始する。エンドエフェクタ２３の往復旋回動作を所定速度以下の遅い一定速度で行う理由は、加減速トルクの影響を無視できるようにするためである。

この後、ステップ１０３に進み、エンコーダ３６の時系列データ（サーボモータ３１の回転角の時系列データ）を取得して、このエンコーダ３６の時系列データから関節角度の時系列データ（図７参照）を算出する。そして、次のステップ１０４で、サーボアンプ３４へのトルク指令である駆動電流指令の時系列データを取得して、この駆動電流指令の時系列データと上記ステップ１０３で算出した関節角度の時系列データとに基づいて往復旋回動作の各関節角度におけるサーボモータ３１のトルクである往路トルクと復路トルクを算出する（図９参照）。

これらステップ１０３と１０４の処理は、往復旋回動作を終了するまで所定周期で実行される。コントローラ３３は、ステップ１０５で、往復旋回動作終了と判断した時点で、ステップ１０６に進み、上記ステップ１０４で算出した各関節角度における往路トルクと復路トルクとの平均値（つまり各関節角度における往路トルクと復路トルクとを足して２で割り算した値）を各関節角度における重力トルクとして算出する。

この後、ステップ１０７に進み、上記ステップ１０６で算出した重力トルクの波形を線形近似して１次関数の係数を算出し、重力トルクが０となる関節角度を同定する。この際、重力トルクが０となる関節角度から慣性パラメータの１つであるエンドエフェクタ２３の重心位置を同定しても良いし、重力トルクが０となる関節角度をそのままエンドエフェクタ２３の重心位置の代用情報として用いるようにしても良い。

そして、次のステップ１０８で、算出した重力トルクの波形を記憶部３５に記憶されている前記複数の基準重りの重力トルクの波形と比較して両者の差が最小となる基準重りの重量をエンドエフェクタ２３の重量として同定する。

この後、ステップ１０９に進み、上記ステップ１０７と１０８の同定結果に応じてサーボモータ３１の制御パラメータを変更して、本プログラムを終了する。

以上のようにして、コントローラ３３は、生産開始前に図６の慣性パラメータ同定プログラムを実行して、慣性パラメータの同定結果に応じてサーボモータ３１の制御パラメータを変更してから生産を開始する。

以上説明した本実施例によれば、５軸垂直多関節ロボット１１の手首部２１に取り付けたエンドエフェクタ２３を重力トルクが０となる関節角度を含む所定区間で往復旋回動作させることで、各関節角度における往路トルクと復路トルクを算出して、それらの算出値に基づいて各関節角度におけるサーボモータ３１のトルクから摩擦トルクを補償した重力トルクを算出し、各関節角度における重力トルクに基づいてエンドエフェクタ２３の慣性パラメータ（重量や重心位置に関するパラメータ）を同定するようにしたので、ユーザがどの様なエンドエフェクタ２３を５軸垂直多関節ロボット１１の手首部２１に取り付けても、ユーザ側で簡単に慣性パラメータを同定することができる。これにより、慣性パラメータの同定結果に応じてサーボモータ３１の制御パラメータを適正な制御パラメータに変更することができ、エンドエフェクタ２３の旋回動作のみならず、５軸垂直多関節ロボット１１の各種動作での位置決め性能を向上させることができる。

尚、本実施例では、５軸垂直多関節ロボット１１の制御装置３２にエンドエフェクタ２３の慣性パラメータを同定する機能を搭載したが、５軸垂直多関節ロボット１１の制御装置３２とは別の場所にエンドエフェクタ２３の慣性パラメータを同定する慣性パラメータ同定システムを構築して、その慣性パラメータ同定システムによる慣性パラメータの同定結果に応じてサーボモータ３１の制御パラメータを適正な制御パラメータに変更するようにしても良い。この際、制御パラメータの変更は、５軸垂直多関節ロボット１１の制御装置３２が慣性パラメータの同定結果に応じて自動的に行うようにしても良いし、作業者が手動で制御パラメータを変更するようにしても良い。

その他、本発明は、５軸垂直多関節ロボット１１に限定されず、４軸以下又は６軸以上の垂直多関節ロボットに適用しても良く、また、垂直多関節ロボットの構成を適宜変更しても良い等、要旨を逸脱しない範囲内で種々変更して実施できることは明らかである。

１１…５軸垂直多関節ロボット、１３…固定ベース、１４…第１関節軸、１５…第１アーム、１６…第２関節軸、１７…第２アーム、１８…第３関節軸、１９…第３アーム、２０…第４関節軸、２１…手首部、２２…第５関節軸、２３…エンドエフェクタ、３１…サーボモータ、３２…制御装置、３３…コントローラ、３４…サーボアンプ、３５…記憶部、３６…エンコーダ、４１…制御部、４２…関節角度算出部、４３…トルク算出部、４４…重力トルク算出部、４５…慣性パラメータ同定部、４６…制御パラメータ変更部

Claims (7)

1. 垂直多関節ロボットのアーム先端の手首部に交換可能に取り付けたエンドエフェクタをモータで駆動される前記手首部の関節軸によって旋回動作させるときの慣性パラメータを同定する垂直多関節ロボットの慣性パラメータ同定システムにおいて、
   前記エンドエフェクタを前記手首部の関節軸に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させる制御部と、
   前記エンドエフェクタの往復旋回動作中に前記手首部の関節軸の関節角度を計測して算出する関節角度算出部と、
   前記エンドエフェクタの往復旋回動作中に各関節角度における前記モータのトルクを計測して算出するトルク算出部と、
   前記関節角度算出部及び前記トルク算出部で算出した往路の各関節角度における前記モータのトルク（以下「往路トルク」という）と復路の各関節角度における前記モータのトルク（以下「復路トルク」という）とに基づいて各関節角度における前記モータのトルクから摩擦トルクを補償した重力トルクを算出する重力トルク算出部と、
   前記重力トルク算出部で算出した各関節角度における重力トルクに基づいて前記慣性パラメータを同定する慣性パラメータ同定部と、
   事前に重量が異なる複数の基準重りを用意して、その基準重りを１個ずつ前記アーム先端の手首部に付け替えて前記往復旋回動作させて各関節角度における重力トルクを算出して前記複数の基準重りの各関節角度における重力トルクの算出データを記憶した記憶部とを備え、
   前記慣性パラメータ同定部は、前記慣性パラメータの１つである前記エンドエフェクタの重量を同定する際に、前記重力トルク算出部で算出した各関節角度における重力トルクを前記記憶部に記憶されている前記複数の基準重りの各関節角度における重力トルクと比較して両者の差が最小となる前記基準重りの重量を前記エンドエフェクタの重量として同定する、垂直多関節ロボットの慣性パラメータ同定システム。
2. 前記制御部は、前記エンドエフェクタの往復旋回動作を所定速度以下の一定速度で行う、請求項１に記載の垂直多関節ロボットの慣性パラメータ同定システム。
3. 前記重力トルク算出部は、前記関節角度算出部及び前記トルク算出部で算出した各関節角度における往路トルクと復路トルクとの平均値を各関節角度における重力トルクとして算出する、請求項１又は２に記載の垂直多関節ロボットの慣性パラメータ同定システム。
4. 前記慣性パラメータ同定部は、前記慣性パラメータの１つである前記エンドエフェクタの重心位置の情報として、前記重力トルクが０となる関節角度を同定する、請求項１乃至３のいずれかに記載の垂直多関節ロボットの慣性パラメータ同定システム。
5. 請求項１乃至４のいずれかに記載の垂直多関節ロボットの慣性パラメータ同定システムを搭載した垂直多関節ロボットの制御装置であって、
   生産開始前に前記慣性パラメータ同定システムで同定した前記慣性パラメータに基づいて前記垂直多関節ロボットの制御パラメータを変更する制御パラメータ変更部を備える、垂直多関節ロボットの制御装置。
6. 垂直多関節ロボットのアーム先端の手首部に交換可能に取り付けたエンドエフェクタをモータで駆動される前記手首部の関節軸によって旋回動作させるときの慣性パラメータを同定する垂直多関節ロボットの慣性パラメータ同定方法において、
   前記エンドエフェクタを前記手首部の関節軸に作用する重力トルクが０となる関節角度を含む所定区間で往復旋回動作させ、前記エンドエフェクタの往復旋回動作中に前記手首部の関節軸の関節角度を計測して算出すると共に、各関節角度における前記モータのトルクを計測して算出し、
   算出した往路の各関節角度における前記モータのトルクと復路の各関節角度における前記モータのトルクとに基づいて各関節角度における前記モータのトルクから摩擦トルクを補償した重力トルクを算出し、
   算出した各関節角度における重力トルクに基づいて前記慣性パラメータを同定する、垂直多関節ロボットの慣性パラメータ同定方法であって、
   事前に重量が異なる複数の基準重りを用意して、その基準重りを１個ずつ前記アーム先端の手首部に付け替えて前記往復旋回動作させて各関節角度における重力トルクを算出して前記複数の基準重りの各関節角度における重力トルクの算出データを記憶部に記憶しておき、
   前記慣性パラメータの１つである前記エンドエフェクタの重量を同定する際に、前記算出した各関節角度における重力トルクを前記記憶部に記憶されている前記複数の基準重りの各関節角度における重力トルクと比較して両者の差が最小となる前記基準重りの重量を前記エンドエフェクタの重量として同定する、垂直多関節ロボットの慣性パラメータ同定方法。
7. 請求項６に記載の垂直多関節ロボットの慣性パラメータ同定方法で同定した前記慣性パラメータを用いて前記垂直多関節ロボットを制御する垂直多関節ロボットの制御方法であって、
   生産開始前に記慣性パラメータ同定方法で同定した前記慣性パラメータに基づいて前記垂直多関節ロボットの制御パラメータを変更してから生産を開始する、垂直多関節ロボットの制御方法。

Images