JP6846607B2 - ロボットの制御方法 - Google Patents

Description

本発明はモータにより駆動されるロボットに装着された負荷の重力トルクを算出する方法に関するものである。

近年、モータによって駆動される複数の回転軸を持ったロボットにおいて安全性の向上や破損の防止を目的とした機能の高精度化が求められている。また、タクトタイムの短縮や追従性能向上を目的とした動作改善や振動抑制制御の向上、さらにロボットによる溶接や精密作業といった仕事の品質向上の実現が求められている。

このような用途を実現するため、各用途での制御方法において、ロボットのアームおよびアームの先端に装着される負荷の質量や重心位置といった情報を必要とする。このうち、ロボットが備えるアームの質量と重心位置は、設計時に使用したＣＡＤ等の設計ツールから事前に知ることができる。

しかし、ロボットのアーム先端に任意の負荷を装着する場合には、別途負荷の質量と重心位置を知ることが必要となる。ロボットが負荷の質量と重心位置を知る方法として、ロボットの操作者がロボットコントローラに対して手入力する方法があった。

また手入力しない方法として、静止状態において負荷の未装着時におけるモータ各軸のトルクと、負荷の装着時に微小量だけ持ち上げた時のトルクとの差分により負荷の質量を算出する方法が知られている（例えば、特許文献１参照）。

また、直交する２つの回転軸を中心に回転する各アームを等角速度で往復運動させて、取り付けた負荷の質量に対する重力トルクの大きさと前記２つの回転中心軸間を最短に結ぶ直線の長さとから負荷の質量と重心位置を算出する方法が知られている（例えば、特許文献２参照）。

特開２０１１−２３５３７４号公報 特開２０１０−７６０７４号公報

しかしながら、負荷の質量と重心位置を手入力する方法では、ロボットの操作者が負荷の質量と重心位置を予め算出や測定によって求めておく必要があり、作業者の負担増や入力忘れ、入力ミスといった可能性があった。

また、手入力しない方法として、特許文献１に示すような負荷の未装着時での手先トルクの推定値と負荷装着時に微小量だけ持ち上げた時の手先のトルク推定値との差分により負荷の質量を推定する方法では、測定したい負荷に対して、装着時と未装着時の両方の状態でトルクの測定が必要となる。また摩擦の方向を考慮した微調整が必要となっており、負荷推定作業者の負担増となってしまう可能性があった。

摩擦の方向を考慮する特許文献２に示すような、直交する２軸にそれぞれかかる重力トルクとその２つの回転中心軸間を最短に結ぶ直線の長さとから負荷の質量と重心位置を算出する方法では、等角速度往復回転運動により動摩擦や粘性摩擦の影響を打ち消すことよって、推定を行っていた。しかし回転方向により動摩擦や粘性摩擦の影響が異なる場合には、その影響が残り、負荷の推定が困難であった。

本発明は、回転方向により動摩擦や粘性摩擦の影響が異なる場合でも、負荷の重力トルクの算出が可能とするロボットの制御方法を提供することを目的とする。

上記課題を解決するために、本発明のロボットの制御方法は、モータにより駆動され、回転軸方向が同じである第一の回転軸と第二の回転軸とを備え、第一の回転軸を中心に回転するアームの先端側に対して、直接または間接的に負荷を装着して用いるロボットの制御方法であって、第一の回転軸を第二の回転軸の関節角に一致する関節角を中心とした正方向と負方向への往復回転運動である加振をさせながら、第二の回転軸を等角速度で９０度以上回転させることにより第一の回転軸を相対的に回転させて、第一の回転軸にかかる負荷の重力トルクを算出するものである。

また、上記に加えて、回転する第二の回転軸の動作角と相対的に回転される第一の回転軸の動作角とを同じと見なし、第一の回転軸の算出された重力トルクと第二の回転軸の動作角を関連付けるものとしてもよい。

