JP2012168788A - 運動推定装置及びプログラム - Google Patents

Abstract

【課題】全ての運動成分を計測するセンサを用いずに、移動体の運動を精度よく推定することができるようにする。
【解決手段】対応点検索部２４によって、第１の画像及び第２の画像の各々から対応点を検索する。運動情報取得部２６によって、第１の画像及び第２の画像間の自車両の運動を取得する。３次元位置算出部２８によって、第１の画像上における複数の特徴点の３次元位置を取得する。対応点予測部３０によって、取得された自車両の運動に基づいて、３次元位置が取得された各特徴点の、第２の画像上の位置を予測する。ピッチ角変動推定部３２によって、検索された各対応点における第２の画像上の位置と、予測された第２の画像上の各特徴点の位置との差分に基づいて、ピッチ角変動を推定し、信頼度算出部３４によって、各対応点の信頼度を算出する。運動推定部３６によって、算出された各対応点の信頼度に基づいて、自車両の運動を推定する。
【選択図】図１

Description

本発明は、運動推定装置及びプログラムに係り、特に、移動体の運動を推定する運動推定装置及びプログラムに関する。

従来より、屋外に存在する物体の三次元形状を精度良く特定する画像処理装置が知られている（特許文献１）。この画像処理装置では、複数の地点で撮影された画像間で同一部分の点を特定する。正しく同一部分の点を検出するために、センシング手段により画像間の位置・姿勢情報を取得し、得られた位置・姿勢情報に基づいたエピポーラ拘束条件を利用している。つまり、得られた位置・姿勢情報により決定するエピポーラ線上で同一点の探索を行っている。

また、画像間での特徴点の誤対応を抑制し、自車運動および３次元構造を算出する車両用画像処理装置が知られている（特許文献２）。この車両用画像処理装置では、画像間での特徴点の誤対応を抑えるため、センサ情報から運動を推定し、推定した運動に基づいて１枚目の画像で検出した各特徴点に対して２枚目の画像での位置を予測し、その位置を中心とした領域に探索領域を限定して、特徴点に対応する２枚目の画像での位置を検出している。

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

上記の特許文献１に記載の技術では、画像を用いずに画像間の位置・姿勢情報を検出可能なセンサを利用し、検出される位置・姿勢情報から決定されるエピポーラ拘束条件を満たす対応点のみを探索することで、点の誤対応を防いでいる。しかし、これを実現するためには、回転と平行移動の６自由度の成分をすべて精度良く計測できるセンサが必要であり、このようなセンサは一般的な車両では通常搭載されていない、という問題がある。

また、上記の特許文献２に記載の技術では、センサ情報または過去の自車運動情報から得られる運動情報に基づいて特徴点の探索領域を限定することで、点の誤対応を抑制している。車両速度、加速度、及び回転角などの車両運動を計測するセンサが必要となるが、一般的にはすべての運動成分を計測できるセンサは搭載されていない。そのようなセンサがない場合には、一般的な車載センサでの計測が困難であって、かつ、過去の自車運動情報からも推定できない、急激なピッチ変動のような運動が発生すると、正しく探索領域を設定することができず、正しい特徴点の対応を求めることができない、という問題がある。また、そのような急激なピッチ変動に対応するために探索領域を広く設定した場合には、誤対応の抑制効果が低下してしまう。

本発明は、上記の問題点を解決するためになされたもので、全ての運動成分を計測するセンサを用いずに、移動体の運動を精度よく推定することができる運動推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る運動推定装置は、移動体の外部を撮像した第１の画像及び第２の画像の各々から、前記第１の画像及び第２の画像間で対応した特徴点を検索する検索手段と、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の位置及び姿勢の変化を表わす運動を取得する運動取得手段と、前記第１の画像上における複数の特徴点の３次元位置を取得する３次元位置取得手段と、前記運動取得手段によって取得された前記移動体の運動に基づいて、前記３次元位置取得手段によって３次元位置が取得された前記複数の特徴点の、前記第２の画像上の位置を予測する位置予測手段と、前記検索手段によって検索された前記対応した特徴点の複数の組における前記第２の画像上の複数の特徴点の位置と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置との差分に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の姿勢の上下方向の変化を表わす上下回転運動を推定する上下回転運動推定手段と、前記検索手段によって検索された前記対応した特徴点の複数の組と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置と、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動とに基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を推定する運動推定手段とを含んで構成されている。

本発明に係るプログラムは、コンピュータを、移動体の外部を撮像した第１の画像及び第２の画像の各々から、前記第１の画像及び第２の画像間で対応した特徴点を検索する検索手段、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の位置及び姿勢の変化を表わす運動を取得する運動取得手段、前記第１の画像上における複数の特徴点の３次元位置を取得する３次元位置取得手段と、前記運動取得手段によって取得された前記移動体の運動に基づいて、前記３次元位置取得手段によって３次元位置が取得された前記複数の特徴点の、前記第２の画像上の位置を予測する位置予測手段、前記検索手段によって検索された前記対応した特徴点の複数の組における前記第２の画像上の複数の特徴点の位置と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置との差分に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の姿勢の上下方向の変化を表わす上下回転運動を推定する上下回転運動推定手段、及び前記検索手段によって検索された前記対応した特徴点の複数の組と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置と、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動とに基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を推定する運動推定手段として機能させるためのプログラムである。

本発明によれば、検索手段によって、移動体の外部を撮像した第１の画像及び第２の画像の各々から、前記第１の画像及び第２の画像間で対応した特徴点を検索する。運動取得手段によって、第１の画像及び第２の画像の各々を撮像したときの移動体の位置及び姿勢の変化を表わす運動を取得する。

そして、３次元位置取得手段によって、第１の画像上における複数の特徴点の３次元位置を取得する。位置予測手段によって、運動取得手段によって取得された移動体の運動に基づいて、３次元位置取得手段によって３次元位置が取得された複数の特徴点の、第２の画像上の位置を予測する。

