JP2004132933A

JP2004132933A - アクティブセンサの位置・姿勢推定方法及びその装置、並びにアクティブセンサの位置・姿勢推定プログラム

Info

Publication number: JP2004132933A
Application number: JP2002300173A
Authority: JP
Inventors: Saika O; 王　彩華; Hideki Tanahashi; 棚橋　英樹; Hidekazu Hirayu; 平湯　秀和; Katsutaka Sato; 佐藤　雄隆
Original assignee: Japan Science and Technology Agency; Softopia Japan Foundation
Current assignee: Japan Science and Technology Agency; Softopia Japan Foundation
Priority date: 2002-10-15
Filing date: 2002-10-15
Publication date: 2004-04-30

Abstract

【課題】全方位画像の統計量を用いて、数少ない参照地点に基づいてセンサの現在の位置と姿勢を精度良くロバストに推定する。
【解決手段】複数の参照地点でアクティブセンサ１１が取得した全方位画像に基づいて各参照地点の全方位エッジヒストグラムを生成する。アクティブセンサ１１が取得した現在位置の全方位画像に基づき、現在位置の全方位エッジヒストグラムを生成する。参照地点と現在位置の全方位エッジヒストグラムを動的計画法によりマッチングし、各方位におけるヒストグラムの総合シフト量を算出する。総合シフト量に基づき各参照地点に対する現在位置のアクティブセンサ１１の回転量と移動方向及び相対的な移動量を算出する。現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサ１１の現在位置と回転角度を推定する。
【選択図】　図１

Description

