JP2011011321A - ロボットシステム及びロボットシステムのキャリブレーション方法 - Google Patents

Abstract

【課題】キャリブレーションを容易且つ高精度に行なう。
【解決手段】キャリブレーション冶具５をハンド２ｂに固定し、ハンド２ｂを移動させて、キャリブレーション冶具５上の特徴点を所定の位置に移動させる。このときのカメラ座標系における特徴点の位置座標から、カメラ座標系における、キャリブレーション冶具５の座標系である冶具座標系の原点の位置及び姿勢を検出する。冶具座標系はハンド２ｂを基準とするアーム先端座標系と一致するため、カメラ座標系における特徴点と、アーム先端座標系との相対関係を検出することができ、すなわちキャリブレーション冶具５とハンド２ｂとの相対関係を検出することができる。このキャリブレーション冶具５とハンド２ｂとの相対関係を用いて、カメラ座標系とロボット座標系との剛体変換行列を求める。
【選択図】 図２

Description

本発明は、ステレオカメラの撮像画像に基づきロボット本体を駆動制御するようにしたロボットシステム及びロボットシステムのキャリブレーション方法に関する。

ステレオカメラを用いることによりロボット等の自動機械に視覚を持たせ、作業対象の３次元位置や姿勢を検出・識別し、作業対象の３次元位置や姿勢に基づいて自動機械に動作を行なわせるシステム（以下、ロボットシステムともいう）が提案されている。
このようなロボットシステムにおいては、ステレオカメラのカメラ自体のパラメータ、ステレオカメラシステムとしてのパラメータ、ステレオカメラを基準とするカメラ座標系とロボット本体の例えば設置位置を基準とするロボット座標系とを変換するためのパラメータ等を求める必要がある。

つまり、一つのシステムにおいて、パラメータを求める作業が複数回必要である。そのため、各パラメータ算出用の冶具をロボットに取り付け、パラメータの算出を自動化する様々な手法が提案されている。
カメラ座標系とロボット座標系との相対関係を算出する作業においては、ロボット座標系とカメラ座標系との剛体変換行列を求める方法が知られている。一般的には、ロボット座標系における座標とカメラ座標系における座標とが両方とも既知である点を複数求め、両者の剛体変換行列を数学的に解く方法が用いられている。

また、ステレオカメラなどに代表されるカメラ座標系における座標を高精度に計測するために、計測用ピースや、特徴点を持つ冶具等といった特徴的な冶具を用いる方法（例えば、特許文献１、特許文献２参照）や、冶具等を取り付けた後、ロボットのアームを長方形に動作させる方法（例えば、特許文献３参照）等が提案されている。

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

しかしながら、このような手法においては、実際に剛体変換行列を求めようとすると、ロボット座標系における冶具上の特徴点座標が必要である。それを算出するためには、ロボットと冶具上の特徴点との位置関係が既知でなければならない。
そのために、ロボットと冶具上の特徴点との位置関係をロボットシステムに予め記憶させておき、冶具を高精度にロボットに設置することのできる設置機構を用いる必要があった。また、冶具を高精度にロボットに設置することのできる設置機構を用いない場合にあっては、別途、ロボットと冶具上の特徴点との位置関係を高精度に検出するための機構を設ける必要があった。このため、ロボットと冶具上の特徴点との位置関係を高精度に把握するための特別な機構等を必要とすることなく、容易且つ高精度にキャリブレーションを行なう方法が望まれていた。なお、本明細書では、カメラ座標系における特徴点の位置座標に基づきロボット座標系とカメラ座標系との剛体変換行列を求める処理をキャリブレーションと称する。
そこで、この発明は、上記従来の未解決の問題点に着目してなされたものであり、キャリブレーションを容易且つ高精度に行なうことの可能なロボットシステム及びロボットシステムのキャリブレーション方法を提供することを目的としている。

上記目的を達成するために、本発明の請求項１にかかるロボットシステムによれば、ロボット本体と、前記ロボット本体を異なる複数の視点から撮影する多視点カメラと、キャリブレーション冶具を用いてキャリブレーションを行なうと共に、前記多視点カメラの撮像画像に基づき前記ロボット本体を駆動制御する制御手段と、を備えたロボットシステムにおいて、前記キャリブレーション冶具は特徴点を有し且つ前記ロボット本体に固定され、前記制御手段は、前記ロボット本体を制御し、前記キャリブレーション冶具上の前記特徴点を、予め設定した相対関係検出条件を満足する複数の地点に移動させる特徴点移動制御手段と、前記キャリブレーション冶具上の同一特徴点の、前記複数の地点でのカメラ座標系における位置座標を検出し、当該位置座標に基づき、前記キャリブレーション冶具が固定される前記ロボット本体における部位に設定された座標系と一致する冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を検出する相対関係検出手段と、を備えることを特徴としている。

また、請求項２にかかるロボットシステムによれば、前記相対関係検出手段は、前記カメラ座標系における前記特徴点の位置座標に基づき、前記カメラ座標系における前記冶具座標系の原点位置を検出する原点座標検出手段と、前記カメラ座標系における前記冶具座標系の姿勢を検出する姿勢検出手段と、を有し、前記カメラ座標系における前記冶具座標系の原点の位置及び姿勢と前記特徴点の位置座標とに基づき、前記冶具座標系と前記特徴点との相対関係を検出することを特徴としている。

