特許 6193833 速度計測装置

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

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

(11)【特許番号】6193833

(24)【登録日】2017年8月18日

(45)【発行日】2017年9月6日

(54)【発明の名称】速度計測装置

(51)【国際特許分類】

   G01S 19/52 20100101AFI20170828BHJP

   G01S 19/47 20100101ALI20170828BHJP

   G01C 21/28 20060101ALI20170828BHJP

【ＦＩ】

   G01S19/52

   G01S19/47

   G01C21/28

【請求項の数】4

【全頁数】26

(21)【出願番号】特願2014-216304(P2014-216304)

(22)【出願日】2014年10月23日

(65)【公開番号】特開2016-85060(P2016-85060A)

(43)【公開日】2016年5月19日

【審査請求日】2016年2月19日

(73)【特許権者】

【識別番号】000145806

【氏名又は名称】株式会社小野測器

(74)【代理人】

【識別番号】100094330

【弁理士】

【氏名又は名称】山田 正紀

(74)【代理人】

【識別番号】100109689

【弁理士】

【氏名又は名称】三上 結

(72)【発明者】

【氏名】市川 和宏

【審査官】 三田村 陽平

(56)【参考文献】

【文献】 特開２０１３−１７０９０４（ＪＰ，Ａ）

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

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

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

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

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

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

【文献】 国際公開第２０１２／１５３５０１（ＷＯ，Ａ１）

【文献】 米国特許出願公開第２０１４／０２１４３１７（ＵＳ，Ａ１）

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

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

Ｇ０１Ｃ ２１／００−２１／３６

Ｇ０１Ｐ ７／００

Ｇ０１Ｐ １５／００−１５／１８

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

【請求項1】

ＩＭＵ由来の、第１の周期で更新される第１の速度データを補正データ分だけ補正して第２の速度データを生成する速度補正部と、
ＧＰＳ受信機由来の、前記第１の周期よりも１周期の時間が長い第２の周期で更新される第３の速度データを補間処理することにより、前記第１の周期で更新される第４の速度データを生成する補間処理部と、
前記第２の速度データを制御データに基づく遅延量だけ遅延させることにより第５の速度データを生成する遅延処理部と、
前記第４の速度データ由来の第６の速度データと前記第５の速度データとの間の差分データを算出する差分演算部と、
前記差分データに基づいて前記補正データを算出する補正データ算出部と、
前記差分データ、前記補正データ、および前記補正データ算出部で該差分データから該補正データを算出する過程で算出される途中のデータの中から選択されたいずれかのデータの過去の複数のサンプリング周期にわたる積算値がこれを越えると発散のおそれが高まる閾値を越えないように調整された遅延量だけ前記第２の速度データを遅延させる前記制御データを生成することにより、前記第５の速度データを前記第６の速度データへの同期に近づけさせる制御データ生成部とを備えたことを特徴とする速度計測装置。

【請求項2】

当該速度計測装置が、速度データを取り扱うとともに位置データを取り扱うものであり、前記差分演算部が、速度データどうしの差分と位置データどうしの差分とからなる差分データを算出するものであって、
前記制御データ生成部が、速度と位置の双方の差分からなる前記差分データ由来のデータに基づいて前記制御データを算出するものであることを特徴とする請求項１記載の速度計測装置。

【請求項3】

前記第４の速度データに遅延を含む処理を施すことにより前記第６の速度データを生成する第２の遅延処理部をさらに備えたことを特徴とする請求項１又は２記載の速度計測装置。

【請求項4】

前記補正データ算出部が、ＩＭＵ由来のデータとＧＰＳ由来のデータとに基づいてＧＰＳ由来のデータの信頼度を表わす良否係数を算出し、前記差分データ由来のデータに基づくとともに該良否係数に基づいて、前記補正データを算出するものであることを特徴とする請求項１から３のうちいずれか１項記載の速度計測装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車両等の移動体に搭載されてその移動体の速度を計測する速度計測装置に関する。

【背景技術】

【0002】

移動体に搭載されたＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）
とＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の双方とを使って、その移動体の速度を高精度に計測する速度計測装置が知られている（例えば、特許文献１，２参照）。

【0003】

ここで、ＩＭＵからは、例えば１００Ｈｚ等のクロックに同期して、ほぼリアルタイムで更新されたデータが出力される。ただし、ＩＭＵでは直接的には加速度が計測され、その加速度を積分することにより速度が計測される。このためＩＭＵで計測される速度は、ＩＭＵで直接に計測される加速度のオフセット誤差が順次累積されることになる。したがって、この速度データを補正しないままにしておくと真の値から徐々に離れて行き、次第に大きな誤差を含むデータとなってしまうおそれがある。

【0004】

一方、ＧＰＳ受信機での速度計測法の１つとして、ＧＰＳ衛星から送られてくる電波の搬送波のドプラシフト量に基づいて速度（以下、「ドプラ速度」と称することがある）を計測する計測法が知られている。この計測法を採用すると高精度な速度計測を行なうことができる。しかしながら、例えば、木々やビル群等によってＧＰＳ衛星からの電波が反射されることなどに起因して同一信号の電波が複数方向から時間差を持って到達するという、いわゆるマルチパスの影響を受けると、計測される速度データに大きなノイズが重畳するなど、高精度な速度計測を行なうことのできる条件には制約がある。

