JP5482047B2 - 速度算出装置、速度算出方法及びナビゲーション装置 - 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を用いて速度Ｖを算出することにより、全ての道路環境下で速度Ｖを正確に算出することができる。

＜２．第２の実施の形態＞
第２の実施の形態によるＰＮＤ５０（図４〜図８）は、第１の実施の形態によるＰＮＤ１と比較して、当該ＰＮＤ５０本体の取付傾きを考慮しつつ、一段と高精度な速度Ｖを算出するようになされていることが主な相違点である。

［２−１．顎上げ状態で使用されたときの悪影響］
この第２の実施の形態におけるＰＮＤ５０では、図３８に示すように、当該ＰＮＤ５０が車両の進行方向であるＸ軸に沿って背面側へ倒れるように、Ｙ軸（図５）を中心としてＱ度（例えば１２０度）だけ傾けられた状態（以下、これを顎上げ状態と呼ぶ）で使用された場合の速度算出結果に生じる悪影響を予め除去するようになされている。

この顎上げ状態とは、ＰＮＤ５０本体がクレードル３を介して設置された際に顎上げ状態になって固定される場合と、ＰＮＤ５０本体がクレードル３を介して設置された後にチルト機能等によって顎上げ状態に傾けられて使用される場合とを含む。

なおＰＮＤ５０では、顎上げ状態だけでなく、その逆となる顎下げ状態すなわち当該ＰＮＤ５０が車両の進行方向であるＸ軸に沿って手前側へ倒れるように、Ｙ軸（図５）を中心として−Ｑ度だけ傾けられた状態で使用された場合についても以下と同様に考えればよい。

例えばＰＮＤ５０は、ＰＮＤ５０本体がＸ軸に沿って倒れるように傾斜せず、Ｚ軸に垂直な状態のまま使用される場合（図５、図１３）、図３９に示すように、本発明の速度算出処理を用いて算出した速度Ｖ（以下、これを自律速度Ｖと呼ぶ）と、ＧＰＳ速度Ｖ_gとの速度比ＶＰ（ＧＰＳ速度Ｖ_g／自律速度Ｖ）が経過時間に拘わらず理想的には「１」の固定値に近づくことが予想される。

しかしながら、実際上ＰＮＤ５０は、ＰＮＤ５０本体がＸ軸に沿って倒れるように、Ｙ軸（図５）を中心としてＱ度（例えば１２０度）だけ傾けられた顎上げ状態で使用された場合、異常に大きな速度比ＶＰ（ＧＰＳ速度Ｖ_g／自律速度Ｖ）が３箇所の異常値ＥＶ１〜ＥＶ３として検出される。

異常値ＥＶ１は、図４０に示すように、高速道路のパーキングから走行車線へ向かって加速している頃のタイミング（経過時間）に相当していることが判明した。また異常値ＥＶ２、ＥＶ３においても、図４１、図４２に示すように、高速道路の走行車線を加速走行中のタイミング（経過時間）に相当していることが判明した。

すなわちＰＮＤ５０では、図４３に示すように、概して加速区間や減速区間に於いて自律速度ＶがＧＰＳ速度Ｖ_gよりも極端に突出したような大きな値を出力することが分かる。

実際上ＰＮＤ５０は、ＰＮＤ５０本体がＺ軸に垂直な状態のまま使用された場合、Ｘ軸方向の加速度α_xとＺ軸方向の加速度α_zとの間が本来無相関になるものの、ＰＮＤ５０本体が顎上げ状態で使用された場合、図４４に示すように、Ｘ軸方向の加速度α_xの値に応じてＺ軸方向の加速度α_zの値が変化するという相関関係（直線で示すような傾き）を持ってしまう。

これらの結果からＰＮＤ５０では、ＰＮＤ５０本体が顎上げ状態で使用された場合、加速区間や減速区間において自律速度Ｖを算出するためのＺ軸方向の加速度α_zにＸ軸方向の加速度α_xが混入してしまうことが容易に分かる。

すなわちＰＮＤ５０では、Ｚ軸方向の加速度α_zにＸ軸方向の加速度α_xが混入してしまうことにより、Ｚ軸方向の加速度α_zが本来よりも大きな値となって自律速度Ｖの算出結果に誤差を生じさせてしまうことになる。

