JP4985166B2 - 自己位置推定装置 - Google Patents

Description

本発明は、自己位置を推定する自己位置推定装置に関する。

従来、車両に搭載されたナビゲーションシステムなどの移動体において、自己位置を推定する技術として、下記の非特許文献１に開示されたランドマークデータベースを用いた投票による位置推定方法がある。この位置推定方法では、予めランドマークデータベースに所定の特徴を有する目印であるランドマークが観測できる範囲を登録しておく。また、位置推定を行う際には、ランドマークを含む画像を撮像し、この画像内からランドマークを抽出する。それから、抽出されたランドマークとランドマークデータベース内のランドマークとを比較し、両ランドマークが一致した場合に、ランドマークが観測できる位置を投票する。そして、投票値がもっとも高くなった位置を現在の位置と推定するというものである。
中川知香他２名、自然特徴点ランドマークデータベースを用いた投票に基づく静止画像からのカメラ位置・姿勢推定、画像の認識・理解シンポジウム（ＭＩＲＵ２００６）講演論文集、電子情報通信学会パターン認識・メディア理解研究専門委員会（ＰＲＭＵ）、Ｊｕｌｙ２００６、２４９−２５４頁

しかし、上記非特許文献１に記載された従来技術では、抽出されたランドマークとランドマークデータベース内のランドマークとの一致度が一定値以上となる場合、自己位置を推定するにあたり、これらのランドマークを同等に取り扱っている。このため、類似するランドマークが抽出された際には、互いのランドマークを誤認識することがあり、誤認識されたランドマークが複数存在する場合には、自己位置を正確に推定することができなくなることがあるという問題があった。

そこで、本発明の課題は、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識し、高い精度で自己位置を推定することができる自己推定装置を提供することにある。

上記課題を解決した本発明に係る自己位置推定装置は、ランドマークを検出するランドマーク検出手段と、ランドマークの特徴量を記憶するランドマーク特徴量記憶手段と、新規事項の追加に該当ランドマーク検出手段によって検出されたランドマークと、ランドマーク特徴量記憶手段に記憶されたランドマークの特徴量と一致度であるランドマーク一致度を取得する一致度取得手段と、新規事項の追加に該当自己位置から新規事項の追加に該当ランドマークまでの距離を検出する距離検出手段と、新規事項の追加に該当ランドマークを検出可能とされる検出範囲を、複数の新規事項の追加に該当ランドマークにそれぞれ対応させて取得する検出範囲取得手段と、新規事項の追加に該当距離検出手段によって検出された距離と新規事項の追加に該当検出範囲取得手段によって取得された検出範囲とに基づいて、自己が存在可能である自己存在可能範囲を推定する自己存在可能範囲推定手段と、複数の新規事項の追加に該当ランドマークにそれぞれ対応する新規事項の追加に該当自己存在可能範囲内における自己位置の候補となる自己位置候補点を設定する自己位置候補点設定手段と、新規事項の追加に該当自己位置候補点に対して投票を行う投票手段と、新規事項の追加に該当投票手段の投票結果が所定となったときに、新規事項の追加に該当しきい値を超えた自己位置候補点を自己位置と推定する自己位置推定手段と、を備え、新規事項の追加に該当投票手段は、投票値が所定の値となるまで新規事項の追加に該当ランドマーク一致度が高いランドマークに対応する自己存在可能範囲における新規事項の追加に該当自己位置候補点から順番に投票を行うものである。

本発明に係る自己位置推定装置においては、自己位置を推定する際、自己位置の推定に用いるランドマークとして、ランドマーク信頼度が高いランドマークを順番に採用するようにしている。このため、類似するランドマークが抽出されたとしても、ランドマーク信頼度の低いランドマークの影響を小さくすることができる。したがって、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識することができ、もって高い精度で自己位置を推定することができる。

