特許 5980084 ロボットハンドの誤差検出方法

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

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

(11)【特許番号】5980084

(24)【登録日】2016年8月5日

(45)【発行日】2016年8月31日

(54)【発明の名称】ロボットハンドの誤差検出方法

(51)【国際特許分類】

   B25J 9/10 20060101AFI20160818BHJP

【ＦＩ】

   B25J9/10 A

【請求項の数】2

【全頁数】10

(21)【出願番号】特願2012-225573(P2012-225573)

(22)【出願日】2012年10月11日

(65)【公開番号】特開2014-76517(P2014-76517A)

(43)【公開日】2014年5月1日

【審査請求日】2015年10月9日

(73)【特許権者】

【識別番号】591172054

【氏名又は名称】株式会社明和ｅテック

(74)【代理人】

【識別番号】100107700

【弁理士】

【氏名又は名称】守田 賢一

(72)【発明者】

【氏名】石川 坂幸

(72)【発明者】

【氏名】松井 勇志

(72)【発明者】

【氏名】山田 浩臣

【審査官】 臼井 卓巳

(56)【参考文献】

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

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

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

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

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

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

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

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

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

【請求項1】

関節によって相対回転可能に互いに直列に連結された複数のアームより構成されるロボットハンドの誤差検出方法であって、前記各アームを単独で回転させた際のロボットハンド先端の先端位置ベクトルをそれぞれ三点測定して、これら測定された先端位置ベクトルに基づいて前記各アームの回転時における回転軸ベクトルと回転面の中心ベクトルをそれぞれ算出するステップと、前記回転軸ベクトルおよび前記中心ベクトルより前記各関節の関節位置ベクトルと前記各アームのアーム長ベクトルを算出するステップと、算出された前記関節位置ベクトルと予め記憶されたマスタ関節位置ベクトルの差である関節位置誤差ベクトルを算出するステップと、前記関節位置誤差ベクトル、前記回転軸ベクトルおよびアーム長ベクトルより、各アームの角度と予め記憶されたマスタ角度との誤差角度を算出するステップとを具備するロボットハンドの誤差検出方法。

【請求項2】

前記誤差角度を使用して前記関節位置ベクトル、前記アーム長ベクトル、前記関節位置誤差ベクトルの少なくとも一つを補償するステップをさらに備える請求項１に記載のロボットハンドの誤差検出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明はロボットハンドの誤差検出方法に関し、特に、多関節ロボットハンドの誤差検出に好適に使用できる誤差検出方法に関する。

【背景技術】

【0002】

多関節ロボットハンドでは各関節に生じた誤差が、工具等を把持して作業を行うロボットハンド先端の誤差となって現れる。そこで、各関節に生じた誤差を特定するための誤差検出方法が提案されており、例えば特許文献１においては、ロボットハンド先端に装着したカメラでロボットのベース近傍の固定被写点を撮影し、ロボットハンドを複数の異なる姿勢にした際の、各姿勢において撮影された固定被写点像の位置と理論上の位置との差を算出して、これらの差より各関節に生じた誤差を検出する方法が示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開平５−８１８５

【発明の概要】

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

【0004】

しかし、上記従来の誤差検出方法では、各関節のアームの角度ずれ（誤差角度）の絶対値を検出することができないという問題があった。

【0005】

そこで、本発明はこのような課題を解決するもので、ロボットハンドを構成する各関節におけるアームの角度ずれ（誤差角度）の絶対値を検出することが可能なロボットハンドの誤差検出方法を提供することを目的とする。

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

【0006】

