JP2009074861A - 移動量計測装置及び位置計測装置 - Google Patents

Abstract

【課題】簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができるようにする。
【解決手段】ＧＰＳから、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における受信信号の衛星番号群とを取得し（１４０）、第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一である場合には（１４４）、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の車速を算出する（１４６）。第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でない場合には（１４４）、過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索し（１４８）、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出する（１５２）。
【選択図】図７

Description

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

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

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

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

上記の目的を達成するために第１の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。

第１の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。そして、移動量算出手段によって、測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出する。

このように、同一の衛星群からの信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

上記の第１の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段を更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

上記の第１の発明に係る移動量計測装置は、移動体の速度に応じてパルス信号を出力する速度センサを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、速度センサのパルス信号に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

上記の第１の発明に係る位置計測装置は、移動体の外部を撮像する単眼の撮像手段と、移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出し、第１時刻と第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

上記の第１の発明に係る測位手段は、衛星群の各衛星から衛星を識別するための識別情報を含む信号を複数受信し、移動量算出手段は、測位手段によって識別情報の組み合わせが同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出することができる。このように、衛星からの信号に含まれる識別情報によって、同一の衛星群から信号を受信したか否かを判断することができる。

第２の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。

第２の発明に係る移動量計測装置によれば、単眼の撮像手段によって、移動体の外部を撮像し、速度センサによって、移動体の速度に応じてパルス信号を出力する。そして、移動量算出手段によって、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出する。また、移動量算出手段によって、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出する。

このように、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

第３の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記移動体の外部を撮像する単眼の撮像手段と、前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。

第３の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。単眼の撮像手段によって、移動体の外部を撮像し、距離算出手段によって、撮像手段によって撮像された複数の画像に基づいて、画像の特徴点までの距離を算出する。速度センサによって、移動体の速度に応じてパルス信号を出力する。

そして、移動量算出手段によって、測位手段によって測位された移動体の位置、距離算出手段によって算出された距離、及び速度センサから出力されたパルス信号に基づいて、移動体の移動量を算出する。

このように、衛星群から受信した複数の信号に基づく移動体の位置、複数の画像に基づく距離、及び速度センサのパルス信号に基づいて、移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

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

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

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

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

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

以上説明したように、本発明の移動量計測装置によれば、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる、という効果が得られる。

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

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

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

運動計測部１２は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部２０と、撮像画像に基づいて、自車両の運動を推定する運動推定部２４とを備えている。

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

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

運動推定部２４は、画像撮像部２０によって撮像された複数の撮像画像に基づいて、３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出する。

移動量計測部１４は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、自車両の位置を測定するＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置から移動量を推定する移動量推定部２８とを備えている。移動量推定部２８は、ＧＰＳ２６による測位位置から、自車両の移動量として並進成分スカラ量又は車速を推定する。例えば、第１時刻と第１時刻から計測単位期間Ｔ_ｍｓ経過した第２時刻との各々の測位位置に基づいて、計測単位期間Ｔ_ｍｓの車速を推定する。

ここで、本発明の原理について説明する。ＧＰＳ２６によって測定される測位位置は、ランダム誤差とオフセット誤差とを有している。ランダム誤差は、時々刻々と変化するものであるが、現在では、ＳＡ(Ｓｅｌｅｃｔｉｖｅ Ａｖａｉｌａｂｉｌｉｔｙ)という規制が解除され、かつ、受信機の性能が向上しているため、安定してＧＰＳ信号が得られる環境下にいる場合には、ランダム誤差の変動分が比較的小さい。

しかし、ＧＰＳ２６によって測定される測位位置は、長周期の誤差も有しているため、短時間で見るとオフセットがあるものと考えられる。この長周期の誤差は、ＧＰＳ信号が伝播する際に通過する、大気圏や電離層における遅延モデルで生じる誤差によるところが大きい。この誤差は、大気圏や電離層の状態変化や衛星の配置によって変化するため、長周期の誤差となる。

同一の衛星群から信号を受信し続けている場合は、時系列間において同一のオフセットが生じていると考えられるが、受信している複数の信号を送信している衛星群に変化があった場合には、オフセットの値に変化が生じる。

受信している複数の信号を送信している衛星群が変化しないときに、その測位位置の変化から移動量を算出した場合は、実際の移動量とよく一致した値が得られる。しかし、受信信号の衛星群が変化した場合には、衛星群が変化した前後で、オフセット量やその正負が変化するため、移動量を算出すると誤差が大きくなる。

