JP5365218B2 - ロボットビジョンシステムおよび自動キャリブレーション方法 - Google Patents

Description

本発明は、ロボットビジョンシステムにおける自動キャリブレーション技術に関する。

ステレオカメラを用いることによってロボット等の自動機械に視覚を持たせ、作業対象の三次元位置や姿勢を検出・識別し、自動機械に動作を行なわせるシステムとしてロボットビジョンシステムが知られている。

ロボットビジョンシステムにおいては、ステレオカメラに用いられるカメラのパラメータ、ステレオカメラシステムのパラメータ、およびステレオカメラの座標系と自動機械の座標系との間で座標系を変換するためのパラメータを求める必要があり、パラメータを求める作業が複数回必要である。

ステレオカメラシステムのパラメータを求める基礎行列算出作業では、複数のカメラの相対関係を求める。そのため、複数のカメラの画像内に収めるように基礎行列算出用冶具（一般的にはチェッカーボードや等間隔に円が描かれているプレート）を設置する。そして、画像内に映っている冶具上の点を画像同士対応させることにより、相対関係パラメータを数学的に算出する方法が一般的に採用されている（例えば、非特許文献１参照。）。

この場合、基礎行列算出用冶具を複数のカメラで撮影し、撮影した複数の基礎行列算出用冶具の画像を重畳させた１つの画像内で冶具上のマーカー位置を累積することになるが、その際、それらマーカー位置が三次元的に広範囲に分布することが望ましいため、カメラの画像を確認しながら基礎行列算出用冶具、例えばチェッカーボードを移動させるという煩雑な作業が必要となる。

一方、ステレオカメラの座標系と、自動機械の座標系との間の変換のパラメータを求める相対関係算出作業では、ステレオカメラの座標系と自動機械の座標系との間の剛体変換行列を求める。そのために、自動機械の座標系の座標と、ステレオカメラによって求められる三次元座標とが両方とも既知である点を複数求め、２つの座標の間の剛体変換行列を数学的に解く方法が一般的に採用されている。

ステレオカメラなどに代表される画像処理座標系における座標を高精度に計測するために、特許文献１や２に示される特徴的な冶具を用いる方法や、特許文献３に示される自動機械のアームを指し棒が長方形を描くように動作させる方法などが提案されている。しかし、これらの手法では、特殊な冶具を必要とし、かつ、自動機械の動作位置の確認（画面内に冶具が入っているかどうか）などの基礎行列算出作業の準備に煩雑な作業を必要とするという問題がある。また、基礎行列算出と剛体変換行列算出とが分離して独立に行われるため手間がかかるという問題がある。

すなわち、従来は、ステレオカメラを用いたロボットビジョンシステムを構築する場合、ステレオカメラの基礎行列算出作業と、ステレオカメラ座標系と自動機械（ロボット）座標系の相対関係を表す剛体変換行列算出作業との、最低２回のパラメータ算出作業（キャリブレーション作業）が必要である。

このような作業は、通常、ロボットビジョンシステムを設置する際に一度実行すればよい作業であるが、他作業者との衝突、地震による機材のズレなどにより、２台のカメラの相対的な位置関係が変化したり、カメラとロボットの位置関係が変化したりした場合は、再度実行しなければならない作業であり、手間をかけずに短時間で仕上げることが望まれている。

特開平１０−３４０１１２号公報 特開平１１−３３９６２号公報 特開平９−１２８５４９号公報

富田文明，高橋裕信「ステレオカメラのセルフキャリブレーション」、情報処理学会誌３１巻５号、１９９０年、Ｐ６５０−Ｐ６５９ David A. Forsyth, Jean Ponce 著、大北 剛 訳「コンピュータビジョン」、共立出版、２００７年、Ｐ２４６−Ｐ２４８ K. S. Arun, T. S. Huang, and S. D. Blostein,"Least-squares Fitting of Two 3-d Point Sets", IEEE Trans. on Pattern Analysis and Machine Intelligence,Vol. 9, No. 5, pp. 698-701 (1987)

本発明は、以上の問題点を考慮してなされたものであり、キャリブレーション作業を自動で手間をかけることなく行なうことを可能としたロボットビジョンシステムおよび自動キャリブレーション方法を提供することを目的とする。

上記課題を解決するために、請求項１に係るロボットビジョンシステムによれば、第１および第２のカメラにより構成されるステレオカメラと、アームを有するロボットと、前記ステレオカメラおよび前記ロボットを駆動し制御する制御装置と、を備え、前記ロボットは、可動部にステレオカメラの基礎行列算出に用いる複数のマーカーを備え、前記制御装置は、前記第１のカメラおよび第２のカメラにより撮像された前記マーカーの画像を用いて前記ステレオカメラの基礎行列を算出する基礎行列算出部と、前記基礎行列と前記第１のカメラおよび第２のカメラにより撮像された画像とを用いて、前記各マーカーのステレオカメラ座標系における三次元位置を算出する三次元復元部と、ロボット座標系における前記各マーカーの三次元位置を算出するマーカー位置算出部と、前記マーカー毎に、前記マーカー位置算出部で算出されたロボット座標系における三次元位置と、前記三次元復元部で算出されたステレオカメラ座標系における三次元位置と、を対応付け情報として記憶する対応付け情報記憶部と、該対応付け情報に基づいて、前記マーカー毎に、ロボット座標系とステレオカメラ座標系との剛体変換行列を算出する剛体変換行列演算部と、を有することを特徴とする。

