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