JP6890422B2 - 情報処理装置、情報処理装置の制御方法およびプログラム - Google Patents

Description

本発明は、情報処理装置、情報処理装置の制御方法およびプログラムに関する。

近年のロボット技術の発展とともに、工業製品の組み立てのような複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットでは、ハンドなどのエンドエフェクタにより部品を把持して組み付けを行う。ロボットによる部品の組み付け作業では、ハンドによる部品把持時のずれや正常でない部品の混入等の原因により組み付けに失敗することがある。そのため、部品を組み付けた後に、正常に組み付けが行われたかどうかを確認する部品組み付け後検査が必要になる。近年、この組み付け後検査を自動化する取り組みが行われ始めている。

特許文献１では、２部品により構成される組み付け済み部品の撮影画像を用いて組み付け後検査を行う方法が開示されている。この方法では、ロボットにより把持・移動された組み付け元部品の位置姿勢と、組み付けの相手となる組付け先部品の位置姿勢とをそれぞれ算出し、両者の相対関係を理想的な組付け状態と比較することで、組み付けの良否を判定している。

特開２０１５−１１４７２２号公報

Ｒ． Ｙ． Ｔｓａｉ， "Ａ ｖｅｒｓａｔｉｌｅ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｈｉｇｈ−ａｃｃｕｒａｃｙ ３Ｄ ｍａｃｈｉｎｅ ｖｉｓｉｏｎ ｍｅｔｒｏｌｏｇｙ ｕｓｉｎｇ ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ ＴＶ ｃａｍｅｒａｓ ａｎｄ ｌｅｎｓｅｓ， " ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．ＲＡ−３，ｎｏ．４，１９８７． 立野，小竹，内山，"ビンピッキングのための距離・濃淡画像を最ゆうに統合する高精度高安定なモデルフィッティング手法"，電子情報通信学会論文誌D， 情報・システム J９４-D（８）， １４１０−１４２２， ２０１１．

しかしながら、組み付け済み部品の撮影画像では、組み付けられた部位の周辺で、一方の部品が他方の部品を隠蔽する領域が発生する。この隠蔽領域の影響により、組み付け先部品あるいは組み付け元部品の位置姿勢を安定して算出できない場合がある。特許文献１のように、部品の３次元形状モデルを用いて、画像上の幾何特徴との対応間距離を最小化する方法では、隠蔽領域が多いとロバスト性および精度が低下してしまい、部品の正しい位置姿勢が算出できないという課題がある。

本発明は、上記の課題を鑑みてなされたものであり、組み付けによって他の部品による隠蔽が発生する部品に対して精度よくロバストに位置姿勢を取得可能にする技術を提供することを目的とする。

上記の目的を達成する本発明に係る情報処理装置は、
組み付け先の物体と組み付け元の物体とが組み付けられた場合における両物体間の理想の相対位置姿勢を取得する取得手段と、
前記組み付け先の物体の形状を表す第一の形状モデルおよび前記組み付け元の物体の形状を表す第二の形状モデルと前記理想の相対位置姿勢とに基づいて、前記組み付け先の物体と前記組み付け元の物体とが組み付けられた状態の画像から、前記組み付け元の物体が前記組み付け先の物体を隠蔽する隠蔽領域を特定する特定手段と、
前記隠蔽領域に対して、前記組み付け元の物体が前記組み付け先の物体を隠蔽する度合いに基づいて、前記組み付け先の物体の位置姿勢を取得する際の寄与度を設定する設定手段と、
前記第一の形状モデルまたは前記画像に基づいて、前記組み付け先の物体の位置姿勢を取得するための情報を生成する生成手段と、
前記生成手段により生成された情報と前記寄与度とに基づいて、前記組み付け先の物体の位置姿勢を取得する位置姿勢取得手段と、
を備えることを特徴とする。

本発明によれば、組み付けによって他の部品による隠蔽が発生する部品に対しても精度よくロバストに位置姿勢を取得することが可能となる。

第１の実施形態に係る組み付け元部品および組み付け先部品の一例を示す図。 第１の実施形態に係る情報処理装置の構成を示す図。 第１の実施形態に係る情報処理装置が実施する、組み付け先部品の位置姿勢算出処理の概略手順を示すフローチャート。 第１の実施形態に係る情報処理装置が実施する、隠蔽領域の特定処理の手順を示すフローチャート。 第１の実施形態に係る情報処理装置が実施する、位置姿勢算出処理の詳細手順を示すフローチャート。 第１の実施形態に係る組み付け済み部品の撮影画像の一例を示す図。 第１の実施形態に係る仮想カメラに対するモデルの配置を説明する図。 第１の実施形態に係る仮想画像の作成を説明する図。 第１の実施形態に係る領域特定画像の作成を説明する図。 第１の実施形態に係るモデル幾何特徴の抽出を説明する図。 第１の実施形態に係るモデル幾何特徴の抽出を説明する図。 第２の実施形態に係るロボットシステムの構成例を示す図。 第１の実施形態の変形例１−２に係る情報処理装置の構成を示す図。

