JP2018158418A - ロボットの負荷重心位置推定装置及びロボットの負荷重心位置推定方法 - Google Patents

Abstract

【課題】水平４軸構成のロボットアームが扱う負荷について、重心位置を推定する。【解決手段】先端に質量ｍが既知のワーク１１がある状態で、アームが第１初期位置から第１推定用位置に至るまでの第１動作期間に、第１軸，第２軸の定速領域につき角速度ω１，ω２を取得する（Ｓ１〜Ｓ５）。同第１動作期間に第４軸の静止トルクτｕ４及び定速トルクτｃ４を取得し（Ｓ６）、第１パラメータＴ１を演算する（Ｓ７）。アームを、第１初期位置について第４軸位置をθ４からθ４’に変化させた第２初期位置から第２推定用位置に至るまでの第２動作期間に、第１軸，第２軸の定速領域につき角速度ω１’，ω２’を取得する（Ｓ８〜Ｓ１１）。同第２動作期間に第４軸の静止トルクτｕ４’及び定速トルクτｃ４’を取得し（Ｓ１２）第２パラメータＴ２を演算すると（Ｓ１４）、パラメータＴ１，Ｔ２に基づきワーク１１の重心位置ｘｃｏｇ，ｙｃｏｇを算出する（Ｓ１５〜Ｓ１７）。【選択図】図１

Description

本発明は、水平４軸構成のロボットアームの先端に負荷がある状態で、前記負荷の重心位置を推定する装置及び方法に関する。

従来より、垂直６軸型のロボットアームが負荷であるワークを把持した状態で、そのワークの重心位置を推定する技術については、例えば特許文献１等に開示されている。

特許第５３２７７２２号公報

垂直６軸型のロボットアームでは、第５軸の動作によってワークに対し重力を作用させる状態を変化させることができるので、ワークの重心位置を推定することは可能である。特許文献１等に開示されている技術はそのような構成を前提としているため、水平４軸構成のように、ワークに対して重力を作用させる状態を変化させることができないロボットアームに対しては前記技術を適用することができない。

本発明は上記事情に鑑みてなされたものであり、その目的は、水平４軸構成のロボットアームが扱う負荷についても、重心位置を推定できるロボットの負荷重心位置推定装置及びロボットの負荷重心位置推定方法を提供することにある。

請求項１記載のロボットの負荷重心位置推定装置によれば、第１速度取得部は、先端に質量ｍが既知の負荷がある状態で、水平４軸構成のロボットアームを第１初期位置から、第１軸及び／又は第２軸を動作させて第１推定用位置に至るまでの第１動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する。第１トルク取得部は、第１動作期間における第４軸の静止トルク及び定速トルクを取得する。

ここで、静止トルクとは、第１軸及び／又は第２軸を第１動作期間で動作させる前の状態を維持するために第４軸に掛けられるトルクである。また、定速トルクとは、第１軸及び／又は第２軸が第１動作期間で動作している際に、第４軸を静止状態と同じように維持するために第１軸及び／又は第２軸への抗力として発生させるトルクである。

第１パラメータ演算部は、第４軸から負荷までの重心距離をｌ_ｈ，第４軸位置をθ_４，前記θ_４を基準とする負荷の重心方向をθ_ｈとすると、前記速度並びに前記静止トルク及び定速トルクに基づいて第１パラメータＴ_１＝ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）を演算する。

第２速度取得部は、ロボットアームを、第１初期位置について第４軸位置をθ_４’に変化させた第２初期位置から第１軸及び／又は第２軸を動作させて第２推定用位置に至るまでの第２動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する。第２推定用位置は、第１推定用位置から第４軸位置のみを上記のように変化させたものとなる。

第２トルク取得部は、第２動作期間における第４軸の静止トルク及び定速トルクを取得する。第２パラメータ演算部は、前記速度並びに前記静止トルク及び定速トルクに基づいて第２パラメータＴ_２＝ｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）を演算する。そして、重心位置算出部は、第１パラメータＴ_１，第２パラメータＴ_２に基づいて、負荷の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを算出する。