また、請求項３にかかるロボットシステムによれば、前記特徴点移動制御手段は、前記同一特徴点が、前記冶具座標系の原点を中心とする球の表面に分布するように前記ロボット本体を制御し、前記原点座標検出手段は、同一平面上にない少なくとも４地点での、前記カメラ座標系における前記同一特徴点の位置座標に基づき、前記カメラ座標系における前記冶具座標系の原点の位置座標を演算することを特徴としている。

さらに、請求項４にかかるロボットシステムによれば、前記特徴点移動制御手段は、前記同一特徴点が、前記冶具座標系の原点を通る少なくとも２本の直線上に分布するように前記ロボット本体を制御し、前記原点座標検出手段は、前記冶具座標系の原点を通る同一直線上の地点での、前記カメラ座標系における前記同一特徴点の位置座標に基づき、前記直線の方程式をそれぞれ演算し、前記複数の直線の交点座標から、前記カメラ座標系における前記冶具座標系の原点の位置座標を演算することを特徴としている。

さらにまた、請求項５にかかるロボットシステムによれば、前記姿勢検出手段は、前記特徴点移動制御手段により前記ロボット本体を制御したときの、前記キャリブレーション冶具上の任意の特徴点の冶具座標系における移動ベクトルと、前記冶具座標系における移動ベクトルに対応する前記カメラ座標系における同一特徴点の移動ベクトルとの対であって、座標系ごとの移動ベクトルが平行ではない少なくとも２対の移動ベクトルを参照対とし、当該参照対に基づき前記カメラ座標系における前記冶具座標系の姿勢を演算することを特徴としている。

なおさらに、請求項６に係るロボットシステムによれば、前記制御手段は、前記相対関係検出手段により検出された前記冶具座標系と前記特徴点との相対関係に基づいて求めた前記特徴点のロボット座標系における位置座標と、前記カメラ座標系における特徴点の位置座標とを用いて、前記カメラ座標系と前記ロボット座標系との剛体変換行列を求める剛体変換行列算出手段を、備えることを特徴としている。

また、本発明の請求項７にかかるロボットシステムのキャリブレーション方法は、ロボット本体と、前記ロボット本体を異なる複数の視点から撮影する多視点カメラと、キャリブレーション冶具を用いてキャリブレーションを行なうと共に、前記多視点カメラの撮像画像に基づき前記ロボット本体を駆動制御する制御手段と、を備えたロボットシステムのキャリブレーション方法において、前記キャリブレーション冶具は特徴点を有し且つ前記ロボット本体に固定され、前記ロボット本体を制御して前記キャリブレーション冶具上の特徴点を、予め設定した相対関係検出条件を満足する複数の地点に移動させるステップと、前記キャリブレーション冶具上の同一特徴点の、前記複数の地点でのカメラ座標系における位置座標を検出し、検出した位置座標に基づき、前記キャリブレーション冶具が固定される前記ロボット本体における部位に設定された座標系と一致する冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を検出するステップと、前記冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を用いて前記キャリブレーションを行なうステップと、を備えることを特徴としている。

本発明によれば、予め設定した条件を満足するようにキャリブレーション冶具を動作させ、各地点でのカメラ座標系における特徴点の位置座標をもとに、ロボット本体のキャリブレーション冶具が固定される部位に設定された冶具座標系とキャリブレーション冶具上の特徴点との相対関係を、自動的に検出することができる。したがって、この相対関係を用いて、カメラ座標系とロボット座標系との剛体変換行列を算出することにより、キャリブレーションを容易且つ高精度に行なうことができると共に、キャリブレーション冶具上の特徴点と冶具座標系との相対関係を検出する処理から、剛体変換行列を算出するまでの処理を一括して自動的に行なうことができる。

本発明を適用したロボットシステムの構成を示す図である。 ロボットアーム装置と冶具座標系との関係を表す説明図である。 ロボットアーム装置上の任意の点の、ロボット座標系における位置の検出方法を説明するための図である。 アーム先端座標系と特徴点との関係を表す説明図である。 冶具座標系の原点位置座標を検出する際の、ハンドを回転させる場合の一例である。 冶具座標系の原点位置座標を検出する際に、ハンドの回転に伴い、特徴点により形成される球の一例である。 キャリブレーション時の制御装置の処理手順の一例を示すフローチャートである。 根元関節座標系を冶具座標系とした場合の、キャリブレーション冶具の配置例である。

以下、本発明の実施の形態を説明する。
図１は、本発明を適用したロボットシステムの一例を示す概略構成図である。
図１において、１は複数のカメラを有し複数の視点から撮影することが可能な多視点カメラである。ここでは多視点カメラとしてステレオカメラを用いている。２はロボットアーム装置（以下、ロボット本体ともいう。）、３はロボットアーム装置２を駆動制御するロボットコントローラ、４はロボットシステム全体を制御する制御装置、５はキャリブレーションに用いるキャリブレーション冶具である。