以下、添付の図面を参照しながら、本発明の実施形態について詳述する。

（第１の実施形態：モデルから隠蔽領域を除外して位置姿勢を算出）
＜概要＞
本実施形態では、情報処理装置を組み付け先部品の位置姿勢算出に適用した場合について説明する。

図１（ｃ）に一例を示すような２部品（２物体）から構成される組み付け済み部品３０を取り扱うものとする。組み付け済み部品３０は、図１（ａ）に示すような、ロボットハンド（不図示）により把持・移動される組み付け元部品１０と、当該組み付け元部品１０を組み付ける相手となる図１（ｂ）に示すような組み付け先部品２０とを含んで構成される。本実施形態では、組み付け済み部品３０の撮影画像に基づき、組み付け先部品２０の位置姿勢を算出することを想定する。なお、本実施形態で述べる「位置姿勢」とは、組み付け先部品２０と当該部品を撮影する撮影装置（不図示）との位置姿勢の関係のことを意味する。

以下、本実施形態における位置姿勢算出処理の概要を簡単に述べる。まず、組み付け元部品１０および組み付け先部品２０の３次元形状モデルを用意する。さらに、理想的な組付け状態における、両者の相対位置姿勢を設定しておく。そして、組み付け済み部品３０の画像を撮影して組み付け先部品２０の位置姿勢を算出する。

まず、撮影画像における組み付け先部品２０の概略位置姿勢を設定する。ここで、組み付け先部品２０の概略位置姿勢と、理想的な組み付け状態における組み付け元部品１０との相対位置姿勢から、撮影画像における組み付け元部品１０の概略位置姿勢も求めることが可能である。そこで、それぞれの概略位置姿勢に基づいて組み付け先部品２０および組み付け元部品１０の３次元形状モデルを画像上に投影する。

そして、組み付け先部品２０の三次元形状モデルと画像上の幾何特徴（画像特徴）との対応関係を用いて位置姿勢を算出する。このとき、組み付け先部品２０の三次元形状モデルに対して、組み付け元部品１０による隠蔽が生じている部位は利用しない。これにより、隠蔽領域によるモデルと計測データとの不一致の影響を受けず、ロバストかつ高精度に位置姿勢を算出することが可能である。

以下の説明では、組み付け先部品と組み付け元部品との相対関係に基づき、計測対象の組み付け先部品２０が画像上で隠蔽される領域を特定し、隠蔽されない部位の三次元形状モデルを用いて位置姿勢算出を行う方法について詳細を述べる。

＜情報処理装置の構成＞
まずは図２を参照して、本実施形態に係る情報処理装置１の構成例を説明する。情報処理装置１は、モデル取得部１１０と、相対位置姿勢取得部１２０と、画像取得部１３０と、概略位置姿勢取得部１４０と、隠蔽領域特定部１５０と、位置姿勢取得部１６０とを備えている。

モデル取得部１１０は、組み付け元部品１０の形状を表す３次元形状モデル１００と、組み付け先部品２０の形状を表す３次元形状モデル２００とを取得する。３次元形状モデルは、従来同様のものを用いればよいため、ここでは詳細な説明については省略するが、例えば、各点を結んで構成される面の情報、面を構成する線分の情報によって定義される。３次元形状モデル１０および２０は、モデル取得部１１０に保存されており、隠蔽領域特定部１５０に入力される。

相対位置姿勢取得部１２０は、組み付け元部品１０および組み付け先部品２０が理想的に組み付けられた時の２部品間の相対位置姿勢を隠蔽領域特定部１５０に入力する。以後、この相対位置姿勢を組み付け位置姿勢と呼ぶことにする。組み付け位置姿勢は、たとえば、２部品の３次元形状モデルを仮想空間上で操作して理想的な組み付け状態に配置し、その状態における２部品間の座標系の相対関係を求めることで事前に設定しておく。あるいは、あらかじめ２部品の寸法情報に基づき、数値計算で求めておいてもよい。そのほか、理想組み付け時の２部品間の配置関係が特定できればいかなる方法でもかまわない。

２次元画像撮影装置４０は、２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では、２次元画像撮影装置４０は濃淡画像を出力する。２次元画像撮影装置４０が撮影する画像は画像取得部１３０を介して情報処理装置１に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、または、非特許文献１で開示される方法によって事前にキャリブレーションしておく。

距離画像撮影装置５０は、計測対象である物体表面上の点の３次元情報を計測する。距離画像撮影装置５０として距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとして、波長の異なる色ＩＤを付与したマルチスリットラインを対象に照射し、その反射光をカメラで撮影して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。

しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。

