JP6198104B2

JP6198104B2 - ３次元物体認識装置及び３次元物体認識方法

Info

Publication number: JP6198104B2
Application number: JP2013053561A
Authority: JP
Inventors: 朋弘仲道
Original assignee: 3D Media Co Ltd
Current assignee: Kyoto Robotics Corp
Priority date: 2013-03-15
Filing date: 2013-03-15
Publication date: 2017-09-20
Anticipated expiration: 2033-03-15
Also published as: JP2014178967A

Description

本発明は、形状が既知の認識対象となる３次元物体を認識するための３次元物体認識装置及び３次元物体認識方法に関する。

生産ラインにおいてロボットアームによる部品等の正確な操作を可能とするため、山積みにされた部品等を個々に認識し、各部品の位置及び姿勢を認識する３次元物体認識装置が近年開発されている。

従来、このような３次元物体認識装置としては、例えば、認識対象物を所定方向からカメラで撮影した画像から認識対象物のエッジすなわち輪郭等の特徴を抽出し、撮影画像を構成する各画素について最も近いエッジまでの距離をそれぞれ計算し、認識対象物の輪郭形状を表わす３次元モデルを撮影画像上に射影して照合することにより認識対象物の位置姿勢を認識するものがある。また、撮影画像を構成する各画素に、最も近いエッジまでの距離を画素値としてそれぞれ記憶させたディスタンスマップを作成し、このディスタンスマップを参照することにより、処理速度を向上させた３次元物体認識装置が提案されている（例えば、特許文献１参照）。また、３次元物体認識装置としては、認識対象物の表面の点の３次元座標を示す３次元点群を用いて認識対象物の位置姿勢を認識するものもある。

特開２０１０−２０５０９５号公報

しかしながら、特許文献１の３次元物体認識装置等のように輪郭を用いて認識対象物の位置姿勢を認識する手法では、２次元画像を用いて認識を行っているため、似通った画像が存在すれば、本来は認識対象物が存在しない場所に誤って認識されてしまう虞があるとともに、画像から輪郭が抽出されない場合には、認識対象物の位置姿勢を認識できないという問題がある。また、このような問題が生じた場合には処理時間も遅くなる。また、３次元点群を用いて認識対象物の位置姿勢を認識する手法では、輪郭部分の位置決めに曖昧性が残るという問題がある。

本発明は、上記のような課題に鑑みてなされたものであって、認識対象物の位置姿勢の誤認識を抑制して、認識精度を向上させ、且つ処理速度を向上させることができる３次元物体認識装置及び３次物体認識方法を提供することを目的とする。

上記目的を達成するために、請求項１に記載の３次元物体認識装置は、認識対象物の輪郭及び表面形状を表わす３次元モデルを記憶する３次元モデル記憶手段と、前記認識対象物を所定方向から撮影して画像を取得する画像取得手段と、前記認識対象物の表面の点の３次元座標を示す３次元点群を計測する３次元計測手段と、前記画像取得手段により取得した画像又は前記３次元計測手段により得られた３次元計測結果から前記認識対象物のエッジを抽出するエッジ抽出手段と、前記エッジ抽出手段により抽出されたエッジとあらゆる位置姿勢における前記３次元モデルの輪郭形状を示す３次元輪郭点に基づいて求められる輪郭評価値、及び、前記３次元計測手段により得られた３次元計測結果とあらゆる位置姿勢における前記３次元モデルの表面形状を示す３次元面点に基づいて求められる点群評価値を用いて前記認識対象物の位置姿勢を評価する位置姿勢評価手段と、を備えることを特徴としている。

請求項２記載の３次元物体認識装置は、前記位置姿勢評価手段が、前記輪郭評価値と前記点群評価値の比率を調整することを特徴としている。

請求項３記載の３次元物体認識装置は、前記画像取得手段により取得した画像を構成する各画素に、前記エッジ抽出手段により抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させたディスタンスマップを記憶するディスタンスマップ記憶手段と、前記３次元計測手段により計測された３次元点群の前記画像取得手段により取得された画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた対応座標マップを記憶する対応座標マップ記憶手段と、を備え、前記位置姿勢評価手段は、前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価することを特徴としている。

請求項４記載の３次元物体認識装置は、前記画像取得手段により取得した画像の解像度を異なる比率で低下させた複数枚の画像を有する画像ピラミッドを作成する画像ピラミッド作成手段と、前記位置姿勢評価手段により得られた最も高い評価値を初期値として用いて、前記認識対象物の位置姿勢の最適化を行う位置姿勢最適化手段と、を備え、前記エッジ抽出手段は、前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像から前記認識対象物のエッジを抽出し、前記ディスタンスマップ記憶手段は、前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像の各画素に、前記エッジ抽出手段により前記各解像度の画像から抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させた各解像度のディスタンスマップを記憶し、前記対応座標マップ記憶手段は、前記３次元計測手段により計測された３次元点群の前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる各解像度の画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた各解像度の対応座標マップを記憶し、前記位置姿勢評価手段は、解像度が最も低い前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、解像度が最も低い前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価し、前記位置姿勢最適化手段は、前記位置姿勢評価手段により得られた最も高い評価値を初期値とし、該初期値を前記位置姿勢評価手段で用いられた前記ディスタンスマップ及び前記対応座標マップよりも高い解像度のディスタンスマップ及び対応座標マップを用いて、予め設定された精度に到達又は最も高い解像度のディスタンスマップ及び対応座標マップを用いた位置姿勢の評価が終了するまで最適化を行うことを特徴としている。

