JP6129981B2 - 移動体位置推定装置および移動体位置推定方法 - Google Patents

Description

本発明は、移動体位置推定装置および移動体位置推定方法に関する。

ロボットや自動車などの移動体の位置を推定する装置は、従来から知られている。従来技術では、移動体に、エンコーダやジャイロセンサなどの内界センサと、カメラや測位衛星信号受信機（ＧＰＳ）やレーザ距離センサなどの外界センサとを搭載し、これらセンサからの検出値をもとに、移動体の位置を推定する。

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

第２の特許文献では、局所地図と大域地図を備え、外界センサの観測結果から抽出したランドマーク候補と大域地図に登録されたランドマークとの照合結果を局所地図に保存して次回のランドマーク照合に利用する。第２の特許文献では、大域地図に登録されたランドマークと一致しないが、局所地図に登録したランドマーク候補と一致するランドマーク候補を新たにランドマークとして大域地図に登録する。さらに第２の特許文献では、登録されているランドマークであっても長期間にわたって観測されないものは削除する。第２の特許文献の記載の従来技術によれば、ランドマーク候補と大域地図に登録されたランドマークとの照合結果を利用することで、ランドマーク候補と大域地図に登録されたランドマークとを直接照合する数を減らし、計算量を削減できる。また、第２の特許文献に記載の従来技術によれば、ランドマーク候補の照合結果に基づいて大域地図を逐次更新するため、環境の変化に柔軟に対応することができる。

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

従来のカメラを用いる位置推定方法では、画像から検出した特徴点の変位量を基にして移動体の移動量を算出し、前回算出した位置に移動量を加算することで移動体の現在位置を推定する。しかし、この方法では、絶対位置が変動するような特徴点が検出された場合、移動体の移動量の誤差が累積する。

第１の特許文献では、ステレオ視しながらトラッキングした全特徴点の並進移動量を基に静止物体を判別し、静止物体の位置を基準とするため、自己位置の累積誤差を低減することができる。しかし、移動体が回転したときの特徴点の並進移動量は、移動体から特徴点までの距離に依存する。このため第１の特許文献では、移動体から各物体までの距離が均一でない場合に、それらの物体が静止物体であるかを判別するのは難しい。また、第１の特許文献では、特徴の少ない環境下では、必要十分な数の特徴点を抽出することができないため、静止物体の判別は難しい。

そこで、変動しない特徴点の位置が登録された地図を予め用意する手法や、エンコーダなどの内界センサとカメラを併用することで、周囲環境に依存せずに位置の累積誤差を低減する手法（第２の特許文献）が提案されている。

第２の特許文献では、位置が変動しないランドマークの地図を用意し、レーザ距離センサやカメラなどの外界センサによりランドマーク候補を検出する。第２の特許文献では、エンコーダなどの内界センサ値から算出した暫定の位置を基に、ランドマーク候補の位置を算出し、ランドマーク候補と地図を照合する。ランドマーク候補と地図を毎回照合する処理は高負荷であるため、第２の特許文献では、前回に地図と照合した前ランドマーク候補と現ランドマーク候補とを対応付けることにより、現ランドマーク候補と地図とを直接照合する数を減らしている。しかし、ランドマーク候補が同時に多数検出される環境下では、前ランドマーク候補と現ランドマーク候補との対応付けの処理負荷が増大する。

さらに、移動体の位置を推定する場合、推定処理の計算時間中に移動体が移動した量だけ位置推定精度は悪化する。特に、自動車などの高速移動体は、１回の計算時間内に移動する量が大きいため、処理負荷を低減してできるだけ高速に計算できるようにすることが肝要である。

本発明は上記課題に鑑みてなされたもので、その目的は、移動体の位置推定に要する処理の負荷を低減することができるようにした移動体位置推定装置および移動体位置推定方法を提供することにある。本発明の他の目的は、所定の基準を満たす所定の特徴点を規定数だけ確保し、それら規定数の所定の特徴点だけを追跡することで、移動体の位置を比較的少ない処理負荷で推定できるようにした移動体位置推定装置および移動体位置推定方法を提供することにある。

上記課題を解決すべく、本発明の一つの観点に従う移動体位置推定装置は、移動体に取り付けられ、周囲環境を撮像する撮像部と、周囲環境を撮像した画像から抽出される特徴点の座標を地図に対応付けて管理する地図管理部と、撮像部で撮像した画像から抽出される特徴点のうち所定の基準で選択される所定の特徴点を追跡する特徴点追跡部と、特徴点追跡部が追跡する所定の特徴点の数が規定数となるように管理する特徴点数管理部と、特徴点追跡部が追跡する所定の特徴点の座標と地図管理部の管理する地図とから位置を推定する位置推定部と、を備える。

移動体の内部状態を検出する内界状態検出部の検出値と位置推定部が推定した位置とに基づいて、移動体の暫定位置と姿勢を算出する暫定位置算出部を設け、特徴点追跡部は、暫定位置算出部で算出した暫定位置および姿勢と撮像部で撮像した画像とを用いて、所定の特徴点を追跡してもよい。

地図管理部は、周囲環境の画像と、画像を撮影したときの撮像位置および姿勢と、画像から抽出される特徴点の画像上における画像上座標と、特徴点の移動体座標系における三次元座標とを、地図に対応付けて管理することができる。

特徴点追跡部は、所定の特徴点が画像上に出現すると見込まれる出現予定座標を、暫定位置算出部の算出した暫定位置に基づいて算出し、出現予定座標を含んで設定される所定領域内の画像から特徴点を抽出し、抽出した特徴点と所定の特徴点とのマッチング尤度を算出し、所定領域内で抽出した特徴点のうちマッチング尤度が最大となる特徴点を所定の特徴点として使用することができる。

特徴点数管理部は、所定の特徴点の数が規定数に満たないと判定した場合、地図管理部で管理された周囲環境の画像と撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、所定の基準を満たす特徴点を、新たな所定の特徴点として追加することもできる。

特徴点数管理部は、所定の特徴点の数が規定数であると判定した場合、各所定の特徴点が所定の基準を満たしているか確認し、所定の基準を満たしていない特徴点は所定の特徴点ではないと判断して特徴点追跡部による追跡対象から除外し、地図管理部で管理された周囲環境の画像と撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、所定の基準を満たす特徴点を新たな所定の特徴点として追加することもできる。

本発明によれば、規定数の所定の特徴点だけを追跡すればよいため、処理負荷を軽減して移動体の位置を推定することができる。

