JP2004512517A - スレーブ慣性測定システムの姿勢アライメント - Google Patents

Abstract

基準ナビゲーション・フレームに対して静止している車両によって支持された回転式移動プラットフォームに接続されたスレーブ慣性測定システムの姿勢アライメントに対する方法は、マスター基準慣性測定システムを回転式移動プラットフォームに取り付けかつ基準ナビゲーション・フレームに対するマスター基準慣性測定システムの加速度および角速度の測定を用いてマスター基準システム姿勢を決定する段階を含む。スレーブ・システム姿勢は、スレーブ・システム・ナビゲーション基準フレームに対するスレーブ慣性測定システムの加速度および角速度の測定を用いてかつ姿勢差を決定するためにスレーブ・システム姿勢をマスター基準システム姿勢と比較して決定される。姿勢差は、スレーブ・システム姿勢に対する修正を取得するために処理される。
【選択図】図１０

Description

【０００１】
関連出願に対する相互参照
出願人は、回転式移動プラットフォームで姿勢マッチングおよび速度観測を用いた変換アライメントに対する２０００年７月２８日に出願した、米国仮出願第６０／２２１，３１５号の利益を主張する。
【０００２】
発明における政府の権利の文書：米国政府は、米国陸軍契約第４３０００２７８４８２号の下においてこの発明における権利を有する。
【０００３】
発明の背景
本発明は、一般的にナビゲーション装置および方法に関する。本発明は、特に、車両に取り付けられたポッドから発射されるロケットのような回転式移動プラットフォームに支持された車両の姿勢誤差を修正する装置および方法に関する。
【０００４】
発明の概要
本発明は、回転式移動プラットフォームにおける姿勢整合トランスファー・アラインおよび速度観測を用いる変換アライメント・システムに関する。
【０００５】
基準ナビゲーション・フレームに対して静止している車両によって支持される回転式移動プラットフォームに接続されるスレーブ慣性測定システムの姿勢アライメントに対する本発明の方法は、マスター基準慣性測定システムを回転式移動プラットフォームに取り付ける段階および基準ナビゲーション・フレームに対するマスター基準慣性測定システムの加速度および角速度の測定を用いてマスター基準システム姿勢を決定する段階を含む。本方法は、スレーブ・システム・ナビゲーション基準フレームに対するスレーブ慣性測定システムの加速度および角速度の測定を用いることによってスレーブ・システム姿勢を決定する段階および姿勢差を決定するために前記スレーブ・システム姿勢を前記マスター基準システム姿勢と比較する段階を更に含む。スレーブ・システム姿勢に対する修正を取得するために姿勢差を処理する。
【０００６】
本発明は、初期化、変換アライン調整四元数メッセージ処理、四元数平均化およびゼロ速度更新を含んでいる、ボアサイト計算に対する一連の事象および補償機構に対する要件を含むことができる。
【０００７】
本発明は、スレーブ・システムを変換アラインする、カルマン・フィルタに対する観測を供給するために姿勢差をフィルタリングする段階を含むことができる。姿勢の差分は、大きい角度に適応させる四元数機構を介して達成されるのが好ましい。フィルタリングは、四元数、方向余弦マトリックス、または一組のオイラー角であるよりもフィルタリングに適するベクトル角への変換によって実行されるのが好ましい。
【０００８】
ボアサイト補償は、スレーブＩＭＵセンサ出力および初期近似ボアサイト角と比較して基準慣性測定ユニット（ＩＭＵ）測定を用いて実行することができる。基準ナビゲーション（ＮＡＶ）フレーム四元数に対するマスター基準本体は、記憶されたボアサイト四元数によって回転されかつナビゲーション・フレーム四元数に対するスレーブ本体と比較される。サンプルのそれぞれが比較され、かつ平均差が計算される。各サンプルに対する四元数“差”に対応している回転ベクトルは、対応マスター基準四元数およびスレーブ四元数測定間で計算される。次いで、ベクトル差の３成分が平均される。そのように取得された平均回転ベクトルは、カルマン・フィルタに処理される。アルゴリズムは、異なる四元数サンプルの正確なタイミング・アライメントおよび四元数測定を用いて微小角回転角度を計算するための機械化を供給する。
【０００９】
また、本発明は、スレーブ・システムで“ゼロ速度更新”を実行する方法を含むことができる。スレーブ・ユニットのナビゲーション軸アライメントを改善するために用いることができる擬似ナル速度が生成される。この方法は、角速度および加速度に頼らないで実施され、その代わりに、回転により誘導された位置変化を補償するために姿勢における変化を用いる。
【００１０】
速度誤差は、スレーブＩＭＵの測定された速度およびＮＡＶフレームにおけるスレーブＩＭＵの位置における既知の変化を用いて計算される。この機械化は、スレーブ・システム本体のナビゲーション・フレームにおける位置変化の比較である。位置における計算された変化は、同じ既知の時間間隔にわたり積分されたナビゲーション速度から引き算される。結果は、速度誤差に対するカルマン・フィルタ状態に対する観測である。
【００１１】
好適な実施形態の詳細な説明
本発明は、図２に示すような発射ポッド２２または２４に取り付けられた図１に示すような、ロケットのような、回転式移動プラットフォーム２０の、姿勢マッチング、変換アラインおよび速度観測を用いる変換アライメント機械化に関する。図２および８を参照すると、発射前に、ロケット２０は、トラックまたはタンクのような陸上車両２３に取り付けられるポッド２２に、通常、含まれる。図２および８は、複数のロケット２０を含んでいるポッド２２および２４のそれぞれを有する第２のポッド２４を示す。ポッド２２および２４は、発射シーケンス中以外には、移動ロック（ＴＬ）位置に通常、保持される。ロケット２０を制御することは、安定ナビゲーション基準フレームにロケットをアラインすることを含む。これは、基準本体として機能するために車両２３に固定されたマスター慣性測定ユニット（ＩＭＵ）２６およびスレーブ・システム本体として機能するためにロケット２０に固定されたスレーブ・センサ２８の軸を用いて達成される。しかしながら、更に知られていないことは、マスターＩＭＵ２６軸およびスレーブＩＭＵ２３軸の相対アライメントである。この相対アライメントは、ボアサイトとして知られている。ロケット２６が消耗品なので、スレーブ・センサ２８は、一般的に、基準センサ２６よりも正確ではなく、時々アライメントにおける誤差を結果として生じる。本発明は、ロケット２０が発射される前にボアサイト誤差を修正しかつ基準ナビゲーション・フレームにスレーブ・センサを位置合せする。
【００１２】
ボアサイト誤差を補償するために必要な計算のあるものは、より一般的なベクトルの代わりに四元数を用いてより都合よく実行される。四元数は、互いに対して回転する座標系間の変換を含む計算において都合よく用いられる。四元数の基本特性の簡単な説明が本発明を理解することを容易にするために示される。四元数は、複素数系の一般化の結果である。複素数系では、３つの−１の逆交換超複素根が存在し、それは四元数表記で文字

