JP2009074859A - 運動計測装置及び位置計測装置 - Google Patents

Abstract

【課題】簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができるようにする。
【解決手段】撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する（１６０）。そして、ジャイロセンサで検出された３軸角速度を取得し（１６２）、自車両の外部を撮像した複数の画像に基づいて算出された３軸角速度の平均値と、ジャイロセンサによって検出された３軸角速度の平均値との差に基づいて算出された補正量ΔＲを取得する（１６４）。取得した３軸角速度の各々について、角速度Ｒ_ｊｙｒｏから、取得した補正量ΔＲを減算して、補正した角速度Ｒ´_ｊｙｒｏを算出し（１６６）、補正された３軸角速度と、得られた並進方向を示す成分とを、自車両の運動の推定値として出力する（１６８）。
【選択図】図８

Description

本発明は、運動計測装置及び位置計測装置に係り、特に、移動体の運動を計測する運動計測装置、及び移動体の運動に基づいて移動体の位置を計測する位置計測装置に関する。

従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己（車）運動認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。また、遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて回転を求め、近傍用のステレオカメラを用いて並進を求める。また、２枚の画像間のオプティカルフローに基づいて、移動体の運動を検出する。
特開２００３−２４７８２４

しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、ロバスト性が得られない、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる運動計測装置及び位置計測装置を提供することを目的とする。

上記の目的を達成するために第１の発明に係る運動計測装置は、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する画像算出手段と、前記移動体の運動の前記３軸角速度の少なくとも１つを検出する角速度検出センサと、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段とを含んで構成されている。

第１の発明に係る運動計測装置によれば、画像算出手段によって、移動体の外部を撮像した複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つを算出する。ここで、移動体の外部を撮像した複数の画像は、単眼カメラで取得した少なくとも２枚の時系列画像である。

また、角速度検出センサによって、移動体の運動の３軸角速度の少なくとも１つを検出する。そして、運動推定手段によって、画像算出手段によって算出された３軸角速度の少なくとも１つと、角速度検出センサによって検出された３軸角速度の少なくとも１つとに基づいて、移動体の運動の３軸角速度の少なくとも１つを推定する。

このように、移動体の外部を撮像した複数枚の画像に基づいて算出された３軸角速度と、角速度検出センサによって検出された３軸角速度とに基づいて、移動体の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる。

上記の角速度検出センサを、ジャイロセンサとすることができる。

第１の発明に係る運動推定手段は、画像算出手段によって算出された３軸角速度の少なくとも１つに基づいて、角速度検出センサによって検出された３軸角速度の少なくとも１つの補正量を算出する補正量算出手段を備え、補正量算出手段によって算出された補正量に基づいて、角速度検出センサによって検出された３軸角速度の少なくとも１つを補正し、補正した３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。

ここで、補正量を、角速度検出センサによって検出される３軸角速度の少なくとも１つのオフセット量とすることができる。

これによって、角速度検出センサによって検出された３軸角速度のオフセット量を補正することができ、移動体の運動の３軸角速度を精度よく計測することができる。

上記の補正量算出手段は、画像算出手段によって算出された３軸角速度の少なくとも１つと、角速度検出センサによって検出された３軸角速度の少なくとも１つとの差に基づいて、補正量を算出することができる。これによって、角速度検出センサによって検出された３軸角速度のオフセット量に相当する補正量を算出することができる。

上記の運動計測装置は、画像算出手段によって算出された３軸角速度の少なくとも１つについて、該３軸角速度の少なくとも１つがどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段を更に含み、補正量算出手段は、信頼度算出手段によって算出された信頼度が所定値以上である３軸角速度の少なくとも１つに基づいて、補正量を算出することができる。これによって、信頼度の高い３軸角速度を用いて、精度よく補正量を算出することができる。

上記の補正量算出手段を備えた運動推定手段は、補正された３軸角速度の少なくとも１つと画像算出手段によって算出された３軸角速度の少なくとも１つとの差が所定値以上である場合、補正された３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定し、補正された３軸角速度の少なくとも１つと画像算出手段によって算出された３軸角速度の少なくとも１つとの差が所定値未満である場合、画像算出手段によって算出された３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。これによって、移動体の外部を撮像した画像に基づいて算出された３軸角速度と、補正された３軸角速度との差が小さい場合には、外部の画像に基づいて算出された３軸角速度を採用し、差が大きい場合には、補正された３軸角速度を採用して、ロバスト性を確保して、移動体の運動を精度よく計測することができる。

上記の補正量算出手段を備えた運動推定手段は、補正された３軸角速度の少なくとも１つの絶対値が所定値以上である場合、補正された３軸角速度の少なくとも１つを更に補正し、補正した３軸角速度の少なくとも１つを、移動体の運動の３軸角速度の少なくとも１つとして推定することができる。これによって、補正された３軸角速度の絶対値が大きすぎる場合には、更に補正することができるため、移動体の運動を精度よく計測することができる。

上記の運動計測装置は、移動体の外部を撮像する単眼の撮像手段を更に含み、画像算出手段は、撮像手段によって撮像された複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つを算出することができる。

上記の画像算出手段は、複数の画像に基づいて、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を算出し、運動推定手段は、画像算出手段によって算出された３軸角速度の少なくとも１つ及び並進方向と、角速度検出センサによって検出された３軸角速度の少なくとも１つとに基づいて、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定することができる。

第２の発明に係る位置計測装置は、上記の運動計測装置と、前記移動体の移動量を推定する移動量推定手段と、前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つと、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段とを含んで構成されている。

第２の発明に係る位置計測装置によれば、運動計測装置によって、移動体の運動の３軸角速度の少なくとも１つを推定し、また、移動量推定手段によって、移動体の移動量を推定する。

そして、位置算出手段によって、運動計測装置によって推定された移動体の運動の３軸角速度の少なくとも１つと、移動量推定手段によって推定された移動体の移動量とに基づいて、移動体の位置を算出する。

このように、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。

第３の発明に係る位置計測装置は、上記の移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定する運動計測装置と、前記移動体の移動量を推定する移動量推定手段と、前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段とを含んで構成されている。

第３の発明に係る位置計測装置によれば、運動計測装置によって、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、また、移動量推定手段によって、移動体の移動量を推定する。

そして、位置算出手段によって、運動計測装置によって推定された移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、移動量推定手段によって推定された移動体の移動量とに基づいて、移動体の位置を算出する。

