移動体用測位装置
【課題】本発明は、移動体のリアルタイムの位置を、高精度で測位する移動体用測位装置を提供することを目的とする。
【解決手段】現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置１００であって、
前記現在時刻が、ＧＰＳ演算データ更新時には、ＧＰＳ演算データ、慣性航法測位手段３０、速度検出手段４０及び誤差補正手段５０を用いて前記移動体の初期位置を算出し、該初期位置算出の際に算出された補正値を用いて、移動体モデル演算手段６０が前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時と異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、該初期位置算出の際に算出された補正値を用いて、前記移動体モデル演算手段が前記現在時刻までの移動体モデルを演算し、前記移動体を測位することを特徴とする。
【解決手段】現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置１００であって、
前記現在時刻が、ＧＰＳ演算データ更新時には、ＧＰＳ演算データ、慣性航法測位手段３０、速度検出手段４０及び誤差補正手段５０を用いて前記移動体の初期位置を算出し、該初期位置算出の際に算出された補正値を用いて、移動体モデル演算手段６０が前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時と異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、該初期位置算出の際に算出された補正値を用いて、前記移動体モデル演算手段が前記現在時刻までの移動体モデルを演算し、前記移動体を測位することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体用測位装置に関し、特に、ＧＰＳ測位手段と、慣性航法測位手段とを有する移動体用測位装置に関する。
【背景技術】
【０００２】
従来から、ＧＰＳ（Global Positioning System）受信機による搬送波位相を用いた測位と、ＩＮＳ（Internal Navigation System、慣性航法システム）による測位とを組み合わせ、ＧＰＳ受信機の位置検出間隔より短い時間内の位置データをリアルタイムで求める移動体の検出方法と装置が知られている（例えば、特許文献１参照）。
【０００３】
特許文献１に記載の移動体の検出方法と装置においては、ＧＰＳ測位値が存在する過去の点におけるＩＮＳセンサデータの積算による位置と、ＧＰＳによる位置の差を見て、ＩＮＳセンサの加速度バイアスを求めて速度を補正し、現在時刻に追い付くまで積算演算を実施し、位置演算を行う。
【０００４】
つまり、ＧＰＳの位置データが計測された時刻ｔ０では、（ｔ−３）のＧＰＳ位置データおよび（ｔ−２）のＧＰＳ位置データのジャイロ角度データを取り込み、その差が必要計測精度以内（ぼぼ直線移動）の場合に、（ｔ−３）の時刻から（ｔ−２）の時刻までのＩＮＳ位置データを積算したものを（ｔ−３）にＧＰＳ位置データに加算して（ｔ−２）の位置データとし、これと時刻（ｔ−２）のＧＰＳ位置データとの差からＩＮＳの加速度のバイアスを求め、（ｔ−３）の時刻から（ｔ−２）の時刻までのＩＮＳの速度誤差を出す。そして、このＩＮＳの速度誤差を（ｔ−２）の時刻の速度に加減算して速度補正をし、これを用いて現在時刻ｔ０までのＩＮＳによる移動距離を積算して時刻ｔ０の現在位置を得て、リアルタイムの移動体の位置を検出している。
【特許文献１】特開平８−３０４０９２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述の特許文献１に記載の構成では、直線運動時しか補正が行われないため、例えば移動体が車両の場合には、車両回転等を含む実際の走行時において、測位精度を十分に確保できないという問題があった。また、加速度バイアスの補正が、ＧＰＳ位置データ更新時のタイミングでしか行われないため、この点からも測位精度に欠けるという問題があった。
【０００６】
そこで、本発明は、ＧＰＳ測位と慣性航法測位の組み合わせを利用して、移動体のリアルタイムの位置を、移動体が旋回中であったり、ＧＰＳ位置データ更新時以外の任意のタイミングであったりしても、高精度で測位する移動体用測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、第１の発明に係る移動体用測位装置は、現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段と、前記速度を用いて移動体モデルを演算する移動体モデル演算手段とを有し、前記現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置であって、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと一致したときには、前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の前記ＧＰＳ演算データ更新時における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル算出手段が前記ＧＰＳ演算データ更新時から前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記時間遅れ分遡った時刻における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記時間遅れ分遡った時刻から前記現在時刻までの移動体モデルを演算し、前記移動体の位置を測位することを特徴とする。
【０００８】
これにより、移動体モデルを用いて現在時刻における位置演算を行うため、移動体が直線運動以外の運動を行っているときでも、測位精度を確保することができる。また、現在時刻がＧＰＳ演算データの更新タイミングに一致しなかった場合でも検出値が補正されるため、測位精度を確保することができる。
【０００９】
第２の発明は、第１の発明に係る移動体用測位装置において、
前記移動体モデル演算手段は、前記出力周期毎に、前記移動体モデルを演算して前記移動体の位置を測位することを特徴とする。
【００１０】
これにより、移動体用測位装置が、ＧＰＳ演算データ更新周期とは異なる出力周期である場合にも、常に移動体モデルを用いて高精度の位置を出力することができる。
【００１１】
第３の発明に係る移動体用測位装置は、現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段とを有し、前記現在時刻における移動体の位置を測位し、前記データ更新周期で出力する移動体用測位装置であって、
前記データ更新周期において、前記ＧＰＳ演算データが更新されたか否かを判定するＧＰＳ演算データ更新判定手段を有し、
該ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていると判定されたときには、前記ＧＰＳ演算データ更新時における前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、前記ＧＰＳ演算データ更新後から前記現在時刻までの間は、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位し、
前記ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていないと判定されたときには、前記ＧＰＳ演算データが更新されるべき時刻から前記現在時刻までの間を、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位することを特徴とする。
【００１２】
これにより、ＧＰＳ演算データの更新がない場合であっても、慣性航法用検出値及び速度を補正し、これらの補正値を用いて慣性航法と速度を組み合わせた位置演算を実行することにより、測位精度を確保することができる。
【００１３】
第４の発明は、第３の発明に係る移動体用測位装置において、
前記速度を用いて、移動体モデルを演算する移動体モデル演算手段と、
前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記移動体モデルを用いた測位演算との演算プロファイルを決定する演算プロファイル決定手段と、を更に有することを特徴とする請求項３に記載の移動体用測位装置。
【００１４】
これにより、測位精度を確保しつつ、演算負荷を低減し、用途に応じた精度で適切にリアルタイムの移動体位置を測位することができる。
【００１５】
第５の発明は、第４の発明に係る移動体用測位装置において、
前記移動体の挙動が大きいときには、前記移動体モデルを用いた測位演算以外の演算により前記演算プロファイルを決定することを特徴とする。
【００１６】
これにより、移動体の挙動が大きいときには、より変化の大きい検出値の方に精度の良い演算の比重を多くすることができ、測位が不正確になりやすい状況下でも、測位精度を確保することができる。
【００１７】
第６の発明は、第１〜５のいずれかの発明に係る移動体用測位装置において、
前記誤差補正手段は、カルマンフィルタを含むことを特徴とする。
【００１８】
これにより、カルマンフィルタを用いて種々の検出値の誤差を高精度に検出することができ、誤差を適切に補正して位置演算の精度を高めることができる。
【００１９】
第７の発明は、第１〜６のいずれかの発明に係る移動体用測位装置において、
前記移動体は、車両であることを特徴とする。
【００２０】
これにより、高精度なリアルタイム性の高い車載ナビゲーションシステムを提供することができ、車両の現在位置を高精度でユーザに知らせることができる。
【発明の効果】
【００２１】
本発明によれば、移動体の現在時刻におけるリアルタイム位置を、高精度で測位することができる。
【発明を実施するための最良の形態】
【００２２】
以下、図面を参照して、本発明を実施するための最良の形態の説明を行う。
【００２３】
図１は、本発明を適用した実施例に係る移動体用測位装置１００の全体構成を示した図である。本実施例に係る移動体用測位装置１００は、種々の移動体に適用可能であり、例えば、車両、自動二輪車、鉄道、船舶、航空機、フォークリフト、移動ロボット、人間に携帯されて移動する携帯電話等の情報端末等の移動体に適用できる。本実施例においては、移動体用測位装置１００を、車両に適用した例について説明するが、上述の移動体にも同様に適用することができる。
【００２４】
本実施例に係る移動体用測位装置１００は、ＧＰＳ測位手段１０と、慣性航法測位手段２０と、慣性航法用検出手段３０と、速度検出手段である車輪速センサ４０と、誤差補正手段であるカルマンフィルタ５０とを有する。また、本実施例に係る移動用測位装置１００は、必要に応じて、車両モデル演算手段６０と、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０と、演算プロファイル決定手段９０とを備えてよい。
【００２５】
ＧＰＳ測位手段１０は、主たる構成要素としてＧＰＳ受信機及びＧＰＳアンテナ（図示せず）を備える。ＧＰＳ測位手段１０のＧＰＳ受信機は、ＧＰＳアンテナを介して入力される衛星信号に基づいて、車両位置及び車両速度を測位する。車両位置及び車両速度は、いわゆる単独測位法で測位されてもよい。尚、ここでは、車両位置及び車両速度は、緯度、経度及び高度の座標系（ｌｌｈとＮＥＤ）で求められるものとする。また、ＧＰＳ受信機は、測位過程で車両位置及び車両速度の誤差分散を算出する。誤差分散は、緯度、経度及び高度の成分毎に導出される。誤差分散の算出方法は、任意の適切な方法が採用されてよい。ＧＰＳ測位装置１０により算出される誤差分散は、ＧＰＳ演算データのデータ更新周期毎に、誤差分散補正部７０に入力される。
【００２６】
慣性航法測位手段２０は、図１に示すように、慣性航法用検出手段３０の出力信号に基づいて、車両位置、速度や車両姿勢（方位角）を算出する。慣性航法用検出手段３０は、例えば、３軸加速度センサ及び３軸角速度センサを含むＩＭＵ（慣性測定ユニット）として構成されてよい。慣性航法による車両位置の測位方法は、多種多様でありえ、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値（移動体用測位装置１００の最終的な測位結果の前回値のフィードバック）に積算することで導出されてよい。尚、ここでは、慣性航法測位手段２０においても、車両位置及び車両速度は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。
【００２７】
慣性航法測位手段２０による慣性航法を用いた測位は、加速度センサのバイアスや、角速度センサのドリフトがそのまま積分されるため、出力に誤差を生じてしまう。これを補正するため、ＧＰＳ測位手段１０からの位置及び速度と、車輪速センサ４０からの速度とが拘束条件として利用され、ＧＰＳ測位手段１０の遮断時には、車輪速センサ４０からの速度のみが拘束条件として用いられる。
【００２８】
慣性航法測位手段２０により測位される車両位置及び車両速度（ＩＮＳ仮測位結果）は、ＧＰＳ演算データのデータ更新周期毎に、ＧＰＳ測位手段１０により測位される車両位置及び車両速度（ＧＰＳ測位結果）との差分値が取られ、当該差分値がカルマンフィルタ５０に入力される。即ち、車両位置の差及び車両速度の差が、観測量ｚとしてカルマンフィルタ５０に入力される。この際、ＧＰＳ測位結果及びＩＮＳ仮測位結果は、時間的に同期した態様で差分が取られる（例えばＧＰＳ時刻を基準として同期させて差分が取られる）。
【００２９】
同様に、慣性航法測位手段２０により測位された車両速度は、車両モデル演算手段６０から出力される推定車両速度との差分値が取られ、これもカルマンフィルタ５０に入力される。つまり、車両速度の差が、観測量ｚとしてカルマンフィルタ５０に入力される。なお、ＧＰＳ測位手段１０によるデータ更新周期は、例えば１〔秒〕、２〔秒〕といった秒単位のレベルであるが、慣性航法用検出手段３０及び車輪速センサ４０は、殆どリアルタイムに近い状態で検出が可能であるので、慣性航法測位手段２０から出力される車両速度及び車両モデル演算手段６０から出力される推定車両速度は、例えば１０〔Ｈｚ〕、２０〔Ｈｚ〕というレベルでの高速出力が可能である。
【００３０】
カルマンフィルタ５０は、ＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値を入力として、ＧＰＳ測位結果及びＩＮＳ仮測位結果のそれぞれの信頼性により確率的に最も正しい値になるように、状態量であるＩＮＳ補正値ηを推定する。ＩＮＳ補正値ηは、車両位置及び速度に関する補正値に加えて、車両姿勢、加速度センサ及び角速度センサのバイアス、ドリフトに関する補正値を含んでよい。カルマンフィルタ５０では、例えば、以下のようにして、ＩＮＳ補正値ηが推定される。状態方程式は、以下のように設定される。
η（ｔｎ）＝Ｆ・η（ｔｎ−１）＋Ｇ・ｕ（ｔｎ−１）＋Γ・ｗ（ｔｎ−１）
ここで、η（ｔｎ）は、時刻ｔ＝ｔｎでの状態変数を表わし、また、ｕ（ｔｎ−１）及びｗ（ｔｎ−１）は、それぞれ、時刻ｔ＝ｔｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔｎ）は、慣性航法測位手段２０で推定される車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）のそれぞれの誤差δｒ（ＩＮＳ）及びδｖ（ＩＮＳ）、慣性航法測位手段２０で推定される車両の姿勢の誤差δε（ＩＮＳ）、慣性航法用検出手段３０の角速度センサのバイアス誤差δｂ、慣性航法用検出手段３０の加速度センサのドリフト誤差δｄ、及び、車両のタイヤの半径誤差δｓ等を含んでよい。
【００３１】
また、観測方程式は、次のように設定される。
ｚ（ｔｎ）＝Ｈ（ｔｎ）・η（ｔｎ）＋ｖ（ｔｎ）
観測量ｚ（ｔｎ）は、時刻ｔ＝ｔｎ−１でのＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値である。Ｈ（ｔｎ）は観測行列であり、ｖ（ｔｎ）は観測雑音である。
【００３２】
カルマンフィルタ５０は、上述のように、ＧＰＳ演算データによる位置及び速度と、車輪速センサ４０からの速度の拘束条件と慣性航法との位置差、速度差から、慣性航法測位手段２０、慣性航法用検出手段３０及び車輪速センサ４０にどの程度の大きさの誤差があるのかを推定し、それぞれを補正先に戻す役割を担う誤差補正手段である。よって、図１においても、カルマンフィルタ５０の推定誤差は、慣性航法測位手段２０、慣性航法用検出手段３０及び車輪速センサ４０にフィードバックされている。
【００３３】
車両モデル演算手段６０は、車載の各種センサの出力を入力として、車両の位置を推定する車両モデルを演算する手段である。車両モデルの構築方法は、種々提案されており、如何なる方法であってもよい。例えば、車輪速に、タイヤの半径変動を乗じて積算すれば車両位置が算出されるが、これに方位角の要素を考慮するようにした関係を示す数式であってもよい。図示の車両モデル演算手段６０は、車輪速センサ４０の出力を入力として、位置が既知の基点からの車両の移動量を推定して車両の位置を推定するモデルである。尚、ここでは、車両モデル６０においても、推定車両位置は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。車両の移動量が、例えばWGS84に基づく地球固定座標系等のような他の座標系で算出されている場合、当該移動量を座標変換し、当該座標変換した移動量を、経度及び高度の座標系の基点に積算することで、経度及び高度の座標系の推定車両位置を算出することができる。車両モデル演算手段６０により演算された車両モデルから推定された車両位置は、誤差分散補正部７０に入力される。
【００３４】
誤差分散補正部７０は、ＧＰＳ測位手段１０から入力される誤差分散を補正する手段である。誤差分散補正部７０は、補正した誤差分散をカルマンフィルタ５０に入力する。カルマンフィルタ５０では、誤差分散補正部７０から入力される誤差分散が観測ノイズの分散として用いられて、状態量（ＩＮＳ補正値η）が推定される。ＧＰＳ測位手段１０からのＧＰＳ演算データも補正する場合には、このような誤差分散補正部７０を設けるようにしてもよい。
【００３５】
ＧＰＳ演算データ更新判定手段８０は、ＧＰＳ測位手段１０のＧＰＳ演算データの更新が行われたか否かを判定するための手段である。ＧＰＳ測位手段１０のＧＰＳ受信機は、障害等によりＧＰＳ衛星からの信号を受信することができず、ＧＰＳ遮断が発生する場合がある。よって、ＧＰＳ演算データ更新判定手段８０は、そのようなＧＰＳ遮断が発生していないか否かを、データ更新周期におけるＧＰＳ衛星からの信号の有無により、ＧＰＳ演算データの更新がなされているか否かの判定を行う。
【００３６】
演算プロファイル決定手段９０は、ＧＰＳ測位手段１０からの車両位置及び車両速度のＧＰＳ演算データ、慣性航法測位手段２０による車両位置及び車両速度の推定、車両モデル演算手段６０による車両モデルを用いた車両位置及び車両速度の推定を、どのように組み合わせて移動体の位置を測位するかの演算割合を定める手段である。移動体の位置の測位精度は、用途に応じて要求度合いが異なるので、要求される測位精度と演算負荷を考慮して適切な演算プロファイルを定めるようにしてよく、演算プロファイル決定手段９０は、そのような適切な演算プロファイルを決定する機能を有する。演算プロファイル決定手段９０は、演算プロファイルを決定するための演算処理を行うので、ＣＰＵ（Central Processing Unit、中央処理装置）を備えたマイクロコンピュータや、特定の演算処理用のＡＳＩＣ（Application Specific Integrated Circuit）等として構成されてよい。
【００３７】
なお、以後、ＧＰＳ測位、慣性航法測位及び速度を用いた測位の協調アルゴリズムを、ＧＰＳ／ＩＮＳ／車輪速と表し、慣性航法測位及び速度を用いた測位の協調アルゴリズムを、ＩＮＳ／車輪速と表すものとする。
【００３８】
図２は、本実施例に係る移動体用測位装置１００により実現される、カルマンフィルタ５０による誤差補正を適用したＧＰＳ／ＩＮＳ／車輪速及びＩＮＳ／車輪速の協調アルゴリズムの一例を示すフローチャートである。
【００３９】
ステップ１００では、慣性航法用検出手段３０の出力値（例えば３軸方向の加速度、及び、３軸まわりの角速度）がサンプリングされる。慣性航法用検出手段３０の出力値は、後述のステップ１５０のカルマンフィルタ５０により推定されるＩＮＳ補正値ηに基づいて補正を受けてよい。即ち、慣性航法用検出手段３０の出力値のバイアスやドリフトの誤差が補正されてよい。
【００４０】
ステップ１１０では、慣性航法測位手段２０において、上記ステップ１００で得られる慣性航法用検出手段３０の出力値に基づいて、車両推定位置及び車両推定速度、姿勢等が導出される。
【００４１】
ステップ１２０では、カルマンフィルタ５０においてカルマンフィルタ５０の時間更新が実行される。カルマンフィルタ５０の時間更新は、例えば以下のように表される。
η（ｔｎ）（−）＝η（ｔｎ−１）（＋）＋ｕ（ｔｎ−１）
Ｐ（ｔｎ）（−）＝Ｆ・Ｐ（ｔｎ−１）（＋）・ＦＴ＋Γ・Ｑ（ｔｎ−１）・ΓＴ
尚、Ｐは、予測・推定誤差の共分散行列であり、Ｑは、外乱ｗの共分散行列（正定値対称行列）である。尚、符号（＋）及び（−）は更新前後を表す。
【００４２】
ステップ１３０では、今回周期（ｔｎ）の観測量ｚ（ｔｎ）に基づいて観測行列Ｈ（ｔｎ）が演算される。
【００４３】
ステップ１４０では、カルマンゲインＫｋが以下のように演算される。
Ｋ（ｔｎ）＝Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）・（Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）＋Ｒ（ｔｎ））−１
ここで、Ｒ（ｔｎ）は、観測ノイズの分散行列である。Ｒ（ｔｎ）は、誤差分散補正部１４０で生成され、次の通りである。
Ｒ（ｔｎ）＝Ｍｋ（ｔｎ）・Ｗｋ（ｔｎ）
ここで、Ｍｋ（ｔｎ）は、非対角成分がゼロの分散更新用行列であり、デフォルトは単位行列である。分散更新用行列Ｍｋ（ｔｎ）の対角成分は、分散更新用ゲインである。Ｗｋ（ｔｎ）は、非対角成分がゼロの行列であり、対角成分には、ＧＰＳ測位手段１０により算出される誤差分散の各成分（緯度成分、経度成分、高度成分）が代入される。
【００４４】
ステップ１５０では、上記のステップ１４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、状態量η（ｔｎ）が以下のように算出される。
η（ｔｎ）（＋）＝η（ｔｎ）（−）＋Ｋ（ｔｎ）・（ｚ（ｔｎ）−Ｈ（ｔｎ）・η（ｔｎ）（−））
ステップ１６０では、上記のステップ１５０で導出された状態量η（ｔｎ）に基づいて、誤差補正が実行される。即ち、慣性航法用検出手段３０の出力値のバイアス及びドリフト補正が実行され、慣性航法測位手段２０において車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）、姿勢の補正が実行される。その結果、誤差補正後の車両推定位置等が最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）として得られる。尚、上記のステップ１００乃至１７０の処理は、その周期の慣性航法用検出手段３０の出力値に基づいて、例えば状態量η（ｔｎ）が収束するまで繰り返し実行されてもよい。また、車輪速センサ４０のスケーリングファクタ（車両モデル）についても、状態量η（ｔｎ）に含まれるタイヤ半径誤差δｓの推定値に基づいて、補正されてよい。
【００４５】
つまり、詳細には後述するが、誤差補正手段であるカルマンフィルタ５０により、ＧＰＳ演算データがあるときは、ＧＰＳ／ＩＮＳ／車輪速協調測位の結果が誤差補正後の測位結果として得られ、ＧＰＳ演算データが無いときには、ＩＮＳ／車輪速協調測位の結果が、誤差補正後の測位結果として得られる。車輪速に関しても、誤差補正後の値を得ることができ、これを用いて車両モデルを演算することが可能となる。
【００４６】
ステップ１７０では、上記のステップ１４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、共分散行列Ｐが以下のように更新される。
Ｐ（ｔｎ）（＋）＝Ｐ（ｔｎ）（−）−Ｋ（ｔｎ）・Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）
以下、図１及び図２に示した本実施例に係る移動体用測位装置１００の具体的な演算内容を示す実施例について説明する。
【実施例１】
【００４７】
図３は、実施例１に係る移動体用測位装置１００によるＧＰＳ／ＩＮＳ協調測位の内容を説明するための図である。図３は、実施例１に係るＧＰＳ／ＩＮＳ協調測位の演算内容を、時系列的に示している。実施例１に係る移動体用測位装置１００においては、移動体モデルを用いた移動体の現在位置の演算を行う。移動体が車両である場合を示した図１の全体構成図においては、車両モデル演算手段６０を用いて車両の測位を行うので、車両モデル演算手段６０を備える。一方、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０と、演算プロファイル決定手段９０は、必ずしも備えられなくてもよい実施形態である。
【００４８】
図３（ａ）は、ＧＰＳ演算データのデータ更新周期を示した図である。図３（ａ）において、ＧＰＳ演算データは、データ更新周期Ｔ＝１．０〔ｓ〕で演算データの更新がなされている。そして、ＧＰＳ演算データは、リアルタイムの時刻よりも、２〔ｓ〕遅れて移動体用測位装置１００から出力される関係があるとする。つまり、ＧＰＳ測位手段１０で受信し、演算して出力したＧＰＳ演算データは、そのまま出力すると、２秒の時間遅れを有する２秒前の車両位置が出力されることになる。
【００４９】
図３（ｂ）は、車輪速／ＩＮＳ協調測位演算の時系列を示した図である。図３（ｂ）において、車輪速センサ４０及び慣性航法測位手段２０による慣性航法測位に用いられる慣性航法用検出手段３０は、加速度センサ、角加速度センサ等であり、ほぼリアルタイムに検出値を取得できる。よって、車輪速／ＩＮＳ協調測位演算は、例えば１０〔Ｈｚ〕の周期で演算を行うことができる。よって、図３（ｂ）においては、Ｔ＝１．０〔ｓ〕からＴ＝３．０〔ｓ〕の間には、２０個の検出値の存在が示されている。
【００５０】
図３（ｃ）は、Ｔ＝１．０〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｃ）において、Ｔ＝１．０〔ｓ〕の時刻を初期値として、車両モデルの演算が行われているが、Ｔ＝１．０〔ｓ〕の時刻においては、図３（ａ）に示すように、ＧＰＳ演算データが存在するので、ＧＰＳ／ＩＮＳ／車輪速協調測位演算のカルマンフィルタ５０の結果を用いて、車両の初期位置を算出する。同様に、ＧＰＳ／ＩＮＳ／車輪速協調測位演算のカルマンフィルタ５０の結果を用いて、慣性航法用検出値である加速度、角速度についてバイアス、ドリフト補正を行う。また、車輪速についても補正を行う。そして、これらの補正されたセンサ値を用いて車両モデルによる位置演算を繰り返して積算し、現在時刻のＴ＝３．０〔ｓ〕に追い付くまで車両モデルによる位置演算を実行する。これにより、ＧＰＳ演算データが存在するときには、ＧＰＳ／ＩＮＳ／車輪速の初期位置を用いて、車両モデルにより現在時刻の車両位置を演算することができる。
【００５１】
図３（ｄ）は、Ｔ＝１．１〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｄ）において、Ｔ＝１．１〔ｓ〕の時刻を初期値として、車両モデルの演算が行われているが、Ｔ＝１．１〔ｓ〕では、ＧＰＳ演算データは存在せず、車輪速／ＩＮＳ協調測位演算に用いられるデータのみが存在する。よって、この場合には、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、車両の初期位置を算出する。同様に、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、加速度、角速度、車輪速等のセンサ値の補正を行い、バイアス、ドリフト補正を行う。そして、現在時刻Ｔ＝３．１〔ｓ〕に追い付くためのＴ＝１．２〜３．１〔ｓ〕までの車両モデルの演算を、Ｔ＝１．１〔ｓ〕の時刻の車輪速／ＩＮＳのカルマンフィルタ５０の結果を用い、これに基づいて車両モデルによる位置演算を実施する。これにより、ＧＰＳ演算データが存在しないときであっても、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、車両モデルの演算により現在時刻の車両位置を演算することができる。
【００５２】
図４は、ＧＰＳ測位手段１０により取得されるＧＰＳ演算データが、実際の現在時刻よりも２秒の遅れを持つ場合の測位演算ルーチンを説明するための図である。ＧＰＳ演算データは、Ｔ＝１．０〔ｓ〕、２．０〔ｓ〕・・・の１秒周期でデータ更新されている。また、移動体用測位装置１００は、Ｔ＝１．１〔ｓ〕、１．２〔ｓ〕、１．３〔ｓ〕・・・で位置データが出力され、出力周期は０．１秒である場合を考える。
【００５３】
図４（ａ）は、現在時刻がＴ＝３．０〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．０〔ｓ〕のときには、ＧＰＳ演算データは、２秒前のＴ＝１．０〔ｓ〕の測位データを更新しているタイミングであり、ＧＰＳ更新データが存在するタイミングである。つまり、初期時刻をＴｐａｓｓとし、ＧＰＳ演算データの更新時刻（以下、「ＧＰＳ時刻」という。）をＴｇｐｓとすると、Ｔｐａｓｓ＝Ｔｇｐｓとなるタイミングである。従って、この場合には、ＧＰＳ／車輪速／ＩＮＳ協調測位演算の測位データを初期値として用いるようにする。ここで、データ更新されたＧＰＳ演算データをそのまま出力すると、２秒前の車両位置を出力してしまうことになるので、現在時刻までのＴ＝１．１〜３．０〔ｓ〕の時間の車両の位置変化を演算する。このとき、車両モデル演算手段６０により、車両モデルを用いて現在時刻Ｔ＝３．０〔ｓ〕における車両位置を算出するための追い付き演算を行うので、車両モデルの演算に必要な車輪速センサ値を含む加速度、角速度等を用いて、Ｔ＝１．１〜３．０〔ｓ〕までの積算を行う。また、このとき、Ｔ＝１．０〔ｓ〕における初期値Ｔｐａｓｓを用いて、カルマンフィルタ５０を用いて、加速度センサに対応するバイアス補正、角速度センサに対応するドリフト補正、車輪速センサ４０に対応する車輪速補正や、これらのセンサの取り付け面補正等が行われる。
【００５４】
つまり、初期時刻Ｔｐａｓｓは、ＧＰＳ時刻Ｔｇｐｓと一致しており、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の演算データが存在するので、これに対して誤差補正手段であるカルマンフィルタ５０を用いて誤差補正値を算出し、これを用いてＴ＝１．１〜３．０〔ｓ〕の間の車両モデルによる位置演算を実行し、現在時刻Ｔ＝３．０〔ｓ〕における車両位置を測位する。
【００５５】
図４（ｂ）は、現在時刻がＴ＝３．１〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．１〔ｓ〕のときには、２秒前はＴ＝１．１〔ｓ〕であり、ＧＰＳ演算データの更新タイミングとは異なるタイミングである。このときの初期時刻Ｔｐａｓｓは、図４（ａ）で示した前回の初期時刻Ｔｐａｓｓに１周期分の出力周期ＳＴを加えた時刻であり、初期時刻Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴ＝Ｔｇｐｓ＋ＳＴ＝１．０〔ｓ〕＋０．１〔ｓ〕＝１．１〔ｓ〕である。このとき、位置データは、車輪速／ＩＮＳ協調測位演算による測位データしか存在しないので、これを初期値（初期位置）として用いる。また、時刻Ｔ＝１．２〔ｓ〕から現在時刻Ｔ＝３．１〔ｓ〕までの演算は、図４（ａ）と同様に、車両モデルを演算するのに必要な加速度、角速度、車輪速センサ値を用いて行われる。このとき、カルマンフィルタ５０によるバイアス、ドリフト、車輪速、取り付け面補正等は、Ｔ＝１．１〔ｓ〕における車輪速／ＩＮＳ協調測位演算による測位データを用いて行われる。そして、ここで算出した補正値を用いて、Ｔ＝１．２〜３．１〔ｓ〕の車両モデルによる位置演算が実行され、現在時刻Ｔ＝３．１〔ｓ〕の車両位置が測位される。
【００５６】
図４（ｃ）は、現在時刻Ｔ＝３．９〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．９のときには、図４（ｂ）と同様に、初期時刻Ｔｐａｓｓは、前回の出力周期における初期時刻Ｔｐａｓｓに、出力周期ＳＴ分を加えた時刻となり、Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴである。図４（ｂ）において、初期値に車輪速／ＩＮＳ協調測位演算による測位データが用いられた後、次の更新タイミングＴ＝２．０〔ｓ〕でＧＰＳ演算データが更新されるまでは、ＧＰＳ演算データの更新データは存在せず、車輪速／ＩＮＳ協調測位演算による測位を実行し続けることになる。よって、Ｔ＝３．９〔ｓ〕においても、初期時刻Ｔｐａｓｓ＝１．９〔ｓ〕における車輪速／ＩＮＳ協調測位演算による測位データを初期値（初期位置）とし、これを用いてカルマンフィルタ５０によるバイアス、ドリフト、車輪速、取り付け面の補正値を算出する。そして、この補正値を用いて、加速度、角速度、車輪速センサ値を補正し、Ｔ＝２．０〜３．９〔ｓ〕までの車両モデルを積算して、現在時刻Ｔ＝３．９〔ｓ〕における車両位置を演算し、出力する。
【００５７】
図４（ｂ）、（ｃ）の測位演算処理は、図３に示したように、ＧＰＳ演算データの更新周期が１〔Ｈｚ〕、つまりＴ＝１．０〔ｓ〕とすると、現在時刻Ｔ＝４．０〔ｓ〕、初期時刻Ｔｐａｓｓ＝２．０〔ｓ〕となったときに、Ｔｐａｓｓ＝Ｔｇｐｓとなり、図４（ａ）で説明した測位演算処理が実行される。このような一連の測位演算ルーチンの実行により、ＧＰＳ演算データの更新タイミングで取得されるデータが過去の車両位置を示す測位データであっても、車両モデルを用いて位置演算を実行することにより、現在時刻における車両位置を高精度で測位することができる。
【００５８】
図５は、実施例１に係る移動体用測位装置１００の測位演算処理フローを示した処理フロー図である。
【００５９】
ステップ２００では、ＧＰＳ測位手段１０において、ＧＰＳ演算データが更新されたか否かが判定される。これは、現在時刻において、ＧＰＳ演算データが更新されたか否かを判定すればよい。また、ＧＰＳ演算データが更新されたか否かは、例えば、ＧＰＳ演算データ更新判定手段８０により判定されてもよい。
【００６０】
ステップ２００において、ＧＰＳ演算データが更新されたと判定されたときにはステップ２１０に進み、ＧＰＳ演算データが更新されていないと判定されたときには、ステップ２２０に進む。
【００６１】
ステップ２１０では、初期時刻ＴｐａｓｓをＧＰＳ時刻Ｔｇｐｓとし、ＧＰＳの時間遅れを考慮し、過去のＧＰＳ時刻に戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行し、ＧＰＳ／ＩＮＳ／車輪速協調測位演算による測位データを算出する。測位データには、車両の位置だけでなく、車両速度や車両姿勢のデータも含まれてよい。また、このとき、カルマンフィルタ５０を用いて、初期時刻Ｔｐａｓｓにおける補正値を算出しておく。
【００６２】
また、ステップ２２０では、現在時刻からＧＰＳ演算データのデータ更新周期分遡った初期時点Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴにおける、ＩＮＳ／車輪速協調測位演算による測位データを算出する。ステップ２１０と同様に、測位データには、車両の位置だけでなく、車両速度や車両姿勢のデータも含まれてよい。また、このとき、カルマンフィルタ５０を用いて、初期時刻Ｔｐａｓｓにおける補正値を算出しておく。
【００６３】
ステップ２３０では、次回の初期時刻Ｔｐａｓｓでのカルマンフィルタ５０を用いた演算のために、ステップ２１０又はステップ２３０で算出した車両位置、車両速度、車両姿勢等の測位データ履歴を記憶して残しておく。記憶は、通常のＲＡＭ（Random Access Memory）等に用いられるメモリが適用されてよい。
【００６４】
ステップ２４０では、ステップ２１０又はステップ２２０で求めた車両位置を初期値（初期位置）と定める。つまり、初期時刻ＴｐａｓｓにおけるＧＰＳ／ＩＮＳ／車輪速協調測位演算又はＩＮＳ／車輪速協調測位演算により求めた車両位置を初期値とする。
【００６５】
ステップ２５０では、初期時刻Ｔｐａｓｓから現在時刻までの車両モデルの演算に必要な車両センサ値を、ステップ２１０又はステップ２２０で求めた初期時刻Ｔｐａｓｓ時の補正値を用いて、誤差補正を行う。車両センサ値は、加速度、角速度、車輪速度等の検出値が用いられてよい。
【００６６】
ステップ２６０では、ステップ２４０で決定した初期値を基点として、ステップ２５０で算出した初期時刻Ｔｐａｓｓから現在時刻までの補正値を用いて、初期時刻Ｔｐａｓｓから現在時刻までの、車両モデルによる積算を順次実行する。車両モデルによる位置演算は、車両モデル演算手段６０により行われてよい。そして、現在時刻における車両位置を算出し、出力して処理フローを終了する。
【００６７】
なお、ステップ２６０において実行される追い付き演算は、移動体用測位装置１００の出力周期に応じて実施される。例えば、出力周期がＩＮＳ／車輪速協調測位演算の測位データ算出周期よりも大きい場合には、データ出力を、出力周期に合わせて減少させるようにしてもよい。
【００６８】
図５に示した処理フローは、実施例１に係る移動体用測位装置１００の出力周期毎に演算され、繰り返される。このように、実施例１に係る移動体用測位装置１００の演算処理内容によれば、車両モデルを用いて車両の現在時刻における位置を演算算出するので、リアルタイム性を実現できるとともに、高精度な測位を行うことができる。
【実施例２】
【００６９】
実施例２に係る移動体用測位装置１００は、図１の全体構成図において、ＧＰＳ演算データ更新判定手段８０が必要とされ、車両モデル演算手段６０、誤差分散補正部７０及び演算プロファイル決定部９０は必ずしも必要とされない実施形態である。
【００７０】
図６は、実施例２に係る移動体用測位装置１００の演算処理内容を説明するための図である。図６において、ＧＰＳ演算データ、車輪速／ＩＮＳ協調測位演算、現在時刻との関係を表す時系列が示されている。実施例２においては、移動体用測位装置１００の測位出力周期がＧＰＳ測位手段１０のＧＰＳ演算データのデータ更新周期と一致する場合の演算処理の一例について説明する。図６において、ＧＰＳ演算データのデータ更新周期及び移動体用測位装置１００の測位出力周期はともにＴ＝１．０〔ｓ〕であり、Ｔ＝１．０〔ｓ〕、Ｔ＝２．０〔ｓ〕、Ｔ＝３．０〔ｓ〕・・・にデータ更新が行われる例が示されている。
【００７１】
Ｔ＝１．０〔ｓ〕のときには、ＧＰＳ演算データが更新され、ＧＰＳ演算データが存在する状態である。このとき、現在時刻は、Ｔ＝２．０〔ｓ〕である。Ｔ＝１．０〔ｓ〕において、車輪速／ＩＮＳ協調測位演算の測位データも存在するが、上述のように、ＧＰＳ演算データが存在するので、Ｔ＝１．０〔ｓ〕においては、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。このとき、カルマンフィルタ５０を用いて誤差補正を行うようにして、測位精度を高く保つ。
【００７２】
Ｔ＝１．１〜２．０〔ｓ〕の時間においては、ＧＰＳ演算データのデータ更新はなされず、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を行うことはできない。しかしながら、車輪速／ＩＮＳ協調測位演算の演算データは算出できるので、車輪速／ＩＮＳ協調測位演算による測位を行うことができる。そして、この場合にも、カルマンフィルタ５０による誤差補正を行うことができる。つまり、Ｔ＝１．１〜２．０〔ｓ〕の時間においては、車輪速／ＩＮＳ協調測位演算を順次実行するとともに、カルマンフィルタ５０による補正を各センサのサンプリング周期毎に行い、測位精度を高めることができる。実施例１においては、移動体用測位装置１００の出力周期が短かったために、カルマンフィルタ５０による検出値補正は、初期値に対してしか実行できなかったが、実施例２においては、過去のＧＰＳ時間Ｔｇｐｓから現在時刻まで一気にまとめて計算することができ、その時に一気にまとめてカルマンフィルタ５０を用いて演算を行うことができるので、測位精度を高く保つことができる。そして、現在時刻Ｔ＝２．０〔ｓ〕の測位演算結果を、Ｔ＝１．０〔ｓ〕のＧＰＳ演算データを用いてリアルタイムに出力することができる。
【００７３】
このように、初期時刻にＧＰＳ演算データが存在する場合には、初期時刻ＴｐａｓｓのみＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行し、それ以降から現在時刻までは、車輪速／ＩＮＳ協調測位演算を実行し、カルマンフィルタ５０を用いて一気に現在位置の測位を行うことができる。
【００７４】
また、Ｔ＝２．０〔ｓ〕は、ＧＰＳ演算データのデータ更新周期のタイミングであり、ＧＰＳ演算データが本来的にはデータ更新される筈であるが、図６においては、Ｔ＝２．０〔ｓ〕でＧＰＳ演算データが更新されていない例が示されている。例えば、車両がＧＰＳ衛星からの信号を受信することができない場所にあり、ＧＰＳ遮断となる場合がある。そのような場合には、Ｔ＝２．０〔ｓ〕の時刻でＧＰＳ／ＩＮＳ／車輪速協調測位演算を行うことはできないが、車輪速／ＩＮＳ協調測位演算のための車両センサ値は存在するので、車輪速／ＩＮＳ協調測位演算を行うことができる。この場合には、初期時刻ＴｐａｓｓのＴ＝２．０〔ｓ〕から現在時刻Ｔ＝３．０〔ｓ〕まで、車輪速／ＩＮＳ協調測位演算により、カルマンフィルタ５０により誤差補正された補正値を用いて、一気に測位演算を行うことができる。初期値に、ＧＰＳ／ＩＮＳ／ＩＮＳ協調測位演算を用いることはできないが、追い付き演算で行われる車輪速／ＩＮＳ協調測位演算の総てを、補正された車両センサ値を用いて行うことができるので、測位精度を高く保つことができる。
【００７５】
このように、ＧＰＳ遮断が発生しており、ＧＰＳ演算データがデータ更新周期において更新されなかった場合であっても、車輪速／ＩＮＳ協調測位演算を、カルマンフィルタ５０を用いて実行することにより、現在時刻における位置を高精度に測位することができる。
【００７６】
なお、ＧＰＳ演算データがデータ更新周期において更新されたか否かは、ＧＰＳ演算データ更新判定手段９０により判定してよい。
【００７７】
図７は、実施例２に係る移動体用測位装置１００の演算処理内容を示した処理フロー図である。
【００７８】
ステップ３００では、ＧＰＳ測位手段１０のＧＰＳ演算データが更新したか否かが判定される。ＧＰＳ演算データが更新したか否かは、ＧＰＳ演算データ更新判定手段９０により行われてよい。
【００７９】
ステップ３００において、ＧＰＳ演算データがデータ更新していると判定された場合にはステップ３１０に進み、ＧＰＳ演算データがデータ更新していないと判定された場合にはステップ３２０に進む。
【００８０】
ステップ３１０では、過去のＧＰＳ時刻Ｔｇｐｓに戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて実行する。過去のＧＰＳ時刻Ｔｇｐｓは、例えば、図６においては、Ｔ＝１．０〔ｓ〕の時刻に相当する。その後、現在時刻まで、ＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて順次実行する。ＩＮＳ／車輪速協調測位演算は、各車両センサのサンプリング周期毎に順次行い、現在時刻まで積算し、現在時刻における車両位置を求める。そして、現在時刻における測位位置を出力し、処理フローを終了する。
【００８１】
一方、ステップ３２０では、ＧＰＳ遮断が発生していても、ＧＰＳ測位データを受信した場合のＧＰＳ時刻から、現在時刻までのＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて順次実行する。この場合、例えば、図６においては、ＧＰＳ時刻はＴ＝２．０〔ｓ〕に相当し、これが初期時刻Ｔｐａｓｓになる。ＩＮＳ／車輪速協調測位演算は、初期時刻Ｔｐａｓｓも含めて各車両センサのサンプリング周期毎に順次行い、現在時刻まで積算し、現在時刻における車両位置を求める。そして、現在時刻における測位位置を出力し、処理フローを終了する。
【００８２】
なお、図７に示した処理フローは、移動体用測位装置１００の測位出力周期毎に実行され、これはＧＰＳ測位手段１０のデータ更新周期と一致する。よって、例えば、ＧＰＳ測位手段１０のＧＰＳ演算データの更新周期が１秒の場合は、１秒毎に図７の処理フローが実行され、繰り返される。
【００８３】
このように、実施例２に係る移動用測位装置１００によれば、カルマンフィルタ５０を最大限活用することにより、高精度に現在時刻における車両位置を測位することができる。
【実施例３】
【００８４】
実施例３に係る移動体用測位装置１００は、実施例１と実施例２の混合的な演算処理を行う。実施例３に係る移動体用測位装置１００は、図１における全体構成図においては、車両モデル演算手段６０と、演算プロファイル決定手段９０とを備え、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０は、必ずしも必要とされない実施形態である。
【００８５】
実施例２においては、リアルタイム性の確保のために、現在時刻まで追い付くための演算処理でカルマンフィルタ５０を連続的に用いるため、演算負荷が増大する場合がある。このような場合を考慮し、実施例３においては、実施例２と同様にＧＰＳ演算データのデータ更新周期と移動体用測位装置１００の出力周期が一致する場合において、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算、車両モデルによる測位演算を混合状態で実行し、精度を確保しつつ演算負荷を低減させる制御の例について説明する。
【００８６】
図８は、実施例３に係る移動体用測位装置１００の演算プロファイル決定手段９０で実行される演算プロファイル決定のための演算処理内容について説明するための図である。図８において、横軸は、リアルタイム性確保のための追い付き演算の結果算出に要する時間を示し、縦軸は測位誤差の大きさを示している。また、横軸に平行に引かれた線ＰＭは、許容できる測位誤差の大きさを示している。
【００８７】
図８において、例えば、追い付き演算が２０回必要な場合について考える。追い付き演算には演算時間が必要で、この時間が長すぎると、リアルタイム性を損なってゆくことになる。特に、車速が大きい場合には、単位時間に車両が進む距離も大きいので、例えば、追い付き演算に要する時間が長くなると、実際位置との誤差は大きくなる。よって、許容測位誤差と車速に応じて、追い付き演算に割り当て可能な時間、つまり追い付き演算に費やせる許容時間は変化することになる。
【００８８】
図８において、許容時間をＴｐ秒とすると、車速が高くなったときには、Ｔｐ秒は左に移動して許容時間は小さくなり、車速が低くなったときには、Ｔｐ秒は右に移動して許容時間は大きくなることが分かる。ここで、更に、許容時間Ｔｐ秒を、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデルによる測位演算を演算回数で振り分けると、（１）、（２）式のように表すことができる。
【００８９】
【数１】
（１）、（２）式は、演算回数を２０回としたときの、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデルによる測位演算の振り分け条件式を示している。ａは、ＩＮＳ／車輪速協調測位演算の１回分の演算時間であり、ｘは、ＩＮＳ／車輪速協調測位演算の演算回数である。ｂは、車両モデルの１回分の演算時間であり、ｙは、車両モデルによる測位演算の演算回数である。ｃは、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の１回分の演算時間であり、ｚは、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の演算回数である。
【００９０】
（２）式より、ｘ、ｙ、ｚの演算合計回数は２０回であり、これを割り振って（１）式を満たすように演算プロファイルを決定する。ＧＰＳ／ＩＮＳ／車輪速協調測位演算は、現在時刻がＧＰＳ演算データの更新タイミングであり、かつＧＰＳ遮断が発生していないときには実行でき、それ以外の場合は実行できないので、ｚ＝１、０のいずれかである。ＩＮＳ／車輪速協調測位演算は、カルマンフィルタ５０により検出値補正を行うので、精度は高いが、処理負荷が大きく、演算時間も大きくなる場合が多い。一方、車両モデルによる測位演算は、個々の演算毎に個別の補正までは行わないので、ＩＮＳ／車輪速協調測位演算よりも精度は低くなる場合が多いが、処理負荷は小さくなり、演算時間が小さくなる場合が多い。これは、設定された車両モデルにもよるが、そのような車両モデルを設定することは十分可能であり、また一般的である。よって、一般的には、ｂ＜ａとなる場合が多い。このような条件を考慮し、なるべく高精度の測位を実現し、かつ許容時間Ｔｐ内とするには、（１）を満たす範囲で、ｘを最大とするような設定とすればよい。これにより、許容時間Ｔｐの範囲内で、最も高精度で測位を行うことができる。
【００９１】
また、ｘが定まったときに、２０回の演算の中でＩＮＳ／車輪速協調測位演算をいつ行うかの割り振りがあるが、ｘを、車両センサ値の変化が大きいタイミングになるべく多く割り当てるようにしてもよい。つまり、加速度、角加速度、車輪速度等の車両センサ値の変化が大きい状態は、車両の挙動が大きい部分であり、車両モデルでは誤差が蓄積し易い部分である。よって、そのようなタイミングでは、なるべく精度の高いＩＮＳ／車輪速協調測位演算を実行するようにし、測位精度を高く保つようにすることが好ましい。
【００９２】
また、車両センサ値は、上述の加速度、角加速度、車輪速度の他、ステアリング角、ブレーキ油圧、アクセルペダル等のセンサ値を含んでよい。車両の挙動が大きい場合には、旋回が大きい場合や、急な加減速が行われた場合が含まれるので、ステアリング角や、ブレーキ油圧、アクセルペダル等によっても、車両の挙動が大きい状態であることを検出できるからである。
【００９３】
なお、図８及び（１）、（２）式で示した演算は、演算プロファイル決定手段９０で行うようにしてよい。
【００９４】
次に、図９を用いて、演算プロファイル決定手段９０で実行される、図８とは異なる演算プロファイル決定の演算処理内容について説明する。図９は、図８とは異なる演算プロファイル決定の演算処理内容の説明図である。
【００９５】
図９（ａ）は、ＧＰＳ演算データが、２秒前の車両位置を示している場合であって、現在時刻Ｔ＝３．０〔ｓ〕における車両位置を出力する場合の演算処理の時系列を示した図である。図９（ａ）において、Ｔ＝１．０〔ｓ〕においては、ＧＰＳ演算データが更新されており、ＧＰＳ演算データが存在する。よって、初期時間Ｔｐａｓｓ＝１．０〔ｓ〕においては、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。その後、図８で説明した演算プロファイルに従い、現在時刻Ｔ＝３．０〔ｓ〕まで、カルマンフィルタ５０を用いた車輪速／ＩＮＳ協調測位演算と、車両モデルを用いた測位演算とが混合で一気にまとめて計算され、出力される。このとき、図９（ａ）に示すように、追い付き演算に要する許容時間Ｔｐ内であれば、若干の遅れは許容される。
【００９６】
図９（ｂ）は、現在時刻Ｔ＝４．０〔ｓ〕における車両位置を出力する場合の演算処理の時系列を示した図である。図９（ｂ）において、ＧＰＳ演算データのデータ更新周期は１〔Ｈｚ〕、つまりＴ＝１．０〔ｓ〕であるので、Ｔ＝２．０〔ｓ〕において、本来的にはＧＰＳ演算データが更新されるタイミングであるが、ＧＰＳ遮断が発生し、ＧＰＳ演算データが更新されなかった場合が示されている。
【００９７】
かかる場合においては、今までの実施例の演算処理においては、Ｔ＝２．０〔ｓ〕の測位データを初期値として現在時刻Ｔ＝４．０〔ｓ〕までの追い付き演算が実行されていたが、Ｔ＝２．０〜３．０〔ｓ〕において、図９（ａ）と図９（ｂ）は同じ演算プロファイルを示している。このような場合には、図９（ｂ）におけるＰ１の部分が、現在時刻Ｔ＝３．０〔ｓ〕で実行した演算プロファイルと重複するので、カルマンフィルタ５０を用いた演算を実行しないようにする。そして、Ｐ２のＴ＝３．０〜４．０〔ｓ〕の部分についてのみ演算プロファイルを作成し、追い付き演算を実行するようにする。これにより、前の出力周期における測位データの出力の際に既に演算された重複する演算プロファイルについては、この演算処理を省くことができ、演算処理負担を軽減することができる。
【００９８】
次に、図１０を用いて、実施例３に係る移動体用測位装置１００の処理フローについて説明する。図１０は、実施例３に係る移動体用測位装置１００により実行される演算処理の処理フロー図である。
【００９９】
ステップ４００では、ＧＰＳ演算データが更新されたか否かが判定される。ＧＰＳ演算データが更新されたか否かの判定は、例えば、ＧＰＳ演算データ更新判定手段８０により実行されてもよい。
【０１００】
ステップ４００において、ＧＰＳ演算データが更新されていると判定されたときにはステップ４１０に進む。
【０１０１】
ステップ４１０では、演算プロファイル決定手段９０により、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデル測位演算を、どのような演算プロファイルで演算させるかを決定する。これは、図８において説明したように、必要に応じて（１）、（２）式のような演算式を用い、追い付き演算に費やす許容時間Ｔｐ、要求される測位精度、処理負荷等を考慮して、演算プロファイルを決定するようにしてよい。
【０１０２】
ステップ４２０では、Ｔｐａｓｓ＝Ｔｇｐｓで初期時刻ＴｐａｓｓがＧＰＳ時刻Ｔｇｐｓと一致するので、過去のＧＰＳ時刻Ｔｇｐｓに戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。それ以降は、ステップ４１０で決定された演算プロファイルに従って、現在時刻まで測位演算を順次実行し、現在の位置を求める。そして、測位位置を出力し、処理フローを終了する。
【０１０３】
一方、ステップ４００に戻り、ＧＰＳ演算データが更新されておらず、ＧＰＳ遮断の状態であると判定されたときには、ステップ４３０に進む。
【０１０４】
ステップ４３０では、演算プロファイル決定手段９０により、演算プロファイルの決定がなされるが、図９（ｂ）において説明したように、前回の出力周期時の演算で重複する部分のカルマンフィルタの演算は実行しないようにする。重複しない部分については、ＩＮＳ／車輪速協調測位演算と、車両モデル測位演算とをどのような演算プロファイルで演算させるかを決定する。
【０１０５】
ステップ４４０では、ステップ４３０で決定された演算プロファイルに従い、現在時刻まで測位演算を順次実行し、現在の位置を求める。そして、測位位置を出力し、処理フローを終了する。
【０１０６】
図９の処理フローは、移動体用測位装置１００の出力周期及びＧＰＳ測位手段１０のデータ更新周期毎に繰り返し実行される。
【０１０７】
実施例３に係る移動体用測位装置１００によれば、演算プロファイル決定手段９０を用いて、演算処理負荷と測位精度の双方を考慮して、測位精度を確保しつつ演算負荷を低減させ、リアルタイムにおける移動体の位置を測位することができる。
【０１０８】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなく、上述した実施例に種々の変形及び置換を加えることができる。
【０１０９】
特に、図１及び図２に係る実施例及び実施例１〜３においては、移動体用測位装置１００を、車両に適用した場合の演算処理内容について説明したが、車輪速を移動体の速度、車両モデルを移動体モデルとすれば、種々の移動体に本発明を適用することができる。
【図面の簡単な説明】
【０１１０】
【図１】本発明を適用した実施例に係る移動体用測位装置１００の全体構成図である。
【図２】カルマンフィルタ５０による誤差補正を適用したＧＰＳ／ＩＮＳ／車輪速及びＩＮＳ／車輪速の協調アルゴリズムの一例を示すフローチャートである。
【図３】実施例１に係る移動体用測位装置１００のＧＰＳ／ＩＮＳ協調測位演算の説明図である。図３（ａ）は、ＧＰＳ演算データのデータ更新周期を示した図である。図３（ｂ）は、車輪速／ＩＮＳ協調測位演算の時系列を示した図である。図３（ｃ）は、Ｔ＝１．０〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｄ）は、Ｔ＝１．１〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。
【図４】ＧＰＳ演算データが２秒の遅れを持つ場合の測位演算ルーチンの説明図である。図４（ａ）は、現在時刻がＴ＝３．０〔ｓ〕のときの測位演算処理を示す図である。図４（ｂ）は、現在時刻がＴ＝３．１〔ｓ〕のときの測位演算処理を示す図である。図４（ｃ）は、現在時刻Ｔ＝３．９〔ｓ〕のときの測位演算処理を示す図である。
【図５】実施例１に係る移動体用測位装置１００の測位演算の処理フロー図である。
【図６】実施例２に係る移動体用測位装置１００の演算処理内容を説明図である。
【図７】実施例２に係る移動体用測位装置１００の演算処理内容の処理フロー図である。
【図８】実施例３に係る移動体用測位装置１００の演算プロファイル決定の説明図である。
【図９】図８とは異なる演算プロファイル決定の演算処理内容の説明図である。図９（ａ）は、現在時刻Ｔ＝３．０〔ｓ〕の車両位置を出力する場合の演算処理の時系列図である。図９（ｂ）は、現在時刻Ｔ＝４．０〔ｓ〕の車両位置を出力する場合の演算処理の時系列図である。
【図１０】実施例３に係る移動体用測位装置１００の演算処理の処理フロー図である。
【符号の説明】
【０１１１】
１０ ＧＰＳ測位手段
２０ 慣性航法測位手段
３０ 慣性航法用検出手段
４０ 車輪速センサ
５０ カルマンフィルタ
６０ 車両モデル演算手段
７０ 誤差分散補正部
８０ ＧＰＳ演算データ更新判定手段
９０ 演算プロファイル決定手段
１００ 移動体用測位装置
【技術分野】
【０００１】
本発明は、移動体用測位装置に関し、特に、ＧＰＳ測位手段と、慣性航法測位手段とを有する移動体用測位装置に関する。
【背景技術】
【０００２】
従来から、ＧＰＳ（Global Positioning System）受信機による搬送波位相を用いた測位と、ＩＮＳ（Internal Navigation System、慣性航法システム）による測位とを組み合わせ、ＧＰＳ受信機の位置検出間隔より短い時間内の位置データをリアルタイムで求める移動体の検出方法と装置が知られている（例えば、特許文献１参照）。
【０００３】
特許文献１に記載の移動体の検出方法と装置においては、ＧＰＳ測位値が存在する過去の点におけるＩＮＳセンサデータの積算による位置と、ＧＰＳによる位置の差を見て、ＩＮＳセンサの加速度バイアスを求めて速度を補正し、現在時刻に追い付くまで積算演算を実施し、位置演算を行う。
【０００４】
つまり、ＧＰＳの位置データが計測された時刻ｔ０では、（ｔ−３）のＧＰＳ位置データおよび（ｔ−２）のＧＰＳ位置データのジャイロ角度データを取り込み、その差が必要計測精度以内（ぼぼ直線移動）の場合に、（ｔ−３）の時刻から（ｔ−２）の時刻までのＩＮＳ位置データを積算したものを（ｔ−３）にＧＰＳ位置データに加算して（ｔ−２）の位置データとし、これと時刻（ｔ−２）のＧＰＳ位置データとの差からＩＮＳの加速度のバイアスを求め、（ｔ−３）の時刻から（ｔ−２）の時刻までのＩＮＳの速度誤差を出す。そして、このＩＮＳの速度誤差を（ｔ−２）の時刻の速度に加減算して速度補正をし、これを用いて現在時刻ｔ０までのＩＮＳによる移動距離を積算して時刻ｔ０の現在位置を得て、リアルタイムの移動体の位置を検出している。
【特許文献１】特開平８−３０４０９２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述の特許文献１に記載の構成では、直線運動時しか補正が行われないため、例えば移動体が車両の場合には、車両回転等を含む実際の走行時において、測位精度を十分に確保できないという問題があった。また、加速度バイアスの補正が、ＧＰＳ位置データ更新時のタイミングでしか行われないため、この点からも測位精度に欠けるという問題があった。
【０００６】
そこで、本発明は、ＧＰＳ測位と慣性航法測位の組み合わせを利用して、移動体のリアルタイムの位置を、移動体が旋回中であったり、ＧＰＳ位置データ更新時以外の任意のタイミングであったりしても、高精度で測位する移動体用測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、第１の発明に係る移動体用測位装置は、現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段と、前記速度を用いて移動体モデルを演算する移動体モデル演算手段とを有し、前記現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置であって、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと一致したときには、前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の前記ＧＰＳ演算データ更新時における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル算出手段が前記ＧＰＳ演算データ更新時から前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記時間遅れ分遡った時刻における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記時間遅れ分遡った時刻から前記現在時刻までの移動体モデルを演算し、前記移動体の位置を測位することを特徴とする。
【０００８】
これにより、移動体モデルを用いて現在時刻における位置演算を行うため、移動体が直線運動以外の運動を行っているときでも、測位精度を確保することができる。また、現在時刻がＧＰＳ演算データの更新タイミングに一致しなかった場合でも検出値が補正されるため、測位精度を確保することができる。
【０００９】
第２の発明は、第１の発明に係る移動体用測位装置において、
前記移動体モデル演算手段は、前記出力周期毎に、前記移動体モデルを演算して前記移動体の位置を測位することを特徴とする。
【００１０】
これにより、移動体用測位装置が、ＧＰＳ演算データ更新周期とは異なる出力周期である場合にも、常に移動体モデルを用いて高精度の位置を出力することができる。
【００１１】
第３の発明に係る移動体用測位装置は、現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段とを有し、前記現在時刻における移動体の位置を測位し、前記データ更新周期で出力する移動体用測位装置であって、
前記データ更新周期において、前記ＧＰＳ演算データが更新されたか否かを判定するＧＰＳ演算データ更新判定手段を有し、
該ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていると判定されたときには、前記ＧＰＳ演算データ更新時における前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、前記ＧＰＳ演算データ更新後から前記現在時刻までの間は、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位し、
前記ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていないと判定されたときには、前記ＧＰＳ演算データが更新されるべき時刻から前記現在時刻までの間を、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位することを特徴とする。
【００１２】
これにより、ＧＰＳ演算データの更新がない場合であっても、慣性航法用検出値及び速度を補正し、これらの補正値を用いて慣性航法と速度を組み合わせた位置演算を実行することにより、測位精度を確保することができる。
【００１３】
第４の発明は、第３の発明に係る移動体用測位装置において、
前記速度を用いて、移動体モデルを演算する移動体モデル演算手段と、
前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記移動体モデルを用いた測位演算との演算プロファイルを決定する演算プロファイル決定手段と、を更に有することを特徴とする請求項３に記載の移動体用測位装置。
【００１４】
これにより、測位精度を確保しつつ、演算負荷を低減し、用途に応じた精度で適切にリアルタイムの移動体位置を測位することができる。
【００１５】
第５の発明は、第４の発明に係る移動体用測位装置において、
前記移動体の挙動が大きいときには、前記移動体モデルを用いた測位演算以外の演算により前記演算プロファイルを決定することを特徴とする。
【００１６】
これにより、移動体の挙動が大きいときには、より変化の大きい検出値の方に精度の良い演算の比重を多くすることができ、測位が不正確になりやすい状況下でも、測位精度を確保することができる。
【００１７】
第６の発明は、第１〜５のいずれかの発明に係る移動体用測位装置において、
前記誤差補正手段は、カルマンフィルタを含むことを特徴とする。
【００１８】
これにより、カルマンフィルタを用いて種々の検出値の誤差を高精度に検出することができ、誤差を適切に補正して位置演算の精度を高めることができる。
【００１９】
第７の発明は、第１〜６のいずれかの発明に係る移動体用測位装置において、
前記移動体は、車両であることを特徴とする。
【００２０】
これにより、高精度なリアルタイム性の高い車載ナビゲーションシステムを提供することができ、車両の現在位置を高精度でユーザに知らせることができる。
【発明の効果】
【００２１】
本発明によれば、移動体の現在時刻におけるリアルタイム位置を、高精度で測位することができる。
【発明を実施するための最良の形態】
【００２２】
以下、図面を参照して、本発明を実施するための最良の形態の説明を行う。
【００２３】
図１は、本発明を適用した実施例に係る移動体用測位装置１００の全体構成を示した図である。本実施例に係る移動体用測位装置１００は、種々の移動体に適用可能であり、例えば、車両、自動二輪車、鉄道、船舶、航空機、フォークリフト、移動ロボット、人間に携帯されて移動する携帯電話等の情報端末等の移動体に適用できる。本実施例においては、移動体用測位装置１００を、車両に適用した例について説明するが、上述の移動体にも同様に適用することができる。
【００２４】
本実施例に係る移動体用測位装置１００は、ＧＰＳ測位手段１０と、慣性航法測位手段２０と、慣性航法用検出手段３０と、速度検出手段である車輪速センサ４０と、誤差補正手段であるカルマンフィルタ５０とを有する。また、本実施例に係る移動用測位装置１００は、必要に応じて、車両モデル演算手段６０と、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０と、演算プロファイル決定手段９０とを備えてよい。
【００２５】
ＧＰＳ測位手段１０は、主たる構成要素としてＧＰＳ受信機及びＧＰＳアンテナ（図示せず）を備える。ＧＰＳ測位手段１０のＧＰＳ受信機は、ＧＰＳアンテナを介して入力される衛星信号に基づいて、車両位置及び車両速度を測位する。車両位置及び車両速度は、いわゆる単独測位法で測位されてもよい。尚、ここでは、車両位置及び車両速度は、緯度、経度及び高度の座標系（ｌｌｈとＮＥＤ）で求められるものとする。また、ＧＰＳ受信機は、測位過程で車両位置及び車両速度の誤差分散を算出する。誤差分散は、緯度、経度及び高度の成分毎に導出される。誤差分散の算出方法は、任意の適切な方法が採用されてよい。ＧＰＳ測位装置１０により算出される誤差分散は、ＧＰＳ演算データのデータ更新周期毎に、誤差分散補正部７０に入力される。
【００２６】
慣性航法測位手段２０は、図１に示すように、慣性航法用検出手段３０の出力信号に基づいて、車両位置、速度や車両姿勢（方位角）を算出する。慣性航法用検出手段３０は、例えば、３軸加速度センサ及び３軸角速度センサを含むＩＭＵ（慣性測定ユニット）として構成されてよい。慣性航法による車両位置の測位方法は、多種多様でありえ、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値（移動体用測位装置１００の最終的な測位結果の前回値のフィードバック）に積算することで導出されてよい。尚、ここでは、慣性航法測位手段２０においても、車両位置及び車両速度は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。
【００２７】
慣性航法測位手段２０による慣性航法を用いた測位は、加速度センサのバイアスや、角速度センサのドリフトがそのまま積分されるため、出力に誤差を生じてしまう。これを補正するため、ＧＰＳ測位手段１０からの位置及び速度と、車輪速センサ４０からの速度とが拘束条件として利用され、ＧＰＳ測位手段１０の遮断時には、車輪速センサ４０からの速度のみが拘束条件として用いられる。
【００２８】
慣性航法測位手段２０により測位される車両位置及び車両速度（ＩＮＳ仮測位結果）は、ＧＰＳ演算データのデータ更新周期毎に、ＧＰＳ測位手段１０により測位される車両位置及び車両速度（ＧＰＳ測位結果）との差分値が取られ、当該差分値がカルマンフィルタ５０に入力される。即ち、車両位置の差及び車両速度の差が、観測量ｚとしてカルマンフィルタ５０に入力される。この際、ＧＰＳ測位結果及びＩＮＳ仮測位結果は、時間的に同期した態様で差分が取られる（例えばＧＰＳ時刻を基準として同期させて差分が取られる）。
【００２９】
同様に、慣性航法測位手段２０により測位された車両速度は、車両モデル演算手段６０から出力される推定車両速度との差分値が取られ、これもカルマンフィルタ５０に入力される。つまり、車両速度の差が、観測量ｚとしてカルマンフィルタ５０に入力される。なお、ＧＰＳ測位手段１０によるデータ更新周期は、例えば１〔秒〕、２〔秒〕といった秒単位のレベルであるが、慣性航法用検出手段３０及び車輪速センサ４０は、殆どリアルタイムに近い状態で検出が可能であるので、慣性航法測位手段２０から出力される車両速度及び車両モデル演算手段６０から出力される推定車両速度は、例えば１０〔Ｈｚ〕、２０〔Ｈｚ〕というレベルでの高速出力が可能である。
【００３０】
カルマンフィルタ５０は、ＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値を入力として、ＧＰＳ測位結果及びＩＮＳ仮測位結果のそれぞれの信頼性により確率的に最も正しい値になるように、状態量であるＩＮＳ補正値ηを推定する。ＩＮＳ補正値ηは、車両位置及び速度に関する補正値に加えて、車両姿勢、加速度センサ及び角速度センサのバイアス、ドリフトに関する補正値を含んでよい。カルマンフィルタ５０では、例えば、以下のようにして、ＩＮＳ補正値ηが推定される。状態方程式は、以下のように設定される。
η（ｔｎ）＝Ｆ・η（ｔｎ−１）＋Ｇ・ｕ（ｔｎ−１）＋Γ・ｗ（ｔｎ−１）
ここで、η（ｔｎ）は、時刻ｔ＝ｔｎでの状態変数を表わし、また、ｕ（ｔｎ−１）及びｗ（ｔｎ−１）は、それぞれ、時刻ｔ＝ｔｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔｎ）は、慣性航法測位手段２０で推定される車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）のそれぞれの誤差δｒ（ＩＮＳ）及びδｖ（ＩＮＳ）、慣性航法測位手段２０で推定される車両の姿勢の誤差δε（ＩＮＳ）、慣性航法用検出手段３０の角速度センサのバイアス誤差δｂ、慣性航法用検出手段３０の加速度センサのドリフト誤差δｄ、及び、車両のタイヤの半径誤差δｓ等を含んでよい。
【００３１】
また、観測方程式は、次のように設定される。
ｚ（ｔｎ）＝Ｈ（ｔｎ）・η（ｔｎ）＋ｖ（ｔｎ）
観測量ｚ（ｔｎ）は、時刻ｔ＝ｔｎ−１でのＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値である。Ｈ（ｔｎ）は観測行列であり、ｖ（ｔｎ）は観測雑音である。
【００３２】
カルマンフィルタ５０は、上述のように、ＧＰＳ演算データによる位置及び速度と、車輪速センサ４０からの速度の拘束条件と慣性航法との位置差、速度差から、慣性航法測位手段２０、慣性航法用検出手段３０及び車輪速センサ４０にどの程度の大きさの誤差があるのかを推定し、それぞれを補正先に戻す役割を担う誤差補正手段である。よって、図１においても、カルマンフィルタ５０の推定誤差は、慣性航法測位手段２０、慣性航法用検出手段３０及び車輪速センサ４０にフィードバックされている。
【００３３】
車両モデル演算手段６０は、車載の各種センサの出力を入力として、車両の位置を推定する車両モデルを演算する手段である。車両モデルの構築方法は、種々提案されており、如何なる方法であってもよい。例えば、車輪速に、タイヤの半径変動を乗じて積算すれば車両位置が算出されるが、これに方位角の要素を考慮するようにした関係を示す数式であってもよい。図示の車両モデル演算手段６０は、車輪速センサ４０の出力を入力として、位置が既知の基点からの車両の移動量を推定して車両の位置を推定するモデルである。尚、ここでは、車両モデル６０においても、推定車両位置は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。車両の移動量が、例えばWGS84に基づく地球固定座標系等のような他の座標系で算出されている場合、当該移動量を座標変換し、当該座標変換した移動量を、経度及び高度の座標系の基点に積算することで、経度及び高度の座標系の推定車両位置を算出することができる。車両モデル演算手段６０により演算された車両モデルから推定された車両位置は、誤差分散補正部７０に入力される。
【００３４】
誤差分散補正部７０は、ＧＰＳ測位手段１０から入力される誤差分散を補正する手段である。誤差分散補正部７０は、補正した誤差分散をカルマンフィルタ５０に入力する。カルマンフィルタ５０では、誤差分散補正部７０から入力される誤差分散が観測ノイズの分散として用いられて、状態量（ＩＮＳ補正値η）が推定される。ＧＰＳ測位手段１０からのＧＰＳ演算データも補正する場合には、このような誤差分散補正部７０を設けるようにしてもよい。
【００３５】
ＧＰＳ演算データ更新判定手段８０は、ＧＰＳ測位手段１０のＧＰＳ演算データの更新が行われたか否かを判定するための手段である。ＧＰＳ測位手段１０のＧＰＳ受信機は、障害等によりＧＰＳ衛星からの信号を受信することができず、ＧＰＳ遮断が発生する場合がある。よって、ＧＰＳ演算データ更新判定手段８０は、そのようなＧＰＳ遮断が発生していないか否かを、データ更新周期におけるＧＰＳ衛星からの信号の有無により、ＧＰＳ演算データの更新がなされているか否かの判定を行う。
【００３６】
演算プロファイル決定手段９０は、ＧＰＳ測位手段１０からの車両位置及び車両速度のＧＰＳ演算データ、慣性航法測位手段２０による車両位置及び車両速度の推定、車両モデル演算手段６０による車両モデルを用いた車両位置及び車両速度の推定を、どのように組み合わせて移動体の位置を測位するかの演算割合を定める手段である。移動体の位置の測位精度は、用途に応じて要求度合いが異なるので、要求される測位精度と演算負荷を考慮して適切な演算プロファイルを定めるようにしてよく、演算プロファイル決定手段９０は、そのような適切な演算プロファイルを決定する機能を有する。演算プロファイル決定手段９０は、演算プロファイルを決定するための演算処理を行うので、ＣＰＵ（Central Processing Unit、中央処理装置）を備えたマイクロコンピュータや、特定の演算処理用のＡＳＩＣ（Application Specific Integrated Circuit）等として構成されてよい。
【００３７】
なお、以後、ＧＰＳ測位、慣性航法測位及び速度を用いた測位の協調アルゴリズムを、ＧＰＳ／ＩＮＳ／車輪速と表し、慣性航法測位及び速度を用いた測位の協調アルゴリズムを、ＩＮＳ／車輪速と表すものとする。
【００３８】
図２は、本実施例に係る移動体用測位装置１００により実現される、カルマンフィルタ５０による誤差補正を適用したＧＰＳ／ＩＮＳ／車輪速及びＩＮＳ／車輪速の協調アルゴリズムの一例を示すフローチャートである。
【００３９】
ステップ１００では、慣性航法用検出手段３０の出力値（例えば３軸方向の加速度、及び、３軸まわりの角速度）がサンプリングされる。慣性航法用検出手段３０の出力値は、後述のステップ１５０のカルマンフィルタ５０により推定されるＩＮＳ補正値ηに基づいて補正を受けてよい。即ち、慣性航法用検出手段３０の出力値のバイアスやドリフトの誤差が補正されてよい。
【００４０】
ステップ１１０では、慣性航法測位手段２０において、上記ステップ１００で得られる慣性航法用検出手段３０の出力値に基づいて、車両推定位置及び車両推定速度、姿勢等が導出される。
【００４１】
ステップ１２０では、カルマンフィルタ５０においてカルマンフィルタ５０の時間更新が実行される。カルマンフィルタ５０の時間更新は、例えば以下のように表される。
η（ｔｎ）（−）＝η（ｔｎ−１）（＋）＋ｕ（ｔｎ−１）
Ｐ（ｔｎ）（−）＝Ｆ・Ｐ（ｔｎ−１）（＋）・ＦＴ＋Γ・Ｑ（ｔｎ−１）・ΓＴ
尚、Ｐは、予測・推定誤差の共分散行列であり、Ｑは、外乱ｗの共分散行列（正定値対称行列）である。尚、符号（＋）及び（−）は更新前後を表す。
【００４２】
ステップ１３０では、今回周期（ｔｎ）の観測量ｚ（ｔｎ）に基づいて観測行列Ｈ（ｔｎ）が演算される。
【００４３】
ステップ１４０では、カルマンゲインＫｋが以下のように演算される。
Ｋ（ｔｎ）＝Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）・（Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）＋Ｒ（ｔｎ））−１
ここで、Ｒ（ｔｎ）は、観測ノイズの分散行列である。Ｒ（ｔｎ）は、誤差分散補正部１４０で生成され、次の通りである。
Ｒ（ｔｎ）＝Ｍｋ（ｔｎ）・Ｗｋ（ｔｎ）
ここで、Ｍｋ（ｔｎ）は、非対角成分がゼロの分散更新用行列であり、デフォルトは単位行列である。分散更新用行列Ｍｋ（ｔｎ）の対角成分は、分散更新用ゲインである。Ｗｋ（ｔｎ）は、非対角成分がゼロの行列であり、対角成分には、ＧＰＳ測位手段１０により算出される誤差分散の各成分（緯度成分、経度成分、高度成分）が代入される。
【００４４】
ステップ１５０では、上記のステップ１４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、状態量η（ｔｎ）が以下のように算出される。
η（ｔｎ）（＋）＝η（ｔｎ）（−）＋Ｋ（ｔｎ）・（ｚ（ｔｎ）−Ｈ（ｔｎ）・η（ｔｎ）（−））
ステップ１６０では、上記のステップ１５０で導出された状態量η（ｔｎ）に基づいて、誤差補正が実行される。即ち、慣性航法用検出手段３０の出力値のバイアス及びドリフト補正が実行され、慣性航法測位手段２０において車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）、姿勢の補正が実行される。その結果、誤差補正後の車両推定位置等が最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）として得られる。尚、上記のステップ１００乃至１７０の処理は、その周期の慣性航法用検出手段３０の出力値に基づいて、例えば状態量η（ｔｎ）が収束するまで繰り返し実行されてもよい。また、車輪速センサ４０のスケーリングファクタ（車両モデル）についても、状態量η（ｔｎ）に含まれるタイヤ半径誤差δｓの推定値に基づいて、補正されてよい。
【００４５】
つまり、詳細には後述するが、誤差補正手段であるカルマンフィルタ５０により、ＧＰＳ演算データがあるときは、ＧＰＳ／ＩＮＳ／車輪速協調測位の結果が誤差補正後の測位結果として得られ、ＧＰＳ演算データが無いときには、ＩＮＳ／車輪速協調測位の結果が、誤差補正後の測位結果として得られる。車輪速に関しても、誤差補正後の値を得ることができ、これを用いて車両モデルを演算することが可能となる。
【００４６】
ステップ１７０では、上記のステップ１４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、共分散行列Ｐが以下のように更新される。
Ｐ（ｔｎ）（＋）＝Ｐ（ｔｎ）（−）−Ｋ（ｔｎ）・Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）
以下、図１及び図２に示した本実施例に係る移動体用測位装置１００の具体的な演算内容を示す実施例について説明する。
【実施例１】
【００４７】
図３は、実施例１に係る移動体用測位装置１００によるＧＰＳ／ＩＮＳ協調測位の内容を説明するための図である。図３は、実施例１に係るＧＰＳ／ＩＮＳ協調測位の演算内容を、時系列的に示している。実施例１に係る移動体用測位装置１００においては、移動体モデルを用いた移動体の現在位置の演算を行う。移動体が車両である場合を示した図１の全体構成図においては、車両モデル演算手段６０を用いて車両の測位を行うので、車両モデル演算手段６０を備える。一方、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０と、演算プロファイル決定手段９０は、必ずしも備えられなくてもよい実施形態である。
【００４８】
図３（ａ）は、ＧＰＳ演算データのデータ更新周期を示した図である。図３（ａ）において、ＧＰＳ演算データは、データ更新周期Ｔ＝１．０〔ｓ〕で演算データの更新がなされている。そして、ＧＰＳ演算データは、リアルタイムの時刻よりも、２〔ｓ〕遅れて移動体用測位装置１００から出力される関係があるとする。つまり、ＧＰＳ測位手段１０で受信し、演算して出力したＧＰＳ演算データは、そのまま出力すると、２秒の時間遅れを有する２秒前の車両位置が出力されることになる。
【００４９】
図３（ｂ）は、車輪速／ＩＮＳ協調測位演算の時系列を示した図である。図３（ｂ）において、車輪速センサ４０及び慣性航法測位手段２０による慣性航法測位に用いられる慣性航法用検出手段３０は、加速度センサ、角加速度センサ等であり、ほぼリアルタイムに検出値を取得できる。よって、車輪速／ＩＮＳ協調測位演算は、例えば１０〔Ｈｚ〕の周期で演算を行うことができる。よって、図３（ｂ）においては、Ｔ＝１．０〔ｓ〕からＴ＝３．０〔ｓ〕の間には、２０個の検出値の存在が示されている。
【００５０】
図３（ｃ）は、Ｔ＝１．０〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｃ）において、Ｔ＝１．０〔ｓ〕の時刻を初期値として、車両モデルの演算が行われているが、Ｔ＝１．０〔ｓ〕の時刻においては、図３（ａ）に示すように、ＧＰＳ演算データが存在するので、ＧＰＳ／ＩＮＳ／車輪速協調測位演算のカルマンフィルタ５０の結果を用いて、車両の初期位置を算出する。同様に、ＧＰＳ／ＩＮＳ／車輪速協調測位演算のカルマンフィルタ５０の結果を用いて、慣性航法用検出値である加速度、角速度についてバイアス、ドリフト補正を行う。また、車輪速についても補正を行う。そして、これらの補正されたセンサ値を用いて車両モデルによる位置演算を繰り返して積算し、現在時刻のＴ＝３．０〔ｓ〕に追い付くまで車両モデルによる位置演算を実行する。これにより、ＧＰＳ演算データが存在するときには、ＧＰＳ／ＩＮＳ／車輪速の初期位置を用いて、車両モデルにより現在時刻の車両位置を演算することができる。
【００５１】
図３（ｄ）は、Ｔ＝１．１〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｄ）において、Ｔ＝１．１〔ｓ〕の時刻を初期値として、車両モデルの演算が行われているが、Ｔ＝１．１〔ｓ〕では、ＧＰＳ演算データは存在せず、車輪速／ＩＮＳ協調測位演算に用いられるデータのみが存在する。よって、この場合には、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、車両の初期位置を算出する。同様に、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、加速度、角速度、車輪速等のセンサ値の補正を行い、バイアス、ドリフト補正を行う。そして、現在時刻Ｔ＝３．１〔ｓ〕に追い付くためのＴ＝１．２〜３．１〔ｓ〕までの車両モデルの演算を、Ｔ＝１．１〔ｓ〕の時刻の車輪速／ＩＮＳのカルマンフィルタ５０の結果を用い、これに基づいて車両モデルによる位置演算を実施する。これにより、ＧＰＳ演算データが存在しないときであっても、車輪速／ＩＮＳ協調測位演算のカルマンフィルタ５０の結果を用いて、車両モデルの演算により現在時刻の車両位置を演算することができる。
【００５２】
図４は、ＧＰＳ測位手段１０により取得されるＧＰＳ演算データが、実際の現在時刻よりも２秒の遅れを持つ場合の測位演算ルーチンを説明するための図である。ＧＰＳ演算データは、Ｔ＝１．０〔ｓ〕、２．０〔ｓ〕・・・の１秒周期でデータ更新されている。また、移動体用測位装置１００は、Ｔ＝１．１〔ｓ〕、１．２〔ｓ〕、１．３〔ｓ〕・・・で位置データが出力され、出力周期は０．１秒である場合を考える。
【００５３】
図４（ａ）は、現在時刻がＴ＝３．０〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．０〔ｓ〕のときには、ＧＰＳ演算データは、２秒前のＴ＝１．０〔ｓ〕の測位データを更新しているタイミングであり、ＧＰＳ更新データが存在するタイミングである。つまり、初期時刻をＴｐａｓｓとし、ＧＰＳ演算データの更新時刻（以下、「ＧＰＳ時刻」という。）をＴｇｐｓとすると、Ｔｐａｓｓ＝Ｔｇｐｓとなるタイミングである。従って、この場合には、ＧＰＳ／車輪速／ＩＮＳ協調測位演算の測位データを初期値として用いるようにする。ここで、データ更新されたＧＰＳ演算データをそのまま出力すると、２秒前の車両位置を出力してしまうことになるので、現在時刻までのＴ＝１．１〜３．０〔ｓ〕の時間の車両の位置変化を演算する。このとき、車両モデル演算手段６０により、車両モデルを用いて現在時刻Ｔ＝３．０〔ｓ〕における車両位置を算出するための追い付き演算を行うので、車両モデルの演算に必要な車輪速センサ値を含む加速度、角速度等を用いて、Ｔ＝１．１〜３．０〔ｓ〕までの積算を行う。また、このとき、Ｔ＝１．０〔ｓ〕における初期値Ｔｐａｓｓを用いて、カルマンフィルタ５０を用いて、加速度センサに対応するバイアス補正、角速度センサに対応するドリフト補正、車輪速センサ４０に対応する車輪速補正や、これらのセンサの取り付け面補正等が行われる。
【００５４】
つまり、初期時刻Ｔｐａｓｓは、ＧＰＳ時刻Ｔｇｐｓと一致しており、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の演算データが存在するので、これに対して誤差補正手段であるカルマンフィルタ５０を用いて誤差補正値を算出し、これを用いてＴ＝１．１〜３．０〔ｓ〕の間の車両モデルによる位置演算を実行し、現在時刻Ｔ＝３．０〔ｓ〕における車両位置を測位する。
【００５５】
図４（ｂ）は、現在時刻がＴ＝３．１〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．１〔ｓ〕のときには、２秒前はＴ＝１．１〔ｓ〕であり、ＧＰＳ演算データの更新タイミングとは異なるタイミングである。このときの初期時刻Ｔｐａｓｓは、図４（ａ）で示した前回の初期時刻Ｔｐａｓｓに１周期分の出力周期ＳＴを加えた時刻であり、初期時刻Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴ＝Ｔｇｐｓ＋ＳＴ＝１．０〔ｓ〕＋０．１〔ｓ〕＝１．１〔ｓ〕である。このとき、位置データは、車輪速／ＩＮＳ協調測位演算による測位データしか存在しないので、これを初期値（初期位置）として用いる。また、時刻Ｔ＝１．２〔ｓ〕から現在時刻Ｔ＝３．１〔ｓ〕までの演算は、図４（ａ）と同様に、車両モデルを演算するのに必要な加速度、角速度、車輪速センサ値を用いて行われる。このとき、カルマンフィルタ５０によるバイアス、ドリフト、車輪速、取り付け面補正等は、Ｔ＝１．１〔ｓ〕における車輪速／ＩＮＳ協調測位演算による測位データを用いて行われる。そして、ここで算出した補正値を用いて、Ｔ＝１．２〜３．１〔ｓ〕の車両モデルによる位置演算が実行され、現在時刻Ｔ＝３．１〔ｓ〕の車両位置が測位される。
【００５６】
図４（ｃ）は、現在時刻Ｔ＝３．９〔ｓ〕のときの測位演算処理を示す図である。現在時刻がＴ＝３．９のときには、図４（ｂ）と同様に、初期時刻Ｔｐａｓｓは、前回の出力周期における初期時刻Ｔｐａｓｓに、出力周期ＳＴ分を加えた時刻となり、Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴである。図４（ｂ）において、初期値に車輪速／ＩＮＳ協調測位演算による測位データが用いられた後、次の更新タイミングＴ＝２．０〔ｓ〕でＧＰＳ演算データが更新されるまでは、ＧＰＳ演算データの更新データは存在せず、車輪速／ＩＮＳ協調測位演算による測位を実行し続けることになる。よって、Ｔ＝３．９〔ｓ〕においても、初期時刻Ｔｐａｓｓ＝１．９〔ｓ〕における車輪速／ＩＮＳ協調測位演算による測位データを初期値（初期位置）とし、これを用いてカルマンフィルタ５０によるバイアス、ドリフト、車輪速、取り付け面の補正値を算出する。そして、この補正値を用いて、加速度、角速度、車輪速センサ値を補正し、Ｔ＝２．０〜３．９〔ｓ〕までの車両モデルを積算して、現在時刻Ｔ＝３．９〔ｓ〕における車両位置を演算し、出力する。
【００５７】
図４（ｂ）、（ｃ）の測位演算処理は、図３に示したように、ＧＰＳ演算データの更新周期が１〔Ｈｚ〕、つまりＴ＝１．０〔ｓ〕とすると、現在時刻Ｔ＝４．０〔ｓ〕、初期時刻Ｔｐａｓｓ＝２．０〔ｓ〕となったときに、Ｔｐａｓｓ＝Ｔｇｐｓとなり、図４（ａ）で説明した測位演算処理が実行される。このような一連の測位演算ルーチンの実行により、ＧＰＳ演算データの更新タイミングで取得されるデータが過去の車両位置を示す測位データであっても、車両モデルを用いて位置演算を実行することにより、現在時刻における車両位置を高精度で測位することができる。
【００５８】
図５は、実施例１に係る移動体用測位装置１００の測位演算処理フローを示した処理フロー図である。
【００５９】
ステップ２００では、ＧＰＳ測位手段１０において、ＧＰＳ演算データが更新されたか否かが判定される。これは、現在時刻において、ＧＰＳ演算データが更新されたか否かを判定すればよい。また、ＧＰＳ演算データが更新されたか否かは、例えば、ＧＰＳ演算データ更新判定手段８０により判定されてもよい。
【００６０】
ステップ２００において、ＧＰＳ演算データが更新されたと判定されたときにはステップ２１０に進み、ＧＰＳ演算データが更新されていないと判定されたときには、ステップ２２０に進む。
【００６１】
ステップ２１０では、初期時刻ＴｐａｓｓをＧＰＳ時刻Ｔｇｐｓとし、ＧＰＳの時間遅れを考慮し、過去のＧＰＳ時刻に戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行し、ＧＰＳ／ＩＮＳ／車輪速協調測位演算による測位データを算出する。測位データには、車両の位置だけでなく、車両速度や車両姿勢のデータも含まれてよい。また、このとき、カルマンフィルタ５０を用いて、初期時刻Ｔｐａｓｓにおける補正値を算出しておく。
【００６２】
また、ステップ２２０では、現在時刻からＧＰＳ演算データのデータ更新周期分遡った初期時点Ｔｐａｓｓ＝Ｔｐａｓｓ＋ＳＴにおける、ＩＮＳ／車輪速協調測位演算による測位データを算出する。ステップ２１０と同様に、測位データには、車両の位置だけでなく、車両速度や車両姿勢のデータも含まれてよい。また、このとき、カルマンフィルタ５０を用いて、初期時刻Ｔｐａｓｓにおける補正値を算出しておく。
【００６３】
ステップ２３０では、次回の初期時刻Ｔｐａｓｓでのカルマンフィルタ５０を用いた演算のために、ステップ２１０又はステップ２３０で算出した車両位置、車両速度、車両姿勢等の測位データ履歴を記憶して残しておく。記憶は、通常のＲＡＭ（Random Access Memory）等に用いられるメモリが適用されてよい。
【００６４】
ステップ２４０では、ステップ２１０又はステップ２２０で求めた車両位置を初期値（初期位置）と定める。つまり、初期時刻ＴｐａｓｓにおけるＧＰＳ／ＩＮＳ／車輪速協調測位演算又はＩＮＳ／車輪速協調測位演算により求めた車両位置を初期値とする。
【００６５】
ステップ２５０では、初期時刻Ｔｐａｓｓから現在時刻までの車両モデルの演算に必要な車両センサ値を、ステップ２１０又はステップ２２０で求めた初期時刻Ｔｐａｓｓ時の補正値を用いて、誤差補正を行う。車両センサ値は、加速度、角速度、車輪速度等の検出値が用いられてよい。
【００６６】
ステップ２６０では、ステップ２４０で決定した初期値を基点として、ステップ２５０で算出した初期時刻Ｔｐａｓｓから現在時刻までの補正値を用いて、初期時刻Ｔｐａｓｓから現在時刻までの、車両モデルによる積算を順次実行する。車両モデルによる位置演算は、車両モデル演算手段６０により行われてよい。そして、現在時刻における車両位置を算出し、出力して処理フローを終了する。
【００６７】
なお、ステップ２６０において実行される追い付き演算は、移動体用測位装置１００の出力周期に応じて実施される。例えば、出力周期がＩＮＳ／車輪速協調測位演算の測位データ算出周期よりも大きい場合には、データ出力を、出力周期に合わせて減少させるようにしてもよい。
【００６８】
図５に示した処理フローは、実施例１に係る移動体用測位装置１００の出力周期毎に演算され、繰り返される。このように、実施例１に係る移動体用測位装置１００の演算処理内容によれば、車両モデルを用いて車両の現在時刻における位置を演算算出するので、リアルタイム性を実現できるとともに、高精度な測位を行うことができる。
【実施例２】
【００６９】
実施例２に係る移動体用測位装置１００は、図１の全体構成図において、ＧＰＳ演算データ更新判定手段８０が必要とされ、車両モデル演算手段６０、誤差分散補正部７０及び演算プロファイル決定部９０は必ずしも必要とされない実施形態である。
【００７０】
図６は、実施例２に係る移動体用測位装置１００の演算処理内容を説明するための図である。図６において、ＧＰＳ演算データ、車輪速／ＩＮＳ協調測位演算、現在時刻との関係を表す時系列が示されている。実施例２においては、移動体用測位装置１００の測位出力周期がＧＰＳ測位手段１０のＧＰＳ演算データのデータ更新周期と一致する場合の演算処理の一例について説明する。図６において、ＧＰＳ演算データのデータ更新周期及び移動体用測位装置１００の測位出力周期はともにＴ＝１．０〔ｓ〕であり、Ｔ＝１．０〔ｓ〕、Ｔ＝２．０〔ｓ〕、Ｔ＝３．０〔ｓ〕・・・にデータ更新が行われる例が示されている。
【００７１】
Ｔ＝１．０〔ｓ〕のときには、ＧＰＳ演算データが更新され、ＧＰＳ演算データが存在する状態である。このとき、現在時刻は、Ｔ＝２．０〔ｓ〕である。Ｔ＝１．０〔ｓ〕において、車輪速／ＩＮＳ協調測位演算の測位データも存在するが、上述のように、ＧＰＳ演算データが存在するので、Ｔ＝１．０〔ｓ〕においては、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。このとき、カルマンフィルタ５０を用いて誤差補正を行うようにして、測位精度を高く保つ。
【００７２】
Ｔ＝１．１〜２．０〔ｓ〕の時間においては、ＧＰＳ演算データのデータ更新はなされず、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を行うことはできない。しかしながら、車輪速／ＩＮＳ協調測位演算の演算データは算出できるので、車輪速／ＩＮＳ協調測位演算による測位を行うことができる。そして、この場合にも、カルマンフィルタ５０による誤差補正を行うことができる。つまり、Ｔ＝１．１〜２．０〔ｓ〕の時間においては、車輪速／ＩＮＳ協調測位演算を順次実行するとともに、カルマンフィルタ５０による補正を各センサのサンプリング周期毎に行い、測位精度を高めることができる。実施例１においては、移動体用測位装置１００の出力周期が短かったために、カルマンフィルタ５０による検出値補正は、初期値に対してしか実行できなかったが、実施例２においては、過去のＧＰＳ時間Ｔｇｐｓから現在時刻まで一気にまとめて計算することができ、その時に一気にまとめてカルマンフィルタ５０を用いて演算を行うことができるので、測位精度を高く保つことができる。そして、現在時刻Ｔ＝２．０〔ｓ〕の測位演算結果を、Ｔ＝１．０〔ｓ〕のＧＰＳ演算データを用いてリアルタイムに出力することができる。
【００７３】
このように、初期時刻にＧＰＳ演算データが存在する場合には、初期時刻ＴｐａｓｓのみＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行し、それ以降から現在時刻までは、車輪速／ＩＮＳ協調測位演算を実行し、カルマンフィルタ５０を用いて一気に現在位置の測位を行うことができる。
【００７４】
また、Ｔ＝２．０〔ｓ〕は、ＧＰＳ演算データのデータ更新周期のタイミングであり、ＧＰＳ演算データが本来的にはデータ更新される筈であるが、図６においては、Ｔ＝２．０〔ｓ〕でＧＰＳ演算データが更新されていない例が示されている。例えば、車両がＧＰＳ衛星からの信号を受信することができない場所にあり、ＧＰＳ遮断となる場合がある。そのような場合には、Ｔ＝２．０〔ｓ〕の時刻でＧＰＳ／ＩＮＳ／車輪速協調測位演算を行うことはできないが、車輪速／ＩＮＳ協調測位演算のための車両センサ値は存在するので、車輪速／ＩＮＳ協調測位演算を行うことができる。この場合には、初期時刻ＴｐａｓｓのＴ＝２．０〔ｓ〕から現在時刻Ｔ＝３．０〔ｓ〕まで、車輪速／ＩＮＳ協調測位演算により、カルマンフィルタ５０により誤差補正された補正値を用いて、一気に測位演算を行うことができる。初期値に、ＧＰＳ／ＩＮＳ／ＩＮＳ協調測位演算を用いることはできないが、追い付き演算で行われる車輪速／ＩＮＳ協調測位演算の総てを、補正された車両センサ値を用いて行うことができるので、測位精度を高く保つことができる。
【００７５】
このように、ＧＰＳ遮断が発生しており、ＧＰＳ演算データがデータ更新周期において更新されなかった場合であっても、車輪速／ＩＮＳ協調測位演算を、カルマンフィルタ５０を用いて実行することにより、現在時刻における位置を高精度に測位することができる。
【００７６】
なお、ＧＰＳ演算データがデータ更新周期において更新されたか否かは、ＧＰＳ演算データ更新判定手段９０により判定してよい。
【００７７】
図７は、実施例２に係る移動体用測位装置１００の演算処理内容を示した処理フロー図である。
【００７８】
ステップ３００では、ＧＰＳ測位手段１０のＧＰＳ演算データが更新したか否かが判定される。ＧＰＳ演算データが更新したか否かは、ＧＰＳ演算データ更新判定手段９０により行われてよい。
【００７９】
ステップ３００において、ＧＰＳ演算データがデータ更新していると判定された場合にはステップ３１０に進み、ＧＰＳ演算データがデータ更新していないと判定された場合にはステップ３２０に進む。
【００８０】
ステップ３１０では、過去のＧＰＳ時刻Ｔｇｐｓに戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて実行する。過去のＧＰＳ時刻Ｔｇｐｓは、例えば、図６においては、Ｔ＝１．０〔ｓ〕の時刻に相当する。その後、現在時刻まで、ＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて順次実行する。ＩＮＳ／車輪速協調測位演算は、各車両センサのサンプリング周期毎に順次行い、現在時刻まで積算し、現在時刻における車両位置を求める。そして、現在時刻における測位位置を出力し、処理フローを終了する。
【００８１】
一方、ステップ３２０では、ＧＰＳ遮断が発生していても、ＧＰＳ測位データを受信した場合のＧＰＳ時刻から、現在時刻までのＩＮＳ／車輪速協調測位演算を、カルマンフィルタ５０により補正された補正値を用いて順次実行する。この場合、例えば、図６においては、ＧＰＳ時刻はＴ＝２．０〔ｓ〕に相当し、これが初期時刻Ｔｐａｓｓになる。ＩＮＳ／車輪速協調測位演算は、初期時刻Ｔｐａｓｓも含めて各車両センサのサンプリング周期毎に順次行い、現在時刻まで積算し、現在時刻における車両位置を求める。そして、現在時刻における測位位置を出力し、処理フローを終了する。
【００８２】
なお、図７に示した処理フローは、移動体用測位装置１００の測位出力周期毎に実行され、これはＧＰＳ測位手段１０のデータ更新周期と一致する。よって、例えば、ＧＰＳ測位手段１０のＧＰＳ演算データの更新周期が１秒の場合は、１秒毎に図７の処理フローが実行され、繰り返される。
【００８３】
このように、実施例２に係る移動用測位装置１００によれば、カルマンフィルタ５０を最大限活用することにより、高精度に現在時刻における車両位置を測位することができる。
【実施例３】
【００８４】
実施例３に係る移動体用測位装置１００は、実施例１と実施例２の混合的な演算処理を行う。実施例３に係る移動体用測位装置１００は、図１における全体構成図においては、車両モデル演算手段６０と、演算プロファイル決定手段９０とを備え、誤差分散補正部７０と、ＧＰＳ演算データ更新判定手段８０は、必ずしも必要とされない実施形態である。
【００８５】
実施例２においては、リアルタイム性の確保のために、現在時刻まで追い付くための演算処理でカルマンフィルタ５０を連続的に用いるため、演算負荷が増大する場合がある。このような場合を考慮し、実施例３においては、実施例２と同様にＧＰＳ演算データのデータ更新周期と移動体用測位装置１００の出力周期が一致する場合において、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算、車両モデルによる測位演算を混合状態で実行し、精度を確保しつつ演算負荷を低減させる制御の例について説明する。
【００８６】
図８は、実施例３に係る移動体用測位装置１００の演算プロファイル決定手段９０で実行される演算プロファイル決定のための演算処理内容について説明するための図である。図８において、横軸は、リアルタイム性確保のための追い付き演算の結果算出に要する時間を示し、縦軸は測位誤差の大きさを示している。また、横軸に平行に引かれた線ＰＭは、許容できる測位誤差の大きさを示している。
【００８７】
図８において、例えば、追い付き演算が２０回必要な場合について考える。追い付き演算には演算時間が必要で、この時間が長すぎると、リアルタイム性を損なってゆくことになる。特に、車速が大きい場合には、単位時間に車両が進む距離も大きいので、例えば、追い付き演算に要する時間が長くなると、実際位置との誤差は大きくなる。よって、許容測位誤差と車速に応じて、追い付き演算に割り当て可能な時間、つまり追い付き演算に費やせる許容時間は変化することになる。
【００８８】
図８において、許容時間をＴｐ秒とすると、車速が高くなったときには、Ｔｐ秒は左に移動して許容時間は小さくなり、車速が低くなったときには、Ｔｐ秒は右に移動して許容時間は大きくなることが分かる。ここで、更に、許容時間Ｔｐ秒を、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデルによる測位演算を演算回数で振り分けると、（１）、（２）式のように表すことができる。
【００８９】
【数１】
（１）、（２）式は、演算回数を２０回としたときの、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデルによる測位演算の振り分け条件式を示している。ａは、ＩＮＳ／車輪速協調測位演算の１回分の演算時間であり、ｘは、ＩＮＳ／車輪速協調測位演算の演算回数である。ｂは、車両モデルの１回分の演算時間であり、ｙは、車両モデルによる測位演算の演算回数である。ｃは、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の１回分の演算時間であり、ｚは、ＧＰＳ／ＩＮＳ／車輪速協調測位演算の演算回数である。
【００９０】
（２）式より、ｘ、ｙ、ｚの演算合計回数は２０回であり、これを割り振って（１）式を満たすように演算プロファイルを決定する。ＧＰＳ／ＩＮＳ／車輪速協調測位演算は、現在時刻がＧＰＳ演算データの更新タイミングであり、かつＧＰＳ遮断が発生していないときには実行でき、それ以外の場合は実行できないので、ｚ＝１、０のいずれかである。ＩＮＳ／車輪速協調測位演算は、カルマンフィルタ５０により検出値補正を行うので、精度は高いが、処理負荷が大きく、演算時間も大きくなる場合が多い。一方、車両モデルによる測位演算は、個々の演算毎に個別の補正までは行わないので、ＩＮＳ／車輪速協調測位演算よりも精度は低くなる場合が多いが、処理負荷は小さくなり、演算時間が小さくなる場合が多い。これは、設定された車両モデルにもよるが、そのような車両モデルを設定することは十分可能であり、また一般的である。よって、一般的には、ｂ＜ａとなる場合が多い。このような条件を考慮し、なるべく高精度の測位を実現し、かつ許容時間Ｔｐ内とするには、（１）を満たす範囲で、ｘを最大とするような設定とすればよい。これにより、許容時間Ｔｐの範囲内で、最も高精度で測位を行うことができる。
【００９１】
また、ｘが定まったときに、２０回の演算の中でＩＮＳ／車輪速協調測位演算をいつ行うかの割り振りがあるが、ｘを、車両センサ値の変化が大きいタイミングになるべく多く割り当てるようにしてもよい。つまり、加速度、角加速度、車輪速度等の車両センサ値の変化が大きい状態は、車両の挙動が大きい部分であり、車両モデルでは誤差が蓄積し易い部分である。よって、そのようなタイミングでは、なるべく精度の高いＩＮＳ／車輪速協調測位演算を実行するようにし、測位精度を高く保つようにすることが好ましい。
【００９２】
また、車両センサ値は、上述の加速度、角加速度、車輪速度の他、ステアリング角、ブレーキ油圧、アクセルペダル等のセンサ値を含んでよい。車両の挙動が大きい場合には、旋回が大きい場合や、急な加減速が行われた場合が含まれるので、ステアリング角や、ブレーキ油圧、アクセルペダル等によっても、車両の挙動が大きい状態であることを検出できるからである。
【００９３】
なお、図８及び（１）、（２）式で示した演算は、演算プロファイル決定手段９０で行うようにしてよい。
【００９４】
次に、図９を用いて、演算プロファイル決定手段９０で実行される、図８とは異なる演算プロファイル決定の演算処理内容について説明する。図９は、図８とは異なる演算プロファイル決定の演算処理内容の説明図である。
【００９５】
図９（ａ）は、ＧＰＳ演算データが、２秒前の車両位置を示している場合であって、現在時刻Ｔ＝３．０〔ｓ〕における車両位置を出力する場合の演算処理の時系列を示した図である。図９（ａ）において、Ｔ＝１．０〔ｓ〕においては、ＧＰＳ演算データが更新されており、ＧＰＳ演算データが存在する。よって、初期時間Ｔｐａｓｓ＝１．０〔ｓ〕においては、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。その後、図８で説明した演算プロファイルに従い、現在時刻Ｔ＝３．０〔ｓ〕まで、カルマンフィルタ５０を用いた車輪速／ＩＮＳ協調測位演算と、車両モデルを用いた測位演算とが混合で一気にまとめて計算され、出力される。このとき、図９（ａ）に示すように、追い付き演算に要する許容時間Ｔｐ内であれば、若干の遅れは許容される。
【００９６】
図９（ｂ）は、現在時刻Ｔ＝４．０〔ｓ〕における車両位置を出力する場合の演算処理の時系列を示した図である。図９（ｂ）において、ＧＰＳ演算データのデータ更新周期は１〔Ｈｚ〕、つまりＴ＝１．０〔ｓ〕であるので、Ｔ＝２．０〔ｓ〕において、本来的にはＧＰＳ演算データが更新されるタイミングであるが、ＧＰＳ遮断が発生し、ＧＰＳ演算データが更新されなかった場合が示されている。
【００９７】
かかる場合においては、今までの実施例の演算処理においては、Ｔ＝２．０〔ｓ〕の測位データを初期値として現在時刻Ｔ＝４．０〔ｓ〕までの追い付き演算が実行されていたが、Ｔ＝２．０〜３．０〔ｓ〕において、図９（ａ）と図９（ｂ）は同じ演算プロファイルを示している。このような場合には、図９（ｂ）におけるＰ１の部分が、現在時刻Ｔ＝３．０〔ｓ〕で実行した演算プロファイルと重複するので、カルマンフィルタ５０を用いた演算を実行しないようにする。そして、Ｐ２のＴ＝３．０〜４．０〔ｓ〕の部分についてのみ演算プロファイルを作成し、追い付き演算を実行するようにする。これにより、前の出力周期における測位データの出力の際に既に演算された重複する演算プロファイルについては、この演算処理を省くことができ、演算処理負担を軽減することができる。
【００９８】
次に、図１０を用いて、実施例３に係る移動体用測位装置１００の処理フローについて説明する。図１０は、実施例３に係る移動体用測位装置１００により実行される演算処理の処理フロー図である。
【００９９】
ステップ４００では、ＧＰＳ演算データが更新されたか否かが判定される。ＧＰＳ演算データが更新されたか否かの判定は、例えば、ＧＰＳ演算データ更新判定手段８０により実行されてもよい。
【０１００】
ステップ４００において、ＧＰＳ演算データが更新されていると判定されたときにはステップ４１０に進む。
【０１０１】
ステップ４１０では、演算プロファイル決定手段９０により、ＧＰＳ／ＩＮＳ／車輪速協調測位演算、ＩＮＳ／車輪速協調測位演算及び車両モデル測位演算を、どのような演算プロファイルで演算させるかを決定する。これは、図８において説明したように、必要に応じて（１）、（２）式のような演算式を用い、追い付き演算に費やす許容時間Ｔｐ、要求される測位精度、処理負荷等を考慮して、演算プロファイルを決定するようにしてよい。
【０１０２】
ステップ４２０では、Ｔｐａｓｓ＝Ｔｇｐｓで初期時刻ＴｐａｓｓがＧＰＳ時刻Ｔｇｐｓと一致するので、過去のＧＰＳ時刻Ｔｇｐｓに戻り、ＧＰＳ／ＩＮＳ／車輪速協調測位演算を実行する。それ以降は、ステップ４１０で決定された演算プロファイルに従って、現在時刻まで測位演算を順次実行し、現在の位置を求める。そして、測位位置を出力し、処理フローを終了する。
【０１０３】
一方、ステップ４００に戻り、ＧＰＳ演算データが更新されておらず、ＧＰＳ遮断の状態であると判定されたときには、ステップ４３０に進む。
【０１０４】
ステップ４３０では、演算プロファイル決定手段９０により、演算プロファイルの決定がなされるが、図９（ｂ）において説明したように、前回の出力周期時の演算で重複する部分のカルマンフィルタの演算は実行しないようにする。重複しない部分については、ＩＮＳ／車輪速協調測位演算と、車両モデル測位演算とをどのような演算プロファイルで演算させるかを決定する。
【０１０５】
ステップ４４０では、ステップ４３０で決定された演算プロファイルに従い、現在時刻まで測位演算を順次実行し、現在の位置を求める。そして、測位位置を出力し、処理フローを終了する。
【０１０６】
図９の処理フローは、移動体用測位装置１００の出力周期及びＧＰＳ測位手段１０のデータ更新周期毎に繰り返し実行される。
【０１０７】
実施例３に係る移動体用測位装置１００によれば、演算プロファイル決定手段９０を用いて、演算処理負荷と測位精度の双方を考慮して、測位精度を確保しつつ演算負荷を低減させ、リアルタイムにおける移動体の位置を測位することができる。
【０１０８】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなく、上述した実施例に種々の変形及び置換を加えることができる。
【０１０９】
特に、図１及び図２に係る実施例及び実施例１〜３においては、移動体用測位装置１００を、車両に適用した場合の演算処理内容について説明したが、車輪速を移動体の速度、車両モデルを移動体モデルとすれば、種々の移動体に本発明を適用することができる。
【図面の簡単な説明】
【０１１０】
【図１】本発明を適用した実施例に係る移動体用測位装置１００の全体構成図である。
【図２】カルマンフィルタ５０による誤差補正を適用したＧＰＳ／ＩＮＳ／車輪速及びＩＮＳ／車輪速の協調アルゴリズムの一例を示すフローチャートである。
【図３】実施例１に係る移動体用測位装置１００のＧＰＳ／ＩＮＳ協調測位演算の説明図である。図３（ａ）は、ＧＰＳ演算データのデータ更新周期を示した図である。図３（ｂ）は、車輪速／ＩＮＳ協調測位演算の時系列を示した図である。図３（ｃ）は、Ｔ＝１．０〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。図３（ｄ）は、Ｔ＝１．１〔ｓ〕を開始時刻とする車両モデル演算の時系列を示した図である。
【図４】ＧＰＳ演算データが２秒の遅れを持つ場合の測位演算ルーチンの説明図である。図４（ａ）は、現在時刻がＴ＝３．０〔ｓ〕のときの測位演算処理を示す図である。図４（ｂ）は、現在時刻がＴ＝３．１〔ｓ〕のときの測位演算処理を示す図である。図４（ｃ）は、現在時刻Ｔ＝３．９〔ｓ〕のときの測位演算処理を示す図である。
【図５】実施例１に係る移動体用測位装置１００の測位演算の処理フロー図である。
【図６】実施例２に係る移動体用測位装置１００の演算処理内容を説明図である。
【図７】実施例２に係る移動体用測位装置１００の演算処理内容の処理フロー図である。
【図８】実施例３に係る移動体用測位装置１００の演算プロファイル決定の説明図である。
【図９】図８とは異なる演算プロファイル決定の演算処理内容の説明図である。図９（ａ）は、現在時刻Ｔ＝３．０〔ｓ〕の車両位置を出力する場合の演算処理の時系列図である。図９（ｂ）は、現在時刻Ｔ＝４．０〔ｓ〕の車両位置を出力する場合の演算処理の時系列図である。
【図１０】実施例３に係る移動体用測位装置１００の演算処理の処理フロー図である。
【符号の説明】
【０１１１】
１０ ＧＰＳ測位手段
２０ 慣性航法測位手段
３０ 慣性航法用検出手段
４０ 車輪速センサ
５０ カルマンフィルタ
６０ 車両モデル演算手段
７０ 誤差分散補正部
８０ ＧＰＳ演算データ更新判定手段
９０ 演算プロファイル決定手段
１００ 移動体用測位装置
【特許請求の範囲】
【請求項１】
現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段と、前記速度を用いて移動体モデルを演算する移動体モデル演算手段とを有し、前記現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置であって、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと一致したときには、前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の前記ＧＰＳ演算データ更新時における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記ＧＰＳ演算データ更新時から前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記時間遅れ分遡った時刻における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記時間遅れ分遡った時刻から前記現在時刻までの移動体モデルを演算し、前記移動体の位置を測位することを特徴とする移動体用測位装置。
【請求項２】
前記移動体モデル演算手段は、前記出力周期毎に、前記移動体モデルを演算して前記移動体の位置を測位することを特徴とする請求項１に記載の移動体用測位装置。
【請求項３】
現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段とを有し、前記現在時刻における移動体の位置を測位し、前記データ更新周期で出力する移動体用測位装置であって、
前記データ更新周期において、前記ＧＰＳ演算データが更新されたか否かを判定するＧＰＳ演算データ更新判定手段を有し、
該ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていると判定されたときには、前記ＧＰＳ演算データ更新時における前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、前記ＧＰＳ演算データ更新後から前記現在時刻までの間は、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位し、
前記ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていないと判定されたときには、前記ＧＰＳ演算データが更新されるべき時刻から前記現在時刻までの間を、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位することを特徴とする移動体用測位装置。
【請求項４】
前記速度を用いて、移動体モデルを演算する移動体モデル演算手段と、
前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記移動体モデルを用いた測位演算との演算プロファイルを決定する演算プロファイル決定手段と、を更に有することを特徴とする請求項３に記載の移動体用測位装置。
【請求項５】
前記演算プロファイル決定手段は、前記移動体の挙動が大きいときには、前記移動体モデルを用いた測位演算以外の演算により前記演算プロファイルを決定することを特徴とする請求項４に記載の移動体用測位装置。
【請求項６】
前記誤差補正手段は、カルマンフィルタを含むことを特徴とする請求項１乃至５のいずれか一項に記載の移動体用測位装置。
【請求項７】
前記移動体は、車両であることを特徴とする請求項１乃至６のいずれか一項に記載の移動体用測位装置。
【請求項１】
現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段と、前記速度を用いて移動体モデルを演算する移動体モデル演算手段とを有し、前記現在時刻における移動体の位置を測位し、所定の出力周期で出力する移動体用測位装置であって、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと一致したときには、前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記移動体の前記ＧＰＳ演算データ更新時における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記ＧＰＳ演算データ更新時から前記現在時刻までの移動体モデルを演算して前記移動体の位置を測位し、
前記現在時刻が、前記ＧＰＳ演算データ更新時のタイミングと異なったときには、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて前記時間遅れ分遡った時刻における前記移動体の初期位置を算出し、該初期位置算出の際に前記誤差補正手段により算出された補正値を用いて、前記移動体モデル演算手段が前記時間遅れ分遡った時刻から前記現在時刻までの移動体モデルを演算し、前記移動体の位置を測位することを特徴とする移動体用測位装置。
【請求項２】
前記移動体モデル演算手段は、前記出力周期毎に、前記移動体モデルを演算して前記移動体の位置を測位することを特徴とする請求項１に記載の移動体用測位装置。
【請求項３】
現在時刻から所定の時間遅れを有するＧＰＳ演算データを、所定のデータ更新周期で更新するＧＰＳ測位手段と、慣性航法用検出手段により検出された慣性航法用検出値を用いて慣性航法により測位する慣性航法測位手段と、速度を検出する速度検出手段と、前記慣性航法用検出値、前記慣性航法測位手段の算出値及び前記速度の誤差を補正する誤差補正手段とを有し、前記現在時刻における移動体の位置を測位し、前記データ更新周期で出力する移動体用測位装置であって、
前記データ更新周期において、前記ＧＰＳ演算データが更新されたか否かを判定するＧＰＳ演算データ更新判定手段を有し、
該ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていると判定されたときには、前記ＧＰＳ演算データ更新時における前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度及び前記誤差補正手段を用いて前記移動体の初期位置を算出し、前記ＧＰＳ演算データ更新後から前記現在時刻までの間は、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位し、
前記ＧＰＳ演算データ更新判定手段により、前記ＧＰＳ演算データが更新されていないと判定されたときには、前記ＧＰＳ演算データが更新されるべき時刻から前記現在時刻までの間を、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いて、前記現在時刻における前記移動体の位置を測位することを特徴とする移動体用測位装置。
【請求項４】
前記速度を用いて、移動体モデルを演算する移動体モデル演算手段と、
前記ＧＰＳ演算データ、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記慣性航法測位手段、前記速度検出手段及び前記誤差補正手段を用いた測位演算と、前記移動体モデルを用いた測位演算との演算プロファイルを決定する演算プロファイル決定手段と、を更に有することを特徴とする請求項３に記載の移動体用測位装置。
【請求項５】
前記演算プロファイル決定手段は、前記移動体の挙動が大きいときには、前記移動体モデルを用いた測位演算以外の演算により前記演算プロファイルを決定することを特徴とする請求項４に記載の移動体用測位装置。
【請求項６】
前記誤差補正手段は、カルマンフィルタを含むことを特徴とする請求項１乃至５のいずれか一項に記載の移動体用測位装置。
【請求項７】
前記移動体は、車両であることを特徴とする請求項１乃至６のいずれか一項に記載の移動体用測位装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１０−１９７０３（Ｐ２０１０−１９７０３Ａ）
【公開日】平成２２年１月２８日（２０１０．１．２８）
【国際特許分類】
【出願番号】特願２００８−１８０６６７（Ｐ２００８−１８０６６７）
【出願日】平成２０年７月１０日（２００８．７．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２２年１月２８日（２０１０．１．２８）
【国際特許分類】
【出願日】平成２０年７月１０日（２００８．７．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]