(57)【要約】【課題】      実現可能なジンバル可動範囲を越えることなく、ジンバルロックを避け、全天にわたり連続的な追尾制御を安定して実現することができるジンバル装置を提供する。【解決手段】  ジンバル装置１は、天頂からＥＬ軸２０の回転方向と直交する方向に目標が移動する場合、角速度が過大にならないように、Ａｚ軸１０とｘＥＬ軸３０との動作を配分しながら追尾動作を行う。【選択図】      図１
2006106910.tif 000002
【特許請求の範囲】
【請求項１】
第１のジンバル軸を中心に回転する第１のジンバルと、
前記第１のジンバル軸に直交する第２のジンバル軸を中心に回転し、前記第１のジンバルに支持された第２のジンバルと、
前記第１のジンバル軸と同一直交点において前記第２のジンバル軸に直交する第３のジンバル軸を中心に回転し、前記第２のジンバルに支持された第３のジンバルと、
前記第３のジンバルに支持され、目標の位置データを出力する目標位置認識手段と、
前記第１のジンバルを回転駆動する第１の駆動手段と、
前記第２のジンバルを回転駆動する第２の駆動手段と、
前記第３のジンバルを回転駆動する第３の駆動手段と、
前記第１のジンバル軸を中心に回転する前記第１のジンバルの角速度を検出する第１の角速度検出手段と、
前記第２のジンバル軸を中心に回転する前記第２のジンバルの角速度を検出する第２の角速度検出手段と、
前記第３のジンバル軸を中心に回転する前記第３のジンバルの角速度を検出する第３の角速度検出手段と、
前記第１のジンバルの前記第２のジンバルに対する回転角度を検出する第１の角度検出手段と、
前記第２のジンバルの前記第３のジンバルに対する回転角度を検出する第２の角度検出手段と、
前記第３のジンバルのジンバル固定部に対する回転角度を検出する第３の角度検出手段と、
前記目標位置認識手段から出力される位置データ、前記第１乃至第３の角速度検出手段のそれぞれにより検出した角速度データ及び前記第１乃至第３の角度検出手段のそれぞれにより検出した角度データに基づき、目標の追尾において、前記第１から第３のジンバルの角速度から生成する角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部と、
前記位置データ、前記角速度データ及び前記角度データに基づき、前記第３のジンバルを可動範囲の中央に位置させ、前記第２のジンバルの姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部と、
前記第１の角速度指令算出部により算出された第１の角速度指標値及び第２の角速度指令算出部により算出された第２の角速度指標値に基づき、前記第１乃至第３のジンバルのそれぞれの角速度指令値を生成する角速度指令生成部と、
前記角度指令生成部により生成された角速度指令値に基づき、前記第１乃至第３の駆動手段に対して個別にサーボ制御を行う駆動制御部と、
を備えたことを特徴とするジンバル装置。
【請求項２】
前記目標位置認識手段はエリアセンサであり、このエリアセンサの視軸は前記第３のジンバル軸に直交することを特徴とする請求項１に記載のジンバル装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ジンバル装置に関し、特に地上に設置され若しくは車両、船舶、航空機等に搭載され、任意の位置に存在する目標を追跡するジンバル装置に関する。
【背景技術】
【０００２】
例えば、保安監視や交通情報支援に使用される工業用テレビカメラを備えた追跡装置においては、図１５に示すように、垂直軸（アジマス（Ａｚ）軸）と仰軸（エレベーション（ＥＬ）軸）の２つのジンバル軸を設定することにより、空間の任意の方向に存在する目標を指向することができる。ジンバル装置においては、全天にわたり、目標の動きに対して遅れなくカメラの視軸の制御を行い、目標を連続的に追跡することが要求されている。
【０００３】
ところが、２軸構造を有するジンバル装置には、図１６に示すように、天頂１０１の方向においてジンバルの角速度が制御不能になるジンバルロックと呼ばれる現象が存在する。天頂１０１の方向はＥＬ角が９０度になる方向である。
【０００４】
図１７に示すように、２軸構造を有するジンバル装置１１０は、Ａｚ軸を中心軸として回転する第１のジンバル１１１と、ＥＬ軸を中心軸として回転する第２のジンバル１１２と、第２のジンバル１１２に取り付けられたカメラ１１３とを備えている。このジンバル装置１１０においては、ＥＬ軸を中心軸として水平から垂直に向かってカメラ１１３が９０度回転上昇すると、カメラ１１３の視軸は、図１０１に示す天頂１０１の方向と一致する。このカメラ１１３の視軸と天頂１０１の方向とが一致した状態において、追跡中の目標１００が天頂１０１からＥＬ軸に沿う矢印Ａ方向に向かって移動した場合、カメラ１１３はＥＬ軸に沿って動くことができない。従って、カメラ１１３の視軸は目標１００を追跡することができない。
【０００５】
そこで、ジンバル装置１１０は、Ａｚ軸を中心軸として第１のジンバル１１１を９０度回転させることにより、矢印Ａ方向に向かってカメラ１１３の視軸を移動する制御を行う。しかしながら、第１のジンバル１１１の回転にはその回転を行うモータの駆動能力の範囲において一定の移動時間が必要であり、目標１００の動きに遅れなく追従することは原理的に不可能である。また、目標１００が完全な天頂１０１の位置、すなわちＥＬ角が９０度ではなく天頂１０１の近傍に存在する場合にも、同様の理由により、ジンバル装置１１０においては、追尾制御が不安定になったり、或いは追尾制御が不能に陥る可能性があった。
【０００６】
下記特許文献１には、このような技術的課題を解決することができる、３軸構造を有するジンバル装置が開示されている。このジンバル装置は、Ａｚ軸及びＥＬ軸にこのＥＬ軸に直交するクロスエレベーション軸（ｘＥＬ軸）を更に備えている。また、このジンバル装置は、天頂領域を定義し、この天頂領域とそれ以外の領域とのそれぞれにおいて制御方法の切り替えを行っている。すなわち、低仰角域においては２軸制御を行い、高仰角域においては３軸制御を行う制御方法が採用されている。
【特許文献１】特開平１１−３０８６０４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、前述の特許文献１に開示されたジンバル装置においては、以下の点について配慮がなされていなかった。
【０００８】
（１）天頂領域の定義の仕方、つまり２軸制御と３軸制御との切り替え位置の決め方によって、ジンバル装置の制御性能は次のような影響を受ける。まず、目標が切り替え位置（境界領域）を通過するとき、軸駆動のオン、オフに伴う加減速が発生し、ジンバル装置の追尾性能が悪くなる。更に、目標が切り替え位置を頻繁に遷移する場合、制御方法の切り替えが頻繁に発生し、ジンバル装置の追尾制御が不安定になる。
【０００９】
（２）３軸制御の一手法として、Ａｚ軸の角速度指令を誤差角Δθ［ターゲット角度−現在のＡｚ角度］×ゲインＫｐにより与え、Ａｚ軸が目標方向を向くように動かす制御方法が提案されている。ところが、誤差角Δθの大きさやゲインＫｐの与え方によってＡｚ角速度指令の大きさが変わるので、適切なＡｚ角速度となるように制御系を調整することが難しい。
【００１０】
（３）更に、実際のジンバル装置においては、ｘＥＬ軸はＥＬ軸上に設置されているので、このｘＥＬ軸を中心軸として回転するジンバルの可動範囲は、ＥＬ軸を中心軸として回転するジンバルに制約され狭くなる。すなわち、実際にはｘＥＬ軸上の可動範囲が考慮されておらず、可動範囲限界において、ｘＥＬ角の制御が他の２軸の制御となり不連続になるので、ジンバル装置は良好な追尾を行うことができない。
【００１１】
本発明は、上記課題を解決するためになされたものであり、その目的は、天頂領域かそれ以外の領域かに関係なく、ジンバルの駆動制御を連続して行い、追尾性能を向上することができるとともに、可動範囲限界における追尾性能を向上することができるジンバル装置を提供することである。
【課題を解決するための手段】
【００１２】
上記課題を解決するために、本発明の実施の形態に係るジンバル装置は、第１のジンバル軸を中心に回転する第１のジンバルと、第１のジンバル軸に直交する第２のジンバル軸を中心に回転し、第１のジンバルに支持された第２のジンバルと、第１のジンバル軸と同一直交点において第２のジンバル軸に直交する第３のジンバル軸を中心に回転し、第２のジンバルに支持された第３のジンバルと、第３のジンバルに支持され、目標の位置データを出力する目標位置認識手段と、第１のジンバルを回転駆動する第１の駆動手段と、第２のジンバルを回転駆動する第２の駆動手段と、第３のジンバルを回転駆動する第３の駆動手段と、第１のジンバル軸を中心に回転する第１のジンバルの角速度を検出する第１の角速度検出手段と、第２のジンバル軸を中心に回転する第２のジンバルの角速度を検出する第２の角速度検出手段と、第３のジンバル軸を中心に回転する第３のジンバルの角速度を検出する第３の角速度検出手段と、第１のジンバルの第２のジンバルに対する回転角度を検出する第１の角度検出手段と、第２のジンバルの第３のジンバルに対する回転角度を検出する第２の角度検出手段と、第３のジンバルのジンバル固定部に対する回転角度を検出する第３の角度検出手段と、目標認識手段から出力される位置データ、第１乃至第３の角速度検出手段のそれぞれにより検出した角速度データ及び第１乃至第３の角度検出手段のそれぞれにより検出した角度データに基づき、目標の追尾において、第１乃至第３のジンバルのそれぞれの角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部と、位置データ、角速度データ及び角度データに基づき、第３のジンバルを可動範囲の中央に位置させ、第２のジンバルの姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部と、第１の角速度指令算出部により算出された第１の角速度指標値及び第２の角速度指令算出部により算出された第２の角速度指標値に基づき、第１乃至第３のジンバルのそれぞれの角速度指令値を生成する角速度指令生成部と、角度指令生成部により生成された角速度指令値に基づき、第１乃至第３の駆動手段に対して個別にサーボ制御を行う駆動制御部とを備えている。
【発明の効果】
【００１３】
本発明によれば、天頂領域かそれ以外の領域かに関係なく、ジンバルの駆動制御を安定かつ連続して行い、追尾性能を向上するジンバル装置を提供することを可能とする。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の実施の形態に係るジンバル装置を、図面を参照しながら説明する。
【００１５】
［ジンバル装置の構造］
図１に示すように、ジンバル装置１は、第１のジンバル軸１０を中心に回転する第１のジンバル１１と、第１のジンバル軸１０に直交する第２のジンバル軸２０を中心に回転し、第１のジンバル１１に支持された第２のジンバル２１と、第１のジンバル軸１０と同一直交点において第２のジンバル軸２０に直交する第３のジンバル軸３０を中心に回転し、第２のジンバル２１に支持された第３のジンバル３１と、第３のジンバル３１に支持され、目標の位置データを出力する目標位置認識手段と、第１のジンバル１１を回転駆動する第１の駆動手段１２と、第２のジンバル２１を回転駆動する第２の駆動手段２２と、第３のジンバル３１を回転駆動する第３の駆動手段３２と、第１のジンバル軸１０を中心に回転する第１のジンバル１１の角速度を検出する第１の角速度検出手段１３と、第２のジンバル軸２０を中心に回転する第２のジンバル２１の角速度を検出する第２の角速度検出手段２３と、第３のジンバル軸３０を中心に回転する第３のジンバル３１の角速度を検出する第３の角速度検出手段３３と、第１のジンバル１１の第２のジンバル２１に対する回転角度を検出する第１の角度検出手段１４と、第２のジンバル２１の第３のジンバル３１に対する回転角度を検出する第２の角度検出手段２４と、第３のジンバル３１のジンバル固定部に対する回転角度を検出する第３の角度検出手段２４と、目標位置認識手段から出力される位置データ、第１乃至第３の角速度検出手段１３、２３、３３のそれぞれにより検出した角速度データ及び第１乃至第３の角度検出手段１４、２４、３４のそれぞれにより検出した角度データに基づき、目標の追尾において、第１から第３のジンバル１１、２１、３１の角速度が生成された角速度ベクトルの大きさを最小にする第１の角速度指標値を算出する第１の角速度指令算出部５１と、位置データ、角速度データ及び角度データに基づき、第３のジンバル３１を可動範囲の中央に位置させ、第２のジンバル２１の姿勢を天頂方向から遠ざける第２の角速度指標値を算出する第２の角速度指令算出部５２と、第１の角速度指令算出部５１により算出された第１の角速度指標値及び第２の角速度指令算出部５２により算出された第２の角速度指標値に基づき、第１乃至第３のジンバル１１、２１、３１のそれぞれの角速度指令値を生成する角速度指令生成部５０と、角度指令生成部５０により生成された角速度指令値に基づき、第１乃至第３の駆動手段１２、２２、３２に対して個別にサーボ制御を行う駆動制御部６０とを備えている。
【００１６】
図２に示すように、第１のジンバル軸１０はアジマス軸（以下、単に「Ａｚ軸」という。）、第２のジンバル軸２０はエレベーション軸（以下、単に「ＥＬ軸」という。）、第３のジンバル軸３０はクロスエレベーション軸（以下、単に「ｘＥＬ軸」という。）である。ジンバル装置１は、これらのＡｚ軸１０、ＥＬ軸２０及びｘＥＬ軸３０が一点において直交する３軸構造を備えた３軸旋回装置である。
【００１７】
第１のジンバル１１は、地上に設置される、又は車両、船舶若しくは航空機等に搭載される座面側になる。実施の形態において、第１のジンバル１１は、Ａｚ軸１０を中心として回転する円盤であり、この円盤上のＡｚ軸１０を中心として対向する位置には第２のジンバル２１を支持する支持部１１Ａ及び１１Ｂが配設されている。第２のジンバル２１は、一端が支持部１１Ａに、他端が支持部１１Ｂに回転自在に支持され、凹型部材により構成されている。第３のジンバル３１は、第２のジンバル２１の凹型部材の中央部分に回転自在に支持され、第２のジンバル２１に比べて一回り小さい凹型部材により構成されている。
【００１８】
この第３のジンバル３１に目標位置認識手段が搭載されている（図４及び図５参照）。実施の形態において、目標位置認識手段はセンサ４０（以下、目標位置認識手段はセンサ４０として説明する。）により構成されている。センサ４０には工業用テレビカメラ、詳細には保安監視、交通情報支援等の分野において追尾装置を構築する赤外線カメラを実用的に使用している。ＣＣＤカメラ、ＣＭＯＳセンサカメラ等のエリアセンサも利用することができる。センサ４０は、目標に向かう視軸がｘＥＬ軸３０と直交するように、第３のジンバル３１に配設される。
【００１９】
図３には、Ａｚ軸１０、ＥＬ軸２０及びｘＥＬ軸３０の関係と、Ａｚ軸１０の座標系及びセンサ４０の座標系（赤外線カメラの座標系）の関係とを示す。Ａｚ軸１０の三次元座標はＸ０、Ｙ０及びＺ０において定義することができ、センサ４０の三次元座標はＸＣ、ＹＣ及びＺＣにおいて定義することができる。
【００２０】
図４及び図５には、ジンバル装置１の３軸ジンバル装置部分の具体的な構造を示す。図４に示すジンバル装置１は、センサ４０の視軸ｒを正面に向けた状態にある。第１のジンバル１１、第２のジンバル２１、第３のジンバル３１及びセンサ４０は、天頂方向（図４中、上側）を半球とする円柱形状の外装ケース２で覆われている。外装ケース２は、第１のジンバル１１の支持部１１Ａ、１１Ｂのそれぞれを覆う部分に不透明な外装領域２Ａを備え、この外装領域２Ａ間の円柱の対向する側面と天頂領域とに透明な可視領域２Ｂとを備えている。可視領域２Ｂはセンサ４０の可視範囲になる。図５に示すジンバル装置１は、センサ４０の視軸ｒを天頂に向けた状態にある。
【００２１】
前述の図１に示す第１の駆動手段１２には第１の実施の形態において電動モータが使用され、この電動モータの回転軸はＡｚ軸１０に相当し第１のジンバル１１に連結されている。第１の角度検出手段１４には電動モータの回転角度を検出するエンコーダを使用することができる。第１の角速度検出手段１３には、第１のジンバル１１に配設されたジャイロセンサを使用することができる。
【００２２】
同様に、第２の駆動手段２２には電動モータが使用され、この電動モータの回転軸はＥＬ軸２０に相当し第２のジンバル２１に連結されている。第２の角度検出手段２４にはエンコーダを、第２の角速度検出手段２３にはジャイロセンサを使用することができる。第３の駆動手段３２には電動モータが使用され、この電動モータの回転軸はｘＥＬ軸３０に相当し第３のジンバル３１に連結されている。第３の角度検出手段３４にはエンコーダを、第３の角速度検出手段３３にはジャイロセンサを使用することができる。
【００２３】
駆動制御部６０は、第１の駆動手段１２の回転制御を行う第１のサーボ制御部６１と、第２の駆動手段２２の回転制御を行う第２のサーボ制御部６２と、第３の駆動手段３２の回転制御を行う第３のサーボ制御部６３とを備えている。第１のサーボ制御部６１、第２のサーボ制御部６２、第３のサーボ制御部６３のそれぞれは角速度指令生成部５０から出力される角速度指令に基づき動作するようになっている。
【００２４】
［ジンバル装置の制御方法］
次に、前述のジンバル装置１の制御方法を説明する。
【００２５】
ジンバル装置１において、追尾目標はセンサ４０のセンサ座標系の原点（中心点）Ｏにくるように捕捉追尾されているので、図６に示すように、目標位置座標は原点からの追尾誤差（ΔＸ，ΔＹ）として与えることができる。追尾誤差は、原点Ｏから目標位置までのカメラ（例えばＣＣＤ）の画素数を算出することにより求めることができる。追尾制御は、センサ４０によるトラッキングを行い、図７に示すように、センサ座標系において追尾誤差（ΔＸ，ΔＹ）をＡｚ軸１０、ＥＬ軸２０、ｘＥＬ軸３０の目標速度指令に変換し、第１のジンバル１１、第２のジンバル２１、第３のジンバル３２の角速度制御を行なう。センサ４０の追尾誤差は原点Ｏからの二次元変位（ΔＸ，ΔＹ）として得ることができる。結果的に、図１に示す角速度指令生成部５０は、３軸のＡｚ軸１０、ＥＬ軸２０、ｘＥＬ軸３０のそれぞれの目標角速度指令（ω1,ω2,ω3）を生成する。
【００２６】
３軸制御において必要な２変数（ΔＸ，ΔＹ）から３変数（ω1,ω2,ω3）への変換は、無数の解が存在するので、一通りに決定することができない（不定）。しかし、角速度の組み合わせによっては、その解の制御において系が発散し、制御不能となることがあるため、本実施の形態に係るジンバル装置１においては、この複数解の中から、ジンバルロックを回避し、かつｘＥＬ軸３０の可動範囲内において良好な制御を行う、指標となる、できる限り小さな角速度指令値を算出する。
【００２７】
ｘＥＬ軸３０の可動範囲を考慮する必要性は、例えば、図４及び図５において説明した外装ケース２の不透明な外装領域２Ａを有する構造上の制約に基づくものであり、第３のジンバル３１の可動範囲は、実用上、±１０度〜±２０度の角度範囲内になる。
【００２８】
角速度指令の大きさについては、各軸の角速度からなる角速度ベクトル
【数１】
2006106910.tif 000003
【００２９】
の大きさ
【数２】
2006106910.tif 000004
【００３０】
を小さくすることと定義する。ここで、記号tは転置を表す。
【００３１】
今、ジンバル軸数をｍとする。例えば、２軸ジンバル構造であればジンバル軸数ｍ＝２であり、３軸ジンバル構造であればジンバル軸数ｍ＝３である。図８に示す、センサ４０の視線方向を表す視軸ベクトル
【数３】
2006106910.tif 000005
【００３２】
とジンバルの軸角度ベクトル
【数４】
2006106910.tif 000006
【００３３】
との関係は下記（１）式において表すことができる。
【数５】
2006106910.tif 000007
【数６】
2006106910.tif 000008
【００３４】
はｍ次のベクトル空間を表す。上記（１）式の微小変化を表す関係式は下記（２）式で表すことができる。
【数７】
2006106910.tif 000009
【００３５】
この（２）式は下記ヤコビ行列を係数として表されている。
【数８】
2006106910.tif 000010
【００３６】
角速度制御系を構成する場合、視軸速度ベクトル
【数９】
2006106910.tif 000011
【００３７】
とジンバルの軸角速度ベクトル
【数１０】
2006106910.tif 000012
【００３８】
との関係を表す下記（３）式が使用される。
【数１１】
2006106910.tif 000013
【００３９】
上記（３）式により、ジンバル角速度
【数１２】
2006106910.tif 000014
【００４０】
からカメラ視軸速度
【数１３】
2006106910.tif 000015
【００４１】
を求めることができる。
【００４２】
今、センサ４０の視軸ベクトル
【数１４】
2006106910.tif 000016
【００４３】
を図３に示すカメラ座標系
【数１５】
2006106910.tif 000017
【００４４】
において表した視軸ベクトルを
【数１６】
2006106910.tif 000018
【００４５】
とする。センサ４０上において目標が微小移動した場合、この微小移動した視軸ベクトルは下記（４）において表すことができる。
【数１７】
2006106910.tif 000019
【００４６】
上記（４）式のｘ、ｙ成分はセンサ４０上における目標誤差に相当し、ｚ方向成分
【数１８】
2006106910.tif 000020
【００４７】
はゼロとなる。そこで、
【数１９】
2006106910.tif 000021
【００４８】
のｘ、ｙ成分のみに着目して、新たに下記（５）式から（８）式を定義する。
【数２０】
2006106910.tif 000022
【００４９】
２軸ジンバル構造の場合、
【数２１】
2006106910.tif 000023
【００５０】
であり、
【数２２】
2006106910.tif 000024
【００５１】
（ｄｅｔは行列式記号）ならばＪ（θ）は正則であり逆行列が存在し、下記（９）式において表すことができる。
【数２３】
2006106910.tif 000025
【００５２】
すなわち、センサ４０上の目標の移動に対して、センサ４０を移動するためのジンバル角速度が存在する。
【００５３】
しかしながら、det Ｊ（θ）＝０の場合には、上記（９）式は成立せず、上記（７）式を満足するジンバル角速度は存在しない。このような姿勢は特異姿勢と呼ばれている。２軸ジンバルタレットにおいては、センサが天頂を向いた場合にdet Ｊ（θ）＝０になり、追尾制御が不能になる。また、特異姿勢近傍においては、det Ｊ（θ）が非常に小さな値になり、追尾制御が不安定になる。
【００５４】
一方、ジンバル軸数が３軸以上の場合、
【数２４】
2006106910.tif 000026
【００５５】
は正則でないため逆行列を持たない。Ｊ（θ）が正則でない場合、フルランクでない場合には擬似逆行列を適用することができる。ある角速度ベクトル
【数２５】
2006106910.tif 000027
【００５６】
を与えた場合、上記（７）式の解
【数２６】
2006106910.tif 000028
【００５７】
は下記（１１）式により表すことができる。
【数２７】
2006106910.tif 000029
【００５８】
このとき、目標の移動速度
【数２８】
2006106910.tif 000030
【００５９】
との誤差の大きさ（以下、誤差ノルムという。）
【数２９】
2006106910.tif 000031
【００６０】
を考え、この誤差を最小にする角速度ベクトル
【数３０】
2006106910.tif 000032
【００６１】
の解を求める。
【００６２】
上記（７）式において、新値に対する誤差の大きさを示す下記（１１）式において表される誤差ノルムを考える。
【数３１】
2006106910.tif 000033
【００６３】
この（１２）式の値を最小にする解は下記（１２）式により与えられる。
【数３２】
2006106910.tif 000034
【００６４】
ここで、
【数３３】
2006106910.tif 000035
【００６５】
は擬似逆行列であり、
【数３４】
2006106910.tif 000036
【００６６】
は単位行列であり、
【数３５】
2006106910.tif 000037
【００６７】
は任意ベクトルである。上記（７）式が解を持つ場合には、上記（１２）式が一般解を与え、解を持たなければ、（１１）式に示す誤差ノルムが最小になる近似解を与える。ｋは任意のベクトルであるから、上記（１２）式の第２項は冗長項を表す。（１２）式において表される一般解の中で、特にジンバル角速度の解
【数３６】
2006106910.tif 000038
【００６８】
のノルム
【数３７】
2006106910.tif 000039
【００６９】
を最小にする理想的な解は下記（１３）式において表すことができる。
【数３８】
2006106910.tif 000040
【００７０】
また、詳細は省略するが、
【数３９】
2006106910.tif 000041
【００７１】
の場合において、擬似逆行列は下記（１４）式により与えられる。
【数４０】
2006106910.tif 000042
【００７２】
前述の通り、角速度ベクトルのノルムを最小にするには（１３）式に従って各軸の角速度指令を計算すればよい。ところが、この計算だけでは不十分であり、３軸目のｘＥＬ軸３０の可動範囲が例えば±１０度の極端に狭い可動範囲に設定されている場合も考慮する必要があり、上記（１３）式を用いた計算だけでは容易にｘＥＬ軸３０の可動範囲を越えてしまう。
【００７３】
ｘＥＬ軸３０を中心として回転する第３のジンバル３１の動作について簡単に説明する。例えば、図８に示すように、センサ４０の視軸が正面を向くような姿勢にタレットがあり、目標がＡｚ軸１０周り（θ１軸）に移動する場合を考える。このとき、上記（１３）式によれば、角速度ベクトルの最小値を与える条件は
【数４１】
2006106910.tif 000043
【００７４】
となり、Ａｚ軸１０及びｘＥＬ軸３０が同じ角速度で回転し続ける。Ａｚ軸１０は無限に回転することができるが、ｘＥＬ軸３０はすぐに可動範囲を超えてしまい、追尾制御が不連続になる。
【００７５】
また、図９に示すように、センサ４０の視軸が天頂を向く姿勢にタレットがあり、かつ目標がｘＥＬ軸３０周り（θ３軸）に移動する場合を考える。このとき、上記（１４）式を満たす最適な解は
【数４２】
2006106910.tif 000044
【００７６】
となり、ｘＥＬ軸３０だけが回転し続けることになる。２軸ジンバル構造の場合は、ジンバルロックにはならないものの、ｘＥＬ軸３０の可動範囲を超える時点において追尾制御は不能になる。上記（１３）式とは別に、ｘＥＬ軸３０の動作にしきい値などの方法を加えて制約条件を与えることもできるが、しきい値に該当する条件の領域において追尾制御が不連続になる。以上のような理由に基づき、上記（１３）式による３軸制御だけでは、実用上、課題が残る。
【００７７】
上記（１２）式の第２項の冗長項にはｋという任意に選択することができるベクトルが存在する。このｋを使ってｘＥＬ軸３０の可動範囲の制約条件を角速度指令の生成に反映させることを考える。下記（１５）式に示す、あるスカラー評価関数を考え、この関数の値σをできる限り大きくするベクトルｋを考える。
【数４３】
2006106910.tif 000045
【００７８】
このようなベクトルｋは関数Ψ（θ）の最急勾配ベクトルであるから、θによる関数Ψ（θ）の偏微分を取ればよい。すなわち、下記（１６）式から（１８）式において表すことができる。
【数４４】
2006106910.tif 000046
【００７９】
このとき、上記（１２）式は下記（１９）式となる。
【数４５】
2006106910.tif 000047
【００８０】
この上記（１９）式の第１項は本実施の形態に係る第１の角速度指令の算出値であり、同様に第２項は第２の角速度指令の算出値である。上記（１５）式の評価関数の微分を取ると、下記（２０）式になる。
【数４６】
2006106910.tif 000048
【数４７】
2006106910.tif 000049
【００８１】
は少なくとも非負対称行列であるから、２次形式における非負定行列になる。すなわち、（１９）式の第２項は常に非負（常にゼロ以上）になる。従って、上記（１６）式から（１８）式において定められたベクトルｋは評価関数の値σを大きくすることができる。
【００８２】
評価関数Ψ（θ）は自由に選定することができる。本実施の形態においては、以下の要求に基づき決定された。
【００８３】
（１）ｘＥＬ軸３０はできる限り可動範囲の真ん中にくるようにしたい。
【００８４】
（２）但し、要求（１）においてｘＥＬ軸３０の位置が極端に真ん中に制約されることは避けたい。可動範囲内外の境界に近づいた場合には、強い制動を与えたい。
【００８５】
（３）ＥＬ軸２０はできる限り天頂方向を向かないようにしたい。
【００８６】
３軸ジンバル構造において冗長成分は１自由度しかないので、上記要求（１）と（３）とをそれぞれ別個の評価関数により達成することはできない。そこで、評価関数として、下記（２１）式から（２４）式を考える。
【数４８】
2006106910.tif 000050
【００８７】
上記（２１）式の第１項は２軸ジンバル構造の場合のヤコビ行列の行列式のノルムに相当する。ＥＬ軸２０が天頂に近づくにつれて値が小さくなり、天頂すなわちθ２＝９０度において値がゼロになる。第２項は、θ３の可動範囲を上記（２２）式において−１０＜θ３＜１０（単位は「度」）のように与えた場合、図１０に示すなべ底型の関数になる。図１０において、横軸はｘＥＬ軸３０の角度を示し、縦軸はβを除く第２項の値を示す。各項の係数の符号を上記（２３）式、（２４）式のように与えることで、評価関数Ψ（θ）をできる限り大きくする冗長項ベクトルｋを求めることができる。この算出方法は上記（１６）式及び（１７）式に示す通りである。
【００８８】
冗長項ベクトルｋの各項を具体的に求めてみると、上記（２１）式を（１８）式へ代入すると、下記（２５）式から（２７）式が得られる。
【数４９】
2006106910.tif 000051
【００８９】
この評価関数は、タレットが天頂から離れている場合には第２項のｘＥＬ軸３０の可動範囲に関する制約が支配的となる。また、天頂に近づくと第１項のＥＬ軸２０の条件が効果を表す。αとβは自由に選ぶことができる。
【００９０】
次に、上記（１９）式の具体的な算出方法を説明する。まず最初に、前述の図３に示すように、ベース座標系Σ0と各軸の座標系Σi（ｉ＝１，２，３）、及びセンサ座標系Σcを定義する。Ａｚ軸１０からｘＥＬ軸３０までのジンバル角はθi（ｉ＝１，２，３）とする。更に、各座標系間の変換行列は
【数５０】
2006106910.tif 000052
【００９１】
（ｉ，ｊ＝０，１，２，３，ｃ）とする。ここで、変換行列はΣi座標系からΣj座標系への変換行列（３×３行列）を表す。このとき、下記（Ａ１）式から（Ａ５）式が成り立つ。
【数５１】
2006106910.tif 000053
【００９２】
この（Ａ５）式の
【数５２】
2006106910.tif 000054
【００９３】
はベース座標系から見たセンサ４０の座標系の変換行列であり、（Ａ５）式の第３列は視軸ベクトル（センサ４０のｚ軸方向のベクトル）
【数５３】
2006106910.tif 000055
【００９４】
に一致するので、上記（１）式は下記（Ａ６）式に示すように、視軸ベクトルを電動モータの回転角度において表現することができる。
【数５４】
2006106910.tif 000056
【００９５】
この（Ａ６）式から上記（３）式の視軸速度ベクトル
【数５５】
2006106910.tif 000057
【００９６】
とジンバルの軸角速度ベクトル
【数５６】
2006106910.tif 000058
【００９７】
との関係は下記（Ａ７）式において表すことができる。すなわち、上記（Ａ６）式を微分することにより速度を抽出することができる。
【数５７】
2006106910.tif 000059
【００９８】
但し、三角関数は
【数５８】
2006106910.tif 000060
【００９９】
のように記述するものとする（以降同様）。
【０１００】
前述の図３に示すように、センサ４０の視軸ベクトル
【数５９】
2006106910.tif 000061
【０１０１】
をセンサ４０の座標系
【数６０】
2006106910.tif 000062
【０１０２】
において表したものを
【数６１】
2006106910.tif 000063
【０１０３】
と定義すると、下記（Ａ８）式を得ることができる。
【数６２】
2006106910.tif 000064
【０１０４】
上記（５）式、（６）式、（７）式及び（８）式２基づき、下記（Ａ９）式から（１２）式を定義することができる。
【数６３】
2006106910.tif 000065
【０１０５】
上記（Ａ１２）式のヤコビ行列は上記（Ａ８）式より、下記（Ａ１３）式において表すことができる。
【数６４】
2006106910.tif 000066
【０１０６】
よって、視軸ベクトル
【数６５】
2006106910.tif 000067
【０１０７】
を表す上記（Ａ１１）式を書き下すと、下記（Ａ１４）を得ることができる。
【数６６】
2006106910.tif 000068
【０１０８】
すなわち、電動モータの角速度を与えると、センサ４０の座標系においてセンサ４０の視軸ベクトルの移動速度を算出することができる。
【０１０９】
上記（Ａ１１）式及び（Ａ１４）式において、
【数６７】
2006106910.tif 000069
【０１１０】
から
【数６８】
2006106910.tif 000070
【０１１１】
を求めるには、上記（１２）式、つまり下記（Ａ１５）式に示す関係を用いる。
【数６９】
2006106910.tif 000071
【０１１２】
ここで、
【数７０】
2006106910.tif 000072
【０１１３】
は擬似逆行列であり、
【数７１】
2006106910.tif 000073
【０１１４】
は単位行列であり、
【数７２】
2006106910.tif 000074
【０１１５】
は任意ベクトルである。本実施の形態においては、常に
【数７３】
2006106910.tif 000075
【０１１６】
であり、擬似逆行列
【数７４】
2006106910.tif 000076
【０１１７】
は上記（１４）式と（Ａ１３）式との関係を用いて下記（Ａ１６）に示すように計算することができる。
【数７５】
2006106910.tif 000077
【０１１８】
ここで、
【数７６】
2006106910.tif 000078
【０１１９】
は
【数７７】
2006106910.tif 000079
【０１２０】
の転置行列であり、
【数７８】
2006106910.tif 000080
【０１２１】
は
【数７９】
2006106910.tif 000081
【０１２２】
の逆行列を表す。更に、上記（Ａ１５）式の第２項の冗長項の係数行列
【数８０】
2006106910.tif 000082
【０１２３】
は上記（Ａ１３）式及び（Ａ１６）式を用いて下記（１７）式として表すことができる。
【数８１】
2006106910.tif 000083
【０１２４】
そして、上記（Ａ１５）式の３軸制御を上記（１９）式、（２５）式、（２６）式、（２７）式と、上記（Ａ１６）式、（Ａ１７）式とを用いて書き直すと、下記（Ａ１８）式を得ることができる。
【数８２】
2006106910.tif 000084
【０１２５】
この（Ａ１８）式において、第１項は、目標を追尾する際の、各ジンバル１１、２１、３１の駆動手段（電動モータ）１２、２２、３２の回転速度から成る角速度ベクトルの大きさが最小となるような第１の角速度指標値である。第１の角速度指標値は第１の指令算出部５１において生成される。第２項は、ｘＥＬ軸３０が可動範囲を越えず、できる限り可動範囲の中央にあるように制御し、かつ第２のジンバル２１の姿勢が天頂方向から遠ざかるように各ジンバル軸を制御する第２の角速度指標値である。この第２の角速度指標値は第２の指令算出部５２において生成される。
【０１２６】
角度指令生成部５においては、第１の角速度指標値と第２の角速度指標値とを加算して、角速度指令値を算出し、各軸のサーボ制御部６１、６２、６３に対して角速度指令値を送出する。
【０１２７】
［ジンバル装置の追尾性能］
前述の構成並びに制御方法を備えたジンバル装置１においては、図１１に示すように、センサ４０の視軸が天頂方向に向いている位置からジンバルロックが発生する矢印Ａ方向に移動する場合も、良好な追尾を行うことができる。目標（ターゲット）が天頂から矢印Ａ方向に移動する場合、本実施の形態に係るジンバル装置１は、図１２に示すように、１５度／秒の速度において目標を追尾することができる。図１２において、横軸は時間を示し、縦軸は視軸の角度を示す。
【０１２８】
このとき、本実施の形態に係るジンバル装置１においては、図１３及び図１４に示すように、Ａｚ軸１０は約１．５秒かけて実現可能な角速度において１８０°回転している。また、ｘＥＬ軸３０も実現可能な角速度において移動し、かつ角度移動量は６°程度に収まり、ｘＥＬ軸３０の可動範囲内において連続的な追尾制御を実現することができる。
【０１２９】
以上説明したように、本発明の実施の形態に係るジンバル装置１においては、天頂からＥＬ軸２０の回転方向と直交する方向に目標が移動する場合、角速度が過大にならないように、Ａｚ軸１０とｘＥＬ軸３０との動作を配分しながら追尾動作を行うことができる。すなわち、実現可能なジンバル可動範囲を越えることなく、ジンバルロックを避け、全天にわたり連続的な追尾制御を安定して実現することができるジンバル装置１を提供することができる。
【０１３０】
なお、本発明は、上記実施の形態に限定されるものではない。例えば、本発明は、ｘＥＬ軸３０を中心に回転する第３のジンバル３１に、レーザ、赤外線、レーダ等のセンサを搭載したジンバル装置としてもよい。
【図面の簡単な説明】
【０１３１】
【図１】本発明の実施の形態に係るジンバル装置の全体構成を示すブロック図である。
【図２】図１に示すジンバル装置のジンバルの斜視図である。
【図３】図１に示すジンバル装置の座標系を示す図である。
【図４】図１に示すジンバル装置の具体的な要部の外観構造を示す斜視図である。
【図５】図１に示すジンバル装置の具体的な要部の外観構造を示す斜視図である。
【図６】図１に示すジンバル装置のセンサ座標系を説明する図である。
【図７】図１に示すジンバル装置において追尾誤差から角速度指令を生成する流れを説明する図である。
【図８】図１に示すジンバル装置の要部の斜視図である。
【図９】図１に示すジンバル装置の要部の斜視図である。
【図１０】図１に示すジンバル装置の制御方法においてｘＥＬ軸と評価関数との関係を示す図である。
【図１１】図１に示すジンバル装置の要部斜視図である。
【図１２】図１に示すジンバル装置のセンサの動作特性を示す図である。
【図１３】図１に示すジンバル装置の各軸の角度軌跡を示す図である。
【図１４】図１に示すジンバル装置の各軸の角速度軌跡を示す図である。
【図１５】従来技術に係るＡｚ軸とＥＬ軸とを説明する図である。
【図１６】従来技術に係る天頂方向を説明する図である。
【図１７】従来技術に係る２軸ジンバル装置の斜視図である。
【符号の説明】
【０１３２】
１…ジンバル装置、
１０…第１のジンバル軸（Ａｚ軸）、１１…第１のジンバル、１２…第１の駆動手段、１３…第１の角速度検出手段、１４…第１の角度検出手段、２０…第２のジンバル軸（ＥＬ軸）、２１…第２のジンバル、２２…第２の駆動手段、２３…第２の角速度検出手段、２４…第２の角度検出手段、３０…第３のジンバル軸（ｘＥＬ軸）、３１…第３のジンバル、３２…第３の駆動手段、３３…第３の角速度検出手段、３４…第３の角度検出手段、４０…センサ、５０…角速度指令生成部、５１…第１の指令算出部、５２…第２の指令算出部、５３…角速度検出部、５４…角度検出部、６０…駆動制御部、６１…第１のサーボ制御部、６２…第２のサーボ制御部、６３…第３のサーボ制御部。
【図１】
2006106910.tif 000085
【図２】
2006106910.tif 000086
【図３】
2006106910.tif 000087
【図４】
2006106910.tif 000088
【図５】
2006106910.tif 000089
【図６】
2006106910.tif 000090
【図７】
2006106910.tif 000091
【図８】
2006106910.tif 000092
【図９】
2006106910.tif 000093
【図１０】
2006106910.tif 000094
【図１１】
2006106910.tif 000095
【図１２】
2006106910.tif 000096
【図１３】
2006106910.tif 000097
【図１４】
2006106910.tif 000098
【図１５】
2006106910.tif 000099
【図１６】
2006106910.tif 000100
【図１７】
2006106910.tif 000101