このように、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。

以上説明したように、本発明の運動計測装置によれば、移動体の外部を撮像した複数の画像に基づいて算出された３軸角速度と、角速度検出センサによって検出された３軸角速度とに基づいて、移動体の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、移動体の運動を精度よく計測することができる、という効果が得られる。

本発明の位置計測装置によれば、ロバスト性を確保して精度よく計測された移動体の運動と、推定された移動体の移動量とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる、という効果が得られる。

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

図１に示すように、第１の実施の形態に係る位置計測装置１０は、自車両の運動を計測する運動計測部１２と、自車両の移動量を計測する移動量計測部１４と、計測された自車両の運動及び移動量に基づいて、自車両の位置を推定する位置推定部１６とを備えている。

運動計測部１２は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部２０と、自車両の運動に関連する情報として、自車両の３軸角速度（ピッチ角速度、ヨー角速度、ロール角速度）を検出するジャイロセンサ２２と、撮像画像及びジャイロセンサ２２の検出結果に基づいて、自車両の運動を推定する運動推定部２４とを備えている。

運動推定部２４では、自車両の運動として、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定する。なお、本実施の形態では、運動推定部２４によって、並進量（並進成分スカラ量）は推定しない。

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

また、ジャイロセンサ２２によって３軸角速度を検出する場合を例に説明するが、これに限定されるものではなく、ジャイロセンサ２２によって、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを検出するように構成してもよい。

ここで、ジャイロセンサ２２による検出結果のオフセット量について以下に説明する。通常、ジャイロセンサによって検出される３軸角速度にはオフセット量が存在し、検出された３軸角速度を補正せずにそのままの値を用いて、推測航法による位置推定を実施すると、誤差蓄積によって大きな位置誤差が生じる。また、オフセット量は、機器の温度などによって変動するため、予め設定した補正値を用いることは適切でない。そのため、ＧＰＳの測位結果や地図情報などに基づいて、オフセット量を算出し、オフセット量の補正が実施されるが、都心部などのＧＰＳの受信状態がよくない場所に自車両が存在する場合や、電源投入直後のＧＰＳの受信回数が限られている場合には、オフセット量を正しく算出することができず、オフセット量の補正を十分に行えない可能性がある。

また、カメラの撮像画像に基づいて、運動推定を行う場合、ＧＰＳの受信状態に関わらず、運動の推定が可能である。例えば、図２に示すように、ジャイロセンサ２２によって検出される角速度の時間変化と、単眼カメラの撮像画像に基づいて推定された角速度の時間変化とは、傾向が一致しているが、撮像画像に基づく角速度に対して、ジャイロセンサ２２によって検出される角速度にオフセット量が存在している。しかしながら、撮像画像中の安定した特徴点の有無や日照条件によって、複数の撮像画像に基づいて推定された運動の信頼性が変化する。例えば、図３に示すように、単眼カメラの撮像画像から推定される角速度は、安定して得られない場合がある。

次に、運動推定部２４における運動の推定方法について説明する。運動推定部２４では、単眼カメラのロバスト性を補い、ジャイロセンサ２２に生じるオフセットの影響やカーブ時の出力誤差を低減するために、単眼カメラの撮像画像に基づいて推定される運動と、ジャイロセンサ２２の検出結果とを相互に監視して、状況に応じて適切な運動を推定する。

まず、画像撮像部２０によって撮像された複数の撮像画像に基づいて、３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出する。また、ジャイロセンサ２２によって検出された３軸角速度を取得すると共に、撮像画像に基づいて推定された３軸角速度の信頼性を算出する。

そして、ジャイロセンサ２２によって検出される３軸角速度の値のバラつきを考慮して、一定期間に取得された角速度を平均し、また、撮像画像によって推定される３軸角速度の値のバラつきを考慮して、撮像画像に基づいて推定された一定期間の角速度を平均する。そして、これらの平均値を用いて、ジャイロセンサ２２のオフセット量を補正する。

ここで、画像撮像部２０の撮像画像に基づく３軸角速度の信頼度が、予め定められた閾値よりも低い場合、この３軸角速度の推定値には大きな誤差が含まれていると考えられるため、この信頼度が低い３軸角速度を除いて、３軸角速度の各々の平均値を算出する。また、画像撮像部２０の撮像画像に基づいて推定された３軸角速度、あるいは一時刻前に運動推定部２４によって推定された３軸角速度が、所定値以上の場合、撮像画像中の特徴点を安定して追従できずに誤差が大きくなってしまった可能性があるため、所定値以上の３軸角速度を除いて、３軸角速度の各々の平均値を算出する。

また、ジャイロセンサ２２によって検出された３軸角速度について、電源投入直後に検出された３軸角速度や所定値以上の３軸角速度である場合、検出された３軸角速度に大きな誤差が生じていると考えられるため、電源投入直後に検出された３軸角速度や所定値以上の３軸角速度を除いて、３軸角速度の各々の平均値を算出する。

上記のように算出された、ジャイロセンサ２２による角速度の一定時間Ｔ[ｓｅｃ]分の平均値_ａｖｅＲ_ｊｙｒｏと、撮像画像による角速度の一定時間Ｔ[ｓｅｃ]分の平均値_ａｖｅＲ_{ｉｍａｇｅ}との差ΔＲ（＝_ａｖｅＲ_ｊｙｒｏ− _ａｖｅＲ_{ｉｍａｇｅ}）を算出し、算出された差ΔＲを、ジャイロセンサ２２によって検出された角速度に生じるオフセット量とする。そして、ジャイロセンサ２２によって検出された角速度Ｒ_ｊｙｒｏからオフセット量ΔＲを減算し、減算した値Ｒ‘_ｊｙｒｏ（＝Ｒ_ｊｙｒｏ−ΔＲ）をジャイロセンサ２２によって検出された角速度の補正値とする。また、上記の補正量としてのオフセット量は、常時更新され、熱変動などに対応して、適切な角速度を算出する。

移動量計測部１４は、自車両の車速に関する情報を検出する車速センサ２６と、車速センサ２６によって検出された検出結果から移動量を推定する移動量推定部２８とを備えている。車速センサ２６は、例えば、車速に応じて車速パルス信号を出力する車速パルスセンサで構成され、自車両の対地速に応じた車速パルス信号を出力する。移動量推定部２８は、車速センサの検出結果から、自車両の移動量として並進成分スカラ量又は車速を推定する。例えば、計測単位期間Ｔ_ｍｓにおける車速パルス信号のパルス数に基づいて、車速を推定する。