図２に示すように、受信信号の衛星群に変化がない場合には、ＧＰＳの測位位置から算出した移動体の速度の時間変化と、移動体の実際の速度の時間変化とは、よく一致している。一方、受信信号の衛星群に変化があった場合には、衛星群が変化した前後で、ＧＰＳの測位位置から算出した移動体の速度の誤差が大きくなっている。

上記図２に示すようなＧＰＳの性質を考慮して、移動量推定部２８では、以下に説明するように、ＧＰＳ２６の測位位置を用いて自車両の移動量を推定する。

まず、ＧＰＳ２６によって測定された位置座標と、位置の測定に利用した複数の受信信号の各々に含まれる衛星の識別情報としての衛星番号を保持し、移動量を算出したい時刻間において、受信信号の衛星番号群を比較する。同一の衛星番号群である場合には、同一のオフセットが生じているものとして、移動量を算出したい時刻間の測位位置に基づいて、移動量を算出する。

一方、移動量を算出したい時刻間において、衛星番号群に変化がある場合には、過去の測位結果を保持したデータの中から、同一の衛星番号群の信号を受信した時刻を探索する。該当時刻が存在する場合には、現時刻の測位位置と過去の該当時刻の測位位置とに基づいて、移動量を算出し、また、該当時刻から現在の一時刻前までの移動量が既知であれば、一時刻前から現時刻までの間の移動量を算出することができる。

ただし、該当時刻と現時刻との間に、数１０秒以上の時刻差がある場合、測位位置に大きな隔たりがある場合、又は、該当時刻がない場合については、受信信号の衛星番号群に差異があったとしても、測位位置の変化から移動量を算出すると共に、「低信頼」あるいは「推定誤差大」の可能性があるものとして扱う。

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

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

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

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

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

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

そして、ステップ１０２において、運動推定処理を行い、計測単位期間Ｔ_ｍｓ（例えば、第１時刻から第２時刻までの期間）の自車両の運動の３軸角速度の推定値、あるいは３軸角速度及び並進方向を示す成分の推定値を取得する。上記ステップ１０２の運動推定処理は、図５に示す運動推定処理ルーチンによって実現される。以下に運動推定処理ルーチンについて説明する。

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

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

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

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

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

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

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

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

上述したように、運動推定処理によって、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分が取得される。

次に、位置計測処理ルーチンのステップ１０４で、移動量推定処理を行い、第１時刻と第２時刻との間の自車両の移動量としての車速の推定値を取得する。上記の移動量推定処理は、図７に示す移動量推定処理ルーチンによって実現される。以下に、移動量推定処理ルーチンについて説明する。

まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において測位に利用した複数の受信信号に含まれる複数の衛星番号からなる衛星番号群とを取得する。そして、ステップ１４２で、上記ステップ１４０で取得した測位位置及び衛星番号群をメモリ（図示省略）に記憶する。

次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一であると判定された場合には、第１時刻と第２時刻とにおいて同一の衛星群から信号を受信したと判断し、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でないと判定された場合には、第１時刻と第２時刻とにおいて異なる衛星群から信号を受信したと判断し、ステップ１４８において、メモリに記憶された過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索する。

そして、ステップ１５０において、上記ステップ１４８で第２時刻と同一の衛星番号群であった時刻が検索されたか否かを判定する。上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されたと判定された場合には、ステップ１５２において、メモリに記憶された、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出し、移動量推定処理ルーチンを終了する。

一方、上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されなかったと判定された場合には、ステップ１５４において、第１時刻及び第２時刻の各々で測定された測位位置に基づいて、信頼度の低い推定値として、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

上述したように、移動量推定処理によって、自車両の移動量としての車速が取得される。

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

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

以上説明したように、第１の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。

また、測位に利用した衛星群に変化がない場合の測位位置の変化を用いて、移動量を計測するため、移動量の計測精度を確保することができる。

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

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

第２の実施の形態では、ＧＰＳによる測位位置と、車速センサからの車速パルス信号のパルス数とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。

図８に示すように、第２の実施の形態に係る位置計測装置２１０の移動量計測部２１４は、ＧＰＳ２６と、車速に応じて車速パルス信号を出力する車速パルスセンサで構成され、自車両の対地速に応じた車速パルス信号を出力する車速センサ２２６と、ＧＰＳ２６によって測定された自車両の位置、及び車速センサ２２６の車速パルス信号に基づいて、移動量を推定する移動量推定部２２８とを備えている。