によって表され、数字

は以下の特性を有する：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３）
【００１３】
四元数は、数字ｗと、係数ｘ、ｙ、ｚと

の乗算との線形組合せとして書くことができ、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４）
と書くことができる。
四元数

の共役は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５）
と書かれる。
四元数

は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（６）
と書かれる大きさを有する。
単位四元数は、１の大きさを有し、従って単位四元数に対して

であり、ここで

は乗算の逆であり、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（７）
によって計算することができる。
四元数は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（８）
のように相関である。
しかしながら、四元数は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（９）
のように可換ではない。
【００１４】
数式（４）に加えて四元数

は、行ベクトル、列ベクトルまたはベクトルとスカラーの組合せで書くことができる：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１０）
ここで、ｓ＝ｗかつ

である。二つの四元数

と

の積は、以下の方法で

の掛算特性と標準ベクトル積を用いて書くことができる：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１１）
角度θによる単位ベクトル

の回りに関する回転、または直交変換を表す四元数は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１２）
として書くことができる。
空間における点

は、四元数

によって表すことができる。単位ベクトル

の回りの点

の回転は、以下のように計算される：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１３）
本発明に関連付けられたある計算は、二つの回転を含む。

と

が二つの回転を表している四元数であるならば、第１の回転に続いて第２の回転を有する合回転は、四元数

によって表される。数式（１３）を用いて点Ｐへの複合回転の適用の結果は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１４）
と書かれる。
【００１５】
二つの四元数の掛算

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１５）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１６）
は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１７）
と書くことができる。
また、回転は、マトリックスを用いて計算することもできる。しかしながら、マトリックス掛算は、四元数掛算よりも多くの数学的な計算を必要とする。従って、四元数を用いることは、かかる計算における数値的精度を保存すると同時に時間を節約する。
【００１６】
本発明は、初期化、変換位置合せ四元数メッセージ処理、四元数平均化、およびゼロ速度更新を含む、ボアサイト計算および補償機械化に対する一連の事象を含む。
【００１７】
図３は、慣性測定ユニット（ＩＭＵ）３０を示し、それは、ロケット２０に対する角方位および加速度を供給するための３軸回転センサ（図示省略）および加速度計（図示省略）を含む。ＩＭＵ３０は、周知の回転センサおよび加速度計技術に従って形成することができる。図４は、ロケット２０の角方位、加速度、速度および位置に関するデータを供給する慣性ナビゲーション・ユニット（ＩＮＵ）３３を形成するために全地球測位システム（ＧＰＳ）モジュール３２に結合されたＩＭＵ３０を示す。本発明を実施するために適切なＧＰＳモジュールは、この技術分野で周知である。ＩＭＵ３０は、ＧＰＳモジュール３２をＩＭＵ３０に結合するために用いることができる細長いコネクタ３４を含む。
【００１８】
図５に示すように、システム・ナビゲーション（ＮＡＶ）・フレームは、ワンダー角度方位角α＝０°（＋Ｘ_Ｎ北向き）および＋Ｚ_Ｎ下向きを有するローカル・レベルのＸ_ＮおよびＹ_Ｎとして画定される。ボアサイト修正に関連付けられた５つの基準フレームが存在する：（１）マスターＩＭＵ基準フレーム、（２）基準ナビゲーション（ＮＡＶ）・フレーム、（３）スレーブ・システム本体フレーム、（４）スレーブ・システムＮＡＶフレームおよび（５）スレーブ・システム直交フレーム。マスターＩＭＵ基準本体軸（Ｘ_ＲＢ、Ｙ_ＲＢ、Ｚ_ＲＢ）は、＋Ｘ_ＲＢが前向きに、＋Ｚ_ＲＢが下向きに、および＋Ｙ_ＲＢが右手基準フレームを形成しているＸ_ＲＢおよびＺ_ＲＢに垂直に図２に示される。スレーブ・システム本体フレーム軸（Ｘ_Ｂ、Ｙ_Ｂ、Ｚ_Ｂ）は、図１に示され、かつ＋Ｘ_Ｂがロケット２０のノーズから外に、＋Ｙ_Ｂがスターボード側面から外に、かつ＋Ｚ_Ｂが胴体から外にである。スレーブ・システム基準フレーム軸（Ｘ、Ｙ、Ｚ）は、＋Ｘがコネクタ３４に沿って、＋Ｙがコネクタ３４に垂直にかつ＋Ｚが上向きに図３に示される。
【００１９】
図４は、スレーブ・システム直交フレームを示す。このフレームは、所定の関係を用いてスレーブ・システム本体フレームに数学的に関係付けられる。図５は、マスター基準本体およびマスター基準ＮＡＶフレームに対するスレーブ・システム本体、スレーブ・システム直交、およびスレーブ・システムＮＡＶフレームの関係を示す。また、この図は、基準本体ボアサイトに対するスレーブ・システム本体も示す。図５において、以下の定義が適用される：