【0005】

特許文献１，２には、ＩＭＵ由来の速度データをＧＰＳ受信機由来のドプラ速度データに基づいて補正して出力速度データを生成する構成が開示されている。具体的には、それらの特許文献１，２には、ＧＰＳ受信機由来の速度データと、そのＧＰＳ受信機由来の速度データは遅延を含むため、その遅延量に合わせるように遅延させた出力速度データとの差分データを生成し、その差分データに基づいて、ＩＭＵ由来の速度データを補正するための補正データを算出し、その補正データを用いてＩＭＵ由来の速度データを補正して出力速度データを生成する速度計測装置が提案されている。

【0006】

ここで、特許文献１には、ＧＰＳ由来のドプラ速度データの信頼度を評価して係数を算出し、その係数に応じて、信頼度が高いときにＩＭＵ由来の速度データを強く補正し、信頼度が低いときには補正量を低減させることが提案されている。

【0007】

また、特許文献２には、ＧＰＳ受信機由来の速度データが、例えば２０Ｈｚ等のクロックに同期した、ＩＭＵと比べゆっくりとした周期でしか速度データが得られないＧＰＳ受信機を採用し、そのＧＰＳ受信機由来の速度データに補間処理を施した構成が開示されている。ＧＰＳ受信機由来の速度データは遅れを伴ったデータであって、しかも、この速度データに補間処理を施すことで遅れがさらに付加されることになる。特許文献２では、出力速度データを大きく遅延させることで、ＧＰＳ受信機由来の、補間処理された速度データと、出力速度データとの同期を確保している。

【先行技術文献】

【特許文献】

【0008】

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

【特許文献2】特開２０１３−１７０９０４号公報

【発明の概要】

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

【0009】

ここで、上掲の特許文献１，２において想定されているＧＰＳ受信機と比べ、速度データがさらにゆっくりとした周期で（例えば１０Ｈｚのクロックに同期して）出力されるＧＰＳ受信機を使用することを考える。ゆっくりとした周期で出力されるＧＰＳ受信機は安価に入手でき、コスト上有利である。このＧＰＳ受信機の場合、速度データの更新速度がゆっくりとしているだけでなく、例えば１５０ｍｓ程度の遅れを伴い、かつジッタを伴った速度データが出力される。この速度データに、ＩＭＵ由来の速度データの周期に合わせるための補間処理を施すと、全体として例えば２５０ｍｓ程度もの遅れが生じることになる。このような大きな遅れを伴ったＧＰＳ由来の速度データとの同期をとるために、特許文献２に開示されたとおりに出力速度データを大幅に遅延させると、フィードバック経路上に大きな遅延が生じることになり、出力速度データが発散し、まともな出力速度データが得られないという問題が発生するおそれがある。

【0010】

本発明は、上記事情に鑑み、ＩＭＵ由来の、高速に更新される速度データによるリアルタイム性を確保しつつ、発散させることもなく正確な出力速度データを得ることのできる速度計測装置を提供することを目的とする。

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

【0011】

上記目的を達成する本発明の速度計測装置は、
ＩＭＵ由来の、第１の周期で更新される第１の速度データを補正データ分だけ補正して第２の速度データを生成する速度補正部と、
ＧＰＳ受信機由来の、第１の周期よりも１周期の時間が長い第２の周期で更新される第３の速度データを補間処理することにより、上記第１の周期で更新される第４の速度データを生成する補間処理部と、
上記第２の速度データを制御データに基づく遅延量だけ遅延させることにより第５の速
度データを生成する遅延処理部と、
上記第４の速度データ由来の第６の速度データと上記第５の速度データとの間の差分デ
ータを算出する差分演算部と、
差分データに基づいて上記補正データを算出する補正データ算出部と、
差分データ、速度補正部に入力される補正データ、および補正データ算出部で差分データから補正データを算出する過程で算出される途中のデータの中から選択されたいずれかのデータの過去の複数のサンプリング周期にわたる積算値がこれを越えると発散のおそれが高まる閾値を越えないように調整された遅延量だけ第２の速度データを遅延させる制御データを生成することにより、上記第５の速度データを第６の速度データへの同期に近づけさせる制御データ生成部とを備えたことを特徴とする。

【0012】

ここで、本発明の速度計測装置では、第２の速度データがこの速度計測装置で求められる出力速度データとなる。

【0014】

本発明の速度計測装置は、差分データ等のデータに基づいて、第２の速度データの発散を免れる範囲内で第２の速度データを遅延させる制御データを生成し、その制御データに基づいて第２の速度データを遅延させて第５の速度データを生成する構成としたものである。

【0015】

遅延量のみを考慮したときは、第２の速度データを第６の速度データの遅延量と同一の遅延量まで遅延させることが望ましい。ただし、これでは第２の速度データが発散してしまうおそれがある。