距離画像撮影装置５０が計測した距離画像は、画像取得部１３０を介して情報処理装置１に入力される。また、距離画像撮影装置５０と２次元画像撮影装置４０との光軸は一致しており、２次元画像撮影装置４０が出力する濃淡画像の各画素と、距離画像撮影装置が出力する距離画像の各画素との対応関係は既知であるものとする。しかしながら、本発明の適用は、濃淡画像と距離画像とが同一の視点である場合に限るものではない。

例えば、濃淡画像を撮影する撮影装置と、距離画像を撮影する撮影装置とが別の位置姿勢にあり、濃淡画像と距離画像とをそれぞれ別の視点から撮影しても良い。この場合は、撮影装置間の相対的な位置姿勢は既知であるとして、距離画像中の３次元点群を濃淡画像に投影することにより、濃淡画像と距離画像の対応を取る。

同一の物体を撮影する撮影装置間の相対的な位置姿勢が既知であり、その画像間の対応が計算できる限り、撮影装置の位置関係に特に制限はない。なお、２次元画像撮影装置４０および距離画像撮影装置５０には共通の座標系が設定されているものとする。この座標系を以後、撮影装置座標系と呼ぶことにする。

概略位置姿勢取得部１４０は、情報処理装置１に対する組み付け先部品２０の位置姿勢の概略値を取得する。本実施形態において、情報処理装置１に対する組み付け先部品２０の位置姿勢とは、撮影装置座標系を基準とした組み付け先部品２０の位置及び姿勢を表す。しかし、必ずしも撮影装置座標系を基準にする必要はない。

例えば、撮影装置座標系に対して相対的な位置姿勢が既知であり、かつその位置姿勢が変化しないのであれば、その他の物体を基準としても良い。また、取得する位置姿勢は、組み付け元部品の位置姿勢であってもよい。その場合には、相対位置姿勢取得部１２０により入力される組み付け位置姿勢を用いて、組み付け先部品２０の位置姿勢を算出すればよい。本実施形態においては、組み付け先部品２０の置かれているおおよその位置や姿勢が予め分かっているものとして、事前に教示した値を利用する。

しかし、組み付け先部品２０の位置姿勢の概略値は、必ずしもこのような値である必要はない。例えば、組み付け先部品２０を含む情景を撮影した濃淡画像や距離画像に対して物体認識処理を実施することにより、物体の位置及び姿勢の概略値を計測するようにしても良いし、そのほかの方法で得た概略値を利用しても構わない。

隠蔽領域特定部１５０は、組み付け完了後の組み付け先部品２０を概略位置姿勢から観測した際に、組み付け元部品１０の隠蔽により観測できない領域を特定する。より具体的には、仮想カメラを用意して、仮想カメラを基準として、それぞれ入力された概略位置姿勢および組み付け位置姿勢に基づき、組み付け先部品２０と組み付け元部品１０とを配置して、画像のレンダリングを行う。

このとき、組み付け元部品１０が投影された領域の画素が特定できるよう、固有の色を割りあてて描画を行う。描画により得られた画像は位置姿勢取得部１６０に入力される。入力された画像は、組み付け先部品２０の位置姿勢算出時の精度低下を引き起こす隠蔽領域の影響除外のために利用される。処理の詳細は後述する。

位置姿勢取得部１６０は、撮影装置座標系に対する組み付け先部品２０の位置姿勢を算出する。処理の詳細については後述する。

以上が、情報処理装置１の構成の一例についての説明である。なお、情報処理装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御部、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶部が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力部、ネットワークカード等の通信部等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御部が記憶部に記憶されたプログラムを読み出して実行することで制御される。

＜処理＞
次に、本実施形態における、組み付け先部品２０の位置姿勢算出の処理手順について説明する。図３は、本実施形態に係る情報処理装置１が実施する位置姿勢算出の処理手順を示すフローチャートである。

（ステップＳ１０００）
画像取得部１２０は、２部品が組み付けられた状態の組み付け済み部品３０の濃淡画像および距離画像を取得する。まず、２次元画像撮影装置４０から濃淡画像を取得する。同様に、距離画像撮影装置５０から距離画像を取得する。本実施形態では、距離画像には距離画像撮影装置５０から計測対象物体の表面までの距離が格納されているものとする。

前述のように、２次元画像撮影装置４０と距離画像撮影装置５０との光軸は一致しているため、濃淡画像の各画素と距離画像の各画素との対応は既知である。図６に、組み付け完了後の、組み付け元部品１０および組み付け先部品２０の撮影画像６０の例を示す。なお、説明簡略化のため図６では濃淡画像のみ示している。

（ステップＳ１１００）
概略位置姿勢取得部は、２次元画像撮影装置４０と距離画像撮影装置５０とを含んで構成される撮影装置に対する組み付け先部品２０の位置および姿勢の概略値（後述の概略位置姿勢７１０）を取得する。前述のように、本実施形態では組み付け先部品２０が置かれているおおよその位置や姿勢があらかじめわかっているものとして、その値を概略値として用いる。

