JP7219609B2

JP7219609B2 - 最適経路生成システム

Info

Publication number: JP7219609B2
Application number: JP2018244024A
Authority: JP
Inventors: 直亮板橋; 充河野; 智貴椛山
Original assignee: Subaru Corp
Current assignee: Subaru Corp
Priority date: 2018-12-27
Filing date: 2018-12-27
Publication date: 2023-02-08
Anticipated expiration: 2038-12-27
Also published as: JP2020107013A; US20200211404A1; US11631337B2

Description

本発明は、最適経路生成システムに係り、特に目標に対して所定の状態になるまでの最適な経路を生成するための最適経路生成システムに関する。

例えば、航空機で編隊飛行を行う際に航空機（例えば僚機）を自動操縦で他機（例えばリーダ機）に追従させて飛行させる場合、他機の飛行経路を知る必要がある。
そして、この場合、他機から飛行経路の情報を取得することができる場合は他機から飛行経路の情報を取得し、他機から飛行経路の情報を取得することができない場合は他機の飛行経路を予測するように構成することができる（例えば特許文献１等参照）。

なお、特許文献１には、他機の飛行経路を予測する手法としてカルマンフィルタ理論等を用いる手法が記載されているが、予測手法はこれに限らず、例えば飛行力学的な手法や確率論的な手法等を用いることも可能である。
一方、このようにして飛行経路を認識した他機に追従するように自機を飛行させる方法としては、自機と他機の相対位置や相対速度、相対加速度等に基づいて自機を制御するように構成される場合が多い（例えば特許文献２、３等参照）。

特開２００１－１２４８４９号公報特表２００３－５２２９９０号公報特開平１１－１３９３９６号公報

ところで、編隊飛行中や空中給油中の場合など、自機がすでに他機に追従する状態になった場合は、上記のように自機と他機の相対位置等に基づいて制御することで、自機の位置や速度等を他機に対する所定の位置や速度等に保持することができる。

しかし、例えば、自機が他機に対して離れた位置を飛行しており、自機を現在の飛行状態から他機を追従する飛行状態にするように制御する場合、単に自機と他機の相対位置等に基づいて制御するだけでは他機や地面等と衝突してしまう可能性がある。
例えば、他機の右舷を飛行している自機を他機の左舷に移動させる場合、単に相対位置等に基づいて目標位置を他機の左舷に設定すると、左旋回した自機が他機に右側から衝突する等の事故が生じる可能性がある。

そのため、他機を追従する飛行状態になるように自機を制御するためには、自機が現在の飛行状態から他機を追従する飛行状態になるまでの経路として、他機や地面との衝突等を生じないような経路を設定する必要がある。
そして、これは、例えば車両や船舶等の経路を設定する場合も同様であり、他の乗用車や船舶等との衝突等を生じないような経路を設定することが必要である。また、他の航空機や車両、船舶等の移動する目標だけでなく、例えば空中や地上の所定の地点等の固定された目標に到達するための経路を設定する場合も同様である。

本発明は、上記の点を鑑みてなされたものであり、自機が目標に向かって移動する際に、他の物体との衝突等を生じることがなく、目標に対して所定の状態になるまでの最適な経路を生成することが可能な最適経路生成システムを提供することを目的とする。

前記の問題を解決するために、請求項１に記載の発明は、
目標に対して所定の状態になるまでの最適な経路を生成するための最適経路生成システムにおいて、
自機の現在の状態から所定の経過時間後に前記目標に対して所定の状態になるまでの経路の候補を、自機の現在の位置、速度、加速度のうちの加速度を変化させて複数生成する経路候補生成手段と、
生成された前記経路の候補のうち、所定の制限を逸脱せず、地面又は前記目標を含む他の物体との衝突がない前記経路の候補の中で、評価関数に基づく評価結果が最適になる前記経路の候補を選択して前記最適な経路として生成する最適経路生成手段と、
を備え、
前記最適経路生成手段は、生成された複数の前記経路の候補の各々と、その時点で最適と選択されている最適な経路の候補との対比に基づいて、当該最適な経路の候補を逐次更新する経路更新手段を含み、
前記経路更新手段は、一の経路の候補と前記最適な経路の候補とが、いずれも前記所定の制限を逸脱しないときであって、当該一の経路の候補における前記評価関数に基づく評価結果が前記最適な経路の候補のものよりも良い場合に、当該一の経路の候補を新たな最適な経路の候補として設定する、
ことを特徴とする。

請求項２に記載の発明は、請求項１に記載の最適経路生成システムにおいて、前記経路候補生成手段は、前記経路の候補を、前記加速度のほか、前記経過時間を変化させて複数生成することを特徴とする。

請求項３に記載の発明は、請求項１又は請求項２に記載の最適経路生成システムにおいて、前記経路候補生成手段は、自機の位置を時間ｔの５次関数で表した場合のｔ^３の項の係数ｐ３、ｔ^４の項の係数ｐ４、ｔ^５の項の係数ｐ５を、前記経過時間、自機の現在の状態における位置、速度、加速度及び前記目標に対して所定の状態になった場合の位置、速度、加速度に基づいて算出する計算処理を、３次元の各方向についてそれぞれ行うことで前記経路の候補を生成することを特徴とする。

請求項４に記載の発明は、請求項１から請求項３のいずれか一項に記載の最適経路生成システムにおいて、前記最適経路生成手段は、前記経路の候補の全区間において、前記自機の加速に要する仕事と抵抗損失との総量が前記自機のスラスト性能を超えない前記経路の候補を前記最適な経路の候補として選択することを特徴とする。

請求項５に記載の発明は、請求項１から請求項４のいずれか一項に記載の最適経路生成システムにおいて、前記最適経路生成手段は、前記評価関数に基づく評価結果として、前記経路の候補の全区間における、前記自機の加速に要する仕事と抵抗損失との総量を算出することを特徴とする。