すなわち、異なる第４軸位置について、負荷を水平方向に同じように移動させて第４軸に加えるトルクに基づいて、２つのパラメータＴ_１，Ｔ_２を得る。そして、これらのパラメータＴ_１，Ｔ_２より、重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを以下の式で求めることができる。
ｘ_ｃｏｇ＝ｌ_ｈｃｏｓθ_ｈ
ｙ_ｃｏｇ＝ｌ_ｈｓｉｎθ_ｈ
したがって、水平４軸構成のアームを備えるロボットについても、負荷の水平面内重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを推定できる。

請求項２記載のロボットの負荷重心位置推定装置によれば、第１及び第２速度取得部は、それぞれ第１軸及び第２軸を動作させるので、負荷により大きな遠心力，つまりトルクを作用させることができる。したがって、重心位置をより高い精度で推定できる。

請求項３記載のロボットの負荷重心位置推定装置によれば、第１及びパラメータ演算部は、前記θ_４，θ_４’の一方を０°とし、他方を９０°に設定する。これにより、２つのパラメータＴ_１，Ｔ_２より重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを直ちに求めることができ、演算処理が簡単になる。

請求項４記載のロボットの負荷重心位置推定装置によれば、第１及び第２速度取得部は第３軸を上端位置に固定して動作させる。これにより、アームを第１，第２推定用位置に動作させる際に、３軸の動きにより負荷に発生させる振動を抑制できるので、重心位置をより高い精度で推定できる。

一実施形態であり、重心位置推定処理の内容を示すフローチャート アームを第１初期位置から第１推定用位置に移動させる状態を、４軸アームモデルで示す図 第１軸及び第２軸の速度波形を示す図 第４軸のトルク波形を示す図 第４軸トルクの値に静止摩擦力の影響が及ぶことを説明する図 静止摩擦力の影響を含んだ状態で得られる第４軸トルクの一例を示す図 静止摩擦力の影響を排除して得られる第４軸トルクの一例を示す図 アームを第２初期位置から第２推定用位置に移動させる状態を、４軸アームモデルで示す図 産業用ロボットのシステム構成を示す図 ４軸アームのモデルを示す図 定速領域における第４軸のモータトルクτ_Ｍ４を示す図

以下、一実施形態について図面を参照して説明する。図９は、一般的な産業用ロボットのシステム構成を示している。ロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。コントローラ３は制御装置，負荷重心位置推定装置に相当する。

ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上に垂直軸であるＺ軸方向の軸心を持つ第１軸Ｊ１を中心に回転可能に連結された第１アーム６と、第１アーム６の先端部上にＺ軸方向の軸心を持つ第２軸Ｊ２を中心に回転可能に連結された第２アーム７と、第２アーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。シャフト８を上下動させる際の軸が第３軸Ｊ３であり、回転させる際の軸が第４軸Ｊ４である。シャフト８の下端部には、フランジ９が位置決めされて着脱可能に取り付けられる。

ベース５、第１アーム６、第２アーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能する。アーム先端であるフランジ９には、ツールとして例えばエアチャック１０が取り付けられている。図９ではエアチャック１０により、負荷に相当するワーク１１を吸着した状態を示している。ロボット２に設けられる複数の軸Ｊ１〜Ｊ４は、それぞれに対応して設けられる図示しないモータにより駆動される。各モータの近傍には、それぞれの回転軸の回転角度，回転位置を検出するための図示しない位置検出器が設けられている。

一般に、産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作する。コントローラ３は、その動作プログラムに基づいてモータの駆動をフィードバック制御し、ロボット２のアームの動作制御を行う。

ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。