前記ロボットアーム装置２は、複数の関節を有するアーム２ａと、アーム２ａの先端に取り付けられたワークを把持するハンド２ｂと、を備える。
ロボットコントローラ３は、制御装置４から出力された制御対象部位の目標座標情報を受け取り、制御対象部位が目標座標情報に基づく位置に移動するように、ロボットアーム装置２を駆動制御する。
制御装置４はパーソナルコンピュータなどで構成され、ステレオカメラ１の撮影画像を画像処理してワーク等の制御対象部位の位置を検出し、これに基づき、制御対象部位の目標座標を演算する。そして、これをＬＡＮ等の通信回線を介してロボットコントローラ３に出力する。

また、制御装置４は、起動時等の予め設定されたタイミングでキャリブレーションを行なう。このキャリブレーションは、ロボットアーム装置２に固定したキャリブレーション冶具５を用いて行なう。
キャリブレーション冶具５は、図１に示すように、例えば、正方形の黒領域と白領域とが市松模様に配置されたチェッカーボードで構成され、黒領域の角どうしが突き合わさる部分が特徴点として用いられる。

ここでは、キャリブレーション冶具５として正方形の黒領域と白領域とが市松模様に配置されたチェッカーボードを用いているが、これに限るものではない。例えば、白地のボード上に、菱形或いは三角形の黒領域が、角どうしを突き合わせて配置されたもの等であっても適用することができる。なお、キャリブレーションに適用できる公知のその他の模様を適用することもできる。
また、キャリブレーション冶具５は、図２に示すように、ロボットアーム装置２の先端の可動部、例えばハンド２ｂに、キャリブレーションを行なうときに取り付けられる。ただし、キャリブレーション冶具５が通常の動作中に障害とならない場合には着脱可能な構造を採用せず、完全に固定しても良い。

キャリブレーションを開始すると、制御装置４は、ロボットコントローラ３に目標座標情報を出力してロボットアーム装置２を予め設定した手順で動作させることによりキャリブレーション冶具５を移動させる。キャリブレーション冶具５の移動中にステレオカメラ１でキャリブレーション冶具５を撮影し、キャリブレーション冶具５上の特徴点のカメラ座標系における位置座標を取得する。さらに、特徴点のカメラ座標系における位置座標に基づき、キャリブレーション冶具５とロボットアーム装置２との相対関係を検出する。そして、この相対関係を用いてカメラ座標系とロボット座標系との剛体変換行列を求める。なお、ステレオカメラ１のカメラ自体のパラメータや、ステレオカメラ１のシステムとしてのパラメータ等は予め検出済みであり、カメラ座標系における特徴点の位置座標は取得可能であるものとする。

前記カメラ座標系は、図１にΣ_Cで示すように、ステレオカメラ１を基準とする座標系である。また、前記ロボット座標系は、図１にΣ_Rで示すように、例えば、ロボットアーム装置２が設置されているロボット設置面のロボット位置を基準として定義されるロボットアーム装置２を基準とした座標系である。
カメラ座標系とロボット座標系との剛体変換行列を求めるには、キャリブレーション冶具５上の特徴点のカメラ座標系およびロボット座標系それぞれにおける位置座標を求める必要がある。

カメラ座標系における特徴点の位置座標は、前記の通り取得可能である。
一方、ロボット座標系におけるロボット装置２上にある点の位置座標、例えばアーム先端位置は、図３（ａ）に示すようにロボットアーム装置２の関節角度、関節間パラメータ等の固有データからロボット座標系におけるロボット装置２上にある点の位置を算出する手法である順運動学を用いて求めることができる。同様にして、図３（ｂ）に示すように所定の関節からキャリブレーション冶具５上の特徴点までのベクトルを求めることにより、ロボットアーム装置２がどのような姿勢をとっていても、関節角度及び関節間パラメータと所定の関節からキャリブレーション冶具５上の特徴点までのベクトルとによって、特徴点のロボット座標系における位置座標を順運動学を用いて算出することができる。

よって、制御装置４では、所定の関節からキャリブレーション冶具５上の特徴点までのベクトルを自動で算出し、算出したベクトルを用いてキャリブレーションを行なうこととする。
ここで、キャリブレーション冶具５が固定される前記ロボットアーム装置２における部位に設定された座標系と一致する座標系を冶具座標系とする。具体的には、図２に示すように、キャリブレーション冶具５は、ロボットアーム装置２の先端に位置するハンド２ｂに固定されており、ハンド２ｂとキャリブレーション冶具５は一体に動作する。このため、ロボットアーム装置２のハンド２ｂに設定されるアーム先端座標系と、キャリブレーション冶具５に設定される冶具座標系とは一致するものとなる。

つまり、キャリブレーション冶具５とロボットアーム装置２との相対関係を算出するということは、キャリブレーション冶具５上の特徴点に関するハンド２ｂを基準とするアーム先端座標系における特徴点の位置座標、すなわち冶具座標系における特徴点の位置座標を求めることになる。
図４に示すように、ステレオカメラ１の撮像画像に基づき計測した特徴点の位置座標Ｐ（ｘ，ｙ，ｚ）は、カメラ座標系における位置座標である。このため、カメラ座標系とロボット座標系との剛体変換行列を求めるには、カメラ座標系における位置座標Ｐ（ｘ，ｙ，ｚ）をアーム先端座標系（冶具座標系）における位置座標Ｃ（Ｘ，Ｙ，Ｚ）に変換する必要がある。これは、アーム先端座標系（冶具座標系）における特徴点の位置座標を把握できれば、前記の通り順運動学によりロボット座標系における位置座標に変換することができるからである。

