JP6936974B2 - 位置姿勢推定装置、位置姿勢推定方法及びプログラム - Google Patents

Description

本発明は、物体の位置及び姿勢を推定する位置姿勢推定装置、位置姿勢推定方法及びプログラムに関する。

近年におけるロボット技術の飛躍的な発展に伴い、人手で行っていた組立や加工といった作業をロボットによって自動化する取り組みが増えてきている。ロボットにより組立や加工を自動化する際、部品や搬送物など、ロボットが把持する物体（以下、「ワーク」ともいう。）の位置及び姿勢（以下、「位置姿勢」ともいう。）の推定を要することがある。従来、ワークの位置姿勢を推定するために、ワークをカメラで撮像し、得られた画像（二次元画像。以下、「２Ｄ画像」ともいう。）に基づいて、ワークの位置姿勢を推定する手法が各種提案されている。

特許文献１には、三次元形状が既知であるワークの位置姿勢を推定する際に、距離センサで計測されたワーク表面の三次元座標データ（３Ｄデータ）とカメラで撮像した画像（２Ｄ画像）の情報を１つの誤差関数にまとめて、６つのパラメータ（具体的には、ｘ，ｙ，ｚ軸に沿った３つの並進パラメータと、ｘ，ｙ，ｚ軸まわりの３つの回転パラメータ）を最適化することにより、３Ｄデータと２Ｄ画像の両方に最も当てはまりのよいワークの位置姿勢を推定する手法が開示されている。なお、以下では、説明の便宜上、ｘ，ｙ，ｚ軸に沿った３つの並進パラメータを、それぞれ並進ｘ，ｙ，ｚと呼び、ｘ，ｙ，ｚ軸まわりの３つの回転パラメータを、それぞれ回転φ，γ，θと呼ぶ。

特許第５３９３３１８号

しかしながら、一般に、２Ｄ画像は物体の大きさの変化、すなわちカメラの奥行方向の変化が現れにくいため、２Ｄ画像を用いてワークの位置姿勢を推定すると、カメラの奥行方向のパラメータである並進ｚと回転φ及び回転γの推定精度が低くなる。そのため、特許文献１のように、２Ｄ画像も用いてカメラの奥行方向を含むワークの位置姿勢を推定する手法の場合、推定された位置姿勢は、カメラの奥行方向の推定精度が低いという問題があった。

本発明は、以上説明した事情を鑑みてなされたものであり、従来に比して、カメラの奥行方向に対する物体の位置姿勢の推定精度を向上することが可能な位置姿勢推定装置等を提供することを目的とする。

本発明の一態様に係る位置姿勢推定装置は、物体の位置姿勢を推定する推定装置であって、物体の二次元画像を取得する第１取得部と、物体の三次元形状をあらわす三次元モデルを取得する第２取得部と、物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を取得する第３取得部と、三次元モデルと二次元画像とを用いて、二次元画像平面上の異なる２つの軸であるｘ軸及びｙ軸方向それぞれの並進と、当該ｘ軸及びｙ軸に直交するｚ軸を中心とする回転とについて、位置姿勢情報を更新する第２の位置姿勢推定部と更新された位置姿勢情報によってあらわされる位置姿勢を、物体の位置姿勢として出力する出力部とを具備することを要旨とする。
かかる構成によれば、第２の位置姿勢推定部は、物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を受け取ると、二次元画像で精度良く推定可能なｘ軸及びｙ軸方向それぞれの並進（すなわち並進ｘ，ｙ）と、ｘ軸及びｙ軸に直交するｚ軸を中心とする回転（すなわち、回転θ）のみを最適化する。
これにより、二次元画像を用いた３次元位置姿勢推定の欠点である、カメラの奥行方向の位置（すなわち、並進ｚ）と回転（すなわち、回転φ及び回転γ）の３パラメータについては精度良く推定できないという従来手法の問題を未然に防ぐことができ、従来に比して、カメラの奥行方向に対する物体の位置姿勢の推定精度を向上することが可能となる。

上記構成にあっては、第２の位置姿勢推定部は、三次元モデルを、位置姿勢情報に基づいて、二次元画像と共通の平面に投影した二次元モデルを生成する二次元モデル生成部と、二次元画像と二次元モデルの位置合わせを行うことにより、ｘ軸及びｙ軸方向それぞれの並進パラメータと、ｚ軸を中心とする回転パラメータとを最適化する最適化部と、最適化された３つのパラメータを用いて、位置姿勢情報を更新する更新部とを具備する態様であってもよい。

上記構成にあっては、物体について計測された物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、三次元モデルと三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部と、をさらに備え、第１の位置姿勢推定部には、第３取得部で取得した位置姿勢情報が入力され、第１の位置姿勢推定部で更新された位置姿勢情報は第２の位置姿勢推定部に渡される態様であってもよい。

上記構成にあっては、物体について計測された物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、三次元モデルと三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部とをさらに備え、第１の位置姿勢推定部には、第２の位置姿勢推定部で更新された位置姿勢情報が入力され、第１の位置姿勢推定部で更新された位置姿勢情報は出力部に渡される態様であってもよい。

上記構成にあっては、物体について計測された物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、三次元モデルと三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部とをさらに備え、第３取得部で取得された位置姿勢情報は、第１及び第２の位置姿勢推定部にそれぞれ渡され、第１及び第２の位置姿勢推定部でそれぞれ更新された位置姿勢情報は前記出力部に渡され、出力部は、第１及び第２の位置姿勢推定部で更新されたそれぞれの位置姿勢情報に基づいて、物体の位置姿勢を出力する態様であってもよい。

上記構成にあっては、第１の位置姿勢推定部は、位置合わせを行う際、三次元座標データの各点に対応する三次元モデルの各点を、対応する三次元座標データの各点に近づけるように、ｘ軸、ｙ軸及びｚ軸方向それぞれの並進パラメータと、ｘ軸、ｙ軸及びｚ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された前記６つのパラメータを用いて、前記位置姿勢情報を更新する態様であってもよい。

