(57)【要約】【課題】回転不変かつスケール不変な特徴量を用いて高速に移動体の自己位置を推定するためにランドマークデータベースを作成する。【解決手段】ランドマーク学習装置は、時系列画像データに基づいて、画像ごとに特徴点を抽出して特徴量を計算する特徴点抽出部１０２と、過去画像で得られたトラッキングすべき点の画像座標およびＩＤと、現在の画像とに基づいて、トラッキングすべき点の現在の画像座標を出力する特徴点トラッキング部１０３と、２回目以降の画像処理時に、各画像間でパターン認識により特徴量が類似する特徴点を選出し、選出された特徴点が時系列でトラッキング可能であった場合に、その特徴点をランドマーク候補点とし、ランドマーク候補点の数が閾値以下になった場合に残った点をランドマークとして判定する特徴点判定部１０４と、を有する。【選択図】図１
2010033447.tif 000002
【特許請求の範囲】
【請求項１】
移動体に取り付けられたカメラを用いて時系列で前記移動体の周囲の画像を撮像して得られた画像データに基づいて、得られた画像ごとに特徴点を抽出して特徴量を計算する学習段階特徴点抽出部と、
特徴点判定部から過去画像において得られたトラッキングすべき点の画像座標およびＩＤと、前記時系列で得られた現在の画像とに基づいて、トラッキングすべき点の現在の画像座標を出力する学習段階特徴点トラッキング部と、
所定回目画像処理時には前記学習段階特徴点抽出部から得られた特徴点の座標と特徴量にＩＤを付けて保存し、前記所定回目以降の画像処理時に、各画像間でパターン認識により特徴量が類似する特徴点を選出し、前記選出された特徴点が時系列でトラッキング可能であった場合に、その特徴点をランドマーク候補点とし、ランドマーク候補点の数が閾値以下になった場合に残った点をランドマークとして判定する特徴点判定部と、
前記画像を撮像しているときの前記移動体の座標を取得するデッドレコニングデータ取得部と、
前記デッドレコニングデータ取得部で取得された移動体の座標に基づいて前記特徴点判定部でランドマークと判定された点の座標を測定する特徴点座標測定部と、
前記特徴点判定部でランドマークと判定された点の特徴量と、前記特徴点座標測定部で得られた各ランドマークの座標とを保存するランドマークデータ保存部と、
を有することを特徴とする画像処理装置。
【請求項２】
前記ランドマークデータ保存部に前記ランドマークの特徴量と座標が保存された状態で、前記移動体に取り付けられたカメラを用いて時系列で前記移動体の周囲の画像を撮像して得られた画像に基づいて、前記移動体の自己位置推定を行なうための特徴点を抽出する自己位置算出段階特徴点抽出部と、
前記ランドマーク保存部に保存されたランドマークの特徴量と前記自己位置算出段階特徴点抽出部で抽出された特徴点の特徴量との照合を行ない、マッチング尤度付で照合結果を出力する特徴点マッチング部と、
マッチングした特徴点をトラッキングする自己位置算出段階特徴点トラッキング部と、
マッチングした特徴点または自己位置算出段階特徴点トラッキング部でトラッキングされた特徴点を、尤度と配置に基づき、３点以上ずつの特徴点組にする特徴点組生成部と、
前記特徴点組から自己位置を推定する自己位置推定部と、
過去の自己位置推定結果から現在の予測自己位置を計算する自己位置予測部と、
前記各特徴点組から推定された自己位置と、前記各特徴点組から推定された自己位置の重み付平均自己位置と、前記予測自己位置のうち、一つ以上を利用することで、適切な自己位置を判定し、判定された自己位置と各特徴点の位置関係から特徴点をトラッキングするか否かを判断し、トラッキングすると判断した特徴点の数に応じて、自己位置推定法を変更する自己位置判定部と、
をさらに具備すること、を特徴とする請求項１に記載の画像処理装置。
【請求項３】
前記ランドマークデータ保存部に前記ランドマークの特徴量と座標が保存された状態で前記移動体の自己位置推定を行なうための特徴点を抽出する自己位置算出段階特徴点抽出部と、
前記ランドマーク保存部に保存されたランドマークの特徴量と前記自己位置算出段階特徴点抽出部で抽出された特徴点の特徴量との照合を行ない、マッチング尤度付で照合結果を出力する特徴点マッチング部と、
マッチングした特徴点をトラッキングする自己位置算出段階特徴点トラッキング部と、
マッチングした特徴点または自己位置算出段階特徴点トラッキング部でトラッキングされた特徴点を、尤度と配置に基づき、３点以上ずつの特徴点組にする特徴点組生成部と、
前記特徴点組から自己位置を推定する自己位置推定部と、
過去の自己位置推定結果から現在の予測自己位置を計算する自己位置予測部と、
前記各特徴点組から推定された自己位置と、前記各特徴点組から推定された自己位置の重み付平均自己位置と、前記予測自己位置のうち、一つ以上を利用することで、自己位置を判定し、判定された自己位置と各特徴点の位置関係から特徴点をトラッキングするか否かを判断し、トラッキングすると判断した特徴点の数に応じて、自己位置推定法を変更する自己位置判定部と、
をさらに具備すること、を特徴とする請求項１記載の画像処理装置。
【請求項４】
前記特徴点組生成部は、前記移動体の自己位置推定を行なうための特徴点を、移動体からの方位が互いに比較的近いもの同士を集めた３つ以上のグループに分類し、各グループに含まれる特徴点を方位順にナンバリングし、各グループで同じナンバーを持つ特徴点同士を特徴点組とすることを特徴とする請求項２または請求項３に記載の画像処理装置。
【請求項５】
前記自己位置判定部は、判定された自己位置から計上された特徴点数と閾値を比較することによって、以後の自己位置推定処理に使用する特徴点をトラッキングにより獲得するか、新規に特徴点抽出手法を用いて獲得するかを決定することを特徴とする請求項２ないし請求項４のいずれか一項に記載の画像処理装置。
【請求項６】
移動体に取り付けられたカメラを用いて時系列で前記移動体の周囲の画像を撮像し、
前記画像に基づいて、得られた画像ごとに特徴点を抽出して特徴量を計算し、
過去画像において得られたトラッキングすべき点の画像座標およびＩＤと、前記時系列で得られた現在の画像とに基づいて、トラッキングすべき点の現在の画像座標を出力し、
所定回目画像処理時には前記特徴点抽出部から得られた特徴点の座標と特徴量にＩＤを付けて保存し、
前記所定回目以降の画像処理時に、各画像間でパターン認識により特徴量が類似する特徴点を選出し、前記選出された特徴点が時系列でトラッキング可能であった場合に、その特徴点をランドマーク候補点とし、ランドマーク候補点の数が閾値以下になった場合に残った点をランドマークとして判定し、
前記画像を撮像しているときの前記移動体の座標を前記画像情報によらずに取得し、
前記移動体の座標に基づいて前記ランドマークと判定された点の座標を測定し、
前記ランドマークと判定された点の特徴量と座標をランドマークデータとして保存する、
ことを特徴とする画像処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体から撮像された画像を処理する装置および方法に関し、特に、移動体の自己位置と姿勢を推定するのに利用可能なものに関する。
【背景技術】
【０００２】
ロボットなどの移動体にとって、目標地点到達のためにカメラ画像を用いて自己位置を推定することは重要である。そこで従来から、移動台車に取り付けたカメラから画像を撮像し、画像の回転変化およびスケール変化に強い特徴量を用いて、ランドマークと特徴点の対応をとり、自己位置推定を行なう装置が知られている。
【０００３】
特許文献１には、ランドマークとしてコーナー点を用い、各画像でスケール変化だけに強い特徴量を計算し、自己位置を検出する方法について記載されている。この文献に記載された方法では、特徴点位置を、トラッキング手法を用いて予測することで、より安定して自己位置を推定する。
【０００４】
また、回転不変かつスケール不変な特徴量抽出法として非特許文献１および非特許文献２が知られている。
【０００５】
さらに、回転不変かつスケール不変特徴量を用いた特徴点マッチングの従来例として、非特許文献３の方法が知られている。
【特許文献１】特開２００６−２０９７７０号公報
【非特許文献１】Lowe, D.G.著 "Distinctive image features from scale-invariant keypoints", International Journal of Computer Vision, Vol. 60, No. 2, pp. 91-110, 2004.
【非特許文献２】Herbert Bay, Tinne Tuytelaars, and Luc Van GOol著 "Surf: Speed up robust features", In The ninth European Conference on Computer Vision, 2006.
【非特許文献３】A. C. Murillo, J. J. Guerrero and C. Sagues著 "SURF features for efficient robot localization with omnidirectional images", 2007 IEEE International Conference on Robotics and Automation, pp. 3901 - 3907, 2007.
【発明の開示】
【発明が解決しようとする課題】
【０００６】
特許文献１に記載された技術では、天井のコーナー点だけを考慮し、特徴量もスケール変化だけを考慮して計算している点にある。そのため計算は速いが、移動による回転変化が大きい広角画像における自己位置推定などには利用することができない。
【０００７】
従来の、回転変化およびスケール変化に強い特徴量を持った特徴点を検出するための計算コストは大きく、実時間で自己位置推定ができない。
【０００８】
非特許文献３に記載の方法では、ＳＩＦＴ（Scale Invariant Feature Transform）よりは速く計算できるといわれるＳＵＲＦ（Speeded Up Robust Features）を利用しているが、ＳＵＲＦもまだ計算時間がかかって実用化は困難である。
【０００９】
本発明は上記課題を解決するためのものであって、移動体から撮像した画像に基づき、回転変化およびスケール変化に強い特徴量を用いて高速に移動体の自己位置を推定できるようにすることを目的としている。
【課題を解決するための手段】
【００１０】
上記の目的を達成するために、本発明に係る画像処理装置は、移動体に取り付けられたカメラを用いて時系列で前記移動体の周囲の画像を撮像して得られた画像データに基づいて、得られた画像ごとに特徴点を抽出して特徴量を計算する学習段階特徴点抽出部と、特徴点判定部から過去画像において得られたトラッキングすべき点の画像座標およびＩＤと、前記時系列で得られた現在の画像とに基づいて、トラッキングすべき点の現在の画像座標を出力する学習段階特徴点トラッキング部と、所定回目画像処理時には前記学習段階特徴点抽出部から得られた特徴点の座標と特徴量にＩＤを付けて保存し、前記所定回目以降の画像処理時に、各画像間でパターン認識により特徴量が類似する特徴点を選出し、前記選出された特徴点が時系列でトラッキング可能であった場合に、その特徴点をランドマーク候補点とし、ランドマーク候補点の数が閾値以下になった場合に残った点をランドマークとして判定する特徴点判定部と、前記画像を撮像しているときの前記移動体の座標を取得するデッドレコニングデータ取得部と、前記デッドレコニングデータ取得部で取得された移動体の座標に基づいて前記特徴点判定部でランドマークと判定された点の座標を測定する特徴点座標測定部と、前記特徴点判定部でランドマークと判定された点の特徴量と、前記特徴点座標測定部で得られた各ランドマークの座標とを保存するランドマークデータ保存部と、を有することを特徴とする。
【００１１】
また、本発明に係る画像処理方法は、移動体に取り付けられたカメラを用いて時系列で前記移動体の周囲の画像を撮像し、前記画像に基づいて、得られた画像ごとに特徴点を抽出して特徴量を計算し、過去画像において得られたトラッキングすべき点の画像座標およびＩＤと、前記時系列で得られた現在の画像とに基づいて、トラッキングすべき点の現在の画像座標を出力し、所定回目画像処理時には前記特徴点抽出部から得られた特徴点の座標と特徴量にＩＤを付けて保存し、前記所定回目以降の画像処理時に、各画像間でパターン認識により特徴量が類似する特徴点を選出し、前記選出された特徴点が時系列でトラッキング可能であった場合に、その特徴点をランドマーク候補点とし、ランドマーク候補点の数が閾値以下になった場合に残った点をランドマークとして判定し、前記画像を撮像しているときの前記移動体の座標を前記画像情報によらずに取得し、前記移動体の座標に基づいて前記ランドマークと判定された点の座標を測定し、前記ランドマークと判定された点の特徴量と座標をランドマークデータとして保存する、ことを特徴とする。
【発明の効果】
【００１２】
本発明によれば、移動体から撮像した画像に基づき、移動に伴う画像の回転変化およびスケール変化が大きい場合（例えば全方位カメラ画像を用いる場合）においても、高速に自己位置推定ができる。
【発明を実施するための最良の形態】
【００１３】
以下、図面を参照して本発明に係る自己位置測定装置の実施形態を説明する。図１は本発明の実施形態に係るランドマーク学習装置のブロック図である。また、図２は本発明の実施形態に係る自己位置算出装置の概略ブロック図、図３は図２の自己位置推定関連部の詳細を示すブロック図、図４は図３の自己位置推定関連部において構成特徴点数が３の場合の特徴点組の例を示す説明図、図５は図３の自己位置推定部における自己位置推定処理手順を示すフローチャートである。
【００１４】
本実施形態に係る自己位置測定装置は、自己位置推定のためのランドマークとなる特徴点を選出するランドマーク学習装置１００と、移動台車移動時に学習したランドマークとマッチングを取ることで自己位置推定を行なう自己位置算出装置２００の二つの装置に分解できる。
【００１５】
以下の説明で、自己位置とは、特に断りがない限り、移動台車の座標と姿勢の情報を含んでいるものとする。
【００１６】
はじめに、ランドマーク学習装置１００について図１を中心に説明する。
【００１７】
画像撮像部１０１は移動台車（図示せず）に搭載されたカメラにより時系列に移動台車周囲のステレオ画像を撮像するものである。画像撮像部１０１はたとえば曲面ミラーを装着した広角カメラが好ましいが、一般的なカメラを利用しても実現可能である。画像撮像部１０１で取得された画像は、特徴点抽出部１０２と特徴点トラッキング部１０３に出力される。
【００１８】
特徴点抽出部１０２は、画像から、ヘッシアン行列と輝度勾配を計算することによって回転変化およびスケール変化に強い特徴点の抽出、特徴量の計算を行なう。抽出した特徴点の画像座標と特徴量を特徴点判定部１０４に出力する。ここで、回転変化およびスケール変化に強い特徴点を抽出し、特徴量を計算する手法は、ここに記す限りではない。
【００１９】
特徴点トラッキング部１０３は、特徴点判定部１０４から過去画像において得られたトラッキングすべき点の画像座標とＩＤ（識別情報）を受け取り、画像撮像部１０１から現在の画像を取得することで、トラッキングすべき点の現在の画像座標を特徴点判定部１０４に出力する。また、トラッキング不能になった点があればトラッキング不能点のＩＤを特徴点判定部１０４に出力する。
【００２０】
特徴点判定部１０４は、初回画像処理時は特徴点抽出部から得られた特徴点の座標と特徴量にＩＤを付けて保存する。
【００２１】
特徴点判定部１０４は、二回目の処理以降は、現在画像における回転変化およびスケール変化に強い特徴点の画像座標、特徴量とトラッキングにより得られたトラッキング中の特徴点の画像座標とトラッキング不能ＩＤを受け取る。トラッキング可能であった点については、画像上で閾値距離内にある回転変化およびスケール変化に強い特徴点の特徴量と保存されているトラッキング可能な点の特徴量のユークリッド距離を計算する。
【００２２】
特徴量のユークリッド距離が閾値以下である点を検出したトラッキング可能点は、引き続きトラッキングを行なうため、特徴点トラッキング部１０３に該当点の画像座標を出力する。閾値以下の点が見つからなかった点やトラッキング不能とされた点のＩＤは消去する。消去されずに残ったトラッキング可能点の個数が閾値以下になった場合、残った点の画像座標と特徴量を特徴点座標測定部１０７に出力し、出力後初めての画像処理は、初回画像処理時と同様の処理を行なう。
【００２３】
ここでは、時系列で特徴点の対応を取るためにユークリッド距離を用いるとしたが、特徴点の対応を取ることができる他の手法を用いることも可能である。
【００２４】
移動制御部１０５は、オドメトリデータを用いてあらかじめ指定された軌道に沿って移動台車を移動させ、デッドレコニングデータ取得部１０６へエンコーダデータを出力する。
【００２５】
デッドレコニングデータ取得部１０６は、移動開始時の世界座標とエンコーダデータから、現在移動台車が位置する世界座標を計算し、特徴点座標測定部１０７に出力する。
【００２６】
特徴点座標測定部１０７は、特徴点判定部１０４から出力された画像座標とステレオ画像を用いて、ステレオ視を行ない、該当する特徴点までの方向と距離を測定する。特徴点までの方向および距離と、デッドレコニングデータから、特徴点の世界座標を算出する。算出された世界座標と特徴量をランドマークデータベース１０８に出力する。ここでは、特徴点座標測定のためステレオ視を用いたが、ステレオ視以外の手法で距離を求めてもよい。また、ステレオ視以外の手法で求める場合は、画像撮像部１０１では単眼画像を取得してもよい。
【００２７】
ランドマークデータベース１０８は、特徴点座標測定部１０７から出力された特徴点の世界座標と特徴量をＩＤ付きで管理する。
【００２８】
つぎに、自己位置算出装置２００について図２および図３を中心に説明する。
【００２９】
図２に示すように、自己位置算出装置２００は、画像撮像部２０１と、自己位置推定関連部２２０と、移動制御部（フリースペース移動部）２１２とを有する。
【００３０】
画像撮像部２０１は、時系列で画像を撮像し、図３に示すように、撮像した画像を自己位置推定関連部２２０の特徴点抽出部２０２と特徴点トラッキング部２０５に出力する。
【００３１】
特徴点抽出部２０２は、画像撮像部２０１が初回画像を取得した場合と、自己位置判定部２１１から指示があった場合に、特徴点抽出部１０２と同様の手法で、画像から回転変化およびスケール変化に強い特徴点を抽出し、特徴量を計算する。抽出した特徴点の画像座標と特徴量を特徴点マッチング部２０４に出力する。
【００３２】
ランドマークデータベース２０３は、前述のランドマーク学習装置１００によって作成されたランドマークデータベース１０８であり、学習された特徴点の世界座標と特徴量をＩＤ付きで管理する。
【００３３】
特徴点マッチング部２０４は、特徴点抽出部２０２から得られた特徴点ｉのｎ次元特徴量Ｅｉとランドマークデータベース２０３上の特徴量Ｌｊ（ｊ＝０，１，・・・，（保存ランドマーク数−１））のユークリッド距離ｄｉｊを計算し、最も小さなｄｉｊ値の逆数をマッチング尤度Ｄｉとする。特徴点抽出部２０２から出力された特徴点のうち、Ｄｉが閾値以下の特徴点は以降の処理には利用しない。Ｄｉを与えるランドマークデータベース２０３上のランドマークｊの世界座標を特徴点ｉの世界座標とする。世界座標が得られた特徴点の世界座標と画像座標とマッチング尤度を特徴点組生成部２０６に出力する。ここではマッチングにユークリッド距離を用いたが、マッチング尤度を出力できるマッチング手法であれば、他の手法を用いてマッチングを行なってもよい。
【００３４】
特徴点トラッキング部２０５は、自己位置判定部２１１から出力されたトラッキングすべき特徴点の過去画像における画像座標と、画像撮像部２０１から得られた過去と現在の画像から、トラッキングすべき特徴点の現在画像における画像座標を計算し、自己位置判定部２１１から同時に送られてきている、トラッキングすべき特徴点の世界座標、マッチング尤度とともに特徴点組生成部２０６に出力する。
【００３５】
特徴点組生成部２０６は、特徴点マッチング部２０４または特徴点トラッキング部２０５から送られてきた特徴点の画像座標を用いて、移動台車から見た各特徴点の方位を算出する。算出された方位をもとに特徴点をソーティングし、図４のように、方位が比較的近いもの同士を集めて、グループＡ，Ｂ，Ｃの三グループにほぼ三等分する。図４の白丸および黒丸は画像中の特徴点であり、特徴点傍の数字はマッチング尤度を表す。また、黒丸は各グループで最もマッチング尤度が高い点（以後グループマスター特徴点と呼ぶ）を示す。
【００３６】
各グループの先頭から順に特徴点の三つ組みを作成していく。図４のグループＣのように、グループ内の特徴点数がグループＡより少ない場合、グループＡの最終点と三つ組になる点は、グループの最終点を代用して三つ組みを生成する。またグループマスター特徴点を各グループで選出しておく。特徴点マッチング部２０４または特徴点トラッキング部２０５から出力された特徴点情報、生成した三つ組み情報、グループマスター特徴点を自己位置推定部２０８に出力する。
【００３７】
図示の例では、三点を用いて推定を行なうため、三グループに分けた上で三つ組みを生成したが、ｎ個の点（ｎ＞３）を用いて推定を行なう際は、ｎ個のグループに分け、ｎ個の特徴点を有する特徴点組を生成する。
【００３８】
地図情報データベース２０７は、ロボット移動可能領域の座標を保存している。
【００３９】
自己位置推定部２０８は、三つ組みｊを構成する各点の世界座標（ｘｉ，ｙｉ）と画像とを用いて得られる移動台車からの方位θｉ（ｉ＝０，１，２）から移動台車の自己位置候補座標（Ｘｊ，Ｙｊ）、姿勢Θjを式１により算出する。
【数１】
2010033447.tif 000003
【００４０】
ただし、ａｉ，ｂｉ，ｃｉ（ｉ＝０，１，２）は式２のように表される定数である。
【数２】
2010033447.tif 000004
【００４１】
三つ組みｊから得られた自己位置候補座標・姿勢と、三つ組みを構成する各特徴点の世界座標を用いて、移動台車から見た、三つ組みを構成する各特徴点の方位を逆算する。逆算した値をθｉ’（ｉ＝０，１，２）として、式３に示す方位尤度ＥＲｊを計算する。
【数３】
2010033447.tif 000005
【００４２】
ここで、式１または式２の分母がゼロ付近になり、丸め誤差が起こりうる場合や、算出された（Ｘｊ，Ｙｊ）が、地図情報データベース２０７から判断できる、地図上で移動台車が位置することができない座標である場合や、ＥＲｊが閾値以下であった場合は、エラーとみなす。エラーが起こった場合は、三つ組みを構成する特徴点のうちマッチング尤度が最も低い特徴点とマッチング尤度が最も低い特徴点が所属するグループのグループマスター特徴点を入れ替え、同様の計算を行なう。入れ替え後もエラーが出る場合はエラーを出した三つ組みを以降の処理で使用しない。
【００４３】
エラーが出なかった三つ組みｊについては式４に示す距離尤度ＥＤｊを計算する。ただし、ａ、ｂ、ｃは三つ組みを構成する特徴点のＩＤである。
【数４】
2010033447.tif 000006
【００４４】
ＥＲｊとＥＤｊの値を重みとして、各三つ組みから計算された自己位置候補座標・姿勢の重み付平均座標・姿勢を（Ｘａｖｅ，Ｙａｖｅ，Θａｖｅ）とする。ここでは、距離尤度と方位尤度を用いて重み付平均を求める手法について述べたが、どちらか一方でもかまわないし、他の尤度を用いてもかまわない。
【００４５】
最後に、エラーを出さなかった自己位置候補座標・姿勢（Ｘｊ，Ｙｊ，Θｊ）と、（Ｘａｖｅ，Ｙａｖｅ，Θａｖｅ）とマッチングした各特徴点の画像中の方位、世界座標を自己位置判定部２１１に出力する。エラーを出さなかった三つ組みが一つもなかった場合は自己位置判定部２１１に「ＮＵＬＬ」を出力する。
【００４６】
時系列自己位置データベース２０９は、自己位置判定部２１１が自己位置推定座標・姿勢を出力する際、出力された座標・姿勢を時系列で保存する。自己位置判定部２１１から自己位置が出力されない時刻の自己位置は、自己位置予測部２１０から出力された座標・姿勢を保存する。
【００４７】
自己位置予測部２１０は、時系列自己位置データベース２０９から時系列の自己位置座標・姿勢を取得し、たとえばカルマンフィルタにより自己位置を予測する。ここでは、カルマンフィルタを用いたが他の予測手法を用いてもよい。
【００４８】
自己位置判定部２１１は、自己位置推定部２０８からＮＵＬＬが出力されている場合は、エラーとする。エラーとなった場合は、現在まで用いていた点を排除し、特徴点抽出部２０２に新たな特徴点を抽出する指令を出し、時系列自己位置データベース２０９に自己位置が出力できなかったことを記録する。
【００４９】
自己位置候補座標・姿勢（Ｘｊ，Ｙｊ，Θｊ）（ｊ＝０，１，・・・，（エラーが出なかった三つ組み数−１））と重み付平均座標・姿勢（Ｘａｖｅ，Ｙａｖｅ，Θａｖｅ）、自己位置予測部２１０から出力された予測自己位置座標・姿勢（Ｘｐ，Ｙｐ，Θｐ）のそれぞれについて、マッチングした特徴点の世界座標を用いて、移動台車から見たマッチングした特徴点の方位θｋｍ（ｋ＝０，１，・・・，（エラーが出なかった三つ組みの数−１），ａｖｅ，ｐ  ｍ＝０，１，・・・，（マッチングした特徴点の数−１））を逆算する。式５から得られる番号ｆの座標・姿勢（Ｘｆ，Ｙｆ，Θｆ）を最終自己位置座標・姿勢候補とする。
【数５】
2010033447.tif 000007
【００５０】
ただし、θｔは定数、Ｚ（Ｐ）は命題Ｐが真のときに１、偽のときに０（ゼロ）を返す関数である。
【００５１】
ここで、｜θｆｍ−θｍ｜＜θｔを満たす特徴点ｍの個数が閾値以下であった場合や、（Ｘｆ，Ｙｆ，Θｆ）が時系列自己位置データベース２０９より得られる、過去の時系列自己位置座標・姿勢から移動台車の機構的に到達できない位置・姿勢であった場合は、自己位置推定エラーとする。エラーとなった場合は、現在まで用いていた点を排除し、特徴点抽出部２０２に新たな特徴点を抽出する指令を出し、時系列自己位置データベース２０９に自己位置が出力できなかったことを記録する。また、自己位置推定部２０８からＮＵＬＬが出力されている場合も含めて、エラーが連続して閾値回数以上続く場合は、現在位置での自己位置推定が困難であると判断し、フリースペース移動部２１２に移動指令を出す。
【００５２】
エラーではない場合は、｜θｆｍ−θｍ｜＜θｔを満たす特徴点ｍをトラッキングするよう特徴点トラッキング部２０５に指令を出し、時系列自己位置データベース２０９に推定された自己位置を登録する。
【００５３】
フリースペース移動部２１２は、通常は目的地までの移動を行なっているが、自己位置判定部２１１から指令があった場合、障害物検出センサで判定した、長距離移動できる方向へ移動する。
【００５４】
図５は、図３中の自己位置推定部２０８における自己位置推定処理手順を示すフローチャートである。ステップＳ１で、各三点組について、式１により自己位置候補座標・姿勢（Ｘｃ，Ｙｃ，Θｃ）を計算し、式３により方位尤度ＥＲｃを計算する。つぎに、ステップＳ２で、自己位置候補座標（Ｘｃ，Ｙｃ）が移動不能領域にある場合、式１または式２でゼロ付近の値での割り算がある場合、方位尤度ＥＲｃが閾値以下の場合、のいずれかの場合は、その三点組の処理は断念して、ステップＳ３で特徴点の入れ替えを行なう。
【００５５】
全三点組についての処理が終わったときに、処理を断念しなかった組の数がゼロの場合はステップＳ４でＮＵＬＬ出力とする。処理を断念しなかった組の数が１以上の場合は、ステップＳ５で、重み付平均座標・姿勢（Ｘａｖｅ，Ｙａｖｅ，Θａｖｅ）を計算し、ステップＳ６で、重み付平均座標・姿勢と、処理を断念しなかった各組についての自己位置候補座標・姿勢（Ｘｋ，Ｙｋ，Θｋ）とを出力する。
【００５６】
以上、本発明の実施形態について説明したが、これは単なる例示であって、本発明はこれに限定されるものではない。
【００５７】
たとえば、図１に示すランドマーク学習装置１００によって得られたランドマークデータベースは、必ずしも図２および図３に示す自己位置算出装置２００で利用する必要はなく、他の構成の自己位置算出装置で利用することもできる。
【００５８】
また、図２および図３に示す自己位置算出装置２００で利用するランドマークデータベース２０３は、必ずしも図１に示すランドマーク学習装置１００によって得られたものでなくてもよい。たとえば、ランドマーク学習装置では必ずしも高速処理を求められないので、ランドマーク学習装置ではトラッキングを行なわない従来の方法を用いてランドマークデータベースを生成し、その後の自己位置算出装置ではトラッキングを用いた高速処理を行なうようにしてもよい。
【００５９】
さらに、上記実施形態では移動体として移動台車を用いるものとしたが、必ずしも車輪が付いたものである必要はなく、種々の走行手段がありうる。
【図面の簡単な説明】
【００６０】
【図１】本発明の実施形態に係るランドマーク学習装置のブロック図。
【図２】本発明の実施形態に係る自己位置算出装置の概略ブロック図。
【図３】図２の自己位置推定関連部の詳細を示すブロック図。
【図４】図３の自己位置推定関連部において構成特徴点数が３の場合の特徴点組の例を示す説明図。
【図５】図３の自己位置推定部における自己位置推定処理手順を示すフローチャート。
【符号の説明】
【００６１】
１００：ランドマーク学習装置
１０１：画像撮像部
１０２：特徴点抽出部
１０３：特徴点トラッキング部
１０４：特徴点判定部
１０５：移動制御部
１０６：デッドレコニングデータ取得部
１０７：特徴点座標測定部
１０８：ランドマークデータベース
２００：自己位置算出装置
２０１：画像撮像部
２０２：特徴点抽出部
２０３：ランドマークデータベース
２０４：特徴点マッチング部
２０５：特徴点トラッキング部
２０６：特徴点組生成部
２０７：地図情報データベース
２０８：自己位置推定部
２０９：時系列自己位置データベース
２１０：自己位置予測部
２１１：自己位置判定部
２１２：移動制御部（フリースペース移動部）
２２０：自己位置推定関連部
【図１】
2010033447.tif 000008
【図２】
2010033447.tif 000009
【図３】
2010033447.tif 000010
【図４】
2010033447.tif 000011
【図５】
2010033447.tif 000012