JP5962842B2 - 空間安定装置、空間安定方法、及び空間安定プログラム - Google Patents

Description

本発明は、空間安定装置、空間安定方法、及び空間安定プログラム記憶媒体に関し、特に回転自在に直列連結された複数の制御対象を空間安定化する空間安定装置、空間安定方法、及び空間安定プログラム記憶媒体に関する。

空間安定装置では、固定部と１つ以上の制御対象との間は、それぞれ１本の回転軸を備えた連結部を介して直列に連結され、連結部の回転角が制御される。

空間安定装置の一例が、特許文献１に記載されている。特許文献１の空間安定装置は、制御対象であるアウタジンバル及びインナジンバルとアウタトルカと、インナトルカと、アウタサーボ増幅器と、インナサーボ増幅器とを備える。アウタトルカは、固定部に対するアウタジンバルの回転を駆動する。インナトルカは、アウタジンバルに対するインナジンバルの回転を駆動する。アウタサーボ増幅器は、アウタトルカの駆動信号を出力する。インナサーボ増幅器は、インナトルカの駆動信号を出力する。アウタジンバルは、固定部に対して１本の回転軸の回りに回転自在に連結される。インナジンバルは、アウタジンバルに対して１本の回転軸の回りに回転自在に連結され、ペイロードに固定される。アウタジンバルの回転軸方向と、インナジンバルの回転軸方向とは同じである。

特許文献１には、２つの構成（以下、「構成１」、「構成２」という。）が記載されている。

構成１は、上記の構成に加え、更に、慣性系に対するインナジンバルの回転軸回りの回転角度を検出するインナ慣性センサと、アウタジンバルに対するインナジンバルの回転角度を検出する角度センサとを備える。

構成２は、構成１に加え、更に、慣性系に対するアウタジンバルの回転軸回りの回転角度を検出するアウタ慣性センサを備える。

特許文献１の空間安定装置は、以下のように動作する。

構成１及び構成２の両方において、インナサーボ増幅器は、インナ慣性センサの出力を入力して、インナ慣性センサの出力に基づいてインナトルカを駆動して、アウタジンバルに対するインナジンバルの回転角度を制御する。

構成１では、アウタサーボ増幅器は、角度センサの出力を入力して、角度センサの出力に基づいてアウタトルカを駆動して、固定部に対するアウタジンバルの回転角度を制御する。

構成２では、アウタサーボ増幅器は、アウタ慣性センサ及び角度センサの出力を入力して、アウタ慣性センサ及び角度センサの出力に基づいてアウタトルカを駆動して、固定部に対するアウタジンバルの回転角度を制御する。

上記動作の結果、構成１の場合、インナジンバルは、インナ慣性センサからのフィードバックを有するので、インナサーボ増幅器を用いて空間安定化される。

これに対して、構成２の場合、インナジンバルは、インナ慣性センサからのフィードバックを有するので、インナサーボ増幅器を用いて空間安定化される。構成２の場合、更に、アウタジンバルは、アウタ慣性センサからのフィードバックを有するので、アウタサーボ増幅器を用いて更に空間安定化され、空間安定性が向上される。

特開２００４−３６１１２１号公報（第２−４ページ、図１−２）

特許文献１に開示されている技術では、構成１の場合、アウタ慣性センサが無いため、構成２の場合に比べて空間安定性が低いという問題がある。これに対して、構成２の場合、アウタ慣性センサを備えるため、構成１の場合に比べて追加の慣性センサが必要であるという問題がある。

本発明の目的は、１つの慣性センサで高い空間安定性を実現できる空間安定装置、空間安定方法、及び空間安定プログラム記憶媒体を提供することにある。

本発明の空間安定装置は、移動体に対し、第１の回転軸の回りに回転自在に連結される第１の制御対象と、第１の制御対象に対し、第２の回転軸の回りに回転自在に連結される第２の制御対象と、移動体と第１の制御対象のいずれかの角速度を計測して移動体と第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力する慣性検出手段と、移動体に対する第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する第１の角速度検出手段と、第１の角度の目標角度信号を出力する第１指令生成手段と、第１の制御対象に対する第２の制御対象の第２の角度の目標角度信号を出力する第２指令生成手段と、角速度評価信号に基づく信号と第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する第１の加算手段と、第１の角度の目標角度信号に基づく信号と、第１の計測角速度信号に基づく信号とに基づいて、第１の角度を制御する第１の制御手段と、第２の角度の目標角度信号に基づく信号と、第２の計測角速度信号に基づく信号とに基づいて、第２の角度を制御する第２の制御手段と、を備えることを特徴とする。

本発明によれば、１つの慣性センサで高い空間安定性を実現することが可能である。

本発明の第１の実施形態における空間安定装置の構成を示すブロック図である。 本発明の第１の実施形態における空間安定装置のハードウェア構成の一例を示すブロック図である。 本発明の第１の実施形態における空間安定装置の縮減された構成を示すブロック図である。 本発明の第４の実施形態における空間安定装置の構成を示すブロック図である。 本発明の第５の実施形態における空間安定装置の構成を示すブロック図である。 本発明の第６の実施形態における空間安定装置の構成を示すブロック図である。 本発明の第７の実施形態における空間安定装置の構成を示すブロック図である。 本発明の第１の実施形態における空間安定装置の最小限の構成を示すブロック図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。尚、すべての図面において、同様な構成要素には同様の符号を付し、適宜説明を省略する。
（第１の実施形態）
図１は、本発明の第１の実施形態における空間安定装置１００の構成を示すブロック図である。尚、以下では、空間安定装置１００を取り付ける固定部と最終的な空間安定化対象であるペイロードとの間が、２つの連結部を介して、それぞれ１本の回転軸の回りに回転自在に連結される場合について説明する。以降、固定部に近い側の連結部の回転軸を「第１回転軸」、ペイロードに近い側の連結部の回転軸を「第２回転軸」という。

本実施形態の空間安定装置１００は、第１制御対象１０３と、第１角度検出器１０４と、第１制御器１０５と、第１ドライバ１０６と、第１指令生成器１０７と、慣性センサ１０８と、第２制御対象１１３と、第２角度検出器１１４と、第２制御器１１５と、第２ドライバ１１６と、第２指令生成器１１７と、第１角速度検出器１１８と、第１加算器１２２と、第１座標変換器１２３と、第２座標変換器１２４と、第１軌道生成器１２５と、第１減算器１２６とを備える。

第１制御対象１０３は、空間安定装置１００が取り付けられる固定部（図示なし）に対し１本の回転軸（第１回転軸）の回りに回転自在に連結される。

第１角度検出器１０４は、請求項内の第１の角度検出手段であり、固定部に対する第１制御対象１０３の第１回転軸の回りの角度（以降、「第１角度」という。）の信号を出力する。

第１制御器１０５は、請求項内の第１の制御手段であり、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第１制御対象１０３を制御する信号を第１ドライバ１０６に出力する。

第１ドライバ１０６は、第１角度を制御する信号を入力して、第１制御対象１０３の第１回転軸の回りの回転（第１角度）を駆動する。以降、固定部に固定された座標系を「固定部座標系」又は「座標系０」という。尚、固定部は、空間安定装置１００が取り付けられる対象であり、固定部自体が不動物である必要はない。すなわち、固定部は、移動体、固定物、又は別の空間安定装置の制御対象であってもよい。

尚、第１制御器１０５は、第１制御対象１０３を直接制御してもよい。第１制御器１０５が第１制御対象１０３を直接制御する場合には、第１制御器１０５は、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第１制御対象１０３を制御する。又、第１制御器１０５が第１制御対象１０３を直接制御する場合には、空間安定装置１００には、第１ドライバ１０６は含まれない。

第１指令生成器１０７は、請求項内の第１指令生成手段であり、第１角度の目標角度の信号を出力する。

尚、第１指令生成器１０７が出力する第１角度の目標角度は、第１軌道生成器１２５により、あらかじめ保持されてもよい。第１角度の目標角度が第１軌道生成器１２５によりあらかじめ保持される場合には、第１軌道生成器１２５は、あらかじめ設定された目標角度に従い、目標角度へ至るための角度軌道及び角速度軌道の信号を出力する。又、第１角度の目標角度が第１軌道生成器１２５によりあらかじめ保持される場合には、空間安定装置１００には、第１指令生成器１０７は含まれない。

