JP5625293B2 - 速度算出装置、速度算出方法及びナビゲーション装置 - Google Patents

Description

本発明は、速度算出装置、速度算出方法及びナビゲーション装置に関し、例えば携帯型ナビゲーション装置に適用して好適なものである。

従来、ナビゲーション装置においては、複数のＧＰＳ（Global Positioning System）衛星からの測位信号（以下、これをＧＰＳ信号とも呼ぶ）を受信し、当該ＧＰＳ信号に基づいて移動体（例えば車両等）の現在位置を算出するようになされている。

しかしながらこのようなナビゲーション装置では、当該ナビゲーション装置を載置した車両が例えばトンネルや地下駐車場等の中に入った場合、ＧＰＳ衛星からのＧＰＳ信号を受信できず、従って、当該ＧＰＳ信号に基づいて車両の現在位置を算出することができない。

そこでナビゲーション装置のなかには、ＧＰＳ信号が受信できなくなった場合であっても、コーナリング時において、車両の進行方向に直交した水平方向の加速度と、当該進行方向に直交した垂直軸周りの角速度とを基に進行方向の速度を算出し、当該進行方向の速度に基づいて現在位置を算出するものがある（例えば、特許文献１参照）。

特開２００８−７６３８９公報

ところで上述したナビゲーション装置では、コーナリング時においては進行方向の速度を算出することができるものの、直進時においては進行方向の速度を算出することができないため、必ずしも全ての移動面（すなわち道路等）の環境下で進行方向の速度を算出することができないという問題があった。

本発明は以上の点を考慮してなされたもので、移動面の環境に関わらず移動体の速度を高精度に算出し得る速度算出装置、速度算出方法及びナビゲーション装置を提案しようとするものである。

かかる課題を解決するため本発明の速度算出装置においては、所定の移動面上を移動する移動体に搭載され、移動面のうねりに応じて発生する垂直方向の加速度を検出する垂直方向加速度検出部と、移動体に搭載され、移動面のうねりに応じて発生する移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、移動体の進行方向への速度を検出し基準速度とする速度検出部と、所定期間における垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、所定期間における水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、最大加速度と最小加速度との差を最大角速度と最小角速度との差で除算することにより、移動体の進行方向の速度を算出する速度算出部と、速度を変数とし所定の定数を用いる関数に速度を適用することにより当該速度を補正する速度補正部とを設け、関数は、速度を算出する算出式において最大加速度、最小加速度、最大角速度及び最小角速度を、移動面のうねりにおける曲率半径と基準速度とを用いた変数にそれぞれ置き換え基準速度について表した関数でなるようにした。

また本発明の速度算出方法においては、所定の移動面のうねりに応じて当該移動面上を移動する移動体に発生する垂直方向の加速度を検出する垂直方向加速度検出ステップと、移動面のうねりに応じて移動体に発生する移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出ステップと、移動体の進行方向への速度を検出し基準速度とする速度検出ステップと、所定期間における垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、所定期間における水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、最大加速度と最小加速度との差を最大角速度と最小角速度との差で除算することにより、移動体の進行方向の速度を算出する速度算出ステップと、速度を変数とし所定の定数を用いる関数に速度を適用することにより当該速度を補正する速度補正ステップとを設け、関数は、速度を算出する算出式において最大加速度、最小加速度、最大角速度及び最小角速度を、移動面のうねりにおける曲率半径と基準速度とを用いた変数にそれぞれ置き換え基準速度について表した関数でなるようにした。

さらに本発明のナビゲーション装置においては、所定の移動面のうねりに応じて当該移動面上を移動する移動体に発生する垂直方向の加速度を検出する垂直方向加速度検出部と、移動面のうねりに応じて移動体に発生する移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、移動体の進行方向への速度を検出し基準速度とする速度検出部と、所定期間における垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、所定期間における水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、最大加速度と最小加速度との差を最大角速度と最小角速度との差で除算することにより、移動体の進行方向の速度を算出する速度算出部と、速度を変数とし所定の定数を用いる関数に速度を適用することにより当該速度を補正する速度補正部と、進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、垂直軸周りの角速度に基づいて移動体が旋回した角度を算出する角度算出部と、速度算出部により算出された進行方向の速度と、角度算出部により算出された角度とに基づいて、移動体の位置を算出する位置算出部とを設け、関数は、速度を算出する算出式において最大加速度、最小加速度、最大角速度及び最小角速度を、移動面のうねりにおける曲率半径と基準速度とを用いた変数にそれぞれ置き換え基準速度について表した関数でなるようにした。

これにより、移動面のうねりに応じて発生する垂直方向の加速度と、進行方向と直交した水平軸周りの角速度とを検出し、当該垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を全ての道路環境下で算出することができ、さらに速度に応じて発生し得る誤差を当該速度に応じた補正処理により縮小させることができる。

本発明によれば、移動面のうねりに応じて発生する垂直方向の加速度と、進行方向と直交した水平軸周りの角速度とを検出し、当該垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出し、さらに速度に応じて発生し得る誤差を当該速度に応じた補正処理により縮小させることにより、移動面の環境に関わらず移動体の速度を高精度に算出し得る速度算出装置、速度算出方法及びナビゲーション装置を実現することができる。

凹凸路面の走行時の様子を示す略線図である。 カーブ走行時の様子を示す略線図である。 速度及び角度を用いた現在位置算出手法を示す略線図である。 ＰＮＤの全体構成を示す略線図である。 ＰＮＤの座標系の定義を示す略線図である。 ＰＮＤにおけるセンサ構成を示す略線図である。 ＰＮＤの回路構成を示す略線図である。 速度算出部の構成を示す略線図である。 高度及び角度の関係を示す略線図である。 低速時における路面角度の様子を示す略線図である。 高速時における路面角度の様子を示す略線図である。 超低速時における路面角度の様子を示す略線図である。 クレードルによる振動の様子を示す略線図である。 ハイパスフィルタ処理後の加算加速度及び加算角速度を示す略線図である。 ４０９６データ点ごとにフーリエ変換を施した加算角速度を示す略線図である。 ４０９６データ点ごとにフーリエ変換を施した加算加速度を示す略線図である。 加算加速度に対するローパスフィルタ処理の比較を示す略線図である。 加算角速度に対するローパスフィルタ処理の比較を示す略線図である。 低速時におけるフロント加速度及びリア加速度の関係を示す略線図である。 中速及び高速時におけるフロント加速度及びリア加速度の関係を示す略線図である。 ３種類の設置位置による加速度、ピッチレート及び速度のシミュレーション例を示す略線図である。 最大値及び最小値の関係を示す略線図である。 速度とデータ点数との関係を示す略線図である。 異なる円弧の長さによる加速度及びピッチレートの様子を示す略線図である。 速度算出処理を用いた現在位置算出処理手順の説明に供するフローチャートである。 加速度、角速度及び速度の測定結果例を示す略線図である。 測定結果とリファレンスとの比較（１）を示す略線図である。 測定結果とリファレンスとの比較（２）を示す略線図である。 測定結果とリファレンスとの比較（３）を示す略線図である。 測定結果とリファレンスとの比較（４）を示す略線図である。 測定結果とリファレンスとの比較（５）を示す略線図である。 カーブ時における測定結果とリファレンスとの比較（１）を示す略線図である。 カーブ時における測定結果とリファレンスとの比較（２）を示す略線図である。 カーブ時における測定結果とリファレンスとの比較（３）を示す略線図である。 地図上の行路と走行軌跡との比較を示す略線図である。 軽車両に載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。 ミニバンに載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。 第２の実施の形態によるＰＮＤの回路構成を示す略線図である。 ＧＰＳ速度と速度比との関係（１）を示す略線図である。 第２の実施の形態による速度算出部の構成を示す略線図である。 ＧＰＤ速度と速度比との関係（２）を示す略線図である。 第２の実施の形態による速度補正処理手順を示すフローチャートである。 第３の実施の形態によるＰＮＤの回路構成を示す略線図である。 円弧、速度及び角速度の関係を示す略線図である。 定数を変化させたときの速度と速度比との関係を示す略線図である。 ＧＰＳ速度と速度比との関係（３）を示す略線図である。 曲率半径の変化を示す略線図である。 速度の変化を示す略線図である。 曲率半径及び速度の変化を示す略線図である。 第３の実施の形態による速度算出部の構成を示す略線図である。 第３の実施の形態による速度補正処理手順を示すフローチャートである。 他の実施の形態による使用例を示す略線図である。

以下、発明を実施するための形態（以下実施の形態とする）について、図面を用いて説明する。なお、説明は以下の順序で行う。
１．第１の実施の形態（ナビゲーション装置・速度Ｖの補正を行わない例）
２．第２の実施の形態（ナビゲーション装置・補正係数により速度Ｖを補正する例）
３．第３の実施の形態（ナビゲーション装置・速度モデルを用いて速度Ｖを補正する例）
４．他の実施の形態

＜１．第１の実施の形態＞
［１−１．基本原理］
本発明においてはナビゲーション装置として携帯型ナビゲーション装置（以下、これをＰＮＤ（Personal Navigation Device）とも呼ぶ）を用い、そのＰＮＤにより移動体としての車両の速度及び現在位置を算出する基本原理について説明する。

［１−１−１．速度算出原理］
実際上、移動面としての道路を走行中の車両は、平らな道路を走行することが殆どなく、図１（Ａ）に示すような全体として凹状の道路、及び図１（Ｂ）に示すような全体として凸状の道路を走行するのが実状である。

ここで車両の座標系は、当該車両の前後方向をＸ軸、当該Ｘ軸に直交した水平方向をＹ軸、上下方向をＺ軸によって表される。

この車両の例えばダッシュボード上に載置されたＰＮＤは、車両が凹状の道路（図１（Ａ））を走行したとき、当該ＰＮＤに設けられた３軸加速度センサによって、Ｚ軸に沿った下方向の加速度α_zを例えば５０［Ｈｚ］のサンプリング周波数で検出する。

またＰＮＤは、当該ＰＮＤに設けられたＹ軸ジャイロセンサによって進行方向に直交したＹ軸周りの角速度（以下、これをピッチレートとも呼ぶ）ω_yを５０［Ｈｚ］のサンプリング周波数で検出する。

ここでＰＮＤでは、Ｚ軸に沿った下方向の加速度α_zを正と定義し、また図１（Ａ）に示すような凹状の路面に沿って形成される仮想上の円を進行方向に対して上向きに縦回転する際のピッチレートω_yを正と定義している。

このＰＮＤでは、３軸加速度センサによって検出した加速度α_z及びＹ軸ジャイロセンサによって検出したピッチレートω_yを用い、次式

によって進行方向の速度Ｖを１秒間当たり５０回、算出し得るようになされている。

またＰＮＤは、車両が凸状の道路（図１（Ｂ））を走行するとき、当該ＰＮＤに設けられた３軸加速度センサによってＺ軸に沿った上方向の加速度α_z’を例えば５０［Ｈｚ］のサンプリング周波数で検出し、また当該ＰＮＤに設けられたＹ軸ジャイロセンサによってＹ軸周りのピッチレートω_y’を例えば５０［Ｈｚ］のサンプリング周波数で検出する。

そしてＰＮＤでは、３軸加速度センサによって検出した加速度α_z’及びＹ軸ジャイロセンサによって検出したピッチレートω_y’を用い、次式

によって進行方向の速度Ｖ’を１秒間当たり５０回、算出し得るようになされている。

ここでは説明の便宜上、負の加速度α_zを加速度α_z’として説明しているが、実際には、３軸加速度センサは加速度α_z’を加速度α_zの負の値として検出している。またピッチレートω_y’についても同様に、負のピッチレートω_yをピッチレートω_y’として説明しているが、実際には、Ｙ軸ジャイロセンサは、ピッチレートω_y’をピッチレートω_yの負の値として検出している。従って、実際には速度Ｖ’も、速度Ｖとして算出される。

［１−１−２．現在位置算出原理］
次に、上述した速度算出原理により算出した速度Ｖと、Z軸回りの角速度とに基づいて現在位置を算出する現在位置算出原理について説明する。

図２に示すように、ＰＮＤは、車両が例えば左旋回している時のＺ軸回りの角速度（以下、これをヨーレートとも呼ぶ）ω_zを当該ＰＮＤに設けられたＺ軸ジャイロセンサによって例えば５０［Ｈｚ］のサンプリング周波数で検出する。

次にＰＮＤは、図３に示すように、前回の位置Ｐ０における速度Ｖと、ジャイロセンサにより検出したヨーレートω_zにサンプリング周期（この場合、０．０２［ｓ］）を積算することにより得られる角度θとを基に、前回の位置Ｐ０から現在位置Ｐ１までの変化量を求める。そしてＰＮＤは、この変化量を前回の位置Ｐ０に加えることによって現在位置Ｐ１を算出し得るようになされている。

［１−２．ＰＮＤの構成］
上述した本発明の基本原理を用いて車両の速度及び現在位置を算出するＰＮＤの具体的構成について説明する。

［１−２−１．ＰＮＤの外観構成］
図４に示すように、ＰＮＤ１は、当該ＰＮＤ１における前面に表示部２が設けられており、当該ＰＮＤ１に内蔵された例えば不揮発性メモリ（図示せず）に格納されている地図データに応じた地図画像等を表示部２に対して表示し得るようになされている。

またＰＮＤ１は、車両のダッシュボード上に吸盤３Ａを介して取付けられたクレードル３によって保持されると共に、当該ＰＮＤ１とクレードル３とが機械的かつ電気的に接続される。

これによりＰＮＤ１は、クレードル３を介して車両のバッテリから供給される電源電力により動作すると共に、当該クレードル３から取り外されたときには内蔵のバッテリから供給される電力によって独立した状態でも動作するようになされている。

ここでＰＮＤ１は、その表示部２が車両の進行方向に対してほぼ垂直となるように設置されている。このときＰＮＤ１の座標系は、図５に示すように、当該車両の前後方向（進行方向）をＸ軸、当該Ｘ軸に直交した水平方向をＹ軸、上下方向をＺ軸によって表される。

この座標系では、車両の進行方向をＸ軸の正と定義し、また右方向をＹ軸の正と定義し、さらに下方向をＺ軸の正と定義することとする。

［１−２−２．ＰＮＤのセンサ構成］
図６に示すように、ＰＮＤ１は、その内部に３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７が設けられている。

３軸加速度センサ４は、Ｘ軸に沿った加速度α_x、Ｙ軸に沿った加速度α_y、及びＺ軸に沿った加速度α_zをそれぞれ電圧値として検出するようになされている。