請求項５記載の３次元物体認識装置は、前記位置姿勢評価手段又は前記位置姿勢最適化手段により得られた前記認識対象物の位置姿勢の評価結果を表示する表示手段を備えることを特徴としている。

請求項６記載の３次元物体認識方法は、認識対象物の輪郭及び表面形状を表わす３次元モデルを３次元モデル記憶手段に記憶する３次元モデル記憶ステップと、前記認識対象物を所定方向から撮影して画像を取得する画像取得ステップと、前記認識対象物の表面の点の３次元座標を示す３次元点群を計測する３次元計測ステップと、前記画像取得ステップで取得した画像又は前記３次元計測ステップで得られた３次元計測結果から前記認識対象物のエッジを抽出するエッジ抽出ステップと、前記エッジ抽出ステップで抽出されたエッジとあらゆる位置姿勢における前記３次元モデルの輪郭形状を示す３次元輪郭点に基づいて求められる輪郭評価値、及び、前記３次元計測ステップで得られた３次元計測結果とあらゆる位置姿勢における前記３次元モデルの表面形状を示す３次元面点に基づいて求められる点群評価値を用いて前記認識対象物の位置姿勢を評価する位置姿勢評価ステップと、を備えることを特徴としている。

請求項７記載の３次元物体認識方法は、前記位置姿勢評価ステップが、前記輪郭評価値と前記点群評価値の比率を調整することを特徴としている。

請求項８記載の３次元物体認識方法は、前記画像取得ステップで取得した画像を構成する各画素に、前記エッジ抽出ステップで抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させたディスタンスマップをディスタンスマップ記憶手段に記憶するディスタンスマップ記憶ステップと、前記３次元計測ステップで計測された３次元点群の前記画像取得ステップにより取得された画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた対応座標マップを対応座標マップ記憶手段に記憶する対応座標マップ記憶ステップと、を備え、前記位置姿勢評価ステップは、前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価することを特徴としている。

請求項９記載の３次元物体認識方法は、前記画像取得ステップで取得した画像の解像度を異なる比率で低下させた複数枚の画像を有する画像ピラミッドを作成する画像ピラミッド作成ステップと、前記位置姿勢評価ステップで得られた最も高い評価値を初期値として用いて、前記認識対象物の位置姿勢の最適化を行う位置姿勢最適化ステップと、を備え、前記抽出ステップは、前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像から前記認識対象物のエッジを抽出し、前記ディスタンスマップ記憶ステップは、前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像の各画素に、前記エッジ抽出ステップで前記各解像度の画像から抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させた各解像度のディスタンスマップを前記ディスタンスマップ記憶手段に記憶し、前記対応座標マップ記憶ステップは、前記３次元計測ステップで計測された３次元点群の前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる各解像度の画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた各解像度の対応座標マップを前記対応座標マップ記憶手段に記憶し、前記位置姿勢評価ステップは、解像度が最も低い前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、解像度が最も低い前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価し、前記位置姿勢最適化ステップは、前記位置姿勢評価ステップで得られた最も高い評価値を初期値とし、該初期値を前記位置姿勢評価ステップで用いられた前記ディスタンスマップ及び前記対応座標マップよりも高い解像度のディスタンスマップ及び対応座標マップを用いて、予め設定された精度に到達又は最も高い解像度のディスタンスマップ及び対応座標マップを用いた位置姿勢の評価が終了するまで最適化を行うことを特徴としている。

請求項１０記載の３次元物体認識方法は、前記位置姿勢評価ステップ又は前記位置姿勢最適化ステップにより得られた前記認識対象物の位置姿勢の評価結果を表示する表示ステップを備えることを特徴としている。

請求項１及び６に記載の発明によれば、画像上のエッジを用いた輪郭評価値と、３次元点群を用いた点群評価値との双方を融合した評価値を用いて、認識対象物の位置姿勢の評価を行うので、認識対象物の位置姿勢の誤認識を抑制し、認識精度及び処理速度を向上させることができる。

請求項２及び７に記載の発明によれば、輪郭評価値と点群評価値の比率を調整することができるので、画像からエッジが抽出できないような場合でも輪郭評価値の比率を下げて点群評価値の比率を上げることにより認識対象物の位置姿勢を認識することができる。