また、請求項２に係るロボットビジョンシステムによれば、請求項１に記載のロボットビジョンシステムにおいて、前記制御装置は、前記各マーカーが前記ステレオカメラにより撮影された複数の画像を重畳させた画像に基づき、前記ロボットのアームの移動方向を決定する移動方向決定部を有し、前記決定された移動方向に前記アームを移動させ、前記各マーカーを撮影することを特徴とする。

また、請求項３に係るロボットビジョンシステムによれば、請求項１または２に記載のロボットビジョンシステムにおいて、前記ロボットは、前記複数のマーカーを有する基礎行列算出用冶具を前記ロボットのアーム先端部に固定する機構を備えることを特徴とする。

また、請求項４に係るロボットビジョンシステムによれば、請求項３に記載のロボットビジョンシステムにおいて、前記制御装置は、前記基礎行列算出用冶具の前記アーム先端位置および姿勢に対する前記各マーカーの相対位置を記憶したマーカー相対位置記憶部と、ロボット座標系におけるアーム先端部位置を順運動学により算出する前記アーム先端位置算出部と、を有し、前記マーカー位置算出部は、前記アーム先端位置算出部にて算出された前記アーム先端位置および姿勢、前記マーカー相対位置記憶部に記憶された前記各マーカーの相対位置を用いて、ロボット座標系における前記各マーカーの三次元位置を算出することを特徴とする。

また、請求項５に係る自動キャリブレーション方法によれば、第１および第２のカメラにより構成されるステレオカメラと、アームを有するロボットと、前記ステレオカメラおよび前記ロボットを駆動し制御する制御装置と、ロボットの可動部にステレオカメラの基礎行列算出に用いる複数のマーカーと、を有するロボットビジョンシステムが実行する自動キャリブレーション方法において、前記第１のカメラおよび第２のカメラにより撮像された前記マーカーの画像を用いて前記ステレオカメラの基礎行列を算出する基礎行列算出ステップと、前記基礎行列と前記第１のカメラおよび第２のカメラにより撮像された画像とを用いて、前記各マーカーのステレオカメラ座標系における三次元位置を算出する三次元復元ステップと、ロボット座標系における前記各マーカーの三次元位置を算出するマーカー位置算出ステップと、前記マーカー毎に、前記マーカー位置算出ステップで算出されたロボット座標系における三次元位置と、前記三次元復元ステップで算出されたステレオカメラ座標系における三次元位置と、を対応付け情報として記憶する対応付け情報記憶ステップと、該対応付け情報に基づいて、前記マーカー毎に、ロボット座標系とステレオカメラ座標系との剛体変換行列を算出する剛体変換行列演算ステップと、を有することを特徴とする。

提案するロボットビジョンシステムによれば、ロボットの可動部にステレオカメラの基礎行列算出作業を行なうための複数のマーカーを設置することにより、基礎行列算出作業を行うための複数のマーカーの三次元位置がロボット座標系上で既知となり、そのロボット座標系のマーカーの三次元位置を、ステレオカメラ座標系のマーカーの三次元位置とも対応付けることにより、ステレオカメラの基礎行列算出作業と、ステレオカメラ座標系とロボット座標系の相対関係を示す剛体変換行列算出作業とを同時に実行することが可能となり、キャリブレーション作業の手間を軽減することが可能となる。

また、ステレオカメラの基礎行列算出作業を行なうための複数のマーカーをロボットビジョンシステムのロボットの可動部に設置し、ステレオカメラを構成する各カメラの画像認識とロボットのアーム制御とを同一の制御装置により制御することにより、基礎行列算出作業を行なうための複数のマーカーを２台のカメラの画像内に収まる位置に移動させることにより、カメラ画像を確認しながら手作業で基礎行列算出作業を行なうための複数のマーカーを移動させる作業が不要となる。

また、提案するロボットビジョンシステムによれば、ステレオカメラの基礎行列算出作業と、ステレオカメラ座標系とロボット座標系の相対関係を示す剛体変換行列算出作業とを一度の作業で行なうため、ステレオカメラの基礎行列算出作業とステレオカメラ座標系とロボット座標系の相対関係算出作業とでそれぞれに専用の冶具を用意する必要がなくなる。そして、ステレオカメラの基礎行列算出作業を行った画像とマーカーのロボット座標とから、ステレオカメラ座標系とロボット座標系の相対関係算出を行っているので、測定誤差を減らすことができ、剛体変換行列を高精度で算出することができる。

本発明の一実施形態に係るロボットビジョンシステムの構成を示す図である。 機械的位置の算出方法の概略を示す図である。 カメラの画像認識によるチェッカーボードの移動例を示す図である。 過去に撮影した認識点を参照した、認識点の分布を均一にするためのチェッカーボードの移動例を示す図である。 チェッカーボード交点の検出処理（その１）を示す図である。 チェッカーボード交点の検出処理（その２）を示す図である。 チェッカーボード交点の検出処理（その３）を示す図である。 自動キャリブレーションの実施方法を示す図（その１）である。 自動キャリブレーションの実施方法を示す図（その２）である。 ステレオカメラ座標系とロボット座標系の剛体変換行列の算出方法を示す図である。 本実施形態のロボットビジョンシステムの機能ブロック図である。 自動キャリブレーション処理のフローチャートである。

