姿勢判定方法、位置算出方法及び姿勢判定装置
【課題】移動体に対するセンサーの姿勢を判定するための新たな手法を提供する。
【解決手段】移動体に設置されたセンサー３により移動ベクトルが計測される。そして、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢が判定され、移動体が停止している場合に、移動体の速度に関する制約条件を用いて、センサー３の計測結果が補償される。
【解決手段】移動体に設置されたセンサー３により移動ベクトルが計測される。そして、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢が判定され、移動体が停止している場合に、移動体の速度に関する制約条件を用いて、センサー３の計測結果が補償される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に対するセンサーの姿勢を判定する方法等に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、センサーの活用が注目されている。センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。センサーの計測結果を利用して慣性航法演算を行って移動体（例えば自転車や自動車、電車、船、飛行機など）の位置算出を行う技術も考案されている。
【０００３】
慣性航法演算においては、センサーの計測結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、移動体の移動方向に関する制約条件を利用して、カルマンフィルターを適用して慣性航法演算結果を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１０／００１９９６３号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の技術は、センサーの計測結果には誤差が含まれることを前提とし、移動体の移動方向に関する制約条件を利用して、慣性航法演算結果である移動体の位置や速度といった諸量に含まれる誤差を推定するものである。但し、この技術は、センサーの座標系と移動体の座標系との正確な対応関係を把握していることが前提である。
【０００６】
センサーの計測結果はセンサーの座標系（ローカル座標系）での値である。移動体の座標系での値ではない。従って、移動体の移動方向に関する制約条件を利用する場合には、センサーの座標系と移動体の座標系との間で座標変換を行うか、或いは、センサーの座標系と移動体の座標系とがぴったり一致するようにセンサーを移動体に固定する必要がある。しかし、キャリブレーションを行うとしても、センサーの座標系から移動体の座標系に正確に変換することは困難であるし、センサーの座標系と移動体の座標系とがぴったり一致するようにセンサーを移動体に固定するのも困難である。僅かではあるが誤差が発生し得るし、時間が経過すると誤差が大きくなり得る。
【０００７】
また、慣性航法用のセンサーを備えた携帯型電話機やナビゲーション装置、ＰＤＡ（Personal Digital Assistant）、パソコンといった持ち運び可能な電子機器を移動体に持ち込んで配置し、その電子機器で慣性航法演算を行う場合には、センサーの座標系と移動体の座標系との対応関係を正確に把握することは至難である。
【０００８】
センサーの座標系と移動体の座標系との間で正確な座標変換ができない場合は、センサーの座標系と移動体の座標系との間で辻褄が合わなくなるため、慣性航法演算結果に誤差が生じる。また、慣性航法演算を継続するにつれて、実際の位置と演算した位置との乖離が徐々に大きくなっていく傾向があった。
【０００９】
センサーの座標系と移動体の座標系との正確な対応関係を把握するためには、移動体に対するセンサーの正確な姿勢を判定する必要がある。
【００１０】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体に対するセンサーの姿勢を判定するための新たな手法を提案することにある。
【課題を解決するための手段】
【００１１】
以上の課題を解決するための第１の形態は、移動体に設置された移動ベクトルを計測するセンサーが前記移動ベクトルを計測することと、前記移動体が移動を開始した際に前記センサーが計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定することと、を含む姿勢判定方法である。
【００１２】
また、他の形態として、移動体に設置された移動ベクトルを計測するセンサーが前記移動体が移動を開始した際に計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定する姿勢判定装置を構成してもよい。
【００１３】
この第１の形態等によれば、移動体が移動を開始した際にセンサーが計測した移動ベクトルを用いて、移動体に対するセンサーの姿勢を判定する。移動ベクトルは、移動体の加速度ベクトルや速度ベクトルといった、移動体の移動に関するベクトルのことを意味する。また、移動体に対するセンサーの姿勢とは、移動体に対するセンサーの相対的な姿勢のことを意味する。実施形態で詳細に後述するが、移動体が移動を開始した際にセンサーにより計測された移動ベクトルを用いることで、移動体に対するセンサーの姿勢を判定することができる。
【００１４】
また、第２の形態として、第１の形態の姿勢判定方法であって、前記移動体が停止している場合に、前記移動体の速度に関する第１の制約条件を用いて、前記センサーの計測結果を補償すること、を更に含む姿勢判定方法を構成してもよい。
【００１５】
この第２の形態によれば、移動体が停止している場合に、移動体の速度に関する第１の制約条件を用いて、センサーの計測結果を補償する。これにより、第１の形態と相まって、移動体が停止している場合に補償された移動ベクトルを用いてセンサーの姿勢を判定することができるようになり、姿勢判定をより精確に行うことが可能となる。
【００１６】
また、第３の形態として、第１又は第２の形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体に対する前記姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することである、姿勢判定方法を構成してもよい。
【００１７】
この第３の形態によれば、移動体に対する姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することで、センサーの姿勢判定を簡易に行うことができる。
【００１８】
また、第４の形態として、第１〜第３の何れかの形態の姿勢判定方法であって、前記移動ベクトルを計測することは、前記移動体の移動空間を定める座標系である絶対座標系での移動ベクトルを計測することであり、前記姿勢を判定することは、前記移動ベクトルと、前記絶対座標系で表した前記センサーの向きとを用いて、前記センサーの前記姿勢を判定することである、姿勢判定方法を構成してもよい。
【００１９】
この第４の形態によれば、移動体の移動空間を定める絶対座標系での移動ベクトルと、絶対座標系で表したセンサーの向きとを用いて、移動体に対するセンサーの姿勢を判定する。絶対座標系で表したセンサーの向きが分かれば、絶対座標系での移動ベクトルを、センサーの座標系に座標変換することができる。そして、変換後の移動ベクトルを用いることで、センサーの姿勢を適切に判定することができる。
【００２０】
また、第５の形態として、第１〜第３の何れかの形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することと、前記速度ベクトルを用いて前記センサーの姿勢を判定することと、を含む姿勢判定方法を構成してもよい。
【００２１】
この第５の形態によれば、前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することで、前記センサーのバイアス誤差等を取り除いた速度ベクトルを算出することが可能となる。
【００２２】
また、第６の形態として、第１〜第５の何れかの形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体の停止が解除されて前記移動体の速度又は加速度が所定の閾値に達した時の前記移動ベクトルを用いて前記姿勢を判定することである、姿勢判定方法を構成してもよい。
【００２３】
この第６の形態によれば、移動体の停止が解除されて移動体の速度又は加速度が所定の閾値に達した時の移動ベクトルを用いて姿勢を判定する。移動体の停止が解除されてもすぐには姿勢判定を行わず、移動体の速度又は加速度がある程度の大きさとなった時点で姿勢判定を行うようにすることで、姿勢判定の正確性を向上させることができる。
【００２４】
また、第７の形態として、第１〜第６の何れかの形態の姿勢判定方法を用いて前記姿勢を判定することと、前記姿勢と前記移動ベクトルとを用いて前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【００２５】
この第７の形態によれば、上記の形態の姿勢判定方法を用いて判定した姿勢と移動ベクトルとを用いることで、移動体の位置を適切に算出することができる。
【００２６】
また、第８の形態として、第７の形態の位置算出方法であって、前記位置を算出することは、前記移動ベクトルを用いて前記絶対座標系での前記移動体の絶対座標位置を算出することと、前記姿勢と前記絶対座標系で表した前記センサーの向きとを用いて、前記絶対座標位置を前記移動体を基準とする移動体座標位置に座標変換することと、前記移動体座標位置と、前記移動体の移動方向に関する第２の制約条件とを用いて、前記絶対座標位置を補正することと、を含む、位置算出方法を構成してもよい。
【００２７】
この第８の形態によれば、移動ベクトルを用いて絶対座標系での移動体の絶対座標位置を算出する。例えば、絶対座標系での移動ベクトルを積分することで、移動体の位置を算出する。絶対座標系と移動体を基準とする座標系とを結び付けるためには、移動体に対するセンサーの相対的な姿勢と、絶対座標系で表したセンサーの向きとが必要となる。そこで、上記の形態の姿勢判定方法を用いて判定したセンサーの姿勢と、絶対座標系で表したセンサーの向きとを用いて、絶対座標位置を移動体座標位置に座標変換する。これにより、移動体を基準とする座標系での演算が可能となる。その上で、移動体座標位置と移動体の移動方向に関する第２の制約条件とを用いることで、移動体の絶対座標位置を適切に補正することができる。
【図面の簡単な説明】
【００２８】
【図１】全体システムのシステム構成図。
【図２】第１の取付姿勢判定システムのシステム構成図。
【図３】第２の取付姿勢判定システムのシステム構成図。
【図４】第１の位置算出システムのシステム構成図。
【図５】第１の慣性航法システムのシステム構成図。
【図６】第２の位置算出システムのシステム構成図。
【図７】第２の慣性航法システムのシステム構成図。
【図８】従来の手法で位置算出を行ったシミュレーション結果の一例。
【図９】実施形態の手法で位置算出を行ったシミュレーション結果の一例。
【図１０】カーナビゲーション装置の機能構成を示すブロック図。
【図１１】処理部を機能ブロックとして表した概念図。
【図１２】ナビゲーション処理の流れを示すフローチャート。
【図１３】補正処理の流れを示すフローチャート。
【図１４】従来のナビゲーション処理による位置算出の実験結果の一例。
【図１５】実施例のナビゲーション処理による位置算出の実験結果の一例。
【図１６】第２のナビゲーション処理の流れを示すフローチャート。
【図１７】進行方向切替対応ロジックを適用しない場合の実験結果の一例。
【図１８】進行方向切替対応ロジックを適用した場合の実験結果の一例。
【図１９】変形例における処理部を機能ブロックとして表した概念図。
【図２０】変形例における処理部を機能ブロックとして表した概念図。
【図２１】第２の取付姿勢判定処理の流れを示すフローチャート。
【発明を実施するための形態】
【００２９】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００３０】
１．原理
１−１．全体システム
図１は、本実施形態における姿勢判定方法及び位置算出方法に係る全体システムのシステム構成の一例を示す図である。全体システムは、センサー３が搭載された移動体を有するシステムとして構成される。移動体は、自転車や自動車（四輪自動車及びバイクを含む）、電車、飛行機、船等の何れでもよいが、本実施形態では四輪自動車として説明する。
【００３１】
本実施形態では、３種類の座標系を定義する。第１の座標系は、センサー３に対応付けられたセンサー座標系であるローカル座標系である。本明細書では、ローカル座標系のことをＬ（Local）フレームとも言う。ローカル座標系は、センサー３に対応付けられた三次元直交座標系であり、本実施形態では、ローカル座標系の３軸をｘ軸、ｙ軸及びｚ軸と表記する。
【００３２】
第２の座標系は、移動体に対応付けられた移動体座標系である。本明細書では、移動体座標系のことをＶ（Vehicle）フレームとも言う。Ｖフレームは、例えば、移動体の前方を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする三次元直交座標系として定義される。本実施形態では、移動体座標系の３軸をＲ軸、Ｐ軸及びＱ軸と表記する。
【００３３】
第３の座標系は、移動体の移動空間を定める座標系である絶対座標系である。本明細書では、絶対座標系のことをＡ（Absolute）フレームとも言う。Ａフレームは、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系として定義される。本実施形態では、絶対座標系の３軸をＸ軸、Ｙ軸及びＺ軸と表記する。
【００３４】
センサー３は、移動体に搭載される各種のセンサーである。例えば、センサー３の計測結果を利用して自立的に位置算出や速度算出を行う慣性航法システムに適用する場合は、センサー３として、加速度センサー３Ａや姿勢センサー３Ｂといった慣性航法用のセンサー（慣性センサー）が用いられる。
【００３５】
加速度センサー３Ａは、移動体の加速度をローカル座標系で検出するセンサーであり、例えばＭＥＭＳ（Micro Electro Mechanical Systems）の技術を利用したＭＥＭＳセンサーが用いられる。加速度センサー３Ａは移動体に固定される。加速度センサー３Ａが計測した値は、ローカル座標系で計測した移動体の加速度となる。
【００３６】
姿勢センサー３Ｂは、絶対座標系におけるセンサー３の向きである絶対姿勢を検出するためのセンサーであり、例えばジャイロセンサー３Ｃや磁気センサーといったセンサーが用いられる。姿勢センサー３Ｂは、ジャイロセンサー３Ｃや磁気センサー単体として構成してもよいし、これらを組み合わせて構成してもよい。また、加速度センサーを含んで姿勢センサー３Ｂを構成してもよい。姿勢センサー３Ｂは移動体に固定される。姿勢センサー３Ｂが計測した値は、絶対座標系で表したセンサー３の向きとなる。
【００３７】
移動体の加速度や速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則として、加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。また、本実施形態では、移動体の加速度ベクトル及び速度ベクトルを総称して、移動体の「移動ベクトル」と呼ぶ。なお、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。
【００３８】
また、センサー３の姿勢は、ロール角、ピッチ角及びヨー角といったオイラー角で表される。絶対座標系で表したセンサー３の向きを「絶対姿勢」と表現し、その姿勢角のことを「絶対姿勢角」と称する。また、移動体に対するセンサー３の相対的な姿勢（相対姿勢）のことを「取付姿勢」と表現し、その姿勢角（相対姿勢角）のことを「取付姿勢角」と称する。
【００３９】
また、以下の説明で参照する図面については、各種のセンサー３のブロックを二重線で示し、センサー３の計測結果を利用して演算処理を行う処理ブロックを一重線で示す。一重線で示される処理ブロックは、センサー３に搭載されるプロセッサーや、電子機器に搭載されるプロセッサー（ホストプロセッサー）が処理主体となる処理ブロックである。なお、各処理ブロックの処理の主体は、本発明を適用するシステムに応じて適宜設定可能であるものとする。
【００４０】
１−２．取付姿勢判定方法
本実施形態における取付姿勢判定方法について説明する。センサー３が移動体に取り付けられた場合に、移動体に対するセンサー３の相対的な姿勢である取付姿勢を判定することが１つの目的である。本実施形態では、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、センサー３の取付姿勢を判定する。前述したように、移動ベクトルには、速度ベクトルや加速度ベクトルが含まれる。そのため、本実施形態では、移動体が移動を開始した際に計測された速度ベクトルや加速度ベクトルを用いて、センサー３の取付姿勢を判定することになる。
【００４１】
（１）第１の取付姿勢判定方法
図２は、第１の取付姿勢判定方法に係る第１の取付姿勢判定システム１Ａ（第１の姿勢判定装置）のシステム構成図である。第１の取付姿勢判定システム１Ａは、例えば、加速度センサー３Ａと、ローカル座標速度ベクトル算出部５と、取付姿勢判定部１０とを有する。
【００４２】
ローカル座標速度ベクトル算出部５は、加速度センサー３Ａから入力したローカル座標加速度ベクトルを積分及び継ぎ足し処理して、ローカル座標速度ベクトルを算出する。本実施形態において、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【００４３】
具体的には、ローカル座標速度ベクトル算出部５は、所定時間分のローカル座標加速度ベクトルを積分することで、その所定時間の間におけるローカル座標系で表した速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新したローカル座標速度ベクトルに継ぎ足すことで、ローカル座標速度ベクトルを新たに算出・更新する。
【００４４】
取付姿勢判定部１０は、ローカル座標速度ベクトル算出部５から入力したローカル座標速度ベクトルを用いて取付姿勢を判定する。取付姿勢判定部１０は、移動体が停止している状態から移動を開始した際に計測される速度ベクトルを用いて、移動体に対するセンサー３の姿勢（取付姿勢）を判定する。
【００４５】
本実施形態では、移動体が停止している場合に、移動体の速度に関する制約条件（第１の制約条件）を用いて、センサー３の計測結果を補償する。移動体が停止しているのであれば、理想的には、移動体の速度ベクトルはゼロベクトルとなる。よって、移動体が停止している場合には、移動体の速度ベクトルの各成分がゼロであるとする運動モデルを適用することができる。但し、ここで言う移動体の速度ベクトルとは、移動体座標系（Ｖフレーム）での速度ベクトルである。
【００４６】
上記の運動モデルに基づいて、移動体の速度ベクトルを補正する手法が適用可能である。具体的には、停止中は移動体の速度ベクトルの各成分がゼロであるとする停止時速度制約条件を与えて、例えばカルマンフィルター（ＫＦ（Kalman Filter））を利用して、センサー３の計測結果を補正する手法が挙げられる。
【００４７】
カルマンフィルターは、時々刻々と変化する外部観測量を利用して、確率理論に基づいて系の状態を推定する推定手法の１つである。例えば、推定対象とする「状態」をセンサー３の計測結果に含まれる誤差とし、上記の速度制約条件を「観測情報」として利用して、センサー３の計測結果を補正する手法が考えられる。また、カルマンフィルター以外にも、上記の停止時速度制約条件に基づいて、センサー３の計測結果にリセットをかけるなどの手法も考えられる。つまり、停止時速度制約条件を用いて、センサー３をキャリブレーションするのである。
【００４８】
上記のように、移動体が停止している場合に、移動体の速度に関する制約条件（第１の制約条件）を用いてセンサー３の計測結果を補償することで、センサー３の計測結果に含まれる誤差を低減させる（小さくする）ことができる。誤差が低減された状態で移動体が移動を開始すると、暫くの間は、センサー３の計測結果は誤差が低減された状態となる。そこで、本実施形態では、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢（取付姿勢）を判定する。
【００４９】
ローカル座標系で表した速度ベクトル（ローカル座標速度ベクトル）と、移動体座標系で表した速度ベクトル（移動体座標速度ベクトル）との間には、次式（１）のような関係がある。
【数１】
【００５０】
本明細書における数式では、速度ベクトルの成分を小文字の“ｖ”で表す。また、速度ベクトルの表記において、座標系の種類を上付きの大文字の添え字で示す。“Ｌ”はローカル座標系、“Ｖ”は移動体座標系、“Ａ”は絶対座標系をそれぞれ意味する。また、対応する座標系の各軸の成分を下付きの小文字の添え字で示す。例えば、ローカル座標系の３軸であれば“ｘ”、“ｙ”及び“ｚ”の表記を用いる。
【００５１】
また、本明細書では、座標変換行列を大文字の“Ｃ”で表す。各座標変換行列において、下付きの添え字は座標変換前の座標系を示し、上付きの添え字は座標変換後の座標系を示す。例えば、“ＣLV”は、ローカル座標系（Ｌフレーム）から移動体座標系（Ｖフレーム）への座標変換行列を示し、“ＣAL”は、絶対座標系（Ａフレーム）からローカル座標系（Ｌフレーム）への座標変換行列を示す。
【００５２】
ここで、移動体の移動方向に関する制約条件（第２の制約条件）を適用する。例えば、移動体が四輪自動車であれば、通常走行において四輪自動車はジャンプやドリフトすることはないと仮定することができる。つまり、移動体の縦横方向の速度成分はゼロであると仮定し、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロであるとする制約条件を設定する。つまり、移動体の速度に関して｛ｖPV＝ｖQV＝０｝とする制約条件を設定する。
【００５３】
この場合、式（１）から、取付姿勢のピッチ成分であるピッチ角“θ”及びヨー成分であるヨー角“Ψ”を、次式（２）及び（３）のように算出することができる。
【数２】
【数３】
【００５４】
ピッチ角“θ”及びヨー角“Ψ”は、それぞれ移動体座標系のＰ軸及びＱ軸回りのセンサー３の回転角を示す。つまり、ピッチ角“θ”及びヨー角“Ψ”は、移動体に対するセンサー３の相対的な姿勢角を表していると言える。
【００５５】
なお、自動車が移動を開始した直後は、自動車の速度はゼロに近い値となる。この状態で式（２）及び（３）に従って取付姿勢角を算出すると、浮動小数点数同士の演算により、十分な演算精度が保証されない可能性がある。そこで、自動車が移動を開始してもすぐには取付姿勢角を算出せず、速度又は加速度がある程度大きくなった時点で、取付姿勢を判定すると効果的である。例えば、自動車のブレーキ制御が解除された直後から所定時間（１秒以下の時間として定められる。）経過時に取付姿勢を判定することとしたり、ブレーキ制御が解除された後に「１ｋｍ／ｈ以上２０ｋｍ／ｈ以下」の所定速度に到達した際に取付姿勢を判定するなどすると効果的である。
【００５６】
（２）第２の取付姿勢判定方法
図３は、第２の取付姿勢判定方法に係る第２の取付姿勢判定システム１Ｂ（第２の姿勢判定装置）のシステム構成図である。第２の取付姿勢判定システム１Ｂは、例えば、加速度センサー３Ａと、姿勢センサー３Ｂと、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、取付姿勢判定部１０とを有する。本実施形態において、加速度センサー３Ａ及び姿勢センサー３Ｂは、共通のローカル座標系での値を計測するように、例えば一体的に構成される。
【００５７】
ＬＡ座標変換行列算出部２０は、姿勢センサー３Ｂから入力したセンサー３の絶対姿勢角（絶対座標系で表したセンサー３の向き）を用いて、ローカル座標系から絶対座標系への座標変換行列（以下、「ＬＡ座標変換行列」と称する。）を算出する。なお、ＬＡ座標変換行列それ自体は公知であるため、数式等を用いた説明は省略する。
【００５８】
絶対座標加速度ベクトル算出部３０は、ＬＡ座標変換行列算出部２０から入力したＬＡ座標変換行列を用いて、加速度センサー３Ａから入力したローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する。
【００５９】
絶対座標速度ベクトル算出部４０は、絶対座標加速度ベクトル算出部３０から入力した絶対座標加速度ベクトルを積分及び継ぎ足し処理することで、絶対座標速度ベクトルを算出する。つまり、所定時間分の絶対座標加速度ベクトルを積分することで、その所定時間の間の移動体の速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新した絶対座標速度ベクトルに継ぎ足すことで、絶対座標速度ベクトルを新たに算出・更新する。
【００６０】
取付姿勢判定部１０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルと、姿勢センサー３Ｂから入力した絶対姿勢角とを用いて、加速度センサー３Ａの取付姿勢を判定する。取付姿勢判定部１０は、絶対座標系からローカル座標系への座標変換行列（以下、「ＡＬ座標変換行列」と称す。）を算出するＡＬ座標変換行列算出部１１を機能部として有する。
【００６１】
絶対座標系で表した速度ベクトル（絶対座標速度ベクトル）と、移動体座標系で表した速度ベクトル（移動体座標速度ベクトル）との間には、次式（４）のような関係がある。
【数４】
【００６２】
但し、“ＣAL”はＡＬ座標変換行列を示す。また、“ＣLV”はローカル座標系（Ｌフレーム）から移動体座標系（Ｖフレーム）への座標変換行列（以下、「ＬＶ座標変換行列」と称す。）を示す。
【００６３】
式（４）から、絶対座標系と移動体座標系とを結び付けるためには、２段階の座標変換が必要であることがわかる。つまり、第１段階として、ＡＬ座標変換行列“ＣAL”を用いて、絶対座標速度ベクトルをローカル座標速度ベクトルに座標変換する。その後、第２段階として、ＬＶ座標変換行列“ＣLV”を用いて、ローカル座標速度ベクトルを移動体座標速度ベクトルに座標変換する。
【００６４】
ＡＬ座標変換行列“ＣAL”は、絶対座標系で表したセンサー３の向き（絶対姿勢）により定まる行列であり、ＬＡ座標変換行列算出部２０により算出されるＬＡ座標変換行列の逆行列である。この場合、ＡＬ座標変換行列を用いて絶対座標速度ベクトルをローカル座標速度ベクトルに座標変換すると、式（４）は式（１）と一致する。つまり、次式（５）が成立するため、式（４）及び式（５）から、式（１）が導出される。
【数５】
【００６５】
このように式（１）を導出することができるため、第１の取付姿勢判定方法と同様に、取付姿勢を判定することができる。つまり、移動体座標系（Ｖフレーム）において、Ｑ軸方向及びＰ軸方向（縦横方向）の速度成分はゼロであるとする制約条件｛ｖPV＝ｖQV＝０｝に基づき、式（２）及び（３）に従って取付姿勢角を算出する。
【００６６】
上記を実現するため、取付姿勢判定部１０のＡＬ座標変換行列算出部１１は、姿勢センサー３Ｂから入力した絶対姿勢角を用いて、ＡＬ座標変換行列を算出する。そして、取付姿勢判定部１０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルを、ＡＬ座標変換行列算出部１１により算出されたＡＬ座標変換行列を用いてローカル座標速度ベクトルに座標変換する。そして、最終的に、ローカル座標速度ベクトルを用いて、式（２）及び式（３）に従って取付姿勢角を算出する。
【００６７】
１−３．位置算出方法
次に、上記の取付姿勢判定方法を用いて判定したセンサー３の取付姿勢と、移動ベクトルとを用いて、移動体の位置を算出する位置算出方法について説明する。
【００６８】
（１）第１の位置算出方法
図４は、第１の位置算出方法に係る第１の位置算出システム２Ａ（第１の位置算出装置）のシステム構成図である。第１の位置算出システム２Ａは、例えば、第１の慣性航法システム（第１のＩＮＳ（Inertial Navigation System））４Ａと、取付姿勢判定部１０と、移動状況判定部６０と、速度制約条件設定部７０と、ＬＶ座標変換行列算出部８０と、補正部９０とを有する。
【００６９】
第１の慣性航法システム４Ａは、ＩＮＳとして知られる慣性航法システムであり、そのシステム構成の一例を図５に示す。第１の慣性航法システム４Ａは、例えば、加速度センサー３Ａと、ジャイロセンサー３Ｃと、ローカル座標速度ベクトル算出部５と、絶対姿勢判定部１５と、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、絶対座標位置算出部５０とを有する。
【００７０】
ローカル座標速度ベクトル算出部５は、加速度センサー３Ａから入力したローカル座標加速度ベクトルを積分及び継ぎ足し処理することで、ローカル座標速度ベクトルを算出する。
【００７１】
絶対姿勢判定部１５は、ジャイロセンサー３Ｃから入力したローカル座標角速度を積分及び継ぎ足し処理することで、絶対姿勢角を判定する。具体的には、絶対座標系で表したセンサー３の所与の基準姿勢からの姿勢角変化分を、ジャイロセンサー３Ｃから入力したローカル座標角速度を積分することで算出する。そして、基準姿勢に姿勢角変化分を継ぎ足すことで、絶対姿勢角を新たに算出・更新する。
【００７２】
ＬＡ座標変換行列算出部２０、絶対座標加速度ベクトル算出部３０及び絶対座標速度ベクトル算出部４０は、図３と同様に、それぞれＬＡ座標変換行列、絶対座標加速度ベクトル及び絶対座標速度ベクトルを算出する。
【００７３】
絶対座標位置算出部５０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルを積分及び継ぎ足し処理することで、移動体の絶対座標位置を算出する。つまり、所定時間分の絶対座標速度ベクトルを積分することで、その所定時間の間における位置変化分を算出する。そして、算出した位置変化分を最後に更新した絶対座標位置に継ぎ足すことで、絶対座標位置を新たに算出・更新する。
【００７４】
第１の慣性航法システム４Ａからは、ローカル座標速度ベクトル算出部５により算出されたローカル速度ベクトルと、絶対姿勢判定部１５により算出された絶対姿勢角と、絶対座標速度ベクトル算出部４０により算出された絶対座標速度ベクトルと、絶対座標位置算出部５０により算出された絶対座標位置とが出力される。
【００７５】
図４の説明に戻り、取付姿勢判定部１０は、第１の慣性航法システム４Ａから入力したローカル座標速度ベクトルを用いて、移動体に対する第１の慣性航法システム４Ａの取付姿勢を判定する。
【００７６】
移動状況判定部６０は、移動体の移動状況を判定する。移動体の移動状況判定は、種々の手法により実現可能である。例えば、慣性航法システムが具備する加速度センサー３Ａやジャイロセンサー３Ｃの計測結果を利用して判定を行う方法が挙げられる。また、移動体が自動車である場合には、自動車に搭載される車速検出システム（例えば車速パルス）により計測された車速を利用して、移動体の移動状況を判定してもよい。
【００７７】
速度制約条件設定部７０は、移動状況判定部６０から入力した移動状況に基づいて、補正部９０に適用する速度制約条件を設定する。本実施形態では、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との２種類の速度制約条件を切り替えて設定する。
【００７８】
停止時速度制約条件は、移動体の速度に関する第１の制約条件であり、移動体の停止時に適用する制約条件である。詳細には、移動体の停止時には、移動体の速度ベクトルはゼロベクトルになるという仮定に基づき設定される制約条件である。この停止時速度制約条件を適用する座標系は任意であるが、例えば絶対座標系で適用することが考えられる。この場合は、例えば｛ｖXA＝ｖYA＝ｖZA＝０｝を制約条件として与える。
【００７９】
移動時速度制約条件は、移動体の移動方向に関する第２の制約条件であり、移動体の移動時に適用する制約条件である。前述したように、移動体が四輪自動車であれば、通常走行時において移動体はジャンプやドリフトすることはないとする仮定に基づき設定される制約条件である。この場合は、例えば｛ｖPV＝ｖQV＝０｝を制約条件として与える。この制約条件は、移動体座標系において有効な条件である。
【００８０】
ＬＶ座標変換行列算出部８０は、取付姿勢判定部１０から入力した取付姿勢角を用いて、ローカル座標系から移動体座標系への座標変換行列（以下、「ＬＶ座標変換行列」と称する。）を算出する。なお、ＬＶ座標変換行列それ自体は公知であるため、数式等を用いた説明は省略する。
【００８１】
補正部９０は、第１の慣性航法システム４Ａから入力した慣性航法演算結果を補正する。補正部９０は、慣性航法演算結果のうち、少なくとも絶対座標位置を補正するように構成されていればよい。つまり、少なくとも絶対座標位置に対して誤差推定演算を行い、推定した位置誤差を用いて絶対座標位置を推定するように構成されていればよい。
【００８２】
本実施形態では、補正部９０が、絶対座標位置だけではなく、絶対座標速度ベクトル及び絶対姿勢角も併せて補正する場合を例示する。補正部９０は、誤差推定部９１と、ＡＬ座標変換行列算出部９３とを有する。
【００８３】
誤差推定部９１は、第１の慣性航法システム４Ａから入力した慣性航法演算結果に含まれる誤差（以下、「慣性航法演算誤差」と称す。）を、所定の誤差推定演算を行うことで推定する。慣性航法演算誤差の推定方法としては種々の手法を適用可能であるが、典型的な手法としては、カルマンフィルターを適用することができる。
【００８４】
具体的には、状態ベクトル“Ｘ”を、例えば次式（６）のように設定する。
【数６】
但し、“δＰ”、“δＶ”及び“δＡ”は、それぞれ絶対座標位置の誤差（位置誤差）、絶対座標速度ベクトルの誤差（速度ベクトル誤差）及び絶対姿勢角の誤差（姿勢角誤差）である。
【００８５】
移動体の停止時には、停止時速度制約条件に従って、例えば次式（７）のような停止時観測ベクトル“Ｚstop”を設定する。また、移動体の移動時には、移動時速度制約条件に従って、例えば次式（８）のような移動時観測ベクトル“Ｚmove”を設定する。
【数７】
【数８】
【００８６】
但し、式（８）において、｜Ｖ｜は速度の大きさを示す。速度の大きさは、慣性航法演算で得られた絶対座標速度ベクトルの大きさ（スカラー）として計算してもよいし、車速検出システム等の外部システムから取得した速度の大きさとして計算してもよい。
【００８７】
上記のように状態ベクトル“Ｘ”及び観測ベクトル“Ｚ”を設定し、カルマンフィルターの理論に基づく予測演算（時刻更新）及び補正演算（観測更新）を行って、状態ベクトル“Ｘ”の推定値（状態推定値）を求める。
【００８８】
上記の定義式において、状態ベクトル“Ｘ”は、絶対座標系で表した慣性航法演算誤差として定義されている。停止時観測ベクトル“Ｚstop”は絶対座標系で定義されているため、同じ座標系での演算が可能である。しかし、移動時観測ベクトル“Ｚmove”は移動体座標系で定義されているため、同じ座標系での演算が困難である。
【００８９】
そこで、ＡＬ座標変換行列算出部９３は、第１の慣性航法システム４Ａから入力した絶対姿勢角を用いて、ＡＬ座標変換行列を算出する。そして、算出したＡＬ座標変換行列と、ＬＶ座標変換行列算出部８０から入力したＬＶ座標変換行列とを用いて、絶対座標系で表した状態ベクトル“Ｘ”を、移動体座標系で表した状態ベクトル“Ｘ”に座標変換する。つまり、２種類の座標変換行列を用いて、状態ベクトル“Ｘ”に２段階の座標変換を施すことになる。
【００９０】
同様に、ＡＬ座標変換行列及びＬＶ座標変換行列を用いて、慣性航法演算結果である絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を、それぞれ移動体座標系で表した移動体座標位置、移動体座標速度ベクトル及び移動体姿勢角に座標変換する。つまり、２種類の座標変換行列を用いて、慣性航法演算結果に２段階の座標変換を施すことになる。
【００９１】
移動体座標系に座標変換した状態ベクトル“Ｘ”に対して、移動体座標系で定義した移動時観測ベクトル“Ｚmove”を適用して、カルマンフィルターを利用した誤差推定演算を行って慣性航法演算誤差を推定する。そして、移動体座標系で推定された慣性航法演算誤差を用いて、上記の移動体座標位置、移動体座標速度ベクトル及び移動体姿勢角をそれぞれ補正する。そして、移動体座標系から絶対座標系に再変換することで、最終的な補正結果を得る。
【００９２】
補正部９０から出力される補正位置、補正速度ベクトル及び補正姿勢角は、第１の慣性航法システム４Ａにフィードバックされる。第１の慣性航法システム４Ａでは、補正部９０から入力した補正位置、補正速度ベクトル及び補正姿勢角を用いて、それぞれ絶対座標位置算出部５０、絶対座標速度ベクトル算出部４０及び絶対姿勢判定部１５により算出された絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角が補正される。
【００９３】
（２）第２の位置算出方法
図６は、第２の位置算出方法に係る第２の位置算出システム２Ｂ（第２の位置算出装置）のシステム構成図である。第２の位置算出システム２Ｂは、例えば、第２の慣性航法システム（第２のＩＮＳ）４Ｂと、取付姿勢判定部１０と、移動状況判定部６０と、速度制約条件設定部７０と、ＬＶ座標変換行列算出部８０と、補正部９０とを有する。
【００９４】
図７は、第２の慣性航法システム４Ｂのシステム構成の一例を示す図である。第２の慣性航法システム４Ｂは、加速度センサー３Ａと、ジャイロセンサー３Ｃと、絶対姿勢判定部１５と、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、絶対座標位置算出部５０とを有する。図５の第１の慣性航法システム４Ａにおいて、ローカル座標速度ベクトル算出部５を除いた構成である。
【００９５】
第２の慣性航法システム４Ｂからは、絶対姿勢判定部１５により算出された絶対姿勢角と、絶対座標速度ベクトル算出部４０により算出された絶対座標速度ベクトルと、絶対座標位置算出部５０により算出された絶対座標位置とが出力される。
【００９６】
図６の説明に戻り、取付姿勢判定部１０は、第２の慣性航法システム４Ｂから入力した絶対座標速度ベクトルを用いて、移動体に対する第２の慣性航法システム４Ｂの取付姿勢を判定する。取付姿勢判定部１０は、ＡＬ座標変換行列算出部１１を機能部として有し、図３の第２の取付姿勢判定システム１Ｂと同様に、取付姿勢を判定する。
【００９７】
補正部９０は、取付姿勢判定部１０から入力した取付姿勢角及びＡＬ座標変換行列と、速度制約条件設定部７０から入力した速度制約条件と、ＬＶ座標変換行列算出部８０から入力したＬＶ座標変換行列とを用いて、第２の慣性航法システム４Ｂから入力した慣性航法演算結果を補正する。補正部９０は、誤差推定部９１を機能部として有する。
【００９８】
補正部９０からは、慣性航法演算結果に対する補正結果である補正位置、補正速度ベクトル及び補正姿勢角が出力される。また、これらの補正結果は、第２の慣性航法システム４Ｂにフィードバックされる。第２の慣性航法システム４Ｂは、補正部９０から入力した補正結果を利用して、慣性航法演算結果を補正する。
【００９９】
１−４．シミュレーション結果
上記の位置算出方法の有効性を示すために、移動体の位置算出を行うシミュレーション実験を行った結果について説明する。設置ずれが生ずるようにセンサー３を移動体に取り付け、上記の位置算出方法を用いて移動体の位置を算出するシミュレーション実験を行った。
【０１００】
図８は、従来の手法を用いたシミュレーション結果を示す。それに対し、図９は、本実施形態の手法を用いたシミュレーション結果を示す。各図において、横軸は東西方向、縦軸は南北方向を示し、単位はメートル［ｍ］である。東西方向“０ｍ”、南北方向“０ｍ”の位置をスタート地点とし、大きく反時計回り方向に１周周回するようなダイヤ型の経路を辿った。ゴール地点はスタート地点と同じである。移動体の真の軌跡を破線で示し、算出した位置の軌跡を実線で示す。
【０１０１】
図８を見ると、従来の手法を用いた場合は、重大な位置誤差が生じていることがわかる。つまり、ダイヤ型の軌跡に沿って位置が算出されるどころか、スタート直後から東方向に向かって位置が逸れていき、最終的には、ゴール地点から程遠い位置をゴール地点と判定していることがわかる。これは、センサー３の取付姿勢を判定せずに位置算出を行った結果である。つまり、ローカル座標系と移動体座標系とが一致していないにも関わらず、移動体座標系で定義される移動時速度制約条件（第２の制約条件）を適用して位置算出を行ったために、当初から誤った方向に位置が算出され、時間経過に伴って誤差が累積的に増加した結果である。
【０１０２】
それに対し、図９を見ると、本実施形態の手法を用いた場合は、ダイヤ型の真の軌跡に沿ったほぼ正確な軌跡が得られていることがわかる。これは、センサー３の取付姿勢を可能な限り正確に判定して、位置算出を行うことができた結果である。つまり、センサー３の取付姿勢を判定した上で、ローカル座標系と移動体座標系とのズレ分を考慮して移動体座標系への座標変換を行い、移動体座標系で定義される移動時速度制約条件を適用して位置算出を行ったため、このような良好な結果が得られたのである。
【０１０３】
２．実施例
次に、上記の姿勢判定方法及び位置算出方法を適用したナビゲーションシステムの実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【０１０４】
本実施例におけるナビゲーションシステムは、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、原理で説明した取付姿勢判定システム（姿勢判定装置）及び位置算出システム（位置算出装置）を適用したカーナビゲーション装置１０００が設置されたシステムとして構成される。
【０１０５】
カーナビゲーション装置１０００は、自動車に搭載される電子機器であり、加速度センサー３Ａ及びジャイロセンサー３Ｃを有するセンサーユニットとしてＩＭＵ（Inertial Measurement Unit）４００を具備している。ＩＭＵ４００は、慣性計測ユニットとして知られるセンサーユニットであり、ローカル座標系で表した加速度ベクトル及び角速度を計測して出力可能に構成されている。
【０１０６】
カーナビゲーション装置１０００は、ＩＭＵ４００の計測結果を利用して慣性航法演算処理を行うことで、絶対座標系での自動車の位置を算出する。本実施例では、絶対座標系としてＮＥＤ座標系を適用する場合について説明する。つまり、カーナビゲーション装置１０００は、ＮＥＤ座標系での自動車の位置を算出する。そして、カーナビゲーション装置１０００は、算出位置をプロットしたナビゲーション画面を生成し、当該ナビゲーション画面を表示部３００であるディスプレイに表示させる。
【０１０７】
２−１．機能構成
図１０は、カーナビゲーション装置１０００の機能構成の一例を示すブロック図である。カーナビゲーション装置１０００は、主要な機能構成として、処理部１００と、操作部２００と、表示部３００と、ＩＭＵ４００と、記憶部５００とを備えて構成される。
【０１０８】
処理部１００は、記憶部５００に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１０００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。処理部１００は、ＩＭＵ４００の計測結果を利用した慣性航法演算処理を行って自動車の位置等を算出する。処理部１００は、自動車に対するＩＭＵ４００の姿勢を判定する姿勢判定装置として機能するとともに、自動車の位置を算出する位置算出装置として機能する。
【０１０９】
図１１は、処理部１００に係る機能ブロックを中心に示したブロック図である。処理部１００は、慣性航法演算部１０５と、取付姿勢判定部１１０と、移動状況判定部１６０と、速度制約条件設定部１７０と、ＬＶ座標変換行列算出部１８０と、補正部１９０とを機能部として有する。
【０１１０】
慣性航法演算部１０５は、ＩＭＵ４００から入力したローカル座標加速度ベクトル及びローカル座標角速度を用いて、慣性航法演算処理を行って自動車のＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角を算出する。
【０１１１】
補正部１９０は、取付姿勢判定部１１０から入力したＩＭＵ４００の取付姿勢角と、速度制約条件設定部１７０から入力した速度制約条件とを用いて、慣性航法演算部１０５から入力したＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角を補正する。補正部１９０は、ＫＦ誤差推定部１９１と、ＮＬ座標変換行列１９３とを機能部として有する。
【０１１２】
ＫＦ誤差推定部１９１は、カルマンフィルターを利用してＮＥＤ座標系での慣性航法演算結果に含まれる慣性航法演算誤差を推定する。
【０１１３】
ＮＬ座標変換行列算出部１９３は、慣性航法演算部１０５から入力した絶対姿勢角を用いて、ＮＥＤ座標系からローカル座標系への座標変換行列（以下、「ＮＬ座標変換行列」と称す。）を算出する。
【０１１４】
図１０の説明に戻り、操作部２００は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１００に出力する。この操作部２００の操作により、目的地の入力等の各種指示入力がなされる。
【０１１５】
表示部３００は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１００から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３００には、ナビゲーション画面等が表示される。
【０１１６】
ＩＭＵ４００は、慣性計測ユニットとして知られるセンサーユニットであり、例えば加速度センサー３Ａ及びジャイロセンサー３Ｃを有して構成される。
【０１１７】
記憶部５００は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１０００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１８】
２−２．データ構成
図１０に示すように、記憶部５００には、プログラムとして、処理部１００により読み出され、ナビゲーション処理（図１２参照）として実行されるナビゲーションプログラム５１０が記憶されている。ナビゲーションプログラム５１０は、補正処理（図１３参照）として実行される補正プログラム５１１をサブルーチンとして含む。これらの処理については、フローチャートを用いて詳細に後述する。
【０１１９】
また、記憶部５００には、データとして、速度制約条件５２０と、ＩＭＵ計測データ５３０と、慣性航法演算データ５４０と、取付姿勢データ５５０と、座標変換行列データ５６０と、推定誤差データ５７０と、補正慣性航法演算データ５８０とが記憶される。
【０１２０】
速度制約条件５２０は、自動車の移動状況に応じた速度の制約条件であり、停止時速度制約条件５２１と、移動時速度制約条件５２３とがこれに含まれる。
【０１２１】
ＩＭＵ計測データ５３０は、ＩＭＵ４００の計測結果のデータである。加速度センサー３Ａにより計測されたローカル座標加速度ベクトルと、ジャイロセンサー３Ｃにより計測されたローカル座標角速度とが時系列に記憶される。
【０１２２】
慣性航法演算データ５４０は、処理部１００がＩＭＵ４００の計測結果を利用して慣性航法演算処理を行うことで算出したＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角のデータである。
【０１２３】
取付姿勢データ５５０は、処理部１００により算出されたＩＭＵ４００の取付姿勢角が記憶されたデータである。座標変換行列データ５６０は、処理部１００が算出した各種の座標変換行列が記憶されたデータである。
【０１２４】
推定誤差データ５７０は、処理部１００がＫＦ誤差推定演算を行って推定した慣性航法演算誤差が記憶されたデータである。また、補正慣性航法演算データ５８０は、処理部１００が、慣性航法演算誤差を用いて慣性航法演算結果を補正したデータである。
【０１２５】
２−３．処理の流れ
図１２は、記憶部５００に記憶されているナビゲーションプログラム５１０が処理部１００により読み出されて実行されることで、カーナビゲーション装置１０００において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、ＩＭＵ４００により計測されたローカル座標加速度ベクトル及びローカル座標角速度が、記憶部５００のＩＭＵ計測データ５３０に随時記憶されるものとする。
【０１２６】
最初に、処理部１００は、初期設定を行う（ステップＡ１）。例えば、カルマンフィルターで利用する各種パラメーター値の初期設定を行う。また、自動車が停止している状態であるとして停止時速度制約条件５２１を「ＯＮ」に設定し、移動時速度制約条件５２３を「ＯＦＦ」に設定する。
【０１２７】
次いで、慣性航法演算部１０５が、慣性航法演算処理を開始する（ステップＡ３）。つまり、慣性航法演算部１０５は、ＩＭＵ計測データ５３０に記憶されているＩＭＵ４００の計測データを利用して慣性航法演算処理を行う。そして、その結果を慣性航法演算データ５４０として記憶部５００に記憶させる。
【０１２８】
その後、補正部１９０は、記憶部５００に記憶されている補正プログラム５１１に従って補正処理を開始する（ステップＡ５）。
【０１２９】
図１３は、補正処理の流れを示すフローチャートである。
先ず、ＫＦ誤差推定部１９１は、移動時速度制約条件５２３が「ＯＮ」に設定されているか否かを判定する（ステップＢ１）。そして、移動時速度制約条件５２３が「ＯＮ」に設定されていると判定した場合は（ステップＢ１；Ｙｅｓ）、ＮＬ座標変換行列算出部１９３が、最新の絶対姿勢角を用いてＮＬ座標変換行列を算出し、記憶部５００の座標変換行列データ５６０に記憶させる（ステップＢ３）。
【０１３０】
次いで、ＫＦ誤差推定部１９１は、ステップＢ３で算出したＮＬ座標変換行列を用いて、ＮＥＤ座標速度ベクトルを移動体座標速度ベクトルに変換する（ステップＢ５）。そして、ＫＦ誤差推定部１９１は、移動時制約速度ベクトルと、移動体座標速度ベクトルとの差を算出して、移動時観測ベクトルとする（ステップＢ７）。
【０１３１】
その後、ＫＦ誤差推定部１９１は、ＫＦ誤差推定処理を行う（ステップＢ９）。すなわち、ステップＢ７で設定した移動時観測ベクトルを観測情報として、予測演算及び補正演算を行って慣性航法演算誤差を推定する。そして、ＫＦ誤差推定部１９１は、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正し、その結果を記憶部５００の補正慣性航法演算データ５８０に記憶させた後（ステップＢ１１）、ステップＢ１に戻る。
【０１３２】
また、ステップＢ１において移動時速度制約条件５２３が「ＯＮ」に設定されていないと判定した場合は（ステップＢ１；Ｎｏ）、ＫＦ誤差推定部１９１は、停止時速度制約条件５２１が「ＯＮ」に設定されているか否かを判定する（ステップＢ１３）。そして、「ＯＮ」に設定されていると判定したならば（ステップＢ１３；Ｙｅｓ）、ＫＦ誤差推定部１９１は、停止時制約速度ベクトルと、ＮＥＤ座標速度ベクトルとの差を算出して、停止時観測ベクトルとする（ステップＢ１５）。
【０１３３】
その後、ＫＦ誤差推定部１９１は、ＫＦ誤差推定処理を行う（ステップＢ１７）。すなわち、ステップＢ１５で設定した停止時観測ベクトルを観測情報として、予測演算及び補正演算を行って慣性航法演算誤差を推定する。そして、ＫＦ誤差推定部１９１は、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正し、その結果を記憶部５００の補正慣性航法演算データ５８０に記憶させた後（ステップＢ１９）、ステップＢ１に戻る。
【０１３４】
また、ステップＢ１３において停止時速度制約条件５２１が「ＯＮ」に設定されていないと判定した場合は（ステップＢ１３；Ｎｏ）、補正部１９０は、ステップＢ１に戻る。
【０１３５】
図１２の説明に戻り、ステップＡ５において補正処理を開始したならば、移動状況判定部１６０は、自動車の移動状況を判定する（ステップＡ７）。そして、処理部１００は、自動車が停止状態から移動を開始したか否かを判定する（ステップＡ９）。例えば、自動車のブレーキ制御の解除信号を外部入力したり、車速情報を外部入力する等して判定する。
【０１３６】
自動車が移動を開始したと判定した場合は（ステップＡ９；Ｙｅｓ）、速度制約条件設定部１７０は、停止時速度制約条件５２１を「ＯＦＦ」に設定する（ステップＡ１１）。そして、処理部１００は、所定の取付姿勢判定発動条件が成立したか否かを判定する（ステップＡ１３）。取付姿勢判定発動条件は、原理でも説明したように、取付姿勢の判定精度を保証するために定める条件である。例えば、移動体の停止が解除された後、移動体の速度又は加速度が所定の閾値に達することを発動条件として定めておくと好適である。
【０１３７】
ステップＡ１３において、未だ取付姿勢判定発動条件が成立していないと判定した場合は（ステップＡ１３；Ｎｏ）、処理部１００は、ステップＡ２７へと処理を移行する。取付姿勢判定発動条件が成立したと判定した場合は（ステップＡ１３；Ｙｅｓ）、取付姿勢判定部１１０は、取付姿勢判定処理を行う（ステップＡ１５）。つまり、最新のローカル座標速度ベクトルを用いて、式（２）及び式（３）に従って取付姿勢角を算出し、記憶部５００の取付姿勢データ５５０を更新する。
【０１３８】
次いで、処理部１００は、ステップＡ１５で算出した取付姿勢角を用いてＬＶ座標変換行列を算出し、記憶部５００の座標変換行列データ５６０を更新する（ステップＡ１７）。そして、処理部１００は、移動時速度制約条件５２３を「ＯＮ」に設定する（ステップＡ１９）。
【０１３９】
一方、ステップＡ９において自動車が移動を開始したことを検知しなかった場合は（ステップＡ９；Ｎｏ）、処理部１００は、自動車が移動状態から停止状態に変化したか否かを判定する（ステップＡ２１）。停止状態に変化しなかったと判定した場合は（ステップＡ２１；Ｎｏ）、処理部１００は、ステップＡ２７に移行する。
【０１４０】
また、自動車が移動状態から停止状態に変化したと判定した場合は（ステップＡ２１；Ｙｅｓ）、処理部１００は、移動時速度制約条件５２３を「ＯＦＦ」に設定するとともに（ステップＡ２３）、停止時速度制約条件５２１を「ＯＮ」に設定する（ステップＡ２５）。そして、処理部１００は、ステップＡ２７に移行する。
【０１４１】
処理部１００は、位置の出力タイミングであるか否かを判定し（ステップＡ２７）、出力タイミングではないと判定した場合は（ステップＡ２７；Ｎｏ）、ステップＡ３１へと処理を移行する。また、出力タイミングであると判定した場合は（ステップＡ２７；Ｙｅｓ）、処理部１００は、最新の補正位置を出力する（ステップＡ２９）。つまり、最新の補正位置に対してマップマッチング処理等を行い、表示部３００のナビゲーション画面を更新する。
【０１４２】
次いで、処理部１００は、ナビゲーション処理を終了するか否かを判定する（ステップＡ３１）。例えば、操作部２００を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。そして、まだ処理を終了しないと判定した場合は（ステップＡ３１；Ｎｏ）、処理部１００は、ステップＡ７に戻る。また、処理を終了すると判定した場合は（ステップＡ３１；Ｙｅｓ）、処理部１００は、ナビゲーション処理を終了する。
【０１４３】
２−４．実験結果
上記のカーナビゲーション装置１０００を自動車に搭載し、実際に自動車を走行させて位置を算出する実験を行った。具体的には、上記のナビゲーション処理に従って自動車の位置を算出し、その位置をプロットする実験を行った。
【０１４４】
図１４は、従来のカーナビゲーション装置１０００による実験結果を示す。それに対し、図１５は、本実施例のカーナビゲーション装置１０００による実験結果を示す。但し、何れの場合もマップマッチング機能はオフとしている。図の見方は、図８及び図９のシミュレーション結果と同じであるが、実際に走行実験を行った結果であるため、経路が異なっている。東西方向“０ｍ”、南北方向“０ｍ”の位置をスタート地点とし、東方向“１２０００ｍ”、南方向“１００００ｍ”の近傍位置をゴール地点として、スタート地点からゴール地点まで自動車を実際に走行させた。
【０１４５】
この結果を見ると、図１４の従来のカーナビゲーション装置１０００による実験結果では、図８のシミュレーション結果と同様に、スタート直後から誤った方向に位置が算出され、最終的には、ゴール地点から程遠い位置をゴール地点と判定している。それに対し、図１５の本実施例のカーナビゲーション装置１０００による実験結果では、図９のシミュレーション結果と同様に、ほぼ真の軌跡に沿った正確な軌跡が得られていることがわかる。これにより、実環境においても、本実施形態の位置算出方法は有効であることが示された。
【０１４６】
３．作用効果
本実施形態によれば、移動体に設置されたセンサー３により移動ベクトルが計測される。そして、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢が判定される。
【０１４７】
第１の取付姿勢判定システム１Ａでは、ローカル座標速度ベクトル算出部５が、加速度センサー３Ａが計測したローカル座標加速度ベクトルを用いてローカル座標速度ベクトルを算出する。そして、取付姿勢判定部１０が、移動体が移動を開始した際のローカル座標速度ベクトルを用いて、加速度センサー３Ａの取付姿勢のうちのピッチ成分及びヨー成分を判定する。
【０１４８】
また、第２の取付姿勢判定システム１Ｂでは、ＬＡ座標変換行列算出部２０が、姿勢センサー３Ｂが計測した絶対座標系で表したセンサー３の向き（絶対姿勢）を用いて、ローカル座標系から絶対座標系への座標変換行列（ＬＡ座標変換行列）を算出する。そして、絶対座標加速度ベクトル算出部３０が、加速度センサー３Ａが計測したローカル座標加速度ベクトルを、ＬＡ座標変換行列を用いて絶対座標加速度ベクトルに座標変換する。さらに、絶対座標速度ベクトル算出部４０は、絶対座標加速度ベクトルを用いて絶対座標速度ベクトルを算出する。取付姿勢判定部１０は、姿勢センサー３Ｂが計測した絶対座標系で表したセンサー３の向きを用いて絶対座標系からローカル座標系への座標変換行列（ＡＬ座標変換行列）を算出し、当該ＡＬ座標変換行列と、絶対座標速度ベクトルとを用いて、加速度センサー３Ａの取付姿勢のうちのピッチ成分及びヨー成分を判定する。
【０１４９】
また、移動体が停止している際には、センサー３の計測結果を補償するための補償演算を行う。例えば、移動体が停止している場合は、移動体の速度ベクトルの各成分はゼロであるとする制約条件（第１の制約条件）を用いて、センサー３の計測結果を補償する。これにより、移動体の停止中は、センサー３の計測結果に含まれる誤差を低減させることができる。誤差が低減された状態で移動体が移動を開始しても、それほど時間が経過していなければ、移動体の速度ベクトルの誤差は抑制されているはずである。そのため、移動体が移動を開始した際の速度ベクトルを利用することで、移動体に対するセンサー３の相対的な姿勢を正しく判定することができる。
【０１５０】
また、第１の位置算出システム２Ａでは、第１の慣性航法システム４Ａの慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）が、補正部９０によって補正される。補正部９０は、取付姿勢判定部１０により判定された第１の慣性航法システム４Ａの取付姿勢と、速度制約条件設定部７０により設定された停止時速度制約条件（第１の制約条件）又は移動時速度制約条件（第２の制約条件）と、ＬＶ座標変換行列算出部８０により算出されたＬＶ座標変換行列とを用いて、所定の誤差推定演算を行って慣性航法演算誤差を推定する。そして、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正する。また、第２の位置算出システム２Ｂにおいても同様に、第２の慣性航法システム４Ｂの慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）が、補正部９０によって補正される。
【０１５１】
このように、第１又は第２の取付姿勢判定方法を用いて判定したセンサー３の取付姿勢と、センサー３が計測した移動ベクトル（加速度ベクトルや速度ベクトル）とを用いることで、ローカル座標系と移動体座標系とのズレ分を考慮し、この２種類の座標系の座標変換を適切に行って、移動体の位置をより正確に算出することができる。
【０１５２】
４．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１５３】
４−１．進行方向切替対応ロジック
上記の実施形態では、移動体が移動を開始した際に求めたＬＶ座標変換行列を用いて位置算出を行うものとして説明した。しかし、移動体は必ずしも進行方向前方にのみ移動するとは限らない。例えば、移動体が四輪自動車であるならば、四輪自動車は前方に進行するに限らず、後方に進行する（バックする）場合もあり得る。
【０１５４】
従って、移動体の進行方向が前進と後進とで切り替わる場合、進行方向の変化の前後で同じＬＶ座標変換行列を用いて計算を行ったのでは、移動体の速度や位置が適切に求まらないことが想定される。なぜならば、本実施形態の手法では、移動体が移動を開始した際の移動体の移動方向に対する移動ベクトルを用いてセンサー３の取付姿勢を判定するため、移動体の移動方向が逆転してしまうと、取付姿勢と移動方向との間で辻褄が合わなくなるためである。
【０１５５】
上記の問題に鑑み、次のような進行方向切替対応ロジックを適用すると好適である。具体的には、例えば移動体座標系での移動体の速度ベクトルに基づいて、移動体の移動方向が逆転したか否かを判定する。そして、移動体の移動方向が逆転した場合に、移動開始時に求めて記憶しておいたＬＶ座標変換行列を１８０°回転させて更新し、更新後のＬＶ座標変換行列を用いて計算を行う。
【０１５６】
図１６は、この場合に、実施例のカーナビゲーション装置１０００が図１２のナビゲーション処理に代えて実行する第２のナビゲーション処理の流れを示すフローチャートである。
【０１５７】
第２のナビゲーション処理では、処理部１００は、ステップＡ１９又はステップＡ２５の後、移動体座標系のロール軸（Ｒ軸）の速度成分の正負の符号が逆転したか否かを判定する（ステップＣ２５）。符号が逆転していない場合は（ステップＣ２５；Ｎｏ）、自動車の移動方向は変化していないものと判断し、ステップＡ２７に移行する。
【０１５８】
それに対し、正負の符号が逆転している場合は（ステップＣ２５；Ｙｅｓ）、処理部１００は、自動車の移動方向が変化したものと判断し、座標変換行列データ５６０に記憶されている最新のＬＶ座標変換行列を１８０°回転させて、座標変換行列データ５６０を更新する（ステップＣ２６）。そして、処理部１００は、ステップＡ２７に移行する。
【０１５９】
図１７及び図１８は、上記の進行方向切替対応ロジックの有効性を示す実験結果である。図１７は、進行方向切替対応ロジックを適用しない場合の実験結果である。それに対し、図１８は、進行方向切替対応ロジックを適用した場合の実験結果である。車の進行方向前方（前進方向）とセンサー３のｘ軸正方向とが一致するようにセンサー３を設置した。つまり、この場合のセンサー３の取付姿勢角は「０°」である。
【０１６０】
自動車を時刻「ｔ０」〜「ｔ１」までの期間停止させた。そして、時刻「ｔ１」においてバックで移動開始し、時刻「ｔ２」において自動車を一旦停止させて、今度は自動車を前方に移動させた。各図において、横軸は時間軸を示し、縦軸は自動車のロール軸速度（単位は［ｍ／ｓ］）及びセンサー３の取付姿勢角（単位は［°］）を示す。また、判定されたセンサー３の取付姿勢角を太実線で示し、算出された自動車のロール軸速度を細実線で示す。
【０１６１】
これらの図を見ると、時刻「ｔ０」〜「ｔ１」までの間は、ロール軸速度はゼロとなっている。そして、時刻「ｔ１」において自動車がバックを開始するとロール軸速度は負の値となり、時刻「ｔ２」までは徐々に減少していく。
【０１６２】
その後、時刻「ｔ２」においてロール軸速度の絶対値が所定の閾値を超えると、取付姿勢判定発動条件が成立する（図１６のステップＡ１３；Ｙｅｓ）。これにより、センサー３の取付姿勢が判定される（図１６のステップＡ１５）。この場合、取付姿勢判定部１０は、自動車が後進（バック）した方向を自動車の移動方向と認識して取付姿勢を判定するため、実際にはセンサー３の取付姿勢角は「０°」であるのに、取付姿勢角を「１８０°」と判定してしまう。
【０１６３】
この場合、取付姿勢角を「１８０°」としてＬＶ座標変換行列を算出し（図１６のステップＡ１７）、当該ＬＶ座標変換行列を用いて計算を行うため、自動車のロール軸速度は正の値として算出されてしまう。自動車はバックで移動しているため、実際にはロール軸速度は負の値である。
【０１６４】
進行方向切替対応ロジックを適用しない場合は、図１７に示すように、時刻「ｔ３」において自動車が前方に移動を開始しても、センサー３の取付姿勢角が「１８０°」であると認識したまま計算を行うため、ロール軸速度の符号は変化しない。そのため、正の値として算出されるべきロール軸速度は負の値として算出され、自動車が前方に加速するにつれて、ロール軸速度は負の方向に増大してしまっている。
【０１６５】
それに対し、進行方向切替対応ロジックを適用した場合は、図１８に示すように、時刻「ｔ３」において自動車が前方に移動を開始したことを検知すると（図１６のステップＣ２５；Ｙｅｓ）、取付姿勢角を「１８０°」として算出したＬＶ座標変換行列を反転させて更新する（図１６のステップＣ２６）。そして、反転後のＬＶ座標変換行列を用いて計算を行うため、算出されるロール軸速度の符号は負から正に変化する。そして、自動車が前方に加速したことで、ロール軸速度は正の方向に増大していく。このように、進行方向切替対応ロジックを適用することで、移動体の進行方向の変化に適切に対応することができる。
【０１６６】
４−２．補正部の補正方法
上記の実施形態では、慣性航法演算結果を補正する手法としてカルマンフィルターを例に挙げて説明したが、補正方法はこれに限られない。例えば、速度制約条件設定部７０により設定された速度制約条件に含まれる制約速度ベクトルと、慣性航法演算で得られた速度ベクトルとの平均化処理を行って速度ベクトルを補正することとしてもよい。
【０１６７】
また、車速検出システムにより計測された移動体の速度や、磁気センサー等の方位センサーにより計測された移動体の向きを用いて、慣性航法演算で得られた速度ベクトルを補正することとしてもよい。例えば、速度ベクトルの大きさについて、慣性航法演算で得られた速度ベクトルの大きさと、車速検出システムにより計測された速度との平均化処理を行うこととしてもよい。また、速度ベクトルの向きについて、慣性航法演算で得られた速度ベクトルの向きと、方位センサーにより計測された移動体の向きとの平均化処理を行うこととしてもよい。
【０１６８】
平均化処理は、単純な算術平均や幾何平均を適用してもよいし、加重平均を適用してもよい。慣性航法演算結果を信用して速度ベクトルを補正するのであれば、慣性航法演算結果の重みを車速検出システムや方位センサーの計測結果の重みよりも高くして加重平均すればよい。逆に、車速検出システムや方位センサーの計測結果を信用して速度ベクトルを補正するのであれば、慣性航法演算結果の重みを車速検出システムや方位センサーの計測結果の重みよりも低くして加重平均すればよい。なお、これらの手法以外にも、補正部の補正手法としては任意の手法を適用可能である。
【０１６９】
４−３．処理の主体
上記の実施例では、電子機器にＩＭＵ４００が搭載され、ＩＭＵ４００の計測結果に基づいて、電子機器の処理部が慣性航法演算処理を行うものとして説明した。しかし、電子機器にＩＮＳを搭載することとし、ＩＮＳの処理部が慣性航法演算処理を行うこととしてもよい。この場合、電子機器の処理部は、ＩＮＳから出力される慣性航法演算結果に含まれる誤差（慣性航法演算誤差）を推定する処理を行う。そして、推定した慣性航法演算誤差を用いて、ＩＮＳから入力した慣性航法演算結果を補正する。
【０１７０】
また、電子機器にＧＰＳ（Global Positioning System）等の衛星測位システムを適用したセンサーユニットを搭載することとし、このセンサーユニットの計測結果を利用して、慣性航法演算結果を補正することとしてもよい。
【０１７１】
図１９は、この場合における電子機器の構成を示す図であり、処理部１００に係る機能ブロックを中心に示したものである。図１９の電子機器では、図１１のＩＭＵ４００の代わりにＩＮＳ７００が搭載され、さらにＧＰＳユニット８００が搭載されている。この場合、処理部１００は慣性航法演算処理を行わないため、図１１における慣性航法演算部１０５は削除されている。
【０１７２】
取付姿勢判定部１１０は、ＩＮＳ７００から入力したローカル座標速度ベクトルを用いて、ＩＮＳ７００の取付姿勢を判定する。補正部１９０のＫＦ誤差推定部１９１は、速度制約条件設定部１７０から入力した速度制約条件の他に、ＧＰＳユニット８００から入力したメジャメント情報を観測情報として、ＩＮＳ７００の慣性航法演算結果に含まれる慣性航法演算誤差を推定する。
【０１７３】
メジャメント情報は、ＧＰＳ衛星から受信したＧＰＳ衛星信号に係る各種の諸量である。例えば、ＧＰＳ衛星信号のキャリア位相やコード位相、ドップラー周波数、擬似距離、擬似距離の変化率（レンジレート）といった諸量がメジャメント情報に含まれる。なお、ＧＰＳのメジャメント情報を観測情報として慣性航法演算誤差を推定する手法については従来公知であるため、数式等を用いた説明は省略する。
【０１７４】
４−４．姿勢判定タイミング
上記の実施形態では、移動体が停止した後に移動を開始する度に、センサー３の取付姿勢を判定するものとして説明した。しかし、姿勢判定タイミングは適宜設定可能である。例えば、センサー３が移動体に設置された後、移動体の初回の移動開始タイミングにのみセンサー３の取付姿勢を判定することとしてもよい。この場合は、初回以降は取付姿勢判定を行わず、初回に判定した取付姿勢を用いて処理を行うようにすればよい。
【０１７５】
４−５．姿勢判定の成分
上記の実施形態では、移動体に対するセンサー３の取付姿勢のピッチ成分及びヨー成分の両方を判定するものとして説明したが、ピッチ成分及びヨー成分の何れか一方のみを判定することとしてもよいのは勿論である。
【０１７６】
４−６．設置ずれの誤差
上記の実施形態では、移動中はセンサー３の取付姿勢に変化が生じないことを前提としていた。しかし、移動中の振動等によってセンサー３の取付姿勢の変化、すなわち設置ずれが生じる可能性がある。そこで、式（６）の状態ベクトルに設置ずれによる誤差角度の成分を追加し、ＫＦ誤差推定部１９１が移動時速度制約条件５２１を「ＯＮ」として求めた移動時観測ベクトルを用いてＫＦ誤差推定処理を行って（ステップＢ９）、設置ずれの誤差角度を推定することとしてもよい。このようにすることで、移動中にセンサー３の取付姿勢が変化した場合であっても、図１２や図１６のステップＡ１５の取付姿勢判定処理で判定された取付姿勢を随時補正することが可能となる。
【０１７７】
４−７．取付姿勢判定処理の変形例
図３等を用いて説明した取付姿勢の判定では、ＩＭＵ４００が計測するローカル座標加速度ベクトルにバイアス誤差がのっている場合、算出されるＡＬ座標変換行列にもバイアス誤差が含まれるため、正確な姿勢の判定ができない場合が考えられる。そこで、次のようにしてもよい。
【０１７８】
すなわち、図２０に示すように、図１１に示した処理部１００の機能ブロックにおいて、ＩＭＵ４００が計測したローカル座標加速度ベクトル及びローカル座標角速度を取付姿勢判定部１１０へ入力するようにする。そして、取付姿勢判定部１１０（処理部１００）は、図２１に示す第２の取付姿勢判定処理を実行する。
【０１７９】
図２１において、処理部１００は、自動車が停止しているか否かを判定する（ステップＤ１）。停止している場合には、ローカル座標速度ベクトルをゼロリセットするとともに（ステップＤ３）、ＩＭＵ４００で計測されているローカル座標角速度及びローカル座標加速度ベクトルを停止時の計測データとして取得・更新する（ステップＤ５）。このときのローカル座標角速度をローカル座標停止時角速度Ωｓと称し、ローカル座標加速度をローカル座標停止時加速度αｓと称する。
【０１８０】
停止が継続している間、ステップＤ３〜Ｄ５を繰り返し行い、停止が解除され、自動車が移動を開始したと判定された場合は（ステップＤ７：ＹＥＳ）、次の処理に移行する。ここで、移動中のＩＭＵ４００で計測されているローカル座標角速度を符号Ωｍとし、ローカル座標加速度を符号αｍとして説明する。まず、ローカル座標の回転情報を算出する（ステップＤ９）。具体的には、ローカル座標停止時角速度Ωｓに対するローカル座標角速度Ωｍの時間変化（時間微分）を積算し、継ぎ足していく。この値は、停止時のローカル座標系に対する座標系自体の回転、すなわちローカル座標回転情報を示す。
【０１８１】
次いで、ローカル座標停止時加速度ベクトルαｓを、ステップＤ９で算出したローカル座標回転情報を用いて逆変換することで、ＩＭＵ４００で計測されているローカル座標系における加速度αｍｓに変換する（ステップＤ１１）。そして、ＩＭＵ４００で計測されているローカル座標加速度αｍと変換した加速度αｍｓとの差を取ることで、当該時点の加速度αｆを求める（ステップＤ１３）。変換した加速度αｍｓと、ＩＭＵ４００で計測されているローカル座標系における加速度αｍとは、同じローカル座標系上での加速度である。そして、もしも両者の加速度ベクトルにバイアス誤差がのっていたとしても、その差を求める結果、バイアス誤差は相殺することになる。また、重力加速度も相殺される。そのため、ＩＭＵ４００で計測されているローカル座標系における自動車自体の純粋な加速度に係るベクトルαｆが得られる。
【０１８２】
次いで、得られた加速度ベクトルαｆを用いて、ローカル座標速度ベクトルを更新する（ステップＤ１５）。更新が初回の場合は、ステップＤ３でリセットしたローカル座標速度ベクトルが初めて更新されるため、停止時のローカル座標系における移動開始直後の速度ベクトルが得られる。以降、取付姿勢の判定タイミングが到来するまでステップＤ９〜Ｄ１５の処理を繰り返す（ステップＤ１７）。取付姿勢の判定タイミングは、図１２のステップＡ１３の取付姿勢判定発動条件の成否で判定すると好適である。
姿勢判定の判定タイミングが到来した場合、最新のローカル座標速度ベクトルをもとに、上述の式（２）及び式（３）を用いて取付姿勢を判定する（ステップＤ１９）。
【０１８３】
以上の第２の取付姿勢判定処理は、ナビゲーション処理の実行中に並行して実行することとするとよい。例えば図１２や図１６のステップＡ３とＡ５の間に、第２の取付姿勢判定処理の実行を開始するステップを加える。この場合、第２の取付姿勢判定処理は、図１２や図１６のステップＡ１５で実行される取付姿勢判定処理の代わりの処理となるため、ステップＡ１５では取付姿勢判定処理を行わずともよい。
【０１８４】
また、第２の取付姿勢判定処理の効果確認試験を行った。ローカル座標系のｘ軸及びｚ軸にバイアス誤差を生じさせた場合の従来の取付姿勢判定処理の判定結果と、第２の取付姿勢判定処理の判定結果とを比較した。ｘ軸に＋１［ｍＧ］、ｚ軸に−１［ｍＧ］のバイアス誤差を生じさせた第１試験では、従来の取付姿勢判定処理ではヨー角の誤差が−３．４０［ｄｅｇ］、ピッチ角の誤差が＋５．５３［ｄｅｇ］であった。一方、第２の取付姿勢判定処理ではヨー角の誤差が＋０．０６［ｄｅｇ］、ピッチ角の誤差が−０．０１［ｄｅｇ］であった。また、ｘ軸に＋１００［ｍＧ］、ｚ軸に−１００［ｍＧ］のバイアス誤差を生じさせた第２試験では、従来の取付姿勢判定処理ではヨー角の誤差が＋７４．２６［ｄｅｇ］、ピッチ角の誤差が−３４．３０［ｄｅｇ］であった。一方、第２の取付姿勢判定処理ではヨー角の誤差が＋０．０５［ｄｅｇ］、ピッチ角の誤差が＋０．００［ｄｅｇ］であった。
【０１８５】
４−８．移動体
本発明は種々の移動体に適用可能である。好適な適用例は四輪自動車であるが、例えば移動時には縦方向や横方向に移動しないといった移動方向に関する制約を課すことができるのであれば、二輪自動車や自転車、電車、船、人間といった移動体にも本発明を適用可能である。
【０１８６】
４−９．電子機器
上記の実施例では、カーナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、携帯型ナビゲーション装置（ポータブルナビ）や携帯型電話機、パソコン、ＰＤＡといった他の電子機器にも本発明を適用可能である。
【符号の説明】
【０１８７】
１Ａ 第１の取付姿勢判定システム、 １Ｂ 第２の取付姿勢判定システム、 ２Ａ 第１の位置算出システム、 ２Ｂ 第２の位置算出システム、 ３ センサー、 ３Ａ 加速度センサー、 ３Ｂ 姿勢センサー、 ３Ｃ ジャイロセンサー、 ４Ａ 第１の慣性航法システム、 ４Ｂ 第２の慣性航法システム、 ５ ローカル座標速度ベクトル算出部、 １０ 取付姿勢判定部、 １５ 絶対姿勢判定部、 ２０ ＬＡ座標変換行列算出部、 ３０ 絶対座標加速度ベクトル算出部、 ４０ 絶対座標速度ベクトル算出部、 ５０ 絶対座標位置算出部、 ６０ 移動状況判定部、 ７０ 速度制約条件設定部、 ８０ ＬＶ座標変換行列算出部、 ９０ 補正部
【技術分野】
【０００１】
本発明は、移動体に対するセンサーの姿勢を判定する方法等に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、センサーの活用が注目されている。センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。センサーの計測結果を利用して慣性航法演算を行って移動体（例えば自転車や自動車、電車、船、飛行機など）の位置算出を行う技術も考案されている。
【０００３】
慣性航法演算においては、センサーの計測結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、移動体の移動方向に関する制約条件を利用して、カルマンフィルターを適用して慣性航法演算結果を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１０／００１９９６３号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の技術は、センサーの計測結果には誤差が含まれることを前提とし、移動体の移動方向に関する制約条件を利用して、慣性航法演算結果である移動体の位置や速度といった諸量に含まれる誤差を推定するものである。但し、この技術は、センサーの座標系と移動体の座標系との正確な対応関係を把握していることが前提である。
【０００６】
センサーの計測結果はセンサーの座標系（ローカル座標系）での値である。移動体の座標系での値ではない。従って、移動体の移動方向に関する制約条件を利用する場合には、センサーの座標系と移動体の座標系との間で座標変換を行うか、或いは、センサーの座標系と移動体の座標系とがぴったり一致するようにセンサーを移動体に固定する必要がある。しかし、キャリブレーションを行うとしても、センサーの座標系から移動体の座標系に正確に変換することは困難であるし、センサーの座標系と移動体の座標系とがぴったり一致するようにセンサーを移動体に固定するのも困難である。僅かではあるが誤差が発生し得るし、時間が経過すると誤差が大きくなり得る。
【０００７】
また、慣性航法用のセンサーを備えた携帯型電話機やナビゲーション装置、ＰＤＡ（Personal Digital Assistant）、パソコンといった持ち運び可能な電子機器を移動体に持ち込んで配置し、その電子機器で慣性航法演算を行う場合には、センサーの座標系と移動体の座標系との対応関係を正確に把握することは至難である。
【０００８】
センサーの座標系と移動体の座標系との間で正確な座標変換ができない場合は、センサーの座標系と移動体の座標系との間で辻褄が合わなくなるため、慣性航法演算結果に誤差が生じる。また、慣性航法演算を継続するにつれて、実際の位置と演算した位置との乖離が徐々に大きくなっていく傾向があった。
【０００９】
センサーの座標系と移動体の座標系との正確な対応関係を把握するためには、移動体に対するセンサーの正確な姿勢を判定する必要がある。
【００１０】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体に対するセンサーの姿勢を判定するための新たな手法を提案することにある。
【課題を解決するための手段】
【００１１】
以上の課題を解決するための第１の形態は、移動体に設置された移動ベクトルを計測するセンサーが前記移動ベクトルを計測することと、前記移動体が移動を開始した際に前記センサーが計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定することと、を含む姿勢判定方法である。
【００１２】
また、他の形態として、移動体に設置された移動ベクトルを計測するセンサーが前記移動体が移動を開始した際に計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定する姿勢判定装置を構成してもよい。
【００１３】
この第１の形態等によれば、移動体が移動を開始した際にセンサーが計測した移動ベクトルを用いて、移動体に対するセンサーの姿勢を判定する。移動ベクトルは、移動体の加速度ベクトルや速度ベクトルといった、移動体の移動に関するベクトルのことを意味する。また、移動体に対するセンサーの姿勢とは、移動体に対するセンサーの相対的な姿勢のことを意味する。実施形態で詳細に後述するが、移動体が移動を開始した際にセンサーにより計測された移動ベクトルを用いることで、移動体に対するセンサーの姿勢を判定することができる。
【００１４】
また、第２の形態として、第１の形態の姿勢判定方法であって、前記移動体が停止している場合に、前記移動体の速度に関する第１の制約条件を用いて、前記センサーの計測結果を補償すること、を更に含む姿勢判定方法を構成してもよい。
【００１５】
この第２の形態によれば、移動体が停止している場合に、移動体の速度に関する第１の制約条件を用いて、センサーの計測結果を補償する。これにより、第１の形態と相まって、移動体が停止している場合に補償された移動ベクトルを用いてセンサーの姿勢を判定することができるようになり、姿勢判定をより精確に行うことが可能となる。
【００１６】
また、第３の形態として、第１又は第２の形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体に対する前記姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することである、姿勢判定方法を構成してもよい。
【００１７】
この第３の形態によれば、移動体に対する姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することで、センサーの姿勢判定を簡易に行うことができる。
【００１８】
また、第４の形態として、第１〜第３の何れかの形態の姿勢判定方法であって、前記移動ベクトルを計測することは、前記移動体の移動空間を定める座標系である絶対座標系での移動ベクトルを計測することであり、前記姿勢を判定することは、前記移動ベクトルと、前記絶対座標系で表した前記センサーの向きとを用いて、前記センサーの前記姿勢を判定することである、姿勢判定方法を構成してもよい。
【００１９】
この第４の形態によれば、移動体の移動空間を定める絶対座標系での移動ベクトルと、絶対座標系で表したセンサーの向きとを用いて、移動体に対するセンサーの姿勢を判定する。絶対座標系で表したセンサーの向きが分かれば、絶対座標系での移動ベクトルを、センサーの座標系に座標変換することができる。そして、変換後の移動ベクトルを用いることで、センサーの姿勢を適切に判定することができる。
【００２０】
また、第５の形態として、第１〜第３の何れかの形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することと、前記速度ベクトルを用いて前記センサーの姿勢を判定することと、を含む姿勢判定方法を構成してもよい。
【００２１】
この第５の形態によれば、前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することで、前記センサーのバイアス誤差等を取り除いた速度ベクトルを算出することが可能となる。
【００２２】
また、第６の形態として、第１〜第５の何れかの形態の姿勢判定方法であって、前記姿勢を判定することは、前記移動体の停止が解除されて前記移動体の速度又は加速度が所定の閾値に達した時の前記移動ベクトルを用いて前記姿勢を判定することである、姿勢判定方法を構成してもよい。
【００２３】
この第６の形態によれば、移動体の停止が解除されて移動体の速度又は加速度が所定の閾値に達した時の移動ベクトルを用いて姿勢を判定する。移動体の停止が解除されてもすぐには姿勢判定を行わず、移動体の速度又は加速度がある程度の大きさとなった時点で姿勢判定を行うようにすることで、姿勢判定の正確性を向上させることができる。
【００２４】
また、第７の形態として、第１〜第６の何れかの形態の姿勢判定方法を用いて前記姿勢を判定することと、前記姿勢と前記移動ベクトルとを用いて前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【００２５】
この第７の形態によれば、上記の形態の姿勢判定方法を用いて判定した姿勢と移動ベクトルとを用いることで、移動体の位置を適切に算出することができる。
【００２６】
また、第８の形態として、第７の形態の位置算出方法であって、前記位置を算出することは、前記移動ベクトルを用いて前記絶対座標系での前記移動体の絶対座標位置を算出することと、前記姿勢と前記絶対座標系で表した前記センサーの向きとを用いて、前記絶対座標位置を前記移動体を基準とする移動体座標位置に座標変換することと、前記移動体座標位置と、前記移動体の移動方向に関する第２の制約条件とを用いて、前記絶対座標位置を補正することと、を含む、位置算出方法を構成してもよい。
【００２７】
この第８の形態によれば、移動ベクトルを用いて絶対座標系での移動体の絶対座標位置を算出する。例えば、絶対座標系での移動ベクトルを積分することで、移動体の位置を算出する。絶対座標系と移動体を基準とする座標系とを結び付けるためには、移動体に対するセンサーの相対的な姿勢と、絶対座標系で表したセンサーの向きとが必要となる。そこで、上記の形態の姿勢判定方法を用いて判定したセンサーの姿勢と、絶対座標系で表したセンサーの向きとを用いて、絶対座標位置を移動体座標位置に座標変換する。これにより、移動体を基準とする座標系での演算が可能となる。その上で、移動体座標位置と移動体の移動方向に関する第２の制約条件とを用いることで、移動体の絶対座標位置を適切に補正することができる。
【図面の簡単な説明】
【００２８】
【図１】全体システムのシステム構成図。
【図２】第１の取付姿勢判定システムのシステム構成図。
【図３】第２の取付姿勢判定システムのシステム構成図。
【図４】第１の位置算出システムのシステム構成図。
【図５】第１の慣性航法システムのシステム構成図。
【図６】第２の位置算出システムのシステム構成図。
【図７】第２の慣性航法システムのシステム構成図。
【図８】従来の手法で位置算出を行ったシミュレーション結果の一例。
【図９】実施形態の手法で位置算出を行ったシミュレーション結果の一例。
【図１０】カーナビゲーション装置の機能構成を示すブロック図。
【図１１】処理部を機能ブロックとして表した概念図。
【図１２】ナビゲーション処理の流れを示すフローチャート。
【図１３】補正処理の流れを示すフローチャート。
【図１４】従来のナビゲーション処理による位置算出の実験結果の一例。
【図１５】実施例のナビゲーション処理による位置算出の実験結果の一例。
【図１６】第２のナビゲーション処理の流れを示すフローチャート。
【図１７】進行方向切替対応ロジックを適用しない場合の実験結果の一例。
【図１８】進行方向切替対応ロジックを適用した場合の実験結果の一例。
【図１９】変形例における処理部を機能ブロックとして表した概念図。
【図２０】変形例における処理部を機能ブロックとして表した概念図。
【図２１】第２の取付姿勢判定処理の流れを示すフローチャート。
【発明を実施するための形態】
【００２９】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００３０】
１．原理
１−１．全体システム
図１は、本実施形態における姿勢判定方法及び位置算出方法に係る全体システムのシステム構成の一例を示す図である。全体システムは、センサー３が搭載された移動体を有するシステムとして構成される。移動体は、自転車や自動車（四輪自動車及びバイクを含む）、電車、飛行機、船等の何れでもよいが、本実施形態では四輪自動車として説明する。
【００３１】
本実施形態では、３種類の座標系を定義する。第１の座標系は、センサー３に対応付けられたセンサー座標系であるローカル座標系である。本明細書では、ローカル座標系のことをＬ（Local）フレームとも言う。ローカル座標系は、センサー３に対応付けられた三次元直交座標系であり、本実施形態では、ローカル座標系の３軸をｘ軸、ｙ軸及びｚ軸と表記する。
【００３２】
第２の座標系は、移動体に対応付けられた移動体座標系である。本明細書では、移動体座標系のことをＶ（Vehicle）フレームとも言う。Ｖフレームは、例えば、移動体の前方を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする三次元直交座標系として定義される。本実施形態では、移動体座標系の３軸をＲ軸、Ｐ軸及びＱ軸と表記する。
【００３３】
第３の座標系は、移動体の移動空間を定める座標系である絶対座標系である。本明細書では、絶対座標系のことをＡ（Absolute）フレームとも言う。Ａフレームは、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系として定義される。本実施形態では、絶対座標系の３軸をＸ軸、Ｙ軸及びＺ軸と表記する。
【００３４】
センサー３は、移動体に搭載される各種のセンサーである。例えば、センサー３の計測結果を利用して自立的に位置算出や速度算出を行う慣性航法システムに適用する場合は、センサー３として、加速度センサー３Ａや姿勢センサー３Ｂといった慣性航法用のセンサー（慣性センサー）が用いられる。
【００３５】
加速度センサー３Ａは、移動体の加速度をローカル座標系で検出するセンサーであり、例えばＭＥＭＳ（Micro Electro Mechanical Systems）の技術を利用したＭＥＭＳセンサーが用いられる。加速度センサー３Ａは移動体に固定される。加速度センサー３Ａが計測した値は、ローカル座標系で計測した移動体の加速度となる。
【００３６】
姿勢センサー３Ｂは、絶対座標系におけるセンサー３の向きである絶対姿勢を検出するためのセンサーであり、例えばジャイロセンサー３Ｃや磁気センサーといったセンサーが用いられる。姿勢センサー３Ｂは、ジャイロセンサー３Ｃや磁気センサー単体として構成してもよいし、これらを組み合わせて構成してもよい。また、加速度センサーを含んで姿勢センサー３Ｂを構成してもよい。姿勢センサー３Ｂは移動体に固定される。姿勢センサー３Ｂが計測した値は、絶対座標系で表したセンサー３の向きとなる。
【００３７】
移動体の加速度や速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則として、加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。また、本実施形態では、移動体の加速度ベクトル及び速度ベクトルを総称して、移動体の「移動ベクトル」と呼ぶ。なお、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。
【００３８】
また、センサー３の姿勢は、ロール角、ピッチ角及びヨー角といったオイラー角で表される。絶対座標系で表したセンサー３の向きを「絶対姿勢」と表現し、その姿勢角のことを「絶対姿勢角」と称する。また、移動体に対するセンサー３の相対的な姿勢（相対姿勢）のことを「取付姿勢」と表現し、その姿勢角（相対姿勢角）のことを「取付姿勢角」と称する。
【００３９】
また、以下の説明で参照する図面については、各種のセンサー３のブロックを二重線で示し、センサー３の計測結果を利用して演算処理を行う処理ブロックを一重線で示す。一重線で示される処理ブロックは、センサー３に搭載されるプロセッサーや、電子機器に搭載されるプロセッサー（ホストプロセッサー）が処理主体となる処理ブロックである。なお、各処理ブロックの処理の主体は、本発明を適用するシステムに応じて適宜設定可能であるものとする。
【００４０】
１−２．取付姿勢判定方法
本実施形態における取付姿勢判定方法について説明する。センサー３が移動体に取り付けられた場合に、移動体に対するセンサー３の相対的な姿勢である取付姿勢を判定することが１つの目的である。本実施形態では、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、センサー３の取付姿勢を判定する。前述したように、移動ベクトルには、速度ベクトルや加速度ベクトルが含まれる。そのため、本実施形態では、移動体が移動を開始した際に計測された速度ベクトルや加速度ベクトルを用いて、センサー３の取付姿勢を判定することになる。
【００４１】
（１）第１の取付姿勢判定方法
図２は、第１の取付姿勢判定方法に係る第１の取付姿勢判定システム１Ａ（第１の姿勢判定装置）のシステム構成図である。第１の取付姿勢判定システム１Ａは、例えば、加速度センサー３Ａと、ローカル座標速度ベクトル算出部５と、取付姿勢判定部１０とを有する。
【００４２】
ローカル座標速度ベクトル算出部５は、加速度センサー３Ａから入力したローカル座標加速度ベクトルを積分及び継ぎ足し処理して、ローカル座標速度ベクトルを算出する。本実施形態において、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【００４３】
具体的には、ローカル座標速度ベクトル算出部５は、所定時間分のローカル座標加速度ベクトルを積分することで、その所定時間の間におけるローカル座標系で表した速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新したローカル座標速度ベクトルに継ぎ足すことで、ローカル座標速度ベクトルを新たに算出・更新する。
【００４４】
取付姿勢判定部１０は、ローカル座標速度ベクトル算出部５から入力したローカル座標速度ベクトルを用いて取付姿勢を判定する。取付姿勢判定部１０は、移動体が停止している状態から移動を開始した際に計測される速度ベクトルを用いて、移動体に対するセンサー３の姿勢（取付姿勢）を判定する。
【００４５】
本実施形態では、移動体が停止している場合に、移動体の速度に関する制約条件（第１の制約条件）を用いて、センサー３の計測結果を補償する。移動体が停止しているのであれば、理想的には、移動体の速度ベクトルはゼロベクトルとなる。よって、移動体が停止している場合には、移動体の速度ベクトルの各成分がゼロであるとする運動モデルを適用することができる。但し、ここで言う移動体の速度ベクトルとは、移動体座標系（Ｖフレーム）での速度ベクトルである。
【００４６】
上記の運動モデルに基づいて、移動体の速度ベクトルを補正する手法が適用可能である。具体的には、停止中は移動体の速度ベクトルの各成分がゼロであるとする停止時速度制約条件を与えて、例えばカルマンフィルター（ＫＦ（Kalman Filter））を利用して、センサー３の計測結果を補正する手法が挙げられる。
【００４７】
カルマンフィルターは、時々刻々と変化する外部観測量を利用して、確率理論に基づいて系の状態を推定する推定手法の１つである。例えば、推定対象とする「状態」をセンサー３の計測結果に含まれる誤差とし、上記の速度制約条件を「観測情報」として利用して、センサー３の計測結果を補正する手法が考えられる。また、カルマンフィルター以外にも、上記の停止時速度制約条件に基づいて、センサー３の計測結果にリセットをかけるなどの手法も考えられる。つまり、停止時速度制約条件を用いて、センサー３をキャリブレーションするのである。
【００４８】
上記のように、移動体が停止している場合に、移動体の速度に関する制約条件（第１の制約条件）を用いてセンサー３の計測結果を補償することで、センサー３の計測結果に含まれる誤差を低減させる（小さくする）ことができる。誤差が低減された状態で移動体が移動を開始すると、暫くの間は、センサー３の計測結果は誤差が低減された状態となる。そこで、本実施形態では、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢（取付姿勢）を判定する。
【００４９】
ローカル座標系で表した速度ベクトル（ローカル座標速度ベクトル）と、移動体座標系で表した速度ベクトル（移動体座標速度ベクトル）との間には、次式（１）のような関係がある。
【数１】
【００５０】
本明細書における数式では、速度ベクトルの成分を小文字の“ｖ”で表す。また、速度ベクトルの表記において、座標系の種類を上付きの大文字の添え字で示す。“Ｌ”はローカル座標系、“Ｖ”は移動体座標系、“Ａ”は絶対座標系をそれぞれ意味する。また、対応する座標系の各軸の成分を下付きの小文字の添え字で示す。例えば、ローカル座標系の３軸であれば“ｘ”、“ｙ”及び“ｚ”の表記を用いる。
【００５１】
また、本明細書では、座標変換行列を大文字の“Ｃ”で表す。各座標変換行列において、下付きの添え字は座標変換前の座標系を示し、上付きの添え字は座標変換後の座標系を示す。例えば、“ＣLV”は、ローカル座標系（Ｌフレーム）から移動体座標系（Ｖフレーム）への座標変換行列を示し、“ＣAL”は、絶対座標系（Ａフレーム）からローカル座標系（Ｌフレーム）への座標変換行列を示す。
【００５２】
ここで、移動体の移動方向に関する制約条件（第２の制約条件）を適用する。例えば、移動体が四輪自動車であれば、通常走行において四輪自動車はジャンプやドリフトすることはないと仮定することができる。つまり、移動体の縦横方向の速度成分はゼロであると仮定し、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロであるとする制約条件を設定する。つまり、移動体の速度に関して｛ｖPV＝ｖQV＝０｝とする制約条件を設定する。
【００５３】
この場合、式（１）から、取付姿勢のピッチ成分であるピッチ角“θ”及びヨー成分であるヨー角“Ψ”を、次式（２）及び（３）のように算出することができる。
【数２】
【数３】
【００５４】
ピッチ角“θ”及びヨー角“Ψ”は、それぞれ移動体座標系のＰ軸及びＱ軸回りのセンサー３の回転角を示す。つまり、ピッチ角“θ”及びヨー角“Ψ”は、移動体に対するセンサー３の相対的な姿勢角を表していると言える。
【００５５】
なお、自動車が移動を開始した直後は、自動車の速度はゼロに近い値となる。この状態で式（２）及び（３）に従って取付姿勢角を算出すると、浮動小数点数同士の演算により、十分な演算精度が保証されない可能性がある。そこで、自動車が移動を開始してもすぐには取付姿勢角を算出せず、速度又は加速度がある程度大きくなった時点で、取付姿勢を判定すると効果的である。例えば、自動車のブレーキ制御が解除された直後から所定時間（１秒以下の時間として定められる。）経過時に取付姿勢を判定することとしたり、ブレーキ制御が解除された後に「１ｋｍ／ｈ以上２０ｋｍ／ｈ以下」の所定速度に到達した際に取付姿勢を判定するなどすると効果的である。
【００５６】
（２）第２の取付姿勢判定方法
図３は、第２の取付姿勢判定方法に係る第２の取付姿勢判定システム１Ｂ（第２の姿勢判定装置）のシステム構成図である。第２の取付姿勢判定システム１Ｂは、例えば、加速度センサー３Ａと、姿勢センサー３Ｂと、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、取付姿勢判定部１０とを有する。本実施形態において、加速度センサー３Ａ及び姿勢センサー３Ｂは、共通のローカル座標系での値を計測するように、例えば一体的に構成される。
【００５７】
ＬＡ座標変換行列算出部２０は、姿勢センサー３Ｂから入力したセンサー３の絶対姿勢角（絶対座標系で表したセンサー３の向き）を用いて、ローカル座標系から絶対座標系への座標変換行列（以下、「ＬＡ座標変換行列」と称する。）を算出する。なお、ＬＡ座標変換行列それ自体は公知であるため、数式等を用いた説明は省略する。
【００５８】
絶対座標加速度ベクトル算出部３０は、ＬＡ座標変換行列算出部２０から入力したＬＡ座標変換行列を用いて、加速度センサー３Ａから入力したローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する。
【００５９】
絶対座標速度ベクトル算出部４０は、絶対座標加速度ベクトル算出部３０から入力した絶対座標加速度ベクトルを積分及び継ぎ足し処理することで、絶対座標速度ベクトルを算出する。つまり、所定時間分の絶対座標加速度ベクトルを積分することで、その所定時間の間の移動体の速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新した絶対座標速度ベクトルに継ぎ足すことで、絶対座標速度ベクトルを新たに算出・更新する。
【００６０】
取付姿勢判定部１０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルと、姿勢センサー３Ｂから入力した絶対姿勢角とを用いて、加速度センサー３Ａの取付姿勢を判定する。取付姿勢判定部１０は、絶対座標系からローカル座標系への座標変換行列（以下、「ＡＬ座標変換行列」と称す。）を算出するＡＬ座標変換行列算出部１１を機能部として有する。
【００６１】
絶対座標系で表した速度ベクトル（絶対座標速度ベクトル）と、移動体座標系で表した速度ベクトル（移動体座標速度ベクトル）との間には、次式（４）のような関係がある。
【数４】
【００６２】
但し、“ＣAL”はＡＬ座標変換行列を示す。また、“ＣLV”はローカル座標系（Ｌフレーム）から移動体座標系（Ｖフレーム）への座標変換行列（以下、「ＬＶ座標変換行列」と称す。）を示す。
【００６３】
式（４）から、絶対座標系と移動体座標系とを結び付けるためには、２段階の座標変換が必要であることがわかる。つまり、第１段階として、ＡＬ座標変換行列“ＣAL”を用いて、絶対座標速度ベクトルをローカル座標速度ベクトルに座標変換する。その後、第２段階として、ＬＶ座標変換行列“ＣLV”を用いて、ローカル座標速度ベクトルを移動体座標速度ベクトルに座標変換する。
【００６４】
ＡＬ座標変換行列“ＣAL”は、絶対座標系で表したセンサー３の向き（絶対姿勢）により定まる行列であり、ＬＡ座標変換行列算出部２０により算出されるＬＡ座標変換行列の逆行列である。この場合、ＡＬ座標変換行列を用いて絶対座標速度ベクトルをローカル座標速度ベクトルに座標変換すると、式（４）は式（１）と一致する。つまり、次式（５）が成立するため、式（４）及び式（５）から、式（１）が導出される。
【数５】
【００６５】
このように式（１）を導出することができるため、第１の取付姿勢判定方法と同様に、取付姿勢を判定することができる。つまり、移動体座標系（Ｖフレーム）において、Ｑ軸方向及びＰ軸方向（縦横方向）の速度成分はゼロであるとする制約条件｛ｖPV＝ｖQV＝０｝に基づき、式（２）及び（３）に従って取付姿勢角を算出する。
【００６６】
上記を実現するため、取付姿勢判定部１０のＡＬ座標変換行列算出部１１は、姿勢センサー３Ｂから入力した絶対姿勢角を用いて、ＡＬ座標変換行列を算出する。そして、取付姿勢判定部１０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルを、ＡＬ座標変換行列算出部１１により算出されたＡＬ座標変換行列を用いてローカル座標速度ベクトルに座標変換する。そして、最終的に、ローカル座標速度ベクトルを用いて、式（２）及び式（３）に従って取付姿勢角を算出する。
【００６７】
１−３．位置算出方法
次に、上記の取付姿勢判定方法を用いて判定したセンサー３の取付姿勢と、移動ベクトルとを用いて、移動体の位置を算出する位置算出方法について説明する。
【００６８】
（１）第１の位置算出方法
図４は、第１の位置算出方法に係る第１の位置算出システム２Ａ（第１の位置算出装置）のシステム構成図である。第１の位置算出システム２Ａは、例えば、第１の慣性航法システム（第１のＩＮＳ（Inertial Navigation System））４Ａと、取付姿勢判定部１０と、移動状況判定部６０と、速度制約条件設定部７０と、ＬＶ座標変換行列算出部８０と、補正部９０とを有する。
【００６９】
第１の慣性航法システム４Ａは、ＩＮＳとして知られる慣性航法システムであり、そのシステム構成の一例を図５に示す。第１の慣性航法システム４Ａは、例えば、加速度センサー３Ａと、ジャイロセンサー３Ｃと、ローカル座標速度ベクトル算出部５と、絶対姿勢判定部１５と、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、絶対座標位置算出部５０とを有する。
【００７０】
ローカル座標速度ベクトル算出部５は、加速度センサー３Ａから入力したローカル座標加速度ベクトルを積分及び継ぎ足し処理することで、ローカル座標速度ベクトルを算出する。
【００７１】
絶対姿勢判定部１５は、ジャイロセンサー３Ｃから入力したローカル座標角速度を積分及び継ぎ足し処理することで、絶対姿勢角を判定する。具体的には、絶対座標系で表したセンサー３の所与の基準姿勢からの姿勢角変化分を、ジャイロセンサー３Ｃから入力したローカル座標角速度を積分することで算出する。そして、基準姿勢に姿勢角変化分を継ぎ足すことで、絶対姿勢角を新たに算出・更新する。
【００７２】
ＬＡ座標変換行列算出部２０、絶対座標加速度ベクトル算出部３０及び絶対座標速度ベクトル算出部４０は、図３と同様に、それぞれＬＡ座標変換行列、絶対座標加速度ベクトル及び絶対座標速度ベクトルを算出する。
【００７３】
絶対座標位置算出部５０は、絶対座標速度ベクトル算出部４０から入力した絶対座標速度ベクトルを積分及び継ぎ足し処理することで、移動体の絶対座標位置を算出する。つまり、所定時間分の絶対座標速度ベクトルを積分することで、その所定時間の間における位置変化分を算出する。そして、算出した位置変化分を最後に更新した絶対座標位置に継ぎ足すことで、絶対座標位置を新たに算出・更新する。
【００７４】
第１の慣性航法システム４Ａからは、ローカル座標速度ベクトル算出部５により算出されたローカル速度ベクトルと、絶対姿勢判定部１５により算出された絶対姿勢角と、絶対座標速度ベクトル算出部４０により算出された絶対座標速度ベクトルと、絶対座標位置算出部５０により算出された絶対座標位置とが出力される。
【００７５】
図４の説明に戻り、取付姿勢判定部１０は、第１の慣性航法システム４Ａから入力したローカル座標速度ベクトルを用いて、移動体に対する第１の慣性航法システム４Ａの取付姿勢を判定する。
【００７６】
移動状況判定部６０は、移動体の移動状況を判定する。移動体の移動状況判定は、種々の手法により実現可能である。例えば、慣性航法システムが具備する加速度センサー３Ａやジャイロセンサー３Ｃの計測結果を利用して判定を行う方法が挙げられる。また、移動体が自動車である場合には、自動車に搭載される車速検出システム（例えば車速パルス）により計測された車速を利用して、移動体の移動状況を判定してもよい。
【００７７】
速度制約条件設定部７０は、移動状況判定部６０から入力した移動状況に基づいて、補正部９０に適用する速度制約条件を設定する。本実施形態では、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との２種類の速度制約条件を切り替えて設定する。
【００７８】
停止時速度制約条件は、移動体の速度に関する第１の制約条件であり、移動体の停止時に適用する制約条件である。詳細には、移動体の停止時には、移動体の速度ベクトルはゼロベクトルになるという仮定に基づき設定される制約条件である。この停止時速度制約条件を適用する座標系は任意であるが、例えば絶対座標系で適用することが考えられる。この場合は、例えば｛ｖXA＝ｖYA＝ｖZA＝０｝を制約条件として与える。
【００７９】
移動時速度制約条件は、移動体の移動方向に関する第２の制約条件であり、移動体の移動時に適用する制約条件である。前述したように、移動体が四輪自動車であれば、通常走行時において移動体はジャンプやドリフトすることはないとする仮定に基づき設定される制約条件である。この場合は、例えば｛ｖPV＝ｖQV＝０｝を制約条件として与える。この制約条件は、移動体座標系において有効な条件である。
【００８０】
ＬＶ座標変換行列算出部８０は、取付姿勢判定部１０から入力した取付姿勢角を用いて、ローカル座標系から移動体座標系への座標変換行列（以下、「ＬＶ座標変換行列」と称する。）を算出する。なお、ＬＶ座標変換行列それ自体は公知であるため、数式等を用いた説明は省略する。
【００８１】
補正部９０は、第１の慣性航法システム４Ａから入力した慣性航法演算結果を補正する。補正部９０は、慣性航法演算結果のうち、少なくとも絶対座標位置を補正するように構成されていればよい。つまり、少なくとも絶対座標位置に対して誤差推定演算を行い、推定した位置誤差を用いて絶対座標位置を推定するように構成されていればよい。
【００８２】
本実施形態では、補正部９０が、絶対座標位置だけではなく、絶対座標速度ベクトル及び絶対姿勢角も併せて補正する場合を例示する。補正部９０は、誤差推定部９１と、ＡＬ座標変換行列算出部９３とを有する。
【００８３】
誤差推定部９１は、第１の慣性航法システム４Ａから入力した慣性航法演算結果に含まれる誤差（以下、「慣性航法演算誤差」と称す。）を、所定の誤差推定演算を行うことで推定する。慣性航法演算誤差の推定方法としては種々の手法を適用可能であるが、典型的な手法としては、カルマンフィルターを適用することができる。
【００８４】
具体的には、状態ベクトル“Ｘ”を、例えば次式（６）のように設定する。
【数６】
但し、“δＰ”、“δＶ”及び“δＡ”は、それぞれ絶対座標位置の誤差（位置誤差）、絶対座標速度ベクトルの誤差（速度ベクトル誤差）及び絶対姿勢角の誤差（姿勢角誤差）である。
【００８５】
移動体の停止時には、停止時速度制約条件に従って、例えば次式（７）のような停止時観測ベクトル“Ｚstop”を設定する。また、移動体の移動時には、移動時速度制約条件に従って、例えば次式（８）のような移動時観測ベクトル“Ｚmove”を設定する。
【数７】
【数８】
【００８６】
但し、式（８）において、｜Ｖ｜は速度の大きさを示す。速度の大きさは、慣性航法演算で得られた絶対座標速度ベクトルの大きさ（スカラー）として計算してもよいし、車速検出システム等の外部システムから取得した速度の大きさとして計算してもよい。
【００８７】
上記のように状態ベクトル“Ｘ”及び観測ベクトル“Ｚ”を設定し、カルマンフィルターの理論に基づく予測演算（時刻更新）及び補正演算（観測更新）を行って、状態ベクトル“Ｘ”の推定値（状態推定値）を求める。
【００８８】
上記の定義式において、状態ベクトル“Ｘ”は、絶対座標系で表した慣性航法演算誤差として定義されている。停止時観測ベクトル“Ｚstop”は絶対座標系で定義されているため、同じ座標系での演算が可能である。しかし、移動時観測ベクトル“Ｚmove”は移動体座標系で定義されているため、同じ座標系での演算が困難である。
【００８９】
そこで、ＡＬ座標変換行列算出部９３は、第１の慣性航法システム４Ａから入力した絶対姿勢角を用いて、ＡＬ座標変換行列を算出する。そして、算出したＡＬ座標変換行列と、ＬＶ座標変換行列算出部８０から入力したＬＶ座標変換行列とを用いて、絶対座標系で表した状態ベクトル“Ｘ”を、移動体座標系で表した状態ベクトル“Ｘ”に座標変換する。つまり、２種類の座標変換行列を用いて、状態ベクトル“Ｘ”に２段階の座標変換を施すことになる。
【００９０】
同様に、ＡＬ座標変換行列及びＬＶ座標変換行列を用いて、慣性航法演算結果である絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を、それぞれ移動体座標系で表した移動体座標位置、移動体座標速度ベクトル及び移動体姿勢角に座標変換する。つまり、２種類の座標変換行列を用いて、慣性航法演算結果に２段階の座標変換を施すことになる。
【００９１】
移動体座標系に座標変換した状態ベクトル“Ｘ”に対して、移動体座標系で定義した移動時観測ベクトル“Ｚmove”を適用して、カルマンフィルターを利用した誤差推定演算を行って慣性航法演算誤差を推定する。そして、移動体座標系で推定された慣性航法演算誤差を用いて、上記の移動体座標位置、移動体座標速度ベクトル及び移動体姿勢角をそれぞれ補正する。そして、移動体座標系から絶対座標系に再変換することで、最終的な補正結果を得る。
【００９２】
補正部９０から出力される補正位置、補正速度ベクトル及び補正姿勢角は、第１の慣性航法システム４Ａにフィードバックされる。第１の慣性航法システム４Ａでは、補正部９０から入力した補正位置、補正速度ベクトル及び補正姿勢角を用いて、それぞれ絶対座標位置算出部５０、絶対座標速度ベクトル算出部４０及び絶対姿勢判定部１５により算出された絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角が補正される。
【００９３】
（２）第２の位置算出方法
図６は、第２の位置算出方法に係る第２の位置算出システム２Ｂ（第２の位置算出装置）のシステム構成図である。第２の位置算出システム２Ｂは、例えば、第２の慣性航法システム（第２のＩＮＳ）４Ｂと、取付姿勢判定部１０と、移動状況判定部６０と、速度制約条件設定部７０と、ＬＶ座標変換行列算出部８０と、補正部９０とを有する。
【００９４】
図７は、第２の慣性航法システム４Ｂのシステム構成の一例を示す図である。第２の慣性航法システム４Ｂは、加速度センサー３Ａと、ジャイロセンサー３Ｃと、絶対姿勢判定部１５と、ＬＡ座標変換行列算出部２０と、絶対座標加速度ベクトル算出部３０と、絶対座標速度ベクトル算出部４０と、絶対座標位置算出部５０とを有する。図５の第１の慣性航法システム４Ａにおいて、ローカル座標速度ベクトル算出部５を除いた構成である。
【００９５】
第２の慣性航法システム４Ｂからは、絶対姿勢判定部１５により算出された絶対姿勢角と、絶対座標速度ベクトル算出部４０により算出された絶対座標速度ベクトルと、絶対座標位置算出部５０により算出された絶対座標位置とが出力される。
【００９６】
図６の説明に戻り、取付姿勢判定部１０は、第２の慣性航法システム４Ｂから入力した絶対座標速度ベクトルを用いて、移動体に対する第２の慣性航法システム４Ｂの取付姿勢を判定する。取付姿勢判定部１０は、ＡＬ座標変換行列算出部１１を機能部として有し、図３の第２の取付姿勢判定システム１Ｂと同様に、取付姿勢を判定する。
【００９７】
補正部９０は、取付姿勢判定部１０から入力した取付姿勢角及びＡＬ座標変換行列と、速度制約条件設定部７０から入力した速度制約条件と、ＬＶ座標変換行列算出部８０から入力したＬＶ座標変換行列とを用いて、第２の慣性航法システム４Ｂから入力した慣性航法演算結果を補正する。補正部９０は、誤差推定部９１を機能部として有する。
【００９８】
補正部９０からは、慣性航法演算結果に対する補正結果である補正位置、補正速度ベクトル及び補正姿勢角が出力される。また、これらの補正結果は、第２の慣性航法システム４Ｂにフィードバックされる。第２の慣性航法システム４Ｂは、補正部９０から入力した補正結果を利用して、慣性航法演算結果を補正する。
【００９９】
１−４．シミュレーション結果
上記の位置算出方法の有効性を示すために、移動体の位置算出を行うシミュレーション実験を行った結果について説明する。設置ずれが生ずるようにセンサー３を移動体に取り付け、上記の位置算出方法を用いて移動体の位置を算出するシミュレーション実験を行った。
【０１００】
図８は、従来の手法を用いたシミュレーション結果を示す。それに対し、図９は、本実施形態の手法を用いたシミュレーション結果を示す。各図において、横軸は東西方向、縦軸は南北方向を示し、単位はメートル［ｍ］である。東西方向“０ｍ”、南北方向“０ｍ”の位置をスタート地点とし、大きく反時計回り方向に１周周回するようなダイヤ型の経路を辿った。ゴール地点はスタート地点と同じである。移動体の真の軌跡を破線で示し、算出した位置の軌跡を実線で示す。
【０１０１】
図８を見ると、従来の手法を用いた場合は、重大な位置誤差が生じていることがわかる。つまり、ダイヤ型の軌跡に沿って位置が算出されるどころか、スタート直後から東方向に向かって位置が逸れていき、最終的には、ゴール地点から程遠い位置をゴール地点と判定していることがわかる。これは、センサー３の取付姿勢を判定せずに位置算出を行った結果である。つまり、ローカル座標系と移動体座標系とが一致していないにも関わらず、移動体座標系で定義される移動時速度制約条件（第２の制約条件）を適用して位置算出を行ったために、当初から誤った方向に位置が算出され、時間経過に伴って誤差が累積的に増加した結果である。
【０１０２】
それに対し、図９を見ると、本実施形態の手法を用いた場合は、ダイヤ型の真の軌跡に沿ったほぼ正確な軌跡が得られていることがわかる。これは、センサー３の取付姿勢を可能な限り正確に判定して、位置算出を行うことができた結果である。つまり、センサー３の取付姿勢を判定した上で、ローカル座標系と移動体座標系とのズレ分を考慮して移動体座標系への座標変換を行い、移動体座標系で定義される移動時速度制約条件を適用して位置算出を行ったため、このような良好な結果が得られたのである。
【０１０３】
２．実施例
次に、上記の姿勢判定方法及び位置算出方法を適用したナビゲーションシステムの実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【０１０４】
本実施例におけるナビゲーションシステムは、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、原理で説明した取付姿勢判定システム（姿勢判定装置）及び位置算出システム（位置算出装置）を適用したカーナビゲーション装置１０００が設置されたシステムとして構成される。
【０１０５】
カーナビゲーション装置１０００は、自動車に搭載される電子機器であり、加速度センサー３Ａ及びジャイロセンサー３Ｃを有するセンサーユニットとしてＩＭＵ（Inertial Measurement Unit）４００を具備している。ＩＭＵ４００は、慣性計測ユニットとして知られるセンサーユニットであり、ローカル座標系で表した加速度ベクトル及び角速度を計測して出力可能に構成されている。
【０１０６】
カーナビゲーション装置１０００は、ＩＭＵ４００の計測結果を利用して慣性航法演算処理を行うことで、絶対座標系での自動車の位置を算出する。本実施例では、絶対座標系としてＮＥＤ座標系を適用する場合について説明する。つまり、カーナビゲーション装置１０００は、ＮＥＤ座標系での自動車の位置を算出する。そして、カーナビゲーション装置１０００は、算出位置をプロットしたナビゲーション画面を生成し、当該ナビゲーション画面を表示部３００であるディスプレイに表示させる。
【０１０７】
２−１．機能構成
図１０は、カーナビゲーション装置１０００の機能構成の一例を示すブロック図である。カーナビゲーション装置１０００は、主要な機能構成として、処理部１００と、操作部２００と、表示部３００と、ＩＭＵ４００と、記憶部５００とを備えて構成される。
【０１０８】
処理部１００は、記憶部５００に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１０００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。処理部１００は、ＩＭＵ４００の計測結果を利用した慣性航法演算処理を行って自動車の位置等を算出する。処理部１００は、自動車に対するＩＭＵ４００の姿勢を判定する姿勢判定装置として機能するとともに、自動車の位置を算出する位置算出装置として機能する。
【０１０９】
図１１は、処理部１００に係る機能ブロックを中心に示したブロック図である。処理部１００は、慣性航法演算部１０５と、取付姿勢判定部１１０と、移動状況判定部１６０と、速度制約条件設定部１７０と、ＬＶ座標変換行列算出部１８０と、補正部１９０とを機能部として有する。
【０１１０】
慣性航法演算部１０５は、ＩＭＵ４００から入力したローカル座標加速度ベクトル及びローカル座標角速度を用いて、慣性航法演算処理を行って自動車のＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角を算出する。
【０１１１】
補正部１９０は、取付姿勢判定部１１０から入力したＩＭＵ４００の取付姿勢角と、速度制約条件設定部１７０から入力した速度制約条件とを用いて、慣性航法演算部１０５から入力したＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角を補正する。補正部１９０は、ＫＦ誤差推定部１９１と、ＮＬ座標変換行列１９３とを機能部として有する。
【０１１２】
ＫＦ誤差推定部１９１は、カルマンフィルターを利用してＮＥＤ座標系での慣性航法演算結果に含まれる慣性航法演算誤差を推定する。
【０１１３】
ＮＬ座標変換行列算出部１９３は、慣性航法演算部１０５から入力した絶対姿勢角を用いて、ＮＥＤ座標系からローカル座標系への座標変換行列（以下、「ＮＬ座標変換行列」と称す。）を算出する。
【０１１４】
図１０の説明に戻り、操作部２００は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１００に出力する。この操作部２００の操作により、目的地の入力等の各種指示入力がなされる。
【０１１５】
表示部３００は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１００から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３００には、ナビゲーション画面等が表示される。
【０１１６】
ＩＭＵ４００は、慣性計測ユニットとして知られるセンサーユニットであり、例えば加速度センサー３Ａ及びジャイロセンサー３Ｃを有して構成される。
【０１１７】
記憶部５００は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１０００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１８】
２−２．データ構成
図１０に示すように、記憶部５００には、プログラムとして、処理部１００により読み出され、ナビゲーション処理（図１２参照）として実行されるナビゲーションプログラム５１０が記憶されている。ナビゲーションプログラム５１０は、補正処理（図１３参照）として実行される補正プログラム５１１をサブルーチンとして含む。これらの処理については、フローチャートを用いて詳細に後述する。
【０１１９】
また、記憶部５００には、データとして、速度制約条件５２０と、ＩＭＵ計測データ５３０と、慣性航法演算データ５４０と、取付姿勢データ５５０と、座標変換行列データ５６０と、推定誤差データ５７０と、補正慣性航法演算データ５８０とが記憶される。
【０１２０】
速度制約条件５２０は、自動車の移動状況に応じた速度の制約条件であり、停止時速度制約条件５２１と、移動時速度制約条件５２３とがこれに含まれる。
【０１２１】
ＩＭＵ計測データ５３０は、ＩＭＵ４００の計測結果のデータである。加速度センサー３Ａにより計測されたローカル座標加速度ベクトルと、ジャイロセンサー３Ｃにより計測されたローカル座標角速度とが時系列に記憶される。
【０１２２】
慣性航法演算データ５４０は、処理部１００がＩＭＵ４００の計測結果を利用して慣性航法演算処理を行うことで算出したＮＥＤ座標位置、ＮＥＤ座標速度ベクトル及び絶対姿勢角のデータである。
【０１２３】
取付姿勢データ５５０は、処理部１００により算出されたＩＭＵ４００の取付姿勢角が記憶されたデータである。座標変換行列データ５６０は、処理部１００が算出した各種の座標変換行列が記憶されたデータである。
【０１２４】
推定誤差データ５７０は、処理部１００がＫＦ誤差推定演算を行って推定した慣性航法演算誤差が記憶されたデータである。また、補正慣性航法演算データ５８０は、処理部１００が、慣性航法演算誤差を用いて慣性航法演算結果を補正したデータである。
【０１２５】
２−３．処理の流れ
図１２は、記憶部５００に記憶されているナビゲーションプログラム５１０が処理部１００により読み出されて実行されることで、カーナビゲーション装置１０００において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、ＩＭＵ４００により計測されたローカル座標加速度ベクトル及びローカル座標角速度が、記憶部５００のＩＭＵ計測データ５３０に随時記憶されるものとする。
【０１２６】
最初に、処理部１００は、初期設定を行う（ステップＡ１）。例えば、カルマンフィルターで利用する各種パラメーター値の初期設定を行う。また、自動車が停止している状態であるとして停止時速度制約条件５２１を「ＯＮ」に設定し、移動時速度制約条件５２３を「ＯＦＦ」に設定する。
【０１２７】
次いで、慣性航法演算部１０５が、慣性航法演算処理を開始する（ステップＡ３）。つまり、慣性航法演算部１０５は、ＩＭＵ計測データ５３０に記憶されているＩＭＵ４００の計測データを利用して慣性航法演算処理を行う。そして、その結果を慣性航法演算データ５４０として記憶部５００に記憶させる。
【０１２８】
その後、補正部１９０は、記憶部５００に記憶されている補正プログラム５１１に従って補正処理を開始する（ステップＡ５）。
【０１２９】
図１３は、補正処理の流れを示すフローチャートである。
先ず、ＫＦ誤差推定部１９１は、移動時速度制約条件５２３が「ＯＮ」に設定されているか否かを判定する（ステップＢ１）。そして、移動時速度制約条件５２３が「ＯＮ」に設定されていると判定した場合は（ステップＢ１；Ｙｅｓ）、ＮＬ座標変換行列算出部１９３が、最新の絶対姿勢角を用いてＮＬ座標変換行列を算出し、記憶部５００の座標変換行列データ５６０に記憶させる（ステップＢ３）。
【０１３０】
次いで、ＫＦ誤差推定部１９１は、ステップＢ３で算出したＮＬ座標変換行列を用いて、ＮＥＤ座標速度ベクトルを移動体座標速度ベクトルに変換する（ステップＢ５）。そして、ＫＦ誤差推定部１９１は、移動時制約速度ベクトルと、移動体座標速度ベクトルとの差を算出して、移動時観測ベクトルとする（ステップＢ７）。
【０１３１】
その後、ＫＦ誤差推定部１９１は、ＫＦ誤差推定処理を行う（ステップＢ９）。すなわち、ステップＢ７で設定した移動時観測ベクトルを観測情報として、予測演算及び補正演算を行って慣性航法演算誤差を推定する。そして、ＫＦ誤差推定部１９１は、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正し、その結果を記憶部５００の補正慣性航法演算データ５８０に記憶させた後（ステップＢ１１）、ステップＢ１に戻る。
【０１３２】
また、ステップＢ１において移動時速度制約条件５２３が「ＯＮ」に設定されていないと判定した場合は（ステップＢ１；Ｎｏ）、ＫＦ誤差推定部１９１は、停止時速度制約条件５２１が「ＯＮ」に設定されているか否かを判定する（ステップＢ１３）。そして、「ＯＮ」に設定されていると判定したならば（ステップＢ１３；Ｙｅｓ）、ＫＦ誤差推定部１９１は、停止時制約速度ベクトルと、ＮＥＤ座標速度ベクトルとの差を算出して、停止時観測ベクトルとする（ステップＢ１５）。
【０１３３】
その後、ＫＦ誤差推定部１９１は、ＫＦ誤差推定処理を行う（ステップＢ１７）。すなわち、ステップＢ１５で設定した停止時観測ベクトルを観測情報として、予測演算及び補正演算を行って慣性航法演算誤差を推定する。そして、ＫＦ誤差推定部１９１は、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正し、その結果を記憶部５００の補正慣性航法演算データ５８０に記憶させた後（ステップＢ１９）、ステップＢ１に戻る。
【０１３４】
また、ステップＢ１３において停止時速度制約条件５２１が「ＯＮ」に設定されていないと判定した場合は（ステップＢ１３；Ｎｏ）、補正部１９０は、ステップＢ１に戻る。
【０１３５】
図１２の説明に戻り、ステップＡ５において補正処理を開始したならば、移動状況判定部１６０は、自動車の移動状況を判定する（ステップＡ７）。そして、処理部１００は、自動車が停止状態から移動を開始したか否かを判定する（ステップＡ９）。例えば、自動車のブレーキ制御の解除信号を外部入力したり、車速情報を外部入力する等して判定する。
【０１３６】
自動車が移動を開始したと判定した場合は（ステップＡ９；Ｙｅｓ）、速度制約条件設定部１７０は、停止時速度制約条件５２１を「ＯＦＦ」に設定する（ステップＡ１１）。そして、処理部１００は、所定の取付姿勢判定発動条件が成立したか否かを判定する（ステップＡ１３）。取付姿勢判定発動条件は、原理でも説明したように、取付姿勢の判定精度を保証するために定める条件である。例えば、移動体の停止が解除された後、移動体の速度又は加速度が所定の閾値に達することを発動条件として定めておくと好適である。
【０１３７】
ステップＡ１３において、未だ取付姿勢判定発動条件が成立していないと判定した場合は（ステップＡ１３；Ｎｏ）、処理部１００は、ステップＡ２７へと処理を移行する。取付姿勢判定発動条件が成立したと判定した場合は（ステップＡ１３；Ｙｅｓ）、取付姿勢判定部１１０は、取付姿勢判定処理を行う（ステップＡ１５）。つまり、最新のローカル座標速度ベクトルを用いて、式（２）及び式（３）に従って取付姿勢角を算出し、記憶部５００の取付姿勢データ５５０を更新する。
【０１３８】
次いで、処理部１００は、ステップＡ１５で算出した取付姿勢角を用いてＬＶ座標変換行列を算出し、記憶部５００の座標変換行列データ５６０を更新する（ステップＡ１７）。そして、処理部１００は、移動時速度制約条件５２３を「ＯＮ」に設定する（ステップＡ１９）。
【０１３９】
一方、ステップＡ９において自動車が移動を開始したことを検知しなかった場合は（ステップＡ９；Ｎｏ）、処理部１００は、自動車が移動状態から停止状態に変化したか否かを判定する（ステップＡ２１）。停止状態に変化しなかったと判定した場合は（ステップＡ２１；Ｎｏ）、処理部１００は、ステップＡ２７に移行する。
【０１４０】
また、自動車が移動状態から停止状態に変化したと判定した場合は（ステップＡ２１；Ｙｅｓ）、処理部１００は、移動時速度制約条件５２３を「ＯＦＦ」に設定するとともに（ステップＡ２３）、停止時速度制約条件５２１を「ＯＮ」に設定する（ステップＡ２５）。そして、処理部１００は、ステップＡ２７に移行する。
【０１４１】
処理部１００は、位置の出力タイミングであるか否かを判定し（ステップＡ２７）、出力タイミングではないと判定した場合は（ステップＡ２７；Ｎｏ）、ステップＡ３１へと処理を移行する。また、出力タイミングであると判定した場合は（ステップＡ２７；Ｙｅｓ）、処理部１００は、最新の補正位置を出力する（ステップＡ２９）。つまり、最新の補正位置に対してマップマッチング処理等を行い、表示部３００のナビゲーション画面を更新する。
【０１４２】
次いで、処理部１００は、ナビゲーション処理を終了するか否かを判定する（ステップＡ３１）。例えば、操作部２００を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。そして、まだ処理を終了しないと判定した場合は（ステップＡ３１；Ｎｏ）、処理部１００は、ステップＡ７に戻る。また、処理を終了すると判定した場合は（ステップＡ３１；Ｙｅｓ）、処理部１００は、ナビゲーション処理を終了する。
【０１４３】
２−４．実験結果
上記のカーナビゲーション装置１０００を自動車に搭載し、実際に自動車を走行させて位置を算出する実験を行った。具体的には、上記のナビゲーション処理に従って自動車の位置を算出し、その位置をプロットする実験を行った。
【０１４４】
図１４は、従来のカーナビゲーション装置１０００による実験結果を示す。それに対し、図１５は、本実施例のカーナビゲーション装置１０００による実験結果を示す。但し、何れの場合もマップマッチング機能はオフとしている。図の見方は、図８及び図９のシミュレーション結果と同じであるが、実際に走行実験を行った結果であるため、経路が異なっている。東西方向“０ｍ”、南北方向“０ｍ”の位置をスタート地点とし、東方向“１２０００ｍ”、南方向“１００００ｍ”の近傍位置をゴール地点として、スタート地点からゴール地点まで自動車を実際に走行させた。
【０１４５】
この結果を見ると、図１４の従来のカーナビゲーション装置１０００による実験結果では、図８のシミュレーション結果と同様に、スタート直後から誤った方向に位置が算出され、最終的には、ゴール地点から程遠い位置をゴール地点と判定している。それに対し、図１５の本実施例のカーナビゲーション装置１０００による実験結果では、図９のシミュレーション結果と同様に、ほぼ真の軌跡に沿った正確な軌跡が得られていることがわかる。これにより、実環境においても、本実施形態の位置算出方法は有効であることが示された。
【０１４６】
３．作用効果
本実施形態によれば、移動体に設置されたセンサー３により移動ベクトルが計測される。そして、移動体が移動を開始した際にセンサー３が計測した移動ベクトルを用いて、移動体に対するセンサー３の姿勢が判定される。
【０１４７】
第１の取付姿勢判定システム１Ａでは、ローカル座標速度ベクトル算出部５が、加速度センサー３Ａが計測したローカル座標加速度ベクトルを用いてローカル座標速度ベクトルを算出する。そして、取付姿勢判定部１０が、移動体が移動を開始した際のローカル座標速度ベクトルを用いて、加速度センサー３Ａの取付姿勢のうちのピッチ成分及びヨー成分を判定する。
【０１４８】
また、第２の取付姿勢判定システム１Ｂでは、ＬＡ座標変換行列算出部２０が、姿勢センサー３Ｂが計測した絶対座標系で表したセンサー３の向き（絶対姿勢）を用いて、ローカル座標系から絶対座標系への座標変換行列（ＬＡ座標変換行列）を算出する。そして、絶対座標加速度ベクトル算出部３０が、加速度センサー３Ａが計測したローカル座標加速度ベクトルを、ＬＡ座標変換行列を用いて絶対座標加速度ベクトルに座標変換する。さらに、絶対座標速度ベクトル算出部４０は、絶対座標加速度ベクトルを用いて絶対座標速度ベクトルを算出する。取付姿勢判定部１０は、姿勢センサー３Ｂが計測した絶対座標系で表したセンサー３の向きを用いて絶対座標系からローカル座標系への座標変換行列（ＡＬ座標変換行列）を算出し、当該ＡＬ座標変換行列と、絶対座標速度ベクトルとを用いて、加速度センサー３Ａの取付姿勢のうちのピッチ成分及びヨー成分を判定する。
【０１４９】
また、移動体が停止している際には、センサー３の計測結果を補償するための補償演算を行う。例えば、移動体が停止している場合は、移動体の速度ベクトルの各成分はゼロであるとする制約条件（第１の制約条件）を用いて、センサー３の計測結果を補償する。これにより、移動体の停止中は、センサー３の計測結果に含まれる誤差を低減させることができる。誤差が低減された状態で移動体が移動を開始しても、それほど時間が経過していなければ、移動体の速度ベクトルの誤差は抑制されているはずである。そのため、移動体が移動を開始した際の速度ベクトルを利用することで、移動体に対するセンサー３の相対的な姿勢を正しく判定することができる。
【０１５０】
また、第１の位置算出システム２Ａでは、第１の慣性航法システム４Ａの慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）が、補正部９０によって補正される。補正部９０は、取付姿勢判定部１０により判定された第１の慣性航法システム４Ａの取付姿勢と、速度制約条件設定部７０により設定された停止時速度制約条件（第１の制約条件）又は移動時速度制約条件（第２の制約条件）と、ＬＶ座標変換行列算出部８０により算出されたＬＶ座標変換行列とを用いて、所定の誤差推定演算を行って慣性航法演算誤差を推定する。そして、推定した慣性航法演算誤差を用いて慣性航法演算結果を補正する。また、第２の位置算出システム２Ｂにおいても同様に、第２の慣性航法システム４Ｂの慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）が、補正部９０によって補正される。
【０１５１】
このように、第１又は第２の取付姿勢判定方法を用いて判定したセンサー３の取付姿勢と、センサー３が計測した移動ベクトル（加速度ベクトルや速度ベクトル）とを用いることで、ローカル座標系と移動体座標系とのズレ分を考慮し、この２種類の座標系の座標変換を適切に行って、移動体の位置をより正確に算出することができる。
【０１５２】
４．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１５３】
４−１．進行方向切替対応ロジック
上記の実施形態では、移動体が移動を開始した際に求めたＬＶ座標変換行列を用いて位置算出を行うものとして説明した。しかし、移動体は必ずしも進行方向前方にのみ移動するとは限らない。例えば、移動体が四輪自動車であるならば、四輪自動車は前方に進行するに限らず、後方に進行する（バックする）場合もあり得る。
【０１５４】
従って、移動体の進行方向が前進と後進とで切り替わる場合、進行方向の変化の前後で同じＬＶ座標変換行列を用いて計算を行ったのでは、移動体の速度や位置が適切に求まらないことが想定される。なぜならば、本実施形態の手法では、移動体が移動を開始した際の移動体の移動方向に対する移動ベクトルを用いてセンサー３の取付姿勢を判定するため、移動体の移動方向が逆転してしまうと、取付姿勢と移動方向との間で辻褄が合わなくなるためである。
【０１５５】
上記の問題に鑑み、次のような進行方向切替対応ロジックを適用すると好適である。具体的には、例えば移動体座標系での移動体の速度ベクトルに基づいて、移動体の移動方向が逆転したか否かを判定する。そして、移動体の移動方向が逆転した場合に、移動開始時に求めて記憶しておいたＬＶ座標変換行列を１８０°回転させて更新し、更新後のＬＶ座標変換行列を用いて計算を行う。
【０１５６】
図１６は、この場合に、実施例のカーナビゲーション装置１０００が図１２のナビゲーション処理に代えて実行する第２のナビゲーション処理の流れを示すフローチャートである。
【０１５７】
第２のナビゲーション処理では、処理部１００は、ステップＡ１９又はステップＡ２５の後、移動体座標系のロール軸（Ｒ軸）の速度成分の正負の符号が逆転したか否かを判定する（ステップＣ２５）。符号が逆転していない場合は（ステップＣ２５；Ｎｏ）、自動車の移動方向は変化していないものと判断し、ステップＡ２７に移行する。
【０１５８】
それに対し、正負の符号が逆転している場合は（ステップＣ２５；Ｙｅｓ）、処理部１００は、自動車の移動方向が変化したものと判断し、座標変換行列データ５６０に記憶されている最新のＬＶ座標変換行列を１８０°回転させて、座標変換行列データ５６０を更新する（ステップＣ２６）。そして、処理部１００は、ステップＡ２７に移行する。
【０１５９】
図１７及び図１８は、上記の進行方向切替対応ロジックの有効性を示す実験結果である。図１７は、進行方向切替対応ロジックを適用しない場合の実験結果である。それに対し、図１８は、進行方向切替対応ロジックを適用した場合の実験結果である。車の進行方向前方（前進方向）とセンサー３のｘ軸正方向とが一致するようにセンサー３を設置した。つまり、この場合のセンサー３の取付姿勢角は「０°」である。
【０１６０】
自動車を時刻「ｔ０」〜「ｔ１」までの期間停止させた。そして、時刻「ｔ１」においてバックで移動開始し、時刻「ｔ２」において自動車を一旦停止させて、今度は自動車を前方に移動させた。各図において、横軸は時間軸を示し、縦軸は自動車のロール軸速度（単位は［ｍ／ｓ］）及びセンサー３の取付姿勢角（単位は［°］）を示す。また、判定されたセンサー３の取付姿勢角を太実線で示し、算出された自動車のロール軸速度を細実線で示す。
【０１６１】
これらの図を見ると、時刻「ｔ０」〜「ｔ１」までの間は、ロール軸速度はゼロとなっている。そして、時刻「ｔ１」において自動車がバックを開始するとロール軸速度は負の値となり、時刻「ｔ２」までは徐々に減少していく。
【０１６２】
その後、時刻「ｔ２」においてロール軸速度の絶対値が所定の閾値を超えると、取付姿勢判定発動条件が成立する（図１６のステップＡ１３；Ｙｅｓ）。これにより、センサー３の取付姿勢が判定される（図１６のステップＡ１５）。この場合、取付姿勢判定部１０は、自動車が後進（バック）した方向を自動車の移動方向と認識して取付姿勢を判定するため、実際にはセンサー３の取付姿勢角は「０°」であるのに、取付姿勢角を「１８０°」と判定してしまう。
【０１６３】
この場合、取付姿勢角を「１８０°」としてＬＶ座標変換行列を算出し（図１６のステップＡ１７）、当該ＬＶ座標変換行列を用いて計算を行うため、自動車のロール軸速度は正の値として算出されてしまう。自動車はバックで移動しているため、実際にはロール軸速度は負の値である。
【０１６４】
進行方向切替対応ロジックを適用しない場合は、図１７に示すように、時刻「ｔ３」において自動車が前方に移動を開始しても、センサー３の取付姿勢角が「１８０°」であると認識したまま計算を行うため、ロール軸速度の符号は変化しない。そのため、正の値として算出されるべきロール軸速度は負の値として算出され、自動車が前方に加速するにつれて、ロール軸速度は負の方向に増大してしまっている。
【０１６５】
それに対し、進行方向切替対応ロジックを適用した場合は、図１８に示すように、時刻「ｔ３」において自動車が前方に移動を開始したことを検知すると（図１６のステップＣ２５；Ｙｅｓ）、取付姿勢角を「１８０°」として算出したＬＶ座標変換行列を反転させて更新する（図１６のステップＣ２６）。そして、反転後のＬＶ座標変換行列を用いて計算を行うため、算出されるロール軸速度の符号は負から正に変化する。そして、自動車が前方に加速したことで、ロール軸速度は正の方向に増大していく。このように、進行方向切替対応ロジックを適用することで、移動体の進行方向の変化に適切に対応することができる。
【０１６６】
４−２．補正部の補正方法
上記の実施形態では、慣性航法演算結果を補正する手法としてカルマンフィルターを例に挙げて説明したが、補正方法はこれに限られない。例えば、速度制約条件設定部７０により設定された速度制約条件に含まれる制約速度ベクトルと、慣性航法演算で得られた速度ベクトルとの平均化処理を行って速度ベクトルを補正することとしてもよい。
【０１６７】
また、車速検出システムにより計測された移動体の速度や、磁気センサー等の方位センサーにより計測された移動体の向きを用いて、慣性航法演算で得られた速度ベクトルを補正することとしてもよい。例えば、速度ベクトルの大きさについて、慣性航法演算で得られた速度ベクトルの大きさと、車速検出システムにより計測された速度との平均化処理を行うこととしてもよい。また、速度ベクトルの向きについて、慣性航法演算で得られた速度ベクトルの向きと、方位センサーにより計測された移動体の向きとの平均化処理を行うこととしてもよい。
【０１６８】
平均化処理は、単純な算術平均や幾何平均を適用してもよいし、加重平均を適用してもよい。慣性航法演算結果を信用して速度ベクトルを補正するのであれば、慣性航法演算結果の重みを車速検出システムや方位センサーの計測結果の重みよりも高くして加重平均すればよい。逆に、車速検出システムや方位センサーの計測結果を信用して速度ベクトルを補正するのであれば、慣性航法演算結果の重みを車速検出システムや方位センサーの計測結果の重みよりも低くして加重平均すればよい。なお、これらの手法以外にも、補正部の補正手法としては任意の手法を適用可能である。
【０１６９】
４−３．処理の主体
上記の実施例では、電子機器にＩＭＵ４００が搭載され、ＩＭＵ４００の計測結果に基づいて、電子機器の処理部が慣性航法演算処理を行うものとして説明した。しかし、電子機器にＩＮＳを搭載することとし、ＩＮＳの処理部が慣性航法演算処理を行うこととしてもよい。この場合、電子機器の処理部は、ＩＮＳから出力される慣性航法演算結果に含まれる誤差（慣性航法演算誤差）を推定する処理を行う。そして、推定した慣性航法演算誤差を用いて、ＩＮＳから入力した慣性航法演算結果を補正する。
【０１７０】
また、電子機器にＧＰＳ（Global Positioning System）等の衛星測位システムを適用したセンサーユニットを搭載することとし、このセンサーユニットの計測結果を利用して、慣性航法演算結果を補正することとしてもよい。
【０１７１】
図１９は、この場合における電子機器の構成を示す図であり、処理部１００に係る機能ブロックを中心に示したものである。図１９の電子機器では、図１１のＩＭＵ４００の代わりにＩＮＳ７００が搭載され、さらにＧＰＳユニット８００が搭載されている。この場合、処理部１００は慣性航法演算処理を行わないため、図１１における慣性航法演算部１０５は削除されている。
【０１７２】
取付姿勢判定部１１０は、ＩＮＳ７００から入力したローカル座標速度ベクトルを用いて、ＩＮＳ７００の取付姿勢を判定する。補正部１９０のＫＦ誤差推定部１９１は、速度制約条件設定部１７０から入力した速度制約条件の他に、ＧＰＳユニット８００から入力したメジャメント情報を観測情報として、ＩＮＳ７００の慣性航法演算結果に含まれる慣性航法演算誤差を推定する。
【０１７３】
メジャメント情報は、ＧＰＳ衛星から受信したＧＰＳ衛星信号に係る各種の諸量である。例えば、ＧＰＳ衛星信号のキャリア位相やコード位相、ドップラー周波数、擬似距離、擬似距離の変化率（レンジレート）といった諸量がメジャメント情報に含まれる。なお、ＧＰＳのメジャメント情報を観測情報として慣性航法演算誤差を推定する手法については従来公知であるため、数式等を用いた説明は省略する。
【０１７４】
４−４．姿勢判定タイミング
上記の実施形態では、移動体が停止した後に移動を開始する度に、センサー３の取付姿勢を判定するものとして説明した。しかし、姿勢判定タイミングは適宜設定可能である。例えば、センサー３が移動体に設置された後、移動体の初回の移動開始タイミングにのみセンサー３の取付姿勢を判定することとしてもよい。この場合は、初回以降は取付姿勢判定を行わず、初回に判定した取付姿勢を用いて処理を行うようにすればよい。
【０１７５】
４−５．姿勢判定の成分
上記の実施形態では、移動体に対するセンサー３の取付姿勢のピッチ成分及びヨー成分の両方を判定するものとして説明したが、ピッチ成分及びヨー成分の何れか一方のみを判定することとしてもよいのは勿論である。
【０１７６】
４−６．設置ずれの誤差
上記の実施形態では、移動中はセンサー３の取付姿勢に変化が生じないことを前提としていた。しかし、移動中の振動等によってセンサー３の取付姿勢の変化、すなわち設置ずれが生じる可能性がある。そこで、式（６）の状態ベクトルに設置ずれによる誤差角度の成分を追加し、ＫＦ誤差推定部１９１が移動時速度制約条件５２１を「ＯＮ」として求めた移動時観測ベクトルを用いてＫＦ誤差推定処理を行って（ステップＢ９）、設置ずれの誤差角度を推定することとしてもよい。このようにすることで、移動中にセンサー３の取付姿勢が変化した場合であっても、図１２や図１６のステップＡ１５の取付姿勢判定処理で判定された取付姿勢を随時補正することが可能となる。
【０１７７】
４−７．取付姿勢判定処理の変形例
図３等を用いて説明した取付姿勢の判定では、ＩＭＵ４００が計測するローカル座標加速度ベクトルにバイアス誤差がのっている場合、算出されるＡＬ座標変換行列にもバイアス誤差が含まれるため、正確な姿勢の判定ができない場合が考えられる。そこで、次のようにしてもよい。
【０１７８】
すなわち、図２０に示すように、図１１に示した処理部１００の機能ブロックにおいて、ＩＭＵ４００が計測したローカル座標加速度ベクトル及びローカル座標角速度を取付姿勢判定部１１０へ入力するようにする。そして、取付姿勢判定部１１０（処理部１００）は、図２１に示す第２の取付姿勢判定処理を実行する。
【０１７９】
図２１において、処理部１００は、自動車が停止しているか否かを判定する（ステップＤ１）。停止している場合には、ローカル座標速度ベクトルをゼロリセットするとともに（ステップＤ３）、ＩＭＵ４００で計測されているローカル座標角速度及びローカル座標加速度ベクトルを停止時の計測データとして取得・更新する（ステップＤ５）。このときのローカル座標角速度をローカル座標停止時角速度Ωｓと称し、ローカル座標加速度をローカル座標停止時加速度αｓと称する。
【０１８０】
停止が継続している間、ステップＤ３〜Ｄ５を繰り返し行い、停止が解除され、自動車が移動を開始したと判定された場合は（ステップＤ７：ＹＥＳ）、次の処理に移行する。ここで、移動中のＩＭＵ４００で計測されているローカル座標角速度を符号Ωｍとし、ローカル座標加速度を符号αｍとして説明する。まず、ローカル座標の回転情報を算出する（ステップＤ９）。具体的には、ローカル座標停止時角速度Ωｓに対するローカル座標角速度Ωｍの時間変化（時間微分）を積算し、継ぎ足していく。この値は、停止時のローカル座標系に対する座標系自体の回転、すなわちローカル座標回転情報を示す。
【０１８１】
次いで、ローカル座標停止時加速度ベクトルαｓを、ステップＤ９で算出したローカル座標回転情報を用いて逆変換することで、ＩＭＵ４００で計測されているローカル座標系における加速度αｍｓに変換する（ステップＤ１１）。そして、ＩＭＵ４００で計測されているローカル座標加速度αｍと変換した加速度αｍｓとの差を取ることで、当該時点の加速度αｆを求める（ステップＤ１３）。変換した加速度αｍｓと、ＩＭＵ４００で計測されているローカル座標系における加速度αｍとは、同じローカル座標系上での加速度である。そして、もしも両者の加速度ベクトルにバイアス誤差がのっていたとしても、その差を求める結果、バイアス誤差は相殺することになる。また、重力加速度も相殺される。そのため、ＩＭＵ４００で計測されているローカル座標系における自動車自体の純粋な加速度に係るベクトルαｆが得られる。
【０１８２】
次いで、得られた加速度ベクトルαｆを用いて、ローカル座標速度ベクトルを更新する（ステップＤ１５）。更新が初回の場合は、ステップＤ３でリセットしたローカル座標速度ベクトルが初めて更新されるため、停止時のローカル座標系における移動開始直後の速度ベクトルが得られる。以降、取付姿勢の判定タイミングが到来するまでステップＤ９〜Ｄ１５の処理を繰り返す（ステップＤ１７）。取付姿勢の判定タイミングは、図１２のステップＡ１３の取付姿勢判定発動条件の成否で判定すると好適である。
姿勢判定の判定タイミングが到来した場合、最新のローカル座標速度ベクトルをもとに、上述の式（２）及び式（３）を用いて取付姿勢を判定する（ステップＤ１９）。
【０１８３】
以上の第２の取付姿勢判定処理は、ナビゲーション処理の実行中に並行して実行することとするとよい。例えば図１２や図１６のステップＡ３とＡ５の間に、第２の取付姿勢判定処理の実行を開始するステップを加える。この場合、第２の取付姿勢判定処理は、図１２や図１６のステップＡ１５で実行される取付姿勢判定処理の代わりの処理となるため、ステップＡ１５では取付姿勢判定処理を行わずともよい。
【０１８４】
また、第２の取付姿勢判定処理の効果確認試験を行った。ローカル座標系のｘ軸及びｚ軸にバイアス誤差を生じさせた場合の従来の取付姿勢判定処理の判定結果と、第２の取付姿勢判定処理の判定結果とを比較した。ｘ軸に＋１［ｍＧ］、ｚ軸に−１［ｍＧ］のバイアス誤差を生じさせた第１試験では、従来の取付姿勢判定処理ではヨー角の誤差が−３．４０［ｄｅｇ］、ピッチ角の誤差が＋５．５３［ｄｅｇ］であった。一方、第２の取付姿勢判定処理ではヨー角の誤差が＋０．０６［ｄｅｇ］、ピッチ角の誤差が−０．０１［ｄｅｇ］であった。また、ｘ軸に＋１００［ｍＧ］、ｚ軸に−１００［ｍＧ］のバイアス誤差を生じさせた第２試験では、従来の取付姿勢判定処理ではヨー角の誤差が＋７４．２６［ｄｅｇ］、ピッチ角の誤差が−３４．３０［ｄｅｇ］であった。一方、第２の取付姿勢判定処理ではヨー角の誤差が＋０．０５［ｄｅｇ］、ピッチ角の誤差が＋０．００［ｄｅｇ］であった。
【０１８５】
４−８．移動体
本発明は種々の移動体に適用可能である。好適な適用例は四輪自動車であるが、例えば移動時には縦方向や横方向に移動しないといった移動方向に関する制約を課すことができるのであれば、二輪自動車や自転車、電車、船、人間といった移動体にも本発明を適用可能である。
【０１８６】
４−９．電子機器
上記の実施例では、カーナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、携帯型ナビゲーション装置（ポータブルナビ）や携帯型電話機、パソコン、ＰＤＡといった他の電子機器にも本発明を適用可能である。
【符号の説明】
【０１８７】
１Ａ 第１の取付姿勢判定システム、 １Ｂ 第２の取付姿勢判定システム、 ２Ａ 第１の位置算出システム、 ２Ｂ 第２の位置算出システム、 ３ センサー、 ３Ａ 加速度センサー、 ３Ｂ 姿勢センサー、 ３Ｃ ジャイロセンサー、 ４Ａ 第１の慣性航法システム、 ４Ｂ 第２の慣性航法システム、 ５ ローカル座標速度ベクトル算出部、 １０ 取付姿勢判定部、 １５ 絶対姿勢判定部、 ２０ ＬＡ座標変換行列算出部、 ３０ 絶対座標加速度ベクトル算出部、 ４０ 絶対座標速度ベクトル算出部、 ５０ 絶対座標位置算出部、 ６０ 移動状況判定部、 ７０ 速度制約条件設定部、 ８０ ＬＶ座標変換行列算出部、 ９０ 補正部
【特許請求の範囲】
【請求項１】
移動体に設置された移動ベクトルを計測するセンサーが前記移動ベクトルを計測することと、
前記移動体が移動を開始した際に前記センサーが計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定することと、
を含む姿勢判定方法。
【請求項２】
前記移動体が停止している場合に、前記移動体の速度に関する第１の制約条件を用いて、前記センサーの計測結果を補償すること、
を更に含む請求項１に記載の姿勢判定方法。
【請求項３】
前記姿勢を判定することは、前記移動体に対する前記姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することである、
請求項１又は２に記載の姿勢判定方法。
【請求項４】
前記移動ベクトルを計測することは、前記移動体の移動空間を定める座標系である絶対座標系での移動ベクトルを計測することであり、
前記姿勢を判定することは、前記移動ベクトルと、前記絶対座標系で表した前記センサーの向きとを用いて、前記センサーの前記姿勢を判定することである、
請求項１〜３の何れか一項に記載の姿勢判定方法。
【請求項５】
前記姿勢を判定することは、
前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することと、
前記速度ベクトルを用いて前記センサーの姿勢を判定することと、
を含む、
請求項１〜３の何れか一項に記載の姿勢判定方法。
【請求項６】
前記姿勢を判定することは、前記移動体の停止が解除されて前記移動体の速度又は加速度が所定の閾値に達した時の前記移動ベクトルを用いて前記姿勢を判定することである、
請求項１〜５の何れか一項に記載の姿勢判定方法。
【請求項７】
請求項１〜６の何れか一項に記載の姿勢判定方法を用いて前記姿勢を判定することと、
前記姿勢と前記移動ベクトルとを用いて前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項８】
前記位置を算出することは、
前記移動ベクトルを用いて前記絶対座標系での前記移動体の絶対座標位置を算出することと、
前記姿勢と前記絶対座標系で表した前記センサーの向きとを用いて、前記絶対座標位置を、前記移動体を基準とする移動体座標位置に座標変換することと、
前記移動体座標位置と、前記移動体の移動方向に関する第２の制約条件とを用いて、前記絶対座標位置を補正することと、
を含む、
請求項７に記載の位置算出方法。
【請求項９】
移動体に設置された移動ベクトルを計測するセンサーが前記移動体が移動を開始した際に計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定する姿勢判定装置。
【請求項１】
移動体に設置された移動ベクトルを計測するセンサーが前記移動ベクトルを計測することと、
前記移動体が移動を開始した際に前記センサーが計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定することと、
を含む姿勢判定方法。
【請求項２】
前記移動体が停止している場合に、前記移動体の速度に関する第１の制約条件を用いて、前記センサーの計測結果を補償すること、
を更に含む請求項１に記載の姿勢判定方法。
【請求項３】
前記姿勢を判定することは、前記移動体に対する前記姿勢のロール成分を判定せず、ピッチ成分及びヨー成分のうちの少なくも一方を判定することである、
請求項１又は２に記載の姿勢判定方法。
【請求項４】
前記移動ベクトルを計測することは、前記移動体の移動空間を定める座標系である絶対座標系での移動ベクトルを計測することであり、
前記姿勢を判定することは、前記移動ベクトルと、前記絶対座標系で表した前記センサーの向きとを用いて、前記センサーの前記姿勢を判定することである、
請求項１〜３の何れか一項に記載の姿勢判定方法。
【請求項５】
前記姿勢を判定することは、
前記移動体の停止時に前記センサーにより計測された移動ベクトルと、移動を開始した際に計測された移動ベクトルとを用いて、前記センサーの計測座標系であるローカル座標系における速度ベクトルを算出することと、
前記速度ベクトルを用いて前記センサーの姿勢を判定することと、
を含む、
請求項１〜３の何れか一項に記載の姿勢判定方法。
【請求項６】
前記姿勢を判定することは、前記移動体の停止が解除されて前記移動体の速度又は加速度が所定の閾値に達した時の前記移動ベクトルを用いて前記姿勢を判定することである、
請求項１〜５の何れか一項に記載の姿勢判定方法。
【請求項７】
請求項１〜６の何れか一項に記載の姿勢判定方法を用いて前記姿勢を判定することと、
前記姿勢と前記移動ベクトルとを用いて前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項８】
前記位置を算出することは、
前記移動ベクトルを用いて前記絶対座標系での前記移動体の絶対座標位置を算出することと、
前記姿勢と前記絶対座標系で表した前記センサーの向きとを用いて、前記絶対座標位置を、前記移動体を基準とする移動体座標位置に座標変換することと、
前記移動体座標位置と、前記移動体の移動方向に関する第２の制約条件とを用いて、前記絶対座標位置を補正することと、
を含む、
請求項７に記載の位置算出方法。
【請求項９】
移動体に設置された移動ベクトルを計測するセンサーが前記移動体が移動を開始した際に計測した前記移動ベクトルを用いて、前記移動体に対する前記センサーの姿勢を判定する姿勢判定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【公開番号】特開２０１２−１９４１７５（Ｐ２０１２−１９４１７５Ａ）
【公開日】平成２４年１０月１１日（２０１２．１０．１１）
【国際特許分類】
【出願番号】特願２０１１−２７５２３５（Ｐ２０１１−２７５２３５）
【出願日】平成２３年１２月１６日（２０１１．１２．１６）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．ＩＮＳ
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年１０月１１日（２０１２．１０．１１）
【国際特許分類】
【出願日】平成２３年１２月１６日（２０１１．１２．１６）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．ＩＮＳ
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]