JPWO2016208504A1 - 宇宙機の姿勢制御装置およびｃｍｇのジンバル角演算方法 - Google Patents

Abstract

ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定し、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータを設定範囲内で変化させながら代数方程式を解くことで、与えられた角運動量を実現するために必要な複数のＣＭＧのジンバル角の解を求めるように構成する。

Description

本発明は、宇宙機に搭載されたコントロールモーメントジャイロ（ＣＭＧ：Ｃｏｎｔｒｏｌ Ｍｏｍｅｎｔ Ｇｙｒｏ）を用いて姿勢制御を行う宇宙機の姿勢制御装置、および与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求めるＣＭＧのジンバル角演算方法に関するものである。

ＣＭＧを用いて宇宙機の３軸姿勢制御を行う場合、３台以上の１軸ジンバルＣＭＧが用いられることが多い。このようなＣＭＧのシステムにおいて、ＣＭＧ全体の角運動量の目標値を与えたとき、その角運動量を実現する各ＣＭＧのジンバル角を演算する逆キネマティクス演算を行うことがある。なお、ＣＭＧ全体の角運動量とは、各ＣＭＧが有する角運動量ベクトルのベクトル和に相当する。

ここで、例えば、ＣＭＧの運動計画が行われる場合、またはＣＭＧを用いてリアルタイムで姿勢制御が行われる場合において、逆キネマティクス演算を行うことで、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求める必要が生じる（例えば、特許文献１参照）。

特許文献１に記載の従来技術では、ニュートン法のような繰り返し計算を用いて、逆キネマティクス演算の解が求められている。具体的には、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の近似解を何らかの方法で求め、ニュートン法のような繰り返し計算を用いて、与えられた角運動量を実現するジンバル角の解を求める。

特開２００６−２４０３７５号公報（５頁３６行〜６頁１４行、１０頁５行〜４４行）

しかしながら、従来技術には以下のような課題がある。
特許文献１に記載の従来技術のように、ニュートン法のような繰り返し計算によって、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求める場合、必ずしもその解が得られるとは限らない。そのため、所望の姿勢変更を実現するためのＣＭＧの運動計画を行うことができず、結果として、姿勢変更を実現することができない可能性がある。

また、ＣＭＧの台数が４台以上になると、一般に、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解が無数に存在する。しかしながら、ニュートン法のような繰り返し計算によって各ＣＭＧのジンバル角の解を求める場合、あらかじめ演算したジンバル角の近似解の近傍の解しか得ることができない。したがって、このように演算した各ＣＭＧのジンバル角の解が、無数に存在する解の中で、姿勢変更を行う上で考慮すべき評価関数を最小化する解であるという保証はない。

さらに、ニュートン法のような繰り返し計算の収束性を保証するためには、十分に精度の高いジンバル角の近似解をもとに計算を始める必要があるが、そのような近似解を確実に演算する方法は必ずしも確立されてはいない。

本発明は、前記のような課題を解決するためになされたものであり、ニュートン法のような繰り返し計算を用いなくても、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求めることのできる宇宙機の姿勢制御装置およびＣＭＧのジンバル角演算方法を得ることを目的とする。

本発明における宇宙機の姿勢制御装置は、スピン軸回りに回転するホイールと、ホイールを支持するとともにスピン軸に対して直交するジンバル軸回りに回転するジンバルとを有し、宇宙機に搭載される複数のＣＭＧを用いて、姿勢制御を行う宇宙機の姿勢制御装置であって、与えられたＣＭＧ全体の角運動量を実現するために必要な複数のＣＭＧのジンバル角の解を求める逆キネマティクス演算部と、宇宙機の姿勢角および姿勢角速度の観測値と、各ＣＭＧのジンバル角およびジンバル角速度の観測値をフィードバックすることで、宇宙機において所望の姿勢変更を実現するために必要な各ＣＭＧのジンバル制御トルクを演算するジンバル制御トルク演算部と、を備え、ジンバル制御トルク演算部は、ＣＭＧ全体の角運動量を逆キネマティクス演算部に与えることで得られる複数のＣＭＧのジンバル角の解を用いて、複数のＣＭＧのジンバル制御トルクを演算し、逆キネマティクス演算部は、ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定し、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータの値を設定範囲内で変化させながら代数方程式を解くことで、与えられたＣＭＧ全体の角運動量を実現する複数のＣＭＧのジンバル角の解を求めるものである。

また、本発明におけるＣＭＧのジンバル角演算方法は、ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定するステップと、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータの値を設定範囲内で変化させながら代数方程式を解くことで、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求めるステップと、を備えたものである。

本発明によれば、ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定し、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータを設定範囲内で変化させながら代数方程式を解くことで、与えられた角運動量を実現する各ＣＭＧのジンバル角の解を求めるように構成する。これにより、ニュートン法のような繰り返し計算を用いなくても、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求めることのできる宇宙機の姿勢制御装置およびＣＭＧのジンバル角演算方法を得ることができる。

本発明の実施の形態１における宇宙機の姿勢制御装置によって制御されるＣＭＧの概略構成を示す斜視図である。 図１のＣＭＧを人工衛星に複数台配置する場合の配置例を示す説明図である。 本発明の実施の形態１における宇宙機の姿勢制御装置の構成を示すブロック図である。 本発明の実施の形態１におけるジンバル制御トルク演算部の構成を示すブロック図である。 本発明の実施の形態１における宇宙機の姿勢制御装置のハードウェア構成の一例を示す構成図である。 本発明の実施の形態１における宇宙機の姿勢制御装置のハードウェア構成の別例を示す構成図である。 本発明の実施の形態１における順キネマティクス演算部の動作を示すフローチャートである。 本発明の実施の形態２における宇宙機の姿勢制御装置の構成を示すブロック図である。 本発明の実施の形態２における宇宙機の姿勢制御装置が、人工衛星の姿勢変更を行うときの最短駆動時間を演算する際の手順を示すフローチャートである。 図２の４台のＣＭＧに対するＣＭＧ全体の角運動量包絡面を示す説明図である。 本発明の実施の形態２における宇宙機の姿勢制御装置が、角運動量の向きを定めたときの角運動量包絡面の値を演算する際の手順を示すフローチャートである。 本発明の実施の形態３における宇宙機の姿勢制御装置が、中間ジンバル角を経由するＣＭＧのジンバル角の運動軌道を演算する際の手順を示すフローチャートである。 本発明の実施の形態３における宇宙機の姿勢制御装置が、中間ジンバル角を演算する際の手順を示すフローチャートである。 本発明の実施の形態４における宇宙機の姿勢制御装置の構成を示すブロック図である。 本発明の実施の形態４における宇宙機の姿勢制御装置が、各ＣＭＧのジンバル角速度補正値を演算する際の手順を示すフローチャートである。

以下、本発明による宇宙機の姿勢制御装置およびＣＭＧのジンバル角演算方法を、好適な実施の形態にしたがって図面を用いて説明する。なお、図面の説明においては、同一部分または相当部分には同一符号を付し、重複する説明を省略する。また、以下の実施の形態では、宇宙機が人工衛星である場合を例示するが、宇宙船等の他の宇宙機に対しても本願発明が適用可能である。

実施の形態１．
図１は、本発明の実施の形態１における宇宙機の姿勢制御装置１０によって制御されるＣＭＧ１の概略構成を示す斜視図である。図１において、ＣＭＧ１は、人工衛星の姿勢制御に用いられるアクチュエータであり、１軸ジンバルＣＭＧである。また、ＣＭＧ１は、スピン軸回りに高速で回転するホイールと、そのホイールを支持するとともにスピン軸に対して直交するジンバル軸回りに回転するジンバルとを備えて構成される。

ＣＭＧ１において、ホイールは、高速回転することで、スピン軸回りに一定量の角運動量を有する。また、ジンバルをジンバル軸回りに回転させ、ホイールの角運動量の向きを回転させることで、人工衛星本体に大きな反作用トルクが働く。この反作用トルクは、人工衛星の姿勢制御に利用される。