そのため、まずキャリブレーション冶具５上の各特徴点のカメラ座標系における位置座標を、ステレオカメラ１の撮像画像に基づき計測したカメラ座標系における計測データに基づき算出する。次に、冶具座標系が、カメラ座標系内のどの位置に、またどの姿勢で存在するかを算出する。
カメラ座標系における冶具座標系の原点位置を求めるためには、冶具座標系の原点位置を直接的に計測することはできないため、冶具座標系の原点位置と所定の関係を有する原点位置以外の計測できる点を用いて、冶具座標系の原点位置を算出する必要がある。例えば、各計測点と冶具座標系原点との距離が全て等しいという関係や、計測した対となる２点を結ぶ直線は冶具座標系の原点を通る直線であって、その２以上の直線の交点は冶具座標系の原点となるという関係を用いることにより、冶具座標系の原点位置を算出することができる。

つまり、冶具座標系の原点位置を算出するにはキャリブレーション冶具５上の１つの特徴点に対し、三次元座標において少なくとも同一平面上にない４点を求めればよい。
例えば、計測した点が、計測した点から冶具座標系原点位置までの距離が等しいという関係を満足する点である場合には、次式（１）の関係式を用いることができる。
ｒ²＝（ｘ−ｘｃ）²＋（ｙ−ｙｃ）²＋（ｚ−ｚｃ）² ……（１）
ただし、ｒは計測した点から冶具座標系原点位置までの距離、（ｘｃ，ｙｃ，ｚｃ）はカメラ座標系における冶具座標系の原点位置、Ｐ（ｘ，ｙ，ｚ）は計測点のカメラ座標系における計測点の位置座標である。

（１）式に示す関係式の解を求めるためには、冶具座標系の原点位置を固定した状態でキャリブレーション冶具５を移動させたときの、キャリブレーション冶具５上の同一特徴点の位置座標を最低４回計測する必要がある。
また、計測した点が、計測した２点を結ぶ直線は冶具座標系の原点を通る直線であるという関係を満足する点である場合には、次式（２）の関係式を用いることができる。

ここで、（ａ_x，ａ_y，ａ_z）は直線の方向ベクトルであり、ｋは任意の変数である。
（２）式で表される直線式を求めるためには最低２点必要であり、直線上の点を算出するためには最低２本の直線が必要である。つまり冶具座標系の原点位置を通る２つの直線上の２点を計測すれば、その直線の交点が冶具座標系の原点となる。
本実施形態では、ロボットアーム装置２を駆動しキャリブレーション冶具５を移動させて、ステレオカメラ１の撮像画像に基づき、複数の地点における特徴点の位置座標を計測する。

ここでは、計測した同一特徴点の位置座標と冶具座標系の原点との距離が全て等しいという関係を用いて冶具座標系の原点の位置座標を求める場合について説明する。
図１に示すようにロボットアーム装置２のハンド２ｂにキャリブレーション冶具５を固定すると、前記のようにアーム先端座標系と冶具座標系とは一致している。
次に、アーム先端座標系つまり冶具座標系の原点位置を固定した状態で、キャリブレーション冶具５上の各特徴点が冶具座標系の原点を中心とした球の表面上を移動するようにロボットアーム装置２を駆動制御する。

例えば、アーム先端座標系の何れかの軸を中心として、キャリブレーション冶具５が固定されたハンド２ｂを回転させる。例えば、図５に示すように、アーム先端座標系のＹ軸を中心として可動部を回転させる。この動作をアーム先端座標系の何れか２つの軸に対して行なうことで、キャリブレーション冶具５上の特徴点（図５中のＭ）を、アーム先端座標系の原点位置を中心とする球の表面に沿って移動させることができる。

さらに、ロボットアーム装置２が動作中の状態ｉにおける特徴点ｊのカメラ座標系における位置座標Ｐ_ij（ｘ_ij，ｙ_ij，ｚ_ij）を計測する。そして、キャリブレーション冶具５が各状態（ｉ＝１、２、３、……）にあるときの特徴点ｊの位置データを、点集合Ｑ_j＝｛Ｐ_1j，Ｐ_2j，Ｐ_3j，…，Ｐ_ij，…｝としてまとめる。
つまり、キャリブレーション冶具５を移動させる際には、カメラ座標系において冶具座標系の原点位置を算出可能とするために、アーム先端座標系の原点位置を動かさず、キャリブレーション冶具５が固定されたロボットアーム装置２のハンド２ｂの姿勢（角度）を変化させている。