また、上記に加えて、ロボットは、第一の回転軸と第二の回転軸の間に、第一の回転軸と第二の回転軸とは回転軸方向が異なる第三の回転軸を備えていてもよい。負荷の重力トルクの算出は、第一の回転軸と動作角が同じと見なされる第二の回転軸が第一の所定の位置にある場合の負荷の重力トルクと、第一の所定の位置から９０度回転させた第三の所定の位置にある場合の負荷の重力トルクと、を求めることを含んでもよい。さらに、第三の回転軸を等角速度で９０度以上往復回転運動させて、第三の回転軸が第二の所定の位置にある場合の負荷の重力トルクと、第二の所定の位置から９０度回転させた第四の所定の位置にある場合の負荷の重力トルクと、を求めてもよい。さらに、第二の回転軸が第一の所定の位置にある場合と第三の所定の位置にある場合とでそれぞれ求められた負荷の重力トルクと、第三の回転軸が第二の所定の位置にある場合と第四の所定の位置にある場合とでそれぞれ求められた負荷の重力トルクと、第一の回転軸の回転中心と第三の回転軸の回転中心とを最短に結ぶ軸間距離と、を用いることで負荷における質量および重心位置を求めることとしてもよい。

装着された負荷に対して、回転方向によって動摩擦や粘性摩擦の影響が異なり、負荷の重力トルクが算出困難である場合においても、負荷の重力トルクの算出が可能となる。

本発明の実施の形態におけるロボットを構成する回転軸の制御方法を示すブロック図 本発明の実施の形態におけるロボットを構成する回転軸のＸ−Ｚ平面における配置の一例を示す図 本発明の実施の形態におけるロボットを構成する回転軸のＹ−Ｚ平面における配置の一例を示す図 本発明の実施の形態におけるロボットの、図２に相当するＸ−Ｚ平面視の図 本発明の実施の形態におけるロボットの、図３に相当するＹ−Ｚ平面視の図 第４軸が第五の所定の位置にある場合を示すＹ−Ｚ平面図 第４軸が第五の所定の位置から負方向に９０度回転した第六の所定の位置にある場合を示すＹ−Ｚ平面図 第４軸が第六の所定の位置を超えて第五の所定の位置から負方向に９０度以上回転した位置にある場合を示すＹ−Ｚ平面図 第４軸が第六の所定の位置および第五の所定の位置を超えて図５Ｃに示された位置から正方向に９０度以上回転した位置にある場合を示すＹ−Ｚ平面図 図５Ａ〜図５Ｄに示される等角速度運動を行った場合の第６軸にかかる重力トルクの一例を示す図 本発明の実施の形態におけるロボットを構成する回転軸の制御方法を示すブロック図 第５軸が第二の所定の位置にある場合を示すＸ−Ｚ平面図 第５軸が第二の所定の位置から正方向に９０度回転した第四の所定の位置にある場合を示すＸ−Ｚ平面図 第５軸が第四の所定の位置を超えて第二の所定の位置から正方向に９０度以上回転した位置にある場合を示すＸ−Ｚ平面図 第５軸が第四の所定の位置および第二の所定の位置を超えて図８Ｃに示される位置から負方向に９０度以上回転した位置にある場合を示すＸ−Ｚ平面図

以下、本発明のロボットの制御方法の実施の形態について、図面を用いて説明する。

（実施の形態１）
本実施の形態では、Ｘ−Ｚ平面において表現される図２とＹ−Ｚ平面において表現される図３に示す、第１軸３５、第２軸３６、第３軸３７、第４軸３８（第二の回転軸）、第５軸３９（第三の回転軸）、第６軸４０（第一の回転軸）の回転軸を備えたロボットについて説明する。またロボットは回転軸と同数のアームを備え、各アームは対応する回転軸を中心に回転する。例えば、第４軸アーム４８は、第４軸３８を中心に回転し、第５軸アーム４９は、第５軸３９を中心に回転し、第６軸アーム５０は、第６軸４０を中心に回転する。このロボットは、６軸の垂直多関節型のロボットであり、ロボットの接地面にもっとも近い軸が第１軸３５であり、ロボットの最も先端側の軸が第６軸４０である。Ｘ−Ｙ平面はロボットの接地面に平行であり、Ｚ方向はロボットの接地面に対して垂直である。