上記構成にあっては、第１の位置姿勢推定部は、位置合わせを行う際、三次元座標データの各点に対応する三次元モデルの各点を、対応する三次元座標データの各点に近づけるように、z軸方向の並進パラメータと、ｘ軸及びｙ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された３つのパラメータを用いて、位置姿勢情報を更新する態様であってもよい。

本発明によれば、３Ｄデータと２Ｄ画像の両方の情報を用いつつも、従来に比してワークの位置姿勢の推定精度を向上することが可能となる。

物体認識装置の全体構成を示す図である。 物体認識装置のハードウェア構成を示す図である。 画像処理装置の構成を示す図である。 第１の位置合わせ処理を示すフローチャートである。 第２の位置合わせ処理を示すフローチャートである。 ワークの位置姿勢を例示した図である。

以下、図面を参照して本発明の実施形態について詳細に説明する。

Ａ．本実施形態
Ａ−１．適用例
図１を参照して、本発明が適用される場面の一例について説明する。

物体認識装置１は、ピッキングシステムを備えた生産ラインなどに設置され、カメラ１１ａから取り込まれた画像を用いて、テンプレートマッチングによりトレイ３上の物体２の位置・姿勢を認識（３次元の物体認識）するシステムである。トレイ３上には、認識対象の物体（すなわち、ワーク）２がバラ積みされている。物体認識装置１は、カメラ１１ａから所定の時間間隔で画像を取り込み、画像処理装置１０によって画像に含まれる各ワーク２の位置及び姿勢を認識する処理を実行し、その結果を例えばＰＬＣ（プログラマブルロジックコントローラ）４やディスプレイ１２などに出力する。物体認識装置１の出力である認識結果は、例えば、ピッキング・ロボットの制御、加工装置や印字装置の制御、ワーク２の検査や計測などに利用される。

Ａ−２．（ハードウェア構成）
図２を参照して、物体認識装置１のハードウェア構成を説明する。物体認識装置１は、概して、３Ｄセンサ１１と画像処理装置１０から構成される。
３Ｄセンサ１１は、ワーク２を３次元計測するためのセンサであり、カメラ１１ａを備えている。カメラ１１ａは、ワーク２のデジタル画像を画像処理装置１０に取り込むための撮像デバイスであり、例えばＣＭＯＳ（Complementary Metal-Oxide-Semiconductor）カメラやＣＣＤ（Charge-Coupled Device）カメラを好適に用いることができる。解像度、カラー／モノクロ、静止画像／動画、階調、データ形式などの入力画像の形式は任意であり、ワーク２の種類やセンシングの目的に合わせて適宜選択することができる。Ｘ線画像やサーモ画像など、可視光像以外の特殊な画像を物体認識や検査に利用する場合には、その画像に合わせたカメラを用いてもよい。

画像処理装置１０は、ＣＰＵ（中央演算処理装置）１０ａと、ワークメモリとして用いられるメインメモリ１０ｂと、固定記憶部であるハードディスク１０ｅと、カメラインターフェイス１０ｃと、入力インターフェイス１０ｆと、表示コントローラ１０ｇと、ＰＬＣインターフェイス１０ｈと、通信インターフェイス１０ｉと、データリーダ／ライタ１０ｊとを含む。これらの各部は、バス１０ｋを介して、互いにデータ通信可能に接続されている。

カメラインターフェイス１０ｃは、ＣＰＵ１０ａとカメラ１１ａとの間のデータ伝送を仲介する部分であり、カメラ１１ａからの画像データを一時的に蓄積するための画像バッファ１０ｄを有していてもよい。入力インターフェイス１０ｆは、ＣＰＵ１０ａと入力部との間のデータ伝送を仲介する。入力部には、マウス１３、キーボード、タッチパネル、ジョグコントローラなどが含まれていてもよい。表示コントローラ１０ｇは、液晶モニタなどのディスプレイ１２に接続され、当該ディスプレイでの表示を制御する。ＰＬＣインターフェイス１０ｈは、ＣＰＵ１０ａとＰＬＣ４との間のデータ伝送を仲介する。通信インターフェイス１０ｉは、ＣＰＵ１０ａとコンソール、あるいはパーソナルコンピュータやサーバ装置などとの間のデータ伝送を仲介する。データリーダ／ライタ１０ｊは、ＣＰＵ１０ａと記録媒体であるメモリカード１４との間のデータ伝送を仲介する。

画像処理装置１０は、汎用的なアーキテクチャを有するコンピュータで構成可能であり、ＣＰＵ１０ａが、ハードディスク１０ｅ又はメモリカード１４に格納されたプログラムを読み込むことで、各種処理を実行する。このようなプログラムは、メモリカード１４や光ディスクなどのコンピュータ読取可能な記録媒体に格納された状態で流通するか、インターネットなどを通じて提供される。なお、本実施形態に係るプログラムは単体のアプリケーションプログラムとして提供されてもよいし、他のプログラムの一部に組み込まれるモジュールとして提供されてもよい。また、そのプログラムによって実行される処理の一部又は全部をＡＳＩＣなどの専用回路で実行してもよい。

Ａ−３．（画像処理装置の構成）
図３に、画像処理装置１０の構成を示す。画像処理装置１０は、ＣＰＵ１０ａが、ハードディスク１０ｅ等に格納されたプログラムを読み込み、実行することで、テンプレート作成装置１２０、３次元概略位置姿勢認識装置１３０、３次元詳細位置姿勢推定装置１４０として動作する。