慣性センサ１０８は、請求項内の慣性検出手段であり、慣性センサ１０８に固定された直交座標系（以降、「慣性センサ座標系」という。）の３つの成分の角速度（以降、「動揺角速度」という。）信号を出力する。尚、慣性センサ１０８は、固定部に固定され、慣性センサ座標系は座標系０と一致するものとする。

第２制御対象１１３は、第１制御対象１０３に対し１本の回転軸（第２回転軸）の回りに回転自在に連結される。尚、制御対象は、別の空間安定装置の固定部であってもよい。

第２角度検出器１１４は、請求項内の第２の角度検出手段であり、第１制御対象１０３に対する第２制御対象１１３の第２回転軸の回りの角度（以降、「第２角度」という。）の信号を出力する。

第２制御器１１５は、請求項内の第２の制御手段であり、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第２制御対象１１３を制御する信号を第２ドライバ１１６に出力する。

第２ドライバ１１６は、第２角度を制御する角度の信号を入力して、第１制御対象１０３に対する第２制御対象１１３の第２回転軸の回りの回転（第２角度）を駆動する。

尚、第２制御器１１５は、第２制御対象１１３を直接制御してもよい。第２制御器１１５が第２制御対象１１３を直接制御する場合には、第２制御器１１５は、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第２制御対象１１３を制御する。又、第２制御器１１５が第２制御対象１１３を直接制御する場合には、空間安定装置１００には、第２ドライバ１１６は含まれない。

第２指令生成器１１７は、請求項内の第２の指令生成手段であり、第２角度の目標角度の信号を出力する。

尚、第２指令生成器１１７が出力する第２角度の目標角度は、第２制御器１１５により、あらかじめ保持されてもよい。第２角度の目標角度が第２制御器１１５によりあらかじめ保持される場合には、第２制御器１１５は、あらかじめ設定された目標角度に従い、検出又は推定した、現在の角度又は角速度の信号を入力して、第２制御対象１１３を制御する信号を第２ドライバ１１６に出力する。又、第２角度の目標角度が第２制御器１１５によりあらかじめ保持される場合には、空間安定装置１００には、第２指令生成器１１７は含まれない。

第１角速度検出器１１８は、請求項内の第１の角速度検出手段であり、第１角度の時間的変化率である制御対象角速度の信号を出力する。尚、第１角速度検出器１１８は、第１角度検出器１０４が出力する、第１角度の信号を入力して、第１角度の信号を時間微分して出力してもよい。又は、第１角速度検出器１１８は、第１制御器１０５の内部で状態推定器を構成して、第１角度と第１制御器１０５の出力である第１制御対象１０３を制御する信号から角速度の推定値の信号を生成し、角速度の推定値の信号を出力してもよい。

第１加算器１２２は、請求項内の第１の加算手段であり、２つの入力信号を加算した信号を出力する。尚、第１加算器１２２は、入力信号の組が、それぞれ複数の成分を含むときは、成分毎に加算した信号を出力する。又、ある成分については、１つの入力信号しかなく、加算できない場合には、その成分については、第１加算器１２２は、入力信号をそのまま出力する。

第１座標変換器１２３は、請求項内の第１座標変換手段であり、慣性センサ座標系における動揺角速度、第１角度及び第２角度の信号を入力する。そして、第１座標変換器１２３は、第１制御対象１０３に固定された座標系（以降、「第１座標系」又は「座標系１」という。）における動揺角速度の慣性センサ座標系から第１座標系への回転のオイラー角による３つの成分（以降、「第１座標系における動揺角速度のオイラー角成分」という。）の信号を出力する。尚、第１制御器１０５へフィードバックされる信号成分は、出力の３つの成分のうち、１つの第１角度の成分である。

第２座標変換器１２４は、請求項内の第２座標変換手段であり、第１座標系における動揺角速度のオイラー角成分、及び第２角度の信号を入力する。そして、第２座標変換器１２４は、第２制御対象１１３に固定された座標系（以降、「第２座標系」又は「座標系２」という。）における動揺角速度の第１座標系から第２座標系への回転のオイラー角による３つの成分（以降、「第２座標系における動揺角速度のオイラー角成分」という。）の信号を出力する。尚、第２制御器１１５へフィードバックされる信号成分は、出力の３つの成分のうち、１つの第２角度の成分である。

第１軌道生成器１２５は、請求項内の第１の軌道生成手段であり、目標角度の信号を入力して、目標角度へ至るための角度軌道及び角速度軌道の信号を出力する。尚、「軌道」とは、時間的な変化過程を意味する。

第１減算器１２６は、請求項内の第１の減算手段であり、２つの入力信号の一方から他方を減算した信号を出力する。

図２は、空間安定装置１００のハードウェア構成の一例を示すブロック図である。

空間安定装置１００のコンピュータ９０７は、第１ドライバ１０６、第１角度検出器１０４、第２ドライバ１１６、第２角度検出器１１４、慣性センサ１０８、第１角速度検出器１１８とデータの送受信を行う。

コンピュータ９０７は、記憶装置９０１と、ＣＰＵ（Central Processing Unit）９０３と、キーボード９０４と、モニタ９０５と、Ｉ／Ｏ（Input/Output）９０８とを備え、これらが内部バス９０６で接続される。

Ｉ／Ｏ９０８は、第１ドライバ１０６、第１角度検出器１０４、第２ドライバ１１６、第２角度検出器１１４、慣性センサ１０８、第１角速度検出器１１８に接続される。Ｉ／Ｏ９０８は、コンピュータ９０７と、第１ドライバ１０６、第１角度検出器１０４、第２ドライバ１１６、第２角度検出器１１４、慣性センサ１０８、第１角速度検出器１１８とデータの送受信を行う。

記憶装置９０１は、ＣＰＵ９０３の第１制御器１０５等の動作プログラム等を格納する。ＣＰＵ９０３は、コンピュータ９０７全体を制御し、記憶装置９０１に格納された動作プログラムを実行し、Ｉ／Ｏ９０８を介して第１制御器１０５等のプログラムの実行やデータの送受信を行なう。

なお、コンピュータ９０７は、ＣＰＵ９０３のみを備え、外部に備えられた、記憶装置９０１、メモリ９０２、キーボード９０４、モニタ９０５、及びＩ／Ｏ９０８を用いて動作してもよい。

次に、本実施形態の動作を説明する。

第１制御器１０５は、第１角度に関し、第１軌道生成器１２５から目標角度軌道の信号を入力し、第１角度検出器１０４から第１角度の現在角度信号を入力する。

又、第１制御器１０５は、慣性センサ１０８出力の第１座標系における動揺角速度のオイラー角成分の第１角度成分信号を入力する。

第１制御器１０５は、目標角度軌道と現在角度の差分が０になるように、第１制御対象１０３を制御する信号を生成する。

尚、第１制御器１０５は、負のフィードバックとして、第１座標変換器１２３から動揺角速度の第１角度成分を入力して目標角度軌道から減算し、動揺角度をキャンセルするように制御信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。

第１座標変換器１２３は、慣性センサ１０８の角速度出力の慣性センサ座標系の３つの成分の信号を入力する。又、第１座標変換器１２３は、第１角度検出器１０４から第１角度の信号を、第２角度検出器１１４から第２角度の信号を入力する。第１座標変換器１２３は、慣性センサ１０８の角速度出力を第１座標系のオイラー角成分に変換した信号を出力する。尚、出力の成分のうち、第１角度の成分の出力が第１制御器１０５にフィードバックされる。

第１減算器１２６は、第１角速度検出器１１８からの現在角速度の信号から、第１軌道生成器１２５からの目標角速度軌道の信号を減算して、第１角度の微分である現在角速度からの、目標角速度の偏差信号を出力する。

第１加算器１２２は、第１減算器１２６からの第１角度の現在角速度偏差の信号と、第１座標変換器１２３からの動揺角速度を第１座標系のオイラー角成分に変換した信号とを加算して、第１角度回りに関する角速度（以降、「第１残留動揺角速度」という。）の第１座標系のオイラー角成分の信号を出力する。