上記目的を達成するために、本第１発明は、関節（Ｐ1〜Ｐ6）によって相対回転可能に互いに直列に連結された複数のアーム（Ｌ1〜Ｌ6）より構成されるロボットハンド（１）の誤差検出方法であって、前記各アーム（Ｌ1〜Ｌ6）を単独で回転させた際のロボットハンド先端（Ｐ７）の先端位置ベクトルをそれぞれ三点測定して、これら測定された先端位置ベクトル（Ｐ7An*,Ｐ7Bn*，Ｐ7Cn*）に基づいて前記各アーム（Ｌ1〜Ｌ6）の回転時における回転軸ベクトル(ｋn*)と回転面の中心ベクトル（Ｏn*）をそれぞれ算出するステップと、前記回転軸ベクトル(ｋn*)および前記中心ベクトル（Ｏn*）より前記各関節（Ｐ1〜Ｐ6）の関節位置ベクトル（Ｐn*）と前記各アーム（Ｌ1〜Ｌ6）のアーム長ベクトル（Ｌn*）を算出するステップと、算出された前記関節位置ベクトルと予め記憶されたマスタ関節位置ベクトルの差である関節位置誤差ベクトル（ΔＰn*）を算出するステップと、前記関節位置誤差ベクトル（ΔＰn*）、前記回転軸ベクトル(ｋn*)およびアーム長ベクトル（Ｌn*）より、各アーム（Ｌ1〜Ｌ6）の角度と予め記憶されたマスタ角度との誤差角度（Δθn）を算出するステップとを具備する。

【0007】

本第１発明によれば、ロボットハンドを構成する各関節のアームの角度ずれ（誤差角度）の絶対値を正確に検出することができる。

【0008】

本第２発明では、前記誤差角度（Δθn）を使用して前記関節位置ベクトル（Ｐn*）、前記アーム長ベクトル（Ｌn*）、前記関節位置誤差ベクトル（ΔＰn*）の少なくとも一つを補償するステップをさらに備える。

【0009】

本第２発明によれば、各関節のアームの誤差角度に基づいて、関節位置ベクトル、アーム長ベクトル、関節位置誤差ベクトルの少なくとも一つを良好に補償することができる。

【発明の効果】

【0010】

以上のように、本発明のロボットハンドの誤差検出方法によれば、ロボットハンドを構成する各関節におけるアームの角度ずれ（誤差角度）の絶対値を精度良く検出することができる。

【図面の簡単な説明】

【0011】

【図1】本発明方法を実施する装置の構成を示す図である。

【図2】多関節ロボットハンドの概念図である。

【図3】基準座標系と平面座標系の関係を示す図である。

【図4】平面座標系上での幾何学的位置関係を示す図である。

【図5】平面座標系上での幾何学的位置関係を示す図である。

【図6】ロボットハンドの一部の概念図である。

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

【0012】

なお、以下に説明する実施形態はあくまで一例であり、本発明の要旨を逸脱しない範囲で当業者が行う種々の設計的改良も本発明の範囲に含まれる。

【0013】

図１は本発明を実施する装置の構成を示すもので、多関節ロボットハンド１の先端に装着された測定用治具２を、カメラ３で撮像する。カメラ３はコンピュータ４に接続されており、コンピュータ４のプログラムによって、以下に説明する誤差検出および誤差補償の手順が実行される。コンピュータ４はカメラ３を介し、必要に応じて、固定された基準点Ｓｔ（基準座標系の原点となる）を撮像して３次元認識の精度を維持する。

【0014】

図２には多関節ロボットハンド１の一例を概念的に示す。ロボットハンド１の各関節Ｐ1〜Ｐ6は地上側から6箇所にあり、Ｐ7はロボットハンド１の先端である。各関節Ｐ1〜Ｐ6および先端Ｐ7（測定用治具の先端）はアームＬ1〜Ｌ6で連結されており、本実施形態では、Ｐ1は固定関節、Ｐ2，Ｐ3，Ｐ5はそれぞれアームＬ2，Ｌ3，Ｌ5を旋回回転させる関節、Ｐ4，Ｐ6はそれぞれアームＬ4，Ｌ6をその軸周りに回転させる関節となっている。

【0015】

（旋回回転する関節の誤差検出）
最初に各関節Ｐn（n＝2〜6）をそれぞれ回転させて、その時の、先端Ｐ7の軌跡のうちから３点のベクトル座標Ｐ7An*,Ｐ7Bn*，Ｐ7Cn*を得る。なお、以下、*印を付した記号はベクトルであることを示す。続いて下式（１）〜（３）で示される３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*を通る旋回軌跡の回転半径ｄnを算出する。回転半径ｄnは旋回回転する関節Ｐn（n＝２，３，５）についてのみ算出することができる。また、下式（１）〜（３）においてｍ＞ｎである。