位置推定部１６は、計測された自車両の運動及び移動量に基づいて、自車両の相対位置の変化量を算出し、過去の絶対位置と相対位置の変化量とに基づいて、自車両の絶対位置を推定する。以下に、相対位置の変化量を算出する方法について図４を用いて説明する。

まず、図４に示すような座標系において、初期位置を（Ｘ（０）、Ｙ（０）、Ｚ（０））とし、時刻ｔにおいて、移動量（速度Ｖ（ｔ））、運動（３軸角速度（ヨー角速度φ（ｔ）、ピッチ角速度θ（ｔ）、ロール角速度ψ（ｔ））、並進方向を示す成分（ａ_ｘ、ａ_ｙ、ａ_ｚ））が得られると、下記の（１）式のように、時刻ｔ＋Δｔにおける相対位置（Ｘ（ｔ＋Δｔ）、Ｙ（ｔ＋Δｔ）、Ｚ（ｔ＋Δｔ））を更新することができる。

ただし、ａ_ｘ／｜ａ｜、ａ_ｙ／｜ａ｜、及びａ_ｚ／｜ａ｜は、正規化された並進方向を示す成分の各々を表わす。

そして、自車両に搭載されたＧＰＳ（図示省略）から、経度及び緯度などで表わされる絶対位置情報が過去に取得されていた場合には、絶対位置情報が示す絶対位置と、算出された相対位置とに基づいて、自車両の現在の絶対位置を計測する。なお、絶対位置情報を、必ずしもＧＰＳの測位位置から取得する必要はなく、例えば路側の位置が既知である設備から、通信などによって移動体の絶対位置情報を間欠的に取得してもよいし、予め位置が既知であるランドマーク情報が登録された地図などから、自車両の位置を取得してもよい。また、ＧＰＳと同じ頻度で取得する必要もなく、移動量と運動とで補完できる間隔で取得すればよく、取得手段も問わない。

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

まず、位置計測装置１０において、図５に示す補正量算出処理ルーチンが実行される。ステップ１００において、撮像画像に基づく運動推定処理が行われる。上記ステップ１００は、図６に示す撮像画像に基づく運動推定処理ルーチンによって実現される。以下に、撮像画像に基づく運動推定処理ルーチンについて説明する。

まず、ステップ１４０において、異なるタイミングで画像撮像部２０によって自車両前方を撮像した第１の画像及び第２の画像を取得し、ステップ１４２において、上記ステップ１４０で取得した第１の画像を複数の領域に分割する。このステップ１４２では、例えば、予め与えられた定数ｙ１、ｙ２を各領域の境界線（ｙ軸の切片座標）として、第１の画像を、画像上段の上方領域と、中段の中間領域と、下段の下方領域との３つの領域に分割する。

次のステップ１４４において、上記ステップ１４２で得られた各分割画像中から、それぞれ所定の数だけ特徴点（不動点であることが期待される点）を抽出する。ここで、特徴点とは、周囲の点とは区別でき、追跡が容易な点のことである。このステップ１４４では、例えば、二次元的に濃淡変化の勾配値が大きくなる画素を検出する、一般的な周知の方法（例えば、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に特徴点を抽出する。

そして、ステップ１４６において、上記ステップ１４４で抽出された第１の画像における特徴点の各々を、上記ステップ１４０で取得した第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検出する。このとき、２時刻間にて対応する２つの特徴点は、同一の不動点であることが期待されているので、第１の画像と第２の画像とで対応する点とその周囲の点との輝度は、殆ど変化しないという仮定を用い、この仮定に基づいて各特徴点の追跡を行う。この様な追跡処理を実施する際には、例えば、周知のルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などを用いると良い。また、各特徴点の追跡処理の後、第１の画像と第２の画像との間で対応する特徴点の各組の各画像内座標を、１つの適当な形式に整理する。

そして、ステップ１４８において、上記ステップ１４６で得られた２時刻間で互いに対応する特徴点の組から、２つの撮像時刻間における画像撮像部２０の相対的な位置関係、即ち、画像撮像部２０を搭載した自車両のその２時刻間における運動に関連する情報を推定する。

以下、図７を用いて、抽出された特徴点（不動点）と移動体の運動との関係を説明する。図７に示すように、移動体の運動は、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとから構成される。例えば、第１の画像上の特徴点ｘ_１に対応する第２の画像上の特徴点をｘ_２とすると、これらの特徴点は、同一の不動点を示す点である。

ここで、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、対応する２つの特徴点の関係を、以下の（２）式〜（４）式で表すことができる。

上記の（２）式〜（４）式のように、上記の２時刻間における８組以上の互いに対応する特徴点の組（即ち、同一の不動点）が求まっていれば、それに基づいて、上記の基礎行列Ｆを算出することができる。そして、カメラのキャリブレーション行列Ｋが既知であれば、上記の式からも分かる様に、その２時刻間における移動体の運動、即ち、その２時刻間における自車両の回転行列Ｒと並進ベクトルｔとを求めることができる。

そして、ステップ１５０において、上記ステップ１４８で求めた自車両の回転行列Ｒと並進ベクトルｔとに基づいて、自車両の３軸角速度と並進方向を示す成分とを算出し、算出した３軸角速度と並進方向を示す成分とを、自車両の運動の推定値として出力して、撮像画像に基づく運動推定処理ルーチンを終了する。

そして、上記図５のステップ１０２において、上記ステップ１００で推定された３軸角速度がどれだけ信頼できるかを示す信頼度を算出する。このステップ１０２では、例えば、上記ステップ１００の撮像画像に基づく運動推定処理において、第１の画像と第２の画像との間で対応付けられた特徴点の数が多いほど、高い信頼度が算出される。すなわち、どれだけ特徴点が照合できたかを表わす度合いを、信頼度として算出する。

そして、ステップ１０４において、ジャイロセンサ２２で検出された３軸角速度を取得する。なお、以下のステップ１０６〜ステップ１２４の説明では、３軸角速度の何れかの角速度Ｒ_ｊｙｒｏについて説明する。