なお、本発明にいう「ランドマーク」とは、位置推定において目印になるものであり、実環境に存在する物体、この物体の任意の点に記された記号などを含むものである。この「ランドマーク」は、移動可能な物体および移動可能な物体の任意の点でもよく、固定物および固定物の任意の点でもよい。また、ここでいう「ランドマークの信頼度」とは、自己位置を推定する際に、有効なランドマークであるかを示す指標となるものである。

このように、投票値が所定の値となるまでランドマーク信頼度が高いランドマークから順番に投票を行うことにより、ランドマーク信頼度が高いランドマークによって投票値が求められることになり、その分精度よく自己位置を推定することができる。

また、上記課題を解決した本発明に係る自己位置推定装置は、ランドマークを検出するランドマーク検出手段と、ランドマーク検出手段によって検出されたランドマークの信頼度であるランドマーク信頼度を取得する信頼度取得手段と、ランドマーク検出手段によって検出された複数のランドマークに基づいて、自己位置を推定する自己位置推定手段と、自己位置からランドマークまでの距離を検出する距離検出手段と、ランドマークを検出可能とされる検出範囲をランドマークごとに取得する検出範囲取得手段と、を備え、自己位置推定手段は、距離検出手段によって検出された距離と検出範囲取得手段によって取得された検出範囲とに基づいて、自己が存在可能である自己存在可能範囲を推定する自己存在可能範囲推定手段と、複数のランドマークにそれぞれ対応する自己存在可能範囲内における自己位置の候補となる自己位置候補点に対して投票を行う投票手段と、を有しており、投票手段は、ランドマーク信頼度が高いランドマークに対応する自己存在可能範囲における自己位置候補点についての投票値の大きさを、ランドマーク信頼度が低いランドマークに対応する自己存在可能範囲における自己位置候補点についての投票値の大きさよりも大きくするものである。

本発明に係る自己位置推定装置では、ランドマーク信頼度が高いランドマークについての投票値の大きさを、ランドマーク信頼度が低いランドマークについての投票値の大きさよりも大きくしている。このため、類似するランドマークが抽出されたとしても、ランドマーク信頼度が低いランドマークの影響が小さい投票値を得ることができる。したがって、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識することができ、もって高い精度で自己位置を推定することができる。

このとき、自己位置存在可能範囲推定手段は、ランドマーク信頼度が高いランドマークについての自己存在可能範囲の広さを、ランドマーク信頼度が低いランドマークについての自己存在可能範囲の広さよりも狭く推定する態様とすることもできる。

このように、ランドマーク信頼度が高いランドマークについての自己存在可能範囲の広さを、ランドマーク信頼度が低いランドマークについての自己存在可能範囲の広さよりも狭く推定することにより、自己存在可能範囲同士の重なり合いが少なくなる。その結果、自己存在可能範囲が重なった部分を誤って現在位置と推定することを回避することができる。

本発明に係る自己位置推定装置によれば、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識し、高い精度で自己位置を推定することができる。

以下、本発明による自己位置推定装置の好適な実施形態について、図面を参照しながら説明する。なお、図面の説明において、同一または相当要素には同一の符号を付し、重複する説明は省略する。

本発明の自己位置推定装置は、たとえばナビゲーションシステムが搭載された車両や人間の生活環境中で移動可能なロボット等の移動体に搭載され、移動体の自己位置を推定するものである。この自己位置推定装置は、対象物を検出し、検出された対象物（物体や物体に記された記号など）をランドマークとして、ランドマークと地図データとを照合し、照合できたランドマークに基づいて、移動体の自己位置を推定する。地図データは、ランドマークの絶対的な位置および特徴に関する情報を含むものである。

図１は、本発明の第１の実施形態に係る自己位置推定装置を示すシステム構成図である。図１に示すように、自己位置推定装置１は、ランドマークとして用いられる対象物を検出するステレオカメラ２と、自己位置推定処理を実行する自己位置推定用電子制御ユニット（以下、自己位置推定ＥＣＵという）３とを備えている。また、自己位置推定ＥＣＵ３は、データベース１１、ランドマーク抽出部１２、地図照合部１３、および自己位置推定部１４を備えている。

