JP2004012286A - Position and attitude estimation system of moving object by gps - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a position and attitude estimation system of a moving object by GPS wherein attitude and position can be estimated simultaneously under a condition that the number of necessary satellites is smaller than the normal number of satellites for GPS (three satellites in a case of two antennas). <P>SOLUTION: In order to receive radio wave from GPS satellites, at least one pair of antennas 1, 2 fixed on the moving object are fixed. By using a calculating means of position and azimuth of the moving object, positions of the respective antennas and inclination of segment between the antennas are calculated according to a navigation message, psudorange or psudorange and transfer phase, and geometrical condition of the antennas. It is also possible that inclination of segment between the receiving antennas can be calculated by a method wherein correction information from a base station is received by using a correction information receiving means, and phase difference between the receiving antennas and phase difference between the respective receiving antennas and an antenna of the base station are obtained by using the calculating means of position and azimuth of the moving object. <P>COPYRIGHT: (C)2004,JPO

Description

【００１１】
ここで
ｌはＡアンテナ　とＢアンテナ　間の既知距離
ｘ　_ｉ　ｙ　_ｉ　ｚ　_ｉはｉ番目の衛星の座標（衛星軌道情報により既知）
ｘ　^Ａ　，ｙ　^Ａ　，ｚ　^ＡはＡアンテナの座標
ｘ　^Ｂ　，ｙ　^Ｂ　，ｚ　^ＢはＢアンテナの座標
Ｐ　^Ａ　_ｉ　，　Ｐ　^Ｂ　_ｉはｉ番目の衛星のＡアンテナとＢアンテナでの観測疑似距離（受信電波計測により既知）
ｈ　^Ａ　_ｘｉ，ｈ　^Ａ　_ｙｉ　，ｈ　^Ａ　_ｚｉはｉ番目の衛星とＡアンテナ間の方向余弦（衛星軌道情報により既知）
ｈ　^Ｂ　_ｘｉ，ｈ　^Ｂ　_ｙｉ　，ｈ　^Ｂ　_ｚｉはｉ番目の衛星とＢアンテナ間の方向余弦（衛星軌道情報により既知）
△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_ＡはＡアンテナの位置座標補正値
△ｘ　_Ｂ　，△ｙ　_Ｂ　，△ｚ　_ＢはＢアンテナの位置座標補正値
δｔは受信時刻誤差
【００１２】
以上の式１の一般解は、最小２乗法によってえられる。通常（幾何学条件を考慮しない）の手段で導かれる方程式は衛星数が４個以下の場合では解が存在しない。以上の方式では３衛星（６個の測定値・ｎ　＝３）の場合未知変数７つ（△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_Ａ　，△ｘ　_Ｂ　，△ｙ　_Ｂ　，△ｚ　_Ｂ　，　ｔ　）に対して７つの独立方程式が得られる。以上の方式を用いることで、受信している衛星が３衛星の場合でもＧＰＳのみを使用して３次元絶対位置算出ができるようになる。
【００１３】
Ａアンテナ１とＢアンテナ２の座標の値からアンテナ間線分のピッチとヨーの演算を行う。ピッチとヨーは原点が現在位置・ｘ軸が東方向向き・ｙ軸が北方向向きの座標に対する車両座標系の回転角度として演算可能である。
以上のシステムはＧＰＳ　適用範囲を大きく広げることができ、同時にピッチ、ヨーの推定、精度の向上もできる。
【００１４】
図５は、図１に示した第１の装置を用いて実現したＧＰＳによる移動体の位置・姿勢推定システムの第１の実施例を説明するための流れ図である。
ステップ１００〜ステップ１１０で構成されている。
この実施例は、移動体の位置方位演算アルゴリズムで疑似距離とアンテナの幾何学的条件を使用する。アンテナの幾何学的条件を示す方程式はステップ１０５に示すように最小２乗法を用いて解く。
【００１５】
（移動体の位置と方位の推定システムの第２の実施例）
移動体の位置と方位を推定するシステムの第２の実施例は、前述の実施例同様に図１に示す装置を用いて実施される。
【００１６】
幾何学条件を考慮した両アンテナからｎ個衛星の測定による出力方程式は以下のようになる。
式７

【００１７】
ここで、
ｌはＡアンテナ　とＢアンテナ　間の既知距離
ｘ　_ｉ　（ｔ），　ｙ　_ｉ　（ｔ），　ｚ　_ｉ　（ｔ）はｔ時刻でｉ番目の衛星の座標（衛星軌道情報により既知）
ｘ　（ｔ），ｙ　（ｔ），ｚ　（ｔ）はＡアンテナ（またはＢ）の座標
ρ_ｉ　^Ａ　（ｔ）　，ρ_ｉ　^Ｂ　（ｔ）はｔ時刻でｉ番目の衛星のＡアンテナとＢアンテナでの観測疑似距離（受信電波計測により既知）
α　（ｔ），β　（ｔ）はｔ時刻でＡＢアンテナ間線分の方向オイラー角度（未知変数）δｔ　（ｔ）は受信時刻誤差
【００１８】
以上の方程式とｘ　（ｔ），ｙ　（ｔ），ｚ　（ｔ），α　（ｔ），β　（ｔ），　Ａｔ　（ｔ）を状態変数として含める状態方程式ｆ　（ｘ　（ｔ），ｙ　（ｔ），ｚ　（ｔ），α　（ｔ），β　（ｔ），　Ａｔ　（ｔ），．．．）によってシステム方程式が記述される。このシステム方程式をカルマンフィルタ手法に用いるとｘ　（ｔ），ｙ　（ｔ），ｚ　（ｔ），α　（ｔ），β　（ｔ），δｔ（ｔ）が推定可能になる。
以上の方式を用いることで、受信している衛星が３衛星の場合でもＧＰＳのみを使用して長時間３次元絶対位置算出ができるようになる。
また方位角度が３衛星以上存在すれば短期間初期化（静止状態を含む）で出力可能となる。ピッチとヨーは原点が現在位置・ｘ軸が東方向向き・ｙ軸が北方向向きの座標に対する車両座標系の回転角度として演算可能である。
以上のシステムはＧＰＳ　適用範囲を大きく広げることができ、同時にピッチ、ヨーの推定、精度の向上もできる。
【００１９】
図６は実施例２の位置・方位演算処理手段のフローチャートを示している。
ステップ２００〜ステップ２０９で構成されている。この実施例は前述のようにシステム方程式をカルマンフィルタ手法により演算する。
【００２０】
（移動体の位置と方位の推定システムの第３の実施例）
この第３の実施例は、第２図に示す装置により実行される。図７Ａ（ステップ３００〜３１９）と図７Ｂ（ステップ３０７０〜３０７８）は実施例３の位置・方位演算処理アルゴリズムのフローチャートを示している。図７Ｂは図７Ａのステップ３０７の詳細を示す流れ図である。
この装置の基本的な構成は図１と殆どかわらない。アンテナＡ・ＢはＧＰＳ衛星からの電波を受信するために、移動体に既知距離ｌに設置され、受信手段４で同時に受信された電波を同一クロックで周波変換をで行う。
信号検出復調手段５で信号の検出と解読を行い航法メッセージ（このメッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータ導出が可能である）、擬似距離、搬送波の位相を同一クロックで導出する。この第３の実施例を実行するためには搬送波の位相が必要になる点は先の構成と異なる。
位置方位演算手段７は、これらの情報と既知アンテナ幾何学情報（相対姿勢・距離等）をもとに移動体位置・方位計演算アルゴリズムにより算出する。
【００２１】
図２は、本発明によるＧＰＳによる移動体の位置・姿勢推定システムを実行するさらに第２の装置のブロック図である。ＧＰＳ受信手段４においてＡアンテナ１とＢアンテナ２から電波を受信する。
受信手段４では発振器３からの同一クロックで擬似距離、搬送波の位相の導出６を行う。
位置方位演算手段は７は、航法メッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータを導出し、これらの情報と擬似距離、搬送波の位相と既知アンテナ間距離情報がＧＰＳによる移動体の位置・方位演算アルゴリズムにより処理される。
【００２２】
同一クロックを用いているためアンテナ間一重位相差　Ｄφ_ｉＡＢ　（ｔ）を演算することで受信機の内部時計誤差による位相データの誤差、衛星の時計誤差による位相データの誤差が消去される。
さらにアンテナ間一重位相差　Ｄφ_ｉＡＢ　（ｔ）を以下の方程式で近似して演算できる。

