測位装置及び測位方法
【課題】電波航法により絶対位置、絶対速度を得ることができない場合、或いは、電波航法による測位精度が低い場合でも、自律航法における測定誤差を的確に推測する。
【解決手段】電波を受信している衛星の数や配置等に基づいて、電波航法で得られる絶対測位情報が高信頼度であるかを判定し（ステップＳ２）、高信頼度であるときには、電波航法による絶対測位情報を利用して、自律航法による測位結果の測定誤差を推定する（ステップＳ４）。電波航法による絶対測位情報が高信頼度でない場合は、捕捉衛星の中から受信パワーや仰角がしきい値よりも大きい高信頼度衛星を選択し（ステップＳ５）、この高信頼度衛星との間の擬似距離及びドップラ周波数を算出しこれを利用して、自律航法による測位結果の測定誤差を推定する（ステップＳ６）。そして推定した測定誤差を用いて自律航法による測位結果を補正し、測位装置１０本体の測定位置を得る。
【解決手段】電波を受信している衛星の数や配置等に基づいて、電波航法で得られる絶対測位情報が高信頼度であるかを判定し（ステップＳ２）、高信頼度であるときには、電波航法による絶対測位情報を利用して、自律航法による測位結果の測定誤差を推定する（ステップＳ４）。電波航法による絶対測位情報が高信頼度でない場合は、捕捉衛星の中から受信パワーや仰角がしきい値よりも大きい高信頼度衛星を選択し（ステップＳ５）、この高信頼度衛星との間の擬似距離及びドップラ周波数を算出しこれを利用して、自律航法による測位結果の測定誤差を推定する（ステップＳ６）。そして推定した測定誤差を用いて自律航法による測位結果を補正し、測位装置１０本体の測定位置を得る。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＧＰＳ衛星等からの電波を利用して測位を行う電波航法により得られる測定値と、自律航法により得られる測定値とに基づいて、測位を行うようにした測位装置及び測位方法に関する。
【背景技術】
【０００２】
近年、車載型端末装置や、携帯型端末装置等、さまざまな分野で、車両や歩行者の現在位置情報の提供が求められている。この現在位置の測位手法として、ＧＰＳ（グローバルポジショニングシステム）等による電波航法、角速度センサ等の方位センサや、加速度センサ、車速センサ等の速度センサを用いた自律航法、等の手法が提案されている。
ＧＰＳ衛星を用いた電波航法には、長時間安定性がある一方、測位環境への依存性がある。つまり、高層ビルが立ち並ぶ都市部や、山間部では、ＧＰＳ衛星からの電波が、建物等に遮断されると、測定に十分な数の衛星を捕捉することができず、測位できなくなる可能性がある。また、建物等に反射されたマルチパスの影響を受けると、測位精度の低下につながる可能性がある。
【０００３】
このようなＧＰＳ衛星の欠点を補う次世代衛星システムとして、準天頂衛星（ＱＺＳＳ）が注目を集めている。すなわち、このＱＺＳＳ衛星を、常に日本の天頂付近に配置し、このＱＺＳＳ衛星を利用してＧＰＳ衛星の補間や補強を行うことで、測位サービスの改善が期待されている。
一方、自律航法では、各種センサで計測される角速度や、加速度を積分することで、絶対位置や絶対方位を推測している。このため、短時間での推測精度は良いが、計測値の誤差の影響により、この計測値を積分して得られる推測位置の誤差は、時間と共に増加する。
【０００４】
このような電波航法及び自律航法によるそれぞれの欠点を補う為に、複数種類の航法手段を組み合わせて、速度や位置の検出精度を高めるようにしたハイブリッド航法が提案されている。
このハイブリッド航法においては、例えば、カルマンフィルタを用い、電波航法による計測値を利用して、自律航法による計測値の測定誤差を推定し、これを用いて自律航法による計測値の補正を行っている。この測定誤差の推定方法として、例えば、Loosely Coupled Modeと、Tightly Coupled Modeとがある。
【０００５】
前記Loosely Coupled Modeは、電波航法では、４個以上のＧＰＳ衛星を捕捉することができれば、測位演算を行うことで移動体の絶対位置を特定することができることから、この電波航法による絶対測位情報（絶対位置、絶対速度）を利用して、カルマンフィルタにより、自律航法で用いる各種自律センサの測定誤差を推定し、推定した自律センサの誤差を用いて、自律センサによる測定値を補正する。
【０００６】
一方、Tightly Coupled Modeは、電波航法により得られる絶対測位情報（絶対位置、絶対速度）の代わりに、ＧＰＳ衛星までの擬似距離（レンジ）やドップラ周波数を観測値として利用して、自律センサの測定誤差の推定を行うものである。
また、例えば、特許文献１には、常に４個のＧＰＳ衛星を捕捉することができなくても、少なくとも、１個のＧＰＳ衛星を捕捉することができれば、捕捉したＧＰＳ衛星までの擬似距離（レンジ）や、その変化率（レンジレート）を用いて、航法装置に搭載された自律センサの測定誤差を推定するようにした、航法装置が提案されている。
【特許文献１】特開平６−３４１８４７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
ところで、上記Loosely Coupled Modeにおいては、その観測モデルは簡単なため、フィルタのサイズは小さく、実現しやすく、また、受信機の機種への依存性がなく移植性もよい。その反面、ＧＰＳ衛星を４個以上捕捉することができない場合には、絶対測位情報（絶対位置、絶対速度）を得ることができない。カルマンフィルタでは、測定誤差の前回の推定値から測定誤差の予測を行う予測モードと、この予測モードで予測した測定誤差と、電波航法により算出した絶対測位情報（絶対位置、絶対速度）とを用いて測定誤差を推定する推定モードとを繰り返し行うことで、測定誤差を推定しているため、前述のように、絶対測位情報（絶対位置、絶対速度）を得ることができない場合には、予測モードのみで測定誤差の推定を行うことになる。
【０００８】
したがって、測定誤差の推定精度は、主にシステムモデルの精度や、システムノイズ、また、電波航法による測位ができなくなる直前の測定誤差の推定精度に左右されることになり、時間と共に低下し、特に、高層ビルが立ち並ぶ都市部や、山間部では、衛星からの電波は建物等により遮断されたり、反射されたりすることがよくあるための、測位精度の向上が課題となっている。
【０００９】
また、Tightly Coupled Modeにおいては、擬似距離（レンジ）やドップラ周波数は、少なくとも１個のＧＰＳ衛星を捕捉することができれば得ることができる。したがって、常に４個以上のＧＰＳ衛星を捕捉することができない場合であっても、自律航法で用いる自律センサの測定誤差の推定を行うことができる。その反面、観測モデルが複雑になるため、フィルタのサイズが大きくなり、計算負荷も重くなる。また、絶対測位情報（絶対位置、絶対速度）ではなく、ＧＰＳ衛星までの擬似距離（レンジ）や、ドップラ周波数を利用するため、ＧＰＳ受信機の機種へ依存するという問題がある。
【００１０】
また、上述の特許文献１記載の発明のように、ＧＰＳ衛星までの擬似距離（レンジ）やその変化率（レンジレート）を用いて測定誤差を推測する方法においては、電波航法によって絶対測位情報（絶対位置、絶対速度）を確保することができなくても、少なくとも1
個のＧＰＳ衛星を捕捉することができれば、その擬似距離（レンジ）やその変化率（レンジレート）を用いることで、自律センサの測定誤差を推定し、自律センサの測定値の補正を行うことができるため、常に高い精度で位置推定を行うことができる。
【００１１】
その反面、ＧＰＳ衛星を多数受信している場合には、測定誤差の推定に用いる擬似距離（レンジ）やその変化率（レンジレート）の数も増加することになり、カルマンゲインの演算に用いられる観測行列、つまり、Ｈ行列のサイズが、捕捉したＧＰＳ衛星の数に応じて増加することになる。このため、Ｈ行列のサイズが大きくなるほど、計算負荷も増加することになる。また、捕捉したＧＰＳ衛星までの擬似距離（レンジ）やその変化率（レンジレート）を利用して測定誤差を推定しているため、マルチパスの影響を受けている場合や受信パワーが低い衛星からの観測情報も利用して測定誤差の推定を行うことになり、場合によっては、測定誤差を的確に推定することができない可能性がある。
【００１２】
そこで、この発明は、上記従来の未解決の問題に着目してなされたものであり、電波航法により絶対測位情報（絶対位置、絶対速度）を得ることができない場合、或いは、電波航法による絶対測位の測位精度が低い環境下においても、自律航法における測定誤差を的確に推定し、この測定差を用いて自律航法による測位情報を補正することで、より高精度な測位結果を得ることの可能な測位装置及び測位方法を提供することを目的としている。
【課題を解決するための手段】
【００１３】
上記した課題を解決するために、本発明の測位装置は、複数の衛星からの衛星電波を用いて電波航法により測位を行い、受信機の絶対位置及び絶対速度を含む絶対測位情報を得る電波航法測位手段と、衛星電波の受信パワー及びその衛星の仰角に基づいて高信頼度衛星を選択する高信頼度衛星選択手段と、当該高信頼度衛星選択手段で選択された高信頼度衛星からの衛星電波を用いて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を測定する相対関係観測手段と、自律航法により前記受信機の測位を行う自律航法測位手段と、前記電波航法測位手段で得た前記絶対測位情報及び前記相対関係観測手段で測定した相対関係観測値を用いて前記自律航法測位手段の測位情報の測定誤差を推定する測定誤差推定手段と、当該測定誤差推定手段で推定した測定誤差を用いて前記自律航法測位手段の測位情報を補正して前記受信機の測定位置を得る補正手段と、を備えた測位装置であって、前記測定誤差推定手段は、前記受信機の測位状況に基づいて前記絶対測位情報が高信頼度であるかどうかを判定する高信頼度判定手段と、当該高信頼度判定手段で前記絶対測位情報が高信頼度であると判定されるとき、前記絶対測位情報を用いて前記測定誤差を推定する第１の測定誤差推定手段と、前記高信頼度判定手段で前記絶対測位情報が高信頼度ではないと判定されるとき、前記相対関係観測値を用いて前記測定誤差を推定する第２の測定誤差推定手段と、を備えることを特徴としている。
【００１４】
上記構成によれば、受信機での測位状況に基づいて、電波航法により得られる受信機の絶対測位情報が高信頼度であるかどうかを判定し、高信頼度であると判定されるときには絶対測位情報を用いて第１の測定誤差推定手段により測定誤差の推定を行い、高信頼度ではないと判定されるときには擬似距離及びドップラ周波数を用いて第２の測定誤差推定手段により測定誤差の推定を行う。
【００１５】
そして、自律航法による測位情報を、第１の測定誤差推定手段又は第２の測定誤差推定手段で推定した測定誤差により補正して、受信機の測定位置を得るから、高信頼度の絶対測位情報を得ることができるときには、絶対測位情報を用いて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。逆に、高信頼度の絶対測位情報を得ることができない場合には、信頼度の低い絶対測位情報は用いずに、マルチパス等の影響の少ない高信頼度衛星からの衛星電波を用いて測定した擬似距離やドップラ周波数に基づいて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。したがって、精度の低い情報に基づいて測定誤差の推定を行うことを回避し、より的確な測定誤差を用いて補正をすることで、より的確な測位結果を得ることができる。
【００１６】
また、上記した測位装置において、前記高信頼度判定手段は、前記受信機で前記衛星電波を受信している衛星の数及びその配置と、ポジションシグマとのうちの少なくとも何れか１つに基づいて、前記絶対測位情報が高信頼度であるかどうかを判定することを特徴としている。
上記構成によれば、衛星電波を受信している衛星の数やその配置、或いはポジションシグマのうちの少なくとも１つに基づいて、絶対測位情報が高信頼度であるかどうかを判定するから、絶対測位情報の信頼度が高いかどうかを的確に判定することができる。
【００１７】
また、上記した測位装置において、前記測定誤差推定手段は、カルマンフィルタを用いて前記測定誤差を推定し、前記絶対測位情報及び前記相対関係観測値を、前記カルマンフィルタにおける観測方程式の観測値として用いることを特徴としている。
上記構成によれば、カルマンフィルタを用いて測定誤差を推定し、その観測方程式の観測値として絶対測位情報及び相対関係観測値を用いるから、この観測値で構成される観測値行列のサイズを比較的小さくすることができる。したがって、この観測値行列のサイズが大きいことに起因するカルマンフィルタの演算処理に伴う処理負荷を低減することができる。
【００１８】
また、上記した測位装置において、前記自律航法測位手段は、前記受信機の位置及び速度を測定し、前記第１の測定誤差推定手段は、前記電波航法測位手段で測定した絶対位置及び絶対速度と、前記自律航法測位手段で測定した測定位置及び測定速度との差を前記観測値とすることを特徴としている。
上記構成によれば、第１の測定誤差推定手段は、電波航法測位手段で測定した絶対位置及び絶対速度と、自律航法測位手段で測定した測定位置及び測定速度との差を、カルマンフィルタの観測方程式の観測値として設定するから、観測値行列は、位置誤差と、速度誤差とから構成されることになり、観測値行列のサイズを小さくすることができる。
【００１９】
また、上記した測位装置において、前記第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、前記自律航法測位手段の測位情報と前記高信頼度衛星からの衛星電波に含まれる高信頼度衛星の位置情報とに基づいて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を予測する相対関係予測手段を備え、前記高信頼度衛星の、前記相対関係観測手段で測定した擬似距離と前記相対関係予測手段で予測した擬似距離との差を前記擬似距離の観測方程式の観測値とし、且つ前記相対関係観測手段で測定したドップラ周波数と前記相対関係予測手段で予測したドップラ周波数との差を前記ドップラ周波数の観測方程式の観測値とすることを特徴としている。
【００２０】
上記構成によれば、第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、高信頼度衛星の衛星電波を用いて相対関係観測手段で測定した擬似距離と、相対関係予測手段で予測した擬似距離との差を、擬似距離の観測方程式の観測値として設定し、同様に、相対関係観測手段で測定したドップラ周波数と、相対関係予測手段で予測したドップラ周波数との差を、ドップラ周波数の観測方程式の観測値として設定する。このとき、高信頼度衛星からの衛星電波を利用した相対関係のみを観測値として設定するから、観測値行列のサイズは、高信頼度衛星の数に応じたサイズとなり、さらに、電波航法による高信頼度の絶対測位情報を得ることができない場合の、高信頼度衛星の数はある程度制限されるから、観測値行列のサイズをある程度の値に制限することができ、観測値行列のサイズが増大することを抑制することができる。
【００２１】
また、上記した測位装置において、前記受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信可能に形成されていることを特徴としている。
上記構成によれば、受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信できる。ここで、準天頂衛星は、常に日本の天頂付近に存在するように配置された衛星であるから、受信機では常に準天頂衛星、つまり高信頼度衛星からの衛星電波を受信することになる。したがって、電波航法により高信頼度の絶対位置情報を得ることができずこの絶対位置情報を用いた測定誤差の推定を行うことができない場合であっても、この準天頂衛星を用いて第２の測定誤差推定手段により確実に測定誤差の推定を行うことができる。そして、この準天頂衛星は、高信頼度衛星であるから、的確に測定誤差の推定を行うことができ、ＧＰＳ衛星からの衛星電波の受信状況に関わらず、測定誤差の推定を的確に行うことができる。
【００２２】
また、本発明の測位方法は、複数の衛星からの衛星電波を用いて測位した受信機の絶対位置及び絶対速度を含む絶対測位情報と、高信頼度衛星からの衛星電波を用いて算出した前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数とを用いて、自律航法による測位情報の測定誤差を推定し、推定した測定誤差を用いて前記自律航法による測位情報を補正して前記受信機の測定位置を得る測位方法であって、前記受信機の測位状況に基づき、前記絶対測位情報が高信頼度であるかどうかを判定し、前
記絶対測位情報が高信頼度であると判定されるときには前記絶対測位情報を用いて前記測定誤差を推定し、前記絶対測位情報が高信頼度ではないと判定されるときには前記擬似距離及び前記ドップラ周波数を用いて前記測定誤差を推定することを特徴としている。
【００２３】
上記構成によれば、電波航法による絶対測位情報が高信頼度であると判定されるときにはこの絶対測位情報を用いて、自律航法による測位情報の測定誤差を推定し、前記絶対測位情報が高信頼度でないと判定されるときには、高信頼度衛星までの擬似距離とドップラ周波数とを用いて推定するから、高信頼度の絶対測位情報を得ることができるときにはこの絶対測位情報を用いて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができ、逆に、高信頼度の絶対測位情報を得ることができない場合には、信頼度の低い絶対測位情報は用いずに、マルチパス等の影響の少ない高信頼度衛星からの衛星電波を用いて測定した擬似距離やドップラ周波数に基づいて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。したがって、精度の低い情報に基づいて測定誤差の推定を行うことを回避することができ、測定誤差の推定をより的確に行うことができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の実施の形態を説明する。
図１は、本発明を適用した測位装置１０の一例を示すブロック図であり、この測位装置１０は、地球の大気圏外を周回するＧＰＳ衛星及び準天頂衛星ＱＺＳＳから送信された電波を受信するＧＰＳ／ＱＺＳＳ受信機１と、ジャイロセンサを含んで構成され、測位装置１０本体のヨー方向の角速度を検出する方位センサとしての角速度センサ２及び加速度を検出する加速度センサ３と、前記ＧＰＳ／ＱＺＳＳ受信機１で受信した受信信号に基づいて電波航法により測位を行うと共にＧＰＳ衛星やＱＺＳＳ衛星とＧＰＳ／ＱＺＳＳ受信機１との相対関係を表す衛星までの擬似距離やドップラ周波数等の観測情報を算出し、また、角速度センサ２及び加速度センサ３の検出信号に基づいて自律航法による測位演算を行い、この測位情報を、前記電波航法による絶対測位情報及び観測情報に基づいて推定した自律航法による測位演算の測定誤差に応じて補正し、これを測位結果とする演算処理装置４と、を備え、演算処理装置４では算出した測位結果を、例えば表示部５に出力する。
【００２５】
この演算処理装置４では、前記ＧＰＳ／ＱＺＳＳ受信機１の測位状況に基づいて得た絶対測位情報の信頼度が高いかどうかを判定し、測位演算により得た絶対測位情報の信頼度が高いと判定されるときには、測位結果レベルのフィルタモードで前記自律航法による測位演算の測定誤差の推定を行う。具体的には、電波航法による絶対測位情報（絶対位置、絶対速度）に基づき、自律航法による各種センサの測定誤差や測位演算結果の誤差を推定する。
【００２６】
一方、測位状況から、電波航法により測位演算を行うことはできるがその信頼度は低いと予測されるとき、或いは電波航法により測位演算を行うことはできないと判定されるときには、観測情報レベルのフィルタモードで前記自律航法による測位演算の測定誤差の推定を行う。具体的には、ＧＰＳ／ＱＺＳＳ衛星からの電波の観測情報、つまり、擬似距離及びドップラ周波数に基づき、自律航法による各種センサの測定誤差や測位演算結果の誤差を推定する。
そして、このようにして推定した自律航法による各種センサの測定誤差や測位演算結果の誤差に基づき、各種センサの測定値の補正や測位演算結果の補正を行い、補正後の測位演算結果を、測位装置１０本体の測位結果とする。
【００２７】
図２は、前記演算処理装置４で実行される測位演算処理の処理手順の一例を示すフローチャートである。
演算処理装置４では、ＧＰＳ／ＱＺＳＳ受信機１で、ＧＰＳ衛星やＱＺＳＳ衛星からの
電波を受信し測位演算して、ステップＳ１からステップＳ２に移行し、測位状況を判定し、電波航法により絶対測位情報を得ることができるかどうか、また、絶対測位情報を得ることができる場合には、その絶対測位情報は、高信頼度であるか低信頼度であるかを判断する。
【００２８】
この判断は、例えば、ＧＰＳ／ＱＺＳＳ受信機１で電波を受信することができた捕捉衛星の数、衛星の配置による測位精度の低下率を表す位置精度低下率ＰＤＯＰ及び測位精度を表すポシションシグマ等に基づいて行う。
このようにして測位状況を判定したならばステップＳ３に移行し、測位状況を判定した結果、高信頼度の絶対測位情報を得ることができると判定された場合にはステップＳ４に移行し、測位結果レベルのフィルタモードでの測位演算を行い、電波航法による絶対測位情報（絶対位置、絶対速度）を用いて自律航法における測定誤差の推定を行い、これに基づき所定の補正を行って、測位装置１０本体の測位結果を得る。
【００２９】
一方、測位状況を判定した結果、絶対測位情報を得ることは可能であるが、その信頼度が低い場合、或いは、測位演算を行うことができない場合いはステップＳ５に移行し、捕捉衛星の中から高信頼度衛星を選択する。
具体的には、捕捉しているＧＰＳ衛星、ＱＺＳＳ衛星のそれぞれについて、受信信号のパワーと、その仰角とから判断する。例えば、衛星からの電波に含まれるＰＮコードについて自己が保持するＰＮコードとの相関値を取り、相関値のピーク値と、ノイズフロアの値との差が、予め設定したしきい値以上であり、且つ、衛星の仰角が予め設定したしきい値以上であるとき、この衛星を高信頼度衛星と判断する。
【００３０】
ここで、ＧＰＳ／ＱＺＳＳ受信機１では、準天頂衛星ＱＺＳＳからの電波も受信するようにしており、この準天頂衛星ＱＺＳＳは、常に何れか１つは日本の天頂付近に存在する衛星であり、仰角７０度以上の位置に存在し、ビルの谷間でも遮られることなくマルチパスの影響をうけにくい衛星である。したがって、少なくとも１つは高信頼度衛星が存在することになる。
【００３１】
次いで、ステップＳ６に移行し、高信頼度衛星として判定された捕捉衛星について、その電波の観測情報、つまり、擬似距離及びドップラ周波数に基づいて、観測情報レベルのフィルタモードでの測位演算を行い、観測情報を用いて自律航法における測定誤差の推定を行い、これに基づき所定の補正を行い、測位装置１０本体の測位結果を得る。なお、前記擬似距離は、測位装置１０での衛星電波の受信時刻と受信信号に含まれる衛星の送信時刻との差に光速を乗算することで算出することができる。また、ドップラ周波数は、搬送周波数ｆ〔Ｈｚ〕と、ドップラ効果による周波数偏移Δｆ（＝ｆ・ρ／ｃ）〔Ｈｚ〕とから、ｆ±Δｆで算出することができる。なお、ρは測位装置１０本体と衛星との相対速度、ｃは光速である。
【００３２】
そして、ステップＳ７に移行し、ステップＳ４で算出した測位結果レベルのフィルタモード又はステップＳ６で算出した擬似距離レベルのフィルタモードにより算出した測位結果を表示部５に出力する。そして、ステップＳ１に戻り、上述の処理を繰り返し行う。
【００３３】
図３は、測位結果レベルのフィルタモードでの測位演算の処理手順を示すブロック図である。
この測位結果レベルのフィルタモードでは、ＧＰＳ衛星又はＱＺＳＳ衛星から受信した受信信号に基づいて、測位演算部１１において、公知の電波航法による手順で測位演算を行う。つまり、この測位結果レベルのフィルタモードでは、高信頼度の測位演算が可能と判断されていることから、捕捉衛星のうち、高信頼度の測位演算が可能との判断結果を得た、少なくとも４個以上のＧＰＳ衛星又はＱＺＳＳ衛星からの受信信号を用いて公知の手
順で測位装置１０本体の絶対位置及び絶対速度を算出する。
【００３４】
一方、自律センサナビゲーション１２では、角速度センサ２及び加速度センサ３で検出した角速度及び加速度と、後述のカルマンフィルタ１３で推定された測定誤差とに基づいて公知の自律航法による測位演算を行い、測位装置１０本体の位置、速度及び方位を推測する。そして、測位演算部１１で算出した電波航法による絶対測位情報（絶対位置、絶対速度）と、自律センサナビゲーション１２で算出した自律航法による測位演算結果とに基づいて、カルマンフィルタ１３により、自律航法による測定誤差を推測する。そして、カルマンフィルタ１３により推定した測定誤差と、今回の測位演算結果とを補正部１４で加算し、これを測位装置１０本体の今回の測位結果とする。
【００３５】
ここで、カルマンフィルタについて簡単に説明する。
カルマンフィルタは、観測信号と統計的な先験情報とを用いて、信号の生成過程のモデル化の際に導入された導入変数の最適な推定値（最小分散推定値）を逐次求めるアルゴリズムとして導かれており、予測モードと推定モードとの二段階で処理が繰り返される。
ここで、状態ベクトルをｘとすると、状態空間モデルは、次式（１）及び（２）で表すことができる。なお、変数の添字ｋは、時刻を表している。
【００３６】
ｘk+1＝φkｘk＋ｕk（状態方程式） ……（１）
【００３７】
ｚk＝Ｈkｘk＋εk（観測方程式） ……（２）
【００３８】
なお、式中のｘkは状態ベクトル、φkは状態遷移行列、ｕkはプロセスノイズとしての
ガウス白色誤差（平均ベクトル０、共分散行列Ｑkをもつ正規分布Ｎ（０，Ｑk）で表すことができる）、ｚkは観測値、Ｈkは観測値ｚkと状態ベクトルｘkとを関連づけるための観測行列、εkは観測値の誤差としてのガウス白色誤差（平均ベクトル０，共分散行列Ｒkをもつ正規分布Ｎ（０，Ｒk）で表すことができる）である。
【００３９】
予測モードでは、（３）式に示す状態ベクトルの予測と、（４）式に示す推定誤差共分散の予測とを行い、（５）式にしたがってカルマンゲインＫkを算出する。
また、推定モードでは、（６）式に示す状態ベクトルの推定と、（７）式に示す推定誤差共分散の推定とを行う。
【００４０】
ｘ-k＝φkｘ+k-1 ……（３）
【００４１】
Ｐ-k＝φkＰ+k-1φkT＋Ｑk-1 ……（４）
【００４２】
Ｋk ＝Ｐ-kＨkT［ＨkＰ-kＨkT＋Ｒk］-1 ……（５）
【００４３】
ｘ+k＝ｘ-k＋Ｋk［ｚk−Ｈkｘ-k］ ……（６）
【００４４】
Ｐ+k＝［Ｉ−ＫkＨk］Ｐ-k ……（７）
【００４５】
なお、式中の添字Ｔは転置行列を表す。また、ｘ-kは状態ベクトルｘの予測値、ｘ+k、ｘ+k-1は状態ベクトルの推定値、Ｐ-kは推定誤差共分散の予測値、Ｐ+k、Ｐ+k-1は推定誤差共分散の推定値を表す。また、φkは状態遷移行列、Ｑk-1は前記プロセスノイズとしてのガウス白色誤差ｕの共分散行列、Ｈkは前記観測行列、Ｒkは前記観測値の誤差としてのガウス白色誤差εkの共分散行列である。
【００４６】
そして、上記（１）、（２）式の状態空間モデル及び観測値ｚkを用い、前記（３）〜
（７）式を繰り返し行うことにより、観測値ｚkに応じてカルマンフィルタゲインＫkが収束し、より的確な状態ベクトルｘ+kの推測を行うものである。
次に、図１に示すように、ＧＰＳ／ＱＺＳＳ受信機１、角速度センサ２及び加速度センサ３を備えた測位装置１０において、カルマンフィルタ１３で処理を行う場合について説明する。なお、ここでは、説明を簡単にするために、二次元ナビゲーションを行う場合について説明する。
【００４７】
測位装置１０における、状態ベクトルＸを、次式（８）で表す。
Ｘ＝［δＰE δＰN δＶE δＶN δθg Ｂg Ｂａx Ｂａy］T ……（８）
【００４８】
なお、（８）式中の、δＰEは東方向位置誤差、δＰNは北方向位置誤差、δＶEは東方
向速度誤差、δＶNは北方向速度誤差、δθgはヨー方向角度誤差、Ｂgは角速度センサの
バイアス、ＢａxはＸ軸方向の加速度センサのバイアス、ＢａyはＹ軸方向の加速度センサのバイアスである。
【００４９】
前記角速度センサのバイアス及び加速度バイアスは確定的な変化はなく、前回の誤差にノイズが負荷されたものして、次式（９）〜（１１）により表すことができる。
【００５０】
Ｂg（ｋ＋１）＝Ｂg（ｋ）＋ｗg ……（９）
【００５１】
Ｂａx（ｋ＋１）＝Ｂａx（ｋ）＋ｗax ……（１０）
【００５２】
Ｂａy（ｋ＋１）＝Ｂａy（ｋ）＋ｗay ……（１１）
【００５３】
また、ヨー方向角度誤差δθgは、次式（１２）に示すように、前回の角速度センサの
バイアスＢgに前回からの経過時間すなわちサンプリング間隔Ｔを乗算することにより算
出される方位誤差とノイズとを、前回の誤差に加算して算出される。
【００５４】
δθg（ｋ＋１）＝δθg（ｋ）＋Ｔ・Ｂg（ｋ）＋ｗθg ……（１２）
【００５５】
ここで、状態方程式を、次式（１３）とする。
Ｘk+1＝φＸk＋ｗ ……（１３）
【００５６】
なお、式中のφは、状態ベクトルの時間的な変化を与える状態遷移行列、ｗはシステムノイズであって次式（１４）及び（１５）で表す。なお、式中のＴはサンプリング間隔、ａxはＸ軸方向の加速度、ａyはＹ軸方向の加速度、θはヨー方向角度である。
【００５７】
【数１】
【００５８】
次に、観測方程式を、次式（１６）とする。
Ｙ＝Ｈ・Ｘ＋ｒ ……（１６）
【００５９】
なお、Ｙは観測値であって次式（１７）で表し、Ｈは観測値Ｙと状態ベクトルＸとを関連付けるための観測行列、ｒは観測ノイズである。
【００６０】
【数２】
【００６１】
（１７）式中のＰE,INS、ＰN,INS、ＶE,INS、ＶN,INSは、自律航法による測位演算により得た、東方向位置、北方向位置、東方向速度、北方向速度であり、ＰE,GPS、ＰN,GPS、ＶE,GPS、ＶN,GPSは、電波航法による測位演算により得た、東方向位置、北方向位置、東方向速度、北方向速度である。
そして、まず、測位装置１０の初期測位位置（ｘ，ｙ，ｚ）を設定し、さらに、状態ベクトル推定値Ｘ+k-1及び誤差共分散行列Ｐの推定値Ｐ+k-1として、所定の初期値を設定する。この初期値は、例えば、最小自乗法により測位演算を行うことにより、現在の測位装置１０の位置と状態ベクトル推定値Ｘ+k-1とを求めることができる。また、誤差共分散行列Ｐの推定値Ｐ+k-1としては予め設定した所定の値を設定する。
【００６２】
なお、ここでは、測位装置１０の初期値及び状態ベクトルの初期値として、最小自乗法による演算結果を用いた場合について説明したがこれに限るものではなく、他の手法により初期値を設定してもよい。
そして、前記測位演算部１１で電波航法により測位演算を行い、東方向位置ＰE,GPS、
北方向位置ＰN,GPS、東方向速度ＶE,GPS、北方向速度ＶN,GPSを獲得する。同様に、自律
センサナビゲーション１２において、自律航法により、東方向位置ＰE,INS、北方向位置
ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを獲得する。
【００６３】
そして、これら電波航法及び自律航法による測位演算結果及び、前記（１６）及び（１７）式から、観測方程式を得る。
そして、前記（３）及び（４）式から、状態ベクトルＸ-k及び推定誤差行列分散Ｐ-kの予測を行い、（５）式からカルマンフィルタゲインＫkを算出する。
次いで、（５）式から得たカルマンフィルタゲインＫkと前記（６）及び（７）式から
、状態ベクトルＸ+k及び推定誤差行列分散Ｐ+kの推定を行う。
【００６４】
以上の手順を、繰り返し行い、前記（８）式で表す状態ベクトルＸの各成分、つまり、自律航法において測位演算を行う際の、測位演算結果の誤差や、角速度センサ２や加速度センサ３による測定誤差を推測する。
そして、この推定した測定誤差を用いて補正部１４において、自律航法により演算した東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ１３により得た測定誤差の推定値を自律センサナビゲーション１２にフィードバックする。
【００６５】
そして、前記自律センサナビゲーション１２では、測定誤差の推定値をもとに今回の測位結果、つまり、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向
速度ＶN,INSを補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差
δθg、角速度センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方
向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処
理を行う際に、これら測定誤差の推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、測定誤差の推定値により補正された前回の測位結果とをもとに、今回の測位演算を行う。
【００６６】
次に、図４は、観測情報レベルのフィルタモードでの処理手順を示すブロック図である。
この観測情報レベルでのフィルタモードでは、測位演算部２１において、捕捉しているＧＰＳ衛星又はＱＺＳＳ衛星について、ＰＮコードの相関値や仰角に基づいて高信頼度衛星として選択された衛星についてその衛星までの擬似距離及びドップラ周波数を、観測擬似距離及び観測ドップラ周波数として算出する。
【００６７】
つまり、この観測情報レベルのフィルタモードでは、４個以上のＧＰＳ衛星又はＱＺＳＳ衛星からの電波を受信しておらず測位演算を行うことができないか、又は、電波航法により高信頼度の測位結果を得ることができない状況であるから、高信頼度衛星、つまり、マルチパス等の影響を受けていない信頼度の高い受信信号に基づく擬似距離及びドップラ周波数を用いて、自律航法による測定誤差の推定を行う。
【００６８】
一方、自律ナビゲーション２２では、角速度センサ２及び加速度センサ３で検出した角速度及び加速度、及び後述のカルマンフィルタ１３で推定された測定誤差の推定値に基づいて、公知の自律航法による測位演算を行い、測位装置１０本体の位置、速度及び方位を
算出する。さらに、測位演算の結果得た測位装置１０本体の位置と、受信信号に含まれる高信頼度衛星の今回の位置とに基づいて高信頼度衛星までの擬似距離を予測する。また、測位装置１０本体の速度と、高信頼度衛星の速度とに基づいて測位装置１０及び高信頼度衛星との間の相対速度を算出し、これに基づいてドップラ周波数を予測する。そして、これらを予測擬似距離及び予測ドップラ周波数とする。
【００６９】
そして、測位演算部２１で選択した高信頼度衛星の観測擬似距離及び観測ドップラ周波数と、自律センサナビゲーション２２で予測した予測擬似距離及び予測ドップラ周波数とに基づいて、カルマンフィルタ２３により、自律航法による測定推定結果の推定誤差を推測する。そして、この推定した測定誤差を用いて補正部２４において、自律航法により演算した測位結果、すなわち、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ２３により得た測定誤差の推定値を自律センサナビゲーション２２にフィードバックする。
【００７０】
そして、前記自律センサナビゲーション２２では、測定誤差の推定値をもとに今回の測位結果、つまり、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向
速度ＶN,INSを補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差
δθg、角速度センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方
向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処
理を行う際に、これら測定誤差の推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、測定誤差の推定値により補正された前回の測位結果とをもとに、今回の測位演算を行う。
【００７１】
この観測情報レベルのフィルタモードにおけるカルマンフィルタ２３では、拡張カルマンフィルタを適用して線形化し、観測方程式を、次式（１８）及び（１９）とする。
【００７２】
Ｙ１＝Ｈ・Ｘ＋ｒ１ ……（１８）
Ｙ２＝Ｈ・Ｘ＋ｒ２ ……（１９）
【００７３】
なお、Ｙ１、Ｙ２は観測値であって次式（２０）、（２１）で表す。
【００７４】
【数３】
【００７５】
そして、上述の測位結果レベルのフィルタモードと同様に、まず、測位装置１０本体の初期測位位置（ｘ，ｙ，ｚ）を設定し、さらに、状態ベクトル推定値Ｘ+k-1及び誤差共分散行列Ｐの推定値Ｐ+k-1として、所定の初期値を設定する。
【００７６】
そして、前記測位演算部２１及び自律センサナビゲーション２２からの高信頼度衛星の
観測擬似距離及び観測ドップラ周波数と、予測擬似距離及び予測ドップラ周波数と、前記（１８）〜（２１）から観測方程式を得る。
そして、前記（１）及び（２）式から、状態ベクトルＸ-k及び推定誤差行列分散Ｐ-kの予測を行い、（３）式からカルマンフィルタゲインＫkを算出する。
【００７７】
次いで、（３）式から得たカルマンフィルタゲインＫkと前記（４）及び（５）式から
、状態ベクトルＸ+k及び推定誤差行列分散Ｐ+kの推定を行う。
以上の手順を繰り返し行い、前記（８）式で表す状態ベクトルＸの各成分、つまり、自律航法において測位演算を行う際の、測位演算結果の誤差や、角速度センサ２や加速度センサ３による測定誤差を推測する。
【００７８】
そして、この推測した測定誤差を用いて、補正部２４において、自律航法により演算した東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ２３により得た測定誤差を自律センサナビゲーション２２にフィードバックする。
【００７９】
そして、前記自律センサナビゲーション２２では、測定誤差をもとに今回の測位結果を補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差δθg、角速度
センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処理を行う際に、
これら測定誤差推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、推測した測定誤差により補正した前回の測位結果とをもとに、今回の測位演算を行う。
【００８０】
次に、本発明の動作を説明する。
ＧＰＳ／ＱＺＳＳ受信機１で、ＧＰＳ衛星又はＱＺＳＳ衛星から電波を受信し、測位演算して、捕捉衛星の数、捕捉衛星の配置に応じた位置精度低下率ＰＤＯＰ、ポシションシグマ等に基づいて、測位状況を判断し、電波航法により高信頼度の絶対測位情報を獲得することができるかどうかを判定する（ステップＳ１、Ｓ２）。
【００８１】
そして、高信頼度の絶対測位情報を得ることができると判定されたときには、ステップＳ３からステップＳ４に移行し、測位結果レベルのフィルタモードで測定誤差の推定を行う。つまり、電波航法により算出した測位装置１０本体の絶対位置及び絶対速度と、角速度センサ２及び加速度センサ３の検出信号と、前回のサンプリング周期で算出されたヨー角方向角度誤差δθg、ジャイロバイアスＢg、加速度センサのバイアスＢａx、Ｂａy及び、東方向及び北方向の位置誤差δＰE、δＰN、東方向及び北方向の速度誤差δＶE、δＶNにより補正した前回の測位装置１０本体の測位結果とに基づいて、今回の位置、速度及び方位を算出する。
【００８２】
そして、これらに基づき前記（１７）式にしたがって測定誤差の観測値を設定し、カルマンフィルタ処理を行って、測定誤差を推定する。そして、この測定誤差の推定値により、自律航法により得た測位結果を補正し、補正後の値を測位装置１０本体の測位結果として表示部５に出力する。また、測定誤差の推定値に基づいて、自律航法による測位結果を補正しこれを今回の測位結果として記憶すると共に、角速度センサ２や加速度センサ３の測定誤差の推定値を所定の記憶領域に記憶する。
以上の処理を、高信頼度の測位演算を行うことが可能な間、繰り返し行う。
【００８３】
ここで、前記カルマンフィルタ処理では、前記（１７）式に示すように、測定値として、電波航法により得た東方向及び北方向の位置及び東方向及び北方向の速度を用いて処理
を行っており、４×１の行列で構成される。
カルマンフィルタ処理では、（５）式に示すように、観測方程式の観測行列の逆行列を用いて演算を行っているため、この観測行列のサイズが大きい場合には演算処理装置４の演算処理負荷が増加することになる。しかしながら、（１７）式に示すように、観測値Ｙとして、４×１の行列を用い、比較的サイズの小さい行列を用いているから、処理負荷の増加を低減することができる。
【００８４】
また、このとき、電波航法により測位演算された高信頼度の測位結果と、自律航法による測位結果とに基づいて、測定誤差の観測値Ｙを設定しているから、マルチパス等の影響を受けていないより誤差の少ない観測値Ｙを用いて測定誤差を推定することができ、より的確に測定誤差の推定を行うことができる。
したがって、このようにして推定した測定誤差を用いて、自律航法による測位結果を補正しこれを用いて測位装置１０本体の位置を得ることで、より的確な測位結果を得ることができる。
【００８５】
一方、電波航法による絶対測位情報を得ることはできるがその信頼度が比較的低いと判定される場合、或いは、４個以上の衛星を捕捉することができず測位演算を行うことができない場合には、ステップＳ３からステップＳ５に移行し、ＰＮコードの相関値や衛星の仰角に基づき、捕捉衛星の中から高信頼度衛星を選択し、この高信頼度衛星の観測擬似距離及び観測ドップラ周波数を獲得する。
また、高信頼度衛星の位置、速度及び受信機の位置、速度に基づいて、今回の擬似距離及びドップラ周波数を予測し、これらに基づき前記（２０）、（２１）式から測定誤差の観測値を設定し、カルマンフィルタ処理を行って、測定誤差の推定値を算出する。
【００８６】
そして、この測定誤差の推定値により、自律航法により得た測位結果を補正し、補正後の測位結果を、測位装置１０本体の測位結果として出力する。また、測定誤差の推定値に基づいて、自律航法による測位結果を補正しこれを今回の測位結果として記憶すると共に、角速度センサ２や加速度センサ３の測定誤差の推定値を所定の記憶領域に記憶する。
この処理を、電波航法による測位結果が比較的低信頼度である間、或いは、４個以上の衛星を捕捉することができず自律航法による測位結果を得ることができない間、繰り返し行う。
【００８７】
ここで、前記カルマンフィルタ処理では、前記（２０）及び（２１）式に示すように、測定値として、高信頼度衛星からの観測擬似距離及び観測ドップラ周波数と、自律航法による測位結果に基づき算出した予測擬似距離及び予測ドップラ周波数との差を、観測値として用いており、高信頼度衛星の数をｎとすると、２ｎ×１の行列で構成される。前述のように、電波航法により高信頼度な測位結果を得ることができる場合には、測位結果レベルのフィルタモードでの測位演算を行うから、高信頼度衛星であると判定される衛星の数は、ある程度制限される。したがって、前記観測値Ｙは、比較的サイズの小さな行列で表されることになる。
【００８８】
このため、この観測情報レベルのフィルタモードの場合も、観測値Ｙとして、比較的サイズの小さい行列を用いているから、カルマンフィルタ処理を実行する際の演算処理装置４での処理負荷を低減することができる。
また、このとき、捕捉衛星のうち、高信頼度衛星と判断した衛星からの精度の高い観測擬似距離及び観測ドップラ周波数を利用して測定誤差の観測値Ｙを設定しているから、より誤差の少ない測定誤差の観測値を用いて測定誤差を推定することができる。したがって、このようにして推定した誤差のより少ない測定誤差を用いて、自律航法による測位結果を補正することで、より的確な、測位装置１０本体の測位結果を得ることができる。特に、マルチパスの影響を受けた場合には精度が大幅に低下するが、上述のように高信頼度衛
星と判断した衛星の観測擬似距離及び観測ドップラ周波数を利用することによって、より的確な測位結果を得ることができる。
【００８９】
また、高信頼度衛星がない場合には、観測値Ｙを設定することができないため、カルマンフィルタ処理では予測モードだけで誤差推定を行わざるを得ないが、上述のように、ＧＰＳ／ＱＺＳＳ受信機１では、ＧＰＳ衛星だけでなく、ＱＺＳＳ衛星からの電波も受信する。前記ＱＺＳＳ衛星は、常に日本の天頂付近に存在する仰角７０度以上の位置に存在する衛星であって、マルチパスの影響の少ない高信頼度衛星であるから、少なくとも高信頼度衛星が１つは存在することになる。
【００９０】
したがって、仮に、高信頼度衛星であるＧＰＳ衛星が存在しない場合であっても、必ず１つは高信頼度衛星（ＱＺＳＳ衛星）が存在し、その電波を受信することができるから、高信頼度衛星からのより適切な観測擬似距離及び観測ドップラ周波数を獲得することができる。
したがって、これらの、より適切な観測擬似距離及び観測ドップラ周波数を用いることによって、より適切に測定誤差の推定を行うことができる。よって、この測定誤差を用いて、自律航法による測位結果を補正することで、より誤差の少ない測位結果を得ることができる。
【００９１】
なお、上記実施の形態においては、ＧＰＳ衛星及びＱＺＳＳ衛星からの電波を受信する場合について説明したが、例えば、ＧＰＳ衛星からの電波のみを受信する場合であっても適用することができ、この場合も上記と同等の作用効果を得ることができる。また、ＱＺＳＳ衛星を利用しない場合には、ＧＰＳ衛星からの電波の受信状況によっては高信頼度衛星が一つも存在しない場合がある。高信頼度衛星ではない衛星からの受信信号に基づく測位演算を行った場合、その測位精度は低下する。このため、このように高信頼度衛星を確保することができない場合には観測値は用いずに、カルマンフィルタ処理において予測モードのみにより測定誤差の推定を行うようにしてもよく、或いは、多少測位精度が低下する可能性があるが、捕捉衛星のうち、信頼度が比較的高い衛星からの受信信号を用いて観測値を設定するようにしてもよく、運用状況に応じて選択すればよい。
【００９２】
ここで、上記実施の形態において、図３の測位演算部１１が電波航法測位手段に対応し、図２のステップＳ５の処理が高信頼度衛星選択手段に対応し、図４の測位演算部２１が相対関係観測手段に対応し、図３及び図４の自律センサナビゲーション１２及び２２が自律航法測位手段に対応し、図３及び図４のカルマンフィルタ１３及び２３が測定誤差推定手段に対応し、図３及び図４の補正部１４及び２４が補正手段に対応している。また、図２のステップＳ２及びステップＳ３の処理が高信頼度判定手段に対応し、ステップＳ４の処理が第１の測定誤差推定手段に対応し、ステップＳ６の処理が第２の測定誤差推定手段に対応している。
【００９３】
また、図４の自律センサナビゲーション２２で、予測擬似距離及び予測ドップラ周波数を算出する処理が相対関係予測手段に対応している。
【図面の簡単な説明】
【００９４】
【図１】本発明の測位装置の一例を示すブロック図である。
【図２】演算処理装置で実行される測位演算処理の処理手順の一例を示すフローチャートである。
【図３】測位結果レベルのフィルタモードでの測位演算の手順を示すブロック図である。
【図４】観測情報レベルのフィルタモードでの測位演算の手順を示すブロック図である。
【符号の説明】
【００９５】
１ ＧＰＳ／ＱＺＳＳ受信機、２ 角速度センサ、３ 加速度センサ、４ 演算処理装置、１１、２１ 測位演算部、 １２、２２ 自律センサナビゲーション、１３、２３ カルマンフィルタ、１４、２４ 補正部
【技術分野】
【０００１】
本発明は、ＧＰＳ衛星等からの電波を利用して測位を行う電波航法により得られる測定値と、自律航法により得られる測定値とに基づいて、測位を行うようにした測位装置及び測位方法に関する。
【背景技術】
【０００２】
近年、車載型端末装置や、携帯型端末装置等、さまざまな分野で、車両や歩行者の現在位置情報の提供が求められている。この現在位置の測位手法として、ＧＰＳ（グローバルポジショニングシステム）等による電波航法、角速度センサ等の方位センサや、加速度センサ、車速センサ等の速度センサを用いた自律航法、等の手法が提案されている。
ＧＰＳ衛星を用いた電波航法には、長時間安定性がある一方、測位環境への依存性がある。つまり、高層ビルが立ち並ぶ都市部や、山間部では、ＧＰＳ衛星からの電波が、建物等に遮断されると、測定に十分な数の衛星を捕捉することができず、測位できなくなる可能性がある。また、建物等に反射されたマルチパスの影響を受けると、測位精度の低下につながる可能性がある。
【０００３】
このようなＧＰＳ衛星の欠点を補う次世代衛星システムとして、準天頂衛星（ＱＺＳＳ）が注目を集めている。すなわち、このＱＺＳＳ衛星を、常に日本の天頂付近に配置し、このＱＺＳＳ衛星を利用してＧＰＳ衛星の補間や補強を行うことで、測位サービスの改善が期待されている。
一方、自律航法では、各種センサで計測される角速度や、加速度を積分することで、絶対位置や絶対方位を推測している。このため、短時間での推測精度は良いが、計測値の誤差の影響により、この計測値を積分して得られる推測位置の誤差は、時間と共に増加する。
【０００４】
このような電波航法及び自律航法によるそれぞれの欠点を補う為に、複数種類の航法手段を組み合わせて、速度や位置の検出精度を高めるようにしたハイブリッド航法が提案されている。
このハイブリッド航法においては、例えば、カルマンフィルタを用い、電波航法による計測値を利用して、自律航法による計測値の測定誤差を推定し、これを用いて自律航法による計測値の補正を行っている。この測定誤差の推定方法として、例えば、Loosely Coupled Modeと、Tightly Coupled Modeとがある。
【０００５】
前記Loosely Coupled Modeは、電波航法では、４個以上のＧＰＳ衛星を捕捉することができれば、測位演算を行うことで移動体の絶対位置を特定することができることから、この電波航法による絶対測位情報（絶対位置、絶対速度）を利用して、カルマンフィルタにより、自律航法で用いる各種自律センサの測定誤差を推定し、推定した自律センサの誤差を用いて、自律センサによる測定値を補正する。
【０００６】
一方、Tightly Coupled Modeは、電波航法により得られる絶対測位情報（絶対位置、絶対速度）の代わりに、ＧＰＳ衛星までの擬似距離（レンジ）やドップラ周波数を観測値として利用して、自律センサの測定誤差の推定を行うものである。
また、例えば、特許文献１には、常に４個のＧＰＳ衛星を捕捉することができなくても、少なくとも、１個のＧＰＳ衛星を捕捉することができれば、捕捉したＧＰＳ衛星までの擬似距離（レンジ）や、その変化率（レンジレート）を用いて、航法装置に搭載された自律センサの測定誤差を推定するようにした、航法装置が提案されている。
【特許文献１】特開平６−３４１８４７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
ところで、上記Loosely Coupled Modeにおいては、その観測モデルは簡単なため、フィルタのサイズは小さく、実現しやすく、また、受信機の機種への依存性がなく移植性もよい。その反面、ＧＰＳ衛星を４個以上捕捉することができない場合には、絶対測位情報（絶対位置、絶対速度）を得ることができない。カルマンフィルタでは、測定誤差の前回の推定値から測定誤差の予測を行う予測モードと、この予測モードで予測した測定誤差と、電波航法により算出した絶対測位情報（絶対位置、絶対速度）とを用いて測定誤差を推定する推定モードとを繰り返し行うことで、測定誤差を推定しているため、前述のように、絶対測位情報（絶対位置、絶対速度）を得ることができない場合には、予測モードのみで測定誤差の推定を行うことになる。
【０００８】
したがって、測定誤差の推定精度は、主にシステムモデルの精度や、システムノイズ、また、電波航法による測位ができなくなる直前の測定誤差の推定精度に左右されることになり、時間と共に低下し、特に、高層ビルが立ち並ぶ都市部や、山間部では、衛星からの電波は建物等により遮断されたり、反射されたりすることがよくあるための、測位精度の向上が課題となっている。
【０００９】
また、Tightly Coupled Modeにおいては、擬似距離（レンジ）やドップラ周波数は、少なくとも１個のＧＰＳ衛星を捕捉することができれば得ることができる。したがって、常に４個以上のＧＰＳ衛星を捕捉することができない場合であっても、自律航法で用いる自律センサの測定誤差の推定を行うことができる。その反面、観測モデルが複雑になるため、フィルタのサイズが大きくなり、計算負荷も重くなる。また、絶対測位情報（絶対位置、絶対速度）ではなく、ＧＰＳ衛星までの擬似距離（レンジ）や、ドップラ周波数を利用するため、ＧＰＳ受信機の機種へ依存するという問題がある。
【００１０】
また、上述の特許文献１記載の発明のように、ＧＰＳ衛星までの擬似距離（レンジ）やその変化率（レンジレート）を用いて測定誤差を推測する方法においては、電波航法によって絶対測位情報（絶対位置、絶対速度）を確保することができなくても、少なくとも1
個のＧＰＳ衛星を捕捉することができれば、その擬似距離（レンジ）やその変化率（レンジレート）を用いることで、自律センサの測定誤差を推定し、自律センサの測定値の補正を行うことができるため、常に高い精度で位置推定を行うことができる。
【００１１】
その反面、ＧＰＳ衛星を多数受信している場合には、測定誤差の推定に用いる擬似距離（レンジ）やその変化率（レンジレート）の数も増加することになり、カルマンゲインの演算に用いられる観測行列、つまり、Ｈ行列のサイズが、捕捉したＧＰＳ衛星の数に応じて増加することになる。このため、Ｈ行列のサイズが大きくなるほど、計算負荷も増加することになる。また、捕捉したＧＰＳ衛星までの擬似距離（レンジ）やその変化率（レンジレート）を利用して測定誤差を推定しているため、マルチパスの影響を受けている場合や受信パワーが低い衛星からの観測情報も利用して測定誤差の推定を行うことになり、場合によっては、測定誤差を的確に推定することができない可能性がある。
【００１２】
そこで、この発明は、上記従来の未解決の問題に着目してなされたものであり、電波航法により絶対測位情報（絶対位置、絶対速度）を得ることができない場合、或いは、電波航法による絶対測位の測位精度が低い環境下においても、自律航法における測定誤差を的確に推定し、この測定差を用いて自律航法による測位情報を補正することで、より高精度な測位結果を得ることの可能な測位装置及び測位方法を提供することを目的としている。
【課題を解決するための手段】
【００１３】
上記した課題を解決するために、本発明の測位装置は、複数の衛星からの衛星電波を用いて電波航法により測位を行い、受信機の絶対位置及び絶対速度を含む絶対測位情報を得る電波航法測位手段と、衛星電波の受信パワー及びその衛星の仰角に基づいて高信頼度衛星を選択する高信頼度衛星選択手段と、当該高信頼度衛星選択手段で選択された高信頼度衛星からの衛星電波を用いて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を測定する相対関係観測手段と、自律航法により前記受信機の測位を行う自律航法測位手段と、前記電波航法測位手段で得た前記絶対測位情報及び前記相対関係観測手段で測定した相対関係観測値を用いて前記自律航法測位手段の測位情報の測定誤差を推定する測定誤差推定手段と、当該測定誤差推定手段で推定した測定誤差を用いて前記自律航法測位手段の測位情報を補正して前記受信機の測定位置を得る補正手段と、を備えた測位装置であって、前記測定誤差推定手段は、前記受信機の測位状況に基づいて前記絶対測位情報が高信頼度であるかどうかを判定する高信頼度判定手段と、当該高信頼度判定手段で前記絶対測位情報が高信頼度であると判定されるとき、前記絶対測位情報を用いて前記測定誤差を推定する第１の測定誤差推定手段と、前記高信頼度判定手段で前記絶対測位情報が高信頼度ではないと判定されるとき、前記相対関係観測値を用いて前記測定誤差を推定する第２の測定誤差推定手段と、を備えることを特徴としている。
【００１４】
上記構成によれば、受信機での測位状況に基づいて、電波航法により得られる受信機の絶対測位情報が高信頼度であるかどうかを判定し、高信頼度であると判定されるときには絶対測位情報を用いて第１の測定誤差推定手段により測定誤差の推定を行い、高信頼度ではないと判定されるときには擬似距離及びドップラ周波数を用いて第２の測定誤差推定手段により測定誤差の推定を行う。
【００１５】
そして、自律航法による測位情報を、第１の測定誤差推定手段又は第２の測定誤差推定手段で推定した測定誤差により補正して、受信機の測定位置を得るから、高信頼度の絶対測位情報を得ることができるときには、絶対測位情報を用いて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。逆に、高信頼度の絶対測位情報を得ることができない場合には、信頼度の低い絶対測位情報は用いずに、マルチパス等の影響の少ない高信頼度衛星からの衛星電波を用いて測定した擬似距離やドップラ周波数に基づいて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。したがって、精度の低い情報に基づいて測定誤差の推定を行うことを回避し、より的確な測定誤差を用いて補正をすることで、より的確な測位結果を得ることができる。
【００１６】
また、上記した測位装置において、前記高信頼度判定手段は、前記受信機で前記衛星電波を受信している衛星の数及びその配置と、ポジションシグマとのうちの少なくとも何れか１つに基づいて、前記絶対測位情報が高信頼度であるかどうかを判定することを特徴としている。
上記構成によれば、衛星電波を受信している衛星の数やその配置、或いはポジションシグマのうちの少なくとも１つに基づいて、絶対測位情報が高信頼度であるかどうかを判定するから、絶対測位情報の信頼度が高いかどうかを的確に判定することができる。
【００１７】
また、上記した測位装置において、前記測定誤差推定手段は、カルマンフィルタを用いて前記測定誤差を推定し、前記絶対測位情報及び前記相対関係観測値を、前記カルマンフィルタにおける観測方程式の観測値として用いることを特徴としている。
上記構成によれば、カルマンフィルタを用いて測定誤差を推定し、その観測方程式の観測値として絶対測位情報及び相対関係観測値を用いるから、この観測値で構成される観測値行列のサイズを比較的小さくすることができる。したがって、この観測値行列のサイズが大きいことに起因するカルマンフィルタの演算処理に伴う処理負荷を低減することができる。
【００１８】
また、上記した測位装置において、前記自律航法測位手段は、前記受信機の位置及び速度を測定し、前記第１の測定誤差推定手段は、前記電波航法測位手段で測定した絶対位置及び絶対速度と、前記自律航法測位手段で測定した測定位置及び測定速度との差を前記観測値とすることを特徴としている。
上記構成によれば、第１の測定誤差推定手段は、電波航法測位手段で測定した絶対位置及び絶対速度と、自律航法測位手段で測定した測定位置及び測定速度との差を、カルマンフィルタの観測方程式の観測値として設定するから、観測値行列は、位置誤差と、速度誤差とから構成されることになり、観測値行列のサイズを小さくすることができる。
【００１９】
また、上記した測位装置において、前記第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、前記自律航法測位手段の測位情報と前記高信頼度衛星からの衛星電波に含まれる高信頼度衛星の位置情報とに基づいて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を予測する相対関係予測手段を備え、前記高信頼度衛星の、前記相対関係観測手段で測定した擬似距離と前記相対関係予測手段で予測した擬似距離との差を前記擬似距離の観測方程式の観測値とし、且つ前記相対関係観測手段で測定したドップラ周波数と前記相対関係予測手段で予測したドップラ周波数との差を前記ドップラ周波数の観測方程式の観測値とすることを特徴としている。
【００２０】
上記構成によれば、第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、高信頼度衛星の衛星電波を用いて相対関係観測手段で測定した擬似距離と、相対関係予測手段で予測した擬似距離との差を、擬似距離の観測方程式の観測値として設定し、同様に、相対関係観測手段で測定したドップラ周波数と、相対関係予測手段で予測したドップラ周波数との差を、ドップラ周波数の観測方程式の観測値として設定する。このとき、高信頼度衛星からの衛星電波を利用した相対関係のみを観測値として設定するから、観測値行列のサイズは、高信頼度衛星の数に応じたサイズとなり、さらに、電波航法による高信頼度の絶対測位情報を得ることができない場合の、高信頼度衛星の数はある程度制限されるから、観測値行列のサイズをある程度の値に制限することができ、観測値行列のサイズが増大することを抑制することができる。
【００２１】
また、上記した測位装置において、前記受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信可能に形成されていることを特徴としている。
上記構成によれば、受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信できる。ここで、準天頂衛星は、常に日本の天頂付近に存在するように配置された衛星であるから、受信機では常に準天頂衛星、つまり高信頼度衛星からの衛星電波を受信することになる。したがって、電波航法により高信頼度の絶対位置情報を得ることができずこの絶対位置情報を用いた測定誤差の推定を行うことができない場合であっても、この準天頂衛星を用いて第２の測定誤差推定手段により確実に測定誤差の推定を行うことができる。そして、この準天頂衛星は、高信頼度衛星であるから、的確に測定誤差の推定を行うことができ、ＧＰＳ衛星からの衛星電波の受信状況に関わらず、測定誤差の推定を的確に行うことができる。
【００２２】
また、本発明の測位方法は、複数の衛星からの衛星電波を用いて測位した受信機の絶対位置及び絶対速度を含む絶対測位情報と、高信頼度衛星からの衛星電波を用いて算出した前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数とを用いて、自律航法による測位情報の測定誤差を推定し、推定した測定誤差を用いて前記自律航法による測位情報を補正して前記受信機の測定位置を得る測位方法であって、前記受信機の測位状況に基づき、前記絶対測位情報が高信頼度であるかどうかを判定し、前
記絶対測位情報が高信頼度であると判定されるときには前記絶対測位情報を用いて前記測定誤差を推定し、前記絶対測位情報が高信頼度ではないと判定されるときには前記擬似距離及び前記ドップラ周波数を用いて前記測定誤差を推定することを特徴としている。
【００２３】
上記構成によれば、電波航法による絶対測位情報が高信頼度であると判定されるときにはこの絶対測位情報を用いて、自律航法による測位情報の測定誤差を推定し、前記絶対測位情報が高信頼度でないと判定されるときには、高信頼度衛星までの擬似距離とドップラ周波数とを用いて推定するから、高信頼度の絶対測位情報を得ることができるときにはこの絶対測位情報を用いて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができ、逆に、高信頼度の絶対測位情報を得ることができない場合には、信頼度の低い絶対測位情報は用いずに、マルチパス等の影響の少ない高信頼度衛星からの衛星電波を用いて測定した擬似距離やドップラ周波数に基づいて測定誤差を推定することで、マルチパス等の影響の少ない測定誤差を推定することができる。したがって、精度の低い情報に基づいて測定誤差の推定を行うことを回避することができ、測定誤差の推定をより的確に行うことができる。
【発明を実施するための最良の形態】
【００２４】
以下、本発明の実施の形態を説明する。
図１は、本発明を適用した測位装置１０の一例を示すブロック図であり、この測位装置１０は、地球の大気圏外を周回するＧＰＳ衛星及び準天頂衛星ＱＺＳＳから送信された電波を受信するＧＰＳ／ＱＺＳＳ受信機１と、ジャイロセンサを含んで構成され、測位装置１０本体のヨー方向の角速度を検出する方位センサとしての角速度センサ２及び加速度を検出する加速度センサ３と、前記ＧＰＳ／ＱＺＳＳ受信機１で受信した受信信号に基づいて電波航法により測位を行うと共にＧＰＳ衛星やＱＺＳＳ衛星とＧＰＳ／ＱＺＳＳ受信機１との相対関係を表す衛星までの擬似距離やドップラ周波数等の観測情報を算出し、また、角速度センサ２及び加速度センサ３の検出信号に基づいて自律航法による測位演算を行い、この測位情報を、前記電波航法による絶対測位情報及び観測情報に基づいて推定した自律航法による測位演算の測定誤差に応じて補正し、これを測位結果とする演算処理装置４と、を備え、演算処理装置４では算出した測位結果を、例えば表示部５に出力する。
【００２５】
この演算処理装置４では、前記ＧＰＳ／ＱＺＳＳ受信機１の測位状況に基づいて得た絶対測位情報の信頼度が高いかどうかを判定し、測位演算により得た絶対測位情報の信頼度が高いと判定されるときには、測位結果レベルのフィルタモードで前記自律航法による測位演算の測定誤差の推定を行う。具体的には、電波航法による絶対測位情報（絶対位置、絶対速度）に基づき、自律航法による各種センサの測定誤差や測位演算結果の誤差を推定する。
【００２６】
一方、測位状況から、電波航法により測位演算を行うことはできるがその信頼度は低いと予測されるとき、或いは電波航法により測位演算を行うことはできないと判定されるときには、観測情報レベルのフィルタモードで前記自律航法による測位演算の測定誤差の推定を行う。具体的には、ＧＰＳ／ＱＺＳＳ衛星からの電波の観測情報、つまり、擬似距離及びドップラ周波数に基づき、自律航法による各種センサの測定誤差や測位演算結果の誤差を推定する。
そして、このようにして推定した自律航法による各種センサの測定誤差や測位演算結果の誤差に基づき、各種センサの測定値の補正や測位演算結果の補正を行い、補正後の測位演算結果を、測位装置１０本体の測位結果とする。
【００２７】
図２は、前記演算処理装置４で実行される測位演算処理の処理手順の一例を示すフローチャートである。
演算処理装置４では、ＧＰＳ／ＱＺＳＳ受信機１で、ＧＰＳ衛星やＱＺＳＳ衛星からの
電波を受信し測位演算して、ステップＳ１からステップＳ２に移行し、測位状況を判定し、電波航法により絶対測位情報を得ることができるかどうか、また、絶対測位情報を得ることができる場合には、その絶対測位情報は、高信頼度であるか低信頼度であるかを判断する。
【００２８】
この判断は、例えば、ＧＰＳ／ＱＺＳＳ受信機１で電波を受信することができた捕捉衛星の数、衛星の配置による測位精度の低下率を表す位置精度低下率ＰＤＯＰ及び測位精度を表すポシションシグマ等に基づいて行う。
このようにして測位状況を判定したならばステップＳ３に移行し、測位状況を判定した結果、高信頼度の絶対測位情報を得ることができると判定された場合にはステップＳ４に移行し、測位結果レベルのフィルタモードでの測位演算を行い、電波航法による絶対測位情報（絶対位置、絶対速度）を用いて自律航法における測定誤差の推定を行い、これに基づき所定の補正を行って、測位装置１０本体の測位結果を得る。
【００２９】
一方、測位状況を判定した結果、絶対測位情報を得ることは可能であるが、その信頼度が低い場合、或いは、測位演算を行うことができない場合いはステップＳ５に移行し、捕捉衛星の中から高信頼度衛星を選択する。
具体的には、捕捉しているＧＰＳ衛星、ＱＺＳＳ衛星のそれぞれについて、受信信号のパワーと、その仰角とから判断する。例えば、衛星からの電波に含まれるＰＮコードについて自己が保持するＰＮコードとの相関値を取り、相関値のピーク値と、ノイズフロアの値との差が、予め設定したしきい値以上であり、且つ、衛星の仰角が予め設定したしきい値以上であるとき、この衛星を高信頼度衛星と判断する。
【００３０】
ここで、ＧＰＳ／ＱＺＳＳ受信機１では、準天頂衛星ＱＺＳＳからの電波も受信するようにしており、この準天頂衛星ＱＺＳＳは、常に何れか１つは日本の天頂付近に存在する衛星であり、仰角７０度以上の位置に存在し、ビルの谷間でも遮られることなくマルチパスの影響をうけにくい衛星である。したがって、少なくとも１つは高信頼度衛星が存在することになる。
【００３１】
次いで、ステップＳ６に移行し、高信頼度衛星として判定された捕捉衛星について、その電波の観測情報、つまり、擬似距離及びドップラ周波数に基づいて、観測情報レベルのフィルタモードでの測位演算を行い、観測情報を用いて自律航法における測定誤差の推定を行い、これに基づき所定の補正を行い、測位装置１０本体の測位結果を得る。なお、前記擬似距離は、測位装置１０での衛星電波の受信時刻と受信信号に含まれる衛星の送信時刻との差に光速を乗算することで算出することができる。また、ドップラ周波数は、搬送周波数ｆ〔Ｈｚ〕と、ドップラ効果による周波数偏移Δｆ（＝ｆ・ρ／ｃ）〔Ｈｚ〕とから、ｆ±Δｆで算出することができる。なお、ρは測位装置１０本体と衛星との相対速度、ｃは光速である。
【００３２】
そして、ステップＳ７に移行し、ステップＳ４で算出した測位結果レベルのフィルタモード又はステップＳ６で算出した擬似距離レベルのフィルタモードにより算出した測位結果を表示部５に出力する。そして、ステップＳ１に戻り、上述の処理を繰り返し行う。
【００３３】
図３は、測位結果レベルのフィルタモードでの測位演算の処理手順を示すブロック図である。
この測位結果レベルのフィルタモードでは、ＧＰＳ衛星又はＱＺＳＳ衛星から受信した受信信号に基づいて、測位演算部１１において、公知の電波航法による手順で測位演算を行う。つまり、この測位結果レベルのフィルタモードでは、高信頼度の測位演算が可能と判断されていることから、捕捉衛星のうち、高信頼度の測位演算が可能との判断結果を得た、少なくとも４個以上のＧＰＳ衛星又はＱＺＳＳ衛星からの受信信号を用いて公知の手
順で測位装置１０本体の絶対位置及び絶対速度を算出する。
【００３４】
一方、自律センサナビゲーション１２では、角速度センサ２及び加速度センサ３で検出した角速度及び加速度と、後述のカルマンフィルタ１３で推定された測定誤差とに基づいて公知の自律航法による測位演算を行い、測位装置１０本体の位置、速度及び方位を推測する。そして、測位演算部１１で算出した電波航法による絶対測位情報（絶対位置、絶対速度）と、自律センサナビゲーション１２で算出した自律航法による測位演算結果とに基づいて、カルマンフィルタ１３により、自律航法による測定誤差を推測する。そして、カルマンフィルタ１３により推定した測定誤差と、今回の測位演算結果とを補正部１４で加算し、これを測位装置１０本体の今回の測位結果とする。
【００３５】
ここで、カルマンフィルタについて簡単に説明する。
カルマンフィルタは、観測信号と統計的な先験情報とを用いて、信号の生成過程のモデル化の際に導入された導入変数の最適な推定値（最小分散推定値）を逐次求めるアルゴリズムとして導かれており、予測モードと推定モードとの二段階で処理が繰り返される。
ここで、状態ベクトルをｘとすると、状態空間モデルは、次式（１）及び（２）で表すことができる。なお、変数の添字ｋは、時刻を表している。
【００３６】
ｘk+1＝φkｘk＋ｕk（状態方程式） ……（１）
【００３７】
ｚk＝Ｈkｘk＋εk（観測方程式） ……（２）
【００３８】
なお、式中のｘkは状態ベクトル、φkは状態遷移行列、ｕkはプロセスノイズとしての
ガウス白色誤差（平均ベクトル０、共分散行列Ｑkをもつ正規分布Ｎ（０，Ｑk）で表すことができる）、ｚkは観測値、Ｈkは観測値ｚkと状態ベクトルｘkとを関連づけるための観測行列、εkは観測値の誤差としてのガウス白色誤差（平均ベクトル０，共分散行列Ｒkをもつ正規分布Ｎ（０，Ｒk）で表すことができる）である。
【００３９】
予測モードでは、（３）式に示す状態ベクトルの予測と、（４）式に示す推定誤差共分散の予測とを行い、（５）式にしたがってカルマンゲインＫkを算出する。
また、推定モードでは、（６）式に示す状態ベクトルの推定と、（７）式に示す推定誤差共分散の推定とを行う。
【００４０】
ｘ-k＝φkｘ+k-1 ……（３）
【００４１】
Ｐ-k＝φkＰ+k-1φkT＋Ｑk-1 ……（４）
【００４２】
Ｋk ＝Ｐ-kＨkT［ＨkＰ-kＨkT＋Ｒk］-1 ……（５）
【００４３】
ｘ+k＝ｘ-k＋Ｋk［ｚk−Ｈkｘ-k］ ……（６）
【００４４】
Ｐ+k＝［Ｉ−ＫkＨk］Ｐ-k ……（７）
【００４５】
なお、式中の添字Ｔは転置行列を表す。また、ｘ-kは状態ベクトルｘの予測値、ｘ+k、ｘ+k-1は状態ベクトルの推定値、Ｐ-kは推定誤差共分散の予測値、Ｐ+k、Ｐ+k-1は推定誤差共分散の推定値を表す。また、φkは状態遷移行列、Ｑk-1は前記プロセスノイズとしてのガウス白色誤差ｕの共分散行列、Ｈkは前記観測行列、Ｒkは前記観測値の誤差としてのガウス白色誤差εkの共分散行列である。
【００４６】
そして、上記（１）、（２）式の状態空間モデル及び観測値ｚkを用い、前記（３）〜
（７）式を繰り返し行うことにより、観測値ｚkに応じてカルマンフィルタゲインＫkが収束し、より的確な状態ベクトルｘ+kの推測を行うものである。
次に、図１に示すように、ＧＰＳ／ＱＺＳＳ受信機１、角速度センサ２及び加速度センサ３を備えた測位装置１０において、カルマンフィルタ１３で処理を行う場合について説明する。なお、ここでは、説明を簡単にするために、二次元ナビゲーションを行う場合について説明する。
【００４７】
測位装置１０における、状態ベクトルＸを、次式（８）で表す。
Ｘ＝［δＰE δＰN δＶE δＶN δθg Ｂg Ｂａx Ｂａy］T ……（８）
【００４８】
なお、（８）式中の、δＰEは東方向位置誤差、δＰNは北方向位置誤差、δＶEは東方
向速度誤差、δＶNは北方向速度誤差、δθgはヨー方向角度誤差、Ｂgは角速度センサの
バイアス、ＢａxはＸ軸方向の加速度センサのバイアス、ＢａyはＹ軸方向の加速度センサのバイアスである。
【００４９】
前記角速度センサのバイアス及び加速度バイアスは確定的な変化はなく、前回の誤差にノイズが負荷されたものして、次式（９）〜（１１）により表すことができる。
【００５０】
Ｂg（ｋ＋１）＝Ｂg（ｋ）＋ｗg ……（９）
【００５１】
Ｂａx（ｋ＋１）＝Ｂａx（ｋ）＋ｗax ……（１０）
【００５２】
Ｂａy（ｋ＋１）＝Ｂａy（ｋ）＋ｗay ……（１１）
【００５３】
また、ヨー方向角度誤差δθgは、次式（１２）に示すように、前回の角速度センサの
バイアスＢgに前回からの経過時間すなわちサンプリング間隔Ｔを乗算することにより算
出される方位誤差とノイズとを、前回の誤差に加算して算出される。
【００５４】
δθg（ｋ＋１）＝δθg（ｋ）＋Ｔ・Ｂg（ｋ）＋ｗθg ……（１２）
【００５５】
ここで、状態方程式を、次式（１３）とする。
Ｘk+1＝φＸk＋ｗ ……（１３）
【００５６】
なお、式中のφは、状態ベクトルの時間的な変化を与える状態遷移行列、ｗはシステムノイズであって次式（１４）及び（１５）で表す。なお、式中のＴはサンプリング間隔、ａxはＸ軸方向の加速度、ａyはＹ軸方向の加速度、θはヨー方向角度である。
【００５７】
【数１】
【００５８】
次に、観測方程式を、次式（１６）とする。
Ｙ＝Ｈ・Ｘ＋ｒ ……（１６）
【００５９】
なお、Ｙは観測値であって次式（１７）で表し、Ｈは観測値Ｙと状態ベクトルＸとを関連付けるための観測行列、ｒは観測ノイズである。
【００６０】
【数２】
【００６１】
（１７）式中のＰE,INS、ＰN,INS、ＶE,INS、ＶN,INSは、自律航法による測位演算により得た、東方向位置、北方向位置、東方向速度、北方向速度であり、ＰE,GPS、ＰN,GPS、ＶE,GPS、ＶN,GPSは、電波航法による測位演算により得た、東方向位置、北方向位置、東方向速度、北方向速度である。
そして、まず、測位装置１０の初期測位位置（ｘ，ｙ，ｚ）を設定し、さらに、状態ベクトル推定値Ｘ+k-1及び誤差共分散行列Ｐの推定値Ｐ+k-1として、所定の初期値を設定する。この初期値は、例えば、最小自乗法により測位演算を行うことにより、現在の測位装置１０の位置と状態ベクトル推定値Ｘ+k-1とを求めることができる。また、誤差共分散行列Ｐの推定値Ｐ+k-1としては予め設定した所定の値を設定する。
【００６２】
なお、ここでは、測位装置１０の初期値及び状態ベクトルの初期値として、最小自乗法による演算結果を用いた場合について説明したがこれに限るものではなく、他の手法により初期値を設定してもよい。
そして、前記測位演算部１１で電波航法により測位演算を行い、東方向位置ＰE,GPS、
北方向位置ＰN,GPS、東方向速度ＶE,GPS、北方向速度ＶN,GPSを獲得する。同様に、自律
センサナビゲーション１２において、自律航法により、東方向位置ＰE,INS、北方向位置
ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを獲得する。
【００６３】
そして、これら電波航法及び自律航法による測位演算結果及び、前記（１６）及び（１７）式から、観測方程式を得る。
そして、前記（３）及び（４）式から、状態ベクトルＸ-k及び推定誤差行列分散Ｐ-kの予測を行い、（５）式からカルマンフィルタゲインＫkを算出する。
次いで、（５）式から得たカルマンフィルタゲインＫkと前記（６）及び（７）式から
、状態ベクトルＸ+k及び推定誤差行列分散Ｐ+kの推定を行う。
【００６４】
以上の手順を、繰り返し行い、前記（８）式で表す状態ベクトルＸの各成分、つまり、自律航法において測位演算を行う際の、測位演算結果の誤差や、角速度センサ２や加速度センサ３による測定誤差を推測する。
そして、この推定した測定誤差を用いて補正部１４において、自律航法により演算した東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ１３により得た測定誤差の推定値を自律センサナビゲーション１２にフィードバックする。
【００６５】
そして、前記自律センサナビゲーション１２では、測定誤差の推定値をもとに今回の測位結果、つまり、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向
速度ＶN,INSを補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差
δθg、角速度センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方
向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処
理を行う際に、これら測定誤差の推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、測定誤差の推定値により補正された前回の測位結果とをもとに、今回の測位演算を行う。
【００６６】
次に、図４は、観測情報レベルのフィルタモードでの処理手順を示すブロック図である。
この観測情報レベルでのフィルタモードでは、測位演算部２１において、捕捉しているＧＰＳ衛星又はＱＺＳＳ衛星について、ＰＮコードの相関値や仰角に基づいて高信頼度衛星として選択された衛星についてその衛星までの擬似距離及びドップラ周波数を、観測擬似距離及び観測ドップラ周波数として算出する。
【００６７】
つまり、この観測情報レベルのフィルタモードでは、４個以上のＧＰＳ衛星又はＱＺＳＳ衛星からの電波を受信しておらず測位演算を行うことができないか、又は、電波航法により高信頼度の測位結果を得ることができない状況であるから、高信頼度衛星、つまり、マルチパス等の影響を受けていない信頼度の高い受信信号に基づく擬似距離及びドップラ周波数を用いて、自律航法による測定誤差の推定を行う。
【００６８】
一方、自律ナビゲーション２２では、角速度センサ２及び加速度センサ３で検出した角速度及び加速度、及び後述のカルマンフィルタ１３で推定された測定誤差の推定値に基づいて、公知の自律航法による測位演算を行い、測位装置１０本体の位置、速度及び方位を
算出する。さらに、測位演算の結果得た測位装置１０本体の位置と、受信信号に含まれる高信頼度衛星の今回の位置とに基づいて高信頼度衛星までの擬似距離を予測する。また、測位装置１０本体の速度と、高信頼度衛星の速度とに基づいて測位装置１０及び高信頼度衛星との間の相対速度を算出し、これに基づいてドップラ周波数を予測する。そして、これらを予測擬似距離及び予測ドップラ周波数とする。
【００６９】
そして、測位演算部２１で選択した高信頼度衛星の観測擬似距離及び観測ドップラ周波数と、自律センサナビゲーション２２で予測した予測擬似距離及び予測ドップラ周波数とに基づいて、カルマンフィルタ２３により、自律航法による測定推定結果の推定誤差を推測する。そして、この推定した測定誤差を用いて補正部２４において、自律航法により演算した測位結果、すなわち、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ２３により得た測定誤差の推定値を自律センサナビゲーション２２にフィードバックする。
【００７０】
そして、前記自律センサナビゲーション２２では、測定誤差の推定値をもとに今回の測位結果、つまり、東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向
速度ＶN,INSを補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差
δθg、角速度センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方
向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処
理を行う際に、これら測定誤差の推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、測定誤差の推定値により補正された前回の測位結果とをもとに、今回の測位演算を行う。
【００７１】
この観測情報レベルのフィルタモードにおけるカルマンフィルタ２３では、拡張カルマンフィルタを適用して線形化し、観測方程式を、次式（１８）及び（１９）とする。
【００７２】
Ｙ１＝Ｈ・Ｘ＋ｒ１ ……（１８）
Ｙ２＝Ｈ・Ｘ＋ｒ２ ……（１９）
【００７３】
なお、Ｙ１、Ｙ２は観測値であって次式（２０）、（２１）で表す。
【００７４】
【数３】
【００７５】
そして、上述の測位結果レベルのフィルタモードと同様に、まず、測位装置１０本体の初期測位位置（ｘ，ｙ，ｚ）を設定し、さらに、状態ベクトル推定値Ｘ+k-1及び誤差共分散行列Ｐの推定値Ｐ+k-1として、所定の初期値を設定する。
【００７６】
そして、前記測位演算部２１及び自律センサナビゲーション２２からの高信頼度衛星の
観測擬似距離及び観測ドップラ周波数と、予測擬似距離及び予測ドップラ周波数と、前記（１８）〜（２１）から観測方程式を得る。
そして、前記（１）及び（２）式から、状態ベクトルＸ-k及び推定誤差行列分散Ｐ-kの予測を行い、（３）式からカルマンフィルタゲインＫkを算出する。
【００７７】
次いで、（３）式から得たカルマンフィルタゲインＫkと前記（４）及び（５）式から
、状態ベクトルＸ+k及び推定誤差行列分散Ｐ+kの推定を行う。
以上の手順を繰り返し行い、前記（８）式で表す状態ベクトルＸの各成分、つまり、自律航法において測位演算を行う際の、測位演算結果の誤差や、角速度センサ２や加速度センサ３による測定誤差を推測する。
【００７８】
そして、この推測した測定誤差を用いて、補正部２４において、自律航法により演算した東方向位置ＰE,INS、北方向位置ＰN,INS、東方向速度ＶE,INS、北方向速度ＶN,INSを補正し、これを測位結果として出力する。
また、このカルマンフィルタ２３により得た測定誤差を自律センサナビゲーション２２にフィードバックする。
【００７９】
そして、前記自律センサナビゲーション２２では、測定誤差をもとに今回の測位結果を補正し、これを今回の測位結果として記憶する。また、ヨー方向角度誤差δθg、角速度
センサのバイアスＢg、Ｘ軸方向の加速度センサのバイアスＢａx、Ｙ軸方向の加速度センサのバイアスＢａyを、所定の記憶領域に記憶しておき、次に測位演算処理を行う際に、
これら測定誤差推定値を用いて、角速度センサ２及び加速度センサ３の検出信号を補正し、補正後の角速度センサ２及び加速度センサ３の検出信号と、推測した測定誤差により補正した前回の測位結果とをもとに、今回の測位演算を行う。
【００８０】
次に、本発明の動作を説明する。
ＧＰＳ／ＱＺＳＳ受信機１で、ＧＰＳ衛星又はＱＺＳＳ衛星から電波を受信し、測位演算して、捕捉衛星の数、捕捉衛星の配置に応じた位置精度低下率ＰＤＯＰ、ポシションシグマ等に基づいて、測位状況を判断し、電波航法により高信頼度の絶対測位情報を獲得することができるかどうかを判定する（ステップＳ１、Ｓ２）。
【００８１】
そして、高信頼度の絶対測位情報を得ることができると判定されたときには、ステップＳ３からステップＳ４に移行し、測位結果レベルのフィルタモードで測定誤差の推定を行う。つまり、電波航法により算出した測位装置１０本体の絶対位置及び絶対速度と、角速度センサ２及び加速度センサ３の検出信号と、前回のサンプリング周期で算出されたヨー角方向角度誤差δθg、ジャイロバイアスＢg、加速度センサのバイアスＢａx、Ｂａy及び、東方向及び北方向の位置誤差δＰE、δＰN、東方向及び北方向の速度誤差δＶE、δＶNにより補正した前回の測位装置１０本体の測位結果とに基づいて、今回の位置、速度及び方位を算出する。
【００８２】
そして、これらに基づき前記（１７）式にしたがって測定誤差の観測値を設定し、カルマンフィルタ処理を行って、測定誤差を推定する。そして、この測定誤差の推定値により、自律航法により得た測位結果を補正し、補正後の値を測位装置１０本体の測位結果として表示部５に出力する。また、測定誤差の推定値に基づいて、自律航法による測位結果を補正しこれを今回の測位結果として記憶すると共に、角速度センサ２や加速度センサ３の測定誤差の推定値を所定の記憶領域に記憶する。
以上の処理を、高信頼度の測位演算を行うことが可能な間、繰り返し行う。
【００８３】
ここで、前記カルマンフィルタ処理では、前記（１７）式に示すように、測定値として、電波航法により得た東方向及び北方向の位置及び東方向及び北方向の速度を用いて処理
を行っており、４×１の行列で構成される。
カルマンフィルタ処理では、（５）式に示すように、観測方程式の観測行列の逆行列を用いて演算を行っているため、この観測行列のサイズが大きい場合には演算処理装置４の演算処理負荷が増加することになる。しかしながら、（１７）式に示すように、観測値Ｙとして、４×１の行列を用い、比較的サイズの小さい行列を用いているから、処理負荷の増加を低減することができる。
【００８４】
また、このとき、電波航法により測位演算された高信頼度の測位結果と、自律航法による測位結果とに基づいて、測定誤差の観測値Ｙを設定しているから、マルチパス等の影響を受けていないより誤差の少ない観測値Ｙを用いて測定誤差を推定することができ、より的確に測定誤差の推定を行うことができる。
したがって、このようにして推定した測定誤差を用いて、自律航法による測位結果を補正しこれを用いて測位装置１０本体の位置を得ることで、より的確な測位結果を得ることができる。
【００８５】
一方、電波航法による絶対測位情報を得ることはできるがその信頼度が比較的低いと判定される場合、或いは、４個以上の衛星を捕捉することができず測位演算を行うことができない場合には、ステップＳ３からステップＳ５に移行し、ＰＮコードの相関値や衛星の仰角に基づき、捕捉衛星の中から高信頼度衛星を選択し、この高信頼度衛星の観測擬似距離及び観測ドップラ周波数を獲得する。
また、高信頼度衛星の位置、速度及び受信機の位置、速度に基づいて、今回の擬似距離及びドップラ周波数を予測し、これらに基づき前記（２０）、（２１）式から測定誤差の観測値を設定し、カルマンフィルタ処理を行って、測定誤差の推定値を算出する。
【００８６】
そして、この測定誤差の推定値により、自律航法により得た測位結果を補正し、補正後の測位結果を、測位装置１０本体の測位結果として出力する。また、測定誤差の推定値に基づいて、自律航法による測位結果を補正しこれを今回の測位結果として記憶すると共に、角速度センサ２や加速度センサ３の測定誤差の推定値を所定の記憶領域に記憶する。
この処理を、電波航法による測位結果が比較的低信頼度である間、或いは、４個以上の衛星を捕捉することができず自律航法による測位結果を得ることができない間、繰り返し行う。
【００８７】
ここで、前記カルマンフィルタ処理では、前記（２０）及び（２１）式に示すように、測定値として、高信頼度衛星からの観測擬似距離及び観測ドップラ周波数と、自律航法による測位結果に基づき算出した予測擬似距離及び予測ドップラ周波数との差を、観測値として用いており、高信頼度衛星の数をｎとすると、２ｎ×１の行列で構成される。前述のように、電波航法により高信頼度な測位結果を得ることができる場合には、測位結果レベルのフィルタモードでの測位演算を行うから、高信頼度衛星であると判定される衛星の数は、ある程度制限される。したがって、前記観測値Ｙは、比較的サイズの小さな行列で表されることになる。
【００８８】
このため、この観測情報レベルのフィルタモードの場合も、観測値Ｙとして、比較的サイズの小さい行列を用いているから、カルマンフィルタ処理を実行する際の演算処理装置４での処理負荷を低減することができる。
また、このとき、捕捉衛星のうち、高信頼度衛星と判断した衛星からの精度の高い観測擬似距離及び観測ドップラ周波数を利用して測定誤差の観測値Ｙを設定しているから、より誤差の少ない測定誤差の観測値を用いて測定誤差を推定することができる。したがって、このようにして推定した誤差のより少ない測定誤差を用いて、自律航法による測位結果を補正することで、より的確な、測位装置１０本体の測位結果を得ることができる。特に、マルチパスの影響を受けた場合には精度が大幅に低下するが、上述のように高信頼度衛
星と判断した衛星の観測擬似距離及び観測ドップラ周波数を利用することによって、より的確な測位結果を得ることができる。
【００８９】
また、高信頼度衛星がない場合には、観測値Ｙを設定することができないため、カルマンフィルタ処理では予測モードだけで誤差推定を行わざるを得ないが、上述のように、ＧＰＳ／ＱＺＳＳ受信機１では、ＧＰＳ衛星だけでなく、ＱＺＳＳ衛星からの電波も受信する。前記ＱＺＳＳ衛星は、常に日本の天頂付近に存在する仰角７０度以上の位置に存在する衛星であって、マルチパスの影響の少ない高信頼度衛星であるから、少なくとも高信頼度衛星が１つは存在することになる。
【００９０】
したがって、仮に、高信頼度衛星であるＧＰＳ衛星が存在しない場合であっても、必ず１つは高信頼度衛星（ＱＺＳＳ衛星）が存在し、その電波を受信することができるから、高信頼度衛星からのより適切な観測擬似距離及び観測ドップラ周波数を獲得することができる。
したがって、これらの、より適切な観測擬似距離及び観測ドップラ周波数を用いることによって、より適切に測定誤差の推定を行うことができる。よって、この測定誤差を用いて、自律航法による測位結果を補正することで、より誤差の少ない測位結果を得ることができる。
【００９１】
なお、上記実施の形態においては、ＧＰＳ衛星及びＱＺＳＳ衛星からの電波を受信する場合について説明したが、例えば、ＧＰＳ衛星からの電波のみを受信する場合であっても適用することができ、この場合も上記と同等の作用効果を得ることができる。また、ＱＺＳＳ衛星を利用しない場合には、ＧＰＳ衛星からの電波の受信状況によっては高信頼度衛星が一つも存在しない場合がある。高信頼度衛星ではない衛星からの受信信号に基づく測位演算を行った場合、その測位精度は低下する。このため、このように高信頼度衛星を確保することができない場合には観測値は用いずに、カルマンフィルタ処理において予測モードのみにより測定誤差の推定を行うようにしてもよく、或いは、多少測位精度が低下する可能性があるが、捕捉衛星のうち、信頼度が比較的高い衛星からの受信信号を用いて観測値を設定するようにしてもよく、運用状況に応じて選択すればよい。
【００９２】
ここで、上記実施の形態において、図３の測位演算部１１が電波航法測位手段に対応し、図２のステップＳ５の処理が高信頼度衛星選択手段に対応し、図４の測位演算部２１が相対関係観測手段に対応し、図３及び図４の自律センサナビゲーション１２及び２２が自律航法測位手段に対応し、図３及び図４のカルマンフィルタ１３及び２３が測定誤差推定手段に対応し、図３及び図４の補正部１４及び２４が補正手段に対応している。また、図２のステップＳ２及びステップＳ３の処理が高信頼度判定手段に対応し、ステップＳ４の処理が第１の測定誤差推定手段に対応し、ステップＳ６の処理が第２の測定誤差推定手段に対応している。
【００９３】
また、図４の自律センサナビゲーション２２で、予測擬似距離及び予測ドップラ周波数を算出する処理が相対関係予測手段に対応している。
【図面の簡単な説明】
【００９４】
【図１】本発明の測位装置の一例を示すブロック図である。
【図２】演算処理装置で実行される測位演算処理の処理手順の一例を示すフローチャートである。
【図３】測位結果レベルのフィルタモードでの測位演算の手順を示すブロック図である。
【図４】観測情報レベルのフィルタモードでの測位演算の手順を示すブロック図である。
【符号の説明】
【００９５】
１ ＧＰＳ／ＱＺＳＳ受信機、２ 角速度センサ、３ 加速度センサ、４ 演算処理装置、１１、２１ 測位演算部、 １２、２２ 自律センサナビゲーション、１３、２３ カルマンフィルタ、１４、２４ 補正部
【特許請求の範囲】
【請求項１】
複数の衛星からの衛星電波を用いて電波航法により測位を行い、受信機の絶対位置及び絶対速度を含む絶対測位情報を得る電波航法測位手段と、
衛星電波の受信パワー及びその衛星の仰角に基づいて高信頼度衛星を選択する高信頼度衛星選択手段と、
当該高信頼度衛星選択手段で選択された高信頼度衛星からの衛星電波を用いて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を測定する相対関係観測手段と、
自律航法により前記受信機の測位を行う自律航法測位手段と、
前記電波航法測位手段で得た前記絶対測位情報及び前記相対関係観測手段で測定した相対関係観測値を用いて前記自律航法測位手段の測位情報の測定誤差を推定する測定誤差推定手段と、
当該測定誤差推定手段で推定した測定誤差を用いて前記自律航法測位手段の測位情報を補正して前記受信機の測定位置を得る補正手段と、を備えた測位装置であって、
前記測定誤差推定手段は、前記受信機の測位状況に基づいて前記絶対測位情報が高信頼度であるかどうかを判定する高信頼度判定手段と、
当該高信頼度判定手段で前記絶対測位情報が高信頼度であると判定されるとき、前記絶対測位情報を用いて前記測定誤差を推定する第１の測定誤差推定手段と、
前記高信頼度判定手段で前記絶対測位情報が高信頼度ではないと判定されるとき、前記相対関係観測値を用いて前記測定誤差を推定する第２の測定誤差推定手段と、を備えることを特徴とする測位装置。
【請求項２】
前記高信頼度判定手段は、前記受信機で前記衛星電波を受信している衛星の数及びその配置と、ポジションシグマとのうちの少なくとも何れか１つに基づいて、前記絶対測位情報が高信頼度であるかどうかを判定することを特徴とする請求項１記載の測位装置。
【請求項３】
前記測定誤差推定手段は、カルマンフィルタを用いて前記測定誤差を推定し、
前記絶対測位情報及び前記相対関係観測値を、前記カルマンフィルタにおける観測方程式の観測値として用いることを特徴とする請求項１又は請求項２記載の測位装置。
【請求項４】
前記自律航法測位手段は、前記受信機の位置及び速度を測定し、
前記第１の測定誤差推定手段は、前記電波航法測位手段で測定した絶対位置及び絶対速度と、前記自律航法測位手段で測定した測定位置及び測定速度との差を前記観測値とすることを特徴とする請求項３記載の測位装置。
【請求項５】
前記第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、
前記自律航法測位手段の測位情報と前記高信頼度衛星からの衛星電波に含まれる高信頼度衛星の位置情報とに基づいて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を予測する相対関係予測手段を備え、
前記高信頼度衛星の、前記相対関係観測手段で測定した擬似距離と前記相対関係予測手段で予測した擬似距離との差を前記擬似距離の観測方程式の観測値とし、且つ前記相対関係観測手段で測定したドップラ周波数と前記相対関係予測手段で予測したドップラ周波数との差を前記ドップラ周波数の観測方程式の観測値とすることを特徴とする請求項３又は請求項４記載の測位装置。
【請求項６】
前記受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信可能に形成されていることを特徴とする請求項１から請求項５の何れか１項に記載の測位装置。
【請求項７】
複数の衛星からの衛星電波を用いて測位した受信機の絶対位置及び絶対速度を含む絶対測位情報と、高信頼度衛星からの衛星電波を用いて算出した前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数とを用いて、自律航法による測位情報の測定誤差を推定し、推定した測定誤差を用いて前記自律航法による測位情報を補正して前記受信機の測定位置を得る測位方法であって、
前記受信機の測位状況に基づき、前記絶対測位情報が高信頼度であるかどうかを判定し、
前記絶対測位情報が高信頼度であると判定されるときには前記絶対測位情報を用いて前記測定誤差を推定し、
前記絶対測位情報が高信頼度ではないと判定されるときには前記擬似距離及び前記ドップラ周波数を用いて前記測定誤差を推定することを特徴とする測位方法。
【請求項１】
複数の衛星からの衛星電波を用いて電波航法により測位を行い、受信機の絶対位置及び絶対速度を含む絶対測位情報を得る電波航法測位手段と、
衛星電波の受信パワー及びその衛星の仰角に基づいて高信頼度衛星を選択する高信頼度衛星選択手段と、
当該高信頼度衛星選択手段で選択された高信頼度衛星からの衛星電波を用いて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を測定する相対関係観測手段と、
自律航法により前記受信機の測位を行う自律航法測位手段と、
前記電波航法測位手段で得た前記絶対測位情報及び前記相対関係観測手段で測定した相対関係観測値を用いて前記自律航法測位手段の測位情報の測定誤差を推定する測定誤差推定手段と、
当該測定誤差推定手段で推定した測定誤差を用いて前記自律航法測位手段の測位情報を補正して前記受信機の測定位置を得る補正手段と、を備えた測位装置であって、
前記測定誤差推定手段は、前記受信機の測位状況に基づいて前記絶対測位情報が高信頼度であるかどうかを判定する高信頼度判定手段と、
当該高信頼度判定手段で前記絶対測位情報が高信頼度であると判定されるとき、前記絶対測位情報を用いて前記測定誤差を推定する第１の測定誤差推定手段と、
前記高信頼度判定手段で前記絶対測位情報が高信頼度ではないと判定されるとき、前記相対関係観測値を用いて前記測定誤差を推定する第２の測定誤差推定手段と、を備えることを特徴とする測位装置。
【請求項２】
前記高信頼度判定手段は、前記受信機で前記衛星電波を受信している衛星の数及びその配置と、ポジションシグマとのうちの少なくとも何れか１つに基づいて、前記絶対測位情報が高信頼度であるかどうかを判定することを特徴とする請求項１記載の測位装置。
【請求項３】
前記測定誤差推定手段は、カルマンフィルタを用いて前記測定誤差を推定し、
前記絶対測位情報及び前記相対関係観測値を、前記カルマンフィルタにおける観測方程式の観測値として用いることを特徴とする請求項１又は請求項２記載の測位装置。
【請求項４】
前記自律航法測位手段は、前記受信機の位置及び速度を測定し、
前記第１の測定誤差推定手段は、前記電波航法測位手段で測定した絶対位置及び絶対速度と、前記自律航法測位手段で測定した測定位置及び測定速度との差を前記観測値とすることを特徴とする請求項３記載の測位装置。
【請求項５】
前記第２の位置誤差推定手段は、前記測定誤差を、前記擬似距離の観測方程式と、前記ドップラ周波数の観測方程式とを用いて個別に推定し、
前記自律航法測位手段の測位情報と前記高信頼度衛星からの衛星電波に含まれる高信頼度衛星の位置情報とに基づいて前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数を予測する相対関係予測手段を備え、
前記高信頼度衛星の、前記相対関係観測手段で測定した擬似距離と前記相対関係予測手段で予測した擬似距離との差を前記擬似距離の観測方程式の観測値とし、且つ前記相対関係観測手段で測定したドップラ周波数と前記相対関係予測手段で予測したドップラ周波数との差を前記ドップラ周波数の観測方程式の観測値とすることを特徴とする請求項３又は請求項４記載の測位装置。
【請求項６】
前記受信機は、ＧＰＳ衛星及び準天頂衛星からの衛星電波を受信可能に形成されていることを特徴とする請求項１から請求項５の何れか１項に記載の測位装置。
【請求項７】
複数の衛星からの衛星電波を用いて測位した受信機の絶対位置及び絶対速度を含む絶対測位情報と、高信頼度衛星からの衛星電波を用いて算出した前記受信機と前記高信頼度衛星との間の擬似距離及び前記高信頼度衛星のドップラ周波数とを用いて、自律航法による測位情報の測定誤差を推定し、推定した測定誤差を用いて前記自律航法による測位情報を補正して前記受信機の測定位置を得る測位方法であって、
前記受信機の測位状況に基づき、前記絶対測位情報が高信頼度であるかどうかを判定し、
前記絶対測位情報が高信頼度であると判定されるときには前記絶対測位情報を用いて前記測定誤差を推定し、
前記絶対測位情報が高信頼度ではないと判定されるときには前記擬似距離及び前記ドップラ周波数を用いて前記測定誤差を推定することを特徴とする測位方法。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００７−２４８２７１（Ｐ２００７−２４８２７１Ａ）
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願番号】特願２００６−７２２９９（Ｐ２００６−７２２９９）
【出願日】平成１８年３月１６日（２００６．３．１６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願日】平成１８年３月１６日（２００６．３．１６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]