JP5569681B2 - 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 - Google Patents

Description

本発明は、移動体にジャイロ，加速度計，磁気センサおよび速度計を搭載し、これらセンサより移動体の姿勢の誤差を推定して除去する、精度の高い移動体の姿勢推定装置およびその姿勢推定方法に関する。

移動体、例えば航空機の姿勢を正確に推定し、推定した結果を計器表示したり、操縦に利用することにより安全な操縦の手助けとなる。
航空機などの航行時の位置，速度，姿勢および方位を計測するものとして慣性航行装置（ＩＭＵ）があり、これは非常に高い精度で、基準となる絶対姿勢角からの相対的な姿勢角変化を計算することができる。
移動体の姿勢を推定する方法としてジャイロなどのセンサを用い、誤差修正を推定するカルマンフィルタを取り入れたものが種々提案されている。

特許文献１の姿勢計測方法，姿勢制御装置，方位計及びコンピュータプログラムは、従来よりも短い時間で物体の姿勢または真北を含む方位を計測することができることを目的としたものである。具体的な構成におけるカルマンフィルタの時間更新は姿勢計測装置の姿勢をクオータニオンを用いて表示した状態変数の推定値ハットおよび誤差共分散は時間に対して不変であるとして前ステップの観測更新で得られた値と等しくする。また、観測更新ではジャイロおよび加速度センサを用いて互いに直交する各３軸回りの角速度及び各３軸方向の加速度を測定し、測定した角速度及び加速度の値を観測値として状態変数の推定値ハット及び誤差共分散を計算する。誤差共分散が十分小さくなるまでカルマンフィルタの処理を繰り返し、姿勢を示す状態変数を得るものである。

特許文献２の慣性航法装置，慣性航法装置の初期化方法及び記録媒体は、ある程度の動作環境においてもイニシャルアライメントの高精度・高速化を図り、航空体の運用上の制約を小さくすることを目的とするものである。具体的な構成はイニシャルアライメント処理のファインアライメントにおいてカルマンフィルタの処理を行う処理手段と、この処理手段による処理の際、カルマンフィルタの収束状況を監視し、カルマンフィルタの収束速度が過度であるか否かを判断する監視手段と、この監視手段で収束速度が過度であると判断された場合、カルマンフィルタで推定された方位誤差を基にカルマンフィルタの更新処理をする更新手段とを具備し、処理手段は更新手段による更新後、再度カルマンフィルタの処理を行うものである。

特許文献３の方位姿勢基準装置は、ストラップダウン型の方位姿勢基準装置においてアライメントを迅速化し、インフライトでも実施可能にすることを目的とするものである。座標変換マトリックス（ＣＴＭ）の誤差修正において、姿勢角誤差修正と方位角誤差修正を分離し、姿勢角誤差修正を行ってから方位角誤差修正を行う。まず姿勢角誤差の推定値が所定の角度で得られるようになったとき、姿勢誤差の推定値によって座標変換マトリックスの誤差修正をし、それが終了した後に、方位角の設定値によって座標変換マトリックスを誤差修正するものである。方位姿勢誤差演算部としてカルマンフィルタが使用されている。

特許文献４の移動体制御装置及び移動体制御方法は、軽量、小型でかつ安価なセンサ等を利用して複合航法システムを実現することを目的とするものである。具体的には慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する。このとき慣性航法データについての状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算するにあたり、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、位置、姿勢、速度の各誤差を演算し、当該誤差情報を乗じることで位置、姿勢、速度の情報が補正され、移動体の制御に供される移動体制御装置である。

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

特許文献１は姿勢計測においてカルマンフィルタを用い誤差を十分小さくなるように処理するものであるが、センサとして３軸それぞれの方向の加速度を測定する加速度センサと３軸回りの角速度を測定するジャイロのみを入力手段として用いているものであり、磁気センサ，速度計から得られるデータも考慮に入れたものではない。
また、特許文献２は慣性航法装置としてカルマンフィルタを用いているものの、方位や姿勢角の誤差を推定し、推定した誤差を除去する構成ではなく、またその処理をするためのセンサ部はジャイロセンサおよびアクセロメータのみである。
特許文献３はカルマンフィルタにより方位角誤差修正を分離し姿勢角誤差修正を行ってから方位角誤差修正を行うものであり、センサとしてＸジャイロ，ＹジャイロおよびＺジャイロならびにＸ加速度計，Ｙ加速度計およびＺ加速度計からの角速度および加速度のみのデータを得るものである。また特許文献４はセンサとして加速度センサ，ジャイロセンサおよびＧＰＳ信号受信部の各データを用い、微小変化単位四元数として表示される誤差情報の演算には磁気センサや速度計の観測値を用いるものではない。