【００２３】
以上の式を用いて搬送波位相測定の幾何学、２アンテナ幾何学条件を含めたｎ個衛星測定方程式は以下のようになる。

更に、

【００２４】
ここで、
ｌはＡアンテナ　とアンテナＢ間の既知距離
ｘ　^Ａ　（ｔ）　，ｙ　^Ａ（ｔ）　，ｚ　^Ａ（ｔ）はｔ時刻でＡアンテナの座標
ｘ　^Ｂ　（ｔ）　，ｙ　^Ｂ（ｔ）　，ｚ　^Ｂ（ｔ）はｔ時刻でＢアンテナの座標
Ｄφ_ｉＢＡ　（ｔ）はｔ時刻でｉ番目、受信機Ｂ、受信機Ａによる一重位相（受信電波計測により既知）
ｈ　^Ａ　_ｘｉ（ｔ），ｈ　^Ａ　_ｙｉ（ｔ）　，ｈ　^Ａ　_ｚｉ　（ｔ）はｔ時刻でｉ番目の衛星とＡアンテナ間の方向余弦（衛星軌道情報により既知）
Ｎ　_ｉＢＡはｉ番目衛星、受信機Ａ、受信機Ｂによる一重位相の整数値アンビギュティ
ｃは光速
ｆは搬送波周波数
さらに、幾何学条件を示す方程式、ｎ個衛星の測定による測定方程式を近似値
ｘ　^Ａ　，ｙ　^Ａ　，ｚ　^Ａのまわりでテイラー展開し１次までの項をとって得たＧＰＳ　３次元測位方程式は以下の用になる。
【００２５】

【００２６】
ここで、
ｘ　_ｉ　（ｔ），　ｙ　_ｉ　（ｔ），　ｚ　_ｉ　（ｔ）はｉ番目の衛星の座標（衛星軌道情報により既知）
Ｐ　^Ａ　_ｉ　（ｔ）　，　Ｐ　^Ｂ　_ｉ　（ｔ）はｉ番目の衛星のＡアンテナ１とＢアンテ２での観測疑似距離（受信電波計測により既知）
△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_ＡはＡアンテナの位置座標補正値
δｔ　（ｔ）は受信時刻誤差
式９の未知数はアンテナ間ベクタＢＡの３次元成分と各一重位相毎式整数アンビギュイティ（Ｎ　_ｉＢＡ）ということになる。この式はｍ　＝０では未知数を求めることができない。この問題は複数エポックに対応した（ｍ≧１）式９を用いて化観測性を確保した方程式を用いることで解決できる。
各整数アンビギュイティＮ　_ｉＢＡの値が収束するまで複数の時刻ｔにおける式９の解を最小２乗法で得ることでシステム初期化を行いながらアンテナ間ベクタＢＡの３次元成分推定が可能になる。この初期化アルゴリズムで得られた解はＦｌｏａｔ解と呼ばれる。
一旦収束したＮ　_ｉＢＡの値が得られると式９は式１９のように書き表すことが出来る（ここでｃ／ｆ　Ｎ　_１ＢＡ　，　ｃ／ｆ　Ｎ　_２ＢＡ　，　ｃ／ｆ　Ｎ　_ｎＢＡのあたいは既知である）。
ｃ／ｆ　Ｎ　_１ＢＡ　，　ｃ／ｆ　Ｎ　_２ＢＡ　，　ｃ／ｆ　Ｎ　_ｎＢＡが既知であるのでこの式の解は最小２乗法で得られる。この式で得られた解はＦｉｘ解と呼ばれる。

【００２７】
以上の式９または式１９で得られたアンテナ間ベクタＢＡの３次元成分を式１８に用いてアンテナ位置推定値が得られる。式１８，式９，式１９の解は最小２乗法のような一般的なアルゴリズムで得られる。
以上の式１９では２衛星（２個の一重位相測定・ｎ　＝２）の場合未知変数３つ（ｄｘ_ＢＡ（ｔ），ｄｙ　_ＢＡ　（ｔ），ｄｚ_ＢＡ　（ｔ））に対して３つの独立方程式が得られる。
また式１８では同様に２衛星（４個の測定・ｎ　＝２）の場合未知変数４つ（△ｘ　_Ａ　（ｔ）　，△ｙ　_Ａ　（ｔ）　，△ｚ　_Ａ　（ｔ），　ｃδｔ　（ｔ））に対して５つの独立方程式が得られる。この場合衛星２個があるとシステムの可観測性が存在し姿勢が推定可能となる。
【００２８】
以上の方式を用いることで、受信している衛星が２衛星の場合でもＧＰＳのみを使用して３次元絶対位置算出ができるようになる。
アンテナ間ベクタＢＡ　３次元成分の値からピッチとヨーの演算を行う。ピッチとヨーは原点が現在位置・ｘ軸が東方向向き・ｙ軸が北方向向きの座標に対する車両座標系の回転角度として演算可能である。
以上のシステムはＧＰＳ　適用範囲を大きく広げることができ、同時にピッチ、ヨーの推定、精度の向上もできる。
【００２９】
図７Ａ（ステップ３００〜３１９）と図７Ｂ（ステップ３０７０〜３０７８）は実施例３の位置・方位演算処理アルゴリズムのフローチャートを示している。図７Ｂは図７Ａのステップ３０７の詳細を示す流れ図である。式１９を最小２乗る法を用いて解き、式Ｘ（ｔ）を求めている。
【００３０】
（移動体の位置と方位の推定システムの第４の実施例）
図３は、本発明によるシステムの第４の実施例を実施するために利用される装置のブロック図である。移動体でＧＰＳ衛星からの電波を受信するために、Ａアンテナ１とＢアンテナ２が既知距離ｌだけ離れて移動体に設けられている。
受信手段４ａおよび受信手段４ｂは、各アンテナが受信した各電波周波変換を行う。各受信手段の出力はそれぞれ信号検出復調手段５ａ，５ｂにに接続されている。受信手段４ａおよび信号検出復調手段５ａは発振器４ａの発生するクロックで動作させられ、受信手段４ｂおよび信号検出復調手段５ｂは発振器４ｂの発生するクロックで動作させられる。
図１、図２に示した装置は、同一のクロックで動作さけられているがこの装置は個別の発振器を使用している。以上の構成により各アンテナ信号の検出が行われる。この信号は位置方位演算手段７に接続される。Ｃアンテナ９は基地局（図示せず）から送信された補正情報を受信し、補正情報受信機１０に接続する。位置方位演算手段７にはさらに、前記補正情報受信機１０からの補正情報が接続されている。位置方位演算手段７は航法メッセージ（このメッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータ導出が可能である）、を導出する各手段を備えている。
【００３１】
動作について説明する。移動体に搭載された２つのアンテナ１，２から電波の受信を行い、ＧＰＳ受信手段４ａ，４ｂで各信号の周波数変換を行う。
信号検出復調手段により各信号の検出と解読を行い航法メッセージ、擬似距離、搬送波の位相の導出を行う。
基地局から送信された補正情報はＣアンテナ９で受信され、補正情報受信機１０で処理さる。航法メッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータを導出し、これらの情報と擬似距離、搬送波の位相、補正情報と既知アンテナ間距離情報がＧＰＳ位置・方位演算アルゴリズムにあたえられる。
【００３２】
エポックｔにおいて受信機Ａアンテナと受信機Ｂアンテナ間二重位相差Ｄ　Ｄφ_ｉｊＡＢ（ｔ）、受信機Ｘアンテナと基地局Ｒアンテナ間二重位相差Ｄ　Ｄφ_ｉｊＸＲ（ｔ）は以下の方程式のようになる。