図２は、図１のＣＭＧ１を人工衛星に複数台配置する場合の配置例を示す説明図である。図２において、ＣＭＧ１は、通常、図２に示すように４台、または４台以上の台数が人工衛星に配置され、これら複数台のＣＭＧ１を協調して動作させることで、人工衛星の姿勢制御が実現可能となる。図２に示したＣＭＧ１の配置は、各ＣＭＧ１が四角錐の底辺に配置されていることからピラミッド配置と呼ばれ、ＣＭＧ１の典型的な配置例の一つである。

図３は、本発明の実施の形態１における宇宙機の姿勢制御装置１０の構成を示すブロック図である。図３において、宇宙機の姿勢制御装置１０（以下では、姿勢制御装置１０と略すことがある）は、逆キネマティクス演算部１９と、ジンバル制御トルク演算部２０とを備える。

ジンバル制御トルク演算部２０は、人工衛星の姿勢角および姿勢角速度の観測値と、各ＣＭＧ１のジンバル角およびジンバル角速度の観測値とをフィードバックすることで、人工衛星において所望の姿勢変更を実現するために必要な各ＣＭＧ１のジンバル制御トルクを演算する。なお、ジンバル制御トルク演算部２０の具体的な構成例については、後述する。

ジンバル制御トルク演算部２０は、ジンバル制御制御トルクを演算する過程において、逆キネマティクス問題を解く必要がある。そこで、ジンバル制御トルク演算部２０は、ＣＭＧ１全体の角運動量を逆キネマティクス演算部１９に与える。

逆キネマティクス演算部１９は、ジンバル制御トルク演算部２０から与えられたＣＭＧ１全体の角運動量を実現するために必要な複数のＣＭＧ１のジンバル角の解を求める。ここで、逆キネマティクス演算部１９において、与えられたＣＭＧ１全体の角運動量から、各ＣＭＧ１のジンバル角の解を求める処理を、逆キネマティクス演算と呼ぶ。

ジンバル制御トルク演算部２０は、ＣＭＧ１全体の角運動量を逆キネマティクス演算部１９に与えることで得られる複数のＣＭＧ１のジンバル角の解を用いて、複数のＣＭＧ１のジンバル制御トルクを演算する。

次に、ジンバル制御トルク演算部２０の構成について、図４を参照しながら説明する。図４は、本発明の実施の形態１におけるジンバル制御トルク演算部２０の構成を示すブロック図である。

図４において、ジンバル制御トルク演算部２０は、運動計画部１１、順キネマティクス演算部１２、第１の減算器１３、姿勢制御部１４、ステアリング制御部１５、加算器１６、第２の減算器１７、ジンバル制御部１８を備える。

次に、姿勢制御装置１０のハードウェア構成の一例について、図５を参照しながら説明する。図５は、本発明の実施の形態１における宇宙機の姿勢制御装置１０のハードウェア構成の一例を示す構成図である。なお、図５では、姿勢制御装置１０は、処理回路１０１によって実現される場合を例示している。

図５において、人工衛星としての衛星３０は、受信機１１１、センサ１１２、姿勢制御装置１０およびＣＭＧジンバルダイナミクス２を備える。受信機１１１は、地上の衛星利用者によって指定される、衛星３０の姿勢変更の目標値に関する情報を受信する。センサ１１２は、衛星３０の姿勢角および姿勢角速度を観測する。

姿勢制御装置１０は、受信機１１１からの信号を受けて姿勢変更軌道を計画し、さらに、センサ１１２からの姿勢角および姿勢角速度の情報をフィードバックすることで、衛星３０を制御するために必要な各ＣＭＧ１のジンバル制御トルクを演算する。

処理回路は、専用のハードウェアであっても、メモリに格納されるプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、ＤＳＰともいう）であってもよい。

次に、姿勢制御装置１０のハードウェア構成の別例について、図６を参照しながら説明する。図６は、本発明の実施の形態１における宇宙機の姿勢制御装置１０のハードウェア構成の別例を示す構成図である。なお、図６では、姿勢制御装置１０は、プロセッサ１０２およびメモリ１０３によって実現される場合を例示している。また、図５の処理回路１０１は、プロセッサ（ＣＰＵ）１０２とメモリ１０３に当たる。

図６において、衛星３０は、先の図５と同様に、受信機１１１、センサ１１２、姿勢制御装置１０およびＣＭＧジンバルダイナミクス２を備える。

処理回路１０１が、専用のハードウェアである場合、処理回路１０１は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ、ＦＰＧＡ、またはこれらを組み合わせたものが該当する。運動計画部１１、順キネマティクス演算部１２、姿勢制御部１４、ステアリング制御部１５、ジンバル制御部１８の各部の機能それぞれを処理回路で実現してもよいし、各部の機能をまとめて処理回路で実現してもよい。

処理回路がプロセッサ（ＣＰＵ）１０２の場合、運動計画部１１、順キネマティクス演算部１２、第１の減算器１３、姿勢制御部１４、ステアリング制御部１５、加算器１６、第２の減算器１７、ジンバル制御部１８および逆キネマティクス演算部１９の機能は、ソフトウェア、ファームウェア、またはソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェアおよびファームウェアは、プログラムとして記述され、メモリに格納される。処理回路は、メモリに記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。

すなわち、姿勢制御装置１０は、処理回路１０１により実行されるときに、運動計画ステップ、順キネマティクス演算ステップ、第１の減算ステップ、姿勢制御ステップ、ステアリング制御ステップ、加算ステップ、第２の減算ステップ、ジンバル制御ステップおよび逆キネマティクス演算ステップが結果的に実行されることになるプログラムを格納するためのメモリを備える。また、これらのプログラムは、運動計画部１１、順キネマティクス演算部１２、第１の減算器１３、姿勢制御部１４、ステアリング制御部１５、加算器１６、第２の減算器１７、ジンバル制御部１８および逆キネマティクス演算部１９の手順および方法をコンピュータに実行させるものであるともいえる。

ここで、メモリとは、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ、ＥＥＰＲＯＭ等の、不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等が該当する。

メモリは、上述のプログラムの他、運動計画部１１で決定されるジンバル角計画値およびジンバル角速度計画値と、順キネマティクス演算部１２で設定される姿勢角目標値および姿勢角速度目標値と、姿勢制御部１４で計算される姿勢制御トルクと、ステアリング制御部１５で計算されるジンバル角速度補正値と、ジンバル制御部１８で計算されるジンバル制御トルクと、逆キネマティクス演算部１９で計算されるジンバル角の解とを記憶する。上述のメモリに記憶された情報は、適宜読みだされ処理に用いられ、修正される。

なお、運動計画部１１、順キネマティクス演算部１２、第１の減算器１３、姿勢制御部１４、ステアリング制御部１５、加算器１６、第２の減算器１７、ジンバル制御部１８および逆キネマティクス演算部１９の各機能について、一部を専用のハードウェアで実現し、一部をソフトウェアまたはファームウェアで実現するようにしてもよい。例えば、運動計画部１１については専用のハードウェアとしての処理回路でその機能を実現し、逆キネマティクス演算部１９については、処理回路がメモリに格納されたプログラムを読み出して実行することによってその機能を実現することが可能である。なお、上記において、図５では、処理回路１０１は、図６のプロセッサ１０２に当たり、メモリは、図６のメモリ１０３に当たる。

このように、処理回路は、ハードウェア、ソフトウェア、ファームウェア、またはこれらの組み合わせによって、上述の各機能を実現することができる。以下、姿勢制御装置１０の各部の動作を説明する。

図４の説明に戻り、姿勢制御装置１０のジンバル制御トルク演算部２０に人工衛星の姿勢変更の目標値が与えられると、運動計画部１１は、その姿勢変更目標値に従って人工衛星が姿勢変更を行う際の各ＣＭＧ１の運動計画を行う。具体的には、運動計画部１１は、入力された姿勢変更目標値に適合する各ＣＭＧ１のジンバル角の運動軌道を、ジンバル角計画値およびジンバル角速度計画値として演算する。また、運動計画部１１は、演算した各ＣＭＧ１のジンバル角計画値およびジンバル角速度計画値を、順キネマティクス演算部１２および加算器１６に出力する。