またＹ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７は、Ｙ軸周りのピッチレートω_y、Ｚ軸周りのヨーレートω_z及び周囲の気圧ＰＲをそれぞれ電圧値として検出するようになされている。

［１−２−３．ＰＮＤの回路構成］
図７に示すように、ＰＮＤ１の制御部１１は、ＣＰＵ（Central Processing Unit）構成でなり、例えば不揮発性メモリ等でなる記憶部１２から読み出した基本プログラムによって全体を統括制御するようになされている。

またＰＮＤ１は、制御部１１が記憶部１２から読み出した各種アプリケーションプログラムに従って後述する速度算出処理等を実行するようになされている。

この制御部１１は、速度算出処理等を実行する際、ＧＰＳ処理部２１、速度算出部２２、角度算出部２３、高度算出部２４、位置算出部２５及びナビゲーション部２６として機能するようになされている。

このＰＮＤ１では、ＧＰＳアンテナＡＮＴによって受信した複数のＧＰＳ衛星からのＧＰＳ信号を制御部１１のＧＰＳ処理部２１へ送出する。

ＧＰＳ処理部２１は、複数のＧＰＳ信号をそれぞれ復調することにより得られる軌道データと、複数のＧＰＳ衛星から車両までの距離データとに基づいて車両の現在位置を正確に測位することにより現在位置データＮＰＤ１を得、これをナビゲーション部２６へ送出する。

ナビゲーション部２６は、現在位置データＮＰＤ１を基に車両の現在位置が含まれる周辺の地図データを記憶部１２から読み出し、その現在位置が含まれる地図画像を生成した後、表示部２へ出力することにより当該地図画像を表示するようになされている。

ところで３軸加速度センサ４は、加速度α_x、α_y及びα_zを例えば５０［Ｈｚ］のサンプリング周波数で検出し、当該加速度α_x、α_y及びα_zのうち、加速度α_zが示された加速度データＡＤを制御部１１の速度算出部２２へ送出する。

Ｙ軸ジャイロセンサ５は、ピッチレートω_yを例えば５０［Ｈｚ］のサンプリング周波数で検出しており、当該ピッチレートω_yが示されたピッチレートデータＰＤを制御部１１の速度算出部２２へ送出する。

速度算出部２２は、３軸加速度センサ４から供給された加速度データＡＤに相当する加速度α_zと、Ｙ軸ジャイロセンサ５から供給されたピッチレートデータＰＤに相当するピッチレートω_yとを基に、（１）式を用いて１秒当たり５０回、速度Ｖを算出し、当該速度Ｖが示された速度データＶＤを位置算出部２５へ送出する。

またＺ軸ジャイロセンサ６は、ヨーレートω_zを例えば５０［Ｈｚ］のサンプリング周波数で検出しており、当該ヨーレートω_zが示されたヨーレートデータＹＤを制御部１１の角度算出部２３へ送出する。

角度算出部２３は、Ｚ軸ジャイロセンサ６から供給されたヨーレートデータＹＤに相当するヨーレートω_zにサンプリング周期（この場合、０．０２［ｓ］）を積算することにより、車両が右旋回又は左旋回したときの角度θを算出し、その角度θが示された角度データＤＤを位置算出部２５へ送出する。

位置算出部２５は、速度算出部２２から供給された速度データＶＤに相当する速度Ｖ、及び角度算出部２３から供給された角度データＤＤに相当する角度θを基に、図３に示したような前回の位置Ｐ０から現在位置Ｐ１までの変化量を求める。そして位置算出部２５は、この変化量を前回の位置Ｐ０に加えることによって現在位置Ｐ１を算出し、その現在位置Ｐ１が示された現在位置データＮＰＤ２をナビゲーション部２６へ送出する。

一方、気圧センサ７は、周囲の気圧ＰＲを例えば５０［Ｈｚ］のサンプリング周波数で検出しており、当該気圧ＰＲが示された気圧データＰＲＤを高度算出部２４へ送出する。

高度算出部２４は、気圧センサ７から供給された気圧データＰＲＤに相当する気圧ＰＲに基づいて車両の高度を算出し、その高度が示された高度データＨＤをナビゲーション部２６へ送出する。

ナビゲーション部２６は、位置算出部２５から供給された現在位置データＮＰＤ２、及び高度算出部２４から供給された高度データＨＤを基に、車両の現在位置が含まれる周辺の地図データを記憶部１２から読み出し、その現在位置が含まれる地図画像を生成した後、表示部２へ出力することにより当該地図画像を表示するようになされている。

［１−３．速度算出処理］
次に、３軸加速度センサ４から供給された加速度データＡＤに相当する加速度α_z、及びＹ軸ジャイロセンサ５から供給されたピッチレートデータＰＤに相当するピッチレートω_yに基づいて速度Ｖを速度算出部２２によって算出する速度算出処理について詳しく説明する。

速度算出部２２は、速度算出処理を実行する際、図８に示すように、データ取得部３１、ハイパスフィルタ部３２、ローパスフィルタ部３３、速度計算部３４、平滑化及びノイズ除去部３５及び速度出力部３６として機能する。

速度算出部２２のデータ取得部３１は、３軸加速度センサ４から供給される加速度データＡＤ、及びＹ軸ジャイロセンサ５から供給されるピッチレートデータＰＤをそれぞれ取得し、当該加速度データＡＤ及びピッチレートデータＰＤをハイパスフィルタ部３２へ送出する。

ハイパスフィルタ部３２は、データ取得部３１から供給された加速度データＡＤ及びピッチレートデータＰＤの直流成分をカットし、その結果得られる加速度データＡＤ１及びピッチレートデータＰＤ１をローパスフィルタ部３３へ送出する。

ローパスフィルタ部３３は、ハイパスフィルタ部３２から供給された加速度データＡＤ１及びピッチレートデータＰＤ１に対して後述するローパスフィルタ処理を施し、その結果得られる加速度データＡＤ２及びピッチレートデータＰＤ２を速度計算部３４へ送出する。

速度計算部３４は、ローパスフィルタ部３３から供給された加速度データＡＤ２及びピッチレートデータＰＤ２に対して後述する速度計算処理を施し、その結果得られる速度データＶＤ１を平滑化及びノイズ除去部３５へ送出する。

平滑化及びノイズ除去部３５は、速度計算部３４から供給された速度データＶＤ１に対して後述する平滑化及びノイズ除去処理を施し、その結果得られる速度データＶＤを速度出力部３６へ送出する。

速度出力部３６は、車両の速度Ｖを表すデータとして、平滑化及びノイズ除去部３５から供給された速度データＶＤを位置算出部２５へ送出する。

このようにして速度算出部２２は、３軸加速度センサ４から供給された加速度データＡＤ、及びＹ軸ジャイロセンサ５から供給されたピッチレートデータＰＤに基づいて車両の速度Ｖを算出するようになされている。

［１−３−１．ローパスフィルタ処理］
次に、ハイパスフィルタ部３２から供給された加速度データＡＤ１及びピッチレートデータＰＤ１に対してローパスフィルタ部３３により施されるローパスフィルタ処理について詳しく説明する。

ところで、気圧センサ７により取得された気圧データＰＲＤに相当する気圧ＰＲに基づく高度Ｈと、Ｙ軸ジャイロセンサ５により取得されたピッチレートデータＰＤに相当するピッチレートω_yに基づくＹ軸周りの水平方向に対する角度φとの関係を図９に示す。ここで角度φは、進行方向（Ｘ軸）に対して上方向を正と定義している。

図９における約１２００１データ点（２４０［ｓ］）から高度Ｈが急に低くなるとき、すなわち車両が下り坂を下っているとき、角度φも約０．５［ｄｅｇ］から急に約−２．５［ｄｅｇ］へ下がっていることからも明らかなように、高度Ｈと角度φとの間には相関関係がある。

このように高度Ｈが変化する際、角度φも高度Ｈの変化に伴って変化しており、このことから、ＰＮＤ１は、Ｙ軸ジャイロセンサ５によって車両の進行方向における路面のうねりを検出できることが分かる。

次に、図９における角度φだけを図１０（Ａ）に示す。また図１０（Ｂ）には、図１０（Ａ）における５００１データ点から６００１データ点までの角度φを示し、このとき車両は速度２０［ｋｍ／ｈ］未満の低速で走行している。図１０（Ｂ）からも明らかなように、角度φは、１秒間当たり１〜２回振動していることが分かる。

従って、車両に搭載されたＰＮＤ１では、車両が速度２０［ｋｍ／ｈ］未満の低速で走行している際、Ｙ軸ジャイロセンサ５により取得されたピッチレートデータＰＤに相当するピッチレートω_yに基づく角度φを１〜２［Ｈｚ］の振動として検出している。

また図１１（Ａ）には、図１０（Ａ）と同様に、図９における角度φだけを示す。図１１（Ｂ）には、図１１（Ａ）における２２００１データ点から２３００１データ点までの角度φを示し、このとき車両は速度６０［ｋｍ／ｈ］以上の高速で走行している。

これによると、ＰＮＤ１では、車両が速度６０［ｋｍ／ｈ］以上の高速で走行している際、Ｙ軸ジャイロセンサ５により取得されたピッチレートデータＰＤに相当するピッチレートω_yに基づく角度φも１〜２［Ｈｚ］の振動として検出している。

さらにＰＮＤ１では、図１２に示すように、車両が速度１０［ｋｍ／ｈ］未満の超低速で走行している際のＹ軸ジャイロセンサ５により取得されたピッチレートデータＰＤに相当するピッチレートω_yに基づく角度φも１〜２［Ｈｚ］の振動として検出している。

従ってＰＮＤ１では、Ｙ軸ジャイロセンサ５によりピッチレートω_yを検出する際、車両の走行速度に関わらず、当該ピッチレートω_yを１〜２［Ｈｚ］の振動として検出している。

ところでＰＮＤ１は、車両のダッシュボード上に吸盤３Ａを介して取付けられたクレードル３によって保持されている。図１３に示すように、クレードル３は、吸盤３Ａの上方に本体部３Ｂが設けられており、当該本体部３Ｂの所定高さの位置に設けられた支持点３Ｃによって一端が支持され、他端によりＰＮＤ１を支持するＰＮＤ支持部３Ｄが設けられている。

従ってＰＮＤ１は、車両が路面のうねりに応じて振動する際、ＰＮＤ支持部３Ｄの支持点３Ｃを中心に上下方向に例えば加速度α_c及び角速度ω_cで振動する。

従って、実際上、３軸加速度センサ４は、車両が路面のうねりに応じて振動することにより発生するＺ軸方向の加速度α_z（図１）に対して、ＰＮＤ支持部３Ｄの支持点３Ｃを中心とした振動に伴う加速度α_cが加算された加速度（以下、これを加算加速度と呼ぶ）α_czを検出していることになる。

またＹ軸ジャイロセンサ５は、車両が路面のうねりに応じて振動することにより発生するＹ軸周りのピッチレートω_y（図１）に対して、ＰＮＤ支持部３Ｄの支持点３Ｃを中心とした振動に伴う角速度ω_cが加算された角速度（以下、これを加算角速度と呼ぶ）ω_cyを検出していることになる。

従ってローパスフィルタ部３３は、加算加速度α_czが示された加速度データＡＤ１と、加算角速度ω_cyが示されたピッチレートデータＰＤ１とをデータ取得部３１及びハイパスフィルタ部３２を介して取得することになる。

ここでハイパスフィルタ部３２によってハイパスフィルタ処理が施された後の加速度データＡＤ１に相当する加算加速度α_cz及びピッチレートデータＰＤ１に相当する加算角速度ω_cyを図１４に示す。そして図１５には、図１４に示した加算角速度ω_cyを４０９６データ点ごとにフーリエ変換したグラフを示す。

具体的に図１５（Ａ）は、図１４における１〜４０９６データ点までの加算角速度ω_cyに対してフーリエ変換したグラフである。以下同様に、図１５（Ｂ）、（Ｃ）及び（Ｄ）は、図１４における４０９７〜８１９２データ点、８１９３〜１２２８８データ点及び１２２８９〜１６３８４データ点までの加算角速度ω_cyに対してそれぞれフーリエ変換したグラフである。

また図１５（Ｅ）、（Ｆ）、（Ｇ）及び（Ｈ）は、図１４における１６３８５〜２０４８０データ点、２０４８１〜２４５７６データ点、２４５７７〜２８６７２データ点及び２８６７３〜３２７６８データ点までの加算角速度ω_cyに対してそれぞれフーリエ変換したグラフである。

図１５（Ａ）〜（Ｈ）のうち、特に図１５（Ｃ）〜（Ｈ）に顕著に表れているように、１〜２［Ｈｚ］の周波数成分と、約１５［Ｈｚ］の周波数成分とが、大きな値を示している。

すなわちＰＮＤ１は、Y軸ジャイロセンサ５によって、上述したような路面のうねりによって１〜２［Ｈｚ］で振動するピッチレートω_yと、ＰＮＤ１を保持するクレードル３によって約１５［Ｈｚ］で振動する角速度ω_cと合成された加算角速度ω_cyを検出している。

一方、図１６には、図１４に示した加算加速度α_czを４０９６データ点ごとにフーリエ変換したグラフを示す。

具体的に図１６（Ａ）は、図１４における１〜４０９６データ点までの加算加速度α_ｃｚに対してフーリエ変換したグラフである。以下同様に、図１６（Ｂ）、（Ｃ）及び（Ｄ）は、図１４における４０９７〜８１９２データ点、８１９３〜１２２８８データ点及び１２２８９〜１６３８４データ点までの加算加速度α_czに対してそれぞれフーリエ変換したグラフである。

また図１６（Ｅ）、（Ｆ）、（Ｇ）及び（Ｈ）は、図１４における１６３８５〜２０４８０データ点、２０４８１〜２４５７６データ点、２４５７７〜２８６７２データ点及び２８６７３〜３２７６８データ点までの加算加速度α_czに対してそれぞれフーリエ変換したグラフである。

ここで加算角速度ω_cy（図１５（Ｃ）〜（Ｈ））に１〜２［Ｈｚ］の周波数成分と約１５［Ｈｚ］の周波数成分とが発生している以上、加算加速度α_czにも１〜２［Ｈｚ］の周波数成分と、約１５［Ｈｚ］の周波数成分とが発生していることが予想される。

すなわちＰＮＤ１は、３軸加速度センサ４によって、上述したような路面のうねりによって１〜２［Ｈｚ］で振動する加速度α_zと、ＰＮＤ１を保持するクレードル３によって約１５［Ｈｚ］で振動する加速度α_cとが合成された加算加速度α_czを検出している。