は、マスター基準ＮＡＶフレームからマスター基準本体フレームまでの回転を示す方向余弦マトリックスでありかつ四元数

として利用可能である。
【００２０】

は、スレーブ・システムＮＡＶフレームからスレーブ・システム直交フレームまでの回転を示す方向余弦マトリックスでありかつ四元数

として利用可能である。
【００２１】

は、スレーブ・システム直交フレームからスレーブ・システム本体フレームまでの回転を示す方向余弦マトリックス（所定の関係）でありかつ四元数

として利用可能である。
【００２２】

は、マスター基準本体フレームからスレーブ・システム本体フレームまでの回転を示す方向余弦マトリックス（ボアサイト）−（未知）である。（四元数表現ｑ_Ｌ→Ｂまたは

によっても示される）。
【００２３】

は、基準ＮＡＶフレームからスレーブ・システムＮＡＶフレームまでの回転を示す方向余弦マトリックスである。
【００２４】
基準ＮＡＶフレーム傾斜がゼロでかつα＝α_Ｒ＝０であるならば、

は、システムＮＡＶ傾斜φ_ｘおよびφ_ｙおよび方位角回転φ_ｚによる回転に対応する。
【００２５】
発射前シーケンスに５つの主要事象が存在し、それらはロケット２０が発射される前に発生する。これらは、以下に説明されかつ、一定の縮尺に従わずに描かれている、図９の事象時間線に示される。最初の事象は、パワー・オン（ＰＯ）であり、それはシーケンスの実行を初期化する。
【００２６】
第２の事象は、図１０のフロー図に示す初期化データ命令４０を含む発射装置初期化メッセージである。初期化データ命令４０は、マスター基準ＮＡＶフレーム四元数４２に対するマスター基準本体フレームを初期化しかつスレーブ・システム本体フレーム四元数４４に対するマスター基準本体を初期化し、それらはロケット・ポッド２０における選択されたボアの緯度、経度、高度、グランド位置合せ命令、方位およびボアサイトの以前の知識を表す。また、初期化データ命令４０は、ＮＡＶフレーム四元数４６に対するスレーブ・システム直交フレームを初期化しかつスレーブ・システム本体フレーム四元数４８に対するスレーブ直交フレームを初期化する四元数