請求項６に記載の発明は、請求項１から請求項５のいずれか一項に記載の最適経路生成システムにおいて、前記最適経路生成手段は、生成した前記経路の候補が前記所定の制限を逸脱する場合は、前記制限を逸脱している区間での前記位置又は前記速度を所定の位置又は速度に補正して当該経路の候補を生成し直すことを特徴とする。

請求項７に記載の発明は、請求項１から請求項６のいずれか一項に記載の最適経路生成システムにおいて、前記最適経路生成手段は、生成された前記経路の候補のいずれもが、当該経路候補に沿って自機を飛行させた場合に自機が地面又は前記目標を含む他の物体に衝突するものである場合は、衝突までの時間が最も長い前記経路の候補を選択することを特徴とする。

本発明によれば、自機と目標とがすでに近接している場合は勿論、自機が目標に対して離れた位置にいる場合でも、自機が目標に向かって移動する際に、他の物体との衝突等を生じることがなく、目標に対して所定の状態になるまでの最適な経路を生成することが可能となる。

本実施形態に係る最適経路生成システムの全体構成を表すブロック図である。自機Ａ、目標Ｂ、目標の経路Ｒ０、生成される複数の経路候補Ｒｎ等の一例を表すイメージ図である。複数個の加速度の変化量の設定のしかたの一例を表す図である。最適経路生成手段における処理の一例の手順を説明するためのフローチャートである。

以下、本発明に係る最適経路生成システムの実施の形態について、図面を参照して説明する。
なお、本発明に係る最適経路生成システムは、目標に対して所定の状態になるまでの最適な経路を生成するためのシステムであり、以下では、最適経路生成システムが航空機（例えば編隊飛行を行う際の僚機）に搭載されており、目標も航空機（例えば編隊飛行を行う際のリーダ機）である場合を想定して説明するが、最適経路生成システムは車両や船舶等に搭載されていてもよい。
また、最適経路生成システムは航空機や車両等に搭載されていなくてもよく、例えば、別の場所等に配置された最適経路生成システムが生成した経路を無線等で航空機等に送信するように構成することも可能である。

また、以下では、最適経路生成システムが最適な経路を生成する対象を自機というが（すなわち最適経路生成システムは自機の最適な経路を生成するが）、上記のように自機は車両や船舶等であってもよく、自機という語にはそれらの場合も含まれる。
また、目標も、航空機である場合に限定されず、車両や船舶、人等であってもよい。さらに、目標は、航空機や車両等のように移動する目標（以下、移動目標という。）に限定されず、例えば空中や地上の所定の地点等の固定された目標（以下、固定目標という。）であってもよい。

［全体構成］
図１は、本実施形態に係る最適経路生成システムの全体構成を表すブロック図である。
本実施形態では、最適経路生成システム１は、情報取得手段１０と、記憶手段２０と、経路候補生成手段３０と、最適経路生成手段４０とを備えている。

最適経路生成システム１は、例えばＣＰＵ（Central Processing Unit）やＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、入出力インターフェース等がバスに接続されたコンピュータ等で構成することが可能である。
そして、本実施形態では、最適経路生成システム１は、自機の最適経路の生成処理を行う時点で目標の経路をすでに把握しているものとする。

例えば、自機と目標との間で無線通信を行うことができる場合には、無線通信モジュール等の情報取得手段１０で目標から位置や速度、加速度の情報を取得することができる。
また、自機と目標との間で無線通信を行うことができない場合や無線通信を行わない場合には、例えばレーダやカメラ、センサ等の情報取得手段１０を用いて取得した情報から目標の位置や速度、加速度を算出することができる。なお、いずれの場合も、速度や加速度を位置や速度の変化率として算出することも可能である。

そして、最適経路生成システム１は、取得又は算出した目標の位置、速度、加速度（過去に取得又は算出した目標の位置等を含んでもよい。）に基づいて目標の経路Ｒ０を予測することで目標の経路Ｒ０を把握するように構成することが可能である。
一方、他の装置やシステムが予測した目標の経路Ｒ０を、情報取得手段１０（例えば通信ネットワークや無線通信手段等）を介して最適経路生成システム１が取得することで、最適経路生成システム１が目標の経路Ｒ０を把握するように構成することも可能である。

いずれの場合も、予測方法は任意の方法を採用することが可能である。そして、上記のようにして最適経路生成システム１が把握した目標の経路Ｒ０は、最適経路生成システム１の記憶手段２０に保存される。
また、記憶手段２０には、この他、最適経路生成システム１に以下で説明する自機の最適経路の生成処理を行わせるためのプログラム等や各種のデータやパラメータ等も保存されている。

なお、目標が固定目標である場合には、目標の経路Ｒ０は目標が固定されている位置ということになる（速度や加速度は０。）。
また、目標が固定目標である場合は、目標の経路Ｒ０（目標の固定位置）は変わらないため目標の経路Ｒ０を更新する必要はないが、目標が移動目標である場合は、目標の状態（位置、速度、加速度）が時々刻々変化するため、予測される目標の経路Ｒ０が絶えず変化する。そのため、最適経路生成システム１は、予測される目標の経路Ｒ０が変化するごとに記憶手段２０に保存している目標の経路Ｒ０を更新するとともに、目標の経路Ｒ０を更新した場合には、更新した目標の経路Ｒ０に基づいて自機の最適経路の生成処理を行うようになっている。

以下、最適経路生成システム１の経路候補生成手段３０や最適経路生成手段４０について説明する。
また、それとともに、本実施形態にかかる最適経路生成システム１の作用についてもあわせて説明する。