そこでローパスフィルタ部３３は、ハイパスフィルタ部３２から供給された加速度データＡＤ１及びピッチレートデータＰＤ１に対してローパスフィルタ処理を施し、約１５［Ｈｚ］の周波数成分、すなわちクレードル３にＰＮＤ１が保持されることによって発生する加速度α_c及び角速度ω_cをそれぞれ取り除くようになされている。

ここで図１６（Ｈ）の縦軸を対数軸に変換したグラフを図１７（Ａ）に示し、２８６７３〜３２７６８データ点までの加算加速度α_czに対してカットオフ周波数２［Ｈｚ］のＩＩＲ（Infinite Impulse Response）フィルタを２回、４回及び６回施した後にフーリエ変換したグラフをそれぞれ図１７（Ｂ）、（Ｃ）及び（Ｄ）に示す。

また図１８（Ａ）に図１５（Ｈ）の縦軸を対数軸に変換したグラフを示し、２８６７３〜３２７６８データ点までの加算角速度ω_cyに対して、加算加速度α_czと同様に、カットオフ周波数２［Ｈｚ］のＩＩＲフィルタを２回、４回及び６回施した後にフーリエ変換したグラフをそれぞれ図１８（Ｂ）、（Ｃ）及び（Ｄ）に示す。

図１７（Ｂ）〜（Ｄ）及び図１８（Ｂ）〜（Ｄ）に示したように、このＰＮＤ１では、ハイパスフィルタ部３２から供給された加速度データＡＤ１及びピッチレートデータＰＤ１に対して、カットオフ周波数２［Ｈｚ］のＩＩＲフィルタを４回以上施すことにより、約１５［Ｈｚ］の周波数成分を取り除くことができる。

従って本実施の形態によるローパスフィルタ部３３は、ハイパスフィルタ部３２から供給された加速度データＡＤ１及びピッチレートデータＰＤ１に対して、カットオフ周波数２［Ｈｚ］のＩＩＲフィルタを４回施し、その結果得られる加速度データＡＤ２及びピッチレートデータＰＤ２を速度計算部３４へ送出する。

従ってローパスフィルタ部３３は、加算加速度α_czからクレードル３におけるＰＮＤ支持部３Ｄの支持点３Ｃを中心とした振動に伴う加速度α_cを取り除くことによって、路面のうねりによって発生する加速度α_zだけを抽出することができる。

また従ってローパスフィルタ部３３は、加算角速度ω_cyからクレードル３におけるＰＮＤ支持部３Ｄの支持点３Ｃを中心とした振動に伴う角速度ω_cを取り除くことによって、路面のうねりによって発生するピッチレートω_yだけを抽出することができる。

［１−３−２．速度計算処理］
次に、ローパスフィルタ部３３から供給された加速度データＡＤ２及びピッチレートデータＰＤ２を基に、速度計算部３４によって速度Ｖを算出する速度計算処理について詳しく説明する。

まず始めに、車両の前方側であるダッシュボードの上と、当該車両の後方側であるリアガラス付近にそれぞれＰＮＤ１が載置された状態で、当該車両が速度２０［ｋｍ／ｈ］未満の低速、速度６０［ｋｍ／ｈ］未満の中速、及び速度６０［ｋｍ／ｈ］以上の高速で走行した際の前方側及び後方側の加速度データＡＤ２に相当する加速度α_zをそれぞれ図１９、図２０（Ａ）及び（Ｂ）に示す。

ここで、図１９、図２０（Ａ）及び（Ｂ）においては、前方側に載置されたＰＮＤ１で検出された加速度α_zをフロント加速度と呼び、後方側に載置されたＰＮＤ１で検出された加速度α_zをリア加速度と呼ぶ。

図１９、図２０（Ａ）及び（Ｂ）からも明らかなように、車両の走行速度に関わらず、フロント加速度に対してリア加速度の位相が遅れていることが分かる。この位相遅れは、車両の前輪軸と後輪軸との距離であるホイールベースを走行速度により除算した値とほぼ等しい。

次に、図２１（Ａ）〜（Ｃ）には、車両のダッシュボード上（前輪軸からホイールベールの３０％に相当）、中央及び後輪軸上にそれぞれＰＮＤ１を載置した際の加速度データＡＤ２に相当する加速度α_zとピッチレートデータＰＤ２に相当するピッチレートω_yとの関係を表すシミュレーション結果の一例を示す。また図２１（Ｄ）〜（Ｆ）には、図２１（Ａ）〜（Ｃ）により示すシミュレーション結果により得られた加速度α_z及びピッチレートω_yとに基づいて、（１）式に従って速度Ｖを算出した結果を示す。

ここで、このシミュレーションでは、振幅０．１［ｍ］及び波長２０［ｍ］の正弦波でうねる路面上を、ホイールベールが２．５［ｍ］でなる車両が速度５［ｍ／ｓ］で走行する場合を仮定した。

図２１（Ａ）〜（Ｃ）からも明らかなように、この加速度α_zは、車両におけるＰＮＤ１の搭載位置が後方へ移動するに連れて位相が遅れる。一方、ピッチレートω_yは、車両におけるＰＮＤ１の搭載位置に関わらず、位相のずれを生じることはない。

従って図２１（Ｂ）に示したように、ＰＮＤ１を車両の中央に搭載した場合、加速度α_zとピッチレートω_yとの位相のずれがほとんどなく、そのため図２１（Ｅ）に示したように、（１）式を用いて算出した速度Ｖは、ほぼ一定となる。

しかしながら、図２１（Ａ）及び（Ｃ）に示したように、ＰＮＤ１を搭載した位置が車両の中央に対して前後に移動すると、加速度α_zとピッチレートω_yとの位相のずれが大きくなる。そのため図２１（Ｄ）及び（Ｆ）に示したように、（１）式を用いて算出した速度Ｖは、加速度α_zとピッチレートω_yとの位相のずれにより、ＰＮＤ１を車両の中央に搭載した場合（図２１（Ｅ））の速度Ｖと比して、誤差が大きくなる。

特に、車両の速度Ｖが速度２０［ｋｍ／ｈ］未満の低速時に、加速度α_zとピッチレートω_yとの位相のずれが大きくなるので、速度Ｖの算出誤差が大きくなる。

そこで速度計算部３４は、図２２に示すように、ローパスフィルタ部３３から供給された加速度データＡＤ２に相当する加速度α_zの前回の位置Ｐ０（図３）に対応するデータ点Ｐｍを中心とした２５データ点又は７５データ点分の範囲から、最大値及び最小値をそれぞれ最大加速度α_z,max及び最小加速度α_z,minとして抽出する。

また速度計算部３４は、ローパスフィルタ部３３から供給されたピッチレートデータＰＤ２に相当するピッチレートω_yのデータ点Ｐｍを中心とした２５データ点又は７５データ点分の範囲から、最大値及び最小値をそれぞれ最大ピッチレートω_y,max及び最小ピッチレートω_y,minとして抽出する。

すなわち速度計算部３４は、加速度α_z及びピッチレートω_yに発生し得る位相のずれよりも広い範囲のなかから、最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minとをそれぞれ抽出する。

そして速度計算部３４は、加速度データＡＤ２から抽出した最大加速度α_z,max及び最小加速度α_z,minと、ピッチレートデータＰＤ２から抽出した最大ピッチレートω_y,max及び最小ピッチレートω_y,minとを用い、上述した（１）式を変形した

によって前回の位置Ｐ０（図３）での進行方向の速度Ｖを算出し、その結果得られる速度データＶＤ１を平滑化及びノイズ除去部３５へ送出する。

すなわち速度計算部３４は、（３）式を用いることにより、加速度α_z及びピッチレートω_yに位相のずれが発生している場合であっても、当該位相のずれの影響を取り除いた速度Ｖを算出することができる。

ところで図２３に示すように、速度計算部３４は、前回の位置Ｐ０での進行方向の速度Ｖを算出する際、加速中であれば、前々回の位置（図示せず）の速度（以下、これを前値速度とも呼ぶ）Ｖ_n-1が速度０［ｋｍ／ｈ］から速度３５［ｋｍ／ｈ］までのとき２５データ点分の範囲を用い、前値速度Ｖ_n-1が速度３５［ｋｍ／ｈ］を超えると、７５データ点分の範囲を用いるようにする。

また速度計算部３４は、前回の位置Ｐ０での進行方向の速度Ｖを算出する際、減速中であれば、前値速度Ｖ_n-1が速度２５［ｋｍ／ｈ］以上のとき７５データ点分の範囲を用い、前値速度Ｖ_n-1が速度２５［ｋｍ／ｈ］未満になると、２５データ点分の範囲を用いるようにする。

従って速度計算部３４は、速度Ｖに応じて、最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minとを抽出する際、データ範囲を２５データ点又は７５データ点に切り替える。

このとき速度計算部３４は、車両の速度Ｖが例えば速度２５［ｋｍ／ｈ］以下の低速である場合には路面の微妙な変化により急激に加速度α_z及びピッチレートω_yが変化するので、その急激な変化に対応するためにデータ範囲を狭く設定する。

また速度計算部３４は、車両の速度Ｖが速度３５［ｋｍ／ｈ］以上である場合には車両のサスペンションの影響も大きく、加速度α_z及びピッチレートω_yがゆっくり変化するので、そのゆっくりとした変化に対応するためにデータ範囲を広く設定する。

このように速度計算部３４は、最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minと抽出する際のデータ範囲を車両の速度Ｖに応じて切り替えることにより、当該速度Ｖに応じた路面や車両の状況を反映することができ、速度Ｖの算出精度を向上させることができる。

また速度計算部３４は、最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minとを抽出する際、加速時と減速時とでデータ範囲を変更するようなヒステリシス性を持たすようになされている。

これにより速度計算部３４は、速度Ｖを算出する際のデータ範囲にヒステリシス性を持たせなかった場合に生じるデータ範囲の切り替え速度付近での頻繁なデータ範囲の切り替えを行う必要がなくなる。この結果速度計算部３４は、このような頻繁な切り替えにより生じる速度Ｖの算出誤差を無くすことができ、その分、速度Ｖの算出精度をより向上させることができる。

［１−３−３．平滑化及びノイズ除去処理］
次に、速度計算部３４により算出された速度データＶＤ１に対して、平滑化及びノイズ除去部３５により施される平滑化及びノイズ除去処理について詳しく説明する。

まず始めに、平滑化及びノイズ除去部３５は、速度計算部３４から供給された速度データＶＤ１に対して、カットオフ周波数を可変にした１次ＩＩＲのローパスフィルタ処理を施すようになされている。

具体的に、平滑化及びノイズ除去部３５は、前回の位置Ｐ０での進行方向の速度Ｖを算出する際、前値速度Ｖ_n-1に基づいてカットオフ周波数を決定する。

ここで、ＰＮＤ１では、車両の走行速度が例えば速度６０［ｋｍ／ｈ］以上の高速時、速度計算部３４により算出された速度Ｖにノイズが大きく含まれており、当該速度Ｖのばらつきが大きくなる。そこで平滑化及びノイズ除去部３５は、前値速度Ｖ_n-1が速度６０［ｋｍ／ｈ］以上であった場合、カットオフ周波数を小さく設定したローパスフィルタを用いる。

これに対して平滑化及びノイズ除去部３５は、前値速度Ｖ_n-1が速度６０［ｋｍ／ｈ］未満であった場合、カットオフ周波数を大きく設定したローパスフィルタを用いる。

ところで、速度計算部３４により算出された速度Ｖが例えば速度１０［ｋｍ／ｈ］未満の超低速であった場合、例えば（１）式又は（３）式の分母の値であるピッチレートω_yが小さくなり、その結果、（１）式又は（３）式を用いて算出される速度Ｖが実際値より非常に大きくなってしまうことが考えられる。

そこで平滑化及びノイズ除去部３５は、ローパスフィルタ部３３からローパスフィルタ処理が施された加速度データＡＤ２及びピッチレートデータＰＤ２を取得し、当該ピッチレートデータＰＤ２に相当するピッチレートω_yが所定の閾値未満であった場合、速度Ｖが過大であると判断し、ローパスフィルタ処理を施した後の速度Ｖを０とする。

一方、図２４（Ａ）に示すように、ＰＮＤ１は、路面のうねりの円弧Ｂ１が車両のホイールベースＷより大きい場合、上述したような基本原理を用いて正確に速度Ｖを算出することができる。

しかしながら図２４（Ｂ）に示すように、例えば路面のうねりの円弧Ｂ２が車両のホイールベースＷより小さい場合、車両の前輪がうねりを乗り越える際、車両に対して垂直方向の加速度α_b及び車両の後輪を中心としたＹ軸周りの角速度ω_bが発生する。

このときＰＮＤ１は、路面のうねりに応じた１〜２［Ｈｚ］の振動により発生する加速度α_z及びピッチレートω_y（図２４（Ａ））を検出することなく、加速度α_b及び角速度ω_b（図２４（Ｂ））を３軸加速度センサ４及びＹ軸ジャイロセンサ５によって検出することになる。

ここで加速度α_bは、路面のうねりの円弧Ｂ１が車両のホイールベースＷより大きい場合に発生する加速度α_zよりも大きな値を取り、また角速度ω_bも路面のうねりの円弧Ｂ１が車両のホイールベースＷより大きい場合に発生するピッチレートω_yよりも大きな値を取る。

また、路面のうねりの円弧Ｂ２が車両のホイールベースＷより小さい場合に発生する加速度α_b及び角速度ω_bを基に（１）式又は（３）式を用いて算出した速度（以下、これを小円弧速度とも呼ぶ。）を速度Ｖ_bとする。

上述した加速度α_bが角速度ω_bより大きく変化することから、速度Ｖ_bは、路面のうねりの円弧Ｂ１が車両のホイールベースＷより大きい場合に発生する加速度α_z及びピッチレートω_yを基に（１）式又は（３）式を用いて算出した速度Ｖよりも、非常に大きな値を取る。

このためＰＮＤ１の速度算出部１１は、路面のうねりの円弧Ｂ２が車両のホイールベースＷより小さい場合、加速度α_b及び角速度ω_bを用いた小円弧速度Ｖ_bを算出することにより、速度Ｖを過大な値として算出してしまうことになる。

そこで平滑化及びノイズ除去部３５は、ローパスフィルタ部３３からローパスフィルタ処理が施された加速度データＡＤ２及びピッチレートデータＰＤ２を取得し、当該加速度データＡＤ２に相当する加速度α_z及びピッチレートデータＰＤ２に相当するピッチレートω_yが所定の閾値より大きいか否かを判断する。

