JP2008058187A - ナビゲーション装置、位置検出方法及び位置検出プログラム - Google Patents

Abstract

【課題】測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出できるようにする。
【解決手段】自律速度算出ユニット１１は、クレードル着脱検出部１６からの着脱情報ＣＤを基に、ナビゲーション装置１がクレードル４に装着されている場合、学習した取付傾きを用いて高精度な自律速度Ｖ_ｔを算出することができ、一方ナビゲーション装置１がクレードル４から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ１４の取付傾きを用いた自律速度Ｖ_ｔの算出を中止するため、誤った自律速度Ｖ_ｔの出力を未然に防止することができ、ナビゲーションユニット１２における現在位置の算出精度を低下させずに済む。
【選択図】 図１１

Description

本発明は、ナビゲーション装置、位置検出方法及び位置検出プログラムに関し、例えば車両に搭載されるナビゲーション装置に適用して好適なものである。

従来、ナビゲーション装置においては、ＧＰＳ(Global Positioning System)衛星等の測位手段から受信した測位情報（例えばＧＰＳ信号）に基づいて車両の現在位置を算出して表示するが、例えばトンネル等の中に入って当該ＧＰＳ信号を受信できなくなったときは加速度センサの検出結果に基づいて算出された車両の進行方向加速度に基づいて車両の走行速度を推定し、当該走行速度に基づいて推定した現在位置を表示することが行われている（例えば、特許文献１参照）。
特開2004-138553公報

ところでかかる構成のナビゲーション装置においては、加速度センサの検出結果に基づいて進行方向加速度を算出するようになされているが、それは、あくまで加速度センサの加速度検出軸が車両の進行方向と一致していることが前提である。

しかしながら、取り付け及び取り外しが容易なポータブルタイプのナビゲーション装置における実際の使用環境では、加速度センサが搭載されている本体部の取付傾きがユーザ毎に異なり、必ずしも加速度センサの加速度検出軸と車両の進行方向とが一致した設置条件を満たせるわけではない。

そこで、ナビゲーション装置において、ＧＰＳ衛星から受信したＧＰＳ信号に基づいて算出される進行方向を基に、加速度センサの検出結果に基づいて進行方向加速度とのずれ量（すなわち取付傾き）を算出する手法が考えられる。この場合ナビゲーション装置は、この取付傾きを用いて進行方向加速度を算出すれば、車両の走行速度や現在位置を高精度に推定し得ると考えられる。

しかしながら、かかるナビゲーション装置は、車両から取り外された場合、車両に対する相対的な位置や角度が変化してしまうため、算出した取付傾きをそのまま使い続けると、進行方向加速度を不適切に算出することになってしまい、車両の走行速度や現在位置の算出精度を大幅に低下させてしまう可能性があるという問題があった。

本発明は以上の点を考慮してなされたもので、測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを提案しようとするものである。

かかる課題を解決するため本発明においては、所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出し、測位情報を基に算出された移動体の速度に応じて当該移動体に作用する移動体加速度を算出し、本体部に作用する加速度を観測し、移動体加速度及び観測された加速度に基づいて加速度センサの移動体に対する取付傾きを求め、測位情報が供給されないときに、観測された加速度及び取付傾きを基に移動体の推定速度及び推定位置を算出し、本体部が台座部に対して装着されていないことが検出された場合、推定速度の算出を停止させるようにした。

これにより、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止できる。

本発明によれば、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止でき、かくして測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを実現できる。

以下、図面について、本発明の一実施の形態を詳述する。

（１）ナビゲーション装置の構成
（１−１）外観構成
図１において、ナビゲーション装置１は、ＧＰＳアンテナ２によりＧＰＳ衛星（図示せず）からのＧＰＳ信号等を基に現在位置を算出し、表示部３に地図画面や所望の目的地までの経路案内画面等を表示することにより、ユーザに現在位置や周辺地図、或いは案内経路等を提示し得るようになされている。

このナビゲーション装置１は、ユーザにより容易に持ち運ばれ得るようポータブル型に構成され、当該ユーザの家庭内や車両の車室内において使用されることが想定されている。

またナビゲーション装置１は、台座部としての専用のクレードル４に容易に装着され得ると共に、当該クレードル４から容易に取り外され得るようになされている。実際上、このクレードル４が車両の車室内におけるダッシュボード上等に予め取り付けられることにより、ナビゲーション装置１は、当該車両に対して容易に取り付けられ、又は容易に取り外されるようになされている。

因みにＧＰＳアンテナ２は、フロントガラスの下等に設置されることにより、車両のボディ等により遮断されることなく、上空の衛星軌道を周回するＧＰＳ衛星からのＧＰＳ信号を良好に受信し得るようになされている。

（１−２）ナビゲーション装置の回路構成
図２に示すように、ナビゲーション装置１は、上述した表示部３に加え、ＧＰＳ信号の受信処理等を行うＧＰＳ処理部１０、ＧＰＳ信号を受信し得ない際に各種センサによる検出値を基に速度を算出する演算処理ブロック１１、ＧＰＳ処理部１０又は自律速度算出ユニット１１からの情報を基に経路案内画面等を表す表示データを生成するナビゲーションユニット１２、及び各種データ等を記憶する記憶部１３等を有している。

ＧＰＳ処理部１０は、ＧＰＳアンテナ２を介して複数のＧＰＳ衛星（図示せず）からのＧＰＳ信号を受信し、当該ＧＰＳ信号を基に所定の位置算出処理を行うことにより位置情報ＰＳを算出し、これをナビゲーションユニット１２及び記憶部１３へ供給する。またＧＰＳ処理部１０は、受信したＧＰＳ信号を基に所定の速度算出処理及び方位算出処理を行うことにより、車両の速度Ｖ及び方位データＤを算出し、これらを自律速度算出ユニット１１及び記憶部１３へ供給する。

これに応じてナビゲーションユニット１２は、位置情報ＰＳに対応した範囲の地図データを所定の地図記憶部（図示せず）から読み出し、当該地図データ上に現在位置を示すマーク等を重畳して表示画面データを生成して、これを表示部３へ送出し表示画面を表示させる。

一方、自律速度算出ユニット１１には、３軸方向の加速度を検出する加速度センサ１４及び周囲の気圧を検出する気圧センサ１５が接続されている。この加速度センサ１４は、３軸方向に作用する加速度を観測して加速度センサ観測値ＡＤを生成し、これを自律速度算出ユニット１１へ供給する。また気圧センサ１５は、周囲の気圧を検出して気圧値ＰＲを生成し、これを自律速度算出ユニット１１へ供給する。

自律速度算出ユニット１１は、加速度センサ観測値ＡＤ及び気圧値ＰＲを基に、車両の速度Ｖ_ｔを算出し、記憶部１３に記憶・更新させるようになされている。また自律速度算出ユニット１１は、ビルの陰やトンネル内等のようにＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、車両の現在位置を推測するための情報として、当該速度Ｖ_ｔをナビゲーションユニット１２へ供給するようになされている（詳しくは後述する）。

これに応じてナビゲーションユニット１２は、ＧＰＳ処理部１０からは位置情報ＰＳを得られないものの、自律速度算出ユニット１１からの車両の速度Ｖ_ｔを基に現在位置を推定し、この現在位置に対応した表示画面を表示部３に表示させる。

因みにナビゲーション装置１は、いわゆる据付型のナビゲーション装置とは異なり、車両において生成され当該車両の速度に応じて周期が変化する車速パルス信号を利用せずに済むようになされており、当該ナビゲーション装置１を当該車両に取り付ける際の配線処理を簡略化し得るようになされている。

このようにナビゲーション装置１は、ＧＰＳアンテナ２によりＧＰＳ信号を受信し得ない場合、自律速度算出ユニット１１によって加速度センサ観測値ＡＤ及び気圧値ＰＲを基に速度Ｖ_ｔを算出し、ナビゲーションユニット１２によってこれらを基に現在位置（すなわち推定位置）を算出し得るようになされている。

（２）速度の算出原理
次に、ナビゲーション装置１における自律速度算出ユニット１１による速度Ｖ_ｔの算出原理について説明する。