移動体および移動体位置推定装置の機能構成を示すブロック図。 （ａ）は撮像した画像の様子を示し、（ｂ）は追跡対象の特徴点の数を管理しながら追跡することで移動体の位置を推定する様子を示す。 地図管理部で管理する地図データの例を示す。 トラッキング処理を示すフローチャートである。 トラッキングの例を示す説明図である。 特徴点管理処理を示すフローチャートである。 特徴点を管理する例を示す説明図である。 第２実施例に係り、特徴点管理処理を示すフローチャートである。 特徴点を管理する例を示す説明図である。 特徴点を管理する他の例を示す説明図である。

以下、図面に基づいて、本発明の実施の形態を説明する。本実施形態では、以下に詳述する通り、撮像部で撮像した画像から所定の基準を満たす所定の特徴点だけを追跡対象として選択し、規定数の所定の特徴点の座標に基づいて移動体の位置を推定する。これにより、本実施形態によれば、移動体の位置推定に必要な所定の特徴点の処理負荷を低減できる。また、本実施形態によれば、処理負荷の低減により処理速度を高速化できるため、自動車などの高速移動体の位置も推定できる。

図１〜図７を用いて第１実施例を説明する。本実施例では、以下に述べるように、自動車やロボットなどの移動体１０の上部に、広域範囲を撮像可能な広角カメラ１０１を上向きに搭載する。移動体１０に搭載された位置推定装置１００は、広域範囲内の多数の特徴点の中から、移動体１０の位置を推定するために必要な規定数の特徴点だけを抽出して追跡することで、比較的低処理負荷で移動体１０の位置を推定する。

ここで、以下に述べる「特徴点」とは、画像処理における不変特徴量である。例えば建物や窓などの輪郭を形成するコーナやエッジのような、近傍画素の輝度値の勾配が大きい方向がある、点または線を形成する点群が不変特徴量である。特徴点のうち、位置推定のためのトラッキング対象となる特徴点を所定の特徴点と呼ぶ。

「画像上座標」とは、画像の縦方向と横方向で張られる２次元の直交座標であり、画像の最小単位である１ピクセルを１目盛とする。さらに、「３Ｄ座標」とは、任意点をゼロ点とするカーテシアン座標である。「移動体座標系」とは、移動体の前方向をＸ軸、移動体の左右方向をＹ軸、移動体の高さ方向をＺ軸とする座標系である。なお、移動体１０の位置を推定するための位置推定装置１００は、移動体１０に搭載されるため、位置推定を「自己位置推定」と表現することもできる。

図１は、移動体１０の機能構成と位置推定装置１００の機能構成とを示すブロック図である。移動体１０は、例えば、乗用車などの自動車、移動機能を有するロボット、建設機械などの移動する物体である。

移動体１０の車体１１には、例えばその下側に、タイヤなどから構成される移動機構１２が設けられている。移動機構１２は、車体１１を移動させるための機構であればよく、ホイール、クローラ、歩行用脚部などでもよい。

移動体１０は、例えば、位置推定装置１００のほかに、カメラ１０１、内界センサ１０２、移動体制御装置１０３を搭載している。位置推定装置１００は、後述のように、例えば撮像部１１０、暫定位置算出部１１１、トラッキング部１１２、特徴点数管理部１１３、地図管理部１１４、位置推定部１１５を含む。

カメラ１０１は、移動体１０の周囲の特徴点をできるだけ広範囲から検出するために、広角レンズを搭載した広角カメラであることが望ましい。さらに、歩行者や他の移動体などの位置が変動する各種物体が混在する環境の中で位置が変動しない特徴点を検出するために、カメラ１０１は移動体１０上部に上向きに設置することが望ましい。ただし、移動体１０の周囲から複数の特徴点を抽出可能であるならば、カメラ１０１の種類や設置位置は特に問わない。また、カメラ１台を利用した単眼カメラであってもよいし、複数台のカメラを利用した複眼カメラであってもよい。

カメラ１０１は、撮像部１１０と通信するための通信手段を搭載する。その通信手段としては、例えば、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）接続により画像を転送する通信プロトコルＰＴＰ（Ｐｉｃｔｕｒｅ Ｔｒａｎｓｆｅｒ Ｐｒｏｔｏｃｏｌ）や、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）接続により画像を転送する通信プロトコルＰＴＰ／ＩＰ（Ｉｎｔｅｒｎｅｔ Ｐｒｏｔｏｃｏｌ）などがある。画像を転送できる手段であればよく、上記の通信手段に限定されない。

撮像部１１０は、カメラ１０１を用いて周囲環境の画像を検出し、検出した画像を後述のトラッキング部１１２に渡す。周囲環境の画像とは、移動体の周囲に存在する建物や窓枠、家具などの物体の画像である。移動体１０は、屋外のみならず屋内も走行可能であるため、周囲環境の画像には屋内の物体の画像も含まれる。画像としては、各ピクセルに例えば２５６階調の輝度値を埋め込んだグレースケール画像や、色調を埋め込んだカラー画像を用いることができる。

カラー画像を使用する場合は、例えば公知のグレースケール変換とよばれるビット変換手法を用いて、カラー画像をグレースケール画像に変換する。グレースケール変換には、例えば、それぞれ２５６階調ある３原色のうちの１色の階調を輝度値とする手法や、ＮＴＳＣ（Ｎａｔｉｏｎａｌ Ｔｅｌｅｖｉｓｉｏｎ Ｓｙｓｔｅｍ Ｃｏｍｍｉｔｔｅｅ）加重平均法とよばれる三原色の階調の加重平均を輝度値とする手法がある。色調を輝度値に変換する手法であれば、どの手法を用いてもよい。

本実施例では、屋内外の暗所および明所で画像処理を行うため、各ピクセルの輝度値を標準化または２５６階調に正規化することが望ましい。ただし、輝度値の標準化または正規化を行わなくても、画像を処理して特徴点を抽出できればよく、輝度値の標準化または正規化は必ずしも必要ではない。

また、サブピクセル化とよばれる処理を行ってもよい。サブピクセル化とは、１ピクセルよりも細かい仮想的な単位（サブピクセル）を設け、補間した色調または輝度値をサブピクセルに埋め込む画像処理である。これにより、以降の処理精度を向上できる。

内界センサ１０２は、移動体１０の内部状態を検出して出力するセンサであり、「内部状態検出部」に該当する。内界センサ１０２は、例えば、移動体１０の移動量を検出するモジュールとして構成することができる。内界センサ１０２としては、例えば、車輪の回転数を検出するエンコーダや、移動体１０の状態を検出するジャイロセンサ、加速度計、ジャイロセンサと加速度計を組み合わせたＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）などを用いることができる。なお、ＩＭＵは、地磁気センサを内蔵している。