また、運動計画部１１は、ＣＭＧ１の運動計画を演算する過程において、逆キネマティクス問題を解く必要がある。そこで、運動計画部１１は、ＣＭＧ１全体の角運動量を逆キネマティクス演算部１９に出力し、逆キネマティクス演算部１９から返される各ＣＭＧ１のジンバル角を用いて運動計画を行う。

運動計画部１１は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、運動計画部１１は、人工衛星の姿勢変更の目標値等をメモリから読み出し、演算したジンバル角計画値およびジンバル角速度計画値をメモリに記憶する。

次に、順キネマティクス演算部１２について、図７を参照しながら説明する。図７は、本発明の実施の形態１における順キネマティクス演算部１２の動作を示すフローチャートである。

順キネマティクス演算部１２に各ＣＭＧ１のジンバル角計画値が与えられると、順キネマティクス演算部１２は、与えられた各ＣＭＧ１のジンバル角計画値を受け取り（ステップＳ２１）、入力された各ＣＭＧ１のジンバル角計画値から、そのジンバル角を実現するＣＭＧ１全体の角運動量を演算する（ステップＳ２２）。ここで、順キネマティクス演算部１２において、入力された各ＣＭＧ１のジンバル角計画値から、ＣＭＧ１全体の角運動量の解を求める処理を、順キネマティクス演算と呼ぶ。

図４の構成では、順キネマティクス演算部１２は、運動計画部１１から入力された各ＣＭＧ１のジンバル角計画値から、その計画値を実現可能とするＣＭＧ１全体の角運動量を演算する。

また、順キネマティクス演算部１２は、ステップＳ２２で計算されたＣＭＧ１全体の角運動量から、人工衛星の姿勢角速度の目標値を演算する（ステップＳ２３）。ＣＭＧ１を含む人工衛星全体では、総角運動量が慣性空間において保存されるので、このように、ＣＭＧ１全体の角運動量から、人工衛星の姿勢角速度の目標値を定めることができる。

順キネマティクス演算部１２は、ステップＳ２３で計算された姿勢角速度の目標値を時間積分することで、人工衛星の姿勢角の目標値を演算し（ステップＳ２４）、一連の処理を終了する（ステップＳ２５）。以上のように演算された人工衛星の姿勢角目標値および姿勢角速度目標値は、第１の減算器１３に出力される。

順キネマティクス演算部１２は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、順キネマティクス演算部１２は、運動計画部１１で演算したジンバル角計画値およびジンバル角速度計画値をメモリから読み出し、演算した人工衛星の姿勢角目標値および姿勢角速度目標値をメモリに記憶する。

第１の減算器１３は、順キネマティクス演算部１２から入力された人工衛星の姿勢角目標値および姿勢角速度目標値と、衛星姿勢ダイナミクス３から入力された人工衛星の姿勢角および姿勢角速度との偏差を演算して姿勢制御部１４に出力する。なお、衛星姿勢ダイナミクス３から入力された人工衛星の姿勢角および姿勢角速度は、人工衛星の姿勢角および姿勢角速度の観測値に相当する。

姿勢制御部１４は、第１の減算器１３から入力された偏差を０にするように、人工衛星の姿勢を制御するための姿勢制御トルクを演算してステアリング制御部１５に出力する。このように、第１の減算器１３から入力された偏差から、人工衛星の姿勢角を姿勢角目標値に近づけるとともに姿勢角速度を姿勢角速度目標値に近づけるようにフィードバック制御が行われる。

このように、姿勢制御部１４は、順キネマティクス演算部１２から入力された人工衛星の姿勢角目標値および姿勢角速度目標値と、人工衛星の姿勢角および姿勢角速度の観測値とから、人工衛星の姿勢を制御するための姿勢制御トルクを演算して出力する。

姿勢制御部１４は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、姿勢制御部１４は、第１の減算器１３から入力された偏差をメモリから読み出し、演算した姿勢制御トルクをメモリに記憶する。

ステアリング制御部１５は、姿勢制御部１４から入力された姿勢制御トルクから、その姿勢制御トルクに整合する各ＣＭＧ１のジンバル角補正値およびジンバル角速度補正値を演算して加算器１６に出力する。

また、ステアリング制御部１５は、その演算の過程において、逆キネマティクス問題を解く必要がある。そこで、ステアリング制御部１５は、ＣＭＧ１全体の角運動量を逆キネマティクス演算部１９に出力し、逆キネマティクス演算部１９から返される各ＣＭＧ１のジンバル角を用いてジンバル角補正値およびジンバル角速度補正値を演算する。

ステアリング制御部１５は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、ステアリング制御部１５は、演算したジンバル角補正値およびジンバル角速度補正値をメモリに記憶する。

加算器１６は、ステアリング制御部１５から入力された各ＣＭＧ１のジンバル角補正値およびジンバル角速度補正値と、運動計画部１１から入力された各ＣＭＧ１のジンバル角計画値およびジンバル角速度計画値とをそれぞれ加算する。また、加算器１６は、演算した加算値を、各ＣＭＧ１のジンバル角目標値およびジンバル角速度目標値として第２の減算器１７に出力する。

第２の減算器１７は、加算器１６から入力された各ＣＭＧ１のジンバル角目標値およびジンバル角速度目標値と、ＣＭＧジンバルダイナミクス２から入力された各ＣＭＧ１のジンバル角およびジンバル角速度との偏差を演算してジンバル制御部１８に出力する。なお、ＣＭＧジンバルダイナミクス２から入力された各ＣＭＧ１のジンバル角およびジンバル角速度は、各ＣＭＧ１のジンバル角およびジンバル角速度の観測値に相当する。

ジンバル制御部１８は、第２の減算器１７から入力された偏差を０とするように、各ＣＭＧ１のジンバルの回転を制御するためのジンバル制御トルクを演算してＣＭＧジンバルダイナミクス２に出力する。このように、第２の減算器１７から入力された偏差から、各ＣＭＧ１のジンバル角をジンバル角目標値に近づけるとともにジンバル角速度をジンバル角速度目標値に近づけるようにフィードバック制御が行われる。

そのため、各ＣＭＧ１に適切なジンバル制御トルクが与えられることとなり、このジンバル制御トルクの反作用トルクは、衛星姿勢ダイナミクス３に与えられる。この反作用トルクが与えられることで、姿勢角および姿勢角速度が姿勢角目標値および姿勢角速度目標値に従うように人工衛星が制御される。

このように、ジンバル制御部１８は、加算器１６から入力された各ＣＭＧ１のジンバル角目標値およびジンバル角速度目標値と、各ＣＭＧ１のジンバル角およびジンバル角速度の観測値とから、各ＣＭＧ１のジンバル制御トルクを演算して出力する。

ジンバル制御部１８は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、ジンバル制御部１８は、演算した各ＣＭＧ１のジンバル制御トルクをメモリに記憶する。

続いて、逆キネマティクス演算部１９による逆キネマティクス演算について説明する。前述したとおり、順キネマティクス演算部１２は、入力された各ＣＭＧ１のジンバル角に対して順キネマティクス演算を施すことで、ＣＭＧ１全体の角運動量を演算している。この順キネマティクス演算は、容易に行うことが可能であり、順キネマティクス演算の解、すなわち、ＣＭＧ１全体の角運動量の解も一意に定まる。

これに対して、逆キネマティクス演算の解、すなわち、各ＣＭＧ１のジンバル角の解は、通常、一意には定まらず、その解を厳密に求める方法も知られていない。なお、姿勢制御装置１０がこのような逆キネマティクス演算を行う必要性としては、具体的には、例えば、以下の２点が挙げられる。すなわち、第１に、運動計画部１１による各ＣＭＧ１のジンバル角の運動軌道の演算する際に、逆キネマティクス演算の解が必要となることがある。第２に、ステアリング制御部１５による各ＣＭＧ１のジンバル角補正値およびジンバル角速度補正値を演算する際に、逆キネマティクス演算の解が必要となることがある。

そこで、ニュートン法のような繰り返し計算を用いて、逆キネマティクス演算が一般的に行われている。具体的には、与えられたＣＭＧ１全体の角運動量を近似的に実現する各ＣＭＧ１のジンバル角の値を何らかの方法で求め、ニュートン法のような繰り返し計算を用いて、与えられた角運動量を実現するジンバル角の解を求めることが一般的に行われている。