第２座標変換器１２４は、第１残留動揺角速度の第１座標系におけるオイラー角成分の信号を入力する。又、第２座標変換器１２４は、第２角度検出器１１４から第２角度の信号を入力する。第２座標変換器１２４は、第１残留動揺角速度を第２座標系のオイラー角成分に変換した信号を出力する。尚、出力の成分のうち、第２角度の成分の出力が第２制御器１１５にフィードバックされる。

第２制御器１１５は、第２角度に関し、第２指令生成器１１７から目標角度の信号を入力し、第２角度検出器１１４から第２角度の現在角度信号を入力する。尚、目標角度は、角度軌道ではない。

又、第２制御器１１５は、第１座標系における第１残留動揺角速度のオイラー角成分の第２角度成分信号を入力する。

第２制御器１１５は、目標角度と現在角度の差分が０になるように、第２制御対象１１３を制御する信号を生成する。

尚、第２制御器１１５は、負のフィードバックとして、第２座標変換器１２４から第１残留動揺角速度の第２角度成分を入力して目標角度から減算し、残留動揺角度をキャンセルするように制御信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。

さて、以下では、座標系ｉから座標系ｋへの座標変換が行列^kＣ_iで表されるものとする。座標系ｉに対する座標系ｋの回転のオイラー角による表現の一つであるロール角、ピッチ角、ヨー角による表現はφ、θ、Ψで表されるものとする。以下では、ロール角、ピッチ角、ヨー角の回転軸をロール軸、ピッチ軸、ヨー軸という。

座標系ｉから座標系ｋへの座標変換の一例を数１で表す。
［数１］

又、座標系０に対する座標系１の回転のロール角、ピッチ角、ヨー角による表現はφ₁、θ₁、Ψ₁で、座標系１に対する座標系２の回転のロール角、ピッチ角、ヨー角による表現はφ₂、θ₂、Ψ₂で表されるものとする。座標系０から座標系１への座標変換、座標系１から座標系２への座標変換は、それぞれ数２、数３で表される。
［数２］

［数３］

又、座標系０に対する座標系１の回転のヨー角の回転軸と、第１回転軸の方向とが一致し、第１角度はΨ₁₁で表されるものとする。又、座標系１を第１角度Ψ₁₁回転した結果が座標系２と一致するものとし、座標系２の回転のヨー角の回転軸と、第２回転軸の方向とが一致し、第２角度はΨ₂₂で表されるものとする。又、慣性センサ座標系における角速度は［ω_x ω_y ω_z］^Tで表されるものとする。又、第１座標変換器１２３の出力は［ω_x1 ω_y1 ω_z1］^Tで、第１加算器１２２の出力は［ω_xa1 ω_ya1 ω_za1］^Tで、第２座標変換器１２４の出力は［ω_x2 ω_y2 ω_z2］^Tで表されるものとする。

第１座標変換器１２３は、数４−数５に従い、入力を出力に変換する。その結果、慣性センサ座標系における角速度は、座標系１における角速度に変換される。又、第２座標変換器１２４は、数６に従い、入力を出力に変換する。その結果、座標系１における角速度は、座標系２における角速度に変換される。尚、慣性センサ座標系の３軸のうちの１軸（ｚ軸）を第１回転軸の方向に一致させる場合を示した。
［数４］

［数５］

［数６］

第１座標変換器１２３、第２座標変換器１２４は、それぞれ、数４−数５、数６以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数７、数８を利用した座標変換を行ってもよい。尚、慣性センサ座標系の３軸のうちの１軸（ｚ軸）を第１回転軸の方向に一致させる場合を示した。
［数７］

［数８］

以上説明したように、本実施形態における空間安定装置１００は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第２制御器１１５の両方が、慣性センサ１０８の出力を利用して、まず第１制御器１０５にて動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。同時に、第２制御器１１５は、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。

尚、空間安定装置１００は、φ₁＝θ₁＝Ψ₁＝０であるとき、慣性センサ座標系の３軸のうちの１軸（本実施形態ではｚ軸であるがｚ軸に限定されない）が第１回転軸及び第２回転軸の方向と一致していれば、慣性センサ座標系の３軸のうちの残り２軸の成分を０と置き換えた１成分の出力のみを利用して動作する。

上記実施形態では、第１軌道生成器１２５の出力する角度軌道が目標角度である。そして、実施形態では、第１制御器１０５は、第１角度検出器１０４の出力である第１角度を現在角度として入力し、第１制御対象１０３の第１角度を制御する制御信号を生成するという角度制御を実施する。同様に、上記実施形態では、第２指令生成器１１７の出力が目標角度である。そして、第２制御器１１５は、第２角度検出器１１４の出力である第２角度を現在角度として入力し、第２制御対象１１３の第２角度を制御する制御信号を生成するという角度制御を実施する。

しかしながら、上記実施形態は角度制御に限定されず、第１軌道生成器１２５の出力する角速度が目標角速度であってもよい。そして、第１制御器１０５は、第１角度検出器１０４の出力である第１角度を微分した角速度を現在角速度として入力し、第１制御対象１０３の第１角速度を制御する制御信号を生成するという角速度制御を行ってもよい。同様に第２指令生成器１１７の出力が目標角速度であってもよい。そして、第２制御器１１５は、第２角度検出器１１４の出力である第２角度を微分した角速度を現在角速度として入力し、第２制御対象１１３の第２角速度を制御する制御信号を生成するという角速度制御を行ってもよい。

本発明に他の例である、縮減された構成を有する空間安定装置を図３に示す。

上述のように、第１指令生成器１０７が出力する第１角度の目標角度は、第１軌道生成器１２５により、あらかじめ保持されてもよい。第１角度の目標角度が第１軌道生成器１２５によりあらかじめ保持される場合には、第１軌道生成器１２５は、あらかじめ設定された目標角度に従い、目標角度へ至るための角度軌道及び角速度軌道の信号を出力する。又、第１角度の目標角度が第１軌道生成器１２５によりあらかじめ保持される場合には、空間安定装置には、第１指令生成器１０７が含まれない。

尚、第２指令生成器１１７が出力する第２角度の目標角度は、第２制御器１１５により、あらかじめ保持されてもよい。第２角度の目標角度が第２制御器１１５によりあらかじめ保持される場合には、第２制御器１１５は、あらかじめ設定された目標角度に従い、検出又は推定した、現在の角度又は角速度の信号を入力して、第２制御対象１１３を制御する信号を第２ドライバ１１６に出力する。又、第２角度の目標角度が第２制御器１１５によりあらかじめ保持される場合には、空間安定装置には、第２指令生成器１１７が含まれない。

又、第１制御器１０５は、第１制御対象１０３を直接制御してもよい。第１制御器１０５が第１制御対象１０３を直接制御する場合には、第１制御器１０５は、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第１制御対象１０３を制御する。又、第１制御器１０５が第１制御対象１０３を直接制御する場合には、空間安定装置には、第１ドライバ１０６が含まれない。

又、第２制御器１１５は、第２制御対象１１３を直接制御してもよい。第２制御器１１５が第２制御対象１１３を直接制御する場合には、第２制御器１１５は、目標角度、及び検出又は推定した、現在の角度又は角速度の信号を入力して、第２制御対象１１３を制御する。又、第２制御器１１５が第２制御対象１１３を直接制御する場合には、空間安定装置には、第２ドライバ１１６が含まれない。

更に、図３の空間安定装置には、第１指令生成器１０７、第２指令生成器１１７、第１ドライバ１０６、又は第２ドライバ１１６が追加されてもよい。

本発明に他の例である、最小限の構成を有する空間安定装置を図８に示す。

第１制御器１０５の出力信号が、第１角度の検出信号とみなせる場合には、第１ドライバ１０６及び第１角度検出器１０４は省略可能である。

第２制御器１１５の出力信号が、第２角度の検出信号とみなせる場合には、第２ドライバ１１６及び第２角度検出器１１４は省略可能である。

第１角度の目標角度へ至るための角度軌道又は角速度軌道のいずれも制御されず、第１角度が制御される場合には、第１軌道生成器１２５及び第１減算器は省略可能である。

慣性センサ座標系と第１制御対象１０３に固定された第１座標系とが一致する場合には、第1座標変換器１２３は省略可能である。つまり、慣性センサ１０８がと第１制御対象１０３に固定される場合には、第1座標変換器１２３は省略可能である。