移動体制御装置１０３は、移動体１０の移動や運転室内の環境などを制御する。移動体制御装置１０３は、運転室内に配置された運転装置からの信号に基づいて、移動体１０を移動させることができる。また、移動体制御装置１０３は、内界センサ１０２からの信号と位置推定装置１００からの演算結果とに基づいて、移動体１０を自動的に移動させることもできる。

暫定位置算出部１１１は、内界センサ１０２で検出した値を基に、移動体１０の暫定位置および姿勢を算出する。暫定位置および姿勢の算出手法として、例えば、公知のデッドレコニングとよばれる相対位置推定手法を用いることができる。デッドレコニングは、前回算出した位置から見た相対位置および相対姿勢を内界センサ値から算出し、前回算出した位置および姿勢に前記相対位置および前記相対姿勢を加算することにより、今回の位置および姿勢を推定する手法である。この手法では相対位置を加算していくため、暫定位置の誤差が蓄積する。しかし、本実施例では、後述する位置推定部１１５により、暫定位置および姿勢を修正するため、誤差は蓄積しない。

地図管理部１１４は、撮影点における移動体１０の位置および姿勢と、撮影点で撮像部１１０が撮像した周囲の画像とを対応付けて地図データベースに登録し、管理する。地図データベースを作成する移動体と地図データベースを利用する移動体とは同一である必要はない。例えば、複数の移動体１０の位置推定装置１００と通信ネットワークＣＮを介して接続される地図管理サーバ２０を介して、或る移動体の位置推定装置１００で収集したデータ（位置、姿勢、画像）を他の移動体の位置推定装置１００で利用できる。地図管理サーバ２０は、各移動体の位置推定装置１００からのデータを管理する地図データベースを保持しており、位置推定装置１００からの要求に応じて、その位置推定装置１００の地図管理部１１４に所定範囲の地図データベースを送信することができる。

地図データベースに登録する地図画像データＭ（図３参照）の作成例を説明する。地図画像データＭの構成例は図３で後述する。

例えば、地図管理部１１４は、暫定位置算出部１１１または位置推定部１１５または三角測量または衛星測位などの計測手段により移動体１０の位置と姿勢とが既知であるときに、撮像部１１０で検出した画像を、画像ＩＤ（識別子）を付けて地図データベースに登録する。さらに、地図管理部１１４は、その登録した画像を撮像したときの位置および姿勢も、画像ＩＤを付けて地図データベースに登録する。

さらに、地図管理部１１４は、地図データベースに登録した画像の中から、撮影位置が近い画像を少なくとも２枚取り出し、それぞれの画像から特徴点を検出する。地図管理部１１４は、取り出した各画像の撮像位置と姿勢とを基に、検出した特徴点同士をマッチング処理し、取り出した各画像の撮像位置と姿勢と同一特徴点の視差とを基に、同一であると判断する特徴点の地図上の３Ｄ座標（三次元座標）を算出する。地図管理部１１４は、このようにして検出した特徴点が地図データベースに登録されていない場合、その特徴点の３Ｄ座標を特徴点ＩＤ付きで地図データベースに登録する。さらに、前記取り出した各画像上の特徴点の画像上座標を、画像ＩＤおよび特徴点ＩＤに対応付けて地図データベースに登録する。
前記特徴点の３Ｄ座標が既に地図データベースに登録されている場合は、前記の取り出した各画像上の前記特徴点の画像上座標を画像ＩＤと前記特徴点ＩＤ付で地図データベースに登録する。これにより、地図データベースにおいて、画像と、撮像時の位置および姿勢と、その画像から検出される特徴点の画像上座標と、その特徴点の３Ｄ座標とが、画像ＩＤと特徴点ＩＤに対応付けられて地図データベースに登録される。

なお、地図データベースには、事前に撮像部１１０で周囲環境から検出した画像を用い、オフラインで登録してもよいし、移動体１０が移動しながらオンラインで登録してもよい。地図データベースには、オフライン式、オンライン式のいずれでも登録できる。オフライン式とは、事前に撮影した画像から特徴点などを検出して地図データベースに登録する方法である。オンライン式とは、移動体１０が移動しながら周囲の画像を取得し、取得した画像からほぼリアルタイムで特徴点などを検出して地図データベースに登録する方法である。

オンライン式で地図データベースにデータ（画像や特徴点等）を登録する場合、地図管理部１１４は、暫定位置算出部１１１または位置推定部１１５で算出した位置の近傍で登録された画像があるか確認する。もし無ければトラッキング部１１２または特徴点数管理部１１３で特徴点を検出し、または、画像全体から特徴点を検出して地図データベースに登録することができる。

「特徴点追跡部」としてのトラッキング部１１２は、暫定位置算出部１１１で算出した暫定位置および姿勢を基に、特徴点数管理部１１３で管理された前回の特徴点の画像上座標を、今回の撮像部１１０で検出した画像の画像上座標に変換する。トラッキング部１１２は、今回の撮像部１１０で検出した画像の画像上座標付近から特徴点を検出し、検出した特徴点と前回の特徴点とをマッチングすることで、特徴点数管理部１１３で管理された前回の特徴点を今回の撮像部１１０で検出した画像上で追跡（トラッキング）する。トラッキング処理の詳細は後述する。

特徴点数管理部１１３は、トラッキング部１１２によりトラッキング対象となった特徴点の数を確認し、特徴点数が予め設定される規定数となるように管理する。トラッキング対象の特徴点は、トラッキング対象点と呼ぶこともできる。特徴点数管理部１１３は、トラッキング対象の特徴点の数が規定数未満の場合、地図管理部１１４で管理された地図データベースから特徴点を抽出し、トラッキング対象の特徴点が規定数になるように調整する。また、トラッキング対象の特徴点の数が規定数の場合でも、それぞれの特徴点の配置が位置推定精度を悪化させるような配置である場合は、地図データベースから位置推定精度向上に寄与する特徴点を抽出し、トラッキング対象の特徴点を入れ替える。詳細は後述で説明する。

位置推定部１１５は、特徴点数管理部１１３で管理された特徴点の画像上座標を用いて、その特徴点が移動体座標系において見える方向を算出する。位置推定部１１５は、方向と特徴点の３Ｄ座標とから算出した方向が一致するように、暫定位置算出部１１１で算出した暫定位置および姿勢を修正する。

