JP5430172B2 - 方位算出装置、方位算出装置の方位算出方法および方位算出プログラム - Google Patents

Description

本発明は、例えば、移動体の方位の初期値を算出する方位算出装置、方位算出装置の方位算出方法および方位算出プログラムに関するものである。

ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）のみを利用して姿勢方位を決定する従来技術では、アンテナ間の基線長が長い場合や、搬送波位相の観測ノイズが大きい廉価な受信機を使用した場合に、ミスＦＩＸ率が増加したり、ＦＩＸ解が得られなかったりする。特に、可視衛星数が少ないときに起こりやすい。

特開２００７−１０１４８４号公報 特開２００７−１６３３３５号公報

本発明は、例えば、可視衛星数が少なくても正しい姿勢方位を少ない処理数で測定できるようにすることを目的とする。

本発明の方位算出装置は、対象物に設置される加速度計により計測された加速度値に基づいて、前記対象物に設置される第１のＧＰＳアンテナから前記対象物に設置される第２のＧＰＳアンテナへの線分を基線として基線ベクトルを表す三次元の成分のうち基線ベクトルの鉛直成分をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する鉛直成分算出部と、前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と前記第１のＧＰＳアンテナにより観測される搬送波位相と前記第２のＧＰＳアンテナにより観測される搬送波位相とに基づいて、搬送波の整数値バイアスの二重差をＣＰＵを用いて算出する整数値バイアス算出部と、前記整数値バイアス算出部により算出された搬送波の整数値バイアスの二重差に基づいて、前記基線ベクトルを表す三次元の成分のうち前記基線ベクトルの鉛直成分を含まない前記基線ベクトルの二次元成分をＣＰＵを用いて算出する二次元成分算出部と、前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と前記二次元成分算出部により算出された前記基線ベクトルの二次元成分とに基づいて、前記対象物の方位角をＣＰＵを用いて算出する方位算出部とを備える。

本発明によれば、例えば、可視衛星数が少なくても正しい姿勢方位を少ない処理数で測定できる。

実施の形態１におけるＩＡＧＰＳ１００の構成図。 実施の形態１における姿勢方位決定装置２００のハードウェア資源の一例を示す図。 実施の形態１における姿勢方位標定方法を示すフローチャート。 実施の形態１における姿勢方位決定装置２００の機能構成図。 実施の形態１における姿勢方位決定処理（Ｓ２００）のフローチャート。 実施の形態１における３軸加速度値１３９と加速度姿勢値（φ_Ｉ、θ_Ｉ）との関係図。 実施の形態１における加速度姿勢値（φ_Ｉ、θ_Ｉ）と基線ベクトル鉛直成分値ｂ^ｄ _ａｂとの関係図。 実施の形態１におけるＧＰＳアンテナ−ＧＰＳ衛星間の距離ρと基線ベクトルｂとの関係図。 実施の形態１における基線ベクトル探索の概念図。

実施の形態１．
特定体（移動体、停止物）にＧＰＳアンテナおよび加速度計を設置し、ＧＰＳアンテナの観測データと加速度計の計測値とに基づいて特定体の姿勢方位（回転角［ロール角］、仰角［ピッチ角］、方位角［ヨー角］）を決定する形態の一例を説明する。

図１は、実施の形態１におけるＩＡＧＰＳ１００の構成図である。
実施の形態１におけるＩＡＧＰＳ１００の構成について、図１に基づいて以下に説明する。

ＩＡＧＰＳ１００（Ｉｎｅｒｔｉａ Ａｉｄｅｄ ＧＰＳ）は、プラットフォーム１０１と姿勢方位決定装置２００（方位算出装置の一例）とを有する。

プラットフォーム１０１は、３つのＧＰＳ（ＧＰＳアンテナ＋ＧＰＳ受信機）、３軸加速度計１３０および３軸ジャイロ１４０を備え、姿勢方位を決定したい移動体（図示省略）に設置（固定）される。プラットフォーム１０１は剛体であって変形せず、プラットフォーム１０１に備わる各機器の相対位置は変わらないものとする。
例えば、プラットフォーム１０１は、車両（移動体の一例）の天板に設置される。

各ＧＰＳアンテナ（１１１、１１２、１１３）は、可視であるＧＰＳ衛星（図示省略）から到達した搬送波および当該搬送波の位相（以下、「搬送波位相」という）を観測（検出）する。搬送波は、複数存在するＧＰＳ衛星（可視、不可視ともに）それぞれから発信され、ＧＰＳ衛星の軌道情報や搬送波の発信時刻（ＧＰＳ衛星の内部時計に基づく時刻）などの情報を搬送する。搬送波位相は、ＧＰＳ衛星からＧＰＳアンテナまでの距離内に収まる搬送波の個数（整数値＋小数値）の小数値に相当する。搬送波の個数の整数値に相当するものを「整数値バイアス」という。
各ＧＰＳアンテナは１直線状に並ばないように配置されている。
各ＧＰＳアンテナはいずれかのＧＰＳ受信機と接続している。

各ＧＰＳ受信機（１２１、１２２、１２３）は、自己と接続するＧＰＳアンテナにより検出された搬送波から当該搬送波により搬送された情報を取得し、取得した情報に基づいて当該ＧＰＳアンテナの位置を測位する。

３軸加速度計１３０は、任意の座標系（以下、「本体座標系」という）を表す３軸（ｘ、ｙ、ｚ）それぞれの方向におけるプラットフォーム１０１の加速度を計測する。
３軸ジャイロ１４０は、本体座標系を表す３軸それぞれの方向におけるプラットフォーム１０１の角速度を計測する。
例えば、本体座標系は、プラットフォーム１０１の長手方向を「ｘ軸」、プラットフォーム１０１の幅方向を「ｙ軸」、プラットフォーム１０１のｘｙ平面と直交する方向を「ｚ軸」として表される。

姿勢方位決定装置２００は、各ＧＰＳアンテナにより検出された搬送波位相、各ＧＰＳ受信機により搬送波から取得された情報、各ＧＰＳ受信機により測位されたＧＰＳアンテナの位置、３軸加速度計１３０により計測されたプラットフォーム１０１の加速度および３軸ジャイロ１４０により計測されたプラットフォーム１０１の角速度に基づいて、プラットフォーム１０１を設置された移動体の姿勢方位を決定する。

