JP6394002B2 - 追尾制御装置および追尾制御方法 - Google Patents

Description

本発明は、追尾制御装置および追尾制御方法に関し、特に、目標物に対する光軸を補正する機能を備えた追尾制御装置および追尾制御方法に関する。

従来の光軸補正機能を備えた追尾制御装置は、図６のブロック構成図に示すような構成からなっている。図６は、従来の追尾制御装置の構成を示すブロック構成図である。図６に示す従来の追尾制御装置１０Ａは、特許文献１の特開２０１２−２５３４８４号公報「捕捉追尾制御装置」にも記載されているように、目標位置情報を検出する目標位置検出器２０を備え、目標位置検出器２０の検出結果に基づいて、指向させたい方向に追尾制御装置１０Ａの光学部全体を指向させる粗追尾駆動部３１（大型ジンバル等）を制御する粗追尾制御部３０と、光路途中に設けた偏光装置を駆動させることにより高精度に該光学部を指向させる精追尾駆動部４１を制御する角度近似補正部４０すなわち精追尾制御部と、を備えて構成されている。

粗追尾制御部３０の他に、角度近似補正部４０すなわち精追尾制御部を備えている理由は、粗追尾制御部３０によって制御する粗追尾駆動部３１は構造的に大型となることがあり、高精度に光軸の指向誤差角を補正し切れないことが生じる可能性があるためであり、角度近似補正部４０すなわち精追尾制御部により、光路途中に設けた偏光装置すなわちミラーを駆動することにより、正しい指向位置まで光軸を補正するようにすることが必要である。

図６の従来の追尾制御装置１０Ａの動作を、図７のフローチャートを用いて説明する。図７のフローチャートにおいて、粗追尾制御部３０は、目標位置検出器２０により検出した目標物に関する画像データを取得して（ステップＳ１）、追尾制御装置１０Ａの光学部全体を、検出した目標物の方向に指向させるための粗追尾用角度を算出して（ステップＳ２）、粗追尾駆動部３１に対して出力する。

さらに、角度近似補正部４０すなわち精追尾制御部において、光路途中に設けられた偏光装置すなわちミラーの反射角／駆動角と光学角との近似的な関係からミラーを駆動する駆動角を算出することにより（ステップＳ３）、ミラーの角度を補正する補正角度を算出する（ステップＳ４）。しかる後、算出した補正角度を平滑化フィルタにより平滑化して（ステップＳ５）、目標角度、目標角速度を算出し（ステップＳ６）、精追尾駆動部４１に対して出力する（ステップＳ７）。而して、光学部全体を目標物の方向に指向させることができる。

ステップＳ３においてミラーの駆動角を近似的に算出する計算例を次に説明する。駆動されるミラーすなわち駆動ミラーのＡＺ軸傾き角度をＸ、駆動ミラーのＥＬ軸傾き角度をＹ、ジンバルのＥＬ角をθとすると、駆動ミラー射出光の傾きの近似値α２、β２は、次の式（１）で与えられる。

一方、望遠鏡の入射角の近似値α３、β３、および、望遠鏡射出角の近似値α４、β４は、それぞれ、次の式（２）および式（３）で与えられる。

ここで、鏡筒座標系において、ＡＺ軸、ＥＬ軸に対する偏角量δＡＺ、δＥＬは、次の式（４）で与えられる。

以上の各式より、方位角と高度との変位から、駆動ミラーの角度は、以下の式（５）を用いて近似的に求めることができる。

特開２０１２−２５３４８４号公報（第３−５頁）

しかしながら、従来の光軸補正機能を備えた画像追尾制御装置においては、次のような問題があった。第１に、光軸補正機能を行なう駆動ミラーの制御に当たって、駆動ミラーの角度を微調整する際の補正量を正確に算出することが複雑であるため、近似計算を用いて簡易的に目標角を算出していた。このため、補正角度の目標値に誤差が生じてしまい、高精度に光軸を補正することができないという問題があった。さらに、第２に、取得される角度ずれ情報に画像処理分の遅延時間が含まれてしまうため、補正角度の目標値に誤差が生じてしまい、リアルタイムでの高精度な光軸補正ができないという問題もあった。

つまり、駆動ミラーの角度を微調整する際に、ジンバルを用いる場合においては、光軸を９０°折り曲げるために４５°の傾斜ミラーを採用している。このため、光軸の座標変換処理を行なったりすることが必要となり、補正量の算出が複雑になるので、前述のフローチャートにおいて説明したように、４５°入射角を有した光の反射をモデルとして簡易的な近似計算を行なって目標角を算出していた。ところが、光路途中に配置した精追尾駆動部４１によって光路を微調整した場合、精追尾駆動部４１以降の経路において光軸が回転軸の交点を通過していないために、算出された近似値が、正確な目標値にはなっておらず、指向角度の正確な補正ができなかったり、角度補正に時間を要してしまったりする。このため、算出した補正値にゲインをかける等の処理をさらに追加することによって調整せざるを得ない事態を招いていた。