暫定位置および姿勢の修正方法の例を説明する。例えば、特徴点数管理部１１３で管理された特徴点（特徴点ＩＤ（ｉ））の３Ｄ座標をベクトルｐ（ｉ）、特徴点の画像上２Ｄ座標から算出した特徴点の見える方向をベクトルｍ（ｉ）、移動体１０の真の位置をベクトルｔ、移動体１０の真の姿勢を回転行列Ｒとして、次の数１で示す評価関数が最小になるように、位置を表すベクトルｔと姿勢を表すＲとを算出する。

上記の数１は、特徴点の見える方向を線分とし、線分と特徴点の３Ｄ座標のユークリッド（Ｅｕｃｌｉｄ）距離の２乗和を表す。ユークリッド距離に代えて、マハラノビス（Ｍａｈａｌａｎｏｂｉｓ）距離などのその他の距離を用いてもよい。

数１で示す評価関数を最小化する手法として、例えば、公知の非線形最小化手法であるレーベンバーグ・マーカート法を用いことができる。それに限らず、例えば、公知の最急降下法などのその他の最小化法を用いてもよい。

なお、最小化手法を適用する際の、位置を表すベクトルｔと姿勢を表すＲの初期値として、暫定位置算出部１１１で算出した暫定位置および姿勢を用いるのがよい。また、最小化手法を適用するためには、少なくとも３点の特徴点が必要である。よって、本実施例では、特徴点数管理部１１３で管理する特徴点の規定数を「３」以上に設定する。

図２は、撮像部１１０で撮影した画像と特徴点の例を示す説明図である。広角レンズを有するカメラ１０１で移動体１０の上部から上向きに屋外の風景を撮影すると、図２（ａ）に示すように、移動体１０の周囲に存在する建物などの各種物体の画像ＧＰ１〜ＧＰ９を撮影することができる。

画像ＧＰ１〜ＧＰ８は建物の全部または一部などを示す構造物の画像であり、画像ＧＰ０９は太陽の画像である。トラッキング対象となり得る画像ＧＰ１〜ＧＰ８は広角レンズの周辺部に位置し、太陽ＧＰ９は内部寄りに位置する。このため、各種物体の輪郭などは比較的明かである。本実施例では、各種物体の画像ＧＰ１〜ＧＰ８の輪郭から特徴点Ｐを検出する。

図２（ｂ）は、移動体１０の位置を推定する方法の概要を示す。移動体１０は、符号１０（１）、符号１０（２）、符号１０（３）の順番で移動するものとする。最初、移動体１０（１）の位置で、撮影した画像ＴＧ１から特徴点Ｐを抽出し、地図管理部１１４で管理している地図画像についての特徴点とマッチング処理する。

第１位置で撮影した画像ＴＧ１から検出した特徴点のうち、その第１位置近傍で管理済みの地図画像データＭ１から検出された特徴点とのマッチング尤度が最大の特徴点を、トラッキング対象の特徴点ＴＰとして選択する。トラッキング対象の特徴点ＴＰは「所定の特徴点」に該当し、例えば周方向に所定角度以上離れるようにして、規定数Ｎ（例えば３）だけ選択される。

位置推定装置１００は、移動体１０が第２位置１０（２）に移動したときの画像ＴＧ２を撮影し、その画像ＴＧ２から検出する特徴点とトラッキング対象の特徴点ＴＰをマッチング処理して、トラッキングする。

位置推定装置１００は、移動体１０が第２位置１０（２）から第３位置１０（３）にさらに移動したときも周囲環境の画像ＴＧ３を撮影して、トラッキング対象の特徴点を検出する。このとき、トラッキング対象の特徴点が規定数未満である場合、位置推定装置１００は、地図管理部１１４で管理している地図画像データＭ３から新たなトラッキング対象の特徴点ＴＰ（ｎｅｗ）を補充する。地図画像データＭ３は、第３位置１０（３）の近傍で撮影されて保存されたデータであり、複数の特徴点Ｐを含んでいる。位置推定装置１００は、トラッキング対象の特徴点の数が規定数となるように、地図画像データＭ３から新たなトラッキング対象となる特徴点を選択する。これにより、位置推定装置１００は、トラッキング対象の特徴点ＴＰを規定数だけ持つ画像ＴＧ３ａを得る。このように、本実施例の位置推定装置１００は、トラッキング対象の特徴点を規定数だけトラッキングすることにより、比較的低負荷で位置を推定できるようになっている。

図３は、地図管理部１１４で管理する地図画像データＭの構成例を示す。地図画像データＭは、例えば、画像を撮影したときの移動体の位置情報、画像を撮影したときの移動体の姿勢、撮影した画像から検出した特徴点に関する情報とを対応付けている。特徴点に関する情報は、例えば、特徴点を識別するための特徴点ＩＤ、特徴点の画像上の座標、特徴点の３Ｄ座標、撮影した画像データの格納先アドレスなどを含む。

図４は、トラッキング部１１２が実行するトラッキング処理を示す。図５は、トラッキング処理の概要を示す。図４では、トラッキング部１１２を動作の主体として述べるが、位置推定装置１００を動作主体として述べることもできる。

まず、トラッキング部１１２は、前回の特徴点数管理処理（図６で後述）で管理された特徴点が１つ以上あったか判定する（Ｓ１００）。前回の特徴点が１つも無い場合（Ｓ１００：ＮＯ）、トラッキング対象が無いため本処理は終了する。

前回の特徴点が１つ以上有る場合（Ｓ１００：ＹＥＳ）、トラッキング部１１２は、特徴点をトラッキングするために、ループ変数ｉに「０」を設定して（ｉ＝０）、ループ処理（Ｓ１０２〜Ｓ１０９）に入る（Ｓ１０１）。

トラッキング部１１２は、ｉループ目の処理開始時に、暫定位置算出部１１１で今回算出した暫定位置および姿勢と、特徴点数管理部１１３で管理された前回の特徴点（特徴点ＩＤをＩＤ（ｉ）とする）の３Ｄ座標とを用いて、撮像部１１０で今回検出した画像上における特徴点ＩＤ（ｉ）の特徴点の暫定の画像上座標を算出する（Ｓ１０２）。

ステップＳ１０２の処理を図５を用いて説明する。ステップＳ１０２では、前回の画像上の特徴点Ｐ［ＩＤ（１）］を、内界センサ値から算出した移動量ΔＬを用いて画像上座標Ｃに移動させ、画像上座標Ｃを今回の画像上の特徴点の暫定の画像上座標とする。

