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

Abstract

【課題】対象物体の位置及び姿勢を推定する際の計算コストを低減させる位置姿勢推定装置、位置姿勢推定方法を提供する。【解決手段】対象物体検出部が、検出された点群データから、対象物体を表す点群データを検出するＳ１００。パターン群生成部が、対象物体を表す点群データの位置で、モデルデータの位置及び姿勢の各々を変化させたパターン群を生成するＳ１０８。画像変換部が、検出された点群データと、パターン群生成部によって生成されたパターン群の各パターンの点群データとを、距離画像へ変換するＳ１１０。画像照合部が、検出された点群データの距離画像と、パターン群の各パターンの点群データの距離画像の各々とを照合するＳ１１２。位置姿勢推定部が、画像照合部によって得られた照合結果に基づいて、パターンの点群データの距離画像を選択し、選択された距離画像に対応するパターンに基づいて対象物体の位置及び姿勢を推定するＳ１１４。【選択図】図１１

Description

開示の技術は、位置姿勢推定装置、位置姿勢推定方法、及び位置姿勢推定プログラムに関する。

対象物の存在を判定する対象物画像判定装置が知られている。この対象物画像判定装置では、監視画像にて対象物が設置物により隠蔽されずに投影される推定投影領域を、空間内の各位置に対象物モデルを配置して求め、当該位置と推定投影領域とを対応付けた推定投影領域データを予め記憶部に保存する。そして、対象物画像判定装置は、追跡処理では推定投影領域データを参照し、注目対象物の予測位置での推定投影領域を取得する。そして、対象物画像判定装置は、推定投影領域の画像特徴から対象物の像の存在を判定する。

また、対象物の追跡を行う画像処理装置が知られている。この画像処理装置は、追跡開始が判定されたら当該画像フレームのエッジ画像を生成する。そして、画像処理装置は、追跡対象の形状を表すＢスプライン曲線の制御点列を、あらかじめ準備した複数の基準形状を表すＢスプライン曲線の制御点列の線形和で表現する際に各制御点列にかかる係数のセットの空間にパーティクルを分布させる。画像処理装置は、さらに形状空間ベクトルの空間においてもパーティクルを分布させ、各パーティクルの尤度観測、確率密度分布を取得する。そして、画像処理装置は、確率密度分布によって各パラメータに重み付け平均して得られる曲線を追跡結果として生成する。

特開２０１２−１５５５９５号公報 国際公開第２０１０／０７３４３２号

近年では、対象物体の位置及び姿勢を推定する際に３次元の点群データが用いられることが多い。しかし、３次元点群データを用いて対象物体の位置及び姿勢を推定する場合には、計算コストが増加してしまう。

一つの側面では、開示の技術は、３次元の点群データを用いて対象物体の位置及び姿勢を推定する場合に、計算コストを低減させることが目的である。

一つの実施態様では、３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出する。そして、検出された前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成する。そして、検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換する。そして、検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合する。そして、照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する。

一つの側面として、３次元の点群データを用いて対象物体の位置及び姿勢を推定する場合に、計算コストを低減させることができる、という効果を有する。

本実施形態に係る位置姿勢推定装置の概略ブロック図である。 ３次元の点群データの一例を示す図である。 点群データが格納されたテーブルの一例を示す図である。 モデルデータが格納されたテーブルの一例を示す図である。 ３次元の点群データであるモデルデータの一例を示す図である。 点群データＸから検出された対象物体ｃの点群データＣを説明するための説明図である。 モデルデータをランダムに変化させたパターン群を説明するための説明図である。 距離画像の一例を示す図である。 センサによって検出された点群データの距離画像とパターン群の各パターンの距離画像との照合を説明するための説明図である。 本実施形態に係る位置姿勢推定装置の制御部として機能するコンピュータの概略構成を示すブロック図である。 本実施形態における位置姿勢推定処理の一例を示すフローチャートである。

以下、図面を参照して開示の技術の実施形態の一例を詳細に説明する。

[実施形態]
図１に示す位置姿勢推定装置１０は、センサ１２と、制御部１４とを備えている。本実施形態では、位置姿勢推定装置１０がパーティクルフィルタを用いて対象物体の位置及び姿勢を推定する場合を例に説明する。また、本実施形態の位置姿勢推定装置１０は、対象物体の位置及び姿勢に基づいて、対象物体の追跡を行う。

