JP6210748B2 - 三次元位置計測装置、及び三次元位置計測装置のキャリブレーションずれ判定方法 - Google Patents

Description

本発明は、第１カメラ及び第２カメラを有するステレオカメラにより撮像した画像を用いて対象物の位置を三次元計測する三次元位置計測装置、及びその三次元位置計測装置のキャリブレーションずれ判定方法に関する。

近年、例えばロボット装置を用いた生産設備等において、ステレオカメラを用いたワーク等の三次元位置計測が行われており、その位置計測に基づき各種の生産作業（把持、組付け、ラベル貼付け等の作業）が行われている。このような三次元位置計測を正確に行うためには、事前にステレオカメラに対するキャリブレーションを実施する必要がある。例えばキャリブレーションずれが発生してしまうと、ワークの位置計測精度に影響が生じ、作業の失敗等が生じて、生産効率の低下を招く虞がある。また、例えばキャリブレーションずれの判定を実行する度に生産設備のシステムを停止していると、同じく生産効率の低下を招く虞がある。そのため、例えば生産設備のシステムの稼働中に自動的にキャリブレーションずれを判定することが望まれる。

ところで、キャリブレーションずれの発生は、大まかに、ステレオカメラ内部の位置ずれ（例えば２つのカメラの相対位置ずれ）と、ステレオカメラ外部の設置位置ずれと、が要因として挙げられる。キャリブレーションずれが発生した際は、速やかに要因箇所に対する的確なキャリブレーションを実行し、生産作業の正確性を戻すことが望ましいので、この要因となる箇所を正確に特定することが求められる。

そこで、基準マーカをステレオカメラの撮像範囲内（視野内）に配置しておき、基準マーカの三次元位置を測定することでキャリブレーションずれを判定するものが提案されている（特許文献１参照）。この特許文献１のものは、基準マーカの三次元計測の結果、画像上の基準マーカがエピポーラ線から所定量ずれた場合には、ステレオカメラの内部の位置ずれによるキャリブレーションずれと判定する。また、基準マーカの三次元計測の結果、画像上の基準マーカがエピポーラ線から所定量以内にあって、かつ基準マーカの位置座標系にずれがある場合には、ステレオカメラ全体の位置ずれによるキャリブレーションずれと判定する。このようにキャリブレーションずれの要因箇所を特定できると、速やかなキャリブレーションが可能となり、速やかな生産設備の復帰が望めることになる。

特開２００４−３５４２５６号公報

ところで、例えばステレオカメラの内部にあって、カメラ間の距離がずれる、いわゆる基線方向のずれが発生することもあるという問題がある。例えばステレオカメラの内部にあって、レンズの固定用ネジ等が緩み、レンズが水平方向にずれたりすると、カメラ間の距離がずれる。この際、上記特許文献１のキャリブレーションずれの判定手法であると、画像上の基準マーカがエピポーラ線から所定量以内にあり、かつカメラ間の距離に基づき演算する奥行き方向の座標系がずれることになる。すると、上記特許文献１のキャリブレーションずれの判定手法では、ステレオカメラ内部の位置ずれにも拘らず、ステレオカメラ外部の設置位置ずれによるキャリブレーションずれと誤判定してしまうという問題がある。

そこで本発明は、キャリブレーションずれの発生箇所を正確に判定することが可能な三次元位置計測装置、及び三次元位置計測装置のキャリブレーションずれ判定方法を提供することを目的とするものである。

本発明は、第１カメラ及び第２カメラを有するステレオカメラにより撮像した画像を用いて対象物の位置を三次元計測する三次元位置計測装置において、前記ステレオカメラから奥行き方向の距離が異なり、かつ前記距離の初期値が記憶されている複数のマーカ、の少なくとも一方のマーカに関する位置計測値のずれ量が所定量よりも大きい場合にキャリブレーションずれの発生を判定する制御部を備え、前記制御部は、前記キャリブレーションずれの発生を判定した場合において、前記ステレオカメラの内部における前記第１カメラと前記第２カメラとの相対位置がずれたステレオカメラ内部のキャリブレーションずれを判定し、前記ステレオカメラ内部のキャリブレーションずれと判定されなかった場合に、前記ステレオカメラの設置位置がずれたステレオカメラ外部のキャリブレーションずれと判定し、前記ステレオカメラ内部のキャリブレーションずれの１つとして、前記複数のマーカを前記第１カメラ及び前記第２カメラにより撮像させ、前記複数のマーカの間の距離を算出し、算出した距離が前記初期値に対して所定値よりも大きくずれた場合に、前記第１カメラと前記第２カメラとの基線長が変化した、前記ステレオカメラ内部のキャリブレーションずれと判定する制御部を備えたことを特徴とする。

また、本発明は、第１カメラ及び第２カメラを有するステレオカメラにより撮像した画像を用いて対象物の位置を三次元計測する三次元位置計測装置におけるキャリブレーションずれを判定する三次元位置計測装置のキャリブレーションずれ判定方法において、制御部が、前記ステレオカメラから奥行き方向の距離が異なり、かつ前記距離の初期値が記憶されている複数のマーカを、前記第１カメラ及び前記第２カメラにより撮像する撮像工程と、前記制御部が、前記複数のマーカの少なくとも一方のマーカに関する位置計測値のずれ量が所定量よりも大きい場合にキャリブレーションずれの発生を判定する工程と、前記制御部が、前記キャリブレーションずれの発生を判定した場合において、前記ステレオカメラの内部における前記第１カメラと前記第２カメラとの相対位置がずれたステレオカメラ内部のキャリブレーションずれを判定し、前記ステレオカメラ内部のキャリブレーションずれと判定されなかった場合に、前記ステレオカメラの設置位置がずれたステレオカメラ外部のキャリブレーションずれと判定する工程と、前記制御部が、前記複数のマーカの間の距離を算出する算出工程と、前記制御部が、前記ステレオカメラ内部のキャリブレーションずれの１つとして、算出した距離が前記初期値に対して所定値よりも大きくずれた場合に、前記第１カメラと前記第２カメラとの基線長が変化したステレオカメラ内部のキャリブレーションずれと判定する工程と、を備えたことを特徴とする。

本発明によると、奥行き方向の距離の初期値が記憶されている複数のマーカ間の距離を算出し、算出した距離が初期値と所定値以上ずれた場合に、第１カメラと第２カメラとの基線長が変化したステレオカメラ内部のキャリブレーションずれと判定できる。即ち、画像上のマーカがエピポーラ線から所定量以内にあっても、ステレオカメラ外部の設置位置ずれによるキャリブレーションずれと誤判定してしまうことを防止できる。従って、キャリブレーションずれの発生箇所を正確に判定することが可能となる。