【0016】

本発明では、差分データ等のデータをモニタリングして、発散を免れる程度に遅延させる構成としたため、リアルタイム性が確保された高精度の第２の速度データを得ることができる。

【0017】

ここで、本発明の速度計測装置が、速度データを取り扱うとともに位置データを取り扱うものであり、上記差分演算部が、速度データどうしの差分と位置データどうしの差分とからなる差分データを算出するものであって、
上記制御データ生成部が、速度と位置の双方の差分からなる差分データ由来のデータに基づいて制御データを算出するものであることが好ましい。

【0018】

速度データとともに位置データを取り扱う場合、第２の速度データの遅延量を定める制御データを、速度データどうしの差分と位置データどうしの差分とからなる差分データ由来のデータに基づいて算出すると、この装置で取り扱う速度と位置の双方について高精度なデータを生成することができる。

【0019】

さらに、本発明の速度計測装置は、上記第４の速度データに遅延を含む処理を施すことにより上記第６の速度データを生成する第２の遅延処理部をさらに備えたものであってもよい。

【0020】

後述する実施形態において例示するように、本発明の速度計測装置では、上記の第２の遅延処理部に相当する処理を行なうことがある。本発明の速度計測装置は、その場合であっても、発散を免れ、リアルタイム性が確保された高精度な速度データ（第２の速度データ）を得ることができる。

【0021】

さらに、本発明の速度計測装置において、上記補正データ算出部が、ＩＭＵ由来のデータとＧＰＳ由来のデータとに基づいてＧＰＳ由来のデータの信頼度を表わす良否係数を算出し、上記差分データ由来のデータに基づくとともに良否係数に基づいて、上記補正データを算出するものであることが好ましい。

【0022】

上記の良否係数を算出し、その良否係数を用いて補正データを算出すると、より正確な第２の速度データを算出することができる。

【発明の効果】

【0023】

以上の本発明によれば、リアルタイム性が確保され、かつ発散させることなく高精度な速度データを生成することができる。

【図面の簡単な説明】

【0024】

【図1】本発明の第１実施形態としての速度計測装置の構成を示したブロック図である。

【図2】本発明の第２実施形態としての速度計測装置の構成を示したブロック図である。

【図3】収束判定部における処理内容を示したフローチャートである。

【図4】第２実施形態における効果を示した図である。

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

【0025】

以下、本発明の実施の形態を説明する。

【0026】

ここでは、基本的な構成を第１実施形態として説明し、次いで、より具体的な構成を第２実施形態として説明する。

【0027】

図１は、本発明の第１実施形態としての速度計測装置の構成を示したブロック図である。

【0028】

この図１に示す速度計測装置１０は、一例として自動車に搭載され、その同じ自動車に搭載されたＩＭＵ１およびＧＰＳ受信機２の双方からデータを受け取ってこの自動車の速度を表わす速度データを生成して出力する装置である。この速度計測装置１０で様々な態様の速度データを取り扱うため、相互の区別のために第１の速度データ、第２の速度データ・・・等の用語を用いている。図１では、第１の速度データ、第２の速度データ、・・・等を、（１），（２），・・・等の記号で表している。この速度計測装置１０において最終的に生成される出力速度データは、ここでは、第２の速度データと称する。

【0029】

ＩＭＵ１は、慣性計測装置と呼ばれ、３軸のジャイロセンサと３軸の加速度センサが組み込まれていて、３次元の角速度と３方向の加速度が求められる。

【0030】

このＩＭＵ１では、角速度と加速度が、リアルタイムで、かつ例えば１０ｍｓ等の第１の周期で求められて、それらを表わすデータが１００Ｈｚのクロックに同期して出力され、この速度計測装置１０に入力される。ＩＭＵ１から出力される角速度および加速度は、そのＩＭＵ１が搭載されている自動車の進行方向を基準としたときの角速度および加速度である。このＩＭＵ１からのデータは、速度計測装置１０の速度算出部１１に入力され、この速度算出部１１では、その入力されたデータに基づいて、この速度計測装置１０が搭載されている自動車(以下、単に「自動車」と称する)の速度が算出される。具体的には、加速度の積算による速度の算出と角速度から求められる角度に基づく座標変換とにより、自動車の進行方向基準ではなく、地軸基準の速度（北方向、東方向、下方向）の各速度が算出される。ここでは、この速度算出部１１で算出された速度を表わすデータを「第１の速度データ」と称する。上述の通り、ＩＭＵ１からは、第１の周期（例えば１０ｍｓ間隔）でデータが出力され、速度算出部１１においても第１の速度データがその第１の周期で繰り返し算出される。

【0031】

この第１の速度データは、速度補正部１２に入力される。速度補正部１２では、速度算出部１１で算出されたＩＭＵ１由来の、第１の周期で更新される第１の速度データが、後述するようにして算出された補正データ分だけ補正されて、第２の速度データが生成される。上述の通り、この第２の速度データは、この速度計測装置１０から出力される出力速度データであって、リアルタイム性が確保された出力速度データである。