第１制御対象１０３に固定された第１座標系と第２制御対象１１３に固定された第２座標系とが一致する場合には、第２座標変換器１２４は省略可能である。つまり、第１制御対象１０３が固定部に対し回転自在に連結される第１回転軸と、第２制御対象１１３が第１制御対象１０３に対し回転自在に連結される第２回転軸とが一致する場合には、第２座標変換器１２４は省略可能である。

すなわち、本発明に最小限の構成を有する空間安定装置１００は、第１の制御対象１０３と、第２の制御対象１１３と、慣性センサ１０８と、第１角速度検出器１１８と、第１指令生成器１０７と、第２指令生成器１１７と、第１加算器１２２と、第１制御器１０５と、第２制御器１１５と、を備える。

第１の制御対象１０３は、移動体に対し、第１の回転軸の回りに回転自在に連結される。

第２の制御対象１１３は、第１の制御対象１０３に対し、第２の回転軸の回りに回転自在に連結される。

慣性センサ１０８は、移動体と第１の制御対象１０３のいずれかの角速度を計測して移動体と第１の制御対象１０３のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力する、請求項内の慣性検出手段である。

第１角速度検出器１１８は、移動体に対する第１の制御対象１０３の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する、請求項内の第１の角速度検出手段である。

第１指令生成器１０７は、第１の角度の目標角度信号を出力する、請求項内の第１指令生成手段である。

第２指令生成器１１７は、第１の制御対象１０３に対する第２の制御対象１１３の第２の角度の目標角度信号を出力する、請求項内の第２指令生成手段である。

第１加算器１２２は、角速度評価信号に基づく信号と第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する、請求項内の第１の加算手段である。

第１制御器１０５は、第１の角度の目標角度信号に基づく信号と、第１の計測角速度信号に基づく信号とに基づいて、第１の角度を制御する、請求項内の第１の制御手段である。

第２制御器１１５は、第２の角度の目標角度信号に基づく信号と、第２の計測角速度信号に基づく信号とに基づいて、第２の角度を制御する、請求項内の第２の制御手段である。
（第２の実施形態）
本発明の第２の実施形態における空間安定装置の構成は、図１に示した第１の実施形態における空間安定装置１００の構成と同じである。第１の実施形態と本実施形態とで共通する説明は省略し、第１の実施形態に対する本実施形態の相違点のみについて説明する。

本発明の第１の実施形態では、慣性センサ１０８は、固定部に固定されるが、本実施形態では、慣性センサ１０８は、第１制御対象１０３に固定される。そのため、第１制御器１０５は、慣性センサ１０８の出力である動揺角速度を、第１座標変換器１２３が第１座標系におけるオイラー角成分に変換した角速度の第１角度成分の信号を入力する。

第１座標変換器１２３は、数９に従い、入力を出力に変換する。その結果、慣性センサ座標系における角速度は、座標系１における角速度に変換される。
［数９］

尚、第１座標変換器１２３は、数９以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数１０を利用した座標変換を行ってもよいが、数１０は数９と同一である。
［数１０］

尚、空間安定装置は、慣性センサ座標系の３軸のうちの１軸（本実施形態ではｚ軸であるがｚ軸に限定されない）が第１回転軸及び第２回転軸の方向と一致していれば、慣性センサ座標系の３軸のうちの残り２軸の成分を０と置き換えた１成分の出力のみを利用して動作する。

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第２制御器１１５の両方が、慣性センサ１０８の出力を利用して、まず第１制御器１０５にて動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。同時に、第２制御器１１５は、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。
（第３の実施形態）
本発明の第３の実施形態における空間安定装置の構成は、図１に示した第１の実施形態における空間安定装置の構成と同じである。第１の実施形態と本実施形態とで共通する説明は省略し、第１の実施形態に対する本実施形態の相違点のみについて説明する。

本発明の第１の実施形態では、慣性センサ１０８は、固定部に固定されるが、本実施形態では、慣性センサ１０８は、第２制御対象１１３に固定される。そのため、第１制御器１０５は、慣性センサ座標系における動揺角速度を、第１座標変換器１２３が第１座標系におけるオイラー角成分に変換した角速度の信号を入力する。

第１座標変換器１２３は、数１１に従い、入力を出力に変換する。その結果、慣性センサ座標系における角速度は、座標系１における角速度に変換される。

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第２制御器１１５の両方が、慣性センサ１０８の出力を利用して、まず第１制御器１０５にて動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。同時に、第２制御器１１５は、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。
［数１１］

尚、第１座標変換器１２３は、数１１以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数１２を利用した座標変換を行ってもよい。
［数１２］

尚、空間安定装置は、慣性センサ座標系の３軸のうちの１軸（本実施形態ではｚ軸であるがｚ軸に限定されない）が第１回転軸及び第２回転軸の方向と一致していれば、φ₂＝θ₂＝Ψ₂＝０として、慣性センサ座標系の３軸のうちの残り２軸の成分を０と置き換えた１成分の出力のみを利用して動作する。
（第４の実施形態）
図４は、本発明の第４の実施形態における空間安定装置の構成を示すブロック図である。本発明の第１の実施形態と本実施形態とで共通する説明は省略し、本発明の第１の実施形態に対する本実施形態の相違点のみについて説明する。

本実施形態の空間安定装置１００は、図１に示した第１の実施形態の構成に対して、第１積分器１０９と、第２積分器１１９と、を更に備える。

第１積分器１０９は、入力信号を時間積分した信号を出力する。具体的には、後述の動揺角速度のオイラー角成分を積分し、動揺角度の第１角度成分信号に変換する。

第２積分器１１９は、入力信号を時間積分した信号を出力する。

第１制御器１０５は、慣性センサ１０８出力の第１座標系における動揺角速度のオイラー角成分を、第１積分器１０９が角度に変換した動揺角度の第１角度成分信号を入力する。

尚、第１制御器１０５は、負のフィードバックとして、第１積分器１０９から動揺角度の第１角度成分を入力して目標角度軌道から減算し、動揺角度をキャンセルするように制御信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。

又、第２制御器１１５は、第１座標系における第１残留動揺角速度のオイラー角成分を、第２積分器１１９で角度に変換した残留動揺角度の第２角度成分信号を入力する。

尚、第２制御器１１５は、負のフィードバックとして、第２積分器１１９から残留動揺角度の第２角度成分を入力して目標角度から減算し、残留動揺角度をキャンセルするように制御信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第２制御器１１５の両方が、慣性センサ１０８の出力を、第１積分器１０９及び第２積分器１１９が角度に変換した出力を利用して、まず第１制御器１０５にて動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように角度を制御される。同時に、第２制御器１１５は、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように第２制御器１１５から制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように角度を制御される。

尚、本実施形態では、第１の実施形態との相違点を説明した。しかしながら、本実施形態の慣性センサ１０８は、第１の実施形態と同様に固定部に固定されるものには限定されない。慣性センサ１０８は、角速度を出力するものであれば、第１積分器１０９又は第２積分器１１９はそれを時間積分して角度を出力できる。従って、本実施形態の慣性センサ１０８は、第２の実施形態と同様に第１制御対象１０３に固定されてもよいし、第３の実施形態と同様に第２制御対象１１３に固定されてもよい。
（第５の実施形態）
図５は、本発明の第５の実施形態における空間安定装置の構成を示すブロック図である。本発明の第４の実施形態と本実施形態とで共通する説明は省略し、本発明の第４の実施形態に対する本実施形態の相違点のみについて説明する。

本実施形態の空間安定装置１００は、図４に示した第４の実施形態の構成に対して、第１位相補償器１１０と、第２位相補償器１２０と、第３位相補償器１２７と、第４位相補償器１２９と、第１増幅器１１１と、第２増幅器１２１と、第３増幅器１２８と、第４増幅器１３０と、第１角度検出器１０４と、第１制御器１０５と、第１ドライバ１０６と、第１指令生成器１０７と、を更に備える。

第１位相補償器１１０、第２位相補償器１２０、第３位相補償器１２７、第４位相補償器１２９は、それぞれの入力信号に、例えば、数１３に示す伝達関数で示すフィルタ処理を施し（数１３中のｆ₁、ｆ₂はユーザーが任意に設定できる周波数を示す）、それぞれの入力信号の位相をあらかじめ設定された周波数値に従い変化させた信号を出力する。
［数１３］