【０００１】
【発明の属する技術分野】
本発明は、アクティブセンサの位置・姿勢推定方法及びその装置、並びにアクティブセンサの位置・姿勢推定プログラムに関するものである。
【０００２】
【従来の技術】
アクティブセンサの位置及び姿勢の推定は、ロボットナビゲーションやコンピュータビジョンにおける最も重要な課題となっている。
【０００３】
近年現れた全方位センサは周りの環境の３６０°視野角の情報を同時に取得できるため、ロボットやセンサの位置及び推定にも大変有効であり、ロボットや自動監視分野では注目が集まっている。
【０００４】
上記のような全方位センサから得られる全方位画像は、３６０°のシーンの情報を持っているため、ロボットの位置及び姿勢推定によく使われ、全方位画像を用いたロボット位置推定については、様々な方法が提案されている。
【０００５】
これらの方法は主としてビュー・ベースド（Ｖｉｅｗ−ｂａｓｅｄ）・マッチングと、幾何特徴マッチングに分けることができる。
ビュー・ベースド・マッチングでは、環境中の学習経路上の各地点の全方位画像から抽出された特徴量を予め記憶し、センサの位置を推定するとき、現位置で得られた全方位画像から同じく特徴量を抽出し、予め記憶した学習画像の特徴量と比較することによって、学習経路にある最も近い地点を求める。
【０００６】
例えば、ノン・モノトニック（Ｎｏｎ−ｍｏｎｏｔｏｎｉｃ）連続ＤＰを用いて、全方位画像から抽出した回転不変特徴量の時空間系列をマッチングすることによって、ロボットの位置推定を行う技術が提案されている（非特許文献１参照）。
【０００７】
又、時空間系列画像を固有空間で表し、画像の固有空間の特徴量と学習画像のトラジェクトリとのマッチングによって、位置推定を行うことも提案されている（非特許文献２参照）。
【０００８】
さらに、全方位画像の時空間系列の自己相関画像から回転不変特徴を抽出し、固有空間内でのそれらの特徴量に対してＫ−Ｌ展開で固有空間を構築し、固有空間内で最も接近した学習画像を検索することによって位置推定を行うことが提案されている（非特許文献３参照）。
【０００９】
一方、幾何特徴マッチングでは、シーンの中にあるエッジやコーナーなどの幾何特徴を用いる（非特許文献４〜６参照）。
非特許文献４では、全方位画像を用いて、環境マップとシーンの垂直エッジをマッチングし、ロボットの位置を推定している。
【００１０】
或いは、時系列の全方位画像のフレーム間でコーナ追跡（非特許文献５参照）や、床のエッジ追跡（非特許文献６参照）で得られた特徴点の対応付けからセンサの移動パラメータ（相対的な位置、姿勢）を推定している。
【００１１】
【非特許文献１】
西村，野崎，岡，”Ｎｏｎ−ｍｏｎｏｔｏｎｉｃ連続ＤＰによるスプッティングに基づく移動ロボットの時系列を用いた大局的な位置推定”，電子情報通信学会論文誌　１９９８年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ８１−Ｄ−ＩＩ，Ｎｏ．８，ｐ．１８７６−１８８４
【非特許文献２】
エヌ・ウィンターズ（Ｎ．　Ｗｉｎｔｅｒｓ），ジェイ・ギャスパー（Ｊ．　Ｇａｓｐａｒ），ジィ・ラセイ（Ｇ．　Ｌａｃｅｙ），ジェイ・サントス−ヴィクタ（Ｊ．　Ｓａｎｔｏｓ−Ｖｉｃｔｏｒ），”ロボットナビゲーションのための全方位画像（Ｏｍｎｉ−ｄｉｒｅｃｔｉｏｎａｌ　Ｖｉｓｉｏｎ　ｆｏｒ　Ｒｏｂｏｔ　Ｎａｖｉｇａｔｉｏｎ　）”，「全方位画像研究会」における米国電気電子学会論文誌（Ｐｒｏｃ．　ｏｆ　ＩＥＥＥ　Ｗｏｒｋｓｈｏｐ　ｏｎ　　Ｏｍｎｉ−ｄｉｒｅｃｔｉｏｎａｌ　Ｖｉｓｉｏｎ　），米国，２０００年，ｐ．２１−２８
【非特許文献３】
岩佐，粟飯原，横矢，竹村，”全方位画像を用いた記憶に基づく位置推定”，電子情報通信学会論文誌　２００１年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ８４−Ｄ−ＩＩ，Ｎｏ．２，ｐ．３１０−３２０
【非特許文献４】
八木（Ｙ．　Ｙａｇｉ），西沢（Ｙ．　Ｎｉｓｈｉｚａｗａ　），谷内田（Ｍ．　Ｙａｃｈｉｄａ），”全方位画像センサ（ＣＯＰＩＣＳ）を備えた移動ロボットのためのマップ・ベースのナビゲーション（Ｍａｐ−Ｂａｓｅｄ　Ｎａｖｉｇａｔｉｏｎ　ｆｏｒ　ａ　Ｍｏｂｉｌｅ　Ｒｏｂｏｔ　ｗｉｔｈ　Ｏｍｎｉｄｉｒｅｃｔｉｏｎａｌ　ＩｍａｇｅＳｅｎｓｏｒ　ＣＯＰＩＣＳ）”，「ロボットとオートメーション」の米国電気電子学会論文誌（ＩＥＥＥ　Ｔｒａｎｓ．　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ），米国，１９９５年，Ｖｏｌ．１１，Ｎｏ．５，ｐ６３４−６４８
【非特許文献５】
ジェイ・ギャスパー（Ｊ．　Ｇａｓｐａｒ），エヌ・ウィンターズ（Ｎ．　Ｗｉｎｔｅｒｓ），ジィ・ラセイ（Ｇ．　Ｌａｃｅｙ），ジェイ・サントス−ヴィクタ（Ｊ．　Ｓａｎｔｏｓ−Ｖｉｃｔｏｒ），”全方位画像カメラを用いたヴィジョン・ベースのナビゲーションと環境画像（Ｖｉｓｉｏｎ　Ｂａｓｅｄ　Ｎａｖｉｇａｔｉｏｎ　ａｎｄ　Ｅｎｖｉｒｏｎｍｅｎｔａｌ　Ｒｅｐｒｅｓｅｎｔａｔｉｏｎｓ　ｗｉｔｈ　ａｎ　Ｏｍｎｉｄｉｒｅｃｔｉｏｎａｌ　Ｃａｍｅｒａ　）”，「ロボットとオートメーション」の米国電気電子学会論文誌（ＩＥＥＥ　Ｔｒａｎｓ．　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ），米国，２０００年，Ｖｏｌ．１６，Ｎｏ．６，ｐ８９０−８９８
【非特許文献６】
山澤，八木，谷内田，”移動ロボットのナビゲーションのための全方位視覚センサ　Ｈｙｐｅｒ　Ｏｍｎｉ　Ｖｉｓｉｏｎの提案”，電子情報通信学会論文誌　１９９６年，Ｄ−ＩＩ，Ｖｏｌ．Ｊ７９−Ｄ−ＩＩ，Ｎｏ．５，ｐ．６９８−７０７
【００１２】
【発明が解決しようとする課題】
非特許文献１〜３に示された方法は、回転不変特徴を用いるため、センサの回転によらずに位置を推定することが可能である。しかしながら、その反面、センサの向きを推定するには他の処理が必要となる。
【００１３】
又、これらの方法では、現地点から学習した経路上にある最も近い点しか認識できず、経路から離れた場合の現地点の正確な位置の推定は難しい。
非特許文献４〜６に示された方法では、一般的に複雑な実環境では、安定的な特徴点抽出や特徴点の対応付けが困難であり、特徴点抽出の誤差や誤対応が位置推定に大きな影響を与える。
【００１４】
本発明は、全方位画像がシーンの全体的な情報量を持っていることに注目し、上記問題点を解決するためになされたものである。すなわち、本発明は、全方位画像の統計量を用いて、数少ない参照地点に基づいてセンサの現在の位置と姿勢を精度良くロバストに推定するアクティブセンサの位置・姿勢推定方法及びその装置、並びに位置・姿勢推定プログラムを提供することを目的としている。
【００１５】
そして、本発明では、全方位画像における個々のエッジをマッチングすることなく、その統計量であるヒストグラムを用い、ノイズや環境の複雑さに対する頑丈性が高いアクティブセンサの位置・姿勢推定方法及び装置、並びにアクティブセンサの位置・姿勢推定プログラムを提供することにある。
【００１６】
【課題を解決するための手段】
上記問題点を解決するため、請求項１記載の発明は、全方位画像を取得可能であり、環境内を移動可能なアクティブセンサの位置・姿勢推定方法において、環境内の複数の参照地点で全方位画像を取得し、同全方位画像に基づいて各参照地点における全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成して記憶し、次に、前記環境内の任意の地点に移動した際に、同任意の地点（以下、現在位置という）で全方位画像を取得し、現在位置における全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成し、前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出し、前記総合シフト量に基づいて、前記各参照地点に対する現在位置のアクティブセンサの回転量、移動方向及び相対的な移動量を算出し、前記現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの現在位置と姿勢を推定することを特徴とするアクティブセンサの位置・姿勢推定方法を要旨とするものである。
【００１７】
請求項２の発明は、請求項１において、前記マッチングは、動的計画法によるものであり、総合シフト量は、動的計画法にて得られた最小コストパスに基づくものであることを特徴とする。
【００１８】
請求項３の発明は、アクティブセンサにて取得した環境内の複数の参照地点における全方位画像を入力し、同全方位画像に基づいて全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成する第１生成手段と、前記生成した各参照地点の参照地点ヒストグラムを記憶する記憶手段と、環境内の任意の地点に移動した際に、同任意の地点（以下、現在位置という）で全方位画像を入力し、現在位置おける全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成する第２生成手段と、前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出する第１算出手段と、前記総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出する第２算出手段と、現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの位置と姿勢を推定する推定手段とを備えたことを特徴とするアクティブセンサの位置・姿勢推定装置を要旨とするものである。
【００１９】
請求項４の発明は、請求項３において、前記第１算出手段は動的計画法により演算することを特徴とする。
請求項５の発明は、コンピュータを、環境内の複数の参照地点における全方位画像をアクティブセンサから入力し、同全方位画像に基づいて全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成する第１生成手段と、前記生成した各参照地点の参照地点ヒストグラムを記憶する記憶手段と、環境内の任意の地点（以下、現在位置という）でアクティブセンサが取得した全方位画像に基づいて、現在位置おける全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成する第２生成手段と、前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出する第１算出手段と、前記総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出する第２算出手段と、現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの位置と姿勢を推定する推定手段として機能させるためのアクティブセンサの位置・姿勢推定プログラムを要旨とするものである。
【００２０】
【発明の実施の形態】
以下、本発明の位置・姿勢推定装置を具体化した一実施形態を図１〜図１０を参照して説明する。
【００２１】
本実施形態の位置・姿勢推定装置は、アクティブセンサ１１とコンピュータ１６にて構成されている。
図１はアクティブセンサの電気的構成を示すブロック図である。図２はアクティブセンサ１１の機械的構成の概略図である。
【００２２】
アクティブセンサ１１は、複数の車輪を有する車体βを備えており、車体β内に設けられた図示しない電動モータにより前記車輪が駆動されて環境Ｋ内の任意の位置に向けて自動走行（直線及び曲線に沿った走行を含む）及びが可能である。なお、図２においては、説明の便宜上、環境Ｋは、車体βに比較して、小さく図示している。
【００２３】
アクティブセンサ１１は、複数の３眼ステレオユニット（以下、単にステレオユニット１２という）、メモリユニット１５、同期信号発生器１７等を備えている。メモリユニット１５、コンピュータ１６、同期信号発生器１７等は車体β内に格納されている。
【００２４】
前記複数のステレオユニット１２は、全方位画像取得手段に相当する。
又、前記コンピュータ１６は、第１生成手段、第２生成手段、第１算出手段、第２算出手段及び推定手段に相当する。
【００２５】
アクティブセンサ１１は、３次元空間内の、全ての方向におけるカラー画像と３次元情報を、同時刻にリアルタイムで取得することが可能な装置である。
ステレオユニット１２は、３個のビデオカメラからなり、各ステレオユニット１２は正２０面体の各面上にそれぞれ配置されている。
【００２６】
そして、各ステレオユニット１２は同特性を備え、各面に配置したステレオユニット１２により、全方向のカラー画像と白黒画像（距離画像）を同一時刻にリアルタイムで取得可能なシステムである。これによって、３次元空間上の全ての方向におけるカラー画像と３次元情報を同一時刻に得ることができる。
【００２７】
又、同じ特性を持つステレオユニット１２を正２０面体の各面上に配置したことにより、３次元空間を均等に分割し、高解像度の情報の取得が可能である。なお、このアクティブセンサ１１は、「”実環境センシングのための全方向ステレオシステム（ＳＯＳ）”、電気学会論文誌Ｃ．Ｖｏｌ．１２１−Ｃ，Ｎｏ．５，ｐｐ．８７６−８８１，２００１」に記載されている。
【００２８】
ステレオユニット１２では、図１に示すように、１つの基準ビデオカメラＶＣｓと、一対の参照用ビデオカメラＶＣとからなる。そして、参照用ビデオカメラＶＣは基準ビデオカメラＶＣｓの光軸を交線として、互いに直交する一対の平面に含ませるように配置されている。そして、これらのカメラにより、２つのステレオペアを構成するように配置されている。
【００２９】
各ステレオユニット１２からは、１枚のカラー画像と２枚の白黒画像とからなるステレオ画像が取得され、全方向の２０枚のカラー画像と４０枚の白黒画像を１セットとして１５セット／秒でメモリユニット１５に転送する。メモリユニット１５は、転送されてきた全方位の画像データ（以下、全方位画像という）を記憶する。
【００３０】
各ステレオユニット１２の各ビデオカメラには、同期信号発生器１７から共通の外部同期信号が供給されている。このことにより、ディジタル化されたフレームにおいて、完全に同期した画像データが得られる。
【００３１】
コンピュータ１６は、所定の周期毎に、同コンピュータ１６内に備えているＲＯＭ１６ａに予め格納した位置・姿勢推定プログラムを実行する。前記ＲＯＭ１６ａを備えたコンピュータ１６は記憶手段に相当する。
【００３２】
又、コンピュータ１６は、メモリユニット１５にアクセスし、全方位画像をその時々に取得する。
（作用）
さて、以上のように構成された位置・姿勢推定装置の作用を図３〜図１０を参照して説明する。
【００３３】
図３は、コンピュータ１６が実行する位置・姿勢推定プログラムのフローチャートであり、コンピュータ１６は、所定周期毎に実行し、アクティブセンサ１１の現在位置及び姿勢を推定する。
【００３４】
以下、各ステップ毎に説明する。
１．　Ｓ１０（エッジの検出）
ステップ（以下、Ｓで表す）１０では、エッジ検出を行う。
【００３５】
すなわち、基準ビデオカメラＶＣｓ（センタカメラ）が取得した全方位画像に対してＬｏＧ（Ｌａｐｌａｓｉａｎ　ｏｆ　Ｇａｕｓｓｉａｎ）フィルタを通し、ゼロクロス点をエッジとして検出する。
【００３６】
２．　Ｓ２０（最大エッジ勾配の算出）
次に、最大エッジ勾配の算出を行う。
すなわち、Ｓｏｂｅｌフィルタにて各エッジ画素におけるエッジの勾配（強度）を計算し、画像中の最大エッジ勾配（すなわち、最大エッジ強度）を求める。
【００３７】
これは、ＬｏＧフィルタはノイズに影響されやすいため、ノイズや照明の影響で偽のエッジ多く存在するためであり、これを除くための処理である。
この処理によって、強度が最大エッジ勾配値の５％未満のエッジ画素をノイズとしてエッジから削除する。
【００３８】
３．　Ｓ３０（現在位置のエッジヒストグラムの生成）
次に、Ｓ３０において、各ステレオユニット１２のセンタカメラである基準ビデオカメラＶＣｓで得られたエッジ点から、３６０°のパノラマエッジ画像座標系に写像し、縦方向投影のエッジヒストグラムを生成する。エッジヒストグラムは統計量に相当する。
【００３９】
前記３６０°のパノラマエッジ画像は３６０°の円筒画像ともいう。すなわち、この円筒画像におけるエッジを縦方向（円筒の軸心に沿った方向）に投影して、全方位エッジヒストグラムを生成するのである。
【００４０】
図５は、生成された全方位エッジヒストグラムの例を示している。図５において、横軸は０〜３６０°の範囲を示し、縦軸はそのヒストグラムである。図４は、図５に対応するパノラマエッジ画像を示している。
【００４１】
（位置・姿勢と全方位エッジヒストグラムとの関係）
ここで、アクティブセンサ１１の位置・姿勢とヒストグラムとの関係を説明する。
【００４２】
アクティブセンサ１１の移動や回転は、アクティブセンサ１１で得られた全方位画像のエッジヒストグラムのシフトを引き起こす。
アクティブセンサ１１の回転によって生じたエッジヒストグラムの回転量はすべての方位角において一定である。しかし、アクティブセンサ１１の平行移動で生じたエッジヒストグラムの移動量は移動方向とエッジの方位角に関係する。
【００４３】
図６に示すように、アクティブセンサ１１が参照地点αからある移動方向ωに向いて動いたとき、ω±ｎπ，ｎ＝０，１の方位角においてはエッジヒストグラムの移動量が小さい。ｎ＝０は移動方向ωであり、ｎ＝１は移動方向ωとは反対方向の意味である。
【００４４】
なお、総合シフト量とは、前記回転量と移動量の合計の量である。又、移動方向ωは、図６に示すように参照地点αを中心として所定の方向の向きを０°としたとき、その０°からω°離れた方向をいう。
【００４５】
ω±（２ｎ＋１）π／２の方位角においてはそのエッジヒストグラムの移動量が大きくなる。
アクティブセンサ１１が移動方向ωに沿う移動距離をＬとし、移動しながら回転角度φにて回転したとする。
【００４６】
この場合、ある方位角θに位置するエッジピクセルの円筒座標系（３６０°のパノラマエッジ画像座標系）における総合シフト量δ_θは次の式で決まる。
【００４７】
【数１】