以下、ＧＰＳアンテナａ１１１を主局「ａ」、ＧＰＳアンテナｂ１１２を従局（１）「ｂ」、ＧＰＳアンテナｃ１１３を従局（２）「ｃ」とする。
また、主局ａから従局ｂへの線分のベクトルを基線ベクトル「ｂ_ａｂ」、主局ａから従局ｃへの線分のベクトルを基線ベクトル「ｂ_ａｃ」とする。
また、本体座標系（ｘｙｚ座標系）における基線ベクトルを「ｂ^ｂ _ａｂ」「ｂ^ｂ _ａｃ」と記し、姿勢方位決定装置２００により決定する移動体の姿勢方位を表す座標系（以下、「航法座標系」という）における基線ベクトルを「ｂ^ｎ _ａｂ」「ｂ^ｎ _ａｃ」と記す。
本体座標系の基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）は、予め計測されているものとする。

図２は、実施の形態１における姿勢方位決定装置２００のハードウェア資源の一例を示す図である。
図２において、姿勢方位決定装置２００は、ＣＰＵ９１１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）（マイクロプロセッサ、マイクロコンピュータともいう）を備えている。ＣＰＵ９１１は、バス９１２を介して通信ボード、表示装置、キーボード、マウス、ドライブ装置、プリンタ装置（以上、図示省略）、記憶装置９２０と接続され、これらのハードウェアデバイスを制御する。ドライブ装置は、ＦＤ（Ｆｌｅｘｉｂｌｅ Ｄｉｓｋ）、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｃ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）などの記憶媒体を読み書きする装置である。ＲＯＭ、ＲＡＭ、磁気ディスク装置は、記憶装置９２０の一例である。

通信ボードは、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。

記憶装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。

プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９１１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。

ファイル群９２４には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
例えば、ファイル群９２４には、各ＧＰＳアンテナにより検出された搬送波位相、各ＧＰＳ受信機により搬送波から取得された情報、各ＧＰＳ受信機により測位されたＧＰＳアンテナの位置（以下、「ＧＰＳ観測データ１２９」という）が含まれる。
また、ファイル群９２４には、３軸加速度計１３０を構成するｘ軸加速度計１３１、ｙ軸加速度計１３２およびｚ軸加速度計１３３それぞれにより計測されたプラットフォーム１０１の加速度を示す値（以下、「３軸加速度値１３９」という）が含まれる。
また、ファイル群９２４には、３軸ジャイロ１４０を構成するｘ軸ジャイロ１４１、ｙ軸ジャイロ１４２およびｚ軸ジャイロ１４３それぞれにより計測されたプラットフォーム１０１の角速度を示す値（以下、「３軸角速度値１４９」という）が含まれる。
また、ファイル群９２４には、姿勢方位決定装置２００によりＧＰＳ観測データ１２９と３軸加速度値１３９とに基づいて決定される停止時の移動体の姿勢方位の値（以下、「姿勢方位初期値２９１」という）、姿勢方位決定装置２００により姿勢方位初期値２９１とＧＰＳ観測データ１２９と３軸角速度値１４９とに基づいて標定される移動開始後の移動体の姿勢方位の値（以下、「姿勢方位値２９２」という）が含まれる。

実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。

実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。

図３は、実施の形態１における姿勢方位標定方法を示すフローチャートである。
実施の形態１における姿勢方位標定方法について、図３に基づいて以下に説明する。

まず、プラットフォーム１０１を設置された移動体の停止時に、各ＧＰＳ（ＧＰＳアンテナ＋ＧＰＳ受信機）によりＧＰＳ観測データ１２９を取得し、３軸加速度計１３０により３軸加速度値１３９を取得する（Ｓ１００）。

そして、姿勢方位決定装置２００は、Ｓ１００において取得されたＧＰＳ観測データ１２９と３軸加速度値１３９とに基づいて、移動体の姿勢方位初期値２９１を決定する（Ｓ２００：姿勢方位決定処理）。
姿勢方位決定処理（Ｓ２００）の詳細については後述する。

移動体の移動開始後、各ＧＰＳによりＧＰＳ観測データ１２９を取得し、３軸ジャイロ１４０により３軸角速度値１４９を取得する（Ｓ３００）。

姿勢方位決定装置２００は、姿勢方位初期値２９１、ＧＰＳ観測データ１２９および３軸角速度値１４９に基づいて移動体の姿勢方位値２９２を標定する（Ｓ４００：姿勢方位標定処理）。

姿勢方位標定処理（Ｓ４００）として、例えば、特許文献２に開示されている技術「姿勢標定方法」を用いることができる。

上記姿勢方位標定方法により、移動体の姿勢方位初期値２９１を決定し（Ｓ１００〜Ｓ２００）、姿勢方位初期値２９１を用いて移動体の移動開始後の姿勢方位値２９２を標定することができる（Ｓ３００〜Ｓ４００）。

図４は、実施の形態１における姿勢方位決定装置２００の機能構成図である。
実施の形態１における姿勢方位決定装置２００の機能構成について、図４に基づいて以下に説明する。

姿勢方位決定装置２００（方位算出装置の一例）は、加速度姿勢算出部２１０、基線ベクトル鉛直成分算出部２１１（鉛直成分算出部の一例）、整数値バイアス候補算出部２２０（プライマリ衛星選択部、整数値バイアス算出部の一例）、基線ベクトル候補算出部２３０（二次元成分算出部の一例）、基線ベクトル残差検定部２４０（二次元成分二次候補選択部の一例）、基線ベクトルなす角検定部２５０（二次元成分三次候補の一例）、方向余弦行列候補算出部２６０、方向余弦行列姿勢検定部２６１（方位算出部、姿勢方位決定部の一例）、姿勢方位標定部２７０および装置記憶部２９０を備える。

装置記憶部２９０は、各「〜部」で使用される各種データを記憶媒体を用いて記憶する。
例えば、装置記憶部２９０には、移動体の停止時および移動体の移動開始後に各ＧＰＳにより取得されたＧＰＳ観測データ１２９、移動体の停止時に３軸加速度計１３０により取得された３軸加速度値１３９および移動体の移動開始後に３軸ジャイロ１４０により取得された３軸角速度値１４９が記憶される。

加速度姿勢算出部２１０は、３軸加速度値１３９に基づいて移動体の姿勢値（ロール角φ_Ｉ、ピッチ角θ_Ｉ）を「加速度姿勢値」としてＣＰＵを用いて算出する。

基線ベクトル鉛直成分算出部２１１は、加速度姿勢算出部２１０により算出された加速度姿勢値（φ_Ｉ、θ_Ｉ）に基づいて、航法座標系におけるプラットフォーム１０１の基線ベクトル（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を表す三次元成分のうち鉛直成分（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を「基線ベクトル鉛直成分値」としてＣＰＵを用いて算出する。