ツールをロボットハンドにより把持した状態のロボット装置の概略構成を示す説明図。 ツールを置き台に載置した状態のロボット装置の概略構成を示す説明図。 三次元位置計測装置の構成を示すブロック図。 ツール及びステレオカメラを示す側面図。 ステレオカメラの構造を示す図で、（ａ）は背面断面図、（ｂ）は左カメラの側面断面図、（ｃ）は右カメラの側面断面図。 キャリブレーションずれ判定制御を示すフローチャート。 初期値保存処理を示すフローチャート。 ステレオカメラの視野を示す説明図。 ステレオ平行化後の左右カメラの撮影画像を示す説明図。

以下、本発明に係る実施の形態を図１乃至図９に沿って説明する。まず、三次元位置計測装置１を備えたロボット装置１００の構成について図１乃至図５に沿って説明する。

ロボット装置１００は、架台９の上面に設置された多関節ロボットアーム２を有しており、多関節ロボットアーム２の先端部には、ロボットハンド３が取り付けられて備えられている。また、ロボット装置１００は、ロボットハンド３により把持自在（着脱自在）で、かつワーク（対象物）７に対する作業が可能なツール４を備えている。具体的には、ツール４は、ワーク（部品）７を把持して他の部品に組付け作業するピンセット８を有している。さらに、ツール４には、当該ツール４の一部であるピンセット８が撮像範囲内となるように、ステレオカメラ５が固定されている。

一方、ロボット装置１００には、上記架台９の上面にあって、ロボットハンド３により把持を解除してツール４を置くための置き台６が備えられている。さらに、架台９の上面にあって、ロボットアーム２と対角の端部には、積層回転灯１０が配設されて備えられている。積層回転灯１０は、例えば上から赤色、黄色、緑色の回転灯が積層されて備えられている。

また、ロボット装置１００には、ロボットアーム２、ロボットハンド３、ツール４のピンセット８、ステレオカメラ５、積層回転灯１０などを制御する主制御装置（制御部）２０が備えられている。主制御装置２０はコンピュータであり、主制御装置内の各部はハードウェアや回路で構成されてもよいし、ソフトウェアで処理されてもよい。

具体的には、図３に示すように、主制御装置２０は、ステレオカメラ５と共に三次元位置計測装置１を構成しており、画像処理部２１、演算部２２、記憶部２３、制御部２４、表示部２５を有している。制御部２４は、ステレオカメラ５に対して計測指令を送信するタイミングや、ロボットアーム２、ロボットハンド３、ピンセット８の駆動を制御する。画像処理部２１は、ステレオカメラ５から伝送された画像データについて画像処理を行う。演算部２２は、画像処理部２１から送られてきた処理結果の演算や、詳しくは後述するキャリブレーションずれの判定処理の演算などを行う。記憶部２３は、詳しくは後述するマーカ１８、１９の三次元計測位置の初期値等のデータを記憶する。また、記憶部（記憶媒体）２３は、詳しくは後述するキャリブレーションずれ判定を行うための各種プログラム（判定プログラムを含む）が記録されている。そして、表示部２５は、詳しくは後述するキャリブレーションずれの判定結果を表示する。

ついで、ツール４及びステレオカメラ５の構造について図４に沿って説明する。図４に示すように、ツール４には、固定プレート１１が設けられている。その固定プレート１１には、ステー１６の一端部がねじ１２によって固定されており、そのステー１６の他端部には、ねじ１７によりステレオカメラ５が固定されている。なお、本実施の形態では、図４に示すように、ステレオカメラ５の基線方向をｘ軸とし、基線方向と直交する画像の上下方向をｙ軸とし、ｘ軸及びｙ軸に直交する方向をｚ軸とする。つまりｚ軸の方向は、ステレオカメラ５の奥行き方向成分である。

また、ツール４には、上述したようにワーク７を把持自在なピンセット８が配設されている。そのピンセット８は、ステレオカメラ５側に配置された平板８ａと、ステレオカメラ５から視て平板８ａの裏側に配置された平板８ｂとを有している。平板８ａ及び平板８ｂは、上記制御部２４により不図示のモータ等で駆動制御されることで板厚方向に開閉自在に駆動され、ワーク７を把持可能となっている。

上記ピンセット８の平板８ａのステレオカメラ５側の表面は、表面処理により白色に近い色に処理されていると共に、例えば黒色で丸型（黒丸）のマーカ１８，１９が描かれている。なお、表面が黒色、マーカが白色でもよい。マーカ１８，１９は、図４に示すように、ステレオカメラ５からの奥行き方向（ｚ軸の方向）の距離が異なるように配置されている。また、マーカ１８，１９は、ステレオカメラ５の左カメラ３０及び右カメラ４０（図５参照）の撮像範囲内に配置されている（図８参照）。

なお、マーカ１８，１９の形状は、黒丸に限定される必要はない。また、マーカ１８、１９は、ステレオカメラ５から視てｙ軸の方向に一直線上に並んだ位置に限定されるものではなく、ステレオカメラ５からの奥行き方向（ｚ軸の方向）の距離が異なっていればよい。特にステレオカメラ５とマーカ１８，１９との相対位置関係が既知となるのであれば、マーカ１８，１９は、置き台６上や架台９に配置してもよい。

さらに、マーカの数は２つに限定されるものではなく、２つ以上あってもよい。マーカ数が増加するほど、計測点が増えるため、キャリブレーションずれの判定精度は向上するが、本実施の形態では、説明を簡易化するため、マーカ数が２つの場合について説明する。

また、本実施の形態では、後述するキャリブレーションずれの判定を実行する場合は、基本的にピンセット８の平板８ａ及び平板８ｂを閉じた状態で行うようにする。しかしながら、閉じた状態に拘らず、マーカ１８、１９の位置が既知の位置となるのであれば、平板８ａの位置はどの位置であってもキャリブレーションずれの判定を実行し得る。さらに、本実施の形態では、平板８ａ及び平板８ｂが両方とも開閉駆動するものを説明している。しかしながら、平板８ａが固定で、平板８ｂだけが開閉駆動される構造であれば、ステレオカメラ５とマーカ１８、１９との相対位置関係が常に既知の位置となるので、ピンセット８の駆動中であってもキャリブレーションずれの判定を実行可能となる。

次に、ステレオカメラ５の内部構造について図５に沿って説明する。図５（ａ）に示すように、ステレオカメラ５は左カメラ（第１カメラ）３０、右カメラ（第２カメラ）４０と、これら２つのカメラ３０、４０を保持する支持体５０から構成されている。カメラ３０、４０にあるイメージセンサ基板３１、４１には、ＣＭＯＳ素子を撮像素子とするイメージセンサ３２、４２が実装されている。イメージセンサ基板３１、４１は、支持体５０に、ねじ５１、５２とねじ５３、５４とによりそれぞれ固定されている。

また、図５（ｂ）に示すように、カメラ３０、４０の内部構造はそれぞれ同様の構造である。上述したイメージセンサ基板３１、４１は、レンズマウント３５、４５にねじ３３、３４と４３、４４により固定されている。また、レンズ３９、４９はレンズマウント３５、４５にねじ３６、３７とねじ４６、４７により固定されている。