ここで、ｄ_θは方位角θに位置するエッジの３次元での奥行き（アクティブセンサ１１中心までの距離）である。実際には、多くの場合同じ方位にあるエッジは異なる奥行きをもっているため、式（１）は理想的な場合（同じ方位のエッジは同一の奥行きを持つ場合）だけを表している。
【００４８】
式（１）は、エッジの奥行きに影響されるが、図７（ｃ）に示すように、ｓｉｎで近似することができ、ｓｉｎ曲線と同様に２πの周期を持つ。その総合シフト量の符号はπ周期毎に反転する。
【００４９】
なお、図７（ｃ）は全方位エッジヒストグラムの総合シフト量を式（１）で計算した場合の曲線とｓｉｎ曲線を表した説明図である。
同図において、Ａはｓｉｎ曲線（正弦波曲線）であり、Ｂは、円筒形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量を式（１）に基づいて演算した曲線である（図７（ａ）参照）。
【００５０】
又、Ｃは正方形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量を式（１）に基づいて演算した曲線である（図７（ｂ）参照）。
【００５１】
本実施形態では、式（１）の符号がπ周期毎に反転する特徴を利用して、全方位エッジヒストグラムの総合シフト量δ_θからアクティブセンサ１１の移動方向ωと回転角度φを簡単かつロバストに推定するのである。
【００５２】
４．　Ｓ４０（動的計画法によるマッチング：ＤＰマッチング）
Ｓ４０では、アクティブセンサ１１の現在位置の全方位エッジヒストグラムと各参照地点の全方位エッジヒストグラムをＤＰマッチングし、それぞれの方位角（各参照地点の方位角）におけるヒストグラムの総合シフト量δ_θを求める。
【００５３】
現在位置の全方位エッジヒストグラムは現在位置ヒストグラムに相当し、参照地点の全方位エッジヒストグラムは参照地点ヒストグラムに相当する。
この総合シフト量δ_θを求めることにより、各参照地点に対するアクティブセンサ１１の現在位置の移動方向ωと回転角度φを算出する。
【００５４】
なお、環境Ｋ内の複数の参照地点、すなわち、環境Ｋ内において、アクティブセンサ１１が移動できる複数の位置は参照地点とされており、同参照地点では、アクティブセンサ１１にて、予め全方位画像が取得されている。そして、同全方位画像に基づいて各参照地点における全方位エッジヒストグラムがコンピュータ１６にてそれぞれ生成されており、それらの全方位エッジヒストグラムは、図示しないＲＯＭ１６ａに予め格納されている。
【００５５】
以下、各参照地点に対する、アクティブセンサ１１の現在位置の全方位エッジヒストグラムの総合シフト量δ_θの算出の仕方を詳細に説明する。
（動的計画法マッチング（ＤＰマッチング）について）
ここでは、現在位置の全方位エッジヒストグラムと、ある参照地点の全方位エッジヒストグラムのＤＰマッチングについて、すなわち、２つの全方位エッジヒストグラムをＤＰマッチングする方法を説明する。
【００５６】
参照地点と現在位置の全方位エッジヒストグラムのそれぞれを
参照地点：Ｈｒ＝｛ｈｒ（ｉ），ｉ＝０，…，…，Ｎ−１｝
と
現在位置：Ｈｃ＝｛ｈｃ（ｊ），ｊ＝０，…，…，Ｎ−１｝
とする。
【００５７】
Ｎは３６０の倍数であり、エッジヒストグラムを生成するときの方位角θで決まる。本実施形態では、Ｎ＝７２０である。すなわち、エッジヒストグラムの角度分解能は０．５°である。参照地点の全方位エッジヒストグラムＨ_ｒの１つのピンｈ_ｒ（ｉ）は、移動と回転によってアクティブセンサ１１の現在位置の全方位エッジヒストグラムＨ_ｃにおいてシフト量ｓ_ｉが生じたとき、ｈ_ｒ（ｉ）とｈ_ｃ（ｉ＋ｓ_ｉ）と似ていると仮定することができる。
【００５８】
ｈ_ｒ（ｉ）とｈ_ｃ（ｉ＋ｓ_ｉ）との差の二乗をｈ_ｒ（ｉ）とｈ_ｃ（ｉ＋ｓ_ｉ）との間のマッチングコストとすると、次のマッチングコストマトリクスＣ（ｓ，ｉ）が得られる。
【００５９】
【数２】

