(57)【特許請求の範囲】
【請求項１】
極座標にて表された極座標データをコンピュータにて処理することにより、前記極座標データを情報圧縮して、前記極座標データの特徴を示す固有値テンプレートを算出する算出方法であって、
前記極座標データを取得する取得ステップ（Ｓ２００、Ｓ３２０）と、
取得された極座標データを、極座標上において、所定数の周波数成分を基底として、回転方向に周波数変換し、所定数の周波数成分からなり、極座標における回転角の関数である第１基底関数と、各周波数成分の係数となる、極座標における半径の関数である第２基底関数とに分解する分解ステップ（Ｓ２１０、Ｓ３３０）と、
前記第２基底関数に対し、極座標上において、前記第１基底関数の周波数成分と同数の周波数成分を基底として、周波数変換を行う周波数変換ステップ（Ｓ２３０）と、
前記第１基底関数、及び、周波数変換された前記第２基底関数を、それぞれ、ユークリッド座標に変換する座標変換ステップ（Ｓ２４０）と、
ユークリッド座標上において、前記第１基底関数の各要素と、周波数変換された前記第２基底関数の各要素とのアダマール積を算出することにより、周波数変換された固有値テンプレートを算出する算出ステップ（Ｓ２５０）と、を備える固有値テンプレートの算出方法。
【請求項２】
前記周波数変換ステップでは、ハンケル変換により、前記第２基底関数の周波数変換を行う請求項１に記載の固有値テンプレートの算出方法。
【請求項３】
前記座標変換ステップでは、極座標上の各座標位置とユークリッド座標の各座標位置との関係がテーブルとして予め定められており、そのテーブルを参照して、ユークリッド座標への変換が行われる請求項１又は２に記載の固有値テンプレートの算出方法。
【請求項４】
前記取得ステップでは、レーザセンサにて、レーザを周囲の異なる角度に照射して、周囲の物体によって反射されたレーザを受信することで、レーザの照射角度毎の周囲物体までの距離を示す極座標データを取得する請求項１乃至３のいずれか１項に記載の固有値テンプレートの算出方法。
【請求項５】
前記取得ステップでは、全方位カメラによって撮影された同心円の画像を前記極座標データとして取得する請求項１乃至３のいずれか１項に記載の固有値テンプレートの算出方法。
【請求項６】
前記取得ステップでは、
カメラ（１０）により、ユークリッド座標系において画像を撮影し、撮影された画像を極座標における画像に変換することにより、極座標に変換された画像を、前記極座標データとして取得する請求項１乃至３のいずれか１項に記載の固有値テンプレートの算出方法。
【請求項７】
撮影された画像について、輝度勾配の強度と向きからなる複素勾配画像を計算する計算ステップ（Ｓ３００）を備え、
前記取得ステップ（Ｓ３２０）は、前記複素勾配画像を極座標における画像に変換して前記極座標データとするものであり、さらに、
前記極座標への変換前、又は変換後に、前記画像中の対象物の面内回転をキャンセルするための補正行列を用いて、画像の各画素値を補正する補正ステップ（Ｓ３１０）と、
前記分解ステップ（Ｓ３３０）により、極座標データとしての画像を、前記第１基底関数と、前記第２基底関数とに分解した後、前記第１基底関数と前記第２基底関数とに、前記補正行列の逆行列を乗算することにより、前記補正行列の影響を除去する逆補正ステップ（Ｓ３４０）と、を備える請求項６に記載の固有値テンプレートの算出方法。
【請求項８】
請求項１乃至７のいずれかに記載の固有値テンプレートの算出方法を用いて、時系列的に前後して取得されるデータ同士をマッチングするマッチング方法であって、
請求項１乃至７のいずれかに記載の固有値テンプレートの算出方法を用いて、時系列的に前後して取得されるいずれか一方のデータの周波数変換された固有値テンプレートをユークリッド座標上において算出し、
固有値テンプレートが算出されない、時系列的に前後して取得される他方のデータに関して、ユークリッド座標において、前記分解ステップにおける所定数の周波数成分と同数の周波数成分を基底として周波数変換される周波数変換データを生成する生成ステップ（Ｓ１１０）と、
周波数変換された固有値テンプレートの各要素と、前記生成ステップにより生成された周波数変換データの各要素とのアダマール積を算出するとともに、その算出結果に対して周波数逆変換を行うことにより、時系列的に前後して取得されるデータ同士の類似度を算出する基礎となる相関値を算出する相関値算出ステップ（Ｓ１２０）と、
前記相関値に基づき類似度を算出し、その類似度から、時系列的に前後して取得されるデータ同士をマッチングさせるマッチングステップ（Ｓ１３０）と、を備えるマッチング方法。
【請求項９】
極座標にて表された極座標データを情報圧縮して、前記極座標データの特徴を示す固有値テンプレートを算出する算出装置であって、
前記極座標データを取得する取得部（Ｓ２００、Ｓ３２０）と、
前記取得部により取得された極座標データを、極座標上において、所定数の周波数成分を基底として、回転方向に周波数変換し、所定数の周波数成分からなり、極座標における回転角の関数である第１基底関数と、各周波数成分の係数となる、極座標における半径の関数である第２基底関数とに分解する分解部（Ｓ２１０、Ｓ３３０）と、
前記第２基底関数に対し、極座標上において、前記第１基底関数の周波数成分と同数の周波数成分を基底として、周波数変換を行う周波数変換部（Ｓ２３０）と、
前記第１基底関数、及び、前記周波数変換部によって周波数変換された前記第２基底関数を、それぞれ、ユークリッド座標に変換する座標変換部（Ｓ２４０）と、
前記ユークリッド座標上において、前記第１基底関数の各要素と、周波数変換された前記第２基底関数の各要素とのアダマール積を算出することにより、周波数変換された固有値テンプレートを算出する算出部（Ｓ２５０）と、を備える固有値テンプレートの算出装置。
【請求項１０】
前記周波数変換部は、ハンケル変換により、前記第２基底関数の周波数変換を行うことを特徴とする請求項９に記載の固有値テンプレートの算出装置。
【請求項１１】
前記座標変換部は、極座標上の各座標位置とユークリッド座標の各座標位置との関係をテーブルとして予め定めており、そのテーブルを参照して、ユークリッド座標への変換を行う請求項９又は１０に記載の固有値テンプレートの算出装置。
【請求項１２】
前記取得部は、レーザセンサを含み、レーザを周囲の異なる角度に照射して、周囲の物体によって反射されたレーザを受信することで、レーザの照射角度毎の周囲物体までの距離を示す極座標データを取得するものである請求項９乃至１１のいずれか１項に記載の固有値テンプレートの算出装置。
【請求項１３】
前記取得部は、全方位カメラを含み、全方位カメラによって撮影された同心円の画像を前記極座標データとして取得するものである請求項９乃至１１のいずれか１項に記載の固有値テンプレートの算出装置。
【請求項１４】
前記取得部は、
ユークリッド座標系において、画像を撮影するカメラ（１０）と、
前記カメラによって撮影された画像を極座標における画像に変換する極座標変換部と、を備え、
前記極座標変換部によって極座標に変換された画像を、前記極座標データとして取得するものである請求項９乃至１１のいずれか１項に記載の固有値テンプレートの算出装置。
【請求項１５】
撮影された画像について、輝度勾配の強度と向きからなる複素勾配画像を計算する計算部（Ｓ３００）を備え、
前記取得部（Ｓ３２０）は、前記複素勾配画像を極座標における画像に変換して前記極座標データとするものであり、さらに、
前記極座標への変換前、又は変換後に、前記画像中の対象物の面内回転をキャンセルするための補正行列を用いて、画像の各画素値を補正する補正部（Ｓ３１０）と、
前記分解部（Ｓ３３０）により、極座標データとしての画像を、前記第１基底関数と、前記第２基底関数とに分解した後、前記第１基底関数と前記第２基底関数とに、前記補正行列の逆行列を乗算することにより、前記補正行列の影響を除去する逆補正部（Ｓ３４０）と、を備える請求項１４に記載の固有値テンプレートの算出装置。
【請求項１６】
請求項９乃至１５のいずれかに記載の固有値テンプレートの算出装置を用いて、時系列的に前後して取得されるデータ同士をマッチングするマッチングシステム（３０）であって、
請求項９乃至１５のいずれかに記載の固有値テンプレートの算出装置は、時系列的に前後して取得されるいずれか一方のデータの周波数変換された固有値テンプレートをユークリッド座標上において算出し、
固有値テンプレートが算出されない、時系列的に前後して取得される他方のデータに関して、ユークリッド座標において、前記分解部における所定数の周波数成分と同数の周波数成分を基底として周波数変換される周波数変換データを生成する生成部（Ｓ１１０）と、
周波数変換された固有値テンプレートの各要素と、前記生成部により生成された周波数変換データの各要素とのアダマール積を算出するとともに、その算出結果に対して周波数逆変換を行うことにより、時系列的に前後して取得されるデータ同士の類似度を算出する基礎となる相関値を算出する相関値算出部（Ｓ１２０）と、
前記相関値に基づき類似度を算出し、その類似度から、時系列的に前後して取得されるデータ同士をマッチングさせるマッチング部（Ｓ１３０）と、を備えるマッチングシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、極座標にて表された極座標データの固有値テンプレートを算出する算出方法及び算出装置、並びに、それらを利用して、時系列的に前後して取得されるデータ同士をマッチングするマッチング方法及びマッチングシステムに関する。
【背景技術】
【０００２】
ロボットなどの移動体に、レーザ光を水平方向にスキャンすることが可能な走査型レーザセンサ（レーザレンジファインダー）やカメラなどを搭載し、移動体の自己位置の推定と環境の地図作成とを同時に行うSimultaneous Localization and Mapping（ＳＬＡＭ）と呼ばれる技術が知られている。
【０００３】
このＳＬＡＭでは、時系列的に相前後して観測したスキャンデータや画像データなどの計測データ、すなわち、時刻ｔで観測した計測データｆ（ｔ）と、時刻ｔ＋１で観測した計測データｆ（ｔ＋１）とをマッチングさせることで、自己位置の推定と環境の地図作成とを行う。
【０００４】
このような、時系列的に相前後して観測される計測データをマッチングさせる手法として、Iterative Closest Point（ＩＣＰ）、Normal Distribution Transform（ＮＤＴ）、Correlative Scan Matchingなどいくつかの手法が知られている。
【０００５】
ＩＣＰは、入力として与えられる２つの点群の位置合わせを自動で行うものである。より具体的には、一方の点群を構成する各点に対し、他方の点群における最近傍点を探索して対応点を定め、対応点間の距離を最小化するような剛体変換を推定する。この対応点探索、剛体変換推定を繰り返すことで、２つの点群の位置合わせを行う。ＮＤＴは、探索空間を格子状（グリッド）に分割することで個々のグリッド内の点データから正規分布を参照スキャンとして準備し、新規入力スキャンとマッチングを行う手法である。また、Correlative Scan Matchingは、計測データ同士の類似度を示す相互相関値を計算し、その相互相関値から計測データ同士をマッチングさせる手法である。
【０００６】
上述した各種の手法の中で、特に、Correlative Scan Matchingは、高精度に計測データをマッチングさせることが可能な手法である。ただし、計測データにより得られる形状の相互相関値の計算は、分解能を向上させ、精度を高めようとするほど計算量が膨大となり、計算時間が長くなる点において課題を有する。
【０００７】
一方、基準となる画像をテンプレート画像とし、撮影された入力画像の中からテンプレート画像を検索するテンプレートマッチングを高速に行う手法として、固有値テンプレート法が提案されている（例えば、特許文献１参照）。この固有値テンプレート法は、入力画像に対して、テンプレート画像を所定の角度で回転させつつ、１画像ずつマッチングする代わりに、回転角度を変化させたテンプレート画像群を特異値分解（ＳＶＤ）で情報圧縮することで得られる新たなテンプレート画像（固有値テンプレート画像）を利用し、効率よく類似度を算出するものである。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第５３１７２５０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
本発明者らは、上述した固有値テンプレート法を、Correlative Scan Matchingに応用することで、計測データのマッチングのための計算時間を短縮できる可能性について検討した。固有値テンプレート法を、Correlative Scan Matchingに応用する場合、時系列的に相前後して観測される計測データの一方を対象として固有値テンプレートを算出し、その固有値テンプレートと、計測データの他方とに基づき、類似度を算出すれば良い。
【００１０】
しかしながら、従来の固有値テンプレート法と同様の手法にて、一方の計測データの固有値テンプレートを算出しようとすると、その固有値テンプレートの算出に多大な時間がかかり、実用上、大きな問題となる。
【００１１】
本発明は、上述した点に鑑みてなされたものであり、固有値テンプレートの算出時間を従来よりも短縮することが可能な固有値テンプレートの算出方法及び算出装置を提供することを第１の目的とする。
【００１２】
また、本発明は、上述した固有値テンプレートの算出方法及び算出装置を利用して、時系列的に前後して取得されるデータ同士を高速かつ高精度にマッチングすることが可能なマッチング方法及びマッチングシステムを提供することを第２の目的とする。
【課題を解決するための手段】
【００１３】
上記第１の目的を達成するために、本発明による固有値テンプレートの算出方法は、極座標にて現された極座標データをコンピュータにて処理することにより、極座標データを情報圧縮して、極座標データの特徴を示す固有値テンプレートを算出するものであって、
極座標データを取得する取得ステップ（Ｓ２００、Ｓ３２０）と、
取得された極座標データを、極座標上において、所定数の周波数成分を基底として、回転方向に周波数変換し、所定数の周波数成分からなり、極座標における回転角の関数である第１基底関数と、各周波数成分の係数となる、極座標における半径の関数である第２基底関数とに分解する分解ステップ（Ｓ２１０、Ｓ３３０）と、
第２基底関数に対し、極座標上において、第１基底関数の周波数成分と同数の周波数成分を基底として、周波数変換を行う周波数変換ステップ（Ｓ２３０）と、
第１基底関数、及び、周波数変換された第２基底関数を、それぞれ、ユークリッド座標に変換する座標変換ステップ（Ｓ２４０）と、
ユークリッド座標上において、第１基底関数の各要素と、周波数変換された第２基底関数の各要素とのアダマール積を算出することにより、周波数変換された固有値テンプレートを算出する算出ステップ（Ｓ２５０）と、を備える。
【００１４】
また、本発明による固有値テンプレートの算出装置は、極座標にて表された極座標データを情報圧縮して、極座標データの特徴を示す固有値テンプレートを算出するものであって、
極座標データを取得する取得部（Ｓ２００、Ｓ３２０）と、
取得部により取得された極座標データを、極座標上において、所定数の周波数成分を基底として、回転方向に周波数変換し、所定数の周波数成分からなり、極座標における回転角の関数である第１基底関数と、各周波数成分の係数となる、極座標における半径の関数である第２基底関数とに分解する分解部（Ｓ２１０、Ｓ３３０）と、
第２基底関数に対し、極座標上において、第１基底関数の周波数成分と同数の周波数成分を基底として、周波数変換を行う周波数変換部（Ｓ２３０）と、
第１基底関数、及び、周波数変換部によって周波数変換された第２基底関数を、それぞれ、ユークリッド座標に変換する座標変換部（Ｓ２４０）と、
ユークリッド座標上において、第１基底関数の各要素と、周波数変換された第２基底関数の各要素とのアダマール積を算出することにより、周波数変換された固有値テンプレートを算出する算出部（Ｓ２５０）と、を備える。
【００１５】
本発明による固有値テンプレートの算出方法及び算出装置では、まず、固有値テンプレートを、ユークリッド座標において、周波数変換された固有値テンプレートとして算出することを目標とする。その理由は、以下の通りである。固有値テンプレートが、周波数変換されず、空間領域における情報そのものである場合、固有値テンプレートとの類似度を求める基礎となる相関値を算出する際、固有値テンプレートと計測データとの畳み込み演算（convolution）が必要となる。この畳み込み演算は、演算量が大きく、処理負荷が高い。それに対して、固有値テンプレートが、周波数変換され、空間周波数領域における情報に変換されていると、計測データの空間周波数領域における情報とのアダマール積により相関値が算出できる。このアダマール積による演算は演算量が僅かで済み、処理負荷を低減することができる。また、ユークリッド座標上において、固有値テンプレートを算出する理由は、並進量を計算できるようにするためである。
【００１６】
ここで、ユークリッド座標において、周波数変換された固有値テンプレートを求めようとした場合、一般的には、ユークリッド座標において、固有値テンプレートに対しｘ軸及びｙ軸の２軸方向にそれぞれ周波数変換を行う２次元ＦＦＴを用いることが考えられる。しかしながら、この２次元ＦＦＴは、比較的、演算負荷の高い処理であり、固有値テンプレートの算出時間を伸長させる大きな原因となる。
【００１７】
そこで、本発明による固有値テンプレートの算出方法及び算出装置では、この２次元ＦＦＴを用いることなく、ユークリッド座標上で、周波数変換された固有値テンプレートが得られるようにした。
【００１８】
そのため、本発明では、まず、固有値テンプレートを算出する基礎となるデータを、極座標データとして取得する。例えば、レーザレンジファインダーにより得られるスキャンデータは、極座標データである。また、例えば、全方位カメラにより取得される画像データも極座標データとして扱うことが可能である。ユークリッド座標系において画像を撮影するカメラの場合、撮影された画像データを極座標に変換すれば良い。
【００１９】
このように極座標データを用いることにより、１つの極座標データから固有値テンプレートを算出することができる。換言すれば、従来の固有値テンプレート法のように、テンプレート画像の回転角度を変化させたテンプレート画像群を作成する必要がなく、固有値テンプレート画像の算出時間の短縮に寄与できる。
【００２０】
さらに、本発明では、取得された極座標データを、極座標上において、所定数の周波数成分を基底として、回転方向に周波数変換し、所定数の周波数成分からなり、極座標における回転角の関数である第１基底関数と、各周波数成分の係数となる、極座標における半径の関数である第２基底関数とに分解する。そして、第２基底関数に対し、極座標上において、第１基底関数の周波数成分と同数の周波数成分を基底として、周波数変換を行う。
【００２１】
極座標上における、第２基底関数に対する周波数変換は、ユークリッド座標上における固有値テンプレートに対する２次元ＦＦＴと同等の意味合いを持つ。これにより、ユークリッド座標上の２次元ＦＦＴを行わずに済むようになる。しかも、極座標上での周波数変換は、第２基底関数に対する一次元の周波数変換で済むので、その周波数変換に要する時間は、ユークリッド座標上の２次元ＦＦＴに比較して、大幅に短縮することができる。
【００２２】
そして、第１基底関数、及び、周波数変換された第２基底関数を、それぞれ、ユークリッド座標に変換し、それぞれの要素のアダマール積を算出することで、ユークリッド座標上において、周波数変換された固有値テンプレートを算出することができる。
【００２３】
次に、上述した第２の目的を達成するために、本発明によるマッチング方法は、上述した固有値テンプレートの算出方法を用いて、時系列的に前後して取得されるデータ同士をマッチングするものであって、
上述した固有値テンプレートの算出方法を用いて、時系列的に前後して取得されるいずれか一方のデータの周波数変換された固有値テンプレートをユークリッド座標上において算出する固有値テンプレート算出ステップ（Ｓ１００）と、
固有値テンプレートが算出されない、時系列的に前後して取得される他方のデータに関して、ユークリッド座標において、分解ステップにおける所定数の周波数成分と同数の周波数成分を基底として周波数変換される周波数変換データを生成する生成ステップ（Ｓ１１０）と、
周波数変換された固有値テンプレートの各要素と、生成ステップにより生成された周波数変換データの各要素とのアダマール積を算出するとともに、その算出結果に対して周波数逆変換を行うことにより、時系列的に前後して取得されるデータ同士の類似度を算出する基礎となる相関値を算出する相関値算出ステップ（Ｓ１２０）と、
相関値に基づき類似度を算出し、その類似度から、時系列的に前後して取得されるデータ同士をマッチングさせるマッチングステップ（Ｓ１３０）と、を備える。
【００２４】
また、本発明によるマッチングシステムは、上述した固有値テンプレートの算出装置を用いて、時系列的に前後して取得されるデータ同士をマッチングするものであって、
上述した固有値テンプレートの算出装置は、時系列的に前後して取得されるいずれか一方のデータの周波数変換された固有値テンプレートをユークリッド座標上において算出し、
固有値テンプレートが算出されない、時系列的に前後して取得される他方のデータに関して、ユークリッド座標において、分解部における所定数の周波数成分と同数の周波数成分を基底として周波数変換される周波数変換データを生成する生成部（Ｓ１１０）と、
周波数変換された固有値テンプレートの各要素と、生成部により生成された周波数変換データの各要素とのアダマール積を算出するとともに、その算出結果に対して周波数逆変換を行うことにより、時系列的に前後して取得されるデータ同士の類似度を算出する基礎となる相関値を算出する相関値算出部（Ｓ１２０）と、
相関値に基づき類似度を算出し、その類似度から、時系列的に前後して取得されるデータ同士をマッチングさせるマッチング部（Ｓ１３０）と、を備える。
【００２５】
上述したマッチング方法及びマッチングシステムによれば、上述した固有値テンプレートの算出方法又は算出装置により、時系列的に前後して取得されるいずれか一方のデータの周波数変換された固有値テンプレートをユークリッド座標上で算出する。また、時系列的に前後して取得される他方のデータに関しては、ユークリッド座標において、分解ステップ（分解部）における所定数の周波数成分と同数の周波数成分を基底として周波数変換された周波数変換データが生成される。そして、固有値テンプレートと周波数変換データとに基づき、相関値、さらには類似度を算出する。この相関値の算出に際しては、固有値テンプレートの各要素と、周波数変換データの各要素とのアダマール積を算出すればよいため、上述したように、処理負荷を低減し、マッチングの処理時間を短くすることができる。そして、以上の構成を備えることで、時系列的に前後して取得されるデータ同士のマッチングに、固有値テンプレート法による手法を導入して、高速かつ高精度にマッチングを行うことができる。
【００２６】
上記括弧内の参照番号は、本発明の理解を容易にすべく、後述する実施形態における具体的な構成との対応関係の一例を示すものにすぎず、なんら本発明の範囲を制限することを意図したものではない。
【００２７】
また、上述した特徴以外の、特許請求の範囲の各請求項に記載した技術的特徴に関しては、後述する実施形態の説明及び添付図面から明らかになる。
【図面の簡単な説明】
【００２８】
【図１】第１実施形態に係るマッチングシステムを含む移動体の移動を制御するための制御システム全体の構成を示す構成図である。
【図２】画像処理装置において実行される、相前後して入力された入力画像のマッチング方法を示すフローチャートである説明する。
【図３】第１実施形態の画像処理装置における、テンプレート画像の固有値テンプレートの算出方法を示すフローチャートである。
【図４】あるテンプレート画像を、固有値と各周波数成分とに分解した場合の、各周波数成分に対する固有値の大きさを示したグラフである。
【図５】比較手法に係る、テンプレート画像の固有値テンプレートの算出方法を示すフローチャートである。
【図６】テンプレート画像の複素勾配画像を極座標データとして用いるための処理を示すフローチャートである。
【図７】複素勾配画像の輝度勾配と、補正行列との関係を模式的に示した説明図である。
【発明を実施するための形態】
【００２９】
以下、本発明の実施形態を、図面を参照しつつ、詳細に説明する。
【００３０】
（第１実施形態）
図１は、第１実施形態に係るマッチングシステム３０を含む移動体の移動を制御するための制御システム５０全体の構成を示している。この制御システム５０は、ロボットや自走式掃除機などの移動体に搭載され、移動体の移動を制御する。そして、マッチングシステム３０は、自己位置の推定と環境の地図作成とを同時に行う、いわゆるＳＬＡＭ（Simultaneous Localization and Mapping）を実行するために、相前後して取得された極座標データのマッチングを行う。
【００３１】
マッチングシステム３０は、移動体の周囲の環境を撮影するためのカメラ１０を備えている。このカメラ１０は、移動方向の環境を撮影するように、水平方向に向けて移動体に搭載されても良いし、天井を撮影するように、上方向に向けて移動体に搭載されても良い。カメラ１０は、所定の時間間隔で繰り返し、移動体の周囲の状況を撮影する。カメラ１０が撮影する画像は、ユークリッド座標上の画像である。撮影された画像は、画像処理装置２０に出力される。
【００３２】
画像処理装置２０は、コンピュータからなり、時系列的に相前後して入力されたいずれか一方の入力画像をテンプレート画像として、そのテンプレート画像の固有値テンプレートを算出する。さらに、画像処理装置２０は、その固有値テンプレートを用いて、時系列的に相前後して入力された他方の入力画像との照合を行い、両画像をマッチングさせる。そして、画像処理装置２０は、両画像をマッチングさせたときの入力画像の回転角及び並進量を算出し、これらの移動体の移動情報に基づいて、移動体の位置を算出する。なお、固有値テンプレートの算出方法及び時系列的に相前後して入力された両画像のマッチング方法については、後に、フローチャートに基づき詳細に説明する。
【００３３】
移動体の位置の算出に関しては、移動体の移動量や移動方向を検出するセンサを用いて移動体の自走位置も算出し、カメラ画像に基づく移動体位置と、センサを用いた移動体の自走位置との両方を用いて、移動体の位置を算出するようにしても良い。
【００３４】
移動体の位置の算出と同時に、画像処理装置２０は、カメラ１０によって撮影された画像に基づいて、画像中における各種の対象物を認識し、環境の地図作成を行う。そして、画像処理装置２０は、算出した移動体の位置及び環境地図を、移動体制御部４０に出力する。移動体制御部４０は、画像処理装置２０から取得した移動体の位置及び環境地図に基づいて、移動体の移動を制御する。
【００３５】
次に、図２のフローチャートを参照して、画像処理装置２０に相前後して入力された入力画像のマッチング方法について説明する。なお、図２のフローチャートでは、先に入力された入力画像をテンプレート画像として、固有値テンプレートを作成する例を示しているが、後に入力された入力画像をテンプレート画像として、固有値テンプレートを作成しても良い。
【００３６】
まず、図２のフローチャートのステップＳ１００では、ユークリッド座標において、先に入力された入力画像ｆｆの、周波数変換された固有値テンプレートＥを作成する。この固有値テンプレートＥは、複数の固有値テンプレートＥｋの集合からなる。ユークリッド座標において、固有値テンプレートＥを作成する理由は、極座標上では、並進量が計算できないためである。このステップＳ１００処理については、後に、図３のフローチャートに基づいて、詳細に説明される。
【００３７】
ここで、テンプレート画像（先に入力された入力画像ｆｆ）の固有値テンプレートとして、周波数変換された固有値テンプレートＥを算出する理由について説明する。
【００３８】
一般的に知られている固有値テンプレート法によれば、複数枚のテンプレート画像からなるテンプレート画像群Ｔを特異値分解により情報圧縮することにより、複数枚のテンプレート画像を、その枚数よりも少ない数の固有値テンプレートｅｋと固有関数φｋとで近似する。
【００３９】
例えば、テンプレート画像群Ｔを、以下の数式１のように定義する。
【数１】
0006565667.tif 000002
【００４０】
そして、テンプレート画像群Ｔを、下記の数式２のように、テンプレート画像群Ｔの画像枚数Ｎよりも少ないＭ枚の固有値テンプレートｅｋ＝［ｅ０，…，ｅＭ−１］と、Ｍ個の固有関数φｋ（θ）＝［φ0 (θ)，…，φM-1(θ)］との積で近似する。
【数２】
0006565667.tif 000003
【００４１】
この近似式を利用することにより、テンプレート画像群Ｔと入力画像ｆとの類似度ｇは、以下の数式３で近似することができる。なお、Ｒは、固有値テンプレートｅと入力画像ｆとが重なる領域を示している。
【数３】
0006565667.tif 000004
【００４２】
この数式３を用いて類似度ｇを算出することにより、畳み込み演算の回数を削減（Ｎ回→Ｍ回）することができ、演算処理を効率化することができる。
【００４３】
ここで、上記類似度ｇの演算は、２つのステップに分けることができる。１つ目のステップは、下記の数式４に示すように、入力画像ｆとＭ枚の固有値テンプレートｅｋとの畳み込み演算により、相関値ベクトル（レスポンスベクトル）ｒ（ｘ，ｙ）を計算する処理である。そして、２つ目のステップは、下記の数式５に示すように、相関値ベクトルｒ（ｘ，ｙ）と固有関数φｋ（θ）との各要素の積の総和から類似度ｇを計算する処理である。
【数４】
0006565667.tif 000005
【数５】
0006565667.tif 000006
【００４４】
上記の数式４のように、固有値テンプレートｅが、空間領域における情報そのものである場合、固有値テンプレートｅと入力画像ｆとの相関値ｒを算出する際、各固有値テンプレートｅｋと入力画像ｆとをそれぞれ畳み込み演算（convolution）することが必要となる。この畳み込み演算は、演算量が大きく、処理負荷が高い。
【００４５】
それに対して、固有値テンプレートｅが、周波数変換され、空間周波数領域における情報に変換（ｅ→Ｅ）されていると、同様に空間周波数領域における情報に変換（ｆ→Ｆ）された入力画像Ｆとのアダマール積の結果に逆周波数変換を施すことにより、各固有値テンプレートＥｋ毎の相関値ｒｋを算出することができる。このアダマール積による演算は、畳み込み演算に比較して演算量が僅かで済み、処理負荷を低減することができる。
【００４６】
このような理由から、ステップＳ１００では、テンプレート画像Ｔの固有値テンプレートとして、周波数変換された固有値テンプレートＥを算出するのである。
【００４７】
続くステップＳ１１０では、以下の数式６に示すように、ユークリッド座標上において、例えば２次元ＦＦＴ（図２では、ＦＦＴ２と示す）により、後に入力された入力画像ｆｌを周波数変換して、空間周波数領域における情報Ｆｌに変換する。
【数６】
0006565667.tif 000007
【００４８】
そして、ステップＳ１２０において、以下の数式７に示すように、周波数変換された各固有値テンプレートＥｋと、周波数変換された、後に入力された入力画像Ｆｌとのアダマール積の演算結果に対して、例えば逆２次元ＦＦＴ演算（図２では、ｉＦＦＴ２と示す）を施すことにより、各固有値テンプレートＥｋ毎の相関値ｒｋを算出する。
【数７】
0006565667.tif 000008
【００４９】
ステップＳ１３０では、以下の数式８に従って、相関値ベクトルｒｋ（ｘ，ｙ）と、固有関数としての周波数基底関数ｅｘｐ（ｉｋθ）との積和演算により、類似度ｇを算出する。なお、本実施形態では、固有関数としてＭ個の周波数成分を基底とする周波数基底関数ｅｘｐ（ｉｋθ）を用いているため、数式８の積和演算は、Ｍ次元ＦＦＴにより置換することができる。そして、Ｍ次元ＦＦＴにより置換した場合、類似度ｇの演算速度の向上を図ることができる。
【数８】
0006565667.tif 000009
【００５０】
ステップＳ１４０では、算出した類似度ｇに基づき、相前後して入力された入力画像ｆｆ、ｆｌをマッチングさせる。具体的には、数式８に示される類似度ｇが最も高くなる、ｘ座標位置、ｙ座標位置、及び回転角θを求める。このｘ座標位置、ｙ座標位置、回転角θの算出において、例えばニュートン法などを適用して、サブピクセル単位で、類似度ｇが最も高くなる位置を推定しても良い。これらｘ座標位置、ｙ座標位置、及び回転角θは、相前後して入力された入力画像ｆｆ、ｆｌ同士のｘ、ｙ座標の位置のずれ及び回転角θのずれを示している。そのため、ステップＳ１５０では、ステップＳ１４０で求めたｘ座標位置、ｙ座標位置、及び回転角θに基づいて、相前後して入力される入力画像が取得された間の移動体の回転角及び並進量を算出することができる。
【００５１】
次に、テンプレート画像の固有値テンプレートの算出方法について、図３のフローチャートを参照しつつ、詳細に説明する。
【００５２】
図３のフローチャートのステップＳ２００では、下記の数式９に示すように、相前後して入力される入力画像ｆｆ、ｆｌの内、テンプレート画像として用いられる先に入力された入力画像ｆｆを、画像中心を原点とする極座標に変換する。なお、ｒは極座標における半径を示し、τは極座標における回転角度を示す。
【数９】
0006565667.tif 000010
【００５３】
次いで、ステップＳ２１０において、下記の数式１０に示すように、極座標に変換された入力画像ｆｆを、回転軸方向にフーリエ変換しつつ、Ｍ個の低周波数成分を基底とする周波数基底関数ｅｘｐ（ｉｋτ）を固有関数として用いた近似式により、周波数分解する。
【数１０】
0006565667.tif 000011
【００５４】
上述した数式１０において、周波数基底関数ｅｘｐ（ｉｋτ）は、回転角の関数であり、請求の範囲における第１基底関数に相当する。また、関数φｋは、請求の範囲における第２基底関数に相当し、極座標における半径の関数であって、周波数基底関数の各周波数成分の係数となるものである。このように、本実施形態では、所定数の周波数成分からなる周波数基底関数を固有関数として、入力画像ｆｆを周波数分解しているので、上述した数式８において、固有関数が周波数基底関数となり、その演算にＦＦＴを用いることができるようになる。
【００５５】
Ｍ個の低周波数成分としては、例えば、直流成分を下限とし、所定の基準周波数を上限とするＭ個の周波数成分を選択することが好ましい。多くの画像では面内回転方向の輝度変化は緩やかなので、テンプレート画像をフーリエ級数展開した場合、低周波領域に、高い値の固有値が集中する傾向があるためである。例えば、図４は、あるテンプレート画像を、固有値と各周波数成分とに分解した場合の、各周波数成分に対する固有値の大きさを示している。この図４からも、低周波成分に対応する固有値が、相対的に高い値を示すことが分かる。
【００５６】
また、上述したように、従来の固有値テンプレート法では、回転角度を変化させたテンプレート画像群を特異値分解して情報圧縮することで固有値テンプレート画像を得ていた。しかしながら、このようなテンプレート画像群Ｔを用いる場合、テンプレート画像を回転する処理を複数回行わなければならず、処理が煩雑となる。また、回転したテンプレート画像を保存しておくために大量のメモリも必要となる。
【００５７】
それに対して、本実施形態においては、１枚のテンプレート画像ｆｆ（ｘ，ｙ）を極座標表現し、その極座標表現したテンプレート画像ｆｆ（ｒ、τ）に対して、回転軸方向に所定数の周波数成分を基底としてフーリエ変換を施して周波数分解するようにした。このようにすることにより、１枚のテンプレート画像ｆｆ（ｘ，ｙ）から固有値テンプレートを作成することができ、テンプレート画像に対する処理を簡素化することができる。
【００５８】
続くステップＳ２２０では、上述した周波数分解結果から、極座標における、固有値テンプレートｅｋ（ｒ、τ）の算出式を下記の数式１１のように定める。なお、数式１１の第１式における演算記号はクロネッカー積を示している。
【数１１】
0006565667.tif 000012
【００５９】
ここで、極座標における固有値テンプレートｅｋ（ｒ、τ）の算出式を数式１１のように定めることができる理由について説明する。
【００６０】
まず、角度θだけ回転したテンプレート画像ｆｆ（ｒ、τ−θ）と入力画像ｆｌ（ｒ、τ）との類似度を極座標パラメータ（ｒ、θ）を使って、以下の数式１２のように定義する。
【数１２】
0006565667.tif 000013
【００６１】
また、極座標変換したテンプレート画像ｆｆ（ｒ，τ）は回転軸方向のフーリエ級数展開により、以下の数式１３のように書き表すことができる。
【数１３】
0006565667.tif 000014
【００６２】
この数式１３は、上述した数式１０のように、Ｍ個の低周波数成分を基底とし、そのＭ個の低周波成分の線形和で近似することができる。数式１０を数式１２に代入し、ｋθについて整理すると、以下の数式１４のように、類似度ｇ（θ）を、上述したＭ個の低周波成分を要素とする周波数基底関数ｅｘｐ（ｉｋθ）の線形和で表現することができる。
【数１４】
0006565667.tif 000015
【００６３】
そして、この数式１４における、固有値テンプレートｅｋ（ｒ、τ）は、上述した数式１１のように定義されるのである。従って、固有値テンプレートｅｋ（ｒ、τ）は、極座標に変換されたテンプレート画像ｆｆをフーリエ級数展開したときの、Ｍ個の低周波成分を要素とする周波数基底関数ｅｘｐ（ｉｋτ）から算出される周波数基底関数ψｋ（τ）と、各低周波成分の係数となる関数φｋ（ｒ）とから算出することができる。換言すれば、固有値テンプレートｅｋ（ｒ、τ）は、周波数基底関数ｅｘｐ（ｉｋτ）と、関数φｋ（ｒ）とに基づいて算出することができる。
【００６４】
ただし、図５に示す比較手法のフローチャートのように、ステップＳ２２０の後のステップＳ２６０において、極座標上で、固有値テンプレートｅｋ（ｒ、τ）を算出しようとすると、関数φｋ（ｒ）と周波数基底関数ψｋ（τ）とのクロネッカー積、すなわち、畳み込み演算が必要となる。この畳み込み演算は、上述したように、演算量が大きく、処理負荷が高い。
【００６５】
さらに、続くステップＳ２７０において、極座標上の固有値テンプレートｅｋ（ｒ、τ）を、ユークリッド座標上の固有値テンプレートｅｋ（ｘ、ｙ）に変換した場合、ユークリッド座標上で、空間領域における情報としての固有値テンプレートｅｋ（ｘ，ｙ）が得られるだけである。そのため、空間周波数領域における情報としての固有値テンプレートＥｋ（ω１，ω２）を得るためには、ステップＳ２８０に示すように、例えば、固有値テンプレートｅｋ（ｘ，ｙ）に対して２次元ＦＦＴを適用して、周波数変換を行う必要がある。このような２次元ＦＦＴによる周波数変換処理は、比較的、演算負荷の高い処理であり、固有値テンプレートＥｋ（ω１，ω２）の算出時間を伸長させる大きな原因となる。
【００６６】
そのため、本実施形態では、図３のフローチャートに示すように、ステップＳ２２０の後に、ステップＳ２３０の処理を行う。具体的には、ステップＳ２３０において、関数φｋ（ｒ）に対し、極座標上において、周波数基底関数ｅｘｐ（ｉｋτ）の周波数成分と同数の周波数成分を基底として、周波数変換を行う。なお、この周波数変換には、ハンケル変換を用いることができる。
【００６７】
極座標上における、関数φｋ（ｒ）に対する周波数変換は、ユークリッド座標上における固有値テンプレートｅｋ（ｘ、ｙ）に対する２次元ＦＦＴと同等の意味合いを持つ。このため、ユークリッド座標上の２次元ＦＦＴを行わずに済むようになる。しかも、極座標上での周波数変換は、関数φｋ（ｒ）に対する一次元の周波数変換で済むので、その周波数変換に要する時間は、ユークリッド座標上の２次元ＦＦＴに比較して、大幅に短縮することができる。
【００６８】
続くステップＳ２４０では、テーブルを参照して、固有値テンプレートＥｋ（ω１，ω２）を計算するための各関数Ｈｋ（φｋ（ｒ））、ψｋτをユークリッド座標上の関数φｋ（ω１，ω２）、ψｋ（ω１，ω２）に変換する。これら関数Ｈｋ（φｋ（ｒ））、ψｋτは、比較手法のように固有値テンプレートｅｋ（ｒ、τ）そのものを極座標からユークリッド座標に変換する場合に比較して、簡単に変換処理を行うことができる。特に、本実施形態では、極座標上の各座標位置とユークリッド座標の各座標位置との関係がテーブルとして予め定められており、そのテーブルを参照して、ユークリッド座標への変換が行われる。そのため、上述した各関数Ｈｋ（φｋ（ｒ））、ψｋτのユークリッド座標への変換処理は、極めて簡単に行うことができる。なお、図３のフローチャートのステップＳ２４０において、「ｔｌ」とはテーブル参照を意味する。
【００６９】
さらに、関数ψｋτに関しては、撮影される画像によらず、一定の周波数基底成分を用いることができるため、予め、ユークリッド座標への変換後の関数ψｋ（ω１，ω２）を算出して、画像処理装置２０内のメモリに保存しておき、その保存している関数ψｋ（ω１，ω２）を用いて、固有値テンプレートＥｋ（ω１，ω２）を算出するようにしても良い。
【００７０】
そして、ステップＳ２５０では、ユークリッド座標に座標変換された各関数φｋ（ω１，ω２）、ψｋ（ω１，ω２）のアダマール積を計算することにより、周波数変換された固有値テンプレートＥｋ（ω１，ω２）を求める。つまり、各関数φｋ（ω１，ω２）、ψｋ（ω１，ω２）は周波数領域の情報に変換されているため、畳み込み演算ではなく、アダマール積の計算により、固有値テンプレートＥｋ（ω１，ω２）を求めることができる。
【００７１】
上述した実施形態による固有値テンプレートの算出速度と、図５の比較手法による固有値テンプレートの算出速度とを実際に計測して比較したところ、本実施形態による固有値テンプレートの算出速度は、比較手法に対して約千倍まで高められることを確認した。
【００７２】
なお、上述した説明では、テンプレート画像を極座標変換したのちに、フーリエ変換する手法を示した。しかしながら、ユークリッド座標で表現されたテンプレートの極座標における周波数変換を、テンプレートの極座標変換を陽に経ずに行うことで高速化が可能な手法が種々検討されている（例えば、Fast Polar Fourie Transformなど、Fast Polar Fourie Transformに関してはhttp://www.cs.technion.ac.il/~elad/software/参照のこと）。このため、このような手法を用いて、ユークリッド座標で表現されたテンプレートの極座標における周波数変換を行っても良い。
【００７３】
また、上述した実施形態において、図３のフローチャートのステップＳ２２０の処理は、固有値テンプレートを算出する上で直接的な影響はないので、省略しても良い。
【００７４】
また、上述した実施形態では、ユークリッド座標上の画像を撮影するカメラ１０を使用する例について説明したが、このカメラ１０に代えて、もしくは、カメラ１０とともに、レーザ光を周囲の異なる角度に照射して、周囲の対象物によって反射されたレーザを受信することで、レーザの照射角度毎の周囲対象物までの距離を示す極座標データを取得するレーザセンサを用いても良い。レーザセンサにより得られる距離データは、移動体を中心とする極座標データである。この極座標データそのものを画像とみなすことにより、上述した実施形態による手法を適用して、極座標データの固有値テンプレートを極短時間の内に算出することができる。また、入力される極座標データをユークリッド座標に変換するとともに、周波数変換を行うことにより、その座標変換及び周波数変換されたデータと、固有値テンプレートとに基づき、相前後して入力される極座標データのマッチングを行うことができる。
【００７５】
さらに、移動体に、全方位カメラを搭載し、全方位カメラによって撮影された同心円の画像を極座標データとしても良い。この場合も、極座標への変換を行うことなく、固有値テンプレートの算出を行うことができる。
【００７６】
（第２実施形態）
次に、本発明の第２実施形態に係る画像処理装置２０について説明する。
【００７７】
本実施形態に係る画像処理装置２０は、照明の変動に対するロバスト化を強化するため、照明の変動にロバストである複素勾配画像を用いて、固有値テンプレートの作成やマッチング処理を行う。輝度勾配の方向は環境の明るさ変動による画像の輝度変化に対して頑強であるためである。そのため、本実施形態に係る画像処理装置２０では、図６に示すように、テンプレート画像の複素勾配画像を極座標データとして用いるための処理を実行する。
【００７８】
図６のフローチャートのステップＳ３００では、下記の数式１５に従い、テンプレート画像から、輝度勾配の強度と方向からなる複素勾配画像ｆを計算する。
【数１５】
0006565667.tif 000016
【００７９】
数式１５において、ｆｘ、ｆｙは、元の入力画像に対してガウシアンフィルタを適用した、水平、垂直方向の微分画像である。
【００８０】
なお、テンプレート画像と、マッチング対象となる入力画像とで背景の輝度が大きく変わると、輝度勾配の方向が反転（π回転）することがある。そこで、数式１５では、輝度勾配の方向が反転した場合でも、内積の値が同じになるように回転成分に２をかけることで、このような変動にロバストにしている。
【００８１】
さらに、位相限定相関法の考え方を適用するため、この複素勾配画像の２次元フーリエ変換した周波数空間における位相成分を取り出した画像（振幅を１にした画像）を生成しても良い。
【００８２】
ここで、テンプレート画像として通常の輝度画像を使う場合は、ある座標の輝度値ｆ（ｘ，ｙ）は、画像中心を原点として面内回転Δθでワープした座標ｆΔθ（ｘ’,ｙ’）における輝度値と一致する。したがって以下の数式１６の関係が成り立つ。なお、数式１６における「imrotate」は画像の回転を意味する関数である。
【数１６】
0006565667.tif 000017
【００８３】
しかしながら、複素勾配画像を使った場合は、座標（ｘ’,ｙ’）における輝度は、ｅｘｐ（ｉΔθ）×ｆΔθ（ｘ’,ｙ’）となり、上記の関係が成り立たない。すなわち、複素勾配画像を使用した場合、画像中の対象物が面内回転していると、極座標上への変換により、以下の数式１７に示す関係となってしまう。
【数１７】
0006565667.tif 000018
【００８４】
そこで、本実施形態では、以下の数式１８に示すように、テンプレート画像の複素勾配画像ｆを、補正行列Ｃを用いて、数式１６の関係が成り立つように補正する。すなわち、複素勾配画像ｆの各座標の輝度ｆ（ｘ，ｙ）に、面内回転をキャンセルするための補正値ｅｘｐ（−ｉθ）を掛ける。補正行列Ｃは、その補正値ｅｘｐ（−ｉθ）が入った行列である。
【数１８】
0006565667.tif 000019
【００８５】
次いで、ステップＳ３２０において、各座標の輝度が補正された複素勾配画像（以下、補正複素勾配画像）ｆ’を、数式１９に示すように極座標上の補正複素勾配画像に変換する。
【数１９】
0006565667.tif 000020
【００８６】
そして、ステップＳ３３０において、極座標変換された補正複素勾配画像ｆ’（ｒ，τ）を、以下の数式２０に示すように、回転軸方向にフーリエ変換しつつ、Ｍ個の低周波数成分を基底とする周波数基底関数ｅｘｐ（ｉｋτ）を固有関数として用いた近似式により、周波数分解する。
【数２０】
0006565667.tif 000021
【００８７】
その後、ステップＳ３４０において、ステップＳ３１０において実行した補正をキャンセルするために、以下の数式２１に示すように、補正行列Ｃを極座標変換した補正行列Ｄの逆行列Ｄ−１を、周波数分解結果に掛ける。
【数２１】
0006565667.tif 000022
【００８８】
なお、この場合、補正行列Ｃを極座標変換した補正行列Ｄは以下の数式２２のように表すことができる。
【数２２】
0006565667.tif 000023
【００８９】
これらの関係を模式的に表すと、図７のようになる。複素勾配画像における各座標における輝度ｆ（ｘ，ｙ）について、原点（画像中心）を中心にした円を考える。そうすると、その輝度勾配の方向は、図７の図面では右上がりとなる。これをキャンセルするような補正行列を掛け算して、補正複素勾配画像を作ると、図７に示すように、輝度勾配の方向がキャンセルされるので、数式１６の関係が成り立つようになる。
【００９０】
なお、上述した例では、極座標変換前に、複素勾配画像に対して、補正行列Ｃを掛け算して補正したが、極座標変換後に、複素勾配画像に対して、補正行列Ｄを掛け算して補正するようにしても良い。
【符号の説明】
【００９１】
１０  カメラ
２０  画像処理装置
３０  マッチングシステム
４０  移動体制御部
５０  制御システム
【図１】
0006565667.tif 000024
【図２】
0006565667.tif 000025
【図３】
0006565667.tif 000026
【図４】
0006565667.tif 000027
【図５】
0006565667.tif 000028
【図６】
0006565667.tif 000029
【図７】
0006565667.tif 000030