JP2014153113A - 速度推定装置及びプログラム - Google Patents

Abstract

【課題】観測衛星数が少ない環境でも、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定する。
【解決手段】車速データＶ^t _wheel、ヨーレイトデータω^t、及びＧＰＳデータを時系列に取得し、相対方位算出部２２で、ω^tに基づいて、時系列の相対方位角θ^t _gyroを算出し、各々を時系列車速蓄積部２０、時系列相対方位蓄積部２４、及び時系列ＧＰＳデータ蓄積部２６に保存する。衛星方向速度算出部２８で、衛星方向速度Ｖｓ^t _iを算出する。自車速度推定部３０で、ＧＰＳデータに基づいて３次元速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータの時間変化分を、方位角、ピッチ角、及びクロックドリフトの時間変化、並びに車速の大きさで拘束した方程式に、観測値を代入して時刻数×各時刻の観測衛星数分の方程式を立式し、得られたパラメータを用いて、自車両の３次元速度ベクトルを推定する。
【選択図】図１

Description

本発明は、速度推定装置及びプログラムに係り、特に、ＧＰＳ（Global Positioning System、全地球測位システム）情報及びＩＮＳ（Inertial Navigation System、慣性航法システム）情報に基づいて、移動体の３次元速度ベクトルを推定する速度推定装置及びプログラムに関する。

従来、ＧＰＳ受信機により受信されるドップラー周波数と自車両の車輪速データとを統合し、自車両の速度を推定する技術が提案されている（例えば、非特許文献１参照）。非特許文献１に記載の技術では、現在時刻における車輪速データとドップラー周波数のデータとを取得し、車輪速データにより車速を拘束した上で、ドップラー周波数を用いて速度ベクトルを推定している。ドップラー周波数のみを用いて速度推定を行う場合、未知パラメータが４つ存在するため、４つ以上のＧＰＳ衛星が観測されることが必要であるのに対して、車輪速データを合わせて用いることで、３つ以上のＧＰＳ衛星が観測されれば速度推定が可能となる。

また、旋回角度センサと距離センサとにより検出した自律移動ベクトルを積算して自車位置を検出する自律航法手段と、ＧＰＳ衛星からの電波を受信して自車位置を検出するＧＰＳ航法手段とを備えたナビゲーション装置において、ＧＰＳ航法手段による車両の進行方向及び速度を示す複数の計測時点におけるＧＰＳ移動ベクトルと、自律航法手段による車両の進行方向及び速度を示す複数の対応する計測時点における自律移動ベクトルとの差分を示す複数の差分データの差に基づいて、ＧＰＳ航法手段により検出されたデータの信頼性を判断する装置が提案されている（例えば、特許文献１参照）。特許文献１に記載の装置では、ＧＰＳ航法手段により検出されたデータの信頼性が低いと判断された場合には、自律航法手段の検出値を用いて位置推定を行っている。

特開２００２−３２３５５１号公報

Y. Kojima, "Proposal for a new localization method using tightly coupled integration based on a precise estimation of trajectory from GPS Doppler", Proceedings of AVEC2010, Laughborough UK, 2010.

しかしながら、上記の非特許文献１に記載の技術では、１時刻分の観測データのみを用いて、「車輪速による車速拘束」という拘束条件のみを利用しているため、利用できるＧＰＳ衛星数に制限があり、かつ速度推定を行う際の拘束条件が少ない。このため、観測されたＧＰＳ衛星数（以下、観測衛星数という）が少ない環境（３未満）では、速度推定を行うことができない、という問題がある。

また、特許文献１記載の技術では、観測衛星数が４未満の場合では、ＧＰＳデータを棄却し、自律航法（ＩＮＳのみによる軌跡推定）を行うが、自律航法では、進行方位誤差及び進行距離誤差が累積するため、観測衛星数が４未満の場所が長く続いた場合（自律航法が長く続いた場合）には、自車位置の推定結果が大きくずれてしまう可能性がある、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、観測衛星数が少ない環境でも、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定することができる速度推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータを時系列に取得する取得手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、該方程式に含まれる時刻毎に変化する未知パラメータが、前記３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定する推定手段と、を含んで構成されている。

本発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータを時系列に取得する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における移動体の３次元速度ベクトルを推定するための方程式であって、方程式に含まれる時刻毎に変化する未知パラメータが、３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束された方程式に基づいて、移動体の３次元速度ベクトルを推定する。

このように、時系列に取得された複数のＧＰＳデータに基づいて移動体の３次元速度ベクトルを推定する場合に、方程式に含まれる時刻毎に変化する未知パラメータを、３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束することにより、未知パラメータ数を削減することができ、各時刻における観測衛星数が少ない環境でも未知パラメータ数以上の方程式を立式できる可能性が大きくなり、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定することができる

また、前記推定手段は、前記高さ方向の速度成分に関連する時間変化する情報を、前記移動体のピッチ角の時間変化とし、該移動体のピッチ角の時間変化を直線近似または曲線近似することにより拘束することができる。これにより、高さ方向の速度成分に関連する時間変化する情報をセンサ等により直接検出することなく、拘束条件として用いることができる。

また、前記推定手段は、前記曲線近似を２次曲線近似とすることができる。これにより、方程式に含まれる未知パラメータ数の増加を抑えつつ、多様な道路勾配に対して整合性を損なうことなく、ピッチ角の時間変化を近似させた拘束条件を設定することができる。

