実施の形態1.
図1は、この発明の実施の形態1による移動体姿勢検出装置の構成を示す図である。本実施の形態による移動体姿勢検出装置は、受信要素1−1〜1−3からなるアンテナ(アンテナ手段)1、衛星方向獲得手段2、基線ベクトル検出手段3及び姿勢演算手段4を含んで構成される。また、基線ベクトル検出手段3は、衛星選択部3a、複数の位相検出部3−1−1,3−1−2,・・・及び基線ベクトル演算部3bを含んで構成される複数の基線ベクトル検出部3−1,3−2,・・・から構成される。
複数の受信要素1−1〜1−3は、それぞれに対応するGPS(Global Positioning System)測位衛星からの航法電波を受信する。アンテナ1は、これら複数の受信要素1−1〜1−3を相互の配置間隔が固定された所定の位置関係となるように配置して構成される。基本的には、受信要素対が一組で構成されたものでもよいが、姿勢角となるピッチ角、ロール角、ヨー角の3軸共に検出するならば、受信要素が一直線上に並ばないように少なくとも2対設ける。また、各受信要素対のうちの一方を位相検出の基準とすることで、合計3個の受信要素で受信要素相互間の受信電波信号の位相を検出することができる。
図示の例では、隣り合う辺の長さがL(m)となるようにアンテナ1内のa,b,c点に受信要素1−1〜1−3をL字形に配置している。また、アンテナ1を移動体に取り付ける際には、地平面に対して水平に移動する移動体において、アンテナ1の受信要素配置面が略天頂を向くように取り付ける。さらに、このとき、アンテナ座標系と移動体の座標系とが一定の位置関係になるように配置する。
例えば、受信要素1−1〜1−3によるL字形の一辺(第1の基線ベクトル)が移動体の前進方向に沿うようにし、受信要素1−1〜1−3によるL字形のもう一方の辺(第2の基線ベクトル)が水平の移動体の進行方向に対して垂直で且つ移動体の左右方向になるように配置する。このようにして、各受信要素1−1〜1−3が天頂に位置する測位衛星あるいは前後方向の測位衛星からの航法電波波長の信号をそれぞれ効率よく受信できる位置に配置する。
なお、このような受信要素1−1〜1−3の配置としたアンテナ1を移動体に搭載すると、天頂の測位衛星と移動体の前後方向の測位衛星との2つの衛星を姿勢検出や測位に使用する場合、受信要素1−1,1−2による第1の基線ベクトルを用いて、移動体のヨー角とピッチ角を精度よく検出できるが、ロール角は検出できない。
水平の移動体の進行方向に対して垂直で且つ移動体の左右方向となる線上に配置した受信要素1−1,1−3による第2の基線ベクトルを用いることで、移動体のヨー角とロール角を検出できるが、ピッチ角は検出できない。従って、両方の基線ベクトル、つまり3つの受信要素を配置して初めて、同時に全ての姿勢角を検出できることになる。反対に、いずれか一つの姿勢角が不要である場合は受信要素を2つ設けるだけでよい。
また、アンテナ1は、例えば板状誘電体の表面に受信要素となる円形導体を1/4波長〜数波長程度のいずれかの所定の間隔に複数個配置した、いわゆるパッチアンテナで構成する。この他、複数のヘリカルアンテナを配置した構成でもよい。さらに、アンテナ1としては、受信要素1個を持つ複数のアンテナを、上述した受信要素の配置関係に従って配置したものでもよい。
さらに、アンテナ1は受信要素をL字形に配置する構成に限定されるものではない。例えば、1つのアンテナ1に対する受信要素数は、上述したようにいずれかの姿勢角を不要とするならば2個であり、全ての姿勢角を得るためには最低限3個必要である。また、4個以上の受信要素を設けることで、各受信要素による検出結果の組み合わせの全てを利用して姿勢角を得ることにより検出精度の向上を図ることができる。この他、各受信要素による検出結果を相互チェックすることにより、各検出結果の異常の有無を監視するように構成してもよい。
衛星方向獲得手段2は、アンテナ1の受信要素又はアンテナ1を搭載した移動体の位置若しくはこれを近似する概略の位置情報から求められる位置と測位衛星の位置とを結ぶ方向を規定する衛星方向情報、つまり当該位置から測位衛星への視線方向を規定する衛星方向情報を、航法電波の受信が予想される測位衛星について求める。
ここで、アンテナ1の受信要素又はアンテナ1を搭載した移動体の位置としては、測位衛星からの航法電波などを受信して求められるアンテナ1の受信要素又はアンテナ1を搭載した移動体の実際の位置を示す情報の他、例えば過去に求めて記憶部に記憶しておいたアンテナ1の受信要素又はアンテナ1を搭載した移動体の位置を、衛星からの電波によるもの以外のジャイロなどにより求められた情報で現在位置に合うように補正した位置を示す情報であってもよい。
アンテナ1の受信要素又はアンテナ1を搭載した移動体の位置を近似する概略の位置情報としては、無線通信基地局との無線通信が可能な領域に対応する位置情報、例えば衛星方向獲得手段2の無線通信部としてセルラ電話などを用意し、セルラ電話の無線通信基地局から入手することができる当該無線通信基地局の無線通信領域(セル)内の無線通信を中継する無線アンテナの位置に相当する情報などがある。また、放送局からの放送受信が可能な領域に対応する位置情報を利用しても良い。
さらに、測位衛星高度に対する位置誤差が許容範囲内となる位置に関する情報であれば、前記概略の位置情報として利用できる。つまり、衛星高度が非常に高く(例えば、20000Km程度)、移動体の実際位置から例えば半径200Km程度の範囲内での位置誤差があっても、移動***置から衛星を見る仰角や方位角の誤差が、tan-1(200/20000)(rad)<1度で低い値となる。従って、測位衛星高度に対する位置誤差が許容範囲内となる位置であれば、固定的に予め用意した地点の位置情報であっても前記概略の位置情報として利用することができる。なお、上述した位置情報については、以降の実施の形態においても説明する。
基線ベクトル検出手段3は、アンテナ1で受信した信号と衛星方向獲得手段2で求めた移動体の位置情報(概略の位置も含む)及び衛星方向情報とに基づいて、少なくとも一対の受信要素の位置ペアでなる基線ベクトルを推定する。姿勢演算手段4では、基線ベクトル検出手段3で推定された少なくとも1つの基線ベクトル情報を基にアンテナ1あるいはアンテナ1を搭載した移動体の姿勢を算出する。
また、衛星方向獲得手段2、基線ベクトル検出手段3及び姿勢演算手段4は、上述したようなアンテナ1や外部との通信機能を備えたコンピュータ装置に対して、本発明に従う移動体姿勢検出プログラムを実行させることで具現化することができる。
もう少し詳細に説明すると、衛星方向獲得手段2、基線ベクトル検出手段3及び姿勢演算手段4の機能を有する各プログラムモジュールから構成される移動体姿勢検出プログラムを上記コンピュータ装置に読み込ませてその動作を制御することにより、上記コンピュータ装置上に図1に示す移動体姿勢検出装置を実現することができる。
なお、以下の説明において、本発明の移動体姿勢検出装置を具現化するコンピュータ自体の構成及びその基本的な機能については、当業者が当該分野の技術常識に基づいて容易に認識できるものであり、本発明の本質に直接関わるものでないので詳細な記載を省略する。
次に動作について説明する。
以降の説明では、例えば北方向をX軸、東方向をY軸、鉛直(下)方向をZ軸とする直交座標系を考え、ローカル座標系と呼ぶこととする。また、移動体が水平でかつ真北を向いた状態をヌル状態(ピッチ角、ロール角、ヨー角ともに0度の状態)とし、このヌル状態からの回転角を移動体の姿勢角とし、これらによって規定される状態を移動体の姿勢とする。また、水平な移動体の進行方向をx軸、水平な移動体の進行方向に垂直で且つ移動体の左右方向に沿った座標軸をy軸、及び水平な移動体の鉛直(下)方向の座標軸をz軸とした直交座標系をアンテナ座標系(若しくは移動体座標系)と呼ぶ。
このように座標系を定義すると、本実施の形態による移動体姿勢検出装置のアンテナ1は、例えば下記のような受信要素の配置となる。
受信要素1−1(基準受信要素)を配置するa点が原点P0であり、受信要素1−2を配置するb点がx軸上の原点P0から距離Lだけ離れた点P1として配置される。ここで、ローカル座標系のZ軸中心に北から東への回転角がヨー角であり、移動体の水平状態から進行方向に沿うx軸を中心に左右への回転角をロール角と呼ぶ。また、進行方向のx軸と直交し、移動体の常態で水平なy軸を中心に水平から前後の回転角をピッチ角と呼ぶ。
各基線ベクトルは、標準的な処理において、ローカル座標系と同様な直交座標系のアンテナ座標系(若しくは移動体座標系)の各軸に一致させる。また、基線ベクトルのいずれかをアンテナ座標系の軸と一致させない場合、後述する姿勢の算出では、基線ベクトルと座標軸との間の固定的な角度オフセットを補正する。
衛星方向獲得手段2は、アンテナ1の受信要素又はアンテナ1を搭載した移動体の位置又はこれを近似する概略の位置からアンテナ1で受信が予想される天頂に位置する測位衛星あるいはその他の複数の測位衛星への、ローカル座標系上における概略の方向を規定する衛星方向情報を求める。
例えば、衛星方向獲得手段2は、後述するような無線通信部あるいは放送受信部を備え、これを介して外部サーバから所定の固定的な位置情報と衛星方向情報を、アンテナ1の受信要素によって受信が予想される測位衛星について受信する。
ここで、外部サーバとは、測位衛星からの電波を受信するレシーバから当該測位衛星についての最新の航法パラメータなどの測位情報を取得し、さらに衛星方向獲得手段2の前記無線通信部あるいは放送受信部と通信可能な領域に対応する位置情報をアンテナ1の受信要素又は移動体の位置を近似する概略の位置情報として当該位置からの衛星方向を衛星方向情報として演算し、衛星方向獲得手段2の前記無線通信部あるいは放送受信部に提供する衛星方向情報提供サーバである。この衛星方向情報提供サーバについての詳細は以降の実施の形態で後述する。
また、衛星方向獲得手段2に、衛星位置情報や衛星方向情報を格納するメモリを設けておき、上記外部サーバから衛星位置情報や衛星方向情報を受信できないような場合、以前に獲得して上記メモリへ記憶しておいた衛星位置情報あるいは衛星位置に関するデフォルトの設定値を用いて現在の衛星方向を算出するように構成しても良い。
基線ベクトル検出手段3は、複数の基線ベクトル検出部3−1,3−2,・・・を含んで構成される。また、基線ベクトル検出部3−1,3−2,・・・は、それぞれ複数の位相検出部を有しており、各位相検出部内にはアンテナ1の受信要素からの入力信号を受信する受信回路が構成される。各位相検出部では、アンテナ1で受信した衛星信号における搬送波位相あるいはコード位相を受信要素ごとに検出する。
衛星選択部3aでは、アンテナ1で衛星信号を受信中の測位衛星数や衛星方向獲得手段2からの衛星方向情報などを用いて、基線ベクトル検出用の衛星選択アルゴリズムに従って、アンテナ1で受信中の測位衛星のうち、基線ベクトル演算部3bによる基線ベクトル算出に利用する測位衛星を選択する。
基線ベクトル検出用の衛星選択アルゴリズムとしては、例えば下記の(1)〜(4)のようなものが考えられる。なお、具体例については後述する。
(1)衛星選択部3aは、アンテナ1で衛星信号を受信中の測位衛星数が2個未満であれば、基線ベクトル演算部3bに基線ベクトルを算出させない。このとき、ジャイロを使った慣性航法INS(Inertial Navigation System)により求めたINS姿勢を姿勢検出結果とする。
(2)衛星選択部3aは、アンテナ1で衛星信号を受信中の測位衛星数が2個であれば、両方を基線ベクトル検出用の衛星として選択する。
(3)衛星選択部3aは、アンテナ1で衛星信号を受信中の測位衛星数が3個以上であれば、直視できる確率が高い天頂に最も近い衛星を優先して選択し、次に天頂に近いもう1機を選択する。このあと、その1機の電波到来方向とは所定角以上離隔しており、且つできるだけ離れた方向に位置するもので、仰角が所定角以上で且つ移動体の前方又は後方に位置する1機を優先的に選択する。このとき、上記条件に合わなければ、アンテナ1で衛星信号を継続して受信した期間が長い衛星を選択する。
(4)衛星選択部3aは、測位衛星の選択条件を決定する数値の差が所定値未満ならば、前回と同じ衛星を選択する。ここで、衛星選択の条件変更はヒステリシスを持たせる。また、衛星信号が受信可能な全てあるいは4つ以上の測位衛星において可能な組み合わせに対応する基線ベクトルを基線ベクトル候補として全て算出させ、ロバスト二乗誤差最小のルールなどに則って最適と思われるベクトル候補を基線ベクトルとして選択・決定してもよい。
また、基線ベクトル演算部3bでは、アンテナ1の受信要素間での受信搬送波の位相差Δφと、衛星方向獲得手段2で求めた衛星位置情報及び衛星方向情報とを基に、ローカル座標系上における、少なくとも1つの基線ベクトル(ベクトル長はLで既知)の推定値を算出する。
ここで、搬送波(キャリア)位相の検出は、例えばBPSK(Binary Phase Shift Keying)変調されたGPS入力信号の場合、公知の技術に従って信号をAD変換し、I(基準位相)成分とQ(90度離相)成分とに分ける。そして、I成分とQ成分のそれぞれについてPLL(Phase Locked Loop)とコスタスループを構成し、これらがロックされた状態において、位相φが、φ=tan-1(Q/I)として算出される。
なお、各位相検出部は、相互に同期をとるために、図示しない受信回路のクロック、レプリカキャリアを共通にしておかれる。レプリカキャリアは、基準とする受信要素に対応する位相検出部の受信回路のキャリア位相ロックループがロックした信号を基に生成される。また、位相検出部間で同期を取らない場合あるいは同期をとりにくい場合は、各位相検出部内の受信回路間あるいは測位衛星間の1重位相差又は2重位相差法により位相差を検出するようにしてもよい。
1重位相差又は2重位相差法により位相差を検出する場合、特に問題となる位相不確定性(フェーズアンビギュイティ)については、例えば特許文献1に開示されるキャリア位相差のアンビギュイティを丸め込みによって整数化して除去する。この他、下記の参考文献に開示される方法を適用してもよい。
(参考文献)特開2002−40124号公報
図2は、基線ベクトル推定原理を説明する図であり、アンテナ1の2つの受信要素で3つの測位衛星からの到来電波を受信した場合を示している。図示の例は、3つの測位衛星からの電波をアンテナ1の2つの受信要素で受けて3つの位相差観測ベクトルD1,D2,D3を得る。そして、各位相差観測ベクトル座標を含み、基線ベクトル方向(衛星方向)と直交する3つの平面1,平面2,平面3を考え、これらのいずれにも基線ベクトル座標が含まれ得ることを利用して、3平面の各交点をベクトル先端とするベクトルを移動体の姿勢測定に用いる基線ベクトルRとして推定する。
なお、アンテナ1の受信要素間は、上述したように距離Lだけ離して配置する。また、基線ベクトルRとは、第1アンテナ(図示の例では点P0に位置する受信要素)の位置P0を基準とする第2アンテナ(例えば、点P0に位置する受信要素を基準とする図1で示した点P1に位置する受信要素)の位置P1までを結ぶ直線の長さと方向を表すベクトルである。なお、基線ベクトルRの長さ|R|は、アンテナ1の受信要素間を距離Lだけ離して配置していることから|R|=Lとなる。
位相差観測ベクトルD1,D2,D3は、第1アンテナの位置P0から平面1,平面2,平面3を構成する各円の中心P01,P02,P03までを結ぶ直線の長さと方向を表すベクトルであり、アンテナ1の2つの受信要素で3つの測位衛星から受信した到来電波の各位相差の観測量d1,d2,d3及び到来電波の送信源となる3つの測位衛星への各視線方向の方向余弦を用いて算出する。平面1,平面2,平面3は、位相差観測ベクトルD1,D2,D3がそれぞれ直交する平面であって、第1アンテナの位置P0を基準とした基線ベクトルRの先端が存在する可能性がある円形領域の点群として定義される。
移動体に固定したアンテナ1の2つの受信要素で3つの測位衛星からの到来電波を受信し、基線ベクトル検出手段3内の位相検出部によって各受信要素の受信電波の位相差d1(第1の衛星分)、d2(第2の衛星分)、d3(第3の衛星分)を観測する。この観測量d1,d2,d3を用いて、後述するベクトル計算により第2アンテナの位置(図1における受信要素1−2が位置する点P2)にベクトル先端を有するベクトルである基線ベクトルRを求める。なお、観測量d1,d2,d3には波数整数アンビギュイティを含むので、後述するようにアンビギュイティを決定する必要がある。
第1アンテナは、移動体のアンテナ1内における任意の基準位置に配置され、第2アンテナは第1アンテナから移動体の進行方向にLだけ離して、且つ同じ高さの位置となるように移動体のアンテナ1内に固定される。図2に示す例では、測位衛星からの到来電波に基づいて各位相差観測ベクトルを求め、これら位相差観測ベクトルから規定されるローカル座標系における第2アンテナの位置を算出することにより、移動体の姿勢や位置の検出に用いる基線ベクトルRを推定するものである。
図2を用いて、3つの測位衛星からの到来電波をアンテナ1の2つの受信要素で受信した場合における基線ベクトル検出処理について説明する。
基線ベクトルRの先端座標位置である座標値(x,y,z)は、図中に示すように平面1、平面2及び平面3のいずれの平面にも含まれる。これを利用して、下記式(1)〜(4)に従い位相差観測ベクトルD1,D2,D3から平面1,平面2,平面3を求める。これにより求めた平面1,平面2,平面3の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用するベクトル候補とする。なお、( )・( )はベクトルの内積である。
また、位相差観測ベクトルD1,D2,D3は、到来電波源である第1〜第3の測位衛星への視線方向の各方向余弦(hx1,hy1,hz1)、(hx2,hy2,hz2)、(hx3,hy3,hz3)を用い、下記式(5)〜(7)に従って算出する。
但し、d1,d2,d3は、位相検出部によって検出された到来電波の各受信要素についての位相差の観測量dd1〜dd3に波数整数アンビギュイティN1,N2,N3をそれぞれ加えた量である。ここで、Lは、基線ベクトルRの長さ|R|であり、λは測位衛星からの到来して位相検出部により位相が観測される到来電波の波長である。また、整数アンビギュイティN1,N2,N3は、−m〜+mの範囲内の値をとる。なお、mはアンテナ1内の受信要素の設置間隔Lを第1,第2,第3の衛星からの搬送波の波長λでそれぞれ除算した値の整数部(m=mod(L/λ))である。
各位相差の観測量dd1〜dd3は、例えば下記のようにして求める。
基線ベクトル演算部3bは、第1の衛星からの到来電波について、第1アンテナである受信要素1−1の位置P0で観測された位相φ0と、第2アンテナである受信要素1−2の位置P1で観測された位相φ1とから位相差Δφ1(=φ0−φ1)を求める。基線ベクトル演算部3bは、位相差Δφ1を基に、dd1=λ・Δφ1/(2π)なる式に従って距離dd1を算出する。同様にして、位相差Δφ2,Δφ3から距離dd2,dd3を算出する。λはGPS測位衛星からの到来電波の波長である。
なお、位相不確定性(フェーズアンビギュイティ)については、例えば搬送波位相観測データに対して観測残差和Iが最小となる、最も確からしいアンビギュイティの組み合わせを推定するように構成してもよい。
観測残差和Iを用いたアンビギュイティの推定処理について説明する。
下記式(8)は、観測残差和Iの評価関数である。
ここで、添え字iは観測対象の電波を送信した衛星を特定する番号であり、1から衛星選択部3aにより選択された衛星数nまでの自然数をとる。jはベースライン番号であり、1から基線数mまでの自然数をとる。また、Δφiは搬送波位相差(1重差)観測量、k
iはアンビギュイティである。さらに、r
jはj番目の基線ベクトル、s
iは衛星への視線方向の単位ベクトル、b
jはj番目のベースラインのラインバイアス(既知)である。さらに、λは衛星電波である搬送波の波長であり、1575.42MHzのL1波の波長(約19cm)又は1227.60MHzのL2波の波長(約24cm)である。
(1)基線ベクトル演算部3bは、アンビギュイティk
iの推定に使用する衛星の組み合わせを決定する。
(2)各基線ベクトルについて基線長Lに相当する±n波長分程度の範囲でアンビギュイティk
iのサーチを行う。この段階では、評価関数のみで解を絞り込む。
(3)続いて、ベースライン長やベースラインの幾何学的関係、移動体の姿勢、可視衛星方向やアンテナ視線との関係を考慮して、妥当な解を残し、あり得ない解を候補から除外する。
(4)次に、基線ベクトル演算部3bは、残った解の候補について、例えば10秒程度の所定期間内で得られる到来電波の情報を用いて上記式(8)を積算し、積算値が最小のものを正解として抽出する。
このようにして、基線ベクトル演算部3bが最も確からしいアンビギュイティの組み合わせを推定する。
また、(hx1,hy1,hz1)T〜(hx3,hy3,hz3)Tは、衛星方向獲得手段2が衛星方向情報として獲得した第1〜3の測位衛星への各視線方向の方向余弦である。これら方向余弦(hx1,hy1,hz1)〜(hx3,hy3,hz3)は、例えば世界座標系(ECEF(Earth Centered, Earth Fixed)座標系など)における移動体(アンテナ1)の概略位置に基づく概略位置ベクトル(xbs,ybs,zbs)と第1〜3の衛星についての各位置ベクトル(xs1,ys1,zs1)とを用いて下記式(9)〜(17)に従って算出される。
hx1=(xs1−xbs)/{(xs1−xbs)2
+(ys1−ybs)2+(zs1−zbs)2}1/2 (9)
hy1=(ys1−ybs)/{(xs1−xbs)2
+(ys1−ybs)2+(zs1−zbs)2}1/2 (10)
hz1=(zs1−zbs)/{(xs1−xbs)2
+(ys1−ybs)2+(zs1−zbs)2}1/2 (11)
hx2=(xs2−xbs)/{(xs2−xbs)2
+(ys2−ybs)2+(zs2−zbs)2}1/2 (12)
hy2=(ys2−ybs)/{(xs2−xbs)2
+(ys2−ybs)2+(zs2−zbs)2}1/2 (13)
hz2=(zs2−zbs)/{(xs2−xbs)2
+(ys2−ybs)2+(zs2−zbs)2}1/2 (14)
hx3=(xs3−xbs)/{(xs3−xbs)2
+(ys3−ybs)2+(zs3−zbs)2}1/2 (15)
hy3=(ys3−ybs)/{(xs3−xbs)2
+(ys3−ybs)2+(zs3−zbs)2}1/2 (16)
hz3=(zs3−zbs)/{(xs3−xbs)2
+(ys3−ybs)2+(zs3−zbs)2}1/2 (17)
移動体の姿勢検出に利用する基線ベクトルRは、世界座標系を移動体の概略位置ベクトル分だけ平行移動し、第1アンテナの位置をローカル座標系の原点P0(0,0,0)に対応させ、第2アンテナの位置P1が座標値(x,y,z)となるベクトルである。
次に、平面1,平面2,平面3の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用する基線ベクトルとして算出する処理について説明する。
先ず、位相差観測ベクトルD1,D2,D3と平面1,平面2,平面3との交点、即ち各円の中心の座標をそれぞれP01(a,b,c)、P02(d,e,f)、P03(g,h,i)とした場合、上記式(1)〜(3)は下記式(18)〜(20)のようにそれぞれ表される。
上記式(18)〜(20)を整理すると、下記式(21)〜(23)のようになる。
ax+by+cz−(a2+b2+c2)=0 (21)
dx+ey+fz−(d2+e2+f2)=0 (22)
gx+hy+iz−(g2+h2+i2)=0 (23)
上記式(21),(22)を用いてzを消去すると、変数はxだけでyを表す下記式(24)〜(26)を得る。
y=Ax+B (24)
A={af/(c−d)}/(e−bf/c) (25)
B=(d2+e2−a2f/c−b2f/c−cf)/(e−bf/c)
={d2+e2−f(a2+b2+c2)/c}/(e−bf/c) (26)
同様に上記式(21),(22)を用いてyを消去すると、変数はxだけでzを表す下記式(27)〜(29)を得る。
z=Cx+D (27)
C={ae/(b−d)}/(f−ce/b) (28)
D=(d2+f2−a2e/b−c2e/b−be)/(f−ce/b)
={d2+f2−e(a2+c2+b2)/b}/(f−ce/b) (29)
上記式(24),(27)を上記式(23)に代入し、変数xだけの1次方程式を求め、これを整理して下記式(30)を得る。
x={(g2+h2+i2)−(hB+iD)}/(g+hA+iC) (30)
上記式(30)のxの値を上記式(24),(27)に代入して、y及びzの値を求めることで、平面1,平面2,平面3の各交点にベクトル先端を有する基線ベクトルR(x,y,z)が算出される。なお、上述した基線ベクトルの演算は、位相検出部3−1−1、3−1−2からの受信電波に関する情報及び衛星方向獲得手段2からの当該電波を送信した測位衛星に関する情報を用いて基線ベクトル演算部3bによりなされる。
上述のようにして、平面1,平面2,平面3の各交点にベクトル先端を有する基線ベクトルR(x,y,z)が算出されると、姿勢演算手段4が、下記式(31),(32)に従って姿勢角であるヨー角やピッチ角を算出する。
ヨー角=acot(x/y) (31)
ピッチ角=acos{(x2+y2)1/2/(x2+y2+z2)1/2} (32)
なお、事前に正確に計測した移動体(アンテナ)姿勢と測位衛星に相当する電波源とを用いて位相差を予備実験的に求め、この位相差についての誤差を計測し、テーブルとして本実施の形態による移動体姿勢検出装置内のメモリに記憶しておき、実使用の際、基線ベクトル検出手段3や姿勢演算手段4が、基線ベクトルや移動体姿勢を算出するとき、メモリから読み出した上記位相差誤差を移動体の観測量から差し引いたもの使用するように構成してもよい。
このようにすることで、アンテナ1内の受信要素間の距離を小さく配置する場合に顕著となる受信要素相互間の電波干渉の影響を減らすことができ、移動体の姿勢や位置の検出精度を高めることができる。この場合、一台一台について行うのではなく、複数の計測を一度に行い、統計的な平均値を得てその後に生産する全ての装置にその平均値テーブルを記憶させて利用するようにしてもよい。このようにすれば、個々について誤差計測を全て行う必要がない。
次に、2つの衛星からの到来電波をアンテナ1の3つの受信要素で受信した場合における基線ベクトル検出処理について説明する。
図3は、基線ベクトル推定原理を説明する図であり、アンテナ1の3つの受信要素で2つの測位衛星からの到来電波を受信した場合を示している。図示の例は、2つの測位衛星からの電波をアンテナ1の3つの受信要素で受けて、基準となる受信要素1−1(以降、第1アンテナと称する)と他の受信要素1−2(以降、第2アンテナと称する)若しくは受信要素1−3(以降、第3アンテナと称する)とのそれぞれの組み合わせから位相差観測ベクトルを求める。
そして、各位相差観測ベクトル座標を含み、基線ベクトル方向(衛星方向)と直交する平面と基線ベクトルの長さを半径とし第1アンテナ位置P0を中心とする球面とを考え、これらのいずれにも基線ベクトル座標が含まれることを利用して、これらの交点にベクトル先端を有するベクトルを基線ベクトル候補として抽出する。続いて、ベクトル候補を構成する各ベクトル間の角度αが、基準の受信要素と他の各受信要素をそれぞれ結ぶ基線ベクトル間の角度Φとして許容される値の範囲に最も近い方を、移動体の姿勢や位置の測定に用いる基線ベクトルの対として推定する。
図において、位相差観測ベクトルD1,D2は、第1アンテナの位置P0から平面1,平面2を構成する各円の中心P01,P02までを結ぶ直線の長さと方向を表すベクトルであり、第1アンテナと第2アンテナとで2つの衛星から受信した到来電波の各位相差の観測量d1,d2及び到来電波の送信源となる2つの測位衛星への視線方向の方向余弦を用いて算出する。
また、位相差観測ベクトルD1’,D2’も同様に、第1アンテナの位置P0から平面1’,平面2’を構成する各円の中心P01’,P02’までを結ぶ直線の長さと方向を表すベクトルであり、第1アンテナと第3アンテナとで2つのGPS測位衛星から受信した到来電波の各位相差の観測量d1’,d2’及び到来電波の送信源となる2つの測位衛星への視線方向の方向余弦を用いて算出する。
平面1,平面2は、位相差観測ベクトルD1,D2に直交する平面であって、第1アンテナの位置P0を基準とした基線ベクトルの先端が存在する可能性がある円形領域の点群として定義される。平面1’,平面2’も同様に、位相差観測ベクトルD1’,D2’に直交する平面であって、第1アンテナの位置P0を基準とした基線ベクトルの先端が存在する可能性がある円形領域の点群として定義される。
移動体に固定したアンテナ1のうち、第1アンテナと第2アンテナとで2つの測位衛星からの到来電波をそれぞれ受信し、基線ベクトル検出手段3内の位相検出部によって各受信要素の受信電波の位相差d1(第1の衛星分)、d2(第2の衛星分)を観測し、位相差観測ベクトルD1,D2を得る。なお、観測量d1,d2には波数整数アンビギュイティを含むので、上述したようにアンビギュイティを決定する必要がある。
第1アンテナから第2アンテナに向かう基線ベクトルRは、位相差観測ベクトルD1,D2にそれぞれ直交する平面1及び平面2と、半径が基線ベクトルRの長さLaで原点P0(第1アンテナの位置)を中心とする球面とのいずれにもその先端が存在する可能性がある。従って、基線ベクトルRは、平面1及び平面2と、半径が基線ベクトルRの長さLaで原点P0を中心とする球面との交点(円形領域の平面1,2の2円の交点に球面が重なる点となり2点存在する)にそれぞれベクトル先端が位置する。
一方、移動体に固定したアンテナ1のうち、第1アンテナと第3アンテナで2つの測位衛星からの到来電波をそれぞれ受信し、基線ベクトル検出手段3の位相検出部によって各受信要素の受信電波の位相差d1’(第1の衛星分)、d2’(第2の衛星分)を観測し、位相差観測ベクトルD1’,D2’を得る。なお、観測量d1’,d2’には波数整数アンビギュイティを含むので、上述したようにアンビギュイティを決定する必要がある。
第1アンテナから第3アンテナに向かう基線ベクトルR’は、基線ベクトルRと同様に位相差観測ベクトルD1’,D2’にそれぞれ直交する平面1’及び平面2’と、半径が基線ベクトルR’の長さLbで原点P0(第1アンテナの位置)を中心とする球面とのいずれにもその先端が存在する可能性がある。従って、基線ベクトルR’は、平面1’及び平面2’と、半径が基線ベクトルR’の長さLbで原点P0を中心とする球面との交点(円形領域の平面1’,2’の2円の交点に球面が重なる点となり2点存在する)にそれぞれベクトル先端が位置する。
第1アンテナは、移動体のアンテナ1内における任意の基準位置に配置され、第2アンテナ及び第3アンテナは第1アンテナから移動体の進行方向にLa,Lbだけそれぞれ離して、且つ同じ高さの位置となるように移動体のアンテナ1内に固定する。このように、図3に示す例では、測位衛星からの到来電波に基づいて各位相差観測ベクトルを求め、これら位相差観測ベクトルから規定されるローカル座標系における第2アンテナや第3アンテナの位置を算出することにより、移動体の姿勢や位置の測定に用いる基線ベクトルを推定するものである。
図3を用いてより詳細に説明する。
先ず、2つの衛星からの到来電波を第1アンテナと第2アンテナで受信した場合における基線ベクトル検出処理について説明する。
第1アンテナから第2アンテナへ向かう基線ベクトルRは、図中に示すように平面1、平面2及び半径Laで原点P0を中心とする球面のいずれにおいても先端座標位置である座標値(x,y,z)(=(x1,y1,z1)、(x2,y2,z2))が含まれる。これを利用して、上記式(1),(2),(4)に従い位相差観測ベクトルD1,D2と基線ベクトルRとから平面1,平面2を求める。但し、式(4)については、LをLaに置き換える。これにより求めた平面1、平面2、及び半径Laで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用するベクトル候補とする。
詳細に、平面1、平面2、及び半径Laで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用する基線ベクトル候補として算出する処理について説明する。
先ず、位相差観測ベクトルD1,D2と平面1,平面2との交点、即ち各円の中心の座標をそれぞれP01(a,b,c)、P02(d,e,f)とした場合、上記式(1),(2)は上記式(18),(19)のようにそれぞれ表される。上記式(18),(19)を整理すると、上記式(21),(22)のようになる。
上記式(21),(22)を用いてzを消去すると、変数はxだけでyを表す上記式(24)〜(26)を得る。同様に上記式(21),(22)を用いてyを消去すると、変数はxだけでzを表す上記式(27)〜(29)を得る。
ここで、上記式(24),(27)を上記式(4)に代入し、変数xだけの2次方程式を求め、これを整理して下記式(33)を得る。
(1+A2+C2)x2+2(AB+CD)x+B2+D2−La2=0 (33)
上記式(33)を解くと、下記式(34)のようになる。
x=[−2(AB+CD)±{(2AB+2CD)2
−4(1+A2+C2)(B2+D2−La2)}1/2]
/{2(1+A2+C2)} (34)
上記式(34)による2つのxの値x1,x2を上記式(24),(27)に代入して、y1,y2及びz1,z2の値を求めることで、平面1、平面2、及び半径Laで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトルR1(x1,y1,z1)、R2(x2,y2,z2)からなる基線ベクトル候補対が算出される。なお、上述した基線ベクトルの演算は、位相検出部3−1−1,3−1−2からの受信電波に関する情報及び衛星方向獲得手段2からの当該電波を送信した測位衛星の方向に関する情報(測位衛星への視線方向の方向余弦などの衛星方向情報)を用いて基線ベクトル演算部3bによりなされる。
次に、2つの衛星からの到来電波を第1アンテナと第3アンテナで受信した場合における基線ベクトル検出処理について説明する。
第1アンテナから第3アンテナへ向かう基線ベクトルR’は、平面1’、平面2’及び半径Lbで原点P0を中心とする球面のいずれにおいても先端座標位置である座標値(x,y,z)(=(x1’,y1’,z1’)、(x2’,y2’,z2’))が含まれる。これを利用して、上記式(1),(2),(4)に従い、位相差観測ベクトルD1’,D2’と基線ベクトルR’とから平面1’,平面2’を求める。但し、式(1),(2)についてはRをR’に置き換え、式(4)についてはLをLbに置き換える。これにより求めた平面1’、平面2’、及び半径Lbで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用するベクトル候補とする。
詳細に、平面1’、平面2’、及び半径Lbで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用する基線ベクトル候補として算出する処理について説明する。
先ず、位相差観測ベクトルD1’,D2’と平面1’,平面2’との交点、即ち各円の中心の座標をそれぞれP01’(a,b,c)、P02’(d,e,f)とした場合、上記式(1),(2)は上記式(18),(19)のようにそれぞれ表される。上記式(18),(19)を整理すると、上記式(21),(22)のようになる。
上記式(21),(22)を用いてzを消去すると、変数はxだけでyを表す上記式(24)〜(26)を得る。同様に上記式(21),(22)を用いてyを消去すると、変数はxだけでzを表す上記式(27)〜(29)を得る。
ここで、上記式(24),(27)を上記式(4)に代入し、変数xだけの2次方程式を求め、これを整理して下記式(35)を得る。
(1+A2+C2)x2+2(AB+CD)x+B2+D2−Lb2=0 (35)
上記式(35)を解くと、下記式(36)のようになる。
x=[−2(AB+CD)±{(2AB+2CD)2
−4(1+A2+C2)(B2+D2−Lb2)}1/2]
/{2(1+A2+C2)} (36)
上記式(36)による2つのxの値x1’,x2’を上記式(24),(27)に代入して、y1’,y2’及びz1’,z2’の値を求めることで、平面1’、平面2’、及び半径Lbで原点P0を中心とする球面の各交点にベクトル先端をそれぞれ有する基線ベクトルR1’(x1’,y1’,z1’)、R2’(x2’,y2’,z2’)からなる基線ベクトル候補対が算出される。なお、上述した基線ベクトルの演算は、位相検出部3−1−1,3−1−3からの受信電波に関する情報及び衛星方向獲得手段2からの当該電波を送信した測位衛星の方向に関する情報(測位衛星への各視線方向の方向余弦などの衛星方向情報)を用いて基線ベクトル演算部3bによりなされる。
次に、基線ベクトル演算部3bは、上述のようにして求めた基線ベクトル候補対を構成する各基線ベクトル同士間の角度αを求め、第1アンテナから第2アンテナに向かう基線ベクトルと第1アンテナから第3アンテナに向かう基線ベクトルとがなす角が一定値Φであることを利用して、上記基線ベクトル候補対のうち角度αが角度Φに近い値(例えば、角度の許容差値を1度とするなど)が得られた方を、移動体の姿勢や位置検出に使用する真の候補対であると推定する。
具体的に説明すると、基線ベクトル候補対を構成する各基線ベクトル同士間の角度αは、第1アンテナから第2アンテナに向かう基線ベクトルRのベクトル候補対を構成する基線ベクトルをそれぞれR1,R2とし、第1アンテナから第3アンテナに向かう基線ベクトルR’のベクトル候補対を構成する基線ベクトルをそれぞれR1’,R2’とすると、例えば下記式(37)〜(40)から求めることができる。
α=acos[{(R1)・(R2)}/(|R1|*|R2|)] (37)
α=acos[{(R1’)・(R2)}/(|R1’|*|R2|)] (38)
α=acos[{(R1)・(R2’)}/(|R1|*|R2’|)] (39)
α=acos[{(R1’)・(R2’)}/(|R1’|*|R2’|)](40)
但し、( )・( )はベクトルの内積、| |*| |はスカラー積を示している。
基線ベクトル演算部3bは、上述のようにして4つの基線ベクトル候補対を構成する各基線ベクトル同士間の角度αを求めると、下記式(41)に示す基線ベクトル間角度の許容範囲を満たすか否かを判定し、角度Φに近い値が得られた候補対を、移動体の姿勢や位置検出に使用する真の候補対であると推定する。
|Φ−γ|<|α|<|Φ+γ| (41)
ここで、γは角度αの角度Φからの誤差の許容値である。
上述のようにして求められた基線ベクトル候補対における、一方の基線ベクトルの先端位置が第2アンテナの位置に相当し、他方の基線ベクトルの先端位置が第3アンテナの位置に相当する。これら基線ベクトルR(x,y,z)を用いて、姿勢演算手段4が、上記式(31),(32)に従い姿勢角であるヨー角やピッチ角を算出する。
なお、事前に正確に計測した移動体(アンテナ)姿勢と測位衛星に相当する電波源とを用いて位相差を予備実験的に求め、この位相差についての誤差を計測し、テーブルとして本実施の形態による移動体姿勢検出装置内のメモリに記憶しておき、実使用の際、基線ベクトル検出手段3や姿勢演算手段4が、基線ベクトルや移動体姿勢を算出するとき、メモリから読み出した上記位相差誤差を移動体の観測量から差し引いたもの使用するように構成してもよい。
このようにすることで、アンテナ1内の受信要素間の距離を小さく配置する場合に顕著となる受信要素相互間の電波干渉の影響を減らすことができ、移動体の姿勢や位置の検出精度を高めることができる。この場合、一台一台について行うのではなく、複数の計測を一度に行い、統計的な平均値を得てその後に生産する全ての装置にその平均値テーブルを記憶させて利用するようにしてもよい。このようにすれば、個々について誤差計測を全て行う必要がない。
次に、2つの測位衛星からの到来電波をアンテナ1内の2つの受信要素でそれぞれ受信した場合における基線ベクトル検出処理について説明する。
例えば、2つの測位衛星からの到来電波を第1アンテナと第2アンテナで受信した場合、第1アンテナから第2アンテナへ向かう基線ベクトルRは、平面1、平面2及び半径Lで原点P0を中心とする球面のいずれにおいても先端座標値(x,y,z)(=R1(x1,y1,z1)、R2(x2,y2,z2))が含まれる。
そこで、上述した2つの測位衛星からの到来電波をアンテナ1内の3つの受信要素で受信した場合における基線ベクトル検出処理と同様にして、上記式(1),(2),(4)に従い位相差観測ベクトルD1,D2から平面1,平面2を求め、平面1、平面2及び半径Lで原点P0を中心とする球面の各交点にベクトル先端を有する基線ベクトル候補対を求める。
このようにして求めた基線ベクトル候補対のうちのいずれの基線ベクトルを、移動体の姿勢や位置検出に用いる基線ベクトルとして用いるか否かは、例えば基線ベクトル候補対の基線ベクトルのピッチ角と、本実施の形態による移動体姿勢検出装置に搭載した角速度センサによる移動体の旋回角βとの差を、上記と同様にして評価して許容される値に近いものと判定された基線ベクトルを移動体の姿勢や位置検出のために選択する。
なお、上述した基線ベクトルの検出処理では、受信要素を少なくとも2つ有するパッチアンテナのようなアンテナ1で測位衛星からの到来電波を受信する例を示したが、本発明はこれに限られない。例えば、アンテナ1をアレイアンテナで構成し、基線ベクトル検出手段3が、アレイアンテナ座標系における電波の到来角θを、MUSIC(MUltiple SIgnal Classification)処理方式を用いて検出し、この検出結果の到来角θに基づいて基線ベクトルを推定するように構成してもよい。
姿勢演算手段4では、上述のようにして基線ベクトル検出手段3で検出された基線ベクトルと、衛星方向獲得手段2で獲得した上記基線ベクトルに関わるGPS測位衛星のローカル座標系上の衛星方向情報とに基づいて、アンテナ1あるいはこれを搭載した移動体のローカル座標系上での姿勢(ヨー角、ピッチ角、ロール角)を算出する。
この際、例えば特許文献1に開示されている方法を採用しても良い。この場合、特許文献1中の式(1)にローカル座標系の基線ベクトルの観測量とアンテナ座標系における基線ベクトルの既知の値とを代入して、特許文献1中の式(2)に示された座標変換オペレータ行列を求める。そして、この行列の要素を用いて、ロール角、ピッチ角、ヨー角(真北からの方位角)を算出するための特許文献1中の式(3)〜式(5)に従いローカル座標上での姿勢を算出する。このようにして求められた移動体の姿勢に関する情報は、姿勢演算手段4から装置外部へと出力される。
以上のように、この実施の形態1によれば、移動体上に設けられ、測位衛星からの航法電波を受信する複数の受信要素1−1〜1−3を配置してなるアンテナ1と、アンテナ1の受信要素又は移動体の位置若しくはこれを近似する概略の位置に関する情報に基づき求められる位置から測位衛星への視線方向を規定する衛星方向情報を求める衛星方向獲得手段2と、少なくとも2つの測位衛星から受信した航法電波について受信要素における位相差をそれぞれ求め、これら位相差及び衛星方向情報に基づいて、測位衛星からの航法電波を受信している少なくとも一対の受信要素の組み合わせの基線ベクトルを検出する基線ベクトル検出手段3と、基線ベクトル検出手段が検出した基線ベクトルにより規定される姿勢を移動体の姿勢として算出する姿勢演算手段4とを備えるので、天頂に近い2つ程度の少ない数の測位衛星からしか電波が届かないような場所であっても、アンテナ1の受信要素間の基線ベクトルを決定するにあたり、測位衛星高度を考慮すると移動***置の誤差が許容できる範囲になることから、2つ程度の少ない数の測位衛星から求められるような精度の低い移動体の位置や当該移動体側と通信可能な位置にある無線通信基地局や放送局などから提供される移動体近隣の位置などの概略の位置を用いて基線ベクトルを決定することができる。これにより、天頂に近い2つ程度の少ない数の測位衛星からしか電波が届かないような場所であっても、移動体の絶対的な姿勢を電子的且つリアルタイムに検出することができる。
なお、上記実施の形態1において、基線ベクトル検出手段3が、天頂に位置する測位衛星(天頂衛星)からの到来電波の受信状態の良悪を検出・判定する監視部を装備し、当該監視部による判定で受信状態が良い場合には天頂に位置する測位衛星を利用し、悪い場合には次に天頂に近い他の測位衛星からの電波を利用するように構成してもよい。
例えば、監視部が、天頂衛星からの電波受信状態として受信信号のS/N比を監視し、S/N比が悪いときには、衛星選択部3aにより次善の衛星に自動的に切替えるように制御する。このようにすることで、姿勢検出のアベイラビリティを向上できる。天頂衛星は、ほぼ真上にあるといっても、車輌等の移動体が移動するところには高架道、電力架線、歩道橋といった電波遮断要因が数多く存在する。これに対して、上述のような構成とすることで、上記のような環境下で姿勢検出ができなくなることを防ぐことができる。
また、アンテナ1における受信要素の配置、つまり既知の基線ベクトルのいずれかをアンテナ座標系の軸と一致させない場合、後述するような姿勢算出では、基線ベクトルと座標軸との間の固定的な角度オフセットが差し引かれ補正される。
例えば、姿勢演算手段4が、アンテナ1を搭載した移動体の傾斜角度を±90度未満の所定値以下に限定して算出するように構成する。これによって、アンテナ1を取り付けた移動体の姿勢を算出する計算量が少なくとも半減する効果がある。つまり、移動体は概ね水平な状態で挙動する性質に着目し、常態からの傾斜角が90度以下の一定限度内に収まる制限を与える。このようにすることで、±90未満の所定の値を超える姿勢角については計算を打ち切ることができ、計算完了周期を短縮することができる。これにより、過度応答性や実時間性のよい姿勢検出結果を得ることができる。
また、姿勢演算手段4が、アンテナ1を搭載した移動体の前後方向について演算する最大傾斜角度を例えば±10度から±50度までのいずれかの値に限定し、左右方向について演算する最大傾斜角度は例えば±20度から±50度までのいずれかの値に限定した範囲について傾斜を算出するようにしてもよい。このようにすることで、アンテナ1を搭載した移動体の傾斜角度を算出する計算量をさらに減少させることができる。従って、計算完了周期をより短縮化することができるので、さらなる過度応答性や実時間性のよい姿勢検出を実行することができる。
実施の形態2.
図4は、この発明の実施の形態2による移動体姿勢検出装置の構成を示す図である。本実施の形態による移動体姿勢検出装置は、アンテナ1、衛星方向獲得手段2、基線ベクトル検出手段3、姿勢演算手段4、角速度検出手段5、INS姿勢演算手段6、速度検出手段7、位置演算手段8及び提示/制御手段9を含んで構成される。なお、図1と同一の符号を付した構成要素は同一若しくはこれに相当するものであり、これらについての重複する説明は省略する。
角速度検出手段5は、レートジャイロなどの角速度センサなどから構成され、移動体のヨー軸、ピッチ軸及びロール軸のうちの少なくとも1つの軸周りの角速度を検出する。INS姿勢演算手段6は、角速度検出手段5が検出した角速度と姿勢演算手段4が算出した移動体の絶対姿勢とを基にして当該移動体の姿勢を算出する。速度検出手段7は、速度センサなどにより構成され、移動体の移動速度を検出する。
位置演算手段8は、INS姿勢演算手段6が算出した姿勢情報と速度検出手段7が算出した移動体の速度とに基づいて移動体の位置を算出する。提示/制御手段9は、本実施の形態による移動体姿勢検出装置に搭載したLCDなどの表示装置(不図示)を介して、INS姿勢演算手段6が算出した姿勢情報や位置演算手段8が算出した移動体の位置情報をユーザに提示する。
図5は、図4中のINS姿勢演算手段の構成を示す図である。図に示すように、INS姿勢演算手段6は、ジャイロ10からの角速度情報を入力するA/D変換器11、固定オフセット補償部12、演算器13,14,17、積分器15及び補償演算部16を含んで構成される。ジャイロ10は、図4中の角速度検出手段5を構成するレートジャイロであり、移動体の3軸のうちの少なくとも1つの軸周りの角速度を検出する。
A/D変換器11は、ジャイロ10からのアナログ信号の角速度情報を入力してディジタル信号に変換する。固定オフセット補償部12は、移動体の3軸のうちの少なくとも1つの軸周りの角速度に関する所定のオフセット値が設定されており、演算器13に上記角速度の検出値が入力されると当該所定のオフセット値を演算器13に出力する。演算器13,14,17は、入力したディジタル信号を適宜加減算する。積分器15は、入力信号の積分値を算出する。補償演算部16は、姿勢演算手段4からの姿勢情報と積分器からの出力情報との演算結果を入力してINS姿勢の補償演算を実行する。
また、図4中に示した衛星方向獲得手段2、基線ベクトル検出手段3、姿勢演算手段4、INS姿勢演算手段6、位置演算手段8及び提示/制御手段9は、アンテナ1や外部との通信機能を備えたコンピュータ装置に対して、本発明に従う移動体姿勢検出プログラムを実行させることで具現化することができる。
もう少し詳細に説明すると、衛星方向獲得手段2、基線ベクトル検出手段3、姿勢演算手段4、INS姿勢演算手段6、位置演算手段8及び提示/制御手段9の機能を有する各プログラムモジュールから構成される移動体姿勢検出プログラムを上記コンピュータ装置に読み込ませてその動作を制御することにより、上記コンピュータ装置上に図4に示す移動体姿勢検出装置を実現することができる。
なお、以下の説明において、本発明の移動体姿勢検出装置を具現化するコンピュータ自体の構成及びその基本的な機能については、当業者が当該分野の技術常識に基づいて容易に認識できるものであり、本発明の本質に直接関わるものでないので詳細な記載を省略する。
次に動作について説明する。
図6は、実施の形態2による移動体姿勢検出装置の動作を示すフローチャートであり、この図に沿って詳細な動作を説明する。
先ず、本実施の形態による移動体姿勢検出装置は、アンテナ1の受信要素を介して測位衛星からの到来電波の受信を開始する。受信要素により受信された測位衛星からの到来電波は、基線ベクトル検出手段3の基線ベクトル検出部3−1,3−2,・・・に入力されて、内部の位相検出部3−1−1,3−1−2,3−1−3,・・・により位相検出される。ここまでの処理がステップST1に相当する。
衛星方向獲得手段2は、受信した到来電波を送信した測位衛星の方向に関する情報(衛星方向情報)と、移動体の位置に関する情報(概略の位置情報)とを取得する(ステップST2)。例えば、図7及び図8に示すようなフローチャートに従って衛星方向情報や概略位置情報を取得する。
図7を用いて、衛星方向情報の取得処理について説明する。
図7(a)に示す例における衛星方向獲得手段2は、外部装置との間で無線通信を実行するための無線通信部あるいは放送受信部を備えている。ステップST7a−1において、衛星方向獲得手段2は、アンテナ1の受信要素が到来電波を受信して位相検出部によりその位相が検出されると、当該受信電波に関して衛星方向情報を更新すべきか否かを判定する。
このとき、衛星方向情報を更新すべきと判断すると、衛星方向獲得手段2は、無線通信部あるいは放送受信部によって外部装置から衛星方向情報を無線通信や放送で受信して以前の衛星方向情報を自動更新する(ステップST7a−2)。外部装置としては、例えば測位衛星からの航法電波を受信して測位情報を取得し、これを基に衛星位置及び移動体の概略の位置情報により求まる位置から衛星への衛星方向情報を求めて提供するセンタサーバなどが考えられる。
また、放送としてセンタサーバから衛星方向情報を受信する場合、例えば放送受信部が測位衛星を経由して当該放送中の衛星方向情報を受信する場合もある。一方、衛星方向情報を更新しないと判定されると、衛星方向情報の更新可否の判定処理に戻る。
図7(b)に示す例では、衛星方向獲得手段2が、図7(a)と同様に、外部装置との間で無線通信を実行するための無線通信部あるいは放送受信部を備えている。ステップST7b−1において、衛星方向獲得手段2は、アンテナ1の受信要素が到来電波を受信してその位相が位相検出部に検出されると、当該受信電波に関して衛星方向情報を更新すべきか否かを判定する。
このとき、衛星方向情報を更新すべきと判断すると、衛星方向獲得手段2は、無線通信部あるいは放送受信部によって外部装置から衛星位置に関する情報を受信する(ステップST7b−2)。さらに、衛星方向獲得手段2は、上記無線通信部あるいは放送受信部によって外部装置から移動体の概略の位置情報を受信する(ステップST7b−3)。また、衛星方向獲得手段2が、移動体の概略の位置として位置演算手段8が算出した移動体のINS位置を入力してもよい。
なお、外部装置としては、例えば測位衛星からの電波を受信して測位情報を取得し、これを基に衛星位置と、外部装置の無線通信部により特定される無線通信エリア内における移動体の概略の位置情報を求めて提供するセンタサーバなどが考えられる。一方、衛星方向情報を更新しないと判定されると、衛星方向情報の更新可否の判定処理に戻る。
また、図7(c)に示す衛星方向獲得手段2は、測位衛星からの電波を受信して測位情報を取得するための無線通信部を備え、取得した測位情報を基に衛星位置とこれにより特定されるエリア内における移動体の実際位置を近似する概略の位置情報を求める。
先ず、衛星方向獲得手段2は、アンテナ1の受信要素が到来電波を受信してその位相が位相検出部に検出されると、上記と同様にして、当該受信電波に関して衛星方向情報を更新すべきか否かを判定する(ステップST7c−1)。
このとき、衛星方向情報を更新すべきと判断すると、衛星方向獲得手段2は、上記無線通信部によって例えば測位衛星からの航法メッセージを受信し、その軌道パラメータを抽出して当該衛星の位置を算出する(ステップST7c−2)。一方、衛星方向情報を更新しないと判定されると、衛星方向情報の更新可否の判定処理に戻る。
次に、衛星方向獲得手段2は、上記無線通信部によって測位衛星から電波を受信し、その到達時間を用いて衛星までの距離を算出し、当該距離と前述の衛星の位置とを用いて移動体の概略の位置情報を求める(ステップST7c−3)。続いて、衛星方向獲得手段2は、測位衛星の位置情報と移動体の位置情報とを用いて、測位衛星への視線方向の方向余弦などの衛星方向情報を算出する(ステップST7c−4)。
図7(d)に示す例では、衛星方向獲得手段2が、図7(a)と同様に、外部装置との間で無線通信を実行するための無線通信部あるいは放送受信部を備えている。衛星方向獲得手段2は、アンテナ1の受信要素が到来電波を受信してその位相が位相検出部に検出されると、当該受信電波に関して衛星方向情報を更新すべきか否かを判定する(ステップST7d−1)。
このとき、衛星方向情報を更新すべきと判断すると、衛星方向獲得手段2は、上記無線通信部あるいは放送受信部によって外部装置から衛星位置情報を無線通信や放送で受信する(ステップST7d−2)。一方、衛星方向情報を更新しないと判定されると、衛星方向情報の更新可否の判定処理に戻る。
次に、衛星方向獲得手段2は、上記無線通信部あるいは放送受信部によって外部装置から移動体の概略の位置情報を受信する(ステップST7d−3)。なお、外部装置としては、例えば測位衛星からの電波を受信して測位情報を取得し、これを基に衛星位置とこれにより特定されるエリア内における移動体の概略の位置情報を求めて提供するセンタサーバなどが考えられる。
このあと、衛星方向獲得手段2は、取得した測位衛星からの測位情報と移動体の位置情報とを用いて、測位衛星への視線方向の方向余弦などの衛星方向情報を算出する(ステップST7d−4)。
図8を用いて、移動体の概略の位置情報を取得する処理について説明する。
図8(a)に示す例では、衛星方向獲得手段2が、無線通信部として既存のセルラ電話機を有している。先ず、衛星方向獲得手段2は、アンテナ1の受信要素によって到来電波が受信され位相検出部によりその位相が検出されると、移動体の現在の位置情報を更新すべきか否かを判定する(ステップST8a−1)。
このとき、位置情報を更新すべきと判断すると、衛星方向獲得手段2は、無線通信部であるセルラ電話機を介して基地局センタから当該基地局センタが管理する無線通信エリア内の位置情報、例えば当該無線通信を中継する基地局アンテナ位置を移動体の概略位置情報として取得して以前の位置情報を更新する(ステップST8a−2)。一方、位置情報を更新しないと判定されると、再び位置情報の更新可否の判定処理に戻る。
図8(b)の例では、衛星方向獲得手段2が、測位衛星高度に対する位置誤差が許容範囲内となる移動体の位置に関する情報として、地図データ中の代表的な地点に関する位置情報をメモリ内に予め格納しておき、姿勢検出処理にあたり当該メモリから前記位置情報を移動体の概略位置情報として読み出す(ステップST8b−1)。
また、図8(c)に示す例では、衛星方向獲得手段2が、無線通信部あるいは放送受信部を備えている。衛星方向獲得手段2は、アンテナ1の受信要素によって到来電波が受信され位相検出部によりその位相が検出されると、移動体の現在の位置情報を更新すべきか否かを判定する(ステップST8c−1)。
このとき、位置情報を更新すべきと判断すると、衛星方向獲得手段2は、無線通信部あるいは放送受信部によって外部装置から移動体の位置情報を無線通信や放送で受信して以前の位置情報を更新する(ステップST8c−2)。外部装置としては、例えば測位衛星からの電波を受信して測位情報を取得し、これを基に衛星位置、移動体の位置情報及び当該移動体から衛星への視線方向を規定する衛星方向情報を求めて提供するセンタサーバなどが考えられる。
また、衛星方向獲得手段2が、位置演算手段8が算出した移動体のINS位置を入力して以前の位置情報を更新するようにしてもよい。
なお、衛星方向獲得手段2による処理については、後述する実施の形態にて構成を明示しつつ詳細に説明する。
ここで、図6に示すフローチャートの説明に戻る。
基線ベクトル検出手段3は、上述のようにして衛星方向獲得手段2によって測位衛星の位置情報や移動体の位置情報が取得されると、本実施の形態による移動体姿勢検出装置がコールドスタートしたのか、ホットスタートしたのかを判定する(ステップST3)。ここで、コールドスタートとは、本発明の移動体姿勢検出装置を実現するコンピュータの電源が完全に切れている状態から起動することを示している。
また、ホットスタートとは、ソフトウェアリセットによってハードウェアチェックの一部を省略して高速に再起動することを示している。ホットスタートでは、ソフトウェアリセットによって本発明による移動体姿勢検出装置がホールトを経由して停止した際の移動体の停止姿勢が内部メモリに保持される。
ステップST3においてコールドスタートであるならば、基線ベクトル検出手段3は、装置内に設けた計時カウンタ(不図示)の時刻Tを0に設定する(ステップST4)。この場合、ステップST5におけるGPS姿勢の算出処理に移行する。ステップST5におけるGPS姿勢の算出処理は、上記実施の形態1で示した基線ベクトル検出手段3及び姿勢演算手段4によるローカル座標系における移動体のGPS姿勢(絶対方位など)を求める処理である。
図9は、GPS姿勢の算出処理を示すフローチャートであり、3つの測位衛星からの到来電波を2つの受信要素で受信した場合の処理を示している。この図に沿って、ステップST5におけるGPS姿勢の算出処理を詳細に説明する。
先ず、コールドスタートであると判断され、基線ベクトル検出手段3により計時カウンタの時刻Tが0に設定されると、基線ベクトル検出部内の衛星選択部3aが、ローカル座標系における移動体の絶対姿勢の算出に利用するGPS測位衛星を選択する(ステップST9−1)。ここでは、3つの衛星が選択される。
図10は、GPS測位衛星の選択処理を示すフローチャートであり、この図に沿って図9中のステップST9−1の処理を説明する。
衛星選択部3aは、アンテナ1の受信要素が受信した受信電波に関する情報や衛星方向獲得手段2が取得した衛星方向情報などに基づいて、現在アンテナ1の受信要素が衛星電波を受信中の衛星数が2個未満であるか否かを判定する(ステップST10−1)。このとき、2個未満と判定されれば、衛星選択部3aは、正確な姿勢検出が不可能であると判断して基線ベクトル演算部3bに基線ベクトルの算出を実行させない(ステップST10−2)。
一方、衛星電波を受信中の衛星数が2個以上であれば、衛星選択部3aは、当該衛星数が2個であるか否かを判定する(ステップST10−3)。このとき、衛星数が2個と判定すると、衛星選択部3aは、衛星電波を受信中の衛星のうち仰角が大きい方を第1の測位衛星とし(ステップST10−4)、仰角が小さい方を第2の測位衛星とする(ステップST10−5)。このあと、図9中のステップST9−2の処理へ移行する。
また、衛星数が2個を超える場合、衛星選択部3aは、衛星電波を受信中の衛星のうち仰角が最大のものを第1の測位衛星とする(ステップST10−6)。このあと、衛星選択部3aは、残った受信中の衛星のうち、衛星方向獲得手段2からの移動***置情報に基づいて特定した移動***置への電波到来方向が、ステップST10−6で決定した第1の測位衛星からの電波到来方向に90度離隔するものに最も近い衛星を第2の測位衛星とする。そして、同様に、第1〜第(i−1)の測位衛星からの電波到来方向に90度離隔するものに最も近い順から第iの測位衛星を決定する。ここまでが、ステップST10−7の処理に相当する。
上述した処理は、例えば受信要素が2つで姿勢検出に利用可能な衛星数が2個である場合、各衛星から到来する電波の方向が直交するときに最も姿勢角の角度誤差が最小になると解釈されることを前提としている。具体的に説明すると、図11に示すように、一定の角度誤差±ε(ラジアン)を与えた場合の位置(基線ベクトル)検出の最大位置誤差は、電波到来方向の交差角αを90度にすると最小値(±21/2・ε)になる。
また、測位衛星からの到来電波は、仰角が小さいほどイオン層などで屈折等の影響を受けて歪やすい。そこで、仰角が最も大きい順に姿勢検出に利用する衛星とする。反対に、例えば仰角が15度未満の衛星については移動体の姿勢検出に利用しないようにする。なお、仰角と共に増加する任意の重み関数を定義して測位衛星の選択処理の判断材料の1つにしてもよい。
ステップST10−7で第2〜第iの測位衛星を決定すると、衛星選択部3aは、衛星電波を受信中の衛星数が4個以上であるか否かを判定する(ステップST10−8)。このとき、衛星電波を受信中の衛星数が3個以上4個未満であれば、図9中のステップST9−2の処理へ移行する。一方、衛星電波を受信中の衛星数が4個以上であれば、衛星選択部3aは、測位モードをFIXとする。
ここで、FIXモードとは、後の処理で、基線ベクトル演算部3bが、衛星電波を受信中の受信要素の可能な組み合わせを全て求め、これらの全ての基線ベクトル候補を算出させ、ロバスト二乗誤差最小のルールなどにより最適と思われる基線ベクトルを選択し決定する処理モードである。
図9に示すフローチャートの説明に戻る。
ステップST9−1で姿勢検出に利用する測位衛星が選択されると、基線ベクトル演算部3bは、基線ベクトル(基準となる第1アンテナから第2アンテナへ向かう基線ベクトル)の長さLの誤差許容値δを初期設定する(ステップST9−2)。初期設定としては、例えば許容値δを受信電波の1波長λの1/100程度の値に設定する。
次に、基線ベクトル演算部3bは、基線ベクトル算出処理で求めた基線ベクトル候補や位相差の整数アンビギュイティNの値などを一時記憶するメモリの内容をクリアし、波数整数アンビギュイティNの値を初期値に設定して初期化処理を実行する(ステップST9−3)。ここで、衛星選択部3aが選択した全ての測位衛星に対応する整数アンビギュイティNiを初期化する。なお、添え字iは観測対象の電波を送信した測位衛星を特定する番号であり、1から衛星選択部3aにより選択された測位衛星数nまでの自然数をとる。Niは、i番目の測位衛星に対応する整数アンビギュイティである。
続いて、基線ベクトル演算部3bは、第1〜第iの測位衛星に対応する整数アンビギュイティN1〜Niについて、各アンビギュイティNが取り得る所定範囲(例えば、−m〜m)の値を順に設定する(ステップST9−4)。但し、mはアンテナ1内の受信要素の設置間隔Lを第1〜第iの測位衛星からの搬送波の波長λで除算した値の整数部である。
基線ベクトル演算部3bは、ステップST9−4で整数アンビギュイティNiの値が設定される度に、そのアンビギュイティNiを用いて基線ベクトル候補を算出する(ステップST9−5)。例えば、上記実施の形態1で示した、3つの測位衛星からの到来電波を2つの受信要素で受信した場合における基線ベクトルの推定原理に従って処理する。
つまり、基線ベクトルRの先端座標位置である座標値(x,y,z)が、図2中の平面1、平面2及び平面3のいずれの平面にも含まれることを利用して、上記式(1)〜(4)に従い、位相差観測ベクトルD1,D2,D3から平面1,平面2,平面3を求める。これにより求めた平面1,平面2,平面3の各交点にベクトル先端を有する基線ベクトルRを移動体の姿勢検出に利用するベクトル候補とする。
なお、位相差観測ベクトルD1,D2,D3の算出には、衛星方向獲得手段2から取得した第1〜第3の測位衛星への視線方向の各方向余弦(hx1,hy1,hz1)、(hx2,hy2,hz2)、(hx3,hy3,hz3)を用い、上記式(5)〜(7)に従って算出される。このとき、基線ベクトル演算部3bは、第1〜第3の測位衛星からの到来電波の各受信要素についての位相差の観測量dd1〜dd3に整数アンビギュイティN(−m〜+m、m=mod(L/λ))をそれぞれ加えて、各受信要素における受信電波の位相差d1,d2,d3を生成する。
上述のようにして、基線ベクトル候補を算出すると、基線ベクトル演算部3bは、当該ベクトル候補の長さの絶対値の自乗値(|R|)2が(L±δ)2の範囲内にあるか否かを判定する(ステップST9−6)。このとき、上記範囲になければ、基線ベクトル演算部3bは、当該基線ベクトル候補を破棄してステップST9−8の処理に移行する。
一方、上記範囲内にあれば、基線ベクトル演算部3bは、その基線ベクトル候補及びこれの算出に用いた整数アンビギュイティNを対にした情報をメモリに一時記憶する(ステップST9−7)。基線ベクトル候補及びこれの算出に用いた整数アンビギュイティNを対にした情報とは、例えばこの基線ベクトル候補の算出に用いた観測量(ddi+Ni)や(λ+wi)と、(L±δ)2の範囲内に入っていたことを示すフラグとを対にした情報である。なお、ddiはi番目の測位衛星からの搬送波についての位相差の観測量である。
ステップST9−8にて、基線ベクトル演算部3bは、第1〜第iの測位衛星に対応する整数アンビギュイティN1〜Niについて、各アンビギュイティNが取り得る所定範囲の値を設定した全ての走査が終了したか否かを判定する。このとき、全てのアンビギュイティNについての走査が終了していなければ、基線ベクトル演算部3bは、ステップST9−4の処理に移行して、各アンビギュイティNが取り得る所定範囲内の値のうち、基線ベクトル候補の算出を行っていないものをそれぞれ設定して、ステップST9−5からステップST9−8までの処理を繰り返す。
一方、全てのアンビギュイティNについての走査が終了していれば、基線ベクトル演算部3bは、ステップST9−9の処理に移行する。ステップST9−9において、基線ベクトル演算部3bは、メモリに設定されたフラグの総数をカウントすることで、メモリ中に一時記憶させた基線ベクトル候補の総数が1つであるか否かを判定する。ここで、基線ベクトル候補の総数が1ではない場合、基線ベクトル演算部3bは、ステップST9−10の処理に移行する。
ステップST9−10において、基線ベクトル演算部3bは、基線ベクトル候補の総数が0であれば、基線ベクトルの長さの誤差許容値δを前回のδ値よりも所定の変更分だけ大きい値に変更した後、ステップST9−3の処理に戻り、変更後のδを用いて前記ステップST9−3からステップST9−8までの処理を繰り返す。
また、ステップST9−10において、基線ベクトル演算部3bは、基線ベクトル候補の総数が2以上であれば、基線ベクトルの長さの誤差許容値δを前回のδ値よりも所定の変更分だけ小さい値に変更した後、ステップST9−3の処理に戻り、変更後のδを用いて前記ステップST9−3からステップST9−8までの処理を繰り返す。
このようにして、基線ベクトル演算部3bは、メモリに一時記憶した基線ベクトル候補の総数が1つとなるまで上述した処理を繰り返す。
ステップST9−9において、基線ベクトル演算部3bがメモリに一時記憶した基線ベクトル候補の総数が1つであると判断すると、姿勢演算手段4は、上述のようにして唯一推定された基線ベクトルに関するデータをメモリから読み出して、当該基線ベクトルの、基準(真北)ベクトルからの回転角(ヨー角あるいは方位角と呼ぶ)と、当該基線ベクトルの、水平からの前後方向傾斜角(ピッチ角)とを算出する(ステップST9−11)。
なお、ここまでの基線ベクトル推定に関する計算処理を、全てECEF(地球中心地球固定)座標系で行っていたのであれば、姿勢演算手段4が、上記基線ベクトルで姿勢を求める際、ECEF座標系から表す際に分かりやすいENU(East, North, Up)座標系へ姿勢を変換するように構成してもよい。続いて、姿勢演算手段4は、ステップST9−11で求めた姿勢角(ヨー角、ピッチ角、ロール角など)を、例えばカルマンフィルタで平滑化する(ステップST9−12)。
図12は、2つの測位衛星からの到来電波を3つの受信要素で受信した場合におけるGPS姿勢の算出処理を示すフローチャートであり、この図に沿って図6中のステップST5における処理を詳細に説明する。
先ず、コールドスタートであると判断され、基線ベクトル検出手段3により計時カウンタの時刻Tが0に設定されると、基線ベクトル検出部内の衛星選択部3aが、ローカル座標系における移動体の絶対姿勢の算出に利用するGPS測位衛星を、上述した図10に示すフローに従って選択する(ステップST12−1)。ここでは、2つの衛星が選択される。
ステップST12−1で姿勢検出に利用する測位衛星が選択されると、基線ベクトル演算部3bは、角度誤差の許容値γを初期化する(ステップST12−2)。初期設定としては、例えば許容値γを1*π/180(rad)程度の値に設定する。
次に、基線ベクトル演算部3bは、基線ベクトル算出処理で求めた基線ベクトル候補や位相差の整数アンビギュイティNの値などを一時記憶するメモリの内容をクリアし、波数整数アンビギュイティNの値を初期値に設定して初期化処理を実行する(ステップST12−3)。ここで、衛星選択部3aが選択した全ての測位衛星に対応する整数アンビギュイティNiを初期化する。なお、添え字iは、観測対象の電波を送信した測位衛星を特定する番号であり、1から衛星選択部3aにより選択された測位衛星数nまでの自然数をとる。Niは、i番目の測位衛星に対応する整数アンビギュイティである。
続いて、基線ベクトル演算部3bは、第1〜第iの測位衛星に対応する整数アンビギュイティN1〜Niについて、各アンビギュイティNが取り得る所定範囲(例えば、−m〜m)の値を順に設定する(ステップST12−4)。但し、mは、アンテナ1内の受信要素の設置間隔Lを第1〜第iの測位衛星からの搬送波の波長λで除算した値の整数部である。
基線ベクトル演算部3bは、ステップST12−4で整数アンビギュイティNiの値が設定される度に、そのアンビギュイティNiを用いて基線ベクトル候補を算出する(ステップST12−5)。例えば、上記実施の形態1で示した2つの測位衛星からの到来電波を3つの受信要素で受信した場合における基線ベクトルの推定原理に従って処理する。
つまり、基線ベクトル演算部3bが、基準となる第1アンテナと他の第2アンテナ及び第3アンテナとのそれぞれの組み合わせから位相差観測ベクトルを求め、これら各位相差観測ベクトルの座標を含み、基線ベクトル方向と直交する平面と基線ベクトルの長さを半径とし第1アンテナ位置を中心とする球面とのいずれにも基線ベクトル座標が含まれることを利用して、これらの交点にベクトル先端を有するベクトル対を基線ベクトル候補対として抽出する。
続いて、基線ベクトル演算部3bは、基線ベクトル候補対を構成する各ベクトル間の角度αが、基準の第1アンテナと他の第2、第3アンテナをそれぞれ結ぶ基線ベクトル間の角度Φとして許容される値の範囲に最も近い方を、移動体の姿勢や位置の測定に用いる基線ベクトル候補対として推定する。
上述のようにして、基線ベクトル候補対を算出すると、基線ベクトル演算部3bは、当該ベクトル候補を構成する各ベクトル間の角度αが、(Φ±γ)の範囲内にあるか否かを判定する(ステップST12−6)。このとき、上記範囲になければ、基線ベクトル演算部3bは、当該基線ベクトル候補を破棄してステップST12−8の処理に移行する。
一方、上記範囲内にあれば、基線ベクトル演算部3bは、その基線ベクトル候補対及びこれの算出に用いた整数アンビギュイティNを対にした情報をメモリに一時記憶する(ステップST12−7)。基線ベクトル候補及びこれの算出に用いた整数アンビギュイティNを対にした情報とは、例えばこの基線ベクトル候補対の算出に用いた観測量(di1+Ni)及び(λ+wi)、(di2+Ni)及び(λ+wi)と、(Φ±γ)の範囲内に入っていたことを示すフラグとをそれぞれ対にした情報である。なお、di1は第1アンテナと第2アンテナにおけるi番目の測位衛星からの搬送波についての位相差の観測量であり、di2は第1アンテナと第3アンテナにおけるi番目の測位衛星からの搬送波についての位相差の観測量である。
ステップST12−8にて、基線ベクトル演算部3bは、第1〜第iの測位衛星に対応する整数アンビギュイティN1〜Niについて、各アンビギュイティNが取り得る所定範囲の値を設定した全ての走査が終了したか否かを判定する。このとき、全てのアンビギュイティNについての走査が終了していなければ、基線ベクトル演算部3bは、ステップST12−4の処理に移行して、各アンビギュイティNが取り得る所定範囲内の値のうち、基線ベクトル候補対の算出を行っていないものをそれぞれ設定して、ステップST12−5からステップST12−8までの処理を繰り返す。
一方、全てのアンビギュイティNについての走査が終了していれば、基線ベクトル演算部3bは、ステップST12−9の処理に移行する。ステップST12−9では、基線ベクトル演算部3bが、メモリに設定されたフラグの総数をカウントすることで、メモリ中に一時記憶させた基線ベクトル候補対の総数が1つであるか否かを判定する。ここで、基線ベクトル候補対の総数が1ではない場合、基線ベクトル演算部3bは、ステップST12−10の処理に移行する。
ステップST12−10において、基線ベクトル演算部3bは、基線ベクトル候補対の総数が0であれば、角度誤差の許容値γを前回値よりも所定の変更分だけ大きい値に変更した後、ステップST12−3の処理に戻り、変更後のγを用いて前記ステップST12−3からステップST12−8までの処理を繰り返す。
また、ステップST12−10において、基線ベクトル演算部3bは、基線ベクトル候補対の総数が2以上であれば、角度誤差の許容値γを前回値よりも所定の変更分だけ小さい値に変更した後、ステップST12−3の処理に戻り、変更後のγを用いて前記ステップST12−3からステップST12−8までの処理を繰り返す。
このようにして、基線ベクトル演算部3bは、メモリに一時記憶した基線ベクトル候補対の総数が1つとなるまで上述した処理を繰り返す。
ステップST12−9において、基線ベクトル演算部3bがメモリに一時記憶した基線ベクトル候補対の総数が1つであると判断すると、姿勢演算手段4は、上述のようにして唯一推定された基線ベクトル候補対に関するデータをメモリから読み出して、当該基線ベクトル候補対のうちのヨー角、ピッチ角に対応する基線ベクトルを基に、基準(真北)ベクトルからの回転角(ヨー角あるいは方位角と呼ぶ)と、基線ベクトルの、水平からの前後方向傾斜角(ピッチ角)を算出し、当該基線ベクトル候補対のうち、他方のロール角に対応する基線ベクトルを基に水平からの左右方向回転角(ロール角)を算出する(ステップST12−11)。
なお、ここまでの基線ベクトル推定に関する計算処理を、全てECEF(地球中心地球固定)座標系で行っていたのであれば、姿勢演算手段4が、上記基線ベクトルで姿勢を求める際、ECEF座標系から表す際に分かりやすいENU(East, North, Up)座標系へ姿勢を変換するように構成してもよい。
続いて、姿勢演算手段4は、ステップST12−11で求めた姿勢角(ヨー角、ピッチ角、ロール角など)を、例えばカルマンフィルタで平滑化する(ステップST12−12)。
上述のようにして、ステップST5におけるGPS姿勢の算出処理が完了すると、姿勢演算手段4は、GPS姿勢の算出処理にて求めた移動体のGPS姿勢を、INS姿勢演算手段6が算出する移動体のINS姿勢の初期値として設定する(ステップST6)。さらに、位置演算手段8は、後述するようにして求めた移動体のINS位置を概略位置として設定する(ステップST7)。
一方、ステップST3でホットスタートと判断された場合も、基線ベクトル検出手段3は、上記計時カウンタの時間Tを0に設定する(ステップST8)。このあと、基線ベクトル検出手段3は、前回にホールトを経由してストップした際に内部メモリに保持された移動体の姿勢情報(移動体の停止姿勢)を、INS姿勢演算手段6が算出する移動体のINS姿勢の初期値として設定する(ステップST9)。さらに、基線ベクトル検出手段3は、前回にホールトを経由してストップした際に内部メモリに保持された移動体の位置情報(移動体の停止位置)を、位置演算手段8が算出する移動体のINS位置の概略値として設定する(ステップST10)。
ステップST7やステップST10で、移動体のINS位置の概略値が設定されると、INS姿勢演算手段6は、図中に破線で囲ったINS姿勢演算処理を実行し、位置演算手段8はINS位置演算処理を実行する。
先ず、INS姿勢演算手段6には、移動体のINS姿勢及びINS位置の初期値として設定された値の有効性を判断するための時刻の閾値T1が予め設定されており、INS姿勢演算処理及びINS位置演算処理を実行するにあたり、計時カウンタの現時刻がT1を経過しているか否かを判定する(ステップST16)。ここで、計時カウンタの現時刻がT1を経過している場合、INS姿勢演算手段6は、ステップST17の処理に移行して計時カウンタの時刻を0に初期化する。
これにより、姿勢演算手段4は、ステップST18からステップST21までのINS姿勢演算及びINS位置演算を開始する。先ず、ステップST18におけるGPS姿勢の算出処理は、上述したステップST5における処理と同様である。
ステップST19において、INS姿勢演算手段6は、ステップST18で姿勢演算手段4が求めたGPS姿勢を姿勢1として設定し、ステップST6やステップST9で設定されたINS姿勢の初期値や、角速度検出手段5により検出された3軸のうちの少なくとも1軸の軸周りの角速度を基に算出したINS姿勢をINS姿勢1として設定する。
図5の例で説明すると、INS姿勢演算手段6には、姿勢演算手段4が求めたGPS姿勢がINS姿勢として入力され、ステップST6やステップST9で設定されたINS姿勢あるいは角速度検出手段5の具体例であるジャイロ10が検出した角速度を基に算出したINS姿勢がINS姿勢1とされる。
続いて、INS姿勢演算手段6は、ステップST19で設定された今回の姿勢(姿勢1)とメモリに前回記憶しておいた移動体の姿勢である姿勢2との差と、今回のINS姿勢(INS姿勢1)と前回に記憶しておいたINS姿勢2との差との差であるΔ姿勢を算出する。Δ姿勢は今回と前回の姿勢におけるオフセットのドリフト量であるオフセット補正量Δoffsetの計時カウンタ時刻T=0からT1までの積分値に相当し、Δoffset・T1で表される。そこで、INS姿勢演算手段6は、上記関係を利用して下記式(42)に従ってΔ姿勢/T1によりオフセット補正量Δoffsetを算出する。
Δ姿勢=(姿勢1−姿勢2)−(INS姿勢1−INS姿勢2)
=Δoffset・T1 (42)
なお、上記処理は、図5におけるINS姿勢演算手段6の内部構成である、姿勢演算手段4が求めたGPS姿勢を受ける演算器17、補償演算部16、その出力を演算する演算器14及び計時カウンタ時刻T=0からT1までの積分値を求める積分器15によって実行される。
そして、INS姿勢演算手段6内の固定オフセット補償部12は、角速度検出手段5(ジャイロ10)が検出した角速度(A/D変換器11がディジタル化したもの)に加算すべきオフセットΔofsとして、前回検出された角速度に加算したオフセットΔofsに今回のオフセット補正量Δoffsetを反映させたオフセットを算出する。例えば、オフセット補正量Δoffsetの重み付けをη(例えば、η=0.1)とし、下記式(43)に沿って今回のオフセットΔofsを算出する。なお、このようにして求めたオフセットΔofsが、後述するステップST22の処理が次回に実行される時のオフセットΔofsとして使用される。
Δofs=Δofs・(1−η)+Δoffset・η (43)
上述した処理がステップST20に相当する。
次に、INS姿勢演算手段6は、今回姿勢演算手段4から入力した姿勢出力である姿勢1の値を、前回の姿勢を示す姿勢2に設定し、今回角速度検出手段5が検出した角速度を基にして算出したINS姿勢の値を、前回のINS姿勢を示すINS姿勢2に設定して内部メモリに記憶する(ステップST21)。
一方、ステップST16で計時カウンタの現時刻がT1を経過していないと判定すると、INS姿勢演算手段6内の固定オフセット補償部12は、角速度検出手段5による移動体の角速度検出値から所定のオフセット値Δofsを差し引き、スケールファクタを用いて微小経過時間ΔTでの角度変化量Δθを算出する(ステップST22)。
次に、固定オフセット補償部12は、ステップST22で算出した角度変化量Δθを、今回、角速度検出手段5が検出した角速度を基にして算出したINS姿勢の値に加算して新たなINS姿勢の値として内部メモリに記憶する(ステップST23)。
なお、ステップST17からステップST21までの処理は、移動体の3軸であるヨー角、ピッチ角、ロール角のうちのいずれか1つについての実行する処理であり、図中に(姿勢数分)と記載したように、必要に応じてヨー角、ピッチ角、ロール角の全ての姿勢数について同様な一連の処理を実行する。この場合、ステップST22、ステップST23に相当する処理も追加される。
INS姿勢演算手段6は、上述したINS姿勢演算処理が完了すると、算出したINS姿勢(ヨー角、ピッチ角、ロール角)に関する情報を、位置演算手段8、提示/制御手段9及び不図示の外部装置に出力する。
位置演算手段8では、速度検出手段7が検出した移動体の速度Vを入力し、スケールファクタを用いて微小経過時間ΔTでの移動距離d=V・ΔTを算出する(ステップST24)。次に、位置演算手段8は、INS姿勢演算手段6から入力したINS姿勢(ヨー角(方位)、ピッチ角、ロール角(方位))に関する情報に基づき、例えば下記式(44)〜(46)に従い移動体のINS位置を算出する(ステップST25)。
位置x=位置x+d・cos(ヨー角) (44)
位置y=位置y+d・sin(ヨー角) (45)
高度=高度+d・sin(ピッチ角) (46)
位置演算手段8は、上述したINS位置演算処理が完了すると、算出したINS位置に関する情報を、提示/制御手段9及び不図示の外部装置に出力する。また、必要に応じて、衛星方向獲得手段2、基線ベクトル検出手段3、姿勢演算手段4及びINS姿勢演算手段6に移動体のINS位置を出力する場合もある。
衛星方向獲得手段2については前述したが、例えば基線ベクトル検出手段3ついては、衛星方向獲得手段2から移動体の位置情報が得られない場合、位置演算手段8からの移動体のINS位置を用いて基線ベクトル推定処理を実行する。また、姿勢演算手段4が基線ベクトル検出手段3により求められた基線ベクトルを用い、移動体のGPS姿勢を算出する際に、位置演算手段8からの移動体のINS位置を用いてもよい。さらに、位置演算手段8からの移動体のINS位置を、INS姿勢演算手段6によるINS位置演算の初期値として利用するようにしてもよい。
ステップST26において、提示/制御手段9は、INS姿勢演算手段6から入力したINS姿勢や位置演算手段8から入力したINS位置を、不図示の表示装置に表示してユーザに提示する。また、表示内容に応じてユーザが入力した情報などに基づき、移動体の動作を制御するための制御情報を生成する。
例えば、姿勢角を、グラフ表示、図形表示、数値表示などで表示装置に表示をする。ここで、ユーザにより外部から姿勢角に関しての閾値を提示/制御手段9に設定するためのインタフェースを設けておくことで、INS姿勢演算手段6から入力したINS姿勢が前記閾値を超えると、表示パターン、表示図形、表示文字、点滅ランプ、音声メッセージ、警告音などのいずれかあるいはいずれかの複数の組み合わせで警告するように構成しても良い。例えば、移動体の姿勢角から導かれる現時点における移動体の傾斜角が閾値を超える場合、音声警告する。
INS姿勢演算手段6から入力したINS姿勢を移動体の制御のための1つの制御量として利用するように提示/制御手段9を構成しても良い。例えば、ピッチ角姿勢を移動体の安全性向上や快適性向上、燃費向上のための自動制御に利用する。
移動体として車両を例に挙げて説明すると、本発明による移動体姿勢検出装置が検出したピッチ角が、通常姿勢時より大であることを検出したなら、通常姿勢値に近い値になるようにサスペンションの制御量を求め、移動体を制御する。また、車両が通行する路面の凸凹のピッチが比較的に短い場合、前後の姿勢の振動のプロファイルを分析して振動の振幅を低減するように振動の位相を考慮して制御する。このように構成することで、車両が凸凹の路面を走行する場合に、本発明の移動体姿勢検出装置を搭載しない車両と比べ凸凹を感じさせない乗り心地を達成することができる。
また、従来の車両の低速走行制御では、上り坂では減速してからでしかアクセルされない、あるいは下り坂ではある程度の自然加速してからでしか減速制御されない。このため、加減速が連なったギクシャクした動きとなり、ドライバが不快な思いをしたり、前後の車との干渉で危険な状態を生みやすい。これに対して、本発明による移動体姿勢検出装置が検出した姿勢情報を用いることで、上り坂では傾斜に応じて自動的にエンジンをアクセルし、下り坂で少なくとも検出された傾斜姿勢に応じて自動的に減速するといった制御が可能であり、前述したような従来の不具合を解消することができる。
ステップST26の処理が完了すると、INS姿勢演算手段6は、移動体が動力停止されるか否かを判定する(ステップST11)。ここで、移動体の動力が停止される場合、INS姿勢演算手段6は、本発明による移動体姿勢検出装置の動力停止される前に、現在設定されているINS姿勢を停止姿勢として不揮発性記憶手段である内部メモリに記憶する(ステップST12)。同様に、位置演算手段8は、動力停止される前に、現在設定されているINS位置を停止位置として内部メモリに記憶する(ステップST13)。
このようにして設定された停止姿勢や停止位置は、次回のホットスタート時に、ステップST9やステップST10で内部メモリから読み出されてINS姿勢やINS位置として設定され、前回停止時に引き続いて移動体の姿勢や位置を即刻得るために使用される。
一方、ステップST11で動力が停止されないと判定された場合、INS姿勢演算手段6は、不図示のタイマの時刻が所定の時間間隔ΔTだけ経過したか否かを判定する(ステップST14)。このとき、ΔTだけ経過していなければ、再びタイマ時刻の経過の判定処理に戻り、ΔTだけ経過していれば、上述した計時カウンタの時刻TをΔTだけインクリメントする(ステップST15)。このあと、ステップST16に移行して、上述したINS姿勢演算処理を実行する。このように移動体の動力が停止されるか否かを基準として、図6中に太線で示した通常処理時のループで処理が繰り返される。
また、例えばステップST25の処理に続き、別個に設けたGPS測位手段で移動体の位置を検出し、これを位置演算手段8が入力して、自己が算出したINS位置との差を評価し、差の一定割合だけGPS測位位置に近づけた位置としてINS位置を補正する処理を追加してもよい。
以上のように、この実施の形態2によれば、例えば天頂衛星と移動体前方あるいは後方に位置する、2個の衛星からの衛星電波のみを用いて移動体の絶対姿勢を連続的に検出することができることから、INS姿勢の演算に使用する角速度検出手段5のオフセットドリフトによるINS姿勢の誤差集積をほぼ常時補正することができる。このように途切れ少なく検出できるため、高精度な姿勢検出を維持することができる。
なお、姿勢演算手段4が求めたGPS姿勢やINS姿勢演算手段6が求めたINS姿勢を位置演算手段8に入力することで、位置演算手段8に対してよりアベイラビリティ(有用性)の良い姿勢情報を与えることができ、より精度の高い位置が得られる上、位置演算手段8から精度の高い位置情報を他の構成手段に帰還することができるのでより信頼度の高い姿勢情報を得ることができる。
また、上記実施の形態2では、上記実施の形態1の構成に、角速度検出手段5、INS姿勢演算手段6、速度検出手段7及び位置演算手段8を設ける例を示したが、例えば図13に示すように上記実施の形態1に角速度検出手段5及びINS姿勢演算手段6を追加した構成であってもよい。
図13の構成であっても、INS姿勢演算手段6が、角速度検出手段5から移動体の姿勢を規定するピッチ角とロール角を求めるための角速度情報を受け、その積分値を基にして移動体のピッチ角やロール角を算出する。アンテナ1が1つの測位衛星からの電波しか受信できない場合、INS姿勢演算手段6が求めた移動体のピッチ角やロール角を拘束条件とすることで、姿勢演算手段4が移動体のヨー角を推定することができる。
また、図13の構成において別個にGPS測位手段を設け、このGPS測位手段によって検出した移動体の位置情報を、図7(c)や図8(c)で示した動作フローで衛星方向獲得手段2が取得すべき移動体の位置情報としても良く、必要に応じて提示/制御手段9、衛星方向獲得手段2、基線ベクトル検出手段3、姿勢演算手段4及びINS姿勢演算手段6に出力しても良い。
例えば、基線ベクトル検出手段3ついては、衛星方向獲得手段2から移動体の位置情報が得られない場合、上記GPS測位手段からの移動体のGPS位置を用いて基線ベクトル推定処理を実行する。また、姿勢演算手段4が基線ベクトル検出手段3により求められた基線ベクトルを用い、移動体のGPS姿勢を算出する際に、当該GPS測位手段からの移動体の位置を用いるようにしてもよい。さらに、当該GPS測位手段からの移動***置をINS姿勢演算手段6によるINS位置演算の初期値として利用してもよい。
このように構成することで、姿勢演算手段4が求めたGPS姿勢をINS姿勢演算手段6に入力することで、INS姿勢演算手段6に対してよりアベイラビリティ(有用性)の良い姿勢情報を与えることができ、より精度の高いINS姿勢が得られる上、INS姿勢演算手段6から精度の高い姿勢情報を他の構成手段に帰還することができるのでより信頼度の高い姿勢情報を得ることができる。また、INS姿勢演算手段6による姿勢角を拘束条件に加えることで、例えば順天頂衛星1機のみしか受信できない場合であっても移動体の姿勢を検出することができる。
実施の形態3.
図14は、この発明の実施の形態3による移動体姿勢検出装置の構成を示す図であり、図1、図4及び図13と同一の符号を付したものは同一又はこれに相当するものである。なお、図14では、上記実施の形態2で示した位置演算手段8とこれに関連する構成要素の図示を省略している。本実施の形態は、衛星方向獲得手段2の詳細な構成を示したものであり、図示の例では無線通信部/放送受信部(通信部)18を有している。
無線通信部/放送受信部18は、無線通信及び/又は放送受信を実行する通信構成を有し、無線通信又は放送受信により通信接続可能な衛星方向情報提供サーバ22から天頂衛星あるいは測位衛星についての、ローカル座標系上の移動体の位置情報と衛星方向情報とを獲得する。
衛星方向情報提供サーバ22は、GPS測位衛星からの電波を受信して処理するレシーバ21と通信可能に接続しており、このレシーバ21から受信した各種情報を用いて最新の航法パラメータなどの、現在の測位情報を取得する。これにより、衛星方向情報提供サーバ22は、各衛星ごとに衛星位置及びこの位置の衛星からの電波が受信可能な特定エリアの代表位置を移動体の概略の位置とみなし、この位置からの衛星方向を規定する衛星方向情報を求める。
また、衛星方向情報提供サーバ22は、ネットワーク23を介して無線通信基地局24a,24bと接続しており、無線通信基地局24a,24b、無線アンテナ19や放送衛星20を経由して無線通信部/放送受信部18と通信する。これにより、衛星方向情報提供サーバ22は、前述の衛星方向情報を無線通信部/放送受信部18が受信可能な所定の通信形態にデコードして、ネットワーク23、無線通信基地局24a,24bなどを介して通信可能領域における移動体側からの要求に応答する形で衛星方向情報を提供する。衛星方向獲得手段2側からみた衛星方向情報の取得動作としては、例えば上記実施の形態2で示した図7(a)に沿うものが考えられる。
無線通信部/放送受信部18が受信可能な通信形態としては、衛星方向情報を含む放送であっても良い。また、衛星方向情報提供サーバ22が衛星方向情報を求めるのではなく、不図示のセンタサーバが移動体の概略位置や衛星の位置に関する情報を求め、このセンタサーバから衛星方向情報提供サーバ22が前記情報を中継して衛星方向情報に加工して提供する、いわゆる中継サーバの形態であってもよい。
なお、衛星方向情報は、ローカル座標系における、所定の位置(移動***置)から衛星への視線方向ベクトル(衛星方向)が求められさえすれば、任意の形式のデータであってよい。衛星方向またはその逆方向へ所定距離だけ離れた点の座標が算出できるデータであれば、例えば勾配を示す形式であってもよいし、方向余弦の形式であってもよい。
以上のように、この実施の形態3によれば、移動***置から衛星への視線方向を規定する衛星方向情報を外部サーバから受けることで、衛星方向を検出して衛星方向情報を生成する構成要素を省略することができる。
実施の形態4.
図15は、この発明の実施の形態4による移動体姿勢検出装置の構成を示す図であり、図1、図4及び図13と同一の符号を付したものは同一又はこれに相当するものである。なお、図15では、上記実施の形態2で示した位置演算手段8とこれに関連する構成要素の図示を省略している。
本実施の形態は、衛星方向獲得手段2の詳細な構成を示したものであり、図示の例ではローカル位置獲得部25及び衛星位置獲得部26を有している。
ローカル位置獲得部25は、アンテナ1あるいはこれを搭載した移動体のローカル座標系上での位置情報(概略位置を含む)を獲得する。その構成としては、例えばGPS測位衛星からの電波を受信して自己の位置座標を得る公知のGPSレシーバと同様な機能を有する測位演算部と、測位衛星からの電波を受信して測位計算した結果を順次記憶するメモリを備える。これにより、ローカル位置獲得部25は、4つ以上の測位衛星からの電波を受信しやすい通信環境にいるときに、これら4つ以上の測位衛星からの電波を受信して前記測位演算部が測位計算した結果を前記メモリに順次記憶しておく。
衛星位置獲得部26は、測位衛星からの電波を受信する受信部を備え、測位衛星から送信される航法メッセージを受信して当該衛星の軌道パラメータを抽出し、ある時刻でのローカル座標系上の衛星位置を算出する。
衛星方向獲得手段2では、例えば上記実施の形態2で示した図7(b)に従って、ローカル位置獲得部26が取得したアンテナ1あるいはこれを搭載した移動体のローカル座標系上での位置情報と、衛星位置獲得部26が取得する天頂あるいは測位衛星の位置とに基づいて、既存の衛星方向算出技術に従いローカル座標系上の衛星方向を近似的に求める。
このとき、2つあるいは3つの衛星からしか衛星電波を受信できない場合、衛星方向獲得手段2は、前述のようにしてローカル位置獲得部25がメモリに記憶しておいた測位結果を、アンテナ1あるいはこれを搭載した移動体のローカル座標系上での概略の位置として利用してローカル座標系上の衛星方向を求める。また、位置演算手段8を備える場合は、衛星方向獲得手段2が、位置演算手段8の出力を利用してローカル座標系上での衛星方向を求めるようにしてもよい。
以上のように、この実施の形態4によれば、上記実施の形態3のように衛星や移動体の位置を外部サーバから取得するのではなく、当該外部サーバにおける衛星や移動体の位置の算出機能を具現化するローカル位置獲得部25と衛星位置獲得部26を設けたので、当該外部サーバとの通信が困難な通信環境下においても移動体の姿勢が検出できる。
なお、上記実施の形態による衛星位置獲得部26は、図16に示すように前記受信部としてアンテナ1を利用する構成であってもよい。この場合、衛星位置獲得部26は、図中に破線で示すように、測位衛星からの電波をアンテナ1のいずれかの受信要素で受信した信号を復調・復号して航法メッセージを取得する。そして、この航法メッセージにおける軌道パラメータや衛星時刻などを基に、地球中心・地球固定直行座標系上の測位衛星の位置を算出する。これが衛星の概略位置となる。
このように構成することで、衛星位置獲得部26が、通常のGPS受信装置における衛星ごとの受信チャンネルと同様の動作をする。しかしながら、衛星位置獲得部26による処理の目的は、衛星方向獲得手段2で衛星方向を求めるための衛星方向ベクトルの一端を求めることであるから、正確な衛星時刻を必ずしも要さない点が通常のGPS受信装置とは大いに異なる。
言い換えると、2衛星からしか電波を利用することができない場合を重視しているので、通常必要となる4衛星による測位結果、つまりアンテナ位置と受信機側の時計誤差(時刻補正に反映)を常時取得することは前提にしていない。
これは、受信機時刻が補正されない場合であっても受信機時刻は正しいものとして衛星位置を算出することを意味する。従って、算出された衛星位置は必ずしも正確ではないことになるが、アンテナ位置を概略の位置で代替しても衛星方向に与える誤差はほとんど無視できる程度であるのと同様に、衛星位置も概略の衛星位置で代替させることができるため、本実施の形態による移動体の姿勢検出に何ら問題を与えない。
例えば、測位衛星の場合、約4Km/秒で軌道上を周回しているが、受信側の時計が仮に1秒程度狂っていたとしても、4Km程度の位置ずれ誤差が含まれるだけである。この誤差の衛星方向への影響は、測位衛星高度を約20000Kmとしての次の計算結果が示すように、0.01度程度と極めて軽微である。
tan-1(4K/20000K)(rad)
≒tan-1(1/5000)(rad)≒0.01(度)
また、上記実施の形態によるローカル位置獲得部25は、図17に示すように、前述したメモリとして候補位置記憶部27を設け、これに記憶された位置情報のうち姿勢検出に利用する位置情報を選択する候補選択部28を設けた構成であってもよい。この候補選択部28に、ユーザからの設定情報を受け付ける、表示装置や入力装置を用いる外部インタフェース部(不図示)を設け、ユーザからの位置情報候補の指定情報に応じて候補選択部28が候補位置記憶部27に予め記憶しておいた位置情報を読み出すように構成する。
例えば、候補選択部28が外部インタフェース部を介して表示装置に位置情報の候補をインデックスとして表示する。これに基づいて、ユーザが位置情報候補の指定情報を入力し、この指定情報で選択指定されたインデックスに対応して、候補選択部28が、候補位置記憶部27の内容を読み出す。このようにして、候補位置記憶部27から読み出された位置情報は、衛星方向獲得手段2が衛星方向を求めるためのアンテナ1あるいはこれを搭載した移動体の位置として使用される。また、この位置は、上記実施の形態1で示したアンテナ座標系の原点P0として扱われる。
最も単純な例では、測位衛星高度に対する位置誤差が許容範囲内となる位置に関する情報として、仕向け地別に代表的な位置を候補位置記憶部27に記憶しておき、随時それを候補位置として読み出して使用するようにしてもよい。また、所定の番号に対応させて候補位置記憶部27に記憶した候補位置を、前記番号の指定に応じて切り替えて指定するディップスイッチを外部インタフェース部として構成することで、ユーザあるいはディーラが、利用地域に相当する番号に対応させて候補位置記憶部27に記憶した候補位置を当該ディップスイッチで切替えて指定するようにしてもよい。
また、位置演算手段8が、移動体の位置を算出するたびにメモリ(不図示)に適宜記憶してその内容を更新するようにしておき、最近に記憶された内容をアンテナ1あるいはこれを搭載した移動体の概略の位置として衛星方向情報の算出に用いるようにしてもよい。さらに、別個にナビゲーション装置を備え、これが求めた移動***置を衛星方向情報を求めるのに使用する構成にしてもよい。
この他、測位衛星が4つ以上受信できているときに周知の方法でアンテナ1あるいはアンテナ1を搭載したレシーバの現在位置を算出し、これを移動体の姿勢推定に用いるようにしても良い。この場合、例えば基線ベクトル検出手段3の位相検出部を、アンテナ1あるいはこれを搭載した移動体の位置を求めるための測位演算に利用する。
実施の形態5.
図18は、この発明の実施の形態5による移動体姿勢検出装置の構成を示す図であり、図1、図4及び図13と同一の符号を付したものは同一又はこれに相当するものである。なお、図18では、上記実施の形態2で示した位置演算手段8とこれに関連する構成要素の図示を省略している。本実施の形態による衛星方向獲得手段2は、ローカル位置獲得部25、衛星位置獲得部26及び無線通信部/放送受信部(通信部)29を含んで構成される。
無線通信部/放送受信部29は、無線通信及び/又は放送受信を実行する通信構成を有し、無線通信又は放送受信により通信接続可能なエリアに存在する位置情報提供サーバ30から当該エリアに対応した位置情報及び/又は天頂衛星あるいは測位衛星の最新の位置情報を受信してローカル位置獲得部25及び衛星位置獲得部26に出力する。
位置情報提供サーバ30は、GPS測位衛星からの電波を受信して自己の位置座標などを得る公知のGPSレシーバを備え、無線通信部/放送受信部29との間で無線通信あるいは放送が可能なエリアに対応した位置情報及び/又は天頂衛星あるいは測位衛星の最新の位置情報などを提供する。つまり、GPSレシーバから受信した各種情報を用いて最新の航法パラメータなどの現在の測位情報を取得する。
また、位置情報提供サーバ30は、ネットワーク23を介して無線通信基地局24a,24b,24c,24dと接続しており、無線通信基地局24a,24b,24c,24d、無線アンテナ19や放送衛星20を経由して無線通信部/放送受信部29と通信する。これにより、衛星方向情報提供サーバ30は、前述の位置情報を無線通信部/放送受信部29が受信可能な所定の通信形態にデコードして、ネットワーク23、無線通信基地局24a,24b,24c,24dなどを介して通信可能領域における移動体からの要求に応答する形で提供する。衛星方向獲得手段2側からみた衛星方向情報の取得動作としては、例えば上記実施の形態2で示した図7(b)、図7(d)、図8(a)、図8(c)に沿うものが考えられる。
衛星位置獲得部26は、測位衛星からの電波を受信する受信部としてアンテナ1を利用してもよい。この場合、衛星位置獲得部26は、図中に破線で示すように、測位衛星からの電波をアンテナ1のいずれかの受信要素で受信した信号を復調・復号して航法メッセージを取得する。そして、この航法メッセージにおける軌道パラメータや衛星時刻などを基に、地球中心・地球固定直行座標系上の測位衛星の位置を算出する。これが衛星の概略位置となる。
例えば、衛星位置獲得部26が、測位衛星からアンテナ1のいずれかの受信要素で受信した信号に基づいて算出した位置情報と、無線通信部/放送受信部29が取得した位置情報とを比較して衛星方向情報の算出に用いる位置情報を補正するように構成しても良い。
無線通信部/放送受信部29の無線通信機能を実現する構成としては、例えば携帯電話やPHSなどの携帯データ通信端末が考えられる。この場合、位置情報提供サーバ30は、図中の無線通信基地局24a,24b,24c,24dに相当する、当該携帯電話の無線通信基地局を介して、衛星位置あるいはアンテナ1の受信要素又はこれを搭載した移動体の位置を近似する概略の位置に相当する所定の代表地点の情報を、当該携帯電話に送信することになる。この概略の位置情報としては、セルラ電話基地局から入手することができる当該基地局の無線通信領域(セル)内の無線通信を中継する無線アンテナの位置に相当する情報などがある。
また、無線通信部/放送受信部29の放送受信機能を実現する構成としては、例えば地域ごとに電波強度に指向性を持たせて放射される衛星放送の放送受信機や、地上放送の放送受信機が挙げられる。この場合、位置情報提供サーバ30は、例えば図中の放送衛星20を介して、衛星位置あるいはアンテナ1の受信要素又はこれを搭載した移動体の位置を近似する概略の位置に相当する所定の代表地点の情報を当該放送受信機に送信することになる。無線通信部/放送受信部29としての放送受信機は、放送衛星20からの受信がある都度、前記位置情報を取得し、この情報を用いて衛星方向獲得手段2が既存の位置情報を更新する。
また、この他、例えば主要な仕向け地が日本国と予め分かっている場合は、日本国内で概略の位置を示す適当な所定の値、例えば東経140度、北緯35度、高度20mといった値を代表的な地点の位置情報としてデフォルトに設定しておく。これにより、ユーザ等によって切替選択が行わなければ、デフォルト値が移動体のローカル座標上の概略の位置として獲得されることになる。
以上のように、この実施の形態5によれば、リアルタイムに衛星位置や移動体の概略位置を取得し、これに基づいて移動体のローカル座標系上の姿勢(特にロール角とピッチ角)を電子的且つリアルタイムに検出することができる。また、リアルタイムに移動体姿勢を求めることで、当該姿勢に基づき移動体が現在移動中の路面傾斜や形状をも電子的且つリアルタイムに検出することができる。さらに、単一の、直視可能確率の高い天頂衛星を用いて、アンテナ1あるいはこれを搭載した移動体の姿勢や、当該移動体の移動中の路面あるいは軌道面の傾斜や形状をリアルタイムに検出することもできる。
また、上記実施の形態による衛星方向獲得手段2が取得する位置情報は、例えば高精度ナビゲーション用の移動***置として利用することができる。従来のGPSナビゲーションシステムでは、少なくとも同時に視野に入る衛星数が4個は必要であったので都会のビル街などでは正確な道を示すことができない場合がしばしばであった。
これに対して、上記実施の形態による移動体姿勢検出装置では、4個以上の衛星からの電波を常時受信して衛星位置及び/又はアンテナ1あるいはこれを搭載した移動体の現在位置を生成する位置情報提供サーバ30からリアルタイムに位置情報を受けるので、前述したような従来の不具合を解消することができる。
実施の形態6.
上記実施の形態では、アンテナ要素1の座標、つまり基線ベクトル原点(始点)を求めるにあたり、搬送波位相未知数(波数アンビギュイティ)を求める等して既知であることを前提にした例を示した。
この実施の形態6は、基線ベクトルの先端(終点)座標の推移(例;2〜3点)を基に、基線ベクトルの始点座標の候補を求め、候補の系列を基に始点座標を推定することにより、波数アンビギュイティ演算を省略したことを最大の特徴とする。具体的には、基線ベクトルの先端座標を求め、その推移を基にして基線ベクトルの始点座標を求める。そして、基線ベクトルの始点と終点の双方の相対的な座標配置から姿勢を算出する。
先行技術や上述した実施の形態では、波数アンビギュイティ演算を予め行う必要があったため、計算負荷が大きく相対的に大きな演算資源を必要であった。また、得られる始点座標候補がシステムノイズの影響を受けやすいためにその信頼度があまり高くない。これに対して、本実施の形態6は、比較的に小さな演算資源を利用できる割に信頼性の高い演算結果が迅速に得られる。
また、実施の形態6では一旦結果が得られた後であれば装置の電源を落とさない限り、保持された演算結果を継続的に利用できる。このため、市街地といった衛星視野の狭いところに移動し受信衛星電波数が4つ未満になっても、受信できた衛星の電波信号のみを使用して、3つなら3次元姿勢が、2つなら2次元姿勢を求めることができる。また、アンテナ要素、つまりアンテナ要素を搭載した移動体が低速度あるいは静止状態であっても問題なくアンテナあるいはアンテナを装着した移動体の姿勢情報が得られる。
図19は、この発明の実施の形態6による移動体の姿勢検出原理を説明するための図であり、2つのアンテナから衛星iまでの距離のアンテナ要素A,B間の差RAB、つまりアンテナ要素A,B間距離の衛星方向成分と、衛星方向余弦h(i)及びアンテナ要素A,B間のベクトル(基線ベクトル)bとの関係を示したものである。ここで、基準とするアンテナ要素Aとは基線ベクトルbの始点座標であり、アンテナ座標系の原点に位置する。また、基準とするアンテナ要素Bとは基線ベクトルbの先端(終点)座標である。h(i)はアンテナから衛星iへの方向余弦であり、RABは基線ベクトルbのh(i)方向成分である。
図20は、この発明の実施の形態6による基線ベクトル検出手段における位相検出部の構成を示す図であり、アンテナ要素A,B間の差RABを求めるための位相検出部、例えばGPS受信機を示している。この位相検出部の前段には、不図示のGPS受信回路が設けられ、既存の典型的なGPS受信機と同様に動作する。つまり、前段部のGPS受信回路は、所定の発信源からの所定周波数の電波を受信し、このGPS受信信号を必要に応じて所定の中間周波数信号にダウンコンバートした後にA/D変換する。
このデジタル中間周波数信号は、図20に示すようなキャリアトラッキングループを主要部とした衛星別チャネル毎の位相検出部に入力される。位相検出部では、キャリアNCO33(数値制御発信器)が生成したレプリカキャリアをcosマップ31及びsinマップ32によりコサイン成分I(基準位相)とサイン成分Q(90度離相)に分け、これらをミキサ34a,34bによりデジタル中間周波信号に掛け合わせたI信号とQ信号を生成する。続いて、I信号及びQ信号が、さらにミキサ35a,35bによって、C/Aコードトラッキングループ36を構成する回路からのC/Aコード信号とそれぞれ乗算される。
ミキサ35a,35bからの出力は、インテグレーション−ダンプ37a,37bにそれぞれ出力され、所定のタイミング期間で積算した一連のI信号及びQ信号が得られる。インテグレーション−ダンプ37a,37bから出力されたI信号及びQ信号は、逆正接演算回路(tan-1)38に入力される。逆正接演算回路(tan-1)38では、入力信号の除算値の逆正接である位相φ=tan-1(Q/I)が求められ、ループフィルタ39に出力される。ループフィルタ39は、入力した位相φに応じた情報M(Nビット)を生成してキャリアNCO33に出力する。キャリアNCO33では、ループフィルタ39からの入力Mに基づいてレプリカキャリアが生成される。
図21は、図20中のキャリアNCOの構成を示す図である。図20に示す位相検出部において、キャリアNCO33から出力されるレプリカキャリア位相情報φ及びレプリカキャリア波数np情報に基づいて搬送波位相Φ(=(np+φ)・λ)が図示しない演算部により算出される。なお、λは搬送波(キャリア)の波長であり、k’はドップラー周波数情報である。
ここで、図21に示すキャリアNCO33の動作例を説明する。
アダー40では、ループフィルタ39からの入力Mが第1レジスタ42の前回のホールド値に加算される。また、第1レジスタ42では、クロック信号(周波数fs)が入力される度に、新たにアダー40の出力が第1レジスタ42にラッチされる。その結果、図21中に示すように、レプリカキャリアの位相出力が階段波形の信号として発生される。この階段波形の1ステップの高さに相当するM値に応じて、Nビットのアダー40、第1レジスタ42がオーバーフローする周期(階段波形、つまり位相情報の周期)が変わる。以上がキャリアNCO33の基本動作である。
また、図21に示すように、キャリアNCO33の基本部分に第2カウンタ45と第2レジスタ44を追加することにより、正規搬送波位相の波数(整数部)npやドップラー周波数情報k’を容易に獲得できることを以下で説明する。
先ず、第1カウンタ43では、クロック周波数fsをカウントし、アダー40のオーバーフロー出力の立ち下がりでクリアされる。また、第2レジスタ44では、オーバーフロー出力の立ち上がりで第1カウンタ43の出力をラッチしてドップラー周波数情報k’を出力する。従って、階段波形の1周期内にクロック周波数fsが幾つカウントされたかが第1カウンタ43でカウントされ、アダー40がオーバフローする度に第2レジスタ44にラッチされ、ドップラー周波数情報k’が更新出力される。
NCO各部の信号例の関係を以下に示す。
(1)中間周波信号の周波数基準値(設定値)fIFは下記式で表される。
fIF=fS/k
なお、fSはクロック周波数(サイクル/秒、設定値)であり、kはレプリカキャリア波形分解能(設定値)である。
(2)中間周波数の実際値(図20に示す回路で観測される値)fIF’は、下記式で表される。
fIF’=fS/k’
なお、k’はドップラー周波数(観測値)である。
(3)中間周波数領域におけるドップラーシフトΔfIFは下記式で表される。
ΔfIF=fIF’−fIF=fS/k’−fIF=fIF・k/k’−fIF
=fIF・(k/k’−1)(Hz)(k’が観測値)
(4)搬送波周波数領域に置けるドップラーシフトΔfは下記式で表される。
Δf=ΔfIF(Hz)
(5)参考として、ローカル発信周波数floに基づいて高周波数frfをダウンコンバートした中間周波数fIF’(ドップラーシフトのある場合)は、下記式で表される。
fIF’=frf’−flo
同じく中間周波数fIF’(ドップラーシフトのない場合)は下記式で表される。
fIF=frf−flo
従って、Δfrf=frf’−frf=fIF’−fIF=ΔfIFとなる。
なお、本実施の形態6による移動体姿勢検出装置の構成は、上記実施の形態1で示した図1と同様であるが、後述するように基線ベクトル検出手段3の基線ベクトル演算部3bによって実施の形態6に特有な基線ベクトル演算処理がなされる。
図22は、実施の形態6による移動体姿勢検出装置の動作を示すフローチャートであり、実施の形態6による基線ベクトル演算部における基線ベクトル演算処理と、これに適した姿勢演算手段における姿勢演算処理の例を示している。なお、これらは、上記実施の形態2で説明した図6におけるステップST18のGPS姿勢処理についての他の処理例に相当する。
また、図22中に太線枠で示した処理ステップは、本実施の形態6において改めて整理して説明する処理(ステップST22−3)又は、実施の形態6に特有な処理(ステップST22−5〜ステップST22−8)である。従って、太線で示したステップについて、順次、詳細に説明する。細線枠で示した処理(ステップST22−1、22−2、22−4、22−9,22−10については、上記実施の形態2で説明した処理とほぼ同様な処理である。
先ず、図19に示した搬送波位相差と方向余弦及び基線ベクトルとの関係を利用して、ステップST22−3の処理を説明する。搬送波位相差と方向余弦及び基線ベクトルとの関係を基本として、様々な組み合わせで実施可能である。例えば、典型的には、基線ベクトル数が1つでアンテナ要素が2つである場合、衛星数、つまりアンテナ1本あたりの方向余弦数は3つである。また、基線ベクトル数が2つでアンテナ要素が3つである場合、衛星数、つまりアンテナ1本あたりの方向余弦数は2つである。さらに、基線ベクトル数が≧3であれば、アンテナ要素も≧3+1、という組み合わせのいずれかが実施される。
なお、衛星数が足りない場合に対応するための近似解法は、上記実施の形態1から5までに示したものを適用し得る。逆に衛星数が多い場合においては、評価関数(例えば、衛星仰角)が大きいものを必要個数(3個)だけ決めても良いし、最少必要個数(3個)以上であった場合は最小二乗誤差計算を行って求めてもよい。ここでは、簡単のため、衛星数が足りない場合に対応させるための構成について説明を行う。
ステップST22−3において、基本ベクトル演算部3aは、図19に示すアンテナ要素A,B間の正確な衛星方向距離差RABを、その方向余弦h(i)と基線ベクトル(つまり、移動体の姿勢)bとを用いて下記式(47)により算出する。なお、アンテナ要素A,Bの設置間隔、つまり基線ベクトル長が短い場合には、衛星iをどちらのアンテナ位置から見ても視線方向は殆ど同じであるので、方向余弦h(i)はアンテナ要素A,Bの両者について同じ値を使用する。
RAB (i)=h(i)・b (47)
しかしながら、上記式(47)中に示すRAB (i)は未知量であるから、代わりに、次に示すSDAB (i)、つまり搬送波位相ΦAと搬送波位相ΦBとの差を利用する。
なお、搬送波位相Φは代わりに、ドップラーシフトΔf(サイクル/秒)、あるいは、擬似距離変化率dR/dt(m/秒)を用いてもよい。ここで、ドップラーシフトΔfを利用する場合の例では、Δf・Δt・λ(m/秒)を基に、積分して距離差を算出する。ただし、λは波長(m)を示す。一方、擬似距離変化率を利用する場合の例では、dR/dt(m/秒)を基に、積分して距離差を算出する。
基線ベクトル検出手段3における位相検出部が出力する搬送波位相Φは、下記式(48)で表される。下記式中の各記号の右上に付した添字(i)は、その記号で示す数量が衛星iについての値であることを示している。また。各記号への右下の添字Xは、その記号で示す数量がアンテナXで受信された値に対応することを示している。
ΦX (i)=RX (i)+esX (i)+erX+NX (i)λ (48)
ec=cΔtsv+E+(cΔttrop+cΔtion)
er=cΔtr+β+mp
ただし、ΦX (i)はアンテナXで受信された搬送波位相φ(波数)・λ(m)である。RX (i)は衛星とアンテナA間の正確な距離(m)、esX (i)は衛星関連誤差(m)である。erXは受信機関連の誤差(m)、NX (i)はアンテナXで受信した搬送波位相における波数アンビギュイティ(整数)である。λはGPS電波の波長(L1電波:0.1903m)である。cは光の速度、Δtrは受信機時計のバイアス誤差、Δtionは対流圏での伝播遅延誤差、Δttropはオゾン層での伝播遅延誤差、Δtsvは衛星時計のバイアス誤差、mpはマルチパス誤差、Eは放送されたephemerisデータの誤差、βは位相トラッキング誤差である。
従って、2つのアンテナ要素Aとアンテナ要素Bとで観測した衛星iからの搬送波位相の差、SDAB (i)=ΦA (i)−ΦB (i)は次のようになる。
SDAB (i)=ΦA (i)−ΦB (i)
=(RA (i)−RB (i))+(esA (i)−esB (i))+(NA (i)−NB (i))λ (49)
ここで、上記式(49)の( )中を、RAB (i)=RA (i)−RB (i)、esAB (i)
=esA (i)−esB (i)、erAB=erA−erB、NAB (i)=NA (i)−NB (i)で表すと、下記式(50)のようになる。
SDAB (i)=ΦA (i)−ΦB (i)
=RAB (i)+erAB+NAB (i) (50)
上記式(50)において、上記式(49)中の衛星要因誤差esAB (i)=esA (i)−esB (i)の項がアンテナ要素A,B間で共通項であるので0となり消去されている。
また、受信機要因誤差erAB=erA−erBの項はクロック誤差が主成分である。従って、受信機のクロックをアンテナ要素A,B間で共通にしていることから、受信機要因誤差は消去されるので、以後この項を無視する。
SD
AB (i)を利用することを考えると、上記式(47)は下記式(51)のように表せる。
SD
AB (i)=Φ
A (i)−Φ
B (i)=R
AB (i)+N
AB (i)λ=h(i)・b (51)
さらに、移動体の姿勢ベクトルである基線ベクトルbの3次元先端座標は、3つの未知要素x,y,zで構成されている。従って、以降では、3つのGPS衛星からの航法電波を一対のアンテナで同時に受信して、3衛星別の受信機間位相差についての3元一次連立方程式をたてて、これを姿勢ベクトルの先端座標を求める下記式(52)に従って解く。なお、説明の便宜上、下記式では姿勢ベクトルの先端座標をbとしている。
但し、R
AB (i)の代わりに用いたSD
AB (i)は上記式(51)で表せるので、アンテナ間差の衛星方向成分の正しい値R
AB (i)にN
AB (i)λだけオフセットが加わったものである。
また、上記式(52)を詳細に示せば、下記式(53)のように表せる。
実施の形態6において、電源が維持されている限りあるいは受信が継続している限り、時々電波受信が途切れるようなことがあっても、Nは一定であるし、方向余弦hも衛星が遠方にあるために短かい期間内ではほんのわずかしか変化しないので、上記式(53)の右辺第2項は、略一定の値をとることになる。つまり、基線ベクトルbの先端座標は中心が一定値だけオフセットした半径R(基線長)の球面上に分布した軌跡を描くことが推定できる。実際には除去できなかった受信機固有の残留ノイズ誤差wなどが残っているので、球面上を中心としてランダムに分布すると考えられる。
本実施の形態6では、上述のような未知の量が短期間内で一定のオフセットと見なせることを、図22に示したフローチャート中の破線で囲った部分の処理(ステップST22―5からステップST22−8まで)で利用して基線ベクトルの始点座標を求める。なお、これらの処理は、基線ベクトル検出手段3内の基線ベクトル始点推定手段(不図示)が実行するものであり、説明の便宜上基線ベクトル検出手段3による処理として説明する。
以上の説明ではクロックを共通化した場合について示したが、クロックを共通化してない複数のGPS受信機を利用して系を構成したような場合、上記式(53)で求めた基線ベクトルbは受信機クロック誤差を含んでいる。これに対し、受信機クロック誤差を除去することを目的として、先に示した受信機間差SDの、さらに衛星間での差(受信機間衛星間2重差)を求めてもよい。この場合は、汎用のGPS受信機を利用できるメリットがあるが、衛星数が1つ余分に必要となるデメリットもある。2重位相差により受信機要因誤差が除去されることの説明は発明の趣旨説明からは外れるために省略する。
次に、図22に示したフローチャート中の破線で囲った部分の処理(ステップST22―5からステップST22−8まで)である、図19中に示した基線ベクトルの始点座標の推定処理について述べる。図23及び図24は基線ベクトルの始点候補座標の算出アルゴリズムの説明図である。
ステップST22−5において、基線ベクトル検出手段3は、角速度が一定以上か否かを終点座標のΔt秒毎の推移を基に判定する。具体的には、図23に示すように、終点推移ベクトルの前回値vec1の絶対値、そして終点座標の推移値ベクトルの今回値vec2の絶対値が、共に所定値以上であるか否か評価する。つまり、例えばΔθxy/Δtが0.005/R(Rはアンテナ間基線長で例えば0.6m)よりも大きいxy面上の角速度で、基線ベクトル終点座標が移動し続けた場合に限って基線ベクトルの始点座標を算出する。
ステップST22−6において、基線ベクトル検出手段3は、ステップST22−5における角速度が一定以上であると、先端座標推移を基に始点候補座標を算出する。具体的には、観測値を基に算出した基線ベクトル先端座標の系列(p1,p2,p3,・・・)を基に、順次基線ベクトルの始点座標を下記のようにして求める。但し、p1は基線ベクトルの終点の前々回値(2Δt秒前)であり、p2は基線ベクトルの終点の前回値(Δt秒前)であり、p3は基線ベクトルの終点の今回値(現在)である。
新たな搬送波位相の観測値diを、例えば1秒ごとに得る度に、その観測値を含めた3連続の観測値(di,di-1,di-2)について、基線ベクトルの先端(終点)座標p1,p2,p3を求め、さらに、その移動(変分)ベクトルvec1、vec2を求める。vec1とvec2を含む面の法線を得るために、vec1とvec2の両者に直交するベクトルm1(=vec1・vec2)(vec1とvec2との外積ベクトルの方向余弦)を求める。なお、・は外積を示している。
次に、ベクトルm1に平行な方向余弦h1(=m1/|m1|)を求める。三角形の頂点p2が上(z軸の正方向)に凸かあるいは下(z軸の負方向)に凸か(vec1z/|vec1xy|とvec2z/|vec2xy|との比較結果)に応じて、h1の極性を変更(−1をかける)する(図24参照)。なお、ベクトルvec1zは終点推移ベクトルの前回値のz方向成分であり、ベクトルvec2zは終点推移ベクトルの今回値のz方向成分である。
続いて、三角形の頂点p1,p2,p3の重心o1{=(p1+p2+p3)/3}を求める。ここで、重心o1を始点とし、図23に示すように、o1からの距離がアンテナ要素の設置間隔R(既知量)だけ離れたh1方向の点c1を終点とする半径ベクトルr1(基線ベクトルノルム*m1)を求める。半径ベクトルr1に始点ベクトルo1を加えて、未知座標上の基線ベクトル始点候補座標c1を求める。なお、ステップST22−6において、求めた終点座標の水平2次元座標系における2次元の今回値と前回値を基に該水平2次元座標系上の始点座標候補を逐次求めてもよい。
ステップST22−7において、基線ベクトル検出手段3は、ステップST22−6に続けて、始点候補座標値が妥当な範囲、具体的には、それまでに推定した始点座標からの推移が基線ベクトル長以内に入っているかを評価する。ステップST22−8において、基線ベクトル検出手段3は、始点候補座標が妥当な範囲内のものであったら、始点候補座標を基に始点座標を推定し、次回更新まで保持する。始点座標推定の動作・作用の具体例は次のようである。
図23に示したような始点候補座標c1を得る度に、例えばc1時系列の分散を移動的に評価して、分散がより小さい場合ほどc1による寄与が大きく、逆に分散が大きいほどc1による寄与が小さくなるように、また処理の経過と共に言わば観測値であるc1の寄与が小さくなるように、前回求めた始点座標推定値C1を変更する(カルマン推定)。
ステップST22−9において、姿勢演算手段4は、推定処理条件が満たされない場合、それまでに求めた基線ベクトルbの始点推定値Cに基づいて姿勢を逐次算出する。具体的に説明すると、基線ベクトルの先端座標P1と平滑化始点座標値C1とを基にして方位を、atan((P1x−Cx)/(P1y−Cy))・180/π(度)から計算する。結果が負であれば360度を加えて、計算値を補正し、北が0度の時計回りで360度までで表現される方位を得る。同様にして、前後傾斜角を、atan((P1z−Cz/(P1z−Cz))・180/π(度)から計算する。上述のようにして得られた基線ベクトルの始点座標、先端(終点)座標を基に、姿勢演算手段4は、移動体の姿勢を求める(ステップST22−10)。
以上のように、この実施の形態6によれば、比較的に小さな演算資源を利用できる割に信頼性の高い演算結果を迅速に得ることができる。また、一旦結果が得られた後は装置電源を落とさない限り、保持された演算結果を継続的に利用できる。このため、市街地のような衛星視野の狭いところに移動して受信衛星電波数が4つ未満となっても、受信できた衛星の電波信号のみを使用して、3つなら3次元姿勢を、2つなら2次元姿勢を求めることができる。さらに、アンテナ要素(を搭載した移動体)が低速度あるいは静止状態であっても問題なく、アンテナあるいはアンテナを装着した移動体の姿勢情報を得ることができる。
1 アンテナ(アンテナ手段)、1−1〜1−3 受信要素、2 衛星方向獲得手段、3 基線ベクトル検出手段、3−1,3−2 基線ベクトル検出部、3−1−1〜3−1−3 位相検出部、3a 衛星選択部、3b 基線ベクトル演算部、4 姿勢演算手段、5 角速度検出手段、6 INS姿勢演算手段、7 速度検出手段、8 位置演算手段、9 提示/制御手段、10 ジャイロ、11 A/D変換器、12 固定オフセット補償部、13,14,17 演算器、15 積分器、16 補償演算部、18,29 無線通信部/放送受信部(通信部)、19 無線アンテナ、20 放送衛星、21 レシーバ、22 衛星方向情報提供サーバ、23 ネットワーク、24a,24b 無線通信基地局、25 ローカル位置獲得部、26 衛星位置獲得部、27 候補位置記憶部、28 候補選択部、30 位置情報提供サーバ。