JP2004326363A - Four-dimensional navigation object, four-dimensional navigation device, and four-dimensional control method of navigation object - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To optimize movement of a four-dimensional navigation object which performs a complicated motion in fluid. <P>SOLUTION: A searching unit 11 finds an optimized passing point to further optimize an evaluation function value under a restricting condition as an optimized solution in an area in the vicinity of a target point. As for midway points, a first midway point WP<SB>0</SB>and a second midway point WP<SB>1</SB>pointed on a side closer to a target point WP<SB>safe</SB>than the first midway point WP<SB>0</SB>. The search unit 11 finds an optimized passing point S1 to further optimize the evaluation function value as the optimized solution in a vicinity area of the second midway point WP<SB>1</SB>in the middle of moving of a navigation main body 101 from a current location Q to the first midway point WP<SB>0</SB>. The optimum solution is smoothly and flexibly found in the vicinity area. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

で表され、制約条件（ｔ∈［ｔ０，ｔ_ｆ］）は、下記式
【数２】

で表され、最適制御問題は評価関数Ｊを最小値化する問題に変換される。
【００４２】
逆システムが構成されれば、ｘ（ｔ）とｕ（ｔ）とは、出力ｙ（ｔ）で表現され得る。逆システムは、逆ダイナミクス解を計算により求める計算機である。逆ダイナミックスの解は、出発点と到達点と所要時間とから求められる運動状態である。運動状態は、通過点集合の飛行軌跡、飛行軌跡上の任意の通過点位置の加速度である。任意通過点の速度は、加速度の積分により計算され得る。この場合には、出力ｙ（ｔ∈［ｔ０，ｔ_ｆ］）は、逆システムにより、時間ｔに関係しない変数Ｘ∈Ｒ^ｋにより表現され得る。特には、出力ｙ（ｔ）の時刻は、通過点位置の運動状態特に通過点位置の速度ベクトルＸで表され得る。ここで、ｔ_０は出発時刻（時間としては０）を示し、ｔ_ｆは目的地到達時刻（出発時点から到達時点までの時間間隔）を示す。出力ｙ（ｔ）は、時間ｔに関係しない変数Ｘ∈Ｒ^ｋにより表現され得る。評価関数Ｊは、新たに、次式で表される。
【００４３】
【数３】

【００４４】
この場合に、制約条件は下記式：
【数４】

で表される。評価関数Ｊと制約条件は、評価シミュレータ７で設計的に設定される。このように、式（１）の最適制御問題は、式（４）で表される評価関数Ｊを最小化する変数Ｘを制約条件（５），（６）のもとで見出す非線形計画問題に変換される。変数Ｘは、非線形計画問題の中で、最適化変数といわれる。
【００４５】
４次元誘導、特に、流体内自由移動体（例示：航空機、宇宙往還機、移動衛星、潜水艇）の４次元誘導では、出力ｙ（ベクトル）として速度ベクトルＶ（絶対値、方向角、経路角）が選ばれ、拘束ｃ_ｙとして横滑り角βが選ばれる。速度ベクトルは、２つのＷａｙ−Ｐｏｉｎｔ（以下、ＷＰ：３次元通過点）の間の３次元補間により得られる経路を連続時間ｔにより微分することにより得られる。非線形計画問題の最適化変数Ｘは、飛行経路を決定するＷＰの４次元ベクトル（３次元位置ｘ，ｙ，ｚと時刻ｔ）として与えられ得る。非線形計画問題の評価関数は、下記のように設計され得る。
【００４６】
【数５】

時間ｔの積分形で表される評価関数Ｊは、３次元座標（ｘ，ｙ，ｚ）の関数として表される。ここで、右辺第４項の被積分関数のうちのＴｈｃは、そのときの速度で飛行しているエンジン推力に一致している。Ｃｔ、Ｃｆ、Ｃｅ、Ｃｕは係数であり、それぞれに、所要時間、燃料消費、トラッキングエラー、制御入力に関係している。燃料消費、トラッキングエラー、制御入力は、ｔ∈［０，ｔ_ｆ］の積分計算により得られ得る。これらの値は、Ｘに対して一意に決定され得る。評価関数Ｊの第１項は飛行時間ｔ_ｆの一次関数項で表され、その第２項は非線形関数項で表される。初期時刻ｔ（＝０）から到達時刻ｔ_ｆまで時間に関して積分される第２項には任意の時刻は現れず、評価関数は速度ベクトルで表され得る。最適化により、拘束条件のもとで最適速度が求められる。この場合には、速度は全飛行区間で一定値解として逆システムにより求められ、又は、複数飛行区間でそれぞれに一定値解として逆システム（飛行経路生成ユニット２）により求められ得る。
【００４７】
図２は、制約条件を示している。その表には、航空機に対応する制約条件として、等式制約条件と不等式制約条件とが示されている。不等式制約条件として、高度、速度、迎角、姿勢角、角速度、加重倍数が示されている。これらの値は、規定される最大値より低くなければならない。等式制約条件として、終端位置エラー、終端速度エラー、終端バンク角が示されている。これらの値は正確に定められるが、計算の都合のために必要である許容誤差が認められる。評価関数値の最適値解を導出する過程で発散が生じる場合には、探索は中止される。
【００４８】
発散が生じる場合には評価関数と制約条件を正当に評価することができない。発散の状態を評価することにより、ペナルティー関数Ｊｎｅｗが設計される。ｉ番目の制約条件の状態変数の値はｐ_ｉで表され、それの上限値と下限値とがｐ_ｉ ^ｍａｘ、ｐ_ｉ ^ｍｉｎで表される。下記の２つの式のいずれかは満足されている。
【数６】

【００４９】
等式制約は、そのままでは計算を困難にする。その困難を緩和するために、緩和係数ｖ_ｉが導入される。ｉ番目の等式制約条件の状態変数はｑ_ｉで表され、その制約の値はｑ_ｉ ^ｃで表され、次式が規定される。
【数７】

【００５０】
このような制約条件に対して、ペナルティー評価関数Ｊｎｅｗが設計される。
【数８】

ここで、Ｃｓ_ｉとＣｃ_ｊは、対応する制約条件に関するスケーリング係数を示し、正の値である。
【００５１】
このように設計された評価関数に対して最適値解が求められる。最適値解を求める探索手法として、既述のＲ−ＴＡＢＵ探索法が採用され得る。Ｒ−ＴＡＢＵ探索法では、図３に示されるように、最適化変数Ｘ（速度に対応する位置座標として示される）の近傍にサイズｈが同じである異なる領域群Ｈ（Ｘ，ｈ_１）が設定される。現在点Ｐと次期目標点Ｑとの間は、ベクトル（後述されるＷＰ_ｉ）で示されている。航空機は、点Ｑを通過するとは限られず、点Ｑの近傍を通過する。点Ｑを中心とする半径ｒの円が探索領域Ｃとして規定される。
【００５２】
探索領域Ｃは、簡易に２次元平面で表されている（又は球面の断面として表されている）。探索領域Ｃは、円Ｃの中に含まれランダムに選択される複数の小円Ｃ１ｊ（ｎ＝１，・・・，ｎ）に分割される。有限個の複数の小円Ｃ１ｊの集合は、円Ｃを覆い尽くす。全小円Ｃ１ｊに関して、評価関数Ｊが計算により求められる。有限個の複数の評価値のうち最小値（又は最大値）を持つ小円Ｃ１ｋが選択される。小円Ｃ１ｋは、有限個の複数の小円Ｃ２ｎに分割される。有限個の複数の小円Ｃ２ｊの集合は、円Ｃ１ｋを覆い尽くす。
【００５３】
全小円Ｃ１ｋに関して、評価関数Ｊが計算により求められる。有限個の複数の評価値のうち最小値を持つ小円Ｃ２ｓが選択される。小円Ｃ１ｎの半径は、小円Ｃの半径のｈ分の１である。小円Ｃ２ｎの半径は、小円Ｃ１ｎの半径のｈ分の１である。同様に、小円Ｃ”ｔｎ”の半径は、小円Ｃ_{（ｔ −１ ）ｎ}のｈ分の１である。適正回数の深度更新探索により、領域Ｈのうち評価値が最大である（関数Ｊが最小又は最大である）最適値化変数Ｘ（点の位置に対応）が深度回数ｔで試行錯誤的に求められる。このように、探索領域である近傍領域区間［ａ，ｂ］（今の場合には、［０，ｒ］，・・・，［ｒ_{ｔ −１}，ｒ_ｔ］が等比級数的に縮小されるＲ−ＴＡＢＵ手法により、次期最適通過点Ｑ’が求められる。その次期最適通過点Ｑ’は、既述の制約条件の全てを充足している。
【００５４】
このような探索区間の絞り込みは、下記のように表される。
【数９】

