目標追尾装置及び目標追尾方法
【課題】移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができるようにする。
【解決手段】進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出部１４と、回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定部１５とを設ける。
【解決手段】進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出部１４と、回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定部１５とを設ける。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、例えば、レーダやＧＰＳなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測値に基づいて移動体の将来位置と到達時刻を推定する目標追尾装置及び目標追尾方法に関するものである。
【背景技術】
【０００２】
近年、移動体の将来位置や、目的地に到達する時刻を計算する技術は、車両のカーナビゲーションシステムや航空管制など、様々な分野で要求されている。
例えば、航空管制では、従来の３次元位置による航法に対して、３次元位置と時間を含む４次元軌道（４ＤＴ：４ Ｄｉｍｅｎｓｉｏｎａｌ Ｔｒａｊｅｃｔｏｒｙ）を航法に取り入れる検討がなされている。
この４ＤＴを管理することで、航空交通量や空域容量の予測が可能となることから、運航に対する安全性の向上が期待される。
【０００３】
航空管制では、予め設定された経由点を通過するように運航するルールが定められており、航空機が経由点に到達する時刻などが算出される。
航空機が等速直進運動によって、飛行ルート上の経由点へ向かう場合には、航空機が経由点に至るまでの飛行ルート上の将来位置や将来到達時刻などを容易に算出することができる。
しかし、実際には、気象状況の悪化（雷雲などの発生）に伴って、飛行ルートの変更を余儀なくされる場合がある。
雷雲などの進入禁止域への進入を回避するために飛行ルートが変更された場合、航空機の将来位置や将来到達時刻が変更されるため、その将来位置や将来到達時刻を再度推定する必要がある。
【０００４】
以下の特許文献１には、移動体の将来位置や将来到達時刻を推定する目標追尾装置が開示されている。
この目標追尾装置では、移動体が等速直進運動によって移動すると仮定して、移動体の現在の速度と機首方向から将来位置を推定するようにしているが、移動体が雷雲などの進入禁止域を回避するために経路を変更すると、等速直進運動による移動ではなくなるため、将来位置の推定精度が悪化することがある。
【０００５】
以下の特許文献２には、将来位置を予測する際、複数の運動モデルの中から適切な運動モデルを選択する目標追尾装置が開示されている。
この目標追尾装置では、センサにより観測された位置と追尾予測位置との差（残差）を用いて、複数の運動モデルの中から適切な運動モデルを選択するようにしている。
しかし、運動モデルを選択する際、雷雲などの進入禁止域の位置を考慮していないため、移動体が雷雲などの進入禁止域を回避する必要がある場合などには、将来位置の推定精度が悪化することがある。
【０００６】
以下の特許文献３には、将来位置を予測する際、Ａ＊アルゴリズムによる最適経路探索方法を用いる目標追尾装置が開示されている。
Ａ＊アルゴリズムは、進入禁止域（障害物）を含む経路候補の移動空間をメッシュ上に細分化して、出発地点からゴール地点（または、経由点）までのノードを決定するものであり、経路を決定する際、移動体の運動を考慮せずに、格子点によって決定される。
このため、自然な経路を得るには、格子点の間隔を細かく設定する必要があり、多くの処理時間を要することがある。
【０００７】
以下の非特許文献１には、低空飛行を行う飛翔体の操舵量をファジィ推論によって決定する技術が開示されている。
このファジィ推論では、地形を回避するのに必要な操舵量と、経由点に到達するのに必要な操舵量とを入力して、低空飛行を行う飛翔体の操舵量を決定している。
この技術の適用先として、目標がレーダ覆域外となり、観測値が得られない場合の長時間予測技術などが考えられる。
ただし、ファジィ推論におけるルールテーブルやメンバーシップ関数の設定は、試行錯誤による経験的な手法により行われることが多い。
このため、ルール数が増大するにしたがって設定作業が困難になることが想定される。また、将来位置の推定精度はファジィ推論のルールテーブルに依存する問題があった。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第２９８５９５２号（段落番号［００２４］）
【特許文献２】特許第４３４８５３５号（段落番号［０００６］、図１）
【特許文献３】特開２００９−２５１７２９号公報（段落番号［００７８］から［００８６］）
【非特許文献】
【０００９】
【非特許文献１】野本著、「ファジー推論による低対地高度飛行の経路計画」、電子情報通信学会論文誌（Ｂ），Ｖｏｌ．Ｊ７９，Ｎｏ．３ ｐｐ．１８３−１９０（１９９５）．
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来の目標追尾装置は以上のように構成されているので、移動体が雷雲などの進入禁止域を回避するために経路が変更された場合、移動体の将来位置や将来到達時刻の推定精度が劣化してしまうという課題があった。
【００１１】
この発明は上記のような課題を解決するためになされたもので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる目標追尾装置及び目標追尾方法を得ることを目的とする。
【課題を解決するための手段】
【００１２】
この発明に係る目標追尾装置は、追尾対象の移動体の観測値を用いて、現時刻における移動体の位置及び速度を算出する位置速度算出手段と、位置速度算出手段により算出された位置及び速度を用いて、指定時間後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、位置速度算出手段により算出された位置及び速度と指向用加速度算出手段により算出された経由点指向用の加速度を用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、回避用加速度算出手段により算出された進入回避用の加速度と位置速度算出手段により算出された位置及び速度を用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定手段とを設け、移動経路予測手段が、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定手段により特定された回避経路が移動体の移動経路であると予測する一方、判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定手段により特定された経由点指向経路が移動体の移動経路であると予測するようにしたものである。
【発明の効果】
【００１３】
この発明によれば、追尾対象の移動体の観測値を用いて、現時刻における移動体の位置及び速度を算出する位置速度算出手段と、位置速度算出手段により算出された位置及び速度を用いて、指定時間後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、位置速度算出手段により算出された位置及び速度と指向用加速度算出手段により算出された経由点指向用の加速度を用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、回避用加速度算出手段により算出された進入回避用の加速度と位置速度算出手段により算出された位置及び速度を用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定手段とを設け、移動経路予測手段が、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定手段により特定された回避経路が移動体の移動経路であると予測する一方、判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定手段により特定された経由点指向経路が移動体の移動経路であると予測するように構成したので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる効果がある。
【図面の簡単な説明】
【００１４】
【図１】この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】この発明の実施の形態１による目標追尾装置の処理内容を示すフローチャートである。
【図３】式（２）の運動モデルを示す概念図である。
【図４】経由点指向方向を示す説明図である。
【図５】経由点指向経路を示す説明図である。
【図６】移動体の移動経路と到達時刻を示す説明図である。
【図７】回避用加速度算出部１４の処理内容を示すフローチャートである。
【図８】水平方向の仮想経路の設定例を示す説明図である。
【図９】進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を示す説明図である。
【図１０】回避用加速度算出部１４により生成される回避加速度マップの一例を示す説明図である。
【図１１】進入回避用の加速度に用いる加速度の組み合わせの選択例１を示す説明図である。
【図１２】進入回避用の加速度に用いる加速度の組み合わせの選択例２を示す説明図である。
【図１３】進入回避用の加速度に用いる加速度の組み合わせの選択例３を示す説明図である。
【図１４】進入回避用の加速度に用いる加速度の組み合わせの選択例４を示す説明図である。
【図１５】進入回避用の加速度に用いる加速度の組み合わせの選択例５を示す説明図である。
【図１６】回避経路における回避地点と回避時刻を示す説明図である。
【図１７】複数の回避地点がある場合の移動経路を示す説明図である。
【図１８】この発明の実施の形態２による目標追尾装置を示す構成図である。
【図１９】経路長が異なる回避経路を示す説明図である。
【図２０】この発明の実施の形態３による目標追尾装置を示す構成図である。
【図２１】この発明の実施の形態３による目標追尾装置の移動経路選択部３４の処理内容を示すフローチャートである。
【図２２】回避用加速度算出部３１により算出された進入回避用加速度の一例を示す説明図である。
【発明を実施するための形態】
【００１５】
実施の形態１．
図１はこの発明の実施の形態１による目標追尾装置を示す構成図である。
図１の目標追尾装置は、航空機、船舶、車両などの移動体を追尾する装置であるが、この実施の形態１では、移動体が航空機である例を説明する。
図１において、追尾処理部１は例えばカルマンフィルタなどの追尾フィルタを用いて、追尾対象の移動体の観測値（例えば、レーダやＧＰＳなどのセンサから得られる移動体の位置などを示す観測情報）から、現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する処理を実施する。なお、追尾処理部１は位置速度算出手段を構成している。
【００１６】
経路予測部２は移動体の経路を変更する加速度ベクトルｕｋが考慮されている運動モデルに対して、追尾処理部１により算出された状態ベクトルｘｋを適用することで、移動体の移動経路を予測するとともに、移動体が当該移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
経由点位置情報格納部３は例えばＲＡＭやハードディスクなどの記憶装置から構成されており、移動体の経由点の位置を示す経由点位置情報を格納している。
進入禁止域位置情報格納部４は例えばＲＡＭやハードディスクなどの記憶装置から構成されており、雷雲などの進入禁止域（障害物）の位置を示す進入禁止域位置情報を格納している。
【００１７】
経路予測部２の指向用加速度算出部１１は追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後（指定時間後）に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する処理を実施する。なお、指向用加速度算出部１１は指向用加速度算出手段を構成している。
経由点指向経路特定部１２は追尾処理部１により算出された状態ベクトルｘｋと指向用加速度算出部１１により算出された経由点指向用の加速度ベクトルｕｋを上記運動モデルに適用することで、移動体が経由点に至るまでの経路である経由点指向経路を特定する処理を実施する。なお、経由点指向経路特定部１２は経由点指向経路特定手段を構成している。
【００１８】
進入禁止域衝突判定部１３は経由点指向経路特定部１２により特定された経由点指向経路上の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで、その経由点指向経路が進入禁止域内に入る経路であるか否かを判定する処理を実施する。なお、進入禁止域衝突判定部１３は判定手段を構成している。
回避用加速度算出部１４は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。なお、回避用加速度算出部１４は回避用加速度算出手段を構成している。
【００１９】
回避経路特定部１５は回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する処理を実施する。なお、回避経路特定部１５は回避経路特定手段を構成している。
移動経路予測部１６は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部１６は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部１６は移動経路予測手段及び到達時刻算出手段を構成している。
【００２０】
図１の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、回避用加速度算出部１４、回避経路特定部１５及び移動経路予測部１６のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、回避用加速度算出部１４、回避経路特定部１５及び移動経路予測部１６の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
図２はこの発明の実施の形態１による目標追尾装置の処理内容を示すフローチャートである。
【００２１】
次に動作について説明する。
追尾処理部１は、例えばカルマンフィルタなどの追尾フィルタを実装しており、レーダやＧＰＳなどのセンサから追尾中の移動体の観測値（移動体の位置などを示す観測情報）を受けると、その観測値に基づいて現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する（図２のステップＳＴ１）。
ここでは、追尾処理部１がカルマンフィルタを実装している例を示しているが、追尾フィルタはカルマンフィルタに限るものではなく、他の公知のフィルタを実装するようにしてもよい。
【００２２】
経路予測部２は、追尾処理部１が状態ベクトルｘｋを算出すると、下記の式（２）に示すように、移動体の経路を変更する加速度ベクトルｕｋが考慮されている運動モデルに対して、その状態ベクトルｘｋを適用することで、移動体の移動経路を予測するとともに、移動体が当該移動経路上を移動して経由点に到達する時刻を算出する。
式（２）において、ｘｋは時刻ｔｋにおける状態ベクトル（位置・速度）、Φｋは時刻ｔｋから時刻ｔｋ＋１に推移する際の既知の状態ベクトル推移行列、ｕｋは運動座標における加速度ベクトル、Γｋは加速度ベクトルｕｋの運動座標から北基準直交座標に変換する際の既知の変換行列である。
【００２３】
ここで、図３は式（２）の運動モデルを示す概念図である。
移動体は加速度ベクトルｕｋが設定されることで、図３に示すように、直線的に移動している状態から、左旋回／右旋回を行う状態、上昇／下降を行う状態又は加速／減速を行う状態に変化する。
加速度ベクトルｕｋは、レーダやＧＰＳなどのセンサからは観測不可能な値であるので、この実施の形態１では、後述する指向用加速度算出部１１及び回避用加速度算出部１４が算出するようにしている。
以下、経路予測部２の処理内容を具体的に説明する。
【００２４】
この実施の形態１では、移動体である航空機は、図４に示すように、経由点位置情報格納部３により格納されている経由点位置情報が示す経由点を目指して飛行しているものとする。
指向用加速度算出部１１は、追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後（指定時間後）に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する（ステップＳＴ２）。
即ち、指向用加速度算出部１１は、下記の式（３）〜（５）を計算することで、経由点指向用の加速度ベクトルｕｋを算出する。
【００２５】
ただし、ｖｇは移動体の水平方向の加速度、ｗｇは移動体の鉛直方向の加速度、θｖは移動体の水平方向の指向角度、θｗは移動体の鉛直方向の指向角度である。
【００２６】
経由点指向経路特定部１２は、指向用加速度算出部１１が経由点指向用の加速度ベクトルｕｋを算出すると、その経由点指向用の加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図５に示すように、移動体が経由点に至るまでの経路（予測時間Ｔｍａｘまでの経路）である経由点指向経路を特定する（ステップＳＴ３）。
即ち、経由点指向経路特定部１２は、式（２）に基づいて、現時刻ｔｋからＴａ秒後における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で移動するものとして、経由点に至るまでの経由点指向経路を特定する（図６を参照）。
【００２７】
進入禁止域衝突判定部１３は、経由点指向経路特定部１２が経由点指向経路を特定すると、例えば時刻ΔＴ刻みで、その経由点指向経路上での移動体の位置を算出し、各時刻における移動体の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで、その経由点指向経路が進入禁止域内に入る経路であるか否かを判定する（ステップＳＴ４）。
【００２８】
回避用加速度算出部１４は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、現時刻ｔｋにおける移動体の進行方向を基準とする７種類の運動モデル（直進、加速、減速、左旋回、右旋回、上昇、下降）を考慮して（図３を参照）、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する（ステップＳＴ５）。
以下、回避用加速度算出部１４による進入回避用の加速度の算出処理を具体的に説明する。ただし、ここでは、説明の簡単化のため、移動体が加速及び減速を行わないものとする。
【００２９】
図７は回避用加速度算出部１４の処理内容を示すフローチャートである。
まず、回避用加速度算出部１４は、移動体の性能から想定される水平方向の最大加速度を上限として、Δｕステップで水平方向の加速度の候補を設定する。
回避用加速度算出部１４は、水平方向の加速度の候補を複数設定すると、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、複数の仮想経路を生成する（図７のステップＳＴ２１）。
図８は水平方向の仮想経路の設定例を示す説明図であり、図８では、３個の仮想経路を設定している例を示している。
【００３０】
回避用加速度算出部１４は、複数の仮想経路を生成すると、仮想経路毎に、当該仮想経路が進入禁止域内に入る経路であるか否かを判定する。
仮想経路が進入禁止域内に入る経路であるか否かの判定は、進入禁止域衝突判定部１３と同様に、仮想経路上での各時刻における移動体の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで判定することができる。
【００３１】
回避用加速度算出部１４は、当該仮想経路が進入禁止域内に入る経路である場合、移動体の性能から想定される鉛直方向の最大加速度を上限として、進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出する（図９を参照）。
即ち、回避用加速度算出部１４は、移動体が進入禁止域内に進入する進入時刻Ｔｃ（図８の例では、２ΔＴの時刻）を算出し、その進入時刻Ｔｃにおいて、例えば、Δｕステップで、鉛直方向の加速度を大きくすることで、当該仮想経路を鉛直方向に段階的に変更する。
回避用加速度算出部１４は、当該仮想経路を鉛直方向に段階的に変更することで、当該仮想経路が進入禁止域内に入る経路から、進入禁止域内に入らない経路に変化する際の鉛直方向の加速度を探索する。
ただし、鉛直方向の最大加速度を設定しても、当該仮想経路が進入禁止域内に入る経路である場合、必要最小限の鉛直方向の加速度として、最大加速度を設定する。
【００３２】
ここでは、当該仮想経路を鉛直方向に段階的に変更することで、必要最小限の鉛直方向の加速度を探索する例を示したが、下記の式（６）を計算することで、必要最小限の鉛直方向の加速度を算出するようにしてもよい。
ただし、Ｈは進入禁止域を回避するために必要な高度差、Ｔｃは進入時刻（ΔＴステップで設定した時刻の中で、進入禁止域内に入る最小の時刻）、ｗｇ，Ｈは高度差Ｈを回避するために必要な鉛直方向の加速度である。
【００３３】
回避用加速度算出部１４は、仮想経路毎に、最小限の鉛直方向の加速度を算出すると、当該仮想経路に対応する水平方向の加速度（当該仮想経路を生成する際に、経由点指向経路の変更に用いている水平方向の加速度の候補）と、当該仮想経路に対応する鉛直方向の加速度との組み合わせが得られるので、当該仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせを示す回避加速度マップを生成する（ステップＳＴ２２）。
図１０は回避用加速度算出部１４により生成される回避加速度マップの一例を示す説明図である。
図１０の例では、ＡＢＣＤで囲まれている領域が進入禁止域であり、上記領域の外側の領域が進入禁止域外である。
【００３４】
回避用加速度算出部１４は、各仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせを示す回避加速度マップを生成すると、その回避加速度マップを利用して、各仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせの中から、進入禁止域内への進入を回避するために必要な進入回避用の加速度に用いる加速度の組み合わせを選択する。
具体的には、以下のようにして、進入回避用の加速度に用いる加速度の組み合わせを選択する。
【００３５】
図１１〜図１５は進入回避用の加速度に用いる加速度の組み合わせの選択例を示す説明図である。
図において、ｖｇは経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度、ｗｇは経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度、ｖ’ｇは水平方向の進入回避用加速度、ｗ’ｇは鉛直方向の進入回避用加速度である。
この実施の形態１では、移動体が民間の航空機である等の場合、鉛直方向の加速度を変化させるよりも、水平方向の加速度を変化させることが、より自然な運航であるため（対地高度を一定とする運航）、水平方向を変化させる組み合わせを優先的に選択するものとする（鉛直方向の加速度が小さい組み合わせを優先的に選択するものとする）。
特に、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ水平方向の回避加速度を変化させる組み合わせを優先的に選択するものとする。
【００３６】
［優先順位１］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ加速度であり、鉛直方向の進入回避用加速度ｗ’ｇがゼロ（対地高度が一定）である組み合わせを選択する。
このケースは図１１に示すケース１に該当し、水平方向の進入回避用加速度ｖ’ｇは、下記の式（７）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇ（Δｖｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する（ステップＳＴ２３，ＳＴ２４）。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００３７】
［優先順位２］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度ｗ’ｇが経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ加速度の組み合わせを選択する。
このケースは図１２に示すケース２に該当し、水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２４）。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（８）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇ（Δｗｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する（ステップＳＴ２５，ＳＴ２６）。
【００３８】
［優先順位３］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、その加速度ｖｇと逆符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと逆の符号を持つ加速度であり、鉛直方向の進入回避用加速度ｗ’ｇがゼロ（対地高度が一定）である組み合わせを選択する。
このケースは図１３に示すケース３に該当し、水平方向の進入回避用加速度ｖ’ｇは、下記の式（９）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇを加えることで算出する（ステップＳＴ２３，ＳＴ２４）。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００３９】
［優先順位４］
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度ｗ’ｇが経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度である組み合わせを選択する。
このケースは図１４に示すケース４に該当し、水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２７）。
ｖ’ｇ＝ｖｇ，ｍａｘ
また、鉛直方向の進入回避用加速度ｗ’ｇは、経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度ｗｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２７）。
ｗ’ｇ＝ｗｇ，ｍａｘ
【００４０】
ここでは、ケース４に該当する場合、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定し、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定する例を示したが、最大加速度に設定しても、進入禁止域内への進入を回避することができないので、追尾航跡が更新される次の時刻まで待って、進入回避用の加速度を設定する処理を再度実施するようにしてもよい。
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
【００４１】
ここでは、水平方向の回避加速度を最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合（以下、「場合Ａ」と称する）を優先順位２として、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同符号では、水平方向の回避加速度を最大値としても進入禁止域内となるが、その加速度ｖｇと逆の符号で、水平方向の回避加速度を変化させることで進入禁止域外となる場合（以下、「場合Ｂ」と称する）を優先順位３としているが、優先順位を逆にして、場合Ａを優先順位３にして、場合Ｂを優先順位２にしてもよい。
【００４２】
また、優先順位３のケースに該当しない場合、直ちに、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定して、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定しているが、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと逆符号である条件下で、水平方向の回避加速度を最大加速度ｖｇ，ｍａｘに設定して、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合には、そのように進入回避用加速度を設定するようにしてもよい。
【００４３】
ここでは、回避用加速度算出部１４が、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を変化させる組み合わせを優先的に選択するものを示したが、図１５に示すように、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号によらず、変化が最小の加速度を水平方向の進入回避用加速度ｖ’ｇとして優先的に設定するようにしてもよい。
即ち、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号と逆の符号を持つ最小限の値ｖｇ，ＬＩＭが、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号と同じ符号を持つ最小限の値ｖｇ，ＬＩＭより小さい場合、逆の符号を持つ最小限の値ｖｇ，ＬＩＭから水平方向の進入回避用加速度ｖ’ｇを算出するようにしてもよい。
また、ここでは、説明の簡単化のために、移動体が加速及び減速を行わないものとして説明したが、加速及び減速を含めて仮想経路を生成するようにしてもよいことは言うまでもない。
【００４４】
回避経路特定部１５は、回避用加速度算出部１４が進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ、鉛直方向の進入回避用加速度ｗ’ｇ）を算出すると、その進入回避用の加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図１６に示すように、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する（図２のステップＳＴ６）。
【００４５】
即ち、回避経路特定部１５は、移動体が進入禁止域内に進入する進入時刻Ｔｃを回避時刻Ｔvとして、加速度ベクトルｕｋと状態ベクトルｘｋを式（２）の運動モデルに適用することで、回避時刻Ｔvにおける移動体の位置（図１６では、「回避地点」と記述している）と、移動体の速度を算出する。
回避経路特定部１５は、その回避地点と移動体の速度を追尾航跡の位置・速度とみなすことで、移動体の出発地点（または、前回の回避地点）から回避地点までの経路を回避経路として特定する。
【００４６】
回避経路特定部１５は、回避経路における移動体の各時刻の位置及び速度を指向用加速度算出部１１にフィードバックすることで、指向用加速度算出部１１が各時刻の位置及び速度から経由点用の加速度を算出し、進入禁止域衝突判定部１３において、経由点指向経路特定部１２により特定された経由点指向経路が進入禁止域内に入らない経路と判定されるまで、指向用加速度算出部１１〜回避経路特定部１５の間でループ処理を実施する。
【００４７】
移動経路予測部１６は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合（ステップＳＴ４）、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する（ステップＳＴ７）。
移動経路予測部１６は、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測すると、式（２）に基づいて、現時刻ｔｋからＴａ秒後（指定時間後）における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する（図６を参照）。
【００４８】
また、移動経路予測部１６は、その指向終了位置と経由点の間の距離Ｒａと、その指向終了位置における移動体の速度Ｖａとから、移動体が経由点に到達する時刻Ｔpを算出する（ステップＳＴ８）。
【００４９】
移動経路予測部１６は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、図１６に示すように、回避経路特定部１５により特定された回避経路（現地点から回避地点までの経路）が移動体の移動経路であると予測する（ステップＳＴ９）。
移動経路予測部１６は、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測すると、回避時刻Ｔvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
【００５０】
移動経路予測部１６は、図１７に示すように、複数の回避地点がある場合、最終の回避地点から経由点に到達する時間と、複数の回避地点間の移動に要する時間（複数の回避地点における回避時刻Ｔvの差分）との和を求めることで、移動体が経由点に到達する時刻Ｔpを算出する（ステップＳＴ１０）。
式（１２）において、右辺第２項は式（１１）の右辺と同じ意味であり、Ｔv，ｉはｉ−１番目からｉ番目の回避地点に到達するまでに要する時間である。ただし、ｉ＝１である場合に限り、現在位置から１番目の回避地点に到達するまでに要する時間である。
【００５１】
図１の目標追尾装置は、移動体の追尾を継続する場合、ステップＳＴ１の処理に戻り、ステップＳＴ１〜ＳＴ１０の処理を繰り返し実施する（ステップＳＴ１１）。
【００５２】
以上で明らかなように、この実施の形態１によれば、追尾対象の移動体の観測値を用いて、現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する追尾処理部１と、追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する指向用加速度算出部１１と、追尾処理部１により算出された状態ベクトルｘｋと指向用加速度算出部１１により算出された経由点指向用の加速度ベクトルｕｋを用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定部１２と、経由点指向経路特定部１２により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する進入禁止域衝突判定部１３と、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出部１４と、回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定部１５とを設け、移動経路予測部１６が、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測するように構成したので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる効果を奏する。
【００５３】
また、この実施の形態１によれば、移動経路予測部１６が、移動体が予測した移動経路上を移動して経由点に到達する時刻を算出するように構成したので、移動体が経由点に到達する時刻を高精度に推定することができる効果を奏する。
【００５４】
また、この実施の形態１によれば、回避用加速度算出部１４が、水平方向及び鉛直方向の複数の加速度の組み合わせのうち、鉛直方向の加速度が小さい組み合わせを優先的に選択するように構成したので、例えば、移動体が民間の航空機である等の場合には、より自然な運航（対地高度を一定とする運航）に見合う進入回避用加速度を算出することができるようになり、その結果、移動体の将来位置や経由点到達時刻等の推定精度を高めることができる効果を奏する。
【００５５】
なお、この実施の形態１では、指向用加速度算出部１１が追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出するものを示したが、式（２）の右辺第１項の状態ベクトルｘｋの速度成分については、追尾フィルタによって、レーダやＧＰＳなどのセンサの観測値が示す位置から推定した速度を用いるようにしてもよい。また、移動体から得られる速度情報を用いるようにしてもよい。
【００５６】
例えば、移動体が航空機である場合、航空機の機体から真対気速度が得られるので、風速データを加味することで対地速度が算出することができる。
このため、対地速度を状態ベクトルｘｋの速度成分として用いるようにしてもよい。
予め、目標進行予定の空間における風速データが既知の場合には、風速による状態ベクトルの速度変化を仮想経路の生成や、移動経路の予測に反映することが可能である。
【００５７】
この実施の形態１では、回避用加速度算出部１４が進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出するものを示したが、複数の回避地点が存在する場合、複数の回避地点の間で、進入回避用加速度が大きく異なるケースが発生することがある。
そこで、隣り合う回避地点で、進入回避用加速度が急激に変化する状況の発生を防止するために、予め加速度変化率パラメータΔαを設定し、時間方向に対して、Δα刻みで進入回避用加速度を変化させるようにしてもよい。
【００５８】
この実施の形態１では、進入禁止域が１つである例を説明したが、進入禁止域が複数存在する場合でも、同様の処理を繰り返すことで、進入禁止域内への進入を回避することが可能である。
【００５９】
実施の形態２．
上記実施の形態１では、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避用加速度算出部１４が、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出し、回避経路特定部１５が回避用加速度算出部１４により算出された進入回避用の加速度を用いて、回避経路を特定するものを示したが、図１９に示すように、進入禁止域が進行方向に対して奥行きがある形を成している場合、回避経路特定部１５により特定される可能性がある回避経路（図中、実線の経路）が、他の回避経路（図中、点線の経路）よりも経路長が長くなる場合がある。
この実施の形態２では、進入禁止域が進行方向に対して奥行きがある形を成している場合でも、経路長が短い回避経路を特定することができるようにしている。
【００６０】
図１８はこの発明の実施の形態２による目標追尾装置を示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
進入禁止域容積算出部２１は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域における経由点指向経路の進行方向左側の容積を算出するとともに、その進入禁止域における経由点指向経路の進行方向右側の容積を算出する処理を実施する。なお、進入禁止域容積算出部２１は容積算出手段を構成している。
【００６１】
回避用加速度算出部２２は進入禁止域容積算出部２１により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。なお、回避用加速度算出部２２は回避用加速度算出手段を構成している。
【００６２】
図１８の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部２２、回避経路特定部１５及び移動経路予測部１６のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部２２、回避経路特定部１５及び移動経路予測部１６の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
【００６３】
次に動作について説明する。
進入禁止域容積算出部２１は、進入禁止域衝突判定部１３が上記実施の形態１と同様にして、経由点指向経路が進入禁止域内に入る経路であるか否かの判定処理を行うと、その判定結果が、経由点指向経路が進入禁止域内に入る経路である旨を示していれば、進入禁止域位置情報格納部４により格納されている進入禁止域位置情報を参照して、進入禁止域の形状を確認する。
進入禁止域容積算出部２１は、進入禁止域の形状を確認すると、その進入禁止域と経由点指向経路特定部１２により特定された経由点指向経路の位置関係を把握し、その経由点指向経路に対して、進入禁止域を進行方向左側のエリアと右側のエリアに分断する。
【００６４】
進入禁止域容積算出部２１は、進行方向左側のエリア及び右側のエリアの形状を把握することで、進行方向左側のエリアの容積と、進行方向右側のエリアの容積とを算出して、その算出結果を回避用加速度算出部２２に出力する。
なお、正確な容積を算出することができない場合には、容積に相当する量（例えば、奥行きの長さ×幅）を算出し、容積に相当する量をエリアの容積として回避用加速度算出部２２に出力する。
【００６５】
回避用加速度算出部２２は、進入禁止域容積算出部２１が進行方向左側のエリアの容積と、進行方向右側のエリアの容積とを算出すると、容積が小さいエリア側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
ただし、容積が小さいエリア側の方向に移動体が移動することでは、進入禁止域内への進入を回避することができない場合、容積が大きいエリア側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
図１９の例では、進行方向左側のエリアの容積が、進行方向右側のエリアの容積より小さいので、下記に示す優先順位にしたがって、進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
【００６６】
［優先順位１］
「水平方向の回避加速度を進行方向左側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、下記の式（１３）に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇ（Δｖｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００６７】
［優先順位２］
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、進行方向左側の向きに最大加速度ｖｇ，ｍａｘを設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（１４）に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇ（Δｗｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する。
【００６８】
［優先順位３］
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、水平方向の回避加速度を進行方向右側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、下記の式（１５）に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇを加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００６９】
［優先順位４］
「水平方向の回避加速度を進行方向右側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、進行方向右側の向きに最大加速度ｖｇ，ｍａｘを設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（１６）に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇを加えることで算出する。
【００７０】
［優先順位５］
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度ｗｇ，ｍａｘに設定する。
ｗ’ｇ＝ｗｇ，ｍａｘ
【００７１】
優先順位５の場合、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定し、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定する例を示したが、最大加速度に設定しても、進入禁止域内への進入を回避することができないので、追尾航跡が更新される次の時刻まで待って、進入回避用の加速度を設定する処理を再度実施するようにしてもよい。
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
【００７２】
ここでは、水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合（以下、「場合Ａ」と称する）を優先順位２として、水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、水平方向の回避加速度を進行方向右側に変化させることで進入禁止域外となる場合（以下、「場合Ｂ」と称する）を優先順位３としているが、優先順位を逆にして、場合Ａを優先順位３にして、場合Ｂを優先順位２にしてもよい。
【００７３】
回避経路特定部１５は、回避用加速度算出部２２が進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ、鉛直方向の進入回避用加速度ｗ’ｇ）を算出すると、上記実施の形態１と同様に、その進入回避用の加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図１９に示すように、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する。
これにより、回避用加速度算出部２２により高い優先順位１，２で進入回避用加速度が算出されている場合、進入禁止域が進行方向に対して奥行きがある形を成していても、経路長が短い経路（図中、点線の経路）が回避経路となる。
移動経路予測部１６の処理内容は、上記実施の形態１と同様であるため説明を省略する。
【００７４】
以上で明らかなように、この実施の形態２によれば、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域における経由点指向経路の進行方向左側の容積を算出するとともに、その進入禁止域における経由点指向経路の進行方向右側の容積を算出する進入禁止域容積算出部２１を設け、回避用加速度算出部２２が、進入禁止域容積算出部２１により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出するように構成したので、進入禁止域が進行方向に対して奥行きがある形を成している場合でも、経路長が短い回避経路を特定することができる効果を奏する。
【００７５】
実施の形態３．
上記実施の形態１，２では、回避経路特定部１５が１つの回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部１６が回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測するものを示したが、回避経路特定部１５が複数の回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部１６が回避経路特定部１５により特定された複数の回避経路が移動体の移動経路であると予測し、複数の移動経路の中で、経由点の到達時刻Ｔp又は進入回避用の加速度が最小となる移動経路を選択するようにしてもよい。
【００７６】
図２０はこの発明の実施の形態３による目標追尾装置を示す構成図であり、図において、図１８と同一符号は同一または相当部分を示すので説明を省略する。
回避用加速度算出部３１は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。
回避用加速度算出部３１における進入回避用の加速度の算出方法は、図１の回避用加速度算出部１４の算出方法と同一であってもよいし、図１８の回避用加速度算出部２２の算出方法と同一であってもよいが、回避用加速度算出部３１では、進入回避用の加速度を複数算出する。
なお、回避用加速度算出部３１は回避用加速度算出手段を構成している。
【００７７】
回避経路特定部３２は回避用加速度算出部３１により算出された複数の進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る複数の回避経路を特定する処理を実施する。なお、回避経路特定部３２は回避経路特定手段を構成している。
移動経路予測部３３は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部３２により特定された複数の回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部３３は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部３３は移動経路予測手段及び到達時刻算出手段を構成している。
【００７８】
移動経路選択部３４は移動経路予測部３３により複数の回避経路が移動体の移動経路であると予測された場合、複数の移動経路の中で、移動経路予測部３３により算出された経由点到達時刻Ｔp又は回避用加速度算出部３１により算出された進入回避用の加速度が最小となる移動経路を選択する処理を実施する。なお、移動経路選択部３４は移動経路選択手段を構成している。
【００７９】
図２０の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部３１、回避経路特定部３２、移動経路予測部３３及び移動経路選択部３４のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部３１、回避経路特定部３２、移動経路予測部３３及び移動経路選択部３４の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
図２１はこの発明の実施の形態３による目標追尾装置の移動経路選択部３４の処理内容を示すフローチャートである。
【００８０】
次に動作について説明する。
回避用加速度算出部３１は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を複数算出する。
回避用加速度算出部３１における進入回避用の加速度の算出方法は、特に問わないが、図１の回避用加速度算出部１４と同様にして、例えば、図２２に示すように、第１の進入回避用加速度（水平方向の進入回避用加速度ｖ’ｇ１，鉛直方向の進入回避用加速度ｗ’ｇ１）を算出し、図１８の回避用加速度算出部２２と同様にして、第２の進入回避用加速度（水平方向の進入回避用加速度ｖ’ｇ２，鉛直方向の進入回避用加速度ｗ’ｇ２）を算出する。
以降の説明では、説明の簡単化のため、図２２に示すように、回避用加速度算出部３１が、２つの進入回避用加速度（第１の進入回避用加速度、第２の進入回避用加速度）を算出するものとするが、３以上の進入回避用加速度を算出するようにしてもよい。
【００８１】
回避経路特定部３２は、回避用加速度算出部３１が第１の進入回避用加速度と第２の進入回避用加速度を算出すると、図１の回避経路特定部１５と同様に、第１の進入回避用加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、移動体が進入禁止域内に入らずに経由点に至る第１の回避経路を特定する。
また、第２の進入回避用加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと状態ベクトルｘｋを式（２）の運動モデルに適用することで、移動体が進入禁止域内に入らずに経由点に至る第２の回避経路を特定する。
言うまでもないが、回避用加速度算出部３１がＮ個の進入回避用加速度を算出していれば、回避経路特定部３２がＮ個の回避経路を特定する。
【００８２】
移動経路予測部３３は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、図１の移動経路予測部１６と同様に、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する。
移動経路予測部３３は、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測すると、図１の移動経路予測部１６と同様に、現時刻ｔｋからＴａ秒後における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部３３は、図１の移動経路予測部１６と同様に、その指向終了位置と経由点の間の距離Ｒａと、その指向終了位置における移動体の速度Ｖａとから、移動体が経由点に到達する時刻Ｔpを算出する。
【００８３】
移動経路予測部３３は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部３２により特定された第１及び第２の回避経路（現地点から回避地点までの経路）のそれぞれが移動体の移動経路であると予測する。
移動経路予測部３３は、回避経路特定部３２により特定された第１及び第２の回避経路が移動体の移動経路であると予測すると、回避時刻Ｔvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部３３は、図１の移動経路予測部１６と同様にして、移動体が第１の回避経路上を移動する場合の経由点到達時刻Ｔp，１を算出し、また、移動体が第２の回避経路上を移動する場合の経由点到達時刻Ｔp，２を算出する。
【００８４】
移動経路選択部３４は、移動経路予測部３３により経由点指向経路が移動体の移動経路であると予測された場合、その移動経路と移動経路予測部３３により算出された経由点到達時刻Ｔpを出力する。
一方、移動経路予測部３３により第１及び第２の回避経路が移動体の移動経路であると予測された場合、移動体が第１の回避経路上を移動する場合の経由点到達時刻Ｔp，１と、移動体が第２の回避経路上を移動する場合の経由点到達時刻Ｔp，２との時刻差ΔＴpを算出する（図２１のステップＳＴ３１）。
【００８５】
移動経路選択部３４は、経由点到達時刻の時刻差ΔＴpを算出すると、その時刻差ΔＴpを所定の閾値と比較し（ステップＳＴ３２）、その時刻差ΔＴpが所定の閾値より大きければ、経由点到達時刻Ｔpが最も早い移動経路（回避経路）を選択し、その移動経路と最も早い経由点到達時刻Ｔpを出力する（ステップＳＴ３３）。
例えば、経由点到達時刻Ｔp，１が経由点到達時刻Ｔp，２より早ければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、経由点到達時刻Ｔp，２が経由点到達時刻Ｔp，１より早ければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８６】
移動経路選択部３４は、経由点到達時刻の時刻差ΔＴpが所定の閾値以下である場合、移動体が第１の回避経路上を移動する場合の鉛直方向の進入回避用加速度ｗ’ｇ１と、移動体が第２の回避経路上を移動する場合の鉛直方向の進入回避用加速度ｗ’ｇ２との加速度差Δｗ’ｇを算出する（ステップＳＴ３４）。
【００８７】
移動経路選択部３４は、鉛直方向の加速度差Δｗ’ｇを算出すると、その加速度差Δｗ’ｇを所定の閾値と比較し（ステップＳＴ３５）、その加速度差Δｗ’ｇが所定の閾値より大きければ、鉛直方向の進入回避用加速度ｗ’ｇが最も小さい移動経路（回避経路）を選択し、その移動経路と経由点到達時刻Ｔpを出力する（ステップＳＴ３６）。
例えば、進入回避用加速度ｗ’ｇ１が進入回避用加速度ｗ’ｇ２より小さければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、進入回避用加速度ｗ’ｇ２が進入回避用加速度ｗ’ｇ１より小さければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８８】
一方、加速度差Δｗ’ｇが所定の閾値以内である場合、移動体が第１の回避経路上を移動する場合の水平方向の進入回避用加速度ｖ’ｇ１と、移動体が第２の回避経路上を移動する場合の水平方向の進入回避用加速度ｖ’ｇ２とを比較し、水平方向の進入回避用加速度ｖ’ｇが最も小さい移動経路（回避経路）を選択し、その移動経路と経由点到達時刻Ｔpを出力する（ステップＳＴ３７）。
例えば、進入回避用加速度ｖ’ｇ１が進入回避用加速度ｖ’ｇ２より小さければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、進入回避用加速度ｖ’ｇ２が進入回避用加速度ｖ’ｇ１より小さければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８９】
以上で明らかなように、この実施の形態３によれば、移動経路予測部３３により複数の回避経路が移動体の移動経路であると予測された場合、移動経路選択部３４が、複数の移動経路の中で、移動経路予測部３３により算出された経由点到達時刻Ｔp又は回避用加速度算出部３１により算出された進入回避用の加速度が最小となる移動経路を選択するように構成したので、現実の運航に近いスムーズな移動経路を予測することができる効果を奏する。
【００９０】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【００９１】
１ 追尾処理部（位置速度算出手段）、２ 経路予測部、３ 経由点位置情報格納部、４ 進入禁止域位置情報格納部、１１ 指向用加速度算出部（指向用加速度算出手段）、１２ 経由点指向経路特定部（経由点指向経路特定手段）、１３ 進入禁止域衝突判定部（判定手段）、１４ 回避用加速度算出部（回避用加速度算出手段）、１５ 回避経路特定部（回避経路特定手段）、１６ 移動経路予測部（移動経路予測手段、到達時刻算出手段）、２１ 進入禁止域容積算出部（容積算出手段）、２２ 回避用加速度算出部（回避用加速度算出手段）、３１ 回避用加速度算出部（回避用加速度算出手段）、３２ 回避経路特定部（回避経路特定手段）、３３ 移動経路予測部（移動経路予測手段、到達時刻算出手段）、３４ 移動経路選択部（移動経路選択手段）。
【技術分野】
【０００１】
この発明は、例えば、レーダやＧＰＳなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測値に基づいて移動体の将来位置と到達時刻を推定する目標追尾装置及び目標追尾方法に関するものである。
【背景技術】
【０００２】
近年、移動体の将来位置や、目的地に到達する時刻を計算する技術は、車両のカーナビゲーションシステムや航空管制など、様々な分野で要求されている。
例えば、航空管制では、従来の３次元位置による航法に対して、３次元位置と時間を含む４次元軌道（４ＤＴ：４ Ｄｉｍｅｎｓｉｏｎａｌ Ｔｒａｊｅｃｔｏｒｙ）を航法に取り入れる検討がなされている。
この４ＤＴを管理することで、航空交通量や空域容量の予測が可能となることから、運航に対する安全性の向上が期待される。
【０００３】
航空管制では、予め設定された経由点を通過するように運航するルールが定められており、航空機が経由点に到達する時刻などが算出される。
航空機が等速直進運動によって、飛行ルート上の経由点へ向かう場合には、航空機が経由点に至るまでの飛行ルート上の将来位置や将来到達時刻などを容易に算出することができる。
しかし、実際には、気象状況の悪化（雷雲などの発生）に伴って、飛行ルートの変更を余儀なくされる場合がある。
雷雲などの進入禁止域への進入を回避するために飛行ルートが変更された場合、航空機の将来位置や将来到達時刻が変更されるため、その将来位置や将来到達時刻を再度推定する必要がある。
【０００４】
以下の特許文献１には、移動体の将来位置や将来到達時刻を推定する目標追尾装置が開示されている。
この目標追尾装置では、移動体が等速直進運動によって移動すると仮定して、移動体の現在の速度と機首方向から将来位置を推定するようにしているが、移動体が雷雲などの進入禁止域を回避するために経路を変更すると、等速直進運動による移動ではなくなるため、将来位置の推定精度が悪化することがある。
【０００５】
以下の特許文献２には、将来位置を予測する際、複数の運動モデルの中から適切な運動モデルを選択する目標追尾装置が開示されている。
この目標追尾装置では、センサにより観測された位置と追尾予測位置との差（残差）を用いて、複数の運動モデルの中から適切な運動モデルを選択するようにしている。
しかし、運動モデルを選択する際、雷雲などの進入禁止域の位置を考慮していないため、移動体が雷雲などの進入禁止域を回避する必要がある場合などには、将来位置の推定精度が悪化することがある。
【０００６】
以下の特許文献３には、将来位置を予測する際、Ａ＊アルゴリズムによる最適経路探索方法を用いる目標追尾装置が開示されている。
Ａ＊アルゴリズムは、進入禁止域（障害物）を含む経路候補の移動空間をメッシュ上に細分化して、出発地点からゴール地点（または、経由点）までのノードを決定するものであり、経路を決定する際、移動体の運動を考慮せずに、格子点によって決定される。
このため、自然な経路を得るには、格子点の間隔を細かく設定する必要があり、多くの処理時間を要することがある。
【０００７】
以下の非特許文献１には、低空飛行を行う飛翔体の操舵量をファジィ推論によって決定する技術が開示されている。
このファジィ推論では、地形を回避するのに必要な操舵量と、経由点に到達するのに必要な操舵量とを入力して、低空飛行を行う飛翔体の操舵量を決定している。
この技術の適用先として、目標がレーダ覆域外となり、観測値が得られない場合の長時間予測技術などが考えられる。
ただし、ファジィ推論におけるルールテーブルやメンバーシップ関数の設定は、試行錯誤による経験的な手法により行われることが多い。
このため、ルール数が増大するにしたがって設定作業が困難になることが想定される。また、将来位置の推定精度はファジィ推論のルールテーブルに依存する問題があった。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第２９８５９５２号（段落番号［００２４］）
【特許文献２】特許第４３４８５３５号（段落番号［０００６］、図１）
【特許文献３】特開２００９−２５１７２９号公報（段落番号［００７８］から［００８６］）
【非特許文献】
【０００９】
【非特許文献１】野本著、「ファジー推論による低対地高度飛行の経路計画」、電子情報通信学会論文誌（Ｂ），Ｖｏｌ．Ｊ７９，Ｎｏ．３ ｐｐ．１８３−１９０（１９９５）．
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来の目標追尾装置は以上のように構成されているので、移動体が雷雲などの進入禁止域を回避するために経路が変更された場合、移動体の将来位置や将来到達時刻の推定精度が劣化してしまうという課題があった。
【００１１】
この発明は上記のような課題を解決するためになされたもので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる目標追尾装置及び目標追尾方法を得ることを目的とする。
【課題を解決するための手段】
【００１２】
この発明に係る目標追尾装置は、追尾対象の移動体の観測値を用いて、現時刻における移動体の位置及び速度を算出する位置速度算出手段と、位置速度算出手段により算出された位置及び速度を用いて、指定時間後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、位置速度算出手段により算出された位置及び速度と指向用加速度算出手段により算出された経由点指向用の加速度を用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、回避用加速度算出手段により算出された進入回避用の加速度と位置速度算出手段により算出された位置及び速度を用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定手段とを設け、移動経路予測手段が、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定手段により特定された回避経路が移動体の移動経路であると予測する一方、判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定手段により特定された経由点指向経路が移動体の移動経路であると予測するようにしたものである。
【発明の効果】
【００１３】
この発明によれば、追尾対象の移動体の観測値を用いて、現時刻における移動体の位置及び速度を算出する位置速度算出手段と、位置速度算出手段により算出された位置及び速度を用いて、指定時間後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、位置速度算出手段により算出された位置及び速度と指向用加速度算出手段により算出された経由点指向用の加速度を用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、回避用加速度算出手段により算出された進入回避用の加速度と位置速度算出手段により算出された位置及び速度を用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定手段とを設け、移動経路予測手段が、判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定手段により特定された回避経路が移動体の移動経路であると予測する一方、判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定手段により特定された経由点指向経路が移動体の移動経路であると予測するように構成したので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる効果がある。
【図面の簡単な説明】
【００１４】
【図１】この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】この発明の実施の形態１による目標追尾装置の処理内容を示すフローチャートである。
【図３】式（２）の運動モデルを示す概念図である。
【図４】経由点指向方向を示す説明図である。
【図５】経由点指向経路を示す説明図である。
【図６】移動体の移動経路と到達時刻を示す説明図である。
【図７】回避用加速度算出部１４の処理内容を示すフローチャートである。
【図８】水平方向の仮想経路の設定例を示す説明図である。
【図９】進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を示す説明図である。
【図１０】回避用加速度算出部１４により生成される回避加速度マップの一例を示す説明図である。
【図１１】進入回避用の加速度に用いる加速度の組み合わせの選択例１を示す説明図である。
【図１２】進入回避用の加速度に用いる加速度の組み合わせの選択例２を示す説明図である。
【図１３】進入回避用の加速度に用いる加速度の組み合わせの選択例３を示す説明図である。
【図１４】進入回避用の加速度に用いる加速度の組み合わせの選択例４を示す説明図である。
【図１５】進入回避用の加速度に用いる加速度の組み合わせの選択例５を示す説明図である。
【図１６】回避経路における回避地点と回避時刻を示す説明図である。
【図１７】複数の回避地点がある場合の移動経路を示す説明図である。
【図１８】この発明の実施の形態２による目標追尾装置を示す構成図である。
【図１９】経路長が異なる回避経路を示す説明図である。
【図２０】この発明の実施の形態３による目標追尾装置を示す構成図である。
【図２１】この発明の実施の形態３による目標追尾装置の移動経路選択部３４の処理内容を示すフローチャートである。
【図２２】回避用加速度算出部３１により算出された進入回避用加速度の一例を示す説明図である。
【発明を実施するための形態】
【００１５】
実施の形態１．
図１はこの発明の実施の形態１による目標追尾装置を示す構成図である。
図１の目標追尾装置は、航空機、船舶、車両などの移動体を追尾する装置であるが、この実施の形態１では、移動体が航空機である例を説明する。
図１において、追尾処理部１は例えばカルマンフィルタなどの追尾フィルタを用いて、追尾対象の移動体の観測値（例えば、レーダやＧＰＳなどのセンサから得られる移動体の位置などを示す観測情報）から、現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する処理を実施する。なお、追尾処理部１は位置速度算出手段を構成している。
【００１６】
経路予測部２は移動体の経路を変更する加速度ベクトルｕｋが考慮されている運動モデルに対して、追尾処理部１により算出された状態ベクトルｘｋを適用することで、移動体の移動経路を予測するとともに、移動体が当該移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
経由点位置情報格納部３は例えばＲＡＭやハードディスクなどの記憶装置から構成されており、移動体の経由点の位置を示す経由点位置情報を格納している。
進入禁止域位置情報格納部４は例えばＲＡＭやハードディスクなどの記憶装置から構成されており、雷雲などの進入禁止域（障害物）の位置を示す進入禁止域位置情報を格納している。
【００１７】
経路予測部２の指向用加速度算出部１１は追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後（指定時間後）に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する処理を実施する。なお、指向用加速度算出部１１は指向用加速度算出手段を構成している。
経由点指向経路特定部１２は追尾処理部１により算出された状態ベクトルｘｋと指向用加速度算出部１１により算出された経由点指向用の加速度ベクトルｕｋを上記運動モデルに適用することで、移動体が経由点に至るまでの経路である経由点指向経路を特定する処理を実施する。なお、経由点指向経路特定部１２は経由点指向経路特定手段を構成している。
【００１８】
進入禁止域衝突判定部１３は経由点指向経路特定部１２により特定された経由点指向経路上の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで、その経由点指向経路が進入禁止域内に入る経路であるか否かを判定する処理を実施する。なお、進入禁止域衝突判定部１３は判定手段を構成している。
回避用加速度算出部１４は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。なお、回避用加速度算出部１４は回避用加速度算出手段を構成している。
【００１９】
回避経路特定部１５は回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する処理を実施する。なお、回避経路特定部１５は回避経路特定手段を構成している。
移動経路予測部１６は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部１６は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部１６は移動経路予測手段及び到達時刻算出手段を構成している。
【００２０】
図１の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、回避用加速度算出部１４、回避経路特定部１５及び移動経路予測部１６のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、回避用加速度算出部１４、回避経路特定部１５及び移動経路予測部１６の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
図２はこの発明の実施の形態１による目標追尾装置の処理内容を示すフローチャートである。
【００２１】
次に動作について説明する。
追尾処理部１は、例えばカルマンフィルタなどの追尾フィルタを実装しており、レーダやＧＰＳなどのセンサから追尾中の移動体の観測値（移動体の位置などを示す観測情報）を受けると、その観測値に基づいて現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する（図２のステップＳＴ１）。
ここでは、追尾処理部１がカルマンフィルタを実装している例を示しているが、追尾フィルタはカルマンフィルタに限るものではなく、他の公知のフィルタを実装するようにしてもよい。
【００２２】
経路予測部２は、追尾処理部１が状態ベクトルｘｋを算出すると、下記の式（２）に示すように、移動体の経路を変更する加速度ベクトルｕｋが考慮されている運動モデルに対して、その状態ベクトルｘｋを適用することで、移動体の移動経路を予測するとともに、移動体が当該移動経路上を移動して経由点に到達する時刻を算出する。
式（２）において、ｘｋは時刻ｔｋにおける状態ベクトル（位置・速度）、Φｋは時刻ｔｋから時刻ｔｋ＋１に推移する際の既知の状態ベクトル推移行列、ｕｋは運動座標における加速度ベクトル、Γｋは加速度ベクトルｕｋの運動座標から北基準直交座標に変換する際の既知の変換行列である。
【００２３】
ここで、図３は式（２）の運動モデルを示す概念図である。
移動体は加速度ベクトルｕｋが設定されることで、図３に示すように、直線的に移動している状態から、左旋回／右旋回を行う状態、上昇／下降を行う状態又は加速／減速を行う状態に変化する。
加速度ベクトルｕｋは、レーダやＧＰＳなどのセンサからは観測不可能な値であるので、この実施の形態１では、後述する指向用加速度算出部１１及び回避用加速度算出部１４が算出するようにしている。
以下、経路予測部２の処理内容を具体的に説明する。
【００２４】
この実施の形態１では、移動体である航空機は、図４に示すように、経由点位置情報格納部３により格納されている経由点位置情報が示す経由点を目指して飛行しているものとする。
指向用加速度算出部１１は、追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後（指定時間後）に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する（ステップＳＴ２）。
即ち、指向用加速度算出部１１は、下記の式（３）〜（５）を計算することで、経由点指向用の加速度ベクトルｕｋを算出する。
【００２５】
ただし、ｖｇは移動体の水平方向の加速度、ｗｇは移動体の鉛直方向の加速度、θｖは移動体の水平方向の指向角度、θｗは移動体の鉛直方向の指向角度である。
【００２６】
経由点指向経路特定部１２は、指向用加速度算出部１１が経由点指向用の加速度ベクトルｕｋを算出すると、その経由点指向用の加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図５に示すように、移動体が経由点に至るまでの経路（予測時間Ｔｍａｘまでの経路）である経由点指向経路を特定する（ステップＳＴ３）。
即ち、経由点指向経路特定部１２は、式（２）に基づいて、現時刻ｔｋからＴａ秒後における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で移動するものとして、経由点に至るまでの経由点指向経路を特定する（図６を参照）。
【００２７】
進入禁止域衝突判定部１３は、経由点指向経路特定部１２が経由点指向経路を特定すると、例えば時刻ΔＴ刻みで、その経由点指向経路上での移動体の位置を算出し、各時刻における移動体の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで、その経由点指向経路が進入禁止域内に入る経路であるか否かを判定する（ステップＳＴ４）。
【００２８】
回避用加速度算出部１４は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、現時刻ｔｋにおける移動体の進行方向を基準とする７種類の運動モデル（直進、加速、減速、左旋回、右旋回、上昇、下降）を考慮して（図３を参照）、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する（ステップＳＴ５）。
以下、回避用加速度算出部１４による進入回避用の加速度の算出処理を具体的に説明する。ただし、ここでは、説明の簡単化のため、移動体が加速及び減速を行わないものとする。
【００２９】
図７は回避用加速度算出部１４の処理内容を示すフローチャートである。
まず、回避用加速度算出部１４は、移動体の性能から想定される水平方向の最大加速度を上限として、Δｕステップで水平方向の加速度の候補を設定する。
回避用加速度算出部１４は、水平方向の加速度の候補を複数設定すると、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、複数の仮想経路を生成する（図７のステップＳＴ２１）。
図８は水平方向の仮想経路の設定例を示す説明図であり、図８では、３個の仮想経路を設定している例を示している。
【００３０】
回避用加速度算出部１４は、複数の仮想経路を生成すると、仮想経路毎に、当該仮想経路が進入禁止域内に入る経路であるか否かを判定する。
仮想経路が進入禁止域内に入る経路であるか否かの判定は、進入禁止域衝突判定部１３と同様に、仮想経路上での各時刻における移動体の位置と進入禁止域位置情報格納部４により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで判定することができる。
【００３１】
回避用加速度算出部１４は、当該仮想経路が進入禁止域内に入る経路である場合、移動体の性能から想定される鉛直方向の最大加速度を上限として、進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出する（図９を参照）。
即ち、回避用加速度算出部１４は、移動体が進入禁止域内に進入する進入時刻Ｔｃ（図８の例では、２ΔＴの時刻）を算出し、その進入時刻Ｔｃにおいて、例えば、Δｕステップで、鉛直方向の加速度を大きくすることで、当該仮想経路を鉛直方向に段階的に変更する。
回避用加速度算出部１４は、当該仮想経路を鉛直方向に段階的に変更することで、当該仮想経路が進入禁止域内に入る経路から、進入禁止域内に入らない経路に変化する際の鉛直方向の加速度を探索する。
ただし、鉛直方向の最大加速度を設定しても、当該仮想経路が進入禁止域内に入る経路である場合、必要最小限の鉛直方向の加速度として、最大加速度を設定する。
【００３２】
ここでは、当該仮想経路を鉛直方向に段階的に変更することで、必要最小限の鉛直方向の加速度を探索する例を示したが、下記の式（６）を計算することで、必要最小限の鉛直方向の加速度を算出するようにしてもよい。
ただし、Ｈは進入禁止域を回避するために必要な高度差、Ｔｃは進入時刻（ΔＴステップで設定した時刻の中で、進入禁止域内に入る最小の時刻）、ｗｇ，Ｈは高度差Ｈを回避するために必要な鉛直方向の加速度である。
【００３３】
回避用加速度算出部１４は、仮想経路毎に、最小限の鉛直方向の加速度を算出すると、当該仮想経路に対応する水平方向の加速度（当該仮想経路を生成する際に、経由点指向経路の変更に用いている水平方向の加速度の候補）と、当該仮想経路に対応する鉛直方向の加速度との組み合わせが得られるので、当該仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせを示す回避加速度マップを生成する（ステップＳＴ２２）。
図１０は回避用加速度算出部１４により生成される回避加速度マップの一例を示す説明図である。
図１０の例では、ＡＢＣＤで囲まれている領域が進入禁止域であり、上記領域の外側の領域が進入禁止域外である。
【００３４】
回避用加速度算出部１４は、各仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせを示す回避加速度マップを生成すると、その回避加速度マップを利用して、各仮想経路に対応する水平方向及び鉛直方向の加速度の組み合わせの中から、進入禁止域内への進入を回避するために必要な進入回避用の加速度に用いる加速度の組み合わせを選択する。
具体的には、以下のようにして、進入回避用の加速度に用いる加速度の組み合わせを選択する。
【００３５】
図１１〜図１５は進入回避用の加速度に用いる加速度の組み合わせの選択例を示す説明図である。
図において、ｖｇは経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度、ｗｇは経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度、ｖ’ｇは水平方向の進入回避用加速度、ｗ’ｇは鉛直方向の進入回避用加速度である。
この実施の形態１では、移動体が民間の航空機である等の場合、鉛直方向の加速度を変化させるよりも、水平方向の加速度を変化させることが、より自然な運航であるため（対地高度を一定とする運航）、水平方向を変化させる組み合わせを優先的に選択するものとする（鉛直方向の加速度が小さい組み合わせを優先的に選択するものとする）。
特に、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ水平方向の回避加速度を変化させる組み合わせを優先的に選択するものとする。
【００３６】
［優先順位１］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ加速度であり、鉛直方向の進入回避用加速度ｗ’ｇがゼロ（対地高度が一定）である組み合わせを選択する。
このケースは図１１に示すケース１に該当し、水平方向の進入回避用加速度ｖ’ｇは、下記の式（７）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇ（Δｖｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する（ステップＳＴ２３，ＳＴ２４）。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００３７】
［優先順位２］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度ｗ’ｇが経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ加速度の組み合わせを選択する。
このケースは図１２に示すケース２に該当し、水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２４）。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（８）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇ（Δｗｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する（ステップＳＴ２５，ＳＴ２６）。
【００３８】
［優先順位３］
「経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、その加速度ｖｇと逆符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと逆の符号を持つ加速度であり、鉛直方向の進入回避用加速度ｗ’ｇがゼロ（対地高度が一定）である組み合わせを選択する。
このケースは図１３に示すケース３に該当し、水平方向の進入回避用加速度ｖ’ｇは、下記の式（９）に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇを加えることで算出する（ステップＳＴ２３，ＳＴ２４）。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００３９】
［優先順位４］
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度ｖ’ｇが経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度ｗ’ｇが経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度である組み合わせを選択する。
このケースは図１４に示すケース４に該当し、水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２７）。
ｖ’ｇ＝ｖｇ，ｍａｘ
また、鉛直方向の進入回避用加速度ｗ’ｇは、経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度ｗｇ，ｍａｘに設定する（ステップＳＴ２３，ＳＴ２７）。
ｗ’ｇ＝ｗｇ，ｍａｘ
【００４０】
ここでは、ケース４に該当する場合、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定し、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定する例を示したが、最大加速度に設定しても、進入禁止域内への進入を回避することができないので、追尾航跡が更新される次の時刻まで待って、進入回避用の加速度を設定する処理を再度実施するようにしてもよい。
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
【００４１】
ここでは、水平方向の回避加速度を最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合（以下、「場合Ａ」と称する）を優先順位２として、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同符号では、水平方向の回避加速度を最大値としても進入禁止域内となるが、その加速度ｖｇと逆の符号で、水平方向の回避加速度を変化させることで進入禁止域外となる場合（以下、「場合Ｂ」と称する）を優先順位３としているが、優先順位を逆にして、場合Ａを優先順位３にして、場合Ｂを優先順位２にしてもよい。
【００４２】
また、優先順位３のケースに該当しない場合、直ちに、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定して、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定しているが、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと逆符号である条件下で、水平方向の回避加速度を最大加速度ｖｇ，ｍａｘに設定して、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合には、そのように進入回避用加速度を設定するようにしてもよい。
【００４３】
ここでは、回避用加速度算出部１４が、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号である条件下で、水平方向の回避加速度を変化させる組み合わせを優先的に選択するものを示したが、図１５に示すように、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号によらず、変化が最小の加速度を水平方向の進入回避用加速度ｖ’ｇとして優先的に設定するようにしてもよい。
即ち、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号と逆の符号を持つ最小限の値ｖｇ，ＬＩＭが、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇの符号と同じ符号を持つ最小限の値ｖｇ，ＬＩＭより小さい場合、逆の符号を持つ最小限の値ｖｇ，ＬＩＭから水平方向の進入回避用加速度ｖ’ｇを算出するようにしてもよい。
また、ここでは、説明の簡単化のために、移動体が加速及び減速を行わないものとして説明したが、加速及び減速を含めて仮想経路を生成するようにしてもよいことは言うまでもない。
【００４４】
回避経路特定部１５は、回避用加速度算出部１４が進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ、鉛直方向の進入回避用加速度ｗ’ｇ）を算出すると、その進入回避用の加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図１６に示すように、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する（図２のステップＳＴ６）。
【００４５】
即ち、回避経路特定部１５は、移動体が進入禁止域内に進入する進入時刻Ｔｃを回避時刻Ｔvとして、加速度ベクトルｕｋと状態ベクトルｘｋを式（２）の運動モデルに適用することで、回避時刻Ｔvにおける移動体の位置（図１６では、「回避地点」と記述している）と、移動体の速度を算出する。
回避経路特定部１５は、その回避地点と移動体の速度を追尾航跡の位置・速度とみなすことで、移動体の出発地点（または、前回の回避地点）から回避地点までの経路を回避経路として特定する。
【００４６】
回避経路特定部１５は、回避経路における移動体の各時刻の位置及び速度を指向用加速度算出部１１にフィードバックすることで、指向用加速度算出部１１が各時刻の位置及び速度から経由点用の加速度を算出し、進入禁止域衝突判定部１３において、経由点指向経路特定部１２により特定された経由点指向経路が進入禁止域内に入らない経路と判定されるまで、指向用加速度算出部１１〜回避経路特定部１５の間でループ処理を実施する。
【００４７】
移動経路予測部１６は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合（ステップＳＴ４）、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する（ステップＳＴ７）。
移動経路予測部１６は、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測すると、式（２）に基づいて、現時刻ｔｋからＴａ秒後（指定時間後）における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する（図６を参照）。
【００４８】
また、移動経路予測部１６は、その指向終了位置と経由点の間の距離Ｒａと、その指向終了位置における移動体の速度Ｖａとから、移動体が経由点に到達する時刻Ｔpを算出する（ステップＳＴ８）。
【００４９】
移動経路予測部１６は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、図１６に示すように、回避経路特定部１５により特定された回避経路（現地点から回避地点までの経路）が移動体の移動経路であると予測する（ステップＳＴ９）。
移動経路予測部１６は、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測すると、回避時刻Ｔvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
【００５０】
移動経路予測部１６は、図１７に示すように、複数の回避地点がある場合、最終の回避地点から経由点に到達する時間と、複数の回避地点間の移動に要する時間（複数の回避地点における回避時刻Ｔvの差分）との和を求めることで、移動体が経由点に到達する時刻Ｔpを算出する（ステップＳＴ１０）。
式（１２）において、右辺第２項は式（１１）の右辺と同じ意味であり、Ｔv，ｉはｉ−１番目からｉ番目の回避地点に到達するまでに要する時間である。ただし、ｉ＝１である場合に限り、現在位置から１番目の回避地点に到達するまでに要する時間である。
【００５１】
図１の目標追尾装置は、移動体の追尾を継続する場合、ステップＳＴ１の処理に戻り、ステップＳＴ１〜ＳＴ１０の処理を繰り返し実施する（ステップＳＴ１１）。
【００５２】
以上で明らかなように、この実施の形態１によれば、追尾対象の移動体の観測値を用いて、現時刻ｔｋにおける移動体の位置及び速度を示す状態ベクトルｘｋを算出する追尾処理部１と、追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出する指向用加速度算出部１１と、追尾処理部１により算出された状態ベクトルｘｋと指向用加速度算出部１１により算出された経由点指向用の加速度ベクトルｕｋを用いて、移動体が経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定部１２と、経由点指向経路特定部１２により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する進入禁止域衝突判定部１３と、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出部１４と、回避用加速度算出部１４により算出された進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する回避経路特定部１５とを設け、移動経路予測部１６が、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測するように構成したので、移動体が雷雲などの進入禁止域を回避するために経路が変更される場合でも、移動体の将来位置などを高精度に推定することができる効果を奏する。
【００５３】
また、この実施の形態１によれば、移動経路予測部１６が、移動体が予測した移動経路上を移動して経由点に到達する時刻を算出するように構成したので、移動体が経由点に到達する時刻を高精度に推定することができる効果を奏する。
【００５４】
また、この実施の形態１によれば、回避用加速度算出部１４が、水平方向及び鉛直方向の複数の加速度の組み合わせのうち、鉛直方向の加速度が小さい組み合わせを優先的に選択するように構成したので、例えば、移動体が民間の航空機である等の場合には、より自然な運航（対地高度を一定とする運航）に見合う進入回避用加速度を算出することができるようになり、その結果、移動体の将来位置や経由点到達時刻等の推定精度を高めることができる効果を奏する。
【００５５】
なお、この実施の形態１では、指向用加速度算出部１１が追尾処理部１により算出された状態ベクトルｘｋを用いて、現時刻ｔｋからＴａ秒後に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルｕｋを算出するものを示したが、式（２）の右辺第１項の状態ベクトルｘｋの速度成分については、追尾フィルタによって、レーダやＧＰＳなどのセンサの観測値が示す位置から推定した速度を用いるようにしてもよい。また、移動体から得られる速度情報を用いるようにしてもよい。
【００５６】
例えば、移動体が航空機である場合、航空機の機体から真対気速度が得られるので、風速データを加味することで対地速度が算出することができる。
このため、対地速度を状態ベクトルｘｋの速度成分として用いるようにしてもよい。
予め、目標進行予定の空間における風速データが既知の場合には、風速による状態ベクトルの速度変化を仮想経路の生成や、移動経路の予測に反映することが可能である。
【００５７】
この実施の形態１では、回避用加速度算出部１４が進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出するものを示したが、複数の回避地点が存在する場合、複数の回避地点の間で、進入回避用加速度が大きく異なるケースが発生することがある。
そこで、隣り合う回避地点で、進入回避用加速度が急激に変化する状況の発生を防止するために、予め加速度変化率パラメータΔαを設定し、時間方向に対して、Δα刻みで進入回避用加速度を変化させるようにしてもよい。
【００５８】
この実施の形態１では、進入禁止域が１つである例を説明したが、進入禁止域が複数存在する場合でも、同様の処理を繰り返すことで、進入禁止域内への進入を回避することが可能である。
【００５９】
実施の形態２．
上記実施の形態１では、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避用加速度算出部１４が、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出し、回避経路特定部１５が回避用加速度算出部１４により算出された進入回避用の加速度を用いて、回避経路を特定するものを示したが、図１９に示すように、進入禁止域が進行方向に対して奥行きがある形を成している場合、回避経路特定部１５により特定される可能性がある回避経路（図中、実線の経路）が、他の回避経路（図中、点線の経路）よりも経路長が長くなる場合がある。
この実施の形態２では、進入禁止域が進行方向に対して奥行きがある形を成している場合でも、経路長が短い回避経路を特定することができるようにしている。
【００６０】
図１８はこの発明の実施の形態２による目標追尾装置を示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
進入禁止域容積算出部２１は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域における経由点指向経路の進行方向左側の容積を算出するとともに、その進入禁止域における経由点指向経路の進行方向右側の容積を算出する処理を実施する。なお、進入禁止域容積算出部２１は容積算出手段を構成している。
【００６１】
回避用加速度算出部２２は進入禁止域容積算出部２１により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。なお、回避用加速度算出部２２は回避用加速度算出手段を構成している。
【００６２】
図１８の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部２２、回避経路特定部１５及び移動経路予測部１６のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部２２、回避経路特定部１５及び移動経路予測部１６の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
【００６３】
次に動作について説明する。
進入禁止域容積算出部２１は、進入禁止域衝突判定部１３が上記実施の形態１と同様にして、経由点指向経路が進入禁止域内に入る経路であるか否かの判定処理を行うと、その判定結果が、経由点指向経路が進入禁止域内に入る経路である旨を示していれば、進入禁止域位置情報格納部４により格納されている進入禁止域位置情報を参照して、進入禁止域の形状を確認する。
進入禁止域容積算出部２１は、進入禁止域の形状を確認すると、その進入禁止域と経由点指向経路特定部１２により特定された経由点指向経路の位置関係を把握し、その経由点指向経路に対して、進入禁止域を進行方向左側のエリアと右側のエリアに分断する。
【００６４】
進入禁止域容積算出部２１は、進行方向左側のエリア及び右側のエリアの形状を把握することで、進行方向左側のエリアの容積と、進行方向右側のエリアの容積とを算出して、その算出結果を回避用加速度算出部２２に出力する。
なお、正確な容積を算出することができない場合には、容積に相当する量（例えば、奥行きの長さ×幅）を算出し、容積に相当する量をエリアの容積として回避用加速度算出部２２に出力する。
【００６５】
回避用加速度算出部２２は、進入禁止域容積算出部２１が進行方向左側のエリアの容積と、進行方向右側のエリアの容積とを算出すると、容積が小さいエリア側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
ただし、容積が小さいエリア側の方向に移動体が移動することでは、進入禁止域内への進入を回避することができない場合、容積が大きいエリア側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
図１９の例では、進行方向左側のエリアの容積が、進行方向右側のエリアの容積より小さいので、下記に示す優先順位にしたがって、進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する。
【００６６】
［優先順位１］
「水平方向の回避加速度を進行方向左側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、下記の式（１３）に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇ（Δｖｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００６７】
［優先順位２］
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、進行方向左側の向きに最大加速度ｖｇ，ｍａｘを設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（１４）に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇ（Δｗｇは０以上のオフセット値であって、事前に設定される固定値である）を加えることで算出する。
【００６８】
［優先順位３］
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、水平方向の回避加速度を進行方向右側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、下記の式（１５）に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｖｇ，ＬＩＭに対して、回避量Δｖｇを加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度ｗ’ｇはゼロとする。
ｗ’ｇ＝０
【００６９】
［優先順位４］
「水平方向の回避加速度を進行方向右側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度ｖ’ｇは、進行方向右側の向きに最大加速度ｖｇ，ｍａｘを設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、下記の式（１６）に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値ｗｇ，ＬＩＭに対して、回避量Δｗｇを加えることで算出する。
【００７０】
［優先順位５］
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度ｖ’ｇは、経由点指向用の加速度ベクトルｕｋにおける水平方向の加速度ｖｇと同じ符号を持つ最大加速度ｖｇ，ｍａｘに設定する。
ｖ’ｇ＝ｖｇ，ｍａｘ
鉛直方向の進入回避用加速度ｗ’ｇは、経由点指向用の加速度ベクトルｕｋにおける鉛直方向の加速度ｗｇと同じ符号を持つ最大加速度ｗｇ，ｍａｘに設定する。
ｗ’ｇ＝ｗｇ，ｍａｘ
【００７１】
優先順位５の場合、水平方向の進入回避用加速度ｖ’ｇを最大加速度ｖｇ，ｍａｘに設定し、鉛直方向の進入回避用加速度ｗ’ｇを最大加速度ｗｇ，ｍａｘに設定する例を示したが、最大加速度に設定しても、進入禁止域内への進入を回避することができないので、追尾航跡が更新される次の時刻まで待って、進入回避用の加速度を設定する処理を再度実施するようにしてもよい。
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
【００７２】
ここでは、水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合（以下、「場合Ａ」と称する）を優先順位２として、水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、水平方向の回避加速度を進行方向右側に変化させることで進入禁止域外となる場合（以下、「場合Ｂ」と称する）を優先順位３としているが、優先順位を逆にして、場合Ａを優先順位３にして、場合Ｂを優先順位２にしてもよい。
【００７３】
回避経路特定部１５は、回避用加速度算出部２２が進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ、鉛直方向の進入回避用加速度ｗ’ｇ）を算出すると、上記実施の形態１と同様に、その進入回避用の加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、図１９に示すように、移動体が進入禁止域内に入らずに経由点に至る回避経路を特定する。
これにより、回避用加速度算出部２２により高い優先順位１，２で進入回避用加速度が算出されている場合、進入禁止域が進行方向に対して奥行きがある形を成していても、経路長が短い経路（図中、点線の経路）が回避経路となる。
移動経路予測部１６の処理内容は、上記実施の形態１と同様であるため説明を省略する。
【００７４】
以上で明らかなように、この実施の形態２によれば、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域における経由点指向経路の進行方向左側の容積を算出するとともに、その進入禁止域における経由点指向経路の進行方向右側の容積を算出する進入禁止域容積算出部２１を設け、回避用加速度算出部２２が、進入禁止域容積算出部２１により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出するように構成したので、進入禁止域が進行方向に対して奥行きがある形を成している場合でも、経路長が短い回避経路を特定することができる効果を奏する。
【００７５】
実施の形態３．
上記実施の形態１，２では、回避経路特定部１５が１つの回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部１６が回避経路特定部１５により特定された回避経路が移動体の移動経路であると予測するものを示したが、回避経路特定部１５が複数の回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部１６が回避経路特定部１５により特定された複数の回避経路が移動体の移動経路であると予測し、複数の移動経路の中で、経由点の到達時刻Ｔp又は進入回避用の加速度が最小となる移動経路を選択するようにしてもよい。
【００７６】
図２０はこの発明の実施の形態３による目標追尾装置を示す構成図であり、図において、図１８と同一符号は同一または相当部分を示すので説明を省略する。
回避用加速度算出部３１は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を算出する処理を実施する。
回避用加速度算出部３１における進入回避用の加速度の算出方法は、図１の回避用加速度算出部１４の算出方法と同一であってもよいし、図１８の回避用加速度算出部２２の算出方法と同一であってもよいが、回避用加速度算出部３１では、進入回避用の加速度を複数算出する。
なお、回避用加速度算出部３１は回避用加速度算出手段を構成している。
【００７７】
回避経路特定部３２は回避用加速度算出部３１により算出された複数の進入回避用の加速度と追尾処理部１により算出された状態ベクトルｘｋを用いて、移動体が進入禁止域内に入らずに経由点に至る複数の回避経路を特定する処理を実施する。なお、回避経路特定部３２は回避経路特定手段を構成している。
移動経路予測部３３は進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部３２により特定された複数の回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部３３は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部３３は移動経路予測手段及び到達時刻算出手段を構成している。
【００７８】
移動経路選択部３４は移動経路予測部３３により複数の回避経路が移動体の移動経路であると予測された場合、複数の移動経路の中で、移動経路予測部３３により算出された経由点到達時刻Ｔp又は回避用加速度算出部３１により算出された進入回避用の加速度が最小となる移動経路を選択する処理を実施する。なお、移動経路選択部３４は移動経路選択手段を構成している。
【００７９】
図２０の例では、目標追尾装置の構成要素である追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部３１、回避経路特定部３２、移動経路予測部３３及び移動経路選択部３４のそれぞれが専用のハードウェア（例えば、ＣＰＵを実装している半導体集積回路、あるいは、ワンチップマイコンなど）で構成されているものを想定しているが、目標追尾装置の全部又は一部がコンピュータで構成されている場合、追尾処理部１、指向用加速度算出部１１、経由点指向経路特定部１２、進入禁止域衝突判定部１３、進入禁止域容積算出部２１、回避用加速度算出部３１、回避経路特定部３２、移動経路予測部３３及び移動経路選択部３４の処理内容を示すプログラムの全部又は一部をコンピュータのメモリに格納し、当該コンピュータのＣＰＵが当該メモリに格納されているプログラムを実行するようにしてもよい。
図２１はこの発明の実施の形態３による目標追尾装置の移動経路選択部３４の処理内容を示すフローチャートである。
【００８０】
次に動作について説明する。
回避用加速度算出部３１は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度（水平方向の進入回避用加速度ｖ’ｇ，鉛直方向の進入回避用加速度ｗ’ｇ）を複数算出する。
回避用加速度算出部３１における進入回避用の加速度の算出方法は、特に問わないが、図１の回避用加速度算出部１４と同様にして、例えば、図２２に示すように、第１の進入回避用加速度（水平方向の進入回避用加速度ｖ’ｇ１，鉛直方向の進入回避用加速度ｗ’ｇ１）を算出し、図１８の回避用加速度算出部２２と同様にして、第２の進入回避用加速度（水平方向の進入回避用加速度ｖ’ｇ２，鉛直方向の進入回避用加速度ｗ’ｇ２）を算出する。
以降の説明では、説明の簡単化のため、図２２に示すように、回避用加速度算出部３１が、２つの進入回避用加速度（第１の進入回避用加速度、第２の進入回避用加速度）を算出するものとするが、３以上の進入回避用加速度を算出するようにしてもよい。
【００８１】
回避経路特定部３２は、回避用加速度算出部３１が第１の進入回避用加速度と第２の進入回避用加速度を算出すると、図１の回避経路特定部１５と同様に、第１の進入回避用加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと追尾処理部１により算出された状態ベクトルｘｋを式（２）の運動モデルに適用することで、移動体が進入禁止域内に入らずに経由点に至る第１の回避経路を特定する。
また、第２の進入回避用加速度を加速度ベクトルｕｋとして、その加速度ベクトルｕｋと状態ベクトルｘｋを式（２）の運動モデルに適用することで、移動体が進入禁止域内に入らずに経由点に至る第２の回避経路を特定する。
言うまでもないが、回避用加速度算出部３１がＮ個の進入回避用加速度を算出していれば、回避経路特定部３２がＮ個の回避経路を特定する。
【００８２】
移動経路予測部３３は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、図１の移動経路予測部１６と同様に、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測する。
移動経路予測部３３は、経由点指向経路特定部１２により特定された経由点指向経路が移動体の移動経路であると予測すると、図１の移動経路予測部１６と同様に、現時刻ｔｋからＴａ秒後における指向終了位置を計算し、Ｔａ秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部３３は、図１の移動経路予測部１６と同様に、その指向終了位置と経由点の間の距離Ｒａと、その指向終了位置における移動体の速度Ｖａとから、移動体が経由点に到達する時刻Ｔpを算出する。
【００８３】
移動経路予測部３３は、進入禁止域衝突判定部１３により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部３２により特定された第１及び第２の回避経路（現地点から回避地点までの経路）のそれぞれが移動体の移動経路であると予測する。
移動経路予測部３３は、回避経路特定部３２により特定された第１及び第２の回避経路が移動体の移動経路であると予測すると、回避時刻Ｔvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部３３は、図１の移動経路予測部１６と同様にして、移動体が第１の回避経路上を移動する場合の経由点到達時刻Ｔp，１を算出し、また、移動体が第２の回避経路上を移動する場合の経由点到達時刻Ｔp，２を算出する。
【００８４】
移動経路選択部３４は、移動経路予測部３３により経由点指向経路が移動体の移動経路であると予測された場合、その移動経路と移動経路予測部３３により算出された経由点到達時刻Ｔpを出力する。
一方、移動経路予測部３３により第１及び第２の回避経路が移動体の移動経路であると予測された場合、移動体が第１の回避経路上を移動する場合の経由点到達時刻Ｔp，１と、移動体が第２の回避経路上を移動する場合の経由点到達時刻Ｔp，２との時刻差ΔＴpを算出する（図２１のステップＳＴ３１）。
【００８５】
移動経路選択部３４は、経由点到達時刻の時刻差ΔＴpを算出すると、その時刻差ΔＴpを所定の閾値と比較し（ステップＳＴ３２）、その時刻差ΔＴpが所定の閾値より大きければ、経由点到達時刻Ｔpが最も早い移動経路（回避経路）を選択し、その移動経路と最も早い経由点到達時刻Ｔpを出力する（ステップＳＴ３３）。
例えば、経由点到達時刻Ｔp，１が経由点到達時刻Ｔp，２より早ければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、経由点到達時刻Ｔp，２が経由点到達時刻Ｔp，１より早ければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８６】
移動経路選択部３４は、経由点到達時刻の時刻差ΔＴpが所定の閾値以下である場合、移動体が第１の回避経路上を移動する場合の鉛直方向の進入回避用加速度ｗ’ｇ１と、移動体が第２の回避経路上を移動する場合の鉛直方向の進入回避用加速度ｗ’ｇ２との加速度差Δｗ’ｇを算出する（ステップＳＴ３４）。
【００８７】
移動経路選択部３４は、鉛直方向の加速度差Δｗ’ｇを算出すると、その加速度差Δｗ’ｇを所定の閾値と比較し（ステップＳＴ３５）、その加速度差Δｗ’ｇが所定の閾値より大きければ、鉛直方向の進入回避用加速度ｗ’ｇが最も小さい移動経路（回避経路）を選択し、その移動経路と経由点到達時刻Ｔpを出力する（ステップＳＴ３６）。
例えば、進入回避用加速度ｗ’ｇ１が進入回避用加速度ｗ’ｇ２より小さければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、進入回避用加速度ｗ’ｇ２が進入回避用加速度ｗ’ｇ１より小さければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８８】
一方、加速度差Δｗ’ｇが所定の閾値以内である場合、移動体が第１の回避経路上を移動する場合の水平方向の進入回避用加速度ｖ’ｇ１と、移動体が第２の回避経路上を移動する場合の水平方向の進入回避用加速度ｖ’ｇ２とを比較し、水平方向の進入回避用加速度ｖ’ｇが最も小さい移動経路（回避経路）を選択し、その移動経路と経由点到達時刻Ｔpを出力する（ステップＳＴ３７）。
例えば、進入回避用加速度ｖ’ｇ１が進入回避用加速度ｖ’ｇ２より小さければ、第１の移動経路（第１の回避経路）と経由点到達時刻Ｔp，１を出力する。一方、進入回避用加速度ｖ’ｇ２が進入回避用加速度ｖ’ｇ１より小さければ、第２の移動経路（第２の回避経路）と経由点到達時刻Ｔp，２を出力する。
【００８９】
以上で明らかなように、この実施の形態３によれば、移動経路予測部３３により複数の回避経路が移動体の移動経路であると予測された場合、移動経路選択部３４が、複数の移動経路の中で、移動経路予測部３３により算出された経由点到達時刻Ｔp又は回避用加速度算出部３１により算出された進入回避用の加速度が最小となる移動経路を選択するように構成したので、現実の運航に近いスムーズな移動経路を予測することができる効果を奏する。
【００９０】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【００９１】
１ 追尾処理部（位置速度算出手段）、２ 経路予測部、３ 経由点位置情報格納部、４ 進入禁止域位置情報格納部、１１ 指向用加速度算出部（指向用加速度算出手段）、１２ 経由点指向経路特定部（経由点指向経路特定手段）、１３ 進入禁止域衝突判定部（判定手段）、１４ 回避用加速度算出部（回避用加速度算出手段）、１５ 回避経路特定部（回避経路特定手段）、１６ 移動経路予測部（移動経路予測手段、到達時刻算出手段）、２１ 進入禁止域容積算出部（容積算出手段）、２２ 回避用加速度算出部（回避用加速度算出手段）、３１ 回避用加速度算出部（回避用加速度算出手段）、３２ 回避経路特定部（回避経路特定手段）、３３ 移動経路予測部（移動経路予測手段、到達時刻算出手段）、３４ 移動経路選択部（移動経路選択手段）。
【特許請求の範囲】
【請求項１】
追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出手段と、上記位置速度算出手段により算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、上記位置速度算出手段により算出された位置及び速度と上記指向用加速度算出手段により算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、上記経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、上記回避用加速度算出手段により算出された進入回避用の加速度と上記位置速度算出手段により算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定手段により特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定手段により特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測手段とを備えた目標追尾装置。
【請求項２】
移動体が移動経路予測手段により予測された移動経路上を移動して経由点に到達する時刻を算出する到達時刻算出手段を設けたことを特徴とする請求項１記載の目標追尾装置。
【請求項３】
回避用加速度算出手段は、現時刻における移動体の進行方向を基準とする複数種類の運動モデルを考慮して、進入禁止域内への進入を回避するために必要な最小限の進入回避用の加速度を算出することを特徴とする請求項１または請求項２記載の目標追尾装置。
【請求項４】
回避用加速度算出手段は、移動体に対する水平方向の加速度の候補を複数用意し、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、当該仮想の経路が進入禁止域内に入る経路であれば、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出する一方、当該仮想の経路が進入禁止域内に入らない経路であれば、上記鉛直方向の加速度をゼロに設定し、
上記移動体に対する水平方向及び鉛直方向の複数の加速度の組み合わせの中から、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を選択することを特徴とする請求項１から請求項３のうちのいずれか１項記載の目標追尾装置。
【請求項５】
回避用加速度算出手段は、仮想の経路が進入禁止域内に入る経路であれば、移動体が上記進入禁止域内に進入する進入時刻を算出し、その進入時刻と当該進入時刻における進入禁止域の高さを用いて、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出することを特徴とする請求項４記載の目標追尾装置。
【請求項６】
回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせのうち、鉛直方向の加速度が小さい組み合わせを優先的に選択することを特徴とする請求項４または請求項５記載の目標追尾装置。
【請求項７】
回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせの中に、鉛直方向の加速度がゼロの組み合わせが１以上ある場合、鉛直方向の加速度がゼロの組み合わせの中から、水平方向の加速度が経由点指向用の加速度と同じ符号である組み合わせを優先的に選択することを特徴とする請求項４または請求項５記載の目標追尾装置。
【請求項８】
判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域における上記経由点指向経路の進行方向左側の容積及び進行方向右側の容積を算出する容積算出手段を設け、
回避用加速度算出手段は、上記容積算出手段により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する
ことを特徴とする請求項１または請求項２記載の目標追尾装置。
【請求項９】
回避用加速度算出手段が進入禁止域内への進入を回避するために必要な進入回避用の加速度を複数算出し、回避経路特定手段が上記回避用加速度算出手段により算出された複数の加速度を用いて、複数の回避経路を特定し、移動経路予測手段が上記回避経路特定手段により特定された複数の回避経路が移動体の移動経路であると予測する場合、
上記移動経路予測手段により予測された複数の移動経路の中で、到達時刻算出手段により算出された到達時刻又は上記回避用加速度算出手段により算出された進入回避用の加速度が最小となる移動経路を選択する移動経路選択手段を設けた
ことを特徴とする請求項２記載の目標追尾装置。
【請求項１０】
位置速度算出手段が追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出処理ステップと、指向用加速度算出手段が上記位置速度算出処理ステップで算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出処理ステップと、経由点指向経路特定手段が上記位置速度算出処理ステップで算出された位置及び速度と上記指向用加速度算出処理ステップで算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定処理ステップと、判定手段が上記経由点指向経路特定処理ステップで特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定処理ステップと、回避用加速度算出手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出処理ステップと、回避経路特定手段が上記回避用加速度算出処理ステップで算出された進入回避用の加速度と上記位置速度算出処理ステップで算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定処理ステップと、移動経路予測手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定処理ステップで特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定処理ステップで経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定処理ステップで特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測処理ステップとを備えた目標追尾方法。
【請求項１】
追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出手段と、上記位置速度算出手段により算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、上記位置速度算出手段により算出された位置及び速度と上記指向用加速度算出手段により算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、上記経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、上記回避用加速度算出手段により算出された進入回避用の加速度と上記位置速度算出手段により算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定手段により特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定手段により特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測手段とを備えた目標追尾装置。
【請求項２】
移動体が移動経路予測手段により予測された移動経路上を移動して経由点に到達する時刻を算出する到達時刻算出手段を設けたことを特徴とする請求項１記載の目標追尾装置。
【請求項３】
回避用加速度算出手段は、現時刻における移動体の進行方向を基準とする複数種類の運動モデルを考慮して、進入禁止域内への進入を回避するために必要な最小限の進入回避用の加速度を算出することを特徴とする請求項１または請求項２記載の目標追尾装置。
【請求項４】
回避用加速度算出手段は、移動体に対する水平方向の加速度の候補を複数用意し、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、当該仮想の経路が進入禁止域内に入る経路であれば、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出する一方、当該仮想の経路が進入禁止域内に入らない経路であれば、上記鉛直方向の加速度をゼロに設定し、
上記移動体に対する水平方向及び鉛直方向の複数の加速度の組み合わせの中から、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を選択することを特徴とする請求項１から請求項３のうちのいずれか１項記載の目標追尾装置。
【請求項５】
回避用加速度算出手段は、仮想の経路が進入禁止域内に入る経路であれば、移動体が上記進入禁止域内に進入する進入時刻を算出し、その進入時刻と当該進入時刻における進入禁止域の高さを用いて、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出することを特徴とする請求項４記載の目標追尾装置。
【請求項６】
回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせのうち、鉛直方向の加速度が小さい組み合わせを優先的に選択することを特徴とする請求項４または請求項５記載の目標追尾装置。
【請求項７】
回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせの中に、鉛直方向の加速度がゼロの組み合わせが１以上ある場合、鉛直方向の加速度がゼロの組み合わせの中から、水平方向の加速度が経由点指向用の加速度と同じ符号である組み合わせを優先的に選択することを特徴とする請求項４または請求項５記載の目標追尾装置。
【請求項８】
判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域における上記経由点指向経路の進行方向左側の容積及び進行方向右側の容積を算出する容積算出手段を設け、
回避用加速度算出手段は、上記容積算出手段により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する
ことを特徴とする請求項１または請求項２記載の目標追尾装置。
【請求項９】
回避用加速度算出手段が進入禁止域内への進入を回避するために必要な進入回避用の加速度を複数算出し、回避経路特定手段が上記回避用加速度算出手段により算出された複数の加速度を用いて、複数の回避経路を特定し、移動経路予測手段が上記回避経路特定手段により特定された複数の回避経路が移動体の移動経路であると予測する場合、
上記移動経路予測手段により予測された複数の移動経路の中で、到達時刻算出手段により算出された到達時刻又は上記回避用加速度算出手段により算出された進入回避用の加速度が最小となる移動経路を選択する移動経路選択手段を設けた
ことを特徴とする請求項２記載の目標追尾装置。
【請求項１０】
位置速度算出手段が追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出処理ステップと、指向用加速度算出手段が上記位置速度算出処理ステップで算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出処理ステップと、経由点指向経路特定手段が上記位置速度算出処理ステップで算出された位置及び速度と上記指向用加速度算出処理ステップで算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定処理ステップと、判定手段が上記経由点指向経路特定処理ステップで特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定処理ステップと、回避用加速度算出手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出処理ステップと、回避経路特定手段が上記回避用加速度算出処理ステップで算出された進入回避用の加速度と上記位置速度算出処理ステップで算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定処理ステップと、移動経路予測手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定処理ステップで特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定処理ステップで経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定処理ステップで特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測処理ステップとを備えた目標追尾方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【公開番号】特開２０１２−１４９９９７（Ｐ２０１２−１４９９９７Ａ）
【公開日】平成２４年８月９日（２０１２．８．９）
【国際特許分類】
【出願番号】特願２０１１−８７４０（Ｐ２０１１−８７４０）
【出願日】平成２３年１月１９日（２０１１．１．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年８月９日（２０１２．８．９）
【国際特許分類】
【出願日】平成２３年１月１９日（２０１１．１．１９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]