(57)【要約】【課題】ロボットアーム先端のフランジにカメラを取り付けてなる視覚検査装置において、カメラによってフランジ座標Ｆとカメラ座標Ｃとの関係を求める。【解決手段】フランジ１１をフランジ座標ＦのＸＹＺ軸に沿って移動させた時のカメラ１２の撮像画面上での移動ベクトルを求め、これに基づいてフランジ座標Ｆに対するカメラ座標Ｃの傾きを演算する。フランジ１１をカメラ座標ＣのＺｃ軸を中心に回転させたときのマーカＭの撮影画面およびフランジ１１をフランジ座標Ｆの原点を通るカメラ座標ＣのＺｃ軸と平行な直線を中心に回転させたときのマーカＭの撮影画面によってフランジ座標Ｆ上でのカメラ座標Ｃの位置を求める。【選択図】図１
2010112731.tif 000002
【特許請求の範囲】
【請求項１】
手先部に、レンズおよび撮像センサを有したカメラを取り付けたロボットにおいて、
前記手先部の座標に対する前記カメラの座標の位置と傾きとを求める方法であって、
定位置に設けられたマーカが前記カメラの撮影視野内に入る状態で、前記手先部を、当該手先部の座標の各軸Ｘｆ，Ｙｆ，Ｚｆと平行にそれぞれ所望距離移動させて各軸Ｘｆ，Ｙｆ，Ｚｆについて平行移動の前後の前記カメラの撮影画像を取得する第１の行程と、
前記第１の行程で取得した撮影画像から、前記手先部の座標の各軸Ｘｆ，Ｙｆ，Ｚｆについての前記平行移動により生じた前記マーカの移動ベクトルを求める第２の行程と、
前記第１の行程で移動した前記手先部の座標の各軸Ｘｆ，Ｙｆ，Ｚｆについての平行移動距離と、前記第２の行程で得た前記マーカの前記移動ベクトルとから、前記手先部の座標に対する前記カメラの座標の各軸Ｘｃ，Ｙｃ，Ｚｃの傾きを求める第３の行程と、
前記レンズの光軸を前記カメラの座標の各軸Ｘｃ，Ｙｃ，ＺｃのうちのＺｃ軸とし、前記定位置に設けられた前記マーカが前記カメラの撮影視野内に入る状態で、前記手先部を、前記カメラの座標のＸｃ軸および／またはＹｃ軸と平行にそれぞれ所望量移動させて前記Ｘｃ軸および／またはＹｃ軸についての平行移動の前後の撮影画像を取得する第４の行程と、
前記第４の行程で取得した前記マーカの撮影画像により、前記カメラの座標の前記Ｘｃ軸および／またはＹｃ軸についての前記平行移動前の前記マーカの映像位置と前記平行移動後の前記マーカの映像位置との間の画素数を求め、この画素数と、前記カメラの座標の前記Ｘｃ軸および／またはＹｃ軸についての前記平行移動の距離とから、１画素あたりの前記カメラの移動量を演算する第５の行程と、
前記定位置に設けられた前記マーカが前記カメラの撮影視野内に入る状態で、前記カメラの座標の前記Ｚｃ軸が前記手先部の座標の原点を通ると仮定して当該Ｚｃ軸を中心にして前記手先部を所望量だけ回転させ、その回転開始から回転終了までのうちから少なくとも３位置での撮影画像を取得する第６の行程と、
前記第６の行程で取得した前記少なくとも３位置での撮影画像から前記マーカの少なくとも３つの映像位置を得てこれらの映像位置から前記第６の行程での回転の実際の回転中心を求める第７の行程と、
前記第７の行程で求めた回転中心と前記カメラの座標の前記Ｘｃ軸およびＹｃ軸との間の最小画素数を求め、この画素数と、前記第５の行程で求めた１画素当たりの移動量とから、前記カメラの座標の前記Ｚｃ軸に平行で前記手先部の座標の原点を通る軸から前記カメラの座標の前記Ｚｃ軸までの距離を求める第８の行程と、
前記カメラの前記Ｘｃ軸およびＹｃ軸を含む平面が前記手先部の座標の原点を通ると仮定し、前記マーカが前記カメラの撮影視野内に入る状態で、前記手先部を前記カメラの座標の前記Ｘｃ軸またはＹｃ軸と平行に所望量移動させて移動前後の撮影画像を取得する第９の行程と、
第９の行程で取得した撮影画像により、前記カメラの座標の前記Ｘｃ軸またはＹｃ軸と平行に移動する前の前記マーカの映像位置と移動した後の前記マーカの映像位置との間の画素数を求め、この画素数と予め求められた１画素の長さとから前記撮影画像上での前記マーカの移動距離を演算し、この前記マーカの前記撮影画像上での移動距離と前記手先部の移動距離と前記カメラの前記レンズの焦点距離とから前記カメラの座標の原点から前記マーカまでの距離を演算する第１０の行程と、
前記定位置に設けられた前記マーカが前記カメラの撮影視野内に入る状態で、前記カメラの座標の前記Ｘｃ軸およびＹｃ軸を含む平面が前記手先部の座標の原点を通ると仮定して前記手先部を前記カメラの座標のＸｃ軸またはＹｃ軸を中心に所望角度回転させ、その回転前後の撮影画像を取得する第１１の行程と、
前記第１１の行程で取得した撮影画像により回転前の前記マーカの映像位置と回転後の前記マーカの映像位置との間の画素数を求め、この画素数と前記１画素の長さとから前記撮影画像上での前記マーカの移動距離を演算し、この撮影画像上での前記マーカの移動距離と前記第１１行程における回転角度とにより、前記第１１の行程の回転中心となった前記カメラの座標のＸｃ軸またはＹｃ軸と平行な軸であって前記手先部の座標の原点を通る軸から前記マーカまでの距離を演算する第１２の行程と、
前記第１１の行程の回転中心となった前記カメラの座標のＸｃ軸またはＹｃ軸と平行な軸であって前記手先部の座標の原点を通る軸と前記マーカとの間の距離から前記カメラの座標の原点と前記マーカとの間の距離を差し引いて、前記第１１の行程の回転中心となった前記カメラの座標のＸｃ軸またはＹｃ軸と平行な軸であって前記手先部の座標の原点を通る軸から前記カメラの座標の原点までの距離を求める第１３の行程と、
前記第８の行程で求めた前記カメラの座標の前記Ｚｃ軸に平行で前記手先部の座標の原点を通る前軸から前記カメラの座標の前記Ｚｃ軸までの距離と、前記第１３の行程で求めた前記カメラの座標のＸｃ軸またはＹｃ軸と平行な軸であって前記手先部の座標の原点を通る軸から前記カメラの座標の原点までの距離とにより、前記手先部の座標上での前記カメラの座標の原点の位置を求める第１４の行程と、
からなるロボットの座標の結合方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、手先部にカメラを取り付けたロボットにおいて、手先部の座標に対するカメラの座標の位置と傾きを求めるロボットの座標の結合方法に関する。
【背景技術】
【０００２】
工場の製造ラインにおいて、例えば組立の最終行程に、ロボットとカメラを組み合わせて構成した視覚検査装置を設置し、カメラにてワークの所望箇所を撮影し、その撮影画像をディスプレイに表示して視覚検査をすることが行われている。この視覚検査装置は、ロボットの手先部にカメラを取り付け、手先部を予め設定された位置に移動させてカメラによる撮影画像を取得するものである。この視覚検査装置において、手先部の移動位置を設定するためには、予め、ロボットの手先部の座標とカメラの座標との関係を求めておく必要がある。
特許文献１では、３次元カメラを用いてこのロボットの手先部の座標とカメラの座標との関係を求めている。
【特許文献１】特開平１０−６３３１７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、３次元カメラは高価である。視覚検査装置で使用されるカメラは、通常、２次元カメラであるから、２次元カメラで手先部の座標とカメラの座標との関係を求めることができれば、大変に便利である。このため、手先部に取り付けられた２次元カメラを利用して手先部の座標とカメラの座標との関係を求める方法の出現が強く要望されている。
【０００４】
そこで、本発明の目的は、手先部に取り付けられた２次元のカメラによって手先部の座標とカメラの座標との関係を求めることができるロボットの座標の結合方法を提供することにある。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために、本発明では、手先部を、当該手先部の座標の各軸Ｘｆ，Ｙｆ，Ｚｆと平行にそれぞれ所望距離移動させてその移動の前後に、定位置に設けられたマーカを撮影し、その撮影画像を用いて手先部の座標に対するカメラの座標の傾きを求める。次に、カメラの座標の各軸Ｘｃ，Ｙｃ，Ｚｃのうちレンズの光軸をＺｃ軸とし、手先部をカメラの座標のＸｃ軸および／またはＹｃ軸と平行に所望量移動させてその移動前後で前記マーカを撮影し、この撮影画像によりカメラの１画素あたりのカメラの移動量を演算し、カメラの座標の前記Ｚｃ軸が手先部の座標の原点を通ると仮定して当該Ｚｃ軸を中心にして手先部を所望量だけ回転させてその回転開始から回転終了までのうちから少なくとも３位置で前記マーカを撮影し、これら撮影画像と１画素の長さからカメラの座標のＺｃ軸と手先部の座標の原点を通るカメラの座標のＺｃ軸と平行な軸との離間距離を求める。更に、カメラのＸｃ軸，Ｙｃ軸を含む平面が手先部の座標の原点を通ると仮定し、手先部をカメラの座標のＸｃ軸またはＹｃ軸と平行に所望量移動させてその移動前後に前記マーカを撮影し、この撮影画像と１画素当たりのカメラの移動量とレンズの焦点距離から手先部とマーカとの間の距離を演算し、カメラの座標のＸｃ軸とＹｃ軸を含む平面が手先部の座標の原点を通ると仮定して手先部をカメラの座標のＸｃ軸またはＹｃ軸を中心に所望角度回転させてその回転前後にマーカを撮影し、この撮影画像と１画素１画素の長さから回転前後のマーカの相対的移動距離を求め、その移動距離と回転角から、手先部の座標の原点を通る回転中心軸からマーカまでの距離を演算し、回転中心軸とマーカとの距離からカメラの座標の原点とマーカまでの距離を差し引いて、手先部の座標の原点からカメラの座標の原点までの距離を求めるので、手先部に取り付けられたカメラによってロボットの手先部の座標に対するカメラの座標の位置と傾きとを求めることができる。
【発明を実施するための最良の形態】
【０００６】
以下、本発明の一実施形態を図面に基づいて説明する。図１には、視覚検査装置が示されている。この視覚検査装置に用いられるロボット１は、ロボット本体２と、このロボット本体２を制御する制御装置３からなり、更に、ティーチングペンダント４および例えば液晶表示器を主体とする表示装置（表示手段）５を備えて構成されている。上記ティーチングペンダント４は、各種データを入力する入力手段およびロボット本体１を遠隔操作するための操作手段として機能するもので、各種の操作スイッチ４ａおよび表示部４ｂを備えている。
【０００７】
ロボット本体２は、例えば６軸の垂直多関節型のものとして構成され、ベース６と、このベース６に水平方向に旋回加納に支持されたショルダ部７と、このショルダ部７に上下方向に旋回可能に支持された下アーム８と、この下アーム８に上下方向に旋回可能に且つ回転（捻り動作）可能に支持された上アーム９と、この上アーム９に上下方向に旋回可能に支持された手首１０と、この手首１０に回転可能に支持されたフランジ１１から構成されている。上記ショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１はロボットアームを構成するもので、アーム先端の手先部であるフランジ１１には、カメラ１２がステー１２ａを介して取り付けられている。
このロボット本体２を遠隔操作するティーチングペンダント４は、フランジ１１を、後述するフランジ座標の各座標軸と平行に移動させたり、フランジ１１を所望の姿勢に維持したまま移動（並進移動）させたりすることができるように構成されている。
【０００８】
一方、制御装置３は、図２に示すように、ＣＰＵ１３、駆動回路１４、位置検出手段としての位置検出回路１５を備えている。そして、ＣＰＵ（制御手段）１３には、ロボット１全体のシステムプログラムなどを記憶するＲＯＭ（記憶手段）１６、ティーチングペンダント４などによって作成されたロボット本体２の動作プログラムなどを記憶するＲＡＭ（記憶手段）１７が接続されていると共に、ティーチングペンダント４、表示装置５、カメラ１２が接続されている。
【０００９】
上記位置検出回路１５は、各関節、つまりショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１の相対位置を検出するためのもので、この位置検出回路１５には、各関節の駆動モータ１８に設けられた位置センサとしてのロータリエンコーダ１９が接続されている。なお、図２には、駆動モータ１８およびロータリエンコーダ１９は、１個ずつしか図示していないが、実際には、各関節に対して一対一の関係で複数設けられているものである。
【００１０】
そして、位置検出回路１５は、ロータリエンコーダ１９の検出信号によってベース６に対するショルダ部７の回転角度、ショルダ部７に対する下アーム８の旋回角度、下アーム８に対する上アーム９の旋回角度および捻り角度、上アーム９に対する手首１０の旋回角度、手首１０に対するフランジ１１の回転角度を検出し、その動作角度情報はＣＰＵ１３に与えられる。ＣＰＵ１３は、動作プログラムに基づいてショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１を動作させる際、位置検出回路１５からの入力信号をフィードバック信号としてそれらの動作（駆動モータ１８）を制御する。
【００１１】
ここで、ベース６、ショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１には、それぞれ三次元の座標が固定されている。このうち、不動のベース６の座標はロボット座標（図１にＳで示す。）とされるもので、他の座標系は、ショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１の動作によってロボット座標Ｓ上の位置と姿勢（向き）が変化する。ロボット座標Ｓは、例えばベース６の底面とショルダ部７の旋回中心との交点を原点Ｐとし、Ｚ軸を高さ方向（鉛直方向）の座標軸に定め、Ｘ軸およびＹ軸を水平面内における縦横２方向の座標軸に定めている。
【００１２】
そして、制御装置３（ＣＰＵ１３）は、位置検出回路１５から入力されるショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１の各関節の位置検出情報と予めＲＯＭ（アーム長記憶手段）１６に記憶されているショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１のアーム長さ情報により、ショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１の各座標の原点位置と姿勢を、座標変換の演算機能によってロボット座標Ｓ上での位置と姿勢とに変換して認識することができるようになっている。
【００１３】
さて、ショルダ部７、下アーム８、上アーム９、手首１０およびフランジ１１のうち、フランジ１１の座標は、図３に示すように、フランジ１１の前端面（表面）の中心Ｐｆを原点とし、フランジ１１の先端面上で２つの座標軸、フランジ１１の回転軸上で１つの座標軸が定められている。具体的には、原点Ｐｆとフランジ１１に偏心して形成されている小孔１１ａの中心を通る直線をＹ軸とし、この原点Ｐｆを通りＹ軸と直交する直線をＸ軸、原点Ｐｆを通りＸ，Ｙの両軸と直交する直線、つまりフランジ１１の回転中心線と一致する直線をＺ軸と定めている。
【００１４】
そして、ロボット１の位置と姿勢、即ち、フランジ１１の位置と姿勢のうち、位置は、フランジ１１の座標の原点Ｐｆであるフランジ１１の先端面の中心が占めるロボット座標Ｓ上の位置で示される。また、フランジ１１の姿勢は、フランジ１１の座標の原点ＰｆからＺ軸に沿って例えば負方向に伸びる単位長さ「１」のベクトルと、原点ＰｆからＸ軸に沿って例えば正方向に伸びる単位長さ「１」のベクトルを定義し、フランジ１１の座標をその原点Ｐｆがロボット座標Ｓの原点Ｐに一致するように並進させたとき、そのロボット座標Ｓ上での両単位ベクトルで表される。
【００１５】
一方、カメラ１２は、図４に示すように、固定焦点のレンズ２０、このレンズ２０から当該レンズ２０の焦点距離Ｌだけ離れた位置に設置された撮像センサとしての２次元のＣＣＤ撮像画面２１を備えている。ＣＣＤ撮像画面２１（以下、単に撮像画面２１という）は、多数のＣＣＤを縦横に整列配置してなるもので、１つのＣＣＤが１画素を構成している。このカメラ１２にも、三次元座標が設定されている。このカメラ１２の座標の座標は、図４および図５に示すように、撮像画面２１の中心点Ｐｃを原点とし、この原点Ｐｃを通って撮像画面２１を上下方向に二分する直線をＸ軸、原点Ｐｃを通って撮像画面２１を左右方向に二分する直線をＹ軸、原点Ｐｃとレンズ２０の中心を通る直線（光軸に一致）をＺ軸としている。
【００１６】
なお、以下では、フランジ１１の座標およびカメラ１２の座標をそれぞれフランジ座標Ｆおよびカメラ座標Ｃと称し、これらフランジ座標Ｆおよびカメラ座標Ｃの座標軸を互いに区別するために、それぞれの座標軸を表すＸ，Ｙ，Ｚに、ｆおよびｃの添え字を付して示すこととする。
【００１７】
ところで、カメラ１２の撮影ポイントを定めるには、フランジ座標Ｆに対するカメラ座標Ｃの関係を予め知っておかねばならない。つまり、カメラ１２によりワークの撮影対象個所を撮影するには、カメラ１２の光軸（Ｚｃ軸）がワークの撮影対象箇所を向き、且つ撮影対象箇所の全体がカメラ１２の撮影視野内に入った状態でピントが合うように、フランジ１１の位置と姿勢を定めなければならない。そのためには、図５に示すように、フランジ座標Ｆに対するカメラ座標Ｃの位置と傾き（姿勢）を知らねばならない。
前記制御装置３のＲＯＭ１６には、このフランジ座標Ｆに対するカメラ座標Ｃの位置と傾きを求めるためのプログラムが格納されている。以下、このプログラムによってフランジ座標Ｆに対するカメラ座標Ｃの位置と傾きの関係を求める方法について、図６に示すフローチャートおよび図７以降の図面を参照しながら説明する。
【００１８】
図７はロボット本体２をスケルトンで示すもので、併せて、視覚検査対象物であるワークＷも示している。ワークＷには、小さな点状のマーカＭが付されている。このマーカＭは、塗料により描いて形成しても良いし、ワークＷに取り付けられた例えば小ねじの頭で代用しても良い。
まず、フランジ座標Ｆに対するカメラ座標Ｃの傾きを求める。そのために、ユーザは、ティーチングペンダント４を用いて、ロボット本体２のフランジ１１を、フランジ座標ＦのＸｆ軸と平行に、Ｙｆ軸と平行に、Ｚｆ軸と平行に、それぞれ所望の距離だけ移動させる。
【００１９】
このとき、マーカＭにピントが合うように、移動によりマーカＭが撮影視野から外れないように、且つ、カメラ１２ひいてはフランジ１１の姿勢が移動前後で変わらないようにして移動前と移動後の２回撮影する（図６のフローチャートのステップＳ１：第１の行程）。そして、ＣＰＵ１３は、フランジ１１のＸｆ軸、Ｙｆ軸、Ｚｆ軸と平行の移動距離を、それぞれロボット座標Ｓ上でのフランジ座標Ｆの移動前後の位置から求め、ＲＡＭ１７に記憶する。
【００２０】
このときのフランジ１１の移動のさせ方は、一つの軸に平行に移動させた後、元の位置に戻してから次の軸に平行に移動させるようにしても良いし、一つの軸に平行に移動させた後、その位置から次の軸に平行に移動させるようにしても良い。
フランジ１１が、Ｘｆ軸と平行に、Ｙｆ軸と平行に、Ｚｆ軸と平行に、それぞれ所望の距離だけ移動されると、その後、制御装置３のＣＰＵ１３は、フランジ座標Ｆに対するカメラ座標Ｃの傾きを推定する（ステップＳ２）。
【００２１】
このフランジ座標Ｆに対するカメラ座標Ｃの傾きを推定する方法を説明する前に、予備知識として、ロボット座標Ｓの原点ＰからワークＷのマーカＭまでのベクトル（第１ベクトル）、ロボット座標Ｓの原点Ｐからフランジ座標Ｆの原点Ｐｆまでの変換行列（第２行列）、フランジ座標Ｆの原点ＰｆからワークＷのマーカＭまでのベクトル（第３ベクトル）、フランジ座標Ｆの原点Ｐｆからカメラ座標Ｃの原点Ｐｃまでの変換行列（第４行列）、カメラ座標Ｃの原点ＰｃからワークＷのマーカＭまでのベクトル（第５ベクトル）の関係について説明し、これらの関係を基にしてカメラ座標Ｃの傾きを求める式を導くこととする。
【００２２】
ロボット本体２が図７に実線で示す位置（以下、ｉ位置という。）にあるとき、上記第１ベクトル〜第５ベクトルは次の（１）式の関係がある。
【００２３】
【数１】
2010112731.tif 000003
【００２４】
なお、第１ベクトル、第３ベクトルおよび第５ベクトルは、１行４列（第４要素は０）のベクトルで示される。第２行列および第４行列は、４行４列の行列で示される。
また、ロボット本体２が図７に破線で示す位置（以下、ｊ位置という。）にあるとき、上記第１ベクトル〜第５ベクトルは次の（２）式の関係がある。
【００２５】
【数２】
2010112731.tif 000004
【００２６】
なお、第１ベクトルおよび第５ベクトルは、１行４列（第４要素は０）のベクトルで示される。第２行列、第３行列および第４行列は、４行４列の行列で示される。
そして、ロボット本体２をｉ位置からｊ位置に移動させたとき、フランジ１１の移動ベクトルは、ｊ位置における第３ベクトルトとｉ位置における第３ベクトルとの差分に等しいので、次の（３）式で表すことができる。同様に、ロボット本体２をｉ位置からｊ位置に動作させたとき、カメラ１２の移動ベクトルは、ｊ位置における第５ベクトルトとｉ位置における第５ベクトルとの差分に等しいので、次の（４）式で表すことができる。
【００２７】
【数３】
2010112731.tif 000005
【００２８】
なお、フランジ座標の移動ベクトルおよびカメラ座標の移動ベクトルは、１行４列（第４要素は０）のベクトルで示される。
ロボット本体２がｉ位置からｊ位置へ移動したと考えた場合、（２）式は、（１）式の第３ベクトルに（３）式の移動ベクトルを置換し、（１）式の第５ベクトルに（４）式の移動ベクトルを置換した場合に等しいから、次の（５）式が成り立つ。
【００２９】
【数４】
2010112731.tif 000006
【００３０】
この（５）式において、左辺と右辺を、共通のｉ位置での第２ベクトルで除すと、次の（６）式となる。
【００３１】
【数５】
2010112731.tif 000007
【００３２】
上記ステップＳ１において、フランジ１１を、フランジ座標ＦのＸｆ軸、Ｙｆ軸、Ｚｆ軸と平行に、それぞれ所望の距離移動させた状態は、図７において、フランジ１１をｉ位置からｊ位置に移動させた場合に相当する。このときのフランジ１１がフランジ座標ＦのＸｆ軸、Ｙｆ軸、Ｚｆ軸と平行に移動した距離をそれぞれΔｘ、Δｙ、Δｚとして、ｉ位置からｊ位置までの各座標軸方向への移動ベクトルをｉ位置のフランジ座標Ｆを基準にして表すと、次の（７）〜（９）式で表される。
【００３３】
【数６】
2010112731.tif 000008
【００３４】
また、フランジ１１を、フランジ座標ＦのＸｆ軸、Ｙｆ軸、Ｚｆ軸と平行に所望の距離移動させたとき、その移動によるカメラ座標Ｃの移動ベクトルを、ｉ位置のときのカメラ座標Ｃを基準にして当該カメラ座標ＣのＸｃ軸、Ｙｃ軸、Ｚｃ軸と平行な移動ベクトルに分解して表すと、次の（１０）〜（１２）式になる。
【００３５】
【数７】
2010112731.tif 000009
【００３６】
一方、フランジ座標Ｆのｉ位置からｊ位置までの移動ベクトルを、（７）〜（９）式を用いて表すと、次の（１３）式となる。
【００３７】
【数８】
2010112731.tif 000010
【００３８】
前記（６）式の左辺は、（１３）式のフランジ座標Ｆのｉ位置からｊ位置までの移動ベクトルに等しいので、（６）式は、次の（１４）式のように表すことができる。
【００３９】
【数９】
2010112731.tif 000011
【００４０】
そして、（１４）式の右辺を、前記（１０）〜（１３）式を用いて示すと、次の（１５）式となる。従って、（１４）式は、（１６）式のように表すことができる。
【００４１】
【数１０】
2010112731.tif 000012
【００４２】
ところで、フランジ座標Ｆに対するカメラ座標Ｃの傾きを求めるには、カメラ座標Ｃの原点Ｐｃがフランジ座標Ｆの原点Ｐｆに一致している（オフセットが０）と仮定して回転成分のみを考えれば良いので、前記（１６）式は、次の（１７）式となる。
【００４３】
【数１１】
2010112731.tif 000013
【００４４】
なお、括弧内のΔｐｃｘ、Δｐｃｙ、Δｐｃｚは、１行３列のベクトルで示される。
さて、フランジ１１をＸｆ軸、Ｙｆ軸、Ｚｆ軸の各方向に移動させたときのカメラ１２のカメラ座標Ｃ上での移動ベクトルは、Ｘｃ軸方向成分とＹｃ軸方向成分とＺｃ軸方向成分で表すことができる。このうち、Ｘｃ軸方向成分とＹｃ軸方向成分は、カメラ１２の撮像画面２１に写された移動前のマーカＭの映像位置から移動後のマーカＭの映像位置までのＸｃ軸方向成分とＹｃ軸方向成分から得ることができる。
【００４５】
例えば、図８に示すように、フランジ１１をＸｆ軸方向に移動させたとき、移動前のマーカＭの映像位置がＩｘａで示す位置で、移動後のマーカＭの映像位置がＩｘｂであったとすると、その移動ベクトルは、ＩｘａからＩｘｂに至る矢印で示される。ちなみに、撮像画面２１上ではマーカＭは、カメラ１２の実際の移動方向とは上下左右逆になって現れるので、移動ベクトルは、撮像画面１２上のベクトルとは上下左右が逆のベクトルに変換する必要がある。なお、移動後の位置でマーカＭのピントに合っておらず、ぼやけて撮像された場合には、そのぼやけている範囲の中央点をマーカＭの映像位置とする。同様に、マーカＭにピントが合っていても、大きく撮影されている場合には、そのマークＭの画像の図形中心を映像位置とする。
【００４６】
この移動ベクトルのＸｃ軸方向成分とＹｃ軸方向成分の長さは、ＩｘａとＩｘｂを結ぶ直線を斜辺とする直角三角形を描き、その直角三角形のＸｃ軸と平行な辺およびＹｃ軸と平行な辺の長さで示される。そして、各辺の長さは、各辺が通る画素（縦横同じ長さの正方形）の数によって得ることができる。ここでは、フランジ座標Ｆに対するカメラ座標Ｃの傾きだけを求めようとしているので、実際のＸｃ軸方向およびＹｃ軸方向の移動量を求める必要はなく、向きさえ分かれば良い。このため、移動ベクトルのＸｃ軸方向成分とＹｃ軸方向成分の相対的な長さが分かれば良いので、Ｘｃ軸方向成分とＹｃ軸方向成分の大きさを画素数で表すことにする。フランジ１１をＹｆ軸方向およびＺｆ軸方向に移動させたときのカメラ１２カメラ座標Ｃ上でのＸｃ軸、Ｙｃ軸、Ｚｃ軸の各方向の移動ベクトルのＸｃ軸方向成分とＹｃ軸方向成分の大きさも、同様にして撮影画像から求めることができる。
【００４７】
しかしながら、撮影画像から移動ベクトルを検出する方法では、カメラ１２の撮影画面２１が２次元であるから、移動ベクトルのＺｃ軸方向成分を取得することができない。
そこで、（１７）式の回転行列の逆行列をとると、次の（１８）式のようになる。
【００４８】
【数１２】
2010112731.tif 000014
【００４９】
更に、（１８）式のフランジ１１の移動ベクトルの逆行列をとると、次の（１９）式のようになる。
【００５０】
【数１３】
2010112731.tif 000015
【００５１】
この（１９）式の右辺は、カメラ１２のカメラ座標Ｃ上での移動ベクトルであるから、（１９）式は、更に次の（２０）式となる。
【００５２】
【数１４】
2010112731.tif 000016
【００５３】
ここで、ａｘ，ａｙ，ａｚは、前述のフランジ１１をＸｆ軸と平行に移動させたときのカメラ１２のカメラ座標Ｃ上での移動ベクトルをＸｃ軸方向、Ｙｃ軸方向、Ｚｃ軸方向に分解したときの各成分を示す。ｂｘ，ｂｙ，ｂｚは、前述のフランジ１１をＹｆ軸と平行に移動させたときのカメラ１２のカメラ座標Ｃ上での移動ベクトルをＸｃ軸方向、Ｙｃ軸方向、Ｚｃ軸方向に分解したときの各成分を示す。ｃｘ，ｃｙ，ｃｚは、前述のフランジ１１をＺｆ軸と平行に移動させたときのカメラ１２のカメラ座標Ｃ上での移動ベクトルをＸｃ軸方向、Ｙｃ軸方向、Ｚｃ軸方向に分解したときの各成分を示す。
【００５４】
ただし、前述したように、ａｚ，ｂｚ，ｃｚは、未知である。しかし、（２０）式の中辺の逆行列式は、フランジ１１をＸｆ軸、Ｙｆ軸、Ｚｆ軸に平行に移動させたときの移動量であるから、ＣＰＵ１３はＲＯＭ１６が記憶する逆行列式解法プログラムによって当該逆行列式を解くことができる。また、ａｚ，ｂｚ，ｃｚは、次の（２１）〜（２４）式に示された条件を満たすので、（２０）式を解くこと、つまりフランジ座標Ｆに対するカメラ座標Ｃの傾きを求めることができる（第３の行程）。
【００５５】
【数１５】
2010112731.tif 000017
【００５６】
なお、（２１）式は４列４行の同時変換行列のうち３列３行の回転行列の成分であり、回転行列の成分の各ベクトルの大きさ（ノルム）は１になるという条件に基づくものである。（２２）〜（２４）式は、移動ベクトルのＸｃ軸、Ｙｃ軸、Ｚｃ軸の各方向の成分は互いに直交しているので、その内積は０という条件に基づくものである。
以上のことから、ＣＰＵ１３は、フランジ１１がＸｆ軸、Ｙｆ軸、Ｚｆ軸に平行に移動されると、フランジ座標Ｆに対するカメラ座標Ｃの傾きを求めるために、各軸との平行移動について、その移動ベクトルのＸｃ軸方向成分とＹｃ軸方向成分を画素数として求める。次に、ＣＰＵ１３は、（２０）式を解くことによってフランジ座標Ｆに対するカメラ座標Ｃの傾きを求める（推定する）。
【００５７】
このようにして、フランジ座標Ｆに対するカメラ座標Ｃの傾きが求められると、次に、ユーザは、ティーチングペンダント４を用いて、ロボット本体２のフランジ１１を、カメラ座標ＣのＸｃ軸およびＹｃ軸のうちの少なくとも一方、この実施形態では、Ｘｃ軸およびＹｃ軸にそれぞれ平行に、それぞれ所望の距離だけ移動させる。このとき、カメラ１２の姿勢が移動前後で変わらないように、マーカＭがカメラ１２の撮影視野内に入る状態にして、当該マーカＭを、撮像画面２１により移動前と移動後の２回撮影する（ステップＳ３：第４の行程）。
【００５８】
すると、ＣＰＵ１３は、フランジ１１のＸｃ軸、Ｙｃ軸に沿う移動距離を、ロボット座標Ｓ上でのフランジ座標Ｆの移動前後の位置から求め（移動距離演算手段）、ＲＡＭ１７に記憶する。次に、ＣＰＵ１３は、カメラ座標ＣのＸｃ軸と平行な移動、Ｙｃ軸と平行な移動のそれぞれについて、カメラ１２の撮像画面２１に写された移動前のマーカＭの映像位置と移動後の映像位置の間のＸｃ軸方向の画素数とＹｃ軸方向の画素数をカウントする。図９は、フランジ１１がＹｃ軸方向に移動した場合の撮像画面２１でのマーカＭの移動前後の映像位置を例示している。そして、ＣＰＵ１３は、求めたＸｃ，Ｙｃ軸方向の移動距離を、Ｘｃ，Ｙｃ軸方向の画素数で除して、１画素あたりの移動距離を演算する（ステップＳ４：第５の行程）。この１画素当たりの移動量は、マーカＭとカメラ１２との間がマーカＭにピントが正確に合う距離だけ離れている状態の下でのものである。
なお、１画素当たりの移動距離を演算するには、フランジ１１をＸｃ軸方向とＹｃ軸方向のうちの一方の方向にだけ移動させることで求める（推定する）ことができる。Ｘｃ軸方向とＹｃ軸方向の２方向について１画素当たりの移動距離を求めた場合には、その平均をとることによってより正確な１画素当たりの移動量を求めることができる。
【００５９】
次に、ユーザは、カメラ１２のＺｃ軸がフランジ座標Ｆの原点Ｐｆを通ると仮定し、ティーチングペンダント４を操作して仮定のＺｃ軸を中心にフランジ１１を所望の角度だけ回転させる。上記仮定のＺｃ軸を図５にＺｃｆで示す。このＺｃｆ軸を中心とする回転操作は、カメラ１２の姿勢が変わらないように、カメラ１２の撮影視野内にマーカＭが入り、且つマーカＭにピントが合った状態にして行い、回転前後を含む回転中に、少なくとも３回撮影する（ステップＳ５：第６の行程）。図１０（ａ）、（ｂ）、（ｃ）は回転開始時、回転途中の所望の時に１回、回転終了時の合計３回撮影したときのマーカＭの映像位置を例示している。
【００６０】
続いて、ＣＰＵ１３は、この３点のマーカＭの映像位置Ｐ１〜Ｐ３を相互に結んだ直線を少なくとも２本演算し、この２本の直線の中点を通る直交直線Ｌ１，Ｌ２を演算し、この２本の直交直線Ｌ１，Ｌ２の交点を演算する。そして、ＣＰＵ１３は、２本の直交直線の交点を回転中心Ｔｃと決定する（第７の行程）。次に、ＣＰＵ１３は、回転中心Ｔｃとカメラ座標Ｃの原点Ｐｃとの距離と方向を求める（ステップＳ６）。
【００６１】
つまり、回転中心Ｔｃは、フランジ座標Ｆの原点Ｐｆを通る直線であってカメラ座標ＣのＺｃ軸と平行な直線（Ｚｃｆ軸）上にあるから、撮像画面２１上において回転中心Ｔｃの位置からカメラ座標Ｃの原点ＰｃまでのベクトルＶ１を求める。そして、回転中心Ｔｃと原点Ｐｃまでの距離を、上記ベクトルＶ１のＸｃ軸方向成分およびＹｃ軸方向成分についての画素数を求め、これら２軸方向成分の画素数に１画素当たりの距離を乗じてＸｃ軸方向成分およびＹｃ軸方向成分の長さを求め、更に、Ｘｃ軸方向成分およびＹｃ軸方向成分の長さから、カメラ座標Ｃの原点Ｐｃと回転中心Ｔｃとの間の距離を求める。以上により、フランジ座標Ｆの原点Ｐｆを通るカメラ座標ＣのＺｃ軸と平行な直線（Ｚｃｆ軸）、ひいてはフランジ座標Ｆの原点Ｐｆからカメラ座標ＣのＺｃ軸までの方向と距離が求まることとなる（第８の行程）。このフランジ座標Ｆの原点Ｐｆからカメラ座標ＣのＺｃ軸までの方向と距離を示すベクトルを、図５にＶ２で表した。
【００６２】
このようにして、フランジ座標Ｆに対するカメラ座標Ｃの傾きと、フランジ座標Ｆの原点Ｐｆからカメラ座標ＣのＺｃ軸がどの方向にどれだけ離れた位置を通っているかが求められる。そして、カメラ座標Ｃの原点Ｐｃは、Ｚｃ軸が通る直線上にあるのであるが、フランジ座標Ｆの原点Ｐｆの原点からどれだけ離れた位置にあるのかが求まっていない。以下で、そのＺｃ軸が通る直線上のどこにカメラ座標Ｃの原点Ｐｃがあるかを求める。
【００６３】
即ち、ユーザは、カメラ座標ＣのＸｃ軸とＹｃ軸を含む平面がフランジ座標Ｆの原点Ｐｆを通ると仮定し、カメラ座標ＣのＸｃ軸またはＹｃ軸と平行に、ここでは、Ｘｃ軸と平行に所望の距離Ｇだけ移動させる（ステップＳ７：第９の行程）。このときも、カメラ１２の姿勢が移動前後で変わらないように、マーカＭがカメラ１２の撮影視野内に入り、且つマーカＭにピントが合った状態にして、当該マーカＭを移動前と移動後の２回撮影する。
【００６４】
すると、ＣＰＵ１３は、ロボット座標Ｓ上で移動前後のフランジ座標Ｆの位置からフランジ１１の移動距離Ｇを演算すると共に、カメラ１２の撮像画面２１に写された移動前のマーカＭの映像位置と移動後の映像位置との間のＸｃ軸方向の画素数をカウントする。そして、ＣＰＵ１３は、画素数に１画素の長さを乗じて移動前後のマーカＭの撮像画面２１上での移動距離を演算する（ステップＳ４）。なお、１画素の長さは、予めＲＯＭ１６に記憶されている（画素長記憶手段）。そして、ＣＰＵ１３は、Ｘｃ軸と平行のフランジ１１の移動距離Ｇと、撮像画面２１上での移動距離ｇと、カメラ１２のレンズ２０の焦点距離Ｌとにより、カメラ座標Ｃの原点ＰｃからマーカＭまでの距離Ｄを演算する（第１０の行程）。
【００６５】
ここで、図１１に示すように、レンズ２０の中心をＯとし、マーカＭの移動前の位置をＰ１、移動後の位置をＰ２、撮像画面２１上でのマーカＭの移動前の映像位置をＩ１、移動後の映像位置をＩ２とすると、三角形Ｏ，Ｐ１，Ｐ２と三角形Ｏ，Ｉ１，Ｉ２とは相似形である。また、レンズ２１の中心Ｏと撮像画面２１までの距離はレンズ２０の焦点距離Ｌに等しいので、レンズ２１の中心ＯからＰ１とＰ２とを結ぶ直線までの距離ｄは、Ｌ・Ｇ／ｇとなる。そして、Ｐ１とＰ２とを結ぶ直線から撮像画面２１の中心、つまりカメラ座標Ｃの原点Ｐｃまでの距離は、ｄ＋Ｌであるから、Ｄ＝（Ｌ・Ｇ／ｇ）＋Ｌとなる。
【００６６】
次に、ユーザは、カメラ座標ＣのＸｃ軸とＹｃ軸を含む平面がフランジ座標Ｆの原点Ｐｆを通ると仮定し、この仮定した位置でのカメラ座標ＣのＸｃ軸またはＹｃ軸、例えばＸｃ軸を中心に所望の角度θだけ回転させる。このときも、カメラ１２の姿勢が移動前後で変わらないように、マーカＭがカメラ１２の撮影視野内に入り、且つマーカＭにピントが合う状態にして、当該マーカＭを、撮像画面２１により回転前と回転後の２回撮影する（第１１の行程）。
すると、ＣＰＵ１３は、撮像画面２１上において、図１２に示すように回転前のマーカＭの映像位置Ｉ３と回転後のマーカＭの映像位置Ｉ４との撮像画面２１上での距離ｍを求める。この距離ｍは、Ｉ３とＩ４との間のＹｃ軸方向の画素数をカウントし、この画素数に１画素当たりの長さを乗じることによって算出することができる。
【００６７】
次いで、ＣＰＵ１３は、距離ｎと回転角θとから、距離ｎの２位置を生じさせるためのカメラ１２に対するマーカＭの相対的な移動距離Ｎを演算する。このＮは、図１１に示すように、Ｇの長さがｇの長さとなって撮像画面２１上に撮影されるから、Ｎ＝ｎ・Ｇ／ｇとなる。続いて、ＣＰＵ１３は、フランジ座標Ｆの原点Ｐｆを通る直線であってカメラ座標ＣのＸｃ軸と平行な軸（回転中心にしたＸｃｆ軸）からマーカＭまでの距離Ｑを演算する（ステップＳ１０：第１２の行程）。このＱは、Ｑ＝Ｎ／ｔａｎθで求めることができる。
【００６８】
そして、ＣＰＵ１３は、回転中心軸Ｘｃｆ軸からカメラ座標ＣのＸｃ軸およびＹｃ軸を含む平面までの距離Ｒを演算する（ステップＳ１１：第１３行程）。このＲは、Ｒ＝Ｑ−Ｄで求めることができる。
カメラ座標Ｃの原点Ｐｃは、Ｚｃ軸と、Ｘｃ軸およびＹｃ軸を含む平面との交点であり、カメラ座標ＣのＺｃ軸はフランジ座標Ｆの原点Ｐｆから図５のベクトルＶ２で示された点を通っているのである。このことから、カメラ座標Ｃの原点Ｐｃは、フランジ座標Ｆの原点ＰｆからベクトルＶ２で示された点を通るＺｃ軸と、フランジ座標の原点Ｐｆを通るＸｃ軸と平行なＸｃｆ軸を中心に半径がＲの円筒面との交点がカメラ座標Ｃの原点Ｐｃとなる。この場合、Ｚｃ軸と、Ｘｃｆ軸を中心とする半径Ｒの円筒面との交点は２つあるが、そのうちのフランジ座標ＦのＸｆ軸およびＹｆ軸を含む平面よりもマーカＭ側の交点をカメラ座標Ｃの原点Ｐｃとする。そして、ＣＰＵ１３は、求められたカメラ座標Ｃの原点Ｐｃとフランジ座標Ｆの原点Ｐｆとの間の距離を演算する。
【００６９】
以上により、フランジ座標Ｆに対するカメラ座標Ｃの傾きと、フランジ座標Ｆ上でのカメラ座標Ｃの原点Ｐｃ位置とが求まるので、ＣＰＵ１３は、その傾きと位置をＲＡＭ１７に記憶する（ステップＳ１２：第１４の行程）。
このように本実施形態によれば、フランジ１１に取り付けられた２次元のカメラ１２によってフランジ座標Ｆとカメラ座標Ｃとの関係を求めることができる。
【図面の簡単な説明】
【００７０】
【図１】本発明の一実施形態を示すもので、視覚検査装置の斜視図
【図２】ロボットの制御構成を示すブロック図
【図３】フランジ座標を示す斜視図
【図４】カメラの概略的な断面図
【図５】フランジ座標とカメラ座標との関係を示す斜視図
【図６】フランジ座標に対するカメラ座標の関係を求めるためのフローチャート
【図７】ロボット本体のスケルトン図
【図８】移動ベクトルを検出するための撮影画面を示す図
【図９】１画素当たりの移動量を検出するための撮影画面を示す図
【図１０】Ｚｆ軸からのＺｃ軸のずれを検出するための撮影画面を示す図
【図１１】カメラとマーカとの距離を検出するための説明図
【図１２】フランジ座標からのカメラ座標の離間距離を検出するための原理図
【符号の説明】
【００７１】
図面中、２はロボット本体、３は制御装置、４はティーチングペンダント、１１はフランジ（手先部）、１２はカメラ、１３はＣＰＵ、１６はＲＯＭ、２０はレンズ、２１はＣＣＤ撮像画面（撮像センサ）を示す。
【図１】
2010112731.tif 000018
【図２】
2010112731.tif 000019
【図３】
2010112731.tif 000020
【図４】
2010112731.tif 000021
【図５】
2010112731.tif 000022
【図６】
2010112731.tif 000023
【図７】
2010112731.tif 000024
【図８】
2010112731.tif 000025
【図９】
2010112731.tif 000026
【図１０】
2010112731.tif 000027
【図１１】
2010112731.tif 000028
【図１２】
2010112731.tif 000029