位置推定装置、位置推定方法及びプログラム記録媒体
【課題】 推定値が逸脱したか否かを検査し、誤った推定値の場合は回復を試みることのできる位置推定装置を提供する。
【解決手段】 予測部１１はオドメトリセンサ１７が搭載された移動体の位置を予測する。観測部１２は環境内の特徴点を追跡する。予測正誤チェック部１３は、予測部１１における位置の予測の正誤を判断する。予測修正部１４は、予測を修正する。更新部１５は、正しい予測に基づいて移動体の位置及び／又は姿勢を更新する。
【解決手段】 予測部１１はオドメトリセンサ１７が搭載された移動体の位置を予測する。観測部１２は環境内の特徴点を追跡する。予測正誤チェック部１３は、予測部１１における位置の予測の正誤を判断する。予測修正部１４は、予測を修正する。更新部１５は、正しい予測に基づいて移動体の位置及び／又は姿勢を更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カメラ等のセンサが搭載された移動体の位置を推定する位置推定装置、位置推定方法及び位置推定プログラムを記録したプログラム記録媒体に関する。
【背景技術】
【０００２】
加速度センサや速度センサ等のオドメトリセンサと、測距センサや撮像センサ等の撮像センサという２種類のセンサからなるセンサ群を搭載した移動体では、センサ群の情報を用いて自己の位置及び／又は姿勢の推定を行う。適用できる移動体の例としては、例えば自走又は自律型のロボットや、自動車等がある。移動体は、自身のおかれた環境内で障害物を避けた行動経路を採る必要がある。この場合には、例えばステレオビジョンを用いて周囲の環境の３次元情報を計測し、この３次元情報から環境情報を得、上記センサ群を用いて環境内の自己の姿勢・位置を推定する。
【０００３】
下記特許文献１には、外部状態検出手段によって検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成し、３次元地図の情報に基づいて対基準面高さ地図の情報を変更し、変更された地図情報を環境地図として自律的に行動経路を決定し移動動作を制御する移動型ロボット装置が記載されている。また、自動車は、ＧＰＳなどが使用できない状態、例えばトンネル内を走行する場合に、自己の位置を推定するときに使用する。
【０００４】
図８は、環境が固定（変化しない）の状況におかれた、オドメトリセンサ４１とビジョンセンサ４２を搭載したロボット４０を示す。環境が固定であり、特徴点４３〜４７の位置は既知である。特徴点４３〜４７を基に、例えばカルマンフィルタのような予測フィルタを用いて位置を推定するアルゴリズムを採る。なお、オドメトリセンサ４１とビジョンセンサ４２からのセンサ値は、例えば無線信号により、パーソナルコンピュータ４８に送信されてもよい。パーソナルコンピュータ４８は、上記センサ値を受信し、既知である特徴点４３〜４７を基に、例えばカルマンフィルタのような予測フィルタを用いてロボットの位置を推定してもよい。
【０００５】
カルマンフィルタについては、非特許文献１に記載されている。直接観測できないある状態（例えば、位置、速度、加速度などの現在の車の状態）を間接的な情報から計算し、最適な推定を行うフィルタである。状態は確率分布モデルで表現する。確率分布モデルとしては、例えば正規分布を用いることができる。
【０００６】
図９は、カルマンフィルタのアルゴリズムを示すフローチャートである。初期化ステップ（Ｓ０）の後に予測ステップ（Ｓ１）、観測ステップ（Ｓ２）、更新ステップ（Ｓ３）がある。予測ステップ（Ｓ１）は、加速度センサや速度センサ等のオドメトリセンサ４１と、前フレームのシステムの状態の推定結果から、現フレームのロボットの状態を予測する。オドメトリセンサ４１の値や前フレームのカメラの位置は過去の情報であるため、オドメトリセンサ４１の出力値から計算したロボットの位置は予測値となる。観測ステップ（Ｓ２）は、予測ステップ（Ｓ１）で使用したオドメトリセンサ４１とは別の観測センサ（カメラ等）４２で環境を計測する。具体的には、ロボット４０に搭載されている撮像センサ４２を用いて環境を撮像し、撮像した画像から特徴点（画像処理で識別できる目印、ランドマーク）４３〜４７を観測する。更新ステップ（Ｓ３）は、ロボットの位置及び／又は姿勢を観測ステップ（Ｓ２）の観測結果から更新（修正）する。ロボットの位置及び／又は姿勢（状態）を更新する方法は、非特許文献１に記載されている、非線形に対応した一般の拡張カルマンフィルタと同じ方法である。
【０００７】
【特許文献１】特開２００６−１１８８０号公報
【非特許文献１】“A Introduction to the Kalman Filter”, Greg Welch, Gary Bishop, Technical Report 95-041Department of Computer Science, University of North Carolina (1995).
【発明の開示】
【発明が解決しようとする課題】
【０００８】
ところで、上記図９に示したカルマンフィルタのアルゴリズムを用いた位置推定システムにあって、位置の推定値が実際の値から大きく逸脱している場合には、推定値が逸脱したまま計算を続けてしまう。このため、従来の自己推定システムは、ロバストではない。また、たとえ推定が間違ったことがわかったとしても、推定値が間違っていないときのセンサシステムの位置の軌跡が使えないため、蓄えた情報は捨てて再度位置の初期化を行ってから推定するシステムである場合が多い。
【０００９】
本発明は、上記実情に鑑みてなされたものであり、推定値が逸脱したか否かを検査し、誤った推定値の場合は回復を試みることのできる位置推定装置、位置推定方法及びプログラム記録媒体の提供を目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る位置推定装置は、上記課題を解決するために、予測部が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測部が上記予測部で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック部が上記観測部における環境内の特徴点の追跡結果に基づいて上記予測部における移動体の位置の予測の正誤を判断し、予測修正部が上記予測正誤チェック部により上記位置の予測が正しくないと判断すると予測を修正し、更新部が上記予測正誤チェック部により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１１】
本発明に係る位置推定方法は、上記課題を解決するために、予測工程が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測工程が上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック工程が上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における移動体の位置予測の正誤を判断し、予測修正工程が上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正し、更新工程が上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１２】
本発明に係るプログラム記録媒体は、上記課題を解決するために、移動体の位置を予測する位置推定装置にて実行される位置推定プログラムを記録しており、上記位置推定プログラムは、予測工程が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測工程が上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック工程が上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における移動体の位置の予測の正誤を判断し、予測修正工程が上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正し、更新工程が上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１３】
なお、本発明では、上記図８に示したパーソナルコンピュータのような外部信号処理装置にあって、上記オドメトリセンサや観測センサからのセンサ値を無線信号又は有線信号にて受信し、環境内の特徴点を基に、例えばカルマンフィルタのような予測フィルタを用いてロボットの位置を推定してもよい。
【発明の効果】
【００１４】
本発明によれば、推定値が逸脱したか否かを検査し、誤った推定値の場合は回復を試みることができる。このため、推定が誤った場合にはセンサシステムの位置の推定値を回復させることにより前回までのセンサシステムの位置の軌跡を反映させることができるため、システムはロバストである。
【発明を実施するための最良の形態】
【００１５】
以下、本発明を実施するための最良の形態について図面を参照しながら説明する。
【００１６】
先ず、環境が固定（変化しない）の状況におかれた、移動体（例えばロボット）に適用される位置推定装置を実施の形態として説明する。移動体は、オドメトリセンサとビジョンセンサを搭載しており、位置推定装置はオドメトリセンサとビジョンセンサの各センサ値を用いる。
【００１７】
図１は、位置推定装置１０の機能ブロック構成を示すブロック図である。実際には、位置推定装置のハードウェアは、バスを介してＣＰＵ、ＲＡＭ、ＲＯＭ、ＨＤＤ及び各種センサを接続した構成である。ＣＰＵが本発明の位置推定方法に基づいた位置推定プログラムをＨＤＤ、又はＲＯＭから取り出し、ＲＡＭをワークエリアとして逐次実行することにより、図１の機能ブロックが構成される。本発明のプログラム記録媒体は、例えば、ＣＤ−ＲＯＭや半導体メモリなどであり、上記ＨＤＤ等に位置推定プログラムを転送している。もちろん、ＨＤもプログラム記録媒体に相当する。
【００１８】
図１の機能ブロックにおける位置推定装置１０は、上記センサが搭載された移動体の位置を予測する予測部１１と、環境内の特徴点を追跡する観測部１２と、予測部１１における位置の予測の正誤を判断する予測正誤チェック部１３と、予測を修正する予測修正部１４と、正しい予測に基づいて移動体の位置及び／又は姿勢を更新する更新部１５とを備える。
【００１９】
予測部１１にはバッファ１８が接続されている。バッファ１８には、オドメトリセンサ１７により検知されセンサ値が格納される。予測部１１は、バッファ１８に格納されたセンサ値を読み出す。また、予測部１１には更新部１５により更新された位置の推定結果が前フレームの位置の推定結果として次のフレームにおける予測部１１での予測時に用いられるように供給される。このため、予測部１１は、現フレームにおけるオドメトリセンサ１７のセンサ値と、前フレームの位置の推定結果とを予測フィルタによりフィルタリングし、各センサが搭載された移動体の位置を予測する。
【００２０】
オドメトリセンサ１７は、加速度センサや速度センサ等である。移動体の加速度や移動速度を検知する。
【００２１】
観測部１２にはバッファ２０が接続されている。バッファ２０には、上記予測部１１で使用したオドメトリセンサ１７とは別の観測センサ１９により検知されたセンサ値が格納される。観測部１２は、バッファ２０に格納された上記観測センサ１９のセンチ値を読み出す。また、観測部１２には予測修正部１４により修正された予測修正値が供給される。観測部１２は予測部１１で使用したオドメトリセンサ１７とは別の観測センサ１９により環境を計測し、特徴点を毎フレーム毎に追跡して位置を観測する。観測部１２は、予測部１１で求めた予測位置及び／又は姿勢を用いて探索範囲を決め、上記特徴点の追跡を行うようにしてもよい。その場合、観測部１２は、事前にわかっている三次元空間上の特徴点の位置とカルマンフィルタで推定したロボットの位置データから画像内の特徴点の位置を推定し、特徴点付近の輝度画像データと特徴点付近の輝度画像の姿勢データから、移動体の位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形し、上記探索範囲と上記テンプレートを基にテンプレートマッチングを行って上記特徴点の追跡を行う。
【００２２】
予測正誤チェック部１３は、観測部１２における特徴点の追跡結果に基づいて予測部１１における位置の予測の正誤を判断する。具体的に、予測正誤チェック部１３は、観測部１２における特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断する。
【００２３】
予測修正部１４は、予測正誤チェック部１３により上記位置の予測が正しくないと判断されると予測を修正する。具体的に、予測修正部１４は、予測正誤チェック部１３にて、予測が正しくないと判断されると、存在確率分布モデルの標準偏差の値が大きくなるよう変更し、観測部１２における上記探索範囲を広げる。
【００２４】
更新部１５は、予測正誤チェック部１３により上記位置の予測が正しいと判断されると移動体の位置及び／又は姿勢を更新し、更新結果を出力端子１６からフレームごとに出力する。また、更新部１５で更新した位置の推定結果は、上記前フレームの位置の推定結果として次のフレームにおける上記予測部１１での予測時に使われる。
【００２５】
次に、位置推定装置１０の動作について図２のフローチャートを用いて説明する。このフローチャートは、ロボットの位置及び／又は姿勢の推定を目的とした位置推定方法の処理手順を示す。初期化（ステップＳ１０）の後に予測ステップ（Ｓ１１）、観測ステップ（Ｓ１２）、予測の正誤チェックステップ（Ｓ１３）、更新ステップ（Ｓ１５）が続く。また、予測の正誤チェックステップ（Ｓ１３）では、予測の修正ステップ（Ｓ１４）への分岐もある。予測の修正ステップ（Ｓ１４）からは、観測ステップ（Ｓ１２）に戻る。また、更新ステップ（Ｓ１５）の後にはステップＳ１６にて次のフレームの処理に移る。これは、上記図９に示した従来のカルマンフィルタシステムに、形式的には近いものであるが、予測の正誤チェックステップと予測の修正ステップとを新たに設けており、この二つのステップは技術的に容易に想到できるものではない。
【００２６】
ステップＳ１１の予測ステップは、加速度センサや速度センサ等のオドメトリセンサ１７のセンサ値と、前フレームのシステムの状態の推定結果から、現フレームのロボットの状態を予測する。オドメトリセンサ１７の値や前フレームのカメラの位置は過去の情報であるため、オドメトリセンサ１７の出力値から計算したロボットの位置は予測値となる。
【００２７】
ステップＳ１２の観測ステップは、予測ステップで使用したオドメトリセンサ１７とは別の観測センサ（カメラ等）１９で環境を計測する。具体的には、ロボットに搭載されている撮像センサを用いて環境を撮像し、撮像した画像から特徴点（画像処理で識別できる目印、ランドマーク）を観測する。この観測ステップ（Ｓ１２）は、毎フレーム毎に特徴点のトラッキング（追跡）を行い、位置を観測する。
【００２８】
この観測ステップ（Ｓ１２）は、予測ステップ（Ｓ１１）で求めた予測位置及び／又は姿勢を用いて探索範囲を決めて特徴点の追跡を行うことができる。
【００２９】
撮像センサがピンホールカメラモデルに近い場合について説明する。ピンホールカメラモデルは、図３に示すように、大きく分けて２種類のパラメータで構成される。カメラ間の位置及び／又は姿勢に関する外部パラメータ（Extrinsic Parameter）ＰＥと、各カメラ内のパラメータであるカメラ内部パラメータ（Intrinsic Parameter）ＰＩである。
【００３０】
外部パラメータＰＥは、図３にあって、世界座標系（World Coord）ＷＣと３次元空間上のカメラ座標系（Camera Coord）ＣＣとの位置及び／又は姿勢の関係を表している。図３中の世界座標系ＷＣで定義されたｗ＝［ｗｘ ｗｙ ｗｚ］Ｔと、３次元空間上のカメラ座標系ＣＣで定義されたｘ＝［ｘｘ ｘｙ ｘｚ］Ｔとは以下の（１）式の関係が成り立つ。ただし、Ｒは３×３の回転行列、ｔは３×１の並進行列を示す。
【００３１】
【数１】
【００３２】
内部パラメータは３次元空間上のカメラ座標系ＣＣとの位置関係を表す。カメラ内部パラメータＡは以下の（２）式で表される。
【００３３】
【数２】
【００３４】
ただし、ｆは焦点距離、ｋｕ，ｋｖは画素と３次元空間の距離との比率（スケール因子）、ｕ０，ｖ０は像平面の中心の位置（単位は画素）、θは光軸ｏに対するｕ軸とｖ軸とのねじれ（skew）を表す。
【００３５】
３次元空間上のカメラ座標系ＣＣで定義されたｘ＝［ｘｘ ｘｙ ｘｚ］Ｔと、カメラの像平面π上にある画素の位置ｍ〜＝［ｍｉ ｍｊ １］Ｔ（同次座標系で表現）との位置関係は以下の（３）式のとおりである。ただし、λは任意の数である。
【００３６】
【数３】
【００３７】
探索範囲の大きさは、カルマンフィルタの（観測結果と、予測から計算された「予測される観測値」との）予測偏差の共分散行列ΣＶから求める。求める共分散行列ΣＶは下記の（４）式で求める。
【００３８】
【数４】
【００３９】
ただし、上記（４）式にあって、ΣＸは、ロボットの位置及び／又は姿勢を表す存在確率分布の共分散行列である。また、▽Ｈは上記（４）式を、［ｍｉ ｍｊ］Ｔ＝Ｈ（Ｒ，ｔ，［Ｘ Ｙ Ｚ］Ｔ）とλを消すように式変形した関数Ｈのナブラ（各入力パラメータでの偏微分値を各項目に持つ偏微分行列）、ΣＳは観測時に生じる観測ノイズ（前提として正規分布に従う）の共分散行列とする。
【００４０】
本実施の形態では、存在確率分布として正規分布モデルを用いている。図４は正規分布モデルを説明するための図である。横軸は状態を示し、縦軸は存在確率を示す。移動体が存在する範囲を確率で示している。図４に示すように、状態の平均μから、±2倍の標準偏差（±2）の範囲には、約95%の確率で移動体が存在する。本実施の形態は、図４で示す範囲（状態の平均μから、±2倍の標準偏差（±２σ）の範囲）を探索範囲とする。もちろん、標準偏差の 3倍の範囲（±3倍）でも構わない。その時は約 97.5%の確率で存在することになる。範囲が大きくなるほど、探索範囲内に存在する確率は大きくなるが、計算量は多くなる。状況に応じて変更するのが好ましい。
【００４１】
観測ステップ（Ｓ１２）では探索範囲が決まると、特徴点追跡のために、特徴点のマッチングを行う。マッチングの方法は色々ある。どの方法でも構わないが、本実施の形態は輝度情報を用いた特徴点周辺のテンプレートマッチングを行う。図５は、特徴点周辺のテンプレートマッチングを説明するための図である。例えば、ハードディスクＨＤに記憶されている、事前にわかっている三次元空間上の特徴点の位置Ｄ０とカルマンフィルタで推定したロボットの位置データＤ１から、まずステップＳ２１にて画像内の特徴点の位置を推定する。また、特徴点付近の輝度画像（初期設定）データＤ２と特徴点付近の輝度画像の姿勢（初期設定）データＤ３から、ロボットの位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形する(ステップＳ２２)。以上のステップＳ２１とステップＳ２２から、探索範囲とテンプレートが得られるので、ステップＳ２３にて、その情報（探索範囲とテンプレート）を元にテンプレートマッチングを行う。ステップＳ２３の結果が特徴点の前フレームにあった特徴点追跡の結果データＤ４となる。
【００４２】
次に、ステップＳ１３の予測の正誤チェックステップについて説明する。ロボットの位置及び／又は姿勢の推定が実際の位置及び／又は姿勢と近い予測を行っている場合は、特徴点追跡は成功すると考えられる。逆に、ロボットの位置及び／又は姿勢の推定が実際とは逸脱した予測を行っている場合は、特徴点追跡は失敗すると考えられる。図６は予測が正しい場合（ａ）と、誤った予測をした場合（ｂ）を示す図である。カメラ焦点位置２１の予測が正しい場合（図６（ａ））、実際の特徴点２２は像平面２３の予測探索範囲２４内にあり、カメラ位置からの予測特徴点２５の近くにある。カメラ焦点位置の予測が正しくない場合（図６（ｂ））、実際の特徴点２２は像平面２３の予測探索範囲２４外にあり、カメラ位置からの予測特徴点２５から大きくはずれている。特徴点追跡の結果が成否であるかの判断は、マッチングスコアで行う。
【００４３】
具体的に、予測の正誤チェックステップ（Ｓ１３）では、特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断する。
【００４４】
次に、ステップＳ１５の更新ステップについて説明する。更新ステップ（Ｓ１５）は、ロボットの位置及び／又は姿勢を観測ステップの観測結果から更新する。ロボットの位置及び／又は姿勢（状態）を更新する方法は、非特許文献１に記載されている、非線形に対応した一般の拡張カルマンフィルタと同じ方法である。
【００４５】
次に、ステップＳ１４の予測の修正ステップについて説明する。予測の正誤チェックステップ（ステップＳ１３）にて、予測が正しくないと判断したときには、予測の修正ステップ（ステップＳ１４）にて予測を修正する。ロボットの位置が正しくないので、図４に示した存在確率分布モデルの標準偏差の値が大きくなるよう変更する。この処理で、ロボットの位置及び／又は姿勢の存在確率分布が曖昧になるが、多くの範囲の可能性を広げ、実際の状態をカバーすることが出来るようになる。
【００４６】
ロボットの位置が1次元で表現している場合を例として説明する。ロボットの位置が1次元で表現されている場合は、存在確率分布は図７のようになる。予測の正誤チェックステップ（Ｓ１３）にて、図７（ａ）のように予測が間違っていると判断したとき、実際のロボットの位置は存在確率分布２６の範囲にはないと考えられる。
【００４７】
言い換えると、予測の正誤チェックステップ（Ｓ１３）にて予測が間違っていると判断したとき、ロボットの位置は間違って予測した存在確率分布の範囲２６から外れた範囲２７又は２８にあると考えられる。本実施の形態では、図７（ｂ）に示すようにロボットの位置の存在確率分布の標準偏差σの値を大きくする。そのことにより、ロボットの位置の曖昧さが大きくなるが、ロボットの位置の予測の範囲(存在確率)２９も、上記範囲２６に範囲３０及び３１を加えた分、大きくなる。これは、１次元の例であるが２次元以上の状態変数の場合も同様に標準偏差の値を大きくすることで予測を修正する。
【００４８】
予測の修正ステップ（Ｓ１４）にて予測の修正を行った後に再度ステップＳ１２の観測フェーズに移り、同じフレーム(時間)で処理を行い、次のフレーム(時間)には移らない。ステップＳ１３の予測の正誤チェックステップで、予測が正しいと判断されるまで繰り返す。但し、予測があまりにも大きく逸脱している時は、トラッキングしたい特徴点が画像内にない場合があるため、トラッキングが成功することがないという問題がある。
【００４９】
その間、各センサの出力は、バッファ１８及び２０に保存する。計算速度が充分速いと、ステップＳ１４の予測の修正フェーズにより予測が時間的に合わなくなったとしても、バッファ１８及び２０に格納されている過去の各センサの出力値を使って時間的に追いつくことが可能である。
【００５０】
次に、上記存在確率分布の共分散行列ΣＸについて詳細に説明する。まず、状態変数ベクトルｘは（５）式によって表される。ただし、ｘはカメラの位置、ωはカメラの姿勢を表す。また、Ｐ１は特徴点の位置#1、Ｐｎは特徴点の位置#nを表す。
【００５１】
【数５】
【００５２】
すると、状態変数の共分散行列ΣＸは（６）式によって表される。この（６）式では、各行、各列に状態変数成分に対応する共分散値が入る。
【００５３】
【数６】
【００５４】
ここで、本発明の位置推定方法がマルコフ連鎖であることを考える。マルコフ連鎖は、考慮すべき過去が一つ前の時点のみの過程（process）を指し、それより前の過去は考慮していない。以下の（７）式で表される。
【００５５】
【数７】
【００５６】
このマルコフ連鎖で考えると、上記図２に示したフローチャートは以下の各ステップとなる。先ず、予測ステップでは、オドメトリセンサから得られる移動体の速度ｘ'、角速度ω'を用いて次フレームの移動体の位置を予測する。式は以下（８）式の通りである。但し、ｘ−ｔ＋１は、時刻 t+1 の予測状態である。Σ−ｘｔ＋１は時刻 t+1 の予測状態の共分散行列、Σａは予測ノイズ(平均0分散Σａの速度であるとしている)。▽はナブラ、▽Fは関数Fを状態変数を元に一次偏微分した行列を表す。
【００５７】
【数８】
【００５８】
次に、観測ステップについて説明する。時刻 t+1 の予測状態Σ−ｘｔ＋１より、カメラと特徴点の予測位置がわかっているため、カメラ画像の特徴点の射影位置を予測することが可能である。特徴点#の予測された射影位置ｓ＾＃ｔ＋１を算出する式は以下（９）式の通りである。但し、λはある数、Ａはカメラ内部パラメータ行列、Ｒ（ω−ｔ＋１）は回転行列を表す。
【００５９】
【数９】
【００６０】
予測とは別に、特徴点の画像内の位置がｓ＃ｔ＋１と観測されたとする。観測値(トラッキングで求めた位置) ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間に差ｖ＾が発生する。その予測値と観測値との差ｖ＾の分散Σｖ＾を求めることができる。式は以下の（１０）式となる。
【００６１】
【数１０】
【００６２】
上記（１０）式にあって、▽はナブラ，∇Hは関数Hを，状態変数を元に一次偏微分した行列を表し、Σｓは計測誤差を表す。また、ｖ＾は、単位は画素で、予測した画像内の特徴点の位置と、観測した画像内の特徴点の位置との差を示している。ｖ＾は平均0，分散Σｖ＾，の分布をすると仮定すると，予測位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾) ( 標準偏差の2倍 )の範囲に観測値ｓ＃ｔ＋１がある確率は約95%であるといえる。言い換えると、予測位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾)の範囲を探索すれば、95%の確率で探索が成功する。
【００６３】
次に、更新ステップについて説明する。観測ステップで得られた、観測値をもとに、画素ｖ＾を次の（１１）式のように求める。
【００６４】
【数１１】
【００６５】
そしてカルマンゲインＷを次の（１２）式のように求め、状態変数ｘｔ＋１を（１３）式のように、また共分散行列Σｘｔ＋１を（１４）式のように更新する。
【００６６】
【数１２】
【００６７】
【数１３】
【００６８】
【数１４】
【００６９】
以上の処理で１フレームの処理が完了する。
【００７０】
さらに、上記１フレームの処理の中で、予測の正誤チェックステップは、観測ステップの「観測値(トラッキングで求めた位置) ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間に発生する差ｖ＾」がｖ＾の分散Σｖ＾から求めた2.0*sqrt(Σｖ＾) (標準偏差の2倍) の範囲内にないことがわかったとき、その特徴点とカメラとの位置関係の推定は間違っているとする。トラッキングの場合に言い換えると、予測の画像内の特徴点の位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾) (標準偏差の2倍)の範囲でのトラッキングが失敗した時、その特徴点とカメラとの位置関係の推定は間違っているとする。
【００７１】
カメラフレームに入っていると予想される特徴点群の半分以上の推定が間違っている時、状態変数全体の推定が間違っていると判断する。
【００７２】
次に、予測の修正ステップについて説明する。予測の修正ステップでは、状態変数全体の推定が間違っている状態は、カメラの位置推定が間違っている状態とする。
【００７３】
本発明では、「全体の推定が間違っている」ことは、カメラの推定位置が間違っていることと同じなので、カメラの状態変数の分散の値を大きくすることで対処する。数式で表すと以下（１５）式のようになる。
【００７４】
【数１５】
【００７５】
カメラの位置及び／又は姿勢の分散が大きくなると、観測フェーズの観測値ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間の差ｖ＾の分散の値が（１６）式のように大きくなる。
【００７６】
【数１６】
【００７７】
これは、探索範囲が大きくなることを意味している。
【図面の簡単な説明】
【００７８】
【図１】位置推定装置の構成を示すブロック図である。
【図２】位置推定装置の動作を説明するためのフローチャートである。
【図３】ピンホールカメラモデルを説明するための図である。
【図４】正規分布モデルを説明するための図である。
【図５】特徴点周辺のテンプレートマッチングを説明するための図である。
【図６】予測が正しい場合と、誤った予測をした場合を示す図である。
【図７】ロボットの位置が1次元で表現されている場合の存在確率分布を示す図である。
【図８】環境が固定（変化しない）の状況におかれた、オドメトリセンサとビジョンセンサを搭載したロボットを示す図である。
【図９】カルマンフィルタのアルゴリズムを示すフローチャートである。
【符号の説明】
【００７９】
１０ 位置推定装置、１１ 予測部、１２ 観測部、１３ 予測正誤チェック部、１４ 予測修正部、１５ 更新部、１７ オドメトリセンサ、１８，２０ バッファ、１９ 観測センサ
【技術分野】
【０００１】
本発明は、カメラ等のセンサが搭載された移動体の位置を推定する位置推定装置、位置推定方法及び位置推定プログラムを記録したプログラム記録媒体に関する。
【背景技術】
【０００２】
加速度センサや速度センサ等のオドメトリセンサと、測距センサや撮像センサ等の撮像センサという２種類のセンサからなるセンサ群を搭載した移動体では、センサ群の情報を用いて自己の位置及び／又は姿勢の推定を行う。適用できる移動体の例としては、例えば自走又は自律型のロボットや、自動車等がある。移動体は、自身のおかれた環境内で障害物を避けた行動経路を採る必要がある。この場合には、例えばステレオビジョンを用いて周囲の環境の３次元情報を計測し、この３次元情報から環境情報を得、上記センサ群を用いて環境内の自己の姿勢・位置を推定する。
【０００３】
下記特許文献１には、外部状態検出手段によって検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成し、３次元地図の情報に基づいて対基準面高さ地図の情報を変更し、変更された地図情報を環境地図として自律的に行動経路を決定し移動動作を制御する移動型ロボット装置が記載されている。また、自動車は、ＧＰＳなどが使用できない状態、例えばトンネル内を走行する場合に、自己の位置を推定するときに使用する。
【０００４】
図８は、環境が固定（変化しない）の状況におかれた、オドメトリセンサ４１とビジョンセンサ４２を搭載したロボット４０を示す。環境が固定であり、特徴点４３〜４７の位置は既知である。特徴点４３〜４７を基に、例えばカルマンフィルタのような予測フィルタを用いて位置を推定するアルゴリズムを採る。なお、オドメトリセンサ４１とビジョンセンサ４２からのセンサ値は、例えば無線信号により、パーソナルコンピュータ４８に送信されてもよい。パーソナルコンピュータ４８は、上記センサ値を受信し、既知である特徴点４３〜４７を基に、例えばカルマンフィルタのような予測フィルタを用いてロボットの位置を推定してもよい。
【０００５】
カルマンフィルタについては、非特許文献１に記載されている。直接観測できないある状態（例えば、位置、速度、加速度などの現在の車の状態）を間接的な情報から計算し、最適な推定を行うフィルタである。状態は確率分布モデルで表現する。確率分布モデルとしては、例えば正規分布を用いることができる。
【０００６】
図９は、カルマンフィルタのアルゴリズムを示すフローチャートである。初期化ステップ（Ｓ０）の後に予測ステップ（Ｓ１）、観測ステップ（Ｓ２）、更新ステップ（Ｓ３）がある。予測ステップ（Ｓ１）は、加速度センサや速度センサ等のオドメトリセンサ４１と、前フレームのシステムの状態の推定結果から、現フレームのロボットの状態を予測する。オドメトリセンサ４１の値や前フレームのカメラの位置は過去の情報であるため、オドメトリセンサ４１の出力値から計算したロボットの位置は予測値となる。観測ステップ（Ｓ２）は、予測ステップ（Ｓ１）で使用したオドメトリセンサ４１とは別の観測センサ（カメラ等）４２で環境を計測する。具体的には、ロボット４０に搭載されている撮像センサ４２を用いて環境を撮像し、撮像した画像から特徴点（画像処理で識別できる目印、ランドマーク）４３〜４７を観測する。更新ステップ（Ｓ３）は、ロボットの位置及び／又は姿勢を観測ステップ（Ｓ２）の観測結果から更新（修正）する。ロボットの位置及び／又は姿勢（状態）を更新する方法は、非特許文献１に記載されている、非線形に対応した一般の拡張カルマンフィルタと同じ方法である。
【０００７】
【特許文献１】特開２００６−１１８８０号公報
【非特許文献１】“A Introduction to the Kalman Filter”, Greg Welch, Gary Bishop, Technical Report 95-041Department of Computer Science, University of North Carolina (1995).
【発明の開示】
【発明が解決しようとする課題】
【０００８】
ところで、上記図９に示したカルマンフィルタのアルゴリズムを用いた位置推定システムにあって、位置の推定値が実際の値から大きく逸脱している場合には、推定値が逸脱したまま計算を続けてしまう。このため、従来の自己推定システムは、ロバストではない。また、たとえ推定が間違ったことがわかったとしても、推定値が間違っていないときのセンサシステムの位置の軌跡が使えないため、蓄えた情報は捨てて再度位置の初期化を行ってから推定するシステムである場合が多い。
【０００９】
本発明は、上記実情に鑑みてなされたものであり、推定値が逸脱したか否かを検査し、誤った推定値の場合は回復を試みることのできる位置推定装置、位置推定方法及びプログラム記録媒体の提供を目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る位置推定装置は、上記課題を解決するために、予測部が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測部が上記予測部で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック部が上記観測部における環境内の特徴点の追跡結果に基づいて上記予測部における移動体の位置の予測の正誤を判断し、予測修正部が上記予測正誤チェック部により上記位置の予測が正しくないと判断すると予測を修正し、更新部が上記予測正誤チェック部により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１１】
本発明に係る位置推定方法は、上記課題を解決するために、予測工程が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測工程が上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック工程が上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における移動体の位置予測の正誤を判断し、予測修正工程が上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正し、更新工程が上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１２】
本発明に係るプログラム記録媒体は、上記課題を解決するために、移動体の位置を予測する位置推定装置にて実行される位置推定プログラムを記録しており、上記位置推定プログラムは、予測工程が現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測し、観測工程が上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム毎に追跡して位置を観測し、予測正誤チェック工程が上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における移動体の位置の予測の正誤を判断し、予測修正工程が上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正し、更新工程が上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する。
【００１３】
なお、本発明では、上記図８に示したパーソナルコンピュータのような外部信号処理装置にあって、上記オドメトリセンサや観測センサからのセンサ値を無線信号又は有線信号にて受信し、環境内の特徴点を基に、例えばカルマンフィルタのような予測フィルタを用いてロボットの位置を推定してもよい。
【発明の効果】
【００１４】
本発明によれば、推定値が逸脱したか否かを検査し、誤った推定値の場合は回復を試みることができる。このため、推定が誤った場合にはセンサシステムの位置の推定値を回復させることにより前回までのセンサシステムの位置の軌跡を反映させることができるため、システムはロバストである。
【発明を実施するための最良の形態】
【００１５】
以下、本発明を実施するための最良の形態について図面を参照しながら説明する。
【００１６】
先ず、環境が固定（変化しない）の状況におかれた、移動体（例えばロボット）に適用される位置推定装置を実施の形態として説明する。移動体は、オドメトリセンサとビジョンセンサを搭載しており、位置推定装置はオドメトリセンサとビジョンセンサの各センサ値を用いる。
【００１７】
図１は、位置推定装置１０の機能ブロック構成を示すブロック図である。実際には、位置推定装置のハードウェアは、バスを介してＣＰＵ、ＲＡＭ、ＲＯＭ、ＨＤＤ及び各種センサを接続した構成である。ＣＰＵが本発明の位置推定方法に基づいた位置推定プログラムをＨＤＤ、又はＲＯＭから取り出し、ＲＡＭをワークエリアとして逐次実行することにより、図１の機能ブロックが構成される。本発明のプログラム記録媒体は、例えば、ＣＤ−ＲＯＭや半導体メモリなどであり、上記ＨＤＤ等に位置推定プログラムを転送している。もちろん、ＨＤもプログラム記録媒体に相当する。
【００１８】
図１の機能ブロックにおける位置推定装置１０は、上記センサが搭載された移動体の位置を予測する予測部１１と、環境内の特徴点を追跡する観測部１２と、予測部１１における位置の予測の正誤を判断する予測正誤チェック部１３と、予測を修正する予測修正部１４と、正しい予測に基づいて移動体の位置及び／又は姿勢を更新する更新部１５とを備える。
【００１９】
予測部１１にはバッファ１８が接続されている。バッファ１８には、オドメトリセンサ１７により検知されセンサ値が格納される。予測部１１は、バッファ１８に格納されたセンサ値を読み出す。また、予測部１１には更新部１５により更新された位置の推定結果が前フレームの位置の推定結果として次のフレームにおける予測部１１での予測時に用いられるように供給される。このため、予測部１１は、現フレームにおけるオドメトリセンサ１７のセンサ値と、前フレームの位置の推定結果とを予測フィルタによりフィルタリングし、各センサが搭載された移動体の位置を予測する。
【００２０】
オドメトリセンサ１７は、加速度センサや速度センサ等である。移動体の加速度や移動速度を検知する。
【００２１】
観測部１２にはバッファ２０が接続されている。バッファ２０には、上記予測部１１で使用したオドメトリセンサ１７とは別の観測センサ１９により検知されたセンサ値が格納される。観測部１２は、バッファ２０に格納された上記観測センサ１９のセンチ値を読み出す。また、観測部１２には予測修正部１４により修正された予測修正値が供給される。観測部１２は予測部１１で使用したオドメトリセンサ１７とは別の観測センサ１９により環境を計測し、特徴点を毎フレーム毎に追跡して位置を観測する。観測部１２は、予測部１１で求めた予測位置及び／又は姿勢を用いて探索範囲を決め、上記特徴点の追跡を行うようにしてもよい。その場合、観測部１２は、事前にわかっている三次元空間上の特徴点の位置とカルマンフィルタで推定したロボットの位置データから画像内の特徴点の位置を推定し、特徴点付近の輝度画像データと特徴点付近の輝度画像の姿勢データから、移動体の位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形し、上記探索範囲と上記テンプレートを基にテンプレートマッチングを行って上記特徴点の追跡を行う。
【００２２】
予測正誤チェック部１３は、観測部１２における特徴点の追跡結果に基づいて予測部１１における位置の予測の正誤を判断する。具体的に、予測正誤チェック部１３は、観測部１２における特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断する。
【００２３】
予測修正部１４は、予測正誤チェック部１３により上記位置の予測が正しくないと判断されると予測を修正する。具体的に、予測修正部１４は、予測正誤チェック部１３にて、予測が正しくないと判断されると、存在確率分布モデルの標準偏差の値が大きくなるよう変更し、観測部１２における上記探索範囲を広げる。
【００２４】
更新部１５は、予測正誤チェック部１３により上記位置の予測が正しいと判断されると移動体の位置及び／又は姿勢を更新し、更新結果を出力端子１６からフレームごとに出力する。また、更新部１５で更新した位置の推定結果は、上記前フレームの位置の推定結果として次のフレームにおける上記予測部１１での予測時に使われる。
【００２５】
次に、位置推定装置１０の動作について図２のフローチャートを用いて説明する。このフローチャートは、ロボットの位置及び／又は姿勢の推定を目的とした位置推定方法の処理手順を示す。初期化（ステップＳ１０）の後に予測ステップ（Ｓ１１）、観測ステップ（Ｓ１２）、予測の正誤チェックステップ（Ｓ１３）、更新ステップ（Ｓ１５）が続く。また、予測の正誤チェックステップ（Ｓ１３）では、予測の修正ステップ（Ｓ１４）への分岐もある。予測の修正ステップ（Ｓ１４）からは、観測ステップ（Ｓ１２）に戻る。また、更新ステップ（Ｓ１５）の後にはステップＳ１６にて次のフレームの処理に移る。これは、上記図９に示した従来のカルマンフィルタシステムに、形式的には近いものであるが、予測の正誤チェックステップと予測の修正ステップとを新たに設けており、この二つのステップは技術的に容易に想到できるものではない。
【００２６】
ステップＳ１１の予測ステップは、加速度センサや速度センサ等のオドメトリセンサ１７のセンサ値と、前フレームのシステムの状態の推定結果から、現フレームのロボットの状態を予測する。オドメトリセンサ１７の値や前フレームのカメラの位置は過去の情報であるため、オドメトリセンサ１７の出力値から計算したロボットの位置は予測値となる。
【００２７】
ステップＳ１２の観測ステップは、予測ステップで使用したオドメトリセンサ１７とは別の観測センサ（カメラ等）１９で環境を計測する。具体的には、ロボットに搭載されている撮像センサを用いて環境を撮像し、撮像した画像から特徴点（画像処理で識別できる目印、ランドマーク）を観測する。この観測ステップ（Ｓ１２）は、毎フレーム毎に特徴点のトラッキング（追跡）を行い、位置を観測する。
【００２８】
この観測ステップ（Ｓ１２）は、予測ステップ（Ｓ１１）で求めた予測位置及び／又は姿勢を用いて探索範囲を決めて特徴点の追跡を行うことができる。
【００２９】
撮像センサがピンホールカメラモデルに近い場合について説明する。ピンホールカメラモデルは、図３に示すように、大きく分けて２種類のパラメータで構成される。カメラ間の位置及び／又は姿勢に関する外部パラメータ（Extrinsic Parameter）ＰＥと、各カメラ内のパラメータであるカメラ内部パラメータ（Intrinsic Parameter）ＰＩである。
【００３０】
外部パラメータＰＥは、図３にあって、世界座標系（World Coord）ＷＣと３次元空間上のカメラ座標系（Camera Coord）ＣＣとの位置及び／又は姿勢の関係を表している。図３中の世界座標系ＷＣで定義されたｗ＝［ｗｘ ｗｙ ｗｚ］Ｔと、３次元空間上のカメラ座標系ＣＣで定義されたｘ＝［ｘｘ ｘｙ ｘｚ］Ｔとは以下の（１）式の関係が成り立つ。ただし、Ｒは３×３の回転行列、ｔは３×１の並進行列を示す。
【００３１】
【数１】
【００３２】
内部パラメータは３次元空間上のカメラ座標系ＣＣとの位置関係を表す。カメラ内部パラメータＡは以下の（２）式で表される。
【００３３】
【数２】
【００３４】
ただし、ｆは焦点距離、ｋｕ，ｋｖは画素と３次元空間の距離との比率（スケール因子）、ｕ０，ｖ０は像平面の中心の位置（単位は画素）、θは光軸ｏに対するｕ軸とｖ軸とのねじれ（skew）を表す。
【００３５】
３次元空間上のカメラ座標系ＣＣで定義されたｘ＝［ｘｘ ｘｙ ｘｚ］Ｔと、カメラの像平面π上にある画素の位置ｍ〜＝［ｍｉ ｍｊ １］Ｔ（同次座標系で表現）との位置関係は以下の（３）式のとおりである。ただし、λは任意の数である。
【００３６】
【数３】
【００３７】
探索範囲の大きさは、カルマンフィルタの（観測結果と、予測から計算された「予測される観測値」との）予測偏差の共分散行列ΣＶから求める。求める共分散行列ΣＶは下記の（４）式で求める。
【００３８】
【数４】
【００３９】
ただし、上記（４）式にあって、ΣＸは、ロボットの位置及び／又は姿勢を表す存在確率分布の共分散行列である。また、▽Ｈは上記（４）式を、［ｍｉ ｍｊ］Ｔ＝Ｈ（Ｒ，ｔ，［Ｘ Ｙ Ｚ］Ｔ）とλを消すように式変形した関数Ｈのナブラ（各入力パラメータでの偏微分値を各項目に持つ偏微分行列）、ΣＳは観測時に生じる観測ノイズ（前提として正規分布に従う）の共分散行列とする。
【００４０】
本実施の形態では、存在確率分布として正規分布モデルを用いている。図４は正規分布モデルを説明するための図である。横軸は状態を示し、縦軸は存在確率を示す。移動体が存在する範囲を確率で示している。図４に示すように、状態の平均μから、±2倍の標準偏差（±2）の範囲には、約95%の確率で移動体が存在する。本実施の形態は、図４で示す範囲（状態の平均μから、±2倍の標準偏差（±２σ）の範囲）を探索範囲とする。もちろん、標準偏差の 3倍の範囲（±3倍）でも構わない。その時は約 97.5%の確率で存在することになる。範囲が大きくなるほど、探索範囲内に存在する確率は大きくなるが、計算量は多くなる。状況に応じて変更するのが好ましい。
【００４１】
観測ステップ（Ｓ１２）では探索範囲が決まると、特徴点追跡のために、特徴点のマッチングを行う。マッチングの方法は色々ある。どの方法でも構わないが、本実施の形態は輝度情報を用いた特徴点周辺のテンプレートマッチングを行う。図５は、特徴点周辺のテンプレートマッチングを説明するための図である。例えば、ハードディスクＨＤに記憶されている、事前にわかっている三次元空間上の特徴点の位置Ｄ０とカルマンフィルタで推定したロボットの位置データＤ１から、まずステップＳ２１にて画像内の特徴点の位置を推定する。また、特徴点付近の輝度画像（初期設定）データＤ２と特徴点付近の輝度画像の姿勢（初期設定）データＤ３から、ロボットの位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形する(ステップＳ２２)。以上のステップＳ２１とステップＳ２２から、探索範囲とテンプレートが得られるので、ステップＳ２３にて、その情報（探索範囲とテンプレート）を元にテンプレートマッチングを行う。ステップＳ２３の結果が特徴点の前フレームにあった特徴点追跡の結果データＤ４となる。
【００４２】
次に、ステップＳ１３の予測の正誤チェックステップについて説明する。ロボットの位置及び／又は姿勢の推定が実際の位置及び／又は姿勢と近い予測を行っている場合は、特徴点追跡は成功すると考えられる。逆に、ロボットの位置及び／又は姿勢の推定が実際とは逸脱した予測を行っている場合は、特徴点追跡は失敗すると考えられる。図６は予測が正しい場合（ａ）と、誤った予測をした場合（ｂ）を示す図である。カメラ焦点位置２１の予測が正しい場合（図６（ａ））、実際の特徴点２２は像平面２３の予測探索範囲２４内にあり、カメラ位置からの予測特徴点２５の近くにある。カメラ焦点位置の予測が正しくない場合（図６（ｂ））、実際の特徴点２２は像平面２３の予測探索範囲２４外にあり、カメラ位置からの予測特徴点２５から大きくはずれている。特徴点追跡の結果が成否であるかの判断は、マッチングスコアで行う。
【００４３】
具体的に、予測の正誤チェックステップ（Ｓ１３）では、特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断する。
【００４４】
次に、ステップＳ１５の更新ステップについて説明する。更新ステップ（Ｓ１５）は、ロボットの位置及び／又は姿勢を観測ステップの観測結果から更新する。ロボットの位置及び／又は姿勢（状態）を更新する方法は、非特許文献１に記載されている、非線形に対応した一般の拡張カルマンフィルタと同じ方法である。
【００４５】
次に、ステップＳ１４の予測の修正ステップについて説明する。予測の正誤チェックステップ（ステップＳ１３）にて、予測が正しくないと判断したときには、予測の修正ステップ（ステップＳ１４）にて予測を修正する。ロボットの位置が正しくないので、図４に示した存在確率分布モデルの標準偏差の値が大きくなるよう変更する。この処理で、ロボットの位置及び／又は姿勢の存在確率分布が曖昧になるが、多くの範囲の可能性を広げ、実際の状態をカバーすることが出来るようになる。
【００４６】
ロボットの位置が1次元で表現している場合を例として説明する。ロボットの位置が1次元で表現されている場合は、存在確率分布は図７のようになる。予測の正誤チェックステップ（Ｓ１３）にて、図７（ａ）のように予測が間違っていると判断したとき、実際のロボットの位置は存在確率分布２６の範囲にはないと考えられる。
【００４７】
言い換えると、予測の正誤チェックステップ（Ｓ１３）にて予測が間違っていると判断したとき、ロボットの位置は間違って予測した存在確率分布の範囲２６から外れた範囲２７又は２８にあると考えられる。本実施の形態では、図７（ｂ）に示すようにロボットの位置の存在確率分布の標準偏差σの値を大きくする。そのことにより、ロボットの位置の曖昧さが大きくなるが、ロボットの位置の予測の範囲(存在確率)２９も、上記範囲２６に範囲３０及び３１を加えた分、大きくなる。これは、１次元の例であるが２次元以上の状態変数の場合も同様に標準偏差の値を大きくすることで予測を修正する。
【００４８】
予測の修正ステップ（Ｓ１４）にて予測の修正を行った後に再度ステップＳ１２の観測フェーズに移り、同じフレーム(時間)で処理を行い、次のフレーム(時間)には移らない。ステップＳ１３の予測の正誤チェックステップで、予測が正しいと判断されるまで繰り返す。但し、予測があまりにも大きく逸脱している時は、トラッキングしたい特徴点が画像内にない場合があるため、トラッキングが成功することがないという問題がある。
【００４９】
その間、各センサの出力は、バッファ１８及び２０に保存する。計算速度が充分速いと、ステップＳ１４の予測の修正フェーズにより予測が時間的に合わなくなったとしても、バッファ１８及び２０に格納されている過去の各センサの出力値を使って時間的に追いつくことが可能である。
【００５０】
次に、上記存在確率分布の共分散行列ΣＸについて詳細に説明する。まず、状態変数ベクトルｘは（５）式によって表される。ただし、ｘはカメラの位置、ωはカメラの姿勢を表す。また、Ｐ１は特徴点の位置#1、Ｐｎは特徴点の位置#nを表す。
【００５１】
【数５】
【００５２】
すると、状態変数の共分散行列ΣＸは（６）式によって表される。この（６）式では、各行、各列に状態変数成分に対応する共分散値が入る。
【００５３】
【数６】
【００５４】
ここで、本発明の位置推定方法がマルコフ連鎖であることを考える。マルコフ連鎖は、考慮すべき過去が一つ前の時点のみの過程（process）を指し、それより前の過去は考慮していない。以下の（７）式で表される。
【００５５】
【数７】
【００５６】
このマルコフ連鎖で考えると、上記図２に示したフローチャートは以下の各ステップとなる。先ず、予測ステップでは、オドメトリセンサから得られる移動体の速度ｘ'、角速度ω'を用いて次フレームの移動体の位置を予測する。式は以下（８）式の通りである。但し、ｘ−ｔ＋１は、時刻 t+1 の予測状態である。Σ−ｘｔ＋１は時刻 t+1 の予測状態の共分散行列、Σａは予測ノイズ(平均0分散Σａの速度であるとしている)。▽はナブラ、▽Fは関数Fを状態変数を元に一次偏微分した行列を表す。
【００５７】
【数８】
【００５８】
次に、観測ステップについて説明する。時刻 t+1 の予測状態Σ−ｘｔ＋１より、カメラと特徴点の予測位置がわかっているため、カメラ画像の特徴点の射影位置を予測することが可能である。特徴点#の予測された射影位置ｓ＾＃ｔ＋１を算出する式は以下（９）式の通りである。但し、λはある数、Ａはカメラ内部パラメータ行列、Ｒ（ω−ｔ＋１）は回転行列を表す。
【００５９】
【数９】
【００６０】
予測とは別に、特徴点の画像内の位置がｓ＃ｔ＋１と観測されたとする。観測値(トラッキングで求めた位置) ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間に差ｖ＾が発生する。その予測値と観測値との差ｖ＾の分散Σｖ＾を求めることができる。式は以下の（１０）式となる。
【００６１】
【数１０】
【００６２】
上記（１０）式にあって、▽はナブラ，∇Hは関数Hを，状態変数を元に一次偏微分した行列を表し、Σｓは計測誤差を表す。また、ｖ＾は、単位は画素で、予測した画像内の特徴点の位置と、観測した画像内の特徴点の位置との差を示している。ｖ＾は平均0，分散Σｖ＾，の分布をすると仮定すると，予測位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾) ( 標準偏差の2倍 )の範囲に観測値ｓ＃ｔ＋１がある確率は約95%であるといえる。言い換えると、予測位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾)の範囲を探索すれば、95%の確率で探索が成功する。
【００６３】
次に、更新ステップについて説明する。観測ステップで得られた、観測値をもとに、画素ｖ＾を次の（１１）式のように求める。
【００６４】
【数１１】
【００６５】
そしてカルマンゲインＷを次の（１２）式のように求め、状態変数ｘｔ＋１を（１３）式のように、また共分散行列Σｘｔ＋１を（１４）式のように更新する。
【００６６】
【数１２】
【００６７】
【数１３】
【００６８】
【数１４】
【００６９】
以上の処理で１フレームの処理が完了する。
【００７０】
さらに、上記１フレームの処理の中で、予測の正誤チェックステップは、観測ステップの「観測値(トラッキングで求めた位置) ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間に発生する差ｖ＾」がｖ＾の分散Σｖ＾から求めた2.0*sqrt(Σｖ＾) (標準偏差の2倍) の範囲内にないことがわかったとき、その特徴点とカメラとの位置関係の推定は間違っているとする。トラッキングの場合に言い換えると、予測の画像内の特徴点の位置ｓ＾＃ｔ＋１を中心とした2.0*sqrt(Σｖ＾) (標準偏差の2倍)の範囲でのトラッキングが失敗した時、その特徴点とカメラとの位置関係の推定は間違っているとする。
【００７１】
カメラフレームに入っていると予想される特徴点群の半分以上の推定が間違っている時、状態変数全体の推定が間違っていると判断する。
【００７２】
次に、予測の修正ステップについて説明する。予測の修正ステップでは、状態変数全体の推定が間違っている状態は、カメラの位置推定が間違っている状態とする。
【００７３】
本発明では、「全体の推定が間違っている」ことは、カメラの推定位置が間違っていることと同じなので、カメラの状態変数の分散の値を大きくすることで対処する。数式で表すと以下（１５）式のようになる。
【００７４】
【数１５】
【００７５】
カメラの位置及び／又は姿勢の分散が大きくなると、観測フェーズの観測値ｓ＃ｔ＋１と予測値ｓ＾＃ｔ＋１との間の差ｖ＾の分散の値が（１６）式のように大きくなる。
【００７６】
【数１６】
【００７７】
これは、探索範囲が大きくなることを意味している。
【図面の簡単な説明】
【００７８】
【図１】位置推定装置の構成を示すブロック図である。
【図２】位置推定装置の動作を説明するためのフローチャートである。
【図３】ピンホールカメラモデルを説明するための図である。
【図４】正規分布モデルを説明するための図である。
【図５】特徴点周辺のテンプレートマッチングを説明するための図である。
【図６】予測が正しい場合と、誤った予測をした場合を示す図である。
【図７】ロボットの位置が1次元で表現されている場合の存在確率分布を示す図である。
【図８】環境が固定（変化しない）の状況におかれた、オドメトリセンサとビジョンセンサを搭載したロボットを示す図である。
【図９】カルマンフィルタのアルゴリズムを示すフローチャートである。
【符号の説明】
【００７９】
１０ 位置推定装置、１１ 予測部、１２ 観測部、１３ 予測正誤チェック部、１４ 予測修正部、１５ 更新部、１７ オドメトリセンサ、１８，２０ バッファ、１９ 観測センサ
【特許請求の範囲】
【請求項１】
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測部と、
上記予測部で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム追跡して位置を観測する観測部と、
上記観測部における環境内の特徴点の追跡結果に基づいて上記予測部における位置の予測の正誤を判断する予測正誤チェック部と、
上記予測正誤チェック部により上記位置の予測が正しくないと判断すると予測を修正する予測修正部と、
上記予測正誤チェック部により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新部と
を備えることを特徴とする位置推定装置。
【請求項２】
上記観測部は、上記予測部で求めた予測位置及び／又は姿勢を用いて探索範囲を決め、上記特徴点の追跡を行うことを特徴とする請求項１記載の位置推定装置。
【請求項３】
上記観測部は、事前に判っている三次元空間上の特徴点の位置とカルマンフィルタで推定した移動体の位置データから画像内の特徴点の位置を推定し、特徴点付近の輝度画像データと特徴点付近の輝度画像の姿勢データから、移動体の位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形し、上記探索範囲と上記テンプレートを基にテンプレートマッチングを行って上記特徴点の追跡を行うことを特徴とする請求項２記載の位置推定装置。
【請求項４】
上記予測正誤チェック部は、上記観測部における特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断することを特徴とする請求項１記載の位置推定装置。
【請求項５】
上記更新部は、更新した移動体の位置推定結果を上記前フレームの移動体の位置推定結果として次のフレームにおける上記予測部での予測時に使わせることを請求項１記載の位置推定装置。
【請求項６】
上記予測修正部は、上記予測正誤チェック部にて予測が正しくないと判断されると、移動体の存在確率分布モデルの標準偏差の値が大きくなるよう変更し、上記観測部における上記探索範囲を広げることを特徴とする請求項１記載の位置推定装置。
【請求項７】
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測工程と、
上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム追跡して位置を観測する観測工程と、
上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における位置の予測の正誤を判断する予測正誤チェック工程と、
上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正する予測修正工程と、
上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新工程と
を備えることを特徴とする位置推定方法。
【請求項８】
移動体の位置を予測する位置推定装置にて実行される位置推定プログラムを記録しているプログラム記録媒体において、
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測工程と、
上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレームに追跡して位置を観測する観測工程と、
上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における位置の予測の正誤を判断する予測正誤チェック工程と、
上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正する予測修正工程と、
上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新工程と
を備える位置推定プログラムを記録しているプログラム記録媒体。
【請求項１】
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測部と、
上記予測部で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム追跡して位置を観測する観測部と、
上記観測部における環境内の特徴点の追跡結果に基づいて上記予測部における位置の予測の正誤を判断する予測正誤チェック部と、
上記予測正誤チェック部により上記位置の予測が正しくないと判断すると予測を修正する予測修正部と、
上記予測正誤チェック部により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新部と
を備えることを特徴とする位置推定装置。
【請求項２】
上記観測部は、上記予測部で求めた予測位置及び／又は姿勢を用いて探索範囲を決め、上記特徴点の追跡を行うことを特徴とする請求項１記載の位置推定装置。
【請求項３】
上記観測部は、事前に判っている三次元空間上の特徴点の位置とカルマンフィルタで推定した移動体の位置データから画像内の特徴点の位置を推定し、特徴点付近の輝度画像データと特徴点付近の輝度画像の姿勢データから、移動体の位置からみた特徴点付近のテンプレートをアフィン拘束にしたがって変形し、上記探索範囲と上記テンプレートを基にテンプレートマッチングを行って上記特徴点の追跡を行うことを特徴とする請求項２記載の位置推定装置。
【請求項４】
上記予測正誤チェック部は、上記観測部における特徴点追跡で成功した特徴点の数と失敗した特徴点の数をカウントし、一定の確率以上の成功率で特徴点追跡が出来ている場合を予測が正しいと判断し、それ以外を予測は正しくないと判断することを特徴とする請求項１記載の位置推定装置。
【請求項５】
上記更新部は、更新した移動体の位置推定結果を上記前フレームの移動体の位置推定結果として次のフレームにおける上記予測部での予測時に使わせることを請求項１記載の位置推定装置。
【請求項６】
上記予測修正部は、上記予測正誤チェック部にて予測が正しくないと判断されると、移動体の存在確率分布モデルの標準偏差の値が大きくなるよう変更し、上記観測部における上記探索範囲を広げることを特徴とする請求項１記載の位置推定装置。
【請求項７】
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測工程と、
上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレーム追跡して位置を観測する観測工程と、
上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における位置の予測の正誤を判断する予測正誤チェック工程と、
上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正する予測修正工程と、
上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新工程と
を備えることを特徴とする位置推定方法。
【請求項８】
移動体の位置を予測する位置推定装置にて実行される位置推定プログラムを記録しているプログラム記録媒体において、
現フレームにおけるオドメトリセンサのセンサ値と、前フレームの移動体の位置推定結果とを予測フィルタによりフィルタリングし、センサが搭載された移動体の位置を予測する予測工程と、
上記予測工程で使用したオドメトリセンサとは別の観測センサにより環境を計測し、環境内の特徴点を毎フレームに追跡して位置を観測する観測工程と、
上記観測工程における環境内の特徴点の追跡結果に基づいて上記予測工程における位置の予測の正誤を判断する予測正誤チェック工程と、
上記予測正誤チェック工程により上記位置の予測が正しくないと判断すると予測を修正する予測修正工程と、
上記予測正誤チェック工程により上記位置の予測が正しいと判断すると移動体の位置及び／又は姿勢を更新する更新工程と
を備える位置推定プログラムを記録しているプログラム記録媒体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００７−３０３８８６（Ｐ２００７−３０３８８６Ａ）
【公開日】平成１９年１１月２２日（２００７．１１．２２）
【国際特許分類】
【出願番号】特願２００６−１３０６０２（Ｐ２００６−１３０６０２）
【出願日】平成１８年５月９日（２００６．５．９）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成１９年１１月２２日（２００７．１１．２２）
【国際特許分類】
【出願日】平成１８年５月９日（２００６．５．９）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]