プラットフォーム１０１の各ＧＰＳアンテナにおいて、４機以上のＧＰＳ衛星から搬送波（および搬送波位相）を観測できるものとする。
整数値バイアス候補算出部２２０は、プラットフォーム１０１の各ＧＰＳアンテナにより搬送波位相を観測される４機以上のＧＰＳ衛星のうち３機のＧＰＳ衛星を「プライマリ衛星」としてＣＰＵを用いて選択する。
整数値バイアス候補算出部２２０は、基線ベクトル鉛直成分算出部２１１により算出された基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）と各ＧＰＳアンテナにより観測された各プライマリ衛星からの搬送波の搬送波位相に基づいて、搬送波の整数値バイアスの二重差の候補（以下、「整数値バイアス候補（∇ΔＮ）」という）をＣＰＵを用いて複数算出する。搬送波位相はＧＰＳ観測データ１２９に含まれる。

基線ベクトル候補算出部２３０は、整数値バイアス候補算出部２２０により算出された複数の整数値バイアス候補（∇ΔＮ）それぞれに基づいて、基線ベクトル（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を表す三次元の成分のうち基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を含まない基線ベクトルの二次元成分の候補を二次元成分候補としてＣＰＵを用いて複数算出する。
以下、基線ベクトル鉛直成分算出部２１１により算出された基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）と基線ベクトル候補算出部２３０により算出された二次元成分候補との組み合わせにより表される基線ベクトル（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を「基線ベクトル一次候補」という。

基線ベクトル残差検定部２４０は、プライマリ衛星以外のＧＰＳ衛星を「セカンダリ衛星」とし、各ＧＰＳアンテナにより観測されたセカンダリ衛星からの搬送波の搬送波位相に基づいて、複数の基線ベクトル一次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）から「基線ベクトル二次候補」をＣＰＵを用いて選択する。

基線ベクトルなす角検定部２５０は、本体座標系（ｘｙｚ座標系）で予め計測された基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）に基づいて、基線ベクトル二次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）から「基線ベクトル三次候補」をＣＰＵを用いて選択する。

方向余弦行列候補算出部２６０は、基線ベクトルなす角検定部２５０により選択された基線ベクトル三次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）に基づいて、本体座標系の値を航法座標系の値に変換する方向余弦行列（Ｃ^ｎ _ｂ）の候補（以下、「方向余弦行列候補」という）をＣＰＵを用いて算出する。

方向余弦行列姿勢検定部２６１は、方向余弦行列候補算出部２６０により算出された方向余弦行列候補（Ｃ^ｎ _ｂ）と加速度姿勢算出部２１０により算出された加速度姿勢値（φ_Ｉ、θ_Ｉ）とに基づいて、姿勢方位初期値２９１をＣＰＵを用いて算出する。

姿勢方位標定部２７０は、姿勢方位初期値２９１、ＧＰＳ観測データ１２９および３軸角速度値１４９に基づいて、移動開始後の移動体の姿勢方位値２９２をＣＰＵを用いて算出する（Ｓ４００：姿勢方位標定処理）。
例えば、姿勢方位標定部２７０は、特許文献１に開示されている技術により、姿勢方位値２９２を算出する。

図５は、実施の形態１における姿勢方位決定処理（Ｓ２００）のフローチャートである。
姿勢方位標定方法（図３参照）において姿勢方位初期値２９１を決定する姿勢方位決定処理（Ｓ２００）について、図５に基づいて以下に説明する。
姿勢方位決定装置２００の各「〜部」は、以下に説明する処理（Ｓ２１０〜Ｓ２６１）をＣＰＵを用いて実行する。

以下、ＮＥＤ（Ｎｏｒｔｈ Ｅａｓｔ Ｄｏｗｎ）座標系（局所水平座標系の一例）を航法座標系として説明する。

加速度姿勢算出部２１０は加速度姿勢値（φ_Ｉ、θ_Ｉ）を算出し（Ｓ２１０）、基線ベクトル鉛直成分算出部２１１は加速度姿勢値（φ_Ｉ、θ_Ｉ）に基づいて基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を算出する（Ｓ２１１）。
整数値バイアス候補算出部２２０は基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を用いて整数値バイアス候補（∇ΔＮ）を算出する（Ｓ２２０）。
基線ベクトル候補算出部２３０は整数値バイアス候補（∇ΔＮ）に基づいて基線ベクトル一次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を算出し（Ｓ２３０）、基線ベクトル残差検定部２４０は二重差残差検定により基線ベクトル二次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を選定し（Ｓ２４０）、基線ベクトルなす角検定部２５０は（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）のなす角に基づいて基線ベクトル三次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を選定する（Ｓ２５０）。
方向余弦行列候補算出部２６０は基線ベクトル三次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）に基づいて方向余弦行列候補（Ｃ^ｎ _ｂ）を算出し（Ｓ２６０）、方向余弦行列姿勢検定部２６１は加速度姿勢値（φ_Ｉ、θ_Ｉ）に基づいて方向余弦行列（Ｃ^ｎ _ｂ）を決定し、決定した方向余弦行列（Ｃ^ｎ _ｂ）に基づいて算出される移動体の姿勢方位初期値２９１（φ、θ、ψ）を出力する（Ｓ２６１）。

以下に各処理（Ｓ２１０〜Ｓ２６１）の詳細について説明する。

＜Ｓ２１０＞
加速度姿勢算出部２１０は、装置記憶部２９０から３軸加速度値１３９を取得し、取得した３軸加速度値１３９に基づいて静止時の移動体の姿勢値を加速度姿勢値（φ_Ｉ、θ_Ｉ）として算出する。

図６は、実施の形態１における３軸加速度値１３９と加速度姿勢値（φ_Ｉ、θ_Ｉ）との関係図である。
図６において、重力加速度を「ｇ」、ｘ軸加速度値を「Ｉ_ｘ」、ｙ軸加速度値を「Ｉ_ｙ」、ｚ軸加速度値を「Ｉ_ｚ」と記す。