センサ１２は、検出対象範囲における、物体の表面上の各点の３次元位置を示す点群データを検出する。センサ１２は、例えば、３次元点群データを検出可能なレーザレーダ、モーションセンサーデバイス等によって実現される。センサ１２は、開示の技術の検出部の一例である。

制御部１４は、センサ１２によって検出された３次元の点群データに含まれる対象物体の位置及び姿勢を推定する。そして、制御部１４は、対象物体を追跡する。制御部１４は、図１に示すように、情報取得部１６と、点群データ記憶部１７と、モデルデータ記憶部１８と、対象物体検出部２０と、パターン群生成部２２と、画像変換部２４と、画像照合部２６と、位置姿勢推定部２８とを備える。

情報取得部１６は、センサ１２によって検出された、３次元の点群データを取得する。図２に、３次元の点群データの一例を示す。図２に示す例では、物体ａの点群データＡ、物体ｂの点群データＢ、及び物体ｃの点群データＣを含む点群データＸが検出されている。なお、点群データＸの各点には３次元位置が付与されている。

点群データ記憶部１７には、情報取得部１６によって取得された点群データＸが時刻と対応付けられて格納される。点群データは各時刻において検出される。点群データＸは、例えば、図３に示すようにテーブルの形式で格納される。例えば、図３に示す点群データ「(q1,r1,s1),(q2,r2,s2),…,(qx,rx,sx)」は、「(q1,r1,s1)」、「(q2,r2,s2)」、・・・、「(qx,rx,sx)」の各点が含まれることを表している。また、「(q1,r1,s1)」、「(q2,r2,s2)」、・・・、「(qx,rx,sx)」の各々は、各点の３次元位置座標を示している。

モデルデータ記憶部１８には、予め用意された、検出したい対象物体を表す３次元の点群データであるモデルデータが格納されている。モデルデータは、例えば、図４に示すような形式で格納される。図４に示す例では、モデルデータ「(Q1,R1,S1),(Q2,R2,S2),…,(Qx,Rx,Sx)」は、各点の３次元位置座標を示している。モデルデータは、検出したい対象物体に応じて予め用意される。例えば、対象物体として人を設定する場合には、図５に示すように、人物モデルＵを予め用意することができる。本実施形態では、モデルデータの一例として、人物モデルＵを用いる場合を例に説明する。

対象物体検出部２０は、点群データ記憶部１７に格納された点群データの各々から最新の点群データを取得する。以下では、対象物体検出部２０は、点群データ記憶部１７に格納された点群データから、最新の点群データとして上記図２に示した点群データＸが取得される場合を例に説明する。

対象物体検出部２０は、取得した点群データＸから、対象物体を表す点群データを検出する。例えば、対象物体として人を検出する場合、対象物体検出部２０は、取得した点群データＸとモデルデータ記憶部１８に格納されたモデルデータの一例である人物モデルＵとに基づいて、点群データＸから、対象物体ｃを表す点群データＣを検出する。対象物体検出部２０は、パーティクルフィルタにおける観測処理として、対象物体の検出処理を行う。

例えば、対象物体検出部２０は、点群データＸとモデルデータ記憶部１８に格納されたモデルデータである人物モデルＵとから、Signature of Histograms of Orientations（ＳＨＯＴ）特徴量を抽出し、３次元特徴マッチングを行う。

ＳＨＯＴ特徴量を用いて３次元特徴マッチングを行う場合には、モデルデータ内の特徴的な部分が予め設定され、特徴的な部分の位置と当該部分における特徴量とがキーポイントとして予め設定される。この場合、対象物体検出部２０は、人物モデルＵのキーポイントにおける特徴量と、点群データＸのキーポイントにおける特徴量との間で、３次元特徴マッチングを行う。そして、対象物体検出部２０は、人物モデルＵのキーポイントと点群データＸのキーポイントとの間の３次元特徴マッチングの結果に基づいて、点群データＸのうち人物モデルＵに対応する点群データＣを特定する。