そして平滑化及びノイズ除去部３５は、加速度データＡＤ２に相当する加速度α_z及びピッチレートデータＰＤ２に相当するピッチレートω_yが所定の閾値より大きい場合、速度Ｖが過大であると判断し、ローパスフィルタ処理を施した後の速度Ｖを用いるのではなく前値速度Ｖ_n-1を用いるようにする。すなわち、平滑化及びノイズ除去部３５は、超低速時以外で速度Ｖが過大な値をとったとき、速度Ｖが誤っている可能性が高いので前値速度Ｖ_n-1を用いるようになされている。

このように平滑化及びノイズ除去部３５は、ローパスフィルタ処理を施した後の速度Ｖが過大な値であった場合、超低速時であったときは速度Ｖを０とし、それ以外のときは前値速度Ｖ_n-1を速度Ｖとすることにより、当該速度Ｖを一段と正確に算出することができる。

［１−４．速度算出処理を用いた位置算出処理手順］
次に、ＰＮＤ１の制御部１１が、上述したような速度算出処理を用いて現在位置を算出する位置算出処理手順について、図２５のフローチャートを用いて説明する。

実際上、制御部１１は、ルーチンＲＴ１の開始ステップから入ってステップＳＰ１へ移り、３軸加速度センサ４により検出された加速度データＡＤと、Ｙ軸ジャイロセンサ５により検出されたピッチレートデータＰＤとを速度算出処理部２２のデータ取得部３１によって取得した後、次のステップＳＰ２へ移る。

ステップＳＰ２において制御部１１は、加速度データＡＤ及びピッチレートデータＰＤに対してハイパスフィルタ処理を速度算出部２２のハイパスフィルタ部３２により施し、次のステップＳＰ３へ移る。

ステップＳＰ３において制御部１１は、ハイパスフィルタ処理が施された加速度データＡＤ１及びピッチレートデータＰＤ１に対して、例えばカットオフ周波数１［Ｈｚ］の４次ＩＩＲフィルタであるローパスフィルタ処理を速度算出部２２のローパスフィルタ部３３によって施し、次のステップＳＰ４へ移る。

ステップＳＰ４において制御部１１は、ローパスフィルタ処理が施された加速度データＡＤ２に相当する加速度α_z及びピッチレートデータＰＤ２に相当するピッチレートω_yを基に、（３）式を用いて速度算出部２２の速度計算部３４によって速度Ｖを算出し、次のステップＳＰ５へ移る。

ステップＳＰ５において制御部１１は、ステップＳＰ４において算出された速度Ｖが示された速度データＶＤに対して平滑化及びノイズ除去処理を施す。

具体的に、制御部１１は、ステップＳＰ４において算出された速度Ｖが示された速度データＶＤ１に対してカットオフ周波数を可変にしたローパスフィルタ処理を施す。

そして制御部１１は、ローパスフィルタ処理を施した後の速度Ｖが過大な値であると判断した場合、例えば速度１０［ｋｍ／ｈ］未満の超低速時であったときは速度Ｖを０とし、それ以外のときは前値速度Ｖ_n-1を速度Ｖとし、次のステップＳＰ６へ移る。

ステップＳＰ６において制御部１１は、Ｚ軸ジャイロセンサ６により検出されたヨーレートデータＹＤを角度算出部２３によって取得し、次のステップＳＰ７へ移る。

ステップＳＰ７において制御部１１は、ヨーレートデータＹＤに相当するヨーレートω_zにサンプリング周期である０．０２［秒］を積算することにより角度θが示された角度データＤＤを角度算出部２３によって算出し、次のステップＳＰ８へ移る。

ステップＳＰ８において制御部１１は、ステップＳＰ５において平滑化及びノイズ除去処理を施された速度データＶＤ、及びステップＳＰ７において算出された角度データＤＤに基づいて現在位置データＮＰＤ２を算出し、次のステップＳＰ９へ移る。

ステップＳＰ９において制御部１１は、位置算出部２５から供給された現在位置データＮＰＤ２を基に、車両の現在位置が含まれる周辺の地図データを記憶部１２から読み出し、その現在位置が含まれる地図画像を生成した後、表示部２へ出力し、次のステップＳＰ１０へ移って処理を終了する。

［１−５．測定結果］
上述した速度算出処理によって算出された測定結果を図２６〜図３７に示す。なお図２６〜図３５は、セダンタイプの乗用車に載置されたＰＮＤ１による測定結果を示し、図３６及び図３７では、それぞれ軽自動車及びミニバンタイプの車両に載置されたＰＮＤ１による測定結果を示す。

図２６（Ａ）には、３軸加速度センサ４及びＹ軸ジャイロセンサ５により検出された加速度データＡＤ及びピッチレートデータＰＤに相当する加速度α_z及びピッチレートω_yを示し、図２６（Ｂ）には、当該加速度α_z及びピッチレートω_yを用いて（３）式によって算出された速度Ｖを示す。

図２６（Ａ）及び（Ｂ）からも明らかなように、ＰＮＤ１では、車両の速度Ｖが大きくなるに連れて加速度α_zが大きくなる一方、ピッチレートω_yはほぼ一定の値をとることが分かる。

次に、ＰＮＤ１により速度算出処理を行うことによって算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄのグラフと、当該速度Ｖ及び距離Ｄの比較のためにＰＮＤ１が搭載された車両の車速パルスから算出した速度Ｖ_ref及び当該速度Ｖ_refを用いて算出された距離Ｄ_refのグラフとを図２７〜図３１に示す。因みに図２７〜図３１は、ＰＮＤ１を搭載した車両がそれぞれ異なる道路を走行した場合でのグラフを示すものである。

なお、ここでは、車両の車速パルスから算出された速度をリファレンス速度とも呼び、またリファレンス速度を用いて算出された距離をリファレンス距離とも呼ぶ。

図２７（Ａ）は、本発明の速度算出処理を用いて算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄを示し、図２７（Ｂ）は、当該図２７（Ａ）に示された速度Ｖ及び距離Ｄと比較するためのリファレンス速度Ｖ_ref及びリファレンス距離Ｄ_refを示す。

図２７（Ａ）及び（Ｂ）に示したように、速度Ｖはリファレンス速度Ｖ_refとほぼ相似関係にあり、当該速度Ｖに基づいて算出される距離Ｄもリファレンス距離Ｄ_refに対して１０％未満の誤差しか生じていない。

また図２８（Ａ）は、本発明の速度算出処理を用いて算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄを示し、図２８（Ｂ）は、当該図２８（Ａ）に示された速度Ｖ及び距離Ｄと比較するためのリファレンス速度Ｖ_ref及びリファレンス距離Ｄ_refを示す。

さらに図２９（Ａ）は、本発明の速度算出処理を用いて算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄを示し、図２９（Ｂ）は、当該図２９（Ａ）に示された速度Ｖ及び距離Ｄと比較するためのリファレンス速度Ｖ_ref及びリファレンス距離Ｄ_refを示す。

さらに図３０（Ａ）は、本発明の速度算出処理を用いて算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄを示し、図３０（Ｂ）は、当該図３０（Ａ）に示された速度Ｖ及び距離Ｄと比較するためのリファレンス速度Ｖ_ref及びリファレンス距離Ｄ_refを示す。

さらに図３１（Ａ）は、本発明の速度算出処理を用いて算出された速度Ｖ及び当該速度Ｖを用いて算出された距離Ｄを示し、図３１（Ｂ）は、当該図３１（Ａ）に示された速度Ｖ及び距離Ｄと比較するためのリファレンス速度Ｖ_ref及びリファレンス距離Ｄ_refを示す。

図２７（Ａ）〜図３１（Ａ）に示した速度Ｖは、車両が異なる道路を走行した場合においても、図２６（Ａ）に示した速度Ｖと同様に、図２７（Ｂ）〜図３１（Ｂ）に示したリファレンス速度Ｖ_refとほぼ相似関係にあり、当該速度Ｖに基づいて算出される距離Ｄもリファレンス距離Ｄ_refに対して１０％未満の誤差しか生じていない。

次に、図３２（Ａ）には、ＰＮＤ１によって速度算出処理を用いて算出された速度Ｖ及び距離Ｄのグラフを示し、図３２（Ｂ）には、リファレンス速度Ｖ_ref及びリファレンス速度Ｖ_refから算出されたリファレンス距離Ｄ_refのグラフを示す。さらに図３２（Ｃ）には、ＰＮＤ１のＺ軸ジャイロセンサ６によって検出されたヨーレートω_zのグラフを示す。

ここで図３２（Ｃ）に示したヨーレートω_zは、その値が約２０［ｄｅｇ／ｓ］を超えるとき車両が右折したことを表しており、またその値が約−２０［ｄｅｇ／ｓ］を下回ったとき左折したことを表している。

従って図３２（Ｃ）に示したように、右折及び左折を複数回連続で繰り返した場合においても、ＰＮＤ１によって算出された速度Ｖ（図３２（Ａ））は、リファレンス速度Ｖ_ref（図３２（Ｂ））とほぼ相似関係にあり、当該速度Ｖに基づいて算出される距離Ｄもリファレンス距離Ｄ_refに対して１０％未満の誤差しか生じていない。

また図３３（Ａ）には、図３２（Ａ）とは異なる道路を走行した場合でのＰＮＤ１によって速度算出処理を用いて算出された速度Ｖ及び距離Ｄのグラフを示し、図３３（Ｂ）には、リファレンス速度Ｖ_ref及びリファレンス速度Ｖ_refから算出されたリファレンス距離Ｄ_refのグラフを示す。さらに図３３（Ｃ）には、Ｚ軸ジャイロセンサ６によって検出されたヨーレートω_zのグラフを示す。

さらに図３４（Ａ）には、図３２（Ａ）及び図３３（Ａ）とは異なる道路を走行した場合でのＰＮＤ１によって速度算出処理を用いて算出された速度Ｖ及び距離Ｄのグラフを示し、図３４（Ｂ）には、リファレンス速度Ｖ_ref及びリファレンス速度Ｖ_refから算出されたリファレンス距離Ｄ_refのグラフを示す。さらに図３４（Ｃ）には、Ｚ軸ジャイロセンサ６によって検出されたヨーレートω_zのグラフを示す。

これら結果からも、車両が多数のカーブを走行しても、ＰＮＤ１によって算出された速度Ｖは、リファレンス速度Ｖ_refとほぼ相似関係にあり、当該速度Ｖに基づいて算出される距離Ｄもリファレンス距離Ｄ_refに対して１０％未満の誤差しか生じていないことが分かる。

次に、図３５（Ａ）に示す地図のスタートＳから行路Ｋに沿ってゴールＧまで車両が走行した際、当該車両に搭載されたＰＮＤ１によって算出された現在位置をプロットした走行軌跡Ｔを図３５（Ｂ）に示す。

このように走行軌跡Ｔ（図３５（Ｂ））は、車両が走行した行路Ｋ（図３５（Ａ））とほぼ同じ大きさで、かつ相似関係にあり、従ってＰＮＤ１が現在位置をほぼ正確に算出し得ることが分かる。

次に、図３６には、軽自動車に載置されたＰＮＤ１によって算出された速度Ｖ及び距離Ｄと、当該速度Ｖ及び距離Ｄの比較のためにＧＰＳアンテナＡＮＴを介して受信したＧＰＳ信号を基に算出した速度Ｖ_g及び当該速度Ｖ_gから算出された距離Ｄ_gとを重ねて示す。

なお、以下、ＧＰＳアンテナＡＮＴを介して受信したＧＰＳ信号を基に算出した速度をＧＰＳ速度とも呼び、ＧＰＳ速度から算出された距離をＧＰＳ距離とも呼ぶ。

また図３７には、ミニバンタイプの車両に載置されたＰＮＤ１によって算出された速度Ｖ及び距離Ｄと、当該速度Ｖ及び距離Ｄの比較のためにＧＰＳ信号を基に算出したＧＰＳ速度Ｖ_g及びＧＰＳ速度Ｖ_gから算出されたＧＰＳ距離Ｄ_gとを重ねて示す。

図３６及び図３７に示したように、車両の大きさ、すなわちホイールベースが異なる複数の車両において、本発明におけるＰＮＤ１によって算出された速度Ｖは、ＧＰＳ速度Ｖ_gとほぼ相似関係にあり、当該速度Ｖに基づいて算出される距離ＤもＧＰＳ距離Ｄ_gに対して１０％未満の誤差しか生じていないことが分かる。

因みに、図３６及び図３７において、ＧＰＳ速度Ｖ_gは、車両が例えばトンネル等に入ってＧＰＳ信号が受信できなかった場合、０として算出している。

［１−６．動作及び効果］
以上の構成において、ＰＮＤ１は、路面のうねりによって発生する車両の進行方向に垂直なＺ軸方向の加速度α_zを３軸加速度センサ４により検出し、路面のうねりによって発生する当該進行方向と直交したＹ軸周りのピッチレートω_yをＹ軸ジャイロセンサ５により検出する。

そしてＰＮＤ１は、３軸加速度センサ４によって検出された加速度α_z及びＹ軸ジャイロセンサ５によって検出されたピッチレートω_yを基に、（１）式又は（３）式に従って速度Ｖを算出するようにした。

従ってＰＮＤ１は、３軸加速度センサ４及びＹ軸ジャイロセンサ５だけを用いた簡易な構成により、ＧＰＳ信号が受信することができない場合であっても、全ての道路環境下で車両の速度Ｖを正確に算出することができる。

またＰＮＤ１は、車両と着脱自在な構成において、当該車両から車速パルス信号を転送するためのケーブルをわざわざユーザに接続させるという煩雑な操作を強いることない分、使い勝手を向上することができる。

またＰＮＤ１は、車両の進行方向に垂直なＺ軸周りのヨーレートω_zをＺ軸ジャイロセンサ６によって検出し、速度Ｖ及び当該ヨーレートω_zに基づいて現在位置を算出するようにした。

これによりＰＮＤ１は、ＧＰＳ信号が受信することができない場合であっても、３軸加速度センサ４、Ｙ軸ジャイロセンサ５及びＺ軸ジャイロセンサ６を設けるだけの簡易な構成により、全ての道路環境下で車両の現在位置を正確に算出することができる。

さらにＰＮＤ１は、速度Ｖを算出する際、加速度データＡＤ１及びピッチレートデータＰＤ１に対してローパスフィルタ処理を施すようにした。従ってＰＮＤ１は、路面のうねりによって発生する１〜２［Ｈｚ］で振動する加速度α_z及びピッチレートω_yに対して十分に大きな周波数でなるクレードル３によって発生する例えば約１５［Ｈｚ］で振動する加速度α_c及び角速度ω_cの成分を除去することができる。

