JP2006106910A - ジンバル装置 - Google Patents

Abstract

【課題】 実現可能なジンバル可動範囲を越えることなく、ジンバルロックを避け、全天にわたり連続的な追尾制御を安定して実現することができるジンバル装置を提供する。
【解決手段】 ジンバル装置１は、天頂からＥＬ軸２０の回転方向と直交する方向に目標が移動する場合、角速度が過大にならないように、Ａｚ軸１０とｘＥＬ軸３０との動作を配分しながら追尾動作を行う。
【選択図】 図１

Description

本発明は、ジンバル装置に関し、特に地上に設置され若しくは車両、船舶、航空機等に搭載され、任意の位置に存在する目標を追跡するジンバル装置に関する。

例えば、保安監視や交通情報支援に使用される工業用テレビカメラを備えた追跡装置においては、図１５に示すように、垂直軸（アジマス（Ａｚ）軸）と仰軸（エレベーション（ＥＬ）軸）の２つのジンバル軸を設定することにより、空間の任意の方向に存在する目標を指向することができる。ジンバル装置においては、全天にわたり、目標の動きに対して遅れなくカメラの視軸の制御を行い、目標を連続的に追跡することが要求されている。

ところが、２軸構造を有するジンバル装置には、図１６に示すように、天頂１０１の方向においてジンバルの角速度が制御不能になるジンバルロックと呼ばれる現象が存在する。天頂１０１の方向はＥＬ角が９０度になる方向である。

図１７に示すように、２軸構造を有するジンバル装置１１０は、Ａｚ軸を中心軸として回転する第１のジンバル１１１と、ＥＬ軸を中心軸として回転する第２のジンバル１１２と、第２のジンバル１１２に取り付けられたカメラ１１３とを備えている。このジンバル装置１１０においては、ＥＬ軸を中心軸として水平から垂直に向かってカメラ１１３が９０度回転上昇すると、カメラ１１３の視軸は、図１０１に示す天頂１０１の方向と一致する。このカメラ１１３の視軸と天頂１０１の方向とが一致した状態において、追跡中の目標１００が天頂１０１からＥＬ軸に沿う矢印Ａ方向に向かって移動した場合、カメラ１１３はＥＬ軸に沿って動くことができない。従って、カメラ１１３の視軸は目標１００を追跡することができない。

そこで、ジンバル装置１１０は、Ａｚ軸を中心軸として第１のジンバル１１１を９０度回転させることにより、矢印Ａ方向に向かってカメラ１１３の視軸を移動する制御を行う。しかしながら、第１のジンバル１１１の回転にはその回転を行うモータの駆動能力の範囲において一定の移動時間が必要であり、目標１００の動きに遅れなく追従することは原理的に不可能である。また、目標１００が完全な天頂１０１の位置、すなわちＥＬ角が９０度ではなく天頂１０１の近傍に存在する場合にも、同様の理由により、ジンバル装置１１０においては、追尾制御が不安定になったり、或いは追尾制御が不能に陥る可能性があった。

下記特許文献１には、このような技術的課題を解決することができる、３軸構造を有するジンバル装置が開示されている。このジンバル装置は、Ａｚ軸及びＥＬ軸にこのＥＬ軸に直交するクロスエレベーション軸（ｘＥＬ軸）を更に備えている。また、このジンバル装置は、天頂領域を定義し、この天頂領域とそれ以外の領域とのそれぞれにおいて制御方法の切り替えを行っている。すなわち、低仰角域においては２軸制御を行い、高仰角域においては３軸制御を行う制御方法が採用されている。
特開平１１−３０８６０４号公報

しかしながら、前述の特許文献１に開示されたジンバル装置においては、以下の点について配慮がなされていなかった。

（１）天頂領域の定義の仕方、つまり２軸制御と３軸制御との切り替え位置の決め方によって、ジンバル装置の制御性能は次のような影響を受ける。まず、目標が切り替え位置（境界領域）を通過するとき、軸駆動のオン、オフに伴う加減速が発生し、ジンバル装置の追尾性能が悪くなる。更に、目標が切り替え位置を頻繁に遷移する場合、制御方法の切り替えが頻繁に発生し、ジンバル装置の追尾制御が不安定になる。