（２−１）進行方向加速度の算出原理
自律速度算出ユニット１１では、車両の速度Ｖ_ｔを高精度に算出するべく、当該車両の進行方向の純粋な加速度（以下、これを進行方向加速度と呼ぶ）を検出したい、といった要求がある。

しかしながら、ナビゲーション装置１がクレードル４を介して車両に取り付けられる場合、加速度センサ１４の加速度検出軸は、当該車両の進行方向と一致しない可能性が高い。その場合、加速度センサ１４から出力される加速度センサ観測値ＡＤは、重力加速度ｇや方位変化に伴う横方向加速度の混入や、当該加速度センサ１４における進行方向加速度のゲインの変化等による影響を受ける可能性がある。

このため自律速度算出ユニット１１は、加速度センサ１４からの加速度センサ観測値ＡＤをそのまま用いた場合、進行方向加速度の検出精度が大きく低下してしまう可能性がある。

そこで自律速度算出ユニット１１では、ナビゲーション装置１、車両及び地表面の間で座標の変換処理を行い、また重力加速度成分や横方向加速度成分の分離処理、さらにはオフセット調整処理等を行うことにより、進行方向加速度を高精度に算出し得るようになされている。

（２−２）２つの座標軸間の関係
図３に示すように、ｘｙ座標系からｘ′ｙ′座標系への変換行列Ｔを次式、

で表し、ｘｙ座標系からみた位置ｐ（cosθ，sinθ）が、ｘ′ｙ′座標系からみた位置ｐ′（１，０）であるとした場合、次式の関係が成立し、これが、２つの座標系の関係を表す最も基本的な関係式となる。

また座標軸と回転角度との関係を、図４に示すように定義する。すなわち、前後・左右・上下が定義できる物体において、前後方向をｘ軸、左右方向をｙ軸、上下方向をｚ軸とし、ｘ軸周りの回転角度φを「ティルト」と呼び（以下、これをティルト角度φと呼ぶ）、ｙ軸周りの回転角度θを「スイング」と呼び（以下、これをスイング角度θと呼ぶ）、ｚ軸周りの回転角度ψを「パン」と呼ぶ（以下、これをパン角度ψと呼ぶ）。

（２−３）道路傾斜が存在するときの地表面と車両との関係
次に、道路傾斜が存在するときの地表面と車両との関係について説明する。因みに、道路傾斜が存在しない地表面（路面）は、車両の進行方向に沿ったｘ軸、車両の進行方向とは直交した横方向（水平方向）に沿ったｙ軸、及び重力加速度方向に沿った鉛直方向のｚ軸によって表される。

（２−３−１）地表面の地表座標系と道路傾斜の車両座標系
図５に示すように、道路傾斜上に位置している車両の車両座標系は、道路傾斜上で車両の進行方向に沿ったｘ′軸、道路傾斜上で車両の進行方向とは直交した横方向（水平方向）に沿ったｙ′軸、及び車両の高さ方向に沿ったｚ′軸によって表される。

この道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）では、車両がｘ′軸方向に沿って太矢印方向へ進行する際の後ろ向きに働く加速度成分の符号を正と定義し、車両が右折する際の左側方向に働く加速度成分の符号を正と定義し、車両の上向き方向に働く重力加速度成分の符号を正と定義している。但し、符号の正負については、これに限るものではなく、上述の条件と逆であっても良い。

この場合、地表面の地表座標系（ｘ軸、ｙ軸、ｚ軸）と、道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）とを関係付けるオイラー角で考えると、道路の進行方向における上り坂や下り坂を表す前後傾斜はｙ′軸周りのスイング角度θで示され、道路の進行方向と直交した横方向（水平方向）の右下下がりや左下下がりを表す左右傾斜は、ｘ′軸周りのティルト角度φで示される。なお、ｚ′軸周りの回転はパン角度ψで示されるが、当該パン角度ψによって表される道路の傾斜は存在しない。

（２−３−２）地表面と車両との関係を示す座標変換式
ここで、地表面の地表座標系（ｘ軸、ｙ軸、ｚ軸）に対する道路傾斜上に位置した車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）の変換行列Ａを、次式で定義する。

この場合、変換行列Ａは、道路の前後傾斜が先で、道路の左右傾斜が後なので、前後傾斜の項に対して左右傾斜の項を右側から乗算するようになされている。

従って、地表座標系（ｘ軸、ｙ軸、ｚ軸）は、次式で表される。

これにより、道路傾斜上に位置する車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）では、重力加速度ｇは、次式のように観測される。

（２−４）車両と加速度センサとの関係
続いて、車両と、当該車両に取り付けられるナビゲーション装置１に搭載された加速度センサ１４との関係について説明する。

（２−４−１）車両の車両座標系及び加速度センサのセンサ座標系
図５に示したように、道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）に対して、当該車両に対して任意の角度で取り付けられるナビゲーション装置１に搭載された３軸方向の加速度センサ１４におけるセンサ座標系については、図６に示すようにｘ″軸、ｙ″軸及びｚ″軸で表される。

この場合、ナビゲーション装置１が車両に取り付けられたときの加速度センサ１４に対するセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）としては、当該ナビゲーション装置１がユーザからみて上向き又は下向きに取り付けられた場合にｙ″軸周りのスイング角度θ′で示され、当該ナビゲーション装置１がユーザからみて左下下がり又は右下下がりに取り付けられた場合にｘ″軸周りのティルト角度φ′で示され、当該ナビゲーション装置１がユーザからみて右側運転席のドライバー向き又は左側助手席側のパッセンジャー向きに取り付けられた場合にｚ″軸周りのパン角度ψ′によって示される。

（２−４−２）車両座標系とセンサ座標系との関係を示す座標変換式
ここで、道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）に対する３軸方向の加速度センサ１４におけるセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）の変換行列Ｂを、次式で定義する。

この（６）式で表される変換行列Ｂでは、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に右側から乗算されるように選定されており、以降の計算過程においては一度選定された乗算順序を崩さないようにしなければならない。

但し、変換行列Ｂとしては、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に限られるものではなく、例えば、道路の左右傾斜が加速度センサ１４の取付角度の３軸（ｘ″軸、ｙ″軸、ｚ″軸）に分配されて観測される場合、ティルト角度φ′、スイング角度θ′、パン角度ψ′の順番に右側から乗算されるように選定されてもよく、一度選定された乗算順序を崩さなければ、その他種々の乗算順序であってもよい。

ここで、道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）は、加速度センサ１４のセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）との関係で、次式のように表される。

従って、（７）式を変形すれば、加速度センサ１４のセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）は、次式のように表される。

ここで加速度センサ１４のセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）は、道路傾斜上に位置している車両の車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）が、上述したように（４）式の関係を有することから、（８）式は、次式で表される。

従って、加速度センサ１４のセンサ座標系（ｘ″軸、ｙ″軸、ｚ″軸）では、重力加速度ｇは、次式のように観測される。

この重力加速度ｇを表す（１０）式では、変換行列Ｂの逆行列Ｂ^−１に相当する部分が車両に対する加速度センサ１４の傾きを表し、かつ変換行列Ａの逆行列Ａ^−１に相当する部分が地表面に対する車両の傾きを表している。

実際上、加速度センサ１４が車両に対して所定方向の傾きを持って取り付けられ、かつ道路傾斜上に車両が位置することにより当該車両が地表面に対して所定角度の傾きを持って存在している場合、重力加速度ｇは、次式のように観測される。

（２−４−３）加速度センサに対する運動加速度（進行方向加速度及び横方向加速度）
また、加速度センサ１４のセンサ座標系（ｘ″軸、ｙ″軸及びｚ″軸）では、車両の進行方向加速度αｘ及び横方向加速度 αｙを定義し、道路に前後傾斜及び左右傾斜が存在せず、車両に対して加速度センサ１４の取付傾きだけが生じている場合、加速度センサ１４からみたときの進行方向加速度αｘ及び横方向加速度
αｙを表す運動加速度αについては、変換行列Ｂの逆行列Ｂ^−１を用いて、次式

で表されるため、これを変換行列Ｂの逆行列Ｂ^−１を用いて展開すると、次式のように観測される。