本発明における宇宙機の姿勢制御装置では、ニュートン法のような繰り返し計算を用いることなく、逆キネマティクス演算を厳密に行うことが可能な逆キネマティクス演算部１９を備えていることを技術的特徴としている。以下、逆キネマティクス演算部１９による逆キネマティクス演算について、さらに説明する。

人工衛星に配置されるＣＭＧ１の台数、すなわち姿勢制御装置１０によって制御されるＣＭＧ１の台数が３台以上の場合、ＣＭＧ１全体の角運動量が実現可能な大きさであれば、その角運動量を実現する各ＣＭＧ１のジンバル角の解が存在する。ただし、ＣＭＧ１の台数がｎ台（ｎは４以上の整数）の場合、（ｎ−３）個のＣＭＧ１のジンバル角の解が必ずしも一意には定まらない。

ここで、解が必ずしも一意には定まらない（ｎ−３）個のジンバル角をまとめてフリーパラメータθ_fと呼ぶ。本発明では、このフリーパラメータθ_fという概念を新たに導入する。

フリーパラメータθ_fの値を適当に定めるとき、（ｎ−３）個のジンバル角以外の残りの３個のジンバル角θ_j（ｊ＝１，２，３）は、下式（１）の代数方程式を満たす必要がある。

式（１）において、ｈ_iはＣＭＧ１全体の角運動量の各成分値から、フリーパラメータθ_fの分を差し引いた既知量である。また、ａ_ijおよびｂ_ijは、各ＣＭＧ１の配置によって定まる係数であり、各ＣＭＧ１の配置を決めれば既知量となる。式（１）の未知量は、３個のジンバル角θ_j（ｊ＝１，２，３）となるが、ｓｉｎθ_jとｃｏｓθ_jの間には、よく知られているように、下式（２）の関係式が成立する。

ここで、ｓｉｎθ_jとｃｏｓθ_jを未知量と見なすことにすれば、式（１）および式（２）は、６個の未知量に対する６個の式となる。ただし、これらの式は、連立代数方程式となるので一般には解を得ることが容易ではない。そこで、ＣＭＧ１全体の角運動量を表す座標系を適切に設定することで、ａ_ijおよびｂ_ijを、０を含む簡単な形で表すことができる。その場合、ｓｉｎθ_jとｃｏｓθ_jの中のどれかの変数だけを含む１変数の代数方程式に変換することができる。

１変数の代数方程式の解は容易に得られるので、その解を用いて他の変数を求めれば、最終的にすべての変数の解を求めることができる。この計算の中には、代数方程式の解を求める計算が含まれるが、ニュートン法のような解の近似値を用いた繰り返し計算を行うことなく、厳密にＣＭＧ１全体の角運動量を満たす解が得られる。

また、フリーパラメータθ_fの値は任意となり、フリーパラメータθ_fの値がそのまま（ｎ−３）個のジンバル角の解となる。したがって、フリーパラメータθ_fの値を設定範囲内で変化させながら、変化後のフリーパラメータθ_fの値ごとに、３個のジンバル角θ_j（ｊ＝１，２，３）を演算すれば、複数組の各ＣＭＧ１のジンバル角の解が得られる。なお、設定範囲とは、フリーパラメータθ_fの値が動き得る範囲であり、どのようにフリーパラメータθ_fの値を動かすかは、あらかじめ設計しておけばよい。

以上より、原理的には、ＣＭＧ１全体の角運動量を実現するすべてのジンバル角の解を得ることができる。また、複数組の各ＣＭＧ１のジンバル角の解のうちの、１組の各ＣＭＧ１のジンバル角の解を、無数にある解の中でどれがもっとも適切な解であるかを評価する指標である評価関数を用いて選択すれば、ＣＭＧ１全体の角運動量を満たす各ＣＭＧ１の最適解が得られる。なお、複数組の各ＣＭＧ１のジンバル角の解から、１組の各ＣＭＧ１のジンバル角の最適解を特定する際には、例えば、公知の評価関数を用いればよい。

このように、逆キネマティクス演算部１９は、逆キネマティクス演算を行う際の余分な自由度となる（ｎ−３）個のジンバル角をフリーパラメータθ_fとしてあらかじめ与えられるので、余分な自由度がない状態で、各ＣＭＧ１のジンバル角を一意に定めることが可能となる。

続いて、具体例として、ｎ＝４の場合における逆キネマティクス演算について説明する。この具体例では、図２に示すように、人工衛星に固定された座標系ｘｙｚを考え、±ｘ軸上にＣＭＧ１（１）およびＣＭＧ１（３）が位置し、±ｙ軸上にＣＭＧ１（２）およびＣＭＧ１（４）が位置するとする。また、ジンバル軸ｅ_g1，ｅ_g2，ｅ_g3，ｅ_g4の各方向が＋ｚ軸から角度βだけ倒れているとする。また、ジンバル角θ₁，θ₂，θ₃，θ₄が０のときのスピン軸ｅ_s1，ｅ_s2，ｅ_s3，ｅ_s4がそれぞれ＋ｙ，−ｘ，−ｙ，＋ｘ方向に一致するとする。

このように座標系をとるとき、各ＣＭＧ１のジンバル角がθ₁，θ₂，θ₃，θ₄であるときのＣＭＧ１全体の角運動量のｘ，ｙ，ｚ成分Ｈ₁，Ｈ₂，Ｈ₃は、下式（３）のように表される。

ただし、式（３）において、ｈは、各ＣＭＧ１が保有する角運動量の大きさであり、定数である。なお、ＣＭＧ１が４台存在する場合において、図２に示すピラミッド配置における座標系のとり方、および式（３）は、公知である。

続いて、ｎ＝４、すなわち、ＣＭＧ１の台数が４台であるので、フリーパラメータθ_fの次元が１（＝４−３）となり、θ_f＝θ₄とする。このとき、ＣＭＧ１全体の角運動量の各成分Ｈ₁，Ｈ₂，Ｈ₃から、フリーパラメータθ_fの分をそれぞれ差し引いたものを、ｈ₁，ｈ₂，ｈ₃とすると、下式（４）のように表される。

また、式（４）を式（３）に代入すると、下式（５）のように表される。なお、式（５）が式（１）に対応している。

さらに、式（５）を変形すると、下式（６）のように表される。

ここで、一般的に任意の２つの角度φ₁とφ₂に関して、下式（７）の恒等式が成立する。

φ₁をθ₁、φ₂をθ₃に置き換えて、式（６）を式（７）に代入すると、下式（８）のように表される。

また、式（２）から、ｓｉｎ²θ₂＋ｃｏｓ²θ₂＝１の関係式が成立するので、その関係式を用いると、式（８）から、式（９）の関係式が得られる。

式（９）において、左辺は、ｓｉｎθ₂の２次式とｃｏｓθ₂の積であり、右辺は、ｓｉｎθ₂の４次式である。したがって、式（９）の両辺を２乗することで、ｓｉｎθ₂の８次式が得られる。すなわち、ＣＭＧ１全体の角運動量を表す座標系を適切に設定することで、ｓｉｎθ₂だけを含む１変数の代数方程式に変換されることとなる。また、ｓｉｎθ₂だけを含む１変数の代数方程式を解くこと得られるジンバル角θ₂の解を用いれば、ジンバル角θ₁，θ₃の解を得ることができる。

また、フリーパラメータθ₄の値を設定範囲内で変化させながら、変化後のフリーパラメータθ₄の値ごとに、３個のジンバル角θ₁，θ₂，θ₃を演算することで、与えられた角運動量を実現する各ＣＭＧのジンバル角の解の組み合わせを複数組演算することができる。

このように、逆キネマティクス演算部１９による逆キネマティクス演算によって、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータθ_fと、ＣＭＧ１全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータθ_fの値を設定範囲内で変化させながら代数方程式を解くことで、与えられたＣＭＧ１全体の角運動量を実現するために必要な複数のＣＭＧ１のジンバル角の解を求めることができる。