【００５５】
このような数学的規定のもとで、Ｒ−ＴＡＢＵ探索手法のアルゴリズムは、下記のステップにより構成される。
Ｒ−ＴＡＢＵ−Ｓ１：
適正な初期解Ｘ、ステップ縮小率Ｌ、ステップ数ｎｓ、カウント数ｎｃが定められる。
Ｒ−ＴＡＢＵ−Ｓ２：
初期解Ｘの近傍領域群Ｈ（Ｘ，ｈ）が定められる。：カウント用整数がｉ，ｊで表され、ｉ＝１，ｊ＝０。
Ｒ−ＴＡＢＵ−Ｓ３：
近傍領域Ｈ_ｉ（Ｘ，ｈ_ｉ）の中に、ランダムに解Ｙ_ｉが生成される。
【００５６】
Ｒ−ＴＡＢＵ−Ｓ４：
試行解Ｙ_ｉが初期解Ｘよりよい解であれば、そのＹ_ｉが初期解Ｘに代置される。
Ｒ−ＴＡＢＵ−Ｓ５：
ｊ＝ｊ＋１の代置が実行され、ｊ＜ｎｃであれば、次のステップはＲ−ＴＡＢＵ−Ｓ３に移行し、ｊ＜ｎｃでなければ、ｉ＝ｉ＋１の代置が実行され、ｊ＝０が設定され、次のステップはＲ−ＴＡＢＵ−Ｓ６に移行する。
【００５７】
Ｒ−ＴＡＢＵ−Ｓ６：
ｉ＜＝ｎｓであれば、次のステップはＲ−ＴＡＢＵ−Ｓ３に移行し、ｉ＜＝ｎｓでなければ、次のステップはＲ−ＴＡＢＵ−Ｓ７に移行する。
Ｒ−ＴＡＢＵ−Ｓ７：
終了判定がｙｅｓであれば、Ｘが最適解として抽出される。終了判定がｎｏであれば、ステップはＲ−ＴＡＢＵ−Ｓ２に移行する。
【００５８】
図４は、隣りあう２点の指示点が与えられた場合の最適飛行経路を示している。第（ｊ−１）指示通過点Ｐ_{ｊ −１}と第ｊ指示通過点Ｐ_ｊが通過時刻とともにパイロットにより初期的に最適解探索ユニット１１の運動経路設定器１３に設定されている場合に、航空機は第（ｊ−１）指示通過点Ｐ_{ｊ −１}と第ｊ指示通過点Ｐ_ｊとを結ぶ直線（１点鎖線）に沿って飛行することは特別な場合を除いてあり得ない。第（ｊ−１）指示通過点Ｐ_{ｊ −１}に向かう航空機は、現在位置に基づいて、第（ｊ−１）指示通過点Ｐ_{ｊ −１}の第（ｊ−１）近傍２１を通過する。
【００５９】
第１（ｊ−１）近傍２１を通過する航空機は、第１（ｊ−１）近傍２１の適正化点に基づいて、第ｊ指示通過点Ｐ_ｊの第ｊ近傍２２を通過する。最適化通過点は、最適解探索ユニット１１で最適化計算により求められる。第（ｊ−１）最適化通過点Ｐ’_{ｊ −１}は第（ｊ−１）近傍２１の中に見出され、第ｊ最適化通過点Ｐ’_ｊは第ｊ近傍２２の中に見出され、最適飛行経路２３は滑らかな実線で表されている。他の最適化候補の２点鎖線図示の飛行経路２４は、その最適化度の点では、最適飛行経路２３より劣っている（ことが評価シミュレータ７により判定される。）。
【００６０】
（ＩＩ）オフライン探索法
２点以上の多点が指示される多点通過経路に関して全体的に可能な限り一意的に最適解を導出する最適制御は、オフライン制御と呼ばれる。オフライン制御では、全経路の最適解が求められ、航空機はその最適解経路で飛行する。状態空間探索問題では、状態空間Ｓ、初期状態ｓ_０∈Ｓ、目標状態判定関数Ｐ：ｓ→｛Ｔｒｕｅ， Ｆａｌｓｅ｝、基本操作Ｏ_ｉ（ｓ_ｉ）＝ｓ_ｉ＋１が定義される。ＷＰに関する概念として、”ＷＰポインタ、と、”先端ＷＰ”とが、次のように定義される。
【００６１】
ＷＰポインタ：
状態空間探索問題では、あるＷＰがその前のＷＰからどのように移動するかを決定する。具体的には、１つのＷＰとそれの次のＷＰとの間の相対的位置と相対時間は、ベクトルにより指定される。そのようなベクトルは、ＷＰポインタと呼ばれる。ＷＰポインタを初期状態から他の状態まで足し加えることにより、ＷＰの位置が順次に指定される。図５に示されるように、ｉ番目のＷＰはベクトルＷＰＰｉで表され、ｉ−１番目のＷＰ_{ｉ −１}からｉ番目のＷＰ_ｉに向かうＷＰポインタはＷＰＰｉで表現される。
ＷＰＰ_ｉ＝ＷＰ_ｉ−ＷＰ_{ｉ −１}
【００６２】
先端ＷＰ：
終端のＷＰを含まないＷＰの系列の最先端のＷＰ（目的地のＷＰの１つ手前のＷＰ）は、先端ＷＰと呼ばれてＷＰＰ_ｆｆで表される。
【００６３】
集合と操作の定義：
ＷＰ_ｆｆに関して、集合と操作は次のように定義される。
・状態Ｓ：状態ｓは先端のＷＰであるＷＰ_ｆｆの近傍領域。Ｓはその集合。
・初期状態ｓ_０∈Ｓ：誘導開始時の位置（ＷＰ_０）
・目的状態判別Ｐ：→｛Ｔｒｕｅ， Ｆａｌｓｅ｝：最適化による全制約条件の充足が探索目的とされ、展開済みの近傍領域ｓの全てに対して探索が実行される。全ての制約が充足される解が見つかればＴｒｕｅ、そうでなければＦａｌｓｅ。
・基本操作Ｏ_ｉ（ｓ_ｉ）＝ｓ_ｉ＋１：適当なＷＰポインタはＷＰＰ_ｎｅｗで表される。先端ＷＰは、ＷＰ_ｆｆ←ＷＰ_ｆｆ＋ＷＰＰ_ｎｅｗのように更新される。このように更新されたＷＰＰ_ｆｆの近傍空間は近傍領域ｓ_ｉ＋１に設定される。
【００６４】
状態集合Ｓ：
状態である近傍領域ｓは、ある有限な空間に一致している。今考えられている近傍領域ｓは、先端のＷＰであるＷＰ_ｆｆの近傍領域である。ＷＰ_ｆｆは、１つ手前のＷＰであるＷＰ_{ｆｆ −１}にＷＰポインタＷＰＰ_ｆｆを加えることにより、次のように表現することができる。
ＷＰ_ｆｆ＝ＷＰ_{ｆｆ −１}＋ＷＰＰ_ｆｆ
【００６５】
ここで、ＷＰ_{ｆｆ −１}が固定されていれば、近傍領域ｓの範囲はＷＰ_{ｆｆ −１}が指し得る範囲のみにより決定することができる。よって、初期状態であるＷＰ_０からＷＰポインタを順に加えていくときに、ＷＰポインタが指し得る範囲を決めることにより、全ての近傍領域ｓを決定することができる。ＷＰポインタが指し得る範囲は、航空機の運動性能（例示：旋回半径、推力、機体構造、機内幼児の存在）によって定められ得る。機内に幼児が存在する場合には、加速度の上限が低く抑えられる。
【００６６】
展開可能判別：
基本操作の連鎖により、状態ｓは順次に生成される。このように基本操作により状態ｓを展開するときに、適切なＷＰポインタＷＰＰ_ｎｅｗ（図５のＷＰＰ_ｉ）が用いられている。飛行経路を指定する２つのＷＰの間は、実際には、スプライン関数で連続的に結ばれている。ＷＰを展開する場合には、ＷＰ_ｆｆと終端のＷＰの間の経路の上に、次のＷＰ_ｆｆが展開されるように、ＷＰＰ_ｎｅｗが決定される。先端のＷＰの近傍領域である状態ｓが展開可能であることは、次のように定義される。
【００６７】
・ｓが展開可能≡ＷＰ_ｆｆ←ＷＰ_ｆｆ＋ＷＰＰ_ｎｅｗとして設定される新たな近傍空間ｓ_ｉ＋１に、ＷＰ_０〜ＷＰ_ｆｆまでの飛行に関する制約条件を全て満たすようなＷＰ_ｆｆが含まれていること（終端条件が満たされ必要はない）。
【００６８】
連続空間の離散化：
連続した空間として形成されているＷＰの近傍領域であるｓは、状態空間探索問題の前提条件である”問題の解の候補が離散的であり、有効に数え上げる”ことができない。ここで、ＴＡＢＵ探索手法がＲ−ＴＡＢＵ探索手法に拡張されるように、特定される１つのＷＰの近傍領域ｓにサイズｈが互いに異なる複数の領域群Ｈ（ＷＰ，ｈ）が設定される。各領域の中でＷＰがランダムに発生し、そのＷＰで各領域を代表させることが有効である。各領域Ｈ_ｊ（ＷＰ，ｈ_ｊ）の代表ＷＰがＷＰ_Ｈｊで表され、近傍ｓがＷＰ_Ｈｊの集合として考えられれば、有限個のＷＰが存在するｓは有効に数えられることができる。
【００６９】
ＷＰポインタの最適化：
全ての状態ｓは、ＷＰポインタの系列ＷＰＰ_ｉ（ｉ＝１，・・・，ｆｆ）により決定され得る。目標状態判別とＷＰの展開可能判別は、ＷＰをランダムに生成することにより連続空間が離散化されて定義されるｓに対して、それぞれの判別の制約条件を充足することを目的とする最適化問題として定義され得る。
・目標状態判別と展開可能判別は、ランダムに生成されるＷＰポインタの系列ＷＰＰ_ｉ（ｉ＝１，・・・，ｆｆ）と終端状態に向かう時刻ｔ_ｆとが最適化問題の変数であり、それぞれの判別の制約条件を充足することを目的にする最適化問題に変換される。このように変換された最適化問題の解法のためには、Ｒ−ＴＡＢＵ手法が適用され得る。
【００７０】
評価関数の設定：
評価（値）関数として、展開可能判別と目標状態判別のためのｆ’_ｓ（ｎ）と、目標判別のためのｆ_ｆ（ｎ）との２種類が設計されて、評価シミュレータ７の関数設定部分（図示されず）に設定される。両者は、制約条件をペナルティー関数法で取り込む。展開可能判別と展開節点選択のための評価関数（展開可能判別評価関数といわれる）ｆ’_ｓ（ｎ）の設計手法は、下記のように表現される。
ｆ’_ｓ（ｎ）＝ｇ（ｎ）＋ｈ’（ｎ）＋Ｑ_ｓ（ｎ）
【００７１】
ここで、ｈ’（ｎ）は、ヒューリスティック関数であり、４次元運動データ４に対応してヒューリスティク関数生成ユニット１４で設定される。展開可能判別評価関数は、評価シミュレータ７に設定されるとともに、最適解探索ユニット１１に導入されて最適解探索ユニット１１に設定される。ヒューリスティック関数は、飛行経路生成ユニット２が出力する４次元運動データ４に対応する評価関数部分として最適化ユニット９で生成されて最適解探索ユニット１１に導入されるが、評価シミュレータ７に設定される必然性はない。
【００７２】
現在の節点ｎ（図５のｆｆに相当）が示す状態は、ｓである。状態ｓは、先端のＷＰであるＷＰ_ｆｆの近傍領域を示している。ここで、ＷＰ_ｆｆの時間要素ｔ_ｆｆは、最適化の対象には含まれず、予め指定されている（今の問題は、規定されている時刻ｔ_ｆに到達するまでの燃料消費量が最小化される問題）。ｇ（ｎ）は、初期節点から節点ｎまでのコスト（例示：燃料消費量）である。ｇ（ｎ）は、次のように構成される。
【数１０】

【００７３】
展開可能判別と展開節点の評価のための飛行シミュレーションでは、計算コストの軽減のために、ｔ_ｆｆまで積分が実行されている。目標到達点の１つ手前の節点（ｎ_ｆｆ）と目標到達点の間の飛行のためのコストが、予想的に表される。そのような予想的なコスト関数は、既述の通りヒューリスティック関数と呼ばれ、ヒューリスティック関数ｈ’（ｎ）の導入が必須的である。ヒューリスティック関数ｈ’（ｎ）は、”目標状態にどれだけ近いか”を評価するための関数として必須的に導入されている。ヒューリスティック関数ｈ’（ｎ）は、具体的には、次のように設計され得る。
【００７４】
【数１１】

【００７５】
ここで、Θ_ｗｅ、Ψ_ｗｅは、終端直前の目標飛行経路の経路角と方位角の差（単位はＤｅｇｒｅｅ）をそれぞれに示し、Ｃ_Θｗ、Ｃ_Ψｗはそのスケーリング係数を示す。ヒューリスティック関数ｈ’（ｎ）の第１項であるＣ_ｔ（ｔ_ｆ−ｔ_ｆｆ）は、最適解探索ユニット１１が内蔵するクロックのクロック基準の現時刻の評価対象ＷＰの先端点から目標点までにかかる予測コストの主要部分である。
【００７６】
次に、ペナルティー関数法の制約条件の評価値Ｑ_ｓ（ｎ）が示される。Ｑ_ｓ（ｎ）は、制約条件が足し加えられた関数であり、ＷＰ_０〜ＷＰ_ｆｆの制約条件を含んでいる。評価値Ｑ_ｓ（ｎ）は、次のように設計される。
【００７７】
【数１２】

【００７８】
次に、目標判別のための評価関数（目標判別評価関数）ｆ_ｆ（ｎ）の設計方法が示される。目標判別評価関数ｆ_ｆ（ｎ）は、次式で表される。
ｆ_ｆ（ｎ）＝ｇ（ｎ）＋ｈ（ｎ）＋Ｑ_ｆ（ｎ）
ここで、ｇ（ｎ）＋ｈ（ｎ）は、次式で示される。
【数１３】

【００７９】
目標判別評価関数ｆ_ｆ（ｎ）の制約条件の評価値Ｑ_ｆ（ｎ）は、全ての制約条件を考慮していて、ｆ_ｆ（ｎ）は次式で示される値になる。
【数１４】