次に、ツールを含むワーク１１の重心位置を推定する原理について説明する。図１０は、ロボット２の４軸アームモデルを示す。本実施形態では、アームの第１軸及び第２軸を動作させた際に、第４軸に作用する遠心トルクに基づいて重心位置を推定する。ツールを含むワーク１１の質量ｍは既知であるとする。第４軸を固定して第１軸及び第２軸を動作させた際に、第４軸に作用する遠心力Ｆは、（１）式となる。
Ｆ＝ｍ｛（２ω_１＋ω_２）ω_１Ｌ_２＋ω_１ ^２（Ｌ_１＋Ｌ_２）｝ …（１）
ω_１：第１軸角速度 ，Ｌ_１：第１アーム６のリンク長
ω_２：第２軸角速度 ，Ｌ_２：第２アーム７のリンク長
この時、第４軸に作用する遠心トルクτ_ｆ４は、（２）式となる。
τ_ｆ４＝Ｆｌ_ｈｓｉｎ（θ_２＋θ_４＋θ_ｈ） …（２）
θ_２：第２軸角度 ，θ_４：第４軸角度Ｌ_１
θ_ｈ：（ツール＋ワーク）の重心方向，ｌ_ｈ：（ツール＋ワーク）の重心距離

ロボット２が動作した際に、モータに加わるトルクτ_Ｍは、（３）式となる。
τ_Ｍ＝（τ_Ｉ＋τ_ｆ＋τ_Ｇ＋Ｆｒｃ）／Ｇｅａｒ
＋（Ｍ_Ｉｄω／ｄｔ＋Ｍ_ｖｉｓω）Ｇｅａｒ＋Ｍ_Ｆｒｃ …（３）
τ_Ｉ：イナーシャトルク Ｍ_Ｉ：モータのイナーシャ
τ_ｆ：遠心トルク Ｍ_ｖｉｓ：モータの粘性 Ｇｅａｒ：ギア比
τ_Ｇ：重力トルク Ｍ_Ｆｒｃ：モータの摩擦 ω：モータの角速度
Ｆｒｃ：摩擦トルク ｄω／ｄｔ：モータの角加速度

各軸の加速度＝０であれば、イナーシャトルクτ_Ｉ＝０となる。したがって、図１１に示す定速領域における第４軸のモータトルクτ_Ｍ４は、（４）式となる。
τ_Ｍ４＝τ_ｆ４／Ｇｅａｒ＝Ｆｌ_ｈｓｉｎ（θ_２＋θ_４＋θ_ｈ）／Ｇｅａｒ …（４）
ここで、θ_２＝０となるようにして第４軸のモータトルクτ_Ｍ４を取得すれば、
ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）＝τ_ｆ４Ｇｅａｒ／Ｆ …（５）
となる。

求めようとするワーク１１の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇは、
ｘ_ｃｏｇ＝ｌ_ｈｃｏｓθ_ｈ
ｙ_ｃｏｇ＝ｌ_ｈｓｉｎθ_ｈ …（６）
である。したがって、第４軸角度θ_４を変化させた２回のアーム動作により第４軸のモータトルクτ_Ｍ４を取得し、重心距離ｌ_ｈ，重心方向θ_ｈを決定すれば、重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを求めることができる。

次に、以上の原理に基づく本実施形態の作用について図１から図８を参照して説明する。図１は、コントローラ３により実行される重心位置推定処理の内容を示すフローチャートである。先ず、ロボット２のアームを第１初期位置に移動させる（Ｓ１）。第１初期位置は例えば、図２に示すように、
（Ｊ１，Ｊ２，Ｊ３，Ｊ４）＝Ｊ（−４０，−４０，２２０，０）
とする。尚、Ｊ３＝２２０は、第３軸を上端にまで移動させた状態に対応する。

次に、アームに第１推定用位置を取らせるための動作を開始する（Ｓ２）。第１推定用位置は、例えば
Ｊ（４０，４０，２２０，０）
とする。第４軸は、初期位置のままＪ４＝０とする。そして、アームが第１推定用位置に移動するまでの第１動作期間内に、第１軸及び第２軸の速度波形及び第４軸のトルク波形をサンプリングする（Ｓ３）。これらはそれぞれ、例えば図３，図４に示すような波形となる。