実際上、自律速度Ｖを算出するに当っては、図２２においても説明したように、最大加速度α_z,maxと最小加速度α_z,minとの差分を用いるようになされているため、Ｚ軸方向の加速度α_zにはＸ軸方向の加速度α_xが混入された状態の絶対値として反映されてしまう。

そこでＰＮＤ５０は、Ｚ軸方向の加速度α_zに混入しているＸ軸方向の加速度α_xの混入度合いを相関係数Ｋ（後述する）として学習し、次式

により、Ｘ軸方向の加速度α_xをＺ軸方向の加速度α_zから予め除去した真のＺ軸方向の加速度α_z´を求める必要がある。

これによりＰＮＤ５０は、ＧＰＳ速度Ｖ_gと自律速度Ｖとの誤差を減少させ、図３９に示した速度比ＶＰ（ＧＰＳ速度Ｖ_g／自律速度Ｖ）が経過時間に拘わらず理想的な「１」の固定値に近づけ得るようになされている。

なお、ＰＮＤ５０では、Ｘ軸方向の加速度α_xの値が「０」付近の場合、僅かなノイズによっても当該加速度α_xの値に大きな影響を与え、当該ノイズが支配的になってしまうので、自律速度Ｖの算出に際しては、加速度｜α_x｜が所定の閾値ＴＨ（例えば０．０７５[m/S/S]）よりも大きいときに限り、相関係数Ｋを求めるようになされている。

［２−２．速度算出原理］
速度算出部５２では、Ｚ軸方向の加速度α_zとＸ軸方向の加速度α_xとの傾きを表す相関係数Ｋを予め学習により求め、それを用いてＺ軸方向の加速度α_zに混入しているＸ軸方向の加速度α_xを除去することにより真のＺ軸方向の加速度α_z´を求める。

そして速度算出部５２では、そのＺ軸方向の加速度α_z´とＹ軸周りのピッチレートω_yとを用いて、次式

により自律速度Ｖを算出するようになされている。

ここで速度算出部５２は、相関係数Ｋを予め学習により求めるに当って、例えばｎ回目のサンプリング結果に基づいて算出した相関係数Ｋｎを、次式

によって算出する。

しかしながら、この相関係数Ｋｎは、サンプリング毎の算出結果にばらつき（誤差）が大きいため、当該相関係数Ｋｎをそのまま次回サンプリング時のＺ軸方向の加速度α_zに対する補正（Ｚ軸方向の加速度α_zに混入しているＸ軸方向の加速度α_xを除去すること）に用いることはできない。

従って速度算出部５２では、誤差を含んだ数多くの相関係数Ｋｎを用いて、誤差の少ない最終的な相関係数Ｋを得る必要がある。例えば、単純な方法としては、サンプリング毎に得られる相関係数Ｋｎに対して、過去一定時間における全ての相関係数Ｋｎに対して平均値を算出することにより最終的な相関係数Ｋを求めることが考えられる。

だが、この方法では、過去一定時間における全ての相関係数Ｋｎを保存しておくバッファが必要であること、Ｘ軸方向の加速度｜α_x｜が「０」よりも非常に大きければ（閾値ＴＨよりも大きければ）相関係数Ｋｎの誤差は小さくなるが、その点が考慮されておらず非効率的であることの２点のデメリットがある。

そこで速度算出部５２では、最終的な相関係数Ｋを算出するためＩＩＲ(Infinite Impulse Response)フィルタでなる新たな相関係数学習部（後述する）を用いることにより、過去一定時間における全ての相関係数Ｋｎを保存しておくバッファを不要とするようになされている。

この相関係数学習部では、最終的な相関係数Ｋを、次式

によって求めるようになされている。

これにより相関係数学習部では、前回の学習結果である相関係数Ｋだけをバッファしておくだけで済むようになされている。なお（７）式では、Ｇａｉｎが所定の固定値である。

さらに相関係数学習部では、このＧａｉｎを固定値にするのではなく、Ｘ軸方向の加速度｜α_x｜の大きさによって可変とすることにより、比較的誤差の小さな相関係数Ｋｎを重視して最終的な相関係数Ｋが一段と早く相応しい値に収束するようになされている。

具体的に相関係数学習部では、（７）式におけるＧａｉｎを、次式

によって求める。