そして、上下回転運動推定手段によって、検索手段によって検索された対応した特徴点の複数の組における第２の画像上の複数の特徴点の位置と、位置予測手段によって予測された第２の画像上の複数の特徴点の位置との差分に基づいて、第１の画像及び第２の画像の各々を撮像したときの移動体の姿勢の上下方向の変化を表わす上下回転運動を推定する。運動推定手段によって、検索手段によって検索された対応した特徴点の複数の組と、位置予測手段によって予測された第２の画像上の複数の特徴点の位置と、上下回転運動推定手段によって推定された移動体の上下回転運動とに基づいて、第１の画像及び第２の画像の各々を撮像したときの移動体の運動を推定する。

このように、取得された移動体の運動と特徴点の３次元位置とに基づいて予測された第２の画像上の特徴点の位置との差分に基づいて、移動体の上下回転運動を推定して、移動体の運動を推定することにより、全ての運動成分を計測するセンサを用いずに、移動体の運動を精度よく推定することができる。

本発明に係る運動推定装置は、検索手段によって検索された対応した特徴点の複数の組と、位置予測手段によって予測された第２の画像上の複数の特徴点の位置と、上下回転運動推定手段によって推定された移動体の上下回転運動とに基づいて、検索手段によって検索された対応した特徴点の複数の組の各々について、静止物を表わす領域上の特徴点の対応であって、かつ、特徴点の対応の正しさを示す信頼度を算出する信頼度算出手段を更に含み、運動推定手段は、検索手段によって検索された対応した特徴点の複数の組と、対応した特徴点の複数の組について算出された信頼度とに基づいて、移動体の運動を推定するようにすることができる。

本発明に係る運動推定装置は、検索手段によって検索された対応した特徴点の複数の組における第２の画像上の複数の特徴点の位置と、位置予測手段によって予測された第２の画像上の複数の特徴点の位置を、上下回転運動推定手段によって推定された移動体の上下回転運動の分だけ変更した位置とに基づいて、検索手段によって検索された対応した特徴点の全ての組のうち、対応した特徴点の複数の組を選択する選択手段を更に含み、運動推定手段は、選択手段によって選択された対応した特徴点の複数の組に基づいて、移動体の運動を推定するようにすることができる。

上記の運動推定装置は、移動体の運動を計測する運動計測手段を更に含み、運動取得手段は、運動計測手段によって計測された移動体の運動に基づいて、第１の画像及び第２の画像の各々を撮像したときの移動体の運動を取得するようにすることができる。

上記の運動取得手段は、運動推定手段によって過去に推定された移動体の運動に基づいて、第１の画像及び第２の画像の各々を撮像したときの移動体の運動を推定するようにすることができる。

また、上記の３次元位置取得手段は、運動推定手段によって推定された、第１の画像より前に撮像された前画像及び第１の画像の各々を撮像したときの移動体の運動と、検索手段によって検索された前画像及び第１の画像で対応した特徴点の各組とに基づいて、第１の画像上における各特徴点の３次元位置を算出するようにすることができる。

上記の運動推定装置は、第１の画像上における移動体が走行する道路領域を特定する道路領域特定手段を更に含み、３次元位置取得手段は、予め求められた道路と移動体の外部を撮像する撮像手段との位置関係に基づいて、道路領域特定手段によって特定された第１の画像上における道路領域内の各特徴点の３次元位置を算出するようにすることができる。

以上説明したように、本発明の運動推定装置及びプログラムによれば、取得された移動体の運動と特徴点の３次元位置とに基づいて予測された第２の画像上の特徴点の位置との差分に基づいて、移動体の上下回転運動を推定して、移動体の運動を推定することにより、全ての運動成分を計測するセンサを用いずに、移動体の運動を精度よく推定することができる、という効果が得られる。

本発明の第１の実施の形態に係る運動推定装置を示すブロック図である。 （Ａ）対応点の第１の画像上の位置を示す図、及び（Ｂ）対応点の第２の画像上の位置と予測された特徴点の位置とを示す図である。 差分フローを説明するための図である。 抽出された特徴点と移動体の運動との関係を説明するための図である。 本発明の第１の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る運動推定装置におけるピッチ角変動推定処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る運動推定装置における推定処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る運動推定装置を示すブロック図である。 本発明の第２の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態に係る運動推定装置を示すブロック図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係る運動推定装置１０は、自車両の前方の画像を撮像する単眼のカメラで構成される撮像装置１２と、自車両の運動として、車速を計測する車速センサ１４と、自車両の運動として、ヨー角速度を計測するヨー角速度センサ１６と、撮像装置１２によって撮像された画像、車速センサ１４によって計測された車速、及びヨー角速度センサ１６によって計測されたヨー角速度に基づいて、自車両の運動を推定して外部装置（図示省略）に出力するコンピュータ１８とを備えている。なお、車速センサ１４及びヨー角速度センサ１６が、運動計測手段の一例である。

本実施の形態では、撮像装置１２の単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。

ヨー角速度センサ１６は、例えば、ジャイロセンサを用いて構成されている。

コンピュータ１８では、自車両の運動として、自車両の３軸角速度と並進方向を示す成分とを推定する。コンピュータ１８は、ＣＰＵと、ＲＡＭと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１８は、撮像装置１２によって撮像された複数の画像を取得する画像取得部２０と、画像取得部２０により取得した複数の画像から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部２２と、特徴点抽出部２２により得られた２つの画像の各々における特徴点から、当該２つの画像間で対応する特徴点（以下、対応点とも称する）を検索する対応点検索部２４と、を備えている。

