加速度算出方法、位置算出方法及び加速度算出装置
【課題】移動体の加速度を適切に求めるための新たな手法の提案。
【解決手段】移動体に設置された加速度センサー2が検出したローカル座標系であるBフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー3の検出結果を用いて、絶対座標系であるNフレームにおける加速度(Nフレーム加速度)に変換される。すなわち、ジャイロセンサー3の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Nフレーム加速度が算出される。そして、Nフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理を利用して推定され、当該バイアス値を用いてNフレーム加速度が補正される。
【解決手段】移動体に設置された加速度センサー2が検出したローカル座標系であるBフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー3の検出結果を用いて、絶対座標系であるNフレームにおける加速度(Nフレーム加速度)に変換される。すなわち、ジャイロセンサー3の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Nフレーム加速度が算出される。そして、Nフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理を利用して推定され、当該バイアス値を用いてNフレーム加速度が補正される。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、加速度算出方法、位置算出方法及び加速度算出装置に関する。
【背景技術】
【0002】
測位用信号を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。GPSでは、複数のGPS衛星の位置や各GPS衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置座標と時計誤差とを求める位置算出計算を行う。
【0003】
測位用信号を利用した位置算出では、種々の誤差要因によって位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、移動体の速度ベクトルを用いた慣性航法演算を行うことにより、GPSを利用して算出した位置を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開平8−68651号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
従来から用いられている慣性航法演算では、例えば加速度センサーを用いるのであれば、加速度センサーにより検出された加速度を積分することで移動体の速度ベクトルを算出し、今回の速度ベクトルの方向に今回の速度ベクトル分の移動量(移動距離)を前回位置に加算することで、移動体の位置を新たに算出・更新する手法がとられる。ここで「ベクトル」とは、方向及び大きさを表す便宜上の用語である。方向及び大きさを表す表現としては、例えば直交座標系や球座標系における各軸の値としてもよいことは勿論であるが、何れの表記法をとったとしても「ベクトル」と等価であることは自明である。従って、本明細書においては、方向及び大きさの両方を表す用語として「ベクトル」を用いる。
【0006】
加速度センサーは移動体に固定的に設置される。従って、加速度センサーが検出するのは、移動体自身を基準としたローカルな座標系(以下、「ローカル座標系」と称する。)における加速度である。しかし、慣性航法演算において求めたいのは、例えば地球表面といった移動体を含む座標系(以下、「絶対座標系」と称する。)における移動体の位置(位置座標)である。そのため、加速度センサーにより検出されたローカル座標系における加速度を、絶対座標系における加速度に変換して利用する必要がある。
【0007】
ローカル座標系から絶対座標系への座標変換を行うためには、移動体の姿勢(より正確には絶対座標系での姿勢)の情報が必要となる。移動体の姿勢を正しく求めることができれば、座標変換を適切に行うことができる。例えば、移動体に設置されたジャイロセンサーにより検出された角速度を用いて、移動体の姿勢を算出することが考えられる。しかし、移動体の姿勢を正しく求めることは容易ではない。なぜなら、ジャイロセンサーの検出結果には、典型的にはゼロ点バイアスといった誤差成分が含まれるため、算出される移動体の姿勢にも誤差が含まれてしまうためである。
【0008】
ここでもう1つ問題となるのは、加速度センサーの検出結果にもゼロ点バイアスといった誤差成分が含まれることである。そのため、ローカル座標系から絶対座標系に変換された加速度には、加速度センサーの誤差成分と、ジャイロセンサーの誤差成分との2重の誤差成分が含まれることになる。この2重の誤差成分を分離することは極めて困難である。慣性航法演算では、絶対座標系における移動体の加速度を積分しながら移動体の速度ベクトルを逐次的に求めていくため、速度ベクトルに含まれる誤差が時間経過に伴って累積的に増加するという大きな問題があった。
【0009】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体の加速度を適切に求めるための新たな手法を提案することにある。
【課題を解決するための手段】
【0010】
以上の課題を解決するための第1の形態は、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、を含む加速度算出方法である。
【0011】
また、他の形態として、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、を備えた加速度算出装置を構成してもよい。
【0012】
この第1の形態等によれば、移動体に設置された加速度センサーが検出した移動体のローカル座標系におけるローカル加速度が、移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換される。そして、変換された加速度に内在するバイアス値が、その変換された加速度と移動体の移動状態とを用いて推定され、当該バイアス値を用いて、絶対座標系に変換された加速度が補正される。
【0013】
加速度センサーの検出結果及びジャイロセンサーの検出結果には、それぞれバイアス値が含まれ得るが、それらの大きさを個別に知ることは困難である。そこで、第1の形態等では、ジャイロセンサーの検出結果を用いてローカル座標系から絶対座標系に変換された加速度に着目した。そして、変換された加速度に内在するバイアス値を、その変換された加速度それ自体と移動体の移動状態とを用いて推定することとした。これにより、加速度センサーのバイアス値とジャイロセンサーのバイアス値とを分離せずとも、絶対座標系における加速度を適切に補正することが可能となり、移動体の加速度を適切に求めることができる。
【0014】
また、第2の形態として、第1の形態の加速度算出方法であって、前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、加速度算出方法を構成してもよい。
【0015】
この第2の形態によれば、バイアス値を状態推定値とし、絶対座標系に変換された加速度を観測値とするカルマンフィルター処理が行われて、当該加速度に内在するバイアス値が推定される。カルマンフィルターを利用した処理により、バイアス値の推定を適切に行うことができる。
【0016】
また、第3の形態として、第2の形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、加速度算出方法を構成してもよい。
【0017】
より具体的には、例えば第4の形態として、第3の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、加速度算出方法を構成してもよい。
【0018】
この第4の形態によれば、無加速状態での想定誤差パラメーター値を、加減速状態の場合に比べて小さい値とする。想定誤差パラメーター値は、例えば、観測値に含まれ得る誤差の大きさを想定したパラメーター値である。想定誤差パラメーター値を小さくするということは、観測値に含まれ得る誤差を小さく見積もることを意味する。そのため、移動体の移動状態が無加速状態の場合には、観測値を信用・重視してバイアス値を推定することができる。
【0019】
また、例えば第5の形態として、第3の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、加速度算出方法を構成することも可能である。
【0020】
この第5の形態によれば、移動体の移動状態が無加速状態の場合には、状態推定値の観測値への追従性を加減速状態の場合よりも高める想定誤差パラメーター値とする。絶対座標系に変換された加速度を観測値とするため、移動体の移動状態が無加速状態の場合には、絶対座標系に変換された加速度に追従させるようにバイアス値を推定することができる。
【0021】
また、第6の形態として、第2〜第5の何れかの形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、加速度算出方法を構成してもよい。
【0022】
絶対座標系における加速度が過度に大きい場合に、その加速度をそのまま観測値としてカルマンフィルター処理を行うと、観測値の大きさに引きずられてバイアス値が適切に推定されなくなるおそれがある。かかる問題は移動体の移動状態が加減速状態の場合に起こり易い。そこで、第6の形態では、移動体の移動状態が加減速状態の場合に絶対座標系に変換された加速度が所定の限界値を超えた場合、当該限界値を観測値としてカルマンフィルター処理を行う。このように観測値の上限を定めることで、上記の問題を解決することができる。
【0023】
また、第7の形態として、第1の形態の加速度算出方法であって、前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、加速度算出方法を構成してもよい。
【0024】
移動体が無加速状態であれば、原理的には加速度はゼロであるため、加速度の検出値はバイアス値に相当する値であると言える。そこで、第7の形態では、移動体が無加速状態である場合の絶対座標系に変換された加速度の平均化処理を行い、その平均化された値を加速度に内在するバイアス値と推定する。
【0025】
また、第8の形態として、第1〜第7の何れかの形態の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、前記移動体の向きを検出することと、前記移動体の向きを用いて前記速度ベクトルを補正することと、前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【0026】
この第8の形態によれば、上述した加速度算出方法によって算出された加速度を用いて移動体の速度ベクトルが算出される。そして、検出した移動体の向きを用いて速度ベクトルが補正され、当該補正された速度ベクトルを用いて移動体の位置が算出される。速度ベクトルの算出とは別に移動体の向きを検出し、検出した移動体の向きを用いて速度ベクトルを補正することで、移動体の速度ベクトルを適切に求めることができる。そして、この速度ベクトルを用いることで、移動体の位置が正確に求まる。
【図面の簡単な説明】
【0027】
【図1】加速度算出システムのシステム構成の説明図。
【図2】Nフレーム加速度ベクトルの南北成分の時間変化を示す図。
【図3】観測誤差の切替の説明図。
【図4】位置算出システムのシステム構成の説明図。
【図5】カーナビゲーション装置の機能構成の一例を示すブロック図。
【図6】処理部を機能ブロックとして表した概念図。
【図7】観測誤差設定用テーブルのテーブル構成の一例を示す図。
【図8】センサーデータのデータ構成の一例を示す図。
【図9】推定バイアス値データのデータ構成の一例を示す図。
【図10】加速度ベクトルデータのデータ構成の一例を示す図。
【図11】出力位置データのデータ構成の一例を示す図。
【図12】ナビゲーション処理の流れを示すフローチャート。
【図13】Nフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図14】(A)Nフレーム速度ベクトルの南北成分の時間変化。(B)Nフレーム速度ベクトルの東西成分の時間変化。(C)Nフレーム速度ベクトルの高度成分の時間変化。
【図15】位置算出を行った実験結果を示す図。
【図16】第2のNフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図17】変形例における処理部を機能ブロックとして表した概念図。
【図18】第2のナビゲーション処理の流れを示すフローチャート。
【図19】(A)移動体の直進時におけるNフレーム速度ベクトル補正の説明図。(B)移動体の回転時におけるNフレーム速度ベクトル補正の説明図。
【発明を実施するための形態】
【0028】
以下、図面を参照して、本発明の好適な実施形態について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【0029】
1.システム構成
図1は、本実施形態における加速度算出システム1のシステム構成の説明図である。加速度算出システム1は、各種の移動体に適用され、当該移動体の加速度を算出するシステムである。加速度算出システム1には、慣性センサーの一種である加速度センサー2及びジャイロセンサー3が含まれる。
【0030】
加速度センサー2及びジャイロセンサー3は、移動体に設置され、移動体を基準としたローカル座標系におけるローカル加速度及びローカル角速度を検出して出力するように構成されている。本実施形態では、このローカル座標系を機体座標系(Body Frame)(以下、「Bフレーム」と称する。)として説明する。そして、本実施形態では、加速度センサー2により検出されたBフレームにおける加速度を、絶対座標系の一種であるNED(North East Down)座標系(以下、「Nフレーム」と称する。)における加速度に変換する。
【0031】
移動体の加速度及び速度は、方向及び大きさを持っている。そのため、本明細書では、移動体の加速度及び速度を「加速度ベクトル」及び「速度ベクトル」として説明する。また、ローカル座標系である「Bフレーム」において定義されるベクトルと、絶対座標系である「Nフレーム」において定義されるベクトルとを区別するため、各変量を表す文言の先頭にフレームの種類を付して説明する。例えば、Bフレームにおける加速度ベクトルは「Bフレーム加速度ベクトル」と称し、Nフレームにおける加速度ベクトルは「Nフレーム加速度ベクトル」と称する。
【0032】
Bフレームは、移動体の前方を正とする前後方向をX軸(ロール軸)、右方を正とする左右方向をY軸(ピッチ軸)、鉛直下方を正とする上下方向をZ軸(ヨー軸)とする三次元直交座標系である。それに対し、Nフレームは、北方を正とする南北方向をN軸、東方を正とする東西方向をE軸、鉛直下方を正とする高度方向をD軸とする三次元直交座標系である。
【0033】
座標変換の際には、加速度センサー2の検出結果(Bフレームの加速度ベクトル)のうち、加速度の向き(加速度ベクトルの向き)を座標変換し、加速度の大きさ(加速度ベクトルの大きさ)は維持する。言い換えると、加速度センサー2の相対的な検出方向を、絶対座標系における方向(絶対方向)に座標変換する。
【0034】
加速度算出システム1は、処理ブロックとして、移動状態判定部10と、姿勢算出部20と、座標変換部30と、バイアス推定部40と、バイアス補正部50とを備える。移動状態判定部10は、移動体の移動状態を判定する。本実施形態では、移動状態判定部10は、主に移動体が無加速である状態(以下、「無加速状態」と称する。)と、移動体が加速/減速している状態(以下、「加減速状態」と称する。)との何れの移動状態であるかを判定する。無加速状態には、移動体が停止している状態(停止状態)と、移動体が等速で移動している状態(等速状態)とが含まれる。
【0035】
姿勢算出部20は、ジャイロセンサー3により検出されたBフレーム角速度を積分及び継ぎ足していくことで、移動体の姿勢角を算定する。ここで、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【0036】
具体的には、姿勢算出部20は、所定時間の間に検出されたBフレーム角速度を「積分」することで、その所定時間の間の移動体の姿勢角の変化方向及び変化量を算出する。所定時間はBフレーム角速度の検出時間間隔よりも長く設定され、通常は一定時間とされる。例えば、所定時間を100ミリ秒とし、Bフレーム角速度の検出時間間隔(より正確にはジャイロセンサー3から出力された信号のサンプリング時間間隔)が1〜10ミリ秒の間で適宜調整されるとした場合には、検出時間間隔がいくつであろうとも、必ず、100ミリ秒毎に移動体の姿勢角の変化方向及び変化量を算出することとなる。
【0037】
そして、100ミリ秒毎に算出される姿勢角の変化方向及び変化量を、最後に更新した姿勢角に「継ぎ足す」ことで、最新の姿勢角(以下、この最新の姿勢角によって定まる移動体の最新の姿勢のことを「算定姿勢」と称する。)を算出・更新する。そして、その算定姿勢を座標変換部30に出力する。移動体の姿勢がわかれば、移動体の移動方向もわかる。そのため、姿勢算出部20は、移動体の移動方向を検出する移動方向検出部とも言える。
【0038】
座標変換部30は、姿勢算出部20から入力した算定姿勢に基づいて、加速度センサー2の検出結果であるBフレーム加速度ベクトルを、Nフレーム加速度ベクトルに変換する座標変換処理を行う。具体的には、姿勢算出部20から入力した移動体の姿勢角を用いて、BフレームからNフレームへの座標変換行列を算出する。そして、当該座標変換行列を用いて加速度センサー2の検出結果を変換して、バイアス推定部40に出力する。座標変換部30は、Nフレーム加速度ベクトルを算出する算出部とも言える。
【0039】
バイアス推定部40は、座標変換部30から出力されるNフレーム加速度ベクトルを用いて、当該Nフレーム加速度ベクトルに内在するバイアス値を推定する。ここで、バイアス値とは、Nフレーム加速度ベクトルの各成分にそれぞれ付加される誤差値(典型的にはゼロ点バイアス)のことを意味する。
【0040】
本実施形態において、バイアス推定部40は、カルマンフィルターを利用した推定処理(カルマンフィルター処理)を行ってバイアス値を推定する。カルマンフィルターは、離散的な誤差を有する観測値を用いて、あるシステムの時々刻々と変換する量を推定するための手法である。そして、バイアス推定部40は、推定したバイアス値(以下、「推定バイアス値」と称す。)をバイアス補正部50に出力する。
【0041】
バイアス補正部50は、バイアス推定部40から出力される推定バイアス値を用いて、座標変換部30から出力されたNフレーム加速度ベクトルを補正する。すなわち、Nフレーム加速度ベクトルの各成分それぞれに対して、対応する成分のバイアス値を加減算する。そして、その結果をNフレーム補正加速度ベクトルとして出力する。
【0042】
2.原理
2−1.バイアス値の推定
本願発明者は、Nフレーム加速度ベクトルを実際に算出する実験を行った。移動体の移動状態を変化させて、慣性センサー(加速度センサー2及びジャイロセンサー3)を利用して慣性航法演算によりNフレーム加速度ベクトルを算出した。また、衛星測位システムの一種であるGPS(Global Positioning System)を利用してNフレームにおける移動体の速度ベクトルを算出し、その速度ベクトルを微分することでNフレーム加速度ベクトルを同様に算出した。
【0043】
図2は、この場合の実験結果の一例を示す図である。ここでは、Nフレーム加速度ベクトルの南北成分(南北方向の加速度)の時間変化を一例として示して説明する。図2において、横軸は時間を示し、縦軸は南北方向の加速度(単位はメートル毎秒毎秒[m/s2])を示している。移動体の移動状態を、加減速状態→無加速状態→加減速状態の順に変化させてNフレーム加速度を算出した。図2において、慣性航法を利用して算出したNフレーム加速度には「慣性航法」、GPSを利用して算出したNフレーム加速度には「GPS」と付記した。
【0044】
この図を見ると、グラフの中央部において、GPSを利用して算出したNフレーム加速度がほぼ“0”となっている区間が存在する。この区間が、移動体が無加速状態であった区間である。理論上、移動体が無加速状態であれば進行方向の加速度は“0”である。このことから、GPSを利用して算出したNフレーム加速度は正しいことを意味する。しかし、その一方で、慣性航法を利用して算出したNフレーム加速度を見ると、無加速状態の区間において値が“0”となっておらず、マイナスの値を中心として上下に振動していることがわかる。
【0045】
この無加速状態の区間におけるNフレーム加速度の“0”からのズレは、Nフレームの加速度に含まれるバイアス値に相当するものと考えることができる。そこで、本願発明者は、移動体の無加速状態において観測されるNフレーム加速度に着目し、Nフレーム加速度に内在するバイアス値を、Nフレーム加速度と移動体の移動状態とを用いて推定する手法を考案した。その手法の一例として、本実施形態では、カルマンフィルターを利用した推定処理を行ってNフレーム加速度に含まれるバイアス値を推定する手法について説明する。
【0046】
具体的な定式化を行う。本実施形態では、カルマンフィルターにおいて推定対象とするシステムの状態推定値“X”をバイアス値とし、次式(1)のように設定する。
【数1】
但し、「(bN,bE,bD)」は、それぞれNフレーム加速度ベクトルのバイアス値の南北方向(N)、東西方向(E)及び高度方向(D)の各成分である。
【0047】
また、状態推定値“X”の精度を表すために、状態推定値“X”を構成するバイアス値の各成分の誤差共分散“P”を表す行列(誤差共分散行列)を設定する。
【0048】
カルマンフィルターでは、外部から与えられる観測値“Z”を利用して1ステップ前に予測した状態推定値“X”及び誤差共分散“P”を補正する補正処理(観測更新)と、補正した状態推定値“X”及び誤差共分散“P”から次のステップの状態推定値“X”及び誤差共分散“P”を予測する予測処理(時刻更新)との2つの処理を繰り返し行う。そして、この繰り返し処理により、尤もらしい状態推定値“X”を推定する。
【0049】
本実施形態では、Nフレーム加速度ベクトルの各成分を観測値“Z”として入力する。すなわち、観測値“Z”を次式(2)のように設定する。
【数2】
但し、「(AN,AE,AD)」は、それぞれNフレーム加速度ベクトルの南北方向(N)、東西方向(E)及び高度方向(D)の各成分である。
【0050】
補正処理(観測更新)は、次式(3)〜(5)で定式化される。
【数3】
【数4】
【数5】
【0051】
予測処理(時刻更新)は、次式(6)及び(7)で定式化される。
【数6】
【数7】
【0052】
予測処理において、“Φ”は状態遷移行列と呼ばれ、補正処理で補正された状態推定値“X”から次の時刻の状態推定値“X”への遷移を表す行列として定義される。また、“Q”はシステムノイズ(プロセスノイズ)と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。本実施形態では、これらの行列を次式(8)及び(9)のように設定する。
【数8】
【数9】
【0053】
補正処理において、“H”は観測行列と呼ばれる行列である。観測行列“H”は、状態推定値“X”の次元を観測値“Z”の次元に変換するための変換行列として用いられる。観測行列“H”を1ステップ前に予測された状態推定値“X”に作用させることで、観測値の予測値“HX”が得られる。本実施形態では、この観測行列“H”を次式(10)のように設定する。
【数10】
但し、観測行列“H”の行と列の並びは、状態推定値“X”の3次元の成分の並び(バイアス値の南北成分、東西成分、高度成分)にそれぞれ対応する。
【0054】
式(10)を見ると、観測行列“H”は、対角成分が“1”であり、それ以外の成分が“0”である対角行列となっている。本実施形態では、状態推定値“X”をNフレーム加速度ベクトルのバイアス値とし、観測値“Z”をNフレーム加速度ベクトルとしている。そのため、式(10)の観測行列“H”を状態推定値“X”に作用させることは、Nフレーム加速度ベクトルのバイアス値を、Nフレーム加速度ベクトルにそのまま変換することを意味する。
【0055】
式(3)において、“K”はカルマンゲインと呼ばれる補正係数である。カルマンゲイン“K”は、予測された状態推定値“X”を重視するか、実際の観測値“Z”と予測された観測値“HX”との差“Z−HX”を重視するかを決定付けるパラメーター値として用いられる。
【0056】
式(4)は、実際の観測値“Z”と予測された観測値“HX”との差“Z−HX”を用いて、状態推定値“X”を補正することを表している。また、式(5)は、1ステップ前に予測された誤差共分散“P”とカルマンゲイン“K”と観測行列“H”とを用いて、誤差共分散“P”を補正することを表している。
【0057】
ところで、カルマンゲイン“K”は、観測誤差“R”の大きさによって決定付けられる。観測誤差“R”は、観測値“Z”に含まれる想定誤差の大きさを表すパラメーター(想定誤差パラメーター)である。観測誤差“R”は、期待値(平均値)を“0”とする正規分布型のホワイトノイズと仮定して設定される。
【0058】
本実施形態では、観測値“Z”をNフレーム加速度ベクトルの南北方向、東西方向及び高度方向の3成分としている。そのため、観測誤差“R”を、観測値“Z”の各成分の共分散を表す3×3の行列(観測誤差行列)として定義する。観測誤差行列の対角成分には、正規分布(ガウス分布)における標準偏差“σ”の2乗値(=分散“σ2”)を想定誤差値として設定する。
【0059】
移動体の移動状態に応じた適切なバイアス推定を実現するために、本実施形態では、移動体の移動状態に応じて、想定誤差パラメーターである観測誤差“R”の値を切り替える処理を行う。すなわち、移動体の移動状態に応じて、観測誤差行列の対角成分に設定する想定誤差値を可変に設定する。以下、この観測誤差の切替の詳細について説明する。
【0060】
2−2.観測誤差“R”の切替
図3は、観測誤差“R”の切替の説明図である。図3では、Nフレーム加速度ベクトルの南北方向、東西方向及び高度方向の成分のうちの南北成分(N成分)を例示している。横軸は時間である。また、模式的に、加速度の時間変化を点線で示し、推定バイアス値の理想的な時間変化を実線で示している。
【0061】
移動体が無加速状態である場合は、移動体の移動分の加速度は“0”である。そのため、観測値“Z”として与えられるNフレーム加速度ベクトルが、Nフレーム加速度ベクトルのバイアス値に相当すると考えることができる。このことから、移動体が無加速状態である場合は、状態推定値“X”の観測値“Z”への追従性を比較的高めるように観測誤差“R”を設定することが適切である。
【0062】
そこで、移動体が無加速状態である場合は、観測誤差“R”を小さく設定する。具体的には、観測誤差“R”を、例えば次式(11)のように設定する。
【数11】
【0063】
観測誤差“R”を小さくすることで、無加速状態では、図3に示すように、観測値“Z”に追従するように状態推定値“X”が推定されるようになる。その理由は、式(3)から導くことができる。式(3)において、観測誤差“R”を“0”に近付けると(R→0)、カルマンゲイン“K”は“H−1”に近づく。すると、式(4)は「X(t)=Z(t)」となる。これは、状態推定値“X”を観測値“Z”とすること、すなわち観測値“Z”を信用・重視して状態推定値“X”を補正することを意味する。
【0064】
それに対して、移動体が加減速状態である場合は、移動体の移動分の加速度にバイアス値が重畳された値が観測される。すなわち、観測値“Z”(Nフレーム加速度ベクトル)には、バイアス値の他に移動分の加速度が含まれるため、観測値“Z”に追従させるように状態推定値“X”を補正することは適切ではない。よって、状態推定値“X”の観測値“Z”への追従性を比較的劣化させるように観測誤差“R”を設定することが適切である。
【0065】
そこで、移動体が加減速状態である場合は、観測誤差“R”を大きく設定する。具体的には、観測誤差“R”を、例えば次式(12)のように設定する。
【数12】
【0066】
観測誤差“R”を大きくすることで、加減速状態では、図3に示すように、無加速状態において推定されたバイアス値が維持されるようにバイアス値が推定される。なぜならば、式(3)からわかるように、観測誤差“R”を“+∞”に近付けると(R→+∞)、カルマンゲイン“K”は“0”に近づく。すると、式(4)は「X(t)=X(t)」となる。これは、状態推定値“X”を1ステップ前に予測された状態推定値“X”とすること、すなわち1ステップ前に予測された状態推定値“X”を信用・重視して状態推定値“X”を補正することを意味する。無加速状態において推定されたバイアス値が加減速状態において維持されるため、バイアス値の推定が適切に行われる。
【0067】
2−3.観測値“Z”の調整
上述した観測誤差“R”の切替に加えて、本実施形態では、観測値“Z”の調整(クリッピング)を行う。移動体が加減速状態である場合は、Nフレーム加速度ベクトルは大きく変動し得る。Nフレーム加速度ベクトルが過度に大きくなると、上述した観測誤差“R”の切替を行っても、バイアス値の推定が適切に行われなくなるおそれがある。
【0068】
前述したように、加減速状態においては、観測誤差“R”を大きくしてカルマンゲイン“K”を“0”に近付けることにより、式(4)を「X(t)=X(t)」と近似することが理想的である。しかし、観測値“Z”が過度に大きい場合には、カルマンゲイン“K”を小さくしても、式(4)の右辺第2項の“K(Z−HX)”がある程度大きな値を持つようになり、理想状態が崩れてしまう。すると、無加速状態において推定されたバイアス値が維持されずに、観測値“Z”に引きずられて値が変動する可能性がある。
【0069】
かかる問題に対処するため、先ず、観測値“Z”の絶対値が所定の限界値である閾値“θ”を超えているか否かを判定する。そして、閾値“θ”を超えている場合は、観測値“Z”を調整する処理を行う。具体的には、観測値“Z”が負の値であり、その絶対値が閾値“θ”を超えている場合には(Z<−θ)、観測値“Z”を“−θ”に調整する(Z=−θ)。また、観測値“Z”が正の値であり、閾値“θ”を超えている場合には(Z>θ)、観測値“Z”を“θ”に調整する(Z=θ)。
【0070】
なお、この場合に限界値として設定する閾値“θ”の値については、適用するシステムに応じて適宜設定される。
【0071】
3.実施例
次に、上記の加速度算出システム1を適用した位置算出システム5の実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【0072】
3−1.システム構成
図4は、本実施例における位置算出システム5のシステム構成の説明図である。位置算出システム5は、移動体の一種である四輪自動車(以下、単に「自動車」と称す。)に、加速度算出装置及び位置算出装置を具備した電子機器の一種であるカーナビゲーション装置6が搭載されたシステムである。
【0073】
カーナビゲーション装置6は、自動車に搭載される電子機器であり、自動車の加速度を算出する加速度算出装置及び自動車の位置を算出する位置算出装置を備えて構成される。また、カーナビゲーション装置6は、加速度センサー2及びジャイロセンサー3を具備するセンサーユニットであるIMU(Inertial Measurement Unit)600を搭載している。IMU600は、自動車に対する相対的な座標系における加速度及び角速度を検出して出力する。本実施例では、この相対的な座標系がBフレームと一致するような姿勢でカーナビゲーション装置6が自動車に設置・固定されているものとして説明する。
【0074】
カーナビゲーション装置6は、Nフレームにおいて、GPSを利用した位置算出演算と、IMU600の検出結果を利用した慣性航法演算との2種類の演算を行い、自動車の出力位置を算定する処理を行う。そして、算定した出力位置をプロットしたナビゲーション画面を生成して、表示部500に表示させる。なお、本実施例では、慣性航法演算を用いて算出した位置を、後述するGPS位置算出部200により算出された位置と区別するために「算定位置」と称する。
【0075】
3−2.機能構成
図5は、カーナビゲーション装置6の機能構成の一例を示すブロック図である。カーナビゲーション装置6は、GPSアンテナ100と、GPS位置算出部200と、処理部300と、操作部400と、表示部500と、IMU600と、記憶部700とを備えて構成される。
【0076】
GPSアンテナ100は、測位用衛星の一種であるGPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信信号をGPS位置算出部200に出力する。GPS衛星信号は、拡散符号の一種であるCA(Coarse and Acquisition)コードによって、スペクトラム拡散方式として知られるCDMA(Code Division Multiple Access)方式によって変調された1.57542[GHz]の通信信号である。CAコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号であり、GPS衛星毎に異なるものである。
【0077】
GPS位置算出部200は、GPSアンテナ100から出力された信号に基づいて自動車の位置を計測する回路或いは装置であり、いわゆるGPS受信装置に相当する機能ブロックである。図示を省略するが、GPS位置算出部200は、RF受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、RF受信回路部と、ベースバンド処理回路部とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0078】
ベースバンド処理回路部は、RF受信回路部から出力された受信信号に対して相関処理等を行ってGPS衛星信号を捕捉・追尾する。そして、GPS衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置(位置座標)を算出する。そして、算出した位置をGPSデータ727として処理部300に出力する。
【0079】
処理部300は、記憶部700に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置6の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部300は、IMU600の検出結果を利用した慣性航法演算処理を行って自動車の位置(位置座標)を算出する。そして、GPS位置算出部200がGPSを利用して演算した位置(GPS演算位置)と、処理部300が慣性航法を利用して演算した位置(慣性航法演算位置)とをもとに、自動車の現在位置を指し示した地図を表示部500に表示させる処理を行う。
【0080】
図6は、処理部300を機能ブロックとして表した概念図である。処理部300は、移動状態判定部310と、姿勢算出部320と、座標変換部330と、バイアス推定部340と、バイアス補正部350と、速度ベクトル算出部360と、位置算出部380とを機能部として有する。移動状態判定部310〜バイアス補正部350は、図1の加速度算出システム1の移動状態判定部10〜バイアス補正部50にそれぞれ相当する。
【0081】
姿勢算出部320は、ジャイロセンサー3により検出されたBフレームにおける3軸の加速度を積分及び継ぎ足し処理することで、自動車の姿勢を算定する。座標変換部330は、姿勢算出部320から入力した算定姿勢に基づいて、IMU600のBフレームにおける検出結果をNフレームにおける検出結果に座標変換する。
【0082】
バイアス推定部340は、座標変換部330から出力されるNフレーム加速度ベクトルを観測値とするカルマンフィルター処理を行って、Nフレーム加速度ベクトルのバイアス値を推定する。バイアス補正部350は、バイアス推定部340から入力した推定バイアス値を用いて、座標変換部330から出力されるNフレーム加速度ベクトルを補正する。
【0083】
速度ベクトル算出部360は、バイアス補正部350から入力したNフレーム補正加速度ベクトルを積分及び継ぎ足し処理することで、Nフレームにおける自動車の速度ベクトル(Nフレーム速度ベクトル)を算出する。そして、算出した自動車のNフレーム速度ベクトルを位置算出部380に出力する。
【0084】
位置算出部380は、速度ベクトル算出部360から入力した自動車のNフレーム速度ベクトルと、1ステップ前に算出した位置(前回算出位置)とを用いて、Nフレームにおける自動車の位置を算出・更新する。
【0085】
図5の機能ブロックの説明に戻って、操作部400は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部300に出力する。この操作部400の操作により、目的地の入力等の各種指示入力がなされる。
【0086】
表示部500は、LCD(Liquid Crystal Display)等により構成され、処理部300から入力される表示信号に基づいた各種表示を行う表示装置である。表示部500には、ナビゲーション画面等が表示される。
【0087】
IMU600は、加速度センサー2及びジャイロセンサー3を備え、予めセンサーに対応付けて定められたローカル座標系の直交3軸それぞれの軸方向の加速度及び各軸の軸回りの角速度を検出可能に構成される。なお、加速度センサー2及びジャイロセンサー3は、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【0088】
記憶部700は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置6のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0089】
3−3.データ構成
図5に示すように、記憶部700には、プログラムとして、処理部300により読み出され、ナビゲーション処理(図12参照)として実行されるナビゲーションプログラム710が記憶されている。また、ナビゲーションプログラム710には、Nフレーム加速度ベクトル算出処理として実行されるNフレーム加速度ベクトル算出プログラム711と、Nフレーム加速度ベクトル補正処理(図13参照)として実行されるNフレーム加速度ベクトル補正プログラム712とがサブルーチンとして含まれている。
【0090】
ナビゲーション処理とは、処理部300が、Nフレーム加速度ベクトルの算出処理及び補正処理を行うとともに、補正したNフレーム加速度ベクトルを用いてNフレーム速度ベクトルを算出し、当該Nフレーム速度ベクトルを用いて自動車の位置を算定・更新する処理である。そして、算定位置をプロットしたナビゲーション画面を生成して、表示部500に表示させる。ナビゲーション処理については、フローチャートを用いて詳細に後述する。
【0091】
また、記憶部700には、データとして、観測誤差設定用テーブル720と、センサーデータ721と、推定バイアス値データ723と、加速度ベクトルデータ725と、GPSデータ727と、出力位置データ729とが記憶される。
【0092】
観測誤差設定用テーブル720は、観測誤差“R”を設定するために用いられるテーブルであり、そのテーブル構成例を図7に示す。観測誤差設定用テーブル720には、移動体の移動状態と対応付けて、観測誤差行列に設定する想定誤差値(分散“σ2”の値)が定められている。具体的には、無加速状態については、第1の想定誤差値である“12”が定められており、加減速状態については、第2の想定誤差値である“1002”が定められている。
【0093】
センサーデータ721は、IMU600の検出結果が時系列に記憶されたデータであり、そのデータ構成例を図8に示す。センサーデータ721には、時刻7211と対応付けて、加速度センサー2により検出されたBフレーム加速度7213と、ジャイロセンサー3により検出されたBフレーム角速度7215とが時系列に記憶される。
【0094】
推定バイアス値データ723は、推定されたNフレーム加速度ベクトルのバイアス値についてのデータであり、そのデータ構成例を図9に示す。推定バイアス値データ723には、時刻7231と対応付けて、自動車の移動状態7233と、NEDの各成分の推定バイアス値7235とが時系列に記憶される。
【0095】
加速度ベクトルデータ725は、補正前後におけるNフレーム加速度ベクトルについてのデータであり、そのデータ構成例を図10に示す。加速度ベクトルデータ725には、時刻7251と対応付けて、Nフレーム加速度ベクトル7253と、Nフレーム補正加速度ベクトル7255とが時系列に記憶される。
【0096】
GPSデータ727は、GPS位置算出部200により算出・決定された位置(以下、「GPS算定位置」と称す。)が時系列に記憶されたデータである。GPSデータ727は、例えば所定時間間隔(例えば10分間隔)で更新される。
【0097】
出力位置データ729は、表示部500に表示出力させる位置のデータであり、そのデータ構成例を図11に示す。出力位置データ729には、位置算出が行われた時刻7291と対応付けて、出力位置7293と、測位種別7295とが時系列に記憶される。
【0098】
測位種別7295は、対応する出力位置7293が、GPS位置算出部200により算出された位置と、位置算出部380により算出された位置との何れの位置であるかを示すフラグである。GPS位置算出部200により算出された位置である場合は「GPS」が、位置算出部380により算出された位置である場合は「慣性航法」がそれぞれ記憶される。
【0099】
3−4.処理の流れ
図12は、記憶部700に記憶されているナビゲーションプログラム710が処理部300により読み出されて実行されることで、カーナビゲーション装置6において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、IMU600により検出された加速度及び角速度が、記憶部700のセンサーデータ721に随時記憶されるものとする。
【0100】
最初に、処理部300は、初期設定を行う(ステップA1)。例えば、Nフレーム加速度ベクトルのバイアス値の推定をカルマンフィルター処理を利用して行うために、カルマンフィルター処理に係る各種のパラメーター値の初期化を行う。
【0101】
その後、移動状態判定部310は、自動車の移動状態を判定する(ステップA3)。具体的には、IMU600の検出結果に基づいて、自動車が無加速状態であるか、加減速状態であるかを判定する。例えば、加速度センサー2により検出された自動車の進行方向の加速度が“0”に近い値として予め定められた所定の閾値以下又は未満の場合には、自動車は無加速状態であると判断することができる。
【0102】
次いで、処理部300は、記憶部700に記憶されているNフレーム加速度ベクトル算出プログラム711を読み出して実行することで、Nフレーム加速度ベクトル算出処理を行う(ステップA5)。
【0103】
具体的には、姿勢算出部320がジャイロセンサー3の検出結果を積分及び継ぎ足し処理して自動車の姿勢を算出する。そして、座標変換部330が、BフレームからNフレームへの座標変換行列を求め、当該座標変換行列を用いて、加速度センサー2のBフレームにおける検出結果をNフレームにおける検出結果に変換して、Nフレーム加速度ベクトルとする。なお、BフレームからNフレームへの座標変換行列については従来公知であるため、ここでは数式等を用いた詳細な説明を省略する。
【0104】
次いで、処理部300は、記憶部700に記憶されているNフレーム加速度ベクトル補正プログラム712を読み出して実行することで、Nフレーム加速度ベクトル補正処理を行う(ステップA7)。
【0105】
図13は、Nフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
先ず、バイアス推定部340は、状態推定値“X”及び誤差共分散“P”に初期値を設定する(ステップB1)。初回の処理では、状態推定値“X”及び誤差共分散“P”は任意の値(例えば“0”)とすることができる。また、2回目以降の処理では、前回の処理で求められた状態推定値“X”及び誤差共分散“P”(最新の値)とすることができる。
【0106】
次いで、バイアス推定部340は、記憶部700に記憶された観測誤差設定用テーブル720を参照して、観測誤差“R”を設定する。具体的には、移動状態が無加速状態であれば(ステップB3;無加速状態)、第1の想定誤差値(図7の観測誤差設定用テーブル720では“12”)を観測誤差行列に設定する(ステップB5)。また、移動状態が加減速状態であれば(ステップB3;加速/減速状態)、第2の想定誤差値(図7の観測誤差設定用テーブル720では“1002”)を観測誤差行列に設定する(ステップB7)。
【0107】
次いで、バイアス推定部340は、式(3)に従ってカルマンゲイン“K”を算出する(ステップB9)。そして、バイアス推定部340は、Nフレーム加速度ベクトル算出処理で算出したNフレーム加速度ベクトルの各成分それぞれに対する閾値判定を行う(ステップB11)。そして、当該成分の大きさが所定の閾値“θ”を超えている場合は(ステップB11;Yes)、当該成分を閾値“θ”として観測値“Z”を設定する(ステップB13)。また、当該成分の大きさが閾値“θ”を超えていない場合は(ステップB11;No)、当該成分をそのままとして観測値“Z”を設定する(ステップB15)。
【0108】
その後、バイアス推定部340は、式(4)及び(5)に従って状態推定値“X”及び誤差共分散“P”を補正(観測更新)する(ステップB17)。そして、バイアス推定部340は、収束条件が成立したか否かを判定する(ステップB19)。例えば、補正処理で得られた誤差共分散“P”に基づいて、バイアス値に含まれる誤差値が“0”に近い値として予め定められた所定値未満となった場合に、収束条件が成立したと判定する。
【0109】
そして、まだ収束条件が成立していないと判定した場合は(ステップB19;No)、バイアス推定部340は、式(6)及び(7)に従って状態推定値“X”及び誤差共分散“P”を予測(時刻更新)した後(ステップB21)、ステップB9に戻る。ここまでの処理は、Nフレーム加速度ベクトルのバイアス値を推定するためのカルマンフィルター処理に相当する。
【0110】
ステップB19において収束条件が成立したと判定した場合は(ステップB19;Yes)、バイアス補正部350は、状態推定値“X”に含まれる推定バイアス値を用いて、元のNフレーム加速度ベクトルを補正する(ステップB23)。すなわち、元のNフレーム加速度ベクトルの南北方向、東西方向及び高度方向の各成分に対して、バイアス値の南北方向、東西方向及び高度方向の各成分を加減算する。
【0111】
このようにしてバイアス補正を行った後、処理部300は、Nフレーム加速度ベクトル補正処理を終了する。
【0112】
図12のナビゲーション処理に戻って、Nフレーム加速度ベクトル補正処理を行った後、速度ベクトル算出部360は、Nフレーム速度ベクトル算出処理を行う(ステップA9)。具体的には、所定時間分のNフレーム補正加速度ベクトルを積分し、最新のNフレーム速度ベクトルに継ぎ足すことで、Nフレーム速度ベクトルを新たに算出する。
【0113】
次いで、位置算出部380は、位置算出処理を行う(ステップA11)。具体的には、Nフレーム速度ベクトル算出処理で算出したNフレーム速度ベクトルを用いて、Nフレームにおける自動車の位置を算定する。すなわち、前回の出力位置を、Nフレーム速度ベクトルから求められる移動方向に移動距離分だけ移動させた位置を新たな位置として算定・更新する。そして、算定位置を出力位置に決定する。
【0114】
次いで、位置算出部380は、GPS位置算出部200からGPSデータ727を取得したか否かを判定し(ステップA13)、取得したと判定した場合は(ステップA13;Yes)、出力位置のリセットタイミングであるか否かを判定する(ステップA15)。
【0115】
出力位置のリセットタイミングは、種々のタイミングを設定することができる。例えば、自動車が停止したタイミングとすることができる。自動車の停止判定は、例えば速度ベクトルの大きさに基づいて判定することが可能である。すなわち、速度ベクトルの大きさが“0”に近い値として予め定められた所定の閾値以下又は未満である場合に、自動車は停止していると判定することができる。
【0116】
また、例えば、自動車がトンネル内などのGPS測位が不可能な環境から、トンネルの外などのGPS測位が可能な環境に変化したタイミング(GPS衛星信号の捕捉ができなくなってから捕捉ができるようになったタイミング)をリセットタイミングとしてもよい。他には、例えば所定時間が経過する毎(例えば30分毎)のタイミングをリセットタイミングとしてもよい。
【0117】
出力位置のリセットタイミングであると判定した場合は(ステップA15;Yes)、位置算出部380は、リセット処理を行う(ステップA17)。具体的には、ステップA11で決定した出力位置を、GPS位置算出部200から取得したGPSデータ727のGPS算出位置でリセットする。すなわち、出力位置をGPS算出位置に変更する。
【0118】
一方、ステップA13においてGPSデータ727を取得できなかったと判定した場合(ステップA13;No)、又は、ステップA15においてリセットタイミングではないと判定した場合は(ステップA15;No)、位置算出部380は、ステップA19へと処理を移行する。
【0119】
次いで、位置算出部380は、最新の出力位置で表示部500のナビゲーション画面の表示を更新する(ステップA19)。そして、位置算出部380は、位置算出を終了するか否かを判定する(ステップA21)。例えば、操作部400を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、位置算出を終了すると判定する。
【0120】
位置算出部380は、まだ位置算出を終了しないと判定した場合は(ステップA21;No)、ステップA3に戻る。また、位置算出を終了すると判定した場合は(ステップA21;Yes)、位置算出部380は、ナビゲーション処理を終了する。
【0121】
3−5.実験結果
次に、上述した位置算出システム5において、Nフレームにおける速度ベクトルの算出及び位置の算出を行った実験結果について説明する。
【0122】
図14は、Nフレーム速度ベクトルの算出を行った結果を示す図である。図14(A)〜図14(C)は、それぞれNフレーム速度ベクトルの南北方向、東西方向及び高度方向の各成分の時間変化を示している。移動体の真の速度を実線で、従来の手法で算出した速度を破線で、本実施例の手法で算出した速度を点線でそれぞれ示している。
【0123】
この結果を見ると、従来の手法で算出した速度は、時間経過に伴って誤差が累積し、真の速度から乖離していく傾向があることがわかる。特に、高度方向の速度成分の結果が顕著であり、真の速度はほぼゼロであるのに対し、従来の手法で算出した速度は時間経過に伴って線形的に減少しており、誤差が増大していることがわかる。
【0124】
一方、本実施例の手法で算出した速度の時間変化は、真の速度の時間変化とほぼ一致していることがわかる。特に、高度方向の速度成分の結果について大きな効果が得られており、Nフレーム速度ベクトル算出の正確性が大きく向上していることがわかる。
【0125】
図15は、位置算出を行った実験結果を示す図である。上述した位置算出システム5において、自動車を予め定められた周回経路に沿って1周させ、従来の手法で算出した位置と、本実施例の手法で算定した位置とを東西南北の2次元平面にプロットした。
【0126】
図15において、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示している。東西方向0m、南北方向0mの位置がスタート地点であり、出発地点から150m程度東方向に移動した後に、北方向に向かい、大きく時計回り方向に1周周回するような経路を辿った。ゴール地点はスタート地点と同じである。また、図14と同様に、移動体の真の軌跡を実線で、従来の手法で算出した軌跡を破線で、本実施例の手法で算出した軌跡を点線でそれぞれ示している。
【0127】
この図を見ると、従来の手法で算出した位置は、時間経過とともに自動車の真の軌跡から外れていくことがわかる。これは、図14で説明したように、従来の手法では時間経過に伴ってNフレーム速度ベクトルに誤差が累積していくため、このNフレーム速度ベクトルを用いて算出される位置に誤差が重畳されるためである。
【0128】
それに対して、本実施例の手法で算出した位置は、自動車の真の軌跡にほぼ追従していることがわかる。これは、図14で説明したように、本実施例の手法ではNフレーム速度ベクトルが正しく求められるため、このNフレーム速度ベクトルを用いて算出される位置もまた正確なものとなるためである。
【0129】
4.作用効果
移動体に設置された加速度センサー2が検出したローカル座標系であるBフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー3の検出結果を用いて、絶対座標系であるNフレームにおける加速度(Nフレーム加速度)に変換される。すなわち、ジャイロセンサー3の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Nフレーム加速度が算出される。そして、Nフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理によって推定され、推定されたバイアス値を用いてNフレーム加速度が補正される。
【0130】
カルマンフィルター処理では、Nフレーム加速度のバイアス値を状態推定値とし、Nフレーム加速度を観測値として、予測処理及び補正処理が行われる。カルマンフィルター処理では、移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、想定誤差パラメーターの一種である観測誤差の値が切り替えられる。具体的には、移動体が無加速状態の場合には、状態推定値の観測値への追従性を比較的高くするように観測誤差“R”に小さな値が設定される。それに対して、移動体が加減速状態の場合には、状態推定値の観測値への追従性を比較的劣化させるように観測誤差“R”に大きな値が設定される。
【0131】
無加速状態では、移動分の加速度がゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、観測誤差“R”を小さく設定して、状態推定値が観測値に追従し易くすることで、バイアス値の推定を適切に行うことができる。それに対して、加減速状態では、「移動分の加速度+バイアス値」が観測値となる。そのため、状態推定値を観測値に追従させるのではなく、観測誤差“R”を大きく設定して、無加速状態において推定されたバイアス値を維持させるようにバイアス値を推定する。
【0132】
加速度センサー2の検出結果及びジャイロセンサー3の検出結果には、それぞれバイアス値が内在するが、それぞれのバイアス値の大きさを特定することは困難である。そこで、ジャイロセンサー3の検出結果を用いて座標変換された後の加速度に着目し、この変換された加速度に内在するバイアス値を、変換された加速度と移動体の移動状態とを用いて推定することとした。これにより、加速度センサー2のバイアス値と、ジャイロセンサー3のバイアス値とを分離せずとも、移動体の加速度を適切に補正することが可能となる。そして、補正した加速度を用いることで、移動体の速度ベクトルの算出を適切に行うことができ、ひいては位置算出の正確性を向上させることが可能となる。
【0133】
5.変形例
5−1.バイアス値の推定処理
上述した実施形態では、カルマンフィルター処理を行ってNフレーム加速度ベクトルのバイアス値を推定する手法を例に挙げて説明したが、他の手法を用いてバイアス値を推定することも可能である。
【0134】
例えば、平均化処理を利用してバイアス値を推定することが考えられる。具体的には、移動体が無加速状態である場合に、Nフレーム加速度を入力値として平均化処理を行い、平均化された値をNフレーム加速度のバイアス値と推定してもよい。移動体が無加速状態であれば、原理的には進行方向の加速度はゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、無加速状態におけるNフレーム加速度を平均化処理することで、Nフレーム加速度のバイアス値を適切に推定することができる。
【0135】
図16は、この場合にカーナビゲーション装置6の処理部300が、図12のナビゲーション処理においてステップA7のNフレーム加速度ベクトル補正処理の代わりに実行する第2のNフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
【0136】
先ず、移動状態判定部310は、移動体の移動状態を判定する(ステップD1)。そして、移動状態が無加速状態であれば(ステップD1;無加速状態)、バイアス推定部340が、無加速状態において算出されたNフレーム加速度ベクトルを平均化するNフレーム加速度平均化処理を行う(ステップD3)。
【0137】
具体的には、無加速状態と判定された後にNフレーム加速度ベクトル算出処理(図12のステップA5)で算出されたNフレーム加速度ベクトルを蓄積的に記憶しておき、この蓄積記憶されたNフレーム加速度ベクトルの平均値を算出する処理を行う。その後、バイアス推定部340は、平均化された値をバイアス値と推定し、推定バイアス値として記憶部700に記憶させる(ステップD5)。
【0138】
ステップD5の後、又は、ステップD1において移動状態が加減速状態であると判定した場合(ステップD1;加速/減速状態)、バイアス補正部350は、記憶部700に記憶されている推定バイアス値を用いて元のNフレーム加速度ベクトルを補正する(ステップD7)。そして、処理部300は、第2のNフレーム加速度ベクトル補正処理を終了する。
【0139】
なお、平均化処理に限らず、無加速状態でのNフレーム加速度を用いればバイアス値の推定を行うことができる。例えば、無加速状態におけるNフレーム加速度の過去の履歴等の情報を利用して、公知の平滑化手法に基づく平滑化処理を行ってNフレーム加速度の現在のバイアス値を推定することとしてもよい。
【0140】
5−2.Nフレーム速度ベクトルの補正
上述した位置算出システム5において、位置算出の正確性をより向上させるために、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを補正する処理を追加してもよい。
【0141】
図17は、この場合における位置算出システム5の処理部300を機能ブロックとして表した概念図である。なお、図6と同一の機能ブロックについては同一の符号を付して説明を省略する。処理部300は、移動状態判定部310と、姿勢算出部320と、座標変換部330と、バイアス推定部340と、バイアス補正部350と、速度ベクトル算出部360と、速度ベクトル補正部370と、位置算出部380とを備えて構成される。速度ベクトル算出部360の後段に速度ベクトル補正部370を設けた点が特徴である。
【0142】
姿勢算出部320は、ジャイロセンサー3の検出結果を用いて算定した移動体の姿勢を座標変換部330に出力するとともに、当該算定姿勢で示される移動体の移動方向を速度ベクトル補正部370に出力する。速度ベクトル補正部370は、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを、姿勢算出部320から入力した移動体の移動方向を用いて補正する。
【0143】
図18は、この場合に処理部300が実行する第2のナビゲーション処理の流れを示すフローチャートである。なお、図12のナビゲーション処理と同一のステップについては同一の符号を付して説明を省略する。
【0144】
ステップA9においてNフレーム速度ベクトル算出処理を行った後、処理部300は、Nフレーム速度ベクトル補正処理を実行する(ステップC10)。具体的には、速度ベクトル補正部370が、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを、以下説明する原理に従って補正する。
【0145】
図19は、Nフレーム速度ベクトル補正の原理の説明図である。説明を分かり易くするため、ここではNフレームの東西方向(E軸)及び南北方向(N軸)でなる2次元平面において速度ベクトルを補正する場合について図示・説明する。図19(A)は移動体の直進時における速度ベクトルの補正の説明図であり、図19(B)は移動体の回転(旋回)時における速度ベクトルの補正の説明図である。各図において、横軸は東西方向の軸(E軸)を示しており、縦軸は南北方向の軸(N軸)を示している。
【0146】
Nフレーム加速度ベクトルの東西方向成分及び南北方向成分を積分して最後に更新したNフレーム速度ベクトルに継ぎ足すと、Nフレーム速度ベクトルの東西方向成分及び南北方向成分が得られる。図19において、E軸上に記載した矢印がNフレーム速度ベクトルの東西方向成分を示しており、N軸上に記載した矢印がNフレーム速度ベクトルの南北方向成分を示している。
【0147】
この東西方向成分及び南北方向成分を合成することで、大きさが|V|で、向きがθ’であるNフレーム速度ベクトルが得られる。すなわち、動径を|V|、偏角をθ’とする円座標でNフレーム速度ベクトルが定義される。但し、図19では、Nフレーム速度ベクトルの向きを、速度ベクトルとE軸との成す角度で表現している。上記のNフレーム速度ベクトルが、速度ベクトル算出部360により算出されるNフレーム速度ベクトルである。
【0148】
Nフレーム速度ベクトルは、Nフレーム補正加速度ベクトルを積分して継ぎ足すことで算出される。そのため、Nフレーム補正加速度ベクトルに誤差が含まれていると、求められるNフレーム速度ベクトルに誤差が累積するという問題がある。Nフレーム速度ベクトルの東西方向成分及び南北方向成分が正確に求まらなければ、図19に示すNフレーム速度ベクトルの大きさ及び向きが正確に求まらず、誤差を含み得ることになる。
【0149】
そこで、Nフレーム速度ベクトルの大きさ及び向きを次のように補正する。先ず、Nフレーム速度ベクトルの向きθ’を、姿勢算出部320により算出された移動体の移動方向θに補正する。Nフレーム速度ベクトルの向きθ’は、Nフレーム補正加速度ベクトルを積分及び継ぎ足して間接的に求められる向きであるのに対し、移動方向θは、慣性センサーの検出結果から直接的に求められる移動体の向きである。そのため、Nフレーム補正速度ベクトルの向きは、移動体の移動方向に沿った正しい向きとなる。
【0150】
次に、Nフレーム速度ベクトルの大きさ|V|を|V|×αに補正する。ここで、「α」はNフレーム速度ベクトルの補正係数である。補正係数「α」を、移動体の直進時と回転時とで可変に設定して速度ベクトルを補正する。具体的には、移動体の直進時には、図19(A)に示すように補正係数「α=1」として、Nフレーム速度ベクトルの大きさをそのままにする。この場合、Nフレーム速度ベクトルの大きさとNフレーム補正速度ベクトルの大きさとは同じとなる。
【0151】
一方、移動体の回転時には、図19(B)に示すように「α<1」として、Nフレーム速度ベクトルの大きさが小さくなるように補正する。この場合、Nフレーム補正速度ベクトルの大きさは、Nフレーム速度ベクトルの大きさよりも小さくなる。本願発明者が種々の条件で実験を行った結果、移動体の回転時には、補正係数「α」を「1」よりも僅かに小さな値とすることで、速度ベクトルの補正の効果が表れ、位置算出の正確性が向上することがわかった。但し、移動体の回転には、移動体の右折、左折、転回等、1周より短い回転が含まれる。
【0152】
なお、図19では説明を分かり易くするために2次元平面で考えたが、3次元空間に拡張することは容易である。この場合は、東西南北の2次元平面における移動体の向きに加えて、上下方向の向きを考慮して同様の補正を行えばよい。すなわち、動径を|V|、偏角を(θ’,φ’)とする球座標でNフレーム速度ベクトルを定義する。そして、Nフレーム速度ベクトルの向きを姿勢算出部320により算出された姿勢角(θ,φ)に補正し、速度ベクトルの大きさを|V|×αに補正すればよい。
【0153】
図18の第2のナビゲーション処理に戻って、Nフレーム速度ベクトル補正処理を行った後、処理部300は、位置算出処理を行う(ステップA11)。具体的には、位置算出部380が、速度ベクトル補正部370により補正されたNフレーム速度ベクトル(Nフレーム補正速度ベクトル)を用いて、最新の位置を算出・更新する。
【0154】
なお、移動体の移動方向の検出は、ジャイロセンサー3の検出結果を用いることに限られない。信頼性の高い移動方向が得られるのであれば、他の検出手段によって移動方向を検出することとしてもよい。例えば、地磁気センサー等の方位センサーによって移動方向を検出することとしてもよい。
【0155】
5−3.出力位置の決定
上述した実施例では、所定のリセットタイミングにおいて、GPSにより算出された位置で算定位置をリセットするものとして説明したが、GPSを利用して算出した位置(GPS算出位置)と、慣性航法演算により算出した位置(慣性航法演算位置)とを併用して、最終的な出力位置を算出・決定することとしてもよい。例えば、GPS算出位置と慣性航法演算位置とを平均化して最終的な出力位置としてもよい。
【0156】
5−4.カルマンフィルター処理のパラメーター値
上記の実施形態で使用したカルマンフィルター処理のパラメーター値はあくまでも一例であり、適宜設定変更可能である。特に、観測誤差“R”として設定する値は、原理で説明したように移動体の移動状態に応じて大小切り替えることとすればよく、設定する具体的な想定誤差値は適宜変更してよい。
【0157】
5−5.電子機器
上述した実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0158】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の加速度算出及び位置算出を実現することが可能である。
【0159】
5−6.衛星測位システム
上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよいことは勿論である。
【符号の説明】
【0160】
1 加速度算出システム、 2 加速度センサー、 3 ジャイロセンサー、 5 位置算出システム、 6 カーナビゲーション装置、 10 移動状態判定部、 20 姿勢算出部、 30 座標変換部、 40 バイアス推定部、 50 バイアス補正部、 100 GPSアンテナ、 200 GPS位置算出部、 300 処理部、 400 操作部、 500 表示部、 600 IMU、 700 記憶部
【技術分野】
【0001】
本発明は、加速度算出方法、位置算出方法及び加速度算出装置に関する。
【背景技術】
【0002】
測位用信号を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。GPSでは、複数のGPS衛星の位置や各GPS衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置座標と時計誤差とを求める位置算出計算を行う。
【0003】
測位用信号を利用した位置算出では、種々の誤差要因によって位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、移動体の速度ベクトルを用いた慣性航法演算を行うことにより、GPSを利用して算出した位置を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開平8−68651号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
従来から用いられている慣性航法演算では、例えば加速度センサーを用いるのであれば、加速度センサーにより検出された加速度を積分することで移動体の速度ベクトルを算出し、今回の速度ベクトルの方向に今回の速度ベクトル分の移動量(移動距離)を前回位置に加算することで、移動体の位置を新たに算出・更新する手法がとられる。ここで「ベクトル」とは、方向及び大きさを表す便宜上の用語である。方向及び大きさを表す表現としては、例えば直交座標系や球座標系における各軸の値としてもよいことは勿論であるが、何れの表記法をとったとしても「ベクトル」と等価であることは自明である。従って、本明細書においては、方向及び大きさの両方を表す用語として「ベクトル」を用いる。
【0006】
加速度センサーは移動体に固定的に設置される。従って、加速度センサーが検出するのは、移動体自身を基準としたローカルな座標系(以下、「ローカル座標系」と称する。)における加速度である。しかし、慣性航法演算において求めたいのは、例えば地球表面といった移動体を含む座標系(以下、「絶対座標系」と称する。)における移動体の位置(位置座標)である。そのため、加速度センサーにより検出されたローカル座標系における加速度を、絶対座標系における加速度に変換して利用する必要がある。
【0007】
ローカル座標系から絶対座標系への座標変換を行うためには、移動体の姿勢(より正確には絶対座標系での姿勢)の情報が必要となる。移動体の姿勢を正しく求めることができれば、座標変換を適切に行うことができる。例えば、移動体に設置されたジャイロセンサーにより検出された角速度を用いて、移動体の姿勢を算出することが考えられる。しかし、移動体の姿勢を正しく求めることは容易ではない。なぜなら、ジャイロセンサーの検出結果には、典型的にはゼロ点バイアスといった誤差成分が含まれるため、算出される移動体の姿勢にも誤差が含まれてしまうためである。
【0008】
ここでもう1つ問題となるのは、加速度センサーの検出結果にもゼロ点バイアスといった誤差成分が含まれることである。そのため、ローカル座標系から絶対座標系に変換された加速度には、加速度センサーの誤差成分と、ジャイロセンサーの誤差成分との2重の誤差成分が含まれることになる。この2重の誤差成分を分離することは極めて困難である。慣性航法演算では、絶対座標系における移動体の加速度を積分しながら移動体の速度ベクトルを逐次的に求めていくため、速度ベクトルに含まれる誤差が時間経過に伴って累積的に増加するという大きな問題があった。
【0009】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、移動体の加速度を適切に求めるための新たな手法を提案することにある。
【課題を解決するための手段】
【0010】
以上の課題を解決するための第1の形態は、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、を含む加速度算出方法である。
【0011】
また、他の形態として、移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、を備えた加速度算出装置を構成してもよい。
【0012】
この第1の形態等によれば、移動体に設置された加速度センサーが検出した移動体のローカル座標系におけるローカル加速度が、移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換される。そして、変換された加速度に内在するバイアス値が、その変換された加速度と移動体の移動状態とを用いて推定され、当該バイアス値を用いて、絶対座標系に変換された加速度が補正される。
【0013】
加速度センサーの検出結果及びジャイロセンサーの検出結果には、それぞれバイアス値が含まれ得るが、それらの大きさを個別に知ることは困難である。そこで、第1の形態等では、ジャイロセンサーの検出結果を用いてローカル座標系から絶対座標系に変換された加速度に着目した。そして、変換された加速度に内在するバイアス値を、その変換された加速度それ自体と移動体の移動状態とを用いて推定することとした。これにより、加速度センサーのバイアス値とジャイロセンサーのバイアス値とを分離せずとも、絶対座標系における加速度を適切に補正することが可能となり、移動体の加速度を適切に求めることができる。
【0014】
また、第2の形態として、第1の形態の加速度算出方法であって、前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、加速度算出方法を構成してもよい。
【0015】
この第2の形態によれば、バイアス値を状態推定値とし、絶対座標系に変換された加速度を観測値とするカルマンフィルター処理が行われて、当該加速度に内在するバイアス値が推定される。カルマンフィルターを利用した処理により、バイアス値の推定を適切に行うことができる。
【0016】
また、第3の形態として、第2の形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、加速度算出方法を構成してもよい。
【0017】
より具体的には、例えば第4の形態として、第3の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、加速度算出方法を構成してもよい。
【0018】
この第4の形態によれば、無加速状態での想定誤差パラメーター値を、加減速状態の場合に比べて小さい値とする。想定誤差パラメーター値は、例えば、観測値に含まれ得る誤差の大きさを想定したパラメーター値である。想定誤差パラメーター値を小さくするということは、観測値に含まれ得る誤差を小さく見積もることを意味する。そのため、移動体の移動状態が無加速状態の場合には、観測値を信用・重視してバイアス値を推定することができる。
【0019】
また、例えば第5の形態として、第3の形態の加速度算出方法であって、前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、加速度算出方法を構成することも可能である。
【0020】
この第5の形態によれば、移動体の移動状態が無加速状態の場合には、状態推定値の観測値への追従性を加減速状態の場合よりも高める想定誤差パラメーター値とする。絶対座標系に変換された加速度を観測値とするため、移動体の移動状態が無加速状態の場合には、絶対座標系に変換された加速度に追従させるようにバイアス値を推定することができる。
【0021】
また、第6の形態として、第2〜第5の何れかの形態の加速度算出方法であって、前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、加速度算出方法を構成してもよい。
【0022】
絶対座標系における加速度が過度に大きい場合に、その加速度をそのまま観測値としてカルマンフィルター処理を行うと、観測値の大きさに引きずられてバイアス値が適切に推定されなくなるおそれがある。かかる問題は移動体の移動状態が加減速状態の場合に起こり易い。そこで、第6の形態では、移動体の移動状態が加減速状態の場合に絶対座標系に変換された加速度が所定の限界値を超えた場合、当該限界値を観測値としてカルマンフィルター処理を行う。このように観測値の上限を定めることで、上記の問題を解決することができる。
【0023】
また、第7の形態として、第1の形態の加速度算出方法であって、前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、加速度算出方法を構成してもよい。
【0024】
移動体が無加速状態であれば、原理的には加速度はゼロであるため、加速度の検出値はバイアス値に相当する値であると言える。そこで、第7の形態では、移動体が無加速状態である場合の絶対座標系に変換された加速度の平均化処理を行い、その平均化された値を加速度に内在するバイアス値と推定する。
【0025】
また、第8の形態として、第1〜第7の何れかの形態の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、前記移動体の向きを検出することと、前記移動体の向きを用いて前記速度ベクトルを補正することと、前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、を含む位置算出方法を構成してもよい。
【0026】
この第8の形態によれば、上述した加速度算出方法によって算出された加速度を用いて移動体の速度ベクトルが算出される。そして、検出した移動体の向きを用いて速度ベクトルが補正され、当該補正された速度ベクトルを用いて移動体の位置が算出される。速度ベクトルの算出とは別に移動体の向きを検出し、検出した移動体の向きを用いて速度ベクトルを補正することで、移動体の速度ベクトルを適切に求めることができる。そして、この速度ベクトルを用いることで、移動体の位置が正確に求まる。
【図面の簡単な説明】
【0027】
【図1】加速度算出システムのシステム構成の説明図。
【図2】Nフレーム加速度ベクトルの南北成分の時間変化を示す図。
【図3】観測誤差の切替の説明図。
【図4】位置算出システムのシステム構成の説明図。
【図5】カーナビゲーション装置の機能構成の一例を示すブロック図。
【図6】処理部を機能ブロックとして表した概念図。
【図7】観測誤差設定用テーブルのテーブル構成の一例を示す図。
【図8】センサーデータのデータ構成の一例を示す図。
【図9】推定バイアス値データのデータ構成の一例を示す図。
【図10】加速度ベクトルデータのデータ構成の一例を示す図。
【図11】出力位置データのデータ構成の一例を示す図。
【図12】ナビゲーション処理の流れを示すフローチャート。
【図13】Nフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図14】(A)Nフレーム速度ベクトルの南北成分の時間変化。(B)Nフレーム速度ベクトルの東西成分の時間変化。(C)Nフレーム速度ベクトルの高度成分の時間変化。
【図15】位置算出を行った実験結果を示す図。
【図16】第2のNフレーム加速度ベクトル補正処理の流れを示すフローチャート。
【図17】変形例における処理部を機能ブロックとして表した概念図。
【図18】第2のナビゲーション処理の流れを示すフローチャート。
【図19】(A)移動体の直進時におけるNフレーム速度ベクトル補正の説明図。(B)移動体の回転時におけるNフレーム速度ベクトル補正の説明図。
【発明を実施するための形態】
【0028】
以下、図面を参照して、本発明の好適な実施形態について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【0029】
1.システム構成
図1は、本実施形態における加速度算出システム1のシステム構成の説明図である。加速度算出システム1は、各種の移動体に適用され、当該移動体の加速度を算出するシステムである。加速度算出システム1には、慣性センサーの一種である加速度センサー2及びジャイロセンサー3が含まれる。
【0030】
加速度センサー2及びジャイロセンサー3は、移動体に設置され、移動体を基準としたローカル座標系におけるローカル加速度及びローカル角速度を検出して出力するように構成されている。本実施形態では、このローカル座標系を機体座標系(Body Frame)(以下、「Bフレーム」と称する。)として説明する。そして、本実施形態では、加速度センサー2により検出されたBフレームにおける加速度を、絶対座標系の一種であるNED(North East Down)座標系(以下、「Nフレーム」と称する。)における加速度に変換する。
【0031】
移動体の加速度及び速度は、方向及び大きさを持っている。そのため、本明細書では、移動体の加速度及び速度を「加速度ベクトル」及び「速度ベクトル」として説明する。また、ローカル座標系である「Bフレーム」において定義されるベクトルと、絶対座標系である「Nフレーム」において定義されるベクトルとを区別するため、各変量を表す文言の先頭にフレームの種類を付して説明する。例えば、Bフレームにおける加速度ベクトルは「Bフレーム加速度ベクトル」と称し、Nフレームにおける加速度ベクトルは「Nフレーム加速度ベクトル」と称する。
【0032】
Bフレームは、移動体の前方を正とする前後方向をX軸(ロール軸)、右方を正とする左右方向をY軸(ピッチ軸)、鉛直下方を正とする上下方向をZ軸(ヨー軸)とする三次元直交座標系である。それに対し、Nフレームは、北方を正とする南北方向をN軸、東方を正とする東西方向をE軸、鉛直下方を正とする高度方向をD軸とする三次元直交座標系である。
【0033】
座標変換の際には、加速度センサー2の検出結果(Bフレームの加速度ベクトル)のうち、加速度の向き(加速度ベクトルの向き)を座標変換し、加速度の大きさ(加速度ベクトルの大きさ)は維持する。言い換えると、加速度センサー2の相対的な検出方向を、絶対座標系における方向(絶対方向)に座標変換する。
【0034】
加速度算出システム1は、処理ブロックとして、移動状態判定部10と、姿勢算出部20と、座標変換部30と、バイアス推定部40と、バイアス補正部50とを備える。移動状態判定部10は、移動体の移動状態を判定する。本実施形態では、移動状態判定部10は、主に移動体が無加速である状態(以下、「無加速状態」と称する。)と、移動体が加速/減速している状態(以下、「加減速状態」と称する。)との何れの移動状態であるかを判定する。無加速状態には、移動体が停止している状態(停止状態)と、移動体が等速で移動している状態(等速状態)とが含まれる。
【0035】
姿勢算出部20は、ジャイロセンサー3により検出されたBフレーム角速度を積分及び継ぎ足していくことで、移動体の姿勢角を算定する。ここで、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。
【0036】
具体的には、姿勢算出部20は、所定時間の間に検出されたBフレーム角速度を「積分」することで、その所定時間の間の移動体の姿勢角の変化方向及び変化量を算出する。所定時間はBフレーム角速度の検出時間間隔よりも長く設定され、通常は一定時間とされる。例えば、所定時間を100ミリ秒とし、Bフレーム角速度の検出時間間隔(より正確にはジャイロセンサー3から出力された信号のサンプリング時間間隔)が1〜10ミリ秒の間で適宜調整されるとした場合には、検出時間間隔がいくつであろうとも、必ず、100ミリ秒毎に移動体の姿勢角の変化方向及び変化量を算出することとなる。
【0037】
そして、100ミリ秒毎に算出される姿勢角の変化方向及び変化量を、最後に更新した姿勢角に「継ぎ足す」ことで、最新の姿勢角(以下、この最新の姿勢角によって定まる移動体の最新の姿勢のことを「算定姿勢」と称する。)を算出・更新する。そして、その算定姿勢を座標変換部30に出力する。移動体の姿勢がわかれば、移動体の移動方向もわかる。そのため、姿勢算出部20は、移動体の移動方向を検出する移動方向検出部とも言える。
【0038】
座標変換部30は、姿勢算出部20から入力した算定姿勢に基づいて、加速度センサー2の検出結果であるBフレーム加速度ベクトルを、Nフレーム加速度ベクトルに変換する座標変換処理を行う。具体的には、姿勢算出部20から入力した移動体の姿勢角を用いて、BフレームからNフレームへの座標変換行列を算出する。そして、当該座標変換行列を用いて加速度センサー2の検出結果を変換して、バイアス推定部40に出力する。座標変換部30は、Nフレーム加速度ベクトルを算出する算出部とも言える。
【0039】
バイアス推定部40は、座標変換部30から出力されるNフレーム加速度ベクトルを用いて、当該Nフレーム加速度ベクトルに内在するバイアス値を推定する。ここで、バイアス値とは、Nフレーム加速度ベクトルの各成分にそれぞれ付加される誤差値(典型的にはゼロ点バイアス)のことを意味する。
【0040】
本実施形態において、バイアス推定部40は、カルマンフィルターを利用した推定処理(カルマンフィルター処理)を行ってバイアス値を推定する。カルマンフィルターは、離散的な誤差を有する観測値を用いて、あるシステムの時々刻々と変換する量を推定するための手法である。そして、バイアス推定部40は、推定したバイアス値(以下、「推定バイアス値」と称す。)をバイアス補正部50に出力する。
【0041】
バイアス補正部50は、バイアス推定部40から出力される推定バイアス値を用いて、座標変換部30から出力されたNフレーム加速度ベクトルを補正する。すなわち、Nフレーム加速度ベクトルの各成分それぞれに対して、対応する成分のバイアス値を加減算する。そして、その結果をNフレーム補正加速度ベクトルとして出力する。
【0042】
2.原理
2−1.バイアス値の推定
本願発明者は、Nフレーム加速度ベクトルを実際に算出する実験を行った。移動体の移動状態を変化させて、慣性センサー(加速度センサー2及びジャイロセンサー3)を利用して慣性航法演算によりNフレーム加速度ベクトルを算出した。また、衛星測位システムの一種であるGPS(Global Positioning System)を利用してNフレームにおける移動体の速度ベクトルを算出し、その速度ベクトルを微分することでNフレーム加速度ベクトルを同様に算出した。
【0043】
図2は、この場合の実験結果の一例を示す図である。ここでは、Nフレーム加速度ベクトルの南北成分(南北方向の加速度)の時間変化を一例として示して説明する。図2において、横軸は時間を示し、縦軸は南北方向の加速度(単位はメートル毎秒毎秒[m/s2])を示している。移動体の移動状態を、加減速状態→無加速状態→加減速状態の順に変化させてNフレーム加速度を算出した。図2において、慣性航法を利用して算出したNフレーム加速度には「慣性航法」、GPSを利用して算出したNフレーム加速度には「GPS」と付記した。
【0044】
この図を見ると、グラフの中央部において、GPSを利用して算出したNフレーム加速度がほぼ“0”となっている区間が存在する。この区間が、移動体が無加速状態であった区間である。理論上、移動体が無加速状態であれば進行方向の加速度は“0”である。このことから、GPSを利用して算出したNフレーム加速度は正しいことを意味する。しかし、その一方で、慣性航法を利用して算出したNフレーム加速度を見ると、無加速状態の区間において値が“0”となっておらず、マイナスの値を中心として上下に振動していることがわかる。
【0045】
この無加速状態の区間におけるNフレーム加速度の“0”からのズレは、Nフレームの加速度に含まれるバイアス値に相当するものと考えることができる。そこで、本願発明者は、移動体の無加速状態において観測されるNフレーム加速度に着目し、Nフレーム加速度に内在するバイアス値を、Nフレーム加速度と移動体の移動状態とを用いて推定する手法を考案した。その手法の一例として、本実施形態では、カルマンフィルターを利用した推定処理を行ってNフレーム加速度に含まれるバイアス値を推定する手法について説明する。
【0046】
具体的な定式化を行う。本実施形態では、カルマンフィルターにおいて推定対象とするシステムの状態推定値“X”をバイアス値とし、次式(1)のように設定する。
【数1】
但し、「(bN,bE,bD)」は、それぞれNフレーム加速度ベクトルのバイアス値の南北方向(N)、東西方向(E)及び高度方向(D)の各成分である。
【0047】
また、状態推定値“X”の精度を表すために、状態推定値“X”を構成するバイアス値の各成分の誤差共分散“P”を表す行列(誤差共分散行列)を設定する。
【0048】
カルマンフィルターでは、外部から与えられる観測値“Z”を利用して1ステップ前に予測した状態推定値“X”及び誤差共分散“P”を補正する補正処理(観測更新)と、補正した状態推定値“X”及び誤差共分散“P”から次のステップの状態推定値“X”及び誤差共分散“P”を予測する予測処理(時刻更新)との2つの処理を繰り返し行う。そして、この繰り返し処理により、尤もらしい状態推定値“X”を推定する。
【0049】
本実施形態では、Nフレーム加速度ベクトルの各成分を観測値“Z”として入力する。すなわち、観測値“Z”を次式(2)のように設定する。
【数2】
但し、「(AN,AE,AD)」は、それぞれNフレーム加速度ベクトルの南北方向(N)、東西方向(E)及び高度方向(D)の各成分である。
【0050】
補正処理(観測更新)は、次式(3)〜(5)で定式化される。
【数3】
【数4】
【数5】
【0051】
予測処理(時刻更新)は、次式(6)及び(7)で定式化される。
【数6】
【数7】
【0052】
予測処理において、“Φ”は状態遷移行列と呼ばれ、補正処理で補正された状態推定値“X”から次の時刻の状態推定値“X”への遷移を表す行列として定義される。また、“Q”はシステムノイズ(プロセスノイズ)と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。本実施形態では、これらの行列を次式(8)及び(9)のように設定する。
【数8】
【数9】
【0053】
補正処理において、“H”は観測行列と呼ばれる行列である。観測行列“H”は、状態推定値“X”の次元を観測値“Z”の次元に変換するための変換行列として用いられる。観測行列“H”を1ステップ前に予測された状態推定値“X”に作用させることで、観測値の予測値“HX”が得られる。本実施形態では、この観測行列“H”を次式(10)のように設定する。
【数10】
但し、観測行列“H”の行と列の並びは、状態推定値“X”の3次元の成分の並び(バイアス値の南北成分、東西成分、高度成分)にそれぞれ対応する。
【0054】
式(10)を見ると、観測行列“H”は、対角成分が“1”であり、それ以外の成分が“0”である対角行列となっている。本実施形態では、状態推定値“X”をNフレーム加速度ベクトルのバイアス値とし、観測値“Z”をNフレーム加速度ベクトルとしている。そのため、式(10)の観測行列“H”を状態推定値“X”に作用させることは、Nフレーム加速度ベクトルのバイアス値を、Nフレーム加速度ベクトルにそのまま変換することを意味する。
【0055】
式(3)において、“K”はカルマンゲインと呼ばれる補正係数である。カルマンゲイン“K”は、予測された状態推定値“X”を重視するか、実際の観測値“Z”と予測された観測値“HX”との差“Z−HX”を重視するかを決定付けるパラメーター値として用いられる。
【0056】
式(4)は、実際の観測値“Z”と予測された観測値“HX”との差“Z−HX”を用いて、状態推定値“X”を補正することを表している。また、式(5)は、1ステップ前に予測された誤差共分散“P”とカルマンゲイン“K”と観測行列“H”とを用いて、誤差共分散“P”を補正することを表している。
【0057】
ところで、カルマンゲイン“K”は、観測誤差“R”の大きさによって決定付けられる。観測誤差“R”は、観測値“Z”に含まれる想定誤差の大きさを表すパラメーター(想定誤差パラメーター)である。観測誤差“R”は、期待値(平均値)を“0”とする正規分布型のホワイトノイズと仮定して設定される。
【0058】
本実施形態では、観測値“Z”をNフレーム加速度ベクトルの南北方向、東西方向及び高度方向の3成分としている。そのため、観測誤差“R”を、観測値“Z”の各成分の共分散を表す3×3の行列(観測誤差行列)として定義する。観測誤差行列の対角成分には、正規分布(ガウス分布)における標準偏差“σ”の2乗値(=分散“σ2”)を想定誤差値として設定する。
【0059】
移動体の移動状態に応じた適切なバイアス推定を実現するために、本実施形態では、移動体の移動状態に応じて、想定誤差パラメーターである観測誤差“R”の値を切り替える処理を行う。すなわち、移動体の移動状態に応じて、観測誤差行列の対角成分に設定する想定誤差値を可変に設定する。以下、この観測誤差の切替の詳細について説明する。
【0060】
2−2.観測誤差“R”の切替
図3は、観測誤差“R”の切替の説明図である。図3では、Nフレーム加速度ベクトルの南北方向、東西方向及び高度方向の成分のうちの南北成分(N成分)を例示している。横軸は時間である。また、模式的に、加速度の時間変化を点線で示し、推定バイアス値の理想的な時間変化を実線で示している。
【0061】
移動体が無加速状態である場合は、移動体の移動分の加速度は“0”である。そのため、観測値“Z”として与えられるNフレーム加速度ベクトルが、Nフレーム加速度ベクトルのバイアス値に相当すると考えることができる。このことから、移動体が無加速状態である場合は、状態推定値“X”の観測値“Z”への追従性を比較的高めるように観測誤差“R”を設定することが適切である。
【0062】
そこで、移動体が無加速状態である場合は、観測誤差“R”を小さく設定する。具体的には、観測誤差“R”を、例えば次式(11)のように設定する。
【数11】
【0063】
観測誤差“R”を小さくすることで、無加速状態では、図3に示すように、観測値“Z”に追従するように状態推定値“X”が推定されるようになる。その理由は、式(3)から導くことができる。式(3)において、観測誤差“R”を“0”に近付けると(R→0)、カルマンゲイン“K”は“H−1”に近づく。すると、式(4)は「X(t)=Z(t)」となる。これは、状態推定値“X”を観測値“Z”とすること、すなわち観測値“Z”を信用・重視して状態推定値“X”を補正することを意味する。
【0064】
それに対して、移動体が加減速状態である場合は、移動体の移動分の加速度にバイアス値が重畳された値が観測される。すなわち、観測値“Z”(Nフレーム加速度ベクトル)には、バイアス値の他に移動分の加速度が含まれるため、観測値“Z”に追従させるように状態推定値“X”を補正することは適切ではない。よって、状態推定値“X”の観測値“Z”への追従性を比較的劣化させるように観測誤差“R”を設定することが適切である。
【0065】
そこで、移動体が加減速状態である場合は、観測誤差“R”を大きく設定する。具体的には、観測誤差“R”を、例えば次式(12)のように設定する。
【数12】
【0066】
観測誤差“R”を大きくすることで、加減速状態では、図3に示すように、無加速状態において推定されたバイアス値が維持されるようにバイアス値が推定される。なぜならば、式(3)からわかるように、観測誤差“R”を“+∞”に近付けると(R→+∞)、カルマンゲイン“K”は“0”に近づく。すると、式(4)は「X(t)=X(t)」となる。これは、状態推定値“X”を1ステップ前に予測された状態推定値“X”とすること、すなわち1ステップ前に予測された状態推定値“X”を信用・重視して状態推定値“X”を補正することを意味する。無加速状態において推定されたバイアス値が加減速状態において維持されるため、バイアス値の推定が適切に行われる。
【0067】
2−3.観測値“Z”の調整
上述した観測誤差“R”の切替に加えて、本実施形態では、観測値“Z”の調整(クリッピング)を行う。移動体が加減速状態である場合は、Nフレーム加速度ベクトルは大きく変動し得る。Nフレーム加速度ベクトルが過度に大きくなると、上述した観測誤差“R”の切替を行っても、バイアス値の推定が適切に行われなくなるおそれがある。
【0068】
前述したように、加減速状態においては、観測誤差“R”を大きくしてカルマンゲイン“K”を“0”に近付けることにより、式(4)を「X(t)=X(t)」と近似することが理想的である。しかし、観測値“Z”が過度に大きい場合には、カルマンゲイン“K”を小さくしても、式(4)の右辺第2項の“K(Z−HX)”がある程度大きな値を持つようになり、理想状態が崩れてしまう。すると、無加速状態において推定されたバイアス値が維持されずに、観測値“Z”に引きずられて値が変動する可能性がある。
【0069】
かかる問題に対処するため、先ず、観測値“Z”の絶対値が所定の限界値である閾値“θ”を超えているか否かを判定する。そして、閾値“θ”を超えている場合は、観測値“Z”を調整する処理を行う。具体的には、観測値“Z”が負の値であり、その絶対値が閾値“θ”を超えている場合には(Z<−θ)、観測値“Z”を“−θ”に調整する(Z=−θ)。また、観測値“Z”が正の値であり、閾値“θ”を超えている場合には(Z>θ)、観測値“Z”を“θ”に調整する(Z=θ)。
【0070】
なお、この場合に限界値として設定する閾値“θ”の値については、適用するシステムに応じて適宜設定される。
【0071】
3.実施例
次に、上記の加速度算出システム1を適用した位置算出システム5の実施例について説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
【0072】
3−1.システム構成
図4は、本実施例における位置算出システム5のシステム構成の説明図である。位置算出システム5は、移動体の一種である四輪自動車(以下、単に「自動車」と称す。)に、加速度算出装置及び位置算出装置を具備した電子機器の一種であるカーナビゲーション装置6が搭載されたシステムである。
【0073】
カーナビゲーション装置6は、自動車に搭載される電子機器であり、自動車の加速度を算出する加速度算出装置及び自動車の位置を算出する位置算出装置を備えて構成される。また、カーナビゲーション装置6は、加速度センサー2及びジャイロセンサー3を具備するセンサーユニットであるIMU(Inertial Measurement Unit)600を搭載している。IMU600は、自動車に対する相対的な座標系における加速度及び角速度を検出して出力する。本実施例では、この相対的な座標系がBフレームと一致するような姿勢でカーナビゲーション装置6が自動車に設置・固定されているものとして説明する。
【0074】
カーナビゲーション装置6は、Nフレームにおいて、GPSを利用した位置算出演算と、IMU600の検出結果を利用した慣性航法演算との2種類の演算を行い、自動車の出力位置を算定する処理を行う。そして、算定した出力位置をプロットしたナビゲーション画面を生成して、表示部500に表示させる。なお、本実施例では、慣性航法演算を用いて算出した位置を、後述するGPS位置算出部200により算出された位置と区別するために「算定位置」と称する。
【0075】
3−2.機能構成
図5は、カーナビゲーション装置6の機能構成の一例を示すブロック図である。カーナビゲーション装置6は、GPSアンテナ100と、GPS位置算出部200と、処理部300と、操作部400と、表示部500と、IMU600と、記憶部700とを備えて構成される。
【0076】
GPSアンテナ100は、測位用衛星の一種であるGPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信信号をGPS位置算出部200に出力する。GPS衛星信号は、拡散符号の一種であるCA(Coarse and Acquisition)コードによって、スペクトラム拡散方式として知られるCDMA(Code Division Multiple Access)方式によって変調された1.57542[GHz]の通信信号である。CAコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号であり、GPS衛星毎に異なるものである。
【0077】
GPS位置算出部200は、GPSアンテナ100から出力された信号に基づいて自動車の位置を計測する回路或いは装置であり、いわゆるGPS受信装置に相当する機能ブロックである。図示を省略するが、GPS位置算出部200は、RF受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、RF受信回路部と、ベースバンド処理回路部とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0078】
ベースバンド処理回路部は、RF受信回路部から出力された受信信号に対して相関処理等を行ってGPS衛星信号を捕捉・追尾する。そして、GPS衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置(位置座標)を算出する。そして、算出した位置をGPSデータ727として処理部300に出力する。
【0079】
処理部300は、記憶部700に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置6の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部300は、IMU600の検出結果を利用した慣性航法演算処理を行って自動車の位置(位置座標)を算出する。そして、GPS位置算出部200がGPSを利用して演算した位置(GPS演算位置)と、処理部300が慣性航法を利用して演算した位置(慣性航法演算位置)とをもとに、自動車の現在位置を指し示した地図を表示部500に表示させる処理を行う。
【0080】
図6は、処理部300を機能ブロックとして表した概念図である。処理部300は、移動状態判定部310と、姿勢算出部320と、座標変換部330と、バイアス推定部340と、バイアス補正部350と、速度ベクトル算出部360と、位置算出部380とを機能部として有する。移動状態判定部310〜バイアス補正部350は、図1の加速度算出システム1の移動状態判定部10〜バイアス補正部50にそれぞれ相当する。
【0081】
姿勢算出部320は、ジャイロセンサー3により検出されたBフレームにおける3軸の加速度を積分及び継ぎ足し処理することで、自動車の姿勢を算定する。座標変換部330は、姿勢算出部320から入力した算定姿勢に基づいて、IMU600のBフレームにおける検出結果をNフレームにおける検出結果に座標変換する。
【0082】
バイアス推定部340は、座標変換部330から出力されるNフレーム加速度ベクトルを観測値とするカルマンフィルター処理を行って、Nフレーム加速度ベクトルのバイアス値を推定する。バイアス補正部350は、バイアス推定部340から入力した推定バイアス値を用いて、座標変換部330から出力されるNフレーム加速度ベクトルを補正する。
【0083】
速度ベクトル算出部360は、バイアス補正部350から入力したNフレーム補正加速度ベクトルを積分及び継ぎ足し処理することで、Nフレームにおける自動車の速度ベクトル(Nフレーム速度ベクトル)を算出する。そして、算出した自動車のNフレーム速度ベクトルを位置算出部380に出力する。
【0084】
位置算出部380は、速度ベクトル算出部360から入力した自動車のNフレーム速度ベクトルと、1ステップ前に算出した位置(前回算出位置)とを用いて、Nフレームにおける自動車の位置を算出・更新する。
【0085】
図5の機能ブロックの説明に戻って、操作部400は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部300に出力する。この操作部400の操作により、目的地の入力等の各種指示入力がなされる。
【0086】
表示部500は、LCD(Liquid Crystal Display)等により構成され、処理部300から入力される表示信号に基づいた各種表示を行う表示装置である。表示部500には、ナビゲーション画面等が表示される。
【0087】
IMU600は、加速度センサー2及びジャイロセンサー3を備え、予めセンサーに対応付けて定められたローカル座標系の直交3軸それぞれの軸方向の加速度及び各軸の軸回りの角速度を検出可能に構成される。なお、加速度センサー2及びジャイロセンサー3は、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【0088】
記憶部700は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置6のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0089】
3−3.データ構成
図5に示すように、記憶部700には、プログラムとして、処理部300により読み出され、ナビゲーション処理(図12参照)として実行されるナビゲーションプログラム710が記憶されている。また、ナビゲーションプログラム710には、Nフレーム加速度ベクトル算出処理として実行されるNフレーム加速度ベクトル算出プログラム711と、Nフレーム加速度ベクトル補正処理(図13参照)として実行されるNフレーム加速度ベクトル補正プログラム712とがサブルーチンとして含まれている。
【0090】
ナビゲーション処理とは、処理部300が、Nフレーム加速度ベクトルの算出処理及び補正処理を行うとともに、補正したNフレーム加速度ベクトルを用いてNフレーム速度ベクトルを算出し、当該Nフレーム速度ベクトルを用いて自動車の位置を算定・更新する処理である。そして、算定位置をプロットしたナビゲーション画面を生成して、表示部500に表示させる。ナビゲーション処理については、フローチャートを用いて詳細に後述する。
【0091】
また、記憶部700には、データとして、観測誤差設定用テーブル720と、センサーデータ721と、推定バイアス値データ723と、加速度ベクトルデータ725と、GPSデータ727と、出力位置データ729とが記憶される。
【0092】
観測誤差設定用テーブル720は、観測誤差“R”を設定するために用いられるテーブルであり、そのテーブル構成例を図7に示す。観測誤差設定用テーブル720には、移動体の移動状態と対応付けて、観測誤差行列に設定する想定誤差値(分散“σ2”の値)が定められている。具体的には、無加速状態については、第1の想定誤差値である“12”が定められており、加減速状態については、第2の想定誤差値である“1002”が定められている。
【0093】
センサーデータ721は、IMU600の検出結果が時系列に記憶されたデータであり、そのデータ構成例を図8に示す。センサーデータ721には、時刻7211と対応付けて、加速度センサー2により検出されたBフレーム加速度7213と、ジャイロセンサー3により検出されたBフレーム角速度7215とが時系列に記憶される。
【0094】
推定バイアス値データ723は、推定されたNフレーム加速度ベクトルのバイアス値についてのデータであり、そのデータ構成例を図9に示す。推定バイアス値データ723には、時刻7231と対応付けて、自動車の移動状態7233と、NEDの各成分の推定バイアス値7235とが時系列に記憶される。
【0095】
加速度ベクトルデータ725は、補正前後におけるNフレーム加速度ベクトルについてのデータであり、そのデータ構成例を図10に示す。加速度ベクトルデータ725には、時刻7251と対応付けて、Nフレーム加速度ベクトル7253と、Nフレーム補正加速度ベクトル7255とが時系列に記憶される。
【0096】
GPSデータ727は、GPS位置算出部200により算出・決定された位置(以下、「GPS算定位置」と称す。)が時系列に記憶されたデータである。GPSデータ727は、例えば所定時間間隔(例えば10分間隔)で更新される。
【0097】
出力位置データ729は、表示部500に表示出力させる位置のデータであり、そのデータ構成例を図11に示す。出力位置データ729には、位置算出が行われた時刻7291と対応付けて、出力位置7293と、測位種別7295とが時系列に記憶される。
【0098】
測位種別7295は、対応する出力位置7293が、GPS位置算出部200により算出された位置と、位置算出部380により算出された位置との何れの位置であるかを示すフラグである。GPS位置算出部200により算出された位置である場合は「GPS」が、位置算出部380により算出された位置である場合は「慣性航法」がそれぞれ記憶される。
【0099】
3−4.処理の流れ
図12は、記憶部700に記憶されているナビゲーションプログラム710が処理部300により読み出されて実行されることで、カーナビゲーション装置6において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、IMU600により検出された加速度及び角速度が、記憶部700のセンサーデータ721に随時記憶されるものとする。
【0100】
最初に、処理部300は、初期設定を行う(ステップA1)。例えば、Nフレーム加速度ベクトルのバイアス値の推定をカルマンフィルター処理を利用して行うために、カルマンフィルター処理に係る各種のパラメーター値の初期化を行う。
【0101】
その後、移動状態判定部310は、自動車の移動状態を判定する(ステップA3)。具体的には、IMU600の検出結果に基づいて、自動車が無加速状態であるか、加減速状態であるかを判定する。例えば、加速度センサー2により検出された自動車の進行方向の加速度が“0”に近い値として予め定められた所定の閾値以下又は未満の場合には、自動車は無加速状態であると判断することができる。
【0102】
次いで、処理部300は、記憶部700に記憶されているNフレーム加速度ベクトル算出プログラム711を読み出して実行することで、Nフレーム加速度ベクトル算出処理を行う(ステップA5)。
【0103】
具体的には、姿勢算出部320がジャイロセンサー3の検出結果を積分及び継ぎ足し処理して自動車の姿勢を算出する。そして、座標変換部330が、BフレームからNフレームへの座標変換行列を求め、当該座標変換行列を用いて、加速度センサー2のBフレームにおける検出結果をNフレームにおける検出結果に変換して、Nフレーム加速度ベクトルとする。なお、BフレームからNフレームへの座標変換行列については従来公知であるため、ここでは数式等を用いた詳細な説明を省略する。
【0104】
次いで、処理部300は、記憶部700に記憶されているNフレーム加速度ベクトル補正プログラム712を読み出して実行することで、Nフレーム加速度ベクトル補正処理を行う(ステップA7)。
【0105】
図13は、Nフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
先ず、バイアス推定部340は、状態推定値“X”及び誤差共分散“P”に初期値を設定する(ステップB1)。初回の処理では、状態推定値“X”及び誤差共分散“P”は任意の値(例えば“0”)とすることができる。また、2回目以降の処理では、前回の処理で求められた状態推定値“X”及び誤差共分散“P”(最新の値)とすることができる。
【0106】
次いで、バイアス推定部340は、記憶部700に記憶された観測誤差設定用テーブル720を参照して、観測誤差“R”を設定する。具体的には、移動状態が無加速状態であれば(ステップB3;無加速状態)、第1の想定誤差値(図7の観測誤差設定用テーブル720では“12”)を観測誤差行列に設定する(ステップB5)。また、移動状態が加減速状態であれば(ステップB3;加速/減速状態)、第2の想定誤差値(図7の観測誤差設定用テーブル720では“1002”)を観測誤差行列に設定する(ステップB7)。
【0107】
次いで、バイアス推定部340は、式(3)に従ってカルマンゲイン“K”を算出する(ステップB9)。そして、バイアス推定部340は、Nフレーム加速度ベクトル算出処理で算出したNフレーム加速度ベクトルの各成分それぞれに対する閾値判定を行う(ステップB11)。そして、当該成分の大きさが所定の閾値“θ”を超えている場合は(ステップB11;Yes)、当該成分を閾値“θ”として観測値“Z”を設定する(ステップB13)。また、当該成分の大きさが閾値“θ”を超えていない場合は(ステップB11;No)、当該成分をそのままとして観測値“Z”を設定する(ステップB15)。
【0108】
その後、バイアス推定部340は、式(4)及び(5)に従って状態推定値“X”及び誤差共分散“P”を補正(観測更新)する(ステップB17)。そして、バイアス推定部340は、収束条件が成立したか否かを判定する(ステップB19)。例えば、補正処理で得られた誤差共分散“P”に基づいて、バイアス値に含まれる誤差値が“0”に近い値として予め定められた所定値未満となった場合に、収束条件が成立したと判定する。
【0109】
そして、まだ収束条件が成立していないと判定した場合は(ステップB19;No)、バイアス推定部340は、式(6)及び(7)に従って状態推定値“X”及び誤差共分散“P”を予測(時刻更新)した後(ステップB21)、ステップB9に戻る。ここまでの処理は、Nフレーム加速度ベクトルのバイアス値を推定するためのカルマンフィルター処理に相当する。
【0110】
ステップB19において収束条件が成立したと判定した場合は(ステップB19;Yes)、バイアス補正部350は、状態推定値“X”に含まれる推定バイアス値を用いて、元のNフレーム加速度ベクトルを補正する(ステップB23)。すなわち、元のNフレーム加速度ベクトルの南北方向、東西方向及び高度方向の各成分に対して、バイアス値の南北方向、東西方向及び高度方向の各成分を加減算する。
【0111】
このようにしてバイアス補正を行った後、処理部300は、Nフレーム加速度ベクトル補正処理を終了する。
【0112】
図12のナビゲーション処理に戻って、Nフレーム加速度ベクトル補正処理を行った後、速度ベクトル算出部360は、Nフレーム速度ベクトル算出処理を行う(ステップA9)。具体的には、所定時間分のNフレーム補正加速度ベクトルを積分し、最新のNフレーム速度ベクトルに継ぎ足すことで、Nフレーム速度ベクトルを新たに算出する。
【0113】
次いで、位置算出部380は、位置算出処理を行う(ステップA11)。具体的には、Nフレーム速度ベクトル算出処理で算出したNフレーム速度ベクトルを用いて、Nフレームにおける自動車の位置を算定する。すなわち、前回の出力位置を、Nフレーム速度ベクトルから求められる移動方向に移動距離分だけ移動させた位置を新たな位置として算定・更新する。そして、算定位置を出力位置に決定する。
【0114】
次いで、位置算出部380は、GPS位置算出部200からGPSデータ727を取得したか否かを判定し(ステップA13)、取得したと判定した場合は(ステップA13;Yes)、出力位置のリセットタイミングであるか否かを判定する(ステップA15)。
【0115】
出力位置のリセットタイミングは、種々のタイミングを設定することができる。例えば、自動車が停止したタイミングとすることができる。自動車の停止判定は、例えば速度ベクトルの大きさに基づいて判定することが可能である。すなわち、速度ベクトルの大きさが“0”に近い値として予め定められた所定の閾値以下又は未満である場合に、自動車は停止していると判定することができる。
【0116】
また、例えば、自動車がトンネル内などのGPS測位が不可能な環境から、トンネルの外などのGPS測位が可能な環境に変化したタイミング(GPS衛星信号の捕捉ができなくなってから捕捉ができるようになったタイミング)をリセットタイミングとしてもよい。他には、例えば所定時間が経過する毎(例えば30分毎)のタイミングをリセットタイミングとしてもよい。
【0117】
出力位置のリセットタイミングであると判定した場合は(ステップA15;Yes)、位置算出部380は、リセット処理を行う(ステップA17)。具体的には、ステップA11で決定した出力位置を、GPS位置算出部200から取得したGPSデータ727のGPS算出位置でリセットする。すなわち、出力位置をGPS算出位置に変更する。
【0118】
一方、ステップA13においてGPSデータ727を取得できなかったと判定した場合(ステップA13;No)、又は、ステップA15においてリセットタイミングではないと判定した場合は(ステップA15;No)、位置算出部380は、ステップA19へと処理を移行する。
【0119】
次いで、位置算出部380は、最新の出力位置で表示部500のナビゲーション画面の表示を更新する(ステップA19)。そして、位置算出部380は、位置算出を終了するか否かを判定する(ステップA21)。例えば、操作部400を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、位置算出を終了すると判定する。
【0120】
位置算出部380は、まだ位置算出を終了しないと判定した場合は(ステップA21;No)、ステップA3に戻る。また、位置算出を終了すると判定した場合は(ステップA21;Yes)、位置算出部380は、ナビゲーション処理を終了する。
【0121】
3−5.実験結果
次に、上述した位置算出システム5において、Nフレームにおける速度ベクトルの算出及び位置の算出を行った実験結果について説明する。
【0122】
図14は、Nフレーム速度ベクトルの算出を行った結果を示す図である。図14(A)〜図14(C)は、それぞれNフレーム速度ベクトルの南北方向、東西方向及び高度方向の各成分の時間変化を示している。移動体の真の速度を実線で、従来の手法で算出した速度を破線で、本実施例の手法で算出した速度を点線でそれぞれ示している。
【0123】
この結果を見ると、従来の手法で算出した速度は、時間経過に伴って誤差が累積し、真の速度から乖離していく傾向があることがわかる。特に、高度方向の速度成分の結果が顕著であり、真の速度はほぼゼロであるのに対し、従来の手法で算出した速度は時間経過に伴って線形的に減少しており、誤差が増大していることがわかる。
【0124】
一方、本実施例の手法で算出した速度の時間変化は、真の速度の時間変化とほぼ一致していることがわかる。特に、高度方向の速度成分の結果について大きな効果が得られており、Nフレーム速度ベクトル算出の正確性が大きく向上していることがわかる。
【0125】
図15は、位置算出を行った実験結果を示す図である。上述した位置算出システム5において、自動車を予め定められた周回経路に沿って1周させ、従来の手法で算出した位置と、本実施例の手法で算定した位置とを東西南北の2次元平面にプロットした。
【0126】
図15において、横軸は東西方向、縦軸は南北方向(単位はメートル)をそれぞれ示している。東西方向0m、南北方向0mの位置がスタート地点であり、出発地点から150m程度東方向に移動した後に、北方向に向かい、大きく時計回り方向に1周周回するような経路を辿った。ゴール地点はスタート地点と同じである。また、図14と同様に、移動体の真の軌跡を実線で、従来の手法で算出した軌跡を破線で、本実施例の手法で算出した軌跡を点線でそれぞれ示している。
【0127】
この図を見ると、従来の手法で算出した位置は、時間経過とともに自動車の真の軌跡から外れていくことがわかる。これは、図14で説明したように、従来の手法では時間経過に伴ってNフレーム速度ベクトルに誤差が累積していくため、このNフレーム速度ベクトルを用いて算出される位置に誤差が重畳されるためである。
【0128】
それに対して、本実施例の手法で算出した位置は、自動車の真の軌跡にほぼ追従していることがわかる。これは、図14で説明したように、本実施例の手法ではNフレーム速度ベクトルが正しく求められるため、このNフレーム速度ベクトルを用いて算出される位置もまた正確なものとなるためである。
【0129】
4.作用効果
移動体に設置された加速度センサー2が検出したローカル座標系であるBフレームにおける移動体の加速度が、移動体に設置されたジャイロセンサー3の検出結果を用いて、絶対座標系であるNフレームにおける加速度(Nフレーム加速度)に変換される。すなわち、ジャイロセンサー3の検出結果を積分することで移動体の姿勢が算定され、当該算定姿勢を用いた座標変換処理により、Nフレーム加速度が算出される。そして、Nフレーム加速度に内在するバイアス値が、当該加速度を観測値とするカルマンフィルター処理によって推定され、推定されたバイアス値を用いてNフレーム加速度が補正される。
【0130】
カルマンフィルター処理では、Nフレーム加速度のバイアス値を状態推定値とし、Nフレーム加速度を観測値として、予測処理及び補正処理が行われる。カルマンフィルター処理では、移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、想定誤差パラメーターの一種である観測誤差の値が切り替えられる。具体的には、移動体が無加速状態の場合には、状態推定値の観測値への追従性を比較的高くするように観測誤差“R”に小さな値が設定される。それに対して、移動体が加減速状態の場合には、状態推定値の観測値への追従性を比較的劣化させるように観測誤差“R”に大きな値が設定される。
【0131】
無加速状態では、移動分の加速度がゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、観測誤差“R”を小さく設定して、状態推定値が観測値に追従し易くすることで、バイアス値の推定を適切に行うことができる。それに対して、加減速状態では、「移動分の加速度+バイアス値」が観測値となる。そのため、状態推定値を観測値に追従させるのではなく、観測誤差“R”を大きく設定して、無加速状態において推定されたバイアス値を維持させるようにバイアス値を推定する。
【0132】
加速度センサー2の検出結果及びジャイロセンサー3の検出結果には、それぞれバイアス値が内在するが、それぞれのバイアス値の大きさを特定することは困難である。そこで、ジャイロセンサー3の検出結果を用いて座標変換された後の加速度に着目し、この変換された加速度に内在するバイアス値を、変換された加速度と移動体の移動状態とを用いて推定することとした。これにより、加速度センサー2のバイアス値と、ジャイロセンサー3のバイアス値とを分離せずとも、移動体の加速度を適切に補正することが可能となる。そして、補正した加速度を用いることで、移動体の速度ベクトルの算出を適切に行うことができ、ひいては位置算出の正確性を向上させることが可能となる。
【0133】
5.変形例
5−1.バイアス値の推定処理
上述した実施形態では、カルマンフィルター処理を行ってNフレーム加速度ベクトルのバイアス値を推定する手法を例に挙げて説明したが、他の手法を用いてバイアス値を推定することも可能である。
【0134】
例えば、平均化処理を利用してバイアス値を推定することが考えられる。具体的には、移動体が無加速状態である場合に、Nフレーム加速度を入力値として平均化処理を行い、平均化された値をNフレーム加速度のバイアス値と推定してもよい。移動体が無加速状態であれば、原理的には進行方向の加速度はゼロであるため、加速度の観測値はバイアス値に相当する値である。そのため、無加速状態におけるNフレーム加速度を平均化処理することで、Nフレーム加速度のバイアス値を適切に推定することができる。
【0135】
図16は、この場合にカーナビゲーション装置6の処理部300が、図12のナビゲーション処理においてステップA7のNフレーム加速度ベクトル補正処理の代わりに実行する第2のNフレーム加速度ベクトル補正処理の流れを示すフローチャートである。
【0136】
先ず、移動状態判定部310は、移動体の移動状態を判定する(ステップD1)。そして、移動状態が無加速状態であれば(ステップD1;無加速状態)、バイアス推定部340が、無加速状態において算出されたNフレーム加速度ベクトルを平均化するNフレーム加速度平均化処理を行う(ステップD3)。
【0137】
具体的には、無加速状態と判定された後にNフレーム加速度ベクトル算出処理(図12のステップA5)で算出されたNフレーム加速度ベクトルを蓄積的に記憶しておき、この蓄積記憶されたNフレーム加速度ベクトルの平均値を算出する処理を行う。その後、バイアス推定部340は、平均化された値をバイアス値と推定し、推定バイアス値として記憶部700に記憶させる(ステップD5)。
【0138】
ステップD5の後、又は、ステップD1において移動状態が加減速状態であると判定した場合(ステップD1;加速/減速状態)、バイアス補正部350は、記憶部700に記憶されている推定バイアス値を用いて元のNフレーム加速度ベクトルを補正する(ステップD7)。そして、処理部300は、第2のNフレーム加速度ベクトル補正処理を終了する。
【0139】
なお、平均化処理に限らず、無加速状態でのNフレーム加速度を用いればバイアス値の推定を行うことができる。例えば、無加速状態におけるNフレーム加速度の過去の履歴等の情報を利用して、公知の平滑化手法に基づく平滑化処理を行ってNフレーム加速度の現在のバイアス値を推定することとしてもよい。
【0140】
5−2.Nフレーム速度ベクトルの補正
上述した位置算出システム5において、位置算出の正確性をより向上させるために、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを補正する処理を追加してもよい。
【0141】
図17は、この場合における位置算出システム5の処理部300を機能ブロックとして表した概念図である。なお、図6と同一の機能ブロックについては同一の符号を付して説明を省略する。処理部300は、移動状態判定部310と、姿勢算出部320と、座標変換部330と、バイアス推定部340と、バイアス補正部350と、速度ベクトル算出部360と、速度ベクトル補正部370と、位置算出部380とを備えて構成される。速度ベクトル算出部360の後段に速度ベクトル補正部370を設けた点が特徴である。
【0142】
姿勢算出部320は、ジャイロセンサー3の検出結果を用いて算定した移動体の姿勢を座標変換部330に出力するとともに、当該算定姿勢で示される移動体の移動方向を速度ベクトル補正部370に出力する。速度ベクトル補正部370は、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを、姿勢算出部320から入力した移動体の移動方向を用いて補正する。
【0143】
図18は、この場合に処理部300が実行する第2のナビゲーション処理の流れを示すフローチャートである。なお、図12のナビゲーション処理と同一のステップについては同一の符号を付して説明を省略する。
【0144】
ステップA9においてNフレーム速度ベクトル算出処理を行った後、処理部300は、Nフレーム速度ベクトル補正処理を実行する(ステップC10)。具体的には、速度ベクトル補正部370が、速度ベクトル算出部360により算出されたNフレーム速度ベクトルを、以下説明する原理に従って補正する。
【0145】
図19は、Nフレーム速度ベクトル補正の原理の説明図である。説明を分かり易くするため、ここではNフレームの東西方向(E軸)及び南北方向(N軸)でなる2次元平面において速度ベクトルを補正する場合について図示・説明する。図19(A)は移動体の直進時における速度ベクトルの補正の説明図であり、図19(B)は移動体の回転(旋回)時における速度ベクトルの補正の説明図である。各図において、横軸は東西方向の軸(E軸)を示しており、縦軸は南北方向の軸(N軸)を示している。
【0146】
Nフレーム加速度ベクトルの東西方向成分及び南北方向成分を積分して最後に更新したNフレーム速度ベクトルに継ぎ足すと、Nフレーム速度ベクトルの東西方向成分及び南北方向成分が得られる。図19において、E軸上に記載した矢印がNフレーム速度ベクトルの東西方向成分を示しており、N軸上に記載した矢印がNフレーム速度ベクトルの南北方向成分を示している。
【0147】
この東西方向成分及び南北方向成分を合成することで、大きさが|V|で、向きがθ’であるNフレーム速度ベクトルが得られる。すなわち、動径を|V|、偏角をθ’とする円座標でNフレーム速度ベクトルが定義される。但し、図19では、Nフレーム速度ベクトルの向きを、速度ベクトルとE軸との成す角度で表現している。上記のNフレーム速度ベクトルが、速度ベクトル算出部360により算出されるNフレーム速度ベクトルである。
【0148】
Nフレーム速度ベクトルは、Nフレーム補正加速度ベクトルを積分して継ぎ足すことで算出される。そのため、Nフレーム補正加速度ベクトルに誤差が含まれていると、求められるNフレーム速度ベクトルに誤差が累積するという問題がある。Nフレーム速度ベクトルの東西方向成分及び南北方向成分が正確に求まらなければ、図19に示すNフレーム速度ベクトルの大きさ及び向きが正確に求まらず、誤差を含み得ることになる。
【0149】
そこで、Nフレーム速度ベクトルの大きさ及び向きを次のように補正する。先ず、Nフレーム速度ベクトルの向きθ’を、姿勢算出部320により算出された移動体の移動方向θに補正する。Nフレーム速度ベクトルの向きθ’は、Nフレーム補正加速度ベクトルを積分及び継ぎ足して間接的に求められる向きであるのに対し、移動方向θは、慣性センサーの検出結果から直接的に求められる移動体の向きである。そのため、Nフレーム補正速度ベクトルの向きは、移動体の移動方向に沿った正しい向きとなる。
【0150】
次に、Nフレーム速度ベクトルの大きさ|V|を|V|×αに補正する。ここで、「α」はNフレーム速度ベクトルの補正係数である。補正係数「α」を、移動体の直進時と回転時とで可変に設定して速度ベクトルを補正する。具体的には、移動体の直進時には、図19(A)に示すように補正係数「α=1」として、Nフレーム速度ベクトルの大きさをそのままにする。この場合、Nフレーム速度ベクトルの大きさとNフレーム補正速度ベクトルの大きさとは同じとなる。
【0151】
一方、移動体の回転時には、図19(B)に示すように「α<1」として、Nフレーム速度ベクトルの大きさが小さくなるように補正する。この場合、Nフレーム補正速度ベクトルの大きさは、Nフレーム速度ベクトルの大きさよりも小さくなる。本願発明者が種々の条件で実験を行った結果、移動体の回転時には、補正係数「α」を「1」よりも僅かに小さな値とすることで、速度ベクトルの補正の効果が表れ、位置算出の正確性が向上することがわかった。但し、移動体の回転には、移動体の右折、左折、転回等、1周より短い回転が含まれる。
【0152】
なお、図19では説明を分かり易くするために2次元平面で考えたが、3次元空間に拡張することは容易である。この場合は、東西南北の2次元平面における移動体の向きに加えて、上下方向の向きを考慮して同様の補正を行えばよい。すなわち、動径を|V|、偏角を(θ’,φ’)とする球座標でNフレーム速度ベクトルを定義する。そして、Nフレーム速度ベクトルの向きを姿勢算出部320により算出された姿勢角(θ,φ)に補正し、速度ベクトルの大きさを|V|×αに補正すればよい。
【0153】
図18の第2のナビゲーション処理に戻って、Nフレーム速度ベクトル補正処理を行った後、処理部300は、位置算出処理を行う(ステップA11)。具体的には、位置算出部380が、速度ベクトル補正部370により補正されたNフレーム速度ベクトル(Nフレーム補正速度ベクトル)を用いて、最新の位置を算出・更新する。
【0154】
なお、移動体の移動方向の検出は、ジャイロセンサー3の検出結果を用いることに限られない。信頼性の高い移動方向が得られるのであれば、他の検出手段によって移動方向を検出することとしてもよい。例えば、地磁気センサー等の方位センサーによって移動方向を検出することとしてもよい。
【0155】
5−3.出力位置の決定
上述した実施例では、所定のリセットタイミングにおいて、GPSにより算出された位置で算定位置をリセットするものとして説明したが、GPSを利用して算出した位置(GPS算出位置)と、慣性航法演算により算出した位置(慣性航法演算位置)とを併用して、最終的な出力位置を算出・決定することとしてもよい。例えば、GPS算出位置と慣性航法演算位置とを平均化して最終的な出力位置としてもよい。
【0156】
5−4.カルマンフィルター処理のパラメーター値
上記の実施形態で使用したカルマンフィルター処理のパラメーター値はあくまでも一例であり、適宜設定変更可能である。特に、観測誤差“R”として設定する値は、原理で説明したように移動体の移動状態に応じて大小切り替えることとすればよく、設定する具体的な想定誤差値は適宜変更してよい。
【0157】
5−5.電子機器
上述した実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0158】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の加速度算出及び位置算出を実現することが可能である。
【0159】
5−6.衛星測位システム
上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよいことは勿論である。
【符号の説明】
【0160】
1 加速度算出システム、 2 加速度センサー、 3 ジャイロセンサー、 5 位置算出システム、 6 カーナビゲーション装置、 10 移動状態判定部、 20 姿勢算出部、 30 座標変換部、 40 バイアス推定部、 50 バイアス補正部、 100 GPSアンテナ、 200 GPS位置算出部、 300 処理部、 400 操作部、 500 表示部、 600 IMU、 700 記憶部
【特許請求の範囲】
【請求項1】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、
を含む加速度算出方法。
【請求項2】
前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、
請求項1に記載の加速度算出方法。
【請求項3】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、
請求項2に記載の加速度算出方法。
【請求項4】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、
請求項3に記載の加速度算出方法。
【請求項5】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、
請求項3に記載の加速度算出方法。
【請求項6】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、
請求項2〜5の何れか一項に記載の加速度算出方法。
【請求項7】
前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、
請求項1に記載の加速度算出方法。
【請求項8】
請求項1〜7の何れか一項に記載の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、
前記移動体の向きを検出することと、
前記移動体の向きを用いて前記速度ベクトルを補正することと、
前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項9】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、
を備えた加速度算出装置。
【請求項1】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換することと、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定することと、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出することと、
を含む加速度算出方法。
【請求項2】
前記推定することは、前記バイアス値を状態推定値とし、前記変換された加速度を観測値とするカルマンフィルター処理を行って前記バイアス値を推定することを含む、
請求項1に記載の加速度算出方法。
【請求項3】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が無加速状態であるか加減速状態であるかに応じて、前記カルマンフィルター処理で用いる想定誤差パラメーター値を切り替えることを含む、
請求項2に記載の加速度算出方法。
【請求項4】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態での値を、前記加減速状態の場合に比べて小さい値とすることを含む、
請求項3に記載の加速度算出方法。
【請求項5】
前記想定誤差パラメーター値を切り替えることは、前記無加速状態の場合に、前記状態推定値の前記観測値への追従性を前記加減速状態の場合よりも高める想定誤差パラメーター値とすることを含む、
請求項3に記載の加速度算出方法。
【請求項6】
前記カルマンフィルター処理を行うことは、前記移動体の移動状態が加減速状態の場合に前記変換された加速度が所定の限界値を超えた場合、当該限界値を前記観測値として前記カルマンフィルター処理を行うことを含む、
請求項2〜5の何れか一項に記載の加速度算出方法。
【請求項7】
前記推定することは、前記移動体が無加速状態である場合の前記変換された加速度の平均化処理を行い、前記平均化された値を前記バイアス値と推定することを含む、
請求項1に記載の加速度算出方法。
【請求項8】
請求項1〜7の何れか一項に記載の加速度算出方法によって算出された加速度を用いて前記移動体の速度ベクトルを算出することと、
前記移動体の向きを検出することと、
前記移動体の向きを用いて前記速度ベクトルを補正することと、
前記補正された速度ベクトルを用いて前記移動体の位置を算出することと、
を含む位置算出方法。
【請求項9】
移動体に設置された加速度センサーが検出した前記移動体のローカル座標系におけるローカル加速度を、前記移動体に設置されたジャイロセンサーの検出結果を用いて絶対座標系における加速度に変換する変換部と、
前記変換された加速度に内在するバイアス値を、当該加速度と前記移動体の移動状態とを用いて推定する推定部と、
前記バイアス値を用いて前記変換された加速度を補正することで、前記移動体の加速度を算出する加速度算出部と、
を備えた加速度算出装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【公開番号】特開2012−2678(P2012−2678A)
【公開日】平成24年1月5日(2012.1.5)
【国際特許分類】
【出願番号】特願2010−138114(P2010−138114)
【出願日】平成22年6月17日(2010.6.17)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成24年1月5日(2012.1.5)
【国際特許分類】
【出願日】平成22年6月17日(2010.6.17)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]