また、抽出される角度情報は、前述のように、画像処理分の遅延時間を含んでいるため、補正角度の目標値に誤差が生じてしまい、リアルタイムでの高精度な光軸補正ができないという問題もあった。

（本発明の目的）
本発明は、かかる問題に鑑みてなされたものであり、正確な目標追尾性能を達成することが可能な追尾制御装置および追尾制御方法を提供することを、その目的としている。

前述の課題を解決するため、本発明による追尾制御装置および追尾制御方法は、主に、次のような特徴的な構成を採用している。

（１）本発明による追尾制御装置は、目標物に対して光学部全体を指向させて該目標物を粗追尾する粗追尾駆動部と光路上に配置した駆動ミラー及び反射ミラーの角度を微調整して前記目標物を精追尾する精追尾駆動部とを少なくとも備えた追尾制御装置であって、前記粗追尾駆動部を制御する粗追尾制御部と前記精追尾駆動部を制御する精追尾制御部とを少なくとも備え、前記粗追尾制御部は、前記目標物の現在位置を予測推定して、予測推定した結果に基づいて、前記粗追尾駆動部を制御し、前記精追尾制御部は、前記粗追尾駆動部および前記精追尾駆動部からの情報に基づき、光路上に配置した前記駆動ミラー及び前記反射ミラーの法線ベクトルを算出し、算出した前記法線ベクトルに基づいて、前記精追尾駆動部を制御することを特徴とする。

（２）本発明による追尾制御方法は、目標物に対して光学部全体を指向させて該目標物を粗追尾する粗追尾駆動部と光路上に配置した駆動ミラー及び反射ミラーの角度を微調整して前記目標物を精追尾する精追尾駆動部とを少なくとも備えた追尾制御装置における追尾制御方法であって、前記粗追尾駆動部を制御する粗追尾制御ステップと前記精追尾駆動部を制御する精追尾制御ステップとを少なくとも有し、前記粗追尾制御ステップは、前記目標物の現在位置を予測推定して、予測推定した結果に基づいて、前記粗追尾駆動部を制御し、前記精追尾制御ステップは、前記粗追尾駆動部および前記精追尾駆動部からの情報に基づき、光路上に配置した前記駆動ミラー及び前記反射ミラーの法線ベクトルを算出し、算出した前記法線ベクトルに基づいて、前記精追尾駆動部を制御することを特徴とする。

本発明の追尾制御装置および追尾制御方法によれば、以下のような効果を奏することができる。

すなわち、本発明による追尾制御装置および追尾制御方法は、目標位置の高精度の予測を行ない、かつ、駆動ミラー、反射ミラーの各ミラーの法線ベクトルを利用して、光軸ベクトルを正確に演算しながら、各ミラーの駆動角度を制御することにより、追尾特性を大幅に向上させることが可能である。

本発明による追尾制御装置の一構成例を示すブロック構成図である。 図１の追尾制御装置に関する外観イメージの一例を示すイメージ図である。 図１の追尾制御装置の目標位置推定器すなわち粗追尾制御機能および協調制御補償器すなわち精追尾制御機能の動作アルゴリズムの一例を示すフローチャートである。 図１の追尾制御装置に備えた駆動ミラーおよび反射ミラーにおける画像補正のイメージを説明するためのイメージ図である。 図１の追尾制御装置において駆動ミラーの目標角度およびジンバル目標値を算出する手順の一例を示すフローチャートである。 従来の追尾制御装置の構成を示すブロック構成図である。 図６の従来の追尾制御装置の動作を説明するためのフローチャートである。

以下、本発明による追尾制御装置および追尾制御方法の好適な実施形態について添付図を参照して説明する。なお、以下の説明においては、本発明による追尾制御装置および追尾制御方法について説明するが、かかる追尾制御方法をコンピュータにより実行可能な追尾制御プログラムとして実施するようにしても良いし、あるいは、追尾制御プログラムをコンピュータにより読み取り可能な記録媒体に記録するようにしても良いことは言うまでもない。また、以下の各図面に付した図面参照符号は、理解を助けるための一例として各要素に便宜上付記したものであり、本発明を図示の態様に限定することを意図するものではないことも言うまでもない。