【００８０】
目標点ＷＰ_ｆには、近傍領域は原則的に設定されない。目標点としては、着陸空港の４次元誘導停止点（完全自動航行の停止点）又は軌道衛星の４次元接合点が例示され、近傍領域の面積又は体積は実質的に零である。目標判別評価関数ｆ_ｆｎは、予定到達時刻まで時間に関して積分されている。目標判別評価関数ｆ_ｆｎは、最終的評価値を記述する。目標点（現実に最終的に指示されている最終目標点に限られず、厳密に規定される途中通過点を含む。例示：経路途中に含まれる中間着陸点）より１つ手前の通過点として規定される節点までを記述する経路の節点展開可能判別関数は、全ての制約条件が充足されていて、目標点の１つ手前の通過点近傍に確実に到達することができる。
【００８１】
状態空間探索による最適値問題の解法（アルゴリズムＡ^＊）：
ステップＳ１−Ａ^＊：
先端状態ＷＰ_ｆｆは、初期状態ＷＰ_０として設定される。
ＷＰ_ｆｆ←ＷＰ_０
整数ｉがｉ＝０に設定され、ＷＰ_ｆｆは、図５に示されるように、ＷＰ_０がウエイポイント１２として設定される。制御パラメータの探索範囲とＲ−ＴＡＢＵ探索手法（又は改良Ｒ−ＴＡＢＵ探索手法）の各種のパラメータが設定される。
ステップＳ２−Ａ^＊：
ｉ番目のＷＰであるＷＰ_ｉに関して、基本操作Ｏ_ｉが実行される。ｉ←ｉ＋１の更新が実行される。
【００８２】
ステップＳ３−Ａ^＊
ｉ番目のＷＰポインタであるＷＰＰ_ｉに関して、ＷＰＰ_ｉ←ＷＰＰ_ｎｅｗの更新が実行される。
ステップＳ４−Ａ^＊
ＷＰポインタＷＰＰ_ｉに関して、指すことができる範囲（探索範囲）ｓ_ｉが球座標で設定される。
【００８３】
ステップＳ５−Ａ^＊
ＷＰポインタの系列ＷＰＰ_ｊ（ｊ＝１，・・・，ｉ）の球座標成分と終端時間ｔ_ｆが最適化変数Ｘにより表されて最適化問題が構成され、最適化方法としてＲ−ＴＡＢＵ手法が適用される。評価関数としては、Ａ^＊アルゴリズムの評価関数：
ｆ’_ｓ（ｎ）＝ｇ（ｎ）＋ｈ’（ｎ）＋Ｑ_ｓ（ｎ）
が適用される。同時に、制御パラメータは、最適化変数Ｘに含められて最適化が実行される。ヒューリスティック関数ｈ’（ｎ）がヒューリスティク関数生成ユニット１４で生成されて、ヒューリスティック関数ｈ’（ｎ）を部分的に含む展開可能判別評価関数ｆ’_ｓ（ｎ）が最適解探索ユニット１１で記述される。
【００８４】
ステップＳ６−Ａ^＊：
最適化計算が、最適解探索ユニット１１で実行される。制約が充足されなければ、ステップはステップＳ１−Ａ^＊に移行し、制約が満たされれば、ステップはステップＳ７−Ａ^＊に移行する。
ステップＳ７−Ａ^＊：
更に、最適化計算が実行される。ある程度に最適化計算が続けられた時点で得られている解のうちで最良解が確保される（Ａ^＊アルゴリズムによる展開節点の選択に相当する。）。
【００８５】
ステップＳ８−Ａ^＊：
ステップＳ７−Ａ^＊の解が初期解として扱われ、既述の評価関数：
ｆ_ｆ（ｎ）＝ｇ（ｎ）＋ｈ（ｎ）＋Ｑ_ｆ（ｎ）
に対して最適化が実行される（目標状態判別）。式（１５）の展開可能判別評価関数ｆ’_ｓ（ｎ）が最適化により求められてｈ’_ｎが求められ、式（１７）により式（２０）の｛ｇ（ｎ）＋ｈ（ｎ）｝が知られ、式（１９）の目標状態判別評価関数ｆ_ｆ（ｎ）が求められる。
【００８６】
ステップＳ９−Ａ^＊：
規定の試行回数（又は時間）のうちに制約を充足する解が得られなければ、その状態が目標状態であると推定されて、それが解として出力される。そうでなければ、ステップはステップＳ２−Ａ^＊に移行する。
【００８７】
目標状態判別評価関数ｆ_ｆ（ｎ）の最小値探索によりそれの値として適正値が見出されれば、その適正値が最適解探索ユニット１１からウエイポイント生成ユニット１２に出力され、その最適化適正値に対応する次のＷＰポインタＷＰＰ_ｉ＋１がウエイポイント生成ユニット１２で生成される。その新たなＷＰポインタＷＰＰ_ｉ＋１に基づいて、次の区間の飛行経路が計算され、航空機にコマンドが与えられ、次周期の最適解探索が最適解探索ユニット１１で繰り返される。このような最適解探索の繰り返しループのアルゴリズムにより、ＷＰ_ｆｆの近傍に含まれる最適到達点が最適化され、最後に、目標状態判別評価関数ｆ_ｆ（ｎ）により航空機の最終目標到達点までの全飛行経路が最適化されて決定される。
【００８８】
既述のオフライン制御飛行の最適化は、航空機の離陸準備の段階で決定され、最適化飛行経路は１通りに決定され、リアルタイム制御の可能性は特には考慮されていない。
（３）オンライン探索法
計算機の計算速度が十分に速い場合は、現在点と目標点の間の最適飛行経路をリアルタイムに計算することができることは自明的である。気流の変動その他の予測不可能な要因が存在する場合には、計算速度が十分に速くない計算機を用いてリアルタイム制御であるオンライン飛行制御が重要である。
【００８９】
オンライン飛行制御に適用される最適化計算では、次の２つの事項が重要である。
・一定時間以内に実行可能解が生成されること：
最適化計算の収束性に影響を及ぼすいくつかのパラメータがうまく調整され、所望の性能が得られ得るようになされていること。
・安全性が確保されること：
一定時間以内に次の誘導則が生成され得なかったときには、誘導制御則は更新されずに、飛行が続行されるように、危険回避を含む誘導制御則の更新プログラムが組まれる。更には、実機搭載時に確実に一定時間以内に誘導則が生成され得るように、最適化計算に許される制限時間がマージンが含められて決定されること。
【００９０】
初期状態の設定：
現在（現在時刻）のＷＰであるＷＰ_０が、初期状態として図６に示されるように設定される。現在の飛行位置Ｐ_０に対して、初期状態ＷＰ_０はこれから飛行して進む方向にある。生成されるＷＰ_０は、未来の飛行経路にある。
【００９１】
目標状態判別と展開判別：
ＷＰ_０に１回の基本操作が実行されて、ウエイポイント生成ユニット１２でＷＰ_１が生成される。ＷＰ_１に１回の基本操作が実行されて、ウエイポイント生成ユニット１２でＷＰ_２が生成される。ＷＰ_１とＷＰ_２は、ウエイポイント生成ユニット１２で同時的に設定される。ＷＰ_２は、オフライン探索の先端点として設定される。オフライン探索が、ＷＰ_２に対して図１で示される閉ループ最適化探索が実行され、ＷＰ_２に対して、節点展開判別と目標状態判別が順番に実行される。ヒューリスティック関数は、ＷＰ_２から目的地ＷＰ_ｆまでのコストを推定する。両判別は、既述の最適化問題の解に対して実行される。
【００９２】
安全性の確保：
次のＷＰの展開のためには、２つ先のＷＰまでの評価値が用いられる。現在の探索の対象のＷＰ_２が展開可能に確保されて、現在の展開試行で指定される時間以内に節点が展開され得ない場合の飛行経路が確保される。ＷＰ_２まで可能である節点展開が確保され、目標状態判別は全節点展開判断が終わった後に開始される。現在時刻から近未来である短い区間に関して安全に且つその短い区間のうちで最低限的に最適である飛行経路の確保がリアルタイムに可能である飛行方法が、次に述べられるオンライン制御方法である。
【００９３】
初期状態の移動：
更新周期ｔ_ｒｅｆ以内でＷＰ_２が確保され目標点まで最適化の探索が最適解探索ユニット１１で実行されて、その目標判別がＦａｌｓｅであれば、航空機はＷＰ_１まで移動する。このような移動は、安全に最適的に実行され得る。そのＷＰ_１が、新たにＷＰ_０として設定される。ＷＰ_１に移動する場合には、生成される経路の途中にあるＷＰ_１の上で目標経路が切り替えられる。
【００９４】
オンライン飛行制御のための評価関数の設計：
現在地のＷＰはＷＰ_０に設定される。ＷＰ_０に対応する現在の節点は、ｎで表される。節点ｎに基本操作が１回ずつ２回が実行されて得られる節点は、手前から順に、ｎ_１，ｎ_２で表される。ｎ_１，ｎ_２に対応する状態ｓに含まれるＷＰは、それぞれにＷＰ_１，ＷＰ_２で表される。展開可能判別と展開節点選択の最適化計算の評価関数ｆ’_ｓ（ｎ，ｎ_１）は、次のように設計される。
ｆ’_ｓ（ｎ，ｎ_１）＝ｃ（ｎ，ｎ_２）＋ｈ’（ｎ_２）＋Ｑ_ｓ（ｎ，ｎ_２）
ここで、Ｑ_ｓ（ｎ，ｎ_２）には、ペナルティー関数法の制約条件が足し加えられた関数であり、ＷＰ_０〜ＷＰ_２の制約条件が含まれる。
【００９５】
このような評価関数では、次の節点を展開するために、２つ先の節点までの評価値が用いられている。このことにより、現在の節点ｎ_２を展開可能に確保しているので、次の節点展開試行で指定される時間の範囲内で節点が展開され得なかったときに、ＷＰ_２に到達する前に次期節点展開が即時的に可能である。
【００９６】
目標状態判別関数である命題：
”最適化による全制約条件の充足を探索目的として、展開済みの近傍領域ｓの全てに対して探索を実行し、全ての制約を充足する解が見つかればＴｒｕｅ、全ての制約を充足する解が見つからなければＦａｌｓｅ。”
に関して、既述のアルゴリズムと同じ最適化アルゴリズムを用いることができる。この場合には、評価関数は、次式により設計される。
ｆ_ｆ（ｎ，ｎ_１）＝ｃ（ｎ，ｎ_１）＋ｈ（ｎ_２）＋Ｑ_ｆ（ｎ）
ここで、Ｑ_ｆ（ｎ）は、ペナルティー関数法の制約条件を足し加えた関数であり、ＷＰ_０〜ＷＰ_Ｎｎｅｗの制約条件（終端条件を含む）を含んでいる（ＷＰ_Ｎｎｅｗは、目標地点のＷＰである。）。
【００９７】
本発明によるオンライン飛行制御のリアルタイムアルゴリズムＲＴＡ^＊は、下記のように表現される。
ステップＳ１−ＲＴＡ^＊：
初期状態は、ＷＰ_０で表される。制御パラメータの探索範囲とＲ−ＴＡＢＵ探索手法（又は改良Ｒ−ＴＡＢＵ探索手法）の各種のパラメータが設定される。
ステップＳ２−ＲＴＡ^＊：
ＷＰ_０からｔ_ｒｅｆ秒だけ現在の速度で直進飛行した位置が新たにＷＰ_０として設定される。新規設定前のＷＰ_０（図６の点Ｑ）から３ｔ_ｒｅｆだけ現在の速度で直進進行した位置（Ｐ_２）のＷＰのベクトルは、ＷＰ_ｓａｆｅで表される。ＷＰ_ｓａｆｅは、安全確保のためのＷＰである。
【００９８】
ステップＳ３−ＲＴＡ^＊：
ＷＰ_０に基本操作Ｏ_０が実行され、得られるＷＰはＷＰ_１で表される。
ステップＳ４−ＲＴＡ^＊：
１番目のＷＰポインタであるＷＰＰ_１に関して、ＷＰＰ_１←ＷＰＰ_ｎｅｗの更新が実行される。
ステップＳ５−ＲＴＡ^＊：
ＷＰポインタであるＷＰＰ_１に関して、指すことができる範囲（探索範囲）ｓ_１が球座標で設定される。
【００９９】
ステップＳ６−ＲＴＡ^＊：
ＷＰ_１に基本操作Ｏ_１が実行され、得られるＷＰはＷＰ_２で表される。
ステップＳ７−ＲＴＡ^＊：
２番目のＷＰポインタであるＷＰＰ_２に関して、ＷＰＰ_２←ＷＰＰ_ｎｅｗの更新が実行される。
ステップＳ８−ＲＴＡ^＊：
ＷＰポインタのであるＷＰＰ_２に関して、指すことができる範囲（探索範囲）ｓ_２が球座標で設定される。
【０１００】
ステップＳ９−ＲＴＡ^＊：
ＷＰポインタの系列ＷＰＰ_１とＷＰＰ_２のそれぞれの球座標成分と終端時間ｔ_ｆとが最適化変数Ｘとされて最適化問題が構成され、改良Ｒ−ＴＡＢＵ手法が適用される。展開可能判別評価関数としては、ＲＴＡ_＊アルゴリズムの展開可能判別評価関数：
ｆ’_ｓ（ｎ，ｎ_１）＝ｃ（ｎ，ｎ_２）＋ｈ’（ｎ_２）＋Ｑ_ｓ（ｎ，ｎ_２）
が適用される。同時に、制御パラメータは、最適化変数Ｘに含められて最適化が実行される。
【０１０１】
ステップＳ９−ＲＴＡ^＊：
最適化計算が実行される。ＷＰＰ_１の上で（例示：ｔ_ｒｅｆ／２秒以内）制約条件が充足される解が見出されなければ、ステップはステップＳ１１−ＲＴＡ^＊に移行し、制約条件が満たされれば、ステップはステップＳ１０−ＲＴＡ^＊に移行する（ＷＰの展開可能判別に相当する）。
ステップＳ１０−ＲＴＡ^＊：
ｔ_ｒｅｆ／２秒まで、更に最適化計算が続行され、よりよい解が探索される。ｔ_ｒｅｆ／２秒になった時点で、ステップはステップＳ１２−ＲＴＡ^＊に移行する（Ａ^＊アルゴリズムによる展開節点の選択に相当する）。
【０１０２】
ステップＳ１１−ＲＴＡ^＊：
評価関数ｆ’_ｓ（ｎ，ｎ_１）に対して、更に制約条件の充足可能性が探索される。ｔ_ｒｅｆ秒以内に制約条件が充足されれば、ステップはステップＳ１２−ＲＴＡ^＊に移行し、充足されなければ、ステップはステップＳ１４−ＲＴＡ^＊に移行する。
ステップＳ１２−ＲＴＡ^＊：
現在得られている解が初期解として扱われ、既述の目的状態判別評価関数：
ｆ_ｆ（ｎ，ｎ_１）＝ｃ（ｎ，ｎ_２）＋ｈ（ｎ_２）＋Ｑ_ｆ（ｎ）
に対して最適化が実行される（目標状態判別）。
【０１０３】
ステップＳ１３−ＲＴＡ^＊：
ｔ_ｒｅｆ秒以内に制約条件を充足する解が得られれば、その状態が目標状態であると推定されて、それが解として出力される。そうでなければ、ＷＰ_０←ＷＰ_１、ＷＰ_ｓａｆｅ←ＷＰ_２の更新が実行され、ステップはステップＳ３−ＲＴＡ^＊に移行する。（ＷＰ_１に向かう飛行経路を選択することに相当する。）
【０１０４】
ステップＳ１４−ＲＴＡ^＊：
ＷＰ_ｓａｆｅが存在しなければ探索は失敗してプロセスは終了する。ＷＰ_ｓａｆｅが存在すれば、ＷＰ_０←ＷＰ_ｓａｆｅ、ＷＰ_ｓａｆｅ＝０の更新が実行され、ステップはステップＳ３−ＲＴＡ^＊に移行する。（ＷＰ_ｓａｆｅに向かう飛行経路を選択することに相当する。）
【０１０５】
このように２つ先のＷＰ（＝ＷＰ_２）まで飛行経路の最適探索を実行する。１つ先のＷＰ（＝ＷＰ_１）まで移動する途中で、更により最適である解が探索されて求められる。飛行経路ＷＰ_１の上で制約条件を充足する解を発見することができずにＷＰ_１に到達してしまう場合には、探索は失敗に帰する。探索失敗の場合には、目標点の変更（着陸空港の変更）が必要である。
【０１０６】
ＷＰ_１に到達するまでに制約条件を充足する最適解を見出して、ＷＰ_２がＷＰ_１に再設定され、ＷＰ_３が新規にＷＰ_２に再設定されて、ＲＴＡ^＊のアルゴリズムが次周期探索として実行され、ＷＰＰ_ｆｆが新規にＷＰ_２に新規に再設定されるまで、このようなＲＴＡ^＊アルゴリズムが繰り返えされ、航空機がＷＰＰ_ｆｆの先端点に到達するまで、リアルタイムに最適化探索が実行される。
【０１０７】
近未来の飛行経路上に他の航空機が存在し、又は、近未来の飛行経路上に雷雲が発生し、又は、目標到達時刻が急に変更されるような場合には、通過点が変更され、変更されたウエイポイントに関して、逐次最適化計算（２通過点単位の節点展開と全経路の最適化）が実行される。
【０１０８】
実施の他の形態：
最適解探索手法は、既述のＴＡＢＵ探索手法に限られない。他の探索手法として、遺伝的進化手法、焼き鈍し手法、ローカルリサーチ手法が周知的に知られている。これらの手法のうちの任意の手法にＡ^＊アルゴリズム又はＲＴＡ^＊アルゴリズムが組み合わせられて有効に適用される。遺伝的アルゴリズムでは、最適化変数が突然変異的に又は遺伝子交換的に変更される。遺伝的アルゴリズムは、局所解に陥る可能性が低く複数解を導出する最適化手法として周知である。
【０１０９】
利用例：
本発明による４次元自動航行体は、パイロットの操縦が困難であり、又は、パイロットが不存在である航行体に利用されて特に有用である。軌道衛星でなく軌道を自由に変更することが求められる衛星の最適制御は、人によっては不可能に近い。他の衛星に接近自在である宇宙線、軌道の高低の変更が要求される災害探知衛星、衛星間連絡船に関する最適化は燃料消費の最小化が特に求められる点で効果的である。暴風圏を回避する船舶に関する最適化は、燃料消費の最小化が特に求められる点で効果的である。
【０１１０】
運動方程式の逆解であり時間が消去された軌道解は、物理的パラメータと位置座標により記述される評価関数に１対１に対応する。従って、４次元航法の最適解法は、運動方程式の逆解の最適化問題に変換され得る。その変換は、物理的な普遍的法則により可能である。最適化のアルゴリズムの中にＴＡＢＵ法が採択されることは重要である。遺伝的アルゴリズム、焼き鈍し法、ローカルリサーチ法はＴＡＢＵ法に代えられて有効に利用されるが、遺伝的アルゴリズム、焼き鈍し法、ローカルリサーチ法がＴＡＢＵ法に組み合わされて利用されることは顕著に効果がある。
【０１１１】
【発明の効果】
本発明による４次元自動航行体、４次元航法装置、及び、航行体の４次元制御方法は、リアルタイムの４次元航行を実現することができる。更には、最適解を柔軟に発見することができ、航行体を滑らかに誘導することができる。離発着を含めて全航行を自動化することができる。
【図面の簡単な説明】
【図１】図１は、本発明による４次元航法の最適化システムの実施の形態を示すシステムブロック図である。
【図２】図２は、制約条件を示す表である。
【図３】図３は、ＴＡＢＵ法を示す示すグラフである。
【図４】図４は、最適飛行経路の探索のアルゴリズムを示すグラフである。
【図５】図５は、最適飛行経路の他の探索のアルゴリズムを示すグラフである。
【図６】図６は、最適飛行経路の更に他の探索のアルゴリズムを示すグラフである。
【符号の説明】
１０１…航行本体
１０２…コンピュータ
１０３…航路
１…４次元航法ユニット
２…飛行経路生成ユニット
５…コマンド生成ユニット
６…コマンド
９…探索ユニット
１１…適正解探索ユニット
１２…ウエイポイント生成ユニット
１４…ヒューリスティック関数生成ユニット
４１…フィードバックゲイン
ＷＰ_０…第１途中ウエイポイント
ＷＰ_１…第２途中ウエイポイント
ＷＰ_ｓａｆｅ…目標点
ＷＰ_ｆ…最終目標点
ＷＰ_{ｉ −１}…第１途中ウエイポイント
ＷＰ_ｉ…第２途中ウエイポイント
ＷＰ_ｆｆ…第３途中ウエイポイント
ｓ１…適正化通過点[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a four-dimensional automatic navigator, a four-dimensional navigation device, and a four-dimensional control method for a navigator, and in particular, optimizes a route of a navigator that flies or navigates by receiving fluid resistance such as an aircraft or a ship. The present invention relates to a four-dimensional automatic navigation body, a four-dimensional navigation device, and a four-dimensional control method for a navigation body.
[0002]
[Prior art]
Flying objects such as aircraft and ships, and navigating objects (the flying object and the navigating object are referred to as navigating objects if these are not distinguished from each other) are preferably defined with a movement time between a start point and an end point. . Departure times and arrival times are scheduled for passenger aircraft and passenger ships. In an aircraft, a change between a takeoff time and a landing time is performed on a daily basis. The sudden snowfall at the scheduled landing airport causes a sudden change in the landing airport. Ships arriving earlier than scheduled will be denied entry.
[0003]
According to Newton's equation of motion, if the acceleration is defined momentarily between two points, the motion path between the two points is uniquely determined, but the motion path between the two points is infinitely spatial, In addition, there are countless times, and the motion path between the two points is not uniquely determined. The expert relies on empirical knowledge to determine the spatio-temporal movement path between two points (four-dimensional movement path) in real time, but the expert has sudden changes in the initial conditions that cause the movement path change. It is difficult to properly deal with changes.
[0004]
From the countless motion paths, one four-dimensional motion path corresponding to the remaining fuel amount and the travel time is adopted by the pilot. The motion path adopted by the pilot is not an optimal solution for a given target value. In an emergency state such as a sudden change in weather or a shortage of fuel due to fuel disposal, it is required that an optimal route solution be derived instantaneously, especially in an emergency, without depending on the pilot's empirical judgment. Optimality is a physical and economic property such as safety, minimum time, and minimum fuel consumption.
[0005]
When the positions of the two points and the travel time between the two points are defined, a constraint condition is necessary so that the motion path can be uniquely derived as an inverse solution from the equation of motion by inverse dynamics. As such a constraint condition, an instantaneous acceleration between two points is exemplified, and the acceleration is particularly zero. The moving condition where the acceleration is zero is a constant speed. If such an instantaneous acceleration condition is given, the motion path is uniquely determined. Even when the time and position of the waypoint are determined, the movement path can be uniquely determined, but the navigation body is required to move with a smoothly changing acceleration when passing through the waypoint. It is not necessary to pass through the midway point to be performed, but it is required to pass near the midway point and move at an appropriate acceleration.
[0006]
In order to select an appropriate solution or an optimal solution from among a myriad of solutions, two points of time are designated, there is no time variable, and the minimum amount of fuel in the motion path Z = Z (Xj) represented by the unknown variable Xj The problem of minimizing the target value as described above is called a four-dimensional optimization problem. A mathematical technique for optimization is known from Non-Patent Document 1 below. As a mathematical technique for obtaining a plurality of proper solutions in a wide range, a genetic algorithm is mathematically well known as disclosed in Non-Patent Document 2 below.
[0007]
The solution of the optimization problem has been studied in various details by mathematicians and is well known as is known in the following literature. In optimizing the flight path of an aircraft, it is meaningless to arbitrarily select one of a plurality of solutions known in the mathematical academic literature, and one properly selected solution remains unchanged. It is not appropriate to use it. An aircraft that cannot make a right angle turn at one point is required to move on an arc trajectory having an appropriate momentary curvature. Aircraft and ships have complicated movements such as skidding in the fluid (the concept of skidding of airframes and hulls does not exist in the field of pure mathematics). A motion path is obtained from the equation of motion that incorporates such motion, and the optimization problem of finding the optimal solution among the motion paths requires improvement of the optimization method corresponding to the mathematical structure that is unique to fluid mechanics .
[0008]
There is a need to establish a technology for optimizing the motion of a four-dimensional navigation body that moves in a complicated manner in a fluid. Furthermore, it is important to optimize the four-dimensional motion in real time. Furthermore, it is desired to establish a technology for realizing the next route in real time while finding the minimum solution.
[Non-patent document 1]
Mutsunori Yanagiura and Toshihide Ibaraki, "On Meta Strategies for Combinatorial Optimization Problems," IEICE Transactions Vol. J83-D1, No. 1, pp. 3-25, January 2000
[Non-patent document 2]
Masatoshi Saka, Masahiro Tanaka, "Genetic Algorithm", Asakura Shoten, 1995
[0009]
[Problems to be solved by the invention]
An object of the present invention is to provide a four-dimensional automatic navigation body, a four-dimensional navigation device, and a four-dimensional control method for a navigation body that establish a technology for optimizing the movement of a four-dimensional navigation body that moves in a complicated manner in a fluid. To provide.
Another object of the present invention is to provide a four-dimensional automatic navigator, a four-dimensional navigation device, and a four-dimensional control method for a navigator, which optimize a four-dimensional motion in real time.
Still another object of the present invention is to provide a four-dimensional automatic navigation system, a four-dimensional navigation device, and a four-dimensional navigation system that establish a technique for realizing the next route optimization in real time while finding the minimum necessary solution. It is to provide a control method.
Still another object of the present invention is to provide a four-dimensional automatic navigation body, a four-dimensional navigation device, and a four-dimensional control method for a navigation body, which establish a technique for flexibly finding an optimal solution.
[0010]
[Means for Solving the Problems]
Means for solving the problem are expressed as follows. The technical items appearing in the expression are appended with numbers, symbols, etc. in parentheses (). The numbers, symbols, and the like refer to technical items that constitute at least one embodiment or a plurality of the embodiments of the present invention, in particular, the embodiments or the examples. Corresponds to the reference numbers, reference symbols, and the like assigned to the technical matters expressed in the drawings corresponding to. Such reference numbers and reference symbols clarify the correspondence and bridging between the technical matters described in the claims and the technical matters of the embodiments or examples. Such correspondence / bridge does not mean that the technical matters described in the claims are interpreted as being limited to the technical matters of the embodiments or the examples.
[0011]
A four-dimensional automatic navigation body according to the present invention includes a navigation body (101) that navigates in a fluid, and a computer (102) that automates navigation of the navigation body (101) and is mounted on the navigation body (101). I have. The computer (102) is composed of a four-dimensional navigation unit (1) for four-dimensionally determining the route of the navigation body (101) and a search unit (9). A four-dimensional navigation unit (1) comprising: a route generation unit (2) for generating a route (103) based on a current way point designated between the current position of the navigation body (101) and a target point; And a command generation unit (5) for deriving a command (6) corresponding to (103) according to inverse dynamics. The search unit (9) includes a way point generation unit (12) that generates way points in the middle, and a proper solution search unit that calculates and calculates a proper solution that further optimizes the evaluation function value of the evaluation function corresponding to the command (6). (11). The search unit (11) finds, as a proper solution, a proper passing point that further optimizes the evaluation function value under the constraint condition in a region near the target point. The way point on the way is the first way point (WP₀) And the first waypoint (WP₀) From the target point (WP_safeOr WP_ffOr WP_f) Side, the second waypoint (WP)₁) And (by the pilot). The search unit (11) sets the optimized passage point (S1) for further optimizing the evaluation function value as an appropriate solution, and sets the navigation body (101) from the current position (Q) of the navigation body (101) to the first halfway point ( WP₀) On the way to the second way point (WP)₁) In the neighborhood.
[0012]
Here, the target point is usually the final target point (WP_f), But the target point is the first waypoint (WP)₀), One or more nodes (WP₂Or WP_j, J> 2) are specified. Such designations are primarily designated by the pilot, but it is not denied that the designation is made by a ground controller or that successive designated points are automatically designated.
[0013]
In such a four-dimensional navigation abstractly illustrated in FIG. 6, a node two or three ahead is designated as a way point on the way, and it is strictly necessary to pass exactly through the designated way point two or three ahead. It is optimized (more optimized or optimized) in the vicinity of those designated way points without being made a condition. Such optimization is achieved by the navigation body (101) having the first waypoint WP.₀Alternatively, it is executed earlier in time than the time zone in which it reaches the vicinity. The point expansion that can safely reach the final target point or the vicinity thereof without being optimized over the entire route is performed for the time being, and the calculation time can be reduced. It is not denied that the calculation of the optimization of all paths is executed by another computer in the simultaneous parallel processing. This optimization navigation can shorten the calculation time by a flexible solution, and as a result of the shortening, real-time four-dimensional control is possible.
[0014]
A four-dimensional passing point sequence is specified, a motion path candidate is calculated by an inverse solution, and an acceleration is calculated in consideration of the skidding of the turning trajectory portion of the aircraft corresponding to the motion path candidate. A command (6) for controlling the engine thrust and the steering angle is virtually generated, and the fuel consumption is calculated based on such a motion condition. The exercise condition Xj (xj) and the fuel consumption Q are represented by a functional relationship Q: Q = Q (Xj (xj)). Here, xj is the coordinates of the navigation route or a velocity vector corresponding to the coordinates, and Xj is the number of revolutions of the turbine of the engine, the rudder angle of the fuselage, the side slip angle, the viscous resistance of air, the atmospheric pressure, and many other physical properties. Parameters, and are variables or constants with respect to the position coordinates xj. If the value of the functional relationship Q is not in the proper range, the variable Xj is further searched. The optimization calculation is repeated until a proper solution is obtained by the search calculation loop. If there is no proper solution, a new waypoint is designated. A solution that passes through the designated way point is not necessary, and is appropriate within an area that can pass through within the range of self-performance conditions such as skidding and turning performance of the aircraft (area near the designated way point). The solution is determined flexibly, and the absence of a proper solution is effectively avoided. The navigation body (101) can sequentially reach the appropriate target value and reach the vicinity of the final target point while sequentially finding the appropriate solution. At the final target point corresponding to the landing point, it is important that the azimuth limit condition of the aircraft is satisfied. Such a proper solution deriving technique for sequentially calculating a proper solution in a time series corresponds to a skill of a skilled person, and its suitability is certain.
[0015]
It is obvious that the second way point is designated by the first way point on the side of the final target point adjacent to the first way point. The search unit (11) can update and specify the first way point and the second way point while moving to the first way point. Such updating can dynamically (flexibly) find a solution, and is excellent in practicality.
[0016]
The search allowable time tref is set in the search unit (11), and the search allowable time tref is set at the current time tref.₀From the first waypoint (WP₀) Is set as a time until an estimated time (approximately) to arrive at (tref−t)₀) / K until the first way point and the second way point WP₁Is not updated. In this case, it is obvious that k is a number larger than 1 in terms of sequentially deriving a proper solution. k is set as 2. In this case, the traveling speed is controlled to be substantially constant, and the next plurality of way points are updated before reaching the intermediate point between the first way point and the second way point. k is preferably smaller than 2. The distance between the first way point and the second way point is appropriately set according to the navigation safety and the calculation speed.
[0017]
The search allowable (limit) time is set as a value obtained by dividing the distance from the current time to the first waypoint by the speed at the current time. The trajectory between the two points is substantially a straight line, and since the turning is performed near each of the two points, the speed between the two points is substantially constant. Constant speed is preferred to minimize fuel consumption. Accordingly, if the speed at the current time is expressed by the scalar speed v, the distance D between the first way point and the second way point is approximately represented by D = v × tref.
[0018]
The evaluation functions include an expandable determination evaluation function indicating the expandability of nodes that respectively expand to the final way point and the intermediate way point, and a target state determination evaluation function indicating the target point reachability. It is necessary to be able to reach the respective target points of a plurality of nodes and to reach the final target point at the same time. The expandable discriminant evaluation function is given by the following equation:
f '_s(N, n₁) = C (n, n₂) + H '(n₂) + Q_s(N, n₂)
The target state determination evaluation function is represented by the following equation:
f_f(N, n₁) = C (n, n₂) + H (n₂) + Q_f(N)
Is represented by Here, the subscript s indicates a region near the target point, and f ′_s(N, n₁) Of (n, n₁) Indicates that the navigation body (101) at the initial point is an evaluation value of the node n corresponding to the arrival point at which the navigation body (101) has reached the area near the target point, and c (n) indicates the first value from the initial point to the node n. H ′ (n) indicates a second evaluation value portion (heuristic function) corresponding to the route from node n to the target point;_s(N) indicates a third evaluation value portion corresponding to the constraint condition from the initial point to the node n, and f_f(N) indicates the evaluation value from the initial point to the target point. The proper solution is derived by optimizing the target state determination evaluation function.
[0019]
The optimization is not performed for the entire route to the final target point, but a predictive evaluation function for reaching an appropriate target point is set, the amount of calculation for optimization is greatly reduced, and real-time control is further facilitated.
[0020]
As the navigation body, one element selected from a set including an aircraft, a helicopter, a spacecraft, a satellite, a ship, and a submarine is appropriately exemplified. Such a navigation body is mechanically distinctive in that it slides in response to air resistance and water resistance.
[0021]
The computer 102 can be disconnected from the navigation body 101. A recording medium that describes a program for operating the computer 102 can be separated from the computer 102. Such media are available in the free market as media.
[0022]
The four-dimensional navigation apparatus according to the present invention includes a four-dimensional navigation unit (1) for determining a navigation route of a navigation body (101) in a fluid in four dimensions and a search unit (9 or 11). A four-dimensional navigation unit (1) generates a route (103) based on a current position of the navigation body (101) and an intermediate way point specified between a target point before a final target point. (2) and a command generation unit (5) for deriving a command corresponding to the route (103) according to inverse dynamics (an inverse solution method of motion equation). The search unit (1) includes a waypoint generation unit (12 or 13) for generating waypoints on the way, and a proper solution for calculating a proper solution for further optimizing the evaluation function value of the evaluation function corresponding to the command (6). And a search unit (11). The search unit (9) finds an optimized passing point that further optimizes the evaluation function value under the constraint condition in a region near the target point designated as an appropriate solution.
[0023]
The point that a proper solution is obtained in the neighborhood region is different from the mathematical method of optimization. Within the neighborhood, the proper solution is essentially different from the pure mathematical method in that it is described by a non-zero acceleration.
[0024]
The evaluation function is represented by f, and f = f (V), where V is a three-dimensional velocity vector, and the three-dimensional velocity vector is described by a three-dimensional position vector. Given the time and the position coordinates of the target point, the velocity vector is determined by the calculator using inverse dynamics.
[0025]
The way point on the way is the first way point (WP_{i -1}) And the final target point (WP_f) Side, the second waypoint (WP)_i) And a third way point (WP) designated on the side of the final target point from the second way point._ff) Is set. It is arbitrarily free to be multipointed. The search unit (11) determines a proper pass point that further optimizes the evaluation function value as a proper solution, and determines a first halfway point near the first halfway point, a second halfway point near the second halfway point, and a third halfway way. Each is found in the third halfway point vicinity of the point. At each node, optimization is done with non-zero acceleration.
[0026]
The evaluation functions include an expandable discrimination evaluation function indicating the expandability that expands to the nodes corresponding to the final way point and the intermediate way point, and a target state judgment evaluation function that indicates the target point reachability, and can be expanded. The discriminant evaluation function is as follows:
f '_s(N) = g (n) + h ′ (n) + Q_s(N)
The target state determination evaluation function is represented by the following equation:
f_f(N) = g (n) + h (n) + Q_f(N)
Is represented by Here, the subscript s indicates a region near the target point, and f ′_s(N) of (n) indicates that the navigation body at the initial point is the evaluation value of the node n corresponding to the arrival point at which the navigation body has reached the area near the target point, and g (n) indicates the evaluation value from the initial point to the node n. H ′ (n) indicates a second evaluation value portion corresponding to a route from the node n to the target point, and Q ′_s(N) indicates a third evaluation value portion corresponding to the constraint condition from the initial point to the node n, and f_f(N) indicates the evaluation value from the initial point to the target point. The proper solution in this case is derived by optimizing the target state determination evaluation function. In this case, a proper solution is found off-line, and real-time control is not always required.
[0027]
As in the case of real-time control, a heuristic function generation unit (14) is added to the search unit. The route is input to a heuristic function generation unit (14). h '(n) is generated by the heuristic function generation unit (14). The route corresponding to the proper solution is output from the search unit (11) as a feedback gain, and the feedback gain (41) is input to the command generation unit (5).
[0028]
The four-dimensional control method for a navigation body according to the present invention uses a navigation body (101) that passes through a first neighborhood position selected from each neighborhood of a plurality of points between two points, using the time and the position of the two points as boundary conditions. ) Calculating the first navigation route between the two points by the inverse dynamics computer, and calculating the first evaluation function value of the evaluation function relating to the motion of the navigation body (101) corresponding to the first navigation route by the function value calculator. Calculating the second navigation route between the two points of the navigation body passing through the second neighborhood position selected from the neighborhood, using the time and position of the two points as boundary conditions, using a reverse dynamics computer. Calculating, by a function value calculator, a second evaluation function value of an evaluation function relating to the motion of the navigation body corresponding to the second navigation route, and comparing the magnitude relation between the first evaluation function value and the second evaluation function value The calculation is performed by a computer, and the motion of the navigation body is controlled on the navigation route corresponding to the evaluation function value whose value is more appropriate among the first evaluation function value and the second evaluation function value. . A smooth route is optimally required under various constraints.
[0029]
It is significantly useful that the search unit employs a TABU search method as a mathematical search method. The TABU search method finds an optimal solution in a small area while dividing the neighborhood into smaller smaller areas. The search space of the TABU method corresponds to the space that is the search area of the four-dimensional navigation, and the advantage of the TABU method is that it is an important advantage that it is inherited as it is or is improved for the optimization of the four-dimensional navigation. It is.
[0030]
BEST MODE FOR CARRYING OUT THE INVENTION
Corresponding to the figure, in the embodiment of the four-dimensional navigation optimization system according to the present invention, a four-dimensional control computer is mounted on the navigation body of a four-dimensional automatic navigation body. As shown in FIG. 1, the navigation body 101 of the four-dimensional automatic navigation body includes a four-dimensional control computer 102 for controlling the four-dimensional movement of the navigation body 101, which is mounted on the navigation body 101. The navigation body 102 navigates on a motion path 103 optimized by the four-dimensional control computer 102. Hereinafter, the navigation body 101 is described as an aircraft body.
[0031]
The four-dimensional control computer 102 includes the four-dimensional navigation unit 1. The four-dimensional navigation unit 1 includes an inverse dynamics calculation unit (flight path generation unit) 2 as shown in FIG. When the four-dimensional motion point sequence 3 specified by the starting point and the arrival point (addition of a passing point is possible) is initially given, the inverse dynamics calculation unit 2 performs the four-dimensional motion based on the motion equation. Data 4 is back calculated. The starting point, the passing point on the way, and the reaching point are each constituted by four-dimensional data.
[0032]
The four-dimensional motion point sequence is a set of three-dimensional positions and times corresponding to the positions. The four-dimensional motion data 4 is obtained by inverse dynamics as momentary acceleration. By integrating the acceleration temporally from the initial time to the instantaneous time, the instantaneous speed can be obtained. By integrating the speed over time, a three-dimensional position is obtained every moment, and the number of dimensions of the variable is reduced by one. A set of four-dimensional motion data that is a continuation of the three-dimensional position points matches the flight path.
[0033]
The four-dimensional navigation unit 1 further includes a command generation unit 5. The four-dimensional motion data (flight path) 4 output by the inverse dynamics calculation unit 2 is input to the command generation unit 5. Based on the four-dimensional motion data 4, the command generation unit 5 moves the moving object (for example, a flying object such as an aircraft that is a flying object in the atmosphere, a spacecraft that is a flying object outside the atmosphere, or a liquid) in the four-dimensional movement path 3. A motion control command 6 for positioning the hull, which is a navigation object, is generated. The motion control command 6 is a control signal transmitted to a machine element in order to give a three-dimensional acceleration to a flying object such as an engine thrust and a steering angle.
[0034]
The four-dimensional navigation unit 1 further includes an evaluation simulator 7. The exercise control command 6 output from the command generation unit 5 is input to the evaluation simulator 7. The evaluation simulator 7 calculates and outputs an evaluation value 8 based on the exercise control command 6. The evaluation value 8 is described by a variable corresponding to the exercise position, as described later. The variable is a variable to be optimized, as will be described later, and is represented by three-dimensional position coordinates from which the time t has been deleted, particularly, a velocity vector corresponding to the three-dimensional position coordinates. The evaluation function indicating the evaluation value is described by an optimization variable and a parameter (the motion control command described above). The parameters can be described by optimization variables.
[0035]
The optimization unit 9 includes an optimum solution search unit 11. The evaluation value 8 is cyclically input to the optimization unit 9. The optimal solution search unit 11 derives a velocity vector of an optimal solution that minimizes an evaluation value 8 that is a value of an evaluation function using a four-dimensional coordinate vector as a variable of an optimization problem. The optimal solution search unit 11 further outputs a feedback gain 41 corresponding to the velocity vector of the optimal solution. The feedback gain 41 is input to the command generation unit 5 as a feedback signal together with the four-dimensional motion data 4.
[0036]
The optimization search unit 11 can obtain the optimum value (minimum value or maximum value) of the evaluation function by various search methods. Way points, which are three-dimensional passing point sequences, out of the time-series four-dimensional passing points corresponding to the optimum value, are generated by the way point generation unit 12.
[0037]
The optimal solution search unit 11 forms a motion path setting unit 13 in which a pilot initially sets a discrete four-dimensional motion path (initial way point) 3. The four-dimensional set coordinates (t, x, y, z) 12, which are essential passing points, are initially input by the motion path setting unit 13. The four-dimensional set coordinates (t, x, y, z) 12 include control start set coordinates (t00, x00, y00, z00) for starting motion control of a moving object during navigation, and motion control of the moving object. At least initially, two points of the control end setting coordinates (t0n, x0n, y0n, z0n) (example: runway start coordinates corresponding to the moving object just before landing) to be ended are included. Further, the coordinates (t0j, x0j, y0j, z0j) set in the middle of the finite point of the motion path connecting the two coordinates are initially included. These coordinates are hereinafter typically represented by four-dimensional initial coordinates (way points) (t0j, x0j, y0j, z0j). The optimized (optimized) motion path 3 derived by the optimal solution search unit 11 is output from the optimal solution search unit 11 with or without being set in the motion path setting unit 13. The four-dimensional motion path 3 derived by the optimal solution search unit 11 is input to the flight path generation unit 2. Based on the four-dimensional motion path 3 updated in this way, a motion path that is further optimized by the command generation unit 5, the evaluation simulator 7, and the optimal solution search unit 11 is obtained.
[0038]
The four-dimensional motion data 4 output from the flight path generation unit 2 is input to a heuristic function generation unit 14. The heuristic function generation unit 14, as described later, determines a proper evaluation value, particularly an optimum evaluation value, for a flying object passing through a node between a preceding node (passing point) and a final point. It is given as a predictive objective function part describing the tics function. The predictive target function part generated by the heuristic function generation unit 14 defines a part of an objective function (evaluation function) designed and set in the optimal solution search unit 11.
[0039]
Details of general mathematical methods for optimization of 4D guidance
In the four-dimensional guidance optimization mathematical approach, the optimal control problem can be transformed into a nonlinear programming problem. An algorithm of an optimization search method is formulated in the nonlinear programming problem. The solution of the optimal control problem in which the search for optimization is introduced includes an offline search method and an online search method. Hereinafter, (I) optimization of four-dimensional guidance, (II) offline search method, and (III) online search method will be described in detail.
[0040]
(I) Optimization of 4D guidance
The optimal control problem is mathematically transformed into a nonlinear programming problem by four-dimensional derivation. The state equation is expressed as time t, vector state quantity x (t) ｔRⁿ, Vector control amount u (t) ∈R^mIt is composed of The output of the equation of state (in the present invention, the three-dimensional velocity v every moment) is y (t) ∈R^kIs represented by In this case, the constraint condition c of the (mk) term (or dimension) or (mk) term (the number of terms matches the number of dimensions)_y(T) ∈R^{m − k}is necessary.
[0041]
The evaluation function J including time is
(Equation 1)