請求項３及び８に記載の発明によれば、画像取得手段により取得した画像を構成する各画素に、エッジ抽出手段により抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させたディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる輪郭評価値と、３次元計測手段により計測された３次元点群の画像取得手段により取得された画像上の各画像座標に、異なる方向から認識対象物を撮像した場合に得られる画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた対応座標マップ上に、あらゆる位置姿勢における３次元モデルの３次元面点を射影して照合することにより得られる点群評価値との双方を融合した評価値を用いて、認識対象物の位置姿勢の評価を行うので、認識対象物の位置姿勢の誤認識を抑制し、認識精度及び処理速度をより向上させることができる。

請求項４及び９に記載の発明によれば、画像取得手段により取得した画像の解像度を異なる比率で低下させた複数枚の画像を有する画像ピラミッドを作成する。そして、前記画像ピラミッドにおける各解像度の画像の各画素に、エッジ抽出手段により前記各解像度の画像から抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させた各解像度のディスタンスマップのうち解像度が最も低いディスタンスマップ上に、あらゆる位置姿勢における３次元モデルの３次元輪郭点を射影して照合することにより得られる輪郭評価値と、３次元計測手段により計測された３次元点群の前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像上の各画像座標に、異なる方向から認識対象物を撮像した場合に得られる各解像度の画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた各解像度の対応座標マップのうち解像度が最も低い対応座標マップ上に、あらゆる位置姿勢における３次元モデルの３次元面点を射影して照合することにより得られる点群評価値との双方を融合した評価値を用いて、認識対象物の位置姿勢の評価を行い、最も高い評価値を初期値として、認識対象物の位置姿勢の最適化を行うので、処理速度をより向上させることができる。

請求項５及び１０に記載の発明によれば、認識対象物の位置姿勢の評価結果を視覚的に容易に把握することができる。

本発明の実施形態に係る３次元物体認識装置の構成の一例を示す概略模式図である。本発明の実施形態に係る３次元物体認識装置による処理の流れを示すフローチャートである。画像ピラミッドについて説明するための概略説明図である。ディスタンスマップについて説明するための概略説明図である。対応座標マップについて説明するための概略説明図である。対応座標マップの作成の仕方の一例について説明するための概略説明図である。輪郭評価値の一例について説明するための説明図であって、（ａ）は輪郭（エッジ）までの距離と評価値の関係を示しており、（ｂ）は角度の差と評価値との関係を示している。

以下に本発明の実施形態に係る３次元物体認識装置１について、図面を参照しつつ説明する。図１に示すように、３次元物体認識装置１は、作業台２の上に載置された３次元形状を有する認識対象物３の位置姿勢を認識するためのものであって、この認識対象物３の画像の取得及び認識対象物３の表面の点の３次元座標を示す３次元点群の計測を行うための３次元センサ４と、認識対象物３を把持するためのロボットアーム５と、３次元センサ４から入力される画像や点群データに基づいてロボットアーム５の動作を制御するコンピュータ６とを備えている。

３次元センサ４は、認識対象物３の画像を取得するための機能（画像取得手段）及び認識対象物３の表面の点の３次元座標を示す３次元点群の計測を行う機能（３次元計測手段）を有するものであって、従来公知の３次元計測技術等を適用することができる。また、３次元センサ４として、例えば、認識対象物３に対してパターン光を投光する投光手段（不図示）と、このパターン光が投光された認識対象物３を異なる位置に設けられた基準カメラと参照カメラとからなるステレオカメラ（不図示）とを備え、該ステレオカメラにより撮像して得られた複数の画像間で対応する画素を特定し、対応付けられた基準画像上の画素と、参照画像上の画素との位置の差（視差）に三角測量の原理を適用することにより、基準カメラから当該画素に対応する計測対象物上の点までの距離を計測して認識対象物３の３次元点群を取得しても良い。尚、３次元センサ４の数は、特に限定されるものではなく、認識対象物３の画像の取得及び３次元点群の計測を行うことができれば良く、１台又は２台以上の複数であっても良い。また、認識対象物３の画像を取得するための機能と３次元計測を行うための機能を別々に設けるように構成されていても良い。

コンピュータ６は、図１に示すように、３次元センサ４により得られた３次元計測結果や画像データ等を記憶する画像メモリ７と、認識対象物３の認識を行うための処理プログラム等を格納するハードディスク８と、該ハードディスク８から読み出された処理プログラムを一時記憶するＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）９と、この処理プログラムに従って３次元認識処理を行うＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｅｓｓｉｎｇＵｎｉｔ）１０と、画像メモリ７に記憶された画像データやＣＰＵ１０によって求められた認識結果等を表示するための表示部１１と、マウスやキーボード等で構成される操作部１２と、これら各部を互いに接続するシステムバス１３とを有している。尚、本実施形態では、３次元物体３の認識を行う処理プログラムをハードディスク８に格納している例を示しているが、これに代えて、コンピュータ読み取り可能な記憶媒体（不図示）に格納しておき、この記録媒体から処理プログラムを読み出すように構成することも可能である。

