速度推定装置及びプログラム
【課題】観測衛星数が少ない環境でも、安定して精度良く移動体の速度ベクトルを推定する。
【解決手段】車速データＶtwheel、ヨーレイトデータωt、及びＧＰＳデータを時系列に取得し、各々を時系列車速蓄積部２０、時系列ヨーレイト蓄積部２２、及び時系列ＧＰＳデータ蓄積部２４に保存する。方位角時間変化算出部２６で、時系列のヨーレイトデータωtに基づいて、方位角の時間変化θtgyroを算出し、衛星方向速度算出部２８で、衛星方向速度Ｖｓtiを算出する。自車速度推定部３０で、ＧＰＳデータに基づいて速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータの時間変化分を、方位角の時間変化及びクロックドリフトの時間変化で拘束した方程式に、観測値を代入して、時刻数×各時刻の観測衛星数分の方程式を立式し、得られたパラメータを用いて、自車両の速度ベクトルを推定する。
【解決手段】車速データＶtwheel、ヨーレイトデータωt、及びＧＰＳデータを時系列に取得し、各々を時系列車速蓄積部２０、時系列ヨーレイト蓄積部２２、及び時系列ＧＰＳデータ蓄積部２４に保存する。方位角時間変化算出部２６で、時系列のヨーレイトデータωtに基づいて、方位角の時間変化θtgyroを算出し、衛星方向速度算出部２８で、衛星方向速度Ｖｓtiを算出する。自車速度推定部３０で、ＧＰＳデータに基づいて速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータの時間変化分を、方位角の時間変化及びクロックドリフトの時間変化で拘束した方程式に、観測値を代入して、時刻数×各時刻の観測衛星数分の方程式を立式し、得られたパラメータを用いて、自車両の速度ベクトルを推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、速度推定装置及びプログラムに係り、特に、ＧＰＳ（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.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の非特許文献１に記載の技術では、１時刻分の観測データのみを用いて、「車輪速による車速拘束」という拘束条件のみを利用しているため、利用できるＧＰＳ衛星数に制限があり、かつ速度推定を行う際の拘束条件が少ない。このため、観測されたＧＰＳ衛星数（以下、観測衛星数という）が少ない環境（３未満）では、速度推定を行うことができない、という問題がある。
【０００７】
また、特許文献１記載の技術では、観測衛星数が４未満の場合では、ＧＰＳデータを棄却し、自律航法（ＩＮＳのみによる軌跡推定）を行うが、自律航法では、進行方位誤差及び進行距離誤差が累積するため、観測衛星数が４未満の場所が長く続いた場合（自律航法が長く続いた場合）には、自車位置が大きくずれてしまう可能性がある、という問題がある。
【０００８】
本発明は、上記の問題点を解決するためになされたもので、観測衛星数が少ない環境でも、安定して精度良く移動体の速度ベクトルを推定することができる速度推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記目的を達成するために、第１の発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段と、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、を含んで構成されている。
【００１０】
第１の発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得し、算出手段が、取得手段により取得された時系列のヨーレイトに基づいて、移動体の進行方向の方位角の時間変化を算出する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、移動体の速度ベクトルを推定する。
【００１１】
また、上記目的を達成するために、第２の発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、を含んで構成されている。
【００１２】
第２の発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、移動体の速度ベクトルを推定する。
【００１３】
このように、時系列に取得された複数のＧＰＳデータに基づいて移動体の速度ベクトルを推定する場合に、方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減することができる。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く移動体の速度ベクトルを推定することができる。
【００１４】
また、第２の発明の速度推定装置は、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段を含んで構成することができ、前記推定手段は、前記未知パラメータが、さらに前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定することができる。このように、拘束条件が増えるほど、未知パラメータ数を削減することができる。
【００１５】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得されたヨーレイトの精度に基づいて、前記算出手段及び前記推定手段における該ヨーレイトの使用可否を判定するヨーレイト使用可否判定手段を含んで構成することができる。
【００１６】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含んで構成することができる。
【００１７】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含んで構成することができる。
【００１８】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含んで構成することができる。
【００１９】
このように、速度の大きさやヨーレイトの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、ＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より精度良く移動体の速度ベクトルを推定することができる。
【００２０】
また、第３の発明の速度推定プログラムは、コンピュータを、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段、及び前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段として機能させるためのプログラムである。
【００２１】
また、第４の発明の速度推定プログラムは、コンピュータを、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段、及び前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段として機能させるためのプログラムである。
【００２２】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２３】
以上説明したように、本発明の速度推定装置及びプログラムによれば、時系列に取得された複数のＧＰＳデータに基づいて移動体の速度ベクトルを推定する場合に、方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減することができる。これにより、各時刻における観測衛星数が少ない環境でも未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く移動体の速度ベクトルを推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２４】
【図１】第１の実施の形態に係る速度推定装置を示すブロック図である。
【図２】ＧＰＳデータに基づく速度ベクトルの推定を説明するための図である。
【図３】第１の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。
【図４】第２の実施の形態に係る速度推定装置を示すブロック図である。
【図５】第２の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２５】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、自車両の速度ベクトルを推定する速度推定装置に、本発明を適用した場合を例に説明する。
【００２６】
図１に示すように、第１の実施の形態に係る速度推定装置１０は、自車両の速度の大きさを検出する車速センサ１２と、自車両のヨーレイトを検出するヨーレイトセンサ１４と、ＧＰＳ衛星から送信されるＧＰＳデータを受信するＧＰＳ装置１６と、自車両の速度ベクトルを推定するコンピュータ１８とを備えている。
【００２７】
車速センサ１２は、車輪速から出力された車速データを検出する。なお、車速センサ１２としては、レーザレーダ、ミリ波レーダ、ステレオカメラ等、ＧＰＳ装置１６以外であれば、車輪速を検出するセンサ以外を用いてもよい。
【００２８】
ヨーレイトセンサ１４は、ジャイロセンサなどを用いることができ、自車両のヨーレイトデータを検出する。なお、ヨーレイトセンサ１４としては、レーザレーダ、移動ステレオなど、ＧＰＳ装置１６以外であれば、ジャイロセンサ以外のセンサを用いてもよい。
【００２９】
ＧＰＳ装置１６は、測位及び速度推定に必要となる擬似距離、ドップラー周波数、衛星番号、衛星位置、衛星速度等を含むＧＰＳデータを、ＧＰＳ衛星から受信する。
【００３０】
コンピュータ１８は、ＣＰＵ、後述する速度推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、時系列に取得された車速データを蓄積する時系列車速蓄積部２０と、時系列に取得されたヨーレイトデータを蓄積する時系列ヨーレイト蓄積部２２と、時系列に取得されたＧＰＳデータを蓄積する時系列ＧＰＳデータ蓄積部２４と、時系列のヨーレイトデータから方位角の時間変化を算出する方位角時間変化算出部２６と、ＧＰＳデータを用いて自車両からＧＰＳ衛星方向への速度ベクトルを算出する衛星方向速度算出部２８と、各観測値及び未知パラメータの時間変化分を拘束した方程式に基づいて、自車両の速度ベクトルを推定する自車速度推定部３０と、を含んだ構成で表すことができる。
【００３１】
時系列車速蓄積部２０には、車速センサ１２で検出された車速データが時系列に保存される。データ保存のトリガは、一定時間経過時でもよいし、一定距離走行時でもよい。なお、時刻ｔに車速センサ１２で検出された車速データをＶtwheelとすると、最終的に推定する速度ベクトル（Ｖｘt，Ｖｙt，Ｖｚt）とＶtwheelとの関係は、下記（１）式のように設定することができる。
【００３２】
【数１】
【００３３】
時系列ヨーレイト蓄積部２２には、ヨーレイトセンサ１４で検出されたヨーレイトデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。
【００３４】
時系列ＧＰＳデータ蓄積部２４には、ＧＰＳ装置１６で取得されたＧＰＳデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。
【００３５】
方位角時間変化算出部２６は、自車両の進行方向のある時点における方位角に対する時刻ｔの相対方位角（方位角の時間変化）θtgyroを下記（２）式により算出する。なお、Δｔはタイムステップを示し、ωtは時刻ｔに検出されたヨーレイトデータを示す。
【００３６】
【数２】
【００３７】
衛星方向速度算出部２８は、時系列ＧＰＳデータ蓄積部２４に蓄積されたＧＰＳデータに含まれるドップラー周波数を用いて、自車両からＧＰＳ衛星方向の速度ベクトル（衛星方向速度）を算出する。
【００３８】
ここで、図２に、ドップラー周波数を用いた速度ベクトル算出の概要を示す。ドップラー周波数を用いた速度推定では、観測された各ＧＰＳ衛星のドップラー周波数に基づき、各ＧＰＳ衛星方向への速度成分をそれぞれ算出し、各々を合成することで自車両の速度ベクトルを求める。そのための各ＧＰＳ衛星方向への速度成分は下記（３）式により算出される。
【００３９】
【数３】
【００４０】
ここで、Ｖｓtiは自車両からＧＰＳ衛星ｉ方向への自車速度ベクトルの大きさ、Ｖｘｓti、Ｖｙｓti、ＶｚｓtiはＧＰＳ衛星ｉの各速度成分、Ｄtiはドップラー周波数、ｆ１はＧＰＳデータの搬送波の周波数、Ｃは光速、ｒtiは自車両とＧＰＳ衛星ｉ間の擬似距離、Ｇｘti、Ｇｙti、Ｇｚtiは自車両からＧＰＳ衛星ｉへの視線ベクトル、Ｘtsi、Ｙtsi、Ｚtsiは衛星位置、Ｘtv、Ｙtv、Ｚtvは自車両位置を示す。添え字ｔは時刻ｔのデータであることを表し、添え字ｉはｉ番目のＧＰＳ衛星のデータであることを表す。自車両位置は、擬似距離を用いた一般的な測位方法またはその他の測位方法を用いて算出した値を用いることができる。
【００４１】
（３）式に示す自車両から各ＧＰＳ衛星方向への速度ベクトルの大きさと、最終的に推定する自車両の速度ベクトルとの関係式を下記（５）式に示す。なお、ＣｂｖtはＧＰＳ装置１６の時計誤差の時間変化（クロックドリフト）である。
【００４２】
【数４】
【００４３】
（５）式に示す連立方程式に、ＧＰＳデータから算出したＶｓti、Ｇｘti、Ｇｙti及びＧｚtiを代入して解くことにより、自車両の速度ベクトルを推定することができる。衛星方向速度算出部２８では、このＶｓti、Ｇｘti、Ｇｙti及びＧｚtiを算出する。衛星方向速度Ｖｓtiは、ドップラー周波数から求められる各ＧＰＳ衛星に対する自車両の相対速度、ＧＰＳデータに含まれる衛星位置の時系列データから求められる各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の衛星位置と自車両位置とから求められる衛星方向に基づいて算出することができる。Ｇｘti、Ｇｙti及びＧｚtiは、（４）式により算出することができる。
【００４４】
自車速度推定部３０は、後述する拘束条件で拘束された速度推定のための方程式に基づいて、自車両の速度ベクトルを推定する。速度推定に用いる各観測値の時系列データの範囲は、現時刻からの経過時間や走行距離などに基づき決定する。以下では、速度推定に使用する時系列データの内、最古のデータの時刻をｔ＝０（初期時刻）として扱う。
【００４５】
ここで、本実施の形態の原理について説明する。
【００４６】
（５）式の連立方程式を解くにあたり、解を得るための条件は、通常「未知パラメータ数＝立式数」である、（５）式では、Ｖｘt、Ｖｙt、Ｖｚt、及びＣｂｖtの４つが未知パラメータであるため、時系列データの全時刻における未知パラメータ数は、「未知パラメータ数＝時刻数×４」となる。これに対して、「立式数＝時刻数×各時刻での観測衛星数」であるため、各時刻での観測衛星数が平均４つ以上ない場合には、速度推定結果が得られないことになる。特に、都市部のように建物の遮蔽により観測衛星数が少ない環境では、上記条件を満たすのは困難である。
【００４７】
そこで、本実施の形態では、以下の拘束条件を取り入れることで、未知パラメータ数を削減し、観測衛星数が少ない環境でも速度推定の解が得られる可能性を向上させる。本実施の形態で用いる拘束条件は、下記に示す拘束条件１〜３である。
【００４８】
【数５】
【００４９】
拘束条件１は、推定する速度ベクトルの各成分の大きさを車速データで拘束し、かつ時間変化分を方位角時間変化算出部２６で算出した方位角の時間変化で拘束したものである。拘束条件１内のθ0は初期時刻における自車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３内のＣｂｖ0は初期時刻におけるクロックドリフト、及びＡはクロックドリフトの時間変化の傾きを示す。なお、ここでは、拘束条件１及び２により、二次元平面内の速度成分のみを拘束する場合について説明するが、３軸ジャイロセンサなどにより取得したピッチレイトを用いて、３次元空間における速度成分を拘束してもよい。上記拘束条件１〜３を、（５）式に取り入れた結果、下記（６）式が得られる。
【００５０】
【数６】
【００５１】
（６）式において、未知パラメータは、θ0、Ｃｂｖ0、及びＡであり、これらの未知パラメータを求めることで、自車両の速度ベクトルを推定することができる。（６）式では、方位角の時間変化及びクロックドリフトを用いた拘束条件により、未知パラメータの時間変化分を拘束して、未知パラメータ数を削減している。（６）式では、未知パラメータ数を３つまで削減したため、「立式数＝時刻数×各時刻の観測衛星数」が３以上であれば、速度推定結果を得ることができる。例えば、３時刻（ｔ0，ｔ1，ｔ2）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのＧＰＳデータを用いて、速度推定結果を算出可能である。
【００５２】
従って、自車速度推定部３０は、上記の本実施の形態の原理に従って、時間変化分を拘束した（６）式を用いて、自車両の速度ベクトルを推定する。具体的には、時系列車速蓄積部２０に蓄積された時系列の車速データＶtwheel、方位角時間変化算出部２６で算出された方位角の時間変化θtgyro、並びに衛星方向速度算出部２８で算出された衛星方向速度Ｖｓti及びＧｘti、Ｇｙti、Ｇｚtiを（６）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。これらの方程式を解いて未知パラメータθ0、Ｃｂｖ0、及びＡを求める。そして、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出する。
【００５３】
次に、第１の実施の形態に係る速度推定装置１０の作用について説明する。
【００５４】
各時刻ｔにおいて、車速センサ１２により車速データＶtwheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータωtを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ１８において、図３に示す速度推定処理ルーチンが実行される。
【００５５】
ステップ１００で、観測値として、車速センサ１２により検出された車速データＶtwheel、ヨーレイトセンサ１４により検出されたヨーレイトデータωt、及びＧＰＳ装置１６により受信したＧＰＳデータを時系列に取得する。そして、各々を、時系列車速蓄積部２０、時系列ヨーレイト蓄積部２２、及び時系列ＧＰＳデータ蓄積部２４に保存する。
【００５６】
次に、ステップ１０２で、上記ステップ１００で保存された時系列のヨーレイトデータωtに基づいて、（２）式により、方位角の時間変化θtgyroを算出する。
【００５７】
次に、ステップ１０４で、上記ステップ１００で保存された時系列のＧＰＳデータに基づいて、衛星方向速度Ｖｓtiを算出する。また、（４）式により、Ｇｘti、Ｇｙti及びＧｚtiを算出する。
【００５８】
次に、ステップ１０６で、時間変化分を拘束した（６）式に、上記ステップ１００で取得した車速データＶtwheel、上記ステップ１０２で算出した方位角の時間変化θtgyro、及び上記ステップ１０４で算出したＶｓti、Ｇｘti、Ｇｙti、Ｇｚtiを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。
【００５９】
次に、ステップ１０８で、上記ステップ１０６で立式した方程式を解いて、未知パラメータθ0、Ｃｂｖ0、及びＡを求める。そして、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。
【００６０】
以上説明したように、第１の実施の形態の速度推定装置によれば、複数時刻におけるＧＰＳデータを用いて自車両の速度ベクトルを推定する場合に、速度推定のための方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減する。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く自車両の速度ベクトルを推定することができる。
【００６１】
なお、第１の実施の形態では、車速データ及び方位角の時間変化による拘束、並びにクロックバイアスの時間変化による拘束の全てを用いる場合について説明したが、これに限定されない。車速データ、方位角の時間変化、クロックバイアスの時間変化各々のみによる拘束条件としてもよいし、また、各々を適宜組み合わせた拘束条件としてもよい。ただし、時間変化分を拘束することにより未知パラメータ数を削減するという観点から、方位角の時間変化及びクロックバイアスの時間変化の少なくとも一方を用いた拘束条件であることが望ましい。
【００６２】
拘束条件として、方位角の時間変化のみを用いた場合には、（６）式において、Ｖtwheel、θ0、及びＣｂｖt（Ｃｂｖt＝Ｃｂｖ0＋Ａｔの拘束がないため、（６）式の「Ｃｂｖ0−Ａｔ」を「Ｃｂｖt」に置き換え）が未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋１」となる。全ての拘束条件を適用した場合に比べると未知パラメータ数は多くなるが、拘束条件がない場合（未知パラメータ数＝時刻数×４）に比べると、未知パラメータ数を削減することができる。
【００６３】
また、拘束条件として、クロックドリフトの時間変化のみを用いた場合には、（６）式において、Ｖtwheel、θ0、θtgyro、Ｃｂｖ0、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋３」となり、上記と同様に、未知パラメータ数を削減することができる。
【００６４】
また、拘束条件として、クロックドリフトの時間変化及び車速データを用いた場合には、（６）式において、θ0、θtgyro、Ｃｂｖ0、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋３」となり、上記と同様に、未知パラメータ数を削減することができる。
【００６５】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の速度推定装置において、第１の実施の形態の速度推定装置１０と同一の構成については、同一符号を付して詳細な説明を省略する。
【００６６】
図４に示すように、第２の実施の形態の速度推定装置２１０は、車速センサ１２と、ヨーレイトセンサ１４と、ＧＰＳ装置１６と、コンピュータ２１８とを備えている。このコンピュータ２１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図４に示すように、車速センサ１２で検出された車速データの使用可否を判定する車速使用可否判定部３２と、ヨーレイトセンサ１４で検出されたヨーレイトデータの使用可否を判定するヨーレイト使用可否判定部３４と、観測されたＧＰＳ衛星から受信状態の良いrＧＰＳ衛星を選択する衛星選択部３６と、時系列車速蓄積部２０と、時系列ヨーレイト蓄積部２２と、時系列ＧＰＳデータ蓄積部２４と、方位角時間変化算出部２６と、衛星方向速度算出部２８と、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択部３８と、各観測値及び未知パラメータの時間変化分を拘束した選択された未知パラメータ数の方程式に基づいて、自車両の速度ベクトルを推定する自車速度推定部２３０と、を含んだ構成で表すことができる。
【００６７】
車速使用可否判定部３２は、車速センサ１２で検出された車速データの信頼性判定を行い、車速データが使用可能か否かを判定する。例えば、車速データを車輪速から検出した場合には、低速時には車輪速分解能力が低下することにより、車速データの精度が劣化するため、一定値以下の車速データは使用不可と判定する。また、カーブ時には、タイヤのスリップによって車速データの誤差が増加するため、ヨーレイトデータが一定値以上のときに検出された車速データは使用不可と判定する。
【００６８】
ヨーレイト使用可否判定部３４は、ヨーレイトセンサ１４で検出されたヨーレイトデータの信頼性判定を行い、ヨーレイトデータが使用可能か否かを判定する。例えば、カーブ時には、タイヤのスリップによって車両の進行方向と車両の向きとの間にずれが生じるため、一定値以上のヨーレイトデータは使用不可と判定する。
【００６９】
衛星選択部３６は、受信したＧＰＳデータの中で、受信状態が良いＧＰＳ衛星からのＧＰＳデータを選択する。都心部では、建物によりＧＰＳ信号の反射（マルチパス）などが起こり、擬似距離やドップラー周波数の精度が劣化する場合があるため、例えば、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差が大きい場合には、そのＧＰＳデータは誤りであると判定して、排除する。
【００７０】
未知パラメータ数選択部３８は、自車速度推定部２３０で用いる方程式に含まれる未知パラメータ数を選択する。通常、未知パラメータ数を増やすことで、より高精度な速度推定を行うことが可能となるが、観測されたＧＰＳ衛星の種類数が未知パラメータ数を下回る場合、解は不安定性を増し、正確な速度推定結果が得られない場合がある。そこで、未知パラメータ数選択部３８は、時系列ＧＰＳデータ蓄積部２４に保存された時系列のＧＰＳデータにおいて、全時刻で観測されたＧＰＳ衛星の種類数に応じて、自車速度推定部２３０で速度推定を行う未知パラメータ数を選択する。ＧＰＳ衛星の種類数は、全時刻におけるＧＰＳ衛星の衛星番号から重複を排除してカウントすることにより求めることができる。例えば、観測されたＧＰＳ衛星の種類数を「３」、「４」、「５以上」のように場合わけを行い、各場合の未知パラメータ数を、それぞれ「３」、「４」、「５」のように選択することができる。
【００７１】
自車速度推定部２３０は、未知パラメータ数選択部３８で選択された未知パラメータ数に応じた方程式を立式した上で、自車両の速度ベクトルを推定する。例えば、選択されたパラメータ数が「３」の場合には、第１の実施の形態と同様に、（６）式により方程式を立式する。この場合、未知パラメータは、θ0、Ｃｂｖ0、及びＡの３つである。選択された未知パラメータ数が「４」の場合には、下記（７）式により方程式を立式する。（７）式では、ヨーレイトセンサ１４が有するオフセット性の誤差ωoffsetが未知パラメータとして追加されている。すなわち、未知パラメータは、θ0、Ｃｂｖ0、Ａ、及びωoffsetの４つである。選択された未知パラメータ数が「５」の場合には、下記（８）式により方程式を立式する。（８）式では、さらに、車速センサ１２が有するスケール誤差Ｓが未知パラメータとして追加されている。すなわち、未知パラメータは、θ0、Ｃｂｖ0、Ａ、ωoffset、及びＳの５つである。
【００７２】
【数７】
【００７３】
次に、第２の実施の形態に係る速度推定装置２１０の作用について説明する。
【００７４】
各時刻ｔにおいて、車速センサ１２により車速データＶtwheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータωtを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ２１８において、図５に示す速度推定処理ルーチンが実行される。なお、第１の実施の形態の速度推定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。
【００７５】
ステップ１００で、各観測値を時系列に取得し、次に、ステップ２００で、上記ステップ１００で取得した各時刻ｔにおける車速データＶtwheelの使用可否を判定する。使用可と判定された車速データＶtwheelは時系列車速蓄積部２０に保存し、使用不可と判定された車速データＶtwheelは排除する。
【００７６】
次に、ステップ２０２で、上記ステップ１００で取得した各時刻ｔにおけるヨーレイトデータωtの使用可否を判定する。使用可と判定されたヨーレイトデータωtは時系列ヨーレイト蓄積部２２に保存し、使用不可と判定されたヨーレイトデータωtは排除する。
【００７７】
次に、ステップ２０４で、上記ステップ１００で取得した各時刻ｔにおける各ＧＰＳ衛星からのＧＰＳデータの中で、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差に基づいて、受信状態が良いＧＰＳデータを選択する。受信状態が良いと判定されたＧＰＳデータは時系列ＧＰＳデータ蓄積部２４に保存し、受信状態が悪いと判定されたＧＰＳデータは排除する。
【００７８】
次に、ステップ１０２で、方位角の時間変化θtgyroを算出し、次に、ステップ１０４で、衛星方向速度ｓti、並びに（４）式のＧｘti、Ｇｙti及びＧｚtiを算出する。
【００７９】
次に、ステップ２０６で、時系列データの全時刻における観測されたＧＰＳ衛星の種類数を求めて、ＧＰＳ衛星の種類数に応じて、後段のステップ２０８で立式する方程式に含まれる未知パラメータ数を選択する。
【００８０】
次に、ステップ２０８で、上記ステップ２０６で選択された未知パラメータ数に応じた方程式に、上記ステップ２００で保存された車速データＶtwheel、上記ステップ１０２で算出した方位角の時間変化θtgyro、及び上記ステップ１０４で算出したＶｓti、Ｇｘti、Ｇｙti、Ｇｚtiを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。この際、上記ステップ２００または２０２で、使用不可と判定された車速データＶtwheelまたはヨーレイトデータωtが存在する時刻ｔについては、方程式は立式されない。
【００８１】
次に、ステップ１０８で、上記ステップ２０８で立式した方程式を解いて、選択された未知パラメータ数分の未知パラメータを求めて、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。
【００８２】
以上説明したように、第２の実施の形態の速度推定装置によれば、車速データ、ヨーレイトデータの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より速度ベクトルの推定精度が向上する。
【００８３】
なお、上記の実施の形態では、車両に搭載される速度推定装置について説明したが、本発明の速度推定装置が搭載される移動体は車両に限定されない。例えば、速度推定装置をロボットに搭載してもよいし、歩行者が携帯できるように速度推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【００８４】
１０、２１０ 速度推定装置
１２ 車速センサ
１４ ヨーレイトセンサ
１６ ＧＰＳ装置
１８、２１８ コンピュータ
２０ 時系列車速蓄積部
２２ 時系列ヨーレイト蓄積部
２４ 時系列ＧＰＳデータ蓄積部
２６ 方位角時間変化算出部
２８ 衛星方向速度算出部
３０、２３０ 自車速度推定部
３２ 車速使用可否判定部
３４ ヨーレイト使用可否判定部
３６ 衛星選択部
３８ 未知パラメータ数選択部
【技術分野】
【０００１】
本発明は、速度推定装置及びプログラムに係り、特に、ＧＰＳ（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.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記の非特許文献１に記載の技術では、１時刻分の観測データのみを用いて、「車輪速による車速拘束」という拘束条件のみを利用しているため、利用できるＧＰＳ衛星数に制限があり、かつ速度推定を行う際の拘束条件が少ない。このため、観測されたＧＰＳ衛星数（以下、観測衛星数という）が少ない環境（３未満）では、速度推定を行うことができない、という問題がある。
【０００７】
また、特許文献１記載の技術では、観測衛星数が４未満の場合では、ＧＰＳデータを棄却し、自律航法（ＩＮＳのみによる軌跡推定）を行うが、自律航法では、進行方位誤差及び進行距離誤差が累積するため、観測衛星数が４未満の場所が長く続いた場合（自律航法が長く続いた場合）には、自車位置が大きくずれてしまう可能性がある、という問題がある。
【０００８】
本発明は、上記の問題点を解決するためになされたもので、観測衛星数が少ない環境でも、安定して精度良く移動体の速度ベクトルを推定することができる速度推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記目的を達成するために、第１の発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段と、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、を含んで構成されている。
【００１０】
第１の発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得し、算出手段が、取得手段により取得された時系列のヨーレイトに基づいて、移動体の進行方向の方位角の時間変化を算出する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、移動体の速度ベクトルを推定する。
【００１１】
また、上記目的を達成するために、第２の発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、を含んで構成されている。
【００１２】
第２の発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、移動体の速度ベクトルを推定する。
【００１３】
このように、時系列に取得された複数のＧＰＳデータに基づいて移動体の速度ベクトルを推定する場合に、方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減することができる。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く移動体の速度ベクトルを推定することができる。
【００１４】
また、第２の発明の速度推定装置は、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段を含んで構成することができ、前記推定手段は、前記未知パラメータが、さらに前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定することができる。このように、拘束条件が増えるほど、未知パラメータ数を削減することができる。
【００１５】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得されたヨーレイトの精度に基づいて、前記算出手段及び前記推定手段における該ヨーレイトの使用可否を判定するヨーレイト使用可否判定手段を含んで構成することができる。
【００１６】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含んで構成することができる。
【００１７】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含んで構成することができる。
【００１８】
また、第１または第２の発明の速度推定装置は、前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含んで構成することができる。
【００１９】
このように、速度の大きさやヨーレイトの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、ＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より精度良く移動体の速度ベクトルを推定することができる。
【００２０】
また、第３の発明の速度推定プログラムは、コンピュータを、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段、及び前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段として機能させるためのプログラムである。
【００２１】
また、第４の発明の速度推定プログラムは、コンピュータを、ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段、及び前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段として機能させるためのプログラムである。
【００２２】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２３】
以上説明したように、本発明の速度推定装置及びプログラムによれば、時系列に取得された複数のＧＰＳデータに基づいて移動体の速度ベクトルを推定する場合に、方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減することができる。これにより、各時刻における観測衛星数が少ない環境でも未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く移動体の速度ベクトルを推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２４】
【図１】第１の実施の形態に係る速度推定装置を示すブロック図である。
【図２】ＧＰＳデータに基づく速度ベクトルの推定を説明するための図である。
【図３】第１の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。
【図４】第２の実施の形態に係る速度推定装置を示すブロック図である。
【図５】第２の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２５】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、自車両の速度ベクトルを推定する速度推定装置に、本発明を適用した場合を例に説明する。
【００２６】
図１に示すように、第１の実施の形態に係る速度推定装置１０は、自車両の速度の大きさを検出する車速センサ１２と、自車両のヨーレイトを検出するヨーレイトセンサ１４と、ＧＰＳ衛星から送信されるＧＰＳデータを受信するＧＰＳ装置１６と、自車両の速度ベクトルを推定するコンピュータ１８とを備えている。
【００２７】
車速センサ１２は、車輪速から出力された車速データを検出する。なお、車速センサ１２としては、レーザレーダ、ミリ波レーダ、ステレオカメラ等、ＧＰＳ装置１６以外であれば、車輪速を検出するセンサ以外を用いてもよい。
【００２８】
ヨーレイトセンサ１４は、ジャイロセンサなどを用いることができ、自車両のヨーレイトデータを検出する。なお、ヨーレイトセンサ１４としては、レーザレーダ、移動ステレオなど、ＧＰＳ装置１６以外であれば、ジャイロセンサ以外のセンサを用いてもよい。
【００２９】
ＧＰＳ装置１６は、測位及び速度推定に必要となる擬似距離、ドップラー周波数、衛星番号、衛星位置、衛星速度等を含むＧＰＳデータを、ＧＰＳ衛星から受信する。
【００３０】
コンピュータ１８は、ＣＰＵ、後述する速度推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、時系列に取得された車速データを蓄積する時系列車速蓄積部２０と、時系列に取得されたヨーレイトデータを蓄積する時系列ヨーレイト蓄積部２２と、時系列に取得されたＧＰＳデータを蓄積する時系列ＧＰＳデータ蓄積部２４と、時系列のヨーレイトデータから方位角の時間変化を算出する方位角時間変化算出部２６と、ＧＰＳデータを用いて自車両からＧＰＳ衛星方向への速度ベクトルを算出する衛星方向速度算出部２８と、各観測値及び未知パラメータの時間変化分を拘束した方程式に基づいて、自車両の速度ベクトルを推定する自車速度推定部３０と、を含んだ構成で表すことができる。
【００３１】
時系列車速蓄積部２０には、車速センサ１２で検出された車速データが時系列に保存される。データ保存のトリガは、一定時間経過時でもよいし、一定距離走行時でもよい。なお、時刻ｔに車速センサ１２で検出された車速データをＶtwheelとすると、最終的に推定する速度ベクトル（Ｖｘt，Ｖｙt，Ｖｚt）とＶtwheelとの関係は、下記（１）式のように設定することができる。
【００３２】
【数１】
【００３３】
時系列ヨーレイト蓄積部２２には、ヨーレイトセンサ１４で検出されたヨーレイトデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。
【００３４】
時系列ＧＰＳデータ蓄積部２４には、ＧＰＳ装置１６で取得されたＧＰＳデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。
【００３５】
方位角時間変化算出部２６は、自車両の進行方向のある時点における方位角に対する時刻ｔの相対方位角（方位角の時間変化）θtgyroを下記（２）式により算出する。なお、Δｔはタイムステップを示し、ωtは時刻ｔに検出されたヨーレイトデータを示す。
【００３６】
【数２】
【００３７】
衛星方向速度算出部２８は、時系列ＧＰＳデータ蓄積部２４に蓄積されたＧＰＳデータに含まれるドップラー周波数を用いて、自車両からＧＰＳ衛星方向の速度ベクトル（衛星方向速度）を算出する。
【００３８】
ここで、図２に、ドップラー周波数を用いた速度ベクトル算出の概要を示す。ドップラー周波数を用いた速度推定では、観測された各ＧＰＳ衛星のドップラー周波数に基づき、各ＧＰＳ衛星方向への速度成分をそれぞれ算出し、各々を合成することで自車両の速度ベクトルを求める。そのための各ＧＰＳ衛星方向への速度成分は下記（３）式により算出される。
【００３９】
【数３】
【００４０】
ここで、Ｖｓtiは自車両からＧＰＳ衛星ｉ方向への自車速度ベクトルの大きさ、Ｖｘｓti、Ｖｙｓti、ＶｚｓtiはＧＰＳ衛星ｉの各速度成分、Ｄtiはドップラー周波数、ｆ１はＧＰＳデータの搬送波の周波数、Ｃは光速、ｒtiは自車両とＧＰＳ衛星ｉ間の擬似距離、Ｇｘti、Ｇｙti、Ｇｚtiは自車両からＧＰＳ衛星ｉへの視線ベクトル、Ｘtsi、Ｙtsi、Ｚtsiは衛星位置、Ｘtv、Ｙtv、Ｚtvは自車両位置を示す。添え字ｔは時刻ｔのデータであることを表し、添え字ｉはｉ番目のＧＰＳ衛星のデータであることを表す。自車両位置は、擬似距離を用いた一般的な測位方法またはその他の測位方法を用いて算出した値を用いることができる。
【００４１】
（３）式に示す自車両から各ＧＰＳ衛星方向への速度ベクトルの大きさと、最終的に推定する自車両の速度ベクトルとの関係式を下記（５）式に示す。なお、ＣｂｖtはＧＰＳ装置１６の時計誤差の時間変化（クロックドリフト）である。
【００４２】
【数４】
【００４３】
（５）式に示す連立方程式に、ＧＰＳデータから算出したＶｓti、Ｇｘti、Ｇｙti及びＧｚtiを代入して解くことにより、自車両の速度ベクトルを推定することができる。衛星方向速度算出部２８では、このＶｓti、Ｇｘti、Ｇｙti及びＧｚtiを算出する。衛星方向速度Ｖｓtiは、ドップラー周波数から求められる各ＧＰＳ衛星に対する自車両の相対速度、ＧＰＳデータに含まれる衛星位置の時系列データから求められる各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の衛星位置と自車両位置とから求められる衛星方向に基づいて算出することができる。Ｇｘti、Ｇｙti及びＧｚtiは、（４）式により算出することができる。
【００４４】
自車速度推定部３０は、後述する拘束条件で拘束された速度推定のための方程式に基づいて、自車両の速度ベクトルを推定する。速度推定に用いる各観測値の時系列データの範囲は、現時刻からの経過時間や走行距離などに基づき決定する。以下では、速度推定に使用する時系列データの内、最古のデータの時刻をｔ＝０（初期時刻）として扱う。
【００４５】
ここで、本実施の形態の原理について説明する。
【００４６】
（５）式の連立方程式を解くにあたり、解を得るための条件は、通常「未知パラメータ数＝立式数」である、（５）式では、Ｖｘt、Ｖｙt、Ｖｚt、及びＣｂｖtの４つが未知パラメータであるため、時系列データの全時刻における未知パラメータ数は、「未知パラメータ数＝時刻数×４」となる。これに対して、「立式数＝時刻数×各時刻での観測衛星数」であるため、各時刻での観測衛星数が平均４つ以上ない場合には、速度推定結果が得られないことになる。特に、都市部のように建物の遮蔽により観測衛星数が少ない環境では、上記条件を満たすのは困難である。
【００４７】
そこで、本実施の形態では、以下の拘束条件を取り入れることで、未知パラメータ数を削減し、観測衛星数が少ない環境でも速度推定の解が得られる可能性を向上させる。本実施の形態で用いる拘束条件は、下記に示す拘束条件１〜３である。
【００４８】
【数５】
【００４９】
拘束条件１は、推定する速度ベクトルの各成分の大きさを車速データで拘束し、かつ時間変化分を方位角時間変化算出部２６で算出した方位角の時間変化で拘束したものである。拘束条件１内のθ0は初期時刻における自車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３内のＣｂｖ0は初期時刻におけるクロックドリフト、及びＡはクロックドリフトの時間変化の傾きを示す。なお、ここでは、拘束条件１及び２により、二次元平面内の速度成分のみを拘束する場合について説明するが、３軸ジャイロセンサなどにより取得したピッチレイトを用いて、３次元空間における速度成分を拘束してもよい。上記拘束条件１〜３を、（５）式に取り入れた結果、下記（６）式が得られる。
【００５０】
【数６】
【００５１】
（６）式において、未知パラメータは、θ0、Ｃｂｖ0、及びＡであり、これらの未知パラメータを求めることで、自車両の速度ベクトルを推定することができる。（６）式では、方位角の時間変化及びクロックドリフトを用いた拘束条件により、未知パラメータの時間変化分を拘束して、未知パラメータ数を削減している。（６）式では、未知パラメータ数を３つまで削減したため、「立式数＝時刻数×各時刻の観測衛星数」が３以上であれば、速度推定結果を得ることができる。例えば、３時刻（ｔ0，ｔ1，ｔ2）の観測衛星数がそれぞれ１であったとしても、観測されたＧＰＳ衛星からのＧＰＳデータを用いて、速度推定結果を算出可能である。
【００５２】
従って、自車速度推定部３０は、上記の本実施の形態の原理に従って、時間変化分を拘束した（６）式を用いて、自車両の速度ベクトルを推定する。具体的には、時系列車速蓄積部２０に蓄積された時系列の車速データＶtwheel、方位角時間変化算出部２６で算出された方位角の時間変化θtgyro、並びに衛星方向速度算出部２８で算出された衛星方向速度Ｖｓti及びＧｘti、Ｇｙti、Ｇｚtiを（６）式に代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。これらの方程式を解いて未知パラメータθ0、Ｃｂｖ0、及びＡを求める。そして、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出する。
【００５３】
次に、第１の実施の形態に係る速度推定装置１０の作用について説明する。
【００５４】
各時刻ｔにおいて、車速センサ１２により車速データＶtwheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータωtを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ１８において、図３に示す速度推定処理ルーチンが実行される。
【００５５】
ステップ１００で、観測値として、車速センサ１２により検出された車速データＶtwheel、ヨーレイトセンサ１４により検出されたヨーレイトデータωt、及びＧＰＳ装置１６により受信したＧＰＳデータを時系列に取得する。そして、各々を、時系列車速蓄積部２０、時系列ヨーレイト蓄積部２２、及び時系列ＧＰＳデータ蓄積部２４に保存する。
【００５６】
次に、ステップ１０２で、上記ステップ１００で保存された時系列のヨーレイトデータωtに基づいて、（２）式により、方位角の時間変化θtgyroを算出する。
【００５７】
次に、ステップ１０４で、上記ステップ１００で保存された時系列のＧＰＳデータに基づいて、衛星方向速度Ｖｓtiを算出する。また、（４）式により、Ｇｘti、Ｇｙti及びＧｚtiを算出する。
【００５８】
次に、ステップ１０６で、時間変化分を拘束した（６）式に、上記ステップ１００で取得した車速データＶtwheel、上記ステップ１０２で算出した方位角の時間変化θtgyro、及び上記ステップ１０４で算出したＶｓti、Ｇｘti、Ｇｙti、Ｇｚtiを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。
【００５９】
次に、ステップ１０８で、上記ステップ１０６で立式した方程式を解いて、未知パラメータθ0、Ｃｂｖ0、及びＡを求める。そして、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。
【００６０】
以上説明したように、第１の実施の形態の速度推定装置によれば、複数時刻におけるＧＰＳデータを用いて自車両の速度ベクトルを推定する場合に、速度推定のための方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減する。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く自車両の速度ベクトルを推定することができる。
【００６１】
なお、第１の実施の形態では、車速データ及び方位角の時間変化による拘束、並びにクロックバイアスの時間変化による拘束の全てを用いる場合について説明したが、これに限定されない。車速データ、方位角の時間変化、クロックバイアスの時間変化各々のみによる拘束条件としてもよいし、また、各々を適宜組み合わせた拘束条件としてもよい。ただし、時間変化分を拘束することにより未知パラメータ数を削減するという観点から、方位角の時間変化及びクロックバイアスの時間変化の少なくとも一方を用いた拘束条件であることが望ましい。
【００６２】
拘束条件として、方位角の時間変化のみを用いた場合には、（６）式において、Ｖtwheel、θ0、及びＣｂｖt（Ｃｂｖt＝Ｃｂｖ0＋Ａｔの拘束がないため、（６）式の「Ｃｂｖ0−Ａｔ」を「Ｃｂｖt」に置き換え）が未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋１」となる。全ての拘束条件を適用した場合に比べると未知パラメータ数は多くなるが、拘束条件がない場合（未知パラメータ数＝時刻数×４）に比べると、未知パラメータ数を削減することができる。
【００６３】
また、拘束条件として、クロックドリフトの時間変化のみを用いた場合には、（６）式において、Ｖtwheel、θ0、θtgyro、Ｃｂｖ0、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋３」となり、上記と同様に、未知パラメータ数を削減することができる。
【００６４】
また、拘束条件として、クロックドリフトの時間変化及び車速データを用いた場合には、（６）式において、θ0、θtgyro、Ｃｂｖ0、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋３」となり、上記と同様に、未知パラメータ数を削減することができる。
【００６５】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の速度推定装置において、第１の実施の形態の速度推定装置１０と同一の構成については、同一符号を付して詳細な説明を省略する。
【００６６】
図４に示すように、第２の実施の形態の速度推定装置２１０は、車速センサ１２と、ヨーレイトセンサ１４と、ＧＰＳ装置１６と、コンピュータ２１８とを備えている。このコンピュータ２１８を以下で説明する速度推定処理ルーチンに従って機能ブロックで表すと、図４に示すように、車速センサ１２で検出された車速データの使用可否を判定する車速使用可否判定部３２と、ヨーレイトセンサ１４で検出されたヨーレイトデータの使用可否を判定するヨーレイト使用可否判定部３４と、観測されたＧＰＳ衛星から受信状態の良いrＧＰＳ衛星を選択する衛星選択部３６と、時系列車速蓄積部２０と、時系列ヨーレイト蓄積部２２と、時系列ＧＰＳデータ蓄積部２４と、方位角時間変化算出部２６と、衛星方向速度算出部２８と、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択部３８と、各観測値及び未知パラメータの時間変化分を拘束した選択された未知パラメータ数の方程式に基づいて、自車両の速度ベクトルを推定する自車速度推定部２３０と、を含んだ構成で表すことができる。
【００６７】
車速使用可否判定部３２は、車速センサ１２で検出された車速データの信頼性判定を行い、車速データが使用可能か否かを判定する。例えば、車速データを車輪速から検出した場合には、低速時には車輪速分解能力が低下することにより、車速データの精度が劣化するため、一定値以下の車速データは使用不可と判定する。また、カーブ時には、タイヤのスリップによって車速データの誤差が増加するため、ヨーレイトデータが一定値以上のときに検出された車速データは使用不可と判定する。
【００６８】
ヨーレイト使用可否判定部３４は、ヨーレイトセンサ１４で検出されたヨーレイトデータの信頼性判定を行い、ヨーレイトデータが使用可能か否かを判定する。例えば、カーブ時には、タイヤのスリップによって車両の進行方向と車両の向きとの間にずれが生じるため、一定値以上のヨーレイトデータは使用不可と判定する。
【００６９】
衛星選択部３６は、受信したＧＰＳデータの中で、受信状態が良いＧＰＳ衛星からのＧＰＳデータを選択する。都心部では、建物によりＧＰＳ信号の反射（マルチパス）などが起こり、擬似距離やドップラー周波数の精度が劣化する場合があるため、例えば、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差が大きい場合には、そのＧＰＳデータは誤りであると判定して、排除する。
【００７０】
未知パラメータ数選択部３８は、自車速度推定部２３０で用いる方程式に含まれる未知パラメータ数を選択する。通常、未知パラメータ数を増やすことで、より高精度な速度推定を行うことが可能となるが、観測されたＧＰＳ衛星の種類数が未知パラメータ数を下回る場合、解は不安定性を増し、正確な速度推定結果が得られない場合がある。そこで、未知パラメータ数選択部３８は、時系列ＧＰＳデータ蓄積部２４に保存された時系列のＧＰＳデータにおいて、全時刻で観測されたＧＰＳ衛星の種類数に応じて、自車速度推定部２３０で速度推定を行う未知パラメータ数を選択する。ＧＰＳ衛星の種類数は、全時刻におけるＧＰＳ衛星の衛星番号から重複を排除してカウントすることにより求めることができる。例えば、観測されたＧＰＳ衛星の種類数を「３」、「４」、「５以上」のように場合わけを行い、各場合の未知パラメータ数を、それぞれ「３」、「４」、「５」のように選択することができる。
【００７１】
自車速度推定部２３０は、未知パラメータ数選択部３８で選択された未知パラメータ数に応じた方程式を立式した上で、自車両の速度ベクトルを推定する。例えば、選択されたパラメータ数が「３」の場合には、第１の実施の形態と同様に、（６）式により方程式を立式する。この場合、未知パラメータは、θ0、Ｃｂｖ0、及びＡの３つである。選択された未知パラメータ数が「４」の場合には、下記（７）式により方程式を立式する。（７）式では、ヨーレイトセンサ１４が有するオフセット性の誤差ωoffsetが未知パラメータとして追加されている。すなわち、未知パラメータは、θ0、Ｃｂｖ0、Ａ、及びωoffsetの４つである。選択された未知パラメータ数が「５」の場合には、下記（８）式により方程式を立式する。（８）式では、さらに、車速センサ１２が有するスケール誤差Ｓが未知パラメータとして追加されている。すなわち、未知パラメータは、θ0、Ｃｂｖ0、Ａ、ωoffset、及びＳの５つである。
【００７２】
【数７】
【００７３】
次に、第２の実施の形態に係る速度推定装置２１０の作用について説明する。
【００７４】
各時刻ｔにおいて、車速センサ１２により車速データＶtwheelを検出し、ヨーレイトセンサ１４によりヨーレイトデータωtを検出し、ＧＰＳ装置１６によりＧＰＳデータを受信しているときに、コンピュータ２１８において、図５に示す速度推定処理ルーチンが実行される。なお、第１の実施の形態の速度推定処理ルーチンと同一の処理については、同一符号を付して詳細な説明を省略する。
【００７５】
ステップ１００で、各観測値を時系列に取得し、次に、ステップ２００で、上記ステップ１００で取得した各時刻ｔにおける車速データＶtwheelの使用可否を判定する。使用可と判定された車速データＶtwheelは時系列車速蓄積部２０に保存し、使用不可と判定された車速データＶtwheelは排除する。
【００７６】
次に、ステップ２０２で、上記ステップ１００で取得した各時刻ｔにおけるヨーレイトデータωtの使用可否を判定する。使用可と判定されたヨーレイトデータωtは時系列ヨーレイト蓄積部２２に保存し、使用不可と判定されたヨーレイトデータωtは排除する。
【００７７】
次に、ステップ２０４で、上記ステップ１００で取得した各時刻ｔにおける各ＧＰＳ衛星からのＧＰＳデータの中で、ＧＰＳデータの受信時のＳＮ比や、擬似距離またはドップラー周波数の残差に基づいて、受信状態が良いＧＰＳデータを選択する。受信状態が良いと判定されたＧＰＳデータは時系列ＧＰＳデータ蓄積部２４に保存し、受信状態が悪いと判定されたＧＰＳデータは排除する。
【００７８】
次に、ステップ１０２で、方位角の時間変化θtgyroを算出し、次に、ステップ１０４で、衛星方向速度ｓti、並びに（４）式のＧｘti、Ｇｙti及びＧｚtiを算出する。
【００７９】
次に、ステップ２０６で、時系列データの全時刻における観測されたＧＰＳ衛星の種類数を求めて、ＧＰＳ衛星の種類数に応じて、後段のステップ２０８で立式する方程式に含まれる未知パラメータ数を選択する。
【００８０】
次に、ステップ２０８で、上記ステップ２０６で選択された未知パラメータ数に応じた方程式に、上記ステップ２００で保存された車速データＶtwheel、上記ステップ１０２で算出した方位角の時間変化θtgyro、及び上記ステップ１０４で算出したＶｓti、Ｇｘti、Ｇｙti、Ｇｚtiを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。この際、上記ステップ２００または２０２で、使用不可と判定された車速データＶtwheelまたはヨーレイトデータωtが存在する時刻ｔについては、方程式は立式されない。
【００８１】
次に、ステップ１０８で、上記ステップ２０８で立式した方程式を解いて、選択された未知パラメータ数分の未知パラメータを求めて、求めたパラメータθ0と、車速データＶtwheel、及び方位角の時間変化θtgyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。
【００８２】
以上説明したように、第２の実施の形態の速度推定装置によれば、車速データ、ヨーレイトデータの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、観測されたＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より速度ベクトルの推定精度が向上する。
【００８３】
なお、上記の実施の形態では、車両に搭載される速度推定装置について説明したが、本発明の速度推定装置が搭載される移動体は車両に限定されない。例えば、速度推定装置をロボットに搭載してもよいし、歩行者が携帯できるように速度推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【００８４】
１０、２１０ 速度推定装置
１２ 車速センサ
１４ ヨーレイトセンサ
１６ ＧＰＳ装置
１８、２１８ コンピュータ
２０ 時系列車速蓄積部
２２ 時系列ヨーレイト蓄積部
２４ 時系列ＧＰＳデータ蓄積部
２６ 方位角時間変化算出部
２８ 衛星方向速度算出部
３０、２３０ 自車速度推定部
３２ 車速使用可否判定部
３４ ヨーレイト使用可否判定部
３６ 衛星選択部
３８ 未知パラメータ数選択部
【特許請求の範囲】
【請求項１】
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段と、
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段と、
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、
を含む速度推定装置。
【請求項２】
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段と、
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、
を含む速度推定装置。
【請求項３】
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段を含み、
前記推定手段は、前記未知パラメータが、さらに前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する
請求項２記載の速度推定装置。
【請求項４】
前記取得手段により取得されたヨーレイトの精度に基づいて、前記算出手段及び前記推定手段における該ヨーレイトの使用可否を判定するヨーレイト使用可否判定手段を含む請求項１または請求項３記載の速度推定装置。
【請求項５】
前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含む請求項２または請求項３記載の速度推定装置。
【請求項６】
前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含む請求項１〜請求項５のいずれか１項記載の速度推定装置。
【請求項７】
前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含む請求項１〜請求項６のいずれか１項記載の速度推定装置。
【請求項８】
コンピュータを、
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段、
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段、及び
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段
として機能させるための速度推定プログラム。
【請求項９】
コンピュータを、
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段、及び
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段
として機能させるための速度推定プログラム。
【請求項１】
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段と、
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段と、
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、
を含む速度推定装置。
【請求項２】
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段と、
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、
を含む速度推定装置。
【請求項３】
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段を含み、
前記推定手段は、前記未知パラメータが、さらに前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する
請求項２記載の速度推定装置。
【請求項４】
前記取得手段により取得されたヨーレイトの精度に基づいて、前記算出手段及び前記推定手段における該ヨーレイトの使用可否を判定するヨーレイト使用可否判定手段を含む請求項１または請求項３記載の速度推定装置。
【請求項５】
前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含む請求項２または請求項３記載の速度推定装置。
【請求項６】
前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含む請求項１〜請求項５のいずれか１項記載の速度推定装置。
【請求項７】
前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含む請求項１〜請求項６のいずれか１項記載の速度推定装置。
【請求項８】
コンピュータを、
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体のヨーレイトを時系列に取得する取得手段、
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段、及び
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段
として機能させるための速度推定プログラム。
【請求項９】
コンピュータを、
ＧＰＳ衛星から送信されたＧＰＳデータ、及び移動体の速度の大きさを時系列に取得する取得手段、及び
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータが、前記取得手段により取得された速度の大きさ、及びクロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段
として機能させるための速度推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１３−１１３７８９（Ｐ２０１３−１１３７８９Ａ）
【公開日】平成２５年６月１０日（２０１３．６．１０）
【国際特許分類】
【出願番号】特願２０１１−２６２３７６（Ｐ２０１１−２６２３７６）
【出願日】平成２３年１１月３０日（２０１１．１１．３０）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２５年６月１０日（２０１３．６．１０）
【国際特許分類】
【出願日】平成２３年１１月３０日（２０１１．１１．３０）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]