特徴点抽出部２２は、撮像装置１２から得られる異なる時刻に撮像した２枚の画像から、それぞれ特徴点を抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＴｏｍａｓｉ−Ｋａｎａｄｅの方法や、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。特徴点の数としては、全画像に対して３０〜５００点程度を想定すると良いが、状況に応じて、固定であってもよいし、可変であってもよい。適切な抽出方法であればその手法は上記の限りではない。

Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（１）式によって、行列Ｍを計算する。

ただし、Ｉ_ｕ，Ｉ_ｖはそれぞれ水平方向、垂直方向の微分、Ｇ_σは標準偏差σのガウス分布による平滑化を表す。

そして、上記（１）式で計算された行列Ｍの固有値λ_１、λ_２を用いて、以下の（２）式によりコーナー強度を計算する。

ただし、ｋは予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。Ｈａｒｒｉｓオペレータを用いる方法では、このコーナー強度がしきい値以上でかつ極大となる点を選択し、選択された点を特徴点として抽出する。

対応点検索部２４は、特徴点抽出部２２において２つの画像の各々から抽出された特徴点について、２つの画像間での対応付けを行って、２つの画像間の対応点を検索する。画像間での特徴点の対応付けは、特徴点周辺の画像パターンの相関値が最大となり、かつ相関値がしきい値以上となるペアを求めることにより行われる。例えばＳＳＤ（Ｓｕｍ ｏｆ ｓｑｕａｒｅｄ ｄｉｆｆｅｒｅｎｃｅｓ）による相関を用いる場合、第１の画像の特徴点ｆと第２の画像の特徴点ｇの相関値は以下の（３）式により計算される値が小さいほど大きな値となるように設定される。

ただし、特徴点の画像座標はｆ＝（ｆ_ｘ，ｆ_ｙ），ｇ＝（ｇ_ｘ，ｇ_ｙ）であり、Ｉ（ｘ，ｙ），Ｊ（ｘ，ｙ）はそれぞれ第１の画像、第２の画像の座標（ｘ，ｙ）における輝度を表す。また、ｗは相関を計算する周辺領域の大きさを決定する変数である。相関値の計算では、輝度でなく輝度勾配を用いる方法や、勾配方向のヒストグラムなどの特微量による相関を用いてもよい。

また、コンピュータ１８は、車速センサ１４及びヨー角速度センサ１６の各々からの出力に基づいて、自車両の運動を取得する運動情報取得部２６と、後述する運動推定部３６による前回の推定値に基づいて、１つの画像上の特徴点の３次元位置を算出する３次元位置算出部２８と、特徴点抽出部２２によって抽出された特徴点、運動情報取得部２６によって算出された自車両の運動、及び３次元位置算出部２８によって算出された特徴点の３次元位置に基づいて、１つの画像の特徴点に対応する他の画像における特徴点の位置を予測する対応点予測部３０とを更に備えている。なお、３次元位置算出部２８が、３次元位置取得手段の一例であり、対応点予測部３０が、位置予測手段の一例である。

運動情報取得部２６は、車速センサ１４及びヨー角速度センサ１６の各々からの出力に基づいて、第１の画像及び第２の画像を撮像した間における、３次元空間での３自由度の回転成分（ピッチ角、ヨー角、ロール角）と３自由度の平行移動成分（左右方向、上下方向、前後方向）を表わす自車両の運動を算出する。なお、自車両の運動の各成分は、第１の画像を撮影したときのカメラのカメラ座標系で表されるものとする。また、搭載されているセンサで計測できない成分は０とする。本実施の形態では、車速センサ１４及びヨー角速度センサ１６を搭載しているため、ピッチ角とロール角の回転成分、上下方向の平行移動は計測されず、それらの運動成分は０として運動情報が出力される。

３次元位置算出部２８は、後述する運動推定部３６によって前の時刻に推定された自車両の運動（現時点の第１の画像とその１つ前に撮像された画像（前の時刻における第１の画像と第２の画像）との間の運動）と、対応点検索部２４によって前の時刻に検索された特徴点の対応（現時点の第１の画像とその１つ前に撮像された画像との間における特徴点の対応）とに基づいて、三角測量の原理に従って、第１の画像上での特徴点の３次元位置を算出する。

対応点予測部３０は、運動情報取得部２６から出力される第１の画像及び第２の画像間の自車両の運動と、３次元位置算出部２８から出力される第１の画像上の特徴点の３次元位置とを用いて、特徴点抽出部２２によって抽出された第１の画像の各特徴点が、第２の画像上のどの位置に現れるかを予測する。第１の画像を基準としたときの第２の画像における相対的な位置・姿勢が回転行列Ｒと平行移動ベクトルｔで表され、第１の画像の特徴点ｆの３次元位置が（Ｘ，Ｙ，Ｚ）である場合、第２の画像で対応する点の座標（＾ｘ’，＾ｙ’）は、以下の（４）式に従って算出される。

なお、Ｋは撮像装置のキャリブレーション行列であり、以下の式で表される。

ただし、ｆ_ｘ，ｆ_ｙは撮像装置のｘ方向、ｙ方向の焦点距離、（ｃ_ｘ，ｃ_ｙ）は撮像装置の画像中心の画像座標を表す。

また、コンピュータ１８は、対応点検索部２４によって検索された対応点、及び対応点予測部３０によって予測された第２の画像上の特徴点の位置に基づいて、第１の画像と第２の画像の間で発生したピッチ角変動量を推定するピッチ角変動推定部３２と、対応点検索部２４によって検索された対応点、ピッチ角変動推定部３２によって推定されたピッチ角変動量、及び対応点予測部３０によって予測された第２の画像上の特徴点の位置に基づいて、各対応点について信頼度を算出する信頼度算出部３４と、対応点検索部２４によって検索された各対応点、及び信頼度算出部３４によって算出された各対応点の信頼度に基づいて、自車両の運動を推定する運動推定部３６とを更に備えている。なお、ピッチ角変動推定部３２が、上下回転運動推定手段の一例であり、ピッチ角変動量が、上下回転運動の一例である。