因みに、車両の加速時には後ろ向きの加速度（いわゆるＧ）がかかること、及び右カーブでは左向きの遠心力が発生することに留意して、加速度センサ１４のティルト角度φ′、スイング角度θ′及びパン角度ψ′に対する正負の符号が定められる。

（２−５）加速度センサ観測値
加速度センサ１４に対するセンサ座標系（ｘ″軸、ｙ″軸及びｚ″軸）の重力加速度ｇと運動加速度αとを合成したものを、当該加速度センサ１４によって観測される加速度センサ観測値Ａ_Ｓと定義した場合、当該加速度センサ観測値Ａ_Ｓは、（１０）式及び（１２）式に基づいて、次式で表される。

これを展開した場合、加速度センサ観測値Ａ_Ｓは、次式のように表される。

（２−５−１）加速度センサの零点オフセットとゲイン
ところで、電圧値として表される加速度センサ観測値Ａ_Ｓとしては、加速度センサ１４の零点オフセットＯＦ及びゲインＧＥのことを考慮する必要がある。

ここで加速度センサ１４の零点オフセットＯＦとは、加速度センサ１４に対して加速度成分が全く働いていないときのｘ″軸、ｙ″軸及びｚ″軸にそれぞれ発生する電圧値（[V]）を示す。

また加速度センサ１４のゲインＧＥとは、加速度センサ１４に対して例えば１ｇ（＝９．８［m/sec²］）の負荷がかかったときのｘ″軸、ｙ″軸及びｚ″軸にそれぞれ発生する電圧値（[V／(m/sec²)]）がどの程度変化するかを示す。

この零点オフセットＯＦ及びゲインＧＥを考慮したときの加速度センサ観測値ＡＤは、（１４）式に基づいて、次式で表される。

なお、この（１６）式中に含まれる加速度センサ観測値Ａ_Ｓの単位は、［m/sec²］であり、加速度センサ観測値ＡＤの単位としては、［V］となる。

ここで零点オフセットＯＦは、次式

で表され、ゲインＧＥは、次式で表される。

従って、零点オフセットＯＦ及びゲインＧＥを考慮したときの加速度センサ観測値ＡＤは、次式で表されることになる。

ここで、（１９）式を構成する各要素のうち、ゲインＧＥｘ、ＧＥｙ、ＧＥｚ、零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、車両に対する加速度センサ１４の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′、道路の前後傾斜を示すスイング角度θ、道路の左右傾斜を示すティルト角度φがそれぞれ未知数である。

しかし、ゲインＧＥｘ、ＧＥｙ、ＧＥｚに関しては加速度センサ１４の仕様書に記載されている値もしくは実際に検出された値を用い、道路の前後傾斜を示すスイング角度θについては、ナビゲーション装置１に内蔵された気圧センサ１５の出力結果（気圧値ＰＲ）と対応付けられた高度情報を基に算出し、誤差は若干出るが道路の左右傾斜を示すティルト角度φはないものとして扱うものとする。

但し、これら以外の例えば重力加速度ｇについては、ＧＰＳ衛星から得られるＧＰＳ信号に基づいて算出した現在位置から緯度を取得し、緯度と重力加速度とが対応付けられた緯度−重力加速度変換テーブルから当該緯度に対応した重力加速度ｇを取得すればよい。

また、車両の進行方向加速度αｘについては、ＧＰＳ信号に基づく車両の走行速度に基づいてリファレンスとして算出することが可能であり、車両の横方向加速度
αｙについても、ＧＰＳ信号に基づく車両の方位変化及び走行速度に基づきリファレンスとして算出することが可能である。

これにより（１９）式では、そのうち未知数が加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、車両に対する加速度センサ１４の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′の６個になり、これらを求める必要がある。

（２−６）加速度センサの取付角度及びオフセットの学習
ところで、加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ及びゲインＧＥｘ、ＧＥｙ、ＧＥｚを考慮した加速度センサ観測値ＡＤ＝Ｉと定義し、（１６）式中に含まれる次式

と定義した場合、上述のＩ（加速度センサ観測値ＡＤ）は、次式で表される。なお、式中のＳθ、Ｃθについては、それぞれsinθ、cosθを意味するものとする。

ここで、（２０）式中における変換行列Ａの逆行列Ａ^−１は、地表面に対する道路の前後傾斜を表しており、これは気圧センサ１５の出力結果（気圧値ＰＲ）に対応付けられている高度情報を基に導くことが可能であり、重力加速度ｇ、進行方向加速度αｘ及び横方向加速度
αｙについても、上述したようにリファレンスとして算出可能であるため、（２１）式中のＥは既知数である。

この既知数Ｅは、（３）式に示したように変換行列Ａの逆行列Ａ^−１を用いれば、次式のように変形することができる。

従って、（２１）式では、車両に対する加速度センサ１４の取付傾き（ティルト角度φ′、スイング角度θ′及びパン角度ψ′）と、加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚとが未知であり、加速度センサ観測値ＡＤは、次式のように変形することができる。

この（２３）式を更にニュートンラプソン法で用いられる形態に変形し、次式

のような３つの関数式ｆｘ、ｆｙ、ｆｚに表す。

ここで、（２４）式〜（２６）式では未知数が零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計６個であるのに対し、３つの関数式ｆｘ、ｆｙ、ｆｚしか存在しないので、このままでは解くことができない。

但し、（２４）式〜（２６）式中の既知数Ｅの値は、毎秒得られるＧＰＳ信号に基づいて変化するため、少なくとも２秒間待てば、既知数Ｅの値が変化したそれぞれ異なる６種類の関数式ｆｘ_１、ｆｙ_１、ｆｚ_１、ｆｘ_２、ｆｙ_２、ｆｚ_２を得られるが、車両に加減速の変化がなければ、複数種類の関数式ｆｘ、ｆｙ、ｆｚを得ることはできない。そこで、車両の実際の走行状態を想定し、加減速の変化が生じそうな１０数秒分に相当する複数種類の関数式ｆｘ_１〜ｆｘ_ｔ、ｆｙ_１〜ｆｙ_ｔ、ｆｚ_１〜ｆｚ_ｔを得ることとする。

この場合、未知数の数（６個）よりも関数式ｆｘ_１〜ｆｘ_ｔ、ｆｙ_１〜ｆｙ_ｔ、ｆｚ_１〜ｆｚ_ｔの数が遥かに多くなるものの、検出誤差等を考慮すると、解を一意に定めることができない可能性が高い。そこで、関数式ｆｘ_１〜ｆｘ_ｔ、ｆｙ_１〜ｆｙ_ｔ、ｆｚ_１〜ｆｚ_ｔを満たすような尤もらしい未知数の値を得るべく、最小二乗法を用いることとする。

具体的な計算手法として、ここでは多次元のニュートンラプソン法の適用を考える。基本的な１次元のニュートンラプソン法における変化量δは、次式

で表される。ニュートンラプソン法では、この変化量δが所定の収束判定閾値以下になったとき、未知数の値の真解が得られたと認識する。

この（２７）式は、次式

のように変形され、これをＮ次元に拡張すると、次式のように表される。

従って、この場合、具体的には次式

で表される左辺左側の偏導関数値と、右辺の関数値を計算することにより、左辺右側の最小二乗解（変化量δ）を求める。

このようにして求められた最小二乗解（変化量δ）を用いて、未知数の解を次式

によって算出し、この（３１）式の左辺に示された未知数の解を用いて、再度（３０）式により最小二乗解（変化量δ）を求め、この最小二乗解（変化量δ）を用いて（３１）式の未知数の解を求めるといった繰返し計算を所定の回数分だけ行えば良い。

ところで、（３１）式で用いられる右辺第１項の初期値は、次式によって与えられる。

この（３２）式に示す初期値は、ティルト角度(φ′)₀、スイング角度(θ′) ₀及びパン角度(ψ′) ₀ が０[rad]であることを示している。また、零点オフセット(ＯＦｘ) ₀、(ＯＦｙ)
₀、(ＯＦｚ₀ )については、加速度センサ１４に加速度が一切作用していないときの電圧値が２．５（[V]）であることを示している。

因みに加速度センサ１４は、実際に検出した加速度に応じて、加速度センサ観測値ＡＤの各成分（具体的には電位）をそれぞれ０[V]〜５[V]の範囲で変動させるようになされている。