ここで、基準α_xを０．１５[m/S/S]（例えば図４４におけるＸ軸加速度方向に対する２００ｄｉｇｉｔ（２００データ点）に相当）として設定し、基準Ｇａｉｎを１／１００００（例えば本実施の形態における５０［Ｈｚ］のサンプリング周波数では時定数２００秒（１００００／５０＝２００）に相当）に設定する。

すなわち相関係数学習部では、例えば基準α_xを０．１５[m/S/S]、基準Ｇａｉｎを１／１００００に設定することにより、３軸加速度センサ４が過去およそ２００秒以降にサンプリングしたＸ軸方向の加速度｜α_x｜の値を用いてＧａｉｎを求めることになる。

つまり相関係数学習部は、Ｇａｉｎの値を求める際、３軸加速度センサ４の出力値を２００秒毎に更新するようになされている。これにより相関係数学習部は、ＰＮＤ５０本体に対する顎上げ状態が変更（取付角度が変更）されたときのことを考慮し、取付角度が変更される前のあまりにも古い過去のデータを参照して相応しくない誤ったＧａｉｎを算出しないようになされている。

［２−３．速度算出処理］
速度算出部５２は、図８との対応部分に同一符号を付した図４５に示すように、新たにＺ軸方向加速度補正部７０及び相関係数学習部７１が新たに設けられている以外、速度算出部２２と同様に構成されている。

なお、速度算出部５２は、実際上クレードル３に設置され、ＧＰＳ測位区間であって、かつＧＰＳ速度が１．０［ｍ／Ｓ］以上のとき、最終的な相関係数Ｋを求めるようにしている。これにより速度算出部５２は、ＰＮＤ５０本体をユーザが手に持っているときや、ＰＮＤ５０本体の取付角度を調節しているときを除き、車両の走行中に限って相関係数Ｋを求め得るようになされている。

実際上、速度算出部５２では、データ取得部３１において、加速度α_zが示された加速度データＡＤだけでなく、加速度α_xが示された加速度データＡＸについても、Ｙ軸ジャイロセンサ５から供給されるピッチレートデータＰＤと共にハイパスフィルタ部３２へ送出する。

ハイパスフィルタ部３２は、加速度データＡＤ、加速度データＡＸ及びピッチレートデータＰＤの直流成分をカットすることによりオフセット分を除去し、その結果得られる加速度データＡＤ１、加速度データＡＸ１及びピッチレートデータＰＤ１をローパスフィルタ部３３へ送出する。

ローパスフィルタ部３３は、加速度データＡＤ１、加速度データＡＸ１及びピッチレートデータＰＤ１に対して、上述したようなローパスフィルタ処理を施し、その結果得られる加速度データＡＤ２、加速度データＡＸ２をＺ軸方向加速度補正部７０及び相関係数学習部７１へ送出すると共に、ピッチレートデータＰＤ２を速度計算部３４へ送出する。

相関係数学習部７１は、加速度データＡＤ２により示されたＺ軸方向の加速度α_z、加速度データＡＸ２により示されたＸ軸方向の加速度α_xを用いると共に、上述した（６）式乃至（８）式に従って最終的な相関係数Ｋを算出し、それをＺ軸方向加速度補正部７０へ出力する。

Ｚ軸方向加速度補正部７０は、加速度データＡＤ２の加速度α_zと、加速度データＡＸ２の加速度α_xと、相関係数学習部７１から供給された最終的な相関係数Ｋとを基に上述した（４）式に従って補正することにより、真のＺ軸方向の加速度α_z´を求め、その真のＺ軸方向の加速度α_z´が示されたＺ軸方向の加速度データＡＤ３を速度算出部３４へ送出する。

速度計算部３４は、Ｚ軸方向加速度補正部７０から供給された加速度データＡＤ３が示す真のＺ軸方向の加速度α_z´と、ピッチレートデータＰＤ２が示すＹ軸周りのピッチレートω_yとを用いて、上述した（５）式に従って誤差の少ない自律速度Ｖを算出し、当該自律速度Ｖを表す速度データＶＤ１を平滑化及びノイズ除去部３５へ送出する。

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

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