［経路候補生成手段］
経路候補生成手段３０は、自機の現在の状態から所定の経過時間後に目標に対して所定の状態になるまでの経路の候補Ｒ１、Ｒ２、…（以下、経路候補Ｒｎという。）を、自機の現在の位置ｘｓ、速度ｖｓ、加速度ａｓのうちの加速度ａｓを変化させて複数生成するようになっている。なお、位置ｘ、速度ｖ、加速度ａは、以下、特に断りのない限りそれぞれ３次元のベクトルを表す。
また、本実施形態では、経路候補生成手段３０は、経路候補Ｒｎを、自機の加速度ａｓのほか、上記の経過時間Ｔ（すなわち自機の現在の状態から目標に対して所定の状態になるまでに要する時間Ｔ）を変化させて複数生成するようになっている。

すなわち、本実施形態では、例えば図２のイメージ図に示すように自機Ａが目標Ｂの左側を飛行する状態から目標Ｂの右側を横一線に並んで飛行する状態になるまでの最適経路を生成する場合、経路候補生成手段３０は、自機Ａの現在の状態（Ｔ＝０）から所定の経過時間Ｔ（Ｔ＝Ｔ１、Ｔ２、Ｔ３、…）後に目標Ｂに対して所定の状態（この場合は目標Ｂの横一線に並んで飛行する状態）になるまでの経路候補Ｒｎ（Ｒ１、Ｒ２、Ｒ３、…）を、自機Ａの現在の加速度ａｓや経過時間Ｔを種々変化させて複数生成するようになっている。

ここで、目標に対する所定の状態とは、例えば、編隊飛行の場合には、図２に示したように、所定の経過時間Ｔ後に自機Ａが目標Ｂ（リーダ機）と同じ速度で、目標Ｂから所定の方向と距離だけオフセットした位置を飛行する状態をいう。
また、例えば、巡視船が不審船を追跡する場合には、所定の経過時間Ｔ後に自機Ａ（巡視船）が目標Ｂ（不審船）に追いつく状態とすることができる。また、例えば、車両で固定目標に行く場合には、所定の経過時間Ｔ後に自機Ａ（車両）が目標Ｂ（固定目標）に到達する状態とすることができる。
以下、具体的に説明する。

［初期条件の設定］
経路候補生成手段３０は、初期条件として、自機Ａが現在追従している経路における位置指令、速度指令及び加速度指令に基づいて自機Ａの現在の位置ｘｓ、速度ｖｓ、加速度ａｓを設定する。
続いて、経路候補生成手段３０は、自機Ａの現在の位置ｘｓ、速度ｖｓ、加速度ａｓのうちの加速度ａｓを複数とおりに変化させるために、例えば図３に示すように、自機Ａの速度ｖｓから割り出される自機Ａの現在の進行方向に直交する方向に複数個の加速度ａの変化量Δａを設定する（なお、Δａ＝０の場合を含んでもよい。以下同じ。）。設定した加速度ａの各変化量Δａをそれぞれ自機Ａの現在の加速度ａｓに加算することで、複数個の加速度ａｓを設定する。

なお、自機Ａの現在の位置ｘｓと速度ｖｓについては、上記の位置指令や速度指令から変化させない。
経路候補生成手段３０は、上記のようにして自機Ａの現在の位置ｘｓや速度ｖｓ、複数個の加速度ａｓを設定すると、それらを記憶手段２０に一時的に保存させる。なお、以下、加速度ａｓは変化量Δａが加算された後の加速度ａｓを表す。

［終端条件（経過時間）の設定］
続いて、経路候補生成手段３０は、終端条件として、複数の経過時間Ｔ＝Ｔ１、Ｔ２、…（図２参照）を設定する。
この場合、複数の経過時間Ｔとして、複数の経過時間Ｔを予め任意に設定しておき、記憶手段２０に保存しておくように構成することも可能である。

また、例えば、自機Ａと目標Ｂの現在の各位置からヘッドオンの状態（すなわち向かい合った状態）で互いに最大速度で飛行した場合に互いに衝突するまでの時間を最短時間とし、自機Ａが目標Ｂの速度よりやや速い速度（例えば目標Ｂの速度の１．１倍）でゆっくり飛行して自機Ａが目標Ｂに対して所定の状態になるまでの時間を最長時間として、最短時間と最長時間との間を等分（例えば１０等分）するなどして複数の経過時間Ｔを設定することも可能である。
この場合は、経路候補生成手段３０は、設定した複数の経過時間Ｔを記憶手段２０に一時的に保存させる。

［終端条件（位置、速度、加速度）の設定］
続いて、経路候補生成手段３０は、終端条件として、自機Ａが、上記のようにして設定した経過時間Ｔ後に前述した目標Ｂに対して所定の状態になった場合の自機Ａの位置ｘｅ、速度ｖｅ、加速度ａｅを設定する。
終端条件としての自機Ａの位置ｘｅ、速度ｖｅ、加速度ａｅは、目標Ｂが移動目標である場合には上記の経過時間Ｔごとに異なる。

具合的には、経路候補生成手段３０は、予測された目標Ｂの経路Ｒ０（例えば図２参照）に基づいて、経過時間Ｔが経過した時点での目標Ｂの位置ｘ０(T)、速度ｖ０(T)、加速度ａ０(T)を割り出す。
そして、編隊飛行を行う場合は、終端条件としての自機Ａ（すなわち目標Ｂに対して所定の状態になった自機Ａ）の位置ｘｅを、目標Ｂの位置ｘ０(T)から所定の方向と距離だけオフセットした位置に設定する。また、この場合、終端条件としての自機Ａの速度ｖｅや加速度ａｅについては、目標Ｂの速度ｖ０(T)や加速度ａ０(T)と同じ値を設定する。

経路候補生成手段３０は、この処理を、複数の経過時間Ｔについてそれぞれ行う。
そして、経過時間Ｔごとに、経過時間Ｔと、当該経過時間Ｔに対応する終端条件としての自機Ａの位置ｘｅ、速度ｖｅ、加速度ａｅとをそれぞれ対応付けて記憶手段２０に一時的に記憶させる。

