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

Description

本発明は、ロボット装置などに好適に使用され得る位置推定装置、位置推定方法及びプログラムに関し、特に局所特徴量を使用して位置推定する位置推定装置、位置推定方法及びプログラム関する。

自己位置の推定・特定は、人間や機械にとっては必須の能力である。今自分はどこにいるかということを知ることは、ロボットやコンピュータビジョンにとっては、常に重要である。特に、可動式のロボットにとって、今自分がどこにいるかを把握することは、ナビゲーションシステムのための基本的要求となっている。

従来、特許文献１に記載の位置検出装置がある。この位置検出装置では、移動体の前方視野の輝度画像を取得する輝度画像取得手段と、輝度画像取得手段と同一の視野を有し、輝度画像取得手段が輝度画像を取得するのと同時に距離画像を取得する距離画像取得手段と、少なくとも連続する２フレームの輝度画像からそれぞれ特徴点を抽出する特徴点抽出手段と、特徴点抽出手段によって抽出された特徴点の２フレーム間の位置の変位量を距離画像に基づいて算出し、当該変位量から自己位置を算出するための基準特徴点を選択する基準特徴点選択手段とを備えている。

特開２００２−０４８５１３号公報

ところで、現在画像を撮影した場所が、ロボットが以前も訪れた場所であるか、又は全く知らない場所であるかを識別することは大変難しい。特徴量の抽出の仕方によっては、全く知らない場所をある場所に関連づけてしまうことがある。したがって、現在の位置がデータベースに登録済みの場所であるか、新しい場所であるかを切り分ける能力は大変重要である。また、当該撮影した場所が新しい場所であることが認識できれば、ＤＢを拡大していく、すなわち、地図を学習していくことが可能になる。このような、移動体、特にロボット装置に好適に搭載される位置推定装置の開発が望まれている。

本発明は、このような問題点を解決するためになされたものであり、現在の位置が既に登録済みの場所であるか、未登録の場所であるかを認識することができる位置推定装置、位置推定方法及びプログラムを提供することを目的とする。

本発明に係る位置推定装置は、入力画像から局所特徴量を抽出する特徴量抽出手段と、各登録場所と局所特徴量が対応づけられて保存されているデータベースを参照し、入力画像と登録場所とのマッチングを求めるマッチング手段と、マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を算出する類似度算出手段と、前記類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する認定手段とを有するものである。

本発明においては、マッチングを取った後、近傍の登録場所を含めて類似度を算出することで、よりノイズに強く、正確に入力画像がデータベースに登録された登録画像であるか否かを推定することができる。

また、前記データベースは、前記各登録場所の局所特徴量を使用して作成された、いずれの登録場所にも属さない非登録場所及びその特徴量を有することができる。これにより、新規の場所の特定率を向上する。

更に、前記非登録場所は、各登録場所の局所特徴量の一部又は全部が対応付けられているものとすることができる。

更にまた、前記入力画像が、前記非登録場所と最もマッチングする場合、当該入力画像とその特徴量を新しい登録場所としてデータベースに登録することができる。これにより、データベースを更新することができる。

また、マッチングスコアは、前記データベースに含まれる登録場所の数及び当該登録場所のうち、入力画素とマッチングした登録場所の数に基づき算出されることができる。

更に、マッチングスコアをｓ_ｔ、前記データベースに含まれる登録場所の数をｎ_ｔ、当該登録場所のうち、入力画素とマッチングした登録場所の数をｎ_ｗｋ＝Ｍ_ｊ、０＜ｊ＜ｎ_ｔ、ｊ≠ｉ、ｍ_ｉは、入力画像Ｍ_ｔと現在のモデルＭ_ｉとの間でマッチした局所特徴量の数、ｋ、ｉ、ｊ、ｔは整数としたとき、下記により前記マッチングスコアｓ_ｔを求めることができる。これにより、多くの登録モデルに存在する特徴量とマッチングした場合はスコアを低く、少数、例えばたった１つの登録モデルに存在する特徴量とマッチングした場合は、スコアを高く設定することができる。ｉは、データベースの中に登録されたインデックスを示す。

更にまた、前記類似度算出手段は、マッチングスコアと、ガウス分布から得られる遷移確率とに基づき類似度スコアを算出定することができる。類似スコアにより、更に位置特定率を向上することができる。

また、前記類似度スコアをβ_ｉ、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す前記遷移確率をｐ_Ｔ（ｉ，ｋ）、考慮する隣接登録画像に関する値をω、ｉ、ｋを整数とすると、下記により、前記類似度スコアβ_ｉを求めることができる。類似度スコアにより、マッチングした登録画像の近傍場所のデータも考慮されることができる。

更に、前記類似度算出手段は、前記類似度スコアを正規化した正規化スコアを算出することができる。類似度スコアは、最もマッチする登録モデルと、これに類似する登録モデルがある場合、特徴量の多くが共通となるため、類似度スコアが当該類似する登録モデルとの方が高くなる場合があり得るため、正規化することが好ましい。