車速センサ２２６は、例えば、車速が６０ｋｍ／ｈ（回転数が６３７ｒｐｍ）のときに、１秒間におけるパルス数が１０．６となる車速パルス信号を出力する。

移動量推定部２２８は、計測単位期間Ｔ_ｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、車速センサ２２６からの車速パルス信号のパルス数をカウントし、パルス数から車速を算出して移動量の推定値とする。

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

まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において受信した信号に含まれる衛星番号群とを取得する。

次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ２５２で、上記ステップ２５０で取得されたパルス数に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

なお、位置計測処理ルーチン及び運動推定処理ルーチンについては、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、速度センサからの車速パルス信号のパルス数に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

また、速度センサの車速パルス信号を用いることで、短期的な移動量の計測精度を確保することができる。

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

第３の実施の形態では、ＧＰＳによる測位位置と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。

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

運動移動量計測部３１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部３２８とを備えている。

移動量推定部３２８は、計測単位期間Ｔ_ｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、画像撮像部２０によって撮像された複数の撮像画像に基づいて、車速を算出して移動量の推定値とする。

移動量推定部３２８は、撮像された複数の撮像画像に基づいて、車速を算出する場合、撮像画像から復元した３次元空間の路面平面を推定する。そして、予め計測された、画像撮像部２０の単眼カメラが設置されている路面上からの高さや単眼カメラの光軸の角度に基づいて、第１時刻及び第２時刻に撮像された撮像画像の各々から、路面上の近傍の特徴点までの距離を計測し、特徴点までの距離の変化に基づいて、車速を算出する。特に、低速時は移動量が少ないため、自車両周辺の路面を路面平面として推定してもよい。

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

まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。

次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像を画像撮像部２０から取得して、取得した撮像画像に基づいて、３次元空間の路面平面を推定する。

そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を算出すると共に、第２時刻に撮像された撮像画像から、上記と同じ特徴点までの距離を算出する。

次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

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

このように、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

なお、上記の実施の形態では、撮像画像から推定された路面平面上の特徴点と、画像撮像部の高さ及び角度とに基づいて、特徴点までの距離を算出する場合を例に説明したが、これに限定されるものではなく、例えば、撮像画像から、大きさが既知の物体（例えば、白線や標識）を抽出し、抽出された物体の撮像画像上の大きさに基づいて、抽出された物体までの距離を算出するようにしてもよい。この場合には、算出された大きさが既知の物体までの距離の変化から、車速を算出するようにすればよい。

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

第４の実施の形態では、速度センサからの車速パルス信号と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。

図１２に示すように、第４の実施の形態に係る位置計測装置４１０は、自車両の運動を計測すると共に、自車両の移動量を計測する運動移動量計測部４１２と、位置推定部１６と、ＧＰＳ２６とを備えている。

運動移動量計測部４１２は、画像撮像部２０と、運動推定部２４と、車速センサ２２６と、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、自車両の移動量を推定する移動量推定部４２８とを備えている。

ここで、本発明の原理について説明する。車速センサ２２６から出力される車速パルス信号は、連続値ではなく、一定の移動が行われた場合に出力される仕組みである。車速パルス信号から算出される車速の時系列変化と、実際の車速の時系列変化とを比較すると、車速パルス信号から算出される車速の値は、真値に対して大きなズレはないが、連続値ではなく離散値となる。信号処理を工夫して、推定精度を上げることは可能であるが、一般的な車速パルス信号の移動量分解能は４０ｃｍ程度であるため、低速時に、移動量が得られないことがある。

本実施の形態では、上記の車速パルス信号を出力する車速センサ２２６の性質を考慮し、移動量推定部４２８は、車速センサ２２６から出力される車速パルス信号を用いて、以下に説明するように、自車両の移動量としての車速を推定する。

まず、車速センサ２２６から計測単位期間Ｔ_ｍｓに出力された車速パルス信号のパルス数をカウントする。このパルス数が、たとえば１以上である場合は、パルス数から換算される車速を移動量として出力する。計測単位期間Ｔ_ｍｓ内にパルス数がカウントされなかった場合（パルス数が０の場合）には、移動の有無の計測が容易ではない。この場合、撮像画像に基づいて計測される計測単位期間Ｔ_ｍｓの距離の変化から、移動量としての車速を算出する。なお、パルス数に関する閾値を暫定的に「１」としたが、特に１に限定されるものではなく、他の適切な値を閾値として採用してもよい。

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

まず、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数Ｎｐを取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。

次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

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