また、前記取得手段は、前記移動体の高度を検出する高度センサで検出された高度情報を時系列に取得し、前記推定手段は、前記高さ方向の速度成分に関連する時間変化する情報を、前記取得手段により取得された高度情報の時間変化とすることができる。このように、高度センサにより直接検出した高度情報を用いて、高さ方向の速度成分に関連する時間変化する情報による拘束条件を設定することにより、より精度良く３次元速度ベクトルを推定することができる。

また、前記取得手段は、前記移動体のヨーレイト及び前記移動体の速度の大きさの少なくとも一方を時系列に取得し、前記推定手段は、前記方程式に含まれる時刻毎に変化する未知パラメータが、前記高さ方向の速度成分に関連する時間変化する情報と共に、前記取得手段により取得された時系列のヨーレイトに基づいて算出された前記移動体の進行方向の方位角の時間変化、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化の少なくとも１つにより拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定することができる。このように、他の拘束条件も用いることにより、方程式に含まれる未知パラメータをより削減することができる。

また、本発明の速度推定装置は、前記取得手段により取得された高度情報の精度に基づいて、前記推定手段における該高度情報の使用可否を判定する高度情報使用可否判定手段を含んで構成することができる。

また、本発明の速度推定装置は、前記方位角の時間変化の算出精度に基づいて、前記推定手段における該方位角の時間変化の使用可否を判定する方位角使用可否判定手段を含んで構成することができる。

また、本発明の速度推定装置は、前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含んで構成することができる。

また、本発明の速度推定装置は、前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含んで構成することができる。

また、本発明の速度推定装置は、前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含んで構成することができる。

このように、高度情報、方位角の時間変化、及び速度の大きさの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、ＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より精度良く移動体の３次元速度ベクトルを推定することができる。

また、本実施の形態に係る速度推定プログラムは、コンピュータを、ＧＰＳ衛星から送信されたＧＰＳデータを時系列に取得する取得手段、及び前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、該方程式に含まれる時刻毎に変化する未知パラメータが、前記３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定する推定手段として機能させるためのプログラムである。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明の速度推定装置及びプログラムによれば、時系列に取得された複数のＧＰＳデータに基づいて移動体の３次元速度ベクトルを推定する場合に、方程式に含まれる時刻毎に変化する未知パラメータを、３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束することにより、未知パラメータ数を削減することができる。これにより、各時刻における観測衛星数が少ない環境でも未知パラメータ数以上の方程式を立式できる可能性が大きくなり、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定することができる、という効果が得られる。

第１の実施の形態に係る速度推定装置を示すブロック図である。 ＧＰＳデータに基づく３次元速度ベクトルの推定を説明するための図である。 第１の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。 第２の実施の形態に係る速度推定装置を示すブロック図である。 第２の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。 第３の実施の形態に係る速度推定装置を示すブロック図である。 第３の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、自車両の３次元速度ベクトルを推定する速度推定装置に、本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係る速度推定装置１０は、自車両の速度の大きさを検出する車速センサ１２と、自車両のヨーレイトを検出するヨーレイトセンサ１４と、ＧＰＳ衛星から送信されるＧＰＳデータを受信するＧＰＳ装置１６と、自車両の３次元速度ベクトルを推定するコンピュータ１８とを備えている。

車速センサ１２は、車輪速から出力された車速データを検出する。なお、車速センサ１２としては、レーザレーダ、ミリ波レーダ、ステレオカメラ等、ＧＰＳ装置１６以外であれば、車輪速を検出するセンサ以外を用いてもよい。

ヨーレイトセンサ１４は、ジャイロセンサなどを用いることができ、自車両のヨーレイトデータを検出する。なお、ヨーレイトセンサ１４としては、レーザレーダ、移動ステレオなど、ＧＰＳ装置１６以外であれば、ジャイロセンサ以外のセンサを用いてもよい。

ＧＰＳ装置１６は、測位及び速度推定に必要となる擬似距離、ドップラー周波数、衛星番号、衛星位置、衛星速度等を含むＧＰＳデータを、ＧＰＳ衛星から受信する。

コンピュータ１８は、ＣＰＵ、後述する速度推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、時系列に取得された車速データを蓄積する時系列車速蓄積部２０と、時系列に取得されたヨーレイトデータから相対方位角を算出する相対方位算出部２２と、時系列に算出された相対方位角を蓄積する時系列相対方位蓄積部２４と、時系列に取得されたＧＰＳデータを蓄積する時系列ＧＰＳデータ蓄積部２６と、ＧＰＳデータを用いて自車両からＧＰＳ衛星方向への３次元速度ベクトルを算出する衛星方向速度算出部２８と、未知パラメータの時間変化分を拘束した方程式に基づいて、自車両の３次元速度ベクトルを推定する自車速度推定部３０と、を含んだ構成で表すことができる。

時系列車速蓄積部２０には、車速センサ１２で検出された車速データが時系列に保存される。データ保存のトリガは、一定時間経過時でもよいし、一定距離走行時でもよい。なお、時刻ｔに車速センサ１２で検出された車速データをＶ^t _wheelとする。

相対方位算出部２２は、自車両の進行方向のある時点における方位角に対する時刻ｔの相対方位角θ^t _gyroを、ヨーレイトセンサ１４で検出されたヨーレイトデータを用いて、下記（１）式により算出する。なお、Δｔはタイムステップを示し、ω^tは時刻ｔに検出されたヨーレイトデータを示す。

