JP2009041932A - 移動体用測位装置 - Google Patents

Abstract

【課題】衛星電波の受信状態に応じた適切な誤差分散を用いて移動体の位置を測位すること。
【解決手段】本発明による移動体用測位装置は、慣性航法により前記移動体の位置を測位する慣性航法測位手段と、衛星航法により前記移動体の位置を測位する衛星航法測位手段と、前記衛星航法測位手段の測位結果と前記慣性航法測位手段の測位結果との関係を観測量とし、前記慣性航法測位手段で用いられる補正パラメータを状態量として、前記衛星航法測位手段の測位結果の誤差分散を用いて、前記状態量を推定する状態量推定手段と、前記状態量推定手段により推定された状態量を前記慣性航法測位手段の測位に反映させる手段と、前記移動体で受信される衛星電波の受信状態に応じて前記誤差分散を補正する補正手段とを備えることを特徴とする。
【選択図】図３

Description

本発明は、移動体に搭載され、該移動体の位置を測位する移動体用測位装置に関する。

従来から、衛星航法（ＧＰＳ）から得た位置の誤差分散と、慣性航法（自立航法：ＩＮＳ）から得た位置の誤差分散とを導出し、両者の逆比に対応する比例配分係数に応じてＧＰＳ位置情報とＩＮＳ位置情報とを統合する技術が知られている（例えば、特許文献１参照）。

また、上記の特許文献１では、単にＧＰＳ位置情報とＩＮＳ位置情報とを最終段で結合させるものであるが、より緊密に衛星航法と慣性航法とを併用するＧＰＳ／ＩＮＳ協調測位により、車両位置を測位する車両位置測位装置も知られている（例えば、特許文献２参照）。この特許文献２には、慣性航法装置のドリフトの補正方法や衛星航法装置のオフセットの補正方法が開示されている。
特開平９−７２７４６号公報 特開平１−３１６６０７号公報

ところで、ＧＰＳ／ＩＮＳ協調測位を用いて移動***置を測位する場合、衛星航法で測位される移動***置の誤差分散を用いて、慣性航法装置のドリフト等のような、補正パラメータを推定し補正することが有用である。

しかしながら、衛星航法で測位される移動***置の誤差分散をそのまま用いる場合（即ち補正せずに用いる場合）、移動体で受信される衛星電波の受信状態によっては不適切な誤差分散を用いることになり、その結果、測位精度が悪化する虞がある。

そこで、本発明は、移動体で受信される衛星電波の受信状態に応じた適切な誤差分散を用いて移動体の位置を測位することができる移動体用測位装置の提供を目的とする。

上記目的を達成するため、第１の発明は、移動体に搭載され、該移動体の位置を測位する移動体用測位装置において、
慣性航法により前記移動体の位置を測位する慣性航法測位手段と、
衛星航法により前記移動体の位置を測位する衛星航法測位手段と、
前記衛星航法測位手段の測位結果と前記慣性航法測位手段の測位結果との関係を観測量とし、前記慣性航法測位手段で用いられる補正パラメータを状態量として、前記衛星航法測位手段の測位結果の誤差分散を用いて、前記状態量を推定する状態量推定手段と、
前記状態量推定手段により推定された状態量を前記慣性航法測位手段の測位に反映させる手段と、
前記移動体で受信される衛星電波の受信状態に応じて前記誤差分散を補正する補正手段とを備えることを特徴とする。

第２の発明は、第１の発明に係る移動体用測位装置において、
前記補正手段は、所定数以上の衛星が捕捉された場合には、前記誤差分散を補正せず、所定数未満の衛星が捕捉された場合に、前記誤差分散を補正することを特徴とする。

第３の発明は、第１又は２の発明に係る移動体用測位装置において、
前記補正手段は、所定数以上の衛星が捕捉された観測周期に対応した周期で前記慣性航法測位手段により測位される前記移動体の位置を基点として、その後の前記移動体の位置を推定する移動***置推定手段を備え、前記移動***置推定手段により推定された前記移動体の推定位置を、該推定位置に対応する周期で前記慣性航法測位手段により測位される前記移動体の測位位置と比較し、当該比較結果に基づいて、前記誤差分散を補正することを特徴とする。

第４の発明は、第３の発明に係る移動体用測位装置において、
前記移動***置推定手段は、所定数未満の衛星が捕捉された観測周期に対応した周期の前記移動体の位置を推定することを特徴とする。

第５の発明は、第３又は４の発明に係る移動体用測位装置において、
前記補正手段は、少なくとも２つの直交する方向成分で前記推定位置と前記測位位置との差分を導出し、該導出した各方向成分の差分に基づいて、対応する各方向成分に係る前記誤差分散を補正することを特徴とする。