（ステップＳ１２００）
隠蔽領域特定部１５０は、ステップＳ１１００で取得された位置および姿勢の概略値と、３次元形状モデル１００および２００と、組み付け位置姿勢とを用いて、濃淡画像および距離画像上で、組み付け先部品２０が隠蔽される領域を特定する。

以下、図４のフローチャートを参照して、ステップＳ１２００における隠蔽領域の特定処理について説明する。

（ステップＳ１２１０）
図７に示すように、仮想カメラ７００を用意し、仮想カメラ７００を基準として、ステップＳ１１００で取得された概略位置姿勢７１０に組み付け先部品２０の３次元形状モデル２００を配置する。ここで仮想カメラ７００の内部パラメータは、２次元画像撮影装置４０および距離画像撮影装置５０とできるだけ同一の値にするのが望ましい。しかし、必ずしも同一の値を用いる必要はない。２次元画像撮影装置４０および距離画像撮影装置５０と、後述するステップＳ１２３０で作成する領域特定画像とで、画素の対応がとれるのであれば、異なる内部パラメータを用いてもかまわない。

（ステップＳ１２２０）
ステップＳ１２１０で配置した組み付け先部品２０の３次元形状モデル２００を基準として、相対位置姿勢取得部１２０で取得された組み付け位置姿勢７２０に、組み付け元部品１０の３次元形状モデル１００を配置する。

（ステップＳ１２３０）
ステップＳ１２１０およびＳ１２２０で配置した２つのモデルを仮想カメラ７００で観測した仮想画像８０を図８に示す。本ステップでは、組み付け先部品２０の３次元形状モデル２００が、組み付け元部品１０の３次元形状モデル１００により隠蔽されうる領域を特定可能な画像８０を作成する。具体的には、配置した２部品の３次元形状モデルを仮想画像平面７３０に描画して仮想画像８０を作成する。このとき、隠面処理を行って視点からは陰になり見えない部分を消去して描画する。

さらに、組み付け元部品１０および組み付け先部品２０の３次元形状モデルにはそれぞれ固有の色（輝度値）を割り当てて描画を行う。図９に、得られる仮想画像９０を示す。仮想画像９０の各画素の輝度値を参照することで、組み付け元部品１０の投影領域９００あるいは組み付け先部品２０の投影領域９１０、あるいは背景領域９２０を特定可能である。本ステップで作成した画像を以後、領域特定画像と呼ぶことにする。

なお、領域特定画像の作成方法はこれに限らない。たとえば、レンダリングを行わずに、仮想カメラ７００から仮想画像平面７３０の各画素に対する視線ベクトルを計算し、視線ベクトルと３次元形状モデルとの交点位置により、各画素に投影される対象部品の領域を特定してもよい。具体的には、視線ベクトルと２部品の３次元形状モデルとの交差の有無をチェックし、交点が存在した場合には、その画素に、各３次元形状モデルに固有の輝度値を割り当てて画像を作成する。

２部品とも交差があった画素については、交点が手前の３次元形状モデルに固有の輝度値を割り当てる。このほか、各画素に対して組み付け元部品１０および組み付け先部品２０の各モデルが投影された領域を特定できればいかなる方法であってもかまわない。

（ステップＳ１３００）
位置姿勢取得部１６０は、組み付け先部品２０の３次元形状モデル２００と、濃淡画像および距離画像との対応関係を用いて、組み付け先部品２０の位置姿勢の算出を行う。その際、領域特定画像を参照することで、隠蔽領域の影響を低減する。以下、図５のフローチャートを用いて詳述する。

（ステップＳ１３１０）
位置姿勢取得部１６０は、領域特定画像を利用して、組み付け先部品２０の３次元形状モデル２００から位置姿勢を取得するための情報として幾何特徴を抽出、生成する。本実施形態においては、３次元形状モデルは、複数のＮＵＲＢＳ（Ｎｏｎ−ＵｎｉｆｏｒｍＲａｔｉｏｎａｌ Ｂ−Ｓｐｌｉｎｅ）曲面で構成されているものとする。当該モデルの幾何特徴には、３次元の位置および法線とから構成される物体表面上の局所的な３次元平面情報（以後、「局所面特徴」と称する）と３次元の位置および方向とから構成される物体輪郭上の局所的な３次元線分情報（以後、「局所線特徴」と称する）を抽出するものとする。

なお、単に幾何特徴と称した場合は、局所面特徴と局所線特徴との両方を指すものとする。なお、抽出する幾何特徴は対象形状を表す３次元的な幾何情報であれば良く、特に制限はない。例えば、単純な３次元点の集合でもよいし、稜線を表す３次元ラインの集合でもよい。その他、位置姿勢に利用可能なものであれば、いかなるものであってもよい。