ステレオカメラ２は、周囲の実環境を撮像し、この実環境からランドマークを抽出するための画像データを取得するものである。ステレオカメラ２は、取得した画像データを自己位置推定ＥＣＵ３におけるランドマーク抽出部１２に出力する。

自己位置推定ＥＣＵ３におけるデータベース１１は、ランドマークが設定されている領域（屋外や部屋の中など）の地図、この地図上における複数のランドマークの位置、これらのランドマークのそれぞれの特徴量を記憶している。また、データベース１１は、それぞれのランドマークを観測可能な範囲であり、ランドマークごとに決定された検出範囲である観測可能範囲を記憶している。データベース１１からは、ランドマーク抽出部１２に対してランドマークの特徴量が出力される。また、地図照合部１３に対して、地図上における複数のランドマークの位置およびそれぞれの観測可能範囲を出力する。データベース１１は、検出範囲記憶手段を構成する。

ランドマーク抽出部１２においては、ステレオカメラ２から出力された画像およびデータベース１１から出力されたランドマーク特徴量に基づいて、ステレオカメラ２から出力された画像内におけるランドマークを抽出する。ここで、ステレオカメラ２から出力された画像内におけるランドマークを抽出する際に、画像特徴ＳＩＦＴ（Scale-Invariant Feature Transform、以下「ＳＩＦＴ」という）が用いられる。

データベース１１には、複数のランドマークが登録されており、登録されたランドマークのランドマーク特徴量として、ランドマークをカメラで撮影した際の画像が記憶されている。ＳＩＦＴによるランドマーク抽出では、画像中における物体の角度と大きさの変化に対して不変な特徴量を抽出し、この特徴量とランドマーク特徴量とを比較し、両者の一致度（本発明の信頼度、以下「ランドマーク一致度」という）を算出する。このランドマーク一致度が所定のしきい値以上となったときに、この物体をランドマークとして抽出する。ランドマーク一致度が本発明のランドマーク信頼度となる。

ここで、ランドマークを撮影した際のカメラのカメラ姿勢から±３０度程度ずれると、ランドマークの対応付けが困難になり、ランドマークの抽出が非常に困難となる。したがって、逆に考えると、ＳＩＦＴによってランドマークを抽出することができた場合には、ステレオカメラ２の位置（自己位置）が抽出されたランドマークから±３０度の範囲にあるといえる。したがって、ＳＩＦＴによってランドマークが抽出されることにより、自己位置の自己存在可能範囲を限定されることとなる。

また、ランドマーク抽出部１２は、ランドマークを抽出した場合、ステレオカメラ２から出力された画像に基づいて、抽出したランドマークと自己位置との距離を算出する。抽出したランドマークと自己位置との距離の算出は、たとえば三角法によって行うことができる。さらに、ランドマーク抽出部１２は、ランドマーク一致度を算出する。このランドマーク一致度が所定のしきい値以上となった場合に、抽出したランドマークにランドマーク一致度を付加し、抽出したランドマークと自己位置との距離とともに地図照合部１３に出力する。

地図照合部１３は、ランドマーク抽出部１２から出力されたランドマークをデータベース１１から出力された地図上における複数のランドマークの位置に照合し、地図上におけるランドマーク位置を検出する。また、地図照合部１３では、地図上におけるランドマーク位置を検出したら、ランドマーク抽出部１２から出力された距離およびデータベース１１から出力された観測可能範囲に基づいて、自己が存在可能な範囲であり、抽出されたランドマークに対応する自己存在可能範囲を推定する。ここで、地図照合部１３では、ランドマーク抽出部１２から出力された距離情報が短い場合には、距離情報が長い場合よりも自己存在可能範囲が狭くなるように自己存在可能範囲を推定する。地図照合部１３は、複数のランドマークに対応して推定した自己存在可能範囲およびこれらのランドマークにおけるランドマーク一致度を自己位置推定部１４に出力する。

