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

Description

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

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

しかし、上記非特許文献１に記載された従来技術では、ランドマークを観測することができるすべての範囲をランドマークが観測できる点としている。このため、ランドマークが観測できる点が含まれる範囲はたとえば楕円形となるが、ランドマークが低い位置に設定されていると、ランドマークが観測できる点が非常に広範囲にわたってしまい、その分精度が低くなるという問題があった。

また、自己位置の推定位置を決定するにあたり、ランドマークが観測できる位置の投票値がもっとも高くなった位置を現在の位置と推定している。このため、自己位置を確定するためには、多数のランドマークを設定しなければならないという問題もあった。

そこで、本発明の課題は、ランドマークが低い位置に設定されていたり、検出されるランドマークの数が少なかったりする場合でも、高い精度で自己位置を推定することができる自己推定装置を提供することにある。

上記課題を解決した本発明に係る自己位置推定装置は、ランドマークを検出するランドマーク検出手段と、自己位置からランドマークまでの距離を検出する距離検出手段と、ランドマークを検出可能とされる範囲としてランドマークごとに決定された検出範囲を記憶する検出範囲記憶手段と、距離検出手段によって検出された距離と検出範囲記憶手段に記憶された検出範囲とに基づいて、自己が存在可能であり、複数の自己位置候補点を含む自己存在可能範囲を推定する自己存在可能範囲推定手段と、自己存在可能範囲推定手段で推定した自己存在可能範囲に基づいて自己位置を推定する自己位置推定手段と、を備え、自己位置推定手段は、ランドマーク検出手段によって複数のランドマークが検出された際、複数のランドマークのそれぞれに対して推定された複数の自己存在可能範囲における各自己存在候補位置に投票を行い、投票の結果に基づいて、自己位置を推定するものであり、自己存在可能範囲推定手段は、自己位置からランドマークまでの距離が短い場合に、自己位置からランドマークまでの距離が長い場合と比較して、自己存在可能範囲を狭く推定するものである。

自己位置からランドマークを見る場合、自己位置とランドマークとの距離が短い場合には、その距離が長い場合と比較して狭い範囲からしかランドマークを見ることができない。この特性を利用して、本発明においては、自己位置からランドマークまでの距離が短い場合に、自己位置からランドマークまでの距離が長い場合と比較して、自己存在可能範囲を狭く推定している。したがって、自己位置とランドマークとの距離に応じて自己存在可能範囲が限定されることになるので、ランドマークが低い位置に設定されていたり、検出されるランドマークの数が少なかったりする場合でも、高い精度で自己位置を推定することができる。

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

このように、複数のランドマークのそれぞれに対して推定された自己存在可能範囲に基づいて、自己位置を推定することにより、自己位置をさらに絞り込むことができるので、その分精度よく自己位置を推定することができる。

また、自己存在可能範囲推定手段は、距離検出手段で検出されたランドマークまでの距離との比例した値に検出範囲を調整することによって、自己存在可能範囲を推定する態様とすることができる。
また、自己位置推定手段は、自己存在可能範囲推定手段で推定された自己存在可能範囲における各位置に重み付けを行い、自己存在可能範囲の中心に近い位置では、自己存在可能範囲の中心に遠い位置と比較して、自己位置推定に用いる重み付けを大きくする態様とすることができる。

自己位置推定に重み付けを用いることにより、自己存在可能範囲内における自己位置をさらに狭い範囲に絞り込むことができる。このとき、重み付けを行うにあたり、自己存在可能範囲の中心に近い位置の方が、自己存在可能範囲の中心に遠い位置よりも自己位置である可能性が高い。したがって、自己存在可能範囲の中心に近い位置では、自己存在可能範囲の中心に遠い位置と比較して、自己位置推定に用いる重み付けを大きくすることにより、さらに精度よく自己位置を推定することができる。

さらに、自己位置推定手段は、自己存在可能範囲推定手段で推定された自己存在可能範囲における各位置に重み付けを行い、ランドマークに近い位置では、ランドマークから遠い位置と比較して、自己位置推定に用いる重み付けを大きくする態様とすることもできる。

ランドマークに近い位置では、ランドマークから遠い位置と比較して一般に距離検出の精度が高くなっている。このため、ランドマークに近い位置では、ランドマークから遠い位置と比較して、自己位置推定に用いる重み付けを大きくすることにより、さらに精度よく自己位置を推定することができる。