＜テンプレート作成装置１２０＞
テンプレート作成装置１２０は、ワーク２の物体の輪郭の特徴や内部凹凸形状の特徴を表すテンプレートを作成する。詳述すると、テンプレート作成装置１２０は、対象物体２の３ＤＣＡＤ、もしくは３Ｄセンサ１１から取得した３Ｄデータを用いて、様々な視点から見たワーク２の２Ｄ画像を生成する。そして、テンプレート作成装置１２０は、生成した輝度画像および距離画像に基づき、複数のテンプレートを作成する。テンプレート作成装置１２０は、作成した複数のテンプレートをテンプレート情報として３次元概略位置姿勢認識装置１３０及び３次元詳細位置姿勢推定装置１４０に供給する。なお、テンプレート（すなわち、３Ｄモデル）には任意の形式を用いることができ、例えば、テンプレートとして画像中の複数の特徴点の特徴量を記述した配列形式を用いることができる。特徴量としては、例えば、ピクセル値（輝度）、輝度勾配方向、量子化勾配方向、Ｈｏｇ（Histograms of Oriented Gradients）、ＨＡＡＲ−Ｌｉｋｅ、ＳＩＦＴ（Scale-Invariant Feature Transform）のほか、距離画像を利用した特徴量として法線方向、法線方向分布などを用いることができる。

＜３次元概略位置姿勢認識装置１３０＞
３次元概略位置姿勢認識装置１３０は、３Ｄセンサ１１のカメラ１１ａから取り込まれた２Ｄ画像及び３Ｄデータに対し、テンプレート作成装置１２０から供給されるテンプレート情報を用いてテンプレートマッチングを行い、画像中の３次元物体（すなわち、ワーク２）の位置姿勢を認識する。詳述すると、３次元概略位置姿勢認識装置１３０は、まず、３Ｄセンサ１１からワーク２の入力画像を取り込み、取り込んだ入力画像（元画像）から、異なる解像度を持つ画像の集合（いわゆる画像ピラミッド）を作成する。３次元概略位置姿勢認識装置１３０は、作成した画像ピラミッドに対し特徴抽出処理を施すことで、ワーク２の入力画像の特徴量を抽出する。ここで、抽出される特徴量は、テンプレートのものと同種の特徴であり、例えばピクセル値などが挙げられる。３次元概略位置姿勢認識装置１３０は、抽出したワーク２の入力画像の特徴量と、テンプレート作成装置１２０から供給されるワーク２のテンプレート情報とを用いてテンプレートマッチングを行う。３次元概略位置姿勢認識装置１３０は、テンプレートマッチングにより、３Ｄモデルの大まかな位置姿勢を認識し、認識結果を３Ｄモデルの更新前の位置姿勢をあらわす更新前位置姿勢情報として３次元詳細位置姿勢推定装置１４０に供給する。

＜３次元詳細位置姿勢推定装置１４０＞
３次元詳細位置姿勢推定装置１４０は、テンプレート作成装置１２０から供給されるテンプレート情報及び３次元概略位置姿勢認識装置１３０から供給される更新前位置姿勢情報を利用して、ワーク２の３次元位置姿勢の推定を行う。３次元詳細位置姿勢推定装置１４０は、画像取得部１４１、３Ｄモデル取得部１４２、更新前位置姿勢取得部１４３、カメラパラメータ取得部１４４、第１の位置姿勢推定部１４５、第２の位置姿勢推定部１４６、３次元位置合わせ結果出力部１４７を備えている。

３次元詳細位置姿勢推定装置１４０は、第１の位置姿勢推定部１４５において３Ｄデータを用いた３次元位置姿勢推定を行う一方、第２の位置姿勢推定部１４６において２Ｄ画像を用いた３次元位置姿勢推定を行う。ここで、第２の位置姿勢推定部１４６は、第１の位置姿勢推定部１４５における３Ｄデータを用いた３次元位置姿勢推定の結果を基に、２Ｄ画像で精度良く推定可能な並進ｘ、ｙと回転θの３パラメータのみを最適化する点に１つの特徴がある（詳細は後述）。以下、各部について説明する。

画像取得部（第１取得部、第４取得部）１４１は、３Ｄセンサ１１からワーク２の３Ｄデータ（三次元座標データ）や２Ｄ画像を取得する。画像取得部１４１は、取得した３Ｄデータを第１の位置姿勢推定部１４５に出力するとともに、取得した２Ｄ画像を第２の位置姿勢推定部１４６に出力する。

３Ｄモデル取得部（第２取得部）１４２は、テンプレート作成装置１２０から３Ｄモデルをあらわすテンプレート情報を取得すると、取得したテンプレート情報を３Ｄモデルデータとして、第１の位置姿勢推定部１４５及び第２の位置姿勢推定部１４６に出力する。

更新前位置姿勢取得部（第３取得部）１４３は、３次元概略位置姿勢認識装置１３０において認識された３Ｄモデルの大まかな位置姿勢をあらわす更新前位置姿勢情報を取得し、取得した更新前位置姿勢情報を、第１の位置姿勢推定部１４５に出力する。なお、更新前位置姿勢情報は、３つの並進パラメータ（ｘ，ｙ，ｚ）と、ｘ軸、ｙ軸、ｚ軸まわりの３つの回転パラメータ（φ，γ，θ）とを含む６つのパラメータ（ｘ，ｙ，ｚ，φ，γ，θ）で表現される。

カメラパラメータ取得部１４４は、３Ｄセンサ１１がワーク２の３Ｄデータや２Ｄ画像を生成する際に利用したカメラ１１ａの内部パラメータを、メインメモリ１１２などから取得し、取得した内部パラメータを、第１の位置姿勢推定部１４５及び第２の位置姿勢推定部１４６に出力する。内部パラメータとしては、例えばカメラ１１ａの焦点距離やカメラ１１ａの中心位置などが挙げられる。

第１の位置姿勢推定部１４は、カメラパラメータ取得部１４４から供給されるカメラ１１ａの内部パラメータとともに、更新前位置姿勢取得部１４３から供給される３Ｄモデルの更新前位置姿勢情報、３Ｄモデル取得部１４２から供給される３Ｄモデルデータ、画像取得部１４１から供給される３Ｄデータを利用して、３Ｄモデルデータの３Ｄ点群と３Ｄデータの３Ｄ点群の位置合わせを行う（以下、「第１の位置合わせ処理」という；詳細は後述）。なお、本実施形態では、第１の位置合わせ処理の際に、３Ｄ点群同士の位置合わせを行うための手法としてＩＣＰ(Iterative Closest Point)アルゴリズムを利用する場合を想定するが、その他の手法を用いてもよい。第１の位置姿勢推定部１４は、第１の位置合わせ処理を行うことで、３Ｄモデルの位置姿勢を更新し、更新後の３Ｄモデルの位置姿勢をあらわす更新３Ｄモデルデータ（第１の位置姿勢情報）を、第２の位置姿勢推定部１５に出力する。