以上説明したように、第４の実施の形態に係る位置計測装置によれば、計測単位期間における車速パルス信号のパルス数が１以上である場合には、車速パルス信号のパルス数に基づいて移動量を算出し、計測単位期間における車速パルス信号のパルス数が０である場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。

また、自車両が低速時には、低速時の計測精度が高くない車速パルス信号の代わりに、撮像画像から推定した近傍の路面平面上の特徴点までの距離の変化に基づいて、移動量を算出することにより、停止判定も可能になる。

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

第５の実施の形態では、ＧＰＳによる測位位置と、撮像画像と、車速センサからの車速パルス信号とを組み合わせて、自車両の移動量を推定している点が、第４の実施の形態と異なっている。

図１４に示すように、第５の実施の形態に係る位置計測装置５１０の運動移動量計測部５１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、車速センサ２２６と、ＧＰＳ２６によって測定された測位位置、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部５２８とを備えている。

移動量推定部５２８は、計測単位期間Ｔ_ｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とする。一方、衛星番号群が変化した場合、車速センサ２２６から計測単位期間Ｔ_ｍｓに出力された車速パルス信号のパルス数をカウントし、このパルス数が、たとえば１以上である場合は、車速センサ２２６の車速パルス信号から車速を算出して移動量の推定値とする。計測単位期間Ｔ_ｍｓ内にパルス数がカウントされなかった場合には、撮像画像に基づいて計測される計測単位期間Ｔ_ｍｓの距離の変化から、移動量としての車速を算出する。

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

まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。

次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。

次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。

このように、計測単位期間の第１時刻と第２時刻とにおいて、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が大きい場合には、速度センサからの車速パルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が小さい場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。

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

第６の実施の形態では、カルマンフィルタ等を用いた最適処理手法によって、自車両の移動量としての車速を推定している点が第５の実施の形態と異なっている。

第６の実施の形態に係る位置計測装置５１０の移動量推定部５２８では、画像撮像部２０の撮像画像に基づいて計測される距離と、ＧＰＳ２６によって測定される測位位置と、車速センサ２２６からの車速パルス信号のパルス数とを観測値とし、自車両の車速を推定値として、カルマンフィルタによる最適処理手法を用いた処理を行う。

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

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

また、車速センサ２２６から出力された車速パルス信号のパルス数の観測値ｙ_ｐｕｌｓが得られた場合に、観測値ｙ_ｐｕｌｓのバラつきを考慮して、観測ノイズｗ_ｐｕｌｓを設定し、観測行列Ｈ_ｐｕｌｓについて、観測値ｙ_ｐｕｌｓと推定値ｘとの関係式から、観測行列Ｈ_ｐｕｌｓを設定する。

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

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

第６の実施の形態に係る位置計測装置５１０では、上述した移動量推定処理ルーチンにおけるステップ３５０からステップ３５４までの処理と同様の処理が繰り返し実行され、算出された路面平面上の特徴点までの距離が繰り返し出力される。また、ＧＰＳ２６によって自車両の位置が繰り返し測定される。また、車速センサ２２６から、車速パルス信号が連続して出力され、パルス数が繰り返しカウントされる。

また、位置計測装置５１０において、図１６に示す移動量推定処理ルーチンが実行される。まず、ステップ６００において、撮像画像に基づいて計測される路面平面上の特徴点までの距離を取得し、ステップ６０２において、ＧＰＳ２６から測定された測位位置を取得する。そして、ステップ６０４において、車速センサ２２６からの車速パルス信号に基づいてカウントされたパルス数を取得する。

そして、ステップ６０６において、上記ステップ６００で取得した距離と、上記ステップ６０２で取得した測位位置と、上記ステップ６０４で取得したパルス数とに基づいて、移動量としての車速の推定値ｘに対して、初期値を設定する。次のステップ６０８では、時間ｔ_ｉ−１にΔｔを加算して、時間ｔ_ｉに更新する。そして、ステップ６１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。

次のステップ６１２において、現タイミングに、撮像画像に基づいて計測される路面平面上の特徴点までの距離、ＧＰＳ２６から測定された測位位置、及び車速センサ２２６からの車速パルス信号のパルス数の何れかが、観測値として入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ６１４において、上記ステップ６１０で算出された推定値ｘの予測値の車速を、自車両の移動量の推定値として出力し、ステップ６０８へ戻る。

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

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

一方、上記ステップ６１６で、観測値として、撮像画像に基づいて計測される路面平面上の特徴点までの距離が入力されたと判定された場合には、ステップ６２０において、入力された特徴点までの距離を観測値として、観測行列Ｈ_{ｉｍａｇｅ}、観測値ｙ_{ｉｍａｇｅ}、及び観測ノイズｗ_{ｉｍａｇｅ}を設定する。