ところで、本件発明者は移動体の姿勢推定を行うにあたり、ジャイロおよび加速度計からの角速度や加速度の測定値だけでなく、磁気センサおよび速度計の方位角，地磁気伏角および速度の値も利用し、これらセンサの出力をカルマンフィルタで所定の処理をすることにより、姿勢誤差を推定し精度の良好な移動体の姿勢を推定できると考察した。
そこで実際の姿勢に対し精度良く姿勢推定するためには磁気センサにより得られる地磁気と加速度計により得られる重力加速度を姿勢推定するために入力するベクトルとして用いることを考えた場合、加速度について重力以外にも空気などに由来する外力の成分が加算されてしまい、特に旋回時や加速度時に姿勢推定に誤差が生ずる原因となる。

これに対処するために重力加速度以外の外力に由来する加速度成分を除去する方法が考えられる。
〔１〕ピトー管によるＸ，Ｙ，Ｚ方向外力の推定方法
ピトー管により得られた速度に角速度を乗じることによりＹ，Ｚ軸について旋回加速度が得られ、さらにピトー管により得られた速度の時間変化からＸ軸に関する加速度が得られる。これらを加速度計により得られた値から引くことにより（１）式のように重力加速度を得ることができる。ただし、ａは測定された加速度，ｇは重力加速度，ωは角速度である。ｖ_x はＸ軸に関する速度である。

カルマンフィルタに入力される加速度は（１）式の修正を受けるが、観測方程式は従来のカルマンフィルタと変わることがない。
この方式は旋回加速度と前後方向の加速度以外の外力を除去できない欠点がある。また、風速が一定でない場合、本来存在しない加速度が入力されてしまうことになる。例えば、航空機が向かい風で飛行しているときに風速が強まりつつあるとすると抗力が増えるため機体は減速するが、ピトー管の値だけ見ると加速しているように見えるため計算された重力加速度が実際とは異なる向きに現れてしまう。

〔２〕カルマンフィルタを用いたピトー管によらない外力の推定方法
カルマンフィルタが推定する状態量と外力を追加する。加速度計により得られた値から推定された外力を引くと（２）式のように重力加速度が得られる。ピトー管によらないため風速の変化などの影響を受けることはない。

観測方程式はつぎによって求められる。
機体固定座標で測定された重力加速度ベクトルｇ^b と現在の推定値から得られると予想される重力加速度ベクトルｇ^b'との差と姿勢推定値のずれｑ_e との関係を示す観測方程式は

となる。これに外力の推定を加えるとａ＝ｇ＋ｆ_e から（４）式のようになる。

つぎに共分散の時間変化については以下の通りである。
ｆ_e は航空機の運動に関する操作量であるから、時間変化を定式化するのは不可能であるためｆ_e の値は観測による修正以外では変化させないことにする。

共分散行列は、現在の推定値の不確かさを表す量であるから、Ｓのｆ_e に関する対角成分に適当な数値を入力すれば、ｆ_e の不確かさを適宜上昇させることができ、加速度が観測されたときに修正されるようにすることができる。

ｆ_e の過去の推定値を推定に利用し続けていくため、急な旋回やスロットルの急激な操作などの外力が激しい変化に弱いと考えられる。また、共分散の時間変化を決めるパラメータであるＳの大きさも、外力が激しく変化すると予想される場合には大きく、変化が少ないと予想される場合には小さく設定する必要があるが、これを定量的に決定する方法を考える必要が生ずる。