逆キネマティクス演算部１９は、処理回路１０１、またはプロセッサ１０２で処理してもよいし、独立した処理回路で実現してもよい。また、逆キネマティクス演算部１９は、ＣＭＧ１全体の角運動量をメモリから読み出し、演算した各ＣＭＧ１のジンバル角の解をメモリに記憶する。

以上、本実施の形態１によれば、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個の余分な自由度のジンバル角をフリーパラメータとして設定し、ｎ個のジンバル角のうちの３個のジンバル角と、フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、フリーパラメータの値を設定範囲内で変化させながら代数方程式を解くことで、与えられた角運動量を実現するために必要な複数のＣＭＧのジンバル角の解を求めるように構成する。

これにより、ニュートン法のような繰り返し計算を用いなくても、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を精度良く演算することができる。また、ＣＭＧ全体の角運動量が与えられたときにその角運動量を満たす各ＣＭＧのジンバル角の解をすべて演算することができ、そのジンバル角の解の中から評価関数を最小化する解の組み合わせを選択し、選択した解を用いて姿勢制御を行うことで、効率的かつ滑らかな姿勢変更を実現することができる。

実施の形態２．
本発明の実施の形態２では、逆キネマティクス演算部１９による逆キネマティクス演算を利用して、運動計画部１１が人工衛星の姿勢変更を行うときの最短駆動時間を演算する場合について説明する。なお、本実施の形態２では、先の実施の形態１と同様である点の説明を省略し、先の実施の形態１と異なる点を中心に説明する。

図８は、本発明の実施の形態２における宇宙機の姿勢制御装置１０の構成を示すブロック図である。図８において、運動計画部１１は、人工衛星の姿勢変更を行うときの最短駆動時間を演算する。

続いて、運動計画部１１による最短駆動時間の演算について、図９を参照しながら説明する。図９は、本発明の実施の形態２における宇宙機の姿勢制御装置１０が、人工衛星の姿勢変更を行うときの最短駆動時間を演算する際の手順を示すフローチャートである。

ここで、図９に示すように、人工衛星の姿勢変更目標値が入力されたとき（ステップＳ１）、その姿勢変更を１軸回りの回転で実現することを考える。このような回転軸のことをオイラー軸と呼ぶ。運動計画部１１は、ステップＳ１で与えられた姿勢変更目標値からオイラー軸を演算する（ステップＳ２）。このオイラー軸は、姿勢変更目標値から、簡単な計算で求められる。

このオイラー軸の向きの単位ベクトルをαとすると、運動計画部１１は、ＣＭＧ１全体の角運動量を単位ベクトルαの向きにして、その向きにＣＭＧ１全体が発生しうる最大角運動量を演算する（ステップＳ３）。また、運動計画部１１は、ステップＳ３で演算した最大角運動量から、近似的な最短駆動時間を演算し（ステップＳ４）、一連の処理を終了する（ステップＳ５）。

このように、姿勢制御装置１０は、図９に示す手順に従って、人工衛星が姿勢変更を行う際に、その姿勢変更軸の向きにＣＭＧ全体が発生しうる最大角運動量を演算するとともに、その演算結果を用いて人工衛星の姿勢変更を行うときの最短駆動時間を演算する。

ここで、本実施の形態２における姿勢制御装置１０では、逆キネマティクス演算部１９による逆キネマティクス演算を利用して、図９のステップＳ３における最大角運動量の演算を行うように構成していることを技術的特徴としている。

図１０は、図２の４台のＣＭＧ１に対するＣＭＧ１全体の角運動量包絡面を示す説明図である。なお、図１０では、ＣＭＧ１全体のとりうる最大角運動量が３次元的に図示されている。

ここで、図９のステップＳ３におけるＣＭＧ１の最大角運動量の演算とは、角運動量の向きを定めたときに、その向きの角運動量包絡面における角運動量の大きさを演算することにあたる。

図１０に示すように、角運動量包絡面は複雑な形状をしているので、最大角運動量の演算は必ずしも容易ではない。通常、その角運動量の向きに対応する特異ベクトルと呼ばれるベクトルを演算することで角運動量包絡面の値を演算する。しかしながら、角運動量の向きと特異ベクトルとは常に１対１に対応するものではなく、すべての場合に、この特異ベクトルを確実に見いだす方法は見つかっていない。

以下、逆キネマティクス演算部１９による逆キネマティクス演算を利用した最大角運動量の演算について、図１１を参照しながら説明する。図１１は、本発明の実施の形態２における宇宙機の姿勢制御装置１０が、角運動量の向きを定めたときの角運動量包絡面の値を演算する際の手順を示すフローチャートである。図１１のフローチャートの処理では、逆キネマティクス演算によるジンバル解の有無によって、二分法を用いて角運動量包絡面の値を演算するように構成されている。

ステップＳ３０１において、運動計画部１１は、角運動量の大きさの最小値ｈ_minと最大値ｈ_maxの初期値を定める。

この例では、１台のＣＭＧ１の角運動量の大きさをｈ_wとして、ｎ台のＣＭＧによる角運動量の大きさの最大値ｈ_max＝ｎｈ_w、最小値ｈ_min＝０としている。ｈ_maxは、ＣＭＧ１全体の発生できない角運動量の大きさの最小値、ｈ_minは、ＣＭＧ１全体の発生できる角運動量の大きさの最大値に対応する。また、ｈ_max−ｈ_minが設定精度Δｈに可能な限り近づくまで、二分法が実行される。

ステップＳ３０２において、運動計画部１１は、（ｈ_max＋ｈ_min）／２を中間値として、その中間値を角運動量の大きさｈとして設定する。また、運動計画部１１は、設定した中間値ｈと、単位ベクトルαとから、角運動量ｈαを演算して逆キネマティクス演算部１９に出力する。この角運動量ｈαは、ＣＭＧ１全体が発生しうる最大角運動量の候補値に相当する。

ステップＳ３０３において、逆キネマティクス演算部１９は、フリーパラメータθ_fの値を設定する。

ステップＳ３０４において、逆キネマティクス演算部１９は、ステップＳ３０３で設定したフリーパラメータθ_fを用いて、運動計画部１１から入力された角運動量ｈαに対して逆キネマティクス演算を施す。すなわち、逆キネマティクス演算部１９は、角運動量ｈαを実現する各ＣＭＧ１のジンバル角の解を求める。また、逆キネマティクス演算部１９は、演算結果を運動計画部１１に出力する。

ステップＳ３０５において、運動計画部１１は、逆キネマティクス演算部１９から入力された演算結果から、角運動量ｈαを実現するジンバル角の解の有無を調べる。運動計画部１１は、角運動量ｈαを実現するジンバル角の解が存在しないと判断した場合には、ステップＳ３０６の処理が実行され、角運動量ｈαを実現するジンバル角の解が存在すると判断した場合には、ステップＳ３０９の処理が実行される。

ステップＳ３０６において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いたかどうかを探索する。ステップＳ３０６において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いていないと判断した場合には、フリーパラメータθ_fの動きうる領域が残っているので、ステップＳ３０７の処理が実行される。

ステップＳ３０７において、逆キネマティクス演算部１９は、フリーパラメータθ_fの値を更新して、ステップＳ３０４に戻り、更新後のフリーパラメータθ_fを用いて、ステップＳ３０４以降の処理を再び実行される。

一方、ステップＳ３０６において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いたと判断した場合には、フリーパラメータθ_fを設定範囲内で変化させてもＣＭＧ１全体の角運動量ｈαに対するジンバル角の解が存在しないこととなるので、ステップＳ３０８の処理が実行される。

ステップＳ３０８において、運動計画部１１は、角運動量ｈαを実現するジンバル角の解が存在しないと判断して、ｈ_max＝ｈとする。

ステップＳ３０９において、運動計画部１１は、角運動量ｈαを実現するジンバル角の解が存在し、ＣＭＧ１全体の角運動量がｈαの値を取り得ると判断して、ｈ_min＝ｈとする。

ステップＳ３１０において、運動計画部１１は、ｈ_max−ｈ_minの値を演算し、その値が設定精度Δｈ未満である場合には、その時点でのｈの値を、αの向きの最大角運動量の大きさの値とする。続いて、ステップＳ３１１において、一連の処理が終了となる。