キャリブレーション冶具５は、ロボットアーム装置２のハンド２ｂに固定され、アーム先端座標系と冶具座標系とは一致しているため、キャリブレーション冶具５上の各特徴点は、冶具座標系の原点位置を固定したまま移動することになる。このため、カメラ座標系において、同一特徴点の位置座標は、冶具座標系の原点位置を中心とする球の表面上に分布する。
したがって、キャリブレーション冶具５上の特徴点ｊの点集合Ｑ_jは、図６に示すように、カメラ座標系における冶具座標系の原点位置座標（ｘｃ，ｙｃ，ｚｃ）を中心とした、半径ｒ_j＝｛（ｘ_ij−ｘｃ）²＋（ｙ_ij−ｙｃ）²＋（ｚ_ij−ｚｃ）²｝^1/2の球の表面上に分布することになる。

そこで、点集合Ｑ_jから、各計測点が表面に分布する球の方程式を導く。すなわち、球の方程式は、次式（３）で表すことができるため、この（３）式を、点集合Ｑ_jと最小自乗法などの手法を用いて解き、特徴点ｊのカメラ座標系における冶具座標系の原点座標Ｏ_j（ｘｃ_j，ｙｃ_j，ｚｃ_j）を求める。
ｒ_j ²＝（ｘ_ij−ｘｃ_j）²＋（ｙ_ij−ｙｃ_j）²＋（ｚ_ij−ｚｃ_j）² ……（３）
同様の手順で他の特徴点についても球の方程式を解き、各特徴点についてカメラ座標系における冶具座標系の原点座標、すなわちカメラ座標系におけるアーム先端座標系の原点座標を求める。

ここで、キャリブレーション冶具５上の各特徴点から算出した球は、全て冶具座標系の原点を中心とした球を構成している（ただし、球の半径は各特徴点によって異なる。）。つまり、特徴点毎に算出した冶具座標系の原点位置座標は全て等しくなるはずである。したがって、特徴点毎のカメラ座標系における冶具座標系の原点位置座標から、最小自乗法を用いてカメラ座標系における冶具座標系の原点座標推定値Ｏ＾（ｘｃ＾，ｙｃ＾，ｚｃ＾）を求める。

次に、カメラ座標系における冶具座標系の姿勢を求める。
冶具座標系の姿勢を求めるためには、カメラ座標系における、ある特徴点Ｐ_ij（ｘ_ij，ｙ_ij，ｚ_ij）を、冶具座標系上の点Ｃｉｊ（Ｘ_ij，Ｙ_ij，Ｚ_ij）に変換する次式（４）の回転行列を求める必要がある。
Ｃ_ij＝Ｒ（Ｐ_ij−ｔ） ……（４）
なお、（４）式中のｔは、カメラ座標系の原点位置と冶具座標系の原点位置とを結ぶベクトルと等しくなり、カメラ座標系における冶具座標系の原点位置Ｏ＾（ｘｃ＾，ｙｃ＾，ｚｃ＾）と等しい値となる。
式（４）中の、回転行列Ｒは、例えば次式（５）で表すことができる。

（５）式は、回転行列Ｒを、座標変換の回転順番をＺ軸回転→Ｘ軸回転→Ｚ軸回転として分解した場合の回転行列式である。ここで、Ｃφ＝cosφ、Ｓφ＝sinφ、θ＝Ｘ軸回転角度、Ψは１回目のＺ軸回転角度、Φは２回目のＺ軸回転角度である。
なお、回転行列Ｒは上記（５）に限定されることはなく、公知の変換を表す他の行列を採用することもできる。例えば、クォータニオン等を用いても良い。
ここで、冶具座標系とカメラ座標系とで対応する２つのベクトルを用いることで（５）式の回転行列Ｒを解くことができる。具体的には、カメラ座標系のベクトルを回転行列Ｒで変換したベクトルが、対応する冶具座標系のベクトルと等しくなるとの関係から、θ、Φ、Ψを求めることとなる。

そのために、冶具座標系を回転させずに冶具座標系の姿勢を一定としたまま、キャリブレーション冶具５を移動させ、同一特徴点の同一直線上にない計測点を最低３点計測する。
前述のように、冶具座標系とアーム先端座標系は一致するので、アーム先端座標系を回転させず、アーム先端座標系の姿勢を一定としたまま、アーム先端座標系の原点を、アーム先端座標系におけるベクトルＶ１（Ｍ_1x，Ｍ_1y，Ｍ_1z）に相当するだけ移動させ、さらにベクトルＶ２（Ｍ_2x，Ｍ_2y，Ｍ_2z）に相当するだけ移動させる。そして、アーム先端座標系の原点を移動させる前、ベクトルＶ１移動後、ベクトルＶ２移動後において、キャリブレーション冶具５上の特徴点の、カメラ座標系における位置座標を検出する。なお、ベクトルＶ１とベクトルＶ２とは、平行ではない。

ここで、アーム先端座標系を回転させずアーム先端座標系の姿勢を一定としているため、カメラ座標系における各特徴点間の移動ベクトルは、アーム先端座標系におけるベクトルＶ１およびベクトルＶ２と等しくなる。さらに、アーム先端座標系と冶具座標系は一致するので、冶具座標系とカメラ座標系とにおいて対応するベクトルを２本求めることができたことになる。したがって、これらのベクトルを用いることにより、回転行列Ｒを求めることができる。すなわち、カメラ座標系における冶具座標系の姿勢（角度）を求めることができる。