第２の位置姿勢推定部１５は、カメラパラメータ取得部１４４から供給されるカメラ１１ａの内部パラメータとともに、第１の位置姿勢推定部１４から供給される更新３Ｄモデルデータ、画像取得部１４１から供給される２Ｄ画像を利用して、２Ｄ画像平面にあたる並進ｘ、ｙと回転θの位置合わせを行う（以下、「第２の位置合わせ処理」という；詳細は後述）。第２の位置姿勢推定部１５は、第２の位置合わせを行うことで、３Ｄモデルの位置姿勢を最終更新する。そして、第２の位置姿勢推定部１５は、最終更新後の３Ｄモデルの位置姿勢をあらわす最終更新３Ｄモデルデータ（第２の位置姿勢情報）を、３次元位置合わせ結果出力部１４７に出力する。

３次元位置合わせ結果出力部（出力部）１４７は、第２の位置姿勢推定部１５から供給される最終更新３Ｄモデルデータを、外部装置や液晶パネルなどに出力する。最終更新３Ｄモデルデータは、例えば、ワークの検査・計測のほか、ワークの位置決めや組立、ピッキング・ロボットの制御などに利用される。

Ａ−４．（第１の位置合わせ処理）
以下、第１の位置姿勢推定部１４によって実行される第１の位置合わせ処理を説明する。図４は、第１の位置合わせ処理を示すフローチャートである。

第１の位置姿勢推定部１４は、３Ｄモデルデータと３Ｄデータを用いて、対応点探索を行う（ステップＳ１００）。対応点探索は、３Ｄ空間上において、３Ｄモデルの各点（すなわち３Ｄ座標）につき、最近傍となる３Ｄデータの各点（すなわち３Ｄ座標）を探索していく処理を意味する。代表的な対応点探索方法として、下記第１及び第２の方法が知られているが、第２の方法は、第１の方法に比べて探索範囲が狭く、高速に処理できることから、本実施形態では第２の方法を採用する。
[第１の方法]
３Ｄ空間上において、直接、３Ｄモデルの各点について最近傍となる３Ｄデータの各点を探索していく方法。
[第２の方法]
カメラ１１ａの内部パラメータを用いて、３Ｄモデルの各点を３Ｄデータと同じ２Ｄ画像平面へ投影し、２Ｄ画像平面上において投影された３Ｄモデルの各点について、近い距離にある３Ｄデータの各点から、最近傍となる３Ｄデータの各点を探索していく方法。

第１の位置姿勢推定部１４は、上記対応点探索で見つかった３Ｄモデルの各点と３Ｄデータの各点のペア（以下、「対応点ペア」ともいう。）の間の誤差を、「点と点の距離」や「点と面の距離」などの誤差メトリック（差異を定量化するもの）を用いて算出する。そして、第１の位置姿勢推定部１４は、目的関数（誤差関数）を最小化するように、３Ｄモデルの各点を、対応する３Ｄデータの各点に近づけるための６パラメータの差分（Δｘ，Δｙ，Δｚ，Δφ，Δγ，Δθ）を求める。そして、第１の位置姿勢推定部１４は、求めた６パラメータの差分（Δｘ，Δｙ，Δｚ，Δφ，Δγ，Δθ）を用いて６パラメータを更新することで、最適化した６パラメータ（ｘ+Δｘ，ｙ+Δｙ，ｚ+Δｚ，φ+Δφ，γ+Δγ，θ+Δθ）を求める（ステップＳ１１０）。

第１の位置姿勢推定部１４は、最適化した６パラメータを用いて、３Ｄモデルの更新前の位置姿勢を更新する（ステップＳ１２０）。すなわち、第１の位置姿勢推定部１４は、更新前位置姿勢取得部１４３から受け取った更新前位置姿勢情報をもとに、３Ｄモデルの更新前の位置姿勢を把握し、把握した３Ｄモデルの更新前の位置姿勢を、最適化した６パラメータを用いて更新する。

第１の位置姿勢推定部１４は、３Ｄモデルの更新前の位置姿勢を更新すると、３Ｄモデルが、３Ｄデータに十分に近づいた状態で位置合わせができたか否かを判定（以下、「収束判定」ともいう。）する（ステップＳ１３０）。収束判定の方法について一例を挙げると、第１の位置姿勢推定部１４は、３Ｄモデルの３Ｄ点群の前回の位置姿勢Ｐｏｓｅ（ｎ−１）と、今回の位置姿勢Ｐｏｓｅ（ｎ）とを比較し、位置姿勢の更新幅Ｌｕｐを求め、求めた位置姿勢の更新幅Ｌｕｐが設定されている収束閾値Ｄｔｈを下回ったか否かを確認する。第１の位置姿勢推定部１４は、求めた位置姿勢の更新幅Ｌｕｐが設定されている収束閾値Ｄｔｈを下回っている場合には、３Ｄモデルの３Ｄ点群が、３Ｄデータの点群に十分に近づいた状態で位置合わせができたと判断する一方、求めた位置姿勢の更新幅Ｌｕｐが設定されている収束閾値Ｄｔｈを超えている場合には、３Ｄモデルの３Ｄ点群が、３Ｄデータの点群に未だ十分に近づいていないと判断する。

第１の位置姿勢推定部１４は、３Ｄモデルの３Ｄ点群が、３Ｄデータの３Ｄ点群に未だ十分に近づいていないと判断すると（ステップＳ１３０；ＮＯ）、ステップＳ１００に戻り、ステップＳ１００→ステップＳ１１０→ステップＳ１３０といった一連の処理を繰り返し実行する。