尚、ある変換器の入力及び出力信号が時刻ｔ≧０の関数ｘ（ｔ）及びｙ（ｔ）であるとき、そのラプラス変換をＸ（ｓ）及びＹ（ｓ）とすると、Ｙ（ｓ）／Ｘ（ｓ）を伝達関数という。時刻ｔ≧０で定義される関数ｆ（ｔ）のラプラス変換は、複素数ｓの関数である数１４で定義される。
［数１４］

第１増幅器１１１、第２増幅器１２１、第３増幅器１２８、第４増幅器１３０は、それぞれの入力信号のゲインを、それぞれにあらかじめ設定された値に従い変化させた信号を出力する。

第１座標変換器１２３の出力と第１積分器１０９又は第１制御器１０５との間に、第１位相補償器１１０と第１増幅器１１１とが挿入される。そのため、慣性センサ１０８出力の第１座標系における動揺角速度のオイラー角成分または第１積分器１０９が角度に変換した動揺角度の第１角度成分信号の位相（慣性センサ１０８出力から第１角度までの遅延）とゲイン（動揺角度の第１角度成分の振幅と第１角度振幅の比）を調整することが可能である。第１位相補償器１１０が保持する位相特性と、第１増幅器１１１が保持するゲインの値とを変化させることにより、第１制御器１０５は遅延がなく最適なゲイン特性（ゲイン＝１）の信号を受けることが可能となり空間安定の応答性が向上する。

第２座標変換器１２４の出力と第２積分器１１９と第２制御器１１５の間に、第２位相補償器１２０と第２増幅器１２１とが挿入される。そのため、第１座標系における第１残留動揺角速度のオイラー角成分または残留動揺角度の第２角度成分信号の位相（慣性センサ１０８出力から第２角度までの遅延）とゲイン（動揺角度の第２角度成分の振幅と第２角度振幅の比）を調整することが可能である。第２位相補償器１２０が保持する位相特性と、第２増幅器１２１が保持するゲインの値とを変化させることにより、第２制御器１１５は遅延がなく最適なゲイン特性（ゲイン＝１）の信号を受けることが可能となり空間安定の応答性が向上する。

第１座標変換器１２３の出力と第１制御器１０５との間に、第３位相補償器１２７と第３増幅器１２８とが挿入される。そのため、慣性センサ１０８出力の第１座標系における動揺角速度のオイラー角成分信号の位相（遅延）とゲインを調整することが可能である。第１制御器１０５は、動揺角度の第１角度成分信号を目標とし検出角度信号のフィードバックを利用した制御（比例制御）を行う。第１制御器１０５は、更に、第１座標系における動揺角速度のオイラー角成分信号を目標とし検出角度の時間微分である検出角速度信号のフィードバックを利用した制御（微分制御）を行う。そのため、第１制御器１０５は、空間安定の応答性を向上できる。第３位相補償器１２７が保持する位相特性と、第３増幅器１２８が保持するゲインの値とを変化させることにより、第１制御器１０５は最適な位相およびゲイン特性の信号を受けることが可能である。

第２座標変換器１２４の出力と第２制御器１１５との間に、第４位相補償器１２９と第４増幅器１３０とが挿入される。そのため、第２制御器１１５に対する第１座標系における第１残留動揺角速度のオイラー角成分信号の位相（遅延）とゲインを調整することが可能である。第２制御器１１５は、残留動揺角度の第２角度成分信号を目標とし検出角度信号のフィードバックを利用した制御（比例制御）を行う。第２制御器１１５は、更に、第１座標系における第１残留動揺角速度のオイラー角成分信号を目標とし検出角速度信号のフィードバックを利用して制御（微分制御）を行う。そのため、第２制御器１１５は、空間安定の応答性を向上できる。第４位相補償器１２９が保持する位相差の値と、第４増幅器１３０が保持するゲインの値とを変化させることにより、第２制御器１１５は最適な位相およびゲイン特性の信号を受けることが可能である。

以上説明したように、本実施形態における空間安定装置は、第１の実施形態における効果に加えて、空間安定の応答性を向上できる。第１の理由は、第１制御器１０５と第２制御器１１５の両方が、第１位相補償器１１０及び第１増幅器１１１、又は、第２位相補償器１２０及び第２増幅器１２１を利用して、遅延がなく最適なゲインの動揺角度及び残留動揺角度信号を受けることが可能であるからである。第２の理由は、第１制御器１０５と第２制御器１１５の両方が、第３位相補償器１２７及び第３増幅器１２８、又は、第４位相補償器１２９及び第４増幅器１３０を利用して、遅延がなく最適なゲインの動揺角速度及び第１残留動揺角速度信号を受けることが可能であるからである。

尚、本実施形態における空間安定装置は、第３位相補償器１２７及び第３増幅器１２８、第４位相補償器１２９及び第４増幅器１３０がない構成でもよい。この場合、第１制御器１０５と第２制御器１１５の両方が、遅延がなく最適なゲインの動揺角度及び残留動揺角度信号を受けることが可能である。また、本実施形態における空間安定装置は、第１位相補償器１１０及び第１増幅器１１１、第２位相補償器１２０及び第２増幅器１２１がない構成でもよい。この場合、第１制御器１０５と第２制御器１１５の両方が、遅延がなく最適なゲインの動揺角速度及び第１残留動揺角速度信号を受けることが可能である。
（第６の実施形態）
図６は、本発明の第６の実施形態における空間安定装置の構成を示すブロック図である。本発明の第１の実施形態及び本発明の第５の実施形態と本実施形態とで共通する説明は省略し、本実施形態の相違点のみについて説明する。

本実施形態の空間安定装置１００は、図１に示した第１の実施形態の構成に対して、図５に示した第５の実施形態の第３増幅器１２８、第３位相補償器１２７、第４増幅器１３０、第４位相補償器１２９と、を更に備える。

第３増幅器１２８、第３位相補償器１２７、第４増幅器１３０、第４位相補償器１２９は、図５に示した第５の実施形態の第３増幅器１２８、第３位相補償器１２７、第４増幅器１３０、第４位相補償器１２９と同等である。

第１座標変換器１２３の出力と第１制御器１０５との間に、第３位相補償器１２７と第３増幅器１２８とが挿入される。そのため、慣性センサ１０８出力の第１座標系における動揺角速度のオイラー角成分信号の位相（遅延）とゲインを調整することが可能である。

第１制御器１０５は、第１座標系における動揺角速度のオイラー角成分信号を目標とし、検出角度の時間微分である検出角速度信号のフィードバックを利用した制御（微分制御）を行うので、空間安定の応答性を向上できる。

第３位相補償器１２７が保持する位相特性と、第３増幅器１２８が保持するゲインの値とを変化させることにより、第１制御器１０５は、最適な位相およびゲイン特性の信号を受けることが可能である。

第２座標変換器１２４の出力と第２制御器１１５との間に、第４位相補償器１２９と第４増幅器１３０とが挿入される。そのため、第２制御器１１５に対する第１座標系における第１残留動揺角速度のオイラー角成分信号の位相（遅延）とゲインを調整することが可能である。

第２制御器１１５は、第１座標系における第１残留動揺角速度のオイラー角成分信号を目標とし検出角速度信号のフィードバックを利用して制御（微分制御）を行うので、空間安定の応答性を向上できる。

第４位相補償器１２９が保持する位相差の値と、第４増幅器１３０が保持するゲインの値とを変化させることにより、第２制御器１１５は、最適な位相およびゲイン特性の信号を受けることが可能である。

以上説明したように、本実施形態における空間安定装置は、第１の実施形態における効果に加えて、空間安定の応答性を更に向上できる。その理由は、第１制御器１０５と第２制御器１１５の両方が、第３位相補償器１２７及び第３増幅器１２８、又は、第４位相補償器１２９及び第４増幅器１３０を利用して、遅延がなく最適なゲインの動揺角速度及び第１残留動揺角速度信号を受けることが可能であるからである。

尚、本実施形態における空間安定装置１００は、第３位相補償器１２７及び第３増幅器１２８または第４位相補償器１２９及び第４増幅器１３０がない構成も可能である。