〔３〕ピトー管とカルマンフィルタを組み合わせた外力の推定方法
先に示したピトー管を利用しない外力の推定にピトー管を使った旋回加速度の推定を組み合わせることが考えられる。
操作によって大きく変化すると考えられる旋回加速度を推定値から除外するので急な外力の変化にも強く、推定される外力の大きさは小さくなり共分散の時間変化を決めるパラメータの決定はピトー管を用いない場合に比べて精度を必要としなくなる。

観測方程式と共分散の時間変化については先に示したピトー管を利用しない外力の推定方法と同じである。
旋回による加速度以外の外力が急激に変化したときに対応することができず、共分散の時間変化を決めるパラメータであるＳの大きさを決める方法を定量的に決める方法も必要となる。

本発明は上記考察に鑑みなしたもので、その目的は軽量，小型および安価であって、低精度なセンサ群（ジャイロ，加速度計，速度計および磁気センサ）とカルマンフィルタを組み合わせることにより姿勢情報の計算にあたって四元数を用い、センサの観測値から計算する修正量も四元数とすることにより高精度で移動体の姿勢誤差を推定し、十分な精度で移動体の姿勢を推定することができる慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法を提供することにある。

前記目的を達成するために本発明の請求項１の姿勢推定装置は、ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサと、前記ジャイロで観測する角速度に基づき姿勢を表す微小変化単位四元数であるクオータニオンを求めるクオータニオン演算手段と、前記加速度計で観測する加速度および前記速度計で観測する速度に基づき重力加速度を求める重力加速度演算部と、前記磁気センサで観測する磁気ベクトルと、前記クオータニオン演算手段で求めたクオータニオンに基づき所定の式より算出される磁気ベクトルの推定値との差を算出する第１の差演算手段と、前記演算部の重力加速度と、前記クオータニオン演算手段で求めたクオータニオンに基づき所定の式より算出される重力加速度の推定値との差を算出する第２の差演算手段と、前記第１の差演算手段の磁気ベクトルデータおよび第２の差演算手段の重力加速度データより共分散行列Ｐの変数の行列を求める変数演算部，前記変数演算部の変数の行列を入力し、所定の式よりカルマンゲインを求めるカルマンフィルタ部ならびに前記変数演算部の共分散行列Ｐの変数の行列および前記カルマンフィルタ部のカルマンゲインを入力し、所定の式より共分散行列Ｐの推定値を算出する状態推定フィルタ演算手段と、前記カルマンゲイン，前記第１の差演算手段の磁気ベクトルデータおよび前記第２の差演算手段の重力加速度データによりクオータニオン誤差および外力の誤差量を算出する誤差演算手段とを備え、前記クオータニオン演算手段において前記クオータニオン誤差と前記ジャイロで観測する角速度に基づき算出される姿勢の各情報のクオータニオンにより姿勢の推定値を演算し、該演算した推定値をフィードバックすることにより、移動体の姿勢の誤差を補正した情報を出力することを特徴とする。
本発明の請求項２の姿勢推定方法は、ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサと、角速度に基づき姿勢を表す微小変化単位四元数であるクオータニオンを求めるクオータニオン演算手段と、重力加速度以外の加速度を除去する重力加速度演算部と、観測値と推測値の差を算出する磁気ベクトルの第１の差演算手段および観測値と推測値の差を算出する重力加速度の第２の差演算手段と、カルマンフィルタを用い、共分散行列の推測値を算出する状態推定フィルタ演算手段と、前記カルマンフィルタで求めたカルマンゲイン，前記第１の差演算手段の磁気ベクトルデータおよび前記第２の差演算手段の重力加速度データによりクオータニオン誤差および外力の誤差量を算出する誤差演算手段とを有し、ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサの観測値に基づいて状態推定フィルタ演算を行い、移動体の姿勢の情報を出力し、前記観測値において前記状態推定フィルタ演算における、移動体の姿勢の各誤差を演算し、状態推定フィルタ演算手段が出力する姿勢の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として計算された誤差情報を用いて、前記姿勢の各誤差を演算し、前記状態推定フィルタ演算手段が出力する、移動体の姿勢の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることにより補正されることを特徴とする。

