(57)【要約】【課題】１軸加速度センサと１軸角速度センサとを用いて、ヨー方向取付角を精度良く検出することが可能なナビゲーション装置を提供する。【解決手段】取付角検出用履歴作成部２０は、加速度計測部１６によって計測された加速度計測値と、加速度推定部１７によって推定された加速度推定値とを１組として、最新の予め定める組数分の履歴を作成する。ヨー方向取付角検出部は、作成された各組の履歴に基づいて、加速度推定値が計算された角度毎に、加速度推定値と加速度計測値との差異の標準偏差を求めるとともに、差異の標準偏差が極小となる加速度推定値を与える計算に用いられた角度を、ヨー方向取付角として求める。取付角検出用履歴作成部２０は、加速度計測値と、距離計測部１４によって計測された速度および前後方向加速度と、ヨー角計測部１５によって計測されたヨー角とを１組として、履歴を作成してもよい。【選択図】図１
2015004593.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載可能なナビゲーション装置であって、
前記移動体の速度に応じた信号として、前記移動体の移動距離に応じたパルス信号を出力する速度センサと、
自装置の筐体の垂直方向における前記移動体の角速度を検知する検知軸を有し、前記角速度に応じた信号を出力する角速度センサと、
前記移動体の進行方向に沿う方向であって、前記自装置の筐体の前後方向における前記移動体の加速度を検知する検知軸を有し、前記加速度に応じた信号を出力する加速度センサと、
前記速度センサから出力される前記パルス信号に基づいて、前記移動体の移動距離、速度および前後方向加速度を計測する距離計測部と、
予め定めるタイミング毎に、予め定める検知軸の方向における前記移動体の角速度およびヨー角を計測するヨー角計測部と、
予め定めるタイミング毎に、予め定める検知軸の方向における前記移動体の加速度を計測する加速度計測部と、
ヨー方向における前記筐体の取付角の探索範囲である検索角度範囲について、予め定める角度刻み毎に、その角度と前記筐体のピッチ方向における取付角とに対応する検知軸の方向における前記移動体の加速度を推定する加速度推定部と、
前記加速度計測部によって計測された加速度計測値と、前記加速度推定部によって推定された加速度推定値とを１組として、または前記加速度計測値と、前記距離計測部によって計測された速度および前後方向加速度と、前記ヨー角計測部によって計測されたヨー角とを１組として、最新の予め定める組数分の履歴を作成する取付角検出用履歴作成部と、
作成された各組の履歴に基づいて、前記加速度推定値が計算された角度毎に、前記加速度推定値と前記加速度計測値との差異の標準偏差を求めるとともに、前記差異の標準偏差が極小となる加速度推定値を与える計算に用いられた角度を、前記ヨー方向における前記筐体の取付角として求めるヨー方向取付角検出部とを備えることを特徴とするナビゲーション装置。
【請求項２】
前記取付角検出用履歴作成部は、前記加速度推定部によって推定された複数の加速度推定値と、前記加速度計測部によって計測された１つの加速度計測値とを１組として、最新の予め定める組数分の履歴を作成し、
前記ヨー方向取付角検出部は、前記取付角検出用履歴作成部によって前記履歴が作成された後に、作成された各組の履歴に基づいて、前記加速度推定値が計算された角度毎に、前記加速度推定値と前記加速度計測値との差異の標準偏差を求めるとともに、前記差異の標準偏差が極小となる加速度推定値を与える計算に用いられた角度を、前記ヨー方向における前記筐体の取付角として求めることを特徴とする請求項１に記載のナビゲーション装置。
【請求項３】
前記取付角検出用履歴作成部は、前記加速度計測部によって計測された加速度計測値と、前記距離計測部によって計測された速度および前後方向加速度と、前記ヨー角計測部によって計測されたヨー角とを１組として、最新の予め定める組数分の履歴を作成し、
前記加速度推定部は、前記取付角検出用履歴作成部によって前記履歴が作成された後に、作成された各組の履歴に基づいて、ヨー方向における前記筐体の取付角の探索範囲である検索角度範囲について、予め定める角度刻み毎に、その角度と前記筐体のピッチ方向における取付角とに対応する検知軸の方向における前記移動体の加速度を推定することを特徴とするナビゲーション装置。
【請求項４】
前記加速度推定部は、前記距離計測部によって計測された前記速度および前記前後方向加速度と、前記ヨー角計測部によって計測されたヨー角と、重力加速度とを用いて、ピッチ方向およびヨー方向における前記筐体の取付角として選択された取付角における前記前後方向加速度を推定することを特徴とする請求項１〜３のいずれか１つに記載のナビゲーション装置。
【請求項５】
前記ヨー方向取付角検出部は、前記ヨー方向における前記筐体の取付角の検索角度範囲内で、前記加速度推定値と加速度計測値との差異の標準偏差が、前記取付角が大きくなる方向に行くのに従って小さくなるか、または前記取付角が小さくなる方向に行くのに従って小さくなる場合、前記標準偏差が小さくなる方向に前記検索角度範囲を移動するとともに、前記検索角度範囲内の両端以外で前記標準偏差が最小となる角度があり、前記最小となる角度よりも前記取付角が大きくなる方向および小さくなる方向の両方に行くのに従って、前記標準偏差が大きくなる場合は、前記最小となる角度を極小と判断することを特徴とする請求項４に記載のナビゲーション装置。
【請求項６】
前記加速度推定部は、前記ヨー方向における前記筐体の取付角が検出されていない場合、予め定める角度よりも大きい角度毎に前記加速度を推定し、前記ヨー方向における前記筐体の取付角が検出された場合は、検出された前記取付角を中心とする前記検索角度範囲について、前記予め定める角度よりも小さい角度毎に前記加速度を推定することを特徴とする請求項４または５に記載のナビゲーション装置。
【請求項７】
前記加速度推定部は、自装置の筐体の前後方向、左右方向および垂直方向の３軸の加速度を推定し、
前記加速度推定部によって推定された３軸の加速度の推定値と、前記距離計測部によって計測された速度および前後方向加速度と、前記ヨー角計測部によって計測されたヨー角と、ピッチ方向取付角とを用いて、各軸の取付角が全て０°の場合の前後方向加速度を推定するとともに、前記３軸の加速度の推定値からピッチ角を推定するピッチ角推定部を備えることを特徴とする請求項４に記載のナビゲーション装置。
【請求項８】
前記取付角検出用履歴作成部は、前記移動体が予め定める角度以上旋回する毎に、履歴を作成し、
前記移動体の左折または右折が１回行われた場合に、前記履歴の予め定める組数分が全て格納されることを特徴とする請求項１〜３のいずれか１つに記載のナビゲーション装置。
【請求項９】
前記取付角検出用履歴作成部は、前記ヨー方向取付角検出部によって、ヨー方向検索範囲内に極小が２つ以上存在すると判断された場合、および前記ピッチ角推定部によって推定されたピッチ角が予め定める値よりも大きくなった場合の少なくとも一方の場合に、前記履歴を消去することを特徴とする請求項５に記載のナビゲーション装置。
【請求項１０】
前記加速度センサは、前記前後方向加速度とともに、自装置の筐体の左右方向の加速度、および垂直方向の加速度の少なくとも一方を計測する２軸または３軸の加速度センサであり、
前記ヨー方向取付角検出部は、前記前後方向加速度を計測可能な検知軸について、前記ヨー方向における前記筐体の取付角を検出するとともに、前記差異が予め定める値以下であり、各方向で検出された角度が一致するとき、その角度を前記ヨー方向における前記筐体の取付角として求めることを特徴とする請求項１〜３のいずれか１つに記載のナビゲーション装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に搭載され、現在地から目的地までの経路案内などを行うナビゲーション装置に関する。
【背景技術】
【０００２】
ナビゲーション装置は、たとえば自動車などの車両に搭載され、表示画面に表示される道路地図上で、車両の位置表示および現在地から目的地までの経路案内などを行う。道路地図上で車両の位置表示を行うにあたっては、全地球測位システム（Global Positioning System；略称：ＧＰＳ）と各種センサとを用いて、車両の姿勢角および慣性力などが計測または観測される。そして、マップマッチングと呼ばれる処理によって、地図データの道路リンク上で車両の位置の更新が行われる。
【０００３】
ナビゲーション装置は、筐体が傾斜して車両に取付けられることがある。筐体が傾斜して取付けられると、角速度センサおよび加速度センサなどの各センサの検知軸も傾斜する。角速度センサおよび加速度センサを用いて、姿勢角および慣性力などを正確に計測するためには、検知軸が傾斜した角速度センサおよび加速度センサの特性を考慮する必要がある。
【０００４】
また、車両の位置、姿勢角および慣性力などを航法演算によって求めるためには、それらを表現する直交３軸座標系を決める必要がある。特に、ナビゲーション装置の筐体が、予め定める取付角で傾斜して車両に設置される場合には、車両の姿勢角および慣性力を表現する複数の座標系を定義し、各座標系の間でセンサ計測値などを変換する必要がある。
【０００５】
センサ計測値の変換を行うにあたっては、基準とする座標系から別の座標系へ移るときに、基準座標系に対する回転を用いる方法が提案されている。この回転は、Ｘ，Ｙ，Ｚの各軸に対する回転角であるロール方向、ピッチ方向およびヨー方向の各回転角で表される。
【０００６】
座標系のＸ，Ｙ，Ｚ軸の定義は複数提案されている。たとえば、後述する図２に示す車体座標系（Body-Frame）では、車体１０の前後方向に延びる軸をＸＢ軸といい、車体１０の左右方向であって、車体１０の前後方向に直交する方向に延びる軸をＹＢ軸といい、車体１０の上下方向であって、車体１０の前後方向および左右方向に直交する方向に延びる軸をＺＢ軸という。
【０００７】
また、後述する図３に示すセンサ座標系（Sensor-Frame）では、Ｘ，Ｙ，Ｚ軸（以下、それぞれ、ＸＳ軸，ＹＳ軸，ＺＳ軸という場合がある）は、車体座標系のＸ，Ｙ，Ｚ軸、すなわちＸＢ軸、ＹＢ軸、ＺＢ軸に対して、ロール方向、ピッチ方向、ヨー方向の各取付角θｒ、θｐ、θｙで傾斜している。
【０００８】
各軸方向のセンサを用いて、ナビゲーション装置の取付角が検出される（たとえば、特許文献１〜４参照）。たとえば、特許文献１に開示される技術では、後述する図２および図３に示す座標系と同じ車体座標系およびセンサ座標系が使用される。特許文献１に開示される技術では、少なくとも２軸の加速度センサと、２軸の角速度センサとが用いられる。２軸の角速度センサは、センサ座標系のＸＳ軸の角速度であるロール角速度と、ＺＳ軸の角速度であるヨー角速度とを検出する。
【０００９】
特許文献２に開示される技術では、後述する図２および図３に示す座標系と異なる車体座標系およびセンサ座標系が使用される。特許文献２に開示される技術では、２軸の加速度センサと、１軸、具体的にはＺＳ軸の角速度センサとを使用して、ピッチ方向およびヨー方向の取付角が検出可能とされる。
【００１０】
特許文献３に開示される技術では、後述する図２のＸ軸をπ回転した車体座標系が使用される。特許文献３に開示される技術では、１軸たとえばＸＳ軸の加速度センサと、１軸たとえばＺＳ軸の角速度センサとを使用して、ピッチ方向、ヨー方向、ロール方向の３軸全ての取付角が検出可能とされる。
【００１１】
特許文献４に開示される技術では、後述する図２および図３に示す座標系と同じ車体座標系およびセンサ座標系が使用される。特許文献４に開示される技術では、１軸たとえばＸＳ軸の加速度センサと、１軸たとえばＺＳ軸の角速度センサとを使用して、ピッチ方向取付角θｐおよびヨー方向取付角θｙが検出される。
【先行技術文献】
【特許文献】
【００１２】
【特許文献１】特許４６５５９０１号公報（請求項１，６、段落［０００６］，［００１２］〜［００１５］，［００２６］〜［００３６］）
【特許文献２】特開２００５−１４７６９６号公報（請求項１，８、段落［０００７］〜［００１０］，［００１３］〜［００１４］，［００３１］〜［００４５］，［００７０］〜［００７７］）
【特許文献３】特許４７３９３７８号公報（請求項１〜３、段落［０００５］，［００１９］〜［００２１］，［００３５］〜［００４４］）
【特許文献４】特許４４４３６２１号公報（請求項１，２、段落［００２１］，［００２８］，［００３９］〜［００４１］，［００５０］，［００５６］〜［００６０］）
【発明の概要】
【発明が解決しようとする課題】
【００１３】
たとえば、ループ橋または立体駐車場のようなスロープを車両が走行すると、車両に搭載されたナビゲーション装置は、角速度センサの検知軸が傾斜するので、感度が低下するという問題がある。これに対して、補正式を用いれば、たとえば坂道を車両が走行しても、ヨー角の感度の低下分を補正することができる。
【００１４】
しかし、ヨー方向に検知軸が傾斜した加速度センサは、ヨー方向取付角θｐに応じて前後加速度が減衰し、左右加速度、すなわち遠心力の影響を受ける。これによって、加速度センサからピッチ角φＨｐを正しく計測することができなくなる。
【００１５】
この不正確なピッチ角を用いてヨー角の感度の低下分を補正すると、前述のループ橋または立体駐車場のようなスロープを車両が走行するときなどに、方位誤差を生じるという問題がある。この問題を解決するためには、ヨー方向取付角を検出する必要がある。
【００１６】
また、ピッチ方向およびヨー方向の各取付角の自動検出性能は、ナビゲーション装置に内蔵されるセンサの構成によって決まる。たとえば、３軸加速度センサと３軸角速度センサとを備えている場合には、３軸の取付角を検出するのに必要な３軸のセンサ計測値を利用できるが、高性能になる分、高価になる。
【００１７】
これに対し、３軸加速度センサと３軸角速度センサとを使用できない、いわゆる廉価版のナビゲーション装置では、備えていない検知軸のセンサ計測値はないものの、ピッチ方向およびヨー方向ともに、約４５°以下の取付角に対応できることがユーザから求められている。
【００１８】
前述の特許文献１および２に開示されるナビゲーション装置では、２軸以上の加速度センサを備える必要があるので、１軸加速度および１軸角速度センサのみを用いる廉価版のナビゲーション装置としては使用することができない。
【００１９】
また、前述の特許文献３に開示されるナビゲーション装置では、内蔵するセンサは１軸加速度センサおよび１軸角速度センサであるが、３軸全ての取付角を検出可能とされている。しかし、特許文献３に開示される技術では、前述の検知軸が傾斜した角速度センサおよび加速度センサの特性を考慮せずに、ピッチ方向取付角θｐが算出されている。
【００２０】
特許文献３に開示される技術で用いられている式は、車両が水平面に停車しているときにピッチ方向取付角θｐを検出する計算式である。特許文献３に開示される技術では、種々の傾斜角が存在する実際の道路または駐車場などで水平面を検出する方法を考慮していない。したがって、ピッチ方向取付角θｐの検出誤差が大きくなり、さらにピッチ方向取付角θｐを用いて算出されるヨー方向取付角θｙの精度も十分とはいえないという問題がある。
【００２１】
特許文献１〜３に開示されるナビゲーション装置では、いずれも車両が水平面を走行しているときに限定して取付角を検出しているので、車両が水平面を走行しているか否かの判断精度が取付角の検出精度を左右し、最終的には車両の位置精度を決める大きな要因になっている。
【００２２】
また、前述の特許文献４に開示されるナビゲーション装置は、特許文献３に開示される技術と同様に、センサとしては、１軸加速度センサおよび１軸角速度センサのみを備えている。特許文献４に開示される技術では、車両が坂道を含む道路または駐車場などを走行しても、自動的に水平面を走行しているか否かを判断して、ピッチ方向取付角θｐを検出し、さらには、水平面での右左折走行時にヨー方向取付角θｙを検出する。
【００２３】
しかし、特許文献４に開示される技術では、ヨー方向取付角θｙの検出については、加速度センサによる加速度計測値と加速度推定値との差異そのものを取扱っている。したがって、加速度センサに約１°相当の零点誤差、および温度変動（ドリフト）があると、ヨー方向取付角θｙの検出精度が低下するおそれがある。
【００２４】
本発明の目的は、１軸加速度センサと１軸角速度センサとを用いて、ヨー方向取付角を精度良く検出することが可能なナビゲーション装置を提供することである。
【課題を解決するための手段】
【００２５】
本発明のナビゲーション装置は、移動体に搭載可能なナビゲーション装置であって、移動体の速度に応じた信号として、移動体の移動距離に応じたパルス信号を出力する速度センサと、自装置の筐体の垂直方向における移動体の角速度を検知する検知軸を有し、角速度に応じた信号を出力する角速度センサと、移動体の進行方向に沿う方向であって、自装置の筐体の前後方向における移動体の加速度を検知する検知軸を有し、加速度に応じた信号を出力する加速度センサと、速度センサから出力されるパルス信号に基づいて、移動体の移動距離、速度および前後方向加速度を計測する距離計測部と、予め定めるタイミング毎に、予め定める検知軸の方向における移動体の角速度およびヨー角を計測するヨー角計測部と、予め定めるタイミング毎に、予め定める検知軸の方向における移動体の加速度を計測する加速度計測部と、ヨー方向における筐体の取付角の探索範囲である検索角度範囲について、予め定める角度刻み毎に、その角度と筐体のピッチ方向における取付角とに対応する検知軸の方向における移動体の加速度を推定する加速度推定部と、加速度計測部によって計測された加速度計測値と、加速度推定部によって推定された加速度推定値とを１組として、または加速度計測値と、距離計測部によって計測された速度および前後方向加速度と、ヨー角計測部によって計測されたヨー角とを１組として、最新の予め定める組数分の履歴を作成する取付角検出用履歴作成部と、作成された各組の履歴に基づいて、加速度推定値が計算された角度毎に、加速度推定値と加速度計測値との差異の標準偏差を求めるとともに、差異の標準偏差が極小となる加速度推定値を与える計算に用いられた角度を、ヨー方向における筐体の取付角として求めるヨー方向取付角検出部とを備えることを特徴とする。
【発明の効果】
【００２６】
本発明のナビゲーション装置によれば、検知軸が１軸の加速度センサと、検知軸が１軸の角速度センサとを用いて、ヨー方向における筐体の取付角（以下「ヨー方向取付角」という場合がある）を検出することができる。また、ヨー方向取付角の検索角度範囲において、加速度推定値と加速度計測値との差異の標準偏差が極小となる角度をヨー方向取付角とするので、約１°相当の加速度センサの零点誤差、および温度変動（ドリフト）の影響を受け難い。また、緩やかな道路傾斜があっても、ヨー方向取付角を検出することができる。
【図面の簡単な説明】
【００２７】
【図１】本発明の第１の実施の形態であるナビゲーション装置１の構成を示すブロック図である。
【図２】本発明の第１の実施の形態のナビゲーション装置１で使用される座標系を示す図である。
【図３】本発明の第１の実施の形態のナビゲーション装置１で使用される座標系を示す図である。
【図４】本発明の第１の実施の形態のナビゲーション装置１における姿勢・慣性力計測処理の処理手順を示すフローチャートである。
【図５】本発明の第１の実施の形態のナビゲーション装置１における姿勢・慣性力計測処理の処理手順を示すフローチャートである。
【図６】ＸＳ軸加速度の推定値の一例を示すグラフである。
【図７】最新の予め定める数分の履歴に格納されたＸＳ軸加速度の推定値および計測値について、予め定める角度刻み毎の差異の標準偏差からヨー方向取付角を検出する方法を示すグラフである。
【図８】図７に示す領域４０の拡大図である。
【図９】ＸＳ軸加速度の計測値および推定値を示すグラフである。
【図１０】ＸＳ軸加速度の計測値から変換したＸＢ軸加速度、および速度センサによるＸＢ軸加速度の一例を示すグラフである。
【図１１】ＹＳ軸加速度の計測値および推定値を示すグラフである。
【図１２】ＺＳ軸加速度の計測値および推定値を示すグラフである。
【図１３】本発明の第２の実施の形態であるナビゲーション装置２の構成を示すブロック図である。
【図１４】本発明の第２の実施の形態であるナビゲーション装置２における姿勢・慣性力計測処理の処理手順を示すフローチャートである。
【図１５】本発明の第２の実施の形態であるナビゲーション装置２における姿勢・慣性力計測処理の処理手順を示すフローチャートである。
【図１６】第１の前提技術で使用される座標系を示す図である。
【図１７】第１の前提技術で使用される座標系を示す図である。
【図１８】第２の前提技術で使用される座標系を示す図である。
【図１９】第２の前提技術で使用される座標系を示す図である。
【図２０】第３の前提技術で使用される座標系を示す図である。
【図２１】車両５０の走行状態の一例を示す図である。
【図２２】車両５０の走行状態の一例を示す図である。
【発明を実施するための形態】
【００２８】
＜前提技術＞
本発明のナビゲーション装置を説明する前に、本発明の前提技術のナビゲーション装置を説明する。ナビゲーション装置は、移動体、たとえば自動車などの車両に搭載され、表示画面に表示される道路地図上で、車両の位置表示および現在地から目的地までの経路案内などを行う。
【００２９】
道路地図上で車両の位置表示を行うにあたっては、全地球測位システム（Global Positioning System；略称：ＧＰＳ）と各種センサとを用いて、車両の姿勢角および慣性力などが計測または観測される。そして、マップマッチングと呼ばれる処理によって、地図データの道路リンク上で車両の位置の更新が行われる。
【００３０】
ナビゲーション装置は、筐体が傾斜して車両に取付けられることがある。筐体が傾斜して取付けられると、角速度センサおよび加速度センサなどの各センサの検知軸も傾斜する。角速度センサおよび加速度センサの検知軸は、たとえばピッチ方向およびヨー方向に傾斜する。検知軸がピッチ方向およびヨー方向に傾斜した角速度センサおよび加速度センサを用いて、姿勢角および慣性力などを正確に計測するためには、以下の（１）〜（４）の特性を考慮する必要がある。
【００３１】
（１）角速度センサおよび加速度センサは、検知軸に作用する角速度および加速度をそれぞれ計測する。検知軸が傾斜すると、検知軸に作用する角速度および加速度の感度が、その傾斜角に応じて低下し、逆に傾斜方向にある他の軸の感度が大きくなる。取付角に依存して検知軸が３次元で傾斜する場合は、取付角に応じて各軸に配分された角速度および加速度が検出される。
【００３２】
（２）３軸のセンサの場合は、３軸のセンサ信号から、任意の取付角における任意の検知軸のセンサ信号に変換できる。しかし、２軸または１軸のセンサの場合は、備えていない検知軸で計測すべき角速度および加速度がないので、限定条件付きでなければ、任意の取付角における任意の検知軸のセンサ信号に変換、すなわち復元できない。
【００３３】
（３）角速度センサについては、車両が停車中であるときには角速度が発生しないので、停車中に計測された角速度を零点と見ることができる。
【００３４】
（４）加速度センサは、検知軸傾斜による重力成分を常に検出するので、加速度センサのみで、検知軸の傾斜要因、たとえば取付角および道路傾斜角を判断することができない。したがって、停車中の加速度を零点と見ることができない。
【００３５】
また、車両の位置、姿勢角および慣性力などを航法演算によって求めるためには、それらを表現する直交３軸座標系を決める必要がある。特に、ナビゲーション装置の筐体が、予め定める取付角で傾斜して車両に設置される場合には、車両の姿勢角および慣性力を表現する複数の座標系を定義し、各座標系の間でセンサ計測値などを変換する必要がある。
【００３６】
センサ計測値の変換を行うにあたっては、基準とする座標系から別の座標系へ移るときに、基準座標系に対する回転を用いる方法が提案されている。この回転は、Ｘ，Ｙ，Ｚの各軸に対する回転角であるロール方向、ピッチ方向およびヨー方向の各回転角で表される。以下の説明では、ロール方向の回転角を「ロール角」といい、ピッチ方向の回転角を「ピッチ角」といい、ヨー方向の回転角を「ヨー角」という場合がある。
【００３７】
座標系のＸ，Ｙ，Ｚの各軸の定義は複数提案されている。ここでは、後述する図２を参照し、車体１０の前後方向、左右方向および上下方向の各方向に延びる軸で構成される車体座標系（Body-Frame）を基準座標系として説明する。
【００３８】
車体座標系では、車体１０の前後方向に延びる軸をＸＢ軸といい、車体１０の左右方向であって、車体１０の前後方向に直交する方向に延びる軸をＹＢ軸といい、車体１０の上下方向であって、車体１０の前後方向および左右方向に直交する方向に延びる軸をＺＢ軸という。各軸の名称の添え字の「Ｂ」は、「車体座標系（Body-Frame）」であることを示す。各軸の詳細については後述する。
【００３９】
また、後述する図３を参照して、センサ検知軸が示す座標系であるセンサ座標系（Sensor-Frame）について説明する。センサ座標系のＸ，Ｙ，Ｚ軸（以下、それぞれ、ＸＳ軸，ＹＳ軸，ＺＳ軸という場合がある）は、車体座標系のＸ，Ｙ，Ｚ軸、すなわちＸＢ軸、ＹＢ軸、ＺＢ軸に対して、ロール方向、ピッチ方向、ヨー方向の各取付角θｒ、θｐ、θｙで傾斜している。各軸の名称の添え字の「Ｓ」は、「センサ座標系（Sensor-Frame）」であることを示す。
【００４０】
ロール方向、ピッチ方向およびヨー方向の各取付角θｒ，θｐ，θｙ（以下、それぞれ、ロール方向取付角θｒ、ピッチ方向取付角θｐ、ヨー方向取付角θｙという場合がある）が分かれば、車体座標系からセンサ座標系に変換する座標変換行列ＣＢＳを以下の式（１）によって求めることができる。この座標変換行列ＣＢＳを用いれば、以下の式（２）によって、車体座標系の計測量ＭＢをセンサ座標系の計測量ＭＳに座標変換することができる。
【００４１】
【数１】
2015004593.tif 000003
【００４２】
【数２】
2015004593.tif 000004
【００４３】
同様に、ロール方向、ピッチ方向およびヨー方向の各取付角θｒ，θｐ，θｙが分かれば、センサ座標系から車体座標系に変換する座標変換行列ＣＳＢを以下の式（３）によって求めることができる。この座標変換行列ＣＳＢを用いれば、以下の式（４）によって、センサ座標系の計測量ＭＳを車体座標系の計測量ＭＢに座標変換することができる。この計測量は、計算目的に応じて、姿勢角または慣性力となる。
【００４４】
【数３】
2015004593.tif 000005
【００４５】
【数４】
2015004593.tif 000006
【００４６】
以上の式（１）〜式（４）の座標変換式をそれぞれ駆使して、ナビゲーション装置の取付角が検出される。
【００４７】
図１６および図１７は、第１の前提技術で使用される座標系を示す図である。ナビゲーション装置５１は、車両５０に搭載される。第１の前提技術では、ナビゲーション装置５１は、後述する図２および図３と同じ車体座標系およびセンサ座標系を使用する。第１の前提技術は、前述の特許文献１に開示される技術に相当する。図１６において、参照符号「５２」で示される矢印は、ピッチ方向の回転を表す。図１７において、参照符号「５３」で示される矢印は、ヨー方向の回転を表す。
【００４８】
第１の前提技術では、ナビゲーション装置５１は、検知軸が少なくとも２軸の加速度センサと、検知軸が２軸の角速度センサとを備える。２軸の加速度センサは、軸の組合せは指定されない。２軸の角速度センサは、センサ座標系のＸＳ軸まわりの角速度であるロール角速度と、センサ座標系のＺＳ軸まわりの角速度であるヨー角速度とを検出する。
【００４９】
第１の前提技術では、ピッチ方向およびヨー方向の取付角が検出可能とされる。２軸以上の加速度センサを用いて算出された取付角と、２軸以上の角速度センサを用いて算出された取付角とが一致した場合に、取付角として採用される。
【００５０】
第１の前提技術では、ロール方向取付角θｒが０°で固定されている。したがって、車体座標系からセンサ座標系に変換する前述の式（１）の座標変換行列ＣＢＳは、ピッチ方向取付角θｐおよびヨー方向取付角θｙのみに基づいて、以下の式（５）に示すように簡略化される。
【００５１】
【数５】
2015004593.tif 000007
【００５２】
式（５）の座標変換行列ＣＢＳを用いて、以下の式（６）および式（７）によって、車体座標系の３軸加速度ＡＢ（ＡＢＸ，ＡＢＹ，ＡＢＺ）および３軸角速度ωＢ（ωＢＸ，ωＢＹ，ωＢＺ）が、センサ座標系の３軸加速度ＡＳ（ＡＳＸ，ＡＳＹ，ＡＳＺ）および３軸角速度ωＳ（ωＳＸ，ωＳＹ，ωＳＺ）にそれぞれ変換される。
【００５３】
【数６】
2015004593.tif 000008
【００５４】
【数７】
2015004593.tif 000009
【００５５】
式（６）および式（７）の関係から、センサ座標系のＸ，Ｙ，Ｚの各軸の加速度および角速度が、以下の式（８）〜式（１３）によって、それぞれ個別に求められる。
【００５６】
【数８】
2015004593.tif 000010
【００５７】
【数９】
2015004593.tif 000011
【００５８】
【数１０】
2015004593.tif 000012
【００５９】
【数１１】
2015004593.tif 000013
【００６０】
【数１２】
2015004593.tif 000014
【００６１】
【数１３】
2015004593.tif 000015
【００６２】
ここで、速度センサから出力されるパルス信号から算出された加速度をＡＳＰＤとし、重力加速度をＧとすると、車両５０が水平面上を直進して走行しているときの車体座標系の３軸加速度ＡＢは（ＡＳＰＤ，０，Ｇ）となる。これを式（９）に代入することによって、式（９）が式（１４）に変形される。式（１４）を用いて、ヨー方向取付角θｙが推定される。
【００６３】
【数１４】
2015004593.tif 000016
【００６４】
また、ＡＢＹ＝０から、式（８）および式（１０）が、それぞれ、式（１５）および式（１６）に簡略化される。式（１５）および式（１６）を用いて、ピッチ方向取付角θｐが推定される。
【００６５】
【数１５】
2015004593.tif 000017
【００６６】
【数１６】
2015004593.tif 000018
【００６７】
また車両５０が水平面を旋回して走行しているときには、車体座標系の３軸角速度ωＢは（０，０，ωＢＺ）となる。これを式（１１）および式（１３）に代入することによって、式（１１）および式（１３）は、それぞれ、以下の式（１７）および式（１８）に簡略化される。式（１７）および式（１８）から、ピッチ方向取付角θｐは、以下の式（１９）によって算出される。
【００６８】
【数１７】
2015004593.tif 000019
【００６９】
【数１８】
2015004593.tif 000020
【００７０】
【数１９】
2015004593.tif 000021
【００７１】
図１８および図１９は、第２の前提技術で使用される座標系を示す図である。第２の前提技術では、後述の図２および図３に示す座標系と異なる車体座標系およびセンサ座標系が使用される。第２の前提技術は、前述の特許文献２に開示される技術に相当する。
【００７２】
第２の前提技術のナビゲーション装置５１では、２軸の加速度センサと、１軸、具体的にはＺＳ軸の角速度センサとを使用して、ピッチ方向およびヨー方向の取付角が検出可能とされる。
【００７３】
車両５０が水平面上を旋回しているときに、速度センサから出力されるパルス信号から算出された速度をＶＳＰＤとし、加速度をＡＳＰＤとし、ヨーレートをωＢＺとして、車体座標系のＸ軸の加速度ＡＳＸおよびＹ軸の加速度ＡＳＹが、それぞれ、以下の式（２０）および式（２１）によって求められる。
【００７４】
【数２０】
2015004593.tif 000022
【００７５】
【数２１】
2015004593.tif 000023
【００７６】
式（２０）および式（２１）から、ヨー方向取付角θｙおよびピッチ方向取付角θｐは、それぞれ、以下の式（２２）および式（２３）によって算出される。
【００７７】
【数２２】
2015004593.tif 000024
【００７８】
【数２３】
2015004593.tif 000025
【００７９】
また、車両５０が直進しているときに限定すると、式（２２）および式（２３）は、それぞれ、以下の式（２４）および式（２５）に示すように簡略化される。式（２４）および式（２５）を用いて、ヨー方向取付角θｙおよびピッチ方向取付角θｐが求められる。
【００８０】
【数２４】
2015004593.tif 000026
【００８１】
【数２５】
2015004593.tif 000027
【００８２】
ヨー方向取付角θｙおよびピッチ方向取付角θｐが検出された後は、以下の式（２６）によって、ＹＢ軸の加速度に変換される。式（２６）から、ピッチ角φｐは、以下の式（２７）によって求められる。
【００８３】
【数２６】
2015004593.tif 000028
【００８４】
【数２７】
2015004593.tif 000029
【００８５】
図２０は、第３の前提技術で使用される座標系を示す図である。第３の前提技術では、後述の図２のＸ軸をπ回転した図２０に示す車体座標系が使用される。第３の前提技術のナビゲーション装置５１では、１軸たとえばＸＳ軸の加速度センサと、１軸たとえばＺＳ軸の角速度センサとを使用して、ピッチ方向、ヨー方向、ロール方向の３軸全ての取付角が検出可能とされる。第３の前提技術は、前述の特許文献３に開示される技術に相当する。
【００８６】
図２０において、参照符号「６１」で示される矢印は、Ｘ軸まわりの回転であるロール方向の回転を表す。参照符号「６２」で示される矢印は、Ｙ軸まわりの回転であるピッチ方向の回転を表す。参照符号「６３」で示される矢印は、Ｚ軸まわりの回転であるヨー方向の回転を表す。
【００８７】
第３の前提技術では、前後方向加速度がゼロ（０）になるとき、たとえば車両５０が停車時または等速走行時の加速度センサの出力ＡＳＸを用いて、加速度センサの出力ＡＳＸを表す前述の式（８）においてＡＢＸ＝０およびＡＢＹ＝０として、以下の式（２８）によってピッチ方向取付角θｐが算出される。
【００８８】
【数２８】
2015004593.tif 000030
【００８９】
車両５０が水平面上を直進しているときのＧＰＳの位置の変化量から前後方向加速度ＡＸ−ＧＰＳが求められ、前述の式（８）においてＡＢＹ＝０として、以下の式（２９）によってヨー方向取付角θｙが算出される。ロール方向取付角θｒについては、説明を省略する。
【００９０】
【数２９】
2015004593.tif 000031
【００９１】
第４の前提技術のナビゲーション装置では、後述する図２および図３の座標系と同じ座標系、具体的には、第１の前提技術と同様に、図１６および図１７に示す車体座標系およびセンサ座標系が使用される。第４の前提技術では、１軸たとえばＸＳ軸の加速度センサと、１軸たとえばＺＳ軸の角速度センサとを使用して、ピッチ方向取付角θｐおよびヨー方向取付角θｙが検出される。第４の前提技術は、前述の特許文献４に開示される技術に相当する。
【００９２】
第４の前提技術では、車両５０が水平面上を走行しているときにＸＳ軸の加速度センサが計測した加速度について、以下の式（３０）および式（３１）によって、予め定める角度毎の推定値ＡＳＸ−ＥＳＴが算出される。式（３０）および式（３１）は、前述の特許文献４では、電圧に換算した式で表現されているが、第４の前提技術では、他の前提技術と同様に、加速度形式で表現されている。
【００９３】
【数３０】
2015004593.tif 000032
【００９４】
【数３１】
2015004593.tif 000033
【００９５】
ここで、ＶＳＰＤは、速度センサから出力されるパルス信号から算出された速度であり、ωＢＺは、速度センサから出力されるパルス信号から算出された加速度であり、ωＢＺは、角速度センサによって計測されたヨーレートである。
【００９６】
第４の前提技術のナビゲーション装置は、算出された複数の推定値ＡＳＸ−ＥＳＴと加速度センサによる計測値ＡＳＸとを記録し、計測値と最も一致する推定値を算出した角度をヨー方向取付角として採用する。そして、ヨー方向取付角θｙに基づいて式（３０）によって算出されたＸ軸加速度の推定値ＡＳＸ−ＥＳＴと、加速度センサによって計測された前後加速度計測値ＡＳＸとから、ピッチ角φｐは、以下の式（３２）によって算出される。
【００９７】
【数３２】
2015004593.tif 000034
【００９８】
図２１および図２２は、車両５０の走行状態の一例を示す図である。たとえば、図２１に示すようなループ橋または立体駐車場のスロープ７０を車両５０が走行すると、車両５０に搭載されたナビゲーション装置は、角速度センサの検知軸が傾斜するので、感度が低下する。したがって、車両５０の位置として、参照符号「７１」で示される本来の位置ではなく、参照符号「７２」で示される位置を誤って検出してしまうという問題がある。
【００９９】
これに対して、以下の式（３３）を用いれば、ヨー角の感度の低下分を補正することができる。これによって、たとえば図２２に示すような坂道７５を車両５０が走行しても、車両５０の位置を精度良く検出することができる。
【０１００】
【数３３】
2015004593.tif 000035
【０１０１】
しかし、ヨー方向に検知軸が傾斜した加速度センサは、前述の特性（１）に示すように、ヨー方向取付角θｐに応じて前後加速度が減衰し、左右加速度、すなわち遠心力の影響を受ける。これによって、加速度センサからピッチ角φＨｐを正しく計測することができなくなる。
【０１０２】
この不正確なピッチ角を用いてヨー角の感度の低下分を補正すると、前述の図２１に示すようなループ橋または立体駐車場のスロープ７０を車両５０が走行するときなどに、方位誤差を生じるという問題がある。この問題を解決するためには、ヨー方向取付角を検出する必要がある。
【０１０３】
また、ピッチ方向およびヨー方向の各取付角の自動検出性能は、ナビゲーション装置に内蔵されるセンサの構成によって決まる。たとえば、３軸加速度センサと３軸角速度センサとを備えている場合には、前述の特性（２）から、３軸の取付角を検出するのに必要な３軸のセンサ計測値を利用できるが、高性能になる分、高価になる。
【０１０４】
これに対し、３軸加速度センサと３軸角速度センサとを使用できない、いわゆる廉価版のナビゲーション装置では、備えていない検知軸のセンサ計測値はないものの、ピッチ方向およびヨー方向ともに、約４５°以下の取付角に対応できることがユーザから求められている。
【０１０５】
前述の第１および第２の前提技術のナビゲーション装置では、２軸以上の加速度センサを備える必要があるので、１軸加速度および１軸角速度センサのみを用いる廉価版のナビゲーション装置としては使用することができない。
【０１０６】
また、前述の第３の前提技術のナビゲーション装置では、内蔵するセンサは１軸加速度センサおよび１軸角速度センサであるが、３軸全ての取付角を検出可能とされている。しかし、第３の前提技術では、前述の特性（４）を考慮せずに、ピッチ方向取付角θｐが算出されている。
【０１０７】
第３の前提技術で用いられている式は、車両５０が水平面に停車しているときにピッチ方向取付角θｐを検出する計算式である。第３の前提技術では、種々の傾斜角が存在する実際の道路または駐車場などで水平面を検出する方法を考慮していない。したがって、ピッチ方向取付角θｐの検出誤差が大きくなり、さらにピッチ方向取付角θｐを用いて算出されるヨー方向取付角θｙの精度も十分とはいえないという問題がある。
【０１０８】
第１〜第３の前提技術のナビゲーション装置では、いずれも車両５０が水平面を走行しているときに限定して取付角を検出しているので、車両５０が水平面を走行しているか否かの判断精度が取付角の検出精度を左右し、最終的には車両５０の位置精度を決める大きな要因になっている。
【０１０９】
また、前述の第４の前提技術のナビゲーション装置は、第３の前提技術と同様に、センサとしては、１軸加速度センサおよび１軸角速度センサのみを備えている。第４の前提技術では、車両５０が坂道を含む道路または駐車場などを走行しても、自動的に水平面を走行しているか否かを判断して、ピッチ方向取付角θｐを検出し、さらには、水平面での右左折走行時にヨー方向取付角θｙを検出する。
【０１１０】
しかし、第４の前提技術では、ヨー方向取付角θｙの検出については、加速度センサによる加速度計測値と加速度推定値との差異そのものを取扱っている。したがって、加速度センサに約１°相当の零点誤差、および温度変動（ドリフト）があると、ヨー方向取付角θｙの検出精度が低下するおそれがある。
【０１１１】
そこで本発明のナビゲーション装置では、以下の実施の形態に示す構成を採用している。
【０１１２】
＜第１の実施の形態＞
図１は、本発明の第１の実施の形態であるナビゲーション装置１の構成を示すブロック図である。図１では、ナビゲーション装置１の構成のうち、車体の姿勢および慣性力の計測に必要な構成を示し、経路探索および経路案内などの機能を実現するための構成は省略している。
【０１１３】
ナビゲーション装置１は、速度センサ１１、角速度センサ１２、加速度センサ１３、距離計測部１４、ヨー角計測部１５、加速度計測部１６、加速度推定部１７、加速度変換部１８、ピッチ角推定部１９、取付角検出用履歴作成部２０、ヨー方向取付角検出部２１、地図データ記憶部２２および道路照合部２３を備えて構成される。
【０１１４】
ナビゲーション装置１は、たとえばコンピュータによって構成されており、不図示のメモリに記憶されている制御プログラムを実行することによって、距離計測部１４、ヨー角計測部１５、加速度計測部１６、加速度推定部１７、加速度変換部１８、ピッチ角推定部１９、取付角検出用履歴作成部２０およびヨー方向取付角検出部２１の各機能が実現される。
【０１１５】
速度センサ１１、角速度センサ１２、加速度センサ１３、距離計測部１４、ヨー角計測部１５、加速度計測部１６、加速度推定部１７、加速度変換部１８、ピッチ角推定部１９、取付角検出用履歴作成部２０、ヨー方向取付角検出部２１、地図データ記憶部２２および道路照合部２３は、ナビゲーション装置１の筐体に収容される。具体的には、前述の各機能を実現するコンピュータが、筐体に収容される。
【０１１６】
本実施の形態のナビゲーション装置１は、移動体、たとえば自動車などの車両に搭載可能に構成される。車両に搭載される場合、ナビゲーション装置１は、車両を構成する車体に取付けられる。
【０１１７】
図２および図３は、本発明の第１の実施の形態のナビゲーション装置１で使用される座標系を示す図である。図２は、ナビゲーション装置１が取付けられた車体１０を示す斜視図である。図３（ａ）は、ナビゲーション装置１が取付けられた車体１０を示す平面図であり、図３（ｂ）は、ナビゲーション装置１が取付けられた車体１０を示す右側面図であり、図３（ｃ）は、ナビゲーション装置１が取付けられた車体１０を示す正面図である。
【０１１８】
本実施の形態のナビゲーション装置１では、図２に示す車体座標系、および図３に示すセンサ座標系が使用される。図２に示す車体座標系は、車体１０の前後方向、左右方向および上下方向の各方向に延びる軸で構成される。車体座標系では、車体１０の前後方向に延びる軸をＸＢ軸といい、車体１０の左右方向であって、車体１０の前後方向に直交する方向に延びる軸をＹＢ軸といい、車体１０の上下方向であって、車体１０の前後方向および左右方向に直交する方向に延びる軸をＺＢ軸という。
【０１１９】
車体座標系において、ＸＢ軸とＹＢ軸とは互いに直交し、ＺＢ軸は、ＸＢ軸とＹＢ軸とで規定されるＸＢ−ＹＢ平面に対して直交する。車体１０の前後方向のうち、前方向をＸＢ軸の正方向とする。車体１０の左右方向のうち、右方向をＹＢ軸の正方向とする。車体１０の上下方向のうち、下方向をＺＢ軸の正方向とする。
【０１２０】
ここで、車体１０の前後方向のうち、前方向とは、車両の運転席に着座した運転手の正面方向に一致する方向をいい、後方向とは、車両の運転席に着座した運転手の背面方向に一致する方向をいう。車体１０の左右方向のうち、左方向とは、車両の運転席に着座した運転手の左方向に一致する方向をいい、右方向とは、車両の運転席に着座した運転手の右方向に一致する方向をいう。車体１０の上下方向のうち、上方向とは、車両の運転席に着座した運転手の上方向に一致する方向をいい、下方向とは、車両の運転席に着座した運転手の下方向に一致する方向をいう。
【０１２１】
図３に示すセンサ座標系は、センサ検知軸が示す座標系である。図３に示すセンサ座標系は、ナビゲーション装置１の筐体の前後方向、左右方向および上下方向の各方向に延びる軸で構成される。センサ座標系では、筐体の前後方向に延びる軸をＸｓ軸といい、筐体の左右方向であって、筐体の前後方向に直交する方向に延びる軸をＹｓ軸といい、筐体の上下方向であって、筐体の前後方向および左右方向に直交する方向に延びる軸をＺｓ軸という。筐体の前後方向は、車両の進行方向に沿う方向となる。筐体の上下方向は、筐体の垂直方向に相当する。
【０１２２】
ナビゲーション装置１は、筐体が傾斜して車両１０の車体に取付けられることがある。ナビゲーション装置１の筐体が傾斜して取付けられると、角速度センサ１２および加速度センサ１３などの各センサの検知軸も傾斜する。この場合、検知軸に相当するセンサ座標系のＸ，Ｙ，Ｚ軸、すなわちＸＳ軸，ＹＳ軸，ＺＳ軸は、車体座標系のＸ，Ｙ，Ｚ軸、すなわちＸＢ軸、ＹＢ軸、ＺＢ軸に対して、傾斜している。
【０１２３】
以下の説明では、車体座標系の各軸に対するセンサ座標系の各軸の傾斜角度を、「取付角」という。図３（ｃ）に示すように、車体１０の前方向から見て、車体座標系のＺＢ軸に対するセンサ座標系のＺＳ軸の傾斜角を「ロール方向取付角」といい、「θｒ」で表す。図３（ｂ）に示すように、車体１０の右方向から見て、車体座標系のＸＢ軸に対するセンサ座標系のＸＳ軸の傾斜角を「ピッチ方向取付角」といい、「θｐ」で表す。図３（ａ）に示すように、車体１０の上方向から見て、車体座標系のＸＢ軸に対するセンサ座標系のＸｓ軸の傾斜角を「ヨー方向取付角」といい、「θｙ」で表す。
【０１２４】
図１に戻って、速度センサ１１は、車両の速度に応じた信号として、車両の移動距離に応じたパルス信号を出力する。速度センサ１１は、出力したパルス信号を距離計測部１４に与える。
【０１２５】
角速度センサ１２は、たとえばジャイロによって構成される。角速度センサ１２は、車両の角速度として、自装置であるナビゲーション装置１の筐体の垂直方向における車両の角速度を検知する検知軸を有し、角速度に応じた信号を出力する。筐体の垂直方向は、前述のように筐体の上下方向に相当し、前述の図３に示すセンサ座標系のＺＳ軸の延びる方向に一致する。このセンサ座標系のＺＳ軸が、前述の検知軸に相当する。
【０１２６】
角速度センサ１２は、筐体の垂直方向における車両の角速度として、ＺＳ軸まわりの角速度であるヨー方向の角速度（以下「ＺＳ軸のヨーレート」という場合がある）を検出する。具体的には、角速度センサ１２は、予め定める周期毎に、ＺＳ軸のヨーレートを検出する。角速度センサ１２は、検出した角速度をヨー角計測部１５に与える。
【０１２７】
加速度センサ１３は、車両の加速度として、自装置であるナビゲーション装置１の筐体の前後方向における車両の加速度を検知する検知軸を有し、加速度に応じた信号を出力する。前述の図３に示すセンサ座標系のＸＳ軸が、前述の検知軸に相当する。加速度センサ１３は、車両の加速度として、筐体の前後方向に延びるＸＳ軸方向の加速度（以下「ＸＳ軸の前後方向加速度」、または単に「ＸＳ軸の前後加速度」という場合がある）を検出する。加速度センサ１３は、予め定める周期毎に、Ｘｓ軸の前後方向加速度を検出する。加速度センサ１３は、検出した加速度、具体的にはＸｓ軸の前後方向加速度を、加速度計測部１６に与える。
【０１２８】
距離計測部１４は、車両の移動距離を計測する。具体的には、距離計測部１４は、速度センサ１１から与えられるパルス信号に基づいて、車両の移動距離を算出する。より詳細には、距離計測部１４は、予め定めるタイミング毎に速度センサ１１から出力されるパルス信号の数に基づいて、車両の移動距離を算出する。また距離計測部１４は、算出した移動距離に基づいて、車両の速度、および前後方向加速度を算出する。距離計測部１４は、算出した移動距離、速度、および前後方向加速度を、加速度変換部１８、ピッチ角推定部１９および道路照合部２３に与える。
【０１２９】
ヨー角計測部１５は、車両のヨー角を計測する。具体的には、ヨー角計測部１５は、角速度センサ１２から与えられる角速度に基づいて、ＺＳ軸のヨー角を算出する。ヨー角計測部１５は、算出したＺＳ軸のヨー角を、加速度推定部１７、加速度変換部１８および道路照合部２３に与える。
【０１３０】
加速度計測部１６は、加速度センサ１３から与えられる加速度に基づいて、ＸＳ軸の前後方向加速度を計測する。加速度計測部１６は、計測したＸＳ軸の前後方向加速度を、加速度計測値として、加速度変換部１８および取付角検出用履歴作成部２０に与える。
【０１３１】
加速度推定部１７は、距離計測部１４から与えられた移動距離、速度、および前後方向加速度と、ヨー角計測部１５から与えられたＺＳ軸のヨー角とに基づいて、ヨー方向検出角度範囲内について、予め定める角度刻み毎のＸＳ軸加速度を推定する。加速度推定部１７は、推定した予め定める角度刻み毎のＸＳ軸加速度を、加速度推定値として、取付角検出用履歴作成部２０に与える。
【０１３２】
取付角検出用履歴作成部２０は、加速度計測部１６から与えられた１つの加速度計測値と、加速度推定部１７から与えられた複数の加速度推定値とを１組とするＸＳ軸加速度情報を、最新の予め定める組数分だけ格納する。
【０１３３】
ヨー方向取付角検出部２１は、取付角検出用履歴作成部２０から与えられる格納情報に基づいて、ヨー方向取付角θｙを検出する。ヨー方向取付角検出部２１は、検出したヨー方向取付角θｙを加速度変換部１８に与える。
【０１３４】
加速度変換部１８は、加速度計測部１６から与えられたＸＳ軸の加速度計測値と、ＹＳ軸およびＺＳ軸の加速度推定値とから、ＸＢ軸の前後方向加速度を求める。加速度変換部１８は、求めたＸＢ軸の前後方向加速度をピッチ角推定部１９に与える。
【０１３５】
ピッチ角推定部１９は、距離計測部１４から与えられた移動距離、速度、および前後方向加速度と、加速度変換部１８から与えられたＸＢ軸の前後方向加速度とから、坂道走行時などのピッチ角φｐを算出する。ピッチ角推定部１９は、算出したピッチ角φｐをヨー角計測部１５および道路照合部２３に与える。
【０１３６】
地図データ記憶部２２は、たとえばハードディスクドライブ（Hard Disk Drive；略称：ＨＤＤ）装置または半導体メモリなどの記憶装置によって実現される。地図データ記憶部２２は、予め定める範囲内の道路について、地図データを記憶する。地図データは、線形データおよび座標点などで表現される道路リンクなどを含む。
【０１３７】
道路照合部２３は、距離計測部１４から与えられた移動距離、速度および前後方向加速度と、ヨー角計測部１５から与えられたヨー角φｙと、ピッチ角推定部１９から与えられたピッチ角φｐとに基づいて、地図データ記憶部２２から読出した道路リンク上において、車両の位置を同定する。
【０１３８】
以上のように構成される本実施の形態のナビゲーション装置１によれば、ヨー方向取付角θｙを検出することができる。本実施の形態における角速度センサ１２および加速度センサ１３の零点補正、ならびにピッチ方向取付角の検出は、公知の方法、たとえば前述の特許文献４に開示された方法によって行うことができる。
【０１３９】
図４および図５は、本発明の第１の実施の形態のナビゲーション装置１における姿勢・慣性力計測処理の処理手順を示すフローチャートである。姿勢・慣性力計測処理では、車体の姿勢および慣性力の計測に関する処理が行われる。姿勢・慣性力計測処理は、ヨー方向取付角検出処理を含む。図４および図５に示すフローチャートの処理は、予め定める周期毎にと開始され、ステップａ１に移行する。
【０１４０】
以下、図４および図５に示すフローチャートについて、図６〜図１０を参照して、説明する。図６は、ＸＳ軸加速度の推定値の一例を示すグラフである。図６において、横軸は、予め定める周期のセンサ計測数を示し、縦軸は、Ｘｓ軸加速度の推定値［ｍ／ｓ２］を示す。図６において、参照符号「３１」〜「３９」で示されるグラフは、それぞれ、検索角度範囲が、＋４０ｄｅｇ、＋３０ｄｅｇ、＋２０ｄｅｇ、＋１０ｄｅｇ、０ｄｅｇ、−１０ｄｅｇ、−２０ｄｅｇ、−３０ｄｅｇ、−４０ｄｅｇのときの結果を表す。
【０１４１】
図７および図８は、最新の予め定める数分の履歴に格納されたＸＳ軸加速度の推定値および計測値について、予め定める角度刻み毎の差異の標準偏差からヨー方向取付角を検出する方法を示すグラフである。図８は、図７に示す領域４０の拡大図である。図７および図８において、横軸は、Ｘｓ軸加速度の推定値を算出するときの仮のヨー方向取付角［ｄｅｇ］を示し、縦軸は、計測値と推定値との差異の標準偏差［ｍ／ｓ２］を示す。
【０１４２】
図９は、ＸＳ軸加速度の計測値および推定値を示すグラフである。図９において、横軸は、予め定める周期のセンサ計測数を示し、縦軸は、Ｘｓ軸加速度の計測値［ｍ／ｓ２］および推定値［ｍ／ｓ２］を示す。図９において、参照符号「４１」で示されるグラフは、Ｘｓ軸加速度の計測値を示し、参照符号「４２」で示されるグラフは、Ｘｓ軸加速度の推定値を示す。
【０１４３】
図１０は、ＸＳ軸加速度の計測値から変換したＸＢ軸加速度、および速度センサによるＸＢ軸加速度の一例を示すグラフである。図１０において、横軸は、予め定める周期のセンサ計測数を示し、縦軸は、Ｘｓ軸加速度の計測値［ｍ／ｓ２］、ＸＢ軸加速度の変換値［ｍ／ｓ２］、およびパルス信号による前後加速度［ｍ／ｓ２］を示す。図１０において、参照符号「４３」で示されるグラフは、Ｘｓ軸加速度の計測値を示し、参照符号「４４」で示されるグラフは、ＸＢ軸加速度の変換値を示し、参照符号「４５」で示されるグラフは、パルス信号による前後加速度を示す。
【０１４４】
ステップａ１において、道路照合部２３は、初期化が必要か否かを判断する。ステップａ１において初期化が必要であると判断された場合は、ステップａ２に移行し、初期化が必要でないと判断された場合は、ステップａ３に移行する。
【０１４５】
ステップａ２において、道路照合部２３は、姿勢・慣性力計測処理を初期化する。具体的には、姿勢・慣性力計測処理に関係する計測値、推定値および算出値などを初期の値として設定される値に戻す。初期化すると、ステップａ３に移行する。
【０１４６】
ステップａ３において、距離計測部１４は、移動距離、速度および前後方向加速度を計測する。具体的には、距離計測部１４は、本計測周期で計測したパルス信号数に、距離に換算する係数を乗算して、移動距離を算出するとともに、移動距離の微分から速度を算出し、速度の微分から前後方向加速度を算出する。移動距離、速度および前後方向加速度を計測すると、ステップａ４に移行する。
【０１４７】
ステップａ４において、ヨー角計測部１５は、ヨー角を計測する。具体的には、ヨー角計測部１５は、角速度センサ１２が出力したＺＳ軸角速度から別途検出された角速度零点を減算して、センサ座標系のヨー角を算出する。ヨー角を計測すると、ステップａ５に移行する。
【０１４８】
ステップａ５において、加速度計測部１６は、加速度を計測する。具体的には、加速度計測部１６は、加速度センサ１３が出力したＸＳ軸加速度をセンサ座標系の前後方向加速度として計測する。加速度を計測すると、図５のステップａ６に移行する。
【０１４９】
図５のステップａ６〜ステップａ１０は、ヨー方向取付角の検出を行う処理である。ステップａ６において、加速度推定部１７は、ピッチ方向取付角が検出されたか否かを判断する。ステップａ６においてピッチ方向取付角が検出されていると判断された場合は、ステップａ７に移行し、ピッチ方向取付角が検出されていないと判断された場合は、ピッチ角を算出することができないので、全ての処理手順を終了する。
【０１５０】
ステップａ７において、加速度推定部１７は、加速度を推定する。具体的には、加速度推定部１７は、図６に示すように、ヨー方向取付角の検索角度範囲について、距離計測部１４によって計測された速度と前後加速度、およびヨー角計測部１５によって計測されたヨー角を用いて、予め定める角度刻み毎にその角度とピッチ方向取付角に基づいたＸＳ軸加速度ＡＳ−ＥＳＴｉを、以下に示す式（３４）と式（３５）とを用いて推定する。ステップａ７において加速度が推定されると、ステップａ８に移行する。
【０１５１】
【数３４】
2015004593.tif 000036
【０１５２】
式（３４）において、ＡＳＰＤは距離計測部１４によって計測された前後加速度[ｍ／ｓ２]であり、ＡＢＹ−ＥＳＴは式（３０）で求めた左右加速度[ｍ／ｓ２]であり、Ｇは重力加速度（定数）[ｍ／ｓ２]である。
【０１５３】
【数３５】
2015004593.tif 000037
【０１５４】
式（３５）において、ＣＢＳで用いる各取付角は、ピッチ方向取付角θｐ、ヨー方向取付角θｙ、およびロール方向取付角θｒである。ピッチ方向取付角θｐは、別途設定された角度である。ヨー方向取付角θｙは、ヨー方向取付角の角度範囲内の指定角度ｉであり、予め定める角度刻み間隔に指定される。ロール方向取付角θｒはゼロ（０）である。
【０１５５】
ステップａ８において、取付角検出用履歴作成部２０は、履歴を作成する。具体的には、取付角検出用履歴作成部２０は、予め定める角度旋回する毎に、複数の加速度推定値と１つの加速度計測値とを１組とするＸＳ軸加速度情報を最新の予め定める組数分だけ格納する。目安として、右折あるいは左折が１回行われたときに、予め定める組数分の履歴を作成できるように、履歴作成タイミングと履歴の大きさを調整する。ステップａ８において履歴が作成されると、ステップａ９に移行する。
【０１５６】
ステップａ９において、取付角検出用履歴作成部２０は、作成された履歴の数が、予め定める数以上か否かを判断する。ステップａ９において、履歴格納数が予め定める数以上である場合は、ステップａ１０に移行し、履歴格納数が予め定める数未満である場合は、ヨー方向取付角の新たな検出はできないので、ステップａ１１に移行する。
【０１５７】
ステップａ１０において、ヨー方向取付角検出部２１は、ヨー方向取付角の検出処理を行う。具体的には、ヨー方向取付角検出部２１は、図７および図８に示すように、ＸＳ軸について、加速度の推定値を計算した角度毎に、加速度の推定値と加速度の計測値との差異の標準偏差を求め、差異の標準偏差が極小σｍｉｎとなる加速度の推定値の計算に用いた角度をヨー方向取付角として採用する。
【０１５８】
図７および図８に示すように、ヨー方向取付角が検出されていない場合は、加速度の推定値と加速度の計測値との差異の標準偏差を５°刻み毎に計算する。標準偏差が最小となる角度に対して、その左右の標準偏差が大きくなる場合は、ヨー方向取付角が検出された、すなわち検出済みと判断するが、左右どちらかの標準偏差のみが大きくなる場合は、ヨー方向取付角が検出されていない、すなわち未検出のままとする。
【０１５９】
また、ヨー方向取付角が検出済みの場合は、検出済みのヨー方向取付角を中心とする±５°範囲について、加速度の推定値と加速度の計測値との差異の標準偏差を１°刻み毎に計算し、前述と同様にしてヨー方向取付角を検出し、予め定める方法で更新する。ステップａ１０においてヨー方向取付角の検出処理が行われると、ステップａ１１に移行する。
【０１６０】
ステップａ１１において、加速度変換部１８は、ヨー方向取付角が検出されたか否かを判断する。ステップａ１１において、ヨー方向取付角が検出されていると判断された場合は、ピッチ角を推定するために、ステップａ１２に移行し、ヨー方向取付角が検出されていないと判断された場合は、ピッチ角を推定することができないので、全ての処理手順を終了する。
【０１６１】
ステップａ１２において、加速度変換部１８は、加速度を変換する。具体的には、加速度変換部１８は、別途設定されたピッチ方向取付角、およびヨー方向取付角検出部２１によって検出されたヨー方向取付角に基づいて、式（３４）と式（３５）とで推定したＹＳ軸、ＺＳ軸の加速度推定値、ＸＳ軸の加速度計測値から、以下に示す式（３６）と式（３７）とによって、図１０に示すＸＢ軸加速度を算出する。
【０１６２】
【数３６】
2015004593.tif 000038
【０１６３】
式（３６）において、ＡＳＸは加速度センサ１３によって計測される前後加速度計測値[ｍ／ｓ２]であり、ＡＳＹ−ＥＳＴは式（３５）で求めた左右方向加速度[ｍ／ｓ２]であり、ＡＳＺ−ＥＳＴは式（３５）で求めた垂直方向加速度[ｍ／ｓ２]である。
【０１６４】
【数３７】
2015004593.tif 000039
【０１６５】
式（３７）において、ＣＳＢで用いる各取付角は、ピッチ方向取付角θｐ、ヨー方向取付角θｙ、およびロール方向取付角θｒである。ピッチ方向取付角θｐは、別途設定されたピッチ方向取付角であり、ヨー方向取付角θｙは、ステップａ１０で検出されたヨー方向取付角であり、ロール方向取付角θｒはゼロ（０）である。
【０１６６】
ステップａ１３において、ピッチ角推定部１９は、ピッチ角を推定する。具体的には、ピッチ角推定部１９は、加速度変換処理８のＸＢ軸加速度変換値ＡＢＸと、距離計測部１４の前後加速度ＡＳＰＤとから、以下に示す式（３８）でピッチ角を算出することによって、ピッチ角を推定する。ステップａ１３においてピッチ角を推定すると、全ての処理手順を終了する。
【０１６７】
【数３８】
2015004593.tif 000040
【０１６８】
本実施の形態では、ＸＳ軸のみの１軸加速度センサを備えた場合のヨー方向取付角検出方法の一例を説明したが、このような構成に限定されない。図１１は、ＹＳ軸加速度の計測値および推定値を示すグラフである。図１２は、ＺＳ軸加速度の計測値および推定値を示すグラフである。
【０１６９】
図１１において、横軸は、予め定める周期のセンサ計測数を示し、縦軸は、Ｙｓ軸加速度の計測値［ｍ／ｓ２］および推定値［ｍ／ｓ２］を示す。図１１において、参照符号「４６」で示されるグラフは、Ｙｓ軸加速度の計測値を示し、参照符号「４７」で示されるグラフは、Ｙｓ軸加速度の推定値を示す。
【０１７０】
図１２において、横軸は、予め定める周期のセンサ計測数を示し、縦軸は、Ｚｓ軸加速度の計測値［ｍ／ｓ２］および推定値［ｍ／ｓ２］を示す。図１２において、参照符号「４８」で示されるグラフは、Ｚｓ軸加速度の計測値を示し、参照符号「４９」で示されるグラフは、Ｚｓ軸加速度の推定値を示す。
【０１７１】
Ｘｓ軸に加えて、ＹＳ軸もしくはＺＳ軸もある２軸、またはＹＳ軸およびＺＳ軸もある３軸の加速度センサを備える場合には、図１１に示すＹＳ軸加速度の計測値および推定値の一例、および図１２に示すＺＳ軸加速度の計測値および推定値の一例のように、ＸＳ軸と同様に、ＹＳ軸およびＺＳ軸の加速度をそれぞれ推定することができる。したがって、ＸＳ軸以外の軸を用いて、ヨー方向取付角を検出してもよいし、また、備えてある軸全てを用いて、ヨー方向取付角を検出してもよい。
【０１７２】
また、本実施の形態では、ヨー方向取付角度全範囲について、５°刻みでヨー方向取付角を粗く検出してから、検出済みの角度を中心とする予め定めるヨー方向取付角度範囲について、１°刻みでヨー方向取付角を詳細に検出する。これに限定されず、ヨー方向取付角度全範囲の端から予め定めるヨー方向取付角度範囲について、最初から１°刻みで検出してもよい。また、検出分解能をさらに上げてもよい。
【０１７３】
また、本実施の形態では、右折あるいは左折が１回行われたときに、予め定める組数分の取付角検出用履歴を作成できるように履歴作成タイミングと履歴の大きさとを調整する。これに限定されず、ヨー方向取付角の検出状況に応じて調整してもよい。さらには、ヨー方向取付角の検出精度が低下するリスクが高くなる以下のケース（Ａ），（Ｂ）では、履歴を初期化してもよい。
【０１７４】
（Ａ）ヨー方向取付角の検索角度範囲内で極小が２つ以上確認されたとき。
【０１７５】
（Ｂ）ピッチ角が予め定める角度以上になったとき。
【０１７６】
また、本実施の形態では、オイラー角（ロール、ピッチ、ヨー）による前述の式（１）〜式（４）の座標変換行列を用いたが、四元数による座標変換を行ってもよい。
【０１７７】
以上のように本実施の形態によれば、取付角検出用履歴作成部２０によって最新の予め定める組数分の履歴が作成され、作成された各組の履歴に基づいて、ヨー方向取付角検出部２１によってヨー方向取付角θｙが検出される。これによって、検知軸が１軸の加速度センサ１３と、検知軸が１軸の角速度センサ１２とを用いて、ヨー方向取付角θｙを検出することができる。特に本実施の形態では、ピッチ方向取付角θｐおよびヨー方向取付角θｙがともに４５°以下の範囲において、ヨー方向取付角θｙを検出することができる。
【０１７８】
また本実施の形態では、ヨー方向取付角θｙの検索角度範囲において、加速度推定値と加速度計測値との差異の標準偏差が極小となる角度をヨー方向取付角θｙとするので、約１°相当の加速度センサの零点誤差、および温度変動（ドリフト）の影響を受け難い。また、緩やかな道路傾斜があっても、ヨー方向取付角θｙを検出することができる。
【０１７９】
このように本実施の形態では、ヨー方向取付角θｙを検出することができるので、ナビゲーション装置１の筐体が、ピッチ方向およびヨー方向に傾斜して車体に取付けられた場合であっても、各センサの出力に作用する他軸の影響を補正して、姿勢角および慣性力などを高精度に計測することができる。
【０１８０】
特に本実施の形態では、取付角検出用履歴作成部２０は、加速度推定部１７によって推定された複数の加速度推定値と、加速度計測部１６によって計測された１つの加速度計測値とを１組として、最新の予め定める組数分の履歴を作成する。この履歴に基づいて、ヨー方向取付角検出部２１によって、ヨー方向取付角θｙが検出される。したがって、ヨー方向取付角θｙを検出可能なナビゲーション装置１を簡単な構成で実現することができる。
【０１８１】
また本実施の形態では、加速度推定部１７は、距離計測部１４によって計測された速度および前後方向加速度と、記ヨー角計測部１５によって計測されたヨー角と、重力加速度とを用いて、ピッチ方向およびヨー方向における筐体の取付角として選択された取付角における前後方向加速度を推定する。これによって、ほぼ平坦な場所に限定すれば、速度センサ１１と角速度センサ１２とを用いて、任意の取付角度における３軸加速度を推定することができる。したがって、任意軸の加速度について、加速度計測値に合う加速度推定値を見つけることによって、その加速度推定値の算出に用いた角度からヨー方向取付角θｙを検出することができる。
【０１８２】
また本実施の形態では、ヨー方向取付角検出部２１は、ヨー方向取付角の検索角度範囲内で、加速度推定値と加速度計測値との差異の標準偏差が、取付角が大きくなる方向に行くのに従って小さくなるか、または取付角が小さくなる方向に行くのに従って小さくなる場合、標準偏差が小さくなる方向に検索角度範囲を移動する。またヨー方向取付角検出部２１は、検索角度範囲内の両端以外で標準偏差が最小となる角度があり、その最小となる角度よりも取付角が大きくなる方向および小さくなる方向の両方に行くのに従って、標準偏差が大きくなる場合は、その最小となる角度を極小と判断する。
【０１８３】
これによって、ヨー方向取付角の検索範囲について、予め定める角度刻み毎の加速度推定値と加速度計測値との差異の標準偏差の傾向からヨー方向取付角θｙがある方向が分かるので、自動的に、すなわち使用者が操作することなく、ヨー方向取付角の検索範囲を探し、特定することができる。
【０１８４】
また本実施の形態では、加速度推定部１７は、ヨー方向取付角が検出されていない場合、予め定める角度よりも大きい角度毎に加速度を推定し、ヨー方向取付角が検出された場合は、検出されたヨー方向取付角を中心とする検索角度範囲について、予め定める角度よりも小さい角度毎に加速度を推定する。
【０１８５】
これによって、ヨー方向取付角θｙの検出状況に応じて検索範囲と検索分解能とを変更することができるので、初期の検出を迅速に行うことができる。また検出回数が上がる毎に段階的に検出分解能を小さくすることができる。また、検出処理の負荷を軽減することができる。
【０１８６】
また本実施の形態では、加速度推定部１７は、ナビゲーション装置１の筐体の前後方向、左右方向および垂直方向の３軸の加速度を推定する。そして、ピッチ角推定部１９は、加速度推定部１７によって推定された３軸の加速度の推定値と、距離計測部１４によって計測された速度および前後方向加速度と、ヨー角計測部１５によって計測されたヨー角と、ピッチ方向取付角とを用いて、各軸の取付角が全て０°の場合の前後方向加速度を推定するとともに、３軸の加速度の推定値からピッチ角を推定する。
【０１８７】
このように構成されることによって、速度センサ１１と角速度センサ１２とを用いて、任意の取付角度における３軸加速度を推定することができるので、左右方向加速度の影響を受けない前後方向加速度に変換して、正確なピッチ角φｐを算出することができる。これによって、水平面のヨー角φｙをより正確に計算することができる。したがって、ヨー方向取付角θｙに起因する車両の位置の検出精度の低下、および誤マッチングを回避することができる。
【０１８８】
また本実施の形態では、取付角検出用履歴作成部２０は、車両が予め定める角度以上旋回する毎に履歴を作成し、車両の左折または右折が１回行われた場合に、履歴の予め定める組数分が全て格納される。これによって、１回の右左折からヨー方向取付角θｙを検出することができるので、初期の検出を迅速に行うことができる。また右左折する毎にヨー方向取付角θｙの検出精度を高めることができる。したがって、車両の位置の検出精度を迅速に良好な状態にすることができる。
【０１８９】
また本実施の形態では、取付角検出用履歴作成部２０は、ヨー方向取付角検出部２１によって、ヨー方向検索範囲内に極小が２つ以上存在すると判断された場合、およびピッチ角推定部１９によって推定されたピッチ角が予め定める値よりも大きくなった場合の少なくとも一方の場合には、履歴を消去する。これによって、履歴からヨー方向取付角θｙを正しく検出できない状況であることが分かるので、検出精度が低下する状況では、ヨー方向取付角θｙが検出されないようにすることができる。したがって、ヨー方向取付角θｙの誤検出を低減することができる。
【０１９０】
また本実施の形態では、加速度センサ１３が２軸以上の加速度センサである場合、たとえば、加速度センサ１３が、前後方向加速度とともに、ナビゲーション装置１の筐体の左右方向の加速度、および垂直方向の加速度の少なくとも一方を計測する２軸または３軸の加速度センサである場合には、ヨー方向取付角検出部２１は、検知軸毎に検出したヨー方向取付角θｙが一致するときに、そのヨー方向取付角θｙを採用する。
【０１９１】
すなわち、ヨー方向取付角検出部２１は、前後方向加速度を計測可能な検知軸について、ヨー方向取付角を検出するとともに、加速度計測値と加速度推定値との差異が予め定める値以下であり、各方向で検出された角度が一致するとき、その角度をヨー方向取付角として求める。これによって、ヨー方向取付角θｙの信頼性が向上するので、より高精度にヨー方向取付角θｙを検出することができる。
【０１９２】
また、ヨー方向取付角θｙが既知のときに、推定値と計測値との差異が大きくなる場合には、坂道を走行中と判断できるので、ヨー方向取付角θｙを検出した後は、水平状態であるか否かをより正確に判断することができる。これによって、検出する度に、ヨー方向取付角θｙを、より正確な値に更新することができる。
【０１９３】
＜第２の実施の形態＞
図１３は、本発明の第２の実施の形態であるナビゲーション装置２の構成を示すブロック図である。ナビゲーション装置２は、前述の第１の実施の形態のナビゲーション装置１と同様に、速度センサ１１、角速度センサ１２、加速度センサ１３、距離計測部１４、ヨー角計測部１５、加速度計測部１６、加速度推定部１７、加速度変換部１８、ピッチ角推定部１９、取付角検出用履歴作成部２０、ヨー方向取付角検出部２１、地図データ記憶部２２および道路照合部２３を備えて構成される。
【０１９４】
本実施の形態のナビゲーション装置２は、第１の実施の形態のナビゲーション装置１と同一の構成であるが、ヨー方向取付角検出に係わる加速度推定部１７、取付角検出用履歴作成部２０およびヨー方向取付角検出部２１の動作が異なる。したがって、本実施の形態では、第１の実施の形態と同一の部分については説明を省略し、異なる部分について説明する。
【０１９５】
取付角検出用履歴作成部２０は、加速度計測部１６によって計測された加速度計測値と、距離計測部１４によって計測された速度および前後方向加速度と、ヨー角計測部１５によって計測されたヨー角とを１組として、最新の予め定める組数分の履歴を作成する。
【０１９６】
取付角検出用履歴作成部２０によって履歴を作成した後は、加速度推定部１７は、ヨー方向取付角検出部２１から指定された推定条件に従って、ヨー方向取付角の検索角度範囲について、履歴各組の格納情報を用いて、予め定める角度刻み毎に、その角度とピッチ方向取付角に対応した任意検知軸の加速度を推定する。推定条件は、検索角度範囲、検索分解能および検知軸などである。
【０１９７】
ヨー方向取付角検出部２１は、ヨー方向取付角の検出状況に応じて、加速度推定条件を加速度推定部１７に与えるとともに、加速度推定部１７の加速度推定値を計算した角度毎に、加速度推定値と加速度計測値との差異の標準偏差を求め、前記差異の標準偏差が極小となる加速度推定値の計算に用いた角度をヨー方向取付角とする。
【０１９８】
図１４および図１５は、本発明の第２の実施の形態であるナビゲーション装置２における姿勢・慣性力計測処理の処理手順を示すフローチャートである。図１４および図１５に示すフローチャートの処理は、予め定める周期毎に開始され、ステップｂ１に移行する。
【０１９９】
図１４および図１５に示すフローチャートのステップｂ１〜ステップｂ５およびステップｂ１１〜ステップｂ１３の処理は、前述の図４および図５に示すフローチャートのステップａ１〜ステップａ５およびステップａ１１〜ステップａ１３の処理と同一であるので、説明を省略する。図１４のステップｂ５において加速度が計測されると、図１５のステップｂ６に移行する。
【０２００】
ステップｂ６〜ステップｂ１０は、ヨー方向取付角の検出を行う処理である。ステップｂ６において、取付角検出用履歴作成部２０は、履歴を作成する。
【０２０１】
具体的には、取付角検出用履歴作成部２０は、予め定める角度旋回する毎に、加速度計測部１６によって計測された加速度計測値と、距離計測部１４によって計測された速度および前後方向加速度と、ヨー角計測部１５によって計測されたヨー角とを１組として、最新の予め定める組数分の履歴を作成する。目安として、右折あるいは左折が１回行われたときに、予め定める組数分の履歴を作成できるように、履歴作成タイミングと履歴の大きさを調整する。ステップｂ６において履歴が作成されると、ステップｂ７に移行する。
【０２０２】
ステップｂ７において、加速度推定部１７は、ピッチ方向取付角が検出されたか否かを判断する。ステップｂ７において、ピッチ方向取付角が検出されていると判断された場合は、ステップｂ８に移行し、ピッチ方向取付角が検出されていないと判断された場合は、ピッチ角を算出することができないので、全ての処理手順を終了する。
【０２０３】
ステップｂ８において、取付角検出用履歴作成部２０は、作成された履歴の数が、予め定める数以上か否かを判断する。ステップｂ８において、履歴格納数が予め定める数以上であると判断された場合は、ステップｂ９に移行し、履歴格納数が予め定める数未満であると判断された場合は、ヨー方向取付角の新たな検出はできないので、ステップｂ１１に移行する。
【０２０４】
ステップｂ９において、加速度推定部１７は、加速度を推定する。具体的には、加速度推定部１７は、ヨー方向取付角検出部２１から指定された推定条件に従って、ヨー方向取付角の検索角度範囲について履歴各組の格納情報を用いて、予め定める角度刻み毎にその角度とピッチ方向取付角に対応した任意検知軸の加速度を図６に示すように推定する。ステップｂ９において加速度が推定されると、ステップｂ１０に移行する。
【０２０５】
ステップｂ１０において、ヨー方向取付角検出部２１は、ヨー方向取付角の検出処理を行う。具体的には、ヨー方向取付角検出部２１は、ヨー方向取付角の検出状況に応じて、加速度推定条件を加速度推定部１７に与え、加速度推定部１７によって推定された加速度推定値を計算した角度毎に、加速度推定値と加速度計測値との差異の標準偏差を取得する。そして、図７および図８に示すように、ＸＳ軸について差異の標準偏差が極小となる加速度推定値の計算に用いた角度をヨー方向取付角として採用する。
【０２０６】
ステップｂ１０においてヨー方向取付角の検出処理が行われると、ステップｂ１１に移行する。そして、ステップｂ１１〜ステップｂ１３において、前述の図４および図５に示すフローチャートのステップａ１１〜ステップａ１３と同様の処理を行う。
【０２０７】
本実施の形態では、前述の第１の実施の形態と同様に、次のように行ってもよい。たとえば、ＹＳ軸もしくはＺＳ軸もある２軸、またはＹＳ軸およびＺＳ軸もある３軸の加速度センサを備える場合には、ＸＳ軸以外の軸を用いて、ヨー方向取付角を検出してもよいし、備えてある軸全てを用いて、ヨー方向取付角を検出してもよい。
【０２０８】
また、ヨー方向取付角度全範囲について、ヨー方向取付角を粗く検出してから、検出済みの角度を中心とする予め定めるヨー方向取付角度範囲について、ヨー方向取付角を詳細に検出してもよいし、ヨー方向取付角度全範囲の端から予め定めるヨー方向取付角度範囲について、最初から詳細に検出してもよい。
【０２０９】
また、ヨー方向取付角の検出精度が低下するリスクが高くなる以下のケース（Ａ），（Ｂ）では、履歴を初期化してもよい。
【０２１０】
（Ａ）ヨー方向取付角の検索角度範囲内で極小が２つ以上確認されたとき。
【０２１１】
（Ｂ）ピッチ角が予め定める角度以上になったとき。
【０２１２】
また、本実施の形態では、オイラー角による前述の式（１）〜式（４）の座標変換行列を用いたが、四元数による座標変換を行ってもよい。
【０２１３】
以上に述べた第２の実施の形態によれば、第１の実施の形態と同様の効果を得ることができる。さらに、本実施の形態では、取付角検出用履歴を１回作成すれば、ヨー方向取付角θｙの検出角度範囲および検索角度の分解能を何度も変えて、ヨー方向取付角θｙを再計算することが可能である。したがって、何度も右左折することなく、ヨー方向取付角θｙを高分解能で迅速に検出することができる。
【０２１４】
以上に述べた第１および第２の実施の形態のナビゲーション装置１，２は、車両に搭載される車載用のナビゲーション装置、すなわちカーナビゲーション装置であるが、車両に搭載可能な、ＰＮＤ（Portable Navigation Device）、または、携帯端末装置たとえば携帯電話機、スマートフォンもしくはタブレット型端末装置にも適用することができる。
【０２１５】
なお、本発明は、その発明の範囲内において、各実施の形態を自由に組み合わせることが可能である。また、各実施の形態の任意の構成要素を適宜、変更または省略することが可能である。
【符号の説明】
【０２１６】
１，２  ナビゲーション装置、１１  速度センサ、１２  角速度センサ、１３  加速度センサ、１４  距離計測部、１５  ヨー角計測部、１６  加速度計測部、１７  加速度推定部、１８  加速度変換部、１９  ピッチ角推定部、２０  取付角検出用履歴作成部、２１  ヨー方向取付角検出部、２２  地図データ記憶部、２３  道路照合部。
【図１】
2015004593.tif 000041
【図２】
2015004593.tif 000042
【図３】
2015004593.tif 000043
【図４】
2015004593.tif 000044
【図５】
2015004593.tif 000045
【図６】
2015004593.tif 000046
【図７】
2015004593.tif 000047
【図８】
2015004593.tif 000048
【図１３】
2015004593.tif 000049
【図１４】
2015004593.tif 000050
【図１５】
2015004593.tif 000051
【図１６】
2015004593.tif 000052
【図１７】
2015004593.tif 000053
【図１８】
2015004593.tif 000054
【図１９】
2015004593.tif 000055
【図２０】
2015004593.tif 000056
【図２１】
2015004593.tif 000057
【図２２】
2015004593.tif 000058
【図９】
2015004593.tif 000059
【図１０】
2015004593.tif 000060
【図１１】
2015004593.tif 000061
【図１２】
2015004593.tif 000062