ここで、本ロボット装置１００を用いた生産工程の概略について説明する。まず、ツール４の先端にある電動のピンセット８を用いてワーク７を把持し、これを不図示の本体部品に組み付ける。組み付けが完了したら、ツール４を置き台６に置き、ロボットアーム２、ロボットハンド３は、別の部品を本体部品に組み付ける。本体部品の組み付けが全て完了したら、新たに供給された組み付け前の本体部品の組み付けを開始するため、再度ツール４を把持し、ワーク７の組み付け工程に入る。これらの工程を繰り返し実施する。

続いて、上記の生産工程の詳細について説明する。図１に示すように、ロボット装置１００の使用時には、架台９上に設置されたロボットアーム２の先端にロボットハンド３が取り付けられている。図２に示すように、ロボット装置１００の使用開始時（使用開始前）には、ツール４は置き台６に置かれている。ロボット装置１００は、ツール４をロボットハンド３を用いて把持するために、主制御装置２０内の制御部２４からロボットアーム２に対して、シリアル通信路を介して、移動指令を送りロボットアーム２を置き台６付近の所定位置まで移動させる。移動が完了したら、制御部２４からロボットハンド３に対して把持指令を送信し、ツール４を把持する。

次に、ツール４の先端にあるピンセット８を用いて架台９上に配置されたワーク７を把持するため、ロボットアーム２に対して制御部２４からシリアル通信路を介して移動指令を送信し、ワーク７付近にピンセット８の先端が来るようにツール４を移動させる。このとき、ワーク７の位置姿勢は不定であるため、ワーク７の把持を行う前に、ワーク７をツール４に組み付けられたステレオカメラ５を用いて撮影し、ワーク７の位置姿勢を画像処理により三次元位置計測する。画像処理は、主制御装置２０内の画像処理部２１において、ステレオカメラ５から伝送される画像データに基づいて行われる。

画像処理部２１で行われた処理結果は演算部２２に送信され、演算部２２はこの処理結果と後述のキャリブレーション結果を用いて、ロボットアーム２の移動量を演算する。この演算した移動量のデータを制御部２４に送信し、制御部２４からロボットアーム２に対してこの移動量分だけ移動するように指令を送信することにより、ロボットアーム２はワーク７を把持するのに適した位置に移動する。移動が完了したら、制御部２４からツール４に対してピンセット８を閉じるように指令を送信することにより、ピンセット８がワーク７を把持する。制御部２４は把持したワーク７を本体部品に組み付けるために、所定の位置にロボットアーム２を移動させ、移動した後にピンセット８を開くように指令を送信する。

以上のように、ワーク７の組み付けが完了されたら、ツール４を置き台６に置くために、制御部２４はロボットアーム２に対して置き台６付近の所定位置まで移動するように指令を送信する。移動が完了したら、制御部２４はロボットハンド３に対して開くように指令を送信することにより、ツール４は置き台６に置かれる。このとき、ツール４及びステレオカメラ５に衝撃が加わる可能性があり、キャリブレーションずれが発生する一因となる可能性がある。

ツール４を置き台６に置いたら、ロボットアーム２、ロボットハンド３は別の部品を不図示の本体部品に組み付ける工程を実施する。別の部品の組み付け工程が完了したら、新たにワーク７及び本体部品が供給される。新たに供給された本体部品にワーク７を組み付けるために、制御部２４はロボットハンド３を用いてツール４を把持する工程に戻る。本ロボット装置１００を用いた生産工程はこれらの処理を繰り返し実施する。

なお、本ロボット装置１００には上から赤色、黄色、緑色の積層回転灯１０が架台９上に取り付けられている。正常時は緑色の回転灯が点灯し、詳しくは後述する診断の結果、キャリブレーションずれが発生した場合には赤色または黄色の回転灯が点灯する。詳しくは後述するように、点灯している色により復帰方法が異なる。この積層回転灯１０は主制御装置２０から制御可能である。

続いて、本ロボット装置１００の三次元位置計測装置１におけるキャリブレーションについて説明する。キャリブレーションは、例えば上述した生産工程を実施する前に実行することが好ましいが、必ずしも生産工程を実施する毎に行う必要はない。

キャリブレーションには大きく分けて、
Ａ：ステレオカメラ５の内部のキャリブレーション
Ｂ：ステレオカメラ５の外部のキャリブレーション
の２つがある。上記Ａのキャリブレーションを以下「内部キャリブレーション」といい、上記Ｂのキャリブレーションを以下「外部キャリブレーション」という。

内部キャリブレーションとは、ステレオカメラ５を構成するカメラ３０，４０の各々の焦点距離、画像中心、レンズ歪み等の、撮影特性に関するパラメータと、それらカメラ３０，４０間の相対位置姿勢を規定するパラメータに関するキャリブレーションである。

この内部キャリブレーションの方法の一例について説明する。まず、ステレオカメラ５を用いて形状の分かっている対象を撮影する。ステレオカメラ５内のカメラ３０，４０の撮影画像を画像処理することにより、対象の形状を、画像座標を用いて計測する。また、既知である対象の形状と画像座標上の対象の形状との相関性は、上述した撮影特性に関する各パラメータを用いて表すことが可能である。この相関性から各パラメータを求めることが内部キャリブレーションである。

ただし、画像処理により対象の形状を計測する場合、量子化誤差が存在するため、読み取り誤差が発生するのは避けることができない。そのため、対象の位置姿勢を変えながら複数枚撮影し、各撮影画像において、対象の形状と画像座標上の対象の形状の相関性を求め、これらの結果を最小２乗法等を用いて近似することにより、この読み取り誤差の影響を小さくする必要がある。このように、内部キャリブレーションを実施する場合には、対象の位置姿勢を変化させながら撮影する必要があるため、ステレオカメラ５をロボット装置１００から取外し、専用装置を用いてキャリブレーションを実施する必要がある。

次に、外部キャリブレーションＢとは、ステレオカメラ５とツール４（ピンセット８）との外部装置間の位置姿勢（設置位置）に関するキャリブレーションであり、つまりは、ハンドアイキャリブレーションである。外部キャリブレーションは、内部キャリブレーションの結果を用いて実施するため、内部キャリブレーションの結果が変化した場合、外部キャリブレーションも再度実施する必要がある。

この外部キャリブレーション方法の一例について説明する。まず、ステレオカメラ５の視野内に形状の分かっている対象を配置し、ロボットアーム２の位置姿勢を変えながら、この対象を様々な方向から複数枚撮影する。このとき、ロボットアーム２の移動量から、この対象とステレオカメラ５との位置の変化量を求めることができる。これと、ステレオカメラ５のカメラ３０，４０の撮影画像を画像処理することにより求まる、画像座標上の対象の形状との相関性から、ステレオカメラ５とツール４との位置関係を算出することができる。