第６の発明は、第５の発明に係る移動体用測位装置において、
前記補正手段は、前記導出した差分が大きい場合には、前記導出した差分が小さい場合に比べて高い増加割合で、前記誤差分散が増加する方向に前記誤差分散を補正することを特徴とする。

第７の発明は、第１〜６のうちのいずれかの発明に係る移動体用測位装置において、
前記補正手段は、衛星電波の受信結果に基づいて、少なくとも２つの直交する方向で衛星電波の受信状態を表す指標値を導出し、該導出した各方向成分の指標値に基づいて、対応する各方向成分に係る前記誤差分散を補正することを特徴とする。

第８の発明は、第７の発明に係る移動体用測位装置において、
前記指標値は、各方向を基準とした所定範囲内に存在する衛星数、及び、前記移動体における各衛星からの衛星電波の受信強度に基づくことを特徴とする。

本発明によれば、移動体で受信される衛星電波の受信状態に応じた適切な誤差分散を用いて移動体の位置を測位することができる移動体用測位装置が得られる。

以下、図面を参照して、本発明を実施するための最良の形態の説明を行う。

図１は、本発明による移動体用測位装置の一実施例を含む車両位置測位システム１００を示すシステム構成図である。

尚、以下では、移動体としての車両の位置を適切に測位する車両位置測位システムを例示するが、本発明は、車両以外の如何なる移動体にも適用可能である。その他の移動体としては、自動二輪車、鉄道、船舶、航空機、ホークリフト、ロボットや、人の移動に伴い移動する携帯電話等の情報端末等がありうる。

車両位置測位システム１００は、主要な構成要素として、衛星航法測位装置１１０と、慣性航法測位装置２００と、カルマンフィルタ（状態推定器）１２０と、車両モデル１３０と、誤差分散補正部１４０とを含む。

衛星航法測位装置１１０は、主たる構成要素としてＧＰＳ受信機及びＧＰＳアンテナ（図示せず）を備える。衛星航法測位装置１１０のＧＰＳ受信機は、ＧＰＳアンテナを介して入力される衛星信号に基づいて、車両位置及び車両速度を測位する。車両位置及び車両速度は、いわゆる単独測位法で測位されてもよい。尚、ここでは、車両位置及び車両速度は、緯度、経度及び高度の座標系（ｌｌｈとＮＥＤ）で求められるものとする。また、ＧＰＳ受信機は、測位過程で車両位置及び車両速度の誤差分散を算出する。誤差分散は、緯度、経度及び高度の成分毎に導出される。誤差分散の算出方法は、任意の適切な方法が採用されてよい。衛星航法測位装置１１０により算出される誤差分散は、測位周期毎に、誤差分散補正部１４０に入力される。

慣性航法測位装置２００は、図１に示すように、３軸加速度センサ及び３軸角速度センサを含むＩＭＵ（慣性測定ユニット）２０２の出力信号に基づいて、車両位置、速度や車両姿勢（方位角）を算出する。慣性航法による車両位置の測位方法は、多種多様でありえ、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値（車両位置測位システム１００の最終的な測位結果の前回値のフィードバック）に積算することで導出されてよい。尚、ここでは、慣性航法測位装置２００においても、車両位置及び車両速度は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。慣性航法測位装置２００により測位される車両位置及び車両速度（ＩＮＳ仮測位結果）は、測位周期毎に、衛星航法測位装置１１０により測位される車両位置及び車両速度（ＧＰＳ測位結果）との差分値が取られ、当該差分値がカルマンフィルタ１２０に入力される。即ち、車両位置の差及び車両速度の差が、観測量ｚとしてカルマンフィルタ１２０に入力される。この際、ＧＰＳ測位結果及びＩＮＳ仮測位結果は、時間的に同期した態様で差分が取られる（例えばＧＰＳ時刻を基準として同期させて差分が取られる）。