図２と図３において、距離Ｘｌ，距離Ｙｌ，距離Ｚｌは第６軸４０の回転中心から第６軸アーム５０の先端に装着された負荷４１の重心位置までを最短で結ぶ軸間距離（負荷オフセット）のＸ成分，Ｙ成分，Ｚ成分を示す。

また図２において、アーム長Ｌｉｉとアーム長Ｌｊは、第５軸３９の回転中心から第６軸４０の回転中心までの距離（第５軸アーム４９のアーム長）のＺ成分とＸ成分を示す。本実施形態では、図３に示すように第５軸３９と第６軸４０の各回転中心がＺ方向に平行な同一線上となる例をしめしている。第５軸アーム４９のアーム長のＹ成分は、０である。

これらの寸法のうち、負荷４１の重心位置に関する距離Ｘｌ、距離Ｙｌおよび距離Ｚｌは未知の数値である。そして第５軸３９と第６軸４０に関するアーム長Ｌｉｉとアーム長Ｌｊはロボットの構成に基づいて予め知ることができる既知の数値である。

図４Ａは図２に相当するＸ−Ｚ平面視の第３軸以降の図であり、図４Ｂは図３に相当するＹ−Ｚ平面視の第４軸以降の図である。但し、図４Ｂは、図４Ａに比べて見やすいように拡大して記載している。

図４Ａにおいて距離Ｌ５は、第５軸３９の回転中心から負荷４１の重心位置までの長さを示している。図４Ｂにおいて距離Ｌ６は、第６軸４０の回転中心から負荷４１の重心位置までの長さを示している。ここで、距離Ｌ５は、アーム長Ｌｉｉ、アーム長Ｌｊ、距離Ｘｌ，距離Ｚｌから算出可能であり、距離Ｌ６は、距離Ｙｌ，距離Ｚｌから算出可能である。

次に、ロボットを構成するアームの先端に装着された負荷４１に関し、アーム先端軸である第６軸４０にかかる負荷４１の重力トルクを算出する方法について説明する。

まず、図１はロボットの回転軸の制御方法を示すブロック図である。そしてこのブロック図で行われる制御は図示しないロボットコントローラ内で行われるものである。ロボット全体としては図１と同様にブロック図が複数軸分存在するが、図１のブロック図を代表としてモータの１軸分の制御について説明する。

図１の位置制御ブロック１において、ロボットの関節角指令θｒｃに減速比Ｒｇを乗じて計算されたモータ位置指令θｃｏｍとモータ位置フィードバックθｍの差分値と位置比例ゲインＫＰＰから速度ループ指令ωｃｏｍを生成する。

図１の速度制御ブロック１２において、速度ループ指令ωｃｏｍとモータ速度フィードバックωｍの差分値と速度比例ゲインＫＰと速度積分ゲインＫＩとからモータ電流指令Ｉｍを生成する。

図１のモータと外力を示した動力学制御ブロック１８において、減速機を剛体と仮定すると、モータ発生トルクτｍは、モータ駆動側から見れば式（数１）で表され、負荷側から見れば式（数２）で表される。

ただし、式（数１）、式（数２）における記号は以下の通りである。

Ｋｔ：モータトルク定数。
Ｉｍ：モータ電流。
αｍ：モータ角加速度（ωｍの微分値）。
ωｍ：モータ角速度。
Ｊｍ：モータイナーシャ（ロータ+減速機の一次側、減速機の二次側に換算）。
Ｄ：粘性摩擦係数。
τμ：摩擦トルク。
τｄｙｎ：動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和）。
Ｒｇ：減速機の減速比。

摩擦トルクτμは以下の式で計算できる。

ただし、
Ｋμ：動摩擦の大きさ。

式（数２）における動力学トルクτｄｙｎは、低速で等角速度運動時に、角速度や角加速度に起因する慣性力、遠心力、コリオリ力は無視でき、重力トルク成分として近似できるので、以下の様に表すことができる。

