ナビゲーション装置、位置検出方法及び位置検出プログラム
【課題】測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出できるようにする。
【解決手段】自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されている場合、学習した取付傾きを用いて高精度な自律速度Vtを算出することができ、一方ナビゲーション装置1がクレードル4から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ14の取付傾きを用いた自律速度Vtの算出を中止するため、誤った自律速度Vtの出力を未然に防止することができ、ナビゲーションユニット12における現在位置の算出精度を低下させずに済む。
【解決手段】自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されている場合、学習した取付傾きを用いて高精度な自律速度Vtを算出することができ、一方ナビゲーション装置1がクレードル4から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ14の取付傾きを用いた自律速度Vtの算出を中止するため、誤った自律速度Vtの出力を未然に防止することができ、ナビゲーションユニット12における現在位置の算出精度を低下させずに済む。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ナビゲーション装置、位置検出方法及び位置検出プログラムに関し、例えば車両に搭載されるナビゲーション装置に適用して好適なものである。
【背景技術】
【0002】
従来、ナビゲーション装置においては、GPS(Global Positioning System)衛星等の測位手段から受信した測位情報(例えばGPS信号)に基づいて車両の現在位置を算出して表示するが、例えばトンネル等の中に入って当該GPS信号を受信できなくなったときは加速度センサの検出結果に基づいて算出された車両の進行方向加速度に基づいて車両の走行速度を推定し、当該走行速度に基づいて推定した現在位置を表示することが行われている(例えば、特許文献1参照)。
【特許文献1】特開2004-138553公報
【発明の開示】
【発明が解決しようとする課題】
【0003】
ところでかかる構成のナビゲーション装置においては、加速度センサの検出結果に基づいて進行方向加速度を算出するようになされているが、それは、あくまで加速度センサの加速度検出軸が車両の進行方向と一致していることが前提である。
【0004】
しかしながら、取り付け及び取り外しが容易なポータブルタイプのナビゲーション装置における実際の使用環境では、加速度センサが搭載されている本体部の取付傾きがユーザ毎に異なり、必ずしも加速度センサの加速度検出軸と車両の進行方向とが一致した設置条件を満たせるわけではない。
【0005】
そこで、ナビゲーション装置において、GPS衛星から受信したGPS信号に基づいて算出される進行方向を基に、加速度センサの検出結果に基づいて進行方向加速度とのずれ量(すなわち取付傾き)を算出する手法が考えられる。この場合ナビゲーション装置は、この取付傾きを用いて進行方向加速度を算出すれば、車両の走行速度や現在位置を高精度に推定し得ると考えられる。
【0006】
しかしながら、かかるナビゲーション装置は、車両から取り外された場合、車両に対する相対的な位置や角度が変化してしまうため、算出した取付傾きをそのまま使い続けると、進行方向加速度を不適切に算出することになってしまい、車両の走行速度や現在位置の算出精度を大幅に低下させてしまう可能性があるという問題があった。
【0007】
本発明は以上の点を考慮してなされたもので、測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを提案しようとするものである。
【課題を解決するための手段】
【0008】
かかる課題を解決するため本発明においては、所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出し、測位情報を基に算出された移動体の速度に応じて当該移動体に作用する移動体加速度を算出し、本体部に作用する加速度を観測し、移動体加速度及び観測された加速度に基づいて加速度センサの移動体に対する取付傾きを求め、測位情報が供給されないときに、観測された加速度及び取付傾きを基に移動体の推定速度及び推定位置を算出し、本体部が台座部に対して装着されていないことが検出された場合、推定速度の算出を停止させるようにした。
【0009】
これにより、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止できる。
【発明の効果】
【0010】
本発明によれば、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止でき、かくして測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを実現できる。
【発明を実施するための最良の形態】
【0011】
以下、図面について、本発明の一実施の形態を詳述する。
【0012】
(1)ナビゲーション装置の構成
(1−1)外観構成
図1において、ナビゲーション装置1は、GPSアンテナ2によりGPS衛星(図示せず)からのGPS信号等を基に現在位置を算出し、表示部3に地図画面や所望の目的地までの経路案内画面等を表示することにより、ユーザに現在位置や周辺地図、或いは案内経路等を提示し得るようになされている。
【0013】
このナビゲーション装置1は、ユーザにより容易に持ち運ばれ得るようポータブル型に構成され、当該ユーザの家庭内や車両の車室内において使用されることが想定されている。
【0014】
またナビゲーション装置1は、台座部としての専用のクレードル4に容易に装着され得ると共に、当該クレードル4から容易に取り外され得るようになされている。実際上、このクレードル4が車両の車室内におけるダッシュボード上等に予め取り付けられることにより、ナビゲーション装置1は、当該車両に対して容易に取り付けられ、又は容易に取り外されるようになされている。
【0015】
因みにGPSアンテナ2は、フロントガラスの下等に設置されることにより、車両のボディ等により遮断されることなく、上空の衛星軌道を周回するGPS衛星からのGPS信号を良好に受信し得るようになされている。
【0016】
(1−2)ナビゲーション装置の回路構成
図2に示すように、ナビゲーション装置1は、上述した表示部3に加え、GPS信号の受信処理等を行うGPS処理部10、GPS信号を受信し得ない際に各種センサによる検出値を基に速度を算出する演算処理ブロック11、GPS処理部10又は自律速度算出ユニット11からの情報を基に経路案内画面等を表す表示データを生成するナビゲーションユニット12、及び各種データ等を記憶する記憶部13等を有している。
【0017】
GPS処理部10は、GPSアンテナ2を介して複数のGPS衛星(図示せず)からのGPS信号を受信し、当該GPS信号を基に所定の位置算出処理を行うことにより位置情報PSを算出し、これをナビゲーションユニット12及び記憶部13へ供給する。またGPS処理部10は、受信したGPS信号を基に所定の速度算出処理及び方位算出処理を行うことにより、車両の速度V及び方位データDを算出し、これらを自律速度算出ユニット11及び記憶部13へ供給する。
【0018】
これに応じてナビゲーションユニット12は、位置情報PSに対応した範囲の地図データを所定の地図記憶部(図示せず)から読み出し、当該地図データ上に現在位置を示すマーク等を重畳して表示画面データを生成して、これを表示部3へ送出し表示画面を表示させる。
【0019】
一方、自律速度算出ユニット11には、3軸方向の加速度を検出する加速度センサ14及び周囲の気圧を検出する気圧センサ15が接続されている。この加速度センサ14は、3軸方向に作用する加速度を観測して加速度センサ観測値ADを生成し、これを自律速度算出ユニット11へ供給する。また気圧センサ15は、周囲の気圧を検出して気圧値PRを生成し、これを自律速度算出ユニット11へ供給する。
【0020】
自律速度算出ユニット11は、加速度センサ観測値AD及び気圧値PRを基に、車両の速度Vtを算出し、記憶部13に記憶・更新させるようになされている。また自律速度算出ユニット11は、ビルの陰やトンネル内等のようにGPSアンテナ2によってGPS信号を受信できない場合、車両の現在位置を推測するための情報として、当該速度Vtをナビゲーションユニット12へ供給するようになされている(詳しくは後述する)。
【0021】
これに応じてナビゲーションユニット12は、GPS処理部10からは位置情報PSを得られないものの、自律速度算出ユニット11からの車両の速度Vtを基に現在位置を推定し、この現在位置に対応した表示画面を表示部3に表示させる。
【0022】
因みにナビゲーション装置1は、いわゆる据付型のナビゲーション装置とは異なり、車両において生成され当該車両の速度に応じて周期が変化する車速パルス信号を利用せずに済むようになされており、当該ナビゲーション装置1を当該車両に取り付ける際の配線処理を簡略化し得るようになされている。
【0023】
このようにナビゲーション装置1は、GPSアンテナ2によりGPS信号を受信し得ない場合、自律速度算出ユニット11によって加速度センサ観測値AD及び気圧値PRを基に速度Vtを算出し、ナビゲーションユニット12によってこれらを基に現在位置(すなわち推定位置)を算出し得るようになされている。
【0024】
(2)速度の算出原理
次に、ナビゲーション装置1における自律速度算出ユニット11による速度Vtの算出原理について説明する。
【0025】
(2−1)進行方向加速度の算出原理
自律速度算出ユニット11では、車両の速度Vtを高精度に算出するべく、当該車両の進行方向の純粋な加速度(以下、これを進行方向加速度と呼ぶ)を検出したい、といった要求がある。
【0026】
しかしながら、ナビゲーション装置1がクレードル4を介して車両に取り付けられる場合、加速度センサ14の加速度検出軸は、当該車両の進行方向と一致しない可能性が高い。その場合、加速度センサ14から出力される加速度センサ観測値ADは、重力加速度gや方位変化に伴う横方向加速度の混入や、当該加速度センサ14における進行方向加速度のゲインの変化等による影響を受ける可能性がある。
【0027】
このため自律速度算出ユニット11は、加速度センサ14からの加速度センサ観測値ADをそのまま用いた場合、進行方向加速度の検出精度が大きく低下してしまう可能性がある。
【0028】
そこで自律速度算出ユニット11では、ナビゲーション装置1、車両及び地表面の間で座標の変換処理を行い、また重力加速度成分や横方向加速度成分の分離処理、さらにはオフセット調整処理等を行うことにより、進行方向加速度を高精度に算出し得るようになされている。
【0029】
(2−2)2つの座標軸間の関係
図3に示すように、xy座標系からx′y′座標系への変換行列Tを次式、
【0030】
【数1】
【0031】
で表し、xy座標系からみた位置p(cosθ,sinθ)が、x′y′座標系からみた位置p′(1,0)であるとした場合、次式の関係が成立し、これが、2つの座標系の関係を表す最も基本的な関係式となる。
【0032】
【数2】
【0033】
また座標軸と回転角度との関係を、図4に示すように定義する。すなわち、前後・左右・上下が定義できる物体において、前後方向をx軸、左右方向をy軸、上下方向をz軸とし、x軸周りの回転角度φを「ティルト」と呼び(以下、これをティルト角度φと呼ぶ)、y軸周りの回転角度θを「スイング」と呼び(以下、これをスイング角度θと呼ぶ)、z軸周りの回転角度ψを「パン」と呼ぶ(以下、これをパン角度ψと呼ぶ)。
【0034】
(2−3)道路傾斜が存在するときの地表面と車両との関係
次に、道路傾斜が存在するときの地表面と車両との関係について説明する。因みに、道路傾斜が存在しない地表面(路面)は、車両の進行方向に沿ったx軸、車両の進行方向とは直交した横方向(水平方向)に沿ったy軸、及び重力加速度方向に沿った鉛直方向のz軸によって表される。
【0035】
(2−3−1)地表面の地表座標系と道路傾斜の車両座標系
図5に示すように、道路傾斜上に位置している車両の車両座標系は、道路傾斜上で車両の進行方向に沿ったx′軸、道路傾斜上で車両の進行方向とは直交した横方向(水平方向)に沿ったy′軸、及び車両の高さ方向に沿ったz′軸によって表される。
【0036】
この道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)では、車両がx′軸方向に沿って太矢印方向へ進行する際の後ろ向きに働く加速度成分の符号を正と定義し、車両が右折する際の左側方向に働く加速度成分の符号を正と定義し、車両の上向き方向に働く重力加速度成分の符号を正と定義している。但し、符号の正負については、これに限るものではなく、上述の条件と逆であっても良い。
【0037】
この場合、地表面の地表座標系(x軸、y軸、z軸)と、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)とを関係付けるオイラー角で考えると、道路の進行方向における上り坂や下り坂を表す前後傾斜はy′軸周りのスイング角度θで示され、道路の進行方向と直交した横方向(水平方向)の右下下がりや左下下がりを表す左右傾斜は、x′軸周りのティルト角度φで示される。なお、z′軸周りの回転はパン角度ψで示されるが、当該パン角度ψによって表される道路の傾斜は存在しない。
【0038】
(2−3−2)地表面と車両との関係を示す座標変換式
ここで、地表面の地表座標系(x軸、y軸、z軸)に対する道路傾斜上に位置した車両の車両座標系(x′軸、y′軸、z′軸)の変換行列Aを、次式で定義する。
【0039】
【数3】
【0040】
この場合、変換行列Aは、道路の前後傾斜が先で、道路の左右傾斜が後なので、前後傾斜の項に対して左右傾斜の項を右側から乗算するようになされている。
【0041】
従って、地表座標系(x軸、y軸、z軸)は、次式で表される。
【0042】
【数4】
【0043】
これにより、道路傾斜上に位置する車両の車両座標系(x′軸、y′軸、z′軸)では、重力加速度gは、次式のように観測される。
【0044】
【数5】
【0045】
(2−4)車両と加速度センサとの関係
続いて、車両と、当該車両に取り付けられるナビゲーション装置1に搭載された加速度センサ14との関係について説明する。
【0046】
(2−4−1)車両の車両座標系及び加速度センサのセンサ座標系
図5に示したように、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)に対して、当該車両に対して任意の角度で取り付けられるナビゲーション装置1に搭載された3軸方向の加速度センサ14におけるセンサ座標系については、図6に示すようにx″軸、y″軸及びz″軸で表される。
【0047】
この場合、ナビゲーション装置1が車両に取り付けられたときの加速度センサ14に対するセンサ座標系(x″軸、y″軸、z″軸)としては、当該ナビゲーション装置1がユーザからみて上向き又は下向きに取り付けられた場合にy″軸周りのスイング角度θ′で示され、当該ナビゲーション装置1がユーザからみて左下下がり又は右下下がりに取り付けられた場合にx″軸周りのティルト角度φ′で示され、当該ナビゲーション装置1がユーザからみて右側運転席のドライバー向き又は左側助手席側のパッセンジャー向きに取り付けられた場合にz″軸周りのパン角度ψ′によって示される。
【0048】
(2−4−2)車両座標系とセンサ座標系との関係を示す座標変換式
ここで、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)に対する3軸方向の加速度センサ14におけるセンサ座標系(x″軸、y″軸、z″軸)の変換行列Bを、次式で定義する。
【0049】
【数6】
【0050】
この(6)式で表される変換行列Bでは、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に右側から乗算されるように選定されており、以降の計算過程においては一度選定された乗算順序を崩さないようにしなければならない。
【0051】
但し、変換行列Bとしては、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に限られるものではなく、例えば、道路の左右傾斜が加速度センサ14の取付角度の3軸(x″軸、y″軸、z″軸)に分配されて観測される場合、ティルト角度φ′、スイング角度θ′、パン角度ψ′の順番に右側から乗算されるように選定されてもよく、一度選定された乗算順序を崩さなければ、その他種々の乗算順序であってもよい。
【0052】
ここで、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)は、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)との関係で、次式のように表される。
【0053】
【数7】
【0054】
従って、(7)式を変形すれば、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)は、次式のように表される。
【0055】
【数8】
【0056】
ここで加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)は、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)が、上述したように(4)式の関係を有することから、(8)式は、次式で表される。
【0057】
【数9】
【0058】
従って、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)では、重力加速度gは、次式のように観測される。
【0059】
【数10】
【0060】
この重力加速度gを表す(10)式では、変換行列Bの逆行列B−1に相当する部分が車両に対する加速度センサ14の傾きを表し、かつ変換行列Aの逆行列A−1に相当する部分が地表面に対する車両の傾きを表している。
【0061】
実際上、加速度センサ14が車両に対して所定方向の傾きを持って取り付けられ、かつ道路傾斜上に車両が位置することにより当該車両が地表面に対して所定角度の傾きを持って存在している場合、重力加速度gは、次式のように観測される。
【0062】
【数11】
【0063】
(2−4−3)加速度センサに対する運動加速度(進行方向加速度及び横方向加速度)
また、加速度センサ14のセンサ座標系(x″軸、y″軸及びz″軸)では、車両の進行方向加速度αx及び横方向加速度 αyを定義し、道路に前後傾斜及び左右傾斜が存在せず、車両に対して加速度センサ14の取付傾きだけが生じている場合、加速度センサ14からみたときの進行方向加速度αx及び横方向加速度
αyを表す運動加速度αについては、変換行列Bの逆行列B−1を用いて、次式
【0064】
【数12】
【0065】
で表されるため、これを変換行列Bの逆行列B−1を用いて展開すると、次式のように観測される。
【0066】
【数13】
【0067】
因みに、車両の加速時には後ろ向きの加速度(いわゆるG)がかかること、及び右カーブでは左向きの遠心力が発生することに留意して、加速度センサ14のティルト角度φ′、スイング角度θ′及びパン角度ψ′に対する正負の符号が定められる。
【0068】
(2−5)加速度センサ観測値
加速度センサ14に対するセンサ座標系(x″軸、y″軸及びz″軸)の重力加速度gと運動加速度αとを合成したものを、当該加速度センサ14によって観測される加速度センサ観測値ASと定義した場合、当該加速度センサ観測値ASは、(10)式及び(12)式に基づいて、次式で表される。
【0069】
【数14】
【0070】
これを展開した場合、加速度センサ観測値ASは、次式のように表される。
【0071】
【数15】
【0072】
(2−5−1)加速度センサの零点オフセットとゲイン
ところで、電圧値として表される加速度センサ観測値ASとしては、加速度センサ14の零点オフセットOF及びゲインGEのことを考慮する必要がある。
【0073】
ここで加速度センサ14の零点オフセットOFとは、加速度センサ14に対して加速度成分が全く働いていないときのx″軸、y″軸及びz″軸にそれぞれ発生する電圧値([V])を示す。
【0074】
また加速度センサ14のゲインGEとは、加速度センサ14に対して例えば1g(=9.8[m/sec2])の負荷がかかったときのx″軸、y″軸及びz″軸にそれぞれ発生する電圧値([V/(m/sec2)])がどの程度変化するかを示す。
【0075】
この零点オフセットOF及びゲインGEを考慮したときの加速度センサ観測値ADは、(14)式に基づいて、次式で表される。
【0076】
【数16】
【0077】
なお、この(16)式中に含まれる加速度センサ観測値ASの単位は、[m/sec2]であり、加速度センサ観測値ADの単位としては、[V]となる。
【0078】
ここで零点オフセットOFは、次式
【0079】
【数17】
【0080】
で表され、ゲインGEは、次式で表される。
【0081】
【数18】
【0082】
従って、零点オフセットOF及びゲインGEを考慮したときの加速度センサ観測値ADは、次式で表されることになる。
【0083】
【数19】
【0084】
ここで、(19)式を構成する各要素のうち、ゲインGEx、GEy、GEz、零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′、道路の前後傾斜を示すスイング角度θ、道路の左右傾斜を示すティルト角度φがそれぞれ未知数である。
【0085】
しかし、ゲインGEx、GEy、GEzに関しては加速度センサ14の仕様書に記載されている値もしくは実際に検出された値を用い、道路の前後傾斜を示すスイング角度θについては、ナビゲーション装置1に内蔵された気圧センサ15の出力結果(気圧値PR)と対応付けられた高度情報を基に算出し、誤差は若干出るが道路の左右傾斜を示すティルト角度φはないものとして扱うものとする。
【0086】
但し、これら以外の例えば重力加速度gについては、GPS衛星から得られるGPS信号に基づいて算出した現在位置から緯度を取得し、緯度と重力加速度とが対応付けられた緯度−重力加速度変換テーブルから当該緯度に対応した重力加速度gを取得すればよい。
【0087】
また、車両の進行方向加速度αxについては、GPS信号に基づく車両の走行速度に基づいてリファレンスとして算出することが可能であり、車両の横方向加速度
αyについても、GPS信号に基づく車両の方位変化及び走行速度に基づきリファレンスとして算出することが可能である。
【0088】
これにより(19)式では、そのうち未知数が加速度センサ14の零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′の6個になり、これらを求める必要がある。
【0089】
(2−6)加速度センサの取付角度及びオフセットの学習
ところで、加速度センサ14の零点オフセットOFx、OFy、OFz及びゲインGEx、GEy、GEzを考慮した加速度センサ観測値AD=Iと定義し、(16)式中に含まれる次式
【0090】
【数20】
【0091】
と定義した場合、上述のI(加速度センサ観測値AD)は、次式で表される。なお、式中のSθ、Cθについては、それぞれsinθ、cosθを意味するものとする。
【0092】
【数21】
【0093】
ここで、(20)式中における変換行列Aの逆行列A−1は、地表面に対する道路の前後傾斜を表しており、これは気圧センサ15の出力結果(気圧値PR)に対応付けられている高度情報を基に導くことが可能であり、重力加速度g、進行方向加速度αx及び横方向加速度
αyについても、上述したようにリファレンスとして算出可能であるため、(21)式中のEは既知数である。
【0094】
この既知数Eは、(3)式に示したように変換行列Aの逆行列A−1を用いれば、次式のように変形することができる。
【0095】
【数22】
【0096】
従って、(21)式では、車両に対する加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)と、加速度センサ14の零点オフセットOFx、OFy、OFzとが未知であり、加速度センサ観測値ADは、次式のように変形することができる。
【0097】
【数23】
【0098】
この(23)式を更にニュートンラプソン法で用いられる形態に変形し、次式
【0099】
【数24】
【0100】
【数25】
【0101】
【数26】
【0102】
のような3つの関数式fx、fy、fzに表す。
【0103】
ここで、(24)式〜(26)式では未知数が零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個であるのに対し、3つの関数式fx、fy、fzしか存在しないので、このままでは解くことができない。
【0104】
但し、(24)式〜(26)式中の既知数Eの値は、毎秒得られるGPS信号に基づいて変化するため、少なくとも2秒間待てば、既知数Eの値が変化したそれぞれ異なる6種類の関数式fx1、fy1、fz1、fx2、fy2、fz2を得られるが、車両に加減速の変化がなければ、複数種類の関数式fx、fy、fzを得ることはできない。そこで、車両の実際の走行状態を想定し、加減速の変化が生じそうな10数秒分に相当する複数種類の関数式fx1〜fxt、fy1〜fyt、fz1〜fztを得ることとする。
【0105】
この場合、未知数の数(6個)よりも関数式fx1〜fxt、fy1〜fyt、fz1〜fztの数が遥かに多くなるものの、検出誤差等を考慮すると、解を一意に定めることができない可能性が高い。そこで、関数式fx1〜fxt、fy1〜fyt、fz1〜fztを満たすような尤もらしい未知数の値を得るべく、最小二乗法を用いることとする。
【0106】
具体的な計算手法として、ここでは多次元のニュートンラプソン法の適用を考える。基本的な1次元のニュートンラプソン法における変化量δは、次式
【0107】
【数27】
【0108】
で表される。ニュートンラプソン法では、この変化量δが所定の収束判定閾値以下になったとき、未知数の値の真解が得られたと認識する。
【0109】
この(27)式は、次式
【0110】
【数28】
【0111】
のように変形され、これをN次元に拡張すると、次式のように表される。
【0112】
【数29】
【0113】
従って、この場合、具体的には次式
【0114】
【数30】
【0115】
で表される左辺左側の偏導関数値と、右辺の関数値を計算することにより、左辺右側の最小二乗解(変化量δ)を求める。
【0116】
このようにして求められた最小二乗解(変化量δ)を用いて、未知数の解を次式
【0117】
【数31】
【0118】
によって算出し、この(31)式の左辺に示された未知数の解を用いて、再度(30)式により最小二乗解(変化量δ)を求め、この最小二乗解(変化量δ)を用いて(31)式の未知数の解を求めるといった繰返し計算を所定の回数分だけ行えば良い。
【0119】
ところで、(31)式で用いられる右辺第1項の初期値は、次式によって与えられる。
【0120】
【数32】
【0121】
この(32)式に示す初期値は、ティルト角度(φ′)0、スイング角度(θ′) 0及びパン角度(ψ′) 0 が0[rad]であることを示している。また、零点オフセット(OFx) 0、(OFy)
0、(OFz0 )については、加速度センサ14に加速度が一切作用していないときの電圧値が2.5([V])であることを示している。
【0122】
因みに加速度センサ14は、実際に検出した加速度に応じて、加速度センサ観測値ADの各成分(具体的には電位)をそれぞれ0[V]〜5[V]の範囲で変動させるようになされている。
【0123】
この(31)式では、左辺が最終的な解であり、右辺第1項が(31)式により前回求められた未知数の解(学習結果)を示し、右辺第2項が上述した(30)式の変化量δを示している。この(31)式の左辺により、零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′の6個の未知数が全て判明する。
【0124】
このように自律速度算出ユニット11は、車両がGPSアンテナ2によりGPS信号を受信し得るような走行環境下にあるとき、零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個の未知数を解として求める。
【0125】
その後、自律速度算出ユニット11は、車両がGPS信号を受信し得ないような走行環境となったとき、加速度センサ14の零点オフセットOFx、OFy、OFz、加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いると共に、加速度センサ14の観測結果や路面の高度差に基づいて進行方向加速度を推定し、これを基に積分計算することにより車両の速度や現在位置を算出(推定)するようになされている。
【0126】
因みに、(30)式〜(32)式中における添え字のiは、ニュートンラプソン法による繰り返し計算時の繰返回数を表しており、例えば、(30)式の右辺に含まれる(fx1)iは、次式
【0127】
【数33】
【0128】
を意味し、(30)式の左辺左側の行列に含まれる例えば∂(fx1)i/∂(ψ′)iは、∂fx1/∂ψ′に対して、((ψ′)i,(θ′)i,(φ′)i,(OFx)i,(OFy)i,(OFz)i)を代入したものを意味している。
【0129】
また、fx1の添え字(この場合であれば「1」)は、例えば10数秒間分まとめて処理する際の秒数を示す。従って、例えば10秒間分まとめて処理するのであれば、(30)式の左辺左側では、fx1〜fx10、fy1〜fy10、fz1〜fz10まで存在し、そのときの行列サイズは30(=3×10)行6列になる。
【0130】
なお、(31)式の左辺で得られた未知数の解としては、真値を中心にばらつきがある可能性が高く、また時間の経過とともに真値が変動する可能性もあるため、本発明では(31)式で得られる未知数の解を現時点までの例えば過去数秒〜数分間に渡って算出し、それを平均化又は平滑化することにより、そのときに応じた最終的な未知数の解を学習結果として求め得るようになされている。
【0131】
(2−7)加速度センサを用いた自律動作
ところで、加速度センサ観測値ADは、(16)式と同様、次式のように表される。
【0132】
【数34】
【0133】
この(34)式を進行方向加速度αx及び横方向加速度αyについて整理すると、次式
【0134】
【数35】
【0135】
のように変形され、この(35)式中の変換行列Bは、次式
【0136】
【数36】
【0137】
で表され、この(35)式中の右辺第2項は、次式のように表される。
【0138】
【数37】
【0139】
従って、上述の(35)式に対して(36)式及び(37)式を代入することにより、次式を得ることができる。
【0140】
【数38】
【0141】
【数39】
【0142】
【数40】
【0143】
この(38)式によって、進行方向加速度αxを求めるには、道路の前後傾斜を表すy′軸周りのスイング角度θが必要である。このスイング角度θは、次式
【0144】
【数41】
【0145】
により、高度差ΔHと、速度Vtとによって表される。
【0146】
この時点での未知数は、速度Vt、進行方向加速度αx、及び道路の前後傾斜を表すスイング角度θであるが、これらを一つ一つ解くのではなく、最終的に求めたい速度Vtを一気に求めることを考える。
【0147】
ここで、速度Vt−1は、これから最終的に求めたい速度Vtの一つ前の時点で判明した速度データである。従って、GPS信号を受信できていた状態からGPS信号を受信できなくなった最初の時点における速度Vt−1としては、GPS信号に基づいて最後に算出された速度V0があてはまる。
【0148】
この場合、(38)式のうち、既知の部分をβ(以下、これを既知部分βと呼ぶ)としてまとめると、既知部分βは、次式
【0149】
【数42】
【0150】
で表され、これを用いて(38)式を、次式
【0151】
【数43】
【0152】
のように変形することができる。
【0153】
ここで速度Vtと進行方向加速度αxとの関係は、次式
【0154】
【数44】
【0155】
のように表すことができ、上述の(41)式、(43)式を代入することにより、(44)式は、次式のように変形することができる。
【0156】
【数45】
【0157】
この(45)式を速度Vtについて整理すると、次式
【0158】
【数46】
【0159】
によって表され、これを速度Vtについて解くと、当該速度Vtは、次式によって表される。
【0160】
【数47】
【0161】
このように自律速度算出ユニット11では、GPS信号の受信時に、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ14の零点オフセットOFx、OFy、OFzを予め学習して記憶しておく。その後、自律速度算出ユニット11は、ビル等の陰に隠れる等してGPS信号を受信できなくなった場合、予め学習しておいた加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ14の零点オフセットOFx、OFy及びOFzを用い、加速度センサ観測値ADx″、ADy″、ADz″及び高度差ΔHに基づいて進行方向加速度αxを自律的に算出(推定)することができるので、その自律的に算出した進行方向加速度αxを基に車両の速度Vt(以下、これを自律速度Vtと呼ぶ)を求めることができる。
【0162】
これに応じてナビゲーション装置1では、自律速度算出ユニット11により算出された車両の自律速度Vtを積分することにより、GPS信号を受信できない走行環境下であっても、当該車両の現在位置を正確に算出し、表示部3の地図上に現在位置を継続して表示し得るようになされている。
【0163】
(3)自律速度算出ユニットの具体的構成
次に、上述した速度の算出原理に従い速度Vtを算出する自律速度算出ユニット11の構成について、GPS測位時(学習時)とGPS非測位時(自律時)とに分けてそれぞれ説明する。
【0164】
(3−1)GPS測位時(学習時)における自律速度算出ユニットの構成
図7において、自律速度算出ユニット11は、図示しないCPU(Central Processing Unit)を中心に構成されており、図示しないROM(Read Only Memory)から基本プログラムや速度算出プログラム等の各種アプリケーションプログラムを読み出して図示しないRAM(Random Access Memory)上に展開することにより、道路傾斜計算部20、学習処理部21及び自律計算部22といった各処理機能をソフトウェア的に実現し得るようになされている。
【0165】
但し、自律速度算出ユニット11としては、これに限るものではなく、当該自律速度算出ユニット11における道路傾斜計算部20、学習処理部21及び自律計算部22をハードウェア的に構成するようにしても良い。
【0166】
自律速度算出ユニット11は、GPS処理部10により算出される車両の速度V及び方位データDを道路傾斜計算部20及び学習処理部21へ供給する。また自律速度算出ユニット11は、気圧センサ15によって検出された気圧値PRを道路傾斜計算部20へ供給すると共に、加速度センサ14によって観測される加速度センサ観測値ADを学習処理部21へ供給する。
【0167】
道路傾斜計算部20の進行距離算出部31は、GPS処理部10から供給される速度Vに基づいて車両の進行距離Lmを算出し、その進行距離Lmを前後傾斜算出部33へ送出する。
【0168】
また、道路傾斜計算部20の高度差算出部32は、一般的な気圧と高度との対応関係が予めテーブル化された気圧高度対応テーブルTBLを記憶部13(図2)に記憶しており、時刻t0における気圧値PR0及び時刻t1における気圧値PR1を基に、気圧高度対応テーブルTBLから気圧値PR0及びPR1にそれぞれ対応した高度情報h0及びh1を読み出す。
【0169】
そして高度差算出部32は、気圧値PR0に対応した車両の高度情報h0と、気圧値PR1に対応した車両の高度情報h1との差分である高度差ΔHを算出し、これを前後傾斜算出部33へ送出するようになされている。
【0170】
前後傾斜算出部33は、進行距離算出部31から供給される進行距離Lmと、高度差算出部32から供給される高度差ΔHとに基づいて、車両座標系(x′軸、y′軸、z′軸)において道路の前後傾斜を示すスイング角度θを算出し、これを学習処理部21の取付角度及びオフセット学習処理部44へ送出する。
【0171】
学習処理部21の進行方向加速度算出部41は、GPS処理部10から供給される速度Vを微分することにより車両の進行方向加速度αxをリファレンスとして算出し、これを取付角度及びオフセット学習処理部44へ送出する。
【0172】
一方、角速度算出部42は、GPS処理部10から供給される方位データDを微分することにより角速度dDを算出し、これを横方向加速度算出部43へ送出する。横方向加速度算出部43は、GPS処理部10から供給される速度V及び角速度算出部42から供給される角速度dDに基づいて横方向加速度αyをリファレンスとして算出し、これを取付角度及びオフセット学習処理部44へ送出する。
【0173】
取付角度及びオフセット学習処理部44は、加速度センサ14から供給される加速度センサ観測値ADと、進行方向加速度算出部41から供給される進行方向加速度αxと、横方向加速度算出部43から供給される横方向加速度αyとを基に、加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を学習し得るようになされている。
【0174】
このようなGPS測位時における自律速度算出ユニット11の学習処理手順をまとめてみれば、図8に示すように自律速度算出ユニット11は、ルーチンRT1の開始ステップから入って次のステップSP1へ移る。このステップSP1において自律速度算出ユニット11は、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度V及び方位データDをGPSデータとして取得すると共に学習処理部21へ供給し、次のステップSP2へ移る。
【0175】
ステップSP2において自律速度算出ユニット11は、加速度センサ14の出力である加速度センサ観測値ADを取得して学習処理部21の取付角度及びオフセット学習処理部44へ供給し、次のステップSP3へ移る。
【0176】
ステップSP3において自律速度算出ユニット11は、道路傾斜計算部20の進行距離算出部31で算出された進行距離Lm、及び高度差算出部32で算出された高度差ΔHに基づいて、前後傾斜算出部33により車両座標系(x′軸、y′軸、z′軸)において道路の前後傾斜を示すスイング角度θを算出し、次のステップSP4へ移る。
【0177】
ステップSP4において自律速度算出ユニット11は、ステップSP1で取得した車両の速度Vが速度情報として信頼たり得るデータであるか否かを当該速度Vに付加されているフラグに基づいて判断し、否定結果が得られたときは、速度Vが信頼できない速度情報に過ぎないため、次のステップSP16へ移って処理を終了する。
【0178】
これに対してステップSP4で肯定結果が得られると、自律速度算出ユニット11は次のステップSP5へ移り、GPS処理部10から受け取った速度Vを基に進行方向加速度算出部41により進行方向加速度αxをリファレンスとして算出すると共に、GPS処理部10から受け取った速度V及び方位データDを基に横方向加速度算出部43により横方向加速度αyをリファレンスとして算出し、次のステップSP6へ移る。
【0179】
ステップSP6において自律速度算出ユニット11は、加速度センサ観測値AD及び上述の(22)式で示された既知数Eを学習用入力データとして設定し、次のステップSP7へ移る。
【0180】
ステップSP7においてカーナビゲーション装置1は、加速度センサ14のオフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるために必要な関数式が未知数よりも多いか否かを判定する。
【0181】
ここで否定結果が得られると、このことは関数式が未知数よりも多くないため、これら未知数を求めることが出来ないことを表しており、このとき自律速度算出ユニット11はステップSP16へ移って処理を終了する。
【0182】
これに対してステップSP7で肯定結果が得られると、このことは関数式が未知数よりも多いため、これら未知数を求めることが可能であることを表しており、このとき自律速度算出ユニット11は次のステップSP8へ移る。
【0183】
ステップSP8において自律速度算出ユニット11は、このルーチンRT1における学習処理手順によって求められた前回の学習結果(加速度センサ14のオフセットOFx、OFy及びOFz並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′)の値を上述した(30)式の左辺左側の行列の値として初期値設定し、次のステップSP9へ移る。
【0184】
ステップSP9において自律速度算出ユニット11は、上述した(30)式における左辺左側の偏導関数値と、右辺の関数値を計算し、次のステップSP10へ移る。
【0185】
ステップSP10において自律速度算出ユニット11は、上述した(30)式における右辺の各要素(関数値)の合計が所定の収束判定閾値よりも小さくなったか否かを判定し、肯定結果が得られると次のステップSP15へ移り、否定結果が得られると次のステップSP11へ移る。
【0186】
ステップSP11において自律速度算出ユニット11は、上述した(30)式の右辺における関数値の各要素の合計が、ニュートンラプソン法における所定の収束判定閾値よりも小さくなっていないので、ステップSP9で計算した偏導関数値及び関数値により、(30)式における左辺右側の最小二乗解(変化量δ)を求め、次のステップSP12へ移る。
【0187】
ステップSP12において自律速度算出ユニット11は、ステップSP11で求めた最小二乗解(変化量δ)を用いて(31)式の左辺に相当する最終解(加速度センサ14のオフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′)を求め、次のステップSP13へ移る。
【0188】
ステップSP13において自律速度算出ユニット11は、(30)式の左辺右側で示される最小二乗解(変化量δ)の各要素の合計が所定の収束判定閾値よりも小さいか否かを判定し、否定結果が得られると次のステップSP14へ移るのに対し、肯定結果が得られると次のステップSP15へ移る。
【0189】
ステップSP14において自律速度算出ユニット11は、ステップSP9〜ステップSP14までの処理を繰返し行うニュートンラプソン法のループ回数が所定のループ回数閾値よりも少ないか否かを判定し、肯定結果が得られると、ステップSP9へ戻って、以降の処理を繰り返す。この場合、ステップSP9において自律速度算出ユニット11は、直前に求められた加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いて、上述した(30)式中における左辺左側の偏導関数値と、右辺の関数値を計算する。
【0190】
これに対し、ステップSP14で否定結果が得られると、繰返し計算のループ回数が所定のループ回数閾値よりも多いにも拘わらず、ステップSP10又はステップSP13で関数値の各要素の合計又は最小二乗解(変化量δ)の各要素の合計が収束しなかったことを表しており、このとき自律速度算出ユニット11は、次のステップSP16へ移って未知数の解を求めることなく終了する。
【0191】
ステップSP15において自律速度算出ユニット11は、(31)式の左辺で求められた未知数の最終解を現時点まで過去数秒〜数分間に渡って算出し、その結果得られる複数個の最終解を用いて平滑化処理することにより、最も真値に近い最終解を学習結果として得、次のステップSP16へ移って処理を終了する。
【0192】
なお自律速度算出ユニット11では、ステップSP15において平滑化処理に限るものではなく、処理量を減少させたいのであれば、単純な平均化処理することにより学習結果を得るようにしても良い。
【0193】
(3−2)GPS非測位時(自律時)における自律速度算出ユニット11の構成
図9に示すように、自律速度算出ユニット11は、GPS信号を受信できていた走行環境下からGPS信号を受信できない走行環境下になった場合、GPS受信部3から速度V及び方位データDを受け取ることができなくなる。このとき自律速度算出ユニット11は、気圧センサ15からの気圧値PRを道路傾斜計算部20の高度差算出部32へ供給すると共に、加速度センサ14からの加速度センサ観測値ADを自律計算部22の速度算出部51へ供給する。
【0194】
道路傾斜計算部20の高度差算出部32は、気圧高度対応テーブルTBLに基づいて、時刻t0における気圧値PR0に対応した高度情報と時刻t1における気圧値PR1に対応した高度情報との差分である高度差ΔHを算出し、これを自律計算部22の速度算出部51へ送出する。
【0195】
また、学習処理部21の取付角度及びオフセット学習処理部44は、GPS信号を受信できているときに予め学習し記憶しておいた加速度センサ14の零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を自律計算部22の速度算出部51へ送出する。
【0196】
速度算出部51は、高度差算出部32から供給される高度差ΔHと、加速度センサ14から供給される加速度センサ観測値ADと、オフセット学習処理部43から供給される加速度センサ14の零点オフセットOFx、OFy、OFzと、加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′とを基に、(47)式等を用いて進行方向加速度αxを自律的に算出(推定)し、その進行方向加速度αxを基に車両の自律速度Vtを算出(推定)し得るようになされている。
【0197】
従ってナビゲーションユニット12は、GPS信号を受信できない走行環境下であっても、このようにして求めた車両の自律速度Vtを積分することにより当該車両の現在位置を正確に算出し、表示部3の地図上に現在位置を継続して表示し得るようになされている。
【0198】
このようなGPS非測位時における自律速度算出ユニット11の自律速度算出処理手順をまとめてみれば、図10に示すように自律速度算出ユニット11は、ルーチンRT2の開始ステップから入って次のステップSP21へ移る。このステップSP2において自律速度算出ユニット11は、複数のGPS衛星からのGPS信号に基づいて算出された車両の速度V及び方位データDをGPSデータとしてGPS処理部10から学習処理部21で受け取ることにより取得し、次のステップSP22へ移る。
【0199】
ステップSP22において自律速度算出ユニット11は、気圧センサ15から供給される気圧値PRを道路傾斜計算部20の高度差算出部32で受け取ることにより取得し、次のステップSP23へ移る。
【0200】
ステップSP23において自律速度算出ユニット11は、高度差算出部32により気圧高度対応テーブルTBLを用いて高度差ΔHを算出し、次のステップSP24へ移る。
【0201】
ステップSP24において自律速度算出ユニット11は、ステップSP21で取得した車両の速度Vが速度情報として信頼たり得るデータであるか否かを当該速度Vに付加されているフラグに基づいて判断し、肯定結果が得られたときは、速度Vが信頼できる速度情報であって、GPS信号を受信できている走行環境下にあるため、自律速度算出処理を行う必要がなく、次のステップSP27へ移って処理を終了する。
【0202】
これに対してステップSP24で否定結果が得られると、このことはGPS信号を受信できない走行環境下であって、速度Vが信頼できない速度情報に過ぎないことを表しており、このとき自律速度算出ユニット11は次のステップSP25へ移る。
【0203】
ステップSP25において自律速度算出ユニット11は、GPS信号を受信できない走行環境下にあるので、予めGPS信号の受信時に学習しておいた加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、零点オフセットOFx、OFy、OFzを用いて、上述の(43)式等を用いた計算処理により進行方向加速度αxを推定し、これを積分計算することにより車両の自律速度Vtを求め、次のステップSP26へ移る。
【0204】
ステップSP26において自律速度算出ユニット11は、このようにして求めた車両の自律速度Vtをナビゲーションユニット12へ出力し、次のステップSP27へ移って処理を終了する。
【0205】
これに応じてナビゲーションユニット12は、GPS信号を受信できない走行環境下であっても、自律速度Vtに基づいて当該車両の現在位置を一段と正確に推定して表示部3の地図上に現在位置を継続表示することができる。
【0206】
(4)ナビゲーション装置及びクレードルの着脱状態と学習結果の利用
(4−1)クレードルの着脱状態の検出と自律速度の算出
ところでナビゲーション装置1(図1)は、上述したように、車両に取り付けられたクレードル4に対して容易に装着され、また容易に取り外され得るようになされている。
【0207】
このため、ナビゲーション装置1に関しては、例えばユーザにより当該ナビゲーション装置1がクレードル4から取り外され、当該ユーザの手元で行き先設定や経由地設定等の操作が行われた後、当該クレードル4に再度装着される、といった使用状況が想定され得る。
【0208】
このようにクレードル4から取り外されているとき、ナビゲーション装置1は、車両に対する相対的な角度、すなわち加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)が、当該クレードル4に装着されていたときとは異なることになる。
【0209】
しかしながら、上述した(6)式において定義したセンサ座標系は、ナビゲーション装置1(すなわち加速度センサ14)が車両に対して固定され動かないことを前提としている。
【0210】
このためナビゲーション装置1の自律速度算出ユニット11(図2)は、クレードル4から取り外されている間、GPSアンテナ2によってGPS信号を受信できない場合、上述した手法により学習した加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)を用いると、誤った自律速度Vtを算出することになってしまう。
【0211】
そこでナビゲーション装置1は、クレードル4との着脱状態を検出し、当該クレードル4との着脱状態に応じて自律速度算出ユニット11における自律速度Vtの算出手法を切り換えるようになされている。
【0212】
具体的にナビゲーション装置1は、クレードル着脱検出部16によりクレードル4との着脱状態を検出し、着脱情報CDを自律速度算出ユニット11へ供給するようになされている。
【0213】
自律速度算出ユニット11は、着脱情報CDを基にクレードル4に装着されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、(6)式において定義したセンサ座標系が保持されていることから、上述した手法により学習した加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′(以下、これらをまとめて学習結果と呼ぶ)を用いることにより、高精度に自律速度Vtを算出し出力する。
【0214】
一方、自律速度算出ユニット11は、着脱情報CDを基にクレードル4から取り外されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、(6)式において定義したセンサ座標系が変化してしまったことから、当該クレードル4に装着されていたときの最後の自律速度Vtが継続しているものと見なし、当該自律速度Vtをそのまま出力する。
【0215】
この場合、自律速度算出ユニット11は、一般に車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Vtを出力する。すなわち自律速度算出ユニット11は、加速度センサ14の学習結果を利用できず高精度な自律速度Vtを算出できないものの、いわば次善の策として直前の自律速度Vtを出力する。
【0216】
これにより、ナビゲーションユニット12において、実際の現在位置に比較的近い現在位置が算出されることを期待できる。
【0217】
またナビゲーション装置1は、クレードル4に再度装着された場合、当該クレードル4との嵌合精度や車両に対するクレードル4の取付位置のずれ等の要因により、車両に対する取付傾きが前回と近い状態にあるとは想定されるものの、当該取付傾きが前回とは微妙に異なっている可能性がある。
【0218】
そこで自律速度算出ユニット11は、ナビゲーション装置1がクレードル4に再度装着されたことを検出した場合、当該クレードル4から取り外される直前に学習した学習結果を初期値として、再度学習処理を行うようになされている。
【0219】
このとき自律速度算出ユニット11は、前回の学習結果をそのまま用いるのではなく、再度学習処理を行うことにより、取付傾きの違いによる誤った自律速度Vtを出力せずに済む。また自律速度算出ユニット11は、前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。
【0220】
因みにナビゲーションユニット12は、GPSアンテナ2によってGPS信号を受信できる場合、ナビゲーション装置1とクレードル4との着脱状態に拘わらず、GPS処理部10から供給される位置情報PSを基に、現在位置を高精度に算出し得るようになされている。
【0221】
このように自律速度算出ユニット11は、クレードル4との装着状態に応じて取付傾き等の学習結果を利用するか否かを切り換えることにより、自律速度Vtの算出精度をできるだけ低下させないようになされている。
【0222】
(4−2)自律速度算出切換処理手順
実際上、自律速度算出ユニット11は、クレードル4との装着状態及びGPS信号の受信状態に応じて学習処理又は自律速度算出処理を切り換える場合、図11に示す自律速度算出切換処理手順RT3に従った処理を行うようになされている。
【0223】
すなわち自律速度算出ユニット11は、所定の周期(例えば1秒)毎に自律速度算出切換処理手順RT3を開始し、ステップSP31へ移る。ステップSP31において自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されているか否かを判定する。ここで肯定結果が得られると、このことはセンサ座標系が保持されていることを表しており、このとき自律速度算出ユニット11は次のステップSP32へ移る。
【0224】
ステップSP32において自律速度算出ユニット11は、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度Vが速度情報として信頼たり得るデータであるか否かを、当該速度Vに付加されているフラグに基づいて判定する。ここで肯定結果が得られると、このことはGPS信号を十分に受信できており学習処理を行い得ることを表しており、このとき自律速度算出ユニット11は次のステップSP33へ移る。
【0225】
ステップSP33において自律速度算出ユニット11は、過去数回分の着脱情報CD等を基に、ナビゲーション装置1がクレードル4に装着された直後であるか否かを判定する。ここで否定結果が得られると、このことはナビゲーション装置1がしばらくの間クレードル4に装着されたまま取り外されておらず、これまでの学習結果を継続して利用し得ることを表しており、このとき自律速度算出ユニット11は次のステップSP34へ移る。
【0226】
ステップSP34において自律速度算出ユニット11は、上述したGPS受信時における学習処理手順RT1(図8)を実行して学習結果を更新し、その後ステップSP40へ移って処理を終了する。
【0227】
一方ステップSP33において肯定結果が得られると、このことはナビゲーション装置1がクレードル4に再装着された直後であるため、加速度センサ14の取付傾き等を学習し直す必要があることを表しており、このとき自律速度算出ユニット11は、次のステップSP35へ移る。
【0228】
ステップSP35において自律速度算出ユニット11は、ナビゲーション装置1がクレードル4から取り外される前に学習した学習結果を記憶部13から読み出して初期値とし、GPS受信時における学習処理手順RT1(図8)を実行して新たな学習結果を算出し、その後ステップSP40へ移って処理を終了する。
【0229】
一方、ステップSP32において否定結果が得られると、このことはGPS信号を受信できない走行環境下にあることを表しており、このとき自律速度算出ユニット11は、次のステップSP36へ移る。
【0230】
ステップSP36において自律速度算出ユニット11は、自律速度算出処理手順RT2(図10)を実行することにより自律速度Vtを算出して出力し、その後ステップSP40へ移って処理を終了する。
【0231】
これに対して、ステップSP31において否定結果が得られると、このことはナビゲーション装置1がクレードル4から取り外されているために、センサ座標系が変化してしまっていることを表しており、このとき自律速度算出ユニット11は次のステップSP38へ移る。
【0232】
ステップSP38において自律速度算出ユニット11は、ステップSP32と同様、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度Vが速度情報として信頼たり得るデータであるか否かを、当該速度Vに付加されているフラグに基づいて判定する。
【0233】
このステップSP38において肯定結果が得られると、このことはGPS信号を十分に受信できる走行環境下にあり自律速度Vtを算出する必要がないことを表しており、このとき自律速度算出ユニット11は、次のステップSP40へ移って処理を終了する。
【0234】
一方、ステップSP38において否定結果が得られると、このことはGPS信号を受信できない走行環境下にあり、且つセンサ座標系が変化してしまっているために、加速度センサ14からの加速度センサ観測地ADを基に自律速度Vtを算出し得ないことを表しており、このとき自律速度算出ユニット11は、次のステップSP39へ移る。
【0235】
ステップSP39において自律速度算出ユニット11は、最後に算出した自律速度Vt又は最後にGPS処理部10から取得した速度Vのうち新しい方を自律速度Vtとして出力し、次のステップSP40へ移って処理を終了する。
【0236】
その後ナビゲーションユニット12は、自律速度算出ユニット11から供給される自律速度Vtを基に現在位置(推定位置)を算出し、当該現在位置に応じた地図情報や経路案内等を表示部3に表示させる。
【0237】
(5)動作及び効果
以上の構成において、ナビゲーション装置1の自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1とクレードル4との着脱状態を認識する。
【0238】
自律速度算出ユニット11は、着脱情報CDを基にクレードル4に装着されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、学習した学習結果(すなわち加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′)を用いることにより、高精度に自律速度Vtを算出し出力する。
【0239】
一方、自律速度算出ユニット11は、着脱情報CDを基にクレードル4から取り外されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、当該クレードル4に装着されていたときの最後の速度が継続しているものと見なし、当該最後の速度をそのまま出力する。
【0240】
この場合、自律速度算出ユニット11は、ナビゲーション装置1がクレードル4から取り外されたときに、(6)式におけるセンサ座標系が変化してしまったことを認識することができ、学習結果である加速度センサ14の取付傾き(すなわちティルト角度φ′、スイング角度θ′及びパン角度ψ′)を誤って用いることなく、自律速度Vtの算出を中止することができる。
【0241】
このとき自律速度算出ユニット11は、クレードル着脱検出部16によりクレードル4から取り外されたことを検出した時点から学習結果の利用を中止するため、ナビゲーション装置1が当該クレードル4から取り外される際にユーザにより加えられる力を車両の速度の変化として誤認識してしまうことも未然に防止できる。
【0242】
また自律速度算出ユニット11は、加速度センサ14の学習結果を利用できず高精度な自律速度Vtを算出できないものの、車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Vt実際の車両の速度に近い値として出力することができる。
【0243】
一方、自律速度算出ユニット11は、ナビゲーション装置1がクレードル4に再度装着されたことを認識した場合、当該クレードル4から取り外される直前に学習した学習結果を初期値として再度学習処理を行う。
【0244】
この場合、自律速度算出ユニット11は、車両に対する取付傾きが前回とは微妙に異なっている可能性があるため、取付傾きの違いによる誤った自律速度Vtを出力せずに済み、また前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。
【0245】
この結果、ナビゲーション装置1では、クレードル4との着脱状態やGPS信号の受信状態に拘わらず、ナビゲーションユニット12において、現在位置が常に高精度に算出されることを期待できる。
【0246】
以上の構成によれば、ナビゲーション装置1の自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されている場合、学習した取付傾きを用いて高精度な自律速度Vtを算出することができ、一方ナビゲーション装置1がクレードル4から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ14の取付傾きを用いた自律速度Vtの算出を中止するため、誤った自律速度Vtの出力を未然に防止することができ、ナビゲーションユニット12における現在位置の算出精度を低下させずに済む。
【0247】
(6)他の実施の形態
なお上述した実施の形態においては、ナビゲーション装置1がクレードル4から取り外され、且つGPS信号を充分に受信できない場合、自律速度算出ユニット11から最後に算出した自律速度Vt又は最後にGPS処理部10から取得した速度Vを現在の自律速度Vtとして出力するようにした場合について述べたが、本発明はこれに限らず、例えば直前の速度Vの変化の割合を算出し、当該変化の割合に従って自律速度Vtを変化させていくようにし、又は自律速度Vtを一切出力しないようにする等しても良く、要はクレードル4に装着されていたときに学習した取付傾きを用いないようにすれば良い。
【0248】
また上述した実施の形態においては、ナビゲーション装置1がクレードル4から一度取り外されて再度装着された場合に、前回の学習結果を初期値として再度学習を行うようにした場合について述べたが、本発明はこれに限らず、例えば(16)式のような初期値を用いて学習処理を最初からやり直すようにし、又は前回の学習結果を用いて直ちに自律速度Vtの算出を開始する等しても良い。
【0249】
さらに上述した実施の形態においては、(30)〜(32)式を用いたニュートンラプソン法により零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個の未知数を解として求めるようにした場合について述べたが、本発明はこれに限らず、他の種々の算出手法により零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるようにしても良い。
【0250】
さらに上述した実施の形態においては、(42)式〜(47)式を用いて自律速度Vtを算出するようにした場合について述べたが、本発明はこれに限らず、加速度センサ観測値ADx″、ADy″、ADz″を用いた他の種々の演算手法により自律速度Vtを算出するようにしても良い。この場合、高度差ΔHを用いずに当該自律速度Vtを算出するようにしても良い。
【0251】
さらに上述した実施の形態においては、本発明のナビゲーション装置1を移動体としての車両に搭載するようにした場合について述べたが、本発明はこれに限らず、船舶、電車、その他種々の移動体に搭載するようにしても良い。
【0252】
さらに上述した実施の形態においては、GPSアンテナ2を介して複数のGPS衛星からのGPS信号を受信し、これら複数のGPS信号に基づきGPS処理部10により車両の速度Vを算出すると共に車両の方位データDを算出するようにした場合について述べたが、本発明はこれに限らず、例えば準天頂衛星システム、グローナス(GLONASS:Global Navigation Satellite System)やガリレオ(GALILEO)等の種々の衛星測位システムを利用し、それぞれの衛星信号に基づき車両の速度Vを算出すると共に車両の方位データDを算出するようにしても良い。
【0253】
さらに上述の実施の形態においては、自律速度算出ユニット11がROMから読み出して展開した速度算出プログラムに従って上述したルーチンRT3の自律速度算出切換処理手順(図11)を実行するようにした場合について述べたが、本発明はこれに限らず、記録媒体からインストールした速度算出プログラムや、インターネットからダウンロードした速度算出プログラムに従ってルーチンRT3の自律速度算出切換処理手順を実行するようにしても良い。
【0254】
さらに上述した実施の形態においては、本体部としてのナビゲーション装置1と、加速度センサとしての加速度センサ14と、推定速度算出手段としての自律速度算出ユニット11及びナビゲーションユニット12と、着脱検出子としてのクレードル着脱検出部16と、加速度算出手段、取付傾き算出手段及び推定速度算出制御手段としての自律速度算出ユニット11とによってナビゲーション装置としてのナビゲーション装置1を構成する場合について述べたが、本発明はこれに限らず、その他種々の回路構成でなる本体部と、加速度算出手段と、加速度センサと、取付傾き算出手段と、推定速度算出手段と、着脱検出子と、推定速度算出制御手段とによってナビゲーション装置を構成するようにしても良い。
【産業上の利用可能性】
【0255】
本発明は、本発明のナビゲーション装置、ナビゲーション情報算出方法及びナビゲーション情報算出プログラムは、例えば車両に対してクレードルを介して着脱されるポータブルタイプのナビゲーション装置に適用したが、クレードルが用意されGPS受信機能を有するPDA(Personal Digital Assistant)、携帯電話機、パーソナルコンピュータ等の種々の電子機器に適用することができる。
【図面の簡単な説明】
【0256】
【図1】ナビゲーション装置及びクレードルの構成を示す略線的斜視図である。
【図2】ナビゲーション装置の回路構成を示す略線図である。
【図3】2つの座標軸間の関係を示す略線的斜視図である。
【図4】角度の定義の説明に供する略線図である。
【図5】道路傾斜がある地表面上に位置している車両の車両座標系の説明に供する略線図である。
【図6】車両に取り付けられた加速度センサのセンサ座標系の説明に供する略線図である。
【図7】GPS測位時(学習時)におけるナビゲーション装置の構成を示す略線的ブロック図である。
【図8】GPS受信時における学習処理手順を示す略線的フローチャートである。
【図9】GPS非測位時(自律時)におけるナビゲーション装置の構成を示す略線的ブロック図である。
【図10】自律速度算出処理手順を示す略線的フローチャートである。
【図11】自律速度算出切換処理手順を示す略線的フローチャートである。
【符号の説明】
【0257】
1……ナビゲーション装置、2……GPSアンテナ、3……表示部、4……クレードル、10……GPS処理部、11……自律速度算出ユニット、12……ナビゲーションユニット、13……記憶部、14……加速度センサ、15……気圧センサ、16……クレードル着脱検出部、AD……加速度センサ観測値、CD……着脱情報、V……速度、D……方位データ、Vt……自律速度。
【技術分野】
【0001】
本発明は、ナビゲーション装置、位置検出方法及び位置検出プログラムに関し、例えば車両に搭載されるナビゲーション装置に適用して好適なものである。
【背景技術】
【0002】
従来、ナビゲーション装置においては、GPS(Global Positioning System)衛星等の測位手段から受信した測位情報(例えばGPS信号)に基づいて車両の現在位置を算出して表示するが、例えばトンネル等の中に入って当該GPS信号を受信できなくなったときは加速度センサの検出結果に基づいて算出された車両の進行方向加速度に基づいて車両の走行速度を推定し、当該走行速度に基づいて推定した現在位置を表示することが行われている(例えば、特許文献1参照)。
【特許文献1】特開2004-138553公報
【発明の開示】
【発明が解決しようとする課題】
【0003】
ところでかかる構成のナビゲーション装置においては、加速度センサの検出結果に基づいて進行方向加速度を算出するようになされているが、それは、あくまで加速度センサの加速度検出軸が車両の進行方向と一致していることが前提である。
【0004】
しかしながら、取り付け及び取り外しが容易なポータブルタイプのナビゲーション装置における実際の使用環境では、加速度センサが搭載されている本体部の取付傾きがユーザ毎に異なり、必ずしも加速度センサの加速度検出軸と車両の進行方向とが一致した設置条件を満たせるわけではない。
【0005】
そこで、ナビゲーション装置において、GPS衛星から受信したGPS信号に基づいて算出される進行方向を基に、加速度センサの検出結果に基づいて進行方向加速度とのずれ量(すなわち取付傾き)を算出する手法が考えられる。この場合ナビゲーション装置は、この取付傾きを用いて進行方向加速度を算出すれば、車両の走行速度や現在位置を高精度に推定し得ると考えられる。
【0006】
しかしながら、かかるナビゲーション装置は、車両から取り外された場合、車両に対する相対的な位置や角度が変化してしまうため、算出した取付傾きをそのまま使い続けると、進行方向加速度を不適切に算出することになってしまい、車両の走行速度や現在位置の算出精度を大幅に低下させてしまう可能性があるという問題があった。
【0007】
本発明は以上の点を考慮してなされたもので、測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを提案しようとするものである。
【課題を解決するための手段】
【0008】
かかる課題を解決するため本発明においては、所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出し、測位情報を基に算出された移動体の速度に応じて当該移動体に作用する移動体加速度を算出し、本体部に作用する加速度を観測し、移動体加速度及び観測された加速度に基づいて加速度センサの移動体に対する取付傾きを求め、測位情報が供給されないときに、観測された加速度及び取付傾きを基に移動体の推定速度及び推定位置を算出し、本体部が台座部に対して装着されていないことが検出された場合、推定速度の算出を停止させるようにした。
【0009】
これにより、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止できる。
【発明の効果】
【0010】
本発明によれば、本体部が台座部から取り外されたときに、推定速度の算出を停止させることができ、本体部が台座部に装着されていることを前提に学習した取付傾きを用いることによる、誤った推定速度及び推定位置の算出を未然に防止でき、かくして測位情報を取得できないときであっても、移動体の速度及び現在位置を高精度に算出し得るナビゲーション装置、位置検出方法及び位置検出プログラムを実現できる。
【発明を実施するための最良の形態】
【0011】
以下、図面について、本発明の一実施の形態を詳述する。
【0012】
(1)ナビゲーション装置の構成
(1−1)外観構成
図1において、ナビゲーション装置1は、GPSアンテナ2によりGPS衛星(図示せず)からのGPS信号等を基に現在位置を算出し、表示部3に地図画面や所望の目的地までの経路案内画面等を表示することにより、ユーザに現在位置や周辺地図、或いは案内経路等を提示し得るようになされている。
【0013】
このナビゲーション装置1は、ユーザにより容易に持ち運ばれ得るようポータブル型に構成され、当該ユーザの家庭内や車両の車室内において使用されることが想定されている。
【0014】
またナビゲーション装置1は、台座部としての専用のクレードル4に容易に装着され得ると共に、当該クレードル4から容易に取り外され得るようになされている。実際上、このクレードル4が車両の車室内におけるダッシュボード上等に予め取り付けられることにより、ナビゲーション装置1は、当該車両に対して容易に取り付けられ、又は容易に取り外されるようになされている。
【0015】
因みにGPSアンテナ2は、フロントガラスの下等に設置されることにより、車両のボディ等により遮断されることなく、上空の衛星軌道を周回するGPS衛星からのGPS信号を良好に受信し得るようになされている。
【0016】
(1−2)ナビゲーション装置の回路構成
図2に示すように、ナビゲーション装置1は、上述した表示部3に加え、GPS信号の受信処理等を行うGPS処理部10、GPS信号を受信し得ない際に各種センサによる検出値を基に速度を算出する演算処理ブロック11、GPS処理部10又は自律速度算出ユニット11からの情報を基に経路案内画面等を表す表示データを生成するナビゲーションユニット12、及び各種データ等を記憶する記憶部13等を有している。
【0017】
GPS処理部10は、GPSアンテナ2を介して複数のGPS衛星(図示せず)からのGPS信号を受信し、当該GPS信号を基に所定の位置算出処理を行うことにより位置情報PSを算出し、これをナビゲーションユニット12及び記憶部13へ供給する。またGPS処理部10は、受信したGPS信号を基に所定の速度算出処理及び方位算出処理を行うことにより、車両の速度V及び方位データDを算出し、これらを自律速度算出ユニット11及び記憶部13へ供給する。
【0018】
これに応じてナビゲーションユニット12は、位置情報PSに対応した範囲の地図データを所定の地図記憶部(図示せず)から読み出し、当該地図データ上に現在位置を示すマーク等を重畳して表示画面データを生成して、これを表示部3へ送出し表示画面を表示させる。
【0019】
一方、自律速度算出ユニット11には、3軸方向の加速度を検出する加速度センサ14及び周囲の気圧を検出する気圧センサ15が接続されている。この加速度センサ14は、3軸方向に作用する加速度を観測して加速度センサ観測値ADを生成し、これを自律速度算出ユニット11へ供給する。また気圧センサ15は、周囲の気圧を検出して気圧値PRを生成し、これを自律速度算出ユニット11へ供給する。
【0020】
自律速度算出ユニット11は、加速度センサ観測値AD及び気圧値PRを基に、車両の速度Vtを算出し、記憶部13に記憶・更新させるようになされている。また自律速度算出ユニット11は、ビルの陰やトンネル内等のようにGPSアンテナ2によってGPS信号を受信できない場合、車両の現在位置を推測するための情報として、当該速度Vtをナビゲーションユニット12へ供給するようになされている(詳しくは後述する)。
【0021】
これに応じてナビゲーションユニット12は、GPS処理部10からは位置情報PSを得られないものの、自律速度算出ユニット11からの車両の速度Vtを基に現在位置を推定し、この現在位置に対応した表示画面を表示部3に表示させる。
【0022】
因みにナビゲーション装置1は、いわゆる据付型のナビゲーション装置とは異なり、車両において生成され当該車両の速度に応じて周期が変化する車速パルス信号を利用せずに済むようになされており、当該ナビゲーション装置1を当該車両に取り付ける際の配線処理を簡略化し得るようになされている。
【0023】
このようにナビゲーション装置1は、GPSアンテナ2によりGPS信号を受信し得ない場合、自律速度算出ユニット11によって加速度センサ観測値AD及び気圧値PRを基に速度Vtを算出し、ナビゲーションユニット12によってこれらを基に現在位置(すなわち推定位置)を算出し得るようになされている。
【0024】
(2)速度の算出原理
次に、ナビゲーション装置1における自律速度算出ユニット11による速度Vtの算出原理について説明する。
【0025】
(2−1)進行方向加速度の算出原理
自律速度算出ユニット11では、車両の速度Vtを高精度に算出するべく、当該車両の進行方向の純粋な加速度(以下、これを進行方向加速度と呼ぶ)を検出したい、といった要求がある。
【0026】
しかしながら、ナビゲーション装置1がクレードル4を介して車両に取り付けられる場合、加速度センサ14の加速度検出軸は、当該車両の進行方向と一致しない可能性が高い。その場合、加速度センサ14から出力される加速度センサ観測値ADは、重力加速度gや方位変化に伴う横方向加速度の混入や、当該加速度センサ14における進行方向加速度のゲインの変化等による影響を受ける可能性がある。
【0027】
このため自律速度算出ユニット11は、加速度センサ14からの加速度センサ観測値ADをそのまま用いた場合、進行方向加速度の検出精度が大きく低下してしまう可能性がある。
【0028】
そこで自律速度算出ユニット11では、ナビゲーション装置1、車両及び地表面の間で座標の変換処理を行い、また重力加速度成分や横方向加速度成分の分離処理、さらにはオフセット調整処理等を行うことにより、進行方向加速度を高精度に算出し得るようになされている。
【0029】
(2−2)2つの座標軸間の関係
図3に示すように、xy座標系からx′y′座標系への変換行列Tを次式、
【0030】
【数1】
【0031】
で表し、xy座標系からみた位置p(cosθ,sinθ)が、x′y′座標系からみた位置p′(1,0)であるとした場合、次式の関係が成立し、これが、2つの座標系の関係を表す最も基本的な関係式となる。
【0032】
【数2】
【0033】
また座標軸と回転角度との関係を、図4に示すように定義する。すなわち、前後・左右・上下が定義できる物体において、前後方向をx軸、左右方向をy軸、上下方向をz軸とし、x軸周りの回転角度φを「ティルト」と呼び(以下、これをティルト角度φと呼ぶ)、y軸周りの回転角度θを「スイング」と呼び(以下、これをスイング角度θと呼ぶ)、z軸周りの回転角度ψを「パン」と呼ぶ(以下、これをパン角度ψと呼ぶ)。
【0034】
(2−3)道路傾斜が存在するときの地表面と車両との関係
次に、道路傾斜が存在するときの地表面と車両との関係について説明する。因みに、道路傾斜が存在しない地表面(路面)は、車両の進行方向に沿ったx軸、車両の進行方向とは直交した横方向(水平方向)に沿ったy軸、及び重力加速度方向に沿った鉛直方向のz軸によって表される。
【0035】
(2−3−1)地表面の地表座標系と道路傾斜の車両座標系
図5に示すように、道路傾斜上に位置している車両の車両座標系は、道路傾斜上で車両の進行方向に沿ったx′軸、道路傾斜上で車両の進行方向とは直交した横方向(水平方向)に沿ったy′軸、及び車両の高さ方向に沿ったz′軸によって表される。
【0036】
この道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)では、車両がx′軸方向に沿って太矢印方向へ進行する際の後ろ向きに働く加速度成分の符号を正と定義し、車両が右折する際の左側方向に働く加速度成分の符号を正と定義し、車両の上向き方向に働く重力加速度成分の符号を正と定義している。但し、符号の正負については、これに限るものではなく、上述の条件と逆であっても良い。
【0037】
この場合、地表面の地表座標系(x軸、y軸、z軸)と、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)とを関係付けるオイラー角で考えると、道路の進行方向における上り坂や下り坂を表す前後傾斜はy′軸周りのスイング角度θで示され、道路の進行方向と直交した横方向(水平方向)の右下下がりや左下下がりを表す左右傾斜は、x′軸周りのティルト角度φで示される。なお、z′軸周りの回転はパン角度ψで示されるが、当該パン角度ψによって表される道路の傾斜は存在しない。
【0038】
(2−3−2)地表面と車両との関係を示す座標変換式
ここで、地表面の地表座標系(x軸、y軸、z軸)に対する道路傾斜上に位置した車両の車両座標系(x′軸、y′軸、z′軸)の変換行列Aを、次式で定義する。
【0039】
【数3】
【0040】
この場合、変換行列Aは、道路の前後傾斜が先で、道路の左右傾斜が後なので、前後傾斜の項に対して左右傾斜の項を右側から乗算するようになされている。
【0041】
従って、地表座標系(x軸、y軸、z軸)は、次式で表される。
【0042】
【数4】
【0043】
これにより、道路傾斜上に位置する車両の車両座標系(x′軸、y′軸、z′軸)では、重力加速度gは、次式のように観測される。
【0044】
【数5】
【0045】
(2−4)車両と加速度センサとの関係
続いて、車両と、当該車両に取り付けられるナビゲーション装置1に搭載された加速度センサ14との関係について説明する。
【0046】
(2−4−1)車両の車両座標系及び加速度センサのセンサ座標系
図5に示したように、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)に対して、当該車両に対して任意の角度で取り付けられるナビゲーション装置1に搭載された3軸方向の加速度センサ14におけるセンサ座標系については、図6に示すようにx″軸、y″軸及びz″軸で表される。
【0047】
この場合、ナビゲーション装置1が車両に取り付けられたときの加速度センサ14に対するセンサ座標系(x″軸、y″軸、z″軸)としては、当該ナビゲーション装置1がユーザからみて上向き又は下向きに取り付けられた場合にy″軸周りのスイング角度θ′で示され、当該ナビゲーション装置1がユーザからみて左下下がり又は右下下がりに取り付けられた場合にx″軸周りのティルト角度φ′で示され、当該ナビゲーション装置1がユーザからみて右側運転席のドライバー向き又は左側助手席側のパッセンジャー向きに取り付けられた場合にz″軸周りのパン角度ψ′によって示される。
【0048】
(2−4−2)車両座標系とセンサ座標系との関係を示す座標変換式
ここで、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)に対する3軸方向の加速度センサ14におけるセンサ座標系(x″軸、y″軸、z″軸)の変換行列Bを、次式で定義する。
【0049】
【数6】
【0050】
この(6)式で表される変換行列Bでは、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に右側から乗算されるように選定されており、以降の計算過程においては一度選定された乗算順序を崩さないようにしなければならない。
【0051】
但し、変換行列Bとしては、パン角度ψ′、スイング角度θ′、ティルト角度φ′の順に限られるものではなく、例えば、道路の左右傾斜が加速度センサ14の取付角度の3軸(x″軸、y″軸、z″軸)に分配されて観測される場合、ティルト角度φ′、スイング角度θ′、パン角度ψ′の順番に右側から乗算されるように選定されてもよく、一度選定された乗算順序を崩さなければ、その他種々の乗算順序であってもよい。
【0052】
ここで、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)は、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)との関係で、次式のように表される。
【0053】
【数7】
【0054】
従って、(7)式を変形すれば、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)は、次式のように表される。
【0055】
【数8】
【0056】
ここで加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)は、道路傾斜上に位置している車両の車両座標系(x′軸、y′軸、z′軸)が、上述したように(4)式の関係を有することから、(8)式は、次式で表される。
【0057】
【数9】
【0058】
従って、加速度センサ14のセンサ座標系(x″軸、y″軸、z″軸)では、重力加速度gは、次式のように観測される。
【0059】
【数10】
【0060】
この重力加速度gを表す(10)式では、変換行列Bの逆行列B−1に相当する部分が車両に対する加速度センサ14の傾きを表し、かつ変換行列Aの逆行列A−1に相当する部分が地表面に対する車両の傾きを表している。
【0061】
実際上、加速度センサ14が車両に対して所定方向の傾きを持って取り付けられ、かつ道路傾斜上に車両が位置することにより当該車両が地表面に対して所定角度の傾きを持って存在している場合、重力加速度gは、次式のように観測される。
【0062】
【数11】
【0063】
(2−4−3)加速度センサに対する運動加速度(進行方向加速度及び横方向加速度)
また、加速度センサ14のセンサ座標系(x″軸、y″軸及びz″軸)では、車両の進行方向加速度αx及び横方向加速度 αyを定義し、道路に前後傾斜及び左右傾斜が存在せず、車両に対して加速度センサ14の取付傾きだけが生じている場合、加速度センサ14からみたときの進行方向加速度αx及び横方向加速度
αyを表す運動加速度αについては、変換行列Bの逆行列B−1を用いて、次式
【0064】
【数12】
【0065】
で表されるため、これを変換行列Bの逆行列B−1を用いて展開すると、次式のように観測される。
【0066】
【数13】
【0067】
因みに、車両の加速時には後ろ向きの加速度(いわゆるG)がかかること、及び右カーブでは左向きの遠心力が発生することに留意して、加速度センサ14のティルト角度φ′、スイング角度θ′及びパン角度ψ′に対する正負の符号が定められる。
【0068】
(2−5)加速度センサ観測値
加速度センサ14に対するセンサ座標系(x″軸、y″軸及びz″軸)の重力加速度gと運動加速度αとを合成したものを、当該加速度センサ14によって観測される加速度センサ観測値ASと定義した場合、当該加速度センサ観測値ASは、(10)式及び(12)式に基づいて、次式で表される。
【0069】
【数14】
【0070】
これを展開した場合、加速度センサ観測値ASは、次式のように表される。
【0071】
【数15】
【0072】
(2−5−1)加速度センサの零点オフセットとゲイン
ところで、電圧値として表される加速度センサ観測値ASとしては、加速度センサ14の零点オフセットOF及びゲインGEのことを考慮する必要がある。
【0073】
ここで加速度センサ14の零点オフセットOFとは、加速度センサ14に対して加速度成分が全く働いていないときのx″軸、y″軸及びz″軸にそれぞれ発生する電圧値([V])を示す。
【0074】
また加速度センサ14のゲインGEとは、加速度センサ14に対して例えば1g(=9.8[m/sec2])の負荷がかかったときのx″軸、y″軸及びz″軸にそれぞれ発生する電圧値([V/(m/sec2)])がどの程度変化するかを示す。
【0075】
この零点オフセットOF及びゲインGEを考慮したときの加速度センサ観測値ADは、(14)式に基づいて、次式で表される。
【0076】
【数16】
【0077】
なお、この(16)式中に含まれる加速度センサ観測値ASの単位は、[m/sec2]であり、加速度センサ観測値ADの単位としては、[V]となる。
【0078】
ここで零点オフセットOFは、次式
【0079】
【数17】
【0080】
で表され、ゲインGEは、次式で表される。
【0081】
【数18】
【0082】
従って、零点オフセットOF及びゲインGEを考慮したときの加速度センサ観測値ADは、次式で表されることになる。
【0083】
【数19】
【0084】
ここで、(19)式を構成する各要素のうち、ゲインGEx、GEy、GEz、零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′、道路の前後傾斜を示すスイング角度θ、道路の左右傾斜を示すティルト角度φがそれぞれ未知数である。
【0085】
しかし、ゲインGEx、GEy、GEzに関しては加速度センサ14の仕様書に記載されている値もしくは実際に検出された値を用い、道路の前後傾斜を示すスイング角度θについては、ナビゲーション装置1に内蔵された気圧センサ15の出力結果(気圧値PR)と対応付けられた高度情報を基に算出し、誤差は若干出るが道路の左右傾斜を示すティルト角度φはないものとして扱うものとする。
【0086】
但し、これら以外の例えば重力加速度gについては、GPS衛星から得られるGPS信号に基づいて算出した現在位置から緯度を取得し、緯度と重力加速度とが対応付けられた緯度−重力加速度変換テーブルから当該緯度に対応した重力加速度gを取得すればよい。
【0087】
また、車両の進行方向加速度αxについては、GPS信号に基づく車両の走行速度に基づいてリファレンスとして算出することが可能であり、車両の横方向加速度
αyについても、GPS信号に基づく車両の方位変化及び走行速度に基づきリファレンスとして算出することが可能である。
【0088】
これにより(19)式では、そのうち未知数が加速度センサ14の零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′の6個になり、これらを求める必要がある。
【0089】
(2−6)加速度センサの取付角度及びオフセットの学習
ところで、加速度センサ14の零点オフセットOFx、OFy、OFz及びゲインGEx、GEy、GEzを考慮した加速度センサ観測値AD=Iと定義し、(16)式中に含まれる次式
【0090】
【数20】
【0091】
と定義した場合、上述のI(加速度センサ観測値AD)は、次式で表される。なお、式中のSθ、Cθについては、それぞれsinθ、cosθを意味するものとする。
【0092】
【数21】
【0093】
ここで、(20)式中における変換行列Aの逆行列A−1は、地表面に対する道路の前後傾斜を表しており、これは気圧センサ15の出力結果(気圧値PR)に対応付けられている高度情報を基に導くことが可能であり、重力加速度g、進行方向加速度αx及び横方向加速度
αyについても、上述したようにリファレンスとして算出可能であるため、(21)式中のEは既知数である。
【0094】
この既知数Eは、(3)式に示したように変換行列Aの逆行列A−1を用いれば、次式のように変形することができる。
【0095】
【数22】
【0096】
従って、(21)式では、車両に対する加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)と、加速度センサ14の零点オフセットOFx、OFy、OFzとが未知であり、加速度センサ観測値ADは、次式のように変形することができる。
【0097】
【数23】
【0098】
この(23)式を更にニュートンラプソン法で用いられる形態に変形し、次式
【0099】
【数24】
【0100】
【数25】
【0101】
【数26】
【0102】
のような3つの関数式fx、fy、fzに表す。
【0103】
ここで、(24)式〜(26)式では未知数が零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個であるのに対し、3つの関数式fx、fy、fzしか存在しないので、このままでは解くことができない。
【0104】
但し、(24)式〜(26)式中の既知数Eの値は、毎秒得られるGPS信号に基づいて変化するため、少なくとも2秒間待てば、既知数Eの値が変化したそれぞれ異なる6種類の関数式fx1、fy1、fz1、fx2、fy2、fz2を得られるが、車両に加減速の変化がなければ、複数種類の関数式fx、fy、fzを得ることはできない。そこで、車両の実際の走行状態を想定し、加減速の変化が生じそうな10数秒分に相当する複数種類の関数式fx1〜fxt、fy1〜fyt、fz1〜fztを得ることとする。
【0105】
この場合、未知数の数(6個)よりも関数式fx1〜fxt、fy1〜fyt、fz1〜fztの数が遥かに多くなるものの、検出誤差等を考慮すると、解を一意に定めることができない可能性が高い。そこで、関数式fx1〜fxt、fy1〜fyt、fz1〜fztを満たすような尤もらしい未知数の値を得るべく、最小二乗法を用いることとする。
【0106】
具体的な計算手法として、ここでは多次元のニュートンラプソン法の適用を考える。基本的な1次元のニュートンラプソン法における変化量δは、次式
【0107】
【数27】
【0108】
で表される。ニュートンラプソン法では、この変化量δが所定の収束判定閾値以下になったとき、未知数の値の真解が得られたと認識する。
【0109】
この(27)式は、次式
【0110】
【数28】
【0111】
のように変形され、これをN次元に拡張すると、次式のように表される。
【0112】
【数29】
【0113】
従って、この場合、具体的には次式
【0114】
【数30】
【0115】
で表される左辺左側の偏導関数値と、右辺の関数値を計算することにより、左辺右側の最小二乗解(変化量δ)を求める。
【0116】
このようにして求められた最小二乗解(変化量δ)を用いて、未知数の解を次式
【0117】
【数31】
【0118】
によって算出し、この(31)式の左辺に示された未知数の解を用いて、再度(30)式により最小二乗解(変化量δ)を求め、この最小二乗解(変化量δ)を用いて(31)式の未知数の解を求めるといった繰返し計算を所定の回数分だけ行えば良い。
【0119】
ところで、(31)式で用いられる右辺第1項の初期値は、次式によって与えられる。
【0120】
【数32】
【0121】
この(32)式に示す初期値は、ティルト角度(φ′)0、スイング角度(θ′) 0及びパン角度(ψ′) 0 が0[rad]であることを示している。また、零点オフセット(OFx) 0、(OFy)
0、(OFz0 )については、加速度センサ14に加速度が一切作用していないときの電圧値が2.5([V])であることを示している。
【0122】
因みに加速度センサ14は、実際に検出した加速度に応じて、加速度センサ観測値ADの各成分(具体的には電位)をそれぞれ0[V]〜5[V]の範囲で変動させるようになされている。
【0123】
この(31)式では、左辺が最終的な解であり、右辺第1項が(31)式により前回求められた未知数の解(学習結果)を示し、右辺第2項が上述した(30)式の変化量δを示している。この(31)式の左辺により、零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′の6個の未知数が全て判明する。
【0124】
このように自律速度算出ユニット11は、車両がGPSアンテナ2によりGPS信号を受信し得るような走行環境下にあるとき、零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個の未知数を解として求める。
【0125】
その後、自律速度算出ユニット11は、車両がGPS信号を受信し得ないような走行環境となったとき、加速度センサ14の零点オフセットOFx、OFy、OFz、加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いると共に、加速度センサ14の観測結果や路面の高度差に基づいて進行方向加速度を推定し、これを基に積分計算することにより車両の速度や現在位置を算出(推定)するようになされている。
【0126】
因みに、(30)式〜(32)式中における添え字のiは、ニュートンラプソン法による繰り返し計算時の繰返回数を表しており、例えば、(30)式の右辺に含まれる(fx1)iは、次式
【0127】
【数33】
【0128】
を意味し、(30)式の左辺左側の行列に含まれる例えば∂(fx1)i/∂(ψ′)iは、∂fx1/∂ψ′に対して、((ψ′)i,(θ′)i,(φ′)i,(OFx)i,(OFy)i,(OFz)i)を代入したものを意味している。
【0129】
また、fx1の添え字(この場合であれば「1」)は、例えば10数秒間分まとめて処理する際の秒数を示す。従って、例えば10秒間分まとめて処理するのであれば、(30)式の左辺左側では、fx1〜fx10、fy1〜fy10、fz1〜fz10まで存在し、そのときの行列サイズは30(=3×10)行6列になる。
【0130】
なお、(31)式の左辺で得られた未知数の解としては、真値を中心にばらつきがある可能性が高く、また時間の経過とともに真値が変動する可能性もあるため、本発明では(31)式で得られる未知数の解を現時点までの例えば過去数秒〜数分間に渡って算出し、それを平均化又は平滑化することにより、そのときに応じた最終的な未知数の解を学習結果として求め得るようになされている。
【0131】
(2−7)加速度センサを用いた自律動作
ところで、加速度センサ観測値ADは、(16)式と同様、次式のように表される。
【0132】
【数34】
【0133】
この(34)式を進行方向加速度αx及び横方向加速度αyについて整理すると、次式
【0134】
【数35】
【0135】
のように変形され、この(35)式中の変換行列Bは、次式
【0136】
【数36】
【0137】
で表され、この(35)式中の右辺第2項は、次式のように表される。
【0138】
【数37】
【0139】
従って、上述の(35)式に対して(36)式及び(37)式を代入することにより、次式を得ることができる。
【0140】
【数38】
【0141】
【数39】
【0142】
【数40】
【0143】
この(38)式によって、進行方向加速度αxを求めるには、道路の前後傾斜を表すy′軸周りのスイング角度θが必要である。このスイング角度θは、次式
【0144】
【数41】
【0145】
により、高度差ΔHと、速度Vtとによって表される。
【0146】
この時点での未知数は、速度Vt、進行方向加速度αx、及び道路の前後傾斜を表すスイング角度θであるが、これらを一つ一つ解くのではなく、最終的に求めたい速度Vtを一気に求めることを考える。
【0147】
ここで、速度Vt−1は、これから最終的に求めたい速度Vtの一つ前の時点で判明した速度データである。従って、GPS信号を受信できていた状態からGPS信号を受信できなくなった最初の時点における速度Vt−1としては、GPS信号に基づいて最後に算出された速度V0があてはまる。
【0148】
この場合、(38)式のうち、既知の部分をβ(以下、これを既知部分βと呼ぶ)としてまとめると、既知部分βは、次式
【0149】
【数42】
【0150】
で表され、これを用いて(38)式を、次式
【0151】
【数43】
【0152】
のように変形することができる。
【0153】
ここで速度Vtと進行方向加速度αxとの関係は、次式
【0154】
【数44】
【0155】
のように表すことができ、上述の(41)式、(43)式を代入することにより、(44)式は、次式のように変形することができる。
【0156】
【数45】
【0157】
この(45)式を速度Vtについて整理すると、次式
【0158】
【数46】
【0159】
によって表され、これを速度Vtについて解くと、当該速度Vtは、次式によって表される。
【0160】
【数47】
【0161】
このように自律速度算出ユニット11では、GPS信号の受信時に、車両に対する加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ14の零点オフセットOFx、OFy、OFzを予め学習して記憶しておく。その後、自律速度算出ユニット11は、ビル等の陰に隠れる等してGPS信号を受信できなくなった場合、予め学習しておいた加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、当該加速度センサ14の零点オフセットOFx、OFy及びOFzを用い、加速度センサ観測値ADx″、ADy″、ADz″及び高度差ΔHに基づいて進行方向加速度αxを自律的に算出(推定)することができるので、その自律的に算出した進行方向加速度αxを基に車両の速度Vt(以下、これを自律速度Vtと呼ぶ)を求めることができる。
【0162】
これに応じてナビゲーション装置1では、自律速度算出ユニット11により算出された車両の自律速度Vtを積分することにより、GPS信号を受信できない走行環境下であっても、当該車両の現在位置を正確に算出し、表示部3の地図上に現在位置を継続して表示し得るようになされている。
【0163】
(3)自律速度算出ユニットの具体的構成
次に、上述した速度の算出原理に従い速度Vtを算出する自律速度算出ユニット11の構成について、GPS測位時(学習時)とGPS非測位時(自律時)とに分けてそれぞれ説明する。
【0164】
(3−1)GPS測位時(学習時)における自律速度算出ユニットの構成
図7において、自律速度算出ユニット11は、図示しないCPU(Central Processing Unit)を中心に構成されており、図示しないROM(Read Only Memory)から基本プログラムや速度算出プログラム等の各種アプリケーションプログラムを読み出して図示しないRAM(Random Access Memory)上に展開することにより、道路傾斜計算部20、学習処理部21及び自律計算部22といった各処理機能をソフトウェア的に実現し得るようになされている。
【0165】
但し、自律速度算出ユニット11としては、これに限るものではなく、当該自律速度算出ユニット11における道路傾斜計算部20、学習処理部21及び自律計算部22をハードウェア的に構成するようにしても良い。
【0166】
自律速度算出ユニット11は、GPS処理部10により算出される車両の速度V及び方位データDを道路傾斜計算部20及び学習処理部21へ供給する。また自律速度算出ユニット11は、気圧センサ15によって検出された気圧値PRを道路傾斜計算部20へ供給すると共に、加速度センサ14によって観測される加速度センサ観測値ADを学習処理部21へ供給する。
【0167】
道路傾斜計算部20の進行距離算出部31は、GPS処理部10から供給される速度Vに基づいて車両の進行距離Lmを算出し、その進行距離Lmを前後傾斜算出部33へ送出する。
【0168】
また、道路傾斜計算部20の高度差算出部32は、一般的な気圧と高度との対応関係が予めテーブル化された気圧高度対応テーブルTBLを記憶部13(図2)に記憶しており、時刻t0における気圧値PR0及び時刻t1における気圧値PR1を基に、気圧高度対応テーブルTBLから気圧値PR0及びPR1にそれぞれ対応した高度情報h0及びh1を読み出す。
【0169】
そして高度差算出部32は、気圧値PR0に対応した車両の高度情報h0と、気圧値PR1に対応した車両の高度情報h1との差分である高度差ΔHを算出し、これを前後傾斜算出部33へ送出するようになされている。
【0170】
前後傾斜算出部33は、進行距離算出部31から供給される進行距離Lmと、高度差算出部32から供給される高度差ΔHとに基づいて、車両座標系(x′軸、y′軸、z′軸)において道路の前後傾斜を示すスイング角度θを算出し、これを学習処理部21の取付角度及びオフセット学習処理部44へ送出する。
【0171】
学習処理部21の進行方向加速度算出部41は、GPS処理部10から供給される速度Vを微分することにより車両の進行方向加速度αxをリファレンスとして算出し、これを取付角度及びオフセット学習処理部44へ送出する。
【0172】
一方、角速度算出部42は、GPS処理部10から供給される方位データDを微分することにより角速度dDを算出し、これを横方向加速度算出部43へ送出する。横方向加速度算出部43は、GPS処理部10から供給される速度V及び角速度算出部42から供給される角速度dDに基づいて横方向加速度αyをリファレンスとして算出し、これを取付角度及びオフセット学習処理部44へ送出する。
【0173】
取付角度及びオフセット学習処理部44は、加速度センサ14から供給される加速度センサ観測値ADと、進行方向加速度算出部41から供給される進行方向加速度αxと、横方向加速度算出部43から供給される横方向加速度αyとを基に、加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を学習し得るようになされている。
【0174】
このようなGPS測位時における自律速度算出ユニット11の学習処理手順をまとめてみれば、図8に示すように自律速度算出ユニット11は、ルーチンRT1の開始ステップから入って次のステップSP1へ移る。このステップSP1において自律速度算出ユニット11は、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度V及び方位データDをGPSデータとして取得すると共に学習処理部21へ供給し、次のステップSP2へ移る。
【0175】
ステップSP2において自律速度算出ユニット11は、加速度センサ14の出力である加速度センサ観測値ADを取得して学習処理部21の取付角度及びオフセット学習処理部44へ供給し、次のステップSP3へ移る。
【0176】
ステップSP3において自律速度算出ユニット11は、道路傾斜計算部20の進行距離算出部31で算出された進行距離Lm、及び高度差算出部32で算出された高度差ΔHに基づいて、前後傾斜算出部33により車両座標系(x′軸、y′軸、z′軸)において道路の前後傾斜を示すスイング角度θを算出し、次のステップSP4へ移る。
【0177】
ステップSP4において自律速度算出ユニット11は、ステップSP1で取得した車両の速度Vが速度情報として信頼たり得るデータであるか否かを当該速度Vに付加されているフラグに基づいて判断し、否定結果が得られたときは、速度Vが信頼できない速度情報に過ぎないため、次のステップSP16へ移って処理を終了する。
【0178】
これに対してステップSP4で肯定結果が得られると、自律速度算出ユニット11は次のステップSP5へ移り、GPS処理部10から受け取った速度Vを基に進行方向加速度算出部41により進行方向加速度αxをリファレンスとして算出すると共に、GPS処理部10から受け取った速度V及び方位データDを基に横方向加速度算出部43により横方向加速度αyをリファレンスとして算出し、次のステップSP6へ移る。
【0179】
ステップSP6において自律速度算出ユニット11は、加速度センサ観測値AD及び上述の(22)式で示された既知数Eを学習用入力データとして設定し、次のステップSP7へ移る。
【0180】
ステップSP7においてカーナビゲーション装置1は、加速度センサ14のオフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるために必要な関数式が未知数よりも多いか否かを判定する。
【0181】
ここで否定結果が得られると、このことは関数式が未知数よりも多くないため、これら未知数を求めることが出来ないことを表しており、このとき自律速度算出ユニット11はステップSP16へ移って処理を終了する。
【0182】
これに対してステップSP7で肯定結果が得られると、このことは関数式が未知数よりも多いため、これら未知数を求めることが可能であることを表しており、このとき自律速度算出ユニット11は次のステップSP8へ移る。
【0183】
ステップSP8において自律速度算出ユニット11は、このルーチンRT1における学習処理手順によって求められた前回の学習結果(加速度センサ14のオフセットOFx、OFy及びOFz並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′)の値を上述した(30)式の左辺左側の行列の値として初期値設定し、次のステップSP9へ移る。
【0184】
ステップSP9において自律速度算出ユニット11は、上述した(30)式における左辺左側の偏導関数値と、右辺の関数値を計算し、次のステップSP10へ移る。
【0185】
ステップSP10において自律速度算出ユニット11は、上述した(30)式における右辺の各要素(関数値)の合計が所定の収束判定閾値よりも小さくなったか否かを判定し、肯定結果が得られると次のステップSP15へ移り、否定結果が得られると次のステップSP11へ移る。
【0186】
ステップSP11において自律速度算出ユニット11は、上述した(30)式の右辺における関数値の各要素の合計が、ニュートンラプソン法における所定の収束判定閾値よりも小さくなっていないので、ステップSP9で計算した偏導関数値及び関数値により、(30)式における左辺右側の最小二乗解(変化量δ)を求め、次のステップSP12へ移る。
【0187】
ステップSP12において自律速度算出ユニット11は、ステップSP11で求めた最小二乗解(変化量δ)を用いて(31)式の左辺に相当する最終解(加速度センサ14のオフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′)を求め、次のステップSP13へ移る。
【0188】
ステップSP13において自律速度算出ユニット11は、(30)式の左辺右側で示される最小二乗解(変化量δ)の各要素の合計が所定の収束判定閾値よりも小さいか否かを判定し、否定結果が得られると次のステップSP14へ移るのに対し、肯定結果が得られると次のステップSP15へ移る。
【0189】
ステップSP14において自律速度算出ユニット11は、ステップSP9〜ステップSP14までの処理を繰返し行うニュートンラプソン法のループ回数が所定のループ回数閾値よりも少ないか否かを判定し、肯定結果が得られると、ステップSP9へ戻って、以降の処理を繰り返す。この場合、ステップSP9において自律速度算出ユニット11は、直前に求められた加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を用いて、上述した(30)式中における左辺左側の偏導関数値と、右辺の関数値を計算する。
【0190】
これに対し、ステップSP14で否定結果が得られると、繰返し計算のループ回数が所定のループ回数閾値よりも多いにも拘わらず、ステップSP10又はステップSP13で関数値の各要素の合計又は最小二乗解(変化量δ)の各要素の合計が収束しなかったことを表しており、このとき自律速度算出ユニット11は、次のステップSP16へ移って未知数の解を求めることなく終了する。
【0191】
ステップSP15において自律速度算出ユニット11は、(31)式の左辺で求められた未知数の最終解を現時点まで過去数秒〜数分間に渡って算出し、その結果得られる複数個の最終解を用いて平滑化処理することにより、最も真値に近い最終解を学習結果として得、次のステップSP16へ移って処理を終了する。
【0192】
なお自律速度算出ユニット11では、ステップSP15において平滑化処理に限るものではなく、処理量を減少させたいのであれば、単純な平均化処理することにより学習結果を得るようにしても良い。
【0193】
(3−2)GPS非測位時(自律時)における自律速度算出ユニット11の構成
図9に示すように、自律速度算出ユニット11は、GPS信号を受信できていた走行環境下からGPS信号を受信できない走行環境下になった場合、GPS受信部3から速度V及び方位データDを受け取ることができなくなる。このとき自律速度算出ユニット11は、気圧センサ15からの気圧値PRを道路傾斜計算部20の高度差算出部32へ供給すると共に、加速度センサ14からの加速度センサ観測値ADを自律計算部22の速度算出部51へ供給する。
【0194】
道路傾斜計算部20の高度差算出部32は、気圧高度対応テーブルTBLに基づいて、時刻t0における気圧値PR0に対応した高度情報と時刻t1における気圧値PR1に対応した高度情報との差分である高度差ΔHを算出し、これを自律計算部22の速度算出部51へ送出する。
【0195】
また、学習処理部21の取付角度及びオフセット学習処理部44は、GPS信号を受信できているときに予め学習し記憶しておいた加速度センサ14の零点オフセットOFx、OFy、OFz、車両に対する加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′を自律計算部22の速度算出部51へ送出する。
【0196】
速度算出部51は、高度差算出部32から供給される高度差ΔHと、加速度センサ14から供給される加速度センサ観測値ADと、オフセット学習処理部43から供給される加速度センサ14の零点オフセットOFx、OFy、OFzと、加速度センサ14の取付傾きを表すティルト角度φ′、スイング角度θ′及びパン角度ψ′とを基に、(47)式等を用いて進行方向加速度αxを自律的に算出(推定)し、その進行方向加速度αxを基に車両の自律速度Vtを算出(推定)し得るようになされている。
【0197】
従ってナビゲーションユニット12は、GPS信号を受信できない走行環境下であっても、このようにして求めた車両の自律速度Vtを積分することにより当該車両の現在位置を正確に算出し、表示部3の地図上に現在位置を継続して表示し得るようになされている。
【0198】
このようなGPS非測位時における自律速度算出ユニット11の自律速度算出処理手順をまとめてみれば、図10に示すように自律速度算出ユニット11は、ルーチンRT2の開始ステップから入って次のステップSP21へ移る。このステップSP2において自律速度算出ユニット11は、複数のGPS衛星からのGPS信号に基づいて算出された車両の速度V及び方位データDをGPSデータとしてGPS処理部10から学習処理部21で受け取ることにより取得し、次のステップSP22へ移る。
【0199】
ステップSP22において自律速度算出ユニット11は、気圧センサ15から供給される気圧値PRを道路傾斜計算部20の高度差算出部32で受け取ることにより取得し、次のステップSP23へ移る。
【0200】
ステップSP23において自律速度算出ユニット11は、高度差算出部32により気圧高度対応テーブルTBLを用いて高度差ΔHを算出し、次のステップSP24へ移る。
【0201】
ステップSP24において自律速度算出ユニット11は、ステップSP21で取得した車両の速度Vが速度情報として信頼たり得るデータであるか否かを当該速度Vに付加されているフラグに基づいて判断し、肯定結果が得られたときは、速度Vが信頼できる速度情報であって、GPS信号を受信できている走行環境下にあるため、自律速度算出処理を行う必要がなく、次のステップSP27へ移って処理を終了する。
【0202】
これに対してステップSP24で否定結果が得られると、このことはGPS信号を受信できない走行環境下であって、速度Vが信頼できない速度情報に過ぎないことを表しており、このとき自律速度算出ユニット11は次のステップSP25へ移る。
【0203】
ステップSP25において自律速度算出ユニット11は、GPS信号を受信できない走行環境下にあるので、予めGPS信号の受信時に学習しておいた加速度センサ14の取付傾きを示すティルト角度φ′、スイング角度θ′及びパン角度ψ′と、零点オフセットOFx、OFy、OFzを用いて、上述の(43)式等を用いた計算処理により進行方向加速度αxを推定し、これを積分計算することにより車両の自律速度Vtを求め、次のステップSP26へ移る。
【0204】
ステップSP26において自律速度算出ユニット11は、このようにして求めた車両の自律速度Vtをナビゲーションユニット12へ出力し、次のステップSP27へ移って処理を終了する。
【0205】
これに応じてナビゲーションユニット12は、GPS信号を受信できない走行環境下であっても、自律速度Vtに基づいて当該車両の現在位置を一段と正確に推定して表示部3の地図上に現在位置を継続表示することができる。
【0206】
(4)ナビゲーション装置及びクレードルの着脱状態と学習結果の利用
(4−1)クレードルの着脱状態の検出と自律速度の算出
ところでナビゲーション装置1(図1)は、上述したように、車両に取り付けられたクレードル4に対して容易に装着され、また容易に取り外され得るようになされている。
【0207】
このため、ナビゲーション装置1に関しては、例えばユーザにより当該ナビゲーション装置1がクレードル4から取り外され、当該ユーザの手元で行き先設定や経由地設定等の操作が行われた後、当該クレードル4に再度装着される、といった使用状況が想定され得る。
【0208】
このようにクレードル4から取り外されているとき、ナビゲーション装置1は、車両に対する相対的な角度、すなわち加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)が、当該クレードル4に装着されていたときとは異なることになる。
【0209】
しかしながら、上述した(6)式において定義したセンサ座標系は、ナビゲーション装置1(すなわち加速度センサ14)が車両に対して固定され動かないことを前提としている。
【0210】
このためナビゲーション装置1の自律速度算出ユニット11(図2)は、クレードル4から取り外されている間、GPSアンテナ2によってGPS信号を受信できない場合、上述した手法により学習した加速度センサ14の取付傾き(ティルト角度φ′、スイング角度θ′及びパン角度ψ′)を用いると、誤った自律速度Vtを算出することになってしまう。
【0211】
そこでナビゲーション装置1は、クレードル4との着脱状態を検出し、当該クレードル4との着脱状態に応じて自律速度算出ユニット11における自律速度Vtの算出手法を切り換えるようになされている。
【0212】
具体的にナビゲーション装置1は、クレードル着脱検出部16によりクレードル4との着脱状態を検出し、着脱情報CDを自律速度算出ユニット11へ供給するようになされている。
【0213】
自律速度算出ユニット11は、着脱情報CDを基にクレードル4に装着されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、(6)式において定義したセンサ座標系が保持されていることから、上述した手法により学習した加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′(以下、これらをまとめて学習結果と呼ぶ)を用いることにより、高精度に自律速度Vtを算出し出力する。
【0214】
一方、自律速度算出ユニット11は、着脱情報CDを基にクレードル4から取り外されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、(6)式において定義したセンサ座標系が変化してしまったことから、当該クレードル4に装着されていたときの最後の自律速度Vtが継続しているものと見なし、当該自律速度Vtをそのまま出力する。
【0215】
この場合、自律速度算出ユニット11は、一般に車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Vtを出力する。すなわち自律速度算出ユニット11は、加速度センサ14の学習結果を利用できず高精度な自律速度Vtを算出できないものの、いわば次善の策として直前の自律速度Vtを出力する。
【0216】
これにより、ナビゲーションユニット12において、実際の現在位置に比較的近い現在位置が算出されることを期待できる。
【0217】
またナビゲーション装置1は、クレードル4に再度装着された場合、当該クレードル4との嵌合精度や車両に対するクレードル4の取付位置のずれ等の要因により、車両に対する取付傾きが前回と近い状態にあるとは想定されるものの、当該取付傾きが前回とは微妙に異なっている可能性がある。
【0218】
そこで自律速度算出ユニット11は、ナビゲーション装置1がクレードル4に再度装着されたことを検出した場合、当該クレードル4から取り外される直前に学習した学習結果を初期値として、再度学習処理を行うようになされている。
【0219】
このとき自律速度算出ユニット11は、前回の学習結果をそのまま用いるのではなく、再度学習処理を行うことにより、取付傾きの違いによる誤った自律速度Vtを出力せずに済む。また自律速度算出ユニット11は、前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。
【0220】
因みにナビゲーションユニット12は、GPSアンテナ2によってGPS信号を受信できる場合、ナビゲーション装置1とクレードル4との着脱状態に拘わらず、GPS処理部10から供給される位置情報PSを基に、現在位置を高精度に算出し得るようになされている。
【0221】
このように自律速度算出ユニット11は、クレードル4との装着状態に応じて取付傾き等の学習結果を利用するか否かを切り換えることにより、自律速度Vtの算出精度をできるだけ低下させないようになされている。
【0222】
(4−2)自律速度算出切換処理手順
実際上、自律速度算出ユニット11は、クレードル4との装着状態及びGPS信号の受信状態に応じて学習処理又は自律速度算出処理を切り換える場合、図11に示す自律速度算出切換処理手順RT3に従った処理を行うようになされている。
【0223】
すなわち自律速度算出ユニット11は、所定の周期(例えば1秒)毎に自律速度算出切換処理手順RT3を開始し、ステップSP31へ移る。ステップSP31において自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されているか否かを判定する。ここで肯定結果が得られると、このことはセンサ座標系が保持されていることを表しており、このとき自律速度算出ユニット11は次のステップSP32へ移る。
【0224】
ステップSP32において自律速度算出ユニット11は、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度Vが速度情報として信頼たり得るデータであるか否かを、当該速度Vに付加されているフラグに基づいて判定する。ここで肯定結果が得られると、このことはGPS信号を十分に受信できており学習処理を行い得ることを表しており、このとき自律速度算出ユニット11は次のステップSP33へ移る。
【0225】
ステップSP33において自律速度算出ユニット11は、過去数回分の着脱情報CD等を基に、ナビゲーション装置1がクレードル4に装着された直後であるか否かを判定する。ここで否定結果が得られると、このことはナビゲーション装置1がしばらくの間クレードル4に装着されたまま取り外されておらず、これまでの学習結果を継続して利用し得ることを表しており、このとき自律速度算出ユニット11は次のステップSP34へ移る。
【0226】
ステップSP34において自律速度算出ユニット11は、上述したGPS受信時における学習処理手順RT1(図8)を実行して学習結果を更新し、その後ステップSP40へ移って処理を終了する。
【0227】
一方ステップSP33において肯定結果が得られると、このことはナビゲーション装置1がクレードル4に再装着された直後であるため、加速度センサ14の取付傾き等を学習し直す必要があることを表しており、このとき自律速度算出ユニット11は、次のステップSP35へ移る。
【0228】
ステップSP35において自律速度算出ユニット11は、ナビゲーション装置1がクレードル4から取り外される前に学習した学習結果を記憶部13から読み出して初期値とし、GPS受信時における学習処理手順RT1(図8)を実行して新たな学習結果を算出し、その後ステップSP40へ移って処理を終了する。
【0229】
一方、ステップSP32において否定結果が得られると、このことはGPS信号を受信できない走行環境下にあることを表しており、このとき自律速度算出ユニット11は、次のステップSP36へ移る。
【0230】
ステップSP36において自律速度算出ユニット11は、自律速度算出処理手順RT2(図10)を実行することにより自律速度Vtを算出して出力し、その後ステップSP40へ移って処理を終了する。
【0231】
これに対して、ステップSP31において否定結果が得られると、このことはナビゲーション装置1がクレードル4から取り外されているために、センサ座標系が変化してしまっていることを表しており、このとき自律速度算出ユニット11は次のステップSP38へ移る。
【0232】
ステップSP38において自律速度算出ユニット11は、ステップSP32と同様、GPS処理部10により複数のGPS衛星からのGPS信号に基づいて算出された車両の速度Vが速度情報として信頼たり得るデータであるか否かを、当該速度Vに付加されているフラグに基づいて判定する。
【0233】
このステップSP38において肯定結果が得られると、このことはGPS信号を十分に受信できる走行環境下にあり自律速度Vtを算出する必要がないことを表しており、このとき自律速度算出ユニット11は、次のステップSP40へ移って処理を終了する。
【0234】
一方、ステップSP38において否定結果が得られると、このことはGPS信号を受信できない走行環境下にあり、且つセンサ座標系が変化してしまっているために、加速度センサ14からの加速度センサ観測地ADを基に自律速度Vtを算出し得ないことを表しており、このとき自律速度算出ユニット11は、次のステップSP39へ移る。
【0235】
ステップSP39において自律速度算出ユニット11は、最後に算出した自律速度Vt又は最後にGPS処理部10から取得した速度Vのうち新しい方を自律速度Vtとして出力し、次のステップSP40へ移って処理を終了する。
【0236】
その後ナビゲーションユニット12は、自律速度算出ユニット11から供給される自律速度Vtを基に現在位置(推定位置)を算出し、当該現在位置に応じた地図情報や経路案内等を表示部3に表示させる。
【0237】
(5)動作及び効果
以上の構成において、ナビゲーション装置1の自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1とクレードル4との着脱状態を認識する。
【0238】
自律速度算出ユニット11は、着脱情報CDを基にクレードル4に装着されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、学習した学習結果(すなわち加速度センサ14の零点オフセットOFx、OFy及びOFz、並びに当該加速度センサ14の取付傾きであるティルト角度φ′、スイング角度θ′及びパン角度ψ′)を用いることにより、高精度に自律速度Vtを算出し出力する。
【0239】
一方、自律速度算出ユニット11は、着脱情報CDを基にクレードル4から取り外されていることを認識したとき、GPSアンテナ2によってGPS信号を受信できない場合、当該クレードル4に装着されていたときの最後の速度が継続しているものと見なし、当該最後の速度をそのまま出力する。
【0240】
この場合、自律速度算出ユニット11は、ナビゲーション装置1がクレードル4から取り外されたときに、(6)式におけるセンサ座標系が変化してしまったことを認識することができ、学習結果である加速度センサ14の取付傾き(すなわちティルト角度φ′、スイング角度θ′及びパン角度ψ′)を誤って用いることなく、自律速度Vtの算出を中止することができる。
【0241】
このとき自律速度算出ユニット11は、クレードル着脱検出部16によりクレードル4から取り外されたことを検出した時点から学習結果の利用を中止するため、ナビゲーション装置1が当該クレードル4から取り外される際にユーザにより加えられる力を車両の速度の変化として誤認識してしまうことも未然に防止できる。
【0242】
また自律速度算出ユニット11は、加速度センサ14の学習結果を利用できず高精度な自律速度Vtを算出できないものの、車両の速度が急激に変化する可能性は低いと考えられることに基づき、直前の自律速度Vt実際の車両の速度に近い値として出力することができる。
【0243】
一方、自律速度算出ユニット11は、ナビゲーション装置1がクレードル4に再度装着されたことを認識した場合、当該クレードル4から取り外される直前に学習した学習結果を初期値として再度学習処理を行う。
【0244】
この場合、自律速度算出ユニット11は、車両に対する取付傾きが前回とは微妙に異なっている可能性があるため、取付傾きの違いによる誤った自律速度Vtを出力せずに済み、また前回の学習結果を初期値とすることにより、新たな取付傾きを短時間で学習することができる。
【0245】
この結果、ナビゲーション装置1では、クレードル4との着脱状態やGPS信号の受信状態に拘わらず、ナビゲーションユニット12において、現在位置が常に高精度に算出されることを期待できる。
【0246】
以上の構成によれば、ナビゲーション装置1の自律速度算出ユニット11は、クレードル着脱検出部16からの着脱情報CDを基に、ナビゲーション装置1がクレードル4に装着されている場合、学習した取付傾きを用いて高精度な自律速度Vtを算出することができ、一方ナビゲーション装置1がクレードル4から取り外されたことを認識した場合、学習したセンサ座標系が変化したことを認識して学習結果である加速度センサ14の取付傾きを用いた自律速度Vtの算出を中止するため、誤った自律速度Vtの出力を未然に防止することができ、ナビゲーションユニット12における現在位置の算出精度を低下させずに済む。
【0247】
(6)他の実施の形態
なお上述した実施の形態においては、ナビゲーション装置1がクレードル4から取り外され、且つGPS信号を充分に受信できない場合、自律速度算出ユニット11から最後に算出した自律速度Vt又は最後にGPS処理部10から取得した速度Vを現在の自律速度Vtとして出力するようにした場合について述べたが、本発明はこれに限らず、例えば直前の速度Vの変化の割合を算出し、当該変化の割合に従って自律速度Vtを変化させていくようにし、又は自律速度Vtを一切出力しないようにする等しても良く、要はクレードル4に装着されていたときに学習した取付傾きを用いないようにすれば良い。
【0248】
また上述した実施の形態においては、ナビゲーション装置1がクレードル4から一度取り外されて再度装着された場合に、前回の学習結果を初期値として再度学習を行うようにした場合について述べたが、本発明はこれに限らず、例えば(16)式のような初期値を用いて学習処理を最初からやり直すようにし、又は前回の学習結果を用いて直ちに自律速度Vtの算出を開始する等しても良い。
【0249】
さらに上述した実施の形態においては、(30)〜(32)式を用いたニュートンラプソン法により零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′の計6個の未知数を解として求めるようにした場合について述べたが、本発明はこれに限らず、他の種々の算出手法により零点オフセットOFx、OFy、OFz、ティルト角度φ′、スイング角度θ′及びパン角度ψ′を求めるようにしても良い。
【0250】
さらに上述した実施の形態においては、(42)式〜(47)式を用いて自律速度Vtを算出するようにした場合について述べたが、本発明はこれに限らず、加速度センサ観測値ADx″、ADy″、ADz″を用いた他の種々の演算手法により自律速度Vtを算出するようにしても良い。この場合、高度差ΔHを用いずに当該自律速度Vtを算出するようにしても良い。
【0251】
さらに上述した実施の形態においては、本発明のナビゲーション装置1を移動体としての車両に搭載するようにした場合について述べたが、本発明はこれに限らず、船舶、電車、その他種々の移動体に搭載するようにしても良い。
【0252】
さらに上述した実施の形態においては、GPSアンテナ2を介して複数のGPS衛星からのGPS信号を受信し、これら複数のGPS信号に基づきGPS処理部10により車両の速度Vを算出すると共に車両の方位データDを算出するようにした場合について述べたが、本発明はこれに限らず、例えば準天頂衛星システム、グローナス(GLONASS:Global Navigation Satellite System)やガリレオ(GALILEO)等の種々の衛星測位システムを利用し、それぞれの衛星信号に基づき車両の速度Vを算出すると共に車両の方位データDを算出するようにしても良い。
【0253】
さらに上述の実施の形態においては、自律速度算出ユニット11がROMから読み出して展開した速度算出プログラムに従って上述したルーチンRT3の自律速度算出切換処理手順(図11)を実行するようにした場合について述べたが、本発明はこれに限らず、記録媒体からインストールした速度算出プログラムや、インターネットからダウンロードした速度算出プログラムに従ってルーチンRT3の自律速度算出切換処理手順を実行するようにしても良い。
【0254】
さらに上述した実施の形態においては、本体部としてのナビゲーション装置1と、加速度センサとしての加速度センサ14と、推定速度算出手段としての自律速度算出ユニット11及びナビゲーションユニット12と、着脱検出子としてのクレードル着脱検出部16と、加速度算出手段、取付傾き算出手段及び推定速度算出制御手段としての自律速度算出ユニット11とによってナビゲーション装置としてのナビゲーション装置1を構成する場合について述べたが、本発明はこれに限らず、その他種々の回路構成でなる本体部と、加速度算出手段と、加速度センサと、取付傾き算出手段と、推定速度算出手段と、着脱検出子と、推定速度算出制御手段とによってナビゲーション装置を構成するようにしても良い。
【産業上の利用可能性】
【0255】
本発明は、本発明のナビゲーション装置、ナビゲーション情報算出方法及びナビゲーション情報算出プログラムは、例えば車両に対してクレードルを介して着脱されるポータブルタイプのナビゲーション装置に適用したが、クレードルが用意されGPS受信機能を有するPDA(Personal Digital Assistant)、携帯電話機、パーソナルコンピュータ等の種々の電子機器に適用することができる。
【図面の簡単な説明】
【0256】
【図1】ナビゲーション装置及びクレードルの構成を示す略線的斜視図である。
【図2】ナビゲーション装置の回路構成を示す略線図である。
【図3】2つの座標軸間の関係を示す略線的斜視図である。
【図4】角度の定義の説明に供する略線図である。
【図5】道路傾斜がある地表面上に位置している車両の車両座標系の説明に供する略線図である。
【図6】車両に取り付けられた加速度センサのセンサ座標系の説明に供する略線図である。
【図7】GPS測位時(学習時)におけるナビゲーション装置の構成を示す略線的ブロック図である。
【図8】GPS受信時における学習処理手順を示す略線的フローチャートである。
【図9】GPS非測位時(自律時)におけるナビゲーション装置の構成を示す略線的ブロック図である。
【図10】自律速度算出処理手順を示す略線的フローチャートである。
【図11】自律速度算出切換処理手順を示す略線的フローチャートである。
【符号の説明】
【0257】
1……ナビゲーション装置、2……GPSアンテナ、3……表示部、4……クレードル、10……GPS処理部、11……自律速度算出ユニット、12……ナビゲーションユニット、13……記憶部、14……加速度センサ、15……気圧センサ、16……クレードル着脱検出部、AD……加速度センサ観測値、CD……着脱情報、V……速度、D……方位データ、Vt……自律速度。
【特許請求の範囲】
【請求項1】
所定の台座部に装着された状態で移動体に取り付けられ、所定の測位手段から供給される測位情報を基に上記移動体の現在位置を検出する本体部と、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出手段と、
上記本体部に作用する加速度を観測する加速度センサと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出手段と、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きを基に上記移動体の推定速度及び推定位置を算出する推定速度算出手段と、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出子と、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出手段による上記推定速度の算出を停止させる推定速度算出制御手段と
を具えることを特徴とするナビゲーション装置。
【請求項2】
上記推定速度算出制御手段は、
上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記推定速度算出手段により最後に算出した推定速度をこの時点における推定速度とする
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項3】
上記推定速度算出制御手段は、
上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記測位情報を基に最後に算出された速度をこの時点における推定速度とする
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項4】
上記取付傾き算出手段は、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記取付傾きの算出を停止する
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項5】
上記取付傾き検出手段は、
所定の初期値を基に演算処理を再帰的に繰り返すことにより上記取付傾きを算出し、
上記本体部が上記台座部から取り外され再度装着された場合、上記本体部が上記台座部から取り外される前に検出した上記取付傾きを上記初期値として、当該取付傾きを再度算出する
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項6】
上記測位情報を基に算出された上記移動体の速度に応じて当該移動体の進行方向加速度を算出すると共に、上記測位情報に基づいて算出された上記移動体の速度及び方位に応じて上記進行方向加速度とは直交した水平方向加速度を算出する加速度算出手段と
を具え、
上記加速度センサは、
上記移動体の運動加速度及び重力加速度を観測し、
上記取付傾き算出手段は、
所定の気圧センサからの気圧値に応じて算出した路面の高度差と、上記移動体の速度に応じた進行距離とに基づいて上記路面の上記進行方向に対する傾斜角度を算出し、上記進行方向加速度と、上記水平方向加速度と、上記加速度センサによる運動加速度及び重力加速度を含む観測値と、上記傾斜角度とにより表される多次元の関数式に基づいて上記取付傾きを求める
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項7】
所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
上記本体部に作用する加速度を観測する加速度観測ステップと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
を具えることを特徴とする位置検出方法。
【請求項8】
ナビゲーション装置に対して、
所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
上記本体部に作用する加速度を観測する加速度観測ステップと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
を実行させることを特徴とする位置検出プログラム。
【請求項1】
所定の台座部に装着された状態で移動体に取り付けられ、所定の測位手段から供給される測位情報を基に上記移動体の現在位置を検出する本体部と、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出手段と、
上記本体部に作用する加速度を観測する加速度センサと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出手段と、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きを基に上記移動体の推定速度及び推定位置を算出する推定速度算出手段と、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出子と、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出手段による上記推定速度の算出を停止させる推定速度算出制御手段と
を具えることを特徴とするナビゲーション装置。
【請求項2】
上記推定速度算出制御手段は、
上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記推定速度算出手段により最後に算出した推定速度をこの時点における推定速度とする
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項3】
上記推定速度算出制御手段は、
上記本体部が上記台座部に装着されていないことが検出され、且つ上記測位情報が供給されないとき、上記測位情報を基に最後に算出された速度をこの時点における推定速度とする
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項4】
上記取付傾き算出手段は、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記取付傾きの算出を停止する
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項5】
上記取付傾き検出手段は、
所定の初期値を基に演算処理を再帰的に繰り返すことにより上記取付傾きを算出し、
上記本体部が上記台座部から取り外され再度装着された場合、上記本体部が上記台座部から取り外される前に検出した上記取付傾きを上記初期値として、当該取付傾きを再度算出する
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項6】
上記測位情報を基に算出された上記移動体の速度に応じて当該移動体の進行方向加速度を算出すると共に、上記測位情報に基づいて算出された上記移動体の速度及び方位に応じて上記進行方向加速度とは直交した水平方向加速度を算出する加速度算出手段と
を具え、
上記加速度センサは、
上記移動体の運動加速度及び重力加速度を観測し、
上記取付傾き算出手段は、
所定の気圧センサからの気圧値に応じて算出した路面の高度差と、上記移動体の速度に応じた進行距離とに基づいて上記路面の上記進行方向に対する傾斜角度を算出し、上記進行方向加速度と、上記水平方向加速度と、上記加速度センサによる運動加速度及び重力加速度を含む観測値と、上記傾斜角度とにより表される多次元の関数式に基づいて上記取付傾きを求める
ことを特徴とする請求項1に記載のナビゲーション装置。
【請求項7】
所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
上記本体部に作用する加速度を観測する加速度観測ステップと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
を具えることを特徴とする位置検出方法。
【請求項8】
ナビゲーション装置に対して、
所定の測位手段から供給される測位情報を基に、所定の台座部に装着されて移動体に取り付けられた本体部の現在位置を検出する検出ステップと、
上記測位情報を基に算出された上記移動体の速度に応じて、当該移動体に作用する移動体加速度を算出する加速度算出ステップと、
上記本体部に作用する加速度を観測する加速度観測ステップと、
上記移動体加速度及び上記観測された加速度に基づいて上記加速度センサの上記移動体に対する取付傾きを求める取付傾き算出ステップと、
上記測位情報が供給されないときに、上記観測された加速度及び上記取付傾きに基づき上記移動体の推定速度及び推定位置を算出する推定速度算出ステップと、
上記本体部が上記台座部に対して装着されているか否かを検出する着脱検出ステップと、
上記本体部が上記台座部に対して装着されていないことが検出された場合、上記推定速度算出ステップによる上記推定速度の算出を停止させる推定速度算出制御ステップと
を実行させることを特徴とする位置検出プログラム。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2008−58187(P2008−58187A)
【公開日】平成20年3月13日(2008.3.13)
【国際特許分類】
【出願番号】特願2006−236488(P2006−236488)
【出願日】平成18年8月31日(2006.8.31)
【出願人】(000002185)ソニー株式会社 (34,172)
【Fターム(参考)】
【公開日】平成20年3月13日(2008.3.13)
【国際特許分類】
【出願日】平成18年8月31日(2006.8.31)
【出願人】(000002185)ソニー株式会社 (34,172)
【Fターム(参考)】
[ Back to top ]