カルマンフィルタ１２０は、ＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値を入力として、ＧＰＳ測位結果及びＩＮＳ仮測位結果のそれぞれの信頼性により確率的に最も正しい値になるように、状態量であるＩＮＳ補正値ηを推定する。ＩＮＳ補正値ηは、車両位置及び速度に関する補正値に加えて、車両姿勢、加速度センサ及び角速度センサのバイアス、ドリフトに関する補正値を含んでよい。カルマンフィルタ１２０では、例えば、以下のようにして、ＩＮＳ補正値ηが推定される。状態方程式は、以下のように設定される。
η（ｔ_ｎ）＝Ｆ・η（ｔ_ｎ−１）＋Ｇ・ｕ（ｔ_ｎ−１）＋Γ・ｗ（ｔ_ｎ−１）
ここで、η（ｔ_ｎ）は、時刻ｔ＝ｔ_ｎでの状態変数を表わし、また、ｕ（ｔ_ｎ−１）及びｗ（ｔ_ｎ−１）は、それぞれ、時刻ｔ＝ｔ_ｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔ_ｎ）は、慣性航法測位装置２００で推定される車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）のそれぞれの誤差δｒ（ＩＮＳ）及びδｖ（ＩＮＳ）、慣性航法測位装置２００で推定される車両の姿勢の誤差δε（ＩＮＳ）、ＩＭＵ２０２の角速度センサのバイアス誤差δｂ、ＩＭＵ２０２の加速度センサのドリフト誤差δｄ、及び、車両のタイヤの半径誤差δｓ等を含んでよい。

また、観測方程式は、次のように設定される。
ｚ（ｔ_ｎ）＝Ｈ（ｔ_ｎ）・η（ｔ_ｎ）＋ｖ（ｔ_ｎ）
観測量ｚ（ｔ_ｎ）は、時刻ｔ＝ｔ_ｎ−１でのＧＰＳ測位結果及びＩＮＳ仮測位結果の差分値である。Ｈ（ｔ_ｎ）は観測行列であり、ｖ（ｔ_ｎ）は観測雑音である。

車両モデル１３０は、車載の各種センサの出力を入力として、車両の位置を推定するモデルである。この種の車両モデルの構築方法は、種々提案されており、如何なる方法であってもよい。図示の車両モデル１３０は、車輪速センサ１３２の出力を入力として、位置が既知の基点（後述）からの車両の移動量を推定して車両の位置を推定するモデルである。尚、ここでは、車両モデル１３０においても、推定車両位置は、緯度、経度及び高度の座標系（ＮＥＤ）で求められるものとする。車両の移動量が、例えばWGS84に基づく地球固定座標系等のような他の座標系で算出されている場合、当該移動量を座標変換し、当該座標変換した移動量を、経度及び高度の座標系の基点に積算することで、経度及び高度の座標系の推定車両位置を算出することができる。車両モデル１３０により推定された車両位置は、誤差分散補正部１４０に入力される。

誤差分散補正部１４０は、車両で受信される衛星電波の受信状態に応じて、衛星航法測位装置１１０から入力される誤差分散を補正する。誤差分散補正部１４０は、補正した誤差分散をカルマンフィルタ１２０に入力する。カルマンフィルタ１２０では、誤差分散補正部１４０から入力される誤差分散が観測ノイズの分散として用いられて、状態量（ＩＮＳ補正値η）が推定される。誤差分散補正部１４０の誤差分散補正処理については、後に詳説する。

図２は、本実施例の車両位置測位システム１００により実現される測位演算処理を示すフローチャートである。

ステップ３００では、ＩＭＵ２０２の出力値（例えば３軸方向の加速度、及び、３軸まわりの角速度）がサンプリングされる。ＩＭＵ２０２の出力値は、後述のステップ３５０のカルマンフィルタ１２０により推定されるＩＮＳ補正値ηに基づいて補正を受けてよい。即ち、ＩＭＵ２０２の出力値のバイアスやドリフトの誤差が補正されてよい。

ステップ３１０では、慣性航法測位装置２００において、上記ステップ３００で得られるＩＭＵの出力値に基づいて、車両推定位置及び車両推定速度、姿勢等が導出される。

ステップ３２０では、カルマンフィルタ１２０においてカルマンフィルタ１２０の時間更新が実行される。カルマンフィルタ１２０の時間更新は、例えば以下のように表される。
η（ｔ_ｎ）^（−）＝η（ｔ_ｎ−１）^（＋）＋ｕ（ｔ_ｎ−１）
Ｐ（ｔ_ｎ）^（−）＝Ｆ・Ｐ（ｔ_ｎ−１）^（＋）・Ｆ^Ｔ＋Γ・Ｑ（ｔ_ｎ−１）・Γ^Ｔ
尚、Ｐは、予測・推定誤差の共分散行列であり、Ｑは、外乱ｗの共分散行列（正定値対称行列）である。尚、符号（＋）及び（−）は更新前後を表す。

ステップ３３０では、今回周期（ｔ_ｎ）の観測量ｚ（ｔ_ｎ）に基づいて観測行列Ｈ（ｔ_ｎ）が演算される。

