JP2016224530A - ジンバル制御システム - Google Patents

Abstract

【課題】ジンバルを所定位置に滑らかに停止させることができるとともに、ジンバルが機械的制限に衝突するおそれを低減できるジンバル制御システムを提供する。
【解決手段】本発明によるジンバル制御システムは、ジンバル２の角度及び角速度を検出するためのセンサ１０と、ジンバル２を回転駆動するアクチュエータ１１と、センサ１０及びアクチュエータ１１に接続されて、センサ１０の出力に基づいてアクチュエータ１１によるジンバル２の回転駆動を制御する制御手段１２とを備えている。ジンバル２には、操作許容範囲が予め設定されている。制御手段１２は、センサ１０の出力に基づいて検出されるジンバル２の検出角が操作許容範囲を超えた場合に、操作許容範囲の外側に設定された停止角に検出角が近づくにつれてジンバル２の角速度を小さくする駆動信号をアクチュエータ１１に入力する。
【選択図】図１

Description

本発明は、ジンバルの動作を制御するジンバル制御システムに関する。

従来用いられていたこの種のジンバル制御システムとしては、例えば下記の特許文献１等に示されている構成を挙げることができる。すなわち、従来構成では、ジンバルの角度が、予め設定された操作限界角に一致するか又はこの操作限界角を超えた場合に、ジンバルを駆動するアクチュエータへの駆動信号を停止することによりジンバルを停止させている。

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

上記のような従来のジンバル制御システムでは、ジンバルの角度が、予め設定された操作限界角に一致するか又はこの操作限界角を超えた場合に、ジンバルを駆動するアクチュエータへの駆動信号を停止することによりジンバルを停止させているので、以下のような問題が生じるおそれがある。すなわち、設定角の周辺でアクチュエータへの駆動信号が停止されるので、ジンバルの挙動が急に変わってしまう。このため、ペイロードとしてカメラがジンバルに搭載されている場合に、カメラの映像にブレが生じてしまう。また、ジンバルが惰性で変位して機械的制限に衝突することもある。

本発明は、上記のような課題を解決するためになされたものであり、その目的は、ジンバルを所定位置に滑らかに停止させることができるとともに、ジンバルが機械的制限に衝突するおそれを低減できるジンバル制御システムを提供することである。

本発明に係るジンバル制御システムは、ジンバルの角度及び角速度を検出するためのセンサと、ジンバルを回転駆動するアクチュエータと、センサ及びアクチュエータに接続されて、センサの出力に基づいてアクチュエータによるジンバルの回転駆動を制御する制御手段とを備え、ジンバルには、操作許容範囲が予め設定されており、制御手段は、センサの出力に基づいて検出されるジンバルの検出角が操作許容範囲を超えた場合に、操作許容範囲の外側に設定された停止角に検出角が近づくにつれてジンバルの角速度を小さくする駆動信号をアクチュエータに入力する。

本発明のジンバル制御システムによれば、制御手段は、センサの出力に基づいて検出されるジンバルの検出角が操作許容範囲を超えた場合に、操作許容範囲の外側に設定された停止角に検出角が近づくにつれてジンバルの角速度を小さくする駆動信号をアクチュエータに入力するので、ジンバルを所定位置に滑らかに停止させることができるとともに、ジンバルが機械的制限に衝突するおそれを低減できる。

本発明の実施の形態１によるジンバル制御システムの構成を示すブロック図である。 図１の制御手段の構成を示すブロック図である。 図１のジンバルの角度位置を概略的に示す説明図である。 図２の角速度指令処理部による処理選択動作を示すフローチャートである。 図４のＡ〜Ｂ区間処理を示すフローチャートである。 図４のＢ〜Ｃ区間処理を示すフローチャートである。 図４の−Ａ〜−Ｂ区間処理を示すフローチャートである。 図４の−Ｂ〜−Ｃ区間処理を示すフローチャートである。