ただし、当然ながら、この手法でも、読み取り誤差が生じるため、最小２乗法等の近似方法を用いてこの誤差を小さくする必要がある。このように、外部キャリブレーションは、ステレオカメラ５をツール４から取り外すことなく、本ロボット装置１００を含む生産設備を用いて実施することができる。これら内部キャリブレーションや外部キャリブレーションのキャリブレーション結果は主制御装置２０の記憶部２３に保存される。

以上説明した内部キャリブレーションや外部キャリブレーションのキャリブレーション結果のどちらか一方でも、キャリブレーションを実施した時からずれることをキャリブレーションずれと呼ぶ。例えば長期間使用すると、部材の経年変化や、振動、衝撃によるカメラやレンズの固定ねじの緩み等が発生する。すると、ステレオカメラ５の内部のカメラ３０，４０の位置関係のずれ（ステレオカメラ内部のキャリブレーションずれ）やステレオカメラ５のツール４に対する設置位置のずれ（ステレオカメラ外部のキャリブレーションずれ）が発生する。このようなキャリブレーションずれの発生は、基本的には避けられないものである。

ついで、キャリブレーションずれの判定制御について主に図６及び図７のフローチャートに沿って説明する。まず、ステップＳ１における測定結果の初期値保存の処理について説明する。本処理は、ステレオカメラ５を用いてマーカ１８、１９の位置計測を実施し、その結果を初期値として主制御装置２０内の記憶部２３に保存する処理である。なおこの際は、システム（三次元位置計測装置１）が正常であることを表示するために、積層回転灯１０の緑色の回転灯を点灯する。

このステップＳ１における測定結果の初期値保存の処理は、基本的に、ステレオカメラ５をツール４に組み付けた後に、１度だけ実施する処理である。ただし、例えばステレオカメラ５を交換し、内部キャリブレーションや外部キャリブレーションを再度実施した場合などには、再び本処理を実施する必要がある。本処理で保存した初期値は、図６に示す制御において、キャリブレーションずれが発生していないか判定するために使用される。

本ステップＳ１の詳細な処理を図７に示し、これを説明する。ステップＳ１−１においては、主制御装置２０内の制御部２４からステレオカメラ５に対して、撮影指令を送信し、主制御装置２０内の画像処理部２１にステレオカメラ５内の各カメラ３０、４０で撮影した画像を取り込む。各カメラ３０、４０で撮影した画像は、例えば図８に示すように、右カメラ４０の画像６０Ｒと、左カメラ３０の画像６０Ｌとが取得される。２つの画像６０Ｒ，６０Ｌは、それぞれ２つのマーカ１８，１９がその視野に納まるように撮影される。

ステップＳ１−２では、レンズ歪み補正処理を実行する。即ち、画像処理部２１において、ステップＳ１−１で取り込んだカメラ３０，４０の画像６０Ｒ，６０Ｌに対してレンズ歪みを補正処理する。例えば最初に行ったキャリブレーション結果として、レンズ３９，４９の歪みパラメータが求まっているため、これらのパラメータを用いて歪み補正を実施する。

ステップＳ１−３では、ステレオ平行化処理を実行する。本処理の目的は、図９に示すように、カメラ３０，４０で同じ特徴点を抽出した場合、エピポーラ線拘束によりこの特徴点が同一の高さとなるように、撮影画像を画像処理することである。これにより、エピポーラ線からのずれ量を簡単に算出することができるため、これ以降の処理の簡易化が図れる。

同一の高さになるようにするためには、カメラ３０，４０の撮影方向が同じであり、カメラ４０の撮像面がカメラ３０の撮像面を水平軸方向に移動したようなカメラ配置となっているステレオカメラで撮影した画像のように、撮影画像を処理する必要である。ステレオカメラ５内部のキャリブレーション結果として、カメラ３０，４０の相対位置関係を表すパラメータが求まっているので、このステレオ平行化処理では、これらのパラメータを用いて、画像処理部２１において、撮影画像を回転、並進させる。これにより、図８に示す２つの画像６０Ｒ、６０Ｌが、ステレオ平行化処理されて、図９に示すステレオ平行化後の左カメラ撮影画像５０Ｌと、ステレオ平行化後の右カメラ撮影画像５０Ｒとになる。

ステップＳ１−４では、黒丸中心位置計測処理を実行する。即ち、本実施の形態ではマーカ１８，１９の形状は黒丸としているため、黒丸の中心位置を計測する。まず、画像処理部２１において、ステレオカメラ５内部のカメラ３０、４０それぞれの撮影画像データの輝度レベルに着目して、マーカが識別できるような適当な閾値を設けて２値化処理を実施する。そして、各撮影画像の閾値以下の領域のエッジについて楕円近似を行い、その楕円中心の画像座標（つまり中心位置）を求める。この時、カメラ３０，４０の撮影画像から求まった各楕円中心の画像座標を（ｕｌ，ｖｌ）（ｕｒ，ｖｒ）とする。この結果を画像処理部２１から演算部２２に送信する。ただし、中心位置の計測方法は上記処理に限定されるものではない。

ステップＳ１−５では、ステレオ計測処理を実行する。即ち、ステップＳ１−４で求めた楕円中心（ｕｌ，ｖｌ）、（ｕｒ，ｖｒ）から求まる視差ｕｌ−ｕｒ、キャリブレーション結果として求まっているカメラ３０，４０の相対位置関係、各カメラ３０，４０の焦点距離等を、パラメータとする。そして、演算部２２は、これらパラメータを用いて、三角測量の原理に基づきステレオ計測を実施し、マーカ１８，１９の位置の３次元座標を求める。

ステップＳ１−６では、規定回数以上計測したか否かを判別する。この規定回数はあらかじめ主制御装置２０内の記憶部２３に設定しておく。ここで、複数回計測する理由について説明する。ステップＳ１−１からＳ１−５のステレオ計測には一定のばらつきが避けられず、１度だけ実施し初期値を決めると信頼性に欠けるので、複数回計測を行って平均値を求める必要がある。また、後述するステップＳ４，Ｓ７，Ｓ９で使用する各値（所定量、所定値、許容値）は、計測精度よりも十分大きな値にする必要性があるため、計測結果の標準偏差も同時に求める必要がある。本実施の形態では測定回数をｎ回と定義し、ｎは１０以上とすることが好ましい。測定回数がｎ回以下と判別された場合にはステップＳ１−１に戻り、規定回数に達したらステップＳ１−７に移る。

ステップＳ１−７では、ステップＳ１−５のステレオ計測処理により求まった、ｉ回目のマーカ１８の測定結果を（ｘ_１−０（ｉ），ｙ_１−０（ｉ），ｚ_１−０（ｉ））、マーカ１９の測定結果を（ｘ_２−０（ｉ），ｙ_２−０（ｉ），ｚ_２−０（ｉ））とする。これらｎ回の測定結果の相加平均値（ｘ_１−０，ｙ_１−０，ｚ_１−０）、（ｘ_２−０，ｙ_２−０，ｚ_２−０）をステレオ計測結果の初期値（真値）とする。