ステップ３４０では、カルマンゲインＫ_ｋが以下のように演算される。
Ｋ（ｔ_ｎ）＝Ｐ（ｔ_ｎ）^（−）・Ｈ^Ｔ（ｔ_ｎ）・（Ｈ（ｔ_ｎ）・Ｐ（ｔ_ｎ）^（−）・Ｈ^Ｔ（ｔ_ｎ）＋Ｒ（ｔ_ｎ））^−１
ここで、Ｒ（ｔ_ｎ）は、観測ノイズの分散行列である。Ｒ（ｔ_ｎ）は、誤差分散補正部１４０で生成され、次の通りである。
Ｒ（ｔ_ｎ）＝Ｍ_ｋ（ｔ_ｎ）・Ｗ_ｋ（ｔ_ｎ）
ここで、Ｍ_ｋ（ｔ_ｎ）は、非対角成分がゼロの分散更新用行列であり、デフォルトは単位行列である。分散更新用行列Ｍ_ｋ（ｔ_ｎ）の対角成分は、分散更新用ゲインであり、これについては後に詳説する。Ｗ_ｋ（ｔ_ｎ）は、非対角成分がゼロの行列であり、対角成分には、衛星航法測位装置１１０により算出される誤差分散の各成分（緯度成分、経度成分、高度成分）が代入される。

ステップ３５０では、上記のステップ３４０で得られるカルマンゲインＫ（ｔ_ｎ）に基づいて、状態量η（ｔ_ｎ）が以下のように算出される。
η（ｔ_ｎ）^（＋）＝η（ｔ_ｎ）^（−）＋Ｋ（ｔ_ｎ）・（ｚ（ｔ_ｎ）−Ｈ（ｔ_ｎ）・η（ｔ_ｎ）^（−））
ステップ３６０では、上記のステップ３５０で導出された状態量η（ｔ_ｎ）に基づいて、誤差補正が実行される。即ち、ＩＭＵ２０２の出力値のバイアス及びドリフト補正が実行され、慣性航法測位装置２００において車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）、姿勢の補正が実行される。その結果、誤差補正後の車両推定位置等が最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）として得られる。尚、上記のステップ３００乃至３７０の処理は、その周期のＩＭＵ２０２の出力値に基づいて、例えば状態量η（ｔ_ｎ）が収束するまで繰り返し実行されてもよい。また、車輪速センサ１３２のスケーリングファクタ（車両モデル）についても、状態量η（ｔ_ｎ）に含まれるタイヤ半径誤差δｓの推定値に基づいて、補正されてよい。

ステップ３７０では、上記のステップ３４０で得られるカルマンゲインＫ（ｔ_ｎ）に基づいて、共分散行列Ｐが以下のように更新される。
Ｐ（ｔ_ｎ）^（＋）＝Ｐ（ｔ_ｎ）^（−）−Ｋ（ｔ_ｎ）・Ｈ（ｔ_ｎ）・Ｐ（ｔ_ｎ）^（−）
図３は、本実施例の車両位置測位システム１００により実現される主要処理を示すフローチャートである。

ステップ４００では、誤差分散補正部１４０において、衛星航法測位装置１１０のＧＰＳ受信機で受信される衛星数（捕捉衛星数）が所定数以上か否かが判定される。所定数は、衛星航法測位装置１１０による測位に必要な最小限の衛星数よりも十分大きい数であり、例えば８〜１０の範囲内の数であってよい。なお、地図データ等に基づいて、車両が、交差点や低層市街地のような電波障害物が周囲に存在しない領域に存在すると判断できる場合に、捕捉衛星数が所定数以上であると推定してもよい。或いは、逆に、車両が、高層ビル等が林立する地域に存在すると判断できる場合に、捕捉衛星数が所定数未満であると推定してもよい。捕捉衛星数が所定数以上である場合には、ステップ４０２に進む。捕捉衛星数が所定数未満の場合には、ステップ４０８に進む。尚、捕捉衛星数が所定数未満の場合でも、捕捉衛星数が衛星航法測位装置１１０の測位に必要な最小衛星数（例えば４）より少ない場合には、今回周期の誤差分散が得られないので、今回周期の処理は終了する。他言すると、捕捉衛星数が所定数未満であって、捕捉衛星数が衛星航法測位装置１１０の測位に必要な最小衛星数と同じ若しくはそれ以上の場合に、ステップ４０８に進む。

ステップ４０２では、誤差分散補正部１４０において、今回周期のフラグＦｌａｇがオンにセットされる。