更にまた、前記認定手段は、前記類似度スコアから前記類似度スコアの標準偏差と平均値の和を引いた値が所定の閾値以上である場合、前記入力画像を前記登録場所であると認定することができる。正規化により、より確実に入力画像の位置を特定することができる。

また、前記認定手段の認定結果を補正する補正手段を更に有し、前記補正手段は、前記類似度スコアを正規化した正規化スコアに基づき当該認定結果を補正した補正スコアを算出することができる。補正することで、更に場所の特定率を向上することができる。

更に、前記類似度スコアをβｉ、前記類似度スコアの標準偏差及び平均をそれぞれσ及びμ、Ｔ＝σ＋μ、ｉは整数としたとき、前記正規化スコアＣ_ｉは、下記の式により求めることができる。

更にまた、前記補正手段は、ω＋１≦ｉ≦ｎｔ−（ω＋１）とし、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す前記遷移確率をｐ_Ｔ（ｉ，ｋ）、考慮する隣接登録画像に関する値をω、ｉ、ｋを整数とすると、前記補正スコアＣ_ｊは、下記により求めることができる。補正することで、更に場所の特定率を向上することができる。

また、前記特徴量抽出手段は、前記入力画像それぞれから、局所特徴量を抽出する局所特徴量抽出手段と、前記局所特徴量抽出手段により抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量を連続特徴量として選択する連続特徴量選択手段と、各前記連続特徴量の平均を不変特徴量として求める不変特徴量算出手段とを有するものとすることができる。これにより、連続して撮影された画像を使用し、連続する２枚の画像間で特徴量のマッチングをとり、さらにマッチングを取った特徴量が連続して出現するもののみを抽出し、その平均の局所特徴量を求めることで、撮影位置の変化にロバストな特徴を抽出することができ、それを使用して位置を推定するため、正確に位置を特定することが可能となる。連続画像とは、ビデオ画像から取得される連続的に撮影された複数枚の画像セットである。

更に、前記局所特徴量は、ＳＩＦＴ（Scale Invariant Feature Transformation）及び／又はＳＵＲＦ（Speed Up Robustness Features）の特徴量とすることができる。また、これらＳＩＦＴやＳＵＲＦに限らず、スケール、回転の変動、又はノイズ等に対してロバストな他の局所特徴量を用いることも可能である。これにより、これら既存の局所特徴量を用いることで、これらの特徴量が有する性能もそのまま引き継がれ、照明変化等にも頑健な特徴として抽出・記述することが可能となる。

本発明に係る位置推定方法は、入力画像から局所特徴量を抽出する特徴量抽出工程と、各登録場所と局所特徴量が対応づけられて保存されているデータベースを参照し、入力画像と登録場所とのマッチングを求めるマッチング工程と、マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を算出する類似度算出工程と、前記類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する認定工程とを有するものである。

また、本発明に係るプログラムは、上述した位置推定処理をコンピュータに実行させるものである。

本発明によれば、現在の位置が既に登録済みの場所であるか、未登録の場所であるかを認識することができる位置推定装置、位置推定方法及びプログラムを提供することができる。

本発明の実施の形態にかかる位置推定装置を示す図である。 本発明の実施の形態にかかる位置推定装置を示すフローチャートである。 正規化前後の類似度スコア及び正規化スコアを示す図である。 補正前後のマッチングスコアを示す図である。 本発明の実施の形態にかかる位置推定装置の特徴量抽出部を示す図である。 本発明の実施の形態にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。 不変特徴量抽出方法を示すフローチャートである。 本発明の実施の形態にかかる位置推定装置の推定結果を示すものである。 同じく、本発明の実施の形態にかかる位置推定装置の推定結果を示すものである。 同じく、本発明の実施の形態にかかる位置推定装置の推定結果を示すものである。 同じく、本発明の実施の形態にかかる位置推定装置の推定結果を示すものである。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、移動型のロボット装置などに搭載される、位置を推定する位置推定装置に適用したものである。

図１は、本実施の形態にかかる位置推定装置を示す図である。図１に示すように、位置推定装置は、特徴量抽出部１１、マッチング部１３、類似度算出部１５、登録場所認定部１７、登録場所補正部２１及びデータベース１９を有する。

特徴量抽出部１１は、入力画像から局所特徴量を抽出する。局所特徴量としては、ＳＩＦＴ（Scale Invariant Feature Transformation）及び／又はＳＵＲＦ（Speed Up Robustness Features）、又は後述するＰＩＲＦ（Position-Invariant Robust Features）等を使用することができる。