自己位置推定部１４では、地図照合部１３から出力された自己存在可能範囲に複数の自己位置の候補となる自己位置候補点を設定し、この自己位置候補点に対する投票を行う。ここで、自己位置候補点に投票を行うにあたり、投票を行うランドマークとして、ランドマーク一致度が最も高いランドマークおよび次にランドマーク一致度が高いランドマークを採用する。自己位置推定部１４では、これらのランドマークに対応する自己存在可能範囲における自己位置候補点から順番に自己位置候補点に対する投票を行う。

その結果、投票値が所定のしきい値、本実施形態では２票以上である場合に自己位置候補点を自己位置と推定する。また、ランドマーク一致度が最も高いランドマークに対応する自己存在可能範囲に設定された自己位置候補点の投票値が所定のしきい値未満である場合には、投票を行うランドマークとして、ランドマーク一致度が次に高いランドマークを採用する。このランドマーク一致度が次に高いランドマークに対応する自己存在可能範囲をランドマーク一致度が最も高いランドマークおよび次に高いランドマークに対応する自己存在可能範囲に加えて投票を行う。以後、自己位置候補点における投票値の最高値が所定のしきい値以上となるまでこの作業を繰り返す。

次に、本実施形態に係る自己位置推定装置における自己位置の推定手順について説明する。図２は、本実施形態に係る自己位置推定装置における自己位置推定手順を示すフローチャートである。

図２に示すように、本実施形態に係る自己位置推定装置１においては、ステレオカメラ２で撮影されて出力された画像をランドマーク抽出部１２に入力する（Ｓ１）。このとき、ランドマーク抽出部１２には、データベース１１からランドマークの特徴量が出力される。ランドマーク抽出部１２では、ステレオカメラ２から出力された画像およびデータベース１１から出力されたランドマークの特徴量に基づいて、画像中における物体の特徴量と、ランドマーク特徴量との一致度（ランドマーク一致度）を求める（Ｓ２）。ここで求めた一致度が所定のしきい値以上である場合に、画像中におけるこの物体をランドマークとして抽出する（Ｓ３）。ランドマーク一致度の算出およびランドマークの抽出には、ＳＩＦＴが用いられる。

ＳＩＦＴにおいては、ランドマークを登録した地点からカメラの姿勢がある程度の角度、たとえば±３０度程度以上変化すると、そのランドマークとの対応付けが非常に困難となってしまう。たとえば、図３（ａ）に示す物体Ｍの表面に記された図形をランドマークＲとすると、ランドマークＲを正面に見た位置から９０度ずれた位置から見た物体Ｍは、図３（ｂ）に示すようになる。この位置から撮影した画像では、ランドマークＲが撮影されず、ランドマークを抽出することができない。

また、ランドマークＲを正面に見た位置から時計周り方向を正として、＋３０度、０度、−３０度程度周った位置から見た物体Ｍは、それぞれ図３（ｃ）、（ｄ）、（ｅ）に示すようになる。図３（ｃ）〜（ｅ）に示すように、ランドマークＲを正面から見た位置から±３０程度周った位置からは、ランドマークＲを抽出することができるが、これより大きく周った位置からは、ランドマークＲを抽出することができなくなる。こうして、ランドマークの抽出を行う。

ランドマークの抽出が済んだら、抽出したランドマークとステレオカメラ２が設置された自己位置との距離を算出する（Ｓ４）。この距離の算出は、抽出されたすべてのランドマークに対して行い、その手法として三角法を用いる。こうして、ランドマークの抽出およびランドマークと自己位置との距離の算出が済んだら、ランドマーク抽出部１２は、抽出したランドマークおよびランドマークと自己位置との距離を地図照合部１３に出力する。また、地図照合部１３には、データベース１１から地図、この地図上におけるランドマークの位置、および観測可能範囲が出力される。