【0032】

一方、ＧＰＳ受信機２からは、ＧＰＳ搬送波のドプラシフト量から求められるドプラ速度を表わす第３の速度データが出力される。この第３の速度データは、自動車の進行方向基準ではなく、地軸基準の、北向き、東向き、下向きの３軸の速度を表わす速度データである。このＧＰＳ受信機２から出力される第３の速度データは、上記の第１の周期よりも１周期の時間が長い第２の周期、例えば１００ｍｓごとに更新されるデータである。しかも、このＧＰＳ受信機２から出力される速度データは、出力される時点において既に１４０〜１５０ｍｓ程度の遅れを伴い、かつジッタ（遅れのゆれ）を持った速度データである。

【0033】

ＧＰＳ受信機２から出力された第３の速度データは、補間処理部１３に入力され、この補間処理部１３では、その第３の速度データを補間処理することにより、上記の第１の周期で更新される第４の速度データが生成される。この補間処理部１３では、第２の周期（例えば１００ｍｓ）で更新される第３の速度データが線形補間され、第１の周期１周期分の時間（例えば１０ｍｓ）ごとに更新される第４の速度データが生成される。ここで、この補間処理部１３では、ＧＰＳ受信機２からの、第２の周期（例えば１００ｍｓ）離れた隣接する２つの速度データのうちの時間的に後の速度データの出力を待って補間処理が実行されるため、この第４の速度データは、その補間処理により第２の周期１周期分（例えば１００ｍｓ）だけ更に遅れたデータとなる。

【0034】

一方、速度補正部１２から出力された第２の速度データは、遅延処理部１４に入力され、この遅延処理部１４において制御データに基づく遅延量だけ遅延されることにより第５の速度データとなる。制御データについては後述する。遅延処理部１４における遅延処理により生成された第５の速度データは、差分演算部１５に入力される。補間処理１３での補間処理により生成された第４の速度データ由来の第６の速度データも、差分演算部１５に入力される。

【0035】

ここで、後述する第２実施形態において具体例を示すように、本発明の態様によっては、補間処理部１３と差分演算部１５との間に、遅延を含む処理を施す第２の遅延処理部１８が配置されることがある。そこで、ここでは、補間処理部１３で生成される速度データを第４の速度データと称し、差分演算部１５に入力される速度データを第６の速度データと称する。第２の遅延処理部１８が配置されない態様のときは、第４の速度データがそのまま第６の速度データとなる。

【0036】

差分演算部１５では、第６の速度データと第５の速度データとの間の差分が演算されて両速度の差分値を表わす差分データが算出され、補正データ算出部１６と制御データ生成部１７に入力される。補正データ算出部１６では、入力された差分データに基づいて補正データを算出する。

【0037】

ＧＰＳ受信機２では、ドプラ速度が生成されるとともに、速度分散に基づいて、その生成したドプラ速度の精度の指標となるドプラ精度指標が生成される。本実施形態の補正データ算出部１６では、そのドプラ精度指標を受け取り、そのドプラ精度指標に基づいて、ある閾値を境にして精度が高いと判定されるときに差分データに補正係数を乗算して補正データとして出力し、低いと判定とされるときに補正量ゼロからなる補正データを出力する。

【0038】

前述の通り、速度補正部１２では、速度算出部１１で算出されたＩＭＵ１由来の第１の速度データを補正データに基づいて補正し、この速度計測装置１０の出力速度データである第２の速度データを生成する。したがって、この実施形態における速度補正部１２では、ＧＰＳ受信機２で生成されるドプラ精度指標に応じて、ＧＰＳ受信機２で生成されたドプラ速度の精度が高いと判定されたときに差分データに補正係数が乗算された補正データにより第１の速度データが補正されて第２の速度データが生成され、ドプラ速度の精度が低いと判定されたときには、第１の速度データがそのまま第２の速度データとして出力される。このようにして、この速度計測装置１０では、高精度の第２の速度データが生成される。この第２の速度データは、ＩＭＵ１からデータが出力される周期である第１の周期で更新され、かつリアルタイム性が確保されたデータである。

【0039】

ここで、ＧＰＳ受信機２由来の差分演算部１５に入力される第６の速度データは、第２の遅延処理部１８が配置されていない態様においても、前述の通り大きな遅延を伴った速度データである。第２の遅延処理部１８が配置された態様の場合は、遅延がさらに大きくなる。差分演算部１５では、遅延した第６の速度データの遅延量と同一の遅延量の第５の速度データとの差分が求められることが望ましい。しかしながら、遅延処理部１４は、速度補正部１２から出力された第２の速度データのフィードバック経路上に配置された遅延要素である。したがって第２の速度データをこの遅延処理部１４で大きく遅延させた第５の速度データを生成すると、第２の速度データが発散してしまい、まともな出力速度データとして成立しなくなるおそれがある。そこで、本実施形態では、以下の構成により発散を防いでいる。

【0040】