幾何特徴の抽出は、領域特定画像上で組み付け先部品２０が投影された画素をランダムにサンプリングし、その画素に対応する３次元形状モデル２００上の幾何特徴を逆投影して算出することで行う。画素のサンプリングは全画素に対して行ってもよいし、均一に行ってもよい。このとき、領域特定画像の各画素の値を参照し、組み付け先部品以外が投影された領域からは幾何特徴を抽出しない。これにより、図１０に示すように、組み付け元部品１０による隠蔽部位を除外して、隠蔽が生じていない部位から、位置姿勢を取得するための情報としての幾何特徴（局所面特徴１００１および局所線特徴１００２）を抽出可能となる。ただし、３次元形状モデルの表面上から当該モデルの幾何特徴を抽出できれば、その方法は特に問わない。

なお、全方向から３次元形状モデルを描画して領域特定画像をそれぞれ作成し、各方向から隠蔽なく観測できるモデル幾何特徴を、各方向に対応付けて登録しておいてもよい。これにより、組み付け先部品２０の位置及び姿勢の概略値とカメラパラメータとから算出される視線ベクトルに最も近い方向に登録された幾何特徴を選択することで幾何特徴の抽出が可能である。なお、各方向のベクトルと幾何特徴の法線との内積を比較し、方向ベクトルと幾何特徴の法線とが対向している幾何特徴のみを登録するようにしてもよい。

（ステップＳ１３２０）
位置姿勢取得部１６０は、ステップＳ１２１０において取得された、組み付け先部品２０の概略位置姿勢に基づき、ステップＳ１０００で計測された距離画像中の３次元点群と、３次元形状モデル２００との対応付けを行う。部品の概略位置姿勢と距離画像撮影装置５０の内部パラメータとを用いて、ステップＳ１３１０で抽出した各局所面特徴を距離画像上に投影する。そして、投影した各局所面特徴に対応する距離画像上の距離点を、各面に対応する３次元点として保持する。

次に、位置姿勢推定部１６０は、濃淡画像上のエッジと３次元形状モデルとの対応付けを行う。ステップＳ１３２０と同様に、組み付け先部品２０の概略位置姿勢と２次元画像撮影装置４０の内部パラメータとを用いて３次元形状モデルを構成する局所線特徴を画像へ投影し、画像上で検出されたエッジと３次元形状モデル中の局所線特徴とを対応付ける。エッジが各局所線特徴に対応して複数検出された場合には、複数検出されたエッジのうち、投影された局所線特徴に対して画像上で最も近いエッジを対応付ける。

（ステップＳ１３３０）
位置姿勢取得部１６０は、ステップＳ１３２０で得られた、３次元形状モデル中の各面に対応する距離画像中の３次元点と、３次元形状モデル中の各線分に対応する濃淡画像上のエッジとの対応データに基づいて、組み付け先部品２０および組み付け元部品１０の位置姿勢を算出する。本ステップでは、算出した対応データに基づいて、計測データと３次元形状モデルとの間の誤差が最小になるように、線形連立方程式を解くことで、位置姿勢の更新を行う。

ここで、画像上の距離と３次元空間中での距離とでは尺度が異なるため、単純に連立方程式を解くだけでは計測データのどちらか一方に寄与率が偏ってしまう可能性がある。そこで本実施形態では、非特許文献２に示すような最尤推定に基づく最適化を行うことで、尺度を合わせた位置姿勢推定を行う。

最尤推定に基づく位置姿勢推定方法に関しては、本発明の本質に関わる話ではないため、詳細な説明は省略する。なお、位置及び姿勢の算出方法は、上述の最尤推定に基づく手法に限るものでなく、例えば、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法による繰り返し演算を行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。

なお、ここでは、位置合わせに濃淡画像および距離画像を利用する方法について述べたが、濃淡画像のみ、あるいは距離画像のみを用いて位置姿勢の算出を行う場合においてももちろん、同様の方法が利用できる。

（ステップＳ１４００）
位置姿勢取得部１６０は、ステップＳ１３３０で更新した位置姿勢が、収束しているか否か、すなわち、さらに反復計算を必要とするか否かの判定を行う。具体的には、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後との差がほぼ０である場合に収束したと判定する。収束していなければ（Ｓ１４００；ＮＯ）、ステップＳ１１００に戻り、更新した位置姿勢を用いて、再度位置姿勢推定処理を行う。収束していると判断した場合は（Ｓ１４００；ＹＥＳ）、処理を終了し、撮影装置に対する、組み付け先部品２０の位置姿勢の最終的な推定値が決定される。

以上、第１の実施形態では、組み付け先部品２０と組み付け元部品１０との相対関係に基づき、計測対象の組み付け先部品２０が画像上で隠蔽される領域を特定し、隠蔽されない部位の３次元形状モデルを用いて位置姿勢算出を行う方法について述べた。

