JP3754340B2

JP3754340B2 - Position detection device

Info

Publication number: JP3754340B2
Application number: JP2001316749A
Authority: JP
Inventors: 敬佐藤; 裕司川口; 敏彦塚田; 弘幸松原; 隆志和田
Original assignee: Denso Wave Inc; Toyota Central R&D Labs Inc
Current assignee: Denso Wave Inc; Toyota Central R&D Labs Inc
Priority date: 2001-10-15
Filing date: 2001-10-15
Publication date: 2006-03-08
Anticipated expiration: 2021-10-15
Also published as: JP2003121112A

Description

【０００１】
【発明の属する技術分野】
本発明は、例えばロボット等の位置検出対象に設けられたマーカを、傾斜した方向から撮像した場合でも、その中心位置を正確に検出することを可能とした位置検出装置に関する。
【０００２】
【発明が解決しようとする課題】
産業用ロボット例えば組立用の多関節（６軸）型ロボットを用いたシステムにおいては、ロボットアームの位置が、重力等の影響を受けることによって、理論上（計算機環境上）の位置から若干の位置ずれを生ずる場合がある。そこで、その位置ずれの補正のため、ロボットアームに例えば円形のマーカを設けると共に、固定設備側に、そのマーカ部分を撮像してマーカ位置を検出する視覚認識装置を設けることが考えられている。この場合、前記視覚認識装置は、ＣＣＤカメラによりマーカ部分を撮像し、その画像データを画像処理装置により処理して、前記マーカ画像の重心位置を算出することにより、マーカの中心位置を検出するようになっている。
【０００３】
ところで、上記のように視覚認識によりマーカ位置を検出する場合、ＣＣＤカメラとマーカとが正対して位置する（マーカ表示面がＣＣＤカメラの光軸に垂直となっている）ときには、マーカ画像の重心位置によりマーカの中心位置を高精度で検出することができるが、実際には、マーカ表示面がＣＣＤカメラに対して傾斜していることも多く、この場合斜めに投影された画像を撮像してしまうことになる。このため、撮影したマーカ画像が真円ではなく歪みを有した状態となり、その重心を求めても、マーカの真の中心位置に一致しない不都合が生ずる。
【０００４】
本発明は上記事情に鑑みてなされたもので、その目的は、撮像手段によりマーカを撮像することに基づいて位置検出を行うにものにあって、マーカを傾斜した方向から撮像した場合でも、その中心位置を正確に検出することを可能とした位置検出装置を提供するにある。
【０００５】
【課題を解決するための手段】
本発明者らは、円形のマーカを傾斜方向から撮像した場合におけるマーカ画像の重心位置の真の中心位置からのずれ量は、その傾斜角度と、撮像視点から投影面までの距離と、マーカの半径とによってほぼ決まることに着目し、マーカを、半径の異なる少なくとも２つの同心円を有したものとし、それら同心円の半径が異なることに起因して各円の画像の重心が異なってくることを利用して、マーカの真の中心位置を求めることが可能であることを確認し、本発明を成し遂げたのである。
【０００６】
即ち、本発明の請求項１の位置検出装置によれば、位置検出対象に設けられるマーカを、半径の異なる２以上の同心円を同一の平面上に表示して構成し、撮像手段のマーカを撮像した画像データに基づいてマーカの中心位置を算出する算出手段を、各同心円画像の重心位置を夫々算出し、それら複数の重心位置からマーカの中心位置を求めるように構成したので、マーカを傾斜した方向から撮像した場合でも、そのマーカの中心位置を正確に算出することが可能となる。
【０００７】
より具体的には、本発明者らの研究によれば、半径ｒ1 の外円及び半径ｍｒ1 （０＜ｍ＜１）の内円の２つの同心円について夫々重心位置φ1 及びφ2 を算出することにより、マーカの真の中心位置φを、
【数２】