地図照合部１３では、ランドマーク抽出部１２から出力されたランドマークおよび地図上におけるランドマークの位置を照合し（Ｓ５）、地図上における抽出されたランドマークの位置を特定する。ランドマークの位置を特定したら、各ランドマークとの距離およびランドマークを観測可能範囲に基づいて、自己存在可能範囲を推定する（Ｓ６）。自己存在可能範囲は、次のようにして推定される。図４は、ランドマークＲが抽出された領域を平面視する図である。図４において、抽出されたランドマークＲの観測可能範囲Ｅは、データベース１１に予め記憶された範囲であり、ランドマークＲを登録した際に撮影されたランドマークＲの位置から所定の範囲、本実施形態では±３０度の範囲に設定されている。このため、観測可能範囲Ｅとしては、中心角が６０度とされた扇形が設定されている。

このランドマークＲの観測可能範囲Ｅから、ランドマークＲとの距離に基づいて自己存在可能範囲を絞り込むことによって自己存在可能範囲Ｐを推定する。自己存在可能範囲Ｐとしては、観測可能範囲Ｅにおける厚さを持った円弧を推定する。自己存在可能範囲Ｐを推定するにあたり、まず、円弧の半径Ｌを推定する。この半径Ｌは、自己位置とランドマークＲとの距離に基づいて推定され、自己位置とランドマークＲとの距離と比例した値とされる。ここでの円弧の半径Ｌとは、厚さを有する円弧のうち、もっとも内側の線までの距離をいう。

このように、自己存在可能範囲Ｐを設定するにあたり、円弧状の観測可能範囲Ｅを設定し、自己位置とランドマークＲとの距離と比例した値の半径Ｌを設定して自己存在可能範囲Ｐを設定する。このため、自己位置からランドマークＲまでの距離が短い場合に、自己位置からランドマークＲまでの距離が長い場合と比較して、自己存在可能範囲Ｐが狭く推定される。

円弧の半径Ｌを求めたら、円弧の厚さを求める。ここで、ステレオカメラ２で撮影された画像では、ランドマークと自己位置との距離が長い場合、その距離が短い場合よりも距離の計測誤差が大きくなる傾向にある。この計測誤差を考慮して、円弧の厚さＴは、自己位置とランドマークＲとの距離に基づいて求め、自己位置とランドマークＲとの距離が短い場合に、自己位置とランドマークＲとの距離が長い場合と比較して、円弧の厚さＴを小さく設定する。地図照合部１３は、推定した自己存在可能範囲Ｐを自己位置推定部１４に出力する。

自己存在可能範囲Ｐの推定が済んだら、自己位置推定部１４においては、出力された自己存在可能範囲Ｐ内に自己位置候補点を設定する（Ｓ７）。自己位置候補点は、ランドマーク一致度が最も高いランドマークおよびランドマーク一致度が次に高いランドマークに対応する自己存在可能範囲内Ｐから設定する。ここでの自己位置候補点は、たとえば自己存在可能範囲Ｐを格子状に区切って形成される各点（領域）とすることもができるし、自己存在可能範囲Ｐ内における各画素あるいは数個の画素の集合体を自己位置候補点とすることもできる。

自己位置候補点を設定したら、自己位置候補点に対する投票を行う（Ｓ８）。自己位置候補点に対する投票では、各自己位置候補点における投票値をそれぞれ投票する。このとき、各自己位置候補点に対して、同等の数値となる投票値を投票する。続いて、各自己位置候補点における投票値がしきい値を以上となるか否かを判断する（Ｓ９）。

ここで、ランドマーク一致度が最も高いランドマークおよびランドマーク一致度が次に高いランドマークに対応する自己存在可能範囲Ｐが重複する場合には、その重複位置に対応する自己位置候補点における投票値がしきい値以上となる。この場合に、その重複位置に対応する自己位置候補点を自己位置として推定する（Ｓ１０）。