ここで全方位エッジヒストグラムが２πの周期を持つため、ｉ＋ｓ≧Ｎの場合、
ｈ_ｃ（ｉ＋ｓ）≡ｈ_ｃ（ｉ＋ｓ−Ｎ）とする。
【００６０】
｛（ｈ_ｒ（ｉ），ｈ_ｃ（ｉ＋ｓ_ｉ）），ｉ＝０，…，Ｎ−１｝が正しいマッチングである場合、それらのマッチングペアはマッチングコストマトリクスＣ（ｓ，ｉ）の中でコストが低く、かつ式（１）の形をした曲線をなす。ここでは、（ｈ_ｒ（ｉ），ｈ_ｃ（ｉ＋ｓ_ｉ））はｈ_ｒ（ｉ）とｈ_ｃ（ｉ＋ｓ_ｉ）とのマッチングペアを表している。
【００６１】
図８は、アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（ｓ，ｉ）を示す。なお、説明の便宜上、マトリクス中のマッチングコストの低いパスをセンタリングし、上下をカットしている。又、図中、「．−＋＊＆％＃＄＠ＡＢＣＤ」の各記号は、コストのレベルをそれぞれ示し、「．−＋＊＆％＃＄＠ＡＢＣＤ」の順番は、左から右に向かってコストの低い順から高い順に並べている。すなわち、マッチングコストは「．」＜「 − 」＜「＋」＜「＊」＜「＆」＜「％」＜「＃」＜「＄」＜「＠」＜「Ａ」＜「Ｂ」＜「Ｃ」＜「Ｄ」の大小関係となっている。
【００６２】
図中、縦軸はエッジのシフト量ｓ、横軸はｉであり、方位角θに相当する。又、図中、Ｃ（ｓ，ｉ）の中のｓｉｎ曲線に似たコストの低い曲線は各方位角θにおけるエッジヒストグラムの総合シフト量に対応する。Ｃ（ｓ，ｉ）の中のｓｉｎ曲線に似たコストの低い曲線を探索することで、Ｈ_ｒとＨ_ｃの間の全方位ヒストグラムの総合シフト量が求められる。
【００６３】
本実施形態では、計算コストの低い動的計画法（ＤＰ）を用いて、Ｃ（ｓ，ｉ）から周期２πを持ち、かつ連続した最小コストのパスを求め、そのパスからロバストに回転角度φと移動方向ωを推定する。
【００６４】
Ｃ（ｓ，ｉ）から周期２πを持ちかつ連続した最小コストのパスは次の条件付き最小化問題に定義することができる。
【００６５】
【数３】

