移動量計測装置及び位置計測装置
【課題】簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができるようにする。
【解決手段】ＧＰＳから、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における受信信号の衛星番号群とを取得し（１４０）、第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一である場合には（１４４）、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の車速を算出する（１４６）。第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でない場合には（１４４）、過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索し（１４８）、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出する（１５２）。
【解決手段】ＧＰＳから、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における受信信号の衛星番号群とを取得し（１４０）、第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一である場合には（１４４）、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の車速を算出する（１４６）。第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でない場合には（１４４）、過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索し（１４８）、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出する（１５２）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動量計測装置及び位置計測装置に係り、特に、移動体の移動量を計測する移動量計測装置、及び移動体の移動量に基づいて移動体の位置を計測する位置計測装置に関する。
【背景技術】
【０００２】
従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己（車）運動認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて運動の回転を求め、近傍用のステレオカメラを用いて移動量としての並進を求める。
【特許文献１】特開２００３−２４７８２４
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、ロバスト性が得られない、という問題がある。
【０００４】
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる移動量計測装置及び位置計測装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために第１の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【０００６】
第１の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。そして、移動量算出手段によって、測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出する。
【０００７】
このように、同一の衛星群からの信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０００８】
上記の第１の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段を更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０００９】
上記の第１の発明に係る移動量計測装置は、移動体の速度に応じてパルス信号を出力する速度センサを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、速度センサのパルス信号に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１０】
上記の第１の発明に係る位置計測装置は、移動体の外部を撮像する単眼の撮像手段と、移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出し、第１時刻と第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１１】
上記の第１の発明に係る測位手段は、衛星群の各衛星から衛星を識別するための識別情報を含む信号を複数受信し、移動量算出手段は、測位手段によって識別情報の組み合わせが同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出することができる。このように、衛星からの信号に含まれる識別情報によって、同一の衛星群から信号を受信したか否かを判断することができる。
【００１２】
第２の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【００１３】
第２の発明に係る移動量計測装置によれば、単眼の撮像手段によって、移動体の外部を撮像し、速度センサによって、移動体の速度に応じてパルス信号を出力する。そして、移動量算出手段によって、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出する。また、移動量算出手段によって、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出する。
【００１４】
このように、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１５】
第３の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記移動体の外部を撮像する単眼の撮像手段と、前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【００１６】
第３の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。単眼の撮像手段によって、移動体の外部を撮像し、距離算出手段によって、撮像手段によって撮像された複数の画像に基づいて、画像の特徴点までの距離を算出する。速度センサによって、移動体の速度に応じてパルス信号を出力する。
【００１７】
そして、移動量算出手段によって、測位手段によって測位された移動体の位置、距離算出手段によって算出された距離、及び速度センサから出力されたパルス信号に基づいて、移動体の移動量を算出する。
【００１８】
このように、衛星群から受信した複数の信号に基づく移動体の位置、複数の画像に基づく距離、及び速度センサのパルス信号に基づいて、移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１９】
第４の発明に係る位置計測装置は、上記の移動量計測装置と、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つとに基づいて、前記移動体の位置を計測する位置計測手段とを含んで構成されている。
【００２０】
本発明に係る位置計測装置によれば、移動量計測装置によって、移動体の移動量を算出し、また、運動推定手段によって、移動体の運動の３軸角速度の少なくとも１つを推定する。
【００２１】
そして、位置計測手段によって、移動量計測装置によって算出された移動体の移動量と、運動推定手段によって推定された移動体の運動の３軸角速度の少なくとも１つとに基づいて、移動体の位置を算出する。
【００２２】
このように、ロバスト性を確保して精度よく計測された移動体の移動量と、推定された移動体の運動とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。
【００２３】
また、上記の運動推定手段は、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、位置計測手段は、移動量計測装置によって算出された移動体の移動量と、運動推定手段によって推定された移動体の運動の３軸角速度の少なくとも１つ及び並進方向とに基づいて、移動体の位置を計測することができる。
【発明の効果】
【００２４】
以上説明したように、本発明の移動量計測装置によれば、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる、という効果が得られる。
【００２５】
本発明の位置計測装置によれば、ロバスト性を確保して精度よく計測された移動体の移動量と、推定された移動体の運動とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００２６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された位置計測装置に本発明を適用した場合を例に説明する。
【００２７】
図１に示すように、第１の実施の形態に係る位置計測装置１０は、自車両の運動を計測する運動計測部１２と、自車両の移動量を計測する移動量計測部１４と、計測された自車両の運動及び移動量に基づいて、自車両の位置を推定する位置推定部１６とを備えている。
【００２８】
運動計測部１２は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部２０と、撮像画像に基づいて、自車両の運動を推定する運動推定部２４とを備えている。
【００２９】
運動推定部２４では、自車両の運動として、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定する。なお、本実施の形態では、運動推定部２４によって、並進量（並進成分スカラ量）は推定しない。
【００３０】
本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。
【００３１】
運動推定部２４は、画像撮像部２０によって撮像された複数の撮像画像に基づいて、３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出する。
【００３２】
移動量計測部１４は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、自車両の位置を測定するＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置から移動量を推定する移動量推定部２８とを備えている。移動量推定部２８は、ＧＰＳ２６による測位位置から、自車両の移動量として並進成分スカラ量又は車速を推定する。例えば、第１時刻と第１時刻から計測単位期間Ｔｍｓ経過した第２時刻との各々の測位位置に基づいて、計測単位期間Ｔｍｓの車速を推定する。
【００３３】
ここで、本発明の原理について説明する。ＧＰＳ２６によって測定される測位位置は、ランダム誤差とオフセット誤差とを有している。ランダム誤差は、時々刻々と変化するものであるが、現在では、ＳＡ(Ｓｅｌｅｃｔｉｖｅ Ａｖａｉｌａｂｉｌｉｔｙ)という規制が解除され、かつ、受信機の性能が向上しているため、安定してＧＰＳ信号が得られる環境下にいる場合には、ランダム誤差の変動分が比較的小さい。
【００３４】
しかし、ＧＰＳ２６によって測定される測位位置は、長周期の誤差も有しているため、短時間で見るとオフセットがあるものと考えられる。この長周期の誤差は、ＧＰＳ信号が伝播する際に通過する、大気圏や電離層における遅延モデルで生じる誤差によるところが大きい。この誤差は、大気圏や電離層の状態変化や衛星の配置によって変化するため、長周期の誤差となる。
【００３５】
同一の衛星群から信号を受信し続けている場合は、時系列間において同一のオフセットが生じていると考えられるが、受信している複数の信号を送信している衛星群に変化があった場合には、オフセットの値に変化が生じる。
【００３６】
受信している複数の信号を送信している衛星群が変化しないときに、その測位位置の変化から移動量を算出した場合は、実際の移動量とよく一致した値が得られる。しかし、受信信号の衛星群が変化した場合には、衛星群が変化した前後で、オフセット量やその正負が変化するため、移動量を算出すると誤差が大きくなる。
【００３７】
図２に示すように、受信信号の衛星群に変化がない場合には、ＧＰＳの測位位置から算出した移動体の速度の時間変化と、移動体の実際の速度の時間変化とは、よく一致している。一方、受信信号の衛星群に変化があった場合には、衛星群が変化した前後で、ＧＰＳの測位位置から算出した移動体の速度の誤差が大きくなっている。
【００３８】
上記図２に示すようなＧＰＳの性質を考慮して、移動量推定部２８では、以下に説明するように、ＧＰＳ２６の測位位置を用いて自車両の移動量を推定する。
【００３９】
まず、ＧＰＳ２６によって測定された位置座標と、位置の測定に利用した複数の受信信号の各々に含まれる衛星の識別情報としての衛星番号を保持し、移動量を算出したい時刻間において、受信信号の衛星番号群を比較する。同一の衛星番号群である場合には、同一のオフセットが生じているものとして、移動量を算出したい時刻間の測位位置に基づいて、移動量を算出する。
【００４０】
一方、移動量を算出したい時刻間において、衛星番号群に変化がある場合には、過去の測位結果を保持したデータの中から、同一の衛星番号群の信号を受信した時刻を探索する。該当時刻が存在する場合には、現時刻の測位位置と過去の該当時刻の測位位置とに基づいて、移動量を算出し、また、該当時刻から現在の一時刻前までの移動量が既知であれば、一時刻前から現時刻までの間の移動量を算出することができる。
【００４１】
ただし、該当時刻と現時刻との間に、数１０秒以上の時刻差がある場合、測位位置に大きな隔たりがある場合、又は、該当時刻がない場合については、受信信号の衛星番号群に差異があったとしても、測位位置の変化から移動量を算出すると共に、「低信頼」あるいは「推定誤差大」の可能性があるものとして扱う。
【００４２】
位置推定部１６は、計測された自車両の運動及び移動量に基づいて、自車両の相対位置の変化量を算出し、過去にＧＰＳ２６で計測された絶対位置と相対位置の変化量とに基づいて、自車両の絶対位置を推定する。以下に、相対位置の変化量を算出する方法について図３を用いて説明する。
【００４３】
まず、図３に示すような座標系において、初期位置を（Ｘ（０）、Ｙ（０）、Ｚ（０））とし、時刻ｔにおいて、移動量（速度Ｖ（ｔ））、運動（３軸角速度（ヨー角速度φ（ｔ）、ピッチ角速度θ（ｔ）、ロール角速度ψ（ｔ））、並進方向を示す成分（ａｘ、ａｙ、ａｚ））が得られると、下記の（１）式のように、時刻ｔ＋Δｔにおける相対位置（Ｘ（ｔ＋Δｔ）、Ｙ（ｔ＋Δｔ）、Ｚ（ｔ＋Δｔ））を更新することができる。
【００４４】
【数１】
ただし、ａｘ／｜ａ｜、ａｙ／｜ａ｜、及びａｚ／｜ａ｜は、正規化された並進方向を示す成分の各々を表わす。
【００４５】
そして、自車両に搭載されたＧＰＳ２６から、経度及び緯度などで表わされる絶対位置情報が過去に取得されていた場合には、絶対位置情報が示す絶対位置と、算出された相対位置とに基づいて、自車両の現在の絶対位置を計測する。なお、絶対位置情報を、必ずしもＧＰＳ２６の測位位置から取得する必要はなく、例えば路側の位置が既知である設備から、通信などによって移動体の絶対位置情報を間欠的に取得してもよいし、予め位置が既知であるランドマーク情報が登録された地図などから、自車両の位置を取得してもよい。また、ＧＰＳ２６と同じ頻度で取得する必要もなく、移動量と運動とで補完できる間隔で取得すればよく、取得手段も問わない。
【００４６】
次に、第１の実施の形態に係る位置計測装置１０の作用について説明する。なお、位置計測装置１０を搭載した自車両が走行しているときに、自車両の位置を計測する場合を例に説明する。
【００４７】
位置計測装置１０において、図４に示す位置計測処理ルーチンが実行される。まず、ステップ１００において、ＧＰＳ２６によって自車両の位置を測位し、測位した位置を絶対位置の初期位置として設定する。
【００４８】
そして、ステップ１０２において、運動推定処理を行い、計測単位期間Ｔｍｓ（例えば、第１時刻から第２時刻までの期間）の自車両の運動の３軸角速度の推定値、あるいは３軸角速度及び並進方向を示す成分の推定値を取得する。上記ステップ１０２の運動推定処理は、図５に示す運動推定処理ルーチンによって実現される。以下に運動推定処理ルーチンについて説明する。
【００４９】
まず、ステップ１２０において、第１時刻及び第２時刻の各々のタイミングで画像撮像部２０によって自車両前方を撮像した第１の画像及び第２の画像を取得する。そして、ステップ１２２において、上記ステップ１２０で取得した第１の画像を複数の領域に分割する。このステップ１２２では、例えば、予め与えられた定数ｙ１、ｙ２を各領域の境界線（ｙ軸の切片座標）として、第１の画像を、画像上段の上方領域と、中段の中間領域と、下段の下方領域との３つの領域に分割する。
【００５０】
次のステップ１２４において、上記ステップ１２２で得られた各分割画像中から、それぞれ所定の数だけ特徴点（不動点であることが期待される点）を抽出する。ここで、特徴点とは、周囲の点とは区別でき、追跡が容易な点のことである。このステップ１２４では、例えば、二次元的に濃淡変化の勾配値が大きくなる画素を検出する、一般的な周知の方法（例えば、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に特徴点を抽出する。
【００５１】
そして、ステップ１２６において、上記ステップ１２４で抽出された第１の画像における特徴点の各々を、上記ステップ１２０で取得した第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検出する。このとき、第１時刻と第２時刻との間で対応する特徴点は、同一の不動点であることが期待されているので、第１の画像と第２の画像とで対応する点とその周囲の点との輝度は、殆ど変化しないという仮定を用い、この仮定に基づいて各特徴点の追跡を行う。この様な追跡処理を実施する際には、例えば、周知のルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などを用いると良い。また、各特徴点の追跡処理の後、第１の画像と第２の画像との間で対応する特徴点の各組の各画像内座標を、１つの適当な形式に整理する。
【００５２】
そして、ステップ１２８において、上記ステップ１２６で得られた第１時刻と第２時刻との間で互いに対応する特徴点の組から、第１時刻と第２時刻との間における画像撮像部２０の相対的な位置関係、即ち、画像撮像部２０を搭載した自車両の第１時刻と第２時刻との間における運動に関連する情報を推定する。
【００５３】
以下、図６を用いて、抽出された特徴点（不動点）と移動体の運動との関係を説明する。図６に示すように、移動体の運動は、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとから構成される。例えば、第１の画像上の特徴点ｘ１に対応する第２の画像上の特徴点をｘ２とすると、これらの特徴点は、同一の不動点を示す点である。
【００５４】
ここで、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、対応する２つの特徴点の関係を、以下の（２）式〜（４）式で表すことができる。
【００５５】
【数２】
【００５６】
上記の（２）式〜（４）式のように、上記の２時刻間における８組以上の互いに対応する特徴点の組（即ち、同一の不動点）が求まっていれば、それに基づいて、上記の基礎行列Ｆを算出することができる。そして、カメラのキャリブレーション行列Ｋが既知であれば、上記の式からも分かる様に、その２時刻間における移動体の運動、即ち、その２時刻間における自車両の回転行列Ｒと並進ベクトルｔとを求めることができる。
【００５７】
そして、ステップ１３０において、上記ステップ１２８で求めた自車両の回転行列Ｒと並進ベクトルｔとに基づいて、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出し、算出した３軸角速度、あるいは３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【００５８】
上述したように、運動推定処理によって、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分が取得される。
【００５９】
次に、位置計測処理ルーチンのステップ１０４で、移動量推定処理を行い、第１時刻と第２時刻との間の自車両の移動量としての車速の推定値を取得する。上記の移動量推定処理は、図７に示す移動量推定処理ルーチンによって実現される。以下に、移動量推定処理ルーチンについて説明する。
【００６０】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において測位に利用した複数の受信信号に含まれる複数の衛星番号からなる衛星番号群とを取得する。そして、ステップ１４２で、上記ステップ１４０で取得した測位位置及び衛星番号群をメモリ（図示省略）に記憶する。
【００６１】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一であると判定された場合には、第１時刻と第２時刻とにおいて同一の衛星群から信号を受信したと判断し、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００６２】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でないと判定された場合には、第１時刻と第２時刻とにおいて異なる衛星群から信号を受信したと判断し、ステップ１４８において、メモリに記憶された過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索する。
【００６３】
そして、ステップ１５０において、上記ステップ１４８で第２時刻と同一の衛星番号群であった時刻が検索されたか否かを判定する。上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されたと判定された場合には、ステップ１５２において、メモリに記憶された、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出し、移動量推定処理ルーチンを終了する。
【００６４】
一方、上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されなかったと判定された場合には、ステップ１５４において、第１時刻及び第２時刻の各々で測定された測位位置に基づいて、信頼度の低い推定値として、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００６５】
上述したように、移動量推定処理によって、自車両の移動量としての車速が取得される。
【００６６】
次に、位置計測処理ルーチンのステップ１０６において、上記ステップ１０２で得られた自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、上記ステップ１０４で得られた自車両の車速とに基づいて、自車両の相対位置の変化量を算出する。次のステップ１０８では、上記ステップ１００で設定された初期位置と、上記ステップ１０６で算出された相対位置の変化量とに基づいて、自車両の絶対位置を算出する。
【００６７】
そして、ステップ１１２において、上記ステップ１０２からステップ１１０によって絶対位置を計測してから、計測単位期間Ｔｍｓが経過したか否かを判定し、計測単位期間Ｔｍｓが経過すると、ステップ１０２へ戻り、再び自車両の絶対位置を計測して、自車両の絶対位置を更新する。
【００６８】
以上説明したように、第１の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【００６９】
また、測位に利用した衛星群に変化がない場合の測位位置の変化を用いて、移動量を計測するため、移動量の計測精度を確保することができる。
【００７０】
また、ロバスト性を確保して精度よく計測された自車両の車速と、推定された自車両の運動の３軸角速度及び並進方向を示す成分とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【００７１】
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００７２】
第２の実施の形態では、ＧＰＳによる測位位置と、車速センサからの車速パルス信号のパルス数とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【００７３】
図８に示すように、第２の実施の形態に係る位置計測装置２１０の移動量計測部２１４は、ＧＰＳ２６と、車速に応じて車速パルス信号を出力する車速パルスセンサで構成され、自車両の対地速に応じた車速パルス信号を出力する車速センサ２２６と、ＧＰＳ２６によって測定された自車両の位置、及び車速センサ２２６の車速パルス信号に基づいて、移動量を推定する移動量推定部２２８とを備えている。
【００７４】
車速センサ２２６は、例えば、車速が６０ｋｍ／ｈ（回転数が６３７ｒｐｍ）のときに、１秒間におけるパルス数が１０．６となる車速パルス信号を出力する。
【００７５】
移動量推定部２２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、車速センサ２２６からの車速パルス信号のパルス数をカウントし、パルス数から車速を算出して移動量の推定値とする。
【００７６】
次に、第２の実施の形態に係る移動量推定処理ルーチンについて、図９を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して説明を省略する。
【００７７】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において受信した信号に含まれる衛星番号群とを取得する。
【００７８】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００７９】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ２５２で、上記ステップ２５０で取得されたパルス数に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００８０】
なお、位置計測処理ルーチン及び運動推定処理ルーチンについては、第１の実施の形態と同様であるため、説明を省略する。
【００８１】
以上説明したように、第２の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、速度センサからの車速パルス信号のパルス数に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００８２】
また、速度センサの車速パルス信号を用いることで、短期的な移動量の計測精度を確保することができる。
【００８３】
次に、第３の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００８４】
第３の実施の形態では、ＧＰＳによる測位位置と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【００８５】
図１０に示すように、第３の実施の形態に係る位置計測装置３１０は、自車両の運動を計測すると共に、自車両の移動量を計測する運動移動量計測部３１２と、位置推定部１６とを備えている。
【００８６】
運動移動量計測部３１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部３２８とを備えている。
【００８７】
移動量推定部３２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、画像撮像部２０によって撮像された複数の撮像画像に基づいて、車速を算出して移動量の推定値とする。
【００８８】
移動量推定部３２８は、撮像された複数の撮像画像に基づいて、車速を算出する場合、撮像画像から復元した３次元空間の路面平面を推定する。そして、予め計測された、画像撮像部２０の単眼カメラが設置されている路面上からの高さや単眼カメラの光軸の角度に基づいて、第１時刻及び第２時刻に撮像された撮像画像の各々から、路面上の近傍の特徴点までの距離を計測し、特徴点までの距離の変化に基づいて、車速を算出する。特に、低速時は移動量が少ないため、自車両周辺の路面を路面平面として推定してもよい。
【００８９】
次に、第３の実施の形態に係る移動量推定処理ルーチンについて、図１１を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して、詳細な説明を省略する。
【００９０】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。
【００９１】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００９２】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像を画像撮像部２０から取得して、取得した撮像画像に基づいて、３次元空間の路面平面を推定する。
【００９３】
そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を算出すると共に、第２時刻に撮像された撮像画像から、上記と同じ特徴点までの距離を算出する。
【００９４】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００９５】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【００９６】
このように、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００９７】
なお、上記の実施の形態では、撮像画像から推定された路面平面上の特徴点と、画像撮像部の高さ及び角度とに基づいて、特徴点までの距離を算出する場合を例に説明したが、これに限定されるものではなく、例えば、撮像画像から、大きさが既知の物体（例えば、白線や標識）を抽出し、抽出された物体の撮像画像上の大きさに基づいて、抽出された物体までの距離を算出するようにしてもよい。この場合には、算出された大きさが既知の物体までの距離の変化から、車速を算出するようにすればよい。
【００９８】
次に、第４の実施の形態について説明する。なお、第１の実施の形態及び第２の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００９９】
第４の実施の形態では、速度センサからの車速パルス信号と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【０１００】
図１２に示すように、第４の実施の形態に係る位置計測装置４１０は、自車両の運動を計測すると共に、自車両の移動量を計測する運動移動量計測部４１２と、位置推定部１６と、ＧＰＳ２６とを備えている。
【０１０１】
運動移動量計測部４１２は、画像撮像部２０と、運動推定部２４と、車速センサ２２６と、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、自車両の移動量を推定する移動量推定部４２８とを備えている。
【０１０２】
ここで、本発明の原理について説明する。車速センサ２２６から出力される車速パルス信号は、連続値ではなく、一定の移動が行われた場合に出力される仕組みである。車速パルス信号から算出される車速の時系列変化と、実際の車速の時系列変化とを比較すると、車速パルス信号から算出される車速の値は、真値に対して大きなズレはないが、連続値ではなく離散値となる。信号処理を工夫して、推定精度を上げることは可能であるが、一般的な車速パルス信号の移動量分解能は４０ｃｍ程度であるため、低速時に、移動量が得られないことがある。
【０１０３】
本実施の形態では、上記の車速パルス信号を出力する車速センサ２２６の性質を考慮し、移動量推定部４２８は、車速センサ２２６から出力される車速パルス信号を用いて、以下に説明するように、自車両の移動量としての車速を推定する。
【０１０４】
まず、車速センサ２２６から計測単位期間Ｔｍｓに出力された車速パルス信号のパルス数をカウントする。このパルス数が、たとえば１以上である場合は、パルス数から換算される車速を移動量として出力する。計測単位期間Ｔｍｓ内にパルス数がカウントされなかった場合（パルス数が０の場合）には、移動の有無の計測が容易ではない。この場合、撮像画像に基づいて計測される計測単位期間Ｔｍｓの距離の変化から、移動量としての車速を算出する。なお、パルス数に関する閾値を暫定的に「１」としたが、特に１に限定されるものではなく、他の適切な値を閾値として採用してもよい。
【０１０５】
次に、第４の実施の形態における移動量推定処理ルーチンについて、図１３を用いて説明する。なお、第２の実施の形態及び第３の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１０６】
まず、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数Ｎｐを取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１０７】
一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。
【０１０８】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１０９】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【０１１０】
以上説明したように、第４の実施の形態に係る位置計測装置によれば、計測単位期間における車速パルス信号のパルス数が１以上である場合には、車速パルス信号のパルス数に基づいて移動量を算出し、計測単位期間における車速パルス信号のパルス数が０である場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０１１１】
また、自車両が低速時には、低速時の計測精度が高くない車速パルス信号の代わりに、撮像画像から推定した近傍の路面平面上の特徴点までの距離の変化に基づいて、移動量を算出することにより、停止判定も可能になる。
【０１１２】
次に、第５の実施の形態について説明する。なお、第１の実施の形態及び第４の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【０１１３】
第５の実施の形態では、ＧＰＳによる測位位置と、撮像画像と、車速センサからの車速パルス信号とを組み合わせて、自車両の移動量を推定している点が、第４の実施の形態と異なっている。
【０１１４】
図１４に示すように、第５の実施の形態に係る位置計測装置５１０の運動移動量計測部５１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、車速センサ２２６と、ＧＰＳ２６によって測定された測位位置、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部５２８とを備えている。
【０１１５】
移動量推定部５２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とする。一方、衛星番号群が変化した場合、車速センサ２２６から計測単位期間Ｔｍｓに出力された車速パルス信号のパルス数をカウントし、このパルス数が、たとえば１以上である場合は、車速センサ２２６の車速パルス信号から車速を算出して移動量の推定値とする。計測単位期間Ｔｍｓ内にパルス数がカウントされなかった場合には、撮像画像に基づいて計測される計測単位期間Ｔｍｓの距離の変化から、移動量としての車速を算出する。
【０１１６】
次に、第５の実施の形態に係る移動量推定処理ルーチンについて、図１５を用いて説明する。なお、第１の実施の形態〜第４の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１１７】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。
【０１１８】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【０１１９】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１２０】
一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。
【０１２１】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１２２】
このように、計測単位期間の第１時刻と第２時刻とにおいて、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が大きい場合には、速度センサからの車速パルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が小さい場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【０１２３】
次に、第６の実施の形態について説明する。なお、第６の実施の形態に係る位置計測装置は、第５の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１２４】
第６の実施の形態では、カルマンフィルタ等を用いた最適処理手法によって、自車両の移動量としての車速を推定している点が第５の実施の形態と異なっている。
【０１２５】
第６の実施の形態に係る位置計測装置５１０の移動量推定部５２８では、画像撮像部２０の撮像画像に基づいて計測される距離と、ＧＰＳ２６によって測定される測位位置と、車速センサ２２６からの車速パルス信号のパルス数とを観測値とし、自車両の車速を推定値として、カルマンフィルタによる最適処理手法を用いた処理を行う。
【０１２６】
カルマンフィルタによる最適処理手法では、画像撮像部２０の単眼カメラの撮像画像に基づいて計測される距離の観測値ｙｉｍａｇｅが得られた場合に、観測値ｙｉｍａｇｅのバラつきを考慮して、観測ノイズｗｉｍａｇｅを設定し、観測行列Ｈｉｍａｇｅについて、観測値ｙｉｍａｇｅと推定値ｘとの関係式から、観測行列Ｈｉｍａｇｅを設定する。
【０１２７】
また、ＧＰＳ２６によって測位された測位位置の観測値ｙｇｐｓが得られた場合に、観測値ｙｇｐｓのバラつきを考慮して、観測ノイズｗｇｐｓを設定し、観測行列Ｈｇｐｓについて、観測値ｙｇｐｓと推定値ｘとの関係式から、観測行列Ｈｇｐｓを設定する。
【０１２８】
また、車速センサ２２６から出力された車速パルス信号のパルス数の観測値ｙｐｕｌｓが得られた場合に、観測値ｙｐｕｌｓのバラつきを考慮して、観測ノイズｗｐｕｌｓを設定し、観測行列Ｈｐｕｌｓについて、観測値ｙｐｕｌｓと推定値ｘとの関係式から、観測行列Ｈｐｕｌｓを設定する。
【０１２９】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１３０】
次に、第６の実施の形態に係る位置計測装置５１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１３１】
第６の実施の形態に係る位置計測装置５１０では、上述した移動量推定処理ルーチンにおけるステップ３５０からステップ３５４までの処理と同様の処理が繰り返し実行され、算出された路面平面上の特徴点までの距離が繰り返し出力される。また、ＧＰＳ２６によって自車両の位置が繰り返し測定される。また、車速センサ２２６から、車速パルス信号が連続して出力され、パルス数が繰り返しカウントされる。
【０１３２】
また、位置計測装置５１０において、図１６に示す移動量推定処理ルーチンが実行される。まず、ステップ６００において、撮像画像に基づいて計測される路面平面上の特徴点までの距離を取得し、ステップ６０２において、ＧＰＳ２６から測定された測位位置を取得する。そして、ステップ６０４において、車速センサ２２６からの車速パルス信号に基づいてカウントされたパルス数を取得する。
【０１３３】
そして、ステップ６０６において、上記ステップ６００で取得した距離と、上記ステップ６０２で取得した測位位置と、上記ステップ６０４で取得したパルス数とに基づいて、移動量としての車速の推定値ｘに対して、初期値を設定する。次のステップ６０８では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ６１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１３４】
次のステップ６１２において、現タイミングに、撮像画像に基づいて計測される路面平面上の特徴点までの距離、ＧＰＳ２６から測定された測位位置、及び車速センサ２２６からの車速パルス信号のパルス数の何れかが、観測値として入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ６１４において、上記ステップ６１０で算出された推定値ｘの予測値の車速を、自車両の移動量の推定値として出力し、ステップ６０８へ戻る。
【０１３５】
上記ステップ６１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ６１６において、どの観測値が入力されたのかを判定する。
【０１３６】
上記ステップ６１６で、観測値としてＧＰＳ２６から測定された測位位置が入力されたと判定された場合には、ステップ６１８において、入力された測位位置を観測値として、観測行列Ｈｇｐｓ、観測値ｙｇｐｓ、及び観測ノイズｗｇｐｓを設定する。
【０１３７】
一方、上記ステップ６１６で、観測値として、撮像画像に基づいて計測される路面平面上の特徴点までの距離が入力されたと判定された場合には、ステップ６２０において、入力された特徴点までの距離を観測値として、観測行列Ｈｉｍａｇｅ、観測値ｙｉｍａｇｅ、及び観測ノイズｗｉｍａｇｅを設定する。
【０１３８】
また、上記ステップ６１６で、観測値として、車速センサ２２６からの車速パルス信号のパルス数が入力されたと判定された場合には、ステップ６２２において、入力されたパルス数を観測値として、観測行列Ｈｐｕｌｓ、観測値ｙｐｕｌｓ、及び観測ノイズｗｐｕｌｓを設定する。
【０１３９】
そして、ステップ６２４において、上記ステップ６１８、上記ステップ６２０、又は上記ステップ６２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ６２６では、上記ステップ６２４で算出された推定値ｘの車速を、自車両の移動量の推定値として出力し、ステップ６０８へ戻る。
【０１４０】
このように、カルマンフィルタを用いた最適処理手法によって、自車両の外部を撮像した複数の撮像画像に基づいて計測された路面平面上の特徴点までの距離と、ＧＰＳによって測定された測位位置と、車速センサからの車速パルス信号のパルス数とに基づいて、自車両の移動量の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【０１４１】
また、ロバスト性を確保して精度よく計測された自車両の移動量と、推定された自車両の運動の３軸角速度及び並進方向を示す成分とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【０１４２】
なお、上記の実施の形態では、カルマンフィルタで推定する例を記述したが、周知のパーティクルフィルタなど、他の最適推定手法を用いてもよく、適切に解を推定できる手法であれば、その方法はこの限りではない。
【０１４３】
また、誤差共分散を、推定された自車両の移動量の推定値に対する信頼度として用いてもよい。例えば、推定値と共に算出された誤差共分散によって、推定値の信頼度が低く、推定値の誤差が大きいと判断される場合に、この推定値を、自車両の移動量の推定値として採用しないようにしてもよい。
【０１４４】
次に、第７の実施の形態について説明する。なお、第７の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１４５】
第７の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の位置を計測している点が第１の実施の形態と異なっている。
【０１４６】
第７の実施の形態に係る位置計測装置１０の位置推定部１６では、運動推定部２４によって推定された運動（３軸角速度、並進方向を示す成分）、移動量推定部２８によって推定された移動量（車速）、及びＧＰＳ２６によって測定された絶対位置(Ｘｇ,Ｙｇ,Ｚｇ)を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行う。
【０１４７】
カルマンフィルタを用いた最適処理手法では、運動推定部２４によって推定された運動の観測値ｙｍｏｖｅが得られた場合に、観測値ｙｍｏｖｅのバラつきを考慮して、観測ノイズｗｍｏｖｅを設定し、観測行列Ｈｍｏｖｅについて、観測値ｙｍｏｖｅと推定値ｘとの関係式から、観測行列Ｈｍｏｖｅを設定する。
【０１４８】
また、移動量推定部２８によって推定された移動量の観測値ｙｔｒａｖが得られた場合に、観測値ｙｔｒａｖのバラつきを考慮して、観測ノイズｗｔｒａｖを設定し、観測行列Ｈｔｒａｖについて、観測値ｙｔｒａｖと推定値ｘとの関係式から、観測行列Ｈｔｒａｖを設定する。
【０１４９】
また、ＧＰＳ２６によって測定された絶対位置の観測値ｙｇｐｓが得られた場合に、観測値ｙｇｐｓのバラつきを考慮して、観測ノイズｗｇｐｓを設定し、観測行列Ｈｇｐｓについて、観測値ｙｇｐｓと推定値ｘとの関係式から、観測行列Ｈｇｐｓを設定する。
【０１５０】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１５１】
次に、第７の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１５２】
第７の実施の形態に係る位置計測装置１０では、第１の実施の形態で説明した運動推定処理ルーチンが繰り返し実行され、また、上述した移動量推定処理ルーチンが繰り返し実行される。また、ＧＰＳ２６によって、所定時間ごと（例えば１秒ごと）に自車両の絶対位置が測定される。
【０１５３】
また、位置計測装置１０において、図１７に示す位置計測処理ルーチンが実行される。まず、ステップ７００において、運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ７０２において、移動量推定処理ルーチンによって推定された車速を取得する。次のステップ７０４では、ＧＰＳ２６によって測定された自車両の絶対位置を取得する。
【０１５４】
そして、ステップ７０６において、上記ステップ７００で取得した３軸角速度及び並進方向を示す成分と、上記ステップ７０２で取得した車速と、上記ステップ７０４で取得した絶対位置とに基づいて、絶対位置、方位角、車速、及び３軸角速度の推定値ｘに対して、初期値を設定する。次のステップ７０８では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ７１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１５５】
次のステップ７１２において、現タイミングに、観測値として、運動推定処理ルーチンによって推定された３軸角速度、移動推定処理ルーチンによって推定された車速、又はＧＰＳ２６によって測定された絶対位置が入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ７１４において、上記ステップ７１０で算出された推定値ｘの予測値の絶対位置を、自車両の絶対位置の推定値として出力し、ステップ７０８へ戻る。
【０１５６】
上記ステップ７１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ７１６において、上記ステップ７１２でどの観測値が入力されたかを判定する。
【０１５７】
上記ステップ７１６で、観測値として、運動推定処理ルーチンで推定された３軸角速度及び並進方向を示す成分が入力されたと判定された場合には、ステップ７１８において、入力された３軸角速度及び並進方向を示す成分を観測値として、観測行列Ｈｍｏｖｅ、観測値ｙｍｏｖｅ、及び観測ノイズｗｍｏｖｅを設定する。
【０１５８】
一方、上記ステップ７１６で、観測値として、移動量推定処理ルーチンによって推定された車速が入力されたと判定された場合には、ステップ７２０において、入力された車速を観測値として、観測行列Ｈｔｒａｖ、観測値ｙｔｒａｖ、及び観測ノイズｗｔｒａｖを設定する。
【０１５９】
また、上記ステップ７１６で、観測値として、ＧＰＳ２６によって測定された絶対位置が入力されたと判定された場合には、ステップ７２２において、入力された絶対位置を観測値として、観測行列Ｈｇｐｓ、観測値ｙｇｐｓ、及び観測ノイズｗｇｐｓを設定する。
【０１６０】
そして、ステップ７２４において、上記ステップ７１８、上記ステップ７２０、又は上記ステップ７２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ７２６では、上記ステップ７２４で算出された推定値ｘの絶対位置を、自車両の絶対位置の推定値として出力し、ステップ７０８へ戻る。
【０１６１】
このように、カルマンフィルタを用いた最適処理手法によって、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速と、ＧＰＳによって測位された位置とに基づいて、自車両の絶対位置の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の絶対位置を精度よく計測することができる。
【０１６２】
なお、上記の実施の形態では、カルマンフィルタを用いた位置計測処理において、推定された自車両の運動の３軸角速度及び並進方向を示す成分、推定された自車両の車速、及びＧＰＳによって測位された絶対位置が、観測値として入力される場合を例に説明したが、これに限定されるものではない。例えば、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、及び推定された自車両の車速とに基づいて推定された自車両の相対位置の変化量が、観測値として、カルマンフィルタを用いた位置計測処理に入力されるようになっていてもよい。この場合には、推定された自車両の相対位置の変化量、及びＧＰＳによって測位された絶対位置を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行うようにすればよい。
【０１６３】
また、カルマンフィルタに限らず、パーティクルフィルタなどの最適推定手法を用いてもよい。
【０１６４】
次に、第８の実施の形態について説明する。第８の実施の形態では、ジャイロセンサによって、自車両の運動の３軸角速度を検出している。
【０１６５】
第８の実施の形態に係る位置計測装置は、ジャイロセンサによって検出された自車両の運動の３軸角速度と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。
【０１６６】
次に、第９の実施の形態について説明する。第９の実施の形態では、ジャイロセンサ及び単眼カメラによって撮像された撮像画像に基づいて、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定している。
【０１６７】
第９の実施の形態に係る位置計測装置は、ジャイロセンサ及び撮像画像によって推定された自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。
【０１６８】
なお、上記の第１の実施の形態〜第９の実施の形態では、撮像画像又はジャイロセンサに基づいて３軸角速度を推定する場合を例に説明したが、これに限定されるものではなく、撮像画像又はジャイロセンサに基づいて、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを推定するように構成してもよい。
【０１６９】
また、自車両の移動量として、車速を推定する場合を例に説明したが、これに限定されるものではなく、移動量として、移動距離又は移動ベクトルを推定するようにしてもよい。
【０１７０】
また、位置計測処理をオンラインで逐次処理する場合を例に説明したが、これに限定されるものではなく、自車両の運動の推定値及び移動量の推定値を取得した結果を用いて、オフラインで後処理として位置計測処理を行うようにしてもよい。
【図面の簡単な説明】
【０１７１】
【図１】本発明の第１の実施の形態に係る位置計測装置を示すブロック図である。
【図２】ＧＰＳの測位位置に基づいて算出される車速の時間変化と実際の車速の時間変化とを示すグラフである。
【図３】相対位置の変化量の算出方法を説明するための座標系を示すイメージ図である。
【図４】本発明の第１の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【図５】本発明の第１の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図６】抽出された特徴点と移動体の運動との関係を説明するための図である。
【図７】本発明の第１の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第２の実施の形態に係る位置計測装置を示すブロック図である。
【図９】本発明の第２の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第３の実施の形態に係る位置計測装置を示すブロック図である。
【図１１】本発明の第３の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第４の実施の形態に係る位置計測装置を示すブロック図である。
【図１３】本発明の第４の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第５の実施の形態に係る位置計測装置を示すブロック図である。
【図１５】本発明の第５の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１６】本発明の第６の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第７の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【符号の説明】
【０１７２】
１０、２１０、３１０、４１０、５１０ 位置計測装置
１２、３１２ 運動計測部
１４、２１４ 移動量計測部
１６ 位置推定部
２０ 画像撮像部
２４ 運動推定部
２６ ＧＰＳ
２８、２２８、３２８、４２８、５２８ 移動量推定部
２２６ 車速センサ
３１２、４１２、５１２ 運動移動量計測部
５２８ 移動量推定部
Ｎｐ パルス数
【技術分野】
【０００１】
本発明は、移動量計測装置及び位置計測装置に係り、特に、移動体の移動量を計測する移動量計測装置、及び移動体の移動量に基づいて移動体の位置を計測する位置計測装置に関する。
【背景技術】
【０００２】
従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己（車）運動認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて運動の回転を求め、近傍用のステレオカメラを用いて移動量としての並進を求める。
【特許文献１】特開２００３−２４７８２４
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、ロバスト性が得られない、という問題がある。
【０００４】
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる移動量計測装置及び位置計測装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために第１の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【０００６】
第１の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。そして、移動量算出手段によって、測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出する。
【０００７】
このように、同一の衛星群からの信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０００８】
上記の第１の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段を更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０００９】
上記の第１の発明に係る移動量計測装置は、移動体の速度に応じてパルス信号を出力する速度センサを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合には、速度センサのパルス信号に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１０】
上記の第１の発明に係る位置計測装置は、移動体の外部を撮像する単眼の撮像手段と、移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、測位手段によって第１時刻に測位された移動体の位置と、測位手段によって第２時刻に測位された移動体の位置とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出し、第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出し、第１時刻と第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出することができる。これによって、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１１】
上記の第１の発明に係る測位手段は、衛星群の各衛星から衛星を識別するための識別情報を含む信号を複数受信し、移動量算出手段は、測位手段によって識別情報の組み合わせが同一の衛星群から受信した複数の信号に基づいて測位された移動体の複数の位置に基づいて、移動体の移動量を算出することができる。このように、衛星からの信号に含まれる識別情報によって、同一の衛星群から信号を受信したか否かを判断することができる。
【００１２】
第２の発明に係る移動量計測装置は、移動体の外部を撮像する単眼の撮像手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【００１３】
第２の発明に係る移動量計測装置によれば、単眼の撮像手段によって、移動体の外部を撮像し、速度センサによって、移動体の速度に応じてパルス信号を出力する。そして、移動量算出手段によって、第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、第１時刻から第２時刻までの期間に出力されたパルス信号に基づいて、移動体の移動量を算出する。また、移動量算出手段によって、第１時刻から第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、撮像手段によって第１時刻に撮像された画像と第２時刻に撮像された画像とに基づいて、第１時刻から第２時刻までの期間における移動体の移動量を算出する。
【００１４】
このように、パルス信号のパルス数が大きい場合には、速度センサのパルス信号に基づいて移動量を算出し、パルス信号のパルス数が小さい場合には、単眼の撮像手段によって撮像された画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１５】
第３の発明に係る移動量計測装置は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、前記移動体の外部を撮像する単眼の撮像手段と、前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、前記移動体の速度に応じてパルス信号を出力する速度センサと、前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段とを含んで構成されている。
【００１６】
第３の発明に係る移動量計測装置によれば、測位手段によって、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する。単眼の撮像手段によって、移動体の外部を撮像し、距離算出手段によって、撮像手段によって撮像された複数の画像に基づいて、画像の特徴点までの距離を算出する。速度センサによって、移動体の速度に応じてパルス信号を出力する。
【００１７】
そして、移動量算出手段によって、測位手段によって測位された移動体の位置、距離算出手段によって算出された距離、及び速度センサから出力されたパルス信号に基づいて、移動体の移動量を算出する。
【００１８】
このように、衛星群から受信した複数の信号に基づく移動体の位置、複数の画像に基づく距離、及び速度センサのパルス信号に基づいて、移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００１９】
第４の発明に係る位置計測装置は、上記の移動量計測装置と、前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つとに基づいて、前記移動体の位置を計測する位置計測手段とを含んで構成されている。
【００２０】
本発明に係る位置計測装置によれば、移動量計測装置によって、移動体の移動量を算出し、また、運動推定手段によって、移動体の運動の３軸角速度の少なくとも１つを推定する。
【００２１】
そして、位置計測手段によって、移動量計測装置によって算出された移動体の移動量と、運動推定手段によって推定された移動体の運動の３軸角速度の少なくとも１つとに基づいて、移動体の位置を算出する。
【００２２】
このように、ロバスト性を確保して精度よく計測された移動体の移動量と、推定された移動体の運動とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる。
【００２３】
また、上記の運動推定手段は、移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、位置計測手段は、移動量計測装置によって算出された移動体の移動量と、運動推定手段によって推定された移動体の運動の３軸角速度の少なくとも１つ及び並進方向とに基づいて、移動体の位置を計測することができる。
【発明の効果】
【００２４】
以上説明したように、本発明の移動量計測装置によれば、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる、という効果が得られる。
【００２５】
本発明の位置計測装置によれば、ロバスト性を確保して精度よく計測された移動体の移動量と、推定された移動体の運動とに基づいて、移動量の位置を算出することにより、簡易な構成で、ロバスト性を確保して、移動体の位置を精度よく計測することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００２６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された位置計測装置に本発明を適用した場合を例に説明する。
【００２７】
図１に示すように、第１の実施の形態に係る位置計測装置１０は、自車両の運動を計測する運動計測部１２と、自車両の移動量を計測する移動量計測部１４と、計測された自車両の運動及び移動量に基づいて、自車両の位置を推定する位置推定部１６とを備えている。
【００２８】
運動計測部１２は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部２０と、撮像画像に基づいて、自車両の運動を推定する運動推定部２４とを備えている。
【００２９】
運動推定部２４では、自車両の運動として、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定する。なお、本実施の形態では、運動推定部２４によって、並進量（並進成分スカラ量）は推定しない。
【００３０】
本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。
【００３１】
運動推定部２４は、画像撮像部２０によって撮像された複数の撮像画像に基づいて、３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出する。
【００３２】
移動量計測部１４は、複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、自車両の位置を測定するＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置から移動量を推定する移動量推定部２８とを備えている。移動量推定部２８は、ＧＰＳ２６による測位位置から、自車両の移動量として並進成分スカラ量又は車速を推定する。例えば、第１時刻と第１時刻から計測単位期間Ｔｍｓ経過した第２時刻との各々の測位位置に基づいて、計測単位期間Ｔｍｓの車速を推定する。
【００３３】
ここで、本発明の原理について説明する。ＧＰＳ２６によって測定される測位位置は、ランダム誤差とオフセット誤差とを有している。ランダム誤差は、時々刻々と変化するものであるが、現在では、ＳＡ(Ｓｅｌｅｃｔｉｖｅ Ａｖａｉｌａｂｉｌｉｔｙ)という規制が解除され、かつ、受信機の性能が向上しているため、安定してＧＰＳ信号が得られる環境下にいる場合には、ランダム誤差の変動分が比較的小さい。
【００３４】
しかし、ＧＰＳ２６によって測定される測位位置は、長周期の誤差も有しているため、短時間で見るとオフセットがあるものと考えられる。この長周期の誤差は、ＧＰＳ信号が伝播する際に通過する、大気圏や電離層における遅延モデルで生じる誤差によるところが大きい。この誤差は、大気圏や電離層の状態変化や衛星の配置によって変化するため、長周期の誤差となる。
【００３５】
同一の衛星群から信号を受信し続けている場合は、時系列間において同一のオフセットが生じていると考えられるが、受信している複数の信号を送信している衛星群に変化があった場合には、オフセットの値に変化が生じる。
【００３６】
受信している複数の信号を送信している衛星群が変化しないときに、その測位位置の変化から移動量を算出した場合は、実際の移動量とよく一致した値が得られる。しかし、受信信号の衛星群が変化した場合には、衛星群が変化した前後で、オフセット量やその正負が変化するため、移動量を算出すると誤差が大きくなる。
【００３７】
図２に示すように、受信信号の衛星群に変化がない場合には、ＧＰＳの測位位置から算出した移動体の速度の時間変化と、移動体の実際の速度の時間変化とは、よく一致している。一方、受信信号の衛星群に変化があった場合には、衛星群が変化した前後で、ＧＰＳの測位位置から算出した移動体の速度の誤差が大きくなっている。
【００３８】
上記図２に示すようなＧＰＳの性質を考慮して、移動量推定部２８では、以下に説明するように、ＧＰＳ２６の測位位置を用いて自車両の移動量を推定する。
【００３９】
まず、ＧＰＳ２６によって測定された位置座標と、位置の測定に利用した複数の受信信号の各々に含まれる衛星の識別情報としての衛星番号を保持し、移動量を算出したい時刻間において、受信信号の衛星番号群を比較する。同一の衛星番号群である場合には、同一のオフセットが生じているものとして、移動量を算出したい時刻間の測位位置に基づいて、移動量を算出する。
【００４０】
一方、移動量を算出したい時刻間において、衛星番号群に変化がある場合には、過去の測位結果を保持したデータの中から、同一の衛星番号群の信号を受信した時刻を探索する。該当時刻が存在する場合には、現時刻の測位位置と過去の該当時刻の測位位置とに基づいて、移動量を算出し、また、該当時刻から現在の一時刻前までの移動量が既知であれば、一時刻前から現時刻までの間の移動量を算出することができる。
【００４１】
ただし、該当時刻と現時刻との間に、数１０秒以上の時刻差がある場合、測位位置に大きな隔たりがある場合、又は、該当時刻がない場合については、受信信号の衛星番号群に差異があったとしても、測位位置の変化から移動量を算出すると共に、「低信頼」あるいは「推定誤差大」の可能性があるものとして扱う。
【００４２】
位置推定部１６は、計測された自車両の運動及び移動量に基づいて、自車両の相対位置の変化量を算出し、過去にＧＰＳ２６で計測された絶対位置と相対位置の変化量とに基づいて、自車両の絶対位置を推定する。以下に、相対位置の変化量を算出する方法について図３を用いて説明する。
【００４３】
まず、図３に示すような座標系において、初期位置を（Ｘ（０）、Ｙ（０）、Ｚ（０））とし、時刻ｔにおいて、移動量（速度Ｖ（ｔ））、運動（３軸角速度（ヨー角速度φ（ｔ）、ピッチ角速度θ（ｔ）、ロール角速度ψ（ｔ））、並進方向を示す成分（ａｘ、ａｙ、ａｚ））が得られると、下記の（１）式のように、時刻ｔ＋Δｔにおける相対位置（Ｘ（ｔ＋Δｔ）、Ｙ（ｔ＋Δｔ）、Ｚ（ｔ＋Δｔ））を更新することができる。
【００４４】
【数１】
ただし、ａｘ／｜ａ｜、ａｙ／｜ａ｜、及びａｚ／｜ａ｜は、正規化された並進方向を示す成分の各々を表わす。
【００４５】
そして、自車両に搭載されたＧＰＳ２６から、経度及び緯度などで表わされる絶対位置情報が過去に取得されていた場合には、絶対位置情報が示す絶対位置と、算出された相対位置とに基づいて、自車両の現在の絶対位置を計測する。なお、絶対位置情報を、必ずしもＧＰＳ２６の測位位置から取得する必要はなく、例えば路側の位置が既知である設備から、通信などによって移動体の絶対位置情報を間欠的に取得してもよいし、予め位置が既知であるランドマーク情報が登録された地図などから、自車両の位置を取得してもよい。また、ＧＰＳ２６と同じ頻度で取得する必要もなく、移動量と運動とで補完できる間隔で取得すればよく、取得手段も問わない。
【００４６】
次に、第１の実施の形態に係る位置計測装置１０の作用について説明する。なお、位置計測装置１０を搭載した自車両が走行しているときに、自車両の位置を計測する場合を例に説明する。
【００４７】
位置計測装置１０において、図４に示す位置計測処理ルーチンが実行される。まず、ステップ１００において、ＧＰＳ２６によって自車両の位置を測位し、測位した位置を絶対位置の初期位置として設定する。
【００４８】
そして、ステップ１０２において、運動推定処理を行い、計測単位期間Ｔｍｓ（例えば、第１時刻から第２時刻までの期間）の自車両の運動の３軸角速度の推定値、あるいは３軸角速度及び並進方向を示す成分の推定値を取得する。上記ステップ１０２の運動推定処理は、図５に示す運動推定処理ルーチンによって実現される。以下に運動推定処理ルーチンについて説明する。
【００４９】
まず、ステップ１２０において、第１時刻及び第２時刻の各々のタイミングで画像撮像部２０によって自車両前方を撮像した第１の画像及び第２の画像を取得する。そして、ステップ１２２において、上記ステップ１２０で取得した第１の画像を複数の領域に分割する。このステップ１２２では、例えば、予め与えられた定数ｙ１、ｙ２を各領域の境界線（ｙ軸の切片座標）として、第１の画像を、画像上段の上方領域と、中段の中間領域と、下段の下方領域との３つの領域に分割する。
【００５０】
次のステップ１２４において、上記ステップ１２２で得られた各分割画像中から、それぞれ所定の数だけ特徴点（不動点であることが期待される点）を抽出する。ここで、特徴点とは、周囲の点とは区別でき、追跡が容易な点のことである。このステップ１２４では、例えば、二次元的に濃淡変化の勾配値が大きくなる画素を検出する、一般的な周知の方法（例えば、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に特徴点を抽出する。
【００５１】
そして、ステップ１２６において、上記ステップ１２４で抽出された第１の画像における特徴点の各々を、上記ステップ１２０で取得した第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検出する。このとき、第１時刻と第２時刻との間で対応する特徴点は、同一の不動点であることが期待されているので、第１の画像と第２の画像とで対応する点とその周囲の点との輝度は、殆ど変化しないという仮定を用い、この仮定に基づいて各特徴点の追跡を行う。この様な追跡処理を実施する際には、例えば、周知のルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などを用いると良い。また、各特徴点の追跡処理の後、第１の画像と第２の画像との間で対応する特徴点の各組の各画像内座標を、１つの適当な形式に整理する。
【００５２】
そして、ステップ１２８において、上記ステップ１２６で得られた第１時刻と第２時刻との間で互いに対応する特徴点の組から、第１時刻と第２時刻との間における画像撮像部２０の相対的な位置関係、即ち、画像撮像部２０を搭載した自車両の第１時刻と第２時刻との間における運動に関連する情報を推定する。
【００５３】
以下、図６を用いて、抽出された特徴点（不動点）と移動体の運動との関係を説明する。図６に示すように、移動体の運動は、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとから構成される。例えば、第１の画像上の特徴点ｘ１に対応する第２の画像上の特徴点をｘ２とすると、これらの特徴点は、同一の不動点を示す点である。
【００５４】
ここで、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、対応する２つの特徴点の関係を、以下の（２）式〜（４）式で表すことができる。
【００５５】
【数２】
【００５６】
上記の（２）式〜（４）式のように、上記の２時刻間における８組以上の互いに対応する特徴点の組（即ち、同一の不動点）が求まっていれば、それに基づいて、上記の基礎行列Ｆを算出することができる。そして、カメラのキャリブレーション行列Ｋが既知であれば、上記の式からも分かる様に、その２時刻間における移動体の運動、即ち、その２時刻間における自車両の回転行列Ｒと並進ベクトルｔとを求めることができる。
【００５７】
そして、ステップ１３０において、上記ステップ１２８で求めた自車両の回転行列Ｒと並進ベクトルｔとに基づいて、自車両の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を算出し、算出した３軸角速度、あるいは３軸角速度及び並進方向を示す成分を、自車両の運動の推定値として出力して、運動推定処理ルーチンを終了する。
【００５８】
上述したように、運動推定処理によって、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分が取得される。
【００５９】
次に、位置計測処理ルーチンのステップ１０４で、移動量推定処理を行い、第１時刻と第２時刻との間の自車両の移動量としての車速の推定値を取得する。上記の移動量推定処理は、図７に示す移動量推定処理ルーチンによって実現される。以下に、移動量推定処理ルーチンについて説明する。
【００６０】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において測位に利用した複数の受信信号に含まれる複数の衛星番号からなる衛星番号群とを取得する。そして、ステップ１４２で、上記ステップ１４０で取得した測位位置及び衛星番号群をメモリ（図示省略）に記憶する。
【００６１】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一であると判定された場合には、第１時刻と第２時刻とにおいて同一の衛星群から信号を受信したと判断し、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００６２】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおいて受信信号の衛星番号群が同一でないと判定された場合には、第１時刻と第２時刻とにおいて異なる衛星群から信号を受信したと判断し、ステップ１４８において、メモリに記憶された過去の衛星番号群から、第２時刻における衛星番号群と同一の衛星番号群である時刻を検索する。
【００６３】
そして、ステップ１５０において、上記ステップ１４８で第２時刻と同一の衛星番号群であった時刻が検索されたか否かを判定する。上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されたと判定された場合には、ステップ１５２において、メモリに記憶された、検索された時刻に測定された測位位置と、第２時刻における測位位置とに基づいて、自車両の車速を算出し、移動量推定処理ルーチンを終了する。
【００６４】
一方、上記ステップ１５０で、第２時刻と同一の衛星番号群であった時刻が検索されなかったと判定された場合には、ステップ１５４において、第１時刻及び第２時刻の各々で測定された測位位置に基づいて、信頼度の低い推定値として、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００６５】
上述したように、移動量推定処理によって、自車両の移動量としての車速が取得される。
【００６６】
次に、位置計測処理ルーチンのステップ１０６において、上記ステップ１０２で得られた自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、上記ステップ１０４で得られた自車両の車速とに基づいて、自車両の相対位置の変化量を算出する。次のステップ１０８では、上記ステップ１００で設定された初期位置と、上記ステップ１０６で算出された相対位置の変化量とに基づいて、自車両の絶対位置を算出する。
【００６７】
そして、ステップ１１２において、上記ステップ１０２からステップ１１０によって絶対位置を計測してから、計測単位期間Ｔｍｓが経過したか否かを判定し、計測単位期間Ｔｍｓが経過すると、ステップ１０２へ戻り、再び自車両の絶対位置を計測して、自車両の絶対位置を更新する。
【００６８】
以上説明したように、第１の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【００６９】
また、測位に利用した衛星群に変化がない場合の測位位置の変化を用いて、移動量を計測するため、移動量の計測精度を確保することができる。
【００７０】
また、ロバスト性を確保して精度よく計測された自車両の車速と、推定された自車両の運動の３軸角速度及び並進方向を示す成分とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【００７１】
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００７２】
第２の実施の形態では、ＧＰＳによる測位位置と、車速センサからの車速パルス信号のパルス数とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【００７３】
図８に示すように、第２の実施の形態に係る位置計測装置２１０の移動量計測部２１４は、ＧＰＳ２６と、車速に応じて車速パルス信号を出力する車速パルスセンサで構成され、自車両の対地速に応じた車速パルス信号を出力する車速センサ２２６と、ＧＰＳ２６によって測定された自車両の位置、及び車速センサ２２６の車速パルス信号に基づいて、移動量を推定する移動量推定部２２８とを備えている。
【００７４】
車速センサ２２６は、例えば、車速が６０ｋｍ／ｈ（回転数が６３７ｒｐｍ）のときに、１秒間におけるパルス数が１０．６となる車速パルス信号を出力する。
【００７５】
移動量推定部２２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、車速センサ２２６からの車速パルス信号のパルス数をカウントし、パルス数から車速を算出して移動量の推定値とする。
【００７６】
次に、第２の実施の形態に係る移動量推定処理ルーチンについて、図９を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して説明を省略する。
【００７７】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々において受信した信号に含まれる衛星番号群とを取得する。
【００７８】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００７９】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ２５２で、上記ステップ２５０で取得されたパルス数に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００８０】
なお、位置計測処理ルーチン及び運動推定処理ルーチンについては、第１の実施の形態と同様であるため、説明を省略する。
【００８１】
以上説明したように、第２の実施の形態に係る位置計測装置によれば、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、速度センサからの車速パルス信号のパルス数に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００８２】
また、速度センサの車速パルス信号を用いることで、短期的な移動量の計測精度を確保することができる。
【００８３】
次に、第３の実施の形態について説明する。なお、第１の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００８４】
第３の実施の形態では、ＧＰＳによる測位位置と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【００８５】
図１０に示すように、第３の実施の形態に係る位置計測装置３１０は、自車両の運動を計測すると共に、自車両の移動量を計測する運動移動量計測部３１２と、位置推定部１６とを備えている。
【００８６】
運動移動量計測部３１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、ＧＰＳ２６によって測定された自車両の位置、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部３２８とを備えている。
【００８７】
移動量推定部３２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とし、衛星番号群が変化した場合、画像撮像部２０によって撮像された複数の撮像画像に基づいて、車速を算出して移動量の推定値とする。
【００８８】
移動量推定部３２８は、撮像された複数の撮像画像に基づいて、車速を算出する場合、撮像画像から復元した３次元空間の路面平面を推定する。そして、予め計測された、画像撮像部２０の単眼カメラが設置されている路面上からの高さや単眼カメラの光軸の角度に基づいて、第１時刻及び第２時刻に撮像された撮像画像の各々から、路面上の近傍の特徴点までの距離を計測し、特徴点までの距離の変化に基づいて、車速を算出する。特に、低速時は移動量が少ないため、自車両周辺の路面を路面平面として推定してもよい。
【００８９】
次に、第３の実施の形態に係る移動量推定処理ルーチンについて、図１１を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して、詳細な説明を省略する。
【００９０】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。
【００９１】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【００９２】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像を画像撮像部２０から取得して、取得した撮像画像に基づいて、３次元空間の路面平面を推定する。
【００９３】
そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を算出すると共に、第２時刻に撮像された撮像画像から、上記と同じ特徴点までの距離を算出する。
【００９４】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【００９５】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【００９６】
このように、同一の衛星群からの信号に基づいて測位された自車両の複数の位置に基づいて、自車両の移動量を算出し、異なる衛星群から信号を受信した場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【００９７】
なお、上記の実施の形態では、撮像画像から推定された路面平面上の特徴点と、画像撮像部の高さ及び角度とに基づいて、特徴点までの距離を算出する場合を例に説明したが、これに限定されるものではなく、例えば、撮像画像から、大きさが既知の物体（例えば、白線や標識）を抽出し、抽出された物体の撮像画像上の大きさに基づいて、抽出された物体までの距離を算出するようにしてもよい。この場合には、算出された大きさが既知の物体までの距離の変化から、車速を算出するようにすればよい。
【００９８】
次に、第４の実施の形態について説明する。なお、第１の実施の形態及び第２の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【００９９】
第４の実施の形態では、速度センサからの車速パルス信号と、撮像画像とを組み合わせて、自車両の移動量を推定している点が第１の実施の形態と異なっている。
【０１００】
図１２に示すように、第４の実施の形態に係る位置計測装置４１０は、自車両の運動を計測すると共に、自車両の移動量を計測する運動移動量計測部４１２と、位置推定部１６と、ＧＰＳ２６とを備えている。
【０１０１】
運動移動量計測部４１２は、画像撮像部２０と、運動推定部２４と、車速センサ２２６と、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、自車両の移動量を推定する移動量推定部４２８とを備えている。
【０１０２】
ここで、本発明の原理について説明する。車速センサ２２６から出力される車速パルス信号は、連続値ではなく、一定の移動が行われた場合に出力される仕組みである。車速パルス信号から算出される車速の時系列変化と、実際の車速の時系列変化とを比較すると、車速パルス信号から算出される車速の値は、真値に対して大きなズレはないが、連続値ではなく離散値となる。信号処理を工夫して、推定精度を上げることは可能であるが、一般的な車速パルス信号の移動量分解能は４０ｃｍ程度であるため、低速時に、移動量が得られないことがある。
【０１０３】
本実施の形態では、上記の車速パルス信号を出力する車速センサ２２６の性質を考慮し、移動量推定部４２８は、車速センサ２２６から出力される車速パルス信号を用いて、以下に説明するように、自車両の移動量としての車速を推定する。
【０１０４】
まず、車速センサ２２６から計測単位期間Ｔｍｓに出力された車速パルス信号のパルス数をカウントする。このパルス数が、たとえば１以上である場合は、パルス数から換算される車速を移動量として出力する。計測単位期間Ｔｍｓ内にパルス数がカウントされなかった場合（パルス数が０の場合）には、移動の有無の計測が容易ではない。この場合、撮像画像に基づいて計測される計測単位期間Ｔｍｓの距離の変化から、移動量としての車速を算出する。なお、パルス数に関する閾値を暫定的に「１」としたが、特に１に限定されるものではなく、他の適切な値を閾値として採用してもよい。
【０１０５】
次に、第４の実施の形態における移動量推定処理ルーチンについて、図１３を用いて説明する。なお、第２の実施の形態及び第３の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１０６】
まず、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数Ｎｐを取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１０７】
一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。
【０１０８】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１０９】
なお、他の処理については、第１の実施の形態と同様であるため、説明を省略する。
【０１１０】
以上説明したように、第４の実施の形態に係る位置計測装置によれば、計測単位期間における車速パルス信号のパルス数が１以上である場合には、車速パルス信号のパルス数に基づいて移動量を算出し、計測単位期間における車速パルス信号のパルス数が０である場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、移動体の移動量を精度よく計測することができる。
【０１１１】
また、自車両が低速時には、低速時の計測精度が高くない車速パルス信号の代わりに、撮像画像から推定した近傍の路面平面上の特徴点までの距離の変化に基づいて、移動量を算出することにより、停止判定も可能になる。
【０１１２】
次に、第５の実施の形態について説明する。なお、第１の実施の形態及び第４の実施の形態と同様の構成となっている部分については、同一符号を付して説明を省略する。
【０１１３】
第５の実施の形態では、ＧＰＳによる測位位置と、撮像画像と、車速センサからの車速パルス信号とを組み合わせて、自車両の移動量を推定している点が、第４の実施の形態と異なっている。
【０１１４】
図１４に示すように、第５の実施の形態に係る位置計測装置５１０の運動移動量計測部５１２は、画像撮像部２０と、運動推定部２４と、ＧＰＳ２６と、車速センサ２２６と、ＧＰＳ２６によって測定された測位位置、車速センサ２２６から出力された車速パルス信号、及び画像撮像部２０によって撮像された撮像画像に基づいて、移動量を推定する移動量推定部５２８とを備えている。
【０１１５】
移動量推定部５２８は、計測単位期間Ｔｍｓにおいて、ＧＰＳ２６で位置を測定した時に利用した受信信号に含まれる衛星番号群に変化が無い場合、測位位置の変化から車速を算出して移動量の推定値とする。一方、衛星番号群が変化した場合、車速センサ２２６から計測単位期間Ｔｍｓに出力された車速パルス信号のパルス数をカウントし、このパルス数が、たとえば１以上である場合は、車速センサ２２６の車速パルス信号から車速を算出して移動量の推定値とする。計測単位期間Ｔｍｓ内にパルス数がカウントされなかった場合には、撮像画像に基づいて計測される計測単位期間Ｔｍｓの距離の変化から、移動量としての車速を算出する。
【０１１６】
次に、第５の実施の形態に係る移動量推定処理ルーチンについて、図１５を用いて説明する。なお、第１の実施の形態〜第４の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１１７】
まず、ステップ１４０において、ＧＰＳ２６から、第１時刻及び第２時刻の各々で測定された測位位置と、第１時刻及び第２時刻の各々における衛星番号群とを取得する。
【０１１８】
次のステップ１４４では、上記ステップ１４０で取得した第１時刻における衛星番号群と、第２時刻における衛星番号群とが同一であるか否かを判定する。上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一であると判定された場合には、ステップ１４６において、第１時刻及び第２時刻で測定された測位位置の変化に基づいて、自車両の移動量としての車速を算出して、移動量推定処理ルーチンを終了する。
【０１１９】
一方、上記ステップ１４４で、第１時刻と第２時刻とにおける衛星番号群が同一でないと判定された場合には、ステップ２５０において、第１時刻から第２時刻までの間に車速センサ２２６から出力された車速パルス信号のパルス数を取得する。そして、ステップ４５０において、上記ステップ２５０で取得されたパルス数Ｎｐが、１以上であるか否かを判定する。上記ステップ４５０で、パルス数Ｎｐが１以上であると判定された場合には、ステップ２５２で、上記ステップ２５０で取得されたパルス数Ｎｐに基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１２０】
一方、上記ステップ４５０で、パルス数Ｎｐが０であると判定された場合には、ステップ３５０において、第１時刻より前に撮像された撮像画像に基づいて、３次元空間の路面平面を推定する。そして、ステップ３５２で、画像撮像部２０から第１時刻及び第２時刻の各々で撮像された撮像画像を取得し、ステップ３５４において、第１時刻及び第２時刻の各々に撮像された撮像画像から、上記ステップ３５０で推定された路面平面上の特徴点までの距離を各々算出する。
【０１２１】
次のステップ３５６では、上記ステップ３５４で算出された第１時刻に対する距離と第２時刻に対する距離との変化に基づいて、自車両の車速を算出して、移動量推定処理ルーチンを終了する。
【０１２２】
このように、計測単位期間の第１時刻と第２時刻とにおいて、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が大きい場合には、速度センサからの車速パルス信号に基づいて移動量を算出し、異なる衛星群から信号を受信した場合であって、かつ、車速パルス信号のパルス数が小さい場合には、単眼のカメラによって撮像された撮像画像に基づいて移動量を算出するため、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【０１２３】
次に、第６の実施の形態について説明する。なお、第６の実施の形態に係る位置計測装置は、第５の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１２４】
第６の実施の形態では、カルマンフィルタ等を用いた最適処理手法によって、自車両の移動量としての車速を推定している点が第５の実施の形態と異なっている。
【０１２５】
第６の実施の形態に係る位置計測装置５１０の移動量推定部５２８では、画像撮像部２０の撮像画像に基づいて計測される距離と、ＧＰＳ２６によって測定される測位位置と、車速センサ２２６からの車速パルス信号のパルス数とを観測値とし、自車両の車速を推定値として、カルマンフィルタによる最適処理手法を用いた処理を行う。
【０１２６】
カルマンフィルタによる最適処理手法では、画像撮像部２０の単眼カメラの撮像画像に基づいて計測される距離の観測値ｙｉｍａｇｅが得られた場合に、観測値ｙｉｍａｇｅのバラつきを考慮して、観測ノイズｗｉｍａｇｅを設定し、観測行列Ｈｉｍａｇｅについて、観測値ｙｉｍａｇｅと推定値ｘとの関係式から、観測行列Ｈｉｍａｇｅを設定する。
【０１２７】
また、ＧＰＳ２６によって測位された測位位置の観測値ｙｇｐｓが得られた場合に、観測値ｙｇｐｓのバラつきを考慮して、観測ノイズｗｇｐｓを設定し、観測行列Ｈｇｐｓについて、観測値ｙｇｐｓと推定値ｘとの関係式から、観測行列Ｈｇｐｓを設定する。
【０１２８】
また、車速センサ２２６から出力された車速パルス信号のパルス数の観測値ｙｐｕｌｓが得られた場合に、観測値ｙｐｕｌｓのバラつきを考慮して、観測ノイズｗｐｕｌｓを設定し、観測行列Ｈｐｕｌｓについて、観測値ｙｐｕｌｓと推定値ｘとの関係式から、観測行列Ｈｐｕｌｓを設定する。
【０１２９】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１３０】
次に、第６の実施の形態に係る位置計測装置５１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１３１】
第６の実施の形態に係る位置計測装置５１０では、上述した移動量推定処理ルーチンにおけるステップ３５０からステップ３５４までの処理と同様の処理が繰り返し実行され、算出された路面平面上の特徴点までの距離が繰り返し出力される。また、ＧＰＳ２６によって自車両の位置が繰り返し測定される。また、車速センサ２２６から、車速パルス信号が連続して出力され、パルス数が繰り返しカウントされる。
【０１３２】
また、位置計測装置５１０において、図１６に示す移動量推定処理ルーチンが実行される。まず、ステップ６００において、撮像画像に基づいて計測される路面平面上の特徴点までの距離を取得し、ステップ６０２において、ＧＰＳ２６から測定された測位位置を取得する。そして、ステップ６０４において、車速センサ２２６からの車速パルス信号に基づいてカウントされたパルス数を取得する。
【０１３３】
そして、ステップ６０６において、上記ステップ６００で取得した距離と、上記ステップ６０２で取得した測位位置と、上記ステップ６０４で取得したパルス数とに基づいて、移動量としての車速の推定値ｘに対して、初期値を設定する。次のステップ６０８では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ６１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１３４】
次のステップ６１２において、現タイミングに、撮像画像に基づいて計測される路面平面上の特徴点までの距離、ＧＰＳ２６から測定された測位位置、及び車速センサ２２６からの車速パルス信号のパルス数の何れかが、観測値として入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ６１４において、上記ステップ６１０で算出された推定値ｘの予測値の車速を、自車両の移動量の推定値として出力し、ステップ６０８へ戻る。
【０１３５】
上記ステップ６１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ６１６において、どの観測値が入力されたのかを判定する。
【０１３６】
上記ステップ６１６で、観測値としてＧＰＳ２６から測定された測位位置が入力されたと判定された場合には、ステップ６１８において、入力された測位位置を観測値として、観測行列Ｈｇｐｓ、観測値ｙｇｐｓ、及び観測ノイズｗｇｐｓを設定する。
【０１３７】
一方、上記ステップ６１６で、観測値として、撮像画像に基づいて計測される路面平面上の特徴点までの距離が入力されたと判定された場合には、ステップ６２０において、入力された特徴点までの距離を観測値として、観測行列Ｈｉｍａｇｅ、観測値ｙｉｍａｇｅ、及び観測ノイズｗｉｍａｇｅを設定する。
【０１３８】
また、上記ステップ６１６で、観測値として、車速センサ２２６からの車速パルス信号のパルス数が入力されたと判定された場合には、ステップ６２２において、入力されたパルス数を観測値として、観測行列Ｈｐｕｌｓ、観測値ｙｐｕｌｓ、及び観測ノイズｗｐｕｌｓを設定する。
【０１３９】
そして、ステップ６２４において、上記ステップ６１８、上記ステップ６２０、又は上記ステップ６２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ６２６では、上記ステップ６２４で算出された推定値ｘの車速を、自車両の移動量の推定値として出力し、ステップ６０８へ戻る。
【０１４０】
このように、カルマンフィルタを用いた最適処理手法によって、自車両の外部を撮像した複数の撮像画像に基づいて計測された路面平面上の特徴点までの距離と、ＧＰＳによって測定された測位位置と、車速センサからの車速パルス信号のパルス数とに基づいて、自車両の移動量の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の移動量を精度よく計測することができる。
【０１４１】
また、ロバスト性を確保して精度よく計測された自車両の移動量と、推定された自車両の運動の３軸角速度及び並進方向を示す成分とに基づいて、自車両の位置を計測することにより、簡易な構成で、ロバスト性を確保して、自車両の位置を精度よく計測することができる。
【０１４２】
なお、上記の実施の形態では、カルマンフィルタで推定する例を記述したが、周知のパーティクルフィルタなど、他の最適推定手法を用いてもよく、適切に解を推定できる手法であれば、その方法はこの限りではない。
【０１４３】
また、誤差共分散を、推定された自車両の移動量の推定値に対する信頼度として用いてもよい。例えば、推定値と共に算出された誤差共分散によって、推定値の信頼度が低く、推定値の誤差が大きいと判断される場合に、この推定値を、自車両の移動量の推定値として採用しないようにしてもよい。
【０１４４】
次に、第７の実施の形態について説明する。なお、第７の実施の形態に係る位置計測装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【０１４５】
第７の実施の形態では、カルマンフィルタを用いた最適処理手法によって、自車両の位置を計測している点が第１の実施の形態と異なっている。
【０１４６】
第７の実施の形態に係る位置計測装置１０の位置推定部１６では、運動推定部２４によって推定された運動（３軸角速度、並進方向を示す成分）、移動量推定部２８によって推定された移動量（車速）、及びＧＰＳ２６によって測定された絶対位置(Ｘｇ,Ｙｇ,Ｚｇ)を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行う。
【０１４７】
カルマンフィルタを用いた最適処理手法では、運動推定部２４によって推定された運動の観測値ｙｍｏｖｅが得られた場合に、観測値ｙｍｏｖｅのバラつきを考慮して、観測ノイズｗｍｏｖｅを設定し、観測行列Ｈｍｏｖｅについて、観測値ｙｍｏｖｅと推定値ｘとの関係式から、観測行列Ｈｍｏｖｅを設定する。
【０１４８】
また、移動量推定部２８によって推定された移動量の観測値ｙｔｒａｖが得られた場合に、観測値ｙｔｒａｖのバラつきを考慮して、観測ノイズｗｔｒａｖを設定し、観測行列Ｈｔｒａｖについて、観測値ｙｔｒａｖと推定値ｘとの関係式から、観測行列Ｈｔｒａｖを設定する。
【０１４９】
また、ＧＰＳ２６によって測定された絶対位置の観測値ｙｇｐｓが得られた場合に、観測値ｙｇｐｓのバラつきを考慮して、観測ノイズｗｇｐｓを設定し、観測行列Ｈｇｐｓについて、観測値ｙｇｐｓと推定値ｘとの関係式から、観測行列Ｈｇｐｓを設定する。
【０１５０】
そして、設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、推定値ｘ及び推定誤差共分散行列を算出して出力する。観測値が得られない場合には、推定値の予測値と推定誤差共分散行列の予測値とを算出して出力する。
【０１５１】
次に、第７の実施の形態に係る位置計測装置１０の作用について説明する。なお、第１の実施の形態と同様の部分については、同一符号を付して詳細な説明を省略する。
【０１５２】
第７の実施の形態に係る位置計測装置１０では、第１の実施の形態で説明した運動推定処理ルーチンが繰り返し実行され、また、上述した移動量推定処理ルーチンが繰り返し実行される。また、ＧＰＳ２６によって、所定時間ごと（例えば１秒ごと）に自車両の絶対位置が測定される。
【０１５３】
また、位置計測装置１０において、図１７に示す位置計測処理ルーチンが実行される。まず、ステップ７００において、運動推定処理ルーチンによって推定された３軸角速度及び並進方向を示す成分を取得し、ステップ７０２において、移動量推定処理ルーチンによって推定された車速を取得する。次のステップ７０４では、ＧＰＳ２６によって測定された自車両の絶対位置を取得する。
【０１５４】
そして、ステップ７０６において、上記ステップ７００で取得した３軸角速度及び並進方向を示す成分と、上記ステップ７０２で取得した車速と、上記ステップ７０４で取得した絶対位置とに基づいて、絶対位置、方位角、車速、及び３軸角速度の推定値ｘに対して、初期値を設定する。次のステップ７０８では、時間ｔｉ−１にΔｔを加算して、時間ｔｉに更新する。そして、ステップ７１０において、１時刻前の推定値ｘに基づいて、推定値ｘの予測値を算出すると共に、推定誤差共分散の予測値を算出する。
【０１５５】
次のステップ７１２において、現タイミングに、観測値として、運動推定処理ルーチンによって推定された３軸角速度、移動推定処理ルーチンによって推定された車速、又はＧＰＳ２６によって測定された絶対位置が入力されたか否かを判定し、現タイミングに観測値が入力されなかった場合には、ステップ７１４において、上記ステップ７１０で算出された推定値ｘの予測値の絶対位置を、自車両の絶対位置の推定値として出力し、ステップ７０８へ戻る。
【０１５６】
上記ステップ７１２において、現タイミングに観測値が入力されたと判定された場合には、ステップ７１６において、上記ステップ７１２でどの観測値が入力されたかを判定する。
【０１５７】
上記ステップ７１６で、観測値として、運動推定処理ルーチンで推定された３軸角速度及び並進方向を示す成分が入力されたと判定された場合には、ステップ７１８において、入力された３軸角速度及び並進方向を示す成分を観測値として、観測行列Ｈｍｏｖｅ、観測値ｙｍｏｖｅ、及び観測ノイズｗｍｏｖｅを設定する。
【０１５８】
一方、上記ステップ７１６で、観測値として、移動量推定処理ルーチンによって推定された車速が入力されたと判定された場合には、ステップ７２０において、入力された車速を観測値として、観測行列Ｈｔｒａｖ、観測値ｙｔｒａｖ、及び観測ノイズｗｔｒａｖを設定する。
【０１５９】
また、上記ステップ７１６で、観測値として、ＧＰＳ２６によって測定された絶対位置が入力されたと判定された場合には、ステップ７２２において、入力された絶対位置を観測値として、観測行列Ｈｇｐｓ、観測値ｙｇｐｓ、及び観測ノイズｗｇｐｓを設定する。
【０１６０】
そして、ステップ７２４において、上記ステップ７１８、上記ステップ７２０、又は上記ステップ７２２で設定された観測行列、観測値、及び観測ノイズに基づいて、カルマンフィルタで、カルマンゲインＫ、推定値ｘ、及び推定誤差共分散行列を算出する。次のステップ７２６では、上記ステップ７２４で算出された推定値ｘの絶対位置を、自車両の絶対位置の推定値として出力し、ステップ７０８へ戻る。
【０１６１】
このように、カルマンフィルタを用いた最適処理手法によって、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、推定された自車両の車速と、ＧＰＳによって測位された位置とに基づいて、自車両の絶対位置の推定値を算出することにより、簡易な構成で、ロバスト性を確保して、自車両の絶対位置を精度よく計測することができる。
【０１６２】
なお、上記の実施の形態では、カルマンフィルタを用いた位置計測処理において、推定された自車両の運動の３軸角速度及び並進方向を示す成分、推定された自車両の車速、及びＧＰＳによって測位された絶対位置が、観測値として入力される場合を例に説明したが、これに限定されるものではない。例えば、推定された自車両の運動の３軸角速度及び並進方向を示す成分と、及び推定された自車両の車速とに基づいて推定された自車両の相対位置の変化量が、観測値として、カルマンフィルタを用いた位置計測処理に入力されるようになっていてもよい。この場合には、推定された自車両の相対位置の変化量、及びＧＰＳによって測位された絶対位置を観測値とし、自車の絶対位置、方位角、車速、及び３軸角速度を推定値として、カルマンフィルタを用いた最適処理手法による位置計測処理を行うようにすればよい。
【０１６３】
また、カルマンフィルタに限らず、パーティクルフィルタなどの最適推定手法を用いてもよい。
【０１６４】
次に、第８の実施の形態について説明する。第８の実施の形態では、ジャイロセンサによって、自車両の運動の３軸角速度を検出している。
【０１６５】
第８の実施の形態に係る位置計測装置は、ジャイロセンサによって検出された自車両の運動の３軸角速度と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。
【０１６６】
次に、第９の実施の形態について説明する。第９の実施の形態では、ジャイロセンサ及び単眼カメラによって撮像された撮像画像に基づいて、自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分を推定している。
【０１６７】
第９の実施の形態に係る位置計測装置は、ジャイロセンサ及び撮像画像によって推定された自車両の運動の３軸角速度、あるいは３軸角速度及び並進方向を示す成分と、移動量推定部によって推定された自車両の移動量とに基づいて、自車両の位置を推定する。
【０１６８】
なお、上記の第１の実施の形態〜第９の実施の形態では、撮像画像又はジャイロセンサに基づいて３軸角速度を推定する場合を例に説明したが、これに限定されるものではなく、撮像画像又はジャイロセンサに基づいて、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを推定するように構成してもよい。
【０１６９】
また、自車両の移動量として、車速を推定する場合を例に説明したが、これに限定されるものではなく、移動量として、移動距離又は移動ベクトルを推定するようにしてもよい。
【０１７０】
また、位置計測処理をオンラインで逐次処理する場合を例に説明したが、これに限定されるものではなく、自車両の運動の推定値及び移動量の推定値を取得した結果を用いて、オフラインで後処理として位置計測処理を行うようにしてもよい。
【図面の簡単な説明】
【０１７１】
【図１】本発明の第１の実施の形態に係る位置計測装置を示すブロック図である。
【図２】ＧＰＳの測位位置に基づいて算出される車速の時間変化と実際の車速の時間変化とを示すグラフである。
【図３】相対位置の変化量の算出方法を説明するための座標系を示すイメージ図である。
【図４】本発明の第１の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【図５】本発明の第１の実施の形態に係る位置計測装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図６】抽出された特徴点と移動体の運動との関係を説明するための図である。
【図７】本発明の第１の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第２の実施の形態に係る位置計測装置を示すブロック図である。
【図９】本発明の第２の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第３の実施の形態に係る位置計測装置を示すブロック図である。
【図１１】本発明の第３の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第４の実施の形態に係る位置計測装置を示すブロック図である。
【図１３】本発明の第４の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第５の実施の形態に係る位置計測装置を示すブロック図である。
【図１５】本発明の第５の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１６】本発明の第６の実施の形態に係る位置計測装置における移動量推定処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第７の実施の形態に係る位置計測装置における位置計測処理ルーチンの内容を示すフローチャートである。
【符号の説明】
【０１７２】
１０、２１０、３１０、４１０、５１０ 位置計測装置
１２、３１２ 運動計測部
１４、２１４ 移動量計測部
１６ 位置推定部
２０ 画像撮像部
２４ 運動推定部
２６ ＧＰＳ
２８、２２８、３２８、４２８、５２８ 移動量推定部
２２６ 車速センサ
３１２、４１２、５１２ 運動移動量計測部
５２８ 移動量推定部
Ｎｐ パルス数
【特許請求の範囲】
【請求項１】
複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項２】
前記移動体の外部を撮像する単眼の撮像手段を更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項３】
前記移動体の速度に応じてパルス信号を出力する速度センサを更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項４】
前記移動体の外部を撮像する単眼の撮像手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項５】
移動体の外部を撮像する単眼の撮像手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサと、
第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項６】
複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
前記移動体の外部を撮像する単眼の撮像手段と、
前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサと、
前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項７】
請求項１〜請求項６の何れか１項記載の移動量計測装置と、
前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、
前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つとに基づいて、前記移動体の位置を計測する位置計測手段と、
を含む位置計測装置。
【請求項８】
前記運動推定手段は、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、
前記位置計測手段は、前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つ及び前記並進方向とに基づいて、前記移動体の位置を計測する請求項７記載の位置計測装置。
【請求項１】
複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
前記測位手段によって同一の衛星群から受信した複数の信号に基づいて測位された前記移動体の複数の位置に基づいて、前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項２】
前記移動体の外部を撮像する単眼の撮像手段を更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項３】
前記移動体の速度に応じてパルス信号を出力する速度センサを更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項４】
前記移動体の外部を撮像する単眼の撮像手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサとを更に含み、
前記移動量算出手段は、第１時刻と該第１時刻より後の第２時刻とにおいて同一の衛星群から複数の信号を受信した場合、前記測位手段によって前記第１時刻に測位された前記移動体の位置と、前記測位手段によって前記第２時刻に測位された前記移動体の位置とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻と前記第２時刻とにおいて異なる衛星群から複数の信号を受信した場合であって、かつ、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する請求項１記載の移動量計測装置。
【請求項５】
移動体の外部を撮像する単眼の撮像手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサと、
第１時刻から該第１時刻より後の第２時刻までの期間に出力されたパルス信号のパルス数が、所定数以上である場合、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号に基づいて、前記移動体の移動量を算出し、前記第１時刻から前記第２時刻までの期間に出力されたパルス信号のパルス数が、所定数未満である場合、前記撮像手段によって前記第１時刻に撮像された画像と前記第２時刻に撮像された画像とに基づいて、前記第１時刻から前記第２時刻までの期間における前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項６】
複数の衛星からなる衛星群から複数の信号を受信し、受信した複数の信号に基づいて、移動体の位置を測定する測位手段と、
前記移動体の外部を撮像する単眼の撮像手段と、
前記撮像手段によって撮像された複数の画像に基づいて、前記画像の特徴点までの距離を算出する距離算出手段と、
前記移動体の速度に応じてパルス信号を出力する速度センサと、
前記測位手段によって測位された前記移動体の位置、前記距離算出手段によって算出された前記距離、及び前記速度センサから出力されたパルス信号に基づいて、前記移動体の移動量を算出する移動量算出手段と、
を含む移動量計測装置。
【請求項７】
請求項１〜請求項６の何れか１項記載の移動量計測装置と、
前記移動体の運動の３軸角速度の少なくとも１つを推定する運動推定手段と、
前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つとに基づいて、前記移動体の位置を計測する位置計測手段と、
を含む位置計測装置。
【請求項８】
前記運動推定手段は、前記移動体の運動の３軸角速度の少なくとも１つ及び並進方向を推定し、
前記位置計測手段は、前記移動量計測装置によって算出された前記移動体の移動量と、前記運動推定手段によって推定された前記移動体の運動の前記３軸角速度の少なくとも１つ及び前記並進方向とに基づいて、前記移動体の位置を計測する請求項７記載の位置計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２００９−７４８６１（Ｐ２００９−７４８６１Ａ）
【公開日】平成２１年４月９日（２００９．４．９）
【国際特許分類】
【出願番号】特願２００７−２４２７０５（Ｐ２００７−２４２７０５）
【出願日】平成１９年９月１９日（２００７．９．１９）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２１年４月９日（２００９．４．９）
【国際特許分類】
【出願日】平成１９年９月１９日（２００７．９．１９）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]