差分演算部１５で生成された差分データは、補正データ演算部１６とともに制御データ生成部１７にも入力される。この制御データ生成部１７では、入力された差分データに基づいて、第２の速度データの発散を免れる範囲内で第２の速度データを遅延させるための制御データが生成される。本実施形態における制御データ生成部１７は、以下の演算により、第２の速度データの発散のおそれの有無を判定している。

【0041】

差分演算部１５から出力される差分データを、

【0042】

【数1】

【0043】

但し、

【0044】

【数2】

【0045】

は、差分速度の、それぞれ北向き、東向き、下向きの各成分を表わす。
としたとき、

【0046】

【数3】

【0047】

但し、

【0048】

【数4】

【0049】

は、それぞれ、今回出力された差分データから

【0050】

【数5】

【0051】

周期前に出力された差分データに至る一連のｎ個の差分データであることを表わ している。すなわち

【0052】

【数6】

【0053】

は、過去

【0054】

【数7】

【0055】

周期分の差分データの２乗和を表わしている。
に基づき、ある閾値を、

【0056】

【数8】

【0057】

としたときに、

【0058】

【数9】

【0059】

のときに発散のおそれがなく、

【0060】

【数10】

【0061】

のときに発散のおそれがある、と判定し、

【0062】

【数11】

【0063】

の範囲内で大きな遅延を指示する制御データを生成している。

【0064】

この制御データ生成部１７で生成された制御データは遅延処理部１４に入力され、第２の速度データをその制御データに応じた遅延量だけ遅延させる。したがって差分演算部１５には、第２の速度データが発散しないレベルで遅延された第５の速度データが入力される。これにより、速度補正部１２からは、リアルタイム性が確保された高精度の第２の速度データが出力される。

【0065】

なお、ここでは、制御データ生成部１７には差分データが入力され、制御データ生成部１７ではその差分データに基づいて制御データが生成されている。ただし、速度補正部１２での補正には補正データが用いられており、第２の速度データが発散するか否かは、この補正データに依存する。そこで、この補正データを制御データ生成部１７に入力し、制御データ生成部１７ではその補正データに基づく制御データを生成してもよい。

【0066】

次に、この第１実施形態と比べ、より具体的な実施形態である第２実施形態について説明する。

【0067】

図２は、本発明の第２実施形態としての速度計測装置の構成を示したブロック図である。この図２に示すＩＭＵ１、ＧＰＳ受信機２、および速度計測装置２０も、図１に示す第１実施形態の場合と同様、自動車に搭載されたものである。この図２に示す第２実施形態の説明にあたっては、図１に示す第１実施形態における要素と同一の要素については、説明を省略することがある。

【0068】

ＩＭＵ１からは、１００Ｈｚのクロックに同期して、３次元の角速度と３方向の加速度のデータが出力され、ストラップダウンナビゲータ部２１に入力される。このストラップダウンナビゲータ部２１は、図１に示す第１実施形態における速度算出部１１と速度補正部１２とを合わせたものに相当する。すなわち、このストラップダウンナビゲータ部２１では、ＩＭＵ１から出力された、自動車の進行方向基準の角速度と加速度に基づいて、地軸基準の速度データが算出される。また、このストラップダウンナビゲータ部２１の場合、図１の第１実施形態とは異なり、このストラップダウンナビゲータ部２１においてその速度データがさらに積算処理されて位置データも生成される。このストラップダウンナビゲータ部２１ではさらに、その生成された速度データと位置データが補正データに基づいて補正されて出力速度データと出力位置データが生成される。ストラップダウンナビゲータ部２１自体は良く知られた構成であり、ここでのこれ以上の詳細は省略する。この図２において、補正データは、

【0069】

【数12】

【0070】

と表記されている。ここで、

【0071】

【数13】

【0072】

はカルマンゲイン係数、

【0073】

【数14】

【0074】

は良否係数、

【0075】

【数15】

【0076】

は差分データにより表わされる差分値である。この差分値

【0077】

【数16】

【0078】

には速度の差分と位置の差分との双方が含まれている。詳細は後述する。

【0079】

ＧＰＳ受信機２からは１０Ｈｚのクロックに同期して、ドプラ速度を表わす速度データと位置データが出力される。これらの速度データおよび位置データはいずれも地軸基準の３次元データである。これらの速度データおよび位置データは、１０Ｈｚのクロックに同期して１００ｍｓごとに更新される。ただし、更新されるデータは実時間からすると既に１５０ｍｓ程度遅延し、しかも数ｍｓのジッタ（遅延のゆれ）を伴ったデータである。これら出力された速度データおよび位置データは補間処理部２２に入力される。この補間処理部２２は、第１実施形態における補間処理部１３に相当する。ＧＰＳ受信機２からはさらに、第１実施形態において説明した速度分散に基づくドプラ精度指標も出力される。このドプラ精度指標は補間処理部２２を通過して良否係数算出部２３に入力される。良否係数算出部２３については後述する。

【0080】