ステップ４０４では、前回周期のフラグＦｌａｇ＿ｏｌｄがオフであった場合には、誤差分散補正部１４０において、今回周期（ｔ_ｎ）の分散更新用行列Ｍ_ｋ（ｔ_ｎ）が単位行列にリセットされる。この場合、観測ノイズの分散行列Ｒ（ｔ_ｎ）は、今回周期で衛星航法測位装置１１０から入力される誤差分散に基づく行列Ｗ_ｋ（ｔ_ｎ）となる。即ち、今回周期で衛星航法測位装置１１０から入力される誤差分散は実質的に補正されない。

ステップ４０６では、慣性航法測位装置２００及びカルマンフィルタ１２０において、上述の図２の測位演算処理に従って、上記のステップ４０４で生成された分散更新用行列Ｍ_ｋ（ｔ_ｎ）を用いて、車両位置が測位される。即ち、今回周期で衛星航法測位装置１１０から入力される誤差分散に基づく行列Ｗ_ｋ（ｔ_ｎ）を観測ノイズの分散行列Ｒ（ｔ_ｎ）として、車両位置が測位される。この結果得られる車両位置（ＧＰＳ／ＩＮＳ協調測位結果）は、今後の測位の基点として用いられる。ここで、複数周期継続して捕捉衛星数が所定数以上存在する場合、衛星航法測位装置１１０から入力される誤差分散が最小（極小）となる周期で測位された車両位置が基点として用いられてもよい。例えば、交差点通過時の車速と交差点の地図データ（交差点の大きさ等を示す地図データ）から、交差点通過に要する時間を予測し、当該予測される通過時間分のバッファを作成する。そして、予測通過時間中の各周期で衛星航法測位装置１１０から入力される誤差分散（車両位置の誤差分散）と、予測通過時間中の各周期で得られるＧＰＳ／ＩＮＳ協調測位結果を随時保存し、誤差分散に関してはフィルタリングする。そして、交差点終盤に車両が差し掛かったと判断（ＧＰＳ／ＩＮＳ協調測位結果と地図データとの比較等により判断）した場合に、バッファ内から誤差分散が最小となる周期に係るＧＰＳ／ＩＮＳ協調測位結果を抽出し、それを基点としてもよい。

ステップ４０８では、誤差分散補正部１４０において、今回周期のフラグＦｌａｇがオフにセットされる。

ステップ４１０では、誤差分散補正部１４０において、今回周期で衛星航法測位装置１１０から入力される誤差分散Ｗ_ｋ（ｔ_ｎ）に、分散更新用行列Ｍ_ｋ（ｔ_ｎ）を乗じて、観測ノイズの分散行列Ｒ（ｔ_ｎ）が生成される。即ち、今回周期で衛星航法測位装置１１０から入力される誤差分散が実質的に補正される。そして、慣性航法測位装置２００及びカルマンフィルタ１２０において、誤差分散補正部１４０で生成された観測ノイズの分散行列Ｒ（ｔ_ｎ）を用いて、上述の図２の測位演算処理に従って、車両位置が測位される。即ち、今回周期で衛星航法測位装置１１０から入力される誤差分散に、分散更新用行列Ｍ_ｋ（ｔ_ｎ）を乗じて得られる行列を観測ノイズの分散行列Ｒ（ｔ_ｎ）として、車両位置（ＧＰＳ／ＩＮＳ協調測位結果）が測位される。

ステップ４１２では、前回周期のフラグＦｌａｇ＿ｏｌｄがオンであった場合には、誤差分散補正部１４０において、前回周期にてステップ４０６で得られる車両位置（ＧＰＳ／ＩＮＳ協調測位結果）が基点に設定される。

ステップ４１４では、誤差分散補正部１４０において、上記のステップ４０４又は４１０で得られるＧＰＳ／ＩＮＳ協調測位結果と共に基点に関する情報が、車両モデル１３０に供給され、或いは、車両モデル１３０がアクセス可能なメモリに保存される。また、フラグＦｌａｇ＿ｏｌｄが更新される。即ち、次回周期の処理に備えて、フラグＦｌａｇ＿ｏｌｄ＝今回周期のフラグＦｌａｇとして更新される。

ステップ４１６では、誤差分散補正部１４０において、今回周期のフラグＦｌａｇがオンであるか否かが判定される。今回周期のフラグＦｌａｇがオンである場合、即ち今回周期の捕捉衛星数が所定数以上である場合には、今回周期の処理は終了する。この場合、上記のステップ４０４で得られた車両位置が、最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）として、例えばナビゲーション装置（図示せず）に供給される。一方、今回周期のフラグＦｌａｇがオフである場合、即ち今回周期の捕捉衛星数が所定数未満である場合には、ステップ４１８に進む。