ピッチ角変動推定部３２は、以下に説明するように、ピッチ角変動量を推定する。

まず、対応点検索部２４から出力される各対応点の第１の画像での画像座標と第２の画像での画像座標、および対応点予測部３０から出力される各特徴点の第２の画像での予測画像座標を入力として、各特徴点に対して差分フローｄ_iを計算する。図２（Ａ）、（Ｂ）に示すように、ｉ番目の対応点の第１の画像での画像座標が（ｘ_i，ｙ_i）、ｉ番目の対応点の第２の画像での画像座標が（ｘ’_i，ｙ’_i）、対応点予測部３０から得られた第２の画像での予測画像座標が（＾ｘ’_i，＾ｙ’_i）であるとき、差分フローｄ_i＝（ｄ_ix，ｄ_iy）は、以下の（５）式に従って計算される。

図３で示す実線のベクトルが差分フローｄ_i＝（ｄ_ix，ｄ_iy）であり、破線で示す２つのベクトルが、検索された対応点に基づくフローを示すベクトル（ｘ'_i−ｘ_i，ｙ'_i−ｙ_i）と、予測された特徴点に基づくフローを示すベクトル（＾ｘ'_i−ｘ_i，＾ｙ'_i−ｙ_i）である。このように、差分フローは、検索された対応点に基づくフローと、予測された特徴点に基づくフローとの差分の動きベクトルを示している。

差分フローｄ_iは、上記図３に示すように、対応点検索部２４で検出された対応点から得られるオプティカルフローと、対応点予測部３０で予測された第２の画像上の特徴点の位置から得られるオプティカルフローとの差分を表しており、運動情報取得部２６から出力された運動情報と実際の運動のずれを表している。画像間で大きなピッチ角変動があり、それが運動情報取得部２６からの運動情報に含まれない場合には、この差分フローは主にピッチ角変動により発生したフローと考えることができる。

そこで、本実施の形態では、ピッチ角変動推定部３２によって、各特徴点の差分フローに基づいて、運動情報取得部２６からの運動情報に含まれていないピッチ角変動成分を推定する。

信頼度算出部３４は、対応点検索部２４から出力されるｉ番目の対応点（ｉ＝１，・・・，ｎ）について、以下の（６）式に従って、静止物を表わす領域上の特徴点の対応であって、かつ、特徴点の対応の正しさを示す信頼度ｃ_ｉを算出する。

ただし、ｉ番目の対応点の第１の画像での画像座標が（ｘ_ｉ，ｙ_ｉ）、ｉ番目の対応点の第２の画像での画像座標が（ｘ_ｉ’，ｙ_ｉ’）、対応点予測部３０から得られたｉ番目の特徴点の第２の画像での画像座標が（＾ｘ_ｉ’，＾ｙ_ｉ’）である。また、ｗは、ピッチ角変動推定部３２で推定されたピッチ角変動量であり、ｋは予め設定される定数である。

運動推定部３６は、信頼度算出部３４によって算出された各対応点の信頼度を用いて、第１の画像と第２の画像の間の位置及び姿勢の変化を、第１の画像と第２の画像を撮像したときの自車両の運動として推定する。

２枚の画像で対応する特徴点の座標から、２つの画像が撮像されたときの位置及び姿勢の変化を計算するとき、計算する位置及び姿勢の変化は、図４に示すような第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔから構成される６自由度の運動である。

ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標Ｉ_ｉと第２の画像におけるｎ点の対応点の画像座標Ｉ_ｉ’とについて（ｎ≧８）、対応点の対応関係が正しくて誤差がなければ、位置及び姿勢の変化を示す行列として、以下の（７）式を満たす３×３行列Ｆが存在する。

ただし、Ｉ_ｉ＝(ｕ_ｉ，ｖ_ｉ，１)^Ｔ、Ｉ_ｉ’＝(ｕ_ｉ’，ｖ_ｉ’，１)^Ｔであり、第１の画像での画像座標（ｕ_ｉ，ｖ_ｉ）の点に対応する第２の画像での点の画像座標が（ｕ_ｉ’，ｖ_ｉ’）である。

ここで、上記（７）式を満たす行列Ｆは、定数倍の不定性を持っている。すなわち、Ｆ_ｓが上記（７）式を満たす場合には、αＦ_ｓも上記（７）式を満たす（ただし、αは実数）。よって、行列Ｆを以下の（８）式のように表すことができる。

また、上記（７）式、（８）式より、以下の（９）式が得られる。

ここで、８組以上の対応点Ｉ_ｉ、Ｉ_ｉ’があれば、上記（９）式が少なくとも８つ得られるため、８つの変数ｆ_１１〜ｆ_３２を求めることができる。

また、実画像で特徴点の対応を求めた場合には、対応する画像座標には誤差が含まれ、特徴点の中には誤った対応関係を持つ点も存在することがあるため、以下の（１０）式で計算される評価値Ｚ_ｒが最大となるような行列Ｆを求める。

ただし、Ｉ_iは、ｉ番目の対応点の第１の画像での画像座標を表わす同次座標ベクトルであり、Ｉ_i’は、ｉ番目の対応点の第２の画像での画像座標を表わす同次座標ベクトルである。また、ｃ_iはｉ番目の対応点の信頼度、δ（ｘ）はｘが真なら１、そうでないなら０をとる関数、ｄ（ａ、ｂ）はａとｂとの距離を表し、θはあらかじめ設定するしきい値である。ｎは、対応点検索部２４で検索された対応点の数である。

また、撮像装置１２のキャリブレーション行列Ｋが既知である場合には、上述したように特定された行列Ｆに基づいて、以下の（１１）式、（１２）式に従って、回転行列Ｒと並進ベクトルｔとを計算し、第１の画像及び第２の画像の各々が撮像されたときの撮像装置１２の運動（位置及び姿勢の変化）として出力する。