そして、取得した速度波形より定速となっている領域を探索し（Ｓ４）、当該領域における第１軸，第２軸の角速度ω_１，ω_２を取得する（Ｓ５）。ステップＳ５は第１速度取得部に相当する。また、第４軸のトルク波形より、静止トルクτ_ｕ４，定速トルクτ_ｃ４を取得する（Ｓ６）。ステップＳ６は第１トルク取得部に相当する。これらの演算パラメータを（５）式に代入し、ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）を算出する（Ｓ７）。但し、遠心トルクτ_ｆ４については、定速トルクτ_ｃ４と静止トルクτ_ｕ４との差として与える。
ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）＝（τ_ｃ４−τ_ｕ４）Ｇｅａｒ／Ｆ …（７）

図５に示すように、実際に第４軸トルクの値を取得すると、静止摩擦力の影響があることで動作毎に得られる値が異なる。したがって、（７）式のように動作前の静止トルク値との差分をとることで、静止摩擦力の影響を除去する。図６は、静止摩擦力の影響を含んだ状態で得られる第４軸トルクの一例を示し、図７は、静止摩擦力の影響を排除して得られる第４軸トルクの一例を示している。

次に、アームを、第２初期位置に移動させる（Ｓ８）。第２初期位置は、第１初期位置から第４軸位置のみをθ_４からθ_４’に変化させた位置である。
Ｊ（−４０，−４０，２２０，９０）
それから、アームに、第２推定用位置を取らせるための動作を開始する（Ｓ９）。図８に示すように、第２推定用位置も、第１推定用位置から第４軸位置のみをθ_４’に変化させた位置となる。
Ｊ（４０，４０，２２０，９０）

以降のステップＳ１０〜Ｓ１３は、第２初期位置→第２推定用位置への移動について、ステップＳ３〜Ｓ６と同様の処理を行うものである。但し、ステップＳ１１で得られる定速角速度はω_１’，ω_２’となり、ステップＳ１２で得られるトルクは静止トルクτ_ｕ４’，定速トルクτ_ｃ４’となる。ステップＳ１１は第２速度取得部に相当し、ステップＳ１２は第２トルク取得部に相当する。また、第１動作期間に対応するものが第２動作期間となる。そして、取得された各演算パラメータを用いて（８）式によりｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）を算出する（Ｓ１４）。
ｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）＝（τ_ｃ４’−τ_ｕ４’）Ｇｅａｒ
／［ｍ｛（２ω_１’＋ω_２’）ω_１’Ｌ_２＋ω_１’^２（Ｌ_１＋Ｌ_２）｝］ …（８）

ここで、本実施形態のようにθ_４＝０°，θ_４’＝９０°であれば（Ｓ１５；ＹＥＳ）、（７）式は
ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）＝ｌ_ｈｓｉｎθ_ｈ＝ｙ_ｃｏｇ …（９）
となり、（８）式は
ｌ_ｈｓｉｎ（９０°＋θ_ｈ）＝ｌ_ｈｃｏｓθ_ｈ＝ｘ_ｃｏｇ …（１０）
となって、ワーク１１の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇが得られる（Ｓ１７）。

一方、θ_４＝０°，θ_４’＝９０°でなければ（Ｓ１５；ＮＯ）、以下の（１１）式，（１２）式より重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを得る（Ｓ１６）。ここで、
Ｔ_１＝ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）
Ｔ_２＝ｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）
とすれば、
ｌ_ｈｓｉｎθ_ｈ＝（Ｔ_１ｓｉｎθ_４’−Ｔ_２ｓｉｎθ_４）
／ｓｉｎ（θ_４’−θ_４） …（１１）
ｌ_ｈｃｏｓθ_ｈ＝（Ｔ_１ｃｏｓθ_４’−Ｔ_２ｃｏｓθ_４）
／ｓｉｎ（θ_４−θ_４’） …（１２）
となる。ステップＳ１５〜Ｓ１７は、重心位置算出部に相当する。
尚、ワーク１１の実際の重心位置と、本実施形態で得られた重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇとの間に誤差が含まれる場合は、誤差値に応じた補正係数を用いて補正すれば良い。