ステップ４１８では、車両モデル１３０において、上記のステップ４１２で設定された基点を用いて、車両位置が推定される。例えば、基点の位置（既知）と、基点からの移動量及び方向に基づいて、今回周期の車両位置が推定される。車両位置の推定には、車両モデル１３０に代えて若しくは加えて、ナビゲーション装置の地図データ、車載カメラの撮像データ、車車間通信で得られるデータ、路側に設置される各種設備（インフラ）から得られるデータ等が用いられてもよい。

ステップ４２０では、誤差分散補正部１４０において、上記のステップ４１０で得られるＧＰＳ／ＩＮＳ協調測位結果と、上記のステップ４１８で得られる推定車両位置との緯度成分の差分ａｂｓ（ｄφ）及び経度成分の差分ａｂｓ（ｄλ）が演算される。また、これに加えて、高度成分の差分（ｄｈ）が演算されてもよい。

ステップ４２２では、誤差分散補正部１４０において、分散更新用行列Ｍ_ｋ（ｔ_ｎ）が、上記ステップ４２０で得られた差分ａｂｓ（ｄφ）及び横方向の差分ａｂｓ（ｄλ）に応じて、設定される。例えば、車両位置（（車両位置の誤差分散）に関する分散更新用行列Ｍ_ｋ（ｔ_ｎ）の対角成分、即ち分散更新用ゲインは、（１＋Ｋ_φ・ａｂｓ（ｄφ）、１＋Ｋ_λ・ａｂｓ（ｄλ）、１＋Ｋ_ｈ・ａｂｓ（ｄｈ））とされる。尚、上記のステップ４２０で、高度成分の差分（ｄｈ）が演算されない場合には、分散更新用ゲインは、（１＋Ｋ_φ・ａｂｓ（ｄφ）、１＋Ｋ_λ・ａｂｓ（ｄλ）、１）とされてよい。ここで、Ｋ_φ、Ｋ_λ及びＫ_ｈは、所定の係数（ゲイン）を表し、固定値であってもよいし、可変値であってもよい。このようにして得られた分散更新用ゲインは、上記のステップ４１０の処理に反映される。この結果、誤差補正後の車両推定位置等が最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）として得られる。尚、上記のステップ４１０，４２０及び４２２の処理は、その周期の観測量ｚ及び誤差分散に基づいて、例えば各差分（ひいては分散更新用ゲイン）が収束するまで繰り返し実行されてもよい。このようにして得られた最終的な今回周期の測位結果（ＧＰＳ／ＩＮＳ協調測位結果）は、例えばナビゲーション装置（図示せず）に供給される。

図４は、図３の処理が実行される典型的な場面の一例を示し、車両の移動する道路環境を概略的に示す平面図である。尚、図４において、黒丸マークは、基点（図３のステップ４０６，４１２参照）を表し、×マークは、車両モデル１３０により基点を元に推定された推定車両位置（図３のステップ４１８参照）を表し、黒三角マークは、ＧＰＳ／ＩＮＳ協調測位結果（図３のステップ４１０参照）を表す。図４に示す道路環境では、交差点の各角に高層なビルが林立している。ここでは、交差点内に車両が位置（図の実線の位置）するときに捕捉衛星数が所定数以上となり、それに伴い、基点が演算・設定され、車両が交差点を通過した後（図の点線の位置以降）、捕捉衛星数が所定数未満となることとする。また、図４の上下方向を緯度方向（実質的に南北方向に対応）とし、図４の左右方向を経度方向（実質的に東西方向に対応）とする。また、図４には、対比用に、本実施例により補正される誤差分散を表す領域と、補正されない誤差分散を表す領域とが模式的に楕円形領域で示されている。

図４の例では、車両が交差点を通過した後、ＧＰＳ／ＩＮＳ協調測位結果と推定車両位置の差分が徐々に大きくなり、この際、経度成分の差分ａｂｓ（ｄλ）が、緯度成分の差分ａｂｓ（ｄφ）よりも大きくなっている。かかる場合、本実施例では、経度成分に係る分散更新用ゲインの方が、緯度成分に係る分散更新用ゲインよりも大きく補正される。即ち、経度成分に係る分散更新用ゲインの補正割合が、緯度成分に係る分散更新用ゲインの補正割合よりも大きくなる。従って、仮にＫ_φ及びＫ_λが同一の値の場合、経度成分に係る分散更新用ゲインの方が、緯度成分に係る分散更新用ゲインよりも大きくなる。このように、本実施例によれば、ＧＰＳ／ＩＮＳ協調測位結果と推定車両位置の差分を、緯度方向及び経度方向（選択的に高度方向）に分解することで、その各方向の差分に基づいて、衛星航法測位装置１１０から入力される誤差分散（車両位置の誤差分散）を適切な値へと補正することができる。