（本発明の特徴）
本発明の実施形態の説明に先立って、本発明の特徴についてその概要をまず説明する。本発明による追尾制御装置は、飛翔体等の目標物を捕捉追尾するために、粗追尾制御を行なう粗追尾駆動部と、精追尾制御を行なう精追尾駆動部と、粗追尾駆動部および精追尾駆動部を協調させて制御する協調制御器と、を少なくとも備えて構成されている。そして、光軸の制御方式として、協調制御器において目標物の運動および目標物に対する追尾動作の遅延を加味して目標物の位置を予測推定し、かつ、各ミラー部の、光の入射／反射ベクトルおよびミラーの法線ベクトルの関係を使用して、正確なミラー目標角度を決定する方式を導入することにより、高精度に目標を追尾することを主要な特徴としている。

すなわち、本発明による追尾制御装置においては、精追尾駆動部と粗追尾駆動部と協調制御器とを少なくとも備えた追尾制御装置において、追尾制御装置全体を制御する協調制御器に、画像情報から取得される目標物の位置情報の特定および追記動作等の遅延時間分の補正を行なうために、粗追尾駆動部を制御する粗追尾制御機能の中に予測推定機能を設けている。また、協調制御器に、光軸の補正を行なうために光路上に設けた２軸以上を駆動する精追尾駆動部の正確な制御を可能にするために、反射ミラー、駆動ミラーの法線ベクトルを利用して正確な目標駆動角度を演算する精追尾制御機能を設けている。而して、高精度な捕捉追尾を実現することを主要な特徴としている。すなわち、精追尾駆動部からの角度出力値と粗追尾駆動部からの角度出力値とに基づき、法線ベクトルを算出して、算出した法線ベクトルに基づいて、精追尾駆動部と粗追尾駆動部とを制御することによって、正確なミラー目標角度に調整することを可能にすることを主要な特徴としている。

（実施形態の構成例）
本発明による追尾制御装置の一実施形態としてその構成例について、図１を参照して詳細に説明する。図１は、本発明による追尾制御装置の一構成例を示すブロック構成図である。図１に示す追尾制御装置１０は、飛翔体等の目標物を捕捉追尾するために、カメラ等の画像取得部２００が撮像した目標物に関する画像情報（偏角情報、測距情報等の情報）を制御装置１００において画像処理することにより得られる画面上の目標物の位置情報に基づいて、駆動部３００の粗追尾駆動部３０１および精追尾駆動部３０２を制御して、高精度に角度補正を行なうことが可能な構成としている。ここで、粗追尾駆動部３０１は、ＡＺ軸（アジマス軸）／ＥＬ軸（エレベーション軸）の２軸以上を駆動する大型ジンバル等を備え、また、精追尾駆動部３０２は、ＡＺ軸／ＥＬ軸の２軸以上を高精度に駆動する高精度追尾機構を備えている。

制御装置１００には、粗追尾駆動部３０１を制御するための粗追尾制御器１０１、精追尾駆動部３０２を制御するための精追尾制御器１０２、追尾制御装置１０の各部を協調させて全体を制御する協調制御器１０３を少なくとも備えている。粗追尾制御器１０１には、シンバル制御補償器１０１ａ等を含み、精追尾制御器１０２には、高精度追尾機構用制御補償器１０２ａ等を含む。

また、協調制御器１０３には、画像取得部２００が撮像した目標物に関する画像情報を画像処理し、粗追尾制御部として、大凡の目標物に関する目標角度と目標角速度とを精度良く推定する目標位置推定器１０３ａ、および、精追尾制御部として、目標位置推定器１０３ａが推定した目標角度と目標角速度とを参照して、さらに、高精度に目標物に関する目標角度と目標角速度とを算出する協調制御補償器１０３ｂ、を少なくとも備えている。つまり、目標位置推定器１０３ａの中には、粗追尾制御機能の一つとして、目標物に関する画像情報から取得される目標位置情報の遅延時間分の補正を行ないながら目標物を予測推定する機能を備えている。また、協調制御補償器１０３ｂの中には、精追尾制御機能の一つとして、光軸の補正を行なうために、反射ミラー、駆動ミラーの法線ベクトルを管理して、正確な目標駆動角度を演算することによって、光路上に設けた２軸以上を駆動する精追尾駆動部３０２の正確な制御を行なう機能を備えている。