時系列相対方位蓄積部２４には、相対方位算出部２２で算出された相対方位角θ^t _gyroが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。

時系列ＧＰＳデータ蓄積部２６には、ＧＰＳ装置１６で取得されたＧＰＳデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。

衛星方向速度算出部２８は、時系列ＧＰＳデータ蓄積部２６に蓄積されたＧＰＳデータに含まれるドップラー周波数を用いて、自車両からＧＰＳ衛星方向の３次元速度ベクトル（衛星方向速度）を算出する。

ここで、図２に、ドップラー周波数を用いた３次元速度ベクトル算出の概要を示す。ドップラー周波数を用いた速度推定では、観測された各ＧＰＳ衛星のドップラー周波数に基づき、各ＧＰＳ衛星方向への速度成分をそれぞれ算出し、各々を合成することで自車両の３次元速度ベクトルを求める。そのための各ＧＰＳ衛星方向への速度成分は下記（２）式により算出される。

ここで、Ｖｓ^t _iは自車両からＧＰＳ衛星ｉ方向への速度ベクトルの大きさ、Ｖｘｓ^t _i、Ｖｙｓ^t _i、Ｖｚｓ^t _iはＧＰＳ衛星ｉの各速度成分、Ｄ^t _iはドップラー周波数、ｆ１はＧＰＳデータの搬送波の周波数、Ｃは光速、ｒ^t _iは自車両とＧＰＳ衛星ｉ間の擬似距離、Ｇｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iは自車両からＧＰＳ衛星ｉへの視線ベクトル、Ｘ^t _si、Ｙ^t _si、Ｚ^t _siは衛星位置、Ｘ^t _v、Ｙ^t _v、Ｚ^t _vは自車両位置を示す。添え字ｔは時刻ｔのデータであることを表し、添え字ｉはｉ番目のＧＰＳ衛星のデータであることを表す。自車両位置は、擬似距離を用いた一般的な測位方法またはその他の測位方法を用いて算出した値を用いることができる。

（２）式に示す自車両から各ＧＰＳ衛星方向への速度ベクトルの大きさと、最終的に推定する自車両の３次元速度ベクトルとの関係式を下記（４）式に示す。なお、（Ｖｘ^t，Ｖｙ^t，Ｖｚ^t）は最終的に推定する自車両の３次元速度ベクトル、及びＣｂｖ^tはＧＰＳ装置１６の時計誤差の時間変化（クロックドリフト）である。

（４）式に示す連立方程式に、ＧＰＳデータから算出したＶｓ^t _i、Ｇｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iを代入して解くことにより、自車両の３次元速度ベクトルを推定することができる。衛星方向速度算出部２８では、このＶｓ^t _i、Ｇｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iを算出する。衛星方向速度Ｖｓ^t _iは、ドップラー周波数から求められる各ＧＰＳ衛星に対する自車両の相対速度、ＧＰＳデータに含まれる衛星位置の時系列データから求められる各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の衛星位置と自車両位置とから求められる衛星方向に基づいて算出することができる。Ｇｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iは、（３）式により算出することができる。

自車速度推定部３０は、後述する拘束条件で拘束された速度推定のための方程式に基づいて、自車両の３次元速度ベクトルを推定する。速度推定に用いる各観測値の時系列データの範囲は、現時刻からの経過時間や走行距離などに基づき決定する。以下では、速度推定に使用する時系列データの内、最古のデータの時刻をｔ＝０（初期時刻）として扱う。

ここで、本実施の形態の原理について説明する。

（４）式の連立方程式を解くにあたり、解を得るための条件は、通常「未知パラメータ数＝立式数」である、（４）式では、Ｖｘ^t、Ｖｙ^t、Ｖｚ^t、及びＣｂｖ^tの４つが未知パラメータであるため、時系列データの全時刻における未知パラメータ数は、「未知パラメータ数＝時刻数×４」となる。これに対して、「立式数＝時刻数×各時刻での観測衛星数」であるため、各時刻での観測衛星数が平均４つ以上ない場合には、速度推定結果が得られないことになる。特に、都市部のように建物の遮蔽により観測衛星数が少ない環境では、上記条件を満たすのは困難である。

そこで、本実施の形態では、以下の拘束条件を取り入れることで、未知パラメータ数を削減し、観測衛星数が少ない環境でも速度推定の解が得られる可能性を向上させる。第１の実施の形態で用いる拘束条件は、下記に示す拘束条件１〜４である。

拘束条件１は、推定する３次元速度ベクトルの各成分の大きさを、時系列車速蓄積部２０に蓄積された時系列の車速データＶ^t _wheelで拘束したものである。また、拘束条件１内のθ^tは自車両の時系列の方位角、及びφ_tは自車両の時系列のピッチ角である。

拘束条件２は、拘束条件１内の時間変化分である時系列の方位角θ^tを時系列相対方位蓄積部２４に蓄積された相対方位角θ^t _gyroで拘束したものである。拘束条件２内のθ⁰は初期時刻における自車両の進行方向の方位角である。