以下図面に基づいて、本発明の実施形態について詳細を説明する。
図１は、本発明の一実施形態に係るロボットビジョンシステムの構成を示す図である。
このロボットビジョンシステムは、図には示されていないが、２台のカメラにより構成されるステレオカメラ１と、ステレオカメラ１により取得した画像を画像処理して三次元計測・認識を行なうパーソナルコンピュータ（以下、“ＰＣ”という）２と、先端部に基礎行列算出用冶具としてのチェッカーボード３が取り付けられたアームを有するロボット４と、ＰＣ２からＬＡＮ（Local Area Network）等の通信手段を通して受信した目標座標に基づいてロボット４を制御するロボットコントローラ５と、により構成される。なお、図１には示されていないが、始めに、ロボット４のアームの先端のハンドに設けられた穴部に、チェッカーボード３の裏面に設けられた凸部を挿通させて固定具（不図示）によりチェッカーボード３を固定している。

なお、キャリブレーションが完了した後は、チェッカーボード３をハンドから取り外し、ロボット４のアームの先端のハンドによりワーク（作業対象）を把持し作業を進めることになる。

上記構成において、キャリブレーション作業を自動で行なう前に事前作業として、チェッカーボード３とロボット４との機械的位置を正確に求めるための作業を行う必要がある。その機械的位置の算出方法の概略を、図２を参照して説明する。

この作業では、図２の固定位置に設置された指示ピン８のロボット座標系の座標と、アーム先端の位置・姿勢とから、アーム先端位置７を原点とするアーム先端座標系におけるチェッカーボード３上の交点座標（これは、チェッカーボード３上の各マス目の頂点位置である。）を推定する。なお、この作業では、カメラによる撮影は行わない。

この作業では、アームを動かし、チェッカーボード３上の交点と指示ピンとを合わせ、ロボット座標系における指示ピン８の座標（Ｘｐ，Ｙｐ，Ｚｐ）とアーム先端の位置・姿勢とによって、アーム先端座標系におけるチェッカーボード３上の交点座標（ｘ，ｙ，ｚ）が後述する（式１）により算出される。

そして、アーム先端座標系におけるチェッカーボード３上の交点座標が求まることにより、アーム先端座標系におけるアーム先端からチェッカーボード３の各交点に伸びるベクトルが求まるために、アームがどのような姿勢であっても、そのときのロボット座標系におけるアーム先端座標を順運動学により算出すれば、ロボット座標系におけるチェッカーボード３上の各交点座標も求まることになる。

この過程を以下に詳細に説明する。
始めに、アームの根元位置６を原点とするロボット座標系において、機構的に既知な位置に指示ピン８を設置する。

アーム先端７あるいはその近傍にチェッカーボード３を取り付け、チェッカーボード３上のいずれかの交点と指示ピン８の先端Ｐが同一点となるようにアームを動かし、その同一点となったときのアーム先端座標（Ｘｒ，Ｙｒ，Ｚｒ）、ロボット座標系の各軸とアーム先端座標系の各軸とのなす角度（Ｒｘ，Ｒｙ，Ｒｚ）を順運動学により算出する。

そして、そのアーム先端座標（Ｘｒ，Ｙｒ，Ｚｒ）、角度（Ｒｘ，Ｒｙ，Ｒｚ）、指示ピンの先端座標（Ｘｐ，Ｙｐ，Ｚｐ）により、下記（式１）を用いて、そのときのアーム先端座標（Ｘｒ，Ｙｒ，Ｚｒ）を原点とするアーム先端座標系における指示ピン８に一致する交点座標（ｘ，ｙ，ｚ）を求める。

この作業をチェッカーボード３上の各交点に対して行うことによって、チェッカーボード３上の全ての交点のアーム先端座標系における座標を求めることができる。なお、チェッカーボード３上の一直線上にない三点につき、アーム先端座標系における座標を求めておき、それを用いて、チェッカーボード３の姿勢を推定し、アーム先端座標系におけるチェッカーボード３上の全ての交点座標を推定することもできる。この場合、各交点間の距離や交点の数（縦横比）が必要となる。

このようにして求めたアーム先端座標系における各交点座標が、ＰＣ２のメモリ上にチェッカーボード３の各マーカーのアーム先端に対する相対的な位置関係を表すマーカー相対位置として、自動キャリブレーション作業を行う前に記憶される。自動キャリブレーション作業を行う際には、後述するように、アーム先端座標を求めれば、このメモリに保持されるマーカー相対位置を用いて、チェッカーボード３上のロボット座標系における各マーカーの三次元座標を算出することができる。

上述したように、キャリブレーションを行なう際、ステレオカメラの画像上において、チェッカーボードが画面内に収まっているかどうかをＰＣ２のソフトウェアのアルゴリズムにより確認することになるが、そのための移動処理について以下に説明する。

図３は、カメラの画像認識によるチェッカーボードの移動例を示す図である。
始めにチェッカーボードの交点数（縦数・横数）を設定しておく。次にカメラの画像に入らなかった点の座標値の平均値を座標値とする点を基点とし、画像の中心点により定まるベクトルを求める。

そして、求めたベクトルの方向をロボット（チェッカーボード）の移動方向として決定し、ロボットの移動を開始する。移動中に上記ＰＣ２のソフトウェアにより交点の認識を行い、すべての交点を認識した位置でロボットを停止させる。

次に認識点の分布をより均一化させるための処理を行なう。
図４は、過去に撮影した認識点を参照した、認識点の分布を均一にするためのチェッカーボードの移動例を示す図である。なお、白抜きの○が過去に撮影したチェッカーボードに認識点、灰色の○が現在撮影使用としているチェッカーボードの認識点を表している。