ステップ１０６で、上記ステップ１０４で取得した角速度Ｒ_ｊｙｒｏが、予め定められた閾値Ｒ_ｔｈ未満であるか否かを判定する。上記ステップ１０６で、角速度Ｒ_ｊｙｒｏが、閾値Ｒ_ｔｈ未満であると判定された場合には、ステップ１０８において、上記ステップ１０４で取得した角速度Ｒ_ｊｙｒｏを、平均計算対象区間Ｔ_{ａｖｅ_ｊｙｒｏ}の角速度として採用する。一方、上記ステップ１０６で、角速度Ｒ_ｊｙｒｏが、閾値Ｒ_ｔｈ以上であると判定された場合には、ステップ１１０において、上記ステップ１０４で取得した角速度Ｒ_ｊｙｒｏを、平均計算対象区間Ｔ_{ａｖｅ_ｊｙｒｏ}の角速度として採用せずに、前回求められた平均計算対象区間Ｔ_{ａｖｅ_ｊｙｒｏ}の角速度を保持する。

そして、ステップ１１２において、平均計算対象区間Ｔ_{ａｖｅ_ｊｙｒｏ}の角速度を平均して、角速度の平均値_ａｖｅＲ_ｊｙｒｏを算出する。

次のステップ１１４では、上記ステップ１０２で算出された信頼度が、予め定められた閾値Ｐ_ｔｈより大きいか否かを判定し、信頼度が、閾値Ｐ_ｔｈより大きい場合には、ステップ１１６で、上記ステップ１００で推定された角速度Ｒ_{ｉｍａｇｅ}が、予め定められた閾値Ｒ_ｔｈ未満であるか否かを判定する。上記ステップ１１６で、角速度Ｒ_{ｉｍａｇｅ}が、閾値Ｒ_ｔｈ未満であると判定された場合には、ステップ１１８において、上記ステップ１００で取得した角速度Ｒ_{ｉｍａｇｅ}を、平均計算対象区間Ｔ_{ａｖｅ_ｉｍａｇｅ}の角速度として採用して、ステップ１２２へ移行する。一方、上記ステップ１１６で、角速度Ｒ_{ｉｍａｇｅ}が、閾値Ｒ_ｔｈ以上であると判定された場合には、得られた角速度が大きいと判断し、ステップ１２０へ移行する。また、上記ステップ１１４において、算出された信頼度が、閾値Ｐ_ｔｈ未満である場合には、得られた角速度の信頼度が低いと判断し、ステップ１２０へ移行する。

ステップ１２０では、上記ステップ１００で取得した角速度Ｒ_{ｉｍａｇｅ}を、平均計算対象区間Ｔ_{ａｖｅ_ｉｍａｇｅ}の角速度として採用せずに、前回求められた平均計算対象区間Ｔ_{ａｖｅ_ｉｍａｇｅ}の角速度を保持して、ステップ１２２へ移行する。

そして、ステップ１２２では、平均計算対象区間Ｔ_{ａｖｅ_ｉｍａｇｅ}の角速度を平均して、角速度の平均値_ａｖｅＲ_{ｉｍａｇｅ}を算出し、ステップ１２４において、上記ステップ１１２で算出された角速度の平均値_ａｖｅＲ_ｊｙｒｏから、上記ステップ１２２で算出された角速度の平均値_ａｖｅＲ_{ｉｍａｇｅ}を減算して、平均値の差ΔＲ（＝_ａｖｅＲ_ｊｙｒｏ−_ａｖｅＲ_{ｉｍａｇｅ}）を補正量として算出して、メモリ（図示省略）に記憶し、補正量算出処理ルーチンを終了する。

なお、上記ステップ１０６〜ステップ１２４の処理は、ジャイロセンサ２２が出力する３軸角速度の各々について実行され、３軸角速度の各々について補正量が算出されて、メモリ（図示省略）に記憶される。また、上述した補正量算出処理ルーチンは、定期的に実行され、３軸角速度の補正量が、定期的に更新される。

なお、各判定に必要な閾値は、経験的に決定した定数でもよいし、データ群から数値解析によって決定した定数でもよい。また、データや周辺状況に応じて動的に変動するような機構をいれて、閾値を決定するようにしてもよい。

また、位置計測装置１０において、図８に示す運動推定処理ルーチンが実行される。まず、ステップ１６０において、上述したステップ１００と同様に、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、補正量算出処理ルーチンで算出され、メモリに記憶された３軸角速度各々の補正量ΔＲを取得する。

次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒ_ｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´_ｊｙｒｏを算出し、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

また、位置計測装置１０において、図９に示す移動量推定処理ルーチンが実行される。まず、ステップ１７０で、計測単位期間Ｔ_ｍｓに車速センサ２６から出力される車速パルスのパルス数をカウントする。そして、ステップ１７２において、上記ステップ１７０でカウントされたパルス数に基づいて、自車両の車速を推定し、ステップ１７４において、上記ステップ１７２で推定された自車両の車速を、自車両の移動量の推定値として出力して、移動量推定処理ルーチンを終了する。

また、位置計測装置１０において、図１０に示す位置計測処理ルーチンが実行される。まず、ステップ１８０で、ＧＰＳによって自車両の位置を測位し、測位した位置を初期位置として設定する。

そして、ステップ１８２において、上述した運動推定処理ルーチンを実行して、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分の推定値を取得し、ステップ１８４において、上記の移動量推定処理ルーチンを実行して、自車両の移動量としての車速の推定値を取得する。

そして、ステップ１８６において、上記ステップ１８２で取得した自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、上記ステップ１８４で取得した自車両の車速とに基づいて、自車両の相対位置の変化量を算出する。次のステップ１８８では、上記ステップ１８０で設定された初期位置と、上記ステップ１８６で算出された相対位置の変化量とに基づいて、自車両の絶対位置を算出する。

そして、ステップ１９２において、上記ステップ１８２からステップ１８８によって絶対位置を計測してから、計測単位期間Ｔ_ｍｓが経過したか否かを判定し、計測単位期間Ｔ_ｍｓが経過すると、ステップ１８２へ戻り、再び自車両の絶対位置を計測して、自車両の絶対位置を更新する。

次に、本実施の形態に係る位置計測装置１０の計測結果について説明する。初期位置として真値を用いて、ジャイロセンサ２２によって検出された３軸角速度について、オフセット量の補正を行って、相対的な位置の計測を行った。また、比較対象として、初期位置として真値を用いて、ジャイロセンサ２２によって検出された３軸角速度について、オフセット量の補正を行わずに、相対的な位置の計測を行った。