And the constraint condition (t∈ [t0, t_f]) Is the following formula
(Equation 2)

The optimal control problem is converted to a problem that minimizes the evaluation function J.
[0042]
If an inverse system is configured, x (t) and u (t) can be represented by output y (t). An inverse system is a computer that calculates an inverse dynamics solution by calculation. The solution of the inverse dynamics is a motion state obtained from the starting point, the arrival point, and the required time. The motion state is a flight trajectory of a set of passing points, and acceleration at an arbitrary passing point position on the flight trajectory. The velocity of the arbitrary passing point can be calculated by integrating the acceleration. In this case, the output y (t∈ [t0, t_f]) Is a variable X∈R that is independent of time t due to the inverse system.^kCan be represented by In particular, the time of the output y (t) can be represented by the motion state of the passing point position, particularly the velocity vector X of the passing point position. Where t₀Indicates the departure time (time 0), t_fIndicates a destination arrival time (time interval from the departure time to the arrival time). The output y (t) is a variable X∈R independent of time t.^kCan be represented by The evaluation function J is newly expressed by the following equation.
[0043]
(Equation 3)

[0044]
In this case, the constraint is:
(Equation 4)

Is represented by The evaluation function J and the constraint conditions are set by the evaluation simulator 7 in design. As described above, the optimal control problem of Expression (1) is a nonlinear programming problem in which the variable X that minimizes the evaluation function J expressed by Expression (4) is found under the constraints (5) and (6). Is converted. The variable X is called an optimization variable in the nonlinear programming problem.
[0045]
In the four-dimensional guidance, particularly in the four-dimensional guidance of a free moving object in a fluid (for example, an aircraft, a spacecraft, a mobile satellite, and a submarine), a velocity vector V (absolute value, direction angle, path angle) is used as an output y (vector). ) Is selected and c_yIs selected as the side slip angle β. The velocity vector is obtained by differentiating a path obtained by three-dimensional interpolation between two Way-Points (hereinafter, WP: three-dimensional passing points) with a continuous time t. The optimization variable X of the nonlinear programming problem can be given as a four-dimensional vector (three-dimensional position x, y, z and time t) of the WP that determines the flight path. The evaluation function for a nonlinear programming problem can be designed as follows.
[0046]
(Equation 5)