補間処理部２２では、ＧＰＳ受信機２から出力されてきた１００ｍｓごとの速度データおよび位置データが、線形補間により、ＩＭＵ１由来のデータと同じ周期、すなわち１０ｍｓごとの速度データおよび位置データに変換される。この変換により生成された１０ｍｓごとの速度データおよび位置データは、同期ずれ補正部２４に入力される。この同期ずれ補正部２４では、ＩＭＵ１から出力されるデータと、補間処理部２２において補間処理された、ＧＰＳ受信機２由来のデータとの間の、１０ｍｓ未満の同期ずれが補正される（前掲の特許文献２参照）。ここで、ＩＭＵ１から出力されるデータは、１００Ｈｚのクロックに同期したデータであるが、ＩＭＵ１では、この１００Ｈｚのクロックの生成に水晶発振器を利用しており、実時間からすると誤差を含んでいる。

【0081】

一方、ＧＰＳ受信機２から出力されるデータは、１０Ｈｚのクロックに同期したデータである。ただし、ＩＭＵ１の場合と異なり、このＧＰＳ受信機２で生成される１０Ｈｚというクロックの周期は正確である。ただし、出力データ自体が１５０ｍｓ程度遅延しており、かつ数ｍｓ程度のジッタ（遅延のゆれ）を含んでいる。そこで、同期ずれ補正部２４では、ＩＭＵ１のクロック（１００Ｈｚ）とＧＰＳ受信機２のクロック（１０Ｈｚ）とを比較し、ＩＭＵ１由来のデータとＧＰＳ受信機２由来のデータとの間の、クロック（１００Ｈｚ）１周期未満の同期ずれを補正するように、補間処理部２２から出力されたデータの遅れが補正される。

【0082】

なお、この同期ずれ補正部２４では、速度データと位置データの双方について同期ずれを補正してもよいが、本実施形態の主目的は正確な速度データを生成することにあり、したがって、この同期ずれ補正部２４では、速度データのみの同期ずれを補正するものであってもよい。

【0083】

同期ずれ補正部２４から出力されたデータは、次に遅延処理部２５に入力される。この遅延処理部２５では、入力されてきた速度データおよび位置データが、後述する良否係数算出部２３における良否係数βの算出に要する時間と同じ時間だけ遅延される。ここで、良否係数算出部２３における良否係数βの算出演算は、１００Ｈｚのクロック（１０ｍｓ周期）に同期して実行されるため、この良否係数算出部２３における良否係数算出部に要する時間は１０ｍｓ単位の時間である。したがって遅延処理部２５でもそれと同じ１０ｍｓ単位の時間だけ遅延される。

【0084】

本実施形態では、同期ずれ補正部２４と遅延処理部２５とを合わせたものが、第１実施形態において破線で示した第２の遅延処理部１８（図１参照）の一例に相当する。

【0085】

遅延処理部２５から出力された速度データおよび位置データは、差分演算部２６に入力される。

【0086】

一方、ストラップダウンナビゲータ部２１から出力された出力速度データおよび出力位置データは、同期化処理部２７を経由することで遅延を受けた後、差分演算部２６に入力される。この同期化処理部２７は、入力されてきた出力速度データおよび出力位置データを、後述する収束判定部２８で生成された制御データに応じた遅延量だけ遅延させる。

【0087】

ここで、同期化処理部２７は、第１実施形態（図１参照）における遅延処理部１４に対応し、収束判定部２８は、第１実施形態における制御データ生成部１７に対応する。ただし、この第２実施形態では速度データとともに位置データも取り扱っており、同期化処理部２７および収束判定部２８においても、速度データとともに位置データも取り扱われている。

【0088】

同期化処理部２７で遅延処理を受けた出力速度データおよび出力位置データは、差分演算部２６に入力される。差分演算部２６では、同期化処理部２７で遅延処理を受けた出力速度データおよび出力位置データと、遅延処理部２５から出力された、ＧＰＳ受信機２由来の速度データおよび位置データとの間の差分が算出されて、差分データが算出される。具体的には、遅延処理部２５から出力された速度データ

【0089】

【数17】

【0090】

および位置データ

【0091】

【数18】

【0092】

を、それぞれ、

【0093】

【数19】

【0094】

【数20】

【0095】

但し、

【0096】

【数21】

【0097】

は、速度の北向きの成分

【0098】

【数22】

【0099】

は、速度の東向きの成分

【0100】

【数23】

【0101】

は、速度の下向きの成分

【0102】

【数24】

【0103】

は、位置の北向きの成分

【0104】

【数25】

【0105】

は、位置の東向きの成分

【0106】

【数26】

【0107】

は、位置の下向きの成分
を表わす。
とし、同期化処理部２７で遅延処理を受けた出力速度データ

【0108】

【数27】

【0109】

および出力位置データ

【0110】

【数28】

【0111】

を、それぞれ、

【0112】

【数29】

【0113】

【数30】

【0114】

但し、

【0115】

【数31】

【0116】

は、速度の北向きの成分

【0117】

【数32】

【0118】

は、速度の東向きの成分

【0119】

【数33】

【0120】

は、速度の下向きの成分

【0121】

【数34】

【0122】

は、位置の北向きの成分

【0123】

