JP5097888B2

JP5097888B2 - 空間安定装置の制御ループ

Info

Publication number: JP5097888B2
Application number: JP2007318189A
Authority: JP
Inventors: 紳吾片桐
Original assignee: Tamagawa Seiki Co Ltd
Current assignee: Tamagawa Seiki Co Ltd
Priority date: 2007-12-10
Filing date: 2007-12-10
Publication date: 2012-12-12
Anticipated expiration: 2027-12-10
Also published as: JP2009140391A

Description

本発明は、空間安定装置の制御ループに関し、特に、速度指令を積分処理する第１積分処理部を主制御ループの前段に設け、速度指令及び角度指令の入力状態と第２スイッチ処理部の入力選択とにより、共通の制御ブロックで速度制御ループ及び角度制御ループを構成できるようにすることで、作成を簡略化できるとともに、検証を容易化できるようにするための新規な改良に関するものである。

従来用いられていたこの種の空間安定装置としては、例えば下記の特許文献１等に示されている装置が挙げられる。図２は、従来の空間安定装置を示す構成図である。図において、航空機や車両等の移動体（図示せず）に固定された固定部１には、この固定部１に対して回転自在なＡＺジンバル２が取り付けられ、このＡＺジンバル２には、該ＡＺジンバルに対して回転自在なＥＬジンバル３が取り付けられ、これらＡＺジンバル２及びＥＬジンバル３によりジンバル機構４が構成されている。前記ＥＬジンバル３には、例えば可視カメラ、赤外線カメラ、及びレーダアンテナ等のペイロード５が取り付けられており、ペイロード５の視軸線６はジンバル機構４の動作によりジンバル２，３の回転軸周りに移動可能とされている。

前記ペイロード５には、ジンバル２，３の回転軸周りの回転角速度を検出する角速度センサ７が取り付けられている。また、前記ジンバル機構４の軸部には、各ジンバル２，３の回転角度を検出する例えばレゾルバ等のＡＺ，ＥＬ角度センサ８，９と、各ジンバル２，３を駆動させるＡＺ，ＥＬトルカ１０，１１とが取り付けられている。角速度センサ７及びＡＺ，ＥＬ角度センサ８，９の信号は制御装置１２に入力され、これらの信号と速度指令１３及び角度指令１４とに基づいて、制御装置１２によってＡＺ，ＥＬトルカ１０，１１の動作が制御される。この制御装置１２の制御モードには、速度制御モードと、角度制御モードとが含まれている。

次に、図３は、図２の空間安定装置の速度制御ループ２０を示すブロック図である。図１の制御装置１２による速度制御モードでの制御動作は、この速度制御ループ２０に基づいて行われる。図３において、速度制御ループ２０には、減算部２１と、速度ゲイン処理部２２と、レート積分処理部２３と、第１加算部２４と、制御プラント２５と、積分処理部２７とが含まれている。減算部２１は、前記速度指令１３を入力としており、この速度指令１３と図２の角速度センサ７の出力である対空間速度７ａとの差を出力する。速度ゲイン処理部２２は、減算部２１の出力２１ａを入力としており、この出力２１ａに速度ゲインを乗算する。レート積分処理部２３は、前記速度ゲイン処理部２２と同様に前記減算部２１の前記出力２１ａを入力としており、この出力２１ａを積分するとともに、この積分した出力２１ａにレート積分ゲインを乗算する。第１加算部２４は、速度ゲイン処理部２２の出力２２ａとレート積分処理部２３の出力２３ａとを入力としており、これら出力２２ａ，２３ａを加算する。

制御プラント２５は、図２のＡＺ，ＥＬ角度センサ８，９、及びＡＺ，ＥＬトルカ１０，１１から構成されている。すなわち、前記第１加算部２４の出力２４ａに基づいてＡＺ，ＥＬトルカ１０，１１が駆動されて、ＡＺ，ＥＬ角度センサ８，９の角度信号が微分された状態でプラント出力２５ａとして積分処理部２７に入力される。すなわち、この速度制御ループ２０は、視軸線６の空間に対する速度が速度指令１３に合致するように、ジンバル機構４を駆動する。速度指令１３が零である場合、速度制御ループ２０は、固定部１の移動に拘わらず視軸線６を常に一定方向に向ける空間安定制御を行う。積分処理部２７は、前記プラント出力２５ａを積分して、角度出力２７ａを出力する。