一般的なジャイロセンサの検出結果には、その特性により、オフセット誤差や回転時の振幅誤差があるため、図１１に示すように、オフセットを補正した場合の計測結果と、補正しなかった場合の計測結果とを比較すると、オフセット誤差を補正しない場合は、極端に軌跡が広がったり、旋回方向によっては小さくなったりする。一方、撮像画像に基づいて、随時オフセット誤差を修正して、相対的な位置を計測すると、真値の軌跡の形状に比較的近い軌跡が得られることがわかった。

以上説明したように、第１の実施の形態に係る位置計測装置によれば、自車両の外部を撮像した複数の画像に基づいて算出された３軸角速度の平均値と、ジャイロセンサによって検出された３軸角速度の平均値との差を、補正量として算出し、算出した補正量によって、ジャイロセンサによって検出された３軸角速度のオフセット量を補正して、自車両の運動の３軸角速度を推定することにより、簡易な構成で、ロバスト性を確保して、自車両の運動の３軸角速度を精度よく計測することができる。

また、複数の撮像画像に基づいて算出され、かつ、信頼度の高い３軸角速度の平均値を用いて、ジャイロセンサのオフセット量に相当する補正量を算出するため、精度よく補正量を算出することができる。

また、ロバスト性を確保して精度よく計測された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。

また、車両に搭載されている単眼カメラとジャイロセンサとを用いることで、通常のカーナビで用いられるジャイロを用いるよりも高精度な位置推定を実現することができる。

また、車両外部の撮像画像は、照明環境にも影響を受け易いため、撮像画像に基づいて自車両の運動を算出できない場合があったが、ジャイロセンサを併せて用いることで、自車両の運動を常に計測することができる。

従来、ジャイロセンサをＧＰＳと併せて利用する場合は、ＧＰＳで得られる角度方向の変化量とジャイロセンサで得られる変化量を監視して、ジャイロセンサのオフセット量の補正を行っていたが、ＧＰＳの測位間隔は通常１Ｈｚと長く、また測位精度も高くないため、角度方向の変動が大きかった。また、都心部などでは測位頻度が低下するため、安定してオフセット補正を行なうことができない問題があった。一方、本実施の形態では、単眼カメラの撮像画像を用いて３軸角速度を推定することで、精度の高い３軸角速度をカメラの撮像レートで取得できるため、ジャイロセンサのオフセット量を効果的に補正することができる。

次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。

第２の実施の形態では、撮影画像に基づいて推定される３軸角速度の信頼度が高い場合、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値としている点が第１の実施の形態と異なっている。

第２の実施の形態に係る位置計測装置１０の運動推定部２４では、画像撮像部２０の撮像画像に基づく３軸角速度に対する信頼度を算出し、算出した信頼度が所定値以上である場合には、ジャイロセンサ２２で検出された３軸角速度を用いずに、画像撮像部２０の撮像画像に基づいて推定される３軸角速度を、自車両の運動の推定値として出力する。

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

まず、ステップ１６０において、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ２００において、上記ステップ１０２と同様に、上記ステップ１００で推定された３軸角速度に対する信頼度を算出し、ステップ２０２において、上記ステップ２００で算出された信頼度が、予め定められた閾値Ｐ_ｔｈより大きいか否かを判定する。

上記ステップ２０２で、信頼度が、閾値Ｐ_ｔｈより大きいと判定された場合には、撮像画像に基づいて推定された推定値の信頼度が高いと判断し、ステップ２０４で、上記ステップ１６０で算出された３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

一方、上記ステップ２０２で、信頼度が、閾値Ｐ_ｔｈ以下であると判定された場合には、撮像画像に基づいて推定された推定値の信頼度が低いと判断し、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、上述した補正量算出処理ルーチンで算出され、メモリに記憶された３軸角速度の各々の補正量ΔＲを取得する。

次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒ_ｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´_ｊｙｒｏを算出し、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。

このように、撮影画像に基づいて推定される３軸角速度に対する信頼度が高い場合に、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値とし、撮影画像に基づいて推定される３軸角速度の信頼度が低い場合に、ジャイロセンサから検出された各角速度の補正値を、自車両の運動の推定値とすることにより、簡易な構成で、ロバスト性を確保して、精度よく自車両の運動を推定することができる。

なお、３軸角速度の全てがジャイロセンサから出力されない場合には、ジャイロセンサから検出された角速度の補正値と、撮像画像に基づいて推定された他の角速度とを、自車両の運動の推定値として出力すればよい。

次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。

第３の実施の形態では、算出される補正量によって補正されたジャイロセンサからの３軸角速度と、撮像画像に基づいて推定される３軸角速度との差が、小さい場合には、撮像画像に基づく３軸角速度の推定値を、自車両の運動の推定値としている点と、補正されたジャイロセンサからの３軸角速度の絶対値が、所定値以上である場合には、補正された３軸角速度を更に補正して、補正した３軸角速度を、自車両の運動の推定値としている点とが、主に第１の実施の形態と異なっている。

第３の実施の形態に係る位置計測装置１０の運動推定部２４では、画像撮像部２０の撮像画像に基づく３軸角速度と、ジャイロセンサ２２から検出された３軸角速度の補正値との差が、一定量Ｄ_ｔｈ以上になる場合は、特に単眼カメラの撮像画像に基づく推定値に誤差があると判断し、ジャイロセンサ２２から検出された３軸角速度の補正値を、自車両の運動の推定値として出力する。

また、運動推定部２４は、ジャイロセンサ２２から検出された３軸角速度の補正値の絶対量が、一定量Ｒ_ｔｈ以上である場合は、絶対量が真値よりも大きくなっている可能性が高いと判断し、３軸角速度の補正値に所定の補正係数を乗じた値を、自車両の運動の推定値として出力する。

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

まず、ステップ１６０において、撮像画像に基づく運動推定処理を行い、自車両の運動の３軸角速度及び並進方向を示す成分の推定値を算出する。そして、ステップ１６２において、ジャイロセンサ２２で検出された３軸角速度を取得し、ステップ１６４において、上述した補正量算出処理ルーチンで算出された３軸角速度の各々の補正量ΔＲを取得する。