移動ロック（ＴＬ）解放は、パワー・オンの後でかつ最初の位置への移動の前に発生する。初期化メッセージは、この期間中いつでも発生することができる。初期化処理に対するタイミング・シーケンスを図６に示す。
【００２７】
第３の事象は、最初のアライメント位置を設定することであり、それは移動ロックが解放されかつポッド２２が安定化された後の選択されたロケット・ポッド２２の位置である。
【００２８】
第４の事象は、マスター基準システムからの位置１の情報の受取りの表示の後でその第２のアライメント位置にロケット・ポッド２２が移動するときである。
【００２９】
第５の事象は、マスター基準システムからの位置２の情報の受取りの表示の後で位置３にロケット・ポッド２２が移動するときに発生する。位置３は、一般に、ローカル・レベルの上で２０°から６０°の高度および初期位置から９０°から１８０°の方位角であるような発射角である。マスター・システムからのこの最後のマッチング・メッセージの受取り後に、ロケット２０は、発射の準備が整う。
【００３０】
最初の３つの初期化位置のそれぞれにおいて、ロケット２０は、１０Ｈｚデータの２秒を受け取るのが好ましく、それは２０四元数サンプルに対応する。更新は、全てのロケットが発射されるまでまたはロケット・ポッド２２が再び積み込まれかつパワーがオフにされるまで発射位置で続けられる。マスターＩＮＳは、定期的更新に対して３０秒毎に１０Ｈｚデータの１秒の四元数メッセージを供給するのが好ましい。パワーがサイクルされるならば、ロケットは、再初期化される。
【００３１】
スレーブ・システム本体初期化４８に直交するスレーブ・システムは、２００Ｈｚで初期化メッセージ・フラグに対してポーリングするアルゴリズムによって制御される。四元数初期化は、マスター基準（発射装置）フレームをマスター基準測地フレーム四元数（ｑ_{Ｌ→Ｇｅｏ}）４２におよびマスター基準フレームを初期化データ命令４０から受け取ったスレーブ・システム・フレーム四元数（ｑ_Ｌ→Ｂ）４４データに利用する。マスター基準本体初期化四元数（ｑ_{Ｌ→Ｇｅｏ}）４２、スレーブ・システム四元数に対する初期マスター基準本体（ｑ_Ｂ→Ｌ）４４およびスレーブ・システム本体四元数に直交するスレーブ・システム（ｑ_０→Ｂ）４８は、以下に示すようにスレーブ・システムＮＡＶフレーム四元数（ｑ_０→Ｎ）５２にスレーブ・システム直交フレームを初期化する。初期化および変換位置合せ、α_ｒｅｆ＝０、に対して、それゆえに、最初は、スレーブ・システムＮＡＶフレーム四元数（ｑ_０→Ｎ）に対するスレーブ・システム直交フレームは、測地フレーム四元数に対するスレーブ・システム直交フレームに等しい。
【００３２】
以下の数式が適用される：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１８）
ここで、
ｑ_{０→Ｇｅｏ}＝測地フレーム四元数に対するスレーブ・システム直交フレーム（α＝０であるＮＡＶフレーム）；
ｑ_{Ｌ→Ｇｅｏ}＝測地フレーム四元数に対するマスター基準本体フレーム；
ｑ_Ｌ→Ｂ＝スレーブ・システム本体フレーム四元数に対するマスター基準本体フレーム（ボアサイト）；
ｑ_０→Ｂ＝スレーブ・システム本体フレーム四元数に対するスレーブ・システム直交フレーム；および
ｑ^＊は、逆四元数であり、それは回転四元数のベクトル部をネゲートすることによって達成される。
【００３３】
スレーブ・システム・フレームで表された不特定のベクトルＶは、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（１９）
によってシステムＮＡＶフレームに変換される。
これが初期化処理を終了させる。
【００３４】
四元数メッセージ処理は、四元数を定期的に更新することを含む。四元数更新は、四元数の測地フレームの組に対するマスター基準本体フレームの受取りにより実行される。変換位置合せモードにより１０または２０組の四元数が存在する。２０サンプル・モードに対して、処理アルゴリズムは、１０サンプルで実行されるが、しかし連続して二度、バッファ・ポインタおよびワード・アドレスをシフトする。
【００３５】
バッファリング処理は、内部的にスレーブ・システム四元数をバッファする。このバッファリングは、２００Ｈｚの周波数であるのが好ましい。これらは、ＩＭＵによって計算されたようなスレーブ・システムＮＡＶフレーム四元数（ｑ_Ｂａ）に対するスレーブ・システム本体フレームである。このデータは、以下のように計算されたインデックスを用いてバッファされる：
【００３６】
２００Ｈｚにおいて
Ｉｆ（＋＋Ｌ_{２００ｈｚｃｎｔ}＝ ＝ｉｎｔｅｒｖａｌ）
｛Ｌ_{２００ｈｚｃｎｔ}＝０
ｑ_{ｉｎｄｅｘ}＋＋
ｑ_{ｉｎｄｅｘ}＆＝（ｂｕｆｆｌｅｎｇｔｈ−１）
ｑ_ｂｕｆｆ［ｑ_{ｉｎｄｅｘ}］＝ｑ_Ｂａ｝
（更新メッセージが受け取られた）ならば
｛ｑ_{ｉｎｄｅｘｌａｔｃｈ}＝ｑ_{ｉｎｄｅｘ}
Ｌａｔｃｈ_{２００Ｈｚｃｎｔ}＝Ｌ_{２００Ｈｚｃｎｔ}
ここで：
ｂｕｆｆｌｅｎｇｔｈ＝５１２（２の累乗でなければならない、バッファのサイズ）
ｉｎｔｅｒｖａｌ＝１
ｑ_Ｂａ＝スレーブ・システムＮＡＶシステム出力四元数に対するスレーブ・システム本体；および
Ｌ_{２００Ｈｚｃｎｔ}＝２００Ｈｚカウンタ
【００３７】
ＩＭＵソフトウェアは、バッファされたデータから中央スレーブ本体ＮＡＶ四元数を抽出するために上記で計算したインデックスを用いる。カルマン処理の測定マトリックス

に要求される方向余弦