マッチング部１３は、各登録場所と局所特徴量が対応づけられて保存されているデータベース１９を参照し、入力画像と登録場所とのマッチングを求める。データベース１９は、各登録場所毎にその識別情報と、当該登録場所の特徴量の値を対応付けたモデルテーブル１９３を登録場所の個数だけ有する。例えば、モデルテーブルは、６００（Ｍ_１〜Ｍ_６００）、特徴量の個数は１００（ｐ１＿１〜ｐ１＿１００）等とすることができる。

また、データベース１９は、各登録場所の局所特徴量を使用して作成された、いずれの登録場所にも属さない非登録場所Ｍ_０及びその特徴量を有する。ここでは、当該テーブルをモデルテーブル１９１という。非登録場所Ｍ_０は、各登録場所の局所特徴量の一部又は全部が対応付けられたものであり、例えば各登録場所の局所特徴量のうち、ランダムに抽出した１０％の特徴量からなるもの等とすることができる。

マッチング部１３は、入力画像が、非登録場所Ｍ_０と最もマッチングする場合、当該入力画像とその特徴量を新しい登録場所としてデータベース１９に登録する。このマッチング部１３においては、マッチングスコアは、データベース１９に含まれる登録場所の数及び当該登録場所のうち、入力画像とマッチングした登録場所の数に基づき算出される。ここで、マッチングスコアをｓ_ｔ、データベースに含まれる登録場所の数をｎ_ｔ、当該登録場所のうち、入力画素とマッチングした登録場所の数をｎ_ｗｋ＝Ｍ_ｊ、０＜ｊ＜ｎ_ｔ、ｊ≠ｉ、ｍ_ｉは、入力画像Ｍ_ｔと現在のモデルＭ_ｉとの間でマッチした局所特徴量の数、ｋ、ｉ、ｊ、ｔは整数としたとき、下記式（１）によりマッチングスコアｓ_ｔを求めることができる。

類似度算出部１５は、マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を判定する。この類似度算出部１５は、マッチングスコアと、ガウス分布から得られる遷移確率とに基づき類似度スコアを算出する。ここで、類似度スコアをβ_ｉ、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す遷移確率をｐ_Ｔ（ｉ，ｋ）、考慮する隣接登録画像の数をω、ｉ、ｋを整数とすると、下記式（２）により、類似度スコアβ_ｉを求めることができる。

ここで、類似度算出部１５は、類似度スコアβを正規化する。正規化することにより、よりノイズにロバストなスコアを取得することができる。本実施の形態においては、類似度スコアをβ_ｉ、類似度スコアの標準偏差及び平均をそれぞれσ及びμ、Ｔ＝σ＋μ、ｉは整数としたとき、正規化スコアＣ_ｉは、下記の式（３）により求めることができる。

登録場所認定部１７は、類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する。具体的には、類似度算出部１５は、類似度スコアから類似度スコアの標準偏差σと平均値μとの和Ｔを引いた値が所定の閾値τ２より大きい場合、入力画像を前記登録場所であると判定する。

登録場所補正部２１は、登録場所認定部１７の認定結果を補正する。具体的には、登録場所補正部２１は、類似度スコアを正規化した正規化スコアを補正する。

ω＋１≦ｉ≦ｎｔ−（ω＋１）とし、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す遷移確率をｐ_Ｔ（ｉ，ｋ）、考慮する隣接登録画像の数をω、ｉ、ｋを整数とすると、補正スコアＣ_ｊは、下記式（４）により求まる。補正結果に応じて、入力画像がいずれの登録画像に一致するかを補正して求めることができる。

次に、本実施の形態にかかる位置推定方法について説明する。図２は、本実施の形態にかかる位置推定装置を示すフローチャートである。図２に示すように、特徴量抽出部１１は、先ず入力画像を取得する。特徴量抽出部１１が特徴量として後述するＰＩＲＦを使用する場合には、この入力画像として、連続画像が入力される（ステップＳ１）。そして、類似度算出部１５は、特徴量を抽出する（ステップＳ２）。特徴量抽出部１１は、入力画像から抽出した複数の局所特徴量と入力画像が表わす場所（以下、入力場所という。）とを対応づけたモデルＭ_ｔを生成する（ステップＳ３）。

次に、マッチング部１３は、データベース１９を参照してマッチングスコアＳ_ｔを算出する。先ず、現在のモデル（入力画像モデル）Ｍ_ｔと、データベース１９に登録してある全てのモデルとでマッチングを行う。各モデルにおける各特徴量が所定の閾値以上でマッチングが取れている場合は、マッチングしているものとしてとり扱う。