次に、図４は、図２の空間安定装置の角度制御ループ３０を示すブロック図である。図１の制御装置１２による角度制御モードでの制御動作は、この角度制御ループ３０に基づいて行われる。図４において、角度制御ループ３０には、減算部３１と、位置ゲイン処理部３２と、制御プラント２５とが含まれている。前記減算部３１は、前記角度指令１４を入力としており、この角度指令１４と前記制御プラント２５のプラント出力２５ａとの差を出力する。前記位置ゲイン処理部３２は、前記減算部３１の出力３１ａを入力としており、この出力３１ａに位置ゲインを乗算する。

制御プラント２５は、前述の速度制御ループ２０と同様に、図２のＡＺ，ＥＬ角度センサ８，９、及びＡＺ，ＥＬトルカ１０，１１から構成されている。この角度制御ループ３０では、前記位置ゲイン処理部３２の出力３２ａに基づいてＡＺ，ＥＬトルカ１０，１１が駆動されて、ＡＺ，ＥＬ角度センサ８，９の信号がプラント出力２５ａとして前記減算部３１にフィードバックされる。従って、この角度制御ループ３０は、視軸線６の固定部１に対する相対的な角度が角度指令１４に合致するように、ジンバル機構４を駆動する。

特開２００４−３６１１２１号公報

上記のような従来の空間安定装置の制御ループでは、速度制御ループ２０と角度制御ループ３０とを互いに独立して設けているので、ループの作成及び検証が煩雑になる問題があった。

本発明は、上記のような課題を解決するためになされたものであり、その目的は、作成を簡略化できるとともに、検証を容易化できる空間安定装置の制御ループを提供することである。

本発明に係る空間安定装置の制御ループは、固定部に対して回転自在なＡＺジンバルと、前記ＡＺジンバルに対して回転自在であるとともにペイロードが取付けられたＥＬジンバルと、前記ペイロードに取付けられ前記ＡＺジンバル及び前記ＥＬジンバルの回転軸周りの回転角速度を検出する角速度センサと、前記ＡＺジンバル及び前記ＥＬジンバルの回転角度を検出するＡＺ角度センサ及びＥＬ角度センサと、前記ＡＺジンバル及び前記ＥＬジンバルを駆動するＡＺトルカ及びＥＬトルカとを有する空間安定装置を制御するための空間安定装置の制御ループであって、速度指令を入力とする第１積分処理部と、前記第１積分処理部の出力、前記速度指令、及び角度指令を入力とする主制御ループとを備え、前記主制御ループは、前記第１積分処理部の出力と角度指令とを入力とする第１演算処理部と、前記第１演算処理部の出力を入力とする位置ゲイン処理部と、前記位置ゲイン処理部の出力と前記速度指令とを加算する第２演算処理部と、前記第２演算処理部の出力を入力とする速度ゲイン処理部と、前記ＡＺ角度センサ、前記ＥＬ角度センサ、前記ＡＺトルカ及び前記ＥＬトルカから構成されており、前記速度ゲイン処理部の出力を入力とするとともに、前記ＡＺ角度センサ及び前記ＥＬ角度センサの角度信号が微分された状態のプラント出力を出力する制御プラントと、前記制御プラントのプラント出力を入力とする第２積分処理部と、前記プラント出力と前記角速度センサの出力とを入力として、該プラント出力と該角速度センサの出力のいずれか一方を出力として前記第２演算処理部に入力する第１スイッチ処理部と、前記第１スイッチ処理部の前記出力を入力とする第３積分処理部と、前記第３積分処理部の出力と前記第２積分処理部の出力とのいずれか一方を入力とするとともに、出力を前記第１演算処理部に入力する第２スイッチ処理部とからなり、前記角度指令が非入力であるとともに、前記速度指令が前記第１積分処理部及び前記第２演算処理部に入力され、前記第２スイッチ処理部が前記第３積分処理部の出力を出力として前記第１演算処理部に入力した場合に、前記第２演算処理部が前記位置ゲイン処理部の出力と前記速度指令とを加算した信号から前記第１スイッチ処理部の出力を減算するとともに、前記第１演算処理部が前記第１積分処理部の出力から前記第２スイッチ処理部の出力を減算することで、速度制御ループを構成し、前記速度指令が非入力であるとともに、前記角度指令が前記第１演算処理部に入力され、前記第２スイッチ処理部が前記第２積分処理部の出力を出力として前記第１演算処理部に入力した場合に、前記第２演算処理部が前記位置ゲイン処理部の出力を前記速度ゲイン処理部に入力するとともに、前記第１演算処理部が前記角度指令から前記第２スイッチ処理部の出力を減算することで、角度制御ループを構成する。

