特許 6795540 ロボットを用いた負荷の重量及び重心位置を推定するための装置、方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6795540

(24)【登録日】2020年11月16日

(45)【発行日】2020年12月2日

(54)【発明の名称】ロボットを用いた負荷の重量及び重心位置を推定するための装置、方法及びプログラム

(51)【国際特許分類】

   B25J 9/10 20060101AFI20201119BHJP

【ＦＩ】

   B25J9/10 A

【請求項の数】9

【全頁数】12

(21)【出願番号】特願2018-82929(P2018-82929)

(22)【出願日】2018年4月24日

(65)【公開番号】特開2019-188514(P2019-188514A)

(43)【公開日】2019年10月31日

【審査請求日】2019年9月11日

【早期審査対象出願】

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(72)【発明者】

【氏名】内藤 康広

【審査官】 貞光 大樹

(56)【参考文献】

【文献】 特開平３−１４２１７９（ＪＰ，Ａ）

【文献】 特開平９−９１００４（ＪＰ，Ａ）

【文献】 特開平２−３００６４４（ＪＰ，Ａ）

【文献】 特開平７−２０５０７５（ＪＰ，Ａ）

【文献】 特開２００４−２５３８７（ＪＰ，Ａ）

【文献】 特開２０１０−７６０７４（ＪＰ，Ａ）

【文献】 特開２０１１−２３５３７４（ＪＰ，Ａ）

【文献】 特開２０１７−５６５２５（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１６／０３０５８４２（ＵＳ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

Ｇ０１Ｍ １／１２

Ｇ０１Ｇ １／００ − ２３／４８

(57)【特許請求の範囲】

【請求項1】

ロボットの可動部を駆動する軸に作用するトルクを検出するトルクセンサと、
前記可動部に取り付けられた物体が第１の姿勢で第１の位置にあるときに前記トルクセンサで検出された、前記軸に作用する第１のトルクと、前記第１のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１と、前記物体が前記第１の姿勢で前記第１の位置とは異なる第２の位置にあるときに前記トルクセンサで検出された、前記軸に作用する第２のトルクと、前記第２のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２と、前記第１の位置と、前記第２の位置とを用いて、前記物体の重量を計算する計算部と、
を有する、物体の重量の推定装置。

【請求項2】

前記計算部は、
前記第１のトルクをＴ１、前記第２のトルクをＴ２とし、
前記第１のトルクを検出したときの前記可動部の位置及び姿勢において前記トルクＭ１を取得し、前記第２のトルクを検出したときの前記可動部の位置及び姿勢において前記トルクＭ２を取得し、
前記第１の位置から前記第２の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤを計算し、
前記物体の重量Ｗを、
Ｗ＝｜（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）｜／Ｄ
なる式によって計算する、請求項１に記載の推定装置。

【請求項3】

ロボットの可動部を駆動する軸に作用するトルクを検出するトルクセンサと、
前記可動部に取り付けられた物体が互いに異なる３つの姿勢にあるときのそれぞれにおいて前記トルクセンサで検出された、前記軸に作用する第１のトルク、第２のトルク及び第３のトルクと、前記第１のトルクを検出したときの前記可動部の第１の位置及び姿勢と、前記第１の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１と、前記第２のトルクを検出したときの前記可動部の第２の位置及び姿勢と、前記第２の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２と、前記第３のトルクを検出したときの前記可動部の第３の位置及び姿勢と、前記第３の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ３と、前記物体の重量とを用いて、前記物体の重心位置を計算する計算部と、
を有する、物体の重心位置の推定装置。

【請求項4】

前記計算部は、
前記物体の重量をＷとし、
前記第１のトルクをＴ１、前記第２のトルクをＴ２、前記第３のトルクをＴ３とし、
前記第１の位置及び姿勢Ｅ１において前記トルクＭ１を取得し、前記第２の位置及び姿勢Ｅ２において前記トルクＭ２を取得し、前記第３の位置及び姿勢Ｅ３において前記トルクＭ３を取得し、
前記第１の位置から前記第２の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ１を計算し、前記第２の位置から前記第３の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ２を計算し、前記第３の位置から前記第１の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ３を計算し、
前記物体の重心位置（Ｐｘ，Ｐｙ，Ｐｚ）を、少なくとも、
（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）＝Ｗ×（Ｄ１−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２）
（Ｔ３−Ｍ３）−（Ｔ２−Ｍ２）＝Ｗ×（Ｄ２−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３）
（Ｔ１−Ｍ１）−（Ｔ３−Ｍ３）＝Ｗ×（Ｄ３−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１）
なる３つの式によって計算する、請求項３に記載の推定装置。

