(57)【特許請求の範囲】
【請求項１】
移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、
上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、
上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、
上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、
上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度計算部と
を有し、
上記真垂直方向加速度検出部は、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
上記相関係数算出部は、上記進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
さらに、上記相関係数算出部は、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
速度算出装置。
【請求項２】
上記本体部の取付使用角度に応じて水平軸周りの角速度に混入した上記移動体における
Ｚ軸回りのヨー角速度の混入度合を第２相関係数として求める第２相関係数算出部と、
上記第２相関係数に基づいて算出した上記水平軸回りの角速度に混入している上記ヨー
角速度を当該水平軸回りの角速度から減算することにより真の水平軸回りの角速度を求め
る真水平軸回り角速度検出部と
を具え、
上記速度計算部は、上記真の垂直方向の加速度及び上記真の水平軸周りの角速度に基づ
いて上記移動体の進行方向の速度を算出する
請求項１に記載の速度算出装置。
【請求項３】
上記第２相関係数算出部は、上記Ｚ軸回りのヨー角速度が所定の閾値よりも大きいとき
に限り、上記第２相関係数を求める
請求項２に記載の速度算出装置。
【請求項４】
垂直方向加速度取得部により、移動体の接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得ステップと、
水平方向角速度取得部により、上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得ステップと、
上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数算出部によって相関係数として求める相関係数算出ステップと、
上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて所定の真垂直方向加速度検出部により真の垂直方向の加速度を求める真垂直方向加速度検出ステップと、
速度計算部により上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度算出ステップと
を有し、
上記真垂直方向加速度検出ステップでは、上記真垂直方向加速度検出部が、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
上記相関係数算出ステップでは、上記相関係数算出部が、上記進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
さらに、上記相関係数算出ステップでは、上記相関係数算出部が、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
速度算出方法。
【請求項５】
移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、
上記接触面のうねりに応じて発生する上記移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、
上記移動体に搭載されたときの本体部の取付使用角度に応じて、上記垂直方向の加速度に混入した上記移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、
上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、
上記真の垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記移動体の進行方向の速度を算出する速度計算部と、
上記進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、
上記垂直軸周りの角速度に基づいて上記移動体が旋回した角度を算出する角度算出部と、
上記速度計算部により算出された上記進行方向の速度と、上記角度算出部により算出された上記角度とに基づいて、上記移動体の位置を算出する位置算出部と
を有し、
上記真垂直方向加速度検出部は、上記相関係数に基づいて算出した上記垂直方向の加速度に混入している上記進行方向の加速度を当該垂直方向の加速度から減算することにより上記真の垂直方向の加速度を求め、
上記相関係数算出部は、上記進行方向の加速度が所定の閾値よりも大きいときに限り、
サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、
さらに、上記相関係数算出部は、上記ローパスフィルタにおけるゲインを上記進行方向の加速度の大きさに応じて可変とする
ナビゲーション装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、速度算出装置、速度算出方法及びナビゲーション装置に関し、例えば携帯型ナビゲーション装置に適用して好適なものである。
【背景技術】
【０００２】
従来、ナビゲーション装置においては、複数のＧＰＳ（Global Positioning System）衛星からの測位信号（以下、これをＧＰＳ信号とも呼ぶ）を受信し、当該ＧＰＳ信号に基づいて車両の現在位置を算出するようになされている。
【０００３】
しかしながらこのようなナビゲーション装置では、当該ナビゲーション装置を載置した車両が例えばトンネルや地下駐車場等の中に入った場合、ＧＰＳ衛星からのＧＰＳ信号を受信できず、従って、当該ＧＰＳ信号に基づいて車両の現在位置を算出することができない。
【０００４】
そこでナビゲーション装置のなかには、ＧＰＳ信号が受信できなくなった場合であっても、コーナリング時において、車両の進行方向に直交した水平方向の加速度と、当該進行方向に直交した垂直軸周りの角速度とを基に進行方向の速度を算出し、当該進行方向の速度に基づいて現在位置を算出するものがある（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００８−７６３８９公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで上述したナビゲーション装置では、コーナリング時においては進行方向の速度を算出することができるものの、直進時においては進行方向の速度を算出することができないため、必ずしも全ての道路環境下で進行方向の速度を算出することができないという問題があった。
【０００７】
本発明は以上の点を考慮してなされたもので、道路環境に関わらず、全ての道路環境下で車両の速度を高精度に算出し得る速度算出装置、速度算出方法及びナビゲーション装置を提案しようとするものである。
【課題を解決するための手段】
【０００８】
かかる課題を解決するため本発明の速度算出装置においては、移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、接触面のうねりに応じて発生する移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、移動体に搭載されたときの本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出する速度計算部とを設け、真垂直方向加速度検出部が、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求め、相関係数算出部が、進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、さらに、相関係数算出部が、ローパスフィルタにおけるゲインを進行方向の加速度の大きさに応じて可変とするようにした。
【０００９】
また本発明の速度算出方法においては、垂直方向加速度取得部により、移動体の接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得ステップと、水平方向角速度取得部により、接触面のうねりに応じて発生する移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得ステップと、移動体に搭載されたときの本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数算出部によって相関係数として求める相関係数算出ステップと、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度に基づいて所定の真垂直方向加速度検出部により真の垂直方向の加速度を求める真垂直方向加速度検出ステップと、速度計算部により真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出する速度算出ステップとを有し、真垂直方向加速度検出ステップでは、真垂直方向加速度検出部が、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求め、相関係数算出ステップでは、相関係数算出部が、進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、さらに、相関係数算出ステップでは、相関係数算出部が、ローパスフィルタにおけるゲインを進行方向の加速度の大きさに応じて可変とするようにした。
【００１０】
これにより、移動体に搭載された本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求め、その相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求めた後、真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出することができるので、本体部の取付使用角度が傾けられた状態でも、傾けられていないときと同様の正確な速度を算出することができる。
【００１１】
さらに本発明のナビゲーション装置においては、移動体との接触面のうねりに応じて発生する垂直方向の加速度を取得する垂直方向加速度取得部と、接触面のうねりに応じて発生する移動体の進行方向と直交した水平軸周りの角速度を取得する水平方向角速度取得部と、移動体に搭載されたときの本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求める相関係数算出部と、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度に基づいて真の垂直方向の加速度を求める真垂直方向加速度検出部と、真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出する速度計算部と、進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、垂直軸周りの角速度に基づいて移動体が旋回した角度を算出する角度算出部と、速度計算部により算出された進行方向の速度と、角度算出部により算出された角度とに基づいて、移動体の位置を算出する位置算出部とを設け、真垂直方向加速度検出部が、相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求め、相関係数算出部が、進行方向の加速度が所定の閾値よりも大きいときに限り、サンプリング毎に求まる新しい相関係数を入力とし、ローパスフィルタによる処理を行うことにより最終的な相関係数を求め、さらに、相関係数算出部が、ローパスフィルタにおけるゲインを進行方向の加速度の大きさに応じて可変とするようにした。
【００１２】
これにより、移動体に搭載された本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求め、その相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求めた後、真の垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を算出し、当該進行方向の速度と、角度算出部により算出された角度とに基づいて移動体の位置を算出することができるので、本体部の取付使用角度が傾けられた状態でも、傾けられていないときと同様の正確な車両の位置を算出することができる。
【発明の効果】
【００１３】
本発明によれば、移動体に搭載された本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求め、その相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求めた後、真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出することができるので、本体部の取付使用角度が傾けられた状態でも、傾けられていないときと同様の正確な速度を算出し得る速度算出装置及び速度算出方法を実現することができる。
【００１４】
また本発明によれば、移動体に搭載された本体部の取付使用角度に応じて、垂直方向の加速度に混入した移動体における進行方向の加速度の混入度合を相関係数として求め、その相関係数に基づいて算出した垂直方向の加速度に混入している進行方向の加速度を当該垂直方向の加速度から減算することにより真の垂直方向の加速度を求めた後、真の垂直方向の加速度及び水平軸周りの角速度に基づいて移動体の進行方向の速度を算出し、当該進行方向の速度と、角度算出部により算出された角度とに基づいて移動体の位置を算出することができるので、本体部の取付使用角度が傾けられた状態でも、傾けられていないときと同様の正確な移動体の位置を算出し得るナビゲーション装置を実現することができる。
【図面の簡単な説明】
【００１５】
【図１】凹凸路面の走行時の様子を示す略線図である。
【図２】カーブ走行時の様子を示す略線図である。
【図３】速度及び角度を用いた現在位置算出手法を示す略線図である。
【図４】ＰＮＤの全体構成を示す略線図である。
【図５】ＰＮＤの座標系の定義を示す略線図である。
【図６】ＰＮＤにおけるセンサ構成を示す略線図である。
【図７】ＰＮＤの回路構成を示す略線図である。
【図８】速度算出部の構成を示す略線図である。
【図９】高度及び角度の関係を示す略線図である。
【図１０】低速時における路面角度の様子を示す略線図である。
【図１１】高速時における路面角度の様子を示す略線図である。
【図１２】超低速時における路面角度の様子を示す略線図である。
【図１３】クレードルによる振動の様子を示す略線図である。
【図１４】ハイパスフィルタ処理後の加算加速度及び加算角速度を示す略線図である。
【図１５】４０９６データ点ごとにフーリエ変換を施した加算角速度を示す略線図である。
【図１６】４０９６データ点ごとにフーリエ変換を施した加算加速度を示す略線図である。
【図１７】加算加速度に対するローパスフィルタ処理の比較を示す略線図である。
【図１８】加算角速度に対するローパスフィルタ処理の比較を示す略線図である。
【図１９】低速時におけるフロント加速度及びリア加速度の関係を示す略線図である。
【図２０】中速及び高速時におけるフロント加速度及びリア加速度の関係を示す略線図である。
【図２１】３種類の設置位置による加速度、ピッチレート及び速度のシミュレーション例を示す略線図である。
【図２２】最大値及び最小値の関係を示す略線図である。
【図２３】速度とデータ点数との関係を示す略線図である。
【図２４】異なる円弧の長さによる加速度及びピッチレートの様子を示す略線図である。
【図２５】速度算出処理を用いた現在位置算出処理手順の説明に供するフローチャートである。
【図２６】加速度、角速度及び速度の測定結果例を示す略線図である。
【図２７】測定結果とリファレンスとの比較（１）を示す略線図である。
【図２８】測定結果とリファレンスとの比較（２）を示す略線図である。
【図２９】測定結果とリファレンスとの比較（３）を示す略線図である。
【図３０】測定結果とリファレンスとの比較（４）を示す略線図である。
【図３１】測定結果とリファレンスとの比較（５）を示す略線図である。
【図３２】カーブ時における測定結果とリファレンスとの比較（１）を示す略線図である。
【図３３】カーブ時における測定結果とリファレンスとの比較（２）を示す略線図である。
【図３４】カーブ時における測定結果とリファレンスとの比較（３）を示す略線図である。
【図３５】地図上の行路と走行軌跡との比較を示す略線図である。
【図３６】軽車両に載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図３７】ミニバンに載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図３８】第２の実施の形態による顎上げ状態を示す略線図である。
【図３９】顎上げ状態におけるＧＰＳ速度と自律速度との速度比を示す略線図である。
【図４０】異常値ＥＶ１の出力場所を示す略線図である。
【図４１】異常値ＥＶ２の出力場所を示す略線図である。
【図４２】異常値ＥＶ３の出力場所を示す略線図である。
【図４３】速度算出処理で求めた自律速度とＧＰＳ速度との比較結果を示す略線図である。
【図４４】Ｘ軸加速度とＺ軸加速度との相関性を示す略線図である。
【図４５】第２の実施の形態による速度算出部の光栄を示す略線図である。
【図４６】他の実施の形態による速度算出部の構成を示す略線図である。
【図４７】他の実施の形態による使用例を示す略線図である。
【発明を実施するための形態】
【００１６】
以下、発明を実施するための形態（以下実施の形態とする）について、図面を用いて説明する。なお、説明は以下の順序で行う。
１．第１の実施の形態（ナビゲーション装置・取付傾きの生じていない例）
２．第２の実施の形態（ナビゲーション装置・取付傾きによって混入する不要な加速度成分による影響を除去する例）
３．他の実施の形態
【００１７】
＜１．第１の実施の形態＞
［１−１．基本原理］
本発明においてはナビゲーション装置として携帯型ナビゲーション装置（以下、これをＰＮＤ（Personal Navigation Device）とも呼ぶ）を用い、そのＰＮＤにより車両の速度及び現在位置を算出する基本原理について説明する。
【００１８】
［１−１−１．速度算出原理］
実際上、道路を走行中の車両は、平らな道路を走行することが殆どなく、図１（Ａ）に示すような全体として凹状の道路、及び図１（Ｂ）に示すような全体として凸状の道路を走行するのが実状である。
【００１９】
ここで車両の座標系は、当該車両の前後方向をＸ軸、当該Ｘ軸に直交した水平方向をＹ軸、上下方向をＺ軸によって表される。
【００２０】
この車両の例えばダッシュボード上に載置されたＰＮＤは、車両が凹状の道路（図１（Ａ））を走行したとき、当該ＰＮＤに設けられた３軸加速度センサによって、Ｚ軸に沿った下方向の加速度αzを例えば５０［Ｈｚ］のサンプリング周波数で検出する。
【００２１】
またＰＮＤは、当該ＰＮＤに設けられたＹ軸ジャイロセンサによって進行方向に直交したＹ軸周りの角速度（以下、これをピッチレートとも呼ぶ）ωyを５０［Ｈｚ］のサンプリング周波数で検出する。
【００２２】
ここでＰＮＤでは、Ｚ軸に沿った下方向の加速度αzを正と定義し、また図１（Ａ）に示すような凹状の路面に沿って形成される仮想上の円を進行方向に対して上向きに縦回転する際のピッチレートωyを正と定義している。
【００２３】
このＰＮＤでは、３軸加速度センサによって検出した加速度αz及びＹ軸ジャイロセンサによって検出したピッチレートωyを用い、次式
【００２４】
【数１】
0005482047.tif 000002
【００２５】
によって進行方向の速度Ｖを１秒間当たり５０回、算出し得るようになされている。
【００２６】
またＰＮＤは、車両が凸状の道路（図１（Ｂ））を走行するとき、当該ＰＮＤに設けられた３軸加速度センサによってＺ軸に沿った上方向の加速度αz’を例えば５０［Ｈｚ］のサンプリング周波数で検出し、また当該ＰＮＤに設けられたＹ軸ジャイロセンサによってＹ軸周りのピッチレートωy’を例えば５０［Ｈｚ］のサンプリング周波数で検出する。
【００２７】
そしてＰＮＤでは、３軸加速度センサによって検出した加速度αz’及びＹ軸ジャイロセンサによって検出したピッチレートωy’を用い、次式
【００２８】
【数２】
0005482047.tif 000003
【００２９】
によって進行方向の速度Ｖ’を１秒間当たり５０回、算出し得るようになされている。
【００３０】
ここでは説明の便宜上、負の加速度α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とを用い、上述した（１）式を変形した
【０１１３】
【数３】
0005482047.tif 000004
【０１１４】
によって前回の位置Ｐ０（図３）での進行方向の速度Ｖを算出し、その結果得られる速度データＶＤ１を平滑化及びノイズ除去部３５へ送出する。
【０１１５】
すなわち速度計算部３４は、（３）式を用いることにより、加速度α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の混入度合いを相関係数Ｋ（後述する）として学習し、次式
【０２０４】
【数４】
0005482047.tif 000005
【０２０５】
により、Ｘ軸方向の加速度αxをＺ軸方向の加速度αzから予め除去した真のＺ軸方向の加速度αz´を求める必要がある。
【０２０６】
これによりＰＮＤ５０は、ＧＰＳ速度Ｖgと自律速度Ｖとの誤差を減少させ、図３９に示した速度比ＶＰ（ＧＰＳ速度Ｖg／自律速度Ｖ）が経過時間に拘わらず理想的な「１」の固定値に近づけ得るようになされている。
【０２０７】
なお、ＰＮＤ５０では、Ｘ軸方向の加速度αxの値が「０」付近の場合、僅かなノイズによっても当該加速度αxの値に大きな影響を与え、当該ノイズが支配的になってしまうので、自律速度Ｖの算出に際しては、加速度｜αx｜が所定の閾値ＴＨ（例えば０．０７５[m/S/S]）よりも大きいときに限り、相関係数Ｋを求めるようになされている。
【０２０８】
［２−２．速度算出原理］
速度算出部５２では、Ｚ軸方向の加速度αzとＸ軸方向の加速度αxとの傾きを表す相関係数Ｋを予め学習により求め、それを用いてＺ軸方向の加速度αzに混入しているＸ軸方向の加速度αxを除去することにより真のＺ軸方向の加速度αz´を求める。
【０２０９】
そして速度算出部５２では、そのＺ軸方向の加速度αz´とＹ軸周りのピッチレートωyとを用いて、次式
【０２１０】
【数５】
0005482047.tif 000006
【０２１１】
により自律速度Ｖを算出するようになされている。
【０２１２】
ここで速度算出部５２は、相関係数Ｋを予め学習により求めるに当って、例えばｎ回目のサンプリング結果に基づいて算出した相関係数Ｋｎを、次式
【０２１３】
【数６】
0005482047.tif 000007
【０２１４】
によって算出する。
【０２１５】
しかしながら、この相関係数Ｋｎは、サンプリング毎の算出結果にばらつき（誤差）が大きいため、当該相関係数Ｋｎをそのまま次回サンプリング時のＺ軸方向の加速度αzに対する補正（Ｚ軸方向の加速度αzに混入しているＸ軸方向の加速度αxを除去すること）に用いることはできない。
【０２１６】
従って速度算出部５２では、誤差を含んだ数多くの相関係数Ｋｎを用いて、誤差の少ない最終的な相関係数Ｋを得る必要がある。例えば、単純な方法としては、サンプリング毎に得られる相関係数Ｋｎに対して、過去一定時間における全ての相関係数Ｋｎに対して平均値を算出することにより最終的な相関係数Ｋを求めることが考えられる。
【０２１７】
だが、この方法では、過去一定時間における全ての相関係数Ｋｎを保存しておくバッファが必要であること、Ｘ軸方向の加速度｜αx｜が「０」よりも非常に大きければ（閾値ＴＨよりも大きければ）相関係数Ｋｎの誤差は小さくなるが、その点が考慮されておらず非効率的であることの２点のデメリットがある。
【０２１８】
そこで速度算出部５２では、最終的な相関係数Ｋを算出するためＩＩＲ(Infinite Impulse Response)フィルタでなる新たな相関係数学習部（後述する）を用いることにより、過去一定時間における全ての相関係数Ｋｎを保存しておくバッファを不要とするようになされている。
【０２１９】
この相関係数学習部では、最終的な相関係数Ｋを、次式
【０２２０】
【数７】
0005482047.tif 000008
【０２２１】
によって求めるようになされている。
【０２２２】
これにより相関係数学習部では、前回の学習結果である相関係数Ｋだけをバッファしておくだけで済むようになされている。なお（７）式では、Ｇａｉｎが所定の固定値である。
【０２２３】
さらに相関係数学習部では、このＧａｉｎを固定値にするのではなく、Ｘ軸方向の加速度｜αx｜の大きさによって可変とすることにより、比較的誤差の小さな相関係数Ｋｎを重視して最終的な相関係数Ｋが一段と早く相応しい値に収束するようになされている。
【０２２４】
具体的に相関係数学習部では、（７）式におけるＧａｉｎを、次式
【０２２５】
【数８】
0005482047.tif 000009
【０２２６】
によって求める。
【０２２７】
ここで、基準α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として学習し、次式
【０２７１】
【数９】
0005482047.tif 000010
【０２７２】
により、Ｚ軸回りのヨーレートωzをＹ軸回りのピッチレートωyから予め除去した真のＹ軸回りのピッチレートωy´を求める。
【０２７３】
この場合ＰＮＤ５０では、Ｚ軸回りのヨーレートωｚの値が「０」付近の場合、僅かなノイズによっても当該ヨーレートωzの値に大きな影響を与え、当該ノイズが支配的になってしまうので、自律速度Ｖyの算出に際しては、ヨーレート｜ωz｜が所定の閾値ＴＨy（例えば１[deg/S]）よりも大きいときに限り、相関係数Ｋyを求める。
【０２７４】
実際上、ＰＮＤ５０の速度算出部（後述する）では、Ｙ軸回りのピッチレートωyとＺ軸回りのヨーレートωzとの傾きを表す相関係数Ｋyを予め学習により求め、それを用いてＹ軸回りのピッチレートωyに混入しているＺ軸回りのヨーレートωzを除去することにより真のＹ軸周りのピッチレートωy´を求める。
【０２７５】
そして速度算出部では、そのＺ軸方向の加速度αzと、真のＹ軸周りのピッチレートωy´とを用いて、次式
【０２７６】
【数１０】
0005482047.tif 000011
【０２７７】
により自律速度Ｖyを算出するようになされている。
【０２７８】
ここで速度算出部は、相関係数Ｋyを予め学習により求めるに当って、例えばｎ回目のサンプリング結果に基づいて算出した相関係数Ｋyｎを、次式
【０２７９】
【数１１】
0005482047.tif 000012
【０２８０】
によって算出する。
【０２８１】
しかしながら、この相関係数Ｋyｎは、サンプリング毎の算出結果にばらつき（誤差）が大きいため、当該相関係数Ｋyｎをそのまま次回サンプリング時のＹ軸回りのピッチレートωyに対する補正（Ｙ軸回りのピッチレートωyに混入しているＺ軸回りのヨーレートωzを除去すること）に用いることはできない。
【０２８２】
従って速度算出部では、誤差を含んだ数多くの相関係数Ｋyｎを用いて、誤差の少ない最終的な相関係数Ｋyを得る必要がある。例えば、単純な方法としては、サンプリング毎に得られる相関係数Ｋｎに対して、過去一定時間における全ての相関係数Ｋｎに対して平均値を算出することにより最終的な相関係数Ｋyを求めることが考えられる。
【０２８３】
だが、この方法では、過去一定時間における全ての相関係数Ｋyｎを保存しておくバッファが必要であること、Ｚ軸回りヨーレート｜ωz｜が「０」よりも非常に大きければ（閾値ＴＨyよりも大きければ）相関係数Ｋyｎの誤差は小さくなるが、その点が考慮されておらず非効率的であることの２点のデメリットがある。
【０２８４】
そこで速度算出部では、最終的な相関係数Ｋyを算出するためＩＩＲフィルタでなる新たな相関係数学習部（後述する）を用いることにより、過去一定時間における全ての相関係数Ｋyｎを保存しておくバッファを不要とする。
【０２８５】
この相関係数学習部では、最終的な相関係数Ｋyを、次式
【０２８６】
【数１２】
0005482047.tif 000013
【０２８７】
によって求めるようになされている。
【０２８８】
これにより相関係数学習部では、前回の学習結果である相関係数Ｋyだけをバッファしておくだけで済むようになされている。なお（１２）式では、Ｇａｉｎyが所定の固定値である。
【０２８９】
さらに相関係数学習部では、このＧａｉｎyを固定値にするのではなく、Ｚ軸回りのヨーレート｜ωz｜の大きさによって可変とすることにより、比較的誤差の小さな相関係数Ｋyｎを重視して最終的な相関係数Ｋyが一段と早く相応しい値に収束するようになされている。
【０２９０】
具体的に相関係数学習部では、（１２）式におけるＧａｉｎyを、次式
【０２９１】
【数１３】
0005482047.tif 000014
【０２９２】
によって求める。
【０２９３】
ここで、（１３）式における意味は、上述した（８）式において説明した趣旨と同様であるため、ここでは便宜上その説明を省略する。
【０２９４】
実際上、図４５との対応部分に同一符号を付した図４６に示すように、速度算出部９２は、速度算出部５２のＺ軸方向加速度補正部７０に代えてピッチレート補正部１００を新たに設け、速度算出部５２の相関係数学習部７１に代えて相関係数学習部１０１が新たに設けられている以外、速度算出部５２と同様に構成されている。
【０２９５】
速度算出部９２では、データ取得部３１において、加速度α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を速度Ｖとするようにすればよい。
【０３１３】
さらに上述した第１及び第２の実施の形態においては、ＰＮＤ１、５０の制御部１１が、予め記憶部１２に格納されているアプリケーションプログラムに従い、上述したルーチンＲＴ１の現在位置算出処理手順等を行うようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１、５０の制御部１１が、記憶媒体からインストールしたアプリケーションプログラムや、インターネットからダウンロードしたアプリケーションプログラム、その他種々のルートによってインストールしたアプリケーションプログラムに従って上述した現在位置算出処理手順を行うようにしても良い。
【０３１４】
さらに上述した実施の形態においては、垂直方向加速度検出部としての３軸加速度センサ４、水平方向角速度検出部としてのＹ軸ジャイロセンサ５、相関係数学習部としての相関係数学習部７１、真垂直方向加速度検出部としてのＺ軸方向加速度補正部７０及び速度計算部としての速度計算部３４によって本発明の速度算出装置としてのＰＮＤ１、５０を構成するようにした場合について述べた。本発明はこれに限らず、この他種々の構成でなる垂直方向加速度検出部、水平方向角速度検出部、相関係数学習部、真垂直方向加速度検出部及び速度算出部によって速度算出装置を構成するようにしても良い。
【産業上の利用可能性】
【０３１５】
本発明の速度算出装置、速度算出方法及びナビゲーション装置は、例えば車両以外にも、バイク、電車等のその他種々に実装されたＰＮＤや据置型のナビゲーション装置等に適用することができる。
【符号の説明】
【０３１６】
１、５０……ＰＮＤ、２……表示部、３……クレードル、４……３軸加速度センサ、５……Ｙ軸ジャイロセンサ、６……Ｚ軸ジャイロセンサ、７……気圧センサ、１１……制御部、１２……記憶部、２１……ＧＰＳ処理部、２２、５２……速度算出部、２３……角度算出部、２４……高度算出部、２５……位置算出部、２６……ナビゲーション部、３１……データ取得部、３２、６２……ハイパスフィルタ部、３３、６３……ローパスフィルタ部、３４……速度計算部、３５……平滑化及びノイズ除去部、３６……速度出力部、６７……カットオフ周波数設定部。
【図１】
0005482047.tif 000015
【図２】
0005482047.tif 000016
【図３】
0005482047.tif 000017
【図４】
0005482047.tif 000018
【図５】
0005482047.tif 000019
【図６】
0005482047.tif 000020
【図７】
0005482047.tif 000021
【図８】
0005482047.tif 000022
【図９】
0005482047.tif 000023
【図１０】
0005482047.tif 000024
【図１１】
0005482047.tif 000025
【図１２】
0005482047.tif 000026
【図１３】
0005482047.tif 000027
【図１４】
0005482047.tif 000028
【図１５】
0005482047.tif 000029
【図１６】
0005482047.tif 000030
【図１７】
0005482047.tif 000031
【図１８】
0005482047.tif 000032
【図１９】
0005482047.tif 000033
【図２０】
0005482047.tif 000034
【図２１】
0005482047.tif 000035
【図２２】
0005482047.tif 000036
【図２３】
0005482047.tif 000037
【図２４】
0005482047.tif 000038
【図２５】
0005482047.tif 000039
【図２６】
0005482047.tif 000040
【図２７】
0005482047.tif 000041
【図２８】
0005482047.tif 000042
【図２９】
0005482047.tif 000043
【図３０】
0005482047.tif 000044
【図３１】
0005482047.tif 000045
【図３２】
0005482047.tif 000046
【図３３】
0005482047.tif 000047
【図３４】
0005482047.tif 000048
【図３５】
0005482047.tif 000049
【図３６】
0005482047.tif 000050
【図３７】
0005482047.tif 000051
【図３８】
0005482047.tif 000052
【図３９】
0005482047.tif 000053
【図４０】
0005482047.tif 000054
【図４１】
0005482047.tif 000055
【図４２】
0005482047.tif 000056
【図４３】
0005482047.tif 000057
【図４４】
0005482047.tif 000058
【図４５】
0005482047.tif 000059
【図４６】
0005482047.tif 000060
【図４７】
0005482047.tif 000061