また、このステップＳ１−７では、後述するステップＳ４で使用するずれ量に関する所定量を決定するためにステレオ計測結果のばらつき、即ち標準偏差を求める。ここでΔＬを、ｉ回目のマーカ位置の計測結果と、ｎ回の計測結果の相加平均値とのずれ量として定義する。本実施の形態では、マーカ１８の位置計測結果を用いるため、マーカ１８の測定結果のΔＬ_１−０（ｉ）は、下記数式（１）のように演算する。ただし、マーカ１９を先に計測してもよい。

そして、このΔＬ_１−０（ｉ）のｎ回の相加平均値をΔＬ_１−０とすると、この計測の標準偏差σ_ΔＬ１は以下の数式（２）を用いて表せる。

また、ここでは、後述するステップＳ７で使用する所定値を決定するために、Δｚの標準偏差も求める。Δｚとはマーカ１８とマーカ１９との奥行き方向成分（奥行き方向の距離）の位置計測結果の差分である。Δｚのｎ回の測定結果の相加平均値Δｚ_０は、Δｚ_０＝ｚ_２−０−ｚ_１−０と求めることができる。ｉ回目の測定結果Δｚ_０（ｉ）をΔｚ_０（ｉ）＝ｚ_２−０（ｉ）−ｚ_１−０（ｉ）とすると、標準偏差σ_Δｚは以下の数式（３）を用いて算出することができる。

次に、ステップＳ１−８では、エピポーラずれ結果の平均値と標準偏差を算出する。即ち、ステップＳ１−４で求めた黒丸中心の画像座標（ｕｌ，ｖｌ）、（ｕｒ，ｖｒ）に着目すると、ステップＳ１−３のステレオ平行化処理により生ずるエピポーラ線拘束条件によってキャリブレーションずれが発生していなければ理論的にはｖｌ＝ｖｒとなる。しかしながら、実際にはキャリブレーション直後でも、ｖｌとｖｒは近い値とはなるが、一致した値にはならない。なぜならば、キャリブレーション結果は最適化計算によって求めるため、各パラメータには必ず誤差が存在するからである。

本実施の形態では、エピポーラ線からのずれΔＥを、ΔＥ＝ｖｌ−ｖｒと定義する。このエピポーラ線からのずれをｎ回計測した結果の相加平均値をΔＥ_０、ｉ回目の測定結果をΔＥ_０（ｉ）とする。このとき、エピポーラ線からのずれΔＥの計測結果の標準偏差をσ_ΔＥとすると、σ_ΔＥは以下の数式（４）を用いて算出することができる。

以上の各処理が終了すると、ステップＳ１−７，Ｓ１−８で求めた計測結果（ｘ_１−０，ｙ_１−０，ｚ_１−０）、（ｘ_２−０，ｙ_２−０，ｚ_２−０）、Δｚ_０、ΔＥ_０、σ_ΔＬ１、σ_Δｚとσ_ΔＥを初期値（真値）として記憶部２３に保存する（Ｓ１−９）。以上により図７に示すステップＳ１の初期値保存の処理が終了する。

続いて、図６のステップＳ２に進むと、まず、主制御装置２０がステレオカメラ５の診断を行うのに適切な状態にあるか判断する。診断を行える状態であると判断すれば、マーカ１８の位置を計測するステップＳ３に進み、診断を行える状態でないと判断した場合には、診断を行える状態になるまでステップＳ２を繰り返す（Ｓ２のＮＯ）。

ここで、上記の診断を行える状態とはなにかについて説明する。例えば、ピンセット８が開閉動作中に診断を実施した場合は、マーカが動いているため、誤診断をしてしまう。従って、診断可能な状態か否かの判断は、ピンセット８が閉じているか否かの判断を行う処理である。主制御装置２０内の制御部２４は、ロボットアーム２、ロボットハンド３、ツール４も制御しているため、これらがどのような状態にあるか把握することが可能である。そこで、ツール４のピンセット８の状態を監視することにより、診断可能であるか否かを判断する。なお、診断可能な状態か否かの判断は、ワーク７の位置の計測時に同時に診断を行ってもよい。この場合、ワーク７がマーカを隠してしまう形状ではないことという制約があるが、計測と同時に診断できるため、システム全体としての信頼性を高めることができる。

上記のように診断可能であると判断すると（Ｓ２のＹＥＳ）、ステップＳ３に進み、ステレオカメラ５の診断の一環としてマーカ１８の位置を撮像（撮像工程）して計測する。この処理内容は、上述した図７のステップＳ１−１からＳ１−５と同様であるので説明を省略する。

そして、ステップＳ４では、マーカ１８の位置（位置計測値）に関するずれ量が所定量以下（所定量よりも大きい）か否かを判定し、キャリブレーションずれが発生したか否かを判定する。詳細には演算部２２で、ステップＳ３により演算したマーカ１８の位置計測結果（位置計測値）（ｘ_１−ｊ，ｙ_１−ｊ，ｚ_１−ｊ）と、ステップＳ１により記憶部２３に保存したマーカ１８の位置計測の初期結果（ｘ_１−０，ｙ_１−０，ｚ_１−０）とを比較する。そして、演算したマーカ１８の位置が初期値として保存した初期結果から所定量よりも大きくずれているか判断する。なお、本実施の形態では、ステップＳ３，Ｓ４においてマーカ１８の計測結果を用いたフローについて説明しているが、マーカ１９の計測結果を用いてもよい。

なお、ずれ量ΔＬ_１−ｊの定義式は以下の数式（５）とする。

ここで、ずれ量ΔＬ_１のキャリブレーションずれ判定の閾値としての所定量ΔＬ_ｔ１の設定方法の一例について説明する。この所定量ΔＬ_ｔ１はＳ１−７で求めた計測誤差σ_ΔＬ１よりも大きくする必要があるが、大きくしすぎると、キャリブレーションずれ判定の信頼性が低下してしまう。本実施の形態では所定量ΔＬ_ｔ１は標準偏差σ_ΔＬ１の５倍の値とする。ただし、この許容値は５倍に限定されることはなく、照明条件の安定性等の測定環境により異なるので、少なくとも２倍以上とすることが望ましい。

ステップＳ４で、マーカ１８のずれ量が所定量以下であると判定された場合は（Ｓ４のＹＥＳ）、つまりステレオカメラ５に関してキャリブレーションずれが発生していないと判断され、ステップＳ１２〜Ｓ１４の正常時の使用方法に移行する。

まず、ステップＳ１２では、ロボットアーム２やロボットハンド３等が、部品位置（ワーク７の位置）を計測する状態になるまで待機する（Ｓ１２のＮＯ）。主制御装置２０が部品位置を計測する状態になったと判断した場合には（Ｓ１２のＹＥＳ）、ステップＳ１３に進み、部品の位置姿勢を計測する処理を実施する。なお、部品位置を計測する状態か否かの判断は、ステレオカメラ５がワーク７を計測する際に、ステレオカメラ５が所定の位置にいるか否かを判断する処理である。本実施の形態では、ロボットアーム２、ロボットハンド３、ピンセット８の位置姿勢から、主制御装置２０内の制御部２４が判断する。