（２）３軸制御の一手法として、Ａｚ軸の角速度指令を誤差角Δθ［ターゲット角度−現在のＡｚ角度］×ゲインＫｐにより与え、Ａｚ軸が目標方向を向くように動かす制御方法が提案されている。ところが、誤差角Δθの大きさやゲインＫｐの与え方によってＡｚ角速度指令の大きさが変わるので、適切なＡｚ角速度となるように制御系を調整することが難しい。

（３）更に、実際のジンバル装置においては、ｘＥＬ軸はＥＬ軸上に設置されているので、このｘＥＬ軸を中心軸として回転するジンバルの可動範囲は、ＥＬ軸を中心軸として回転するジンバルに制約され狭くなる。すなわち、実際にはｘＥＬ軸上の可動範囲が考慮されておらず、可動範囲限界において、ｘＥＬ角の制御が他の２軸の制御となり不連続になるので、ジンバル装置は良好な追尾を行うことができない。

本発明は、上記課題を解決するためになされたものであり、その目的は、天頂領域かそれ以外の領域かに関係なく、ジンバルの駆動制御を連続して行い、追尾性能を向上することができるとともに、可動範囲限界における追尾性能を向上することができるジンバル装置を提供することである。

上記課題を解決するために、本発明の実施の形態に係るジンバル装置は、第１のジンバル軸を中心に回転する第１のジンバルと、第１のジンバル軸に直交する第２のジンバル軸を中心に回転し、第１のジンバルに支持された第２のジンバルと、第１のジンバル軸と同一直交点において第２のジンバル軸に直交する第３のジンバル軸を中心に回転し、第２のジンバルに支持された第３のジンバルと、第３のジンバルに支持され、目標の位置データを出力する目標位置認識手段と、第１のジンバルを回転駆動する第１の駆動手段と、第２のジンバルを回転駆動する第２の駆動手段と、第３のジンバルを回転駆動する第３の駆動手段と、第１のジンバル軸を中心に回転する第１のジンバルの角速度を検出する第１の角速度検出手段と、第２のジンバル軸を中心に回転する第２のジンバルの角速度を検出する第２の角速度検出手段と、第３のジンバル軸を中心に回転する第３のジンバルの角速度を検出する第３の角速度検出手段と、第１のジンバルの第２のジンバルに対する回転角度を検出する第１の角度検出手段と、第２のジンバルの第３のジンバルに対する回転角度を検出する第２の角度検出手段と、第３のジンバルのジンバル固定部に対する回転角度を検出する第３の角度検出手段と、目標認識手段から出力される位置データ、第１乃至第３の角速度検出手段のそれぞれにより検出した角速度データ及び第１乃至第３の角度検出手段のそれぞれにより検出した角度データに基づき、目標の追尾において、第１乃至第３のジンバルのそれぞれの角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部と、位置データ、角速度データ及び角度データに基づき、第３のジンバルを可動範囲の中央に位置させ、第２のジンバルの姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部と、第１の角速度指令算出部により算出された第１の角速度指標値及び第２の角速度指令算出部により算出された第２の角速度指標値に基づき、第１乃至第３のジンバルのそれぞれの角速度指令値を生成する角速度指令生成部と、角度指令生成部により生成された角速度指令値に基づき、第１乃至第３の駆動手段に対して個別にサーボ制御を行う駆動制御部とを備えている。

本発明によれば、天頂領域かそれ以外の領域かに関係なく、ジンバルの駆動制御を安定かつ連続して行い、追尾性能を向上するジンバル装置を提供することを可能とする。

以下、本発明の実施の形態に係るジンバル装置を、図面を参照しながら説明する。