上述したように、データベース１９は、モデルＭ_０のモデルテーブル１９１を有している。これは、各モデルが有する全部又は一部の特徴量をそれぞれ有しているもので、場所が特定できないものである。このモデルＭ_０は、例えば他のモデルの３乃至４倍の特徴量を有する。もし入力モデルがこのモデルＭ_０と最もマッチングが取れている場合は、入力モデルは新しいモデルとみなされ（ステップＳ５：Ｙｅｓ）、当該入力モデルＭ_ｔを新規の場所としてデータベース１９に追加する（ステップＳ１３，１２）。ここで、モデルテーブル１９１に登録してある特徴量の例えばランダムに５つ抽出して破棄し、新規に登録する入力モデルＭ_ｔの特徴量からランダムに５つ抽出して、モデルＭ_０の特徴量の更新を行うことができる。又は、例えば新規の登録場所が例えば３００枚追加になった時点でモデルＭ_０の特徴量を全部破棄し、データベースにある各モデルからランダムに例えば３つの特徴量を抽出し、これを合わせてモデルＭ_０の特徴量とすることも可能である。

次に、入力モデルＭ_ｔが非登録場所モデルＭ_０に類似しない場合について説明する。入力モデルＭ_ｔと各モデルの特徴量を比較するが、例えば、東京タワーのような、広範囲の場所から認識できるような特徴量は、その場所固有の特徴量としては、精度の低いものとなる。すなわち、入力モデルのある特徴量のうち、データベース１９の６００の登録モデルのうちの例えば１０％と一致してしまうような特徴量は、識別力が低いものと言える。一方で、入力モデルのある特徴量が、ある登録モデル１つの特徴量と一致する場合、その特徴量は識別能力が高いと見なすことができる。

これらを考慮して、各特徴量についてマッチングを取りつつ、識別力の高い特徴量のマッチングには高いスコアを、識別力の弱い特徴量のマッチングには低いスコアを付与する。このため、本実施の形態においては、下記式（１）を使用する。この式（１）に示すように、入力画素とマッチングした登録場所の数をｎ_ｗｋが多いほどスコアＳ_ｔが小さくなることがわかる。

次に、このスコアＳ_ｔの最大値が所定の閾値τより大きいか否かを判定する（ステップＳ６）。ここで、スコアＳ_ｔの最大値がＭ_０ではなかったとしても、所定の閾値τより小さい場合は、入力画像モデルは登録モデルとマッチングしていないと判断し、ステップＳ１３に進み、当該入力画像モデルＭ_ｔを新規の場所として登録する（ステップＳ１２）。所定の閾値τは例えば、３等とすることができる。

ここで、このスコアＳ_ｔは、非常にセンシティブでありノイズに敏感である。そこで、このスコアＳ_ｔから、隣接する登録モデルのデータを使用して別のスコアを求める。このスコアを、本明細書においては、類似度スコアβという。上述したように、類似度スコアβは、式（２）により求まる（ステップＳ７）。

次に、この類似度スコアを正規化する。いくつの特徴量がマッチすればよいかを予測することは困難である。したがって、例えば隣接する登録モデルとの間でよい類似度スコアが得られていたとしても、最もマッチする登録モデルと、これに類似する登録モデルがある場合、特徴量の多くが共通となるため、類似度スコアが当該類似する登録モデルとの方が高くなる場合があり得る。したがって、類似度スコアは正規化されなければならない。本実施の形態においては、上述のように正規化スコアは、式（３）により求める。

図３は、正規化前後の類似度スコア及び正規化スコアを示す図である。図３（ａ）に示すように、１５個の類似度スコアを正規化に使用する。ここで、ｌ_ｎを、考慮する隣接登録モデルの個数とし、ｌ_ｎ＝７として、１５個の類似度スコアを使用する例を示している。標準偏差σ及びその平均μの合計をＴで示す。ｌ_ｎは、値Ｔを算出する際の制限として利用する。これは、登録モデル全体において、Ｔは非常に幅のある数であるためである。なお、ｌ_ｎの値は、結果に大きな影響を及ぼすものではないが、Ｔを毎回計算するにあたり、同じスケールに確定するために使用するものである。正規化スコアを図３（ｂ）に示す。なお、正規化はスコアのトレンドをきれいな正規分布の形になるように修正する。

そして、類似度スコアβ−Ｔが所定の閾値τ２より大きいか判定する（ステップＳ８）。ここで、β−Ｔが閾値τ２より小さかった場合は、ステップＳ１１に進み、当該入力画像モデルは、登録場所とマッチングしないと判断し、ステップＳ１２に進み、当該入力画像モデルを新たな登録場所モデルとして登録する。以上により、入力画像を登録画像のいずれかに一致するものと判断してもよいが、本実施の形態においては、更に、ここで求められた登録場所を更に補正する。

図４は、補正前後のマッチングスコアを示す図である。図４（ａ）は、最大スコアＣ_ｊ，ｊ＝８７を示す。ここで、マッチングスコアは、左右対称であることが好ましいため、これを上述した式（４）で補正し補正スコアＣ_ｊを求める。

これにより、図４（ｂ）に示すように、Ｃ_ｊ，ｊ＝８７に補正される。なお、このように入力画像は、登録画像モデルＭ_８７に一致すると判定した場合、入力画像モデルはそのまま破棄することも可能であるが、登録画像モデルＭ_８７を更新することも可能である。例えば、いくつかの特徴量を入れ替えたり、平均してもよい。