この（３１）式では、左辺が最終的な解であり、右辺第１項が（３１）式により前回求められた未知数の解（学習結果）を示し、右辺第２項が上述した（３０）式の変化量δを示している。この（３１）式の左辺により、零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、車両に対する加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′の６個の未知数が全て判明する。

このように自律速度算出ユニット１１は、車両がＧＰＳアンテナ２によりＧＰＳ信号を受信し得るような走行環境下にあるとき、零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計６個の未知数を解として求める。

その後、自律速度算出ユニット１１は、車両がＧＰＳ信号を受信し得ないような走行環境となったとき、加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いると共に、加速度センサ１４の観測結果や路面の高度差に基づいて進行方向加速度を推定し、これを基に積分計算することにより車両の速度や現在位置を算出(推定)するようになされている。

因みに、（３０）式〜（３２）式中における添え字のｉは、ニュートンラプソン法による繰り返し計算時の繰返回数を表しており、例えば、（３０）式の右辺に含まれる(ｆ_ｘ１)_ｉは、次式

を意味し、（３０）式の左辺左側の行列に含まれる例えば∂(ｆ_ｘ１)_ｉ／∂(ψ′)_ｉは、∂ｆ_ｘ１／∂ψ′に対して、((ψ′)_ｉ，(θ′)_ｉ，(φ′)_ｉ，(ＯＦｘ)_ｉ，(ＯＦｙ)_ｉ，(ＯＦｚ)_ｉ)を代入したものを意味している。

また、ｆ_ｘ１の添え字（この場合であれば「１」）は、例えば１０数秒間分まとめて処理する際の秒数を示す。従って、例えば１０秒間分まとめて処理するのであれば、（３０）式の左辺左側では、ｆ_ｘ１〜ｆ_ｘ１０、ｆ_ｙ１〜ｆ_ｙ１０、ｆ_ｚ１〜ｆ_ｚ１０まで存在し、そのときの行列サイズは３０（＝３×１０）行６列になる。

なお、（３１）式の左辺で得られた未知数の解としては、真値を中心にばらつきがある可能性が高く、また時間の経過とともに真値が変動する可能性もあるため、本発明では（３１）式で得られる未知数の解を現時点までの例えば過去数秒〜数分間に渡って算出し、それを平均化又は平滑化することにより、そのときに応じた最終的な未知数の解を学習結果として求め得るようになされている。

（２−７）加速度センサを用いた自律動作
ところで、加速度センサ観測値ＡＤは、（１６）式と同様、次式のように表される。

この（３４）式を進行方向加速度αｘ及び横方向加速度αｙについて整理すると、次式

のように変形され、この（３５）式中の変換行列Ｂは、次式

で表され、この（３５）式中の右辺第２項は、次式のように表される。

従って、上述の（３５）式に対して（３６）式及び（３７）式を代入することにより、次式を得ることができる。

この（３８）式によって、進行方向加速度αｘを求めるには、道路の前後傾斜を表すｙ′軸周りのスイング角度θが必要である。このスイング角度θは、次式

により、高度差ΔＨと、速度Ｖ_ｔとによって表される。

この時点での未知数は、速度Ｖ_ｔ、進行方向加速度αｘ、及び道路の前後傾斜を表すスイング角度θであるが、これらを一つ一つ解くのではなく、最終的に求めたい速度Ｖ_ｔを一気に求めることを考える。

ここで、速度Ｖ_ｔ−１は、これから最終的に求めたい速度Ｖ_ｔの一つ前の時点で判明した速度データである。従って、ＧＰＳ信号を受信できていた状態からＧＰＳ信号を受信できなくなった最初の時点における速度Ｖ_ｔ−１としては、ＧＰＳ信号に基づいて最後に算出された速度Ｖ_０があてはまる。

この場合、（３８）式のうち、既知の部分をβ（以下、これを既知部分βと呼ぶ）としてまとめると、既知部分βは、次式

で表され、これを用いて（３８）式を、次式

のように変形することができる。

ここで速度Ｖ_ｔと進行方向加速度αｘとの関係は、次式

のように表すことができ、上述の（４１）式、（４３）式を代入することにより、（４４）式は、次式のように変形することができる。

この（４５）式を速度Ｖ_ｔについて整理すると、次式

によって表され、これを速度Ｖ_ｔについて解くと、当該速度Ｖ_ｔは、次式によって表される。

このように自律速度算出ユニット１１では、ＧＰＳ信号の受信時に、車両に対する加速度センサ１４の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚを予め学習して記憶しておく。その後、自律速度算出ユニット１１は、ビル等の陰に隠れる等してＧＰＳ信号を受信できなくなった場合、予め学習しておいた加速度センサ１４の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ及びＯＦｚを用い、加速度センサ観測値ＡＤｘ″、ＡＤｙ″、ＡＤｚ″及び高度差ΔＨに基づいて進行方向加速度αｘを自律的に算出（推定）することができるので、その自律的に算出した進行方向加速度αｘを基に車両の速度Ｖ_ｔ（以下、これを自律速度Ｖ_ｔと呼ぶ）を求めることができる。

これに応じてナビゲーション装置１では、自律速度算出ユニット１１により算出された車両の自律速度Ｖ_ｔを積分することにより、ＧＰＳ信号を受信できない走行環境下であっても、当該車両の現在位置を正確に算出し、表示部３の地図上に現在位置を継続して表示し得るようになされている。

（３）自律速度算出ユニットの具体的構成
次に、上述した速度の算出原理に従い速度Ｖｔを算出する自律速度算出ユニット１１の構成について、ＧＰＳ測位時（学習時）とＧＰＳ非測位時（自律時）とに分けてそれぞれ説明する。

（３−１）ＧＰＳ測位時（学習時）における自律速度算出ユニットの構成
図７において、自律速度算出ユニット１１は、図示しないＣＰＵ（Central Processing Unit）を中心に構成されており、図示しないＲＯＭ（Read Only Memory）から基本プログラムや速度算出プログラム等の各種アプリケーションプログラムを読み出して図示しないＲＡＭ(Random Access Memory)上に展開することにより、道路傾斜計算部２０、学習処理部２１及び自律計算部２２といった各処理機能をソフトウェア的に実現し得るようになされている。

但し、自律速度算出ユニット１１としては、これに限るものではなく、当該自律速度算出ユニット１１における道路傾斜計算部２０、学習処理部２１及び自律計算部２２をハードウェア的に構成するようにしても良い。

自律速度算出ユニット１１は、ＧＰＳ処理部１０により算出される車両の速度Ｖ及び方位データＤを道路傾斜計算部２０及び学習処理部２１へ供給する。また自律速度算出ユニット１１は、気圧センサ１５によって検出された気圧値ＰＲを道路傾斜計算部２０へ供給すると共に、加速度センサ１４によって観測される加速度センサ観測値ＡＤを学習処理部２１へ供給する。

道路傾斜計算部２０の進行距離算出部３１は、ＧＰＳ処理部１０から供給される速度Ｖに基づいて車両の進行距離Ｌｍを算出し、その進行距離Ｌｍを前後傾斜算出部３３へ送出する。

また、道路傾斜計算部２０の高度差算出部３２は、一般的な気圧と高度との対応関係が予めテーブル化された気圧高度対応テーブルＴＢＬを記憶部１３（図２）に記憶しており、時刻ｔ０における気圧値ＰＲ０及び時刻ｔ１における気圧値ＰＲ１を基に、気圧高度対応テーブルＴＢＬから気圧値ＰＲ０及びＰＲ１にそれぞれ対応した高度情報ｈ０及びｈ１を読み出す。

そして高度差算出部３２は、気圧値ＰＲ０に対応した車両の高度情報ｈ０と、気圧値ＰＲ１に対応した車両の高度情報ｈ１との差分である高度差ΔＨを算出し、これを前後傾斜算出部３３へ送出するようになされている。

前後傾斜算出部３３は、進行距離算出部３１から供給される進行距離Ｌｍと、高度差算出部３２から供給される高度差ΔＨとに基づいて、車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）において道路の前後傾斜を示すスイング角度θを算出し、これを学習処理部２１の取付角度及びオフセット学習処理部４４へ送出する。

