(57)【要約】【課題】高精度な状態算出値が得られる小型の状態算出装置を提供する。【解決手段】状態算出装置１０は、アンテナ１００Ａ，１００Ｂ，１００Ｃ、受信部１１１Ａ，１１１Ｂ，１１１Ｃ、位相差算出部１２、および演算部１３を備える。受信部１１１Ａ，１１１Ｂ，１１１Ｃは、アンテナ１００Ａ，１００Ｂ，１００Ｃで受信したＧＮＳＳ信号の搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを算出する。位相差算出部１２は、アンテナ１００Ａ，１００Ｂ，１００Ｃをマスターアンテナまたはスレーブアンテナのいずれかに切り替えて設定する。位相差算出部１２は、マスターアンテナとスレーブアンテナの組み合わせ毎に搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて複数のアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを算出する。演算部１３は、複数のアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを用いて姿勢角ＡＴを算出する。【選択図】  図１
2016104033.tif 000002
【特許請求の範囲】
【請求項１】
ＧＮＳＳ信号を受信する第１、第２、第３のＧＮＳＳアンテナと、
前記第１のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する第１のＧＮＳＳ受信部と、
前記第２のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する第２のＧＮＳＳ受信部と、
前記第３のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する第３のＧＮＳＳ受信部と、
前記第１のＧＮＳＳアンテナ、前記第２のＧＮＳＳアンテナ、および前記第３のＧＮＳＳアンテナの内２つを選択して、マスターアンテナが少なくとも１でも異なる前記マスターアンテナとスレーブアンテナとの組合せを３つ以上決定し、組み合わせに用いられたＧＮＳＳアンテナの搬送波位相を用いて、組み合わせ毎にアンテナ間位相差を算出する位相差算出部と、
前記組み合わせ毎に算出された複数のアンテナ間位相差を用いて姿勢角を算出する演算部と、
を備える、状態算出装置。
【請求項２】
請求項１に記載の状態算出装置であって、
前記演算部は、
前記複数のアンテナ間位相差からアンテナ間の幾何距離差を算出し、
前記幾何距離差から前記姿勢角を算出する、
状態算出装置。
【請求項３】
請求項２に記載の状態算出装置であって、
前記演算部は、誤差算出部と統合処理部を備え、
前記誤差算出部は、前記幾何距離差と統合処理部が算出した過去の姿勢角に基づく幾何距離差とを用いて、姿勢角の算出誤差を推定し、
前記統合処理部は、慣性センサが計測した角速度また加速度と前記姿勢角の算出誤差とを用いて姿勢角を算出する、
状態算出装置。
【請求項４】
請求項３に記載の状態算出装置であって、
前記誤差推定部は、
複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される位置と前記統合処理部が算出した過去の位置とを用いて前記位置の算出誤差を推定し、
前記複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される速度と前記統合処理部が算出した過去の速度とを用いて前記速度の算出誤差を推定し、
前記位置の算出誤差と前記速度の算出誤差を前記統合処理部に出力し、
前記統合処理部は、前記慣性センサが計測した角速度また加速度と、前記位置の算出誤差および前記速度の算出誤差を用いて、位置および速度を算出する、
状態算出装置。
【請求項５】
請求項４に記載の状態算出装置であって、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの位置、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの位置、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの位置を用いて特定点の位置を算出する位置算出部と、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの速度、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの速度、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの速度を用いて特定点の速度を算出する速度算出部と、をさらに備え、
前記誤差推定部は、
前記特定点の位置と前記統合処理部が算出した前記特定点の過去の位置とを用いた前記位置の算出誤差、または、前記特定点の速度と前記統合処理部が算出した前記特定点の過去の速度とを用いた前記速度の算出誤差、を推定する、
状態算出装置。
【請求項６】
請求項１乃至請求項５のいずれかに記載の状態算出装置であって、
前記姿勢角はヨー角である、
状態算出装置。
【請求項７】
前記第１のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する工程と、
前記第２のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する工程と、
前記第３のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する工程と、
前記第１のＧＮＳＳアンテナ、前記第２のＧＮＳＳアンテナ、および前記第３のＧＮＳＳアンテナの内２つを選択して、マスターアンテナが少なくとも１でも異なる前記マスターアンテナとスレーブアンテナとの組合せを３つ以上決定し、組み合わせに用いられたＧＮＳＳアンテナの搬送波位相を用いて、組み合わせ毎にアンテナ間位相差を算出する工程と、
前記組み合わせ毎に算出された複数のアンテナ間位相差を用いて姿勢角を算出する工程と、
を有する、状態算出方法。
【請求項８】
請求項７に記載の状態算出方法であって、
前記姿勢角を算出する工程は、
前記複数のアンテナ間位相差からアンテナ間の幾何距離差を算出し、
前記幾何距離差から前記姿勢角を算出する、
状態算出方法。
【請求項９】
請求項８に記載の状態算出方法であって、
前記姿勢角を算出する工程は、誤差算出工程と統合処理工程を有し、
前記誤差算出工程は、前記幾何距離差と統合処理工程で算出した過去の姿勢角に基づく幾何距離差とを用いて、姿勢角の算出誤差を推定し、
前記統合処理工程は、慣性センサが計測した角速度また加速度と前記姿勢角の算出誤差とを用いて姿勢角を算出する、
状態算出方法。
【請求項１０】
請求項９に記載の状態算出方法であって、
前記姿勢角とともに、位置および速度を算出し、
前記誤差推定工程は、
複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される位置と前記統合処理工程で算出した過去の位置とを用いて前記位置の算出誤差を推定し、
前記複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される速度と前記統合処理工程で算出した過去の速度とを用いて前記速度の算出誤差を推定し、
前記位置の算出誤差と前記速度の算出誤差を前記統合処理工程に出力し、
前記統合処理工程は、前記慣性センサが計測した角速度また加速度と、前記位置の算出誤差および前記速度の算出誤差を用いて、前記位置および前記速度を算出する、
状態算出方法。
【請求項１１】
請求項１０に記載の状態算出装置であって、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの位置、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの位置、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの位置を用いて特定点の位置を算出する位置算出工程と、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの速度、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの速度、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの速度を用いて特定点の速度を算出する速度算出工程と、をさらに有し、
前記姿勢角、位置および速度を算出する工程は、
前記特定点の位置と統合処理工程が算出した前記特定点の過去の位置とを用いた前記位置の算出誤差、または、前記特定点の速度と前記統合処理部が算出した前記特定点の過去の速度とを用いた前記速度の算出誤差、を推定する、
状態算出方法。
【請求項１２】
ＧＮＳＳ信号を受信して、移動体の姿勢角を算出する処理を情報処理装置に実行させる状態算出プログラムであって、
前記情報処理装置は、
前記第１のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する処理と、
前記第２のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する処理と、
前記第３のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する処理と、
前記第１のＧＮＳＳアンテナ、前記第２のＧＮＳＳアンテナ、および前記第３のＧＮＳＳアンテナの内２つを選択して、マスターアンテナが少なくとも１でも異なる前記マスターアンテナとスレーブアンテナとの組合せを３つ以上決定し、組み合わせに用いられたＧＮＳＳアンテナの搬送波位相を用いて、組み合わせ毎にアンテナ間位相差を算出する処理と、
前記組み合わせ毎に算出された複数のアンテナ間位相差を用いて姿勢角を算出する処理と、
を実行する、状態算出プログラム。
【請求項１３】
請求項１２に記載の状態算出プログラムであって、
前記情報処理装置は、
前記姿勢角を算出する処理において、
前記複数のアンテナ間位相差からアンテナ間の幾何距離差を算出し、
前記幾何距離差から前記姿勢角を算出する、
状態算出プログラム。
【請求項１４】
請求項１３に記載の状態算出プログラムであって、
前記情報処理装置は、
前記姿勢角を算出する処理に、誤差算出処理と統合処理を含み、
前記誤差算出処理において、前記幾何距離差と統合処理で算出した過去の姿勢角に基づく幾何距離差とを用いて、姿勢角の算出誤差を推定し、
前記統合処理において、慣性センサが計測した角速度また加速度と前記姿勢角の算出誤差とを用いて姿勢角を算出する、
状態算出プログラム。
【請求項１５】
請求項１４に記載の状態算出プログラムであって、
前記情報処理装置は、
前記姿勢角とともに、位置および速度を算出し、
前記誤差推定において、
複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される位置と前記統合処理で算出した過去の位置とを用いて前記位置の算出誤差を推定し、
前記複数のＧＮＳＳアンテナで受信した前記ＧＮＳＳ信号から算出される速度と前記統合処理で算出した過去の速度とを用いて前記速度の算出誤差を推定し、
前記位置の算出誤差と前記速度の算出誤差を前記統合処理に出力し、
前記統合処理において、前記慣性センサが計測した角速度また加速度と、前記位置の算出誤差および前記速度の算出誤差を用いて、前記位置および前記速度を算出する、
状態算出プログラム。
【請求項１６】
請求項１５に記載の状態算出プログラムであって、
前記情報処理装置は、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの位置、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの位置、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの位置を用いて特定点の位置を算出する位置算出処理と、
前記第１のＧＮＳＳ受信部で算出した前記第１のＧＮＳＳアンテナの速度、前記第２のＧＮＳＳ受信部で算出した前記第２のＧＮＳＳアンテナの速度、および、前記第３のＧＮＳＳ受信部で算出した前記第３のＧＮＳＳアンテナの速度を用いて特定点の速度を算出する速度算出処理と、をさらに実行し、
前記姿勢角、位置および速度を算出する処理において、
前記特定点の位置と統合処理で算出した前記特定点の過去の位置とを用いた前記位置の算出誤差、または、前記特定点の速度と前記統合処理で算出した前記特定点の過去の速度とを用いた前記速度の算出誤差、を推定する、
状態算出プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、船舶、飛行体、自動車等の移動体における姿勢角、速度、位置等の航行状態を算出する状態算出装置、状態算出方法、および状態算出プログラムに関する。
【背景技術】
【０００２】
非特許文献１には、複数のＧＰＳ（Ｇｒｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）アンテナ、複数のＧＰＳアンテナにそれぞれ繋がる複数のＧＰＳ受信機、ＩＭＵセンサ、および、統合処理部を備える構成が記載されている。【０００３】
複数のＧＰＳアンテナは、ＧＰＳ衛星が送信するＧＰＳ信号を受信し、ＧＰＳ受信機に出力する。複数のＧＰＳアンテナは、マスターのＧＰＳアンテナと複数のスレーブのＧＰＳアンテナから構成されている。マスターのＧＰＳアンテナと複数のスレーブのＧＰＳアンテナは、マスターのＧＰＳアンテナとスレーブのＧＰＳアンテナとの距離がそれぞれに可能な限り長くなるように、移動体に配置されている。これは、ＧＰＳアンテナ間の距離（基線長）が長いほど、移動体の状態算出値を精度良く検出できることが知られているからである。ここで、移動体の状態算出値としては、例えば、移動体の姿勢角である。【０００４】
ＧＰＳ受信機は、受信したＧＰＳ信号の擬似距離、Δレンジ、および搬送波位相を算出して出力する。ＩＭＵセンサは、角速度センサおよび加速度センサを備え、移動体の角速度および加速度を計測して出力する。統合処理部は、ＩＭＵセンサからの角速度および加速度に基づいて、移動体の速度、加速度、および姿勢角を算出する。この際、統合処理部は、複数のＧＰＳ受信機から得られた擬似距離、Δレンジ、および搬送波位相を用いて、算出する位置、速度、および姿勢角の誤差を補正する。【０００５】
また、非特許文献２には、４台のＧＮＳＳアンテナを用いて二重位相差を算出し、二重位相差を用いて、位置、速度、および姿勢角を算出することが記載されている。この構成では、４機のＧＮＳＳアンテナのうち、１機のＧＮＳＳアンテナを基準アンテナに設定し、基線ベクトルを設定している。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】A Low Cost GPS/INS Sensorfor Small UAVs Augmented with Multiple GPS Antennas;Rui Hirokawa, RyusukeOhata, Takuji Ebinuma, Taro Suzuki; ION GNSS 20th International TechnicalMeeting of the Satellite Division, 25-28, September 2007, Fort Worth, TX
【非特許文献２】Low-cost Real-time Tightly-CoupledGNSS/INS Navigation System Based on Carrier-phase Double-differences for UAVApplications; Lopez Casariego, E., E. Perez, G. Falco, M. Campo-Cossio Gutierrez,F. Zacchello, and S. Bories; ION GNSS+, September 2014
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述の非特許文献１に記載の構成を用いた場合、ＧＰＳアンテナ間の距離を長くする必要があるため、状態算出装置の全体形状が大きくなってしまう。このため、例えば、ＧＰＳアンテナの設置位置が制限される場合には、ＧＰＳアンテナを設置できない、または、高精度な状態算出値を取得することができないという問題が生じる。【０００８】
また、上述の非特許文献２に記載の構成では、配置された複数のＧＮＳＳアンテナの１機が基準アンテナに設定されているので、ＧＮＳＳアンテナの個数によって形成できる基線の数が限られてしまう。このため、精度を向上しようと基線の本数を増加させようとすれば、少なくとも基線の本数分だけアンテナの個数が増加しなければならず、小型化が阻害される。【０００９】
したがって、本発明の目的は、高精度な状態算出値が得られる小型の状態算出装置を提供することにある。
【課題を解決するための手段】
【００１０】
本発明の状態算出装置は、ＧＮＳＳ信号を受信する第１、第２、第３のＧＮＳＳアンテナと、第１、第２、第３のＧＮＳＳ受信部、位相差算出部、および、演算部を備える。第１のＧＮＳＳ受信部は、第１のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する。第２のＧＮＳＳ受信部は、第２のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する。第３のＧＮＳＳ受信部は、第３のＧＮＳＳアンテナで受信したＧＮＳＳ信号の搬送波位相を算出する。位相差算出部は、第１のＧＮＳＳアンテナ、第２のＧＮＳＳアンテナ、および第３のＧＮＳＳアンテナの内２つを選択して、マスターアンテナが少なくとも１つでも異なるマスターアンテナとスレーブアンテナとの組合せを３つ以上決定し、組み合わせに用いられたＧＮＳＳアンテナの搬送波位相を用いて、組み合わせ毎にアンテナ間位相差を算出するアンテナ間位相差を算出する。演算部は、組み合わせ毎に算出された複数のアンテナ間位相差を用いて姿勢角を算出する。【００１１】
この構成では、姿勢角の推定演算に利用するアンテナ間位相差の数を多くすることができる。これによって、姿勢角の推定精度を向上させることができる。【００１２】
また、この発明の状態算出装置の演算部は、誤差算出部と統合処理部を備える。誤差算出部は、アンテナ間位相差から得られる幾何距離差と統合処理部が算出した過去の姿勢角に基づく幾何距離差とを用いて姿勢角の算出誤差を推定する。統合処理部は、慣性センサが計測した角速度また加速度と姿勢角の算出誤差とを用いて姿勢角を算出する。【００１３】
この構成では、慣性センサの計測値も用いることができるので、姿勢角を安定して出力することができる。さらに、上述のＧＮＳＳ信号を用いた高精度な姿勢角によって姿勢角の算出誤差を推定できるので、より高精度な姿勢角を出力できる。【００１４】
また、この発明の状態算出装置の演算部は、誤差算出部と統合処理部を備える。誤差算出部は、幾何距離差と統合処理部が算出した過去の姿勢角に基づく幾何距離差とを用いて、姿勢角の算出誤差を推定する。この際、誤差算出部は、姿勢角とともに、位置および速度の算出誤差も推定する。統合処理部は、慣性センサが計測した角速度また加速度と姿勢角の算出誤差、位置の算出誤差、および速度の算出誤差とを用いて姿勢角、位置、および速度を算出する。そして、状態算出装置は、位置算出部、および速度算出部をさらに備える。位置算出部は、第１のＧＮＳＳ受信部で算出した第１のＧＮＳＳアンテナの位置、第２のＧＮＳＳ受信部で算出した第２のＧＮＳＳアンテナの位置、および、第３のＧＮＳＳ受信部で算出した第３のＧＮＳＳアンテナの位置を用いて特定点の位置を算出する。速度算出部は、第１のＧＮＳＳ受信部で算出した第１のＧＮＳＳアンテナの速度、第２のＧＮＳＳ受信部で算出した第２のＧＮＳＳアンテナの速度、および、第３のＧＮＳＳ受信部で算出した第３のＧＮＳＳアンテナの速度を用いて特定点の速度を算出する。誤差推定部は、特定点の位置と統合処理部が算出した特定点の過去の位置とを用いた位置の算出誤差、または、特定点の速度と統合処理部が算出した特定点の過去の速度とを用いた速度の算出誤差、を推定する。【００１５】
この構成では、誤差推定処理とは別処理で算出した特定位置および特定速度を誤差推定処理に利用することによって、誤差推定処理の処理負荷を軽減できる。【００１６】
また、この発明の状態算出装置では、姿勢角はヨー角である。この構成では、移動体の向いている方位を高精度に算出できる。
【発明の効果】
【００１７】
この発明によれば、装置の配置環境等に影響されることなく、姿勢角等の状態算出値を高精度に得られる。
【図面の簡単な説明】
【００１８】
【図１】本発明の第１の実施形態に係る状態算出装置の構成を示すブロック図
【図２】本発明の第１の実施形態に係る状態算出装置に用いられるアンテナの位置関係を示す図
【図３】本発明の第１の実施形態に係る状態算出方法のフローチャート
【図４】図３のステップＳ１０６の具体的な処理を示すフローチャート
【図５】本発明の第１の実施形態に係る状態算出方法のフローチャート
【図６】図５のステップＳ２０６の具体的な処理を示すフローチャート
【図７】本発明の第２の実施形態に係る状態算出装置のブロック図
【図８】本発明の第２の実施形態に係る状態算出方法のフローチャート
【図９】本発明の第３の実施形態に係る状態算出装置のブロック図
【図１０】本発明の第３の実施形態に係る状態算出方法のフローチャート
【図１１】アンテナが４機である場合のアンテナの位置関係を示す図
【発明を実施するための形態】
【００１９】
本発明の第１の実施形態に係る状態算出装置、状態算出方法、および、状態算出プログラムについて、図を参照して説明する。本実施形態では、移動体を船舶として当該船舶の航行状態を算出する装置を説明するが、その他の海上移動体、海中移動体、自動車等の陸上移動体、飛行機等の空中移動体の移動状態を算出する場合についても、以下の構成を適用することができる。
【００２０】
図１は、本発明の第１の実施形態に係る状態算出装置の構成を示すブロック図である。図２は、本発明の第１の実施形態に係る状態算出装置に用いられるアンテナの位置関係を示す図である。
【００２１】
図１に示すように、状態算出装置１０は、アンテナ部１００、受信部１１Ａ，１１Ｂ，１１Ｃ、位相算出部１２、および、演算部１３を備える。
【００２２】
アンテナ部１００は、第１アンテナ１００Ａ、第２アンテナ１００Ｂ、および、第３アンテナ１００Ｃを備える。アンテナ部１００は、船体における上空が開放された位置に配置されている。第１アンテナ１００Ａは第１のＧＮＳＳアンテナに相当し、第２アンテナ１００Ｂは第２のＧＮＳＳアンテナに相当し、第３アンテナ１００Ｃは、第３のＧＮＳＳアンテナに相当する。
【００２３】
図２に示すように、第１アンテナ１００Ａ、第２アンテナ１００Ｂ、および、第３アンテナ１００Ｃの配置パターンは、二次元の広がりを有する。具体的な配置態様の一例としては、図２に示すように、第１アンテナ１００Ａと第３アンテナ１００Ｃは、これらのアンテナを結ぶ基線が船首と船尾を結ぶ方向（船首−船尾方向）に平行になるように、配置されている。第２のアンテナ１００Ｂは、第１アンテナ１００Ａと第３アンテナ１００Ｃとを結ぶ基線上およびこの基線を延長した直線上とは異なる位置に配置されている。図２の例では、第２アンテナ１００Ｂは、船首−船尾方向において、第１アンテナ１００Ａと第３アンテナ１００Ｃとの間に配置されている。第２アンテナ１００Ｂは、第１アンテナ１００Ａと第３アンテナ１００Ｃとを結ぶ基線に対して直交する方向に、この基線から距離を置いて配置されている。図２の例では、第２アンテナ１００Ｂは、第１アンテナ１００Ａと第３アンテナ１００Ｃに対して等距離の位置に配置されている。
【００２４】
第１アンテナ１００Ａと第２アンテナ１００Ｂとの距離、第２アンテナ１００Ｂと第３アンテナ１００Ｃとの距離、および、第３アンテナ１００Ｃと第１アンテナ１００Ａとの距離は短い。これにより、アンテナ部１００の配置の自由度を向上することができる。また、整数値バイアスの決定を容易にすることができる。さらに、これらのアンテナ間の距離は、ＧＰＳ信号の波長λの長さ未満であることが好ましく、１／２（λ／２）程度であるとより好ましい。アンテナ間の距離をλ／２程度とすることで、整数値バイアスの決定をさらに容易にすることができる。
【００２５】
なお、この配置は一例であり、３機以上のアンテナが、二次元の広がりを有するように配置されていればよい。
【００２６】
第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃは、ＧＰＳ（Ｇｒｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）衛星が送信するＧＰＳ信号を受信して出力する。なお、本実施形態ではＧＰＳを例に示すが、ＧＮＳＳ（Ｇｌｏｂａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓａｔｅｌｌｉｔｅ  Ｓｙｓｔｅｍ）の他のシステムに対しても、本実施形態の構成を適用することができる。
【００２７】
受信部１１Ａは、第１のＧＮＳＳ受信部に相当し、第１アンテナ１００Ａに接続されている。受信部１１Ａは、ＧＰＳ信号を捕捉、追尾し、ＧＰＳ信号毎（ＧＰＳ衛星毎）に擬似距離を算出する。受信部１１Ａは、擬似距離から単独測位法を用いて、第１アンテナ１００Ａの位置（図１に示すＡ点の位置）ＰＯＡを算出する。受信部１１Ａは、第１アンテナ１００Ａの位置ＰＯＡを、演算部１３に出力する。受信部１１Ａは、ＧＰＳ信号毎（ＧＰＳ衛星毎）に搬送波位相測定値ＰＹＡを算出する。受信部１１Ａは、搬送波位相測定値ＰＹＡを、位相差算出部１２に出力する。
【００２８】
受信部１１Ｂは、第２のＧＮＳＳ受信部に相当し、第２アンテナ１００Ｂに接続されている。受信部１１Ｂは、ＧＰＳ信号を捕捉、追尾し、ＧＰＳ信号毎（ＧＰＳ衛星毎）に擬似距離を算出する。受信部１１Ｂは、擬似距離から単独測位法を用いて、第２アンテナ１００Ｂの位置（図１に示すＢ点の位置）ＰＯＢを算出する。受信部１１Ｂは、第２アンテナ１００Ｂの位置ＰＯＢを、演算部１３に出力する。受信部１１Ｂは、ＧＰＳ信号毎（ＧＰＳ衛星毎）に搬送波位相測定値ＰＹＢを算出する。受信部１１Ｂは、搬送波位相測定値ＰＹＢを、位相差算出部１２に出力する。
【００２９】
受信部１１Ｃは、第３のＧＮＳＳ受信部に相当し、第３アンテナ１００Ｃに接続されている。受信部１１Ｃは、ＧＰＳ信号を捕捉、追尾し、ＧＰＳ信号毎（ＧＰＳ衛星毎）に擬似距離を算出する。受信部１１Ｃは、擬似距離から単独測位法を用いて、第３アンテナ１００Ｃの位置（図１に示すＣ点の位置）ＰＯＣを算出する。受信部１１Ｃは、第３アンテナ１００Ｃの位置ＰＯＣを、演算部１３に出力する。受信部１１Ｃは、ＧＰＳ信号毎（ＧＰＳ衛星毎）に搬送波位相測定値ＰＹＣを算出する。受信部１１Ｃは、搬送波位相測定値ＰＹＣを、位相差算出部１２に出力する。
【００３０】
受信部１１Ａ，１１Ｂ，１１Ｃは同期されている。例えば、受信部１１Ａ，１１Ｂ，１１Ｃには、共通のクロック信号が入力されており、受信部１１Ａ，１１Ｂ，１１Ｃは、このクロック信号に同期して、ＧＰＳ信号の捕捉・追尾を行う。
【００３１】
位相差算出部１２は、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの内の２機を組み合わせて、組み合わせ毎にアンテナ間位相差を算出する。この際、位相差算出部１２は、組合せによって、１機のアンテナをマスターアンテナまたはスレーブアンテナのいずれかに設定して、アンテナ間位相差を算出する。なお、組み合わせは、順列（Ｐ：パーミテーション）で決定してもよく、組み合わせ（Ｃ：コンビネーション）で決定してもよい。具体的には、位相差算出部１２は、次の処理を実行する。
【００３２】
位相差算出部１２は、第１の組み合わせとして、第１アンテナ１００Ａをマスターアンテナに設定し、第２アンテナ１００Ｂをスレーブアンテナに設定する。位相差算出部１２は、搬送波位相測定値ＰＹＡと搬送波位相測定値ＰＹＢとの差分値を算出して、アンテナ間位相差ΔζＡＢ（＝ＰＹＢ−ＰＹＡ）を算出する。
【００３３】
位相差算出部１２は、第２の組み合せとして、第２アンテナ１００Ｂをマスターアンテナに設定し、第３アンテナ１００Ｃをスレーブアンテナに設定する。位相差算出部１２は、搬送波位相測定値ＰＹＢと搬送波位相測定値ＰＹＣとの差分値を算出して、アンテナ間位相差ΔζＢＣ（＝ＰＹＣ−ＰＹＢ）を算出する。
【００３４】
位相差算出部１２は、第３の組み合わせとして、第３アンテナ１００Ｃをマスターアンテナに設定し、第１アンテナ１００Ａをスレーブアンテナに設定する。位相差算出部１２は、搬送波位相測定値ＰＹＣと搬送波位相測定値ＰＹＡとの差分値を算出して、アンテナ間位相差ΔζＣＡ（＝ＰＹＡ−ＰＹＣ）を算出する。
【００３５】
このような処理を行うことによって、従来のマスターアンテナを固定する態様よりも、予め決められたアンテナ数において、より多くのアンテナ間位相差を算出することができる。逆に、予め決められたアンテナ間位相差を得るために、より少ないアンテナ数にすることができる。
【００３６】
位相差算出部１２は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを演算部１３に出力する。
【００３７】
演算部１３は、ＧＰＳ信号に重畳された航法メッセージを解析して、衛星位置を取得する。演算部１３は、受信部１１Ａ，１１Ｂ，１１Ｃで受信しているＧＰＳ信号の送信元であるＧＰＳ衛星の位置を、少なくとも取得していればよい。
【００３８】
演算部１３は、衛星位置と第１、第２、第３のアンテナ位置ＰＯＡ，ＰＯＢ，ＰＯＣとを用いて、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡ毎の方向余弦を算出する。具体的には、演算部１３は、第１、第２のアンテナ位置ＰＯＡ，ＰＯＢと、第１アンテナ１００Ａと第２アンテナ１００Ｂの両方で受信したＧＰＳ信号の送信元であるＧＰＳ衛星の衛星位置と用いて、アンテナ間位相差ΔζＡＢに対応する方向余弦を算出する。演算部１３は、第２、第３のアンテナ位置ＰＯＢ，ＰＯＣと、第２アンテナ１００Ｂと第３アンテナ１００Ｃの両方で受信したＧＰＳ信号の送信元であるＧＰＳ衛星の衛星位置と用いて、アンテナ間位相差ΔζＢＣに対応する方向余弦を算出する。演算部１３は、第３、第１のアンテナ位置ＰＯＣ，ＰＯＡと、第３アンテナ１００Ｃと第１アンテナ１００Ａの両方で受信したＧＰＳ信号の送信元であるＧＰＳ衛星の衛星位置と用いて、アンテナ間位相差ΔζＣＡに対応する方向余弦を算出する。演算部１３は、各方向余弦を用いて方向余弦行列を設定する。
【００３９】
演算部１３は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと方向余弦行列とを用いて、姿勢角ＡＴを算出する。姿勢角ＡＴは、ロール角φ、ピッチ角θ、およびヨー角ψから構成される。なお、姿勢角ＡＴとしては、少なくともヨー角ψが算出されていればよい。
【００４０】
より具体的には、演算部１３は、ＬＡＭＤＡ法等の既知の方法を用いて、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡ毎に整数値バイアスを推定して決定する。演算部１３は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと整数値バイアスとを用いて、各アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡに対応する幾何距離差を算出する。
【００４１】
演算部１３は、幾何距離差と方向余弦行列を用い、最小二乗法等を適用することによって、姿勢角ＡＴを算出する。
【００４２】
このような構成および処理を用いることによって、アンテナの台数が少なくてもアンテナ間位相差を多く算出でき、高精度に姿勢角を算出することができる。そして、アンテナ間位相差を多く算出できることによって、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃ間の距離が短くても、高精度に姿勢角を算出することができる。また、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの内いずれかの１機でＧＰＳ信号が受信できなくても姿勢角の算出を継続することができるので、ロバスト性を向上させることができる。
【００４３】
さらに、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃ間の距離を短くできることによって、アンテナ部１００を小型化できる。これにより、設置スペースの制限を受けにくい。したがって、省スペースで姿勢角を高精度に算出できる状態算出装置１０を実現することができる。
【００４４】
なお、図１では、位相差算出部１２と演算部１３とを別の機能部とする態様を示した。しかしながら、位相差算出部１２と演算部１３とを１つの情報処理装置で形成してもよい。さらには、受信部１１Ａ，１１Ｂ，１１Ｃ、位相差算出部１２、および演算部１３を１つの情報処理装置で形成してもよい。この場合、次に示す状態算出方法を実現するプログラムを予め記憶しておき、情報処理装置は、このプログラムを読み出して実行すればよい。
【００４５】
図３は、本発明の第１の実施形態に係る状態算出方法のフローチャートである。図４は、図３のステップＳ１０６の具体的な処理を示すフローチャートである。図３、図４は、１重位相差を用いる場合を示す。
【００４６】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃで受信したＧＰＳ信号を捕捉、追尾する。情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣと搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを算出する（Ｓ１０１）。
【００４７】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃに対して、マスターアンテナとスレーブアンテナを設定することで組み合わせて、アンテナの組み合わせを複数種類設定する（Ｓ１０２）。この際、情報処理装置は、各アンテナをマスターアンテナとしてもスレーブアンテナとしても設定する。これにより、１機のアンテナがマスターアンテナとして設定される組み合わせとスレーブアンテナとして設定される組み合わせとを実現できる。
【００４８】
情報処理装置は、各アンテナの組み合わせにおいて、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて、アンテナ間の１重位相差であるアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを算出する（Ｓ１０３）。アンテナ間の１重位相差とは、２機のアンテナが共通のＧＰＳ衛星からのＧＰＳ信号を受信した際に各アンテナが受信したＧＰＳ信号の位相差である。
【００４９】
情報処理装置は、追尾しているＧＰＳ信号に重畳された航法メッセージを解析して、衛星位置を取得する（Ｓ１０４）。
【００５０】
情報処理装置は、アンテナの組み合わせ毎に、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣと衛星位置から方向余弦を算出して、方向余弦行列を設定する（Ｓ１０５）。
【００５１】
情報処理装置は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと方向余弦行列とを用い、最小二乗法等を適用することによって姿勢角ＡＴを算出する（Ｓ１０６）。
【００５２】
より具体的には、図４に示すように、情報処理装置は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡのそれぞれに対応する整数値バイアスを算出する（Ｓ１６１）。この際、上述のように、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの間の距離を短くすることによって、整数値バイアスを高速且つ確実に算出することができる。特に、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの間の距離を、ＧＰＳ信号の波長の１／２程度とすることで、整数値バイアスをさらに高速且つ正確に算出することができる。
【００５３】
情報処理装置は、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと整数値バイアスから、幾何距離差を算出する（Ｓ１６２）。幾何距離差とは、共通のＧＰＳ信号を受信する２機のアンテナにおいて、一方のアンテナとＧＰＳ衛星との幾何距離と、他方のアンテナとＧＰＳ衛星との幾何距離との差である。
【００５４】
情報処理装置は、幾何距離差と方向余弦行列を用いて、最小二乗法等を適用することによって、姿勢角を算出する（Ｓ１６３）。
【００５５】
なお、上述の処理では、アンテナ間の１重位相差を用いる例を示したが、２重位相差を用いてもよい。図５は、本発明の第１の実施形態に係る状態算出方法のフローチャートである。図６は、図５のステップＳ２０６の具体的な処理を示すフローチャートである。図５、図６は、２重位相差を用いる場合を示す。
【００５６】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃで受信したＧＰＳ信号を捕捉・追尾する。情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣと搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを算出する（Ｓ２０１）。
【００５７】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃに対して、マスターアンテナとスレーブアンテナを設定することで組み合わせて、アンテナの組み合わせを複数種類設定する（Ｓ２０２）。この際、情報処理装置は、各アンテナをマスターアンテナとしてもスレーブアンテナとしても設定する。これにより、１機のアンテナがマスターアンテナとして設定される組み合わせとスレーブアンテナとして設定される組み合わせとを実現できる。
【００５８】
情報処理装置は、各アンテナの組み合わせにおいて、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて、２重位相差を算出する（Ｓ１０３）。２重位相差とは、２機のアンテナが共通する２機のＧＰＳ衛星からのＧＰＳ信号を受信した際の位相差である。具体的には、一方のＧＰＳ衛星に対する２機のアンテナの位相差と、他方のＧＰＳ衛星に対する２機のアンテナの位相差との差である。
【００５９】
情報処理装置は、追尾しているＧＰＳ信号に重畳された航法メッセージを解析して、衛星位置を取得する（Ｓ２０４）。
【００６０】
情報処理装置は、アンテナの組み合わせ毎に、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣと衛星位置から方向余弦を算出して、方向余弦行列を設定する（Ｓ２０５）。
【００６１】
情報処理装置は、２重位相差と方向余弦行列とを用いてカルマンフィルタを設定し、姿勢角ＡＴを推定する（Ｓ２０６）。
【００６２】
より具体的には、図６に示すように、情報処理装置は、２重位相差のそれぞれに対応する整数値バイアスを算出する（Ｓ２６１）。この際、上述のように、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの間の距離を短くすることによって、整数値バイアスを高速且つ確実に算出することができる。特に、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの間の距離を、ＧＰＳ信号の波長の１／２程度とすることで、整数値バイアスをさらに高速且つ正確に算出することができる。
【００６３】
情報処理装置は、２重位相差と整数値バイアスから、幾何距離の２重差（幾何距離差の差）を算出する（Ｓ２６２）。
【００６４】
情報処理装置は、幾何距離の２重差と方向余弦行列とを用い、最小二乗法等を適用することによって、姿勢角を算出する（Ｓ２６３）。
【００６５】
次に、第２の実施形態に係る状態算出装置、状態算出方法、および状態算出プログラムについて、図を参照して説明する。図７は、本発明の第２の実施形態に係る状態算出装置のブロック図である。
【００６６】
第１の実施形態に係る状態算出装置１０は、ＧＰＳ信号のみによって姿勢角を推定する態様であったが、本実施形態に係る状態算出装置１０Ａは、慣性センサ２０をさらに備え、慣性センサ２０の出力する角速度ωＩＭＵと加速度ａＩＭＵも用いて姿勢角を算出する。さらに、本実施形態に係る状態算出装置１０Ａは、ＧＰＳ信号に基づく観測値と角速度ωＩＭＵと加速度ａＩＭＵを用いて姿勢角、位置および速度を算出する。
【００６７】
本実施形態に係る状態算出装置１０Ａは、第１の実施形態に係る状態算出装置１０に対して演算部１３Ａの構成が異なるものであり、他の部分の基本的な構成は、第１の実施形態に係る状態算出装置１０と同じである。
【００６８】
状態算出装置１０Ａは、慣性センサ２０に接続されている。なお、慣性センサ２０を状態算出装置１０Ａに含んでもよい。慣性センサ２０は、加速度センサ２１および角速度センサ２２を備える。加速度センサ２１は、移動体の加速度ａＩＭＵを計測する。角速度センサ２２は、移動体の角速度ωＩＭＵを計測する。
【００６９】
演算部１３Ａは、誤差推定部１３１Ａ、統合処理部１３２Ａ、ＧＮＳＳ速度算出部１３３Ａ、および、ＧＮＳＳ位置算出部１３４Ａを備える。
【００７０】
受信部１１Ａは、第１アンテナ１００Ａの位置ＰＯＡ、速度ＰＶＡおよび搬送波位相ＰＹＡを算出する。受信部１１Ａは、第１アンテナ１００Ａの位置ＰＯＡをＧＮＳＳ位置算出部１３４Ａに出力する。受信部１１Ａは、第１アンテナ１００Ａの速度ＰＶＡをＧＮＳＳ速度算出部１３３Ａに出力する。受信部１１Ａは、第１アンテナ１００Ａでの搬送波位相測定値ＰＹＡを位相差算出部１２に出力する。
【００７１】
受信部１１Ｂは、第２アンテナ１００Ｂの位置ＰＯＢ、速度ＰＶＢおよび搬送波位相ＰＹＢを算出する。受信部１１Ｂは、第２アンテナ１００Ｂの位置ＰＯＢをＧＮＳＳ位置算出部１３４Ａに出力する。受信部１１Ｂは、第２アンテナ１００Ｂの速度ＰＶＢをＧＮＳＳ速度算出部１３３Ａに出力する。受信部１１Ｂは、第２アンテナ１００Ｂでの搬送波位相測定値ＰＹＢを位相差算出部１２に出力する。
【００７２】
受信部１１Ｃは、第３アンテナ１００Ｃの位置ＰＯＣ、速度ＰＶＣおよび搬送波位相ＰＹＣを算出する。受信部１１Ｃは、第３アンテナ１００Ｃの位置ＰＯＣをＧＮＳＳ位置算出部１３４Ａに出力する。受信部１１Ｃは、第３アンテナ１００Ｃの速度ＰＶＣをＧＮＳＳ速度算出部１３３Ａに出力する。受信部１１Ｃは、第３アンテナ１００Ｃでの搬送波位相測定値ＰＹＣを位相差算出部１２に出力する。
【００７３】
位相差演算部１２は、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて算出したアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを、誤差推定部１３１Ａに出力する。
【００７４】
ＧＮＳＳ速度算出部１３３Ａは、速度ＰＶＡ，ＰＶＢ，ＰＶＣを用いて、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃが装着される移動体の特定位置の速度ＰＶＯを算出する。特定位置は、例えば、図１に示すように、アンテナ部１００を平面視して、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの中心位置Ｏである。特定位置の速度ＰＶＯは、例えば、特定位置と第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃとの距離を用いて第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの速度ＰＶＡ，ＰＶＢ，ＰＶＣを加重平均処理することによって算出される。ＧＮＳＳ速度算出部１３３Ａは、特定位置の速度ＰＶＯを誤差推定部１３１Ａに出力する。
【００７５】
ＧＮＳＳ位置算出部１３４Ａは、位置ＰＯＡ，ＰＯＢ，ＰＯＣを用いて、第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃが装着される移動体の特定位置の位置（座標）ＰＯＯを算出する。特定位置の位置ＰＯＯは、例えば、特定位置と第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃとの距離を用いて第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣを加重平均処理することによって算出される。ＧＮＳＳ位置算出部１３４Ａは、特定位置の位置ＰＯＯと第１、第２、第３アンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣを誤差推定部１３１Ａに出力する。
【００７６】
誤差推定部１３１Ａは、特定位置の位置ＰＯＯ、速度ＰＶＯ、およびアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと、統合処理部１３２Ａが算出した前回の統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを用いて、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定する。
【００７７】
具体的には、誤差推定部１３１Ａは、ＧＰＳ信号に重畳された航法メッセージを解析して、衛星位置を取得する。
【００７８】
誤差推定部１３１Ａは、衛星位置と第１、第２、第３のアンテナ位置ＰＯＡ，ＰＯＢ，ＰＯＣとを用いて、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡ毎の方向余弦を算出する。誤差推定部１３１Ａは、各方向余弦を用いて方向余弦行列を設定する。
【００７９】
誤差推定部１３１Ａは、特定位置の位置ＰＯＯと前回の演算で得られた統合位置ＰＵＮとの差ΔＰＯを算出する。誤差推定部１３１Ａは、特定位置の速度ＰＶＯと前回の演算で得られた統合速度ＶＵＮとの差ΔＶＯを算出する。
【００８０】
誤差推定部１３１Ａは、前回の演算で得られた統合姿勢角ＡＴＵＮを用いて、統合演算結果に基づくアンテナ間位相差を算出する。誤差推定部１３１Ａは、ＧＰＳ信号に基づくアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと、統合演算結果に基づくアンテナ間位相差とを差分演算して、アンテナ間位相差の差ΔΔζＡＢ，ΔΔζＢＣ，ΔΔζＣＡを算出する。
【００８１】
誤差推定部１３１Ａは、位置の差ΔＰＯ、速度の差ΔＶＯ、および、アンテナ間位相差の差ΔΔζＡＢ，ΔΔζＢＣ，ΔΔζＣＡを観測値（観測ベクトル）に設定する。誤差推定部１３１Ａは、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定値（状態ベクトル）に設定する。誤差推定部１３１Ａは、これらの観測値、推定値、方向余弦行列を用いて、カルマンフィルタを設定する。誤差推定部１３１Ａは、このカルマンフィルタを用いて、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定する。
【００８２】
誤差推定部１３１Ａは、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを統合処理部１３２Ａに出力する。
【００８３】
統合処理部１３２Ａは、慣性センサ２０から出力された加速度ａＩＭＵおよび角速度ωＩＭＵを用いて、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを算出する。この際、統合処理部１３２Ａは、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴによる補正を行う。
【００８４】
このような構成および処理を用いることによって、移動体の位置、速度、および姿勢角を高精度に算出することができる。また、この構成では、上述のように小型のアンテナ部１００を用いているので、省スペースで位置、速度および姿勢角を高精度に算出できる状態算出装置１０Ａを実現することができる。また、慣性センサ２０の計測値を用いることによって、位置、速度および姿勢角を安定して算出することができる。
【００８５】
なお、上述の誤差推定部１３１Ａにおいて、加速度センサ２１のセンサ誤差εａおよび角速度センサ２２のセンサ誤差εωを推定することもできる。この場合、誤差推定部１３１Ａの観測値には、加速度ａＩＭＵおよび角速度ωＩＭＵが加えられる。
【００８６】
統合処理部１３２Ａは、センサ誤差εａ，εωも用いて、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを算出する。これにより、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮをさらに高精度に算出することができる。
【００８７】
また、本実施形態に係る状態算出装置１０Ａは、誤差推定部１３１Ａのカルマンフィルタの観測値に、カルマンフィルタと別途処理で算出した位置ＰＯＯおよび速度ＰＶＯを用いる。これにより、後述の第３の実施形態に係る状態算出装置得１０Ｂ（擬似距離、搬送波位相変化量および衛星位置変化量を用いる態様）と比較して、カルマンフィルタの処理負荷を軽減して、姿勢角を高精度に算出することができる。
【００８８】
なお、図７では、位相差算出部１２、誤差推定部１３１Ａ、統合処理部１３２Ａ、ＧＮＳＳ速度算出部１３３Ａ、およびＧＮＳＳ位置算出部１３４Ａをそれぞれ別の機能部とする態様を示した。しかしながら、これらの機能部を一つの情報処理装置で形成してもよい。さらには、受信部１１Ａ，１１Ｂ，１１Ｃもこの一つの情報処理装置に含んで形成してもよい。この場合、次に示す状態算出方法を実現するプログラムを予め記憶しておき、情報処理装置は、このプログラムを読み出して実行すればよい。
【００８９】
図８は、本発明の第２の実施形態に係る状態算出方法のフローチャートである。なお、本実施形態では、１重位相差を用いる場合を説明するが、第１の実施形態と同様に、２重位相差を用いることもできる。
【００９０】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃで受信したＧＰＳ信号を捕捉・追尾する。情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣ、速度ＰＶＡ，ＰＶＢ，ＰＶＣと搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを算出する（Ｓ４０１）。
【００９１】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの位置ＰＯＡ，ＰＯＢ，ＰＯＣを用いて特定位置の位置ＰＯＯを算出し、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃの速度ＰＶＡ，ＰＶＢ，ＰＶＣを用いて特定位置の速度ＰＶＯを算出する（Ｓ４０２）。
【００９２】
情報処理装置は、慣性センサ２０から加速度ａＩＭＵおよび角速度ωＩＭＵを取得する（Ｓ４０３）。
【００９３】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃに対して、マスターアンテナとスレーブアンテナを設定することで組み合わせて、アンテナの組み合わせを複数種類設定する。この際、情報処理装置は、各アンテナをマスターアンテナとしてもスレーブアンテナとしても設定する。情報処理装置は、各アンテナの組み合わせにおいて、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて、アンテナ間の１重位相差であるアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを算出する（Ｓ４０４）。
【００９４】
情報処理装置は、特定位置の位置ＰＯＯ、速度ＰＶＯ、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡと、前回の演算で得られた統合位置ＰＵＮ、統合速度ＶＵＮ、統合姿勢角ＡＴＵＮとを用いて、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定する（Ｓ４０５）。この際、情報処理装置は、カルマンフィルタ等を用いる。なお、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮは適宜設定しておけばよく、カルマンフィルタによる統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮの誤差推定を継続し、この誤差で補正する演算を行うことによって、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮは、真値に近づく。
【００９５】
情報処理装置は、加速度ａＩＭＵ、角速度ωＩＭＵ、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを用いて、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを算出する（Ｓ４０６）。
【００９６】
次に、第３の実施形態に係る状態算出装置、状態算出方法、および状態算出プログラムについて、図を参照して説明する。図９は、本発明の第３の実施形態に係る状態算出装置のブロック図である。
【００９７】
本実施形態に係る状態算出装置１０Ｂは、演算部１３Ｂの構成が第２の実施形態に係る状態算出装置１０Ａと異なる。また、受信部１１Ａ，１１Ｂ，１１Ｃの処理が第２の実施形態に係る状態算出装置１０Ａと異なる。
【００９８】
受信部１１Ａは、擬似距離ρＡ、搬送波位相変化量ΔＰＹＡ、および、衛星位置変化量ΔＰｓａｔＡを、ＧＰＳ信号の追尾処理において取得する。搬送波位相変化量は、所定の時間間隔での搬送波位相の変化量である。衛星位置変化量は、所定の時間間隔での衛星位置の変化量である。衛星位置の変化量は、航法メッセージを解析することによって得られる。
【００９９】
受信部１１Ｂは、擬似距離ρＢ、搬送波位相変化量ΔＰＹＢ、および、衛星位置変化量ΔＰｓａｔＢを、ＧＰＳ信号の追尾処理において取得する。
【０１００】
受信部１１Ｃは、擬似距離ρＣ、搬送波位相変化量ΔＰＹＣ、および、衛星位置変化量ΔＰｓａｔＣを、ＧＰＳ信号の追尾処理において取得する。
【０１０１】
演算部１３Ｂは、誤差推定部１３１Ｂ、および統合処理部１３２Ｂを備える。
【０１０２】
誤差推定部１３１Ｂには、擬似距離ρＡ，ρＢ，ρＣ、搬送波位相変化量ΔＰＹＡ，ΔＰＹＢ，ΔＰＹＣ、衛星位置変化量ΔＰｓａｔＡ，ΔＰｓａｔＢ，ΔＰｓａｔＣ、アンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡが入力される。また、誤差推定部１３１Ｂには、前回の統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮが入力される。
【０１０３】
誤差推定部１３１Ｂは、これらの入力値から観測値を設定し、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定値とするカルマンフィルタを設定する。誤差推定部１３１Ｂは、このカルマンフィルタを演算処理することによって、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定し、統合処理部１３２Ｂに出力する。
【０１０４】
統合処理部１３２Ｂは、加速度ａＩＭＵおよび角速度ωＩＭＵを用いて、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを算出する。この際、統合処理部１３２Ｂは、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴによる補正を行う。
【０１０５】
このような構成を用いても、第２の実施形態と同様に姿勢角を高精度に算出することができる。さらに、本実施形態の構成では、位置および速度も高精度に算出することができる。
【０１０６】
なお、図９では、位相差算出部１２、誤差推定部１３１Ｂ、および統合処理部１３２Ｂをそれぞれ別の機能部とする態様を示した。しかしながら、これらの機能部を一つの情報処理装置で形成してもよい。さらには、受信部１１Ａ，１１Ｂ，１１Ｃもこの一つの情報処理装置に含んで形成してもよい。この場合、次に示す状態算出方法を実現するプログラムを予め記憶しておき、情報処理装置は、このプログラムを読み出して実行すればよい。
【０１０７】
図１０は、本発明の第３の実施形態に係る状態算出方法のフローチャートである。本実施形態に係る状態算出方法は、カルマンフィルタの観測値が異なり、この観測値の変化によってカルマンフィルタの設定が異なるが、基本的な処理は第２の実施形態に係る状態算出方法と同じである。なお、本実施形態では、１重位相差を用いる場合を説明するが、第１、第２の実施形態と同様に、２重位相差を用いることもできる。
【０１０８】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃで受信したＧＰＳ信号を捕捉・追尾する。情報処理装置は、擬似距離ρＡ，ρＢ，ρＣ、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣ、搬送波位相変化量ΔＰＹＡ，ΔＰＹＢ，ΔＰＹＣ、および衛星位置変化量ΔＰｓａｔＡ，ΔＰｓａｔＢ，ΔＰｓａｔＣを算出する（Ｓ５０１）。
【０１０９】
情報処理装置は、慣性センサ２０から加速度ａＩＭＵおよび角速度ωＩＭＵを取得する（Ｓ５０２）。
【０１１０】
情報処理装置は、第１、第２、第３のアンテナ１００Ａ，１００Ｂ，１００Ｃに対して、マスターアンテナとスレーブアンテナを設定することで組み合わせて、アンテナの組み合わせを複数種類設定する。この際、情報処理装置は、各アンテナをマスターアンテナとしてもスレーブアンテナとしても設定する。情報処理装置は、各アンテナの組み合わせにおいて、搬送波位相測定値ＰＹＡ，ＰＹＢ，ＰＹＣを用いて、アンテナ間の１重位相差であるアンテナ間位相差ΔζＡＢ，ΔζＢＣ，ΔζＣＡを算出する（Ｓ５０３）。
【０１１１】
情報処理装置は、擬似距離ρＡ，ρＢ，ρＣ、搬送波位相変化量ΔＰＹＡ，ΔＰＹＢ，ΔＰＹＣ、および衛星位置変化量ΔＰｓａｔＡ，ΔＰｓａｔＢ，ΔＰｓａｔＣと、前回の演算で得られた統合位置ＰＵＮ、統合速度ＶＵＮ、統合姿勢角ＡＴＵＮとを用いて、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを推定する（Ｓ５０４）。この際、情報処理装置は、カルマンフィルタ等を用いる。
【０１１２】
情報処理装置は、加速度ａＩＭＵ、角速度ωＩＭＵ、位置の算出誤差εＰ、速度の算出誤差εＶ、および姿勢角の算出誤差εＡＴを用いて、統合位置ＰＵＮ、統合速度ＶＵＮ、および統合姿勢角ＡＴＵＮを算出する（Ｓ５０５）。
【０１１３】
なお、上述の説明では、アンテナを３機用いる態様を示したが、４機以上であってもよい。図１１は、アンテナが４機である場合のアンテナの位置関係を示す図である。
【０１１４】
アンテナ部１０は、第１アンテナ１００Ａ、第２アンテナ１００Ｂ、第３アンテナ１００Ｃ、および、第４アンテナ１００Ｄを備える。
【０１１５】
図１１に示すように、第１アンテナ１００Ａと第２アンテナ１００Ｂは、これらのアンテナを結ぶ基線が船首と船尾を結ぶ方向（船首−船尾方向）に平行になるように、配置されている。第３アンテナ１００Ｃと第４アンテナ１００Ｄは、これらのアンテナを結ぶ基線が船首−船尾方向に平行になるように、配置されている。
【０１１６】
第１アンテナ１００Ａと第４アンテナ１００Ｄは、これらのアンテナを結ぶ基線が右舷と左舷を結ぶ方向（船首−船尾方向に直交する方向）に平行になるように、配置されている。第２アンテナ１００Ｂと第３アンテナ１００Ｃは、これらのアンテナを結ぶ基線が船首−船尾方向に直交する方向に平行になるように、配置されている。
【０１１７】
この構成では、第１アンテナ１００Ａ、第２アンテナ１００Ｂ、第３アンテナ１００Ｃ、および、第４アンテナ１００Ｄのいずれか１機が本発明の第１のＧＮＳＳアンテナに対応し、他のアンテナの内の２機が本発明の第２のＧＮＳＳアンテナに対応する。
【０１１８】
そして、この構成を用いても、上述の各実施形態に示した作用効果を得ることができる。
【０１１９】
なお、３機以上のアンテナの内の２機のアンテナを結ぶ基線が船首−船尾方向に平行になるように、アンテナを配置することが好ましい。このような配置を用いることによって、ヨー角ψの算出をさらに容易にすることができる。
【符号の説明】
【０１２０】
１０，１０Ａ，１０Ｂ：状態算出装置
１１Ａ，１１Ｂ，１１Ｃ：受信部
１２：位相差算出部
１３：演算部
２０：慣性センサ
２１：加速度センサ
２２：角速度センサ
１００：アンテナ部
１００Ａ，１００Ｂ，１００Ｃ，１００Ｄ：アンテナ
１３１Ａ，１３１Ｂ：誤差推定部
１３２Ａ，１３２Ｂ：統合処理部
１３３Ａ：ＧＮＳＳ速度算出部
１３４Ａ：ＧＮＳＳ位置算出部
【図１】
2016104033.tif 000003
【図２】
2016104033.tif 000004
【図３】
2016104033.tif 000005
【図４】
2016104033.tif 000006
【図５】
2016104033.tif 000007
【図６】
2016104033.tif 000008
【図７】
2016104033.tif 000009
【図８】
2016104033.tif 000010
【図９】
2016104033.tif 000011
【図１０】
2016104033.tif 000012
【図１１】
2016104033.tif 000013
【国際調査報告】
2016104033.tif 000014
2016104033.tif 000015
2016104033.tif 000016
2016104033.tif 000017