トラッキング部１１２は、ステップＳ１０２で算出した特徴点ＩＤ（ｉ）を有する特徴点の暫定の画像上座標の近傍領域で、特徴点を検出する（Ｓ１０３）。

ここで、近傍領域とは「所定領域」に該当し、前回の特徴点が今回出現すると見込まれる出現予定座標を含む領域である。近傍領域は、ステップＳ１０２において、暫定位置および姿勢を微小変化させたときに、特徴点ＩＤ（ｉ）の特徴点が取り得る画像上座標の領域を算出して用いる。これに限らず、暫定の画像上座標を中心とする四角形や円などの領域を近隣領域として設定してもよい。近傍領域で特徴点を検出できなかった場合、近傍領域を拡張して、再度特徴点の検出を行っても良い。

図５を用いてステップＳ１０３の処理を説明する。暫定の画像上座標Ｃを中心に今回の画像上で近傍領域ＮＡを設け、近傍領域ＮＡの中から特徴点群Ｐｇｒｐを検出し、特徴点群Ｐｇｒｐと前回の画像上の特徴点Ｐ［ＩＤ（１）］とでマッチング処理を行う。

トラッキング部１１２は、ステップＳ１０３で特徴点ＩＤ（ｉ）を持つ特徴点に対応する特徴点が検出されたか判定する（Ｓ１０３）。一つも検出されなかった場合、トラッキング部１１２は、特徴点ＩＤ（ｉ）を持つ特徴点をトラッキング対象から除外する（Ｓ１０３：ＮＯ）。

トラッキング部１１２は、トラッキング対象の特徴点が１つ以上検出された場合（Ｓ１０４：ＹＥＳ）、検出した特徴点群と、特徴点ＩＤ（ｉ）を持つ前回検出した特徴点とをそれぞれマッチング処理し、それぞれのマッチング尤度を算出する（Ｓ１０５）。

マッチング処理の手法として、例えば、特徴点の近傍画素の輝度値パターンなどを表した特徴量のユークリッド距離の逆数をマッチング尤度とするパターンマッチングや、特徴点近傍にテンプレート窓を用意し、テンプレート窓内の各画素の相関をマッチング尤度とするテンプレートマッチング等が知られている。本実施例では、マッチング尤度を算出できる手法であれば、どのようなマッチング手法を採用してもよい。

トラッキング部１１２は、マッチングできたかを判断するための基準となる閾値ＭＬを予め設定しており、ステップＳ１０５でマッチング処理した特徴点のうち、マッチング尤度が最大で、かつ閾値ＭＬより大きい特徴点を選択する（Ｓ１０６）。マッチング尤度の最大値が閾値ＭＬ以下の場合（Ｓ１０６：ＮＯ）、特徴点ＩＤ（ｉ）を持つ特徴点をトラッキング対象から除外する。

トラッキング部１１２は、前回の特徴点ＩＤ（ｉ）の特徴点の画像上座標を、ステップＳ１０６で選択した特徴点の画像上座標に更新する（Ｓ１０７）。さらに、トラッキング部１１２は、ステップＳ１０７において、特徴点ＩＤ（ｉ）の特徴点の連続トラッキング回数に「１」を足す。

トラッキング部１１２は、ステップＳ１０７で更新した特徴点ＩＤ（ｉ）の特徴点の画像上座標と、連続トラッキング回数と、ステップＳ１０５で算出したマッチング尤度の最大値とを保存する（Ｓ１０８）。

トラッキング部１１２は、前回の特徴点数管理処理で管理された全特徴点に対して、ステップＳ１０２からＳ１０８までの処理が完了した場合はトラッキング処理を終了する（Ｓ１０９：ＹＥＳ）。

前回の特徴点数管理処理で管理された全特徴点に対してステップＳ１０２からＳ１０８までの処理が完了していない場合（Ｓ１０９：ＮＯ）、トラッキング部１１２は、未処理の特徴点の中から処理対象の特徴点を１つ選択し（Ｓ１１０）、その選択した特徴点に対してステップＳ１０２からＳ１０８までの処理を適用する。

図６は、特徴点数管理部１１３の実行する特徴点数管理処理を示す。また、図７は、特徴点の規定数を３点としたときの、特徴点数管理処理の概要を示す。

特徴点数管理部１１３は、トラッキング部１１２でトラッキングできた特徴点の数ＮＴＰが１点または０点の場合（Ｓ２００：ＮＯ）、後述するステップＳ２０５の処理を行い、トラッキング部１１２でトラッキングできた特徴点の数ＮＴＰが２点以上の場合（Ｓ２００：ＹＥＳ）、後述のステップＳ２０１〜Ｓ２０３のループ処理を行う。

特徴点数管理部１１３は、トラッキング部１１２でトラッキングできた特徴点のうちの１点に注目し、その注目点の近傍領域を設定する（Ｓ２０１）。本実施例において近傍領域は、撮像部１１０で検出した画像上における注目点の偏角±３６０゜／（４×（特徴点規定数Ｎ））の領域とする。

注目点の近傍領域に含まれるトラッキングできた特徴点のうち、トラッキング処理で保存したマッチング尤度が最大の特徴点以外の特徴点を、次回のトラッキング処理でのトラッキング対象から除外する。

ここで、マッチング尤度が最大の特徴点が２つ以上ある場合は、トラッキング処理（トラッキング部１１２）で保存した連続トラッキング回数が最も多い特徴点以外の特徴点を、トラッキング対象から除外する。

ここで、図７を参照して説明する。図７の左側に示すように、特徴点ＰｂとＰｃとの２点の特徴点が近接する画像ＴＧ１０の場合、処理過程の画像ＴＧ１１に示すように、２点の特徴点Ｐｂ、Ｐｃのいずれからも離れた特徴点Ｐａを選択すると、その特徴点Ｐａの近傍領域ＮＡ１には他の特徴点が存在しない。このため、その選択した特徴点Ｐａは、トラッキング対象の特徴点ＴＰ１として残る。

そして、次の処理過程を示す画像ＴＧ１２では、選択した特徴点Ｐｂの近傍領域ＮＡ２に他の特徴点Ｐｃが含まれるため、特徴点数管理部１１３は、マッチング尤度と連続トラッキング回数とを基に、トラッキング対象から除外する特徴点を決定する。