図６に、人物モデルＵと点群データＸとの間の３次元特徴マッチング結果の例を示す。図６に示すように、３次元マッチングを行うことにより、上記図２に示す点群データＸのうち、人物モデルＵに対応する点群データＣが対象物体ｃとして検出される。例えば、対象物体検出部２０は、３次元特徴マッチングによって得られる値が、予め定められた閾値以上である点群データの領域を点群データＣとして検出する。

次に、対象物体検出部２０は、点群データＣと人物モデルＵとを一致させる。点群データ同士を一致させる手法としては、例えばIterative Closest Point（ＩＣＰ）を用いることができる。

そして、対象物体検出部２０は、点群データＣと人物モデルＵとの照合結果に応じて、点群データＣに人物モデルＵを一致させたときの、人物モデルＵの位置及び姿勢を検出する。これにより、点群データＣに対応する人物モデルＵの位置及び姿勢が検出される。

パターン群生成部２２は、対象物体検出部２０によって検出された対象物体ｃを表す点群データＣの位置で、点群データＣに対応する人物モデルＵの位置及び姿勢の各々をランダムに変化させた複数のパターンを含むパターン群を生成する。パターン群に含まれる複数のパターンは、パーティクルフィルタにおける複数のパーティクルとして生成される。

図７に、パターン群の一例を示す。パターン群生成部２２は、例えば図７に示すように、対象物体ｃを表す点群データＣに一致させた人物モデルＵの重心Ｐを起点として、人物モデルＵの位置及び姿勢の各々をランダムに変化させた複数のパターンを生成する。

画像変換部２４は、対象物体検出部２０によって検出された対象物体ｃの点群データＣと、パターン群生成部２２によって生成されたパターン群の各パターンの点群データとを、距離画像へ変換する。距離画像としては、例えば、図８に示すような、センサ１２から各画素に対応する点までの距離が遠いほど画素値の値が大きく、センサ１２からの距離が近いほど画素値の値が小さくなるような画像が生成される。

例えば、画像変換部２４は、以下の式（１）に従って、点群データに含まれる各点を距離画像の各画素へ変換する。

（１）

ここで、上記式（１）の（Ｘ，Ｙ，Ｚ）は、点群データに含まれる各点の３次元位置を表す。また、（ｘ’，ｙ’，ｚ’）の（ｘ’，ｙ’）は、距離画像における２次元位置を表し、ｚ’は当該２次元位置における画素値を表す。また、上記式（１）における以下の行列の各要素は内部パラメータを表す。内部パラメータは、予め設定される。

上記式（１）を演算すると、以下の式（２）が導かれる。点群データの各点の３次元位置（Ｘ，Ｙ，Ｚ）は、以下の式（２）に従って、距離画像における２次元位置（ｘ’，ｙ’）へ変換され、かつ距離画像における２次元位置（ｘ’，ｙ’）の画素値はｚ’（＝Ｚ）が格納される。

（２）

画像照合部２６は、画像変換部２４によって得られた、点群データＣの距離画像とパターン群の各パターンの点群データの距離画像の各々とを照合する。本実施形態では、点群データＣの距離画像と、パターン群の各パターンの点群データの距離画像の各々とを背景差分によって照合する場合を例に説明する。

具体的には、画像照合部２６は、図９に示すように、点群データＣの距離画像Ｉｃとパターン群の各パターンの点群データの距離画像Ｉｐの各々とを背景差分によって照合し、背景差分値の逆数をパーティクルフィルタにおける尤度とする。なお、背景差分とは、距離画像Ｉｃの各位置における画素値と距離画像Ｉｐの対応する各位置における画素値との差分を求め、画像全体における差分の総和を背景差分値として算出する方法である。

位置姿勢推定部２８は、画像照合部２６によって得られた照合結果の一例である尤度に応じて、尤度が予め定められた閾値よりも大きい距離画像Ｉｐｓを選択する。位置姿勢推定部２８の距離画像の選択は、パーティクルフィルタにおけるリサンプリング処理として行われる。そして、位置姿勢推定部２８は、パーティクルフィルタにおける予測処理として、選択された距離画像Ｉｐｓのパターンに基づいて、対象物体ｃの位置及び姿勢を推定する。例えば、位置姿勢推定部２８は、選択された複数の距離画像Ｉｐｓに対応するパターンの各々が示す位置及び姿勢の平均を、対象物体ｃの位置及び姿勢を表す値として推定する。