の式で簡単に算出することができる（請求項２の発明）。尚、マーカの中心位置φの算出方法としては、これ以外にも、複数の同心円の各円の重心と面積とから最小自乗法により求めるなど、様々な方法がある。
【０００８】
また、このとき、算出手段を、撮像手段の光軸に垂直な面に対するマーカの表示面の傾斜角度を併せて算出するように構成することができ（請求項３の発明）、これにより、マーカの位置だけでなくマーカ表示面つまり位置検出対象の撮像手段に対する傾斜角度をも検出することが可能となる。さらには、本発明の位置検出装置は、ロボットの位置検出に適用し、算出手段により算出されたマーカの中心位置に基づいてロボットの位置補正を行うことができ（請求項４の発明）、これにより、比較的簡単で安価な構成で、ロボットの位置補正を正確に行うことができるようになる。
【０００９】
【発明の実施の形態】
以下、本発明を例えば組立用の垂直多関節（６軸）型ロボットの位置補正に適用した一実施例について、図面を参照しながら説明する。図１は、本実施例に係る位置検出装置の位置検出対象となるロボット本体１が、組立作業設備の設置面（作業台）２上に設置されている様子（外観）を概略的に示しており、まず、このロボット本体１の全体構成について簡単に述べる。
【００１０】
このロボット本体１は、前記設置面２上に固定設置されたベース３上に、第１アーム４が垂直軸を中心に回動（旋回）可能に設けられ、その第１アーム４の先端に、第２アーム５が水平軸を中心に回動可能に設けられ、その第２アーム５の先端に、第３アーム６が水平軸を中心に回動可能に設けられている。さらに、その第３アーム６の先端面部に、第４アーム７が同軸回転可能に設けられ、第４アーム７の先端には、第５アーム８が回転可能に設けられ、第５アーム８の先端面に第６アーム９が同軸回転可能に設けられて構成されている。そして、前記第６アーム９の先端面は、図示しないハンド等のエンドエフェクタが着脱（交換）自在に取付けられるエンドエフェクタ取付面１０とされている。
【００１１】
また、上記したロボット本体１の各アーム４〜９は、エンコーダ付きのサーボモータ（図示せず）によりそれぞれ駆動されるようになっており、それら各サーボモータは、ロボット制御装置（コントローラ）１１（図２にのみ図示）によりフィードバック制御されるようになっている。このロボット制御装置１１は、マイコンを主体として構成され、図２に示すように、前記ロボット本体１を制御する制御部１２や、作業プログラムや作業位置データ等が記憶される記憶部１３を備えて構成されている。これにて、ロボット制御装置１１は、記憶部１３に記憶された作業プログラムに従い、作業位置データに基づいてロボット本体１を制御し、以て部品の組立作業等を自動的に実行させるようになっている。
【００１２】
さて、上記ロボット本体１の位置（この場合、エンドエフェクタ取付面１０の位置及び姿勢）を検出するための、本実施例に係る位置検出装置１４の構成について、以下述べる。ここで、まず図１に示すように、前記エンドエフェクタ取付面１０の中心部には、ターゲットとなるマーカＭが設けられるようになっている。本実施例では、このマーカＭは、エンドエフェクタ取付面１０の中心を中心とした半径の異なる２つの同心円、即ち外円Ｃ1 及び内円Ｃ2 を平面Ｓ（図３，図４参照）上に表示して構成されている。位置検出装置１４は、例えばロボット本体１が作業開始前（あるいは教示前）において原点位置に位置している状態で、エンドエフェクタ取付面１０に設けられたマーカＭ（同心円Ｃ1 ，Ｃ2 ）の３次元の中心位置φ及び前記平面Ｓの傾斜角度θを検出するようになっている。
【００１３】
前記位置検出装置１４は、図２に示すように、前記マーカＭ部分を撮像する撮像手段たるＣＣＤカメラ１５や、マイコンを主体してなり前記ＣＣＤカメラ１５からの画像データを処理する視覚認識装置１６等から構成される。前記ＣＣＤカメラ１５は、図１に示すように、設備の設置面２上に、前記ロボット本体１に対向して前記エンドエフェクタ取付面１０のマーカＭを撮像可能な位置（且つ作業の邪魔にならない位置）に設けられる。
【００１４】
このとき、本実施例では、このＣＣＤカメラ１５は、ロボット本体１に向って左右に２個が設けられており、詳しい説明は省略するが、これら２台のＣＣＤカメラ１５を用いることにより、ＣＣＤカメラ１５からマーカＭ（投影面）までの距離Ｌをも高精度に検出できるようになっている。さらに、各ＣＣＤカメラ１５は、垂直軸を中心に回動（旋回）可能に設けられると共に、水平軸を中心に上下方向に回動（角度変更）可能に設けられており、カメラ駆動装置１７によって、前記マーカＭを視野のほぼ中央部にて撮像できる向き（理論的に正対する位置）となるように移動されるようになっている。
【００１５】
前記視覚認識装置１６は、図２に示すように、前記ＣＣＤカメラ１５により撮像されたマーカＭ部分の画像データを記憶する画像記憶部１８や、その画像データを処理する演算処理部１９等を備えて構成される。また、前記演算処理部１９は、前記カメラ駆動装置１７の駆動制御をも行うようになっており、この際、各ＣＣＤカメラ１５の位置（三次元的な撮影角度）を自ら認識できることは勿論である。
【００１６】
そして、詳しくは後の作用説明にて述べるように、この視覚認識装置１６（演算処理部１９）は、そのソフトウエア構成（処理プログラムの実行）により、前記ＣＣＤカメラ１５によりマーカＭ部分を撮像し、その画像データを処理して、同心円Ｃ1 ，Ｃ2 の画像上の重心位置φ1 ，φ2 を夫々算出し、それら重心位置φ1 ，φ2 から前記マーカＭの中心位置φを求めるようになっている。従って、演算処理部１９が算出手段として機能するようになっている。
【００１７】
このとき、より具体的には、演算処理部１９には、前記マーカＭの外円Ｃ1 の半径ｒ1 や、内円Ｃ2 の半径ｒ2 等の予め必要なデータが入力されており、内円Ｃ2 の半径ｒ2 をｒ2 ＝ｍｒ1 （０＜ｍ＜１）とすると、算出された重心位置φ1 及びφ2 から、マーカＭの真の中心位置φを、
【数３】