【0016】

【数1】

【0017】

上記回転半径ｄnの算出は以下の手順で行う。
最初に上記３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*を通る回転軌跡が存在する回転面の、中心ベクトルＯn*を以下のように計算する。

【0018】

図３に示すように、上記３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*（図中は（n）を省略している）が含まれる平面（平面座標系）の原点をＧ1（n）*とし、点Ｇ1（n）*,Ｇ2（n）*間のベクトルをＶ1*、点Ｇ１（n）*,Ｇ3（n）*間のベクトルをＶ2*とすると、平面座標系の単位ベクトルｉ*，ｊ*，ｋ*は下式（４）〜（６）で求められる。ここでｋ*は上記平面に垂直な、回転軸の向きを示す単位方向ベクトルである。

【0019】

【数2】

【0020】

点Ｇ1（n）*の、基準座標系での座標値を（Ｘ1,Ｙ1,Ｚ1）とし、単位ベクトルｉ*，ｊ*，ｋ*の各座標値をそれぞれ（ｉx,ｉy,ｉz）（ｊx,ｊy,ｊz）（ｋx,ｋy,ｋz）とすると、変換行列Ｔpは下式（７）のようになる。

【0021】

【数3】

【0022】

これにより、点Ｇ2（n）*,Ｇ3（n）*は下式（８），（９）によって平面座標系上の点Ｇ2（n）*´,Ｇ3（n）*´に変換される（図３）。

【0023】

【数4】

【0024】

上記３点Ｇ1（n）*´, Ｇ2（n）*´, Ｇ3（n）*´,を通る円（旋回軌跡）の平面座標系上での中心Ｏn*´の座標（uc,vc,0）は図４に示す幾何学的位置関係より、下式（１０）のようなものとなる。

【0025】

【数5】

【0026】

なお、中心Ｏn*´から各点Ｇ1（n）*´,Ｇ2（n）*´,Ｇ3（n）*´へのベクトルを、図５に示す(図中は（n）を省略している)ようにそれぞれＶcＧ1*´，ＶcＧ2*´，ＶcＧ*3´とすると、点Ｇ1（n）*´,Ｇ2（n）*´間の角度θ12、およびＧ2（n）*´,Ｇ3（n）*´間の角度θ23は下式（１１），（１２）で表される。

【0027】

【数6】

【0028】

平面座標系上で位置が特定された中心Ｏn*´は下式（１３）によって基準座標系上の中心Ｏn*に変換される。
Ｏn*＝Ｔp・Ｏn*´…（１３）
これにより、回転半径ｄnがｄn＝｜Ｇ1*−Ｏn*｜で算出される。

【0029】

続いて、各アームＬ2,Ｌ3,Ｌ5のアーム長Ｌn（n＝２，３，５）を、各アームのその回転軸に対する設計上の角度φn（φn≠０）と上記回転半径ｄnより、下式（１４）で算出する。
Ｌn＝ｄn／sinφn…（１４）
なお、φn＝０の場合のＬｎ（n＝４，６）は予め測定しておく。

【0030】

その後、各関節の位置ベクトルＰn*（n＝１〜６）および各アームの長さベクトルＬn*を下式（１５），（１６）によって算出する。

【0031】

【数7】

【0032】

上式（１５）中のｋn*は関節Ｐnの回転軸の単位方向ベクトルである。また上式（１５），（１６）の算出はn＝６→１の順で行う。
ロボットハンド１の設置初期等に上記手順で算出されたＰn*、Ｌn*の値はマスタ値として予めコンピュータ４内に記憶される。

【0033】

ロボットハンド１を使用している過程で、上記と同様の手順で各関節Ｐ1〜Ｐ6の位置ベクトルＰn*(n＝１〜６)が算出され、位置ベクトルＰn*の上記マスタ値との誤差ΔＰn*が算出される。そして、下式（１７），（１８）によって各関節Ｐn（n＝１〜３）について、その回転軸の、マスタ角度との誤差（ずれ）角度Δθnが検出（算出）される。検出された誤差角度Δθnが所定値よりも大きい場合には、ロボットハンド１の異常として該当する関節Ｐn（ｎ＝１〜６）を報知する。検出された誤差角度Δθnが所定値よりも小さい場合には、後述する誤差補償が行なわれる。