ただし、
θｒ：ロボット関節角（減速機出力角度）＝θｍ／Ｒｇ
τｇＡ[θｒ]：ロボットのアームに起因する重力トルク。
τｇＬ[θｒ]：ロボットに装着された負荷に起因する重力トルク。

両方ともロボット関節角θｒにより一意に決定される。

ロボットを構成するアーム等の部品の質量と形状は既知である。

そしてロボットのアームに起因する重力トルクτｇＡ［θｒ］は、全軸のロボットの関節角がわかれば計算できる。

図１のアーム重力トルク演算ブロック２４は、当該軸のロボット関節角θｒと静止状態にある他軸のロボット関節角θｒを入力し、ロボットを構成するアーム等の部品の質量と形状から、アームに起因する重力トルクτｇＡ［θｒ］を計算する。

等角速度運動時では、モータ角加速度αｍは０である。式（数１）〜式（数５）から負荷の重力トルクτｇＬ［θｒ］を求める式を導出すると以下になる。

ここで、負方向(ｓｇｎ＝−１)に−ωｍ０で等角速度運動させた時の電流をＩｍ[θｒ]ｍとし、正方向(ｓｇｎ＝１)にωｍ０で等角速度運動させた時のモータ電流をＩｍ[θｒ]ｐとして式（数６）を表すと、それぞれ以下の式（数７）、式（数８）となる。

式（数７）、式（数８）を加算して２で割ると以下の様になる。

ただし、

つまり、低速でかつ等角速度にて負荷を往復回転運動させ、負方向の運動中にロボット関節角θｒとその角度でのモータ電流値Ｉｍ[θｒ]ｍのデータを蓄積し、正方向の運動中にロボット関節角θｒとその角度でのモータ電流値Ｉｍ[θｒ]ｐのデータを蓄積する。そして、所定の角度角Δθｒの往復回転運動の後で、同じ関節角θｒ同士で正負方向の平均をとれば（足して２で割れば）、摩擦力に関係する項が除去される。したがって、粘性摩擦係数Ｄや動摩擦Ｋμの値を求める必要がなく、重力トルクτｇＬ[θｒ]を求めることができる。

ただし、この方法は、回転方向により動摩擦の影響の大きさが異なる場合において適用できない。例えば構造的な特徴があるハイポイドギア等の減速機を介した回転軸を持つロボットの例が該当する。

このような場合では式（数７）、式（数８）はそれぞれ以下のようになる。

ただし、式（数１１）、式（数１２）における記号は以下の通りである。

Ｋ’μ₋：負方向回転時における動摩擦の大きさ。
Ｋ’μ_＋：正方向回転時における動摩擦の大きさ。

式（数１１）、式（数１２）を加算して２で割ると以下の様になる。

ただし、

式（数１３）に示すように、負方向と正方向の動摩擦の大きさをそれぞれ求めなければ、重力トルクτｇＬ[θｒ]を求めることができない。しかしながら、実際に動摩擦の大きさを測定または推定するのは困難である。したがって、動摩擦の影響を受けない方法で重力トルクτｇＬ[θｒ]を求めることが望ましい。

ここで、本発明の実施の形態１のロボット制御方法は、モータにより駆動され、回転軸方向が同じである第６軸４０（第一の回転軸）と第４軸３８（第二の回転軸）とを備え、第６軸４０を中心に回転する第６軸アーム５０の先端側に対して、直接または間接的に負荷４１を装着して用いるロボットの制御方法であって、第６軸４０を加振させながら、第４軸３８を回転させることにより第６軸４０を相対的に回転させて、第６軸４０にかかる負荷４１の重力トルクを算出するものである。

まず、図５Ａ〜図５Ｄに示すように第４軸３８を９０度以上回転させて、第６軸４０にかかる負荷４１の重力トルクを求める。

