JP3726884B2 - 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム - Google Patents
慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム Download PDFInfo
- Publication number
- JP3726884B2 JP3726884B2 JP2001128261A JP2001128261A JP3726884B2 JP 3726884 B2 JP3726884 B2 JP 3726884B2 JP 2001128261 A JP2001128261 A JP 2001128261A JP 2001128261 A JP2001128261 A JP 2001128261A JP 3726884 B2 JP3726884 B2 JP 3726884B2
- Authority
- JP
- Japan
- Prior art keywords
- estimated value
- equation
- observation
- inertial measurement
- parameter
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims description 22
- 238000005259 measurement Methods 0.000 title claims description 18
- 239000011159 matrix material Substances 0.000 claims description 44
- 238000012545 processing Methods 0.000 claims description 22
- 230000014509 gene expression Effects 0.000 claims description 21
- 238000006243 chemical reaction Methods 0.000 claims description 19
- 230000010354 integration Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 6
- 230000035945 sensitivity Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 10
- 239000013598 vector Substances 0.000 description 10
- 230000006870 function Effects 0.000 description 8
- 238000004891 communication Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 230000004069 differentiation Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- OVSKIKFHRZPJSS-UHFFFAOYSA-N 2,4-D Chemical compound OC(=O)COC1=CC=C(Cl)C=C1Cl OVSKIKFHRZPJSS-UHFFFAOYSA-N 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
Images
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)
Description
【0001】
【発明の属する技術分野】
この発明は、慣性計測装置からの信号に基づき姿勢を推定するための姿勢推定装置及び方法並びにプログラムに関する。
【0002】
【従来の技術】
近年、宇宙開発の分野において大型で多機能な人工衛星の他に、単機能ではあるが開発の容易なマイクロサット(超小型人工衛星)が注目されている。これらマイクロサットの開発が容易であることは人工衛星の単価を引き下げることと相俟って、宇宙開発をより身近なものにすると考えられる。現在、ピギーバック衛星として多くのマイクロサットが打ち上げられている。
【0003】
単機能・安価であることを身上とするマイクロサットであるが、その高性能化には3軸制御が不可欠である。そのためには高性能な姿勢観測系が必要となるが、衛星の開発費の高騰を考えればあまり高価な機材を使用することができない。
【0004】
従来、3軸衛星の姿勢センサとして各種センサが用いられている。例えば、慣性基準装置などのジャイロから姿勢角速度を検出し、これを積分することによりロール、ヨーを含めて正確な姿勢推定を行うことが知られている。慣性基準装置は、レートジャイロなどを3軸に搭載し、パッケージにしたものである。ジャイロの信号を直接積分して使うと、動的な応答のよい検出ができるメリットがあるが、反面、長時間ではドリフトが累積してくるという問題がある。それを補正するために、スタートラッカなどを用い、絶対基準からの誤差を検出系にフィードバックすることが行われている。しかし、この手法は高価なセンサを必要とし、単機能・安価であるマイクロサットに適用できない。
【0005】
【発明が解決しようとする課題】
本発明は係る課題に鑑みてなされたもので、精度がよくかつ安価である姿勢推定装置及び方法、すなわち、比較的単価の低い圧電振動ジャイロやファイバージャイロ等の機材を用いて人工衛星の姿勢推定を行うことができる慣性計測装置を用いた姿勢推定装置及び方法並びにプログラムを提供することを目的とする。
【0006】
この発明によれば、スタートラッカなどのセンサを必要とすることなく、ジャイロからの情報のみで姿勢の推定を行うことができる。このような状況は従来の人工衛星においても生じる。例えば、実際の運用下において人工衛星がマニューバ中でスタートラッカなどのセンサを使用できない状態にあるときなどに対応する。
【0007】
【課題を解決するための手段】
この発明に係る慣性計測装置を用いた姿勢推定装置は、慣性計測装置から状態量ω(t)を受けて予め定められたシステム方程式及び観測方程式に入力するとともに、これらに関して時間更新処理及び観測更新処理を行うカルマンフィルタと、前記カルマンフィルタの出力に基づき修正ロドリゲスパラメータα(t)の時間微分の推定値を生成する処理部と、前記処理部の出力に基づき修正ロドリゲスパラメータの推定値を生成する積分処理部と、予め与えられた初期値及び前記積分処理部の出力に基づき観測感度行列を更新するとともに、その更新出力を前記カルマンフィルタに供給するシステム伝播行列生成部と、前記積分処理部の出力に基づき座標変換行列Rを生成する変換マトリクス生成部と、前記変換マトリクス生成部の出力に基づき姿勢推定を行う姿勢推定部とを備えるものである。
【0008】
好ましくは、前記修正ロドリゲスパラメータは、eを回転軸の方向余弦、θを回転軸回りの回転角とするとき、後述の式(2.2.31)(2.2.32)で与えられる。
【0009】
好ましくは、前記観測方程式は、後述の式(2.3.1)としたとき、式(2.3.2)に観測ノイズを加えることにより与えられる。
【0010】
好ましくは、前記システム方程式は、後述の式(2.3.5)の右辺をf(a(t),aドット(t),t)とおいたときの後述の式(2.3.6)にシステムノイズを加えることにより与えられる。
【0011】
この発明に係る慣性計測装置を用いた姿勢推定方法は、慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを備えるものである。
【0012】
この発明に係るプログラムは、コンピュータに、
慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを実行させるためのものである。
【0013】
この発明に係るプログラムは、例えば、記録媒体に記録される。
媒体には、例えば、フレキシブルディスク、ハードディスク、磁気テープ、光磁気ディスク、CD(CD−ROM、Video−CDを含む)、DVD(DVD−Video、DVD−ROM、DVD−RAMを含む)、ROMカートリッジ、バッテリバックアップ付きのRAMメモリカートリッジ、フラッシュメモリカートリッジ、不揮発性RAMカートリッジ等を含む。
【0014】
また、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等の通信媒体を含む。インターネットもここでいう通信媒体に含まれる。
【0015】
媒体とは、何等かの物理的手段により情報(主にデジタルデータ、プログラム)が記録されているものであって、コンピュータ、専用プロセッサ等の処理装置に所定の機能を行わせることができるものである。要するに、何等かの手段でもってコンピュータにプログラムをダウンロードし、所定の機能を実行させるものであればよい。
【0016】
本発明は、修正ロドリゲスパラメータを拡張カルマンフィルタへ適用したものである。
姿勢の伝播式としてはクォータニオンを用いた変換式が一般に知られている。クォータニオンはオイラー角と異なり、姿勢に対して特異点が存在せず、超越関数を使用しないなどの利点が存在し、一般に衛星の姿勢の記述にはこれが用いられている。しかし、これをジャイロからのみの情報に対して推定を行うとすると、推定アルゴリズムであるカルマンフィルタに必要な観測方程式が成立しない。カルマンフィルタによる推定を行うためには必要な方程式、すなわちシステム方程式と観測方程式とを成立させる必要がある。そこで、本発明においてクォータニオンの応用である修正ロドリゲスパラメータを用いることにより、それらの式を成立させることに成功したのである。
【0017】
【発明の実施の形態】
この発明の実施の形態に係る装置/方法について図面を参照して説明する。以下の説明は、本発明の実施の形態に係る装置/方法を人工衛星の姿勢推定に用いる場合を例にとったものである。この発明の実施の形態に係る装置/方法は、人工衛星の姿勢情報をジャイロからの出力のみにより長時間、精度良く推定するものである。人工衛星の姿勢決定方程式として、修正ロドリゲスパラメータを使用している。
【0018】
図1はこの発明の実施の形態に係る装置のブロック図である。センサであるレートジャイロ1から状態量(角速度)ω(t)を受けて、カルマンフィルタ3はシステム方程式と観測方程式を生成するとともに、次式に従い時間更新処理及び観測更新処理を行う。なお、システム方程式及び観測方程式については後述する。
【0019】
【数6】
【0020】
上記式において、下添字で示された+はカルマンフィルタによる観測更新処理後の変数であることを示す。同じく、下添字で示されたkはΔt時間間隔で離散的に処理される計算式の実行がk番目であることを示し、同様にk+1はk+1番目であることを示す。右肩添字の−1は逆行列を示す。右肩添字のTは転置行列を示す。
【0021】
時間更新及び観測更新について簡単に説明を加える。時間更新とは、線形モデル
【数7】
を積分する処理のことである。上記の場合は
【数8】
を積分する処理になる。これは計算により予測値を求めていく処理である。また、同時に誤差の共分散行列も更新する。誤差の共分散行列に関する式は
【数9】
で表され、更新処理の際はこれを積分処理することになる。その結果、Pバー(t)が求められる。
【0022】
観測更新とは、新しく得られた観測量と計算予測値から新しい推定値を求める処理のことである。上記の場合の式は
【数10】
と表される。ハットの付いたものは推定値であり、バーのついたものは予測値である。この処理は観測された値と予測された観測値を比較することにより、そこに誤差に関する情報を得、それに対してあるゲインを掛けて予測値に対して付加し、それをもって推定値とするという意味である。特に
【数11】
の部分はイノベーション過程もしくはO−Cと呼ばれ、観測値と計算予測された観測値を比較する過程である。それによりどの程度、計算予測値と観測値が異なっているか比較する。この比較により誤差情報が得られるが、その中には有益なものと意味のないものがある。それをどのように分配して予測値に付加するのかを決定するのが
【数12】
と書かれているカルマンゲインと呼ばれるものである。
このカルマンゲインは
【数13】
という式により求められる(この計算のために時間更新でPバー(t)を求める必要があった)。なお、R(tk)は観測ノイズの共分散行列と呼ばれるものである。
また、誤差の共分散行列についてもカルマンゲインを用いて
【数14】
のように更新する。
前述のような処理を行う「時間更新」「観測更新」であるが、これらは新しい観測値により観測更新が行われた後、次の新しい観測値が得られるまでの間、時間更新により「位置」「姿勢」などの状態量を推定するために用いられる。
【0023】
処理部4はカルマンフィルタ3の出力に基づきαドットハット(修正ロドリゲスパラメータα(t)の時間微分αドット(t)の推定値)を生成する。
積分処理部5は処理部4の出力に基づき修正ロドリゲスパラメータの推定値αハット、α0ハットを生成する。
【0024】
システム伝播行列生成部6は初期値設定部2の出力及び積分処理部5の出力に基づき観測感度行列を更新して、その更新出力をカルマンフィルタ3に供給する。システム伝播行列とは線形システム
【数15】
のF(t)のことである。今回の場合、後述の式(2.3.5)の状態量αドットを偏微分することにより得られる(後述の式(2.3.8)から式(2.3.16)がその成分に相当する)。これは状態量と状態量の微分値を関係を示す行列である。観測感度行列とは
【数16】
のH(t)のことである。観測される値と状態量の関係を示す行列である(後述の式(2.3.1)により求められる。ただし、解析的に解くことはできないので数値的に演算処理を行う)。
【0025】
変換マトリクス生成部7は積分処理部5の出力に基づき座標変換行列Rを生成する。
姿勢推定部8は変換マトリクス生成部7の出力に基づき姿勢推定を行う。
【0026】
次に動作について説明する。まず本発明の処理の概要について説明し、次に本発明の理論及びアルゴリズムの詳細について説明し、最後にシミュレーション結果について説明する。
【0027】
本発明の実施の形態において、公知の拡張カルマンフィルタを利用する。拡張カルマンフィルタは推定したい状態量の初期値を与えておき、それと観測量の時間履歴を元に状態量の推定値を逐次計算していく手法である。
【0028】
図2のその概略フローチャートを示す。
S1:レートジャイロ1から信号を受けて修正ロドリゲスパラメータα(t)の時間微分αドット(t)の状態量、ω(t)を観測量とするシステム方程式と観測方程式を作成する。
S2:拡張カルマンフィルタを用いてαドット(t)の推定値を求める。
S3:この推定値を数値積分することによりα(t)の推定値を求める。
S4:この推定値に基づき変換マトリクスR(t)を決定する。
S5:変換マトリクスR(t)に基づき時刻tにおける姿勢の推定値を求める。
【0029】
修正ロドリゲスパラメータを用いることにより、システム方程式と観測方程式を生成することができ、人工衛星の姿勢推定に拡張カルマンフィルタを適用することができた。これによりドリフトの影響を軽減することができ、レートジャイロのみにより高精度な姿勢推定が可能になったのである。従来の手法ではシステム方程式と観測方程式を生成することができず、人工衛星の姿勢推定に拡張カルマンフィルタを適用することができなかった。
【0030】
次に理論について説明する。
[理論]
1.任意の軸周りのベクトルの回転
あるベクトルxが任意の軸の周りを角度θだけ回転させた場合を考える。軸方向の単位ベクトルをe、回転後のベクトルをx'とする。ここで次の式が成立する。
【0031】
【数17】
ただし、Iは単位マトリックスを表す。また、
【0032】
【数18】
倍角、半角の公式より
【0033】
【数19】
ここで任意の軸回りθの回転をあらわすクォータニオンqを
【0034】
【数20】
とすると、
【0035】
【数21】
となる。ただし、
【0036】
【数22】
【0037】
したがって、時刻t0における軸の向きと時刻tにおける変換マトリクスRが分かれば、そのtにおける軸の向き、すなわち姿勢が分かる。
【0038】
2.システム方程式
3次元空間の基底ベクトルをc1,c2,c3とし、この3つのベクトルがある角速度ベクトル
【0039】
【数23】
で表される軸の周りを回転するとき、c1,c2,c3の時間積分について
【0040】
【数24】
なる関係が成り立つ.
(2.2.2)式をマトリックス表現でひとつの式にまとめると
【0041】
【数25】
なるωハットを使って
【0042】
【数26】
となる。ただし、
【0043】
【数27】
【0044】
3.修正ロドリゲスパラメータの利用とその一階微分方程式
以下のような修正ロドリゲスパラメータ:αとα0を定義する。
【数28】
【0045】
すなわち、αは3次元のベクトルである。eは回転軸の方向余弦、θは回転軸回りの回転角を表す。(2.2.31)式で定義した修正ロドリゲスパラメータを用いると(2.1.4)式のクォータニオンの要素をαで表現できる。αで表現されるa、Vを使えば、(2.1.5)式の変換マトリクスもαを使って表すことができる。
【0046】
さらに、αの1階微分方程式を作ることでRの時間履歴を求められる。αの1階微分方程式は高々3本の連立方程式である。
(2.2.32)式より
【0047】
【数29】
よって、
【0048】
【数30】
次にVについて考える。
【0049】
【数31】
変換マトリクスRは(2.2.34)、(2.2.35)式より
【0050】
【数32】
(2.2.34)、(2.2.35)式を用いてαの一階微分方程式を求めると、次式のようになる。
【0051】
【数33】
【0052】
[アルゴリズムの詳細]
4.拡張カルマンフィルタへの適用
本発明の実施の形態において、衛星の互いに直交する3軸に取りつけられたレートジャイロから計測された角速度ベクトルω(t)を基に姿勢推定を行う。
具体的には、(2.2.41)式から修正ロドリゲスパラメータα(t)の時間微分αドット(t)の状態量、ω(t)を観測量とするシステム方程式と観測方程式を作成し、拡張カルマンフィルタを用いてαドット(t)の推定値を求める。これを数値積分することによりα(t)の推定値を求め、(2.2.36)式に代入して変換マトリクスR(t)を決定し、(2.1.5)式より時刻tにおける姿勢の推定値を求める。
【0053】
この場合、(2.1.5)式の右辺のxが人工衛星の任意の固定軸をあらわす方向ベクトルの初期値、左辺のx'がt秒後における固定軸の向きになる。x'とxのノルムは等しい。
はじめに観測方程式を求める。(2.2.41)式より
【0054】
【数34】
とおけば
【0055】
【数35】
これに観測ノイズを加えれば拡張カルマンフィルタにおける観測方程式となる。
次にシステム方程式を求める。(2.2.32)式より
【0056】
【数36】
(2.2.41)式を展開すると次のようになる。
【0057】
【数37】
ωの時間微分dω/dtは数値的に意味のないものとして消去すると、(2.3.4)式の1階微分は以下のようになる。
【0058】
【数38】
上式の右辺をf(a(t),aドット(t),t)とおけば
【0059】
【数39】
となる。この式にシステムノイズを加えれば拡張カルマンフィルタにおけるシステム方程式となる。
(2.3.6)式は非線型微分方程式なので、システム伝播行列を求めるためには(2.3.5)式を状態量ベクトルで偏微分した行列Fを求める必要がある。
【0060】
【数40】
(2.3.5)式よりFの各要素は以下の通りである。
【0061】
【数41】
【0062】
[シミュレーション結果]
図3にシミュレーション結果を示す。図3は、観測ノイズ標準偏差=3×10−5[rad/sec]のときの機体軸の方向の推定誤差を示す。図3によれば、はじめのうち誤差は時間に比例して増加し、1.2×105[sec]で誤差は2.2[deg]に達する。しかし、誤差はその後静定する。本発明の実施の形態によれば、カルマンゲインは速やかなに収束しており、良好に情報の復元を行うことができる。
【0063】
しかも、図3から明らかなように、本発明の実施の形態によればドリフトは累積せず、誤差は一定範囲内に収まる。これに対し、従来の手法ではドリフトは累積し、誤差が一定範囲内に収まることはなかった。この点で本発明の実施の形態の手法は非常に優れている。
【0064】
本発明は、以上の実施の形態に限定されることなく、特許請求の範囲に記載された発明の範囲内で、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。
【0065】
本発明は、人工衛星のみでなく、ジャイロを用いて姿勢を推定するもの全般、すなわち慣性計測装置を用いるものすべてに対して適用でき、精度の向上を保証するものであり、その応用分野は広範囲にわたる。例えば、本発明は、ビデオカメラの手ぶれ防止機能や、カーナビゲーションシステムなどの姿勢推定などに適用できる。
【0066】
また、本明細書において、手段とは必ずしも物理的手段を意味するものではなく、各手段の機能が、ソフトウェアによって実現される場合も包含する。さらに、一つの手段の機能が、二つ以上の物理的手段により実現されても、若しくは、二つ以上の手段の機能が、一つの物理的手段により実現されてもよい。
【0067】
【発明の効果】
以上のように、この発明によれば、慣性計測装置の出力のみに基づき高精度な姿勢推定を実現できる。特に、従来問題となっていたドリフトを除去することができる。
【図面の簡単な説明】
【図1】 この発明の実施の形態に係る装置のブロック図である。
【図2】 この発明の実施の形態に係る処理フローチャートである。
【図3】 シミュレーション結果の例である。
【符号の説明】
1 レートジャイロ
2 初期値設定部
3 カルマンフィルタ
4 修正ロドリゲスパラメータαの時間微分の推定値の生成部
5 積分処理部
6 システム伝播行列生成部
7 変換マトリクス生成部
8 姿勢推定部
【発明の属する技術分野】
この発明は、慣性計測装置からの信号に基づき姿勢を推定するための姿勢推定装置及び方法並びにプログラムに関する。
【0002】
【従来の技術】
近年、宇宙開発の分野において大型で多機能な人工衛星の他に、単機能ではあるが開発の容易なマイクロサット(超小型人工衛星)が注目されている。これらマイクロサットの開発が容易であることは人工衛星の単価を引き下げることと相俟って、宇宙開発をより身近なものにすると考えられる。現在、ピギーバック衛星として多くのマイクロサットが打ち上げられている。
【0003】
単機能・安価であることを身上とするマイクロサットであるが、その高性能化には3軸制御が不可欠である。そのためには高性能な姿勢観測系が必要となるが、衛星の開発費の高騰を考えればあまり高価な機材を使用することができない。
【0004】
従来、3軸衛星の姿勢センサとして各種センサが用いられている。例えば、慣性基準装置などのジャイロから姿勢角速度を検出し、これを積分することによりロール、ヨーを含めて正確な姿勢推定を行うことが知られている。慣性基準装置は、レートジャイロなどを3軸に搭載し、パッケージにしたものである。ジャイロの信号を直接積分して使うと、動的な応答のよい検出ができるメリットがあるが、反面、長時間ではドリフトが累積してくるという問題がある。それを補正するために、スタートラッカなどを用い、絶対基準からの誤差を検出系にフィードバックすることが行われている。しかし、この手法は高価なセンサを必要とし、単機能・安価であるマイクロサットに適用できない。
【0005】
【発明が解決しようとする課題】
本発明は係る課題に鑑みてなされたもので、精度がよくかつ安価である姿勢推定装置及び方法、すなわち、比較的単価の低い圧電振動ジャイロやファイバージャイロ等の機材を用いて人工衛星の姿勢推定を行うことができる慣性計測装置を用いた姿勢推定装置及び方法並びにプログラムを提供することを目的とする。
【0006】
この発明によれば、スタートラッカなどのセンサを必要とすることなく、ジャイロからの情報のみで姿勢の推定を行うことができる。このような状況は従来の人工衛星においても生じる。例えば、実際の運用下において人工衛星がマニューバ中でスタートラッカなどのセンサを使用できない状態にあるときなどに対応する。
【0007】
【課題を解決するための手段】
この発明に係る慣性計測装置を用いた姿勢推定装置は、慣性計測装置から状態量ω(t)を受けて予め定められたシステム方程式及び観測方程式に入力するとともに、これらに関して時間更新処理及び観測更新処理を行うカルマンフィルタと、前記カルマンフィルタの出力に基づき修正ロドリゲスパラメータα(t)の時間微分の推定値を生成する処理部と、前記処理部の出力に基づき修正ロドリゲスパラメータの推定値を生成する積分処理部と、予め与えられた初期値及び前記積分処理部の出力に基づき観測感度行列を更新するとともに、その更新出力を前記カルマンフィルタに供給するシステム伝播行列生成部と、前記積分処理部の出力に基づき座標変換行列Rを生成する変換マトリクス生成部と、前記変換マトリクス生成部の出力に基づき姿勢推定を行う姿勢推定部とを備えるものである。
【0008】
好ましくは、前記修正ロドリゲスパラメータは、eを回転軸の方向余弦、θを回転軸回りの回転角とするとき、後述の式(2.2.31)(2.2.32)で与えられる。
【0009】
好ましくは、前記観測方程式は、後述の式(2.3.1)としたとき、式(2.3.2)に観測ノイズを加えることにより与えられる。
【0010】
好ましくは、前記システム方程式は、後述の式(2.3.5)の右辺をf(a(t),aドット(t),t)とおいたときの後述の式(2.3.6)にシステムノイズを加えることにより与えられる。
【0011】
この発明に係る慣性計測装置を用いた姿勢推定方法は、慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを備えるものである。
【0012】
この発明に係るプログラムは、コンピュータに、
慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを実行させるためのものである。
【0013】
この発明に係るプログラムは、例えば、記録媒体に記録される。
媒体には、例えば、フレキシブルディスク、ハードディスク、磁気テープ、光磁気ディスク、CD(CD−ROM、Video−CDを含む)、DVD(DVD−Video、DVD−ROM、DVD−RAMを含む)、ROMカートリッジ、バッテリバックアップ付きのRAMメモリカートリッジ、フラッシュメモリカートリッジ、不揮発性RAMカートリッジ等を含む。
【0014】
また、電話回線等の有線通信媒体、マイクロ波回線等の無線通信媒体等の通信媒体を含む。インターネットもここでいう通信媒体に含まれる。
【0015】
媒体とは、何等かの物理的手段により情報(主にデジタルデータ、プログラム)が記録されているものであって、コンピュータ、専用プロセッサ等の処理装置に所定の機能を行わせることができるものである。要するに、何等かの手段でもってコンピュータにプログラムをダウンロードし、所定の機能を実行させるものであればよい。
【0016】
本発明は、修正ロドリゲスパラメータを拡張カルマンフィルタへ適用したものである。
姿勢の伝播式としてはクォータニオンを用いた変換式が一般に知られている。クォータニオンはオイラー角と異なり、姿勢に対して特異点が存在せず、超越関数を使用しないなどの利点が存在し、一般に衛星の姿勢の記述にはこれが用いられている。しかし、これをジャイロからのみの情報に対して推定を行うとすると、推定アルゴリズムであるカルマンフィルタに必要な観測方程式が成立しない。カルマンフィルタによる推定を行うためには必要な方程式、すなわちシステム方程式と観測方程式とを成立させる必要がある。そこで、本発明においてクォータニオンの応用である修正ロドリゲスパラメータを用いることにより、それらの式を成立させることに成功したのである。
【0017】
【発明の実施の形態】
この発明の実施の形態に係る装置/方法について図面を参照して説明する。以下の説明は、本発明の実施の形態に係る装置/方法を人工衛星の姿勢推定に用いる場合を例にとったものである。この発明の実施の形態に係る装置/方法は、人工衛星の姿勢情報をジャイロからの出力のみにより長時間、精度良く推定するものである。人工衛星の姿勢決定方程式として、修正ロドリゲスパラメータを使用している。
【0018】
図1はこの発明の実施の形態に係る装置のブロック図である。センサであるレートジャイロ1から状態量(角速度)ω(t)を受けて、カルマンフィルタ3はシステム方程式と観測方程式を生成するとともに、次式に従い時間更新処理及び観測更新処理を行う。なお、システム方程式及び観測方程式については後述する。
【0019】
【数6】
【0020】
上記式において、下添字で示された+はカルマンフィルタによる観測更新処理後の変数であることを示す。同じく、下添字で示されたkはΔt時間間隔で離散的に処理される計算式の実行がk番目であることを示し、同様にk+1はk+1番目であることを示す。右肩添字の−1は逆行列を示す。右肩添字のTは転置行列を示す。
【0021】
時間更新及び観測更新について簡単に説明を加える。時間更新とは、線形モデル
【数7】
を積分する処理のことである。上記の場合は
【数8】
を積分する処理になる。これは計算により予測値を求めていく処理である。また、同時に誤差の共分散行列も更新する。誤差の共分散行列に関する式は
【数9】
で表され、更新処理の際はこれを積分処理することになる。その結果、Pバー(t)が求められる。
【0022】
観測更新とは、新しく得られた観測量と計算予測値から新しい推定値を求める処理のことである。上記の場合の式は
【数10】
と表される。ハットの付いたものは推定値であり、バーのついたものは予測値である。この処理は観測された値と予測された観測値を比較することにより、そこに誤差に関する情報を得、それに対してあるゲインを掛けて予測値に対して付加し、それをもって推定値とするという意味である。特に
【数11】
の部分はイノベーション過程もしくはO−Cと呼ばれ、観測値と計算予測された観測値を比較する過程である。それによりどの程度、計算予測値と観測値が異なっているか比較する。この比較により誤差情報が得られるが、その中には有益なものと意味のないものがある。それをどのように分配して予測値に付加するのかを決定するのが
【数12】
と書かれているカルマンゲインと呼ばれるものである。
このカルマンゲインは
【数13】
という式により求められる(この計算のために時間更新でPバー(t)を求める必要があった)。なお、R(tk)は観測ノイズの共分散行列と呼ばれるものである。
また、誤差の共分散行列についてもカルマンゲインを用いて
【数14】
のように更新する。
前述のような処理を行う「時間更新」「観測更新」であるが、これらは新しい観測値により観測更新が行われた後、次の新しい観測値が得られるまでの間、時間更新により「位置」「姿勢」などの状態量を推定するために用いられる。
【0023】
処理部4はカルマンフィルタ3の出力に基づきαドットハット(修正ロドリゲスパラメータα(t)の時間微分αドット(t)の推定値)を生成する。
積分処理部5は処理部4の出力に基づき修正ロドリゲスパラメータの推定値αハット、α0ハットを生成する。
【0024】
システム伝播行列生成部6は初期値設定部2の出力及び積分処理部5の出力に基づき観測感度行列を更新して、その更新出力をカルマンフィルタ3に供給する。システム伝播行列とは線形システム
【数15】
のF(t)のことである。今回の場合、後述の式(2.3.5)の状態量αドットを偏微分することにより得られる(後述の式(2.3.8)から式(2.3.16)がその成分に相当する)。これは状態量と状態量の微分値を関係を示す行列である。観測感度行列とは
【数16】
のH(t)のことである。観測される値と状態量の関係を示す行列である(後述の式(2.3.1)により求められる。ただし、解析的に解くことはできないので数値的に演算処理を行う)。
【0025】
変換マトリクス生成部7は積分処理部5の出力に基づき座標変換行列Rを生成する。
姿勢推定部8は変換マトリクス生成部7の出力に基づき姿勢推定を行う。
【0026】
次に動作について説明する。まず本発明の処理の概要について説明し、次に本発明の理論及びアルゴリズムの詳細について説明し、最後にシミュレーション結果について説明する。
【0027】
本発明の実施の形態において、公知の拡張カルマンフィルタを利用する。拡張カルマンフィルタは推定したい状態量の初期値を与えておき、それと観測量の時間履歴を元に状態量の推定値を逐次計算していく手法である。
【0028】
図2のその概略フローチャートを示す。
S1:レートジャイロ1から信号を受けて修正ロドリゲスパラメータα(t)の時間微分αドット(t)の状態量、ω(t)を観測量とするシステム方程式と観測方程式を作成する。
S2:拡張カルマンフィルタを用いてαドット(t)の推定値を求める。
S3:この推定値を数値積分することによりα(t)の推定値を求める。
S4:この推定値に基づき変換マトリクスR(t)を決定する。
S5:変換マトリクスR(t)に基づき時刻tにおける姿勢の推定値を求める。
【0029】
修正ロドリゲスパラメータを用いることにより、システム方程式と観測方程式を生成することができ、人工衛星の姿勢推定に拡張カルマンフィルタを適用することができた。これによりドリフトの影響を軽減することができ、レートジャイロのみにより高精度な姿勢推定が可能になったのである。従来の手法ではシステム方程式と観測方程式を生成することができず、人工衛星の姿勢推定に拡張カルマンフィルタを適用することができなかった。
【0030】
次に理論について説明する。
[理論]
1.任意の軸周りのベクトルの回転
あるベクトルxが任意の軸の周りを角度θだけ回転させた場合を考える。軸方向の単位ベクトルをe、回転後のベクトルをx'とする。ここで次の式が成立する。
【0031】
【数17】
ただし、Iは単位マトリックスを表す。また、
【0032】
【数18】
倍角、半角の公式より
【0033】
【数19】
ここで任意の軸回りθの回転をあらわすクォータニオンqを
【0034】
【数20】
とすると、
【0035】
【数21】
となる。ただし、
【0036】
【数22】
【0037】
したがって、時刻t0における軸の向きと時刻tにおける変換マトリクスRが分かれば、そのtにおける軸の向き、すなわち姿勢が分かる。
【0038】
2.システム方程式
3次元空間の基底ベクトルをc1,c2,c3とし、この3つのベクトルがある角速度ベクトル
【0039】
【数23】
で表される軸の周りを回転するとき、c1,c2,c3の時間積分について
【0040】
【数24】
なる関係が成り立つ.
(2.2.2)式をマトリックス表現でひとつの式にまとめると
【0041】
【数25】
なるωハットを使って
【0042】
【数26】
となる。ただし、
【0043】
【数27】
【0044】
3.修正ロドリゲスパラメータの利用とその一階微分方程式
以下のような修正ロドリゲスパラメータ:αとα0を定義する。
【数28】
【0045】
すなわち、αは3次元のベクトルである。eは回転軸の方向余弦、θは回転軸回りの回転角を表す。(2.2.31)式で定義した修正ロドリゲスパラメータを用いると(2.1.4)式のクォータニオンの要素をαで表現できる。αで表現されるa、Vを使えば、(2.1.5)式の変換マトリクスもαを使って表すことができる。
【0046】
さらに、αの1階微分方程式を作ることでRの時間履歴を求められる。αの1階微分方程式は高々3本の連立方程式である。
(2.2.32)式より
【0047】
【数29】
よって、
【0048】
【数30】
次にVについて考える。
【0049】
【数31】
変換マトリクスRは(2.2.34)、(2.2.35)式より
【0050】
【数32】
(2.2.34)、(2.2.35)式を用いてαの一階微分方程式を求めると、次式のようになる。
【0051】
【数33】
【0052】
[アルゴリズムの詳細]
4.拡張カルマンフィルタへの適用
本発明の実施の形態において、衛星の互いに直交する3軸に取りつけられたレートジャイロから計測された角速度ベクトルω(t)を基に姿勢推定を行う。
具体的には、(2.2.41)式から修正ロドリゲスパラメータα(t)の時間微分αドット(t)の状態量、ω(t)を観測量とするシステム方程式と観測方程式を作成し、拡張カルマンフィルタを用いてαドット(t)の推定値を求める。これを数値積分することによりα(t)の推定値を求め、(2.2.36)式に代入して変換マトリクスR(t)を決定し、(2.1.5)式より時刻tにおける姿勢の推定値を求める。
【0053】
この場合、(2.1.5)式の右辺のxが人工衛星の任意の固定軸をあらわす方向ベクトルの初期値、左辺のx'がt秒後における固定軸の向きになる。x'とxのノルムは等しい。
はじめに観測方程式を求める。(2.2.41)式より
【0054】
【数34】
とおけば
【0055】
【数35】
これに観測ノイズを加えれば拡張カルマンフィルタにおける観測方程式となる。
次にシステム方程式を求める。(2.2.32)式より
【0056】
【数36】
(2.2.41)式を展開すると次のようになる。
【0057】
【数37】
ωの時間微分dω/dtは数値的に意味のないものとして消去すると、(2.3.4)式の1階微分は以下のようになる。
【0058】
【数38】
上式の右辺をf(a(t),aドット(t),t)とおけば
【0059】
【数39】
となる。この式にシステムノイズを加えれば拡張カルマンフィルタにおけるシステム方程式となる。
(2.3.6)式は非線型微分方程式なので、システム伝播行列を求めるためには(2.3.5)式を状態量ベクトルで偏微分した行列Fを求める必要がある。
【0060】
【数40】
(2.3.5)式よりFの各要素は以下の通りである。
【0061】
【数41】
【0062】
[シミュレーション結果]
図3にシミュレーション結果を示す。図3は、観測ノイズ標準偏差=3×10−5[rad/sec]のときの機体軸の方向の推定誤差を示す。図3によれば、はじめのうち誤差は時間に比例して増加し、1.2×105[sec]で誤差は2.2[deg]に達する。しかし、誤差はその後静定する。本発明の実施の形態によれば、カルマンゲインは速やかなに収束しており、良好に情報の復元を行うことができる。
【0063】
しかも、図3から明らかなように、本発明の実施の形態によればドリフトは累積せず、誤差は一定範囲内に収まる。これに対し、従来の手法ではドリフトは累積し、誤差が一定範囲内に収まることはなかった。この点で本発明の実施の形態の手法は非常に優れている。
【0064】
本発明は、以上の実施の形態に限定されることなく、特許請求の範囲に記載された発明の範囲内で、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。
【0065】
本発明は、人工衛星のみでなく、ジャイロを用いて姿勢を推定するもの全般、すなわち慣性計測装置を用いるものすべてに対して適用でき、精度の向上を保証するものであり、その応用分野は広範囲にわたる。例えば、本発明は、ビデオカメラの手ぶれ防止機能や、カーナビゲーションシステムなどの姿勢推定などに適用できる。
【0066】
また、本明細書において、手段とは必ずしも物理的手段を意味するものではなく、各手段の機能が、ソフトウェアによって実現される場合も包含する。さらに、一つの手段の機能が、二つ以上の物理的手段により実現されても、若しくは、二つ以上の手段の機能が、一つの物理的手段により実現されてもよい。
【0067】
【発明の効果】
以上のように、この発明によれば、慣性計測装置の出力のみに基づき高精度な姿勢推定を実現できる。特に、従来問題となっていたドリフトを除去することができる。
【図面の簡単な説明】
【図1】 この発明の実施の形態に係る装置のブロック図である。
【図2】 この発明の実施の形態に係る処理フローチャートである。
【図3】 シミュレーション結果の例である。
【符号の説明】
1 レートジャイロ
2 初期値設定部
3 カルマンフィルタ
4 修正ロドリゲスパラメータαの時間微分の推定値の生成部
5 積分処理部
6 システム伝播行列生成部
7 変換マトリクス生成部
8 姿勢推定部
Claims (6)
- 慣性計測装置から状態量ω(t)を受けて予め定められたシステム方程式及び観測方程式に入力するとともに、これらに関して時間更新処理及び観測更新処理を行うカルマンフィルタと、前記カルマンフィルタの出力に基づき修正ロドリゲスパラメータα(t)の時間微分の推定値を生成する処理部と、前記処理部の出力に基づき修正ロドリゲスパラメータの推定値を生成する積分処理部と、予め与えられた初期値及び前記積分処理部の出力に基づき観測感度行列を更新するとともに、その更新出力を前記カルマンフィルタに供給するシステム伝播行列生成部と、前記積分処理部の出力に基づき座標変換行列Rを生成する変換マトリクス生成部と、前記変換マトリクス生成部の出力に基づき姿勢推定を行う姿勢推定部とを備える慣性計測装置を用いた姿勢推定装置。
- 慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを備える慣性計測装置を用いた姿勢推定方法。 - コンピュータに、
慣性計測装置から信号を受けて修正ロドリゲスパラメータα(t)の時間微分の状態量と状態量ω(t)を観測量とするシステム方程式及び観測方程式を生成する第1ステップと、
拡張カルマンフィルタを用いて前記パラメータα(t)の時間微分の推定値を求める第2ステップと、
前記第2ステップで求められた推定値を数値積分して前記パラメータα(t)の推定値を求める第3ステップと、
前記第3ステップで求められた推定値に基づき変換マトリクスR(t)を決定する第4ステップと、
前記変換マトリクスR(t)に基づき姿勢の推定値を求めるステップとを実行させるためのプログラム。
Priority Applications (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2001128261A JP3726884B2 (ja) | 2001-04-25 | 2001-04-25 | 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム |
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 |
EP02724638A EP1382936A4 (en) | 2001-04-25 | 2002-04-24 | DEVICE AND METHOD FOR LAYING ESTIMATION USING VULNERABILITY MEASURING DEVICES AND PROGRAM |
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 JP2002323322A (ja) | 2002-11-08 |
JP3726884B2 true 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) |
Families Citing this family (18)
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 | 北京控制工程研究所 | 一种月球探测器软着陆阶段惯性导航方法 |
TWI384198B (zh) * | 2009-07-09 | 2013-02-01 | Univ Nat Chiao Tung | Angle measurement gyroscope system and angle estimation method |
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 | 扬州大学 | 一种基于多传感器数据融合的小车实时姿态检测方法 |
CN115507845B (zh) * | 2022-10-18 | 2023-07-25 | 闽江学院 | 一种顾及虚拟观测值的卫星姿态融合估计方法 |
Family Cites Families (15)
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 |
-
2001
- 2001-04-25 JP JP2001128261A patent/JP3726884B2/ja not_active Expired - Fee Related
-
2002
- 2002-04-24 EP EP02724638A patent/EP1382936A4/en not_active Withdrawn
- 2002-04-24 US US10/476,402 patent/US7248948B2/en not_active Expired - Fee Related
- 2002-04-24 WO PCT/JP2002/004103 patent/WO2002088633A1/ja not_active Application Discontinuation
Also Published As
Publication number | Publication date |
---|---|
WO2002088633A1 (fr) | 2002-11-07 |
US20040176881A1 (en) | 2004-09-09 |
JP2002323322A (ja) | 2002-11-08 |
EP1382936A1 (en) | 2004-01-21 |
US7248948B2 (en) | 2007-07-24 |
EP1382936A4 (en) | 2005-11-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3726884B2 (ja) | 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム | |
JP3947531B2 (ja) | 加速度誤差の補正方法及び装置、並びにそれを利用した慣性航法システム | |
EP1585939B1 (en) | Attitude change kalman filter measurement apparatus and method | |
CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及*** | |
CN110887480B (zh) | 基于mems传感器的飞行姿态估计方法及*** | |
CN111551174A (zh) | 基于多传感器惯性导航***的高动态车辆姿态计算方法及*** | |
JP6255924B2 (ja) | センサー用ic、センサーデバイス、電子機器及び移動体 | |
US20100114517A1 (en) | Method and system for orientation sensing | |
CA2477677C (en) | Autonomous velocity estimation and navigation | |
JP2012173190A (ja) | 測位システム、測位方法 | |
JP7025215B2 (ja) | 測位システム及び測位方法 | |
US20230366680A1 (en) | Initialization method, device, medium and electronic equipment of integrated navigation system | |
US11150090B2 (en) | Machine learning zero-rate level calibration | |
CN115033844A (zh) | 一种无人机状态估计方法、***、设备及可读存储介质 | |
CN113432612B (zh) | 飞行体的导航方法、装置及*** | |
US20180120112A1 (en) | Performance of inertial sensing systems using dynamic stability compensation | |
JP2015094631A (ja) | 位置算出装置及び位置算出方法 | |
JP2006038650A (ja) | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム | |
CN107228672B (zh) | 一种适用于姿态机动工况下的星敏和陀螺数据融合方法 | |
KR20060030188A (ko) | 동적 장치 제어 방법 및 그 장치 | |
US20200400704A1 (en) | Method and apparatus with posture estimation | |
CN114353825B (zh) | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及*** | |
CN114111770B (zh) | 一种水平姿态测量方法、***、处理设备及存储介质 | |
CN115839726B (zh) | 磁传感器和角速度传感器联合标定的方法、***及介质 | |
CN110579212B (zh) | 室内定位方法及装置 |
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 |