［ジンバル装置の構造］
図１に示すように、ジンバル装置１は、第１のジンバル軸１０を中心に回転する第１のジンバル１１と、第１のジンバル軸１０に直交する第２のジンバル軸２０を中心に回転し、第１のジンバル１１に支持された第２のジンバル２１と、第１のジンバル軸１０と同一直交点において第２のジンバル軸２０に直交する第３のジンバル軸３０を中心に回転し、第２のジンバル２１に支持された第３のジンバル３１と、第３のジンバル３１に支持され、目標の位置データを出力する目標位置認識手段と、第１のジンバル１１を回転駆動する第１の駆動手段１２と、第２のジンバル２１を回転駆動する第２の駆動手段２２と、第３のジンバル３１を回転駆動する第３の駆動手段３２と、第１のジンバル軸１０を中心に回転する第１のジンバル１１の角速度を検出する第１の角速度検出手段１３と、第２のジンバル軸２０を中心に回転する第２のジンバル２１の角速度を検出する第２の角速度検出手段２３と、第３のジンバル軸３０を中心に回転する第３のジンバル３１の角速度を検出する第３の角速度検出手段３３と、第１のジンバル１１の第２のジンバル２１に対する回転角度を検出する第１の角度検出手段１４と、第２のジンバル２１の第３のジンバル３１に対する回転角度を検出する第２の角度検出手段２４と、第３のジンバル３１のジンバル固定部に対する回転角度を検出する第３の角度検出手段２４と、目標位置認識手段から出力される位置データ、第１乃至第３の角速度検出手段１３、２３、３３のそれぞれにより検出した角速度データ及び第１乃至第３の角度検出手段１４、２４、３４のそれぞれにより検出した角度データに基づき、目標の追尾において、第１から第３のジンバル１１、２１、３１の角速度が生成された角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部５１と、位置データ、角速度データ及び角度データに基づき、第３のジンバル３１を可動範囲の中央に位置させ、第２のジンバル２１の姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部５２と、第１の角速度指令算出部５１により算出された第１の角速度指標値及び第２の角速度指令算出部５２により算出された第２の角速度指標値に基づき、第１乃至第３のジンバル１１、２１、３１のそれぞれの角速度指令値を生成する角速度指令生成部５０と、角度指令生成部５０により生成された角速度指令値に基づき、第１乃至第３の駆動手段１２、２２、３２に対して個別にサーボ制御を行う駆動制御部６０とを備えている。

図２に示すように、第１のジンバル軸１０はアジマス軸（以下、単に「Ａｚ軸」という。）、第２のジンバル軸２０はエレベーション軸（以下、単に「ＥＬ軸」という。）、第３のジンバル軸３０はクロスエレベーション軸（以下、単に「ｘＥＬ軸」という。）である。ジンバル装置１は、これらのＡｚ軸１０、ＥＬ軸２０及びｘＥＬ軸３０が一点において直交する３軸構造を備えた３軸旋回装置である。

第１のジンバル１１は、地上に設置される、又は車両、船舶若しくは航空機等に搭載される座面側になる。実施の形態において、第１のジンバル１１は、Ａｚ軸１０を中心として回転する円盤であり、この円盤上のＡｚ軸１０を中心として対向する位置には第２のジンバル２１を支持する支持部１１Ａ及び１１Ｂが配設されている。第２のジンバル２１は、一端が支持部１１Ａに、他端が支持部１１Ｂに回転自在に支持され、凹型部材により構成されている。第３のジンバル３１は、第２のジンバル２１の凹型部材の中央部分に回転自在に支持され、第２のジンバル２１に比べて一回り小さい凹型部材により構成されている。

この第３のジンバル３１に目標位置認識手段が搭載されている（図４及び図５参照）。実施の形態において、目標位置認識手段はセンサ４０（以下、目標位置認識手段はセンサ４０として説明する。）により構成されている。センサ４０には工業用テレビカメラ、詳細には保安監視、交通情報支援等の分野において追尾装置を構築する赤外線カメラを実用的に使用している。ＣＣＤカメラ、ＣＭＯＳセンサカメラ等のエリアセンサも利用することができる。センサ４０は、目標に向かう視軸がｘＥＬ軸３０と直交するように、第３のジンバル３１に配設される。

図３には、Ａｚ軸１０、ＥＬ軸２０及びｘＥＬ軸３０の関係と、Ａｚ軸１０の座標系及びセンサ４０の座標系（赤外線カメラの座標系）の関係とを示す。Ａｚ軸１０の三次元座標はＸ０、Ｙ０及びＺ０において定義することができ、センサ４０の三次元座標はＸＣ、ＹＣ及びＺＣにおいて定義することができる。

図４及び図５には、ジンバル装置１の３軸ジンバル装置部分の具体的な構造を示す。図４に示すジンバル装置１は、センサ４０の視軸ｒを正面に向けた状態にある。第１のジンバル１１、第２のジンバル２１、第３のジンバル３１及びセンサ４０は、天頂方向（図４中、上側）を半球とする円柱形状の外装ケース２で覆われている。外装ケース２は、第１のジンバル１１の支持部１１Ａ、１１Ｂのそれぞれを覆う部分に不透明な外装領域２Ａを備え、この外装領域２Ａ間の円柱の対向する側面と天頂領域とに透明な可視領域２Ｂとを備えている。可視領域２Ｂはセンサ４０の可視範囲になる。図５に示すジンバル装置１は、センサ４０の視軸ｒを天頂に向けた状態にある。