過去に撮影した複数回分（ここでは２回分）のチェッカーボードの認識点を重ねあわせて得られる画像に対し、現在撮影しようとしているチェッカーボードのカメラ画像をさらに重ねあわせて、その合成結果の画像を複数の領域に切り分ける。そして、過去に撮影したチェッカーボードの認識点が少ない領域９−１、９−２を割り出し、現在撮影しようとしているチェッカーボードがその認識点が少ない領域９−１、９−２を補うように位置を補正する。これにより、チェッカーボード上の交点をほぼ均一にカメラ画像上に分散させることができる。

この位置補正すなわち移動により、チェッカーボードの位置が適切な位置に設定されたことになるので、そのときのカメラ画像、ロボット座標をデータとしてメモリに保存しておく。理論上最低３枚の画像があれば基礎行列を算出可能である。しかし、利用に耐えうる基礎行列を算出するには、２０枚程度の画像が必要である。すなわち、上述の交点の分散処理を、撮影する２０回分程度のチェッカーボードの画像につき、逐次実行する必要がある。

次に、各カメラ画像上のチェッカーボード交点の検出処理を行なう。
このチェッカーボード交点の検出処理について続いて説明する。
始めに、図５に示すように、チェッカーボード３に対し、その上位置１０−１と下位置１０−２を判別する処理を行なう。図５に示す場合のように、チェッカーボード３のマスの数が上下方向（ここでは“８”マス）と左右方向（ここでは“９”マス）で異なる場合には、上位置１０−１と下位置１０−２の画像パターンは明らかに異なっているため、上位置１０−１と下位置１０−２の判別が可能である。

次に、図６に示すように、（１）で、チェッカーボード３上の交点を算出するために、チェッカーボード３上の矩形（黒色）を検出し、（２）で、検出した矩形（黒色）のそれぞれについて、他の矩形（黒色）と隣接する数（交点を成す数）を計測し、その交点数が“１”である矩形（黒色）１１−１および１１−２を求める。チェッカーボードのマスの数が上下方向と左右方向で異なる場合には、上位置１０−１と下位置１０−２の画像パターンは明らかに異なっているために、上位置１０−１と下位置１０−２のいずれか一方の左右端部の２つの矩形（黒色）が上記（２）で検出されることになる。

続く（３）では、開始点を決定する処理を行なう。ここでは、検出した矩形（黒色）１１−１および１１−２がそれぞれ他の矩形（黒色）と成す交点１２−１、１２−２のいずれかを開始点として決定する。決定方法としては、開始点候補が左上の位置にくるようにチェッカーボードを回転し、その開始点候補がある行の交点数が設定されたコーナー数（これは、チェッカーボード３の行数あるいは列数のいずれかを示している）に合致するかどうかを判定することで行っている。

図６の（４）では、交点１２−１が開始点候補としてチェッカーボード３の左上の位置にある。また、図６の（５）では、交点１２−２が開始点候補としてチェッカーボード３の左上の位置にある。

交点１２−１、１２−２のうち、設定されたコーナー数に合致した方が図７の（６）に示すように開始点１３として特定される（これは、コーナー数＝“８”の場合の処理の例であり、交点１２−１が開始点として特定されている）。

そして、図７の（７）に示すように、同一の行では左から右に昇順に番号を割り振るようにして、チェッカーボード３上の左上の位置にある開始点１３から右下の位置にある点まで、昇順に、１、２、３、・・・と番号が割り振られる。これにより、チェッカーボード３上の交点が一定の順序で検出できる。

その後、チェッカーボード３の同一点を右カメラ（不図示）から取り込まれた右画像と左カメラ（不図示）から取り込まれた左画像において対応させ、その対応付けを複数ペア行なうことで、左右両カメラの対応点を求める。

左カメラの画像ｉ（ｉ番目の画像）上の（上述のようにして番号付けを行った）マーカー番号ｊについてマーカーの２次元座標（ＸＬ_ｉｊ，ＹＬ_ｉｊ）が画像解析の処理結果として測定される。また、右カメラの画像ｉ上のマーカー番号ｊについてマーカーの２次元座標（ＸＲ_ｉｊ，ＹＲ_ｉｊ）が画像解析の処理結果として測定される。

そして、ロボット座標系における各マーカーの三次元座標（ＸＣ_ｉｊ，ＹＣ_ｉｊ，ＺＣ_ｉｊ）を求めることで、以下の３つの位置情報を対応付けた情報をメモリ内に記憶する。

左カメラにおけるマーカーの２次元座標（ＸＬ_ｉｊ，ＹＬ_ｉｊ）
右カメラにおけるマーカーの２次元座標（ＸＲ_ｉｊ，ＹＲ_ｉｊ）
ロボット座標系におけるマーカーの三次元座標（ＸＣ_ｉｊ，ＹＣ_ｉｊ，ＺＣ_ｉｊ）

ロボット座標系における各マーカーの三次元座標は以下の手順により求める。

始めに、順運動学により、ロボット座標系におけるロボットアームの先端座標（Ｘｒ，Ｙｒ，Ｚｒ）および角度（Ｒｘ，Ｒｙ，Ｒｚ）が求まる。そして、この先端座標、角度と、予め求めておいたチェッカーボード３の各マーカーのアーム先端に対する相対的な位置関係を表すマーカー相対位置（ｘ，ｙ，ｚ）とから、上記（式１）を（Ｘｐ，Ｙｐ，Ｚｐ）について解いた式を用いて、チェッカーボードのそのマーカーのロボット座標系における三次元座標（Ｘｐ，Ｙｐ，Ｚｐ）を求める。