拘束条件３は、拘束条件１内の時間変化分である時系列のピッチ角φ^tを２次曲線で近似したものである。拘束条件３内のφ⁰は初期時刻におけるピッチ角、αは１次項の係数、及びβは２次項の係数である。これは、短い区間の道路勾配、すなわちピッチ角の時間変化は、概ね２次曲線で近似できるとの仮定の下での拘束条件である。なお、道路勾配の近似は２次近似に限定されず、０次近似や１次近似でもよいし、３次以上で近似してもよい。ただし、次数が低い場合には道路勾配の近似精度が低くなり、次数が高い場合には３次元速度ベクトルを推定する際の方程式に含まれる未知パラメータ数が多くなるため、双方のバランスの取れた適切な次数を設定すればよい。なお、２次近似とした場合には、未知パラメータ数の増加を抑えつつ、多様な道路勾配に対して整合性を損なわない近似となるため、第１の実施の形態における拘束条件３として用いるのに好適である。

拘束条件４は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件４内のＣｂｖ⁰は初期時刻におけるクロックドリフト、及びＡはクロックドリフトの時間変化の傾きを示す。

上記拘束条件１〜４を、（４）式に取り入れた結果、下記（５）式が得られる。

（５）式において、未知パラメータは、θ⁰、φ⁰、α、β、Ｃｂｖ⁰、及びＡであり、これらの未知パラメータを求めることで、自車両の３次元速度ベクトルを推定することができる。（５）式では、拘束条件１〜４を導入することにより、未知パラメータ数を６つまで削減したため、「立式数＝時刻数×各時刻の観測衛星数」が６以上であれば、速度推定結果を得ることができる。例えば、３時刻（ｔ₀，ｔ₁，ｔ₂）の観測衛星数がそれぞれ２であったとしても、観測されたＧＰＳ衛星からのＧＰＳデータを用いて、速度推定結果を算出可能である。

従って、自車速度推定部３０は、上記の本実施の形態の原理に従って、時間変化分を拘束した（５）式を用いて、自車両の３次元速度ベクトルを推定する。具体的には、時系列車速蓄積部２０に蓄積された時系列の車速データＶ^t _wheel、時系列相対方位蓄積部２４に蓄積された相対方位θ^t _gyro、並びに衛星方向速度算出部２８で算出された衛星方向速度Ｖｓ^t _i及びＧｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iを（５）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。これらの方程式を解いて未知パラメータθ⁰、φ⁰、α、β、Ｃｂｖ⁰、及びＡを求める。そして、求めたパラメータθ⁰及び時系列の相対方位角θ^t _gyroを拘束条件２に代入して時系列の方位角θ^tを求める。また、求めたパラメータφ⁰、α、及びβを拘束条件３に代入して時系列のピッチ角φ^tを求める。求めた時系列の方位角θ^t及び時系列のピッチ角φ^t、並びに時系列の車速データＶ^t _wheelを拘束条件１に代入して、自車両の３次元速度ベクトルを算出する。

次に、第１の実施の形態に係る速度推定装置１０の作用について説明する。

各時刻ｔにおいて、車速センサ１２により車速データＶ^t _wheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータω^tを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ１８において、図３に示す速度推定処理ルーチンが実行される。

ステップ１００で、観測値として、車速センサ１２により検出された車速データＶ^t _wheel、ヨーレイトセンサ１４により検出されたヨーレイトデータω^t、及びＧＰＳ装置１６により受信したＧＰＳデータを時系列に取得する。そして、車速データＶ^t _wheelを時系列車速蓄積部２０に保存すると共に、ＧＰＳデータを時系列ＧＰＳデータ蓄積部２６に保存する。

次に、ステップ１０２で、上記ステップ１００で取得した時系列のヨーレイトデータω^tに基づいて、（１）式により、時系列の相対方位角θ^t _gyroを算出する。そして、算出した時系列の相対方位角θ^t _gyroを時系列相対方位蓄積部２４に保存する。

次に、ステップ１０４で、上記ステップ１００で保存された時系列のＧＰＳデータに基づいて、（２）式により、衛星方向速度Ｖｓ^t _iを算出する。また、（３）式により、Ｇｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iを算出する。

次に、ステップ１０６で、上記ステップ１００で、時系列車速蓄積部２０に蓄積された時系列の車速データＶ^t _wheel、及び上記ステップ１０２で、時系列相対方位蓄積部２４に蓄積された相対方位θ^t _gyro、並びに上記ステップ１０４で算出された衛星方向速度Ｖｓ^t _i及びＧｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iを、拘束条件１〜４に基づいて時間変化分を拘束した（５）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。

次に、ステップ１０８で、上記ステップ１０６で立式した方程式を解いて、未知パラメータθ⁰、φ⁰、α、β、Ｃｂｖ⁰、及びＡを求める。そして、求めたパラメータθ⁰及び時系列の相対方位角θ^t _gyroを拘束条件２に代入して時系列の方位角θ^tを求める。また、求めたパラメータφ⁰、α、及びβを拘束条件３に代入して時系列のピッチ角φ^tを求める。求めた時系列の方位角θ^t及び時系列のピッチ角φ^t、並びに時系列の車速データＶ^t _wheelを拘束条件１に代入して、自車両の３次元速度ベクトルを算出し、速度推定処理ルーチンを終了する。

以上説明したように、第１の実施の形態に係る速度推定装置によれば、複数時刻におけるＧＰＳデータを用いて自車両の３次元速度ベクトルを推定する場合に、速度推定のための方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減する。この際、高さ方向の速度に関係する値として、時系列のピッチ角を、道路勾配を２次曲線で拘束した拘束条件を用いる。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定することができる。