ここで、ｓ_ｉは求めたいエッジヒストグラムの総合シフト量である。
【００６６】
ｈ_ｒ（ｉ）とｈ_ｃ（ｊ）の周期がＮであるため、Ｃ（ｓ，ｉ）のインデックスのｓとｉに関しては、ｓ±Ｎ→ｓとｉ±Ｎ→ｉで計算される。
パスが特定の行ｋから始まる（すなわち、ｓ_０＝ｋ）と仮定した場合には、式（２）の最小化は次のように動的計画法で求めることができる。
【００６７】
【数４】

【００６８】
【数５】

【００６９】
【数６】

Ｓ（ｓ，ｉ）を計算するときは、ｍｉｎ｛Ｓ（ｓ−１，ｉ−１），Ｓ（ｓ，ｉ−１），Ｓ（ｓ＋１，ｉ−１）｝の中のいずれが最小になっていたかを記憶しておき、Ｃ_ｍｉｎ（ｋ）まで来たパスを逆に辿れば、最小コストのパス（以下、最小コストパスという）が得られる。
【００７０】
ｋ＝０，…，Ｎ−１に対して、上記のようにＣ_ｍｉｎ（ｋ）を計算し、そのうち、
最小値Ｃ＾_ｍｉｎ（ｋ）＝Ｃ_ｍｉｎ（ｋ＾）
を求める。
【００７１】
Ｃ_ｍｉｎ（ｋ＾）が対応しているパスがＨ_ｒとＨ_ｃの間の最適マッチングとする。
以下、上記パスを最適マッチングパスという。
上記計算により、２πの周期を持ち、かつ連続性のあるエッジヒストグラムのマッチングパスを探索できる。
【００７２】
そして、以下、現在位置の全方位エッジヒストグラムと他の参照地点の全方位エッジヒストグラムのマッチングについても同様に処理する。
５．　Ｓ５０（姿勢と移動方向の推定）
Ｓ５０では、Ｓ４０で得られたアクティブセンサ１１の現在位置と参照地点の全方位エッジヒストグラムの総合シフト量から参照地点に対するアクティブセンサ１１の移動方向ωと回転角度φを推定する。
【００７３】
（回転の推定）
まず、アクティブセンサ１１の回転の推定について説明する。
ＤＰマッチングで得られた最小コストパスであるｓ_ｉ，（ｉ＝０，…，Ｎ−１）は、アクティブセンサ１１の現在位置と参照地点のそれぞれの全方位エッジヒストグラムＨ_ｃとＨ_ｒの間の総合シフト量を表している。図９中のｓｉｎ曲線の近傍に示された波形はＤＰマッチングで得られた最小コストパスを示す。
【００７４】
式（１）で示したように、これらの総合シフト量はアクティブセンサ１１の回転角度φで生じたヒストグラム全体の回転量と、移動方向ωにおける平行移動で生じた各方位角での移動量からなる。
【００７５】
式（１）から分かるように、ヒストグラムの総合シフト量は回転角度φに相当する回転量ｓ_φを引けば、引いた後のシフト量はπ周期毎に反転する。すなわち、回転角度φを中心にπ周期で、上下反転する（図９参照）。
【００７６】
従って、本実施形態のＳ５０では、回転量ｓ_φは次の式で演算することにより推定する。
【００７７】
【数７】

【００７８】
【数８】

すなわち、回転量ｓ_φがエッジヒストグラムのシフト量ｓ_ｉ，（ｉ＝０，…，Ｎ−１）を上下２等分に分けることになる（図９参照）。
【００７９】
（アクティブセンサ１１の移動方向ωの推定）
次に、式（１）がｓｉｎ曲線の周期性を保たれていることを利用して、アクティブセンサ１１の移動方向ωをロバストに推定する。
【００８０】
この推定の根拠は下記の通りである。
上述したように回転量ｓ_φを引いた後のエッジヒストグラムのシフト量ｓ_ｉ’をｓｉｎ曲線に近似する。しかし、図７に示しているように、これらのシフト量は、エッジの奥行きや空間の形に影響される。
【００８１】
エッジヒストグラムのシフト量ｓ_ｉ’は、エッジの奥行きや空間の形の影響でｓｉｎ曲線からずれるが、図７に示すように（０，π）の区間では、正の値を、（π，２π）の区間では、負の値をもっていると仮定することができる。
【００８２】
従って、シフト量ｓ_ｉ’の符号を用いて、ｓｉｎ曲線をロバストに当てはめることができる。
ここで、回転角度φにより、シフト量ｓ_ｉ’の−１，０，１の３つの値に変換し、−１と１の値に対応するシフト量ｓ_ｉ’の中心がそれぞれなるべくｓｉｎ曲線の負と正のピークに対応するように移動方向ωを決定する。
【００８３】
これは次の最大化問題になる。
【００８４】
【数９】

【００８５】
【数１０】

ここで、回転角度φに相当するｓ_φとｓ_φ±１を０にすることにより、エッジヒストグラムのシフト量ｓ_ｉ’は符号へのノイズの影響を軽減することができる。
【００８６】
式（９）の左辺の微分を０とすると、次の方程式が得られる。
【００８７】
【数１１】