そして、位置姿勢推定部２８は、推定された対象物体ｃの位置及び姿勢を、人物モデルＵに反映する。例えば、位置姿勢推定部２８は、選択された距離画像Ｉｐｓに対応する各パターンの各々が示す位置及び姿勢の平均が、人物モデルＵの位置及び姿勢となるように反映する。

対象物体検出部２０は、点群データ記憶部１７から次時刻の点群データを取得し、取得した点群データから、位置姿勢推定部２８によって対象物体ｃの位置及び姿勢が反映された人物モデルＵに基づいて、点群データ内の対象物体ｃを検出する。

制御部１４における、対象物体検出部２０、パターン群生成部２２、画像変換部２４、画像照合部２６、及び位置姿勢推定部２８の各処理が繰り返されることにより、対象物体ｃの追跡処理が行われる。

位置姿勢推定装置１０の制御部１４は、例えば、図１０に示すコンピュータ５０で実現することができる。コンピュータ５０はＣＰＵ５１、一時記憶領域としてのメモリ５２、及び不揮発性の記憶部５３を備える。また、コンピュータ５０は、センサ１２や表示装置及び入力装置等の入出力装置（図示省略）が接続される入出力interface（Ｉ／Ｆ）５４、及び記録媒体５９に対するデータの読み込み及び書き込みを制御するread/write（Ｒ／Ｗ）部５５を備える。また、コンピュータ５０は、インターネット等のネットワークに接続されるネットワークＩ／Ｆ５６を備える。ＣＰＵ５１、メモリ５２、記憶部５３、入出力Ｉ／Ｆ５４、Ｒ／Ｗ部５５、及びネットワークＩ／Ｆ５６は、バス５７を介して互いに接続される。

記憶部５３は、Hard Disk Drive（ＨＤＤ）、solid state drive（ＳＳＤ）、フラッシュメモリ等によって実現できる。記憶媒体としての記憶部５３には、コンピュータ５０を位置姿勢推定装置１０の制御部１４として機能させるための位置姿勢推定プログラム６０が記憶されている。位置姿勢推定プログラム６０は、情報取得プロセス６２と、対象物体検出プロセス６３と、パターン群生成プロセス６４と、画像変換プロセス６５と、画像照合プロセス６６と、位置姿勢推定プロセス６７とを有する。また、記憶部５３は、点群データ記憶部１７を構成する情報が記憶される点群データ記憶領域６８を有する。また、記憶部５３は、モデルデータ記憶部１８を構成する情報が記憶されるモデルデータ記憶領域６９を有する。

ＣＰＵ５１は、位置姿勢推定プログラム６０を記憶部５３から読み出してメモリ５２に展開し、位置姿勢推定プログラム６０が有するプロセスを順次実行する。ＣＰＵ５１は、情報取得プロセス６２を実行することで、図１に示す情報取得部１６として動作する。また、ＣＰＵ５１は、対象物体検出プロセス６３を実行することで、図１に示す対象物体検出部２０として動作する。また、ＣＰＵ５１は、パターン群生成プロセス６４を実行することで、図１に示すパターン群生成部２２として動作する。また、ＣＰＵ５１は、画像変換プロセス６５を実行することで、図１に示す画像変換部２４として動作する。また、ＣＰＵ５１は、画像照合プロセス６６を実行することで、図１に示す画像照合部２６として動作する。また、ＣＰＵ５１は、位置姿勢推定プロセス６７を実行することで、図１に示す位置姿勢推定部２８として動作する。また、ＣＰＵ５１は、点群データ記憶領域６８から情報を読み出して、点群データ記憶部１７をメモリ５２に展開する。また、ＣＰＵ５１は、モデルデータ記憶領域６９から情報を読み出して、モデルデータ記憶部１８をメモリ５２に展開する。これにより、位置姿勢推定プログラム６０を実行したコンピュータ５０が、位置姿勢推定装置１０の制御部１４として機能することになる。そのため、ソフトウェアである位置姿勢推定プログラム６０を実行するプロセッサはハードウェアである。

なお、位置姿勢推定プログラム６０により実現される機能は、例えば半導体集積回路、より詳しくはApplication Specific Integrated Circuit（ＡＳＩＣ）等で実現することも可能である。