尚、本実施例では、上述の如く、ＧＰＳ／ＩＮＳ協調測位結果と推定車両位置の差分を、緯度方向及び経度方向（選択的に高度方向）に分解しているが、任意の２方向以上の方向に分解してもよい。例えば、ＧＰＳ／ＩＮＳ協調測位結果と推定車両位置の差分は、車両の進行方向（若しくは前後方向）と横方向に分解されてもよい。即ち、ＧＰＳ／ＩＮＳ協調測位結果及び推定車両位置が、例えばWGS84に基づく地球固定座標系で算出されている場合、車両の進行方向の差分ａｂｓ（ｄｘ）、及び、横方向の差分ａｂｓ（ｄｙ）が、緯度成分の差分ａｂｓ（ｄφ）及び経度成分の差分ａｂｓ（ｄλ）に代えて演算されてもよい。同様に、車両の上下方向の差分ａｂｓ（ｄｚ）が、高度成分の差分（ｄｈ）に代えて演算されてもよい。

図５は、誤差分散補正部１４０による上述の分散更新用ゲインの係数Ｋ_φ、Ｋ_λの可変設定処理の一例を示すフローチャートである。図６は、図５の説明図であり、緯度方向と経度方向の距離閾値を示す。

図５を参照するに、ステップ５００では、衛星航法測位装置１１０で受信される各ＧＰＳ衛星の電波強度及び衛星位置に関する情報に基づいて、電波強度が所定閾値以上であり、且つ、緯度方向の位置が所定閾値距離（緯度方向閾値距離）内であるＧＰＳ衛星の個数Ｃ１が算出される。尚、衛星位置に関する情報は、航法メッセージに含まれる衛星軌道情報（エフェメリスやアルマナク）に基づくものであってよい。ここで、緯度方向閾値距離は、図６にて示すように、緯度方向を基準として一定の幅を有する距離（範囲）であってよい。

ステップ５０２では、同様に、衛星航法測位装置１１０で受信される各ＧＰＳ衛星の電波強度及び衛星位置に関する情報に基づいて、電波強度が所定閾値以上であり、且つ、経度方向の位置が所定閾値距離範囲内であるＧＰＳ衛星の個数Ｃ２が算出される。ここで、経度方向閾値距離は、図６にて示すように、経度方向を基準として一定の幅を有する距離（範囲）であってよい。

ステップ５０４では、上記のステップ５００で算出された衛星個数Ｃ１と上記のステップ５０２で算出された衛星個数Ｃ２が比較され、Ｃ１−Ｃ２が所定の衛星個数閾値より大きい場合には、ステップ５０６に進む。尚、衛星個数閾値は、例えば５以上の比較的有意な差を表すものであってもよい。

ステップ５０６では、Ｃ１−Ｃ２が所定の衛星個数閾値より大きいので、緯度方向に電波状況が良好であると判断する。

ステップ５０８では、緯度方向に電波状況が良好であるので、緯度方向に係る分散更新用ゲインの係数Ｋ_φを小さく設定し、緯度方向に係る誤差分散（車両位置の誤差分散）の補正割合が小さくされる。これは、仮に同じ測位分散σ^２を持ったＧＰＳ衛星がＮ個観測可能であったときのトータルの位置分散はσ^２／（Ｎ−１）で表せ、Ｎが大きいほど分散が小さくなるからである。即ち、例えば緯度方向で観測可能なＧＰＳ衛星の数が、経度方向で観測可能なＧＰＳ衛星の数よりも多い場合には、緯度方向の位置分散が小さくなる。

尚、図５のフローチャートは、緯度方向に係る誤差分散の補正割合を小さくする処理に関するものであるが、経度方向に係る誤差分散の補正割合を小さくする処理についても同様である。即ち、Ｃ２−Ｃ１が所定の衛星個数閾値より大きい場合には、経度方向に電波状況が良好であると判断し、経度方向に係る分散更新用ゲインの係数Ｋ_λを小さく設定し、経度方向に係る誤差分散（車両位置の誤差分散）の補正割合が小さくすればよい。尚、｜Ｃ１−Ｃ２｜が衛星個数閾値より小さい場合には、各分散更新用ゲインの係数Ｋ_φ、Ｋ_λは、所定のデフォルト値に維持されてもよい。