学習処理部２１の進行方向加速度算出部４１は、ＧＰＳ処理部１０から供給される速度Ｖを微分することにより車両の進行方向加速度αｘをリファレンスとして算出し、これを取付角度及びオフセット学習処理部４４へ送出する。

一方、角速度算出部４２は、ＧＰＳ処理部１０から供給される方位データＤを微分することにより角速度ｄＤを算出し、これを横方向加速度算出部４３へ送出する。横方向加速度算出部４３は、ＧＰＳ処理部１０から供給される速度Ｖ及び角速度算出部４２から供給される角速度ｄＤに基づいて横方向加速度αｙをリファレンスとして算出し、これを取付角度及びオフセット学習処理部４４へ送出する。

取付角度及びオフセット学習処理部４４は、加速度センサ１４から供給される加速度センサ観測値ＡＤと、進行方向加速度算出部４１から供給される進行方向加速度αｘと、横方向加速度算出部４３から供給される横方向加速度αｙとを基に、加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに車両に対する加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を学習し得るようになされている。

このようなＧＰＳ測位時における自律速度算出ユニット１１の学習処理手順をまとめてみれば、図８に示すように自律速度算出ユニット１１は、ルーチンＲＴ１の開始ステップから入って次のステップＳＰ１へ移る。このステップＳＰ１において自律速度算出ユニット１１は、ＧＰＳ処理部１０により複数のＧＰＳ衛星からのＧＰＳ信号に基づいて算出された車両の速度Ｖ及び方位データＤをＧＰＳデータとして取得すると共に学習処理部２１へ供給し、次のステップＳＰ２へ移る。

ステップＳＰ２において自律速度算出ユニット１１は、加速度センサ１４の出力である加速度センサ観測値ＡＤを取得して学習処理部２１の取付角度及びオフセット学習処理部４４へ供給し、次のステップＳＰ３へ移る。

ステップＳＰ３において自律速度算出ユニット１１は、道路傾斜計算部２０の進行距離算出部３１で算出された進行距離Ｌｍ、及び高度差算出部３２で算出された高度差ΔＨに基づいて、前後傾斜算出部３３により車両座標系（ｘ′軸、ｙ′軸、ｚ′軸）において道路の前後傾斜を示すスイング角度θを算出し、次のステップＳＰ４へ移る。

ステップＳＰ４において自律速度算出ユニット１１は、ステップＳＰ１で取得した車両の速度Ｖが速度情報として信頼たり得るデータであるか否かを当該速度Ｖに付加されているフラグに基づいて判断し、否定結果が得られたときは、速度Ｖが信頼できない速度情報に過ぎないため、次のステップＳＰ１６へ移って処理を終了する。

これに対してステップＳＰ４で肯定結果が得られると、自律速度算出ユニット１１は次のステップＳＰ５へ移り、ＧＰＳ処理部１０から受け取った速度Ｖを基に進行方向加速度算出部４１により進行方向加速度αｘをリファレンスとして算出すると共に、ＧＰＳ処理部１０から受け取った速度Ｖ及び方位データＤを基に横方向加速度算出部４３により横方向加速度αｙをリファレンスとして算出し、次のステップＳＰ６へ移る。

ステップＳＰ６において自律速度算出ユニット１１は、加速度センサ観測値ＡＤ及び上述の（２２）式で示された既知数Ｅを学習用入力データとして設定し、次のステップＳＰ７へ移る。

ステップＳＰ７においてカーナビゲーション装置１は、加速度センサ１４のオフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに当該加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるために必要な関数式が未知数よりも多いか否かを判定する。

ここで否定結果が得られると、このことは関数式が未知数よりも多くないため、これら未知数を求めることが出来ないことを表しており、このとき自律速度算出ユニット１１はステップＳＰ１６へ移って処理を終了する。

これに対してステップＳＰ７で肯定結果が得られると、このことは関数式が未知数よりも多いため、これら未知数を求めることが可能であることを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ８へ移る。

ステップＳＰ８において自律速度算出ユニット１１は、このルーチンＲＴ１における学習処理手順によって求められた前回の学習結果（加速度センサ１４のオフセットＯＦｘ、ＯＦｙ及びＯＦｚ並びに当該加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′）の値を上述した（３０）式の左辺左側の行列の値として初期値設定し、次のステップＳＰ９へ移る。

ステップＳＰ９において自律速度算出ユニット１１は、上述した（３０）式における左辺左側の偏導関数値と、右辺の関数値を計算し、次のステップＳＰ１０へ移る。

ステップＳＰ１０において自律速度算出ユニット１１は、上述した（３０）式における右辺の各要素（関数値）の合計が所定の収束判定閾値よりも小さくなったか否かを判定し、肯定結果が得られると次のステップＳＰ１５へ移り、否定結果が得られると次のステップＳＰ１１へ移る。

ステップＳＰ１１において自律速度算出ユニット１１は、上述した（３０）式の右辺における関数値の各要素の合計が、ニュートンラプソン法における所定の収束判定閾値よりも小さくなっていないので、ステップＳＰ９で計算した偏導関数値及び関数値により、（３０）式における左辺右側の最小二乗解（変化量δ）を求め、次のステップＳＰ１２へ移る。

ステップＳＰ１２において自律速度算出ユニット１１は、ステップＳＰ１１で求めた最小二乗解（変化量δ）を用いて（３１）式の左辺に相当する最終解（加速度センサ１４のオフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに当該加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′）を求め、次のステップＳＰ１３へ移る。

ステップＳＰ１３において自律速度算出ユニット１１は、（３０）式の左辺右側で示される最小二乗解（変化量δ）の各要素の合計が所定の収束判定閾値よりも小さいか否かを判定し、否定結果が得られると次のステップＳＰ１４へ移るのに対し、肯定結果が得られると次のステップＳＰ１５へ移る。

ステップＳＰ１４において自律速度算出ユニット１１は、ステップＳＰ９〜ステップＳＰ１４までの処理を繰返し行うニュートンラプソン法のループ回数が所定のループ回数閾値よりも少ないか否かを判定し、肯定結果が得られると、ステップＳＰ９へ戻って、以降の処理を繰り返す。この場合、ステップＳＰ９において自律速度算出ユニット１１は、直前に求められた加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに当該加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いて、上述した（３０）式中における左辺左側の偏導関数値と、右辺の関数値を計算する。

これに対し、ステップＳＰ１４で否定結果が得られると、繰返し計算のループ回数が所定のループ回数閾値よりも多いにも拘わらず、ステップＳＰ１０又はステップＳＰ１３で関数値の各要素の合計又は最小二乗解（変化量δ）の各要素の合計が収束しなかったことを表しており、このとき自律速度算出ユニット１１は、次のステップＳＰ１６へ移って未知数の解を求めることなく終了する。

ステップＳＰ１５において自律速度算出ユニット１１は、（３１）式の左辺で求められた未知数の最終解を現時点まで過去数秒〜数分間に渡って算出し、その結果得られる複数個の最終解を用いて平滑化処理することにより、最も真値に近い最終解を学習結果として得、次のステップＳＰ１６へ移って処理を終了する。

なお自律速度算出ユニット１１では、ステップＳＰ１５において平滑化処理に限るものではなく、処理量を減少させたいのであれば、単純な平均化処理することにより学習結果を得るようにしても良い。

（３−２）ＧＰＳ非測位時（自律時）における自律速度算出ユニット１１の構成
図９に示すように、自律速度算出ユニット１１は、ＧＰＳ信号を受信できていた走行環境下からＧＰＳ信号を受信できない走行環境下になった場合、ＧＰＳ受信部３から速度Ｖ及び方位データＤを受け取ることができなくなる。このとき自律速度算出ユニット１１は、気圧センサ１５からの気圧値ＰＲを道路傾斜計算部２０の高度差算出部３２へ供給すると共に、加速度センサ１４からの加速度センサ観測値ＡＤを自律計算部２２の速度算出部５１へ供給する。

道路傾斜計算部２０の高度差算出部３２は、気圧高度対応テーブルＴＢＬに基づいて、時刻ｔ０における気圧値ＰＲ０に対応した高度情報と時刻ｔ１における気圧値ＰＲ１に対応した高度情報との差分である高度差ΔＨを算出し、これを自律計算部２２の速度算出部５１へ送出する。