一方、第１の位置姿勢推定部１４は、３Ｄモデルの３Ｄ点群が、３Ｄデータの３Ｄ点群に十分に近づいた状態で位置合わせができていると判断すると（ステップＳ１３０；ＹＥＳ）、更新後の３Ｄモデルの位置姿勢をあらわす更新３Ｄモデルデータを第２の位置姿勢推定部１５に出力し、以上説明した第１の位置合わせ処理を終了する。

なお、ステップＳ１００→ステップＳ１１０→ステップＳ１２０→ステップＳ１３０といった一連の処理を、所定回数繰り返し実行したとしても、位置姿勢の更新幅Ｌｕｐが、収束閾値Ｄｔｈを下回らないケースが生じ得る。このようなケースに対処するべく、ステップＳ１００→ステップＳ１１０→ステップＳ１２０→ステップＳ１３０といった一連の処理の繰り返し回数に、例えば上限回数Ｎ（Ｎ≧１）を設定しておき、一連の処理の繰り返し回数が上限回数Ｎに達した場合には、強制的に、第１の位置合わせ処理を終了してもよい。

Ａ−５．（第２の位置合わせ処理）
以下、第２の位置姿勢推定部１５によって実行される第２の位置合わせ処理を説明する。図５は、第２の位置合わせ処理を示すフローチャートである。

第２の位置姿勢推定部（二次元モデル生成部）１５は、例えば第１の位置姿勢推定部１４による３Ｄの位置合わせの位置姿勢と、３Ｄモデルデータと、カメラ１１ａの内部パラメータとを用いて、３Ｄモデルを２Ｄ画像平面に投影し、２Ｄ画像の輪郭の勾配特徴量を算出することで、２Ｄモデルを生成する（ステップＳ２００）。なお、本実施形態では、計測処理時にオンラインで２Ｄモデルを生成しているが、３Ｄテンプレートを作成するオフライン処理にて２Ｄモデルを生成することも可能である。このように、オフライン処理にて２Ｄモデルを事前生成している場合には、すでに生成された複数の２Ｄモデルのうち、現時点の姿勢に最も近い２Ｄモデルを選択し、選択した２Ｄモデルを使って位置合わせ（後述）を行うことになる。

第２の位置姿勢推定部１５は、２Ｄモデルを生成すると、２Ｄモデルデータと２Ｄ画像を用いて、対応点探索を行う（ステップＳ２１０）。具体的には、第２の位置姿勢推定部１５は、２Ｄモデルの生成と同様な方法で、２Ｄ画像の勾配特徴を算出する。そして、第２の位置姿勢推定部１５は、２Ｄモデルの勾配特徴と対応する２Ｄ画像上の画素（すなわち２Ｄ画像の勾配特徴）を、対応点として導出する。

第２の位置姿勢推定部（最適化部）１５は、２Ｄ画像上の対応点を基に、２Ｄ画像平面上で２Ｄ画像と２Ｄモデルの位置合わせを行うことで、２Ｄ画像上での並進ｘ、ｙ[単位:pixel]と回転θの３パラメータを最適化する（ステップＳ２２０）。第２の位置姿勢推定部１５は、２Ｄ画像で最適化した２Ｄ空間の３パラメータ（並進ｘ，ｙ，[単位:pixel]，回転θ）を、３Ｄ空間の３パラメータ（並進ｘ，ｙ，[単位:mm]，回転θ）に変換する（ステップＳ２３０）。具体的には、上述した２Ｄモデル生成時に、２Ｄモデルの各２Ｄ座標[pixel]に対応する３Ｄモデルの３Ｄ座標[mm]が算出できるため、第２の位置姿勢推定部１５は、“最適化した２Ｄ空間の３パラメータを用いて２Ｄモデルの位置姿勢を更新した２Ｄ座標”と“３Ｄモデルの３Ｄ座標”の対応関係とカメラ１１ａの内部パラメータを用いてＰｎＰ問題を解くことで、２Ｄ空間の位置姿勢を３Ｄ空間の位置姿勢へ変換する（ステップＳ２３０）。

第２の位置姿勢推定部（更新部）１５は、変換した３Ｄ空間の３パラメータ（並進ｘ，ｙ，[単位:mm]，回転θ）を用いて、第１の位置姿勢推定部１４によって更新された３Ｄモデルの位置姿勢（以下、「更新後の３Ｄモデルの位置姿勢」ともいう。）をさらに更新する（ステップＳ２４０）。すなわち、第２の位置姿勢推定部１５は、まず、第１の位置姿勢推定部１４から受け取った更新３Ｄモデルデータから、３Ｄモデルの位置姿勢を把握する。そして、第２の位置姿勢推定部１５は、変換した３Ｄ空間の３パラメータ（並進ｘ，ｙ，[単位:mm]，回転θ）を用いて、更新後の３Ｄモデルの位置姿勢をさらに更新する。その後、第２の位置姿勢推定部１５は、ステップＳ１３０に示す第１の位置姿勢推定部１５による収束判定と同様な方法で、収束判定を行う（ステップＳ２５０）。

第２の位置姿勢推定部１５は、収束判定において、位置姿勢の更新幅Ｌｕｐが収束閾値Ｄｔｈを超えていることから、３Ｄモデルの３Ｄ点群が、３Ｄデータの３Ｄ点群に未だ十分に近づいていないと判断すると（ステップＳ２５０；ＮＯ）、ステップＳ２００に戻り、ステップＳ２００→ステップＳ２１０→ステップＳ２２０→ステップＳ２３０→ステップＳ２４０→ステップＳ２５０といった一連の処理を繰り返し実行する。

一方、第２の位置姿勢推定部１５は、収束判定において、位置姿勢の更新幅Ｌｕｐが収束閾値Ｄｔｈを下回っていることから、３Ｄモデルの３Ｄ点群が、３Ｄデータの３Ｄ点群に十分に近づいていると判断すると（ステップＳ２５０；ＹＥＳ）、最終更新後の３Ｄモデルの位置姿勢をあらわす最終更新３Ｄモデルデータを、３次元位置合わせ結果出力部１４７に出力し（ステップＳ２６０）、以上説明した第２の位置合わせ処理を終了する。なお、ステップＳ２６０において、第２の位置姿勢推定部１５は、最終更新３Ｄモデルデータとともに、最終更新後の３Ｄモデルの位置姿勢を得るためにステップＳ２４０において導出した位置姿勢の６パラメータを、最適パラメータ情報として３次元位置合わせ結果出力部１４７に出力してもよい。