次にステップＳ１３では、ステレオカメラ５を用いてワーク７を撮影し、その撮影画像に対して任意の画像処理を施すことによって、ワーク（部品）７の位置姿勢を判断する。このワーク７の三次元位置及び姿勢の計測手法は、既知の手法であるので、その説明を省略する。

そして、ステップＳ１４では、ステップＳ１３のワーク７の位置計測が規定回数実行されたか否かを判断する。即ち、ロボットハンド３がツール４を把持してからツール４を置き台６に置くまでに、複数のワーク７の位置計測を計測する場合など、複数回の位置計測を実施する場合がある。規定回数に達するまでは（Ｓ１４のＮＯ）、上記ステップＳ１２，Ｓ１３を繰り返す。そして、規定回数に達した場合には（Ｓ１４のＹＥＳ）、例えばすべてのワーク７の計測が完了したとして、ステップＳ２に戻る。なお、この規定回数は、あらかじめ主制御装置２０内の記憶部２３に保存されている。

一方、上記ステップＳ４において、マーカ１８の位置が所定量よりも大きくずれていと判定した場合には（Ｓ４のＮＯ）、つまりキャリブレーションずれが発生したと判定する。キャリブレーションずれが発生した場合は、まずステップＳ５において、ずれ発生要因個所を判別するためにマーカ１９を撮像（撮像工程）して位置計測処理を実行する。なお、本実施の形態ではステップＳ５でマーカ１９の位置計測を行うが、ステップＳ３，Ｓ４でマーカ１９の位置計測結果を用いた場合は、ステップＳ５でマーカ１８の位置計測を行うことになる。また、位置計測方法に関しては、ステップＳ３と同様であり、ステップＳ１−１からＳ１−５と同じであるので、その説明を省略する。

次にステップＳ６では、演算部２２において、マーカ１８とマーカ１９との位置計測結果のｚ成分の差分Δｚ_ｊを算出する（算出工程）。ｚ成分は図４に示すように、ステレオカメラ５の奥行き方向成分である。即ち、ステップＳ３で求めたマーカ１８の位置計測結果（ｘ_１−ｊ，ｙ_１−ｊ，ｚ_１−ｊ）と、ステップＳ５で計測したマーカ１９の位置計測結果（ｘ_２−ｊ，ｙ_２−ｊ，ｚ_２−ｊ）を用いて、Δｚ_ｊ＝ｚ_２−ｊ−ｚ_１−ｊを算出する。

ステップＳ７は、キャリブレーションずれの発生要因箇所を特定するための処理の一環である。このステップＳ７では、上記算出したマーカ１８とマーカ１９とのｚ成分の差分Δｚ_ｊ（即ち奥行き方向の距離）が、ステップＳ１で保存した初期値の距離に対して所定値以下のずれであるか否かを判定する（判定工程）。

ここで、マーカ１８、１９間の距離の計測結果Δｚ_ｊが、主制御装置２０内の記憶部２３にあらかじめ保存しておいた計測距離の初期結果Δｚ_０から所定値よりも大きくずれていたとする（Ｓ７のＮＯ）。この場合には、ステレオカメラ５内部の各カメラ３０，４０の相対位置関係が基線方向に（つまり基線長が）ずれたと判断し、ステップＳ１１に進み、システムの復帰方法２である、内部キャリブレーションを行った後、外部キャリブレーションを行う。

上記ステップＳ７で用いる所定値Δｚ_ｔは、ステップＳ１−７で求めた計測誤差σ_Δｚよりも大きくする必要があるが、大きくしすぎると、診断の信頼性が低下してしまう。そこで、本実施の形態では所定値Δｚ_ｔは標準偏差σ_Δｚの５倍の値とする。ただし、この許容値は５倍に限定されることはなく、照明条件の安定性等の測定環境により異なるので、少なくとも２倍以上とすることが望ましい。

ここで、本ステップＳ７の処理によってなぜ、カメラ３０，４０の相対位置が基線方向にずれたのかを判別可能であるかについて説明する。説明の簡単化のため、平行ステレオカメラを例にとって説明する。平行ステレオ時、奥行き方向ｚ成分の測定結果は以下の数式（６）を用いて算出することができる。この時、ｂ：基線長（基準長）、ｆ：焦点距離、ｄ：視差である。

カメラ３０，４０の相対位置の基線方向のずれがΔｂだけ発生し、その時のマーカ１８計測時の視差がｄ１、マーカ１９計測時の視差をｄ２とすると、本来計測されるべき、マーカ１８とマーカ１９間のｚ成分の差分は以下の数式（７）となる。

しかしながら、実際の計測時は、ステレオカメラ５のキャリブレーション結果である基線長ｂはキャリブレーションずれが発生する前の値を使用するため、計測結果は以下の数式（８）のようになる。

このように、真値（本実施の形態ではステップＳ１で求めた初期値を用いる）と計測結果の間に誤差が発生する。カメラ３０，４０の相対位置に基線方向のずれが生じていない場合、つまりはΔｂ＝０の場合、真値と計測結果にはこのような誤差は生じない。また、単一のマーカ１８のｚ成分の計測結果だけで判断しようとした場合、ステレオカメラ５の位置が変化した場合にもｚ成分の計測結果は変化するので、基線方向のずれが発生していないのに発生したと誤診断してしまうことになる。以上のように、マーカ１８、１９間の距離を算出することにより、カメラ３０，４０の相対位置が基線方向にずれたか診断可能である。

一方、ステップＳ７で算出したマーカ１８，１９間の距離が所定値以下であった場合は（Ｓ７のＹＥＳ）、つまりカメラ３０，４０が基線方向にずれていないので、ステップＳ８に進み、各カメラ３０，４０の相対位置関係が垂直方向にずれていないか判断する。

ステップＳ８では、マーカ１８のエピポーラ線からのずれを計測する。このステップＳ８は、マーカ１８のエピポーラ線からのずれの計測方法であるが、これはステップＳ１−８で説明した方法と同様であるので省略する。なお、本実施の形態では、マーカ１８のエピポーラ線からのずれを計測するが、マーカ１９のエピポーラ線からのずれを計測してもよいし、マーカ１８，１９両方のエピポーラ線からのずれを計測してもよい。マーカ１８，１９両方のエピポーラ線からのずれを計測する場合、計測点が増加するので診断の信頼性は向上する。また、ここで求まったエピポーラ線からのずれ量をΔＥ_ｊと定義する。