［経路候補の生成］
続いて、経路候補生成手段３０は、加速度ａｓや経過時間Ｔを変化させながら、自機Ａの現在の状態（位置ｘｓ、速度ｖｓ、加速度ａｓ）から所定の経過時間Ｔ後に目標Ｂに対して所定の状態になるまでの経路候補Ｒｎ（Ｒ１、Ｒ２、…）を複数生成させる。
すなわち、経路候補生成手段３０は、上記の複数の経過時間Ｔ＝Ｔ１、Ｔ２、…の中から１つの経過時間Ｔ（例えばＴ１）を抽出し、抽出した経過時間Ｔの下で、初期条件の設定の際に設定した複数個の加速度ａｓについてそれぞれ経路候補Ｒｎを生成させる。

そして、経路候補生成手段３０は、この処理を、各経過時間Ｔについてそれぞれ行うことで複数の経路候補Ｒｎを生成させるようになっている。
そのため、例えば、前述した終端条件の設定で経過時間Ｔとして１０個の経過時間Ｔが設定され、前述した初期条件の設定で自機Ａの現在の加速度ａｓとして９個の加速度ａｓが設定されている場合は、経路候補Ｒｎは９０個生成される。

ここで、経路候補生成手段３０における経路候補Ｒｎの生成について具体的に説明する前に、経路候補Ｒｎの生成（算出）方法の一例について説明する。
なお、この経路候補Ｒｎの生成方法の一例の説明における位置ｘ、速度ｖ、加速度ａは、３次元のベクトルを表すものではなく、３次元ベクトルのうちのｘ成分を表すものである。位置、速度、加速度（３次元ベクトル）のｙ成分、ｚ成分については説明を省略するが、以下のｘ成分と同様にして計算される。

例えば、自機Ａの位置ｘを時間ｔの５次関数で表すとすると、自機Ａの位置ｘは、自機Ａの現在の位置ｘｓ、速度ｖｓ、加速度ａｓを用いて下記の（１）式のように表すことができる。

ここで、ｐ３、ｐ４、ｐ５は、自機Ａの位置ｘを時間ｔの５次関数で表した場合のｔ^３の項、ｔ^４の項、ｔ^５の項の各係数を表す。

また、速度ｖと加速度ａは位置ｘの１階微分、２階微分で算出することができるため、自機Ａの速度ｖ、加速度ａは、上記の（１）式を１階微分、２階微分して下記の（２）式、（３）式のように表すことができる。

そして、複数の経過時間Ｔの中から１つの経過時間Ｔを抽出した場合、その経過時間Ｔを上記の（１）～（３）式の時間ｔに代入すると、算出される位置ｘ、速度ｖ、加速度ａは、上記の［終端条件（位置、速度、加速度）の設定］で当該経過時間Ｔについて設定された位置ｘｅ、速度ｖｅ、加速度ａｅになるはずであるから、それらを上記の（１）～（３）式に代入して下記の（４）～（６）式が得られる。

そして、これらを変形して行列式の形にすると、下記の（７）式が得られる。

そのため、設定された経過時間Ｔや初期条件として設定された自機Ａの現在の位置ｘｓ、速度ｖｓ、加速度ａｓ、終端条件として設定された自機Ａの位置ｘｅ、速度ｖｅ、加速度ａｅを上記の（７）式に代入して計算することで各係数ｐ３、ｐ４、ｐ５を算出することができる。

そして、算出した各係数ｐ３、ｐ４、ｐ５を上記の（１）～（３）式にそれぞれ代入することで１つの経路候補Ｒｎの位置ｘ、速度ｖ、加速度ａ（３次元ベクトル）のｘ成分が算出される。
そして、この計算を位置ｘ、速度ｖ、加速度ａ（３次元ベクトル）のｙ成分やｚ成分についても同様に行うことで、１つの経路候補Ｒｎを算出して生成することができる。

このように、経路候補生成手段３０は、自機Ａの位置ｘを時間ｔの５次関数で表した場合のｔ^３の項の係数ｐ３、ｔ^４の項の係数ｐ４、ｔ^５の項の係数ｐ５を、経過時間Ｔと、自機Ａの現在の状態における位置ｘｓ、速度ｖｓ、加速度ａｓと、目標Ｂに対して所定の状態になった場合の位置ｘｅ、速度ｖｅ、加速度ａｅに基づいて算出して、自機Ａの位置ｘを時間ｔの５次関数で表す計算処理を、３次元の各方向（ｘ方向、ｙ方向、ｚ方向）についてそれぞれ行うことで経路候補Ｒｎを生成するように構成することが可能である。

なお、経路候補Ｒｎの生成方法は上記の方法に限定されず、他の生成方法を採用することも可能である。
そして、経路候補生成手段３０は、上記のようにして複数の経路候補Ｒｎを生成すると、生成した各経路候補Ｒｎを記憶手段２０に一時的に保存させるようになっている（図１参照）。

［経路候補生成手段］
最適経路生成手段４０（図１参照）は、上記のようにして経路候補生成手段３０により生成された経路候補Ｒｎのうち、所定の制限を逸脱せず、地面や目標Ｂを含む他の物体との衝突がない経路候補Ｒｎの中で、評価関数に基づく評価結果が最適になる経路候補Ｒｎを選択して最適な経路として生成するようになっている。

［制限と経路補正］
ここで、上記の「制限」について説明する。
上記のようにして経路候補生成手段３０で生成された経路候補Ｒｎの中には、経路候補Ｒｎに沿って自機Ａを飛行させた場合に、経路候補Ｒｎ上のある区間で、例えば自機Ａの速度が自機Ａの最高速度を超過したり、パイロットに異常に大きなＧがかかったり、あるいは地面や目標Ｂ（あるいは他の物体）と衝突したりするような経路候補Ｒｎが存在し得る。