以下、３次元物体認識装置１による処理の流れについて図２のフローチャートを用いながら説明する。本実施形態に係る３次元物体認識装置１では、図２に示すように、まずオフラインで認識対象物３の認識用の３次元モデルを作成して３次元モデル記憶手段１４に記憶しておく（Ｓ１０１）。３次元モデルは、認識対象物３の３次元形状情報を含むものであって、認識対象物３の輪郭形状を表わす３次元輪郭点データ及び表面形状を表わす３次元面点データ等を有しており、３次元ＣＡＤ等を利用して作成される。ここでは、３次元ＣＡＤ等を利用して、予めオフラインで３次元センサ４の位置から考えて可能性のある全範囲に渡って、あらゆる姿勢（３自由度）における３次元モデルを作成し、３次元モデル記憶手段１４に記憶している。尚、この３次元モデルの作成方法は、特に限定されるものではなく、認識対象物３の３次元形状情報を含むものであれば良く、従来公知の方法を用いることができる。

次に、３次元センサ４により認識対象物３の表面の点の３次元座標を示す３次元点群の計測及び認識対象物３の画像を取得する（Ｓ１０２）。そして、３次元センサ４から認識対象物３を撮影した原画像が入力されると、画像ピラミッド作成手段２１では、この入力された原画像に基づいて、画像ピラミッドを作成し（Ｓ１０３）、図１に示す画像メモリ７に記憶する。

画像ピラミッド１７は、図３に示すように、３次元センサ４により取得した原画像１８の解像度を異なる比率で低下させた複数枚の画像１８Ａ〜１８ｃを有するものである。ＣＰＵ１０では、例えば、図３に示すように、縦横両方向にそれぞれｎ個ずつのピクセルが並んだ原画像１８が入力された場合、この原画像を最高解像度画像として、１／２縮小を繰り返すことにより、縦横両方向にそれぞれｎ／２個のピクセルが並んだ第１ピラミッド画像１８Ａ、縦横両方向にそれぞれｎ／４個のピクセルが並んだ第２ピラミッド画像１８Ｂ、縦横両方向にそれぞれｎ／８個のピクセルが並んだ第３ピラミッド画像１８Ｃを作成する。尚、図３では、３段階の異なる低解像度の画像１８Ａ〜１８ｃを有する画像ピラミッド１７を作成した例を示しているが、段階数は特に限定されるものではなく、入力画像の大きさ等に応じて適宜変更しても良い。

次に、エッジ抽出手段２２では、画像ピラミッド作成手段２１により作成された画像ピラミッド１７における各解像度のそれぞれの画像１８〜１８Ｃに対して認識対象物のエッジを抽出する（Ｓ１０４）。尚、エッジの抽出の仕方は、特に限定されるものではなく、例えば、３次元計測により得られるデプス値又は面の法線方向の違いに基づいてエッジを抽出しても良いし、輝度画像からエッジを抽出しても良く、従来公知の方法を用いることができる。

次に、ＣＰＵ１０では、エッジ抽出手段２２で各解像度のそれぞれの画像から抽出されたエッジを用いて、各解像度毎のディスタンスマップを作成し、それらをディスタンスマップ記憶手段１５に記憶する（Ｓ１０５）。ディスタンスマップ１９は、図４に示すように、画像ピラミッド作成手段２１により作成された画像ピラミッド１７における各解像度の画像１８〜１８Ｃに対してそれぞれ認識対象物のエッジ２０ａ（斜線で示す部分）を抽出して、画像を構成する各画素２０に、抽出したエッジ２０ａのうち最も近い最近エッジ点までの距離と、最近エッジ点の向きとを画素値として記録させたものである。このディスタンスマップ１９は、例えば、９９５年１２月発刊の電子情報通信学会論文誌Vol.J７８、No.１２の「ユークリッド距離変換アルゴリズムの効率化」、加藤敏洋、平田富夫、斉藤豊文、吉瀬謙二、第１７５０〜１７５７頁に記載されている方法に基づいて作成されるものであり、ここではその詳細な作成の仕方の説明については省略する。このように、各解像度毎のディスタンスマップ１９を作成して記憶しておくことにより、画素２０を参照することで、その画素２０から最も近いエッジ２０ａまでの距離がわかるので、後述する位置姿勢評価手段２３及び最適化手段２４での処理速度を向上させることができる。