前述の図１に示す第１の駆動手段１２には第１の実施の形態において電動モータが使用され、この電動モータの回転軸はＡｚ軸１０に相当し第１のジンバル１１に連結されている。第１の角度検出手段１４には電動モータの回転角度を検出するエンコーダを使用することができる。第１の角速度検出手段１３には、第１のジンバル１１に配設されたジャイロセンサを使用することができる。

同様に、第２の駆動手段２２には電動モータが使用され、この電動モータの回転軸はＥＬ軸２０に相当し第２のジンバル２１に連結されている。第２の角度検出手段２４にはエンコーダを、第２の角速度検出手段２３にはジャイロセンサを使用することができる。第３の駆動手段３２には電動モータが使用され、この電動モータの回転軸はｘＥＬ軸３０に相当し第３のジンバル３１に連結されている。第３の角度検出手段３４にはエンコーダを、第３の角速度検出手段３３にはジャイロセンサを使用することができる。

駆動制御部６０は、第１の駆動手段１２の回転制御を行う第１のサーボ制御部６１と、第２の駆動手段２２の回転制御を行う第２のサーボ制御部６２と、第３の駆動手段３２の回転制御を行う第３のサーボ制御部６３とを備えている。第１のサーボ制御部６１、第２のサーボ制御部６２、第３のサーボ制御部６３のそれぞれは角速度指令生成部５０から出力される角速度指令に基づき動作するようになっている。

［ジンバル装置の制御方法］
次に、前述のジンバル装置１の制御方法を説明する。

ジンバル装置１において、追尾目標はセンサ４０のセンサ座標系の原点（中心点）Ｏにくるように捕捉追尾されているので、図６に示すように、目標位置座標は原点からの追尾誤差（ΔＸ，ΔＹ）として与えることができる。追尾誤差は、原点Ｏから目標位置までのカメラ（例えばＣＣＤ）の画素数を算出することにより求めることができる。追尾制御は、センサ４０によるトラッキングを行い、図７に示すように、センサ座標系において追尾誤差（ΔＸ，ΔＹ）をＡｚ軸１０、ＥＬ軸２０、ｘＥＬ軸３０の目標速度指令に変換し、第１のジンバル１１、第２のジンバル２１、第３のジンバル３２の角速度制御を行なう。センサ４０の追尾誤差は原点Ｏからの二次元変位（ΔＸ，ΔＹ）として得ることができる。結果的に、図１に示す角速度指令生成部５０は、３軸のＡｚ軸１０、ＥＬ軸２０、ｘＥＬ軸３０のそれぞれの目標角速度指令（ω₁,ω₂,ω₃）を生成する。

３軸制御において必要な２変数（ΔＸ，ΔＹ）から３変数（ω₁,ω₂,ω₃）への変換は、無数の解が存在するので、一通りに決定することができない（不定）。しかし、角速度の組み合わせによっては、その解の制御において系が発散し、制御不能となることがあるため、本実施の形態に係るジンバル装置１においては、この複数解の中から、ジンバルロックを回避し、かつｘＥＬ軸３０の可動範囲内において良好な制御を行う、指標となる、できる限り小さな角速度指令値を算出する。

ｘＥＬ軸３０の可動範囲を考慮する必要性は、例えば、図４及び図５において説明した外装ケース２の不透明な外装領域２Ａを有する構造上の制約に基づくものであり、第３のジンバル３１の可動範囲は、実用上、±１０度〜±２０度の角度範囲内になる。

角速度指令の大きさについては、各軸の角速度からなる角速度ベクトル

の大きさ

を小さくすることと定義する。ここで、記号tは転置を表す。

今、ジンバル軸数をｍとする。例えば、２軸ジンバル構造であればジンバル軸数ｍ＝２であり、３軸ジンバル構造であればジンバル軸数ｍ＝３である。図８に示す、センサ４０の視線方向を表す視軸ベクトル

とジンバルの軸角度ベクトル

との関係は下記（１）式において表すことができる。

はｍ次のベクトル空間を表す。上記（１）式の微小変化を表す関係式は下記（２）式で表すことができる。

この（２）式は下記ヤコビ行列を係数として表されている。

角速度制御系を構成する場合、視軸速度ベクトル