次に、特徴量抽出部について詳細に説明する。上述したように、特徴量としては、局所特徴量であれば、どのようなものでも使用可能であるが、本願発明者等が先に出願しているＰＩＲＦを使用することで、位置推定確率を向上させることができる。

図５は、本発明の実施の形態にかかる位置推定装置の特徴量抽出部を示す図である。本実施の形態における特徴量抽出部は、連続して撮影された連続画像からなる入力画像から不変特徴量を抽出する。

特徴量抽出部１１は、入力画像それぞれから、局所特徴量を抽出する局所特徴量抽出部１１１と、局所特徴量抽出部１１１により抽出された局所特徴量について、連続する入力画像間でマッチングをとる特徴量マッチング部１１３と、特徴量マッチング部１１３により所定数連続する画像間でマッチングが取れた特徴量を連続特徴量として選択する連続特徴量選択部１１５と、各連続特徴量の平均を不変特徴量として求める不変特徴量算出部１１７とを有する。

以下の説明においては、不変特徴量算出部１１７が抽出する不変特徴量のことを、特徴量ＰＩＲＦ（Position-Invariant Robust Features）という。特徴量抽出部１１は、撮影位置の変化に影響を受けにくい（局所）特徴量として特徴量ＰＩＲＦを抽出する。

これは、本願発明者が位置の変化にロバストな特徴量を抽出すべく鋭意実験研究した結果、近くの対象については撮影位置や撮影時間帯の変化による見え方の差（特徴量変化）が大きいが、遠くの対象については変化が小さい（ランドマークの特徴量はあまり変化しない）ことを知見し、その結果本特徴量ＰＩＲＦを抽出する方法を見出したものである。

本実施の形態にかかる特徴量抽出部１１は、簡単には、連続画像間で局所特徴のマッチングを行い、予め定めた枚数間で、連続してマッチングのとれている特徴を選択し、選択された特徴において、それとマッチングのとれている全特徴の平均を特徴量ＰＩＲＦとして抽出・記述するものである。

この特徴量抽出部１１により撮影位置の変化に頑健な局所特徴のみを抽出・記述することができる。また、記述子としては、局所特徴であれば様々なものが適用可能であり、上述のように既存の局所特徴量を用いることで、既存の特徴量が持つ性能もそのまま引き継がれ、照明変化等にも頑健な特徴として抽出・記述することが可能となる

以下、各ブロックについて詳細に説明する。図６は、本実施の形態にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。局所特徴量抽出部１１１には、連続して撮影された連続画像が入力画像として入力される。ここで、ＰＩＲＦで要求される連続画像とは、ある画像セットであって、一定のフレームで、例えば１秒毎に２フレームなど、毎秒毎に連続的に撮影されたビデオ画像をいう。すなわち、ビデオからキャプチャされた画像は一般的に連続的であり、ＰＩＲＦにおける連続画像は、ビデオ画像を使用したものでなければならない。画像の取得率は、カメラの速度に応じて設定される。たとえば、カメラが車に搭載されていた場合、カメラの速度は１分間に約１０００ｍ／分であり、ビデオからキャプチャされる連続画像はおよそ５０乃至１００フレーム／秒となる。

本実施の形態においては、入力画像として全方位画像を用い、連続する画像は、１つのプレイス（place）を撮影したものを使用する。後述するように、このプレイスとは、例えば交差点から交差点までのある領域とする。当該プレイスは、いくつかの連続画像からなる複数のサブプレイスに分割される。すなわち、１つのプレイスは、複数のサブプレイスから構成される。このサブプレイス毎に１又は複数の不変特徴量を抽出する。この不変特徴量の集合をサブプレイスＰＩＲＦ辞書（PIRF-dictionary）という。１つのプレイスから抽出された全不変特徴量の集合、すなわち、上記サブプレイスＰＩＲＦ辞書の集合によりＰＩＲＦ辞書が構成される。１つのプレイスには１つのＰＩＲＦ辞書が対応する。このサブプレイスＰＩＲＦ辞書及びＰＩＲＦ辞書が抽出されたプレイスの識別情報が、ＰＩＲＦ辞書と共に上述のエリアデータベース１１に格納される。

先ず、局所特徴量抽出部１１１は、既存の局所特徴量抽出方法を使用して局所特徴量を抽出する。局所特徴量抽出部１１１は、例えば、ＳＩＦＴ（Scale Invariant Feature Transformation）、又はＳＵＲＦ（Speed Up Robustness Features）の特徴量を使用することができる。または、これらＳＩＦＴやＳＵＲＦに限らず、他の局所特徴量を使用することができることは勿論である。特に、スケール、回転の変動、又はノイズ等に対してロバストな他の局所特徴量を用いることが好ましい。これらの局所特徴量を用いることで、既存の特徴量が有する性能もそのまま引き継がれ、照明変化等にも頑健な特徴として抽出・記述することが可能となる。ここで、ｉ番目のエリアにおける全方位画像の数をｎ_ｉとし、その中でｊ番目の画像から抽出された局所特徴量の集合をＵ_ｊ ^ｉとする。ここでは、局所特徴量はｕ→で表す。