の式で算出するようになっている。
【００１８】
さらに、本実施例では、演算処理部１９は、中心位置φと例えば外円Ｃ1 の重心位置φ1 との間のずれ量Δφ、並びに、ＣＣＤカメラ１５からマーカＭの投影面間での距離Ｌ、及び外円Ｃ1 の半径ｒ1 から、前記ＣＣＤカメラ１５の光軸に垂直な面に対する前記マーカＭの表示面Ｓの傾斜角度θを併せて算出するように構成されている。そして、検出されたマーカＭの中心位置φ及びマーカＭの表示面Ｓの傾斜角度θは、ロボット制御装置１１に送られ、ロボット本体１の位置補正（記憶部１３に記憶されている作業位置データの補正）に用いられるようになっている。
【００１９】
次に、上記構成の作用について、図３及び図４も参照して述べる。上述のように、ロボット本体１にあっては、アーム４〜９の位置が、重力等の影響を受けることによって、理論上（計算機環境上）の位置から若干の位置ずれを生ずる場合がある。そこで、その位置ずれの補正のため、組立作業を行う前（あるいは教示作業前）に、位置検出装置１４により、アーム先端のエンドエフェクタ取付面１０のマーカＭの位置を検出し、位置ずれ補正を行う。
【００２０】
ここで、円形のマーカの位置を検出する場合、ＣＣＤカメラ１５とマーカＭとが正対して位置する（マーカＭの表示面ＳがＣＣＤカメラ１５の光軸に垂直となっている）ときには、マーカ画像の重心位置の検出によりマーカＭの中心位置を求めることができる。ところが、実際には、マーカＭの表示面ＳがＣＣＤカメラ１５に対して傾斜していることも多く、この場合、斜めに投影された画像を撮像してしまい、マーカＭの画像が真円ではなく歪みを有した状態となり、その重心を求めても、マーカＭの真の中心位置φに一致しない不都合が生ずる。
【００２１】
今、図３に示すように、半径ｒの円Ｃが表示面Ｓに表示され、これを撮像するＣＣＤカメラ１５の光軸ｌに垂直な面に対してその表示面Ｓが角度θ傾斜している場合、円Ｃの真の中心位置に対する画像の重心位置の誤差の角度成分Δφは、以下ようにして求めることができる。但し、ここでは、ＣＣＤカメラ１５の視点Ｖを原点とし、視点Ｖと円Ｃの中心とを結ぶ線（光軸ｌ）をｚ軸に一致させ、表示面Ｓがｘ軸方向に対して角度θ傾いているものとし、視点Ｖ（原点）から投影面までの距離をＬとしている。
【００２２】
即ち、円Ｃを含む球の方程式は、
【数４】

で表され、面Ｓの方程式は、
ｘｓｉｎθ＋（ｚ−Ｌ）ｃｏｓθ＝０
ｚ−Ｌ＝−ｘｔａｎθ …（２）
（２）式を（１）式に代入すると、
【数５】

となる。
【００２３】
これにて、円Ｃの円周の座標は、
【数６】

となり、これから、光軸ｌに垂直な平面（ｚ＝Ｌ）に投影された座標は、
【数７】

となる。ここで、ａ（Ｌ−ｘｔａｎθ）＝Ｌより、ａ＝Ｌ／（Ｌ−ｘｔａｎθ）であるから、上記座標は、
【数８】

となる。
【００２４】
Ｘ＝Ｌｘ／（Ｌ−ｘｔａｎθ）とおくと、ｘ＝ＬＸ／（Ｌ＋Ｘｔａｎθ）より、上記座標は、
【数９】

で表される。この式から、重心を求めるときの積分範囲は、
【数１０】

となる。
【００２５】
さらに、距離Ｌで規格化するために、ｔ＝Ｘ／Ｌ、α＝ｒ／Ｌとおくと、Ｘ＝Ｌｔより、
【数１１】

この式より、重心を求めるときの積分範囲は、
【数１２】

となる。
【００２６】
数値計算の結果、重心位置の誤差（傾きθに起因した真の中心からのずれ）の角度成分Δφは、次式で近似される。
【００２７】
【数１３】