The evaluation function J expressed in the integral form of the time t is expressed as a function of the three-dimensional coordinates (x, y, z). Here, Thc of the integrand of the fourth term on the right side matches the thrust of the engine flying at the speed at that time. Ct, Cf, Ce, and Cu are coefficients, each of which is related to required time, fuel consumption, tracking error, and control input. The fuel consumption, tracking error, and control input are t∈ [0, t_f] Can be obtained. These values can be uniquely determined for X. The first term of the evaluation function J is the flight time t_fThe second term is represented by a non-linear function term. From the initial time t (= 0) to the arrival time t_fNo arbitrary time appears in the second term integrated with respect to time, and the evaluation function can be represented by a velocity vector. By the optimization, the optimum speed is obtained under the constraint condition. In this case, the velocity can be obtained by the inverse system as a constant value solution over the entire flight section, or can be obtained by the inverse system (flight path generation unit 2) as a constant value solution for each of a plurality of flight sections.
[0047]
FIG. 2 shows a constraint condition. The table shows equality constraints and inequality constraints as constraints corresponding to the aircraft. As inequality constraints, altitude, speed, angle of attack, attitude angle, angular velocity, and weighting multiple are shown. These values must be lower than the specified maximum. The end position error, the end speed error, and the end bank angle are shown as the equation constraint conditions. These values are accurately defined, but allow for the tolerances required for computational convenience. If divergence occurs in the process of deriving the optimal value solution of the evaluation function value, the search is stopped.
[0048]
When divergence occurs, the evaluation function and the constraint cannot be evaluated properly. By evaluating the state of divergence, a penalty function Jnew is designed. The value of the state variable of the ith constraint is p_iWhere the upper and lower limits are p_i ^max, P_i ^minIs represented by Either of the following two equations is satisfied.
(Equation 6)