図５Ａは第４軸３８が第五の所定の位置にある場合を示している。図５Ｂは、第４軸３８が第五の所定の位置から負方向に９０度回転した第六の所定の位置にある場合を示している。図５Ｃは、第４軸３８が、第六の所定の位置を超えて、第五の所定の位置から負方向に９０度以上回転した位置にある場合を示している。図５Ｄは、第４軸３８が、第六の所定の位置および第五の所定の位置を超えて、図５Ｃに示される位置から正方向に９０度以上回転した位置にある場合を示している。図５Ａ〜図５Ｄに示される通り、第６軸４０の関節角（以下、動作角ともいう）は、回転中の第４軸３８を基準とすると実質的に不変である。すなわち、第６軸４０は、第４軸３８から見ると実質的に回転していない。ここで「実質的に」と表現しているのは、後述の通り、第６軸４０は僅かに加振されているからである。一方、第６軸４０の動作角は、Ｙ−Ｚ平面（ひいてはロボットの接地面）を基準とすると、第４軸３８の動作角に一致する。すなわち、第６軸４０は、ロボットの接地面から見ると、第４軸３８の回転に同期して回転している。したがって、ロボットの接地面を基準とすると、第４軸３８の動作角を第６軸４０の動作角と見なすことができる。

さらに、第４軸３８を回転させる際には、第６軸４０の静止摩擦の影響をなくすために、図５Ａに示すように第６軸４０を加振させる。加振は、第４軸３８の関節角に一致する関節角を中心とした正方向と負方向への往復回転運動である。第６軸４０の加振の振幅は、第４軸３８の往復回転運動の振幅よりも小さい。第６軸４０の加振の周期は、第４軸３８の往復回転運動の周期よりも小さい。第６軸４０の加振の中心は第４軸３８の関節角に一致し、その振幅および周期は小さいので、第６軸４０は、実質的に単独では回転していないと見なすことができる。すなわち、第６軸４０は、第４軸３８から見て実質的に回転していないと見なすことができる。実質的に単独で回転していない第６軸４０は、動摩擦および粘性摩擦の影響を受けない。また加振された第６軸４０は、静止摩擦の影響も受けない。そのため、第６軸４０にかかる負荷４１の重力トルクを正確に算出することが可能となる。具体的には、第６軸４０を駆動するモータに流れるモータ電流の大きさと、第６軸４０の動作角と見なすことができる第４軸３８の動作角とを測定する。この測定結果を、式（数６）の右辺第１項に代入する。粘性摩擦と動摩擦の影響は受けないので、式（数６）の右辺第２項および第３項は無視できる。式（数６）の右辺第４項は既知である。これにより、第６軸４０にかかる負荷４１の重力トルクτｇＬ（θｒ）を算出できる。

また、第４軸３８の回転速度は、低速で等角速度とするのが良い。これは低速で等角速度で回転運動させると慣性力、遠心力、コリオリ力が無視できる程小さくなり、逆に高速で運動させると軸に角速度や角加速度による慣性力、遠心力、コリオリ力の影響が大きくなり、正確な重力トルクを求められない場合があるためである。

さらに、第４軸３８の往復回転運動の振幅は、第五の所定の位置と第六の所定の位置とを含む９０度以上の範囲（以下、所定の範囲という）が好ましい。これにより、第五の所定の位置及び第六の所定の位置ではともに第４軸３８が回転中であるため、減速機のバックラッシュの影響を受けずに負荷４１の重力トルクを求めることが出来る。

さらに、精度向上のため図６に示すように、式（数６）により求めた重力トルクに対して、加振の成分を取り除くフィルタ処理を行うこととしてもよい。そして、実際に回転した第４軸３８の動作角を、負荷が取り付けられ加振している第６軸４０の動作角と同じと見なし、上述の求めた第６軸４０にかかる負荷４１の重力トルクと、第６軸４０の動作角と同じと見なした第４軸３８の動作角とを関連付ける。第４軸３８を往復回転運動させて、正方向の運動中に求めた負荷４１の重力トルクのデータと負方向の運動中に求めた負荷４１の重力トルクのデータとを蓄積する。そして同じ動作角同士で正負方向の平均をとることにより、算出する負荷４１の重力トルクの精度をさらに向上させることが出来る。