しかし、そのような経路は好ましくないため、本実施形態では、経路候補Ｒｎに対して、例えば以下のような制限が課されている。
（１）速度が最高速度以下であること。
（２）パイロットにかかるＧが５Ｇ以下であること。
（３）迎角が３０度以下であること。
（４）高度が２００ｍ以上であること。
（５）他機との離隔距離が５０ｍ以上であること。
（６）危険エリアに進入しないこと。

なお、危険エリアとは、例えば、飛行禁止空域やミサイル発射基地の周辺、あるいは乱気流が発生している場所等を表す。
また、制限は、状況に応じて適宜設定されることは言うまでもない。例えば、曲技飛行（アクロバット飛行）を行うような場合には、（５）の他機（例えばリーダ機）との離隔距離の制限をより短いものとしたり、あるいは離隔距離に関する制限をなくしたりすることが可能である。

そして、最適経路生成手段４０では、後述するように、生成された経路候補Ｒｎが上記のような所定の制限を逸脱する場合は、制限を逸脱している区間での位置ｘや速度ｖを所定の位置ｘや速度ｖに補正して当該経路候補Ｒｎを生成し直すようになっている。
すなわち、本実施形態では、最適経路生成手段４０は、生成された経路候補Ｒｎにおいて例えば高度が２００ｍ未満になる区間がある場合は、その区間で高度が２００ｍになるように経路候補Ｒｎを補正するようになっている。また、生成した経路候補Ｒｎにおいて例えば速度が最高速度を超える区間がある場合は、その区間で速度が最高速度（あるいは最高速度より若干遅い速度）になるように経路候補Ｒｎを補正するようになっている。

［スラスト性能について］
また、生成された経路候補Ｒｎに沿って自機Ａを飛行させた場合に、自機Ａの加速に要する仕事と空気による抵抗損失との総量が自機Ａのスラスト性能を超えてしまうと、自機Ａは経路候補Ｒｎに沿った飛行を行うことができない。
そのため、本実施形態では、最適経路生成手段４０は、経路候補Ｒｎを選択する場合の前提として、生成された経路候補Ｒｎの全区間において（すなわち時間ｔが０から設定された経過時間Ｔが経過するまでの間に）、自機Ａの加速に要する仕事と抵抗損失との総量が自機Ａのスラスト性能を超えない経路候補Ｒｎを最適な経路Ｒbest（以下、最適経路Ｒbestという。）の候補として選択するようになっている。

なお、本実施形態では、この自機Ａのスラスト性能に関する評価として、自機Ａの加速に要する仕事と抵抗損失との総量と、自機Ａのスラスト性能との差を考慮した評価値Ｑ１が用いられるようになっている。
また、本実施形態では、上記の評価値Ｑ１については、理論上は０以下であることが望ましいが、プログラム上の数値誤差に対する保証として、０以下とする代わりに１０^－５以下であるか否かを判断基準として用いている（後述する図４参照）。

［評価関数に基づく評価結果］
また、最適経路生成手段４０は、上記のように、評価関数に基づく評価結果が最適になる経路候補Ｒｎを最適経路Ｒbestとして選択するようになっている。
その際、評価関数は、最適経路Ｒbestを選択する際に最も重視される観点に基づいて設定される。

例えば、経路候補Ｒｎに沿って飛行した際のエネルギー損失を少なくするという観点で最適経路Ｒbestを選択する場合には、最適経路生成手段４０は、例えば自機Ａの加速に要する仕事と抵抗損失との総量を計算する評価関数で算出される評価結果として評価値Ｑ２を算出するように構成することが可能である。

なお、以下では、このようにエネルギー損失を最小にする場合について説明するが、例えば、経路候補Ｒｎに沿って飛行した際のエネルギー損失を少なくするという観点で最適経路Ｒbestを選択する場合には、前述した経過時間Ｔを評価関数や評価値Ｑ２として用いるように構成することが可能である。
この場合は、例えば、上記の経過時間Ｔの間隔を小さくするようにして複数の経過時間Ｔを設定し、生成された経路候補Ｒｎのうち、上記の制限を逸脱せず、地面や目標Ｂ等の他の物体との衝突がない経路候補Ｒｎの中で、経過時間Ｔが最も短くなる（評価結果が最適になる）経路候補Ｒｎを選択して最適経路Ｒbestとして生成するように構成される。
また、この他にも、最適経路Ｒbestを選択する際に最も重視される観点に基づいて評価関数を適切に設定することで、その観点について最適経路Ｒbestを生成することが可能となる。

［最適経路生成処理の例］
以下、最適経路生成手段４０における処理の例について、図４のフローチャートに基づいて説明する。
本実施形態では、最適経路生成手段４０は、複数の経路候補Ｒｎの中から経路候補Ｒｎを順次抽出していき（ステップＳ１）、より適切な経路候補Ｒｎが現れるごとにメモリに記憶した最適経路候補を当該経路候補Ｒｎに更新していく（ステップＳ５）。そして、このようにして選択した最適な経路候補Ｒｎ（すなわち最終的にメモリに残っている最適経路候補）を最適経路Ｒbestとして生成するようになっている（ステップＳ１９）。
以下、具体的に説明する。

最適経路生成手段４０は、上記のようにして経路候補生成手段３０が生成した複数の経路候補Ｒｎの中から１つの経路候補Ｒｎを抽出する（ステップＳ１）。
続いて、最適経路生成手段４０は、ステップＳ２、Ｓ３の判断処理を行う。なお、ステップＳ２、Ｓ３の判断処理では、抽出した経路候補Ｒｎやメモリに保存されている最適経路候補が上記の制限を逸脱しておらず、全ての条件をクリアしている場合を「生成に成功」を表現し、経路候補Ｒｎや最適経路候補が上記の制限を逸脱しており、クリアしていない条件が１つ以上ある場合を「生成に失敗」と表現している。

