(57)【特許請求の範囲】
【請求項１】
第１の物体が第２の物体に組み付けられた組み付け済物体の画像を取得する画像取得手段と、
部位ごとに前記第１の物体又は前記第２の物体のいずれかの属性が付与された前記組み付け済物体の３次元形状モデルを取得するモデル取得手段と、
前記画像に基づいて前記組み付け済物体の位置姿勢を取得する位置姿勢取得手段と、
前記組み付け済物体の画像に基づいて、前記位置姿勢の前記３次元形状モデルの、前記第１の物体の属性が付与された部位について当該第１の物体の組み付けの状態を評価するための第１の評価値と、前記第２の物体の属性が付与された部位について当該第２の物体の組み付けの状態を評価するための第２の評価値とを取得する評価値取得手段と、
前記第１の評価値及び前記第２の評価値に基づいて組み付けの成否を判定する判定手段と、
を備えることを特徴とする情報処理装置。
【請求項２】
前記判定手段は、前記第１の評価値及び前記第２の評価値が共に閾値以下である場合に組み付けが成功したと判定することを特徴とする請求項１に記載の情報処理装置。
【請求項３】
前記判定手段は、前記第１の評価値及び前記第２の評価値の少なくとも１つが閾値を超過する場合に組み付けが失敗したと判定することを特徴とする請求項１又は２に記載の情報処理装置。
【請求項４】
前記評価値取得手段は、
前記位置姿勢の前記３次元形状モデルの前記第１の物体の属性が付与された部位の幾何特徴と、前記画像とから取得される第１の残差に基づいて、前記第１の評価値を取得し、
前記位置姿勢の前記３次元形状モデルの前記第２の物体の属性が付与された部位の幾何特徴と、前記画像とから取得される第２の残差に基づいて、前記第２の評価値を取得する
ことを特徴とする請求項１乃至３の何れか１項に記載の情報処理装置。
【請求項５】
前記第１の評価値は、前記第１の残差の平均値、中央値、又は前記第１の残差のヒストグラムから得られる最頻値であることを特徴とする請求項４に記載の情報処理装置。
【請求項６】
前記第２の評価値は、前記第２の残差の平均値、中央値、又は前記第２の残差のヒストグラムから得られる最頻値であることを特徴とする請求項４又は５に記載の情報処理装置。
【請求項７】
前記幾何特徴は、前記３次元形状モデルを構成する局所面特徴又は局所線特徴であることを特徴とする請求項４乃至６の何れか１項に記載の情報処理装置。
【請求項８】
前記判定手段による判定結果を出力する出力手段をさらに備えることを特徴とする請求項１乃至７の何れか１項に記載の情報処理装置。
【請求項９】
前記画像は、濃淡画像および距離画像の少なくとも一方を含むことを特徴とする請求項１乃至８の何れか１項に記載の情報処理装置。
【請求項１０】
情報処理装置の制御方法であって、
画像取得手段が、第１の物体が第２の物体に組み付けられた組み付け済物体の画像を取得する画像取得工程と、
モデル取得手段が、部位ごとに前記第１の物体又は前記第２の物体のいずれかの属性が付与された前記組み付け済物体の３次元形状モデルを取得するモデル取得工程と、
位置姿勢取得手段が、前記画像に基づいて前記組み付け済物体の位置姿勢を取得する位置姿勢取得工程と、
評価値取得手段が、前記組み付け済物体の画像に基づいて、前記位置姿勢の前記３次元形状モデルの、前記第１の物体の属性が付与された部位について当該第１の物体の組み付けの状態を評価するための第１の評価値と、前記第２の物体の属性が付与された部位について当該第２の物体の組み付けの状態を評価するための第２の評価値とを取得する評価値取得工程と、
判定手段が、前記第１の評価値及び前記第２の評価値に基づいて組み付けの成否を判定する判定工程と、
を有することを特徴とする情報処理装置の制御方法。
【請求項１１】
コンピュータを、請求項１乃至９の何れか１項に記載の情報処理装置として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、情報処理装置、情報処理装置の制御方法およびプログラムに関する。
【背景技術】
【０００２】
近年のロボット技術の発展とともに、工業製品の組み立てのような複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタにより部品を把持して組み付けを行う。ロボットによる部品の組み付け作業では、ハンドによる部品把持時のずれや正常でない部品の混入等の原因により組み付けに失敗することがある。そのため、部品を組み付けた後に、正常に組み付けが行われたかどうかを確認する部品組み付け後検査が必要になる。
【０００３】
特許文献１では、２部品により構成される組み付け済部品の撮影画像を用いて組み付け後検査を行う方法が開示されている。その中で、組み付けが完了した状態の組み付け済部品の３次元形状モデルを用いて組み付け成否を判定する方法に関する記載がある。この方法では、組み付け済部品の３次元形状モデルを用いて位置姿勢を算出した後、着目部位における３次元形状モデルと撮影画像との残差により組み付け成否を判定している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１５−１１４７２２号公報
【非特許文献】
【０００５】
【非特許文献１】Ｒ．  Ｙ．  Ｔｓａｉ，  "Ａ  ｖｅｒｓａｔｉｌｅ  ｃａｍｅｒａ  ｃａｌｉｂｒａｔｉｏｎ  ｔｅｃｈｎｉｑｕｅ  ｆｏｒ  ｈｉｇｈ−ａｃｃｕｒａｃｙ  ３Ｄ  ｍａｃｈｉｎｅ  ｖｉｓｉｏｎ  ｍｅｔｒｏｌｏｇｙ  ｕｓｉｎｇ  ｏｆｆ−ｔｈｅ−ｓｈｅｌｆ  ＴＶ  ｃａｍｅｒａｓ  ａｎｄ  ｌｅｎｓｅｓ，  "  ＩＥＥＥ  Ｊｏｕｒｎａｌ  ｏｆ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．ＲＡ−３，ｎｏ．４，１９８７．
【非特許文献２】立野，小竹，内山，"ビンピッキングのための距離・濃淡画像を最ゆうに統合する高精度高安定なモデルフィッティング手法"，電子情報通信学会論文誌D， 情報・システム J９４-D（８）， １４１０−１４２２， ２０１１．
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献１に記載の方法では、組み付け失敗時に撮影画像との残差が大きくなる部位を着目部位として適切に設定する必要がある。ここで、正しく組み付けられていない組み付け済部品に対して位置姿勢を算出した場合、組み付け元部品の残差が大きくなるかもしれないし、あるいは、組み付け先部品の残差が大きくなるかもしれない。
【０００７】
しかしながら、この残差の大きさや分布は、組み付け済部品を構成する２部品の大きさの違い、形状、観測視点などに影響を受けるため、着目部位を適切に設定するのが難しいという課題がある。
【０００８】
本発明は、上記の課題に鑑みてなされたものであり、着目部位の設定なしに、組み付け成否の判定精度を向上させる技術を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記の目的を達成する本発明に係る情報処理装置は、
第１の物体が第２の物体に組み付けられた組み付け済物体の画像を取得する画像取得手段と、
部位ごとに前記第１の物体又は前記第２の物体のいずれかの属性が付与された前記組み付け済物体の３次元形状モデルを取得するモデル取得手段と、
前記画像に基づいて前記組み付け済物体の位置姿勢を取得する位置姿勢取得手段と、
前記組み付け済物体の画像に基づいて、前記位置姿勢の前記３次元形状モデルの、前記第１の物体の属性が付与された部位について当該第１の物体の組み付けの状態を評価するための第１の評価値と、前記第２の物体の属性が付与された部位について当該第２の物体の組み付けの状態を評価するための第２の評価値とを取得する評価値取得手段と、
前記第１の評価値及び前記第２の評価値に基づいて組み付けの成否を判定する判定手段と、
を備えることを特徴とする。
【発明の効果】
【００１０】
本発明によれば、着目部位の設定なしに、組み付け成否の判定精度を向上させることが可能となる。
【図面の簡単な説明】
【００１１】
【図１】第１の実施形態に係る組み付け元部品および組み付け先部品の一例を示す図。
【図２】第１の実施形態に係る情報処理装置の構成を示す図。
【図３】残差に関する説明図。
【図４】第１の実施形態に係る情報処理装置が実施する組み付けの成否判定処理の概略手順を示すフローチャート。
【図５】第１の実施形態に係る情報処理装置が実施する位置姿勢取得処理の詳細手順を示すフローチャート。
【図６】第１の実施形態に係る情報処理装置が実施する組み付けの成否判定処理の詳細手順を示すフローチャート。
【図７】第２の実施形態に係るロボットシステムの構成例を示す図。
【発明を実施するための形態】
【００１２】
以下、図面を参照しながら実施形態を説明する。なお、以下の実施形態において示す構成は一例に過ぎず、本発明は図示された構成に限定されるものではない。
【００１３】
まず、図３を参照しながら、残差に関する説明を行う。図３（ａ）のケースは、組み付け元部品８０の組み付けに失敗して奥まで嵌めきれず、組み付け先部品８１に対して少し浮いた状態である。このとき、撮影装置８２の撮影画像を用いて算出した位置姿勢に組み付け済部品（組み付け済物体）の３次元形状モデル８３を配置し、３次元点８４との残差８５により組み付け成否を判定したとする。この場合、組み付け元部品８０に対して残差が大きくなり、組み付け先部品８１に対する残差は小さくなる。
【００１４】
同様に、図３（ｂ）のケースは、組み付け元部品９０の組み付けに失敗して奥まで嵌めきれず、組み付け先部品９１に対して少し浮いた状態である。このとき、撮影装置９２の撮影画像を用いて算出した位置姿勢に組み付け済部品の３次元形状モデル９３を配置し、３次元点９４との残差９５により組み付け成否を判定したとする。この場合、組み付け先部品９１に対して残差が大きくなり、組み付け元部品９０に対する残差は小さくなる。
【００１５】
このようなケースでは、組み付け失敗時に撮影画像との残差が大きくなる部位を着目部位として適切に設定しないと、組み付け成否の判定を誤る可能性が出てくる。これに対して、本発明の実施形態では、着目部位の設定なしに、組み付け成否の判定精度を向上させる技術を提供する。
【００１６】
（第１の実施形態）
本実施形態では、組み付け済部品の３次元形状モデルに対して、部位ごとに組み付け先部品、あるいは、組み付け元部品のいずれかの属性を付与しておき、各属性に対応した組み付けの評価値をそれぞれ算出して取得し、組み付けの成否判定を行う例を説明する。
【００１７】
図１は、第１の実施形態に係る組み付け元部品および組み付け先部品の一例を示す図である。図１（ｃ）に示すような２部品から構成される組み付け済部品３０について組み付け後検査を行うものとする。組み付け済部品３０は、図１（ａ）に示すようなロボットハンドにより把持・移動される組み付け元部品１０と、当該組み付け元部品１０を組み付ける相手となる図１（ｂ）に示すような組み付け先部品２０とから構成される。図１（ｄ）については後述する。
【００１８】
＜概要＞
本実施形態における組み付け後検査とは、組み付け済部品３０の撮影画像に基づいて、適切に組み付けが行われているか否かを判定することである。具体的には、組み付け済部品３０の撮影画像を用いて組み付け済部品３０の位置姿勢を算出し、算出した位置姿勢において、組み付け済部品３０の３次元形状モデルと撮影画像との残差に基づいて成否を判定する。なお、本実施形態に係る「位置姿勢」とは、組み付け先部品２０と当該部品を撮影する撮影装置との位置姿勢の関係のことを意味する。
【００１９】
ここで、組み付けに成功している場合、組み付け済部品３０の３次元形状モデルのあらゆる部位で残差が小さくなるはずである。逆に、組み付けに失敗している場合、図３の例で前述したように、組み付け元部品１０で残差が大きくなるかもしれないし、あるいは、組み付け先部品２０の残差が大きくなるかもしれない。しかし、いずれの場合においても、少なくとも一方の部品で残差が大きくなるはずである。
【００２０】
そこで、本実施形態では、組み付け済部品３０の３次元形状モデルに対して、部位ごとに組み付け先部品２０、あるいは、組み付け元部品１０のいずれかの属性を付与しておく。そして、組み付け元部品１０に相当する部位、組み付け先部品２０に相当する部位、それぞれについて残差を算出し、少なくとも一方の部品で残差が所定値以上に大きい場合には失敗、両方の部品で残差が所定値未満であれば成功と判定する。
【００２１】
これにより、着目部位の設定を行うことなく、組み付け済部品３０を構成する２部品の相対的な大きさ、形状、観測視点などの影響を受けずに組み付けの成否判定が可能となる。
【００２２】
以下、３次元形状モデルを構成する部位ごとに組み付け先部品２０、または、組み付け元部品１０の属性を付与した組み付け済部品３０の３次元形状モデルを用いて、組み付け後検査を行う方法の詳細を説明する。
【００２３】
＜情報処理装置の構成＞
まずは図２を参照して、本実施形態に係る情報処理装置１の構成例を説明する。情報処理装置１は、モデル取得部１１０と、画像取得部１２０と、位置姿勢取得部１３０と、評価値取得部１４０と、成否判定部１５０とを備えている。
【００２４】
モデル取得部１１０は、組み付け済部品３０の形状を表す３次元形状モデルを取得する。本実施形態では、３次元形状モデルは、３次元位置と３次元法線方向とから構成される物体表面上の局所的な３次元平面情報（以後、「局所面特徴」と呼ぶ）と、３次元位置と３次元線分方向とから構成される物体輪郭上の局所的な３次元線分情報（以後、「局所線特徴」と呼ぶ）とによって構成されるものとする。なお、単に幾何特徴と称した場合は、局所面特徴と局所線特徴との一方又は両方を指すものとする。
【００２５】
ここで、組み付け済部品３０の３次元形状モデルの幾何特徴には、図１（ｄ）に示すように、組み付け元部品１０に相当する部位６０、あるいは、組み付け先部品２０に相当する部位７０、のいずれかの属性をあらかじめ付与しておく。これにより、組み付け元部品１０、あるいは、組み付け先部品２０に属する局所面特徴と局所線特徴とをそれぞれ参照できるものとする。
【００２６】
ただし、３次元形状モデルとして保持する形状情報は、対象形状を表す３次元的な幾何情報であれば良く、表現形式に特に制限はない。例えば、単純な３次元点の集合や、稜線を表す３次元ラインの集合、３次元点３点で構成される面および線の集合で表されるポリゴン形式の形状情報など、他の表現形式で表しても良い。組み付け済部品３０の３次元形状モデルは位置姿勢取得部１３０に入力される。
【００２７】
なお、本実施形態では、組み付け済部品３０の３次元形状モデルとして、局所面特徴と局所線特徴とにより構成されたモデルを例に説明する。しかし、３次元形状モデルとしては他の表現方式を用いてもよい。例えば、３点と３辺および１面により構成されるポリゴンの集合として、３次元形状モデルを表現してもよいし、単純な３次元点の集合として３次元形状モデルを表現してもよい。また陰関数の組み合わせによりパラメトリックに３次元形状モデルを表現する方法を用いてもよい。組み付け済部品３０の形状に即する限り、３次元形状モデルの表現方法に特に制限はない。
【００２８】
図２において、２次元画像撮影装置４０は、２次元画像を撮影するカメラである。撮影される２次元画像は濃淡画像であってもよいしカラー画像であってもよい。本実施形態では、２次元画像撮影装置４０は濃淡画像を出力する。２次元画像撮影装置４０が撮影する画像は画像取得部１２０を介して情報処理装置１に入力される。カメラの焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、使用する機器の仕様を参照するか、または、非特許文献１に開示される方法によって事前にキャリブレーションしておく。
【００２９】
距離画像撮影装置５０は、計測対象である物体表面上の点の３次元情報を計測する。距離画像撮影装置５０には、距離画像を出力する距離センサを用いる。距離画像は、各画素が奥行きの情報を持つ画像である。本実施形態では、距離センサとして、波長の異なる色ＩＤを付与したマルチスリットラインを対象物体に照射し、その反射光をカメラで撮影して三角測量によって距離計測を行うワンショットアクティブ式のものを利用する。
【００３０】
しかしながら、距離センサはこれに限るものではなく、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式であってもよい。また、ステレオカメラが撮影する画像から三角測量によって各画素の奥行きを計算するパッシブ式であってもよい。その他、距離画像を計測するものであればいかなるものであっても本発明の本質を損なうものではない。
【００３１】
距離画像撮影装置５０が取得した距離画像は、画像取得部１２０を介して情報処理装置１に入力される。また、距離画像撮影装置５０と２次元画像撮影装置４０との光軸は一致しており、２次元画像撮影装置４０が出力する濃淡画像の各画素と、距離画像撮影装置５０が出力する距離画像の各画素との対応関係は既知であるものとする。しかしながら、本発明の適用は、濃淡画像と距離画像とが同一の視点である場合に限るものではない。
【００３２】
例えば、濃淡画像を撮影する撮影装置と、距離画像を撮影する撮影装置とが別の位置姿勢にあり、濃淡画像と距離画像とをそれぞれ別の視点から撮影してもよい。この場合は、撮影装置間の相対的な位置姿勢は既知であるとして、距離画像中の３次元点群を濃淡画像に投影することにより、濃淡画像と距離画像との対応を取る。
【００３３】
同一の物体を撮影する撮影装置間の相対的な位置姿勢が既知であり、その画像間の対応が計算できる限り、撮影装置同士の位置関係に特に制限はない。なお、２次元画像撮影装置４０および距離画像撮影装置５０には共通の座標系が設定されているものとする。以降、この座標系を撮影装置座標系と称する。
【００３４】
位置姿勢取得部１３０は、２次元画像撮影装置４０及び距離画像撮影装置５０により撮影された濃淡画像及び距離画像と、モデル取得部１１０により取得された組み付け済部品３０の３次元形状モデルとに基づいて、撮影装置座標系に対する組み付け済部品３０の位置姿勢を算出する。処理の詳細については後述する。
【００３５】
なお、本実施形態では、濃淡画像と距離画像とを同時に利用して位置姿勢を算出する例を説明するが、本発明は濃淡画像のみ、距離画像のみを用いて位置姿勢推定を行う場合においても同様に適用可能である。
【００３６】
評価値取得部１４０は、位置姿勢取得部１３０により算出された位置姿勢情報、および組み付け済部品３０の３次元形状モデルに基づき、３次元形状モデルに付与された属性を参照して、組み付け元部品１０および組み付け先部品２０に対する組み付け評価値をそれぞれ算出して取得する。本実施形態では、組み付け済部品３０の３次元形状モデル上で同一属性を持つ局所面特徴を抽出して、距離画像から得られる３次元点に対して最近傍となる局所面特徴と、３次元点とのペアを求め、局所面特徴と３次元点との３次元距離を残差として算出する。
【００３７】
そして、同一属性の局所面特徴に対して求めた残差の平均値を、その属性の部品に対する組み付け評価値とする。なお、組み付け評価値として算出する値は、以上に述べた方法に限るものでなく、組み付け済部品３０と撮影画像との空間的なずれの大きさに応じて増減する値であれば何でもよく、計算方法および表現に特に制限はない。例えば、３次元形状モデルをポリゴンモデルで表現する場合は、その構成要素である３角メッシュと３次元点との距離を組み付け評価値としてもよい。
【００３８】
ここでは３次元形状モデルの着目部位に関する３次元距離を組み付け評価値として算出する例を示したが、他の方法により組み付け評価値を算出してもよい。例えば、組み付け元部品１０の３次元形状モデルおよび組み付け先部品２０の３次元形状モデルの相対位置姿勢の理想値と観測値との差を、組み付け評価値として利用してもよいし、３次元形状モデルと撮影画像との残差に基づいて組み付け評価値を計算してもよい。
【００３９】
組み付け元部品１０と組み付け先部品２０との組み付け部分のズレ量に応じて変動する値である限り、いかなる方式で算出した値を組み付け評価値として用いても良く、特に制限はない。
【００４０】
成否判定部１５０は、評価値取得部１４０により取得された組み付け元部品１０の評価値と組み付け先部品２０の評価値とに基づき、組み付け成否の判定を行う。
【００４１】
以上が、情報処理装置１の構成の一例についての説明である。なお、情報処理装置１には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御部、ＲＯＭ(Read Only Memory)、ＲＡＭ(Random Access Memory)、ＨＤＤ（Hard Disk Drive）等の記憶部が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力部、ネットワークカード等の通信部等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御部が記憶部に記憶されたプログラムを読み出して実行することで制御される。
【００４２】
＜情報処理装置の処理＞
次に、本実施形態における組み付け成否判定の処理手順について説明する。図４は、本実施形態に係る情報処理装置が実施する組み付け成否判定処理の手順を示すフローチャートである。
【００４３】
（ステップＳ１０００）
画像取得部１２０は、２部品が組み付けられた状態の検査対象部品の濃淡画像および距離画像を取得する。まず、２次元画像撮影装置４０から濃淡画像を取得する。同様に、距離画像撮影装置５０から距離画像を取得する。本実施形態では、距離画像には距離画像撮影装置５０から計測対象物体の表面までの距離が格納されているものとする。前述のように、２次元画像撮影装置４０と距離画像撮影装置５０との光軸は一致しているため、濃淡画像の各画素と距離画像の各画素との対応は既知である。
【００４４】
（ステップＳ１１００）
次に、ステップＳ１１００において、位置姿勢取得部１３０は、ステップＳ１０００において取得された濃淡画像及び距離画像と、３次元形状モデルとの対応付けを行い、対応付け結果に基づいて組み付け先部品２０および組み付け元部品１０の位置姿勢を推定する。以下、図５のフローチャートを参照して、ステップＳ１１００における位置姿勢推定処理について詳述する。
【００４５】
（ステップＳ１１１０）
まず、位置姿勢取得部１３０は、２次元画像撮影装置４０と距離画像撮影装置５０から構成される撮影装置に対する組み付け済部品３０の位置及び姿勢の概略値を取得する。本実施形態では、対象物体が置かれているおおよその位置や姿勢があらかじめわかっているものとして、その値を概略値として用いる。
【００４６】
しかしながら、位置及び姿勢の概略値の設定方法はこれに限るものではない。例えば、位置姿勢取得部１３０は時間軸方向に連続して計測を行うものとして、前回（前時刻）の計測値を概略の位置及び姿勢として用いるものとしてもよい。また、過去の位置及び姿勢の計測結果に基づいて物体の速度や角速度を時系列フィルタにより推定し、過去の位置及び姿勢と推定された速度・加速度から現在の位置及び姿勢を予測したものを用いてもよい。また、様々な姿勢で撮影した対象物体の画像をテンプレートとして保持しておき、入力画像に対してテンプレートマッチングを行うことによって、対象物体の大まかな位置及び姿勢を推定してもよい。
【００４７】
あるいは、他のセンサによる物体の位置及び姿勢の計測が可能である場合には、該センサによる出力値を位置及び姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を、対象物体に装着するレシーバで検出することにより位置及び姿勢を計測する磁気式センサであってもよい。また、対象物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置及び姿勢を計測する光学式センサであってもよい。その他、６自由度の位置及び姿勢を計測するセンサであればいかなるセンサであってもよい。
【００４８】
（ステップＳ１１２０）
次に、位置姿勢取得部１３０は、ステップＳ１１１０において取得された、組み付け済部品３０の概略位置姿勢に基づいて、ステップＳ１０００において取得された距離画像中の３次元点群と、組み付け済部品３０の３次元形状モデルとの対応付けを行う。具体的には、物体の概略位置姿勢と、校正済みの距離画像撮影装置５０の内部パラメータとを用いて、３次元形状モデルを構成する各局所面特徴を距離画像上に投影する。そして、投影した各局所面特徴に対応する距離画像上の距離点を、各面に対応する３次元点として保持する。
【００４９】
（ステップＳ１１３０）
位置姿勢取得部１３０は、濃淡画像上のエッジと３次元形状モデルとの対応付けを行う。ステップＳ１１２０と同様に、それぞれの物体の概略位置姿勢と校正済みの２次元画像撮影装置４０の内部パラメータとを用いて３次元形状モデルを構成する局所線特徴を画像へ投影し、画像上で検出されたエッジと３次元形状モデル中の局所線特徴とを対応付ける。エッジが各局所線特徴に対応して複数検出された場合には、複数検出されたエッジのうち、投影された局所線特徴に対して画像上で最も近いエッジを対応付ける。
【００５０】
（ステップＳ１１４０）
位置姿勢取得部１３０は、ステップＳ１１２０で算出した３次元形状モデル中の各面に対応する距離画像中の３次元点と、ステップＳ１１３０で検出した３次元形状モデル中の各線分に対応する濃淡画像上のエッジとの対応データに基づいて、組み付け先部品２０および組み付け元部品１０の位置姿勢を算出する。本ステップでは、算出した対応データに基づいて、計測データと３次元形状モデルとの間の誤差が最小になるように、線形連立方程式を解くことで、位置姿勢の更新を行う。
【００５１】
ここで、画像上の距離と３次元空間中での距離とでは尺度が異なるため、単純に連立方程式を解くだけでは計測データのどちらか一方に寄与率が偏ってしまう可能性がある。そのため、本実施形態では、非特許文献２に示すような最尤推定に基づく最適化を行うことで、尺度を合わせた位置姿勢取得を行う。
【００５２】
最尤推定に基づく位置姿勢取得方法に関しては、本発明の本質に関わる話ではないため、詳細な処理の記述は省略する。なお、計測対象物体の位置及び姿勢の取得方法は、上述の最尤推定に基づく手法に限るものでなく、例えば、Levenberg-Marquardt法による繰り返し演算を行ってもよいし、よりシンプルな方法である最急降下法によって行ってもよい。また、共役勾配法やICCG法など、他の非線形最適化計算手法を用いてもよい。
【００５３】
（ステップＳ１１５０）
位置姿勢取得部１３０は、ステップＳ１１４０で更新した位置姿勢が、収束しているか否か、すなわち、さらに反復計算を必要とするか否かの判定を行う。具体的には、補正値がほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後との差がほぼ０である場合に収束したと判定する。収束していなければ（Ｓ１１５０；Ｎｏ）、ステップＳ１１２０に戻り、更新した位置姿勢を用いて、再度位置姿勢推定処理を行う。収束していると判断した場合は（Ｓ１１５０；Ｙｅｓ）、この処理は終了し、撮影装置と組み付け済部品３０との相対的な位置姿勢の最終的な推定値が決定される。以上で図５の処理が終了し、図４に戻る。
【００５４】
（ステップＳ１２００）
成否判定部１５０は、３次元形状モデルと、ステップＳ１１００で取得された位置姿勢と、距離画像とに基づいて、組み付け成否を判定する。成否判定部１５０の判定結果は不図示の表示部等を介して出力される。出力の方法は表示に限らず、発光部や音源等を通じて成否を報知できれば何れの方法であってもよい。表示部、発光部、音源等の構成は情報処理装置１が備えていてもよいし、情報処理装置１とは別体の外部装置が備えていてもよい。以下、図６のフローチャートを参照して、ステップＳ１２００における組み付け成否判定処理について詳述する。
【００５５】
（ステップＳ１２１０）
まず、評価値取得部１４０は、３次元形状モデル中から、組み付け元部品１０に属する各局所面特徴を参照して、各面と距離画像中の３次元点との残差を算出し、この残差の平均値を組み付け元部品１０の評価値として算出する。
【００５６】
（ステップＳ１２２０）
次に、評価値取得部１４０は、３次元形状モデル中から、組み付け先部品２０に属する各局所面特徴を参照して、各面と距離画像中の３次元点との残差を算出し、この残差の平均値を組み付け先部品２０の評価値として算出する。
【００５７】
（ステップＳ１２３０）
成否判定部１５０は、ステップＳ１２１０で算出した組み付け元部品１０の評価値と、ステッＳ１２２０で算出した組み付け先部品２０の評価値とに基づき、組み付け成否を判定する。具体的には、組み付け先部品２０の評価値と、組み付け元部品１０の評価値とを、所定の閾値と比較する。閾値の設定については後述するが、２つの評価値に対して共通の値を用いてもよいし、それぞれ別の値に設定してもよい。
【００５８】
ここで、２つの評価値のうち、少なくともいずれか一方の評価値が所定の閾値を超過する場合には組み付け失敗と判定する。逆に、２つの評価値が、いずれも所定の閾値以下である場合には成功と判定する。
【００５９】
なお、閾値は、事前に設定しておく。具体的には、たとえば、組み付け先部品２０と組み付け元部品１０とが正しく組み付けられた部品を撮影した組み付け成功画像を用意する。そして、組み付け成功画像に対してステップＳ１０００からステップＳ１２００までの処理を行い、成功画像に対して算出された組み付け元部品１０の評価値、および組み付け先部品２０の評価値を、それぞれ閾値として設定する。このとき、算出された評価値をそのまま閾値として用いるのではなく、一定のバイアスを付加して、成否判定の厳しさを調整してもよい。あるいは任意の閾値をユーザが手動で設定してもよいし、このほかの方法であってもかまわない。
【００６０】
以上説明したように、本実施形態では、組み付け済部品の３次元形状モデルに対して、部位ごとに組み付け先部品、あるいは、組み付け元部品のいずれかの属性を付与しておき、各属性に対応した組み付けの評価値をそれぞれ算出して、各評価値を用いて組み付けの成否判定を行う。
【００６１】
これにより、組み付け先部品、あるいは、組み付け元部品の少なくとも一方の部品で撮影画像との不一致を検出できるため、着目部位の設定を行うことなく、且つ、組み付け済部品を構成する２部品の相対的な大きさ、形状、観測視点などの影響を受けずに、組み付けの成否を精度よく判定することが可能となる。
【００６２】
なお、本実施形態では、評価値取得部１４０が算出する評価値として、各局所面特徴の平均値を用いたが、中央値を用いてもよいし、あるいは、各局所面特徴に対して算出した残差からヒストグラムを作成し、その最頻値を用いてもよい。また、本実施形態では、局所面特徴を利用して、各面と３次元点との残差を算出したが、局所線特徴を利用してもよい。その他、組み付け評価値の算出に使用する残差やその統計量の計算は、３次元形状モデルと撮影画像との差が検出できる限り、計算方式に特に制限はない。
【００６３】
（変形例１−１）
上述の実施形態では、２つの部品から構成される組み付け済部品を対象とした事例について述べた。しかし、３つ以上の部品で構成される組み付け済部品に対して、組み付けの成否を判定する場合にも同様の方法を適用可能である。この場合にも、組み付け済部品を構成する部品の属性を、組み付け済部品の３次元形状モデルの各形状にそれぞれ付与しておく。そして、３次元形状モデルの属性に基づき、その属性に該当する部位を参照してそれぞれ評価値を算出する。
【００６４】
構成部品のいずれか１つでも評価値が閾値よりも大きければ組み付け失敗、そうでなければ成功と判定する。これにより、３つ以上の部品で構成される組み付け済部品に対しても、着目部位の設定を行うことなく、組み付けの成否を精度よく判定することが可能となる。
【００６５】
（第２の実施形態）
第１の実施形態では、複数部品から構成される組み付け済部品の組み付け成否を判定する行う方法について述べた。これに対して、本実施形態では、ロボットハンドにより把持を行った部品（把持部品）の画像を、ハンドに取り付けた撮影装置で撮影して把持の成否を判定することを想定する。なお、撮影装置はハンドに取り付けず、ロボットの動作範囲内に固定で設置したものを用いてもよい。このとき、ハンドによる隠蔽の影響を受けずに安定的に位置姿勢を算出する例を説明する。
【００６６】
図７に一例を示すような、ロボットアーム７００の先端にロボットハンド７１０を取り付け、ロボットハンド７１０により把持した把持部品７２０を、ロボットハンドに取り付けた撮影装置７３０で撮影して、把持の成否を判定することを想定する。
【００６７】
本実施形態では、正しく把持が行えた状態の、ロボットハンド７１０と把持部品７２０とを一体とみなした３次元形状モデル（以後、把持状態３次元形状モデルと呼ぶことにする）を利用する。ここで、把持状態３次元形状モデルには、ロボットハンド７１０に相当する部位と、把持部品７２０に相当する部位とを区別可能な属性を付与しておくものとする。この把持状態３次元形状モデルを、第１の実施形態における組み付け済部品３０の３次元形状モデルとみなして、同様の処理を実施する。
【００６８】
すなわち、把持状態のロボットハンド７１０および把持部品７２０を一体のものとみなして、撮影画像を用いて位置姿勢を算出し、その後、把持状態３次元形状モデルから各属性に該当する部位を参照して、撮影画像との残差を求める。もし、ロボットハンド７１０に相当する部位、あるいは把持部品７２０に相当する部位のいずれかで残差が大きい場合には、把持に失敗していると判定し、そうでなければ把持に成功していると判定する。
【００６９】
本実施形態によれば、ロボットハンドによる部品の把持の成否を、着目部位の設定を行うことなく精度よく判定することが可能となる。
【００７０】
（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【００７１】
１：情報処理装置、１０：組み付け元部品、２０：組み付け先部品、３０：組み付け済部品、１１０：モデル取得部、１２０：画像取得部、１３０：位置姿勢取得部、１４０：評価値取得部、１５０：成否判定部
【図１】
0006677522.tif 000002
【図２】
0006677522.tif 000003
【図３】
0006677522.tif 000004
【図４】
0006677522.tif 000005
【図５】
0006677522.tif 000006
【図６】
0006677522.tif 000007
【図７】
0006677522.tif 000008