図７は、ロボットの制御方法を示すブロック図である。第４軸３８は、位置制御ブロック１ａ、速度制御ブロック１２ａおよび動力学制御ブロック１８ａにより制御される。第６軸４０は、位置制御ブロック１ｂ、速度制御ブロック１２ｂおよび動力学制御ブロック１８ｂにより制御される。

位置制御ブロック１ａおよび位置制御ブロック１ｂは、図１の位置制御ブロック１と同様に働く。ここで、位置制御ブロック１ａは、第４軸３８についての関節角指令θｒｃ４を受ける。関節角指令θｒｃ４は、低速、等角速度かつ所定の範囲で第４軸３８を往復回転運動させるための指令である。位置制御ブロック１ｂは、第６軸４０についての関節角指令θｒｃ６を受ける。関節角指令θｒｃ６は、第６軸４０を加振させるための指令である。

速度制御ブロック１２ａおよび速度制御ブロック１２ｂは、図１の速度制御ブロック１２と同様に働く。速度制御ブロック１２ａは、第４軸３８を駆動するモータについてのモータ電流指令Ｉｍ４を生成する。速度制御ブロック１２ｂは、第６軸４０を駆動するモータについてのモータ電流指令Ｉｍ６を生成する。

動力学制御ブロック１８ａおよび動力学制御ブロック１８ｂは、図１の動力学制御ブロック１８と同様に働く。動力学制御ブロック１８ａは、第４軸３８を駆動するモータについてのモータ位置指令θｍ４を生成する。動力学制御ブロック１８ｂは、第６軸４０を駆動するモータについてのモータ位置指令θｍ６を生成する。

アーム重力トルク演算ブロック２４は、第４軸３８のロボット関節角（動作角）と、他軸のロボット関節角とに基づいて、アームに起因する重力トルクτｇＡ（θｒ）を算出する。データ蓄積・摩擦トルク除去ブロック２６は、第４軸３８の動作角θｒのデータとそのときの第６軸４０の駆動トルクτｇＩ（θｒ）のデータとを蓄積する。

重力トルク算出ブロック２８は、駆動トルクτｇＩ（θｒ）とアームに起因する重力トルクτｇＡ（θｒ）とから、第６軸４０にかかる負荷４１の重力トルクτｇＬ（θｒ）を算出する。ここで、第６軸４０にかかる負荷４１の重力トルクτｇＬ（θｒ）は、第４軸３８の動作角θｒの関数である。すなわち、算出された第６軸４０にかかる重力トルクと第４軸３８の動作角とが関連付けられている。

重力トルク算出ブロック２８は、さらに、各動作角での重力トルクに対し、図６に示されるようなフィルタ処理および平均処理を実行してもよい。

以上により、第６軸４０にかかる負荷４１の重力トルクを求める方法を示した。本方法は、第６軸４０を実質的に回転させず、第６軸４０と回転軸方向が同じ第４軸３８を回転させることにより第６軸４０を相対的に回転させる。本方法は、さらに、第４軸３８の回転中に第６軸４０を加振させる。

これにより、第６軸４０の回転方向により動摩擦及び粘性摩擦が異なる場合でも、第６軸４０を中心として回転する第６軸アーム５０の先端側に直接または間接的に装着された負荷４１の重力トルクの正確な算出が可能となる。

（実施の形態２）
実施の形態１により、動摩擦の影響を受けずに装着された負荷の重力トルクを求めることができる。求めた重力トルクに関しての利用方法として挙げられるのが、負荷の質量Ｍおよび重心位置の推定である。背景技術で説明した特許文献２の方法では、直交する２軸にかかる負荷の重力トルクを求め、その負荷の質量や重心位置を算出している。そこで、実施の形態１と特許文献２の方法を併用することにより動摩擦の影響を受けずに精度良く、任意の負荷の質量や重心位置を求める例について説明する。

最初に第５軸３９にかかる負荷４１の重力トルクを求める方法について述べるが、これは特許文献２に記載の内容と同じである。第５軸３９に関しては、第二の所定の位置と第二の所定の位置から９０度回転した第四の所定の位置での重力トルクを求める。第二の所定の位置とは第５軸３９を回転中心とする第５軸アーム４９が水平となる位置である。そして、実際に重力トルクを求める際には、第二の所定の位置と第四の所定の位置を含む範囲で第５軸３９を等角速度で往復運動させる。