ここで、
ｒ_ＫＸ（ｔ）はＧＰＳ信機アンテナＸと衛星ｋ間の距離
φ_ＫＸ（ｔ）はＧＰＳ受信機アンテナＸで測定された衛星ｋからの搬送位相データ
以上の式を用いて、衛星１を基準とし搬送波位相測定の幾何学条件を表すｎ個衛星測定方程式は以下のようになる。

さらに、

【００３３】
ここで、
ｌはアンテナＡとアンテナＢ間の既知距離
ｘ　_ｉ　（ｔ）　，ｙ　_ｉ（ｔ）　，ｚ　_ｉ（ｔ）はｔ時刻でｉ番目の衛星の座標（衛星軌道情報により既知）
ｘ　^Ａ　（ｔ）　，ｙ　^Ａ（ｔ）　，ｚ　^Ａ（ｔ）はｔ時刻でアンテナＡの座標
ｘ　^Ｂ　（ｔ）　，ｙ　^Ｂ（ｔ）　，ｚ　^Ｂ（ｔ）はｔ時刻でアンテナＢの座標
ｒ_ＫＸ（ｔ）はＧＰＳ受信機アンテナＸと衛星ｋ間の距離
ｒ_ＫＲ（ｔ）は基地局受信機アンテナＲと衛星ｋ間の距離
△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_Ａはｔ時刻でＡアンテナの位置座標補正値
△ｘ　_Ｂ　，△ｙ　_Ｂ　，△ｚ　_Ｂはｔ時刻でＢアンテナの位置座標補正値
Ｄ　Ｄφ_ｉｊＸＲ（ｔ）はｔ時刻でｉ番目、ｊ番目衛星受信機Ｘ、基地局Ｒによる二重位相　（受信電波計測、補正データにより既知）
Ｄ　Ｄφ_ｉｊＡＢ（ｔ）はｔ時刻でｉ番目、ｊ番目衛星受信機Ａ、受信機Ｂによる二重位相　（受信電波計測により既知）
Ｎ_ｉｊＸＲはｉ番目、ｊ番目衛星受信機Ｘ、基地局Ｒによる二重位相の整数値アンビギュティ
Ｎ_ｉｊＡＢはｉ番目、ｊ番目衛星受信機Ａ、受信機Ｂによる二重位相の整数値アンビギュティ
ｃは光速
ｆは搬送波周波数
【００３４】
各整数アンビギュイティＮ　_ｉｊＲＡ，　Ｎ　_ｉｊＲＢ　，　Ｎ　_ｉｊＡＢの値が収束するまで複数の時刻における式２３のかいを最小２乗法で得ることで各アンテナの３次元座標推定が可能になる。この初期化アルゴリズムで得られた解はＦｌｏａｔ解と呼ばれる。
一旦収束したＮ　_ｉｊＲＡ，　Ｎ　_ｉｊＲＢ　，　Ｎ　_ｉｊＡＢの値が得られると式２３は式３５のように書き表すことができる（ここで各　ｃ／ｆ　Ｎ_ｉｊＲＡ　，　ｃ／ｆ　Ｎ_ｉｊＲＢ　，　ｃ／ｆ　Ｎ_ｉｊＡＢのあたいは既知である）。各ｃ／ｆ　Ｎ　_ｉｊＲＡ　，　ｃ／ｆ　Ｎ_ｉｊＲＢ　，　ｃ／ｆ　Ｎ_ｉｊＡＢが既知であるのでこの式の解は最小２乗法で得られる。この式で得られた解はＦｉｘ解と呼ばれる。

【００３５】
通常（単独アンテナを用いた）の補正データを用いた手段で導かれる方程式は衛星数が４個以下の場合では解が存在しない。以上の方式では３衛星（二重位相６個・ｎ＝３）の場合未知変数６つ（△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_Ａ　，△ｘ　_Ｂ　，△ｙ　_Ｂ　，△ｚ　_Ｂ）に対して６つの独立方程式が得られる。以上の方式を用いることで、受信している衛星が３衛星の場合でもＧＰＳのみを使用して３次元絶対位置算出ができるようになる。
アンテナＡとＢの座標の値からアンテナ間線分のピッチとヨーの演算を行う。ピッチとヨーは原点が現在位置・ｘ軸が東方向向き・ｙ軸が北方向向きの座標に対する車両座標系の回転角度として演算可能である。
以上のシステムはＧＰＳ適用範囲を大きく広げることができ、同時にピッチ、ヨーの推定、精度の向上もできる。
【００３６】
図８は、実施例４の位置・方位演算処理アルゴリズムのフローチャート（ステップ４００〜４１５）を示している。
（移動体の位置と方位の推定システムの第５の実施例）
図４は、本発明によるシステムの第５の実施例を実施するために利用される装置（第４の装置）のブロック図である。
移動体でＧＰＳ衛星からの電波を受信するために、Ａアンテナ１とＢアンテナ２が既知距離ｌだけ離れて移動体に設けられている。
受信手段４の出力は信号検出復調手段５に接続されている。受信手段４および信号検出復調手段５発振器４の発生する同一クロックで動作させられる。
以上の構成により各アンテナ信号の検出が行われる。この信号は位置方位演算手段７に接続される。Ｃアンテナ９は基地局（図示せず）から送信された補正情報を受信し、補正情報受信機１０に接続する。位置方位演算手段７にはさらに、前記補正情報受信機１０からの補正情報が接続されている。位置方位演算手段７は航法メッセージ（このメッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータ導出が可能である）、搬送波の位相を導出する各手段を備えている。
【００３７】
動作について説明する。
ＧＰＳ受信手段４において２つのアンテナ１，２かからの電波の受信を行う。
前記受信手段４で同一クロックを用いて同時に受信された信号の周波数変換を行う。そして信号の検出と解読を行い航法メッセージ、搬送波の位相の導出を同一クロックで行う。
一方基準局（図示せず）から送信された補正情報受信を行う。航法メッセージからアルマナックデータ、エフェメリデータ、ＧＰＳ時刻情報、電離層補正パラメータを導出し、これらの情報と擬似距離、搬送波の位相、補正情報と既知アンテナ間距離情報が位置方位演算手段７のＧＰＳ位置・方位演算アルゴリズムで処理される。
【００３８】
エポックｔにおいて受信機Ａアンテナと受信機Ｂアンテナ間一重位相差Ｄφ_ｉＡＢ　（ｔ）受信機Ｘアンテナと基地局Ｒアンテナ間二重位相差Ｄ　Ｄφ_ｉｊＸＲ（ｔ）は下の方程式のようになる。

ここで、
ｒ_ＫＸ（ｔ）はＧＰＳ信機アンテナＸと衛星ｋ間の距離
φ_ＫＸ（ｔ）はＧＰＳ受信機アンテナＸで測定された衛星ｋからの搬送位相データ
【００３９】
同一クロックを用いているためアンテナ間一重位相差Ｄφ_ｉＡＢ　（ｔ）を演算することで受信機の内部時計誤差による位相データの誤差、衛星の時計誤差による位相データの誤差消去が可能であり、この（受信機アンテナ間）場合二重位相差の演算が必要で無い。
【００４０】
以上の式を用いて、衛星１を基準とし搬送波位相測定の幾何学、２アンテナ幾何学条件を含めたｎ個衛星測定方程式は以下の用になる。