一方、ステップＳ３１０において、運動計画部１１は、ｈ_max−ｈ_minの値を演算し、その値が設定精度Δｈ以上である場合には、ステップＳ３０２へと戻り、ステップＳ３０２以降の処理を再び実行される。

このように、ＣＭＧ１全体が発生しうる最大角運動量の候補値に相当する角運動量ｈαは、その角運動量ｈαを実現する各ＣＭＧのジンバル角の解の有無に応じて、逐次更新されていき、最後に更新した角運動量ｈαを最大角運動量としているので、角運動量の向きによらず、確実に解を得ることができる。

具体的には、ＣＭＧ１全体の角運動量の向きを定めたときの最大角運動量の大きさを、逆キネマティクス演算の解の有無による二分法で演算するので、角運動量の向きによらず、確実に解を得ることができる。

なお、ここでは、最大角運動量の候補値を逐次更新することで最大角運動量を特定する具体的な手法として二分法を用いる場合を例示した。しかしながら、最大角運動量の候補値を実現する各ＣＭＧのジンバル角の解を求め、各ＣＭＧのジンバル角の解の有無に応じて、その候補値を逐次更新することで、最大角運動量を演算する構成であれば、二分法以外の別の手法を用いてもよい。

図９に示すステップＳ１からステップＳ５、および図１１に示すステップＳ３０１からステップＳ３１１の演算は、処理回路１０１を用いて実現されてもよいし、プロセッサ１０２を用いて実現されてもよいし、独立した処理回路を用いて実現されてもよい。運動計画部１１は、上記のステップを演算することで得られる最大角運動量の値および近似的な最短駆動時間をメモリに記憶する。

以上、本実施の形態２によれば、与えられたＣＭＧ全体の角運動量であるＣＭＧ全体が発生しうる最大角運動量の候補値を実現する各ＣＭＧのジンバル角の解を求め、各ＣＭＧのジンバル角の解の有無に応じて、最大角運動量の候補値を逐次更新することで、最大角運動量を演算するように構成する。

これにより、人工衛星が姿勢変更を行う際に、その姿勢変更軸の向きにＣＭＧ全体が発生しうる最大角運動量の値を確実に演算することができる。また、このように演算したＣＭＧ全体が発生しうる最大角運動量を用いることで、姿勢変更に要する最短駆動時間を効率的に演算することができ、その結果、人工衛星が姿勢変更を行う際のＣＭＧの運動計画の信頼性の向上につながる。

実施の形態３．
本発明の実施の形態３では、逆キネマティクス演算部１９による逆キネマティクス演算を利用して、運動計画部１１が中間ジンバル角を経由するジンバル角の運動軌道を演算する場合について説明する。なお、本実施の形態３では、先の実施の形態１、２と同様である点の説明を省略し、先の実施の形態１、２と異なる点を中心に説明する。

図１２は、本発明の実施の形態３における宇宙機の姿勢制御装置１０が、中間ジンバル角を経由するＣＭＧ１のジンバル角の運動軌道を演算する際の手順を示すフローチャートである。

図１２に示すように、運動計画部１１は、人工衛星の姿勢変更目標値から、姿勢変更のオイラー軸αを演算すると、先の実施の形態２と同様に、ＣＭＧ１全体の角運動量をαの向きにして、その向きにＣＭＧ１全体が発生しうる最大角運動量を演算する（ステップＳ６）。

ここで、ステップＳ６で演算した最大角運動量をＣＭＧ１で実現させれば人工衛星がほぼ最短時間で姿勢変更を行うことができる。しかしながら、ＣＭＧ１全体が最大角運動量になると、それ以上の大きさの角運動量が実現できない。したがって、人工衛星の姿勢フィードバック制御において、さらに角運動量を付加するような結果が得られても、その結果を踏まえた制御を実行することができない。

そのため、運動計画部１１でのジンバル角の運動軌道の演算にあたっては、以下のように演算される。すなわち、最大角運動量と向きが同じで、最大角運動量の大きさに対して、例えば２０％程度だけ減らした大きさの角運動量を各ＣＭＧ１で実現させ、その角運動量の値を一定時間保持することで、所望の姿勢変更をほぼ実現する運動軌道が演算される。なお、このときの各ＣＭＧ１で一定時間保持するジンバル角の値を中間ジンバル角と呼ぶ。

運動計画部１１は、このように最大角運動量の大きさを減らした角運動量を、角運動量目標値として演算する（ステップＳ７）。また、運動計画部１１は、演算した角運動量目標値を、逆キネマティクス演算部１９に出力する。

逆キネマティクス演算部１９は、運動計画部１１から入力された角運動量目標値を実現する中間ジンバル角を演算する（ステップＳ８）。なお、ステップＳ８の処理については後述する。

ここで、ＣＭＧ１全体の角運動量の値が最大角運動量である場合には、ＣＭＧ１の台数が４台以上であっても、その角運動量を実現するジンバル角の値が一意に定まる。しかしながら、ＣＭＧ１全体の角運動量の値が最大角運動量よりも小さい場合には、一般に、その角運動量を実現するジンバル角の解が無数に存在する。

したがって、ステップＳ８において、逆キネマティクス演算部１９は、角運動量目標値を実現するジンバル角の解の中で、何らかの評価関数を最小化する解を最適解として選択することで、解の最適化を図る。また、逆キネマティクス演算部１９は、選択した最適解を中間ジンバル角として、その中間ジンバル角を運動計画部１１に出力する。

運動計画部１１は、逆キネマティクス演算部１９から入力された中間ジンバル角に基づいて人工衛星の姿勢変更目標値を実現するジンバル角の運動軌道を演算し（ステップＳ９）、一連の処理を終了する（ステップＳ１０）。

このように、運動計画部１１は、図１２に示す手順に従って、中間ジンバル角を経由するＣＭＧ１のジンバル角の運動軌道を演算する。

ここで、本実施の形態３における姿勢制御装置１０では、逆キネマティクス演算部１９による逆キネマティクス演算を利用して、図１２のステップＳ８における中間ジンバル角の演算を行うように構成していることを技術的特徴としている。

以下、逆キネマティクス演算部１９による逆キネマティクス演算を利用した中間ジンバル角の演算について、図１３を参照しながら説明する。図１３は、本発明の実施の形態３における宇宙機の姿勢制御装置１０が、中間ジンバル角を演算する際の手順を示すフローチャートである。

ステップＳ８０１において、運動計画部１１は、ステップＳ７で演算した角運動量目標値を逆キネマティクス演算部１９に出力する。続いて、ステップＳ８０２において、逆キネマティクス演算部１９は、（ｎ−３）個のジンバル角のフリーパラメータθ_fの値を設定する。

ステップＳ８０３において、逆キネマティクス演算部１９は、ステップＳ８０２で設定したフリーパラメータθ_fを用いて、運動計画部１１から入力された角運動量目標値を実現するジンバル角の解を求める。

ステップＳ８０４において、逆キネマティクス演算部１９は、ステップＳ８０３で求めたジンバル角の解に対する評価関数の値を演算する。なお、評価関数とは、無数にある解の中で、どれがもっとも適切な解であるかを評価する指標のことであり、例えば、ジンバル角の大きさを表す指標やジンバル角の解の状態の特異状態からの隔たりを表す指標などが評価関数として用いられる。この他の指標を用いてもよいことは言うまでもない。

ここで、ステップＳ８０４で用いられる評価関数の一例について説明する。ジンバル角の大きさを表す指標としての評価関数として、例えば、下式（１０）のような評価関数Ｊ_θを用いることができる。ただし、式（１０）では、ＣＭＧ１の台数が４台である場合の評価関数を例示している。

また、一般に、ＣＭＧ１のジンバル角速度と、ＣＭＧ１が搭載されている人工衛星に作用する姿勢制御トルクとの間には、下式（１１）に示す関係式が成立する。

ただし、式（１１）において、θは、ジンバル角から構成されるベクトルであり、τは、人工衛星に作用する姿勢制御トルクである。また、行列Ａは、ヤコビ行列と呼ばれ、各ＣＭＧ１のジンバル角の関数として演算することができる。

