(57)【要約】【課題】移動体の加速度を適切に求めるための新たな手法の提案。【解決手段】移動体に設置された加速度センサー２が検出したローカル座標系であるＢフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー３の検出結果を用いて、絶対座標系であるＮフレームにおける加速度（Ｎフレーム加速度）に変換される。すなわち、ジャイロセンサー３の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Ｎフレーム加速度が算出される。そして、Ｎフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理を利用して推定され、当該バイアス値を用いてＮフレーム加速度が補正される。【選択図】図１
2012002678.tif 000002
【特許請求の範囲】
【請求項１】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、
を含む加速度算出方法。
【請求項２】
前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、
請求項１に記載の加速度算出方法。
【請求項３】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、
請求項２に記載の加速度算出方法。
【請求項４】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、
請求項３に記載の加速度算出方法。
【請求項５】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、
請求項３に記載の加速度算出方法。
【請求項６】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、
請求項２〜５の何れか一項に記載の加速度算出方法。
【請求項７】
前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、
請求項１に記載の加速度算出方法。
【請求項８】
請求項１〜７の何れか一項に記載の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、
前記移動体の向きを検出することと、
前記移動体の向きを用いて前記速度ベクトルを補正することと、
前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項９】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、
を備えた加速度算出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、加速度算出方法、位置算出方法及び加速度算出装置に関する。
【背景技術】
【０００２】
測位用信号を利用した測位システムとしては、ＧＰＳ（Global Positioning System）が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。ＧＰＳでは、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置座標と時計誤差とを求める位置算出計算を行う。
【０００３】
測位用信号を利用した位置算出では、種々の誤差要因によって位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、移動体の速度ベクトルを用いた慣性航法演算を行うことにより、ＧＰＳを利用して算出した位置を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平８−６８６５１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
従来から用いられている慣性航法演算では、例えば加速度センサーを用いるのであれば、加速度センサーにより検出された加速度を積分することで移動体の速度ベクトルを算出し、今回の速度ベクトルの方向に今回の速度ベクトル分の移動量（移動距離）を前回位置に加算することで、移動体の位置を新たに算出・更新する手法がとられる。ここで「ベクトル」とは、方向及び大きさを表す便宜上の用語である。方向及び大きさを表す表現としては、例えば直交座標系や球座標系における各軸の値としてもよいことは勿論であるが、何れの表記法をとったとしても「ベクトル」と等価であることは自明である。従って、本明細書においては、方向及び大きさの両方を表す用語として「ベクトル」を用いる。
【０００６】
加速度センサーは移動体に固定的に設置される。従って、加速度センサーが検出するのは、移動体自身を基準としたローカルな座標系（以下、「ローカル座標系」と称する。）における加速度である。しかし、慣性航法演算において求めたいのは、例えば地球表面といった移動体を含む座標系（以下、「絶対座標系」と称する。）における移動体の位置（位置座標）である。そのため、加速度センサーにより検出されたローカル座標系における加速度を、絶対座標系における加速度に変換して利用する必要がある。
【０００７】
ローカル座標系から絶対座標系への座標変換を行うためには、移動体の姿勢（より正確には絶対座標系での姿勢）の情報が必要となる。移動体の姿勢を正しく求めることができれば、座標変換を適切に行うことができる。例えば、移動体に設置されたジャイロセンサーにより検出された角速度を用いて、移動体の姿勢を算出することが考えられる。しかし、移動体の姿勢を正しく求めることは容易ではない。なぜなら、ジャイロセンサーの検出結果には、典型的にはゼロ点バイアスといった誤差成分が含まれるため、算出される移動体の姿勢にも誤差が含まれてしまうためである。
【０００８】
ここでもう１つ問題となるのは、加速度センサーの検出結果にもゼロ点バイアスといった誤差成分が含まれることである。そのため、ローカル座標系から絶対座標系に変換された加速度には、加速度センサーの誤差成分と、ジャイロセンサーの誤差成分との２重の誤差成分が含まれることになる。この２重の誤差成分を分離することは極めて困難である。慣性航法演算では、絶対座標系における移動体の加速度を積分しながら移動体の速度ベクトルを逐次的に求めていくため、速度ベクトルに含まれる誤差が時間経過に伴って累積的に増加するという大きな問題があった。
【０００９】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体の加速度を適切に求めるための新たな手法を提案することにある。
【課題を解決するための手段】
【００１０】
以上の課題を解決するための第１の形態は、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、を含む加速度算出方法である。
【００１１】
また、他の形態として、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、を備えた加速度算出装置を構成してもよい。
【００１２】
この第１の形態等によれば、移動体に設置された加速度センサーが検出した移動体のローカル座標系におけるローカル加速度が、移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換される。そして、変換された加速度に内在するバイアス値が、その変換された加速度と移動体の移動状態とを用いて推定され、当該バイアス値を用いて、絶対座標系に変換された加速度が補正される。
【００１３】
加速度センサーの検出結果及びジャイロセンサーの検出結果には、それぞれバイアス値が含まれ得るが、それらの大きさを個別に知ることは困難である。そこで、第１の形態等では、ジャイロセンサーの検出結果を用いてローカル座標系から絶対座標系に変換された加速度に着目した。そして、変換された加速度に内在するバイアス値を、その変換された加速度それ自体と移動体の移動状態とを用いて推定することとした。これにより、加速度センサーのバイアス値とジャイロセンサーのバイアス値とを分離せずとも、絶対座標系における加速度を適切に補正することが可能となり、移動体の加速度を適切に求めることができる。
【００１４】
また、第２の形態として、第１の形態の加速度算出方法であって、前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、加速度算出方法を構成してもよい。
【００１５】
この第２の形態によれば、バイアス値を状態推定値とし、絶対座標系に変換された加速度を観測値とするカルマンフィルター処理が行われて、当該加速度に内在するバイアス値が推定される。カルマンフィルターを利用した処理により、バイアス値の推定を適切に行うことができる。
【００１６】
また、第３の形態として、第２の形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、加速度算出方法を構成してもよい。
【００１７】
より具体的には、例えば第４の形態として、第３の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、加速度算出方法を構成してもよい。
【００１８】
この第４の形態によれば、無加速状態での想定誤差パラメーター値を、加減速状態の場合に比べて小さい値とする。想定誤差パラメーター値は、例えば、観測値に含まれ得る誤差の大きさを想定したパラメーター値である。想定誤差パラメーター値を小さくするということは、観測値に含まれ得る誤差を小さく見積もることを意味する。そのため、移動体の移動状態が無加速状態の場合には、観測値を信用・重視してバイアス値を推定することができる。
【００１９】
また、例えば第５の形態として、第３の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、加速度算出方法を構成することも可能である。
【００２０】
この第５の形態によれば、移動体の移動状態が無加速状態の場合には、状態推定値の観測値への追従性を加減速状態の場合よりも高める想定誤差パラメーター値とする。絶対座標系に変換された加速度を観測値とするため、移動体の移動状態が無加速状態の場合には、絶対座標系に変換された加速度に追従させるようにバイアス値を推定することができる。
【００２１】
また、第６の形態として、第２〜第５の何れかの形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、加速度算出方法を構成してもよい。
【００２２】
絶対座標系における加速度が過度に大きい場合に、その加速度をそのまま観測値としてカルマンフィルター処理を行うと、観測値の大きさに引きずられてバイアス値が適切に推定されなくなるおそれがある。かかる問題は移動体の移動状態が加減速状態の場合に起こり易い。そこで、第６の形態では、移動体の移動状態が加減速状態の場合に絶対座標系に変換された加速度が所定の限界値を超えた場合、当該限界値を観測値としてカルマンフィルター処理を行う。このように観測値の上限を定めることで、上記の問題を解決することができる。
【００２３】
また、第７の形態として、第１の形態の加速度算出方法であって、前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、加速度算出方法を構成してもよい。
【００２４】
移動体が無加速状態であれば、原理的には加速度はゼロであるため、加速度の検出値はバイアス値に相当する値であると言える。そこで、第７の形態では、移動体が無加速状態である場合の絶対座標系に変換された加速度の平均化処理を行い、その平均化された値を加速度に内在するバイアス値と推定する。
【００２５】
また、第８の形態として、第１〜第７の何れかの形態の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、前記移動体の向きを検出することと、前記移動体の向きを用いて前記速度ベクトルを補正することと、前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【００２６】
この第８の形態によれば、上述した加速度算出方法によって算出された加速度を用いて移動体の速度ベクトルが算出される。そして、検出した移動体の向きを用いて速度ベクトルが補正され、当該補正された速度ベクトルを用いて移動体の位置が算出される。速度ベクトルの算出とは別に移動体の向きを検出し、検出した移動体の向きを用いて速度ベクトルを補正することで、移動体の速度ベクトルを適切に求めることができる。そして、この速度ベクトルを用いることで、移動体の位置が正確に求まる。
【図面の簡単な説明】
【００２７】
【図１】加速度算出システムのシステム構成の説明図。
【図２】Ｎフレーム加速度ベクトルの南北成分の時間変化を示す図。
【図３】観測誤差の切替の説明図。
【図４】位置算出システムのシステム構成の説明図。
【図５】カーナビゲーション装置の機能構成の一例を示すブロック図。
【図６】処理部を機能ブロックとして表した概念図。
【図７】観測誤差設定用テーブルのテーブル構成の一例を示す図。
【図８】センサーデータのデータ構成の一例を示す図。
【図９】推定バイアス値データのデータ構成の一例を示す図。
【図１０】加速度ベクトルデータのデータ構成の一例を示す図。
【図１１】出力位置データのデータ構成の一例を示す図。
【図１２】ナビゲーション処理の流れを示すフローチャート。
【図１３】Ｎフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図１４】（Ａ）Ｎフレーム速度ベクトルの南北成分の時間変化。（Ｂ）Ｎフレーム速度ベクトルの東西成分の時間変化。（Ｃ）Ｎフレーム速度ベクトルの高度成分の時間変化。
【図１５】位置算出を行った実験結果を示す図。
【図１６】第２のＮフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図１７】変形例における処理部を機能ブロックとして表した概念図。
【図１８】第２のナビゲーション処理の流れを示すフローチャート。
【図１９】（Ａ）移動体の直進時におけるＮフレーム速度ベクトル補正の説明図。（Ｂ）移動体の回転時におけるＮフレーム速度ベクトル補正の説明図。
【発明を実施するための形態】
【００２８】
以下、図面を参照して、本発明の好適な実施形態について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００２９】
１．システム構成
図１は、本実施形態における加速度算出システム１のシステム構成の説明図である。加速度算出システム１は、各種の移動体に適用され、当該移動体の加速度を算出するシステムである。加速度算出システム１には、慣性センサーの一種である加速度センサー２及びジャイロセンサー３が含まれる。
【００３０】
加速度センサー２及びジャイロセンサー３は、移動体に設置され、移動体を基準としたローカル座標系におけるローカル加速度及びローカル角速度を検出して出力するように構成されている。本実施形態では、このローカル座標系を機体座標系（Body Frame）（以下、「Ｂフレーム」と称する。）として説明する。そして、本実施形態では、加速度センサー２により検出されたＢフレームにおける加速度を、絶対座標系の一種であるＮＥＤ（North East Down）座標系（以下、「Ｎフレーム」と称する。）における加速度に変換する。
【００３１】
移動体の加速度及び速度は、方向及び大きさを持っている。そのため、本明細書では、移動体の加速度及び速度を「加速度ベクトル」及び「速度ベクトル」として説明する。また、ローカル座標系である「Ｂフレーム」において定義されるベクトルと、絶対座標系である「Ｎフレーム」において定義されるベクトルとを区別するため、各変量を表す文言の先頭にフレームの種類を付して説明する。例えば、Ｂフレームにおける加速度ベクトルは「Ｂフレーム加速度ベクトル」と称し、Ｎフレームにおける加速度ベクトルは「Ｎフレーム加速度ベクトル」と称する。
【００３２】
Ｂフレームは、移動体の前方を正とする前後方向をＸ軸（ロール軸）、右方を正とする左右方向をＹ軸（ピッチ軸）、鉛直下方を正とする上下方向をＺ軸（ヨー軸）とする三次元直交座標系である。それに対し、Ｎフレームは、北方を正とする南北方向をＮ軸、東方を正とする東西方向をＥ軸、鉛直下方を正とする高度方向をＤ軸とする三次元直交座標系である。
【００３３】
座標変換の際には、加速度センサー２の検出結果（Ｂフレームの加速度ベクトル）のうち、加速度の向き（加速度ベクトルの向き）を座標変換し、加速度の大きさ（加速度ベクトルの大きさ）は維持する。言い換えると、加速度センサー２の相対的な検出方向を、絶対座標系における方向（絶対方向）に座標変換する。
【００３４】
加速度算出システム１は、処理ブロックとして、移動状態判定部１０と、姿勢算出部２０と、座標変換部３０と、バイアス推定部４０と、バイアス補正部５０とを備える。移動状態判定部１０は、移動体の移動状態を判定する。本実施形態では、移動状態判定部１０は、主に移動体が無加速である状態（以下、「無加速状態」と称する。）と、移動体が加速／減速している状態（以下、「加減速状態」と称する。）との何れの移動状態であるかを判定する。無加速状態には、移動体が停止している状態（停止状態）と、移動体が等速で移動している状態（等速状態）とが含まれる。
【００３５】
姿勢算出部２０は、ジャイロセンサー３により検出されたＢフレーム角速度を積分及び継ぎ足していくことで、移動体の姿勢角を算定する。ここで、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【００３６】
具体的には、姿勢算出部２０は、所定時間の間に検出されたＢフレーム角速度を「積分」することで、その所定時間の間の移動体の姿勢角の変化方向及び変化量を算出する。所定時間はＢフレーム角速度の検出時間間隔よりも長く設定され、通常は一定時間とされる。例えば、所定時間を１００ミリ秒とし、Ｂフレーム角速度の検出時間間隔（より正確にはジャイロセンサー３から出力された信号のサンプリング時間間隔）が１〜１０ミリ秒の間で適宜調整されるとした場合には、検出時間間隔がいくつであろうとも、必ず、１００ミリ秒毎に移動体の姿勢角の変化方向及び変化量を算出することとなる。
【００３７】
そして、１００ミリ秒毎に算出される姿勢角の変化方向及び変化量を、最後に更新した姿勢角に「継ぎ足す」ことで、最新の姿勢角（以下、この最新の姿勢角によって定まる移動体の最新の姿勢のことを「算定姿勢」と称する。）を算出・更新する。そして、その算定姿勢を座標変換部３０に出力する。移動体の姿勢がわかれば、移動体の移動方向もわかる。そのため、姿勢算出部２０は、移動体の移動方向を検出する移動方向検出部とも言える。
【００３８】
座標変換部３０は、姿勢算出部２０から入力した算定姿勢に基づいて、加速度センサー２の検出結果であるＢフレーム加速度ベクトルを、Ｎフレーム加速度ベクトルに変換する座標変換処理を行う。具体的には、姿勢算出部２０から入力した移動体の姿勢角を用いて、ＢフレームからＮフレームへの座標変換行列を算出する。そして、当該座標変換行列を用いて加速度センサー２の検出結果を変換して、バイアス推定部４０に出力する。座標変換部３０は、Ｎフレーム加速度ベクトルを算出する算出部とも言える。
【００３９】
バイアス推定部４０は、座標変換部３０から出力されるＮフレーム加速度ベクトルを用いて、当該Ｎフレーム加速度ベクトルに内在するバイアス値を推定する。ここで、バイアス値とは、Ｎフレーム加速度ベクトルの各成分にそれぞれ付加される誤差値（典型的にはゼロ点バイアス）のことを意味する。
【００４０】
本実施形態において、バイアス推定部４０は、カルマンフィルターを利用した推定処理（カルマンフィルター処理）を行ってバイアス値を推定する。カルマンフィルターは、離散的な誤差を有する観測値を用いて、あるシステムの時々刻々と変換する量を推定するための手法である。そして、バイアス推定部４０は、推定したバイアス値（以下、「推定バイアス値」と称す。）をバイアス補正部５０に出力する。
【００４１】
バイアス補正部５０は、バイアス推定部４０から出力される推定バイアス値を用いて、座標変換部３０から出力されたＮフレーム加速度ベクトルを補正する。すなわち、Ｎフレーム加速度ベクトルの各成分それぞれに対して、対応する成分のバイアス値を加減算する。そして、その結果をＮフレーム補正加速度ベクトルとして出力する。
【００４２】
２．原理
２−１．バイアス値の推定
本願発明者は、Ｎフレーム加速度ベクトルを実際に算出する実験を行った。移動体の移動状態を変化させて、慣性センサー（加速度センサー２及びジャイロセンサー３）を利用して慣性航法演算によりＮフレーム加速度ベクトルを算出した。また、衛星測位システムの一種であるＧＰＳ（Global Positioning System）を利用してＮフレームにおける移動体の速度ベクトルを算出し、その速度ベクトルを微分することでＮフレーム加速度ベクトルを同様に算出した。
【００４３】
図２は、この場合の実験結果の一例を示す図である。ここでは、Ｎフレーム加速度ベクトルの南北成分（南北方向の加速度）の時間変化を一例として示して説明する。図２において、横軸は時間を示し、縦軸は南北方向の加速度（単位はメートル毎秒毎秒［ｍ／ｓ2］）を示している。移動体の移動状態を、加減速状態→無加速状態→加減速状態の順に変化させてＮフレーム加速度を算出した。図２において、慣性航法を利用して算出したＮフレーム加速度には「慣性航法」、ＧＰＳを利用して算出したＮフレーム加速度には「ＧＰＳ」と付記した。
【００４４】
この図を見ると、グラフの中央部において、ＧＰＳを利用して算出したＮフレーム加速度がほぼ“０”となっている区間が存在する。この区間が、移動体が無加速状態であった区間である。理論上、移動体が無加速状態であれば進行方向の加速度は“０”である。このことから、ＧＰＳを利用して算出したＮフレーム加速度は正しいことを意味する。しかし、その一方で、慣性航法を利用して算出したＮフレーム加速度を見ると、無加速状態の区間において値が“０”となっておらず、マイナスの値を中心として上下に振動していることがわかる。
【００４５】
この無加速状態の区間におけるＮフレーム加速度の“０”からのズレは、Ｎフレームの加速度に含まれるバイアス値に相当するものと考えることができる。そこで、本願発明者は、移動体の無加速状態において観測されるＮフレーム加速度に着目し、Ｎフレーム加速度に内在するバイアス値を、Ｎフレーム加速度と移動体の移動状態とを用いて推定する手法を考案した。その手法の一例として、本実施形態では、カルマンフィルターを利用した推定処理を行ってＮフレーム加速度に含まれるバイアス値を推定する手法について説明する。
【００４６】
具体的な定式化を行う。本実施形態では、カルマンフィルターにおいて推定対象とするシステムの状態推定値“Ｘ”をバイアス値とし、次式（１）のように設定する。
【数１】
2012002678.tif 000003
但し、「（ｂN，ｂE，ｂD）」は、それぞれＮフレーム加速度ベクトルのバイアス値の南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。
【００４７】
また、状態推定値“Ｘ”の精度を表すために、状態推定値“Ｘ”を構成するバイアス値の各成分の誤差共分散“Ｐ”を表す行列（誤差共分散行列）を設定する。
【００４８】
カルマンフィルターでは、外部から与えられる観測値“Ｚ”を利用して１ステップ前に予測した状態推定値“Ｘ”及び誤差共分散“Ｐ”を補正する補正処理（観測更新）と、補正した状態推定値“Ｘ”及び誤差共分散“Ｐ”から次のステップの状態推定値“Ｘ”及び誤差共分散“Ｐ”を予測する予測処理（時刻更新）との２つの処理を繰り返し行う。そして、この繰り返し処理により、尤もらしい状態推定値“Ｘ”を推定する。
【００４９】
本実施形態では、Ｎフレーム加速度ベクトルの各成分を観測値“Ｚ”として入力する。すなわち、観測値“Ｚ”を次式（２）のように設定する。
【数２】
2012002678.tif 000004
但し、「（ＡN，ＡE，ＡD）」は、それぞれＮフレーム加速度ベクトルの南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。
【００５０】
補正処理（観測更新）は、次式（３）〜（５）で定式化される。
【数３】
2012002678.tif 000005
【数４】
2012002678.tif 000006
【数５】
2012002678.tif 000007
【００５１】
予測処理（時刻更新）は、次式（６）及び（７）で定式化される。
【数６】
2012002678.tif 000008
【数７】
2012002678.tif 000009
【００５２】
予測処理において、“Φ”は状態遷移行列と呼ばれ、補正処理で補正された状態推定値“Ｘ”から次の時刻の状態推定値“Ｘ”への遷移を表す行列として定義される。また、“Ｑ”はシステムノイズ（プロセスノイズ）と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。本実施形態では、これらの行列を次式（８）及び（９）のように設定する。
【数８】
2012002678.tif 000010
【数９】
2012002678.tif 000011
【００５３】
補正処理において、“Ｈ”は観測行列と呼ばれる行列である。観測行列“Ｈ”は、状態推定値“Ｘ”の次元を観測値“Ｚ”の次元に変換するための変換行列として用いられる。観測行列“Ｈ”を１ステップ前に予測された状態推定値“Ｘ”に作用させることで、観測値の予測値“ＨＸ”が得られる。本実施形態では、この観測行列“Ｈ”を次式（１０）のように設定する。
【数１０】
2012002678.tif 000012
但し、観測行列“Ｈ”の行と列の並びは、状態推定値“Ｘ”の３次元の成分の並び（バイアス値の南北成分、東西成分、高度成分）にそれぞれ対応する。
【００５４】
式（１０）を見ると、観測行列“Ｈ”は、対角成分が“１”であり、それ以外の成分が“０”である対角行列となっている。本実施形態では、状態推定値“Ｘ”をＮフレーム加速度ベクトルのバイアス値とし、観測値“Ｚ”をＮフレーム加速度ベクトルとしている。そのため、式（１０）の観測行列“Ｈ”を状態推定値“Ｘ”に作用させることは、Ｎフレーム加速度ベクトルのバイアス値を、Ｎフレーム加速度ベクトルにそのまま変換することを意味する。
【００５５】
式（３）において、“Ｋ”はカルマンゲインと呼ばれる補正係数である。カルマンゲイン“Ｋ”は、予測された状態推定値“Ｘ”を重視するか、実際の観測値“Ｚ”と予測された観測値“ＨＸ”との差“Ｚ−ＨＸ”を重視するかを決定付けるパラメーター値として用いられる。
【００５６】
式（４）は、実際の観測値“Ｚ”と予測された観測値“ＨＸ”との差“Ｚ−ＨＸ”を用いて、状態推定値“Ｘ”を補正することを表している。また、式（５）は、１ステップ前に予測された誤差共分散“Ｐ”とカルマンゲイン“Ｋ”と観測行列“Ｈ”とを用いて、誤差共分散“Ｐ”を補正することを表している。
【００５７】
ところで、カルマンゲイン“Ｋ”は、観測誤差“Ｒ”の大きさによって決定付けられる。観測誤差“Ｒ”は、観測値“Ｚ”に含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差“Ｒ”は、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。
【００５８】
本実施形態では、観測値“Ｚ”をＮフレーム加速度ベクトルの南北方向、東西方向及び高度方向の３成分としている。そのため、観測誤差“Ｒ”を、観測値“Ｚ”の各成分の共分散を表す３×３の行列（観測誤差行列）として定義する。観測誤差行列の対角成分には、正規分布（ガウス分布）における標準偏差“σ”の２乗値（＝分散“σ2”）を想定誤差値として設定する。
【００５９】
移動体の移動状態に応じた適切なバイアス推定を実現するために、本実施形態では、移動体の移動状態に応じて、想定誤差パラメーターである観測誤差“Ｒ”の値を切り替える処理を行う。すなわち、移動体の移動状態に応じて、観測誤差行列の対角成分に設定する想定誤差値を可変に設定する。以下、この観測誤差の切替の詳細について説明する。
【００６０】
２−２．観測誤差“Ｒ”の切替
図３は、観測誤差“Ｒ”の切替の説明図である。図３では、Ｎフレーム加速度ベクトルの南北方向、東西方向及び高度方向の成分のうちの南北成分（Ｎ成分）を例示している。横軸は時間である。また、模式的に、加速度の時間変化を点線で示し、推定バイアス値の理想的な時間変化を実線で示している。
【００６１】
移動体が無加速状態である場合は、移動体の移動分の加速度は“０”である。そのため、観測値“Ｚ”として与えられるＮフレーム加速度ベクトルが、Ｎフレーム加速度ベクトルのバイアス値に相当すると考えることができる。このことから、移動体が無加速状態である場合は、状態推定値“Ｘ”の観測値“Ｚ”への追従性を比較的高めるように観測誤差“Ｒ”を設定することが適切である。
【００６２】
そこで、移動体が無加速状態である場合は、観測誤差“Ｒ”を小さく設定する。具体的には、観測誤差“Ｒ”を、例えば次式（１１）のように設定する。
【数１１】
2012002678.tif 000013
【００６３】
観測誤差“Ｒ”を小さくすることで、無加速状態では、図３に示すように、観測値“Ｚ”に追従するように状態推定値“Ｘ”が推定されるようになる。その理由は、式（３）から導くことができる。式（３）において、観測誤差“Ｒ”を“０”に近付けると（Ｒ→０）、カルマンゲイン“Ｋ”は“Ｈ−１”に近づく。すると、式（４）は「Ｘ（ｔ）＝Ｚ（ｔ）」となる。これは、状態推定値“Ｘ”を観測値“Ｚ”とすること、すなわち観測値“Ｚ”を信用・重視して状態推定値“Ｘ”を補正することを意味する。
【００６４】
それに対して、移動体が加減速状態である場合は、移動体の移動分の加速度にバイアス値が重畳された値が観測される。すなわち、観測値“Ｚ”（Ｎフレーム加速度ベクトル）には、バイアス値の他に移動分の加速度が含まれるため、観測値“Ｚ”に追従させるように状態推定値“Ｘ”を補正することは適切ではない。よって、状態推定値“Ｘ”の観測値“Ｚ”への追従性を比較的劣化させるように観測誤差“Ｒ”を設定することが適切である。
【００６５】
そこで、移動体が加減速状態である場合は、観測誤差“Ｒ”を大きく設定する。具体的には、観測誤差“Ｒ”を、例えば次式（１２）のように設定する。
【数１２】
2012002678.tif 000014
【００６６】
観測誤差“Ｒ”を大きくすることで、加減速状態では、図３に示すように、無加速状態において推定されたバイアス値が維持されるようにバイアス値が推定される。なぜならば、式（３）からわかるように、観測誤差“Ｒ”を“＋∞”に近付けると（Ｒ→＋∞）、カルマンゲイン“Ｋ”は“０”に近づく。すると、式（４）は「Ｘ（ｔ）＝Ｘ（ｔ）」となる。これは、状態推定値“Ｘ”を１ステップ前に予測された状態推定値“Ｘ”とすること、すなわち１ステップ前に予測された状態推定値“Ｘ”を信用・重視して状態推定値“Ｘ”を補正することを意味する。無加速状態において推定されたバイアス値が加減速状態において維持されるため、バイアス値の推定が適切に行われる。
【００６７】
２−３．観測値“Ｚ”の調整
上述した観測誤差“Ｒ”の切替に加えて、本実施形態では、観測値“Ｚ”の調整（クリッピング）を行う。移動体が加減速状態である場合は、Ｎフレーム加速度ベクトルは大きく変動し得る。Ｎフレーム加速度ベクトルが過度に大きくなると、上述した観測誤差“Ｒ”の切替を行っても、バイアス値の推定が適切に行われなくなるおそれがある。
【００６８】
前述したように、加減速状態においては、観測誤差“Ｒ”を大きくしてカルマンゲイン“Ｋ”を“０”に近付けることにより、式（４）を「Ｘ（ｔ）＝Ｘ（ｔ）」と近似することが理想的である。しかし、観測値“Ｚ”が過度に大きい場合には、カルマンゲイン“Ｋ”を小さくしても、式（４）の右辺第２項の“Ｋ（Ｚ−ＨＸ）”がある程度大きな値を持つようになり、理想状態が崩れてしまう。すると、無加速状態において推定されたバイアス値が維持されずに、観測値“Ｚ”に引きずられて値が変動する可能性がある。
【００６９】
かかる問題に対処するため、先ず、観測値“Ｚ”の絶対値が所定の限界値である閾値“θ”を超えているか否かを判定する。そして、閾値“θ”を超えている場合は、観測値“Ｚ”を調整する処理を行う。具体的には、観測値“Ｚ”が負の値であり、その絶対値が閾値“θ”を超えている場合には（Ｚ＜−θ）、観測値“Ｚ”を“−θ”に調整する（Ｚ＝−θ）。また、観測値“Ｚ”が正の値であり、閾値“θ”を超えている場合には（Ｚ＞θ）、観測値“Ｚ”を“θ”に調整する（Ｚ＝θ）。
【００７０】
なお、この場合に限界値として設定する閾値“θ”の値については、適用するシステムに応じて適宜設定される。
【００７１】
３．実施例
次に、上記の加速度算出システム１を適用した位置算出システム５の実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【００７２】
３−１．システム構成
図４は、本実施例における位置算出システム５のシステム構成の説明図である。位置算出システム５は、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、加速度算出装置及び位置算出装置を具備した電子機器の一種であるカーナビゲーション装置６が搭載されたシステムである。
【００７３】
カーナビゲーション装置６は、自動車に搭載される電子機器であり、自動車の加速度を算出する加速度算出装置及び自動車の位置を算出する位置算出装置を備えて構成される。また、カーナビゲーション装置６は、加速度センサー２及びジャイロセンサー３を具備するセンサーユニットであるＩＭＵ（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）等の記憶装置によって構成され、カーナビゲーション装置６のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【００８９】
３−３．データ構成
図５に示すように、記憶部７００には、プログラムとして、処理部３００により読み出され、ナビゲーション処理（図１２参照）として実行されるナビゲーションプログラム７１０が記憶されている。また、ナビゲーションプログラム７１０には、Ｎフレーム加速度ベクトル算出処理として実行されるＮフレーム加速度ベクトル算出プログラム７１１と、Ｎフレーム加速度ベクトル補正処理（図１３参照）として実行されるＮフレーム加速度ベクトル補正プログラム７１２とがサブルーチンとして含まれている。
【００９０】
ナビゲーション処理とは、処理部３００が、Ｎフレーム加速度ベクトルの算出処理及び補正処理を行うとともに、補正したＮフレーム加速度ベクトルを用いてＮフレーム速度ベクトルを算出し、当該Ｎフレーム速度ベクトルを用いて自動車の位置を算定・更新する処理である。そして、算定位置をプロットしたナビゲーション画面を生成して、表示部５００に表示させる。ナビゲーション処理については、フローチャートを用いて詳細に後述する。
【００９１】
また、記憶部７００には、データとして、観測誤差設定用テーブル７２０と、センサーデータ７２１と、推定バイアス値データ７２３と、加速度ベクトルデータ７２５と、ＧＰＳデータ７２７と、出力位置データ７２９とが記憶される。
【００９２】
観測誤差設定用テーブル７２０は、観測誤差“Ｒ”を設定するために用いられるテーブルであり、そのテーブル構成例を図７に示す。観測誤差設定用テーブル７２０には、移動体の移動状態と対応付けて、観測誤差行列に設定する想定誤差値（分散“σ2”の値）が定められている。具体的には、無加速状態については、第１の想定誤差値である“１2”が定められており、加減速状態については、第２の想定誤差値である“１００2”が定められている。
【００９３】
センサーデータ７２１は、ＩＭＵ６００の検出結果が時系列に記憶されたデータであり、そのデータ構成例を図８に示す。センサーデータ７２１には、時刻７２１１と対応付けて、加速度センサー２により検出されたＢフレーム加速度７２１３と、ジャイロセンサー３により検出されたＢフレーム角速度７２１５とが時系列に記憶される。
【００９４】
推定バイアス値データ７２３は、推定されたＮフレーム加速度ベクトルのバイアス値についてのデータであり、そのデータ構成例を図９に示す。推定バイアス値データ７２３には、時刻７２３１と対応付けて、自動車の移動状態７２３３と、ＮＥＤの各成分の推定バイアス値７２３５とが時系列に記憶される。
【００９５】
加速度ベクトルデータ７２５は、補正前後におけるＮフレーム加速度ベクトルについてのデータであり、そのデータ構成例を図１０に示す。加速度ベクトルデータ７２５には、時刻７２５１と対応付けて、Ｎフレーム加速度ベクトル７２５３と、Ｎフレーム補正加速度ベクトル７２５５とが時系列に記憶される。
【００９６】
ＧＰＳデータ７２７は、ＧＰＳ位置算出部２００により算出・決定された位置（以下、「ＧＰＳ算定位置」と称す。）が時系列に記憶されたデータである。ＧＰＳデータ７２７は、例えば所定時間間隔（例えば１０分間隔）で更新される。
【００９７】
出力位置データ７２９は、表示部５００に表示出力させる位置のデータであり、そのデータ構成例を図１１に示す。出力位置データ７２９には、位置算出が行われた時刻７２９１と対応付けて、出力位置７２９３と、測位種別７２９５とが時系列に記憶される。
【００９８】
測位種別７２９５は、対応する出力位置７２９３が、ＧＰＳ位置算出部２００により算出された位置と、位置算出部３８０により算出された位置との何れの位置であるかを示すフラグである。ＧＰＳ位置算出部２００により算出された位置である場合は「ＧＰＳ」が、位置算出部３８０により算出された位置である場合は「慣性航法」がそれぞれ記憶される。
【００９９】
３−４．処理の流れ
図１２は、記憶部７００に記憶されているナビゲーションプログラム７１０が処理部３００により読み出されて実行されることで、カーナビゲーション装置６において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、ＩＭＵ６００により検出された加速度及び角速度が、記憶部７００のセンサーデータ７２１に随時記憶されるものとする。
【０１００】
最初に、処理部３００は、初期設定を行う（ステップＡ１）。例えば、Ｎフレーム加速度ベクトルのバイアス値の推定をカルマンフィルター処理を利用して行うために、カルマンフィルター処理に係る各種のパラメーター値の初期化を行う。
【０１０１】
その後、移動状態判定部３１０は、自動車の移動状態を判定する（ステップＡ３）。具体的には、ＩＭＵ６００の検出結果に基づいて、自動車が無加速状態であるか、加減速状態であるかを判定する。例えば、加速度センサー２により検出された自動車の進行方向の加速度が“０”に近い値として予め定められた所定の閾値以下又は未満の場合には、自動車は無加速状態であると判断することができる。
【０１０２】
次いで、処理部３００は、記憶部７００に記憶されているＮフレーム加速度ベクトル算出プログラム７１１を読み出して実行することで、Ｎフレーム加速度ベクトル算出処理を行う（ステップＡ５）。
【０１０３】
具体的には、姿勢算出部３２０がジャイロセンサー３の検出結果を積分及び継ぎ足し処理して自動車の姿勢を算出する。そして、座標変換部３３０が、ＢフレームからＮフレームへの座標変換行列を求め、当該座標変換行列を用いて、加速度センサー２のＢフレームにおける検出結果をＮフレームにおける検出結果に変換して、Ｎフレーム加速度ベクトルとする。なお、ＢフレームからＮフレームへの座標変換行列については従来公知であるため、ここでは数式等を用いた詳細な説明を省略する。
【０１０４】
次いで、処理部３００は、記憶部７００に記憶されているＮフレーム加速度ベクトル補正プログラム７１２を読み出して実行することで、Ｎフレーム加速度ベクトル補正処理を行う（ステップＡ７）。
【０１０５】
図１３は、Ｎフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
先ず、バイアス推定部３４０は、状態推定値“Ｘ”及び誤差共分散“Ｐ”に初期値を設定する（ステップＢ１）。初回の処理では、状態推定値“Ｘ”及び誤差共分散“Ｐ”は任意の値（例えば“０”）とすることができる。また、２回目以降の処理では、前回の処理で求められた状態推定値“Ｘ”及び誤差共分散“Ｐ”（最新の値）とすることができる。
【０１０６】
次いで、バイアス推定部３４０は、記憶部７００に記憶された観測誤差設定用テーブル７２０を参照して、観測誤差“Ｒ”を設定する。具体的には、移動状態が無加速状態であれば（ステップＢ３；無加速状態）、第１の想定誤差値（図７の観測誤差設定用テーブル７２０では“１2”）を観測誤差行列に設定する（ステップＢ５）。また、移動状態が加減速状態であれば（ステップＢ３；加速／減速状態）、第２の想定誤差値（図７の観測誤差設定用テーブル７２０では“１００2”）を観測誤差行列に設定する（ステップＢ７）。
【０１０７】
次いで、バイアス推定部３４０は、式（３）に従ってカルマンゲイン“Ｋ”を算出する（ステップＢ９）。そして、バイアス推定部３４０は、Ｎフレーム加速度ベクトル算出処理で算出したＮフレーム加速度ベクトルの各成分それぞれに対する閾値判定を行う（ステップＢ１１）。そして、当該成分の大きさが所定の閾値“θ”を超えている場合は（ステップＢ１１；Ｙｅｓ）、当該成分を閾値“θ”として観測値“Ｚ”を設定する（ステップＢ１３）。また、当該成分の大きさが閾値“θ”を超えていない場合は（ステップＢ１１；Ｎｏ）、当該成分をそのままとして観測値“Ｚ”を設定する（ステップＢ１５）。
【０１０８】
その後、バイアス推定部３４０は、式（４）及び（５）に従って状態推定値“Ｘ”及び誤差共分散“Ｐ”を補正（観測更新）する（ステップＢ１７）。そして、バイアス推定部３４０は、収束条件が成立したか否かを判定する（ステップＢ１９）。例えば、補正処理で得られた誤差共分散“Ｐ”に基づいて、バイアス値に含まれる誤差値が“０”に近い値として予め定められた所定値未満となった場合に、収束条件が成立したと判定する。
【０１０９】
そして、まだ収束条件が成立していないと判定した場合は（ステップＢ１９；Ｎｏ）、バイアス推定部３４０は、式（６）及び（７）に従って状態推定値“Ｘ”及び誤差共分散“Ｐ”を予測（時刻更新）した後（ステップＢ２１）、ステップＢ９に戻る。ここまでの処理は、Ｎフレーム加速度ベクトルのバイアス値を推定するためのカルマンフィルター処理に相当する。
【０１１０】
ステップＢ１９において収束条件が成立したと判定した場合は（ステップＢ１９；Ｙｅｓ）、バイアス補正部３５０は、状態推定値“Ｘ”に含まれる推定バイアス値を用いて、元のＮフレーム加速度ベクトルを補正する（ステップＢ２３）。すなわち、元のＮフレーム加速度ベクトルの南北方向、東西方向及び高度方向の各成分に対して、バイアス値の南北方向、東西方向及び高度方向の各成分を加減算する。
【０１１１】
このようにしてバイアス補正を行った後、処理部３００は、Ｎフレーム加速度ベクトル補正処理を終了する。
【０１１２】
図１２のナビゲーション処理に戻って、Ｎフレーム加速度ベクトル補正処理を行った後、速度ベクトル算出部３６０は、Ｎフレーム速度ベクトル算出処理を行う（ステップＡ９）。具体的には、所定時間分のＮフレーム補正加速度ベクトルを積分し、最新のＮフレーム速度ベクトルに継ぎ足すことで、Ｎフレーム速度ベクトルを新たに算出する。
【０１１３】
次いで、位置算出部３８０は、位置算出処理を行う（ステップＡ１１）。具体的には、Ｎフレーム速度ベクトル算出処理で算出したＮフレーム速度ベクトルを用いて、Ｎフレームにおける自動車の位置を算定する。すなわち、前回の出力位置を、Ｎフレーム速度ベクトルから求められる移動方向に移動距離分だけ移動させた位置を新たな位置として算定・更新する。そして、算定位置を出力位置に決定する。
【０１１４】
次いで、位置算出部３８０は、ＧＰＳ位置算出部２００からＧＰＳデータ７２７を取得したか否かを判定し（ステップＡ１３）、取得したと判定した場合は（ステップＡ１３；Ｙｅｓ）、出力位置のリセットタイミングであるか否かを判定する（ステップＡ１５）。
【０１１５】
出力位置のリセットタイミングは、種々のタイミングを設定することができる。例えば、自動車が停止したタイミングとすることができる。自動車の停止判定は、例えば速度ベクトルの大きさに基づいて判定することが可能である。すなわち、速度ベクトルの大きさが“０”に近い値として予め定められた所定の閾値以下又は未満である場合に、自動車は停止していると判定することができる。
【０１１６】
また、例えば、自動車がトンネル内などのＧＰＳ測位が不可能な環境から、トンネルの外などのＧＰＳ測位が可能な環境に変化したタイミング（ＧＰＳ衛星信号の捕捉ができなくなってから捕捉ができるようになったタイミング）をリセットタイミングとしてもよい。他には、例えば所定時間が経過する毎（例えば３０分毎）のタイミングをリセットタイミングとしてもよい。
【０１１７】
出力位置のリセットタイミングであると判定した場合は（ステップＡ１５；Ｙｅｓ）、位置算出部３８０は、リセット処理を行う（ステップＡ１７）。具体的には、ステップＡ１１で決定した出力位置を、ＧＰＳ位置算出部２００から取得したＧＰＳデータ７２７のＧＰＳ算出位置でリセットする。すなわち、出力位置をＧＰＳ算出位置に変更する。
【０１１８】
一方、ステップＡ１３においてＧＰＳデータ７２７を取得できなかったと判定した場合（ステップＡ１３；Ｎｏ）、又は、ステップＡ１５においてリセットタイミングではないと判定した場合は（ステップＡ１５；Ｎｏ）、位置算出部３８０は、ステップＡ１９へと処理を移行する。
【０１１９】
次いで、位置算出部３８０は、最新の出力位置で表示部５００のナビゲーション画面の表示を更新する（ステップＡ１９）。そして、位置算出部３８０は、位置算出を終了するか否かを判定する（ステップＡ２１）。例えば、操作部４００を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、位置算出を終了すると判定する。
【０１２０】
位置算出部３８０は、まだ位置算出を終了しないと判定した場合は（ステップＡ２１；Ｎｏ）、ステップＡ３に戻る。また、位置算出を終了すると判定した場合は（ステップＡ２１；Ｙｅｓ）、位置算出部３８０は、ナビゲーション処理を終了する。
【０１２１】
３−５．実験結果
次に、上述した位置算出システム５において、Ｎフレームにおける速度ベクトルの算出及び位置の算出を行った実験結果について説明する。
【０１２２】
図１４は、Ｎフレーム速度ベクトルの算出を行った結果を示す図である。図１４（Ａ）〜図１４（Ｃ）は、それぞれＮフレーム速度ベクトルの南北方向、東西方向及び高度方向の各成分の時間変化を示している。移動体の真の速度を実線で、従来の手法で算出した速度を破線で、本実施例の手法で算出した速度を点線でそれぞれ示している。
【０１２３】
この結果を見ると、従来の手法で算出した速度は、時間経過に伴って誤差が累積し、真の速度から乖離していく傾向があることがわかる。特に、高度方向の速度成分の結果が顕著であり、真の速度はほぼゼロであるのに対し、従来の手法で算出した速度は時間経過に伴って線形的に減少しており、誤差が増大していることがわかる。
【０１２４】
一方、本実施例の手法で算出した速度の時間変化は、真の速度の時間変化とほぼ一致していることがわかる。特に、高度方向の速度成分の結果について大きな効果が得られており、Ｎフレーム速度ベクトル算出の正確性が大きく向上していることがわかる。
【０１２５】
図１５は、位置算出を行った実験結果を示す図である。上述した位置算出システム５において、自動車を予め定められた周回経路に沿って１周させ、従来の手法で算出した位置と、本実施例の手法で算定した位置とを東西南北の２次元平面にプロットした。
【０１２６】
図１５において、横軸は東西方向、縦軸は南北方向（単位はメートル）をそれぞれ示している。東西方向０ｍ、南北方向０ｍの位置がスタート地点であり、出発地点から１５０ｍ程度東方向に移動した後に、北方向に向かい、大きく時計回り方向に１周周回するような経路を辿った。ゴール地点はスタート地点と同じである。また、図１４と同様に、移動体の真の軌跡を実線で、従来の手法で算出した軌跡を破線で、本実施例の手法で算出した軌跡を点線でそれぞれ示している。
【０１２７】
この図を見ると、従来の手法で算出した位置は、時間経過とともに自動車の真の軌跡から外れていくことがわかる。これは、図１４で説明したように、従来の手法では時間経過に伴ってＮフレーム速度ベクトルに誤差が累積していくため、このＮフレーム速度ベクトルを用いて算出される位置に誤差が重畳されるためである。
【０１２８】
それに対して、本実施例の手法で算出した位置は、自動車の真の軌跡にほぼ追従していることがわかる。これは、図１４で説明したように、本実施例の手法ではＮフレーム速度ベクトルが正しく求められるため、このＮフレーム速度ベクトルを用いて算出される位置もまた正確なものとなるためである。
【０１２９】
４．作用効果
移動体に設置された加速度センサー２が検出したローカル座標系であるＢフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー３の検出結果を用いて、絶対座標系であるＮフレームにおける加速度（Ｎフレーム加速度）に変換される。すなわち、ジャイロセンサー３の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Ｎフレーム加速度が算出される。そして、Ｎフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理によって推定され、推定されたバイアス値を用いてＮフレーム加速度が補正される。
【０１３０】
カルマンフィルター処理では、Ｎフレーム加速度のバイアス値を状態推定値とし、Ｎフレーム加速度を観測値として、予測処理及び補正処理が行われる。カルマンフィルター処理では、移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、想定誤差パラメーターの一種である観測誤差の値が切り替えられる。具体的には、移動体が無加速状態の場合には、状態推定値の観測値への追従性を比較的高くするように観測誤差“Ｒ”に小さな値が設定される。それに対して、移動体が加減速状態の場合には、状態推定値の観測値への追従性を比較的劣化させるように観測誤差“Ｒ”に大きな値が設定される。
【０１３１】
無加速状態では、移動分の加速度がゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、観測誤差“Ｒ”を小さく設定して、状態推定値が観測値に追従し易くすることで、バイアス値の推定を適切に行うことができる。それに対して、加減速状態では、「移動分の加速度＋バイアス値」が観測値となる。そのため、状態推定値を観測値に追従させるのではなく、観測誤差“Ｒ”を大きく設定して、無加速状態において推定されたバイアス値を維持させるようにバイアス値を推定する。
【０１３２】
加速度センサー２の検出結果及びジャイロセンサー３の検出結果には、それぞれバイアス値が内在するが、それぞれのバイアス値の大きさを特定することは困難である。そこで、ジャイロセンサー３の検出結果を用いて座標変換された後の加速度に着目し、この変換された加速度に内在するバイアス値を、変換された加速度と移動体の移動状態とを用いて推定することとした。これにより、加速度センサー２のバイアス値と、ジャイロセンサー３のバイアス値とを分離せずとも、移動体の加速度を適切に補正することが可能となる。そして、補正した加速度を用いることで、移動体の速度ベクトルの算出を適切に行うことができ、ひいては位置算出の正確性を向上させることが可能となる。
【０１３３】
５．変形例
５−１．バイアス値の推定処理
上述した実施形態では、カルマンフィルター処理を行ってＮフレーム加速度ベクトルのバイアス値を推定する手法を例に挙げて説明したが、他の手法を用いてバイアス値を推定することも可能である。
【０１３４】
例えば、平均化処理を利用してバイアス値を推定することが考えられる。具体的には、移動体が無加速状態である場合に、Ｎフレーム加速度を入力値として平均化処理を行い、平均化された値をＮフレーム加速度のバイアス値と推定してもよい。移動体が無加速状態であれば、原理的には進行方向の加速度はゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、無加速状態におけるＮフレーム加速度を平均化処理することで、Ｎフレーム加速度のバイアス値を適切に推定することができる。
【０１３５】
図１６は、この場合にカーナビゲーション装置６の処理部３００が、図１２のナビゲーション処理においてステップＡ７のＮフレーム加速度ベクトル補正処理の代わりに実行する第２のＮフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
【０１３６】
先ず、移動状態判定部３１０は、移動体の移動状態を判定する（ステップＤ１）。そして、移動状態が無加速状態であれば（ステップＤ１；無加速状態）、バイアス推定部３４０が、無加速状態において算出されたＮフレーム加速度ベクトルを平均化するＮフレーム加速度平均化処理を行う（ステップＤ３）。
【０１３７】
具体的には、無加速状態と判定された後にＮフレーム加速度ベクトル算出処理（図１２のステップＡ５）で算出されたＮフレーム加速度ベクトルを蓄積的に記憶しておき、この蓄積記憶されたＮフレーム加速度ベクトルの平均値を算出する処理を行う。その後、バイアス推定部３４０は、平均化された値をバイアス値と推定し、推定バイアス値として記憶部７００に記憶させる（ステップＤ５）。
【０１３８】
ステップＤ５の後、又は、ステップＤ１において移動状態が加減速状態であると判定した場合（ステップＤ１；加速／減速状態）、バイアス補正部３５０は、記憶部７００に記憶されている推定バイアス値を用いて元のＮフレーム加速度ベクトルを補正する（ステップＤ７）。そして、処理部３００は、第２のＮフレーム加速度ベクトル補正処理を終了する。
【０１３９】
なお、平均化処理に限らず、無加速状態でのＮフレーム加速度を用いればバイアス値の推定を行うことができる。例えば、無加速状態におけるＮフレーム加速度の過去の履歴等の情報を利用して、公知の平滑化手法に基づく平滑化処理を行ってＮフレーム加速度の現在のバイアス値を推定することとしてもよい。
【０１４０】
５−２．Ｎフレーム速度ベクトルの補正
上述した位置算出システム５において、位置算出の正確性をより向上させるために、速度ベクトル算出部３６０により算出されたＮフレーム速度ベクトルを補正する処理を追加してもよい。
【０１４１】
図１７は、この場合における位置算出システム５の処理部３００を機能ブロックとして表した概念図である。なお、図６と同一の機能ブロックについては同一の符号を付して説明を省略する。処理部３００は、移動状態判定部３１０と、姿勢算出部３２０と、座標変換部３３０と、バイアス推定部３４０と、バイアス補正部３５０と、速度ベクトル算出部３６０と、速度ベクトル補正部３７０と、位置算出部３８０とを備えて構成される。速度ベクトル算出部３６０の後段に速度ベクトル補正部３７０を設けた点が特徴である。
【０１４２】
姿勢算出部３２０は、ジャイロセンサー３の検出結果を用いて算定した移動体の姿勢を座標変換部３３０に出力するとともに、当該算定姿勢で示される移動体の移動方向を速度ベクトル補正部３７０に出力する。速度ベクトル補正部３７０は、速度ベクトル算出部３６０により算出されたＮフレーム速度ベクトルを、姿勢算出部３２０から入力した移動体の移動方向を用いて補正する。
【０１４３】
図１８は、この場合に処理部３００が実行する第２のナビゲーション処理の流れを示すフローチャートである。なお、図１２のナビゲーション処理と同一のステップについては同一の符号を付して説明を省略する。
【０１４４】
ステップＡ９においてＮフレーム速度ベクトル算出処理を行った後、処理部３００は、Ｎフレーム速度ベクトル補正処理を実行する（ステップＣ１０）。具体的には、速度ベクトル補正部３７０が、速度ベクトル算出部３６０により算出されたＮフレーム速度ベクトルを、以下説明する原理に従って補正する。
【０１４５】
図１９は、Ｎフレーム速度ベクトル補正の原理の説明図である。説明を分かり易くするため、ここではＮフレームの東西方向（Ｅ軸）及び南北方向（Ｎ軸）でなる２次元平面において速度ベクトルを補正する場合について図示・説明する。図１９（Ａ）は移動体の直進時における速度ベクトルの補正の説明図であり、図１９（Ｂ）は移動体の回転（旋回）時における速度ベクトルの補正の説明図である。各図において、横軸は東西方向の軸（Ｅ軸）を示しており、縦軸は南北方向の軸（Ｎ軸）を示している。
【０１４６】
Ｎフレーム加速度ベクトルの東西方向成分及び南北方向成分を積分して最後に更新したＮフレーム速度ベクトルに継ぎ足すと、Ｎフレーム速度ベクトルの東西方向成分及び南北方向成分が得られる。図１９において、Ｅ軸上に記載した矢印がＮフレーム速度ベクトルの東西方向成分を示しており、Ｎ軸上に記載した矢印がＮフレーム速度ベクトルの南北方向成分を示している。
【０１４７】
この東西方向成分及び南北方向成分を合成することで、大きさが｜Ｖ｜で、向きがθ’であるＮフレーム速度ベクトルが得られる。すなわち、動径を｜Ｖ｜、偏角をθ’とする円座標でＮフレーム速度ベクトルが定義される。但し、図１９では、Ｎフレーム速度ベクトルの向きを、速度ベクトルとＥ軸との成す角度で表現している。上記のＮフレーム速度ベクトルが、速度ベクトル算出部３６０により算出されるＮフレーム速度ベクトルである。
【０１４８】
Ｎフレーム速度ベクトルは、Ｎフレーム補正加速度ベクトルを積分して継ぎ足すことで算出される。そのため、Ｎフレーム補正加速度ベクトルに誤差が含まれていると、求められるＮフレーム速度ベクトルに誤差が累積するという問題がある。Ｎフレーム速度ベクトルの東西方向成分及び南北方向成分が正確に求まらなければ、図１９に示すＮフレーム速度ベクトルの大きさ及び向きが正確に求まらず、誤差を含み得ることになる。
【０１４９】
そこで、Ｎフレーム速度ベクトルの大きさ及び向きを次のように補正する。先ず、Ｎフレーム速度ベクトルの向きθ’を、姿勢算出部３２０により算出された移動体の移動方向θに補正する。Ｎフレーム速度ベクトルの向きθ’は、Ｎフレーム補正加速度ベクトルを積分及び継ぎ足して間接的に求められる向きであるのに対し、移動方向θは、慣性センサーの検出結果から直接的に求められる移動体の向きである。そのため、Ｎフレーム補正速度ベクトルの向きは、移動体の移動方向に沿った正しい向きとなる。
【０１５０】
次に、Ｎフレーム速度ベクトルの大きさ｜Ｖ｜を｜Ｖ｜×αに補正する。ここで、「α」はＮフレーム速度ベクトルの補正係数である。補正係数「α」を、移動体の直進時と回転時とで可変に設定して速度ベクトルを補正する。具体的には、移動体の直進時には、図１９（Ａ）に示すように補正係数「α＝１」として、Ｎフレーム速度ベクトルの大きさをそのままにする。この場合、Ｎフレーム速度ベクトルの大きさとＮフレーム補正速度ベクトルの大きさとは同じとなる。
【０１５１】
一方、移動体の回転時には、図１９（Ｂ）に示すように「α＜１」として、Ｎフレーム速度ベクトルの大きさが小さくなるように補正する。この場合、Ｎフレーム補正速度ベクトルの大きさは、Ｎフレーム速度ベクトルの大きさよりも小さくなる。本願発明者が種々の条件で実験を行った結果、移動体の回転時には、補正係数「α」を「１」よりも僅かに小さな値とすることで、速度ベクトルの補正の効果が表れ、位置算出の正確性が向上することがわかった。但し、移動体の回転には、移動体の右折、左折、転回等、１周より短い回転が含まれる。
【０１５２】
なお、図１９では説明を分かり易くするために２次元平面で考えたが、３次元空間に拡張することは容易である。この場合は、東西南北の２次元平面における移動体の向きに加えて、上下方向の向きを考慮して同様の補正を行えばよい。すなわち、動径を｜Ｖ｜、偏角を（θ’，φ’）とする球座標でＮフレーム速度ベクトルを定義する。そして、Ｎフレーム速度ベクトルの向きを姿勢算出部３２０により算出された姿勢角（θ，φ）に補正し、速度ベクトルの大きさを｜Ｖ｜×αに補正すればよい。
【０１５３】
図１８の第２のナビゲーション処理に戻って、Ｎフレーム速度ベクトル補正処理を行った後、処理部３００は、位置算出処理を行う（ステップＡ１１）。具体的には、位置算出部３８０が、速度ベクトル補正部３７０により補正されたＮフレーム速度ベクトル（Ｎフレーム補正速度ベクトル）を用いて、最新の位置を算出・更新する。
【０１５４】
なお、移動体の移動方向の検出は、ジャイロセンサー３の検出結果を用いることに限られない。信頼性の高い移動方向が得られるのであれば、他の検出手段によって移動方向を検出することとしてもよい。例えば、地磁気センサー等の方位センサーによって移動方向を検出することとしてもよい。
【０１５５】
５−３．出力位置の決定
上述した実施例では、所定のリセットタイミングにおいて、ＧＰＳにより算出された位置で算定位置をリセットするものとして説明したが、ＧＰＳを利用して算出した位置（ＧＰＳ算出位置）と、慣性航法演算により算出した位置（慣性航法演算位置）とを併用して、最終的な出力位置を算出・決定することとしてもよい。例えば、ＧＰＳ算出位置と慣性航法演算位置とを平均化して最終的な出力位置としてもよい。
【０１５６】
５−４．カルマンフィルター処理のパラメーター値
上記の実施形態で使用したカルマンフィルター処理のパラメーター値はあくまでも一例であり、適宜設定変更可能である。特に、観測誤差“Ｒ”として設定する値は、原理で説明したように移動体の移動状態に応じて大小切り替えることとすればよく、設定する具体的な想定誤差値は適宜変更してよい。
【０１５７】
５−５．電子機器
上述した実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【０１５８】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の加速度算出及び位置算出を実現することが可能である。
【０１５９】
５−６．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムであってもよいことは勿論である。
【符号の説明】
【０１６０】
１  加速度算出システム、  ２  加速度センサー、  ３  ジャイロセンサー、  ５  位置算出システム、  ６  カーナビゲーション装置、  １０  移動状態判定部、  ２０  姿勢算出部、  ３０  座標変換部、  ４０  バイアス推定部、  ５０  バイアス補正部、  １００  ＧＰＳアンテナ、  ２００  ＧＰＳ位置算出部、  ３００  処理部、  ４００  操作部、  ５００  表示部、  ６００  ＩＭＵ、  ７００  記憶部
【図１】
2012002678.tif 000015
【図２】
2012002678.tif 000016
【図３】
2012002678.tif 000017
【図４】
2012002678.tif 000018
【図５】
2012002678.tif 000019
【図６】
2012002678.tif 000020
【図７】
2012002678.tif 000021
【図８】
2012002678.tif 000022
【図９】
2012002678.tif 000023
【図１０】
2012002678.tif 000024
【図１１】
2012002678.tif 000025
【図１２】
2012002678.tif 000026
【図１３】
2012002678.tif 000027
【図１４】
2012002678.tif 000028
【図１５】
2012002678.tif 000029
【図１６】
2012002678.tif 000030
【図１７】
2012002678.tif 000031
【図１８】
2012002678.tif 000032
【図１９】
2012002678.tif 000033