図２は、図１の追尾制御装置１０に関する外観イメージの一例を示すイメージ図である。図２に示すように、追尾制御装置１０は、カメラ等の目標撮像装置からなる画像取得部２００の他、レーザ発生器等の発光部４００、発光部４００から出力されたレーザ光が入射される駆動ミラー５００、駆動ミラー５００からのレーザ光を反射させて目標物に向かって射出する反射ミラー６００を備え、垂直方向のＡＺ軸の周りまたは水平方向のＥＬ軸の周りを、ジンバル等により回動可能な構成とされている。駆動ミラー５００は、アウターシンバル側に配置され、反射ミラー６００は、光学部内反射ミラーであり、ＥＬジンバル側の光学部内に配置されている。駆動ミラー５００は、光軸を補正するために、図１に示した粗追尾駆動部３０１、精追尾駆動部３０２により任意の角度、角速度で、Ｘ軸、Ｙ軸の周りを回動可能な構成とされている。

（実施形態の動作の説明）
次に、図１に示した追尾制御装置１０の動作について、追尾制御を実現するための主構成要素の一つである協調制御器１０３の目標位置推定器１０３ａすなわち粗追尾制御機能および協調制御補償器１０３ｂすなわち精追尾制御機能における動作の一例を以下に説明する。図３は、図１に示した本追尾制御装置１０の目標位置推定器１０３ａすなわち粗追尾制御機能および協調制御補償器１０３ｂすなわち精追尾制御機能の動作アルゴリズムの一例を示すフローチャートである。

図３に示すフローチャートにおいて、粗追尾制御機能側の動作に関しては、ステップＳ１、Ｓ２は、図７に示した従来の追尾制御装置における粗追尾制御機能と同様の動作を行なう。しかし、本追尾制御装置１０の場合は、ステップＳ１とステップＳ２との間にステップＳ１Ａをさらに挿入し、図３のフローチャートの左上側に飛翔体として例示しているように、移動する目標物に関する画像情報を取得し該目標物に対する光学部全体の指向動作を完了するまでの遅延時間分の補正を行ないながら、該目標物の位置を予測推定する動作が追加されている。ステップＳ１Ａの動作の詳細については後述する。また、精追尾制御機能の動作に関しては、ステップＳ４ないしステップＳ７は、図７に示した従来の追尾制御装置における精追尾制御機能と同様の動作を行なう。しかし、本追尾制御装置１０の場合は、ミラーの駆動角を正確に算出するために、従来の追尾制御装置における精追尾制御機能のステップＳ３の代わりに、粗追尾制御機能において予測推定した目標物の現在位置に関する３次元データを用いて、駆動ミラー、反射ミラーの法線ベクトルをまず算出し、その結果を用いて駆動ミラーの駆動角を算出するステップとして、ステップＳ３ＡないしステップＳ３Ｃを用いている。この結果、図３のフローチャートの左下側に例示しているように、目標物（飛翔体）の位置を画面中央に正確に移動させて表示することができる。ステップＳ３ＡないしステップＳ３Ｃの動作の詳細については後述する。

（１）粗追尾制御機能（目標位置推定器１０３ａ）
協調制御器１０３の目標位置推定器１０３ａすなわち粗追尾制御機能においては、座標演算器から出力された目標物の現在位置(ただし、過去の情報を使用して計算されるため、実質は、過去の位置)を該目標物の運動を考慮した運動方程式および粗追尾駆動部３０１の動作遅延を加味し、図３のステップＳ１Ａに示したように、目標位置を予測推定する。ここで、以下に示すような運動方程式を基本として、目標物の移動先を先読みして目標値を与えることによって、全体の系の制御帯域に制限されることなく、目標位置を予測推定することができる。

座標演算器において算出された目標物の位置情報Ｐ（ｋ）、速度情報Ｖ（ｋ）、加速度情報Ａ（ｋ）が式（６）で与えられるとすると、次の式（７）が成立する。

そこで、状態ベクトルＸ_ｋ、ｕ_ｋを式（８）で定義すると、式（７）を基にして、追尾予測システムとしては、次の式（９）、式（１０）によって与えられる状態方程式と観測方程式とからなる外部入力を伴う基本システムによって表現することができる。

したがって、追尾予測システムとして例えばカルマンフィルタを適用した場合、以下のようなアルゴリズムを用いて、最終的に、状態ベクトルＸ_ｋの最小分散推定量を求めることができる。つまり、更新については、次の式（１１）のフィルタ方程式、式（１２）の最適カルマンゲイン、および、式（１３）の推定共分散行列（更新誤差の共分散行列）を適用し、予測については、次の式（１４）のフィルタ方程式（伝播計算）、および、式（１５）の推定共分散行列（残差の共分散行列）を適用する。この結果、画像処理や粗追尾駆動部３０１の動作等における遅延時間を加味しつつ、目標物が移動した位置を予測推定することができる。