【請求項5】

トルクセンサを有するロボットの可動部に物体を取り付けることと、
前記物体が第１の姿勢で第１の位置にあるときに、前記可動部を駆動する軸に作用する第１のトルクを前記トルクセンサで検出することと、
前記第１のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１を取得することと、
前記物体が前記第１の姿勢で前記第１の位置とは異なる第２の位置にあるときに、前記軸に作用する第２のトルクを前記トルクセンサで検出することと、
前記第２のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２を取得することと、
前記第１のトルク、前記第２のトルク、前記トルクＭ１、前記トルクＭ２、前記第１の位置及び前記第２の位置を用いて、前記物体の重量を計算することと、
を含む、物体の重量の推定方法。

【請求項6】

前記第１のトルクをＴ１、前記第２のトルクをＴ２とし、
前記第１の位置から前記第２の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤを計算し、
前記物体の重量Ｗを、
Ｗ＝｜（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）｜／Ｄ
なる式によって計算する、請求項５に記載の推定方法。

【請求項7】

トルクセンサを有するロボットの可動部に、重量が既知の物体を取り付けることと、
前記物体が互いに異なる３つの姿勢にあるときのそれぞれにおいて、前記可動部を駆動する軸に作用する第１のトルク、第２のトルク及び第３のトルクを前記トルクセンサで検出することと、
前記第１のトルクを検出したときの前記可動部の第１の位置及び姿勢Ｅ１において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１を取得し、前記第２のトルクを検出したときの前記可動部の第２の位置及び姿勢Ｅ２において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２を取得し、前記第３のトルクを検出したときの前記可動部の第３の位置及び姿勢Ｅ３において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ３を取得することと、
前記第１のトルク、前記第２のトルク、前記第３のトルク、前記第１の位置及び姿勢Ｅ１、前記第２の位置及び姿勢Ｅ２、前記第３の位置及び姿勢Ｅ３、前記トルクＭ１、前記トルクＭ２、前記トルクＭ３、及び前記物体の重量を用いて、前記物体の重心位置を計算することと、
を含む、物体の重心位置の推定方法。

【請求項8】

前記物体の重量をＷとし、
前記第１のトルクをＴ１、前記第２のトルクをＴ２、前記第３のトルクをＴ３とし、
前記第１の位置から前記第２の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ１を計算し、前記第２の位置から前記第３の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ２を計算し、前記第３の位置から前記第１の位置に至るベクトルの、前記軸に垂直でかつ水平な成分の大きさＤ３を計算し、
前記物体の重心位置（Ｐｘ，Ｐｙ，Ｐｚ）を、少なくとも、
（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）＝Ｗ×（Ｄ１−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２）
（Ｔ３−Ｍ３）−（Ｔ２−Ｍ２）＝Ｗ×（Ｄ２−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３）
（Ｔ１−Ｍ１）−（Ｔ３−Ｍ３）＝Ｗ×（Ｄ３−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１）
なる３つの式によって計算する、請求項７に記載の推定方法。

【請求項9】

請求項５〜８のいずれか１項に記載の推定方法を、前記ロボット及び前記ロボットに付随して使用される演算処理装置に実行させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ロボットを用いて負荷の重量及び重心位置を推定するための装置、方法及びプログラムに関する。

【背景技術】

【0002】

通常、ロボットアーム等のロボットの可動部の先端には、ロボットハンドや工具等の物体（負荷）が取り付けられており、該物体の重量と重心位置を知ることは、ロボットを精度よく制御する上で重要である。

【0003】

これに関する従来技術として、物体が取り付けられたロボットアームの回転軸のトルクを利用して、該ロボットアームに取り付けた物体の重量又は重心位置を計算により推定する装置又は方法が知られている（例えば特許文献１−３参照）。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開平０３−１４２１７９号公報

【特許文献2】特開平０９−０９１００４号公報

【特許文献3】特開平０２−３００６４４号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