静止している移動体には主に重力加速度ｇがかかっている。
このため、移動体がｘ軸回りに回転している場合、図６（ａ）に示すように、重力加速度ｇとしてｙ軸加速度値Ｉ_ｙおよびｚ軸加速度値Ｉ_ｚが計測される。加速度姿勢値（ロール角φ_Ｉ）はｙ軸加速度値Ｉ_ｙとｚ軸加速度値Ｉ_ｚとの大小比率に基づいて算出される。
また、移動体がｙ軸回りに回転している場合、図６（ｂ）に示すように、重力加速度ｇとしてｘ軸加速度値Ｉ_ｘおよびｚ軸加速度値Ｉ_ｚが計測される。加速度姿勢値（ピッチ角θ_Ｉ）はｚ軸加速度値Ｉ_ｘとｚ軸加速度値Ｉ_ｚとの大小比率に基づいて算出される。

図５に戻り、姿勢方位決定処理（Ｓ２００）の説明を続ける。

Ｓ２１０の後、処理はＳ２１１に進む。

＜Ｓ２１１＞
基線ベクトル鉛直成分算出部２１１は、Ｓ２１０において算出された加速度姿勢値（φ_Ｉ、θ_Ｉ）に基づいて、基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を算出する。

図７は、実施の形態１における加速度姿勢値（φ_Ｉ、θ_Ｉ）と基線ベクトル鉛直成分値ｂ^ｄ _ａｂとの関係図である。
図７において、基線ベクトルｂ^ｂ _ａｂのｙ軸成分を「ｂ^ｙ _ａｂ」、基線ベクトルｂ^ｂ _ａｂのｘ軸成分を「ｂ^ｘ _ａｂ」と記す。

移動体がロール角φ_Ｉ分回転している場合、基線ベクトル鉛直成分値ｂ^ｄ _ａｂはロール角φ_Ｉと基線ベクトルのｙ軸成分ｂ^ｙ _ａｂと図７（ａ）に示すような関係にある。
また、移動体がピッチ角θ_Ｉ分回転している場合、基線ベクトル鉛直成分値ｂ^ｄ _ａｂはピッチ角θ_Ｉと基線ベクトルのｘ軸成分ｂ^ｘ _ａｂと図７（ｂ）に示すような関係にある。
基線ベクトル鉛直成分値ｂ^ｄ _ａｂは、ロール角φ_Ｉ、基線ベクトルのｙ軸成分ｂ^ｙ _ａｂ、ピッチ角θ_Ｉ、基線ベクトルのｘ軸成分ｂ^ｘ _ａｂに基づいて算出される。基線ベクトル鉛直成分値ｂ^ｄ _ａｃも同様である。

図５に戻り、姿勢方位決定処理（Ｓ２００）の説明を続ける。

Ｓ２１１の後、処理はＳ２２０に進む。

＜Ｓ２２０＞
整数値バイアス候補算出部２２０は、Ｓ２１１において算出された基線ベクトル鉛直成分値（ｂ^ｄ _ａｂ、ｂ^ｄ _ａｃ）を用いて整数値バイアス候補（∇ΔＮ）を以下のようにして算出する。

プラットフォーム１０１の各ＧＰＳアンテナにおいて、４機以上のＧＰＳ衛星から搬送波を観測しているものとする。
整数値バイアス候補算出部２２０は、装置記憶部２９０からＧＰＳ観測データ１２９を取得する。ＧＰＳ観測データ１２９には、ＧＰＳアンテナにより観測された搬送波位相、ＧＰＳ受信機による測位結果、各ＧＰＳ衛星の軌道情報などが含まれている。
整数値バイアス候補算出部２２０は、各ＧＰＳアンテナにより搬送波を観測された４機以上のＧＰＳ衛星から３機のＧＰＳ衛星を「プライマリ衛星」として選択する。例えば、整数値バイアス候補算出部２２０は、搬送波が観測されたときの各ＧＰＳ衛星の配置に基づいて、移動体から見て天頂方向で互いに重ならない３機のＧＰＳ衛星を「プライマリ衛星」として選択する。各ＧＰＳ衛星の配置は、ＧＰＳ観測データ１２９に含まれる軌道情報から算出される。
以下、３機のプライマリ衛星を「衛星ｉ」「衛星ｊ」「衛星ｋ」とし、４機目以降のセカンダリ衛星を「衛星ｌ」「衛星ｍ」・・・とする。

以下の各記号において、右上の添え字「ｉ」「ｊ」「ｋ」「ｌ」「ｍ」・・・は対応するＧＰＳ衛星を示し、右下の添え字「ａ」「ｂ」「ｃ」は対応するＧＰＳアンテナを示す。

ＧＰＳアンテナａ１１１により衛星ｉから観測された搬送波位相（に相当する距離）Φ^ｉ _ａは、（式１）で表される。

ＧＰＳアンテナｂ１１２により衛星ｉから観測された搬送波位相Φ^ｉ _ｂ、ＧＰＳアンテナａ１１１により衛星ｊから観測された搬送波位相Φ^ｊ _ａ、ＧＰＳアンテナｂ１１２により衛星ｊから観測された搬送波位相Φ^ｊ _ｂも同様である。

搬送波位相Φ^ｉ _ａと搬送波位相Φ^ｉ _ｂとの差である搬送波位相一重差ΔΦ^ｉ _ａｂは、（式１）に基づいて、（式２）で表される。搬送波位相Φ^ｊ _ａと搬送波位相Φ^ｊ _ｂとの差である搬送波位相一重差ΔΦ^ｊ _ａｂも同様である。
さらに、搬送波位相一重差ΔΦ^ｉ _ａｂと搬送波位相一重差ΔΦ^ｊ _ａｂとの差である搬送波位相二重差∇ΔΦ^ｉｊ _ａｂは、（式２）に基づいて、（式３）で表される。
（式２）（式３）において、「Δ」は一重差を示し、「∇Δ」は二重差を示している。また、受信機ノイズε（Φ_ｒｘ）、マルチパスε（Φ_ｍｕｌｔ）およびアンテナフェーズセンターオフセットε（Φ_ａｎｔ）を纏めて観測誤差ε（Φ）で表している。

ここで、ＧＰＳアンテナ間の距離がそれほど大きくない場合、衛星の軌道誤差ｄρ、電離層遅延ｄ_ｉｏｎおよび対流圏遅延ｄ_ｔｒｏｐ（以上、添え字省略）はＧＰＳアンテナ間でほぼ等しいため、各二重差はほぼ「０」となる。
これにより、（式３）は（式４）で表すことができる。

搬送波位相二重差∇ΔΦ（添え字省略）に観測誤差二重差ε（∇ΔΦ）を含めると、ＧＰＳアンテナからＧＰＳ衛星までの距離の二重差∇Δρ（添え字省略）は、（式４）に基づいて（式５）で表される。