[0049]
Equational constraints make calculations difficult as they are. To mitigate the difficulty, the relaxation coefficient v_iIs introduced. The state variable of the i-th equation constraint is q_iAnd the value of the constraint is q_i ^cAnd the following equation is defined.
(Equation 7)

[0050]
A penalty evaluation function Jnew is designed for such constraints.
(Equation 8)

Where Cs_iAnd Cc_jIndicates a scaling factor for the corresponding constraint, and is a positive value.
[0051]
An optimum value solution is obtained for the evaluation function designed in this manner. As a search technique for finding the optimum value solution, the above-described R-TABU search technique can be adopted. In the R-TABU search method, as shown in FIG. 3, different region groups H (X, h) having the same size h near an optimization variable X (indicated as position coordinates corresponding to speed).₁) Is set. Between the current point P and the next target point Q, a vector (WP described later)_i). The aircraft does not necessarily pass through the point Q, but passes near the point Q. A circle having a radius r around the point Q is defined as the search area C.
[0052]
The search area C is simply represented by a two-dimensional plane (or represented by a spherical cross section). The search area C is divided into a plurality of small circles C1j (n = 1,..., N) included in the circle C and selected at random. A set of a finite number of small circles C1j covers the circle C. The evaluation function J is obtained by calculation for all the small circles C1j. The small circle C1k having the minimum value (or the maximum value) among the finite number of evaluation values is selected. The small circle C1k is divided into a finite number of small circles C2n. A set of a finite number of small circles C2j covers the circle C1k.
[0053]
The evaluation function J is obtained by calculation for all the small circles C1k. The small circle C2s having the minimum value among the finite number of evaluation values is selected. The radius of the small circle C1n is 1 / h of the radius of the small circle C. The radius of the small circle C2n is 1 / h of the radius of the small circle C1n. Similarly, the radius of the small circle C “tn” is_{(T -1 ) N}Is 1 / h. By an appropriate number of depth update searches, an optimization variable X (corresponding to the position of a point) having the largest evaluation value (the function J is the minimum or maximum) in the area H is obtained by trial and error at the depth number t. Can be As described above, the neighboring area section [a, b] (in this case, [0, r],..., [R_{t -1}, R_t], The next optimal passing point Q ′ is determined by the R-TABU method in which the geometrical series is reduced. The next optimal passing point Q 'satisfies all of the above-described constraints.
[0054]
Such narrowing of the search section is expressed as follows.
(Equation 9)

[0055]
Under such mathematical rules, the algorithm of the R-TABU search method is constituted by the following steps.
R-TABU-S1:
An appropriate initial solution X, step reduction ratio L, number of steps ns, and number of counts nc are determined.
R-TABU-S2:
A neighborhood region group H (X, h) of the initial solution X is determined. : An integer for counting is represented by i, j, i = 1, j = 0.
R-TABU-S3:
Nearby area H_i(X, h_i), Randomly answer Y_iIs generated.
[0056]
R-TABU-S4:
Trial solution Y_iIs better than the initial solution X, then its Y_iIs replaced by the initial solution X.
R-TABU-S5:
Substitution of j = j + 1 is executed. If j <nc, the next step proceeds to R-TABU-S3. If j <nc, substitution of i = i + 1 is executed and j = 0. Is set, and the next step proceeds to R-TABU-S6.
[0057]
R-TABU-S6:
If i <= ns, the next step shifts to R-TABU-S3. If not i <= ns, the next step shifts to R-TABU-S7.
R-TABU-S7:
If the end determination is yes, X is extracted as the optimal solution. If the end determination is no, the step moves to R-TABU-S2.
[0058]
FIG. 4 shows an optimal flight path when two adjacent designated points are given. (J-1) designated passing point P_{j -1}And the j-th designated passing point P_jIs initially set in the motion path setting unit 13 of the optimal solution search unit 11 by the pilot together with the passing time, the aircraft moves to the (j-1) th designated passing point P_{j -1}And the j-th designated passing point P_jIt is impossible to fly along a straight line (dashed-dotted line) that connects with, except in special cases. (J-1) designated passing point P_{j -1}The aircraft heading for is based on the current position, the (j-1) th designated passing point P_{j -1}Pass through the (j-1) -th neighbor 21 of.
[0059]
The aircraft passing through the first (j-1) neighborhood 21 is based on the optimization point of the first (j-1) neighborhood 21 and the j-th designated passage point P_jPass through the j-th neighborhood 22 of. The optimization passage point is obtained by the optimization solution search unit 11 through an optimization calculation. (J-1) -th optimized pass point P '_{j -1}Is found in the (j-1) -th neighborhood 21 and the j-th optimization pass point P '_jIs found in the j-th neighborhood 22 and the optimal flight path 23 is represented by a smooth solid line. The flight path 24 shown by the two-dot chain line of other optimization candidates is inferior to the optimum flight path 23 in terms of the degree of optimization (it is determined by the evaluation simulator 7).
[0060]
(II) Offline search method
Optimal control that derives an optimal solution as uniquely as possible as a whole with respect to a multipoint passing path in which two or more multipoints are indicated is called off-line control. In the off-line control, an optimal solution for all routes is obtained, and the aircraft flies along the optimal solution route. In the state space search problem, the state space S, the initial state s₀{S, target state determination function P: s → {True, False}, basic operation O_i(S_i) = S_{i + 1}Is defined. As a concept related to WP, a “WP pointer” and a “tip WP” are defined as follows.
[0061]
WP pointer:
In the state space search problem, it is determined how a certain WP moves from the previous WP. Specifically, the relative position and relative time between one WP and the next WP are specified by a vector. Such a vector is called a WP pointer. By adding the WP pointer from the initial state to another state, the position of the WP is sequentially specified. As shown in FIG. 5, the i-th WP is represented by a vector WPPi, and the (i-1) -th WP_{i -1}I-th WP from_iIs represented by WPPi.
WPP_i= WP_i-WP_{i -1}
[0062]
Tip WP:
The most advanced WP of the WP series that does not include the terminal WP (the WP immediately before the destination WP) is called a leading WP and is called a WPP._ffIs represented by
[0063]
Definition of sets and operations:
WP_ffFor, sets and operations are defined as follows:
State S: state s is the WP at the tip_ffThe neighborhood area of. S is the set.
・ Initial state s₀∈S: Position at the start of guidance (WP₀)
-Target state determination P: → {True, False}: The search objective is to satisfy all the constraint conditions by optimization, and the search is executed for all of the expanded neighboring areas s. True if a solution is found that satisfies all constraints, False otherwise.
・ Basic operation O_i(S_i) = S_{i + 1}: Appropriate WP pointer is WPP_newIs represented by Tip WP is WP_ff← WP_ff+ WPP_newWill be updated as follows. WPP updated in this way_ffIs the neighborhood space s_{i + 1}Is set to
[0064]
State set S:
The neighborhood region s, which is a state, matches a certain finite space. The neighborhood area s considered now is WP which is the WP at the tip._ffIs a region in the vicinity of. WP_ffIs the WP that is the previous WP_{ff -1}WP pointer WPP_ffCan be expressed as follows.
WP_ff= WP_{ff -1}+ WPP_ff
[0065]
Where WP_{ff -1}Is fixed, the range of the neighborhood area s is WP_{ff -1}Can be determined only by the range in which Therefore, the initial state WP₀When the WP pointers are sequentially added from, the range in which the WP pointer can point can be determined, so that all the neighboring areas s can be determined. The range that the WP pointer can point can be determined by the kinetic performance of the aircraft (eg, turning radius, thrust, airframe structure, presence of an in-flight infant). When an infant is present in the aircraft, the upper limit of acceleration is kept low.
[0066]
Expandable judgment:
The states s are sequentially generated by a chain of basic operations. Thus, when the state s is expanded by the basic operation, the appropriate WP pointer WPP_new(WPP in FIG. 5_i) Is used. Actually, the two WPs that specify the flight path are continuously connected by a spline function. When deploying WP, WP_ffThe next WP on the path between the_ffWPP so that_newIs determined. The state in which the state s, which is a region near the WP at the tip, is expandable is defined as follows.
[0067]
· S can be deployed @ WP_ff← WP_ff+ WPP_newNew neighborhood space s set as_{i + 1}And WP₀~ WP_ffWP that satisfies all flight restrictions_ffIs included (the termination condition does not need to be satisfied).
[0068]
Discretization of continuous space:
S, which is a neighborhood of WP formed as a continuous space, cannot be effectively counted because “the solution candidates are discrete and are prerequisites for the state space search problem”. Here, a plurality of area groups H (WP, h) having different sizes h are set in the neighborhood area s of one specified WP so that the TABU search technique is extended to the R-TABU search technique. It is effective to randomly generate WPs in each area, and to represent each area with the WP. Each area H_j(WP, h_j) Representative WP is WP_HjAnd the neighborhood s is WP_HjCan be effectively counted if there is a finite number of WPs.
[0069]
WP pointer optimization:
All states s are a sequence of WP pointers WPP_i(I = 1,..., Ff). The target state discrimination and the WP expansion possible discrimination are optimizations for satisfying each discriminating constraint on s defined by discretizing a continuous space by randomly generating a WP. It can be defined as a problem.
The target state determination and the deployable determination are performed by a series of randomly generated WP pointers WPP._i(I = 1,..., Ff) and time t toward the terminal state_fAre variables of the optimization problem, and are converted into an optimization problem for the purpose of satisfying the constraints of each determination. The R-TABU method can be applied to solve the optimization problem thus converted.
[0070]
Set evaluation function:
As an evaluation (value) function, f ′ for expansion determination and target state determination_s(N) and f for target discrimination_f(N) are designed and set in a function setting portion (not shown) of the evaluation simulator 7. Both take in the constraints by the penalty function method. Evaluation function for expansion determination and selection of expansion nodes (referred to as expansion determination evaluation function) f '_sThe design method (n) is expressed as follows.
f '_s(N) = g (n) + h ′ (n) + Q_s(N)
[0071]
Here, h ′ (n) is a heuristic function, and is set by the heuristic function generation unit 14 corresponding to the four-dimensional motion data 4. The expandable discriminant evaluation function is set in the evaluation simulator 7 and introduced into the optimum solution search unit 11 to be set in the optimum solution search unit 11. The heuristic function is generated by the optimization unit 9 as an evaluation function part corresponding to the four-dimensional motion data 4 output by the flight path generation unit 2 and is introduced into the optimal solution search unit 11, but is set in the evaluation simulator 7. There is no necessity.
[0072]
The state indicated by the current node n (corresponding to ff in FIG. 5) is s. State s is WP which is the WP at the tip_ffIs shown in the vicinity. Where WP_ffTime element t_ffAre not included in the optimization target and are specified in advance (the current problem is that the specified time t_fThe problem of minimizing fuel consumption to reach). g (n) is a cost (e.g., fuel consumption) from the initial node to the node n. g (n) is configured as follows.
(Equation 10)

[0073]
In the flight simulation for the determination of the deployment possibility and the evaluation of the deployment node, in order to reduce the calculation cost, t_ffIntegration has been performed up to. The node (n_ff) And the cost of the flight between the destination points is expected. Such a predictive cost function is called a heuristic function as described above, and the introduction of a heuristic function h '(n) is essential. The heuristic function h '(n) is essentially introduced as a function for evaluating "how close to the target state" is. The heuristic function h '(n) can be specifically designed as follows.
[0074]
(Equation 11)

[0075]
Where Θ_we, Ψ_weIndicates the difference between the path angle and the azimuth angle of the target flight path immediately before the end (the unit is Degree), and C indicates_Θw, C_ΨwIndicates the scaling factor. C, the first term of the heuristic function h '(n)_t(T_f-T_ff) Is a main part of the estimated cost required from the leading end point of the evaluation target WP at the current time based on the clock of the clock included in the optimal solution search unit 11 to the target point.
[0076]
Next, the evaluation value Q of the constraint condition of the penalty function method_s(N) is shown. Q_s(N) is a function to which constraints are added, and WP₀~ WP_ffIs included. Evaluation value Q_s(N) is designed as follows.
[0077]
(Equation 12)

[0078]
Next, an evaluation function for target determination (target determination evaluation function) f_fThe design method of (n) is shown. Target discriminant evaluation function f_f(N) is represented by the following equation.
f_f(N) = g (n) + h (n) + Q_f(N)
Here, g (n) + h (n) is represented by the following equation.
(Equation 13)

[0079]
Target discriminant evaluation function f_fEvaluation value Q of constraint condition (n)_f(N) takes all constraints into account and gives f_f(N) is a value represented by the following equation.
[Equation 14]

[0080]
Target point WP_f, The neighborhood area is not set in principle. As the target point, a four-dimensional guidance stop point at the landing airport (a stop point of fully automatic navigation) or a four-dimensional junction point of the orbit satellite is exemplified, and the area or volume of the nearby area is substantially zero. Target discriminant evaluation function f_fn is integrated over time until the scheduled arrival time. Target discriminant evaluation function f_fn describes the final evaluation value. A target point (not limited to the final target point actually specified in the end, but includes a strictly defined waypoint. For example: an intermediate landing point included in the middle of the route) specified as a point one point before The node expandable discriminant function of the path describing up to the target node satisfies all the constraint conditions, and can reliably reach the vicinity of the passing point immediately before the target point.
[0081]
Solution of optimal value problem by state space search (algorithm A^*):
Step S1-A^*:
Tip condition WP_ffIs the initial state WP₀Is set as
WP_ff← WP₀
Integer i is set to i = 0 and WP_ffIs WP, as shown in FIG.₀Is set as the way point 12. The control parameter search range and various parameters of the R-TABU search method (or the improved R-TABU search method) are set.
Step S2-A^*:
WP that is the i-th WP_iRegarding the basic operation O_iIs executed. The update of i ← i + 1 is executed.
[0082]
Step S3-A^*
WPP that is the i-th WP pointer_iRegarding the WPP_i← WPP_newIs updated.
Step S4-A^*
WP pointer WPP_i, The range (search range) s that can be pointed_iIs set in spherical coordinates.
[0083]
Step S5-A^*
WP pointer sequence WPP_j(J = 1,..., I) spherical coordinate component and terminal time t_fIs represented by an optimization variable X to form an optimization problem, and the R-TABU method is applied as an optimization method. As the evaluation function, A^*Algorithm evaluation function:
f '_s(N) = g (n) + h ′ (n) + Q_s(N)
Is applied. At the same time, the control parameters are included in the optimization variable X and optimization is performed. A heuristic function h '(n) is generated by the heuristic function generation unit 14, and the expandable discriminant evaluation function f' partially including the heuristic function h '(n)._s(N) is described in the optimal solution search unit 11.
[0084]
Step S6-A^*:
The optimization calculation is executed by the optimal solution search unit 11. If the constraint is not satisfied, the step proceeds to step S1-A^*The process proceeds to step S7-A if the constraint is satisfied.^*Move to
Step S7-A^*:
Further, an optimization calculation is performed. The best solution is obtained from the solutions obtained when the optimization calculation is continued to some extent (A^*This corresponds to selecting an expansion node by an algorithm. ).
[0085]
Step S8-A^*:
Step S7-A^*Is treated as the initial solution, and the evaluation function described above:
f_f(N) = g (n) + h (n) + Q_f(N)
Is optimized (target state determination). Expandable determination evaluation function f ′ of equation (15)_s(N) is obtained by optimization and h ′_nIs obtained, and {g (n) + h (n)} of Expression (20) is known from Expression (17), and the target state determination evaluation function f of Expression (19) is obtained._f(N) is required.
[0086]
Step S9-A^*:
If a solution that satisfies the constraint is not obtained within the specified number of trials (or time), the state is estimated to be the target state, and is output as a solution. Otherwise, step S2-A^*Move to
[0087]
Target state determination evaluation function f_fIf a proper value is found as the value by the minimum value search of (n), the proper value is output from the optimal solution search unit 11 to the way point generation unit 12, and the next WP pointer corresponding to the optimized proper value is output. WPP_{i + 1}Is generated by the way point generation unit 12. The new WP pointer WPP_{i + 1}, The flight path of the next section is calculated, the command is given to the aircraft, and the optimal solution search in the next cycle is repeated by the optimal solution search unit 11. By the algorithm of the iterative loop for searching for the optimal solution, WP_ffAre optimized, and finally, the target state determination evaluation function f_fAccording to (n), the entire flight path to the final target point of the aircraft is optimized and determined.
[0088]
The optimization of the off-line control flight described above is determined at the stage of preparation for takeoff of the aircraft, the optimized flight path is determined in one way, and the possibility of real-time control is not particularly considered.
(3) Online search method
If the calculation speed of the computer is fast enough, it is obvious that the optimum flight path between the current point and the target point can be calculated in real time. When there are airflow fluctuations and other unpredictable factors, online flight control, which is real-time control using a computer whose calculation speed is not fast enough, is important.
[0089]
In the optimization calculation applied to online flight control, the following two items are important.
A feasible solution must be generated within a certain time:
Several parameters that affect the convergence of the optimization calculation are well tuned to achieve the desired performance.
・ Ensuring safety:
If the next guidance rule cannot be generated within a certain period of time, a guidance control rule update program including danger avoidance is set so that the flight is continued without updating the guidance control rule. Further, the time limit allowed for the optimization calculation is determined to include the margin so that the guidance rule can be generated within a certain time when the actual device is mounted.
[0090]
Initial settings:
WP that is the current (current time) WP₀Is set as an initial state as shown in FIG. Current flight position P₀For the initial state WP₀Is in the direction of flying. Generated WP₀Is in the future flight path.
[0091]
Target state determination and deployment determination:
WP₀The basic operation is performed once, and the waypoint generation unit 12₁Is generated. WP₁The basic operation is performed once, and the waypoint generation unit 12₂Is generated. WP₁And WP₂Are set simultaneously by the way point generation unit 12. WP₂Is set as the starting point of the offline search. Offline search is WP₂Is subjected to the closed-loop optimization search shown in FIG.₂, The node development determination and the target state determination are sequentially performed. The heuristic function is WP₂To destination WP_fEstimate costs up to. Both discriminations are performed on the solution of the aforementioned optimization problem.
[0092]
Ensuring safety:
For the development of the next WP, evaluation values up to the next WP are used. WP of current search target₂Is ensured to be deployable, and a flight path is secured when a node cannot be deployed within the time specified in the current deployment attempt. WP₂Nodal expansion that can be performed is ensured, and the target state determination is started after all the nodal expansion determinations are completed. An on-line control method described below is a flight method in which it is possible to secure in real time a flight route that is safe in a short section that is near to the current time and that is at least optimal in the short section.
[0093]
Initial state move:
Update cycle t_refWithin WP₂Is secured and the search for the optimization is performed up to the target point in the optimum solution search unit 11, and if the target determination is False, the aircraft is WP₁Move up to. Such a transfer can be performed safely and optimally. The WP₁But new WP₀Is set as WP₁To move to the WP in the middle of the generated route₁The target path is switched on.
[0094]
Evaluation function design for online flight control:
WP of current location is WP₀Is set to WP₀Is represented by n. Nodes obtained by executing the basic operation twice on node n once are n in order from the near side.₁, N₂Is represented by n₁, N₂Are included in the state s corresponding to₁, WP₂Is represented by Evaluation function f ′ for optimization calculation of expansion determination and expansion node selection_s(N, n₁) Is designed as follows.
f '_s(N, n₁) = C (n, n₂) + H '(n₂) + Q_s(N, n₂)
Where Q_s(N, n₂) Is a function to which the constraints of the penalty function method are added, and WP₀~ WP₂Is included.
[0095]
In such an evaluation function, evaluation values up to the next node are used to develop the next node. This allows the current node n₂Is expanded so that when a node cannot be expanded within the time specified in the next node expansion trial, WP₂The next nodal expansion is possible immediately before reaching.
[0096]
Proposition that is a target state discriminant function:
"For the purpose of satisfying all constraints by optimization, a search is performed for all of the expanded neighboring regions s, and if a solution that satisfies all constraints is found, True, and a solution that satisfies all constraints is determined. False if not found. "
For, the same optimization algorithm as described above can be used. In this case, the evaluation function is designed by the following equation.
f_f(N, n₁) = C (n, n₁) + H (n₂) + Q_f(N)
Where Q_f(N) is a function to which the constraints of the penalty function method are added, and WP₀~ WP_Nnew(Including termination conditions) (WP_NnewIs the WP of the target point. ).
[0097]
Real-time algorithm RTA for online flight control according to the present invention^*Is expressed as follows.
Step S1-RTA^*:
The initial state is WP₀Is represented by The control parameter search range and various parameters of the R-TABU search method (or the improved R-TABU search method) are set.
Step S2-RTA^*:
WP₀To t_refThe position where it went straight at the current speed for only seconds is newly WP₀Is set as WP before new setting₀3t from (point Q in FIG. 6)_refOnly at the current speed (P₂) Is WP_safeIs represented by WP_safeIs a WP for ensuring safety.
[0098]
Step S3-RTA^*:
WP₀Basic operation O₀Is executed, and the obtained WP is WP₁Is represented by
Step S4-RTA^*:
WPP, the first WP pointer₁Regarding the WPP₁← WPP_newIs updated.
Step S5-RTA^*:
WPP which is a WP pointer₁, The range (search range) s that can be pointed₁Is set in spherical coordinates.
[0099]
Step S6-RTA^*:
WP₁Basic operation O₁Is executed, and the obtained WP is WP₂Is represented by
Step S7-RTA^*:
WPP, the second WP pointer₂Regarding the WPP₂← WPP_newIs updated.
Step S8-RTA^*:
WPP that is the WP pointer₂, The range (search range) s that can be pointed₂Is set in spherical coordinates.
[0100]
Step S9-RTA^*:
WP pointer sequence WPP₁And WPP₂Of each spherical coordinate component and the end time t_fIs an optimization variable X to form an optimization problem, and the improved R-TABU method is applied. RTA can be used as an evaluation function_*Algorithm expandable discriminant evaluation function:
f '_s(N, n₁) = C (n, n₂) + H '(n₂) + Q_s(N, n₂)
Is applied. At the same time, the control parameters are included in the optimization variable X and optimization is performed.
[0101]
Step S9-RTA^*:
An optimization calculation is performed. WPP₁(Example: t_refIf no solution that satisfies the constraint condition is found, the step proceeds to step S11-RTA.^*The process proceeds to step S10-RTA if the constraint condition is satisfied.^*(Corresponding to WP deployment possibility determination).
Step S10-RTA^*:
t_refUntil / 2 seconds, further optimization calculations continue and a better solution is sought. t_ref/ 2 seconds, the step is step S12-RTA^*(A^*This is equivalent to selecting an expansion node by an algorithm).
[0102]
Step S11-RTA^*:
Evaluation function f '_s(N, n₁), The possibility of satisfying the constraint condition is further searched. t_refIf the constraint is satisfied within seconds, the step proceeds to step S12-RTA.^*If not satisfied, the step proceeds to step S14-RTA.^*Move to
Step S12-RTA^*:
The currently obtained solution is treated as the initial solution, and the objective state discriminant evaluation function described above:
f_f(N, n₁) = C (n, n₂) + H (n₂) + Q_f(N)
Is optimized (target state determination).
[0103]
Step S13-RTA^*:
t_refIf a solution that satisfies the constraints is obtained within seconds, it is estimated that the state is the target state, and it is output as a solution. Otherwise, WP₀← WP₁, WP_safe← WP₂Is executed, and the step is Step S3-RTA^*Move to (WP₁This is equivalent to selecting a flight route toward. )
[0104]
Step S14-RTA^*:
WP_safeIf does not exist, the search fails and the process ends. WP_safeIf exists, WP₀← WP_safe, WP_safe= 0 is executed, and the step is Step S3-RTA^*Move to (WP_safeThis is equivalent to selecting a flight route toward. )
[0105]
Thus, the WP (= WP₂Perform an optimal search for the flight path up to). The next WP (= WP₁On the way to (2), an even more optimal solution is searched and found. Flight path WP₁WP cannot find a solution that satisfies the constraints on₁, The search fails. If the search fails, it is necessary to change the target point (change the landing airport).
[0106]
WP₁Find the optimal solution that satisfies the constraints before reaching₂Is WP₁Is reset to WP₃Is a new WP₂Reset to RTA^*Is executed as a next cycle search, and the WPP_ffIs a new WP₂Until the RTA is newly reconfigured,^*The algorithm is repeated and the aircraft_ffThe optimization search is executed in real time until the end point is reached.
[0107]
If another aircraft exists on the near-future flight path, or a thundercloud occurs on the near-future flight path, or the target arrival time is suddenly changed, the passing point will be changed. For the changed way point, sequential optimization calculation (node expansion in units of two passing points and optimization of all paths) are executed.
[0108]
Other forms of implementation:
The optimal solution search method is not limited to the TABU search method described above. As other search methods, a genetic evolution method, an annealing method, and a local research method are well known. Any of these methods can be assigned A^*Algorithm or RTA^*The algorithms are effectively combined and applied. In a genetic algorithm, the optimization variables are changed either mutationally or genetically. Genetic algorithms are well known as an optimization technique that derives multiple solutions with a low possibility of falling into a local solution.
[0109]
Usage example:
The four-dimensional self-propelled vehicle according to the present invention is particularly useful for a vehicle in which pilot control is difficult or where the pilot is absent. Optimal control of satellites that are required to change their orbits freely instead of orbiting satellites is almost impossible for some people. Optimization of cosmic rays that are accessible to other satellites, disaster detection satellites that need to change the orbital height, and inter-satellite ferry are effective in that minimization of fuel consumption is particularly required. Optimization of ships avoiding the storm zone is effective in that minimization of fuel consumption is particularly required.
[0110]
The trajectory solution, which is the inverse solution of the equation of motion and whose time has been eliminated, corresponds one-to-one to the evaluation function described by the physical parameters and the position coordinates. Therefore, the optimal solution of the four-dimensional navigation can be converted into an optimization problem of the inverse solution of the equation of motion. The transformation is possible by physical universal rules. It is important that the TABU method be adopted as an optimization algorithm. The genetic algorithm, annealing method, and local research method are effectively used in place of the TABU method. However, the combination of the genetic algorithm, annealing method, and local research method with the TABU method has a remarkable effect. is there.
[0111]
【The invention's effect】
ADVANTAGE OF THE INVENTION The four-dimensional automatic navigation body, the four-dimensional navigation apparatus, and the four-dimensional control method of a navigation body by this invention can implement | achieve a real-time four-dimensional navigation. Furthermore, the optimum solution can be found flexibly, and the navigation body can be guided smoothly. It is possible to automate the entire voyage including taking off and landing.
[Brief description of the drawings]
FIG. 1 is a system block diagram showing an embodiment of an optimization system for four-dimensional navigation according to the present invention.
FIG. 2 is a table showing constraint conditions.
FIG. 3 is a graph showing a TABU method.
FIG. 4 is a graph showing an algorithm for searching for an optimal flight route.
FIG. 5 is a graph showing an algorithm of another search for an optimal flight route.
FIG. 6 is a graph showing an algorithm of still another search for an optimum flight route.
[Explanation of symbols]
101 ... navigation body
102 ... Computer
103 ... Route
1. 4D navigation unit
2. Flight path generation unit
5 Command generation unit
6. Command
9 Search unit
11 ... Appropriate solution search unit
12 ... way point generation unit
14 Heuristic function generation unit
41 ... Feedback gain
WP₀… First way point
WP₁… Second way point
WP_safe… Target point
WP_f… Final goal
WP_{i -1}… First way point
WP_i… Second way point
WP_ff… The third way point
s1: Optimization pass point

Claims (23)

A navigation body that navigates in the fluid;
Comprising a computer that automates navigation of the navigation body and is mounted on the navigation body,
The computer is
A four-dimensional navigation unit for four-dimensionally defining a route of the navigation body;
Form a search unit and
The four-dimensional navigation unit comprises:
A route generation unit that generates a route based on a current position of the navigation body and an intermediate way point specified between a target point;
A command generation unit that derives a command corresponding to the route according to inverse dynamics,
The search unit comprises:
A way point generating unit for generating the way point on the way,
A proper solution search unit for calculating a proper solution that further optimizes the evaluation function value of the evaluation function corresponding to the command,
The search unit finds an optimized pass point that further optimizes the evaluation function value under constraints as the appropriate solution in a region near the target point,
The waypoints on the way are:
The first waypoint,
A second way point designated closer to the target point than the first way point,
The search unit sets the optimized pass point that further optimizes the evaluation function value as the appropriate solution, and sets the second way as the navigation body moves from the current position of the navigation body to the first way point. A four-dimensional autonomous vehicle found in the vicinity of a point. The four-dimensional automatic navigation body according to claim 1, wherein the second intermediate way point is specified by the first intermediate way point adjacent to the first intermediate point on the side of the target point. 3. The four-dimensional automatic navigation body according to claim 1, wherein the search unit updates and specifies the first halfway point and the second halfway point during the transition to the first halfway point. An allowable search time tref is set in the search unit, and the allowable search time tref is set as a time from a current time t0 to an expected time of arrival at the first halfway point, and is calculated by (tref−t0) / 4. The four-dimensional automatic navigation body according to claim 3, wherein the first way point and the second way point are not updated until k has elapsed, and k is a number greater than one. 5. The four-dimensional automatic navigation body according to claim 4, wherein k is 2. The four-dimensional automatic navigation body according to claim 4, wherein the search allowable time is a value obtained by dividing a distance from the current time to the first intermediate way point by a speed at the current time. The distance D between the first way point and the second way point is approximately represented by D = v × tref if the speed at the current time is represented by a scalar speed v. 4 dimensional automatic navigation body. The evaluation function is:
An expandability determination evaluation function indicating the expandability to expand to nodes corresponding to the target point and the waypoint respectively,
Including a target state determination evaluation function indicating the target point reachability,
The expandable determination evaluation function is represented by the following equation:
f ′ _s (n, n ₁ ) = c (n, n ₂ ) + h ′ (n ₂ ) + Q _s (n, n ₂ )
The target state determination evaluation function is represented by the following equation:
f _f (n, n ₁ ) = c (n, n ₂ ) + h (n ₂ ) + Q _f (n)
In expressed, where the subscript s indicates the area near the target point, the _{f 's (n, n 1} ) of the (n, n _1), the sail body the target point of the initial point Indicates the evaluation value of the node n corresponding to the arrival point that has reached the neighborhood area, c (n) indicates the first evaluation value portion from the initial point to the node n, and h ′ (n) indicates the A second evaluation value part corresponding to the route from the node n to the target point is shown, Q _s (n) is a third evaluation value part corresponding to the constraint condition from the initial point to the node n, and _ff ( n) indicates the evaluation value from the initial point to the target point, c (n, n ₂ ) indicates the cost from n to n ₂ ,
8. The four-dimensional automatic navigation body according to claim 7, wherein the proper solution is derived by optimizing the target state determination evaluation function. The four-dimensional automatic navigation body according to claim 8, wherein the navigation body is one element selected from a set including an aircraft, a helicopter, a spacecraft, a satellite, a ship, and a submarine. The four-dimensional automatic navigation body according to claim 9, wherein the one element slides by receiving a fluid resistance. 11. The four-dimensional automatic navigation body according to claim 1, wherein the search unit employs a TABU search method as a mathematical search method. 12. The four-dimensional navigation system according to claim 11, wherein the TABU search method finds an optimal solution in the small area while dividing the neighborhood into smaller small areas. Automated navigation of the navigation body to configure a computer mounted on the navigation body,
The computer is
A four-dimensional navigation unit for four-dimensionally defining a route of the navigation body;
Form a search unit and
The four-dimensional navigation unit comprises:
A route generation unit that generates a route based on a current position of the navigation body and an intermediate way point specified between a target point;
A command generation unit that derives a command corresponding to the route according to inverse dynamics,
The search unit comprises:
A way point generating unit for generating the way point on the way,
A proper solution search unit for calculating a proper solution that further optimizes the evaluation function value of the evaluation function corresponding to the command,
The search unit finds an optimized pass point that further optimizes the evaluation function value under constraints as the appropriate solution in a region near the target point,
The waypoints on the way are:
The first waypoint,
A second way point designated closer to the target point than the first way point,
The search unit sets the optimized pass point that further optimizes the evaluation function value as the appropriate solution, and sets the second way as the navigation body moves from the current position of the navigation body to the first way point. A four-dimensional navigation device to find in the vicinity of a point. A four-dimensional navigation unit for four-dimensionally determining a route of the navigation body in the fluid;
And a search unit,
The four-dimensional navigation unit comprises:
A route generation unit that generates a route based on a current position of the navigation body and an intermediate way point specified between a target point in front of the target point;
A command generation unit that derives a command corresponding to the route according to inverse dynamics,
The search unit comprises:
A way point generating unit for generating the way point on the way,
Forming a proper solution search unit for calculating a proper solution that further optimizes the evaluation function value of the evaluation function corresponding to the command,
The search unit finds an optimized pass point that further optimizes the evaluation function value under the constraint condition as the appropriate solution in a region near the target point,
The optimization system for four-dimensional navigation, wherein the proper solution is described by a non-zero acceleration in the neighborhood. The evaluation function is represented by f, and f = f (V), where V is a three-dimensional velocity vector, the three-dimensional velocity vector is described by a three-dimensional position vector, and the target condition is defined as a boundary condition. 15. The four-dimensional navigation apparatus according to claim 14, wherein an arrival time of a point and a position coordinate of the target point are given, and the velocity vector is obtained by a calculator using inverse dynamics. The waypoints on the way are:
The first waypoint,
A second waypoint designated from the first waypoint to the final target point;
A third waypoint designated from the second waypoint to the final target point,
The search unit defines a proper passing point that further optimizes the evaluation function value as the proper solution, and determines a first halfway point near a first halfway point, a second halfway point near the second halfway point, and the third halfway point. The four-dimensional navigation device according to claim 14, wherein the four-dimensional navigation device finds each of the way points in the third way vicinity. The evaluation function is:
An expandability determination evaluation function indicating the expandability that expands to nodes corresponding to the final waypoint and the waypoint, respectively,
Including a target state determination evaluation function indicating the target point reachability,
The expandable determination evaluation function is represented by the following equation:
f ′ _s (n) = g (n) + h ′ (n) + Q _s (n)
The target state determination evaluation function is represented by the following equation:
f _f (n) = g (n) + h (n) + Q _f (n)
Where the subscript s indicates the neighborhood of the target point, and (n) of f ′ _s (n) indicates that the navigation body of the initial point reaches the neighborhood of the target point. G (n) indicates the first evaluation value part from the initial point to the node n, and h ′ (n) indicates the evaluation value from the node n to the target point. Indicates a second evaluation value part corresponding to the route of Q, Q _s (n) indicates a third evaluation value part corresponding to the constraint condition from the initial point to the node n, and f _f (n) indicates a value from the initial point. Indicates the evaluation value up to the target point,
17. The four-dimensional navigation device according to claim 16, wherein the proper solution is derived by optimizing the target state determination evaluation function. The four-dimensional navigation device according to claim 17, wherein the proper solution is found off-line. The search unit further comprises a heuristic function generation unit,
The route is input to the heuristic function generation unit,
18. The four-dimensional navigation device according to claim 17, wherein h '(n) is generated by the heuristic function generation unit. 20. The four-dimensional navigation device according to claim 1, wherein a route corresponding to the proper solution is output from the search unit as a feedback gain, and the feedback gain is input to the command generation unit. 21. The four-dimensional navigation apparatus according to claim 14, wherein the search unit adopts a TABU search method as a mathematical search method. 22. The four-dimensional navigation device according to claim 21, wherein the TABU search method finds an optimum solution in the small area while dividing the neighborhood into smaller small areas. Using a time and a position of two points as boundary conditions, a first navigation route between the two points of the navigation body passing through a first neighborhood position selected from each neighborhood of a plurality of points between the two points is inversely dynamic. Computing by a computer;
Calculating, by a function value calculator, a first evaluation function value of an evaluation function relating to the motion of the navigation body corresponding to the first navigation route;
Calculating, by the inverse dynamics calculator, a second navigation route between the two points of the navigation body passing through a second neighborhood position selected from the neighborhoods, using the time and the position of the two points as boundary conditions;
Calculating, by the function value calculator, a second evaluation function value of the evaluation function relating to the motion of the navigation body corresponding to the second navigation route;
Calculating a magnitude relationship between the first evaluation function value and the second evaluation function value by a comparison computer;
A four-dimensional navigation method for a navigation body, comprising the step of controlling the movement of the navigation body on a navigation route corresponding to an evaluation function value whose value is more appropriate among the first evaluation function value and the second evaluation function value.

Images