ロボットの先端に位置するツール等の物体の重量と重心位置を推定する方法として、ロボットの軸を回転させたときの、該軸を駆動するモータの電流を測定し、運動方程式を用いて負荷の重量を計算する方法が知られている。しかしモータ電流を使用する場合、軸に作用する摩擦力を同定する必要があるが、摩擦力を同定するのは一般に困難であり、誤差が生じやすい。

【0006】

また、予めツール等の物体の重量や重心位置を何らかの手段で測定してから該物体をロボットに装着し、作業者が手動でロボット制御装置等に測定結果を入力・設定する方法も考えられるが、測定する手段が容易に入手できなかったり（特に重心位置は測定が難しい）、ツールをロボットに装着した後に、該ツールの重量と重心位置の情報が必要であることが判明したりすることがあるため、物体をロボットに装着した状態で重量や重心位置を推定できる方法、手段が望まれる。

【課題を解決するための手段】

【0007】

本開示の一態様は、ロボットの可動部を駆動する軸に作用するトルクを検出するトルクセンサと、前記可動部に取り付けられた物体が第１の姿勢で第１の位置にあるときに前記トルクセンサで検出された、前記軸に作用する第１のトルクと、前記第１のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１と、前記物体が前記第１の姿勢で前記第１の位置とは異なる第２の位置にあるときに前記トルクセンサで検出された、前記軸に作用する第２のトルクと、前記第２のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２と、前記第１の位置と、前記第２の位置とを用いて、前記物体の重量を計算する計算部と、を有する、物体の重量の推定装置である。

【0008】

本開示の他の態様は、ロボットの可動部を駆動する軸に作用するトルクを検出するトルクセンサと、前記可動部に取り付けられた物体が互いに異なる３つの姿勢にあるときのそれぞれにおいて前記トルクセンサで検出された、前記軸に作用する第１のトルク、第２のトルク及び第３のトルクと、前記第１のトルクを検出したときの前記可動部の第１の位置及び姿勢と、前記第１の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１と、前記第２のトルクを検出したときの前記可動部の第２の位置及び姿勢と、前記第２の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２と、前記第３のトルクを検出したときの前記可動部の第３の位置及び姿勢と、前記第３の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ３と、前記物体の重量とを用いて、前記物体の重心位置を計算する計算部と、を有する、物体の重心位置の推定装置である。

【0009】

本開示のさらなる他の態様は、トルクセンサを有するロボットの可動部に物体を取り付けることと、前記物体が第１の姿勢で第１の位置にあるときに、前記可動部を駆動する軸に作用する第１のトルクを前記トルクセンサで検出することと、前記第１のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１を取得することと、前記物体が前記第１の姿勢で前記第１の位置とは異なる第２の位置にあるときに、前記軸に作用する第２のトルクを前記トルクセンサで検出することと、前記第２のトルクを検出したときの前記可動部の位置及び姿勢において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２を取得することと、前記第１のトルク、前記第２のトルク、前記トルクＭ１、前記トルクＭ２、前記第１の位置及び前記第２の位置を用いて、前記物体の重量を計算することと、を含む、物体の重量の推定方法である。

【0010】

本開示のまたさらなる他の態様は、トルクセンサを有するロボットの可動部に、重量が既知の物体を取り付けることと、前記物体が互いに異なる３つの姿勢にあるときのそれぞれにおいて、前記可動部を駆動する軸に作用する第１のトルク、第２のトルク及び第３のトルクを前記トルクセンサで検出することと、前記第１のトルクを検出したときの前記可動部の第１の位置及び姿勢Ｅ１において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ１を取得し、前記第２のトルクを検出したときの前記可動部の第２の位置及び姿勢Ｅ２において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ２を取得し、前記第３のトルクを検出したときの前記可動部の第３の位置及び姿勢Ｅ３において、前記物体が前記可動部に取り付けられていないときに前記軸に作用するトルクＭ３を取得することと、前記第１のトルク、前記第２のトルク、前記第３のトルク、前記第１の位置及び姿勢Ｅ１、前記第２の位置及び姿勢Ｅ２、前記第３の位置及び姿勢Ｅ３、前記トルクＭ１、前記トルクＭ２、前記トルクＭ３、及び前記物体の重量を用いて、前記物体の重心位置を計算することと、を含む、物体の重心位置の推定方法である。

【発明の効果】

【0011】

本開示によれば、物体をロボットに装着した状態で、ロボットの簡易な動作によって、該物体の重量及び重心位置の少なくとも一方を、高い精度で推定することができる。