とジンバルの軸角速度ベクトル

との関係を表す下記（３）式が使用される。

上記（３）式により、ジンバル角速度

からカメラ視軸速度

を求めることができる。

今、センサ４０の視軸ベクトル

を図３に示すカメラ座標系

において表した視軸ベクトルを

とする。センサ４０上において目標が微小移動した場合、この微小移動した視軸ベクトルは下記（４）において表すことができる。

上記（４）式のｘ、ｙ成分はセンサ４０上における目標誤差に相当し、ｚ方向成分

はゼロとなる。そこで、

のｘ、ｙ成分のみに着目して、新たに下記（５）式から（８）式を定義する。

２軸ジンバル構造の場合、

であり、

（ｄｅｔは行列式記号）ならばＪ（θ）は正則であり逆行列が存在し、下記（９）式において表すことができる。

すなわち、センサ４０上の目標の移動に対して、センサ４０を移動するためのジンバル角速度が存在する。

しかしながら、det Ｊ（θ）＝０の場合には、上記（９）式は成立せず、上記（７）式を満足するジンバル角速度は存在しない。このような姿勢は特異姿勢と呼ばれている。２軸ジンバルタレットにおいては、センサが天頂を向いた場合にdet Ｊ（θ）＝０になり、追尾制御が不能になる。また、特異姿勢近傍においては、det Ｊ（θ）が非常に小さな値になり、追尾制御が不安定になる。

一方、ジンバル軸数が３軸以上の場合、

は正則でないため逆行列を持たない。Ｊ（θ）が正則でない場合、フルランクでない場合には擬似逆行列を適用することができる。ある角速度ベクトル

を与えた場合、上記（７）式の解

は下記（１１）式により表すことができる。

このとき、目標の移動速度

との誤差の大きさ（以下、誤差ノルムという。）

を考え、この誤差を最小にする角速度ベクトル

の解を求める。

上記（７）式において、新値に対する誤差の大きさを示す下記（１１）式において表される誤差ノルムを考える。

この（１２）式の値を最小にする解は下記（１２）式により与えられる。

ここで、

は擬似逆行列であり、

は単位行列であり、

は任意ベクトルである。上記（７）式が解を持つ場合には、上記（１２）式が一般解を与え、解を持たなければ、（１１）式に示す誤差ノルムが最小になる近似解を与える。ｋは任意のベクトルであるから、上記（１２）式の第２項は冗長項を表す。（１２）式において表される一般解の中で、特にジンバル角速度の解

のノルム

を最小にする理想的な解は下記（１３）式において表すことができる。

また、詳細は省略するが、

の場合において、擬似逆行列は下記（１４）式により与えられる。

前述の通り、角速度ベクトルのノルムを最小にするには（１３）式に従って各軸の角速度指令を計算すればよい。ところが、この計算だけでは不十分であり、３軸目のｘＥＬ軸３０の可動範囲が例えば±１０度の極端に狭い可動範囲に設定されている場合も考慮する必要があり、上記（１３）式を用いた計算だけでは容易にｘＥＬ軸３０の可動範囲を越えてしまう。

ｘＥＬ軸３０を中心として回転する第３のジンバル３１の動作について簡単に説明する。例えば、図８に示すように、センサ４０の視軸が正面を向くような姿勢にタレットがあり、目標がＡｚ軸１０周り（θ_１軸）に移動する場合を考える。このとき、上記（１３）式によれば、角速度ベクトルの最小値を与える条件は

となり、Ａｚ軸１０及びｘＥＬ軸３０が同じ角速度で回転し続ける。Ａｚ軸１０は無限に回転することができるが、ｘＥＬ軸３０はすぐに可動範囲を超えてしまい、追尾制御が不連続になる。

また、図９に示すように、センサ４０の視軸が天頂を向く姿勢にタレットがあり、かつ目標がｘＥＬ軸３０周り（θ_３軸）に移動する場合を考える。このとき、上記（１４）式を満たす最適な解は

となり、ｘＥＬ軸３０だけが回転し続けることになる。２軸ジンバル構造の場合は、ジンバルロックにはならないものの、ｘＥＬ軸３０の可動範囲を超える時点において追尾制御は不能になる。上記（１３）式とは別に、ｘＥＬ軸３０の動作にしきい値などの方法を加えて制約条件を与えることもできるが、しきい値に該当する条件の領域において追尾制御が不連続になる。以上のような理由に基づき、上記（１３）式による３軸制御だけでは、実用上、課題が残る。