このようにして、チェッカーボードの各マーカーのロボット座標系における三次元座標を求める。
そして、求めた画像ｉのマーカー番号ｊのロボット座標系におけるマーカーの三次元座標と、左カメラの画像ｉのマーカー番号ｊの２次元座標と、右カメラの画像ｉのマーカー番号ｊの２次元座標とを対応付けて、その対応付け情報をＰＣ２のメモリ内に格納する。

続いて図８および図９を参照して、自動キャリブレーションの実施方法を説明する。
図８に示すように、作業対象の物体のある点Ｍを第１のカメラ（左カメラと右カメラのうちの一方、例えば左カメラ）と第２のカメラ（左カメラと右カメラのうちの他方、例えば右カメラ）で撮影した場合において、第１のカメラの特性に起因する内部パラメータをＡ_１（３行３列のマトリクス）、第２のカメラの特性に起因する内部パラメータをＡ_２（３行３列のマトリクス）、ワールド座標系に対する第１のカメラの相対的な位置を示す列ベクトルをＴ_１、その第１のカメラの相対的な姿勢を示すマトリクス（３行３列）をＲ_１、ワールド座標系に対する第２のカメラの相対的な位置を示す列ベクトルをＴ_２、その第２のカメラの相対的な姿勢を示すマトリクス（３行３列）をＲ_２、第１のカメラの画像上にある点Ｍの位置をｍ_１、第２のカメラの画像上にある点Ｍの位置をｍ_２、とする。

ここで、図９に示すように、回転を表す行列をＲとすると、座標変換を表す行列はＲ^Ｔとなる。このため、第１のカメラを基準とした第２のカメラの相対的な位置ｔは、下記（式２）により求められる。

また、第１のカメラを基準とした第２のカメラの相対的な姿勢Ｒは、下記（式３）により求められる。

また、ｍ^〜 _１およびｍ^〜 _２を、それぞれ第１のカメラおよび第２のカメラにおける画像座標のｍ_１およびｍ_２の斉次座標とし、Ｍ^〜を点Ｍの斉次座標とすると、それらの間には、下記（４）式の関係が成立する。

（式４）において、ｍ^〜 _１＝ ［ｕ_１ ｖ_１ １］^Ｔ（ｍ_１＝（ｕ_１，ｖ_１））であり、ｍ^〜 _２ ＝ ［ｕ_２ ｖ_２ １］^Ｔ（ｍ_２＝（ｕ_２，ｖ_２））である。（式４）をＭ^〜に対して解くと、下記（式５）が得られる。

（式５）より下記（式６）が得られる。

（式６）から、ベクトルＡ_１ ^−１ｍ^〜 _１、ｔ、ＲＡ_２ ^−１ｍ^〜 _２が同一平面上に存在することが分かる。これにより、ベクトルｔと、ベクトルＲＡ_２ ^−１ｍ^〜 _２との外積はベクトルＡ_１ ^−１ｍ^〜 _１と直交するため、下記（式７）が成立する。

ここで、下記（式８）により基礎行列Ｆを定義すると、

（式７）は下記（式９）のように表される。

ステレオカメラのキャリブレーションを行なう際には、（式９）を成分表示した下記（式１０）に、測定された左カメラの画像のマーカーの２次元座標（ＸＬ_ｉｊ，ＹＬ_ｉｊ）とそれに対応する右カメラの画像のマーカーの２次元座標（ＸＲ_ｉｊ，ＹＲ_ｉｊ）とを代入して、基礎行列の各成分を求める。

８点アルゴリズム等の公知の基礎行列算出手法を用いることで基礎行列を求める。８点アルゴリズムとは、上記（式１０）を非同次線形方程式に書き換え、連立方程式の解を８組の左右画像点のペアから計算する手法である（例えば、非特許文献２参照。）。

次に、ステレオ座標系におけるチェッカーボードのマーカーの三次元座標を復元する。これには、前述した方法により求めた基礎行列Ｆと、左右のカメラ画像から検出したマーカーの２次元座標（ＸＬ_ｉｊ，ＹＬ_ｉｊ）および（ＸＲ_ｉｊ，ＹＲ_ｉｊ）を用いる。

まず、（式８）の両辺に、カメラの内部パラメータＡ_１およびＡ_２をかける。すなわち、両辺に対し、左側からＡ_１ ^Ｔをかけ、右側からＡ_２をかける。この結果下記（式１１）が得られる。

（式１１）より（式１２）が得られる。

（式１２）において、右辺は既知であるため、左辺、すなわち、第１のカメラを基準とした第２のカメラの位置ｔおよび姿勢Ｒを求めることになる。このため、算出された［ｔ］_×Ｒを特異値分解して下記（式１３）を得る。

なお、Ｕ，Ｖ，Ｄは３行３列のマトリクスである。この特異値分解の結果と下記行列Ｚ，Ｗにより、下記（式１４）に示すように、第１のカメラを基準とした第２のカメラの位置ｔ＾および姿勢Ｒ＾を推定する。

推定したＲ＾およびｔ＾を上記（式４）に代入することにより、ステレオカメラ座標系におけるマーカーの三次元座標（ＸＳ_ｉｊ，ＹＳ_ｉｊ，ＺＳ_ｉｊ）を求める。ここでは、第１のカメラを基準としているので、（式４）において、Ｒ_１＝Ｉ（Ｉは単位行列）、Ｔ_１＝０（０はゼロベクトル）、Ｒ_２＝Ｒ＾、Ｔ_２＝ｔ＾、となる。この結果、下記（式１５）が得られる。なお、（式１５）において、λは斉次座標を画像座標に戻す際のスカラ倍パラメータであり、ステレオカメラ座標系におけるマーカーの三次元座標は（Ｘ，Ｙ，Ｚ）と略記されている。