【図面の簡単な説明】

【0012】

【図1】好適な実施形態に係る推定装置の一構成例を示す。

【図2】物体の重量を推定する際に、物体が第１の姿勢で第１の位置にある状態を例示する。

【図3】物体の重量を推定する際に、物体が第１の姿勢で第２の位置にある状態を例示する。

【図4】物体の重心位置を推定する際に、物体が第１の姿勢を呈する状態を例示する。

【図5】物体の重心位置を推定する際に、物体が第２の姿勢を呈する状態を例示する。

【発明を実施するための形態】

【0013】

図１は、好適な実施形態に係る推定装置を含むロボットシステム１０の概略構成例を示す。推定装置は、ロボット１４の可動部１２を駆動する軸１６に作用するトルクを検出するトルクセンサ１８を有する。ロボット１４は、後述する動作を行える可動部１２を有するものであればどのような構造のものでもよいが、図示例では、所定位置に設置される基部２０と、基部２０に対して略鉛直方向の軸線回りに旋回可能な旋回胴２２と、旋回胴２２に対して回転可能に取り付けられた上腕２４と、上腕２４に対して回転可能に取り付けられた前腕２６と、前腕２６に対して回転可能に取り付けられた手首部２８とを有する６軸の多関節ロボットである。

【0014】

推定装置は、物体の重量を推定するために使用される場合、可動部１２（ここでは可動部先端に相当する手首部２８）に取り付けられた物体３０が第１の姿勢で第１の位置にあるときにトルクセンサ１８で検出された、軸１６に作用する第１のトルクと、物体３０が該第１の姿勢で第１の位置とは異なる第２の位置にあるときにトルクセンサ１８で検出された、軸１６に作用する第２のトルクと、該第１の位置と、該第２の位置とを用いて、物体３０の重量を計算する計算部３２を有する。なお推定装置は、計算部３２が使用するデータや計算結果等を記憶するための、メモリ等の記憶部３４をさらに有してもよい。

【0015】

また推定装置が物体の重心位置を推定するために使用される場合、推定装置の計算部３２は、可動部１２（ここでは可動部先端に相当する手首部２８）に取り付けられた物体３０が互いに異なる３つの姿勢にあるときのそれぞれにおいてトルクセンサ１８で検出された、軸１６に作用する第１のトルク、第２のトルク及び第３のトルクと、該第１のトルクを検出したときの可動部１２の第１の位置及び姿勢と、該第２のトルクを検出したときの可動部１２の第２の位置及び姿勢と、該第３のトルクを検出したときの可動部１２の第３の位置及び姿勢と用いて、物体３０の重心位置を計算する機能を有する。

【0016】

なお重量又は重心位置を推定すべき物体３０の具体例としては、可動部１２の先端（ここでは手首部２８）に取り付け可能なロボットハンド、工具、溶接トーチ、レーザヘッド等のエンドエフェクタが挙げられ、本開示では負荷と称することもある。従ってロボット１４は、後述する処理によって負荷の重量及び重心位置の少なくとも一方を推定した後は、該負荷をロボット１４から取り外すことなく、推定した値を利用して、ワークの把持や加工等の所定の作業を精度よく行うことができる。或いは、ロボットハンド等で保持可能な部品や加工対象品（ワーク）も、重量又は重心位置を推定すべきものであれば、物体３０に含まれ得る。

【0017】

計算部３２は、上述の物体３０の重量を計算する機能と、物体３０の重心位置を計算する機能との少なくとも一方を有し、図示例では、ロボット１４の動作を制御するロボット制御装置３６内に、プロセッサ等の演算処理装置として組み込むことができる。或いは、ロボット制御装置３６とは別の、パーソナルコンピュータ（ＰＣ）等の計算機（図示せず）に、計算部３２の機能を担わせることもでき、その場合はロボット制御装置３６とＰＣ等の計算機とは、有線又は無線で通信可能に接続されることが好ましい。またロボット制御装置３６は、作業者が各種の設定を行えるように、キーボードやタッチパネル等の適当な入力部（図示せず）を具備してもよい。

【0018】

また計算部３２等のロボット制御装置３６の構成要素は、例えば電算機のＣＰＵ（中央処理装置）等のプロセッサを機能させるためのソフトウェアとしても構成可能である。或いは例えば、当該ソフトウェアの処理の少なくとも一部を実行可能なプロセッサやメモリ等のハードウェアとしても実現可能である。