本実施形態によれば、組み付け先部品の３次元形状モデルに対して、組み付け元部品による隠蔽が生じている部位を利用しないことで、隠蔽領域によるモデルと計測データとの不一致の影響を受けず、ロバストかつ高精度に位置姿勢を算出することが可能となる。

（変形例１−１）
本実施形態では、組み付け先部品２０の計測を例に説明した。しかし、組み付け元品１０の位置姿勢算出においても同様の方法をもちろん適用可能である。組み付け元部品１０が組み付け先部品２０によって隠蔽される状況である場合には、本実施形態と同様の方法を用いて、組み付け先部品２０による隠蔽部位の影響を除外して位置姿勢算出を行うことで、ロバストかつ高精度な推定が可能である。

（変形例１−２）
ここまで述べた方法を利用してさらに、組み付けが正しく行われたか否かの判定（組み付け成否判定）を行ってもよい。組み付け成否の判定は、図１３に示すように、実施形態１の装置構成に、組み付け成否判定部１７０を追加することで実施可能である。まず、前述した方法により位置姿勢算出部１６０は、組み付けられた状態の組み付け元部品１０、および、組み付け先部品２０の位置姿勢をそれぞれ算出する。その後、算出したそれぞれの位置姿勢に基づき、両者の相対位置姿勢を算出する。これに対して、組み付け成否判定部１７０は、理想的な組み付け状態における、組み付け元部品１０と組み付け先部品２０との相対位置姿勢をあらかじめ保持しておき、算出された相対位置姿勢との比較を行う。この結果、位置姿勢に関する両者の差分が所定の値以上であった場合には組み付け失敗、所定の値未満であった場合には組み付け成功と判定する。なお、成否判定には、位置の差分あるいは姿勢の差分のいずれかのみ用いてもよい。これにより、組み付けにより部品間で相互に隠蔽が発生した状況においても組み付け成否を安定的に行うことが可能である。

（変形例１−３）
上述の実施形態では、２つの部品から構成される組み付け済み部品３０を対象とした事例について述べた。しかし、３つ以上の部品で構成される組み付け済み部品に対して、そのうちの１部品の位置姿勢を算出する場合にも同様の方法を適用可能である。この場合にも、構成部品の既知の組み付け情報に基づき、画像撮影上で計測対象の部品が隠蔽される領域を特定し、隠蔽部位からはモデル幾何特徴を抽出せずに位置姿勢算出を行う。これにより、隠蔽部位の影響を除外して、ロバストかつ高精度な推定が可能である。

（第２の実施形態：モデル幾何特徴に付与した隠蔽情報に基づき位置姿勢を算出）
第１の実施形態では、隠蔽なく観測可能な部位からモデル幾何特徴を抽出して位置姿勢を算出する方法について述べた。

これに対して、隠蔽の発生有無にかかわらずモデル幾何特徴は抽出しておき、その際に隠蔽が発生するか否かの属性を付与しておいてもよい。そして各モデル幾何特徴の属性に応じて位置姿勢算出時の寄与度を設定することで隠蔽による影響を軽減することが可能である。具体的には、第１の実施形態のステップＳ１３１０において、隠蔽が発生する部位についても組み付け先部品２０の幾何特徴を抽出しておく。

この際、領域特定画像の対応画素に基づき、隠蔽が発生する部位か否かを特定して、各幾何特徴とともに記録する。そして、ステップＳ１３２０において対応探索を行う際に、隠蔽領域の部位については対応探索を行わないようにする。もしくは、ステップＳ１３３０において位置姿勢を算出する際に、隠蔽が発生する幾何特徴の寄与度を低く、隠蔽が発生しない幾何特徴の寄与度を高く設定して位置姿勢を算出する。

あるいは、領域特定画像の画素値に、隠蔽度合いを示す値を記録しておき、この値に基づき各幾何特徴の寄与度を設定して位置姿勢を算出してもよい。具体的にはまず、領域特定画像の各画素に対して、計測対象の部品を遮蔽する別の部品が存在する場合には、遮蔽する面と計測対象の部品の面との奥行き（視線）方向の距離を隠蔽度として記録しておく。そして、ステップＳ１３１０においてモデル幾何特徴を抽出する際に、領域特定画像の対応画素を参照して、隠蔽度を付与しておく。そして、位置姿勢算出時に、モデル幾何特徴に付与された隠蔽度が大きいものは寄与度を低く、隠蔽度が小さいものは寄与度を高く設定して位置姿勢を算出する。なお、隠蔽度には、奥行き方向の距離以外の値を用いてもよい。たとえば、隠蔽が生じる部位の画素に対して、画像面上で隠蔽が発生しない部位までの最短距離を求め、この値を隠蔽度として利用してもよい。この場合も、隠蔽度が大きいものは寄与度を低く、小さいものは寄与度を高く設定して位置姿勢を算出する。このほか、隠蔽による位置姿勢算出への影響を考慮して、隠蔽度の算出および位置姿勢算出時の寄与度の設定は任意の方法で行えばよい。