また、学習処理部２１の取付角度及びオフセット学習処理部４４は、ＧＰＳ信号を受信できているときに予め学習し記憶しておいた加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、車両に対する加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を自律計算部２２の速度算出部５１へ送出する。

速度算出部５１は、高度差算出部３２から供給される高度差ΔＨと、加速度センサ１４から供給される加速度センサ観測値ＡＤと、オフセット学習処理部４３から供給される加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚと、加速度センサ１４の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′とを基に、（４７）式等を用いて進行方向加速度αｘを自律的に算出（推定）し、その進行方向加速度αｘを基に車両の自律速度Ｖ_ｔを算出（推定）し得るようになされている。

従ってナビゲーションユニット１２は、ＧＰＳ信号を受信できない走行環境下であっても、このようにして求めた車両の自律速度Ｖ_ｔを積分することにより当該車両の現在位置を正確に算出し、表示部３の地図上に現在位置を継続して表示し得るようになされている。

このようなＧＰＳ非測位時における自律速度算出ユニット１１の自律速度算出処理手順をまとめてみれば、図１０に示すように自律速度算出ユニット１１は、ルーチンＲＴ２の開始ステップから入って次のステップＳＰ２１へ移る。このステップＳＰ２において自律速度算出ユニット１１は、複数のＧＰＳ衛星からのＧＰＳ信号に基づいて算出された車両の速度Ｖ及び方位データＤをＧＰＳデータとしてＧＰＳ処理部１０から学習処理部２１で受け取ることにより取得し、次のステップＳＰ２２へ移る。

ステップＳＰ２２において自律速度算出ユニット１１は、気圧センサ１５から供給される気圧値ＰＲを道路傾斜計算部２０の高度差算出部３２で受け取ることにより取得し、次のステップＳＰ２３へ移る。

ステップＳＰ２３において自律速度算出ユニット１１は、高度差算出部３２により気圧高度対応テーブルＴＢＬを用いて高度差ΔＨを算出し、次のステップＳＰ２４へ移る。

ステップＳＰ２４において自律速度算出ユニット１１は、ステップＳＰ２１で取得した車両の速度Ｖが速度情報として信頼たり得るデータであるか否かを当該速度Ｖに付加されているフラグに基づいて判断し、肯定結果が得られたときは、速度Ｖが信頼できる速度情報であって、ＧＰＳ信号を受信できている走行環境下にあるため、自律速度算出処理を行う必要がなく、次のステップＳＰ２７へ移って処理を終了する。

これに対してステップＳＰ２４で否定結果が得られると、このことはＧＰＳ信号を受信できない走行環境下であって、速度Ｖが信頼できない速度情報に過ぎないことを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ２５へ移る。

ステップＳＰ２５において自律速度算出ユニット１１は、ＧＰＳ信号を受信できない走行環境下にあるので、予めＧＰＳ信号の受信時に学習しておいた加速度センサ１４の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚを用いて、上述の（４３）式等を用いた計算処理により進行方向加速度αｘを推定し、これを積分計算することにより車両の自律速度Ｖ_ｔを求め、次のステップＳＰ２６へ移る。

ステップＳＰ２６において自律速度算出ユニット１１は、このようにして求めた車両の自律速度Ｖ_ｔをナビゲーションユニット１２へ出力し、次のステップＳＰ２７へ移って処理を終了する。

これに応じてナビゲーションユニット１２は、ＧＰＳ信号を受信できない走行環境下であっても、自律速度Ｖ_ｔに基づいて当該車両の現在位置を一段と正確に推定して表示部３の地図上に現在位置を継続表示することができる。

（４）ナビゲーション装置及びクレードルの着脱状態と学習結果の利用
（４−１）クレードルの着脱状態の検出と自律速度の算出
ところでナビゲーション装置１（図１）は、上述したように、車両に取り付けられたクレードル４に対して容易に装着され、また容易に取り外され得るようになされている。

このため、ナビゲーション装置１に関しては、例えばユーザにより当該ナビゲーション装置１がクレードル４から取り外され、当該ユーザの手元で行き先設定や経由地設定等の操作が行われた後、当該クレードル４に再度装着される、といった使用状況が想定され得る。

このようにクレードル４から取り外されているとき、ナビゲーション装置１は、車両に対する相対的な角度、すなわち加速度センサ１４の取付傾き（ティルト角度φ′、スイング角度θ′及びパン角度ψ′）が、当該クレードル４に装着されていたときとは異なることになる。

しかしながら、上述した（６）式において定義したセンサ座標系は、ナビゲーション装置１（すなわち加速度センサ１４）が車両に対して固定され動かないことを前提としている。

このためナビゲーション装置１の自律速度算出ユニット１１（図２）は、クレードル４から取り外されている間、ＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、上述した手法により学習した加速度センサ１４の取付傾き（ティルト角度φ′、スイング角度θ′及びパン角度ψ′）を用いると、誤った自律速度Ｖ_ｔを算出することになってしまう。

そこでナビゲーション装置１は、クレードル４との着脱状態を検出し、当該クレードル４との着脱状態に応じて自律速度算出ユニット１１における自律速度Ｖ_ｔの算出手法を切り換えるようになされている。

具体的にナビゲーション装置１は、クレードル着脱検出部１６によりクレードル４との着脱状態を検出し、着脱情報ＣＤを自律速度算出ユニット１１へ供給するようになされている。

自律速度算出ユニット１１は、着脱情報ＣＤを基にクレードル４に装着されていることを認識したとき、ＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、（６）式において定義したセンサ座標系が保持されていることから、上述した手法により学習した加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに当該加速度センサ１４の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′（以下、これらをまとめて学習結果と呼ぶ）を用いることにより、高精度に自律速度Ｖ_ｔを算出し出力する。

一方、自律速度算出ユニット１１は、着脱情報ＣＤを基にクレードル４から取り外されていることを認識したとき、ＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、（６）式において定義したセンサ座標系が変化してしまったことから、当該クレードル４に装着されていたときの最後の自律速度Ｖ_ｔが継続しているものと見なし、当該自律速度Ｖ_ｔをそのまま出力する。

この場合、自律速度算出ユニット１１は、一般に車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Ｖ_ｔを出力する。すなわち自律速度算出ユニット１１は、加速度センサ１４の学習結果を利用できず高精度な自律速度Ｖ_ｔを算出できないものの、いわば次善の策として直前の自律速度Ｖ_ｔを出力する。

これにより、ナビゲーションユニット１２において、実際の現在位置に比較的近い現在位置が算出されることを期待できる。

またナビゲーション装置１は、クレードル４に再度装着された場合、当該クレードル４との嵌合精度や車両に対するクレードル４の取付位置のずれ等の要因により、車両に対する取付傾きが前回と近い状態にあるとは想定されるものの、当該取付傾きが前回とは微妙に異なっている可能性がある。

そこで自律速度算出ユニット１１は、ナビゲーション装置１がクレードル４に再度装着されたことを検出した場合、当該クレードル４から取り外される直前に学習した学習結果を初期値として、再度学習処理を行うようになされている。

このとき自律速度算出ユニット１１は、前回の学習結果をそのまま用いるのではなく、再度学習処理を行うことにより、取付傾きの違いによる誤った自律速度Ｖ_ｔを出力せずに済む。また自律速度算出ユニット１１は、前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。

因みにナビゲーションユニット１２は、ＧＰＳアンテナ２によってＧＰＳ信号を受信できる場合、ナビゲーション装置１とクレードル４との着脱状態に拘わらず、ＧＰＳ処理部１０から供給される位置情報ＰＳを基に、現在位置を高精度に算出し得るようになされている。

このように自律速度算出ユニット１１は、クレードル４との装着状態に応じて取付傾き等の学習結果を利用するか否かを切り換えることにより、自律速度Ｖ_ｔの算出精度をできるだけ低下させないようになされている。

（４−２）自律速度算出切換処理手順
実際上、自律速度算出ユニット１１は、クレードル４との装着状態及びＧＰＳ信号の受信状態に応じて学習処理又は自律速度算出処理を切り換える場合、図１１に示す自律速度算出切換処理手順ＲＴ３に従った処理を行うようになされている。