（式１５）を連立方程式とし、ステレオカメラ座標系におけるマーカーの三次元座標（Ｘ，Ｙ，Ｚ）について解くと、下記（式１６）が得られ、この（式１６）の解を求めることで三次元復元が行われる。

続いて剛体変換行列の算出処理について説明する。
図１０は、ステレオカメラ座標系とロボット座標系の剛体変換行列の算出方法を示す図である。

ステレオカメラ座標系とロボット座標系の剛体変換行列Ｒ（回転成分）およびＴ（並進成分）は、ロボット座標系におけるマーカーの三次元座標（ＸＣ_ｉｊ，ＹＣ_ｉｊ，ＺＣ_ｉｊ）とステレオカメラ座標系におけるマーカーの三次元座標（ＸＳ_ｉｊ，ＹＳ_ｉｊ，ＺＳ_ｉｊ）との対応関係として下記（式１７）により定義される。

ステレオカメラ座標系とロボット座標系の関係式すなわち上記（式１７）から剛体変換行列を求めるには、（式１７）を下記（式１８）のように変形して、この（式１８）の値が最小となる行列Ｒ，Ｔを求める。

求める方法としては、例えば、非特許文献３に記載される公知の手法を用いる。この方法では、ステレオカメラ座標系とロボット座標系の各マーカーの点群の中心を求める。そして、ステレオカメラ座標系をその点群中心の周りに、中心から各点へのベクトルの方向をステレオカメラ座標系とロボット座標系とで合わせるように回転させることにより回転行列Ｒを求め、回転後のステレオカメラ座標系の点群中心と、ロボット座標系の点群中心との距離を求めることで、並進行列Ｔを求める。

なお、以上の説明において（式２）〜（式１８）は公知の数式である。
図１１は、本実施形態のロボットビジョンシステムの機能ブロック図である。
図１１に示すように、ロボットビジョンシステムは、第１のカメラ２１−１、第２のカメラ２１−２、画像記憶部２２、交点検出部２３、基礎行列演算部２４、三次元復元部２５、移動方向決定部２６、アーム制御部２７、ロボットアーム２８、アーム状態記憶部３１、アーム先端位置算出部３２、マーカー位置算出部３３、マーカー相対位置記憶部３４、対応付け処理部３５、対応付け情報記憶部３６、剛体変換行列演算部３７、によって構成される。

第１のカメラ２１−１は、左カメラと右カメラのうちの一方、例えば左カメラに対応し、本実施形態では主としてチェッカーボード（図１１では不図示）を撮影するのに使用する。

第２のカメラ２１−２は、左カメラと右カメラのうちの他方、例えば右カメラに対応し、チェッカーボードを撮影するのに使用する。この第１のカメラ２１−１および第２のカメラ２１−２によりステレオカメラが構成される。

画像記憶部２２は、左右両カメラにより撮影したチェッカーボードが写された複数枚の画像を記憶する。
交点検出部２３は、チェッカーボードが写された複数枚の画像のそれぞれを画像解析して、各画像のチェッカーボード上の複数の交点を検出する。

基礎行列演算部２４は、交点検出部２３により検出された左右両カメラ上の交点をマーカーとし、マーカーの２次元座標を用いて基礎行列Ｆを、例えば上記（式１０）により算出する。

移動方向決定部２６は、図３を参照して説明したように、カメラに入らなかった点の座標値の平均値を座標値とする点と画像の中心点とにより定まるベクトルの方向をアームの移動方向として算出する。

アーム制御部２７は、移動方向決定部２６により算出された移動方向にアームが移動するように制御信号をロボットアーム２８に対して出力する。
アーム状態記憶部３１は、アーム２８のエンコーダ値（アーム２８の各アーム関節にそれぞれ内蔵されている角度計測用エンコーダの値すなわち各関節の角度値）、アーム２８の長さ、チェッカーボードの取り付け位置を記憶する。

アーム先端位置算出部３２は、アーム状態記憶部３１に記憶されるアームの状態から順運動学によりロボット座標系におけるアームの先端座標（Ｘｒ，Ｙｒ，Ｚｒ）および角度（Ｒｘ，Ｒｙ，Ｒｚ）を算出する。

そして、マーカー位置算出部３３は、このアームの先端座標（Ｘｒ，Ｙｒ，Ｚｒ）および角度（Ｒｘ，Ｒｙ，Ｒｚ）と、マーカー相対位置記憶部３４に記憶されるチェッカーボードの各マーカーのアーム先端に対するマーカー相対位置（ｘ，ｙ，ｚ）とから、上記（式１）を（Ｘｐ，Ｙｐ，Ｚｐ）について解いた式を用いて、チェッカーボードの各マーカーのロボット座標系における三次元座標（Ｘｐ，Ｙｐ，Ｚｐ）を求める。

対応付け処理部３５は、マーカー位置算出部３３により算出された画像ｉのマーカー番号ｊのロボット座標系におけるマーカーの三次元座標と、左カメラの画像ｉのマーカー番号ｊの２次元座標と、右カメラの画像ｉのマーカー番号ｊの２次元座標とを対応付けて、その対応付け情報を対応付け情報記憶部３６内に格納する。

三次元復元部２５は、基礎行列演算部２４により算出された基礎行列Ｆと、交点検出部２３により検出されたチェッカーボード上の複数のマーカーの二次元座標とを用いて、ステレオカメラ座標系におけるマーカー（交点）の三次元座標を求める。この際、三次元復元部２５は、対応付け情報記憶部３６のデータを読み出して参照し、算出結果の三次元座標を、算出に使用したマーカーの二次元座標のペアに対応付けて、対応付け情報記憶部３６に書き込む。

