(57)【要約】  【課題】  ロボットの各軸モータトルク値とアーム角度からロボットに備える負荷の質量と重心位置を算出する方法において、回転中心軸が直交する２軸を含む複数のアームのモータトルクとアーム角度を計測し、これより算出される負荷重力に起因するトルク成分の振幅・位相を算出し、回転中心軸が直交する幾何学的関係とから、負荷の質量及び重心位置を算出することで、アーム先端２軸の動作のみの少ない動作領域で算出出来る。しかし、ＣＰＵの処理性能によっては振幅・位相の算出時の複雑な計算で計算誤差が生じる可能性があった。  【解決手段】  直交する２つの回転軸の各軸ごとに所定の位置とそこから９０°回転させた位置の２点の負荷質量に起因する重力トルクを求める。求めた重力トルクとアームの機構的特性を考慮することで複雑な計算をすることなく簡便に負荷の質量・重心位置を算出する。【選択図】  図１
2010076074.tif 000002
【特許請求の範囲】
【請求項１】
モータにより駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、
前記２つの回転軸を中心に回転する前記各アームを１軸ごとに所定の位置と所定の位置から９０度回転させた位置を含むように等角速度で往復運動させ、所定の位置と所定の位置から９０度回転させた位置での角度と電流測定値を求め、これら電流測定値と角度とから前記負荷の質量に起因するトルクの大きさを軸ごとに求め、前記トルクの大きさと前記２つの回転中心軸間を最短に結ぶ直線の長さとから前記負荷の質量と重心位置を算出するロボットの制御方法。
【請求項２】
前記２つの回転軸を中心に回転する各アームを１軸毎に等角速度運動させ所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、これら角度と電流測定値とから負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、求めたトルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出し、請求項１記載の制御方法で算出した負荷の質量及び重心位置との差を求め、それぞれの差が所定の値以下であれば、請求項１で算出した質量及び重心位置との加重平均として前記負荷の質量及び重心位置を求める請求項１記載のロボットの制御方法。
【請求項３】
ロボットは６軸の垂直多関節型ロボットであり、２つの回転中心軸はロボットの最も先端側にある第６軸、前記第６軸に次いでロボットの最も先端側にある第５軸である請求項１または２記載のロボットの制御方法。
【請求項４】
所定の位置は、第５軸を回転中心とするアームがロボットの設置面と水平となる位置である請求項３記載のロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はモータにより駆動されるロボットに取り付けられた負荷の重量と負荷の重心位置を算出する方法に関するものである。
【背景技術】
【０００２】
近年、モータにより駆動される複数軸のアームを備えるロボットにおいて、衝突時の安全性の向上や破壊を防止することを目的とした衝突検出の高精度化が求められている。
【０００３】
また、ロボットの実タクト短縮を目的とした加減速運動性能や振動抑制制御性能の高性能化、さらには複雑な形状部品の嵌め合い作業などを目的とした柔軟制御の実現が求められている。
【０００４】
このような用途を実現するため、各用途での制御方法において、ロボットのアームおよびアームの先端に装着される負荷の質量や重心位置といった情報を必要とする。このうち、ロボットが備えるアームの質量と重心位置は、設計時に使用したＣＡＤ等の設計ツールを使用する等して予め知ることができる。
【０００５】
しかし、ロボットのアーム先端に任意の負荷が装着される場合には、別途負荷の質量と重心位置を知ることが必要となる。
【０００６】
従来、負荷の質量と重心位置を知る方法として、ロボットの操作者がそれらの値をロボットコントローラに対して手入力する方法があった。
【０００７】
また、手入力しない方法としては、ロボットの静止状態におけるモータ各軸の電流値から算出したトルクを用いて、ロボット単体での重力に起因するトルク成分と静止時の摩擦トルクを差し引くことにより負荷の重力に起因するトルクだけを抽出し、負荷の質量と重心位置を算出する方法が知られている（例えば、特許文献１参照）。また、ロボットのモータ各軸を駆動させる時の各軸モータトルクとアーム角度を、算出しようとする負荷の質量と重心位置の数だけ立式された重力に起因する関係式へ代入することによって負荷の質量と重心位置を算出する方法が知られている（例えば、特許文献２参照）。また、直交する２つの回転軸を中心に回転する各アームを往復等角速度運動させて取り付けた負荷の質量に起因する重力トルクの位相・振幅を算出し、これらに基づいて取り付けた負荷の質量と重心を算出する方法が知られている（例えば、特許文献３参照）。
【特許文献１】特開平９−０９１００４号公報
【特許文献２】特開平１１−０４８１８１号公報
【特許文献３】特開２００８−２０７３０４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００８】
しかしながら、上記した負荷の質量と重心位置を手入力する方法では、ロボットの操作者が負荷の質量と重心位置を予め算出や測定によって求めておく必要があり、作業者の負担増となり、場合によっては入力すること自体を忘れてしまうという可能性があった。
【０００９】
また、手入力しない方法として、特許文献１に示すようなロボットを静止させて行う方法では、静止時の摩擦トルクが負荷の質量やロボットの姿勢によって大きく変動することから、算出される負荷の質量と重心位置の精度に大きく影響する可能性があった。
【００１０】
また、特許文献２に示すようなロボットのモータ各軸を駆動させて行う方法では、負荷の質量と重心位置を算出するために、主要駆動軸を含む４つのモータトルク値とアーム角度の測定を行う必要があり、ロボットの動作範囲が制限される場合や動作自体が制限される場合では、負荷の質量と重心位置を算出することができない可能性があった。
【００１１】
また、特許文献３に示すような直交する２軸の重力トルクの位相や振幅に基づいて負荷の質量と重心位置を求める方法では、少ない動作で負荷の質量と重心位置を推定することが可能であるが、その反面、重力トルクの位相や振幅の計算時に三角関数の逆変換などの複雑な計算を必要とするため、ロボットコントローラに搭載されているＣＰＵの処理性能によって計算誤差が左右される可能性があった。
【課題を解決するための手段】
【００１２】
上記課題を解決するために、本発明のロボットの制御方法は、モータにより駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸ごとに所定の位置と所定の位置から９０度回転させた位置を含むように等角速度で往復運動させ、所定の位置と所定の位置から９０度回転させた位置での角度と電流測定値を求め、これら電流測定値と角度とから前記負荷の質量に起因するトルクの大きさを軸ごとに求め、前記トルクの大きさと前記２つの回転中心軸間を最短に結ぶ直線の長さとから前記負荷の質量と重心位置を算出するものである。
【００１３】
また、本発明のロボットの制御方法は、上記に加えて、前記２つの回転軸を中心に回転する各アームを１軸毎に等角速度運動させ所定の位相間隔を有しており所定の角度に位置する２点における各点の角度とその角度における電流測定値とを求め、これら角度と電流測定値とから負荷の質量に起因するトルクの振幅と位相とを各軸ごとに求め、求めたトルクの振幅と位相に基づいて前記負荷の質量及び重心位置を算出し、上記手段の制御方法で算出した負荷の質量及び重心位置との差を求め、それぞれの差が所定の値以下であれば、上記手段で算出した質量及び重心位置との加重平均として前記負荷の質量及び重心位置を求めるものである。
【００１４】
また、本発明のロボットの制御方法は、上記に加えて、ロボットは６軸の垂直多関節型ロボットであり、２つの回転中心軸はロボットの最も先端側にある第６軸、前記第６軸に次いでロボットの最も先端側にある第５軸としたものである。
【００１５】
また、本発明のロボットの制御方法は、上記に加えて、所定の位置を、第５軸を回転中心とするアームがロボットの設置面と水平となる位置としたものである。
【００１６】
上記本願発明では、２つの回転軸の動作範囲が十分とれる場合に、各アームを所定の位置を含むようにして９０°を超える角度、例えば１００°くらいの角度を等角速度で往復運動させて、所定の位置とそこから９０°回転させた位置の２点の負荷質量に起因する重力トルクを求める。そして、求めた重力トルクの大きさとアームの機構的特性を考慮することで複雑な計算をすることなく簡便に負荷の質量・重心位置を推定するものである。
【００１７】
また、同時実行可能である特許文献３の方法を併用し、特許文献３の方法で算出した負荷の質量と重心位置を、上記発明で求めた負荷の質量と重心位置の判定値あるいは補正値として用いることにより、測定動作中にロボットが外力を受けた場合の算出精度が低下するリスクを減じるものである。
【発明の効果】
【００１８】
本願発明のロボットの制御方法によれば、各アームを所定の位置を含むようにして９０°を超える角度、例えば１００°くらいの角度を等角速度で往復運動させて、所定の位置とそこから９０°回転させた位置の２点の負荷質量に起因する重力トルクを求め、求めた重力トルクの大きさとアームの機構的特性を考慮することで簡便に負荷の質量・重心位置を推定することができる。
【発明を実施するための最良の形態】
【００１９】
以下、本発明のロボットの制御方法の実施の形態について、図１から図１２を用いて説明する。
【００２０】
（実施の形態１）
本実施の形態では、垂直座標系において表現される図２と水平座標系において表現される図３に示すような、第１軸３６から第６軸４１までの回転中心軸を有するアームを備えたロボットについて説明する。また本実施の形態では、図２と図３で示すように、アーム先端側の２つの回転軸である第５軸４０と第６軸４１とはねじれの位置の関係であり、２つの軸間を最短に結ぶ直線と一方の回転軸とがなす平面が他方の回転軸と略直交して配置されたロボットの例を示している。なお、このロボットは６軸の垂直多関節型のロボットの例を示しており、ロボットの設置面に最も近い軸が第１軸であり、ロボットの最も先端側の軸が第６軸である。
【００２１】
また、図２と図３において、距離Ｘｌ（負荷オフセットＸ成分）、距離Ｚｌ（負荷オフセットＺ成分）、距離Ｙｌ（負荷オフセットＹ成分）は、第６軸４１のアーム回転中心位置からアーム先端に装着された負荷の重心位置までの３方向の距離を示す。また、図２において、アーム長Ｌｉｉ（第６軸４１のオフセット長１）とアーム長Ｌｊ（第６軸４１のオフセット長２）は、第５軸４０の回転中心から第６軸４１の回転中心までの２方向のアーム長を示す。なお、もう１方向のアーム長は、図３に示すように、第５軸４０と第６軸４１の回転中心が同一線上となる例を示しているので０としている。 
【００２２】
なお、これら寸法のうち、負荷の重心位置である距離Ｘｌ、距離Ｚｌ、距離Ｙｌが求めたい未知の数値である。そして、第５軸４０と第６軸４１に関するアーム長Ｌｉｉとアーム長Ｌｊは、ロボットの構成に基づいて予め知ることができる既知の数値である。
【００２３】
また、図４は図２の垂直座標系に相当する図であり、第５軸４０を回転中心としてアーム４８を回転させた状態を示す図である。図５は図３の水平座標系に相当する図であり、第６軸４１を回転中心として装着した負荷４４を回転させた状態を示す図である。なお、図４と図５において、（ａ）は初期位置を示しており、（ｂ）は負方向に回転した状態を示しており、（ｃ）は正方向に回転した状態を示している。そして、図４における符号４７は、図２で示す第４軸３９を回転中心とするアームの一部を示している。
【００２４】
さらに、図４と図５において、距離Ｌ５と距離Ｌ６は、第５軸４０の回転中心を原点とした時の負荷重心位置までの長さを示しており、図４や図５で示すように、アーム長Ｌｉｉ、アーム長Ｌｊ、負荷の重心位置である距離Ｘｌ、距離Ｚｌ、距離Ｙｌの合成長さで示される。
【００２５】
次に、上述したロボットを構成するアームの先端に備えられた負荷４４に関し、その質量と重心位置を算出する方法について説明する。
【００２６】
始めに、図２と図３に示す回転軸である第５軸４０と第６軸４１を中心に回転する各アームを１軸毎に等角速度運動させ、負荷質量に起因する重力トルクを求める方法について説明する。
【００２７】
図１は、本実施の形態における制御方法を実施するためのブロック図ある。そして、このブロック図で行われる制御は図示しないロボットコントローラ内で行われるものである。なお、ロボット全体としては図１と同様のブロック図が複数軸分存在するが、図１を代表としてモータ１軸分の制御について説明する。
【００２８】
図１において、６は位置制御ブロックであり、ロボット関節角指令θｒｃに減速比Ｒｇを乗じて計算されたモータ位置指令θｃｏｍとモータ位置フィードバックθｍ２の差分値から速度ループ指令ωｃｏｍを生成する。
【００２９】
図１の１０は速度制御ブロックであり、速度ループ指令ωｃｏｍとモータ速度フィードバックωｍの差分値と速度比例ゲインＫＰと速度積分ゲインＫＩとからモータ電流指令Ｉｍを生成する。
【００３０】
図１の１８はモータと外力を示したブロックである。ここで、減速機を剛体と仮定すると、モータ発生トルクτｍは、モータ駆動側から見れば式（数１）で表され、負荷側から見れば式（数２）で表される。
【００３１】
【数１】
2010076074.tif 000003
【００３２】
【数２】
2010076074.tif 000004
【００３３】
動摩擦トルクτμは以下の式で計算できる。
【００３４】
【数３】
2010076074.tif 000005
【００３５】
【数４】
2010076074.tif 000006
【００３６】
式（数２）における動力学トルクτｄｙｎは、低速で等角速度動作時には、角速度や角加速度に起因する慣性力、遠心力、コリオリ力は無視でき、重力トルク成分として近似できるので、以下の様に表すことができる。
【００３７】
【数５】
2010076074.tif 000007
【００３８】
なお、ロボットを構成するアーム等の部品の質量と形状は既知である。そして、ロボット本体アームに起因する重力トルクτｇＡ[θｒ]は、全軸のロボット関節角度が分かれば計算できる。
【００３９】
よって、図１のアーム重力トルク演算ブロック２０は、当該軸のロボット関節角θｒ３６と静止状態にある他軸のロボット関節角θｒ１９を入力し、ロボットを構成するアーム等の部品の質量と形状から、アームに起因する重力トルクτｇＡ[θｒ]を計算する。
【００４０】
低速等速動作時では、モータ角加速度αｍ＝０であり、式（数１）〜（数５）から負荷に起因する重力トルクτｇＬ[θｒ]２４を求める式を導出すると以下となる。
【００４１】
【数６】
2010076074.tif 000008
【００４２】
この重力トルクτｇＬ[θｒ]を求めるにあたり、図４と図５においてそれぞれ（ａ）〜（ｃ）で示すように第５軸４０、第６軸４１をそれぞれ１軸毎に正負方向に所定の角度Δθｒ４９だけ等角速度往復運動させ、各方向のモータ電流値Ｉｍとロボット関節角度θｒを測定する。
【００４３】
ここで、−方向(ｓｇｎ＝−１)に−ωｍ0で等角速度動作させた時の電流をＩｍ[θｒ]ｍとし、＋方向(ｓｇｎ＝１)にωｍ0で等角速度動作させた時のモータ電流をＩｍ[θｒ]ｐとして式（数６）を表すと、それぞれ以下の式（数７）、（数８）となる。
【００４４】
【数７】
2010076074.tif 000009
【００４５】
【数８】
2010076074.tif 000010
【００４６】
式（数７）、（数８）を加算してまとめると以下の様になる
【００４７】
【数９】
2010076074.tif 000011
【００４８】
ただし、
【００４９】
【数１０】
2010076074.tif 000012
【００５０】
つまり、同じ速度の低速で往復動作させ、負方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ[θｒ]mのデータを蓄積し、正方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ[θｒ]ｐのデータを蓄積する。そして、所定の角度Δθｒの往復動作の後で、同じ関節角度θｒ同士で正負方向の平均をとれば（足して２で割れば）、摩擦力に関係する項が除去され、粘性摩擦係数Ｄや動摩擦Ｋμの値を求める必要がなく、τｇＩ[θｒ]２３を求めることができる。
【００５１】
図１のデータ蓄積・摩擦トルク除去ブロック２２で上記処理を実行してτｇＩ[θｒ]２３を求める。そして、このτｇＩ[θｒ]２３からアーム重力トルク演算ブロック２０で求めたアーム重力トルクτｇＡ[θｒ]２１を減算して負荷に起因する重力トルクτｇＬ(θｒ)２４を計算する。
【００５２】
本実施の形態の方法では、後述するように、所定の位置とこの所定の位置から９０°動作した位置の２点の重力トルクを求め、トルクとアームの機構的特徴とから負荷の質量・重心位置を求める。
【００５３】
なお、所定の位置とは第５軸４０を回転中心とするアーム４８が地面と水平となる位置である。なお、第６軸４１については、初期位置は原点位置であり、すなわち軸の角度が０°の位置である。そして、実際に重力トルクを求める際には、所定の位置とそこから９０°動作させた位置を含むように各軸を等角速度で往復動作させて求める。
【００５４】
図６（ａ）は、第５軸４０を回転中心とするアーム４８が所定の位置である水平の位置にある状態を示している。図６（ｂ）はこの所定の位置から第５軸４０を中心にアーム４８を９０°回転動作させた場合のアーム４８の位置を示している。なお、図６（ｃ）はアーム４８の動作開始位置を示しており、図６（ｄ）はアーム４８の往路動作終了位置を示しており、アーム４８は図６（ａ）、図６（ｂ）の位置を含むように、図６（ｃ）の姿勢から図６（ｄ）の姿勢まで動作して、図６（ｃ）の位置へ戻るように動作する。そして、この動作中にアーム４８の水平位置における重力トルクとこの水平位置から９０°回転した位置における重力トルクを前述の（式１０）等により計算する。
【００５５】
第６軸４１を回転中心とする負荷４４についても第５軸４０を回転中心とするアーム４８と同様の動作を行う。図７（ａ）は負荷４４の初期位置を示しており、図７（ｂ）は初期位置から９０°回転動作した位置での負荷４４を示しており、負荷４４はこれらの位置を含むように、図７（ｃ）の姿勢から図７（ｄ）の姿勢へ回転移動後、図７（ｃ）の姿勢まで動作する。そして、この動作中に通過する所定の位置とこの所定の位置から９０°動作した位置での重力トルクを前述の（式１０）等により計算する。
【００５６】
以上により、図２や図３に示す第５軸４０や第６軸４１といった回転軸を中心に回転するアームを１軸毎に等角速度運動させ、各軸について所定の位置とこの所定の位置から９０°回転動作した位置の２点の負荷質量起因の重力トルクを求める方法を示した。
【００５７】
次に、求めた各軸２点の重力トルクを用いて、負荷質量Ｍと負荷の重心位置である距離Ｘｌ、距離Ｚｌ、距離Ｙｌを求める方法について説明する。
【００５８】
図８（ａ）は第５軸４０を回転中心とするアーム４８が所定の位置（水平）にある時の姿勢を示している。そして、図８（ｂ）は第５軸４０を回転中心とするアーム４８が所定の位置（水平）から正方向に９０°回転した時の姿勢を示している。また、図９（ａ）は第６軸４１を回転中心とする負荷４４の初期位置の姿勢を示している。そして、図９（ｂ）は第６軸４１を回転中心とする負荷４４が初期位置から負の方向に９０°回転した時の姿勢を示している。
【００５９】
取り付けた負荷４４の質量をＭ、重力加速度をｇとすれば、アーム４８が水平位置にある場合の重力に起因するトルクＴ１は、回転中心から負荷４４の重心位置までの長さを考慮すれば式（数１１）で表現され、アーム４８が水平位置から９０°回転した時の重力に起因するトルクＴ２は式（数１２）によって表現される。
【００６０】
なお、回転位置は所定の位置と所定の位置から９０°移動した位置であり、これらの位置は垂直の位置関係となるので、特許文献３に示すような複雑な計算となる三角関数の計算を含まない式（数１１）から式（数１４）に示すような式を導出することができる。
【００６１】
【数１１】
2010076074.tif 000013
【００６２】
【数１２】
2010076074.tif 000014
【００６３】
同様に考えて、第６軸４１を回転中心とする負荷４４の任意の初期位置での重力トルクＴ３は図９より（数１３）にて表現でき、初期位置から９０°回転後の重力トルクＴ４は（数１４）にて表現できる。
【００６４】
【数１３】
2010076074.tif 000015
【００６５】
【数１４】
2010076074.tif 000016
【００６６】
なお、一般的にロボット各軸の現状の角度は各軸に設けられたエンコーダ等の回転位置検出装置により検出しているので、アーム４８を水平位置にするためのアーム４８の角度（第５軸４０の角度）は検出でき、負荷４４が初期位置にある時の負荷４４の角度（第６軸４１の角度）は原点位置である０°となるので、前述の式（数１）から式（数１０）に基づいて負荷４４に起因する重力トルクＴ１、Ｔ２、Ｔ３、Ｔ４が計算できる。
【００６７】
さらに、式（数１２）と式（数１４）から
【００６８】
【数１５】
2010076074.tif 000017
【００６９】
となり負荷４４の質量Ｍが求められる。
【００７０】
さらに、式（数１５）により求めた質量Ｍを式（数１１）に代入して距離Ｘｌが求まり、式（数１３）に代入して距離Ｙｌが求まり、式（数１２）または式（数１４）に代入して距離Ｚｌが求まる。
【００７１】
なお、上記説明では、第５軸４０を正方向に９０°回転動作させ、第６軸４１を負方向に９０°回転動作させた場合について説明したが、第５軸４０を負方向に９０°回転動作させ、第６軸４１を正方向に９０°回転動作させた場合でも同様の結果を得ることができる。
【００７２】
以上のように、本実施の形態では、２つの回転軸についてそれぞれ所定の位置とその位置から９０°回転動作させた位置の２点において重力トルクを求めるだけで、簡便に負荷４４の質量や重心位置を求めることができる。
【００７３】
（実施の形態２）
実施の形態１の方法では、簡便に精度良く負荷質量・重心位置を求めることができる。しかしその反面、重力トルクの測定点が各軸２点のみと少ないため、測定点付近で外力を受けた場合には大きな誤差を発生する可能性がある。なお、外力とは、例えば、ロボットや装着負荷さらには装着負荷に取り付けられたケーブル等が測定動作中に周辺物に接触するような場合に発生する力のことである。
【００７４】
一方、背景技術で説明した特許文献３の方法では、複数点の重力トルクを計測して負荷の質量や重心位置を算出している。従って、実施の形態１の方法より外力の影響は少ないといえる。そこで、同時に実行可能である実施の形態１の方法と特許文献３の方法とを併用し、特許文献３の方法で算出した負荷の質量や重心位置を、実施の形態１で求めた負荷の質量や重心位置の判定値あるいは補正値として用いる例について説明する。
【００７５】
本実施の形態において、実施の形態１と同様の構成については同一の番号を付して詳細な説明を省略する。
【００７６】
図１０は本実施の形態における制御方法を実施するためのブロック図である。
【００７７】
実施の形態１と異なる点は、振幅・位相算出手段５４を備えており、負荷に起因する重力トルクの振幅・位相を算出する。さらに、重心位置・質量算出手段５７では、求めた重力トルクの振幅・位相から特許文献３に記載の方法で負荷の質量・重心位置を算出すると共に、実施の形態１で説明した方法でも負荷の質量・重心位置を算出し、両者の計算結果から最終的な負荷の質量・重心位置を算出している。
【００７８】
以下に、本実施の形態について説明する。
【００７９】
最初に負荷に起因する重力トルクの位相・振幅から負荷の質量・重心位置を求める方法について述べるが、これは特許文献３に記載の内容と同じである。
【００８０】
実施の形態１で式（数１）から式（数１０）までの導出方法は説明済みなので省略し、式（数１０）のτｇＩ[θr]２３を求める方法から説明する。
【００８１】
τｇＩ[θr]２３を求めるには、第５軸４０、第６軸４１を同じ速度の低速で往復動作させ、負方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ[θｒ]mのデータを蓄積し、正方向の場合はロボット関節角度θｒとその角度でのモータ電流値Ｉｍ[θｒ]ｐのデータを蓄積する。そして、所定の角度Δθｒの往復動作の後で、同じ関節角度θｒ同士で正負方向の平均をとることで求めることができる。
【００８２】
実際に上記内容を実行するには、所定の動作角Δθｒを２ｎ分割した間隔ｄｒでデータを記録して計算を行う。
【００８３】
【数１６】
2010076074.tif 000018
【００８４】
【数１７】
2010076074.tif 000019
【００８５】
所定の角度Δθｒしか動作させないため、τｇＩ[θｒ]は正弦波の一部となる。図１１はこの時のτｇＩ[θｒ]、 Ｋｇ・Ｒｇ・Ｉｍ[θｒ]、及びＫｇ・Ｒｇ・Ｉｍ[θｒ]ｐの波形の１例を示したものであり、Δθｒ＝３０°、ｄｒ＝１°（ｎ＝１５）である。
【００８６】
図１０のデータ蓄積・摩擦トルク除去ブロック２２で上記処理を実行してτｇＩ[θｒ]２３を求め、そこからアーム重力トルク演算ブロック２０で求めたアーム重力トルクτｇＡ[θｒ]２１を減算し、負荷に起因する重力トルクτｇＬ(θｒ)２４を計算する。すなわち、式（数９）を間隔ｄｒで計算し、図１０の振幅・位相算出手段５４でデータを保存する。
【００８７】
【数１８】
2010076074.tif 000020
【００８８】
アーム重力トルクτｇＡ[θｒ]も、当該軸のみの動作であるので、正弦波の一部となり、式（数１８）で計算される負荷重力トルクτｇＬ[θｒ[k]]も正弦波の一部となる。その振幅をＡ５[k]、位相φ５[k]とすると以下の式で表すことが出来る。
【００８９】
【数１９】
2010076074.tif 000021
【００９０】
次に、この負荷重力トルクτｇＬ(θｒ[k])２４の振幅Ａ５（５５）と位相φ５（５６）を求める手段を説明する。
【００９１】
図１０の振幅・位相算出手段５４においては、間隔ｄｒで２ｎ個保存された負荷重力トルクτｇＬ(θｒ[k])を、n個離れた（Δθｒ／２離れた）下記角度でペアリングを行う。
【００９２】
【数２０】
2010076074.tif 000022
【００９３】
図１２に、k=3とk=12の時のペアリングの例を示す。
【００９４】
ｋ=3の時   τｇＬ[θｒ1[3]]は６４、 τｇＬ[θｒ2[3]]は６５
ｋ=12の時  τｇＬ[θｒ1[12]]は６６、τｇＬ[θｒ2[12]]は６７
このペアリングされたデータが式（数１９）の関係を満足しているとすると、以下の様になる。
【００９５】
【数２１】
2010076074.tif 000023
【００９６】
余弦定理を用いて、式（数２１）を、φ５[k]、Ａ５[k]について解くと以下の式が得られる。
【００９７】
【数２２】
2010076074.tif 000024
【００９８】
式（数２２）において、所定の間隔Δθｒ／２を持つθｒ１[k]とθｒ２[k]の１組を計算すれば位相φ５[k]は求めることができるが、k=1〜nまでｎ個計算し、下式に示すようにｎ組の平均をとることで、推定精度を向上させることが出来る。
【００９９】
【数２３】
2010076074.tif 000025
【０１００】
上記、ｎ組の平均値φ５を式（数２１）に代入して、振幅Ａ５[k]を求めることが出来る。
【０１０１】
【数２４】
2010076074.tif 000026
【０１０２】
位相φ５と同様に、ｎ組の平均をとることで、振幅Ａ５の精度を向上させる。
【０１０３】
【数２５】
2010076074.tif 000027
【０１０４】
第６軸４１でも上記と同様の処理を行う。第５軸４０と同様、負荷重力トルクτｇＬ[θｒ[k]]は正弦波の一部となる。その振幅をＡ６[k]、位相φ６[k]とすると以下の式で表すことが出来る。
【０１０５】
【数２６】
2010076074.tif 000028
【０１０６】
式（数２６）で式（数１９）のｃｏｓと違ってｓｉｎで表しているのは、関節角度θｒ＝０°のとき、第６軸４１のアームが垂直方向に近い姿勢で保持されているので、τｇＬ［θｒ［ｋ］］も最小値に近いと考えられるからである。一方第５軸４０では関節角度θＬ＝０°のとき、駆動するアームが水平方向に近い姿勢で保持されているので、τｇＬ［θｒ［ｋ］］も最大値に近い。それ故、ｃｏｓで表示した。いずれで表示しても、位相が９０°ずれて表現されるだけであるので、特に問題ないが、鋭角で表現できる方を選択する方が、式（数２２）でのtan-1の処理が容易となる。
【０１０７】
式（数２１）に対応した式は以下の様になる。
【０１０８】
【数２７】
2010076074.tif 000029
【０１０９】
正弦定理を用いて、式（数２７）を、φ６、Ａ６について解くと以下の式が得られる
【０１１０】
【数２８】
2010076074.tif 000030
【０１１１】
【数２９】
2010076074.tif 000031
【０１１２】
【数３０】
2010076074.tif 000032
【０１１３】
【数３１】
2010076074.tif 000033
【０１１４】
以上、図２、図３に示す第５軸４０、第６軸４１の回転軸を中心に回転するアームを１軸毎に等角速度運動させ、各軸に印加される負荷質量起因のトルク（正弦波）の振幅とアーム基準角度からの位相を求める方法を示した。
【０１１５】
次に、負荷質量起因のトルク（正弦波）の振幅と位相を用いて、負荷質量Ｍ、重心位置である距離Ｘｌ，距離Ｚｌ，距離Ｙｌを求める方法について説明する。
【０１１６】
式（数２５）、式（数３１）で算出した振幅Ａ５、振幅Ａ６は、トルク成分ＴｇＬの最大値であるから、図２、図３、図４、図５で示すように、負荷の質量をＭ、重力加速度をｇとすると、トルク成分Ｔｇが最大値を取る時は負荷の重心が回転中心の水平方向にある時であるので、各回転中心から負荷の重心位置までの合成長さＬ５、Ｌ６と負荷による力を掛け
【０１１７】
【数３２】
2010076074.tif 000034
【０１１８】
【数３３】
2010076074.tif 000035
【０１１９】
で表される。
【０１２０】
なお、前述したように、合成長さＬ５は、負荷の重心位置である距離Ｘｌ、距離Ｚｌ、距離Ｙｌと第５軸４０と第６軸４１のアーム長Ｌｉｉとアーム長Ｌｊの合成により次式（数３４）のように表される。
【０１２１】
【数３４】
2010076074.tif 000036
【０１２２】
また、式（数３３）において、合成長さＬ６は負荷の重心位置の垂直成分である距離Ｚｌと負荷の重心位置の水平成分である距離Ｙｌの合成長さであり、
【０１２３】
【数３５】
2010076074.tif 000037
【０１２４】
で表される。
【０１２５】
また、位相差φ５およびφ６については、第５軸４０、第６軸４１のアーム角度とアーム配置の条件からそれぞれ、
【０１２６】
【数３６】
2010076074.tif 000038
【０１２７】
【数３７】
2010076074.tif 000039
【０１２８】
となる。なお、式（数３６）は、図４（ａ）で示す負荷位置が式（数１９）における関節角度θｒ＝０の時であり、この時の第５軸４０の回転中心と負荷重心位置とを結ぶ線が水平軸となす角度がφ５であることから導かれる。また式（数３７）は、図５（ａ）で示す負荷位置が式（数２６）における関節角度θｒ＝０の時であり、この時の第６軸４１の回転中心と負荷重心位置とを結ぶ線が鉛直軸となす角度がφ６であることから導かれる。
【０１２９】
さらに式（数３６）、（数３７）に式（数３２）、（数３３）を代入して整理すると、次式（数３８）、（数３９）が導かれる。
【０１３０】
【数３８】
2010076074.tif 000040
【０１３１】
【数３９】
2010076074.tif 000041
【０１３２】
ところで、式（数３８）、（数３９）は同一の負荷の重心位置について計算しているので、当然式（数３８）と式（数３９）のＺｌは等しい。従って、
【０１３３】
【数４０】
2010076074.tif 000042
【０１３４】
となる。
【０１３５】
さらに式（数４０）から負荷の質量Ｍを計算する関係式へと導出することができ、
【０１３６】
【数４１】
2010076074.tif 000043
【０１３７】
式が得られる。
【０１３８】
さらに式（数４１）にて算出した質量Ｍを式（数３８）もしくは式（数３９）へ代入することでＺｌが算出できる。
【０１３９】
また、算出したＺｌとロボットのアーム長さＬｉｉとＬｊを式（数３４）（数３５）へ代入して、Ｘｌ、Ｙｌは、式（数４２）、式（数４３）のようにして求められる。
【０１４０】
【数４２】
2010076074.tif 000044
【０１４１】
【数４３】
2010076074.tif 000045
【０１４２】
ここまでで、負荷質量起因のトルク（正弦波）の振幅と位相を用いて、負荷質量Ｍ、重心位置である距離Ｘｌ，距離Ｚｌ，距離Ｙｌを求める方法について説明した。
【０１４３】
この方法は実施の形態１で述べた負荷の質量・重心位置算出時の動き（所定の位置から所定の位置に９０°加えた位置までの往復動作）でも成り立つので、実施の形態１の負荷質量・重心位置算出方法との併用が可能である。
【０１４４】
重心位置・質量算出手段５７では、後述するように、実施の形態１の方法と特許文献３記載の方法のそれぞれで負荷の質量・重心位置を算出した後に、算出した負荷質量・重心位置を評価して最終的な負荷の質量・重心位置を決定している。
【０１４５】
以下に質量・重心位置導出手段５７における処理内容を説明する。
【０１４６】
実施の形態１の方法で求めた質量をＭ１、重心位置である距離をＸｌ１，Ｙｌ１，Ｚｌ１とし、特許文献３記載の方法において求めた質量をＭ２、重心位置である距離をＸｌ２，Ｙｌ２，Ｚｌ２とする。重心位置・質量算出手段２５ではまず、算出した質量・重心位置のそれぞれの差が所定の値以内にあるかどうかを判断する。
【０１４７】
【数４４】
2010076074.tif 000046
【０１４８】
【数４５】
2010076074.tif 000047
【０１４９】
【数４６】
2010076074.tif 000048
【０１５０】
【数４７】
2010076074.tif 000049
【０１５１】
なお、式（数４４）から（数４７）の条件を満たさない場合は、負荷の質量・重心位置の算出に失敗したと判断して処理を終了する。
【０１５２】
条件を満たした場合には、最終的な負荷の質量Ｍ、重心位置Ｘｌ、Ｙｌ、Ｚｌを次の式で算出する。
【０１５３】
【数４８】
2010076074.tif 000050
【０１５４】
【数４９】
2010076074.tif 000051
【０１５５】
【数５０】
2010076074.tif 000052
【０１５６】
【数５１】
2010076074.tif 000053
【０１５７】
ここで、式（数４８）から式（数５０）における加重平均の係数ａ、ｂの設定方法について説明する。
【０１５８】
図示しないロボットコントローラに搭載されているＣＰＵ処理能力が低く、特許文献３の方法、すなわち式(数１６)から式(数４３)で求めた質量・重心位置の算出精度が期待できない場合は、加重平均の係数ａ、ｂは「１」と設定する。つまり、特許文献３の方法を実施の形態１で算出した質量・重心位置の判定（式（数４４）〜（数４７）で実施）のみに使用する。
【０１５９】
図示しないロボットコントローラに搭載されているＣＰＵ処理能力が高く、特許文献３の方法で求めた質量・重心位置の算出精度が期待できる場合は、加重平均の係数ａ、ｂを「１」未満に設定することで、実施の形態１で求めた質量・重心位置を補正し、外力による算出精度劣化のリスクを減じることができる。
【０１６０】
すなわち、ロボットの設置状態により、外力を受ける可能性が低い場合は加重平均の係数ａ、ｂを「１」近くに設定し、外力を受ける可能性が増加するほど加重平均の係数ａ、ｂを「１」から減じれば良い。
【０１６１】
なお、加重平均による補正を、式(数４８)で算出される質量Ｍで留め、この値を用いて重心位置を再計算してもよい。具体的には質量Ｍを式（数１２）、式（数１４）、式（数３８）、式（数３９）のいずれかに代入することで距離Ｚｌを算出し、続けて式（数１３）または式（数４２）に代入すれば距離Ｙｌが、式（数１１）または式（数４３）に代入すれば距離Ｘｌが算出できる。
【産業上の利用可能性】
【０１６２】
本発明は、モータにより駆動されるロボットの負荷の重量と負荷の重心位置を精度良く算出する制御方法を提供するものであり、衝突検出機能の高感度化を実現し、衝突時の安全性向上や破壊による損失を防止する。また、ロボットの実タクト性能を向上でき、ロボットの利用分野の拡大とバリ取り作業や、複雑な形状部品の嵌め合い作業など各種使用用途に利用できる。
【図面の簡単な説明】
【０１６３】
【図１】本発明の実施の形態１における制御方法を実施するためのブロック図
【図２】本発明の実施の形態におけるロボットを構成する駆動軸の垂直座標系における配置の一例を示す図
【図３】本発明の実施の形態におけるロボットを構成する駆動軸の水平座標系における配置の一例を示す図
【図４】一方の駆動軸が正負方向の等加速運動を行う一例を示す図
【図５】もう一方の駆動軸が正負回転方向の等加速運動を行う一例を示す図
【図６】本発明の実施の形態における一方の駆動軸の動作範囲の一例を示す図
【図７】本発明の実施の形態におけるもう一方の駆動軸の動作範囲の一例を示す図
【図８】本発明の実施の形態における一方の駆動軸が正方向に９０度回転した時の一例を示す図
【図９】本発明の実施の形態におけるもう一方の駆動軸が正方向に９０度回転した時の一例を示す図
【図１０】本発明の実施の形態２における制御方法を実施するためのブロック図
【図１１】図４の等速運動を行った場合のモータ発生トルクの一例を示す図
【図１２】図４の等速運動を行った場合における負荷が起因するトルク成分の特性の一例を示す図
【符号の説明】
【０１６４】
θｒｃ  関節角度指令  
２  モータ回転角  θｍ
θｃｏｍ  モータ回転角指令
ωｍ  モータ角速度  
５  位置比例ゲイン  ＫＰＰ
６  位置制御ブロック
ωｃｏｍ  モータ角速度指令
ＫＰ  速度比例ゲイン
ＫＩ  速度積分ゲイン
１０  速度制御ブロック
Ｉｍ  モータ電流指令
１２  モータトルク定数  Ｋｔ
τｍ  モータ発生トルク
τμ  摩擦トルク  
１５  微分要素
τｄｙｎ  動力学トルク
１７  モータ伝達関数
１８  モータと外力を示したブロック
１９  他軸関節角  θｒ
２０  アーム重力トルク演算ブロック
２１  ロボット本体アームに起因する重力トルク  τｇＡ[θｒ]
２２  データ蓄積・摩擦トルク除去ブロック
２３  モータ発生トルク重力成分  τｇＩ[θｒ]
２４  ロボットに装着された負荷に起因する重力トルク  τｇＬ[θｒ]
２５  重心位置・質量算出手段
Ｘｌ  距離（負荷オフセットＸ成分）
Ｚｌ  距離（負荷オフセットＺ成分）
Ｙｌ  距離（負荷オフセットＹ成分）
２９  負荷質量Ｍ
３０  負荷質量・オフセット推定ブロック
３１  減速比逆数  １／Ｒｇ
３２  トルク定数、減速比の積
３３  関節角  θｒ
３４  減速比  Ｒｇ
３５  他軸のロボットに装着された負荷に起因する重力トルク τgL[θr]
３６  第１軸
３７  第２軸
３８  第３軸
３９  第４軸
４０  第５軸 
４１  第６軸
４２  第６軸オフセット長１  Ｌｉｉ
４３  第６軸オフセット長２  Ｌｊ
４４  負荷 
４７  第４軸の一部
４９  測定動作角  Δθｒ（第５軸）
５０  測定動作角  Δθｒ（第６軸）
５３  負荷が重力によってうける力  ｍ・ｇ
５４  振幅・位相算出手段
５５  負荷重量トルク振幅 Ai
５６  負荷重力トルク位相 φi
５７  重心位置・質量算出手段２
５８  他軸負荷重量トルク振幅 Ai
５９  他軸負荷質量トルク位相 φi
６０  正方向モータ発生トルク  Ｋｔ・Ｒｇ・Ｉｍ[θｒ]ｐ
６１  負方向モータ発生トルク  Ｋｔ・Ｒｇ・Ｉｍ[θｒ]ｍ
６２  データ測定間隔  ｄｒ
６３  データペアリング間隔  Δθｒ／２
６４  負荷起因トルク計測値  （k=3）  τｇＬ[θｒ1[3]]
６５  ６４のペアリングデータ（k=3）  τｇＬ[θｒ2[3]]
６６  負荷起因トルク計測値  （k=12）  τｇＬ[θｒ1[12]]
６７  ６０のペアリングデータ（k=12）  τｇＬ[θｒ2[12]]
【図１】
2010076074.tif 000054
【図２】
2010076074.tif 000055
【図３】
2010076074.tif 000056
【図１０】
2010076074.tif 000057
【図１１】
2010076074.tif 000058
【図１２】
2010076074.tif 000059
【図４】
2010076074.tif 000060
【図５】
2010076074.tif 000061
【図６】
2010076074.tif 000062
【図７】
2010076074.tif 000063
【図８】
2010076074.tif 000064
【図９】
2010076074.tif 000065