なお、第１の実施の形態では、時系列のピッチ角の拘束条件と共に、車速データ、時系列の方位角、及びクロックバイアスの時間変化による拘束条件の全てを用いる場合について説明したが、これに限定されない。時系列のピッチ角の拘束条件のみを用いてもよいし、時系列のピッチ角の拘束条件と、車速データ、時系列の相対方位角、及びクロックバイアスの時間変化による拘束条件の少なくとも１つの拘束条件とを組み合わせて用いてもよい。

拘束条件として、時系列のピッチ角のみを用いた場合には、（５）式において、Ｖ^t _wheel、θ^t（θ^t＝θ⁰＋θ^t _gyroの拘束がないため、（５）式の「θ⁰＋θ^t _gyro」を「θ^t」に置き換え）、Ｃｂｖ^t（Ｃｂｖ^t＝Ｃｂｖ⁰＋Ａｔの拘束がないため、（５）式の「Ｃｂｖ⁰−Ａｔ」を「Ｃｂｖ^t」に置き換え）、φ⁰、α、及びβが未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×３＋３」となる。全ての拘束条件を適用した場合に比べると未知パラメータ数は多くなるが、時間変化する未知パラメータ数を削減することができるため、時刻数を多くとった場合には、未知パラメータ数を削減することができる。

また、拘束条件として、時系列のピッチ角とクロックドリフトの時間変化とを用いた場合には、（５）式において、Ｖ^t _wheel、θ^t（θ^t＝θ⁰＋θ^t _gyroの拘束がないため、（５）式の「θ⁰＋θ^t _gyro」を「θ^t」に置き換え）、Ｃｂｖ⁰、Ａ、φ⁰、α、及びβが未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋５」となり、上記と同様に、時間変化する未知パラメータ数を削減することができる。

また、拘束条件として、時系列のピッチ角と時系列の方位角とを用いた場合には、（５）式において、Ｖ^t _wheel、θ⁰、Ｃｂｖ⁰、Ａ、φ⁰、α、及びβが未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋６」となり、上記と同様に、時間変化する未知パラメータ数を削減することができる。

次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る速度推定装置において、第１の実施の形態に係る速度推定装置１０と同一の構成については、同一符号を付して詳細な説明を省略する。

図４に示すように、第２の実施の形態に係る速度推定装置２１０は、車速センサ１２と、ヨーレイトセンサ１４と、気圧センサ１５と、ＧＰＳ装置１６と、コンピュータ２１８とを備えている。

気圧センサ１５は、気圧の変化に基づいて、自車両の高度情報を検出する。なお、気圧センサ１５に替えて、自車両の高度情報を検出することができるセンサを用いてもよい。

また、コンピュータ２１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図４に示すように、時系列車速蓄積部２０と、相対方位算出部２２と、時系列相対方位蓄積部２４と、時系列ＧＰＳデータ蓄積部２６と、衛星方向速度算出部２８と、時系列に取得された高度情報を蓄積する時系列高度情報蓄積部３１と、自車速度推定部２３０と、を含んだ構成で表すことができる。

時系列高度情報蓄積部３１には、気圧センサ１５で検出された高度情報が時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。なお、時刻ｔに気圧センサ１５で検出された高度情報をｈ^t _baroとする。

自車速度推定部２３０は、第１の実施の形態における自車速度推定部３０と同様に、未知パラメータの時間変化分が拘束された速度推定のための方程式に基づいて、自車両の３次元速度ベクトルを推定する。自車速度推定部２３０では、下記の拘束条件５〜７を用いる。

拘束条件５は、推定する３次元速度ベクトルの進行方向及び車両幅方向の各成分の大きさを、時系列車速蓄積部２０に蓄積された時系列の車速データＶ^t _wheelで拘束し、かつ時系列の方位角θ^tを時系列相対方位蓄積部２４に蓄積された相対方位角θ^t _gyroで拘束したものである。

拘束条件６は、推定する３次元速度ベクトルの高さ方向の成分を、時系列高度情報蓄積部３１に蓄積された時系列の高度情報ｈ^t _boroで拘束したものである。拘束条件６内のΔｔは気圧センサ１５の出力周期である。

拘束条件７は、第１の実施の形態における拘束条件３と同様で、クロックドリフトの短時間内における変化は線形であると仮定した拘束条件である。

上記拘束条件５〜７を、（４）式に取り入れた結果、下記（６）式が得られる。

（６）式において、未知パラメータは、θ⁰、Ｃｂｖ⁰、及びＡであり、これらの未知パラメータを求めることで、自車両の３次元速度ベクトルを推定することができる。（６）式では、拘束条件５〜７を導入することにより、未知パラメータ数を３つまで削減したため、「立式数＝時刻数×各時刻の観測衛星数」が３以上であれば、速度推定結果を得ることができる。例えば、３時刻（ｔ₀，ｔ₁，ｔ₂）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのＧＰＳデータを用いて、速度推定結果を算出可能である。

自車速度推定部２３０は、具体的には、時系列車速蓄積部２０に蓄積された車速データＶ^t _wheel、時系列相対方位蓄積部２４に蓄積された相対方位θ^t _gyro、時系列高度情報蓄積部３１に蓄積された高度情報ｈ^t _baro、既知の気圧センサ１５の出力周期Δｔ、並びに衛星方向速度算出部２８で算出された衛星方向速度Ｖｓ^t _i及びＧｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iを（６）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。これらの方程式を解いて未知パラメータθ⁰、Ｃｂｖ⁰、及びＡを求める。そして、求めたパラメータθ⁰、時系列の車速データＶ^t _wheel、及び時系列の相対方位角θ^t _gyroを拘束条件１に代入して、自車両の３次元速度ベクトルの成分Ｖｘ^t及びＶｙ^tを算出する。また、時系列の高度情報ｈ^t _baro及び既知の気圧センサ１５の出力周期Δｔを拘束条件２に代入して、自車両の３次元速度ベクトルの成分Ｖｚ^tを算出する。