次に、本実施形態に係る位置姿勢推定装置１０の作用について説明する。位置姿勢推定装置１０において、センサ１２が３次元点群データを逐次検出し、情報取得部１６はセンサ１２によって検出された３次元点群データを取得する。そして、情報取得部１６は取得した点群データを点群データ記憶部１７に時刻と対応付けて格納する。点群データ記憶部１７に点群データが格納されると、位置姿勢推定装置１０の制御部１４によって、図１１に示す位置姿勢推定処理が実行される。以下、各処理について詳述する。

ステップＳ１００において、対象物体検出部２０は、点群データ記憶部１７に格納された点群データの各々から最新の点群データと、点群データに対応付けられた時刻とを取得する。

ステップＳ１０２において、対象物体検出部２０は、上記ステップＳ１００で取得された点群データが、初期フレームに対応するデータであるか否かを判定する。例えば、対象物体検出部２０は、センサ１２における点群データの検出の開始時刻を取得し、上記ステップＳ１００で取得された点群データに対応付けられた時刻と、検出の開始時刻とが一致する場合には、初期フレームに対応するデータであると判定する。上記ステップＳ１００で取得された点群データが、初期フレームに対応するデータである場合には、ステップＳ１０４へ進む。一方、上記ステップＳ１００で取得された点群データが、初期フレームに対応するデータでない場合には、ステップＳ１０７へ進む。

ステップＳ１０４において、対象物体検出部２０は、上記ステップＳ１００で取得された点群データと、モデルデータ記憶部１８に格納されたモデルデータとから特徴量を抽出し、３次元特徴マッチングを行う。なお、モデルデータ記憶部１８に格納されたモデルデータは、前回のステップＳ１１６での処理によって、対象物体の位置及び姿勢が反映されている。

ステップＳ１０６において、対象物体検出部２０は、上記ステップＳ１０４で得られた３次元特徴マッチング結果に基づいて、モデルデータに対応する対象物体が存在するか否かを判定する。モデルデータに対応する対象物体が存在する場合には、ステップＳ１０７へ進む。一方、モデルデータに対応する対象物体が存在しない場合には、ステップＳ１００へ戻る。例えば、対象物体検出部２０は、上記ステップＳ１０４の３次元特徴マッチングによって得られる値が、予め定められた閾値以上である場合には、対象物体が存在すると判定する。

ステップＳ１０７において、対象物体検出部２０は、上記ステップＳ１０４で得られた３次元特徴マッチングの値が、予め定められた閾値以上である領域の点群データαを対象物体として検出する。また、対象物体検出部２０は、点群データαとモデルデータとの照合結果に応じて、点群データαが表す対象物体の位置及び姿勢を検出する。そして、対象物体検出部２０は、点群データαとモデルデータとを一致させる。

ステップＳ１０８において、パターン群生成部２２は、上記ステップＳ１０７で検出された対象物体を表す点群データαの位置で、点群データαに対応するモデルデータの位置及び姿勢の各々をランダムに変化させた複数のパターンを含むパターン群を生成する。

ステップＳ１１０において、画像変換部２４は、上記ステップＳ１０７で検出された対象物体の点群データαと、上記ステップＳ１０８で生成されたパターン群の各パターンの点群データとを、距離画像へ変換する。

ステップＳ１１２において、画像照合部２６は、上記ステップＳ１１０で得られた、点群データαの距離画像とパターン群の各パターンの点群データの距離画像の各々とを、背景差分によって照合する。そして、画像照合部２６は、点群データαの距離画像とパターン群の各パターンの点群データの距離画像の各々についての背景差分値の逆数を尤度とする。

ステップＳ１１４において、位置姿勢推定部２８は、上記ステップＳ１１２で得られた尤度に応じて、尤度が予め定められた閾値よりも大きい距離画像を選択する。そして、位置姿勢推定部２８は、選択された複数の距離画像に対応するパターンの各々が示す位置及び姿勢の平均を、対象物体の位置及び姿勢を表す値として推定する。

ステップＳ１１６において、位置姿勢推定部２８は、上記ステップＳ１１４で推定された、対象物体の位置及び姿勢を、モデルデータ記憶部１８に格納されたモデルデータへ反映する。