さらに、

ここで、
ｌはアンテナＡとアンテナＢ間の既知距離
ｘ　_ｉ　（ｔ）　，ｙ　_ｉ（ｔ）　，ｚ　_ｉ（ｔ）はｔ時刻でｉ番目の衛星の座標（衛星軌道情報により既知）
ｘ　^Ａ　（ｔ）　，ｙ　^Ａ（ｔ）　，ｚ　^Ａ（ｔ）はｔ時刻でアンテナＡの座標
ｘ　^Ｂ　（ｔ）　，ｙ　^Ｂ（ｔ）　，ｚ　^Ｂ（ｔ）はｔ時刻でアンテナＢの座標
ｒ_ＫＸ（ｔ）はＧＰＳ受信機アンテナＸと衛星ｋ間の距離
ｒ_ＫＲ（ｔ）は基地局受信機アンテナＲと衛星ｋ間の距離
△ｘ　_Ａ　（ｔ）　，△ｙ　_Ａ　（ｔ）　，△ｚ　_Ａ　（ｔ）はｔ時刻でＡアンテナの位置座標補正値
△ｘ　_Ｂ　（ｔ）　，△ｙ　_Ｂ　（ｔ）　，△ｚ　_Ｂ　（ｔ）はｔ時刻でＢアンテナの位置座標補正値
Ｄ　Ｄφ_ｉｊＸＲ（ｔ）はｔ時刻でｉ番目、ｊ番目衛星受信機Ｘ、基地局Ｒによる二重位相　（受信電波計測、補正データにより既知）
Ｄφ_ＡＢ（ｔ）はｔ時刻でｉ番目衛星受信機Ａ受信機Ｂによる一重位相　（受信電波計測より既知）
Ｎ_ｉｊＸＲはｉ番目、ｊ番目衛星受信機Ｘ、基地局Ｒによる二重位相の整数値アンビギュティ
Ｎ_ｉＡＢ　はｉ番目衛星、受信機Ａ、受信機Ｂによる一重位相の整数値アンビギュティ
ｃは光速
ｆは搬送波周波数
【００４１】
各整数アンビギュイティＮ　_ｉｊＲＡ，　Ｎ　_{ｉｊＸＲＢ}　，　Ｎ　_ｉＡＢ値が収束するまで複数の時刻における式３９の解を最小２乗法で得ることで各アンテナの３次元座標推定が可能になる。この初期化アルゴリズムで得られた解はＦｌｏａｔ解と呼ばれる。
一旦収束したＮ　_ｉｊＲＡ，　Ｎ　_ｉｊＲＢ　，　Ｎ　_ｉＡＢの値が得られると式３９は式６２のように書き表すことが出来る（ここで各　ｃ／ｆ　Ｎ_ｉｊＲＡ　，　ｃ／ｆ　Ｎ_ｉｊＲＢ　，　ｃ／ｆ　Ｎ_ｉＡＢのあたいは既知である）。
各ｃ／ｆ　Ｎ　_ｉｊＲＡ　，　ｃ／ｆ　Ｎ_ｉｊＲＢ　，　ｃ／ｆ　Ｎ_ｉＡＢが既知であるのでこの式の解は最小２乗法で得られる。この式で得られた解はＦｉｘ解と呼ばれる。

