(57)【特許請求の範囲】
【請求項１】
水中航走体の複数の潜航点を入力する潜航点入力ステップと、
前記潜航点ごとの初期の目標値を設定する目標値設定ステップと、
水底地形のデータと前記目標値とを用いて前記水中航走体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記水中航走体の潜航経路をシミュレーションする潜航シミュレーションステップと、
前記潜航シミュレーションステップにおけるシミュレーションで得られた前記潜航経路と前記目標値とに基づいて算出される前記潜航経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、
更新された前記目標値を用いて前記潜航シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記潜航点ごとの最適な前記目標値を導出することを特徴とする水中航走体の経路設定方法。
【請求項２】
請求項１に記載の水中航走体の経路設定方法であって、
前記目標値は、前記潜航点における目標深度であり、
前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における深度と前記目標深度との差を示す値を前記潜航経路に亘って積算した値であることを特徴とする水中航走体の経路設定方法。
【請求項３】
請求項２に記載の水中航走体の経路設定方法であって、
前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、
前記潜航シミュレーションステップは、前記水底地形のデータと前記目標深度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標深度に対する前記水中航走体の計算潜航高度をシミュレーションし、
前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標深度を導出することを特徴とする水中航走体の経路設定方法。
【請求項４】
請求項３に記載の水中航走体の経路設定方法であって、
前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標深度を更新することを特徴とする水中航走体の経路設定方法。
【請求項５】
請求項３又は４に記載の水中航走体の経路設定方法であって、
前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、
前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標深度を更新することを特徴とする水中航走体の経路設定方法。
【請求項６】
請求項１に記載の水中航走体の経路設定方法であって、
前記目標値は、前記潜航点における目標高度であり、
前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における高度と前記目標高度との差を示す値を前記潜航経路に亘って積算した値であることを特徴とする水中航走体の経路設定方法。
【請求項７】
請求項６に記載の水中航走体の経路設定方法であって、
前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、
前記潜航シミュレーションステップは、前記水底地形のデータと前記目標高度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標高度に対する前記水中航走体の計算潜航高度をシミュレーションし、
前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標高度を導出することを特徴とする水中航走体の経路設定方法。
【請求項８】
請求項７に記載の水中航走体の経路設定方法であって、
前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標高度を更新することを特徴とする水中航走体の経路設定方法。
【請求項９】
請求項７又は８に記載の水中航走体の経路設定方法であって、
前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、
前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標高度を更新することを特徴とする水中航走体の経路設定方法。
【請求項１０】
請求項１に記載の水中航走体の経路設定方法であって、
前記目標値は、前記潜航点における前記水中航走体の目標基準姿勢であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における基準姿勢と前記目標基準姿勢との差を示す値を前記潜航経路に亘って積算した値であることを特徴とする水中航走体の経路設定方法。
【請求項１１】
請求項１０に記載の水中航走体の経路設定方法であって、
前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標基準姿勢とを用いて前記水中航走体の前記運動モデルに基づいて前記目標基準姿勢に対する前記水中航走体の計算潜航高度をシミュレーションし、
前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標基準姿勢を導出することを特徴とする水中航走体の経路設定方法。
【請求項１２】
請求項１１に記載の水中航走体の経路設定方法であって、
前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標基準姿勢を更新することを特徴とする水中航走体の経路設定方法。
【請求項１３】
請求項１１又は１２に記載の水中航走体の経路設定方法であって、
前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標基準姿勢を更新することを特徴とする水中航走体の経路設定方法。
【請求項１４】
請求項１〜１３のいずれか１項に記載の水中航走体の経路設定方法であって、
前記潜航シミュレーションステップと前記目標値更新ステップの繰り返し回数が予め定められていることを特徴とする水中航走体の経路設定方法。
【請求項１５】
請求項１に記載の水中航走体の経路設定方法であって、
前記水底地形のデータの代わりに基準経路を用いることを特徴とする水中航走体の経路設定方法。
【請求項１６】
請求項１に記載の水中航走体の経路設定方法であって、
前記水底地形のデータの代わりに、前記潜航点における前記水中航走体の基準角度を用いることを特徴とする水中航走体の経路設定方法。
【請求項１７】
請求項１〜１６に記載の水中航走体の経路設定方法で得られた最適な前記目標値を用いて前記水中航走体を制御することを特徴とする水中航走体の最適制御方法。
【請求項１８】
請求項１７に記載の水中航走体の最適制御方法であって、
最適な前記目標値と前記水中航走体の位置を比較して、比較結果に応じて前記水中航走体の運動制御系を制御することを特徴とする水中航走体の最適制御方法。
【請求項１９】
請求項１８に記載の水中航走体の最適制御方法であって、
最適な前記目標値と前記水中航走体の前記位置が所定の条件を外れた場合に前記水中航走体を緊急浮上させることを特徴とする水中航走体の最適制御方法。
【請求項２０】
請求項１７〜１９のいずれか１項に記載の水中航走体の最適制御方法であって、
さらに前記水底地形を観測する観測制御を行うことを特徴とする水中航走体の最適制御方法。
【請求項２１】
請求項１〜１６に記載の水中航走体の経路設定方法で得られた最適な前記目標値を取得する目標値取得部と、
取得された前記目標値を記憶する記憶部と、
前記記憶部に記憶された前記目標値を用いて駆動手段を制御する運動制御部を備えることを特徴とする水中航走体。
【請求項２２】
請求項２１に記載の水中航走体であって、
深度計測手段、高度計測手段、及び姿勢計測手段の少なくとも１つを備え、
前記運動制御部は、前記深度計測手段、高度計測手段、及び姿勢計測手段の少なくとも１つの測定値と前記記憶部に記憶された前記目標値とを比較して前記駆動手段を制御することを特徴とする水中航走体。
【請求項２３】
請求項２１又は２２に記載の水中航走体であって、
障害物探知手段を備え、
前記運動制御部は、前記障害物探知手段による検出結果に応じて前記駆動手段を制御することを特徴とする水中航走体。
【請求項２４】
請求項２１〜２３のいずれか１項に記載の水中航走体であって、
慣性航法手段を備え、
前記運動制御部は、前記慣性航法手段の出力に基づいて前記駆動手段を制御することを特徴とする水中航走体。
【請求項２５】
請求項２１〜２４のいずれか１項に記載の水中航走体であって、
前記水底地形を観測する観測手段を備えることを特徴とする水中航走体。
【請求項２６】
移動体の複数の移動点を入力する移動点入力ステップと、
前記移動点ごとの初期の目標値を設定する目標値設定ステップと、
地形のデータと前記目標値とを用いて前記移動体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記移動体の移動経路をシミュレーションする経路シミュレーションステップと、
前記経路シミュレーションステップにおけるシミュレーションで得られた前記移動経路と前記目標値とに基づいて算出される前記移動経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、
更新された前記目標値を用いて前記経路シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記移動点ごとの最適な前記目標値を導出することを特徴とする移動体の経路設定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、水中航走体の経路設定方法、それを用いた水中航走体の最適制御方法及び水中航走体並びに移動体の経路設定方法に関する。
【背景技術】
【０００２】
近年、鉱物資源をはじめとするあらゆる資源の供給源として海底等の水底が注目を集めている。それに伴って、水底調査の必要性も高まりつつある。水底の調査には、音波、レーザ、可視光線、電磁波等の信号を媒介とするリモートセンシングの手法が一般に用いられるが、こうした信号には伝搬損失が発生するため、できるだけ水底に近い位置で用いることにより高解像度及び高精度の情報が得られる。そこで、水底付近を所望する軌道や姿勢で水中航走体（例えば潜水艇）を航行させる技術が必要とされている。
【０００３】
水中航走体の深度を設定する深度設定手段を備え、設定された所定の深度に基づいて船尾蛇を駆動制御する技術が開示されている（特許文献１）。ここで、深度設定手段と深度制御手段との間にリミット回路が設けられ、リミット回路によって水中航走体が水底に接触しないように制御される。
【０００４】
水中航走体の自動操縦を行うに当って、運動状態量を推定するために、水中航走体の運動特性に追従する運動モデルを用いて運動状態量の推定精度を向上させた技術が開示されている（特許文献２）。また、潮流等の影響を考慮して、水中航走体の操舵制御を行う技術が開示されている（特許文献３）。
【０００５】
また、水中航走体を航行制御するためには水中航走体の正確な位置を取得する必要があるが、マルチビーム測深器で深度を複数の位置について計測することによって水中航走体の位置を特定する測位技術が開示されている（特許文献４）。水中航走体の速度、深度、高度、姿勢角等の状態情報を取得し、状態情報に基づき安全に航行できる安全域を設定し、安全域内を航行できるように水中航走体を制御する技術が開示されている（特許文献５）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開昭６３−２７３７９７号公報
【特許文献２】特開平０５−０１６８７８号公報
【特許文献３】特開２００３−１２７９８３号公報
【特許文献４】特開２００７−２９２７２９号公報
【特許文献５】特開２０１５−０６３１８１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
音波等の探査信号の反射強度は水底への入射角に反比例する性質がある。すなわち、水底に対して浅い入射（大きな入射角）では垂直入射（０度入射）に比べて反射強度が格段と弱まる。したがって、起伏に富んだ複雑な水底地形の場合には反射信号の強度が地形と相関し、平坦な面である場合に比べて弱くなる。さらに、複雑な水底地形では平らな水底面と比べてマルチパスの問題も顕著となる。その結果、高度計による高度計測値の信頼性も低下する。
【０００８】
こうした事情から、航行安全のために、平坦な水底のように高度の情報が安定的に高い精度で得られる場合を除いて、高度制御航法が採用されることは稀であり、深度制御航法が多く用いられている。
【０００９】
現状、深度制御航法の適用の際は航走体が水底から適切な距離で離れるように目標深度を設定し、水底への衝突を防ぐように制御している。ところが、深度設定では水底地形とＡＵＶダイナミクスとの関係が定量的に評価されておらず、険しい水底地形等において水底衝突のおそれがある。一方、水底への衝突を避けるために水底から十分な距離で離れるように目標深度を設定すると、水底から取得できるデータの質と解像度は著しく低下する。また、水底調査に用いられるセンサ類に応じて水中航走体を特定の姿勢、例えば水底に平行の姿勢をとることが好ましい場合もある。
【００１０】
また、これらの課題は、水中航走体のみならずドローン等の空中を飛行する移動体についても同様である。
【００１１】
上記した引用文献１から５は、いずれも潜航点を決めて、水中航走体の水底への衝突を防ぎつつ、最も近づくことができる最適な潜航経路を得ているものではない。
【００１２】
本発明は、上記課題の少なくとも一つを解決するために、水中航走体や移動体に対して最適な経路を設定して航行制御や移動制御することを可能とする水中航走体の経路設定方法、それを用いた水中航走体の最適制御方法及び水中航走体並びに移動体の経路設定方法を提供することを目的とする。
【課題を解決するための手段】
【００１３】
請求項１に対応した水中航走体の経路設定方法は、水中航走体の複数の潜航点を入力する潜航点入力ステップと、前記潜航点ごとの初期の目標値を設定する目標値設定ステップと、水底地形のデータと前記目標値とを用いて前記水中航走体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記水中航走体の潜航経路をシミュレーションする潜航シミュレーションステップと、前記潜航シミュレーションステップにおけるシミュレーションで得られた前記潜航経路と前記目標値とに基づいて算出される前記潜航経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、更新された前記目標値を用いて前記潜航シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記潜航点ごとの最適な前記目標値を導出する。
【００１４】
ここで、前記目標値は、前記潜航点における目標深度であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における深度と前記目標深度との差を示す値を前記潜航経路に亘って積算した値であることが好適である。
【００１５】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標深度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標深度に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標深度を導出することが好適である。
【００１６】
また、前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標深度を更新することが好適である。
【００１７】
また、前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標深度を更新することが好適である。
【００１８】
また、前記目標値は、前記潜航点における目標高度であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における高度と前記目標高度との差を示す値を前記潜航経路に亘って積算した値であることが好適である。
【００１９】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標高度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標高度に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標高度を導出することが好適である。
【００２０】
また、前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標高度を更新することが好適である。
【００２１】
また、前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標高度を更新することが好適である。
【００２２】
ここで、前記目標値は、前記潜航点における前記水中航走体の目標基準姿勢であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における基準姿勢と前記目標基準姿勢との差を示す値を前記潜航経路に亘って積算した値であることが好適である。
【００２３】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標基準姿勢とを用いて前記水中航走体の前記運動モデルに基づいて前記目標基準姿勢に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標基準姿勢を導出することが好適である。
【００２４】
また、前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標基準姿勢を更新することが好適である。
【００２５】
また、前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標基準姿勢を更新することが好適である。
【００２６】
また、前記潜航シミュレーションステップと前記目標値更新ステップの繰り返し回数が予め定められていることが好適である。
【００２７】
また、前記水底地形のデータの代わりに基準経路を用いることが好適である。また、前記水底地形のデータの代わりに前記潜航点における前記水中航走体の基準姿勢を用いることが好適である。また、前記水底地形のデータの代わりに前記潜航点における前記水中航走体の基準角度を用いることが好適である。
【００２８】
請求項１７に記載の水中航走体の最適制御方法は、上記水中航走体の経路設定方法で得られた最適な前記目標値を用いて前記水中航走体を制御する。
【００２９】
ここで、最適な前記目標値と前記水中航走体の位置を比較して、比較結果に応じて前記水中航走体の運動制御系を制御することが好適である。
【００３０】
また、最適な前記目標値と前記水中航走体の前記位置が所定の条件を外れた場合に前記水中航走体を緊急浮上させることが好適である。
【００３１】
また、さらに前記水底地形を観測する観測制御を行うことが好適である。
【００３２】
請求項２１に記載の水中航走体は、上記水中航走体の経路設定方法で得られた最適な前記目標値を取得する目標値取得部と、取得された前記目標値を記憶する記憶部と、前記記憶部に記憶された前記目標値を用いて駆動手段を制御する運動制御部を備える。
【００３３】
ここで、深度計測手段、高度計測手段、及び姿勢計測手段の少なくとも１つを備え、前記運動制御部は、前記深度計測手段、高度計測手段、及び姿勢計測手段の少なくとも１つの測定値と前記記憶部に記憶された前記目標値とを比較して前記駆動手段を制御することが好適である。
【００３４】
また、障害物探知手段を備え、前記運動制御部は、前記障害物探知手段による検出結果に応じて前記駆動手段を制御することが好適である。
【００３５】
また、慣性航法手段を備え、前記運動制御部は、前記慣性航法手段の出力に基づいて前記駆動手段を制御することが好適である。
【００３６】
また、前記水底地形を観測する観測手段を備えることが好適である。
【００３７】
請求項２６に記載の移動体の経路設定方法は、移動体の複数の移動点を入力する移動点入力ステップと、前記移動点ごとの初期の目標値を設定する目標値設定ステップと、地形のデータと前記目標値とを用いて前記移動体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記移動体の移動経路をシミュレーションする経路シミュレーションステップと、前記経路シミュレーションステップにおけるシミュレーションで得られた前記移動経路と前記目標値とに基づいて算出される前記移動経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、更新された前記目標値を用いて前記経路シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記移動点ごとの最適な前記目標値を導出する。
【発明の効果】
【００３８】
請求項１に対応した水中航走体の経路設定方法によれば、水中航走体の複数の潜航点を入力する潜航点入力ステップと、前記潜航点ごとの初期の目標値を設定する目標値設定ステップと、水底地形のデータと前記目標値とを用いて前記水中航走体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記水中航走体の潜航経路をシミュレーションする潜航シミュレーションステップと、前記潜航シミュレーションステップにおけるシミュレーションで得られた前記潜航経路と前記目標値とに基づいて算出される前記潜航経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、更新された前記目標値を用いて前記潜航シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記潜航点ごとの最適な前記目標値を導出することによって、前記潜航点において水底地形に合わせて最適化された最適な目標値を設定することができる。これにより、前記潜航点において前記水中航走体を最適な目標値で潜航させることができ、目的に応じた最適な潜航状態での航行を実現することができる経路設定が可能となる。
【００３９】
ここで、前記目標値は、前記潜航点における目標深度であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における深度と前記目標深度との差を示す値を前記潜航経路に亘って積算した値であることによって、前記潜航点において水底地形に合わせて最適化された最適目標深度を設定することができる。これにより、前記潜航点において前記水中航走体を最適目標深度となるように潜航させることができ、観測等の目的に応じた最適な潜航深度での航行を実現することができる経路設定が可能となる。
【００４０】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標深度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標深度に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標深度を導出することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点において前記水中航走体を最適目標深度となるように潜航させることができる経路設定が可能となる。
【００４１】
また、前記目標値更新ステップは、前記計算潜航高度と前記最小許容高度に差がある場合に、前記計算潜航高度の最小値が前記最小許容高度に近づくように前記目標深度を更新することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点においてできるだけ水底に近づくように最適目標深度を設定することができる。
【００４２】
また、前記評価関数は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数が小さくなるように前記目標深度を更新することによって、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和が最も小さくなるように複数の前記潜航点において最適目標深度を設定することができる。
【００４３】
また、前記目標値は、前記潜航点における目標高度であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における高度と前記目標高度との差を示す値を前記潜航経路に亘って積算した値であることによって、前記潜航点において水底地形に合わせて最適化された最適目標高度を設定することができる。これにより、前記潜航点において前記水中航走体を最適目標高度となるように潜航させることができ、観測等の目的に応じた最適な潜航高度での航行を実現することができる経路設定が可能となる。
【００４４】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標高度とを用いて前記水中航走体の前記運動モデルに基づいて前記目標高度に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標高度を導出することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点において前記水中航走体を最適目標高度となるように潜航させることができる経路設定が可能となる。
【００４５】
また、前記目標値更新ステップは、前記計算潜航高度と前記最小許容高度に差がある場合に、前記計算潜航高度の最小値が前記最小許容高度に近づくように前記目標高度を更新することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点においてできるだけ水底に近づくように最適目標高度を設定することができる。
【００４６】
また、前記評価関数は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数が小さくなるように前記目標高度を更新することによって、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和が最も小さくなるように複数の前記潜航点において最適目標高度を設定することができる。
【００４７】
ここで、前記目標値は、前記潜航点における前記水中航走体の目標基準姿勢であり、前記評価関数の値は、前記シミュレーションで得られた前記潜航経路における基準姿勢と前記目標基準姿勢との差を示す値を前記潜航経路に亘って積算した値であることによって、前記潜航点において水底地形に合わせて最適化された最適目標基準姿勢を設定することができる。これにより、前記潜航点において前記水中航走体を最適目標基準姿勢となるように潜航させることができ、観測等の目的に応じた最適な潜航姿勢での航行を実現することができる経路設定が可能となる。
【００４８】
また、前記水中航走体の最小許容高度を設定する最小許容高度設定ステップを備え、前記潜航シミュレーションステップは、前記水底地形のデータと前記目標基準姿勢とを用いて前記水中航走体の前記運動モデルに基づいて前記目標基準姿勢に対する前記水中航走体の計算潜航高度をシミュレーションし、前記計算潜航高度と前記最小許容高度を比較することによって最適な前記目標基準姿勢を導出することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点において前記水中航走体を最適目標基準姿勢となるように潜航させることができる経路設定が可能となる。
【００４９】
また、前記目標値更新ステップは、前記計算潜航高度の最小値と前記最小許容高度に差がある場合に、前記計算潜航高度の前記最小値が前記最小許容高度に近づくように前記目標基準姿勢を更新することによって、前記水中航走体が前記最小許容高度よりも水底に近づくことを避けつつ、前記潜航点においてできるだけ水底に近づくように最適目標基準姿勢を設定することができる。
【００５０】
また、前記評価関数の値は、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和とし、前記目標値更新ステップは、前記評価関数の値が小さくなるように前記目標基準姿勢を更新することによって、複数の前記潜航点における前記計算潜航高度と前記最小許容高度との差の絶対値の総和、もしくは差の二乗の総和が最も小さくなるように複数の前記潜航点において最適目標基準姿勢を設定することができる。
【００５１】
また、前記潜航シミュレーションステップと前記目標値更新ステップの繰り返し回数が予め定められていることによって、潜航シミュレーションを適切な回数で切り上げることができる。
【００５２】
また、前記水底地形のデータの代わりに基準経路を用いること、又は、前記水底地形のデータの代わりに前記潜航点における前記水中航走体の基準角度を用いることによって、前記潜航点において前記水中航走体が基準経路又は基準角度に近づくように前記目標値を最適化することができる。
【００５３】
請求項１７に記載の水中航走体の最適制御方法は、上記水中航走体の経路設定方法で得られた最適な前記目標値を用いて前記水中航走体を制御することによって、前記潜航点において水底地形に合わせて最適化された最適な目標値で前記水中航走体を潜航させることができ、目的に応じた最適な潜航状態での航行を実現することができる。
【００５４】
ここで、最適な前記目標値と前記水中航走体の位置を比較して、比較結果に応じて前記水中航走体の運動制御系を制御することによって、前記水中航走体の最適制御方法を具体的に実現することができる。
【００５５】
また、最適な前記目標値と前記水中航走体の前記位置が所定の条件を外れた場合に前記水中航走体を緊急浮上させることによって、前記水中航走体が障害物等に衝突しそうな場合等において危険を回避することができる。
【００５６】
また、さらに前記水底地形を観測する観測制御を行うことによって、観測に適した潜航状態で航行しつつ観測を行うことができる。
【００５７】
請求項２１に記載の水中航走体は、上記水中航走体の経路設定方法で得られた最適な前記目標値を取得する目標値取得部と、取得された前記目標値を記憶する記憶部と、前記記憶部に記憶された前記目標値を用いて駆動手段を制御する運動制御部を備えることによって、前記潜航点において水底地形に合わせて最適化された最適な目標値で潜航することができ、目的に応じた最適な潜航状態での航行を実現することができる。
【００５８】
ここで、深度測手段、高度計測手段、及び姿勢計測手段の少なくとも１つを備え、前記運動制御部は、前記深度計測手段、高度計測手段、及び姿勢計測手段の少なくとも１つの測定値と前記記憶部に記憶された前記目標値とを比較して前記駆動手段を制御することによって、最適な前記目標値を得る前記水中航走体の最適制御方法を具体的に実現することができる。
【００５９】
また、障害物探知手段を備え、前記運動制御部は、前記障害物探知手段による検出結果に応じて前記駆動手段を制御することによって、前記水中航走体が障害物等に衝突しそうな場合等において危険を回避することができる。
【００６０】
また、慣性航法手段を備え、前記運動制御部は、前記慣性航法手段の出力に基づいて前記駆動手段を制御することによって、外部から位置情報を取得することなく、自機のみによって前記潜航点において水底地形に合わせて最適化された最適な目標値で潜航することができる。
【００６１】
また、前記水底地形を観測する観測手段を備えることによって、観測に適した潜航状態で航行しつつ観測を行うことができる。
【００６２】
請求項２６に記載の移動体の経路設定方法は、移動体の複数の移動点を入力する移動点入力ステップと、前記移動点ごとの初期の目標値を設定する目標値設定ステップと、地形のデータと前記目標値とを用いて前記移動体の応答特性及び移動特性を考慮した運動方程式からなる運動モデルに基づいて前記目標値に対する前記移動体の移動経路をシミュレーションする経路シミュレーションステップと、前記経路シミュレーションステップにおけるシミュレーションで得られた前記移動経路と前記目標値とに基づいて算出される前記移動経路に亘る評価関数の値に基づいて前記目標値を更新する目標値更新ステップとを備え、更新された前記目標値を用いて前記経路シミュレーションステップと前記目標値更新ステップを繰り返すことによって複数の前記移動点ごとの最適な前記目標値を導出することによって、前記移動点において地形に合わせて最適化された最適な目標値を設定することができる。これにより、水中航走体以外の移動体においても前記移動点において最適な目標値で移動させることができ、目的に応じた最適な状態での移動を実現することができる経路設定が可能となる。
【図面の簡単な説明】
【００６３】
【図１】本発明の実施の形態における水中航走体の構成概念図である。
【図２】本発明の実施の形態における水中航走体の経路設定装置の構成を示す図である。
【図３】本発明の実施の形態における水中航走体の経路設定方法のフローチャートである。
【図４】本発明の実施の形態における水中航走体の経路設定装置の機能ブロック図である。
【図５】本発明の実施の形態における水中航走体の経路設定処理を説明する図である。
【図６】本発明の実施の形態における水中航走体の最適航行制御のフローチャートである。
【図７】本発明の実施の形態における水中航走体の機能ブロック図である。
【発明を実施するための形態】
【００６４】
＜水中航走体の構成＞
本発明の実施の形態における水中航走体１００は、図１の構成概念図に示すように、艇体１０、主推進器１２、主推進器駆動モータ１４、垂直舵１６、垂直舵駆動モータ１８、水平舵２０、水平舵駆動モータ２２、深度計２４、高度計２６、障害物センサ２８、観測用センサ３０、慣性航法装置３２、制御装置３４、音響通信装置３６及び音響測位装置３８を含んで構成される。水中航走体１００は、例えば、自律型無人潜水機（ＡＵＶ）であるが、これに限定されるものではない。
【００６５】
艇体１０は、艇室等の空間を構成する密閉可能な構造体である。艇体１０は、金属や強化プラスチック等により構成され、水中航走体１００の構成要素を機械的に支持する役割も果たす。
【００６６】
主推進器１２は、水中航走体１００を推進させるための駆動力を発生させる構成要素である。主推進器１２は、例えば、プロペラ、回転軸等を含んで構成される。主推進器駆動モータ１４は、主推進器１２に対して駆動力を与えるためのモータである。主推進器駆動モータ１４は、制御装置３４からの駆動制御信号を受けて、電池１４ａからの電力によって駆動制御信号に応じた回転数及びトルクで主推進器１２の回転軸を回転駆動させる。これにより、駆動軸に接続されたプロペラが回転されて、水中航走体１００に推進力が与えられる。
【００６７】
垂直舵１６は、艇体１０を左右方向に旋回（回頭）させるための舵である。垂直舵１６を艇体１０に対して右又は左に傾けることによって、艇体１０を左又は右に回頭させることができる。垂直舵駆動モータ１８は、垂直舵１６を回転させるための駆動力を発生させるモータである。垂直舵駆動モータ１８は、制御装置３４からの垂直蛇制御信号を受けて、垂直蛇制御信号に応じた角度になるように垂直舵１６を回転駆動させる。
【００６８】
なお、左右にそれぞれ個別の主推進器１２を設けておき、垂直舵１６に依らず、左右の主推進器１２の推力のバランスを調整することにより艇体１０を左右方向に旋回（回頭）させる構成としてもよい。
【００６９】
水平舵２０は、艇体１０を上昇及び下降させるための舵である。水平舵２０を艇体１０に対して上又は下に傾けることによって、艇体１０を頭下げ（ピッチダウン）又は頭上げ（ピッチアップ）させることができる。水平舵駆動モータ２２は、水平舵２０を回転させるための駆動力を発生させるモータである。水平舵駆動モータ２２は、制御装置３４からの水平蛇制御信号を受けて、水平蛇制御信号に応じた角度になるように水平舵２０を回転駆動させる。
【００７０】
深度計２４は、水中航走体１００の水面からの距離（深度）を測定して出力する装置である。深度計２４は、例えば、水中航走体１００の艇体１０に掛かる水圧から深度を測定する。深度計２４は、測定された深度を制御装置３４へ出力する。高度計２６は、水中航走体１００の水底からの距離（高度）を測定して出力する装置である。高度計２６は、例えば、艇体１０の下方に向けて音波を出力し、水底で反射した音波を受信するまでの時間から高度を測定する。高度計２６は、測定された高度を制御装置３４へ出力する。
【００７１】
障害物センサ２８は、水中航走体１００の進行方向（前方）に存在する障害物を検知するためのセンサである。障害物センサ２８は、例えば、艇体１０の前方（又は前方やや下方向）に向けて音波を出力し、その方向に存在する障害物で反射した音波を受信するまでの時間から障害物の存在及びそこまでの距離を測定する。障害物センサ２８は、障害物の検知結果を制御装置３４へ出力する。
【００７２】
観測用センサ３０は、水中航走体１００によって行われる観測のためのセンサである。観測用センサ３０は、観測の目的に合わせて様々なセンサとすることができる。観測用センサ３０は、例えば、音波センサ、電磁波センサ、光センサ、圧力センサ、熱センサ等とすることができる。観測用センサ３０は、観測結果を制御装置３４へ出力する。
【００７３】
慣性航法装置３２は、艇体１０の移動の際の加速度を測定する加速度センサ及びジャイロセンサを備え、これらで測定された加速度を時間で２回積分することによって艇体１０の位置情報を求める。制御装置３４で得られた水中航走体１００の位置情報は制御装置３４へ出力される。
【００７４】
制御装置３４は、深度計２４、高度計２６、障害物センサ２８、観測用センサ３０及び慣性航法装置３２から情報を受けて、これらの情報に基づいて水中航走体１００の航行制御を行う。制御装置３４は、慣性航法装置３２からの位置情報、深度計２４からの深度、高度計２６からの高度の情報に基づいて、予め設定された潜航点及び後述する経路設定方法により設定された目標値（目標深度、目標高度、目標姿勢、目標角度等）に艇体１０が近づくように主推進器１２、垂直舵駆動モータ１８及び水平舵駆動モータ２２を制御する。制御装置３４による航行制御については後述する。
【００７５】
音響通信装置３６は、水中航走体１００の外部と音波によって通信するための装置である。音響通信装置３６は、制御装置３４から深度計２４、高度計２６、障害物センサ２８、観測用センサ３０及び慣性航法装置３２の測定結果や航行に関する情報等を受けて、それらの情報を水中航走体１００の外部（例えば、海上の船舶等）に送信する。また、外部（例えば、海上の船舶等）においてＧＰＳ等によって測定された絶対位置を受信する。
【００７６】
音響測位装置３８は、水中航走体１００の外部の船舶等との相対位置を受信する。音響測位装置３８は、海上の船舶等に設けられたと水中航走体１００の相対位置の情報を取得する。この相対位置情報は、制御装置３４へ入力される。制御装置３４は、音響通信装置３６によって取得された海上の船舶等の絶対位置の情報も取得し、船舶の絶対位置と船舶からの水中航走体１００の相対位置から水中航走体１００の絶対位置を得ることができる。制御装置３４は、このようにして得られた水中航走体１００の絶対位置情報によって慣性航法装置３２から取得した位置情報を補正することができる。例えば、制御装置３４は、音響測位装置３８から取得した相対位置情報から算出された位置と慣性航法装置３２から取得した位置の差が所定値よりも大きくなった場合に音響測位装置３８の位置によって慣性航法装置３２の位置を補正する。慣性航法装置３２は、補正された位置を新たな初期値として水中航走体１００の位置の推定を続ける。
【００７７】
＜水中航走体の経路設定方法＞
本実施の形態における水中航走体１００の経路設定処理について以下に説明する。本実施の形態では、水中航走体１００とは別の経路設定装置２００にて経路設定を行う態様について説明する。ただし、これに限定されるものではなく、水中航走体１００に経路設定装置２００を搭載してもよい。
【００７８】
経路設定装置２００は、図２に示すように、処理部４０、記憶部４２、入力部４４、出力部４６を備えるコンピュータである。処理部４０は、ＣＰＵ等から構成される。処理部４０は、記憶部４２に予め記憶されている経路設定プログラムを実行することにより、後述する経路設定処理を行う。記憶部４２は、経路設定プログラム、艇体運動計算モデル、各種データを記憶するための記憶装置である。記憶部４２は、例えば、半導体メモリ、ハードディスク等とすることができる。入力部４４は、経路設定装置２００に経路設定処理に必要な情報を入力するための装置を備える。入力部４４は、キーボード、マウス等とすることができる。また、入力部４４をネットインターフェース等として他の装置から情報を取得するようにしてもよい。出力部４６は、経路設定装置２００によって得られた経路等の情報を出力するための装置を備える。出力部４６は、ディスプレイ、プリンタ等とすることができる。また、出力部４６をネットインターフェース等として、水中航走体１００の制御装置３４に情報を出力できるようにしてもよい。
【００７９】
経路設定処理は、図３のフローチャートに沿って処理される。経路設定処理を実行することによって、経路設定装置２００は、図４の機能ブロック図に示す各手段として機能する。
【００８０】
ステップＳ１０では、初期設定が行われる。当該処理によって、経路設定装置２００は初期設定手段５０として機能する。初期設定では、入力部４４を用いて、水底地形の情報の入力、最大繰り返し回数（Ｎｅ）の設定及び最小許容高度（ｈａ）の設定が行われる。水底地形の情報は、図５の太実線で示すように、水中航走体１００の航行対象となる領域の水底の座標毎の地形の高低を表す情報である。最大繰り返し回数（Ｎｅ）は、経路設定処理における繰り返しの回数の最大値である。最小許容高度（ｈａ）は、水中航走体１００が水底に接触しないような経路を設定するための水底からの最小の許容高度である。
【００８１】
ステップＳ１２では、潜航点の入力処理が行われる。当該処理によって、経路設定装置２００は潜航点設定手段５２として機能する。潜航点は、平面内（水面、海面に対して平行な面）において水中航走体１００が航行する経路上を離散的な座標点として示した情報である。すなわち、潜航点を順に繋ぐとによって水中航走体１００の二次元の航行経路が得られる。潜航点は、予め観測計画等に基づいた全潜航経路計画によって設定される。
【００８２】
ステップＳ１４では、各潜航点における初期の目標値が設定される。当該処理によって、経路設定装置２００は目標値設定手段５４として機能する。本実施の形態では、目標値は、水中航走体１００の目標深度とする。すなわち、図５に示すように、ステップＳ１２で入力された潜航点毎に水中航走体１００の航行する水中の経路の目標となる深度の初期値（図中、白三角で示す）が設定される。
【００８３】
ステップＳ１６では、艇体運動モデルが設定される。当該処理によって、経路設定装置２００は艇体運動モデル設定手段５６として機能する。艇体運動モデルは、ＡＵＶダイナミクスとも呼ばれ、水中における艇体１０の運動性能を表す運動方程式からなる。具体的には、主推進器１２、垂直舵１６、水平舵２０等の応答特性や艇体１０の移動特性等に基づいて決められる。
【００８４】
ステップＳ１８〜Ｓ３２において、水底地形のデータと目標深度とを用いて水中航走体１００の運動モデルに基づいて目標値に対する水中航走体１００の潜航経路をシミュレーションする潜航シミュレーションと、潜航シミュレーションにおいて得られた潜航経路に基づいて算出される評価関数に基づいて目標深度を更新する目標値更新処理を繰り返して各潜航点における目標値（目標深度）の最適化解析が行われる。
【００８５】
ステップＳ１８では、カウンタｉｅが初期値０に設定される。カウンタｉｅは、シミュレーションの繰り返し回数を数えるため用いられる。ステップＳ２０では、カウンタｉｅに１が加算される。
【００８６】
ステップＳ２２では、潜航シミュレーションが行われる。当該処理によって、経路設定装置２００は目標値追従シミュレーション実施部５８として機能する。処理部４０は、ステップＳ１０で入力された水底地形の情報、ステップＳ１２で入力された潜航点及び現在の各潜航点の目標深度を用いて、ステップＳ１６で設定された艇体運動モデルをもとに艇体１０が各潜航点において水底地形に近づくように水中航走体１００の潜航経路をシミュレーションする。また、艇体１０が最小許容高度（ｈａ）により近づくように水中航走体１００の潜航経路をシミュレーションしてもよい。シミュレーションによって、水中航走体１００の新たな潜航経路、すなわち各潜航点における潜航深度（水面から艇体１０までの距離：以下、計算潜航深度という）及び潜航高度（水底から艇体１０までの距離：以下、計算潜航高度という）が得られる。
【００８７】
ステップＳ２４では、ステップＳ２２におけるシミュレーションで得られた新たな潜航経路での各潜航点における水底からの距離（計算潜航高度）が最小許容高度（ｈａ）より大きいか否かが判定される。当該処理によって、経路設定装置２００は潜航高度確認手段６０として機能する。最小許容高度（ｈａ）より計算潜航高度が低い潜航点が存在する場合、シミュレーションを終了して、ステップＳ３０に処理を移行させる。最小許容高度（ｈａ）より計算潜航高度が低い潜航点が存在しない場合、ステップＳ２６に処理を移行させる。
【００８８】
ステップＳ２６では、カウンタｉｅが最大繰り返し回数（Ｎｅ）以下か否かが判定される。当該処理によって、経路設定装置２００は繰り返し回数確認手段６２として機能する。カウンタｉｅが最大繰り返し回数（Ｎｅ）以下であればステップＳ２８に処理を移行させ、そうでなければステップＳ３０に処理を移行させる。
【００８９】
ステップＳ２８では、評価関数に基づく最適化手法を適用し、各潜航点の目標深度を更新する。当該処理によって、経路設定装置２００は目標値更新手段６４として機能する。評価関数は、水中航走体１００の航行における具体的な目標に対して設定される。例えば、艇体１０をなるべく水面から深く潜水させたい場合は全潜航経路における艇体１０の計算潜航深度の総和を評価関数とする。そして、処理部４０は、評価関数の値ができるだけ大きくなるように各潜航点における目標値を更新する。その後、ステップＳ２０に処理を戻し、新たな目標値を用いて再び潜航経路のシミュレーションを繰り返す。
【００９０】
ステップＳ３０に移行した場合、最適目標値（最適目標深度）を設定する。当該処理によって、経路設定装置２００は最適目標値設定手段６６として機能する。ステップＳ３２では、処理部４０は、図５に示すように、現在設定されている目標深度を最適目標深度（図中、黒丸として示す）として設定して出力する。当該処理によって、経路設定装置２００は最適目標値出力手段６８として機能する。最適目標深度は、水中航走体１００が潜航する際に、水底に接触しないように最小許容高度（ｈａ）を維持しつつ、できるだけ水面から深い潜航経路を取るための各潜航点における目標となる深度である。
【００９１】
以上の処理により、水中航走体１００の航行制御を行う際の各潜航点における目標となる深度（目標深度）が最適化される。したがって、各潜航点において当該最適目標深度となるように水中航走体１００の運動制御系を制御することによって、水中航走体１００を適切な潜航経路で潜航させることができる。
【００９２】
なお、艇体１０をなるべく水底に接近させたい場合には、ステップＳ２８において、全潜航経路における艇体１０の計算潜航高度の総和を評価関数とすればよい。そして、処理部４０は、当該評価関数の値ができるだけ小さくなるように各潜航点における目標値を更新する。その後、ステップＳ２０に処理を戻し、新たな目標値を用いて再び潜航経路のシミュレーションを繰り返す。そして、ステップＳ３０に移行した場合、最適目標値（最適目標高度）を設定する。
【００９３】
以上の処理により、水中航走体１００の航行制御を行う際の各潜航点における目標となる深度（目標高度）が最適化される。したがって、各潜航点において当該最適目標高度となるように水中航走体１００の運動制御系を制御することによって、水中航走体１００を適切な潜航経路で潜航させることができる。
【００９４】
また、本実施の形態では、評価関数を全潜航経路における艇体１０の深度や高度の総和としたがこれに限定されるものではない。例えば、水底地形に基づいて艇体１０を水底に近づける代わりに、予め定められた基準経路（基準深度や基準高度）に近づけたい場合、全潜航経路における艇体１０の深度と基準経路（基準深度や基準高度）との差の絶対値の総和、もしくは差の二乗の総和を評価関数とし、当該評価関数に基づいて目標値（目標深度や目標高度）を最適化すればよい。
【００９５】
また、艇体１０の姿勢を制御したい場合、各潜航点における姿勢（例えば、水底の傾斜角度に対して艇体１０を平行にする艇体角度）を基準姿勢として予め設定しておき、ステップＳ２８において、全潜航経路における艇体１０の姿勢（艇体角度）と基準姿勢との差の絶対値の総和、もしくは差の二乗の総和を評価関数とすればよい。そして、処理部４０は、当該評価関数の値ができるだけ小さくなるように各潜航点における目標値を更新する。そして、ステップＳ３０に移行した場合、最適目標値を設定する。
【００９６】
＜水中航走体の最適航行制御＞
以下、上記水中航走体１００の経路設定処理にて設定された最適目標値に基づいた水中航走体１００の航行制御について説明する。
【００９７】
航行制御処理は、図６のフローチャートに沿って処理される。水中航走体１００の各部は、図７の機能ブロック図に示す各手段として機能して航行制御が実現される。
【００９８】
ステップＳ４０では、最適目標値（最適目標深度）が入力設定される。当該処理によって、水中航走体１００の制御装置３４は最適目標値入力部７０として機能する。制御装置３４の入力手段（図示しない）によって上記水中航走体１００の経路設定方法により設定された最適目標値（最適目標深度）が制御装置３４に入力され、制御装置３４の記憶部７２に記憶される。ここで、水中航走体１００への入力は、制御装置３４に付設した入力装置を用いてもよいし、音響通信装置３６等の外部インターフェースを用いてもよい。
【００９９】
水中航走体１００として自律型無人潜水機（ＡＵＶ）に適用する場合、予め求めた最適目標値（最適目標深度）や条件を、海上の船舶等で水中航走体１００に入力し、海中に投入して自律航走させることもできる。予め求めた最適目標値（最適目標深度）を入力して水中航走体１００の制御を行う利点は、潜航経路をシミュレーションを繰り返して行う時間の遅れに左右されることなく、水中航走体１００を的確に制御できる点である。
【０１００】
ステップＳ４２では、慣性航法装置３２及び音響通信装置３６による水中航走体１００の測位が行われる。制御装置３４は、慣性航法装置３２によって求められた水中航走体１００の位置情報を取得する。当該処理によって、水中航走体１００は自機運動検知部７６として機能する。制御装置３４は、上記のように、音響通信装置３６から得られた位置情報により慣性航法装置３２によって求められた水中航走体１００の位置情報を補正して用いるようにしてもよい。当該処理によって、水中航走体１００は自機位置計算部７８として機能する。
【０１０１】
ステップＳ４４では、水中航走体１００の水中航行の制御が行われる。制御装置３４は、ステップＳ４２において取得した位置情報を用いて、現在の水中航走体１００の位置から各潜航点における最適目標値（最適目標深度）を順に追従するように水中航走体１００を航行制御する。制御装置３４は、主推進器駆動モータ１４、垂直舵駆動モータ１８、水平舵駆動モータ２２に対して駆動制御信号、垂直蛇制御信号及び水平蛇制御信号をそれぞれ出力して駆動制御し、主推進器１２、垂直舵１６及び水平舵２０を動かして艇体１０の挙動を制御する。また、慣性航法装置３２の加速度センサやジャイロセンサによって艇体１０の運動を検知して艇体１０の挙動から位置情報を求められるようにする。このような処理によって、制御装置３４は、艇体１０を各潜航点において最適目標値（最適目標深度）となるように水中航走体１００の運動を制御する。当該処理によって、水中航走体１００は艇体運動制御部７４として機能する。
【０１０２】
ステップＳ４６では、観測用センサ３０によって必要な観測が行われる。すなわち、水中航走体１００の潜航の目的である、例えば水底地形の観測が実施される。制御装置３４は、音響通信装置３６を介して観測結果を艇体１０の外部へ送信するようにしてもよい。当該処理によって、水中航走体１００は観測行動実施部８０として機能する。
【０１０３】
ステップＳ４８では、緊急浮上が必要か否かが判定される。制御装置３４は、障害物センサ２８によって水中航走体１００の航行の障害となる障害物が検知された場合、ステップＳ５０に処理を移行させて艇体１０を緊急浮上させて潜航を終了させる。一方、障害物が検知されていない場合、処理をステップＳ５２に移行させる。当該処理により、水中航走体１００は緊急浮上判断部８２として機能する。なお、緊急浮上は音響通信装置３６を介し、海上の船舶等からの指令により行うこともできる。
【０１０４】
ステップＳ５２では、最終の潜航点に到達したか否かが判定される。制御装置３４は、予め設定された潜航点のうち潜航経路の最終地点となる潜航点まで艇体１０が到達したか否かを判定し、到達していればステップＳ５４に処理を移行させて、水中航走体１００を通常浮上させて潜航を終了させる。最中地点となる潜航点まで艇体１０が到達していなければ、ステップＳ４４に処理を戻して次の潜航点に向けての航行制御を継続する。
【０１０５】
以上のように、本実施の形態における水中航走体の経路設定方法によって設定された潜航経路（各潜航点における目標値）を実現するように水中航走体１００の航行制御を行うことによって、水中航走体１００を目的に適した潜航経路や潜航姿勢で航行させることができる。これにより、水中航走体１００による観測等を最適な状態で実現することができる。
【産業上の利用可能性】
【０１０６】
本発明は、水中航走体の経路設定及び航行制御のみならず、地形のデータや基準姿勢等の予め設定された条件に応じた移動体の移動経路や移動制御に適用することができる。例えば、航空機、ドローンの移動経路の設定や移動制御に適用することができる。
【符号の説明】
【０１０７】
１０  艇体、１２  主推進器、１４  主推進器駆動モータ、１４ａ  電池、１６  垂直舵、１８  垂直舵駆動モータ、２０  水平舵、２２  水平舵駆動モータ、２４  深度計、２６  高度計、２８  障害物センサ、３０  観測用センサ、３２  慣性航法装置、３４  制御装置、３６  音響通信装置、３８  音響測位装置、４０  処理部、４２  記憶部、４４  入力部、４６  出力部、５０  初期設定手段、５２  潜航点設定手段、５４  目標値設定手段、５６  艇体運動モデル設定手段、５８  目標値追従シミュレーション実施部、６０  潜航高度確認手段、６２  繰り返し回数確認手段、６４  目標値更新手段、６６  最適目標値設定手段、６８  最適目標値出力手段、７０  最適目標値入力部、７２  記憶部、７４  艇体運動制御部、７６  自機運動検知部、７８  自機位置計算部、８０  観測行動実施部、８２  緊急浮上判断部、１００  水中航走体、２００  経路設定装置。
【図１】
0006761216.tif 000002
【図２】
0006761216.tif 000003
【図３】
0006761216.tif 000004
【図４】
0006761216.tif 000005
【図５】
0006761216.tif 000006
【図６】
0006761216.tif 000007
【図７】
0006761216.tif 000008