特許 5879977 速度推定装置及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5879977

(24)【登録日】2016年2月12日

(45)【発行日】2016年3月8日

(54)【発明の名称】速度推定装置及びプログラム

(51)【国際特許分類】

   G01S 19/52 20100101AFI20160223BHJP

   G01S 19/47 20100101ALI20160223BHJP

【ＦＩ】

   G01S19/52

   G01S19/47

【請求項の数】7

【全頁数】14

(21)【出願番号】特願2011-262376(P2011-262376)

(22)【出願日】2011年11月30日

(65)【公開番号】特開2013-113789(P2013-113789A)

(43)【公開日】2013年6月10日

【審査請求日】2014年8月22日

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】100079049

【弁理士】

【氏名又は名称】中島 淳

(74)【代理人】

【識別番号】100084995

【弁理士】

【氏名又は名称】加藤 和詳

(72)【発明者】

【氏名】武山 洪二郎

(72)【発明者】

【氏名】小島 祥子

【審査官】 中村 説志

(56)【参考文献】

【文献】 特開平０８−１６０１１９（ＪＰ，Ａ）

【文献】 特開２０１０−２５６３０１（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ５／００− ５／１４

Ｇ０１Ｓ１９／００−１９／５５

(57)【特許請求の範囲】

【請求項1】

ＧＰＳ衛星から送信されたＧＰＳデータ、移動体のヨーレイト、及び移動体の速度の大きさを時系列に取得する取得手段と、
前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の所定の時刻における方位角に対する各時刻における相対方位角で表される方位角の時間変化を算出する算出手段と、
前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータである水平面内における速度ベクトルの大きさが、前記取得手段により取得された移動体の速度の大きさで拘束され、かつ該水平面内における速度ベクトルの方向が、前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、
を含む速度推定装置。

【請求項2】

前記推定手段は、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータであるクロックドリフトが、クロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する請求項１記載の速度推定装置。

【請求項3】

前記取得手段により取得されたヨーレイトの精度に基づいて、前記算出手段及び前記推定手段における該ヨーレイトの使用可否を判定するヨーレイト使用可否判定手段を含む請求項１または請求項２記載の速度推定装置。

【請求項4】

前記取得手段により取得された速度の大きさの精度に基づいて、前記推定手段における該速度の大きさの使用可否を判定する速度使用可否判定手段を含む請求項１〜請求項３のいずれか１項記載の速度推定装置。

【請求項5】

前記取得手段により取得されたＧＰＳデータの受信状態、擬似距離の残差、及びドップラー周波数の残差の少なくとも１つに基づいて、前記方程式の立式に用いるＧＰＳデータを選択するＧＰＳ選択手段を含む請求項１〜請求項４のいずれか１項記載の速度推定装置。

【請求項6】

前記取得手段により取得された複数時刻における複数のＧＰＳデータ各々を送信したＧＰＳ衛星の種類数に応じて、前記方程式に含まれる未知パラメータ数を選択する未知パラメータ数選択手段を含む請求項１〜請求項５のいずれか１項記載の速度推定装置。

【請求項7】

コンピュータを、請求項１〜請求項６のいずれか１項記載の速度推定装置を構成する各手段として機能させるための速度推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

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

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００２−３２３５５１号公報

【非特許文献】

【0005】

【非特許文献1】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.

【発明の概要】

【発明が解決しようとする課題】

【0006】

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

【0007】

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

【0008】

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

【課題を解決するための手段】

【0009】

上記目的を達成するために、本発明の速度推定装置は、ＧＰＳ衛星から送信されたＧＰＳデータ、移動体のヨーレイト、及び移動体の速度の大きさを時系列に取得する取得手段と、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の所定の時刻における方位角に対する各時刻における相対方位角で表される方位角の時間変化を算出する算出手段と、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータである水平面内における速度ベクトルの大きさが、前記取得手段により取得された移動体の速度の大きさで拘束され、かつ該水平面内における速度ベクトルの方向が、前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定する推定手段と、を含んで構成されている。

【0010】

本発明の速度推定装置によれば、取得手段が、ＧＰＳ衛星から送信されたＧＰＳデータ、移動体のヨーレイト、及び移動体の速度の大きさを時系列に取得し、算出手段が、取得手段により取得された時系列のヨーレイトに基づいて、移動体の進行方向の所定の時刻における方位角に対する各時刻における相対方位角で表される方位角の時間変化を算出する。そして、推定手段が、取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻における移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータである水平面内における速度ベクトルの大きさが、取得手段により取得された移動体の速度の大きさで拘束され、かつ水平面内における速度ベクトルの方向が、算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、移動体の速度ベクトルを推定する。