【００２８】
さて、本実施例においては、図４に示すように、マーカＭが、半径ｒ1 の外円Ｃ1 と、半径ｒ2 の内円Ｃ2 （但しｒ2 ＝ｍｒ1 （０＜ｍ＜１））とからなる同心円から構成される、そして、位置検出装置１４は、前記マーカＭ部分をＣＣＤカメラ１５により撮像し、視覚認識装置１６（演算処理部１９）により、その画像データに基づいて、各円Ｃ1 ，Ｃ2 の重心位置φ1 ，φ2 を夫々算出し、それらから次のようにしてマーカＭの真の中心位置φを算出することができる。
【００２９】
即ち、上記（３）式で示されるように、半径ｒの円Ｃに関しては，傾きθに起因した重心位置の誤差の角度成分Δφは、
【数１４】

で表すことができ、Δφが十分に小さいときには、
【数１５】

で近似できる。但し、α＝ｒ／Ｌ、ｋ（θ）＝０．５３９ｓｉｎ２θである。
【００３０】
マーカＭ（円Ｃ1 ，Ｃ2 ）の真の中心位置をφとし、外円Ｃ1 の重心位置φ1 の誤差をΔφ1 、内円Ｃ2 の重心位置φ2 の誤差をΔφ2 とすると、
φ＝φ1 −Δφ1
φ＝φ2 −Δφ2
であるから、α1 ＝ｒ1 ／Ｌ、α2 ＝ｒ2 ／Ｌとすると、上記（４）式から、
【数１６】

が成立つ。
【００３１】
上記（５）式にα2 の二乗を乗算したものから、（６）式にα1 の二乗を乗算したものを減算すると、
【数１７】

となる。これに、α1 ＝ｒ1 ／Ｌ、α2 ＝ｍｒ1 ／Ｌを代入すると、
【数１８】