また、自己位置推定手段は、自己存在可能範囲推定手段で推定された自己存在可能範囲に基づいて、自己位置を推定できるか否かを判定し、自己位置推定手段が自己位置を推定できないと判定した場合に、自己存在可能範囲推定手段は、自己存在可能範囲を拡張する態様とすることもできる。

ランドマークを抽出する際は、ランドマークを誤認識することがあり、誤認識されたランドマークを用いると、自己存在可能範囲推定、さらには自己位置推定の精度を著しく低下させてしまう。ここで、誤認識されたランドマークは、不確定な位置に存在し、正しく抽出されたランドマークの位置のように収束しない。この性質を利用して、まず狭い範囲で自己位置の推定を行い、自己位置の推定ができないと判定した場合には、自己存在可能範囲を拡張することにより、誤認識されたランドマークに基づいて推定された自己存在可能範囲の影響を抑制することができ、もって精度よく自己位置を検出することができる。

本発明に係る自己位置推定装置によれば、ランドマークが低い位置に設定されていたり、検出されるランドマークの数が少なかったりする場合でも、高い精度で自己位置を推定することができる。

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

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

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

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

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

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

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

さらに、ランドマーク抽出部１２においては、ランドマークを抽出した場合、ステレオカメラ２から出力された画像に基づいて、抽出したランドマークと自己位置との距離を算出する。抽出したランドマークと自己位置との距離の算出は、たとえば三角法によって行うことができる。このように、ランドマーク抽出部１２は、ランドマーク検出手段および距離検出手段を構成する。ランドマーク抽出部１２は、抽出したランドマークおよび抽出したランドマークと自己位置との距離を地図照合部１３に出力する。

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

自己位置投票部１４では、地図照合部１３から出力された自己存在可能範囲に自己位置候補点を設定し、各自己位置候補点に対する投票を行う。自己位置投票部１４は、自己位置候補点に対して行った投票の投票結果を自己位置推定手段である自己位置推定部１５に出力する。自己位置推定部１５では、自己位置投票部１４から出力された投票結果に基づいて、自己位置を推定する。

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

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

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

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

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

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

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

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

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

自己存在可能範囲Ｐの推定が済んだら、自己位置投票部１４においては、出力された自己存在可能範囲Ｐ内に自己位置候補点を設定する（Ｓ６）。ここでの自己位置候補点は、たとえば自己存在可能範囲Ｐを格子状に区切って形成される各点（領域）とすることもができるし、自己存在可能範囲Ｐ内における各画素あるいは数個の画素の集合体を自己位置候補点とすることもできる。自己位置候補点を設定したら、各自己位置候補点に対して同一の投票値を付与するとともに、これらの投票値に対して、重み付けを行う（Ｓ７）。

自己位置候補点の投票値に対して重み付けを行う際、図５に示すように、推定した自己存在可能範囲Ｐに対して中心部ほど高く、縁部に向かうにしたがって低くなる重みを付す。図５には、重みが大きい位置ほど濃くなるグラデーションを付すことにより、重み付けの割合を表現している。ＳＩＦＴによってランドマークの抽出を行った際、ランドマークの抽出（マッチング）精度は、視点変化が大きくなるにしたがって低下する。このため、図６に示すように、自己位置としては、ランドマークＲを登録した際にランドマークＲを撮影した位置に近い位置に存在する可能性が高いことになる。このため、図５に示すように、推定した自己存在可能範囲Ｐに対して中心部ほど高く、縁部に向かうにしたがって低くなる重みを各自己位置候補点の投票値に付すことにより、自己位置の推定精度を向上させることができる。

また、上述のとおり、ステレオカメラ２で撮影された画像では、ランドマークと自己位置との距離が長い場合、その距離が短い場合よりも距離の計測誤差が大きくなる傾向にある。このため、さらに、重み付けを行うにあたり、図７（ａ）に示すように、ランドマークＲ１と自己位置との距離が長い場合、図７（ｂ）に示すように、ランドマークＲ２と自己位置との距離が短い場合よりも小さな重みを各自己位置候補点の投票値に付する。このように重み付けを行うことにより、さらに精度よく自己位置を検出することができる。