また、ＣＰＵ１０は、図５及び図６に示すように、３次元センサ４ａにより計測された３次元点群の画像ピラミッド作成手段２１により作成された画像ピラミッド１７における各解像度の画像Ｉ上の画像座標（μ_ｉ，ν_ｉ）に、３次元センサ４ｂで３次元センサ４ａと異なる方向から認識対象物３を撮像した場合に得られる各解像度の画像Ｋ上の、画像座標（μ_ｉ，ν_ｉ）に対応する画像座標である対応座標（μ_ｋ，ν_ｋ）をそれぞれ記憶させた各解像度毎の対応座標マップ２５を作成し、それらを対応座標マップ記憶手段１６に記憶する（Ｓ１０６）。対応座標マップ２５の作成の仕方としては、既にＳ１０２の処理で３次元センサ４ａにより画素ごとの３次元点（３次元点群）が計測されているので、例えば、図６に示すように、画像座標（μ_ｉ，ν_ｉ）の３次元点を３次元センサ４ａとは異なる方向から取得した場合の画像平面Ｋに射影し、画像座標（μ_ｉ，ν_ｉ）に対応する画像座標である対応座標（μ_ｋ，ν_ｋ）を求める。そして、この対応座標（μ_ｋ，ν_ｋ）を画像座標（μ_ｉ，ν_ｉ）に記憶する。この際、３次元センサ４ｂを平行化しておけば、ν_ｉ＝ν_ｋとなるため、処理を簡易化することができる。このような処理を画像Ｉの画素ごとに行うことにより、対応座標マップ２５を作成することができる。図５では、対応座標マップ２５を概念的に示しており、画像座標（μ_ｉ，ν_ｉ）＝（１０，２０）には、対応座標μ_ｋ＝５が記憶されている場合を示している。尚、本実施形態のように、１台の３次元センサ４ａを用いて認識対象物の画像の取得及び３次元計測を行う場合には、図６に示すように、仮想的にもう１台の３次元センサ４ｂが３次元センサ４ａとは異なる所定の位置に存在すると仮定することにより、上記のような方法で対応座標マップ２５を作成することができる。つまり、３次元センサ４ｂの焦点距離、主点座標等のカメラパラメータを全て仮想的に決定する。また、図６に示すように、３次元センサ４ａの第１センサ座標系Ｘｃ＝［ＸｃＹｃＺｃ］^Ｔと仮想の３次元センサ４ｂの第２センサ座標系との間には、回転行列Ｒ及び並進移動ベクトルｔが存在するが、これらも仮想的に決定することにより、３次元センサ４ｂの座標系Ｘｃ’＝［Ｘｃ’ Ｙｃ’ Ｚｃ’］^Ｔを求めることができる。そして、この仮想的に設定した３次元センサ４ｂの画像Ｋ上に３次元点を射影することにより、対応座標（μ_ｋ，ν_ｋ）を求めることが可能となる。また、複数のカメラ、例えば、ステレオカメラを用いて、対応座標マップ２５を作成する場合には、ステレオカメラで認識対象物を撮影し、ステレオマッチングにより、一方のカメラの画像座標（μ_ｉ，ν_ｉ）と対応する、他方のカメラの画像座標（μ_ｋ，ν_ｋ）を得ることにより、対応座標マップ２５を作成すれば良い。

次に、位置姿勢評価手段２３では、Ｓ１０５でディスタンスマップ記憶手段１５に記憶されたディスタンスマップの１９のうち解像度が最も低いディスタンスマップ、及び、Ｓ１０６で対応座標マップ記憶手段１６に記憶された対応座標マップ２５のうち解像度が最も低い対応座標マップにあらゆる位置姿勢（６自由度）の３次元モデルを照合させることにより、認識対象物３の位置姿勢の評価を行う（Ｓ１０７）。位置姿勢評価手段２３では、例えば、下記数式（２）の位置姿勢の評価値算出関数を用いて、認識対象物３の位置姿勢の評価を行う。数式（２）の評価値算出関数は、右辺第１項に示す輪郭評価値と右辺第２項に示す点群評価値を融合させた１つの評価値によって認識対象物３の位置姿勢の評価を行う関数であって、パラメータα（０≦α≦１）によって輪郭評価値と点群評価値の融合比率を調整することができる。また、数式（１）は、モデル座標系のｉ番目の３次元点を３次元センサｋの画像へ射影する数式を表わしている。

数式（２）の評価値算出関数では、下記の数式（３）によって求められる評価値Ｓ_ｋｉを輪郭の評価で用いる全ての点の数Ｉ_ｋで足し合わせて点の数Ｉ_ｋで割ったものを輪郭の評価を行う３次元センサの総数Ｋで足し合わせて３次元センサの総数Ｋで割ったものに、パラメータαを乗じた値である輪郭評価値と、下記の数式（４）によって求められる評価値Ｓ_ｌｊを点群の評価で用いる全ての点の数Ｊ_ｌで足し合わせて点の数Ｊ_ｌで割ったものを点群の評価を行う３次元センサの総数Ｌで足し合わせて３次元センサの総数Ｌで割ったものに、パラメータ（１−α）を乗じた値である点群評価値とを足し合わせたものである。尚、本実施形態のように輪郭の評価及び点群の評価に同じ３次元センサを用いて行う場合には、Ｋ＝Ｌとなる。

数式（３）に示す評価値Ｓ_ｋｉは、下記の数式（５）に示すように、３次元モデルの３次元輪郭点を３次元センサｋの画像上へ射影し、ディスタンスマップを参照させることにより求められる、エッジまでの距離に基づく評価値と、下記の数式（６）に示すように、３次元モデルの３次元輪郭点を３次元センサｋの画像上へ射影した射影点の勾配方向と最近エッジの勾配方向の差に基づく評価値とを掛け合わせたものである。