以上により予測測定した目標位置に基づいて、図３のフローチャートに示すように、粗追尾用角度を算出して（ステップＳ２）、粗追尾駆動部３０１へ出力することにより、粗追尾駆動部３０１のジンバルを駆動させることになる。

（２）精追尾制御機能（協調制御補償器１０３ｂ）
次に、 協調制御器１０３の協調制御補償器１０３ｂすなわち精追尾制御機能の動作について、図４を参照しながらその一例を説明する。図４は、図１の追尾制御装置１０に備えた駆動ミラー５００および反射ミラー６００における画像補正のイメージを説明するためのイメージ図である。図４においては、光学部内に配置した光学部内反射ミラー６００の座標系Σ_ｍｍの^ｍｍＸ軸を光学部内反射ミラー６００の法線ベクトルと一致させ、また、^ｍｍＺ軸を^ＬＺＺ軸と一致させる。

また、図４に示すように、各ベクトルを以下のように定義する。すなわち、光学部内反射ミラー６００から目標物までのベクトルを^ｍｍＰ_ｔと定義し、駆動ミラー５００から光学部内反射ミラー６００までのベクトルを^ｆＰ_ｍｍと定義し、駆動ミラー５００の法線ベクトルをｎ_ｆと定義し、光学部内反射ミラー６００の法線ベクトルをｎ_ｍｍと定義する。

さらに、駆動ミラー５００の駆動結果により補正した各ベクトルの補正ベクトルはすべてアポストリフィ付きの記号として定義する。例えば、光学部内反射ミラー６００から目標物までのベクトルの補正後の補正ベクトルを^ｍｍＰ_ｔ’と定義し、駆動ミラー５００から光学部内反射ミラー６００までのベクトルの補正後の補正ベクトルを^ｆＰ_ｍｍ’と定義する。

（２．１）駆動ミラー５００の法線ベクトルｎ_ｆと光学部内反射ミラー６００の法線ベクトルｎ_ｍｍとの関係
駆動ミラー５００、光学部内反射ミラー６００の各ミラーのベクトルの関係は光の反射の法則に基づいて決定される。つまり、次の式（１６）に示す通り、平面ミラーによる反射ベクトルＰ_１は、反射モデルとして、ミラーの法線ベクトルｎと入射ベクトルＰ_０との関係によって決定される。

次に、駆動ミラー５００による補正前における光学部内反射ミラー６００から目標物までのベクトル^ｍｍＰ_ｔを、ジンバル現在角θ_ｍ、Ψ_ｍおよび直距離ｒｎｇを用いて示すと、次の式（１７）によって与えられる。

また、駆動ミラー５００による補正後における駆動ミラー５００から光学部内反射ミラー６００までのベクトルの補正後の補正ベクトル^ｆＰ_ｍｍ’、および、光学部内反射ミラー６００から目標物までの補正後の補正ベクトル^ｍｍＰ_ｔ’については、次の式（１８）が成立する。すなわち、光学部内反射ミラー６００および駆動ミラー５００周りの光についても反射モデルを構築して、前述した式(１６)に示す反射モデルの入射ベクトルＰ_０と法線ベクトルｎとして、それぞれ、駆動ミラー５００への入射ベクトル^ｇＰ_ｆと駆動ミラー５００の法線ベクトルｎ_ｆ、および、光学部内反射ミラー６００への入射ベクトル^ｆＰ_ｍｍ’と法線ベクトルｎ_ｍｍを適用することにより、式（１８）が得られる。

（２．２）駆動ミラー５００の目標角度およびジンバル目標値の導出
次に、精追尾制御機能における駆動ミラー５００の目標角度およびジンバル目標値を導出する手順についてその一例を、図５に示すフローチャートを参照しながら説明する。図５は、図１の追尾制御装置１０において駆動ミラー５００の目標角度およびジンバル目標値を算出する手順の一例を示すフローチャートである。図５のフローチャートにおいて、目標位置推定器１０３ａすなわち粗追尾制御機能における目標位置の推定動作およびジンバル目標値の導出の動作（図３に示したフローチャートのステップＳ１Ａの動作）の詳細の一例を、ステップＳ１１ないしＳ１５に示している。また、協調制御補償器１０３ｂすなわち精追尾制御機能における駆動ミラー５００の目標角度の導出の動作（図３に示したフローチャートのステップＳ３ＡないしＳ３Ｃの動作）の詳細の一例を、ステップＳ１６ないしＳ１９に示している。