これによりＰＮＤ１は、クレードル３によって発生する振動成分が取り除かれた加速度α_z及びピッチレートω_yを用いて、一段と正確な速度Ｖを算出することができる。

またＰＮＤ１は、加速度α_zのデータ点Ｐ_ｍを中心とした２５データ点又は７５データ点分の範囲から、最大加速度α_z,max及び最小加速度α_z,minを抽出し、ピッチレートω_yのデータ点Ｐ_ｍを中心とした２５データ点又は７５データ点分の範囲から、最大ピッチレートω_y,max及び最小ピッチレートω_y,minを抽出する。

そしてＰＮＤ１は、最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minとを用いて（３）式により速度Ｖを算出する。

これによりＰＮＤ１は、車両内におけるＰＮＤ１の搭載位置により変化する加速度α_zとピッチレートω_yとの位相のずれよりも広い範囲のデータ点を用いることになり、上述した加速度α_zとピッチレートω_yとの位相のずれの影響を除去することができる。

またＰＮＤ１は、加速度α_zとピッチレートω_yを基に（３）式によって算出された速度Ｖが過大な値であった場合、超低速時であったときは速度Ｖを０とし、それ以外のときは速度Ｖを前値速度Ｖ_n-1とすることにより、速度Ｖをより正確に算出することができる。

以上の構成によれば、第１の実施の形態によるＰＮＤ１は、路面のうねりによって発生するＺ軸方向の加速度α_z、及び路面のうねりによって発生するＹ軸周りのピッチレートω_yを検出し、当該加速度α_z及びピッチレートω_yを用いて速度Ｖを算出することにより、全ての道路環境下で速度Ｖを正確に算出することができる。

＜２．第２の実施の形態＞
［２−１．ＰＮＤの構成］
第２の実施の形態によるＰＮＤ５０（図４〜図７）は、第１の実施の形態によるＰＮＤ１と比較して、制御部１１に代わる制御部５１を有している点において相違するものの、他の点については同様に構成されている。

制御部５１は、図７との対応部分に同一符号を付した図３８に示すように、速度算出部２２に代わる速度算出部５２を有する点と、ＧＰＳ処理部２１からのＧＰＳ速度ＶＧ及び角度算出部２３からの角度データＤＤをそれぞれ速度算出部５２へ供給する点とにおいて相違するものの、他の点については制御部１１（図７）と同様に構成されている。

［２−２．速度算出処理］
［２−２−１．速度の補正原理］
ところで第１の実施の形態によるＰＮＤ１では、速度算出部２２（図８）により算出する速度Ｖ（速度データＶＤ）について、車両の真の速度と必ずしも一致しない可能性、すなわち誤差を含む可能性がある。この誤差は、例えばクレードル３の車両に対する取付が不完全な場合に車両に生じる加速度や角速度等をＰＮＤ１が正しく検出できないこと等により生じ得る。

一方、ＰＮＤ１は、複数のＧＰＳ信号を受信でき現在位置データＮＰＤ１を生成できる場合、当該現在位置データＮＰＤ１を基に速度（以下これをＧＰＳ速度ＶＧと呼ぶ）を算出することができる。このＧＰＳ速度ＶＧは、高精度な測位データを基に算出されるものであるため、車両の真の速度と見なすことができる。

そこで、速度算出部２２により生成された速度Ｖに対するＧＰＳ速度ＶＧの比率（以下これを速度比ＲＶと呼ぶ）を、ＧＰＳ速度ＶＧごとにプロットしたところ、図３９のようなグラフが得られた。

速度算出部２２により算出した速度Ｖは、理想的にはＧＰＳ速度ＶＧと常に一致することが望ましい。すなわち速度比ＲＶは、理想的には、図中直線Ｌ０として示したように、ＧＰＳ速度ＶＧに関わらず常に値「１」となることが望ましい。

しかしながら実際の速度比ＲＶは、図３９に示したように、約１０［ｍ／ｓ］以下の比較的低速の領域では直線Ｌ０から大きく離れて離散的に分布している。また速度比ＲＶは、約１０［ｍ／ｓ］以上の中速から高速にかけて、直線Ｌ０に比較的近い箇所に集中して分布している。

この図３９から、速度比ＲＶは、一定の傾向が現れること、及びある程度の速度の範囲（以下これを速度域と呼ぶ）ごとに傾向が異なることがわかる。

このことから、速度Ｖについては、速度域ごとに補正係数を算出しておけば、当該補正係数を用いて補正することにより、誤差を縮小して真の速度に近づけることが可能となる。

［２−２−２．速度算出部の構成］
上述した速度の補正原理に従い、第２の実施の形態による速度算出部５２では、補正係数を用いて速度Ｖを補正するようになされている。

速度算出部５２は、図８との対応部分に同一符号を付した図４０に示すように、速度算出部２２の構成に加えて係数算出部６１が設けられ、また平滑化及びノイズ除去部３５及び速度出力部３６の間に速度補正部６３が設けられている。

係数算出部６１は、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得すると共に、平滑化及びノイズ除去部３５から速度データＶＤを取得する。続いて係数算出部６１は、ＧＰＳ速度ＶＧを取得できたときに、係数算出処理として、次の（４）式に従って当該ＧＰＳ速度ＶＧを速度Ｖで除算することにより補正係数Ｃを算出する。

この補正係数Ｃは、速度Ｖに乗じることにより、当該速度ＶをＧＰＳ速度ＶＧに近づけるよう補正し得るような係数となる。

また係数算出部６１は、速度Ｖが属するおおよその速度の範囲、すなわち速度域に応じて、補正係数Ｃを低速補正係数Ｃ１又は高速補正係数Ｃ２とするようになされている。

具体的に係数算出部６１は、前値速度Ｖ_n-1が速度０［ｋｍ／ｈ］から速度３０［ｋｍ／ｈ］までのとき補正係数Ｃを低速補正係数Ｃ１とし、前値速度Ｖ_n-1が速度３０［ｋｍ／ｈ］を超えると、補正係数Ｃを高速補正係数Ｃ２とする。

因みに係数算出部６１は、車両の加速中及び減速中には、その算出原理上、速度ＶとＧＰＳ速度ＶＧとの間に時間的なずれが生じてしまうため、補正係数Ｃを正しく算出できない。そこで係数算出部６１は、加減速中には補正係数Ｃを算出しないようになされている。これに伴い係数算出部６１は、加速中及び減速中で閾値を速度３５又は２５［ｋｍ／ｈ］のように相違させた速度計算部３４と異なり、両値の中間値である速度３０［ｋｍ／ｈ］を単一の閾値としている。

また車両の方位が変化しているとき、３軸加速度センサ４から供給される加速度データＡＤには、方位の変化に伴う加速度成分が含まれる。これに伴い、速度計算部３４により算出される速度Ｖにも方位の変化に伴う誤差が含まれる。そこで係数算出部６１は、角度算出部２３により算出される角度データＤＤを基に、車両の方位が変化中であると判断したときにも、補正係数Ｃを算出しないようになされている。

さらに係数算出部６１は、直前の低速補正係数Ｃ１及び高速補正係数Ｃ２（以下これらをそれぞれ直前低速補正係数Ｃ１_n-1及び直前高速補正係数Ｃ２_n-1とする）をそれぞれ記憶している。係数算出部６１は、新たに得られた低速補正係数Ｃ１又は高速補正係数Ｃ２（以下これらをそれぞれ最新低速補正係数Ｃ１_n及び最新高速補正係数Ｃ２_nとする）を用いて低速補正係数Ｃ１及び高速補正係数Ｃ２をそれぞれ平準化しながら更新するようになされている。

具体的に係数算出部６１は、低速補正係数Ｃ１を算出したときには次の（５）式に従い、高速補正係数Ｃ２を算出したときには次の（６）式に従って、新たな低速補正係数Ｃ１又は高速補正係数Ｃ２を算出する。

因みに（５）式及び（６）式の定数Ｍは、１よりも大きな値であり、時定数に相当する定数である。

その後係数算出部６１は、（５）式又は（６）式により更新した最新の低速補正係数Ｃ１又は高速補正係数Ｃ２をそれぞれ記憶する。

このように係数算出部６１は、ＧＰＳ速度ＶＧを取得できる間、速度Ｖに応じた補正係数Ｃを算出してその平均値を随時更新することにより、いわば補正係数Ｃの学習処理を行うようになされている。

速度補正部６３は、平滑化及びノイズ除去部３５から速度データＶＤを取得すると共にＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得し、さらに係数算出部６１から最新の補正係数Ｃ、すなわち低速補正係数Ｃ１又は高速補正係数Ｃ２を取得し得るようになされている。

速度補正部６３は、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得できる場合、速度データＶＤをそのまま速度出力部３６へ供給する。

一方速度補正部６３は、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得できない場合、そのときの速度Ｖに応じた最新の補正係数Ｃ、すなわち低速補正係数Ｃ１又は高速補正係数Ｃ２のいずれかを読み出し、次に示す（７）式に従い補正速度ＶＣを算出する。

このとき速度補正部６３は、係数算出部６１と同様、速度計算部３４においてデータ点の範囲を切り替えることに対応して、補正係数Ｃとして低速補正係数Ｃ１又は高速補正係数Ｃ２のいずれかを選択するようになされている。

すなわち速度補正部６３は、加速中であれば、前値速度Ｖ_n-1が速度０［ｋｍ／ｈ］から速度３５［ｋｍ／ｈ］までのとき低速補正係数Ｃ１を選択し、前値速度Ｖ_n-1が速度３５［ｋｍ／ｈ］を超えると、高速補正係数Ｃ２を選択する。

また速度補正部６３は、減速中であれば、前値速度Ｖ_n-1が速度２５［ｋｍ／ｈ］以上のとき高速補正係数Ｃ２を選択し、前値速度Ｖ_n-1が速度２５［ｋｍ／ｈ］未満になると低速補正係数Ｃ１を選択する。

その後速度補正部６３は、補正速度ＶＣを速度データＶＤとして速度出力部３６へ供給する。これに応じて速度出力部３６は、当該速度データＶＤを位置算出部２５（図３８）へ出力する。

このように速度算出部５２は、ＧＰＳ速度ＶＧを取得できるときには速度Ｖに応じた補正係数Ｃを随時更新する学習処理を行い、当該ＧＰＳ速度ＶＧを取得できないときには速度Ｖに応じた補正係数Ｃを用いて当該速度Ｖを補正する補正処理を行うようになされている。

ここで、補正後の速度Ｖ（すなわち補正速度ＶＣ）を用いた速度比ＲＶをＧＰＳ速度ＶＧごとにプロットしたところ、図３９と対応する図４１のようなグラフが得られた。

この図４１からわかるように、速度Ｖを補正することにより、補正前（図３９）と比較して、いずれの速度域においても速度比ＲＶが値「１」に近づいていることがわかる。すなわち、速度算出部５２において補正係数Ｃを用いて速度Ｖを補正することにより、当該速度ＶをＧＰＳ速度ＶＧに近づけることができ、当該速度Ｖの誤差を縮小し得ることが示された。

［２−２−３．速度補正処理手順］
次に、ＰＮＤ５０の制御部５１が、速度算出部５２により速度補正処理を行う際の速度補正処理手順ＲＴ２について、図４２のフローチャートを用いて説明する。

制御部５１は、ルーチンＲＴ２の開始ステップから入ってステップＳＰ２１へ移り、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得したか否かを判定する。ここで肯定結果が得られると、このことは補正係数Ｃの学習処理を行うべきであることを表しており、このとき制御部５１は次のステップＳＰ２２へ移る。

ステップＳＰ２２において制御部５１は、前値速度Ｖ_n-1及び速度Ｖ等を比較することにより、加減速中であるか否かを判定する。ここで否定結果が得られると、このことは加速中又は減速中のいずれでもないことを表しており、このとき制御部５１は次のステップＳＰ２３へ移る。

ステップＳＰ２３において制御部５１は、角度算出部２３からの角度データＤＤを基に、方位変化中であるか否かを判定する。ここで否定結果が得られると、このことは方位変化中ではなく補正係数Ｃの学習処理を行い得ることを表しており、このとき制御部５１は次のステップＳＰ２４へ移る。

ステップＳＰ２４において制御部５１は、係数算出部６１により（４）式に従い速度Ｖ及びＧＰＳ速度ＶＧを基に補正係数Ｃを算出し、次のステップＳＰ２５へ移る。

ステップＳＰ２５において制御部５１は、補正係数Ｃが所定の正常範囲内にあるか否かを判定する。ここで肯定結果が得られると、このことは当該補正係数Ｃを速度Ｖの補正に利用し得ることを表しており、このとき制御部５１は次のステップＳＰ２６へ移る。

ステップＳＰ２６において制御部５１は、速度Ｖが速度３０［ｋｍ／ｈ］以上であるか否かに応じて、補正係数Ｃを高速補正係数Ｃ２又は低速補正係数Ｃ１とする。

続いて制御部５１は、（５）式又は（６）式により高速補正係数Ｃ２又は低速補正係数Ｃ１を平準化しながら更新し、更新後の高速補正係数Ｃ２又は低速補正係数Ｃ１を最新の高速補正係数Ｃ２又は低速補正係数Ｃ１として記憶する。その後制御部５１は、ステップＳＰ２８へ移って一連の処理を終了する。

一方、ステップＳＰ２２又はステップＳＰ２３において肯定結果が得られた場合、このことはこのときの速度Ｖが補正係数Ｃの算出に適さないことを表している。このため制御部５１は、補正係数Ｃを更新することなくステップＳＰ２８へ移って一連の処理を終了する。

またステップＳＰ２５において否定結果が得られると、このことは例えばユーザによりタッチパネルが操作された際の加速度成分等が速度Ｖに含まれている等、算出した補正係数Ｃが異常値であることを表している。このため制御部５１は、算出した補正係数Ｃを用いることなくステップＳＰ２８へ移って一連の処理を終了する。

これに対し、ステップＳＰ２１において否定結果が得られると、このことはＧＰＳ衛星からのＧＰＳ信号を受信できないため速度算出部５２により算出した速度Ｖを基に車両の現在位置等を算出すべきことを表しており、このとき制御部５１は次のステップＳＰ２７へ移る。

ステップＳＰ２７において制御部５１は、速度補正部６３により、速度Ｖに応じた最新の補正係数Ｃ（すなわち低速補正係数Ｃ１又は高速補正係数Ｃ２）を係数算出部６１から取得し、（７）式に従って当該補正係数Ｃを速度Ｖに乗じることにより補正速度ＶＣを算出する。続いて制御部５１は、補正速度ＶＣを新たな速度Ｖとして速度出力部３６へ供給した後、次のステップＳＰ２８へ移って一連の処理を終了する。