本発明の空間安定装置の制御ループによれば、速度指令を積分処理する第１積分処理部が主制御ループの前段に設けられ、速度指令及び角度指令の入力状態と第２スイッチ処理部の入力選択とにより、共通の制御ブロックで速度制御ループ及び角度制御ループを構成できるので、作成を簡略化できるとともに、検証を容易化できる。

以下、本発明を実施するための最良の形態について、図面を参照して説明する。
実施の形態１．
図１は、本発明の実施の形態１による空間安定装置の制御ループを示すブロック図である。なお、ジンバル機構４等の機械構成は従来装置と同様であるので、実施の形態の制御ループの説明に図２を援用する。
図において、実施の形態の制御ループは、第１積分処理部４０と、主制御ループ５０とにより構成される。前記第１積分処理部４０は、速度指令１３を入力としている。前記主制御ループ５０は、この第１積分処理部４０の出力４０ａと、前記速度指令１３と、角度指令１４とを入力としている。すなわち、前記第１積分処理部４０は、前記主制御ループ５０の前段に設けられている。なお、第１積分処理部４０及び主制御ループ５０は、ソフトウェアで構成された制御ブロックであるが、アナログ又はデジタル素子で構成することも可能である。

前記主制御ループ５０は、第１演算処理部５１と、位置ゲイン処理部５２と、第２演算処理部５３と、速度ゲイン処理部５４と、制御プラント２５と、第２積分処理部５５と、第１スイッチ処理部５７と、第３積分処理部５８と、第２スイッチ処理部５９とから構成されている。

前記第１演算処理部５１は、前記第１積分処理部４０の出力４０ａ、前記角度指令１４、及び第２スイッチ処理部５９の出力５９ａを入力としている。位置ゲイン処理部５２は、前記第１演算処理部５１の出力５１ａを入力としている。第２演算処理部５３は、前記速度指令１３、前記位置ゲイン処理部５２の出力５２ａ、及び第１スイッチ処理部５７の出力５７ａを入力としている。速度ゲイン処理部５４は、前記第２演算処理部５３の出力５３ａを入力としている。

制御プラント２５は、図２のＡＺ，ＥＬ角度センサ８，９、及びＡＺ，ＥＬトルカ１０，１１から構成されている。すなわち、前記速度ゲイン処理部５４の出力５４ａに基づいてＡＺ，ＥＬトルカ１０，１１が駆動されて、ＡＺ，ＥＬ角度センサ８，９の角度信号が微分された状態でプラント出力２５ａ（速度信号）として出力される。

第２積分処理部５５は、プラント出力２５ａを入力としている。

第１スイッチ処理部５７は、制御プラント２５のプラント出力２５ａ（軸レート）と図２の角速度センサ７の出力である対空間速度７ａ（対空間レート）とを入力としており、こられ制御プラント２５のプラント出力２５ａと角速度センサ７の対空間速度７ａとのいずれか一方を出力する。第３積分処理部５８は、第１スイッチ処理部５７の出力５７ａを入力としている。第２スイッチ処理部５９は、前記第３積分処理部５８の出力５８ａと前記第２積分処理部５５の出力５５ａとを入力として、これら第３積分処理部５８の出力５８ａと第２積分処理部５５の出力５５ａとのいずれか一方を出力する。

次に、図１の制御ループに基づく制御動作について説明する。前記速度指令１３及び角度指令１４は、いずれか一方が入力される場合に、他方は非入力とされる。まず、速度指令１３が入力される場合について説明する。

速度指令１３が入力されると、第１積分処理部４０は前記速度指令１３を積分する。第２スイッチ処理部５９は、速度指令１３が入力される場合に第３積分処理部５８の出力５８ａを第１演算処理部５１に入力する。

前記第１演算処理部５１は、第１積分処理部４０の出力４０ａから第３積分処理部５８の出力５８ａを減算し、位置ゲイン処理部５２は、第１演算処理部５１の出力５１ａに位置ゲインを乗算する。第２演算処理部５３は、位置ゲイン処理部５２の出力５２ａと速度指令１３とを加算するとともに、この加算したものから第１スイッチ処理部５７の出力５７ａを減算する。速度ゲイン処理部５４は、この第２演算処理部５３の出力５３ａに速度ゲインを乗算する。制御プラント２５では、速度ゲイン処理部５４の出力５４ａに基づいてＡＺ，ＥＬトルカ１０，１１が駆動されて、ＡＺ，ＥＬ角度センサ８，９の角度信号が微分された状態でプラント出力２５ａ（速度信号）として出力される。