最適経路生成手段４０は、抽出した経路候補Ｒｎは生成に失敗しているが、メモリに保存されている最適経路候補は生成に成功している場合には（ステップＳ２：Ｙｅｓ）、生成に失敗している経路候補Ｒｎ（すなわち上記の制限を逸脱している経路候補Ｒｎ）よりも生成に成功している最適経路候補（すなわち上記の制限を逸脱していない最適経路候補）の方を優先して選択すべきであるため、抽出した経路候補Ｒｎについては更なる処理を行わずに処理を終了するようになっている。
そのため、この場合は、最適経路候補の更新（ステップＳ５）は行われない。

一方、最適経路生成手段４０は、ステップＳ２の判断がＮｏの場合、続いて、抽出した経路候補Ｒｎもメモリに保存されている最適経路候補も生成に失敗しているか否かを判断する（ステップＳ３）。
なお、ステップＳ３の判断がＹｅｓの場合の処理については後で説明する。

ステップＳ３の判断がＮｏの場合、この時点で、経路候補Ｒｎと最適経路候補がともに生成に成功しているか、又は経路候補Ｒｎは生成に成功し、最適経路候補は生成に失敗しているかのいずれかになる。
そして、経路候補Ｒｎは生成に成功し、最適経路候補は生成に失敗しているのであれば、上記と同じ理由で、生成に成功している経路候補Ｒｎ（すなわち上記の制限を逸脱していない経路候補Ｒｎ）の方を優先して選択すべきである。

そのため、最適経路生成手段４０は、経路候補Ｒｎは生成に成功し、最適経路候補は生成に失敗している場合には（ステップＳ４：Ｙｅｓ）、メモリに記憶されている最適経路候補を、今回抽出した経路候補Ｒｎに更新する（ステップＳ５）。
本実施形態では、このようにして、複数の経路候補Ｒｎの中から上記の制限を逸脱せずに生成に成功した経路候補Ｒｎを選び出し、その中から最適経路Ｒbestを選び出すようになっている。

また、ステップＳ３の判断がＮｏの場合、この時点で、経路候補Ｒｎと最適経路候補はともに生成に成功していることになる。
最適経路生成手段４０は、続いて、抽出した経路候補Ｒｎにおける評価値Ｑ１（前述した自機Ａのスラスト性能に関する評価値Ｑ１）が１０^－５以下であるか否かを判断する（ステップＳ６）。なお、前述したように、スラスト性能に関する評価値Ｑ１は、０以下とする代わりに１０^－５以下とされている。

最適経路生成手段４０は、抽出した経路候補Ｒｎにおける評価値Ｑ１が１０^－５以下であれば（ステップＳ６：Ｙｅｓ）、抽出した経路候補Ｒｎにおける評価値Ｑ２（前述した評価関数に基づく評価結果としての評価値Ｑ２）がメモリに記憶されている最適経路候補における評価値Ｑ２より良い（この場合は評価値Ｑ２が小さい）か否かを判断する（ステップＳ７）。
そして、最適経路生成手段４０は、抽出した経路候補Ｒｎの方が最適経路候補よりも評価値Ｑ２が良い場合は（ステップＳ７：Ｙｅｓ）、メモリに記憶されている最適経路候補を今回抽出した経路候補Ｒｎに更新するが（ステップＳ５）、抽出した経路候補Ｒｎの方が最適経路候補よりも評価値Ｑ２が悪い場合は（ステップＳ７：Ｎｏ）、最適経路候補を更新しない。

本実施形態では、このようにして、経路候補Ｒｎと最適経路候補のいずれもがスラスト性能に関する評価値Ｑ１が１０^－５以下である場合は、評価関数に基づく評価結果がより良い方を最適経路候補として残すようになっている。
そして、最適経路生成手段４０は、全ての経路候補Ｒｎについて処理が終了していなければ（ステップＳ８；Ｎｏ）、ステップＳ１の処理に戻って次の経路候補Ｒｎを抽出する。

一方、ステップＳ６の判断処理で、今回抽出した経路候補Ｒｎの評価値Ｑ１が１０^－５より大きい場合には（ステップＳ６：Ｎｏ）、最適経路生成手段４０は、メモリに記憶されている最適経路候補の評価値Ｑ１が１０^－５以下であるか否かを判断する（ステップＳ９）。
そして、最適経路候補の評価値Ｑ１が１０^－５以下であれば（ステップＳ９：Ｙｅｓ）、最適経路候補を評価値Ｑ１が１０^－５より大きい経路候補Ｒｎに更新する必要はないため、最適経路生成手段４０は、最適経路候補を更新しない。

また、今回抽出した経路候補Ｒｎの評価値Ｑ１が１０^－５より大きいが（ステップＳ６：Ｎｏ）、最適経路候補の評価値Ｑ１も１０^－５より大きい場合には（ステップＳ９：Ｎｏ）、最適経路生成手段４０は、経路候補Ｒｎの方が最適経路候補よりも評価値Ｑ１が良ければ（ステップＳ１０：Ｙｅｓ）メモリに記憶されている最適経路候補を今回抽出した経路候補Ｒｎに更新する（ステップＳ５）。また、最適経路候補の方が評価値Ｑ１が良ければ（ステップＳ１０：Ｎｏ）、最適経路候補を更新しない。

必ずしもリカバリーが可能である保証はないが、評価値Ｑ１が良いほど（すなわち自機Ａの加速に要する仕事と抵抗損失との総量が自機Ａのスラスト性能を超えた度合いがより小さいほど）リカバリーできる可能性が高くなることが考えられる。
そのため、本実施形態では、最適経路生成手段４０は、上記のように経路候補Ｒｎも最適経路候補もいずれも評価値Ｑ１が１０^－５より大きい場合には、評価値Ｑ１がより良い方を選択して最適経路候補として残すようになっている。

