JP2002323322A - 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム - Google Patents

慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム

Info

Publication number
JP2002323322A
JP2002323322A JP2001128261A JP2001128261A JP2002323322A JP 2002323322 A JP2002323322 A JP 2002323322A JP 2001128261 A JP2001128261 A JP 2001128261A JP 2001128261 A JP2001128261 A JP 2001128261A JP 2002323322 A JP2002323322 A JP 2002323322A
Authority
JP
Japan
Prior art keywords
equation
estimated value
parameter
observation
output
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
JP2001128261A
Other languages
English (en)
Other versions
JP3726884B2 (ja
Inventor
Toru Shiho
徹 志甫
Akira Ishii
亮 石井
Yuichiro Sasajima
雄一郎 笹島
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nihon University
Original Assignee
Nihon University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nihon University filed Critical Nihon University
Priority to JP2001128261A priority Critical patent/JP3726884B2/ja
Priority to EP02724638A priority patent/EP1382936A4/en
Priority to PCT/JP2002/004103 priority patent/WO2002088633A1/ja
Priority to US10/476,402 priority patent/US7248948B2/en
Publication of JP2002323322A publication Critical patent/JP2002323322A/ja
Application granted granted Critical
Publication of JP3726884B2 publication Critical patent/JP3726884B2/ja
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
    • G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/244—Spacecraft control systems
    • B64G1/245—Attitude control algorithms for spacecraft attitude control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/36—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/36—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors
    • B64G1/369—Guiding or controlling apparatus, e.g. for attitude control using sensors, e.g. sun-sensors, horizon sensors using gyroscopes as attitude sensors
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Automation & Control Theory (AREA)
  • Algebra (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

(57)【要約】 【課題】 スタートラッカなどのセンサを必要とするこ
となく、ジャイロからの情報のみで人工衛星の姿勢の推
定を行う。 【解決手段】 慣性計測装置１から状態量ω（ｔ）を受
けて予め定められたシステム方程式及び観測方程式に入
力するとともに、これらに関して時間更新処理及び観測
更新処理を行うカルマンフィルタ３と、前記カルマンフ
ィルタの出力に基づき修正ロドリゲスパラメータα
（ｔ）の時間微分の推定値を生成する処理部４と、前記
処理部の出力に基づき修正ロドリゲスパラメータの推定
値を生成する積分処理部５と、予め与えられた初期値及
び前記積分処理部の出力に基づき観測感度行列を更新
し、その更新出力を前記カルマンフィルタに供給するシ
ステム伝播行列生成部６と、前記積分処理部の出力に基
づき座標変換行列Ｒを生成する変換マトリクス生成部７
と、前記変換マトリクス生成部の出力に基づき姿勢推定
を行う姿勢推定部８とを備える。

Description

【発明の詳細な説明】
【０００１】
【発明の属する技術分野】この発明は、慣性計測装置か
らの信号に基づき姿勢を推定するための姿勢推定装置及
び方法並びにプログラムに関する。
【０００２】
【従来の技術】近年、宇宙開発の分野において大型で多
機能な人工衛星の他に、単機能ではあるが開発の容易な
マイクロサット（超小型人工衛星）が注目されている。
これらマイクロサットの開発が容易であることは人工衛
星の単価を引き下げることと相俟って、宇宙開発をより
身近なものにすると考えられる。現在、ピギーバック衛
星として多くのマイクロサットが打ち上げられている。
【０００３】単機能・安価であることを身上とするマイ
クロサットであるが、その高性能化には３軸制御が不可
欠である。そのためには高性能な姿勢観測系が必要とな
るが、衛星の開発費の高騰を考えればあまり高価な機材
を使用することができない。
【０００４】従来、３軸衛星の姿勢センサとして各種セ
ンサが用いられている。例えば、慣性基準装置などのジ
ャイロから姿勢角速度を検出し、これを積分することに
よりロール、ヨーを含めて正確な姿勢推定を行うことが
知られている。慣性基準装置は、レートジャイロなどを
３軸に搭載し、パッケージにしたものである。ジャイロ
の信号を直接積分して使うと、動的な応答のよい検出が
できるメリットがあるが、反面、長時間ではドリフトが
累積してくるという問題がある。それを補正するため
に、スタートラッカなどを用い、絶対基準からの誤差を
検出系にフィードバックすることが行われている。しか
し、この手法は高価なセンサを必要とし、単機能・安価
であるマイクロサットに適用できない。
【０００５】
【発明が解決しようとする課題】本発明は係る課題に鑑
みてなされたもので、精度がよくかつ安価である姿勢推
定装置及び方法、すなわち、比較的単価の低い圧電振動
ジャイロやファイバージャイロ等の機材を用いて人工衛
星の姿勢推定を行うことができる慣性計測装置を用いた
姿勢推定装置及び方法並びにプログラムを提供すること
を目的とする。
【０００６】この発明によれば、スタートラッカなどの
センサを必要とすることなく、ジャイロからの情報のみ
で姿勢の推定を行うことができる。このような状況は従
来の人工衛星においても生じる。例えば、実際の運用下
において人工衛星がマニューバ中でスタートラッカなど
のセンサを使用できない状態にあるときなどに対応す
る。
【０００７】
【課題を解決するための手段】この発明に係る慣性計測
装置を用いた姿勢推定装置は、慣性計測装置から状態量
ω（ｔ）を受けて予め定められたシステム方程式及び観
測方程式に入力するとともに、これらに関して時間更新
処理及び観測更新処理を行うカルマンフィルタと、前記
カルマンフィルタの出力に基づき修正ロドリゲスパラメ
ータα（ｔ）の時間微分の推定値を生成する処理部と、
前記処理部の出力に基づき修正ロドリゲスパラメータの
推定値を生成する積分処理部と、予め与えられた初期値
及び前記積分処理部の出力に基づき観測感度行列を更新
するとともに、その更新出力を前記カルマンフィルタに
供給するシステム伝播行列生成部と、前記積分処理部の
出力に基づき座標変換行列Ｒを生成する変換マトリクス
生成部と、前記変換マトリクス生成部の出力に基づき姿
勢推定を行う姿勢推定部とを備えるものである。
【０００８】好ましくは、前記修正ロドリゲスパラメー
タは、ｅを回転軸の方向余弦、θを回転軸回りの回転角
とするとき、後述の式(2.2.31)(2.2.32)で与えられる。
【０００９】好ましくは、前記観測方程式は、後述の式
(2.3.1)としたとき、式(2.3.2)に観測ノイズを加えるこ
とにより与えられる。
【００１０】好ましくは、前記システム方程式は、後述
の式(2.3.5)の右辺をｆ(a(t),ａドット(t)，t)とおいた
ときの後述の式(2.3.6)にシステムノイズを加えること
により与えられる。
【００１１】この発明に係る慣性計測装置を用いた姿勢
推定方法は、慣性計測装置から信号を受けて修正ロドリ
ゲスパラメータα（ｔ）の時間微分の状態量と状態量ω
（ｔ）を観測量とするシステム方程式及び観測方程式を
生成する第１ステップと、拡張カルマンフィルタを用い
て前記パラメータα（ｔ）の時間微分の推定値を求める
第２ステップと、前記第２ステップで求められた推定値
を数値積分して前記パラメータα（ｔ）の推定値を求め
る第３ステップと、前記第３ステップで求められた推定
値に基づき変換マトリクスＲ（ｔ）を決定する第４ステ
ップと、前記変換マトリクスＲ（ｔ）に基づき姿勢の推
定値を求めるステップとを備えるものである。
【００１２】この発明に係るプログラムは、コンピュー
タに、慣性計測装置から信号を受けて修正ロドリゲスパ
ラメータα（ｔ）の時間微分の状態量と状態量ω（ｔ）
を観測量とするシステム方程式及び観測方程式を生成す
る第１ステップと、拡張カルマンフィルタを用いて前記
パラメータα（ｔ）の時間微分の推定値を求める第２ス
テップと、前記第２ステップで求められた推定値を数値
積分して前記パラメータα（ｔ）の推定値を求める第３
ステップと、前記第３ステップで求められた推定値に基
づき変換マトリクスＲ（ｔ）を決定する第４ステップ
と、前記変換マトリクスＲ（ｔ）に基づき姿勢の推定値
を求めるステップとを実行させるためのものである。
【００１３】この発明に係るプログラムは、例えば、記
録媒体に記録される。媒体には、例えば、フレキシブル
ディスク、ハードディスク、磁気テープ、光磁気ディス
ク、ＣＤ（ＣＤ−ＲＯＭ、Ｖｉｄｅｏ−ＣＤを含む）、
ＤＶＤ（ＤＶＤ−Ｖｉｄｅｏ、ＤＶＤ−ＲＯＭ、ＤＶＤ
−ＲＡＭを含む）、ＲＯＭカートリッジ、バッテリバッ
クアップ付きのＲＡＭメモリカートリッジ、フラッシュ
メモリカートリッジ、不揮発性ＲＡＭカートリッジ等を
含む。
【００１４】また、電話回線等の有線通信媒体、マイク
ロ波回線等の無線通信媒体等の通信媒体を含む。インタ
ーネットもここでいう通信媒体に含まれる。
【００１５】媒体とは、何等かの物理的手段により情報
（主にデジタルデータ、プログラム）が記録されている
ものであって、コンピュータ、専用プロセッサ等の処理
装置に所定の機能を行わせることができるものである。
要するに、何等かの手段でもってコンピュータにプログ
ラムをダウンロードし、所定の機能を実行させるもので
あればよい。
【００１６】本発明は、修正ロドリゲスパラメータを拡
張カルマンフィルタへ適用したものである。姿勢の伝播
式としてはクォータニオンを用いた変換式が一般に知ら
れている。クォータニオンはオイラー角と異なり、姿勢
に対して特異点が存在せず、超越関数を使用しないなど
の利点が存在し、一般に衛星の姿勢の記述にはこれが用
いられている。しかし、これをジャイロからのみの情報
に対して推定を行うとすると、推定アルゴリズムである
カルマンフィルタに必要な観測方程式が成立しない。カ
ルマンフィルタによる推定を行うためには必要な方程
式、すなわちシステム方程式と観測方程式とを成立させ
る必要がある。そこで、本発明においてクォータニオン
の応用である修正ロドリゲスパラメータを用いることに
より、それらの式を成立させることに成功したのであ
る。
【００１７】
【発明の実施の形態】この発明の実施の形態に係る装置
／方法について図面を参照して説明する。以下の説明
は、本発明の実施の形態に係る装置／方法を人工衛星の
姿勢推定に用いる場合を例にとったものである。この発
明の実施の形態に係る装置／方法は、人工衛星の姿勢情
報をジャイロからの出力のみにより長時間、精度良く推
定するものである。人工衛星の姿勢決定方程式として、
修正ロドリゲスパラメータを使用している。
【００１８】図１はこの発明の実施の形態に係る装置の
ブロック図である。センサであるレートジャイロ１から
状態量（角速度）ω（ｔ）を受けて、カルマンフィルタ
３はシステム方程式と観測方程式を生成するとともに、
次式に従い時間更新処理及び観測更新処理を行う。な
お、システム方程式及び観測方程式については後述す
る。
【００１９】
【数６】
【００２０】上記式において、下添字で示された＋はカ
ルマンフィルタによる観測更新処理後の変数であること
を示す。同じく、下添字で示されたｋはΔｔ時間間隔で
離散的に処理される計算式の実行がｋ番目であることを
示し、同様にｋ＋１はｋ＋１番目であることを示す。右
肩添字の−１は逆行列を示す。右肩添字のＴは転置行列
を示す。
【００２１】時間更新及び観測更新について簡単に説明
を加える。時間更新とは、線形モデル
【数７】 を積分する処理のことである。上記の場合は
【数８】 を積分する処理になる。これは計算により予測値を求め
ていく処理である。また、同時に誤差の共分散行列も更
新する。誤差の共分散行列に関する式は
【数９】 で表され、更新処理の際はこれを積分処理することにな
る。その結果、Ｐバー（ｔ）が求められる。
【００２２】観測更新とは、新しく得られた観測量と計
算予測値から新しい推定値を求める処理のことである。
上記の場合の式は
【数１０】 と表される。ハットの付いたものは推定値であり、バー
のついたものは予測値である。この処理は観測された値
と予測された観測値を比較することにより、そこに誤差
に関する情報を得、それに対してあるゲインを掛けて予
測値に対して付加し、それをもって推定値とするという
意味である。特に
【数１１】 の部分はイノベーション過程もしくはＯ−Ｃと呼ばれ、
観測値と計算予測された観測値を比較する過程である。
それによりどの程度、計算予測値と観測値が異なってい
るか比較する。この比較により誤差情報が得られるが、
その中には有益なものと意味のないものがある。それを
どのように分配して予測値に付加するのかを決定するの
が
【数１２】 と書かれているカルマンゲインと呼ばれるものである。
このカルマンゲインは
【数１３】 という式により求められる（この計算のために時間更新
でＰバー（ｔ）を求める必要があった）。なお、Ｒ（ｔ
k）は観測ノイズの共分散行列と呼ばれるものである。
また、誤差の共分散行列についてもカルマンゲインを用
いて
【数１４】 のように更新する。前述のような処理を行う「時間更
新」「観測更新」であるが、これらは新しい観測値によ
り観測更新が行われた後、次の新しい観測値が得られる
までの間、時間更新により「位置」「姿勢」などの状態
量を推定するために用いられる。
【００２３】処理部４はカルマンフィルタ３の出力に基
づきαドットハット（修正ロドリゲスパラメータα
（ｔ）の時間微分αドット（ｔ）の推定値）を生成す
る。積分処理部５は処理部４の出力に基づき修正ロドリ
ゲスパラメータの推定値αハット、α0ハットを生成す
る。
【００２４】システム伝播行列生成部６は初期値設定部
２の出力及び積分処理部５の出力に基づき観測感度行列
を更新して、その更新出力をカルマンフィルタ３に供給
する。システム伝播行列とは線形システム
【数１５】 のＦ（ｔ）のことである。今回の場合、後述の式（２．
３．５）の状態量αドットを偏微分することにより得ら
れる（後述の式（２．３．８）から式（２．３．１６）
がその成分に相当する）。これは状態量と状態量の微分
値を関係を示す行列である。観測感度行列とは
【数１６】 のＨ（ｔ）のことである。観測される値と状態量の関係
を示す行列である（後述の式（２．３．１）により求め
られる。ただし、解析的に解くことはできないので数値
的に演算処理を行う）。
【００２５】変換マトリクス生成部７は積分処理部５の
出力に基づき座標変換行列Ｒを生成する。姿勢推定部８
は変換マトリクス生成部７の出力に基づき姿勢推定を行
う。
【００２６】次に動作について説明する。まず本発明の
処理の概要について説明し、次に本発明の理論及びアル
ゴリズムの詳細について説明し、最後にシミュレーショ
ン結果について説明する。
【００２７】本発明の実施の形態において、公知の拡張
カルマンフィルタを利用する。拡張カルマンフィルタは
推定したい状態量の初期値を与えておき、それと観測量
の時間履歴を元に状態量の推定値を逐次計算していく手
法である。
【００２８】図２のその概略フローチャートを示す。 Ｓ１：レートジャイロ１から信号を受けて修正ロドリゲ
スパラメータα（ｔ）の時間微分αドット（ｔ）の状態
量、ω（ｔ）を観測量とするシステム方程式と観測方程
式を作成する。 Ｓ２：拡張カルマンフィルタを用いてαドット（ｔ）の
推定値を求める。 Ｓ３：この推定値を数値積分することによりα（ｔ）の
推定値を求める。 Ｓ４：この推定値に基づき変換マトリクスＲ（ｔ）を決
定する。 Ｓ５：変換マトリクスＲ（ｔ）に基づき時刻ｔにおける
姿勢の推定値を求める。
【００２９】修正ロドリゲスパラメータを用いることに
より、システム方程式と観測方程式を生成することがで
き、人工衛星の姿勢推定に拡張カルマンフィルタを適用
することができた。これによりドリフトの影響を軽減す
ることができ、レートジャイロのみにより高精度な姿勢
推定が可能になったのである。従来の手法ではシステム
方程式と観測方程式を生成することができず、人工衛星
の姿勢推定に拡張カルマンフィルタを適用することがで
きなかった。
【００３０】次に理論について説明する。 ［理論］ １．任意の軸周りのベクトルの回転 あるベクトルｘが任意の軸の周りを角度θだけ回転させ
た場合を考える。軸方向の単位ベクトルをｅ、回転後の
ベクトルをｘ'とする。ここで次の式が成立する。
【００３１】
【数１７】 ただし、Ｉは単位マトリックスを表す。また、
【００３２】
【数１８】 倍角、半角の公式より
【００３３】
【数１９】 ここで任意の軸回りθの回転をあらわすクォータニオン
ｑを
【００３４】
【数２０】 とすると、
【００３５】
【数２１】 となる。ただし、
【００３６】
【数２２】
【００３７】したがって、時刻ｔ0における軸の向きと
時刻ｔにおける変換マトリクスＲが分かれば、そのｔに
おける軸の向き、すなわち姿勢が分かる。
【００３８】２．システム方程式 ３次元空間の基底ベクトルをｃ1，ｃ2，ｃ3とし、この
３つのベクトルがある角速度ベクトル
【００３９】
【数２３】 で表される軸の周りを回転するとき、ｃ1，ｃ2，ｃ3の
時間積分について
【００４０】
【数２４】 なる関係が成り立つ.(2.2.2)式をマトリックス表現でひ
とつの式にまとめると
【００４１】
【数２５】 なるωハットを使って
【００４２】
【数２６】 となる。ただし、
【００４３】
【数２７】
【００４４】３．修正ロドリゲスパラメータの利用とそ
の一階微分方程式 以下のような修正ロドリゲスパラメータ：αとα0を定
義する。
【数２８】
【００４５】すなわち、αは３次元のベクトルである。
ｅは回転軸の方向余弦、θは回転軸回りの回転角を表
す。(2.2.31)式で定義した修正ロドリゲスパラメータを
用いると(2.1.4)式のクォータニオンの要素をαで表現
できる。αで表現されるａ、Ｖを使えば、(2.1.5)式の
変換マトリクスもαを使って表すことができる。
【００４６】さらに、αの１階微分方程式を作ることで
Ｒの時間履歴を求められる。αの１階微分方程式は高々
３本の連立方程式である。(2.2.32)式より
【００４７】
【数２９】 よって、
【００４８】
【数３０】 次にＶについて考える。
【００４９】
【数３１】 変換マトリクスＲは(2.2.34)、(2.2.35)式より
【００５０】
【数３２】 (2.2.34)、(2.2.35)式を用いてαの一階微分方程式を求
めると、次式のようになる。
【００５１】
【数３３】
【００５２】［アルゴリズムの詳細］ ４．拡張カルマンフィルタへの適用 本発明の実施の形態において、衛星の互いに直交する３
軸に取りつけられたレートジャイロから計測された角速
度ベクトルω（ｔ）を基に姿勢推定を行う。具体的に
は、(2.2.41)式から修正ロドリゲスパラメータα（ｔ）
の時間微分αドット（ｔ）の状態量、ω（ｔ）を観測量
とするシステム方程式と観測方程式を作成し、拡張カル
マンフィルタを用いてαドット（ｔ）の推定値を求め
る。これを数値積分することによりα（ｔ）の推定値を
求め、(2.2.36)式に代入して変換マトリクスＲ（ｔ）を
決定し、(2.1.5)式より時刻ｔにおける姿勢の推定値を
求める。
【００５３】この場合、(2.1.5)式の右辺のｘが人工衛
星の任意の固定軸をあらわす方向ベクトルの初期値、左
辺のｘ'がｔ秒後における固定軸の向きになる。ｘ'とｘ
のノルムは等しい。はじめに観測方程式を求める。(2.
2.41)式より
【００５４】
【数３４】 とおけば
【００５５】
【数３５】 これに観測ノイズを加えれば拡張カルマンフィルタにお
ける観測方程式となる。次にシステム方程式を求める。
(2.2.32)式より
【００５６】
【数３６】 (2.2.41)式を展開すると次のようになる。
【００５７】
【数３７】 ωの時間微分ｄω／ｄｔは数値的に意味のないものとし
て消去すると、(2.3.4)式の１階微分は以下のようにな
る。
【００５８】
【数３８】 上式の右辺をｆ(a(t),ａドット(t)，t)とおけば
【００５９】
【数３９】 となる。この式にシステムノイズを加えれば拡張カルマ
ンフィルタにおけるシステム方程式となる。(2.3.6)式
は非線型微分方程式なので、システム伝播行列を求める
ためには(2.3.5)式を状態量ベクトルで偏微分した行列
Ｆを求める必要がある。
【００６０】
【数４０】 (2.3.5)式よりＦの各要素は以下の通りである。
【００６１】
【数４１】
【００６２】［シミュレーション結果］図３にシミュレ
ーション結果を示す。図３は、観測ノイズ標準偏差＝３
×１０ −５［rad/sec］のときの機体軸の方向の推定誤
差を示す。図３によれば、はじめのうち誤差は時間に比
例して増加し、１．２×１０５［sec］で誤差は２．２
［deg］に達する。しかし、誤差はその後静定する。本
発明の実施の形態によれば、カルマンゲインは速やかな
に収束しており、良好に情報の復元を行うことができ
る。
【００６３】しかも、図３から明らかなように、本発明
の実施の形態によればドリフトは累積せず、誤差は一定
範囲内に収まる。これに対し、従来の手法ではドリフト
は累積し、誤差が一定範囲内に収まることはなかった。
この点で本発明の実施の形態の手法は非常に優れてい
る。
【００６４】本発明は、以上の実施の形態に限定される
ことなく、特許請求の範囲に記載された発明の範囲内
で、種々の変更が可能であり、それらも本発明の範囲内
に包含されるものであることは言うまでもない。
【００６５】本発明は、人工衛星のみでなく、ジャイロ
を用いて姿勢を推定するもの全般、すなわち慣性計測装
置を用いるものすべてに対して適用でき、精度の向上を
保証するものであり、その応用分野は広範囲にわたる。
例えば、本発明は、ビデオカメラの手ぶれ防止機能や、
カーナビゲーションシステムなどの姿勢推定などに適用
できる。
【００６６】また、本明細書において、手段とは必ずし
も物理的手段を意味するものではなく、各手段の機能
が、ソフトウェアによって実現される場合も包含する。
さらに、一つの手段の機能が、二つ以上の物理的手段に
より実現されても、若しくは、二つ以上の手段の機能
が、一つの物理的手段により実現されてもよい。
【００６７】
【発明の効果】以上のように、この発明によれば、慣性
計測装置の出力のみに基づき高精度な姿勢推定を実現で
きる。特に、従来問題となっていたドリフトを除去する
ことができる。
【図面の簡単な説明】
【図１】 この発明の実施の形態に係る装置のブロック
図である。
【図２】 この発明の実施の形態に係る処理フローチャ
ートである。
【図３】 シミュレーション結果の例である。
【符号の説明】
１ レートジャイロ ２ 初期値設定部 ３ カルマンフィルタ ４ 修正ロドリゲスパラメータαの時間微分の推定値
の生成部 ５ 積分処理部 ６ システム伝播行列生成部 ７ 変換マトリクス生成部 ８ 姿勢推定部
───────────────────────────────────────────────────── フロントページの続き (72)発明者 笹島 雄一郎 東京都千代田区九段南四丁目８番24号 学 校法人 日本大学内 Ｆターム(参考） 2F105 AA05 BB01

Claims (6)

    【特許請求の範囲】
  1. 【請求項１】 慣性計測装置から状態量ω（ｔ）を受け
    て予め定められたシステム方程式及び観測方程式に入力
    するとともに、これらに関して時間更新処理及び観測更
    新処理を行うカルマンフィルタと、前記カルマンフィル
    タの出力に基づき修正ロドリゲスパラメータα（ｔ）の
    時間微分の推定値を生成する処理部と、前記処理部の出
    力に基づき修正ロドリゲスパラメータの推定値を生成す
    る積分処理部と、予め与えられた初期値及び前記積分処
    理部の出力に基づき観測感度行列を更新するとともに、
    その更新出力を前記カルマンフィルタに供給するシステ
    ム伝播行列生成部と、前記積分処理部の出力に基づき座
    標変換行列Ｒを生成する変換マトリクス生成部と、前記
    変換マトリクス生成部の出力に基づき姿勢推定を行う姿
    勢推定部とを備える慣性計測装置を用いた姿勢推定装
    置。
  2. 【請求項２】 前記修正ロドリゲスパラメータは、ｅを
    回転軸の方向余弦、θを回転軸回りの回転角とすると
    き、 【数１】 で与えられることを特徴とする請求項１記載の慣性計測
    装置を用いた姿勢推定装置。
  3. 【請求項３】 前記観測方程式は、 【数２】 とおいたとき、 【数３】 に観測ノイズを加えることにより与えられることを特徴
    とする請求項１記載の慣性計測装置を用いた姿勢推定装
    置。
  4. 【請求項４】 前記システム方程式は， 【数４】 上式の右辺をｆ(a(t),ａドット(t)，t)とおいたときの 【数５】 にシステムノイズを加えることにより与えられることを
    特徴とする請求項１記載の慣性計測装置を用いた姿勢推
    定装置。
  5. 【請求項５】 慣性計測装置から信号を受けて修正ロド
    リゲスパラメータα（ｔ）の時間微分の状態量と状態量
    ω（ｔ）を観測量とするシステム方程式及び観測方程式
    を生成する第１ステップと、 拡張カルマンフィルタを用いて前記パラメータα（ｔ）
    の時間微分の推定値を求める第２ステップと、 前記第２ステップで求められた推定値を数値積分して前
    記パラメータα（ｔ）の推定値を求める第３ステップ
    と、 前記第３ステップで求められた推定値に基づき変換マト
    リクスＲ（ｔ）を決定する第４ステップと、 前記変換マトリクスＲ（ｔ）に基づき姿勢の推定値を求
    めるステップとを備える慣性計測装置を用いた姿勢推定
    方法。
  6. 【請求項６】 コンピュータに、 慣性計測装置から信号を受けて修正ロドリゲスパラメー
    タα（ｔ）の時間微分の状態量と状態量ω（ｔ）を観測
    量とするシステム方程式及び観測方程式を生成する第１
    ステップと、 拡張カルマンフィルタを用いて前記パラメータα（ｔ）
    の時間微分の推定値を求める第２ステップと、 前記第２ステップで求められた推定値を数値積分して前
    記パラメータα（ｔ）の推定値を求める第３ステップ
    と、 前記第３ステップで求められた推定値に基づき変換マト
    リクスＲ（ｔ）を決定する第４ステップと、 前記変換マトリクスＲ（ｔ）に基づき姿勢の推定値を求
    めるステップとを実行させるためのプログラム。
JP2001128261A 2001-04-25 2001-04-25 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム Expired - Fee Related JP3726884B2 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2001128261A JP3726884B2 (ja) 2001-04-25 2001-04-25 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
EP02724638A EP1382936A4 (en) 2001-04-25 2002-04-24 DEVICE AND METHOD FOR LAYING ESTIMATION USING VULNERABILITY MEASURING DEVICES AND PROGRAM
PCT/JP2002/004103 WO2002088633A1 (fr) 2001-04-25 2002-04-24 Appareil et procede d'estimation d'attitude utilisant un materiel et un programme de mesure inertielle
US10/476,402 US7248948B2 (en) 2001-04-25 2002-04-24 Apparatus and method for estimating attitude using inertial measurement equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2001128261A JP3726884B2 (ja) 2001-04-25 2001-04-25 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム

Publications (2)

Publication Number Publication Date
JP2002323322A true JP2002323322A (ja) 2002-11-08
JP3726884B2 JP3726884B2 (ja) 2005-12-14

Family

ID=18976988

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2001128261A Expired - Fee Related JP3726884B2 (ja) 2001-04-25 2001-04-25 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム

Country Status (4)

Country Link
US (1) US7248948B2 (ja)
EP (1) EP1382936A4 (ja)
JP (1) JP3726884B2 (ja)
WO (1) WO2002088633A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011017688A (ja) * 2009-07-09 2011-01-27 Jiaotong Univ 角度測定の方法およびそれを実施するための角度測定ジャイロシステム
CN115507845A (zh) * 2022-10-18 2022-12-23 闽江学院 一种顾及虚拟观测值的卫星姿态融合估计方法

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7454290B2 (en) * 2003-09-18 2008-11-18 The Board Of Trustees Of The Leland Stanford Junior University GPS/INS vehicle attitude system
CN101074881B (zh) * 2007-07-24 2011-04-27 北京控制工程研究所 一种月球探测器软着陆阶段惯性导航方法
US8321076B2 (en) * 2009-12-18 2012-11-27 The Boeing Company On-line inertia estimation for use in controlling an aerospace vehicle
JP2013064695A (ja) * 2011-09-20 2013-04-11 Yamaha Corp 状態推定装置、オフセット更新方法およびオフセット更新プログラム
CN103884340B (zh) * 2014-03-31 2016-08-17 北京控制工程研究所 一种深空探测定点软着陆过程的信息融合导航方法
CN105300379B (zh) * 2015-10-13 2017-12-12 上海新纪元机器人有限公司 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN105253330B (zh) * 2015-10-30 2017-04-05 中国空间技术研究院 一种基于优化的信息融合geo卫星控制系统菜单式设计方法
CN106014570A (zh) * 2016-05-09 2016-10-12 黄安武 处理汽车尾气中一氧化碳的方法及系统
CN106527457B (zh) * 2016-11-17 2019-04-19 天津津航技术物理研究所 航空扫描仪扫描控制指令规划方法
US20210404811A1 (en) * 2017-04-21 2021-12-30 Shanghai Jiao Tong University Method for solving attitude of rigid body based on function iterative integration
CN107738254B (zh) * 2017-08-25 2019-12-24 中国科学院光电研究院 一种机械臂坐标系的转换标定方法与系统
CN107831775B (zh) * 2017-11-14 2021-06-08 哈尔滨工业大学深圳研究生院 基于挠性航天器无角速度测量的姿态控制方法
CN108536164B (zh) * 2018-03-23 2021-10-08 哈尔滨工业大学深圳研究生院 挠性航天器无角速度测量的姿态控制方法
CN110598370B (zh) * 2019-10-18 2023-04-14 太原理工大学 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
CN112527119B (zh) * 2020-12-22 2022-05-27 南京航空航天大学 一种手势位姿数据处理方法及存储介质
CN113029138B (zh) * 2021-04-02 2022-09-06 扬州大学 一种基于多传感器数据融合的小车实时姿态检测方法

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06229722A (ja) * 1993-02-01 1994-08-19 Kinoshita Denshi Kogyo Kk 光学的測長法及び光学的測長器
JPH06229772A (ja) 1993-02-04 1994-08-19 Hitachi Ltd 積分型センシング装置
US5543804A (en) * 1994-09-13 1996-08-06 Litton Systems, Inc. Navagation apparatus with improved attitude determination
JPH0914962A (ja) * 1995-06-28 1997-01-17 Komatsu Ltd 建設車両の位置計測装置
US5757316A (en) * 1997-02-01 1998-05-26 Litton Systems, Inc. Attitude determination utilizing an inertial measurement unit and a plurality of satellite transmitters
JPH11129997A (ja) * 1997-11-04 1999-05-18 Mitsubishi Electric Corp 人工衛星の姿勢制御装置
US6205400B1 (en) * 1998-11-27 2001-03-20 Ching-Fang Lin Vehicle positioning and data integrating method and system thereof
US6298288B1 (en) * 1998-12-16 2001-10-02 Hughes Electronics Corp. Autonomous gyro scale factor and misalignment calibration
US6272432B1 (en) * 1999-05-10 2001-08-07 Hughes Electronics Corporation System and method for correcting star tracker low spatial frequency error in stellar-inertial attitude determination systems
AU775676B2 (en) * 2000-06-01 2004-08-12 Furuno Electric Company, Limited System for determining the heading and/or attitude of a body
US6496778B1 (en) * 2000-09-14 2002-12-17 American Gnc Corporation Real-time integrated vehicle positioning method and system with differential GPS
US6427122B1 (en) * 2000-12-23 2002-07-30 American Gnc Corporation Positioning and data integrating method and system thereof
US6424914B1 (en) * 2000-12-26 2002-07-23 American Gnc Corporation Fully-coupled vehicle positioning method and system thereof
GB2391732B (en) * 2002-05-16 2005-09-07 Furuno Electric Co Attitude sensing apparatus for determining the attitude of a mobile unit
US7193559B2 (en) * 2003-01-21 2007-03-20 Novatel, Inc. Inertial GPS navigation system with modified kalman filter

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011017688A (ja) * 2009-07-09 2011-01-27 Jiaotong Univ 角度測定の方法およびそれを実施するための角度測定ジャイロシステム
CN115507845A (zh) * 2022-10-18 2022-12-23 闽江学院 一种顾及虚拟观测值的卫星姿态融合估计方法

Also Published As

Publication number Publication date
EP1382936A4 (en) 2005-11-02
JP3726884B2 (ja) 2005-12-14
US20040176881A1 (en) 2004-09-09
US7248948B2 (en) 2007-07-24
EP1382936A1 (en) 2004-01-21
WO2002088633A1 (fr) 2002-11-07

Similar Documents

Publication Publication Date Title
JP3726884B2 (ja) 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
Park et al. A scheme for improving the performance of a gyroscope-free inertial measurement unit
JP6255924B2 (ja) センサー用ｉｃ、センサーデバイス、電子機器及び移動体
US10788324B2 (en) Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
EP2340998B1 (en) On-line inertia estimation for use in controlling an aerospace vehicle
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
Stovner et al. Attitude estimation by multiplicative exogenous Kalman filter
US20100114517A1 (en) Method and system for orientation sensing
CA2477677C (en) Autonomous velocity estimation and navigation
JP7025215B2 (ja) 測位システム及び測位方法
KR19980042029A (ko) 스트랩다운 관성 항법 시스템에서의 스컬링 보정을 위한 방법 및 장치
CN113432612B (zh) 飞行体的导航方法、装置及系统
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
US10883831B2 (en) Performance of inertial sensing systems using dynamic stability compensation
JP2015094631A (ja) 位置算出装置及び位置算出方法
JP2006038650A (ja) 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム
Kim et al. Drift error analysis caused by RLG dither axis bending
Oh et al. Development of UAV navigation system based on unscented Kalman filter
CN113959433B (zh) 一种组合导航方法及装置
Damerius et al. A generic inertial navigation system
US20220250773A1 (en) Device and method for determining the attitude of a satellite equipped with gyroscopic actuators, and satellite carrying such a device
CN114353825B (zh) 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统
KR20210000095A (ko) 자세 추정 방법 및 장치

Legal Events

Date Code Title Description
TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20050908

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20050920

R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20081007

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20091007

Year of fee payment: 4

LAPS Cancellation because of no payment of annual fees