数式（５）では、例えば、図７（ａ）に示すように、射影点と最近エッジまでの距離の閾値τ_ａが設定されており、最近エッジまでの距離の差が小さい時には、この関数は１に近い数字を出力し、差が大きくなるにつれて０に近づく結果を出力する。そして、最近エッジまでの距離の自乗が閾値τ_ａの自乗よりも大きくなる場合には、０を出力結果とする。尚、図７（ａ）では、閾値τ_ａ＝４．０に設定した例を示しているが、この閾値τ_ａの値は、特に限定されるものではなく、求められる認識精度等に応じて適宜設定されるものである。また、数式（６）では、例えば、図７（ｂ）に示すように、射影点の勾配方向と最近エッジの勾配方向との内積の閾値τ_β１、τ_β２が設定されており、最近エッジの勾配方向との差の自乗が閾値τ_β１の自乗以下の時には１を出力し、最近エッジの勾配方向との差の自乗が閾値τ_β１の自乗より大きく、τ_β２の自乗より小さい時には、勾配方向の差が閾値τ_β１に近づくにつれて関数は１に近い数値を出力し、勾配方向の差が閾値τ_β２に近づくにつれて関数は０に近づくような結果を出力する。そして、最近エッジの勾配方向との差の自乗が閾値τ_β２の自乗以上の場合には、０を出力結果とする。尚、図７（ｂ）では、閾値τ_β１＝ｃｏｓ２０°、τ_β２＝ｃｏｓ３６°に設定した例を示しているが、この閾値τ_β１、τ_β２の値は、特に限定されるものではなく、求められる認識精度等に応じて適宜設定されるものである。また、数式（５）及び数式（６）は、輪郭を用いた評価に関する関数の一例であり、輪郭の類似度を評価する関数はこれに限られるものではない。

数式（４）に示す評価値Ｓ_ｌｊは、下記の数式（７）に示すように、３次元モデルの３次元面点を３次元センサｋの画像上へ射影し、対応座標マップを参照させることにより得た対応座標と３次元モデルの３次元面点を３次元センサｌの画像上に射影して得た画像座標との距離の差に基づく評価値と、下記の数式（８）に示すように、射影された３次元モデル面上の３次元面点の法線方向と、射影先の画素に対応する３次元計測点の法線方向との内積の差に基づく評価値とを掛け合わせたものである。

数式（７）では、対応座標からの距離の閾値τ_ｂが設定されており、対応座標からの距離の差が小さい時には、この関数は１に近い数字を出力し、差が大きくなるにつれて０に近づく結果を出力する。そして、対応座標からの距離の自乗が閾値τ_ｂの自乗よりも大きくなる場合には、０を出力結果とするものである。また、数式（８）では、射影された３次元モデル面上の３次元面点の法線方向と射影先の画素に対応する３次元計測点の法線方向との内積の閾値τ_γ１、τ_γ２が設定されており、法線方向の差の自乗が閾値τ_γ１の自乗以下の時には１を出力し、法線方向の差の自乗が閾値τ_γ１の自乗より大きく、τ_β２の自乗より小さい時には、法線方向の差が閾値τ_γ１に近づくにつれて関数は１に近い数値を出力し、法線方向の差が閾値τ_γ２に近づくにつれて関数は０に近づくような結果を出力する。そして、法線方向の差の自乗が閾値τ_γ２の自乗以上の場合には、０を出力結果とする。尚、数式（７）及び数式（８）は、点群を用いた評価に関する関数の一例であり、点群の類似度を評価する関数はこれに限られるものではない。

位置姿勢評価手段２３では、例えば、このように数式（２）に示すような評価値算出関数を用いて、認識対象物の位置姿勢の評価を行い、最も評価値の高いものを初期値解候補とする。尚、本実施形態では、３次元モデルを画像上に射影して照合を行っている例を示しているが、３次元モデルを初期探索用に射影した状態のテンプレートを作成し、このテンプレートを用いて認識対象物の位置姿勢の評価を行うようにしても良い。これにより、更に処理速度を向上させることができる。

次に、最適化手段２４では、位置姿勢評価手段２３により得られた初期値解候補を、より高解像度のディスタンスマップと対応座標マップを用いて、最適化を実行し（Ｓ１０８）、位置姿勢の認識精度をより高精度にする。最適化手段２４では、位置姿勢評価手段２３により得られた解候補の位置姿勢を初期値とし、その位置姿勢の３次元モデルをより高解像度である画像に射影し、ディスタンスマップと対応座標マップを用いて、位置姿勢評価手段２３と同様に評価値を計算する。この際、評価値の最大化を実現する手法として、例えば、従来公知のマーカード法等を用いることができる。尚、最適化の手法は、これに限定されるものではなく、従来公知の他の最適化法を用いても良い。