次に、第２の実施の形態に係る速度推定装置２１０の作用について説明する。

各時刻ｔにおいて、車速センサ１２により車速データＶ^t _wheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータω^tを検出し、気圧センサ１５により高度情報ｈ^t _baroを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ２１８において、図５に示す速度推定処理ルーチンが実行される。なお、第１の実施の形態における速度推定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。

ステップ２００で、観測値として、車速センサ１２により検出された車速データＶ^t _wheel、ヨーレイトセンサ１４により検出されたヨーレイトデータω^t、気圧センサ１５により検出された高度情報ｈ^t _baro、及びＧＰＳ装置１６により受信したＧＰＳデータを時系列に取得する。そして、車速データＶ^t _wheelを時系列車速蓄積部２０に保存し、高度情報ｈ^t _baroを時系列高度情報蓄積部３１に保存し、ＧＰＳデータを時系列ＧＰＳデータ蓄積部２６に保存する。

次に、ステップ１０２で、時系列の相対方位θ^t _gyroを算出し、次に、ステップ１０４で、衛星方向速度ｓ^t _i、並びに（４）式のＧｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iを算出する。

次に、ステップ２０２で、上記ステップ２００で、時系列車速蓄積部２０に蓄積された車速データＶ^t _wheel、時系列高度情報蓄積部３１に蓄積された高度情報ｈ^t _baro、及び上記ステップ１０２で、時系列相対方位蓄積部２４に蓄積された相対方位θ^t _gyro、並びに上記ステップ１０４で算出された衛星方向速度Ｖｓ^t _i及びＧｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iを、拘束条件５〜７に基づいて時間変化分を拘束した（６）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。

次に、ステップ１０８で、上記ステップ２０２で立式した方程式を解いて、自車両の３次元速度ベクトルを算出し、速度推定処理ルーチンを終了する。

以上説明したように、第２の実施の形態に係る速度推定装置によれば、推定する速度ベクトルの高さ方向の成分を、気圧センサにより検出された高度情報に基づいて拘束することにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、移動体の高さ方向の速度成分も含む３次元速度ベクトルを、安定して精度良く推定することができる。

なお、第２の実施の形態では、高度情報に基づく高さ方向の速度成分の拘束条件と共に、車速データ、時系列の方位角、及びクロックバイアスの時間変化による拘束条件の全てを用いる場合について説明したが、これに限定されない。高度情報に基づく高さ方向の速度成分の拘束条件のみを用いてもよいし、高度情報に基づく高さ方向の速度成分の拘束条件と、車速データ、時系列の方位角、及びクロックバイアスの時間変化による拘束条件の少なくとも１つの拘束条件とを組み合わせて用いてもよい。

拘束条件として、高度情報に基づく高さ方向の速度成分のみを用いた場合には、（６）式において、Ｖ^t _wheel、θ^t（θ^t＝θ⁰＋θ^t _gyroの拘束がないため、（６）式の「θ⁰＋θ^t _gyro」を「θ^t」に置き換え）、及びＣｂｖ^t（Ｃｂｖ^t＝Ｃｂｖ⁰＋Ａｔの拘束がないため、（６）式の「Ｃｂｖ⁰−Ａｔ」を「Ｃｂｖ^t」に置き換え）が未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×３」となり、未知パラメータ数を削減することができる。

また、拘束条件として、高度情報に基づく高さ方向の速度成分とクロックドリフトの時間変化とを用いた場合には、（６）式において、Ｖ^t _wheel、θ^t（θ^t＝θ⁰＋θ^t _gyroの拘束がないため、（６）式の「θ⁰＋θ^t _gyro」を「θ^t」に置き換え）、Ｃｂｖ⁰、及びＡが未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋２」となり、上記と同様に、未知パラメータ数を削減することができる。

また、拘束条件として、高度情報に基づく高さ方向の速度成分と時系列の方位角とを用いた場合には、（６）式において、Ｖ^t _wheel、θ⁰、Ｃｂｖ⁰、及びＡが未知パラメータとなる。そのため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋３」となり、上記と同様に、未知パラメータ数を削減することができる。

次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る速度推定装置において、第１の実施の形態に係る速度推定装置１０と同一の構成については、同一符号を付して詳細な説明を省略する。

図６に示すように、第３の実施の形態に係る速度推定装置３１０は、車速センサ１２と、ヨーレイトセンサ１４と、ＧＰＳ装置１６と、コンピュータ３１８とを備えている。このコンピュータ３１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図６に示すように、車速センサ１２で検出された車速データの使用可否を判定する車速使用可否判定部３２と、観測されたＧＰＳ衛星から受信状態の良いＧＰＳ衛星を選択する衛星選択部３６と、時系列車速蓄積部２０と、相対方位算出部２２と、相対方位算出部２２で算出された相対方位の使用可否を判定する相対方位使用可否判定部３４と、時系列相対方位蓄積部２４と、時系列ＧＰＳデータ蓄積部２６と、衛星方向速度算出部２８と、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択部３８と、未知パラメータの時間変化分を拘束し、かつ未知パラメータ数が選択された方程式に基づいて、自車両の３次元速度ベクトルを推定する自車速度推定部３３０と、を含んだ構成で表すことができる。