【００４２】
通常（単独アンテナを用いた）の補正データを用いた手段で導かれる方程式は衛星数が４個以下の場合では解が存在しない。以上の式６２では３衛星（一重位相３個・二重位相４個・ｎ＝３）の場合未知変数６つ（△ｘ　_Ａ　，△ｙ　_Ａ　，△ｚ　_Ａ　，△ｘ　_Ｂ　，△ｙ　_Ｂ　，△ｚ　_Ｂ）に対して８つの独立方程式が得られる。以上の方式を用いることで、受信している衛星が３衛星の場合でもＧＰＳのみを使用して３次元絶対位置算出ができるようになる。
アンテナＡとＢの座標の値からアンテナ間線分のピッチとヨーの演算を行う。ピッチとヨーは原点が現在位置・ｘ軸が東方向向き・ｙ軸が北方向向きの座標に対する車両座標系の回転角度として演算可能である。
以上のシステムはＧＰＳ適用範囲を大きく広げることができ、同時にピッチ、ヨーの推定、精度の向上もできる。
【００４３】
図９は実施例５の位置・方位演算処理アルゴリズムのフローチャート（ステップ５００〜５１７）を示している。この実施例は、基地局の情報を利用して演算される各受信アンテナと基地局の位相差を用いる点に特徴がある。
【００４４】
【発明の効果】
以上、説明したように本発明よるシステムによれば、同時に受信された信号を同一クロックで処理することでレシーバクロックを表す未知変数が一つのみになる。幾何学条件を用いて未知変数を含めた測定方程式から独立した方程式が記述可能となる。この新たな方程式を用いることで従来の方式と比較すると（未知変数−未知数を含めた独立方程式）が低減される。
この結果同数の測定（衛星）でも演算可能となった。
【００４５】
本発明では複数アンテナの幾何学条件を用いることにより、測定に必要な衛星の数の低減が可能となり、測定可能な範囲を大きく広げることができ、同時に精度の向上もできる。
【００４６】
また、本発明では、前述のように、複数アンテナの幾何学条件や、同一クロックを用いない場合でも、前記第４，第５の実施例に示すように、移動体に複数のアンテナを搭載し、受信アンテナ間の二重位相差または一重位相差、と各受信アンテナと基地局アンテナ間二重位相差または一重位相差を用いて（未知変数−未知変数を含めた独立方程式）を低減し位置・姿勢を正確に推定できる。
【００４７】
以上詳しく説明した実施例について本発明の範囲内で種々の変形を施すことができる。そのようなものも本発明の範囲に含まれるべきものと理解されるべきである。本発明の実施例は何れも移動体の位置と姿勢を推定することができるように構成されているが、位置と姿勢のいずれかのみを推定する場合も本発明の範囲に含まれと理解されたい。
【図面の簡単な説明】
【図１】本発明によるＧＰＳによる位置・姿勢推定システムを実行する第１の装置のブロック図である。
【図２】本発明によるＧＰＳによる位置・姿勢推定システムを実行する第２の装置のブロック図である。
【図３】本発明によるＧＰＳによる位置・姿勢推定システムを実行する第３の装置のブロック図である。
【図４】本発明によるＧＰＳによる位置・姿勢推定システムを実行する第４の装置のブロック図である。
【図５】本発明によるＧＰＳによる位置・姿勢推定シスの第１の実施例のフローチャートである。
【図６】本発明によるＧＰＳによる位置・姿勢推定シスの第２の実施例のフローチャートである。
【図７Ａ】本発明によるＧＰＳによる位置・姿勢推定シスの第３の実施例のフローチャートの主要部分を示す図である。
【図７Ｂ】図７Ａに示すフローチャートの一部を補充するためのフローチャートである。
【図８】本発明によるＧＰＳによる位置・姿勢推定シスの第４の実施例のフローチャートである。
【図９】本発明によるＧＰＳによる位置・姿勢推定シスの第５の実施例のフローチャートである。
【符号の説明】
１　Ａアンテナ
２　Ｂアンテナ
３　発振器
４　受信手段
５　信号検出復調手段
６　航法メッセージ疑似距離信号
７　位置方位演算手段
８　アンテナ間距離情報信号
９　Ｃアンテナ
１０　補正情報受信手段[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a system for estimating the position and orientation of a moving object by GPS, more specifically, a GPS that enables simultaneous estimation of the position and orientation of a moving object (vehicle, aircraft, ship, person, etc.) using a GPS system. The present invention relates to a system for estimating the position / posture of a moving object by using a computer.
[0002]
[Prior art]
Conventionally, many systems for estimating the position of a moving body have been proposed and put to practical use. However, these conventional techniques cannot reduce the minimum number of required satellites, so that the range of practical application is narrowed, which has been regarded as a problem of the GPS system.
[0003]
[Problems to be solved by the invention]
In an urban area where the number of visible satellites cannot be sufficiently secured by such a position estimation system, it is often difficult to estimate the position, and it has been impossible to simultaneously estimate the attitude of the moving object together with the position.
In the prior art, since the number of minimum satellites required for position estimation cannot be reduced, a revolutionary change in the method is required.
In the present invention, attention has been paid to the phase difference between the receiving antennas and the phase difference between each receiving antenna and the base station antenna by introducing geometric conditions of a plurality of antennas, using the same clock, or.
SUMMARY OF THE INVENTION It is an object of the present invention to be able to simultaneously estimate the attitude and position under conditions that the accuracy is higher than that of a normal GPS and the number of satellites required is smaller than that of a normal GPS (three satellites in the case of two antennas),・ To provide a posture estimation system.
[0004]
[Means for Solving the Problems]
In order to achieve the above object, a position / posture estimation system for a moving object by GPS according to claim 1 according to the present invention,
At least one pair of antennas fixed to a mobile body by predetermined geometric conditions to receive radio waves from GPS satellites;
Receiving means for performing frequency conversion of radio waves simultaneously received by the antenna using the same clock,
With the same clock from the received signal, a pseudo-range or a pseudo-range and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
The navigation message, a pseudorange or a pseudorange and a carrier phase, and a moving object position / azimuth calculating means for calculating a position of each of the antennas or a gradient of a line segment between the antennas based on a geometric condition of the antenna. ing.
[0005]
According to a second aspect of the present invention, there is provided a position / posture estimation system for a moving object based on a GPS.
At least one pair of antennas fixed to a mobile body by predetermined geometric conditions to receive radio waves from GPS satellites;
Receiving means for performing frequency conversion of radio waves simultaneously received by the antenna using the same clock,
With the same clock from the received signal, a pseudo-range or a pseudo-range and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, the pseudorange or the pseudorange and the carrier phase, the correction information from the base station, and the moving object position for calculating the position of each antenna or the inclination of the line segment between the antennas according to the geometric conditions of the antenna. The azimuth calculation means is provided.
[0006]
According to a third aspect of the present invention, there is provided a position / posture estimation system for a moving object by GPS.
At least one pair of antennas fixed to a mobile object for receiving radio waves from GPS satellites;
Receiving means for frequency-converting radio waves simultaneously received by the antenna,
From the received signal, a pseudorange or a pseudorange and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, pseudorange or pseudorange and carrier phase, and the phase difference between the receiving antennas, the position of each antenna or the line segment between each receiving antenna by the phase difference between each receiving antenna and the base station antenna. It is provided with a moving body position / azimuth calculating means for calculating a tilt.
[0007]
According to a fourth aspect of the present invention, there is provided a position / posture estimation system for a moving object by GPS.
At least one pair of antennas fixed to a mobile object for receiving radio waves from GPS satellites;
Receiving means for frequency-converting radio waves simultaneously received by the antenna,
From the received signal, a carrier phase is detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, pseudorange or pseudorange and carrier phase, and the phase difference between the receiving antennas, the position of each antenna or the line segment between each receiving antenna by the phase difference between each receiving antenna and the base station antenna. It is provided with a moving body position / azimuth calculating means for calculating a tilt.
[0008]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, an embodiment of a system for estimating the position and orientation of a moving object by GPS according to the present invention will be described with reference to the drawings and the like.
FIG. 1 is a block diagram of a first device that executes a GPS position / posture estimation system according to the present invention. The A antenna 1 and the B antenna 2 are fixed to a mobile body while maintaining certain geometric conditions. In this embodiment, the A antenna 1 and the B antenna 2 are fixed in the same posture and separated by a distance l. The signals from the satellites received by the A antenna 1 and the B antenna 2 are connected to the receiving means 4. A signal from the same clock 3 is connected to a receiving circuit provided for each antenna. That is, signals from each antenna are processed based on the same clock reference. The output of the receiving means 4 is connected to a signal detection / demodulation circuit 5, which detects the navigation message and the pseudo distance between both antennas with the same clock.
[0009]
The navigation message and the pseudo distance signal 6 from the signal detection / demodulation circuit 5 are connected to a position / azimuth calculating means 7.
A navigation message decryption algorithm is stored in the position / azimuth calculation means 7, whereby the almanac data, efmelis data, GPS time information, and ionospheric correction parameters are decrypted and obtained from the navigation message.
The position and orientation calculation means 7 estimates (calculates) the position and orientation of the moving object from the acquired data and the previously given inter-antenna distance l and pseudo distance by using a position and orientation calculation algorithm, and outputs it. I do.
[0010]
(First Embodiment of System for Estimating Position and Orientation of Moving Object)
Next, an embodiment of a system for estimating the position and orientation of a moving object will be described with reference to the flowchart (steps 100 to 110) shown in FIG. This system is implemented using the apparatus shown in FIG.
The equation representing the geometric condition, the measurement equation based on the measurement of n satellites is approximated by x^A, Y^A, Z^A, X^B, Y^B, Z^BThe GPS three-dimensional positioning equation obtained by performing a Taylor expansion around and taking the terms up to the first order is as follows.

[0011]
here
l is the known distance between antenna A and antenna B
x_iY_i{Z}_iIs the coordinate of the i-th satellite (known from the satellite orbit information)
x^A, Y^A, Z^AIs the coordinates of the A antenna
x^B, Y^B, Z^BIs the coordinates of the B antenna
P^A _i, P^B _iIs the pseudo distance observed by the A and B antennas of the i-th satellite (known from measurement of received radio waves)
h^A _xi, H^A _yi, H^A _ziIs the direction cosine between the i-th satellite and the A antenna (known from satellite orbit information)
h^B _xi, H^B _yi, H^B _ziIs the direction cosine between the i-th satellite and the B antenna (known from satellite orbit information)
△ x_A, △ y_A, △ z_AIs the position coordinate correction value of the A antenna
△ x_B, △ y_B, △ z_BIs the position coordinate correction value of the B antenna
δt is the reception time error
[0012]
The general solution of the above equation 1 is obtained by the least squares method. Equations derived by ordinary means (without considering geometric conditions) have no solution when the number of satellites is four or less. In the above method, in the case of three satellites (six measured values, n = 3), seven unknown variables ({x})_A, △ y_A, △ z_A, △ x_B, △ y_B, △ z_B{, {T}) yields seven independent equations. By using the above method, even when the number of receiving satellites is three, three-dimensional absolute position calculation can be performed using only GPS.
[0013]
The pitch and the yaw of the line segment between the antennas are calculated from the coordinates of the A antenna 1 and the B antenna 2. The pitch and the yaw can be calculated as the rotation angle of the vehicle coordinate system with respect to the coordinates where the origin is the current position, the x axis is in the east direction, and the y axis is in the north direction.
The above system can greatly expand the GPS application range, and at the same time, can estimate pitch and yaw and improve accuracy.
[0014]
FIG. 5 is a flowchart for explaining a first embodiment of a system for estimating the position and orientation of a moving object by GPS implemented using the first device shown in FIG.
It comprises steps 100 to 110.
This embodiment uses a pseudorange and an antenna geometric condition in a mobile position and orientation calculation algorithm. Equations indicating the antenna geometric conditions are solved using the least squares method as shown in step 105.
[0015]
(Second Embodiment of System for Estimating Position and Orientation of Moving Object)
The second embodiment of the system for estimating the position and orientation of a moving object is implemented using the apparatus shown in FIG.
[0016]
An output equation based on measurement of n satellites from both antennas in consideration of geometric conditions is as follows.
Equation 7

[0017]
here,
l is the known distance between antenna A and antenna B
x_i{(T), {y}_i{(T), {z}_i(T) is the coordinate of the i-th satellite at time t (known from satellite orbit information)
x (t), y (t), z (t) are the coordinates of A antenna (or B)
ρ_i ^A{(T)}, ρ_i ^B(T) is the pseudo distance observed by the A antenna and the B antenna of the i-th satellite at time t (known from measurement of received radio waves)
α (t) and β (t) are the time t and the direction Euler angle (unknown variable) between the AB antenna line segments δtδ (t) is the reception time error
[0018]
The above equation and the state equation f (x (t), y 含 め る (t) including x (t), y (t), z (t), α (t), β (t), and {At} (t) as state variables ), Z (t), α (t), β (t), {At} (t),...) Describe the system equation. When this system equation is used for the Kalman filter method, x (t), y (t), z (t), α (t), β (t), and δt (t) can be estimated.
By using the above method, it is possible to calculate a three-dimensional absolute position for a long time using only GPS even when three satellites are being received.
If there are three or more azimuth angles, the output can be performed in a short period of time (including a stationary state). The pitch and the yaw can be calculated as the rotation angle of the vehicle coordinate system with respect to the coordinates where the origin is the current position, the x axis is in the east direction, and the y axis is in the north direction.
The above system can greatly expand the GPS application range, and at the same time, can estimate pitch and yaw and improve accuracy.
[0019]
FIG. 6 shows a flowchart of the position / direction calculation processing means of the second embodiment.
It is composed of steps 200 to 209. In this embodiment, the system equation is calculated by the Kalman filter method as described above.
[0020]
(Third Embodiment of System for Estimating Position and Orientation of Moving Object)
This third embodiment is executed by the apparatus shown in FIG. 7A (steps 300 to 319) and FIG. 7B (steps 3070 to 3078) show a flowchart of the position / azimuth calculation processing algorithm according to the third embodiment. FIG. 7B is a flowchart showing details of step 307 in FIG. 7A.
The basic configuration of this device is almost the same as that of FIG. The antennas A and B are installed at a known distance 1 to the mobile object to receive radio waves from GPS satellites, and perform frequency conversion of radio waves simultaneously received by the receiving means 4 using the same clock.
The signal detection and demodulation means 5 detects and decodes the signal to determine the navigation message (almanac data, ephemeri data, GPS time information, ionospheric correction parameters can be derived from this message), pseudorange, and carrier wave phase with the same clock. Derive. The difference from the previous configuration is that the phase of the carrier is required to execute the third embodiment.
The position / azimuth calculation means 7 calculates the position / azimuth of the mobile object based on the information and the known antenna geometric information (relative attitude, distance, etc.) using an algorithm.
[0021]
FIG. 2 is a block diagram of a second device that executes the system for estimating the position and orientation of a moving object by GPS according to the present invention. The GPS receiving means 4 receives radio waves from the A antenna 1 and the B antenna 2.
The receiving means 4 derives the pseudo distance and the phase of the carrier wave 6 using the same clock from the oscillator 3.
The position / azimuth calculating means 7 derives almanac data, ephemeri data, GPS time information, and ionospheric correction parameters from the navigation message. Is processed by the position / azimuth calculation algorithm.
[0022]
Since the same clock is used, single phase difference between antennas Dφ_iABBy calculating (t), errors in the phase data due to the internal clock error of the receiver and errors in the phase data due to the clock error of the satellite are eliminated.
Furthermore, the single phase difference between antennas Dφ_iAB(T) can be calculated by approximating the following equation.

[0023]
Using the above equations, the n satellite measurement equations including the carrier phase measurement geometry and the two-antenna geometry conditions are as follows.

Furthermore,

[0024]
here,
l is the known distance between antenna A and antenna B
x^A{(T)}, y^A(T), z^A(T) is the coordinate of the A antenna at time t
x^B{(T)}, y^B(T), z^B(T) is the coordinate of the B antenna at time t
Dφ_iBA(T) is the i-th at time t, single phase by receiver B and receiver A (known by measurement of received radio wave)
h^A _xi(T), h^A _yi(T), h^A _zi(T) is the direction cosine between the i-th satellite and the A antenna at time t (known from satellite orbit information)
N_iBAIs the single-phase integer ambiguity of the i-th satellite, receiver A and receiver B
c is the speed of light
f is the carrier frequency
Furthermore, the equation showing the geometric condition and the measurement equation based on the measurement of n satellites are approximated.
x^A, Y^A, Z^AThe GPS three-dimensional positioning equation obtained by performing Taylor expansion around and taking the terms up to the first order is used as follows.
[0025]

[0026]
here,
x_i{(T), {y}_i{(T), {z}_i(T) is the coordinate of the i-th satellite (known from satellite orbit information)
P^A _i{(T)}, {P}^B _i(T) is the pseudo distance observed by the A antenna 1 and B antenna 2 of the i-th satellite (known from measurement of received radio waves)
△ x_A, △ y_A, △ z_AIs the position coordinate correction value of the A antenna
δt (t) is the reception time error
The unknowns in Equation 9 are the three-dimensional components of the inter-antenna vector BA and the integer ambiguities (N_iBA)It turns out that. In this equation, unknown values cannot be obtained when m = 0. This problem can be solved by using an equation that secures the observability using Equation (9) corresponding to a plurality of epochs (m ≧ 1).
Each integer ambiguity N_iBABy obtaining the solution of Equation 9 at a plurality of times t by the least squares method until the value of converges, it is possible to estimate the three-dimensional component of the inter-antenna vector BA while performing system initialization. The solution obtained by this initialization algorithm is called a Float solution.
N once converged_iBAEquation 9 can be written as Equation 19 (where c / f {N})._1BA, C / f N_2BA, C / f N_nBAIs already known).
c / f {N}_1BA, C / f N_2BA, C / f N_nBAIs known, the solution of this equation is obtained by the least squares method. The solution obtained by this equation is called a Fix solution.

[0027]
The three-dimensional component of the inter-antenna vector BA obtained by the above equation 9 or 19 is used in equation 18 to obtain an estimated antenna position. The solutions of Expressions 18, 9, and 19 can be obtained by a general algorithm such as the least squares method.
In the above equation 19, in the case of two satellites (two single phase measurements, n = 2), three unknown variables (dx_BA(T), dy_BA(T), dz_BA独立 (t)) yields three independent equations.
In equation 18, similarly, in the case of two satellites (four measurements, n = 2), four unknown variables ({x})_A{(T)}, {y}_A{(T)}, {z}_AFive independent equations are obtained for {(t), {cδt} (t)). In this case, if there are two satellites, the observability of the system exists and the attitude can be estimated.
[0028]
By using the above method, even when the number of receiving satellites is two, three-dimensional absolute position calculation can be performed using only GPS.
The pitch and yaw are calculated from the value of the inter-antenna vector BA 3D component. The pitch and the yaw can be calculated as the rotation angle of the vehicle coordinate system with respect to the coordinates where the origin is the current position, the x axis is in the east direction, and the y axis is in the north direction.
The above system can greatly expand the GPS application range, and at the same time, can estimate pitch and yaw and improve accuracy.
[0029]
7A (steps 300 to 319) and FIG. 7B (steps 3070 to 3078) show a flowchart of the position / azimuth calculation processing algorithm according to the third embodiment. FIG. 7B is a flowchart showing details of step 307 in FIG. 7A. Equation 19 is solved using the method of least squares to find Equation X (t).
[0030]
(Fourth Embodiment of System for Estimating Position and Orientation of Moving Object)
FIG. 3 is a block diagram of an apparatus used to implement a fourth embodiment of the system according to the present invention. In order for a mobile to receive radio waves from GPS satellites, an A antenna 1 and a B antenna 2 are provided on the mobile at a known distance l.
The receiving unit 4a and the receiving unit 4b perform each radio frequency conversion received by each antenna. The output of each receiving means is connected to signal detection / demodulation means 5a, 5b, respectively. The receiving means 4a and the signal detection / demodulation means 5a are operated by the clock generated by the oscillator 4a, and the reception means 4b and the signal detection / demodulation means 5b are operated by the clock generated by the oscillator 4b.
The devices shown in FIGS. 1 and 2 are operated with the same clock, but use a separate oscillator. With the above configuration, each antenna signal is detected. This signal is connected to the position and orientation calculation means 7. The C antenna 9 receives correction information transmitted from a base station (not shown) and connects to the correction information receiver 10. The correction information from the correction information receiver 10 is further connected to the position and orientation calculation means 7. The position / azimuth calculating means 7 is provided with each means for deriving a navigation message (almanac data, ephemeri data, GPS time information, ionospheric correction parameters can be derived from this message).
[0031]
The operation will be described. Radio waves are received from two antennas 1 and 2 mounted on a moving body, and frequency conversion of each signal is performed by GPS receiving means 4a and 4b.
The signal detection and demodulation means detects and decodes each signal to derive a navigation message, a pseudorange, and a phase of a carrier wave.
The correction information transmitted from the base station is received by the C antenna 9 and processed by the correction information receiver 10. The almanac data, ephemeri data, GPS time information, and ionospheric correction parameters are derived from the navigation message, and the information, pseudorange, carrier phase, correction information, and distance information between known antennas are given to the GPS position / azimuth calculation algorithm. .
[0032]
At epoch t, the double phase difference D Dφ between the receiver A antenna and the receiver B antenna_ijAB(T), double phase difference D Dφ between receiver X antenna and base station R antenna_ijXR(T) becomes like the following equation.

here,
r_KX(T) is the distance between GPS transceiver antenna X and satellite k
φ_KX(T) is carrier phase data from satellite k measured by GPS receiver antenna X
Using the above equation, the n satellite measurement equations representing the geometric conditions of the carrier phase measurement with reference to the satellite 1 are as follows.

further,

[0033]
here,
l is the known distance between antenna A and antenna B
x_i{(T)}, y_i(T), z_i(T) is the coordinate of the i-th satellite at time t (known from satellite orbit information)
x^A{(T)}, y^A(T), z^A(T) is the coordinate of antenna A at time t
x^B{(T)}, y^B(T), z^B(T) is the coordinate of antenna B at time t
r_KX(T) is the distance between the GPS receiver antenna X and the satellite k
r_KR(T) is the distance between the base station receiver antenna R and the satellite k
△ x_A, △ y_A, △ z_AIs the position coordinate correction value of antenna A at time t
△ x_B, △ y_B, △ z_BIs the position coordinate correction value of antenna B at time t
D Dφ_ijXR(T) is the double phase 二 重 by the i-th and j-th satellite receivers X and the base station R at the time t (known from measurement of received radio waves and correction data)
D Dφ_ijAB(T) is the time t, the double phase に よ る by the i-th and j-th satellite receivers A and B (known from measurement of received radio waves)
N_ijXRIs the double-phase integer ambiguity of the i-th and j-th satellite receivers X and the base station R
N_ijABIs the double-phase integer ambiguity of the i-th and j-th satellite receivers A and B.
c is the speed of light
f is the carrier frequency
[0034]
Each integer ambiguity N_ijRA, {N}_ijRB, N_ijABBy obtaining the padding of Expression 23 at a plurality of times by the least squares method until the value of converges, the three-dimensional coordinates of each antenna can be estimated. The solution obtained by this initialization algorithm is called a Float solution.
N once converged_ijRA, {N}_ijRB, N_ijABEquation 23 can be written as Equation 35 (where each {c / f} N_ijRA, C / f N_ijRB, C / f N_ijABIs already known). Each c / f {N}_ijRA, C / f N_ijRB, C / f N_ijABIs known, the solution of this equation is obtained by the least squares method. The solution obtained by this equation is called a Fix solution.

[0035]
An equation derived by means using normal (using a single antenna) correction data has no solution when the number of satellites is four or less. In the above method, in the case of 3 satellites (6 double phases, n = 3), 6 unknown variables ({x})_A, △ y_A, △ z_A, △ x_B, △ y_B, △ z_B) Yields six independent equations. By using the above method, even when the number of receiving satellites is three, three-dimensional absolute position calculation can be performed using only GPS.
The pitch and the yaw of the line segment between the antennas are calculated from the coordinates of the antennas A and B. The pitch and the yaw can be calculated as the rotation angle of the vehicle coordinate system with respect to the coordinates where the origin is the current position, the x axis is in the east direction, and the y axis is in the north direction.
The above system can greatly expand the GPS application range, and at the same time, can estimate pitch and yaw and improve accuracy.
[0036]
FIG. 8 shows a flowchart (steps 400 to 415) of the position / direction calculation algorithm according to the fourth embodiment.
(Fifth Embodiment of System for Estimating Position and Orientation of Moving Object)
FIG. 4 is a block diagram of an apparatus (a fourth apparatus) used to implement the fifth embodiment of the system according to the present invention.
In order for a mobile to receive radio waves from GPS satellites, an A antenna 1 and a B antenna 2 are provided on the mobile at a known distance l.
The output of the receiving means 4 is connected to the signal detection / demodulation means 5. The receiving means 4 and the signal detection / demodulation means 5 are operated by the same clock generated by the oscillator 4.
With the above configuration, each antenna signal is detected. This signal is connected to the position and orientation calculation means 7. The C antenna 9 receives correction information transmitted from a base station (not shown) and connects to the correction information receiver 10. The correction information from the correction information receiver 10 is further connected to the position and orientation calculation means 7. The position / azimuth calculation means 7 includes means for deriving a navigation message (almanac data, ephemeri data, GPS time information, ionospheric correction parameters can be derived from the message), and a carrier phase.
[0037]
The operation will be described.
The GPS receiving means 4 receives radio waves from the two antennas 1 and 2.
The receiving means 4 performs frequency conversion of signals received simultaneously using the same clock. Then, the signals are detected and decoded, and the navigation message and the phase of the carrier are derived by the same clock.
On the other hand, it receives correction information transmitted from a reference station (not shown). The almanac data, ephemeri data, GPS time information, and ionospheric correction parameters are derived from the navigation message, and these information, the pseudorange, the phase of the carrier, the correction information, and the distance information between the known antennas are used as the GPS position information of the position / azimuth calculation means 7. Processed by the azimuth calculation algorithm.
[0038]
Single phase difference Dφ between receiver A antenna and receiver B antenna at epoch t_iAB(T) Double phase difference D Dφ between receiver X antenna and base station R antenna_ijXR(T) becomes like the following equation.

here,
r_KX(T) is the distance between GPS transceiver antenna X and satellite k
φ_KX(T) is carrier phase data from satellite k measured by GPS receiver antenna X
[0039]
Since the same clock is used, single phase difference Dφ between antennas_iABBy calculating (t), it is possible to eliminate the error of the phase data due to the internal clock error of the receiver and the error of the phase data due to the clock error of the satellite. Not necessary.
[0040]
Using the above equations, the n satellite measurement equations including the geometry of the carrier wave phase measurement with reference to the satellite 1 and the geometric conditions of the two antennas are as follows.

further,

here,
l is the known distance between antenna A and antenna B
x_i{(T)}, y_i(T), z_i(T) is the coordinate of the i-th satellite at time t (known from satellite orbit information)
x^A{(T)}, y^A(T), z^A(T) is the coordinate of antenna A at time t
x^B{(T)}, y^B(T), z^B(T) is the coordinate of antenna B at time t
r_KX(T) is the distance between the GPS receiver antenna X and the satellite k
r_KR(T) is the distance between the base station receiver antenna R and the satellite k
△ x_A{(T)}, {y}_A{(T)}, {z}_A(T) is the position coordinate correction value of antenna A at time t
△ x_B{(T)}, {y}_B{(T)}, {z}_B(T) is the position coordinate correction value of B antenna at time t
D Dφ_ijXR(T) is the double phase 二 重 by the i-th and j-th satellite receivers X and the base station R at the time t (known from measurement of received radio waves and correction data)
Dφ_AB(T) is the time t, a single phase よ り by the i-th satellite receiver A and receiver B (known from measurement of received radio waves)
N_ijXRIs the double-phase integer ambiguity of the i-th and j-th satellite receivers X and the base station R
N_iABIs the single-phase integer ambiguity of the i-th satellite, receiver A, and receiver B
c is the speed of light
f is the carrier frequency
[0041]
Each integer ambiguity N_ijRA, {N}_ijXRB, N_iABBy obtaining the solution of Equation 39 at a plurality of times by the least square method until the values converge, three-dimensional coordinate estimation of each antenna becomes possible. The solution obtained by this initialization algorithm is called a Float solution.
N once converged_ijRA, {N}_ijRB, N_iABIs obtained, the expression 39 can be written as the expression 62 (where each {c / f} N_ijRA, C / f N_ijRB, C / f N_iABIs already known).
Each c / f {N}_ijRA, C / f N_ijRB, C / f N_iABIs known, the solution of this equation is obtained by the least squares method. The solution obtained by this equation is called a Fix solution.

[0042]
An equation derived by means using normal (using a single antenna) correction data has no solution when the number of satellites is four or less. In the above equation 62, in the case of three satellites (three single phases, four double phases, n = 3), six unknown variables ({x})_A, △ y_A, △ z_A, △ x_B, △ y_B, △ z_B) Yields eight independent equations. By using the above method, even when the number of receiving satellites is three, three-dimensional absolute position calculation can be performed using only GPS.
The pitch and the yaw of the line segment between the antennas are calculated from the coordinates of the antennas A and B. The pitch and the yaw can be calculated as the rotation angle of the vehicle coordinate system with respect to the coordinates where the origin is the current position, the x axis is in the east direction, and the y axis is in the north direction.
The above system can greatly expand the GPS application range, and at the same time, can estimate pitch and yaw and improve accuracy.
[0043]
FIG. 9 shows a flowchart (steps 500 to 517) of the position / direction calculation algorithm according to the fifth embodiment. This embodiment is characterized in that a phase difference between each receiving antenna and the base station, which is calculated using information on the base station, is used.
[0044]
【The invention's effect】
As described above, according to the system according to the present invention, the signals received at the same time are processed by the same clock, so that there is only one unknown variable representing the receiver clock. Using the geometric condition, an equation independent of the measurement equation including the unknown variable can be described. By using this new equation, (independent equation including unknown variable-unknown number) is reduced as compared with the conventional method.
As a result, even the same number of measurements (satellite) can be calculated.
[0045]
In the present invention, by using the geometric conditions of a plurality of antennas, the number of satellites required for measurement can be reduced, and the measurable range can be greatly expanded, and at the same time, the accuracy can be improved.
[0046]
Further, in the present invention, as described above, even when the geometric conditions of a plurality of antennas and the same clock are not used, as shown in the fourth and fifth embodiments, a plurality of antennas are mounted on a moving body. Using the double phase difference or single phase difference between the receiving antennas and the double phase difference or single phase difference between each receiving antenna and the base station antenna (unknown variable-independent equation including unknown variable) to reduce the position・ Position can be accurately estimated.
[0047]
Various modifications can be made to the embodiment described in detail above within the scope of the present invention. It is to be understood that such should be included in the scope of the present invention. Although all of the embodiments of the present invention are configured to be able to estimate the position and orientation of the moving object, it is understood that the case where only one of the position and orientation is estimated is also included in the scope of the present invention. I want to.
[Brief description of the drawings]
FIG. 1 is a block diagram of a first apparatus that executes a GPS position / posture estimation system according to the present invention.
FIG. 2 is a block diagram of a second device that executes the GPS position / posture estimation system according to the present invention.
FIG. 3 is a block diagram of a third device that executes the GPS position / posture estimation system according to the present invention.
FIG. 4 is a block diagram of a fourth apparatus that executes the GPS position / posture estimation system according to the present invention.
FIG. 5 is a flowchart of a first embodiment of a position / posture estimation system using GPS according to the present invention.
FIG. 6 is a flowchart of a second embodiment of a position / posture estimation system using GPS according to the present invention.
FIG. 7A is a diagram showing a main part of a flowchart of a third embodiment of a GPS position / posture estimation system according to the present invention;
FIG. 7B is a flowchart for supplementing a part of the flowchart shown in FIG. 7A.
FIG. 8 is a flowchart of a fourth embodiment of a position / posture estimation system using GPS according to the present invention.
FIG. 9 is a flowchart of a fifth embodiment of a position / posture estimation system using GPS according to the present invention.
[Explanation of symbols]
1 A antenna
2 B antenna
3 oscillator
4) Receiving means
5 Signal detection and demodulation means
6 Navigation message pseudorange signal
7 ° position and orientation calculation means
8 Distance information signal between antennas
9 C antenna
10 Correction information receiving means