上記構成によれば、ＭＥＭＳセンサなど、軽量，小型、かつ安価なセンサを用いて精度の高い移動体の姿勢を実現すべき誤差を推定する姿勢推定装置および姿勢推定方法を実現でき、該姿勢推定装置，方法から移動体の姿勢や位置などを精度良く表示する姿勢表示装置、例えば、航空機の計器表示器などを提供することができる。

本発明による慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置の概略図であり、航空機に適用した場合である。 本発明による慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置の構成をブロックで示した図である。 ω，Ｘ，Ｙ，Ｚの定義付けを説明するための図である。 本発明を航空機に適用した場合のカルマンフィルタによる処理の流れを説明するための図である。 本発明により推定した航空機の姿勢角について水平旋回しているデータについてのロール角，ピッチの推定結果を示すグラフである。

以下、図面を参照して本発明の実施の形態を詳しく説明する。
図１は、本発明による慣性センサ，加速度計，磁気センサおよび速度計を用いた移動体の姿勢推定装置の概略図であり、航空機に適用した場合である。
機体にジャイロ装置１１，加速度計１２，速度計１３および磁気センサ１４が搭載される。ジャイロ装置はＭＥＭＳ（Micro Electro Mechanical Systems) ジャイロであり、予め振動させている微小振動板が回転により受けるコリオリ力により回転角速度ωを求めるものである。
加速度計１２もＭＥＭＳセンサであり、航空機の移動により加速度ａを出力する。

また、速度計１３はＭＥＭＳセンサで構成されており、対気速度を出力する。また磁気センサ１４は磁気ベクトルを与える。この磁気センサもＭＥＭＳセンサで構成することができる。演算器１５では加速度ａ，速度Ｖ，ω_z ω_y の入力を受けて重力加速度ｇのみを演算して出力するが、実際は外力ｆ_e が完全に除かれていない。
カルマンフィルタ１９はジャイロ装置１１から入力する角速度ωを積分して回転角を算出する。算出された回転角に対し演算器１７ではｆ_e を含む重力加速度ｇ，磁気ベクトルおよび積分器１６からの回転角により姿勢１８を表示するクオータニオンｑの推定値を得ることができる。カルマンフィルタ１９内では各センサからの観測値や推定した値を使用して所定の演算をし共分散行列の形で誤差の推定が行われる。

図２は本発明による慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置の構成をブロックで示した図で、図１と同様、航空機に適用したものである。
これはフィルタ演算部２２で演算して得た外力ｆ_e ２１が出力され、外力ｆ_e が演算器２４に入力され、外力ｆ_e も取り除かれ重力加速度ｇ２３がフィルタ演算部２２に入力される。他の部分の構成は図１と同じ構成である。

つぎに本発明による姿勢推定装置の機能を実現するため、カルマンフィルタ，姿勢角，姿勢角誤差，観測するベクトル，観測方程式に用いる数式を説明する。
なお、数式説明にあたり、座標系，クオータニオン変数および微小な回転を表すクオータニオン変数について定義付けを行う。
〔１〕座標系Ｘ，Ｙ，Ｚ軸およびω
３軸は図３に示すように表されＸ軸は航空機の進行方向であり、ωはＸ，Ｙ，Ｚ軸それぞれの周りの角速度である。Ｘ軸に対しω₀ （ω_x ），Ｙ軸に対しω₁ （ω_y ），Ｚ軸に対しω₂ （ω_z ）と表示される。
〔２〕クオータニオン変数
クオータニオン変数はｑで表され、ｑは

の４つの変数であり、ｑ₀ ，ｑ₁ ，ｑ₂ ，ｑ₃ の４つの変数で姿勢を表す。
〔３〕微小な回転を表すクオータニオン変数
微小な回転を表すクオータニオン変数はｑ_e で表され、ｑ_e は

の４つの変数であり、１，ｑ_e1，ｑ_e2，ｑ_e3で表される。１つ目の成分は「１」という定数となる。すなわち、微妙な回転を表す姿勢誤差である。
（１）カルマンフィルタについて
本発明による姿勢推定装置に用いられるシステムはつぎのように表されるとする。

