３次元計測装置及びその制御方法
【課題】コード化パターンの投影を用いた３次元位置計測を高速に行なえるようにする。
【解決手段】計測対象物体の位置姿勢を計測する３次元計測装置は、計測対象物体に投影するパターンを生成し、生成された複数種類のパターンを投影部により計測対象物体に投影して撮像部により撮影するとともに、投影したパターンと計測対象物体の形状モデルと計測対象物体の概略位置姿勢を示す情報とに基づいて、撮像により取得される撮像画像上のパターンの座標値を算出する。そして、算出された座標値を利用して、撮像画像上の撮影されたパターンと前記投影部により投影されたパターンとを対応づけし、計測対象物体に投影されたパターンと撮像部との距離を求める。また、求めた距離と当該計測対象物体の形状モデルとを用いて計測対象物体の位置姿勢を推定し、概略位置姿勢の情報を更新する。
【解決手段】計測対象物体の位置姿勢を計測する３次元計測装置は、計測対象物体に投影するパターンを生成し、生成された複数種類のパターンを投影部により計測対象物体に投影して撮像部により撮影するとともに、投影したパターンと計測対象物体の形状モデルと計測対象物体の概略位置姿勢を示す情報とに基づいて、撮像により取得される撮像画像上のパターンの座標値を算出する。そして、算出された座標値を利用して、撮像画像上の撮影されたパターンと前記投影部により投影されたパターンとを対応づけし、計測対象物体に投影されたパターンと撮像部との距離を求める。また、求めた距離と当該計測対象物体の形状モデルとを用いて計測対象物体の位置姿勢を推定し、概略位置姿勢の情報を更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、計測対象物体の位置姿勢を計測する３次元計測装置及びその制御方法に関する。
【背景技術】
【０００２】
撮像装置による３次元計測技術には、計測対象物体へ構造化された光を投影することにより明確に対応点の決定が行えるようにする能動型ステレオ法がある。能動型ステレオ法は、物体表面のテクスチャの有無による影響が少ない方法である。能動型ステレオ法の一つであるコード化パターン光投影法では、ｎ回の光の投影と画像の撮影により２ｎ通りの空間コードへの分割が可能となり、高精度計測を高速に行うことができる。コード化の方法としては非特許文献１に開示されているグレーコードが用いられることが多い。
【０００３】
一方、計測対象物体表面の形状モデルが既知で、撮像装置から計測対象物体表面の複数の距離が点群として求められた場合に、点群から最近傍の点との距離の差を小さくするように位置姿勢を推定する方法が非特許文献２で示されている。この方法は、ＩＣＰ（Iterative Closest Point）法として距離計測と形状モデルの位置合わせの手法として広く利用されている。同様に非特許文献３は、計測点群と形状モデルの面との距離を計算するもので、計測対象物体の形状モデルと計測点群の誤差を最小にするように計測対象物体の位置姿勢を推定することができる。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】James R. Bitner, Gideon Erlich, and Edward M. Reingold. “Efficient generation of the binary-reflected Gray code and its applications.” Communications of the ACM, 19(9), pp.517-521, 1976.
【非特許文献２】P.J. Besl and N. D. Mckay. “A method for registration of 3-D shapes, ”IEEE Trans. Pattern Analysis and Machine Intelligence, 14(2),pp.239-256, 1992.
【非特許文献３】Y. Chen and G. Medioni, “Object modeling by registration of multiple range images,” Robotics and Automation,1991. Proceedings., 1991 IEEE International Conference on (1991),vol.3, pp.2724-2729, 1991.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
投影するパターンを投影・撮像している間に計測対象物体が動いていると、パターンの対応が取れずに３次元計測に失敗する。そのため、動く計測対象物体の３次元計測においては、距離計測のための投影・撮像の時間をできるだけ短くする必要がある。計測対象物体が動く例としては、人物の姿勢計測がある。また、投影・撮像側が動くものとしては、ロボットに搭載するビジュアルフィードバックでロボットのアームに計測装置を搭載している場合がある。一般的には、物体の動きよりも速く投影と撮像を行うことで計測物体の３次元計測が可能となる。このため、動く計測対象物体の３次元計測のためのパターンの投影・撮像をより高速に行うことが求められている。
【０００６】
本発明は、上述の課題に鑑みてなされたものであり、コード化パターンの投影を用いた３次元計測を高速に行なえるようにすることを目的とする。
【課題を解決するための手段】
【０００７】
上記の目的を達成するための本発明の一態様による３次元計測装置は以下の構成を備える。すなわち、
計測対象物体の形状モデルを保持する形状モデル保持手段と、
前記計測対象物体の概略位置姿勢を示す情報を保持する概略位置姿勢保持手段と、
明暗で構成された複数種類のパターンを前記計測対象物体に投影する投影手段と、
前記投影手段によりパターンが投影された前記計測対象物体を撮影する撮像手段と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像手段により取得される撮像画像における前記パターンの座標値を算出する投影位置演算手段と、
前記投影位置演算手段により算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像手段との距離を求める距離演算手段と、
前記距離演算手段で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算手段とを備える。
【発明の効果】
【０００８】
本発明によれば、形状モデルに基づいて算出されたパターンの位置に基づいて、撮像画像上のパターンと投影されたパターンの対応付けを行うので、対応付けが高速化され、コード化パターンの投影を用いた３次元計測を高速に行なえる。
【図面の簡単な説明】
【０００９】
【図１】第１実施形態に係る３次元計測装置の構成例を示すブロック図である。
【図２】第１実施形態に係る３次元計測装置の処理を示すフローチャートである。
【図３】グレーコードパターンの投影パターンの例を示す図である。
【図４】第１実施形態に係る投影位置演算部の内容を説明するための図である。
【図５】第１実施形態に係る位置姿勢演算部の処理を示すフローチャートである。
【図６】第１実施形態に係る投影パターン画像の例を示す図である。
【図７】第１実施形態に係るパターン生成部の投影制御機能を説明するフローチャートである。
【図８】第２実施形態における、３次元計測装置を産業用ロボットアームに装着した状態を示す図である。
【図９】第３実施形態に係る３次元計測装置の構成例を示すブロック図である。
【図１０】第３実施形態に係る形状統合部の処理を示すフローチャートである。
【図１１】第４実施形態における、位置姿勢計測方法をプログラムとして実施するための構成例を示すブロック図である。
【発明を実施するための形態】
【００１０】
以下、図面を用いて本発明の各実施形態を説明する。
【００１１】
［第１実施形態］
図１は、本発明の一実施形態に係る３次元計測装置のブロック図である。また、図１に係る処理の流れを図２に示す。以降、これらの図を用いて本実施形態の３次元計測装置１０００の各処理部について説明する。なお、本実施形態による３次元計測装置の例では、計測対象物体の形状モデルと、計測対象物体を撮像した画像と、計測対象物体の概略位置姿勢とを入力とし、推定した位置姿勢結果を出力とする。
【００１２】
Ｓ１０００において、形状モデル入力部１００は、計測対象物体１０の３次元形状であるモデル情報を入力する。モデル情報の入力方法としては、フロッピー（登録商標）ディスク、ＣＤ−ＲＯＭ、メモリカード、ハードディスク、フラッシュメモリ等の記録媒体或いは記録装置から情報を読み出せる構成を用いることが挙げられる。また、形状モデルを登録しているネットワークサーバに接続して、該当する形状モデルをダウンロードして情報を入手しても構わない。さらに、ユーザに該当するモデルの形状をキーボード、マウス等のインターフェースを利用して入力させても構わないし、候補をディスプレイに提示して選択させる方法でも構わない。形状モデル選択用の部品認識を利用しても構わない。いずれの方法においても、計測を行う対象の形状モデルが入力できさえすれば良い。
【００１３】
Ｓ１０２０において、形状モデル保持部１１０は、入力された計測対象物体１０の３次元形状のモデルを保持する。３次元形状モデルの表現手法としては、任意の３次元視点からの計測対象物体の表面の３次元座標が計算できるモデルであれば良く、モデルの表現フォーマットは問わない。形状モデル表面の３次元座標を計算するためのモデルとしては、例えば、
・３次元の頂点と、それらを結ぶ平面の情報で構成されたパッチモデル、
・表面の形状を記述する関数表現や、その係数を組み合わせた曲面関数の集合、
・空間上のボクセルのうち物体が占める領域の値を保持するモデルがあげられる。
【００１４】
Ｓ１０１０において、概略位置姿勢入力部２００は、計測対象物体１０の概略位置姿勢のデータを入力する。ここでの概略位置姿勢とは計測空間内に設置した基準座標系に対する回転３次元計測装置自由度、並進３自由度のパラメータとする。ステップＳ１０３０では、その値を本装置が利用可能な状態に値を変換して格納する。
【００１５】
計測空間における計測対象物体１０の存在する範囲は、パーツフィーダーによる物理的な接触を保持して供給されるような部品などでは位置・姿勢を範囲として規定することができる。また、別の部品認識装置により事前に登録された画像との照合結果の範囲で、位置姿勢を推定することも可能である。ここでは、概略位置姿勢は３次元空間での位置姿勢を定める６自由度による表現であることが望ましい。概略位置姿勢入力部２００の入力方式としては、各位置姿勢のパラメータを数値入力できれば良く、他のセンサと接続して信号入力しても良い。また、３次元計測装置１０００がロボットアームの先端に装着されている場合は、ロボットアームが保持する各関節のエンコーダの値を用いて順運動学による手先の位置姿勢と装着位置とのオフセットを乗じて得られる位置姿勢を利用することも可能である。ここでは、現状の３次元計測空間における計測対象物体１０の概略位置姿勢を取得できるようになっていれば構わない。
【００１６】
Ｓ１０３０において、概略位置姿勢保持部２１０は、概略位置姿勢入力部２００が入力した、計測空間の基準座標系における計測対象物体１０の概略位置姿勢を保持する。保持する方法としては、ＲＡＭ等による保持でも構わないし、不揮発性メモリや外部記憶装置・補助装置に保持しても構わない。また、概略位置姿勢の情報としては、基準座標系における位置と姿勢の６自由度であれば問題無い。既に計測物体と計測空間との拘束が規定されているならば、それを利用して基準座標系における位置と姿勢の表現に変換すれば良い。
【００１７】
Ｓ１０００〜Ｓ１０３０による事前の準備が整った状態で、ステップＳ１０４０から位置姿勢計測を開始する処理を行う。ステップＳ１０４０では、位置姿勢計測処理で用いられる変数の初期化が行われる。３次元計測装置１０００は、計測対象物体１０の位置姿勢を連続して計測することを目的としているため、以降の繰り返し計測時の計測対象物体が変わらない範囲では、ステップＳ１０５０から処理を始めれば良い。
【００１８】
Ｓ１０５０において、パターン生成部３００は、計測対象物体１０に投影するパターンを生成する。空間コードを生成する方法としては非特許文献１に開示されているグレーコードパターンを利用することができる。また、パターン投影部３１０の画素ごとの輝度が制御できるのであれば正弦波パターンを投影しても良い。空間的に離散的なサンプルを行う場合は、格子状に配置された幾何学図形を用いても構わない。すなわち、パターン生成として２次元画像として構成されるパターンを、いずれかのアルゴリズムにより生成することができれば良い。本例では、パターンは２次元の画像のデータとして取り扱い、パターンの生成方法としてはグレーコードパターンを用いる。
【００１９】
図３にグレーコードパターンの例を示す。一つの長方形は、投影パターン画像の領域を示す。白い部分が照明部分、黒い部分は非照明部分であり、投影パターンは明暗による縞状のパターンである。縦縞の模様を逐次照射し、横縞の模様を逐次照明することで、それぞれの画素値は白黒の独立したパターン列を持つことになる。これにより、投影パターン画像の座標値と、実際に計測対象物体表面の照明パターン列を照合することで、投影パターン画像の位置との対応をとることが可能となる。
【００２０】
Ｓ１０６０において、パターン投影部３１０は、Ｓ１０５０でパターン生成部３００が生成したパターンを照明装置により計測対象物体に投影する。パターン投影部３１０は、パターン生成部３００により生成された投影パターン画像を照明光により計測対象物体に投影する。照明光を投影する方法としては、液晶プロジェクタを利用することができる。または、可動する微小な鏡を集積した半導体であるＤＭＤ（Digital Micro-mirror Device）と照明を利用して、投影パターン画像を投影しても構わない。或いは、レーザ光源を動的に偏光する素子を併用した投影により投影パターンを生成してもよい。
【００２１】
Ｓ１０７０において、撮像部４００は、計測空間にある計測対象物体に投影されたパターンの投影像を、撮像素子とレンズを利用して撮影する。撮像素子としては、ＣＣＤやＣＭＯＳによる光−電子変換素子を利用するのが一般的である。レンズは、歪みを事前に校正したものを用いれば良い。撮像部４００により得られた撮像画像は、ＲＡＭ、不揮発メモリの記録素子、もしくはハードディスク、シリコンディスクの外部記録装置などを用いて一時的に保持されても良い。
【００２２】
ステップＳ１０８０において、投影位置演算部８１０は、形状モデル保持部１１０に保持された形状モデル表面のパターンが撮像部４００の撮像面に射影される射影画像座標を計算により求める。ここで、投影位置演算部８１０は、概略位置姿勢保持部２１０に保持された概略位置姿勢と、Ｓ１０５０で生成されたパターン画像の位置との対応から、射影画像座標を求める。より具体的には、投影位置演算部８１０は、パターン生成部３００が生成したパターン画像上の座標値と撮像部４００の撮像画像上に投影される位置（射影画像座標）の関係を、
・形状モデル保持部１１０が保持している計測対象物体の表面のモデルと、
・概略位置姿勢保持部２１０が保持している作業空間での位置姿勢と、
を用いて算出する。
【００２３】
図４は、パターン投影と撮像における計測対象物体との関係を模式的に示す図である。以下、図４を用いて、投影位置演算部８１０の処理について説明する。まず、パターン投影部３１０の投影照明系が理想的な点光源からパターン投影されるとして、焦点距離をｆ＝１とした仮想的な正規化投影画像面を設定し、これをパターン投影画像面と呼ぶ。パターン生成部３００は、パターン投影画像面にパターンを書き込むとパターン投影部３１０が実際の照明、レンズなどのデバイスにより計測対象物体にパターン照明を投影する。実際には、投影のレンズ歪みや理想的な点光源を用いられないことによる影響で、投影される像に歪みが生じる場合があるが、事前に光学補正するか、パターン画像を補正することで影響を低減することができる。
【００２４】
パターン投影画像面のあるパターンの座標をｍ＝［ｕ,ｖ,１］Ｔとする。計測空間の基準座標系に対する投影中心の座標系を投影座標系と定義し、その座標系を回転を３自由度、並進を３自由度の６自由度を用いて表現する。回転Ｒの表現としては、オイラー角、ロール・ピッチ・ヨー、回転軸回転角、４次元数などのいずれの表現手法を用いても構わない。ここでは、回転表現から得られる３ｘ３の回転行列Ｒを用いる。並進はｔｘ、ｔｙ、ｔｚの３成分である３ｘ１の行列ｔ＝［ｔｘ,ｔｙ,ｔｚ］Ｔを用いる。
【００２５】
計測対象物体の表面に投影されるパターン光は、基準座標系でのパターン投影位置Ｍ＝［Ｘ,Ｙ,Ｚ］Ｔに投影されるものとする。光源からパターン投影画像面のパターン座標ｍを通過する直線上にパターン投影位置Ｍが存在するものとすると、光線上の点Ｍは下記の式で表現できる。
【数１】
ここで、λは直線の方向ベクトルのスケールとなる正の値である。Ｌ＝［Ｌｘ,Ｌｙ,Ｌｚ］Ｔは、直線の方向ベクトルの成分を簡単のためにまとめたものである。
【００２６】
次に、式（１）の直線と計測対象物体の形状モデル表面との交点を求め、形状モデル表面上の投影位置Ｍとする。ここでは、形状モデル保持部１１０に保持されているＮ個の三角形パッチのそれぞれと、式（１）の直線が交差するかどうかが判定される。形状モデルのｉ番目の三角形パッチのｊ番目の頂点座標をＰｉｊ＝［ｐｘｉｊ、ｐｙｉｊ、ｐｚｉｊ］Ｔとする。形状モデルは、概略位置姿勢保持部２１０が保持する形状モデルの位置姿勢である回転Ｒｍ、並進ｔｍを使って、計測空間の基準座標系の頂点座標Ｐ’ｉｊを次式（２）のように変換する。
【数２】
【００２７】
三角形パッチの平面の向きは形状モデルにより登録されているものとする。計測空間でのｉ番目の三角形パッチの法線ベクトルｎ’ｉは形状モデルに保持されている三角形パッチの法線ベクトルｎｉに回転Ｒｍを掛けたものとなる。
【００２８】
頂点Ｐ’ｉｊと直線との交点Ｐｃｉを結ぶ線分と、当該三角形パッチの法線ベクトルｎ’ｉは直交するので、内積の関係から
【数３】
となる。式（１）を代入すると次式（４）のようになる。
【数４】
さらに、式（４）をλについてまとめると式（５）となる。
【数５】
これを、式（１）に代入して交点Ｐｃｉの座標として求めると
【数６】
となる。三角形パッチの内部に交点Ｐｃｉが存在するかどうかのチェックとして、三角形頂点と交点Ｐｃｉとの外積を計算して、それぞれが法線ベクトルｎ’ｉと同じ向きの場合に三角形の内部に点があることになる。具体的には次の３つの式が全て正の場合に、交点が三角形内部に存在する。
【数７】
【００２９】
さらに、形状モデルのパッチとの交点は複数存在する場合があるため、投影座標系原点により近い三角形パッチの交点を求める必要がある。Ｎ個の三角形パッチのうち、交点を有するパッチの中からλ＞０の条件で投影座標系原点との距離が最も近い交点を求め、その交点をパターン投影位置Ｍとすればよい。
【数８】
式（８）により得られた形状モデル表面の三角形パッチとの交点が形状モデル表面における推定投影点となる。
【００３０】
次に、撮像部４００が計測対象物体表面のパターンを撮像しているとする。撮像部の撮像系をカメラとして中心射影のモデルとして考える。射影画像面までの焦点距離をｆ’とする。さらに、計測対象物体の表面に投影されているパターン投影位置Ｍが撮像画像上に射影されているとする。その撮像画像上の射影パターン位置をｍ’＝［ｕ’,ｖ’,１］Ｔとする。撮像カメラのレンズ中心を基準座標系における回転と並進の６自由度で表現とし、それぞれＲ’とｔ’の行列として表現すると中心射影の関係から次式（９）のようになる。
【数９】
なお、ｓ’はスケール要素のスカラー値である。
【００３１】
ここでは、パターン投影部３１０のレンズの座標系と撮像部４００の座標系が固定されているとし、事前にそのローカル座標変換は校正手続きにより求まっているとする。具体的な校正手続きは、校正指標を複数撮影して複数の校正指標座標値を用いて最小自乗法により求めることができるし、他の方法で校正しても構わない。ローカル座標変換行列をＲＬとｔＬとすると、下式の関係となる。
【数１０】
【００３２】
式（８）により得られた形状モデル表面のパターン投影位置Ｍと式（１０）を式（９）に代入して、撮像画像上の射影パターン位置ｍ’を得る。
【数１１】
【００３３】
撮像画像上の射影パターン位置ｍ’は、計測対象物体１０の位置姿勢と概略位置姿勢保持部２１０の位置姿勢が一致しているときに、実際の照明装置による投影パターンを撮像したときの座標値と一致する。概略位置姿勢が計測対象物体１０の位置姿勢と僅かに異なっている場合には、投影位置演算部８１０が算出した射影パターン位置ｍ’の画面上の近傍領域に実投影パターンがある。そこで、投影位置演算部８１０は、距離演算部６００に対して照合領域の範囲を指示する。距離演算部６００は、指示された照合領域の範囲、すなわち算出した射影パターン位置ｍ’の近傍領域におけるパターン探索を実行してパターンの対応づけが可能となる。
【００３４】
図２に戻り、Ｓ１０９０において、距離演算部６００は、Ｓ１０７０における撮影で得られた撮像画像からＳ１０８０で推定されたパターン射影画像座標の画素近傍から対応するパターンを探索する。そして、投影系と撮像系とのステレオ計測により各パターンと撮像部４００との距離を計算する。すなわち、距離演算部６００は、撮像部４００が取得した計測対象物体に投影されたパターン（撮影されたパターン）とパターン投影部３１０により投影されたパターンとを対応付け、投影パターン画像の位置と撮影画像の位置との対応を求める。そして、その対応から上記距離を求める。ここで距離演算部６００が対応するパターンを探索する照合領域の範囲は、上述したように投影位置演算部８１０の演算結果により限定されるため、処理速度が向上する。
【００３５】
なお、グレーコードパターンを利用する場合は、白・黒に投影されたパターンをビットとしてみなし、そのエンコードされた符号値とパターン生成時の符号値を照合し、投影パターン画像の位置を決定できる。
【００３６】
距離演算部６００で利用するパターン投影部３１０と撮像部４００とのローカル座標変換行列が事前の校正手続きにより規定されているものとする。そのため、互いに対応する投影されたパターンと撮影されたパターンの座標値を用いて、ステレオ測量により撮像座標系における距離を算出することができる。
【００３７】
一方、位置姿勢演算部７００は、距離演算部６００で求めた距離と形状モデル保持部１１０に保持されている形状モデルを用いて計測対象物体１０の位置姿勢を推定し、概略位置姿勢保持部２１０に保持されている概略位置姿勢の情報を更新する。以下、この処理を説明する。まず、実際に照明されたパターンを撮像し、検出された座標を実投影パターンの撮像画像上の位置を実射影パターン位置ｍ’’とする。パターン位置ｍとローカル座標変換行列ＲＬ、ｔＬ、そして実パターン投影位置Ｍ'の関係は次式（１２）のようになる。
【数１２】
Ｍ’は、左辺の行列式の一般化逆行列を両辺に掛け合わせることで求めることができる。
【００３８】
Ｓ１１１０において位置姿勢演算部７００は、実射影パターン位置と算出した射影パターン位置との差が小さくなるように位置姿勢の推定演算を行う。位置姿勢の推定では、姿勢に対して非線形な計算を伴うため、基本的には繰り返し誤差最小化を適用する。求めようとする計測対象物体の位置姿勢を未知数として、形状モデルの表面の各パターン投影位置から算出した射影パターン位置と観測された実射影パターン位置との距離の二乗の和が最小になるように、未知パラメータである位置姿勢の推定値を更新する。具体的には非特許文献２に開示されるＩＣＰ（Iterative Closest Point）法や、非特許文献３に開示される方法を利用して、形状モデルと計測距離から当該形状モデルの位置姿勢を求めることができる。その際に、初期値としては概略位置姿勢保持部２１０の値を使うことができる。
【００３９】
なお、投影位置演算部８１０では、形状モデルの投影と射影の関係があるパターンのみ使うことができるため、他の物体や背景の領域に対する距離計測の結果を含めないことができる。そのため、位置姿勢演算部７００での形状モデル以外の例外値が無いため、ロバストな位置姿勢演算を行うことができる。
【００４０】
また、位置姿勢演算部７００では、繰り返し演算の結果で生じる残差（実射影パターン位置と更新後の概略位置姿勢で投影位置演算部８１０が算出した射影パターン位置との差）から最適化計算の繰り返しを継続するか否かを判定する。この判定のための閾値としては、ユーザが設定した閾値でもよいし、作業空間における観測面積と撮像装置との距離との関係を用いて設定された閾値でもよい。さらに、３次元計測装置１０００が既定値を保持していて、それを読み出して閾値としても構わない。
【００４１】
いずれかの方法により設定された閾値と残差の値を比較するが、実際には演算後の残差が小さくならない場合もある。そのため、本実施形態では、繰り返し回数を保持しておき、残差の繰り返しごとのトレンドを計算し、繰り返し回数が既定回数を越えた場合もしくはトレンドが増加する傾向にある場合には、位置姿勢演算部７００の最適化演算の繰り返しを停止するようにする。これにより、位置姿勢の最適化演算がうまくいったか否かを判定することができる。
【００４２】
図５を用いて位置姿勢演算部７００の処理について説明をする。最初にＳ１０において、位置姿勢演算部７００は、演算の繰り返し回数の値を０とし、移動平均値の値を初期化する。Ｓ１００において、位置姿勢演算部７００は、距離演算部６００による距離計算結果を得る。Ｓ１１０において、位置姿勢演算部７００は、計測対象物体１０の位置姿勢を最適化計算し、パラメータを更新する。Ｓ１２０において、位置姿勢演算部７００は、位置姿勢の更新した値と残差を取得する。Ｓ１３０において、位置姿勢演算部７００は、Ｓ１２０で取得された残差が既定した閾値以下であるかどうかを判断する。残差が閾値以下の場合は位置姿勢の推定結果が想定する精度を有しているものとして、Ｓ１４０において最適化演算成功のフラグを設定して、繰り返しを停止する。Ｓ１３０において残差が閾値を上回る場合は、位置姿勢の推定の精度が不十分ということでＳ１５０に処理を移す。
【００４３】
Ｓ１５０において、位置姿勢演算部７００は、残差が減少しているか増加しているかのトレンドを計算するため、残差の移動平均を計算する。移動平均のフレーム数ｎはユーザが指定してもよいし、実験的に求めた値を設定しておけば良い。Ｓ１６０において、位置姿勢演算部７００は、その移動平均値が前回計算した移動平均値よりも大きくなる傾向を示す場合には、最適化演算の誤差最小化計算が発散しているとして、Ｓ１７０の最適化が失敗したフラグを設定して繰り返しを停止する。Ｓ１６０において、繰り返しにより誤差が減少する場合は、再度繰り返し演算が必要としてＳ１８０に移る。Ｓ１８０では、位置姿勢演算部７００は、全体の繰り返し回数を保持している値に１を加える。次に、Ｓ１９０において、位置姿勢演算部７００は、繰り返し回数が既定回数を越えたかどうかを判定する。繰り返し回数が既定回数以内であれば、位置姿勢演算部７００は、再度繰り返しの最適化計算を実行するべく処理をＳ１１０へ戻す。一方、既定回数を越えたと判定された場合は、Ｓ２００において、位置姿勢演算部７００は、繰り返し回数を越えたことを示すフラグを設定して、当該繰り返しを停止する。
【００４４】
図２に戻り、Ｓ１１３０において、位置姿勢演算部７００は推定位置姿勢結果を用いて概略位置姿勢保持部２１０の値を更新する。３次元計測装置１０００が連続して計測対象物体１０の位置姿勢を計測している場合は、位置姿勢演算部７００による位置姿勢の推定結果で概略位置姿勢保持部２１０の値を更新することで、連続したフレーム間での計測対象物体の位置姿勢推定が可能となる。
【００４５】
Ｓ１１２０において、パターン生成部３００は、Ｓ１１１０における位置姿勢演算部７００による位置姿勢の推定の状態をうけて、ステップＳ１０５０の投影パターン生成のパラメータを更新する。具体的には、Ｓ１４０，Ｓ１７０，Ｓ２００で設定されるフラグが推定状態として用いられる。推定状態が良い場合（例えば、最適化演算成功のフラグが設定されている場合）は、投影・撮像回数を減らすように、推定状態が悪い場合（例えば、最適化の失敗を示すフラグが設定されている場合）は投影・撮像回数を増やすように投影制御を行う。すなわち、パターン生成部３００における投影制御機能（Ｓ１１２０）では、推定状態の推定結果が良い場合はグレーコードパターンの投影画像の数を減らし、推定結果が悪い場合はグレーコードパターンの投影画像の数を増やすように制御する。
【００４６】
さらに、パターン生成部３００の投影制御機能は、概略位置姿勢保持部２１０に保持されている情報によって示される計測対象物体１０の位置姿勢の変化に応じて投影するパターンの縞の間隔を変更する。すなわち、パターン生成部３００は、位置姿勢の変化が大きいときは、投影するパターンの間隔を大きくするようにし、変化が小さいときはパターンの間隔を小さくするように制御する。なお、概略位置姿勢保持部２１０が保持する情報によって示される計測対象物体１０の位置姿勢の変化とは、計測対象物体１０か３次元計測装置１０００のどちらか、もしくは両方が動いているときの動きを反映する。
【００４７】
図６に投影するパターン画像列を示す。ＰＰ［ｎ］は、ｎ番目の投影する投影パターンを示す。ここでの実施形態としては、ｎが奇数の場合は縦縞、ｎが偶数の場合は横縞にしている。これら縞状のパターンは明暗により形成されている。図３における縞の間隔が小さいほうから画像列を保持する。投影される順序は［ｎ］の順、すなわち縞の間隔が小さいほうから投影されることになる。
【００４８】
図７に第１実施形態によるパターン生成部３００の投影制御機能の処理を示す。以下では、パターンの総数を２ｎとし、縦縞と横縞のペアがｎ組存在するものとする。ここで、Ｗｉ、Ｈｉは整数値とする。Ｗｉはｉ番目に投影するパターンにおけるＰＰ［ｎ］のパターン画像のペアの数を意味する。Ｗｉがｎの場合は、グレーコードパターンの全てのパターン画像を順次投影することを意味し、通常のグレーコードパターンによるパターン光投影法になる。Ｈｉはｉ番目におけるＰＰ［ｎ］のパターン画像列の開始するペアの順位を意味する。Ｈｉが大きいほど、より投影するパターン縞の幅が相対的に大きいものから順次投影することとなる。すなわち、パターン投影部３１０は、パターン間隔を広くしたパターンを照射することになる。物体・装置が動いている時にはパターン投影位置の推定座標の精度が悪くなるが、この構成によれば、投影パターンの間隔を大きくすることにより隣接するパターンとの距離を広くすることができ、パターン識別性を向上する効果が得られる。
【００４９】
以上のようなパターン生成部３００による投影制御機能による処理について図７のフローチャートを用いて説明する。まず、Ｓ３００において、パターン生成部３００は、初期化として変数の初期化を行う。これは、投影制御機能が利用可能になるときに一度設定すれば良い。処理開始であるＳ３１０から処理終了であるＳ５００は、パターン生成部３００の一部の処理の範囲を示すもので、パターン生成部３００の全体の処理の開始・終了ではない。ステップＳ３２０において、パターン生成部３００は、処理の繰り返し回数を保持する変数ｉを１つ増加させる。
【００５０】
Ｓ３３０において、パターン生成部３００は、概略位置姿勢保持部２１０に保持されている情報が示す、更新前後の位置姿勢の変化量を算出する。たとえば、概略位置姿勢保持部２１０が６自由度のパラメータを保持している場合は、ｉ−１回目に保持していた概略位置姿勢の値と、ｉ回目の概略位置姿勢の値とのそれぞれの差の絶対値を計算し、それぞれの絶対値から最大の絶対値を求めて変化量とする。Ｓ３４０において、パターン生成部３００は、Ｓ３３０で算出した変化量と事前に設定した閾値とを比較して、変化量が閾値より大きい場合はＳ３７０へ、小さい場合はＳ３８０へ処理を進める。Ｓ３７０では、パターン生成部３００は、前回のＨｉの値（Ｈｉ−１）に１を加えて今回のＨｉの値とする。ステップＳ３８０では、パターン生成部３００は、前回のＨｉの値（Ｈｉ−１）から１を引いた値を今回のＨｉとして設定する。これにより、物体・装置の動きが大きい場合は縞の間隔が大きいパターンを用いた投影が行われ、動きが小さい場合は縞の間隔が小さいパターンを用いた投影が行われるようになる。
【００５１】
一方、Ｓ４１０において、パターン生成部３００は位置姿勢演算部７００の推定状態を取得し、Ｓ４２０で推定状態の判別を行う。最適化演算成功のフラグが立っている場合は、Ｓ４５０に移行し、最適化演算成功のフラグが立っていない（最適化演算が成功していない）場合はＳ４６０に移行する。Ｓ４５０において、パターン生成部３００は、前回のＷｉの値（Ｗｉ−１）から１を引き、今回のＷｉの値とする。また、Ｓ４６０では、前回のＷｉ（Ｗｉ−１）の値に１を加えて、今回のＷｉの値とする。これは、最適化演算が成功している場合には、撮影するパターンの画像セット数を減らし、失敗している場合は撮影するパターンの画像セット数を増やすように作用する。ステップＳ４００では、ＰＰ［（２×Ｈｉ）−１］から順にＰＰ［２×（Ｈｉ＋Ｗｉ）−２］までのＷｉ組のペアとなるパターン画像をパターン投影部３１０に指示する。ただし、ＰＰ［２×（Ｈｉ＋Ｗｉ）−２］はＰＰ［２ｎ］までしかデータが用意されていないので、Ｈｉが大きくなると、投影されるパターンはＷｉ組よりも少なくなってしまう。そこで、投影されるパターン数を優先させたい場合は、ＰＰ［（２×Ｈｉ）−１］よりも前の順位の画像から始めてもよい。
【００５２】
基本的には最適化演算が成功している場合は、符号化長を減らしても投影位置演算部８１０による撮像画像座標の推定精度が高いため、パターンの対応づけができる。図７で説明した一実施形態ではパターンの投影数をＷｉとして、その値を１ずつ増減することを行っていたが、その増減の値は１に限らず、増減幅は一定でなくても良い。
【００５３】
なお、計測対象物体と撮像部４００との相対運動が大きい場合は、投影位置演算部８１０による推定射影パターン位置と、実際の射影パターン位置との差が大きい。しかし、その値は奥行き方向の値と関係がある。そこで、奥行き方向の距離に応じてパターンの探索領域を変更すると良い。そこで概略位置姿勢のパラメータのうち、姿勢成分と並進成分のパラメータの閾値を別々に比較しても良い。
【００５４】
以上のように、本実施形態によれば、パターン生成部３００の投影制御機能は、位置姿勢演算部７００の推定状態により適応的に投影パターンの回数や計測点を増減するため、推定状態が良い場合は投影回数を減らすことが可能となる。そのため、全体の計測時間の短縮を実現できる。すなわち、本実施形態によれば、投影されるパターンの撮像画像上における位置を形状モデルと概略位置姿勢から推定することで、空間コードのための投影・撮像回数を減らすことができ、処理の高速化が期待できる。
【００５５】
ステップＳ１１２０の機能はパターン生成部３００の投影制御機能により実現する。ステップＳ１１４０において、位置姿勢出力部９００は、Ｓ１１１０で得られた位置姿勢推定結果を本装置の外部へ出力する。出力する情報としては、計測対象物体１０の作業空間における基準座標系での位置姿勢である６自由度の値を出力しても構わないし、撮像部４００の座標系における撮像座標系の原点の位置姿勢を含めて位置姿勢を出力しても構わない。
【００５６】
なお、本発明の一実施形態である３次元計測装置１０００では、独立した装置を利用しても構わない。例えば、パターン投影部３１０には、市販の液晶プロジェクタを利用することができる。撮像部４００には、市販のカメラとレンズを利用することが可能である。３次元計測装置１０００とは、それぞれの装置で利用可能な信号を出力するための機構と、実際の信号を受け取るための入出力部分があれば良い。そのようにすれば、利用者の用途や計測範囲に応じて装置を拡張して利用することができる。
【００５７】
［第２実施形態］
第１実施形態で説明した３次元計測装置１０００の好適な適用事例としては、産業用ロボットアームの手先部に設置して、把持する物体を計測対象物体として、その位置姿勢を計測する目的に利用することがあげられる。以下、図８を用いて３次元計測装置１０００の適用例を説明する。
【００５８】
産業用ロボットアーム３０は、ロボットコントローラ４０の制御による駆動する。本発明の一実施形態である３次元計測装置１０００がアーム腕部に治具により固定されている。産業用ロボットアーム３０の動作は対象物体１１をアーム把持部によりピッキングする。３次元計測装置１０００は、アーム把持部と対象物体１１の相対位置姿勢を逐次推定し、その推定結果をロボットコントローラ４０の制御パラメータとして伝えるためにケーブルにより結線している。
【００５９】
３次元計測装置１０００における概略位置姿勢入力部２００へは、ロボットコントローラ４０が制御する産業用ロボットアーム３０の取り付け位置に、治具のためのオフセットを加えた値を伝えれば良い。また、形状モデル入力部１００には、対象物体１１のＣＡＤモデルを本装置１０００が読み込めるフォーマットに変換したものを入力しておく。なお、対象物体が変わった場合には、その都度、形状モデル入力部１００に該当する形状モデルを入力することで、複数種類の対象物体に対応することができる。装置が事前の保持している形状モデルを選択しても構わないし、別の部品識別装置による部品種の認識結果を使って保持している形状モデルを切り替えても構わない。
【００６０】
３次元計測装置１０００の概略位置姿勢を入力した対象物体１１の位置姿勢が実際の作業空間での配置と異なる場合には、位置姿勢演算部７００での位置姿勢推定の残差が大きくなる。すると、パターン生成部３００が、投影・撮像回数を増加する。それにより、計測対象物体の距離計測と位置姿勢が可能となる。一度精度の高い位置姿勢が行えれば、概略位置姿勢保持部２１０に反映される。
【００６１】
さらに、産業用ロボットアーム３０の手先が対象物体１１に近づくための動作を行うとする。実施のロボットアームは、関節角度のエンコーダ値のノイズや、ロボット自身の自重による撓みにより、エンコーダによる手先位置の作業空間内での絶対位置精度はそれほど高くない場合が多い。概略位置姿勢入力部２００にはロボットの運動した手先位置が入力される。３次元計測装置１０００は、計測のためのパターン投影と撮像を行い、位置姿勢推定結果を出力する。この場合は、ロボット手先位置計算の計算誤差を補正することが主になる。そのため、位置姿勢演算部７００での推定状態が最適化演算成功となる。そこで、パターン生成部３００は投影・撮像の回数を減らすための投影制御を行う。このようにして、最初の概略位置姿勢を推定するときには撮影回数が増加するが、精度の高い位置姿勢が得られれば、それ以降の計測のための撮影回数を減らせることとなり、連続した計測時の投影・撮像の回数が減り計測のタクトタイムが短縮される。
【００６２】
［第３実施形態］
本発明では計測対象物体の形状モデルが未知の場合においても、本発明の一実施形態である装置構成を利用して、基本的には計測対象物体表面の３次元形状を取得することができる。未知の計測対象物体の形状モデルを複数の視点からの画像を使って取得する手続きを加えることで、さらに本発明の適用の範囲を広げることができる。
【００６３】
図９は、第３実施形態による３次元計測装置１０１０の構成例を示すブロック図である。３次元計測装置１０１０は、第１実施形態の３次元計測装置１０００の構成に計測モデル生成部１２０を付加したものである。計測モデル生成部１２０は、形状モデルが無いモデルに対して本装置を適用する際にその機能を発揮する。以下、３次元計測装置１０１０の動作について図１０のフローチャートを用いて説明する。
【００６４】
Ｓ２０００において、３次元計測装置１０１０は、形状モデル利用モードか計測モデル取得モードかをユーザに選択させる。選択方法としては、ボタンやスイッチを用いれば良い。Ｓ２０１０において、３次元計測装置１０１０は、ユーザの入力を判別して形状モデル利用モードかどうかを判定する。Ｙｅｓの場合（形状モデル利用モードの場合）は、Ｓ２０２０へ処理が進む。Ｓ２０２０では、形状モデル保持部１１０に既に登録されている、または形状モデル入力部１００が入力した既存の形状モデルを利用した位置姿勢計測が実行される。すなわち、図１のＳ１０００からの上述した処理により計測対象物体の位置姿勢計測を行う。一方、Ｓ２０１０でＮｏの場合（計測モデル取得モードの場合）は、Ｓ２０４０へ進み、計測モデル生成部１２０の制御下で計測モデル撮影の処理を開始する。なお、最初期においては、最初の撮影時の位置姿勢を基準とするように概略位置姿勢保持部２１０の内容や位置姿勢演算部７００の状態をセットする。
【００６５】
Ｓ２０５０において、計測モデル生成部１２０は、計測対象物体の概略位置姿勢（概略位置姿勢保持部２１０の内容）を取得する。例えば、計測対象物体を回転板の上に設置しているならば、その回転角度から計算される位置姿勢を取得することができる。また、本計測装置が第２実施形態で述べた産業用ロボットアームのアーム部に設置している場合は、ロボットの手先位置を取得することができる。次に、Ｓ２０６０において、計測モデル生成部１２０は、パターン生成部３００に対して全ての投影パターンを投影して距離を計測するように指示する。Ｓ２０７０において、距離演算部６００は全ての距離計測結果を計測モデル生成部１２０に送る。
【００６６】
形状モデルは複数の視点から観察した計測対象物体表面までの距離データを統合することにより得られるので、Ｓ２０８０において計測モデル生成部１２０は、計測対象物体１０を動かすか、３次元計測装置１０１０自体を動かす。なお、その時の移動量は既知とする。Ｓ２０９０において、計測モデル撮影を終了するかどうかを判断する。判断する方法としては、既定回数の撮影が終了した場合やユーザによる指示などを用いてよい。こうして、３次元計測装置１０１０（パターン投影部３１０と撮像部４００）と計測対象物体とを複数の既知の位置関係に配置して行われた距離計測によって複数の距離データが得られる。
【００６７】
Ｓ２１００において、計測モデル生成部１２０は、距離演算部６００より提供された複数の計測距離画像を統合する。統合する方法としては、例えば、重複する距離点群の領域の幾何学的な距離が最小になるように概略位置姿勢を用いて誤差最小化計算を行って統合することがあげられる。Ｓ２１１０において、計測モデル生成部１２０は、統合された計測点群から計測対象物体の領域をユーザに選択させる。例えば、統合された距離画像をユーザが視認できるように表示して、マウスなどで指定させればよい。実際に計測対象物体と規定しているのはユーザであるため、領域を選択してもらうことが望ましい。
【００６８】
Ｓ２１２０において、計測モデル生成部１２０は、統合した距離点群からＳ２１１０で選択された領域を抽出して、形状モデルに変換して登録する。点群から形状モデルである三角形パッチを生成する方法はマーチングキューブ法などを用いれば良い。Ｓ２１３０では、計測モデル生成部１２０が生成した形状モデルを形状モデル保持部１１０に格納し、図１に示すＳ１０４０からの処理を行い、第１実施形態と同様の位置姿勢計測を開始する。
【００６９】
上述の計測モデル生成部を有する３次元計測装置１０１０を用いれば、形状モデルが事前に用意できない場合においても、本装置を利用することが可能となる。
【００７０】
［第４実施形態］
第１、第３実施形態で説明した各処理部はコンピュータのプログラムとしても実行できる。図１１は、パーソナルコンピュータ２１００に撮像部４００としてのカメラ４０１とパターン投影部３１０としてのプロジェクタ３１１を、Ｉ／Ｏインターフェース２１０１、ビデオインターフェース２１０３を介して接続した例を示す。ＣＰＵ２１０２は、上述した実施形態の機能をコンピュータに実行させるプログラムコード２１０５をメモリから読み出して実行することにより、当該コンピュータを３次元計測装置１０００（１０１０）として機能させる。プログラムコード２１０５は、プログラムメモリ或いは不図示の外部記憶装置からＲＡＭ２１０４にロードされ、実行されるようにしてもよい。入力部２１０６は、キーボード、マウス等で構成される。
【００７１】
その際、パーソナルコンピュータのバスや周辺機器を効率的に利用するための仕組みとしてオペレーティングシステムを利用しても構わない。オペレーティングシステムは、本発明の機能を実装したプログラムとは別に提供されていても問題ないし、オペレーティングシステムにおける一つのアプリケーションプログラムとして、本発明の機能を実装した処理プログラムを選択して実行しても構わない。
【００７２】
［第５実施形態］
３次元計測装置１０００の投影位置演算部８１０は、形状モデル保持部１１０の三角形パッチの数が増加すると処理量が増加する。しかしながら、三角形パッチの探索を効率化するために、事前に三角形パッチを投影座標系視点に対してソートしておく方法により処理を高速化できる。
【００７３】
さらに、処理の高速化として３次元のコンピュータグラフィックスをレンダリングするハードウェアである不図示のグラフィックスプロセッサユニットを用いることができる。グラフィックスプロセッサユニットとしては、Ｚバッファからの値を読み出すことができるハードウェアを使えば良い。グラフィックスプロセッサユニットを利用して、レンダリング時のＺバッファの値から、投影座標原点に近い平面までの距離を求めることができる。投影座標系原点を視点とした形状モデルを正規化投影画像平面と一致するようにレンダリングして、パターン座標に相当するＺバッファを求め、その奥行き値を含めて座標を基準座標系へ変換すれば良い。また、同様に、投影画像上の射影パターン位置ｍ’の計算を行う式（１１）にも適用できる。形状表面上の投影パターンＭを撮像座標系に合わせた射影面に投影する処理をグラフィックスプロセッサの射影変換機能を用いて求めれば高速に演算できる。
【技術分野】
【０００１】
本発明は、計測対象物体の位置姿勢を計測する３次元計測装置及びその制御方法に関する。
【背景技術】
【０００２】
撮像装置による３次元計測技術には、計測対象物体へ構造化された光を投影することにより明確に対応点の決定が行えるようにする能動型ステレオ法がある。能動型ステレオ法は、物体表面のテクスチャの有無による影響が少ない方法である。能動型ステレオ法の一つであるコード化パターン光投影法では、ｎ回の光の投影と画像の撮影により２ｎ通りの空間コードへの分割が可能となり、高精度計測を高速に行うことができる。コード化の方法としては非特許文献１に開示されているグレーコードが用いられることが多い。
【０００３】
一方、計測対象物体表面の形状モデルが既知で、撮像装置から計測対象物体表面の複数の距離が点群として求められた場合に、点群から最近傍の点との距離の差を小さくするように位置姿勢を推定する方法が非特許文献２で示されている。この方法は、ＩＣＰ（Iterative Closest Point）法として距離計測と形状モデルの位置合わせの手法として広く利用されている。同様に非特許文献３は、計測点群と形状モデルの面との距離を計算するもので、計測対象物体の形状モデルと計測点群の誤差を最小にするように計測対象物体の位置姿勢を推定することができる。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】James R. Bitner, Gideon Erlich, and Edward M. Reingold. “Efficient generation of the binary-reflected Gray code and its applications.” Communications of the ACM, 19(9), pp.517-521, 1976.
【非特許文献２】P.J. Besl and N. D. Mckay. “A method for registration of 3-D shapes, ”IEEE Trans. Pattern Analysis and Machine Intelligence, 14(2),pp.239-256, 1992.
【非特許文献３】Y. Chen and G. Medioni, “Object modeling by registration of multiple range images,” Robotics and Automation,1991. Proceedings., 1991 IEEE International Conference on (1991),vol.3, pp.2724-2729, 1991.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
投影するパターンを投影・撮像している間に計測対象物体が動いていると、パターンの対応が取れずに３次元計測に失敗する。そのため、動く計測対象物体の３次元計測においては、距離計測のための投影・撮像の時間をできるだけ短くする必要がある。計測対象物体が動く例としては、人物の姿勢計測がある。また、投影・撮像側が動くものとしては、ロボットに搭載するビジュアルフィードバックでロボットのアームに計測装置を搭載している場合がある。一般的には、物体の動きよりも速く投影と撮像を行うことで計測物体の３次元計測が可能となる。このため、動く計測対象物体の３次元計測のためのパターンの投影・撮像をより高速に行うことが求められている。
【０００６】
本発明は、上述の課題に鑑みてなされたものであり、コード化パターンの投影を用いた３次元計測を高速に行なえるようにすることを目的とする。
【課題を解決するための手段】
【０００７】
上記の目的を達成するための本発明の一態様による３次元計測装置は以下の構成を備える。すなわち、
計測対象物体の形状モデルを保持する形状モデル保持手段と、
前記計測対象物体の概略位置姿勢を示す情報を保持する概略位置姿勢保持手段と、
明暗で構成された複数種類のパターンを前記計測対象物体に投影する投影手段と、
前記投影手段によりパターンが投影された前記計測対象物体を撮影する撮像手段と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像手段により取得される撮像画像における前記パターンの座標値を算出する投影位置演算手段と、
前記投影位置演算手段により算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像手段との距離を求める距離演算手段と、
前記距離演算手段で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算手段とを備える。
【発明の効果】
【０００８】
本発明によれば、形状モデルに基づいて算出されたパターンの位置に基づいて、撮像画像上のパターンと投影されたパターンの対応付けを行うので、対応付けが高速化され、コード化パターンの投影を用いた３次元計測を高速に行なえる。
【図面の簡単な説明】
【０００９】
【図１】第１実施形態に係る３次元計測装置の構成例を示すブロック図である。
【図２】第１実施形態に係る３次元計測装置の処理を示すフローチャートである。
【図３】グレーコードパターンの投影パターンの例を示す図である。
【図４】第１実施形態に係る投影位置演算部の内容を説明するための図である。
【図５】第１実施形態に係る位置姿勢演算部の処理を示すフローチャートである。
【図６】第１実施形態に係る投影パターン画像の例を示す図である。
【図７】第１実施形態に係るパターン生成部の投影制御機能を説明するフローチャートである。
【図８】第２実施形態における、３次元計測装置を産業用ロボットアームに装着した状態を示す図である。
【図９】第３実施形態に係る３次元計測装置の構成例を示すブロック図である。
【図１０】第３実施形態に係る形状統合部の処理を示すフローチャートである。
【図１１】第４実施形態における、位置姿勢計測方法をプログラムとして実施するための構成例を示すブロック図である。
【発明を実施するための形態】
【００１０】
以下、図面を用いて本発明の各実施形態を説明する。
【００１１】
［第１実施形態］
図１は、本発明の一実施形態に係る３次元計測装置のブロック図である。また、図１に係る処理の流れを図２に示す。以降、これらの図を用いて本実施形態の３次元計測装置１０００の各処理部について説明する。なお、本実施形態による３次元計測装置の例では、計測対象物体の形状モデルと、計測対象物体を撮像した画像と、計測対象物体の概略位置姿勢とを入力とし、推定した位置姿勢結果を出力とする。
【００１２】
Ｓ１０００において、形状モデル入力部１００は、計測対象物体１０の３次元形状であるモデル情報を入力する。モデル情報の入力方法としては、フロッピー（登録商標）ディスク、ＣＤ−ＲＯＭ、メモリカード、ハードディスク、フラッシュメモリ等の記録媒体或いは記録装置から情報を読み出せる構成を用いることが挙げられる。また、形状モデルを登録しているネットワークサーバに接続して、該当する形状モデルをダウンロードして情報を入手しても構わない。さらに、ユーザに該当するモデルの形状をキーボード、マウス等のインターフェースを利用して入力させても構わないし、候補をディスプレイに提示して選択させる方法でも構わない。形状モデル選択用の部品認識を利用しても構わない。いずれの方法においても、計測を行う対象の形状モデルが入力できさえすれば良い。
【００１３】
Ｓ１０２０において、形状モデル保持部１１０は、入力された計測対象物体１０の３次元形状のモデルを保持する。３次元形状モデルの表現手法としては、任意の３次元視点からの計測対象物体の表面の３次元座標が計算できるモデルであれば良く、モデルの表現フォーマットは問わない。形状モデル表面の３次元座標を計算するためのモデルとしては、例えば、
・３次元の頂点と、それらを結ぶ平面の情報で構成されたパッチモデル、
・表面の形状を記述する関数表現や、その係数を組み合わせた曲面関数の集合、
・空間上のボクセルのうち物体が占める領域の値を保持するモデルがあげられる。
【００１４】
Ｓ１０１０において、概略位置姿勢入力部２００は、計測対象物体１０の概略位置姿勢のデータを入力する。ここでの概略位置姿勢とは計測空間内に設置した基準座標系に対する回転３次元計測装置自由度、並進３自由度のパラメータとする。ステップＳ１０３０では、その値を本装置が利用可能な状態に値を変換して格納する。
【００１５】
計測空間における計測対象物体１０の存在する範囲は、パーツフィーダーによる物理的な接触を保持して供給されるような部品などでは位置・姿勢を範囲として規定することができる。また、別の部品認識装置により事前に登録された画像との照合結果の範囲で、位置姿勢を推定することも可能である。ここでは、概略位置姿勢は３次元空間での位置姿勢を定める６自由度による表現であることが望ましい。概略位置姿勢入力部２００の入力方式としては、各位置姿勢のパラメータを数値入力できれば良く、他のセンサと接続して信号入力しても良い。また、３次元計測装置１０００がロボットアームの先端に装着されている場合は、ロボットアームが保持する各関節のエンコーダの値を用いて順運動学による手先の位置姿勢と装着位置とのオフセットを乗じて得られる位置姿勢を利用することも可能である。ここでは、現状の３次元計測空間における計測対象物体１０の概略位置姿勢を取得できるようになっていれば構わない。
【００１６】
Ｓ１０３０において、概略位置姿勢保持部２１０は、概略位置姿勢入力部２００が入力した、計測空間の基準座標系における計測対象物体１０の概略位置姿勢を保持する。保持する方法としては、ＲＡＭ等による保持でも構わないし、不揮発性メモリや外部記憶装置・補助装置に保持しても構わない。また、概略位置姿勢の情報としては、基準座標系における位置と姿勢の６自由度であれば問題無い。既に計測物体と計測空間との拘束が規定されているならば、それを利用して基準座標系における位置と姿勢の表現に変換すれば良い。
【００１７】
Ｓ１０００〜Ｓ１０３０による事前の準備が整った状態で、ステップＳ１０４０から位置姿勢計測を開始する処理を行う。ステップＳ１０４０では、位置姿勢計測処理で用いられる変数の初期化が行われる。３次元計測装置１０００は、計測対象物体１０の位置姿勢を連続して計測することを目的としているため、以降の繰り返し計測時の計測対象物体が変わらない範囲では、ステップＳ１０５０から処理を始めれば良い。
【００１８】
Ｓ１０５０において、パターン生成部３００は、計測対象物体１０に投影するパターンを生成する。空間コードを生成する方法としては非特許文献１に開示されているグレーコードパターンを利用することができる。また、パターン投影部３１０の画素ごとの輝度が制御できるのであれば正弦波パターンを投影しても良い。空間的に離散的なサンプルを行う場合は、格子状に配置された幾何学図形を用いても構わない。すなわち、パターン生成として２次元画像として構成されるパターンを、いずれかのアルゴリズムにより生成することができれば良い。本例では、パターンは２次元の画像のデータとして取り扱い、パターンの生成方法としてはグレーコードパターンを用いる。
【００１９】
図３にグレーコードパターンの例を示す。一つの長方形は、投影パターン画像の領域を示す。白い部分が照明部分、黒い部分は非照明部分であり、投影パターンは明暗による縞状のパターンである。縦縞の模様を逐次照射し、横縞の模様を逐次照明することで、それぞれの画素値は白黒の独立したパターン列を持つことになる。これにより、投影パターン画像の座標値と、実際に計測対象物体表面の照明パターン列を照合することで、投影パターン画像の位置との対応をとることが可能となる。
【００２０】
Ｓ１０６０において、パターン投影部３１０は、Ｓ１０５０でパターン生成部３００が生成したパターンを照明装置により計測対象物体に投影する。パターン投影部３１０は、パターン生成部３００により生成された投影パターン画像を照明光により計測対象物体に投影する。照明光を投影する方法としては、液晶プロジェクタを利用することができる。または、可動する微小な鏡を集積した半導体であるＤＭＤ（Digital Micro-mirror Device）と照明を利用して、投影パターン画像を投影しても構わない。或いは、レーザ光源を動的に偏光する素子を併用した投影により投影パターンを生成してもよい。
【００２１】
Ｓ１０７０において、撮像部４００は、計測空間にある計測対象物体に投影されたパターンの投影像を、撮像素子とレンズを利用して撮影する。撮像素子としては、ＣＣＤやＣＭＯＳによる光−電子変換素子を利用するのが一般的である。レンズは、歪みを事前に校正したものを用いれば良い。撮像部４００により得られた撮像画像は、ＲＡＭ、不揮発メモリの記録素子、もしくはハードディスク、シリコンディスクの外部記録装置などを用いて一時的に保持されても良い。
【００２２】
ステップＳ１０８０において、投影位置演算部８１０は、形状モデル保持部１１０に保持された形状モデル表面のパターンが撮像部４００の撮像面に射影される射影画像座標を計算により求める。ここで、投影位置演算部８１０は、概略位置姿勢保持部２１０に保持された概略位置姿勢と、Ｓ１０５０で生成されたパターン画像の位置との対応から、射影画像座標を求める。より具体的には、投影位置演算部８１０は、パターン生成部３００が生成したパターン画像上の座標値と撮像部４００の撮像画像上に投影される位置（射影画像座標）の関係を、
・形状モデル保持部１１０が保持している計測対象物体の表面のモデルと、
・概略位置姿勢保持部２１０が保持している作業空間での位置姿勢と、
を用いて算出する。
【００２３】
図４は、パターン投影と撮像における計測対象物体との関係を模式的に示す図である。以下、図４を用いて、投影位置演算部８１０の処理について説明する。まず、パターン投影部３１０の投影照明系が理想的な点光源からパターン投影されるとして、焦点距離をｆ＝１とした仮想的な正規化投影画像面を設定し、これをパターン投影画像面と呼ぶ。パターン生成部３００は、パターン投影画像面にパターンを書き込むとパターン投影部３１０が実際の照明、レンズなどのデバイスにより計測対象物体にパターン照明を投影する。実際には、投影のレンズ歪みや理想的な点光源を用いられないことによる影響で、投影される像に歪みが生じる場合があるが、事前に光学補正するか、パターン画像を補正することで影響を低減することができる。
【００２４】
パターン投影画像面のあるパターンの座標をｍ＝［ｕ,ｖ,１］Ｔとする。計測空間の基準座標系に対する投影中心の座標系を投影座標系と定義し、その座標系を回転を３自由度、並進を３自由度の６自由度を用いて表現する。回転Ｒの表現としては、オイラー角、ロール・ピッチ・ヨー、回転軸回転角、４次元数などのいずれの表現手法を用いても構わない。ここでは、回転表現から得られる３ｘ３の回転行列Ｒを用いる。並進はｔｘ、ｔｙ、ｔｚの３成分である３ｘ１の行列ｔ＝［ｔｘ,ｔｙ,ｔｚ］Ｔを用いる。
【００２５】
計測対象物体の表面に投影されるパターン光は、基準座標系でのパターン投影位置Ｍ＝［Ｘ,Ｙ,Ｚ］Ｔに投影されるものとする。光源からパターン投影画像面のパターン座標ｍを通過する直線上にパターン投影位置Ｍが存在するものとすると、光線上の点Ｍは下記の式で表現できる。
【数１】
ここで、λは直線の方向ベクトルのスケールとなる正の値である。Ｌ＝［Ｌｘ,Ｌｙ,Ｌｚ］Ｔは、直線の方向ベクトルの成分を簡単のためにまとめたものである。
【００２６】
次に、式（１）の直線と計測対象物体の形状モデル表面との交点を求め、形状モデル表面上の投影位置Ｍとする。ここでは、形状モデル保持部１１０に保持されているＮ個の三角形パッチのそれぞれと、式（１）の直線が交差するかどうかが判定される。形状モデルのｉ番目の三角形パッチのｊ番目の頂点座標をＰｉｊ＝［ｐｘｉｊ、ｐｙｉｊ、ｐｚｉｊ］Ｔとする。形状モデルは、概略位置姿勢保持部２１０が保持する形状モデルの位置姿勢である回転Ｒｍ、並進ｔｍを使って、計測空間の基準座標系の頂点座標Ｐ’ｉｊを次式（２）のように変換する。
【数２】
【００２７】
三角形パッチの平面の向きは形状モデルにより登録されているものとする。計測空間でのｉ番目の三角形パッチの法線ベクトルｎ’ｉは形状モデルに保持されている三角形パッチの法線ベクトルｎｉに回転Ｒｍを掛けたものとなる。
【００２８】
頂点Ｐ’ｉｊと直線との交点Ｐｃｉを結ぶ線分と、当該三角形パッチの法線ベクトルｎ’ｉは直交するので、内積の関係から
【数３】
となる。式（１）を代入すると次式（４）のようになる。
【数４】
さらに、式（４）をλについてまとめると式（５）となる。
【数５】
これを、式（１）に代入して交点Ｐｃｉの座標として求めると
【数６】
となる。三角形パッチの内部に交点Ｐｃｉが存在するかどうかのチェックとして、三角形頂点と交点Ｐｃｉとの外積を計算して、それぞれが法線ベクトルｎ’ｉと同じ向きの場合に三角形の内部に点があることになる。具体的には次の３つの式が全て正の場合に、交点が三角形内部に存在する。
【数７】
【００２９】
さらに、形状モデルのパッチとの交点は複数存在する場合があるため、投影座標系原点により近い三角形パッチの交点を求める必要がある。Ｎ個の三角形パッチのうち、交点を有するパッチの中からλ＞０の条件で投影座標系原点との距離が最も近い交点を求め、その交点をパターン投影位置Ｍとすればよい。
【数８】
式（８）により得られた形状モデル表面の三角形パッチとの交点が形状モデル表面における推定投影点となる。
【００３０】
次に、撮像部４００が計測対象物体表面のパターンを撮像しているとする。撮像部の撮像系をカメラとして中心射影のモデルとして考える。射影画像面までの焦点距離をｆ’とする。さらに、計測対象物体の表面に投影されているパターン投影位置Ｍが撮像画像上に射影されているとする。その撮像画像上の射影パターン位置をｍ’＝［ｕ’,ｖ’,１］Ｔとする。撮像カメラのレンズ中心を基準座標系における回転と並進の６自由度で表現とし、それぞれＲ’とｔ’の行列として表現すると中心射影の関係から次式（９）のようになる。
【数９】
なお、ｓ’はスケール要素のスカラー値である。
【００３１】
ここでは、パターン投影部３１０のレンズの座標系と撮像部４００の座標系が固定されているとし、事前にそのローカル座標変換は校正手続きにより求まっているとする。具体的な校正手続きは、校正指標を複数撮影して複数の校正指標座標値を用いて最小自乗法により求めることができるし、他の方法で校正しても構わない。ローカル座標変換行列をＲＬとｔＬとすると、下式の関係となる。
【数１０】
【００３２】
式（８）により得られた形状モデル表面のパターン投影位置Ｍと式（１０）を式（９）に代入して、撮像画像上の射影パターン位置ｍ’を得る。
【数１１】
【００３３】
撮像画像上の射影パターン位置ｍ’は、計測対象物体１０の位置姿勢と概略位置姿勢保持部２１０の位置姿勢が一致しているときに、実際の照明装置による投影パターンを撮像したときの座標値と一致する。概略位置姿勢が計測対象物体１０の位置姿勢と僅かに異なっている場合には、投影位置演算部８１０が算出した射影パターン位置ｍ’の画面上の近傍領域に実投影パターンがある。そこで、投影位置演算部８１０は、距離演算部６００に対して照合領域の範囲を指示する。距離演算部６００は、指示された照合領域の範囲、すなわち算出した射影パターン位置ｍ’の近傍領域におけるパターン探索を実行してパターンの対応づけが可能となる。
【００３４】
図２に戻り、Ｓ１０９０において、距離演算部６００は、Ｓ１０７０における撮影で得られた撮像画像からＳ１０８０で推定されたパターン射影画像座標の画素近傍から対応するパターンを探索する。そして、投影系と撮像系とのステレオ計測により各パターンと撮像部４００との距離を計算する。すなわち、距離演算部６００は、撮像部４００が取得した計測対象物体に投影されたパターン（撮影されたパターン）とパターン投影部３１０により投影されたパターンとを対応付け、投影パターン画像の位置と撮影画像の位置との対応を求める。そして、その対応から上記距離を求める。ここで距離演算部６００が対応するパターンを探索する照合領域の範囲は、上述したように投影位置演算部８１０の演算結果により限定されるため、処理速度が向上する。
【００３５】
なお、グレーコードパターンを利用する場合は、白・黒に投影されたパターンをビットとしてみなし、そのエンコードされた符号値とパターン生成時の符号値を照合し、投影パターン画像の位置を決定できる。
【００３６】
距離演算部６００で利用するパターン投影部３１０と撮像部４００とのローカル座標変換行列が事前の校正手続きにより規定されているものとする。そのため、互いに対応する投影されたパターンと撮影されたパターンの座標値を用いて、ステレオ測量により撮像座標系における距離を算出することができる。
【００３７】
一方、位置姿勢演算部７００は、距離演算部６００で求めた距離と形状モデル保持部１１０に保持されている形状モデルを用いて計測対象物体１０の位置姿勢を推定し、概略位置姿勢保持部２１０に保持されている概略位置姿勢の情報を更新する。以下、この処理を説明する。まず、実際に照明されたパターンを撮像し、検出された座標を実投影パターンの撮像画像上の位置を実射影パターン位置ｍ’’とする。パターン位置ｍとローカル座標変換行列ＲＬ、ｔＬ、そして実パターン投影位置Ｍ'の関係は次式（１２）のようになる。
【数１２】
Ｍ’は、左辺の行列式の一般化逆行列を両辺に掛け合わせることで求めることができる。
【００３８】
Ｓ１１１０において位置姿勢演算部７００は、実射影パターン位置と算出した射影パターン位置との差が小さくなるように位置姿勢の推定演算を行う。位置姿勢の推定では、姿勢に対して非線形な計算を伴うため、基本的には繰り返し誤差最小化を適用する。求めようとする計測対象物体の位置姿勢を未知数として、形状モデルの表面の各パターン投影位置から算出した射影パターン位置と観測された実射影パターン位置との距離の二乗の和が最小になるように、未知パラメータである位置姿勢の推定値を更新する。具体的には非特許文献２に開示されるＩＣＰ（Iterative Closest Point）法や、非特許文献３に開示される方法を利用して、形状モデルと計測距離から当該形状モデルの位置姿勢を求めることができる。その際に、初期値としては概略位置姿勢保持部２１０の値を使うことができる。
【００３９】
なお、投影位置演算部８１０では、形状モデルの投影と射影の関係があるパターンのみ使うことができるため、他の物体や背景の領域に対する距離計測の結果を含めないことができる。そのため、位置姿勢演算部７００での形状モデル以外の例外値が無いため、ロバストな位置姿勢演算を行うことができる。
【００４０】
また、位置姿勢演算部７００では、繰り返し演算の結果で生じる残差（実射影パターン位置と更新後の概略位置姿勢で投影位置演算部８１０が算出した射影パターン位置との差）から最適化計算の繰り返しを継続するか否かを判定する。この判定のための閾値としては、ユーザが設定した閾値でもよいし、作業空間における観測面積と撮像装置との距離との関係を用いて設定された閾値でもよい。さらに、３次元計測装置１０００が既定値を保持していて、それを読み出して閾値としても構わない。
【００４１】
いずれかの方法により設定された閾値と残差の値を比較するが、実際には演算後の残差が小さくならない場合もある。そのため、本実施形態では、繰り返し回数を保持しておき、残差の繰り返しごとのトレンドを計算し、繰り返し回数が既定回数を越えた場合もしくはトレンドが増加する傾向にある場合には、位置姿勢演算部７００の最適化演算の繰り返しを停止するようにする。これにより、位置姿勢の最適化演算がうまくいったか否かを判定することができる。
【００４２】
図５を用いて位置姿勢演算部７００の処理について説明をする。最初にＳ１０において、位置姿勢演算部７００は、演算の繰り返し回数の値を０とし、移動平均値の値を初期化する。Ｓ１００において、位置姿勢演算部７００は、距離演算部６００による距離計算結果を得る。Ｓ１１０において、位置姿勢演算部７００は、計測対象物体１０の位置姿勢を最適化計算し、パラメータを更新する。Ｓ１２０において、位置姿勢演算部７００は、位置姿勢の更新した値と残差を取得する。Ｓ１３０において、位置姿勢演算部７００は、Ｓ１２０で取得された残差が既定した閾値以下であるかどうかを判断する。残差が閾値以下の場合は位置姿勢の推定結果が想定する精度を有しているものとして、Ｓ１４０において最適化演算成功のフラグを設定して、繰り返しを停止する。Ｓ１３０において残差が閾値を上回る場合は、位置姿勢の推定の精度が不十分ということでＳ１５０に処理を移す。
【００４３】
Ｓ１５０において、位置姿勢演算部７００は、残差が減少しているか増加しているかのトレンドを計算するため、残差の移動平均を計算する。移動平均のフレーム数ｎはユーザが指定してもよいし、実験的に求めた値を設定しておけば良い。Ｓ１６０において、位置姿勢演算部７００は、その移動平均値が前回計算した移動平均値よりも大きくなる傾向を示す場合には、最適化演算の誤差最小化計算が発散しているとして、Ｓ１７０の最適化が失敗したフラグを設定して繰り返しを停止する。Ｓ１６０において、繰り返しにより誤差が減少する場合は、再度繰り返し演算が必要としてＳ１８０に移る。Ｓ１８０では、位置姿勢演算部７００は、全体の繰り返し回数を保持している値に１を加える。次に、Ｓ１９０において、位置姿勢演算部７００は、繰り返し回数が既定回数を越えたかどうかを判定する。繰り返し回数が既定回数以内であれば、位置姿勢演算部７００は、再度繰り返しの最適化計算を実行するべく処理をＳ１１０へ戻す。一方、既定回数を越えたと判定された場合は、Ｓ２００において、位置姿勢演算部７００は、繰り返し回数を越えたことを示すフラグを設定して、当該繰り返しを停止する。
【００４４】
図２に戻り、Ｓ１１３０において、位置姿勢演算部７００は推定位置姿勢結果を用いて概略位置姿勢保持部２１０の値を更新する。３次元計測装置１０００が連続して計測対象物体１０の位置姿勢を計測している場合は、位置姿勢演算部７００による位置姿勢の推定結果で概略位置姿勢保持部２１０の値を更新することで、連続したフレーム間での計測対象物体の位置姿勢推定が可能となる。
【００４５】
Ｓ１１２０において、パターン生成部３００は、Ｓ１１１０における位置姿勢演算部７００による位置姿勢の推定の状態をうけて、ステップＳ１０５０の投影パターン生成のパラメータを更新する。具体的には、Ｓ１４０，Ｓ１７０，Ｓ２００で設定されるフラグが推定状態として用いられる。推定状態が良い場合（例えば、最適化演算成功のフラグが設定されている場合）は、投影・撮像回数を減らすように、推定状態が悪い場合（例えば、最適化の失敗を示すフラグが設定されている場合）は投影・撮像回数を増やすように投影制御を行う。すなわち、パターン生成部３００における投影制御機能（Ｓ１１２０）では、推定状態の推定結果が良い場合はグレーコードパターンの投影画像の数を減らし、推定結果が悪い場合はグレーコードパターンの投影画像の数を増やすように制御する。
【００４６】
さらに、パターン生成部３００の投影制御機能は、概略位置姿勢保持部２１０に保持されている情報によって示される計測対象物体１０の位置姿勢の変化に応じて投影するパターンの縞の間隔を変更する。すなわち、パターン生成部３００は、位置姿勢の変化が大きいときは、投影するパターンの間隔を大きくするようにし、変化が小さいときはパターンの間隔を小さくするように制御する。なお、概略位置姿勢保持部２１０が保持する情報によって示される計測対象物体１０の位置姿勢の変化とは、計測対象物体１０か３次元計測装置１０００のどちらか、もしくは両方が動いているときの動きを反映する。
【００４７】
図６に投影するパターン画像列を示す。ＰＰ［ｎ］は、ｎ番目の投影する投影パターンを示す。ここでの実施形態としては、ｎが奇数の場合は縦縞、ｎが偶数の場合は横縞にしている。これら縞状のパターンは明暗により形成されている。図３における縞の間隔が小さいほうから画像列を保持する。投影される順序は［ｎ］の順、すなわち縞の間隔が小さいほうから投影されることになる。
【００４８】
図７に第１実施形態によるパターン生成部３００の投影制御機能の処理を示す。以下では、パターンの総数を２ｎとし、縦縞と横縞のペアがｎ組存在するものとする。ここで、Ｗｉ、Ｈｉは整数値とする。Ｗｉはｉ番目に投影するパターンにおけるＰＰ［ｎ］のパターン画像のペアの数を意味する。Ｗｉがｎの場合は、グレーコードパターンの全てのパターン画像を順次投影することを意味し、通常のグレーコードパターンによるパターン光投影法になる。Ｈｉはｉ番目におけるＰＰ［ｎ］のパターン画像列の開始するペアの順位を意味する。Ｈｉが大きいほど、より投影するパターン縞の幅が相対的に大きいものから順次投影することとなる。すなわち、パターン投影部３１０は、パターン間隔を広くしたパターンを照射することになる。物体・装置が動いている時にはパターン投影位置の推定座標の精度が悪くなるが、この構成によれば、投影パターンの間隔を大きくすることにより隣接するパターンとの距離を広くすることができ、パターン識別性を向上する効果が得られる。
【００４９】
以上のようなパターン生成部３００による投影制御機能による処理について図７のフローチャートを用いて説明する。まず、Ｓ３００において、パターン生成部３００は、初期化として変数の初期化を行う。これは、投影制御機能が利用可能になるときに一度設定すれば良い。処理開始であるＳ３１０から処理終了であるＳ５００は、パターン生成部３００の一部の処理の範囲を示すもので、パターン生成部３００の全体の処理の開始・終了ではない。ステップＳ３２０において、パターン生成部３００は、処理の繰り返し回数を保持する変数ｉを１つ増加させる。
【００５０】
Ｓ３３０において、パターン生成部３００は、概略位置姿勢保持部２１０に保持されている情報が示す、更新前後の位置姿勢の変化量を算出する。たとえば、概略位置姿勢保持部２１０が６自由度のパラメータを保持している場合は、ｉ−１回目に保持していた概略位置姿勢の値と、ｉ回目の概略位置姿勢の値とのそれぞれの差の絶対値を計算し、それぞれの絶対値から最大の絶対値を求めて変化量とする。Ｓ３４０において、パターン生成部３００は、Ｓ３３０で算出した変化量と事前に設定した閾値とを比較して、変化量が閾値より大きい場合はＳ３７０へ、小さい場合はＳ３８０へ処理を進める。Ｓ３７０では、パターン生成部３００は、前回のＨｉの値（Ｈｉ−１）に１を加えて今回のＨｉの値とする。ステップＳ３８０では、パターン生成部３００は、前回のＨｉの値（Ｈｉ−１）から１を引いた値を今回のＨｉとして設定する。これにより、物体・装置の動きが大きい場合は縞の間隔が大きいパターンを用いた投影が行われ、動きが小さい場合は縞の間隔が小さいパターンを用いた投影が行われるようになる。
【００５１】
一方、Ｓ４１０において、パターン生成部３００は位置姿勢演算部７００の推定状態を取得し、Ｓ４２０で推定状態の判別を行う。最適化演算成功のフラグが立っている場合は、Ｓ４５０に移行し、最適化演算成功のフラグが立っていない（最適化演算が成功していない）場合はＳ４６０に移行する。Ｓ４５０において、パターン生成部３００は、前回のＷｉの値（Ｗｉ−１）から１を引き、今回のＷｉの値とする。また、Ｓ４６０では、前回のＷｉ（Ｗｉ−１）の値に１を加えて、今回のＷｉの値とする。これは、最適化演算が成功している場合には、撮影するパターンの画像セット数を減らし、失敗している場合は撮影するパターンの画像セット数を増やすように作用する。ステップＳ４００では、ＰＰ［（２×Ｈｉ）−１］から順にＰＰ［２×（Ｈｉ＋Ｗｉ）−２］までのＷｉ組のペアとなるパターン画像をパターン投影部３１０に指示する。ただし、ＰＰ［２×（Ｈｉ＋Ｗｉ）−２］はＰＰ［２ｎ］までしかデータが用意されていないので、Ｈｉが大きくなると、投影されるパターンはＷｉ組よりも少なくなってしまう。そこで、投影されるパターン数を優先させたい場合は、ＰＰ［（２×Ｈｉ）−１］よりも前の順位の画像から始めてもよい。
【００５２】
基本的には最適化演算が成功している場合は、符号化長を減らしても投影位置演算部８１０による撮像画像座標の推定精度が高いため、パターンの対応づけができる。図７で説明した一実施形態ではパターンの投影数をＷｉとして、その値を１ずつ増減することを行っていたが、その増減の値は１に限らず、増減幅は一定でなくても良い。
【００５３】
なお、計測対象物体と撮像部４００との相対運動が大きい場合は、投影位置演算部８１０による推定射影パターン位置と、実際の射影パターン位置との差が大きい。しかし、その値は奥行き方向の値と関係がある。そこで、奥行き方向の距離に応じてパターンの探索領域を変更すると良い。そこで概略位置姿勢のパラメータのうち、姿勢成分と並進成分のパラメータの閾値を別々に比較しても良い。
【００５４】
以上のように、本実施形態によれば、パターン生成部３００の投影制御機能は、位置姿勢演算部７００の推定状態により適応的に投影パターンの回数や計測点を増減するため、推定状態が良い場合は投影回数を減らすことが可能となる。そのため、全体の計測時間の短縮を実現できる。すなわち、本実施形態によれば、投影されるパターンの撮像画像上における位置を形状モデルと概略位置姿勢から推定することで、空間コードのための投影・撮像回数を減らすことができ、処理の高速化が期待できる。
【００５５】
ステップＳ１１２０の機能はパターン生成部３００の投影制御機能により実現する。ステップＳ１１４０において、位置姿勢出力部９００は、Ｓ１１１０で得られた位置姿勢推定結果を本装置の外部へ出力する。出力する情報としては、計測対象物体１０の作業空間における基準座標系での位置姿勢である６自由度の値を出力しても構わないし、撮像部４００の座標系における撮像座標系の原点の位置姿勢を含めて位置姿勢を出力しても構わない。
【００５６】
なお、本発明の一実施形態である３次元計測装置１０００では、独立した装置を利用しても構わない。例えば、パターン投影部３１０には、市販の液晶プロジェクタを利用することができる。撮像部４００には、市販のカメラとレンズを利用することが可能である。３次元計測装置１０００とは、それぞれの装置で利用可能な信号を出力するための機構と、実際の信号を受け取るための入出力部分があれば良い。そのようにすれば、利用者の用途や計測範囲に応じて装置を拡張して利用することができる。
【００５７】
［第２実施形態］
第１実施形態で説明した３次元計測装置１０００の好適な適用事例としては、産業用ロボットアームの手先部に設置して、把持する物体を計測対象物体として、その位置姿勢を計測する目的に利用することがあげられる。以下、図８を用いて３次元計測装置１０００の適用例を説明する。
【００５８】
産業用ロボットアーム３０は、ロボットコントローラ４０の制御による駆動する。本発明の一実施形態である３次元計測装置１０００がアーム腕部に治具により固定されている。産業用ロボットアーム３０の動作は対象物体１１をアーム把持部によりピッキングする。３次元計測装置１０００は、アーム把持部と対象物体１１の相対位置姿勢を逐次推定し、その推定結果をロボットコントローラ４０の制御パラメータとして伝えるためにケーブルにより結線している。
【００５９】
３次元計測装置１０００における概略位置姿勢入力部２００へは、ロボットコントローラ４０が制御する産業用ロボットアーム３０の取り付け位置に、治具のためのオフセットを加えた値を伝えれば良い。また、形状モデル入力部１００には、対象物体１１のＣＡＤモデルを本装置１０００が読み込めるフォーマットに変換したものを入力しておく。なお、対象物体が変わった場合には、その都度、形状モデル入力部１００に該当する形状モデルを入力することで、複数種類の対象物体に対応することができる。装置が事前の保持している形状モデルを選択しても構わないし、別の部品識別装置による部品種の認識結果を使って保持している形状モデルを切り替えても構わない。
【００６０】
３次元計測装置１０００の概略位置姿勢を入力した対象物体１１の位置姿勢が実際の作業空間での配置と異なる場合には、位置姿勢演算部７００での位置姿勢推定の残差が大きくなる。すると、パターン生成部３００が、投影・撮像回数を増加する。それにより、計測対象物体の距離計測と位置姿勢が可能となる。一度精度の高い位置姿勢が行えれば、概略位置姿勢保持部２１０に反映される。
【００６１】
さらに、産業用ロボットアーム３０の手先が対象物体１１に近づくための動作を行うとする。実施のロボットアームは、関節角度のエンコーダ値のノイズや、ロボット自身の自重による撓みにより、エンコーダによる手先位置の作業空間内での絶対位置精度はそれほど高くない場合が多い。概略位置姿勢入力部２００にはロボットの運動した手先位置が入力される。３次元計測装置１０００は、計測のためのパターン投影と撮像を行い、位置姿勢推定結果を出力する。この場合は、ロボット手先位置計算の計算誤差を補正することが主になる。そのため、位置姿勢演算部７００での推定状態が最適化演算成功となる。そこで、パターン生成部３００は投影・撮像の回数を減らすための投影制御を行う。このようにして、最初の概略位置姿勢を推定するときには撮影回数が増加するが、精度の高い位置姿勢が得られれば、それ以降の計測のための撮影回数を減らせることとなり、連続した計測時の投影・撮像の回数が減り計測のタクトタイムが短縮される。
【００６２】
［第３実施形態］
本発明では計測対象物体の形状モデルが未知の場合においても、本発明の一実施形態である装置構成を利用して、基本的には計測対象物体表面の３次元形状を取得することができる。未知の計測対象物体の形状モデルを複数の視点からの画像を使って取得する手続きを加えることで、さらに本発明の適用の範囲を広げることができる。
【００６３】
図９は、第３実施形態による３次元計測装置１０１０の構成例を示すブロック図である。３次元計測装置１０１０は、第１実施形態の３次元計測装置１０００の構成に計測モデル生成部１２０を付加したものである。計測モデル生成部１２０は、形状モデルが無いモデルに対して本装置を適用する際にその機能を発揮する。以下、３次元計測装置１０１０の動作について図１０のフローチャートを用いて説明する。
【００６４】
Ｓ２０００において、３次元計測装置１０１０は、形状モデル利用モードか計測モデル取得モードかをユーザに選択させる。選択方法としては、ボタンやスイッチを用いれば良い。Ｓ２０１０において、３次元計測装置１０１０は、ユーザの入力を判別して形状モデル利用モードかどうかを判定する。Ｙｅｓの場合（形状モデル利用モードの場合）は、Ｓ２０２０へ処理が進む。Ｓ２０２０では、形状モデル保持部１１０に既に登録されている、または形状モデル入力部１００が入力した既存の形状モデルを利用した位置姿勢計測が実行される。すなわち、図１のＳ１０００からの上述した処理により計測対象物体の位置姿勢計測を行う。一方、Ｓ２０１０でＮｏの場合（計測モデル取得モードの場合）は、Ｓ２０４０へ進み、計測モデル生成部１２０の制御下で計測モデル撮影の処理を開始する。なお、最初期においては、最初の撮影時の位置姿勢を基準とするように概略位置姿勢保持部２１０の内容や位置姿勢演算部７００の状態をセットする。
【００６５】
Ｓ２０５０において、計測モデル生成部１２０は、計測対象物体の概略位置姿勢（概略位置姿勢保持部２１０の内容）を取得する。例えば、計測対象物体を回転板の上に設置しているならば、その回転角度から計算される位置姿勢を取得することができる。また、本計測装置が第２実施形態で述べた産業用ロボットアームのアーム部に設置している場合は、ロボットの手先位置を取得することができる。次に、Ｓ２０６０において、計測モデル生成部１２０は、パターン生成部３００に対して全ての投影パターンを投影して距離を計測するように指示する。Ｓ２０７０において、距離演算部６００は全ての距離計測結果を計測モデル生成部１２０に送る。
【００６６】
形状モデルは複数の視点から観察した計測対象物体表面までの距離データを統合することにより得られるので、Ｓ２０８０において計測モデル生成部１２０は、計測対象物体１０を動かすか、３次元計測装置１０１０自体を動かす。なお、その時の移動量は既知とする。Ｓ２０９０において、計測モデル撮影を終了するかどうかを判断する。判断する方法としては、既定回数の撮影が終了した場合やユーザによる指示などを用いてよい。こうして、３次元計測装置１０１０（パターン投影部３１０と撮像部４００）と計測対象物体とを複数の既知の位置関係に配置して行われた距離計測によって複数の距離データが得られる。
【００６７】
Ｓ２１００において、計測モデル生成部１２０は、距離演算部６００より提供された複数の計測距離画像を統合する。統合する方法としては、例えば、重複する距離点群の領域の幾何学的な距離が最小になるように概略位置姿勢を用いて誤差最小化計算を行って統合することがあげられる。Ｓ２１１０において、計測モデル生成部１２０は、統合された計測点群から計測対象物体の領域をユーザに選択させる。例えば、統合された距離画像をユーザが視認できるように表示して、マウスなどで指定させればよい。実際に計測対象物体と規定しているのはユーザであるため、領域を選択してもらうことが望ましい。
【００６８】
Ｓ２１２０において、計測モデル生成部１２０は、統合した距離点群からＳ２１１０で選択された領域を抽出して、形状モデルに変換して登録する。点群から形状モデルである三角形パッチを生成する方法はマーチングキューブ法などを用いれば良い。Ｓ２１３０では、計測モデル生成部１２０が生成した形状モデルを形状モデル保持部１１０に格納し、図１に示すＳ１０４０からの処理を行い、第１実施形態と同様の位置姿勢計測を開始する。
【００６９】
上述の計測モデル生成部を有する３次元計測装置１０１０を用いれば、形状モデルが事前に用意できない場合においても、本装置を利用することが可能となる。
【００７０】
［第４実施形態］
第１、第３実施形態で説明した各処理部はコンピュータのプログラムとしても実行できる。図１１は、パーソナルコンピュータ２１００に撮像部４００としてのカメラ４０１とパターン投影部３１０としてのプロジェクタ３１１を、Ｉ／Ｏインターフェース２１０１、ビデオインターフェース２１０３を介して接続した例を示す。ＣＰＵ２１０２は、上述した実施形態の機能をコンピュータに実行させるプログラムコード２１０５をメモリから読み出して実行することにより、当該コンピュータを３次元計測装置１０００（１０１０）として機能させる。プログラムコード２１０５は、プログラムメモリ或いは不図示の外部記憶装置からＲＡＭ２１０４にロードされ、実行されるようにしてもよい。入力部２１０６は、キーボード、マウス等で構成される。
【００７１】
その際、パーソナルコンピュータのバスや周辺機器を効率的に利用するための仕組みとしてオペレーティングシステムを利用しても構わない。オペレーティングシステムは、本発明の機能を実装したプログラムとは別に提供されていても問題ないし、オペレーティングシステムにおける一つのアプリケーションプログラムとして、本発明の機能を実装した処理プログラムを選択して実行しても構わない。
【００７２】
［第５実施形態］
３次元計測装置１０００の投影位置演算部８１０は、形状モデル保持部１１０の三角形パッチの数が増加すると処理量が増加する。しかしながら、三角形パッチの探索を効率化するために、事前に三角形パッチを投影座標系視点に対してソートしておく方法により処理を高速化できる。
【００７３】
さらに、処理の高速化として３次元のコンピュータグラフィックスをレンダリングするハードウェアである不図示のグラフィックスプロセッサユニットを用いることができる。グラフィックスプロセッサユニットとしては、Ｚバッファからの値を読み出すことができるハードウェアを使えば良い。グラフィックスプロセッサユニットを利用して、レンダリング時のＺバッファの値から、投影座標原点に近い平面までの距離を求めることができる。投影座標系原点を視点とした形状モデルを正規化投影画像平面と一致するようにレンダリングして、パターン座標に相当するＺバッファを求め、その奥行き値を含めて座標を基準座標系へ変換すれば良い。また、同様に、投影画像上の射影パターン位置ｍ’の計算を行う式（１１）にも適用できる。形状表面上の投影パターンＭを撮像座標系に合わせた射影面に投影する処理をグラフィックスプロセッサの射影変換機能を用いて求めれば高速に演算できる。
【特許請求の範囲】
【請求項１】
計測対象物体の形状モデルを保持する形状モデル保持手段と、
前記計測対象物体の概略位置姿勢を示す情報を保持する概略位置姿勢保持手段と、
明暗で構成された複数種類のパターンを前記計測対象物体に投影する投影手段と、
前記投影手段によりパターンが投影された前記計測対象物体を撮影する撮像手段と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像手段により取得される撮像画像における前記パターンの座標値を算出する投影位置演算手段と、
前記投影位置演算手段により算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像手段との距離を求める距離演算手段と、
前記距離演算手段で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算手段とを備えることを特徴とする３次元計測装置。
【請求項２】
前記位置姿勢演算手段は、更に、前記計測対象物体について行なった位置姿勢の推定の状態を示す推定状態を生成し、
前記投影手段は、前記推定状態に基づいて投影するパターンを制御することを特徴とする請求項１に記載の３次元計測装置。
【請求項３】
前記投影手段は、前記推定状態に基づいて投影するパターンの種類を増減することを特徴とする請求項２に記載の３次元計測装置。
【請求項４】
前記パターンは縞状のパターンを有し、
前記パターン生成手段は、更に、前記位置姿勢演算手段によって算出された前記計測対象物体の位置姿勢の変化の大きさに基づいて投影するパターンの縞の間隔を変更することを特徴とする請求項１乃至請求項３のいずれか１項に記載の３次元計測装置。
【請求項５】
前記撮像手段及び前記投影手段と、前記計測対象物体との位置関係を変化させながら前記撮像手段を機能させることにより得られた複数の撮像画像を用いて前記計測対象物体の形状モデルを生成する計測モデル生成手段を更に備え、
前記形状モデル保持手段は、前記計測モデル生成手段により生成された形状モデルを保持することを特徴とする請求項１乃至４のいずれか１項に記載の３次元計測装置。
【請求項６】
計測対象物体の形状モデルを形状モデル保持手段に保持する工程と、
前記計測対象物体の概略位置姿勢を示す情報を概略位置姿勢保持手段に保持する工程と、
明暗で構成された複数種類のパターンを投影部より前記計測対象物体に投影する投影工程と、
前記投影工程でパターンが投影された前記計測対象物体を撮像部により撮影する撮影工程と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像工程で取得される撮像画像における前記パターンの座標値を算出する投影位置演算工程と、
前記投影位置演算工程で算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像部との距離を求める距離演算工程と、
前記距離演算工程で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算工程とを備えることを特徴とする３次元計測装置の制御方法。
【請求項７】
コンピュータを、請求項１乃至請求項６のいずれか１項に記載の３次元計測装置における各手段として機能させるためのプログラム。
【請求項１】
計測対象物体の形状モデルを保持する形状モデル保持手段と、
前記計測対象物体の概略位置姿勢を示す情報を保持する概略位置姿勢保持手段と、
明暗で構成された複数種類のパターンを前記計測対象物体に投影する投影手段と、
前記投影手段によりパターンが投影された前記計測対象物体を撮影する撮像手段と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像手段により取得される撮像画像における前記パターンの座標値を算出する投影位置演算手段と、
前記投影位置演算手段により算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像手段との距離を求める距離演算手段と、
前記距離演算手段で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算手段とを備えることを特徴とする３次元計測装置。
【請求項２】
前記位置姿勢演算手段は、更に、前記計測対象物体について行なった位置姿勢の推定の状態を示す推定状態を生成し、
前記投影手段は、前記推定状態に基づいて投影するパターンを制御することを特徴とする請求項１に記載の３次元計測装置。
【請求項３】
前記投影手段は、前記推定状態に基づいて投影するパターンの種類を増減することを特徴とする請求項２に記載の３次元計測装置。
【請求項４】
前記パターンは縞状のパターンを有し、
前記パターン生成手段は、更に、前記位置姿勢演算手段によって算出された前記計測対象物体の位置姿勢の変化の大きさに基づいて投影するパターンの縞の間隔を変更することを特徴とする請求項１乃至請求項３のいずれか１項に記載の３次元計測装置。
【請求項５】
前記撮像手段及び前記投影手段と、前記計測対象物体との位置関係を変化させながら前記撮像手段を機能させることにより得られた複数の撮像画像を用いて前記計測対象物体の形状モデルを生成する計測モデル生成手段を更に備え、
前記形状モデル保持手段は、前記計測モデル生成手段により生成された形状モデルを保持することを特徴とする請求項１乃至４のいずれか１項に記載の３次元計測装置。
【請求項６】
計測対象物体の形状モデルを形状モデル保持手段に保持する工程と、
前記計測対象物体の概略位置姿勢を示す情報を概略位置姿勢保持手段に保持する工程と、
明暗で構成された複数種類のパターンを投影部より前記計測対象物体に投影する投影工程と、
前記投影工程でパターンが投影された前記計測対象物体を撮像部により撮影する撮影工程と、
前記形状モデルと前記概略位置姿勢を示す情報とに基づいて前記撮像工程で取得される撮像画像における前記パターンの座標値を算出する投影位置演算工程と、
前記投影位置演算工程で算出された前記座標値に基づいて前記撮像画像における撮影されたパターンを探索し、撮影されたパターンと投影されたパターンとを対応づけることにより前記計測対象物体の表面と前記撮像部との距離を求める距離演算工程と、
前記距離演算工程で求めた距離と前記形状モデル保持手段に保持されている形状モデルを用いて前記計測対象物体の位置姿勢を推定し、前記概略位置姿勢保持手段に保持されている概略位置姿勢の情報を更新する位置姿勢演算工程とを備えることを特徴とする３次元計測装置の制御方法。
【請求項７】
コンピュータを、請求項１乃至請求項６のいずれか１項に記載の３次元計測装置における各手段として機能させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１１−１４１１７４（Ｐ２０１１−１４１１７４Ａ）
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願番号】特願２０１０−１５６１（Ｐ２０１０−１５６１）
【出願日】平成２２年１月６日（２０１０．１．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願日】平成２２年１月６日（２０１０．１．６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]