以下、本発明を実施するための形態について、図面を参照して説明する。
実施の形態１．
図１は、本発明の実施の形態１によるジンバル制御システム１の構成を示すブロック図である。図において、ジンバル制御システム１は、ジンバル２の動作を制御するためのものである。周知のように、ジンバル２には、例えばカメラ等のペイロードが搭載される。ジンバル制御システム１には、センサ１０、アクチュエータ１１及び制御手段１２が含まれている。

センサ１０は、例えばエンコーダやレゾルバ等によって構成されるものであり、ジンバル２の角度及び角速度を検出するための信号を出力する。アクチュエータ１１は、例えばトルカ等によって構成されるものであり、ジンバル２を回転駆動するためのものである。

制御手段１２は、例えばマイクロプロセッサ等の演算装置によって構成されるものであり、センサ１０及びアクチュエータ１１に接続されている。制御手段１２は、センサ１０の出力に基づいてアクチュエータ１１によるジンバル２の回転駆動を制御する。

次に、図２は、図１の制御手段１２の構成を示すブロック図である。図２に示すように、制御手段１２には、角速度指令生成部１２０、角速度指令処理部１２１、電流指令生成部１２２及びＤ／Ａコンバータ１２３が含まれている。

角速度指令生成部１２０は、センサ１０の出力に基づいて検出されるジンバル２の検出角と外部からの位置指令とに基づいて角速度指令を生成する。具体的には、角速度指令生成部１２０は、ジンバル２の検出角と外部からの位置指令との偏差を０にしするための角速度指令を生成する。

角速度指令処理部１２１は、ジンバル２の検出角が後述の操作許容範囲を超えていないか否かを判定する。後に詳しく説明するように、ジンバル２の検出角が操作許容範囲を超えている場合、角速度指令処理部１２１は、角速度指令に処理を加えて、その処理を加えた角速度指令を電流指令生成部１２２に入力する。ジンバル２の検出角が操作許容範囲を超えていない場合、角速度指令処理部１２１は、角速度指令生成部１２０からの角速度指令をそのまま電流指令生成部１２２に入力する。

電流指令生成部１２２は、角速度指令処理部１２１を介して入力された角速度指令に基づいて電流指令を生成する。電流指令は、角速度指令処理部１２１を介して入力された角速度指令と、ジンバル２の出力に基づいて検出される検出角速度との偏差を０にしようとするものである。Ｄ／Ａコンバータ１２３は、電流指令生成部１２２によって生成された電流指令に基づいてアクチュエータ１１に電流を供給する。Ｄ／Ａコンバータ１２３からアクチュエータ１１に供給される電流は、アクチュエータ１１を駆動する駆動信号である。

次に、図３は、図１のジンバル２の角度位置を概略的に示す説明図である。図３に示すように、ジンバル２には、利用者によるジンバル２の角度位置の指定が許容される操作許容範囲が設けられている。本実施の形態では、操作許容範囲は、限界角±Ａ°によって画定されるものとする。

操作許容範囲の外側には、制限角及び停止角が設定されている。本実施の形態では、制限角を±Ｃ°とし、停止角を±Ｂ°とする。

制限角（±Ｃ°）とは、ジンバル２の移動が制限されるべき角度であり、ジンバル２が他の部材と衝突するおそれがある機械的限界角とされることができる。操作許容範囲の限界角と制限角との間（Ａ°〜Ｃ°及び−Ａ°〜−Ｃ°）には、所定の角度範囲が設けられる。

停止角（±Ｂ°）は、ジンバル２の検出角が操作許容範囲を超えた場合に、ジンバル２を停止させようとする角度である。停止角（±Ｂ°）は、操作許容範囲の限界角と制限角との間（Ａ°〜Ｃ°及び−Ａ°〜−Ｃ°）の任意の角度とすることができるが、操作許容範囲の限界角と制限角とから均等に離れた中間角（Ｂ°＝（Ｃ°−Ａ°）／２，−Ｂ°＝（−Ｃ＋Ａ）／２）とすることが好ましい。