が得られる。
【００３２】
従って、この（７）式に、算出されたφ1 ，φ2 、及び、既知のｍを代入することにより、真の中心位置φを容易に算出することができるのである。尚、表示面Ｓに傾斜がない、つまりθ＝０の場合には、各円Ｃ1 ，Ｃ2 の重心位置φ1 ，φ2 が一致し、且つ誤差成分Δφが０となるので、それら重心位置φ1 ，φ2 が中心位置φに一致することは勿論であり、この場合にも上記（７）式が成立つ。つまり、表示面Ｓの傾斜の有無に関係なく、上記（７）式を用いて中心位置φを算出できるのである。
【００３３】
また、詳しい説明は省略するが、距離Ｌが高精度で検出できるので、例えば上記（３）式等から明らかなように、このＬの値や半径ｒ、誤差成分Δφを用いて、表示面Ｓの傾斜角度θを容易に算出することが可能である。そして、算出されたマーカＭの正しい中心位置φ及び表示面Ｓの傾斜角度θに基づいて、ロボット本体１の高精度の位置ずれ補正を行うことができ、ひいては、ロボット本体１による高精度の組立作業を実行することができるのである。
【００３４】
このように本実施例によれば、ＣＣＤカメラ１５によりマーカＭを撮像することに基づいて位置検出を行うにものにあって、マーカＭを同心円Ｃ1 ，Ｃ2 から構成し、それら各円Ｃ1 ，Ｃ2 の重心を算出することに基づいて、上記（７）式を用いて簡単に正しい中心位置φを算出することができる。従って、マーカＭを傾斜した方向から撮像した場合でも、その中心位置φを正確に検出することができるという優れた効果を得ることができる。また、本実施例では、マーカＭの中心位置φに加えて、マーカＭの表示面Ｓの傾斜角度θをも併せて算出するようにしたので、比較的簡単で安価な構成で、ロボット本体１の位置補正を正確に行うことができ、ひいては高精度の作業を行うことができるようになる。
【００３５】
尚、上記した実施例では、マーカＭに半径の異なる２つの同心円Ｃ1 ，Ｃ2 を設けて上記（７）式により中心位置φを算出する構成としたが、マーカの中心位置φの算出方法としては、先に計算した上記（４）式を利用した下記のような方法も考えられる。これらの場合、マーカに３個以上の同心円を設けても良いことは勿論である。
【００３６】
▲１▼複数の同心円の各円の重心と面積とを算出し、それらから最小二乗法により求める。
▲２▼円の半径を等比級数で決めた複数の同心円からマーカを構成し、その比率と、算出された各円の重心と面積とから、最小二乗法により求める。
▲３▼円の大きさにより重心を算出する際の量子化ばらつきが異なるので、それを考慮し重み付け最小二乗法により求める。このとき、円の半径に比例した重み付けを行うことができる。
【００３７】
その他、上記実施例では、撮像手段（ＣＣＤカメラ１５）を移動（向き変更）可能に設けるようにしたが、撮像手段を固定して設けても良く、また、撮像手段は１個のみでも実施可能である。マーカＭを設ける位置としても、エンドエフェクタ取付面１０に限らず、ロボット本体１の位置検出したいところに設ければ良く、この場合、マーカＭを複数箇所に設けることも可能である。上記のようにロボットに適用する際に、ロボット側に撮像手段を設け、固定設備側にマーカを設けるようにしても、同様に実施することができる。自走ロボットにも適用することができる。さらには、ロボットの位置決め以外にも、本発明は、視覚により三次元の位置検出を行うもの全般に適用することができるなど、要旨を逸脱しない範囲内で適宜変更して実施し得るものである。
【図面の簡単な説明】
【図１】本発明の一実施例を示すもので、ロボット本体及びＣＣＤカメラの設置状態を示す斜視図
【図２】システムの電気的構成を概略的に示すブロック図
【図３】表示面の傾きに起因した重心位置の誤差の算出方法を説明するための図
【図４】同心円を用いた中心位置の算出方法を説明するための図
【符号の説明】
図面中、１はロボット本体、１０はエンドエフェクタ取付面、１１はロボット制御装置、１４は位置検出装置、１５はＣＣＤカメラ（撮像手段）、１６は視覚認識装置、１７はカメラ駆動装置、１８は画像記憶部、１９は演算処理部（算出手段）、Ｍはマーカ、Ｃ1 は外円、Ｃ2 は内円、Ｓは表示面を示す。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a position detection device capable of accurately detecting the center position of a marker provided on a position detection target such as a robot even when the marker is imaged from an inclined direction.
[0002]
[Problems to be solved by the invention]
In a system using an industrial robot, for example, a multi-joint (6-axis) robot for assembly, the position of the robot arm is affected by gravity or the like, so that the position is slightly different from the theoretical position (on the computer environment). Deviation may occur. Therefore, in order to correct the displacement, for example, a circular marker is provided on the robot arm, and a visual recognition device that detects the marker position by imaging the marker portion on the fixed equipment side is considered. In this case, the visual recognition device detects the center position of the marker by imaging the marker portion with the CCD camera, processing the image data with the image processing device, and calculating the center of gravity of the marker image. It has become.
[0003]
By the way, when the marker position is detected by visual recognition as described above, when the CCD camera and the marker are located facing each other (the marker display surface is perpendicular to the optical axis of the CCD camera), the center of gravity of the marker image Although the center position of the marker can be detected with high accuracy depending on the position, in reality, the marker display surface is often inclined with respect to the CCD camera. In this case, an image projected obliquely is captured. Will end up. For this reason, the photographed marker image is not in a perfect circle but in a distorted state, and even if the center of gravity is obtained, there is a disadvantage that it does not coincide with the true center position of the marker.
[0004]
The present invention has been made in view of the above circumstances, and its purpose is to perform position detection based on imaging a marker by an imaging means, and even when the marker is imaged from an inclined direction, It is an object of the present invention to provide a position detection device that can accurately detect the center position.
[0005]
[Means for Solving the Problems]
When the circular marker is imaged from the tilt direction, the inventors have determined that the amount of deviation of the center of gravity of the marker image from the true center position is the tilt angle, the distance from the imaging viewpoint to the projection plane, Focusing on the fact that it is almost determined by the radius, it is assumed that the marker has at least two concentric circles with different radii, and the center of gravity of the image of each circle is different due to the different radii of the concentric circles. Thus, it was confirmed that the true center position of the marker could be obtained, and the present invention was accomplished.
[0006]
That is, according to the position detection apparatus of the first aspect of the present invention, the marker provided in the position detection target is configured by displaying two or more concentric circles having different radii on the same plane, and images the marker of the imaging means. Since the calculation means for calculating the center position of the marker based on the image data obtained is configured to calculate the center position of each concentric image and to obtain the center position of the marker from the plurality of center positions, the marker is inclined. Even when the image is taken from the direction, the center position of the marker can be accurately calculated.
[0007]
More specifically, according to the study by the present inventors, the center-of-gravity positions φ1 and φ2 are calculated for two concentric circles of an outer circle with a radius r1 and an inner circle with a radius mr1 (0 <m <1), respectively. , The true center position φ of the marker,
[Expression 2]