すなわち自律速度算出ユニット１１は、所定の周期（例えば１秒）毎に自律速度算出切換処理手順ＲＴ３を開始し、ステップＳＰ３１へ移る。ステップＳＰ３１において自律速度算出ユニット１１は、クレードル着脱検出部１６からの着脱情報ＣＤを基に、ナビゲーション装置１がクレードル４に装着されているか否かを判定する。ここで肯定結果が得られると、このことはセンサ座標系が保持されていることを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ３２へ移る。

ステップＳＰ３２において自律速度算出ユニット１１は、ＧＰＳ処理部１０により複数のＧＰＳ衛星からのＧＰＳ信号に基づいて算出された車両の速度Ｖが速度情報として信頼たり得るデータであるか否かを、当該速度Ｖに付加されているフラグに基づいて判定する。ここで肯定結果が得られると、このことはＧＰＳ信号を十分に受信できており学習処理を行い得ることを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ３３へ移る。

ステップＳＰ３３において自律速度算出ユニット１１は、過去数回分の着脱情報ＣＤ等を基に、ナビゲーション装置１がクレードル４に装着された直後であるか否かを判定する。ここで否定結果が得られると、このことはナビゲーション装置１がしばらくの間クレードル４に装着されたまま取り外されておらず、これまでの学習結果を継続して利用し得ることを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ３４へ移る。

ステップＳＰ３４において自律速度算出ユニット１１は、上述したＧＰＳ受信時における学習処理手順ＲＴ１（図８）を実行して学習結果を更新し、その後ステップＳＰ４０へ移って処理を終了する。

一方ステップＳＰ３３において肯定結果が得られると、このことはナビゲーション装置１がクレードル４に再装着された直後であるため、加速度センサ１４の取付傾き等を学習し直す必要があることを表しており、このとき自律速度算出ユニット１１は、次のステップＳＰ３５へ移る。

ステップＳＰ３５において自律速度算出ユニット１１は、ナビゲーション装置１がクレードル４から取り外される前に学習した学習結果を記憶部１３から読み出して初期値とし、ＧＰＳ受信時における学習処理手順ＲＴ１（図８）を実行して新たな学習結果を算出し、その後ステップＳＰ４０へ移って処理を終了する。

一方、ステップＳＰ３２において否定結果が得られると、このことはＧＰＳ信号を受信できない走行環境下にあることを表しており、このとき自律速度算出ユニット１１は、次のステップＳＰ３６へ移る。

ステップＳＰ３６において自律速度算出ユニット１１は、自律速度算出処理手順ＲＴ２（図１０）を実行することにより自律速度Ｖ_ｔを算出して出力し、その後ステップＳＰ４０へ移って処理を終了する。

これに対して、ステップＳＰ３１において否定結果が得られると、このことはナビゲーション装置１がクレードル４から取り外されているために、センサ座標系が変化してしまっていることを表しており、このとき自律速度算出ユニット１１は次のステップＳＰ３８へ移る。

ステップＳＰ３８において自律速度算出ユニット１１は、ステップＳＰ３２と同様、ＧＰＳ処理部１０により複数のＧＰＳ衛星からのＧＰＳ信号に基づいて算出された車両の速度Ｖが速度情報として信頼たり得るデータであるか否かを、当該速度Ｖに付加されているフラグに基づいて判定する。

このステップＳＰ３８において肯定結果が得られると、このことはＧＰＳ信号を十分に受信できる走行環境下にあり自律速度Ｖ_ｔを算出する必要がないことを表しており、このとき自律速度算出ユニット１１は、次のステップＳＰ４０へ移って処理を終了する。

一方、ステップＳＰ３８において否定結果が得られると、このことはＧＰＳ信号を受信できない走行環境下にあり、且つセンサ座標系が変化してしまっているために、加速度センサ１４からの加速度センサ観測地ＡＤを基に自律速度Ｖ_ｔを算出し得ないことを表しており、このとき自律速度算出ユニット１１は、次のステップＳＰ３９へ移る。

ステップＳＰ３９において自律速度算出ユニット１１は、最後に算出した自律速度Ｖ_ｔ又は最後にＧＰＳ処理部１０から取得した速度Ｖのうち新しい方を自律速度Ｖ_ｔとして出力し、次のステップＳＰ４０へ移って処理を終了する。

その後ナビゲーションユニット１２は、自律速度算出ユニット１１から供給される自律速度Ｖ_ｔを基に現在位置（推定位置）を算出し、当該現在位置に応じた地図情報や経路案内等を表示部３に表示させる。

（５）動作及び効果
以上の構成において、ナビゲーション装置１の自律速度算出ユニット１１は、クレードル着脱検出部１６からの着脱情報ＣＤを基に、ナビゲーション装置１とクレードル４との着脱状態を認識する。

自律速度算出ユニット１１は、着脱情報ＣＤを基にクレードル４に装着されていることを認識したとき、ＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、学習した学習結果（すなわち加速度センサ１４の零点オフセットＯＦｘ、ＯＦｙ及びＯＦｚ、並びに当該加速度センサ１４の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′）を用いることにより、高精度に自律速度Ｖ_ｔを算出し出力する。

一方、自律速度算出ユニット１１は、着脱情報ＣＤを基にクレードル４から取り外されていることを認識したとき、ＧＰＳアンテナ２によってＧＰＳ信号を受信できない場合、当該クレードル４に装着されていたときの最後の速度が継続しているものと見なし、当該最後の速度をそのまま出力する。

この場合、自律速度算出ユニット１１は、ナビゲーション装置１がクレードル４から取り外されたときに、（６）式におけるセンサ座標系が変化してしまったことを認識することができ、学習結果である加速度センサ１４の取付傾き（すなわちティルト角度φ′、スイング角度θ′及びパン角度ψ′）を誤って用いることなく、自律速度Ｖ_ｔの算出を中止することができる。

このとき自律速度算出ユニット１１は、クレードル着脱検出部１６によりクレードル４から取り外されたことを検出した時点から学習結果の利用を中止するため、ナビゲーション装置１が当該クレードル４から取り外される際にユーザにより加えられる力を車両の速度の変化として誤認識してしまうことも未然に防止できる。

また自律速度算出ユニット１１は、加速度センサ１４の学習結果を利用できず高精度な自律速度Ｖ_ｔを算出できないものの、車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Ｖ_ｔ実際の車両の速度に近い値として出力することができる。

一方、自律速度算出ユニット１１は、ナビゲーション装置１がクレードル４に再度装着されたことを認識した場合、当該クレードル４から取り外される直前に学習した学習結果を初期値として再度学習処理を行う。

この場合、自律速度算出ユニット１１は、車両に対する取付傾きが前回とは微妙に異なっている可能性があるため、取付傾きの違いによる誤った自律速度Ｖ_ｔを出力せずに済み、また前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。

この結果、ナビゲーション装置１では、クレードル４との着脱状態やＧＰＳ信号の受信状態に拘わらず、ナビゲーションユニット１２において、現在位置が常に高精度に算出されることを期待できる。

以上の構成によれば、ナビゲーション装置１の自律速度算出ユニット１１は、クレードル着脱検出部１６からの着脱情報ＣＤを基に、ナビゲーション装置１がクレードル４に装着されている場合、学習した取付傾きを用いて高精度な自律速度Ｖ_ｔを算出することができ、一方ナビゲーション装置１がクレードル４から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ１４の取付傾きを用いた自律速度Ｖ_ｔの算出を中止するため、誤った自律速度Ｖ_ｔの出力を未然に防止することができ、ナビゲーションユニット１２における現在位置の算出精度を低下させずに済む。

（６）他の実施の形態
なお上述した実施の形態においては、ナビゲーション装置１がクレードル４から取り外され、且つＧＰＳ信号を充分に受信できない場合、自律速度算出ユニット１１から最後に算出した自律速度Ｖ_ｔ又は最後にＧＰＳ処理部１０から取得した速度Ｖを現在の自律速度Ｖ_ｔとして出力するようにした場合について述べたが、本発明はこれに限らず、例えば直前の速度Ｖの変化の割合を算出し、当該変化の割合に従って自律速度Ｖ_ｔを変化させていくようにし、又は自律速度Ｖ_ｔを一切出力しないようにする等しても良く、要はクレードル４に装着されていたときに学習した取付傾きを用いないようにすれば良い。