【0011】

また、上記目的を達成するために、本発明の速度推定装置における前記推定手段は、前記取得手段により時系列に取得された複数のＧＰＳデータに基づいて立式される各時刻での前記移動体の速度ベクトルを推定するための方程式であって、時刻毎に変化する未知パラメータであるクロックドリフトが、クロックドリフトの時間変化の線形化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定することができる。

【0013】

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

【0014】

また、本発明の速度推定装置は、前記取得手段により取得された時系列のヨーレイトに基づいて、前記移動体の進行方向の方位角の時間変化を算出する算出手段を含んで構成することができ、前記推定手段は、前記未知パラメータが、さらに前記算出手段により算出された方位角の時間変化により拘束された方程式に基づいて、前記移動体の速度ベクトルを推定することができる。このように、拘束条件が増えるほど、未知パラメータ数を削減することができる。

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

このように、速度の大きさやヨーレイトの使用可否を判定したり、受信状態の良いＧＰＳデータを選択したり、ＧＰＳ衛星の種類数に応じて方程式に含まれる未知パラメータ数を選択したりすることで、より精度良く移動体の速度ベクトルを推定することができる。

【0020】

また、第２の発明の速度推定プログラムは、コンピュータを、上記の速度推定装置を構成する各手段として機能させるためのプログラムである。

【0022】

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

【発明の効果】

【0023】

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

【図面の簡単な説明】

【0024】

【図1】第１の実施の形態に係る速度推定装置を示すブロック図である。

【図2】ＧＰＳデータに基づく速度ベクトルの推定を説明するための図である。

【図3】第１の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。

【図4】第２の実施の形態に係る速度推定装置を示すブロック図である。

【図5】第２の実施の形態における速度推定処理ルーチンの内容を示すフローチャートである。

【発明を実施するための形態】

【0025】

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

【0026】

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

【0027】

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

【0028】

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

【0029】

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

【0030】

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

【0031】

時系列車速蓄積部２０には、車速センサ１２で検出された車速データが時系列に保存される。データ保存のトリガは、一定時間経過時でもよいし、一定距離走行時でもよい。なお、時刻ｔに車速センサ１２で検出された車速データをＶ^t_wheelとすると、最終的に推定する速度ベクトル（Ｖｘ^t，Ｖｙ^t，Ｖｚ^t）とＶ^t_wheelとの関係は、下記（１）式のように設定することができる。

【0032】

【数1】

【0033】

時系列ヨーレイト蓄積部２２には、ヨーレイトセンサ１４で検出されたヨーレイトデータが時系列に保存される。データ保存のトリガは、時系列車速蓄積部２０と同じタイミングとする。

【0034】

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

【0035】

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

【0036】

【数2】

【0037】

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

【0038】

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

【0039】

【数3】

【0040】

ここで、Ｖｓ^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は自車両位置を示す。添え字ｔは時刻ｔのデータであることを表し、添え字ｉはｉ番目のＧＰＳ衛星のデータであることを表す。自車両位置は、擬似距離を用いた一般的な測位方法またはその他の測位方法を用いて算出した値を用いることができる。

【0041】

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

【0042】

【数4】

【0043】

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

【0044】

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

【0045】

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

【0046】

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

【0047】

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

【0048】

【数5】

【0049】

拘束条件１は、推定する速度ベクトルの各成分の大きさを車速データで拘束し、かつ時間変化分を方位角時間変化算出部２６で算出した方位角の時間変化で拘束したものである。拘束条件１内のθ⁰は初期時刻における自車両の進行方向の方位角である。拘束条件２は、高さ方向速度の変化は常に微小であると仮定するものである。拘束条件３は、クロックドリフトの時間変化は緩やかであるため、短時間内における変化は線形であると仮定するものである。拘束条件３内のＣｂｖ⁰は初期時刻におけるクロックドリフト、及びＡはクロックドリフトの時間変化の傾きを示す。なお、ここでは、拘束条件１及び２により、二次元平面内の速度成分のみを拘束する場合について説明するが、３軸ジャイロセンサなどにより取得したピッチレイトを用いて、３次元空間における速度成分を拘束してもよい。上記拘束条件１〜３を、（５）式に取り入れた結果、下記（６）式が得られる。

【0050】

【数6】

【0051】

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

【0052】

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

【0053】

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

【0054】

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

【0055】

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

【0056】

次に、ステップ１０２で、上記ステップ１００で保存された時系列のヨーレイトデータω^tに基づいて、（２）式により、方位角の時間変化θ^t_gyroを算出する。

【0057】

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

【0058】