こうして、自己位置候補点の投票値に重み付けを行ったら、自己位置候補点に対する投票を行う（Ｓ８）。自己位置候補点に対する投票では、各自己位置候補点における重み付けられた投票値をそれぞれ投票する。したがって、図４に示すように、自己存在可能範囲Ｐに重複が生じない場合には、自己位置候補点における投票値がそのまま反映されて自己位置推定が行われる。また、図８に示すように、複数、たとえば２つのランドマークＲ１，Ｒ２が抽出され、それぞれのランドマークＲ１、Ｒ２に対する自己存在可能範囲Ｐ１，Ｐ２が推定され、これらの自己存在可能範囲Ｐ１、Ｐ２が互いに重複する場合には、重複する重複自己存在可能範囲ＰＸにおいて、重複する自己位置候補点が存在する。これらの重複する自己位置候補点については、複数の投票値が投票される。このように、ランドマークが複数抽出された際に、その重複部分となる重複自己存在可能範囲ＰＸの自己位置候補点に複数の投票値が投票されることにより、自己位置候補点が存在しうる重複自己存在可能範囲ＰＸをさらに絞り込むことができるので、その分精度よく自己位置を検出することができる。もちろん、さらに多数のランドマークが抽出された場合には、さらに多くの重複する自己位置候補点が存在するが、これらの重複する自己位置候補点についても同様に複数の投票値が投票される。

自己位置候補点に対する投票が済んだら、自己位置投票部１４は、投票結果を自己位置推定部１５に出力する。自己位置推定部１５では、投票結果に基づいて自己位置を推定する（Ｓ９）。具体的には、各自己位置候補点に投票された投票値を加算し、もっとも大きな値となった自己位置候補点を自己位置として推定する。こうして、自己位置推定装置における自己位置推定を終了する。

このように、本実施形態に係る自己位置推定装置１では、ステレオカメラ２による撮影が行われた位置である自己位置を推定するにあたり、自己位置からランドマークＲまでの距離が短い場合に、自己位置からランドマークＲまでの距離が長い場合と比較して、自己存在可能範囲Ｐが狭く自己存在可能範囲Ｐを推定している。このため、自己位置とランドマークＲとの距離に応じて自己存在可能範囲Ｐが限定されることになるので、ランドマークＲが低い位置に設定されていたり、検出されるランドマークＲの数が少なく、たとえば１つのみであったりする場合でも、高い精度で自己位置を推定することができる。

また、本実施形態に係る自己位置推定装置１では、複数のランドマークＲ１、Ｒ２が抽出された場合に、その重複部分には、複数の投票値が投票される。このため、複数のランドマークが抽出された場合、自己位置をさらに精度よく推定することができる。さらに、本実施形態に係る自己位置推定装置１では、自己位置を推定するための投票値に重み付けを行っている。このため、自己位置をさらに狭い範囲まで精度よく推定することができる。しかも、自己位置とランドマークＲとの距離が短い場合には、その距離が長い場合よりも投票値に付与する重みを大きくしている。このため、さらに精度よく自己位置を推定することができる。

また、上記第一の実施形態においては、複数のランドマークが抽出された場合でも、複数のランドマークについては同等に扱うようにしていたが、複数のランドマークが抽出された場合に、自己位置から遠いランドマークよりも、自己位置に近いランドマークを優先的に利用する態様とすることもできる。たとえば、図９に示すように、第１ランドマークＲ１〜第４ランドマークＲが抽出された場合、第１ランドマークＲ１よりも第２ランドマークＲ２の方が自己位置に近く、第３ランドマークＲ３よりも第４ランドマークＲ４の方が自己位置に近い場合には、第１ランドマークＲ１に対する自己存在可能範囲Ｐ１と第３ランドマークＲ３に対する自己存在可能範囲Ｐ３との重複自己存在可能範囲ＰＸ１よりも、第２ランドマークＲ２に対する自己存在可能範囲Ｐ２と第４ランドマークＲ４に対する自己存在可能範囲Ｐ４との重複自己存在可能範囲ＰＸ２を優先的に利用するようにする。

ランドマークと自己位置との関係は、上述のように、自己位置から近い位置のランドマークの方が、距離の計測誤差が小さくなる傾向にある。したがって、ランドマークが複数抽出された場合には、自己位置から遠いランドマークよりも、自己位置に近いランドマークを優先的に利用する態様とすることにより、さらに精度よく自己位置を推定することができる。

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

図１０に示すように、本実施形態に係る自己位置推定装置においては、ステレオカメラ２で撮影されて出力された画像をランドマーク抽出部１２に入力し（Ｓ１１）、次に、ランドマーク抽出部１２において、ステレオカメラ２から出力された画像およびデータベース１１から出力されたランドマークの特徴量に基づいて、画像内におけるランドマークを抽出する（Ｓ１２）。続いて、抽出したランドマークとステレオカメラ２が設置された自己位置との距離を算出し（Ｓ１３）、その後、地図照合部１３において、ランドマーク抽出部１２から出力されたランドマークおよび地図上におけるランドマークの位置を照合する（Ｓ１４）。ここまでは、上記第一の実施形態と同様の手順が進行する。