は、方向余弦マトリックスへの中央四元数の変換によりこのサンプルを用いて決定される。
【００３８】
このセクションは、四元数処理および回転ベクトル平均化に対してその後参照される分析を供給する。マスター基準システム四元数ｑ_{Ｌ→Ｇｅｏ}は、基準四元数５６を供給するために記憶されたボアサイト四元数ｑ_ＢＯＲによって回転される。この回転は、四元数ｑ_ＢＯＲで四元数ｑ_{Ｌ→Ｇｅｏ}を乗算することによって達成される。回転の結果は、スレーブ・システム本体四元数と比較される。サンプル（２０または１０）の全ては、比較されかつ次いで平均差が計算される。四元数自体が直接的に平均化されるならば、４つの平均成分は、有効正規化四元数をもはや表さない。その代わりに、各サンプルに対する四元数“差”に対応している回転ベクトルが対応するマスター基準四元数およびスレーブ・システム四元数測定間で計算される。次いで、ベクトル差の３成分は、計算成分平均処理６０で平均化される。そのように取得された平均回転ベクトルは、カルマン・フィルタ６２に処理される。
【００３９】
基準四元数ｑ_Ｂは、マスター基準本体フレーム・ボアサイト四元数に対するスレーブ・システム本体フレーム四元数で乗算されたマスター基準ＮＡＶフレーム四元数に対するマスター基準本体フレームとして画定され、それは

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２０）
として書くことができる。
コンパクト化のために測地フレーム四元数に対するマスター基準本体フレームｑ_{Ｌ→Ｇｅｏ}は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２１）
として書かれる。
スレーブ・システム本体フレーム四元数に対するマスター基準本体フレームｑ_Ｌ→Ｂ（ボアサイト）は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２２）
として書かれる。
【００４０】
ｑ_Ｂａ＝スレーブ・システムＮＡＶシステム出力四元数に対するスレーブ・システム本体（ｑ_Ｂの近似）ということを思い出すことにより、回転ベクトルに対する微小角近似は：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２３）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２４）
である。
式（２０）を用いて、式（２４）の四元数掛算

は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２５）
と書くことができる。
【００４１】
以下の段階は、式（２３）、（２４）および（２５）を実施するために用いることができる。その他の順番の処理が可能である。
【００４２】
段階１：四元数掛算

を決定する（スカラーおよびベクトルの両方が必要）。四元数

および

は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２６）
および

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２７）
と書くことができる。
【００４３】
式（２６）および（２７）では、添字１、２、３は、

に対応しかつ添字４は、四元数のスカラー成分を示す。マスター基準四元数ｑ_Ｌは、マスター基準ＮＡＶフレーム（測地フレーム）四元数に対するマスター基準フレームでありかつ（１０または２０のいずれかの）各更新中に送られる。ここで四元数ｑ_Ｆを以下のように画定する：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２８）
ｑ_Ｆ４、ｑ_Ｆ１、ｑ_Ｆ２、およびｑ_Ｆ３成分は、スカラー項ｑ_Ｆ４で始まっている四元数乗算を用いてここで以下のように計算される；

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（２９）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３０）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３１）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３２）
この掛算は、全ての１０のサンプルに対して同じ特性ボアサイトｑ_ＢＯＲを用いて（１から１０の）ｑ_Ｆの１０のサンプルに対して行われる。結果は：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３３）
【００４４】
段階２：式（２４）を用いて：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３４）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３５）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３６）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３７）
この乗算は、ｑ_Ｂａおよびｑ_Ｆの時間アライン・サンプルに対して行われる。
【００４５】
ここでφ_εｘ、φ_εｙおよびφ_εｚの平均値が計算される。これらの平均は、
【００４６】

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３８）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３９）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４０）
と書くことができる。
【００４７】
ボアサイト誤差の３つのベクトル成分に対する平均回転角は、カルマン・フィルタに観測として適用される。

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４１）
【００４８】
Ｘ、ＹおよびＺ観測に対する測定ノイズは、問題のアプリケーションに対して適切に予め選択される。
【００４９】
傾斜およびボアサイトに対するボアサイト観測および誤差状態は、

と書かれる。
【００５０】
３つのボアサイト状態に対する初期共分散および処理ノイズは、マスターおよびスレーブ・システム間の機械的アライメントにおける不確実性によって決定される。３つのボアサイト状態に対する遷移マトリックスは：