このようにして第２の実施の形態による速度算出部５２は、Ｚ軸方向の加速度α_zに混入しているＸ軸方向の加速度α_xを予め除去した真のＺ軸方向の加速度α_z´を用いて、ＰＮＤ５０本体が顎上げ状態に取付けられている場合でも、一段と高精度な自律速度Ｖを算出し得るようになされている。

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

このときＰＮＤ５０は、ＰＮＤ５０本体が顎上げ状態で使用された場合、３軸加速度センサ４によって検出された加速度α_zにＸ軸方向の加速度α_xが混入してしまうことを予め考慮し、その混入度合いを最終的な相関係数Ｋとして予め学習する。

そしてＰＮＤ５０は、（４）式に従ってＸ軸方向の加速度α_xをＺ軸方向の加速度α_zから予め除去した真のＺ軸方向の加速度α_z´を求めた後、（５）式に従ってＺ軸方向の加速度α_z´とＹ軸周りのピッチレートω_yとを基に一段と正確な自律速度Ｖを算出する。

従ってＰＮＤ５０は、第１の実施の形態に比べて、ＰＮＤ５０本体が顎上げ状態に取付けられたときでも、その取付角度による誤差を予め除去した高精度な自律速度Ｖを算出することができる。

なお速度算出部５２では、Ｘ軸方向の加速度α_xの値が「０」付近の場合、ノイズが支配的になってしまうことを考慮し、自律速度Ｖの算出に際しては、加速度｜α_x｜が所定の閾値ＴＨよりも大きいときに限り、相関係数Ｋを求めるようにした。

これによりＰＮＤ５０は、Ｘ軸方向の加速度α_xをＺ軸方向の加速度α_zから予め除去することにより真のＺ軸方向の加速度α_z´を求める際、ノイズの影響を予め排除し、一段と正確な自律速度Ｖを算出することができる。

また速度算出部５２では、（８）式に従ってＧａｉｎを算出する際、基準Ｇａｉｎの設定に応じて、自律速度Ｖを算出するために用いられる３軸加速度センサ４の出力値の更新タイミングを調整するようにした。

これによりＰＮＤ５０は、ＰＮＤ５０本体に対する顎上げ状態が微調整（取付角度が変更）されたときにも、取付角度が変更される前のあまりにも古い過去のデータを引きずることなく、最新のデータだけを用いて正確な自律速度Ｖを算出することができる。

以上の構成によれば、第２の実施の形態によるＰＮＤ５０は、ＰＮＤ５０本体が顎上げ状態であった場合でも、当該ＰＮＤ５０本体の取付角度により生じる誤差を予め考慮した高精度の自律速度Ｖを算出することができる。

＜３．他の実施の形態＞
なお上述した第１の実施の形態においては、速度Ｖを計算する際、加速度データＡＤ２に相当する加速度α_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の例えば２５データ点毎の平均値を取ることにより、速度Ｖを１秒当たり２回だけ算出することになる。これによりＰＮＤ１、５０の制御部１１は、速度算出処理に対する処理負荷を軽減することができる。

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

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

さらに上述した第１及び第２の実施の形態においては、加速度α_z及びピッチレートω_yを用いて例えば前回の位置Ｐ０の速度Ｖを算出する際、当該前回の位置Ｐ０の速度Ｖが過大であると判断した場合、前値速度Ｖ_n-1を前回の位置Ｐ０の速度Ｖとするようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０の速度算出部２２、５２は、前回の位置Ｐ０の速度Ｖが前値速度Ｖ_n-1より所定の閾値以上大きかった場合、前回の位置Ｐ０の速度Ｖを、前値速度Ｖ_n-1に車両が加速できるであろう分を加算した値とするようにしてもよい。

またＰＮＤ１の速度算出部２２は、前回の位置Ｐ０の速度Ｖが前値速度Ｖ_n-1より所定の閾値以上小さかった場合、前回の位置Ｐ０の速度Ｖを、前値速度Ｖ_n-1に車両が減速できるであろう分を減算した値とするようにしてもよい。

さらに上述した第１の実施の形態においては、加速度α_z及びピッチレートω_yを基に、（３）式を用いて速度Ｖを算出するようにした場合について述べた。

しかしながら本発明はこれに限らず、ＰＮＤ１の制御部１１は、加速度α_z及びピッチレートω_yを基に（３）式を用いて算出した速度Ｖと、ＧＰＳ信号を基に算出したＧＰＳ速度Ｖ_gとを比較する。

そしてＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖ_gとに誤差が生じていた場合、例えば速度Ｖとの誤差が最小となるように一次関数や２次以上の高次関数等により補正するための補正係数を算出し、記憶部１２へ当該補正係数を記憶する。

従ってＰＮＤ１の速度算出部２２は、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度α_z及びピッチレートω_yを基に（３）式を用いて速度Ｖを算出した後、記憶部１２から補間係数を読み出し、当該補間係数を用いて当該速度Ｖを一次関数や２次以上の高次関数等により補正する。

このようにＰＮＤ１は、ＧＰＳ信号を基に算出したＧＰＳ速度Ｖ_gを基に速度Ｖの補正係数を予め学習しておくことにより、当該速度Ｖの算出精度をより向上させることができる。

なお、ＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖ_gとの補正係数を算出する際、例えば超低速、低速、中速及び高速等のような複数の速度領域に速度Ｖを分割し、当該複数の速度領域ごとに補正係数を算出するようにしても良い。

またＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖ_gとの補正係数を算出する際、所定例えば時速６０［ｋｍ］以上の高速時に対してだけ、補正係数を算出するようにしても良い。

さらに上述した第２の実施の形態においては、当該ＰＮＤ５０が車両の進行方向であるＸ軸に沿って背面側へ倒れるように、Ｙ軸（図５）を中心としてＱ度（例えば１２０度）だけ傾けられた顎上げ状態で使用されたときの影響を予め除去して速度Ｖを算出するようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ５０本体がロール方向に傾けられた状態（所謂右肩上がり状態又は左肩上がり状態）で使用されたときの影響を予め除去して自律速度Ｖ_yを算出するようにしても良い。

この場合、ＰＮＤ５０では、Ｙ軸ジャイロセンサ５によるＹ軸回りのピッチレートω_yに対してＺ軸ジャイロセンサ６によるＺ軸回りのヨー角速度（ヨーレート）ω_zが混入してしまう。

これによりＰＮＤ５０は、ピッチレートω_yが本来よりも大きな値となって、速度Ｖ_yの算出結果に誤差を生じさせてしまう（自律速度が本来よりも小さくなる）ことになる。

そこでＰＮＤ５０は、Ｙ軸回りのピッチレートω_yに混入しているＺ軸回りのヨーレートω_zの混入度合いを相関係数Ｋ_yとして学習し、次式

により、Ｚ軸回りのヨーレートω_zをＹ軸回りのピッチレートω_yから予め除去した真のＹ軸回りのピッチレートω_y´を求める。

この場合ＰＮＤ５０では、Ｚ軸回りのヨーレートω _ｚ の値が「０」付近の場合、僅かなノイズによっても当該ヨーレートω_zの値に大きな影響を与え、当該ノイズが支配的になってしまうので、自律速度Ｖ_yの算出に際しては、ヨーレート｜ω_z｜が所定の閾値ＴＨ_y（例えば１[deg/S]）よりも大きいときに限り、相関係数Ｋ_yを求める。

実際上、ＰＮＤ５０の速度算出部（後述する）では、Ｙ軸回りのピッチレートω_yとＺ軸回りのヨーレートω_zとの傾きを表す相関係数Ｋ_yを予め学習により求め、それを用いてＹ軸回りのピッチレートω_yに混入しているＺ軸回りのヨーレートω_zを除去することにより真のＹ軸周りのピッチレートω_y´を求める。

そして速度算出部では、そのＺ軸方向の加速度α_zと、真のＹ軸周りのピッチレートω_y´とを用いて、次式

により自律速度Ｖ_yを算出するようになされている。

ここで速度算出部は、相関係数Ｋ_yを予め学習により求めるに当って、例えばｎ回目のサンプリング結果に基づいて算出した相関係数Ｋ_yｎを、次式

によって算出する。

しかしながら、この相関係数Ｋ_yｎは、サンプリング毎の算出結果にばらつき（誤差）が大きいため、当該相関係数Ｋ_yｎをそのまま次回サンプリング時のＹ軸回りのピッチレートω_yに対する補正（Ｙ軸回りのピッチレートω_yに混入しているＺ軸回りのヨーレートω_zを除去すること）に用いることはできない。