さらに、各ランドマークとの距離およびランドマークを観測可能範囲に基づいて、自己存在可能範囲を推定する（Ｓ１５）。ここで、本実施形態における自己存在可能範囲の推定を行う際、ＳＩＦＴで抽出可能とされるランドマークを撮影した位置から±３０度の範囲よりも狭い範囲に観測可能範囲を設定し、この狭い範囲に設定された観測可能範囲内から、ランドマークとの距離に基づいて、自己存在可能範囲を推定する。

ＳＩＦＴによってランドマークの検出を行う際、データベース１１に登録されたランドマークを抽出する一方で、データベース１１に登録されたランドマークに類似する異なるランドマーク（以下「異ランドマーク」という）を誤認識して抽出してしまうことがある。このような異ランドマークを誤認識して抽出してしまうと、自己位置推定の精度が低下してしまう。

たとえば、図１１に示すように、２つのランドマークＲ１，Ｒ２と２つの異ランドマークＲ２１，Ｒ２２が抽出されたとする。この場合、２つのランドマークＲ１，Ｒ２に対する自己存在可能範囲が重複する重複自己存在可能範囲ＰＸ１に自己位置が存在する可能性が高いはずである。ところが、２つの異ランドマークＲ２１，Ｒ２２が抽出されていることから、２つの異ランドマークＲ２１，Ｒ２２が重複する重複異自己存在可能範囲ＰＸＷに自己位置が存在すると誤認識される可能性が高くなってしまう。

このような誤認識を防止するために、まずは狭い範囲で自己存在可能範囲を設定し、自己位置が推定可能でない場合には、自己存在可能範囲を拡張するようにする。正しく認識されたランドマークの自己存在可能範囲は、あるエリアに収束する傾向にあるが、誤認識された異ランドマークの自己存在可能範囲は、ばらばらに存在し、あるエリアに収束することはない傾向にある。また、ランドマークおよび異ランドマークを抽出した際には、これらがランドマークであるか異ランドマークであるかは識別できない。このため、図１２に示すように、ランドマークＲ１，Ｒ２、および異ランドマークＲ２１，Ｒ２２に対して、まずは狭い範囲で自己存在可能範囲Ｐ１，Ｐ２，Ｐ２１，Ｐ２２を設定しておく。その後の投票によって一定値以上の投票が得られ、現在位置が求まるまで自己存在可能範囲を順次拡張する。このように自己存在可能範囲を推定することにより、異ランドマークが抽出された際の影響を抑制することができ、精度よく自己位置の推定を行うことができる。

こうして、自己存在可能範囲を推定したら、上記第一の実施形態と同様にして自己位置候補点を設定し（Ｓ１６）、自己位置候補点に重み付けを行う（Ｓ１７）。それから、自己位置候補点に投票を行う（Ｓ１８）。その後、本実施形態に係る自己位置推定装置では、自己位置の推定が可能であるか否かを判定する（Ｓ１９）。自己位置の推定が可能であるか否かは、自己位置候補点における投票値の最大値が所定のしきい値を超えているか否かによって判定する。

その結果、自己位置候補点における投票値の最大値が所定のしきい値を超えておらず、自己位置の推定が可能でないと判定した場合には、ステップＳ１５で求めた自己存在可能範囲を拡張し（Ｓ２０）、ステップＳ１６に戻る。このように、自己存在可能範囲を拡張することにより、複数の自己存在可能範囲が重複して重複自己存在可能範囲ＰＸが生じる。重複自己存在可能範囲ＰＸが生じることにより、自己位置候補点における投票値の最大値が増大し、やがて所定のしきい値を超えることになる。ここで、上述のように異ランドマークはランドマークに比べてその存在エリアが収束しない傾向にあることから、異ランドマークによる重複自己存在可能範囲は生じにくいものとなる。このため、異ランドマークによる影響を抑制することができる。

そして、自己位置候補点における投票値の最大値が所定のしきい値を超えて、自己位置の推定が可能であると判定した場合には、自己位置の推定を行って（Ｓ２１）、自己位置推定を終了する。このように、本実施形態に係る自己位置推定装置では、異ランドマークを誤認識した場合でも、その影響を抑制することができるので、精度よく自己位置の推定を行うことができる。

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

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