である。
【００５１】
現在のボアサイト回転に適用されかつボアサイト更新四元数を形成するために用いられる３つの微小角回転は：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４２）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４３）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４４）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４５）
である。
【００５２】
ボアサイト更新四元数は、現行の四元数近似を改善するために用いられる。これは、新しい近似を形成し、それは：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４６）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４７）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４８）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（４９）
である。
【００５３】
定期的四元数更新は、ポッド２２のロケット２０が発射されるまで続く。ロケット２０が発射され、かつポッド２２の結果として得られた動きが減衰された後、ポッド２２にそのまま残っているロケットは、“ロケットが発射された”フラグを含むマスター基準四元数メッセージを受け取る。このフラグが検出されるときには、３つのボアサイト状態共分散の対角線成分は、可能なボアサイト・シフトを示すために増大させられる（Ｑポンプされる）。ボアサイト状態共分散の３つの対角線成分は、新しい共分散Ｐ_ｎｅｗ＝Ｐ＋ΔＰであるように、ΔＰだけ増分される。対角線外の成分は、変更されない。共分散増大は、四元数メッセージが処理される前に行わなければならない。ΔＰは、発射動力学により予想されたボアサイト・シストによって決定される。新しいマスター基準四元数は、各発射後の最初の更新期間で利用可能になる。四元数観測は、新しいボアサイト誤差を迅速に捕らえる。これが変換位置合せ処理を終了させる。
【００５４】
性能を更に向上させるために、本発明の第２の形態は、擬似ゼロ速度更新を実行する。本発明は、標準ゼロ速度更新スキームに新しい段階を追加する。ゼロ速度更新機械化の第１の計算は、標準ゼロ速度更新処理に対してゼロであると仮定する、Ｖ_ｉ（ＮＡＶフレームの速度ベクトル）を計算する。ジンバル形発射装置のような回転式移動プラットフォームでの使用に対して、スレーブ・システムは、車両自体が静止していてもＮＡＶフレームにおいてノン−ゼロ速度を経験することができるということが認識される。この非ゼロは、発射装置ポッド２２および２４の回転運動の結果として生じる。スレーブ速度に対する修正は、擬似ゼロ速度を形成するために計算される。この速度誤差は、スレーブ・システムＩＭＵの測定された速度およびＩＭＵ位置および旋回点レバー・アームを用いているＮＡＶフレームにおけるスレーブ・システム本体ＩＭＵの位置における既知の変化を用いることによって計算される。
【００５５】
図７は、ＩＭＵ位置（点Ｂ）、方位角旋回（点０）、高度旋回（点Ｐ）、および本発明を理解することに有用である概念的レバー・アームＬ_１、Ｌ_２、Ｌ_３、およびＬ_４を示す。
【００５６】
レバー・アームＬ_１およびＬ_３は、直角に交叉する端部３５および３６をそれぞれ有する。レバー・アームＬ_３の他の端部３７は、ＥＬ＝０に示す高度旋回軸Ｐの回りに回転自在であるように構成される。レバー・アームＬ_１は、直角でレバー・アームＬ_２を二等分する端部３８を有する。スレーブ・システムＩＭＵ位置、点Ｂは、レバー・アームＬ_２の端部３９である。ＩＭＵに対する座標は、右を指しているＸ_Ｇ軸、下向きを指しているＺ_Ｇ軸、およびＸ_Ｇ軸とＹ_Ｇ軸に垂直であるＹ_Ｇ軸を有して点Ｂで示されかつ右手座標系を形成するように構成されている。レバー・アームＬ_４は、高度旋回軸から点Ｏまで拡張し、それは方位角旋回軸上にある。座標系Ｘ_Ｔ、Ｙ_ＴおよびＺ_Ｔは、点Ｏに中心が置かれている。Ｚ_Ｔ軸は、下向きに拡張しかつ方位角旋回軸を画定する。Ｘ_Ｔ軸は、右を指し、かつＹ_Ｔ軸は、前方向を指している。
【００５７】
この機械化は、カルマン・フィルタに同期された１−秒時間間隔に対するスレーブ・システムＩＭＵのナビゲーションの中央のナビゲーション・フレームにおける位置変化の比較である。これは、測地観測に用いられる。これは、発射装置方位角および高度ジンバルの動きによる。
【００５８】
以下の仮定がなされる：
高度構造の点Ｐに配置された、高度旋回軸穴は、Ｙ_Ｇ軸に並行に存在する。
（小塔構造の点Ｐに配置された）高度ピンは、負のＸ_Ｔ軸に配置される。
高度ピンは、Ｙ_Ｔ方向を指している、Ｘ_ＴＹ_Ｔ平面に存在する。
【００５９】
方向余弦マトリックスに対して以下の表記が用いられる：

は、１−秒間隔の開始における、システム本体フレームに対するシステム方向余弦マトリックス、ＮＡＶフレームである。
【００６０】

は、１−秒間隔の終りにおける、スレーブ・システム本体フレームに対するシステム方向余弦マトリックス、ＮＡＶフレームである。
【００６１】

は、マスター基準本体フレームからスレーブ・システム本体フレームまでの回転の方向余弦マトリックスである。このマトリックスは、総合ボアサイトによるフレーム回転を表す。
【００６２】
位置における変化は、点ＯとＢの間の概念的レベル・アーム（Ｌ_ＯＢ）の変化を解くことによって計算される。計算に対する式は：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５０）
【００６３】

の構成要素は、レバー・アームＬ_４（ＶＥＣ）への速度効果を解くために用いられる。

ならば、ＶＥＣは、以下のように計算される：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５１）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５２）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５３）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５４）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５５）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５６）

ならば、ＶＥＣは、以下のように計算される：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５７）

ならば、ＶＥＣは、以下のように計算される：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５８）
それ以外は、

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（５９）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（６０）
【００６４】
ＶＥＣが決定された後、Ｌ_ＯＢベクトルは、以下のように解かれる：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（６１）
【００６５】
ＬＯＢは、１−秒間隔にわたる位置における変化である。これは、各軸に対して測定された速度から差分される。この差分は、以下のカルマン・フィルタ状態に対する観測である：

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（６２）
【００６６】
ここに開示した構造および方法は、本発明の原理を説明する。本発明は、その精神または基本的特性から逸脱することなくその他の特定な形式で実施することができる。説明された実施形態は、限定的ではなく例示的および説明的として全ての形態において考慮されるべきである。従って、上記説明ではなく添付した特許請求の範囲が本発明の適用範囲を画定する。特許請求の範囲の相当物の意味および範囲内にあるここに記述された実施形態に対する全ての変更は、本発明の適用範囲内に含まれる。
【図面の簡単な説明】
【Ｆｉｇ１】マスターＩＭＵ基準本体の直交座標を示す。
【Ｆｉｇ２】スレーブ・システム本体の直交座標を示す。
【Ｆｉｇ３】スレーブＩＭＵ直交フレームの直交座標を示す。
【Ｆｉｇ４】慣性ナビゲーション・ユニット（ＩＮＵ）基準フレームの直交座標を示す。
【Ｆｉｇ５】マスター基準本体およびマスター基準ナビゲーション・フレームに対するスレーブ・システム本体フレーム、スレーブ・システム直交フレームおよびスレーブ・システム・ナビゲーション基準フレームの関係、およびマスター基準本体ボアサイトに対するスレーブ・システム本体を示す。
【Ｆｉｇ６】本発明を実施するために用いられる軸変換を初期化するためのタイミング・シーケンスを示す。
【Ｆｉｇ７】本発明を概念的に図示するレバー・アーム構造を示す。
【Ｆｉｇ８】本発明に対する一対のロケット・ポッドにおけるロケットの番号付けを示す。
【Ｆｉｇ９】本発明に対する事象時間線である。
【Ｆｉｇ１０】本発明に対するソフトウェア図である。