次のステップ１６６では、上記ステップ１６２で取得した３軸角速度の各々について、角速度Ｒ_ｊｙｒｏから、上記ステップ１６４で取得した補正量ΔＲを減算して、補正した角速度Ｒ´_ｊｙｒｏを算出する。そして、ステップ３００において、３軸角速度の各々について、補正した角速度Ｒ´_ｊｙｒｏと、上記ステップ１６０で得られた角速度Ｒ_{ｉｍａｇｅ}との差の絶対値（＝｜Ｒ´_ｊｙｒｏ−Ｒ_{ｉｍａｇｅ}｜）が、予め定められた閾値Ｄ_ｔｈ未満であるか否かを判定する。上記ステップ３００で、角速度Ｒ´_ｊｙｒｏと角速度Ｒ_{ｉｍａｇｅ}との差の絶対値が、閾値Ｄ_ｔｈ未満であると判定されると、ステップ２０４で、上記ステップ１６０で算出された３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

一方、上記ステップ３００で、角速度Ｒ´_ｊｙｒｏと角速度Ｒ_{ｉｍａｇｅ}との差の絶対値が、閾値Ｄ_ｔｈ以上であると判定されると、３軸角速度の各々について、上記ステップ１６６で補正された角速度Ｒ´_ｊｙｒｏの絶対値が、予め定められた閾値Ｒ_ｔｈ未満であるか否かを判定する。上記ステップ３０２で、補正された角速度Ｒ´_ｊｙｒｏの絶対値が、閾値Ｒ_ｔｈ未満であると判定されると、ステップ１６８において、上記ステップ１６６で補正された３軸角速度と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

一方、上記ステップ３０２で、補正された角速度Ｒ´_ｊｙｒｏの絶対値が、閾値Ｒ_ｔｈ以上であると判定されると、ステップ３０４で、３軸角速度の各々について、補正された角速度Ｒ´_ｊｙｒｏが小さくなるように、所定の補正係数ｃｏｒｒ（ｃｏｒｒ＜１）を乗じて、角速度Ｒ´_ｊｙｒｏを更に補正する。そして、次のステップ３０６で、上記ステップ３０４で補正された３軸角速度（＝Ｒ´_ｊｙｒｏ×ｃｏｒｒ）と、上記ステップ１６０で得られた並進方向を示す成分とを、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。

なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。

このように、自車両の外部を撮像した複数の撮像画像に基づいて算出された３軸角速度と、補正された３軸角速度との差が小さい場合には、撮像画像に基づいて算出された３軸角速度を自車両の運動の推定値として採用し、差が大きい場合には、補正された３軸角速度を自車両の運動の推定値として採用することにより、ロバスト性を確保して、自車両の運動を精度よく推定することができる。

また、補正された３軸角速度の絶対値が大きすぎる場合には、更に補正して、補正された３軸角速度を自車両の運動の推定値とすることにより、ロバスト性を確保して、自車両の運動を精度よく推定することができる。

次に、第４の実施の形態について説明する。なお、第４の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。

第４の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の運動の３軸角速度を推定している点が第１の実施の形態と異なっている。

第４の実施の形態に係る位置計測装置１０の運動推定部２４では、ジャイロセンサ２２によって検出された３軸角速度と、画像撮像部２０の撮像画像に基づいて推定される３軸角速度とを観測値とし、自車両の運動の３軸角速度、３軸角速度各々の角加速度、及びジャイロセンサ２２の検出値のオフセット量である補正量を推定値として、カルマンフィルタによる最適処理手法を用いた処理を行う。

カルマンフィルタによる最適処理手法では、画像撮像部２０の単眼カメラの撮像画像に基づく観測値ｙ_{ｉｍａｇｅ}が得られた場合に、観測値ｙ_{ｉｍａｇｅ}のバラつきを考慮して、観測ノイズｗ_{ｉｍａｇｅ}を設定し、観測行列Ｈ_{ｉｍａｇｅ}について、観測値ｙ_{ｉｍａｇｅ}と推定値ｘとの関係式から、観測行列Ｈ_{ｉｍａｇｅ}を設定する。

また、ジャイロセンサ２２から観測値ｙ_ｊｙｒｏが得られた場合に、観測値ｙ_ｊｙｒｏのバラつきを考慮して、観測ノイズｗ_ｊｙｒｏを設定し、観測行列Ｈ_ｊｙｒｏについて、観測値ｙ_ｊｙｒｏと推定値ｘとの関係式から、観測行列Ｈ_ｊｙｒｏを設定する。

そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。

次に、第４の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。

第４の実施の形態に係る位置計測装置１０では、上述した撮像画像に基づく運動推定処理ルーチンが繰り返し実行され、推定された３軸角速度及び並進方向を示す成分が繰り返し出力される。また、ジャイロセンサ２２から３軸角速度が繰り返し検出される。

また、位置計測装置１０において、図１４に示す運動推定処理ルーチンが実行される。まず、ステップ４００において、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ４０２において、ジャイロセンサ２２から検出された３軸角速度を取得する。

そして、ステップ４０４において、上記ステップ４００で取得した３軸角速度と、上記ステップ４０２で取得した３軸角速度とに基づいて、３軸角速度、３軸角速度の各々の角加速度、及び３軸角速度の各々の補正量の推定値ｘに対して、初期値を設定する。次のステップ４０６では、時間ｔ_ｉ−１にΔｔを加算して、時間ｔ_ｉに更新する。そして、ステップ４０８において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。

次のステップ４１０において、現タイミングに、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度、又はジャイロセンサ２２から検出された３軸角速度が、観測値として入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ４１２において、上記ステップ４０８で算出された推定値ｘの予測値の３軸角速度、及び上記ステップ４００で取得した並進方向を示す成分を、自車両の運動の推定値として出力し、ステップ４０６へ戻る。

上記ステップ４１０において、現タイミングに観測値が入力されたと判定された場合には、ステップ４１４において、観測値として、ジャイロセンサ２２から検出された３軸角速度が入力されたのか否かを判定する。

上記ステップ４１４で、観測値としてジャイロセンサ２２から検出された３軸角速度が入力されたと判定された場合には、ステップ４１６において、入力された３軸角速度を観測値として、観測行列Ｈ_ｊｙｒｏ、観測値ｙ_ｊｙｒｏ、及び観測ノイズｗ_ｊｙｒｏを設定する。