そして、Ｓ１０８で最適化された評価結果に基づき、位置及び姿勢が必要な精度を満たしているか否かを判定し（Ｓ１０９）、必要な精度を満たしていると判断した場合は（Ｓ１０９：ＹＥＳ）、その結果を最終結果として出力し（Ｓ１１０）、処理を終了する。一方、必要な精度を満たしていないと判断した場合は（Ｓ１０９：ＮＯ）、まだ位置及び姿勢を評価していない高解像度のディスタンスマップ及び対象座標マップがあるか否かを判定し（Ｓ１１１）、そのような未処理のディスタンスマップ及び対象座標マップがないと判断した場合は（Ｓ１１１：ＮＯ）、その時点での結果を最終結果として出力し（Ｓ１１０）、処理を終了する。一方、未処理のディスタンスマップ及び対象座標マップがあると判断した場合は（Ｓ１１１：ＹＥＳ）、Ｓ１０８へ戻って残りのディスタンスマップ及び対象座標マップについて同様の処理を行う。そして、未処理のディスタンスマップ及び対象座標マップが無くなるまでこれを繰り返す。このように、必要な精度に達するまで、より解像度の高いディスタンスマップ及び対象座標マップを用いて処理を行うことにより、認識対象物３の位置及び姿勢をより高い精度で認識することができる。

３次元物体認識装置１では、このように画像上のエッジを用いた輪郭評価値と、３次元点群を用いた点群評価値との双方を融合した評価値を用いて、認識対象物３の位置姿勢の評価を行うので、認識対象物３の位置姿勢の誤認識を抑制し、認識精度及び処理速度を向上させることができる。また、数式（２）のパラメータαを調整することで、輪郭評価値と点群評価値の融合比率を調整することができるので、画像からエッジが抽出できないような場合でも輪郭評価値の比率を下げて点群評価値の比率を上げることにより認識対象物の位置姿勢を認識することができる。

尚、本発明の実施の形態は上述の形態に限るものではなく、本発明の思想の範囲を逸脱しない範囲で適宜変更することができる。

本発明に係る３次元物体認識装置及び３次元物体認識方法は、生産ライン等における部品等の認識を行うための技術として有効に利用することができる。また、サービスロボットが部屋の中等で自分の位置姿勢を特定する技術としても有効に利用することができる。

１３次元物体認識装置
３認識対象物
４３次元センサ
１１表示部
１４３次元モデル記憶手段
１５ディスタンスマップ記憶手段
１６対応座標マップ記憶手段
１７画像ピラミッド
１８画像
１９ディスタンスマップ
２０画素
２１画像ピラミッド作成手段
２２エッジ抽出手段
２３位置姿勢評価手段
２４最適化手段
２５対応座標マップ

Claims