上記の方法により、運動推定部３６は、第１の画像及び第２の画像における複数組の対応する特徴点の画像座標と、各対応点の信頼度とに基づいて、第１の画像及び第２の画像の間における自車両の運動を計算する。

次に、第１の実施の形態に係る運動推定装置１０の作用について説明する。なお、運動推定装置１０を搭載した自車両が走行しているときに、自車両の運動を推定する場合を例に説明する。

撮像装置１２によって所定時間間隔で自車両前方を撮像し、撮像装置１２によって撮像される度に、運動推定装置１０において、図５に示す運動推定処理ルーチンが繰り返し実行される。ステップ１００において、異なるタイミングで撮像装置１２によって自車両前方を撮像した第１の画像（例えば最新の画像より１つ前に撮像された画像）及び第２の画像（例えば最新の画像）を取得すると共に、ステップ１０２において、第１の画像及び第２の画像を撮像する間に車速センサ１４によって計測された車速及びヨー角速度センサ１６によって計測されたヨー角速度を取得し、第１の画像及び第２の画像を撮像する間の自車両の運動を算出する。

そして、ステップ１０４において、第１の画像及び第２の画像から、それぞれ所定の数だけ特徴点を抽出する。そして、ステップ１０６において、上記ステップ１０４で抽出された第１の画像における特徴点の各々を、第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検索する。

次のステップ１０８では、前回実行された運動推定処理ルーチンにおいて推定された自車両の運動と、検索された２つの画像（今回の第１の画像とその１つ前に撮像された画像）で対応する特徴点とを取得する。そして、ステップ１１０において、上記ステップ１０８で取得した自車両の運動と特徴点の対応とに基づいて、第１の画像における各特徴点の３次元位置を算出する。

ステップ１１２では、上記ステップ１０２で算出された自車両の運動と、上記ステップ１１０で算出された第１の画像における各特徴点の３次元位置とに基づいて、当該第１の画像の各特徴点に対応する第２の画像上の特徴点の位置を予測する。

そして、ステップ１１４において、上記ステップ１０６で検索された各対応点と、上記ステップ１１２で予測された第２の画像上の各特徴点の位置とに基づいて、第１の画像及び第２の画像を撮像する間の自車両のピッチ角変動量を推定する。

次のステップ１１６では、上記ステップ１０６で検索された各対応点と、上記ステップ１１２で予測された第２の画像上の各特徴点の位置と、上記ステップ１１４で推定された自車両のピッチ角変動量とに基づいて、各対応点の信頼度を算出する。

そして、ステップ１１８において、上記ステップ１０６で検索された各対応点と、上記ステップ１１６で算出された各対応点の信頼度とに基づいて、２つの撮像時刻間における撮像装置１２の位置姿勢の変化、即ち、撮像装置１２を搭載した自車両のその２時刻間における運動（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を推定し、外部装置に出力して、運動推定処理ルーチンを終了する。

上記の運動推定処理ルーチンが繰り返し実行され、撮像装置１２によって繰り返し撮像された画像間の各々の自車両の運動が推定される。

上記ステップ１１４は、図６に示すピッチ角変動推定処理ルーチンによって実現される。

まず、ステップ１２０において、各対応点の差分フローｄ_iとして、上記ステップ１０６で検索された対応点の第２の画像上の特徴点の位置と、上記ステップ１１２で予測された、第２の画像上の対応する特徴点の位置との差分を算出する。

そして、ステップ１２２において、推定ピッチ角変動ｐに初期値０を設定すると共に、最小ずれ量ｅ_minに初期値∞を設定する。ステップ１２４では、繰り返し回数を示す変数ｒに初期値１を設定し、ステップ１２６では、選択回数を示す変数ｓに初期値１を設定する。

そして、ステップ１２８において、上記ステップ１０６で検索された各対応点から、ランダムに１つの対応点を選択する。ステップ１３０では、選択した対応点について上記ステップ１２０で算出した差分フローｄ_iに基づいて、ピッチ角変動量を算出する。ここでは、差分フローｄ_iのｘ軸成分ｄ_iｘとｙ軸成分ｄ_iｙから、それぞれピッチ角変動量ｗ_ｘ，ｗ_ｙを、以下の（１３）式を解くことにより求める。

ただし、（ｘ_i，ｙ_i）は選択された対応点の第１の画像での画像座標、ｆ_ｘ，ｆ_ｙはｘ方向、ｙ方向の焦点距離を表す。

そして、ステップ１３２において、上記ステップ１３０で算出したｗ_ｘ、ｗ_ｙに基づくｗ_ｘ／ｗ_ｙが、許容範囲内かどうか判定する。なお、許容範囲は、１を中心とする、例えば０．９〜１．１などのあらかじめ設定された範囲である。差分フローがピッチ角変動によるフローであれば、計算されたｗ_ｘ，ｗ_ｙは同じになることから、ｗ_ｘ／ｗ_ｙが、許容範囲内であれば、ステップ１３８へ進む。

一方、ｗ_ｘ／ｗ_ｙが許容範囲外であれば、対応点の選択をやり直すために、ステプ１３４へ移行し、選択回数ｓを１増やす。ステップ１３６では、選択回数ｓが最大選択回数Ｓ以下であるか否かを判定する。選択回数ｓがＳ以下であれば、上記ステップ１２８へ戻る。一方、選択回数ｓがＳより大きければ、大きなピッチ角変動が起こっておらず、差分フローは、ピッチ角の変動の影響が小さいと判断し、ステップ１５４へ移行する。

ステップ１３８では、ピッチ角変動量ｗをｗ_ｘ，ｗ_ｙの平均に設定し、ステップ１４０において、全対応点に対して、ピッチ角変動量から計算されるフローを算出する。ここでは、ピッチ角がｗであるとき、第１の画像での画像座標が（ｘ_i，ｙ_i）のｉ番目の対応点のピッチ角の変動から計算されるフローＶ_i＝（ｖ_ix，ｖ_iy）は、以下の（１４）式により算出される。