又、本実施形態では、第１の実施形態との相違点を説明した。しかしながら、本実施形態の慣性センサ１０８は、第１の実施形態と同様に固定部に固定されるものには限定されない。慣性センサ１０８は、角速度又は角度の信号を出力するものであれば、第３位相補償器１２７、第４位相補償器１２９、第３増幅器１２８又は第４増幅器１３０は、信号の位相（遅延）とゲインを調整できる。従って、本実施形態の慣性センサ１０８は、第２の実施形態と同様に第１制御対象１０３に固定されてもよいし、第３の実施形態と同様に第２制御対象１１３に固定されてもよい。

更に、空間安定装置１００は、第１の実施形態と同様に、第１積分器１０９、第２積分器１１９、第１増幅器１１１、第２増幅器１２１、第１位相補償器１１０、及び第２位相補償器１２０を含まない構成には限定されない。第１制御器１０５又は第２制御器１１５に対しフィードバック信号を出力する信号路は、角速度又は角度を出力するものであれば、信号の位相（遅延）とゲインを調整できる。あるいは、第１制御器１０５又は第２制御器１１５に対しフィードバック信号を出力する信号路は、角速度の信号を入力するものであれば、第１積分器１０９又は第２積分器１１９を用いて角度の信号に変換できる。従って、本実施形態の空間安定装置１００は、第５の実施形態と同様に、第１積分器１０９、第２積分器１１９、第１増幅器１１１、第２増幅器１２１、第１位相補償器１１０、又は第２位相補償器１２０を含んでもよい。
（第７の実施形態）
図７は、本発明の第７の実施形態における空間安定装置の構成を示すブロック図である。本発明の第１の実施形態と本実施形態とで共通する説明は省略し、本発明の第１の実施形態に対する本実施形態の相違点のみについて説明する。

本実施形態の空間安定装置１００は、図１に示した第１の実施形態の構成に対して、第１制御対象１０３が他の物体に対し２軸に回転自在に連結され、第２制御対象１１３が第１制御対象１０３に対し２軸に回転自在に連結される。第１制御対象１０３の２軸の回転軸のうち１軸は「第１回転軸」で、もう１軸を「第３回転軸」という。第２制御対象１１３の２軸の回転軸のうち１軸は「第２回転軸」で、もう１軸を「第４回転軸」という。尚、第１回転軸及び第２回転軸の方向と、第３回転軸及び第４回転軸の方向とは必ずしも一致してなくてよい。

本実施形態の空間安定装置１００は、第１の実施形態の構成に対して、第１制御対象１０３の第３回転軸の制御系の一部である、第３角度検出器１３１、第３制御器１３２、第３ドライバ１３３、第３指令生成器１３４、第３軌道生成器１３５を更に備える。第３角度検出器１３１、第３制御器１３２、第３ドライバ１３３、第３指令生成器１３４、第３軌道生成器１３５は、それぞれ、第１制御対象１０３の第１回転軸の制御系の一部である、第１角度検出器１０４、第１制御器１０５、第１ドライバ１０６、第１指令生成器１０７、第１軌道生成器１２５と同じ機能を有する。

本実施形態の空間安定装置１００は、第１の実施形態の構成に対して、第３角速度検出器１３６、第２加算器１４１、第２減算器１４２を更に備える。

第３角速度検出器１３６は、第３角度の時間的変化率である制御対象角速度の信号を出力する。尚、第３角速度検出器１３６は、第３角度検出器１３１が出力する、第３角度の信号を入力して、第３角度の信号を時間微分して出力してもよい。又、第３角速度検出器１３６は、第３制御器１３２の内部で状態推定器を構成して、第３角度と第３制御器１３２の出力である第１制御対象１０３を制御する信号から角速度の推定値の信号を生成し、角速度の推定値の信号を出力してもよい。

第２減算器１４２は、第１減算器１２６と同じ機能を有する。第２減算器１４２は、第３角速度検出器１３６からの現在角速度の信号から、第３軌道生成器１３５からの目標角速度軌道の信号を減算して、第３角度の微分である現在角速度からの、目標角速度の偏差信号を出力する。

第２加算器１４１は、第１加算器１２２と同じ機能を有し、第２減算器１４２からの第３角度の現在角速度偏差の信号と、第１座標変換器１２３からの動揺角速度を第１座標系のオイラー角成分に変換した信号とを加算して、第３角度回りに関する残留動揺角速度（以降、「第３残留動揺角速度」という。）の第１座標系のオイラー角成分の信号を出力する。

本実施形態の空間安定装置１００は、第１の実施形態の構成に対して、第２制御対象１１３の第４回転軸の制御系の一部である、第４角度検出器１３７、第４制御器１３８、第４ドライバ１３９、第４指令生成器１４０を更に備える。第４角度検出器１３７、第４制御器１３８、第４ドライバ１３９、第４指令生成器１４０は、それぞれ、第２制御対象１１３の第２回転軸の制御系の一部である、第２角度検出器１１４、第２制御器１１５、第２ドライバ１１６、第２指令生成器１１７と同じ機能を有する。

慣性センサ１０８が第１の実施形態と同じく固定部に固定され、慣性センサ座標系は座標系０と一致するものとする。第１座標変換器１２３は、数１５−数１６に従い、入力を出力に変換する。第３角度はθ₁₁で表されるものとする。その結果、慣性センサ座標系における角速度は、座標系１における角速度に変換される。又、第２座標変換器１２４は、数１７−数１８に従い、入力を出力に変換する。第４角度はθ₂₂で表されるものとする。その結果、座標系１における角速度は、座標系２における角速度に変換される。
［数１５］

［数１６］

［数１７］

［数１８］

第１座標変換器１２３、第２座標変換器１２４は、それぞれ、数１５−数１６、数１７−数１８以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数１９、数２０を利用した座標変換を行ってもよい。尚、慣性センサ座標系の３軸のうちの２軸（y軸、ｚ軸）をそれぞれ第３回転軸、第１回転軸の方向に一致させる場合を示した。
［数１９］

［数２０］

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。慣性センサ１０８の出力を利用して、まず第１制御器１０５と第３制御器１３２は、第１回転軸と第３回転軸それぞれの動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように第１回転軸と第３回転軸それぞれの角度を制御される。それと同時に、第２制御器１１５および第４制御器１３８は、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように第２回転軸と第４回転軸それぞれの角度を制御される。

上記実施形態では、第１制御器１０５は、第１軌道生成器１２５の出力する角度軌道が目標角度である。そして、実施形態では、第１制御器１０５は、第１角度検出器１０４の出力である第１角度を現在角度として入力し、第１制御対象１０３の第１角度を制御する制御信号を生成するという角度制御を実施する。同様に第２制御器１１５、第３制御器１３２、第４制御器１３８も角度制御を実施する。しかながら、上記実施形態は角度制御に限定されず、第１軌道生成器１２５の出力する角速度が目標角速度であってもよい。そして、第１制御器１０５は、第１角度検出器１０４の出力である第１角度を微分した角速度を現在角速度として入力し、第１制御対象１０３の第１角速度を制御する制御信号を生成するという角速度制御を行ってもよい。同様に第２制御器１１５、第３制御器１３２、第４制御器１３８も、角速度制御を実施してもよい。

尚、本実施形態では、第１の実施形態との相違点を説明した。しかしながら、空間安定装置１００は、第１の実施形態と同様に、第１積分器１０９、第２積分器１１９、第１増幅器１１１、第２増幅器１２１、第１位相補償器１１０、及び第２位相補償器１２０を含まない構成には限定されない。第１制御器１０５又は第２制御器１１５に対しフィードバック信号を出力する信号路は、角速度又は角度を出力するものであれば、信号の位相（遅延）とゲインを調整できる。あるいは、第１制御器１０５又は第２制御器１１５に対しフィードバック信号を出力する信号路は、角速度の信号を入力するものであれば、第１積分器１０９又は第２積分器１１９を用いて角度の信号に変換できる。従って、本実施形態の空間安定装置１００は、第５の実施形態と同様に、第１積分器１０９、第２積分器１１９、第１増幅器１１１、第２増幅器１２１、第１位相補償器１１０、又は第２位相補償器１２０を含んでもよい。本実施形態の空間安定装置１００は、第３制御器１３２又は第４制御器１３８に対し、フィードバック信号を出力する信号路を構成するための、積分器、増幅器、又は位相補償器を含んでもよい。
（第８の実施形態）
本発明の第８の実施形態における空間安定装置の構成は、以下に説明する構成を除いて、図７に示した第７の実施形態における空間安定装置の構成と同じである。第７の実施形態と本実施形態とで共通する説明は省略し、第７の実施形態に対する本実施形態の相違点のみについて説明する。