ただし、ｘは推定した状態量，ｕは入力，ｗは外乱である。また、推定値がどの程度信頼できるものであるかを示す共分散行列Ｐも合わせて考える。
○Time Update について
本発明による姿勢推定装置の処理の流れとして時間経過に対し状態量，共分散行列が係わる式はつぎのようになる。

○Measurement Updateについて
本発明による姿勢推定装置の各センサの観測値入力に対し状態量，カルマンゲイン，共分散行列，行列（変数Ｈ）などが係わる式はつぎのようになる。

だだし、ｖを観測雑音として

（２）システムの表現
本発明による姿勢推定装置を移動体に適用したときの姿勢角や姿勢角誤差の表現は、クオータニオンｑを用いる。
○姿勢角の表現
座標系は北をＸ，東をＹ，下をＺとして地面固定座標を、前方をＸ，右側をＹ，下をＺとして機体固定座標をとる。このときの姿勢はクオータニオンｑで表すこととする。
このクオータニオンは地面固定座標から機体固定座標への変数を与える。クオータニオンが座標変換を与えるとは、あるベクトルｘに対して

で与えられるベクトルＸ^' が変換されたベクトルになっていることを意味する。（ただしｑ^* は共役クオータニオン）この変換は行列で表すこともでき、例えば地面固定座標をｎ，機体固定座標をｂとして変換行列を

また、ｑの時間変化は

で表される。ただし、ωは地面固定座標で計測された角速度である。

○姿勢角誤差の表現
姿勢を表すクオータニオンには絶対値が１でなければならないという制約があるが、カルマンフィルタにより得られた差分を加算して推定値を更新する方法ではこの制約を守ることができない。誤差として微小な回転を表すクオータニオン

を考え（ただし、ｑ_e は３つの要素からなるベクトル）、ｑの推定値

と誤差との和ではなく積の形で表すことにする。
また、この誤差の時間変化は機体固定座標で計測された角速度の誤差をΔωとすると

誤差同士の積は微小であるため無視すると

を得る。つまり、

○観測するベクトルの表現
観測量として地磁気ベクトルと重力加速度ベクトルを姿勢角の修正に用いるが、加速度計により得られる加速度はそのままでは重力以外の外力も含んだものであるのでこれを除去する。
まず、移動体が横すべりなしで旋回を行っている場合、旋回により生じる加速度は、速度をＶとして

と表されるので、まずこれを加速度計により得られた加速度から減ずる。
これにより直進時・定常旋回時ともに重力加速度を正しく得ることができるようになったと考えられるが、それ以外の場合においても姿勢を正しく計算するため、旋回加速度以外の外力をカルマンフィルタにより推定する。
以上をまとめると重力加速度ｇは加速度計により得られる加速度ａに以下の計算を施すことにより得られる。

ただし、ｆ_e は推定された旋回加速度以外の外力を示す。

（３）観測方程式
使用する観測量として地磁気センサにより地磁気ベクトルが、加速度計により重力加速度がそれぞれ機体軸上で得られるとする。
このとき、推定値が機体固定座標から誤差の分だけずれた座標ｂ^' への変換を表しているとすると、ｑ_e に対する変換行列は、

であるので、

あるベクトル量について地面固定座標で測定した量をＶⁿ ，機体固定座標で測定した量Ｖ^b と書くことにすると、

となり、機体固定座標で測定されたベクトルＶ^b と、現在の推定値から得られると予想されるベクトルＶ^b'の差を入力とした観測方程式を構成できる。

図４は本発明を航空機に適用した場合のカルマンフィルタによる処理の流れを説明するための図である。以後使用する記号における表現形態を以下のように定義する。
「ｑハット」という表現はクオータニオンｑに推定を示す意味のハットマークを乗せたものと同等とする。クオータニオンだけでなく，Ｐ，ｘなどの記号の後部に「ハット」と表現されている場合も、その記号の推定値とする。
「Ｐバー」という表現は共分散行列Ｐに現在の値を示すものと同等とする。共分散行列だけでなく、ｘなどの記号の後ろに「バー」と表現されている場合も、その記号の現在の値であるとする。
また、数式特定のため（１）式〜（２９）式と記載するが、これら数式は〔発明が解決しようとする課題〕の項目に記載した数式ではなく、〔発明を実施するための形態〕の項目に記載した数式である。さらに図４のカルマンフィルタのアルゴリズムの流れの中で各数式を用いるステップはその数式を計算するための演算部という表現にした。
本発明による姿勢推定装置の処理はカルマンフィルタのアルゴリズムを実行するプログラム処理によって実現される。