車速使用可否判定部３２は、車速センサ１２で検出された車速データの信頼性判定を行い、車速データが使用可能か否かを判定する。例えば、車速データを車輪速から検出した場合には、低速時には車輪速分解能力が低下することにより、車速データの精度が劣化するため、一定値以下の車速データは使用不可と判定する。また、カーブ時には、タイヤのスリップによって車速データの誤差が増加するため、ヨーレイトデータが一定値以上のときに検出された車速データは使用不可と判定する。

相対方位使用可否判定部３４は、相対方位算出部２２で算出された相対方位の信頼性判定を行い、相対方位が使用可能か否かを判定する。例えば、カーブ時には、タイヤのスリップによって車両の進行方向と車両の向きとの間にずれが生じるため、ヨーレイトデータが一定値以上の時の相対方位は使用不可と判定する。

衛星選択部３６は、受信したＧＰＳデータの中で、受信状態が良いＧＰＳ衛星からのＧＰＳデータを選択する。都心部では、建物によりＧＰＳ信号の反射（マルチパス）などが起こり、擬似距離やドップラー周波数の精度が劣化する場合があるため、例えば、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差が大きい場合には、そのＧＰＳデータは誤りであると判定して、排除する。

未知パラメータ数選択部３８は、自車速度推定部３３０で用いる方程式に含まれる未知パラメータ数を選択する。通常、未知パラメータ数を増やすことで、より高精度な速度推定を行うことが可能となるが、観測されたＧＰＳ衛星の種類数が未知パラメータ数を下回る場合、解は不安定性を増し、正確な速度推定結果が得られない場合がある。そこで、未知パラメータ数選択部３８は、時系列ＧＰＳデータ蓄積部２６に保存された時系列のＧＰＳデータにおいて、全時刻で観測されたＧＰＳ衛星の種類数に応じて、自車速度推定部３３０で用いる未知パラメータ数を選択する。ＧＰＳ衛星の種類数は、全時刻におけるＧＰＳ衛星の衛星番号から重複を排除してカウントすることにより求めることができる。例えば、観測されたＧＰＳ衛星の種類数を「４」、「５」、「６以上」のように場合分けを行い、各場合の未知パラメータ数を、それぞれ「４」、「５」、「６」のように選択することができる。

自車速度推定部３３０は、第１の実施の形態と同様の拘束条件１〜４に従い、未知パラメータ数選択部３８で選択された未知パラメータ数に応じて拘束条件３を設定した上で方程式を立式し、自車両の３次元速度ベクトルを推定する。例えば、選択されたパラメータ数が「４」の場合には、拘束条件３を下記（７）式に示すように設定する。また、選択されたパラメータ数が「５」の場合には、拘束条件３を下記（８）式に示すように設定する。また、選択されたパラメータ数が「６」の場合には、拘束条件３を下記（９）式に示すように設定する。

（７）式は、拘束条件３において、ピッチ角の時間変化（道路勾配）を０次近似した場合の拘束条件である。この拘束条件を用いて（５）式により方程式を立式した場合、未知パラメータは、θ⁰、φ⁰、Ｃｂｖ⁰、及びＡの４つとなる。（８）式は、拘束条件３において、ピッチ角の時間変化を１次近似した場合の拘束条件である。この拘束条件を用いて（５）式により方程式を立式した場合、未知パラメータは、θ⁰、φ⁰、α、Ｃｂｖ⁰、及びＡの５つとなる。（９）式は、拘束条件３において、ピッチ角の時間変化を２次近似した場合の拘束条件である。この拘束条件を用いて（５）式により方程式を立式した場合は、第１の実施の形態の場合と同様の方程式が立式され、未知パラメータは、θ⁰、φ⁰、α、β、Ｃｂｖ⁰、及びＡの６つとなる。

次に、第３の実施の形態に係る速度推定装置２１０の作用について説明する。

各時刻ｔにおいて、車速センサ１２により車速データＶ^t _wheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータω^tを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ３１８において、図７に示す速度推定処理ルーチンが実行される。なお、第１の実施の形態における速度推定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。

ステップ１００で、各観測値を時系列に取得し、次に、ステップ３００で、上記ステップ１００で取得した各時刻ｔにおける車速データＶ^t _wheelの使用可否を判定する。使用可と判定された車速データＶ^t _wheelは時系列車速蓄積部２０に保存し、使用不可と判定された車速データＶ^t _wheelは排除する。

次に、ステップ３０２で、上記ステップ１００で取得した各時刻ｔにおける各ＧＰＳ衛星からのＧＰＳデータの中で、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差に基づいて、受信状態が良いＧＰＳデータを選択する。受信状態が良いと判定されたＧＰＳデータは時系列ＧＰＳデータ蓄積部２６に保存し、受信状態が悪いと判定されたＧＰＳデータは排除する。

次に、ステップ１０２で、時系列の相対方位角θ^t _gyroを算出する。次に、ステップ３０４で、上記ステップ１０２で算出した各時刻ｔにおける相対方位角θ^t _gyroの使用可否を判定する。使用可と判定された相対方位角θ^t _gyroは時系列相対方位蓄積部２４に蓄積し、使用不可と判定された相対方位角θ^t _gyroは排除する。

次に、ステップ１０４で、衛星方向速度ｓ^t _i、並びに（４）式のＧｘ^t _i、Ｇｙ^t _i及びＧｚ^t _iを算出する。