It can be easily calculated by the following formula (invention of claim 2). In addition to this, there are various methods for calculating the marker center position φ, such as obtaining by the least square method from the center of gravity and the area of each of a plurality of concentric circles.
[0008]
Further, at this time, the calculating means can be configured to calculate the inclination angle of the display surface of the marker with respect to the surface perpendicular to the optical axis of the imaging means (invention of claim 3). It is possible to detect not only the position but also the tilt angle with respect to the marker display surface, that is, the position detection target imaging means. Furthermore, the position detection device of the present invention can be applied to the position detection of the robot and can correct the position of the robot based on the center position of the marker calculated by the calculation means (the invention of claim 4). Accordingly, the robot position can be accurately corrected with a relatively simple and inexpensive configuration.
[0009]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, an embodiment in which the present invention is applied to position correction of a vertical articulated (6-axis) robot for assembly, for example, will be described with reference to the drawings. FIG. 1 schematically shows a state (appearance) in which a robot body 1 that is a position detection target of a position detection apparatus according to the present embodiment is installed on an installation surface (workbench) 2 of an assembly work facility. First, the overall configuration of the robot body 1 will be briefly described.
[0010]
The robot body 1 is provided on a base 3 fixedly installed on the installation surface 2 such that a first arm 4 can be rotated (turned) about a vertical axis, and at the tip of the first arm 4, The second arm 5 is provided to be rotatable about a horizontal axis, and the third arm 6 is provided at the tip of the second arm 5 to be rotatable about the horizontal axis. Further, a fourth arm 7 is provided on the tip surface portion of the third arm 6 so as to be coaxially rotatable. A fifth arm 8 is provided on the tip of the fourth arm 7 so as to be rotatable, and the tip of the fifth arm 8 is provided. A sixth arm 9 is provided on the surface so as to be coaxially rotatable. The distal end surface of the sixth arm 9 is an end effector mounting surface 10 on which an end effector such as a hand (not shown) can be detachably (replaced).
[0011]
Each of the arms 4 to 9 of the robot body 1 is driven by a servomotor (not shown) with an encoder, and each servomotor is connected to a robot control device (controller) 11 ( The feedback control is performed according to FIG. The robot control device 11 is mainly composed of a microcomputer, and as shown in FIG. 2, includes a control unit 12 for controlling the robot body 1 and a storage unit 13 for storing work programs, work position data, and the like. It is configured. As a result, the robot controller 11 controls the robot body 1 based on the work position data in accordance with the work program stored in the storage unit 13 to automatically execute the parts assembling work and the like. ing.
[0012]
Now, the configuration of the position detection device 14 according to the present embodiment for detecting the position of the robot body 1 (in this case, the position and orientation of the end effector mounting surface 10) will be described below. First, as shown in FIG. 1, a marker M serving as a target is provided at the center of the end effector mounting surface 10. In this embodiment, the marker M displays two concentric circles having different radii around the center of the end effector mounting surface 10, that is, an outer circle C1 and an inner circle C2, on a plane S (see FIGS. 3 and 4). Configured. The position detection device 14 is a three-dimensional marker M (concentric circles C1, C2) provided on the end effector mounting surface 10 in a state where the robot main body 1 is located at the origin position before starting work (or before teaching), for example. And the inclination angle θ of the plane S are detected.
[0013]
As shown in FIG. 2, the position detection device 14 includes a CCD camera 15 that is an imaging means for imaging the marker M portion, and a visual recognition device 16 that mainly processes a microcomputer and processes image data from the CCD camera 15. Etc. As shown in FIG. 1, the CCD camera 15 is located on the installation surface 2 of the equipment so as to face the robot body 1 and can image the marker M of the end effector mounting surface 10 (and does not interfere with the work). Position).
[0014]
At this time, in this embodiment, two CCD cameras 15 are provided on the left and right sides of the robot main body 1, and detailed description thereof is omitted. However, by using these two CCD cameras 15, The distance L from the camera 15 to the marker M (projection plane) can also be detected with high accuracy. Further, each CCD camera 15 is provided so as to be able to turn (turn) around a vertical axis, and can be turned up and down (changed in angle) around a horizontal axis. The marker M is moved so as to be in a direction (theoretically facing the position) at which the marker M can be imaged at substantially the center of the field of view.
[0015]
As shown in FIG. 2, the visual recognition device 16 includes an image storage unit 18 that stores image data of a marker M portion captured by the CCD camera 15, an arithmetic processing unit 19 that processes the image data, and the like. Configured. The arithmetic processing unit 19 also controls the drive of the camera driving device 17 and, of course, can recognize the position (three-dimensional shooting angle) of each CCD camera 15 by itself. is there.
[0016]
As will be described in detail later, the visual recognition device 16 (arithmetic processing unit 19) images the marker M portion by the CCD camera 15 by the software configuration (execution of the processing program). Then, the image data is processed to calculate the center positions φ1 and φ2 of the concentric circles C1 and C2 on the image, and the center position φ of the marker M is obtained from the center positions φ1 and φ2. Therefore, the arithmetic processing unit 19 functions as a calculation unit.
[0017]
More specifically, necessary data such as the radius r1 of the outer circle C1 and the radius r2 of the inner circle C2 of the marker M is input to the arithmetic processing unit 19 in advance. Assuming that the radius r2 is r2 = mr1 (0 <m <1), the true center position φ of the marker M is calculated from the calculated barycentric positions φ1 and φ2.
[Equation 3]