剛体変換行列演算部３７は、マーカー位置算出部３３により算出されたロボット座標系上のマーカーの三次元座標（各交点座標）と、三次元復元部２５により算出されたステレオカメラ座標系上のマーカーの三次元座標（各交点座標）とをそれらが対応付けられて記憶される対応付け情報記憶部３６から取得して剛体変換行列ＲおよびＴを算出する。

図１２は、自動キャリブレーション処理のフローチャートである。
ステップＳ１で、アーム制御部２７からの指示によりアーム２８が初期位置に移動される。続くステップＳ２では、第１のカメラ２１−１および第２のカメラ２１−２によって撮影された画像に対し、画像認識処理が行なわれる。この画像認識処理の結果として、ステップＳ３で、チェッカーボードが画面すなわち撮影されたすべての画像内に含まれているかどうかが判定される。

ステップＳ３でチェッカーボードが画面内に含まれていないと判定された場合（ステップＳ３の判定結果がＮｏの場合）、ステップＳ４で、アーム２８の目標位置が移動方向決定部２６により修正され、ステップＳ１に戻る。

ステップＳ３でチェッカーボードが画面内に含まれていると判定された場合（ステップＳ３の判定結果がＹｅｓの場合）、ステップＳ５で、チェッカーボードのコーナーが検出され、続くステップＳ６で図４を参照して説明したように、過去に撮影したチェッカーボードの認識点も含めて、画面上で、チェッカーボードの認識点が均一に分布しているかどうかが判定される。

ステップＳ６で認識点が画面内に均一に分布していないと判定された場合（ステップＳ６の判定結果がＮｏの場合）、ステップＳ７で、アーム２８の目標位置が移動方向決定部２６により修正され、ステップＳ１に戻る。

ステップＳ６で認識点が画面内に均一に分布していると判定された場合（ステップＳ６の判定結果がＹｅｓの場合）、ステップＳ８で、今回、第１のカメラ２１−１および第２のカメラ２１−２によって撮影された画像が画像記憶部２２に記憶される。

ステップＳ８に続くステップＳ９では、画像記憶部２２に取得した画像が規定枚数に達したかどうかが判定される。
ステップＳ９で取得した画像が規定枚数に達していないと判定された場合（ステップＳ９の判定結果がＮｏの場合）、ステップＳ１０で、アーム２８の目標位置が移動方向決定部２６により修正され、ステップＳ１に戻る。なお、ステップＳ１０では、次の撮影に使用する初期位置をアーム目標位置として設定しているが、ステップＳ４およびＳ７により位置補正処理が行われるために、カメラの視野内にチェッカーボードが部分的に入る任意の位置が目標位置として設定可能である。例えば、前回撮影時のアームの初期位置からランダム数値を引いた座標を次回初期値とすることができる。

ステップＳ９で取得した画像が規定枚数に達したと判定された場合（ステップＳ９の判定結果がＹｅｓの場合）、ステップＳ１１で、チェッカーボードが写された複数枚の左右画像からコーナーが検出されて、図５〜図７等を参照して説明したように、交点検出部２３により、チェッカーボードが写された複数枚の左右画像のそれぞれが画像解析されて、各画像のチェッカーボード上の複数の交点が検出される。

そして、続くステップＳ１２で、基礎行列演算部２４により、交点検出部２３により検出された左右両カメラ上の交点をマーカーとし、マーカーの２次元座標を用いて基礎行列Ｆが例えば上記（式１０）により算出される。

ステップＳ１２に続くステップＳ１３では、アーム先端位置計算部３２により、チェッカーボードのアーム状態記憶部３１に記憶されるアームの状態から順運動学によりロボット座標系におけるアームの先端座標（Ｘｒ，Ｙｒ，Ｚｒ）および角度（Ｒｘ，Ｒｙ，Ｒｚ）が算出される。そして、マーカー位置算出部３３にて、この先端座標および角度と、マーカー相対位置記憶部３４に記憶されるチェッカーボードの各マーカーのアーム先端に対するマーカー相対位置座標（ｘ，ｙ，ｚ）とから、上記（式１）を（Ｘｐ，Ｙｐ，Ｚｐ）について解いた式を用いて、チェッカーボードの各マーカーのロボット座標系における座標（Ｘｐ，Ｙｐ，Ｚｐ）が求められる。なお、この後、ステップＳ１３において、対応付け処理部３５により、マーカー位置算出部３３により算出された画像ｉのマーカー番号ｊのロボット座標系におけるマーカーの三次元座標と、左カメラの画像ｉのマーカー番号ｊの２次元座標と、右カメラの画像ｉのマーカー位置ｊの２次元座標とが対応付けられて、その対応付け情報が対応付け情報記憶部３６内に格納される。

ステップＳ１３に続くステップＳ１４では、三次元復元部２５により、基礎行列演算部２４により算出された基礎行列Ｆと、交点検出部２３により検出されたチェッカーボード上の複数のマーカー位置とを用いて、ステレオカメラ座標系におけるマーカー（交点）の三次元座標が算出される。この際、三次元復元部２５は、対応付け情報記憶部３６のデータを読み出して参照し、算出結果の三次元座標を、算出に使用したマーカーの二次元座標のペアに対応付けて、対応付け情報記憶部３６に書き込む。