以上のように本実施形態によれば、コントローラ３は、先端に質量ｍが既知のワーク１１がある状態で、水平４軸ロボット２のアームが第１初期位置から、第１軸及び第２軸を動作させて第１推定用位置に至るまでの第１動作期間における、第１軸，第２軸それぞれの定速領域について角速度ω_１，ω_２を取得する。また、第１動作期間における第４軸の静止トルクτ_ｕ４及び定速トルクτ_ｃ４を取得する。そして、これらの演算パラメータに基づいて第１パラメータＴ_１を演算する。

また、アームが、第１初期位置について第４軸位置をθ_４からθ_４’に変化させた第２初期位置から第２推定用位置に至るまでの第２動作期間における、第１軸，第２軸それぞれの定速領域について角速度ω_１’，ω_２’を取得する。更に、第２動作期間における第４軸の静止トルクτ_ｕ４’及び定速トルクτ_ｃ４’を取得する。そして、これらの演算パラメータに基づいて第２パラメータＴ_２を演算すると、第１，第２パラメータＴ_１，Ｔ_２に基づいて、ワーク１１の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを算出する。

すなわち、異なる第４軸位置θ_４，θ_４’について、ワーク１１を水平方向に同じように移動させて第４軸に加えるトルクに基づき２つのパラメータＴ_１，Ｔ_２を得ることで、水平４軸構成のアームを備えるロボット２についても、ワーク１１の水平面内重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを推定できる。この場合、第１軸及び第２軸を共に動作させることで、ワーク１１により大きな遠心力，トルクを作用させることができる。したがって、重心位置をより高い精度で推定できる。

また、第４軸位置θ_４，θ_４’の一方を０°とし、他方を９０°に設定することで、２つのパラメータＴ_１，Ｔ_２より重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを直ちに求めることができ、演算処理が簡単になる。更に、第３軸を上端位置に固定して動作させるので、アームを第１，第２推定用位置に動作させる際に、第３軸によりワーク１１に発生する振動を抑制でき、重心位置をより高い精度で推定できる。

本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
初期位置及び推定用位置については、適宜変更して実施しても良い。
十分なトルクが得られれば、第１軸，第２軸の何れか一方のみを動作させても良い。
振動の影響が問題なければ、第３軸は必ずしも上端位置を取る必要は無い。
第２，第３ステップの実行順序を入れ替えても良いし、第６，第７ステップの実行順序を入れ替えても良い。

図面中、２はロボット、３はコントローラ、１１はワークを示す。

Claims (8)

1. 水平４軸構成のロボットアームの先端に質量ｍが既知の負荷がある状態で、前記ロボットアームを第１初期位置から、第１軸及び／又は第２軸を動作させて第１推定用位置に至るまでの第１動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する第１速度取得部と、
   前記第１軸及び／又は第２軸を前記第１動作期間で動作させる前の状態を維持するために第４軸に掛けられるトルクである静止トルク，及び前記第１軸及び／又は第２軸が前記第１動作期間で動作している際に、第４軸を静止状態と同じように維持するために前記第１軸及び／又は第２軸への抗力として発生させる定速トルクを取得する第１トルク取得部と、
   第４軸から前記負荷までの重心距離をｌ_ｈ，第４軸位置をθ_４，前記θ_４を基準とする前記負荷の重心方向をθ_ｈとすると、前記速度並びに前記静止トルク及び定速トルクに基づいて第１パラメータＴ_１＝ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）を演算する第１パラメータ演算部と、
   前記ロボットアームを、前記第１初期位置について第４軸位置をθ_４’に変化させた第２初期位置から第１軸及び／又は第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第２推定用位置に至るまでの第２動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する第２速度取得部と、
   前記第２動作期間における第４軸の静止トルク及び定速トルクを取得する第２トルク取得部と、
   前記速度並びに前記静止トルク及び定速トルクに基づいて第２パラメータＴ_２＝ｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）を演算する第２パラメータ演算部と、
   前記第１パラメータＴ_１及び前記第２パラメータＴ_２に基づいて、前記負荷の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを算出する重心位置算出部とを備えるロボットの負荷重心位置推定装置。