ここで、第１スイッチ処理部５７が、角速度センサ７の対空間速度７ａを、前記第２演算処理部５３と第３積分処理部５８とにフィードバックしているとする。第３積分処理部５８は、角速度センサ７の対空間速度７ａを積分する。この第３積分処理部５８は、前記第１積分処理部４０で積分された速度指令１３から、フィードバックされた第３積分処理部５８の出力５８ａを減算するために、ディメンションを合わせるためのものである。すなわち、第１積分処理部４０、第１演算処理部５１、位置ゲイン処理部５２、及び速度ゲイン処理部５４は、従来の制御ループ（図３）の減算部２１及びレート積分処理部２３に相当する動作を行う。

また、第２演算処理部５３は、速度指令１３から角速度センサ７の対空間速度７ａを減算する。第２演算処理部５３及び速度ゲイン処理部５４は、従来の制御ループ（図３）の減算部２１及び速度ゲイン処理部２２に相当する動作を行う。

従って、前記角度指令１４が非入力であるとともに、前記速度指令１３が前記第１積分処理部４０及び前記第２演算処理部５３に入力され、前記第２スイッチ処理部５９が前記第３積分処理部５８の出力５８ａを入力とした場合に、速度制御ループ２０が構成される。

第１スイッチ処理部５７が、角速度センサ７の対空間速度７ａを、前記第２演算処理部５３と第３積分処理部５８とにフィードバックする場合、速度制御ループ２０は、図２の視軸線６の空間に対する速度が速度指令１３に合致するように、ジンバル機構４を駆動する。これに対して、第１スイッチ処理部５７が、制御プラント２５のプラント出力２５ａを、前記第２演算処理部５３と第３積分処理部５８とにフィードバックする場合、速度制御ループ２０は、視軸線６の固定部１に対する速度が速度指令１３に合致するように、ジンバル機構４を駆動する。

次に、前記角度指令１４が入力される場合について説明する。第２スイッチ処理部５９は、角度指令１４が入力される場合に第２積分処理部５５の角度出力５５ａを第１演算処理部５１に入力する。第１演算処理部５１は、角度指令１４から第２積分処理部５５の角度出力５５ａを減算し、位置ゲイン処理部５２は、第１演算処理部５１の出力５１ａに位置ゲインを乗算する。また、第２演算処理部５３は、位置ゲイン処理部５２の出力５２ａを速度ゲイン処理部５４に入力する。速度ゲイン処理部５４は、この第２演算処理部５３の出力５３ａに速度ゲインを乗算する。制御プラント２５では、速度ゲイン処理部５４の出力５４ａに基づいてＡＺ，ＥＬトルカ１０，１１が駆動されて、ＡＺ，ＥＬ角度センサ８，９の信号が微分された状態でプラント出力２５ａ（速度信号）として出力される。第２積分処理部５５は、前記プラント出力２５ａを積分して、角度出力５５ａを出力する。すなわち、角度指令１４が入力される場合、位置ゲイン処理部５２及び速度ゲイン処理部５４は従来の制御ループ（図３）の位置ゲイン処理部３２に相当する動作を行う。

従って、前記速度指令１３が非入力であるとともに、前記角度指令１４が前記第１演算処理部５１に入力され、前記第２スイッチ処理部５９が前記第２積分処理部５５の角度出力５５ａを入力とした場合に、角度制御ループ３０が構成される。この角度制御ループ３０は、視軸線６の固定部１に対する相対的な角度が角度指令１４に合致するように、ジンバル機構４を駆動する。

本発明の実施の形態１による空間安定装置の制御ループを示すブロック図である。従来の空間安定装置を示す構成図である。図２の空間安定装置の速度制御ループを示すブロック図である。図２の空間安定装置の角度制御ループを示すブロック図である。

７ａ対空間速度
１３速度指令
１４角度指令
２０速度制御ループ
２５制御プラント
３０角度制御ループ
４０第１積分処理部
５０主制御ループ
５１第１演算処理部
５２位置ゲイン処理部
５３第２演算処理部
５４速度ゲイン処理部
５５第２積分処理部
５７第１スイッチ処理部
５８第３積分処理部
５９第２スイッチ処理部

Claims