本実施形態では、以上のように、今回抽出した経路候補Ｒｎとメモリに記憶されている最適経路候補がともに生成に成功している場合には、上記のステップＳ６、Ｓ７の各処理あるいはステップＳ９、Ｓ１０の各処理が行われるようになっている。

次に、ステップＳ３の判断がＹｅｓの場合、すなわち今回抽出した経路候補Ｒｎもメモリに記憶されている最適経路候補もいずれも生成に失敗している場合について説明する。
なお、すでにメモリに生成に成功している最適経路候補が記憶されている場合には、上記のように、最適経路候補が生成に失敗している経路候補Ｒｎに更新されることはないため、ステップＳ３の判断は必ずＮｏになる。そのため、以下の各処理が行われることはない。

最適経路生成手段４０は、ステップＳ３の判断がＹｅｓの場合（すなわち今回抽出された経路候補Ｒｎが上記の制限を逸脱する場合）は、前述したように、経路候補Ｒｎのうち制限を逸脱している区間での位置ｘを補正（例えば高度が２００ｍ未満になる区間では高度を２００ｍに補正）したり速度ｖを補正（例えば速度ｖが最高速度を超える区間では速度ｖが最高速度（あるいは最高速度より若干遅い速度）になるように補正）して（ステップＳ１１）、当該経路候補Ｒｎを生成し直す。

また、このように経路補正した経路候補Ｒｎに沿って自機Ａを飛行させた場合に、自機Ａが地面や目標Ｂ（図２参照）等の他の物体に衝突しない場合には（ステップＳ１２：Ｙｅｓ）、最適経路生成手段４０は、前述したステップＳ６、Ｓ７、Ｓ９、Ｓ１０の各判断処理と同様にステップＳ１３～Ｓ１６の各判断処理を行い、必要に応じてメモリに記憶されている最適経路候補を今回抽出した経路候補Ｒｎに更新する（ステップＳ５）。
なお、前述したように、Ｓ６、Ｓ７、Ｓ９、Ｓ１０の各判断処理は、今回抽出した経路候補Ｒｎとメモリに記憶されている最適経路候補がともに生成に成功している場合に行われるものであるが、ステップＳ１３～Ｓ１６の各判断処理は、今回抽出した経路候補Ｒｎとメモリに記憶されている最適経路候補がともに生成に失敗している場合に行われるものである点で異なる。

一方、ステップＳ１２の判断処理で、経路補正した経路候補Ｒｎに沿って自機Ａを飛行させた場合に自機Ａが他の物体に衝突する場合には（ステップＳ１２：Ｎｏ）、最適経路生成手段４０は、メモリに記憶されている最適経路候補に沿って自機Ａを飛行させた場合に自機Ａが他の物体に衝突するか否かを判断する（ステップＳ１７）。
そして、最適経路候補に沿って自機Ａを飛行させた場合に自機Ａが他の物体に衝突しない場合は（ステップＳ１７：Ｎｏ）、他の物体と衝突する経路候補Ｒｎよりも他の物体と衝突しない最適経路候補の方を優先して選択すべきであるため、最適経路生成手段４０は、最適経路候補を更新しない。

また、最適経路生成手段４０は、最適経路候補に沿って自機Ａを飛行させても自機Ａが他の物体に衝突する場合は（ステップＳ１７：Ｙｅｓ）、今回抽出した経路候補Ｒｎに沿って自機Ａを飛行させた場合の方が、最適経路候補に沿って自機Ａを飛行させた場合よりも衝突までの時間が長い場合には（ステップＳ１８：Ｙｅｓ）、メモリに記憶されている最適経路候補を、今回抽出した経路候補Ｒｎに更新する（ステップＳ５）。
しかし、最適経路候補に沿って自機Ａを飛行させた場合の方が衝突までの時間が長い場合には（ステップＳ１８：Ｎｏ）、最適経路候補を更新しない。

すなわち、本実施形態では、最適経路生成手段４０は、今回抽出した経路候補Ｒｎと、メモリに記憶されている最適経路候補のいずれも、それらに沿って自機Ａを飛行させた場合に自機Ａが他の物体に衝突する場合は、衝突までの時間が長い方を最適経路候補として残すようになっている。
このように構成する理由は、以下のとおりである。

前述したように、本実施形態では、最適経路生成システム１は、予測される目標Ｂの経路Ｒ０（図１、図２参照）が更新されるごとに、更新した目標Ｂの経路Ｒ０に基づいて自機Ａの最適経路Ｒbestの生成処理を行うようになっている。
そのため、現在予測されている目標Ｂの経路Ｒ０と自機Ａの現在の位置ｘ、速度ｖ、加速度ａ等に基づくと、地面や目標Ｂ等の他の物体と衝突するような最適経路Ｒbestしか生成できないような状況であっても、次の最適経路Ｒbestの生成処理の際には、予測される目標Ｂの経路Ｒ０も更新されており自機Ａの現在の位置ｘ、速度ｖ、加速度ａ等も変わっているため、他の物体と衝突しない最適経路Ｒbestが生成される可能性がある。

そのため、他の物体と衝突するような最適経路Ｒbestしか生成できないような状況の下では、可能な限り衝突までの時間が長い経路を最適経路候補として生成しておき、他の物体と衝突しない最適経路Ｒbestが生成される状況になるのを待つことが望ましい。
そのため、本実施形態では、上記のように、最適経路生成手段４０は、生成された経路候補Ｒｎのいずれもが、当該経路候補Ｒｎに沿って自機Ａを飛行させた場合に自機Ａが地面や目標Ｂを含む他の物体に衝突するものである場合は、衝突までの時間が最も長い経路候補Ｒｎを選択するようになっている。