Claims (20)

1. 基準ナビゲーション・フレームに対して静止している車両によって支持される回転式移動プラットフォームに接続されているスレーブ慣性測定システムの姿勢アライメントに対する方法であって、
   マスター基準慣性測定システムを前記回転式移動プラットフォームに取り付ける段階；
   前記基準ナビゲーション・フレームに対する前記マスター基準慣性測定システムの加速度および角速度の測定を用いてマスター基準システム姿勢を決定する段階；
   スレーブ・システム・ナビゲーション基準フレームに対する前記スレーブ慣性測定システムの加速度および角速度の測定を用いることによってスレーブ・システム姿勢を決定する段階；
   姿勢差を決定するために前記スレーブ・システム姿勢を前記マスター基準システム姿勢と比較する段階；および
   前記スレーブ・システム姿勢に対する修正を取得するために前記姿勢差を処理する段階を含む方法。
2. 前記マスター基準システム姿勢の初期値を取得する段階；
   前記スレーブ・システム姿勢の初期値を取得する段階；
   前記基準システム姿勢をサンプル化する段階；
   前記スレーブ・システム姿勢をサンプル化する段階；
   姿勢修正を取得するために前記サンプル化されたマスター基準システム姿勢および前記サンプル化されたスレーブ・システム姿勢を処理する段階；および
   前記スレーブ慣性測定システムを位置合せするために前記スレーブ・システム姿勢に前記姿勢修正を適用する段階を含む初期化段階を更に含む、請求項１に記載の方法。
3. 前記マスター基準システム姿勢を決定する段階は、
   前記マスター基準慣性測定システムに対するマスター基準システム本体フレームを画定する段階；
   前記スレーブ慣性測定システムに対するスレーブ・システム本体フレームを画定する段階；および
   前記マスター基準システム本体フレームと前記スレーブ・システム本体フレームとの間の角変位を決定する段階を更に含む、請求項１に記載の方法。
4. 前記スレーブ慣性測定システムと前記マスター基準慣性測定システムとの間の角変位の値を決定する段階を更に含む請求項１に記載の方法。
5. 前記マスター基準システム姿勢を決定する段階は、
   前記マスター基準慣性測定システムと前記基準ナビゲーション・フレームとの間の角変位を示す測地フレーム四元数に対してマスター基準本体を形成する段階を含む、請求項１に記載の方法。
6. 前記基準ナビゲーション・フレームは、測地座標フレームである請求項５に記載の方法。
7. 前記スレーブ・システム姿勢を決定する段階は、
   前記スレーブ・システム本体と前記スレーブ・システム・ナビゲーション基準フレームとの間の角変位を示すスレーブ・システム本体四元数を形成する段階を含む、請求項５に記載の方法。
8. 前記スレーブ・システム姿勢を前記基準システム姿勢と比較する段階は、
   前記マスター基準本体フレームと前記スレーブ・システム本体フレームとの間の角変位の初期値を示すボアサイト四元数を形成する段階；
   前記マスター基準本体フレームと前記基準ナビゲーション・フレームとの間の角変位の初期値を示す基準ナビゲーション・フレーム四元数を形成する段階；
   前記ボアサイト四元数の角変位と前記基準ナビゲーション・フレーム四元数に対する前記マスター基準本体の角変位を組合せる基準四元数を形成する段階；および
   前記スレーブ・システム本体四元数および前記基準四元数からの回転を示す回転ベクトルを計算する段階を含む、請求項７に記載の方法。
9. 前記スレーブ・システム本体四元数および前記基準四元数のサンプルの選択された数の組を収集する段階；
   サンプルの各組に対する回転ベクトルを計算する段階；および
   前記選択された数のサンプルに対する前記回転ベクトルに対する平均成分値を計算する段階を更に含む、請求項７に記載の方法。
10. ボアサイト更新四元数を計算するために前記平均成分値をカルマン・フィルタでフィルタリングする段階；および
    更新されたボアサイト四元数を計算するために前記ボアサイト更新四元数を用いる段階を更に含む、請求項８に記載の方法。
11. 前記スレーブ慣性測定システムに対する速度ベクトルを決定する段階；
    前記スレーブ・システム・ナビゲーション基準フレームにおける前記スレーブ慣性測定システムの位置変化を決定する段階；および
    前記スレーブ・システム速度ベクトルおよび前記スレーブ・システム・ナビゲーション基準フレームにおける前記スレーブ慣性測定システムの位置変化を用いてベクトル誤差を計算する段階を更に含む、請求項１に記載の方法。