2. 前記第１速度取得部及び前記第２速度取得部は、それぞれ第１軸及び第２軸を動作させる請求項１記載のロボットの負荷重心位置推定装置。
3. 前記第１パラメータ演算部及び前記第２パラメータ演算部は、前記θ_４，θ_４’の一方を０°とし、他方を９０°に設定する請求項１又は２記載のロボットの負荷重心位置推定装置。
4. 前記第１速度取得部及び前記第２速度取得部は、第３軸を上端位置に固定して動作させる請求項１から３の何れか一項に記載のロボットの負荷重心位置推定装置。
5. 水平４軸構成のロボットアームの先端に質量ｍが既知の負荷がある状態で、前記ロボットアームを第１初期位置から、第１軸及び／又は第２軸を動作させて第１推定用位置に移動させる第１ステップと、
   前記ロボットアームが前記第１初期位置から前記第１推定用位置に至るまでの第１動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する第２ステップと、
   前記第１軸及び／又は第２軸を前記第１動作期間で動作させる前の状態を維持するために第４軸に掛けられるトルクである静止トルク，及び前記第１軸及び／又は第２軸が前記第１動作期間で動作している際に第４軸を静止状態と同じように維持するため前記第１軸及び／又は第２軸への抗力として発生させる定速トルクを取得する第３ステップと、
   第４軸から前記負荷までの重心距離をｌ_ｈ，第４軸位置をθ_４，前記θ_４を基準とする前記負荷の重心方向をθ_ｈとすると、前記速度並びに前記静止トルク及び定速トルクに基づいて第１パラメータＴ_１＝ｌ_ｈｓｉｎ（θ_４＋θ_ｈ）を演算する第４ステップと、
   前記ロボットアームを、前記第１初期位置について第４軸位置をθ_４’に変化させた第２初期位置から第１軸及び／又は第２軸を動作させて、第１軸から第３軸は第１推定用位置に等しい第２推定用位置に移動させる第５ステップと、
   前記ロボットアームが前記第２初期位置から前記第２推定用位置に至るまでの第２動作期間における、第１軸及び／又は第２軸が定速となる領域について速度を取得する第６ステップと、
   前記第２動作期間における第４軸の静止トルク及び定速トルクを取得する第７ステップと、
   前記速度並びに前記静止トルク及び定速トルクに基づいて第２パラメータＴ_２＝ｌ_ｈｓｉｎ（θ_４’＋θ_ｈ）を演算する第８ステップと、
   前記第１パラメータＴ_１及び前記第２パラメータＴ_２に基づいて、前記負荷の重心位置ｘ_ｃｏｇ，ｙ_ｃｏｇを算出する第９ステップとからなるロボットの負荷重心位置推定方法。
6. 前記第１及び前記第５ステップにおいて、それぞれ第１軸及び第２軸を動作させる請求項５記載のロボットの負荷重心位置推定方法。
7. 前記第４及び前記第８ステップにおいて、前記θ_４，θ_４’の一方を０°とし、他方を９０°に設定する請求項５又は６記載のロボットの負荷重心位置推定方法。
8. 前記第１及び前記第５ステップにおいて、第３軸を上端位置に固定して動作させる請求項５から７の何れか一項に記載のロボットの負荷重心位置推定方法。
   

Images