次に、特徴量マッチング部１１３が、連続する２枚の画像間全てについて局所特徴量ｕ→を構成する各特徴量のマッチングを行う。すなわち、ｊ＝ｑ番目の画像の全局所特徴量について、ｊ＝ｑ＋１番目の画像の全局所特徴量に対してマッチングを行う。ここでは、それぞれマッチングのとれた特徴へのインデックスをマッチング結果ベクトルｍ_ｑ ^ｉ→として求める。

ここで、マッチング方法の一例について、ＳＩＦＴを例にとって説明する。画像Ｉ_ａから抽出される特徴をｖとする。この特徴ｖが次の画像Ｉ_ａ＋１の特徴ｖ'とマッチングするか否かを判定する。先ず、特徴ｖと画像Ｉ_ａ＋１から抽出した全ての特徴との間のドット積（dot product）を求める。そして、最も類似する特徴ｖ_{ｆｉｒｓｔ}と、２番目に類似する特徴ｖ_{ｓｅｃｏｎｄ}を求めるために、この結果をソートする。もし、
（ｖ_{ｆｉｒｓｔ}・ｖ）／（ｖ_{ｓｅｃｏｎｄ}・ｖ）＞θ
が成立する場合、マッチングのとれた特徴ｖ'＝ｖ_{ｆｉｒｓｔ}と判定する。ここで、閾値θはたとえば０．６とすることができる。上記が成立しない場合は、画像Ｉ_ａにおける特徴ｖにマッチングする特徴は、画像Ｉ_ａ＋１には存在しないと判定する。

図６に示すように、各入力画像から６つの局所特徴量が抽出された場合について説明する。これら６つの局所特徴量間でマッチングを取り、マッチングが取れた場合にのみ、マッチングが取れた特徴量へのインデックスを付す。例えば、ｍ_１ ^ｉ→の１番目の局所特徴量は、ｍ_２ ^ｉ→の３番目の局所特徴量とマッチングがとれていることを示し、ｍ_２ ^ｉ→の３番目の特徴量は、ｍ_３ ^ｉ→の６番目の特徴量とマッチングが取れていることを示す。

次に、連続特徴量選択部１１５が連続特徴量を選択する。先ず、いくつのｍ_ｑ ^ｉ→を使用して連続特徴量を求めるかを決定する。このｍ_ｑ ^ｉ→の数を、本明細書においては、ウィンドウサイズｗともいう。また、このウィンドウサイズｗに含まれるｍ_ｑ ^ｉ→の集合を、サブプレイスという。ここで、ウィンドウサイズｗが大きいほどより頑健な、識別力の高い連続特徴量のみを抽出できるが、大きすぎると特徴数が極端に少なくなってしまう。また、小さすぎると、頑健ではない、識別力のないような特徴量も抽出してしまうので、目的等に応じて最適な大きさとする必要がある。

本実施の形態においては、ウィンドウサイズｗを３とする。したがって、連続する４枚の入力画像を使用して連続特徴量を求める。すなわち、図６に示すように、１番目のサブプレイスには、ｍ_１ ^ｉ→、ｍ_２ ^ｉ→、ｍ_３ ^ｉ→が含まれ、入力画像Ｉ_１、Ｉ_２、Ｉ_３、Ｉ_４が対応する。なお、インデックスの数が０の場合は、次にマッチングする特徴量がないことを示す。よって、図６の場合、１番目のサブプレイスには、３つの連続特徴量が含まれることになる。

連続特徴量選択部１１５は、ウィンドウサイズｗを設定したらこのウィンドウｗをひとつずつずらしながら、そのウィンドウサイズに含まれる全方位画像４枚内で共通して出現する特徴を連続特徴量として抽出する。ウィンドウサイズｗを設定したら、連続特徴量を抽出するために用いる関数を以下のように定義する。ただし、ｂは注目するインデックスベクトルの番号とする。
そして、全てのマッチング結果ベクトルｍ_ｑ ^ｉ→について、ｆ（ｍ_ｘ,ｙ ^ｉ）を計算し、ｆ（ｍ_ｘ,ｙ ^ｉ）＞０となるときの局所特徴量ｕ_ｘ,ｙ ^ｉ→のみを抽出する。入力画像の数がｎ_ｉ、ウィンドウサイズがｗのとき、サブプレイスの数は、ｎ_ｉ−ｗ＋１となる。