一方、上記ステップ４１４で、観測値として、撮像画像に基づく運動推定処理ルーチンによって推定された３軸角速度が入力されたと判定された場合には、ステップ４１８において、入力された３軸角速度を観測値として、観測行列Ｈ_{ｉｍａｇｅ}、観測値ｙ_{ｉｍａｇｅ}、及び観測ノイズｗ_{ｉｍａｇｅ}を設定する。

そして、ステップ４２０において、上記ステップ４１６又はステップ４１８で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ４２２では、上記ステップ４２０で算出された推定値ｘの３軸角速度、及び上記ステップ４００で取得した並進方向を示す成分を、自車両の運動の推定値として出力し、ステップ４０６へ戻る。

また、第１の実施の形態と同様に、移動量推定処理ルーチンが実行され、自車両の移動量の推定値としての車速が算出される。また、上記の運動推定処理ルーチンから出力される自車両の運動の推定値、及び移動量推定処理ルーチンから出力される自車両の車速の推定値を用いて、第１の実施の形態と同様に、位置計測処理ルーチンが実行され、自車両の絶対位置が計測される。

このように、カルマンフィルタを用いた最適処理手法によって、自車両の外部を撮像した複数の撮像画像に基づいて算出された３軸角速度及び並進方向と、ジャイロセンサによって検出された３軸角速度とに基づいて、自車両の運動の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の運動を精度よく計測することができる。

また、ロバスト性を確保して精度よく計測された自車両の運動と、推定された自車両の車速とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。

なお、上記の実施の形態では、カルマンフィルタで推定する例を記述したが、周知のパーティクルフィルタなど、他の最適推定手法を用いてもよく、適切に解を推定できる手法であれば、その方法はこの限りではない。

また、上記の実施の形態において、誤差共分散を、推定された自車両の運動の推定値に対する信頼度として用いてもよい。この場合には、例えば、推定値と共に算出された誤差共分散によって、推定値の誤差が大きいと判断される場合には、この推定値を、自車両の運動の推定値として採用しないようにすることができる。

次に、第５の実施の形態について説明する。なお、第５の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。

第５の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の位置を計測している点が第１の実施の形態と異なっている。

第５の実施の形態に係る位置計測装置１０の位置推定部１６では、運動推定部２４によって推定された運動（３軸角速度、並進方向を示す成分）、移動量推定部２８によって推定された移動量（車速）、及びＧＰＳによって測位された絶対位置(Ｘｇ,Ｙｇ,Ｚｇ)を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行う。

カルマンフィルタを用いた最適処理手法では、運動推定部２４によって推定された運動の観測値ｙ_ｍｏｖｅが得られた場合に、観測値ｙ_ｍｏｖｅのバラつきを考慮して、観測ノイズｗ_ｍｏｖｅを設定し、観測行列Ｈ_ｍｏｖｅについて、観測値ｙ_ｍｏｖｅと推定値ｘとの関係式から、観測行列Ｈ_ｍｏｖｅを設定する。

また、移動量推定部２８によって推定された移動量の観測値ｙ_ｔｒａｖが得られた場合に、観測値ｙ_ｔｒａｖのバラつきを考慮して、観測ノイズｗ_ｔｒａｖを設定し、観測行列Ｈ_ｔｒａｖについて、観測値ｙ_ｔｒａｖと推定値ｘとの関係式から、観測行列Ｈ_ｔｒａｖを設定する。

また、ＧＰＳによって測位された絶対位置の観測値ｙ_ｇｐｓが得られた場合に、観測値ｙ_ｇｐｓのバラつきを考慮して、観測ノイズｗ_ｇｐｓを設定し、観測行列Ｈ_ｇｐｓについて、観測値ｙ_ｇｐｓと推定値ｘとの関係式から、観測行列Ｈ_ｇｐｓを設定する。

そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。

次に、第５の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。

第５の実施の形態に係る位置計測装置１０では、第１の実施の形態で説明した運動推定処理ルーチンが繰り返し実行され、また、上述した移動量推定処理ルーチンが繰り返し実行される。また、ＧＰＳによって、所定時間ごと（例えば１秒ごと）に自車両の絶対位置が測位される。

また、位置計測装置１０において、図１５に示す位置計測処理ルーチンが実行される。まず、ステップ５００において、運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ５０２において、移動量推定処理ルーチンによって推定された車速を取得する。次のステップ５０４では、ＧＰＳによって測位された自車両の絶対位置を取得する。

そして、ステップ５０６において、上記ステップ５００で取得した３軸角速度及び並進方向を示す成分と、上記ステップ５０２で取得した車速と、上記ステップ５０４で取得した絶対位置とに基づいて、絶対位置、方位角、車速、及び３軸角速度の推定値ｘに対して、初期値を設定する。次のステップ５０８では、時間ｔ_ｉ−１にΔｔを加算して、時間ｔ_ｉに更新する。そして、ステップ５１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。

次のステップ５１２において、現タイミングに、観測値として、運動推定処理ルーチンによって推定された３軸角速度、移動推定処理ルーチンによって推定された車速、又はＧＰＳによって測位された絶対位置が入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ５１４において、上記ステップ５１０で算出された推定値ｘの予測値の絶対位置を、自車両の絶対位置の推定値として出力し、ステップ５０８へ戻る。

上記ステップ５１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ５１６において、上記ステップ５１２でどの観測値が入力されたかを判定する。

上記ステップ５１６で、観測値として、運動推定処理ルーチンで推定された３軸角速度及び並進方向を示す成分が入力されたと判定された場合には、ステップ５１８において、入力された３軸角速度及び並進方向を示す成分を観測値として、観測行列Ｈ_ｍｏｖｅ、観測値ｙ_ｍｏｖｅ、及び観測ノイズｗ_ｍｏｖｅを設定する。

一方、上記ステップ５１６で、観測値として、移動量推定処理ルーチンによって推定された車速が入力されたと判定された場合には、ステップ５２０において、入力された車速を観測値として、観測行列Ｈ_ｔｒａｖ、観測値ｙ_ｔｒａｖ、及び観測ノイズｗ_ｔｒａｖを設定する。

また、上記ステップ５１６で、観測値として、ＧＰＳによって測位された絶対位置が入力されたと判定された場合には、ステップ５２２において、入力された絶対位置を観測値として、観測行列Ｈ_ｇｐｓ、観測値ｙ_ｇｐｓ、及び観測ノイズｗ_ｇｐｓを設定する。