ステップＳ１１８において、位置姿勢推定部２８は、対象物体の追跡を終了するか否かを判定する。対象物体の追跡を終了する場合には、位置姿勢推定処理を終了する。対象物体の追跡を終了しない場合には、ステップＳ１００へ戻る。対象物体の追跡を終了するか否かは、例えば、ユーザから入力される情報に応じて決定することができる。

以上説明したように、本実施形態に係る位置姿勢推定装置は、検出された点群データから、対象物体を表す点群データを検出する。そして、位置姿勢推定装置は、検出された対象物体を表す点群データの位置で、位置及び姿勢の各々をランダムに変化させた、対象物体を表す点群データであるモデルデータのパターンを含むパターン群を生成する。そして、位置姿勢推定装置は、検出された点群データと、パターン群の各パターンの点群データとを、距離画像へ変換する。そして、位置姿勢推定装置は、検出された点群データの距離画像と、パターン群の各パターンの点群データの距離画像の各々とを照合する。そして、位置姿勢推定装置は、照合結果に基づいて、パターンの点群データの距離画像を選択し、選択された距離画像に対応するパターンに基づいて対象物体の位置及び姿勢を推定する。これにより、３次元の点群データを用いて対象物体の位置及び姿勢を推定する場合に、３次元の点群データ同士でモデルデータと照合する場合に比べ、計算コストを低減させることができる。

また、パーティクルフィルタにおける尤度計算を３次元の点群データの処理ではなく、距離画像を用いて尤度を計算することにより、計算コストを低減させることができる。そのため、計算量が低減され、処理の高速化が可能となる。具体的には、３次元の情報である３次元の点群データが２次元の情報である距離画像へ変換され、パーティクルフィルタのリサンプリング処理における尤度計算（又は照合処理）が２次元の情報で行われるため、計算コストが低減される。また、３次元の情報が２次元情報へ変換されて扱われるため、パーティクル数が増加した場合であっても処理の高速化が可能となる。

計算コストの概算について以下説明する。

３次元のパーティクルフィルタにおける計算コストは、以下の式（３）に示すように計算される。

（モデルデータに対応する点群データに含まれる点の数［個］×パーティクル数［個］×log（検出された点群データに含まれる点の数［個］））
（３）

上記算出式（３）の各項に、仮の値を代入して計算コストを算出すると、以下のようになる。

10,000［個］×100［個］×log5,000［個］（3.69897）＝3,698,970

一方、本実施形態における計算コストは、以下の式（４）に示すように計算される。

（距離画像のウィンドウ縦幅［pixel］×距離画像のウィンドウ横幅［pixel］×パーティクル数［個］）
（４）

100［pixel］×80［pixel］×100［個］=800,000

上記に示すように、本実施形態の計算コストは、３次元のパーティクルフィルタにおける計算コストの１／４．６となり、計算コストを低減させることができる。

なお、上記では、位置姿勢推定プログラムが記憶部に予め記憶（インストール）されている態様を説明したが、これに限定されない。開示の技術に係るプログラムは、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ＵＳＢメモリ等の記録媒体に記録された形態で提供することも可能である。

本明細書に記載された全ての文献、特許出願及び技術規格は、個々の文献、特許出願及び技術規格が参照により取り込まれることが具体的かつ個々に記された場合と同程度に、本明細書中に参照により取り込まれる。

次に、上記実施形態の変形例を説明する。

上記実施形態では、位置姿勢推定部２８は、画像照合部２６によって得られた尤度に応じて、尤度が予め定められた閾値よりも大きい距離画像を選択する場合を例に説明したが、これに限定されるものではない。例えば、画像照合部２６によって得られた尤度に応じて、尤度が最も大きい距離画像を選択してもよい。この場合には、位置姿勢推定部２８は、尤度が最も大きい距離画像のパターンを、対象物体の位置及び姿勢を表す値として推定する。

また、上記実施形態では、画像照合の一例として背景差分を用いる場合を例に説明したがこれに限定されるものではなく、他の画像照合方法を用いてもよい。例えば、各距離画像から特徴量を抽出し、各特徴量の一致の度合いに応じて、距離画像同士の照合を行っても良い。