上の式（１１）から次のように、移動方向ωを直接求める。
【００８８】
【数１２】

なお、式（１１）は、式（９）の最大化のみならず、最小化も含んでいる。ｔａｎ（θ）の周期がπであるから、移動方向ω又はω＋πのどちらかが式（９）を最大化することが分かる。
【００８９】
なお、式（１）をｓｉｎ曲線で近似したとき、振幅ａを次の式で推定する。
振幅ａは参照地点からの移動距離Ｌに比例する。
【００９０】
【数１３】

上記のようにして、本実施形態のＳ５０では、式（１２）に基づいて、移動方向ωを算出する。
【００９１】
Ｓ５０では、上記のようにして、参照地点に対するアクティブセンサ１１の回転角度φや移動方向ωと、移動距離Ｌに比例するマッチングパスの振幅ａを推定することができる。なお、振幅ａにはエッジの奥行きの影響も含んでいるため、参照地点からのアクティブセンサ１１の移動距離Ｌの相対値しか表していない。このため、１つの参照地点だけでは、アクティブセンサ１１の正確な現在位置を推定するのは困難であるため、Ｓ６０において、アクティブセンサ１１の現在位置を正確に推定する。
【００９２】
（Ｓ６０）（アクティブセンサ１１の現在位置の推定）
Ｓ６０では、アクティブセンサ１１の現在位置の推定を行う。
本実施形態では、アクティブセンサ１１の現在位置に最も近い２つの参照地点を用いて、図１０に示すように、２つの参照地点に対する移動方向ω１と移動方向ω２の交点を求め、アクティブセンサ１１の正確な現在位置を推定する。
【００９３】
アクティブセンサ１１の現在位置と参照地点が近ければ近いほどその全方位エッジヒストグラムが似ているため、ＤＰマッチングで得られた最適マッチングのコストを用いて現在位置に最も近い参照地点を決定する。
【００９４】
以下は、現在位置の算出の仕方である。
求めたいアクティブセンサ１１の現在位置をＰ_ｃ＝（ｘ，ｙ）とし、現在位置から最も近い２つの参照地点をＰ_１＝（ｘ_１，ｙ_１），Ｐ_２＝（ｘ_２，ｙ_２）とする。
【００９５】
Ｐ_１とＰ_２からの移動方向ω_１とω_２のベクトルをそれぞれｖ_１とｖ_２、エッジヒストグラムにおける最適マッチングのパスの振幅をそれぞれａ_１とａ_２とする。一般的に、次の２つの連立方程式からアクティブセンサ１１の現在位置Ｐ_ｃの座標（ｘ，ｙ）を演算する。
【００９６】
【数１４】

【００９７】
【数１５】

なお、Ｐ_ｃ，Ｐ_１，Ｐ_２はほぼ同一直線上にあるとした場合、すなわち、∠（Ｐ_１ ^→Ｐ_２，ｖ_１）及び∠（Ｐ_２ ^→Ｐ_１，ｖ_２）が小さいとき（本実施形態では、これらの角度が３０°よりも小さいとき）は、下記の式（１６）、式（１７）でアクティブセンサ１１の現在位置Ｐ_ｃの座標（ｘ，ｙ）を演算する。この理由は、Ｐ_ｃ，Ｐ_１，Ｐ_２はほぼ同一直線上にあるとした場合、式（１４）、式（１５）の式では、移動方向の推定誤差がアクティブセンサ１１の位置推定の大きな誤差となるためである。
【００９８】
【数１６】

【００９９】
【数１７】

ここで、ｄはＰ_１とＰ_２の間の距離であり、ｇ_１とｇ_２は次の式で算出される。
【０１００】
【数１８】