Ａ−６.（比較例）
図６は、ワークの位置姿勢を例示した図であり、αは、３次元概略位置姿勢認識装置１３０によって認識されるワークの位置姿勢１、βは、位置姿勢１をもとに、３Ｄデータを用いて３次元位置姿勢推定（３Ｄ）を行った場合のワークの位置姿勢２、γは、位置姿勢２をもとに、さらに２Ｄ画像を用いて３次元位置姿勢推定（３Ｄ＋２Ｄ）を行った場合のワークの位置姿勢３を示す。

３次元概略位置姿勢認識装置１３０によって認識されるワークの位置姿勢は、Ｘ方向及びＹ方向では合っているものの、Ｚ方向でズレが生じる（図６のαに示す位置姿勢１参照）。
位置姿勢１をもとに、３Ｄデータのみを用いて３次元位置姿勢推定を行うと、Ｚ方向では合っているものの、Ｘ方向及びＹ方向でズレが生じてしまう（図６のβに示す位置姿勢２参照）。
この点、３Ｄデータを用いて３次元位置姿勢推定を行った後、位置姿勢２をもとに、さらに２Ｄ画像を用いて３次元位置姿勢推定を行うと、Ｘ方向、Ｙ方向及びＺ方向のいずれにおいてもズレが生じることなく、高精度に位置姿勢を検出できることがわかる（図６のγに示す位置姿勢３参照）。

以上説明したように、本実施形態によれば、第１の位置姿勢推定部１４５は、３Ｄデータを用いて６パラメータ（すなわち、並進ｘ，ｙ，ｚ及び回転φ，γ，θ）を最適化する一方、第２の位置姿勢推定部１４６は、第１の位置姿勢推定部１４５における３Ｄデータを用いた３次元位置姿勢推定の結果を基に、２Ｄ画像で精度良く推定可能な３パラメータ（すなわち並進ｘ，ｙ及び回転θ）のみを最適化する。このように、２Ｄ画像で精度良く推定可能な並進ｘ，ｙと回転θの３パラメータのみを最適化することで、２Ｄ画像を用いた３次元位置姿勢推定の欠点である、カメラの奥行方向の位置（並進ｚ）と回転（回転φ及び回転γ）の３パラメータについては精度良く推定できないという問題を未然に防ぐことができ、３Ｄデータと２Ｄ画像の情報を一つの数式にまとめて６パラメータを最適化していた従来の３次元位置姿勢推定方法に比べて、精度良く３次元位置推定することが可能となる。
また、最適化に関しては、上記従来の３次元位置姿勢推定方法に比べて、処理を高速化することが可能となる。すなわち、従来手法においては、３Ｄデータと２Ｄ画像の情報を一つの数式にまとめて複雑な非線形最適化問題を解くことが必要であったのに対して、本実施形態においては、３Ｄデータを用いた６パラメータの最適化と２Ｄ画像を用いた３パラメータの最適化の２回の線形最適化問題を解けばよく、複雑な非線形最適化問題を解く必要がない分、処理を高速化することができる。

Ｂ．変形例
上述した本実施形態は、本発明を説明するための例示にすぎず、その要旨を逸脱しない限り、さまざまな変形が可能である。
例えば、３Ｄデータを用いた３次元位置姿勢推定（以下、「３Ｄ推定」）と２Ｄ画像を用いた３次元位置推定（以下、「２Ｄ推定」）の組み合わせについて、様々なバリエーションが考えられる。本実施形態では、３Ｄ推定の後に２Ｄ推定を行う態様を例示したが、２Ｄ推定の後に３Ｄ推定を行ってもよく、また、３Ｄ推定と２Ｄ推定を並列に行ってもよい。例えば、２Ｄ推定の後に３Ｄ推定を行う場合、第１の位置姿勢推定部１４５には、第２の位置姿勢推定部１４６で更新された位置姿勢情報が入力され、第１の位置姿勢推定部１４５で更新された位置姿勢情報は３次元位置合わせ結果出力部１４７に出力される構成とすればよい。一方、３Ｄ推定と２Ｄ推定を並列に行う場合、更新前位置姿勢取得部１４３で取得された位置姿勢情報が第１の位置姿勢推定部１４５及び第２の位置姿勢推定部１４６に供給され、第１の位置姿勢推定部１４５及び第２の位置姿勢推定部１４６でそれぞれ更新された位置姿勢情報は３次元位置合わせ結果出力部１４７に供給され、３次元位置合わせ結果出力部１４７は、第１の位置姿勢推定部１４５及び第２の位置姿勢推定部１４６で更新されたそれぞれの位置姿勢情報に基づいて、物体の位置姿勢を出力するように構成すればよい。

なお、本実施形態では、３Ｄ推定において６パラメータ（並進ｘ、ｙ、ｚ及び回転φ，γ，θ）を最適化したが、これに限る趣旨ではない。例えば、２Ｄ推定において最適化されるのは並進ｘ，ｙと回転θの３パラメータであることから、３Ｄ推定においては残りの３パラメータ（すなわち、並進ｚ及び回転φ，γ）を最適化するようにしてもよい。具体的には、第１の位置姿勢推定部１４は、目的関数（誤差関数）を最小化するように、３Ｄモデルの各点を、対応する３Ｄデータの各点に近づけるための３パラメータ（すなわち、並進ｚ及び回転φ，γ）を最適化すればよい。

Ｃ．その他
本明細書において、「部」とは、単に物理的構成を意味するものではなく、その「部」が実行する処理をソフトウェアによって実現する場合も含む。また、１つの「部」や装置が実行する処理を２つ以上の物理的構成や装置により実現されても、２つ以上の「部」や装置が実行する処理を１つの物理的手段や装置により実現されてもよい。