以上により、カメラ座標系における、冶具座標系の位置及び姿勢（角度）が算出されたため、カメラ座標系における特徴点の計測データを、冶具座標系における位置座標に変換することが可能となる。
つまり、カメラ座標系におけるある特徴点Ｐ_ij（ｘ_ij，ｙ_ij，ｚ_ij）を冶具座標系上の点Ｃ_ij（Ｘ_ij，Ｙ_ij，Ｚ_ij）に変換する前記（４）式において、回転行列Ｒは相対角度を使った３行３列の行列からなるカメラ座標系における冶具座標系の姿勢を表し、ｔはカメラ座標系における冶具座標系の原点位置Ｏ＾（ｘｃ＾，ｙｃ＾，ｚｃ＾）を表す。そして、これらのパラメータを用いることにより、前記（４）式から、特徴点のカメラ座標系における位置座標を、冶具座標系における位置座標に変換する。
これにより、特徴点の、冶具座標系における位置座標が得られることになる。
なお、回転行列Ｒについても複数の特徴点を使用し、より多数のベクトルを用い、最小自乗法などを用いることにより、精度を向上させることも可能である。

以上により、特徴点の、冶具座標系における位置座標が得られ、冶具座標系はアーム先端座標系と一致するため、特徴点つまりキャリブレーション冶具５と、アーム先端座標系との相対関係を算出することができ、すなわち、キャリブレーション冶具５とロボットアーム装置２との相対関係を算出することができることになる。さらに、キャリブレーション冶具５上のアーム先端座標系（冶具座標系）における特徴点の位置座標と、ロボットアーム装置２の関節角度及び関節間パラメータとから、特徴点のロボット座標系における位置座標を、順運動学を用いて算出する。そして、各特徴点のカメラ座標系とロボット座標系の位置座標を用いて公知の手順で剛体変換行列を算出する。

制御装置４では、キャリブレーションを行なうときに、図７に示すフローチャートにしたがって以上の処理を実行し、キャリブレーション冶具５とロボットアーム装置２との相対関係の算出を自動的に行い、これを用いてカメラ座標系における特徴点の位置座標を、ロボット座標系における位置座標に変換し、カメラ座標系とロボット座標系の剛体変換行列を算出する。

すなわち、図７において、まずステップＳ１で、原点検出動作制御を行い、ロボットアーム装置２を駆動制御して、特徴点を移動させる。すなわちキャリブレーション冶具５が固定されたロボットアーム装置２のハンド２ｂにおいて、例えば、Ｚ軸、Ｘ軸、Ｚ軸の順に回転させ、アーム先端座標系の原点位置を固定したままキャリブレーション冶具５を移動させる。また、Ｚ軸回転前、Ｚ軸回転後、Ｘ軸回転後、Ｚ軸回転後の各時点での、カメラ座標系における各特徴点の位置座標を収集し、各状態における特徴点の点集合Ｑｊを生成する。
そして、各状態における特徴点の点集合Ｑｊを取得したならば、ステップＳ２に移行し、点集合Ｑｊに基づき、カメラ座標系における、冶具座標系の原点の位置座標を演算する。

次に、ステップＳ３に移行して、姿勢検出動作制御を行い、ロボットアーム装置２を駆動制御して特徴点を移動させる。すなわち、キャリブレーション冶具５が固定された、ロボットアーム装置２のハンド２ｂを回転させず姿勢を一定としたまま、キャリブレーション冶具５を移動させる。このとき、移動に伴う特徴点の移動ベクトルが同一直線状にない２つ以上のベクトルとなるように移動させる。また、各移動地点におけるカメラ座標系における特徴点の位置座標を収集する。なお、姿勢検出動作制御におけるロボットアーム装置２のハンド２ｂの移動指示は、アーム先端座標系における位置座標を用いて指示することができる。アーム先端座標系と冶具座標系は前記の通り一致しているので、冶具座標系における移動ベクトルを把握できるからである。

そして、所定の位置に特徴点を移動させたならば、ステップＳ４に移行し、冶具座標系における２本以上の移動ベクトルと、この冶具座標系における移動ベクトルに対応するカメラ座標系における特徴点の移動ベクトルとに基づき、カメラ座標系における、冶具座標系の姿勢（傾き）を演算する。
次いで、ステップＳ５に移行し、カメラ座標系における冶具座標系の原点の位置座標及び姿勢（傾き）に基づき、カメラ座標系における特徴点の位置座標を冶具座標系における特徴点の位置座標に変換する。次に、順運動学を用いて、冶具座標系における特徴点の位置座標をロボット座標系における特徴点の位置座標に変換する。さらに、カメラ座標系における特徴点の位置座標とロボット座標系における特徴点の位置座標を用いて、カメラ座標系とロボット座標系との剛体変換行列を公知の手順で算出する。そして処理を終了する。