【0019】

（第１実施例）
次に、推定装置を用いて物体の重量を推定する方法（処理）を、第１の実施例として説明する。先ず、図２に示すように、手首部２８に取り付けられた物体３０が第１の姿勢で第１の位置Ｐ１にあるときに、軸１６に作用するトルク（第１のトルクＴ１）をトルクセンサ１８で検出する。このとき、第１のトルクＴ１と物体３０の重量Ｗとの間には、以下の式（１）が成り立つ。但し式（１）において、Ｍ１は図２の状態（すなわち第１のトルクＴ１を検出したときの可動部１２の位置及び姿勢）において、物体３０がない場合に軸１６に作用するトルクであり、ｄ１は図２の状態において物体３０の重心位置Ｇを通る鉛直線３８と、軸１６との水平方向距離である。
Ｔ１＝Ｍ１＋Ｗ×ｄ１ （１）

【0020】

そして図３に示すように、ロボット１４を動作させて、物体３０を、第１の位置にあったときと同じ姿勢（すなわち第１の姿勢）で、かつ第１の位置とは異なる第２の位置に移動させ、軸１６に作用するトルク（第２のトルクＴ２）をトルクセンサ１８で検出する。このとき、第２のトルクＴ２と物体３０の重量Ｗとの間には、以下の式（２）が成り立つ。但し式（２）において、Ｍ２は図３の状態（すなわち第２のトルクＴ２を検出したときの可動部１２の位置及び姿勢）において、物体３０がない場合に軸１６に作用するトルクであり、ｄ２は図３の状態において物体３０の重心位置Ｇを通る鉛直線と、軸１６との水平方向距離である。
Ｔ２＝Ｍ２＋Ｗ×ｄ２ （２）

【0021】

式（１）及び（２）から、以下の式（３）が得られる。
Ｔ２−Ｔ１＝Ｍ２−Ｍ１＋Ｗ×（ｄ２−ｄ１） （３）

【0022】

ここで、第１の位置及び第２の位置における物体３０の姿勢は同じであるから、物体が第１の位置から第２の位置まで移動したときの、ツール代表点等の可動部の代表位置（ここでは可動部先端に相当する手首部２８）の水平方向移動距離Ｄと、物体の重心位置Ｇの水平方向の移動距離（ｄ２−ｄ１）は一致する。手首部２８の水平方向移動距離Ｄは、ロボット制御装置３６からロボット１４に送られた移動指令や、ロボット１４の各軸に設けたエンコーダの値等を用いれば容易に知ることができるので、式（３）を変形すると、以下の式（４）が得られる。
Ｗ＝（（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１））／Ｄ （４）

【0023】

ここでＭ１及びＭ２は、第１及び第２のトルクの検出前又は検出後に、物体３０をロボット１４に取り付けない状態で、トルクセンサ１８を用いて軸１６に作用するトルクとして検出して取得してもよいし、或いは、計算部３２等がロボット１４の各部の質量や重心位置等を用いた計算を行うことによって取得することもできる。後者の具体例としては、ニュートンオイラー法が挙げられる。

【0024】

よって式（４）の右辺は全て既知の情報であるので、同式から、物体３０の重量Ｗが求まる。なお本実施例ではｄ１＜ｄ２の場合を考えたが、ｄ１＞ｄ２の場合も考慮すると、以下の式（５）を用いることが好ましい。
Ｗ＝｜（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）｜／Ｄ （５）

【0025】

式（５）からわかるように、物体３０の重量Ｗを推定するためには、Ｔ１、Ｔ２、Ｍ１、Ｍ２及びＤが与えられれば足り、ｄ１及びｄ２そのものは求める必要がない。従って物体の重心位置Ｇが未知であっても、上述の動作をロボット１４に行わせることにより、物体の重量Ｗを正確に推定することができる。なお第１の位置及び第２の位置には特に制約はないが、両位置間の水平方向距離Ｄ（ｄ２−ｄ１）はゼロではない。

【0026】

また第１の実施例では、第１の位置及び第２の位置のそれぞれにおいてロボット１４を静止させて第１及び第２のトルクを検出してもよいし、ロボット１４の可動部１２が一定速度で動いている（つまり可動部１２の加速度がゼロである）間に、任意の２つのタイミング（但し物体３０の姿勢は同一）で検出したトルク値をそれぞれ第１及び第２のトルクとしてもよい。いずれの場合であっても上記の式（１）−（５）は成り立つからである。

