位置姿勢計測装置
【課題】 対象物体と非対象物体の像を含む距離画像データを入力として対象物体の位置姿勢を推定する処理の高速化、及びメモリ消費量の軽減を目的とする。
【解決手段】 物体を撮影した距離画像上で物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する。そのために、距離画像の領域を分割した部分領域群のうち少なくとも１つの部分領域を物体領域として設定する。物体領域に物体の形状モデルを当て嵌めて、物体の位置姿勢を推定する。当て嵌めの誤差に基づいて、物体領域を更新する。
【解決手段】 物体を撮影した距離画像上で物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する。そのために、距離画像の領域を分割した部分領域群のうち少なくとも１つの部分領域を物体領域として設定する。物体領域に物体の形状モデルを当て嵌めて、物体の位置姿勢を推定する。当て嵌めの誤差に基づいて、物体領域を更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置姿勢を計測する技術に関し、特に対象物体を撮影した距離画像に基づいて対象物体の位置姿勢を算出する方法に関する。
【背景技術】
【０００２】
近年ロボット技術の発展とともに、これまで人間が行っていた工業製品の組立をロボットが代わりに行うようになりつつある。ロボットによる組立作業では、ハンドなどのエンドエフェクタによって部品を把持する。このため把持の対象となる部品とロボットとの相対的な位置姿勢を計測し、移動計画を策定して実際にアクチュエータを制御する必要がある。ロボットの位置姿勢計測には、カメラや距離センサ等で得られる、２次元画像や距離画像を用いる方法がある。オフィスや工場のシーンや工業製品の部品など、対象が直線や曲線、濃淡変化の少ない面など単調な特徴しか持たない場合には距離画像を用いる場合がある。
【０００３】
具体的な位置姿勢計測として、非特許文献１，２に、距離画像へのモデルフィッティングによる物体の位置姿勢の計測や推定を行う手法が開示されている。また、非特許文献１，３に、時系列で得られる距離画像をもとに、フレーム間での物体の位置姿勢の変化を推定する手法が開示されている。
【０００４】
ここでは、計測データとモデルとの対応間の距離を最小化を行う。
【０００５】
しかし、対象物体の周囲や背後に存在する対象物体以外の物体のデータが混在する場合や、計測データのノイズ・位置姿勢の概略値の誤差などの要因により誤った対応対データが最適化処理に混在する場合がある。これにより、位置姿勢推定精度が低下する。
【０００６】
そこで、誤りに小さな重みを設定し、誤りが処理結果に悪影響を及ぼすことを防ぐロバスト推定などが利用されている。対象物体に比べ、対象物体以外の物体はモデルとの対応間距離が大きくなる傾向があり、ロバスト推定を適用すると、最適化計算に対する非対象物体のデータからの影響が軽減する。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平０１−００９３０７
【非特許文献】
【０００８】
【非特許文献１】Ｐ．Ｊ．Ｂｅｓｌ ａｎｄ Ｎ．Ｄ．ＭｃＫａｙ，”Ａ ｍｅｔｈｏｄ ｆｏｒ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ３−Ｄ ｓｈａｐｅｓ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１４，ｎｏ．２，ｐｐ．２３９−２５６，１９９２．
【非特許文献２】Ｂ．Ｚｈｅｎｇ，Ｒ．Ｉｓｈｉｋａｗａ，Ｔ．Ｏｉｓｈｉ，Ｊ．Ｔａｋａｍａｔｓｕ，Ｋ．Ｉｋｅｕｃｈｉ，”６−ＤＯＦ Ｐｏｓｅ Ｅｓｔｉｍａｔｉｏｎ ｆｒｏｍ Ｓｉｎｇｌｅ Ｕｌｔｒａｓｏｕｎｄ Ｉｍａｇｅ Ｕｓｉｎｇ ３Ｄ ＩＰ Ｍｏｄｅｌｓ，”Ｐｒｏｃ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ ｏｎ Ｏｂｊｅｃｔ Ｔｒａｃｋｉｎｇ ａｎｄ Ｃｌａｓｓｉｆｉｃａｔｉｏｎ Ｂｅｙｏｎｄ ｔｈｅ Ｖｉｓｉｂｌｅ Ｓｐｅｃｔｒｕｍ （ＯＴＣＢＶＳ０８），ｐｐ．１−８，２００８．
【非特許文献３】Ｄ．Ａ．Ｓｉｍｏｎ，Ｍ．Ｈｅｂｅｒｔ，ａｎｄ Ｔ．Ｋａｎａｄｅ，”Ｒｅａｌ−ｔｉｍｅ ３−Ｄ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｕｓｉｎｇ ａ ｈｉｇｈ−ｓｐｅｅｄ ｒａｎｇｅ ｓｅｎｓｏｒ，”Ｐｒｏｃ．１９９４ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ’９４），ｐｐ．２２３５−２２４１，１９９４．
【非特許文献４】Ｍ．Ｍ．Ｂｌａｎｅ，Ｚ．Ｌｉｅ，Ｈ．Ｃｉｖｉ，Ｄ．Ｂ．Ｃｏｏｐｅｒ，”Ｔｈｅ ３Ｌ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｆｉｔｔｉｎｇ ｉｍｐｌｉｃｉｔ ｐｏｌｙｎｏｍｉａｌ ｃｕｒｖｅｓ ａｎｄ ｓｕｒｆａｃｅｓ ｔｏ ｄａｔａ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．３，ｐｐ．２９８−３１３，２０００．
【非特許文献５】Ｔ．Ｓａｈｉｎ，Ｍ．Ｕｎｅｌ，”Ｆｉｔｔｉｎｇ Ｇｌｏｂａｌｌｙ Ｓｔａｂｉｌｉｚｅｄ Ａｌｇｅｂｒａｉｃ Ｓｕｒｆａｃｅｓ ｔｏ Ｒａｎｇｅ Ｄａｔａ，”Ｐｒｏｃ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．２，ｐｐ．１０８３−１０８８，２００５．
【非特許文献６】Ｚ．Ｚｈａｎｇ，”Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．
【非特許文献７】Ｘ．Ｊｉａｎｇ ａｎｄ Ｈ．Ｂｕｎｋｅ，”Ｒａｎｇｅ ｉｍａｇｅ ｓｅｇｍｅｎｔａｔｉｏｎ：Ａｄａｐｔｉｖｅ ｇｒｏｕｐｉｎｇ ｏｆ ｅｄｇｅｓ ｉｎｔｏ ｒｅｇｉｏｎｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ Ａｓｉａｎ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ １９９８（ＡＣＣＶ’９８），ｐｐ．２９９−３０６，１９９８．
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、前述したような位置姿勢推定方法では、処理対象とする３次元点群の数が多くなるほど、処理負荷が増大し、かつメモリ消費量が増大する。物体の位置姿勢を推定する際に、処理対象の３次元点群を対象物体のみに限定するのが理想的である。しかし、従来のロバスト推定を用いた方法では、本来は不必要な非対象物体のデータをモデル当て嵌めの処理から除外してはいない。このため、従来の方法では、処理速度の低下・メモリ消費量の増大という課題があった。
【００１０】
本発明は、以上の課題を鑑みてなされたものであり、対象物体と非対象物体の像を含む距離画像データを入力として対象物体の位置姿勢を推定する処理の高速化、及びメモリ消費量の軽減を目的とする。
【課題を解決するための手段】
【００１１】
上記の課題を解決するため、本願発明では、以下の方法を有する。
【００１２】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測方法であって、分割手段が、前記距離画像の領域を部分領域に分割する分割工程と、設定手段が、前記分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定工程と、推定手段が、前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定工程と、更新手段が、前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新工程とを有することを特徴とする位置姿勢計測方法。
【発明の効果】
【００１３】
位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。
【図面の簡単な説明】
【００１４】
【図１】本願の実施形態を実現するためのコンピュータの構成を表すブロック図である。
【図２】第１の実施形態の方法の処理全体の流れを示すフローチャートである。
【図３】第１の実施形態の距離画像領域分割工程の流れを示すフローチャートである。
【図４】第１の実施形態のモデル当て嵌め工程の流れを示すフローチャートである。
【図５】第１の実施形態の物体領域更新工程の流れを示すフローチャートである。
【図６】３次元シーンの例を示す図である。
【図７】第２の実施形態の方法の処理全体の流れを示すフローチャートである。
【図８】第２の実施形態の物体領域更新工程の流れを示すフローチャートである。
【発明を実施するための形態】
【００１５】
本発明は、例えば、以下の実施例によって実現することができる。
【実施例】
【００１６】
［第１の実施形態］
本実施形態では、山積み状態になっている同一形状の物体群から、ロボットハンドで１個ずつ物体を把持して取り上げることを想定する。そのため、物体群の上方から距離画像を撮影する。撮影した距離画像には複数の物体までの距離値が記録されているが、その中から一番上方にある物体が占める領域を検出し、物体の位置姿勢を推定する。
【００１７】
本実施形態における処理の概略は以下のようになる。
【００１８】
まず、距離画像を同一の面に属すると推定される部分的な領域に分割する。分割された領域のうち、距離値が最小の点を含む領域、すなわち最上部にある領域から開始して、対象物体の位置姿勢を推定する。このとき、各々の領域では、把持対象とする物体に属すると推定される領域を追加しながら物体の形状モデルを当て嵌め、対象物体の位置姿勢を推定する。追加する領域はモデルを当て嵌めたときの誤差が小さくなるように選択する。この選択方法により、選択した領域群の中に複数の物体のデータが含まれる危険性を軽減することができる。
【００１９】
なお本実施形態では、部分領域という、距離画像を同一の面に属すると推定される領域に分割した各々の領域を用いる。また、物体領域という、１つ以上の部分領域から構成され、距離画像上で対象物体に属すると推定した領域を用いる。
【００２０】
以下に、図を用いて本実施形態を詳細に説明する。
【００２１】
図１は、本実施形態で用いるコンピュータのハードウェア構成である。図１において、ＣＰＵ１０１は、装置全体の動作を制御する。メモリ１０２には、ＣＰＵ１０１の動作に用いるプログラムやデータを格納する。バス１０３は、各構成モジュール間のデータ転送を行う。記憶装置１０４は、ＣＰＵ１０１に読み込むプログラムやデータを格納する。入力部１０５は、プログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成する。表示部は１０６はプロセスの動作結果を表示する。データ入出力部１０７では、装置外部とのデータを入出力する。距離画像計測装置はデータ入出力部１０７を介して本実施形態の装置に接続されている。
【００２２】
図２は、本実施形態の位置姿勢計測方法に関わる処理全体の流れを示すフローチャートである。
【００２３】
ステップＳ２０１では、処理で用いるデータの初期設定を行う。具体的には、記憶装置１０５から形状モデルデータをメモリ１０２にロードする。また、距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離、画像歪み係数）、及び距離画像の画素値と距離値の変換比率をデータ入出力部１０７に接続された不図示の距離画像計測装置から入力する。
【００２４】
物体の形状モデルは陰多項式モデルであり、（１）に示す式で定義される。ここで、Ｘ＝（ｘ，ｙ，ｚ）が３次元空間中の点の座標、ｎが陰多項式の次数である。（１）の式が０になる座標（ｘ，ｙ，ｚ）が、陰多項式モデルが表す物体の表面、０より大きい値になる座標が対象物体の外部、０より小さい値になる座標が対象物体の内部を表す。
【００２５】
【数１】
【００２６】
この陰多項式モデルは対象物体のＣＡＤモデルやレンジファインダ等で計測した３次元点群からあらかじめ生成しておく。例えば、非特許文献４に記載のように、陰多項式で表現された３次元モデルを構築する。陰多項式モデルは対象となる３次元形状のみを適切に表現するように構築することが難しい。この手法では、対象形状の表面上の点、ある距離ｃだけ外側の点、ｃだけ内側の点の３組の点群と、対応する陰多項式の値の２乗が最小となるよう陰多項式の係数を決定する。例えば、複雑な形状の陰多項式をより安定して求めるため、非特許文献５に記載のようにリッジ回帰を用いてもよい。
【００２７】
また、入力するカメラパラメータを用いて、距離画像の各画素の２次元座標および画素値から、その画素に対応する３次元空間中の点の撮像部座標系における３次元座標を算出することができる。非特許文献６に記載のように、カメラパラメータは事前に校正しておいてもよい。
【００２８】
ステップＳ２０２では、データ入出力部１０７に接続された不図示の距離画像計測装置から距離画像データを取得し、メモリ１０２に格納する。なお、距離画像の撮影では、例えば、光切断法やステレオ法により撮影してもよい。
【００２９】
ステップＳ２０３では、距離画像を部分領域に分割する。分割結果は正の整数値が記録された２次元のマップ（以下「部分領域マップ」と呼ぶ）としてメモリ１０２に蓄積する。マップの大きさは距離画像の縦・横の画素数に等しい。部分領域マップに記録された整数値は、同一の部分領域に対しては同一の値であり、異なる部分領域は異なる値を持つ。すなわち、部分領域マップに記録されているのは、距離画像の各画素が属する部分領域の識別番号である。したがって、部分領域マップの値を参照することにより、参照したマップ上の位置に相当する距離画像上の画素がいずれの部分領域に属するかを一意に決定することができる。
【００３０】
ステップＳ２０４では、ステップＳ２０３で抽出した部分領域群のうち、距離値が最小の画素を含む部分領域を探索し、物体領域として設定する。これにより、最も上方（距離画像撮影装置に近い側）に存在する部分領域が物体領域となる。以降、本ステップで物体領域に設定した部分領域を含む物体を位置姿勢の計測対象とする。なお、物体領域のデータは、その物体領域に属する部分領域の識別番号のリストとしてメモリ１０２に記録される。本ステップの処理完了後、物体領域データは１つの部分領域の識別番号を含むことになる。
【００３１】
ステップＳ２０５では、物体の形状を表す陰多項式モデルを物体領域内の距離計測点群に当て嵌めることによって、計測対象物体の位置姿勢を推定する。
【００３２】
ステップＳ２０６では、設定済みの物体領域に新たに部分領域を加えるか否かを判定して、判定結果が真なら物体領域を更新する。
【００３３】
ステップＳ２０７では、ステップＳ２０６において物体領域データが更新されたか否か判定し、更新があればステップＳ２０５に戻って対象物体の位置姿勢を再度推定する。更新が無ければ、物体領域が確定し、かつ対象物体の位置姿勢推定も完了したとして処理全体を終了する。
【００３４】
続いて、距離画像を領域分割するステップＳ２０３の詳細を説明する。本ステップは、距離画像を後述するエッジによって囲まれた閉領域ごとに分割する。分割には、非特許文献７の手法を用いてもよい。ここで検出された領域を部分領域とする。
【００３５】
図３は、本ステップの処理の流れを示すフローチャートである。
【００３６】
ステップＳ３０１では、エッジを抽出する。エッジとは距離値が不連続に変化する「ジャンプエッジ」、または物体表面の法線方向が不連続に変化する「クリーズエッジ」である。具体的には、まず距離画像上の直線（「スキャンライン」と呼ぶ）に相当する被写体上の曲線を複数の２次曲線セグメントに分割する。そして、曲線セグメントの各境界点に関して「エッジの強さ」（ジャンプエッジらしさを表す奥行き値の不連続度、クリーズエッジらしさを表すセグメントの法線方向の不連続度）を算出し、エッジの強さが閾値以上の点をエッジとして抽出する。なお、エッジの方向は様々であり、スキャンラインは距離画像の縦方向・横方向・斜め方向（４５°、１３５°）の４方向を用いて、４方向でのエッジの強さが最も高い接続点をエッジとする。以上のように、距離画像上のエッジ画素の位置を表したエッジマップを作成し、メモリ１０２に記録する。
【００３７】
ステップＳ３０２では、ステップＳ３０１で作成したエッジマップをラベリングする。その結果、距離画像は連結したエッジ画素によって区切られる閉領域群に分割される。分割結果は、距離画像と同じサイズのラベル画像としてメモリ１０２に記録する。なお、ラベルの値は閉領域ごとに一意に設定される。
【００３８】
ステップＳ３０３では、ステップＳ３０２で検出した閉領域の中から面積Ｔ（は部分領域の面積閾値）以上の領域を選択し、選択した閉領域のラベル値のリスト（以下、このリストを「領域リスト」と呼ぶ）としてメモリ１０２に記録する。
【００３９】
ステップＳ３０４では、領域リストに登録された閉領域の数が０か否か判定する。数が０ならば、ステップＳ３１２に進む。一方、数が０でなければ、ステップＳ３０５に進む。
【００４０】
ステップＳ３０５では、領域リストに登録された閉領域群から未処理の閉領域Ｒを取り出す。
【００４１】
ステップＳ３０６では、閉領域Ｒに３次元平面または３次元曲面を当て嵌める。
【００４２】
ステップＳ３０７では、ステップＳ３０６にて平面または曲面を当て嵌めた際の当て嵌め誤差が予め定められたの閾値未満であるか否か判定する。閾値未満であれば、閉領域Ｒには複数の面が含まれないと推定されＳ３０８に進む。一方、閾値以上と判定されれば、ステップＳ３０９へ進む。なお、ここで当て嵌め誤差とは、閉領域Ｒの各画素に相当する３次元点と、当て嵌めた平面または曲面上の最近傍点との間の距離の２乗和を領域Ｒの全画素について平均をとったものである。ステップＳ３０７で当て嵌め誤差が閾値以上と判定された場合、閉領域Ｒに複数の面が含まれる可能性がある。これは、距離画像に含まれるノイズなどの影響により、閉領域の境界に存在するエッジが途切れているためと推定される。
【００４３】
ステップＳ３０９では、途切れたエッジを接続して、複数の面を区分するため、エッジマップに膨張処理を施す。
【００４４】
ステップＳ３１０では、エッジマップをラベリングする。
【００４５】
ステップＳ３１１では、閉領域の候補を抽出する。ステップＳ３１０およびＳ３１１の処理内容は、それぞれステップＳ３０２およびＳ３０３と同様である。
【００４６】
ステップＳ３０７では、当て嵌め誤差が閾値未満と判定された場合、閉領域Ｒには単一の面のみ含まれると推定されるため、ステップＳ３０８に進む。
【００４７】
ステップＳ３０８では、閉領域Ｒを部分領域として登録する。
【００４８】
ステップＳ３１２では、エッジ部の画素など閉領域として未登録の画素を周囲の閉領域に統合する。このステップでも平面あるいは曲面を当て嵌めて、当て嵌め誤差が閾値未満なら統合する。
【００４９】
次に、ステップＳ２０５のモデル当て嵌め工程の詳細を説明する。本工程では、計測対象物体の位置姿勢の概略値を反復演算により繰り返し補正することにより位置姿勢を算出する。
【００５０】
図４はモデル当て嵌め工程の流れを示すフローチャートである。
【００５１】
ステップＳ４０１では、データ入出力部１０９に接続された不図示の距離画像計測装置に対する物体の位置姿勢の概略値を推定する。この処理は、特許文献１に記載のように、物体領域の距離画像データを入力として、物体認識的方法により実現してもよい。ただし、物体の位置姿勢の概略値を求める方法はこれに限るものではない。例えば、モデル当て嵌め工程を一度以上経過していれば、その工程で算出した物体の位置姿勢を表す値をメモリ１０２に記憶しておく。その値を次回のモデル当て嵌め工程において物体の概略位置姿勢の値として使うことが可能である。あるいは、過去の計測値から物体の運動の速度や角速度を線形フィルタやカルマンフィルタなど時系列フィルタリング処理により推定する。推定された速度や角速度によって現在の時刻における物体の位置姿勢を予測したものを用いてもよい。また、他のセンサによる物体の位置姿勢の計測が可能である場合には、該センサによる出力値を位置姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置姿勢を計測する光学式センサであってもよい。その他、６自由度の位置姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合には、その値を概略値として用いてもよい。
【００５２】
ステップＳ４０２では、対象物体の位置姿勢の概略値を用いて、物体領域内の各画素に対応する３次元空間中の座標を距離画像計測装置の撮像部座標系から物体モデルの座標系に変換する。なお、以下の説明では、本ステップで座標変換の結果、座標を算出した点を「物体領域の３次元点」と呼ぶことにする。
【００５３】
ステップＳ４０３では、陰多項式モデルから構築される距離場と物体領域の３次元点群の各座標に基づいて、３次元点群とモデル表面との距離の総和に対応する、（２）に示すエネルギー関数が最小化されるように位置姿勢を最適化する。
【００５４】
【数２】
【００５５】
ここでΩは物体領域の３次元点群であり、ｘ ∈Ω である。また、ｄｉｓｔは陰多項式モデルから計算される距離で、モデル表面上では０、内部では負の値、外部では正の値をとり、（３）のように定義される。
【００５６】
【数３】
【００５７】
位置姿勢の最適化は具体的には、最急降下法に基づき（４）の式による勾配から３次元点群座標ＸＫ＋１を求める（６）。ここでＸは Ｘ∈ＲＮ×３で、Ｎ個の物体領域の３次元点の座標を表す。
【００５８】
【数４】
【００５９】
【数５】
【００６０】
【数６】
【００６１】
元の３次元点群座標Ｘｋと求めたＸｋ＋１から（７）に示す行列Ａを作成し、Ａ＝ＵＳＶＴのように特異値分解を行い、位置姿勢の概略値を更新する。ここで¬ＸはＸの平均を、ｔは対象物体の位置、Ｒは姿勢を表す行列である。
【００６２】
【数７】
【００６３】
【数８】
【００６４】
【数９】
【００６５】
ステップＳ４０４では、収束判定を行い、収束していればモデル当て嵌め処理全体を終了し、そうでなければステップＳ４０２に戻る。収束判定では、（４）の式の値がほぼ０である場合や、位置姿勢に関する更新前後での誤差ベクトルの二乗和の差がほぼ０である場合に収束したと判定する。なお、上記の説明ではエネルギー最適化処理に最急降下法を用いているが、これに限るものではない。より収束の速いＧａｕｓｓ−Ｎｅｗｔｏｎ法やＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよい。また、共役勾配法など、他の非線型最適化計算手法を用いてもよい。
【００６６】
ステップＳ２０６の物体領域更新工程の詳細を説明する。図５は物体領域更新工程の流れを示すフローチャートである。
【００６７】
ステップＳ５０１では、物体領域の３次元点群について（１０）で定義される物体の陰多項式モデルとの平均距離を算出する。ただし、ここではＮは物体領域の３次元点の数であり、Φは物体領域の３次元点群である。
【００６８】
【数１０】
【００６９】
ただし、Ｎは算出の対象とする距離画像上の領域内の３次元点数で、
【００７０】
【数１１】
【００７１】
ただし、Φは算出の対象とする距離画像上の領域内の３次元点群である。
【００７２】
ステップＳ５０２では、物体領域に含まれない部分領域の中から、物体領域に追加する部分領域の候補Ｃを選ぶ。具体的には、物体領域及び物体領域に含まれない各部分領域のそれぞれに属する画素群に対応する３次元点群の重心位置を算出する。そして、物体領域に関する重心位置と各部分領域に関する重心位置との距離を算出し、距離が小さい順でｎ個の部分領域を物体領域に追加する部分領域の候補Ｃとして選択する。なお、ｎは予め定められた１以上の正の整数である。
【００７３】
ステップＳ５０３では、候補Ｃの中から未処理の部分領域を１つ選択する。ここで選択した部分領域をａとする。
【００７４】
ステップＳ５０４では、部分領域ａを物体領域に追加するか否か判定するための評価値を算出する。具体的には、部分領域ａに関して、（１０）で表される物体を表す陰多項式モデルと部分領域との平均距離を算出する。ただし、ここではＮは部分領域ａ内の３次元点の数、Φは部分領域ａ内の３次元点群である。
【００７５】
ステップＳ５０５では、次に、ステップＳ５０１で求めた平均距離に所定比率（＞１．０）をかけたものと、ステップＳ５０４で求めた平均距離とを比較する。後者が前者未満であれば、ステップＳ５０６に進む。一方、後者が前者以上と判定されれば、ステップＳ５０７に進む。
【００７６】
ステップＳ５０６では、部分領域ａを物体領域に追加する。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストに、部分領域ａの識別番号を加える。
【００７７】
ステップＳ５０７では、追加する部分領域候補Ｃに含まれる部分領域すべてを処理したか否か判定し、処理済であれば物体領域更新処理を終了する。一方、未処理の部分領域があれば、ステップＳ５０３に戻る。このようにして、すべての追加候補の部分領域に関して物体領域に追加するか否かを判定する処理を繰り返して、物体領域を更新する。
【００７８】
以上、本実施形態に関わる位置姿勢計測装置の処理の流れを説明した。
【００７９】
図６に本手法の方法を適用して、複数の物体が撮影された距離画像から１つの物体の位置姿勢を推定する様子を示す。ただし、物体領域に追加する物体領域の候補の数ｎは１とする。
【００８０】
図６（ａ）は、距離画像を撮影する対象の物体の像であり、同一形状の物体が２つ含まれている。図６（ｂ）は、これらの物体を撮影した距離画像を部分領域に分割した様子である。異なる領域を区別して表示するため、領域ごとに塗りのパターンが異なるように表してある。なお、距離画像は背景領域６１６を有する。図６（ｃ）から図６（ｅ）は物体領域を更新する各ステップを表しており、灰色に表示した部分が物体領域に相当する。図６（ｃ）は初期の物体領域であり、ステップＳ２０４の終了後の状態に相当する。距離画像撮影装置からみて最も手前側の測定点を含む部分領域６２１が物体領域として抽出されている。ステップＳ２０６にて、物体領域の更新を行うと、物体領域は図６（ｄ）に示すようになり、初期の物体領域に部分領域６３２が加わっていることが分かる。同様に２度目の物体領域の更新工程を経ると、図６（ｅ）のように部分領域６４３がさらに加わって、物体領域が３つの部分領域６４１，６４２，６４３によって構成されることになる。
【００８１】
図６（ｅ）の状態から、さらに物体領域の更新工程に進むと、部分領域６４４，６４５を物体領域に加えるか否かを判定する。しかし、部分領域６４４，６４５は部分領域６４１，６４２，６４３が属する物体とは異なる物体上の領域なので、ステップＳ５０４で算出するモデルとの平均距離が比較的大きな値となる。そのため、部分領域６４４，６４５は物体領域に加えないように判定される。
【００８２】
以上の様な処理経過を経て、最終的には部分領域６４１，６４２，６４３を含む物体が距離画像から抽出され、その位置姿勢を求めることが出来る。
【００８３】
本実施形態の方法によれば、位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。
【００８４】
なお、本実施形態は上記に限らず様々な変形例が存在する。
【００８５】
［変形例１−１］
上記の説明ではステップＳ２０６において、物体領域内の３次元点群の重心近傍に重心を持つ部分領域を、物体領域に追加する領域の候補として選択している。しかし、追加する部分領域候補の選択方法はこれに限るものではない。例えば、ステップＳ２０４で設定する初期物体領域内の３次元点群の重心近傍に重心を持つ部分領域を選択してもよい。あるいは、３次元座標の重心ではなく、距離画像上における２次元座標の重心が近い部分領域を選択してもよい。
【００８６】
［変形例１−２］
上記の説明では、物体領域に近接する度合いが高い部分領域を物体領域に追加する候補として選定している。しかし、追加候補の部分領域の選定方法はこれに限らない。例えば、面積すなわち領域に属する画素数が大きい部分領域を候補としてもよい。これは、面積の大きい領域ほど距離画像の計測ノイズなどのランダムな誤差の影響を受けにくいため、形状モデルとの距離の算出や物体領域更新後のモデル当て嵌めを高精度に行うことができるからである。
【００８７】
［変形例１−３］
他の部分領域の候補選定方法としては例えば、距離画像上で対象物体が占める領域の情報を利用するものが挙げられる。具体的には、モデル当て嵌め工程にて算出した対象物体の位置姿勢に基づいて形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして、各部分領域と対象物体が占める領域との重複率（すなわち両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が高い順に、物体領域に追加する部分領域の候補として選定する。
【００８８】
［変形例１−４］
さらには、追加候補の部分領域を選定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３に説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率の３種類の評価値に重みをかけて加算した値に基づいて候補部分領域を選定する。
【００８９】
［変形例２−１］
上記に説明した物体領域の更新工程（図５）では、物体領域と部分領域に関する形状モデルとの平均距離の大小を比較することによって部分領域を物体領域に追加する可否を判定している。しかし、可否の判定方法はこれに限らず、平均距離が小さい部分領域が優先的に追加されれば、他の方法を適用することができる。例えば、追加候補の部分領域群のうち、平均距離が最小の部分領域を物体領域に追加してもよい。
【００９０】
［変形例２−２］
部分領域の追加可否を判定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３に説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率、及び形状モデルとの平均距離の４種類の評価値に重みをかけて加算した値に基づいて追加可否を判定する。
【００９１】
［変形例２−３］
また、部分領域は１つずつ追加する必然性はなく、複数の部分領域を同時に物体領域に追加してもよい。その場合、追加する部分領域の候補群から、評価値（例えば部分領域の３次元点群とモデルとの平均距離）が上位複数個の部分領域を物体領域に追加すべく選択する。
【００９２】
［変形例３−１］
部分領域の分割方法は上記の説明に限るものでなく、任意の手法を用いることができることは言うまでもない。例えば、所定の評価基準に基づいて距離画像の部分領域を近似するパラメトリックな面の種類（種類ごとに面の数式表現が異なる）を選択する。選択した種類の面で距離画像の表す３次元形状を近似する。例えば、非特許文献８に記載のように、近似させてもよい。また、近似誤差の少ない領域を一続きの面として抽出してもよい。
【００９３】
［変形例３−２］
あるいは、距離データに応じて距離画像を分割するのではなく、所定の分割方法で分割してもよい。例えば、１画素ごとに分割してもよいし、あるいは距離画像の縦横をそれぞれ１０画素ごとに格子状に区切るなどしてもよい。
【００９４】
［変形例４−１］
上記の説明では、初期の物体領域に含まれる部分領域として、最も距離値が小さい画素を含む部分領域を選択していた。しかし、初期物体領域を設定する方法はこれに限るものではなく、最も上方に配置されている物体上の領域である可能性のある部分領域を選択すればよい。例えば、距離値の平均が最小の部分領域を選択してもよいし、距離値が小さい順に上位ｍ個（ｍは２以上の正の整数）の画素の平均距離値が最小の部分領域を選択してもよい。
【００９５】
［変形例４−２］
また、初期の物体領域として距離値が小さい部分領域を優先するのは、物体を上方から撮像し、上方からロボットハンドで把持することを想定しているためである。しかし、その想定が異なれば、想定に適合した初期物体領域の設定方法を採用すればよい。例えば、特定色の物体の位置姿勢を推定するのであれば、距離画像のほかにカラー画像を撮影し、当該の特定色に最も色が合致するカラー画像上の領域に対応する距離画像上の領域を探索し、その距離画像上の領域を含む部分領域を初期物体領域とすればよい。
【００９６】
［変形例４−３］
あるいは、対象物体の概略の位置姿勢が予め分かっていれば、各部分領域について３次元点群の座標と（１０）で表される平均距離を算出し、平均距離が最小の部分領域を初期物体領域としてもよい。
【００９７】
［変形例４−４］
変形４−３と同様に対象物体の概略の位置姿勢が予め分かっていれば、距離画像上で対象物体が占める領域の情報を利用して初期物体領域を決定することができる。具体的には、対象物体の位置姿勢の概略値に基づいて形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして、各部分領域と対象物体が占める領域との重複率（すなわち両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が最も高い部分領域を初期の物体領域として選ぶ。
【００９８】
［変形例４−５］
また、初期の物体領域に含まれる部分領域の数は１つに限るものではなく、複数でもよい。例えば、距離値の平均が小さい順（変形例４−１の場合）、特定色との合致度が高い順（変形例４−２の場合）、平均距離が小さい順（変形例４−３の場合）、重複率の高い順（変形例４−４の場合）に所定の数の部分領域を物体領域として選択する。
【００９９】
［変形例５］
前述の説明では（１０）で定義される陰多項式モデルと３次元点群との平均距離を、部分領域を追加するか否か判定するなどの用途で評価値として用いている。しかしながら、判定基準とする評価値はこれに限るものではなく、物体のモデルと計測点群との乖離の度合いを示す値であればよい。例えば平均距離に替えて、（１２）、（１３）で表される評価値を用いてもよい。
【０１００】
【数１２】
【０１０１】
ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数で、Φは当該領域内の３次元点群である。
【０１０２】
【数１３】
【０１０３】
ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数で、Φは当該領域内の３次元点群である。
【０１０４】
これらの評価値ＤＲＭＳ（１２）、Ｄａｖｅｒａｇｅ（１３）は形状モデルと３次元点群との面の位置の一致度合いを表す。
【０１０５】
［第２の実施形態］
前述の実施形態では、ある部分領域を物体領域に追加する前に、その部分領域が対象物体に相当する領域か否かを判定し、判定結果が真ならば、その部分領域を以後一貫して物体領域の一部としていた。本実施形態では、物体領域に追加済みのある部分領域が非対象物体に相当する部分領域であるか否か判定し、判定結果が真ならば、その部分領域を物体領域から削除する構成を説明する。
【０１０６】
図７は、本実施形態の装置に関する処理全体の流れを示すフローチャートである。
【０１０７】
図７におけるステップＳ７０１からＳ７０４の処理は、それぞれステップＳ２０１からステップＳ２０４と同じ内容である。ステップＳ７０５はステップＳ２０５と同様であるが、物体の位置姿勢を算出した段階で、（１０）で表される計測点と物体モデルとの平均距離を物体領域に関して算出し、メモリ１０２に格納する。この際、前回の物体モデル当て嵌め処理で算出してメモリ１０２に格納した平均距離データを上書きせずに、メモリ１０２内の別の記憶領域にコピーしておく。
【０１０８】
ステップＳ７０６では、物体領域に含まれる部分領域を更新する。本ステップの処理においては、物体領域に部分領域が追加されるか、あるいは追加済みの部分領域が物体領域から除外される。
【０１０９】
ステップＳ７０７では、ステップＳ７０６にて物体領域から部分領域が除外されたか否か判定する。除外されたと判定した場合は、物体の位置姿勢推定処理全体を終了する。ここで、処理を終了するのは、対象物体に相当する部分領域はすべて物体領域として選択済みであり、直近に追加した部分領域が非対象物体に相当すると想定するからである。一方、ステップＳ７０７にて部分領域が除外されていないと判定した場合は、ステップＳ７０５に戻り、物体領域の３次元点群に物体の形状モデルを当て嵌めることによって対象物体の位置姿勢を更新する。
【０１１０】
図１２は、ステップＳ７０６における物体領域更新処理の流れを示すフローチャートである。
【０１１１】
ステップＳ８０１では、物体領域の３次元点群と物体モデルとの平均距離について、直近および前回の物体モデル当て嵌め工程（ステップＳ７０５）で算出した値を比較し、増加していればステップＳ８０２に進む。一方、増加していないと判定すれば、ステップＳ８０３へ進む。
【０１１２】
ステップＳ８０２では、直近に物体領域に追加した部分領域を物体領域から取り除く。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストから、取り除く部分領域の識別番号を削除する。
【０１１３】
ステップＳ８０３では、物体領域に含まれない部分領域の中から選んだ部分領域を物体領域に追加する。この処理は図５のフローチャートに示す流れと同じである。
【０１１４】
以上に説明した処理を行うことにより、物体領域に追加すべく選択した部分領域の妥当性、すなわち当該部分領域が対象物体に相当する度合いを、物体モデルを当て嵌めたときの当て嵌め誤差で確かめることができる。その結果、第１の実施形態のように部分領域ごとの評価値で追加する部分領域を選択する構成に比べ、誤って非対象物体に相当する部分領域を物体領域として選択する危険性が軽減され、より正確に物体領域を決定することが可能である。
【０１１５】
なお、上記の説明では追加済みの部分領域を削除するか否かの判定基準として（１０）で表される平均距離を用いた。しかし、判定基準はこれに限るものではなく、物体モデルの当て嵌め誤差に比例して増加する値であり、かつ物体領域のサイズに依存しないように規格化された値ならば任意の値を用いることができる。例えば、ステップＳ４０３（図４）の処理における位置姿勢更新後の誤差ベクトルの大きさを、物体領域に含まれる画素で平均化した値を基準値としてもよい。
【０１１６】
また、上記の説明では部分領域を物体領域から削除した場合に処理全体を終了しているが、さらに物体領域の更新を継続してもよい。この場合、除外した部分領域が次回の物体領域更新工程で再び物体領域に加えられて無限ループに陥らないように、除外した部分領域は物体領域として追加する候補から除外する。あるいは物体領域に追加する部分領域の候補の中で選択順位を下げる（例えば最下位に設定する）という処理を追加する。
【０１１７】
なお、本実施形態は上記に限らず変形例が存在する。
【０１１８】
［変形例１］
上記の説明では、部分領域を物体領域から除外するか否か判定する基準を平均距離の増減としている。しかし、判定基準はこれに限らず、部分領域を追加する前後でのモデル当て嵌めの精度の変化が評価できればよい。例えば、直近に物体領域に追加した部分領域に関する平均距離と、物体領域に関する平均距離とを比較し、前者が後者以上であれば直近に追加した部分領域を削除すると判定する（逆に、前者が後者未満であれば部分領域を削除しない）。
【０１１９】
［変形例２］
あるいは、物体領域に関する平均距離に替えて、物体領域から直近に追加した部分領域を除外した領域に関する平均距離を用いてもよい。
【０１２０】
［変形例３］
さらに、平均距離に替えて、第１の実施形態の変形例４で説明した評価値を用いて部分領域を除外するか否か判定してもよい。
【０１２１】
［第３の実施形態］
前述の実施形態では、陰多項式で表現した物体の形状モデルを物体領域の３次元点群に当て嵌めることによって、物体の位置姿勢を推定している。しかしながら、位置姿勢の推定方法はこれに限るものではなく、物体の形状モデルおよび物体領域の３次元点群の情報に基づくものであれば、任意の方法を用いることが出来る。
【０１２２】
本実施形態では、形状モデルとして三角形の平面パッチ群から構成されるパッチモデルとし、このモデルと物体領域の３次元点群との間で対応点の対を求める。対応点対間の距離の和が最小になるように物体の位置姿勢を最適化する構成を説明する。
【０１２３】
本実施形態の処理全体の流れは図２のフローチャートと同様である。ただし、ステップＳ２０１で記憶装置１０４から入力する形状モデルデータは、三角形パッチの各頂点の識別番号と座標、各三角形パッチの識別番号とそのパッチを構成する頂点の識別番号から構成されるものとする。
【０１２４】
また、位置姿勢の推定工程（ステップＳ４０３、図４）では、まず物体領域の３次元点群およびモデル上の点の組からなる最近傍点対を求める。求めた最近傍点対はメモリ１０２に最近傍点対のリストとして記憶される。
【０１２５】
具体的には、まず物体領域の３次元点群から最近傍点対の候補となるサンプル点を選択する。サンプル点は、距離画像上の物体領域内の１画素ごとに選択してもよいし、所定数の画素（例えば５画素）ごとに画像領域を間引きして選択してもよい。次に、サンプル点¬ｐに対応する形状モデル上の最近傍点を求める。サンプル点¬ｐと３頂点¬ｒ１，¬ｒ２，¬ｒ３からなる三角形パッチｔとの間の最小距離ｄは（１４）のように表される。
【０１２６】
【数１４】
【０１２７】
ただし、ｕ∈［０，１］，ｖ∈［０，１］，ｗ∈［０，１］である。形状モデルＴがＮ個の三角形パッチｔ１，…，ｔＮから構成されるとすると、サンプル点¬ｐと形状モデルＴ上の最近傍点との間の距離は（１５）のように表される。
【０１２８】
【数１５】
【０１２９】
すなわちサンプル点の最近傍点を求めるには、サンプル点と各三角形パッチとの最小距離および最近傍点の座標を表す係数の組（ｕ，ｖ）を算出する。すべての三角形パッチの中でサンプル点との最小距離が最小となる三角形パッチと係数の組（ｕ，ｖ）を探索する。以上に説明した最近傍点対の探索処理の結果は、サンプル点の３次元座標、形状モデル上の最近傍点が存在する三角形パッチの識別番号、最近傍点の座標を表す係数（ｕ，ｖ）、の組を全てのサンプル点に対して羅列したリストとしてメモリ１０２に記録される。
【０１３０】
次に、最近傍点対リストに登録された最近傍点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、結果を対象物体の位置姿勢とする。距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ，ｙ，ｚ）に変換することが出来る。ここで、計測対象物体の位置姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。いま、ステップＳ４０１で算出した対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ０，ｙ０，ｚ０）に変換されるとする。（ｘ，ｙ，ｚ）は対象物体の位置姿勢により変化するものであり、（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって（１６）のように近似できる。
【０１３１】
【数１６】
【０１３２】
メモリ１０２に格納されたある最近傍点対の形状モデル側最近傍点が存在する三角形パッチ面の対象物体座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１，ａ，ｂ，ｃ，ｅは定数）とする。正しいｓによって変換される（ｘ，ｙ，ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）を満たすと仮定する。（１６）を平面の方程式に代入すると（１７）が得られる。
【０１３３】
【数１７】
【０１３４】
ただし、ｑ＝ａｘ０＋ｂｙ０＋ｃｚ０（定数）とした。
【０１３５】
（１７）は最近傍点対すべてについて成り立つため、（１８）のようなΔｓｉに関する線形連立方程式が成り立つ。
【０１３６】
【数１８】
【０１３７】
ここで、（１８）を（１９）のように表す。
【０１３８】
【数１９】
【０１３９】
（１９）をもとに、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔｓを求める。しかし、距離画像の計測点群に含まれるノイズなどによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値である計測点群データでは誤差ｅ−ｑが大きくなる。そのため、（１８）、（１９）の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差ｅ−ｑが大きいデータには小さな重みを与え、誤差ｅ−ｑが小さいデータには大きな重みを与える。重みは例えば（２０）に示すようなＴｕｋｅｙの関数により与える。
【０１４０】
【数２０】
【０１４１】
ｃは定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればなんでもよい。
【０１４２】
各データに対応する重みをｗｉとする。ここで（２１）のように重み行列Ｗを定義する。
【０１４３】
【数２１】
【０１４４】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。ＮＣは最近傍点対の数である。
【０１４５】
この重み行列Ｗを用いて、（１９）を（２２）のように変形する。
【０１４６】
【数２２】
【０１４７】
（２３）のように（２２）を解くことにより補正値Δｓを求める。
【０１４８】
【数２３】
【０１４９】
このようにして算出した補正値Δｓによりモデルの位置・姿勢の概略値を補正し、結果をメモリ１０２に格納する。
【０１５０】
なお、上述した位置姿勢の最適化計算手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に基づくものであるが、これに限らずＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法や最急降下法などを用いてもよい。
【０１５１】
ステップＳ４０４では、収束判定を行い、収束していれば位置姿勢の推定処理を終了し、そうでなければステップＳ４０２に戻る。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルＥの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。
【０１５２】
物体領域の更新処理は第１の実施形態と同様であるが、部分領域を追加するか否かを判定するための評価値は（１０）に表される平均距離ではなく、ＲＭＳ誤差（誤差ベクトルの要素の二乗和をサンプル点数で除算した値の平方根）を用いる。物体領域に関する誤差ベクトルはステップＳ４０３で算出済みである。物体領域に追加する候補の部分領域に関する誤差ベクトルは、上述の物体領域の場合と同様に算出する。
【０１５３】
［その他の実施形態］
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
【産業上の利用可能性】
【０１５４】
本発明は、ロボットと物体との間の相対的な位置姿勢を推定するのに利用することができる。
【技術分野】
【０００１】
本発明は、３次元形状が既知である物体の位置姿勢を計測する技術に関し、特に対象物体を撮影した距離画像に基づいて対象物体の位置姿勢を算出する方法に関する。
【背景技術】
【０００２】
近年ロボット技術の発展とともに、これまで人間が行っていた工業製品の組立をロボットが代わりに行うようになりつつある。ロボットによる組立作業では、ハンドなどのエンドエフェクタによって部品を把持する。このため把持の対象となる部品とロボットとの相対的な位置姿勢を計測し、移動計画を策定して実際にアクチュエータを制御する必要がある。ロボットの位置姿勢計測には、カメラや距離センサ等で得られる、２次元画像や距離画像を用いる方法がある。オフィスや工場のシーンや工業製品の部品など、対象が直線や曲線、濃淡変化の少ない面など単調な特徴しか持たない場合には距離画像を用いる場合がある。
【０００３】
具体的な位置姿勢計測として、非特許文献１，２に、距離画像へのモデルフィッティングによる物体の位置姿勢の計測や推定を行う手法が開示されている。また、非特許文献１，３に、時系列で得られる距離画像をもとに、フレーム間での物体の位置姿勢の変化を推定する手法が開示されている。
【０００４】
ここでは、計測データとモデルとの対応間の距離を最小化を行う。
【０００５】
しかし、対象物体の周囲や背後に存在する対象物体以外の物体のデータが混在する場合や、計測データのノイズ・位置姿勢の概略値の誤差などの要因により誤った対応対データが最適化処理に混在する場合がある。これにより、位置姿勢推定精度が低下する。
【０００６】
そこで、誤りに小さな重みを設定し、誤りが処理結果に悪影響を及ぼすことを防ぐロバスト推定などが利用されている。対象物体に比べ、対象物体以外の物体はモデルとの対応間距離が大きくなる傾向があり、ロバスト推定を適用すると、最適化計算に対する非対象物体のデータからの影響が軽減する。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平０１−００９３０７
【非特許文献】
【０００８】
【非特許文献１】Ｐ．Ｊ．Ｂｅｓｌ ａｎｄ Ｎ．Ｄ．ＭｃＫａｙ，”Ａ ｍｅｔｈｏｄ ｆｏｒ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ３−Ｄ ｓｈａｐｅｓ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．１４，ｎｏ．２，ｐｐ．２３９−２５６，１９９２．
【非特許文献２】Ｂ．Ｚｈｅｎｇ，Ｒ．Ｉｓｈｉｋａｗａ，Ｔ．Ｏｉｓｈｉ，Ｊ．Ｔａｋａｍａｔｓｕ，Ｋ．Ｉｋｅｕｃｈｉ，”６−ＤＯＦ Ｐｏｓｅ Ｅｓｔｉｍａｔｉｏｎ ｆｒｏｍ Ｓｉｎｇｌｅ Ｕｌｔｒａｓｏｕｎｄ Ｉｍａｇｅ Ｕｓｉｎｇ ３Ｄ ＩＰ Ｍｏｄｅｌｓ，”Ｐｒｏｃ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ ｏｎ Ｏｂｊｅｃｔ Ｔｒａｃｋｉｎｇ ａｎｄ Ｃｌａｓｓｉｆｉｃａｔｉｏｎ Ｂｅｙｏｎｄ ｔｈｅ Ｖｉｓｉｂｌｅ Ｓｐｅｃｔｒｕｍ （ＯＴＣＢＶＳ０８），ｐｐ．１−８，２００８．
【非特許文献３】Ｄ．Ａ．Ｓｉｍｏｎ，Ｍ．Ｈｅｂｅｒｔ，ａｎｄ Ｔ．Ｋａｎａｄｅ，”Ｒｅａｌ−ｔｉｍｅ ３−Ｄ ｐｏｓｅ ｅｓｔｉｍａｔｉｏｎ ｕｓｉｎｇ ａ ｈｉｇｈ−ｓｐｅｅｄ ｒａｎｇｅ ｓｅｎｓｏｒ，”Ｐｒｏｃ．１９９４ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ’９４），ｐｐ．２２３５−２２４１，１９９４．
【非特許文献４】Ｍ．Ｍ．Ｂｌａｎｅ，Ｚ．Ｌｉｅ，Ｈ．Ｃｉｖｉ，Ｄ．Ｂ．Ｃｏｏｐｅｒ，”Ｔｈｅ ３Ｌ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｆｉｔｔｉｎｇ ｉｍｐｌｉｃｉｔ ｐｏｌｙｎｏｍｉａｌ ｃｕｒｖｅｓ ａｎｄ ｓｕｒｆａｃｅｓ ｔｏ ｄａｔａ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．３，ｐｐ．２９８−３１３，２０００．
【非特許文献５】Ｔ．Ｓａｈｉｎ，Ｍ．Ｕｎｅｌ，”Ｆｉｔｔｉｎｇ Ｇｌｏｂａｌｌｙ Ｓｔａｂｉｌｉｚｅｄ Ａｌｇｅｂｒａｉｃ Ｓｕｒｆａｃｅｓ ｔｏ Ｒａｎｇｅ Ｄａｔａ，”Ｐｒｏｃ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，ｖｏｌ．２，ｐｐ．１０８３−１０８８，２００５．
【非特許文献６】Ｚ．Ｚｈａｎｇ，”Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，ｖｏｌ．２２，ｎｏ．１１，ｐｐ．１３３０−１３３４，２０００．
【非特許文献７】Ｘ．Ｊｉａｎｇ ａｎｄ Ｈ．Ｂｕｎｋｅ，”Ｒａｎｇｅ ｉｍａｇｅ ｓｅｇｍｅｎｔａｔｉｏｎ：Ａｄａｐｔｉｖｅ ｇｒｏｕｐｉｎｇ ｏｆ ｅｄｇｅｓ ｉｎｔｏ ｒｅｇｉｏｎｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ Ａｓｉａｎ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ １９９８（ＡＣＣＶ’９８），ｐｐ．２９９−３０６，１９９８．
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、前述したような位置姿勢推定方法では、処理対象とする３次元点群の数が多くなるほど、処理負荷が増大し、かつメモリ消費量が増大する。物体の位置姿勢を推定する際に、処理対象の３次元点群を対象物体のみに限定するのが理想的である。しかし、従来のロバスト推定を用いた方法では、本来は不必要な非対象物体のデータをモデル当て嵌めの処理から除外してはいない。このため、従来の方法では、処理速度の低下・メモリ消費量の増大という課題があった。
【００１０】
本発明は、以上の課題を鑑みてなされたものであり、対象物体と非対象物体の像を含む距離画像データを入力として対象物体の位置姿勢を推定する処理の高速化、及びメモリ消費量の軽減を目的とする。
【課題を解決するための手段】
【００１１】
上記の課題を解決するため、本願発明では、以下の方法を有する。
【００１２】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測方法であって、分割手段が、前記距離画像の領域を部分領域に分割する分割工程と、設定手段が、前記分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定工程と、推定手段が、前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定工程と、更新手段が、前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新工程とを有することを特徴とする位置姿勢計測方法。
【発明の効果】
【００１３】
位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。
【図面の簡単な説明】
【００１４】
【図１】本願の実施形態を実現するためのコンピュータの構成を表すブロック図である。
【図２】第１の実施形態の方法の処理全体の流れを示すフローチャートである。
【図３】第１の実施形態の距離画像領域分割工程の流れを示すフローチャートである。
【図４】第１の実施形態のモデル当て嵌め工程の流れを示すフローチャートである。
【図５】第１の実施形態の物体領域更新工程の流れを示すフローチャートである。
【図６】３次元シーンの例を示す図である。
【図７】第２の実施形態の方法の処理全体の流れを示すフローチャートである。
【図８】第２の実施形態の物体領域更新工程の流れを示すフローチャートである。
【発明を実施するための形態】
【００１５】
本発明は、例えば、以下の実施例によって実現することができる。
【実施例】
【００１６】
［第１の実施形態］
本実施形態では、山積み状態になっている同一形状の物体群から、ロボットハンドで１個ずつ物体を把持して取り上げることを想定する。そのため、物体群の上方から距離画像を撮影する。撮影した距離画像には複数の物体までの距離値が記録されているが、その中から一番上方にある物体が占める領域を検出し、物体の位置姿勢を推定する。
【００１７】
本実施形態における処理の概略は以下のようになる。
【００１８】
まず、距離画像を同一の面に属すると推定される部分的な領域に分割する。分割された領域のうち、距離値が最小の点を含む領域、すなわち最上部にある領域から開始して、対象物体の位置姿勢を推定する。このとき、各々の領域では、把持対象とする物体に属すると推定される領域を追加しながら物体の形状モデルを当て嵌め、対象物体の位置姿勢を推定する。追加する領域はモデルを当て嵌めたときの誤差が小さくなるように選択する。この選択方法により、選択した領域群の中に複数の物体のデータが含まれる危険性を軽減することができる。
【００１９】
なお本実施形態では、部分領域という、距離画像を同一の面に属すると推定される領域に分割した各々の領域を用いる。また、物体領域という、１つ以上の部分領域から構成され、距離画像上で対象物体に属すると推定した領域を用いる。
【００２０】
以下に、図を用いて本実施形態を詳細に説明する。
【００２１】
図１は、本実施形態で用いるコンピュータのハードウェア構成である。図１において、ＣＰＵ１０１は、装置全体の動作を制御する。メモリ１０２には、ＣＰＵ１０１の動作に用いるプログラムやデータを格納する。バス１０３は、各構成モジュール間のデータ転送を行う。記憶装置１０４は、ＣＰＵ１０１に読み込むプログラムやデータを格納する。入力部１０５は、プログラムを起動したり、プログラムの動作を指定したりするための入力装置を構成する。表示部は１０６はプロセスの動作結果を表示する。データ入出力部１０７では、装置外部とのデータを入出力する。距離画像計測装置はデータ入出力部１０７を介して本実施形態の装置に接続されている。
【００２２】
図２は、本実施形態の位置姿勢計測方法に関わる処理全体の流れを示すフローチャートである。
【００２３】
ステップＳ２０１では、処理で用いるデータの初期設定を行う。具体的には、記憶装置１０５から形状モデルデータをメモリ１０２にロードする。また、距離画像計測装置の撮像部のカメラパラメータ（主点位置、焦点距離、画像歪み係数）、及び距離画像の画素値と距離値の変換比率をデータ入出力部１０７に接続された不図示の距離画像計測装置から入力する。
【００２４】
物体の形状モデルは陰多項式モデルであり、（１）に示す式で定義される。ここで、Ｘ＝（ｘ，ｙ，ｚ）が３次元空間中の点の座標、ｎが陰多項式の次数である。（１）の式が０になる座標（ｘ，ｙ，ｚ）が、陰多項式モデルが表す物体の表面、０より大きい値になる座標が対象物体の外部、０より小さい値になる座標が対象物体の内部を表す。
【００２５】
【数１】
【００２６】
この陰多項式モデルは対象物体のＣＡＤモデルやレンジファインダ等で計測した３次元点群からあらかじめ生成しておく。例えば、非特許文献４に記載のように、陰多項式で表現された３次元モデルを構築する。陰多項式モデルは対象となる３次元形状のみを適切に表現するように構築することが難しい。この手法では、対象形状の表面上の点、ある距離ｃだけ外側の点、ｃだけ内側の点の３組の点群と、対応する陰多項式の値の２乗が最小となるよう陰多項式の係数を決定する。例えば、複雑な形状の陰多項式をより安定して求めるため、非特許文献５に記載のようにリッジ回帰を用いてもよい。
【００２７】
また、入力するカメラパラメータを用いて、距離画像の各画素の２次元座標および画素値から、その画素に対応する３次元空間中の点の撮像部座標系における３次元座標を算出することができる。非特許文献６に記載のように、カメラパラメータは事前に校正しておいてもよい。
【００２８】
ステップＳ２０２では、データ入出力部１０７に接続された不図示の距離画像計測装置から距離画像データを取得し、メモリ１０２に格納する。なお、距離画像の撮影では、例えば、光切断法やステレオ法により撮影してもよい。
【００２９】
ステップＳ２０３では、距離画像を部分領域に分割する。分割結果は正の整数値が記録された２次元のマップ（以下「部分領域マップ」と呼ぶ）としてメモリ１０２に蓄積する。マップの大きさは距離画像の縦・横の画素数に等しい。部分領域マップに記録された整数値は、同一の部分領域に対しては同一の値であり、異なる部分領域は異なる値を持つ。すなわち、部分領域マップに記録されているのは、距離画像の各画素が属する部分領域の識別番号である。したがって、部分領域マップの値を参照することにより、参照したマップ上の位置に相当する距離画像上の画素がいずれの部分領域に属するかを一意に決定することができる。
【００３０】
ステップＳ２０４では、ステップＳ２０３で抽出した部分領域群のうち、距離値が最小の画素を含む部分領域を探索し、物体領域として設定する。これにより、最も上方（距離画像撮影装置に近い側）に存在する部分領域が物体領域となる。以降、本ステップで物体領域に設定した部分領域を含む物体を位置姿勢の計測対象とする。なお、物体領域のデータは、その物体領域に属する部分領域の識別番号のリストとしてメモリ１０２に記録される。本ステップの処理完了後、物体領域データは１つの部分領域の識別番号を含むことになる。
【００３１】
ステップＳ２０５では、物体の形状を表す陰多項式モデルを物体領域内の距離計測点群に当て嵌めることによって、計測対象物体の位置姿勢を推定する。
【００３２】
ステップＳ２０６では、設定済みの物体領域に新たに部分領域を加えるか否かを判定して、判定結果が真なら物体領域を更新する。
【００３３】
ステップＳ２０７では、ステップＳ２０６において物体領域データが更新されたか否か判定し、更新があればステップＳ２０５に戻って対象物体の位置姿勢を再度推定する。更新が無ければ、物体領域が確定し、かつ対象物体の位置姿勢推定も完了したとして処理全体を終了する。
【００３４】
続いて、距離画像を領域分割するステップＳ２０３の詳細を説明する。本ステップは、距離画像を後述するエッジによって囲まれた閉領域ごとに分割する。分割には、非特許文献７の手法を用いてもよい。ここで検出された領域を部分領域とする。
【００３５】
図３は、本ステップの処理の流れを示すフローチャートである。
【００３６】
ステップＳ３０１では、エッジを抽出する。エッジとは距離値が不連続に変化する「ジャンプエッジ」、または物体表面の法線方向が不連続に変化する「クリーズエッジ」である。具体的には、まず距離画像上の直線（「スキャンライン」と呼ぶ）に相当する被写体上の曲線を複数の２次曲線セグメントに分割する。そして、曲線セグメントの各境界点に関して「エッジの強さ」（ジャンプエッジらしさを表す奥行き値の不連続度、クリーズエッジらしさを表すセグメントの法線方向の不連続度）を算出し、エッジの強さが閾値以上の点をエッジとして抽出する。なお、エッジの方向は様々であり、スキャンラインは距離画像の縦方向・横方向・斜め方向（４５°、１３５°）の４方向を用いて、４方向でのエッジの強さが最も高い接続点をエッジとする。以上のように、距離画像上のエッジ画素の位置を表したエッジマップを作成し、メモリ１０２に記録する。
【００３７】
ステップＳ３０２では、ステップＳ３０１で作成したエッジマップをラベリングする。その結果、距離画像は連結したエッジ画素によって区切られる閉領域群に分割される。分割結果は、距離画像と同じサイズのラベル画像としてメモリ１０２に記録する。なお、ラベルの値は閉領域ごとに一意に設定される。
【００３８】
ステップＳ３０３では、ステップＳ３０２で検出した閉領域の中から面積Ｔ（は部分領域の面積閾値）以上の領域を選択し、選択した閉領域のラベル値のリスト（以下、このリストを「領域リスト」と呼ぶ）としてメモリ１０２に記録する。
【００３９】
ステップＳ３０４では、領域リストに登録された閉領域の数が０か否か判定する。数が０ならば、ステップＳ３１２に進む。一方、数が０でなければ、ステップＳ３０５に進む。
【００４０】
ステップＳ３０５では、領域リストに登録された閉領域群から未処理の閉領域Ｒを取り出す。
【００４１】
ステップＳ３０６では、閉領域Ｒに３次元平面または３次元曲面を当て嵌める。
【００４２】
ステップＳ３０７では、ステップＳ３０６にて平面または曲面を当て嵌めた際の当て嵌め誤差が予め定められたの閾値未満であるか否か判定する。閾値未満であれば、閉領域Ｒには複数の面が含まれないと推定されＳ３０８に進む。一方、閾値以上と判定されれば、ステップＳ３０９へ進む。なお、ここで当て嵌め誤差とは、閉領域Ｒの各画素に相当する３次元点と、当て嵌めた平面または曲面上の最近傍点との間の距離の２乗和を領域Ｒの全画素について平均をとったものである。ステップＳ３０７で当て嵌め誤差が閾値以上と判定された場合、閉領域Ｒに複数の面が含まれる可能性がある。これは、距離画像に含まれるノイズなどの影響により、閉領域の境界に存在するエッジが途切れているためと推定される。
【００４３】
ステップＳ３０９では、途切れたエッジを接続して、複数の面を区分するため、エッジマップに膨張処理を施す。
【００４４】
ステップＳ３１０では、エッジマップをラベリングする。
【００４５】
ステップＳ３１１では、閉領域の候補を抽出する。ステップＳ３１０およびＳ３１１の処理内容は、それぞれステップＳ３０２およびＳ３０３と同様である。
【００４６】
ステップＳ３０７では、当て嵌め誤差が閾値未満と判定された場合、閉領域Ｒには単一の面のみ含まれると推定されるため、ステップＳ３０８に進む。
【００４７】
ステップＳ３０８では、閉領域Ｒを部分領域として登録する。
【００４８】
ステップＳ３１２では、エッジ部の画素など閉領域として未登録の画素を周囲の閉領域に統合する。このステップでも平面あるいは曲面を当て嵌めて、当て嵌め誤差が閾値未満なら統合する。
【００４９】
次に、ステップＳ２０５のモデル当て嵌め工程の詳細を説明する。本工程では、計測対象物体の位置姿勢の概略値を反復演算により繰り返し補正することにより位置姿勢を算出する。
【００５０】
図４はモデル当て嵌め工程の流れを示すフローチャートである。
【００５１】
ステップＳ４０１では、データ入出力部１０９に接続された不図示の距離画像計測装置に対する物体の位置姿勢の概略値を推定する。この処理は、特許文献１に記載のように、物体領域の距離画像データを入力として、物体認識的方法により実現してもよい。ただし、物体の位置姿勢の概略値を求める方法はこれに限るものではない。例えば、モデル当て嵌め工程を一度以上経過していれば、その工程で算出した物体の位置姿勢を表す値をメモリ１０２に記憶しておく。その値を次回のモデル当て嵌め工程において物体の概略位置姿勢の値として使うことが可能である。あるいは、過去の計測値から物体の運動の速度や角速度を線形フィルタやカルマンフィルタなど時系列フィルタリング処理により推定する。推定された速度や角速度によって現在の時刻における物体の位置姿勢を予測したものを用いてもよい。また、他のセンサによる物体の位置姿勢の計測が可能である場合には、該センサによる出力値を位置姿勢の概略値として用いてもよい。センサは、例えばトランスミッタが発する磁界を物体に装着するレシーバで検出することにより位置姿勢を計測する磁気式センサであってもよい。また、物体上に配置されたマーカをシーンに固定されたカメラによって撮影することにより位置姿勢を計測する光学式センサであってもよい。その他、６自由度の位置姿勢を計測するセンサであればいかなるセンサであってもよい。また、物体の置かれているおおよその位置や姿勢があらかじめわかっている場合には、その値を概略値として用いてもよい。
【００５２】
ステップＳ４０２では、対象物体の位置姿勢の概略値を用いて、物体領域内の各画素に対応する３次元空間中の座標を距離画像計測装置の撮像部座標系から物体モデルの座標系に変換する。なお、以下の説明では、本ステップで座標変換の結果、座標を算出した点を「物体領域の３次元点」と呼ぶことにする。
【００５３】
ステップＳ４０３では、陰多項式モデルから構築される距離場と物体領域の３次元点群の各座標に基づいて、３次元点群とモデル表面との距離の総和に対応する、（２）に示すエネルギー関数が最小化されるように位置姿勢を最適化する。
【００５４】
【数２】
【００５５】
ここでΩは物体領域の３次元点群であり、ｘ ∈Ω である。また、ｄｉｓｔは陰多項式モデルから計算される距離で、モデル表面上では０、内部では負の値、外部では正の値をとり、（３）のように定義される。
【００５６】
【数３】
【００５７】
位置姿勢の最適化は具体的には、最急降下法に基づき（４）の式による勾配から３次元点群座標ＸＫ＋１を求める（６）。ここでＸは Ｘ∈ＲＮ×３で、Ｎ個の物体領域の３次元点の座標を表す。
【００５８】
【数４】
【００５９】
【数５】
【００６０】
【数６】
【００６１】
元の３次元点群座標Ｘｋと求めたＸｋ＋１から（７）に示す行列Ａを作成し、Ａ＝ＵＳＶＴのように特異値分解を行い、位置姿勢の概略値を更新する。ここで¬ＸはＸの平均を、ｔは対象物体の位置、Ｒは姿勢を表す行列である。
【００６２】
【数７】
【００６３】
【数８】
【００６４】
【数９】
【００６５】
ステップＳ４０４では、収束判定を行い、収束していればモデル当て嵌め処理全体を終了し、そうでなければステップＳ４０２に戻る。収束判定では、（４）の式の値がほぼ０である場合や、位置姿勢に関する更新前後での誤差ベクトルの二乗和の差がほぼ０である場合に収束したと判定する。なお、上記の説明ではエネルギー最適化処理に最急降下法を用いているが、これに限るものではない。より収束の速いＧａｕｓｓ−Ｎｅｗｔｏｎ法やＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法によって行ってもよい。また、共役勾配法など、他の非線型最適化計算手法を用いてもよい。
【００６６】
ステップＳ２０６の物体領域更新工程の詳細を説明する。図５は物体領域更新工程の流れを示すフローチャートである。
【００６７】
ステップＳ５０１では、物体領域の３次元点群について（１０）で定義される物体の陰多項式モデルとの平均距離を算出する。ただし、ここではＮは物体領域の３次元点の数であり、Φは物体領域の３次元点群である。
【００６８】
【数１０】
【００６９】
ただし、Ｎは算出の対象とする距離画像上の領域内の３次元点数で、
【００７０】
【数１１】
【００７１】
ただし、Φは算出の対象とする距離画像上の領域内の３次元点群である。
【００７２】
ステップＳ５０２では、物体領域に含まれない部分領域の中から、物体領域に追加する部分領域の候補Ｃを選ぶ。具体的には、物体領域及び物体領域に含まれない各部分領域のそれぞれに属する画素群に対応する３次元点群の重心位置を算出する。そして、物体領域に関する重心位置と各部分領域に関する重心位置との距離を算出し、距離が小さい順でｎ個の部分領域を物体領域に追加する部分領域の候補Ｃとして選択する。なお、ｎは予め定められた１以上の正の整数である。
【００７３】
ステップＳ５０３では、候補Ｃの中から未処理の部分領域を１つ選択する。ここで選択した部分領域をａとする。
【００７４】
ステップＳ５０４では、部分領域ａを物体領域に追加するか否か判定するための評価値を算出する。具体的には、部分領域ａに関して、（１０）で表される物体を表す陰多項式モデルと部分領域との平均距離を算出する。ただし、ここではＮは部分領域ａ内の３次元点の数、Φは部分領域ａ内の３次元点群である。
【００７５】
ステップＳ５０５では、次に、ステップＳ５０１で求めた平均距離に所定比率（＞１．０）をかけたものと、ステップＳ５０４で求めた平均距離とを比較する。後者が前者未満であれば、ステップＳ５０６に進む。一方、後者が前者以上と判定されれば、ステップＳ５０７に進む。
【００７６】
ステップＳ５０６では、部分領域ａを物体領域に追加する。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストに、部分領域ａの識別番号を加える。
【００７７】
ステップＳ５０７では、追加する部分領域候補Ｃに含まれる部分領域すべてを処理したか否か判定し、処理済であれば物体領域更新処理を終了する。一方、未処理の部分領域があれば、ステップＳ５０３に戻る。このようにして、すべての追加候補の部分領域に関して物体領域に追加するか否かを判定する処理を繰り返して、物体領域を更新する。
【００７８】
以上、本実施形態に関わる位置姿勢計測装置の処理の流れを説明した。
【００７９】
図６に本手法の方法を適用して、複数の物体が撮影された距離画像から１つの物体の位置姿勢を推定する様子を示す。ただし、物体領域に追加する物体領域の候補の数ｎは１とする。
【００８０】
図６（ａ）は、距離画像を撮影する対象の物体の像であり、同一形状の物体が２つ含まれている。図６（ｂ）は、これらの物体を撮影した距離画像を部分領域に分割した様子である。異なる領域を区別して表示するため、領域ごとに塗りのパターンが異なるように表してある。なお、距離画像は背景領域６１６を有する。図６（ｃ）から図６（ｅ）は物体領域を更新する各ステップを表しており、灰色に表示した部分が物体領域に相当する。図６（ｃ）は初期の物体領域であり、ステップＳ２０４の終了後の状態に相当する。距離画像撮影装置からみて最も手前側の測定点を含む部分領域６２１が物体領域として抽出されている。ステップＳ２０６にて、物体領域の更新を行うと、物体領域は図６（ｄ）に示すようになり、初期の物体領域に部分領域６３２が加わっていることが分かる。同様に２度目の物体領域の更新工程を経ると、図６（ｅ）のように部分領域６４３がさらに加わって、物体領域が３つの部分領域６４１，６４２，６４３によって構成されることになる。
【００８１】
図６（ｅ）の状態から、さらに物体領域の更新工程に進むと、部分領域６４４，６４５を物体領域に加えるか否かを判定する。しかし、部分領域６４４，６４５は部分領域６４１，６４２，６４３が属する物体とは異なる物体上の領域なので、ステップＳ５０４で算出するモデルとの平均距離が比較的大きな値となる。そのため、部分領域６４４，６４５は物体領域に加えないように判定される。
【００８２】
以上の様な処理経過を経て、最終的には部分領域６４１，６４２，６４３を含む物体が距離画像から抽出され、その位置姿勢を求めることが出来る。
【００８３】
本実施形態の方法によれば、位置姿勢推定処理の対象とする距離画像データを対象物体の領域に限定することにより、処理速度を向上させ、メモリ消費量を抑制することができる。
【００８４】
なお、本実施形態は上記に限らず様々な変形例が存在する。
【００８５】
［変形例１−１］
上記の説明ではステップＳ２０６において、物体領域内の３次元点群の重心近傍に重心を持つ部分領域を、物体領域に追加する領域の候補として選択している。しかし、追加する部分領域候補の選択方法はこれに限るものではない。例えば、ステップＳ２０４で設定する初期物体領域内の３次元点群の重心近傍に重心を持つ部分領域を選択してもよい。あるいは、３次元座標の重心ではなく、距離画像上における２次元座標の重心が近い部分領域を選択してもよい。
【００８６】
［変形例１−２］
上記の説明では、物体領域に近接する度合いが高い部分領域を物体領域に追加する候補として選定している。しかし、追加候補の部分領域の選定方法はこれに限らない。例えば、面積すなわち領域に属する画素数が大きい部分領域を候補としてもよい。これは、面積の大きい領域ほど距離画像の計測ノイズなどのランダムな誤差の影響を受けにくいため、形状モデルとの距離の算出や物体領域更新後のモデル当て嵌めを高精度に行うことができるからである。
【００８７】
［変形例１−３］
他の部分領域の候補選定方法としては例えば、距離画像上で対象物体が占める領域の情報を利用するものが挙げられる。具体的には、モデル当て嵌め工程にて算出した対象物体の位置姿勢に基づいて形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして、各部分領域と対象物体が占める領域との重複率（すなわち両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が高い順に、物体領域に追加する部分領域の候補として選定する。
【００８８】
［変形例１−４］
さらには、追加候補の部分領域を選定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３に説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率の３種類の評価値に重みをかけて加算した値に基づいて候補部分領域を選定する。
【００８９】
［変形例２−１］
上記に説明した物体領域の更新工程（図５）では、物体領域と部分領域に関する形状モデルとの平均距離の大小を比較することによって部分領域を物体領域に追加する可否を判定している。しかし、可否の判定方法はこれに限らず、平均距離が小さい部分領域が優先的に追加されれば、他の方法を適用することができる。例えば、追加候補の部分領域群のうち、平均距離が最小の部分領域を物体領域に追加してもよい。
【００９０】
［変形例２−２］
部分領域の追加可否を判定する基準は１つである必然性はなく、複数の基準を組み合わせてもよい。例えば、変形例１−１から変形例１−３に説明した物体領域への近接度、面積、距離画像上の対象物体領域との重複率、及び形状モデルとの平均距離の４種類の評価値に重みをかけて加算した値に基づいて追加可否を判定する。
【００９１】
［変形例２−３］
また、部分領域は１つずつ追加する必然性はなく、複数の部分領域を同時に物体領域に追加してもよい。その場合、追加する部分領域の候補群から、評価値（例えば部分領域の３次元点群とモデルとの平均距離）が上位複数個の部分領域を物体領域に追加すべく選択する。
【００９２】
［変形例３−１］
部分領域の分割方法は上記の説明に限るものでなく、任意の手法を用いることができることは言うまでもない。例えば、所定の評価基準に基づいて距離画像の部分領域を近似するパラメトリックな面の種類（種類ごとに面の数式表現が異なる）を選択する。選択した種類の面で距離画像の表す３次元形状を近似する。例えば、非特許文献８に記載のように、近似させてもよい。また、近似誤差の少ない領域を一続きの面として抽出してもよい。
【００９３】
［変形例３−２］
あるいは、距離データに応じて距離画像を分割するのではなく、所定の分割方法で分割してもよい。例えば、１画素ごとに分割してもよいし、あるいは距離画像の縦横をそれぞれ１０画素ごとに格子状に区切るなどしてもよい。
【００９４】
［変形例４−１］
上記の説明では、初期の物体領域に含まれる部分領域として、最も距離値が小さい画素を含む部分領域を選択していた。しかし、初期物体領域を設定する方法はこれに限るものではなく、最も上方に配置されている物体上の領域である可能性のある部分領域を選択すればよい。例えば、距離値の平均が最小の部分領域を選択してもよいし、距離値が小さい順に上位ｍ個（ｍは２以上の正の整数）の画素の平均距離値が最小の部分領域を選択してもよい。
【００９５】
［変形例４−２］
また、初期の物体領域として距離値が小さい部分領域を優先するのは、物体を上方から撮像し、上方からロボットハンドで把持することを想定しているためである。しかし、その想定が異なれば、想定に適合した初期物体領域の設定方法を採用すればよい。例えば、特定色の物体の位置姿勢を推定するのであれば、距離画像のほかにカラー画像を撮影し、当該の特定色に最も色が合致するカラー画像上の領域に対応する距離画像上の領域を探索し、その距離画像上の領域を含む部分領域を初期物体領域とすればよい。
【００９６】
［変形例４−３］
あるいは、対象物体の概略の位置姿勢が予め分かっていれば、各部分領域について３次元点群の座標と（１０）で表される平均距離を算出し、平均距離が最小の部分領域を初期物体領域としてもよい。
【００９７】
［変形例４−４］
変形４−３と同様に対象物体の概略の位置姿勢が予め分かっていれば、距離画像上で対象物体が占める領域の情報を利用して初期物体領域を決定することができる。具体的には、対象物体の位置姿勢の概略値に基づいて形状モデルを距離画像上に投影して、投影像が存在する領域を対象物体が占める領域とする。そして、各部分領域と対象物体が占める領域との重複率（すなわち両領域が重複する画素の数を各部分領域に属する画素の数で割った値）が最も高い部分領域を初期の物体領域として選ぶ。
【００９８】
［変形例４−５］
また、初期の物体領域に含まれる部分領域の数は１つに限るものではなく、複数でもよい。例えば、距離値の平均が小さい順（変形例４−１の場合）、特定色との合致度が高い順（変形例４−２の場合）、平均距離が小さい順（変形例４−３の場合）、重複率の高い順（変形例４−４の場合）に所定の数の部分領域を物体領域として選択する。
【００９９】
［変形例５］
前述の説明では（１０）で定義される陰多項式モデルと３次元点群との平均距離を、部分領域を追加するか否か判定するなどの用途で評価値として用いている。しかしながら、判定基準とする評価値はこれに限るものではなく、物体のモデルと計測点群との乖離の度合いを示す値であればよい。例えば平均距離に替えて、（１２）、（１３）で表される評価値を用いてもよい。
【０１００】
【数１２】
【０１０１】
ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数で、Φは当該領域内の３次元点群である。
【０１０２】
【数１３】
【０１０３】
ただし、Ｎは評価値の算出対象とする距離画像上の領域内の３次元点の数で、Φは当該領域内の３次元点群である。
【０１０４】
これらの評価値ＤＲＭＳ（１２）、Ｄａｖｅｒａｇｅ（１３）は形状モデルと３次元点群との面の位置の一致度合いを表す。
【０１０５】
［第２の実施形態］
前述の実施形態では、ある部分領域を物体領域に追加する前に、その部分領域が対象物体に相当する領域か否かを判定し、判定結果が真ならば、その部分領域を以後一貫して物体領域の一部としていた。本実施形態では、物体領域に追加済みのある部分領域が非対象物体に相当する部分領域であるか否か判定し、判定結果が真ならば、その部分領域を物体領域から削除する構成を説明する。
【０１０６】
図７は、本実施形態の装置に関する処理全体の流れを示すフローチャートである。
【０１０７】
図７におけるステップＳ７０１からＳ７０４の処理は、それぞれステップＳ２０１からステップＳ２０４と同じ内容である。ステップＳ７０５はステップＳ２０５と同様であるが、物体の位置姿勢を算出した段階で、（１０）で表される計測点と物体モデルとの平均距離を物体領域に関して算出し、メモリ１０２に格納する。この際、前回の物体モデル当て嵌め処理で算出してメモリ１０２に格納した平均距離データを上書きせずに、メモリ１０２内の別の記憶領域にコピーしておく。
【０１０８】
ステップＳ７０６では、物体領域に含まれる部分領域を更新する。本ステップの処理においては、物体領域に部分領域が追加されるか、あるいは追加済みの部分領域が物体領域から除外される。
【０１０９】
ステップＳ７０７では、ステップＳ７０６にて物体領域から部分領域が除外されたか否か判定する。除外されたと判定した場合は、物体の位置姿勢推定処理全体を終了する。ここで、処理を終了するのは、対象物体に相当する部分領域はすべて物体領域として選択済みであり、直近に追加した部分領域が非対象物体に相当すると想定するからである。一方、ステップＳ７０７にて部分領域が除外されていないと判定した場合は、ステップＳ７０５に戻り、物体領域の３次元点群に物体の形状モデルを当て嵌めることによって対象物体の位置姿勢を更新する。
【０１１０】
図１２は、ステップＳ７０６における物体領域更新処理の流れを示すフローチャートである。
【０１１１】
ステップＳ８０１では、物体領域の３次元点群と物体モデルとの平均距離について、直近および前回の物体モデル当て嵌め工程（ステップＳ７０５）で算出した値を比較し、増加していればステップＳ８０２に進む。一方、増加していないと判定すれば、ステップＳ８０３へ進む。
【０１１２】
ステップＳ８０２では、直近に物体領域に追加した部分領域を物体領域から取り除く。すなわち、物体領域に含まれる部分領域群を表す識別番号のリストから、取り除く部分領域の識別番号を削除する。
【０１１３】
ステップＳ８０３では、物体領域に含まれない部分領域の中から選んだ部分領域を物体領域に追加する。この処理は図５のフローチャートに示す流れと同じである。
【０１１４】
以上に説明した処理を行うことにより、物体領域に追加すべく選択した部分領域の妥当性、すなわち当該部分領域が対象物体に相当する度合いを、物体モデルを当て嵌めたときの当て嵌め誤差で確かめることができる。その結果、第１の実施形態のように部分領域ごとの評価値で追加する部分領域を選択する構成に比べ、誤って非対象物体に相当する部分領域を物体領域として選択する危険性が軽減され、より正確に物体領域を決定することが可能である。
【０１１５】
なお、上記の説明では追加済みの部分領域を削除するか否かの判定基準として（１０）で表される平均距離を用いた。しかし、判定基準はこれに限るものではなく、物体モデルの当て嵌め誤差に比例して増加する値であり、かつ物体領域のサイズに依存しないように規格化された値ならば任意の値を用いることができる。例えば、ステップＳ４０３（図４）の処理における位置姿勢更新後の誤差ベクトルの大きさを、物体領域に含まれる画素で平均化した値を基準値としてもよい。
【０１１６】
また、上記の説明では部分領域を物体領域から削除した場合に処理全体を終了しているが、さらに物体領域の更新を継続してもよい。この場合、除外した部分領域が次回の物体領域更新工程で再び物体領域に加えられて無限ループに陥らないように、除外した部分領域は物体領域として追加する候補から除外する。あるいは物体領域に追加する部分領域の候補の中で選択順位を下げる（例えば最下位に設定する）という処理を追加する。
【０１１７】
なお、本実施形態は上記に限らず変形例が存在する。
【０１１８】
［変形例１］
上記の説明では、部分領域を物体領域から除外するか否か判定する基準を平均距離の増減としている。しかし、判定基準はこれに限らず、部分領域を追加する前後でのモデル当て嵌めの精度の変化が評価できればよい。例えば、直近に物体領域に追加した部分領域に関する平均距離と、物体領域に関する平均距離とを比較し、前者が後者以上であれば直近に追加した部分領域を削除すると判定する（逆に、前者が後者未満であれば部分領域を削除しない）。
【０１１９】
［変形例２］
あるいは、物体領域に関する平均距離に替えて、物体領域から直近に追加した部分領域を除外した領域に関する平均距離を用いてもよい。
【０１２０】
［変形例３］
さらに、平均距離に替えて、第１の実施形態の変形例４で説明した評価値を用いて部分領域を除外するか否か判定してもよい。
【０１２１】
［第３の実施形態］
前述の実施形態では、陰多項式で表現した物体の形状モデルを物体領域の３次元点群に当て嵌めることによって、物体の位置姿勢を推定している。しかしながら、位置姿勢の推定方法はこれに限るものではなく、物体の形状モデルおよび物体領域の３次元点群の情報に基づくものであれば、任意の方法を用いることが出来る。
【０１２２】
本実施形態では、形状モデルとして三角形の平面パッチ群から構成されるパッチモデルとし、このモデルと物体領域の３次元点群との間で対応点の対を求める。対応点対間の距離の和が最小になるように物体の位置姿勢を最適化する構成を説明する。
【０１２３】
本実施形態の処理全体の流れは図２のフローチャートと同様である。ただし、ステップＳ２０１で記憶装置１０４から入力する形状モデルデータは、三角形パッチの各頂点の識別番号と座標、各三角形パッチの識別番号とそのパッチを構成する頂点の識別番号から構成されるものとする。
【０１２４】
また、位置姿勢の推定工程（ステップＳ４０３、図４）では、まず物体領域の３次元点群およびモデル上の点の組からなる最近傍点対を求める。求めた最近傍点対はメモリ１０２に最近傍点対のリストとして記憶される。
【０１２５】
具体的には、まず物体領域の３次元点群から最近傍点対の候補となるサンプル点を選択する。サンプル点は、距離画像上の物体領域内の１画素ごとに選択してもよいし、所定数の画素（例えば５画素）ごとに画像領域を間引きして選択してもよい。次に、サンプル点¬ｐに対応する形状モデル上の最近傍点を求める。サンプル点¬ｐと３頂点¬ｒ１，¬ｒ２，¬ｒ３からなる三角形パッチｔとの間の最小距離ｄは（１４）のように表される。
【０１２６】
【数１４】
【０１２７】
ただし、ｕ∈［０，１］，ｖ∈［０，１］，ｗ∈［０，１］である。形状モデルＴがＮ個の三角形パッチｔ１，…，ｔＮから構成されるとすると、サンプル点¬ｐと形状モデルＴ上の最近傍点との間の距離は（１５）のように表される。
【０１２８】
【数１５】
【０１２９】
すなわちサンプル点の最近傍点を求めるには、サンプル点と各三角形パッチとの最小距離および最近傍点の座標を表す係数の組（ｕ，ｖ）を算出する。すべての三角形パッチの中でサンプル点との最小距離が最小となる三角形パッチと係数の組（ｕ，ｖ）を探索する。以上に説明した最近傍点対の探索処理の結果は、サンプル点の３次元座標、形状モデル上の最近傍点が存在する三角形パッチの識別番号、最近傍点の座標を表す係数（ｕ，ｖ）、の組を全てのサンプル点に対して羅列したリストとしてメモリ１０２に記録される。
【０１３０】
次に、最近傍点対リストに登録された最近傍点対間の距離の総和が最小になるようにするモデルの位置姿勢を算出し、結果を対象物体の位置姿勢とする。距離画像計測装置の撮像部の座標系における３次元座標は、対象物体の位置姿勢ｓによって、対象物体の座標系における３次元座標（ｘ，ｙ，ｚ）に変換することが出来る。ここで、計測対象物体の位置姿勢の自由度は６自由度である。すなわちｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素からなる。いま、ステップＳ４０１で算出した対象物体の概略の位置姿勢により、撮像部座標系のある点が対象物体座標系の点（ｘ０，ｙ０，ｚ０）に変換されるとする。（ｘ，ｙ，ｚ）は対象物体の位置姿勢により変化するものであり、（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって（１６）のように近似できる。
【０１３１】
【数１６】
【０１３２】
メモリ１０２に格納されたある最近傍点対の形状モデル側最近傍点が存在する三角形パッチ面の対象物体座標系における方程式をａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１，ａ，ｂ，ｃ，ｅは定数）とする。正しいｓによって変換される（ｘ，ｙ，ｚ）は、平面の方程式ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１）を満たすと仮定する。（１６）を平面の方程式に代入すると（１７）が得られる。
【０１３３】
【数１７】
【０１３４】
ただし、ｑ＝ａｘ０＋ｂｙ０＋ｃｚ０（定数）とした。
【０１３５】
（１７）は最近傍点対すべてについて成り立つため、（１８）のようなΔｓｉに関する線形連立方程式が成り立つ。
【０１３６】
【数１８】
【０１３７】
ここで、（１８）を（１９）のように表す。
【０１３８】
【数１９】
【０１３９】
（１９）をもとに、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いてΔｓを求める。しかし、距離画像の計測点群に含まれるノイズなどによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値である計測点群データでは誤差ｅ−ｑが大きくなる。そのため、（１８）、（１９）の連立方程式に対する寄与度が大きくなり、その結果得られるΔｓの精度が低下してしまう。そこで、誤差ｅ−ｑが大きいデータには小さな重みを与え、誤差ｅ−ｑが小さいデータには大きな重みを与える。重みは例えば（２０）に示すようなＴｕｋｅｙの関数により与える。
【０１４０】
【数２０】
【０１４１】
ｃは定数である。なお、重みを与える関数はＴｕｋｅｙの関数である必要はなく、例えばＨｕｂｅｒの関数など、誤差の大きいデータには小さな重みを与え、誤差の小さいデータには大きい重みを与える関数であればなんでもよい。
【０１４２】
各データに対応する重みをｗｉとする。ここで（２１）のように重み行列Ｗを定義する。
【０１４３】
【数２１】
【０１４４】
重み行列Ｗは、対角成分以外はすべて０の正方行列であり、対角成分には重みｗｉが入る。ＮＣは最近傍点対の数である。
【０１４５】
この重み行列Ｗを用いて、（１９）を（２２）のように変形する。
【０１４６】
【数２２】
【０１４７】
（２３）のように（２２）を解くことにより補正値Δｓを求める。
【０１４８】
【数２３】
【０１４９】
このようにして算出した補正値Δｓによりモデルの位置・姿勢の概略値を補正し、結果をメモリ１０２に格納する。
【０１５０】
なお、上述した位置姿勢の最適化計算手法はＧａｕｓｓ−Ｎｅｗｔｏｎ法に基づくものであるが、これに限らずＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法や最急降下法などを用いてもよい。
【０１５１】
ステップＳ４０４では、収束判定を行い、収束していれば位置姿勢の推定処理を終了し、そうでなければステップＳ４０２に戻る。収束判定では、補正値Δｓがほぼ０である場合や、誤差ベクトルＥの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。
【０１５２】
物体領域の更新処理は第１の実施形態と同様であるが、部分領域を追加するか否かを判定するための評価値は（１０）に表される平均距離ではなく、ＲＭＳ誤差（誤差ベクトルの要素の二乗和をサンプル点数で除算した値の平方根）を用いる。物体領域に関する誤差ベクトルはステップＳ４０３で算出済みである。物体領域に追加する候補の部分領域に関する誤差ベクトルは、上述の物体領域の場合と同様に算出する。
【０１５３】
［その他の実施形態］
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
【産業上の利用可能性】
【０１５４】
本発明は、ロボットと物体との間の相対的な位置姿勢を推定するのに利用することができる。
【特許請求の範囲】
【請求項１】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測方法であって、
分割手段が、前記距離画像の領域を部分領域に分割する分割工程と、
設定手段が、前記分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定工程と、
推定手段が、前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定工程と、
更新手段が、前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新工程と
を有することを特徴とする位置姿勢計測方法。
【請求項２】
取得手段が、前記距離画像を計測して取得する取得工程を更に有することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
前記分割工程では、距離画像をエッジによって囲まれた閉領域ごとに分割することを特徴とする請求項１又は２に記載の位置姿勢計測方法。
【請求項４】
入力手段が、物体の位置姿勢の概略値を入力する入力工程を更に有し、
前記推定工程では、前記概略値を演算により繰り返し補正することを特徴とする請求項１乃至３の何れか１項に記載の位置姿勢計測方法。
【請求項５】
前記設定工程では、前記距離画像の最も上方にある部分領域を前記物体領域として設定することを特徴とする請求項１乃至４の何れか１項に記載の位置姿勢計測方法。
【請求項６】
前記設定工程では、前記分割した複数の部分領域のうち最も距離の近い部分領域を前記物体領域として設定することを特徴とする請求項１乃至５の何れか１項に記載の位置姿勢計測方法。
【請求項７】
前記更新工程では、前記当て嵌めの誤差に基づいて、前記物体領域に含まれていない部分領域を該物体領域に追加することを特徴とする請求項１乃至６の何れか１項に記載の位置姿勢計測方法。
【請求項８】
前記更新工程では、前記当て嵌めの誤差に基づいて、前記物体領域に含まれている部分領域を該物体領域から削除することを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測方法。
【請求項９】
前記更新工程では、前記推定工程の前後における前記当て嵌めの誤差が予め定められたの閾値よりも大きい場合に、前記物体領域の直近に追加した部分領域を該物体領域から削除する
ことを特徴とする請求項８に記載の位置姿勢計測方法。
【請求項１０】
前記形状モデルは陰多項式により表現されることを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測方法。
【請求項１１】
請求項１乃至１０の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させることを特徴とするプログラム。
【請求項１２】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測装置であって、
前記距離画像の領域を部分領域に分割する分割手段と、
前記距離画像の領域を分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定手段と、
前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定手段と、
前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新手段と、
を備えることを特徴とする位置姿勢計測装置。
【請求項１】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測方法であって、
分割手段が、前記距離画像の領域を部分領域に分割する分割工程と、
設定手段が、前記分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定工程と、
推定手段が、前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定工程と、
更新手段が、前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新工程と
を有することを特徴とする位置姿勢計測方法。
【請求項２】
取得手段が、前記距離画像を計測して取得する取得工程を更に有することを特徴とする請求項１に記載の位置姿勢計測方法。
【請求項３】
前記分割工程では、距離画像をエッジによって囲まれた閉領域ごとに分割することを特徴とする請求項１又は２に記載の位置姿勢計測方法。
【請求項４】
入力手段が、物体の位置姿勢の概略値を入力する入力工程を更に有し、
前記推定工程では、前記概略値を演算により繰り返し補正することを特徴とする請求項１乃至３の何れか１項に記載の位置姿勢計測方法。
【請求項５】
前記設定工程では、前記距離画像の最も上方にある部分領域を前記物体領域として設定することを特徴とする請求項１乃至４の何れか１項に記載の位置姿勢計測方法。
【請求項６】
前記設定工程では、前記分割した複数の部分領域のうち最も距離の近い部分領域を前記物体領域として設定することを特徴とする請求項１乃至５の何れか１項に記載の位置姿勢計測方法。
【請求項７】
前記更新工程では、前記当て嵌めの誤差に基づいて、前記物体領域に含まれていない部分領域を該物体領域に追加することを特徴とする請求項１乃至６の何れか１項に記載の位置姿勢計測方法。
【請求項８】
前記更新工程では、前記当て嵌めの誤差に基づいて、前記物体領域に含まれている部分領域を該物体領域から削除することを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測方法。
【請求項９】
前記更新工程では、前記推定工程の前後における前記当て嵌めの誤差が予め定められたの閾値よりも大きい場合に、前記物体領域の直近に追加した部分領域を該物体領域から削除する
ことを特徴とする請求項８に記載の位置姿勢計測方法。
【請求項１０】
前記形状モデルは陰多項式により表現されることを特徴とする請求項１乃至７の何れか１項に記載の位置姿勢計測方法。
【請求項１１】
請求項１乃至１０の何れか１項に記載の位置姿勢計測方法をコンピュータに実行させることを特徴とするプログラム。
【請求項１２】
物体を撮影した距離画像上で前記物体に相当する物体領域に基づいて、該物体の位置姿勢を計測する位置姿勢計測装置であって、
前記距離画像の領域を部分領域に分割する分割手段と、
前記距離画像の領域を分割した複数の部分領域のうち少なくとも１つの部分領域を前記物体領域として設定する設定手段と、
前記物体領域に前記物体の形状モデルを当て嵌めて、前記物体の位置姿勢を推定する推定手段と、
前記当て嵌めの誤差に基づいて、前記物体領域を更新する更新手段と、
を備えることを特徴とする位置姿勢計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−１７４８７８（Ｐ２０１１−１７４８７８Ａ）
【公開日】平成２３年９月８日（２０１１．９．８）
【国際特許分類】
【出願番号】特願２０１０−４０５９３（Ｐ２０１０−４０５９３）
【出願日】平成２２年２月２５日（２０１０．２．２５）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２３年９月８日（２０１１．９．８）
【国際特許分類】
【出願日】平成２２年２月２５日（２０１０．２．２５）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]