以上、第２の実施形態によれば、モデル特徴に隠蔽部位か否かの属性を付与し、属性に基づいて位置姿勢算出への寄与度を変えることで、ロバストかつ高精度に位置姿勢の算出が可能となる。

（変形例２−１：画像上で隠蔽領域を無効化）
第２の実施形態では、各部位が隠蔽されるか否かの判定結果に基づいてモデル幾何特徴を抽出することで、隠蔽による影響を受けずに位置姿勢を算出する方法について述べた。これに対して、モデル幾何特徴の代わりに、撮影画像から隠蔽が発生する部位を除外して、位置姿勢算出を行ってもよい。この場合、ステップＳ１３１０では、隠蔽の発生有無にかかわらず、モデル幾何特徴を抽出しておく。また、その際に、モデル幾何特徴に対する属性の付与も行わない。代わりに、ステップＳ１３２０においてモデル幾何特徴の対応探索実施後、対応づいた画素の領域特定画像を参照し、その画素が隠蔽領域の場合には、その対応を無効化して位置姿勢算出には利用しない。

あるいは、ステップＳ１０００で入力された濃淡画像および距離画像の各画素に対し、領域特定画像値を参照することで、組み付け先２０部品の投影領域に該当する画素には無効値を設定しておく。図１１は、第１の実施形態における図６の撮影画像６０に対して、組み付け元部品１０による隠蔽領域１１００に無効値を設定した画像の例である。そして、ステップＳ１３２０において、無効値が設定されていない画素のみを対象に対応探索を行う。

なお、本変形例においても、第２の実施形態と同様に、撮影画像に対して隠蔽度を設定して位置姿勢算出時の寄与度を設定してもよい。また、第１の実施形態と本変形例とを組み合わせ、領域特定画像に基づき、モデル幾何特徴と撮影画像との両方から隠蔽領域を除外して位置姿勢を算出してもよい。

以上、変形例２−１によれば、撮影画像に対して隠蔽部位か否かを判定し、隠蔽部位は位置姿勢算出に利用しないことで、その影響を受けず、ロバストかつ高精度に位置姿勢の算出を行うことが可能となる。

（第３の実施形態：ハンドで把持した部品を計測する際に、ハンドによる隠蔽を除外）
第１の実施形態および第２の実施形態では、複数部品の組み付けにより構成される組み付け済み部品のうちの１部品の位置姿勢算出を安定的に行う方法について述べた。

本実施形態では、ロボットハンドにより把持を行った部品（把持部品）の画像を、ハンドに取り付けた撮影装置で撮影して位置姿勢を算出することを想定する。なお、撮影装置はハンドに取り付けず、ロボットの動作範囲内に固定で設置したものを用いてもよい。このとき、ハンドによる隠蔽の影響を受けずに安定的に位置姿勢を算出する方法について述べる。

図１２に一例を示すような、ロボットアーム１２０００の先端にロボットハンド１２０１０を取り付け、ロボットハンド１２０１０により把持した把持部品１２０２０を、ロボットハンド１２０１０に取り付けた撮影装置１２０３０で撮影して、位置姿勢を算出することを想定する。なお、本実施形態で述べる「位置姿勢」とは、把持部品と当該部品を撮影する撮影装置との位置姿勢の関係のことを意味する。

本実施形態は、第１の実施形態における、組み付け元部品１０をロボットハンド１２０１０、組み付け先部品２０を把持部品１２０２０とそれぞれ置き換えることで、基本的に第１の実施形態と同様の装置構成により実施可能である。ただし、本実施形態では、相対位置姿勢取得部１２０は、組み付け情報の代わりに、把持情報として、把持部品１２０２０が理想的に把持された時の、ロボットハンド１２０１０と把持部品１２０２０との相対位置姿勢を隠蔽領域特定部１５０に入力する。

本実施形態によれば、把持を行ったロボットハンドによる隠蔽の影響を受けずに、把持部品の位置姿勢を高精度に算出可能である。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１：情報処理装置、１１０：モデル取得部、１２０：相対位置姿勢取得部、１３０：画像取得部、１４０：概略位置姿勢取得部、１５０：隠蔽領域特定部、１６０：位置姿勢取得部

Claims (13)

