(57)【要約】      （修正有）【課題】  ロボットの各軸モータトルク値とアーム角度からロボットに備える負荷の質量と重心位置を算出する方法においてはモータトルク値とアーム角度の測定をパラメータの数だけ行う必要があるが、ロボットの動作範囲が制限される場合や動作自体が制限される場合においても有効な負荷の質量と重心位置を算出するロボットの方法を提供する。【解決手段】回転中心軸が直交する２軸を含む複数のアームのモータトルクとアーム角度を計測し、これより算出される負荷に起因するトルク成分から回転中心軸が直交する幾何学的関係を導出し、負荷の質量及び重心位置を算出する制御方法。【選択図】  図１
2006075931.tif 000002
【特許請求の範囲】
【請求項１】
モータにて駆動され、回転中心軸が略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させた時のモータトルクを算出し、算出した前記モータトルクから前記アームと前記負荷の質量の両方に起因するトルク成分を各回転軸ごとに求め、求めた前記トルク成分の極大値と、前記トルク成分が０となるアーム関節角度とを各回転軸ごとに計算して求め、前記負荷の質量及び重心位置を算出するロボットの制御方法。
【請求項２】
モータにて駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させた時のモータトルクを算出し、算出した前記モータトルクから前記アームと前記負荷の質量の両方に起因するトルク成分を各回転軸ごとに求め、求めた前記トルク成分の極大値と、前記トルク成分が０となるアーム関節角度とを各回転軸ごとに計算して求め、前記負荷の質量及び重心位置を算出するロボットの制御方法。
【請求項３】
アームを１軸毎に等角速度運動する時、モータを双方向に回転させ、各回転方向についてモータトルクを算出する請求項１または２記載のロボットの制御方法。
【請求項４】
モータを双方向に回転させる時、算出するモータトルクを監視し、前記モータトルクが極大または０となった後でモータ回転方向を反転させる請求項３記載のロボットの制御方法。
【請求項５】
モータトルクが極大となった後でモータ回転方向を反転させた場合、反転後モータトルクが０となった後で回転を停止させる請求項４記載のロボットの制御方法。
【請求項６】
モータトルクが０となった後でモータ回転方向を反転させた場合、反転後モータトルクが極大となった後で回転を停止させる請求項４記載のロボットの制御方法。
【請求項７】
アームを１軸毎に等角速度運動する場合、モータトルクの算出をモータ電流値の計測により行う請求項１から６のいずれかに記載のロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、モータにより駆動されるロボットの負荷の重量と負荷の重心位置を算出する制御方法に関する。
【背景技術】
【０００２】
本発明は、モータにより駆動されるロボットの負荷の重量と負荷の重心位置を算出する制御方法に関する。
【０００３】
近年、モータにより駆動される複数軸のアームを備えるロボットにおいて、衝突時の安全性向上や破壊による損失防止のために、衝突検出の高精度化が求められている。
【０００４】
また、ロボットの実タクト性能の更なる向上を目的とした加減速運動性能や振動抑制制御性能の高性能化や、ロボットの利用分野の拡大とバリ取り作業や、複雑な形状部品の嵌め合い作業など各種使用用途の要求を満足することを目的とした、柔軟制御の実現が求められている。
【０００５】
このような用途を実現するために、各用途での制御方法においてはロボットのアーム、およびアーム先端に装着される負荷の質量と重心位置を必要とする。このうち、ロボットが備えるアームの質量と重心位置は、設計時に使用されたＣＡＤ等の設計ツールによって予め同定することが可能である。しかし、ロボットのアーム先端へ任意の負荷が装着される場合では、別途負荷の質量と重心位置の値を知ることが必要となる。
【０００６】
従来では、制御方法への負荷の質量と重心位置を知る方法としては、ロボットコントローラよりロボットの操作者がそれらの値を手入力する方法がある。
【０００７】
また手入力しない方法として、ロボットが備えるモータ各軸の電流やトルクや角度を測定し、負荷の質量と重心位置を算出とする方法があった。この方法においては、ロボットを静止させて行う方法とロボットのモータ各軸を駆動させて行う２つの方法があった。
【０００８】
このうち、ロボットを静止させて行う方法では、ロボットの静止状態におけるモータ各軸の電流値から算出したトルクを用いて、ロボット単体での重力に起因するトルク成分と静止時の摩擦トルクを引くことにより負荷の重力に起因するトルクだけを抽出し、負荷の質量と重心位置を算出する方法（例えば特許文献１参照）がある。
【０００９】
また、ロボットのモータ各軸を駆動させて行う方法では、ロボットのモータ各軸を駆動させる時の各軸モータトルクとアーム角度を、算出しようとする負荷の質量と重心位置の数だけ立式された重力に起因する関係式へ代入することによって負荷の質量と重心位置を算出する方法（例えば特許文献２参照）がある。
【特許文献１】特開平９−９１００４号公報（図１）
【特許文献２】特開平１１−４８１８１号公報（図１）
【発明の開示】
【発明が解決しようとする課題】
【００１０】
しかしながら、上記した制御方法への負荷の質量と重心位置を知る従来の方法では、ロボットの操作者は負荷の質量と重心位置を予め算出や測定によって求めておく必要があり
、作業者の作業負担や入力忘れによる弊害発生があった。
【００１１】
また手入力しない方法として、上記したロボットを静止させて行う方法では、静止時の摩擦トルクは負荷の質量やロボットの姿勢によって大きく変動することから、正確な値を推定することが難しく、算出される負荷の質量と重心位置の精度に大きく影響する可能性があった。
【００１２】
また、ロボットのモータ各軸を駆動させて行う方法では、負荷の質量と重心位置を算出するために、４つの独立な計算式を備えることに起因して、主要駆動軸を含む４つのモータトルク値とアーム角度の測定を行う必要があった。そのため、ロボットの動作範囲が制限される場合や動作自体が制限される場合では、負荷の質量と重心位置を算出することができない可能性があった。
【００１３】
以上のように従来の負荷の質量と重心位置の推定方法では、それぞれ少ない動作において負荷の質量と重心位置を高精度に推定するという点で問題があった。
【００１４】
そこで本発明では、ロボットに取り付けられた負荷の質量と重心位置を、少ない動作において高精度に推定することによって、少ない動作しかできない状況下においても負荷の質量と重心位置に基づいた制御を可能とする制御方法を提供することを目的とする。
【課題を解決するための手段】
【００１５】
本発明のロボットの制御方法は、モータにて駆動され、回転中心軸が略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させた時のモータトルクを算出し、算出した前記モータトルクから前記アームと前記負荷の質量の両方に起因するトルク成分を各回転軸ごとに求め、求めた前記トルク成分の極大値と、前記トルク成分が０となるアーム関節角度とを各回転軸ごとに計算して求め、前記負荷の質量及び重心位置を算出する。
また、本発明のロボットの制御方法は、モータにて駆動され、２つの回転中心軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸と略直交する２つの回転軸を少なくとも１組有する複数のアームを備え、前記２つの回転軸のうちアーム先端側の回転軸を中心に回転するアームに負荷を装着して用いるロボットの制御方法であって、前記２つの回転軸を中心に回転する前記各アームを１軸毎に等角速度運動させた時のモータトルクを算出し、算出した前記モータトルクから前記アームと前記負荷の質量の両方に起因するトルク成分を各回転軸ごとに求め、求めた前記トルク成分の極大値と、前記トルク成分が０となるアーム関節角度とを各回転軸ごとに計算して求め、前記負荷の質量及び重心位置を算出する。
【００１６】
また本発明のロボットの制御方法は、アームを１軸毎に等角速度運動する時、モータを双方向に回転させ、各回転方向についてモータトルクを算出する。
【００１７】
また本発明のロボットの制御方法は、モータを双方向に回転させる時、算出するモータトルクを監視し、前記モータトルクが極大または０となった後でモータ回転方向を反転させる。また本発明のロボットの制御方法は、モータトルクが極大となった後でモータ回転方向を反転させた場合、反転後モータトルクが０となった後で回転を停止させる。また、本発明のロボットの制御方法は、モータトルクが０となった後でモータ回転方向を反転させた場合、反転後モータトルクが極大となった後で回転を停止させる。
【００１８】
さらに本発明のロボットの制御方法は、アームを１軸毎に等角速度運動する場合、モー
タトルクの算出をモータ電流値の計測により行う。
【発明の効果】
【００１９】
本発明のロボット制御方法は、質量、重心位置とも未知の負荷の数値を極めて容易に知ることができ、得られた数値を制御方法へ用いることによって柔軟制御の実現や衝突検出の高精度化、実タクト性能の更なる向上を行うことができるという効果がある。
【発明を実施するための最良の形態】
【００２０】
以下、本発明のロボットの制御方法を示す好ましい実施例について、図面を参照しながら説明する。
（実施の形態１）
図１は、本実施の形態における制御方法を実施するためのブロック図である。なお、ロボット全体としては図１と同様のブロック図が複数軸分存在するが、図１を代表としてモータ１軸分の制御について示す。
【００２１】
図１において１はコントローラ、２はコントローラ１により制御されるモータ＋アームを示す。そして、コントローラ１は、モータ＋アーム２からアームを駆動するモータの駆動時の電流値ＩＭとモータ回転角θＭの値を得ることができる。さらにコントローラ１内の制御器（図示せず）に内蔵されたメモリ４には、負荷を装着しない状態で制御対象となるアームの質量のみに起因するトルク成分Ｔｇａｉが保存されている。
【００２２】
なおここでトルク成分Ｔｇａｉの末尾ｉは軸数を示し、本実施の形態では６軸ロボットの例を示すので、ｉは１〜６のいずれかをとる。以下も同様である。
【００２３】
そしてこのトルク成分Ｔｇａｉは軸に固有でアームの質量のみに起因し、ロボットアームの構成に依存する固有のものであるので予め既知の数値である。この数値をメモリ４に予め保存しておき、後述するように、測定して得られるアーム+負荷の質量に起因するトルク成分と演算して最先端軸に装着した負荷の質量と重心位置を求める。
【００２４】
また１３は重力トルク算出手段であり、モータ＋アーム２から得る電流値ＩＭとモータ回転角θＭの値からアーム及び負荷の質量に起因するトルク成分Ｔｇａｌｉを求める。さらに、求めたトルク成分Ｔｇａｌｉからトルク成分Ｔｇａｉを差し引いて、負荷のみが起因するトルク成分Ｔｇｉを得る。さらに振幅・位相差算出手段１７により、トルク成分Ｔｇｉの振幅Ａｉおよび位相差φｉを求め、最後に重心位置・質量算出手段１８により、第６軸のアーム回転中心位置からアーム先端に装着する負荷の重心位置までの３方向の距離Ｘｌ、Ｙｌ、Ｚｌと質量ｍとを求める。
【００２５】
なお本実施の形態では、垂直座標系において表現される図２、水平座標表現において表現される図３のような回転中心軸を有するアームを備えたロボットについて説明する。また本実施の形態では、図２、図３で示すように、アーム先端側の２つの回転軸第５、６軸の回転中心軸は、ねじれの位置にあり、軸間を最短に結ぶ直線と一方の回転中心軸とがなす平面が他方の回転中心軸とが略直交して配置されたロボットの例を示している。なお、この本実施の形態における回転中心軸はねじれの位置にある例を示しているが、これ以外に直交する場合であっても以下の説明は成り立つので、本実施の形態では、説明の簡略化のため転軸がねじれの位置にある場合も回転軸が直交しているとして説明する。
【００２６】
なお、図２および図３において、Ｘｌ、Ｚｌ、およびＹｌは、第６軸のアーム回転中心位置からアーム先端に装着する負荷の重心位置までの３方向の距離を示す。また、図２において、ＬｉｉとＬｊは、第５軸２４の回転中心から第６軸２５の回転中心までの２方向のアーム長を示す。なおもう１方向のアーム長は、図３において、第５軸２４と第６軸２
５が回転中心を同一線上に有する例で示しているので０としている。
【００２７】
なお、これら寸法のうち、負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌが求めたい未知の数値であり、第５軸２４と第６軸２５のアーム長ＬｉｉとＬｊは予め既知の数値である。
【００２８】
また、図４は、図２の垂直座標系に相当する図で第５軸回転中心として回転するアーム１２４を回転させた状態を示す図であり、図６は図３の水平座標系相当する図で第６軸に装着した負荷２８を回転させた状態を示す図であり、各図とも（ａ）（ｂ）（ｃ）は各回転位置状態を示す。なお、１２３は図２で示す第４軸２３のアームの一部を示している。
【００２９】
さらに図４および図６において、Ｌ５、Ｌ６は、第５軸２４の回転中心を原点とした時の負荷重心位置までの３方向の長さを示し、図で示すように、アーム長Ｌｉｉ、Ｌｊ、および負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌの合成長さで示される。
【００３０】
次に、上記で説明したようなアーム先端に備えられた負荷について、その質量と重心位置の算出方法について説明する。
【００３１】
この算出にあたっては、まずモータがアームを駆動させるのに必要とするモータトルクを算出する。ここで、算出しようとするモータトルクは、図１のモータ＋アーム２におけるトルクＴＭに相当し、このトルクＴＭを算出するためにはモータ電流値ＩＭを測定し、この測定値と重力トルク算出手段１３にて予め保存していたトルク換算値Ｋｔを用いて換算する。
【００３２】
なお図１中、モータ＋アーム２におけるブロック線図においては、トルク量を中心とした制御量の受け渡しが記載されており、この図では、検出したモータトルクＴＭからトルク換算値の逆数１／Ｋｔ８と演算してモータ電流値ＩＭを求める図となっている。しかし、実際の構成においては、最初にモータ電流を測定し、コントローラ１側でその電流値を得、それを元にモータトルクの算出を行う。
【００３３】
このようしてモータトルクＴＭを測定することができる。なお、求めたモータトルクＴＭは、図１中で示すモータ＋アーム２中の摩擦トルクＴｆｒｃｉを含むアーム及び負荷の質量に起因するトルク成分である。したがって、モータトルクＴＭからアームおよび装着した負荷の両方の質量に起因するトルク成分のみを求めるには、この摩擦トルクＴｆｒｃｉを差し引く必要がある。その方法について説明する。
【００３４】
このトルクＴＭを求めるにあたっては、図４と図６でそれぞれ（ａ）〜（ｃ）で示すように第５軸２４、第６軸２５をそれぞれ１軸毎に正負方向に等角速度往復運動させ、各方向のモータ電流値ＩＭとモータ回転角θＭを測定する。そして、この時測定したモータ電流値ＩＭはトルクＴＭの算出に用い、モータ回転角θＭは、後述する負荷２８の質量と重心位置の算出に用いる。
【００３５】
ここで図４は、図２の垂直座標系におけるロボットの第４軸のアームの一部から先の部分のみを、先端に負荷を装着した状態で示す図で、（ａ）は基本位置、（ｂ）は負方向に回転させた位置、（ｃ）は正方向に回転させた位置における図を示す。そしてトルクＴＭを求めるにあたっては、図４で示すように、第５軸２４を１軸毎に正負方向それぞれに等角速度往復運動させ、それぞれのモータ電流値ＩＭとモータ回転角θＭを測定する。
【００３６】
同様に図６は、図３の水平座標系におけるロボットの第６軸２５部分のみを先端に負荷を装着した状態で示す図で、（ａ）は基本位置、（ｂ）は正方向に回転させた位置、（ｃ）は負方向に回転させた位置における図を示し、第６軸２５を１軸毎に正負方向それぞれ
に等角速度往復運動させ、それぞれのモータ電流値ＩＭとモータ回転角θＭを測定する。
【００３７】
なお図２および図３で示すようにトルクＴＭの測定に用いる第５軸２４および第６軸２５は、負荷を取り付けた状態のアームを回転させる回転軸であり、その回転軸は直交している。
【００３８】
また、同時に求めるモータ回転角θＭは、アーム角度換算値Ｒｇの逆数１／Ｒｇ１１を用いてアーム角度θＬに換算する。なおアーム角度換算値Ｒｇは、モータとアームとの間に接続される機器であって、アームがモータに直接接続される場合であれば１である。
【００３９】
次に、算出したモータトルクＴＭから、モータトルクＴＭに含まれる摩擦トルク成分Ｔｆｒｃｉを相殺し、アーム及び負荷の質量に起因するトルク成分Ｔｇａｌｉだけを求める。この摩擦トルク成分Ｔｆｒｃｉは、モータの回転方向に依存する特徴をもつ。そこで、はじめにモータ角度が正回転となる動作と負回転となる動作を行って各場合のモータトルクＴＭを測定し、両方の値から演算してモータトルクＴＭに含まれる摩擦トルク成分Ｔｆｒｃｉを相殺する。
【００４０】
なお、トルクＴＭの測定時にモータの動作範囲が限られる場合では、常にトルクＴＭの値を監視し、まず一方向に回転させ、トルクＴＭの値が極大あるいは０となった後で軸回転方向を反転させる。そしてトルクＴＭの値が極大になった後反転させた場合は、反転後のトルクＴＭが０となった後で回転を停止させる。またトルクＴＭの値が０になった後反転させた場合は、反転後のトルクＴＭが極大となった後で回転を停止させる。
【００４１】
これについて図５および図７を用いてさらに詳細に説明する。図５および図７はコントローラ１で得られるモータ電流値ＩＭをアームの回転角度について測定したデータの一例を示す図であり、正回転および負回転時の両方について示している。なお、上述したように、トルクＴＭはモータ電流値Ｉに定数（トルク換算値Ｋｔ）を乗算したものであるので、トルクＴＭも図中のモータ電流と同様の図となる。
【００４２】
そして、まず、図５中のａ点のアーム角度位置からスタートして第５軸を正回転させ、モータ電流を測定する。そして測定したモータ電流が極大となった直後のｂ点で反転し、負回転させる。次にモータ電流が０となる点を通過したｃ点付近のアーム角度位置で動作を停止させる。
【００４３】
なお、上記求め方とは反対に、アームを正回転させてからモータ電流が０となった直後に反転し、負回転させてから極大となった直後に停止する方法でもよい。また正回転、負回転の順序は入れ替わってもよい。
【００４４】
このようにしてアーム１回転分の１／４周期のモータ電流データを得る。なお、アーム１回転分のモータ電流データを測定せず、約１／４周期の回転で止めるのは、後述するように、本実施の形態ではアーム回転時の軸にかかるトルクを正弦波とし、振幅、位相差を求めるのに少なくとも約１／４周期必要であるためである。これにより、アームを３６０度回転させなくてもよいので動作範囲が制限されている場合でも十分測定可能である。
【００４５】
次に、測定したアーム正回転時と負回転時のモータ電流から摩擦トルク成分Ｔｆｒｃｉを除去する。その方法は、正回転動作と負回転動作でのアーム角度が等しい位置において、正回転トルクＴｉ＋（図５は第５軸についてのグラフであるので第５軸についての正回転トルクはＴ５となる）と負回転トルクＴｉ−（同Ｔ５−）の和を２で割ることによって摩擦トルク成分Ｔｆｒｃｉを除去する。そしてこの摩擦トルク成分Ｔｆｒｃｉを除去すると、図中で示すように、前記アーム及び負荷の質量に起因するトルク成分Ｔｇａｌｉ（第
５軸の場合Ｔｇａｌ５）を抽出することができる。このトルク成分Ｔｇａｌｉが、アーム及び負荷の両方の質量に起因するトルク成分である。
【００４６】
また、等角速度運動の範囲を広く取れない場合では、抽出できるトルク成分Ｔｇａｌｉのデータの数が少なくなり、後述する負荷のみが起因するトルク成分Ｔｇｉを抽出する時に誤差を含む可能性がある。そこで既に説明したように、範囲が狭くなる場合では、１／４周期の限られた等角速度運動におけるモータトルクＴＭから、摩擦トルク成分Ｔｆｒｃｉを相殺し、最小２乗推定法を用いて、トルク成分Ｔｇａｌｉがもつ振幅と位相を算出し、近似式による算出値を用いることでトルク成分Ｔｇａｌｉの抽出を実現できる。
【００４７】
次に、抽出したトルク成分Ｔｇａｌｉから、アームのみが起因するトルク成分Ｔｇａｉを取り除く。このアームのみが起因するトルク成分Ｔｇａｉは、既に説明したように、ロボット固有で予め既知のものであり、制御器内蔵のメモリ４へ格納されている。
【００４８】
ここで、このトルク成分Ｔｇａｉのメモリ４への格納方法ついて説明する。このトルク成分Ｔｇａｉは、アームの角度位置で異なり、各角度位置についてのデータを格納しておくようにする。なお、これ以外に、メモリ４内に、演算に用いる係数のみを保存しておき、決められた計算式にしたがってトルク成分Ｔｇａｉを求める方法であってもよい。
【００４９】
そして、トルク成分Ｔｇａｌｉから、アーム角度θＬが同じ角度位置でのトルク成分Ｔｇａｉを取り除き、負荷のみが起因するトルク成分Ｔｇｉを算出する。
【００５０】
この算出においては、少なくともアーム角度θＬの絶対値が異なる２点において計算を実施し、前記２点における、負荷の質量のみが起因するトルク成分Ｔｇｉを算出する。
【００５１】
さらに前記２点において算出されたトルクＴｇｉと、その際のアーム角度θＬから、最小２乗推定法を用いて、トルク成分Ｔｇｉの振幅値Ａｉと位相差φｉを算出する。
【００５２】
なお、最小２乗推定法にて用いる近似関数においては、アーム角度に対するアーム姿勢の関係によって用いる数式が異なる。すなわち、アーム角度θＬ＝０°のとき、駆動するアームが水平方向に近い姿勢で保持されている場合は、トルク成分Ｔｇｉは（数１）のように示される。
【００５３】
【数１】
2006075931.tif 000003
また、アーム角度θＬ＝０°のとき、駆動するアームが垂直方向に近い姿勢で保持されている場合は、トルク成分Ｔｇｉは（数２）のように示される。
【００５４】
【数２】
2006075931.tif 000004
となり、各条件により式を使い分ける。
【００５５】
因みに図２における、第５軸２４の関数は、（数１）を用い、図３における第６軸２５の関数は（数２）を用いる。
【００５６】
また、図８と図９は、第６軸２５と第５軸２４の負荷のみが起因するトルク成分Ｔｇｉ
をプロットしたグラフである。各図でＡ６およびＡ５はそれぞれ第６軸、第５軸におけるトルク成分Ｔｇｉの振幅値を示し、φ５、φ６はそれぞれ第６軸、第５軸におけるトルク成分Ｔｇｉの位相差を示す。
【００５７】
なお、ここで位相差φｉは、負荷の重心位置が全て０ではない場合では上式（数１）または（数２）によって算出されるトルク成分Ｔｇｉを０もしくは極大値とする時のアーム角度θＬは０以外の値を持つ。すなわち位相差φｉは０以外の値を持つ。
【００５８】
そして、図２と図３で示されるロボットの第６軸２５の場合、図８に示すように振幅はＡ６、位相差はφ６が測定されると、これらの値を式（数２）に代入してアームに装着した負荷のみが起因するトルク成分Ｔｇ６は、
【００５９】
【数３】
2006075931.tif 000005
となる。（ただしθ６は第６軸のアームの回転角度を示す変数。）
また同様に、第５軸２４の場合、負荷のみが起因するトルク成分Ｔｇ５は、図９に示すように振幅はＡ５、位相差はφ５であることが測定され、この軸の場合は、式（数１）に代入して
【００６０】
【数４】
2006075931.tif 000006
となる。（ただしθ５は第５軸のアームの回転角度を示す変数。）
ところで、上式（数３）、（数４）においてＡ５、Ａ６は、トルク成分Ｔｇの最大値であるから、図２、図３、図４、および図５で示すように、負荷の質量をｍ、重力加速度をｇとすると、トルク成分Ｔｇが最大値を取る時は負荷の重心が回転中心の水平方向にある時であるので、各回転中心から負荷の重心位置までの合成長さＬ５、Ｌ６と負荷による力を掛け
【００６１】
【数５】
2006075931.tif 000007
【００６２】
【数６】
2006075931.tif 000008
で表される。
なお、前述したように、合成長さＬ５は、負荷の重心位置Ｘｌ、Ｚｌ、Ｙｌと第５軸２４と第６軸２５のアーム長ＬｉｉとＬｊの合成により  次式（数７）のように表される。
【００６３】
【数７】
2006075931.tif 000009
また、式（数６）において、Ｌ６は負荷の重心位置の垂直成分Ｚｌと負荷の重心位置の
水平成分Ｙｌの合成長さであり、
【００６４】
【数８】
2006075931.tif 000010
で表される。
【００６５】
また、位相差φ５およびφ６については、第５軸２４、第６軸２５のアーム角度とアーム配置の条件からそれぞれ、
【００６６】
【数９】
2006075931.tif 000011
【００６７】
【数１０】
2006075931.tif 000012
となる。なお、式（数９）は、図４（ａ）で示す負荷位置が式（数３）におけるアーム角度θ５＝０の時であり、この時の第５軸の回転中心と負荷重心位置とを結ぶ線が水平軸となす角度がφ５であることから導かれる。また式（数１０）は、図６（ａ）で示す負荷位置が式（数４）におけるアーム角度θ６＝０の時であり、この時の第６軸の回転中心と負荷重心位置とを結ぶ線が鉛直軸となす角度がφ６であることから導かれる。
さらに式（数９）、（数１０）に式（数５）、（数６）を代入して整理すると、次式（数１１）、（数１２）が導かれる。
【００６８】
【数１１】
2006075931.tif 000013
【００６９】
【数１２】
2006075931.tif 000014
ところで、式（数１１）、（数１２）は同一の負荷の重心位置について計算しているので、当然式（数１１）と式（数１２）のＺｌは等しい。従って、
【００７０】
【数１３】
2006075931.tif 000015
となる。
【００７１】
さらに式（数１３）から負荷の質量ｍを計算する関係式へと導出することができ、
【００７２】
【数１４】
2006075931.tif 000016
式が得られる。
【００７３】
さらに式（数１４）にて算出した質量ｍを式（数１１）もしくは式（数１２）へ代入することでＺｌが、算出できる。
【００７４】
また算出したＺｌとロボットのアーム長さＬｉｉとＬｊを式（数７）（数８）へ代入して、Ｘｌ、Ｙｌは、式（数１５）、式（数１６）のようにして求められる。
【００７５】
【数１５】
2006075931.tif 000017
【００７６】
【数１６】
2006075931.tif 000018
によって算出することができる。
【００７７】
以上で説明したように、回転中心軸が直交する２軸のアームを駆動するモータの駆動時の電流値ＩＭとモータ回転角θＭを測定することにより、負荷のみが起因するトルク成分の関係式数３、数４、数５、数６を得ることができ、ロボットの駆動軸の配置によって導出することができる関係式数７、数８、数９、数１０を用いることによって質量および重心位置が未知の負荷の質量ｍ、重心位置Ｘｌ、Ｚｌ、Ｙｌを算出することができた。
【００７８】
本実施の形態においては、既に説明したように、ロボットが備えるアーム及び負荷の質量に起因するトルク成分を監視し、トルク成分の大きさが極大あるいは零となった後で軸回転方向を反転させ、トルク成分の大きさが極大になり反転させる場合はトルク成分が零となった後で回転を停止させ、トルク成分の大きさが零になり反転させる場合はトルク成分が極大となった後で回転を停止させることにより、等角速度運動を必要最小限とする時のモータ電流を計測する。そして、これを換算することで得られる回転中心軸が異なる２軸のモータトルクから得られる振幅値と位相差からロボットが持つモータの回転中心軸が異なる特性を用いて連立方程式を導出する。さらに連立方程式より先端負荷の質量と重心位置を算出し、制御方法へ用いることによって柔軟制御の実現や衝突検出の高精度化、実タクト性能の更なる向上を行うことができるという効果を奏するものである。
【産業上の利用可能性】
【００７９】
本発明は、モータにより駆動されるロボットの負荷の重量と負荷の重心位置を簡単な測定方法と計算方法により、迅速かつ容易に算出する制御方法を提供するものであり、衝突時の安全性向上や破壊による損失防止、衝突検出の高精度化、ロボットの実タクト性能を向上でき、ロボットの利用分野の拡大とバリ取り作業や、複雑な形状部品の嵌め合い作業など各種使用用途に利用できる。
【図面の簡単な説明】
【００８０】
【図１】本発明の実施の形態１における制御方法を実施するためのブロック図
【図２】ロボットを構成する駆動軸の垂直座標系における配置の一例を示す図
【図３】ロボットを構成する駆動軸の水平座標系における配置の一例を示す図
【図４】一方の駆動軸が正負方向の等加速運動を行う一例を示す図
【図５】図４の等加速運動を行った場合のモータ電流特性の一例を示す図
【図６】もう一方の駆動軸が正負回転方向の等加速運動を行う一例を示す図
【図７】図６の等加速運動を行った場合のモータ電流特性の一例を示す図
【図８】図６の等加速運動を行った場合における負荷が起因するトルク成分の特性の一例を示す図
【図９】図４の等加速運動を行った場合における負荷が起因するトルク成分の特性の一例を示す図
【符号の説明】
【００８１】
２    モータ＋アーム
２４  第５軸
２５  第６軸
２８  負荷
ＴＭ    モータトルク
ＩＭ  モータ電流
【図１】
2006075931.tif 000019
【図２】
2006075931.tif 000020
【図３】
2006075931.tif 000021
【図４】
2006075931.tif 000022
【図５】
2006075931.tif 000023
【図６】
2006075931.tif 000024
【図７】
2006075931.tif 000025
【図８】
2006075931.tif 000026
【図９】
2006075931.tif 000027