図５のフローチャートにおいて、まず、粗追尾制御機能において、目標位置の推定およびジンバル目標値の算出を行なうために、画像取得部２００、レーザセンサ等の上位装置、ジンバル、高精度指向機構等の情報を読み込んで、目標物の誤差角すなわち偏角量δΨ（ＡＺ軸），δθ（ＥＬ軸）、直距離ｒｎｇ、ジンバル現在角Ψ_ｍ（ＡＺ軸），θ_ｍ（ＥＬ軸）に関する情報をそれぞれ入手する（ステップＳ１１）。さらに、入手した情報の異常値を除去する処理を施した後（ステップＳ１２）、平滑化処理を行なう（ステップＳ１３）。

次に、ジンバルから得たジンバル現在角Ψ_ｍ、θ_ｍおよび直距離センサから得た直距離ｒｎｇを前述の式（１７）に適用して、精追尾制御機能による補正前の現在位置（ｘ_ｔ、ｙ_ｔ、ｚ_ｔ）を算出する。精追尾制御機能による補正前の現在位置（ｘ_ｔ、ｙ_ｔ、ｚ_ｔ）が得られると、画像取得部２００からの画像情報から得られた目標指向方向の誤差角すなわち偏角量δΨ，δθの情報を基にして、地球固定座標系において得られる目標位置を、次の式（１９）を用いて現在位置推定演算を行なうことによって推定し、推定現在位置（ｘ_{ｔ＿ｅｓｔ}、ｙ_{ｔ＿ｅｓｔ}、ｚ_{ｔ＿ｅｓｔ}）すなわち地球固定座標系における目標位置（ｘ_ｔ’、ｙ_ｔ’、ｚ_ｔ’）として算出する（ステップＳ１４）。

しかる後、式（１９）の演算によって得られた地球固定座標系における目標位置（ｘ_ｔ’、ｙ_ｔ’、ｚ_ｔ’）を用いて、次の式（２１）によって、ジンバル系のジンバル目標角度Ψ_ｇｒ，θ_ｇｒを算出する（ステップＳ１５）。ジンバル目標角度Ψ_ｇｒ，θ_ｇｒを算出すると、算出したジンバル目標角度Ψ_ｇｒ，θ_ｇｒを用いて、粗追尾駆動部３０１を制御することにより、光学部全体を目標物の方向に指向させることになる。

粗追尾制御機能において地球固定座標系における目標位置（ｘ_ｔ’、ｙ_ｔ’、ｚ_ｔ’）を導出すると、正確な微調整を行なうべき駆動ミラー５００の目標角度を高精度に導出するために、該目標位置（ｘ_ｔ’、ｙ_ｔ’、ｚ_ｔ’）に関する情報は、精追尾制御機能に引き継がれる。ただし、駆動ミラー５００によって補正しようとする目標位置の算出においては、ＡＺジンバル座標系において算出する必要がある。したがって、精追尾制御機能においては、ＡＺジンバル座標系における目標物の現在の座標位置すなわち駆動ミラー５００により補正する目標位置（_ａｚｘ_ｔ’、_ａｚｙ_ｔ’、_ａｚｚ_ｔ’）を、式（１９）の目標位置（ｘ_ｔ’、ｙ_ｔ’、ｚ_ｔ’）からジンバル現在角Ψ_ｍ（ＡＺ軸）を削除することにより、次の式（２０）として与えることになる（ステップＳ１６、図３におけるステップＳ３Ａ）。

目標物の現在位置に関する３次元データとしてＡＺジンバル座標系における目標位置（_ａｚｘ_ｔ’、_ａｚｙ_ｔ’、_ａｚｚ_ｔ’）が得られると、次に、駆動ミラー５００の目標角度の算出を、次の手順にしたがって行なう。まず、光学部内反射ミラー６００の法線ベクトル_ａｚｎ_ｍｍ’を、次の式（２２）を用いて算出する。

さらに、駆動ミラー５００から光学部内反射ミラー６００までの光ベクトル^ｆ _ａｚＰ_ｍｍ’および光学部内反射ミラー６００から目標物までの光ベクトル^ｍｍ _ａｚＰ_ｔ’を、次の式（２３）を用いて算出する。

次いで、駆動ミラー５００の法線ベクトルｎ_ｆ’＝［ａ，ｂ，ｃ］を、次の式（２４）を用いて算出する（ステップＳ１７、図３におけるステップＳ３Ｂ）。

最後に、駆動ミラー５００の法線ベクトルｎ_ｆ’を用いて、駆動ミラー５００の駆動角度（Ｘ、Ｙ）を、次の式（２５）により導出して（ステップＳ１８、図３におけるステップＳ３Ｃ）、協調制御用フィルタとして設定する（ステップＳ１９）。

