(57)【要約】【課題】早期に擬似距離のばらつきを収束させること。【解決手段】本発明による移動体用測位装置１は、慣性航法測位部６０と、衛星と移動体の間の擬似距離を計測するＧＰＳ受信機２０と、擬似距離推測部８０と、衛星軌道情報に係る衛星の位置の分散と、慣性航法測位部６０により導出された前記移動体の位置の分散と、ＧＰＳ受信機２０の時計誤差の分散とに基づいて、擬似距離推測部８０により推測された擬似距離ρ推測の分散ρ推測を算出する分散算出部３０と、を備える。好ましくは、ＧＰＳ受信機２０により計測された擬似距離ρと、擬似距離推測部８０により推測された擬似距離ρ推測とを結合して、結合擬似距離ρＣＯＭを算出する擬似距離結合部４０と、この結合擬似距離ρＣＯＭを用いて移動体位置を測位する測位演算部５０とを更に備え、擬似距離結合部４０は、分散算出部３０により算出された分散ρ推測を用いて、結合擬似距離ρＣＯＭを算出する。【選択図】図２
2008232761.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載され、該移動体の位置を測位する移動体用測位装置において、
慣性航法を用いて前記移動体の位置を算出する慣性航法測位手段と、
衛星からの電波に乗せられたコード情報と、受信機時計の時刻情報とに基づいて、該衛星と前記移動体の間の擬似距離を計測する擬似距離計測手段と、
前記擬似距離計測手段により計測される擬似距離を、衛星軌道情報に基づく衛星の位置の算出値と、前記慣性航法測位手段による前記移動体の位置の算出値と、前記受信機時計の時計誤差の推定値とに基づいて推測する擬似距離推測手段と、
前記衛星軌道情報に係る衛星の位置の分散と、前記慣性航法測位手段により算出された前記移動体の位置の分散と、前記時計誤差の分散とに基づいて、前記擬似距離推測手段により推測された擬似距離の分散を算出する分散算出手段と、を備えることを特徴とする移動体用測位装置。
【請求項２】
前記擬似距離計測手段により計測された擬似距離と、前記擬似距離推測手段により推測された擬似距離とを結合して、結合擬似距離を算出する結合擬似距離算出手段と、
前記結合擬似距離算出手段により算出された前記結合擬似距離を用いて移動体位置を測位する測位演算手段とを更に備え、
前記結合擬似距離算出手段は、前記分散値算出手段により算出された分散を用いて、前記擬似距離の結合処理を行う、請求項１に記載の移動体用測位装置。
【請求項３】
前記擬似距離推測手段により推測された擬似距離段及び擬似距離計測手段により計測された擬似距離は、タイトカップリング型のカルマンフィルタに入力される、請求項１に記載の移動体用測位装置。
【請求項４】
前記分散算出手段は、前記タイトカップリング型のカルマンフィルタで導出される誤差共分散行列の対角成分に基づいて、前記慣性航法測位手段により算出された前記移動体の位置の分散と、前記時計誤差の分散とを算出する、請求項３に記載の移動体用測位装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置を測位する移動体用測位装置に関する。
【背景技術】
【０００２】
従来から、ＧＰＳ受信機の観測情報と、自律航法（慣性航法）用複合センサの計測情報を用いて、車両の現在位置、進行方向及び速度等を計算する複合航法を備えたロケータ装置が知られている（例えば、特許文献１参照）。このロケータ装置では、慣性航法と衛星航法とをルーズカップリング型で融合するカルマンフィルタを用いて、車輪速センサのスケーリングファクタが補正されている。
【０００３】
また、近年では、慣性航法と衛星航法とを融合する際に、ＧＰＳ受信機の測位結果ではなく、ＧＰＳ受信機の観測生データ（例えば擬似距離の計測データ）を直接的に用いるタイトカップリング型カルマンフィルタが提案されている。
【特許文献１】特開２０００−３４６６６１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、実際のＧＰＳ衛星からの電波（衛星信号）には、ＧＰＳ衛星からＧＰＳ受信機までの間に、熱雑音（自然界に存在するノイズ）が加わるので、Ｃ／Ａコード等の擬似雑音符号に基づき擬似距離を算出する構成においては、擬似距離のばらつきを収束させるのに長い時間を要する。
【０００５】
この点、上述の背景技術に記載された文献では、センサのバイアス誤差やドリフト誤差等を補正して測位精度を高める観点から、ルーズカップリング型やタイトカップリング型により慣性航法と衛星航法とを融合させているが、早期に擬似距離のばらつきを収束させる観点から何ら提案がなされていない。
【０００６】
そこで、本発明は、早期に擬似距離のばらつきを収束させることができる移動体用測位装置の提供を目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、第１の発明は、移動体に搭載され、該移動体の位置を測位する移動体用測位装置において、
慣性航法を用いて前記移動体の位置を算出する慣性航法測位手段と、
衛星からの電波に乗せられたコード情報と、受信機時計の時刻情報とに基づいて、該衛星と前記移動体の間の擬似距離を計測する擬似距離計測手段と、
前記擬似距離計測手段により計測される擬似距離を、衛星軌道情報に基づく衛星の位置の算出値と、前記慣性航法測位手段による前記移動体の位置の算出値と、前記受信機時計の時計誤差の推定値とに基づいて推測する擬似距離推測手段と、
前記衛星軌道情報に係る衛星の位置の分散と、前記慣性航法測位手段により算出された前記移動体の位置の分散と、前記時計誤差の分散とに基づいて、前記擬似距離推測手段により推測された擬似距離の分散を算出する分散算出手段と、を備えることを特徴とする。
【０００８】
第２の発明は、第１の発明に係る移動体用測位装置において、
前記擬似距離計測手段により計測された擬似距離と、前記擬似距離推測手段により推測された擬似距離とを結合して、結合擬似距離を算出する結合擬似距離算出手段と、
前記結合擬似距離算出手段により算出された前記結合擬似距離を用いて移動体位置を測位する測位演算手段とを更に備え、
前記結合擬似距離算出手段は、前記分散値算出手段により算出された分散を用いて、前記擬似距離の結合処理を行うことを特徴とする。
【０００９】
第３の発明は、第１の発明に係る移動体用測位装置において、
前記擬似距離推測手段により推測された擬似距離段及び擬似距離計測手段により計測された擬似距離は、タイトカップリング型のカルマンフィルタに入力されることを特徴とする。
【００１０】
第４の発明は、第３の発明に係る移動体用測位装置において、
前記分散算出手段は、前記タイトカップリング型のカルマンフィルタで導出される誤差共分散行列の対角成分に基づいて、前記慣性航法測位手段により算出された前記移動体の位置の分散と、前記時計誤差の分散とを算出することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、早期に擬似距離のばらつきを収束させることができる移動体用測位装置が得られる。
【発明を実施するための最良の形態】
【００１２】
以下、図面を参照して、本発明を実施するための最良の形態の説明を行う。
【００１３】
図１は、本発明に係る移動体用測位装置が適用されるＧＰＳ（Global  Positioning  System）の全体的な構成を示すシステム構成図である。図１に示すように、ＧＰＳは、地球周りを周回するＧＰＳ衛星１０と、地球上に位置し地球上を移動しうる車両９０とから構成される。但し、車両９０は、移動局の一例であり、他の移動体としては、自動二輪車、鉄道、船舶、航空機、ホークリフト、ロボットや、人の移動に伴い移動する携帯電話等の情報端末が想定される。
【００１４】
ＧＰＳ衛星１０は、航法メッセージを地球に向けて常時放送する。航法メッセージには、対応するＧＰＳ衛星１０に関する衛星軌道情報（エフェメリスやアルマナク）、時計の補正値、電離層の補正係数が含まれている。航法メッセージは、Ｃ／Ａコードにより拡散されＬ１波（周波数：1575.42ＭＨz）に乗せられて、地球に向けて常時放送されている。尚、Ｌ１波は、Ｃ／Ａコードで変調されたＳｉｎ波とＰコード（Precision  Code）で変調されたＣｏｓ波の合成波であり、直交変調されている。Ｃ／Ａコード及びＰコードは、擬似雑音（Pseudo  Noise）符号であり、−１と１が不規則に周期的に並ぶ符号列である。
【００１５】
尚、現在、２４個のＧＰＳ衛星１０が高度約２０，０００kmの上空で地球を一周しており、各４個のＧＰＳ衛星１０が５５度ずつ傾いた６つの地球周回軌道面に均等に配置されている。従って、天空が開けている場所であれば、地球上のどの場所にいても、常時、少なくとも５個以上のＧＰＳ衛星１０が観測可能である。
【００１６】
車両９０には、移動体用測位装置１が搭載される。図２は、本発明による移動体用測位装置１の一実施例における主要な機能ブロック図である。
【００１７】
移動体用測位装置１は、図２に示すように、タイトカップリング型で慣性航法と衛星航法とを融合させたタイトカップリング型測位システム５に、分散算出部３０、擬似距離結合部４０、及び測位演算部５０を追加した構成となっている。
【００１８】
タイトカップリング型測位システム５は、ＧＰＳ受信機２０と、慣性航法測位部６０と、状態推定器７０と、擬似距離推測部８０とを備える。
【００１９】
ＧＰＳ受信機２０は、擬似距離及びドップラ速度を計測する機能を有する構成であれば、如何なる構成であってもよい。
【００２０】
図３は、ＧＰＳ受信機２０の内部構成の一例を示す。以下では、説明の複雑化を避けるため、ある１つのＧＰＳ衛星１０１からの衛星信号に関する信号処理（１チャンネルの信号処理）を代表して説明する。以下で説明する信号処理は、観測周期毎(例えば１ｍｓ)に、観測可能な各ＧＰＳ衛星１０１，１０２，１０３等からの衛星信号に対して並列的（同時）に実行される。
【００２１】
ＧＰＳ受信機２０は、ＧＰＳアンテナ２１、高周波回路２２、Ａ／Ｄ(analog-to-digital)変換回路２４、ＤＤＬ［Ｄｅｌａｙ―Locked  Loop］１１０、ＰＬＬ（Ｐｈａｓｅ−Ｌｏｃｋｅｄ  Ｌｏｏｐ）１２０、ドップラ速度算出部１２２、衛星位置算出部１２４、及び、フィルタ１３０を含む。ＤＤＬ１１０は、相互相関演算部１１１，１１２、位相進め部１１３、位相遅れ部１１４、位相ずれ計算部１１５、位相補正量計算部１１６、レプリカＣ／Ａコード生成部１１７、及び、擬似距離算出部１１８を含む。
【００２２】
ＧＰＳアンテナ２１は、ＧＰＳ衛星１０１から発信されている衛生信号を受信し、受信した衛星信号を電圧信号（本例では、周波数１．５ＧＨｚ）に変換する。１．５ＧＨｚの電圧信号をＲＦ(radio  frequency)信号と称する。
【００２３】
高周波回路２２は、ＧＰＳアンテナ２１を介して供給される微弱なＲＦ信号を後段でＡ／Ｄ変換できるレベルまで増幅すると共に、ＲＦ信号の周波数を信号処理できる中間周波数（典型的には、１ＭＨｚ〜２０ＭＨｚ）に変換する。尚、このようにＲＦ信号をダウンコンバートして得られる信号を、ＩＦ（Intermediate  frequency）信号と称する。
【００２４】
Ａ／Ｄ変換回路２４は、高周波回路２２から供給されるＩＦ信号（アナログ信号）を、デジタル信号処理ができるようにデジタルＩＦ信号に変換する。デジタルＩＦ信号は、ＤＤＬ１１０及びＰＬＬ１２０等に供給される。
【００２５】
ＤＤＬ１１０のレプリカＣ／Ａコード生成部１１７では、レプリカＣ／Ａコードが生成される。レプリカＣ／Ａコードとは、ＧＰＳ衛星１０１からの衛星信号に乗せられるＣ／Ａコードに対して、＋１、−１の並びが同一のコードである。
【００２６】
相互相関演算部１１１には、レプリカＣ／Ａコード生成部１１７で生成されるレプリカＣ／Ａコードが、位相進め部１１３を介して入力される。即ち、相互相関演算部１１１には、Ｅａｒｌｙレプリカ符号が入力される。位相進め部１１３では、レプリカＣ／Ａコードが所定の位相だけ進められる。位相進め部１１３で進められる位相進み量をθ１とする。
【００２７】
相互相関演算部１１１には、また、デジタルＩＦ信号が、図示しないミキサにより、ＰＬＬ１２０で生成されるレプリカキャリアが乗算されてから入力される。
【００２８】
相互相関演算部１１１では、入力されるデジタルＩＦ信号と、位相進み量θ１のＥａｒｌｙレプリカ符号を用いて、相関値（Ｅａｒｌｙ相関値ＥＣＡ）が演算される。Ｅａｒｌｙ相関値ＥＣＡは、例えば以下の式で演算される。
Ｅａｒｌｙ相関値ＥＣＡ＝Σ｛（デジタルＩＦ）×（Ｅａｒｌｙレプリカ符号）｝
相互相関演算部１１２には、レプリカＣ／Ａコード生成部１１７で生成されるレプリカＣ／Ａコードが、位相遅れ部１１４を介して入力される。即ち、相互相関演算部１１２には、Ｌａｔｅレプリカ符号が入力される。位相遅れ部１１４では、レプリカＣ／Ａコードが所定の位相だけ遅らされる。位相遅れ部１１４で遅らされる位相遅れ量は、位相進み量θ１と大きさ同一で符号が異なる。
【００２９】
相互相関演算部１１２には、また、デジタルＩＦ信号が、図示しないミキサにより、ＰＬＬ１２０で生成されるレプリカキャリアが乗算されてから入力される。
【００３０】
相互相関演算部１１２では、入力されるデジタルＩＦ信号と、位相遅れ量−θ１のＬａｔｅレプリカ符号を用いて、相関値（Ｌａｔｅ相関値ＬＣＡ）が演算される。Ｌａｔｅ相関値ＬＣＡは、例えば以下の式で演算される。
Ｌａｔｅ相関値ＬＣＡ１＝Σ｛（デジタルＩＦ）×（Ｌａｔｅレプリカ符号）｝
このようにして、相互相関演算部１１１、１１２では、コリレータ間隔ｄ１（“スペーシング”とも称される）を２θ１とした相関値演算が実行される。相互相関演算部１１１、１１２にてそれぞれ演算されたＥａｒｌｙ相関値ＥＣＡ及びＬａｔｅ相関値ＬＣＡは、位相ずれ計算部１１５に入力される。
【００３１】
位相ずれ計算部１１５では、デジタルＩＦ信号と、レプリカＣ／Ａコード生成部１１７で生成されるレプリカＣ／Ａコードとの間に、どの程度位相のずれがあるかが算出される。即ち、位相ずれ計算部１１５では、受信したＣ／Ａコードに対するレプリカＣ／Ａコードの位相ずれ量Δφが算出（推定）される。レプリカＣ／Ａコードの位相ずれ量Δφは、例えば以下の式で演算される。
（位相ずれ量Δφ）＝（ＥＣＡ−ＬＣＡ）／２（ＥＣＡ＋ＬＣＡ）
このようにして算出された位相ずれ量Δφは、位相補正量計算部１１６に入力される。
【００３２】
位相補正量計算部１１６では、位相ずれ量Δφを無くすべく、適切な位相補正量が算出される。適切な位相補正量が、例えば以下の演算式に従って、算出される。
（位相補正量）＝（Ｐゲイン）×（位相ずれ量Δφ）＋（Ｉゲイン）×Σ（位相ずれ量Δφ）
この式は、ＰＩ制御を利用したフィードバック制御を表す式であり、Ｐゲイン及びＩゲインは、それぞれバラツキと応答性の兼ね合いから実験的に決定される。このようにして算出された位相補正量は、レプリカＣ／Ａコード生成部１１７に入力される。
【００３３】
レプリカＣ／Ａコード生成部１１７では、生成されるレプリカＣ／Ａコードの位相が、位相補正量計算部１１６により算出された位相補正量だけ補正される。即ち、レプリカＣ／Ａコードの追尾点が補正される。かくして生成されたレプリカＣ／Ａコードは、上述の如く位相進め部１１３及び位相遅れ部１１４を介して相互相関演算部１１１、１１２に入力されると共に、擬似距離算出部１１８に入力される。尚、相互相関演算部１１１、１１２では、このようにして生成されたレプリカＣ／Ａコードは、次回の観測周期で入力されるＩＦデジタル信号に対する相関値演算に用いられることになる。
【００３４】
擬似距離算出部１１８では、レプリカＣ／Ａコード生成部１１７で生成されるレプリカＣ／Ａコードの位相情報に基づいて、擬似距離ρ’ＣＡが、例えば以下の式により演算される。尚、符号の意味として、擬似距離ρＣＡに付された「’」は、後述のフィルタ処理が実行されていないことを示し、「ＣＡ」は、Ｃ／Ａコードに基づいて算出された擬似距離ρであることを示す。
ρ’ＣＡ＝ＮＣＡ×３００
ここで、ＮＣＡは、ＧＰＳ衛星１０１と車両９０との間のＣ／Ａコードのビット数に相当し、レプリカＣ／Ａコード生成部１１７で生成されるレプリカＣ／Ａコードの位相及び受信機１内部の受信機時計に基づいて算出される。尚、数値３００は、Ｃ／Ａコードが、１ビットの長さが１μｓであり、１ビットに相当する長さが約３００ｍ（１μｓ×光速）であることに由来する。このようにして算出された擬似距離ρ’ＣＡを表す信号は、ＤＤＬ１１０からフィルタ１３０に入力される。
【００３５】
ＰＬＬ１２０では、内部で発生させたキャリアレプリカ信号を用いて、ドップラシフトした受信搬送波（受信キャリア）のドップラ周波数Δｆが測定される。即ち、ＰＬＬ１２０では、レプリカキャリアの周波数ｆｒと既知の搬送波周波数ｆＬ１（1575.42ＭＨz）に基づいて、ドップラ周波数Δｆ（＝ｆｒ−ｆＬ１）が測定される。尚、ＰＬＬ１２０に入力されるデジタルＩＦ信号は、図示しないミキサにより、ＤＤＬ１１０から供給されるレプリカＣ／Ａコードが乗算されたものである。ＰＬＬ１２０からのドップラ周波数Δｆを表す信号は、ドップラ速度算出部１２２及びフィルタ１３０に入力される。
【００３６】
フィルタ１３０では、ドップラ周波数Δｆを用いて、擬似距離ρ’ＣＡのフィルタ処理が実行される。フィルタ１３０では、例えば以下の演算式に従って、フィルタ処理後の擬似距離ρＣＡが計算される。
【００３７】
【数１】
2008232761.tif 000003
ここで、（ｉ）は今回値を表し、（ｉ−１）は前回値を表し、Ｍは、重み係数である。Ｍの値は、精度と応答性を考慮しつつ適切に決定される。また、ΔＶは、ＧＰＳ衛星１０１と車両９０との間の相対速度（ドップラ速度）である。フィルタ処理は、本分野で知られているキャリアスムージングと呼ばれる処理であってよく、上記のハッチフィルタの他、カルマンフィルタを用いても実現可能である。
【００３８】
ドップラ速度算出部１２２では、入力されたドップラ周波数Δｆに基づいて、ＧＰＳ衛星１０１と車両９０との間の相対速度（ドップラ速度）ΔＶが、例えば以下の関係式を用いて、算出される。
Δｆ＝ΔＶ・ｆＬ１／（ｃ−ΔＶ）
尚、ｃは光速を表す。
【００３９】
衛星位置算出部１２４は、航法メッセージの衛星軌道情報に基づいて、ＧＰＳ衛星１０１の、ワールド座標系での現在位置ｒ（ＳＶ）＝（ＸＳＶ、ＹＳＶ、ＺＳＶ）及び移動速度Ｖ（ＳＶ）を計算する。尚、ＧＰＳ衛星１０１は、人工衛星の１つであるので、その運動は、地球重心を含む一定面内（軌道面）に限定される。また、ＧＰＳ衛星１０１の軌道は地球重心を１つの焦点とする楕円運動であり、ケプラーの方程式を逐次数値計算することで、軌道面上でのＧＰＳ衛星１０１の位置が計算できる。また、ＧＰＳ衛星１０１の位置ｒ（ＳＶ）は、ＧＰＳ衛星１０１の軌道面とワールド座標系の赤道面が回転関係にあることを考慮して、軌道面上でのＧＰＳ衛星１０１の位置を３次元的な回転座標変換することで得られる。尚、ワールド座標系とは、図４に示すように、地球重心を原点として、赤道面内で互いに直交するＸ軸及びY軸、並びに、この両軸に直交するＺ軸により定義される。
【００４０】
ＧＰＳ受信機２０にて導出される衛星位置ｒ（ＳＶ）及び衛星移動速度Ｖ（ＳＶ）は、後述の擬似距離推測部８０に入力される。
【００４１】
図２に戻る。慣性航法測位部６０は、例えば車両の挙動を表すことができる状態量を検出する車輪速センサ等の車載センサの出力値を入力とする車両モデルを用いて適切な拘束を付与しつつ、ＩＭＵ（加速度センサ及びジャイロセンサからなる慣性計測装置）の出力信号に基づいて、車両位置、速度や車両姿勢（方位角）を算出する。車両モデルは、例えばセンサのバイアスやドリフトが経時的に変化する性質を有することを考慮したガウスマルコフモデルに基づくものであってよい。この種の慣性航法による車両位置の測位方法は、多種多様でありえ、如何なる方法であってもよい。例えば車両位置は、加速度センサの出力値に、姿勢変換、重力補正、コリオリ力補正を行って２回積分し、当該２回積分により得られる移動距離を、車両位置の前回値に積算することで導出されてよい。姿勢変換を行うための変換式は、ジャイロセンサの出力値に、地球自転を加味する補正を行って積分し、その積分値（ヨー角）と姿勢の前回値とを積算した値を用いて作成されてよい。このようにして得られる車両位置及び速度を、ここでは、それぞれ、車両推定位置ｒ（ＩＮＳ）＝（ＸＩＮＳ、ＹＩＮＳ、ＺＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）という。車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）は、後述の擬似距離推測部８０に入力される。
【００４２】
擬似距離推測部８０は、ＧＰＳ受信機２０から得られる衛星位置ｒ（ＳＶ）及び衛星移動速度Ｖ（ＳＶ）と、慣性航法測位部６０から得られる車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）とに基づいて、以下の式により、推測擬似距離ρ推測及び推測ドップラ速度ΔＶ推測を算出する。
ρ推測＝√｛（ＸＩＮＳ−ＸＳＶ）２＋（ＹＩＮＳ−ＹＳＶ）２＋（ＺＩＮＳ−ＺＳＶ）２｝＋Ｃｔ
ΔＶ推測＝Ｖ（ＩＮＳ）−Ｖ（ＳＶ）
ここで、Ｃｔとは、ＧＰＳ受信機２０にて計測される擬似距離ρＣＡ（或いはフィルタ前の擬似距離ρ’ＣＡ、以下、計測擬似距離ρと称する。）に含まれる時計誤差の推定値、即ち、ＧＰＳ受信機２０内の受信機時計と、ＧＰＳ衛星１０１に搭載される原子時計との間の誤差の推定値である。Ｃｔは、後述の状態推定器７０から供給される。
【００４３】
このようにして擬似距離推測部８０にて導出される推測擬似距離ρ推測及び推測ドップラ速度ΔＶ推測は、それぞれ、ＧＰＳ受信機２０から得られる計測擬似距離ρ及びドップラ速度ΔＶに対して時間的に同期した態様で差分が取られる（例えば同一のＧＰＳ時刻に係る値同士で差分が取られる）。そして、これらの差分値が観測量ｚとして状態推定器７０に入力される。
【００４４】
状態推定器７０は、タイトカップリング型のカルマンフィルタを用いて、各種の状態量ηを推定する。状態方程式は、以下のように設定される。
η（ｔｎ）＝Ｆ・η（ｔｎ−１）＋Ｇ・ｕ（ｔｎ−１）＋Γ・ｗ（ｔｎ−１）
ここで、η（ｔｎ）は、時刻ｔ＝ｔｎでの状態変数を表わし、また、ｕ（ｔｎ−１）及びｗ（ｔｎ−１）は、それぞれ、時刻ｔ＝ｔｎ−１での既知入力及び外乱（システム雑音：正規性白色雑音）である。η（ｔｎ）は、車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）のそれぞれの誤差δｒ（ＩＮＳ）及びδｖ（ＩＮＳ）、慣性航法測位部６０で推定される車両の姿勢の誤差δε（ＩＮＳ）、ジャイロセンサのバイアス誤差δｂ、加速度センサのドリフト誤差δｄ、車両９０のタイヤの半径誤差δｓ、及び、ＧＰＳ受信機２０内の受信機時計の誤差δＣｔを含む。
【００４５】
また、観測方程式は、次のように設定される。
ｚ（ｔｎ）＝Ｈ（ｔｎ）・η（ｔｎ）＋ｖ（ｔｎ）
観測量ｚは、上述の如く、擬似距離推測部８０にて導出される推測擬似距離ρ推測及び推測ドップラ速度ΔＶ推測と、ＧＰＳ受信機２０にて導出される計測擬似距離ρ及びドップラ速度ΔＶとのそれぞれの差分値である。
【００４６】
ここで、観測量ｚは、（ρ−ρ推測、ΔＶ−ΔＶ推測）であるが、車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）に、各種誤差δｒ（ＩＮＳ）、δｖ（ＩＮＳ）、δε（ＩＮＳ）、δｂ、δｄ及びδｓが含まれていると仮定する。この場合、例えば擬似距離に関する観測量は、ρ−ρ推測＝√｛（ＸＩＮＳ＋ΣδＸ＊−ＸＳＶ）２＋（ＹＩＮＳ＋ΣδＹ＊−ＹＳＶ）２＋（ＺＩＮＳ＋ΣδＺ＊−ＺＳＶ）２｝＋Ｃｔ＋δＣｔとなる。（δＸ＊、δＹ＊、δＺ＊）は、各種誤差δｒ（ＩＮＳ）、δｖ（ＩＮＳ）、δε（ＩＮＳ）、δｂ、δｄ及びδｓに対応する各誤差のＸ，Ｙ，Ｚ成分を表す。このように、上記の状態方程式は線形であるのに対して、観測量ｚは、状態変数η（各種誤差）に関して非線形であるため、線形化して上記の観測方程式の観測行列Ｈが求められる。
【００４７】
分散算出部３０は、先ず、ＧＰＳ受信機２０にて導出される計測擬似距離ρの分散σ１２を算出する機能を有する。分散σ１２は、例えば以下の式に従って算出されてよい。
σ１２＝Σ（ρ”１−ave（ρ”１））２／（ｎ−１）    式（１）
式（１）において、ρ”１は、計測擬似距離ρから観測周期毎の擬似距離変化量を差し引いたものである。擬似距離変化量は、上述のドップラ周波数Δｆから導出されてよい。ave（ρ”１）は、データ数ｎのρ”１の平均を表す。
【００４８】
或いは、計測擬似距離ρの分散値σ１２は、例えば以下の式に従って算出されてよい。
σ１２＝λ×√（ｄ１／τ・Ｃ／Ｎ）        式（２）
式（２）において、Ｃ／Ｎは、搬送波の強度（電力）と雑音の強度（電力）の比である。Ｃ／Ｎ以外は固定値であり、λは、Ｃ／Ａコードの１ビットの長さであり、約３００［ｍ］である。ｄ１は、計測擬似距離ρを算出した際に用いたレプリカＣ／Ａコードに係るＥａｒｌｙレプリカ符号とＬａｔｅレプリカ符号の位相差（コリレータ間隔）である。τは、フィルタにおけるフィルタ時定数であり、データ数に相当する。
この場合、Ｃ／Ｎの算出方法は、多種多様であり、例えば以下の式（３）に従って算出されてよい。
Ｃ／Ｎ＝{（ＧＰＳ衛星１０１の発信電波強度）−α×Ｌ２}／（熱雑音レベル）  式（３）
これは、Ｃ／Ｎを理論式により推測する方法があり、熱雑音は基本的に常に一定であり、信号は距離の二乗に比例して減衰するという特徴を利用したものである。ここで、ＧＰＳ衛星１０１の発信電波強度及び熱雑音レベルは、既知の値を用いる。Ｌは、ＧＰＳ衛星１０１と車両９０との間の距離であり、ＧＰＳ衛星１０１の位置と、車両９０の位置とに基づいて、算出される。ＧＰＳ衛星１０１の位置は、衛星位置算出部１２４からの情報が用いられてよく、車両９０の位置は、測位演算部５０における前回周期の測位結果であってよく、又は今回周期の測位結果をフィードバックして利用してもよい。また、αは、減衰係数である。尚、式（２）において、ＧＰＳアンテナ２１の指向性ないしＧＰＳ衛星１０１の仰角を加味するための項を追加してもよい。
或いは、Ｃ／Ｎは、例えば以下の式（４）に従って算出されてよい。
Ｃ／Ｎ＝ｌｏｇ（ＥＣＡ＋ＬＣＡ）＋β    式（４）
これは、Ｃ／Ｎがノイズ電力Ｗｎと信号電力（搬送波電力）Ｗｃを用いて、Ｃ／Ｎ＝１０ｌｏｇ（Ｗｃ／Ｗｎ）で表され、Ｗｃ／ＷｎがＥａｒｌｙ相関値ＥＣＡ及びＬａｔｅ相関値ＬＣＡの和（＝ＥＣＡ＋ＬＣＡ）に比例関係にあることを考慮したものである。式（３）において、βは、適切な固定値であり、例えば既知のノイズ電力Ｗｎを用いて、−１０ｌｏｇＷｎで表されてよい。尚、Ｅａｒｌｙ相関値ＥＣＡ及びＬａｔｅ相関値ＬＣＡは、熱雑音に起因してバラツキが発生するため、式（４）により求めるＣ／Ｎには、バラツキが発生する。従って、以下の式（５）のように、ある程度データをためてフィルタ（平均）処理を行うことで、各相関値ＥＣＡ、ＬＣＡのバラツキの影響を低減することが可能である。
Ｃ／Ｎ＝Σ（Ｃ／Ｎ’）／ｎ    式（５）
Ｃ／Ｎ’は、上記の式（４）により算出されたＣ／Ｎで表す。ｎは、データ数（例えば、１ｍｓ毎にデータを収集し、１００ｍｓ分のデータを用いる場合には、ｎ＝１００）である。尚、データ数ｎは、観測開始時をゼロとして、その後、観測周期（例えば１ｍｓ)毎に１つずつ増加するものである。
或いは、Ｃ／Ｎは、例えば以下の式（６）に従って算出されてよい。
Ｃ／Ｎ＝（Ｃ／Ｎ推測＋ｎ×Ｃ／Ｎ実測）／（ｎ＋１）    式（６）
Ｃ／Ｎ推測は、上記の式（３）により算出されたＣ／Ｎを表し、Ｃ／Ｎ実測は、上記の式（４）又は（５）により算出されたＣ／Ｎを表す。この式（６）では、データ数ｎが増えるに従って、Ｃ／Ｎ実測に大きい重みが付与されるようになっている。即ち、観測開始直後は、実測データ量の少なさに起因したＣ／Ｎ実測のバラツキを考慮してＣ／Ｎ推測に相対的に大きな重みが付与され、時間が経過するに従って、実測データ量の増加に伴って信頼性が高くなったＣ／Ｎ実測に相対的に大きな重みが付与されるようになっている。
【００４９】
分散算出部３０は、更に、擬似距離推測部８０にて導出される推測擬似距離ρ推測の分散σ推測２を算出する機能を有する。
【００５０】
分散σ推測２は、ρ推測＝√｛（ＸＩＮＳ−ＸＳＶ）２＋（ＹＩＮＳ−ＹＳＶ）２＋（ＺＩＮＳ−ＺＳＶ）２｝＋Ｃｔであることから、衛星軌道情報に係る衛星の位置の分散と、慣性航法測位部６０により導出された車両推定位置ｒ（ＩＮＳ）の分散と、ＧＰＳ受信機２０の時計誤差の分散とから算出される。
【００５１】
分散σ推測２は、分散の伝播則により、衛星軌道情報に係る衛星の位置の分散と、慣性航法測位部６０により導出された車両推定位置ｒ（ＩＮＳ）の分散とから算出される分散σｆ２に、ＧＰＳ受信機２０の時計誤差の分散σＣｔ２を足し合わせることにより算出されてよい。即ち、分散σ推測２＝σｆ２＋σＣｔ２として算出されてよい。
【００５２】
分散σｆ２は、分散の伝播則により以下のようにして算出されてよい。
【００５３】
【数２】
2008232761.tif 000004
数２において、ｆ＝√｛（ＸＩＮＳ−ＸＳＶ）２＋（ＹＩＮＳ−ＹＳＶ）２＋（ＺＩＮＳ−ＺＳＶ）２｝である。また、（ＸＩＮＳ０、ＹＩＮＳ０、ＺＩＮＳ０）は、前回周期（１サンプル前）の値を表し、（ＸＳＶ０、ＹＳＶ０、ＺＳＶ０）は、前回周期（１サンプル前）の値を表す。但し、今回周期の各値が用いられてもよい。また、（σＸＩＮＳ２、σＹＩＮＳ２、σＺＩＮＳ２）は、それぞれ、車両推定位置ｒ（ＩＮＳ）のＸ，Ｙ，Ｚ成分の分散を表し、状態推定器７０で導出される誤差共分散行列Ｐにおける対応する対角成分の値が用いられる。（σＸＳＶ２、σＹＳＶ２、σＺＳＶ２）は、それぞれ、衛星位置ｒ（ＳＶ）のＸ，Ｙ，Ｚ成分の分散を表す。（σＸＳＶ２、σＹＳＶ２、σＺＳＶ２）は、過去に得られた衛星位置の分散値を考慮して設定されてもよいし、予め適切な固定値として適宜設定するようにしてもよい。固定値の場合、（σＸＳＶ２、σＹＳＶ２、σＺＳＶ２）は、例えばそれぞれ±10程度であってよい。
【００５４】
ＧＰＳ受信機２０の時計誤差の分散σＣｔ２は、状態推定器７０で導出される誤差共分散行列Ｐにおける対応する対角成分の値が用いられる。
【００５５】
擬似距離結合部４０は、同一ＧＰＳ時刻に係る計測擬似距離ρと推測擬似距離ρ推測とを結合して、１つの擬似距離ρＣＯＭ（以下、「結合擬似距離」という）を算出する。擬似距離結合部４０は、例えば簡易的に以下の式により、結合擬似距離ρＣＯＭを算出してよい。
ρＣＯＭ＝（ρ＋ρ推測）／２
即ち、結合擬似距離ρＣＯＭは、計測擬似距離ρと推測擬似距離ρ推測とを平均化することにより算出されてよい。但し、好ましくは、擬似距離結合部４０は、分散算出部３０から得られる計測擬似距離ρの分散値σ１２及び推測擬似距離ρ推測の分散値σ推測２を用いて、例えば以下の式に従って、第１擬似距離ρと第２擬似距離ρとを結合することで、結合擬似距離ρＣＯＭを算出する。
ρＣＯＭ＝（σ推測２ｍ×ρ＋σ１２ｍ×ρ推測）／（σ推測２ｍ＋σ１２ｍ）
ここで、上付き「ｍ」は、階乗を表し、正の整数（ｍ＝１，２、・・）のうちの適切な値が用いられる。例えば、ｍ＝１であってよい。この場合、結合擬似距離ρＣＯＭは、以下の式に従って算出されることになる。
ρＣＯＭ＝（σ推測２×ρ＋σ１２×ρ推測）／（σ推測２＋σ１２）
このようにして擬似距離結合部４０にて算出される結合擬似距離ρＣＯＭは、測位演算部５０に入力される。
【００５６】
測位演算部５０は、衛星位置算出部１２４から得られる衛星位置ｒ（ＳＶ）と、擬似距離結合部４０から供給される結合擬似距離ρＣＯＭとに基づいて、車両９０の位置（Ｘｕ，Ｙｕ，Ｚｕ）を測位する。測位結果は、例えばナビゲーションシステムに出力されてよい。車両９０の位置は、３つのＧＰＳ衛星１０に対して得られるそれぞれの結合擬似距離ρＣＯＭ及び衛星位置ｒ（ＳＶ）を用いて、三角測量の原理で導出されてよい。この場合、結合擬似距離ρＣＯＭは上述の如く時計誤差を含むので、４つ目のＧＰＳ衛星１０に対して得られる結合擬似距離ρＣＯＭ及び衛星位置ｒ（ＳＶ）を用いて、時計誤差成分が除去されてよい。或いは、５つ以上のＧＰＳ衛星１０に対して得られる結合擬似距離ρＣＯＭ及び衛星位置ｒ（ＳＶ）を用いて、統計的なアルゴリズムを用いて最終的な車両９０の位置（Ｘｕ，Ｙｕ，Ｚｕ）が決定されてもよい。尚、車両９０の位置の測位方法としては、上述のような単独測位に限られず、干渉測位（既知の点に設置された固定局での受信データを併用する方式）であってもよい。干渉測位の場合、上述の如く固定局及び車両９０にてそれぞれ得られる擬似距離の一重位相差や２重位相差等を用いて車両９０の位置が測位されることになる。
【００５７】
次に、以上説明した本実施例の移動体用測位装置１の主要動作について、図５を参照して説明する。図５に示す処理は、所定周期毎に繰り返し実行されるが、ここでは、時刻ｔｎに係る周期を今回周期として説明する。
【００５８】
ステップ３００では、ＩＭＵの出力値（例えば３軸方向の加速度、及び、３軸まわりのヨー）がサンプリングされる。ＩＭＵの値は、後述のステップ３５０の前回周期にて状態推定器７０により算出される誤差に基づいて補正を受けてよい。即ち、ジャイロセンサのバイアス誤差δｂ、加速度センサのドリフト誤差δｄの推定値に基づいて、それぞれの誤差が補正されてよい。また、車輪速センサのスケーリングファクタ（車両モデル）についても、後述のステップ３５０の前回周期にて状態推定器７０により導出されるタイヤ半径誤差δｓの推定値に基づいて、補正されてよい。
【００５９】
ステップ３１０では、慣性航法測位部６０において、上記ステップ３００で得られるＩＭＵの出力値に基づいて、車両推定位置ｒ（ＩＮＳ）及び車両推定速度Ｖ（ＩＮＳ）等が導出される。車両推定位置ｒ（ＩＮＳ）等は、擬似距離推測部８０において推測擬似距離ρ推測（ｔｎ）の算出に用いられる。
【００６０】
ステップ３２０では、状態推定器７０においてカルマンフィルタの時間更新が実行される。カルマンフィルタの時間更新は、例えば以下のように表される。
η（ｔｎ）（−）＝η（ｔｎ−１）（＋）＋ｕ（ｔｎ−１）
Ｐ（ｔｎ）（−）＝Ｆ・Ｐ（ｔｎ−１）（＋）・ＦＴ＋Γ・Ｑ（ｔｎ−１）・ΓＴ
尚、Ｐは、予測・推定誤差の共分散行列であり、Ｑは、外乱ｗの共分散行列（正定値対称行列）である。
【００６１】
ステップ３３０乃至３６０では、状態推定器７０においてカルマンフィルタの観測更新及び誤差推定処理が実行される。具体的には、以下のとおりである。
【００６２】
ステップ３３０では、現在見えている（観測可能な）ＧＰＳ衛星１０のそれぞれに対して、擬似距離推測部８０にて推測擬似距離ρ推測（ｔｎ）が算出されると共に、ＧＰＳ受信機２０にて計測擬似距離ρ（ｔｎ）が計測される。この際、擬似距離推測部８０にて導出される推測擬似距離ρ推測（ｔｎ）の算出に用いる時計誤差Ｃｔは、後述のステップ３５０の前回周期にて状態推定器７０により導出される誤差δＣｔの推定値に基づいて、補正されてよい。次いで、これらの計測擬似距離ρ及び推測擬似距離ρ推測に基づいて、上述の如く観測量ｚ（ｔｎ）が導出される。そして、ステップ３３０では、現在見えているＧＰＳ衛星１０に応じた観測行列Ｈ（ｔｎ）が演算される。尚、観測方程式（即ち、観測行列Ｈ（ｔｎ））は、現在見えているＧＰＳ衛星１０の数に応じて変化する。
【００６３】
ステップ３４０では、カルマンゲインＫ（ｔｎ）が以下のように演算される。
Ｋ（ｔｎ）＝Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）・（Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）・ＨＴ（ｔｎ）＋Ｒ（ｔｎ））−１
尚、Ｒは、観測ノイズ（正規性白色雑音）ｖの共分散行列（正定値対称行列）である。
【００６４】
ステップ３５０では、上記のステップ３４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、推定誤差δη（ｔｎ）が以下のように算出される。
δη（ｔｎ）＝Ｋ（ｔｎ）・ｚ（ｔｎ）
このようにして導出された状態量ηの推定誤差δη（ｔｎ）は、次回周期（ｔｎ＋１）における上記のステップ３１０や３３０における誤差補正に利用される。
【００６５】
ステップ３６０では、上記のステップ３４０で得られるカルマンゲインＫ（ｔｎ）に基づいて、共分散行列Ｐが以下のように更新される。
Ｐ（ｔｎ）（＋）＝Ｐ（ｔｎ）（−）−Ｋ（ｔｎ）・Ｈ（ｔｎ）・Ｐ（ｔｎ）（−）
ステップ３７０では、分散算出部３０において分散σ１２（ｔｎ）、σ推測２（ｔｎ）が算出される。分散σ推測２（ｔｎ）の算出には、上記のステップ３６０で導出されるＰ（ｔｎ）（＋）における対応する対角成分の値、及び、所定の固定値が利用される。
【００６６】
ステップ３８０では、擬似距離結合部４０において計測擬似距離ρ（ｔｎ）及び推測擬似距離ρ推測（ｔｎ）が結合されて結合擬似距離ρＣＯＭ（ｔｎ）が算出される。計測擬似距離ρ（ｔｎ）及び推測擬似距離ρ推測（ｔｎ）の結合には、上記のステップ３７０で導出される分散σ１２（ｔｎ）及びσ推測２（ｔｎ）が用いられる。
【００６７】
ステップ３９０では、測位演算部５０において車両９０の位置が測位される。この測位には、上記のステップ３８０で導出される結合擬似距離ρＣＯＭ（ｔｎ）が用いられる。
【００６８】
以上説明した本実施例の移動体用測位装置１によれば、とりわけ以下の有利な効果が奏される。
【００６９】
上述の如く、本実施例では、１観測周期でのＣ／Ａコードの観測データから、１つのＧＰＳ衛星１０に対する２個の擬似距離（計測擬似距離ρ、推測擬似距離ρ推測）が算出されるので、１つのＧＰＳ衛星１０に対して１観測周期で唯一の擬似距離（例えば計測擬似距離ρのみ）を算出する構成に比べて、擬似距離（結合擬似距離ρＣＯＭ）のばらつきを早期に収束させることができる。
【００７０】
また、２個の擬似距離（計測擬似距離ρ、推測擬似距離ρ推測）を、それぞれの分散σ１２、σ推測２（バラツキ度合いを表す指標）を用いて結合させることで、結合擬似距離ρＣＯＭのばらつきを早期に収束させつつ、高い精度の結合擬似距離ρＣＯＭを得ることができる。また、推測擬似距離ρ推測の分散σ推測２は、タイトカップリング型のカルマンフィルタにて導出される共分散行列Ｐの対角線分を利用して導出されるので、推測擬似距離ρ推測の分散σ推測２を精度良く算出することができる。
【００７１】
また、分散算出部３０と擬似距離推測部８０から推測擬似距離ρ推測とその分散σ推測２を求めることで、受信機内で擬似距離を絞り込む際のパラメータとして用いることができ、擬似距離推定の高速化を行うことも可能である。
【００７２】
また、状態推定器７０において、車両９０のタイヤの半径誤差δｓを考慮したタイトカップリング型のカルマンフィルタを構成することにより、車輪速センサの誤差要因をも補償できる精度の高い車両モデルを用いることができ、慣性航法測位部６０の測位精度ひいては測位演算部５０の測位精度を高めることができる。
【００７３】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなく、上述した実施例に種々の変形及び置換を加えることができる。
【００７４】
例えば、上述の実施例では、既存のＧＰＳ受信機２０を含むタイトカップリング型測位システム５に、分散算出部３０、擬似距離結合部４０及び測位演算部５０を追加する態様で、移動体用測位装置１が実現されているが、本発明はこれに限られず、ＧＰＳ受信機２０内に、分散算出部３０、擬似距離結合部４０及び測位演算部５０の全部若しくは一部がソフトウェア的に及び／又はハードウェア的に組み込まれてもよい。
【００７５】
また、上述の実施例では、Ｃ／Ａコードを用いて計測擬似距離ρを導出しているが、計測擬似距離ρは、Ｌ２波のＰコードに基づいて導出されてもよい。尚、Ｐコードの場合、Ｗコードで暗号化されているので、Ｐコード同期を行う際に、クロス相関方式を利用したＤＬＬにより、Ｐコードを取り出すこととしてよい。Ｐコードに基づく擬似距離ρ’Ｐは、ＧＰＳ衛星１０１でＰコードが０ビット目であるとしてＰコードのＭＰビット目が車両９０にて受信されているかを計測することで、ρ’Ｐ＝ＭＰ×３０として求めることができる。
【００７６】
また、同様の観点から、上述の実施例において、Ｃ／Ａコードに基づく計測擬似距離ρに代えて、車載カメラを用いた道路標識等（例えば、交差点手前の菱形のペイントのような、位置が既知の標識等）の画像認識結果に基づいて導出可能な推測擬似距離、路車間通信や車間通信を介して得られる車両位置に基づいて導出可能な推測擬似距離等を用いてもよい。
【００７７】
また、上述の実施例では、２個の擬似距離（計測擬似距離ρ、推測擬似距離ρ推測）を結合して結合擬似距離ρＣＯＭを算出しているが、推測擬似距離ρ推測を含む３個以上の擬似距離を結合して結合擬似距離ρＣＯＭを算出してもよい。この場合、他の擬似距離としては、Ｐコードに基づく計測擬似距離ρ’Ｐや、異なる追尾方法で得られるＣ／Ａコードに基づく計測擬似距離、車載カメラを用いた道路標識等の画像認識結果に基づいて導出可能な推測擬似距離、路車間通信や車車間通信を介して得られる車両位置に基づいて導出可能な推測擬似距離等が考えられる。この場合、結合態様としては、ある一の擬似距離に対して、１つずつ他の擬似距離を結合させてもよいし、すべての擬似距離を一括的に結合させてもよい。
【００７８】
また、上述の実施例では、ＧＰＳに本発明が適用された例を示したが、本発明は、ＧＰＳ以下の衛星システム、例えばガリレオ等の他のGNSS  (Global  Navigation  Satellite  System)にも適用可能である。
【図面の簡単な説明】
【００７９】
【図１】本発明に係る移動体用測位装置が適用されるＧＰＳの全体的な構成を示すシステム構成図である。
【図２】本発明による移動体用測位装置１の一実施例における主要な機能ブロック図である。
【図３】ＧＰＳ受信機２０の内部構成の一例を示す図である。
【図４】ワールド座標系とローカル座標系との関係、及び、ローカル座標系とボディ座標との関係を示す図である。
【図５】本実施例の移動体用測位装置１の主要動作を示すフローチャートである。
【符号の説明】
【００８０】
１    移動体用測位装置
１０    ＧＰＳ衛星
２０    ＧＰＳ受信機
２１    ＧＰＳアンテナ
２２    高周波回路
２４    Ａ／Ｄ変換回路
３０    分散算出部
４０    擬似距離結合部
５０    測位演算部
６０    慣性航法測位部
７０    状態推定器
８０    擬似距離推測部
１１０    ＤＤＬ
１２０    ＰＬＬ
１３０    フィルタ
【図１】
2008232761.tif 000005
【図２】
2008232761.tif 000006
【図３】
2008232761.tif 000007
【図４】
2008232761.tif 000008
【図５】
2008232761.tif 000009