【0027】

（第２実施例）
次に、推定装置を用いて物体の重心位置を推定する方法（処理）を、第２の実施例として説明する。先ず、図４に示すように、手首部２８に取り付けられた物体３０が第１の姿勢で第１の位置にあるときに、軸１６に作用するトルク（第１のトルクＴ１）をトルクセンサ１８で検出（測定）する。このとき、第１のトルクＴ１と物体３０の重量Ｗとの間には、以下の式（６）が成り立つ。但し式（６）において、Ｍ１は図４の状態（すなわち第１のトルクＴ１を検出したときの可動部１２の位置及び姿勢）において、物体３０がない場合に軸１６に作用するトルクであり、ｄ１は図４の状態において物体３０の重心位置Ｇを通る鉛直線３８と、軸１６との水平方向距離である。
Ｔ１＝Ｍ１＋Ｗ×ｄ１ （６）

【0028】

そして図５に示すように、ロボット１４を動作させて、物体３０を第１の姿勢とは異なる第２の姿勢となるように移動させ、軸１６に作用するトルク（第２のトルクＴ２）をトルクセンサ１８で検出（測定）する。このとき、第２のトルクＴ２と物体３０の重量Ｗとの間には、以下の式（７）が成り立つ。但し式（７）において、Ｍ２は図５の状態（すなわち第２のトルクＴ２を検出したときの可動部１２の位置及び姿勢）において、物体３０がない場合に軸１６に作用するトルクであり、ｄ２は図５の状態において物体３０の重心位置Ｇを通る鉛直線３８と、軸１６との水平方向距離である。
Ｔ２＝Ｍ２＋Ｗ×ｄ２ （７）

【0029】

式（６）及び（７）から、以下の式（８）が得られる。
（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）＝Ｗ×（ｄ２−ｄ１） （８）

【0030】

ここで、ツール代表点等の可動部１２の代表位置（ここでは可動部先端に相当する手首部２８）の水平方向の移動距離Ｄ１、物体３０の重心位置Ｇを表す座標（Ｐｘ，Ｐｙ，Ｐｚ）を第１の姿勢において水平方向に射影した長さ（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１、及び、物体３０の重心位置（Ｐｘ，Ｐｙ，Ｐｚ）を第２の姿勢において水平方向に射影した長さ（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２を用いると、（ｄ２−ｄ１）は以下の式（９）で表される。但し式（９）において、「・」は内積を表し、Ｅ１及びＥ２はそれぞれ、第１及び第２の姿勢について可動部先端（ここでは手首部２８）に定義した先端座標系４０における水平方向ベクトルの単位ベクトルであり、これらはロボット制御装置３６からロボット１４に送られた移動指令や、ロボット１４の各軸に設けたエンコーダの値等を用いれば容易に求めることができる。例えば、図４の例ではＥ１＝（１，０，０）となり、図５の例ではＥ２＝（１／√２，０，１／√２）となる（先端座標系４０がＹ軸回りに４５°回転している）。但し第２の実施例では、手首部等に定義された先端座標系４０と、物体３０との相対位置関係は既知であるとし、これは第１の実施例でも同様である。
ｄ２−ｄ１＝Ｄ１−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２（９）

【0031】

式（８）及び（９）から、以下の式（１０）が得られる。
（Ｔ２−Ｍ２）−（Ｔ１−Ｍ１）＝Ｗ×（Ｄ１−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２） （１０）

【0032】

次に、ロボット１４を動作させて、物体３０を第１及び第２の姿勢のいずれとも異なる第３の姿勢となるように移動させ、軸１６に作用するトルク（第３のトルクＴ３）をトルクセンサ１８で検出（測定）する。このとき、第３のトルクＴ３と物体３０の重量Ｗとの間には、以下の式（１１）が成り立つ。但し式（１１）において、Ｍ３は第３のトルクＴ３を検出したときの可動部の位置及び姿勢において、物体３０がない場合に軸１６に作用するトルクであり、ｄ３は第３のトルクＴ３を検出したときと同じ可動部の位置及び姿勢において、物体３０の重心位置を通る鉛直線と、軸１６との水平方向距離である。
Ｔ３＝Ｍ３＋Ｗ×ｄ３ （１１）