【数35】

【0124】

は、位置の東向きの成分

【0125】

【数36】

【0126】

は、位置の下向きの成分
を表わす。
としたとき、差分演算部２６では、差分値

【0127】

【数37】

【0128】

、すなわち

【0129】

【数38】

【0130】

が算出される。

【0131】

ここで、

【0132】

【数39】

【0133】

は位置誤差

【0134】

【数40】

【0135】

は速度誤差
であって、

【0136】

【数41】

【0137】

【数42】

【0138】

である。

【0139】

差分演算部２６で算出された差分値

【0140】

【数43】

【0141】

を表わす差分データは、収束判定部２８と乗算部２９とに入力される。本実施形態では、乗算部２９と、良否係数算出部２３と、拡張カルマンフィルタ部３０の組合せが、第１実施形態（図１参照）における補正データ算出部１６に対応する。但し、この第２実施形態における補正データ算出アルゴリズムは、第１実施形態のものとは異なっている。詳細は後述する。

【0142】

収束判定部２８では、入力されてきた差分データに基づいて、出力速度データおよび出力位置データを、それらが発散しない範囲内で遅延させる制御データを生成する。同期化処理部２７は、この収束判定部２８で生成された制御データに基づいて、出力速度データおよび出力位置データを発散しない範囲内で遅延させて、遅延処理部２５から出力されるＧＰＳ受信機２由来の速度データおよび位置データとの同期に近づける。

【0143】

図３は、収束判定部における処理内容を示したフローチャートである。

【0144】

この収束判定部２８では、先ず誤差２乗積算値

【0145】

【数44】

【0146】

が、

【0147】

【数45】

【0148】

によって算出される（ステップＳ１）。

【0149】

ここで、

【0150】

【数46】

【0151】

は、いずれも調整係数であり、

【0152】

【数47】

【0153】

は、過去の

【0154】

【数48】

【0155】

周期分のデータの積算を表わす。

【0156】

この収束判定部２８では、次に、この算出した誤差２乗積算値

【0157】

【数49】

【0158】

が閾値

【0159】

【数50】

【0160】

と比較される（ステップＳ２）。誤差２乗積算値

【0161】

【数51】

【0162】

がこの閾値

【0163】

【数52】

【0164】

以下のときは、発散に至るまでに余裕があり、出力速度データおよび出力位置データの遅延量をさらに増加させるべきことを示している。

【0165】

ステップＳ２において、

【0166】

【数53】

【0167】

と判定されたときは、遅延量を１０ｍｓ増加させる（ステップＳ３）。

【0168】

次に、誤差２乗積算値

【0169】

【数54】

【0170】

が閾値

【0171】

【数55】

【0172】

と比較される（ステップＳ４）。この閾値

【0173】

【数56】

【0174】

は、誤差２乗積算値

【0175】

【数57】

【0176】

がこの閾値

【0177】

【数58】

【0178】

を超えると発散のおそれが高まることを示している。ステップＳ４において、

【0179】

【数59】

【0180】

と判定されたときは、遅延量が１０ｍｓ減少される（ステップＳ５）。

【0181】

このようにして遅延量が定められ、その遅延量だけ遅延させることを指示する制御データが出力される（ステップＳ６）。この遅延量の算出は、１０ｍｓ周期で繰り返し行われる。この制御データは、前述の通り、同期化処理部２７に入力され、同期化処理部２７では出力速度データおよび出力位置データがその制御データに応じた遅延量だけ遅延される。これにより、出力速度データおよび出力位置データは発散が防止され、高精度なデータが維持される。

【0182】

なお、ここでは、収束判定部２８では、速度データの場合と位置データの差分との双方を使って、遅延量算出の基礎となる誤差２乗積算値

【0183】

【数60】

【0184】

を算出しているが、速度データの発散の防止および精度維持を目的とする場合、速度データの差分のみに基づいて誤差２乗積算値

【0185】

【数61】

【0186】

を算出してもよい。

【0187】

また、この図３に示すフローチャートは、遅延量算出アルゴリズムの一例を示したものであり、このアルゴリズムに限るものではない。例えば遅延量を一度定めればその後は安定的に動作する系の場合は、図３のフローチャートのステップＳ４，Ｓ５を削り、遅延量ゼロから出発して、誤差２乗積算値

【0188】

【数62】

【0189】

がある閾値

【0190】

【数63】

【0191】

を超えるまで遅延量を増加して、誤差２乗積算値

【0192】

【数64】

【0193】

がその閾値を超えた時点で遅延量算出の演算を終了させ、その時点以降は、その時点の遅延量を表わす制御データに固定してもよい。

【0194】

さらに、本実施形態では、収束判定部２８において、差分演算部２６で算出された差分値

【0195】

【数65】

【0196】

に基づいて制御データを算出しているが、収束判定部２８では、以下において説明する乗算部２９において生成される乗算値

【0197】

【数66】

【0198】

や拡張カルマンフィルタ部３０から出力される補正値

【0199】

【数67】

【0200】

など、差分値

【0201】

【数68】