そして、ステップ１４２において、各対応点について、上記ステップ１４０で算出したフローと、上記ステップ１２０で算出した差分フローとのずれを以下の（１５）式に従って算出する。

次のステップ１４４では、上記ステップ１４２で算出された各対応点に対するずれの降順又は昇順に、対応点を並べて、ちょうど真ん中に並べられた対応点のずれを、ずれの中央値ｅとして求める。そして、ステップ１４６において、ずれの中央値ｅが、最小ずれ量ｅ_minより小さいか否かを判定する。ずれの中央値ｅが、最小ずれ量ｅ_min以上であれば、ステップ１５０へ移行する。一方、ずれの中央値ｅが、最小ずれ量ｅ_min未満であれば、ステップ１４８において、推定ピッチ角変動ｐをｗに更新すると共に、最小ずれ量ｅ_minをeに更新する。

ステップ１５０では、繰り返し回数ｒを１増やして、ステップ１５２において、繰り返し回数ｒが、最大繰り返し数Ｒ以下であるか否かを判定する。繰り返し回数ｒがＲ以下であれば、上記ステップ１２６へ戻るが、繰り返し回数ｒがＲより大きければ、ステップ１５４において、推定ピッチ角変動ｐを、ピッチ角変動量の推定値として出力する。

また、上記ステップ１１８は、図７に示す推定処理ルーチンによって実現される。

まず、ステップ１６０において、繰り返し回数を示す変数ｒを初期値１に設定し、ステップ１６２で、上記ステップ１０６で検索された各対応点から、ランダムに８点の対応点を選択する。

そして、ステップ１６４において、上記ステップ１６２で選択された８つの対応点の組み合わせに基づいて、行列Ｆの要素ｆ_１１〜ｆ_３２を算出して、行列Ｆ_ｒを生成する。次のステップ１６６では、上記ステップ１０６で検索された各対応点と、上記ステップ１６４で生成された行列Ｆ_ｒとに基づいて、上記（１０）式に従って評価値Ｚ_ｒを算出する。ステップ１６８では、繰り返し回数ｒを１増やし、ステップ１７０において、繰り返し回数ｒが、最大繰り返し回数Ｔ以下であるか否かを判定する。繰り返し回数ｒがＴ以下である場合には、上記ステップ１６２へ戻るが、一方、繰り返し回数ｒがＴより大きい場合には、ステップ１７２へ進む。

ステップ１７２では、上記ステップ１６６で算出された評価値Ｚ_ｒが最大となる行列Ｆｒを特定する。そして、ステップ１７４において、上記ステップ１７２で特定された行列Ｆｒに基づいて、上記（１１）式及び（１２）式に従って、第１の画像及び第２の画像を撮像した時刻間における自車両の運動の推定値を算出して、推定処理ルーチンを終了する。

以上説明したように、第１の実施の形態に係る運動推定装置によれば、カルマンフィルタを用いて推定された自車両の運動と特徴点の３次元位置とに基づいて予測された第２の画像上の特徴点の位置と、検索された対応点の第２画像上の位置との差分に基づいて、自車両のピッチ角変動を推定して、対応点の信頼度を算出し、対応点の信頼度を用いて自車両の運動を推定することにより、全ての運動成分を計測するセンサを用いずに、自車両の運動を精度よく推定することができる。

また、走行中に急激なピッチ変動が発生した場合においても、他の車両など周囲の移動物体の影響を受けずに、正しい静止物上の特徴点の対応に基づいて、高精度に自車両の運動を推定することができる。

複数枚の画像を用いて画像の撮影時刻間に発生した運動を推定するとき、画像からコーナーなどの特徴的な点を抽出し、それらの点を画像間で対応付け、その点の対応関係から画像間の位置及び姿勢の変化を求める。このとき、実画像で対応点を検索する場合には誤対応した対応点が含まれていることが避けられない上、他の車両などの移動物体上の対応点も存在するため、それらの誤対応した対応点や移動物体上の対応点を用いて運動（位置・姿勢の変化）を推定すると推定誤差が大きくなる可能性がある。

車載したカメラにより異なる時刻に撮影した画像間に発生する画像上の各点の動きは、各点の３次元位置および車両の運動により決定されるため、各点のおおよその３次元位置およびおおよその車両運動が既知であれば、画像間で対応する点の位置を予測することができる。各点の３次元位置、つまり周囲の環境の３次元構造情報については、前の時刻の自車両の運動と特徴点の対応とから算出可能である。また、自車両の運動情報は、車速センサやヨー角速度センサなどの車載センサから取得できる。従って、これらの情報が得られると、静止物体に関して画像間で発生する点の動きを予測できるため、その予測した動きと異なる動きを示す対応点は、誤対応または移動物体上の対応点である可能性が高いと判定することが可能であり、運動推定の精度劣化を防ぐことができる。

しかし、車両の走行中には、道路上の微小な凹凸などにより急激な上下方向の回転運動、すなわち、ピッチ角変動が発生することがあり、このピッチ角変動は通常の車載センサで計測することができない。また、急激に発生する変動であるため、前の時刻までに推定した運動から推定することも困難である。このピッチ角変動は画像上の点の動きに与える影響が大きいため、ピッチ角変動の回転成分が得られていない運動情報から画像間での点の動きを予測すると、実際の点の動きとは大きく異なる。従って、対応点の誤対応や移動物体上の対応点を正しく判定することができない。

そこで、本実施の形態では、ピッチ角変動なしで予測した点の動きと、検索された対応点の動きとの差分から、ピッチ角変動の回転成分のみを推定した上で、対応点の信頼度を算出する。これにより、急激なピッチ変動が発生した場合においても、誤対応した対応点であるか、及び移動物体上の対応点であるかを正しく判定して、自車両の運動を高精度に推定することができる。