ここで、キャリブレーション冶具５とハンド２ｂとの相対関係は、現状の相対関係に応じた値であり、キャリブレーション冶具５をハンド２ｂに固定したときに、キャリブレーション冶具５とハンド２ｂとの相対関係が前回キャリブレーションを行なったときの相対関係と異なったとしても、現在の状態に応じた相対関係が算出されることになる。
したがって、算出されたキャリブレーション冶具５とハンド２ｂとの相対関係を用いて、特徴点のロボット座標系における位置座標を算出することによって、ロボット座標系における特徴点の位置座標を現在の相対関係に応じて算出することができることになり、キャリブレーション冶具５とハンド２ｂとの相対関係が変化した場合であっても、その影響を受けることなく、ロボット座標系における特徴点の位置座標を算出することができることになる。

また、キャリブレーション冶具５とハンド２ｂとの相対関係の検出を自動的に行なうことができるため、キャリブレーション冶具５とハンド２ｂとの相対関係の検出及び、これを用いた、カメラ座標系とロボット座標系との剛体変換行列の演算とを一括して自動的に行なうことができる。
また、キャリブレーション冶具５とハンド２ｂとの相対関係の検出は、キャリブレーションを行なうときに通常用いるキャリブレーション冶具５を利用して行なっており、また、キャリブレーション冶具５をロボットアーム装置２に固定する際にもその固定位置に高い精度を要求されることはない。

したがって、キャリブレーション冶具５とロボットアーム装置２との位置決めを行なうための特別な機構等を必要とすることなく、キャリブレーションを容易、且つ高精度に行なうことができる。
なお、上記では、冶具座標系の原点位置を算出するための、キャリブレーション冶具５の特徴点を同一球の表面上に分布させる方法として、Ｚ軸回転、Ｘ軸回転及びＺ軸回転を行なう場合について説明したが、これに限るものではなく、他の軸を中心として回転させることも可能であり、また、３回以上回転させることも可能であり、特徴点を球の表面上に分布させることが可能であり、且つ同一平面上にない４箇所以上の地点に同一特徴点を分布させることができればどのように回転させてもよい。

また、上記では、キャリブレーション冶具５の特徴点を同一球の表面上に分布させる方法として、アーム先端座標系の原点位置を固定としたまま、アーム先端座標系の何れか２軸を中心として回転移動させる場合について説明したが、これに限るものではない。特徴点を球の表面上に分布させることが可能であり、且つ同一平面上にない４箇所以上の地点に同一特徴点を分布させることができれば、どのような方法で移動させてもよく、例えば、アーム先端座標系の原点位置を固定していれば、アーム先端座標系の３軸を中心として回転移動させることにより、特徴点を分布させるようにしてもよい。

なお、カメラ座標系における冶具座標系の原点位置を、「計測した２点を結ぶ直線は冶具座標系の原点を通る直線である」という関係を用いて算出する場合には、キャリブレーション冶具５上の特徴点と、冶具座標系の原点位置とを同一直線上に動かすということは不可能であるため、冶具座標系の原点位置を固定とし、原点位置を中心とする円の円周上に、特徴点が分布するように、ロボットアーム装置２を駆動制御する。具体的には、例えば、冶具座標系の原点位置を固定としたまま、アーム先端座標系の何れか１軸を中心としてハンド２ｂを１８０度回転させる。次に、他の１軸を中心として同様に可動部を１８０度回転させる。これにより、冶具座標系の原点位置を中心とする円の直径を表す２点に特徴点が移動することになり、これら２点は、円の中心を通るからすなわち冶具座標系の原点位置を通る直線上の２点を求めることができる。

また、カメラ座標系における冶具座標系の原点位置を、「計測した２点を結ぶ直線は冶具座標系の原点を通る直線である」という関係を用いて算出した場合、カメラ座標系における冶具座標系の姿勢（傾き）を算出する場合には、姿勢検出のために特徴点を再度移動させる必要はない。すなわち、冶具座標系の原点位置を中心とする円の直径を表す２つの特徴点間を結ぶベクトルと、ロボット座標系におけるハンド２ｂの移動点間を結ぶベクトルと、を用いて、上記と同様の手順で算出すればよい。

また、上記実施の形態においては、キャリブレーション冶具５を、ロボットアーム装置２の先端の可動部であるハンド２ｂに取り付ける場合について説明したが、これに限るものではなく、上述のように、冶具座標系の原点位置座標検出及び姿勢（傾き）検出を行なうための特徴点の分布を実現することができる位置であれば、任意の位置に設定することができる。
例えば、図８に示すように、キャリブレーション冶具５を、ロボットアーム装置２のアーム２ａの先端の可動部２ｃに配置することも可能である。

この場合には、キャリブレーション冶具５を配置した可動部２ｃを直接駆動する根元関節座標系が、冶具座標系となる。そして、可動部２ｃとキャリブレーション５との相対関係を上述と同様の手順で求めればよい。
また、上記実施の形態においては、キャリブレーション冶具５としてチェッカーボードを用いた場合について説明したが、位置座標を高精度に検出することのできる特徴点を有する冶具であれば、適用することができることはいうまでもない。
ここで、上記実施の形態において、ロボットアーム装置２がロボット本体に対応し、カメラコントローラ３及び制御装置４が制御手段に対応している。