図６に戻る。特徴点数管理部１１３は、前回のトラッキング処理でトラッキングできた全特徴点に対してステップＳ２０１〜Ｓ２０３が完了した場合、または、次回のトラッキング処理でのトラッキング対象の特徴点が１点になった場合のいずれかであると判定すると（Ｓ２０２：ＹＥＳ）、ループ処理（Ｓ２０１〜Ｓ２０３）を抜ける。例えば、図７では、処理過程の画像ＴＧ１２の終了直後と、右側に示す処理過程の画像ＴＧ２１の終了直後とは、処理Ｓ２０２でＹＥＳと判定される場合に相当する。

特徴点数管理部１１３は、次回のトラッキング処理においてトラッキング対象から除外されていない、トラッキングできた他の特徴点の中から１つを注目点として選択し、ループ処理（Ｓ２０１〜Ｓ２０３）を行う（Ｓ２０３）。

特徴点数管理部１１３は、次回のトラッキング処理でのトラッキング対象となっている特徴点の数が規定数あるか確認し（Ｓ２０４）、トラッキング対象の特徴点数が規定数に達している場合（Ｓ２０４：ＮＯ）、特徴点数管理処理を終了する。

トラッキング対象の特徴点数が規定数に達していない場合（Ｓ２０４：ＹＥＳ）、特徴点数管理部１１３は、この時点における、次回のトラッキング処理でのトラッキング対象点の画像上の近傍領域外で、特徴点を補充する（Ｓ２０５）。トラッキング対象の特徴点の近傍領域以外の領域は、「所定領域以外の他の領域」に対応する。

すなわち、特徴点数管理部１１３は、地図管理部１１４で管理された地図データベースの画像のうち、暫定位置算出部１１１で算出した暫定位置に最も近い位置で撮像した画像を選択する。特徴点数管理部１１３は、現時点で残っているトラッキング対象の特徴点の画像上の近傍領域外において、選択した画像とのマッチング処理を行い、マッチング尤度が最大となる特徴点をトラッキング対象として追加する（Ｓ２０５）。

ここで、近傍領域は、ステップＳ２０１で述べたと同様の手法で設定できる。また、マッチング手法は、マッチング尤度を算出できる手法であれば、どのマッチング手法を採用してもよい。図７において、例えば、処理過程の画像ＴＧ１３や処理過程の画像ＴＧ２３が、ステップ２０５に相当する。

特徴点数管理部１１３は、マッチング尤度の最大値が閾値ＭＬ以上とならない場合、マッチング処理に失敗したものと判断し（Ｓ２０６：ＹＥＳ）、新たな特徴点をトラッキング対象に追加せずに、特徴点数管理処理を終了する。一方、マッチング尤度の最大値が閾値ＭＬより大きい場合（Ｓ２０６：ＮＯ）、特徴点数管理部１１３は、マッチング尤度が最大の特徴点をトラッキング対象に追加し、再度トラッキング対象点数を確認する（Ｓ２０４）。

図７は、トラッキング対象の特徴点を決定し、規定数に足りない場合はトラッキング対象の特徴点を補充する様子を模式的に示す。

一方の処理過程の画像ＴＧ１０〜ＴＧ１３には、近接する２つの特徴点Ｐｂ、Ｐｃと１つだけ離れた場所にある特徴点Ｐａとが含まれている（ＴＧ１０）。

上述の通り、１つだけ離れた特徴点Ｐａに着目すると、その特徴点Ｐａの近傍領域ＮＡ１には他の特徴点が存在しない。このため、特徴点Ｐａは、トラッキング対象として選択される（ＴＧ１１）。

次の特徴点Ｐｂに注目すると、その特徴点Ｐｂの近傍領域ＮＡ２内には他の特徴点Ｐｃが存在する。そこで、特徴点Ｐｂ、Ｐｃのうち、マッチング尤度が最大の特徴点（ここではＰｂとする）をトラッキング対象の特徴点ＴＰ２として選択する（ＴＧ１２）。

本実施例では、規定数Ｎを３としており、３個の特徴点をトラッキングする。しかし、未だ２個の特徴点しか選択していない。そこで、トラッキング対象として選択した特徴点ＴＰ１（Ｐａ）、ＴＰ２（Ｐｂ）の近傍領域ＮＡ１、ＮＡ２以外の他の領域Ａ３ａ、Ａ３ｂに存在する他の特徴点の中から、３番目の特徴点を選択する。特徴点数管理部１１３は、他の領域Ａ３ａ、Ａ３ｂに存在する他の特徴点のうち、暫定位置に対応する地図画像データＭに含まれている特徴点とのマッチング尤度が最大の特徴点を１つ選択し、トラッキング対象の特徴点ＴＰ３として追加する。

他方の処理過程（ＴＧ２０〜ＴＧ２３）では、或る特徴点の近傍領域ＮＡ４内に３つの特徴点Ｐｅ、Ｐｄ、Ｐｆが近接して存在している（ＴＧ２０）。そこで、特徴点数管理部１１３は、３つの特徴点の中から、マッチング尤度が最大の特徴点Ｐｄをトラッキング対象の特徴点ＴＰ４として選択する（ＴＧ２１）。

トラッキング対象の特徴点ＴＰ４の近傍領域ＮＡ４以外の他の領域Ａ４から、上述の方法に従って２番目の特徴点ＴＰ５を選択する（ＴＧ２２）。さらに、特徴点数管理部１１３は、トラッキング対象の特徴点ＴＰ４、ＴＰ５の近傍領域ＮＡ４、ＮＡ５以外の他の領域Ａ５ａ、Ａ５ｂから、３番目の特徴点ＴＰ６を選択する（ＴＧ２３）。

このように構成される本実施例によれば、規定数の特徴点だけをトラッキングすればよいため、比較的低い処理負荷で、移動体１０の位置を推定することができる。

本実施例では、位置の推定に必要な特徴点（トラッキング対象の特徴点）を、所定の幾何条件に基づいて選択するため、位置の推定精度を高めることができる。幾何条件の一例として、本実施例では、特徴点を周方向に所定角度離れるようにして選択するため、３つの特徴点と移動体１０との距離および方向から、移動体１０の位置を比較的高精度に推定することができる。さらに、一例として、本実施例では、或るトラッキング対象の特徴点に設定する近傍領域以外の領域から他のトラッキング対象の特徴点を選択するため、各トラッキング対象の特徴点同士は、近傍領域の大きさに応じて離れている。さらに、本実施例では、近傍領域をトラッキング対象の特徴点（注目点）の偏角±３６０゜／（４×（規定数Ｎ））の領域として定義するため、規定数Ｎが「３」の場合、各トラッキング対象の特徴点同士は少なくとも３０度以上離れている。