図２の制御手段１２は、ジンバル２の検出角が操作許容範囲（−Ａ°〜Ａ°）を超えた場合に、検出角が停止角（±Ｂ°）に近づくにつれてジンバル２の角速度を小さくする駆動信号をアクチュエータ１１に入力する。本実施の形態では、図２の角速度指令処理部１２１が角速度指令に処理を加えることで、このような駆動信号が実現される。

以下、図４〜図８を用いて、角速度指令処理部１２１の動作をより詳しく説明する。

図４は、図２の角速度指令処理部１２１による処理選択動作を示すフローチャートである。図４において、角速度指令処理部１２１は、角速度指令生成部１２０から角速度指令が入力されると、センサ１０の出力に基づいて検出されるジンバル２の検出角Ｄが操作許容範囲を超えているか否かを判定する（ステップＳ４０，Ｓ４１）。すなわち、角速度指令処理部１２１は、ジンバル２の検出角ＤがＡ°よりも大きいか否かを判定するとともに（ステップＳ４０）、ジンバル２の検出角が−Ａ°よりも小さいか否かを判定する（ステップＳ４１）。角速度指令処理部１２１は、ジンバル２の検出角Ｄが操作許容範囲を超えていないと判定した場合、すなわちジンバル２の検出角がＡ°以下であるとともに−Ａ°以上であると判定した場合、角速度指令生成部１２０から入力された角速度指令をそのまま電流指令生成部１２２に入力する通過処理を行う（ステップＳ４２）。

これに対して、ステップＳ４０においてジンバル２の検出角ＤがＡ°よりも大きいと判定した場合、角速度指令処理部１２１は、ジンバル２の検出角ＤがＢ°よりも大きいか否かを判定する（ステップＳ４３）。このとき、ジンバル２の検出角ＤがＢ°以下であると判定すると、角速度指令処理部１２１は、Ａ〜Ｂ区間処理を行う（ステップＳ４４）。また、ステップＳ４３においてジンバル２の検出角ＤがＢ°よりも大きいと判定すると、角速度指令処理部１２１は、Ｂ〜Ｃ区間処理を行う（ステップＳ４５）。これらＡ〜Ｂ区間処理及びＢ〜Ｃ区間処理については、後に詳しく説明する。

一方、ステップＳ４１においてジンバル２の検出角Ｄが−Ａ°よりも小さいと判定した場合、角速度指令処理部１２１は、ジンバル２の検出角Ｄが−Ｂ°よりも小さいか否かを判定する（ステップＳ４６）。このとき、ジンバル２の検出角Ｄが−Ｂ°以上であると判定すると、角速度指令処理部１２１は、−Ａ〜−Ｂ区間処理を行う（ステップＳ４７）。また、ジンバル２の検出角Ｄが−Ｂ°よりも小さいと判定すると、角速度指令処理部１２１は、−Ｂ〜−Ｃ区間処理を行う（ステップＳ４８）。これら−Ａ〜−Ｂ区間処理及び−Ｂ〜−Ｃ区間処理についても、後に詳しく説明する。

この図４の処理選択動作は、角速度指令生成部１２０から角速度指令処理部１２１に角速度指令が入力されるたびに繰り返し行われる。角速度指令生成部１２０は、所定の時間間隔で角速度指令を角速度指令処理部１２１に入力する。

次に、図５は、図４のＡ〜Ｂ区間処理を示すフローチャートである。図において、Ａ〜Ｂ区間処理では、角速度指令処理部１２１は、前回のジンバル２の検出角ＤがＡ°よりも小さかったか否かを判定する（ステップＳ５０）。このとき、前回のジンバル２の検出角ＤがＡ°よりも小さかったと判定すると、角速度指令処理部１２１は、そのときの角速度指令生成部１２０からの入力角速度指令Ｃｉｎ（検出角度がＡ°を超えた際の入力角速度指令Ｃｉｎ）をラッチする（ステップＳ５１）。これに対して、ステップＳ５０において前回のジンバル２の検出角ＤがＡ°以上であると判定した場合、角速度指令処理部１２１は、入力角速度指令Ｃｉｎのラッチ動作を改めて行わず、すでにラッチされている入力角速度指令Ｃｉｎを以下の処理に用いる。