また、上記実施形態では、対象物体の位置及び姿勢の推定方法の一例としてパーティクルフィルタを用いる場合を例に説明したが、これに限定されるものではない。例えば、検出された点群データと、モデルデータの点群データを距離画像へ変換して、カルマンフィルタによって対象物体の位置及び姿勢を推定するようにしてもよい。

また、上記実施形態では、対象物の点群データに一致させたモデルデータの位置及び姿勢の各々を、ランダムに変化させた複数のパターンを生成する場合を例に説明したが、これに限定されるものではない。例えば、対象物の点群データに一致させたモデルデータの位置及び姿勢の各々を、予め定められた量だけ変化させて複数のパターンを生成するようにしてもよい。

以上の実施形態に関し、更に以下の付記を開示する。

（付記１）
３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出する対象物体検出部と、
前記対象物体検出部によって検出された前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成するパターン群生成部と、
前記検出部によって検出された前記点群データと、前記パターン群生成部によって生成された前記パターン群の各パターンの点群データとを、距離画像へ変換する画像変換部と、
前記画像変換部によって得られた、前記検出部によって検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合する画像照合部と、
前記画像照合部によって得られた照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する位置姿勢推定部と、
を含むことを特徴とする位置姿勢推定装置。

（付記２）
前記画像照合部は、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の各パターンの点群データの前記距離画像の各々とを背景差分によって照合する
付記１に記載の位置姿勢推定装置。

（付記３）
前記位置姿勢推定部は、推定された前記対象物体の位置及び姿勢を、前記モデルデータに反映し、
前記対象物体検出部は、前記位置姿勢推定部によって得られた前記モデルデータに基づいて、前記検出部によって検出された前記点群データから、前記対象物体を表す点群データを検出する
付記１又は付記２に記載の位置姿勢推定装置。

（付記４）
前記位置姿勢推定部は、前記画像照合部によって得られた照合結果に基づいて、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の前記距離画像との一致の度合いが、予め定められた閾値よりも大きい前記パターン群の前記距離画像を選択し、選択された前記距離画像のパターンの平均に基づいて前記対象物体の位置及び姿勢を推定する
付記１〜付記３の何れか１項に記載の位置姿勢推定装置。

（付記５）
前記対象物体検出部は、前記検出部によって検出された前記点群データと、対象物体を表す点群データであるモデルデータとの３次元特徴マッチングにより、前記検出部によって検出された前記点群データから、対象物体を表す点群データを検出する
付記１〜付記４の何れか１項に記載の位置姿勢推定装置。

（付記６）
３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出し、
前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成し、
検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換し、
検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合し、
照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する、
処理をコンピュータが実行することを特徴とする位置姿勢推定方法。

（付記７）
前記画像照合部は、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の各パターンの点群データの前記距離画像の各々とを背景差分によって照合する
付記６に記載の位置姿勢推定方法。

（付記８）
前記位置姿勢推定部は、推定された前記対象物体の位置及び姿勢を、前記モデルデータに反映し、
前記対象物体検出部は、前記位置姿勢推定部によって得られた前記モデルデータに基づいて、前記検出部によって検出された前記点群データから、前記対象物体を表す点群データを検出する
付記６又は付記７に記載の位置姿勢推定方法。

（付記９）
前記位置姿勢推定部は、前記画像照合部によって得られた照合結果に基づいて、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の前記距離画像との一致の度合いが、予め定められた閾値よりも大きい前記パターン群の前記距離画像を選択し、選択された前記距離画像のパターンの平均に基づいて前記対象物体の位置及び姿勢を推定する
付記６〜付記８の何れか１項に記載の位置姿勢推定方法。

（付記１０）
前記対象物体検出部は、前記検出部によって検出された前記点群データと、対象物体を表す点群データであるモデルデータとの３次元特徴マッチングにより、前記検出部によって検出された前記点群データから、対象物体を表す点群データを検出する
付記６〜付記９の何れか１項に記載の位置姿勢推定方法。

（付記１１）
３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出し、
前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成し、
検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換し、
検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合し、
照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する、
処理をコンピュータに実行させることを特徴とする位置姿勢推定プログラム。

（付記１２）
前記画像照合部は、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の各パターンの点群データの前記距離画像の各々とを背景差分によって照合する
付記１１に記載の位置姿勢推定プログラム。