一方、ランドマーク一致度が最も高いランドマークおよびランドマーク一致度が次に高いランドマークに対応する自己存在可能範囲Ｐが重複しない場合には、投票値がしきい値以上とはならない。この場合には、ステップＳ６に戻り、自己存在可能範囲を推定する。このときに、ランドマーク一致度が次に高いランドマークにおける自己存在可能範囲を追加して採用する。以後、ステップＳ９において、投票値がしきい値以上となる自己位置候補点が決定されるまで、上記の工程を繰り返す。

このように、本実施形態に係る自己位置推定装置においては、自己位置を推定するための自己存在可能範囲として、ランドマーク一致度が高いランドマークに対応する自己存在可能範囲から順に採用している。このため、高い精度で自己位置を推定することができる。たとえば、図５に示すように、ランドマーク一致度が高い順に４つのランドマークＲ１〜Ｒ４が抽出された場合を想定する。このとき、各ランドマークＲ１〜Ｒ４にそれぞれ対応する自己存在可能範囲Ｐ１〜Ｐ４が設定される。ここで、ランドマークＲ１〜Ｒ４について、そのランドマーク一致度によらずに各ランドマークＲ１〜Ｒ４にそれぞれ対応する自己存在可能範囲Ｐ１〜Ｐ４を用いて投票を行うと、ランドマーク一致度が低いランドマークＲ３，Ｒ４に対応する自己存在可能範囲Ｐ３，Ｐ４の影響を受けて、実際の自己位置とは異なる自己位置候補点Ｘ２の投票値が高くなってしまう。その結果、実際の自己位置とは異なる自己位置候補点Ｘ２が自己位置として推定されてしまう。

これに対して、ランドマーク一致度が高いランドマークに対応する自己存在可能範囲から順に採用して自己位置を推定すると、ランドマーク一致度が高いランドマークＲ１，Ｒ２に対応する自己存在可能範囲Ｐ１，Ｐ２基づいて自己位置が推定される。このとき、ランドマーク一致度が低いランドマークＲ３，Ｒ４に対応する自己存在可能範囲Ｐ３，Ｐ４の影響がなくなり、ランドマークＲ１，Ｒ２の自己存在可能範囲Ｐ１，Ｐ２が重複する自己位置候補点Ｘ１が実際の自己位置と一致する自己位置として推定される。このように、ランドマーク一致度が低いランドマークＲ３、Ｒ４に対応する自己存在可能範囲Ｐ３，Ｐ４の影響を小さくすることができる。したがって、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識することができ、もって高い精度で自己位置を推定することができる。

次に、本発明の第２の実施形態について説明する。本実施形態に係る自己位置推定装置は、上記第１の実施形態と比較して、システム構成は上記第１の実施形態と同様であり、自己位置推定部１４における自己位置候補点に対する投票を行う際の手順が主に異なる。以下、その相違点を中心に本実施形態に係る自己推定装置による自己位置の推定手順について主に説明する。

上記第１の実施形態では、自己位置候補点に対する投票の際、各自己位置候補点における投票値として、各自己位置候補点に対して、同等の数値となる投票値を投票していた。これに対して、本実施形態に係る自己位置推定装置では、自己位置候補点における投票値を、自己位置存在可能範囲に対応するランドマークにおけるランドマーク一致度の高さに応じた値とする。ここでは、ランドマーク一致度が高いランドマークに対応する自己位置存在範囲における自己位置候補点の方が、ランドマーク一致度が低いランドマークに対応する自己位置存在範囲における自己位置候補点よりも高い投票値が投票される。かかる態様によっても、自己位置を推定する際に、ランドマーク一致度が低いランドマークの影響を小さくすることができる。