また、距離の二重差∇Δρは、基線ベクトルｂ_ａｂに基づいて、以下のように表される。

図８は、実施の形態１におけるＧＰＳアンテナ−ＧＰＳ衛星間の距離ρと基線ベクトルｂとの関係図である。
図８の関係に基づいて、ＧＰＳアンテナａ１１１から衛星ｉまでの距離ρ_ａ ^ｉとＧＰＳアンテナｂ１１２から衛星ｉまでの距離ρ_ｂ ^ｉとの差を示す距離の一重差Δρ^ｉ _ａｂは、基線ベクトルｂ_ａｂとＧＰＳアンテナａ１１１から衛星ｉへのＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）ベクトルｅ_ａ ^ｉを用いて、（式６）で表される。衛星ｊについての距離の一重差Δρ^ｊ _ａｂも同様である。
そして、Δρ^ｉ _ａｂとΔρ^ｉ _ａｂとの差を示す距離の二重差∇Δρは、（式６）に基づいて（式７）で表される。

したがって、（式５）と（式７）とに基づいて（式８）に示す関係が成り立つ。

（式８）において、搬送波位相二重差∇ΔΦはＧＰＳ観測データ１２９に含まれる搬送波位相Φに基づいて算出される値である。
また、搬送波の波長λは固定値である。例えば、Ｌ１周波数帯（１５７５．４２ＭＨｚ）の搬送波の波長λは「約１９ｃｍ」である。
また、ＬＯＳベクトルｅは、ＧＰＳアンテナの位置とＧＰＳ衛星の位置とに基づいて算出される値であり、ＧＰＳアンテナの位置とＧＰＳ衛星の位置とはＧＰＳ観測データ１２９に含まれる値である。
また、基線ベクトルｂ_ａｂを表す３次元成分のうち鉛直成分は、Ｓ２１１において基線ベクトル鉛直成分値ｂ^ｄ _ａｂとして算出されている。

したがって、（式８）において整数値バイアス二重差∇ΔＮを含んだ中辺と基線ベクトルｂ_ａｂを含んだ右辺との中で未知である値は、整数値バイアス二重差∇ΔＮと基線ベクトルｂ_ａｂの２次元成分（鉛直成分以外）との３つの値である。

（式８）に衛星ｉと衛星ｋとについて距離の二重差∇Δρの関係式を加えると（式９）が生成される。
（式９）は、右辺を航法座標系（ＮＥＤ座標系）の３成分に分けて（式１０）で表し、さらに、右辺を鉛直成分ｂ^ｄ _ａｂとその他の２成分とに分けて（式１１）で表すことができる。

以下、記号「ｗ」「Ａ_ｎｅ」「ｂ_ｎｅ」「ａ_ｄ」を用いて（式１１）を（式１２）で表す。

基線ベクトルｂ_ａｂの鉛直成分ｂ^ｄ _ａｂを除いた二成分を示す行列ｂ_ｎｅは、（式１２）に基づいて（式１３）で表される。

また、基線ベクトルｂ_ａｂの長さＢは（式１４）で表すことができるため、行列ｂ_ｎｅを用いて（式１５）の関係が成り立つ。
（式１３）を用いて（式１５）を展開すると（式１６）になり、（式１６）の行列Ａ_ｎｅについて纏めると（式１７）になる。

（式１７）右辺の行列Ａ_ｎｅを用いた逆行列は対象行列なので、（式１８）のようにコレスキー分解することができる。

ここで、ＮＥＤ座標系における基線ベクトルｂ_ａｂの二成分（ｂ^ｎ _ａｂ、ｂ^ｅ _ａｂ）の代わりとして、任意の座標系における基線ベクトルｂ_ａｂの二成分（ｂ１、ｂ２）を（式１９）で定義する。
（式１９）を（式１２）に基づいて展開すると（式２０）になる。

基線ベクトルｂ_ａｂの長さＢを用いて（式２１）が成り立つから、「ｂ２」は（式２１）を拘束条件として（式２２）で表される。
（式２２）を（式２０）に基づいて展開すると（式２３）となる。

したがって、衛星ｉと衛星ｋとについての整数値バイアス二重差∇ΔＮは、（式２３）に基づいて（式２４）で表される。整数値バイアス二重差∇ΔＮは、（式２４）で表される範囲に含まれる整数のいずれかである。

また、「ｂ１」は、（式２０）により（式２５）で表されると共に、（式２１）により（式２６）で表される。

したがって、衛星ｉと衛星ｊとについての整数値バイアス二重差∇ΔＮは、（式２５）と（式２６）とに基づいて（式２７）で表される。

Ｓ２２０（図５参照）において、整数値バイアス候補算出部２２０は、（式２４）で表される範囲内の全ての整数を整数値バイアス候補∇ΔＮ_ａｂ ^ｉｊとして算出し、（式２７）で表される２つの整数を整数値バイアス候補∇ΔＮ_ａｂ ^ｉｋとして算出する。
同様に、整数値バイアス候補算出部２２０は、整数値バイアス候補∇ΔＮ_ａｃ ^ｉｊと整数値バイアス候補∇ΔＮ_ａｃ ^ｉｋとを算出する。
Ｓ２２０の後、処理はＳ２３０に進む。

＜Ｓ２３０＞
基線ベクトル候補算出部２３０は、Ｓ２２０において算出された複数の整数値バイアス候補∇ΔＮ_ａｂ ^ｉｊと２つの整数値バイアス候補∇ΔＮ_ａｂ ^ｉｋとの全組み合わせを生成する。例えば、整数値バイアス候補∇ΔＮ_ａｂ ^ｉｊの数が「２０個」、整数値バイアス候補∇ΔＮ_ａｂ ^ｉｋの数が「２個」の場合、基線ベクトル候補算出部２３０は４０（＝２０×２）通りの整数値バイアス候補の組み合わせを生成する。
そして、基線ベクトル候補算出部２３０は、（式１２）の定義に基づいて各整数値バイアス候補の組み合わせ（∇ΔＮ_ａｂ ^ｉｊ、∇ΔＮ_ａｂ ^ｉｋ）を（式１３）に代入し、航法座標系における基線ベクトルｂ_ａｂの二成分（ｂ^ｎ _ａｂ、ｂ^ｅ _ａｂ）の候補を算出する。