上記（１２）式の第２項の冗長項にはｋという任意に選択することができるベクトルが存在する。このｋを使ってｘＥＬ軸３０の可動範囲の制約条件を角速度指令の生成に反映させることを考える。下記（１５）式に示す、あるスカラー評価関数を考え、この関数の値σをできる限り大きくするベクトルｋを考える。

このようなベクトルｋは関数Ψ（θ）の最急勾配ベクトルであるから、θによる関数Ψ（θ）の偏微分を取ればよい。すなわち、下記（１６）式から（１８）式において表すことができる。

このとき、上記（１２）式は下記（１９）式となる。

この上記（１９）式の第１項は本実施の形態に係る第１の角速度指令の算出値であり、同様に第２項は第２の角速度指令の算出値である。上記（１５）式の評価関数の微分を取ると、下記（２０）式になる。

は少なくとも非負対称行列であるから、２次形式における非負定行列になる。すなわち、（１９）式の第２項は常に非負（常にゼロ以上）になる。従って、上記（１６）式から（１８）式において定められたベクトルｋは評価関数の値σを大きくすることができる。

評価関数Ψ（θ）は自由に選定することができる。本実施の形態においては、以下の要求に基づき決定された。

（１）ｘＥＬ軸３０はできる限り可動範囲の真ん中にくるようにしたい。

（２）但し、要求（１）においてｘＥＬ軸３０の位置が極端に真ん中に制約されることは避けたい。可動範囲内外の境界に近づいた場合には、強い制動を与えたい。

（３）ＥＬ軸２０はできる限り天頂方向を向かないようにしたい。

３軸ジンバル構造において冗長成分は１自由度しかないので、上記要求（１）と（３）とをそれぞれ別個の評価関数により達成することはできない。そこで、評価関数として、下記（２１）式から（２４）式を考える。

上記（２１）式の第１項は２軸ジンバル構造の場合のヤコビ行列の行列式のノルムに相当する。ＥＬ軸２０が天頂に近づくにつれて値が小さくなり、天頂すなわちθ_２＝９０度において値がゼロになる。第２項は、θ_３の可動範囲を上記（２２）式において−１０＜θ_３＜１０（単位は「度」）のように与えた場合、図１０に示すなべ底型の関数になる。図１０において、横軸はｘＥＬ軸３０の角度を示し、縦軸はβを除く第２項の値を示す。各項の係数の符号を上記（２３）式、（２４）式のように与えることで、評価関数Ψ（θ）をできる限り大きくする冗長項ベクトルｋを求めることができる。この算出方法は上記（１６）式及び（１７）式に示す通りである。

冗長項ベクトルｋの各項を具体的に求めてみると、上記（２１）式を（１８）式へ代入すると、下記（２５）式から（２７）式が得られる。

この評価関数は、タレットが天頂から離れている場合には第２項のｘＥＬ軸３０の可動範囲に関する制約が支配的となる。また、天頂に近づくと第１項のＥＬ軸２０の条件が効果を表す。αとβは自由に選ぶことができる。

次に、上記（１９）式の具体的な算出方法を説明する。まず最初に、前述の図３に示すように、ベース座標系Σ₀と各軸の座標系Σ_i（ｉ＝１，２，３）、及びセンサ座標系Σcを定義する。Ａｚ軸１０からｘＥＬ軸３０までのジンバル角はθ_i（ｉ＝１，２，３）とする。更に、各座標系間の変換行列は

（ｉ，ｊ＝０，１，２，３，ｃ）とする。ここで、変換行列はΣ_i座標系からΣ_j座標系への変換行列（３×３行列）を表す。このとき、下記（Ａ１）式から（Ａ５）式が成り立つ。

この（Ａ５）式の

はベース座標系から見たセンサ４０の座標系の変換行列であり、（Ａ５）式の第３列は視軸ベクトル（センサ４０のｚ軸方向のベクトル）

に一致するので、上記（１）式は下記（Ａ６）式に示すように、視軸ベクトルを電動モータの回転角度において表現することができる。

この（Ａ６）式から上記（３）式の視軸速度ベクトル

とジンバルの軸角速度ベクトル

との関係は下記（Ａ７）式において表すことができる。すなわち、上記（Ａ６）式を微分することにより速度を抽出することができる。

但し、三角関数は

のように記述するものとする（以降同様）。