また上述した実施の形態においては、ナビゲーション装置１がクレードル４から一度取り外されて再度装着された場合に、前回の学習結果を初期値として再度学習を行うようにした場合について述べたが、本発明はこれに限らず、例えば（１６）式のような初期値を用いて学習処理を最初からやり直すようにし、又は前回の学習結果を用いて直ちに自律速度Ｖ_ｔの算出を開始する等しても良い。

さらに上述した実施の形態においては、（３０）〜（３２）式を用いたニュートンラプソン法により零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計６個の未知数を解として求めるようにした場合について述べたが、本発明はこれに限らず、他の種々の算出手法により零点オフセットＯＦｘ、ＯＦｙ、ＯＦｚ、ティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるようにしても良い。

さらに上述した実施の形態においては、（４２）式〜（４７）式を用いて自律速度Ｖ_ｔを算出するようにした場合について述べたが、本発明はこれに限らず、加速度センサ観測値ＡＤｘ″、ＡＤｙ″、ＡＤｚ″を用いた他の種々の演算手法により自律速度Ｖ_ｔを算出するようにしても良い。この場合、高度差ΔＨを用いずに当該自律速度Ｖ_ｔを算出するようにしても良い。

さらに上述した実施の形態においては、本発明のナビゲーション装置１を移動体としての車両に搭載するようにした場合について述べたが、本発明はこれに限らず、船舶、電車、その他種々の移動体に搭載するようにしても良い。

さらに上述した実施の形態においては、ＧＰＳアンテナ２を介して複数のＧＰＳ衛星からのＧＰＳ信号を受信し、これら複数のＧＰＳ信号に基づきＧＰＳ処理部１０により車両の速度Ｖを算出すると共に車両の方位データＤを算出するようにした場合について述べたが、本発明はこれに限らず、例えば準天頂衛星システム、グローナス（ＧＬＯＮＡＳＳ：Global Navigation Satellite System）やガリレオ（ＧＡＬＩＬＥＯ）等の種々の衛星測位システムを利用し、それぞれの衛星信号に基づき車両の速度Ｖを算出すると共に車両の方位データＤを算出するようにしても良い。

さらに上述の実施の形態においては、自律速度算出ユニット１１がＲＯＭから読み出して展開した速度算出プログラムに従って上述したルーチンＲＴ３の自律速度算出切換処理手順（図１１）を実行するようにした場合について述べたが、本発明はこれに限らず、記録媒体からインストールした速度算出プログラムや、インターネットからダウンロードした速度算出プログラムに従ってルーチンＲＴ３の自律速度算出切換処理手順を実行するようにしても良い。

さらに上述した実施の形態においては、本体部としてのナビゲーション装置１と、加速度センサとしての加速度センサ１４と、推定速度算出手段としての自律速度算出ユニット１１及びナビゲーションユニット１２と、着脱検出子としてのクレードル着脱検出部１６と、加速度算出手段、取付傾き算出手段及び推定速度算出制御手段としての自律速度算出ユニット１１とによってナビゲーション装置としてのナビゲーション装置１を構成する場合について述べたが、本発明はこれに限らず、その他種々の回路構成でなる本体部と、加速度算出手段と、加速度センサと、取付傾き算出手段と、推定速度算出手段と、着脱検出子と、推定速度算出制御手段とによってナビゲーション装置を構成するようにしても良い。

本発明は、本発明のナビゲーション装置、ナビゲーション情報算出方法及びナビゲーション情報算出プログラムは、例えば車両に対してクレードルを介して着脱されるポータブルタイプのナビゲーション装置に適用したが、クレードルが用意されＧＰＳ受信機能を有するＰＤＡ（Personal Digital Assistant）、携帯電話機、パーソナルコンピュータ等の種々の電子機器に適用することができる。

ナビゲーション装置及びクレードルの構成を示す略線的斜視図である。 ナビゲーション装置の回路構成を示す略線図である。 ２つの座標軸間の関係を示す略線的斜視図である。 角度の定義の説明に供する略線図である。 道路傾斜がある地表面上に位置している車両の車両座標系の説明に供する略線図である。 車両に取り付けられた加速度センサのセンサ座標系の説明に供する略線図である。 ＧＰＳ測位時（学習時）におけるナビゲーション装置の構成を示す略線的ブロック図である。 ＧＰＳ受信時における学習処理手順を示す略線的フローチャートである。 ＧＰＳ非測位時（自律時）におけるナビゲーション装置の構成を示す略線的ブロック図である。 自律速度算出処理手順を示す略線的フローチャートである。 自律速度算出切換処理手順を示す略線的フローチャートである。

符号の説明

１……ナビゲーション装置、２……ＧＰＳアンテナ、３……表示部、４……クレードル、１０……ＧＰＳ処理部、１１……自律速度算出ユニット、１２……ナビゲーションユニット、１３……記憶部、１４……加速度センサ、１５……気圧センサ、１６……クレードル着脱検出部、ＡＤ……加速度センサ観測値、ＣＤ……着脱情報、Ｖ……速度、Ｄ……方位データ、Ｖ_ｔ……自律速度。

Claims (8)

1. 所定の台座部に装着された状態で移動体に取り付けられ、所定の測位手段から供給される測位情報を基に上記移動体の現在位置を検出する本体部と、
   上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出手段と、
   上記本体部に作用する加速度を観測する加速度センサと、
   上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出手段と、
   上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きを基に上記移動体の推定速度及び推定位置を算出する推定速度算出手段と、
   上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出子と、
   上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出手段による上記推定速度の算出を停止させる推定速度算出制御手段と
   を具えることを特徴とするナビゲーション装置。
2. 上記推定速度算出制御手段は、
   上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記推定速度算出手段により最後に算出した推定速度をこの時点における推定速度とする
   ことを特徴とする請求項１に記載のナビゲーション装置。
3. 上記推定速度算出制御手段は、
   上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記測位情報を基に最後に算出された速度をこの時点における推定速度とする
   ことを特徴とする請求項１に記載のナビゲーション装置。
4. 上記取付傾き算出手段は、
   上記本体部が上記台座部に対して装着されていないことが検出された場合、上記取付傾きの算出を停止する
   ことを特徴とする請求項１に記載のナビゲーション装置。
5. 上記取付傾き検出手段は、
   所定の初期値を基に演算処理を再帰的に繰り返すことにより上記取付傾きを算出し、
   上記本体部が上記台座部から取り外され再度装着された場合、上記本体部が上記台座部から取り外される前に検出した上記取付傾きを上記初期値として、当該取付傾きを再度算出する
   ことを特徴とする請求項１に記載のナビゲーション装置。
6. 上記測位情報を基に算出された上記移動体の速度に応じて当該移動体の進行方向加速度を算出すると共に、上記測位情報に基づいて算出された上記移動体の速度及び方位に応じて上記進行方向加速度とは直交した水平方向加速度を算出する加速度算出手段と
   を具え、
   上記加速度センサは、
   上記移動体の運動加速度及び重力加速度を観測し、
   上記取付傾き算出手段は、
   所定の気圧センサからの気圧値に応じて算出した路面の高度差と、上記移動体の速度に応じた進行距離とに基づいて上記路面の上記進行方向に対する傾斜角度を算出し、上記進行方向加速度と、上記水平方向加速度と、上記加速度センサによる運動加速度及び重力加速度を含む観測値と、上記傾斜角度とにより表される多次元の関数式に基づいて上記取付傾きを求める
   ことを特徴とする請求項１に記載のナビゲーション装置。
7. 所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
   上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
   上記本体部に作用する加速度を観測する加速度観測ステップと、
   上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
   上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
   上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
   上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
   を具えることを特徴とする位置検出方法。
8. ナビゲーション装置に対して、
   所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
   上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
   上記本体部に作用する加速度を観測する加速度観測ステップと、
   上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
   上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
   上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
   上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
   を実行させることを特徴とする位置検出プログラム。

Images