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