ここで、ヤコビ行列のランクが３の場合には、ＣＭＧ１のジンバルを回転させることで任意のトルクを実現できるが、特定のジンバル角の組み合わせにおいてはランクが２となることがあり、ジンバル角を回転させても任意のトルクを実現できない場合が存在する。このような状態を一般に特異状態と呼ぶ。

また、この特異状態からどれくらい隔たっているかの評価関数として、ヤコビ行列の条件数を用いることができる。すなわち、ジンバル角の解の状態の特異状態からの隔たりを表す指標としての評価関数として、例えば、下式（１２）のような評価関数Ｊ_sを用いることができる。

ただし、式（１２）において、σ_max（Ａ）、σ_min（Ａ）は、それぞれヤコビ行列Ａの最大固有値、最小固有値である。

図１３の説明に戻り、ステップＳ８０５において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いたかどうかを探索する。ステップＳ８０５において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いていないと判断した場合には、ステップＳ８０６の処理が実行される。

ステップＳ８０６において、逆キネマティクス演算部１９は、フリーパラメータθ_fの値を更新して、ステップＳ８０３に戻り、更新後のフリーパラメータθ_fを用いて、ステップＳ８０３以降の処理が再び実行される。

一方、ステップＳ８０５において、逆キネマティクス演算部１９は、フリーパラメータθ_fが設定範囲の全領域を動いたと判断した場合には、ステップＳ８０７において、現時点で得られているジンバル角の解の中で、評価関数を最小にする解を、最適解に近似する近似解とする。ただし、このようにステップＳ８０７で得られた近似解は、フリーパラメータθ_fの値の設定が離散的であるので、フリーパラメータθ_fの値を連続的に変化させた場合の最適解とは厳密にはいえない。

そこで、このような近似解では最適解として十分でない可能性を考慮して、ステップＳ８０８が設けられている。ステップＳ８０８において、逆キネマティクス演算部１９は、ニュートン法を用いて、ステップＳ８０７で得た近似解を修正し、修正した近似解を最適解とする。ただし、この場合、角運動量目標値を厳密に満たす解が近似解として得られているので、ニュートン法の収束が保証されることとなる。

また、逆キネマティクス演算部１９は、近似解を修正することで得られる最適解を中間ジンバル角とする。続いて、ステップＳ８０９において、一連の処理が終了となる。

なお、ステップＳ８０７において、逆キネマティクス演算部１９は、現時点で得られているジンバル角の解の中で、評価関数を最小にする解を最適解であるとして、その最適解を中間ジンバル角としてもよい。この場合、ステップＳ８０８を設ける必要はない。

このように、最大角運動量よりも小さい角運動量目標値を実現するジンバル角の解を逆キネマティクス演算によって求め、求めたジンバル角の解に対して評価関数を用いて最適解を特定し、その最適解を中間ジンバル角としている。

図１２に示すステップＳ６からステップＳ１０、および図１３に示すステップＳ８０１からステップＳ８０９の演算は、処理回路１０１を用いて実現されてもよいし、プロセッサ１０２を用いて実現されてもよいし、独立した処理回路を用いて実現されてもよい。運動計画部１１は、上記のステップを演算することで得られる最適な中間ジンバル角の解、およびジンバル角の運動軌道をメモリに記憶する。

以上、本実施の形態３によれば、与えられたＣＭＧ全体の角運動量である、ＣＭＧ全体が発生しうる最大角運動量よりも小さい第１の角運動量目標値を実現する各ＣＭＧのジンバル角の解を求め、演算した第１の角運動量目標値を実現する各ＣＭＧのジンバル角の解の中から、第１の評価関数を最小化する解を第１の最適解として選択し、選択した第１の最適解を各ＣＭＧの中間ジンバル角とするように構成する。

これにより、ＣＭＧ全体の角運動量目標値から、その角運動量目標値を満たす各ＣＭＧのジンバル角のすべての解を求めることができる。また、そのジンバル角の解の中から評価関数を用いて最適解を求めることで、適切な中間ジンバル角を得ることができ、その結果、姿勢変更時間の短縮およびＣＭＧの特異点の回避につながる。

実施の形態４．
本発明の実施の形態４では、逆キネマティクス演算部１９による逆キネマティクス演算を利用して、ステアリング制御部１５が姿勢制御トルクから各ＣＭＧ１のジンバル角速度補正値を演算する場合について説明する。なお、本実施の形態４では、先の実施の形態１〜３と同様である点の説明を省略し、先の実施の形態１〜３と異なる点を中心に説明する。

図１４は、本発明の実施の形態４における宇宙機の姿勢制御装置１０の構成を示すブロック図である。図１５は、本発明の実施の形態４における宇宙機の姿勢制御装置１０が、各ＣＭＧ１のジンバル角速度補正値を演算する際の手順を示すフローチャートである。

図１５に示すように、ステアリング制御部１５は、姿勢制御部１４から姿勢制御トルクが与えられる（ステップＳ１１）。ここで、この姿勢制御トルクをτとすると、ＣＭＧ１全体の角運動量変化δｈは、下式（１３）で与えられる。

ただし、式（１３）において、Δｔは、サンプリング時間であり、マイナス符号は、ＣＭＧ１に働くトルクの反作用トルクが人工衛星に働くことによるものである。また、ＣＭＧ１全体の角運動量の現在値をｈ₀とすると、そのΔｔ後の角運動量目標値は、ｈ₀＋δｈで与えられる。

ステアリング制御部１５は、式（１３）に従ってＣＭＧ１全体の角運動量変化δｈを演算するとともに、演算した角運動量変化δｈに対してＣＭＧ１全体の角運動量の現在値ｈ₀を加算することで角運動量目標値を演算する（ステップＳ１２）。このように、ステアリング制御部１５は、姿勢制御部１４から入力された姿勢制御トルクから、ＣＭＧ１全体の角運動量の微小変化分を演算し、ＣＭＧ１全体の角運動量の現在値にその微小変化分を加えることで、角運動量目標値を演算する。また、ステアリング制御部１５は、ステップＳ１２で演算した角運動量目標値を逆キネマティクス演算部１９に出力する。

逆キネマティクス演算部１９は、ステアリング制御部１５から入力された角運動量目標値を実現する各ＣＭＧ１のジンバル角の最適解を求める（ステップＳ１３）。ここで、逆キネマティクス演算部１９は、評価関数を用いて、先の図１３のフローチャートの処理と同様の処理を行うことで、各ＣＭＧ１のジンバル角の最適解を求める。すなわち、逆キネマティクス演算部１９は、角運動量目標値を実現するジンバル角の解の中で、何らかの評価関数を最小化する解を、最適解とする。

また、逆キネマティクス演算部１９は、各ＣＭＧ１のジンバル角の最適解をステアリング制御部１５に出力する。

続いて、ステアリング制御部１５は、逆キネマティクス演算部１９から入力された最適解（以下、最適解をθ_mと表記することがある）と、現在のジンバル角の値θ₀との差であるθ_m−θ₀を求め、その差のノルム｜｜θ_m−θ₀｜｜と、設定値Δθとを比較する（ステップＳ１４）。なお、設定値Δθは、ジンバル角速度補正値の大きさの最大値を定める量である。

ステアリング制御部１５は、ノルム｜｜θ_m−θ₀｜｜が設定値Δθ未満である場合には、各ＣＭＧ１のジンバル角速度補正値を、

として設定し（ステップＳ１５）、一連の処理を終了する（ステップＳ１７）。

一方、ステアリング制御部１５は、ノルム｜｜θ_m−θ₀｜｜が設定値Δθ以上である場合には、各ＣＭＧ１のジンバル角速度補正値を、

として設定し（ステップＳ１６）、一連の処理を終了する（ステップＳ１７）。

ここで、ステップＳ１３で用いられる評価関数の一例として、ノルム｜｜θ_m−θ₀｜｜を用いることができる。この場合、ノルム｜｜θ_m−θ₀｜｜において、現在のジンバル角θ₀との差がなるべく小さくなるジンバル角の解を、最適解として選択すればよい。