そして、ステップ５２４において、上記ステップ５１８、ステップ５２０、又はステップ５２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ５２６では、上記ステップ５２４で算出された推定値ｘの絶対位置を、自車両の絶対位置の推定値として出力し、ステップ５０８へ戻る。

このように、カルマンフィルタを用いた最適処理手法によって、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速と、ＧＰＳによって測位された位置とに基づいて、自車両の絶対位置の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の絶対位置を精度よく計測することができる。

なお、上記の実施の形態では、カルマンフィルタを用いた位置計測処理において、推定された自車両の運動の３軸角速度及び並進方向を示す成分、推定された自車両の車速、及びＧＰＳによって測位された絶対位置が、観測値として入力される場合を例に説明したが、これに限定されるものではない。例えば、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、及び推定された自車両の車速とに基づいて推定された自車両の相対位置の変化量が、観測値として、カルマンフィルタを用いた位置計測処理に入力されるようになっていてもよい。この場合には、推定された自車両の相対位置の変化量、及びＧＰＳによって測位された絶対位置を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行うようにすればよい。

また、カルマンフィルタに限らず、パーティクルフィルタなどの最適推定手法を用いてもよい。

また、上記の第１の実施の形態〜第５の実施の形態では、車速センサを用いて、移動量としての車速を推定する場合を例に説明したが、これに限定されるものではなく、他の既知の方法によって、自車両の移動量を推定してもよい。例えば、ＧＰＳからの信号によって自車両の移動量を推定してもよく、また、単眼カメラの撮像画像から、移動量を推定してもよい。単眼カメラの撮像画像から移動量を推定する場合には、撮像画像から復元した３次元特徴点空間の路面を仮定することにより、移動量を計測すればよい。また、移動量として、車速を推定する場合を例に説明したが、移動距離を推定してもよく、また、移動ベクトルを推定してもよい。

また、位置計測処理をオンラインで逐次処理する場合を例に説明したが、これに限定されるものではなく、自車両の運動の推定値及び移動量の推定値を取得した結果を用いて、オフラインで後処理として位置計測処理を行うようにしてもよい。

本発明の第１の実施の形態に係る位置計測装置を示すブロック図である。 撮像画像に基づいて推定される角速度とジャイロセンサによって検出される角速度とを示すグラフである。 撮像画像に基づいて推定される角速度とジャイロセンサによって検出される角速度とを示すグラフである。 相対位置の変化量の算出方法を説明するための座標系を示すイメージ図である。 本発明の第１の実施の形態に係る位置計測装置における補正量算出処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る位置計測装置における撮像画像に基づく運動推定処理ルーチンの内容を示すフローチャートである。 抽出された特徴点と移動体の運動との関係を説明するための図である。 本発明の第１の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る位置計測装置によるオフセット補正を行った場合の相対位置の計測結果と、オフセット補正を行わなかった場合の相対位置の計測結果とを示す図である。 本発明の第２の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第４の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。 本発明の第５の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。

符号の説明

１０ 位置計測装置
１２ 運動計測部
１４ 移動量計測部
１６ 位置推定部
２０ 画像撮像部
２２ ジャイロセンサ
２４ 運動推定部
２６ 車速センサ
２８ 移動量推定部

Claims (12)

1. 移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する画像算出手段と、
   前記移動体の運動の前記３軸角速度の少なくとも１つを検出する角速度検出センサと、
   前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、
   を含む運動計測装置。
2. 前記運動推定手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つに基づいて、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つの補正量を算出する補正量算出手段を備え、前記補正量算出手段によって算出された補正量に基づいて、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つを補正し、前記補正した前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項１記載の運動計測装置。
3. 前記補正量算出手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つと、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとの差に基づいて、前記補正量を算出する請求項２記載の運動計測装置。
4. 前記画像算出手段によって算出された前記３軸角速度の少なくとも１つについて、該３軸角速度の少なくとも１つがどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段を更に含み、
   前記補正量算出手段は、前記信頼度算出手段によって算出された前記信頼度が所定値以上である前記３軸角速度の少なくとも１つに基づいて、前記補正量を算出する請求項２又は３記載の運動計測装置。
5. 前記運動推定手段は、前記補正された前記３軸角速度の少なくとも１つと前記画像算出手段によって算出された前記３軸角速度の少なくとも１つとの差が所定値以上である場合、前記補正された前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定し、前記補正された前記３軸角速度の少なくとも１つと前記画像算出手段によって算出された前記３軸角速度の少なくとも１つとの差が所定値未満である場合、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項２〜請求項４の何れか１項記載の運動計測装置。
6. 前記運動推定手段は、前記補正された前記３軸角速度の少なくとも１つの絶対値が所定値以上である場合、前記補正された前記３軸角速度の少なくとも１つを更に補正し、前記補正した前記３軸角速度の少なくとも１つを、前記移動体の運動の３軸角速度の少なくとも１つとして推定する請求項２〜請求項５の何れか１項記載の運動計測装置。
7. 前記補正量を、前記角速度検出センサによって検出される前記３軸角速度の少なくとも１つのオフセット量とした請求項２〜請求項６の何れか１項記載の運動計測装置。
8. 前記角速度検出センサを、ジャイロセンサとした請求項１〜請求項７の何れか１項記載の運動計測装置。
9. 前記移動体の外部を撮像する単眼の撮像手段を更に含み、
   前記画像算出手段は、前記撮像手段によって撮像された複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つを算出する請求項１〜請求項８の何れか１項記載の運動計測装置。
10. 前記画像算出手段は、前記複数の画像に基づいて、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を算出し、
    前記運動推定手段は、前記画像算出手段によって算出された前記３軸角速度の少なくとも１つ及び並進方向と、前記角速度検出センサによって検出された前記３軸角速度の少なくとも１つとに基づいて、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定する請求項１〜請求項９の何れか１項記載の運動計測装置。
11. 請求項１〜請求項９の何れか１項記載の運動計測装置と、
    前記移動体の移動量を推定する移動量推定手段と、
    前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つと、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段と、
    を含む位置計測装置。
12. 請求項１０記載の運動計測装置と、
    前記移動体の移動量を推定する移動量推定手段と、
    前記運動計測装置によって推定された前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向と、前記移動量推定手段によって推定された前記移動体の移動量とに基づいて、前記移動体の位置を算出する位置算出手段と、
    を含む位置計測装置。

Images