（付記１３）
前記位置姿勢推定部は、推定された前記対象物体の位置及び姿勢を、前記モデルデータに反映し、
前記対象物体検出部は、前記位置姿勢推定部によって得られた前記モデルデータに基づいて、前記検出部によって検出された前記点群データから、前記対象物体を表す点群データを検出する
付記１１又は付記１２に記載の位置姿勢推定プログラム。

（付記１４）
前記位置姿勢推定部は、前記画像照合部によって得られた照合結果に基づいて、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の前記距離画像との一致の度合いが、予め定められた閾値よりも大きい前記パターン群の前記距離画像を選択し、選択された前記距離画像のパターンの平均に基づいて前記対象物体の位置及び姿勢を推定する
付記１１〜付記１３の何れか１項に記載の位置姿勢推定プログラム。

（付記１５）
前記対象物体検出部は、前記検出部によって検出された前記点群データと、対象物体を表す点群データであるモデルデータとの３次元特徴マッチングにより、前記検出部によって検出された前記点群データから、対象物体を表す点群データを検出する
付記１１〜付記１４の何れか１項に記載の位置姿勢推定プログラム。

（付記１６）
３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体を表す点群データを検出し、
検出された前記対象物体を表す点群データの位置で、位置及び姿勢の各々をランダムに変化させた、前記対象物体を表す点群データであるモデルデータのパターンを含むパターン群を生成し、
検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換し、
検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合し、
照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像のパターンに基づいて前記対象物体の位置及び姿勢を推定する、
処理をコンピュータに実行させることを特徴とする位置姿勢推定プログラムを記憶した記憶媒体。

１０ 位置姿勢推定装置
１２ センサ
１４ 制御部
１６ 情報取得部
１７ 点群データ記憶部
１８ モデルデータ記憶部
２０ 対象物体検出部
２２ パターン群生成部
２４ 画像変換部
２６ 画像照合部
２８ 位置姿勢推定部
５０ コンピュータ
５１ ＣＰＵ
５３ 記憶部
５９ 記録媒体
６０ 位置姿勢推定プログラム

Claims (5)

1. ３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出する対象物体検出部と、
   前記対象物体検出部によって検出された前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成するパターン群生成部と、
   前記検出部によって検出された前記点群データと、前記パターン群生成部によって生成された前記パターン群の各パターンの点群データとを、距離画像へ変換する画像変換部と、
   前記画像変換部によって得られた、前記検出部によって検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合する画像照合部と、
   前記画像照合部によって得られた照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する位置姿勢推定部と、
   を含むことを特徴とする位置姿勢推定装置。
2. 前記画像照合部は、前記検出部によって検出された前記点群データの前記距離画像と前記パターン群の各パターンの点群データの前記距離画像の各々とを背景差分によって照合する
   請求項１に記載の位置姿勢推定装置。
3. 前記位置姿勢推定部は、推定された前記対象物体の位置及び姿勢を、前記モデルデータに反映し、
   前記対象物体検出部は、前記位置姿勢推定部によって得られた前記モデルデータに基づいて、前記検出部によって検出された前記点群データから、前記対象物体を表す点群データを検出する
   請求項１又は請求項２に記載の位置姿勢推定装置。
4. ３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出し、
   前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成し、
   検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換し、
   検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合し、
   照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する、
   処理をコンピュータが実行することを特徴とする位置姿勢推定方法。
5. ３次元の点群データを検出する検出部によって検出された前記点群データから、対象物体のモデルデータに基づいて、対象物体を表す点群データを検出し、
   前記対象物体を表す点群データの位置で、前記モデルデータの位置及び姿勢の各々を変化させた複数のパターンの各々を表す複数の点群データを含むパターン群を生成し、
   検出された前記点群データと、生成された前記パターン群の各パターンの点群データとを、距離画像へ変換し、
   検出された前記点群データの前記距離画像と、前記パターン群の各パターンの点群データの前記距離画像の各々とを照合し、
   照合結果に基づいて、前記パターンの点群データの前記距離画像を選択し、選択された前記距離画像に対応するパターンに基づいて前記対象物体の位置及び姿勢を推定する、
   処理をコンピュータに実行させることを特徴とする位置姿勢推定プログラム。