従って速度算出部では、誤差を含んだ数多くの相関係数Ｋ_yｎを用いて、誤差の少ない最終的な相関係数Ｋ_yを得る必要がある。例えば、単純な方法としては、サンプリング毎に得られる相関係数Ｋｎに対して、過去一定時間における全ての相関係数Ｋｎに対して平均値を算出することにより最終的な相関係数Ｋ_yを求めることが考えられる。

だが、この方法では、過去一定時間における全ての相関係数Ｋ_yｎを保存しておくバッファが必要であること、Ｚ軸回りヨーレート｜ω_z｜が「０」よりも非常に大きければ（閾値ＴＨ_yよりも大きければ）相関係数Ｋ_yｎの誤差は小さくなるが、その点が考慮されておらず非効率的であることの２点のデメリットがある。

そこで速度算出部では、最終的な相関係数Ｋ_yを算出するためＩＩＲフィルタでなる新たな相関係数学習部（後述する）を用いることにより、過去一定時間における全ての相関係数Ｋ_yｎを保存しておくバッファを不要とする。

この相関係数学習部では、最終的な相関係数Ｋ_yを、次式

によって求めるようになされている。

これにより相関係数学習部では、前回の学習結果である相関係数Ｋ_yだけをバッファしておくだけで済むようになされている。なお（１２）式では、Ｇａｉｎ_yが所定の固定値である。

さらに相関係数学習部では、このＧａｉｎ_yを固定値にするのではなく、Ｚ軸回りのヨーレート｜ω_z｜の大きさによって可変とすることにより、比較的誤差の小さな相関係数Ｋ_yｎを重視して最終的な相関係数Ｋ_yが一段と早く相応しい値に収束するようになされている。

具体的に相関係数学習部では、（１２）式におけるＧａｉｎ_yを、次式

によって求める。

ここで、（１３）式における意味は、上述した（８）式において説明した趣旨と同様であるため、ここでは便宜上その説明を省略する。

実際上、図４５との対応部分に同一符号を付した図４６に示すように、速度算出部９２は、速度算出部５２のＺ軸方向加速度補正部７０に代えてピッチレート補正部１００を新たに設け、速度算出部５２の相関係数学習部７１に代えて相関係数学習部１０１が新たに設けられている以外、速度算出部５２と同様に構成されている。

速度算出部９２では、データ取得部３１において、加速度α_zが示された加速度データＡＤだけでなく、Ｚ軸回りのヨーレートω_zが示されたヨーレートデータＡＺについても、Ｙ軸ジャイロセンサ５から供給されるＹ軸回りのピッチレートω_yが示されたピッチレートデータＰＤと共にハイパスフィルタ部３２へ送出する。

ハイパスフィルタ部３２は、加速度データＡＤ、ヨーレートＡＺ及びピッチレートデータＰＤの直流成分をカットすることによりオフセット分を除去し、その結果得られる加速度データＡＤ１、ヨーレートデータＡＺ１及びピッチレートデータＰＤ１をローパスフィルタ部３３へ送出する。

ローパスフィルタ部３３は、加速度データＡＤ１、ヨーレートデータＡＺ１及びピッチレートデータＰＤ１に対して、上述したようなローパスフィルタ処理を施し、その結果得られる加速度データＡＤ２を速度計算部３４へ送出する一方、ローパスフィルタ処理を施した結果得られるピッチレートデータＰＤ２及びヨーレートデータＡＺ２については相関係数学習部１０１及びピッチレート補正部１００へ送出する。

相関係数学習部１０１は、ピッチレートデータＰＤ２により示されるＹ軸回りのピッチレートω_yと、ヨーレートデータＡＺ２により示されるＺ軸回りのヨーレートω_zとを用いると共に、上述した（１１）式乃至（１３）式に従って最終的な相関係数Ｋ_yを算出し、それをピッチレート補正部１００へ出力する。

ピッチレート補正部１００は、ピッチレートデータＰＤ２のＹ軸回りのピッチレートω_yと、ヨーレートデータＡＺ２のヨーレートω_zと、相関係数学習部１０１から供給された最終的な相関係数Ｋ_yとを基に上述した（９）式に従って補正することにより、真のＹ軸回りのピッチレートω_y´を求め、そのピッチレートω_y´が示されたピッチレートデータＰＤ３を速度計算部３４へ送出する。