12. 基準ナビゲーション・フレームに対して静止している車両に取り付けられた回転式移動プラットフォームによって支持されたパッドに取り付けられた複数のユニットの選択されたものに接続されるスレーブ慣性測定システムの姿勢アライメントに対する方法であって、
    マスター基準慣性測定システムを前記回転式移動プラットフォームに取り付ける段階；
    前記基準ナビゲーション・フレームに対する加速度および角速度のその測定を用いてマスター基準システム姿勢を決定する段階；
    前記ナビゲーション基準フレームに対する加速度および角速度のその測定を用いることによってスレーブ・システム姿勢を決定する段階；
    前記スレーブ慣性測定システムと前記マスター基準慣性測定システムとの間の角変位の初期値を決定する段階；
    姿勢差を決定するために前記スレーブ・システム姿勢を前記基準システム姿勢と比較する段階；
    前記スレーブ・システム姿勢、および前記スレーブ慣性測定システムと前記マスター基準慣性測定システムとの間の角変位に対する修正を取得するために前記姿勢差を処理する段階；および
    前記選択されたユニットが発射された後にパッドに残っているユニットに信号を供給する段階を含む方法。
13. 静止発射車両に取り付けられた回転式移動プラットフォームから発射されるユニットの姿勢誤差を修正する方法であり、前記ユニットがスレーブ・システム本体フレームとして機能するためにそれに固定されたスレーブ・センサを含みかつ前記回転式移動プラットフォームがマスター基準本体フレームとして機能するためにそれに固定されたマスター慣性測定ユニットを含む方法であって、
    前記マスター基準本体と前記スレーブ・システム本体との間の角変位の初期値を有するボアサイト四元数ｑ_ＢＯＲを初期化する段階；
    前記ユニットが発射される前記発射車両における選択されたボアの緯度、経度、高度および角方位を初期化する段階；
    前記マスター基準本体フレームから測地フレームまでの回転を示す四元数
    

    

    を初期化する段階；
    前記選択されたボアとスレーブ・システム・ナビゲーション・フレームとの間の回転を示す基準四元数
    

    

    を供給するために前記ボアサイトを前記四元数
    

    

    と組合せる段階；
    前記スレーブ・システム本体フレームと前記スレーブ・システム・ナビゲーション・フレームとの間の回転を示すスレーブ・システム本体四元数を供給する段階；
    前記ボアサイト四元数を調整するために更新四元数を計算する段階；および
    前記スレーブ・システム本体四元数と前記基準四元数とを組合せる段階を含む方法。
14. 基準四元数を供給するために前記ボアサイト四元数を前記四元数
    

    

    と組合せる段階は、前記ボアサイト四元数ｑ_ＢＯＲで前記四元数
    

    

    を掛け算する段階を含む、請求項１３に記載の方法。
15. 前記更新四元数を計算する段階は、
    前記マスター基準本体フレームと前記スレーブ・システム本体フレームとの間の角変位の誤差に対する概略値を取得するために前記スレーブ・システム本体四元数で前記基準四元数を掛け算する段階；
    選択された数のサンプルに対して前記角変位の前記誤差の平均値を計算する段階；
    前記ボアサイト更新四元数を生成するためにカルマン・フィルタで前記誤差の前記平均値を処理する段階；および
    更新されたボアサイト四元数を供給するために前記ボアサイト更新四元数と前記ボアサイト四元数を組合せる段階を含む、請求項１４に記載の方法。
16. 前記ボアサイト四元数ｑ_ＢＯＲに対する前記更新を計算する段階は、四元数ｑ_Ｂａおよび
    

    

    の各成分の選択された数のサンプルに対して四元数乗算
    

    

    に対する値を取得するために微小角近似法を用いて回転ベクトルを計算する段階を含む、請求項１５に記載の方法。
17. 四元数乗算
    

    

    の各成分に対する平均値を計算する段階；
    ボアサイト更新四元数の成分を計算するように構成されるカルマン・フィルタに四元数乗算
    

    

    の各成分に対する平均値を適用する段階；および
    更新されたスレーブ・システム本体フレーム・ボアサイト四元数ｑ_ＢＯＲを取得するために前記ボアサイト更新四元数で前記スレーブ・システム本体フレーム・ボアサイト四元数ｑ_ＢＯＲを掛け算する段階を更に含む、請求項１６に記載の方法。
18. 前記ユニットが発射される前に前記マスター基準本体フレームの前記スレーブ・センサの速度を測定する段階；
    選択された時間間隔で前記スレーブ・システム・ナビゲーション・フレームの前記スレーブ・システム本体の位置における変化を測定する段階；および
    前記スレーブ・センサの前記測定した速度および前記スレーブ・システム本体の位置における前記変化を用いて速度誤差を計算する段階を更に含む、請求項１３に記載の方法。
19. 前記計算された速度誤差は、前記スレーブ・システム慣性測定ユニットの改善された精度を供給するためにナビゲーション基準フレームに対する前記スレーブ・システム本体フレームの姿勢を更新するために用いられる、請求項１８に記載の方法。
20. 前記計算された速度誤差は、前記スレーブ慣性測定ユニットの誤差パラメータを推定するために、かつその精度を改善すべく修正を適用するためにカルマン・フィルタで用いられる、請求項１８に記載の方法。

Images