尚、図５に示す処理は、上述の図３の処理のステップ４２２で用いる分散更新用ゲインの係数Ｋ_φ、Ｋ_λの設定方法に関するものであるが、上述の図３の処理とは独立して実現されてもよい。例えば、分散更新用ゲインを、（１＋Ｋ_φ、１＋Ｋ_λ、１＋Ｋ_ｈ）とし、差分ａｂｓ（ｄφ）等を演算しないこととしてもよい。この場合、例えば緯度方向に電波状況が良好である場合に、分散更新用ゲインの係数Ｋ_φを小さく設定することで（例えば係数Ｋ_φをゼロにすることで）、緯度方向に係る誤差分散の補正割合を小さくしてよい。

尚、上述の実施例においては、添付の特許請求の範囲の「補正手段」は、誤差分散補正部１４０及び車両モデル１３０により実現され、同特許請求の範囲の「移動***置推定手段」は、車両モデル１３０により実現されている。

以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなく、上述した実施例に種々の変形及び置換を加えることができる。

例えば、上述の実施例では、ＧＰＳに本発明が適用された例を示したが、本発明は、ＧＰＳ以下の衛星システム、例えばガリレオ等の他のGNSS(Global Navigation Satellite System)にも適用可能である。

本発明による移動体用測位装置の一実施例を含む車両位置測位システム１００を示すシステム構成図である。 本実施例の車両位置測位システム１００により実現される測位演算処理を示すフローチャートである。 本実施例の車両位置測位システム１００により実現される主要処理を示すフローチャートである。 図３の処理の説明図である。 誤差分散補正部１４０による分散更新用ゲインの係数Ｋ_φ、Ｋ_λの可変設定処理の一例を示すフローチャートである。 図５の処理の説明図である。

符号の説明

１００ 車両位置測位システム
１１０ 衛星航法測位装置
１２０ カルマンフィルタ
１３０ 車両モデル
１４０ 誤差分散補正部
２００ 慣性航法測位装置

Claims (8)

1. 移動体に搭載され、該移動体の位置を測位する移動体用測位装置において、
   慣性航法により前記移動体の位置を測位する慣性航法測位手段と、
   衛星航法により前記移動体の位置を測位する衛星航法測位手段と、
   前記衛星航法測位手段の測位結果と前記慣性航法測位手段の測位結果との関係を観測量とし、前記慣性航法測位手段で用いられる補正パラメータを状態量として、前記衛星航法測位手段の測位結果の誤差分散を用いて、前記状態量を推定する状態量推定手段と、
   前記状態量推定手段により推定された状態量を前記慣性航法測位手段の測位に反映させる手段と、
   前記移動体で受信される衛星電波の受信状態に応じて前記誤差分散を補正する補正手段とを備えることを特徴とする、移動体用測位装置。
2. 前記補正手段は、所定数以上の衛星が捕捉された場合には、前記誤差分散を補正せず、所定数未満の衛星が捕捉された場合に、前記誤差分散を補正する、請求項１に記載の移動体用測位装置。
3. 前記補正手段は、所定数以上の衛星が捕捉された観測周期に対応した周期で前記慣性航法測位手段により測位される前記移動体の位置を基点として、その後の前記移動体の位置を推定する移動***置推定手段を備え、前記移動***置推定手段により推定された前記移動体の推定位置を、該推定位置に対応する周期で前記慣性航法測位手段により測位される前記移動体の測位位置と比較し、当該比較結果に基づいて、前記誤差分散を補正する、請求項１又は２に記載の移動体用測位装置。
4. 前記移動***置推定手段は、所定数未満の衛星が捕捉された観測周期に対応した周期の前記移動体の位置を推定する、請求項３に記載の移動体用測位装置。
5. 前記補正手段は、少なくとも２つの直交する方向成分で前記推定位置と前記測位位置との差分を導出し、該導出した各方向成分の差分に基づいて、対応する各方向成分に係る前記誤差分散を補正する、請求項３又は４に記載の移動体用測位装置。
6. 前記補正手段は、前記導出した差分が大きい場合には、前記導出した差分が小さい場合に比べて高い増加割合で、前記誤差分散が増加する方向に前記誤差分散を補正する、請求項５に記載の移動体用測位装置。
7. 前記補正手段は、衛星電波の受信結果に基づいて、少なくとも２つの直交する方向で衛星電波の受信状態を表す指標値を導出し、該導出した各方向成分の指標値に基づいて、対応する各方向成分に係る前記誤差分散を補正する、請求項１〜６のうちのいずれか１項に記載の移動体用測位装置。
8. 前記指標値は、各方向を基準とした所定範囲内に存在する衛星数、及び、前記移動体における各衛星からの衛星電波の受信強度に基づく、請求項７に記載の移動体用測位装置。

Images