次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。

第２の実施の形態では、前の時刻までに推定された車両運動に基づいて、現時点の運動情報を取得している点と、撮像装置と道路との位置関係に基づいて、特徴点の３次元位置を算出している点とが、第１の実施の形態と異なっている。

図８に示すように、第２の実施の形態に係る運動推定装置２１０は、撮像装置１２と、自車両の位置を計測するＧＰＳセンサ２１４と、撮像装置１２によって撮像された画像、及びＧＰＳセンサ２１４によって計測された自車両の位置に基づいて、自車両の運動を推定して外部装置（図示省略）に出力するコンピュータ２１８とを備えている。

コンピュータ２１８は、画像取得部２０と、特徴点抽出部２２と、対応点検索部２４と、地図情報記憶部２２２と、道路領域特定部２２４と、運動情報取得部２２６と、３次元位置算出部２２８と、対応点予測部３０と、ピッチ角変動推定部３２と、信頼度算出部３４と、運動推定部３６とを備えている。

地図情報記憶部２２２は、道路の各地点における車線数や道路幅などの道路情報が格納された地図情報を記憶している。道路領域特定部２２４は、ＧＰＳセンサ２１４によって計測された自車両の位置に応じて、自車両の現在地点における道路情報を取得し、取得した道路情報に基づいて、撮像装置１２によって撮像された画像上の道路領域を特定する。

３次元位置算出部２２８は、予め求められた撮像装置１２と自車両が走行する道路との３次元の位置関係に基づいて、道路領域特定部２２４によって特定された道路領域内の各特徴点の３次元位置を算出する。

運動情報取得部２２６は、運動推定部３６によって前の時刻までに推定された自車両の運動を取得し、例えばカルマンフィルタを用いて、現時点における自車両の運動（第１の画像及び第２の画像間の自車両の運動）を推定する。

対応点予測部３０は、特徴点抽出部２２によって抽出された特徴点、運動情報取得部２２６によって算出された自車両の運動、及び３次元位置算出部２２８によって算出された道路領域内の特徴点の３次元位置に基づいて、３次元位置が算出された第１の画像の特徴点に対応する、第２の画像における特徴点の位置を予測する。

第２の実施の形態に係る運動推定処理ルーチンについて、図９を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。

まず、ステップ１００において、撮像装置１２によって撮像された第１の画像及び第２の画像を取得すると共に、ステップ２５０において、前回までの運動推定処理ルーチンによって推定された自車両の運動を取得し、第１の画像及び第２の画像を撮像する間の自車両の運動を推定する。

そして、ステップ１０４において、第１の画像及び第２の画像から、それぞれ所定の数だけ特徴点を抽出し、ステップ１０６において、第１の画像における特徴点の各々について、第２の画像から、対応する各特徴点をそれぞれ検索する。

次のステップ２５２では、ＧＰＳセンサ２１４によって計測された自車両の位置を取得し、自車両の位置における道路情報を取得する。そして、ステップ２５４では、上記ステップ２５２で取得した道路情報に基づいて、第１の画像上の道路領域を特定する。ステップ２５６では、予め求められた撮像装置１２と道路との位置関係に基づいて、上記ステップ２５４で特定された道路領域内の特徴点の３次元位置を算出する。

ステップ１１２では、上記ステップ２５０で推定された自車両の運動と、上記ステップ２５６で算出された第１の画像における各特徴点の３次元位置とに基づいて、当該第１の画像の各特徴点に対応する第２の画像上の特徴点の位置を予測する。

そして、ステップ１１４において、第１の画像及び第２の画像を撮像する間の自車両のピッチ角変動量を推定し、次のステップ１１６では、各対応点の信頼度を算出する。そして、ステップ１１８において、自車両の運動を推定し、外部装置に出力して、運動推定処理ルーチンを終了する。

このように、各点の３次元位置、つまり周囲の環境の３次元構造情報については、車載したカメラに対する道路面の位置など、おおよその構造を予測可能である。また、自車両の運動情報は、前の時刻までの自車両の運動から推定することができる。

次に、第３の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。

第３の実施の形態では、各対応点に対する信頼度を算出せずに、車両運動を推定している点が、第１の実施の形態と主に異なっている。

図１０に示すように、第３の実施の形態に係る運動推定装置３１０のコンピュータ３１８は、画像取得部２０と、特徴点抽出部２２と、対応点検索部２４と、運動情報取得部２６と、３次元位置算出部２８と、対応点予測部３０と、ピッチ角変動推定部３２と、運動推定部３３６とを備えている。なお、運動推定部３３６が、選択手段及び運動推定手段の一例である。

運動推定部３３６は、対応点検索部２４によって検索された対応点、ピッチ角変動推定部３２によって推定されたピッチ角変動量、及び対応点予測部３０によって予測された第２の画像上の特徴点の位置に基づいて、以下に説明するように、自車両の運動を推定する。

まず、対応点検索部２４によって検索された対応点のうち、検索された対応点の第２の画像上での画像座標と、対応点予測部３０から得られた第２の画像上での予測画像座標にピッチ角変動推定部３２で得られたピッチ角変動分を加えた画像座標と、の距離がしきい値以下である対応点を選択する。

ここで、検索された対応点の第２の画像上での画像座標と、予測画像座標にピッチ変動分を加えた画像座標との距離は、以下の（１６）式で計算される。

そして、選択された対応点を用いて、第１の画像と第２の画像を撮像したときの自車両の運動を推定する。

このとき、評価値を最大にする行列Ｆ_ｒの特定に、対応点の信頼度は用いず、以下の（１７）式で計算される評価値Ｚ_ｒを用いる。

ただし、ｎは、選択された対応点の数である。

なお、第３の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。