最適経路生成手段４０は、以上の各処理を、経路候補生成手段３０が生成した各経路候補Ｒｎについて行う（ステップＳ８）。
そして、全ての経路候補Ｒｎについて処理を終了すると（ステップＳ８：Ｙｅｓ）、最終的にメモリに残っている最適経路候補を最適経路Ｒbestとして生成するようになっている（ステップＳ１９）。

［効果］
以上のように、本実施形態に係る最適経路生成システム１によれば、経路候補生成手段３０は、自機Ａの現在の状態から所定の経過時間Ｔ後に目標Ｂに対して所定の状態になるまでの経路候補Ｒｎを、自機Ａの現在の位置ｘｓ、速度ｖｓ、加速度ａｓのうちの加速度ａｓを変化させて複数生成する。また、最適経路生成手段４０は、生成された経路候補Ｒｎのうち、所定の制限を逸脱せず、地面や目標Ｂを含む他の物体との衝突がない経路候補Ｒｎの中で、評価関数に基づく評価結果（すなわち上記の例では評価値Ｑ２）が最適になる経路候補Ｒｎを選択して最適経路Ｒbestとして生成するように構成した。

そのため、自機Ａと目標Ｂとがすでに近接している場合は勿論、自機Ａが目標Ｂに対して離れた位置にいる場合でも、自機Ａが目標Ｂに向かって移動する際に、他の物体との衝突等を生じることがなく、目標Ｂに対して所定の状態になるまでの最適経路Ｒbestを生成することが可能となる。
そして、本実施形態に係る最適経路生成システム１では、最適経路Ｒbestを選択する際に最も重視される観点に基づいて評価関数を適切に設定することで、この観点に最も適した最適経路Ｒbestを自動的に生成することが可能となる。

なお、本発明が上記の実施形態等に限定されず、本発明の趣旨を逸脱しない限り、適宜変更可能であることは言うまでもない。

１最適経路生成システム
３０経路候補生成手段
４０最適経路生成手段
Ａ自機
ａ加速度
ａｅ目標に対して所定の状態になった場合の加速度
ａｓ自機の現在の加速度
Ｂ目標
ｐ３ｔ^３の項の係数
ｐ４ｔ^４の項の係数
ｐ５ｔ^５の項の係数
Ｑ２評価値（評価関数に基づく評価結果、自機の加速に要する仕事と抵抗損失との総量）
Ｒbest 最適経路（最適な経路）
Ｒｎ、Ｒ１、Ｒ２、… 経路候補（経路の候補）
Ｔ経過時間
ｔ時間
ｖ速度
ｖｅ目標に対して所定の状態になった場合の速度
ｖｓ自機の現在の速度
ｘ位置
ｘｅ目標に対して所定の状態になった場合の位置
ｘｓ自機の現在の位置

Claims

目標に対して所定の状態になるまでの最適な経路を生成するための最適経路生成システムにおいて、
自機の現在の状態から所定の経過時間後に前記目標に対して所定の状態になるまでの経路の候補を、自機の現在の位置、速度、加速度のうちの加速度を変化させて複数生成する経路候補生成手段と、
生成された前記経路の候補のうち、所定の制限を逸脱せず、地面又は前記目標を含む他の物体との衝突がない前記経路の候補の中で、評価関数に基づく評価結果が最適になる前記経路の候補を選択して前記最適な経路として生成する最適経路生成手段と、
を備え、
前記最適経路生成手段は、生成された複数の前記経路の候補の各々と、その時点で最適と選択されている最適な経路の候補との対比に基づいて、当該最適な経路の候補を逐次更新する経路更新手段を含み、
前記経路更新手段は、一の経路の候補と前記最適な経路の候補とが、いずれも前記所定の制限を逸脱しないときであって、当該一の経路の候補における前記評価関数に基づく評価結果が前記最適な経路の候補のものよりも良い場合に、当該一の経路の候補を新たな最適な経路の候補として設定する、
ことを特徴とする最適経路生成システム。
前記経路候補生成手段は、前記経路の候補を、前記加速度のほか、前記経過時間を変化させて複数生成することを特徴とする請求項１に記載の最適経路生成システム。
前記経路候補生成手段は、自機の位置を時間ｔの５次関数で表した場合のｔ^３の項の係数ｐ３、ｔ^４の項の係数ｐ４、ｔ^５の項の係数ｐ５を、前記経過時間、自機の現在の状態における位置、速度、加速度及び前記目標に対して所定の状態になった場合の位置、速度、加速度に基づいて算出する計算処理を、３次元の各方向についてそれぞれ行うことで前記経路の候補を生成することを特徴とする請求項１又は請求項２に記載の最適経路生成システム。
前記最適経路生成手段は、前記経路の候補の全区間において、前記自機の加速に要する仕事と抵抗損失との総量が前記自機のスラスト性能を超えない前記経路の候補を前記最適な経路の候補として選択することを特徴とする請求項１から請求項３のいずれか一項に記載の最適経路生成システム。
前記最適経路生成手段は、前記評価関数に基づく評価結果として、前記経路の候補の全区間における、前記自機の加速に要する仕事と抵抗損失との総量を算出することを特徴とする請求項１から請求項４のいずれか一項に記載の最適経路生成システム。
前記最適経路生成手段は、生成した前記経路の候補が前記所定の制限を逸脱する場合は、前記制限を逸脱している区間での前記位置又は前記速度を所定の位置又は速度に補正して当該経路の候補を生成し直すことを特徴とする請求項１から請求項５のいずれか一項に記載の最適経路生成システム。
前記最適経路生成手段は、生成された前記経路の候補のいずれもが、当該経路候補に沿って自機を飛行させた場合に自機が地面又は前記目標を含む他の物体に衝突するものである場合は、衝突までの時間が最も長い前記経路の候補を選択することを特徴とする請求項１から請求項６のいずれか一項に記載の最適経路生成システム。