［２−３．動作及び効果］
以上の構成において、第２の実施の形態によるＰＮＤ５０は、路面のうねりによって発生する車両の進行方向に垂直なＺ軸方向の加速度α_zを３軸加速度センサ４により検出し、路面のうねりによって発生する当該進行方向と直交したＹ軸周りのピッチレートω_yをＹ軸ジャイロセンサ５により検出する。

そしてＰＮＤ５０は、３軸加速度センサ４によって検出された加速度α_z及びＹ軸ジャイロセンサ５によって検出されたピッチレートω_yを基に、（１）式又は（３）式に従って速度Ｖを算出するようにした。

さらにＰＮＤ５０は、ＧＰＳ速度ＶＧを取得できるときには、係数算出部６１により（４）式に従って速度Ｖに応じた補正係数Ｃを算出し、ＧＰＳ速度ＶＧを取得できないときには、速度補正部６３により（７）式に従って補正係数Ｃを用いて速度Ｖを補正するようにした。

従ってＰＮＤ５０は、第１の実施の形態と同様、３軸加速度センサ４及びＹ軸ジャイロセンサ５だけを用いた簡易な構成により、ＧＰＳ信号が受信することができない場合であっても、全ての道路環境下で車両の速度Ｖを正確に算出することができる。

これに加えてＰＮＤ５０は、ＧＰＳ速度ＶＧが比較的正確であることを利用して適切な補正係数Ｃを算出することができるので、速度Ｖに含まれる誤差を小さく抑えることができる。

さらにＰＮＤ５０は、速度Ｖを２の速度域に分割すると共に当該速度域ごとの補正係数Ｃとして低速補正係数Ｃ１及び高速補正係数Ｃ２をそれぞれ算出するようにした。これによりＰＮＤ５０は、速度Ｖの誤差の傾向が速度域ごとに異なることに応じて、速度域ごとに適切な補正を行うことができるので、速度Ｖに含まれる誤差を極めて小さく抑えることができる。

その他の点について、第２の実施の形態によるＰＮＤ５０は、第１の実施の形態によるＰＮＤ１と同様の作用効果を奏し得る。

以上の構成によれば、ＰＮＤ５０は、路面のうねりによって発生するＺ軸方向の加速度α_z、及び路面のうねりによって発生するＹ軸周りのピッチレートω_yを検出し、当該加速度α_z及びピッチレートω_yを用いて速度Ｖを算出することにより、全ての道路環境下で速度Ｖを正確に算出することができる。さらにＰＮＤ５０は、速度Ｖに応じて、当該速度Ｖが属する速度域ごとの補正係数Ｃを用いて当該速度Ｖを補正することにより、当該速度Ｖに含まれる誤差を極めて小さく抑えることができる。

＜３．第３の実施の形態＞
［３−１．ＰＮＤの構成］
第３の実施の形態によるＰＮＤ７０（図４〜図７）は、第１の実施の形態によるＰＮＤ１と比較して、制御部１１に代わる制御部７１を有している点において相違するものの、他の点については同様に構成されている。

制御部７１は、図７との対応部分に同一符号を付した図４３に示すように、速度算出部２２に代わる速度算出部７２を有する点と、ＧＰＳ処理部２１からのＧＰＳ速度ＶＧを速度算出部７２へ供給する点とが相違するものの、他の点については制御部１１（図７）と同様に構成されている。

［３−２．速度算出処理］
ところで第３の実施の形態によるＰＮＤ７０では、第２の実施の形態によるＰＮＤ５０と同様に速度Ｖを補正するようになされているものの、その補正の手法が第２の実施の形態と相違している。

［３−２−１．速度モデルを用いた速度の補正原理］
第３の実施の形態では、加速度α_z及びピッチレートω_yを基に生成した速度ＶとＧＰＳ速度ＶＧとの関係を表す速度モデルを構築し、当該速度モデルを用いて速度Ｖの補正を行うようになされている。

まずピッチレートω_yについて、オフセット等による誤差を含んでいると仮定し、Ｙ軸ジャイロセンサ５から得られる測定値としてのピッチレートω_yを、真のピッチレートω_yＴと測定誤差Δω_yとの加算値であると見なす。

また加速度α_zについても、同様にオフセット等による誤差を含んでいると仮定し、３軸加速度センサ４から得られる測定値としての加速度α_zを、真の加速度α_zＴと測定誤差Δα_zとの加算値であると見なす。

ここで図４４（Ａ）に示すように、車両が曲率半径Ｒ［ｍ］の略円弧状の路面を真の速度ＶＴ［ｍ／ｓ］、真のピッチレート（すなわち角速度）ωyＴ［ｄｅｇｒｅｅ／ｓ］で走行している場合を想定する。因みに、図中の点Ｐが車両の位置を表している。

このとき真のピッチレートωyＴ、誤差を含むピッチレートω_y、真の加速度α _z Ｔ、及び誤差を含む加速度α _z については、それぞれ次の（８）〜（１１）式のように表すことができる。

ところで上述したように、速度計算部３４では（３）式により速度Ｖを算出している。そこで速度Ｖをモデル化するに際し、速度Ｖと同様に最大加速度α_z,max及び最小加速度α_z,minと、最大ピッチレートω_y,max及び最小ピッチレートω_y,minとを用いることを検討する。

実際の路面では、一定の曲率半径でなる略円弧状の路面が続くことは希であり、実質的には異なる曲率半径の略円弧状の路面が順次連結されていると見なし得る。そこで、図４４（Ｂ）に示すように、路面の曲率半径がＲ_１［ｍ］からＲ_２［ｍ］に切り替わった場合を想定する。

ここで曲率半径Ｒ_２＞Ｒ_１と仮定すると、上述した（３）式における最大加速度α_z,max及び最大ピッチレートω_y,maxが曲率半径Ｒ_１の路面において得られ、最小加速度α_z,min及び最小ピッチレートω_y,minが曲率半径Ｒ_２の路面で得られると考えられる。

そこで（３）式については、（８）〜（１１）式を用いることにより、次の（１２）式のように表すことができる。

ここで次の（１３）式に示す定数Ｋを用いると、（１２）式は次の（１４）式のように表すことができる。

この（１４）式は、速度Ｖが、真の速度ＶＴ、加速度の測定誤差Δα_z、ピッチレートの測定誤差Δω_y及び定数Ｋにより求め得ることを示している。この（１４）式により表される速度Ｖが、上述した速度モデルとなる。

さらに（１４）式を速度ＶＴについて整理すると、次の（１５）式のように表すことができる。

この（１５）式は、真の速度ＶＴが、速度Ｖ、加速度の測定誤差Δα_z、ピッチレートの測定誤差Δω_y及び定数Ｋにより求め得ることを示している。

ところで車両が停車しているとき、真の加速度α_zＴ及び真のピッチレートωyＴはいずれも値「０」になる。そこで車両の停車中、すなわちＧＰＳ速度ＶＧを取得でき且つその値がほぼ速度０［ｋｍ／ｈ］であれば、このときの加速度α_z及びピッチレートωyを、それぞれ加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとみなすことができる。

また車両の速度がほぼ０［ｋｍ／ｈ］のとき加速度α_z及びピッチレートω_yは、本来一定の値「０」となるべきものであるが、実際にはいずれも一定の値とはならず、「０」付近にある程度のばらつきを有する値となる。ここで加速度α_z及びピッチレートω_yそれぞれについて標準偏差を算出すると、各標準偏差は値「０」からの広がりの大きさを表すことになる。

一方、加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yは、いずれも「０」近傍の値をとるべきものである。そこで、各標準偏差に所定の定数をそれぞれ乗じると、各標準偏差を近似的に加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとみなすことができる。

また、（１４）式を定数Ｋについて整理すると、次に示す（１６）式のように表すことができる。

この（１６）式は、定数Ｋが、速度Ｖ、真の速度ＶＴ、加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yにより算出し得ることを示している。ここで加速度の測定誤差Δα_z、及びピッチレートの測定誤差Δω_yについては、上述したように停車中に求めることができる。また真の速度ＶＴとしては、ＧＰＳ速度ＶＧを用いることができる。

従って定数Ｋは、ＧＰＳ速度ＶＧを取得できるときであれば、最新の加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを用いて最新の値に更新することが可能となる。

因みに速度Ｖと速度比ＲＶとの関係は、図４５に示すように、定数Ｋを変化させることにより大きく異なる特性曲線を描く。因みにこの関係は、定数Ｋをおおむね１．００×１０^−６〜３．００×１０^−６としたときに、良好な補正結果が得られるような特性曲線となった。

このようにＰＮＤ７０は、ＧＰＳ信号を受信できＧＰＳ速度ＶＧを生成できるとき、（１６）式に従って最新の定数Ｋを算出しておく、すなわち定数Ｋを学習しておくことができる。

またＰＮＤ７０は、ＧＰＳ信号を受信できずＧＰＳ速度ＶＧを生成できないとき、（１５）式に従い最新の定数Ｋを用いて真の速度ＶＴを算出すること、すなわち速度Ｖを補正することができる。

［３−２−２．加減速時における速度の補正原理］
ところで、車両が加速中又は減速中であるときを含めて、ＧＰＳ速度ＶＧと速度比ＲＶとの関係をプロットすると、図３９と対応する図４６に示すように、直線Ｌ１の近傍に加えて、直線Ｌ２の近傍にも分布が出現した。

以下、直線Ｌ２の近傍に出現した分布について検討する。上述したようにＰＮＤ７０は、（１）式を変形した（３）式に従い、加速度α_z及びピッチレートω_yについて、それぞれデータ点の範囲内における最大値と最小値との差分値を用いて速度Ｖを算出する。

ここで、まずデータ点の範囲内における最大値及び最小値が、当該範囲内における曲率半径Ｒの変化によって生じ、且つ当該区間内で速度Ｖが変化しない場合について検討する。

例えば図４７（Ａ）に示すように、路面が凹状又は凸状のまま、接続点Ｑ１において曲率半径Ｒ_１からＲ_２へ、又は曲率半径Ｒ_２からＲ_１へ変化する場合を想定する。因みに曲率半径Ｒ_２＞Ｒ_１とする。この場合、（３）式は次の（１７）式のように変形することができる。

また図４７（Ｂ）に示すように、路面の凹凸が切り替わりながら、接続点Ｑ２において曲率半径Ｒ_１からＲ_２へ、又は曲率半径Ｒ_２からＲ_１へ変化する場合を想定する。この場合、（３）式は次の（１８）式のように変形することができる。

（１７）式及び（１８）式から、データ点の範囲内で曲率半径が変化したとしても、路面の凹凸が変化するか否かにかかわらず、（３）式により速度Ｖを正しく算出できることがわかる。

次に、データ点の範囲内における曲率半径Ｒが変化せず、車両の速度Ｖが変化する場合について検討する。

例えば図４８に示すように、曲率半径Ｒの路面上の変速点Ｑ３において速度Ｖ_１から速度Ｖ_２へ、又は速度Ｖ_２から速度Ｖ_１へ変化する場合について検討する。因みに速度Ｖ_２＞Ｖ_１とする。この場合、（３）式は次の（１９）式のように変形することができる。

この（１９）式から、図４８に示したような場合、（３）式では速度Ｖを正しく算出できないことがわかる。

さらに、データ点の範囲内で曲率半径Ｒが変化し、且つ速度Ｖも変化する場合について検討する。例えば図４９（Ａ）に示すように、路面が凹状又は凸状のまま、接続点Ｑ４において曲率半径Ｒ_１からＲ_２へ、又は曲率半径Ｒ_２からＲ_１へ変化し、さらに車両の速度Ｖが速度Ｖ_１からＶ_２へ、又は速度Ｖ_２からＶ_１へ変化する場合を想定する。

因みに曲率半径Ｒ_２＞Ｒ_１とし、且つ速度Ｖ_２＞Ｖ_１とする。また計算の都合上、Ｒ_１＝Ｒ、Ｒ_２＝Ｒ＋ΔＲ、Ｖ_１＝Ｖ、Ｖ_２＝Ｖ＋ΔＶとする。

この場合、（３）式は次の（２０）式のように変形することができる。

（２０）式において、Ｒ・ΔＶ＝Ｖ・ΔＲのとき、すなわち（ΔＲ／Ｒ）＝（ΔＶ／Ｖ）のとき、誤差は無限大となる。

また、定数Ｋについての（１３）式を変形すると、次に示す（２１）式のように表すことができる。

この（２１）式から、（ΔＲ／Ｒ）も１．０×１０^−６のオーダーとなる。このことを踏まえると、速度Ｖについては、（２０）式を変形することにより、次の（２２）式のように近似することができる。

この（２２）式から、図４９（Ａ）に示したような場合、すなわち加速度αz及びピッチレートωyのいずれもについて、最大値及び最小値の符号が同一の場合には、速度Ｖが約２倍となってしまい、当該速度Ｖを正しく算出できないことがわかる。

また（２２）式は、図４６において、直線Ｌ２の傾きが直線Ｌ１の傾きの約半分となっていることを裏付けている。

一方、例えば図４９（Ｂ）に示すように、路面の凹凸が切り替わりながら、接続点Ｑ５において曲率半径Ｒ_１からＲ_２へ、又は曲率半径Ｒ_２からＲ_１へ変化し、さらに車両の速度Ｖが速度Ｖ_１からＶ_２へ、又は速度Ｖ_２からＶ_１へ変化する場合を想定する。この場合、（３）式は次の（２３）式のように変形することができる。

（２３）式において、Ｒ・ΔＶ＝Ｖ・ΔＲのとき、すなわち（ΔＲ／Ｒ）＝（ΔＶ／Ｖ）のとき、誤差がほぼ値「０」となる。この場合、速度Ｖを精度良く算出することが可能となる。

また（２１）式から、（ΔＲ／Ｒ）が１．０×１０^−６のオーダーとなり（ΔＶ／Ｖ）よりも極めて小さいため、速度Ｖについては、（２３）式を変形することにより次の（２４）式のように近似することができる。

この（２４）式から、図４９（Ｂ）に示したような場合、速度Ｖが小さく車両が低速であるほど、また速度変化（すなわちΔＶの値）が大きいほど、速度Ｖの誤差が大きくなることがわかる。

このように、（３）式により速度Ｖを算出するＰＮＤ７０は、データ点の範囲内で速度Ｖが変化する場合、加速度α_z及びピッチレートω_yのいずれもについて、最大値及び最小値の符号が相違する場合には正しい速度Ｖを得られるものの、最大値及び最小値の符号が同一の場合には、速度Ｖが約２倍の値になってしまうことが判明した。