また、上記ステップ６１６で、観測値として、車速センサ２２６からの車速パルス信号のパルス数が入力されたと判定された場合には、ステップ６２２において、入力されたパルス数を観測値として、観測行列Ｈ_ｐｕｌｓ、観測値ｙ_ｐｕｌｓ、及び観測ノイズｗ_ｐｕｌｓを設定する。

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

このように、カルマンフィルタを用いた最適処理手法によって、自車両の外部を撮像した複数の撮像画像に基づいて計測された路面平面上の特徴点までの距離と、ＧＰＳによって測定された測位位置と、車速センサからの車速パルス信号のパルス数とに基づいて、自車両の移動量の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。

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

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

また、誤差共分散を、推定された自車両の移動量の推定値に対する信頼度として用いてもよい。例えば、推定値と共に算出された誤差共分散によって、推定値の信頼度が低く、推定値の誤差が大きいと判断される場合に、この推定値を、自車両の移動量の推定値として採用しないようにしてもよい。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、第８の実施の形態について説明する。第８の実施の形態では、ジャイロセンサによって、自車両の運動の３軸角速度を検出している。

第８の実施の形態に係る位置計測装置は、ジャイロセンサによって検出された自車両の運動の３軸角速度と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。

次に、第９の実施の形態について説明する。第９の実施の形態では、ジャイロセンサ及び単眼カメラによって撮像された撮像画像に基づいて、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定している。

第９の実施の形態に係る位置計測装置は、ジャイロセンサ及び撮像画像によって推定された自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。

なお、上記の第１の実施の形態〜第９の実施の形態では、撮像画像又はジャイロセンサに基づいて３軸角速度を推定する場合を例に説明したが、これに限定されるものではなく、撮像画像又はジャイロセンサに基づいて、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを推定するように構成してもよい。

また、自車両の移動量として、車速を推定する場合を例に説明したが、これに限定されるものではなく、移動量として、移動距離又は移動ベクトルを推定するようにしてもよい。

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

本発明の第１の実施の形態に係る位置計測装置を示すブロック図である。 ＧＰＳの測位位置に基づいて算出される車速の時間変化と実際の車速の時間変化とを示すグラフである。 相対位置の変化量の算出方法を説明するための座標系を示すイメージ図である。 本発明の第１の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。 抽出された特徴点と移動体の運動との関係を説明するための図である。 本発明の第１の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る位置計測装置を示すブロック図である。 本発明の第２の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態に係る位置計測装置を示すブロック図である。 本発明の第３の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第４の実施の形態に係る位置計測装置を示すブロック図である。 本発明の第４の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第５の実施の形態に係る位置計測装置を示すブロック図である。 本発明の第５の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第６の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第７の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。

符号の説明

１０、２１０、３１０、４１０、５１０ 位置計測装置
１２、３１２ 運動計測部
１４、２１４ 移動量計測部
１６ 位置推定部
２０ 画像撮像部
２４ 運動推定部
２６ ＧＰＳ
２８、２２８、３２８、４２８、５２８ 移動量推定部
２２６ 車速センサ
３１２、４１２、５１２ 運動移動量計測部
５２８ 移動量推定部
Ｎｐ パルス数

Claims (8)

1. 複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
   前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段と、
   を含む移動量計測装置。
2. 前記移動体の外部を撮像する単眼の撮像手段を更に含み、
   前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
3. 前記移動体の速度に応じてパルス信号を出力する速度センサを更に含み、
   前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
4. 前記移動体の外部を撮像する単眼の撮像手段と、
   前記移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、
   前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
5. 移動体の外部を撮像する単眼の撮像手段と、
   前記移動体の速度に応じてパルス信号を出力する速度センサと、
   第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段と、
   を含む移動量計測装置。
6. 複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
   前記移動体の外部を撮像する単眼の撮像手段と、
   前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、
   前記移動体の速度に応じてパルス信号を出力する速度センサと、
   前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段と、
   を含む移動量計測装置。
7. 請求項１〜請求項６の何れか１項記載の移動量計測装置と、
   前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、
   前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つとに基づいて、前記移動体の位置を計測する位置計測手段と、
   を含む位置計測装置。
8. 前記運動推定手段は、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、
   前記位置計測手段は、前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つ及び前記並進方向とに基づいて、前記移動体の位置を計測する請求項７記載の位置計測装置。

Images