また、図７においてステップＳ１及びステップＳ３の処理が特徴点移動制御手段に対応し、ステップＳ１及びステップＳ３で計測データを収集する処理及びステップＳ２及びステップＳ４の処理が相対関係検出手段に対応している。
また、図７のステップＳ２の処理が原点座標検出手段に対応し、ステップＳ４の処理が姿勢検出手段に対応し、ステップＳ５で剛体変換行列を算出する処理が剛体変換行列算出手段に対応している。

１ ステレオカメラ
２ ロボットアーム装置
２ａ アーム
２ｂ ハンド
４ 制御装置
５ キャリブレーション冶具

Claims (7)

1. ロボット本体と、
   前記ロボット本体を異なる複数の視点から撮影する多視点カメラと、
   キャリブレーション冶具を用いてキャリブレーションを行なうと共に、前記多視点カメラの撮像画像に基づき前記ロボット本体を駆動制御する制御手段と、を備えたロボットシステムにおいて、
   前記キャリブレーション冶具は特徴点を有し且つ前記ロボット本体に固定され、
   前記制御手段は、前記ロボット本体を制御し、前記キャリブレーション冶具上の前記特徴点を、予め設定した相対関係検出条件を満足する複数の地点に移動させる特徴点移動制御手段と、
   前記キャリブレーション冶具上の同一特徴点の、前記複数の地点でのカメラ座標系における位置座標を検出し、当該位置座標に基づき、前記キャリブレーション冶具が固定される前記ロボット本体における部位に設定された座標系と一致する冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を検出する相対関係検出手段と、を備えることを特徴とするロボットシステム。
2. 前記相対関係検出手段は、前記カメラ座標系における前記特徴点の位置座標に基づき、前記カメラ座標系における前記冶具座標系の原点位置を検出する原点座標検出手段と、
   前記カメラ座標系における前記冶具座標系の姿勢を検出する姿勢検出手段と、を有し、
   前記カメラ座標系における前記冶具座標系の原点の位置及び姿勢と前記特徴点の位置座標とに基づき、前記冶具座標系と前記特徴点との相対関係を検出することを特徴とする請求項１記載のロボットシステム。
3. 前記特徴点移動制御手段は、前記同一特徴点が、前記冶具座標系の原点を中心とする球の表面に分布するように前記ロボット本体を制御し、
   前記原点座標検出手段は、同一平面上にない少なくとも４地点での、前記カメラ座標系における前記同一特徴点の位置座標に基づき、前記カメラ座標系における前記冶具座標系の原点の位置座標を演算することを特徴とする請求項２記載のロボットシステム。
4. 前記特徴点移動制御手段は、前記同一特徴点が、前記冶具座標系の原点を通る少なくとも２本の直線上に分布するように前記ロボット本体を制御し、
   前記原点座標検出手段は、前記冶具座標系の原点を通る同一直線上の地点での、前記カメラ座標系における前記同一特徴点の位置座標に基づき、前記直線の方程式をそれぞれ演算し、前記複数の直線の交点座標から、前記カメラ座標系における前記冶具座標系の原点の位置座標を演算することを特徴とする請求項２記載のロボットシステム。
5. 前記姿勢検出手段は、前記特徴点移動制御手段により前記ロボット本体を制御したときの、前記キャリブレーション冶具上の任意の特徴点の冶具座標系における移動ベクトルと、前記冶具座標系における移動ベクトルに対応する前記カメラ座標系における同一特徴点の移動ベクトルとの対であって、座標系ごとの移動ベクトルが平行ではない少なくとも２対の移動ベクトルを参照対とし、
   当該参照対に基づき前記カメラ座標系における前記冶具座標系の姿勢を演算することを特徴とする請求項２から請求項４の何れか１項に記載のロボットシステム。
6. 前記制御手段は、前記相対関係検出手段により検出された前記冶具座標系と前記特徴点との相対関係に基づいて求めた前記特徴点のロボット座標系における位置座標と、前記カメラ座標系における特徴点の位置座標とを用いて、前記カメラ座標系と前記ロボット座標系との剛体変換行列を求める剛体変換行列算出手段を、備えることを特徴とする請求項１から請求項５の何れか１項に記載のロボットシステム。
7. ロボット本体と、
   前記ロボット本体を異なる複数の視点から撮影する多視点カメラと、
   キャリブレーション冶具を用いてキャリブレーションを行なうと共に、前記多視点カメラの撮像画像に基づき前記ロボット本体を駆動制御する制御手段と、を備えたロボットシステムのキャリブレーション方法において、
   前記キャリブレーション冶具は特徴点を有し且つ前記ロボット本体に固定され、
   前記ロボット本体を制御して前記キャリブレーション冶具上の特徴点を、予め設定した相対関係検出条件を満足する複数の地点に移動させるステップと、
   前記キャリブレーション冶具上の同一特徴点の、前記複数の地点でのカメラ座標系における位置座標を検出し、検出した位置座標に基づき、前記キャリブレーション冶具が固定される前記ロボット本体における部位に設定された座標系と一致する冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を検出するステップと、
   前記冶具座標系と前記キャリブレーション冶具上の前記特徴点との相対関係を用いて前記キャリブレーションを行なうステップと、を備えることを特徴とするロボットシステムのキャリブレーション方法。

Images