［３−２−３．速度算出部の構成］
上述した速度の補正原理に従い、第３の実施の形態による速度算出部７２では、速度モデルを用いて速度Ｖを補正すると共に、加速度α_z及びピッチレートω_yにおける最大値及び最小値の符号に応じて当該速度Ｖを補正するようになされている。

速度算出部７２は、図８との対応部分に同一符号を付した図５０に示すように、速度算出部２２の構成に加えて測定誤差算出部８１及び定数算出部８２が設けられ、さらに平滑化及びノイズ除去部３５及び速度出力部３６の間に速度補正部８３が設けられている。

測定誤差算出部８１は、ローパスフィルタ部３３から加速度データＡＤ２及びピッチレートデータＰＤ２を取得すると共に、ＧＰＳ処理部２１（図４３）からＧＰＳ速度ＶＧを取得するようになされている。

測定誤差算出部８１は、ＧＰＳ速度ＶＧが値「０」と見なし得る範囲（例えば０．１［ｍ／ｓ］未満等）にあれば、過去１秒間に加速度データＡＤ２及びピッチレートデータＰＤ２として得られた加速度α_z及びピッチレートω_yそれぞれについて標準偏差を算出する。

さらに測定誤差算出部８１は、加速度α_zの標準偏差及びピッチレートω_yの標準偏差にそれぞれ所定の係数を乗じた値を加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとし、定数算出部８２及び速度補正部８３へ供給する。

定数算出部８２は、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得し、平滑化及びノイズ除去部３５から速度データＶＤを取得すると共に、測定誤差算出部８１から加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを取得する。

続いて定数算出部８２は、ＧＰＳ速度ＶＧを取得でき且つ停車中でないとき（例えば例えば速度が０．１［ｍ／ｓ］以上等）であれば、定数算出処理として、上述した（１６）式に従って定数Ｋを算出する。

さらに定数算出部８２は、第２の実施の形態における補正係数Ｃと同様に、直前の定数Ｋ（以下これを直前定数Ｋ_n-1とする）を記憶しており、新たに得られた定数Ｋ（以下これを最新定数Ｋ_nとする）を用いて当該定数Ｋを平準化しながら更新するようになされている。

具体的に定数算出部８２は、時定数を表す定数Ｍを用い、次の（２５）式に従って新たな定数Ｋを算出し、これを速度補正部８３へ供給するようになされている。

速度補正部８３は、速度計算部３４から最大加速度α_z,max、最小加速度α_z,min、最大ピッチレートω_y,max及び最小ピッチレートω_y,minを取得し、平滑化及びノイズ除去部３５から速度データＶＤを取得する。また速度補正部８３は、測定誤差算出部８１から加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを取得し、定数算出部８２から定数Ｋを取得し、さらにＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得する。

速度補正部８３は、少なくとも最大加速度α_z,max及び最小加速度α_z,minの符号が同一である場合、又は最大ピッチレートω_y,max及び最小ピッチレートω_y,minの符号が同一である場合に、速度Ｖを１／２倍する。

続いて速度補正部８３は、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得できる場合、速度データＶＤをそのまま速度出力部３６へ供給する。

一方ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得できない場合、速度補正部８３は、速度Ｖ、加速度の測定誤差Δα_z、ピッチレートの測定誤差Δω_y及び定数Ｋを用い、（１５）式に従って真の速度ＶＴを算出する。このとき速度補正部８３は、（１５）式により速度Ｖを補正したことになる。

その後速度補正部８３は、真の速度ＶＴ、すなわち補正後の速度Ｖを速度データＶＤとして速度出力部３６へ供給することにより、当該速度データＶＤを位置算出部２５（図４３）へ出力させる。

このように速度算出部７２は、ＧＰＳ速度ＶＧを取得した場合、停車中であれば加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを更新し、移動中であれば定数Ｋを更新し、またＧＰＳ速度ＶＧを取得できない場合、定数Ｋを用いて速度Ｖを補正するようになされている。

［３−２−４．速度補正処理手順］
次に、ＰＮＤ７０の制御部７１が、速度算出部７２により速度補正処理を行う際の速度補正処理手順ＲＴ３について、図５１のフローチャートを用いて説明する。

制御部７１は、ルーチンＲＴ３の開始ステップから入ってステップＳＰ３１へ移り、速度補正部８３により少なくとも最大加速度α_z,max及び最小加速度α_z,minの符号が同一であるか、又は最大ピッチレートω_y,max及び最小ピッチレートω_y,minの符号が同一であるかを判定する。ここで肯定結果が得られると、このことは算出された速度Ｖが本来の速度Ｖの約２倍の値となっていることを表しており、このとき制御部７１は次のステップＳＰ３２へ移る。

ステップＳＰ３２において制御部７１は、速度補正部８３により速度Ｖを１／２倍し、次のステップＳＰ３３へ移る。

一方、ステップＳＰ３１において否定結果が得られると、このことは速度Ｖが正しい速度を表しており１／２倍する必要がないことを表しており、このとき制御部７１は次のステップＳＰ３３へ移る。

ステップＳＰ３３において制御部７１は、平滑化及びノイズ除去部３５によって所定の平滑化処理及びノイズ除去処理を行うと共に、ＧＰＳ処理部２１からＧＰＳ速度ＶＧを取得したか否かを判定する。ここで肯定結果が得られると、このことは加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_y又は定数Ｋの学習処理を行うべきであることを表しており、このとき制御部７１は次のステップＳＰ３４へ移る。

ステップＳＰ３４において制御部７１は、ＧＰＳ速度ＶＧが所定の閾値と比較することにより、車両が停車中であるか否かを判定する。ここで肯定結果が得られると、このことは加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを算出すべきであることを表しており、このとき制御部７１は次のステップＳＰ３５へ移る。

ステップＳＰ３５において制御部７１は、測定誤差算出部８１により、過去１秒間に得られた加速度α_z及びピッチレートω_yそれぞれについて標準偏差を算出する。さらに測定誤差算出部８１は、加速度α_zの標準偏差及びピッチレートω_yの標準偏差にそれぞれ所定の係数を乗じた値を加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとし、ステップＳＰ４０へ移って一連の処理を終了する。

一方、ステップＳＰ３４において否定結果が得られると、このことは車両が走行中であり定数Ｋを算出すべきであることを表しており、このとき制御部７１は次のステップＳＰ３６へ移る。

ステップＳＰ３６において制御部７１は、定数算出部８２により、速度Ｖ、ＧＰＳ速度ＶＧ、加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを用い、（１６）式に従って定数Ｋを算出し、次のステップＳＰ３７へ移る。

ステップＳＰ３７において制御部７１は、定数Ｋが所定の正常範囲内にあるか否か判定する。ここで肯定結果が得られると、このことは当該定数Ｋを速度Ｖの補正に利用し得ることを表しており、このとき制御部７１は次のステップＳＰ３８へ移る。

ステップＳＰ３８において制御部７１は、（２５）式により定数Ｋを平準化しながら更新し、更新後の定数Ｋを最新の定数Ｋとして記憶する。その後制御部７１は、ステップＳＰ４０へ移って一連の処理を終了する。

一方、ステップＳＰ３７において否定結果が得られると、このことは例えばユーザによりタッチパネルが操作された際の加速度成分等が速度Ｖに含まれている等、算出した定数Ｋが異常値であることを表している。このため制御部７１は、算出した定数Ｋを用いることなくステップＳＰ４０へ移って一連の処理を終了する。

これに対し、ステップＳＰ３３において否定結果が得られると、このことはＧＰＳ衛星からのＧＰＳ信号を受信できないため速度算出部７２により算出した速度Ｖを基に車両の現在位置等を算出すべきことを表しており、このとき制御部７１は次のステップＳＰ３９へ移る。

ステップＳＰ３９において制御部７１は、速度補正部８３により、速度Ｖ、最新の定数Ｋ、最新の加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを用い、（１５）式に従って真の速度ＶＴを算出することにより速度Ｖを補正する。続いて制御部７１は、真の速度ＶＴを新たな速度Ｖとして速度出力部３６へ供給した後、次のステップＳＰ４０へ移って一連の処理を終了する。

［３−３．動作及び効果］
以上の構成において、第３の実施の形態によるＰＮＤ７０は、路面のうねりによって発生する車両の進行方向に垂直なＺ軸方向の加速度α_zを３軸加速度センサ４により検出し、路面のうねりによって発生する当該進行方向と直交したＹ軸周りのピッチレートω_yをＹ軸ジャイロセンサ５により検出する。

そしてＰＮＤ７０は、３軸加速度センサ４によって検出された加速度α_z及びＹ軸ジャイロセンサ５によって検出されたピッチレートω_yを基に、（１）式又は（３）式に従って速度Ｖを算出するようにした。

さらにＰＮＤ７０は、ＧＰＳ速度ＶＧを取得できないときには、速度補正部８３により（１５）式に従い定数Ｋを用いて真の速度ＶＴを算出し、これを補正後の速度Ｖとするようにした。

従ってＰＮＤ７０は、第１の実施の形態と同様、３軸加速度センサ４及びＹ軸ジャイロセンサ５だけを用いた簡易な構成により、ＧＰＳ信号が受信することができない場合であっても、全ての道路環境下で車両の速度Ｖを正確に算出することができる。

これに加えてＰＮＤ７０は、ＧＰＳ速度ＶＧを受信できない場合、速度モデルを用いることにより速度Ｖに応じて当該速度Ｖを適切に補正することができるので、補正後の速度Ｖに含まれる誤差を極めて小さく抑えることができる。

またＰＮＤ７０は、ＧＰＳ速度ＶＧを取得できるときには、移動中であれば、定数算出部８２により速度Ｖに応じた定数Ｋを算出するようにした。これによりＰＮＤ７０は、ＧＰＳ速度ＶＧが比較的正確であることを利用して適切な定数Ｋを算出することができ、ＧＰＳ速度ＶＧを取得できなくなったときに最新の定数Ｋを用いて速度Ｖを補正することができる。

さらにＰＮＤ７０は、ＧＰＳ速度ＶＧを取得できるときには、停車中に測定誤差算出部８１により加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを算出するようにした。これによりＰＮＤ７０は、随時変化し得る加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yを適宜更新することができる。

そのうえＰＮＤ７０は、加速度α_z及びピッチレートω_yの最大値及び最小値が同一符号である場合、速度Ｖを１／２倍することにより、速度Ｖの算出原理上発生し得る誤差を適切に補正することができる。

その他の点について、第３の実施の形態によるＰＮＤ７０は、第１の実施の形態によるＰＮＤ１と同様の作用効果を奏し得る。

以上の構成によれば、ＰＮＤ７０は、路面のうねりによって発生するＺ軸方向の加速度α_z、及び路面のうねりによって発生するＹ軸周りのピッチレートω_yを検出し、当該加速度α_z及びピッチレートω_yを用いて速度Ｖを算出することにより、全ての道路環境下で速度Ｖを正確に算出することができる。さらにＰＮＤ７０は、速度モデルを利用し速度Ｖに応じて当該速度Ｖを補正することにより、当該速度Ｖに含まれる誤差を極めて小さく抑えることができる。

＜４．他の実施の形態＞
なお上述した第２の実施の形態においては、速度Ｖの速度域を高速及び低速の２段階に分割し、速度域ごとに低速補正係数Ｃ１及び高速補正係数Ｃ２といった２種類の補正係数Ｃを用いる場合について述べた。

しかしながら本発明はこれに限らず、例えば全ての速度域で共通の補正係数Ｃを用いて速度Ｖを補正しても良く、或いは速度Ｖの速度域を３段階以上に分割し、当該速度域ごとに補正係数を用いるようにしても良い。さらには、一部の速度域において速度Ｖを補正しないようにしても良い。各速度域の境界値については、速度Ｖに含まれる誤差と当該速度Ｖとの関係の傾向等に応じて適宜定めれば良い。

また上述した第２の実施の形態においては、（７）式に従い速度域ごとに速度Ｖに補正係数Ｃを乗じることにより当該速度Ｖを補正するようにした場合について述べた。

しかしながら本発明はこれに限らず、例えば定数項を有する１次関数や２次以上の関数又は指数関数等、種々の演算手法を用いて速度Ｖを補正するようにしても良い。この場合、速度域ごとに種々の係数や定数を相違させても良く、或いは速度域ごとに関数そのものを相違させても良い。

さらに上述した第２の実施の形態においては、ＧＰＳ速度ＶＧを取得できる場合であっても、車両の加減速中及び方位変化中には補正係数Ｃの算出処理及び更新処理を行わないようにした場合について述べた。

しかしながら本発明はこれに限らず、ＧＰＳ速度ＶＧを取得できる場合に、他の条件により補正係数Ｃの算出処理及び更新処理を行わないようにしても良い。或いは、例えば加減速や方位変化に起因して生じる誤差が極めて小さく実質的に無視できることが予め判明している場合等に、車両の加減速中や方位変化中であっても、補正係数Ｃの算出処理や更新処理を行うようにしても良い。

さらに上述した第２の実施の形態においては、低速補正係数Ｃ１及び高速補正係数Ｃ２を（５）式及び（６）式によりそれぞれ平準化して更新するようにした場合について述べた。

しかしながら本発明はこれに限らず、低速補正係数Ｃ１及び高速補正係数Ｃ２について、例えば過去５分以内に算出した値の平均値を更新後の値として用いる等、他の演算手法によりそれぞれ更新するようにしても良い。或いは低速補正係数Ｃ１及び高速補正係数Ｃ２について、それぞれ予め定めた値に固定し更新しないようにしても良い。

さらに上述した第３の実施の形態においては、速度Ｖを（１４）式により表される速度モデルによりモデル化し、（１５）式により当該速度Ｖを補正するようにした場合について述べた。

本発明はこれに限らず、真の速度（すなわちＧＰＳ速度ＶＧ）を用いた他の種々の数式により表される速度モデルにより当該速度Ｖをモデル化し、真の速度について表すよう当該数式を変形して当該真の速度（すなわち補正後の速度Ｖ）を算出するようにしても良い。

さらに上述した第３の実施の形態においては、ＧＰＳ速度ＶＧを取得できるときに（１６）式により定数Ｋを算出し、当該定数Ｋを（２５）式により平準化して更新するようにした場合について述べた。