その次に、角速度指令処理部１２１は、角速度指令生成部１２０からの入力角速度指令Ｃｉｎが角度増大方向の角速度指令であるか否かを判定する（ステップＳ５２）。この判定に用いられる入力角速度指令Ｃｉｎは、ステップＳ５１でラッチされた入力角速度指令Ｃｉｎとは限らない。すなわち、ステップＳ５０において前回のジンバル２の検出角ＤがＡ°以上であると判定された場合には、すでにラッチされている入力角速度指令Ｃｉｎではなく、新たに角速度指令生成部１２０から入力された入力角速度指令Ｃｉｎについて判定が行われる。

このステップＳ５２において、入力角速度指令Ｃｉｎが角度減少方向の角速度指令であると判定した場合、角速度指令処理部１２１は、図４の通過処理を行い、入力角速度指令Ｃｉｎがそのまま電流指令生成部１２２に入力される。これは、ジンバル２の検出角がＡ°からＢ°までの範囲に含まれるときに、入力角速度指令Ｃｉｎが角度減少方向の角速度指令である場合、その入力角速度指令Ｃｉｎがジンバル２を操作許容範囲に戻そうとするものであるため、その入力角速度指令Ｃｉｎをそのまま利用しようとするものである。

一方、ステップＳ５２において、入力角速度指令Ｃｉｎが角度増大方向の角速度指令であると判定した場合、角速度指令処理部１２１は、停止角Ｂ°に検出角が近づくにつれてジンバル２の角速度を小さくする駆動信号を生成するために、角速度指令の処理を行う（ステップＳ５３〜５５）。

この処理には、検出角Ｄ°が停止角Ｂ°のときに値が０となるとともに、検出角Ｄ°が停止角Ｂ°から操作許容範囲の限界角Ａ°に近づくにつれて値が大きくなる三角関数が利用される。本実施の形態では、ｃｏｓ｛０．５π×（｜Ｄ｜−Ａ）／（Ｂ−Ａ）｝が用いられる。以下、この三角関数の位相０．５π×（｜Ｄ｜−Ａ）／（Ｂ−Ａ）を単にＰで表す場合もある。

ステップＳ５２において入力角速度指令Ｃｉｎが角度増大方向の角速度指令であると判定した場合、角速度指令処理部１２１は、ジンバル２の検出角Ｄを用いて、上記三角関数の位相Ｐを算出する（ステップＳ５３）。

その次に、角速度指令処理部１２１は、位相Ｐの制限処理を行う（ステップＳ５４）。この位相Ｐの制限処理では、算出された位相Ｐが０よりも小さいときに位相Ｐを０とみなし、算出された位相Ｐが０．５πよりも大きいときに位相Ｐを０．５πとみなす。

その次に、角速度指令処理部１２１は、ラッチしている入力角速度指令Ｃｉｎに上記した三角関数ｃｏｓ｛０．５π×（｜Ｄ｜−Ａ）／（Ｂ−Ａ）｝を乗算して、角速度指令を生成する（ステップＳ５５）。そして、この角速度指令処理部１２１によって生成された角速度指令が電流指令生成部１２２に入力されて、ジンバル２の検出角Ｄ°が停止角Ｂ°に近づくにつれてジンバル２の角速度を小さくする駆動信号が生成される。図３では、ラッチされた入力角速度指令Ｃｉｎ毎の角速度指令の変化が示されている。

次に、図６は、図４のＢ〜Ｃ区間処理を示すフローチャートである。図において、Ｂ〜Ｃ区間処理では、角速度指令処理部１２１は、ジンバル２の検出角Ｄを用いて、三角関数の位相を算出する（ステップＳ６０）。このＢ〜Ｃ区間処理で用いられる三角関数は、検出角Ｄが停止角Ｂ°のときに値が０となるとともに、検出角Ｄが停止角Ｂ°から制限角Ｃ°に近づくにつれて値が大きくなる三角関数である。本実施の形態では、ｓｉｎ｛０．５π×（｜Ｄ｜−Ｂ）／（Ｃ−Ｂ）｝が用いられる。以下、この三角関数の位相０．５π×（｜Ｄ｜−Ｂ）／（Ｃ−Ｂ）を単にＰで表す場合もある。