図４において観測値である加速度計３１の加速度ａと速度計３２の速度Ｖ，外力ｆ_e およびジャイロ３４からの角速度ωの入力が演算部４０に送られる。演算部４０では（２２）式の計算が行われ、重力加速度が算出される。ここで（２２）式においてω₂ はＺ軸回りの角速度であり、ω₁ はＹ軸回りの角速度である。観測値から得る重力加速度は（２２）式の左辺のｇである。この重力加速度ｇが演算部４１に入力される。また、磁気センサ３３から観測した磁気ベクトルが出力される。
演算部４１の（２５）式におけるｖ^b は機体固定座標で測定されたベクトルであり、ｖ^b は３軸の重力加速度と３軸の磁気ベクトルを含んでいる。分解するとＧ_(b) とＭ_(b) が出力される。

ここで、（２５）式におけるｖ^b'−ｖ^b は（８）式の右辺の括弧内の

と同じになる。
よって

から（８）式は（２８）式に書き換えられる。

（２８）式において右辺の行列は変数Ｈである。

（８）式に注目すると、ｘにバーが付いているものは当初推定値であるが、角速度を入力することにより、バーがハットになり、ｘハットとなる。角速度が入らなければ、ハットをそのままバーに変えて計算する。
よって推定する状態量は（８）式より

となる。よって、演算部４３において