【0033】

式（８）−（１０）に関する説明と同様の処理により、以下の式（１２）及び（１３）が得られる。つまり式（１０）が第１の姿勢と第２の姿勢との関係で得られた式であるのに対し、式（１２）は第２の姿勢と第３の姿勢との関係で得られた式であり、式（１３）は第３の姿勢と第１の姿勢との関係で得られた式である。なおＥ３は、第３の姿勢について可動部先端（ここでは手首部２８）に定義した先端座標系４０における水平方向ベクトルの単位ベクトルであり、これはＥ１又はＥ２と同様、ロボット制御装置３６からロボット１４に送られた移動指令や、ロボット１４の各軸に設けたエンコーダの値等を用いれば容易に求めることができる。
（Ｔ３−Ｍ３）−（Ｔ２−Ｍ２）＝Ｗ×（Ｄ２−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ２＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３） （１２）
（Ｔ１−Ｍ１）−（Ｔ３−Ｍ３）＝Ｗ×（Ｄ３−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ３＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅ１） （１３）

【0034】

手首部２８の水平方向移動距離Ｄ１−Ｄ３は、ロボット制御装置３６からロボット１４に送られた移動指令や、ロボット１４の各軸に設けたエンコーダの値等を用いれば容易に知ることができる。またＭ１−Ｍ３は、第１−第３のトルクの検出前又は検出後に、物体３０をロボット１４に取り付けない状態で、トルクセンサ１８を用いて軸１６に作用するトルクとして検出して取得してもよいし、或いは、計算部３２等がロボット１４の各部の質量や重心位置等を用いた計算を行うことによって取得することもできる。後者の具体例としては、ニュートンオイラー法が挙げられる。

【0035】

上述の処理により、３つの未知数（Ｐｘ，Ｐｙ，Ｐｚ）に対して、３つの等式（１０）、（１２）及び（１３）が得られる。従ってこれらの等式を連立方程式として解くことにより、物体３０の重心位置（Ｐｘ，Ｐｙ，Ｐｚ）を求めることができる。なお上式（１０）、（１２）及び（１３）を一般化すると、以下の式（１４）のようになる。但しｉ、ｊはいずれも１以上の整数であり、それらの上限は物体３０がロボット１４の動作によって呈し得る姿勢の個数に相当する。
（Ｔｉ−Ｍｉ）−（Ｔｊ−Ｍｊ）＝Ｗ×（Ｄｉ−（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅｉ＋（Ｐｘ，Ｐｙ，Ｐｚ）・Ｅｊ） （１４）

【0036】

式（１０）、（１２）及び（１３）からわかるように、物体３０の重心位置を推定するためには、Ｔ１−Ｔ３、Ｍ１−Ｍ３、Ｅ１−Ｅ３、Ｄ１−Ｄ３及びＷが与えられれば足りる。従って物体の重量が既知であれば、上述の動作をロボット１４に行わせることにより、物体の重心位置（座標）を正確に推定することができる。なおここでの制約条件は、両位置間の水平方向距離Ｄ１−Ｄ３がゼロではなく、かつＥ１ｘ＝Ｅ２ｘ＝Ｅ３ｘではなく、かつＥ１ｙ＝Ｅ２ｙ＝Ｅ３ｙではなく、かつＥ１ｚ＝Ｅ２ｚ＝Ｅ３ｚではないことである。但し、Ｅ１＝（Ｅ１ｘ，Ｅ１ｙ，Ｅ１ｚ）、Ｅ２＝（Ｅ２ｘ，Ｅ２ｙ，Ｅ２ｚ）、Ｅ３＝（Ｅ３ｘ，Ｅ３ｙ，Ｅ３ｚ）であるとする。

【0037】

また第２の実施例では、第１−第３の位置のそれぞれにおいてロボット１４を静止させて第１及び第２のトルクを検出してもよいし、ロボット１４の可動部１２が一定速度で動いている（例えば角速度が一定）間に、任意の３つのタイミング（但し物体３０の姿勢は互いに異なる）で検出したトルク値をそれぞれ第１及び第２のトルクとしてもよい。いずれの場合であっても上記の式（６）−（１４）は成り立つからである。

【0038】

