(57)【要約】      （修正有）【課題】水平４軸のロボットアームが扱う負荷について重心位置を推定する。【解決手段】先端に質量ｍのワークを持つ状態で、ロボットのアームの第４軸位置をθａとする第１初期位置と第１推定用位置の間を、第２軸を往復動作させ、第２軸の加速度と第４軸の加減速トルクを取得し、第４軸周りのイナーシャτＩ４ａを算出する。第４軸位置をθｂに変化させた第２初期位置から第２軸を動作させ、第２軸の加速度と第４軸の加減速トルクを取得し、第４軸周りのイナーシャτＩ４ｂを算出する。第４軸位置をθｃに変化させた第３初期位置から第２軸を動作させ、第２軸の加速度と第４軸の加減速トルクを取得し、第４軸周りのイナーシャτＩ４ｃを算出する。第４軸からワークまでの重心距離ｌｈ、ワークの重心方向θｈ、イナーシャτＩ４ａ、τＩ４ｂ及びτＩ４ｃ、第４軸位置θａ，θｂ及びθｃに基づきワークの重心位置ｘｃｏｇ、ｙｃｏｇを算出する。【選択図】図１
2019000941.tif 000002
【特許請求の範囲】
【請求項１】
水平４軸構成のロボットアームの先端に質量ｍが既知の負荷がある状態で、前記ロボットアームを第４軸位置をθａとする第１初期位置から、第２軸を動作させて第１推定用位置に至るまで，及び前記第１推定用位置から再度前記第１初期位置に至るまでの第１動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する第１加速度取得部と、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第１トルク取得部と、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ａを算出する第１イナーシャ算出部と、
前記ロボットアームを、前記第１初期位置について第４軸位置をθｂ（≠θａ）に変化させた第２初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第２推定用位置に至るまで，及び前記第２推定用位置から再度前記第２初期位置に至るまでの第２動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する第２加速度取得部と、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第２トルク取得部と、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ｂを算出する第２イナーシャ算出部と、
前記ロボットアームを、前記第１初期位置について第４軸位置をθｃ（≠θａ，θｂ）に変化させた第３初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第３推定用位置に至るまで，及び前記第２３定用位置から再度前記第３初期位置に至るまでの第３動作期間について加速領域及び減速領域における第２軸の加速度をそれぞれ取得する第３加速度取得部と、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第３トルク取得部と、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ｃを算出する第３イナーシャ算出部と、
第４軸から前記負荷までの重心距離ｌｈ，前記負荷の重心方向θｈ，前記イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃ，前記第４軸位置θａ，θｂ及びθｃに基づいて、前記負荷の重心位置ｘｃｏｇ，ｙｃｏｇを算出する重心位置算出部とを備えるロボットの負荷重心位置推定装置。
【請求項２】
前記第１〜第３加速度取得部は、それぞれ前記第４軸位置θａ，θｂ及びθｃの何れかを、０°，９０°，−９０°に設定する請求項１記載のロボットの負荷重心位置推定装置。
【請求項３】
前記第１〜第３加速度取得部は、第３軸を上端位置に固定して動作させる請求項１又は２記載のロボットの負荷重心位置推定装置。
【請求項４】
水平４軸構成のロボットアームの先端に質量ｍが既知の負荷がある状態で、前記ロボットアームを第４軸位置をθａとする第１初期位置から、第２軸を動作させて第１推定用位置に至るまで，及び前記第１推定用位置から再度前記第１初期位置に至るまでの第１動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する第１ステップと、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第２ステップと、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ａを算出する第３ステップと、
前記ロボットアームを、前記第１初期位置について第４軸位置をθｂ（≠θａ）に変化させた第２初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第２推定用位置に至るまで，及び前記第２推定用位置から再度前記第２初期位置に至るまでの第２動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する第４ステップと、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第５ステップと、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ｂを算出する第６ステップと、
前記ロボットアームを、前記第１初期位置について第４軸位置をθｃ（≠θａ，θｂ）に変化させた第３初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第３推定用位置に至るまで，及び前記第３推定用位置から再度前記第３初期位置に至るまでの第３動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する第７ステップと、
前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する第８ステップと、
前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ｃを算出する第９ステップと、
第４軸から前記負荷までの重心距離ｌｈ，前記負荷の重心方向θｈ，前記イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃ，前記第４軸位置θａ，θｂ及びθｃに基づいて、前記負荷の重心位置ｘｃｏｇ，ｙｃｏｇを算出する第１０ステップとを有するロボットの負荷重心位置推定方法。
【請求項５】
前記第１，第４，第７ステップにおいて、それぞれ前記第４軸位置θａ，θｂ及びθｃの何れかを、０°，９０°，−９０°に設定する請求項４記載のロボットの負荷重心位置推定方法。
【請求項６】
前記第１，第４，第７ステップにおいて、第３軸を上端位置に固定して動作させる請求項４又は５記載のロボットの負荷重心位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、水平４軸構成のロボットアームの先端に負荷がある状態で、前記負荷の重心位置を推定する装置及び方法に関する。
【背景技術】
【０００２】
従来より、垂直６軸型のロボットアームが負荷であるワークを把持した状態で、そのワークの重心位置を推定する技術については、例えば特許文献１等に開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第５３２７７２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
垂直６軸型のロボットアームでは、第５軸の動作によってワークに対し重力を作用させる状態を変化させることができるので、ワークの重心位置を推定することは可能である。特許文献１等に開示されている技術はそのような構成を前提としているため、水平４軸構成のように、ワークに対して重力を作用させる状態を変化させることができないロボットアームに対しては前記技術を適用することができない。
【０００５】
本発明は上記事情に鑑みてなされたものであり、その目的は、水平４軸構成のロボットアームが扱う負荷についても、重心位置を推定できるロボットの負荷重心位置推定装置及びロボットの負荷重心位置推定方法を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１記載のロボットの負荷重心位置推定装置によれば、第１加速度取得部は、水平４軸構成のロボットアームの先端に質量ｍが既知の負荷がある状態で、ロボットアームを第４軸位置をθａとする第１初期位置から、第２軸を動作させて第１推定用位置に至るまで，及び第１推定用位置から再度第１初期位置に至るまでの第１動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する。
【０００７】
第１トルク取得部は、前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得する。第１イナーシャ算出部は、前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ａを算出する。
【０００８】
第２加速度取得部は、前記第１初期位置について第４軸位置をθｂに変化させた第２初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第２推定用位置に至るまで，及び第２推定用位置から再度第２初期位置に至るまでの第２動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する。第２トルク取得部は第２動作期間について、第１トルク取得部と同様に、第４軸の加速トルク，減速トルクを取得する。第２イナーシャ算出部も第２動作期間について、第１イナーシャ算出部と同様にして第４軸周りのイナーシャτＩ４ｂを算出する。
【０００９】
第３加速度取得部は、前記第１初期位置について第４軸位置をθｃに変化させた第３初期位置から第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第３推定用位置に至るまで，及び第３推定用位置から再度第３初期位置に至るまでの第３動作期間における加速領域及び減速領域について第２軸の加速度をそれぞれ取得する。第３トルク取得部は第３動作期間について、同様に第４軸の加速トルク，減速トルクを取得する。第３イナーシャ算出部も、第３動作期間について同様に第４軸周りのイナーシャτＩ４ｃを算出する。そして、重心位置算出部は、第４軸から負荷までの重心距離ｌｈ，前記負荷の重心方向θｈ，イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃ，第４軸位置θａ，θｂ及びθｃに基づいて、負荷の重心位置ｘｃｏｇ，ｙｃｏｇを算出する。
【００１０】
すなわち、それぞれ異なる３つの第４軸位置について、負荷を水平方向に同じように移動させて第４軸に加える加速度に基づいて、各イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃを得る。そして、上記各イナーシャと、第４軸位置θａ，θｂ及びθｃとに基づいて、重心位置ｘｃｏｇ，ｙｃｏｇを以下の式で求めることができる。
ｘｃｏｇ＝ｌｈｃｏｓθｈ
ｙｃｏｇ＝ｌｈｓｉｎθｈ
したがって、水平４軸構成のアームを備えるロボットについても、負荷の水平面内重心位置ｘｃｏｇ，ｙｃｏｇを推定できる。
【００１１】
請求項２記載のロボットの負荷重心位置推定装置によれば、第１〜第３加速度取得部は、それぞれ第４軸位置θａ，θｂ及びθｃの何れかを、０°，９０°，−９０°に設定する。これにより、重心位置ｘｃｏｇ，ｙｃｏｇを直ちに求めることができ、演算処理が簡単になる。
【００１２】
請求項３記載のロボットの負荷重心位置推定装置によれば、第１〜第３加速度取得部は第３軸を上端位置に固定して動作させる。これにより、アームを第１〜第３推定用位置に動作させる際に、３軸の動きにより負荷に発生させる振動を抑制できるので、重心位置をより高い精度で推定できる。
【図面の簡単な説明】
【００１３】
【図１】一実施形態であり、重心位置推定処理の内容を示すフローチャート（その１）
【図２】重心位置推定処理の内容を示すフローチャート（その２）
【図３】第１動作期間の動作を、４軸アームモデルで示す図
【図４】第２軸の速度波形を示す図
【図５】第２軸の加速度波形を示す図
【図６】第４軸のトルク波形を示す図
【図７】第２動作期間の動作を、４軸アームモデルで示す図
【図８】第３動作期間の動作を、４軸アームモデルで示す図
【図９】産業用ロボットのシステム構成を示す図
【図１０】４軸アームのモデルを示す図
【発明を実施するための形態】
【００１４】
以下、一実施形態について図面を参照して説明する。図９は、一般的な産業用ロボットのシステム構成を示している。ロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。コントローラ３は制御装置，負荷重心位置推定装置に相当する。
【００１５】
ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上に垂直軸であるＺ軸方向の軸心を持つ第１軸Ｊ１を中心に回転可能に連結された第１アーム６と、第１アーム６の先端部上にＺ軸方向の軸心を持つ第２軸Ｊ２を中心に回転可能に連結された第２アーム７と、第２アーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。シャフト８を上下動させる際の軸が第３軸Ｊ３であり、回転させる際の軸が第４軸Ｊ４である。シャフト８の下端部には、フランジ９が位置決めされて着脱可能に取り付けられる。
【００１６】
ベース５、第１アーム６、第２アーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能する。アーム先端であるフランジ９には、ツールとして例えばエアチャック１０が取り付けられている。図７ではエアチャック１０により、負荷に相当するワーク１１を吸着した状態を示している。ロボット２に設けられる複数の軸Ｊ１〜Ｊ４は、それぞれに対応して設けられる図示しないモータにより駆動される。各モータの近傍には、それぞれの回転軸の回転角度，回転位置を検出するための図示しない位置検出器が設けられている。
【００１７】
一般に、産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作する。コントローラ３は、その動作プログラムに基づいてモータの駆動をフィードバック制御し、ロボット２のアームの動作制御を行う。
【００１８】
ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。
【００１９】
次に、ツールを含むワーク１１の重心位置を推定する原理について説明する。図１０は、ロボット２の４軸アームモデルを示す。本実施形態では、アームの第２軸を動作させた際に、第４軸に作用する加速度に基づいて重心位置を推定する。ツールを含むワーク１１の質量ｍは既知であるとする。
【００２０】
ロボット２が動作した際に、モータに加わるトルクτＭは、（１）式となる。
【数１】
2019000941.tif 000003
τＩ：イナーシャトルク    ＭＩ：モータのイナーシャ
τｆ：遠心トルク          Ｍｖｉｓ：モータの粘性      Ｇｅａｒ：ギア比
τＧ：重力トルク          ＭＦｒｃ：モータの摩擦      ω：モータの角速度
Ｆｒｃ：摩擦トルク            ω（ドット）＝ｄω／ｄｔ：モータの角加速度
尚、以降では、角加速度を単に「加速度」と称する場合がある。ここで、モータを図２示す角速度パターンで動作させた際に、加速区間でモータに作用するトルクをτＭａｃｃ，原則区間でモータに作用するトルクをτＭｄｃｃとし、それらの差を求める。すると、速度に依存する項τｆ，Ｆｒｃ，Ｍｖｉｓ，ＭＦｒｃが消えて、加速度に依存する項τＩ，ＭＩが残る。
【数２】
2019000941.tif 000004
【００２１】
第２軸を動作させ、第４軸を動作させなければ、第４軸の角加速度はゼロになる。すると、第４軸のイナーシャトルクτＩ４は、（３）式となる。
【数３】
2019000941.tif 000005
第４軸の角度を、それぞれ異なる角度θａ，θｂ，θｃに設定して、ロボットアームに同じ動作をさせた際に、それぞれの動作における第４軸イナーシャをＩａ，Ｉｂ，Ｉｃとすると、
【数４】
2019000941.tif 000006
となる。
【００２２】
求めようとするワーク１１の重心位置ｘｃｏｇ，ｙｃｏｇは、
ｘｃｏｇ＝ｌｈｃｏｓθｈ
ｙｃｏｇ＝ｌｈｓｉｎθｈ            …（６）
である。これらは（４），（５）式より、（７），（８）式のように計算できる。
【数５】
2019000941.tif 000007
特に、θａ＝０，θｂ＝π／２，θｃ＝−π／２に設定すると
【数６】
2019000941.tif 000008
となり、簡単に導出できる。
【００２３】
次に、以上の原理に基づく本実施形態の作用について図１から図８を参照して説明する。図１及び図２は、コントローラ３により実行される重心位置推定処理の内容を示すフローチャートである。先ず、ロボット２のアームを第１初期位置に移動させる（Ｓ１）。第１初期位置は例えば、図３に示すように、
（Ｊ１，Ｊ２，Ｊ３，Ｊ４）＝Ｊ（０，−４０，２２０，０）
とする。尚、Ｊ３＝２２０は、第３軸を上端にまで移動させた状態に対応する。また、この時の第４軸角度をθａとする。
【００２４】
次に、アームに第１推定用位置を取らせた後、再度第１初期位置まで戻す第１動作を開始する（Ｓ２）。第１推定用位置は、例えば
Ｊ（０，４０，２２０，０）
とする。第４軸は、初期位置のままＪ４＝θａ＝０とする。そして、（１）アームが第１推定用位置に移動するまで，及び（２）その第１推定用位置から再度第１初期位置に至るまでの第１動作期間内に、第２軸の加速度波形及び第４軸のトルク波形をサンプリングする（Ｓ３）。これらはそれぞれ、例えば図４，図５に示すような波形となる。この時、第１動作期間の上記動作（１）と上記動作（２）との波形の平均をとることで、摩擦力を除去した波形を得る。
【００２５】
そして、取得した加速度波形について、加速領域，減速領域における第２軸加速度ω（ドット）２ａａ，ω（ドット）２ａｄを取得する（Ｓ４）。ステップＳ２は第１加速度取得部に相当する。また、取得したトルク波形について、加速領域，減速領域における第４軸加速トルクτ４ａａ，τ４ａｄを取得する（Ｓ５）。ステップＳ５は第１トルク取得部に相当する。それから、ステップＳ４，Ｓ５で取得した第２軸加速度及び第４軸トルクを（１１）式に代入して、４軸イナーシャτＩ４ａを算出する（Ｓ６）。
【数７】
2019000941.tif 000009
【００２６】
次に、アームを、第２初期位置に移動させる（Ｓ７）。第２初期位置は、第１初期位置から第４軸位置のみをθａからθｂ＝９０°に変化させた位置である。
Ｊ（０，−４０，２２０，９０）
それから、アームに、第２動作を実行させる（Ｓ８）。図７に示すように、第２推定用位置も、第１推定用位置から第４軸位置のみをθｂ＝９０°に変化させた位置となる。
Ｊ（０，４０，２２０，９０）
第１動作期間と同様に、第２推定用位置から再び第２初期位置に戻すまでが第２動作期間となる。
【００２７】
以降のステップＳ９〜Ｓ１２は、第２動作期間について、ステップＳ３〜Ｓ６と同様の処理を行うものである。但し、ステップＳ１０で得られる第２軸加速度はω（ドット）２ｂａ，ω（ドット）２ｂｄとなり、ステップＳ１１で得られる第４軸トルクはτ４ｂａ，τ４ｂｄとなる。ステップＳ１０は第２加速度取得部に相当し、ステップＳ１１は第２トルク取得部に相当する。そして、ステップＳ１０，Ｓ１１で取得した第２軸加速度及び第４軸トルクを（１２）式に代入して、４軸イナーシャτＩ４ｂを算出する（Ｓ１２）。
【数８】
2019000941.tif 000010
【００２８】
次に、アームを、第３初期位置に移動させる（Ｓ１３）。第３初期位置も、第１初期位置から第４軸位置のみをθａからθｃ＝−９０°に変化させた位置である。
Ｊ（０，−４０，２２０，−９０）
それから、アームに、第３動作を実行させる（Ｓ１４）。図８に示すように、第３推定用位置も、第１推定用位置から第４軸位置のみをθｃ＝−９０°に変化させた位置となる。
Ｊ（０，４０，２２０，−９０）
第１動作期間と同様に、第３推定用位置から再び第３初期位置に戻すまでが第３動作期間となる。
【００２９】
以降のステップＳ１５〜Ｓ１８は、第３動作期間について、ステップＳ３〜Ｓ６と同様の処理を行うものである。但し、ステップＳ１６で得られる第２軸加速度はω（ドット）２ｃａ，ω（ドット）２ｃｄとなり、ステップＳ１７で得られる第４軸トルクはτ４ｃａ，τ４ｃｄとなる。ステップＳ１６は第３加速度取得部に相当し、ステップＳ１７は第３トルク取得部に相当する。そして、ステップＳ１６，Ｓ１７で取得した第２軸加速度及び第４軸トルクを（１３）式に代入して、４軸イナーシャτＩ４ｂを算出する（Ｓ１８）。
【数９】
2019000941.tif 000011
【００３０】
ここで、本実施形態のように、第４軸角度θａ，θｂ，θｃを、それぞれ０°，９０°，−９０°に選択していれば（Ｓ１９；ＹＥＳ）、（９），（１０）式を用いてｌｈｃｏｓθｈ，ｌｈｓｉｎθｈを求め（Ｓ２１）、（６）式よりワーク１１の重心位置ｘｃｏｇ，ｙｃｏｇが得られる（Ｓ２２）。一方、第４軸角度θａ，θｂ，θｃを、それぞれ０°，９０°，−９０°に選択していなければ（Ｓ１９；ＮＯ）、（７），（８）式を用いてｌｈｃｏｓθｈ，ｌｈｓｉｎθｈを求める。ステップＳ１９〜Ｓ２２は、重心位置算出部に相当する。
【００３１】
尚、ワーク１１の実際の重心位置と、本実施形態で得られた重心位置ｘｃｏｇ，ｙｃｏｇとの間に誤差が含まれる場合は、誤差値に応じた補正係数を用いて補正すれば良い。
以上のように本実施形態によれば、コントローラ３は、先端に質量ｍが既知のワーク１１がある状態で、水平４軸ロボット２のアームが第４軸位置をθａとする第１初期位置から、第２軸を動作させて第１推定用位置に至るまで，及び第１推定用位置から再度第１初期位置に至るまでの第１動作期間における加速領域及び減速領域について、第２軸の加速度をそれぞれ取得する。
【００３２】
また、コントローラ３は、前記加速領域における第４軸の加速トルク，及び前記減速領域における第４軸の減速トルクを取得すると、前記加速領域及び減速領域の加速度と、前記加速トルクと、前記減速トルクとに基づいて第４軸周りのイナーシャτＩ４ａを算出する。
【００３３】
コントローラ３は、第１初期位置について第４軸位置をθｂに変化させた第２初期位置から第２軸を動作させて第２動作を行い、第２動作期間における加速領域及び減速領域について、第２軸の加速度をそれぞれ取得する。また、第２動作期間について、第１トルク取得部と同様に、第４軸の加速トルク，減速トルクを取得すると、第４軸周りのイナーシャτＩ４ｂを算出する。
【００３４】
また、コントローラ３は、第１初期位置について第４軸位置をθｃに変化させた第３初期位置から第２軸を動作させて第３動作を行い、第３動作期間における加速領域及び減速領域について、第２軸の加速度をそれぞれ取得する。また、第３動作期間について、第４軸の加速トルク，減速トルクを取得すると、第４軸周りのイナーシャτＩ４ｃを算出する。そして、第４軸から負荷までの重心距離ｌｈ，前記負荷の重心方向θｈ，イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃ，第４軸位置θａ，θｂ及びθｃに基づいて、ワーク１１の重心位置ｘｃｏｇ，ｙｃｏｇを算出する。
【００３５】
すなわち、それぞれ異なる３つの第４軸位置について、負荷を水平方向に同じように移動させて第４軸に加える加速度に基づいて、各イナーシャτＩ４ａ，τＩ４ｂ及びτＩ４ｃを得る。そして、上記各イナーシャと、第４軸位置θａ，θｂ及びθｃとに基づいて、ワーク１１の重心位置ｘｃｏｇ，ｙｃｏｇを求める。したがって、水平４軸構成のアームを備えるロボットについても、ワーク１１の水平面内重心位置を推定できる。
【００３６】
また、第４軸角度θａ，θｂ，θｃを、それぞれ０°，９０°，−９０°に設定することで、（９），（１０）式により重心位置ｘｃｏｇ，ｙｃｏｇを直ちに求めることができ、演算処理が簡単になる。更に、第３軸を上端位置に固定して動作させるので、アームを第１，第２推定用位置に動作させる際に、第３軸によりワーク１１に発生する振動を抑制でき、重心位置をより高い精度で推定できる。
【００３７】
本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
θａ，θｂ，θｃと、角度０°，９０°，−９０°との対応関係を変更しても良い。また、θａ，θｂ，θｃの各設定は、上記の角度に限る必要は無い。
初期位置及び推定用位置については、適宜変更して実施しても良い。
振動の影響が問題なければ、第３軸は必ずしも上端位置を取る必要は無い。
第２，第３ステップの実行順序を入れ替えても良いし、第５，第６ステップの実行順序を入れ替えても良い。また、第８，第９ステップの実行順序を入れ替えても良い。
【符号の説明】
【００３８】
図面中、２はロボット、３はコントローラ、１１はワークを示す。
【図１】
2019000941.tif 000012
【図２】
2019000941.tif 000013
【図３】
2019000941.tif 000014
【図４】
2019000941.tif 000015
【図５】
2019000941.tif 000016
【図６】
2019000941.tif 000017
【図７】
2019000941.tif 000018
【図８】
2019000941.tif 000019
【図９】
2019000941.tif 000020
【図１０】
2019000941.tif 000021