その次に、角速度指令処理部１２１は、位相Ｐの制限処理を行う（ステップＳ６１）。この位相Ｐの制限処理では、算出された位相Ｐが０よりも小さいときに位相Ｐを０とみなし、算出された位相Ｐが０．５πよりも大きいときに位相Ｐを０．５πとみなす。

その次に、角速度指令処理部１２１は、予め設定されている最大負角速度指令−Ｃｍａｘに上記した三角関数ｓｉｎ｛０．５π×（｜Ｄ｜−Ｂ）／（Ｃ−Ｂ）｝を乗算して、角速度指令を生成する（ステップＳ６２）。そして、この角速度指令処理部１２１によって生成された角速度指令が電流指令生成部１２２に入力されて、検出角Ｄが停止角Ｂ°を超えているときに、ジンバル２がより速く停止角Ｂ°まで戻されようとする。

次に、図７は、図４の−Ａ〜−Ｂ区間処理を示すフローチャートである。図において、−Ａ〜−Ｂ区間処理では、角速度指令処理部１２１は、前回のジンバル２の検出角Ｄが−Ａ°よりも大きかったか否かを判定し（ステップＳ７０）、前回のジンバル２の検出角Ｄが−Ａ°よりも大きかったと判定した場合、そのときの角速度指令生成部１２０からの入力角速度指令Ｃｉｎ（検出角度が−Ａ°を超えた際の入力角速度指令Ｃｉｎ）をラッチする（ステップＳ７１）。この後の処理は、図５のステップＳ５２〜５５と同じである。−Ａ〜−Ｂ区間処理で用いられる三角関数は、検出角Ｄ°が停止角−Ｂ°のときに値が０となるとともに、検出角Ｄ°が停止角−Ｂ°から操作許容範囲の限界角−Ａ°に近づくにつれて値が大きくなる三角関数である。この三角関数としても、Ａ〜Ｂ区間処理で用いた三角関数と同じく、ｃｏｓ｛０．５π×（｜Ｄ｜−Ａ）／（Ｂ−Ａ）｝を用いることができる。

次に、図８は、図４の−Ｂ〜−Ｃ区間処理を示すフローチャートである。図において、−Ｂ〜−Ｃ区間処理では、角速度指令処理部１２１は、ジンバル２の検出角Ｄを用いて、三角関数の位相を算出する（ステップＳ８０）。この−Ｂ〜−Ｃ区間処理で用いられる三角関数は、検出角Ｄが停止角−Ｂ°のときに値が０となるとともに、検出角Ｄが停止角−Ｂ°から制限角−Ｃ°に近づくにつれて値が大きくなる三角関数である。本実施の形態では、Ｂ〜Ｃ区間処理と同様にｓｉｎ｛０．５π×（｜Ｄ｜−Ｂ）／（Ｃ−Ｂ）｝が用いられる。

その次に、角速度指令処理部１２１は、位相Ｐの制限処理を行う（ステップＳ８１）。この位相Ｐの制限処理では、算出された位相Ｐが０よりも小さいときに位相Ｐを０とみなし、算出された位相Ｐが０．５πよりも大きいときに位相Ｐを０．５πとみなす。

その次に、角速度指令処理部１２１は、予め設定されている最大正角速度指令＋Ｃｍａｘに上記した三角関数ｓｉｎ｛０．５π×（｜Ｄ｜−Ｂ）／（Ｃ−Ｂ）｝を乗算して、角速度指令を生成する（ステップＳ８２）。そして、この角速度指令処理部１２１によって生成された角速度指令が電流指令生成部１２２に入力されて、検出角Ｄが停止角−Ｂ°を超えているときに、ジンバル２がより速く停止角−Ｂ°まで戻されようとする。