なお第１の実施例と第２の実施例はそれぞれ個別に行うことができるが、両実施例を連続的に行うこともできる。すなわち、先ず第１の実施例で説明した処理によって物体の重量Ｗを推定し、推定された重量Ｗを用いて、第２の実施例で説明した処理によって物体の重心位置Ｇを推定することができる。この場合、第１の実施例における第２の位置及び姿勢（図３）は、第２の実施例における第１の位置及び姿勢（図４）として扱うこともできるので、両実施例を連続的に行う場合、それぞれ個別に行う場合よりも、ロボットの動作の回数を減らすことができる。

【0039】

第１及び第２の実施例におけるトルクセンサ１８としては、非接触式（磁歪式）や接触式（スリップリング）等、特定の軸に作用するトルクを検出（測定）できる種々のトルクセンサが利用可能である。なお本開示において、トルクセンサで検出される「軸に作用するトルク」（トルク検出値）は、ロボットの各軸を駆動するモータのトルクから摩擦等の影響を差し引いた値（又はこれに重力の影響を付加したもの）に相当するので、トルクセンサの使用により、一般に困難かつ精度が低いとされる摩擦の推定・同定を行うことなく、物体の重量又は重心位置を、少なくとも実用的には十分高い精度で推定することができる。但しトルクセンサ１８は、重力方向のトルクを検出でき、かつトルクセンサ１８より先端側のロボット可動部によって、重量測定のための動作（物体の姿勢が等しい２つの位置に移動）か、重心位置測定のための動作（物体の姿勢が互いに異なる３つの位置に移動）が実現できることが要求される。従って例えば、上記実施例ではトルクセンサ１８は上腕２４の駆動軸１６に作用するトルクを検出するが、前腕２６及び手首部２８のみによって上記動作が実現できるのであれば、前腕２６の駆動軸４２（図１参照）に作用するトルクを検出するトルクセンサを用いてもよい。

【0040】

或いはトルクセンサとして、例えばロボット１４の基部２０の内部又は下部に設けられた、６軸の力覚センサ（トルクセンサ）４４（図１参照）を使用することもできる。このようなトルクセンサ４４を使用すれば、ロボット１４に作用する、互いに直交する水平方向の２軸のトルクを測定できるが、例えば重心位置を推定する場合（第２の実施例）、式（１０）、（１２）及び（１３）で表したような連立方程式をそれぞれの方向について（つまり２組（計６つ）の等式を）得ることができる。この場合、２組の連立方程式の解（重心位置）が異なることもあるが、それぞれの解の平均を使用する等、推定精度をさらに高めることができる。このように本開示では、未知数を求めるための最小限の等式の個数を超える等式が得られることは、推定のための情報量が増えることを意味するので、平均値や最小二乗法等を利用して、推定精度を高めることができる。

【0041】

また上述の６軸のトルクセンサ４４や、ロボット１４の外表面に設けられた接触センサ（図示せず）等をロボット１４に使用した場合、ロボット１４は、人間や周辺物等との接触を検知する機能を具備することができる。このような接触検知機能を有するロボットは、人間と作業領域を共用する協働ロボットとして使用可能であるが、接触検知の精度が低いと、ロボットが誤検知をして頻繁に非常停止する等の不具合が生じ得る。しかし本開示によれば、物体の重量又は重心位置を正確に推定できるので、トルクセンサが検出するトルク値から、ロボットの重量によるトルク及び慣性トルク、並びに可動部に取り付けられた負荷の重量によるトルク及び慣性トルクを引くことにより、ロボットに作用する外力を正確に検知することができるようになる。

【0042】

本開示では、上述の処理をロボット、及び推定装置等のロボットに付随して使用される演算処理装置に実行させるためのプログラムを、推定装置の記憶部又は他の記憶装置に記憶させることができる。またプログラムは、該プログラムを記録した、コンピュータが読み取り可能な記録媒体（ＣＤ−ＲＯＭ、ＵＳＢメモリ等）としても提供可能である。

【符号の説明】

【0043】

１０ ロボットシステム
１２ 可動部
１４ ロボット
１６、４２ 駆動軸
１８、４４ トルクセンサ
２０ 基部
２２ 旋回胴
２４ 上腕
２６ 前腕
２８ 手首部
３０ 物体
３２ 計算部
３４ 記憶部
３６ ロボット制御装置
４０ 先端座標系

【図1】

【図2】

【図3】

【図4】

【図5】