本発明の第８の実施形態では、図７に示した構成において、第２角度検出器１１４から第１座標変換器１２３への信号と、第４角度検出器１３７から第１座標変換器１２３への信号と、第１角度検出器１０４から第１座標変換器１２３への信号とが不要である。

本発明の第７の実施形態では、慣性センサ１０８は、固定部に固定されるが、本実施形態では、慣性センサ１０８は、第１制御対象１０３に固定される。そのため、第１制御器１０５は、慣性センサ１０８の出力である動揺角速度を、第１座標変換器１２３が第１座標系におけるオイラー角成分に変換した角速度の第１角度成分の信号を入力する。また、第３制御器１３２は、慣性センサ１０８の出力である動揺角速度を、第１座標変換器１２３が第１座標系におけるオイラー角成分に変換した角速度の第３角度成分の信号を入力する。第１座標変換器１２３は、数２１に従い、入力を出力に変換する。その結果、慣性センサ座標系における角速度は、座標系１における角速度に変換される。
［数２１］

尚、第１座標変換器１２３は、数２１以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数２２を利用した座標変換を行ってもよいが、数２２は数２１と同一である。
［数２２］

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第３制御器１３２の両方が、慣性センサ１０８の出力を利用して、まず第１回転軸と第３回転軸それぞれの動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように第１回転軸と第３回転軸それぞれの角度を制御される。同時に、第２制御器１１５および第４制御器１３８の両方が、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように第２回転軸と第４回転軸それぞれの角度を制御される。
（第９の実施形態）
本発明の第９の実施形態における空間安定装置の構成は、図７に示した第７の実施形態における空間安定装置の構成と同じである。第７の実施形態と本実施形態とで共通する説明は省略し、第７の実施形態に対する本実施形態の相違点のみについて説明する。

本発明の第７の実施形態では、慣性センサ１０８は、固定部に固定されるが、本実施形態では、慣性センサ１０８は、第２制御対象１１３に固定される。そのため、第２制御器１１５は、慣性センサ１０８の出力である動揺角速度を、第２座標変換器１２４が第２座標系におけるオイラー角成分に変換した角速度の第２角度成分の信号を入力する。また、第４制御器１３８は、慣性センサ１０８の出力である動揺角速度を、第２座標変換器１２４が第２座標系におけるオイラー角成分に変換した角速度の第４角度成分の信号を入力する。第２座標変換器１２４は、数２３に従い、入力を出力に変換する。その結果、慣性センサ座標系における角速度は、座標系２における角速度に変換される。
［数２３］

尚、第２座標変換器１２４は、数２３以外に1-2-3 Euler angle sequenceを用いたオイラー角の時間微分の関係式である以下の数２４を利用した座標変換を行ってもよい。
［数２４］

以上説明したように、本実施形態における空間安定装置は、１つの慣性センサで高い空間安定性を実現できる。その理由は、以下のとおりである。第１制御器１０５と第３制御器１３２の両方が、慣性センサ１０８の出力を利用して、まず第１回転軸と第３回転軸それぞれの動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第１制御対象１０３は、慣性空間に対して空間安定化するように第１回転軸と第３回転軸それぞれの角度を制御される。同時に、第２制御器１１５および第４制御器１３８の両方が、第１制御対象１０３では除去できなかった残留動揺角度をキャンセルするように制御指令の信号を出力する。その結果、第２制御対象１１３は、慣性空間に対して空間安定化するように第２回転軸と第４回転軸それぞれの角度を制御される。

以上、実施形態を参照して本願発明を説明したが、本願発明は上記実施形態に限定されるものではない。本願発明の構成や詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

[付記１]
移動体に対し、第１の回転軸の回りに回転自在に連結される第１の制御対象と、
前記第１の制御対象に対し、第２の回転軸の回りに回転自在に連結される第２の制御対象と、
前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定され、第１の制御対象の第１の計測角速度信号を出力する慣性検出手段と、
前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する第１の角速度検出手段と、
前記第１の角度の目標角度信号を出力する第１指令生成手段と、
前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力する第２指令生成手段と、
前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する第１の加算手段と、
前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御する第１の制御手段と、
第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御する第２の制御手段と、
を備えることを特徴とする空間安定装置。
[付記２]
前記移動体に対する前記第１の制御対象の第１の角度を検出して、第１の角度検出信号を出力する第１の角度検出手段と、
前記第１の制御対象に対する前記第２の制御対象の第２の角度を検出して、第２の角度検出信号を出力する第２の角度検出手段と、
をさらに有することを特徴とする付記１に記載の空間安定装置。
[付記３]
前記第１の計測角速度信号、及び前記第１の角度検出信号又は前記第２の角度検出信号を入力して、前記第１の計測角速度信号を前記第１の角度に関する成分を含む１つ以上のオイラー角成分に変換し、前記第1の計測角速度信号に基づく信号を出力する第１座標変換手段を有することを特徴とする付記１または２に記載の空間安定装置。

[付記４]
前記第２の計測角速度信号、及び前記第２の角度検出信号を入力して、前記第２の計測角速度信号を前記第２の角度に関する成分を含む１つ以上のオイラー角成分に変換し、前記第２の計測角速度信号に基づく信号の１つ以上の成分を出力する第２の座標変換手段と、を有することを特徴とする付記１から３のいずれか１項に記載の空間安定装置。
[付記５]
前記第１指令生成手段により出力された前記第１の角度の目標角度の信号が入力して、前記第１の目標角度へ至る角度軌道である第１の目標角度信号及び前記角度軌道に対応する角速度軌道である前記第１の角度の目標角度信号に基づく信号を出力する第１の軌道生成手段と、
前記角速度評価信号から前記第１の角度の目標角度信号に基づく信号を減算して偏差角速度信号を出力する第１の減算手段と、
を有することを特徴とする付記１から４のいずれか１項に記載の空間安定装置。
[付記６]
前記第１の計測角速度信号の１つの成分を時間積分して第１の計測角度信号の１つの成分を第1制御手段へ出力する第１の積分手段と、
前記第２の計測角速度信号の１つの成分を時間積分して第２の計測角度信号の１つの成分を第2制御手段へ出力する第２の積分手段と、
を更に有する、ことを特徴とする付記１から５のいずれか１項に記載の空間安定装置。
[付記７]
前記第1の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第１の値に従い変化させた信号を出力する第１の位相補償手段と、
前記第１の位相補償手段が出力する信号のゲインをあらかじめ設定された第２の値に従い変化させた信号を前記第１の積分手段へ出力する第１の増幅手段と、
前記第２の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第３の値に従い変化させた信号を出力する第２の位相補償手段と、
前記第２の位相補償手段が出力する信号のゲインをあらかじめ設定された第４の値に従い変化させた信号を前記第２の積分手段へ出力する第２の増幅手段と、
を更に備えることを特徴とする付記１から６のいずれか１項に記載の空間安定装置。
[付記８]
前記第１の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第５の値に従い変化させた信号を出力する第３の位相補償手段と、
前記第３の位相補償手段が出力する信号のゲインをあらかじめ設定された第６の値に従い変化させた信号を前記第１の制御手段へ出力する第３の増幅手段と、
前記第２の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第７の値に従い変化させた信号を出力する第４の位相補償手段と、
前記第４の位相補償手段が出力する信号のゲインをあらかじめ設定された第８の値に従い変化させた信号を前記第２の制御手段へ出力する第４の増幅手段と、
を更に備えることを特徴とする付記７に記載の空間安定装置。
[付記９]
移動体に対し、第１の回転軸の回りに回転自在に連結される第１の制御対象と、前記第１の制御対象に対し、第２の回転軸の回りに回転自在に連結される第２の制御対象と、を備える空間安定装置が備えるコンピュータを、
前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力する慣性検出手段と、
前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する第１の角速度検出手段と、
前記第１の角度の目標角度信号を出力する第１指令生成手段と、
前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力する第２指令生成手段と、
前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する第１の加算手段と、
前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御する第１の制御手段と、
第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御する第２の制御手段と、
して機能させるための空間安定プログラムを格納した非一時的な記憶媒体。
[付記１０]
移動体に対し第１の回転軸の回りに回転自在に連結される第１の制御対象の、前記他の物体に対する第１の角度を検出して、第１の角度検出信号を出力し、
前記第１の制御対象に対し第２の回転軸の回りに回転自在に連結される第２の制御対象の、前記第１の制御対象に対する第２の角度を検出して、第２の角度検出信号を出力し、
前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力し、
前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力し、
前記第１の角度の目標角度信号を出力し、
前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力し、
前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力し、
前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御し、
第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御し、
を備えることを特徴とする空間安定方法。
[付記１１]
前記慣性検出手段は、前記移動体に固定されたことを特徴とする付記１から８のいずれか１項に記載の空間安定装置。
[付記１２]
前記慣性検出手段は、前記第１の制御対象に固定されたことを特徴とする付記１から８のいずれか１項に記載の空間安定装置。
[付記１３]
前記慣性検出手段は、前記第２の制御対象に固定されたことを特徴とする付記１から８のいずれか１項に記載の空間安定装置。
[付記１４]
前記移動体に対する前記第１の制御対象の第１の角度に関する評価とは、前記第１の角度を微分して算出した角速度に基づく信号であることを特徴とする付記１から８のいずれか１項に記載の空間安定装置。