図８Ａは、第５軸３９が第二の所定の位置にある場合（第５軸３９を回転中心とする第５軸アーム４９が水平の位置にある状態）を示している。図８Ｂは、第５軸３９が第二の所定の位置から正方向に９０度回転した第四の所定の位置にある場合を示している。図８Ｃは、第５軸３９が第四の所定の位置を超えて、第二の所定の位置から正方向に９０度以上回転した位置にある場合を示している。図８Ｄは、第５軸３９が第四の所定の位置および第二の所定の位置を超えて、図８Ｃに示される位置から負方向に９０度以上回転した位置にある場合を示している。

そして、この動作中に第二の所定の位置で第５軸３９にかかる負荷４１の重力トルクと第四の所定の位置で第５軸３９にかかる負荷４１の重力トルクを前述の式（数７）、式（数８）、式（数９）、式（数１０）により計算する。

次に、実施の形態１で求められた第６軸４０にかかる負荷４１の重力トルクと特許文献２の方法にて求められた第５軸３９にかかる負荷４１の重力トルクを用いて、負荷４１の質量Ｍと負荷４１の重心位置に関する距離Ｘ１，距離Ｙ１，距離Ｚ１を求める方法について説明する。

負荷４１の質量をＭ、重力加速度をｇとする。第５軸３９が第二の所定の位置にある場合の第５軸３９にかかる負荷４１の重力トルクＴ１は回転中心から負荷４１の重心位置までの距離Ｌ５（図４Ａ参照）を考慮すれば式（数１５）で表現される。第５軸３９が第四の所定の位置にある場合の第５軸３９にかかる負荷４１の重力トルクＴ２は式（数１６）によって表現される。

なお、第二の所定の位置と第四の所定の位置は垂直の位置関係となるので、以下のような式を導出できる。

同様に回転中心から負荷４１の重心位置までの距離Ｌ６（図４Ｂ参照）を考えて、第６軸４０および第４軸３８が第一の所定の位置にある場合の第６軸４０にかかる負荷４１の重力トルクＴ３は式（数１７）にて表現できる。第６軸４０および第４軸３８が、第一の所定の位置から９０度回転した第三の所定の位置にある場合の第６軸４０にかかる負荷４１の重力トルクＴ４は式（数１８）にて表現できる。

一般的なロボット各軸の関節角は各軸に設けられたエンコーダ等の回転位置検出装置により検出しているので、第５軸アーム４９を水平位置にするための第５軸３９の関節角は回転位置検出装置により検出でき、負荷４１が第一の所定の位置にあるときの第６軸４０の関節角は水平位置の原点位置となり０度となるので、前述の式（数１）から式（数１４）に基づいて負荷４１に関する重力トルクＴ１、Ｔ２、Ｔ３、Ｔ４が計算できる。

さらに式（数１６）、式（数１８）から、

となり負荷４１の質量Ｍが求められる。

さらに、式（数１９）により求めた質量Ｍを式（数１５）に代入して距離Ｘｌが求まり、式（数１７）に代入して距離Ｙｌが求まり、式（数１６）または式（数１８）に代入して距離Ｚｌが求まる。

以上のように、実施の形態１の第６軸４０（第一の回転軸）と、本実施の形態２での第５軸３９（第三の回転軸）との２つの回転軸について、所定の位置（第一の所定の位置、第二の所定の位置）とその所定位置から９０度回転させた位置（第三の所定の位置、第四の所定の位置）の２点においてそれぞれの重力トルクを求めるものである。