次に、ステップ３０６で、時系列データの全時刻における観測されたＧＰＳ衛星の種類数を求めて、ＧＰＳ衛星の種類数に応じて、後段のステップ３０８で立式する方程式に含まれる未知パラメータ数を選択する。

次に、ステップ３０８で、上記ステップ３０６で選択された未知パラメータ数に応じた方程式に、上記ステップ３００で保存された時系列の車速データＶ^t _wheel、上記ステップ１０２で算出された時系列の相対方位θ^t _gyro、及び上記ステップ１０４で算出したＶｓ^t _i、Ｇｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。この際、上記ステップ３００または３０２で、使用不可と判定された車速データＶ^t _wheelまたはヨーレイトデータω^tが存在する時刻ｔについては、方程式は立式されない。

次に、ステップ１０８で、上記ステップ３０８で立式した方程式を解いて、自車両の３次元速度ベクトルを算出し、速度推定処理ルーチンを終了する。

以上説明したように、第３の実施の形態に係る速度推定装置によれば、車速データや相対方位の使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より３次元速度ベクトルの推定精度が向上する。

なお、第３の実施の形態では、第１の実施の形態に係る速度推定装置に、車速使用可否判定部、相対方位使用可否判定部、衛星選択部、及び未知パラメータ数選択部を加えた構成について説明したが、第２の実施の形態に係る速度推定装置に、車速使用可否判定部、相対方位使用可否判定部、衛星選択部、及び未知パラメータ数選択部を加えた構成としてもよい。また、第２の実施の形態に、気圧センサで検出された高度情報の信頼性判定を行い、高度情報が使用可能か否かを判定する高度情報使用可否判定部を設けてもよい。高度情報使用可否判定部は、例えば、前回検出された高度情報と今回検出された高度情報との差が車輪速から得られる自車の移動距離以上の場合には、その高度情報は使用不可と判定することができる。

また、上記の各実施の形態では、車両に搭載される速度推定装置について説明したが、本発明の速度推定装置が搭載される移動体は車両に限定されない。例えば、速度推定装置をロボットに搭載してもよいし、歩行者が携帯できるように速度推定装置をポータブル端末として構成するようにしてもよい。

１０、２１０、３１０ 速度推定装置
１２ 車速センサ
１４ ヨーレイトセンサ
１５ 気圧センサ
１６ ＧＰＳ装置
１８、２１８、３１８ コンピュータ
２０ 時系列車速蓄積部
２２ 相対方位算出部
２４ 時系列相対方位蓄積部
２６ 時系列ＧＰＳデータ蓄積部
２８ 衛星方向速度算出部
３０、２３０、３３０ 自車速度推定部
３１ 時系列高度情報蓄積部
３２ 車速使用可否判定部
３４ 相対方位使用可否判定部
３６ 衛星選択部
３８ 未知パラメータ数選択部

Claims (11)

1. ＧＰＳ衛星から送信されたＧＰＳデータを時系列に取得する取得手段と、
   前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、該方程式に含まれる時刻毎に変化する未知パラメータが、前記３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定する推定手段と、
   を含む速度推定装置。
2. 前記推定手段は、前記高さ方向の速度成分に関連する時間変化する情報を、前記移動体のピッチ角の時間変化とし、該移動体のピッチ角の時間変化を直線近似または曲線近似することにより拘束する請求項１記載の速度推定装置。
3. 前記推定手段は、前記曲線近似を２次曲線近似とした請求項２記載の速度推定装置。
4. 前記取得手段は、前記移動体の高度を検出する高度センサで検出された高度情報を時系列に取得し、
   前記推定手段は、前記高さ方向の速度成分に関連する時間変化する情報を、前記取得手段により取得された高度情報の時間変化とした
   請求項１記載の速度推定装置。
5. 前記取得手段により取得された高度情報の精度に基づいて、前記推定手段における該高度情報の使用可否を判定する高度情報使用可否判定手段を含む請求項４記載の速度推定装置。
6. 前記取得手段は、前記移動体のヨーレイト及び前記移動体の速度の大きさの少なくとも一方を時系列に取得し、
   前記推定手段は、前記方程式に含まれる時刻毎に変化する未知パラメータが、前記高さ方向の速度成分に関連する時間変化する情報と共に、前記取得手段により取得された時系列のヨーレイトに基づいて算出された前記移動体の進行方向の方位角の時間変化、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化の少なくとも１つにより拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定する
   請求項１〜請求項５のいずれか１項記載の速度推定装置。
7. 前記方位角の時間変化の算出精度に基づいて、前記推定手段における該方位角の時間変化の使用可否を判定する方位角使用可否判定手段を含む請求項６記載の速度推定装置。
8. 前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含む請求項６または請求項７記載の速度推定装置。
9. 前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含む請求項１〜請求項８のいずれか１項記載の速度推定装置。
10. 前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含む請求項１〜請求項９のいずれか１項記載の速度推定装置。
11. コンピュータを、
    ＧＰＳ衛星から送信されたＧＰＳデータを時系列に取得する取得手段、及び
    前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、該方程式に含まれる時刻毎に変化する未知パラメータが、前記３次元速度ベクトルの高さ方向の速度成分に関連する時間変化する情報により拘束された方程式に基づいて、前記移動体の３次元速度ベクトルを推定する推定手段
    として機能させるための速度推定プログラム。

Images