次に、ステップ１０６で、時間変化分を拘束した（６）式に、上記ステップ１００で取得した車速データＶ^t_wheel、上記ステップ１０２で算出した方位角の時間変化θ^t_gyro、及び上記ステップ１０４で算出したＶｓ^t_i、Ｇｘ^t_i、Ｇｙ^t_i、Ｇｚ^t_iを代入して、時刻数×各時刻の観測衛星数分の方程式を立式する。

【0059】

次に、ステップ１０８で、上記ステップ１０６で立式した方程式を解いて、未知パラメータθ⁰、Ｃｂｖ⁰、及びＡを求める。そして、求めたパラメータθ⁰と、車速データＶ^t_wheel、及び方位角の時間変化θ^t_gyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。

【0060】

以上説明したように、第１の実施の形態の速度推定装置によれば、複数時刻におけるＧＰＳデータを用いて自車両の速度ベクトルを推定する場合に、速度推定のための方程式における未知パラメータの時間変化分を拘束することにより、未知パラメータ数を削減する。これにより、各時刻における観測衛星数が少ない環境でも、未知パラメータ数以上の方程式を立式できる可能性が大きくなり、安定して精度良く自車両の速度ベクトルを推定することができる。

【0061】

なお、第１の実施の形態では、車速データ及び方位角の時間変化による拘束、並びにクロックバイアスの時間変化による拘束の全てを用いる場合について説明したが、これに限定されない。車速データ、方位角の時間変化、クロックバイアスの時間変化各々のみによる拘束条件としてもよいし、また、各々を適宜組み合わせた拘束条件としてもよい。ただし、時間変化分を拘束することにより未知パラメータ数を削減するという観点から、方位角の時間変化及びクロックバイアスの時間変化の少なくとも一方を用いた拘束条件であることが望ましい。

【0062】

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

【0063】

また、拘束条件として、クロックドリフトの時間変化のみを用いた場合には、（６）式において、Ｖ^t_wheel、θ⁰、θ^t_gyro、Ｃｂｖ⁰、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×２＋３」となり、上記と同様に、未知パラメータ数を削減することができる。

【0064】

また、拘束条件として、クロックドリフトの時間変化及び車速データを用いた場合には、（６）式において、θ⁰、θ^t_gyro、Ｃｂｖ⁰、及びＡが未知パラメータとなるため、時系列データの全時刻における未知パラメータ数は、「時刻数×１＋３」となり、上記と同様に、未知パラメータ数を削減することができる。

【0065】

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

【0066】

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

【0067】

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

【0068】

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

【0069】

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

【0070】

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

【0071】

自車速度推定部２３０は、未知パラメータ数選択部３８で選択された未知パラメータ数に応じた方程式を立式した上で、自車両の速度ベクトルを推定する。例えば、選択されたパラメータ数が「３」の場合には、第１の実施の形態と同様に、（６）式により方程式を立式する。この場合、未知パラメータは、θ⁰、Ｃｂｖ⁰、及びＡの３つである。選択された未知パラメータ数が「４」の場合には、下記（７）式により方程式を立式する。（７）式では、ヨーレイトセンサ１４が有するオフセット性の誤差ω_offsetが未知パラメータとして追加されている。すなわち、未知パラメータは、θ⁰、Ｃｂｖ⁰、Ａ、及びω_offsetの４つである。選択された未知パラメータ数が「５」の場合には、下記（８）式により方程式を立式する。（８）式では、さらに、車速センサ１２が有するスケール誤差Ｓが未知パラメータとして追加されている。すなわち、未知パラメータは、θ⁰、Ｃｂｖ⁰、Ａ、ω_offset、及びＳの５つである。

【0072】

【数7】

【0073】

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

【0074】

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

【0075】

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

【0076】

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

【0077】

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

【0078】

次に、ステップ１０２で、方位角の時間変化θ^t_gyroを算出し、次に、ステップ１０４で、衛星方向速度ｓ^t_i、並びに（４）式のＧｘ^t_i、Ｇｙ^t_i及びＧｚ^t_iを算出する。

【0079】

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

【0080】

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

【0081】

次に、ステップ１０８で、上記ステップ２０８で立式した方程式を解いて、選択された未知パラメータ数分の未知パラメータを求めて、求めたパラメータθ⁰と、車速データＶ^t_wheel、及び方位角の時間変化θ^t_gyroとを拘束条件１に代入して、自車両の速度ベクトルを算出して出力し、速度推定処理ルーチンを終了する。

【0082】

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

【0083】

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

【符号の説明】

【0084】

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

【図1】

【図2】

【図3】

【図4】

【図5】