【0034】

【数8】

【0035】

ここで、上式（１７）中、ΔＰmn*は、ｎ番目の関節Ｐnの、位置誤差の影響を受けたｍ番目の関節Ｐmの位置誤差ベクトルを示す。また、ｍ＞ｎ≧１である。

【0036】

（誤差補償）
次に、n＝１〜３の各Ｌn*,ｋn*,Ｐn*,ΔＰn*について、Δθnの影響を取り除く（誤差補償）。これは下式（１９）で示される回転行列Rot（ｋ*,θ)を使用して以下のように行われる。
Rot（ｋ*,θ)＝cosθ・Ｉ3＋（１−cosθ）ｋ*・tｋ*＋sinθ［ｋ*×］…（１９）
ここで、Ｉ3は３次の単位行列、tｋ*は単位ベクトルｋ*の転置ベクトル、［ｋ*×］は単位ベクトルｋ*の外積行列である。

【0037】

すなわち、誤差補償後のＬn*,ｋn*,Ｐn*,ΔＰn*をそれぞれＬm*´,ｋm*´,Ｐm*´,ΔＰm*´とすると、これらは下式（２０）〜（２３）を使用して算出（補償）できる。
Ｌm*´＝Rot（ｋn*,−Δθn) Ｌm*…（２０）
ｋm*´＝Rot（ｋn*,−Δθn) ｋm*…（２１）
Ｐm*´＝Rot（ｋn*,−Δθn) （Ｐm*−Ｐn*）＋Ｐn*…（２２）
ΔＰm*´＝Rot（ｋn*,−Δθn)ΔＰm*…（２３）

【0038】

（軸回りに回転する関節の誤差検出・補償）
上式（２０）〜（２３）を使用するに際して、Ｐn（n＝４，６）の関節はそれぞれアームＬn（n＝４，６）をその軸周りに回転させるものであり、これら関節Ｐn（n＝４，６）では回転軸とアームが平行であるから、上式（１８）における｜ｋn*×Ｌn*｜＝０となってΔθnが算出できない。

【0039】

そこでこの場合、図６で、｜Ｐ5*−Ｐ4*｜はアームＬ4の回転で変化せず、また｜Ｏ6*−Ｐ5*｜がアームＬ6の回転で変化しないこと、およびＯ6*にはΔθn（n＝１〜３）の影響がないので｜Ｏ6*−Ｐ4*｜にはΔθ5以外のΔθnの影響がないこと、を考慮すると、図６に示す幾何学的な関係からΔθ5は下式（２４）で算出される。

【数9】

【0040】

算出（検出）されたΔθ5により上式（２０）〜（２３）によってＬ5*,ｋ5*,Ｐ5*,ΔＰ5*の誤差補償が行われ、あるいは当該関節Ｐ6の異常が報知される。

【0041】

次にΔθ4を算出（検出）する。Ｏ6*にはΔθ4以外のΔθnの影響は無いから、下式（２５）によってΔθ4が算出され、上式（２０）〜（２３）によってＬ4*,ｋ4*,Ｐ4*,ΔＰ4*の誤差補償が行われ、あるいは当該関節Ｐ4の異常が報知される。
｜ΔＯ6*｜²＝２｜Ｒ4*×（Ｏ6*−Ｐ4*）｜²(１−cosΔθ4)…（２５）

【0042】

Δθ6については、Ｐ7*とＯ6*が重ならないように測定治具を予め調節しておく（図６参照）ことによって、上式（１）〜（１８）によってΔθ6が算出（検出）され、その後、上式（２０）〜（２３）によってＬ6*,ｋ6*,Ｐ6*,ΔＰ6*の誤差補償が行われ、あるいは当該関節Ｐ4の異常が報知される。

【符号の説明】

【0043】

１…ロボットハンド、２…測定用治具、３…カメラ、４…コンピュータ、Ｐ1,Ｐ2,Ｐ3,Ｐ4，Ｐ5,Ｐ6…関節、Ｌ1,Ｌ2,Ｌ3,Ｌ4,Ｌ5,Ｌ6…アーム。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】