(57)【要約】【課題】観測値が少なくても、センサの誤差を高精度に推定して、移動情報を高精度に算出する。【解決手段】移動情報算出装置１０は、第１、第２誤差推定部１１Ａ，１１Ｂ、および第１、第２慣性航法演算部１２Ａ，１２Ｂを備える。第１誤差推定部１１Ａは、測位移動情報、センサ移動情報、および既に算出されている第１統合移動情報を用いて、慣性センサのセンサ誤差である第１センサ誤差を推定する。第２誤差推定部１１Ｂは、測位移動情報、センサ移動情報、および既に算出されている第２統合移動情報を用いて、第１センサ誤差と異なる種類のセンサ誤差である第２センサ誤差を推定する。第１、第２慣性航法演算部１２Ａ，１２Ｂは、第１センサ誤差と第２センサ誤差を用いてセンサ移動情報を補正した補正センサ移動情報から、今回の第１統合移動情報と第２統合移動情報を算出する。【選択図】  図１
2015102330.tif 000002
【特許請求の範囲】
【請求項１】
測位用信号から得られる測位移動情報と、慣性センサから得られるセンサ移動情報とを用いて、移動体の統合移動情報を算出する移動情報算出装置であって、
前記測位移動情報、前記センサ移動情報、および既に算出されている第１統合移動情報を用いて、前記慣性センサのセンサ誤差である第１センサ誤差を推定する第１誤差推定部と、
前記測位移動情報、前記センサ移動情報、および既に算出されている第２統合移動情報を用いて、前記第１センサ誤差と異なる種類の前記センサ誤差である第２センサ誤差を推定する第２誤差推定部と、
前記第１センサ誤差と前記第２センサ誤差を用いて前記センサ移動情報を補正した補正センサ移動情報から、今回の第１統合移動情報と第２統合移動情報を算出する慣性航法演算部と、
を備える、移動情報算出装置。
【請求項２】
請求項１に記載の移動情報算出装置であって、
前記第１誤差推定部は、前記第１センサ誤差とともに前記第１統合移動情報の算出誤差である第１算出誤差を推定し、
前記第２誤差推定部は、前記第２センサ誤差とともに前記第２統合移動情報の算出誤差である第２算出誤差を推定し、
前記慣性航法演算部は、
前記第１センサ誤差、前記第２センサ誤差、および前記第１算出誤差を用いて前記センサ移動情報を補正した第１補正センサ移動情報から今回の第１統合移動情報を算出するとともに、当該第１統合移動情報を前記第１誤差推定部にフィードバックする第１慣性航法演算部と、
前記第１センサ誤差、前記第２センサ誤差、および前記第２算出誤差を用いて前記センサ移動情報を補正した第２補正センサ移動情報から今回の第２統合移動情報を算出するとともに、当該第２統合移動情報を前記第２誤差推定部にフィードバックする第２慣性航法演算部と、を備える、
移動情報算出装置。
【請求項３】
請求項２に記載の移動情報算出装置であって、
前記第１統合移動情報と前記第２統合移動情報のいずれか、もしくは、前記第１統合移動情報と前記第２統合移動情報に基づく代表値を外部に出力する統合移動情報決定部を、
さらに備える、移動情報算出装置。
【請求項４】
請求項１乃至請求項３のいずれかに記載の移動情報算出装置であって、
前記第１センサ誤差はバイアス誤差であり、
前記第２センサ誤差はスケールファクタ誤差である、
移動情報算出装置。
【請求項５】
請求項１乃至請求項４のいずれかに記載の移動情報算出装置であって、
前記第１誤差推定部と前記第２誤差推定部は、観測値が同じであって未知数が異なるカルマンフィルタからなる、移動情報算出装置。
【請求項６】
請求項１乃至請求項５のいずれかに記載の移動情報算出装置と、
前記測位移動情報を算出する測位用信号受信部と、
前記センサ移動情報を算出する前記慣性センサを備える慣性姿勢計測装置と、
を備える、移動情報算出システム。
【請求項７】
請求項１乃至請求項５のいずれかに記載の移動情報算出装置または請求項６に記載の移動情報算出システムの各部と、
前記統合移動情報に基づいて移動制御を行う制御部と、
前記移動制御に基づいて駆動制御される動力発生部と、
を備える、移動体。
【請求項８】
測位用信号から得られる測位移動情報と、慣性センサから得られるセンサ移動情報とを用いて、移動体の統合移動情報を算出する移動情報算出方法であって、
前記測位移動情報、前記センサ移動情報、および既に算出されている第１統合移動情報を用いて、前記慣性センサのセンサ誤差である第１センサ誤差を推定する第１誤差推定工程と、
前記測位移動情報、前記センサ移動情報、および既に算出されている第２統合移動情報を用いて、前記第１センサ誤差と異なる種類の前記センサ誤差である第２センサ誤差を推定する第２誤差推定工程と、
前記第１センサ誤差と前記第２センサ誤差を用いて前記センサ移動情報を補正した補正センサ移動情報から、今回の第１統合移動情報と第２統合移動情報を算出する慣性航法演算工程と、
を有する、移動情報算出方法。
【請求項９】
請求項８に記載の移動情報算出方法であって、
前記第１誤差推定工程は、前記第１センサ誤差とともに前記第１統合移動情報の算出誤差である第１算出誤差を推定し、
前記第２誤差推定工程は、前記第２センサ誤差とともに前記第２統合移動情報の算出誤差である第２算出誤差を推定し、
前記慣性航法演算工程は、
前記第１センサ誤差、前記第２センサ誤差、および前記第１算出誤差を用いて前記センサ移動情報を補正した第１補正センサ移動情報から今回の第１統合移動情報を算出するとともに、当該第１統合移動情報を前記第１誤差推定部にフィードバックする第１慣性航法演算工程と、
前記第１センサ誤差、前記第２センサ誤差、および前記第２算出誤差を用いて前記センサ移動情報を補正した第２補正センサ移動情報から今回の第２統合移動情報を算出するとともに、当該第２統合移動情報を前記第２誤差推定部にフィードバックする第２慣性航法演算工程と、を有する、
移動情報算出方法。
【請求項１０】
請求項９に記載の移動情報算出方法であって、
前記第１統合移動情報と前記第２統合移動情報のいずれか、もしくは、前記第１統合移動情報と前記第２統合移動情報に基づく代表値を外部に出力する統合移動情報決定工程を、
さらに有する、移動情報算出方法。
【請求項１１】
測位用信号から得られる測位移動情報と、慣性センサから得られるセンサ移動情報とを用いて、移動体の統合移動情報を算出する処理をコンピュータに実行させる移動情報算出プログラムであって、
前記コンピュータは、
前記測位移動情報、前記センサ移動情報、および既に算出されている第１統合移動情報を用いて、前記慣性センサのセンサ誤差である第１センサ誤差を推定する第１誤差推定処理と、
前記測位移動情報、前記センサ移動情報、および既に算出されている第２統合移動情報を用いて、前記第１センサ誤差と異なる種類の前記センサ誤差である第２センサ誤差を推定する第２誤差推定処理と、
前記第１センサ誤差と前記第２センサ誤差を用いて前記センサ移動情報を補正した補正センサ移動情報から、今回の第１統合移動情報と第２統合移動情報を算出する慣性航法演算処理と、
を実行する、移動情報算出プログラム。
【請求項１２】
請求項１１に記載の移動情報算出プログラムであって、
前記コンピュータは、
前記第１誤差推定処理として、前記第１センサ誤差とともに前記第１統合移動情報の算出誤差である第１算出誤差を推定し、
前記第２誤差推定処理として、前記第２センサ誤差とともに前記第２統合移動情報の算出誤差である第２算出誤差を推定し、
前記慣性航法演算処理として、
前記第１センサ誤差、前記第２センサ誤差、および前記第１算出誤差を用いて前記センサ移動情報を補正した第１補正センサ移動情報から今回の第１統合移動情報を算出するとともに、当該第１統合移動情報を前記第１誤差推定部にフィードバックする第１慣性航法演算処理と、
前記第１センサ誤差、前記第２センサ誤差、および前記第２算出誤差を用いて前記センサ移動情報を補正した第２補正センサ移動情報から今回の第２統合移動情報を算出するとともに、当該第２統合移動情報を前記第２誤差推定部にフィードバックする第２慣性航法演算処理と、を実行する、
移動情報算出プログラム。
【請求項１３】
請求項１２に記載の移動情報算出プログラムであって、
前記コンピュータは、
前記第１統合移動情報と前記第２統合移動情報のいずれか、もしくは、前記第１統合移動情報と前記第２統合移動情報に基づく代表値を外部に出力する統合移動情報決定処理を、
実行する、移動情報算出プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、慣性センサを備えた慣性姿勢計測装置（ＩＭＵ：Ｉｎｔｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）と、測位用信号から測位を行う測位装置とを備え、ＩＭＵの出力結果と測位装置の出力結果とから統合的な移動情報を算出する移動情報算出装置、移動情報算出方法、および移動情報算出プログラムに関する。
【背景技術】
【０００２】
従来、慣性姿勢計測装置（ＩＭＵ：Ｉｎｔｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ（以下、ＩＭＵと称する。））と測位装置とを備えた移動情報算出装置が各種考案されている。ＩＭＵは、ジャイロセンサや加速度センサ等の慣性センサを備えており、慣性力を利用して、角速度、姿勢角、加速度を計測する。測位装置は、複数の測位衛星から放送される測位用信号を受信し、各測位衛星と測位装置との距離を算出する。測位装置は、当該距離および距離の変化から、位置、速度を算出する。
【０００３】
一般的に、測位用衛星の観測条件が良好であれば、測位装置によって算出される位置および速度は高精度であるが、観測条件は常時良好であるとは限らない。
【０００４】
したがって、特許文献１のナビゲーション装置に示すように、ＩＭＵで計測した角速度、姿勢角、加速度を、測位装置で算出した位置および速度を補正して利用する統合処理が、現在、移動体の移動情報算出に多く利用されている。
【０００５】
例えば、特許文献１のナビゲーション装置では、ＧＰＳ方位（測位による方位）を用いてジャイロセンサの誤差を補正している。また、特許文献１のナビゲーション装置では、ＧＰＳ速度（測位による速度）を用いて車速センサの誤差を補正している。
【０００６】
ジャイロセンサや車速センサ等のセンサの生じる誤差は、ミスアライメント誤差、バイアス誤差およびスケールファクタ誤差がある。特許文献１に記載のナビゲーション装置では、各センサのバイアス誤差とスケールファクタ誤差は、センサ毎に設定されたカルマンフィルタによって推定されている。すなわち、ジャイロセンサのバイアス誤差およびスケールファクタ誤差は、ジャイロセンサ用のカルマンフィルタによって推定されており、車速センサのバイアス誤差およびスケールファクタ誤差は、車速センサ用のカルマンフィルタによって推定される。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平７−３０１５４１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１に記載のナビゲーション装置のような従来の移動情報算出装置では、誤差を高精度に推定する場合、すなわち、最終的に出力される速度や位置を高精度に算出する場合、次の問題が生じる。
【０００９】
センサの誤差を高精度に推定する場合、上述のバイアス誤差およびスケールファクタ誤差と誤差要因毎に推定算出するだけでなく、各誤差要因を、移動体における直交三軸の誤差成分毎に推定する必要がある。すなわち、センサの誤差を高精度に推定する場合、推定演算で推定する未知数が多くなる。
【００１０】
一方、このような推定では、未知数の数以上の既知数（観測値）を得なければならず、未知数が増加すると、観測値も多く必要となる。この場合、観測値は、測位装置によって算出される位置および速度や、これら位置や速度の元となる擬似距離やドップラ周波数である。このような観測値の数は、推定を行うタイミングにおいて観測可能な衛星数、言い換えれば復調可能な測位用信号数によって決まってしまう。
【００１１】
したがって、上述の移動情報算出装置では、センサの誤差推定を高精度に行おうとすると、観測可能な測位用衛星数が必要数得られない場合が生じ、結果的に推定誤差の精度が低下してしまう。また、観測可能な測位用衛星数が少なくてもセンサの誤差推定を行えるようにした場合には、測位用衛星数が少ないことにより、高精度な誤差推定を行うことができない。
【００１２】
本発明の目的は、観測可能な測位用衛星数が少なくても、センサの誤差を高精度に推定できる移動情報算出装置を提供することにある。
【課題を解決するための手段】
【００１３】
この発明は、測位用信号から得られる測位移動情報と、慣性センサから得られるセンサ移動情報とを用いて、移動体の統合移動情報を算出する移動情報算出装置に関するものであり、次の特徴を有する。移動情報算出装置は、第１誤差推定部、第２誤差推定部、および慣性航法演算部を備える。第１誤差推定部は、測位移動情報、センサ移動情報、および既に算出されている第１統合移動情報を用いて、慣性センサのセンサ誤差である第１センサ誤差を推定する。第２誤差推定部は、測位移動情報、センサ移動情報、および既に算出されている第２統合移動情報を用いて、第１センサ誤差と異なる種類のセンサ誤差である第２センサ誤差を推定する。慣性航法演算部は、第１センサ誤差と第２センサ誤差を用いてセンサ移動情報を補正した補正センサ移動情報から、今回の第１統合移動情報と第２統合移動情報を算出する。
【００１４】
この構成では、センサ誤差を構成する各誤差成分が、複数の誤差推定部で個別に推定される。したがって、各誤差推定部の未知数を低減させることができる。これにより、推定精度を低下させることなく、誤差成分の推定に必要な既知数（測位移動情報およびセンサ移動情報の数）を少なくできる。
【００１５】
また、この発明の移動情報算出装置では、第１誤差推定部は、第１センサ誤差とともに第１統合移動情報の算出誤差である第１算出誤差を推定する。第２誤差推定部は、第２センサ誤差とともに第２統合移動情報の算出誤差である第２算出誤差を推定する。
【００１６】
慣性航法演算部は、第１慣性航法演算部と第２慣性航法演算部を備える。第１慣性航法演算部は、第１センサ誤差、第２センサ誤差、および第１算出誤差を用いてセンサ移動情報を補正した第１補正センサ移動情報から今回の第１統合移動情報を算出するとともに、当該第１統合移動情報を第１誤差推定部にフィードバックする。第２慣性航法演算部は、第１センサ誤差、第２センサ誤差、および第２算出誤差を用いてセンサ移動情報を補正した第２補正センサ移動情報から今回の第２統合移動情報を算出するとともに、当該第２統合移動情報を第２誤差推定部にフィードバックする。
【００１７】
この構成では、センサ誤差の誤差成分毎に誤差推定と統合移動情報の算出のループ処理が個別に構成される。これにより、それぞれ個別のループ処理で各誤差成分を確実且つ高精度に推定でき、高精度な統合移動情報を算出することができる。
【００１８】
また、この発明の移動情報算出装置では、第１統合移動情報と第２統合移動情報のいずれか、もしくは、第１統合移動情報と第２統合移動情報に基づく代表値を外部に出力する統合移動情報決定部をさらに備える。
【００１９】
この構成では、異なるループ処理で得られた複数の統合移動情報を用いて出力用の統合移動情報が決定されるので、より高精度な統合移動情報を算出することが可能になる。
【００２０】
また、この発明の移動情報算出システムは、上述のいずれかに記載の移動情報算出装置、測位移動情報を算出する測位用信号受信部、および、センサ移動情報を算出する慣性センサを備える慣性姿勢計測装置と、備える。
【００２１】
この構成では、測位用信号を受信するアンテナを測位用信号受信部に接続するだけで、統合移動情報を高精度に算出することができる。
【００２２】
また、この発明の移動体は、上述のいずれかに記載の移動情報算出装置または移動情報算出システムの各部と、統合移動情報に基づいて移動制御を行う制御部と、移動制御に基づいて駆動制御される動力発生部と、を備える。
【００２３】
この構成では、上述のように算出された移動情報を用いることで、定常的に移動体を高精度に移動制御することができる。
【発明の効果】
【００２４】
この発明によれば、センサの誤差を高精度に推定して、速度や位置等の移動体に対する移動情報を高精度に算出することができる。
【図面の簡単な説明】
【００２５】
【図１】本発明の第１の実施形態に係る移動情報算出装置のブロック図である。
【図２】本発明の第１の実施形態に係る移動情報算出方法のフローチャートである。
【図３】本発明の第２の実施形態に係る移動情報算出装置のブロック図である。
【図４】本発明の第２の実施形態に係る移動情報算出方法のフローチャートである。
【図５】本発明の第３の実施形態に係る移動情報算出装置のブロック図である
【図６】本発明の第４の実施形態に係る移動情報算出装置のブロック図である。
【図７】本発明の実施形態に係る移動体の概略構成図である。
【発明を実施するための形態】
【００２６】
本発明の第１の実施形態に係る移動情報算出装置および移動情報算出方法について、図を参照して説明する。図１は、本発明の第１の実施形態に係る移動情報算出装置のブロック図である。図２は、本発明の第１の実施形態に係る移動情報算出方法のフローチャートである。
【００２７】
移動情報算出装置１０１は、第１統合処理部１０Ａ、および第２統合処理部１０Ｂを備える。
【００２８】
第１統合処理部１０Ａは、第１誤差推定部１１Ａと第１慣性航法演算部１２Ａを備える第１慣性航法演算部１２Ａは、第１ＩＭＵ誤差補正部１２１Ａ、第１位置速度算出部１２２Ａ、および第１姿勢角算出部１２３Ａを備える。
【００２９】
第１誤差推定部１１Ａには、測位用信号であるＧＰＳ信号を受信することによって算出された位置（ＧＰＳ位置）ＰＧＰＳ、速度（ＧＰＳ速度）ＶＧＰＳが入力される。これら位置ＰＧＰＳ、速度ＶＧＰＳは、測位移動情報に相当する。これらの位置ＰＧＰＳ、速度ＶＧＰＳは、複数の測位用信号を追尾して得られる、コード擬似距離やドップラ周波数によって、既知の方法を算出される。また、第１誤差推定部１１Ａには、ＩＭＵによって計測された加速度（ＩＭＵ加速度）ａＩＭＵと角速度（ＩＭＵ角速度）ωＩＭＵが入力される。これら加速度ａＩＭＵと角速度ωＩＭＵが本発明のセンサ移動情報に相当する。
【００３０】
また、第１誤差推定部１１Ａには、前のサンプリングタイミングでの観測値（位置ＰＧＰＳ、速度ＶＧＰＳ、加速度ａＩＭＵおよび角速度ωＩＭＵ）と、当該前のサンプリングタイミングで推定したバイアス誤差Ｅｒｂｉ、スケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡとによって算出された統合位置Ｐｕｎ、統合速度Ｖｕｎ、および統合姿勢角Φｕｎが入力される。これら統合位置Ｐｕｎ、統合速度Ｖｕｎ、および統合姿勢角Φｕｎが第１統合移動情報に相当する。なお、バイアス誤差Ｅｒｂｉおよびスケールファクタ誤差ＥｒＳＦは、第１、第２統合処理部１０Ａ，１０Ｂに加速度ａＩＭＵと角速度ωＩＭＵを与えるＩＭＵ（図示せず）に備えられた慣性センサ（ジャイロセンサや加速度センサ）のセンサ誤差に相当する。
【００３１】
第１誤差推定部１１Ａは、状態推定方程式であるカルマンフィルタによって構成されている。このカルマンフィルタは、位置ＰＧＰＳ、速度ＶＧＰＳ、統合位置Ｐｕｎ、統合速度Ｖｕｎ、および統合姿勢角Φｕｎが観測値として与えられ、バイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡが未知数に設定されている。
【００３２】
第１誤差推定部１１Ａは、各サンプリングタイミングにおいて、上述の観測値を入力して、未知数であるバイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを推定算出する。第１誤差推定部１１Ａは、推定算出した速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを第１位置速度算出部１２２Ａに出力する。第１誤差推定部１１Ａは、推定算出した姿勢角算出誤差ＥｒΦＡを姿勢角算出部１２３Ａに出力する。これら姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡが本発明の第１算出誤差に相当する。
【００３３】
第１誤差推定部１１Ａは、推定算出したバイアス誤差Ｅｒｂｉを、第１ＩＭＵ誤差補正部１２１Ａに出力する。さらに、第１誤差推定部１１Ａは、推定算出したバイアス誤差Ｅｒｂｉを、第２慣性航法演算部１２Ｂの第２ＩＭＵ誤差補正部１２１Ｂに出力する。このバイアス誤差Ｅｒｂｉが本発明の第１センサ誤差に相当する。
【００３４】
第１ＩＭＵ誤差補正部１２１Ａには、ＩＭＵによって計測された加速度（ＩＭＵ加速度）ａＩＭＵと角速度（ＩＭＵ角速度）ωＩＭＵが入力される。また、第１ＩＭＵ誤差補正部１２１Ａには、第１誤差推定部１１Ａで推定されたバイアス誤差Ｅｒｂｉが入力される。さらには、第１ＩＭＵ誤差補正部１２１Ａには、後述する第２統合処理部１０Ｂの第２誤差推定部１１Ｂで推定されたスケールファクタ誤差ＥｒＳＦが入力される。
【００３５】
第１ＩＭＵ誤差補正部１２１Ａは、加速度ａＩＭＵと角速度ωＩＭＵを、バイアス誤差Ｅｒｂｉおよびスケールファクタ誤差ＥｒＳＦによって補正して、出力する。第１ＩＭＵ誤差補正部１２１Ａは、補正後の加速度ａＩＭＵを第１位置速度算出部１２２Ａに出力し、補正後の角速度ωＩＭＵを第１姿勢角算出部１２３Ａに出力する。これら補正後の加速度ａＩＭＵおよび補正後の角速度ωＩＭＵが本発明の補正後センサ移動情報に相当する。
【００３６】
第１位置速度算出部１２２Ａは、補正後の加速度ａＩＭＵから統合位置Ｐｕｎおよび統合速度Ｖｕｎを算出する。例えば、第１位置速度算出部１２２Ａは、補正後の加速度ａＩＭＵを積算することで統合速度Ｖｕｎを算出し、当該速度Ｖｕｎと予め別の方法で取得した初期値とを用いて統合位置Ｐｕｎを算出する。この際、第１位置速度算出部１２２Ａは、第１誤差推定部１１Ａで推定された速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを用いて、統合位置Ｐｕｎおよび統合速度Ｖｕｎを算出する。
【００３７】
第１位置速度算出部１２２Ａは、統合位置Ｐｕｎおよび統合速度Ｖｕｎを移動情報算出装置１０１の外部に出力するとともに、第１誤差推定部１１Ａにフィードバックする。
【００３８】
第１姿勢角算出部１２３Ａは、補正後の角速度ωＩＭＵから統合姿勢角Φｕｎを算出する。例えば、第１姿勢角算出部１２３Ａは、補正後の角速度ωＩＭＵを積算することで統合姿勢角Φｕｎを算出する。この際、第１姿勢角算出部１２３Ａは、推定された姿勢角算出誤差ＥｒΦＡを用いて、統合姿勢角Φｕｎを算出する。
【００３９】
第１姿勢角算出部１２３Ａは、統合姿勢角Φｕｎを移動情報算出装置１０１の外部に出力するとともに、第１誤差推定部１１Ａにフィードバックする。
【００４０】
このような構成により、第１誤差推定部１１Ａは、バイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを高精度に推定し、第１慣性航法演算部１２Ａは、統合位置Ｐｕｎ、統合速度Ｖｕｎおよび統合姿勢角Φｕｎを高精度に算出することができる。
【００４１】
第２統合処理部１０Ｂは、第２誤差推定部１１Ｂと第２慣性航法演算部１２Ｂを備える。第２慣性航法演算部１２Ｂは、第２ＩＭＵ誤差補正部１２１Ｂ、第２位置速度算出部１２２Ｂ、および第２姿勢角算出部１２３Ｂを備える。
【００４２】
第２誤差推定部１１Ｂには、測位用信号であるＧＰＳ信号を受信することによって算出された位置ＰＧＰＳ、速度ＶＧＰＳが入力される。また、第２誤差推定部１１Ｂには、ＩＭＵによって計測された加速度（ＩＭＵ加速度）ａＩＭＵと角速度（ＩＭＵ角速度）ωＩＭＵが入力される。これらの位置ＰＧＰＳ、速度ＶＧＰＳ、加速度（ＩＭＵ加速度）ａＩＭＵ、および角速度（ＩＭＵ角速度）ωＩＭＵは、第１誤差推定部１１Ａに入力されるものと同じであり、入力タイミングも同期している。
【００４３】
また、第２誤差推定部１１Ｂには、前のサンプリングタイミングでの観測値（位置ＰＧＰＳ、速度ＶＧＰＳ、加速度ａＩＭＵおよび角速度ωＩＭＵ）と、当該前のサンプリングタイミングで推定したバイアス誤差Ｅｒｂｉ、スケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢとによって算出された統合位置ＰｕｎＢ、統合速度ＶｕｎＢ、および統合姿勢角ΦｕｎＢが入力される。これら統合位置ＰｕｎＢ、統合速度ＶｕｎＢ、および統合姿勢角ΦｕｎＢが第２統合移動情報に相当する。
【００４４】
第２誤差推定部１１Ｂは、カルマンフィルタによって構成されている。このカルマンフィルタは、位置ＰＧＰＳ、速度ＶＧＰＳ、統合位置ＰｕｎＢ、統合速度ＶｕｎＢ、および統合姿勢角ΦｕｎＢが観測値として与えられ、バイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢが未知数に設定されている。
【００４５】
第２誤差推定部１１Ｂは、各サンプリングタイミングにおいて、上述の観測値を入力して、未知数であるスケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを推定算出する。第２誤差推定部１１Ｂは、推定算出した速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを第２位置速度算出部１２２Ｂに出力する。第２誤差推定部１１Ｂは、推定算出した姿勢角算出誤差ＥｒΦＢを第２姿勢角算出部１２３Ｂに出力する。これら姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢが本発明の第２算出誤差に相当する。
【００４６】
第２誤差推定部１１Ｂは、推定算出したスケールファクタ誤差ＥｒＳＦを、第２ＩＭＵ誤差補正部１２１Ｂに出力する。さらに、第２誤差推定部１１Ｂは、推定算出したスケールファクタ誤差ＥｒＳＦを、第１慣性航法演算部１２Ａの第１ＩＭＵ誤差補正部１２１Ａに出力する。このスケールファクタ誤差ＥｒＳＦが本発明の第２センサ誤差に相当する。
【００４７】
第２ＩＭＵ誤差補正部１２１Ｂには、ＩＭＵによって計測された加速度ａＩＭＵと角速度ωＩＭＵが入力される。これら加速度ａＩＭＵと角速度ωＩＭＵは、第１ＩＭＵ誤差補正部１２１Ａに入力されるものと同じであり、入力タイミングも同期している。
【００４８】
また、第２ＩＭＵ誤差補正部１２１Ｂには、第２誤差推定部１１Ｂで推定されたスケールファクタ誤差ＥｒＳＦが入力される。さらには、第２ＩＭＵ誤差補正部１２１Ｂには、第１統合処理部１０Ａの第１誤差推定部１１Ａで推定されたバイアス誤差Ｅｒｂｉが入力される。
【００４９】
第２ＩＭＵ誤差補正部１２１Ｂは、加速度ａＩＭＵと角速度ωＩＭＵを、バイアス誤差Ｅｒｂｉおよびスケールファクタ誤差ＥｒＳＦによって補正して、出力する。第２ＩＭＵ誤差補正部１２１Ｂは、補正後の加速度ａＩＭＵを第２位置速度算出部１２２Ｂに出力し、補正後の角速度ωＩＭＵを第２姿勢角算出部１２３Ｂに出力する。
【００５０】
第２位置速度算出部１２２Ｂは、補正後の加速度ａＩＭＵから統合位置ＰｕｎＢおよび統合速度ＶｕｎＢを算出する。例えば、第２位置速度算出部１２２Ｂは、補正後の加速度ａＩＭＵを積算することで統合速度ＶｕｎＢを算出し、当該統合速度ＶｕｎＢと予め別の方法で取得した初期値とを用いて統合位置Ｐｕｎを算出する。この際、第２位置速度算出部１２２Ｂは、第２誤差推定部１１Ｂで推定された速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを用いて、統合位置ＰｕｎＢおよび統合速度ＶｕｎＢを算出する。
【００５１】
第２位置速度算出部１２２Ｂは、統合位置ＰｕｎＢおよび統合速度ＶｕｎＢを、第２誤差推定部１１Ｂにフィードバックする。
【００５２】
第２姿勢角算出部１２３Ｂは、補正後の角速度ωＩＭＵから統合姿勢角ΦｕｎＢを算出する。例えば、第２姿勢角算出部１２３Ｂは、補正後の角速度ωＩＭＵを積算することで統合姿勢角ΦｕｎＢを算出する。この際、第２姿勢角算出部１２３Ｂは、推定された姿勢角算出誤差ＥｒΦＢを用いて、統合姿勢角ΦｕｎＢを算出する。
【００５３】
第２姿勢角算出部１２３Ｂは、統合姿勢角ΦｕｎＢを第２誤差推定部１１Ｂにフィードバックする。
【００５４】
このような構成により、第２誤差推定部１１Ｂは、スケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを高精度に推定し、第２慣性航法演算部１２Ｂは、統合位置ＰｕｎＢ、統合速度ＶｕｎＢおよび統合姿勢角ΦｕｎＢを高精度に算出することができる。
【００５５】
さらに、本実施形態の構成では、第１誤差推定部１１ＡでＩＭＵのセンサに対するバイアス誤差Ｅｒｂｉを推定し、第２誤差推定部１１ＢでＩＭＵのセンサに対するスケールファクタ誤差ＥｒＳＦを推定する。すなわち、対象が同じセンサのセンサ誤差を構成するバイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦを、異なる誤差推定部で推定している。これにより、各誤差推定部で推定する未知数の数が低減し、必要とする観測値（既知数）の数を低減することができる。したがって、本実施形態の構成では、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦとを１つの誤差推定部で推定するよりも、少ない観測値の数で、推定精度を劣化させることなく推定することができる。
【００５６】
ここで、観測値は、上述のように、測位用信号の追尾によって得られるので、観測値の数が低減できることにより、追尾する必要がある測位用信号の数を少なくすることができる。これにより、従来構成よりも追尾可能な測位用信号の数が少なくても、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦを高精度に推定することができ、統合位置Ｐｕｎ、統合速度Ｖｕｎおよび統合姿勢角Φｕｎを高精度に算出することができる。
【００５７】
なお、上述の説明では、第１、第２誤差推定部１１Ａ，１１Ｂをカルマンフィルタで構成する例を示したが、他の状態推定方程式を用いてもよい。また、第１、第２誤差推定部１１Ａ，１１Ｂが個別に構成されていれば、慣性航法演算部を統合することも可能である。
【００５８】
また、上述の構成では、第１統合処理部１０Ａから出力される統合位置Ｐｕｎ、統合速度Ｖｕｎおよび統合姿勢角Φｕｎを外部に出力する例を示したが、第２統合処理部１０Ｂから出力される統合位置ＰｕｎＢ、統合速度ＶｕｎＢおよび統合姿勢角ΦｕｎＢを外部に出力してもよい。
【００５９】
また、上述の説明では、移動情報の算出を、各機能ブロックに分離して行う場合を示している。しかしながら、上述の各機能をプログラム化して記憶おき、コンピュータ等の演算処理機で実行することで、移動情報を算出してもよい（図２参照）。
【００６０】
まず、観測値としてＧＰＳ速度ＶＧＰＳ、ＧＰＳ位置ＰＧＰＳと、ＩＭＵ角速度ωＩＭＵ、ＩＭＵ加速度ａＩＭＵを取得する（Ｓ１０１）。これらの観測値は、予め設定されたサンプリング間隔で継続的に取得される。
【００６１】
次に、バイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを未知数とする第１のカルマンフィルタを設定する。第１のカルマンフィルタの既知数（観測値）は、取得したＧＰＳ速度ＶＧＰＳ、ＧＰＳ位置ＰＧＰＳと、前回の算出処理で得られた統合速度Ｖｕｎ、統合加速度ａｕｎ、統合姿勢角Φｕｎである。そして、第１のカルマンフィルタを実行することで、バイアス誤差Ｅｒｂｉ、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡを推定算出する（Ｓ１０２）。
【００６２】
スケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを未知数とする第２のカルマンフィルタを設定する。第２のカルマンフィルタの既知数（観測値）は、取得したＧＰＳ速度ＶＧＰＳ、ＧＰＳ位置ＰＧＰＳと、前回の算出処理で得られた統合速度ＶｕｎＢ、統合加速度ａｕｎＢ、統合姿勢角ΦｕｎＢである。そして、第２のカルマンフィルタを実行することで、スケールファクタ誤差ＥｒＳＦ、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢを推定算出する（Ｓ１０３）。
【００６３】
次に、取得したＩＭＵ角速度ωＩＭＵ、ＩＭＵ加速度ａＩＭＵを、第１のカルマンフィルタで推定したバイアス誤差Ｅｒｂｉと、第２のカルマンフィルタで推定したスケールファクタ誤差ＥｒＳＦで補正する。補正後のＩＭＵ角速度ωＩＭＵ、ＩＭＵ加速度ａＩＭＵと、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡとを用いて、統合速度Ｖｕｎ、統合加速度ａｕｎ、統合姿勢角Φｕｎを算出する（Ｓ１０４）。このように算出された統合速度Ｖｕｎ、統合加速度ａｕｎ、統合姿勢角Φｕｎは、外部に出力されるとともに、第１のカルマンフィルタにフィードバックされる。
【００６４】
補正後のＩＭＵ角速度ωＩＭＵ、ＩＭＵ加速度ａＩＭＵと、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢとを用いて、統合速度ＶｕｎＢ、統合加速度ａｕｎＢ、統合姿勢角ΦｕｎＢを算出する（Ｓ１０５）。このように算出された統合速度ＶｕｎＢ、統合加速度ａｕｎＢ、統合姿勢角ΦｕｎＢは、第２のカルマンフィルタにフィードバックされる。
【００６５】
このような処理を行うことで、追尾可能な測位用信号の数が少なくても、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦを高精度に推定することができ、統合位置Ｐｕｎ、統合速度Ｖｕｎおよび統合姿勢角Φｕｎを高精度に算出することができる。
【００６６】
なお、ステップＳ１０２とステップＳ１０３は、同時並行に処理してもよく、時系列処理してもよい。また、ステップＳ１０４とステップＳ１０５は、同時並行に処理してもよく、時系列処理してもよい。
【００６７】
次に、第２の実施形態に係る移動情報算出装置および移動情報算出方法について、図を参照して説明する。図３は、本発明の第２の実施形態に係る移動情報算出装置のブロック図である。図４は、本発明の第２の実施形態に係る移動情報算出方法のフローチャートである。
【００６８】
本実施形態の移動情報算出装置１０２は、統合移動情報決定部１２０を備え、当該統合移動情報決定部１２０に関係する箇所を除き、第１の実施形態に示した移動情報算出装置１０１と同じである。したがって、異なる箇所のみを具体的に説明する。
【００６９】
統合移動情報決定部１２０には、第１統合処理部１０Ａから出力される第１の統合位置ＰｕｎＡ、第１の統合速度ＶｕｎＡおよび第１の統合姿勢角ΦｕｎＡと、第２統合処理部１０Ｂから出力される第２の統合位置ＰｕｎＢ、第２の統合速度ＶｕｎＢおよび第２の統合姿勢角ΦｕｎＢとが入力される。
【００７０】
統合移動情報決定部１２０は、第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢとから統合位置Ｐｕｎを決定する。具体的には、統合移動情報決定部１２０は、第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢとの平均値を算出して、統合位置Ｐｕｎとして出力する。なお、統合移動情報決定部１２０は、第１誤差推定部１１Ａを構成する第１のカルマンフィルタの誤差分散と、第２誤差推定部１１Ｂを構成する第２のカルマンフィルタの誤差分散とを比較し、誤差分散が小さい方の統合位置を、移動情報算出装置１０２としての統合位置Ｐｕｎとして出力してもよい。また、統合移動情報決定部１２０は、第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢとを重み付け加算平均処理してもよい。この際、重みは、上述のカルマンフィルタの誤差分散等で設定すればよい。この平均値等の第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢとに基づく、一般的に誤差を抑圧する統計的な演算値が本発明の「代表値」に相当する。以下の速度や姿勢角についても同様である。
【００７１】
統合移動情報決定部１２０は、第１の統合速度ＶｕｎＡと第２の統合速度ＰｕｎＢとから移動情報算出装置１０２としての統合速度Ｖｕｎを決定する。この統合速度Ｖｕｎの決定方法は、統合位置Ｐｕｎと同じである。
【００７２】
統合移動情報決定部１２０は、第１の統合姿勢角ΦｕｎＡと第２の統合姿勢角ΦｕｎＢとから移動情報算出装置１０２としての統合姿勢角Φｕｎを決定する。この統合姿勢角Φｕｎの決定方法は、統合位置Ｐｕｎや統合速度Ｖｕｎと同じである。
【００７３】
このような構成とすることで、移動情報算出装置１０２として、統合位置Ｐｕｎと統合速度Ｖｕｎと統合姿勢角Φｕｎを、より高精度に算出することができる。
【００７４】
なお、上述の説明では、移動情報の算出を、各機能ブロックに分割して行う場合を示している。しかしながら、上述の各機能をプログラム化して記憶おき、コンピュータ等の演算処理機で実行することで、移動情報を算出してもよい（図４参照）。
【００７５】
図４に示す移動情報算出方法（移動情報算出プログラム）は、統合位置Ｐｕｎと統合速度Ｖｕｎと統合姿勢角Φｕｎの算出処理が異なる点を除き、図２に示した移動情報算出方法（移動情報算出プログラム）と同じである。すなわち、図４のステップＳ２０１，Ｓ２０２，Ｓ２０３，Ｓ２０４は、図１のステップＳ１０１，Ｓ１０２，Ｓ１０３，Ｓ１０４のそれぞれと同じである。
【００７６】
第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢとが得られると、これら第１の統合位置ＰｕｎＡと第２の統合位置ＰｕｎＢから、外部出力する統合位置Ｐｕｎを決定する（Ｓ２０６）。この決定方法は、上述の統合移動情報決定部１２０による決定方法と同じである。
【００７７】
第１の統合速度ＶｕｎＡと第２の統合速度ＶｕｎＢとが得られると、これら第１の統合速度ＶｕｎＡと第２の統合速度ＶｕｎＢから、外部出力する統合速度Ｖｕｎを決定する（Ｓ２０７）。この決定方法は、上述の統合移動情報決定部１２０による決定方法と同じである。
【００７８】
第１の統合姿勢角ΦｕｎＡと第２の統合姿勢角ΦｕｎＢとが得られると、これら第１の統合姿勢角ΦｕｎＡと第２の統合姿勢角ΦｕｎＢから、外部出力する統合姿勢角Φｕｎを決定する（Ｓ２０８）。この決定方法は、上述の統合移動情報決定部１２０による決定方法と同じである。
【００７９】
このような方法を用いることで、統合位置Ｐｕｎと統合速度Ｖｕｎと統合姿勢角Φｕｎを、より高精度に算出することができる。
【００８０】
次に、第３の実施形態に係る移動情報算出装置について、図を参照して説明する。図５は、本発明の第３の実施形態に係る移動情報算出装置のブロック図である。
【００８１】
本実施形態の移動情報算出装置１０３は、統合処理部が三個ある場合を示すものであり、基本的な構成は、第１の実施形態に係る移動情報算出装置１０１と同じである。したがって、異なる箇所のみを具体的に説明する。
【００８２】
移動情報算出装置１０３は、第１統合処理部１０Ａ、第２統合処理部１０Ｂ、および第３統合処理部１０Ｃを備える。第１統合処理部１０Ａの第１誤差推定部１１Ａは、バイアス誤差Ｅｒｂｉを推定算出する。第２統合処理部１０Ｂの第２誤差推定部１１Ｂは、スケールファクタ誤差ＥｒＳＦを推定算出する。第３統合処理部１０Ｃの第３誤差推定部１１Ｃは、ミスアライメント誤差Ｅｒｍｉｓを推定算出する。
【００８３】
第１統合処理部１０Ａの第１慣性航法演算部１２Ａは、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦとミスアライメント誤差Ｅｒｍｉｓとで、角速度ωＩＭＵおよび加速度ａＩＭＵを補正する。第１慣性航法演算部１２Ａは、補正後の角速度ωＩＭＵおよび加速度ａＩＭＵと、姿勢角算出誤差ＥｒΦＡ、速度算出誤差ＥｒＶＡ、位置算出誤差ＥｒＰＡとを用いて、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎを算出する。第１慣性航法演算部１２Ａは、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎを外部に出力するとともに、第１誤差推定部１１Ａにフィードバックする。
【００８４】
第２統合処理部１０Ｂの第２慣性航法演算部１２Ｂは、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦとミスアライメント誤差Ｅｒｍｉｓとで、角速度ωＩＭＵおよび加速度ａＩＭＵを補正する。第２慣性航法演算部１２Ｂは、補正後の角速度ωＩＭＵおよび加速度ａＩＭＵと、姿勢角算出誤差ＥｒΦＢ、速度算出誤差ＥｒＶＢ、位置算出誤差ＥｒＰＢとを用いて、統合位置ＰｕｎＢ、統合速度ＶｕｎＢ、統合姿勢角ΦｕｎＢを算出する。第２慣性航法演算部１２Ｂは、統合位置ＰｕｎＢ、統合速度ＶｕｎＢ、統合姿勢角ΦｕｎＢを第２誤差推定部１１Ｂにフィードバックする。
【００８５】
第３統合処理部１０Ｃの第３慣性航法演算部１２Ｃは、バイアス誤差Ｅｒｂｉとスケールファクタ誤差ＥｒＳＦとミスアライメント誤差Ｅｒｍｉｓとで、角速度ωＩＭＵおよび加速度ａＩＭＵを補正する。第３慣性航法演算部１２Ｃは、補正後の角速度ωＩＭＵおよび加速度ａＩＭＵと、姿勢角算出誤差ＥｒΦＣ、速度算出誤差ＥｒＶＣ、位置算出誤差ＥｒＰＣとを用いて、統合位置ＰｕｎＣ、統合速度ＶｕｎＣ、統合姿勢角ΦｕｎＣを算出する。第３慣性航法演算部１２Ｃは、統合位置ＰｕｎＣ、統合速度ＶｕｎＣ、統合姿勢角ΦｕｎＣを誤差推定部１１Ｃにフィードバックする。
【００８６】
このような構成とすることで、観測値の数を増加させることなく、さらに誤差要因を多く推定することができる。これにより、センサの誤差補正をより高精度に行うことができ、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎを、より高精度に算出することができる。
【００８７】
なお、本実施形態の構成に、第２の実施形態に示した統合移動情報決定部を追加してもよい。
【００８８】
次に、本発明の第４の実施形態に係る移動情報算出システムについて、図を参照して説明する。図６は、本発明の第４の実施形態に係る移動情報算出システムのブロック図である。
【００８９】
本実施形態の移動情報算出システム１０４は、上述の第１の実施形態に示した移動情報算出装置１０１に対して、測位用信号受信部２０およびＩＭＵ３０を追加したものである。したがって、移動情報算出装置１０１の構成に関する具体的な説明は省略する。
【００９０】
移動情報算出システム１０４は、移動情報算出装置１０１と、測位用信号受信部２０およびＩＭＵ３０とを備える。
【００９１】
測位用信号受信部２０は、測位用信号であるＧＰＳ信号を受信して追尾し、追尾によるコード位相やキャリア位相から、擬似距離やドップラ周波数を算出する。測位用信号受信部２０は、擬似距離やドップラ周波数から位置ＰＧＰＳや速度ＶＧＰＳを算出し、第１、第２誤差推定部１１Ａ，１１Ｂに出力する。
【００９２】
ＩＭＵ３０は、ジャイロセンサや加速度センサを備えている。ＩＭＵ３０は、当該センサに加わる慣性力を検出することで、加速度ａＩＭＵや角速度ωＩＭＵを計測して、第１、第２ＩＭＵ誤差補正部１２１Ａ，１２１Ｂに出力する。
【００９３】
このような構成とすることで、当該移動情報算出システム１０４を移動体に装着して、測位用信号受信部２０をアンテナに接続するだけで、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎを、高精度に算出することができる。
【００９４】
なお、このような測位用信号受信部２０とＩＭＵ３０とを追加する構成を、他の実施形態に適用してもよい。
【００９５】
以上のような構成からなる移動情報算出装置または移動情報算出システムは、次に示すような移動体に装着される。図７は、本発明の実施形態に係る移動体の概略構成図である。なお、本実施形態の移動体は、第１の実施形態に係る移動情報算出装置１０１を備える例であり、他の実施形態の移動情報算出装置または移動情報算出システムを備えていてもよい。この際、第４の実施形態の移動情報算出装置１０４を備える場合、測位用信号受信部２０、ＩＭＵ３０は省略すればよい。
【００９６】
移動体４００は、移動情報算出装置１０１、測位用信号受信部２０、アンテナ２１、ＩＭＵ３０、制御部４０、および動力発生部５０を備える。
【００９７】
アンテナ２１は、移動体４００に装着され、測位用信号を受信して、測位用信号受信部２０へ出力する。測位用信号受信部２０は、測位用信号を用いて位置ＰＧＰＳや速度ＶＧＰＳを算出し、移動情報算出装置１０１へ出力する。
【００９８】
ＩＭＵ３０は、移動体４００に装着され、移動体４００の姿勢変化を検出して、加速度ａＩＭＵや角速度ωＩＭＵを計測し、移動情報算出装置１０１へ出力する。
【００９９】
移動情報算出装置１０１は、上述の方法により、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎを算出して、制御部４０へ出力する。
【０１００】
制御部４０は、統合位置Ｐｕｎ、統合速度Ｖｕｎ、統合姿勢角Φｕｎに基づいて、移動体４００が所望とする姿勢および移動状態となるように、動力発生部５０を制御する。
【０１０１】
このような構成とすることで、移動体４００は、所望の姿勢および移動状態（速度、加速度、回頭速度等）で移動することができる。そして、上述の構成からなる移動情報算出装置１０１を備えることで、移動体４００の姿勢や移動状態を高精度に算出できるので、移動体４００を高精度に移動制御することができる。
【符号の説明】
【０１０２】
１０１，１０２，１０３：移動情報算出装置
１０４：移動情報算出システム
１０Ａ：第１統合処理部，１０Ｂ：第２統合処理部，１０Ｃ：第３統合処理部
１１Ａ：第１誤差推定部，１１Ｂ：第２誤差推定部，１１Ｃ：第３誤差推定部
１２Ａ：第１慣性航法演算部，１２Ｂ：第２慣性航法演算部，１２Ｃ：第３慣性航法演算部
２０：測位用信号受信部
２１：アンテナ
３０：ＩＭＵ
４０：制御部
５０：動力発生部
１２０：統合移動情報決定部
１２１Ａ：第１ＩＭＵ誤差補正部，１２１Ｂ：第２ＩＭＵ誤差補正部
１２２Ａ：第１位置速度算出部，１２２Ｂ：第２位置速度算出部
１２３Ａ：第１姿勢角算出部，１２３Ｂ：第２姿勢角算出部
４００：移動体
【図１】
2015102330.tif 000003
【図２】
2015102330.tif 000004
【図３】
2015102330.tif 000005
【図４】
2015102330.tif 000006
【図５】
2015102330.tif 000007
【図６】
2015102330.tif 000008
【図７】
2015102330.tif 000009