本明細書において上述した各処理におけるステップは、処理内容に矛盾を生じない範囲で任意に順番を変更し、または並列に実行することができる。

本明細書において説明した各処理を実施するプログラムは、記録媒体に記憶させてもよい。この記録媒体を用いれば、画像処理装置１０を構成する各装置に、上記プログラムをインストールすることができる。ここで、上記プログラムを記憶した記録媒体は、非一過性の記録媒体であっても良い。非一過性の記録媒体は特に限定されないが、例えば、ＣＤ−ＲＯＭ等の記録媒体であっても良い。

（付記１）
物体の位置姿勢を推定する推定装置であって、
前記物体の二次元画像を取得する第１取得部（１４１）と、
前記物体の三次元形状をあらわす三次元モデルを取得する第２取得部（１４２）と、
前記物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を取得する第３取得部（１４３）と、
前記三次元モデルと前記二次元画像とを用いて、前記二次元画像平面上の異なる２つの軸であるｘ軸及びｙ軸方向それぞれの並進と、当該ｘ軸及びｙ軸に直交するｚ軸を中心とする回転とについて、前記位置姿勢情報を更新する第２の位置姿勢推定部（１４６）と
前記更新された位置姿勢情報によってあらわされる位置姿勢を、前記物体の位置姿勢として出力する出力部（１４７）と、
を具備する位置姿勢推定装置。
（付記２）
前記第２の位置姿勢推定部（１４６）は、
前記三次元モデルを、前記位置姿勢情報に基づいて、前記二次元画像と共通の平面に投影した二次元モデルを生成する二次元モデル生成部（ステップＳ２００）と、
前記二次元画像と前記二次元モデルの位置合わせを行うことにより、前記ｘ軸及びｙ軸方向それぞれの並進パラメータと、前記ｚ軸を中心とする回転パラメータとを最適化する最適化部（ステップＳ２２０）と、
最適化された前記３つのパラメータを用いて、前記位置姿勢情報を更新する更新部（ステップＳ２４０）と、
を具備する、付記１に記載の位置姿勢推定装置。
（付記３）
前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部（１４１）と、
前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部（１４５）と、
をさらに備え、
前記第１の位置姿勢推定部には、前記第３取得部で取得した位置姿勢情報が入力され、前記第１の位置姿勢推定部で更新された位置姿勢情報は前記第２の位置姿勢推定部に渡される、
付記１又は２記載の位置姿勢推定装置。
（付記４）
前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部（１４１）と、
前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部（１４５）と、
をさらに備え、
前記第１の位置姿勢推定部には、前記第２の位置姿勢推定部で更新された位置姿勢情報が入力され、前記第１の位置姿勢推定部で更新された位置姿勢情報は前記出力部に渡される、
付記１又は２記載の位置姿勢推定装置。
（付記５）
前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部（１４１）と、
前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部（１４５）と、
をさらに備え、
前記第３取得部で取得された位置姿勢情報は、前記第１及び第２の位置姿勢推定部にそれぞれ渡され、前記第１及び第２の位置姿勢推定部でそれぞれ更新された位置姿勢情報は前記出力部に渡され、
前記出力部は、前記第１及び第２の位置姿勢推定部で更新されたそれぞれの位置姿勢情報に基づいて、前記物体の位置姿勢を出力する
付記１又は２記載の位置姿勢推定装置。
（付記６）
前記第１の位置姿勢推定部（１４５）は、
前記位置合わせを行う際、前記三次元座標データの各点に対応する前記三次元モデルの各点を、対応する前記三次元座標データの各点に近づけるように、前記ｘ軸、ｙ軸及びｚ軸方向それぞれの並進パラメータと、前記ｘ軸、ｙ軸及びｚ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された前記６つのパラメータを用いて、前記位置姿勢情報を更新する、付記３〜５のいずれか１つの付記に記載の位置姿勢推定装置。
（付記７）
前記第１の位置姿勢推定部（１４５）は、
前記位置合わせを行う際、前記三次元座標データの各点に対応する前記三次元モデルの各点を、対応する前記三次元座標データの各点に近づけるように、前記z軸方向の並進パラメータと、前記ｘ軸及びｙ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された前記３つのパラメータを用いて、前記位置姿勢情報を更新する、付記３〜５のいずれか１つの付記に記載の位置姿勢推定装置。

１…物体認識装置、２…ワーク、３…トレイ、４…ＰＬＣ、１０…画像処理装置、１１…３Ｄセンサ、１１ａ…カメラ、１２…ディスプレイ、１３…マウス、１４…メモリカード、１２０…テンプレート作成装置、１３０…３次元概略位置姿勢認識装置、１４０…３次元詳細位置姿勢推定装置、１４１…画像取得部、１４２…３Ｄモデル取得部、１４３…更新前位置姿勢取得部、１４４…カメラパラメータ取得部、１４５…第１の３次元位置姿勢推定部、１４６…第２の３次元位置姿勢推定部、１４７…３次元位置合わせ結果出力部。

Claims (8)

1. 物体の位置姿勢を推定する推定装置であって、
   前記物体の二次元画像を取得する第１取得部と、
   前記物体の三次元形状をあらわす三次元モデルを取得する第２取得部と、
   前記物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を取得する第３取得部と、
   前記三次元モデルと前記二次元画像とを用いて、前記二次元画像平面上の異なる２つの軸であるｘ軸及びｙ軸方向それぞれの並進と、当該ｘ軸及びｙ軸に直交するｚ軸を中心とする回転とについて、前記位置姿勢情報を更新する第２の位置姿勢推定部と
   前記更新された位置姿勢情報によってあらわされる位置姿勢を、前記物体の位置姿勢として出力する出力部とを具備し、
   前記第２の位置姿勢推定部は、
   前記三次元モデルを、前記位置姿勢情報に基づいて、前記二次元画像と共通の平面に投影した二次元モデルを生成する二次元モデル生成部と、
   前記二次元画像と前記二次元モデルの位置合わせを行うことにより、前記ｘ軸及びｙ軸方向それぞれの並進パラメータと、前記ｚ軸を中心とする回転パラメータとにより構成された二次元空間の３つパラメータを最適化する最適化部と、
   最適化された前記二次元空間の３つのパラメータを、三次元空間の３つのパラメータへ変換する変換部と、
   変換された前記三次元空間の３つのパラメータを用いて、前記位置姿勢情報を更新する更新部と、
   を具備する位置姿勢推定装置。