このようなジンバル制御システム１では、制御手段１２は、センサ１０の出力に基づいて検出されるジンバル２の検出角が操作許容範囲を超えた場合に、操作許容範囲の外側に設定された停止角に検出角が近づくにつれてジンバルの角速度を小さくする駆動信号をアクチュエータ１１に入力するので、ジンバル２を所定位置に滑らかに停止させることができる。また、ジンバル２を解放せずに停止まで動作を制御するので、駆動信号を停止する従来構成よりも、ジンバル２が機械的制限に衝突するおそれを低減できる。

また、角速度指令処理部１２１は、検出角が停止角のときに値が０となるとともに、検出角が停止角から操作許容範囲の限界角に近づくにつれて値が大きくなる三角関数を角速度指令に乗算することで、検出角が停止角に近づくにつれてジンバルの角速度を小さくするので、上述の駆動信号をより確実に生成することができる。

さらに、角速度指令処理部１２１は、検出角が操作許容範囲を超えた際の角速度指令生成部からの角速度指令をラッチし、ラッチされた角速度指令に三角関数を乗算するので、操作許容範囲を超えた際にジンバル２の挙動が大きく変わることを回避することができる。

さらにまた、角速度指令処理部１２１は、検出角が停止角を超えた場合に、検出角が停止角のときに値が０となるとともに、検出角が停止角から制限角に近づくにつれて値が大きくなる三角関数を用いて角速度指令を生成して、ジンバルを停止角に戻すので、より速くジンバル２を停止角に停止させることができる。

なお、本実施の形態では、操作許容範囲の限界角、停止角及び制限角の正側の値と負側の値とが同じ絶対値を有するように説明しているが、これらの正側の値と負側の値とは互いに異なる絶対値が異なる値であってもよい。

１ ジンバル制御システム
２ ジンバル
１０ センサ
１１ アクチュエータ
１２ 制御手段
１２０ 角速度指令生成部
１２１ 角速度指令処理部

Claims (4)

1. ジンバルの角度及び角速度を検出するためのセンサと、
   前記ジンバルを回転駆動するアクチュエータと、
   前記センサ及び前記アクチュエータに接続されて、前記センサの出力に基づいて前記アクチュエータによる前記ジンバルの回転駆動を制御する制御手段と
   を備え、
   前記ジンバルには、操作許容範囲が予め設定されており、
   前記制御手段は、前記センサの出力に基づいて検出される前記ジンバルの検出角が前記操作許容範囲を超えた場合に、前記操作許容範囲の外側に設定された停止角に前記検出角が近づくにつれて前記ジンバルの角速度を小さくする駆動信号を前記アクチュエータに入力する
   ことを特徴とするジンバル制御システム。
2. 前記制御手段は、
   角速度指令を生成する角速度指令生成部と、
   前記ジンバルの検出角が前記操作許容範囲を超えた場合に前記角速度指令に処理を加える角速度指令処理部と
   を含み、
   前記角速度指令処理部は、前記検出角が前記停止角のときに値が０となるとともに、前記検出角が前記停止角から前記操作許容範囲の限界角に近づくにつれて値が大きくなる三角関数を前記角速度指令に乗算することで、前記検出角が前記停止角に近づくにつれて前記ジンバルの角速度を小さくする
   ことを特徴とする請求項１記載のジンバル制御システム。
3. 前記角速度指令処理部は、前記検出角が前記操作許容範囲を超えた際の前記角速度指令生成部からの前記角速度指令をラッチし、ラッチされた前記角速度指令に前記三角関数を乗算する
   ことを特徴とする請求項２記載のジンバル制御システム。
4. 前記角速度指令処理部は、前記検出角が前記停止角を超えた場合に、前記検出角が前記停止角のときに値が０となるとともに、前記検出角が前記停止角から前記停止角の外側に設定された制限角に近づくにつれて値が大きくなる三角関数を用いて角速度指令を生成して、前記ジンバルを前記停止角に戻す
   ことを特徴とする請求項２又は請求項３に記載のジンバル制御システム。

Images