It is calculated by the following formula.
[0018]
Furthermore, in the present embodiment, the arithmetic processing unit 19 includes a deviation amount Δφ between the center position φ and, for example, the center of gravity position φ1 of the outer circle C1, and a distance L between the CCD camera 15 and the projection surface of the marker M, Further, the inclination angle θ of the display surface S of the marker M with respect to the surface perpendicular to the optical axis of the CCD camera 15 is also calculated from the radius r1 of the outer circle C1. Then, the detected center position φ of the marker M and the inclination angle θ of the display surface S of the marker M are sent to the robot controller 11 to correct the position of the robot body 1 (work position data stored in the storage unit 13). Correction).
[0019]
Next, the operation of the above configuration will be described with reference to FIGS. As described above, in the robot main body 1, the positions of the arms 4 to 9 may be slightly displaced from the theoretical (computer environment) position due to the influence of gravity or the like. Therefore, before the assembly work (or before the teaching work), the position detection device 14 detects the position of the marker M on the end effector mounting surface 10 at the tip of the arm in order to correct the positional deviation, thereby correcting the positional deviation. Do.
[0020]
Here, when detecting the position of the circular marker, when the CCD camera 15 and the marker M are positioned facing each other (the display surface S of the marker M is perpendicular to the optical axis of the CCD camera 15), the marker The center position of the marker M can be obtained by detecting the position of the center of gravity of the image. However, in practice, the display surface S of the marker M is often inclined with respect to the CCD camera 15. In this case, an image projected obliquely is captured, and the image of the marker M is a perfect circle. Therefore, even if the center of gravity of the marker M is obtained, there is a disadvantage that it does not coincide with the true center position φ of the marker M.
[0021]
Now, as shown in FIG. 3, a circle C having a radius r is displayed on the display surface S, and the display surface S is inclined at an angle θ with respect to a surface perpendicular to the optical axis l of the CCD camera 15 that images the circle C. If there is, the angle component Δφ of the error of the center of gravity position of the image with respect to the true center position of the circle C can be obtained as follows. However, here, the viewpoint V of the CCD camera 15 is the origin, the line (optical axis l) connecting the viewpoint V and the center of the circle C is made to coincide with the z axis, and the display surface S is at an angle θ with respect to the x axis direction. The distance from the viewpoint V (origin) to the projection plane is L.
[0022]
That is, the equation of the sphere containing the circle C is
[Expression 4]

And the equation for the surface S is
xsin θ + (z−L) cos θ = 0
z−L = −xtan θ (2)
Substituting equation (2) into equation (1),
[Equation 5]

It becomes.
[0023]
Now the coordinates of the circumference of the circle C are
[Expression 6]

From this, the coordinates projected on a plane perpendicular to the optical axis l (z = L) are
[Expression 7]

It becomes. Here, since a = L / (L-xtanθ) from a (L-xtanθ) = L, the coordinates are
[Equation 8]

It becomes.
[0024]
If X = Lx / (L−xtanθ), then x = LX / (L + Xtanθ), the coordinates are
[Equation 9]

It is represented by From this equation, the integration range when calculating the center of gravity is
[Expression 10]

It becomes.
[0025]
Furthermore, in order to normalize with the distance L, if t = X / L and α = r / L, then X = Lt,
## EQU11 ##

From this equation, the integration range when calculating the center of gravity is
[Expression 12]

It becomes.
[0026]
As a result of the numerical calculation, the angular component Δφ of the error of the center of gravity position (deviation from the true center due to the inclination θ) is approximated by the following equation.
[0027]
[Formula 13]

[0028]
In this embodiment, as shown in FIG. 4, the marker M is a concentric circle composed of an outer circle C1 having a radius r1 and an inner circle C2 having a radius r2 (where r2 = mr1 (0 <m <1)). Then, the position detection device 14 takes an image of the marker M portion with the CCD camera 15, and the visual recognition device 16 (arithmetic processing unit 19) determines each of the circles C1 and C2 based on the image data. The center of gravity positions φ1 and φ2 are calculated, and the true center position φ of the marker M can be calculated therefrom as follows.
[0029]
That is, as shown in the above equation (3), for the circle C having the radius r, the angle component Δφ of the error in the center of gravity due to the inclination θ is
[Expression 14]

When Δφ is sufficiently small,
[Expression 15]

Can be approximated by However, α = r / L and k (θ) = 0.539 sin 2θ.
[0030]
If the true center position of the marker M (circles C1, C2) is φ, the error of the center of gravity position φ1 of the outer circle C1 is Δφ1, and the error of the center of gravity position φ2 of the inner circle C2 is Δφ2.
φ = φ1 −Δφ1
φ = φ2 −Δφ2
Therefore, when α1 = r1 / L and α2 = r2 / L, from the above equation (4),
[Expression 16]

Is established.
[0031]
Subtracting the formula (5) multiplied by the square of α2 from the formula (6) multiplied by the square of α1 gives
[Expression 17]