以下、基線ベクトル鉛直成分値ｂ^ｄ _ａｂと基線ベクトルｂ_ａｂの二成分（ｂ^ｎ _ａｂ、ｂ^ｅ _ａｂ）の各候補とで表される航法座標系における基線ベクトルを「基線ベクトル一次候補」という。
Ｓ２３０において基線ベクトル一次候補ｂ^ｎ _ａｂは、整数値バイアス候補の組み合わせと同じ数算出される。

同様に、基線ベクトル候補算出部２３０は、基線ベクトル一次候補ｂ^ｎ _ａｃを算出する。

Ｓ２３０の後、処理はＳ２４０に進む。

＜Ｓ２４０：二重差残差検定＞
基線ベクトル残差検定部２４０は、Ｓ２３０において算出された基線ベクトル一次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）に対して以下のように二重差残差検定を行い、基線ベクトル二次候補を選定する。

セカンダリ衛星ｌを対象とした距離の二重差∇Δρはプライマリ衛星（ｊ、ｋ）と同様に（式２８）で表され、整数値バイアス二重差∇ΔＮ_ａｂ ^ｉｌは（式２８）に基づいて（式２９）で表される。

まず、基線ベクトル残差検定部２４０は、各基線ベクトル一次候補ｂ^ｎ _ａｂを（式２９）に代入し、整数値バイアス二重差∇ΔＮ_ａｂ ^ｉｌを算出する。整数値バイアス二重差∇ΔＮ_ａｂ ^ｉｌは、基線ベクトル一次候補ｂ^ｎ _ａｂと同じ数算出される。

（式２９）において、搬送波位相二重差∇ΔΦはＧＰＳ観測データ１２９に含まれる搬送波位相Φに基づいて算出される値である。
また、搬送波の波長λは固定値である。
また、ＬＯＳベクトルｅは、ＧＰＳアンテナの位置とＧＰＳ衛星の位置とに基づいて算出される値であり、ＧＰＳアンテナの位置とＧＰＳ衛星の位置とはＧＰＳ観測データ１２９に含まれる値である。

同様に、基線ベクトル残差検定部２４０は、他のセカンダリ衛星についても整数値バイアス二重差を算出する。

次に、基線ベクトル残差検定部２４０は、各整数値バイアス二重差∇ΔＮを用いて（式３０）を計算し、行列ｖ_ｋを算出する。

また、基線ベクトル残差検定部２４０は、ＧＰＳ受信機の観測誤差（受信機ノイズ）の共分散行列Ｃ_ｋを（式３１）により算出する。

次に、基線ベクトル残差検定部２４０は、行列ｖ_ｋと共分散行列Ｃ_ｋとを用いて（式３２）を計算し、行列ｖ_ｋを正規化した統計値Ｔ_ｋを算出する。

ＧＰＳ受信機の観測誤差はランダム誤差（ホワイトノイズ）であるため、行列ｖ_ｋ内の各値の平均値はほぼ「０」になり、行列ｖ_ｋ内の各値は正規分布すると考えられる。
さらに、行列ｖ_ｋを正規化した統計値Ｔ_ｋは、χ^２分布（分散「１」、自由度「ｎ_ｄｄ−３」）（以下同じ）に従うと考えられる。統計値Ｔ_ｋは、観測誤差の影響により当該行列ｖ_ｋの値となる確率を示す。

そこで、基線ベクトル残差検定部２４０は、以下の条件（ａ）を満たす基線ベクトル一次候補ｂ^ｎ _ａｂを基線ベクトル二次候補として選定する。
（ａ）統計値Ｔ_ｋがχ^２分布において所定の有意水準（危険率ともいう）（例えば、１％または５％）以上である。

条件（ａ）を満たさない場合、基線ベクトル一次候補ｂ^ｎ _ａｂが正しいにも関わらず当該行列ｖ_ｋの値となる確率は有意水準未満（例えば、１％未満または５％未満）であり、基線ベクトル一次候補ｂ^ｎ _ａｂが正しくない可能性が高いためである。

同様に、基線ベクトル残差検定部２４０は、基線ベクトル二次候補ｂ^ｎ _ａｃを選定する。

Ｓ２４０の後、処理はＳ２５０に進む。

＜Ｓ２５０：なす角検定＞
基線ベクトルｂ_ａｂと基線ベクトルｂ_ａｃとがなす角は、航法座標系と本体座標系とで等しい。
また、基線ベクトルｂ_ａｂと基線ベクトルｂ_ａｃとがなす角θ_ｂは（式３３）で表される。
そして、航法座標系における基線ベクトル（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）がなす角と本体座標系における基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）がなす角との差Δθ_ｂは、（式３３）に基づいて、（式３４）で表される。

Ｓ２５０において、基線ベクトルなす角検定部２５０は、各基線ベクトル二次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）と予め計測されている本体座標系における基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）とを（式３４）に代入し、なす角の差Δθ_ｂを算出する。
そして、基線ベクトルなす角検定部２５０は、なす角の差Δθ_ｂが所定値以下である基線ベクトル二次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を基線ベクトル三次候補として選定する。
Ｓ２５０の後、処理はＳ２６０に進む。

＜Ｓ２６０＞
本体座標系の値を航法座標系の値に変換する方向余弦行列Ｃ^ｎ _ｂは、（式３５）で表すことができる。

Ｓ２６０において、方向余弦行列候補算出部２６０は、本体座標系における基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｂ _ａｃ）と各基線ベクトル三次候補（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）とを（式３５）に代入し、方向余弦行列Ｃ^ｎ _ｂの候補を算出する。
Ｓ２６０の後、処理はＳ２６１に進む。

＜Ｓ２６１：加速度姿勢検定＞
基線ベクトル（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）が示す移動体の姿勢方位（φ、θ、ψ）は、方向余弦行列Ｃ^ｎ _ｂを用いて（式３６〜３８）で表すことができる。

Ｓ２６１において、方向余弦行列姿勢検定部２６１は、（式３６〜３８）に基づいて方向余弦行列Ｃ^ｎ _ｂの候補毎に移動体の姿勢方位（φ、θ、ψ）を算出する。
そして、方向余弦行列姿勢検定部２６１は、Ｓ２１０において算出された加速度姿勢値（φ_Ｉ、θ_Ｉ）に最も近い姿勢（φ、θ）を含んだ姿勢方位（φ、θ、ψ）を姿勢方位初期値２９１に決定し、決定した姿勢方位初期値２９１を装置記憶部２９０に記憶する。方向余弦行列姿勢検定部２６１は、決定した姿勢方位初期値２９１を所定の出力装置（例えば、表示装置）に出力してもよい。