速度計算部３４は、ピッチレート補正部１００から供給されたピッチレートデータＰＤ３が示す真のＹ軸回りのピッチレートω_y´と、加速度データＡＤ２が示すＺ軸方向の加速度α_zとを用いて、上述した（１０）式に従って誤差の少ない自律速度Ｖ_yを算出し、当該自律速度Ｖ_yを表す速度データＶＤ２を平滑化及びノイズ除去部３５へ送出する。

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

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

このようにして速度算出部９２は、Ｙ軸回りのピッチレートω_yに混入しているＺ軸回りのヨーレートω_zを予め除去した真のＹ軸回りのピッチレートω_y´を用いて、ＰＮＤ５０本体がロール方向に傾けられた状態に取付けられている場合でも、一段と高精度な自律速度Ｖ_yを算出することができる。

なお、ＰＮＤ５０においては、ＰＮＤ５０本体が顎上げ状態で使用されたときの影響と、ＰＮＤ５０本体がロール方向に傾けられた状態（所謂右肩上がり状態又は左肩上がり状態）で使用されたときの影響との双方を予め除去して自律速度Ｖを算出するようにしても良い。

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

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

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

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

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

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

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

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

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

さらに上述した実施の形態においては、垂直方向加速度検出部としての３軸加速度センサ４、水平方向角速度検出部としてのＹ軸ジャイロセンサ５、相関係数学習部としての相関係数学習部７１、真垂直方向加速度検出部としてのＺ軸方向加速度補正部７０及び速度計算部としての速度計算部３４によって本発明の速度算出装置としてのＰＮＤ１、５０を構成するようにした場合について述べた。本発明はこれに限らず、この他種々の構成でなる垂直方向加速度検出部、水平方向角速度検出部、相関係数学習部、真垂直方向加速度検出部及び速度算出部によって速度算出装置を構成するようにしても良い。

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

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

Claims (5)

1. 移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、
   上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、
   上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、
   上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、
   上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度計算部と
   を有し、
   上記真垂直方向加速度検出部は、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
   上記相関係数算出部は、上記進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
   さらに、上記相関係数算出部は、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
   速度算出装置。
2. 上記本体部の取付使用角度に応じて水平軸周りの角速度に混入した上記移動体における
   Ｚ軸回りのヨー角速度の混入度合を第２相関係数として求める第２相関係数算出部と、
   上記第２相関係数に基づいて算出した上記水平軸回りの角速度に混入している上記ヨー
   角速度を当該水平軸回りの角速度から減算することにより真の水平軸回りの角速度を求め
   る真水平軸回り角速度検出部と
   を具え、
   上記速度計算部は、上記真の垂直方向の加速度及び上記真の水平軸周りの角速度に基づ
   いて上記移動体の進行方向の速度を算出する
   請求項１に記載の速度算出装置。
3. 上記第２相関係数算出部は、上記Ｚ軸回りのヨー角速度が所定の閾値よりも大きいとき
   に限り、上記第２相関係数を求める
   請求項２に記載の速度算出装置。
4. 垂直方向加速度取得部により、移動体の接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得ステップと、
   水平方向角速度取得部により、上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得ステップと、
   上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数算出部によって相関係数として求める相関係数算出ステップと、
   上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて所定の真垂直方向加速度検出部により真の垂直方向の加速度を求める真垂直方向加速度検出ステップと、
   速度計算部により上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度算出ステップと
   を有し、
   上記真垂直方向加速度検出ステップでは、上記真垂直方向加速度検出部が、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
   上記相関係数算出ステップでは、上記相関係数算出部が、上記進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
   さらに、上記相関係数算出ステップでは、上記相関係数算出部が、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
   速度算出方法。
5. 移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、
   上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、
   上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、
   上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、
   上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度計算部と、
   上記進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、
   上記垂直軸周りの角速度に基づいて上記移動体が旋回した角度を算出する角度算出部と、
   上記速度計算部により算出された上記進行方向の速度と、上記角度算出部により算出された上記角度とに基づいて、上記移動体の位置を算出する位置算出部と
   を有し、
   上記真垂直方向加速度検出部は、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
   上記相関係数算出部は、上記進行方向の加速度が所定の閾値よりも大きいときに限り、
   サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
   さらに、上記相関係数算出部は、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
   ナビゲーション装置。

Images