このように本実施例によれば、比較的簡素なアルゴリズムに従ってトラッキング対象の特徴点を規定数に管理することができるため、種々変化する移動状況に速やかに対応して移動体１０の位置を推定することができる。

図８〜図９を用いて第２実施例を説明する。本実施例は、第１実施例の変形例に該当し、第１実施例で述べたよりもトラッキング対象の特徴点（トラッキング対象点）をよりロバストに抽出する。

図８は、本実施例による特徴点数管理処理のフローチャートを示す。図９、図１０は、特徴点の規定数Ｎを３としたときの、本実施例による特徴点数管理処理の概要を示す説明図である。図８に示す特徴点管理処理のうちステップＳ３００、Ｓ３０１、Ｓ３０２、Ｓ３０３、Ｓ３０６、Ｓ３０７、Ｓ３０８は、図６で述べたステップＳ２００、Ｓ２０１、Ｓ２０２、Ｓ２０３、Ｓ２０４、Ｓ２０５、Ｓ２０６に対応する。ただし、近傍領域の構成などは第１実施例と異なる。

特徴点数管理部１１３は、トラッキング処理でトラッキングできた特徴点数が１つまたは０の場合（Ｓ３００：ＮＯ）、ステップＳ３０５の処理を行い、トラッキングできた特徴点数が２つ以上の場合（Ｓ３００：ＹＥＳ）、ステップＳ３０１〜Ｓ３０５のループ処理を行う。

ステップＳ３０１において、特徴点数管理部１１３は、トラッキング処理でトラッキングできた特徴点のうちの１点に注目し、その注目点の近傍領域を設定する。ループ開始時の近傍領域は、撮像部１１０で検出した画像上における注目点の偏角±３６０゜／（２×（特徴点規定数Ｎ））の領域とする。

注目点の近傍領域に含まれる、トラッキングできた特徴点のうち、トラッキング処理で保存したマッチング尤度が最大の特徴点以外を、次回のトラッキング処理でのトラッキング対象から除外する（Ｓ３０１）。ここで、マッチング尤度が最大の特徴点が２点以上ある場合は、トラッキング部１１２で保存した連続トラッキング回数が最も多い特徴点以外の特徴点をトラッキング対象から除外する。

ステップＳ３０２において、特徴点数管理部１１３は、前回のトラッキング処理でトラッキングできた全特徴点に対して、ステップＳ３０１〜Ｓ３０３が完了した場合、または、次回のトラッキング処理でのトラッキング対象の特徴点が１点になった場合、のいずれかである場合（Ｓ３０２：ＹＥＳ）、ループ処理（Ｓ３０１〜Ｓ３０３）を抜ける。

ステップＳ３０３において、特徴点数管理部１１３は、次回のトラッキング処理でのトラッキング対象から除外されていない、他の特徴点の中から注目点を選択し、ループ処理（Ｓ３０１〜Ｓ３０３）を行う。図９の処理過程の画像ＴＧ３１、ＴＧ３２、ＴＧ３３、ＴＧ３４と、図１０の処理過程の画像ＴＧ４１、ＴＧ４２、ＴＧ４３、ＴＧ４４とが、ループ処理Ｓ３０１〜Ｓ３０３に相当する。

特徴点数管理部１１３は、設定した近傍領域でループ処理（Ｓ３０１〜Ｓ３０３）を行った結果、トラッキング対象点の数ＮＴＰが規定数Ｎだけある場合、または、ループ処理Ｓ３０１〜Ｓ３０５の３回目を終了した場合、のいずれかである場合（Ｓ３０４：ＹＥＳ）、ループ処理（Ｓ３０１〜Ｓ３０５）を抜け、そうでない場合（Ｓ３０４：ＮＯ）、ステップＳ３０５に進む。

Ｓ３０５において、特徴点数管理部１１３は、トラッキング対象点を特徴点数管理処理の開始時の状態（初期状態）に戻し、近傍領域のサイズを縮小した後、ループ処理（Ｓ３０１〜Ｓ３０３）に進む。

近傍領域は、１回目のループ処理Ｓ３０１〜Ｓ３０３の終了時に、注目点の偏角±３６０゜／（４×（特徴点規定数Ｎ））となり、２回目のループ処理Ｓ３０１〜Ｓ３０３の終了時に、注目点の偏角±３６０゜／（８×（特徴点規定数Ｎ））となるように、段階的に縮小するように設定されている。図９の処理過程の画像ＴＧ３２から画像ＴＧ３３への移行、図１０の処理過程の画像ＴＧ４１からＴＧ４２への移行、ＴＧ４２からＴＧ４３への移行が、ステップＳ３０５に相当する。

Ｓ３０６において、特徴点数管理部１１３は、現時点で、次回のトラッキング処理でのトラッキング対象となっている特徴点の数ＮＴＰが規定数Ｎに一致する場合（Ｓ３０６：ＹＥＳ）、本処理を終了する。

Ｓ３０７において、特徴点数管理部１１３は、現時点で、次回のトラッキング処理でのトラッキング対象点の画像上の近傍領域外で、地図管理部１１４で管理された地図データベースの画像のうち、暫定位置に最も近い位置で撮像した画像を選択する。特徴点数管理部１１３は、ステップＳ３０７の時点で残っているトラッキング対象点の画像上の近傍領域外で、選択した画像とのマッチング処理を行い、マッチング尤度が最大となる特徴点をトラッキング対象に追加する。

ここで、ステップＳ３０７における近傍領域は、ループ処理（Ｓ３０１〜Ｓ３０５）を抜けたときの、ステップＳ３０５で設定したサイズの近傍領域とする。また、マッチング手法は、マッチング尤度を算出できる手法であれば、どのマッチング手法を採用してもよい。図１０の処理過程の画像ＴＧ４５がステップＳ３０７に相当する。

ステップＳ３０８において、特徴点数管理部１１３は、マッチング尤度の最大値が閾値ＭＬ以上とならない場合（Ｓ３０８：ＹＥＳ）、マッチング処理に失敗したものと判断し、新たな特徴点をトラッキング対象に追加せずに、特徴点数管理処理を終了する。一方、マッチング尤度の最大値が閾値ＭＬより大きい場合（Ｓ３０８：ＮＯ）、特徴点数管理部１１３は、マッチング尤度が最大の特徴点をトラッキング対象として追加し、再度トラッキング対象点数を確認する（Ｓ３０６）。