となり、（２９）式より推定したクオータニオンの姿勢誤差ｑ_e と推定した外力の誤差Δｆ_e が得られる。
なお、演算部４２では計算した（２５）式におけるｖ^b'において、推定されたクオータニオンｑから推定された重力加速度Ｇ_(b')と推定された磁気ベクトルＭ_(b')とが出力されている。演算部４４には観測値から得た重力加速度から分解して得たＧ_(b) と上記推定された重力加速度Ｇ_(b')が入力され、Ｇ_(b) −Ｇ_(b')の演算が行われ、演算部４３と演算部５１にそれぞれ入力される。
また演算部４５では観測した磁気ベクトルから分解して得たＭ_(b) と上記推定された磁気ベクトルＭ_(b')が入力され、Ｍ_(b) −Ｍ_(b')の演算が行われ、演算部４３と演算部５１にそれぞれ入力される。
演算部４３で出力された外力の誤差量Δｆ_e は演算部５５に加えられ、演算部５５でｆ_e が算出され、算出されたｆ_e が演算部４０に入力される。演算部４０では先に述べたように（２２）式の演算が行われ、外力の誤差ｆ_e を除去した重力加速度ｇが得られる。

つぎに姿勢を表すクオータニオンｑを求めるループ４９の動作について説明する。
ジャイロ３４から角速度ωが演算部４６に入力されており、演算部４６では（１６）式が計算されｑの偏微分値を得る。さらに演算部４７ではルンゲ・クッタ法等による微分方程式を解くことによりクオータニオンｑを算出する。演算部４８にはこのクオータニオンｑとクオータニオン誤差ｑ_e が入力され、

の演算が行われる。得られる推定したクオータニオンｑハットは演算部４６の（１６）式にフィードバック入力される構成となっている。ｑハットを演算部１６にフィードバックする経路の中間に切替部５７（ソフト処理）の機能を有している。切替部５７は、磁気センサ，加速度計，速度計で観測した場合は端子ａ側に接続されるため、上述のようにｑハットは演算部４６にフィードバック入力される。しかしながら、例えば、センサや加速度計，速度計が故障したり、また、ある地域で観測値を得ることを望まない，観測できない状況などの場合には観測しないので、かかるセンサなどから入力がないため、切替部５７は端子ｂに接続されるようになっている。
したがって、磁気センサ，加速度計，速度計で観測しない場合には、観測値の入力は切替部５７の端子ｂ側に切り替えられているためフィードバックは行われない。
すなわち、観測した場合は、クオータニオンｑを求めるループは演算部４６，４７，４８により形成され、観測しない場合は演算部４６，４７のループが形成され、クオータニオンｑが繰り返し算出され出力される。
このようにして演算されたクオータニオンｑは姿勢５８の情報として出力される。また、上述したようにクオータニオンｑの推定値は（２５）式におけるＶ^b'の演算部４２に供給される。演算部４２では（２５）式におけるＶ^b'の演算が行われ、クオータニオンｑの推定された姿勢情報からはこの推定値から得た重力加速度と磁気ベクトルが出力され、演算部４４と４５に入力される。

つぎに共分散行列Ｐを求めるループを含む演算部５４の動作について説明する。
上述したように演算部４４でＧ_(b) −Ｇ_(b')の、演算部４５でＭ_(b) −Ｍ_(b')の演算がそれぞれ行われ演算部５１に入力される。演算部５１は（２５）式と等価の式から導き出されたものであり、（２８）式に示す変数Ｈの行列が算出される。変数Ｈは演算部５２の（９）式と、演算部５３の（１０）式に入力される。（９）式はカルマンゲインＫ_K+1 を、（１０）式は共分散行列Ｐ_K+1 ハットを求める式である。
（９）式で示すカルマンゲインＫ_K+1 は右辺に共分散行列Ｐ_K+1 バー，Ｈ_K+1 およびＲ_K+1 を含む項で形成された式であり、変数Ｈは演算部５１から入力される。また、（１０）式で示す共分散行列Ｐ_K+1 ハットは単位行列Ｉ，カルマンゲインＫ_K+1 ，共分散行列Ｐバー，Ｈ_K+1 を含む項で形成された式であり、変数Ｈは演算部５１から入力される。
演算部５２の（９）式には変数Ｈ，Ｒ_K+1 （最初に設定される定数）およびＰ_K+1 バーが入力され、（９）式で演算されてカルマンゲインＫ_K+1 を得る。
また、演算部５３の（１０）式には演算部５１から変数Ｈおよび演算部５２のカルマンゲインＫ_K+1 が入力され、（１０）式で演算されてＰ_K+1 ハットを得る。なお、Ｐ_K+1 バーは（３）式から得ることができる。（３）式のＰ_K+1 バーはΓ，Ψ_K ，Ｓ_K を含む項で形成される式であり、ここではΓは単位行列，Ψ_K およびＳ_K を固定変数として扱う。
演算部５３で演算して出力されるＰ_K+1 ハットは演算部５０の（３）式にフィードバックされる。この経路に切替部５６（ソフト処理）の機能を有している。切替部５６は、上述したように磁気センサ，加速度計，速度計で観測した場合、端子ａ側に接続されるため、Ｐ_K+1 ハットは演算部５０にフィードバック入力される。また、観測しない場合は、センサなどから入力がないため、切替部５６は端子ｂに接続されるようになっている。
磁気センサなどの出力を観測した場合、演算部５０の（３）式にはＰ_K+1 ハットが入力され、さらに（７）式で表されるＳ_K と（５）式で表されるＱ_K が入力され、演算されてＰ_K+1 バーを得る。すなわち、観測した場合は、共分散行列Ｐを求めるループは演算部５０，５２，５３により形成され、観測しない場合は演算部５０のみのループが形成され、共分散行列Ｐが繰り返し算出され出力される。

図５は水平旋回しているデータについてのロール角，ピッチの推定結果を示すグラフである。
以上の姿勢推定がどの程度有効なものであるかを検証するため、飛行データをもとに水平旋回の姿勢推定についてシミュレーションし、図５に示すような結果が得られた。
本発明により求めた姿勢ｑを所定の変換式によってロール角およびピッチ角を導いたもので、ロール角およびピッチ角の誤差の範囲は小さく収まっている。
図５において、実線が推定されたピッチ角，点線が実際のピッチ角，一点鎖線が推定されたロール角，二点鎖線が実際のロール角である。ロール角が大きくなると、誤差が大きくなる傾向にあるが、姿勢誤差と外力が有効に推定されているため、精度良く姿勢を指示していることが理解できる。

以上の実施の形態は、姿勢推定装置を移動体として航空機に適用した場合について説明したが、航空機に限らず、自動車や船舶などにも同様に適用することが可能であり、同じような効果を得ることができる。

航空機などの移動体に搭載し、移動体の姿勢推定を行い、この姿勢推定を計器表示部などに反映する。

１１，３４ ジャイロ装置（ジャイロ）
１２，３１ 加速度計
１３，３２ 速度計
１４，３３ 磁気センサ
１５，１７ 演算器
１６ 積分器
１８，５８ 姿勢
１９ カルマンフィルタ
４０，４１，４２，４３，４４，４５，４６，４７，４８，５０，５１，５２，５３，５５ 演算部
４９ クオータニオンｑを求めるループ
５４ 共分散行列Ｐを求めるループを含む演算部
５６，５７ 切替部

Claims (2)

1. ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサと、
   前記ジャイロで観測する角速度に基づき姿勢を表す微小変化単位四元数であるクオータニオンを求めるクオータニオン演算手段と、
   前記加速度計で観測する加速度および前記速度計で観測する速度に基づき重力加速度を求める重力加速度演算部と、
   前記磁気センサで観測する磁気ベクトルと、前記クオータニオン演算手段で求めたクオータニオンに基づき所定の式より算出される磁気ベクトルの推定値との差を算出する第１の差演算手段と、
   前記演算部の重力加速度と、前記クオータニオン演算手段で求めたクオータニオンに基づき所定の式より算出される重力加速度の推定値との差を算出する第２の差演算手段と、
   前記第１の差演算手段の磁気ベクトルデータおよび第２の差演算手段の重力加速度データより共分散行列Ｐの変数の行列を求める変数演算部，前記変数演算部の変数の行列を入力し、所定の式よりカルマンゲインを求めるカルマンフィルタ部ならびに前記変数演算部の共分散行列Ｐの変数の行列および前記カルマンフィルタ部のカルマンゲインを入力し、所定の式より共分散行列Ｐの推定値を算出する状態推定フィルタ演算手段と、
   前記カルマンゲイン，前記第１の差演算手段の磁気ベクトルデータおよび前記第２の差演算手段の重力加速度データによりクオータニオン誤差および外力の誤差量を算出する誤差演算手段と、
   を備え、
   前記クオータニオン演算手段において前記クオータニオン誤差と前記ジャイロで観測する角速度に基づき算出される姿勢の各情報のクオータニオンにより姿勢の推定値を演算し、該演算した推定値をフィードバックすることにより、移動体の姿勢の誤差を補正した情報を出力することを特徴とする慣性センサ，速度計および磁気センサを用いた移動体の姿勢推定装置。
2. ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサと、
   角速度に基づき姿勢を表す微小変化単位四元数であるクオータニオンを求めるクオータニオン演算手段と、
   重力加速度以外の加速度を除去する重力加速度演算部と、
   観測値と推測値の差を算出する磁気ベクトルの第１の差演算手段および観測値と推測値の差を算出する重力加速度の第２の差演算手段と、
   カルマンフィルタを用い、共分散行列の推測値を算出する状態推定フィルタ演算手段と、
   前記カルマンフィルタで求めたカルマンゲイン，前記第１の差演算手段の磁気ベクトルデータおよび前記第２の差演算手段の重力加速度データによりクオータニオン誤差および外力の誤差量を算出する誤差演算手段と、
   を有し、
   ジャイロおよび加速度計を含む慣性センサ，速度計ならびに磁気センサの観測値に基づいて状態推定フィルタ演算を行い、移動体の姿勢の情報を出力し、
   前記観測値において前記状態推定フィルタ演算における、移動体の姿勢の各誤差を演算し、
   状態推定フィルタ演算手段が出力する姿勢の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として計算された誤差情報を用いて、前記姿勢の各誤差を演算し、
   前記状態推定フィルタ演算手段が出力する、移動体の姿勢の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることにより補正されることを特徴とする慣性センサ，速度計および磁気センサを用いた移動体の姿勢推定方法。

Images