前述の図３に示すように、センサ４０の視軸ベクトル

をセンサ４０の座標系

において表したものを

と定義すると、下記（Ａ８）式を得ることができる。

上記（５）式、（６）式、（７）式及び（８）式２基づき、下記（Ａ９）式から（１２）式を定義することができる。

上記（Ａ１２）式のヤコビ行列は上記（Ａ８）式より、下記（Ａ１３）式において表すことができる。

よって、視軸ベクトル

を表す上記（Ａ１１）式を書き下すと、下記（Ａ１４）を得ることができる。

すなわち、電動モータの角速度を与えると、センサ４０の座標系においてセンサ４０の視軸ベクトルの移動速度を算出することができる。

上記（Ａ１１）式及び（Ａ１４）式において、

から

を求めるには、上記（１２）式、つまり下記（Ａ１５）式に示す関係を用いる。

ここで、

は擬似逆行列であり、

は単位行列であり、

は任意ベクトルである。本実施の形態においては、常に

であり、擬似逆行列

は上記（１４）式と（Ａ１３）式との関係を用いて下記（Ａ１６）に示すように計算することができる。

ここで、

は

の転置行列であり、

は

の逆行列を表す。更に、上記（Ａ１５）式の第２項の冗長項の係数行列

は上記（Ａ１３）式及び（Ａ１６）式を用いて下記（１７）式として表すことができる。

そして、上記（Ａ１５）式の３軸制御を上記（１９）式、（２５）式、（２６）式、（２７）式と、上記（Ａ１６）式、（Ａ１７）式とを用いて書き直すと、下記（Ａ１８）式を得ることができる。

この（Ａ１８）式において、第１項は、目標を追尾する際の、各ジンバル１１、２１、３１の駆動手段（電動モータ）１２、２２、３２の回転速度から成る角速度ベクトルの大きさが最小となるような第１の角速度指標値である。第１の角速度指標値は第１の指令算出部５１において生成される。第２項は、ｘＥＬ軸３０が可動範囲を越えず、できる限り可動範囲の中央にあるように制御し、かつ第２のジンバル２１の姿勢が天頂方向から遠ざかるように各ジンバル軸を制御する第２の角速度指標値である。この第２の角速度指標値は第２の指令算出部５２において生成される。

角度指令生成部５においては、第１の角速度指標値と第２の角速度指標値とを加算して、角速度指令値を算出し、各軸のサーボ制御部６１、６２、６３に対して角速度指令値を送出する。

［ジンバル装置の追尾性能］
前述の構成並びに制御方法を備えたジンバル装置１においては、図１１に示すように、センサ４０の視軸が天頂方向に向いている位置からジンバルロックが発生する矢印Ａ方向に移動する場合も、良好な追尾を行うことができる。目標（ターゲット）が天頂から矢印Ａ方向に移動する場合、本実施の形態に係るジンバル装置１は、図１２に示すように、１５度／秒の速度において目標を追尾することができる。図１２において、横軸は時間を示し、縦軸は視軸の角度を示す。

このとき、本実施の形態に係るジンバル装置１においては、図１３及び図１４に示すように、Ａｚ軸１０は約１．５秒かけて実現可能な角速度において１８０°回転している。また、ｘＥＬ軸３０も実現可能な角速度において移動し、かつ角度移動量は６°程度に収まり、ｘＥＬ軸３０の可動範囲内において連続的な追尾制御を実現することができる。

以上説明したように、本発明の実施の形態に係るジンバル装置１においては、天頂からＥＬ軸２０の回転方向と直交する方向に目標が移動する場合、角速度が過大にならないように、Ａｚ軸１０とｘＥＬ軸３０との動作を配分しながら追尾動作を行うことができる。すなわち、実現可能なジンバル可動範囲を越えることなく、ジンバルロックを避け、全天にわたり連続的な追尾制御を安定して実現することができるジンバル装置１を提供することができる。

なお、本発明は、上記実施の形態に限定されるものではない。例えば、本発明は、ｘＥＬ軸３０を中心に回転する第３のジンバル３１に、レーザ、赤外線、レーダ等のセンサを搭載したジンバル装置としてもよい。

