(57)【要約】【課題】汎用の低精度な慣性計測装置を用いた場合であっても姿勢誤差を適切に補正することができる飛行体用航法装置を提供する。【解決手段】飛行体に搭載される飛行体用航法装置であって、前記飛行体に働く慣性力を検出する検出装置と、前記飛行体の静止時に、前記検出装置によって検出された加速度に対する重力加速度による補正の結果に基づいて、前記飛行体に対する前記検出装置の初期設定姿勢と取付姿勢との誤差成分のうち少なくとも一部である第１誤差を推定する第１推定部と、前記第１推定部によって推定された前記第１誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する測位部とを備える飛行体用航法装置。【選択図】図５
2018169368.tif 000002
【特許請求の範囲】
【請求項１】
飛行体に搭載される飛行体用航法装置であって、
前記飛行体に働く慣性力を検出する検出装置と、
前記飛行体の静止時に、前記検出装置によって検出された加速度に対する重力加速度による補正の結果に基づいて、前記飛行体に対する前記検出装置の初期設定姿勢と取付姿勢との誤差成分のうち少なくとも一部である第１誤差を推定する第１推定部と、
前記第１推定部によって推定された前記第１誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する測位部と
を備える飛行体用航法装置。
【請求項２】
前記飛行体の飛行中に、前記検出装置の検出結果および衛星から受信した電波に基づいて、前記初期設定姿勢と前記取付姿勢との誤差成分のうち少なくとも一部である第２誤差を推定する第２推定部をさらに備え、
前記測位部は、前記第１推定部によって推定された前記第１誤差および前記第２推定部によって推定された前記第２誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する、
請求項１に記載の飛行体用航法装置。
【請求項３】
前記第１誤差は、前記飛行体のピッチ軸およびヨー軸回りの誤差である、
請求項１または２に記載の飛行体用航法装置。
【請求項４】
前記第１推定部は、前記重力加速度による補正の結果に対してカルマンフィルタを適用して前記第１誤差を推定する、
請求項１から３のうちいずれか一項に記載の飛行体用航法装置。
【請求項５】
前記第２誤差は、前記飛行体のロール軸回りの誤差である、
請求項２に記載の飛行体用航法装置。
【請求項６】
前記第２推定部は、前記検出装置の検出結果から算出された前記飛行体の第１速度ベクトルと、前記衛星から受信した電波から算出された前記飛行体の第２速度ベクトルとの内積および外積に基づいて、前記第２誤差を推定する、
請求項５に記載の飛行体用航法装置。
【請求項７】
前記第１速度ベクトルおよび前記第２速度ベクトルは、地表面に対する水平方向の速度ベクトルである、
請求項６に記載の飛行体用航法装置。
【請求項８】
飛行体に搭載され、かつ、前記飛行体に働く慣性力を検出する検出装置を備えた飛行体用航法装置を用いた飛行体用航法であって、
前記検出装置によって検出された加速度に対する重力加速度による補正の結果に基づいて、前記飛行体に対する前記検出装置初期設定姿勢と取付姿勢との誤差成分のうち少なくとも一部である第１誤差を推定する工程と、
前記推定された前記第１誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する工程と、
を備える、飛行体用航法。
【請求項９】
前記飛行体の飛行中に、前記検出装置の検出結果および衛星から受信した電波に基づいて、前記初期設定姿勢と前記取付姿勢との誤差成分のうち少なくとも一部である第２誤差を推定する工程と、
前記推定された前記第１誤差および前記第２誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する工程と、
をさらに備える、請求項８に記載の飛行体用航法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、飛行体用航法装置および飛行体用航法に関する。
【背景技術】
【０００２】
近年、ロケットなどの飛行体の位置や速度を高精度で計測するための航法センサの研究が進められている（特許文献１、２参照）。この航法センサは、センサ源として慣性センサを備えており、位置や速度の計算のために予め設定された姿勢で飛行体に取り付けられる。しかしながら、この位置や速度の計算のために予め設定された姿勢と、航法センサの実際の取付姿勢との間に誤差（以下、「姿勢誤差」という）があると、飛行体の位置や速度を正確に計測することができない。
【０００３】
上記の姿勢誤差による影響を排除するために、航法センサの姿勢誤差を推定し、これを補正する方法が知られている。例えば、ロケット用の航法センサにおいては、姿勢誤差を、飛行体のロール軸、ピッチ軸、およびヨー軸の３つの軸方向の成分に分離し、ピッチ軸およびヨー軸回りの姿勢誤差については、重力加速度の方向ベクトルを用いて補正し、ロール軸回りの姿勢誤差については、地球自転角速度（アースレート）を用いて補正する方法が知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許第８８６８２５８号明細書
【特許文献２】米国特許第５７３９７８７号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ピッチ軸およびヨー軸回りの姿勢誤差に関して、重力加速度の方向ベクトルを用いた補正を行う場合、その補正の精度は慣性センサの精度に依存する。慣性センサの精度が悪いと慣性センサが検知する重力加速度の方向ベクトルそのものに誤差が生じる。このため、誤差を含む重力加速度の方向ベクトルを用いて補正を行った場合、姿勢誤差を適切に取り除くことができない。また、慣性センサの精度は、内蔵される加速度計やジャイロの精度に依存し、高精度なものほどコストが高くなる。低コスト化のため、慣性センサとしては比較的低精度な民生部品が使用される場合が多いため、重力加速度の方向ベクトルを用いた補正では飛行体の位置や速度を正確に計測することができない。
【０００６】
また、ロール軸回りの姿勢誤差に関して、アースレートを用いた補正を行う場合、飛行環境下で使用可能であり、かつ、補正に必要な精度でアースレートを検出可能な高価な慣性センサが必要となる。低価格な慣性センサでは、厳しい飛行環境下で使用可能とすることと、補正に必要な精度でアースレートを検出する性能の両立が難しい。従って、アースレートを用いない姿勢誤差の補正方法が求められている。
【０００７】
本発明は、このような事情を考慮してなされたものであり、汎用の低精度な慣性計測装置を用いた場合であっても姿勢誤差を適切に補正することが可能な飛行体用航法装置および飛行体用航法を提供することを目的の一つとする。
【課題を解決するための手段】
【０００８】
本発明の一態様は、飛行体に搭載される飛行体用航法装置であって、前記飛行体に働く慣性力を検出する検出装置と、前記飛行体の静止時に、前記検出装置によって検出された加速度に対する重力加速度による補正の結果に基づいて、前記飛行体に対する前記検出装置の初期設定姿勢と取付姿勢との誤差成分のうち少なくとも一部である第１誤差を推定する第１推定部と、前記第１推定部によって推定された前記第１誤差が除去された前記初期設定姿勢に基づいて、前記飛行体の位置および速度を導出する測位部とを備える。
【発明の効果】
【０００９】
本発明は、汎用の低精度な慣性計測装置を用いた場合であっても姿勢誤差を適切に補正することができる。
【図面の簡単な説明】
【００１０】
【図１】ロケット１の概略構成図である。
【図２】図１に示されたロケット１のＡ−Ａ’方向における断面図である。
【図３】飛行体用航法装置１００の構成図である。
【図４】電波航法測位モジュール１１０の構成図である。
【図５】航法計算モジュール１３０の構成図である。
【図６】航法計算モジュール１３０におけるレベリング処理の一例を示すフローチャートである。
【図７】ＥＮＵ座標系におけるピッチ角誤差εθＰを示す図である。
【図８】ＥＮＵ座標系におけるヨー角誤差εθＹを示す図である。
【図９】航法計算モジュール１３０における方位角推定処理の一例を示すフローチャートである。
【図１０】ＥＮＵ座標系におけるロール角誤差εθＲを示す図である。
【図１１】ＥＮＵ座標系における方位角誤差θＤを示す図である。
【図１２】レベリング処理のシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１１】
以下、図面を参照して、本発明に係る飛行体用航法装置および飛行体用航法の実施形態について説明する。飛行体用航法装置は、ロケットや人工衛星、宇宙探査機などの飛行体に搭載される装置である。以下、飛行体は、一例として、ロケットであるものとして説明するが、上述したように人工衛星や宇宙探査機であってもよい。以下、このような装置の構成および機能について段階的に開示する。
【００１２】
［ロケット］
図１は、ロケット１の概略構成図である。ロケット１は、例えば、多段ロケットであり、最上段には衛星を格納し、それ以下の段は、最上段を切り離した後、海などに着水し、或いは地上に着地する。ロケット１は、例えば、一段目ロケット１０と、二段目ロケット１２と、最上段の三段目ロケット１４とを備える。ロケット１は、発射された直後は一段目ロケット１０による推進力で飛行し、その後、一段目ロケット１０を切り離した後、二段目ロケット１２の推進力で飛行し、最終的に三段目ロケット１４が人口衛星などのペイロード２０を宇宙空間に到達させる。飛行体用航法装置１００は、例えば、二段目ロケット１２に格納される。
【００１３】
［飛行体用航法装置］
飛行体用航法装置１００が備える慣性計測装置は、ロケット１の位置や速度を高精度で計測するために、例えば、二段目ロケット１２内に予め設定された初期設定姿勢で正確に取り付けられる必要がある。しかしながら、慣性計測装置の実際の取付姿勢と、飛行体用航法装置１００の位置や速度の計算のために予め設定された初期設定姿勢との間に姿勢誤差が生じる場合がある。飛行体用航法装置１００は、この姿勢誤差を補正するための機能を備えている。
【００１４】
図２は、図１に示されたロケット１のＡ−Ａ’方向における断面図である。図２に示すように、飛行体用航法装置１００は、姿勢誤差を、ロケット１の進行方向の軸であるロール軸と、ロケット１の左右方向の軸であるピッチ軸、およびロケット１の上下方向の軸であるヨー軸の３つの軸方向の成分に分離して補正を行う。例えば、飛行体用航法装置１００は、ピッチ軸回りの姿勢誤差（以下、「ピッチ角誤差」という）およびヨー軸回りの姿勢誤差（以下、「ヨー角誤差」という）については、ロケット１の打ち上げ前の静止時に実施するレベリング処理により補正を行う。一方、ロール軸回りの姿勢誤差（以下、「ロール角誤差」という）については、ロケット１の打ち上げ後の飛行中に実施する方位角推定処理により補正を行う。レベリング処理および方位角推定処理の詳細については後述する。
【００１５】
図３は、飛行体用航法装置１００の構成図である。飛行体用航法装置１００は、一以上のアンテナＡＴと、通信装置ＴＭとに接続される。飛行体用航法装置１００は、例えば、電波航法測位モジュール１１０と、ＩＭＵ（Inertial Measurement Unit；慣性計測装置）１２０と、航法計算モジュール１３０と、電源モジュール１４０と、記憶部１５０とを備える。
【００１６】
アンテナＡＴは、外部から電波を受信し、受信した電波に応じた信号（以下、衛星測位信号と称する）をＬＮＡ（Low Noise Amplifier）に出力する。ＬＮＡは、アンテナＡＴから入力された衛星測位信号を増幅する。
【００１７】
電波航法測位モジュール１１０は、ＬＮＡにより増幅された衛星測位信号に基づいて、ロケット１の位置および速度を導出し、航法計算モジュール１３０に出力する。
【００１８】
ＩＭＵ１２０は、例えば、ＭＥＭＳ（Micro Electro Mechanical Systems）や光ファイバによって構成される三軸式加速度センサ、および三軸式ジャイロセンサを含む。ＩＭＵ１２０は、ロケット１に働く慣性力を検出する。ＩＭＵ１２０は、これらのセンサによって検出された値（水平方向、垂直方向、奥行き方向の各加速度、およびピッチ、ロール、ヨーの各レートなど）を航法計算モジュール１３０に出力する。ＩＭＵ１２０は、「検出装置」の一例である。
【００１９】
航法計算モジュール１３０は、ＩＭＵ１２０の実際の取付姿勢と、初期設定姿勢との間の姿勢誤差を補正する。また、航法計算モジュール１３０は、ＩＭＵ１２０により検出された検出値（検出結果）に基づいて、ロケット１の位置および速度を導出する。また、航法計算モジュール１３０は、電波航法測位モジュール１１０により導出された位置および速度と、ＩＭＵ１２０による検出結果を用いて導出された位置および速度とを統合してロケット１の位置および速度を導出する。
【００２０】
電源モジュール１４０は、図示しない電源装置に接続される。電源モジュール１４０は、例えば、保護回路とＤＣ−ＤＣコンバータを備え、飛行体用航法装置１００の各部に電力を供給する。
【００２１】
記憶部１５０は、ロケット１の位置および速度を導出する際の計算に利用される飛行体用航法装置１００のＩＭＵ１２０の初期設定姿勢のデータを記憶する。この初期設定姿勢は、ロケット１に対するＩＭＵ１２０の取付姿勢を予め定義したものである。航法計算モジュール１３０は、ＩＭＵ１２０により検出された検出値に基づいてロケット１の位置および速度を導出する際に、ＩＭＵ１２０が初期設定姿勢でロケット１に取り付けられていることを前提として位置および速度を算出する。
【００２２】
また、記憶部１５０は、電波航法測位モジュール１１０に入力された衛星測位信号や、ＩＭＵ１２０により検出された検出値、電波航法測位モジュール１１０および航法計算モジュール１３０により導出された各種演算結果（ロケット１の位置または速度）などを記憶する。記憶部１５０は、例えば、ＲＯＭ（Read Only Memory）、ＲＡＭ、ＥＥＰＲＯＭ（Electrically ErasableProgrammable Read Only Memory）などにより実現される。また、記憶部１５０は、ＨＤＤ（Hard Disc Drive）、フラッシュメモリなどにより実現されてもよい。
【００２３】
通信装置ＴＭは、例えば、テレメータ回線を利用して情報を地上監視装置（不図示）に送信する。地上監視装置は、例えば、ロケット１が発射された（飛び立った）後のロケット１の位置および速度を、テレメトリ通信によってロケット１から取得する。そして、地上監視装置は、仮にロケット１が落下した場合の落下地点を繰り返し推定し、落下による危険を回避するようにロケット１に指示信号を送信する。
【００２４】
［電波航法測位モジュール］
図４は、電波航法測位モジュール１１０の構成図である。電波航法測位モジュール１１０は、測位側インターフェース１１２と、電波航法測位部１１４とを備える。アンテナＡＴによって受信された衛星測位信号は、ＬＮＡによって増幅されて測位側インターフェース１１２に入力される。測位側インターフェース１１２は、例えば、ＲＦ（Radio Frequency）インターフェースである。
【００２５】
電波航法測位部１１４は、測位側インターフェース１１２に入力された衛星測位信号に対する処理を行ってロケット１の位置および速度を導出し、航法計算モジュール１３０に出力する。電波航法測位部１１４は、アンテナＡＴにより４つ以上の人工衛星から受信された電波の其々に応じた衛星測位信号に基づいて、人工衛星との相対距離（シュートレンジ）を求めることでロケット１の位置を算出する。
【００２６】
［航法計算モジュール］
図５は、航法計算モジュール１３０の構成図である。航法計算モジュール１３０は、例えば、レベリング部１３２と、方位角推定部１３４と、慣性航法測位部１３６とを備える。
【００２７】
レベリング部１３２は、ロケット１の打ち上げ前の機体静止時に、飛行体用航法装置１００の姿勢誤差におけるピッチ角誤差およびヨー角誤差を推定して除去する。レベリング部１３２は、ロケット１の静止時に、ＩＭＵ１２０によって検出された加速度に対する重力加速度による補正の結果に基づいて、ＩＭＵ１２０の初期設定姿勢と取付姿勢との誤差を構成する成分（ピッチ、ヨー、ロールの各軸回りの誤差成分）のうち少なくとも一部成分であるピッチ軸およびヨー軸回りの誤差（第１誤差の一例）を推定する。
【００２８】
姿勢誤差が生じている場合、重力加速度による補正後に“０”となるべき加速度に補正残差が発生する。これは、姿勢誤差によって、誤ったベクトル方向に重力補正を行うことで、重力補正残差が発生したためである。そこで、レベリング部１３２は、重力加速度による補正後の加速度の補正残差を積分し、算出された速度が“０”となるような姿勢角度を求めることで、飛行体用航法装置１００のピッチ角誤差およびヨー角誤差を推定して除去する。例えば、レベリング部１３２は、ＩＭＵ１２０の座標系（ＥＮＵ（East North Up）座標系）上で姿勢誤差をパラメータとした線形近似式を構築し、重力加速度による補正の結果に対してカルマンフィルタを適用してピッチ角誤差およびヨー角誤差を推定して除去する。レベリング部１３２は、「第１推定部」の一例である。
【００２９】
方位角推定部１３４は、ロケット１の打ち上げ後の飛行中に、飛行体用航法装置１００の姿勢誤差におけるロール角誤差を推定して除去する。方位角推定部１３４は、ロケット１の飛行中に、ＩＭＵ１２０の検出結果および衛星から受信した電波に基づいて、初期設定姿勢と取付姿勢との誤差を構成する成分のうち少なくとも一部成分であるロール軸回りの誤差（第２誤差の一例）を推定する。方位角推定部１３４は、打ち上げ後の飛行中にロケット１が姿勢を徐々に水平方向に変えていくタイミングで、機体の進行方向の速度ベクトルに地表面に対する水平成分が生じることを利用する。衛星測位信号に基づいて電波航法測位モジュール１１０によって導出される速度は方位角誤差（ロール角誤差）の影響を受けない。このため、方位角推定部１３４は、電波航法測位モジュール１１０によって導出される速度ベクトルを正のデータとして参照し、この速度ベクトルと、ＩＭＵ１２０の検出値に基づいて算出された速度ベクトルとを比較することで、ロール角誤差を推定する。
【００３０】
例えば、方位角推定部１３４は、ＩＭＵ１２０の三軸式加速度センサの検出値を積分することにより得られるロケット１の機体の水平方向の速度ベクトル、および、電波航法測位モジュール１１０によって導出される速度ベクトルの２つの速度ベクトルを、それぞれ個別に２次のデジタルローパスフィルタを通過させ、それらの内積および外積を求める。そして、方位角推定部１３４は、内積から誤差角度の大きさを推定し、外積から誤差方向を推定することで姿勢誤差を推定して除去する。方位角推定部１３４は、「第２推定部」の一例である。
【００３１】
これら２つの速度ベクトルが成す角度は、ロケット１の打ち上げ前にレベリング部１３２によってピッチ角誤差およびヨー角誤差が除去されている場合、ロール角誤差のみを含む。なお、ＩＭＵ１２０は、時間とともにバイアス誤差が蓄積するため、方位角推定部１３４は、ロケット１の姿勢が徐々に水平方向に変わるタイミングで上記の方位角推定処理を行う。
【００３２】
慣性航法測位部１３６は、ＩＭＵ１２０により検出された検出値、レベリング部１３２によって推定された姿勢誤差および／または方位角推定部１３４によって推定された姿勢誤差が除去された初期設定姿勢に基づいて、ロケット１の位置および速度を導出する。慣性航法測位部１３６は、ＩＭＵ１２０の三軸式加速度センサから入力された値を積分することで速度を算出し、更に、速度を積分することで変位（位置）を算出する。また、慣性航法測位部１３６は、ＩＭＵ１２０の三軸式加速度センサと三軸式ジャイロセンサから入力された値に基づいて、変位と方角のベクトルを合成することで、より正確な姿勢を算出する。慣性航法測位部１３６は、「測位部」の一例である。
【００３３】
また、慣性航法測位部１３６は、ＩＭＵ１２０により検出された検出値に基づいて導出されたロケット１の位置および速度と、電波航法測位モジュール１１０により導出されたロケット１の位置および速度とを統合し、ロケット１の位置および速度を導出する。慣性航法測位部１３６は、例えば、カルマンフィルタを適用することで両者を統合し、ロケット１の位置および速度を導出する。マルチパスの影響などにより、電波航法測位モジュール１１０がアンテナＡＴで衛星の電波を受信できない状況において、慣性航法測位部１３６は、直前までの電波航法測位モジュール１１０により導出されたロケット１の位置および速度を、ＩＭＵ１２０により検出された検出値に基づいて導出されたロケット１の位置および速度で補完する。
【００３４】
電波航法測位モジュール１１０および航法計算モジュール１３０の各々は、例えば、ＭＰＵ（Micro Processing Unit）で構成される。電波航法測位モジュール１１０は、例えば、プログラムメモリ（図示しない）に格納されたプログラムを実行することにより、電波航法測位部１１４を実現する。また、航法計算モジュール１３０は、例えば、プログラムメモリ（図示しない）に格納されたプログラムを実行することにより、レベリング部１３２と、方位角推定部１３４と、慣性航法測位部１３６とを実現する。なお、これらの機能部のうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）などのハードウェアによって実現されてもよいし、ソフトウェアとハードウェアが協働することで実現されてもよい。
【００３５】
［レベリング処理］
以下において、航法計算モジュール１３０のレベリング部１３２によって行われるレベリング処理について説明する。ＥＮＵ座標系のＥ軸およびＮ軸に対して、ＩＭＵ１２０の取付姿勢と、予め設定された初期設定姿勢との間に姿勢誤差が生じている場合、航法計算モジュール１３０における計算において、重力補正後に“０”となるべき加速度に補正残差が発生する。この重力補正後の加速度の補正残差の積分によって発生する速度が“０”になるような姿勢角度を求めることで、ＥＮＵ座標系に対する真の取付姿勢を求めることができる。
【００３６】
図６は、航法計算モジュール１３０におけるレベリング処理の一例を示すフローチャートである。まず、レベリング部１３２は、ＩＭＵ１２０により検出された検出値に基づいて、ＥＮＵ座標系の姿勢誤差に起因する重力補正後の加速度の補正残差を算出する（ステップＳ１０１）。例えば、レベリング部１３２は、ＥＮＵ座標系の姿勢誤差に起因する重力補正後の加速度の補正残差を以下の式（１）から（３）により算出する。式（１）から（３）において、εａＥ、εａＮ、εａＵをそれぞれ、ＥＮＵ座標系の重力補正後の加速度のＥ軸、Ｎ軸、Ｕ軸の成分とし、εθＰ、εθＹをそれぞれ、ロケット１の機体軸座標系のピッチ角誤差、ヨー角誤差とする。なお、ロール方向をＵ軸方向とする。
【００３７】
【数１】
2018169368.tif 000003
【００３８】
図７は、ＥＮＵ座標系におけるピッチ角誤差εθＰを示す図である。また、図８は、ＥＮＵ座標系におけるヨー角誤差εθＹを示す図である。図７および図８では、ＩＭＵ１２０の実際の取付姿勢を破線（ＩＭＵ１２０Ａ）で示し、ＩＭＵ１２０の計算上の初期設定姿勢を実線（ＩＭＵ１２０Ｂ）で示している。
【００３９】
ここで、上記式（１）から（３）において、εθＰおよびεθＹが微小であれば、上記の式（１）および（２）は、以下の式（４）および（５）で近似できる。
【００４０】
【数２】
2018169368.tif 000004
【００４１】
次に、εθＰおよびεθＹがほぼ同じ値であり、かつ、微小であるとすると、上記の式（３）は、以下の式（６）で近似できる。
【００４２】
【数３】
2018169368.tif 000005
【００４３】
ここで、再度、εθＰおよびεθＹがほぼ同じ値であるとすれば、上記の式（６）は、以下の式（７）で近似できる。
【００４４】
【数４】
2018169368.tif 000006
【００４５】
さらに、αｐ、αＹを姿勢推定係数とすると、上記の式（７）は、以下の式（８）のように表される。αｐ、αＹは、ピッチ軸、ヨー軸の目標姿勢精度（目標姿勢角推定誤差）が設定される。
【００４６】
【数５】
2018169368.tif 000007
【００４７】
次に、レベリング部１３２は、算出した加速度の補正残差を用いて、速度誤差εｖＬを算出する（ステップＳ１０３）。速度誤差εｖＬは、以下の式（９）によって表される。
【００４８】
【数６】
2018169368.tif 000008
【００４９】
次に、レベリング部１３２は、算出した速度誤差に対してカルマンフィルタを適用し、姿勢誤差を推定して除去する（ステップＳ１０５）。レベリング部１３２は、上記の式（９）と、ロケット１の打ち上げ前の機体静止時のＥＮＵ座標系速度が“０”という条件を用いて、速度増分を観測値として、姿勢誤差をカルマンフィルタにより推定する。カルマンフィルタの更新周期は、例えば、飛行体用航法装置１００の更新周期１００Ｈｚとする。状態量ｘＬ、遷移行列ＡＬ、観測行列ＣＬは以下の式（１０）によって表される。
【００５０】
【数７】
2018169368.tif 000009
【００５１】
プロセスノイズＱＬについては、ＩＭＵ１２０の加速度ノイズの分散、観測ノイズＲＬはＩＭＵ１２０によって検出されたロケット１の静止時の機体搖動による加速度の変動値により決定されてよい。また、共分散値ＰＬの初期値については、想定される姿勢誤差等から決定されてよい。推定したピッチ角誤差εθＰ、ヨー角誤差εθＹで機体軸姿勢のクォータニオン（Quaternion）を更新していくことで、時間の経過とともに、姿勢が補正されていき、最終的に一定値に収束する。これにより、レベリング部１３２は、ピッチ角誤差およびヨー角誤差を推定して除去する。
【００５２】
次に、レベリング部１３２は、ピッチ角誤差およびヨー角誤差を除去したデータを真の取付姿勢のデータとして記憶部１５０に記憶させる（ステップＳ１０７）。以上により、本フローチャートの処理を終了する。
【００５３】
［方位角推定処理］
以下において、航法計算モジュール１３０の方位角推定部１３４によって行われる方位角推定処理について説明する。方位角推定は、ベロシティーマッチングを用いており、ロケット１の射点を原点としたＥＮＵ座標系において、電波航法測位モジュール１１０により導出されたロケット１の速度ベクトルと、ＩＭＵ１２０により検出された検出値に基づいて導出されたロケット１の速度ベクトルとの内積および外積により、これらの２つのベクトルの成す角を方位角誤差として算出する。これら２つのベクトルの成す角度は、ロケット１の打ち上げ前にレベリング部１３２によってピッチ角誤差およびヨー角誤差が除去されている場合、ロール角誤差のみを含む。
【００５４】
図９は、航法計算モジュール１３０における方位角推定処理の一例を示すフローチャートである。まず、方位角推定部１３４は、ロケット１の打ち上げ後の飛行中にＩＭＵ１２０により検出された検出値に基づいて、ロケット１の速度ベクトルに地表面に対する水平成分が生じているか否かを判定する（ステップＳ２０１）。方位角推定部１３４は、速度ベクトルに地表面に対する水平成分が生じていないと判定した場合、速度ベクトルの監視を継続する。
【００５５】
一方、方位角推定部１３４は、速度ベクトルに地表面に対する水平成分が生じていると判定した場合、すなわち、ロケット１の姿勢が徐々に水平方向に変えていくタイミングであると判定した場合、ＩＭＵ１２０の三軸式加速度計の出力を積分することにより得られるロケット１の機体の水平方向の速度ベクトル（以下、「第１速度ベクトル」という）と、電波航法測位モジュール１１０により導出されたロケット１の機体の水平方向の速度ベクトル（以下、「第２速度ベクトル」という）との２つの速度ベクトルを、それぞれ個別に２次のデジタルローパスフィルタによってフィルタリングしてノイズ除去を行う（ステップＳ２０３）。これにより、ＩＭＵ１２０の検出値に含まれるランダムノイズおよび衛星測位信号に含まれる熱雑音などを除去することができる。
【００５６】
図１０は、ＥＮＵ座標系におけるロール角誤差εθＲを示す図である。図１０では、ＩＭＵ１２０の実際の取付姿勢を破線（ＩＭＵ１２０Ａ）で示し、ＩＭＵ１２０の計算上の初期設定姿勢を実線（ＩＭＵ１２０Ｂ）で示している。
【００５７】
次に、方位角推定部１３４は、ノイズ除去を行った第１速度ベクトルと第２速度ベクトルとの内積を算出する（ステップＳ２０５）。方位角推定部１３４は、算出した内積に基づいて、第１速度ベクトルと第２速度ベクトルとの成す角度、すなわち、誤差角度の大きさを推定する。
【００５８】
次に、方位角推定部１３４は、ノイズ除去を行った第１速度ベクトルと第２速度ベクトルとの外積を算出する（ステップＳ２０７）。方位角推定部１３４は、算出した外積に基づいて、第１速度ベクトルと第２速度ベクトルとの誤差方向、すなわち、どの方向にロール角誤差が生じているかを推定する。
【００５９】
図１１は、ＥＮＵ座標系における方位角誤差θＤを示す図である。図１１に示すように、第１速度ベクトルと、第２速度ベクトルとの成す角度が方位角誤差θＤとなる。
【００６０】
次に、方位角推定部１３４は、推定した誤差角度の大きさおよび誤差方向に基づいて、ロール角誤差を推定する（ステップＳ２０９）。
【００６１】
次に、レベリング部１３２は、記憶部１５０に記憶された計算上の初期設定姿勢のデータ（レベリング部１３２によってピッチ角誤差およびヨー角誤差が除去された取付姿勢のデータ）からロール角誤差を除去し、除去後のデータを真の取付姿勢のデータとして記憶部１５０に記憶させる（ステップＳ２１１）。以上により、本フローチャートの処理を終了する。
【００６２】
［シミュレーション試験］
本出願の出願人は、以下に説明するシミュレーション試験を実施した。図１２は、機体のピッチ角方向およびヨー角方向にそれぞれ１０度の誤差角度を与えた際のレベリング処理のシミュレーション結果を示す。シミュレーション結果より、２０秒以内に誤差角度が０度に収束していることが分かった。
【００６３】
なお、姿勢誤差が想定値よりも大きい場合、その収束に時間を要する場合がある。この場合、αＰ、αＹを含むカルマンフィルタのパラメータを可変とし、姿勢推定時の初期には、収束に要する時間を短縮するために粗い姿勢推定を行うように、パラメータを設定する。そして、その収束状況を確認しつつ、推定精度を向上させるようにカルマンフィルタのパラメータを段階的に変動させることで、姿勢推定の精度を保ちつつ収束に要する時間を短縮させることができる。
【００６４】
以上説明した実施形態の飛行体用航法装置１００および飛行体用航法によれば、上記のようなレベリング処理を行うことで、汎用の比較的低精度な慣性計測装置を用いた場合であっても、ピッチ角誤差およびヨー角誤差を適切に除去することができる。これにより、飛行体用航法装置１００は、ロケット１の飛行安全管制で要求される高精度の位置および速度を計測することができる。さらに、ロケット１の起立時の機体搖動による加速度変動、慣性計測装置の加速度ノイズもあわせて低減することができ、位置および速度の計測値の精度をより向上させることができる。
【００６５】
また、上記のような方位角推定処理を行うことで、アースレートを検出できない汎用の比較的低精度な慣性計測装置を用いた場合であっても、ロール角誤差を適切に除去することができる。さらに、慣性計測装置により検出された検出値に基づいて導出されたロケット１の速度ベクトルおよび電波航法測位モジュール１１０により導出されたロケット１の速度ベクトルは、それぞれ個別に２次のデジタルローパスフィルタによりフィルタリングされているため、慣性計測装置の検出値に含まれるランダムノイズに起因する速度誤差および衛星測位信号に含まれる熱雑音に起因する速度誤差を防ぐことができる。
【００６６】
以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
【符号の説明】
【００６７】
ＡＴ…アンテナ、１…ロケット、１００…飛行体用航法装置、１１０…電波航法測位モジュール、１１２…測位側インターフェース、１１４…電波航法測位部、１２０…ＩＭＵ、１３０…航法計算モジュール、１３２…レベリング部、１３４…方位角推定部、１３６…慣性航法測位部、１４０…電源モジュール、１５０…記憶部、ＴＭ…通信装置
【図１】
2018169368.tif 000010
【図２】
2018169368.tif 000011
【図３】
2018169368.tif 000012
【図４】
2018169368.tif 000013
【図５】
2018169368.tif 000014
【図６】
2018169368.tif 000015
【図７】
2018169368.tif 000016
【図８】
2018169368.tif 000017
【図９】
2018169368.tif 000018
【図１０】
2018169368.tif 000019
【図１１】
2018169368.tif 000020
【図１２】
2018169368.tif 000021