上記の姿勢方位決定処理（Ｓ２１０〜Ｓ２６１）により、姿勢方位初期値２９１を決定することができる。そして、姿勢方位初期値２９１を元にして、移動開始後の移動体の姿勢方位値２９２を標定することができる（Ｓ４００）。

実施の形態１において、以下のようなＩＡＧＰＳ１００について説明した。
ＩＡＧＰＳ１００は、複数のＧＰＳの搬送波位相（二重位相差）を使用する。
剛体であるプラットフォーム１０１には、３台のＧＰＳアンテナと３軸の加速度センサ（３軸加速度計１３０）とが構成されている。３台のＧＰＳアンテナは、相対位置が変化しないように、また１直線状に並ばないようにプラットフォーム１０１に設置されている。３軸の加速度センサは、プラットフォーム１０１と一緒に動くように取り付けられ、それぞれ直交する３軸の加速度を計測する。
姿勢方位決定装置２００（または、姿勢方位決定装置２００の姿勢方位決定方法）は、このプラットフォーム１０１を固定された対象物の姿勢方位を決定する装置（または、アルゴリズム）である。

このような構成のＩＡＧＰＳ１００には、以下のメリットがある。
１）搬送波の整数値バイアスを３次元の球面近傍の空間からではなく、２次元の円近傍の空間から探索することが出来、整数値バイアスの候補数を少なくすることができる。１）について後述する。
２）１）により計算負荷および処理時間が削減されるため、姿勢方位決定装置２００として使用できる計算機の幅が広がる。つまり、高性能（高価）でない計算機を姿勢方位決定装置２００として使用することができる。
３）１）により誤った整数値バイアス候補を大幅に削減できることから、最終的な解のミスＦＩＸ率（誤った整数値バイアス候補を最終的な解として採用してしまう割合）を削減することが出来る。
４）ＧＰＳの観測データから求めた姿勢角と、加速度計を用いて求めた姿勢角とを比較することで、最終的なミスＦＩＸ率をさらに削減することが出来る（Ｓ２６１参照）。
５）従来のＧＰＳを用いた姿勢決定システムには、ミスＦＩＸをなるべく少なくするためにノイズの少ない専用受信機が使用されることが多い。しかし、実施の形態のＩＡＧＰＳ１００では、３）および４）で述べたようにミスＦＩＸ率を削減できるため、廉価なＧＰＳ受信機を使用することができる。
６）１）により可視衛星が４つあればＧＰＳを用いて姿勢方位を決定することができ、ＧＰＳを用いた姿勢方位決定のアベイラビリティを向上することが出来る。

図９は、実施の形態１における基線ベクトル探索の概念図である。
前記１）のメリットについて、図９に基づいて以下に説明する。

姿勢方位決定処理（Ｓ２００）の説明で述べたように（図５参照）、基線ベクトルは整数値バイアスに基づいて算出される（Ｓ２３０）。
したがって、搬送波の整数値バイアス（二重差）の探索は、基線ベクトルを探索することと等しい。

図９において、従局ｂは、主局ａを中心とし、基線ベクトル（ｂ^ｂ _ａｂ、ｂ^ｎ _ａｂ）の長さを半径とする球面上（またはその近傍）に位置する。
ここで、基線ベクトルｂ^ｎ _ａｂの鉛直成分値ｂ^ｄ _ａｂは、加速度姿勢値に基づいて算出されている（Ｓ２１１）。このため、従局ｂは、主局ａから鉛直成分値ｂ^ｄ _ａｂだけ離れた点を中心とする円周上（またはその近傍）に位置することになる。
つまり、２次元の円近傍の空間を探索することにより、基線ベクトルｂ^ｎ _ａｂの候補を特定することができる。一方、鉛直成分値ｂ^ｄ _ａｂが不明である場合、基線ベクトルｂ^ｎ _ａｂの候補を特定するためには、３次元の球面近傍の空間を探索する必要がある。
例えば、基線ベクトルが「２ｍ」程度である場合、前記Ｓ２２０〜Ｓ２３０と同様にして３次元の球面近傍の空間を探索すると基線ベクトルの候補数は「１３００」ほどになるが、２次元の円近傍の空間を探索する本方式の前記Ｓ２２０〜Ｓ２３０では基線ベクトルの候補数が「４０」程度である。

実施の形態１において、複数の基線ベクトル候補（整数値バイアス候補）から一つの姿勢方位初期値２９１を決定するために行われる二重差残差検定（Ｓ２４０）、なす角検定（Ｓ２５０）および加速度姿勢検定（Ｓ２６１）は、いずれか一つまたは二つが実行されても、実行順序が変わっても、他の検定が加わっても構わない。
例えば、二重差残差検定（Ｓ２４０）においてχ^２分布の確率密度が最も高い統計値Ｔ_ｋに対応する基線ベクトルの組み合わせ（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を選定し、選定した基線ベクトルの組み合わせに基づいて姿勢方位初期値２９１を算出しても構わない。
また、なす角検定（Ｓ２５０）において本体座標系のなす角との差Δθ_ｂが最も小さい基線ベクトルの組み合わせ（ｂ^ｎ _ａｂ、ｂ^ｎ _ａｃ）を選定し、選定した基線ベクトルの組み合わせに基づいて姿勢方位初期値２９１を算出しても構わない。

１００ ＩＡＧＰＳ、１０１ プラットフォーム、１１１ ＧＰＳアンテナａ、１１２ ＧＰＳアンテナｂ、１１３ ＧＰＳアンテナｃ、１２１ ＧＰＳ受信機ａ、１２２ ＧＰＳ受信機ｂ、１２３ ＧＰＳ受信機ｃ、１２９ ＧＰＳ観測データ、１３０ ３軸加速度計、１３１ ｘ軸加速度計、１３２ ｙ軸加速度計、１３３ ｚ軸加速度計、１３９ ３軸加速度値、１４０ ３軸ジャイロ、１４１ ｘ軸ジャイロ、１４２ ｙ軸ジャイロ、１４３ ｚ軸ジャイロ、１４９ ３軸角速度値、２００ 姿勢方位決定装置、２１０ 加速度姿勢算出部、２１１ 基線ベクトル鉛直成分算出部、２２０ 整数値バイアス候補算出部、２３０ 基線ベクトル候補算出部、２４０ 基線ベクトル残差検定部、２５０ 基線ベクトルなす角検定部、２６０ 方向余弦行列候補算出部、２６１ 方向余弦行列姿勢検定部、２７０ 姿勢方位標定部、２９０ 装置記憶部、２９１ 姿勢方位初期値、２９２ 姿勢方位値、９１１ ＣＰＵ、９１２ バス、９２０ 記憶装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。