Claims (4)

At least one pair of antennas fixed to a mobile body by predetermined geometric conditions to receive radio waves from GPS satellites;
Receiving means for performing frequency conversion of radio waves simultaneously received by the antenna using the same clock,
With the same clock from the received signal, a pseudorange or a pseudorange and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
The navigation message, the pseudo distance or the pseudo distance and the carrier phase, and the moving object position / azimuth calculating means for calculating the position of each antenna or the inclination of the line segment between the antennas according to the geometric conditions of the antenna. A mobile object position / posture estimation system using GPS. At least one pair of antennas fixed to a mobile body by predetermined geometric conditions to receive radio waves from GPS satellites;
Receiving means for performing frequency conversion of radio waves simultaneously received by the antenna using the same clock,
With the same clock from the received signal, a pseudorange or a pseudorange and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, the pseudorange or the pseudorange and the carrier phase, the correction information from the base station, and the moving object position for calculating the position of each antenna or the inclination of the line segment between the antennas according to the geometric conditions of the antenna. A system for estimating the position and orientation of a moving object by using a GPS, the system including azimuth calculation means. At least one pair of antennas fixed to a mobile object for receiving radio waves from GPS satellites;
Receiving means for frequency-converting radio waves simultaneously received by the antenna,
From the received signal, a pseudorange or a pseudorange and a carrier wave are detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, pseudorange or pseudorange and carrier phase, and the phase difference between the receiving antennas, the position of each antenna or the line segment between each receiving antenna by the phase difference between each receiving antenna and the base station antenna. A system for estimating the position / posture of a moving object by GPS, comprising a moving object position / azimuth calculating means for calculating a tilt. At least one pair of antennas fixed to a mobile object for receiving radio waves from GPS satellites;
Receiving means for frequency-converting radio waves simultaneously received by the antenna,
From the received signal, a carrier phase is detected, and a demodulation means for decoding and demodulating a navigation message,
Correction information receiving means for receiving correction information from the base station,
The navigation message, pseudorange or pseudorange and carrier phase, and the phase difference between the receiving antennas, the position of each antenna or the line segment between each receiving antenna by the phase difference between each receiving antenna and the base station antenna. A system for estimating the position / posture of a moving object by GPS, comprising a moving object position / azimuth calculating means for calculating a tilt.

Images