しかる後、以上により導出した駆動ミラー５００の駆動角度（Ｘ、Ｙ）に基づいて、図３のフローチャートに示すように、精追尾用として補正すべき補正角度を算出して（ステップＳ４）、平滑化フィルタにより平滑化することにより（ステップＳ５）、目標角度、目標角速度を算出し（ステップＳ６）、精追尾用として精追尾駆動部３０２へ出力する（ステップＳ７）ことにより、駆動ミラー５００を目標位置まで正確な角度まで駆動させて、角度を微調整することができる。

なお、図３のフローチャートの説明においては、説明を分かり易くするために、対象とする目標物が飛翔体であった場合について説明したが、本発明は、かかる場合に限るものではなく、対象とする目標物として、目標位置情報を抽出することが可能であって、かつ、目標物に対して高精度に指向させる追尾性能の必要性があれば、如何なる対象物であっても構わないことは言うまでもない。

（実施形態の効果の説明）
以上に詳細に説明したように、本実施形態においては次のような効果が得られる。

すなわち、本実施形態における追尾制御装置１０は、目標位置の高精度の予測を行ない、かつ、駆動ミラー５００、反射ミラー６００の各ミラーの法線ベクトルを利用して、光軸ベクトルを正確に演算しながら、各ミラーの駆動角度を制御することにより、追尾特性を大幅に向上させることが可能である。

さらに、説明すると、次の通りである。本実施形態における追尾制御装置１０は、光軸の補正を行なうためのＡＺ／ＥＬ２軸以上を駆動する大型ジンバル等の粗追尾駆動部３０１と２軸以上を高精度に駆動する精追尾駆動部３０２と、全体を制御する協調制御器１０３とを少なくとも備えて構成している。そして、カメラ等の画像取得部２００で撮像した目標物の画像情報を画像処理することにより得られた画面上の位置情報を基にして、正確に角度補正することを可能にするために、協調制御器１０３には、まず、粗追尾駆動部３０１を制御する目標位置推定器１０３ａ（粗追尾制御機能）の中に、画像情報から取得される目標位置情報の遅延時間分の補正を行ないながら予測推定する機能を設けている。さらに、協調制御器１０３には、粗追尾制御機能により予測推定された目標位置情報に基づき、反射ミラー６００、駆動ミラー５００の法線ベクトルを算出し、算出した結果に基づいて、正確な目標駆動角度を演算することによって、光路上に設けた２軸以上を駆動する精追尾駆動部３０２の正確な制御を行なう協調制御補償器１０３ｂ（精追尾制御機能）を設けている。而して、光軸上の補正誤差の発生を防止し、高精度な目標物の捕捉追尾を可能にしている。

以上、本発明の好適な実施形態の構成を説明した。しかし、かかる実施形態は、本発明の単なる例示に過ぎず、何ら本発明を限定するものではないことに留意されたい。本発明の要旨を逸脱することなく、特定用途に応じて種々の変形変更が可能であることが、当業者には容易に理解できよう。

１０ 追尾制御装置
１０Ａ 追尾制御装置
２０ 目標位置検出器
３０ 粗追尾制御部
３１ 粗追尾駆動部
４０ 角度近似補正部（精追尾制御部）
４１ 精追尾駆動部
１００ 制御装置
１０１ 粗追尾制御器
１０１ａ シンバル制御補償器
１０２ 精追尾制御器
１０２ａ 高精度追尾機構用制御補償器
１０３ 協調制御器
１０３ａ 目標位置推定器
１０３ｂ 協調制御補償器
２００ 画像取得部
３００ 駆動部
３０１ 粗追尾駆動部
３０２ 精追尾駆動部
４００ 発光部
５００ 駆動ミラー
６００ 反射ミラー

Claims (8)

1. 目標物に対して、光路上に配置された反射ミラー及び駆動ミラーを有する光学部全体を指向させて該目標物を粗追尾する粗追尾駆動部と、前記駆動ミラーの角度を微調整して前記目標物を精追尾する精追尾駆動部と、を少なくとも備えた追尾制御装置であって、
   前記粗追尾駆動部を制御する粗追尾制御部と前記精追尾駆動部を制御する精追尾制御部とを少なくとも備え、
   前記反射ミラーは、前記光路上において前記駆動ミラーに比べて前記目標物側に配置されており、
   前記粗追尾制御部は、前記目標物の運動及び前記目標物に対する追尾動作の遅延を加味して前記目標物が移動した位置を予測推定して、予測推定した結果に基づいて、前記粗追尾駆動部を制御し、
   前記精追尾制御部は、前記粗追尾制御部により制御された前記粗追尾駆動部から得た、前記光学部全体の指向方向を示す角度情報に基づき、前記目標物が移動した位置まで前記目標物を精追尾させる目標となる、前記駆動ミラーの法線ベクトルを算出し、算出した前記法線ベクトルに基づいて、前記精追尾駆動部を制御する
   ことを特徴とする追尾制御装置。