固定部（１）に対して回転自在なＡＺジンバル（２）と、
前記ＡＺジンバル（２）に対して回転自在であるとともにペイロード（５）が取付けられたＥＬジンバル（３）と、
前記ペイロード（５）に取付けられ前記ＡＺジンバル（２）及び前記ＥＬジンバル（３）の回転軸周りの回転角速度を検出する角速度センサ（７）と、
前記ＡＺジンバル（２）及び前記ＥＬジンバル（３）の回転角度を検出するＡＺ角度センサ（８）及びＥＬ角度センサ（９）と、
前記ＡＺジンバル（２）及び前記ＥＬジンバル（３）を駆動するＡＺトルカ（１０）及びＥＬトルカ（１１）と
を有する空間安定装置を制御するための空間安定装置の制御ループであって、
速度指令（１３）を入力とする第１積分処理部（４０）と、
前記第１積分処理部（４０）の出力（４０ａ）、前記速度指令（１３）、及び角度指令（１４）を入力とする主制御ループ（５０）と
を備え、
前記主制御ループ（５０）は、
前記第１積分処理部（４０）の出力（４０ａ）と角度指令（１４）とを入力とする第１演算処理部（５１）と、
前記第１演算処理部（５１）の出力（５１ａ）を入力とする位置ゲイン処理部（５２）と、
前記位置ゲイン処理部（５２）の出力（５２ａ）と前記速度指令（１３）とを加算する第２演算処理部（５３）と、
前記第２演算処理部（５３）の出力（５３ａ）を入力とする速度ゲイン処理部（５４）と、
前記ＡＺ角度センサ（８）、前記ＥＬ角度センサ（９）、前記ＡＺトルカ（１０）及び前記ＥＬトルカ（１１）から構成されており、前記速度ゲイン処理部（５４）の出力（５４ａ）を入力とするとともに、前記ＡＺ角度センサ（８）及び前記ＥＬ角度センサ（９）の角度信号が微分された状態のプラント出力（２５ａ）を出力する制御プラント（２５）と、
前記制御プラント（２５）のプラント出力（２５ａ）を入力とする第２積分処理部（５５）と、
前記プラント出力（２５ａ）と前記角速度センサ（７）の出力（７ａ）とを入力として、該プラント出力（２５ａ）と該角速度センサ（７）の出力（７ａ）のいずれか一方を出力（５７ａ）として前記第２演算処理部（５３）に入力する第１スイッチ処理部（５７）と、
前記第１スイッチ処理部（５７）の前記出力（５７ａ）を入力とする第３積分処理部（５８）と、
前記第３積分処理部（５８）の出力（５８ａ）と前記第２積分処理部（５５）の出力（５５ａ）とを入力として、該第３積分処理部（５８）の出力（５８ａ）と該第２積分処理部（５５）の出力（５５ａ）とのいずれか一方を出力（５９ａ）として前記第１演算処理部（５１）に入力する第２スイッチ処理部（５９）と
からなり、
前記角度指令（１４）が非入力であるとともに、前記速度指令（１３）が前記第１積分処理部（４０）及び前記第２演算処理部（５３）に入力され、前記第２スイッチ処理部（５９）が前記第３積分処理部（５８）の出力（５８ａ）を出力（５９ａ）として前記第１演算処理部（５１）に入力した場合に、前記第２演算処理部（５３）が前記位置ゲイン処理部（５２）の出力（５２ａ）と前記速度指令（１３）とを加算した信号から前記第１スイッチ処理部（５７）の出力（５７ａ）を減算するとともに、前記第１演算処理部（５１）が前記第１積分処理部（４０）の出力（４０ａ）から前記第２スイッチ処理部（５９）の出力（５９ａ）を減算することで、速度制御ループ（２０）を構成し、
前記速度指令（１３）が非入力であるとともに、前記角度指令（１４）が前記第１演算処理部（５１）に入力され、前記第２スイッチ処理部（５９）が前記第２積分処理部（５５）の出力（５５ａ）を出力（５９ａ）として前記第１演算処理部（５１）に入力した場合に、前記第２演算処理部（５３）が前記位置ゲイン処理部（５２）の出力（５２ａ）を前記速度ゲイン処理部（５４）に入力するとともに、前記第１演算処理部（５１）が前記角度指令（１４）から前記第２スイッチ処理部（５９）の出力（５９ａ）を減算することで、角度制御ループ（３０）を構成することを特徴とする空間安定装置の制御ループ。