続いてステップＳ９では、演算部２２において、カメラ３０，４０の相対位置が垂直方向にずれたことにより、キャリブレーションずれが発生したのか判別を行う。このステップＳ９の処理は、ステップＳ８で求まったエピポーラ線からのずれ量ΔＥ_ｊが許容値以下であるか判別する処理である。この許容値ΔＥ_ｔはステップＳ１で求めた計測誤差σ_ΔＥよりも大きくする必要があるが、大きくしすぎると、診断の信頼性が低下してしまう。そこで、本実施の形態では許容値ΔＥ_ｔは標準偏差σ_ΔＥの５倍の値とする。ただし、この許容値は５倍に限定されることはなく、照明条件の安定性等の測定環境により異なるので、少なくとも２倍以上とすることが望ましい。

ステップＳ９において、マーカ１８のエピポーラ線からのずれ量が、主制御装置２０内の記憶部２３に保存してあるエピポーラ線からのずれ量の初期値ΔＥ_０から許容値よりも大きくずれていたとする（Ｓ９のＮＯ）。この場合には、各カメラ３０，４０の相対位置関係が垂直方向にずれたと判断し、ステップＳ１１に進み、システムの復帰方法２である、内部キャリブレーションを行った後、外部キャリブレーションを行う。

一方、ステップＳ９において、マーカ１８のエピポーラ線からのずれ量が初期値ΔＥ_０から許容値以下である場合には（Ｓ９のＹＥＳ）、つまりステレオカメラ５内部にずれは発生していないと判断する。しかしながら、上記ステップＳ４においてキャリブレーションずれの発生が判定されているので、ステレオカメラ５とツール４等との外部装置間の位置姿勢にずれが発生したと判断する。この場合には、内部キャリブレーションが不要であるので、ステップＳ１０に進み、システムの復帰方法１である、外部キャリブレーションだけを行う。

ついで、ステップＳ１０（復帰方法１）について説明する。本ステップＳ１０にたどり着くということは、ステレオカメラ５内部にずれは発生していないが、ツール４等の外部装置とステレオカメラ５との間の位置関係にずれが発生したことによって、キャリブレーションずれが発生したということである。よって本復帰処理は、ステレオカメラ５自身の内部キャリブレーションは実施しないが、ステレオカメラ５とツール４（ピンセット８）等の外部装置との間の外部キャリブレーションを実施する。

詳細には、復帰方法１としては、主制御装置２０内の表示部２５にキャリブレーションずれが発生したことと、復帰するためにステレオカメラ５と外部装置との間の外部キャリブレーションの実施を促すことを表示し、積層回転灯１０の赤色の回転灯を点灯する。また、外部キャリブレーションずれが発生している環境であるので、主制御装置２０はロボットアーム２，ロボットハンド３，ツール４に対して、駆動を停止するよう指令を送信し、生産動作を停止する。

次に、ねじ１２，１７等が緩んでいる可能性があるため（図４参照）、これらのねじを締めつけなおす。その後、ステレオカメラ５と外部装置との間の外部キャリブレーションを実施する。外部キャリブレーションが完了したら、ステップＳ１に戻ることによりシステム復帰可能となる。

続いて、ステップＳ１１（復帰方法２）について説明する。本ステップＳ１１にたどり着くということは、ステレオカメラ５内部にずれが発生しているために、内部キャリブレーションずれが発生しているということである。よって、ステレオカメラ５本体を再度内部キャリブレーションする必要がある。また、ステレオカメラ５内部のキャリブレーション結果であるパラメータが変化することにより、ステレオカメラ５とツール４（ピンセット８）等の外部装置との間の外部キャリブレーションも再度実施する必要がある。本復帰方法２は、この外部キャリブレーションと内部キャリブレーションとの２つのキャリブレーションを実施する処理である。

詳細には、復帰方法２としては、主制御装置２０内の表示部２５にキャリブレーションずれが発生したことと、復帰するためにステレオカメラ５の内部キャリブレーションの実施を促すことを表示し、積層回転灯１０の黄色の回転灯を点灯する。また、内部キャリブレーションずれが発生している環境であるので、主制御装置２０はロボットアーム２，ロボットハンド３，ツール４に対して、駆動を停止するよう指令を送信し、生産動作を停止する。

次に、ステレオカメラ５自身を再度内部キャリブレーションするために、ねじ１２を取り外し（図４参照）、ステレオカメラ５をツール４から取り外す。そして、ステレオカメラ５内部の各ねじ（図５参照）が緩んでいる可能性があるため、これらのねじを締めつけなおしてから、専用装置を用いてステレオカメラ５の内部キャリブレーションを実施する。その後、ステレオカメラ５をツール４に組み付けてから、ステレオカメラ５とツール４（ピンセット８）等の外部装置との間の外部キャリブレーションを実施する。このように内部キャリブレーションの実施の後、外部キャリブレーションを実施し、これらキャリブレーションが完了したら、ステップＳ１に戻ることによりシステム復帰可能となる。

以上説明したように本三次元位置計測装置１によると、奥行き方向の距離の真値が既知であるマーカ１８，１９の間の距離を算出する（Ｓ３，Ｓ５，Ｓ６）。そして、算出した距離が真値と所定値以上ずれた場合に（Ｓ７のＮＯ）、カメラ３０，４０との基線長が変化したステレオカメラ内部のキャリブレーションずれと判定できる。即ち、画像上のマーカ１８，１９がエピポーラ線から所定量以内にあっても、ステレオカメラ外部の設置位置ずれによるキャリブレーションずれと誤判定してしまうことを防止できる。従って、キャリブレーションずれの発生箇所を正確に判定することが可能となる。

また、本三次元位置計測装置１は、マーカ１８，１９のうちの少なくとも１つのマーカの画像が、エピポーラ線から許容値よりも大きくずれたとする（Ｓ９のＮＯ）。この場合は、ステレオカメラ５の内部におけるカメラ３０，４０との相対位置がずれたステレオカメラ内部のキャリブレーションずれと判定する。これにより、カメラ３０，４０との基線長が変化したステレオカメラ内部のキャリブレーションずれだけでなく、ステレオカメラ内部のキャリブレーションずれの発生を確実に判定することが可能となる。

さらに、本三次元位置計測装置１は、マーカ１８，１９の少なくとも一方のマーカに関する値のずれ量が所定量よりも大きい場合にキャリブレーションずれの発生を判定する（Ｓ４のＮＯ）。そして、ステレオカメラ５内部のキャリブレーションずれと判定されなかった場合に（Ｓ７のＹＥＳ、Ｓ９のＹＥＳ）、ステレオカメラ５の設置位置がずれたステレオカメラ５外部のキャリブレーションずれと判定する。これにより、ステレオカメラ５内部のキャリブレーションずれだけでなく、ステレオカメラ外部のキャリブレーションずれの発生を確実に判定することが可能となる。

