(57)【要約】【課題】簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができるようにする。【解決手段】撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する（１６０）。そして、ジャイロセンサで検出された３軸角速度を取得し（１６２）、自車両の外部を撮像した複数の画像に基づいて算出された３軸角速度の平均値と、ジャイロセンサによって検出された３軸角速度の平均値との差に基づいて算出された補正量ΔＲを取得する（１６４）。取得した３軸角速度の各々について、角速度Ｒｊｙｒｏから、取得した補正量ΔＲを減算して、補正した角速度Ｒ´ｊｙｒｏを算出し（１６６）、補正された３軸角速度と、得られた並進方向を示す成分とを、自車両の運動の推定値として出力する（１６８）。【選択図】図８
2009074859.tif 000002
【特許請求の範囲】
【請求項１】
移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する画像算出手段と、
前記移動体の運動の前記３軸角速度の少なくとも１つを検出する角速度検出センサと、
前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、
を含む運動計測装置。
【請求項２】
前記運動推定手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つに基づいて、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つの補正量を算出する補正量算出手段を備え、前記補正量算出手段によって算出された補正量に基づいて、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つを補正し、前記補正した前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項１記載の運動計測装置。
【請求項３】
前記補正量算出手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとの差に基づいて、前記補正量を算出する請求項２記載の運動計測装置。
【請求項４】
前記画像算出手段によって算出された前記３軸角速度の少なくとも１つについて、該３軸角速度の少なくとも１つがどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段を更に含み、
前記補正量算出手段は、前記信頼度算出手段によって算出された前記信頼度が所定値以上である前記３軸角速度の少なくとも１つに基づいて、前記補正量を算出する請求項２又は３記載の運動計測装置。
【請求項５】
前記運動推定手段は、前記補正された前記３軸角速度の少なくとも１つと前記画像算出手段によって算出された前記３軸角速度の少なくとも１つとの差が所定値以上である場合、前記補正された前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定し、前記補正された前記３軸角速度の少なくとも１つと前記画像算出手段によって算出された前記３軸角速度の少なくとも１つとの差が所定値未満である場合、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項２〜請求項４の何れか１項記載の運動計測装置。
【請求項６】
前記運動推定手段は、前記補正された前記３軸角速度の少なくとも１つの絶対値が所定値以上である場合、前記補正された前記３軸角速度の少なくとも１つを更に補正し、前記補正した前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項２〜請求項５の何れか１項記載の運動計測装置。
【請求項７】
前記補正量を、前記角速度検出センサによって検出される前記３軸角速度の少なくとも１つのオフセット量とした請求項２〜請求項６の何れか１項記載の運動計測装置。
【請求項８】
前記角速度検出センサを、ジャイロセンサとした請求項１〜請求項７の何れか１項記載の運動計測装置。
【請求項９】
前記移動体の外部を撮像する単眼の撮像手段を更に含み、
前記画像算出手段は、前記撮像手段によって撮像された複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する請求項１〜請求項８の何れか１項記載の運動計測装置。
【請求項１０】
前記画像算出手段は、前記複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を算出し、
前記運動推定手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つ及び並進方向と、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定する請求項１〜請求項９の何れか１項記載の運動計測装置。
【請求項１１】
請求項１〜請求項９の何れか１項記載の運動計測装置と、
前記移動体の移動量を推定する移動量推定手段と、
前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つと、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段と、
を含む位置計測装置。
【請求項１２】
請求項１０記載の運動計測装置と、
前記移動体の移動量を推定する移動量推定手段と、
前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段と、
を含む位置計測装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、運動計測装置及び位置計測装置に係り、特に、移動体の運動を計測する運動計測装置、及び移動体の運動に基づいて移動体の位置を計測する位置計測装置に関する。
【背景技術】
【０００２】
従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己（車）運動認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。また、遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて回転を求め、近傍用のステレオカメラを用いて並進を求める。また、２枚の画像間のオプティカルフローに基づいて、移動体の運動を検出する。
【特許文献１】特開２００３−２４７８２４
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、ロバスト性が得られない、という問題がある。
【０００４】
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる運動計測装置及び位置計測装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために第１の発明に係る運動計測装置は、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する画像算出手段と、前記移動体の運動の前記３軸角速度の少なくとも１つを検出する角速度検出センサと、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段とを含んで構成されている。
【０００６】
第１の発明に係る運動計測装置によれば、画像算出手段によって、移動体の外部を撮像した複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つを算出する。ここで、移動体の外部を撮像した複数の画像は、単眼カメラで取得した少なくとも２枚の時系列画像である。
【０００７】
また、角速度検出センサによって、移動体の運動の３軸角速度の少なくとも１つを検出する。そして、運動推定手段によって、画像算出手段によって算出された３軸角速度の少なくとも１つと、角速度検出センサによって検出された３軸角速度の少なくとも１つとに基づいて、移動体の運動の３軸角速度の少なくとも１つを推定する。
【０００８】
このように、移動体の外部を撮像した複数枚の画像に基づいて算出された３軸角速度と、角速度検出センサによって検出された３軸角速度とに基づいて、移動体の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる。
【０００９】
上記の角速度検出センサを、ジャイロセンサとすることができる。
【００１０】
第１の発明に係る運動推定手段は、画像算出手段によって算出された３軸角速度の少なくとも１つに基づいて、角速度検出センサによって検出された３軸角速度の少なくとも１つの補正量を算出する補正量算出手段を備え、補正量算出手段によって算出された補正量に基づいて、角速度検出センサによって検出された３軸角速度の少なくとも１つを補正し、補正した３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。
【００１１】
ここで、補正量を、角速度検出センサによって検出される３軸角速度の少なくとも１つのオフセット量とすることができる。
【００１２】
これによって、角速度検出センサによって検出された３軸角速度のオフセット量を補正することができ、移動体の運動の３軸角速度を精度よく計測することができる。
【００１３】
上記の補正量算出手段は、画像算出手段によって算出された３軸角速度の少なくとも１つと、角速度検出センサによって検出された３軸角速度の少なくとも１つとの差に基づいて、補正量を算出することができる。これによって、角速度検出センサによって検出された３軸角速度のオフセット量に相当する補正量を算出することができる。
【００１４】
上記の運動計測装置は、画像算出手段によって算出された３軸角速度の少なくとも１つについて、該３軸角速度の少なくとも１つがどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段を更に含み、補正量算出手段は、信頼度算出手段によって算出された信頼度が所定値以上である３軸角速度の少なくとも１つに基づいて、補正量を算出することができる。これによって、信頼度の高い３軸角速度を用いて、精度よく補正量を算出することができる。
【００１５】
上記の補正量算出手段を備えた運動推定手段は、補正された３軸角速度の少なくとも１つと画像算出手段によって算出された３軸角速度の少なくとも１つとの差が所定値以上である場合、補正された３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定し、補正された３軸角速度の少なくとも１つと画像算出手段によって算出された３軸角速度の少なくとも１つとの差が所定値未満である場合、画像算出手段によって算出された３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。これによって、移動体の外部を撮像した画像に基づいて算出された３軸角速度と、補正された３軸角速度との差が小さい場合には、外部の画像に基づいて算出された３軸角速度を採用し、差が大きい場合には、補正された３軸角速度を採用して、ロバスト性を確保して、移動体の運動を精度よく計測することができる。
【００１６】
上記の補正量算出手段を備えた運動推定手段は、補正された３軸角速度の少なくとも１つの絶対値が所定値以上である場合、補正された３軸角速度の少なくとも１つを更に補正し、補正した３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。これによって、補正された３軸角速度の絶対値が大きすぎる場合には、更に補正することができるため、移動体の運動を精度よく計測することができる。
【００１７】
上記の運動計測装置は、移動体の外部を撮像する単眼の撮像手段を更に含み、画像算出手段は、撮像手段によって撮像された複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つを算出することができる。
【００１８】
上記の画像算出手段は、複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を算出し、運動推定手段は、画像算出手段によって算出された３軸角速度の少なくとも１つ及び並進方向と、角速度検出センサによって検出された３軸角速度の少なくとも１つとに基づいて、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定することができる。
【００１９】
第２の発明に係る位置計測装置は、上記の運動計測装置と、前記移動体の移動量を推定する移動量推定手段と、前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つと、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段とを含んで構成されている。
【００２０】
第２の発明に係る位置計測装置によれば、運動計測装置によって、移動体の運動の３軸角速度の少なくとも１つを推定し、また、移動量推定手段によって、移動体の移動量を推定する。
【００２１】
そして、位置算出手段によって、運動計測装置によって推定された移動体の運動の３軸角速度の少なくとも１つと、移動量推定手段によって推定された移動体の移動量とに基づいて、移動体の位置を算出する。
【００２２】
このように、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。
【００２３】
第３の発明に係る位置計測装置は、上記の移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定する運動計測装置と、前記移動体の移動量を推定する移動量推定手段と、前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段とを含んで構成されている。
【００２４】
第３の発明に係る位置計測装置によれば、運動計測装置によって、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、また、移動量推定手段によって、移動体の移動量を推定する。
【００２５】
そして、位置算出手段によって、運動計測装置によって推定された移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、移動量推定手段によって推定された移動体の移動量とに基づいて、移動体の位置を算出する。
【００２６】
このように、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。
【発明の効果】
【００２７】
以上説明したように、本発明の運動計測装置によれば、移動体の外部を撮像した複数の画像に基づいて算出された３軸角速度と、角速度検出センサによって検出された３軸角速度とに基づいて、移動体の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる、という効果が得られる。
【００２８】
本発明の位置計測装置によれば、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００２９】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された位置計測装置に本発明を適用した場合を例に説明する。
【００３０】
図１に示すように、第１の実施の形態に係る位置計測装置１０は、自車両の運動を計測する運動計測部１２と、自車両の移動量を計測する移動量計測部１４と、計測された自車両の運動及び移動量に基づいて、自車両の位置を推定する位置推定部１６とを備えている。
【００３１】
運動計測部１２は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部２０と、自車両の運動に関連する情報として、自車両の３軸角速度（ピッチ角速度、ヨー角速度、ロール角速度）を検出するジャイロセンサ２２と、撮像画像及びジャイロセンサ２２の検出結果に基づいて、自車両の運動を推定する運動推定部２４とを備えている。
【００３２】
運動推定部２４では、自車両の運動として、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定する。なお、本実施の形態では、運動推定部２４によって、並進量（並進成分スカラ量）は推定しない。
【００３３】
本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できるものであれば、設置場所、画角、波長、及び設置個数について問わない。
【００３４】
また、ジャイロセンサ２２によって３軸角速度を検出する場合を例に説明するが、これに限定されるものではなく、ジャイロセンサ２２によって、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを検出するように構成してもよい。
【００３５】
ここで、ジャイロセンサ２２による検出結果のオフセット量について以下に説明する。通常、ジャイロセンサによって検出される３軸角速度にはオフセット量が存在し、検出された３軸角速度を補正せずにそのままの値を用いて、推測航法による位置推定を実施すると、誤差蓄積によって大きな位置誤差が生じる。また、オフセット量は、機器の温度などによって変動するため、予め設定した補正値を用いることは適切でない。そのため、ＧＰＳの測位結果や地図情報などに基づいて、オフセット量を算出し、オフセット量の補正が実施されるが、都心部などのＧＰＳの受信状態がよくない場所に自車両が存在する場合や、電源投入直後のＧＰＳの受信回数が限られている場合には、オフセット量を正しく算出することができず、オフセット量の補正を十分に行えない可能性がある。
【００３６】
また、カメラの撮像画像に基づいて、運動推定を行う場合、ＧＰＳの受信状態に関わらず、運動の推定が可能である。例えば、図２に示すように、ジャイロセンサ２２によって検出される角速度の時間変化と、単眼カメラの撮像画像に基づいて推定された角速度の時間変化とは、傾向が一致しているが、撮像画像に基づく角速度に対して、ジャイロセンサ２２によって検出される角速度にオフセット量が存在している。しかしながら、撮像画像中の安定した特徴点の有無や日照条件によって、複数の撮像画像に基づいて推定された運動の信頼性が変化する。例えば、図３に示すように、単眼カメラの撮像画像から推定される角速度は、安定して得られない場合がある。
【００３７】
次に、運動推定部２４における運動の推定方法について説明する。運動推定部２４では、単眼カメラのロバスト性を補い、ジャイロセンサ２２に生じるオフセットの影響やカーブ時の出力誤差を低減するために、単眼カメラの撮像画像に基づいて推定される運動と、ジャイロセンサ２２の検出結果とを相互に監視して、状況に応じて適切な運動を推定する。
【００３８】
まず、画像撮像部２０によって撮像された複数の撮像画像に基づいて、３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出する。また、ジャイロセンサ２２によって検出された３軸角速度を取得すると共に、撮像画像に基づいて推定された３軸角速度の信頼性を算出する。
【００３９】
そして、ジャイロセンサ２２によって検出される３軸角速度の値のバラつきを考慮して、一定期間に取得された角速度を平均し、また、撮像画像によって推定される３軸角速度の値のバラつきを考慮して、撮像画像に基づいて推定された一定期間の角速度を平均する。そして、これらの平均値を用いて、ジャイロセンサ２２のオフセット量を補正する。
【００４０】
ここで、画像撮像部２０の撮像画像に基づく３軸角速度の信頼度が、予め定められた閾値よりも低い場合、この３軸角速度の推定値には大きな誤差が含まれていると考えられるため、この信頼度が低い３軸角速度を除いて、３軸角速度の各々の平均値を算出する。また、画像撮像部２０の撮像画像に基づいて推定された３軸角速度、あるいは一時刻前に運動推定部２４によって推定された３軸角速度が、所定値以上の場合、撮像画像中の特徴点を安定して追従できずに誤差が大きくなってしまった可能性があるため、所定値以上の３軸角速度を除いて、３軸角速度の各々の平均値を算出する。
【００４１】
また、ジャイロセンサ２２によって検出された３軸角速度について、電源投入直後に検出された３軸角速度や所定値以上の３軸角速度である場合、検出された３軸角速度に大きな誤差が生じていると考えられるため、電源投入直後に検出された３軸角速度や所定値以上の３軸角速度を除いて、３軸角速度の各々の平均値を算出する。
【００４２】
上記のように算出された、ジャイロセンサ２２による角速度の一定時間Ｔ[ｓｅｃ]分の平均値ａｖｅＲｊｙｒｏと、撮像画像による角速度の一定時間Ｔ[ｓｅｃ]分の平均値ａｖｅＲｉｍａｇｅとの差ΔＲ（＝ａｖｅＲｊｙｒｏ− ａｖｅＲｉｍａｇｅ）を算出し、算出された差ΔＲを、ジャイロセンサ２２によって検出された角速度に生じるオフセット量とする。そして、ジャイロセンサ２２によって検出された角速度Ｒｊｙｒｏからオフセット量ΔＲを減算し、減算した値Ｒ‘ｊｙｒｏ（＝Ｒｊｙｒｏ−ΔＲ）をジャイロセンサ２２によって検出された角速度の補正値とする。また、上記の補正量としてのオフセット量は、常時更新され、熱変動などに対応して、適切な角速度を算出する。
【００４３】
移動量計測部１４は、自車両の車速に関する情報を検出する車速センサ２６と、車速センサ２６によって検出された検出結果から移動量を推定する移動量推定部２８とを備えている。車速センサ２６は、例えば、車速に応じて車速パルス信号を出力する車速パルスセンサで構成され、自車両の対地速に応じた車速パルス信号を出力する。移動量推定部２８は、車速センサの検出結果から、自車両の移動量として並進成分スカラ量又は車速を推定する。例えば、計測単位期間Ｔｍｓにおける車速パルス信号のパルス数に基づいて、車速を推定する。
【００４４】
位置推定部１６は、計測された自車両の運動及び移動量に基づいて、自車両の相対位置の変化量を算出し、過去の絶対位置と相対位置の変化量とに基づいて、自車両の絶対位置を推定する。以下に、相対位置の変化量を算出する方法について図４を用いて説明する。
【００４５】
まず、図４に示すような座標系において、初期位置を（Ｘ（０）、Ｙ（０）、Ｚ（０））とし、時刻ｔにおいて、移動量（速度Ｖ（ｔ））、運動（３軸角速度（ヨー角速度φ（ｔ）、ピッチ角速度θ（ｔ）、ロール角速度ψ（ｔ））、並進方向を示す成分（ａｘ、ａｙ、ａｚ））が得られると、下記の（１）式のように、時刻ｔ＋Δｔにおける相対位置（Ｘ（ｔ＋Δｔ）、Ｙ（ｔ＋Δｔ）、Ｚ（ｔ＋Δｔ））を更新することができる。
【００４６】
【数１】
2009074859.tif 000003
ただし、ａｘ／｜ａ｜、ａｙ／｜ａ｜、及びａｚ／｜ａ｜は、正規化された並進方向を示す成分の各々を表わす。
【００４７】
そして、自車両に搭載されたＧＰＳ（図示省略）から、経度及び緯度などで表わされる絶対位置情報が過去に取得されていた場合には、絶対位置情報が示す絶対位置と、算出された相対位置とに基づいて、自車両の現在の絶対位置を計測する。なお、絶対位置情報を、必ずしもＧＰＳの測位位置から取得する必要はなく、例えば路側の位置が既知である設備から、通信などによって移動体の絶対位置情報を間欠的に取得してもよいし、予め位置が既知であるランドマーク情報が登録された地図などから、自車両の位置を取得してもよい。また、ＧＰＳと同じ頻度で取得する必要もなく、移動量と運動とで補完できる間隔で取得すればよく、取得手段も問わない。
【００４８】
次に、第１の実施の形態に係る位置計測装置１０の作用について説明する。なお、位置計測装置１０を搭載した自車両が走行しているときに、自車両の位置を計測する場合を例に説明する。
【００４９】
まず、位置計測装置１０において、図５に示す補正量算出処理ルーチンが実行される。ステップ１００において、撮像画像に基づく運動推定処理が行われる。上記ステップ１００は、図６に示す撮像画像に基づく運動推定処理ルーチンによって実現される。以下に、撮像画像に基づく運動推定処理ルーチンについて説明する。
【００５０】
まず、ステップ１４０において、異なるタイミングで画像撮像部２０によって自車両前方を撮像した第１の画像及び第２の画像を取得し、ステップ１４２において、上記ステップ１４０で取得した第１の画像を複数の領域に分割する。このステップ１４２では、例えば、予め与えられた定数ｙ１、ｙ２を各領域の境界線（ｙ軸の切片座標）として、第１の画像を、画像上段の上方領域と、中段の中間領域と、下段の下方領域との３つの領域に分割する。
【００５１】
次のステップ１４４において、上記ステップ１４２で得られた各分割画像中から、それぞれ所定の数だけ特徴点（不動点であることが期待される点）を抽出する。ここで、特徴点とは、周囲の点とは区別でき、追跡が容易な点のことである。このステップ１４４では、例えば、二次元的に濃淡変化の勾配値が大きくなる画素を検出する、一般的な周知の方法（例えば、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に特徴点を抽出する。
【００５２】
そして、ステップ１４６において、上記ステップ１４４で抽出された第１の画像における特徴点の各々を、上記ステップ１４０で取得した第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検出する。このとき、２時刻間にて対応する２つの特徴点は、同一の不動点であることが期待されているので、第１の画像と第２の画像とで対応する点とその周囲の点との輝度は、殆ど変化しないという仮定を用い、この仮定に基づいて各特徴点の追跡を行う。この様な追跡処理を実施する際には、例えば、周知のルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などを用いると良い。また、各特徴点の追跡処理の後、第１の画像と第２の画像との間で対応する特徴点の各組の各画像内座標を、１つの適当な形式に整理する。
【００５３】
そして、ステップ１４８において、上記ステップ１４６で得られた２時刻間で互いに対応する特徴点の組から、２つの撮像時刻間における画像撮像部２０の相対的な位置関係、即ち、画像撮像部２０を搭載した自車両のその２時刻間における運動に関連する情報を推定する。
【００５４】
以下、図７を用いて、抽出された特徴点（不動点）と移動体の運動との関係を説明する。図７に示すように、移動体の運動は、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとから構成される。例えば、第１の画像上の特徴点ｘ１に対応する第２の画像上の特徴点をｘ２とすると、これらの特徴点は、同一の不動点を示す点である。
【００５５】
ここで、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、対応する２つの特徴点の関係を、以下の（２）式〜（４）式で表すことができる。
【００５６】
【数２】
2009074859.tif 000004
【００５７】
上記の（２）式〜（４）式のように、上記の２時刻間における８組以上の互いに対応する特徴点の組（即ち、同一の不動点）が求まっていれば、それに基づいて、上記の基礎行列Ｆを算出することができる。そして、カメラのキャリブレーション行列Ｋが既知であれば、上記の式からも分かる様に、その２時刻間における移動体の運動、即ち、その２時刻間における自車両の回転行列Ｒと並進ベクトルｔとを求めることができる。
【００５８】
そして、ステップ１５０において、上記ステップ１４８で求めた自車両の回転行列Ｒと並進ベクトルｔとに基づいて、自車両の３軸角速度と並進方向を示す成分とを算出し、算出した３軸角速度と並進方向を示す成分とを、自車両の運動の推定値として出力して、撮像画像に基づく運動推定処理ルーチンを終了する。
【００５９】
そして、上記図５のステップ１０２において、上記ステップ１００で推定された３軸角速度がどれだけ信頼できるかを示す信頼度を算出する。このステップ１０２では、例えば、上記ステップ１００の撮像画像に基づく運動推定処理において、第１の画像と第２の画像との間で対応付けられた特徴点の数が多いほど、高い信頼度が算出される。すなわち、どれだけ特徴点が照合できたかを表わす度合いを、信頼度として算出する。
【００６０】
そして、ステップ１０４において、ジャイロセンサ２２で検出された３軸角速度を取得する。なお、以下のステップ１０６〜ステップ１２４の説明では、３軸角速度の何れかの角速度Ｒｊｙｒｏについて説明する。
【００６１】
ステップ１０６で、上記ステップ１０４で取得した角速度Ｒｊｙｒｏが、予め定められた閾値Ｒｔｈ未満であるか否かを判定する。上記ステップ１０６で、角速度Ｒｊｙｒｏが、閾値Ｒｔｈ未満であると判定された場合には、ステップ１０８において、上記ステップ１０４で取得した角速度Ｒｊｙｒｏを、平均計算対象区間Ｔａｖｅ_ｊｙｒｏの角速度として採用する。一方、上記ステップ１０６で、角速度Ｒｊｙｒｏが、閾値Ｒｔｈ以上であると判定された場合には、ステップ１１０において、上記ステップ１０４で取得した角速度Ｒｊｙｒｏを、平均計算対象区間Ｔａｖｅ_ｊｙｒｏの角速度として採用せずに、前回求められた平均計算対象区間Ｔａｖｅ_ｊｙｒｏの角速度を保持する。
【００６２】
そして、ステップ１１２において、平均計算対象区間Ｔａｖｅ_ｊｙｒｏの角速度を平均して、角速度の平均値ａｖｅＲｊｙｒｏを算出する。
【００６３】
次のステップ１１４では、上記ステップ１０２で算出された信頼度が、予め定められた閾値Ｐｔｈより大きいか否かを判定し、信頼度が、閾値Ｐｔｈより大きい場合には、ステップ１１６で、上記ステップ１００で推定された角速度Ｒｉｍａｇｅが、予め定められた閾値Ｒｔｈ未満であるか否かを判定する。上記ステップ１１６で、角速度Ｒｉｍａｇｅが、閾値Ｒｔｈ未満であると判定された場合には、ステップ１１８において、上記ステップ１００で取得した角速度Ｒｉｍａｇｅを、平均計算対象区間Ｔａｖｅ_ｉｍａｇｅの角速度として採用して、ステップ１２２へ移行する。一方、上記ステップ１１６で、角速度Ｒｉｍａｇｅが、閾値Ｒｔｈ以上であると判定された場合には、得られた角速度が大きいと判断し、ステップ１２０へ移行する。また、上記ステップ１１４において、算出された信頼度が、閾値Ｐｔｈ未満である場合には、得られた角速度の信頼度が低いと判断し、ステップ１２０へ移行する。
【００６４】
ステップ１２０では、上記ステップ１００で取得した角速度Ｒｉｍａｇｅを、平均計算対象区間Ｔａｖｅ_ｉｍａｇｅの角速度として採用せずに、前回求められた平均計算対象区間Ｔａｖｅ_ｉｍａｇｅの角速度を保持して、ステップ１２２へ移行する。
【００６５】
そして、ステップ１２２では、平均計算対象区間Ｔａｖｅ_ｉｍａｇｅの角速度を平均して、角速度の平均値ａｖｅＲｉｍａｇｅを算出し、ステップ１２４において、上記ステップ１１２で算出された角速度の平均値ａｖｅＲｊｙｒｏから、上記ステップ１２２で算出された角速度の平均値ａｖｅＲｉｍａｇｅを減算して、平均値の差ΔＲ（＝ａｖｅＲｊｙｒｏ−ａｖｅＲｉｍａｇｅ）を補正量として算出して、メモリ（図示省略）に記憶し、補正量算出処理ルーチンを終了する。
【００６６】
なお、上記ステップ１０６〜ステップ１２４の処理は、ジャイロセンサ２２が出力する３軸角速度の各々について実行され、３軸角速度の各々について補正量が算出されて、メモリ（図示省略）に記憶される。また、上述した補正量算出処理ルーチンは、定期的に実行され、３軸角速度の補正量が、定期的に更新される。
【００６７】
なお、各判定に必要な閾値は、経験的に決定した定数でもよいし、データ群から数値解析によって決定した定数でもよい。また、データや周辺状況に応じて動的に変動するような機構をいれて、閾値を決定するようにしてもよい。
【００６８】
また、位置計測装置１０において、図８に示す運動推定処理ルーチンが実行される。まず、ステップ１６０において、上述したステップ１００と同様に、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、補正量算出処理ルーチンで算出され、メモリに記憶された３軸角速度各々の補正量ΔＲを取得する。
【００６９】
次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´ｊｙｒｏを算出し、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【００７０】
また、位置計測装置１０において、図９に示す移動量推定処理ルーチンが実行される。まず、ステップ１７０で、計測単位期間Ｔｍｓに車速センサ２６から出力される車速パルスのパルス数をカウントする。そして、ステップ１７２において、上記ステップ１７０でカウントされたパルス数に基づいて、自車両の車速を推定し、ステップ１７４において、上記ステップ１７２で推定された自車両の車速を、自車両の移動量の推定値として出力して、移動量推定処理ルーチンを終了する。
【００７１】
また、位置計測装置１０において、図１０に示す位置計測処理ルーチンが実行される。まず、ステップ１８０で、ＧＰＳによって自車両の位置を測位し、測位した位置を初期位置として設定する。
【００７２】
そして、ステップ１８２において、上述した運動推定処理ルーチンを実行して、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分の推定値を取得し、ステップ１８４において、上記の移動量推定処理ルーチンを実行して、自車両の移動量としての車速の推定値を取得する。
【００７３】
そして、ステップ１８６において、上記ステップ１８２で取得した自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、上記ステップ１８４で取得した自車両の車速とに基づいて、自車両の相対位置の変化量を算出する。次のステップ１８８では、上記ステップ１８０で設定された初期位置と、上記ステップ１８６で算出された相対位置の変化量とに基づいて、自車両の絶対位置を算出する。
【００７４】
そして、ステップ１９２において、上記ステップ１８２からステップ１８８によって絶対位置を計測してから、計測単位期間Ｔｍｓが経過したか否かを判定し、計測単位期間Ｔｍｓが経過すると、ステップ１８２へ戻り、再び自車両の絶対位置を計測して、自車両の絶対位置を更新する。
【００７５】
次に、本実施の形態に係る位置計測装置１０の計測結果について説明する。初期位置として真値を用いて、ジャイロセンサ２２によって検出された３軸角速度について、オフセット量の補正を行って、相対的な位置の計測を行った。また、比較対象として、初期位置として真値を用いて、ジャイロセンサ２２によって検出された３軸角速度について、オフセット量の補正を行わずに、相対的な位置の計測を行った。
【００７６】
一般的なジャイロセンサの検出結果には、その特性により、オフセット誤差や回転時の振幅誤差があるため、図１１に示すように、オフセットを補正した場合の計測結果と、補正しなかった場合の計測結果とを比較すると、オフセット誤差を補正しない場合は、極端に軌跡が広がったり、旋回方向によっては小さくなったりする。一方、撮像画像に基づいて、随時オフセット誤差を修正して、相対的な位置を計測すると、真値の軌跡の形状に比較的近い軌跡が得られることがわかった。
【００７７】
以上説明したように、第１の実施の形態に係る位置計測装置によれば、自車両の外部を撮像した複数の画像に基づいて算出された３軸角速度の平均値と、ジャイロセンサによって検出された３軸角速度の平均値との差を、補正量として算出し、算出した補正量によって、ジャイロセンサによって検出された３軸角速度のオフセット量を補正して、自車両の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、自車両の運動の３軸角速度を精度よく計測することができる。
【００７８】
また、複数の撮像画像に基づいて算出され、かつ、信頼度の高い３軸角速度の平均値を用いて、ジャイロセンサのオフセット量に相当する補正量を算出するため、精度よく補正量を算出することができる。
【００７９】
また、ロバスト性を確保して精度よく計測された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【００８０】
また、車両に搭載されている単眼カメラとジャイロセンサとを用いることで、通常のカーナビで用いられるジャイロを用いるよりも高精度な位置推定を実現することができる。
【００８１】
また、車両外部の撮像画像は、照明環境にも影響を受け易いため、撮像画像に基づいて自車両の運動を算出できない場合があったが、ジャイロセンサを併せて用いることで、自車両の運動を常に計測することができる。
【００８２】
従来、ジャイロセンサをＧＰＳと併せて利用する場合は、ＧＰＳで得られる角度方向の変化量とジャイロセンサで得られる変化量を監視して、ジャイロセンサのオフセット量の補正を行っていたが、ＧＰＳの測位間隔は通常１Ｈｚと長く、また測位精度も高くないため、角度方向の変動が大きかった。また、都心部などでは測位頻度が低下するため、安定してオフセット補正を行なうことができない問題があった。一方、本実施の形態では、単眼カメラの撮像画像を用いて３軸角速度を推定することで、精度の高い３軸角速度をカメラの撮像レートで取得できるため、ジャイロセンサのオフセット量を効果的に補正することができる。
【００８３】
次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００８４】
第２の実施の形態では、撮影画像に基づいて推定される３軸角速度の信頼度が高い場合、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値としている点が第１の実施の形態と異なっている。
【００８５】
第２の実施の形態に係る位置計測装置１０の運動推定部２４では、画像撮像部２０の撮像画像に基づく３軸角速度に対する信頼度を算出し、算出した信頼度が所定値以上である場合には、ジャイロセンサ２２で検出された３軸角速度を用いずに、画像撮像部２０の撮像画像に基づいて推定される３軸角速度を、自車両の運動の推定値として出力する。
【００８６】
次に、第２の実施の形態に係る運動推定処理ルーチンについて、図１２を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【００８７】
まず、ステップ１６０において、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ２００において、上記ステップ１０２と同様に、上記ステップ１００で推定された３軸角速度に対する信頼度を算出し、ステップ２０２において、上記ステップ２００で算出された信頼度が、予め定められた閾値Ｐｔｈより大きいか否かを判定する。
【００８８】
上記ステップ２０２で、信頼度が、閾値Ｐｔｈより大きいと判定された場合には、撮像画像に基づいて推定された推定値の信頼度が高いと判断し、ステップ２０４で、上記ステップ１６０で算出された３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【００８９】
一方、上記ステップ２０２で、信頼度が、閾値Ｐｔｈ以下であると判定された場合には、撮像画像に基づいて推定された推定値の信頼度が低いと判断し、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、上述した補正量算出処理ルーチンで算出され、メモリに記憶された３軸角速度の各々の補正量ΔＲを取得する。
【００９０】
次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´ｊｙｒｏを算出し、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【００９１】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【００９２】
このように、撮影画像に基づいて推定される３軸角速度に対する信頼度が高い場合に、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値とし、撮影画像に基づいて推定される３軸角速度の信頼度が低い場合に、ジャイロセンサから検出された各角速度の補正値を、自車両の運動の推定値とすることにより、簡易な構成で、ロバスト性を確保して、精度よく自車両の運動を推定することができる。
【００９３】
なお、３軸角速度の全てがジャイロセンサから出力されない場合には、ジャイロセンサから検出された角速度の補正値と、撮像画像に基づいて推定された他の角速度とを、自車両の運動の推定値として出力すればよい。
【００９４】
次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００９５】
第３の実施の形態では、算出される補正量によって補正されたジャイロセンサからの３軸角速度と、撮像画像に基づいて推定される３軸角速度との差が、小さい場合には、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値としている点と、補正されたジャイロセンサからの３軸角速度の絶対値が、所定値以上である場合には、補正された３軸角速度を更に補正して、補正した３軸角速度を、自車両の運動の推定値としている点とが、主に第１の実施の形態と異なっている。
【００９６】
第３の実施の形態に係る位置計測装置１０の運動推定部２４では、画像撮像部２０の撮像画像に基づく３軸角速度と、ジャイロセンサ２２から検出された３軸角速度の補正値との差が、一定量Ｄｔｈ以上になる場合は、特に単眼カメラの撮像画像に基づく推定値に誤差があると判断し、ジャイロセンサ２２から検出された３軸角速度の補正値を、自車両の運動の推定値として出力する。
【００９７】
また、運動推定部２４は、ジャイロセンサ２２から検出された３軸角速度の補正値の絶対量が、一定量Ｒｔｈ以上である場合は、絶対量が真値よりも大きくなっている可能性が高いと判断し、３軸角速度の補正値に所定の補正係数を乗じた値を、自車両の運動の推定値として出力する。
【００９８】
次に第３の実施の形態に係る運動推定処理ルーチンについて図１３を用いて説明する。なお、第１の実施の形態及び第２の実施の形態と同様の処理については、同一符号を付して説明を省略する。
【００９９】
まず、ステップ１６０において、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、上述した補正量算出処理ルーチンで算出された３軸角速度の各々の補正量ΔＲを取得する。
【０１００】
次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´ｊｙｒｏを算出する。そして、ステップ３００において、３軸角速度の各々について、補正した角速度Ｒ´ｊｙｒｏと、上記ステップ１６０で得られた角速度Ｒｉｍａｇｅとの差の絶対値（＝｜Ｒ´ｊｙｒｏ−Ｒｉｍａｇｅ｜）が、予め定められた閾値Ｄｔｈ未満であるか否かを判定する。上記ステップ３００で、角速度Ｒ´ｊｙｒｏと角速度Ｒｉｍａｇｅとの差の絶対値が、閾値Ｄｔｈ未満であると判定されると、ステップ２０４で、上記ステップ１６０で算出された３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【０１０１】
一方、上記ステップ３００で、角速度Ｒ´ｊｙｒｏと角速度Ｒｉｍａｇｅとの差の絶対値が、閾値Ｄｔｈ以上であると判定されると、３軸角速度の各々について、上記ステップ１６６で補正された角速度Ｒ´ｊｙｒｏの絶対値が、予め定められた閾値Ｒｔｈ未満であるか否かを判定する。上記ステップ３０２で、補正された角速度Ｒ´ｊｙｒｏの絶対値が、閾値Ｒｔｈ未満であると判定されると、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【０１０２】
一方、上記ステップ３０２で、補正された角速度Ｒ´ｊｙｒｏの絶対値が、閾値Ｒｔｈ以上であると判定されると、ステップ３０４で、３軸角速度の各々について、補正された角速度Ｒ´ｊｙｒｏが小さくなるように、所定の補正係数ｃｏｒｒ（ｃｏｒｒ＜１）を乗じて、角速度Ｒ´ｊｙｒｏを更に補正する。そして、次のステップ３０６で、上記ステップ３０４で補正された３軸角速度（＝Ｒ´ｊｙｒｏ×ｃｏｒｒ）と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【０１０３】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【０１０４】
このように、自車両の外部を撮像した複数の撮像画像に基づいて算出された３軸角速度と、補正された３軸角速度との差が小さい場合には、撮像画像に基づいて算出された３軸角速度を自車両の運動の推定値として採用し、差が大きい場合には、補正された３軸角速度を自車両の運動の推定値として採用することにより、ロバスト性を確保して、自車両の運動を精度よく推定することができる。
【０１０５】
また、補正された３軸角速度の絶対値が大きすぎる場合には、更に補正して、補正された３軸角速度を自車両の運動の推定値とすることにより、ロバスト性を確保して、自車両の運動を精度よく推定することができる。
【０１０６】
次に、第４の実施の形態について説明する。なお、第４の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１０７】
第４の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の運動の３軸角速度を推定している点が第１の実施の形態と異なっている。
【０１０８】
第４の実施の形態に係る位置計測装置１０の運動推定部２４では、ジャイロセンサ２２によって検出された３軸角速度と、画像撮像部２０の撮像画像に基づいて推定される３軸角速度とを観測値とし、自車両の運動の３軸角速度、３軸角速度各々の角加速度、及びジャイロセンサ２２の検出値のオフセット量である補正量を推定値として、カルマンフィルタによる最適処理手法を用いた処理を行う。
【０１０９】
カルマンフィルタによる最適処理手法では、画像撮像部２０の単眼カメラの撮像画像に基づく観測値ｙｉｍａｇｅが得られた場合に、観測値ｙｉｍａｇｅのバラつきを考慮して、観測ノイズｗｉｍａｇｅを設定し、観測行列Ｈｉｍａｇｅについて、観測値ｙｉｍａｇｅと推定値ｘとの関係式から、観測行列Ｈｉｍａｇｅを設定する。
【０１１０】
また、ジャイロセンサ２２から観測値ｙｊｙｒｏが得られた場合に、観測値ｙｊｙｒｏのバラつきを考慮して、観測ノイズｗｊｙｒｏを設定し、観測行列Ｈｊｙｒｏについて、観測値ｙｊｙｒｏと推定値ｘとの関係式から、観測行列Ｈｊｙｒｏを設定する。
【０１１１】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１１２】
次に、第４の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１１３】
第４の実施の形態に係る位置計測装置１０では、上述した撮像画像に基づく運動推定処理ルーチンが繰り返し実行され、推定された３軸角速度及び並進方向を示す成分が繰り返し出力される。また、ジャイロセンサ２２から３軸角速度が繰り返し検出される。
【０１１４】
また、位置計測装置１０において、図１４に示す運動推定処理ルーチンが実行される。まず、ステップ４００において、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ４０２において、ジャイロセンサ２２から検出された３軸角速度を取得する。
【０１１５】
そして、ステップ４０４において、上記ステップ４００で取得した３軸角速度と、上記ステップ４０２で取得した３軸角速度とに基づいて、３軸角速度、３軸角速度の各々の角加速度、及び３軸角速度の各々の補正量の推定値ｘに対して、初期値を設定する。次のステップ４０６では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ４０８において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１１６】
次のステップ４１０において、現タイミングに、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度、又はジャイロセンサ２２から検出された３軸角速度が、観測値として入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ４１２において、上記ステップ４０８で算出された推定値ｘの予測値の３軸角速度、及び上記ステップ４００で取得した並進方向を示す成分を、自車両の運動の推定値として出力し、ステップ４０６へ戻る。
【０１１７】
上記ステップ４１０において、現タイミングに観測値が入力されたと判定された場合には、ステップ４１４において、観測値として、ジャイロセンサ２２から検出された３軸角速度が入力されたのか否かを判定する。
【０１１８】
上記ステップ４１４で、観測値としてジャイロセンサ２２から検出された３軸角速度が入力されたと判定された場合には、ステップ４１６において、入力された３軸角速度を観測値として、観測行列Ｈｊｙｒｏ、観測値ｙｊｙｒｏ、及び観測ノイズｗｊｙｒｏを設定する。
【０１１９】
一方、上記ステップ４１４で、観測値として、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度が入力されたと判定された場合には、ステップ４１８において、入力された３軸角速度を観測値として、観測行列Ｈｉｍａｇｅ、観測値ｙｉｍａｇｅ、及び観測ノイズｗｉｍａｇｅを設定する。
【０１２０】
そして、ステップ４２０において、上記ステップ４１６又はステップ４１８で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ４２２では、上記ステップ４２０で算出された推定値ｘの３軸角速度、及び上記ステップ４００で取得した並進方向を示す成分を、自車両の運動の推定値として出力し、ステップ４０６へ戻る。
【０１２１】
また、第１の実施の形態と同様に、移動量推定処理ルーチンが実行され、自車両の移動量の推定値としての車速が算出される。また、上記の運動推定処理ルーチンから出力される自車両の運動の推定値、及び移動量推定処理ルーチンから出力される自車両の車速の推定値を用いて、第１の実施の形態と同様に、位置計測処理ルーチンが実行され、自車両の絶対位置が計測される。
【０１２２】
このように、カルマンフィルタを用いた最適処理手法によって、自車両の外部を撮像した複数の撮像画像に基づいて算出された３軸角速度及び並進方向と、ジャイロセンサによって検出された３軸角速度とに基づいて、自車両の運動の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の運動を精度よく計測することができる。
【０１２３】
また、ロバスト性を確保して精度よく計測された自車両の運動と、推定された自車両の車速とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【０１２４】
なお、上記の実施の形態では、カルマンフィルタで推定する例を記述したが、周知のパーティクルフィルタなど、他の最適推定手法を用いてもよく、適切に解を推定できる手法であれば、その方法はこの限りではない。
【０１２５】
また、上記の実施の形態において、誤差共分散を、推定された自車両の運動の推定値に対する信頼度として用いてもよい。この場合には、例えば、推定値と共に算出された誤差共分散によって、推定値の誤差が大きいと判断される場合には、この推定値を、自車両の運動の推定値として採用しないようにすることができる。
【０１２６】
次に、第５の実施の形態について説明する。なお、第５の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１２７】
第５の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の位置を計測している点が第１の実施の形態と異なっている。
【０１２８】
第５の実施の形態に係る位置計測装置１０の位置推定部１６では、運動推定部２４によって推定された運動（３軸角速度、並進方向を示す成分）、移動量推定部２８によって推定された移動量（車速）、及びＧＰＳによって測位された絶対位置(Ｘｇ,Ｙｇ,Ｚｇ)を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行う。
【０１２９】
カルマンフィルタを用いた最適処理手法では、運動推定部２４によって推定された運動の観測値ｙｍｏｖｅが得られた場合に、観測値ｙｍｏｖｅのバラつきを考慮して、観測ノイズｗｍｏｖｅを設定し、観測行列Ｈｍｏｖｅについて、観測値ｙｍｏｖｅと推定値ｘとの関係式から、観測行列Ｈｍｏｖｅを設定する。
【０１３０】
また、移動量推定部２８によって推定された移動量の観測値ｙｔｒａｖが得られた場合に、観測値ｙｔｒａｖのバラつきを考慮して、観測ノイズｗｔｒａｖを設定し、観測行列Ｈｔｒａｖについて、観測値ｙｔｒａｖと推定値ｘとの関係式から、観測行列Ｈｔｒａｖを設定する。
【０１３１】
また、ＧＰＳによって測位された絶対位置の観測値ｙｇｐｓが得られた場合に、観測値ｙｇｐｓのバラつきを考慮して、観測ノイズｗｇｐｓを設定し、観測行列Ｈｇｐｓについて、観測値ｙｇｐｓと推定値ｘとの関係式から、観測行列Ｈｇｐｓを設定する。
【０１３２】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１３３】
次に、第５の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１３４】
第５の実施の形態に係る位置計測装置１０では、第１の実施の形態で説明した運動推定処理ルーチンが繰り返し実行され、また、上述した移動量推定処理ルーチンが繰り返し実行される。また、ＧＰＳによって、所定時間ごと（例えば１秒ごと）に自車両の絶対位置が測位される。
【０１３５】
また、位置計測装置１０において、図１５に示す位置計測処理ルーチンが実行される。まず、ステップ５００において、運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ５０２において、移動量推定処理ルーチンによって推定された車速を取得する。次のステップ５０４では、ＧＰＳによって測位された自車両の絶対位置を取得する。
【０１３６】
そして、ステップ５０６において、上記ステップ５００で取得した３軸角速度及び並進方向を示す成分と、上記ステップ５０２で取得した車速と、上記ステップ５０４で取得した絶対位置とに基づいて、絶対位置、方位角、車速、及び３軸角速度の推定値ｘに対して、初期値を設定する。次のステップ５０８では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ５１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１３７】
次のステップ５１２において、現タイミングに、観測値として、運動推定処理ルーチンによって推定された３軸角速度、移動推定処理ルーチンによって推定された車速、又はＧＰＳによって測位された絶対位置が入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ５１４において、上記ステップ５１０で算出された推定値ｘの予測値の絶対位置を、自車両の絶対位置の推定値として出力し、ステップ５０８へ戻る。
【０１３８】
上記ステップ５１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ５１６において、上記ステップ５１２でどの観測値が入力されたかを判定する。
【０１３９】
上記ステップ５１６で、観測値として、運動推定処理ルーチンで推定された３軸角速度及び並進方向を示す成分が入力されたと判定された場合には、ステップ５１８において、入力された３軸角速度及び並進方向を示す成分を観測値として、観測行列Ｈｍｏｖｅ、観測値ｙｍｏｖｅ、及び観測ノイズｗｍｏｖｅを設定する。
【０１４０】
一方、上記ステップ５１６で、観測値として、移動量推定処理ルーチンによって推定された車速が入力されたと判定された場合には、ステップ５２０において、入力された車速を観測値として、観測行列Ｈｔｒａｖ、観測値ｙｔｒａｖ、及び観測ノイズｗｔｒａｖを設定する。
【０１４１】
また、上記ステップ５１６で、観測値として、ＧＰＳによって測位された絶対位置が入力されたと判定された場合には、ステップ５２２において、入力された絶対位置を観測値として、観測行列Ｈｇｐｓ、観測値ｙｇｐｓ、及び観測ノイズｗｇｐｓを設定する。
【０１４２】
そして、ステップ５２４において、上記ステップ５１８、ステップ５２０、又はステップ５２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ５２６では、上記ステップ５２４で算出された推定値ｘの絶対位置を、自車両の絶対位置の推定値として出力し、ステップ５０８へ戻る。
【０１４３】
このように、カルマンフィルタを用いた最適処理手法によって、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速と、ＧＰＳによって測位された位置とに基づいて、自車両の絶対位置の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の絶対位置を精度よく計測することができる。
【０１４４】
なお、上記の実施の形態では、カルマンフィルタを用いた位置計測処理において、推定された自車両の運動の３軸角速度及び並進方向を示す成分、推定された自車両の車速、及びＧＰＳによって測位された絶対位置が、観測値として入力される場合を例に説明したが、これに限定されるものではない。例えば、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、及び推定された自車両の車速とに基づいて推定された自車両の相対位置の変化量が、観測値として、カルマンフィルタを用いた位置計測処理に入力されるようになっていてもよい。この場合には、推定された自車両の相対位置の変化量、及びＧＰＳによって測位された絶対位置を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行うようにすればよい。
【０１４５】
また、カルマンフィルタに限らず、パーティクルフィルタなどの最適推定手法を用いてもよい。
【０１４６】
また、上記の第１の実施の形態〜第５の実施の形態では、車速センサを用いて、移動量としての車速を推定する場合を例に説明したが、これに限定されるものではなく、他の既知の方法によって、自車両の移動量を推定してもよい。例えば、ＧＰＳからの信号によって自車両の移動量を推定してもよく、また、単眼カメラの撮像画像から、移動量を推定してもよい。単眼カメラの撮像画像から移動量を推定する場合には、撮像画像から復元した３次元特徴点空間の路面を仮定することにより、移動量を計測すればよい。また、移動量として、車速を推定する場合を例に説明したが、移動距離を推定してもよく、また、移動ベクトルを推定してもよい。
【０１４７】
また、位置計測処理をオンラインで逐次処理する場合を例に説明したが、これに限定されるものではなく、自車両の運動の推定値及び移動量の推定値を取得した結果を用いて、オフラインで後処理として位置計測処理を行うようにしてもよい。
【図面の簡単な説明】
【０１４８】
【図１】本発明の第１の実施の形態に係る位置計測装置を示すブロック図である。
【図２】撮像画像に基づいて推定される角速度とジャイロセンサによって検出される角速度とを示すグラフである。
【図３】撮像画像に基づいて推定される角速度とジャイロセンサによって検出される角速度とを示すグラフである。
【図４】相対位置の変化量の算出方法を説明するための座標系を示すイメージ図である。
【図５】本発明の第１の実施の形態に係る位置計測装置における補正量算出処理ルーチンの内容を示すフローチャートである。
【図６】本発明の第１の実施の形態に係る位置計測装置における撮像画像に基づく運動推定処理ルーチンの内容を示すフローチャートである。
【図７】抽出された特徴点と移動体の運動との関係を説明するための図である。
【図８】本発明の第１の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第１の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第１の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【図１１】本発明の第１の実施の形態に係る位置計測装置によるオフセット補正を行った場合の相対位置の計測結果と、オフセット補正を行わなかった場合の相対位置の計測結果とを示す図である。
【図１２】本発明の第２の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図１３】本発明の第３の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第４の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図１５】本発明の第５の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【符号の説明】
【０１４９】
１０   位置計測装置
１２   運動計測部
１４   移動量計測部
１６   位置推定部
２０   画像撮像部
２２   ジャイロセンサ
２４   運動推定部
２６   車速センサ
２８   移動量推定部
【図１】
2009074859.tif 000005
【図２】
2009074859.tif 000006
【図３】
2009074859.tif 000007
【図４】
2009074859.tif 000008
【図５】
2009074859.tif 000009
【図６】
2009074859.tif 000010
【図７】
2009074859.tif 000011
【図８】
2009074859.tif 000012
【図９】
2009074859.tif 000013
【図１０】
2009074859.tif 000014
【図１１】
2009074859.tif 000015
【図１２】
2009074859.tif 000016
【図１３】
2009074859.tif 000017
【図１４】
2009074859.tif 000018
【図１５】
2009074859.tif 000019