1. 組み付け先の物体と組み付け元の物体とが組み付けられた場合における両物体間の理想の相対位置姿勢を取得する取得手段と、
   前記組み付け先の物体の形状を表す第一の形状モデルおよび前記組み付け元の物体の形状を表す第二の形状モデルと前記理想の相対位置姿勢とに基づいて、前記組み付け先の物体と前記組み付け元の物体とが組み付けられた状態の画像から、前記組み付け元の物体が前記組み付け先の物体を隠蔽する隠蔽領域を特定する特定手段と、
   前記隠蔽領域に対して、前記組み付け元の物体が前記組み付け先の物体を隠蔽する度合いに基づいて、前記組み付け先の物体の位置姿勢を取得する際の寄与度を設定する設定手段と、
   前記第一の形状モデルまたは前記画像に基づいて、前記組み付け先の物体の位置姿勢を取得するための情報を生成する生成手段と、
   前記生成手段により生成された情報と前記寄与度とに基づいて、前記組み付け先の物体の位置姿勢を取得する位置姿勢取得手段と、
   を備えることを特徴とする情報処理装置。
2. 前記第一の形状モデルおよび前記第二の形状モデルをそれぞれ取得するモデル取得手段をさらに備えることを特徴とする請求項１に記載の情報処理装置。
3. 前記生成手段は、前記第一の形状モデルのモデル特徴を前記情報として生成し、
   前記位置姿勢取得手段は、前記情報として生成された前記モデル特徴と前記画像中の画像特徴とを前記寄与度に基づいて対応付けて前記組み付け先の物体の位置姿勢を取得する請求項１または２に記載の情報処理装置。
4. 前記生成手段は、前記画像中の画像特徴を前記情報として生成し、
   前記位置姿勢取得手段は、前記情報として生成された前記画像特徴と前記第一の形状モデルのモデル特徴とを前記寄与度に基づいて対応付けて前記組み付け先の物体の位置姿勢を取得する請求項１または２に記載の情報処理装置。
5. 前記画像中の画像特徴は、前記画像の幾何特徴であることを特徴とする請求項３または４に記載の情報処理装置。
6. 前記特定手段は、前記組み付け先の物体の概略位置姿勢と、前記理想の相対位置姿勢とに基づいて、前記隠蔽領域を特定することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
7. 前記特定手段は、前記組み付け先の物体の概略位置姿勢に基づいて前記組み付け先の物体の第一の形状モデルをレンダリングした結果と、前記理想の相対位置姿勢に基づいて求められる前記組み付け元の物体の概略位置姿勢に基づいて前記組み付け元の物体をレンダリングした結果とに基づいて、前記隠蔽領域を特定することを特徴とする請求項６に記載の情報処理装置。
8. 前記特定手段は、前記組み付け元の物体の概略位置姿勢と前記組み付け元の物体の第二の形状モデルとに基づいて前記画像中で前記組み付け元の物体が占める領域を示す画像を生成することにより、前記隠蔽領域を特定することを特徴とする請求項１乃至５の何れか１項に記載の情報処理装置。
9. 更に、前記隠蔽領域と前記組み付け元の物体の第二の形状モデルとに基づいて、前記組み付け元の物体の位置姿勢を取得する第二の位置姿勢取得手段を備えることを特徴とする請求項１乃至８の何れか１項に記載の情報処理装置。
10. 前記組み付け先の物体の位置姿勢と前記組み付け元の物体の位置姿勢とに基づいて、前記組み付け先の物体と前記組み付け元の物体との実際の相対位置姿勢を推定する推定手段と、
    前記推定された実際の相対位置姿勢に基づいて、前記組み付け先の物体と前記組み付け元の物体との組み付け状態を判定する判定手段を更に備えることを特徴とする請求項９に記載の情報処理装置。
11. 前記判定手段は、前記実際の相対位置姿勢と前記理想の相対位置姿勢との差分に基づいて、前記組み付け状態を判定することを特徴とする請求項１０に記載の情報処理装置。
12. 情報処理装置の制御方法であって、
    組み付け先の物体と組み付け元の物体とが組み付けられた場合における両物体間の理想の相対位置姿勢を取得する取得工程と、
    前記組み付け先の物体の形状を表す第一の形状モデルおよび前記組み付け元の物体の形状を表す第二の形状モデルと前記理想の相対位置姿勢とに基づいて、前記組み付け先の物体と前記組み付け元の物体とが組み付けられた状態の画像から、前記組み付け元の物体が前記組み付け先の物体を隠蔽する隠蔽領域を特定する特定工程と、
    前記隠蔽領域に対して、前記組み付け元の物体が前記組み付け先の物体を隠蔽する度合いに基づいて、前記組み付け先の物体の位置姿勢を取得する際の寄与度を設定する設定工程と、
    前記第一の形状モデルまたは前記画像に基づいて、前記組み付け先の物体の位置姿勢を取得するための情報を生成する生成工程と、
    前記生成工程で生成された情報と前記寄与度とに基づいて、前記組み付け先の物体の位置姿勢を取得する位置姿勢取得工程と、
    を有することを特徴とする情報処理装置の制御方法。
13. コンピュータを、請求項１乃至１１の何れか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images