It becomes. Substituting α1 = r1 / L and α2 = mr1 / L into this,
[Formula 18]

Is obtained.
[0032]
Therefore, the true center position φ can be easily calculated by substituting the calculated φ1, φ2 and the known m into the equation (7). When the display surface S is not inclined, that is, when θ = 0, the centroid positions φ1 and φ2 of the circles C1 and C2 coincide with each other and the error component Δφ becomes 0. Therefore, the centroid positions φ1 and φ2 Of course coincides with the center position φ, and in this case, the above equation (7) is established. That is, the center position φ can be calculated using the above equation (7) regardless of whether the display surface S is inclined or not.
[0033]
Although the detailed description is omitted, since the distance L can be detected with high accuracy, the display surface S is obtained by using the value of L, the radius r, and the error component Δφ, as is apparent from, for example, the above equation (3). Can be easily calculated. Then, based on the calculated correct center position φ of the marker M and the tilt angle θ of the display surface S, it is possible to correct the displacement of the robot body 1 with high accuracy. The work can be performed.
[0034]
As described above, according to the present embodiment, the position detection is performed based on the imaging of the marker M by the CCD camera 15, and the marker M is composed of concentric circles C1 and C2, and each of these circles C1 and C2. Based on calculating the center of gravity, the correct center position φ can be easily calculated using the above equation (7). Therefore, even when the marker M is imaged from an inclined direction, an excellent effect that the center position φ can be accurately detected can be obtained. In the present embodiment, since the inclination angle θ of the display surface S of the marker M is calculated in addition to the center position φ of the marker M, the robot body 1 has a relatively simple and inexpensive configuration. Thus, the position correction can be performed accurately, and as a result, highly accurate work can be performed.
[0035]
In the above-described embodiment, the marker M is provided with two concentric circles C1 and C2 having different radii, and the center position φ is calculated according to the above equation (7). The following method using the above-described equation (4) is also conceivable. In these cases, of course, three or more concentric circles may be provided on the marker.
[0036]
(1) The center of gravity and area of each circle of a plurality of concentric circles are calculated, and determined from them by the method of least squares.
{Circle around (2)} A marker is formed from a plurality of concentric circles in which the radius of the circle is determined by a geometric series, and is obtained by the least square method from the ratio and the calculated center of gravity and area of each circle.
(3) The quantization variation in calculating the center of gravity differs depending on the size of the circle. At this time, weighting proportional to the radius of the circle can be performed.
[0037]
In addition, in the above embodiment, the image pickup means (CCD camera 15) is provided so as to be movable (change in orientation). However, the image pickup means may be fixed and provided, and only one image pickup means may be provided. It is. The position where the marker M is provided is not limited to the end effector mounting surface 10, and may be provided where the position of the robot body 1 is desired to be detected. In this case, the marker M may be provided at a plurality of locations. When applying to a robot as described above, the same can be implemented even if an imaging means is provided on the robot side and a marker is provided on the fixed equipment side. It can also be applied to a self-propelled robot. Furthermore, besides the positioning of the robot, the present invention can be applied to any device that performs three-dimensional position detection visually, and can be implemented with appropriate modifications within a range not departing from the gist. .
[Brief description of the drawings]
FIG. 1 is a perspective view showing an installation state of a robot body and a CCD camera according to an embodiment of the present invention. FIG. 2 is a block diagram schematically showing an electrical configuration of a system. FIG. 4 is a diagram for explaining a method of calculating an error in the center of gravity due to the inclination. FIG. 4 is a diagram for explaining a method of calculating the center position using concentric circles.
In the drawings, 1 is a robot body, 10 is an end effector mounting surface, 11 is a robot control device, 14 is a position detection device, 15 is a CCD camera (imaging means), 16 is a visual recognition device, 17 is a camera drive device, 18 is An image storage unit, 19 is an arithmetic processing unit (calculation means), M is a marker, C1 is an outer circle, C2 is an inner circle, and S is a display surface.

Claims

A position detection apparatus comprising: an imaging unit that images a marker provided in a position detection target; and a calculation unit that calculates a center position of the marker based on image data of the imaging unit;
The marker is configured by displaying two or more concentric circles having different radii on the same plane,
The position detecting device characterized in that the calculating means calculates a centroid position of each concentric circle image, and obtains a center position of the marker from the plurality of centroid positions.

The calculating means calculates centroid positions φ1 and φ2 for two concentric circles of an outer circle having a radius r1 and an inner circle having a radius mr1 (0 <m <1), and further calculating a center position φ by the following equation: The position detection device according to claim 1.

The position detecting device according to claim 1, wherein the calculating unit is configured to calculate an inclination angle of the display surface of the marker with respect to a surface perpendicular to the optical axis of the imaging unit. .

The position detection apparatus according to claim 1, wherein the position detection target is a robot, and the center position of the marker calculated by the calculation unit is used for position correction of the robot. .