加速度検出方法、位置算出方法及び加速度検出装置
【課題】移動体の加速度をより適切に検出するための新しい手法の提案。
【解決手段】姿勢調整部２０は、移動体に設置された姿勢センサー４の検出姿勢を、移動体に設置された加速度センサー３の検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。そして、座標変換部３０は、姿勢調整部２０により調整された検出姿勢を用いて、移動体のローカル座標系から絶対座標系への座標変換に係る変換係数を求め、当該変換係数を用いて、加速度センサー３の検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換する。
【解決手段】姿勢調整部２０は、移動体に設置された姿勢センサー４の検出姿勢を、移動体に設置された加速度センサー３の検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。そして、座標変換部３０は、姿勢調整部２０により調整された検出姿勢を用いて、移動体のローカル座標系から絶対座標系への座標変換に係る変換係数を求め、当該変換係数を用いて、加速度センサー３の検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、加速度検出方法、位置算出方法及び加速度検出装置に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行って位置算出を行う技術も考案されている。
【０００３】
慣性航法演算においては、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、慣性航法演算を行うことで測定したデータを、ＧＰＳ（Global Positioning System）を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−２３９６４３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に開示されている技術では、慣性航法演算を行うことで測定した移動体の位置や速度、加速度、姿勢角、方位角といった慣性航法演算に係る諸量それぞれについて、カルマンフィルターを利用して、各諸量に含まれ得る誤差を推定している。この場合、慣性航法演算に係る全ての諸量についてカルマンフィルターを利用して誤差推定演算を行う必要があるため、演算量は膨大になりがちである。
【０００６】
慣性航法演算では、例えば移動体に設置された加速度センサーを用いるのであれば、加速度センサーが検出した検出加速度を積分することで移動体の速度を算出し、さらに、その速度を積分することで位置を算出する。
【０００７】
この場合に算出される速度や位置に含まれ得る誤差は、そもそも加速度センサーの検出加速度に含まれ得る誤差に起因するものである。つまり、検出加速度に対して積分を繰り返すために、検出加速度に含まれていた誤差が、時間経過に伴って、速度や位置に誤差として重畳・累積されていくことによるものである。そのため、移動体の加速度を正しく検出することができれば、特許文献１のように慣性航法演算に係る全ての諸量の誤差を推定せずとも、慣性航法演算の精度を向上させることが可能であると考えられる。
【０００８】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体の加速度をより適切に検出するための新しい手法を提案することにある。
【課題を解決するための手段】
【０００９】
以上の課題を解決するための第１の形態は、移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整することと、前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求めることと、前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換することと、を含む加速度検出方法である。
【００１０】
また、他の形態として、移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部と、前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める変換係数算出部と、前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換する変換部と、を備えた加速度検出装置を構成してもよい。
【００１１】
この第１の形態等によれば、移動体に設置された姿勢センサーの検出姿勢を、移動体に設置された加速度センサーの検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。そして、調整された検出姿勢を用いて、加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める。そして、求めた変換係数を用いて、加速度センサーの検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換する。
【００１２】
加速度センサーが検出する加速度ベクトルは、加速度センサーに対応付けられたローカルな座標系における加速度ベクトルである。それに対して、例えば慣性航法演算においては、移動体の移動空間を定める絶対的な座標系を基準として演算を行うことが一般的である。姿勢センサーの検出姿勢には誤差が含まれ得るため、姿勢センサーの検出姿勢を、加速度センサーの検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。その上で、ローカル座標系から絶対座標系への変換係数を求めて座標変換を行うことで、絶対座標系における加速度ベクトルを適切に検出することができる。このようにして検出された絶対座標系における加速度ベクトルは、上記のように慣性航法演算等に好適である。
【００１３】
また、第２の形態として、第１の形態の加速度検出方法であって、前記変換係数を求めることは、前記検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な前記変換係数を求めることである、加速度検出方法を構成してもよい。
【００１４】
この第２の形態によれば、検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な変換係数を求める。そのため、当該変換係数を用いて加速度センサーの検出加速度ベクトルを絶対座標系における加速度ベクトルに変換することで、加速度センサーの検出加速度ベクトルに含まれ得る誤差を低減させることができる。
【００１５】
また、第３の形態として、第１又は第２の形態の加速度検出方法であって、前記調整することと、前記変換係数を求めることと、前記座標変換することと、を前記移動体の移動中に行う、加速度検出方法を構成してもよい。
【００１６】
この第３の形態によれば、姿勢センサーの検出姿勢の調整と、変換係数を求めることと、検出加速度ベクトルの座標変換と、を移動体の移動中に行うことで、移動中に移動体の姿勢が変化した場合であっても、移動体の加速度ベクトルを適切に検出することができる。
【００１７】
また、第４の形態として、第１〜第３の何れかの形態の加速度検出方法であって、前記調整することは、前記検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することを含む、加速度検出方法を構成してもよい。
【００１８】
この第４の形態によれば、検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することで、検出姿勢の調整を簡易に行うことができる。
【００１９】
また、第５の形態として、第１〜第４の何れかの形態の加速度検出方法であって、前記参照ベクトルは、前記移動体に設置された所定の速度計測システムによって計測された前記移動体の速度ベクトルを含み、前記調整することは、前記速度ベクトルを用いて、前記移動体の加速度ベクトルを算出することと、前記検出加速度ベクトルと前記算出した加速度ベクトルとを用いて、前記検出姿勢を調整することと、を含む、加速度検出方法を構成してもよい。
【００２０】
この第５の形態によれば、参照ベクトルには、移動体に設置された所定の速度計測システムによって計測された移動体の速度ベクトルが含まれる。この場合において、速度計測システムによって計測された移動体の速度ベクトルを用いて、移動体の加速度ベクトルを算出する。そして、加速度センサーの検出加速度ベクトルと算出した加速度ベクトルとを用いて、姿勢センサーの検出姿勢を調整する。速度計測システムによって計測された移動体の速度ベクトルを利用することで、姿勢センサーの検出姿勢の調整を適切に行うことができる。
【００２１】
より具体的には、第６の形態のように、第５の形態における速度計測システムが、車速検出システム、又は、衛星測位システムである、加速度検出方法を構成することも可能である。
【００２２】
この第６の形態によれば、車速検出システム、又は、衛星測位システムを利用することで、移動体の速度ベクトルを精度良く計測することが可能となり、移動体の加速度ベクトルの算出に供することができる。
【００２３】
また、第７の形態として、第１〜第６の何れかの形態の加速度検出方法を用いて前記絶対座標系における加速度ベクトルを検出することと、所与の移動基準位置と前記検出された加速度ベクトルとを用いて、前記絶対座標系における前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【００２４】
この第７の形態によれば、所与の移動基準位置と上記の形態の加速度検出方法を用いて検出した加速度ベクトルとを用いることで、移動体の位置を高い正確性で算出することができる。
【図面の簡単な説明】
【００２５】
【図１】加速度検出システムのシステム構成の説明図。
【図２】第１の加速度検出システムのシステム構成の説明図。
【図３】第２の加速度検出システムのシステム構成の説明図。
【図４】第３の加速度検出システムのシステム構成の説明図。
【図５】絶対座標速度ベクトル補正の原理の説明図。
【図６】ナビゲーションシステムのシステム構成の説明図。
【図７】カーナビゲーション装置の機能構成の説明図。
【図８】第１の位置算出システムのシステム構成の説明図。
【図９】ナビゲーション処理の流れを示すフローチャート。
【図１０】第１の位置算出システムによる位置算出結果の説明図。
【図１１】第１の位置算出システムによる姿勢算出結果の説明図。
【図１２】第２の位置算出システムのシステム構成の説明図。
【図１３】第２の位置算出システムによる位置算出結果の説明図。
【図１４】第２の位置算出システムによる姿勢算出結果の説明図。
【図１５】第３の位置算出システムのシステム構成の説明図。
【発明を実施するための形態】
【００２６】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００２７】
１．原理
１−１．システム構成
図１は、本実施形態における加速度検出方法を実現するための主要な機能ブロックで構成される加速度検出システム１のシステム構成図である。加速度検出システム１は、各種の移動体に適用されるシステムであり、移動体の加速度（詳細には加速度ベクトル）を検出して出力するシステムである。
【００２８】
加速度検出システム１は、慣性センサーの一種である加速度センサー３及び姿勢センサー４を備えて構成される。図１では、慣性センサーを二重線で図示し、慣性センサーの検出結果を利用して各種処理を行う処理ブロックを一重線で図示することで、両者を区別している。以降の図面についても同様である。
【００２９】
加速度センサー３は、移動体に設置されて移動体の加速度を検出するセンサーであり、例えばＭＥＭＳ（Micro Electro Mechanical Systems）の技術を利用したＭＥＭＳセンサーが用いられる。本実施形態において、加速度センサー３は、移動体を基準として３軸のローカル座標系における３軸のローカル座標加速度ベクトルを検出して出力するように構成されている。つまり、加速度センサー３に対応付けられたローカル座標系が移動体を基準とする座標系と一致するような姿勢で、加速度センサー３が移動体に設置されているものとして説明する。
【００３０】
姿勢センサー４は、移動体の姿勢を検出するセンサーであり、例えばジャイロセンサーのみで構成してもよいし、磁気センサーや加速度センサーをさらに組み合わせて構成してもよい。姿勢センサー４は移動体に固定され、直交３軸でなるローカル座標系における移動体の姿勢を検出する。すなわち、姿勢センサー４は移動体の姿勢角を計測するための計測器であり、姿勢センサー４が計測した値は、絶対座標系で表した移動体の向きとなる。
【００３１】
移動体の加速度及び速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則的に、単に加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。また、ローカル座標系において定義されるベクトルと、絶対座標系において定義されるベクトルとを区別するため、各変量を表す文言の先頭に座標系の種類を付して説明する。
【００３２】
座標変換の際には、加速度センサー３の検出加速度ベクトルのうち、加速度ベクトルの向きを座標変換し、加速度ベクトルの大きさは維持する。言い換えると、加速度センサー３の相対的な検出方向を、絶対座標系における方向（絶対方向）に座標変換する。
【００３３】
また、本明細書では、姿勢センサー４から出力される姿勢角を「検出姿勢角」と称し、この検出姿勢角によって定まる移動体の姿勢を「検出姿勢」と称する。また、検出姿勢角に対して姿勢調整部２０が調整を行うことで取得した姿勢角を「調整姿勢角」と称し、この調整姿勢角によって定まる移動体の姿勢を「調整姿勢」と称する。
【００３４】
加速度検出システム１は、主要な処理ブロックとして、姿勢調整部２０と、座標変換部３０とを有する。
【００３５】
姿勢調整部２０は、姿勢センサー４の検出姿勢を、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと、移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部である。
【００３６】
座標変換部３０は、姿勢調整部２０により調整された姿勢を用いて、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する変換部である。具体的には、姿勢調整部２０により調整された検出姿勢である調整姿勢を用いて、移動体のローカル座標系から絶対座標系への座標変換に係る変換係数（詳細には座標変換行列）を求める。そして、求めた変換係数を用いて、ローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する。詳細後述するように、座標変換行列は、座標変換に係る変換係数を成分とする行列である。
【００３７】
座標変換部３０は、座標変換に係る変換係数（座標変換行列）を求める変換係数算出部である。座標変換により絶対座標系における加速度ベクトルが算出されるため、座標変換部３０は、絶対座標加速度ベクトル算出部であるとも言える。
【００３８】
１−２．加速度検出の原理
次に、本実施形態における加速度検出の原理について説明する。最初に、姿勢センサー４の検出姿勢を、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整することが、本実施形態の特徴の１つである。次いで、調整された検出姿勢を用いて、加速度センサー３に対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数（座標変換行列）を求める。そして、求めた変換係数を用いて、加速度センサー３の検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換することで、移動体の加速度を検出する。
【００３９】
移動体の移動に係る参照ベクトルとしては、例えば、移動体の加速度ベクトルや速度ベクトルを適用することができる。参照ベクトルは、種々の手法によって取得することが可能である。例えば、移動体に設置された所定の速度計測システムから取得することができる。速度計測システムは、車速検出システム、又は、衛星測位システムである。
【００４０】
車速検出システムでは、例えば車速パルスによって移動体の速度の大きさを検出することができる。磁気センサー等の方位センサーを組み合わせて車速検出システムを構成した場合には、移動体の移動方向も併せて検出することができる。この場合、車速検出システムは、移動体の速度の大きさ及び移動方向を検出する。このため、車速検出システムから、参照ベクトルとして移動体の速度ベクトルを取得することができる。
【００４１】
衛星測位システムは、代表的にはＧＰＳ（Global Positioning System）である。ＧＰＳでは、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星からＧＰＳ受信機までの擬似距離等の情報に基づいて、ＧＰＳ受信機の位置座標及び時計誤差を求める位置計算を行う。なお、ＧＰＳを利用した位置計算は従来公知であるため、ここでは詳細な説明を省略する。
【００４２】
位置計算の他に、ＧＰＳでは、ＧＰＳ衛星及びＧＰＳ受信機の相対的な位置関係の変化により生ずるドップラーを利用して、ＧＰＳ受信機の速度ベクトルを算出することが可能である。すなわち、ＧＰＳ衛星から受信したＧＰＳ衛星信号のドップラー周波数を測定することで、ＧＰＳ受信機とＧＰＳ衛星との間の相対速度ベクトルを算出する。ＧＰＳ衛星の速度ベクトル（移動速度及び移動方向）は、衛星軌道データ（エフェメリスやアルマナック）から求めることができる。そのため、相対速度ベクトルとＧＰＳ衛星の速度ベクトルとを用いれば、移動体の速度ベクトル（移動速度及び移動方向）を算出することができる。このため、ＧＰＳから、参照ベクトルとして移動体の速度ベクトルを取得することができる。
【００４３】
移動体の速度ベクトルを時間微分すれば、移動体の加速度ベクトルが求まる。そのため、上記の速度計測システムから参照ベクトルとして取得した速度ベクトルを時間微分することで、移動体の加速度ベクトルを算出することができる。
【００４４】
そこで、本実施形態の姿勢調整方法では、上記の参照ベクトルを用いて、姿勢調整に用いる移動体の加速度ベクトル（以下、「姿勢調整用加速度ベクトル」と称す。）を算出する。そして、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと算出した姿勢調整用加速度ベクトルとを用いて、検出姿勢角に含まれる誤差（以下、「姿勢角誤差」と称す。）を推定する。この場合、姿勢調整用加速度ベクトルとして、絶対座標系における加速度ベクトルを算出すると好適である。最終的に、推定した姿勢角誤差を用いて検出姿勢角を調整する。
【００４５】
説明を分かり易くするために、座標系を具体的に例示して説明する。ここでは、ローカル座標系として機体座標系（Body Frame）（以下、「Ｂフレーム」と称する。）を適用し、絶対座標系として北東下座標系として知られるＮＥＤ（North East Down）座標系（以下、「Ｎフレーム」と称する。）を適用した場合を一例として説明する。
【００４６】
Ｂフレームは、例えば、移動体の前方を正とする前後方向をＸ軸（ロール軸）、右方を正とする左右方向をＹ軸（ピッチ軸）、鉛直下方を正とする上下方向をＺ軸（ヨー軸）とする三次元直交座標系である。それに対して、Ｎフレームは、例えば、北方を正とする南北方向をＮ軸、東方を正とする東西方向をＥ軸、鉛直下方を正とする高度方向をＤ軸とする三次元直交座標系である。
【００４７】
Ｂフレーム加速度ベクトルからＮフレーム加速度ベクトルへの変換は、次式（１）で定式化される。
【数１】
【００４８】
式（１）における“ｆb”及び“ｆn”は、それぞれＢフレーム加速度ベクトル及びＮフレーム加速度ベクトルであり、次式（２）及び（３）で与えられる。
【数２】
【数３】
【００４９】
また、式（１）における“Ｃbn”はＢフレームからＮフレームへの座標変換行列であり、次式（４）で与えられる。
【数４】
【００５０】
式（４）において、（φ，θ，Ψ）は、それぞれＢフレームにおけるＸ軸（ロール軸）回りの回転角（ロール角）、Ｙ軸（ピッチ軸）回りの回転角（ピッチ角）、Ｚ軸（ヨー軸）回りの回転角（ヨー角）である。また、座標変換行列“Ｃbn”の各成分は、ＢフレームからＮフレームへの座標変換に係る変換係数である。
【００５１】
式（１）〜（４）より、次式（５）が導かれる。
【数５】
【００５２】
ここで、式（５）をロール角“φ”及びピッチ角“θ”の関数とみなし、次式（６）のように定式化する。
【数６】
【００５３】
式（６）において、ハット「＾」付きの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の実測値を示し、ハット「＾」無しの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の真値を示す。また、デルタ「Δ」付きの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の実測値に含まれ得る誤差（姿勢角誤差）を示す。“Δφ”はロール角誤差であり、“Δθ”はピッチ角誤差である。
【００５４】
式（６）を一次の項までテイラー展開すると、次式（７）が導かれる。
【数７】
【００５５】
式（６）及び式（７）から、次式（８）が導かれる。
【数８】
【００５６】
式（８）の左辺について、第１項の“ｆn＝（ｆxn，ｆyn，ｆzn）”は、Ｎフレーム加速度ベクトルの真値を示し、第２項のハット＾付きの“Ｆn（φ，θ）＝（Ｆxn（φ，θ），Ｆyn（φ，θ），Ｆzn（φ，θ））”は、Ｎフレーム加速度ベクトルの実測値を示す。
【００５７】
Ｎフレーム加速度ベクトルの実測値であるハット＾付きの“Ｆn（φ，θ）”は、加速度センサー３の検出加速度ベクトルであるＢフレーム加速度ベクトル“ｆb＝（ｆxb，ｆyb，ｆzb）”と、姿勢センサー４の検出姿勢角（φ，θ，Ψ）とを、それぞれ式（５）に代入することで算出することができる。つまり、Ｎフレーム加速度ベクトルの実測値“Ｆn（φ，θ）”は既知である。
【００５８】
それに対して、Ｎフレーム加速度ベクトルの真値“ｆn＝（ｆxn，ｆyn，ｆzn）”は未知であり、その値を知ることはできない。しかし、本実施形態では、移動体の移動に係る参照ベクトルが外部から与えられる。そこで、参照ベクトルを用いて、絶対座標加速度ベクトルであるＮフレーム加速度ベクトルを算出し、これをＮフレーム加速度ベクトルの真値“ｆn＝（ｆxn，ｆyn，ｆzn）”とみなして計算する。参照ベクトルが十分信頼できるものであれば、参照ベクトルから算出されるＮフレーム加速度ベクトルを真値とみなして計算しても、十分な精度が保証される。
【００５９】
式（８）の右辺について、偏微分行列（ヤコビ行列）の各成分の値は、それぞれ式（５）を、ロール角及びピッチ角の実測値（ハット＾付きの（φ，θ））で偏微分することで求められる。姿勢角の実測値は、姿勢センサー４により検出された姿勢角（検出姿勢角）である。
【００６０】
式（８）において、例えば、左辺を“Δρ”と置き、右辺の偏微分行列を“Ｈ”と置き、右辺の姿勢角誤差を成分とする行列（姿勢角誤差行列）を“Δｘ”と置くと、式（８）は式（９）のように書き換えることができる。
【数９】
【００６１】
式（９）における未知数は、姿勢角誤差行列“Δｘ”のみである。そのため、式（９）は、次式（１０）のように解くことができる。
【数１０】
【００６２】
コンピューター（計算機）を用いた計算では、姿勢角誤差行列“Δｘ”を近似的に求めることになる。例えば、求根アルゴリズムの一種であり、反復法として知られるニュートン法を用いることで、姿勢角誤差行列“Δｘ”の近似解を求めることができる。
【００６３】
姿勢角誤差行列“Δｘ”が求まったならば、姿勢角誤差（ロール角誤差“Δφ”及びピッチ角誤差“Δθ”）を姿勢センサー４の検出姿勢角（ハット＾付きの（φ，θ，Ψ））に加算することで、検出姿勢角を調整する。
【００６４】
なお、本実施形態では、ロール角“φ”及びピッチ角“θ”に含まれる誤差を近似的に求め、これらの誤差を用いて、検出姿勢角のうちのロール角“φ”及びピッチ角“θ”を調整する。つまり、移動体の検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分のみを調整することになる。
【００６５】
上記のようにして求めた調整姿勢角を用いてローカル座標系から絶対座標系への座標変換行列（変換係数）を求め、これを用いて加速度センサー３の検出加速度ベクトルを座標変換すると、検出加速度ベクトルに含まれ得る誤差が座標変換により低減される。つまり、元々検出加速度ベクトルに含まれていた誤差よりも誤差が小さくなる。その理由は、以下の通りである。
【００６６】
加速度センサー３により検出されたローカル座標加速度ベクトルには、移動体の移動による誤差が含まれ得る。この誤差を含み得るローカル座標加速度ベクトルを座標変換した場合は、変換後の絶対座標加速度ベクトルにも当然に誤差が重畳されることとなる。
【００６７】
しかし、上記の原理で説明したように、本実施形態では、加速度センサー３により検出されたローカル座標加速度ベクトルそのものを用いて移動体の検出姿勢を調整し、その調整結果である調整姿勢角を用いて座標変換行列を求めている。これは、加速度センサー３の検出加速度ベクトルに誤差が含まれていることを前提とし、言ってみれば検出加速度ベクトルと辻褄が合うような変換係数を求めることを意味する。つまり、検出加速度ベクトルの誤差を検出姿勢の誤差に含めて考えることで、検出加速度ベクトルに含まれる誤差を低減させようとする考え方である。
【００６８】
上記の姿勢調整部２０による検出姿勢（検出姿勢角）の調整と、座標変換部３０による変換係数（座標変換行列）の算出と、座標変換部３０による座標変換とは、移動体の移動中に行うと好適である。勿論、移動体の停止時に行うこととしてもよいが、本実施形態の手法は、移動体の移動中に特に効果を発揮する。
【００６９】
本実施形態の手法とは別に、移動体が停止している場合に加速度センサー３により検出される加速度ベクトルを用いて、移動体の姿勢を推定する手法が考案されている。具体的には、次式（１１）及び（１２）に従って、ロール角“φ”及びピッチ角“θ”を推定する。
【数１１】
【数１２】
【００７０】
移動体が停止しているのであれば、移動体の姿勢は通常は変化しないため、式（１１）及び（１２）を用いることで、移動体の姿勢を正しく推定することができる。よって、移動体の停止時においては、式（１１）及び（１２）に従って推定したロール角“φ”及びピッチ角“θ”を用いて座標変換行列を求め、加速度センサー３の検出加速度ベクトルを座標変換することで、絶対座標加速度ベクトルを適切に求めることができる。
【００７１】
しかし、移動体の移動中は事態が異なる。移動体の移動により、移動体の姿勢は随時変化するようになる。すると、停止時に推定した姿勢角は正しくなくなり、停止時に推定した姿勢角から求めた座標変換行列は、加速度センサー３の検出加速度ベクトルとの関係で辻褄が合わなくなる。かかる座標変換行列を用いて検出加速度ベクトルを座標変換すると、検出加速度ベクトルに含まれる誤差を低減させることはできず、絶対座標加速度ベクトルには大きな誤差が残存してしまう。
【００７２】
その点、本実施形態の手法では、加速度センサー３の検出加速度ベクトルと辻褄が合うような座標変換行列を求めるため、移動中に移動体の姿勢が随時変化しても問題はなく、検出加速度ベクトルに含まれる誤差を低減させて、誤差の影響が抑制された絶対座標加速度ベクトルを得ることができる。
【００７３】
図１に示した加速度検出システム１は、本実施形態における加速度検出を実現するための最低限の構成要素を示したものである。実際には、加速度検出システムの種々のバリエーションが考えられる。そこで、加速度検出システムのバリエーションについて以下説明する。なお、加速度検出システム１と同一の機能ブロックについては同一の符号を付して説明を省略し、加速度検出システム１とは異なる部分を中心に説明する。
【００７４】
図２は、第１の加速度検出システム１Ａの説明図である。第１の加速度検出システム１Ａは、加速度検出システム１の各機能ブロックの他に、姿勢調整用加速度ベクトル算出部４０を備えて構成される。姿勢調整用加速度ベクトル算出部４０は、移動体の移動に係る参照ベクトルを用いて、姿勢角調整用の加速度ベクトル（姿勢角調整用加速度ベクトル）を算出する。
【００７５】
具体的には、姿勢調整用加速度ベクトル算出部４０は、参照ベクトルとして外部入力した移動体の参照速度ベクトルを用いて絶対座標加速度ベクトルを算出する。例えば、参照速度ベクトルとして絶対座標速度ベクトルが入力される場合は、当該絶対座標速度ベクトルを時間微分することで、絶対座標加速度ベクトルを算出する。そして、算出した絶対座標加速度ベクトルを、姿勢調整用加速度ベクトルとして姿勢調整部２０に出力する。
【００７６】
図３は、第２の加速度検出システム１Ｂの説明図である。第２の加速度検出システム１Ｂは、処理ブロックとして、姿勢調整部２０と、座標変換部３０と、姿勢調整用加速度ベクトル算出部４０と、速度ベクトル算出部５０と、速度ベクトル補正部６０とを有する。
【００７７】
速度ベクトル算出部５０は、座標変換部３０から入力した絶対座標加速度ベクトルを積分及び継ぎ足し処理することで、絶対座標系における移動体の速度ベクトル（絶対座標速度ベクトル）を算出する。そして、算出した絶対座標速度ベクトルを速度ベクトル補正部６０に出力する。ここで、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【００７８】
具体的には、速度ベクトル算出部５０は、所定時間分の絶対座標加速度ベクトルを積分することで、その所定時間の間における移動体の速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新した絶対座標速度ベクトルに継ぎ足すことで、絶対座標速度ベクトルを新たに算出・更新する。
【００７９】
速度ベクトル補正部６０は、速度ベクトル算出部５０から入力した絶対座標速度ベクトルを補正する。具体的には、移動体の移動に係る参照ベクトルとして外部入力した参照速度ベクトルを利用して、絶対座標速度ベクトルを補正する。参照速度ベクトルとしては、例えば絶対座標系における速度ベクトルを取得することができる。そのため、例えば、２種類の絶対座標速度ベクトルの平均処理を行うことで、絶対座標速度ベクトルを補正する。
【００８０】
この場合における平均処理としては、単純な算術平均を適用することもできるし、加重平均を適用することもできる。参照速度ベクトルを信頼して速度ベクトルを補正するのであれば、参照速度ベクトルの重みを速度ベクトル算出部５０により算出された速度ベクトルの重みよりも高くして加重平均すればよい。逆に、速度ベクトル算出部５０により算出された速度ベクトルを信頼して速度ベクトルを補正するのであれば、当該速度ベクトルの重みを参照速度ベクトルの重みよりも高くして加重平均すればよい。
【００８１】
速度ベクトル補正部６０は、補正結果として得られる絶対座標速度ベクトル（以下、「絶対座標補正速度ベクトル」と称す。）を外部出力し、且つ、姿勢調整用加速度ベクトル算出部４０にフィードバックする。姿勢調整用加速度ベクトル算出部４０は、速度ベクトル補正部６０から入力した絶対座標補正速度ベクトルを用いて姿勢調整用加速度ベクトルを算出し、姿勢調整部２０に出力する。
【００８２】
図４は、第３の加速度検出システム１Ｃの説明図である。第３の加速度検出システム１Ｃは、第２の加速度検出システム１Ｂの変形例である。具体的には、第２の加速度検出システム１Ｂとは異なり、第３の加速度検出システム１Ｃでは、姿勢センサー４の検出姿勢が、姿勢調整部２０と速度ベクトル補正部６０とにそれぞれ入力されるように構成されている。
【００８３】
速度ベクトル補正部６０は、移動体の移動に係る参照情報として外部入力した参照速度と、姿勢センサー４の検出姿勢とを用いて、速度ベクトル算出部５０から入力した絶対座標速度ベクトルを補正する。例えば、車速検出システムから参照情報を取得可能な場合において、車速検出システムが移動体の速度の大きさのみを検出可能に構成されている場合は、参照情報として速度の大きさを取得し、姿勢センサー４の検出姿勢と併用して、絶対座標速度ベクトルを補正する。
【００８４】
図５は、絶対座標速度ベクトルの補正の原理を説明するための図である。ここでは、絶対座標系としてＮフレームを考え、東西方向（Ｅ軸）及び南北方向（Ｎ軸）でなる２次元平面において速度ベクトルを補正する場合について図示・説明する。図５において、横軸は東西方向の軸（Ｅ軸）を示しており、縦軸は南北方向の軸（Ｎ軸）を示している。
【００８５】
Ｎフレーム加速度ベクトルの東西方向成分及び南北方向成分を積分して最後に更新したＮフレーム速度ベクトルに継ぎ足すと、Ｎフレーム速度ベクトルの東西方向成分及び南北方向成分が得られる。図５において、Ｅ軸上に記載した矢印がＮフレーム速度ベクトルの東西方向成分を示し、Ｎ軸上に記載した矢印がＮフレーム速度ベクトルの南北方向成分を示す。
【００８６】
この東西方向成分及び南北方向成分を合成することで、大きさが“｜Ｖ１｜”で、向きが“θ１”のＮフレーム速度ベクトルが得られる。すなわち、動径を“｜Ｖ１｜”、偏角を“θ１”とする円座標でＮフレーム速度ベクトルが定義される。但し、図５では、Ｎフレーム速度ベクトルの向きを、速度ベクトルとＥ軸との成す角度で表現している。上記のＮフレーム速度ベクトルが、速度ベクトル算出部５０により算出されるＮフレーム速度ベクトルである。
【００８７】
このＮフレーム速度ベクトルの大きさ及び向きを、例えば次のように補正する。先ず、Ｎフレーム速度ベクトルの大きさ“｜Ｖ１｜”（第１の速度）と、外部入力した参照速度（第２の速度）とを平均処理することで合成する。そして、その合成結果として得られる速度を、Ｎフレーム補正速度ベクトルの大きさ“｜Ｖ２｜”とする。
【００８８】
次に、Ｎフレーム速度ベクトルの向き“θ１”を、姿勢センサー４の検出姿勢に基づいて、移動体の移動方向“θ２”に合わせるように補正する。Ｎフレーム速度ベクトルの向き“θ１”は、Ｎフレーム補正加速度ベクトルを積分及び継ぎ足し処理して間接的に求められる向きであるのに対し、移動方向“θ２”は、姿勢センサー４の検出姿勢から直接的に定まる移動体の向きである。そのため、上記の補正により、Ｎフレーム補正速度ベクトルの向きは、移動体の移動方向に沿ったより正確な向きとなる。
【００８９】
２．実施例
次に、上記の加速度検出システムを適用した位置算出システム及びナビゲーションシステムの実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【００９０】
２−１．システム構成
図６は、本実施例におけるナビゲーションシステムのシステム構成の説明図である。ナビゲーションシステムは、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、加速度検出装置及び位置算出装置を具備した電子機器の一種であるカーナビゲーション装置１０００が搭載されたシステムである。
【００９１】
カーナビゲーション装置１０００は、自動車に搭載される電子機器であり、自動車の加速度ベクトルを算出する加速度検出装置及び自動車の位置を算出する位置算出装置を備えて構成される。また、カーナビゲーション装置１０００は、加速度センサー３及びジャイロセンサー５を有するセンサーユニットとしてＩＭＵ（Inertial Measurement Unit）６００を具備している。
【００９２】
ＩＭＵ６００は、慣性計測ユニットとして知られるセンサーユニットであり、自動車に対する相対的な座標系の加速度ベクトル及び角速度を検出して出力するように設計されている。本実施例では、この相対的な座標系がＢフレームと一致するような姿勢でカーナビゲーション装置１０００が自動車に設置・固定されているものとして説明する。
【００９３】
また、カーナビゲーション装置１０００は、衛星測位システムの一種であるＧＰＳを利用したセンサーユニットとしてＧＰＳユニット２００を具備している。ＧＰＳユニット２００は、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を利用して、例えばＮフレームにおける自動車の位置及び速度ベクトルを計測して出力可能に構成されている。
【００９４】
カーナビゲーション装置１０００は、ＩＭＵ６００の検出結果を利用して慣性航法演算処理を行うことで、自動車の位置を算出する。そして、当該算出位置に対するマップマッチング処理を行うなどしてナビゲーション画面を生成し、生成したナビゲーション画面を表示部５００であるディスプレイに表示させる。
【００９５】
２−２．機能構成
図７は、カーナビゲーション装置１０００の機能構成の一例を示すブロック図である。カーナビゲーション装置１０００は、主要な機能構成として、ＧＰＳアンテナ１００と、ＧＰＳユニット２００と、処理部３００と、操作部４００と、表示部５００と、ＩＭＵ６００と、記憶部７００とを備えて構成される。
【００９６】
ＧＰＳアンテナ１００は、測位用衛星の一種であるＧＰＳ衛星から発信されているＧＰＳ衛星信号を含むＲＦ（Radio Frequency）信号を受信するアンテナであり、受信信号をＧＰＳユニット２００に出力する。ＧＰＳ衛星信号は、拡散符号の一種であるＣＡ（Coarse and Acquisition）コードによって、スペクトラム拡散方式として知られるＣＤＭＡ（Code Division Multiple Access）方式によって変調された１．５７５４２［ＧＨｚ］の通信信号である。ＣＡコードは、コード長１０２３チップを１ＰＮフレームとする繰返し周期１ｍｓの擬似ランダム雑音符号であり、ＧＰＳ衛星毎に異なるものである。
【００９７】
ＧＰＳユニット２００は、ＧＰＳアンテナ１００から出力された信号に基づいて自動車の位置、移動速度及び移動方向を計測する回路或いは装置であり、いわゆるＧＰＳ受信装置に相当する機能ブロックである。図示を省略するが、ＧＰＳユニット２００は、ＲＦ受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、ＲＦ受信回路部と、ベースバンド処理回路部とは、それぞれ別のＬＳＩ（Large Scale Integration）として製造することも、１チップとして製造することも可能である。
【００９８】
ベースバンド処理回路部は、ＲＦ受信回路部から出力された受信信号に対して相関処理等を行ってＧＰＳ衛星信号を捕捉・追尾する。そして、ＧＰＳ衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置を算出する。また、ベースバンド処理回路部は、ＧＰＳ衛星から受信したＧＰＳ衛星信号のドップラー周波数に基づいて、所定の速度算出計算を行って自動車の速度ベクトルを算出する。本実施例では、ＧＰＳユニット２００は、自動車の位置（位置座標）及び速度ベクトルをＮフレームにおいて算出して出力可能に構成されているものとして説明する。
【００９９】
処理部３００は、記憶部７００に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１０００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。
【０１００】
処理部３００は、ＩＭＵ６００の検出結果を利用した慣性航法演算処理を行って自動車の位置（位置座標）を算出する。そして、算出した位置をもとに、自動車の現在位置を指し示した地図を表示部５００に表示させる処理を行う。
【０１０１】
図８は、原理で説明した第１の加速度検出システム１Ａを適用した位置算出システムである第１の位置算出システム２Ａのシステム構成の一例を示す図であり、処理部３００を機能ブロックとして表した概念図である。
【０１０２】
処理部３００は、姿勢算出部３１０と、姿勢調整部３２０と、座標変換部３３０と、姿勢調整用加速度ベクトル算出部３４０と、速度ベクトル算出部３５０と、位置算出部３８０とを機能部として有する。姿勢調整部３２０〜速度ベクトル算出部３５０は、原理で説明した加速度検出システムの姿勢調整部２０〜速度ベクトル算出部５０にそれぞれ対応する。
【０１０３】
姿勢算出部３１０は、ジャイロセンサー５により検出されたＢフレーム角速度を積分及び継ぎ足していくことで、自動車の姿勢角を算出する。すなわち、ジャイロセンサー５及び姿勢算出部３１０によって、原理で説明した加速度検出システムにおける姿勢センサー４が構成される。
【０１０４】
具体的には、姿勢算出部３１０は、ジャイロセンサー５により検出された所定時間分のＢフレーム角速度を積分することで、その所定時間の間の移動体の姿勢角の変化方向及び変化分を算出する。所定時間はＢフレーム角速度の検出時間間隔よりも長く設定され、通常は一定時間とされる。
【０１０５】
例えば、所定時間を１００ミリ秒とし、Ｂフレーム角速度の検出時間間隔（より正確にはジャイロセンサー５から出力された信号のサンプリング時間間隔）が１〜１０ミリ秒の間で適宜調整されるとした場合には、検出時間間隔に関わらず、１００ミリ秒毎に移動体の姿勢角の変化方向及び変化分を算出する。そして、１００ミリ秒毎に算出される姿勢角の変化方向及び変化分を最後に更新した姿勢角に継ぎ足すことで、最新の姿勢角を算出する。
【０１０６】
以下の説明では、姿勢算出部３１０が算出した姿勢角のことを「算出姿勢角」と称し、この算出姿勢角によって定まる自動車の姿勢のことを「算出姿勢」と称する。姿勢算出部３１０は、算出姿勢（算出姿勢角）を姿勢調整部３２０に出力する。
【０１０７】
姿勢調整用加速度ベクトル算出部３４０は、ＧＰＳユニット２００からＮフレームＧＰＳ速度ベクトルを参照速度ベクトルとして取得して、姿勢調整用加速度ベクトルを算出する。そして、算出した姿勢調整用加速度ベクトルを姿勢調整部３２０に出力する。
【０１０８】
位置算出部３８０は、速度ベクトル算出部３５０により算出された自動車のＮフレーム速度ベクトルを用いて、Ｎフレームにおける自動車の位置を算出する。具体的には、Ｎフレーム速度ベクトルを積分及び継ぎ足し処理することで、自動車の位置変化分を算出する。そして、自動車の所与の移動基準位置（初期位置）を基準として、算出した位置変化分を累積的に加算していくことで、自動車の位置を算出・更新する。移動基準位置（初期位置）は、例えばナビゲーションの開始位置としてユーザーにより入力された位置とすることができる。
【０１０９】
図７の説明に戻り、操作部４００は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部３００に出力する。この操作部４００の操作により、目的地の入力等の各種指示入力がなされる。
【０１１０】
表示部５００は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部３００から入力される表示信号に基づいた各種表示を行う表示装置である。表示部５００には、ナビゲーション画面等が表示される。
【０１１１】
ＩＭＵ６００は、加速度センサー３及びジャイロセンサー５を備え、Ｂフレームの各軸における加速度ベクトル及び各軸の軸回りの角速度を検出可能に構成されている。なお、加速度センサー３とジャイロセンサー５とは、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【０１１２】
記憶部７００は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１０００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１３】
２−３．データ構成
図７に示すように、記憶部７００には、プログラムとして、処理部３００により読み出され、ナビゲーション処理（図９参照）として実行されるナビゲーションプログラム７０１が記憶されている。ナビゲーション処理については、フローチャートを用いて詳細に後述する。
【０１１４】
また、記憶部７００には、データとして、センサーデータ７０３と、参照データ７０５と、算出姿勢角データ７０７と、姿勢角誤差データ７０９と、調整姿勢角データ７１１と、移動ベクトルデータ７１３と、算出位置データ７１５とが記憶される。
【０１１５】
センサーデータ７０３は、ＩＭＵ６００により検出されたＢフレーム加速度ベクトル及びＢフレーム角速度が記憶されたデータである。
【０１１６】
参照データ７０５は、自動車の移動に係る参照値が記憶されたデータである。本実施例では、ＧＰＳユニット２００により計測された自動車のＮフレーム速度ベクトルが参照データ７０５として記憶される。
【０１１７】
算出姿勢角データ７０７は、姿勢算出部３１０により算出された自動車の姿勢角が記憶されたデータである。ロール成分、ピッチ成分及びヨー成分の姿勢角がそれぞれ算出されて記憶される。
【０１１８】
姿勢角誤差データ７０９は、姿勢調整部３２０が姿勢角誤差の推定処理を行って推定した姿勢角誤差が記憶されたデータである。例えば、ロール成分及びピッチ成分の誤差が算出されて記憶される。
【０１１９】
調整姿勢角データ７１１は、姿勢調整部３２０が算出姿勢角の調整を行うことで取得した自動車の調整姿勢角が記憶されたデータである。
【０１２０】
移動ベクトルデータ７１３は、自動車の移動状態を示すベクトルのデータであり、座標変換部３３０により算出されたＮフレーム加速度ベクトルや、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルといったデータが記憶される。
【０１２１】
算出位置データ７１５は、位置算出部３８０により算出された自動車の位置が記憶されたデータである。
【０１２２】
２−４．処理の流れ
図９は、記憶部７００に記憶されているナビゲーションプログラム７０１が処理部３００により読み出されて実行されることで、カーナビゲーション装置１０００において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、ＩＭＵ６００により検出された加速度ベクトル及び角速度が、記憶部７００のセンサーデータ７０３に随時記憶されるものとする。
【０１２３】
最初に、姿勢算出部３１０は、姿勢角算出処理を行う（ステップＡ１）。具体的には、ジャイロセンサー５の検出結果を積分及び継ぎ足し処理して自動車の姿勢角を算出し、算出姿勢角データ７０７として記憶部７００に記憶させる。
【０１２４】
次いで、処理部３００は、ＧＰＳユニット２００から参照データ７０５を取得したか否かを判定し（ステップＡ３）、参照データ７０５を取得しなかったと判定した場合は（ステップＡ３；Ｎｏ）、ステップＡ９へと処理を移行する。
【０１２５】
また、参照データ７０５を取得したと判定した場合は（ステップＡ３；Ｙｅｓ）、姿勢調整用加速度ベクトル算出部３４０が、姿勢調整用加速度ベクトル算出処理を行う（ステップＡ５）。すなわち、ＧＰＳユニット２００から取得したＮフレームＧＰＳ速度ベクトルを時間微分することで、Ｎフレームにおける姿勢調整用加速度ベクトルを算出する。
【０１２６】
次いで、姿勢調整部３２０は、姿勢角誤差推定処理を行う（ステップＡ７）。具体的には、加速度センサー３により検出されたＢフレーム加速度ベクトルと、ステップＡ５において算出した姿勢調整用加速度ベクトルと、ステップＡ１において算出した姿勢角とを用いて、例えばニュートン法を用いた反復計算を行って姿勢角誤差を推定する。そして、推定した姿勢角誤差を姿勢角誤差データ７０９として記憶部７００に記憶させる。
【０１２７】
その後、姿勢調整部３２０は、姿勢角誤差データ７０９に記憶された最新の姿勢角誤差を用いて、算出姿勢角データ７０７に記憶された最新の算出姿勢角を調整し、調整姿勢角データ７１１として記憶部７００に記憶させる（ステップＡ９）。
【０１２８】
次いで、座標変換部３３０は、Ｎフレーム加速度ベクトル算出処理を行う（ステップＡ１１）。具体的には、調整姿勢角データ７１１に記憶された最新の調整姿勢角を用いてＢフレームからＮフレームへの座標変換行列を求める。そして、当該座標変換行列を用いて加速度センサー３により検出されたＢフレーム加速度ベクトルをＮフレーム加速度ベクトルに変換し、移動ベクトルデータ７１３として記憶部７００に記憶させる。
【０１２９】
その後、速度ベクトル算出部３５０は、Ｎフレーム速度ベクトル算出処理を行う（ステップＡ１３）。すなわち、所定時間分のＮフレーム加速度ベクトルを積分し、最新のＮフレーム速度ベクトルに継ぎ足すことで、Ｎフレーム速度ベクトルを新たに算出する。そして、移動ベクトルデータ７１３として記憶部７００に記憶させる。
【０１３０】
次いで、位置算出部３８０は、位置算出処理を行う（ステップＡ１５）。具体的には、移動ベクトルデータ７１３に記憶されている所定時間分のＮフレーム速度ベクトルを積分及び継ぎ足し処理することで、移動体の位置変化分を算出する。そして、算出した位置変化分を、算出位置データ７１５に記憶されている最新の算出位置に加算することで、自動車の位置（以下、「慣性航法演算位置」と称す。）を新たに算出・更新する。
【０１３１】
なお、ステップＡ１５の位置算出処理において、ＧＰＳユニット２００からＧＰＳ算出位置を取得し、取得したＧＰＳ算出位置を加味して最終的な自動車の位置（最終位置）を決定するように構成することも可能である。例えば、慣性航法演算位置とＧＰＳ算出位置との平均処理を行うこととしてもよい。また、定期的なタイミングにおいて、ＧＰＳ算出位置で慣性航法演算位置をリセットすることとしてもよい。この場合は、ＧＰＳ算出位置を移動基準位置として、新たに慣性航法演算を開始することとなる。
【０１３２】
その後、位置算出部３８０は、ステップＡ１５で算出した最終位置に対してマップマッチング処理等を行い、表示部５００のナビゲーション画面を更新する（ステップＡ１７）。なお、マップマッチングに係る処理については従来公知であるため、ここでは詳細な説明を省略する。
【０１３３】
次いで、処理部３００は、ナビゲーション処理を終了するか否かを判定する（ステップＡ１９）。例えば、操作部４００を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。まだ処理を終了しないと判定した場合は（ステップＡ１９；Ｎｏ）、処理部３００は、ステップＡ１に戻る。また、終了すると判定した場合は（ステップＡ１９；Ｙｅｓ）、処理部３００は、ナビゲーション処理を終了する。
【０１３４】
２−５．実験結果
図１０は、上述した第１の位置算出システム２Ａにおいて実際に位置算出を行った実験結果の一例を示す図である。自動車を予め定められた周回経路に沿って１周させ、その場合に算出される位置を東西南北の２次元平面にプロットした。
【０１３５】
図１０において、横軸は東西方向、縦軸は南北方向（単位はメートル）をそれぞれ示している。東西方向０ｍ、南北方向０ｍの位置がスタート地点であり、出発地点から北方向に向かい、大きく反時計回り方向に１周周回するような経路を辿った。ゴール地点はスタート地点と同じである。また、自動車の真の軌跡を点線で、姿勢角の調整を行わずに自動車の位置を求めた軌跡を細実線で、姿勢角の調整を行って自動車の位置を求めた軌跡を太実線でそれぞれ示している。
【０１３６】
この図を見ると、姿勢角の調整を行わずに自動車の位置を求めた場合は、最初のうちはほぼ真の軌跡に沿った位置が得られているものの、時間経過に伴って徐々に真の軌跡から外れていき、最終的には大きな位置ずれが発生していることがわかる。これは、姿勢算出部３１０により算出された自動車の姿勢角に誤差が含まれているために、当該姿勢角を用いてＢフレーム加速度ベクトルを座標変換すると、Ｎフレーム加速度ベクトルに誤差が重畳されてしまうためである。
【０１３７】
それに対して、姿勢角の調整を行って自動車の位置を求めた場合は、ほぼ真の軌跡に沿った結果が得られていることがわかる。時間が経過しても真の軌跡から大きく外れることはなく、時間経過に伴う誤差の累積が防止されていることがわかる。
【０１３８】
図１１は、上記の位置算出における姿勢角の時間変化を示すグラフである。図１１（Ａ）はロール角“φ”の時間変化を示し、図１１（Ｂ）はピッチ角“θ”の時間変化を示し、図１１（Ｃ）はヨー角“Ψ”の時間変化を示す。
【０１３９】
各図において、自動車の真の姿勢角を点線で示す。ロール角“φ”及びピッチ角“θ”の真値はゼロである。また、調整を行っていない姿勢角（算出姿勢角）を細実線で示し、調整を行った姿勢角（調整姿勢角）を太実線で示す。なお、本実施形態ではヨー成分については調整を行っていないため、ロール成分及びピッチ成分にのみ調整結果が示されている。
【０１４０】
この結果を見ると、調整を行っていない姿勢角については、ロール角“φ”及びピッチ角“θ”の何れも、時間経過に伴い、真値から値がずれていく傾向があることがわかる。時間経過に伴って、姿勢角の真値からのずれ量（姿勢角誤差）の振幅が大きくなっていくことが特徴的である。図１０に示したように、この算出姿勢角の誤差は、検出加速度ベクトルの誤差と相まって、位置算出の正確性を大きく低下させる要因となっている。
【０１４１】
それに対して、調整姿勢角については、ロール角“φ”及びピッチ角“θ”の何れも、時間が経過しても、ほぼ真値に沿った値が得られていることがわかる。図１０の結果から明らかなように、本実施形態の手法で調整を施した姿勢角を用いて検出加速度を座標変換し、慣性航法演算を行って位置を算出することで、位置算出の正確性が向上する。
【０１４２】
２−６．作用効果
加速度検出システム１Ａにおいて、姿勢算出部３１０は、自動車に設置されたジャイロセンサー５が検出したＢフレーム角速度を用いて、自動車の姿勢（姿勢角）を算出する。姿勢調整部３２０は、姿勢算出部３１０により算出された自動車の姿勢を、自動車に設置された加速度センサー３が検出したＢフレーム加速度ベクトルとＧＰＳユニット２００が計測した参照速度ベクトルとを用いて調整する。すなわち、姿勢調整用加速度ベクトル算出部３４０により、参照速度ベクトルを用いて自動車のＮフレーム加速度ベクトルが算出される。姿勢調整部３２０は、Ｂフレーム加速度ベクトルとＮフレーム加速度ベクトルとを入力値として、姿勢角誤差を推定する所定の推定演算を行って姿勢角誤差を推定する。そして、姿勢調整部３２０は、推定した姿勢角誤差を用いて姿勢角を調整する。
【０１４３】
加速度センサー３が検出する加速度ベクトルは、加速度センサー３に対応付けられたローカルな座標系（例えばＢフレーム）における加速度ベクトルである。それに対し、例えば慣性航法演算においては、移動体の移動空間を定める絶対的な座標系（例えばＮフレーム）を基準として演算を行うことが一般的である。ジャイロセンサー５の検出結果には誤差が含まれ得るため、これを用いて算出される自動車の姿勢角にも誤差が含まれ得る。そこで、ジャイロセンサー５の検出結果を利用して算出した自動車の姿勢角を、加速度センサー３の検出加速度ベクトルとＧＰＳユニット２００の計測結果から求めた加速度ベクトルとを用いて調整する。その上で、ローカル座標系（Ｂフレーム）から絶対座標系（Ｎフレーム）への座標変換行列を求めることで、絶対座標系における加速度ベクトルを適切に求めることができる。
【０１４４】
座標変換部３３０は、姿勢調整部３２０により調整された姿勢角を用いて、ＢフレームからＮフレームへの座標変換行列を求める。この座標変換行列は、加速度センサー３により検出されたＢフレーム加速度ベクトルに含まれ得る誤差を座標変換で低減可能な座標変換行列である。つまり、調整姿勢角に基づき定義される座標変換行列を用いてＢフレーム加速度ベクトルを座標変換することで、Ｂフレーム加速度ベクトルに含まれ得る誤差を低減させ、誤差の影響が抑制されたＮフレーム加速度ベクトルを得ることができる。
【０１４５】
速度ベクトル算出部３５０は、座標変換部３３０により算出されたＮフレーム加速度ベクトルを用いてＮフレーム速度ベクトルを算出する。誤差が低減されたＮフレーム加速度ベクトルを用いてＮフレーム速度ベクトルを求めるため、加速度センサー３が検出したＢフレーム加速度ベクトルに含まれていた誤差がＮフレーム速度ベクトルに重畳することが防止され、時間経過に伴う誤差の累積を防止することができる。
【０１４６】
本実施形態の加速度検出方法では、加速度センサー３の検出加速度ベクトルに含まれ得る誤差を座標変換で低減可能な変換係数（変換行列）を求めて検出加速度ベクトルを座標変換するため、検出加速度ベクトルそれ自体の補正を必要とせずに、誤差の影響が抑制された絶対座標系の加速度ベクトルを検出することができる。そして、所与の移動基準位置と、検出した絶対座標系における加速度ベクトルとを用いることで、移動体の位置をより正確に求めることができる。
【０１４７】
３．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１４８】
３−１．位置算出システム
原理で説明した各種のバリエーションの加速度検出システムを適用して位置算出システムを構築することも勿論可能である。
【０１４９】
図１２は、第２の加速度検出システム１Ｂを適用した第２の位置算出システム２Ｂのシステム構成の一例を示す図である。第２の位置算出システム２Ｂでは、処理部３００は、姿勢算出部３１０と、姿勢調整部３２０と、座標変換部３３０と、姿勢調整用加速度ベクトル算出部３４０と、速度ベクトル算出部３５０と、速度ベクトル補正部３６０と、位置算出部３８０とを機能ブロックとして有する。
【０１５０】
速度ベクトル補正部３６０は、ＧＰＳユニット２００から参照速度ベクトルとしてＮフレームＧＰＳ速度ベクトルを取得し、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルを補正する。そして、その補正結果として得られるＮフレーム補正速度ベクトルを位置算出部３８０に出力する。また、Ｎフレーム補正速度ベクトルを姿勢調整用加速度ベクトル算出部３４０にフィードバックする。
【０１５１】
図１３は、第２の位置算出システム２Ｂを利用して実際に位置算出を行った実験結果の一例を示す図である。図の見方は図１０と同じである。この図を見ると、第２の位置算出システム２Ｂを利用して位置算出を行った場合も、自動車の真の軌跡に沿った軌跡が得られており、誤差の累積が抑えられていることがわかる。
【０１５２】
図１４は、図１３の位置算出結果に対応する姿勢角の時間変化を示すグラフである。図の見方は図１２と同じである。第２の位置算出システム２Ｂにより姿勢角の調整を行った場合も、時間経過に伴う姿勢角誤差の増大が抑えられ、真の姿勢角に近い姿勢角が得られていることがわかる。
【０１５３】
図１５は、第３の加速度検出システム１Ｃを適用した第３の位置算出システム２Ｃのシステム構成の一例を示す図である。第３の位置算出システム２Ｃでは、姿勢算出部３１０により算出された自動車の姿勢（算出姿勢）が速度ベクトル補正部３６０に入力され、且つ、車速検出システム８００により検出された自動車の速度が速度ベクトル補正部３６０に入力されるように構成されている。
【０１５４】
車速検出システム８００は、自動車の速度を検出するシステムであり、例えば車速パルスを備えて構成される。車速検出システム８００は、車軸の回転数に応じた電圧又はパルスを発生させることで車速を検出する。
【０１５５】
速度ベクトル補正部３６０は、姿勢算出部３１０から入力した算出姿勢と、車速検出システム８００から入力した車速とを用いて、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルを補正する。そして、その補正結果であるＮフレーム補正速度ベクトルを位置算出部３８０に出力する。
【０１５６】
３−２．姿勢センサー
上記の実施例では、ジャイロセンサーを姿勢センサーとして例示したが、姿勢センサーはこれに限られない。例えば、地磁気センサーとして知られる方位センサーを利用して移動体の姿勢を検出することとしてもよい。また、ジャイロセンサーに方位センサーや加速度センサーを組み合わせて姿勢センサーを構成してもよい。
【０１５７】
３−３．座標系
上記の実施例では、ローカル座標系の一例として機体座標系（Ｂフレーム）を挙げ、絶対座標系の一例として北東下座標系（Ｎフレーム）を挙げて説明した。しかし、これらの座標系はあくまでも一例であり、他の座標系を適用することとしてもよいことは勿論である。例えば、絶対座標系として、東北上座標系として知られるＥＮＵ（East North Up）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系を適用してもよい。
【０１５８】
３−４．検出姿勢の調整
上記の実施形態では、姿勢センサーの検出姿勢のうちのロール成分及びピッチ成分の両方について調整を行うものとして説明したが、ロール成分又はピッチ成分の何れか一方のみについて調整を行うこととしてもよい。例えば、ロール成分のみについて調整を行う場合は、式（８）をロール角“φ”の式に書き換えてロール角誤差“Δφ”の推定演算を行えばよい。また、ピッチ成分のみについて調整を行う場合は、式（８）をピッチ角“θ”の式に書き換えてピッチ角誤差“Δθ”の推定演算を行えばよい。
【０１５９】
３−５．姿勢角誤差の推定演算
また、上記の実施形態で説明した姿勢角誤差の推定演算はあくまでも一例であり、適宜設計変更可能である。例えば、上記の実施形態では、式（８）〜式（１０）に基づいて、求根アルゴリズムの一種であるニュートン法を用いて姿勢角誤差を推定するものとして説明した。求根アルゴリズムとしては、ニュートン法の他にも、割線法や挟み撃ち法といったアルゴリズムを適用することができることは勿論である。
【０１６０】
また、上記の実施形態とは異なり、カルマンフィルターを利用して姿勢角誤差を推定するように構成することも可能である。この場合は、例えば、姿勢角誤差を状態推定値（状態ベクトル）として設定し、加速度センサーの検出加速度ベクトルと移動体の移動に係る参照ベクトルから求めた姿勢調整用加速度ベクトルとの差分を観測値（観測ベクトル）として設定すればよい。そして、予測演算（時刻更新）と補正演算（観測更新）とを繰り返し行って、最適な状態推定値（姿勢角誤差）を推定する。
【０１６１】
３−６．電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【０１６２】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の加速度検出及び位置算出を実現することが可能である。
【０１６３】
３−７．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムであってもよいことは勿論である。
【符号の説明】
【０１６４】
１ 加速度検出システム、 ２ 位置算出システム、 ３ 加速度センサー、 ４ 姿勢センサー、 ５ ジャイロセンサー、 ２０ 姿勢調整部、 ３０ 座標変換部、 ４０ 姿勢調整用加速度ベクトル算出部、 ５０ 速度ベクトル算出部、 ６０ 速度ベクトル補正部、 １００ ＧＰＳアンテナ、 ２００ ＧＰＳユニット、 ３００ 処理部、 ４００ 操作部、 ５００ 表示部、 ６００ ＩＭＵ、 ７００ 記憶部
【技術分野】
【０００１】
本発明は、加速度検出方法、位置算出方法及び加速度検出装置に関する。
【背景技術】
【０００２】
いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行って位置算出を行う技術も考案されている。
【０００３】
慣性航法演算においては、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、慣性航法演算を行うことで測定したデータを、ＧＰＳ（Global Positioning System）を利用して補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−２３９６４３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に開示されている技術では、慣性航法演算を行うことで測定した移動体の位置や速度、加速度、姿勢角、方位角といった慣性航法演算に係る諸量それぞれについて、カルマンフィルターを利用して、各諸量に含まれ得る誤差を推定している。この場合、慣性航法演算に係る全ての諸量についてカルマンフィルターを利用して誤差推定演算を行う必要があるため、演算量は膨大になりがちである。
【０００６】
慣性航法演算では、例えば移動体に設置された加速度センサーを用いるのであれば、加速度センサーが検出した検出加速度を積分することで移動体の速度を算出し、さらに、その速度を積分することで位置を算出する。
【０００７】
この場合に算出される速度や位置に含まれ得る誤差は、そもそも加速度センサーの検出加速度に含まれ得る誤差に起因するものである。つまり、検出加速度に対して積分を繰り返すために、検出加速度に含まれていた誤差が、時間経過に伴って、速度や位置に誤差として重畳・累積されていくことによるものである。そのため、移動体の加速度を正しく検出することができれば、特許文献１のように慣性航法演算に係る全ての諸量の誤差を推定せずとも、慣性航法演算の精度を向上させることが可能であると考えられる。
【０００８】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体の加速度をより適切に検出するための新しい手法を提案することにある。
【課題を解決するための手段】
【０００９】
以上の課題を解決するための第１の形態は、移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整することと、前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求めることと、前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換することと、を含む加速度検出方法である。
【００１０】
また、他の形態として、移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部と、前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める変換係数算出部と、前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換する変換部と、を備えた加速度検出装置を構成してもよい。
【００１１】
この第１の形態等によれば、移動体に設置された姿勢センサーの検出姿勢を、移動体に設置された加速度センサーの検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。そして、調整された検出姿勢を用いて、加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める。そして、求めた変換係数を用いて、加速度センサーの検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換する。
【００１２】
加速度センサーが検出する加速度ベクトルは、加速度センサーに対応付けられたローカルな座標系における加速度ベクトルである。それに対して、例えば慣性航法演算においては、移動体の移動空間を定める絶対的な座標系を基準として演算を行うことが一般的である。姿勢センサーの検出姿勢には誤差が含まれ得るため、姿勢センサーの検出姿勢を、加速度センサーの検出加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整する。その上で、ローカル座標系から絶対座標系への変換係数を求めて座標変換を行うことで、絶対座標系における加速度ベクトルを適切に検出することができる。このようにして検出された絶対座標系における加速度ベクトルは、上記のように慣性航法演算等に好適である。
【００１３】
また、第２の形態として、第１の形態の加速度検出方法であって、前記変換係数を求めることは、前記検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な前記変換係数を求めることである、加速度検出方法を構成してもよい。
【００１４】
この第２の形態によれば、検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な変換係数を求める。そのため、当該変換係数を用いて加速度センサーの検出加速度ベクトルを絶対座標系における加速度ベクトルに変換することで、加速度センサーの検出加速度ベクトルに含まれ得る誤差を低減させることができる。
【００１５】
また、第３の形態として、第１又は第２の形態の加速度検出方法であって、前記調整することと、前記変換係数を求めることと、前記座標変換することと、を前記移動体の移動中に行う、加速度検出方法を構成してもよい。
【００１６】
この第３の形態によれば、姿勢センサーの検出姿勢の調整と、変換係数を求めることと、検出加速度ベクトルの座標変換と、を移動体の移動中に行うことで、移動中に移動体の姿勢が変化した場合であっても、移動体の加速度ベクトルを適切に検出することができる。
【００１７】
また、第４の形態として、第１〜第３の何れかの形態の加速度検出方法であって、前記調整することは、前記検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することを含む、加速度検出方法を構成してもよい。
【００１８】
この第４の形態によれば、検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することで、検出姿勢の調整を簡易に行うことができる。
【００１９】
また、第５の形態として、第１〜第４の何れかの形態の加速度検出方法であって、前記参照ベクトルは、前記移動体に設置された所定の速度計測システムによって計測された前記移動体の速度ベクトルを含み、前記調整することは、前記速度ベクトルを用いて、前記移動体の加速度ベクトルを算出することと、前記検出加速度ベクトルと前記算出した加速度ベクトルとを用いて、前記検出姿勢を調整することと、を含む、加速度検出方法を構成してもよい。
【００２０】
この第５の形態によれば、参照ベクトルには、移動体に設置された所定の速度計測システムによって計測された移動体の速度ベクトルが含まれる。この場合において、速度計測システムによって計測された移動体の速度ベクトルを用いて、移動体の加速度ベクトルを算出する。そして、加速度センサーの検出加速度ベクトルと算出した加速度ベクトルとを用いて、姿勢センサーの検出姿勢を調整する。速度計測システムによって計測された移動体の速度ベクトルを利用することで、姿勢センサーの検出姿勢の調整を適切に行うことができる。
【００２１】
より具体的には、第６の形態のように、第５の形態における速度計測システムが、車速検出システム、又は、衛星測位システムである、加速度検出方法を構成することも可能である。
【００２２】
この第６の形態によれば、車速検出システム、又は、衛星測位システムを利用することで、移動体の速度ベクトルを精度良く計測することが可能となり、移動体の加速度ベクトルの算出に供することができる。
【００２３】
また、第７の形態として、第１〜第６の何れかの形態の加速度検出方法を用いて前記絶対座標系における加速度ベクトルを検出することと、所与の移動基準位置と前記検出された加速度ベクトルとを用いて、前記絶対座標系における前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【００２４】
この第７の形態によれば、所与の移動基準位置と上記の形態の加速度検出方法を用いて検出した加速度ベクトルとを用いることで、移動体の位置を高い正確性で算出することができる。
【図面の簡単な説明】
【００２５】
【図１】加速度検出システムのシステム構成の説明図。
【図２】第１の加速度検出システムのシステム構成の説明図。
【図３】第２の加速度検出システムのシステム構成の説明図。
【図４】第３の加速度検出システムのシステム構成の説明図。
【図５】絶対座標速度ベクトル補正の原理の説明図。
【図６】ナビゲーションシステムのシステム構成の説明図。
【図７】カーナビゲーション装置の機能構成の説明図。
【図８】第１の位置算出システムのシステム構成の説明図。
【図９】ナビゲーション処理の流れを示すフローチャート。
【図１０】第１の位置算出システムによる位置算出結果の説明図。
【図１１】第１の位置算出システムによる姿勢算出結果の説明図。
【図１２】第２の位置算出システムのシステム構成の説明図。
【図１３】第２の位置算出システムによる位置算出結果の説明図。
【図１４】第２の位置算出システムによる姿勢算出結果の説明図。
【図１５】第３の位置算出システムのシステム構成の説明図。
【発明を実施するための形態】
【００２６】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００２７】
１．原理
１−１．システム構成
図１は、本実施形態における加速度検出方法を実現するための主要な機能ブロックで構成される加速度検出システム１のシステム構成図である。加速度検出システム１は、各種の移動体に適用されるシステムであり、移動体の加速度（詳細には加速度ベクトル）を検出して出力するシステムである。
【００２８】
加速度検出システム１は、慣性センサーの一種である加速度センサー３及び姿勢センサー４を備えて構成される。図１では、慣性センサーを二重線で図示し、慣性センサーの検出結果を利用して各種処理を行う処理ブロックを一重線で図示することで、両者を区別している。以降の図面についても同様である。
【００２９】
加速度センサー３は、移動体に設置されて移動体の加速度を検出するセンサーであり、例えばＭＥＭＳ（Micro Electro Mechanical Systems）の技術を利用したＭＥＭＳセンサーが用いられる。本実施形態において、加速度センサー３は、移動体を基準として３軸のローカル座標系における３軸のローカル座標加速度ベクトルを検出して出力するように構成されている。つまり、加速度センサー３に対応付けられたローカル座標系が移動体を基準とする座標系と一致するような姿勢で、加速度センサー３が移動体に設置されているものとして説明する。
【００３０】
姿勢センサー４は、移動体の姿勢を検出するセンサーであり、例えばジャイロセンサーのみで構成してもよいし、磁気センサーや加速度センサーをさらに組み合わせて構成してもよい。姿勢センサー４は移動体に固定され、直交３軸でなるローカル座標系における移動体の姿勢を検出する。すなわち、姿勢センサー４は移動体の姿勢角を計測するための計測器であり、姿勢センサー４が計測した値は、絶対座標系で表した移動体の向きとなる。
【００３１】
移動体の加速度及び速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則的に、単に加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。また、ローカル座標系において定義されるベクトルと、絶対座標系において定義されるベクトルとを区別するため、各変量を表す文言の先頭に座標系の種類を付して説明する。
【００３２】
座標変換の際には、加速度センサー３の検出加速度ベクトルのうち、加速度ベクトルの向きを座標変換し、加速度ベクトルの大きさは維持する。言い換えると、加速度センサー３の相対的な検出方向を、絶対座標系における方向（絶対方向）に座標変換する。
【００３３】
また、本明細書では、姿勢センサー４から出力される姿勢角を「検出姿勢角」と称し、この検出姿勢角によって定まる移動体の姿勢を「検出姿勢」と称する。また、検出姿勢角に対して姿勢調整部２０が調整を行うことで取得した姿勢角を「調整姿勢角」と称し、この調整姿勢角によって定まる移動体の姿勢を「調整姿勢」と称する。
【００３４】
加速度検出システム１は、主要な処理ブロックとして、姿勢調整部２０と、座標変換部３０とを有する。
【００３５】
姿勢調整部２０は、姿勢センサー４の検出姿勢を、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと、移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部である。
【００３６】
座標変換部３０は、姿勢調整部２０により調整された姿勢を用いて、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する変換部である。具体的には、姿勢調整部２０により調整された検出姿勢である調整姿勢を用いて、移動体のローカル座標系から絶対座標系への座標変換に係る変換係数（詳細には座標変換行列）を求める。そして、求めた変換係数を用いて、ローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する。詳細後述するように、座標変換行列は、座標変換に係る変換係数を成分とする行列である。
【００３７】
座標変換部３０は、座標変換に係る変換係数（座標変換行列）を求める変換係数算出部である。座標変換により絶対座標系における加速度ベクトルが算出されるため、座標変換部３０は、絶対座標加速度ベクトル算出部であるとも言える。
【００３８】
１−２．加速度検出の原理
次に、本実施形態における加速度検出の原理について説明する。最初に、姿勢センサー４の検出姿勢を、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと移動体の移動に係る所与の参照ベクトルとを用いて調整することが、本実施形態の特徴の１つである。次いで、調整された検出姿勢を用いて、加速度センサー３に対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数（座標変換行列）を求める。そして、求めた変換係数を用いて、加速度センサー３の検出加速度ベクトルを絶対座標系における加速度ベクトルに座標変換することで、移動体の加速度を検出する。
【００３９】
移動体の移動に係る参照ベクトルとしては、例えば、移動体の加速度ベクトルや速度ベクトルを適用することができる。参照ベクトルは、種々の手法によって取得することが可能である。例えば、移動体に設置された所定の速度計測システムから取得することができる。速度計測システムは、車速検出システム、又は、衛星測位システムである。
【００４０】
車速検出システムでは、例えば車速パルスによって移動体の速度の大きさを検出することができる。磁気センサー等の方位センサーを組み合わせて車速検出システムを構成した場合には、移動体の移動方向も併せて検出することができる。この場合、車速検出システムは、移動体の速度の大きさ及び移動方向を検出する。このため、車速検出システムから、参照ベクトルとして移動体の速度ベクトルを取得することができる。
【００４１】
衛星測位システムは、代表的にはＧＰＳ（Global Positioning System）である。ＧＰＳでは、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星からＧＰＳ受信機までの擬似距離等の情報に基づいて、ＧＰＳ受信機の位置座標及び時計誤差を求める位置計算を行う。なお、ＧＰＳを利用した位置計算は従来公知であるため、ここでは詳細な説明を省略する。
【００４２】
位置計算の他に、ＧＰＳでは、ＧＰＳ衛星及びＧＰＳ受信機の相対的な位置関係の変化により生ずるドップラーを利用して、ＧＰＳ受信機の速度ベクトルを算出することが可能である。すなわち、ＧＰＳ衛星から受信したＧＰＳ衛星信号のドップラー周波数を測定することで、ＧＰＳ受信機とＧＰＳ衛星との間の相対速度ベクトルを算出する。ＧＰＳ衛星の速度ベクトル（移動速度及び移動方向）は、衛星軌道データ（エフェメリスやアルマナック）から求めることができる。そのため、相対速度ベクトルとＧＰＳ衛星の速度ベクトルとを用いれば、移動体の速度ベクトル（移動速度及び移動方向）を算出することができる。このため、ＧＰＳから、参照ベクトルとして移動体の速度ベクトルを取得することができる。
【００４３】
移動体の速度ベクトルを時間微分すれば、移動体の加速度ベクトルが求まる。そのため、上記の速度計測システムから参照ベクトルとして取得した速度ベクトルを時間微分することで、移動体の加速度ベクトルを算出することができる。
【００４４】
そこで、本実施形態の姿勢調整方法では、上記の参照ベクトルを用いて、姿勢調整に用いる移動体の加速度ベクトル（以下、「姿勢調整用加速度ベクトル」と称す。）を算出する。そして、加速度センサー３の検出加速度ベクトルであるローカル座標加速度ベクトルと算出した姿勢調整用加速度ベクトルとを用いて、検出姿勢角に含まれる誤差（以下、「姿勢角誤差」と称す。）を推定する。この場合、姿勢調整用加速度ベクトルとして、絶対座標系における加速度ベクトルを算出すると好適である。最終的に、推定した姿勢角誤差を用いて検出姿勢角を調整する。
【００４５】
説明を分かり易くするために、座標系を具体的に例示して説明する。ここでは、ローカル座標系として機体座標系（Body Frame）（以下、「Ｂフレーム」と称する。）を適用し、絶対座標系として北東下座標系として知られるＮＥＤ（North East Down）座標系（以下、「Ｎフレーム」と称する。）を適用した場合を一例として説明する。
【００４６】
Ｂフレームは、例えば、移動体の前方を正とする前後方向をＸ軸（ロール軸）、右方を正とする左右方向をＹ軸（ピッチ軸）、鉛直下方を正とする上下方向をＺ軸（ヨー軸）とする三次元直交座標系である。それに対して、Ｎフレームは、例えば、北方を正とする南北方向をＮ軸、東方を正とする東西方向をＥ軸、鉛直下方を正とする高度方向をＤ軸とする三次元直交座標系である。
【００４７】
Ｂフレーム加速度ベクトルからＮフレーム加速度ベクトルへの変換は、次式（１）で定式化される。
【数１】
【００４８】
式（１）における“ｆb”及び“ｆn”は、それぞれＢフレーム加速度ベクトル及びＮフレーム加速度ベクトルであり、次式（２）及び（３）で与えられる。
【数２】
【数３】
【００４９】
また、式（１）における“Ｃbn”はＢフレームからＮフレームへの座標変換行列であり、次式（４）で与えられる。
【数４】
【００５０】
式（４）において、（φ，θ，Ψ）は、それぞれＢフレームにおけるＸ軸（ロール軸）回りの回転角（ロール角）、Ｙ軸（ピッチ軸）回りの回転角（ピッチ角）、Ｚ軸（ヨー軸）回りの回転角（ヨー角）である。また、座標変換行列“Ｃbn”の各成分は、ＢフレームからＮフレームへの座標変換に係る変換係数である。
【００５１】
式（１）〜（４）より、次式（５）が導かれる。
【数５】
【００５２】
ここで、式（５）をロール角“φ”及びピッチ角“θ”の関数とみなし、次式（６）のように定式化する。
【数６】
【００５３】
式（６）において、ハット「＾」付きの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の実測値を示し、ハット「＾」無しの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の真値を示す。また、デルタ「Δ」付きの姿勢角は、それぞれロール角“φ”及びピッチ角“θ”の実測値に含まれ得る誤差（姿勢角誤差）を示す。“Δφ”はロール角誤差であり、“Δθ”はピッチ角誤差である。
【００５４】
式（６）を一次の項までテイラー展開すると、次式（７）が導かれる。
【数７】
【００５５】
式（６）及び式（７）から、次式（８）が導かれる。
【数８】
【００５６】
式（８）の左辺について、第１項の“ｆn＝（ｆxn，ｆyn，ｆzn）”は、Ｎフレーム加速度ベクトルの真値を示し、第２項のハット＾付きの“Ｆn（φ，θ）＝（Ｆxn（φ，θ），Ｆyn（φ，θ），Ｆzn（φ，θ））”は、Ｎフレーム加速度ベクトルの実測値を示す。
【００５７】
Ｎフレーム加速度ベクトルの実測値であるハット＾付きの“Ｆn（φ，θ）”は、加速度センサー３の検出加速度ベクトルであるＢフレーム加速度ベクトル“ｆb＝（ｆxb，ｆyb，ｆzb）”と、姿勢センサー４の検出姿勢角（φ，θ，Ψ）とを、それぞれ式（５）に代入することで算出することができる。つまり、Ｎフレーム加速度ベクトルの実測値“Ｆn（φ，θ）”は既知である。
【００５８】
それに対して、Ｎフレーム加速度ベクトルの真値“ｆn＝（ｆxn，ｆyn，ｆzn）”は未知であり、その値を知ることはできない。しかし、本実施形態では、移動体の移動に係る参照ベクトルが外部から与えられる。そこで、参照ベクトルを用いて、絶対座標加速度ベクトルであるＮフレーム加速度ベクトルを算出し、これをＮフレーム加速度ベクトルの真値“ｆn＝（ｆxn，ｆyn，ｆzn）”とみなして計算する。参照ベクトルが十分信頼できるものであれば、参照ベクトルから算出されるＮフレーム加速度ベクトルを真値とみなして計算しても、十分な精度が保証される。
【００５９】
式（８）の右辺について、偏微分行列（ヤコビ行列）の各成分の値は、それぞれ式（５）を、ロール角及びピッチ角の実測値（ハット＾付きの（φ，θ））で偏微分することで求められる。姿勢角の実測値は、姿勢センサー４により検出された姿勢角（検出姿勢角）である。
【００６０】
式（８）において、例えば、左辺を“Δρ”と置き、右辺の偏微分行列を“Ｈ”と置き、右辺の姿勢角誤差を成分とする行列（姿勢角誤差行列）を“Δｘ”と置くと、式（８）は式（９）のように書き換えることができる。
【数９】
【００６１】
式（９）における未知数は、姿勢角誤差行列“Δｘ”のみである。そのため、式（９）は、次式（１０）のように解くことができる。
【数１０】
【００６２】
コンピューター（計算機）を用いた計算では、姿勢角誤差行列“Δｘ”を近似的に求めることになる。例えば、求根アルゴリズムの一種であり、反復法として知られるニュートン法を用いることで、姿勢角誤差行列“Δｘ”の近似解を求めることができる。
【００６３】
姿勢角誤差行列“Δｘ”が求まったならば、姿勢角誤差（ロール角誤差“Δφ”及びピッチ角誤差“Δθ”）を姿勢センサー４の検出姿勢角（ハット＾付きの（φ，θ，Ψ））に加算することで、検出姿勢角を調整する。
【００６４】
なお、本実施形態では、ロール角“φ”及びピッチ角“θ”に含まれる誤差を近似的に求め、これらの誤差を用いて、検出姿勢角のうちのロール角“φ”及びピッチ角“θ”を調整する。つまり、移動体の検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分のみを調整することになる。
【００６５】
上記のようにして求めた調整姿勢角を用いてローカル座標系から絶対座標系への座標変換行列（変換係数）を求め、これを用いて加速度センサー３の検出加速度ベクトルを座標変換すると、検出加速度ベクトルに含まれ得る誤差が座標変換により低減される。つまり、元々検出加速度ベクトルに含まれていた誤差よりも誤差が小さくなる。その理由は、以下の通りである。
【００６６】
加速度センサー３により検出されたローカル座標加速度ベクトルには、移動体の移動による誤差が含まれ得る。この誤差を含み得るローカル座標加速度ベクトルを座標変換した場合は、変換後の絶対座標加速度ベクトルにも当然に誤差が重畳されることとなる。
【００６７】
しかし、上記の原理で説明したように、本実施形態では、加速度センサー３により検出されたローカル座標加速度ベクトルそのものを用いて移動体の検出姿勢を調整し、その調整結果である調整姿勢角を用いて座標変換行列を求めている。これは、加速度センサー３の検出加速度ベクトルに誤差が含まれていることを前提とし、言ってみれば検出加速度ベクトルと辻褄が合うような変換係数を求めることを意味する。つまり、検出加速度ベクトルの誤差を検出姿勢の誤差に含めて考えることで、検出加速度ベクトルに含まれる誤差を低減させようとする考え方である。
【００６８】
上記の姿勢調整部２０による検出姿勢（検出姿勢角）の調整と、座標変換部３０による変換係数（座標変換行列）の算出と、座標変換部３０による座標変換とは、移動体の移動中に行うと好適である。勿論、移動体の停止時に行うこととしてもよいが、本実施形態の手法は、移動体の移動中に特に効果を発揮する。
【００６９】
本実施形態の手法とは別に、移動体が停止している場合に加速度センサー３により検出される加速度ベクトルを用いて、移動体の姿勢を推定する手法が考案されている。具体的には、次式（１１）及び（１２）に従って、ロール角“φ”及びピッチ角“θ”を推定する。
【数１１】
【数１２】
【００７０】
移動体が停止しているのであれば、移動体の姿勢は通常は変化しないため、式（１１）及び（１２）を用いることで、移動体の姿勢を正しく推定することができる。よって、移動体の停止時においては、式（１１）及び（１２）に従って推定したロール角“φ”及びピッチ角“θ”を用いて座標変換行列を求め、加速度センサー３の検出加速度ベクトルを座標変換することで、絶対座標加速度ベクトルを適切に求めることができる。
【００７１】
しかし、移動体の移動中は事態が異なる。移動体の移動により、移動体の姿勢は随時変化するようになる。すると、停止時に推定した姿勢角は正しくなくなり、停止時に推定した姿勢角から求めた座標変換行列は、加速度センサー３の検出加速度ベクトルとの関係で辻褄が合わなくなる。かかる座標変換行列を用いて検出加速度ベクトルを座標変換すると、検出加速度ベクトルに含まれる誤差を低減させることはできず、絶対座標加速度ベクトルには大きな誤差が残存してしまう。
【００７２】
その点、本実施形態の手法では、加速度センサー３の検出加速度ベクトルと辻褄が合うような座標変換行列を求めるため、移動中に移動体の姿勢が随時変化しても問題はなく、検出加速度ベクトルに含まれる誤差を低減させて、誤差の影響が抑制された絶対座標加速度ベクトルを得ることができる。
【００７３】
図１に示した加速度検出システム１は、本実施形態における加速度検出を実現するための最低限の構成要素を示したものである。実際には、加速度検出システムの種々のバリエーションが考えられる。そこで、加速度検出システムのバリエーションについて以下説明する。なお、加速度検出システム１と同一の機能ブロックについては同一の符号を付して説明を省略し、加速度検出システム１とは異なる部分を中心に説明する。
【００７４】
図２は、第１の加速度検出システム１Ａの説明図である。第１の加速度検出システム１Ａは、加速度検出システム１の各機能ブロックの他に、姿勢調整用加速度ベクトル算出部４０を備えて構成される。姿勢調整用加速度ベクトル算出部４０は、移動体の移動に係る参照ベクトルを用いて、姿勢角調整用の加速度ベクトル（姿勢角調整用加速度ベクトル）を算出する。
【００７５】
具体的には、姿勢調整用加速度ベクトル算出部４０は、参照ベクトルとして外部入力した移動体の参照速度ベクトルを用いて絶対座標加速度ベクトルを算出する。例えば、参照速度ベクトルとして絶対座標速度ベクトルが入力される場合は、当該絶対座標速度ベクトルを時間微分することで、絶対座標加速度ベクトルを算出する。そして、算出した絶対座標加速度ベクトルを、姿勢調整用加速度ベクトルとして姿勢調整部２０に出力する。
【００７６】
図３は、第２の加速度検出システム１Ｂの説明図である。第２の加速度検出システム１Ｂは、処理ブロックとして、姿勢調整部２０と、座標変換部３０と、姿勢調整用加速度ベクトル算出部４０と、速度ベクトル算出部５０と、速度ベクトル補正部６０とを有する。
【００７７】
速度ベクトル算出部５０は、座標変換部３０から入力した絶対座標加速度ベクトルを積分及び継ぎ足し処理することで、絶対座標系における移動体の速度ベクトル（絶対座標速度ベクトル）を算出する。そして、算出した絶対座標速度ベクトルを速度ベクトル補正部６０に出力する。ここで、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【００７８】
具体的には、速度ベクトル算出部５０は、所定時間分の絶対座標加速度ベクトルを積分することで、その所定時間の間における移動体の速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新した絶対座標速度ベクトルに継ぎ足すことで、絶対座標速度ベクトルを新たに算出・更新する。
【００７９】
速度ベクトル補正部６０は、速度ベクトル算出部５０から入力した絶対座標速度ベクトルを補正する。具体的には、移動体の移動に係る参照ベクトルとして外部入力した参照速度ベクトルを利用して、絶対座標速度ベクトルを補正する。参照速度ベクトルとしては、例えば絶対座標系における速度ベクトルを取得することができる。そのため、例えば、２種類の絶対座標速度ベクトルの平均処理を行うことで、絶対座標速度ベクトルを補正する。
【００８０】
この場合における平均処理としては、単純な算術平均を適用することもできるし、加重平均を適用することもできる。参照速度ベクトルを信頼して速度ベクトルを補正するのであれば、参照速度ベクトルの重みを速度ベクトル算出部５０により算出された速度ベクトルの重みよりも高くして加重平均すればよい。逆に、速度ベクトル算出部５０により算出された速度ベクトルを信頼して速度ベクトルを補正するのであれば、当該速度ベクトルの重みを参照速度ベクトルの重みよりも高くして加重平均すればよい。
【００８１】
速度ベクトル補正部６０は、補正結果として得られる絶対座標速度ベクトル（以下、「絶対座標補正速度ベクトル」と称す。）を外部出力し、且つ、姿勢調整用加速度ベクトル算出部４０にフィードバックする。姿勢調整用加速度ベクトル算出部４０は、速度ベクトル補正部６０から入力した絶対座標補正速度ベクトルを用いて姿勢調整用加速度ベクトルを算出し、姿勢調整部２０に出力する。
【００８２】
図４は、第３の加速度検出システム１Ｃの説明図である。第３の加速度検出システム１Ｃは、第２の加速度検出システム１Ｂの変形例である。具体的には、第２の加速度検出システム１Ｂとは異なり、第３の加速度検出システム１Ｃでは、姿勢センサー４の検出姿勢が、姿勢調整部２０と速度ベクトル補正部６０とにそれぞれ入力されるように構成されている。
【００８３】
速度ベクトル補正部６０は、移動体の移動に係る参照情報として外部入力した参照速度と、姿勢センサー４の検出姿勢とを用いて、速度ベクトル算出部５０から入力した絶対座標速度ベクトルを補正する。例えば、車速検出システムから参照情報を取得可能な場合において、車速検出システムが移動体の速度の大きさのみを検出可能に構成されている場合は、参照情報として速度の大きさを取得し、姿勢センサー４の検出姿勢と併用して、絶対座標速度ベクトルを補正する。
【００８４】
図５は、絶対座標速度ベクトルの補正の原理を説明するための図である。ここでは、絶対座標系としてＮフレームを考え、東西方向（Ｅ軸）及び南北方向（Ｎ軸）でなる２次元平面において速度ベクトルを補正する場合について図示・説明する。図５において、横軸は東西方向の軸（Ｅ軸）を示しており、縦軸は南北方向の軸（Ｎ軸）を示している。
【００８５】
Ｎフレーム加速度ベクトルの東西方向成分及び南北方向成分を積分して最後に更新したＮフレーム速度ベクトルに継ぎ足すと、Ｎフレーム速度ベクトルの東西方向成分及び南北方向成分が得られる。図５において、Ｅ軸上に記載した矢印がＮフレーム速度ベクトルの東西方向成分を示し、Ｎ軸上に記載した矢印がＮフレーム速度ベクトルの南北方向成分を示す。
【００８６】
この東西方向成分及び南北方向成分を合成することで、大きさが“｜Ｖ１｜”で、向きが“θ１”のＮフレーム速度ベクトルが得られる。すなわち、動径を“｜Ｖ１｜”、偏角を“θ１”とする円座標でＮフレーム速度ベクトルが定義される。但し、図５では、Ｎフレーム速度ベクトルの向きを、速度ベクトルとＥ軸との成す角度で表現している。上記のＮフレーム速度ベクトルが、速度ベクトル算出部５０により算出されるＮフレーム速度ベクトルである。
【００８７】
このＮフレーム速度ベクトルの大きさ及び向きを、例えば次のように補正する。先ず、Ｎフレーム速度ベクトルの大きさ“｜Ｖ１｜”（第１の速度）と、外部入力した参照速度（第２の速度）とを平均処理することで合成する。そして、その合成結果として得られる速度を、Ｎフレーム補正速度ベクトルの大きさ“｜Ｖ２｜”とする。
【００８８】
次に、Ｎフレーム速度ベクトルの向き“θ１”を、姿勢センサー４の検出姿勢に基づいて、移動体の移動方向“θ２”に合わせるように補正する。Ｎフレーム速度ベクトルの向き“θ１”は、Ｎフレーム補正加速度ベクトルを積分及び継ぎ足し処理して間接的に求められる向きであるのに対し、移動方向“θ２”は、姿勢センサー４の検出姿勢から直接的に定まる移動体の向きである。そのため、上記の補正により、Ｎフレーム補正速度ベクトルの向きは、移動体の移動方向に沿ったより正確な向きとなる。
【００８９】
２．実施例
次に、上記の加速度検出システムを適用した位置算出システム及びナビゲーションシステムの実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【００９０】
２−１．システム構成
図６は、本実施例におけるナビゲーションシステムのシステム構成の説明図である。ナビゲーションシステムは、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、加速度検出装置及び位置算出装置を具備した電子機器の一種であるカーナビゲーション装置１０００が搭載されたシステムである。
【００９１】
カーナビゲーション装置１０００は、自動車に搭載される電子機器であり、自動車の加速度ベクトルを算出する加速度検出装置及び自動車の位置を算出する位置算出装置を備えて構成される。また、カーナビゲーション装置１０００は、加速度センサー３及びジャイロセンサー５を有するセンサーユニットとしてＩＭＵ（Inertial Measurement Unit）６００を具備している。
【００９２】
ＩＭＵ６００は、慣性計測ユニットとして知られるセンサーユニットであり、自動車に対する相対的な座標系の加速度ベクトル及び角速度を検出して出力するように設計されている。本実施例では、この相対的な座標系がＢフレームと一致するような姿勢でカーナビゲーション装置１０００が自動車に設置・固定されているものとして説明する。
【００９３】
また、カーナビゲーション装置１０００は、衛星測位システムの一種であるＧＰＳを利用したセンサーユニットとしてＧＰＳユニット２００を具備している。ＧＰＳユニット２００は、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を利用して、例えばＮフレームにおける自動車の位置及び速度ベクトルを計測して出力可能に構成されている。
【００９４】
カーナビゲーション装置１０００は、ＩＭＵ６００の検出結果を利用して慣性航法演算処理を行うことで、自動車の位置を算出する。そして、当該算出位置に対するマップマッチング処理を行うなどしてナビゲーション画面を生成し、生成したナビゲーション画面を表示部５００であるディスプレイに表示させる。
【００９５】
２−２．機能構成
図７は、カーナビゲーション装置１０００の機能構成の一例を示すブロック図である。カーナビゲーション装置１０００は、主要な機能構成として、ＧＰＳアンテナ１００と、ＧＰＳユニット２００と、処理部３００と、操作部４００と、表示部５００と、ＩＭＵ６００と、記憶部７００とを備えて構成される。
【００９６】
ＧＰＳアンテナ１００は、測位用衛星の一種であるＧＰＳ衛星から発信されているＧＰＳ衛星信号を含むＲＦ（Radio Frequency）信号を受信するアンテナであり、受信信号をＧＰＳユニット２００に出力する。ＧＰＳ衛星信号は、拡散符号の一種であるＣＡ（Coarse and Acquisition）コードによって、スペクトラム拡散方式として知られるＣＤＭＡ（Code Division Multiple Access）方式によって変調された１．５７５４２［ＧＨｚ］の通信信号である。ＣＡコードは、コード長１０２３チップを１ＰＮフレームとする繰返し周期１ｍｓの擬似ランダム雑音符号であり、ＧＰＳ衛星毎に異なるものである。
【００９７】
ＧＰＳユニット２００は、ＧＰＳアンテナ１００から出力された信号に基づいて自動車の位置、移動速度及び移動方向を計測する回路或いは装置であり、いわゆるＧＰＳ受信装置に相当する機能ブロックである。図示を省略するが、ＧＰＳユニット２００は、ＲＦ受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、ＲＦ受信回路部と、ベースバンド処理回路部とは、それぞれ別のＬＳＩ（Large Scale Integration）として製造することも、１チップとして製造することも可能である。
【００９８】
ベースバンド処理回路部は、ＲＦ受信回路部から出力された受信信号に対して相関処理等を行ってＧＰＳ衛星信号を捕捉・追尾する。そして、ＧＰＳ衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置を算出する。また、ベースバンド処理回路部は、ＧＰＳ衛星から受信したＧＰＳ衛星信号のドップラー周波数に基づいて、所定の速度算出計算を行って自動車の速度ベクトルを算出する。本実施例では、ＧＰＳユニット２００は、自動車の位置（位置座標）及び速度ベクトルをＮフレームにおいて算出して出力可能に構成されているものとして説明する。
【００９９】
処理部３００は、記憶部７００に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１０００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。
【０１００】
処理部３００は、ＩＭＵ６００の検出結果を利用した慣性航法演算処理を行って自動車の位置（位置座標）を算出する。そして、算出した位置をもとに、自動車の現在位置を指し示した地図を表示部５００に表示させる処理を行う。
【０１０１】
図８は、原理で説明した第１の加速度検出システム１Ａを適用した位置算出システムである第１の位置算出システム２Ａのシステム構成の一例を示す図であり、処理部３００を機能ブロックとして表した概念図である。
【０１０２】
処理部３００は、姿勢算出部３１０と、姿勢調整部３２０と、座標変換部３３０と、姿勢調整用加速度ベクトル算出部３４０と、速度ベクトル算出部３５０と、位置算出部３８０とを機能部として有する。姿勢調整部３２０〜速度ベクトル算出部３５０は、原理で説明した加速度検出システムの姿勢調整部２０〜速度ベクトル算出部５０にそれぞれ対応する。
【０１０３】
姿勢算出部３１０は、ジャイロセンサー５により検出されたＢフレーム角速度を積分及び継ぎ足していくことで、自動車の姿勢角を算出する。すなわち、ジャイロセンサー５及び姿勢算出部３１０によって、原理で説明した加速度検出システムにおける姿勢センサー４が構成される。
【０１０４】
具体的には、姿勢算出部３１０は、ジャイロセンサー５により検出された所定時間分のＢフレーム角速度を積分することで、その所定時間の間の移動体の姿勢角の変化方向及び変化分を算出する。所定時間はＢフレーム角速度の検出時間間隔よりも長く設定され、通常は一定時間とされる。
【０１０５】
例えば、所定時間を１００ミリ秒とし、Ｂフレーム角速度の検出時間間隔（より正確にはジャイロセンサー５から出力された信号のサンプリング時間間隔）が１〜１０ミリ秒の間で適宜調整されるとした場合には、検出時間間隔に関わらず、１００ミリ秒毎に移動体の姿勢角の変化方向及び変化分を算出する。そして、１００ミリ秒毎に算出される姿勢角の変化方向及び変化分を最後に更新した姿勢角に継ぎ足すことで、最新の姿勢角を算出する。
【０１０６】
以下の説明では、姿勢算出部３１０が算出した姿勢角のことを「算出姿勢角」と称し、この算出姿勢角によって定まる自動車の姿勢のことを「算出姿勢」と称する。姿勢算出部３１０は、算出姿勢（算出姿勢角）を姿勢調整部３２０に出力する。
【０１０７】
姿勢調整用加速度ベクトル算出部３４０は、ＧＰＳユニット２００からＮフレームＧＰＳ速度ベクトルを参照速度ベクトルとして取得して、姿勢調整用加速度ベクトルを算出する。そして、算出した姿勢調整用加速度ベクトルを姿勢調整部３２０に出力する。
【０１０８】
位置算出部３８０は、速度ベクトル算出部３５０により算出された自動車のＮフレーム速度ベクトルを用いて、Ｎフレームにおける自動車の位置を算出する。具体的には、Ｎフレーム速度ベクトルを積分及び継ぎ足し処理することで、自動車の位置変化分を算出する。そして、自動車の所与の移動基準位置（初期位置）を基準として、算出した位置変化分を累積的に加算していくことで、自動車の位置を算出・更新する。移動基準位置（初期位置）は、例えばナビゲーションの開始位置としてユーザーにより入力された位置とすることができる。
【０１０９】
図７の説明に戻り、操作部４００は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部３００に出力する。この操作部４００の操作により、目的地の入力等の各種指示入力がなされる。
【０１１０】
表示部５００は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部３００から入力される表示信号に基づいた各種表示を行う表示装置である。表示部５００には、ナビゲーション画面等が表示される。
【０１１１】
ＩＭＵ６００は、加速度センサー３及びジャイロセンサー５を備え、Ｂフレームの各軸における加速度ベクトル及び各軸の軸回りの角速度を検出可能に構成されている。なお、加速度センサー３とジャイロセンサー５とは、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【０１１２】
記憶部７００は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１０００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【０１１３】
２−３．データ構成
図７に示すように、記憶部７００には、プログラムとして、処理部３００により読み出され、ナビゲーション処理（図９参照）として実行されるナビゲーションプログラム７０１が記憶されている。ナビゲーション処理については、フローチャートを用いて詳細に後述する。
【０１１４】
また、記憶部７００には、データとして、センサーデータ７０３と、参照データ７０５と、算出姿勢角データ７０７と、姿勢角誤差データ７０９と、調整姿勢角データ７１１と、移動ベクトルデータ７１３と、算出位置データ７１５とが記憶される。
【０１１５】
センサーデータ７０３は、ＩＭＵ６００により検出されたＢフレーム加速度ベクトル及びＢフレーム角速度が記憶されたデータである。
【０１１６】
参照データ７０５は、自動車の移動に係る参照値が記憶されたデータである。本実施例では、ＧＰＳユニット２００により計測された自動車のＮフレーム速度ベクトルが参照データ７０５として記憶される。
【０１１７】
算出姿勢角データ７０７は、姿勢算出部３１０により算出された自動車の姿勢角が記憶されたデータである。ロール成分、ピッチ成分及びヨー成分の姿勢角がそれぞれ算出されて記憶される。
【０１１８】
姿勢角誤差データ７０９は、姿勢調整部３２０が姿勢角誤差の推定処理を行って推定した姿勢角誤差が記憶されたデータである。例えば、ロール成分及びピッチ成分の誤差が算出されて記憶される。
【０１１９】
調整姿勢角データ７１１は、姿勢調整部３２０が算出姿勢角の調整を行うことで取得した自動車の調整姿勢角が記憶されたデータである。
【０１２０】
移動ベクトルデータ７１３は、自動車の移動状態を示すベクトルのデータであり、座標変換部３３０により算出されたＮフレーム加速度ベクトルや、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルといったデータが記憶される。
【０１２１】
算出位置データ７１５は、位置算出部３８０により算出された自動車の位置が記憶されたデータである。
【０１２２】
２−４．処理の流れ
図９は、記憶部７００に記憶されているナビゲーションプログラム７０１が処理部３００により読み出されて実行されることで、カーナビゲーション装置１０００において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、ＩＭＵ６００により検出された加速度ベクトル及び角速度が、記憶部７００のセンサーデータ７０３に随時記憶されるものとする。
【０１２３】
最初に、姿勢算出部３１０は、姿勢角算出処理を行う（ステップＡ１）。具体的には、ジャイロセンサー５の検出結果を積分及び継ぎ足し処理して自動車の姿勢角を算出し、算出姿勢角データ７０７として記憶部７００に記憶させる。
【０１２４】
次いで、処理部３００は、ＧＰＳユニット２００から参照データ７０５を取得したか否かを判定し（ステップＡ３）、参照データ７０５を取得しなかったと判定した場合は（ステップＡ３；Ｎｏ）、ステップＡ９へと処理を移行する。
【０１２５】
また、参照データ７０５を取得したと判定した場合は（ステップＡ３；Ｙｅｓ）、姿勢調整用加速度ベクトル算出部３４０が、姿勢調整用加速度ベクトル算出処理を行う（ステップＡ５）。すなわち、ＧＰＳユニット２００から取得したＮフレームＧＰＳ速度ベクトルを時間微分することで、Ｎフレームにおける姿勢調整用加速度ベクトルを算出する。
【０１２６】
次いで、姿勢調整部３２０は、姿勢角誤差推定処理を行う（ステップＡ７）。具体的には、加速度センサー３により検出されたＢフレーム加速度ベクトルと、ステップＡ５において算出した姿勢調整用加速度ベクトルと、ステップＡ１において算出した姿勢角とを用いて、例えばニュートン法を用いた反復計算を行って姿勢角誤差を推定する。そして、推定した姿勢角誤差を姿勢角誤差データ７０９として記憶部７００に記憶させる。
【０１２７】
その後、姿勢調整部３２０は、姿勢角誤差データ７０９に記憶された最新の姿勢角誤差を用いて、算出姿勢角データ７０７に記憶された最新の算出姿勢角を調整し、調整姿勢角データ７１１として記憶部７００に記憶させる（ステップＡ９）。
【０１２８】
次いで、座標変換部３３０は、Ｎフレーム加速度ベクトル算出処理を行う（ステップＡ１１）。具体的には、調整姿勢角データ７１１に記憶された最新の調整姿勢角を用いてＢフレームからＮフレームへの座標変換行列を求める。そして、当該座標変換行列を用いて加速度センサー３により検出されたＢフレーム加速度ベクトルをＮフレーム加速度ベクトルに変換し、移動ベクトルデータ７１３として記憶部７００に記憶させる。
【０１２９】
その後、速度ベクトル算出部３５０は、Ｎフレーム速度ベクトル算出処理を行う（ステップＡ１３）。すなわち、所定時間分のＮフレーム加速度ベクトルを積分し、最新のＮフレーム速度ベクトルに継ぎ足すことで、Ｎフレーム速度ベクトルを新たに算出する。そして、移動ベクトルデータ７１３として記憶部７００に記憶させる。
【０１３０】
次いで、位置算出部３８０は、位置算出処理を行う（ステップＡ１５）。具体的には、移動ベクトルデータ７１３に記憶されている所定時間分のＮフレーム速度ベクトルを積分及び継ぎ足し処理することで、移動体の位置変化分を算出する。そして、算出した位置変化分を、算出位置データ７１５に記憶されている最新の算出位置に加算することで、自動車の位置（以下、「慣性航法演算位置」と称す。）を新たに算出・更新する。
【０１３１】
なお、ステップＡ１５の位置算出処理において、ＧＰＳユニット２００からＧＰＳ算出位置を取得し、取得したＧＰＳ算出位置を加味して最終的な自動車の位置（最終位置）を決定するように構成することも可能である。例えば、慣性航法演算位置とＧＰＳ算出位置との平均処理を行うこととしてもよい。また、定期的なタイミングにおいて、ＧＰＳ算出位置で慣性航法演算位置をリセットすることとしてもよい。この場合は、ＧＰＳ算出位置を移動基準位置として、新たに慣性航法演算を開始することとなる。
【０１３２】
その後、位置算出部３８０は、ステップＡ１５で算出した最終位置に対してマップマッチング処理等を行い、表示部５００のナビゲーション画面を更新する（ステップＡ１７）。なお、マップマッチングに係る処理については従来公知であるため、ここでは詳細な説明を省略する。
【０１３３】
次いで、処理部３００は、ナビゲーション処理を終了するか否かを判定する（ステップＡ１９）。例えば、操作部４００を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。まだ処理を終了しないと判定した場合は（ステップＡ１９；Ｎｏ）、処理部３００は、ステップＡ１に戻る。また、終了すると判定した場合は（ステップＡ１９；Ｙｅｓ）、処理部３００は、ナビゲーション処理を終了する。
【０１３４】
２−５．実験結果
図１０は、上述した第１の位置算出システム２Ａにおいて実際に位置算出を行った実験結果の一例を示す図である。自動車を予め定められた周回経路に沿って１周させ、その場合に算出される位置を東西南北の２次元平面にプロットした。
【０１３５】
図１０において、横軸は東西方向、縦軸は南北方向（単位はメートル）をそれぞれ示している。東西方向０ｍ、南北方向０ｍの位置がスタート地点であり、出発地点から北方向に向かい、大きく反時計回り方向に１周周回するような経路を辿った。ゴール地点はスタート地点と同じである。また、自動車の真の軌跡を点線で、姿勢角の調整を行わずに自動車の位置を求めた軌跡を細実線で、姿勢角の調整を行って自動車の位置を求めた軌跡を太実線でそれぞれ示している。
【０１３６】
この図を見ると、姿勢角の調整を行わずに自動車の位置を求めた場合は、最初のうちはほぼ真の軌跡に沿った位置が得られているものの、時間経過に伴って徐々に真の軌跡から外れていき、最終的には大きな位置ずれが発生していることがわかる。これは、姿勢算出部３１０により算出された自動車の姿勢角に誤差が含まれているために、当該姿勢角を用いてＢフレーム加速度ベクトルを座標変換すると、Ｎフレーム加速度ベクトルに誤差が重畳されてしまうためである。
【０１３７】
それに対して、姿勢角の調整を行って自動車の位置を求めた場合は、ほぼ真の軌跡に沿った結果が得られていることがわかる。時間が経過しても真の軌跡から大きく外れることはなく、時間経過に伴う誤差の累積が防止されていることがわかる。
【０１３８】
図１１は、上記の位置算出における姿勢角の時間変化を示すグラフである。図１１（Ａ）はロール角“φ”の時間変化を示し、図１１（Ｂ）はピッチ角“θ”の時間変化を示し、図１１（Ｃ）はヨー角“Ψ”の時間変化を示す。
【０１３９】
各図において、自動車の真の姿勢角を点線で示す。ロール角“φ”及びピッチ角“θ”の真値はゼロである。また、調整を行っていない姿勢角（算出姿勢角）を細実線で示し、調整を行った姿勢角（調整姿勢角）を太実線で示す。なお、本実施形態ではヨー成分については調整を行っていないため、ロール成分及びピッチ成分にのみ調整結果が示されている。
【０１４０】
この結果を見ると、調整を行っていない姿勢角については、ロール角“φ”及びピッチ角“θ”の何れも、時間経過に伴い、真値から値がずれていく傾向があることがわかる。時間経過に伴って、姿勢角の真値からのずれ量（姿勢角誤差）の振幅が大きくなっていくことが特徴的である。図１０に示したように、この算出姿勢角の誤差は、検出加速度ベクトルの誤差と相まって、位置算出の正確性を大きく低下させる要因となっている。
【０１４１】
それに対して、調整姿勢角については、ロール角“φ”及びピッチ角“θ”の何れも、時間が経過しても、ほぼ真値に沿った値が得られていることがわかる。図１０の結果から明らかなように、本実施形態の手法で調整を施した姿勢角を用いて検出加速度を座標変換し、慣性航法演算を行って位置を算出することで、位置算出の正確性が向上する。
【０１４２】
２−６．作用効果
加速度検出システム１Ａにおいて、姿勢算出部３１０は、自動車に設置されたジャイロセンサー５が検出したＢフレーム角速度を用いて、自動車の姿勢（姿勢角）を算出する。姿勢調整部３２０は、姿勢算出部３１０により算出された自動車の姿勢を、自動車に設置された加速度センサー３が検出したＢフレーム加速度ベクトルとＧＰＳユニット２００が計測した参照速度ベクトルとを用いて調整する。すなわち、姿勢調整用加速度ベクトル算出部３４０により、参照速度ベクトルを用いて自動車のＮフレーム加速度ベクトルが算出される。姿勢調整部３２０は、Ｂフレーム加速度ベクトルとＮフレーム加速度ベクトルとを入力値として、姿勢角誤差を推定する所定の推定演算を行って姿勢角誤差を推定する。そして、姿勢調整部３２０は、推定した姿勢角誤差を用いて姿勢角を調整する。
【０１４３】
加速度センサー３が検出する加速度ベクトルは、加速度センサー３に対応付けられたローカルな座標系（例えばＢフレーム）における加速度ベクトルである。それに対し、例えば慣性航法演算においては、移動体の移動空間を定める絶対的な座標系（例えばＮフレーム）を基準として演算を行うことが一般的である。ジャイロセンサー５の検出結果には誤差が含まれ得るため、これを用いて算出される自動車の姿勢角にも誤差が含まれ得る。そこで、ジャイロセンサー５の検出結果を利用して算出した自動車の姿勢角を、加速度センサー３の検出加速度ベクトルとＧＰＳユニット２００の計測結果から求めた加速度ベクトルとを用いて調整する。その上で、ローカル座標系（Ｂフレーム）から絶対座標系（Ｎフレーム）への座標変換行列を求めることで、絶対座標系における加速度ベクトルを適切に求めることができる。
【０１４４】
座標変換部３３０は、姿勢調整部３２０により調整された姿勢角を用いて、ＢフレームからＮフレームへの座標変換行列を求める。この座標変換行列は、加速度センサー３により検出されたＢフレーム加速度ベクトルに含まれ得る誤差を座標変換で低減可能な座標変換行列である。つまり、調整姿勢角に基づき定義される座標変換行列を用いてＢフレーム加速度ベクトルを座標変換することで、Ｂフレーム加速度ベクトルに含まれ得る誤差を低減させ、誤差の影響が抑制されたＮフレーム加速度ベクトルを得ることができる。
【０１４５】
速度ベクトル算出部３５０は、座標変換部３３０により算出されたＮフレーム加速度ベクトルを用いてＮフレーム速度ベクトルを算出する。誤差が低減されたＮフレーム加速度ベクトルを用いてＮフレーム速度ベクトルを求めるため、加速度センサー３が検出したＢフレーム加速度ベクトルに含まれていた誤差がＮフレーム速度ベクトルに重畳することが防止され、時間経過に伴う誤差の累積を防止することができる。
【０１４６】
本実施形態の加速度検出方法では、加速度センサー３の検出加速度ベクトルに含まれ得る誤差を座標変換で低減可能な変換係数（変換行列）を求めて検出加速度ベクトルを座標変換するため、検出加速度ベクトルそれ自体の補正を必要とせずに、誤差の影響が抑制された絶対座標系の加速度ベクトルを検出することができる。そして、所与の移動基準位置と、検出した絶対座標系における加速度ベクトルとを用いることで、移動体の位置をより正確に求めることができる。
【０１４７】
３．変形例
本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
【０１４８】
３−１．位置算出システム
原理で説明した各種のバリエーションの加速度検出システムを適用して位置算出システムを構築することも勿論可能である。
【０１４９】
図１２は、第２の加速度検出システム１Ｂを適用した第２の位置算出システム２Ｂのシステム構成の一例を示す図である。第２の位置算出システム２Ｂでは、処理部３００は、姿勢算出部３１０と、姿勢調整部３２０と、座標変換部３３０と、姿勢調整用加速度ベクトル算出部３４０と、速度ベクトル算出部３５０と、速度ベクトル補正部３６０と、位置算出部３８０とを機能ブロックとして有する。
【０１５０】
速度ベクトル補正部３６０は、ＧＰＳユニット２００から参照速度ベクトルとしてＮフレームＧＰＳ速度ベクトルを取得し、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルを補正する。そして、その補正結果として得られるＮフレーム補正速度ベクトルを位置算出部３８０に出力する。また、Ｎフレーム補正速度ベクトルを姿勢調整用加速度ベクトル算出部３４０にフィードバックする。
【０１５１】
図１３は、第２の位置算出システム２Ｂを利用して実際に位置算出を行った実験結果の一例を示す図である。図の見方は図１０と同じである。この図を見ると、第２の位置算出システム２Ｂを利用して位置算出を行った場合も、自動車の真の軌跡に沿った軌跡が得られており、誤差の累積が抑えられていることがわかる。
【０１５２】
図１４は、図１３の位置算出結果に対応する姿勢角の時間変化を示すグラフである。図の見方は図１２と同じである。第２の位置算出システム２Ｂにより姿勢角の調整を行った場合も、時間経過に伴う姿勢角誤差の増大が抑えられ、真の姿勢角に近い姿勢角が得られていることがわかる。
【０１５３】
図１５は、第３の加速度検出システム１Ｃを適用した第３の位置算出システム２Ｃのシステム構成の一例を示す図である。第３の位置算出システム２Ｃでは、姿勢算出部３１０により算出された自動車の姿勢（算出姿勢）が速度ベクトル補正部３６０に入力され、且つ、車速検出システム８００により検出された自動車の速度が速度ベクトル補正部３６０に入力されるように構成されている。
【０１５４】
車速検出システム８００は、自動車の速度を検出するシステムであり、例えば車速パルスを備えて構成される。車速検出システム８００は、車軸の回転数に応じた電圧又はパルスを発生させることで車速を検出する。
【０１５５】
速度ベクトル補正部３６０は、姿勢算出部３１０から入力した算出姿勢と、車速検出システム８００から入力した車速とを用いて、速度ベクトル算出部３５０により算出されたＮフレーム速度ベクトルを補正する。そして、その補正結果であるＮフレーム補正速度ベクトルを位置算出部３８０に出力する。
【０１５６】
３−２．姿勢センサー
上記の実施例では、ジャイロセンサーを姿勢センサーとして例示したが、姿勢センサーはこれに限られない。例えば、地磁気センサーとして知られる方位センサーを利用して移動体の姿勢を検出することとしてもよい。また、ジャイロセンサーに方位センサーや加速度センサーを組み合わせて姿勢センサーを構成してもよい。
【０１５７】
３−３．座標系
上記の実施例では、ローカル座標系の一例として機体座標系（Ｂフレーム）を挙げ、絶対座標系の一例として北東下座標系（Ｎフレーム）を挙げて説明した。しかし、これらの座標系はあくまでも一例であり、他の座標系を適用することとしてもよいことは勿論である。例えば、絶対座標系として、東北上座標系として知られるＥＮＵ（East North Up）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系を適用してもよい。
【０１５８】
３−４．検出姿勢の調整
上記の実施形態では、姿勢センサーの検出姿勢のうちのロール成分及びピッチ成分の両方について調整を行うものとして説明したが、ロール成分又はピッチ成分の何れか一方のみについて調整を行うこととしてもよい。例えば、ロール成分のみについて調整を行う場合は、式（８）をロール角“φ”の式に書き換えてロール角誤差“Δφ”の推定演算を行えばよい。また、ピッチ成分のみについて調整を行う場合は、式（８）をピッチ角“θ”の式に書き換えてピッチ角誤差“Δθ”の推定演算を行えばよい。
【０１５９】
３−５．姿勢角誤差の推定演算
また、上記の実施形態で説明した姿勢角誤差の推定演算はあくまでも一例であり、適宜設計変更可能である。例えば、上記の実施形態では、式（８）〜式（１０）に基づいて、求根アルゴリズムの一種であるニュートン法を用いて姿勢角誤差を推定するものとして説明した。求根アルゴリズムとしては、ニュートン法の他にも、割線法や挟み撃ち法といったアルゴリズムを適用することができることは勿論である。
【０１６０】
また、上記の実施形態とは異なり、カルマンフィルターを利用して姿勢角誤差を推定するように構成することも可能である。この場合は、例えば、姿勢角誤差を状態推定値（状態ベクトル）として設定し、加速度センサーの検出加速度ベクトルと移動体の移動に係る参照ベクトルから求めた姿勢調整用加速度ベクトルとの差分を観測値（観測ベクトル）として設定すればよい。そして、予測演算（時刻更新）と補正演算（観測更新）とを繰り返し行って、最適な状態推定値（姿勢角誤差）を推定する。
【０１６１】
３−６．電子機器
上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【０１６２】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の加速度検出及び位置算出を実現することが可能である。
【０１６３】
３−７．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムであってもよいことは勿論である。
【符号の説明】
【０１６４】
１ 加速度検出システム、 ２ 位置算出システム、 ３ 加速度センサー、 ４ 姿勢センサー、 ５ ジャイロセンサー、 ２０ 姿勢調整部、 ３０ 座標変換部、 ４０ 姿勢調整用加速度ベクトル算出部、 ５０ 速度ベクトル算出部、 ６０ 速度ベクトル補正部、 １００ ＧＰＳアンテナ、 ２００ ＧＰＳユニット、 ３００ 処理部、 ４００ 操作部、 ５００ 表示部、 ６００ ＩＭＵ、 ７００ 記憶部
【特許請求の範囲】
【請求項１】
移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整することと、
前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求めることと、
前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換することと、
を含む加速度検出方法。
【請求項２】
前記変換係数を求めることは、前記検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な前記変換係数を求めることである、
請求項１に記載の加速度検出方法。
【請求項３】
前記調整することと、前記変換係数を求めることと、前記座標変換することと、を前記移動体の移動中に行う、
請求項１又は２に記載の加速度検出方法。
【請求項４】
前記調整することは、前記検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することを含む、
請求項１〜３の何れか一項に記載の加速度検出方法。
【請求項５】
前記参照ベクトルは、前記移動体に設置された所定の速度計測システムによって計測された前記移動体の速度ベクトルを含み、
前記調整することは、
前記速度ベクトルを用いて、前記移動体の加速度ベクトルを算出することと、
前記検出加速度ベクトルと前記算出した加速度ベクトルとを用いて、前記検出姿勢を調整することと、
を含む、
請求項１〜４の何れか一項に記載の加速度検出方法。
【請求項６】
前記速度計測システムは、車速検出システム、又は、衛星測位システムである、
請求項５に記載の加速度検出方法。
【請求項７】
請求項１〜６の何れか一項に記載の加速度検出方法を用いて前記絶対座標系における加速度ベクトルを検出することと、
所与の移動基準位置と前記検出された加速度ベクトルとを用いて、前記絶対座標系における前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項８】
移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部と、
前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める変換係数算出部と、
前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換する変換部と、
を備えた加速度検出装置。
【請求項１】
移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整することと、
前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求めることと、
前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換することと、
を含む加速度検出方法。
【請求項２】
前記変換係数を求めることは、前記検出加速度ベクトルに含まれ得る誤差を小さくすることが可能な前記変換係数を求めることである、
請求項１に記載の加速度検出方法。
【請求項３】
前記調整することと、前記変換係数を求めることと、前記座標変換することと、を前記移動体の移動中に行う、
請求項１又は２に記載の加速度検出方法。
【請求項４】
前記調整することは、前記検出姿勢のうちのヨー成分については調整を行わず、ロール成分及びピッチ成分の少なくとも一方を調整することを含む、
請求項１〜３の何れか一項に記載の加速度検出方法。
【請求項５】
前記参照ベクトルは、前記移動体に設置された所定の速度計測システムによって計測された前記移動体の速度ベクトルを含み、
前記調整することは、
前記速度ベクトルを用いて、前記移動体の加速度ベクトルを算出することと、
前記検出加速度ベクトルと前記算出した加速度ベクトルとを用いて、前記検出姿勢を調整することと、
を含む、
請求項１〜４の何れか一項に記載の加速度検出方法。
【請求項６】
前記速度計測システムは、車速検出システム、又は、衛星測位システムである、
請求項５に記載の加速度検出方法。
【請求項７】
請求項１〜６の何れか一項に記載の加速度検出方法を用いて前記絶対座標系における加速度ベクトルを検出することと、
所与の移動基準位置と前記検出された加速度ベクトルとを用いて、前記絶対座標系における前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項８】
移動体に設置された姿勢センサーの検出姿勢を、前記移動体に設置された加速度センサーの検出加速度ベクトルと前記移動体の移動に係る所与の参照ベクトルとを用いて調整する調整部と、
前記調整された前記検出姿勢を用いて、前記加速度センサーに対応付けられたローカル座標系から絶対座標系への座標変換に係る変換係数を求める変換係数算出部と、
前記変換係数を用いて、前記検出加速度ベクトルを前記絶対座標系における加速度ベクトルに座標変換する変換部と、
を備えた加速度検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−１５４７６９（Ｐ２０１２−１５４７６９Ａ）
【公開日】平成２４年８月１６日（２０１２．８．１６）
【国際特許分類】
【出願番号】特願２０１１−１３７４９（Ｐ２０１１−１３７４９）
【出願日】平成２３年１月２６日（２０１１．１．２６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年８月１６日（２０１２．８．１６）
【国際特許分類】
【出願日】平成２３年１月２６日（２０１１．１．２６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]