このように、ＣＭＧ１全体の角運動量目標値を実現するジンバル角の解を逆キネマティクス演算によって求め、求めたジンバル角の解に対して評価関数を用いて最適解を特定し、その最適解を用いてジンバル角速度補正値を演算している。したがって、姿勢制御トルクからジンバル角速度補正値を演算する際にヤコビ行列の逆行列の得られないような特異状態になっても常に適切なジンバル角速度補正値を演算することができ、その結果、特異状態を自然に通過することが可能となる。なお、得られたジンバル角速度補正値を時間積分することで、ジンバル角補正値を得ることができる。

図１５に示すステップＳ１１からステップＳ１７の演算は、処理回路１０１を用いて実現されてもよいし、プロセッサ１０２を用いて実現されてもよいし、独立した処理回路を用いて実現されてもよい。ステアリング制御部１５は、上記のステップを演算することで得られるジンバル角補正値およびジンバル角速度補正値をメモリに記憶する。

以上、本実施の形態４によれば、与えられたＣＭＧ全体の角運動量である、姿勢制御トルクから演算されたＣＭＧ全体の角運動量の微小変化分とＣＭＧ全体の角運動量の現在値とを加えた第２の角運動量目標値を実現する各ＣＭＧのジンバル角の解の中から、第２の評価関数を最小化する解を第２の最適解として選択し、選択した第２の最適解に応じて各ＣＭＧのジンバル角速度補正値を演算するように構成する。

これにより、ＣＭＧ全体の角運動量目標値から、その角運動量目標値を満たす各ＣＭＧのジンバル角のすべての解を求めることができる。また、そのジンバル角の解の中から評価関数を用いて選択した解に応じて各ＣＭＧのジンバル角速度補正値を得ることができる。また、ＣＭＧが特異状態になっても、その特異状態を自然に通過するようなジンバル角速度補正値を得ることができ、その結果、姿勢制御系の信頼性の向上につながる。

なお、以上のように、本実施の形態１〜４について個別に説明してきたが、本実施の形態１〜４の構成は、任意に組み合わせことが可能であり、その場合、各実施の形態の効果が重複して得られることは言うまでもない。

Claims (6)

1. スピン軸回りに回転するホイールと、前記ホイールを支持するとともに前記スピン軸に対して直交するジンバル軸回りに回転するジンバルとを有し、宇宙機に搭載される複数のＣＭＧを用いて、姿勢制御を行う宇宙機の姿勢制御装置であって、
   与えられたＣＭＧ全体の角運動量を実現するために必要な前記複数のＣＭＧのジンバル角の解を求める逆キネマティクス演算部と、
   前記宇宙機の姿勢角および姿勢角速度の観測値と、各ＣＭＧのジンバル角およびジンバル角速度の観測値をフィードバックすることで、前記宇宙機において所望の姿勢変更を実現するために必要な各ＣＭＧのジンバル制御トルクを演算するジンバル制御トルク演算部と、
   を備え、
   前記ジンバル制御トルク演算部は、
   前記ＣＭＧ全体の角運動量を前記逆キネマティクス演算部に与えることで得られる前記複数のＣＭＧのジンバル角の解を用いて、前記複数のＣＭＧの前記ジンバル制御トルクを演算し、
   前記逆キネマティクス演算部は、
   前記ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定し、前記ｎ個のジンバル角のうちの３個のジンバル角と、前記フリーパラメータと、前記ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、前記フリーパラメータの値を設定範囲内で変化させながら前記代数方程式を解くことで、前記与えられた前記ＣＭＧ全体の角運動量を実現する前記複数のＣＭＧのジンバル角の解を求める
   宇宙機の姿勢制御装置。
2. 前記ジンバル制御トルク演算部は、
   各ＣＭＧのジンバル角計画値およびジンバル角速度計画値を演算して出力する運動計画部と、
   前記運動計画部から入力された各ＣＭＧのジンバル角計画値およびジンバル角速度計画値から、前記宇宙機の姿勢角目標値および姿勢角速度目標値を演算して出力する順キネマティクス演算部と、
   前記順キネマティクス演算部から入力された前記宇宙機の姿勢角目標値および姿勢角速度目標値と、前記宇宙機の姿勢角および姿勢角速度の観測値とから、前記宇宙機の姿勢を制御するための姿勢制御トルクを演算して出力する姿勢制御部と、
   前記姿勢制御部から入力された前記姿勢制御トルクから、各ＣＭＧのジンバル角補正値およびジンバル角速度補正値を演算して出力するステアリング制御部と、
   前記運動計画部から入力された各ＣＭＧのジンバル角計画値およびジンバル角速度計画値と、前記ステアリング制御部から入力された各ＣＭＧのジンバル角補正値およびジンバル角速度補正値とをそれぞれ加算することで、各ＣＭＧのジンバル角目標値およびジンバル角速度目標値を演算して出力する加算器と、
   前記加算器から入力された各ＣＭＧのジンバル角目標値およびジンバル角速度目標値と、各ＣＭＧのジンバル角およびジンバル角速度の観測値とから、各ＣＭＧのジンバル制御トルクを演算して出力するジンバル制御部と、
   を備えた請求項１に記載の宇宙機の姿勢制御装置。
3. 前記運動計画部は、
   ＣＭＧ全体が発生しうる最大角運動量の候補値を前記与えられたＣＭＧ全体の角運動量として前記逆キネマティクス演算部に出力し、
   前記逆キネマティクス演算部は、
   前記運動計画部から入力された前記候補値を実現する各ＣＭＧのジンバル角の解を求め、求めた結果を前記運動計画部に出力し、
   前記運動計画部は、
   前記候補値を実現する各ＣＭＧのジンバル角の解の有無に応じて、前記候補値を逐次更新することで、前記最大角運動量を演算する
   請求項２に記載の宇宙機の姿勢制御装置。
4. 前記運動計画部は、
   ＣＭＧ全体が発生しうる最大角運動量よりも小さい第１の角運動量目標値を演算し、演算した前記第１の角運動量目標値を前記与えられたＣＭＧ全体の角運動量として前記逆キネマティクス演算部に出力し、
   前記逆キネマティクス演算部は、
   前記運動計画部から入力された前記第１の角運動量目標値を実現する各ＣＭＧのジンバル角の解を求め、求めた前記第１の角運動量目標値を実現する各ＣＭＧのジンバル角の解の中から、第１の評価関数を最小化する解を第１の最適解として選択し、選択した前記第１の最適解を各ＣＭＧの中間ジンバル角とする
   請求項２または３に記載の宇宙機の姿勢制御装置。
5. 前記ステアリング制御部は、
   前記姿勢制御部から入力された前記姿勢制御トルクから、ＣＭＧ全体の角運動量の微小変化分を演算し、ＣＭＧ全体の角運動量の現在値に前記微小変化分を加えることで、第２の角運動量目標値を演算し、演算した前記第２の角運動量目標値を前記与えられたＣＭＧ全体の角運動量として前記逆キネマティクス演算部に出力し、
   前記逆キネマティクス演算部は、
   前記ステアリング制御部から入力された前記第２の角運動量目標値を実現する各ＣＭＧのジンバル角の解を求め、求めた前記第２の角運動量目標値を実現する各ＣＭＧのジンバル角の解の中から、第２の評価関数を最小化する解を第２の最適解として選択し、選択した前記第２の最適解を前記ステアリング制御部に出力し、
   前記ステアリング制御部は、
   前記逆キネマティクス演算部から入力された前記第２の最適解に応じて各ＣＭＧのジンバル角速度補正値を演算する
   請求項２から４のいずれか１項に記載の宇宙機の姿勢制御装置。
6. ＣＭＧの台数をｎ（ｎは、４以上の整数）とするとき、
   ｎ台のＣＭＧに対応するｎ個のジンバル角のうちの（ｎ−３）個のジンバル角をフリーパラメータとして設定するステップと、
   前記ｎ個のジンバル角のうちの３個のジンバル角と、前記フリーパラメータと、ＣＭＧ全体の角運動量との関係を示す代数方程式を用いて、前記フリーパラメータの値を設定範囲内で変化させながら前記代数方程式を解くことで、与えられたＣＭＧ全体の角運動量を実現する各ＣＭＧのジンバル角の解を求めるステップと、
   を備えたＣＭＧのジンバル角演算方法。

Images