【0202】

以外の、その差分値

【0203】

【数69】

【0204】

由来の値に基づいて制御データを算出してもよい。

【0205】

次に、図２に戻って、良否係数算出部２３、乗算部２９および拡張カルマンフィルタ部３０からなる補正データ算出処理について説明する（前掲の特許文献１参照）。

【0206】

良否係数算出部２３には、補間処理部２２での補間処理により生成されたＧＰＳ受信機２由来の速度データと、ＩＭＵ１から出力された加速度および角速度のデータが入力される。

【0207】

良否係数算出部２３では、ＧＰＳ受信機２由来の速度データを微分して加速度データを算出するとともに、ＩＭＵ１からデータに基づいて地軸基準の加速度データを算出する。この良否係数算出部２３では、さらに、このようにして算出した両者の加速度データどうしの差分の２乗を算出し、その算出結果の移動平均値を算出する。さらに、この移動平均値を規格化処理することにより、０〜１の範囲内の値であって、０に近づくほど誤差が小さく、１に近づくほど誤差が大きいという、誤差の指標値を算出する。この良否係数算出部２３では、

【0208】

【数70】

【0209】

により、良否係数

【0210】

【数71】

【0211】

が算出される。

【0212】

ここで、ドプラ精度指標値は、前述したように、ＧＰＳ受信機２において速度分散に基づいて算出される、ドプラ速度の精度の指標を表わす値である。また、

【0213】

【数72】

【0214】

は調整係数であって、

【0215】

【数73】

【0216】

の範囲内で適切に定められる。

【0217】

良否係数算出部２３で算出された良否係数

【0218】

【数74】

【0219】

は、乗算部２９に入力され、乗算部２９では、差分演算部２６で算出された差分値

【0220】

【数75】

【0221】

に良否係数

【0222】

【数76】

【0223】

が乗算される。

【0224】

この乗算値

【0225】

【数77】

【0226】

は、拡張カルマンフィルタ部３０に入力される。これは、誤差が小さく

【0227】

【数78】

【0228】

のときは、差分演算部２６で算出された差分値

【0229】

【数79】

【0230】

がそのまま拡張カルマンフィルタ部３０の入力として反映され、誤差が大きく

【0231】

【数80】

【0232】

のときは、差分値

【0233】

【数81】

【0234】

は、ゼロあるいは小さい値でしか拡張カルマンフィルタ部３０の入力として反映されないことを意味している。

【0235】

拡張カルマンフィルタ部３０では、運動方程式に基づいてカルマン係数

【0236】

【数82】

【0237】

を算出し、ＩＭＵ１由来のデータを補正するための補正値

【0238】

【数83】

【0239】

を表わす補正データを生成してストラップダウンナビゲータ部２１に向けて出力する。この拡張カルマンフィルタ部３０自体は広く知られているため、ここでのこれ以上の詳細説明は省略する。

【0240】

ストラップダウンナビゲータ部２１では、ＩＭＵ１由来の速度データおよび位置データを、拡張カルマンフィルタ部３０から受け取った補正データにより表わされる補正値

【0241】

【数84】

【0242】

だけ補正することで、出力速度データおよび出力位置データを生成する。

【0243】

このようにして生成された出力速度データおよび出力位置データは、発散することなく、ＩＭＵ１由来の１００Ｈｚクロックに同期した、リアルタイム性が確保されたデータである。しかも、ＧＰＳ受信機２由来のデータによってオフセット誤差等がキャンセルされた高精度のデータである。

【0244】

図４は、第２実施形態における効果を示した図である。

【0245】

図４の横軸は時間（ｓ）、横軸は速度（ｋｍ／ｈ）である。ここでは、加速時のドプラ速度（破線）と出力速度（実線）の変化を表わしている。ここで、ドプラ速度は、ＧＰＳ受信機２の出力であり、出力速度は、ストラップダウンナビゲータ部２１の出力である。

【0246】

ドプラ速度は、変化が粗く、かつ時間遅れがあることが分かる。これに対し、出力速度は、発散もなく、ＩＭＵ１由来のリアルタイム性が確保されており、かつ滑らかに変化していることが分かる。

【0247】

なお、ここでは自動車に搭載された速度計測装置について説明したが、本発明の速度計測装置は自動車に搭載されるものには限られない。本発明の速度計測装置は、例えば船舶や飛行機等、自動車以外の移動体に搭載されるものであってもよい。

【符号の説明】

【0248】

１ ＩＭＵ
２ ＧＰＳ受信機
１０，２０ 速度計測装置
１１ 速度算出部
１２ 速度補正部
１３，２２ 補間処理部
１４，２５ 遅延処理部
１５，２６ 差分演算部
１６ 補正データ算出部
１７ 制御データ生成部
１８ 第２の遅延処理部
２１ ストラップダウンナビゲータ部
２３ 良否係数算出部
２４ 同期ずれ補正部
２７ 同期化処理部
２８ 収束判定部
２９ 乗算部
３０ 拡張カルマンフィルタ部

【図1】

【図2】

【図3】

【図4】