[付記１５]
前記第１の角速度検出手段は、第１の制御対象の第１の角度に関する角速度を推定する推定手段をさらに有し、
前記移動体に対する前記第１の制御対象の第１の角度に関する評価とは、前記推定手段により推定された角速度に基づく信号であることを特徴とする付記１から８のいずれか１項に記載の空間安定装置。

[付記１６]
前記第１の角速度検出手段は、第１の制御対象の第１の角度に関する角速度を推定する推定手段をさらに有し、
前記検出された角速度である角速度評価信号とは、前記推定手段により推定された角速度に基づく信号であることを特徴とする付記１から８のいずれか１項に記載の空間安定装置。
[付記１７]
付記１から８のいずれか１項に記載の空間安定装置はさらに、前記第１の制御対象の第１の角度に対する角速度検出手段をさらに有し、
前記検出された角速度である角速度評価信号とは、前記角速度検出手段により算出された角速度であることを特徴とする付記１から８のいずれか１項に記載の空間安定装置。

この出願は、２０１３年２月２０日に出願された日本出願特願２０１３−０３１２３５を基礎とする優先権を主張し、その開示の全てをここに取り込む。

Claims (10)

1. 移動体に対し、第１の回転軸の回りに回転自在に連結される第１の制御対象と、
   前記第１の制御対象に対し、第２の回転軸の回りに回転自在に連結される第２の制御対象と、
   前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力する慣性検出手段と、
   前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する第１の角速度検出手段と、
   前記第１の角度の目標角度信号を出力する第１指令生成手段と、
   前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力する第２指令生成手段と、
   前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する第１の加算手段と、
   前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御する第１の制御手段と、
   前記第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御する第２の制御手段と、
   を備えることを特徴とする空間安定装置。
2. 前記移動体に対する前記第１の制御対象の第１の角度を検出して、第１の角度検出信号を出力する第１の角度検出手段と、
   前記第１の制御対象に対する前記第２の制御対象の第２の角度を検出して、第２の角度検出信号を出力する第２の角度検出手段と、
   をさらに有することを特徴とする請求項１に記載の空間安定装置。
3. 前記第１の計測角速度信号、及び前記第１の角度検出信号又は前記第２の角度検出信号を入力して、前記第１の計測角速度信号を前記第１の角度に関する成分を含む１つ以上のオイラー角成分に変換し、前記第1の計測角速度信号に基づく信号を出力する第１座標変換手段
   をさらに有することを特徴とする請求項１または２に記載の空間安定装置。
4. 前記第２の計測角速度信号、及び前記第２の角度検出信号を入力して、前記第２の計測角速度信号を前記第２の角度に関する成分を含む１つ以上のオイラー角成分に変換し、前記第２の計測角速度信号に基づく信号の１つ以上の成分を出力する第２の座標変換手段と、
   をさらに有することを特徴とする請求項１から３のいずれか１項に記載の空間安定装置。
5. 前記第１指令生成手段により出力された前記第１の角度の目標角度の信号が入力して、前記第１の目標角度へ至る角度軌道である第１の目標角度信号及び前記角度軌道に対応する角速度軌道である前記第１の角度の目標角度信号に基づく信号を出力する第１の軌道生成手段と、
   前記角速度評価信号から前記第１の角度の目標角度信号に基づく信号を減算して偏差角速度信号を出力する第１の減算手段と、
   をさらに有することを特徴とする請求項１から４のいずれか１項に記載の空間安定装置。
6. 前記第１の計測角速度信号の１つの成分を時間積分して第１の計測角度信号の１つの成分を第1制御手段へ出力する第１の積分手段と、
   前記第２の計測角速度信号の１つの成分を時間積分して第２の計測角度信号の１つの成分を第2制御手段へ出力する第２の積分手段と、
   を更に有する、ことを特徴とする請求項１から５のいずれか１項に記載の空間安定装置。
7. 前記第1の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第１の値に従い変化させた信号を出力する第１の位相補償手段と、
   前記第１の位相補償手段が出力する信号のゲインをあらかじめ設定された第２の値に従い変化させた信号を前記第１の積分手段へ出力する第１の増幅手段と、
   前記第２の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第３の値に従い変化させた信号を出力する第２の位相補償手段と、
   前記第２の位相補償手段が出力する信号のゲインをあらかじめ設定された第４の値に従い変化させた信号を前記第２の積分手段へ出力する第２の増幅手段と、
   を更に備えることを特徴とする請求項１から６のいずれか１項に記載の空間安定装置。
8. 前記第１の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第５の値に従い変化させた信号を出力する第３の位相補償手段と、
   前記第３の位相補償手段が出力する信号のゲインをあらかじめ設定された第６の値に従い変化させた信号を前記第１の制御手段へ出力する第３の増幅手段と、
   前記第２の計測角速度信号に基づく信号の１つの成分の位相をあらかじめ設定された第７の値に従い変化させた信号を出力する第４の位相補償手段と、
   前記第４の位相補償手段が出力する信号のゲインをあらかじめ設定された第８の値に従い変化させた信号を前記第２の制御手段へ出力する第４の増幅手段と、
   を更に備えることを特徴とする請求項７に記載の空間安定装置。
9. 移動体に対し、第１の回転軸の回りに回転自在に連結される第１の制御対象と、前記第１の制御対象に対し、第２の回転軸の回りに回転自在に連結される第２の制御対象と、を備えた空間安定装置が備えるコンピュータを、
   前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力する慣性検出手段と、
   前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力する第１の角速度検出手段と、
   前記第１の角度の目標角度信号を出力する第１指令生成手段と、
   前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力する第２指令生成手段と、
   前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力する第１の加算手段と、
   前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御する第１の制御手段と、
   前記第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御する第２の制御手段と、
   して機能させるための空間安定プログラム。
   
10. 移動体に対し第１の回転軸の回りに回転自在に連結される第１の制御対象の、前記移動体に対する第１の角度を検出して、第１の角度検出信号を出力し、
    前記第１の制御対象に対し第２の回転軸の回りに回転自在に連結される第２の制御対象の、前記第１の制御対象に対する第２の角度を検出して、第２の角度検出信号を出力し、
    前記移動体と前記第１の制御対象のいずれかの角速度を計測して前記移動体と前記第１の制御対象のいずれかに固定された直交座標系の３つの成分の１つ以上の成分を含む第１の計測角速度信号を出力し、
    前記移動体に対する前記第１の制御対象の第１の角度に関する評価又は検出された角速度である角速度評価信号を出力し、
    前記第１の角度の目標角度信号を出力し、
    前記第１の制御対象に対する前記第２の制御対象の第２の角度の目標角度信号を出力し、
    前記角速度評価信号に基づく信号と前記第１の計測角速度信号に基づく信号とを加算して、第２の計測角速度信号を出力し、
    前記第１の角度の目標角度信号に基づく信号と、前記第１の計測角速度信号に基づく信号とに基づいて、前記第１の角度を制御し、
    前記第２の角度の目標角度信号に基づく信号と、前記第２の計測角速度信号に基づく信号とに基づいて、前記第２の角度を制御する、
    ことを特徴とする空間安定方法。

Images