2. 前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、
   前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部と、
   をさらに備え、
   前記第１の位置姿勢推定部には、前記第３取得部で取得した位置姿勢情報が入力され、前記第１の位置姿勢推定部で更新された位置姿勢情報は前記第２の位置姿勢推定部に渡される、
   請求項１記載の位置姿勢推定装置。
3. 前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、
   前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部と、
   をさらに備え、
   前記第１の位置姿勢推定部には、前記第２の位置姿勢推定部で更新された位置姿勢情報が入力され、前記第１の位置姿勢推定部で更新された位置姿勢情報は前記出力部に渡される、
   請求項１記載の位置姿勢推定装置。
4. 前記物体について計測された前記物体表面の三次元座標をあらわす三次元座標データを取得する第４取得部と、
   前記三次元モデルと前記三次元座標データとを用いて位置合わせを行うことによって、位置姿勢情報を更新する第１の位置姿勢推定部と、
   をさらに備え、
   前記第３取得部で取得された位置姿勢情報は、前記第１及び第２の位置姿勢推定部にそれぞれ渡され、前記第１及び第２の位置姿勢推定部でそれぞれ更新された位置姿勢情報は前記出力部に渡され、
   前記出力部は、前記第１及び第２の位置姿勢推定部で更新されたそれぞれの位置姿勢情報に基づいて、前記物体の位置姿勢を出力する
   請求項１記載の位置姿勢推定装置。
5. 前記第１の位置姿勢推定部は、
   前記位置合わせを行う際、前記三次元座標データの各点に対応する前記三次元モデルの各点を、対応する前記三次元座標データの各点に近づけるように、前記ｘ軸、ｙ軸及びｚ軸方向それぞれの並進パラメータと、前記ｘ軸、ｙ軸及びｚ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された前記６つのパラメータを用いて、前記位置姿勢情報を更新する、請求項３または４に記載の位置姿勢推定装置。
6. 前記第１の位置姿勢推定部は、
   前記位置合わせを行う際、前記三次元座標データの各点に対応する前記三次元モデルの各点を、対応する前記三次元座標データの各点に近づけるように、前記z軸方向の並進パラメータと、前記ｘ軸及びｙ軸を中心とするそれぞれの回転パラメータとを最適化し、最適化された前記３つのパラメータを用いて、前記位置姿勢情報を更新する、請求項３または４に記載の位置姿勢推定装置。
7. 物体の位置姿勢を推定する方法であって、
   前記物体の二次元画像を取得する第１取得ステップと、
   前記物体の三次元形状をあらわす三次元モデルを取得する第２取得ステップと、
   前記物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を取得する第３取得ステップと、
   前記三次元モデルと前記二次元画像とを用いて、前記二次元画像平面上の異なる２つの軸であるｘ軸及びｙ軸方向それぞれの並進と、当該ｘ軸及びｙ軸に直交するｚ軸を中心とする回転とについて、前記位置姿勢情報を更新する第２の位置姿勢推定ステップと
   前記更新された位置姿勢情報によってあらわされる位置姿勢を、前記物体の位置姿勢として出力する出力ステップとを含み、
   前記第２の位置姿勢推定ステップは、
   前記三次元モデルを、前記位置姿勢情報に基づいて、前記二次元画像と共通の平面に投影した二次元モデルを生成する二次元モデル生成ステップと、
   前記二次元画像と前記二次元モデルの位置合わせを行うことにより、前記ｘ軸及びｙ軸方向それぞれの並進パラメータと、前記ｚ軸を中心とする回転パラメータとにより構成された二次元空間の３つパラメータを最適化する最適化ステップと、
   最適化された前記二次元空間の３つのパラメータを、三次元空間の３つのパラメータへ変換する変換ステップと、
   変換された前記三次元空間の３つのパラメータを用いて、前記位置姿勢情報を更新する更新ステップと、
   を含む位置姿勢推定方法。
8. コンピュータに、物体の位置姿勢を推定させるためのプログラムであって、
   前記コンピュータに、
   前記物体の二次元画像を取得する第１取得ステップと、
   前記物体の三次元形状をあらわす三次元モデルを取得する第２取得ステップと、
   前記物体について推定された更新前の位置姿勢をあらわす位置姿勢情報を取得する第３取得ステップと、
   前記三次元モデルと前記二次元画像とを用いて、前記二次元画像平面上の異なる２つの軸であるｘ軸及びｙ軸方向それぞれの並進と、当該ｘ軸及びｙ軸に直交するｚ軸を中心とする回転とについて、前記位置姿勢情報を更新する第２の位置姿勢推定ステップと
   前記更新された位置姿勢情報によってあらわされる位置姿勢を、前記物体の位置姿勢として出力する出力ステップとを実行させ、
   前記第２の位置姿勢推定ステップは、
   前記三次元モデルを、前記位置姿勢情報に基づいて、前記二次元画像と共通の平面に投影した二次元モデルを生成する二次元モデル生成ステップと、
   前記二次元画像と前記二次元モデルの位置合わせを行うことにより、前記ｘ軸及びｙ軸方向それぞれの並進パラメータと、前記ｚ軸を中心とする回転パラメータとにより構成された二次元空間の３つパラメータを最適化する最適化ステップと、
   最適化された前記二次元空間の３つのパラメータを、三次元空間の３つのパラメータへ変換する変換ステップと、
   変換された前記三次元空間の３つのパラメータを用いて、前記位置姿勢情報を更新する更新ステップと、
   を含むプログラム。

Images