2. 前記精追尾制御部は、前記目標物が移動した位置に関する３次元データを算出し、算出した前記３次元データに基づいて、前記法線ベクトルを算出することを特徴とする請求項１に記載の追尾制御装置。
3. 前記精追尾制御部は、前記角度情報、前記目標物を撮像した画像情報から得られる目標指向方向の誤差角、及び直距離センサにより得られる前記目標物までの直距離に基づいて、前記３次元データを算出することを特徴とする請求項２に記載の追尾制御装置。
4. 前記精追尾制御部は、前記角度情報として仰角をθ _ｍ とし、前記誤差角として方位角の誤差角をδΨ、仰角の誤差角をδθとし、前記直距離をｒｎｇとして、下式により、前記３次元データとしての前記目標物が移動した位置（ _ａｚ ｘ′ _ｔ ， _ａｚ ｙ′ _ｔ ， _ａｚ ｚ′ _ｔ ）を算出する、
   _ａｚ ｘ′ _ｔ ＝ｒｎｇ・ｃｏｓ（θ _ｍ ＋δθ）・ｃｏｓ（δΨ）
   _ａｚ ｙ′ _ｔ ＝ｒｎｇ・ｃｏｓ（θ _ｍ ＋δθ）・ｓｉｎ（δΨ）
   _ａｚ ｚ′ _ｔ ＝−ｒｎｇ・ｓｉｎ（θ _ｍ ＋δθ）
   ことを特徴とする請求項３に記載の追尾制御装置。
5. 目標物に対して、光路上に配置された反射ミラー及び駆動ミラーを有する光学部全体を指向させて該目標物を粗追尾する粗追尾駆動部と、前記駆動ミラーの角度を微調整して前記目標物を精追尾する精追尾駆動部と、を少なくとも備えた追尾制御装置における追尾制御方法であって、
   前記粗追尾駆動部を制御する粗追尾制御ステップと前記精追尾駆動部を制御する精追尾制御ステップとを少なくとも有し、
   前記反射ミラーは、前記光路上において前記駆動ミラーに比べて前記目標物側に配置されており、
   前記粗追尾制御ステップは、前記目標物の運動及び前記目標物に対する追尾動作の遅延を加味して前記目標物が移動した位置を予測推定して、予測推定した結果に基づいて、前記粗追尾駆動部を制御し、
   前記精追尾制御ステップは、前記粗追尾制御ステップで制御された前記粗追尾駆動部から得た、前記光学部全体の指向方向を示す角度情報に基づき、前記目標物が移動した位置まで前記目標物を精追尾させる目標となる、前記駆動ミラーの法線ベクトルを算出し、算出した前記法線ベクトルに基づいて、前記精追尾駆動部を制御する
   ことを特徴とする追尾制御方法。
6. 前記精追尾制御ステップは、前記目標物が移動した位置に関する３次元データを算出し、算出した前記３次元データに基づいて、前記法線ベクトルを算出することを特徴とする請求項５に記載の追尾制御方法。
7. 前記精追尾制御ステップは、前記角度情報、前記目標物を撮像した画像情報から得られる目標指向方向の誤差角、及び直距離センサにより得られる前記目標物までの直距離に基づいて、前記３次元データを算出することを特徴とする請求項６に記載の追尾制御方法。
8. 前記精追尾制御ステップは、前記角度情報として仰角をθ _ｍ とし、前記誤差角として方位角の誤差角をδΨ、仰角の誤差角をδθとし、前記直距離をｒｎｇとして、下式により、前記３次元データとしての前記目標物が移動した位置（ _ａｚ ｘ′ _ｔ ， _ａｚ ｙ′ _ｔ ， _ａｚ ｚ′ _ｔ ）を算出する、
   _ａｚ ｘ′ _ｔ ＝ｒｎｇ・ｃｏｓ（θ _ｍ ＋δθ）・ｃｏｓ（δΨ）
   _ａｚ ｙ′ _ｔ ＝ｒｎｇ・ｃｏｓ（θ _ｍ ＋δθ）・ｓｉｎ（δΨ）
   _ａｚ ｚ′ _ｔ ＝−ｒｎｇ・ｓｉｎ（θ _ｍ ＋δθ）
   ことを特徴とする請求項７に記載の追尾制御方法。

Images