認識対象物の輪郭及び表面形状を表わす３次元モデルを記憶する３次元モデル記憶手段と、
前記認識対象物を所定方向から撮影して画像を取得する画像取得手段と、
前記認識対象物の表面の点の３次元座標を示す３次元点群を計測する３次元計測手段と、
前記画像取得手段により取得した画像又は前記３次元計測手段により得られた３次元計測結果から前記認識対象物のエッジを抽出するエッジ抽出手段と、
前記エッジ抽出手段により抽出されたエッジとあらゆる位置姿勢における前記３次元モデルの輪郭形状を示す３次元輪郭点に基づいて求められる輪郭評価値、及び、前記３次元計測手段により得られた３次元計測結果とあらゆる位置姿勢における前記３次元モデルの表面形状を示す３次元面点に基づいて求められる点群評価値を用いて前記認識対象物の位置姿勢を評価する位置姿勢評価手段と、を備えることを特徴とする３次元物体認識装置。
前記位置姿勢評価手段は、前記輪郭評価値と前記点群評価値の比率を調整することを特徴とする請求項１に記載の３次元物体認識装置。
前記画像取得手段により取得した画像を構成する各画素に、前記エッジ抽出手段により抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させたディスタンスマップを記憶するディスタンスマップ記憶手段と、
前記３次元計測手段により計測された３次元点群の前記画像取得手段により取得された画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた対応座標マップを記憶する対応座標マップ記憶手段と、を備え、
前記位置姿勢評価手段は、前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価することを特徴とする請求項１又は２に記載の３次元物体認識装置。
前記画像取得手段により取得した画像の解像度を異なる比率で低下させた複数枚の画像を有する画像ピラミッドを作成する画像ピラミッド作成手段と、
前記位置姿勢評価手段により得られた最も高い評価値を初期値として用いて、前記認識対象物の位置姿勢の最適化を行う位置姿勢最適化手段と、を備え、
前記エッジ抽出手段は、前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像から前記認識対象物のエッジを抽出し、
前記ディスタンスマップ記憶手段は、前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像の各画素に、前記エッジ抽出手段により前記各解像度の画像から抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させた各解像度のディスタンスマップを記憶し、
前記対応座標マップ記憶手段は、前記３次元計測手段により計測された３次元点群の前記画像ピラミッド作成手段により作成された画像ピラミッドにおける各解像度の画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる各解像度の画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた各解像度の対応座標マップを記憶し、
前記位置姿勢評価手段は、解像度が最も低い前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、解像度が最も低い前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価し、
前記位置姿勢最適化手段は、前記位置姿勢評価手段により得られた最も高い評価値を初期値とし、該初期値を前記位置姿勢評価手段で用いられた前記ディスタンスマップ及び前記対応座標マップよりも高い解像度のディスタンスマップ及び対応座標マップを用いて、予め設定された精度に到達又は最も高い解像度のディスタンスマップ及び対応座標マップを用いた位置姿勢の評価が終了するまで最適化を行うことを特徴とする請求項３に記載の３次元物体認識装置。
前記位置姿勢評価手段又は前記位置姿勢最適化手段により得られた前記認識対象物の位置姿勢の評価結果を表示する表示手段を備えることを特徴とする請求項４に記載の３次元物体認識装置。
認識対象物の輪郭及び表面形状を表わす３次元モデルを３次元モデル記憶手段に記憶する３次元モデル記憶ステップと、
前記認識対象物を所定方向から撮影して画像を取得する画像取得ステップと、
前記認識対象物の表面の点の３次元座標を示す３次元点群を計測する３次元計測ステップと、
前記画像取得ステップで取得した画像又は前記３次元計測ステップで得られた３次元計測結果から前記認識対象物のエッジを抽出するエッジ抽出ステップと、
前記エッジ抽出ステップで抽出されたエッジとあらゆる位置姿勢における前記３次元モデルの輪郭形状を示す３次元輪郭点に基づいて求められる輪郭評価値、及び、前記３次元計測ステップで得られた３次元計測結果とあらゆる位置姿勢における前記３次元モデルの表面形状を示す３次元面点に基づいて求められる点群評価値を用いて前記認識対象物の位置姿勢を評価する位置姿勢評価ステップと、を備えることを特徴とする３次元物体認識方法。
前記位置姿勢評価ステップは、前記輪郭評価値と前記点群評価値の比率を調整することを特徴とする請求項６に記載の３次元物体認識方法。
前記画像取得ステップで取得した画像を構成する各画素に、前記エッジ抽出ステップで抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させたディスタンスマップをディスタンスマップ記憶手段に記憶するディスタンスマップ記憶ステップと、
前記３次元計測ステップで計測された３次元点群の前記画像取得ステップにより取得された画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた対応座標マップを対応座標マップ記憶手段に記憶する対応座標マップ記憶ステップと、を備え、
前記位置姿勢評価ステップは、前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価することを特徴とする請求項６又は７に記載の３次元物体認識方法。
前記画像取得ステップで取得した画像の解像度を異なる比率で低下させた複数枚の画像を有する画像ピラミッドを作成する画像ピラミッド作成ステップと、
前記位置姿勢評価ステップで得られた最も高い評価値を初期値として用いて、前記認識対象物の位置姿勢の最適化を行う位置姿勢最適化ステップと、を備え、
前記抽出ステップは、前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像から前記認識対象物のエッジを抽出し、
前記ディスタンスマップ記憶ステップは、前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像の各画素に、前記エッジ抽出ステップで前記各解像度の画像から抽出されたエッジのうち最も近いエッジまでの距離と、前記最も近いエッジの向きとを画像値として記憶させた各解像度のディスタンスマップを前記ディスタンスマップ記憶手段に記憶し、
前記対応座標マップ記憶ステップは、前記３次元計測ステップで計測された３次元点群の前記画像ピラミッド作成ステップで作成された画像ピラミッドにおける各解像度の画像上の各画像座標に、前記所定方向と異なる方向から前記認識対象物を撮像した場合に得られる各解像度の画像の前記各画像座標に対応する画像座標である対応座標をそれぞれ記憶させた各解像度の対応座標マップを前記対応座標マップ記憶手段に記憶し、
前記位置姿勢評価ステップは、解像度が最も低い前記ディスタンスマップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元輪郭点を射影して照合することにより得られる前記輪郭評価値、及び、解像度が最も低い前記対応座標マップ上に、あらゆる位置姿勢における前記３次元モデルの前記３次元面点を射影して照合することにより得られる前記点群評価値を用いて前記認識対象物の位置姿勢を評価し、
前記位置姿勢最適化ステップは、前記位置姿勢評価ステップで得られた最も高い評価値を初期値とし、該初期値を前記位置姿勢評価ステップで用いられた前記ディスタンスマップ及び前記対応座標マップよりも高い解像度のディスタンスマップ及び対応座標マップを用いて、予め設定された精度に到達又は最も高い解像度のディスタンスマップ及び対応座標マップを用いた位置姿勢の評価が終了するまで最適化を行うことを特徴とする請求項８に記載の３次元物体認識方法。
前記位置姿勢評価ステップ又は前記位置姿勢最適化ステップにより得られた前記認識対象物の位置姿勢の評価結果を表示する表示ステップを備えることを特徴とする請求項９に記載の３次元物体認識方法。