具体的には、実施の形態１に示される方法で、第６軸４０と動作角が同じと見なされる第４軸３８が第一の所定の位置にある場合の負荷４１の重力トルクと、第一の所定の位置から９０度回転させた第三の所定の位置にある場合の負荷４１の重力トルクと、を求める。さらに、本実施の形態２に示される方法で、第５軸３９を等角速度で９０度以上往復回転運動させて、第５軸３９が第二の所定の位置にある場合の負荷４１の重力トルクと、第二の所定の位置から９０度回転させた第四の所定の位置にある場合の負荷４１の重力トルクと、を求める。さらに、第６軸４０と動作角が同じと見なされる第４軸３８が第一の所定の位置にある場合と第三の所定の位置にある場合とでそれぞれ求められた負荷４１の重力トルクと、第５軸３９が第二の所定の位置にある場合と第四の所定の位置にある場合とでそれぞれ求められた負荷４１の重力トルクと、第６軸４０の回転中心と第５軸３９の回転中心とを最短に結ぶ軸間距離（Ｌｊ，Ｌｉｉ）と、を用いることで、簡便に負荷４１の質量や重心位置を求めることができる。これによりモータにより駆動されるロボットに装着された負荷に関する重力トルクと負荷の質量や重心位置を精度良く算出することが出来る。

本発明は、モータにより駆動されるロボットに装着された負荷に関する重力トルクを精度良く算出する制御方法を提供するものであり、負荷の重量推定や衝突検出機能の高感度化を実現する。また、ロボットの利用分野の拡大と溶接や精密作業などの品質向上に利用できる。

１，１ａ，１ｂ 位置制御ブロック
１２，１２ａ，１２ｂ 速度制御ブロック
１８，１８ａ，１８ｂ 動力学制御ブロック
２４ アーム重力トルク演算ブロック
２６ データ蓄積・摩擦トルク除去ブロック
２８ 重力トルク算出ブロック
３５ 第１軸
３６ 第２軸
３７ 第３軸
３８ 第４軸
３９ 第５軸
４０ 第６軸
４１ 負荷
４８ 第４軸アーム
４９ 第５軸アーム
５０ 第６軸アーム
Ｍ 質量

Claims (3)

1. モータにより駆動され、回転軸方向が同じである第一の回転軸と第二の回転軸とを備え、前記第一の回転軸を中心に回転するアームの先端側に対して、直接または間接的に負荷を装着して用いるロボットの制御方法であって、
   前記第一の回転軸を前記第二の回転軸の関節角に一致する関節角を中心とした正方向と負方向への往復回転運動である加振をさせながら、前記第二の回転軸を等角速度で９０度以上回転させることにより前記第一の回転軸を相対的に回転させて、前記第一の回転軸にかかる前記負荷の重力トルクを算出するロボットの制御方法。
2. 回転する前記第二の回転軸の動作角と相対的に回転される前記第一の回転軸の動作角とを同じと見なし、前記第一の回転軸の算出された重力トルクと前記第二の回転軸の動作角を関連付ける請求項１に記載のロボットの制御方法。
3. 前記ロボットは、前記第一の回転軸と前記第二の回転軸の間に、前記第一の回転軸と前記第二の回転軸とは回転軸方向が異なる第三の回転軸を備え、
   前記負荷の重力トルクの算出は、前記第一の回転軸と動作角が同じと見なされる前記第二の回転軸が第一の所定の位置にある場合の前記負荷の重力トルクと、前記第一の所定の位置から９０度回転させた第三の所定の位置にある場合の前記負荷の重力トルクと、を求めることを含み、
   さらに、
   前記第三の回転軸を等角速度で９０度以上往復回転運動させて、
   前記第三の回転軸が第二の所定の位置にある場合の前記負荷の重力トルクと、前記第二の所定の位置から９０度回転させた第四の所定の位置にある場合の前記負荷の重力トルクと、を求め、
   前記第二の回転軸が前記第一の所定の位置にある場合と前記第三の所定の位置にある場合とでそれぞれ求められた前記負荷の重力トルクと、前記第三の回転軸が前記第二の所定の位置にある場合と前記第四の所定の位置にある場合とでそれぞれ求められた前記負荷の重力トルクと、前記第一の回転軸の回転中心と前記第三の回転軸の回転中心とを最短に結ぶ軸間距離と、を用いることで前記負荷における質量および重心位置を求める請求項１または２に記載のロボットの制御方法。
   

Images