ここでは、ｉ，ｊ＝１，２かつｊ≠ｉであり、・は２つのベクトルの内積演算である。
【０１０１】
なお、現在位置のエッジヒストグラムとの最適マッチングパスのコストの和が最も小さい参照地点を最も近い参照地点として扱う。その理由は下記の通りである。
【０１０２】
多くの場合、最適マッチングパスのコストとパスの振幅ａは同じ大小関係を保つが、エッジの奥行きの影響が強いとき、それらの大小関係は逆になる場合もある。しかし、高いエッジヒストグラムのＤＰマッチングを用いれば、より安定的にアクティブセンサ１１の参照地点に対する回転角と移動方向を推定できるため、本実施形態では、最小のマッチングコストを持つ参照地点を最も近い参照地点として決定しているのである。
【０１０３】
上記のようにして、Ｓ６０において、アクティブセンサ１１の現在位置Ｐ_ｃ＝（ｘ，ｙ）を算出し、このプログラムを終了する。
上記実施の形態によれば、下記に示す効果を有する。
【０１０４】
（１）　本実施形態のアクティブセンサ１１の位置・姿勢推定方法では、環境Ｋ内の複数の参照地点でアクティブセンサ１１が取得した全方位画像に基づいて各参照地点の全方位エッジヒストグラム（参照地点ヒストグラム）を生成してＲＯＭ等に記憶するようにした。
【０１０５】
そして、アクティブセンサ１１が環境Ｋ内の任意の地点に移動した際に、そのときの位置、すなわち、現在位置でアクティブセンサ１１が取得した全方位画像に基づいて、現在位置における全方位エッジヒストグラム（現在位置ヒストグラム）を生成する。
【０１０６】
続いて、参照地点ヒストグラムと現在位置ヒストグラムを動的計画法によりマッチングして、各方位におけるヒストグラムの総合シフト量δ_θ、すなわち、ｓ_ｉを算出した。次に、前記総合シフト量に基づいて、各参照地点に対する現在位置のアクティブセンサ１１の回転角度φに相当する回転量ｓ_φと移動方向ω及び相対的な移動量を算出した。
【０１０７】
そして、現在位置Ｐ_ｃと最も近い２つの参照地点Ｐ_１，Ｐ_２に対する移動方向ω_１，ω_２を用いてアクティブセンサ１１の現在位置Ｐ_ｃと姿勢（回転角度φ）を推定するようにした。
【０１０８】
この結果、全方位画像のエッジヒストグラム（統計量）を用いて、数少ない参照地点に基づいてアクティブセンサ１１の現在位置と回転角度φ（姿勢）を精度良くロバストに推定することができる。
【０１０９】
又、本実施形態では、全方位画像における個々のエッジをマッチングするのではなく、その統計量であるエッジヒストグラムをマッチングすることにより、ノイズや環境Ｋの複雑さに対する頑丈性が高いアクティブセンサ１１の位置・姿勢推定方法とすることができる。
【０１１０】
（２）　本実施形態では、前記総合シフト量を、動的計画法にて得られた最小コストパスに基づき算出した。
この結果、それぞれの全方位における参照地点ヒストグラムと現在位置ヒストグラムを効率的にマッチングすることができ、現在位置ヒストグラムの総合シフト量を効率的に得ることができる。
【０１１１】
（３）　本実施形態の位置・姿勢推定装置は、全方位画像取得手段としての複数のステレオユニット１２にて、環境Ｋ内の地点で全方位画像を取得するようにした。そして、コンピュータ１６は第１生成手段として、前記全方位画像に基づいて複数の参照地点における参照地点ヒストグラムを生成するようにした。コンピュータ１６は記憶手段として、前記生成した各参照地点の参照地点ヒストグラムを記憶するようにした。そして、コンピュータ１６は、第２生成手段として、環境Ｋ内の任意の地点に移動した際に、現在位置で取得した全方位画像に基づいて、現在位置における現在位置ヒストグラムを生成する。又、コンピュータ１６は、第１算出手段として、参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出するようにした。
【０１１２】
さらに、コンピュータ１６は、第２算出手段として、総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出するようにした。
【０１１３】
そして、コンピュータ１６は、推定手段として、現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサ１１の位置と姿勢を推定するようにした。
【０１１４】
この結果、上記（１）の作用効果を奏することができる。
（４）　本実施形態の位置・姿勢推定装置は、総合シフト量を、動的計画法にて得られた最小コストパスに基づき算出するようにした。
【０１１５】
この結果、上記（２）の作用効果を奏することができる。
（５）　本実施形態の、位置・姿勢推定プログラムは、コンピュータ１６を、環境内の複数の参照地点でアクティブセンサ１１が取得した全方位画像に基づいて参照地点ヒストグラムを生成する第１生成手段として機能させるようにした。
【０１１６】
又、位置・姿勢推定プログラムは、コンピュータ１６を、前記生成した各参照地点の参照地点ヒストグラムを記憶する記憶手段として機能させる。さらに、位置・姿勢推定プログラムは、環境内の現在位置でアクティブセンサ１１が取得した全方位画像に基づいて、現在位置おける現在位置ヒストグラムを生成する第２生成手段として機能させる。
【０１１７】
又、位置・姿勢推定プログラムは、コンピュータ１６を、前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出する第１算出手段として機能させる。
【０１１８】
さらに、位置・姿勢推定プログラムは、コンピュータ１６を、前記総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出する第２算出手段として機能させる。又、位置・姿勢推定プログラムは、コンピュータ１６を、現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの位置と姿勢を推定する推定手段として機能させる。
【０１１９】
この結果、上記（１）の作用効果を奏することができる。
（第２実施形態）
次に第２実施形態について説明する。
【０１２０】
第２実施形態では、コンピュータ１６が実行する位置・姿勢推定プログラム中、Ｓ４０のＤＰマッチングの仕方が異なり、図３のフローチャートにおいて、他のステップについては、第１実施形態と同様である。
【０１２１】
又、第２実施形態のハード構成は第１実施形態のハード構成と同一であるため、その説明は省略する。
以下、第２実施形態のＳ４０について説明する。
【０１２２】
第１実施形態におけるＤＰマッチングでは、２πの周期を持ち、かつ連続性のあるエッジヒストグラムのマッチングパスを探索できるが、パスの始まる行を全てｋ＝０，…，Ｎ−１に仮定して行う必要があり、計算コストがかかる。
【０１２３】
これに対し、式（３）の最小化の条件から、パスが閉じているという条件−１≦ｓ_０−ｓ_Ｎ−１≦１を除けば、普通のＤＰマッチングで最小コストパスを効率的に探索が可能である。
【０１２４】
すなわち、ｉ＝０→ｉ＝Ｎ−１順と、その逆順でＤＰマッチングの探索を行い、２つの探索順で得られたＤＰマッチングのマッチングパスから式（３）の条件を満たし、かつコストが最小になるパスを探索する。
【０１２５】
具体的には、Ｓ４０では、下記のようにＤＰマッチングを行う。
式（１９）と式（２０）のように普通のＤＰマッチングで、ｉ＝０→ｉ＝Ｎ−１順のヒストグラムのマッチングパスを計算する。
【０１２６】
【数１９】

【０１２７】
【数２０】

ただし、Ｓ_ｓ（ｓ，−１）＝０，Ｉ_ｓ（ｓ，−１）＝ｓである。
【０１２８】
ｓ＾はｍｉｎ｛Ｓ_ｓ（ｓ−１，ｉ−１），Ｓ_ｓ（ｓ，ｉ−１），Ｓ_ｓ（ｓ＋１，ｉ−１）｝に対応するｓの値である。
Ｉ_ｓ（ｓ，ｉ）は現パスのスタート行を記憶している。
同様に、ｉ＝Ｎ−１→ｉ＝０順のヒストグラムのマッチングパスを次式で計算する。
【０１２９】
【数２１】

【０１３０】
【数２２】

ただし、
Ｓ_ｓ（ｓ，Ｎ）＝０，Ｉ_ｓ（ｓ，Ｎ）＝ｓである。
【０１３１】
ｓ＾はｍｉｎ｛Ｓ_ｒ（ｓ−１，ｉ＋１），Ｓ_ｒ（ｓ，ｉ＋１），Ｓ_ｒ（ｓ＋１，ｉ＋１）｝に対応するｓの値である。
上記の２通りのＤＰマッチングパスから、次のように式（３）の条件を満たす最小コストパスを求める。
【０１３２】
【数２３】