不変特徴量算出部１１７は、同一ウィンドウグループであるサブプレイス内においてマッチングがとれた局所特徴量の平均を求める。これらの平均値からなるベクトルによりＰＩＦＲ辞書が構成される。全サブプレイス（ｎ_ｉ−ｗ＋１）個から抽出された（ｎ_ｉ−ｗ＋１）個のサブプレイスＰＩＦＲ辞書（Ｄ_ｊ ^ｉ，ｊ≦ｎ_ｉ−ｗ＋１）をＰＩＲＦ辞書（Ｄ^ｉ）に登録する。このＰＩＦＲ辞書を構成する、マッチングがとれた局所特徴量の平均各がＰＩＲＦである。

次に、特徴量抽出部の不変特徴量ＰＩＦＲの抽出方法について説明する。図７は、不変特徴量抽出方法を示すフローチャートである。先ず、ｉ＝１、ｊ＝１に初期化する（ステップＳ２１）。ここで、ｎ_ｉは、プレイスＡ_ｊに属する画像の枚数であり、ｗは、ＰＩＲＦを抽出するウィンドウサイズである。

次に、エリアＡ_ｊの画像Ｉ_ｉを入力する（ステップＳ２２）。そして、ｉがウィンドウサイズｗより大きいか否かを判定する（ステップＳ２３）。ｉがウィンドウサイズｗ以下である場合は、ｉをインクリメントし（ステップＳ３０）、ステップＳ２に戻る。

一方、ウィンドウサイズｗより大きい場合は、画像Ｉ_ｉと画像Ｉ_ｉ−１の間でマッチングを取る（ステップＳ２４）。そして、ｗ＋１枚の連続する入力画像（Ｉ_ｉ−ｗ，…，Ｉ_ｉ）から安定している局所特徴量（ＳＩＦＴ）を抽出する（ステップＳ２５）。ここで、本実施の形態においては、ウィンドウサイズｗを３に設定している。ウィンドウサイズｗは、何枚の連続画像からＰＩＲＦを抽出するかを示す。たとえば、ウィンドウサイズｗを３に設定した場合、ＰＩＲＦは４つの連続画像に連続して現れる特徴があるときにのみ得られる。従って、あるプレイスＡの画像イメージが４枚より少ない場合、３つの２画像間で一致する特徴を見つけるのに十分ではない。よって、画像が少なくとも４枚であるとき、プレイスＡからのＰＩＲＦの抽出をスタートすることができる。例えば、現在の画像がＩ_６であるとき、ＰＩＲＦは、４枚の連続画像I_３、Ｉ_４、Ｉ_５、Ｉ_６から求めることができる。

次に、抽出した安定した局所特徴量の平均をＰＩＲＦとして算出する（ステップＳ２６）。次いで、エリアＡ_ｊに含まれるＰＩＲＦを収集し、ＰＩＲＦ辞書Ｄ_ｊに登録する（ステップＳ２７）。そして、ｉ＝ｎ_ｊであるか否かが判断され（ステップＳ２８）、ｉ＝ｎ_ｊであれば、ｉ＝１、ｊ＝ｊ＋１とし、ステップＳ２からの処理を繰り返す（ステップＳ２９）。ｉ＝ｎ_ｊではない場合は、ステップＳ１０に進み、ｉをインクリメントして、ステップＳ２に戻る。

次に、本実施の形態の効果について説明する。図８乃至図１１は、本実施の形態にかかる位置推定装置の推定結果を示すものである。図８乃至図１１は、それぞれ、New College、City Center、Suzukakedai、混合モデルの位置推定結果を示す。図８に示すように、スケールを０．２５としたものでさえも、他の位置推定方法であるＦＡＢ−ＭＡＰよりよい精度を示していることがわかる。その他の結果についても、ＦＡＢ−ＭＡＰより極めて精度よく場所を特定できていることがわかる。

以上説明したように、本実施の形態においては、入力画像と登録画像のマッチングを取った後、登録場所の近傍の登録場所を含めて類似度を算出し、それを正規化して、入力画像がいずれの登録場所に一致するかを判定するため、極めて正確に、入力画像が既にデータベースに登録された登録場所であるか、初めて認識した画像であるかの切り分けを行うことができる。また、登録画像を一致しない入力画像は、そのままデータデータベースに登録することで、自律型の移ロボット装置等に搭載すれば、自己の位置を探索し、位置データベースを自身で拡大させることも可能である。

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１１ 特徴量抽出部
１３ マッチング部
１５ 類似度算出部
１７ 登録場所認定部
１９ データベース
２１ 登録場所補正部
１９３ モデルテーブル
１９１ モデルテーブル
１１１ 局所特徴量抽出部
１１３ 特徴量マッチング部
１１５ 連続特徴量選択部
１１７ 不変特徴量算出部

Claims (17)

1. 入力画像から局所特徴量を抽出する特徴量抽出手段と、
   各登録場所と局所特徴量が対応づけられて保存されているデータベースを参照し、入力画像と登録場所とのマッチングを求めるマッチング手段と、
   マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を算出する類似度算出手段と、
   前記類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する認定手段とを有する位置推定装置。