具体的に、図５に示す例において、第１ランドマークＲ１の信頼度が５、第２ランドマークＲ２の信頼度が４、第３，第４ランドマークＲ３，Ｒ４の信頼度がそれぞれ１であったとする。また、自己位置候補点に投票する投票値として、第１ランドマークＲ１に対応する自己存在可能範囲Ｐ１における自己位置候補点に５が投票される。同様に、第２ランドマークＲ２に対応する自己存在可能範囲Ｐ２における自己位置候補点に４が投票され、第３，第４ランドマークＲ３，Ｒ４に対応する自己存在可能範囲Ｐ３，Ｐ４における自己位置候補点にそれぞれ１が投票される。

この例の場合、第１ランドマークＲ１に対応する自己存在可能範囲Ｐ１と第２ランドマークＲ１に対応する自己存在可能範囲Ｐ２との重複する自己位置候補点Ｘ１では、合計で９の投票値が得られる。その一方、第２〜第４ランドマークＲ２〜Ｒ４に対応する自己存在可能範囲Ｐ２〜Ｐ４が重複する自己位置候補点Ｘ２では、合計で６の投票値が得られることになる。この結果から、実際の自己位置と一致する点を自己位置候補点Ｘ１として推定することができる。

このように、自己位置候補点における投票値を、自己位置存在可能範囲に対応するランドマークにおけるランドマーク一致度の高さに応じた値とすることにより、ランドマーク一致度が低いランドマークに対応する自己存在可能範囲の影響を小さくすることができる。したがって、複数のランドマークが抽出された際に、異なるランドマーク同士を精度良く認識することができ、もって高い精度で自己位置を推定することができる。

次に、本発明の第３の実施形態について説明する。本実施形態に係る自己位置推定装置は、上記第１の実施形態と比較して、システム構成は上記第２の実施形態と同様であり、地図照合部１３における自己存在可能範囲の推定手順が主に異なる。その相違点を中心に本実施形態に係る自己位置推定装置に自己位置推定手順について説明する。

上記第２の実施形態では、自己存在可能範囲を推定する際、自己存在可能範囲を一定の大きさ（中心角が６０度とされた扇形）としていたが、本実施形態では、自己存在可能範囲の推定を行う際、ランドマーク一致度が用いられる。具体的には、ランドマーク一致度が高いほど自己存在可能範囲を狭くする。ＳＩＦＴにおけるランドマークの抽出では、視点変化が小さいほど、ランドマーク一致度が高くなる性質がある。ランドマーク一致度が低くなる要因としては、視点変化のほかに、照明変動、隠れの影響等があるが、ランドマーク一致度が高い場合には、視点変化が少ない可能性が高くなっている。この視点変化が小さいほど、ランドマーク一致度が高くなる性質を利用し、ランドマーク一致度が高いランドマークに対応する自己存在可能範囲については、その自己存在可能範囲を狭くする。逆にランドマーク一致度が低いランドマークに対応する自己存在可能範囲については、その自己存在可能範囲を広くする。

たとえば、ランドマーク一致度が所定のしきい値未満である場合には、図６（ａ）に示すように、自己存在可能範囲ＰをランドマークＲの位置を中心として中心角が６０度の範囲に設定して自己存在可能範囲を広くする。また、ランドマーク一致度が所定のしきい値以上である場合には、図６（ｂ）に示すように、自己存在可能範囲ＰをランドマークＲの位置を中心として中心角が１０度の範囲に設定して自己存在可能範囲を狭くする。あるいは、ランドマーク一致度の度合いによって中心角を変化させ、ランドマーク一致度が高くなるほど中心角を小さくさせる。

このように、ランドマーク一致度が高いほど自己存在可能範囲を狭くすることにより、自己存在可能範囲同士の重なり合いが少なくなる。その結果、自己存在可能範囲が重なった部分を誤って現在位置と推定することを回避することができる。

以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。たとえば、上記第１の実施形態では、上記実施形態では、抽出したランドマークのすべてについてランドマークまでの距離を算出し、自己存在可能範囲を推定しているが、ランドマーク一致度が高く、自己位置候補点に対する投票を行う際に用いられるランドマークのみについてランドマークまでの距離を算出し、自己存在可能範囲を推定する態様とすることもできる。