ただし、−１≦Ｉ_ｓ（ｓ，ｉ）−Ｉ_ｒ（ｓ，ｉ）≦１
例えば、Ｓ_ｓ（ｓ＾，ｉ＾）とＳ_ｒ（ｓ＾，ｉ＾）が式（２３）を最小化した場合、Ｓ_ｓ（ｓ＾，ｉ＾）とＳ_ｒ（ｓ＾，ｉ＾）からそれぞれＳ_ｓ（ｓ，０）とＳ_ｒ（ｓ，Ｎ−１）までのマッチングパスを辿れば、最小コストパスが得られる。
【０１３３】
第１実施形態では、式（５）と式（６）でＮ回のＤＰマッチングの探索を行う必要があるが、第２実施形態では、２回のＤＰマッチングの探索と１回の最小値探索しか行わない。
【０１３４】
なお、現在位置と、参照地点の距離が大きい場合、Ｓ_Ｓ（ｓ，ｉ）とＳ_ｒ（ｓ，ｉ）の中に、式（２３）の条件を満たすパスがない場合がある。
この場合、Ｓ５０，Ｓ６０において、アクティブセンサ１１の現在位置と姿勢を推定する際、アクティブセンサ１１に最も近いと考えられる２つの参照地点の候補から、式（２３）の条件を満たすパスがない参照地点を除外し、残った参照地点から、最も近い２つの参照地点を選択するものとする。
【０１３５】
第２実施形態では、前記第１実施形態の（１）〜（４）に記載の作用効果の他、下記の作用効果を奏する。
（１）　第２実施形態では、ｉ＝０→ｉ＝Ｎ−１順と、その逆順でＤＰマッチングの探索を行い、２つの探索順で得られたＤＰマッチングのマッチングパスから式（３）の条件を満たし、かつコストが最小になるパスを探索するようにしたため、第１実施形態に比して、最小コストパスの探索を効率的にできる。
【０１３６】
なお、本発明の実施形態は上記実施形態に限定されるものではなく、発明の趣旨を逸脱しない範囲で、適宜に変更して次のように実施することもできる。
（１）　前記実施形態では、アクティブセンサ１１として、「”実環境センシングのための全方向ステレオシステム（ＳＯＳ）”、電気学会論文誌Ｃ．Ｖｏｌ．１２１−Ｃ，Ｎｏ．５，ｐｐ．８７６−８８１．２００１」に記載されているものを使用した。これに限らず、他の全方位カメラから得られた全方位画像を入力するようにしてもよい。
【０１３７】
【発明の効果】
以上詳述したように請求項１乃至請求項５に記載の発明によれば、全方位画像の統計量を用いて、数少ない参照地点に基づいてセンサの現在の位置と姿勢を精度良くロバストに推定することができる効果を奏する。
【０１３８】
又、請求項１乃至請求項５に記載の発明によれば、全方位画像における個々のエッジをマッチングすることなく、その統計量であるヒストグラムを用い、ノイズや環境の複雑さに対する頑丈性を高くすることができる効果を奏する。
【図面の簡単な説明】
【図１】アクティブセンサ１１の電気的構成のブロック図。
【図２】アクティブセンサ１１の機械的構成の概略図。
【図３】コンピュータ１６が実行する位置・姿勢推定プログラムのフローチャート。
【図４】図５に対応するパノラマエッジ画像の説明図。
【図５】エッジヒストグラムの例を示す説明図。
【図６】アクティブセンサ１１の平行移動におけるエッジヒストグラムのシフトを説明するための説明図。
【図７】（ａ）はアクティブセンサ１１を中心にした円筒座標系の説明図、（ｂ）は、アクティブセンサ１１を中心にした四角筒座標系の説明図、（ｃ）は、異なる空間系にエッジを写像したときの、ヒストグラムのシフトとｓｉｎ曲線を表した説明図。
【図８】アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（ｓ，ｉ）の説明図。
【図９】総合シフト量から回転量ｓ_φと移動方向ωを求める方法の説明図。
【図１０】２つの参照地点Ｐ_１，Ｐ_２に対する移動方向ω１、移動方向ω２の説明図。
【符号の説明】
１１…アクティブセンサ
１２…ステレオユニット
１６…コンピュータ（記憶手段、第１生成手段、第２生成手段、第１算出手段、及び第２算出手段）
ＶＣ…参照用ビデオカメラ
ＶＣｓ…基準ビデオカメラ

Claims

全方位画像を取得可能であり、環境内を移動可能なアクティブセンサの位置・姿勢推定方法において、
環境内の複数の参照地点で前記アクティブセンサにて取得した全方位画像に基づいて各参照地点における全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成して記憶し、
次に、前記環境内の任意の地点（以下、現在位置という）でアクティブセンサが取得した全方位画像に基づいて現在位置における全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成し、
前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出し、
前記総合シフト量に基づいて、前記各参照地点に対する現在位置のアクティブセンサの回転量、移動方向及び相対的な移動量を算出し、
前記現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの現在位置と姿勢を推定することを特徴とするアクティブセンサの位置・姿勢推定方法。
前記マッチングは、動的計画法によるものであり、総合シフト量は、動的計画法にて得られた最小コストパスに基づくものであることを特徴とする請求項１に記載のアクティブセンサの位置・姿勢推定方法。
アクティブセンサにて取得した環境内の複数の参照地点における全方位画像に基づいて全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成する第１生成手段と、
前記生成した各参照地点の参照地点ヒストグラムを記憶する記憶手段と、
前記環境内の任意の地点（以下、現在位置という）でアクティブセンサが取得した全方位画像に基づいて、現在位置おける全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成する第２生成手段と、
前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出する第１算出手段と、
前記総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出する第２算出手段と、
現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの位置と姿勢を推定する推定手段とを備えたことを特徴とするアクティブセンサの位置・姿勢推定装置。
前記第１算出手段は動的計画法により演算することを特徴とする請求項３に記載のアクティブセンサの位置・姿勢推定装置。
コンピュータを、環境内の複数の参照地点でアクティブセンサが取得した全方位画像に基づいて全方位エッジヒストグラム（以下、参照地点ヒストグラムという）を生成する第１生成手段と、前記生成した各参照地点の参照地点ヒストグラムを記憶する記憶手段と、環境内の任意の地点（以下、現在位置という）でアクティブセンサが取得した全方位画像に基づいて、現在位置おける全方位エッジヒストグラム（以下、現在位置ヒストグラムという）を生成する第２生成手段と、前記参照地点ヒストグラムと現在位置ヒストグラムをマッチングして、各方位におけるヒストグラムの総合シフト量を算出する第１算出手段と、前記総合シフト量に基づいて、前記各参照地点に対する現在位置におけるアクティブセンサの回転量、移動方向及び相対的な移動量を算出する第２算出手段と、現在位置と最も近い２つの参照地点に対する移動方向を用いてアクティブセンサの位置と姿勢を推定する推定手段として機能させるためのアクティブセンサの位置・姿勢推定プログラム。