2. 前記データベースは、前記各登録場所の局所特徴量を使用して作成された、いずれの登録場所にも属さない非登録場所及びその特徴量を有する、請求項１記載の位置推定装置。
3. 前記非登録場所は、各登録場所の局所特徴量の一部又は全部が対応付けられている、請求項２記載の位置推定装置。
4. 前記入力画像が、前記非登録場所と最もマッチングする場合、当該入力画像とその特徴量を新しい登録場所としてデータベースに登録する、請求項２又は３記載の位置推定装置。
5. マッチングスコアは、前記データベースに含まれる登録場所の数及び当該登録場所のうち、入力画素とマッチングした登録場所の数に基づき算出される、請求項１乃至４のいずれか１項記載の位置推定装置。
6. マッチングスコアをｓｔ、前記データベースに含まれる登録場所の数をｎｔ、当該登録場所のうち、入力画素とマッチングした登録場所の数をｎｗｋ＝Ｍｊ、０＜ｊ＜ｎｔ、ｊ≠ｉ、ｍｉは、入力画像Ｍｔと現在のモデルＭｉとの間でマッチした局所特徴量の数、ｋ、ｉ、ｊ、ｔは整数としたとき、下記により前記マッチングスコアｓｔを求めることができる、請求項５記載の位置推定装置。
   
7. 前記類似度算出手段は、マッチングスコアと、ガウス分布から得られる遷移確率とに基づき類似度スコアを算出する、請求項１乃至６のいずれか１項記載の位置推定装置。
8. 前記類似度スコアをβｉ、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す前記遷移確率をｐＴ（ｉ，ｋ）、考慮する隣接登録画像に関する値をω、ｉ、ｋを整数とすると、下記により、前記類似度スコアβｉを求めることができる、請求項７記載の位置推定装置。
   
9. 前記類似度算出手段は、前記類似度スコアを正規化した正規化スコアを算出する、請求項７又は８記載の位置推定装置。
10. 前記認定手段は、前記類似度スコアから前記類似度スコアの標準偏差と平均値の和を引いた値が所定の閾値以上である場合、前記入力画像を前記登録場所であると認定する、請求項７乃至９のいずれか１項記載の位置推定装置。
11. 前記認定手段の認定結果を補正する補正手段を更に有し、前記補正手段は、前記類似度スコアを正規化した正規化スコアに基づき当該認定結果を補正した補正スコアを算出する、請求項７乃至１０のいずれか１項記載の位置推定装置。
12. 前記類似度スコアをβｉ、前記類似度スコアの標準偏差及び平均をそれぞれσ及びμ、Ｔ＝σ＋μ、ｉは整数としたとき、前記正規化スコアＣｉは、下記の式により求めることができる、請求項１１項記載の位置推定装置。
    
13. 前記補正手段は、ω＋１≦ｉ≦ｎｔ−（ω＋１）とし、ガウシアンシグマ＝２としたとき、ｉからｋの距離を示す前記遷移確率をｐＴ（ｉ，ｋ）、考慮する隣接登録画像に関する値をω、ｉ、ｋを整数とすると、前記補正スコアＣｊは、下記により求まる、請求項１１又は１２記載の位置推定装置。
    
14. 前記特徴量抽出手段は、
    前記入力画像それぞれから、局所特徴量を抽出する局所特徴量抽出手段と、
    前記局所特徴量抽出手段により抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、
    前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量を連続特徴量として選択する連続特徴量選択手段と、
    各前記連続特徴量の平均を不変特徴量として求める不変特徴量算出手段とを有する、請求項１乃至１３のいずれか１項記載の位置推定装置。
15. 前記局所特徴量は、ＳＩＦＴ（Scale Invariant Feature Transformation）及び／又はＳＵＲＦ（Speed Up Robustness Features）の特徴量である、請求項１４項記載の位置推定装置。
16. 入力画像から局所特徴量を抽出する特徴量抽出工程と、
    各登録場所と局所特徴量が対応づけられて保存されているデータベースを参照し、入力画像と登録場所とのマッチングを求めるマッチング工程と、
    マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を算出する類似度算出工程と、
    前記類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する認定工程とを有する位置推定方法。
17. 所定の動作をコンピュータに実行させるためのプログラムであって、
    入力画像から局所特徴量を抽出する特徴量抽出工程と、
    各登録場所と局所特徴量が対応づけられて保存されているデータベースを参照し、入力画像と登録場所とのマッチングを求めるマッチング工程と、
    マッチングが所定の閾値以上である場合に、選ばれた登録場所の近傍の登録場所を含めて類似度を判定する類似度算出工程と、
    前記類似度が所定の閾値以上である場合に、当該入力画像が登録場所であると認定する認定工程とを有するプログラム。