このように、推定されたピッチ角変動を用いて、正しい静止物上の対応点を選択することにより、走行中に急激なピッチ変動が発生した場合においても、他の車両など周囲の移動物体の影響を受けずに、高精度に自車両の運動を推定することができる。

なお、上記の第１の実施の形態〜第２の実施の形態では、上記（６）式に従って、対応点に対する信頼度を算出する場合を例に説明したが、これに限定されるものではなく、上記（１６）式で算出される距離が大きいほど信頼度が低くなるような他の関数を用いて、対応点に対する信頼度を算出するようにしてもよい。

また、上記の第１の実施の形態及び第３の実施の形態では、車速センサ及びヨー角速度センサを用いた場合を例に説明したが、これに限定されるものではなく、他のセンサを用いて、ピッチ角成分以外の運動情報を計測するようにしてもよい。

本発明のプログラムは、記憶媒体に格納して提供するようにしてもよい。

１０、２１０、３１０ 運動推定装置
１２ 撮像装置
１４ 車速センサ
１６ ヨー角速度センサ
１８、２１８、３１８ コンピュータ
２２ 特徴点抽出部
２４ 対応点検索部
２６、２２６ 運動情報取得部
２８、２２８ ３次元位置算出部
３０ 対応点予測部
３２ ピッチ角変動推定部
３４ 信頼度算出部
３６、３３６ 運動推定部
２１４ ＧＰＳセンサ
２２２ 地図情報記憶部
２２４ 道路領域特定部

Claims (8)

1. 移動体の外部を撮像した第１の画像及び第２の画像の各々から、前記第１の画像及び第２の画像間で対応した特徴点を検索する検索手段と、
   前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の位置及び姿勢の変化を表わす運動を取得する運動取得手段と、
   前記第１の画像上における複数の特徴点の３次元位置を取得する３次元位置取得手段と、
   前記運動取得手段によって取得された前記移動体の運動に基づいて、前記３次元位置取得手段によって３次元位置が取得された前記複数の特徴点の、前記第２の画像上の位置を予測する位置予測手段と、
   前記検索手段によって検索された前記対応した特徴点の複数の組における前記第２の画像上の複数の特徴点の位置と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置との差分に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の姿勢の上下方向の変化を表わす上下回転運動を推定する上下回転運動推定手段と、
   前記検索手段によって検索された前記対応した特徴点の複数の組と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置と、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動とに基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を推定する運動推定手段と、
   を含む運動推定装置。
2. 前記検索手段によって検索された前記対応した特徴点の複数の組と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置と、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動とに基づいて、前記検索手段によって検索された前記対応した特徴点の複数の組の各々について、静止物を表わす領域上の特徴点の対応であって、かつ、前記特徴点の対応の正しさを示す信頼度を算出する信頼度算出手段を更に含み、
   前記運動推定手段は、前記検索手段によって検索された前記対応した特徴点の複数の組と、前記対応した特徴点の複数の組について算出された前記信頼度とに基づいて、前記移動体の運動を推定する請求項１記載の運動推定装置。
3. 前記検索手段によって検索された前記対応した特徴点の複数の組における前記第２の画像上の複数の特徴点の位置と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置を、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動の分だけ変更した位置とに基づいて、前記検索手段によって検索された前記対応した特徴点の全ての組のうち、前記対応した特徴点の複数の組を選択する選択手段を更に含み、
   前記運動推定手段は、前記選択手段によって選択された前記対応した特徴点の複数の組に基づいて、前記移動体の運動を推定する請求項１記載の運動推定装置。
4. 前記移動体の運動を計測する運動計測手段を更に含み、
   前記運動取得手段は、前記運動計測手段によって計測された前記移動体の運動に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を取得する請求項１〜請求項３の何れか１項記載の運動推定装置。
5. 前記運動取得手段は、前記運動推定手段によって過去に推定された前記移動体の運動に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を推定する請求項１〜請求項３の何れか１項記載の運動推定装置。
6. 前記３次元位置取得手段は、前記運動推定手段によって推定された、前記第１の画像より前に撮像された前画像及び前記第１の画像の各々を撮像したときの前記移動体の運動と、前記検索手段によって検索された前記前画像及び前記第１の画像で対応した特徴点の各組とに基づいて、前記第１の画像上における各特徴点の３次元位置を算出する請求項１〜請求項５の何れか１項記載の運動推定装置。
7. 前記第１の画像上における前記移動体が走行する道路領域を特定する道路領域特定手段を更に含み、
   前記３次元位置取得手段は、予め求められた道路と前記移動体の外部を撮像する撮像手段との位置関係に基づいて、前記道路領域特定手段によって特定された前記第１の画像上における道路領域内の各特徴点の３次元位置を算出する請求項１〜請求項５の何れか１項記載の運動推定装置。
8. コンピュータを、
   移動体の外部を撮像した第１の画像及び第２の画像の各々から、前記第１の画像及び第２の画像間で対応した特徴点を検索する検索手段、
   前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の位置及び姿勢の変化を表わす運動を取得する運動取得手段、
   前記第１の画像上における複数の特徴点の３次元位置を取得する３次元位置取得手段と、
   前記運動取得手段によって取得された前記移動体の運動に基づいて、前記３次元位置取得手段によって３次元位置が取得された前記複数の特徴点の、前記第２の画像上の位置を予測する位置予測手段、
   前記検索手段によって検索された前記対応した特徴点の複数の組における前記第２の画像上の複数の特徴点の位置と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置との差分に基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の姿勢の上下方向の変化を表わす上下回転運動を推定する上下回転運動推定手段、及び
   前記検索手段によって検索された前記対応した特徴点の複数の組と、前記位置予測手段によって予測された前記第２の画像上の複数の特徴点の位置と、前記上下回転運動推定手段によって推定された前記移動体の上下回転運動とに基づいて、前記第１の画像及び前記第２の画像の各々を撮像したときの前記移動体の運動を推定する運動推定手段
   として機能させるためのプログラム。

Images