ステップＳ１４に続くステップＳ１５では、剛体変換行列演算部３７により、マーカー位置算出部３３により算出されたロボット座標系上のマーカーの三次元座標（各交点座標）と、三次元復元部２５により算出されたステレオカメラ座標系上のマーカーの三次元座標（各交点座標）とがそれらが対応付けられて記憶される対応付け情報記憶部３６から取得されて剛体変換行列ＲおよびＴが算出され、一連の処理が終了する。

なお、上記実施形態において、基礎行列算出用冶具はチェッカーボードに限らず等間隔に円が描かれたプレート等の公知の基礎行列算出用冶具を用いることが可能である。また、基礎行列算出用冶具はハンド部に固定する必要はなく、その他のロボットの可動部に固定することも可能である。さらに、基礎行列算出用冶具の模様をロボットの可動部に描いたり、その模様が描かれたシールを貼り付ける等としても良い。

１ ステレオカメラ
２ ＰＣ
３ チェッカーボード
４ ロボット
５ ロボットコントローラ
６ アームの根元位置
７ アームの先端位置
８ 指示ピン
９−１、９−２ カメラ画像上の認識点が少ない領域
１０−１ チェッカーボードの上位置
１０−２ チェッカーボードの下位置
１１−１、１１−２ 検出した矩形（黒色）
１２−１、１２−１ 開始点候補
１３ 開始点
２１−１ 第１のカメラ
２１−２ 第２のカメラ
２２ 画像記憶部
２３ 交点検出部
２４ 基礎行列演算部
２５ 三次元復元部
２６ 移動方向決定部
２７ アーム制御部
２８ ロボットアーム
３１ アーム状態記憶部
３２ アーム先端位置算出部
３３ マーカー位置算出部
３４ マーカー相対位置記憶部
３５ 対応付け処理部
３６ 対応付け情報記憶部
３７ 剛体変換行列演算部

Claims (5)

1. 第１および第２のカメラにより構成されるステレオカメラと、
   アームを有するロボットと、
   前記ステレオカメラおよび前記ロボットを駆動し制御する制御装置と、を備え、
   前記ロボットは、可動部にステレオカメラの基礎行列算出に用いる複数のマーカーを有し、
   前記制御装置は、
   前記第１のカメラおよび第２のカメラにより撮像された前記マーカーの画像を用いて前記ステレオカメラの基礎行列を算出する基礎行列算出部と、
   前記基礎行列と前記第１のカメラおよび第２のカメラにより撮像された画像とを用いて、前記各マーカーのステレオカメラ座標系における三次元位置を算出する三次元復元部と、
   ロボット座標系における前記各マーカーの三次元位置を算出するマーカー位置算出部と、
   前記マーカー毎に、前記マーカー位置算出部で算出されたロボット座標系における三次元位置と、前記三次元復元部で算出されたステレオカメラ座標系における三次元位置と、を対応付け情報として記憶する対応付け情報記憶部と、
   該対応付け情報に基づいて、前記マーカー毎に、ロボット座標系とステレオカメラ座標系との剛体変換行列を算出する剛体変換行列演算部と、を有すること、
   を特徴とするロボットビジョンシステム。
2. 前記制御装置は、前記各マーカーが前記ステレオカメラにより撮影された複数の画像を重畳させた画像に基づき、前記ロボットのアームの移動方向を決定する移動方向決定部を有し、
   前記決定された移動方向に前記アームを移動させ、前記各マーカーを撮影すること、を特徴とする請求項１に記載のロボットビジョンシステム。
3. 前記ロボットは、前記複数のマーカーを有する基礎行列算出用冶具を前記ロボットのアーム先端部に固定する機構を備えること、を特徴とする請求項１または２に記載のロボットビジョンシステム。
4. 前記制御装置は、
   前記基礎行列算出用冶具の前記アーム先端位置および姿勢に対する前記各マーカーの相対位置を記憶したマーカー相対位置記憶部と、
   ロボット座標系におけるアーム先端部位置を順運動学により算出するアーム先端位置算出部と、を有し、
   前記マーカー位置算出部は、前記アーム先端位置算出部にて算出された前記アーム先端位置および姿勢、前記マーカー相対位置記憶部に記憶された前記各マーカーの相対位置を用いて、ロボット座標系における前記各マーカーの三次元位置を算出すること、
   を特徴とする請求項３に記載のロボットビジョンシステム。
5. 第１および第２のカメラにより構成されるステレオカメラと、
   アームを有するロボットと、
   前記ステレオカメラおよび前記ロボットを駆動し制御する制御装置と、
   ロボットの可動部にステレオカメラの基礎行列算出に用いる複数のマーカーと、を有するロボットビジョンシステムが実行する自動キャリブレーション方法において、
   前記第１のカメラおよび第２のカメラにより撮像された前記マーカーの画像を用いて前記ステレオカメラの基礎行列を算出する基礎行列算出ステップと、
   前記基礎行列と前記第１のカメラおよび第２のカメラにより撮像された画像とを用いて、前記各マーカーのステレオカメラ座標系における三次元位置を算出する三次元復元ステップと、
   ロボット座標系における前記各マーカーの三次元位置を算出するマーカー位置算出ステップと、
   前記マーカー毎に、前記マーカー位置算出ステップで算出されたロボット座標系における三次元位置と、前記三次元復元ステップで算出されたステレオカメラ座標系における三次元位置と、を対応付け情報として記憶する対応付け情報記憶ステップと、
   該対応付け情報に基づいて、前記マーカー毎に、ロボット座標系とステレオカメラ座標系との剛体変換行列を算出する剛体変換行列演算ステップと、を有することを特徴とする自動キャリブレーション方法。