本発明の実施の形態に係るジンバル装置の全体構成を示すブロック図である。 図１に示すジンバル装置のジンバルの斜視図である。 図１に示すジンバル装置の座標系を示す図である。 図１に示すジンバル装置の具体的な要部の外観構造を示す斜視図である。 図１に示すジンバル装置の具体的な要部の外観構造を示す斜視図である。 図１に示すジンバル装置のセンサ座標系を説明する図である。 図１に示すジンバル装置において追尾誤差から角速度指令を生成する流れを説明する図である。 図１に示すジンバル装置の要部の斜視図である。 図１に示すジンバル装置の要部の斜視図である。 図１に示すジンバル装置の制御方法においてｘＥＬ軸と評価関数との関係を示す図である。 図１に示すジンバル装置の要部斜視図である。 図１に示すジンバル装置のセンサの動作特性を示す図である。 図１に示すジンバル装置の各軸の角度軌跡を示す図である。 図１に示すジンバル装置の各軸の角速度軌跡を示す図である。 従来技術に係るＡｚ軸とＥＬ軸とを説明する図である。 従来技術に係る天頂方向を説明する図である。 従来技術に係る２軸ジンバル装置の斜視図である。

符号の説明

１…ジンバル装置、
１０…第１のジンバル軸（Ａｚ軸）、１１…第１のジンバル、１２…第１の駆動手段、１３…第１の角速度検出手段、１４…第１の角度検出手段、２０…第２のジンバル軸（ＥＬ軸）、２１…第２のジンバル、２２…第２の駆動手段、２３…第２の角速度検出手段、２４…第２の角度検出手段、３０…第３のジンバル軸（ｘＥＬ軸）、３１…第３のジンバル、３２…第３の駆動手段、３３…第３の角速度検出手段、３４…第３の角度検出手段、４０…センサ、５０…角速度指令生成部、５１…第１の指令算出部、５２…第２の指令算出部、５３…角速度検出部、５４…角度検出部、６０…駆動制御部、６１…第１のサーボ制御部、６２…第２のサーボ制御部、６３…第３のサーボ制御部。

Claims (2)

1. 第１のジンバル軸を中心に回転する第１のジンバルと、
   前記第１のジンバル軸に直交する第２のジンバル軸を中心に回転し、前記第１のジンバルに支持された第２のジンバルと、
   前記第１のジンバル軸と同一直交点において前記第２のジンバル軸に直交する第３のジンバル軸を中心に回転し、前記第２のジンバルに支持された第３のジンバルと、
   前記第３のジンバルに支持され、目標の位置データを出力する目標位置認識手段と、
   前記第１のジンバルを回転駆動する第１の駆動手段と、
   前記第２のジンバルを回転駆動する第２の駆動手段と、
   前記第３のジンバルを回転駆動する第３の駆動手段と、
   前記第１のジンバル軸を中心に回転する前記第１のジンバルの角速度を検出する第１の角速度検出手段と、
   前記第２のジンバル軸を中心に回転する前記第２のジンバルの角速度を検出する第２の角速度検出手段と、
   前記第３のジンバル軸を中心に回転する前記第３のジンバルの角速度を検出する第３の角速度検出手段と、
   前記第１のジンバルの前記第２のジンバルに対する回転角度を検出する第１の角度検出手段と、
   前記第２のジンバルの前記第３のジンバルに対する回転角度を検出する第２の角度検出手段と、
   前記第３のジンバルのジンバル固定部に対する回転角度を検出する第３の角度検出手段と、
   前記目標位置認識手段から出力される位置データ、前記第１乃至第３の角速度検出手段のそれぞれにより検出した角速度データ及び前記第１乃至第３の角度検出手段のそれぞれにより検出した角度データに基づき、目標の追尾において、前記第１から第３のジンバルの角速度から生成する角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部と、
   前記位置データ、前記角速度データ及び前記角度データに基づき、前記第３のジンバルを可動範囲の中央に位置させ、前記第２のジンバルの姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部と、
   前記第１の角速度指令算出部により算出された第１の角速度指標値及び第２の角速度指令算出部により算出された第２の角速度指標値に基づき、前記第１乃至第３のジンバルのそれぞれの角速度指令値を生成する角速度指令生成部と、
   前記角度指令生成部により生成された角速度指令値に基づき、前記第１乃至第３の駆動手段に対して個別にサーボ制御を行う駆動制御部と、
   を備えたことを特徴とするジンバル装置。
2. 前記目標位置認識手段はエリアセンサであり、このエリアセンサの視軸は前記第３のジンバル軸に直交することを特徴とする請求項１に記載のジンバル装置。