Claims (4)

1. 対象物に設置される加速度計により計測された加速度値に基づいて、前記対象物に設置される第１のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）アンテナから前記対象物に設置される第２のＧＰＳアンテナへの線分を基線として基線ベクトルを表す三次元の成分のうち基線ベクトルの鉛直成分をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する鉛直成分算出部と、
   前記第１のＧＰＳアンテナと前記第２のＧＰＳアンテナとにより搬送波位相を観測される４機以上のＧＰＳ衛星のうち３機のＧＰＳ衛星をプライマリ衛星としてＣＰＵを用いて選択するプライマリ衛星選択部と、
   前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と、前記第１のＧＰＳアンテナにより観測される前記プライマリ衛星からの搬送波位相と、前記第２のＧＰＳアンテナにより観測される前記プライマリ衛星からの搬送波位相とに基づいて、搬送波の整数値バイアスの二重差の候補をＣＰＵを用いて整数値バイアス候補として複数算出する整数値バイアス算出部と、
   前記整数値バイアス算出部により算出された複数の整数値バイアス候補それぞれに基づいて、前記基線ベクトルを表す三次元の成分のうち前記基線ベクトルの鉛直成分を含まない前記基線ベクトルの二次元成分の候補をＣＰＵを用いて二次元成分候補として複数算出する二次元成分算出部と、
   前記プライマリ衛星選択部により選択されたプライマリ衛星以外のＧＰＳ衛星をセカンダリ衛星として前記第１のＧＰＳアンテナにより観測される前記セカンダリ衛星からの搬送波位相と前記第２のＧＰＳアンテナにより観測される前記セカンダリ衛星からの搬送波位相とに基づいて、前記二次元成分算出部により算出された複数の二次元成分候補から二次元成分二次候補をＣＰＵを用いて選択する二次元成分二次候補選択部と、
   前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と前記二次元成分二次候補選択部により選択された二次元成分二次候補とに基づいて、前記対象物の方位角と前記対象物の回転角と前記対象物の仰角との候補をＣＰＵを用いて姿勢方位候補として複数算出する方位算出部と、
   前記加速度計により計測された加速度値に基づいて、前記対象物の回転角と前記対象物の仰角とを加速度姿勢としてＣＰＵを用いて算出する加速度姿勢算出部と、
   前記方位算出部により算出された複数の姿勢方位候補と前記加速度姿勢算出部により算出された加速度姿勢とに基づいて、前記対象物の方位角と前記対象物の回転角と前記対象物の仰角とを決定する姿勢方位決定部と
   を備えたことを特徴とする方位算出装置。
2. 前記方位算出装置は、さらに、
   前記鉛直成分算出部により算出される前記基線ベクトルの鉛直成分と前記二次元成分二次候補選択部により選択される二次元成分二次候補とは異なる座標系で予め計測される基線ベクトルに基づいて、前記二次元成分二次候補選択部により選択された二次元成分二次候補から二次元成分三次候補をＣＰＵを用いて選択する二次元成分三次候補選択部を備え、
   前記方位算出部は、前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と前記二次元成分三次候補選択部により選択された二次元成分三次候補とに基づいて前記姿勢方位候補を算出する
   ことを特徴とする請求項１記載の方位算出装置。
3. 鉛直成分算出部とプライマリ衛星選択部と整数値バイアス算出部と二次元成分算出部と二次元成分二次候補選択部と方位算出部と加速度姿勢算出部と姿勢方位決定部とを備える方位算出装置の方位算出方法であり、
   前記鉛直成分算出部が、対象物に設置される加速度計により計測された加速度値に基づいて、前記対象物に設置される第１のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）アンテナから前記対象物に設置される第２のＧＰＳアンテナへの線分を基線として基線ベクトルを表す三次元の成分のうち基線ベクトルの鉛直成分をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出し、
   前記プライマリ衛星選択部が、前記第１のＧＰＳアンテナと前記第２のＧＰＳアンテナとにより搬送波位相を観測される４機以上のＧＰＳ衛星のうち３機のＧＰＳ衛星をプライマリ衛星としてＣＰＵを用いて選択し、
   前記整数値バイアス算出部が、前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と、前記第１のＧＰＳアンテナにより観測される前記プライマリ衛星からの搬送波位相と、前記第２のＧＰＳアンテナにより観測される前記プライマリ衛星からの搬送波位相とに基づいて、搬送波の整数値バイアスの二重差の候補をＣＰＵを用いて整数値バイアス候補として複数算出し、
   前記二次元成分算出部が、前記整数値バイアス算出部により算出された複数の整数値バイアス候補それぞれに基づいて、前記基線ベクトルを表す三次元の成分のうち前記基線ベクトルの鉛直成分を含まない前記基線ベクトルの二次元成分の候補をＣＰＵを用いて二次元成分候補として複数算出し、
   前記二次元成分二次候補選択部が、前記プライマリ衛星選択部により選択されたプライマリ衛星以外のＧＰＳ衛星をセカンダリ衛星として前記第１のＧＰＳアンテナにより観測される前記セカンダリ衛星からの搬送波位相と前記第２のＧＰＳアンテナにより観測される前記セカンダリ衛星からの搬送波位相とに基づいて、前記二次元成分算出部により算出された複数の二次元成分候補から二次元成分二次候補をＣＰＵを用いて選択し、
   前記方位算出部が、前記鉛直成分算出部により算出された前記基線ベクトルの鉛直成分と前記二次元成分二次候補選択部により選択された二次元成分二次候補とに基づいて、前記対象物の方位角と前記対象物の回転角と前記対象物の仰角との候補をＣＰＵを用いて姿勢方位候補として複数算出し、
   前記加速度姿勢算出部が、前記加速度計により計測された加速度値に基づいて、前記対象物の回転角と前記対象物の仰角とを加速度姿勢としてＣＰＵを用いて算出し、
   前記姿勢方位決定部が、前記方位算出部により算出された複数の姿勢方位候補と前記加速度姿勢算出部により算出された加速度姿勢とに基づいて、前記対象物の方位角と前記対象物の回転角と前記対象物の仰角とを決定する
   ことを特徴とする方位算出装置の方位算出方法。
4. 請求項３記載の方位算出装置の方位算出方法をコンピュータに実行させる方位算出プログラム。

Images