このように構成される本実施例も第１実施例と同様の作用効果を奏する。さらに本実施例では、トラッキング対象の特徴点を探すための探索領域である近傍領域のサイズを可変とし、段階的に小さくする構成としたため、撮影した画像に含まれる特徴点が少ない場合であっても、位置推定に必要な数の特徴点をトラッキング対象として選択することができ、位置推定の信頼性および精度が向上する。

なお、本発明は、上述した実施例に限定されない。当業者であれば、本発明の範囲内で、種々の追加や変更等を行うことができる。例えば、撮像した画像を中心角が所定角度の領域に分割し、各領域の中で最もマッチング尤度の高い特徴点をトラッキング対象として選択する構成であってもよい。ただし、この場合は、利用可能な特徴点の存在しない領域がある場合、その領域ではトラッキング対象の特徴点を選択することができないため、位置の推定精度が低下する可能性がある。

また、前記各実施例では、トラッキング対象の特徴点の規定数を「３」とした場合を例に挙げて説明したが、これに限らず、規定数は４以上の値であってもよい。また、近傍領域のサイズも例示である。

１０：移動体、１００：位置推定装置、１０１：カメラ、１０２：内界センサ、１０３：移動体制御装置、１１０：撮像部、１１１：暫定位置算出部、１１２：トラッキング部、１１３：特徴点数管理部、１１４：地図管理部、１１５：位置推定部

Claims (8)

1. 移動体の位置を推定する移動体位置推定装置であって、
   移動体に取り付けられ、周囲環境を撮像する撮像部と、
   周囲環境の画像と、前記画像を撮影したときの撮像位置および姿勢と、前記周囲環境を撮像した画像から抽出される特徴点の画像上における画像上座標と、前記特徴点の移動体座標系における三次元座標とを、地図に対応付けて管理する地図管理部と、
   前記撮像部で撮像した画像から抽出される特徴点のうち所定の基準で選択される所定の特徴点を追跡する特徴点追跡部と、
   前記特徴点追跡部が追跡する前記所定の特徴点の数が規定数となるように管理する特徴点数管理部と、
   前記特徴点追跡部が追跡する前記所定の特徴点の座標と前記地図管理部の管理する地図とから位置を推定する位置推定部と、
   前記移動体の内部状態を検出する内界状態検出部の検出値と前記位置推定部が推定した位置とに基づいて、前記移動体の暫定位置と姿勢を算出する暫定位置算出部とを備え、
   前記特徴点追跡部は、前記暫定位置算出部で算出した暫定位置および姿勢と前記撮像部で撮像した画像とを用いて、前記所定の特徴点を追跡し、
   前記特徴点数管理部は、
   前記所定の特徴点の数が前記規定数に満たないと判定した場合、前記地図管理部で管理された周囲環境の画像と前記撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、前記所定の基準を満たす特徴点を、新たな所定の特徴点として追加し、
   前記所定の特徴点の数が前記規定数であると判定した場合、
   前記各所定の特徴点が前記所定の基準を満たしているか確認し、
   前記所定の基準を満たしていない特徴点は前記所定の特徴点ではないと判断して前記特徴点追跡部による追跡対象から除外し、
   前記地図管理部で管理された周囲環境の画像と前記撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、前記所定の基準を満たす特徴点を新たな所定の特徴点として追加する、
   移動体位置推定装置。
   
2. 前記特徴点追跡部は、
   所定の特徴点が画像上に出現すると見込まれる出現予定座標を、前記暫定位置算出部の算出した暫定位置に基づいて算出し、
   前記出現予定座標を含んで設定される所定領域内の画像から特徴点を抽出し、
   抽出した特徴点と前記所定の特徴点とのマッチング尤度を算出し、
   前記所定領域内で抽出した前記特徴点のうち前記マッチング尤度が最大となる特徴点を前記所定の特徴点として使用する、
   請求項１に記載の移動体位置推定装置。
   
3. 前記所定の基準とは、前記規定数の所定の特徴点の三次元座標から位置を推定するために必要な所定の幾何条件として定義される、
   請求項１または２のいずれか一項に記載の移動体位置推定装置。
   
4. 前記所定の幾何条件とは、前記規定数の前記所定の特徴点が前記画像の中心を基準として周方向に所定角度以上分散していることである、
   請求項３に記載の移動体位置推定装置。
   
5. 前記所定の幾何条件とは、前記所定の特徴点を含むようにして設定される所定領域以外の他の領域に存在する特徴点を選択することである、
   請求項４に記載の移動体位置推定装置。
   
6. 前記所定の特徴点に関する前記所定領域のサイズは、可変である、
   請求項５に記載の移動体位置推定装置。
   
7. 撮像部は、前記移動体の上面に上向きに設置されている、
   請求項１〜６のいずれか一項に記載の移動体位置推定装置。
   
8. 移動体の位置をコンピュータを用いて推定する移動体位置推定方法であって、
   前記コンピュータは、移動体に取り付けられ、周囲環境を撮像する撮像部に接続されており、
   前記撮像部で撮像した画像から抽出される特徴点のうち所定の基準で選択される所定の特徴点を追跡する追跡ステップと、
   前記所定の特徴点の数が規定数となるように管理する特徴点管理ステップと、
   前記周囲環境を撮像した画像から抽出される特徴点の座標が地図に対応付けられた所定の地図データと前記所定の特徴点の座標とから位置を推定する位置推定ステップと、
   前記移動体の内部状態を検出する内界状態検出部の検出値と前記位置推定ステップが推定した位置とに基づいて、前記移動体の暫定位置と姿勢を算出する暫定位置算出ステップと、を前記コンピュータで実行し、
   さらに前記追跡ステップは、前記暫定位置および姿勢と前記撮像部で撮像した画像とを用いて、前記所定の特徴点を追跡し、
   前記特徴点管理ステップは、
   前記所定の特徴点の数と前記規定数とを比較し、
   前記所定の特徴点の数が前記規定数に満たないと判定した場合、前記所定の地図データで管理された周囲環境の画像と前記撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、前記所定の基準を満たす特徴点を、新たな所定の特徴点として追加し、
   前記所定の特徴点の数が前記規定数であると判定した場合、前記各所定の特徴点が前記所定の基準を満たしているか確認し、
   前記所定の基準を満たしていない特徴点は前記所定の特徴点ではないと判断して前記追跡ステップによる追跡対象から除外し、
   前記所定の地図データで管理された周囲環境の画像と前記撮像部で撮像した画像とをマッチング処理することで、マッチング尤度が所定の閾値以上であり、かつ、前記所定の基準を満たす特徴点を新たな所定の特徴点として追加する、
   移動体位置推定方法。

Images