しかしながら本発明はこれに限らず、例えば過去１０分以内に算出した定数Ｋの平均値を更新後の定数Ｋとして用いる等、他の演算手法により任意の期間に算出した定数Ｋを用いて更新するようにしても良い。或いは、最適な定数Ｋを予め算出しておき、当該定数Ｋを更新せず固定したままとしても良い。

さらに上述した第３の実施の形態においては、過去１秒間に得られた加速度α_z及びピッチレートω_yそれぞれの標準偏差に所定の係数を乗じた値を加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとする場合について述べた。

しかしながら本発明はこれに限らず、例えば過去５秒以内に算出した加速度α_z及びピッチレートω_yそれぞれの平均値を更新後の加速度の測定誤差Δα_z及びピッチレートの測定誤差Δω_yとして用いる等、他の演算手法によりそれぞれ更新するようにしても良い。或いは、正確な測定誤差Δα_z及びピッチレートの測定誤差Δω_yを予め計測しておき、更新せず固定したままとしても良い。

さらに上述した第３の実施の形態では、速度Ｖの値にかかわらず、（１５）式に従い定数Ｋを用いて一様に速度Ｖを補正する場合について述べた。しかしながら本発明はこれに限らず、例えば速度Ｖが所定の速度域に含まれる場合にのみ当該速度Ｖを補正するようにしても良い。

さらに上述した第３の実施の形態では、加速度α_z及びピッチレートω_yの最大値及び最小値が同一符号である場合、速度Ｖを１／２倍するようにした場合について述べた。しかしながら本発明はこれに限らず、例えば加速度α_z及びピッチレートω_yの最大値及び最小値が同一符号となる可能性が極めて低い場合に、速度Ｖを１／２倍せず（１５）式による補正のみを行うようにしても良い。或いは、第２の実施の形態において、加速度α_z及びピッチレートω_yの最大値及び最小値が同一符号である場合に速度Ｖを１／２倍しても良い。

さらに上述した実施の形態においては、速度Ｖを計算する際、加速度データＡＤ２に相当する加速度α_zから抽出した最大加速度α_z,max及び最小加速度α_z,minと、角速度データＤＤ２に相当するピッチレートω_yから抽出した最大角速度ω_y,max及び最小角速度ω_y,minとを基に、（３）式を用いて速度Ｖを算出するようにした。

しかしながら本発明はこれに限らず、速度計算部３４は、ローパスフィルタ部３３から供給された加速度データＡＤ２に相当する加速度α_z、及びピッチレートデータＰＤ２に相当するピッチレートω_yの例えば前回の位置Ｐ０に対応するデータ点Ｐ_ｍを中心とした２５データ点又は７５データ点分の分散をそれぞれ求める。そして速度算出部３４は、加速度α_zの分散をピッチレートω_yの分散で除算することにより速度Ｖを算出するようにしても良い。

或いは、速度計算部３４は、ローパスフィルタ部３３から供給された加速度データＡＤ２に相当する加速度α_z、及びピッチレートデータＰＤ２に相当するピッチレートω_yの例えば前回の位置Ｐ０に対応するデータ点Ｐ_ｍを中心とした２５データ点又は７５データ点分の範囲の偏差をそれぞれ求める。そして速度算出部３４は、加速度α_zの偏差をピッチレートω_yの偏差で除算することにより速度Ｖを算出するようにしても良い。これらの場合、第３の実施の形態における速度モデルを当該速度Ｖと対応する数式によりモデル化すれば良い。

また上述した実施の形態においては、３軸加速度センサ４、Ｙ軸ジャイロセンサ５及びＺ軸ジャイロセンサ６により５０［Ｈｚ］のサンプリング周波数で加速度α_x、α_y、α_z、ピッチレートω_y及びヨーレートω_zを測定するようにした場合について述べた。しかしながら本発明はこれに限らず、３軸加速度センサ４、Ｙ軸ジャイロセンサ５及びＺ軸ジャイロセンサ６は、５０［Ｈｚ］以外にも例えば１０［Ｈｚ］等の所定のサンプリング周波数により加速度α_x、α_y、α_z、角速度ω_y及び角速度ω_zを検出するようにしても良い。

さらに上述した実施の形態においては、５０［Ｈｚ］のサンプリング周波数で検出した加速度α_z及びピッチレートω_yを用いて速度Ｖを算出するようにした場合について述べた。しかしながら本発明はこれに限らず、速度算出部２２、５２及び７２は、５０［Ｈｚ］のサンプリング周波数で検出した加速度α_z及びピッチレートω_yの例えば２５データ点毎の平均値を取り、当該加速度α_z及びピッチレートω_yの平均値を用いて速度Ｖを算出するようにしても良い。

この場合、速度算出部２２、５２及び７２は、５０［Ｈｚ］のサンプリング周波数で検出した加速度α_z及びピッチレートω_yの例えば２５データ点毎の平均値を取ることにより、速度Ｖを１秒当たり２回だけ算出することになる。これにより制御部１１、５１及び７１は、速度算出処理に対する処理負荷を軽減することができる。

さらに上述した実施の形態においては、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ部３２によりハイパスフィルタ処理を施すようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０及び７０は、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理を施さないようにしてもよい。

さらに上述した実施の形態においては、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ部３２及びローパスフィルタ部３３によりハイパスフィルタ処理及びローパスフィルタ処理を施すようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０及び７０は、加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理及びローパスフィルタ処理に加えて、移動平均フィルタ処理を施すようにしてもよい。またＰＮＤ１、５０及び７０は、加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理、ローパスフィルタ処理及び移動平均フィルタ処理を任意に組み合わせた処理を施すようにしても良い。

さらに上述した実施の形態においては、ＰＮＤ１、５０及び７０が、電源電力の供給を受けている間、現在位置算出処理手順に従ってナビゲーションを行うにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０及び７０は、電源ボタン（図示せず）がユーザによって押下操作されることによってオフされた場合、当該電源ボタンが押下された時点での現在位置及び高度等を記憶部１２へ記憶する。そしてＰＮＤ１、５０及び７０は、再び電源ボタンがユーザによって押下操作されることによってオンされた場合、記憶部１２から現在位置及び高度等を読み出し、当該現在位置及び高度等から再び現在位置算出処理手順に従ってナビゲーションを行うようにしても良い。

さらに上述した実施の形態においては、ＰＮＤ１、５０及び７０が、車両のダッシュボード上に載置されたクレードル３によって保持されている状態で、速度Ｖを算出するようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０及び７０が、クレードル３から機械的或いは電気的に取り外されたことを認識すると、速度Ｖを０とする、或いは前値速度V_n-1のまま継続するようにしても良い。

さらに上述した実施の形態においては、ＰＮＤ１、５０及び７０が左右方向に長い横置きの状態で使用されるようにした場合について述べた。しかしながら本発明はこれに限らず、図５２に示すように、ＰＮＤ１、５０及び７０は、縦方向に長い縦置きの状態で使用されるようにしてもよい。この場合ＰＮＤ１、５０及び７０は、Ｙ軸ジャイロセンサ５によりＺ軸回りのヨーレートω_ｚを検出し、またＺ軸ジャイロセンサ６によりＹ軸周りのピッチレートω_ｙを検出するようにすればよい。

さらに上述した実施の形態においては、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７がＰＮＤ１、５０及び７０の内部に設けられているようにした場合について述べた。しかしながら本発明はこれに限らず、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７が、ＰＮＤ１、５０及び７０の外部に設けられているようにしてもよい。

またＰＮＤ１、５０及び７０は、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７の取り付け角度を調節できるような調節機構を例えば筐体の側面に設けるようにしても良い。

これによりＰＮＤ１、５０及び７０は、その表示部２が車両の進行方向に対してほぼ垂直となるように設置されていない場合であっても、調節機構をユーザに調節させることによって、例えばＹ軸ジャイロセンサ５の回転軸を車両の垂直方向と揃えることができる。

さらに上述した実施の形態においては、ピッチレートデータＰＤ２に相当するピッチレートω_yが所定の閾値未満であった場合、及び加速度データＡＤ２に相当する加速度α_z及びピッチレートデータＰＤ２に相当するピッチレートω_yがそれぞれ所定の閾値より大きい場合、速度Ｖが過大であると判断するようにした。しかしながら本発明はこれに限らず、制御部１１、５１及び７１は、速度計算部３４によって算出された速度Ｖが前値速度Ｖ_n-1より所定速度以上の大きな値を取ったとき、速度Ｖが過大であると判断するようにしても良い。

この場合、平滑化及びノイズ除去部３５は、速度計算部３４によって算出された速度Ｖが前値速度Ｖ_n-1より所定速度以上の大きな値を取ったときで、かつ前値速度が例えば速度１０［ｋｍ／ｈ］未満の超低速時であった場合、速度Vを０とする。また平滑化及びノイズ除去部３５は、速度計算部３４によって算出された速度Ｖが前値速度Ｖ_n-1より所定速度以上の大きな値を取ったときで、かつ前値速度が例えば速度１０［ｋｍ／ｈ］以上であった場合、前値速度Ｖ_n-1を速度Vとするようにすればよい。

さらに上述した実施の形態においては、ＰＮＤ１、５０及び７０を自動車でなる車両に搭載する場合について述べた。しかしながら本発明はこれに限らず、例えばＰＮＤ１、５０及び７０を水面に沿って進行する船舶に搭載するようにしても良い。

さらに上述した実施の形態においては、ＰＮＤ１、５０及び７０の制御部１１、５１及び７１が、予め記憶部１２に格納されているアプリケーションプログラムに従い、上述したルーチンＲＴ１、ＲＴ２及びＲＴ３の各処理手順を行うようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０及び７０の制御部１１、５１及び７１が、記憶媒体からインストールしたアプリケーションプログラムや、インターネットからダウンロードしたアプリケーションプログラム、その他種々のルートによってインストールしたアプリケーションプログラムに従って上述した各処理手順を行うようにしても良い。

さらに上述した第２の実施の形態においては、垂直方向加速度検出部としての３軸加速度センサ４と、水平方向角速度検出部としてのＹ軸ジャイロセンサ５と、速度算出部としての速度算出部５２と、速度補正部としての速度補正部６３とによって速度算出装置としてのＰＮＤ５０を構成する場合について述べた。

本発明はこれに限らず、この他種々の構成でなる垂直方向加速度検出部と、水平方向角速度検出部と、速度算出部と、速度補正部とによって速度算出装置を構成するようにしても良い。

本発明の速度算出装置、速度算出方法及びナビゲーション装置は、例えば車両以外にも、バイク、電車等のその他種々に実装されたＰＮＤや据置型のナビゲーション装置等に適用することができる。

１、５０、７０……ＰＮＤ、２……表示部、３……クレードル、４……３軸加速度センサ、５……Ｙ軸ジャイロセンサ、６……Ｚ軸ジャイロセンサ、７……気圧センサ、１１、５１、７１……制御部、１２……記憶部、２１……ＧＰＳ処理部、２２、５２、７２……速度算出部、２３……角度算出部、２４……高度算出部、２５……位置算出部、２６……ナビゲーション部、３１……データ取得部、３２……ハイパスフィルタ部、３３……ローパスフィルタ部、３４……速度計算部、３５……平滑化及びノイズ除去部、３６……速度出力部、６１……係数算出部、６３、８３……速度補正部、８１……測定誤差算出部、８２……定数算出部。

Claims (4)

1. 所定の移動面上を移動する移動体に搭載され、上記移動面のうねりに応じて発生する垂直方向の加速度を検出する垂直方向加速度検出部と、
   上記移動体に搭載され、上記移動面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、
   上記移動体の上記進行方向への速度を検出し基準速度とする速度検出部と、
   所定期間における上記垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、上記所定期間における上記水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、上記最大加速度と上記最小加速度との差を上記最大角速度と上記最小角速度との差で除算することにより、上記移動体の進行方向の速度を算出する速度算出部と、
   上記速度を変数とし所定の定数を用いる関数に上記速度を適用することにより当該速度を補正する速度補正部と
   を有し、
   上記関数は、上記速度を算出する算出式において上記最大加速度、上記最小加速度、上記最大角速度及び上記最小角速度を、上記移動面のうねりにおける曲率半径と上記基準速度とを用いた変数にそれぞれ置き換え上記基準速度について表した関数でなる
   速度算出装置。
2. 上記速度補正部は、
   少なくとも上記最大加速度及び上記最小加速度の符号が同一である場合、又は上記最大角速度及び上記最小角速度の符号が同一である場合に、上記速度を１／２倍する
   請求項１に記載の速度算出装置。
3. 所定の移動面のうねりに応じて当該移動面上を移動する移動体に発生する垂直方向の加速度を検出する垂直方向加速度検出ステップと、
   上記移動面のうねりに応じて上記移動体に発生する上記移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出ステップと、
   上記移動体の上記進行方向への速度を検出し基準速度とする速度検出ステップと、
   所定期間における上記垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、上記所定期間における上記水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、上記最大加速度と上記最小加速度との差を上記最大角速度と上記最小角速度との差で除算することにより、上記移動体の進行方向の速度を算出する速度算出ステップと、
   上記速度を変数とし所定の定数を用いる関数に上記速度を適用することにより当該速度を補正する速度補正ステップと
   を有し、
   上記関数は、上記速度を算出する算出式において上記最大加速度、上記最小加速度、上記最大角速度及び上記最小角速度を、上記移動面のうねりにおける曲率半径と上記基準速度とを用いた変数にそれぞれ置き換え上記基準速度について表した関数でなる
   速度算出方法。
4. 所定の移動面のうねりに応じて当該移動面上を移動する移動体に発生する垂直方向の加速度を検出する垂直方向加速度検出部と、
   上記移動面のうねりに応じて上記移動体に発生する上記移動体の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、
   上記移動体の上記進行方向への速度を検出し基準速度とする速度検出部と、
   所定期間における上記垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、上記所定期間における上記水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、上記最大加速度と上記最小加速度との差を上記最大角速度と上記最小角速度との差で除算することにより、上記移動体の進行方向の速度を算出する速度算出部と、
   上記速度を変数とし所定の定数を用いる関数に上記速度を適用することにより当該速度を補正する速度補正部と、
   進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、
   上記垂直軸周りの角速度に基づいて上記移動体が旋回した角度を算出する角度算出部と、
   上記速度算出部により算出された上記進行方向の速度と、上記角度算出部により算出された上記角度とに基づいて、上記移動体の位置を算出する位置算出部と
   を有し、
   上記関数は、上記速度を算出する算出式において上記最大加速度、上記最小加速度、上記最大角速度及び上記最小角速度を、上記移動面のうねりにおける曲率半径と上記基準速度とを用いた変数にそれぞれ置き換え上記基準速度について表した関数でなる
   ナビゲーション装置。

Images