また、上記各実施形態ではランドマークの抽出を行う際にＳＩＦＴを用いているが、指向性ＲＦＩＤタグなどを用いた抽出を行うこともできる。この場合、ランドマーク検出手段としては、ＲＦＩＤタグリーダが用いられる。他方、ランドマーク検出手段としてはレンジファインダなどを用いることもできる。

さらに、自己位置とランドマークとの距離を求めるために三角法を用いているが、多重解像度テンプレートなどを用いることもできる。さらに、上記実施形態においては、自己位置推定装置をロボット等の移動体に適用しているが、たとえば、乗用車等のその他の移動体に適用してもよい。

第１の実施形態に係る自己位置推定装置を示すシステム構成図である。 第２の実施形態に係る自己位置推定装置における自己位置推定手順を示すフローチャートである。 （ａ）はランドマークが表示された物体を示す模式図、（ｂ）〜（ｅ）は、それぞれ角度を代えて見たランドマークを示す模式図である。 ランドマークが抽出された領域を平面視する図である。 複数のランドマークが抽出された際のランドマークと、自己存在可能範囲とを示す図である。 （ａ）は、ランドマーク一致度が低い場合における広い自己存在可能範囲を示す図、（ａ）は、ランドマーク一致度が高い場合における狭い自己存在可能範囲を示す図である。

符号の説明

１…自己位置推定装置、２…ステレオカメラ、３…自己位置推定ＥＣＵ、１１…データベース、１２…ランドマーク抽出部、１３…地図照合部、１４…自己位置推定部、Ｐ，Ｐ１〜Ｐ４…自己存在可能範囲、Ｒ，Ｒ１〜Ｒ４…ランドマーク、Ｘ１，Ｘ２…自己位置候補点。

Claims (3)

1. ランドマークを検出するランドマーク検出手段と、
   ランドマークの特徴量を記憶するランドマーク特徴量記憶手段と、
   前記ランドマーク検出手段によって検出されたランドマークと、ランドマーク特徴量記憶手段に記憶されたランドマークの特徴量と一致度であるランドマーク一致度を取得する一致度取得手段と、
   前記自己位置から前記ランドマークまでの距離を検出する距離検出手段と、
   前記ランドマークを検出可能とされる検出範囲を、複数の前記ランドマークにそれぞれ対応させて取得する検出範囲取得手段と、
   前記距離検出手段によって検出された距離と前記検出範囲取得手段によって取得された検出範囲とに基づいて、自己が存在可能である自己存在可能範囲を推定する自己存在可能範囲推定手段と、
   複数の前記ランドマークにそれぞれ対応する前記自己存在可能範囲内における自己位置の候補となる自己位置候補点を設定する自己位置候補点設定手段と、
   前記自己位置候補点に対して投票を行う投票手段と、
   前記投票手段の投票結果が所定となったときに、前記しきい値を超えた自己位置候補点を自己位置と推定する自己位置推定手段と、を備え、
   前記投票手段は、投票値が所定の値となるまで前記ランドマーク一致度が高いランドマークに対応する自己存在可能範囲における前記自己位置候補点から順番に投票を行うことを特徴とする自己位置推定装置。
2. 前記投票手段は、前記ランドマーク一致度が高いランドマークに対応する自己存在可能範囲における前記自己位置候補点についての投票値の大きさを、前記ランドマーク一致度が低いランドマークに対応する自己存在可能範囲における前記自己位置候補点についての投票値の大きさよりも大きくする請求項１に記載の自己位置推定装置。
3. 前記自己位置存在可能範囲推定手段は、前記ランドマーク一致度が高いランドマークについての自己存在可能範囲の広さを、前記ランドマーク一致度が低いランドマークについての自己存在可能範囲の広さよりも狭く推定する請求項１または請求項２に記載の自己位置推定装置。

Images