(57)【要約】【課題】本発明は、道路環境に関わらず、全ての道路環境下で車両の速度を高精度に算出する。【解決手段】本発明は、路面のうねりによって発生するＺ軸方向の加速度αz、及び路面のうねりによって発生するＹ軸周りのピッチレートωyをそれぞれ３軸加速度センサ４及びY軸ジャイロセンサ５により検出し、当該加速度αz及びピッチレートωyを用いて速度Ｖを算出することにより、全ての道路環境下で速度Ｖを正確に算出することができる。【選択図】図７
2010078595.tif 000002
【特許請求の範囲】
【請求項１】
車両に搭載され、路面のうねりに応じて発生する垂直方向の加速度を検出する垂直方向加速度検出部と、
上記車両に搭載され、上記路面のうねりに応じて発生する上記車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、
上記垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記車両の進行方向の速度を算出する速度算出部と
を有する速度算出装置。
【請求項２】
垂直方向加速度検出部は、
上記垂直方向の加速度を所定のサンプリング周波数により連続して順次検出し、
水平方向角速度検出部は、
上記水平軸周りの角速度を上記サンプリング周波数により連続して順次検出し、
上記速度算出部は、
所定のサンプリング数における上記垂直方向の加速度、及び上記サンプリング数の上記水平軸周りの角速度を用いて上記車両の上記速度を順次算出する
請求項１に記載の速度算出装置。
【請求項３】
上記速度算出部は、
上記速度が所定速度以上のとき上記サンプリング数を多くし、上記所定速度未満のとき上記サンプリング数を少なくするように、上記サンプリング数を変更する
請求項２に記載の速度算出装置。
【請求項４】
上記速度算出部は、
上記速度が増加する場合と上記速度が減少する場合とで、上記サンプリング数を変更することによりヒステリシス性を持たせる
請求項３に記載の速度算出装置。
【請求項５】
上記速度算出部は、
上記サンプリング数の上記垂直方向の加速度から最大値及び最小値をそれぞれ最大加速度及び最小加速度として抽出し、上記サンプリング数の上記水平軸周りの角速度から最大値及び最小値をそれぞれ最大角速度及び最小角速度として抽出した後、上記最大加速度と上記最小加速度との差を上記最大角速度と上記最小角速度との差で除算することにより上記速度を算出する
請求項４に記載の速度算出装置。
【請求項６】
上記速度算出部は、
上記速度算出装置がクレードルに保持されたときに当該クレードルによって発生する所定の周波数よりも低いローパス用カットオフ周波数が設定され、上記垂直方向の加速度及び上記水平軸周りの角速度から上記ローパス用カットオフ周波数以上の周波数成分を除去するローパスフィルタ
をさらに有する請求項１に記載の速度算出装置。
【請求項７】
上記速度算出部は、
上記路面のうねりに相当する周波数よりも低いハイパス用カットオフ周波数が設定され、上記垂直方向の加速度及び上記水平軸周りの角速度から上記ハイパス用カットオフ周波数以下の周波数成分を除去するハイパスフィルタと、
以前の速度に応じて上記ローパス用カットオフ周波数及び上記ハイパス用カットオフ周波数を設定するカットオフ周波数設定部と
をさらに有する請求項６に記載の速度算出装置。
【請求項８】
上記カットオフ周波数設定部は、
上記ローパス用カットオフ周波数及び上記ハイパス用カットオフ周波数をそれぞれ複数段階に切り替えて設定すると共に、上記ローパス用カットオフ周波数及び上記ハイパス用カットオフ周波数を切り替える際の閾値となる速度の値を、上記速度が以前の速度よりも増加している場合と減少している場合とで相違させる
請求項７に記載の速度算出装置。
【請求項９】
上記カットオフ周波数設定部は、
上記速度に応じた所定の関数に従い上記ローパス用カットオフ周波数及び上記ハイパス用カットオフ周波数をそれぞれ設定する
請求項７に記載の速度算出装置。
【請求項１０】
上記速度算出部は、
上記速度が前回の速度より所定閾値以上の大きな値を取った場合で、かつ上記前回の速度が所定速度未満であった場合、上記速度を０とし、
上記速度が上記前回の速度より上記所定閾値以上の大きな値を取った場合で、かつ上記前回の速度が上記所定速度以上であった場合、上記前回の速度を上記速度とする
請求項１に記載の速度算出装置。
【請求項１１】
路面のうねりに応じて車両に発生する垂直方向の加速度を検出する垂直方向加速度検出ステップと、
上記路面うねりに応じて上記車両に発生する上記車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出ステップと、
上記垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記車両の進行方向の速度を算出する速度算出ステップと
を有する速度算出方法。
【請求項１２】
路面のうねりに応じて車両に発生する垂直方向の加速度を検出する垂直方向加速度検出部と、
上記路面のうねりに応じて上記車両に発生する上記車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、
上記垂直方向の加速度及び上記水平軸周りの角速度に基づいて上記車両の進行方向の速度を算出する速度算出部と
進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、
上記垂直軸周りの角速度に基づいて上記車両が旋回した角度を算出する角度算出部と、
上記速度算出部により算出された上記進行方向の速度と、上記角度算出部により算出された上記角度とに基づいて、上記車両の位置を算出する位置算出部と
を有するナビゲーション装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、速度算出装置、速度算出方法及びナビゲーション装置に関し、例えば携帯型ナビゲーション装置に適用して好適なものである。
【背景技術】
【０００２】
従来、ナビゲーション装置においては、複数のＧＰＳ（Global Positioning System）衛星からの測位信号（以下、これをＧＰＳ信号とも呼ぶ）を受信し、当該ＧＰＳ信号に基づいて車両の現在位置を算出するようになされている。
【０００３】
しかしながらこのようなナビゲーション装置では、当該ナビゲーション装置を載置した車両が例えばトンネルや地下駐車場等の中に入った場合、ＧＰＳ衛星からのＧＰＳ信号を受信できず、従って、当該ＧＰＳ信号に基づいて車両の現在位置を算出することができない。
【０００４】
そこでナビゲーション装置のなかには、ＧＰＳ信号が受信できなくなった場合であっても、コーナリング時において、車両の進行方向に直交した水平方向の加速度と、当該進行方向に直交した垂直軸周りの角速度とを基に進行方向の速度を算出し、当該進行方向の速度に基づいて現在位置を算出するものがある（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００８−７６３８９公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで上述したナビゲーション装置では、コーナリング時においては進行方向の速度を算出することができるものの、直進時においては進行方向の速度を算出することができないため、必ずしも全ての道路環境下で進行方向の速度を算出することができないという問題があった。
【０００７】
本発明は以上の点を考慮してなされたもので、道路環境に関わらず、全ての道路環境下で車両の速度を高精度に算出し得る速度算出装置、速度算出方法及びナビゲーション装置を提案しようとするものである。
【課題を解決するための手段】
【０００８】
かかる課題を解決するため本発明の速度算出装置においては、車両に搭載され、路面のうねりに応じて発生する垂直方向の加速度を検出する垂直方向加速度検出部と、車両に搭載され、路面のうねりに応じて発生する車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を算出する速度算出部とを設けるようにした。
【０００９】
また本発明の速度算出方法においては、路面のうねりに応じて車両に発生する垂直方向の加速度を検出する垂直方向加速度検出ステップと、路面のうねりに応じて車両に発生する車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出ステップと、垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を算出する速度算出ステップとを設けるようにした。
【００１０】
さらに本発明のナビゲーション装置においては、路面のうねりに応じて車両に発生する垂直方向の加速度を検出する垂直方向加速度検出部と、路面のうねりに応じて車両に発生する車両の進行方向と直交した水平軸周りの角速度を検出する水平方向角速度検出部と、垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を算出する速度算出部と進行方向に垂直な垂直軸周りの角速度を算出する垂直方向角速度検出部と、垂直軸周りの角速度に基づいて車両が旋回した角度を算出する角度算出部と、速度算出部により算出された進行方向の速度と、角度算出部により算出された角度とに基づいて、移動体の位置を算出する位置算出部とを設けるようにした。
【００１１】
これにより、路面のうねりに応じて発生する垂直方向の加速度と、進行方向と直交した水平軸周りの角速度とを検出し、当該垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を全ての道路環境下で算出することができる。
【発明の効果】
【００１２】
本発明によれば、路面のうねりに応じて発生する垂直方向の加速度と、進行方向と直交した水平軸周りの角速度とを検出し、当該垂直方向の加速度及び水平軸周りの角速度に基づいて車両の進行方向の速度を算出することにより、道路環境に関わらず、全ての道路環境下で車両の速度を高精度に算出し得る速度算出装置、速度算出方法及びナビゲーション装置を実現することができる。
【図面の簡単な説明】
【００１３】
【図１】凹凸路面の走行時の様子を示す略線図である。
【図２】カーブ走行時の様子を示す略線図である。
【図３】速度及び角度を用いた現在位置算出手法を示す略線図である。
【図４】ＰＮＤの全体構成を示す略線図である。
【図５】ＰＮＤの座標系の定義を示す略線図である。
【図６】ＰＮＤにおけるセンサ構成を示す略線図である。
【図７】ＰＮＤの回路構成を示す略線図である。
【図８】速度算出部の構成を示す略線図である。
【図９】高度及び角度の関係を示す略線図である。
【図１０】低速時における路面角度の様子を示す略線図である。
【図１１】高速時における路面角度の様子を示す略線図である。
【図１２】超低速時における路面角度の様子を示す略線図である。
【図１３】クレードルによる振動の様子を示す略線図である。
【図１４】ハイパスフィルタ処理後の加算加速度及び加算角速度を示す略線図である。
【図１５】４０９６データ点ごとにフーリエ変換を施した加算角速度を示す略線図である。
【図１６】４０９６データ点ごとにフーリエ変換を施した加算加速度を示す略線図である。
【図１７】加算加速度に対するローパスフィルタ処理の比較を示す略線図である。
【図１８】加算角速度に対するローパスフィルタ処理の比較を示す略線図である。
【図１９】低速時におけるフロント加速度及びリア加速度の関係を示す略線図である。
【図２０】中速及び高速時におけるフロント加速度及びリア加速度の関係を示す略線図である。
【図２１】３種類の設置位置による加速度、ピッチレート及び速度のシミュレーション例を示す略線図である。
【図２２】最大値及び最小値の関係を示す略線図である。
【図２３】速度とデータ点数との関係を示す略線図である。
【図２４】異なる円弧の長さによる加速度及びピッチレートの様子を示す略線図である。
【図２５】速度算出処理を用いた現在位置算出処理手順の説明に供するフローチャートである。
【図２６】加速度、角速度及び速度の測定結果例を示す略線図である。
【図２７】測定結果とリファレンスとの比較（１）を示す略線図である。
【図２８】測定結果とリファレンスとの比較（２）を示す略線図である。
【図２９】測定結果とリファレンスとの比較（３）を示す略線図である。
【図３０】測定結果とリファレンスとの比較（４）を示す略線図である。
【図３１】測定結果とリファレンスとの比較（５）を示す略線図である。
【図３２】カーブ時における測定結果とリファレンスとの比較（１）を示す略線図である。
【図３３】カーブ時における測定結果とリファレンスとの比較（２）を示す略線図である。
【図３４】カーブ時における測定結果とリファレンスとの比較（３）を示す略線図である。
【図３５】地図上の行路と走行軌跡との比較を示す略線図である。
【図３６】軽車両に載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図３７】ミニバンに載置されたＰＮＤでの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図３８】第２の実施の形態による速度算出部の構成を示す略線図である。
【図３９】速度域ごとのカットオフ周波数を示す略線図である。
【図４０】各カットオフ周波数を固定したときの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図４１】各カットオフ周波数を切り替えたときの測定結果とＧＰＳ信号に基づく速度及び距離との比較を示す略線図である。
【図４２】他の実施形態による使用例を示す略線図である。
【発明を実施するための形態】
【００１４】
以下、発明を実施するための形態（以下実施の形態とする）について、図面を用いて説明する。なお、説明は以下の順序で行う。
１．第１の実施の形態（ナビゲーション装置・カットオフ周波数を固定した例）
２．第２の実施の形態（ナビゲーション装置・カットオフ周波数を速度に応じて切り替えた例）
３．他の実施の形態
【００１５】
＜１．第１の実施の形態＞
［１−１．基本原理］
本発明においてはナビゲーション装置として携帯型ナビゲーション装置（以下、これをＰＮＤ（Personal Navigation Device）とも呼ぶ）を用い、そのＰＮＤにより車両の速度及び現在位置を算出する基本原理について説明する。
【００１６】
［１−１−１．速度算出原理］
実際上、道路を走行中の車両は、平らな道路を走行することが殆どなく、図１（Ａ）に示すような全体として凹状の道路、及び図１（Ｂ）に示すような全体として凸状の道路を走行するのが実状である。
【００１７】
ここで車両の座標系は、当該車両の前後方向をＸ軸、当該Ｘ軸に直交した水平方向をＹ軸、上下方向をＺ軸によって表される。
【００１８】
この車両の例えばダッシュボード上に載置されたＰＮＤは、車両が凹状の道路（図１（Ａ））を走行したとき、当該ＰＮＤに設けられた３軸加速度センサによって、Ｚ軸に沿った下方向の加速度αzを例えば５０［Ｈｚ］のサンプリング周波数で検出する。
【００１９】
またＰＮＤは、当該ＰＮＤに設けられたＹ軸ジャイロセンサによって進行方向に直交したＹ軸周りの角速度（以下、これをピッチレートとも呼ぶ）ωyを５０［Ｈｚ］のサンプリング周波数で検出する。
【００２０】
ここでＰＮＤでは、Ｚ軸に沿った下方向の加速度αzを正と定義し、また図１（Ａ）に示すような凹状の路面に沿って形成される仮想上の円を進行方向に対して上向きに縦回転する際のピッチレートωyを正と定義している。
【００２１】
このＰＮＤでは、３軸加速度センサによって検出した加速度αz及びＹ軸ジャイロセンサによって検出したピッチレートωyを用い、次式
【００２２】
【数１】
2010078595.tif 000003
【００２３】
によって進行方向の速度Ｖを１秒間当たり５０回、算出し得るようになされている。
【００２４】
またＰＮＤは、車両が凸状の道路（図１（Ｂ））を走行するとき、当該ＰＮＤに設けられた３軸加速度センサによってＺ軸に沿った上方向の加速度αz’を例えば５０［Ｈｚ］のサンプリング周波数で検出し、また当該ＰＮＤに設けられたＹ軸ジャイロセンサによってＹ軸周りのピッチレートωy’を例えば５０［Ｈｚ］のサンプリング周波数で検出する。
【００２５】
そしてＰＮＤでは、３軸加速度センサによって検出した加速度αz’及びＹ軸ジャイロセンサによって検出したピッチレートωy’を用い、次式
【００２６】
【数２】
2010078595.tif 000004
【００２７】
によって進行方向の速度Ｖ’を１秒間当たり５０回、算出し得るようになされている。
【００２８】
ここでは説明の便宜上、負の加速度α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とを用い、上述した（１）式を変形した
【０１１１】
【数３】
2010078595.tif 000005
【０１１２】
によって前回の位置Ｐ０（図３）での進行方向の速度Ｖを算出し、その結果得られる速度データＶＤ１を平滑化及びノイズ除去部３５へ送出する。
【０１１３】
すなわち速度計算部３４は、（３）式を用いることにより、加速度α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と直前の速度Ｖとを比較して加速中であるか減速中であるかを判断する。
【０２０１】
続いてカットオフ周波数設定部６７は、テーブルＴＢＬ１及びＴＢＬ２から、判断結果（すなわち加速中又は減速中のいずれか）及び速度Ｖに応じた速度域のハイパス用カットオフ周波数ＦＨ及びローパス用カットオフ周波数ＦＬを読み出し、それぞれをハイパスフィルタ部６２及びローパスフィルタ部６３へ供給する。
【０２０２】
ハイパスフィルタ部６２は、カットオフ周波数設定部６７から供給されたハイパス用カットオフ周波数ＦＨをカットオフ周波数として設定する。続いてハイパスフィルタ部６２は、ハイパスフィルタ部３２（図８）と同様、加速度データＡＤ及びピッチレートデータＰＤの直流成分をカットして加速度データＡＤ１及びピッチレートデータＰＤ１を生成し、これらをローパスフィルタ部６３へ送出する。
【０２０３】
ローパスフィルタ部６３は、カットオフ周波数設定部６７から供給されたローパス用カットオフ周波数ＦＬをカットオフ周波数として設定する。続いてローパスフィルタ部６３は、ローパスフィルタ部３３（図８）と同様、加速度データＡＤ１及びピッチレートデータＰＤ１の高周波成分をカットして加速度データＡＤ２及びピッチレートデータＰＤ２を生成し、これらを速度計算部３４へ送出する。
【０２０４】
この結果、加速度データＡＤ２及びピッチレートデータＰＤ２には、直前の速度Ｖに応じたハイパス用カットオフ周波数ＦＨ及びローパス用カットオフ周波数ＦＬの間に挟まれる周波数成分のみが抽出されることになる。
【０２０５】
このように第２の実施の形態による速度算出部５２は、直前の速度Ｖに応じてハイパス用カットオフ周波数ＦＨ及びローパス用カットオフ周波数ＦＬを切り替えて設定することにより、加速度データＡＤ２及びピッチレートデータＰＤ２として抽出する周波数帯成分を変化させるようになされている。
【０２０６】
［２−２．測定結果］
次に、第１の実施の形態による速度算出部２２により各カットオフ周波数を固定して速度データＶＤを生成する場合と、第２の実施の形態による速度算出部５２により各カットオフ周波数を切り替えながら速度データＶＤ生成する場合の双方について、速度Ｖ及び距離Ｄの測定実験を行った。このときの測定結果を図４０及び図４１にそれぞれ示す。
【０２０７】
ちなみにこの測定実験は、図２６〜図３７に示した場合と異なり、クレードル３の車両に対する取付状態を意図的に不完全なものとし、誤差が生じやすい状態において行った。
【０２０８】
図４０には、各カットオフ周波数を固定した速度算出部２２により算出された速度Ｖ及び距離Ｄと、当該速度Ｖ及び距離Ｄの比較のためにＧＰＳ信号を基に算出したＧＰＳ速度Ｖg及びＧＰＳ速度Ｖgから算出されたＧＰＳ距離Ｄgとを重ねて示す。
【０２０９】
また図４１には、速度Ｖに応じて各カットオフ周波数を切り替える速度算出部５２により算出された速度Ｖ及び距離Ｄと、当該速度Ｖ及び距離Ｄの比較のためにＧＰＳ信号を基に算出したＧＰＳ速度Ｖg及びＧＰＳ速度Ｖgから算出されたＧＰＳ距離Ｄgとを重ねて示す。
【０２１０】
図４０及び４１から、速度Ｖに応じて各カットオフ周波数を切り替えることにより、各カットオフ周波数を固定した場合と比較して、速度ＶとＧＰＳ速度Ｖgとの誤差及び距離ＤとＧＰＳ距離Ｄgとの誤差がいずれも縮小されていることが分かる。
【０２１１】
［２−３．動作及び効果］
以上の構成において、第２の実施の形態によるＰＮＤ５０は、第１の実施の形態と同様、路面のうねりによって発生する車両の進行方向に垂直なＺ軸方向の加速度α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の例えば２５データ点毎の平均値を取ることにより、速度Ｖを１秒当たり２回だけ算出することになる。これによりＰＮＤ１の制御部１１は、速度算出処理に対する処理負荷を軽減することができる。
【０２２３】
さらに上述した実施の形態においては、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ部２２によりハイパスフィルタ処理を施すようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１は、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理を施さないようにしてもよい。
【０２２４】
さらに上述した実施の形態においては、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ部２２及びローパスフィルタ部２３によりハイパスフィルタ処理及びローパスフィルタ処理を施すようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１は、加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理及びローパスフィルタ処理に加えて、移動平均フィルタ処理を施すようにしてもよい。またＰＮＤ１は、加速度データＡＤ及びピッチレートデータＰＤに対して、ハイパスフィルタ処理、ローパスフィルタ処理及び移動平均フィルタ処理を任意に組み合わせた処理を施すようにしても良い。
【０２２５】
さらに上述した実施の形態においては、加速度αz及びピッチレートωyを用いて例えば前回の位置Ｐ０の速度Ｖを算出する際、当該前回の位置Ｐ０の速度Ｖが過大であると判断した場合、前値速度Ｖn-1を前回の位置Ｐ０の速度Ｖとするようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１の速度算出部２２は、前回の位置Ｐ０の速度Ｖが前値速度Ｖn-1より所定の閾値以上大きかった場合、前回の位置Ｐ０の速度Ｖを、前値速度Ｖn-1に車両が加速できるであろう分を加算した値とするようにしてもよい。
【０２２６】
またＰＮＤ１の速度算出部２２は、前回の位置Ｐ０の速度Ｖが前値速度Ｖn-1より所定の閾値以上小さかった場合、前回の位置Ｐ０の速度Ｖを、前値速度Ｖn-1に車両が減速できるであろう分を減算した値とするようにしてもよい。
【０２２７】
さらに上述した実施の形態においては、加速度αz及びピッチレートωyを基に、（３）式を用いて速度Vを算出するようにした場合について述べた。
【０２２８】
しかしながら本発明はこれに限らず、ＰＮＤ１の制御部１１は、加速度αz及びピッチレートωyを基に（３）式を用いて算出した速度Ｖと、ＧＰＳ信号を基に算出したＧＰＳ速度Ｖgとを比較する。
【０２２９】
そしてＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖgとに誤差が生じていた場合、例えば速度Ｖとの誤差が最小となるように一次関数や２次以上の高次関数等により補正するための補正係数を算出し、記憶部１２へ当該補正係数を記憶する。
【０２３０】
従ってＰＮＤ１の速度算出部２２は、３軸加速度センサ４及びＹ軸ジャイロセンサ５によりそれぞれ検出した加速度αz及びピッチレートωyを基に（３）式を用いて速度Ｖを算出した後、記憶部１２から補間係数を読み出し、当該補間係数を用いて当該速度Ｖを一次関数や２次以上の高次関数等により補正する。
【０２３１】
このようにＰＮＤ１は、ＧＰＳ信号を基に算出したＧＰＳ速度Ｖgを基に速度Ｖの補正係数を予め学習しておくことにより、当該速度Ｖの算出精度をより向上させることができる。
【０２３２】
なお、ＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖgとの補正係数を算出する際、例えば超低速、低速、中速及び高速等のような複数の速度領域に速度Ｖを分割し、当該複数の速度領域ごとに補正係数を算出するようにしても良い。
【０２３３】
またＰＮＤ１の制御部１１は、速度ＶとＧＰＳ速度Ｖgとの補正係数を算出する際、所定例えば時速６０［ｋｍ］以上の高速時に対してだけ、補正係数を算出するようにしても良い。
【０２３４】
さらに上述した第２の実施の形態においては、テーブルＴＢＬ１及びＴＢＬ２において、速度域を４段階に分けるようにした場合について述べた。しかしながら本発明はこれに限らず、２段階以上の任意の段階数としても良い。また、ハイパス用カットオフ周波数ＦＨ及びローパス用カットオフ周波数ＦＬそれぞれの速度域の段階数は、必ずしも同一である必要はなく、両者で段階数を相違させるようにしても良い。
【０２３５】
さらに上述した第２の実施の形態においては、テーブルＴＢＬ１及びＴＢＬ２における各速度域の速度を加速時と減速時とで相違させることにより、各カットオフ周波数の切り替えに関しヒステリシス性を持たせるようにした場合について述べた。しかしながら本発明はこれに限らず、たとえば各速度域の速度を加速時と減速時とで一致させ、ヒステリシス性を持たないようにしても良い。
【０２３６】
さらに上述した第２の実施の形態においては、カットオフ周波数設定部６７がテーブルＴＢＬ１及びＴＢＬ２に従い、速度域ごとに各カットオフ周波数を段階的に切り替えるようにした場合について述べた。しかしながら本発明はこれに限らず、たとえば次に示す（４）式及び（５）式のように、速度Ｖを変数とした関数によりハイパス用カットオフ周波数ＦＨ及びローパス用カットオフ周波数ＦＬをそれぞれ算出するようにしても良い。
【０２３７】
【数４】
2010078595.tif 000006
【数５】
2010078595.tif 000007
【０２３８】
またこの場合、関数の形としては１次関数に限らず、２次以上の関数や指数関数等の種々の形でも良く、要は速度Ｖに応じて各カットオフ周波数を一意に定めることができれば良い。
【０２３９】
さらに上述した実施の形態においては、ＰＮＤ１が、電源電力の供給を受けている間、現在位置算出処理手順に従ってナビゲーションを行うにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１は、電源ボタン（図示せず）がユーザによって押下操作されることによってオフされた場合、当該電源ボタンが押下された時点での現在位置及び高度等を記憶部１２へ記憶する。そしてＰＮＤ１は、再び電源ボタンがユーザによって押下操作されることによってオンされた場合、記憶部１２から現在位置及び高度等を読み出し、当該現在位置及び高度等から再び現在位置算出処理手順に従ってナビゲーションを行うようにしても良い。
【０２４０】
さらに上述した実施の形態においては、ＰＮＤ１が、車両のダッシュボード上に載置されたクレードル３によって保持されている状態で、速度Ｖを算出するようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１が、クレードル３から機械的或いは電気的に取り外されたことを認識すると、速度Ｖを０とする、或いは前値速度Vn-1のまま継続するようにしても良い。
【０２４１】
さらに上述した実施の形態においては、ＰＮＤ１が左右方向に長い横置きの状態で使用されるようにした場合について述べた。しかしながら本発明はこれに限らず、図４２に示すように、ＰＮＤ１は、縦方向に長い縦置きの状態で使用されるようにしてもよい。この場合ＰＮＤ１は、Ｙ軸ジャイロセンサ５によりＺ軸回りのヨーレートωｚを検出し、またＺ軸ジャイロセンサ６によりＹ軸周りのピッチレートωｙを検出するようにすればよい。
【０２４２】
さらに上述した実施の形態においては、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７がＰＮＤ１の内部に設けられているようにした場合について述べた。しかしながら本発明はこれに限らず、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７が、ＰＮＤ１の外部に設けられているようにしてもよい。
【０２４３】
またＰＮＤ１は、３軸加速度センサ４、Ｙ軸ジャイロセンサ５、Ｚ軸ジャイロセンサ６及び気圧センサ７の取り付け角度を調節できるような調節機構を例えば当該ＰＮＤ１の側面に設けるようにしても良い。
【０２４４】
これによりＰＮＤ１は、その表示部２が車両の進行方向に対してほぼ垂直となるように設置されていない場合であっても、調節機構をユーザに調節させることによって、例えばＹ軸ジャイロセンサ５の回転軸を車両の垂直方向と揃えることができる。
【０２４５】
さらに上述した実施の形態においては、ピッチレートデータＰＤ２に相当するピッチレートωyが所定の閾値未満であった場合、及び加速度データＡＤ２に相当する加速度αz及びピッチレートデータＰＤ２に相当するピッチレートωyがそれぞれ所定の閾値より大きい場合、速度Ｖが過大であると判断するようにした。しかしながら本発明はこれに限らず、制御部１１は、速度計算部３４によって算出された速度Ｖが前値速度Ｖn-1より所定速度以上の大きな値を取ったとき、速度Ｖが過大であると判断するようにしても良い。
【０２４６】
この場合、平滑化及びノイズ除去部３５は、速度計算部３４によって算出された速度Ｖが前値速度Ｖn-1より所定速度以上の大きな値を取ったときで、かつ前値速度が例えば時速１０［ｋｍ］未満の超低速時であった場合、速度Vを０とする。また平滑化及びノイズ除去部３５は、速度計算部３４によって算出された速度Ｖが前値速度Ｖn-1より所定速度以上の大きな値を取ったときで、かつ前値速度が例えば時速１０［ｋｍ］以上であった場合、前値速度Ｖn-1を速度Vとするようにすればよい。
【０２４７】
さらに上述した実施の形態においては、ＰＮＤ１の制御部１１が、予め記憶部１２に格納されているアプリケーションプログラムに従い、上述したルーチンＲＴ１の現在位置算出処理手順を行うようにした場合について述べた。しかしながら本発明はこれに限らず、ＰＮＤ１の制御部１１が、記憶媒体からインストールしたアプリケーションプログラムや、インターネットからダウンロードしたアプリケーションプログラム、その他種々のルートによってインストールしたアプリケーションプログラムに従って上述した現在位置算出処理手順を行うようにしても良い。
【０２４８】
さらに上述した実施の形態においては、垂直方向加速度検出部として３軸加速度センサ４、水平方向角速度検出部としてＹ軸ジャイロセンサ５、速度算出部として速度算出部２２によって本発明の速度算出装置としてのＰＮＤ１を構成するようにした場合について述べた。本発明はこれに限らず、この他種々の構成でなる垂直方向加速度検出部、水平方向角速度検出部、速度算出部によって速度算出装置を構成するようにしても良い。
【産業上の利用可能性】
【０２４９】
本発明の速度算出装置、速度算出方法及びナビゲーション装置は、例えば車両以外にも、バイク、電車等のその他種々に実装されたＰＮＤや据置型のナビゲーション装置等に適用することができる。
【符号の説明】
【０２５０】
１、５０……ＰＮＤ、２……表示部、３……クレードル、４……３軸加速度センサ、５……Ｙ軸ジャイロセンサ、６……Ｚ軸ジャイロセンサ、７……気圧センサ、１１……制御部、１２……記憶部、２１……ＧＰＳ処理部、２２、５２……速度算出部、２３……角度算出部、２４……高度算出部、２５……位置算出部、２６……ナビゲーション部、３１……データ取得部、３２、６２……ハイパスフィルタ部、３３、６３……ローパスフィルタ部、３４……速度計算部、３５……平滑化及びノイズ除去部、３６……速度出力部、６７……カットオフ周波数設定部。
【図１】
2010078595.tif 000008
【図２】
2010078595.tif 000009
【図３】
2010078595.tif 000010
【図４】
2010078595.tif 000011
【図５】
2010078595.tif 000012
【図６】
2010078595.tif 000013
【図７】
2010078595.tif 000014
【図８】
2010078595.tif 000015
【図９】
2010078595.tif 000016
【図１０】
2010078595.tif 000017
【図１１】
2010078595.tif 000018
【図１２】
2010078595.tif 000019
【図１３】
2010078595.tif 000020
【図１４】
2010078595.tif 000021
【図１５】
2010078595.tif 000022
【図１６】
2010078595.tif 000023
【図１７】
2010078595.tif 000024
【図１８】
2010078595.tif 000025
【図１９】
2010078595.tif 000026
【図２０】
2010078595.tif 000027
【図２１】
2010078595.tif 000028
【図２２】
2010078595.tif 000029
【図２３】
2010078595.tif 000030
【図２４】
2010078595.tif 000031
【図２５】
2010078595.tif 000032
【図２６】
2010078595.tif 000033
【図２７】
2010078595.tif 000034
【図２８】
2010078595.tif 000035
【図２９】
2010078595.tif 000036
【図３０】
2010078595.tif 000037
【図３１】
2010078595.tif 000038
【図３２】
2010078595.tif 000039
【図３３】
2010078595.tif 000040
【図３４】
2010078595.tif 000041
【図３５】
2010078595.tif 000042
【図３６】
2010078595.tif 000043
【図３７】
2010078595.tif 000044
【図３８】
2010078595.tif 000045
【図３９】
2010078595.tif 000046
【図４０】
2010078595.tif 000047
【図４１】
2010078595.tif 000048
【図４２】
2010078595.tif 000049