なお、本実施の形態では、キャリブレーションずれが発生したか否かを判定し（Ｓ４）、その後、それがステレオカメラ５内部のキャリブレーションずれか否かを判定する（Ｓ７，Ｓ９）。これにより、ステレオカメラ５内部のキャリブレーションか、ステレオカメラ５外部のキャリブレーションかを判定している。しかしながら、ステレオカメラ５内部のキャリブレーションずれが発生したか否かを判定した後（Ｓ７，Ｓ９）、キャリブレーションずれが発生したか否かを判定し、ステレオカメラ５外部のキャリブレーションの発生を判定してもよい。つまりステップＳ４を、ステップＳ９の後になるように処理の順序を入れ替えてもよい。

また、本実施の形態では、ステレオカメラ５内部のキャリブレーションずれが発生した場合に、復帰方法２を行うだけのものを説明した。しかしながら、ステレオカメラ５内部でカメラ３０，４０との基線長が変化したキャリブレーションずれであるか否かを判定できるので、内部キャリブレーションの処理（復帰）箇所を特定して、内部キャリブレーションを実施するようにしてもよい。その場合は、ステップＳ７のＮＯの場合と、ステップＳ９のＮＯの場合とで、復帰方法を分けることになる。

また、本実施の形態では、キャリブレーションずれの判定を行う各種プログラムが記憶部２３に記憶されているものとして説明したが、これに限らず、他の記録媒体（例えば光学ディスク、フラッシュメモリ、ハードディスク）であってもよい。

また、本実施の形態では、第１カメラと第２カメラとの２つのカメラ３０，４０を備えたものを説明したが、３つ以上のカメラを備えていてもよい。さらに、２つのマーカ１８，１９を撮像するものについて説明したが、マーカも３つ以上あってもよく、それらマーカのうちの少なくとも２つについて奥行き方向の距離が異なればよい。また、２つ以上のマーカは、撮像範囲内となるようにツール４（ピンセット８）に配置されていることが好ましいが、例えば撮像する対称が違うものであれば、これに限るものではない。つまり本発明は、ステレオカメラを有して対象物を三次元計測するものであれば、種々のものに応用が可能である。

また、本実施の形態では、マーカ１８，１９の位置や距離をキャリブレーションしたステレオカメラ５で複数回撮像して演算することで初期値を演算し、それをマーカ間の距離の真値とした。しかしながら、例えばマーカ１８，１９の距離を直接的（物理的）に測り、その値を真値として記憶部２３に記憶させておいてもよい。

１…三次元位置計測装置：２…多関節ロボットアーム：４…ツール：５…ステレオカメラ：７…対象物（ワーク）：１８，１９…マーカ：２０…制御部（主制御装置）：２３…記録媒体（記憶部）：３０…第１カメラ（左カメラ）：４０…第２カメラ（右カメラ）：６０Ｌ，６０Ｒ…画像：１００…ロボット装置：ｂ…基線長：ΔＥ_ｔ…許容値：ΔＬ_１…ずれ量：ΔＬ_ｔ１…所定量：Δｚ_ｊ…距離：Δｚ_ｔ…所定値

Claims (7)

1. 第１カメラ及び第２カメラを有するステレオカメラにより撮像した画像を用いて対象物の位置を三次元計測する三次元位置計測装置において、
   前記ステレオカメラから奥行き方向の距離が異なり、かつ前記距離の初期値が記憶されている複数のマーカ、の少なくとも一方のマーカに関する位置計測値のずれ量が所定量よりも大きい場合にキャリブレーションずれの発生を判定する制御部を備え、
   前記制御部は、
   前記キャリブレーションずれの発生を判定した場合において、前記ステレオカメラの内部における前記第１カメラと前記第２カメラとの相対位置がずれたステレオカメラ内部のキャリブレーションずれを判定し、前記ステレオカメラ内部のキャリブレーションずれと判定されなかった場合に、前記ステレオカメラの設置位置がずれたステレオカメラ外部のキャリブレーションずれと判定し、
   前記ステレオカメラ内部のキャリブレーションずれの１つとして、前記複数のマーカを前記第１カメラ及び前記第２カメラにより撮像させ、前記複数のマーカの間の距離を算出し、算出した距離が前記初期値に対して所定値よりも大きくずれた場合に、前記第１カメラと前記第２カメラとの基線長が変化した、前記ステレオカメラ内部のキャリブレーションずれと判定する制御部を備えた、
   ことを特徴とする三次元位置計測装置。
2. 前記制御部は、前記ステレオカメラ内部のキャリブレーションずれの１つとして、前記複数のマーカのうちの少なくとも１つのマーカの画像が、エピポーラ線から許容値よりも大きくずれた場合に、前記ステレオカメラ内部のキャリブレーションずれと判定する、
   ことを特徴とする請求項１に記載の三次元位置計測装置。
3. 請求項１または２に記載の三次元位置計測装置と、
   前記対象物に対する作業が可能なツールが先端に着脱自在で、前記三次元位置計測装置の三次元位置計測に基づき制御される多関節ロボットアームと、を備えた、
   ことを特徴とするロボット装置。
4. 請求項３に記載のロボット装置に着脱自在なツールであって、
   一部が撮像範囲内となるように前記ステレオカメラが固定され、かつ前記一部に前記複数のマーカが設けられている、
   ことを特徴とするツール。
5. 第１カメラ及び第２カメラを有するステレオカメラにより撮像した画像を用いて対象物の位置を三次元計測する三次元位置計測装置におけるキャリブレーションずれを判定する三次元位置計測装置のキャリブレーションずれ判定方法において、
   制御部が、前記ステレオカメラから奥行き方向の距離が異なり、かつ前記距離の初期値が記憶されている複数のマーカを、前記第１カメラ及び前記第２カメラにより撮像する撮像工程と、
   前記制御部が、前記複数のマーカの少なくとも一方のマーカに関する位置計測値のずれ量が所定量よりも大きい場合にキャリブレーションずれの発生を判定する工程と、
   前記制御部が、前記キャリブレーションずれの発生を判定した場合において、前記ステレオカメラの内部における前記第１カメラと前記第２カメラとの相対位置がずれたステレオカメラ内部のキャリブレーションずれを判定し、前記ステレオカメラ内部のキャリブレーションずれと判定されなかった場合に、前記ステレオカメラの設置位置がずれたステレオカメラ外部のキャリブレーションずれと判定する工程と、
   前記制御部が、前記複数のマーカの間の距離を算出する算出工程と、
   前記制御部が、前記ステレオカメラ内部のキャリブレーションずれの１つとして、算出した距離が前記初期値に対して所定値よりも大きくずれた場合に、前記第１カメラと前記第２カメラとの基線長が変化したステレオカメラ内部のキャリブレーションずれと判定する工程と、を備えた、
   ことを特徴とする三次元位置計測装置のキャリブレーションずれ判定方法。
6. 請求項５に記載の各工程をコンピュータに実行させるための三次元位置計測装置のキャリブレーションずれ判定プログラム。
7. 請求項６に記載の三次元位置計測装置のキャリブレーションずれ判定プログラムを記録した、コンピュータで読み取り可能な記録媒体。

Images