(57)【要約】      （修正有）【課題】ロボットを構成する各関節角度ずれ（誤差角度）の絶対値を検出することが可能な誤差検出方法を提供すること。【解決手段】各アーム（Ｌ1〜Ｌ6）を単独で回転させた際のロボットハンド先端（Ｐ７）の先端位置ベクトルをそれぞれ三点測定して、これら測定された先端位置ベクトルに基づいて各アームの回転時における回転軸ベクトルと回転面の中心ベクトルをそれぞれ算出するステップと、回転軸ベクトルおよび中心ベクトルより各関節の関節位置ベクトルと各アームのアーム長ベクトルを算出するステップと、算出された関節位置ベクトルと予め記憶されたマスタ関節位置ベクトルの差である関節位置誤差ベクトルを算出するステップと、関節位置誤差ベクトル、回転軸ベクトルおよびアーム長ベクトルより、各アームの角度と予め記憶されたマスタ角度との誤差角度を算出するステップとを備える。【選択図】図２
2014076517.tif 000002
【特許請求の範囲】
【請求項１】
関節によって相対回転可能に互いに直列に連結された複数のアームより構成されるロボットハンドの誤差検出方法であって、前記各アームを単独で回転させた際のロボットハンド先端の先端位置ベクトルをそれぞれ三点測定して、これら測定された先端位置ベクトルに基づいて前記各アームの回転時における回転軸ベクトルと回転面の中心ベクトルをそれぞれ算出するステップと、前記回転軸ベクトルおよび前記中心ベクトルより前記各関節の関節位置ベクトルと前記各アームのアーム長ベクトルを算出するステップと、算出された前記関節位置ベクトルと予め記憶されたマスタ関節位置ベクトルの差である関節位置誤差ベクトルを算出するステップと、前記関節位置誤差ベクトル、前記回転軸ベクトルおよびアーム長ベクトルより、各アームの角度と予め記憶されたマスタ角度との誤差角度を算出するステップとを具備するロボットハンドの誤差検出方法。
【請求項２】
前記誤差角度を使用して前記関節位置ベクトル、前記アーム長ベクトル、前記関節位置誤差ベクトルの少なくとも一つを補償するステップをさらに備える請求項１に記載のロボットハンドの誤差検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットハンドの誤差検出方法に関し、特に、多関節ロボットハンドの誤差検出に好適に使用できる誤差検出方法に関する。
【背景技術】
【０００２】
多関節ロボットハンドでは各関節に生じた誤差が、工具等を把持して作業を行うロボットハンド先端の誤差となって現れる。そこで、各関節に生じた誤差を特定するための誤差検出方法が提案されており、例えば特許文献１においては、ロボットハンド先端に装着したカメラでロボットのベース近傍の固定被写点を撮影し、ロボットハンドを複数の異なる姿勢にした際の、各姿勢において撮影された固定被写点像の位置と理論上の位置との差を算出して、これらの差より各関節に生じた誤差を検出する方法が示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平５−８１８５
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、上記従来の誤差検出方法では、各関節のアームの角度ずれ（誤差角度）の絶対値を検出することができないという問題があった。
【０００５】
そこで、本発明はこのような課題を解決するもので、ロボットハンドを構成する各関節におけるアームの角度ずれ（誤差角度）の絶対値を検出することが可能なロボットハンドの誤差検出方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するために、本第１発明は、関節（Ｐ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）を算出するステップとを具備する。
【０００７】
本第１発明によれば、ロボットハンドを構成する各関節のアームの角度ずれ（誤差角度）の絶対値を正確に検出することができる。
【０００８】
本第２発明では、前記誤差角度（Δθn）を使用して前記関節位置ベクトル（Ｐn*）、前記アーム長ベクトル（Ｌn*）、前記関節位置誤差ベクトル（ΔＰn*）の少なくとも一つを補償するステップをさらに備える。
【０００９】
本第２発明によれば、各関節のアームの誤差角度に基づいて、関節位置ベクトル、アーム長ベクトル、関節位置誤差ベクトルの少なくとも一つを良好に補償することができる。
【発明の効果】
【００１０】
以上のように、本発明のロボットハンドの誤差検出方法によれば、ロボットハンドを構成する各関節におけるアームの角度ずれ（誤差角度）の絶対値を精度良く検出することができる。
【図面の簡単な説明】
【００１１】
【図１】本発明方法を実施する装置の構成を示す図である。
【図２】多関節ロボットハンドの概念図である。
【図３】基準座標系と平面座標系の関係を示す図である。
【図４】平面座標系上での幾何学的位置関係を示す図である。
【図５】平面座標系上での幾何学的位置関係を示す図である。
【図６】ロボットハンドの一部の概念図である。
【発明を実施するための形態】
【００１２】
なお、以下に説明する実施形態はあくまで一例であり、本発明の要旨を逸脱しない範囲で当業者が行う種々の設計的改良も本発明の範囲に含まれる。
【００１３】
図１は本発明を実施する装置の構成を示すもので、多関節ロボットハンド１の先端に装着された測定用治具２を、カメラ３で撮像する。カメラ３はコンピュータ４に接続されており、コンピュータ４のプログラムによって、以下に説明する誤差検出および誤差補償の手順が実行される。コンピュータ４はカメラ３を介し、必要に応じて、固定された基準点Ｓｔ（基準座標系の原点となる）を撮像して３次元認識の精度を維持する。
【００１４】
図２には多関節ロボットハンド１の一例を概念的に示す。ロボットハンド１の各関節Ｐ1〜Ｐ6は地上側から6箇所にあり、Ｐ7はロボットハンド１の先端である。各関節Ｐ1〜Ｐ6および先端Ｐ7（測定用治具の先端）はアームＬ1〜Ｌ6で連結されており、本実施形態では、Ｐ1は固定関節、Ｐ2，Ｐ3，Ｐ5はそれぞれアームＬ2，Ｌ3，Ｌ5を旋回回転させる関節、Ｐ4，Ｐ6はそれぞれアームＬ4，Ｌ6をその軸周りに回転させる関節となっている。
【００１５】
（旋回回転する関節の誤差検出）
最初に各関節Ｐn（n＝2〜6）をそれぞれ回転させて、その時の、先端Ｐ7の軌跡のうちから３点のベクトル座標Ｐ7An*,Ｐ7Bn*，Ｐ7Cn*を得る。なお、以下、*印を付した記号はベクトルであることを示す。続いて下式（１）〜（３）で示される３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*を通る旋回軌跡の回転半径ｄnを算出する。回転半径ｄnは旋回回転する関節Ｐn（n＝２，３，５）についてのみ算出することができる。また、下式（１）〜（３）においてｍ＞ｎである。
【００１６】
【数１】
2014076517.tif 000003
【００１７】
上記回転半径ｄnの算出は以下の手順で行う。
最初に上記３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*を通る回転軌跡が存在する回転面の、中心ベクトルＯn*を以下のように計算する。
【００１８】
図３に示すように、上記３点Ｇ1（n）*，Ｇ2（n）*，Ｇ3（n）*（図中は（n）を省略している）が含まれる平面（平面座標系）の原点をＧ1（n）*とし、点Ｇ1（n）*,Ｇ2（n）*間のベクトルをＶ1*、点Ｇ１（n）*,Ｇ3（n）*間のベクトルをＶ2*とすると、平面座標系の単位ベクトルｉ*，ｊ*，ｋ*は下式（４）〜（６）で求められる。ここでｋ*は上記平面に垂直な、回転軸の向きを示す単位方向ベクトルである。
【００１９】
【数２】
2014076517.tif 000004
【００２０】
点Ｇ1（n）*の、基準座標系での座標値を（Ｘ1,Ｙ1,Ｚ1）とし、単位ベクトルｉ*，ｊ*，ｋ*の各座標値をそれぞれ（ｉx,ｉy,ｉz）（ｊx,ｊy,ｊz）（ｋx,ｋy,ｋz）とすると、変換行列Ｔpは下式（７）のようになる。
【００２１】
【数３】
2014076517.tif 000005
【００２２】
これにより、点Ｇ2（n）*,Ｇ3（n）*は下式（８），（９）によって平面座標系上の点Ｇ2（n）*´,Ｇ3（n）*´に変換される（図３）。
【００２３】
【数４】
2014076517.tif 000006
【００２４】
上記３点Ｇ1（n）*´, Ｇ2（n）*´, Ｇ3（n）*´,を通る円（旋回軌跡）の平面座標系上での中心Ｏn*´の座標（uc,vc,0）は図４に示す幾何学的位置関係より、下式（１０）のようなものとなる。
【００２５】
【数５】
2014076517.tif 000007
【００２６】
なお、中心Ｏn*´から各点Ｇ1（n）*´,Ｇ2（n）*´,Ｇ3（n）*´へのベクトルを、図５に示す(図中は（n）を省略している)ようにそれぞれＶcＧ1*´，ＶcＧ2*´，ＶcＧ*3´とすると、点Ｇ1（n）*´,Ｇ2（n）*´間の角度θ12、およびＧ2（n）*´,Ｇ3（n）*´間の角度θ23は下式（１１），（１２）で表される。
【００２７】
【数６】
2014076517.tif 000008
【００２８】
平面座標系上で位置が特定された中心Ｏn*´は下式（１３）によって基準座標系上の中心Ｏn*に変換される。
Ｏn*＝Ｔp・Ｏn*´…（１３）
これにより、回転半径ｄnがｄn＝｜Ｇ1*−Ｏn*｜で算出される。
【００２９】
続いて、各アームＬ2,Ｌ3,Ｌ5のアーム長Ｌn（n＝２，３，５）を、各アームのその回転軸に対する設計上の角度φn（φn≠０）と上記回転半径ｄnより、下式（１４）で算出する。
Ｌn＝ｄn／sinφn…（１４）
なお、φn＝０の場合のＬｎ（n＝４，６）は予め測定しておく。
【００３０】
その後、各関節の位置ベクトルＰn*（n＝１〜６）および各アームの長さベクトルＬn*を下式（１５），（１６）によって算出する。
【００３１】
【数７】
2014076517.tif 000009
【００３２】
上式（１５）中のｋn*は関節Ｐnの回転軸の単位方向ベクトルである。また上式（１５），（１６）の算出はn＝６→１の順で行う。
ロボットハンド１の設置初期等に上記手順で算出されたＰn*、Ｌn*の値はマスタ値として予めコンピュータ４内に記憶される。
【００３３】
ロボットハンド１を使用している過程で、上記と同様の手順で各関節Ｐ1〜Ｐ6の位置ベクトルＰn*(n＝１〜６)が算出され、位置ベクトルＰn*の上記マスタ値との誤差ΔＰn*が算出される。そして、下式（１７），（１８）によって各関節Ｐn（n＝１〜３）について、その回転軸の、マスタ角度との誤差（ずれ）角度Δθnが検出（算出）される。検出された誤差角度Δθnが所定値よりも大きい場合には、ロボットハンド１の異常として該当する関節Ｐn（ｎ＝１〜６）を報知する。検出された誤差角度Δθnが所定値よりも小さい場合には、後述する誤差補償が行なわれる。
【００３４】
【数８】
2014076517.tif 000010
【００３５】
ここで、上式（１７）中、ΔＰmn*は、ｎ番目の関節Ｐnの、位置誤差の影響を受けたｍ番目の関節Ｐmの位置誤差ベクトルを示す。また、ｍ＞ｎ≧１である。
【００３６】
（誤差補償）
次に、n＝１〜３の各Ｌn*,ｋn*,Ｐn*,ΔＰn*について、Δθnの影響を取り除く（誤差補償）。これは下式（１９）で示される回転行列Rot（ｋ*,θ)を使用して以下のように行われる。
Rot（ｋ*,θ)＝cosθ・Ｉ3＋（１−cosθ）ｋ*・tｋ*＋sinθ［ｋ*×］…（１９）
ここで、Ｉ3は３次の単位行列、tｋ*は単位ベクトルｋ*の転置ベクトル、［ｋ*×］は単位ベクトルｋ*の外積行列である。
【００３７】
すなわち、誤差補償後のＬ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*…（２３）
【００３８】
（軸回りに回転する関節の誤差検出・補償）
上式（２０）〜（２３）を使用するに際して、Ｐn（n＝４，６）の関節はそれぞれアームＬn（n＝４，６）をその軸周りに回転させるものであり、これら関節Ｐn（n＝４，６）では回転軸とアームが平行であるから、上式（１８）における｜ｋn*×Ｌn*｜＝０となってΔθnが算出できない。
【００３９】
そこでこの場合、図６で、｜Ｐ5*−Ｐ4*｜はアームＬ4の回転で変化せず、また｜Ｏ6*−Ｐ5*｜がアームＬ6の回転で変化しないこと、およびＯ6*にはΔθn（n＝１〜３）の影響がないので｜Ｏ6*−Ｐ4*｜にはΔθ5以外のΔθnの影響がないこと、を考慮すると、図６に示す幾何学的な関係からΔθ5は下式（２４）で算出される。
【数９】
2014076517.tif 000011
【００４０】
算出（検出）されたΔθ5により上式（２０）〜（２３）によってＬ5*,ｋ5*,Ｐ5*,ΔＰ5*の誤差補償が行われ、あるいは当該関節Ｐ6の異常が報知される。
【００４１】
次にΔθ4を算出（検出）する。Ｏ6*にはΔθ4以外のΔθnの影響は無いから、下式（２５）によってΔθ4が算出され、上式（２０）〜（２３）によってＬ4*,ｋ4*,Ｐ4*,ΔＰ4*の誤差補償が行われ、あるいは当該関節Ｐ4の異常が報知される。
｜ΔＯ6*｜2＝２｜Ｒ4*×（Ｏ6*−Ｐ4*）｜2(１−cosΔθ4)…（２５）
【００４２】
Δθ6については、Ｐ7*とＯ6*が重ならないように測定治具を予め調節しておく（図６参照）ことによって、上式（１）〜（１８）によってΔθ6が算出（検出）され、その後、上式（２０）〜（２３）によってＬ6*,ｋ6*,Ｐ6*,ΔＰ6*の誤差補償が行われ、あるいは当該関節Ｐ4の異常が報知される。
【符号の説明】
【００４３】
１…ロボットハンド、２…測定用治具、３…カメラ、４…コンピュータ、Ｐ1,Ｐ2,Ｐ3,Ｐ4，Ｐ5,Ｐ6…関節、Ｌ1,Ｌ2,Ｌ3,Ｌ4,Ｌ5,Ｌ6…アーム。
【図１】
2014076517.tif 000012
【図２】
2014076517.tif 000013
【図３】
2014076517.tif 000014
【図４】
2014076517.tif 000015
【図５】
2014076517.tif 000016
【図６】
2014076517.tif 000017