第一の実施形態に係る自己位置推定装置を示すシステム構成図である。 第一の実施形態に係る自己位置推定装置における自己位置推定手順を示すフローチャートである。 （ａ）はランドマークが表示された物体を示す模式図、（ｂ）〜（ｅ）は、それぞれ角度を代えて見たランドマークを示す模式図である。 ランドマークが抽出された領域を平面視する図である。 自己存在可能範囲における自己候補点の重み付けを表す図である。 ランドマークを見る視点の視点変化と、自己位置の正確さの相関度との関係を示すグラフである。 （ａ）はランドマークが自己位置から遠い場合の自己候補点の重み付けを表す図、（ｂ）はランドマークが自己位置から近い場合の自己候補点の重み付けを表す図である。 複数のランドマークが抽出された際のランドマークが抽出された領域を平面視する図である。 複数のランドマークが抽出された際のランドマークと、自己存在可能範囲とを示す図である。 第一の実施形態に係る自己位置推定装置における自己位置推定手順を示すフローチャートであるで。 ランドマークおよび異ランドマークと、自己存在可能範囲とを示す図である。 自己存在可能範囲を狭く設定した場合のランドマークおよび異ランドマークと、自己存在可能範囲とを示す図である。

符号の説明

１…自己位置推定装置、２…ステレオカメラ、１１…データベース、１２…ランドマーク抽出部、１３…地図照合部、１４…自己位置投票部、１５…自己位置推定部、Ｅ…観測可能範囲、３…自己位置推定ＥＣＵ、Ｌ…半径、Ｍ…物体、Ｐ，Ｐ１〜Ｐ４…自己存在可能範囲、ＰＸ，ＰＸ１，ＰＸ２…重複自己存在可能範囲、ＰＸＷ…重複異自己存在可能範囲、Ｒ，Ｒ１〜Ｒ４…ランドマーク、Ｒ２１，Ｒ２２…異ランドマーク。

Claims (5)

1. ランドマークを検出するランドマーク検出手段と、
   自己位置から前記ランドマークまでの距離を検出する距離検出手段と、
   前記ランドマークを検出可能とされる範囲として前記ランドマークごとに決定された検出範囲を記憶する検出範囲記憶手段と、
   前記距離検出手段によって検出された距離と前記検出範囲記憶手段に記憶された検出範囲とに基づいて、自己が存在可能であり、複数の自己位置候補点を含む自己存在可能範囲を推定する自己存在可能範囲推定手段と、
   前記自己存在可能範囲推定手段で推定した自己存在可能範囲に基づいて自己位置を推定する自己位置推定手段と、を備え、
   前記自己位置推定手段は、前記ランドマーク検出手段によって複数のランドマークが検出された際、前記複数のランドマークのそれぞれに対して推定された複数の自己存在可能範囲における各自己存在候補位置に投票を行い、前記投票の結果に基づいて、自己位置を推定するものであり、
   前記自己存在可能範囲推定手段は、前記自己位置から前記ランドマークまでの距離が短い場合に、前記自己位置から前記ランドマークまでの距離が長い場合と比較して、前記自己存在可能範囲を狭く推定することを特徴とする自己位置推定装置。
2. 前記自己存在可能範囲推定手段は、前記距離検出手段で検出された前記ランドマークまでの距離との比例した値に前記検出範囲を調整することによって、前記自己存在可能範囲を推定する請求項１に記載の自己位置推定装置。
3. 前記自己位置推定手段は、前記自己存在可能範囲推定手段で推定された自己存在可能範囲における各位置に重み付けを行い、
   前記自己存在可能範囲の中心に近い位置では、前記自己存在可能範囲の中心に遠い位置と比較して、自己位置推定に用いる重み付けを大きくする請求項２に記載の自己位置推定装置。
4. 前記自己位置推定手段は、前記自己存在可能範囲推定手段で推定された自己存在可能範囲における各位置に重み付けを行い、
   前記ランドマークに近い位置では、前記ランドマークから遠い位置と比較して、自己位置推定に用いる重み付けを大きくする請求項２または請求項３に記載の自己位置推定装置。
5. 前記自己位置推定手段は、前記自己存在可能範囲推定手段で推定された自己存在可能範囲に基づいて、自己位置を推定できるか否かを判定し、
   前記自己位置推定手段が自己位置を推定できないと判定した場合に、前記自己存在可能範囲推定手段は、前記自己存在可能範囲を拡張する請求項２〜請求項４のうちのいずれか１項に記載の自己位置推定装置。

Images