JP2004224312A - Guidance control device, guidance control system, mother ship, guidance control method, and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a guidance control device, a guidance control system, a mother ship, and a guidance control method capable of easy operation from the mother ship, a cruising body controlled from the mother ship and operating toward a target without missing the target, and a program for executing the method by a computer. <P>SOLUTION: The cruising body 2 is launched from the mother ship 1 and controlled from the outside, and the guidance control device 4 controls the cruising body 2. The cruising body 2 measures orientation information on the orientation to which the cruising body 2 faces, speed information on the ground speed of the cruising body 2, and object relative position information on the relative position of the target 3 to the cruising body 2, and outputs the measured data to the guidance control device 4. Based on the orientation information, speed information, object relative position information, and the cruising body position information on the position of the cruising body 2, the guidance control device 4 calculates a control relative distance as the relative distance of the cruising body 2 to the target 3. When the control relative distance is shorter than a preset reference distance, the guidance control device 4 controls the cruising body 2 so as to stop the cruising body 2. When it is shorter than the reference distance, the guidance control device 4 controls the cruising body 2 so that the cruising body can perform preset operations. <P>COPYRIGHT: (C)2004,JPO&NCIPI

Description

ここで、
【数２】

【００６９】
（２）ステップＳ１２：航走体位置計算
ΔＴ時間経過後の航走体２の絶対位置（Ｘ，Ｙ）を、航走体２の対地速度を積分することにより計算する。具体的には、上記入力データに基づいて、以下の式により計算される。
【数３】

ただし、Ｘ＿_ｏｌｄ，Ｙ＿_ｏｌｄは、ΔＴ時間前（直前）のＸ，Ｙである。
【００７０】
（３）ステップＳ１３：相対距離計算
ステップＳ１２で計算した航走体２の絶対位置を用いて、ΔＴ時間経過後の目標３の航走体２に対する相対位置（相対距離）Ｒ_Ｔを計算する。具体的には、上記数式（２）、（３）、（６）、（７）で算出した値に基づいて、以下の式により計算される。
【数４】

【００７１】
（４）ステップＳ１４：位置判断
航走体２が、ソーナースコープで指定された目標３に対して、Ｒ＿_ｄの範囲に達したかどうかの判断を行なう。具体的には、上記数式（８）で算出した値に基づいて、以下の式により判断する。
【数５】

この式を満足するＲ_Ｔが得られた場合は、航走体２は目標３に十分に接近したと判断する。そして、ステップＳ１５へ進む。
この式を満足するＲ_Ｔが得られない場合は、航走体２は目標３に十分に接近していないと判断する。そして、ステップＳ１２へ戻り、次のΔＴ時間後の計算を行なう。
【００７２】
なお、Ｒ＿_ｄは作業内容により、その大きさを自由に変更可能である。例えば、目標３が危険物の場合、接近し過ぎると危険なのでＲ＿_ｄを大きめに取る。また、目標３に対して直接作業を行なう場合には、衝突しない程度になるべく接近したいので、Ｒ＿_ｄを小さめに取る。これにより、作業時間の短縮や、安全性の向上を図ることが出来る。
【００７３】
また、航走体２の対地速度に応じて、航走体２のエンジン停止から航走体２の移動停止までの距離が異なるので、航走体２の対地速度に応じてＲ＿_ｄを変化させることにより、目標３へより正確に航走体を誘導することが可能となる。その場合、航走体２の対地速度と、航走体２のエンジン停止から航走体２の移動停止までの距離との関係を示すテーブルを記憶部７に用意し、対地速度に応じてそのテーブルの値をＲ＿_ｄに加え、新たにＲ＿_ｄ’を生成し、その値を判断基準の式（９）に利用すれば良い。
【００７４】
（５）ステップＳ１５：処理実行
航走体２は目標３に十分に接近している。従って、航走体２は、目標３への接近を終了し、停止する。そして、予め設定された作業（あるいは、母船１から指示された作業）を実行する。なお、作業は、後に指令することにし、目標３の近傍で待機するようにすることも可能である。
【００７５】
以上のようなステップＳ１１〜ステップＳ１５により、航走体２を目標３の近傍へ移動させる第１アルゴリズムが行なわれる。この第１アルゴリズムをコンピュータープログラムとして作成してコンピューターに実行させることができる。
【００７６】
次に、第２アルゴリズムについて説明する。
図８は、本発明である誘導制御装置、誘導制御システム、母船、誘導制御方法及びプログラムについての実施の形態の動作に関わる第２アルゴリズムに用いる座標系を示す図である。
第２アルゴリズムは、目標３の方向へ移動している航走体２の有するソーナー３１が、常に目標３の方向へ向くように、ソーナー３１を制御するアルゴリズムである。
絶対座標系ｘｙｚを設定し、航走体２の絶対座標系における初期位置を（Ｘ_０，Ｙ_０，Ｚ_０）、目標３の絶対座標系における位置を（Ｘ_Ｔ，Ｙ_Ｔ，Ｚ_Ｔ）、航走体２の２次元平面（ｘｙ平面に平行なｘ’ｙ’平面、以下同じ）内での方位をψ、航走体２に対する目標３の２次元平面内での相対方位をψ_Ｔ、航走体２に対する目標３のＺ軸方向の相対方位をθ_Ｔ、航走体２に対する目標３の相対距離をＲ_Ｔとする。ただし、ｘ’軸、ｙ’軸及びｚ’軸は、（Ｘ_０，Ｙ_０，Ｚ_０）を通るｘ軸、ｙ軸及びｚ軸に平行な軸である。
【００７７】
このような座標系において、第２アルゴリズムについて、図９を参照して説明する。
（０）航走体２は、母船１からの指令に基づいて、目標３へ向けて移動を開始している。
また、開始の段階において、以下の入力データが入力される。
・航走体データ（航走体２の運動）
方位：ψ ：方位センサ１７により測定（方位情報）。
航走体俯仰角：θ ピッチ角センサ２０により測定（方位情報に含む）。
深度：Ｚ ：深度検出器１６により測定（深度情報）。
対地速度ｕ＝（ｕ_ｇｘ，ｕ_ｇｙ） ：対地速度検出器１５により測定（速度情報）。
・航走体位置（航走体２の絶対位置）
座標：Ｐ_０＝（Ｘ_０，Ｙ_０） ：母船１の絶対位置（母船位置情報：ＧＰＳ受信装置４１より）と母船１に対する航走体２の相対位置（航走体相対位置情報：水中音響部６より）とから算出（航走体位置情報）。
・ソーナーデータ（目標３の航走体２に対する相対位置）
相対距離：Ｒ_Ｔ ：水中音響部１４により測定（対象相対距離情報）。
相対方位：ψ_Ｔ ：水中音響部１４により測定（対象相対方位情報）。
相対ピッチ角：θ_Ｔ ：水中音響部１４により測定（対象相対ピッチ角情報）。
・制御パラメータ（判断基準と判断対象）
目標チルト角：θ_ＴＩＬＴ ：ソーナー３１の制御目標となるチルト角。
チルト角判断角：Δθ_ＴＩＬＴ 目標ピッチ角θ_Ｐから許容される角度範囲の基準値（予め設定）。
ただし、Δθ_ＴＩＬＴ＜θ_Ｒ である。
（実チルト角θ_ＴＩＬＴ＿_ｄ ：実際のソーナー３１のチルト角）。
・定数
サンプリング時間：ΔＴ ：入力データのサンプリング間隔。
【００７８】
（１）ステップＳ２１：目標の絶対座標計算
目標３の絶対位置（絶対座標）（Ｘ_Ｔ，Ｙ_Ｔ，Ｚ_Ｔ）を計算する。計算は、目標３の航走体２に対する相対位置（相対座標）（Ｘ_ｔ，Ｙ_ｔ，Ｚ_ｔ）を求め、その値と、航走体２の絶対位置（絶対座標）とに基づいて、目標３の絶対位置（Ｘ_Ｔ，Ｙ_Ｔ，Ｚ_Ｔ）を算出する。具体的には、上記入力データに基づいて、以下の式により計算される。
【数６】

【００７９】
（２）ステップＳ２２：チルト角初期化
以降の計算に備えて、チルト角の初期値を設定する。具体的には、以下のようになる。
【数７】

【００８０】
（３）ステップＳ２３：航走体位置計算
ΔＴ時間経過後の航走体２の絶対位置（Ｘ，Ｙ，Ｚ）を、航走体２の対地速度を積分することにより計算する。深度Ｚに関しては、深度検出器１６の値を用いる。具体的には、上記入力データに基づいて、以下の式により計算される。
【数８】

ただし、Ｘ＿_ｏｌｄ，Ｙ＿_ｏｌｄ，Ｚ＿_ｏｌｄは、ΔＴ時間前（直前）のＸ，Ｙ，Ｚである。
【００８１】
（４）ステップＳ２４：目標ピッチ角計算
ステップＳ２３で計算した航走体２の絶対位置と深度偏差（＝Ｚ_Ｔ−Ｚ）を用いて、ΔＴ時間経過後の目標３の航走体２に対するピッチ角（目標ピッチ角θ_Ｐ）を計算する。具体的には、上記数式（１２）〜（１４）、（１７）〜（１９）で算出した値に基づいて、以下の式により計算される。
【数９】

【００８２】
（５）ステップＳ２５：チルト角制御判断
（４）で計算した目標ピッチ角θ_Ｐ（ソーナースコープで指定された目標３の方向）と、ソーナー３１の目標チルト角θ_ＴＩＬＴとの偏差により、チルト角の制御を行なうかどうかの判断を行なう。判断は、偏差が、予め設定されたΔθ_ＴＩＬＴの範囲に入っているかどうかで行なう。具体的には、上記数式（２０）で算出した値に基づいて、以下の式により判断する。
【数１０】

θ_Ｐがこの式を満足する場合は、航走体２のソーナー３１のチルト角θ_ＴＩＬＴ＿_ｄ（目標チルト角θ_ＴＩＬＴに追従している）は、十分な角度を有しており、目標３を見失うことは無いと判断する。そして、ステップＳ２７へ進む。
θ_Ｐがこの式を満足しない場合は、航走体２のソーナー３１のチルト角θ_ＴＩＬＴ＿_ｄは、不十分な角度を有しており、目標３を見失う可能性があると判断する。そして、ステップＳ２６へ進む。
なお、Δθ_ＴＩＬＴは、ソーナー３１の性能により決まる値であり、例えば２０°である。
【００８３】
（６）ステップＳ２６：目標チルト角変更
（５）における判定式（２１）を満たさなかった場合には、目標ピッチ角と目標チルト角との偏差が小さくなるように、目標チルト角を計算して、その値を用いてチルト角を制御する。具体的には、上記数式（２０）で算出した値と、現在のチルト角及び目標チルト角とに基づいて、以下の式により判断する。
【数１１】

そうすることにより、ソーナー３１の範囲から目標３がはみ出ることが無く、目標３を見失う可能性が無くなる。
ステップＳ２７へ進む。
【００８４】
（７）ステップＳ２７：目標ビーム俯仰角計算
目標ピッチ角と目標チルト角の偏差を目標ビーム俯仰角とし、目標３をビーム補足する。具体的には、上記数式（２０）及び（２２）で算出した値に基づいて、以下の式により判断する。
【数１２】

【００８５】
以上のようなステップＳ２１〜ステップＳ２７により、航走体２のソーナー３１の角度を目標３の方向へ常時向かせる第２アルゴリズムが行なわれる。この第２アルゴリズムをコンピュータープログラムとして作成してコンピューターに実行させることができる。
【００８６】
次に、本発明である誘導制御装置、誘導制御システム、母船、誘導制御方法及びプログラムが適用される実施の形態の動作の詳細について説明する。
【００８７】
まず、上記第１及び第２アルゴリズムを用いた航走体２及びソーナー３１の制御に関わる動作について説明する。
ｉ）母船１は、搭載していた航走体２を、探査海域において海中へ送出する。母船１と航走体２とは、ケーブル４２を介した通信が可能である（海中無線通信でも良い）。
ｉｉ）母船１の誘導制御装置４の誘導制御部５は、入出力部９からの航走体２への移動指令（例示：対地速度、方位、深度／緯度、経度、水深など）又は記憶部７の移動スケジュールによる移動指令に基づいて、その移動指令を送受信部８から船走体２の航走体制御システム１２へケーブル４２を介して送信する。
ｉｉｉ）航走体２は、送受信部１９を介して移動指令を受信する。そして、その移動指令に基づいて移動を行なう。
ｉｖ）母船１のＧＰＳ受信装置４１は、ＧＰＳ衛星群４０からのＧＰＳ信号に基づいて、母船１の位置を算出する。そして、その位置を母船位置情報として誘導制御装置４へ出力する。母船位置情報は、その時刻と共に記憶部７に保持される。
ｉｖ）母船１では、適当な時間（例示：ΔＴ）毎に、誘導制御装置４の誘導制御部５からの指令により、水中音響部６が、船外へ母船探査信号としての探査信号を発信する。それと共に、母船探査信号の反射波としての母船反射波を受信する。この場合の反射波は、航走体２から反射したものである。そして、母船探査信号及び母船反射波とに基づいて、航走体２の母船１に対する相対位置を示す航走体相対位置情報を算出する。更に、母船位置情報と航走体相対位置情報とに基づいて、航走体２の絶対位置を示す航走体位置情報を算出する。それらの情報を記憶部７に記憶する。なお、水中音響部６がその探査信号により航走体相対位置情報を得る方法は、従来のソーナーで知られている方法を用いることが出来る。
ただし、航走体相対位置情報を算出する方法は、上記方法に限定されない。
ｖ）航走体２では、航走体制御システム１２の船体制御部１６からの指令により、対地速度検出器１５が、対地速度を計測している。また、深度検出器１６が、水深（深度）を計測している。更に、方位センサ１７が、進行方向の方位を計測している。それら計測された値は、計測時刻と共に記憶部１８に記憶されている。
ｖｉｉ）航走体２では、適当な時間（例示：ΔＴ）毎に、航走体制御システム１２の機体制御部１３からの指令により、水中音響部１４が、船外へ探査信号（ソーナー信号）を発信する。それと共に、探査信号の反射波（エコー）を受信する。そして、探査信号データ（発信時刻、探査信号特性など）及び反射波データ（受信時刻、反射波特性など）、発信時刻あるいは受信時刻に最も近い時刻での速度情報（対地速度）、深度情報（水深）及び方位情報（進行方向の方位）を、送受信部１９から母船１の誘導制御装置４へケーブル４２を介して送信する。
ｖｉｉｉ）母船１の誘導制御装置４は、送受信部８にて、計測データ（探査信号データ及び反射波データ）、速度情報（対地速度）、深度情報（水深）及び方位情報（進行方向の方位）を、ケーブル４２を介して受信する。そして、それらの情報を一旦記憶部７に記憶する。
ｉｘ）母船１の誘導制御装置４は、ソーナー信号処理部１０において、計測データ（探査信号データ及び反射波データ）に基づいて、目標３の航走体２に対する相対位置を示す対象相対位置情報（相対距離としての対象相対距離、目標３の航走体２に対する水平方向の相対方位としての対象相対方位、目標３の航走体２に対する相対俯角としての対象相対ピッチ角）を計算する。計算結果は、記憶部７に保持される。なお、ソーナー信号処理部１０が、探査信号により対象相対位置情報を得る方法は、従来のソーナーで知られている方法を用いることが出来る。
ｘ）母船１の誘導制御装置４は、誘導制御部５において、航走体データ（方位情報、深度情報、速度情報）、航走体位置（航走体位置情報）、ソーナーデータ（対象相対距離、対象相対方位、対象相対ピッチ角）に基づいて、目標３へ向かう制御信号を生成し、ケーブル３を介して航走体２へ出力することにより、その動作を制御する。
ｘｉ）それと同時に、母船１の誘導制御装置４は、誘導制御部５において、航走体データ（方位情報、深度情報、速度情報）、航走体位置（航走体位置情報）、ソーナーデータ（対象相対距離、対象相対方位、対象相対ピッチ角）、制御パラメータ（目標チルト角、チルト角判断角等）を用いて、第２アルゴリズムにより、航走体２が目標３を見失わないように、航走体２のソーナー３１のチルト角を監視、制御している。
ｘｉｉ）それに加えて、母船１の誘導制御装置４は、誘導制御部５において、航走体データ（方位情報、速度情報）、航走体位置（航走体位置情報）、ソーナーデータ（対象相対距離、対象相対方位、制御パラメータ（位置判断距離等）を用いて、第１アルゴリズムにより、航走体２と目標３との距離Ｒ_Ｔが、Ｒ＿_ｄ以下かどうかを監視する。
ｘｉｉｉ）適宜（ｉｉ）〜（ｘｉｉ）を繰り返す。
ｘｉｖ）母船１の誘導制御装置４は、誘導制御部５において、航走体２と目標３との距離Ｒ_Ｔが、Ｒ＿_ｄ以下になったら、航走体２へ停止信号を出力すると共に、予め設定された作業を行なうための指令を示す信号（処理実行信号）を出力する。
ｘｖ）航走体２の機体制御部１３は、誘導制御装置４からの停止信号及び処理実行信号とに基づいて、停止し、予め設定された作業を実行する。
【００８８】
以上の動作により、誘導制御方法及び誘導制御装置、誘導制御システム、並びに当該方法をコンピューターに実行させるプログラムを使用して、航走体２を目標３の近傍において停止させ、目標３において作業を行なうことが可能となる。
【００８９】
また、ソーナー３１のチルト角及び俯仰角を自動で制御することが可能となる。そして、航走体２の操縦者が、操縦以外の動作（チルト角及び俯仰角の制御）に費やす時間を著しく減少することができ、操縦者の負担を大きく軽減することが可能となる。
【００９０】
第１アルゴリズムの（４）の位置判断をより正確に行なう（ΔＴを小さくする）ことにより、航走体２の停止位置をより厳密に制御することが可能となる。
【００９１】
次に、図３に示す表示画面１１−１上で、航走体２の進路を目標３へ設定する動作について説明する。
Ａ）図３（ａ）
表示画面１１−１では、航走体２のソーナー３１の探査信号による計測範囲内の状況が表示される。ここで、目標３が存在した場合、そこからの反射波が発生する。航走体２は、それらの計測データ（探査信号データ及び反射波データ）を母船１の誘導制御装置４へ送信する。
誘導制御装置４では、ソーナー信号処理部１０は、計測データに基づいて、目標３の航走体２に対する相対位置の情報である第２相対位置情報（相対距離としての対象相対距離、相対方位としての対象相対方位、相対俯角としての対象相対ピッチ角）を計算する。そして、誘導制御部５は、その第２相対位置情報に基づいて、操作表示部１１において、表示画面１１−１上にソーナー３１の計測範囲を展開させる。そして、操作表示部１１は、目標３の画像（エコー２２）を映し出す。
ただし、表示画面１１−１の中心方向は、チルト角の方向であり、表示画面１１−１の範囲は、ビームレンジの範囲である。
【００９２】
Ｂ）図３（ｂ）
航走体２の操縦者は、航走体２を目標３（エコー２２）へ向かわせるために、以下の動作を行なう。すなわち、表示画面１１−１において、カーソル２１をトラックボールにより動かし、エコー２２に合わせ、目標指定ボタンを押す。その動作により、カーソル２１は、目標指定シンボル２３に変わり、航走体２の目標として、新たにエコー２２のある場所としての第１場所が入力されたことになる。
目標指定シンボル２３の表示画面１１−１上の位置は、操作表示部１１により、航走体２に対する新たな移動方向として計算される。そして、計算された相対位置（相対距離、相対方向、相対ピッチ角）は、第１相対位置情報（シンボル相対距離情報、シンボル相対方向情報、シンボル相対ピッチ角情報）として誘導制御部５へ出力される。誘導制御部５は、航走体２の移動目標としての第１相対位置情報を航走体２へ出力する。航走体２は、受信した上記各情報で指定された位置へ移動して行く。
【００９３】
その後、航走体２の運動に応じて、航走体２から目標指定シンボル２３の位置の変化が定周期に返信される。その情報に基づいて、誘導制御部５は、目標指定シンボル２３の位置の移動を操作表示部１１へ出力する。それにより、表示画面１１−１上の目標指定シンボル２３の位置が移動する。
一方、エコー２２（目標３）の位置（第２相対位置情報に示される）は、航走体２の認識する位置とは独立に、ソーナー３１による計測データから算出している。従って、航走体２の運動に応じてエコー２２も位置が変化するが、目標指定シンボル２３の位置とは別に変化する。
【００９４】
Ｃ）図３（ｃ）
表示画面１１−１において、航走体２の位置測定誤差の累積によりエコー２２（第２相対位置情報）と目標指定シンボル２３（第１相対位置情報）との間にずれが生じる場合がある。その場合、カーソル２１により目標指定シンボル２３をフック（ドラック）し、移動して、ソーナースコープ上の正しいエコー２２の位置に合わせ、位置調整ボタンを押して、位置補正する。それにより、新たなエコーのある場所としての第２場所が入力される。
位置補正された目標指定シンボル２３の表示画面１１−１上の位置は、操作表示部１１により、航走体２に対する新たな移動方向として計算される。そして、計算された相対位置（相対距離、相対方向、相対ピッチ角）は、第３相対位置情報として誘導制御部５へ出力される。誘導制御部５は、航走体２の移動目標としての第３相対位置情報を航走体２へ出力する。航走体２は、受信した上記各情報で指定された位置へ移動して行く。
【００９５】
目標に到達するまで、必要に応じて、（Ｂ）、（Ｃ）の行程をくり返す。
そして、上記プロセスを行なうことにより、表示画面上で指定した位置（目標）へ航走体の進路を容易に設定することが可能となる。また、途中の段階で目標から外れてくる場合でも、容易に進路の修正を行なうことが可能となる。
【００９６】
上記プロセスは、母船と海洋探査船のような水中における探査活動を行なう潜航艇とを例に説明している。
【００９７】
【発明の効果】
本発明により、母船から制御され目標へ向かって運行する航走体の母船での操作を、目標を見失うことなく容易に行なうことが可能となる。
【図面の簡単な説明】
【図１】本発明である誘導制御装置、誘導制御システム、母船、誘導制御方法及びプログラムの実施の形態に関わる構成を示す説明図である。
【図２】前記実施の形態において適用される母船側の誘導制御の構成を示すブロック図である。
【図３】前記実施の形態における操作表示部の表示画面の一例を示す説明図である。
【図４】前記実施の形態において適用される航走体制御システムの構成を示すブロック図である。
【図５】前記実施の形態に関わる航走体の構成の一部を示す側面図である。
【図６】前記実施の形態の動作に関わる第１アルゴリズムに用いる座標系を示す説明図である。
【図７】前記実施の形態の動作に関わる第１アルゴリズムを示すフロー図である。
【図８】前記実施の形態の動作に関わる第２アルゴリズムに用いる座標系を示す説明図である。
【図９】前記実施の形態の動作に関わる第２アルゴリズムを示すフロー図である。
【符号の説明】
１ 母船
２ 航走体
３ 目標
４ 誘導制御装置
５ 誘導制御部
６ 水中音響部
７ 記憶部
８ 送受信部
９ 入出力部
１０ ソーナー信号処理部
１１ 操作表示部
１１−１ 表示画面
１２ 航走体制御システム
１３ 機体制御部
１４ 水中音響部
１５ 対地速度検出器
１６ 深度検出部
１７ 方位センサ
１８ 記憶部
１９ 送受信部
２１ カーソル
２２ エコー
２３ 目標指定シンボル
３１ ソーナー
３２ カメラ
４０ ＧＰＳ衛星群
４１ ＧＰＳ受信装置
４２ ケーブル[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a guidance control device, a guidance control system, a mother ship, a guidance control method, and a program for causing a computer to execute the method. More particularly, the invention relates to guidance control in a system having a mother ship and a navigation body controlled and controlled by the mother ship. The present invention relates to a device, a guidance control system, a mother ship, a guidance control method, and a program.
[0002]
[Prior art]
In various explorations conducted underwater, an unmanned vehicle controlled by a mother ship serving as a base often performs actual exploration work. In that case, the vehicle has a sonar and can find a target (object) underwater from a distance. However, azimuth control for approaching the target requires manual control on the mother ship while looking at the sonar scope, and requires a very advanced control technique. Furthermore, when heading for a target at a high speed, the target is often out of the range of the sonar, and the target may be lost.
[0003]
Further, in the work near the target, the work has conventionally been performed by looking at the camera image. However, it is not easy to capture the target with the camera due to the transparency in the water and the like, and the work may be difficult.
[0004]
Furthermore, when approaching the target, the pilot needs to concentrate on maneuvering the vehicle. Therefore, it is not easy to simultaneously control the elevation angle of the sonar and the tilt device that controls the central angle of the sonar and the elevation angle of the camera. Therefore, there is a danger of losing the target and collision. In addition, it is necessary for the mother ship to know the exact absolute position (hereinafter, also simply referred to as “position”) of the vehicle.
[0005]
Therefore, there is a need for a technique for easily and automatically guiding a marine vehicle to a target. There is also a need for a technique for automatically stopping a marine vehicle near a target and performing work. Further, there is a need for a technique for always automatically pointing a sonar of a marine vehicle to a target during guidance of the marine vehicle.
[0006]
[Problems to be solved by the invention]
Therefore, an object of the present invention is to provide a guidance control device, a guidance control system, a mother ship, a guidance control method, and a guidance control system capable of easily performing maneuvering on a mother ship in a marine vehicle controlled from the mother ship and traveling toward a target. To provide a program that causes a computer to execute the method.
[0007]
Another object of the present invention is to provide a guidance control device, a guidance control system, a mother ship, and a guidance control device capable of controlling and maneuvering a cruising vehicle controlled from a mother ship and traveling toward the target without losing the target. An object of the present invention is to provide a method and a program for causing a computer to execute the method.
[0008]
Still another object of the present invention is to provide a guidance control device, a guidance control system, a mother ship, a guidance control method capable of easily performing work, processing, and the like relating to a target object, and a program for causing a computer to execute the method. That is.
[0009]
[Means for Solving the Problems]
The means for solving the problem will be described below using the numbers and symbols used in [Embodiments of the Invention]. These numbers and symbols have been added in order to clarify the correspondence between the description of [Claims] and [Embodiments of the Invention]. However, those numbers and symbols must not be used for interpreting the technical scope of the invention described in [Claims].
[0010]
Therefore, in order to solve the above-mentioned problem, the guidance control device of the present invention has a screen display means (11-1) for displaying a search signal, and is provided with a navigation unit (2) transmitted from the mother ship (1). The echo (22) representing the object in the search signal is displayed on the screen display means (11-1), and the first location as the location of the echo (22) on the screen display means (11-1) is designated. At this time, the operation display unit (11) for calculating a first relative position as a relative position to the navigation body (2) at the first place, and movement of the navigation body (2) based on the first relative position. A guidance control unit (5) for controlling the vehicle (2) so that the target is at the first relative position.
[0011]
Further, the guidance control device of the present invention calculates a second relative position as a relative position of the echo (22) with respect to the navigation body (2) based on the search signal output from the navigation body (2). The search signal processing unit (10) is further provided. Then, the operation display unit (11) displays the echo (22) on the screen display means (11-1) based on the second relative position.
[0012]
Further, in the guidance control device according to the present invention, when the operation display unit (11) is displaced from the first relative position and the second relative position on the screen display means (11-1), the second relative position is displayed. When the second location as the location indicating is designated anew, the third relative position is calculated as the relative position of the second location with respect to the vehicle (2). Then, the guidance control unit (5) controls the navigation body (2) so that the moving target of the navigation body (2) is set to the third relative position.
[0013]
In order to solve the above problems, a guidance control system according to the present invention includes a guidance control device (4) according to any one of the above, and a navigation body (2) controlled by the guidance control device (4). Is provided.
[0014]
Further, the guidance control system of the present invention includes a navigation body (2) that is sent from the mother ship (1) and is externally controlled, and a guidance control device (4) that controls the navigation body (2). . The vehicle (2) includes direction information as the direction to which the vehicle (2) is facing, speed information as the ground speed of the vehicle (2), and a target (3) of the vehicle (2). ) Is measured and the target relative position information as a relative position with respect to the navigation body (2), and is output to the guidance control device (4). Then, the guidance control device (4) is configured to control the navigation body based on the azimuth information, the speed information, the target relative position information, and the navigation body position information as the position of the navigation body (2). The control relative distance as the relative distance between (2) and the target (3) is calculated.
[0015]
Further, in the guidance control system according to the present invention, the guidance control device (4) performs navigation so as to stop the navigation body (2) when the control relative distance is equal to or less than a reference distance as a preset distance. Control body (2).
[0016]
Further, in the guidance control system according to the present invention, the guidance control device (4) performs a predetermined operation when the control relative distance is equal to or less than a reference distance as a predetermined distance. Control 2).
[0017]
Further, the guidance control system of the present invention includes a navigation body (2) that is sent from the mother ship (1) and is externally controlled, and a guidance control device (4) that controls the navigation body (2). . The vehicle (2) includes direction information as the direction to which the vehicle (2) is facing, speed information as the ground speed of the vehicle (2), and a target (3) of the vehicle (2). ) Is measured and the target relative position information as a relative position with respect to the navigation body (2), and is output to the guidance control device (4). Then, the guidance control device (4) performs navigation with the horizontal plane based on the azimuth information, the speed information, the target relative position information, and the navigation body position information as the position of the navigation body (2). A target pitch angle is calculated as an angle between the running body (2) and the direction toward the target (3).
[0018]
Further, the guidance control system of the present invention measures the guidance control device (2) by measuring the tilt angle as the angle between the direction of the sonar (31) of the navigation body (2) and the horizontal plane. Output to 4). Then, the guidance control device (4) performs navigation such that the absolute value of the difference between the target tilt angle, which is the target value of the tilt angle, and the target pitch angle is equal to or smaller than the preset reference angle. The running body (2) is controlled.
[0019]
Furthermore, a mother ship of the present invention includes the guidance control system according to any one of the above, and a GPS receiving device (41) capable of specifying its own position.
[0020]
In order to solve the above-mentioned problems, a guidance control method according to the present invention includes a step of displaying, on a screen display means (11-1), an exploration signal of a navigation body (2) sent from a mother ship (1); A step of designating a first place as a place where an echo (22) representing an object displayed on the means (11-1), and a first place as a relative position of the first place with respect to the navigation body (2). Calculating a relative position; and transmitting a control signal to the moving body (2) with the moving target of the moving body (2) as a first relative position.
[0021]
In the guidance control method according to the present invention, the step of displaying the search signal on the screen display means (11-1) includes the steps of: receiving the search signal from the hull (2); and receiving the search signal. Calculating a second relative position as a relative position of the echo (22) with respect to the vehicle (2) on the basis of the above, and displaying the echo (22) on the screen display means (11-) based on the second relative position. 1) displaying the information.
[0022]
Further, in the guidance control method according to the present invention, when the first relative position and the second relative position are displaced on the screen display means (11-1), the second relative position as a location indicating the second relative position is determined. Re-designating the location, calculating the third relative position of the second location relative to the marine vehicle (2), and setting the moving target of the marine vehicle (2) at the third relative position Transmitting a control signal to the vehicle (2).
[0023]
Further, according to the guidance control method of the present invention, the azimuth information indicating the heading of the navigation body (2) sent from the mother ship (1) and the relative position of the target (3) with respect to the navigation body (2) can be obtained. Calculating target position information as the position of the target (3) on the basis of the target relative position information and the vehicle position information as the position of the vehicle (2); Based on the speed information as the ground speed of the vehicle (2) and the vehicle position information, a new voyage as the position of the vehicle (2) after a predetermined reference time has elapsed. Calculating the body position information (S12), and based on the target position information and the new vehicle position information, a control relative distance as a relative distance between the vehicle (2) and the target (3). The calculating step (S13), and the control relative distance is set to a predetermined distance For reference distance below as, comprises a step (S14, S15) to stop the Kohashikarada (2), the target (3) is the target of the movement of Kohashikarada (2).
[0024]
Further, the guidance control method of the present invention includes the step (S15) of stopping the navigation body (2) (S14, S15) and performing a predetermined operation after the navigation body (2) stops. .
[0025]
Further, in the guidance control method according to the present invention, in the steps (S14, S15) for stopping the navigation body (2), when the control relative distance is larger than the reference distance, a step of calculating the new navigation body position information. Return to (S12).
[0026]
Further, the guidance control method according to the present invention is characterized in that the azimuth information indicating the heading of the navigation body (2) sent from the mother ship (1) and the relative position of the target (3) with respect to the navigation body (2). Calculating the target position information as the position of the target (3) based on the target relative position information of the target and the vehicle position information as the position of the vehicle (2) (S21); Based on the speed information as the ground speed of the vehicle (2) and the vehicle position information, a new voyage as the position of the vehicle (2) after a predetermined reference time has elapsed. A step (S22, S23) of calculating body position information, and a horizontal plane and a direction from the vehicle (2) to the target (3) are formed based on the target position information and the new vehicle position information. Calculating a target pitch angle as an angle (S24); If the absolute value of the difference between the target tilt angle, which is the target value of the tilt angle as the angle formed by the sonar and the horizontal plane, and the target pitch angle is larger than a reference angle as a preset angle, Calculating a new target tilt angle, which is a target value of the tilt angle after the lapse of the reference time, based on the target pitch angle, the tilt angle, and the target tilt angle (S25, S26); Calculating a target beam elevation angle which is a target value of a beam elevation angle as an angle between the beam of the sonar (31) and a horizontal plane based on the pitch angle and the new target tilt angle (S27). , Target (3) is a target of movement of the marine vehicle (2).
[0027]
Further, in the guidance control method of the present invention, in the step (S25) of calculating the new target tilt angle, when the absolute value is equal to or smaller than the reference angle, the process proceeds to the step of calculating the target beam elevation angle (S27). In the step (S27) of calculating the target beam elevation angle, the target beam elevation angle is calculated based on the target pitch angle and the target tilt angle.
[0028]
In order to solve the above-mentioned problems, a program according to the present invention includes a step of displaying, on a screen display means (11-1), an exploration signal of a navigation vehicle (2) sent from a mother ship (1); When a first place as a place where the echo (22) representing an object displayed in (11-1) is located, a first relative position as a relative position of the first place with respect to the navigation body (2) is specified. Calculating a position, and transmitting a control signal to the vehicle (2) with the moving target of the vehicle (2) as a first relative position,
The step of displaying the search signal on the screen display means includes the steps of: receiving the search signal from the marine vehicle; and, based on the received search signal, as a relative position of the echo to the marine vehicle. Calculating a second relative position, and displaying the echo on the screen display means based on the second relative position,
Further, when the first relative position and the second relative position are displaced on the screen display means (11-1), and a second location as a location indicating the second relative position is newly designated, the second relative position is designated. Calculating a third relative position as a relative position with respect to the navigation body (2) at two places; and transmitting a control signal for setting a moving target of the navigation body (2) to the third relative position to the navigation body (2). And transmitting the steps to a computer.
[0029]
In addition, the program according to the present invention includes the azimuth information as the azimuth of the navigation body (2) sent from the mother ship (1) and the navigation body (3) of the target (3) of the navigation body (2). Calculating target position information as the position of the target (3) based on the target relative position information as a relative position to 2) and the navigator position information as the position of the navigator (2); Based on the azimuth information, the speed information as the ground speed of the vehicle (2), and the vehicle position information, the position of the vehicle (2) after a predetermined reference time has elapsed is determined. Calculating a relative position distance between the vehicle (2) and the target (3) based on the step of calculating new vehicle position information and the target position information and the new vehicle position information; The calculating step and the control relative distance are calculated based on a predetermined distance. If the distance is greater than the distance, the computer returns to the step of calculating the new vehicle position information, and if the control relative distance is less than or equal to the reference distance, stops the vehicle (2). .
[0030]
Further, the program according to the present invention includes the azimuth information as the azimuth of the hull (2) sent from the mother ship (1) and the azimuth information as the relative position of the target (3) with respect to the hull (2). Calculating target position information as the position of the target (3) based on the target relative position information and the vehicle position information as the position of the vehicle (2); azimuth information; Based on the speed information as the ground speed of the body (2) and the navigation body position information, new navigation body position information as the position of the navigation body (2) after a predetermined reference time has elapsed is calculated. Based on the calculating step and the target position information and the new vehicle position information, a target pitch angle is calculated as an angle between the horizontal plane and the direction from the vehicle (2) to the target (3). Step, sonar (31) of the airframe (2) and horizontal When the absolute value of the difference between the target tilt angle, which is the target value of the tilt angle as the angle formed by the target pitch angle, and the target pitch angle is larger than a reference angle as a preset angle, the target pitch angle, Calculating a new target tilt angle, which is a target value of the tilt angle after the reference time has elapsed, based on the tilt angle and the target tilt angle, based on the target pitch angle and the new target tilt angle Calculating the target beam elevation angle, which is the target value of the beam elevation angle as the angle formed by the beam of the sonar (31) with the horizontal plane.
[0031]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of a guidance control device, a guidance control system, a mother ship, a guidance control method, and a program for causing a computer to execute the method according to the present invention will be described with reference to the accompanying drawings.
In the present embodiment, a description will be given by taking as an example a navigation body controlled by a mother ship and used for underwater exploration. However, the present invention is also applicable to a traveling body (not limited to underwater) that is controlled by another mother ship and moves away from the mother ship.
In each embodiment, the same or corresponding portions will be denoted by the same reference numerals and described.
[0032]
A configuration of an embodiment of a guidance control device, a guidance control system, a mother ship, a guidance control method, and a program according to the present invention will be described.
FIG. 1 is a diagram showing a configuration related to an embodiment of a guidance control device, a guidance control system, a mother ship, a guidance control method, and a program according to the present invention.
The vehicle includes a mother ship 1 including the guidance control device 4 and the GPS receiving device 41, a navigation body 2 including the navigation body control system 12, and a cable 42 connecting the mother ship 1 and the navigation body 2. The target 3 is a target that the marine vessel 2 should approach or track. A GPS satellite group 40 is used for control.
[0033]
The mother ship 1 transports the hull 2 to the exploration sea area, sends out the hull 2 in the exploration sea area, and controls the operation of the hull 2. It has a guidance control device 4 and a GPS receiving device 41.
The guidance control device 4 belongs to the mother ship 1. The relative position of the ship 2 to the mother ship 1 is known. Then, in order to guide the navigation body 2 to the target 3, various calculations are performed based on information measured by itself and information obtained from the navigation body 2, and various signals for controlling the navigation body 2 are provided. Is generated and transmitted to the vehicle 2. The vehicle 2 communicates via the cable 42. However, it is also possible to use an underwater wireless device (not shown).
The GPS receiver 41 belongs to the mother ship 1 and outputs mother ship position information as the position of the mother ship 1 using the GPS satellites 40. The mother ship position information is exemplified by (latitude, longitude).
[0034]
The watercraft 2 is transported by the mother ship 1 to the exploration sea area, sent out from the mother ship 1 in the exploration sea area, and navigates underwater while being controlled by the mother ship 1 to perform various operations. Communication with the mother ship 1 is performed via the cable 42. The watercraft 2 is exemplified by an unmanned underwater exploration boat.
The vehicle control system 12 belongs to the vehicle 2. It measures and grasps information on the operating state of the marine vehicle 2 such as ground speed and depth, and information on the target 3. And it outputs to guidance control device 4 of mother ship 1. Further, it receives various signals for controlling the hull 2 from the guidance control device 4 of the mother ship 1 and controls the hull 2.
[0035]
The cable 42 connects the mother ship 1 and the marine vehicle 2 so that they can communicate with each other. Various kinds of information output from the navigation body 2 (the navigation body control system 12 thereof) are transmitted to the mother ship 1 (the guidance control device 4). In addition, various signals for controlling the traveling body 2 output from the mother ship 1 are transmitted to the traveling body 2. The wireless communication may be performed without using the cable 42.
[0036]
The target 3 is located in the sea, where the navigation body 2 approaches and performs some processing (work). Alternatively, it is a target point to which the marine vehicle 2 moves. The target 3 is exemplified by a marine organism, a sunken ship in the sea, an archeological site, a mine, a point (latitude, longitude, water depth), and the like.
[0037]
The GPS satellite group 40 is a plurality of satellites used for the operation of generally used GPS. Based on the information from the satellite group, the GPS receiver 41 outputs (calculates) the mother ship position information of the mother ship 1.
[0038]
Next, the guidance control device 4 will be further described.
FIG. 2 is a diagram illustrating a configuration of an embodiment to which the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention are applied. The guidance control device 4 includes a guidance control unit 5, an underwater acoustic device 6, a storage unit 7, a transmission / reception unit 8, an input / output unit 9, a sonar signal processing unit 10 as a search signal processing unit, and an operation display unit 11.
[0039]
The guidance control unit 5 belongs to the guidance control device 4. The guidance control device 4 is controlled. At the same time, the vehicle 2 is controlled. The guidance control unit 5 is exemplified by an information processing device such as a CPU, a memory, and a peripheral device. In addition, based on the relative position information of the vehicle (described later) and the position information of the mother ship, the vehicle position information (for example, latitude, longitude, and depth) indicating the absolute position of the vehicle 2 is calculated. Then, the vehicle data (described later: azimuth information, speed information, depth information, etc.), sonar data (described later: target relative distance information, target relative azimuth information, target relative pitch angle, etc.), the vehicle position information and the reference value Various calculations (described later: first and second algorithms, etc.) are performed based on (referred to as a distance reference value, a tilt angle reference value, and the like) to control the operation of the vehicle 2 (and attached equipment).
[0040]
The underwater acoustic part 6 belongs to the guidance control device 4. It has an underwater acoustic device (not shown) and a wave receiving device (not shown) for receiving a reflected wave of a search signal (underwater sonar signal) emitted by the underwater acoustic device. A search signal is transmitted to the hull 2 and the reflected wave is measured. Then, based on the reflected waves, a relative position (example: distance, direction) of the vehicle, which is a relative position of the vehicle 2 with respect to the mother ship 1, is calculated and output as vehicle relative position information. The underwater sound unit 6 is exemplified by a sonar system.
[0041]
The storage unit 7 belongs to the guidance control device 4. Various information and data used for control of the guidance control device 4 are stored in a table corresponding to the various information and data (for example, a table in which measurement (calculation) time and information and data are associated). Also, an operation algorithm used for the operation is stored. The storage unit 7 is exemplified by a hard disk.
[0042]
The transmission / reception unit 8 belongs to the guidance control device 4. When the vehicle 2 and the mother ship 1 perform communication using the cable 42, they function as an interface. When the cable 42 is not used, it is an undersea radio device.
[0043]
The input / output unit 9 belongs to the guidance control device 4. Input and output of information necessary for control by the guidance control device 4 are performed. The input / output unit 9 is exemplified by a keyboard, a trackball (type mouse), a joystick, a touch panel, and the like.
[0044]
The sonar signal processing unit 10 belongs to the guidance control device 4. Based on the measurement data (described later) of the sonar 31 (described below) obtained by the underwater acoustic part 14 of the marine vehicle 2, the target relative distance as the relative distance of the target 3 to the marine vehicle 2, the marine vehicle of the target 3 The target relative azimuth as the horizontal relative azimuth with respect to the target 2 and the target relative pitch angle as the relative depression angle with respect to the hull 2 of the target 3 are calculated.
[0045]
The operation display unit 11 belongs to the guidance control device 4, has a sonar scope as a screen display unit for displaying an underwater exploration signal (soner signal), and displays a relative situation of the marine vehicle 2 and the target 3 on a screen. . At the same time, by operating the cursor on the display screen, a signal for guiding and controlling the navigation body 2 is generated.
[0046]
Here, the operation display unit 11 will be further described.
FIG. 3 is a diagram illustrating an example of a display screen 11-1 of the sonar scope in the operation display unit 11 in the guidance control device, the guidance control system, and the guidance control method according to the present invention. The display screen 11-1 shows an underwater state detected by the underwater vehicle 2 based on the search signal of the underwater acoustic unit 14. A cursor 21, an echo 22, and a target designation symbol 23 (23 ') are included.
[0047]
The cursor 21 specifies an operation place on the image when performing an operation on the displayed image on the display screen 11-1. Movement and location designation are performed by the input / output unit 9 in FIG. For example, the cursor 21 is moved by operating a trackball (not shown), and the target is designated by pressing a target designation button (not shown).
The echo 22 is a position (and a position at which an exploration signal generated by the underwater acoustic part 14 (described later in FIG. 4) of the marine vehicle 2 is calculated based on a reflected wave that is reflected back to some object and that an object exists. Size).
The target designation symbol 23 (23 ') represents an operation place designated on the image by the cursor 21. Then, the marine vehicle 2 moves toward a position corresponding to the display of the target designation symbol 23.
[0048]
On the display screen 11-1, the operation display unit 11 displays the target 3 based on the target relative distance information, the target relative azimuth information, and the target relative pitch angle (described later), which are information on the relative position of the target 3 with respect to the marine vehicle 2. Is projected and displayed as an echo 22 on a sonar scope. The coordinates at that time are expressed by relative coordinates with respect to the vehicle 2. However, by using the vehicle position information, the target relative distance information, the target relative azimuth information, and the target relative pitch angle can be easily converted to the absolute coordinate system, and the display screen 11-1 is displayed in the absolute coordinate system. May be.
[0049]
In FIG. 3A, an echo 22 and a cursor 21 are displayed on the display screen 11-1. The echo 22 corresponds to the target 3.
In FIG. 3B, on the display screen 11-1, the point at which the target designation symbol 23 is displayed by moving the cursor 21 with the trackball, aligning it with the echo 22, and pressing the target designation button is shown. ing. The vehicle 2 moves to the position specified by the target specifying symbol 23 (controlled by the guidance control unit 5 to do so).
In FIG. 3C, since the displacement between the echo 22 and the target designation symbol 23 has occurred on the display screen 11-1 due to the accumulation of the position measurement errors of the vehicle 2, the target designation symbol 23 is hooked by the cursor 21. (Drag), move to the correct position of the echo 22, drop, and display the point when the target is specified again. The correct position of the echo 22 is calculated by the sonar signal processor 10 based on the search signal.
[0050]
Therefore, the guidance control device 4 of the mother ship 1 executes the guidance control of the navigation body 2 by a method of the following steps, in other words, a program for causing the computer to sequentially execute the following steps.
(1) a step of projecting (displaying) a search signal of the cruising vehicle 2 transmitted from the mother ship 1 on a sonar scope as a screen display means of the operation display unit 11;
(2) designating a first place as a place where an echo representing a target object projected and displayed on the sonar scope is located;
(3) calculating a first relative position as a relative position of the first place with respect to the navigation body on the operation display unit 11;
{Circle over (4)} A step of transmitting a control signal for setting the moving target of the navigation body 2 to the first relative position to the navigation body via the transmission / reception unit 8.
[0051]
Further, the step of projecting the search signal of (1) on a sonar scope of the operation display unit 11 includes:
Receiving the search signal from the vehicle 2 via the transmission / reception unit 8;
Calculating, by the sonar signal processing unit 10, a second relative position as a relative position of the echo with respect to the vehicle 2 based on the received search signal;
Displaying the echo on a sonar scope of the operation display unit 11 based on the second relative position.
[0052]
When the first relative position and the second relative position are shifted on the sonar scope as shown in FIG. 3C, the following steps are further executed.
(5) re-designating a second location as a location indicating the second relative position;
(6) calculating the third relative position as a relative position of the second place with respect to the hull 2 on the operation display unit 11;
{Circle around (7)} A step of transmitting a control signal for setting the moving target of the navigation body to the third relative position to the navigation body.
[0053]
Next, the vehicle control system 12 will be further described.
FIG. 4 is a diagram showing a configuration of the navigation system control system 12 applied in the embodiment of the guidance control device, the guidance control system, the mother ship, the guidance control method and the program according to the present invention. The vehicle control system 12 includes an aircraft control unit 13, an underwater acoustic unit 14, a ground speed detector 15, a depth detector 16, a direction sensor 17, a storage unit 18, a transmission / reception unit 19, and a pitch angle sensor 20.
[0054]
The aircraft control unit 13 belongs to the marine vehicle control system 12. The vehicle 2 is controlled based on a signal for controlling the vehicle 2 transmitted from the mother ship 1. It also manages the input and output of signals and information output from the vehicle 2. The machine control unit 13 is exemplified by an information processing device such as a CPU, a memory, and a peripheral device.
[0055]
The underwater sound unit 14 belongs to the vehicle control system 12. An underwater acoustic device and a wave receiving device for receiving a reflected wave (echo) of a search signal (soner signal) emitted from the underwater acoustic device are provided. A search signal is transmitted to the target 3, and the reflected wave is received and measured. The search signal data (transmission time, search signal characteristics, etc.) and the reflected wave data (reception time, reflected wave characteristics, etc.) as the measurement data of the sonar are transmitted to the guidance control device 4 of the mother ship 1 via the transmission / reception unit 19. Is output to the sonar signal processing unit 10. The underwater acoustic device 14 is exemplified by a sonar.
[0056]
The ground speed detector 15 belongs to the vehicle control system 12, measures the (ground) speed of the vehicle 2, and outputs it as speed information. The ground speed detector 15 is exemplified by a Doppler sonar.
[0057]
The depth detector 16 belongs to the vehicle control system 12, measures the water depth of the vehicle 2, and outputs it as depth information. The depth detector 16 is exemplified by a depth gauge.
[0058]
The azimuth sensor 17 belongs to the navigation system control system 12, measures the traveling azimuth of the marine vehicle 2, and outputs it as azimuth information. The direction sensor 14 is exemplified by a direction magnet.
[0059]
The storage unit 18 belongs to the navigation system control system 12, and stores various measured information (speed information, depth information, azimuth information, search signal data, reflected wave data, etc.) and a navigation schedule (for example, time and (A table describing the position). The storage unit 18 is exemplified by a hard disk.
[0060]
The transmission / reception unit 19 belongs to the vehicle control system 12. When the vehicle 2 and the mother ship 1 perform communication using the cable 42, they function as an interface. The speed information, the depth information, the direction information, the search signal data, the reflected wave data, and the like are transmitted to the mother ship 1 via the cable 42. In addition, it receives a signal (power, control command, etc.) for controlling the marine vehicle 2 transmitted from the mother ship 1 and transmits the signal to each unit and device of the marine vehicle 2.
[0061]
The pitch angle sensor 20 belongs to the navigation system control system 12, measures the navigation angle of the navigation system of the navigation system 2, and outputs it as one of the azimuth information. The pitch angle sensor 20 is exemplified by an angle sensor.
[0062]
Here, with reference to FIG. 5, the underwater acoustic part 14 in the navigation system control system 12 of the navigation system 2 will be further described.
FIG. 5 is a side view showing a part of the configuration of the navigation body 2 according to the embodiment of the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention. The vehicle 2 includes a sonar 31 and a camera 32 of the underwater acoustic unit 14 in the vehicle control system 12. However, the sonar 31 and the camera 32 are inside the marine vehicle 2 and are clearly shown on the surface in the figure.
[0063]
The sonar 31 belongs to the underwater acoustic part 14, emits sound underwater, and grasps the position of the target 3 and the like from the reflected waves. Here, it is installed at the head of the marine vehicle 2. The sonar 31 can swing up and down, thereby changing the tilt angle.
The camera 32 belongs to the underwater acoustic part 14 and captures an underwater state. Here, it is installed at the head of the marine vehicle 2. Here, H indicates a plane parallel to the horizontal plane. The angle between the H plane and the direction of the sonar 31 is defined as a tilt angle (θ _TILT _ _d ), And the sound transmission range of the sonar 31 (the range that can be searched) is set to the beam range (θ). _R ).
[0064]
Next, the operation of the embodiment relating to the guidance control device, the guidance control system, the mother ship, the guidance control method and the program according to the present invention will be described with reference to the accompanying drawings. First, an algorithm applied to the guidance control device, the guidance control system, and the guidance control method will be described.
[0065]
FIG. 6 is a diagram illustrating a coordinate system used in a first algorithm related to the operation of the embodiment of the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention.
The first algorithm is an algorithm for moving the vehicle 2 to the vicinity of the target 3. In this example, it is assumed that the movement of the marine craft 2 is performed in a two-dimensional plane (horizontal plane), and the movement in the depth (water depth) direction is omitted. However, if an ordinary mathematical extension method to three dimensions is used, it can be applied to motion in three-dimensional space.
An absolute coordinate system xy is set, and the initial position of the craft 2 in the absolute coordinate system is set to (X ₀ , Y ₀ ), The position of the target 3 in the absolute coordinate system is (X _T , Y _T ), The azimuth of Vessel 2 and the relative azimuth of target 3 with respect to Vessel 2 _T , The relative distance of the target 3 to the vehicle 2 is R _T And However, the x ′ axis and the y ′ axis are (X ₀ , Y ₀ ) Is parallel to the x and y axes.
[0066]
The first algorithm in such a coordinate system will be described with reference to FIG.
FIG. 7 is a flowchart illustrating a first algorithm related to the operation of the embodiment of the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention.
[0067]
(0) The marine vehicle 2 has started moving toward the target 3 based on a command from the mother ship 1.
In the start stage, the following input data is input.
・ Vessel data (movement of Vessel 2)
Azimuth: ：: Measured by azimuth sensor 17 (azimuth information)
Ground speed u = (u _gx , U _gy ): Measured by the ground speed detector 15 (speed information).
・ Vessel position (absolute position of Vessel 2)
Coordinates: P ₀ = (X ₀ , Y ₀ ): Calculated from the absolute position of the mother ship 1 (mother ship position information: from the GPS receiver 41) and the relative position of the navigation body 2 with respect to the mother ship 1 (the navigation body relative position information: from the underwater acoustic unit 6) (the navigation body). location information).
・ Sonar data (Relative position of target 3 with respect to hull 2)
Relative distance: R _T : Measured by the underwater acoustic part 14 (target relative distance information).
Relative bearing: ψ _T : Measured by the underwater acoustic part 14 (target relative azimuth information).
・ Control parameters (judgment criteria and judgment target)
Position determination distance: R_ _d : Reference value (preset) (reference distance) used for control.
Relative distance: R _T : Position determination distance R_ _d Target to be compared with (control relative distance).
·constant
Sampling time: ΔT: sampling interval of input data (reference time).
[0068]
(1) Step S11: Calculation of absolute coordinates of target
Absolute position of target 3 (absolute coordinates) (X _T , Y _T ) Is calculated. The calculation is based on the relative position (relative coordinates) of the target 3 with respect to the hull 2 (X _t , Y _t ) Is calculated, and the absolute position (X) of the target 3 is determined based on the obtained value and the absolute position (absolute coordinates) of the vehicle 2. _T , Y _T ) Is calculated. Specifically, it is calculated by the following equation based on the input data.
(Equation 1)

here,
(Equation 2)

[0069]
(2) Step S12: Vessel position calculation
The absolute position (X, Y) of the vehicle 2 after the elapse of the ΔT time is calculated by integrating the ground speed of the vehicle 2. Specifically, it is calculated by the following equation based on the input data.
[Equation 3]

Where X_ _old , Y_ _old Are X and Y before (immediately before) ΔT time.
[0070]
(3) Step S13: Relative distance calculation
Using the absolute position of the vehicle 2 calculated in step S12, the relative position (relative distance) R of the target 3 with respect to the vehicle 2 after the elapse of ΔT time _T Is calculated. Specifically, it is calculated by the following equation based on the values calculated by the above equations (2), (3), (6), and (7).
(Equation 4)

[0071]
(4) Step S14: Position determination
Aircraft 2 moves to target 3 specified by the sonar scope with R_ _d It is determined whether or not the range has been reached. Specifically, the determination is made by the following equation based on the value calculated by the above equation (8).
(Equation 5)

R that satisfies this equation _T Is obtained, it is determined that the vehicle 2 has sufficiently approached the target 3. Then, the process proceeds to step S15.
R that satisfies this equation _T Is not obtained, it is determined that the vehicle 2 is not sufficiently close to the target 3. Then, the process returns to step S12 to perform the calculation after the next ΔT time.
[0072]
Note that R_ _d Can be freely changed in size depending on the work content. For example, if the target 3 is a dangerous substance, it is dangerous if approaching too much. _d Take a larger. Further, when working directly on the target 3, it is desirable to approach as close as possible without collision. _d Take smaller. Thereby, the working time can be reduced and the safety can be improved.
[0073]
Further, since the distance from the stop of the engine of the craft 2 to the stop of the movement of the craft 2 is different depending on the ground speed of the craft 2, R_ is determined according to the ground speed of the craft 2. _d , It is possible to more accurately guide the marine vehicle to the target 3. In this case, a table indicating the relationship between the ground speed of the craft 2 and the distance from the stop of the engine of the craft 2 to the stop of the movement of the craft 2 is prepared in the storage unit 7, and the table is provided in accordance with the ground speed. Table value is R_ _d In addition to R_ _d 'Is generated and its value may be used for the criterion equation (9).
[0074]
(5) Step S15: Process execution
Aircraft 2 is sufficiently close to target 3. Therefore, the vehicle 2 ends the approach to the target 3 and stops. Then, a predetermined operation (or an operation instructed by the mother ship 1) is executed. Note that the work may be instructed later, and may be made to wait near the target 3.
[0075]
Through the above steps S11 to S15, the first algorithm for moving the marine vehicle 2 to the vicinity of the target 3 is performed. This first algorithm can be created as a computer program and executed by a computer.
[0076]
Next, the second algorithm will be described.
FIG. 8 is a diagram illustrating a coordinate system used in a second algorithm related to the operation of the embodiment of the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention.
The second algorithm is an algorithm that controls the sonar 31 so that the sonar 31 of the marine vehicle 2 moving in the direction of the target 3 always faces in the direction of the target 3.
An absolute coordinate system xyz is set, and the initial position of the craft 2 in the absolute coordinate system is set to (X ₀ , Y ₀ , Z ₀ ), The position of the target 3 in the absolute coordinate system is (X _T , Y _T , Z _T ), The azimuth in the two-dimensional plane (x'y 'plane parallel to the xy plane, the same applies hereinafter) of the craft 2 ψ, and the relative azimuth of the target 3 in the two-dimensional plane with respect to the craft 2 ψ _T , The relative azimuth of the target 3 in the Z-axis direction with respect to the _T , The relative distance of the target 3 to the vehicle 2 is R _T And However, the x ′ axis, y ′ axis, and z ′ axis are (X ₀ , Y ₀ , Z ₀ ) Are parallel to the x, y, and z axes.
[0077]
The second algorithm in such a coordinate system will be described with reference to FIG.
(0) The marine vehicle 2 has started moving toward the target 3 based on a command from the mother ship 1.
In the start stage, the following input data is input.
・ Vessel data (movement of Vessel 2)
Azimuth: ：: Measured by the azimuth sensor 17 (azimuth information).
Vessel elevation angle: measured by pitch angle sensor 20 (included in bearing information).
Depth: Z: Measured by the depth detector 16 (depth information).
Ground speed u = (u _gx , U _gy ): Measured by the ground speed detector 15 (speed information).
・ Vessel position (absolute position of Vessel 2)
Coordinates: P ₀ = (X ₀ , Y ₀ ): Calculated from the absolute position of the mother ship 1 (mother ship position information: from the GPS receiver 41) and the relative position of the navigation body 2 with respect to the mother ship 1 (the navigation body relative position information: from the underwater acoustic unit 6) (the navigation body). location information).
・ Sonar data (Relative position of target 3 with respect to hull 2)
Relative distance: R _T : Measured by the underwater acoustic part 14 (target relative distance information).
Relative bearing: ψ _T : Measured by the underwater acoustic part 14 (target relative azimuth information).
Relative pitch angle: θ _T : Measured by the underwater acoustic part 14 (target relative pitch angle information).
・ Control parameters (judgment criteria and judgment target)
Target tilt angle: θ _TILT : Tilt angle to be controlled by the sonar 31.
Tilt angle judgment angle: Δθ _TILT Target pitch angle θ _P The reference value of the angle range allowed from (preset).
Where Δθ _TILT <Θ _R It is.
(Actual tilt angle θ _TILT _ _d : Actual tilt angle of the sonar 31).
·constant
Sampling time: ΔT: sampling interval of input data.
[0078]
(1) Step S21: Absolute coordinate calculation of target
Absolute position of target 3 (absolute coordinates) (X _T , Y _T , Z _T ) Is calculated. The calculation is based on the relative position (relative coordinates) of the target 3 with respect to the hull 2 (X _t , Y _t , Z _t ) Is calculated, and the absolute position (X) of the target 3 is determined based on the obtained value and the absolute position (absolute coordinates) of the vehicle 2. _T , Y _T , Z _T ) Is calculated. Specifically, it is calculated by the following equation based on the input data.
(Equation 6)

[0079]
(2) Step S22: Tilt angle initialization
An initial value of the tilt angle is set in preparation for the subsequent calculations. Specifically, it is as follows.
(Equation 7)

[0080]
(3) Step S23: Calculation of the position of the vehicle
The absolute position (X, Y, Z) of the vehicle 2 after the elapse of the ΔT time is calculated by integrating the ground speed of the vehicle 2. As for the depth Z, the value of the depth detector 16 is used. Specifically, it is calculated by the following equation based on the input data.
(Equation 8)

Where X_ _old , Y_ _old , Z_ _old Are X, Y and Z before (immediately before) ΔT time.
[0081]
(4) Step S24: Target pitch angle calculation
The absolute position and depth deviation (= Z) of the hull 2 calculated in step S23 _T -Z), the pitch angle (target pitch angle θ) of the target 3 with respect to the vehicle 2 after the lapse of ΔT time _P ) Is calculated. Specifically, it is calculated by the following formula based on the values calculated by the above formulas (12) to (14) and (17) to (19).
(Equation 9)

[0082]
(5) Step S25: Tilt angle control judgment
Target pitch angle θ calculated in (4) _P (Direction of target 3 specified by sonar scope) and target tilt angle θ of sonar 31 _TILT Then, it is determined whether or not to control the tilt angle based on the deviation from. The judgment is that the deviation is a preset Δθ _TILT Perform whether or not it is within the range. Specifically, the determination is made by the following equation based on the value calculated by the above equation (20).
(Equation 10)

θ _P Satisfies this expression, the tilt angle θ of the sonar 31 of the _TILT _ _d (Target tilt angle θ _TILT Has a sufficient angle, and determines that the target 3 is not lost. Then, the process proceeds to step S27.
θ _P Does not satisfy this expression, the tilt angle θ of the sonar 31 of the _TILT _ _d Has an insufficient angle and determines that there is a possibility that the target 3 may be lost. Then, the process proceeds to step S26.
Note that Δθ _TILT Is a value determined by the performance of the sonar 31 and is, for example, 20 °.
[0083]
(6) Step S26: Target tilt angle change
If the determination formula (21) in (5) is not satisfied, the target tilt angle is calculated so that the deviation between the target pitch angle and the target tilt angle is reduced, and the tilt angle is controlled using the calculated value. I do. Specifically, the determination is made by the following equation based on the value calculated by the above equation (20) and the current tilt angle and the target tilt angle.
[Equation 11]

By doing so, the target 3 does not protrude from the range of the sonar 31 and the possibility of losing the target 3 is eliminated.
Proceed to step S27.
[0084]
(7) Step S27: Calculation of target beam elevation angle
The deviation between the target pitch angle and the target tilt angle is used as the target beam elevation angle, and the target 3 is supplemented with a beam. Specifically, based on the values calculated by the above equations (20) and (22), the determination is made by the following equation.
(Equation 12)

[0085]
Through the above steps S21 to S27, a second algorithm for constantly pointing the angle of the sonar 31 of the marine vehicle 2 toward the target 3 is performed. This second algorithm can be created as a computer program and executed by a computer.
[0086]
Next, details of the operation of the embodiment to which the guidance control device, the guidance control system, the mother ship, the guidance control method, and the program according to the present invention are applied will be described.
[0087]
First, an operation related to the control of the vehicle 2 and the sonar 31 using the first and second algorithms will be described.
i) The mother ship 1 sends out the mounted hull 2 to the sea in the exploration sea area. The mother ship 1 and the marine vessel 2 can communicate with each other via a cable 42 (undersea radio communication may be used).
ii) The guidance control unit 5 of the guidance control device 4 of the mother ship 1 includes a movement command (e.g., ground speed, azimuth, depth / latitude, longitude, water depth, etc.) from the input / output unit 9 to the storage unit, or a storage unit. Based on the movement command according to the movement schedule 7, the movement command is transmitted from the transmission / reception unit 8 to the navigation system control system 12 of the boat 2 via the cable 42.
iii) The vehicle 2 receives a movement command via the transmission / reception unit 19. Then, movement is performed based on the movement command.
iv) The GPS receiver 41 of the mother ship 1 calculates the position of the mother ship 1 based on the GPS signals from the GPS satellites 40. Then, the position is output to the guidance control device 4 as mother ship position information. The mother ship position information is held in the storage unit 7 together with the time.
iv) In the mother ship 1, the underwater acoustic unit 6 transmits a search signal as a mother ship search signal to the outside of the ship at an appropriate time (for example, ΔT) according to a command from the guidance control unit 5 of the guidance control device 4. . At the same time, a reflected wave of the mother ship as a reflected wave of the mother ship search signal is received. The reflected wave in this case is reflected from the marine vehicle 2. Then, based on the mother ship exploration signal and the reflected wave of the mother ship, navigation vehicle relative position information indicating the relative position of the navigation body 2 with respect to the mother ship 1 is calculated. Furthermore, based on the mother ship position information and the relative position information of the navigation body, the navigation body position information indicating the absolute position of the navigation body 2 is calculated. The information is stored in the storage unit 7. In addition, as a method for the underwater acoustic unit 6 to obtain the relative position information of the vehicle based on the search signal, a method known for a conventional sonar can be used.
However, the method of calculating the vehicle relative position information is not limited to the above method.
v) In the vehicle 2, the ground speed detector 15 measures the ground speed according to a command from the hull control unit 16 of the vehicle control system 12. The depth detector 16 measures the water depth (depth). Further, the direction sensor 17 measures the direction of travel. The measured values are stored in the storage unit 18 together with the measurement time.
vii) In the vehicle 2, the underwater acoustic unit 14 outputs an exploration signal (sonar signal) to the outside of the ship at an appropriate time (for example, ΔT) by a command from the body control unit 13 of the vehicle control system 12. Outgoing. At the same time, a reflected wave (echo) of the search signal is received. Then, search signal data (transmission time, search signal characteristics, etc.) and reflected wave data (reception time, reflected wave characteristics, etc.), speed information (ground speed) at the time closest to the transmission time or reception time, depth information ( The water depth) and the azimuth information (the azimuth in the traveling direction) are transmitted from the transmission / reception unit 19 to the guidance control device 4 of the mother ship 1 via the cable 42.
viii) The guidance control device 4 of the mother ship 1 uses the transmission / reception unit 8 to measure data (exploration signal data and reflected wave data), speed information (ground speed), depth information (water depth), and azimuth information (azimuth in the traveling direction). Is received via the cable 42. Then, the information is temporarily stored in the storage unit 7.
ix) The guidance control device 4 of the mother ship 1 uses the sonar signal processing unit 10 to perform target relative position information (relative position information indicating the relative position of the target 3 with respect to the hull 2 based on the measurement data (exploration signal data and reflected wave data). The target relative distance as the relative distance, the target relative azimuth as the horizontal relative azimuth of the target 3 with respect to the hull 2, and the target relative pitch angle as the relative depression angle with respect to the hull 2 of the target 3) are calculated. The calculation result is stored in the storage unit 7. The sonar signal processing unit 10 can obtain the target relative position information from the search signal by using a method known for a conventional sonar.
x) In the guidance control unit 5, the guidance control device 4 of the mother ship 1 uses the navigation body data (azimuth information, depth information, speed information), the navigation body position (the navigation body position information), and the sonar data (target relative distance). Based on the target relative azimuth and the target relative pitch angle), a control signal for the target 3 is generated and output to the marine vehicle 2 via the cable 3 to control the operation.
xi) At the same time, the guidance control device 4 of the mother ship 1 uses the guidance control unit 5 to control the navigation vehicle data (azimuth information, depth information, speed information), the navigation vehicle position (the navigation vehicle position information), and the sonar data ( Using the target relative distance, target relative azimuth, target relative pitch angle) and control parameters (target tilt angle, tilt angle determination angle, etc.), the second algorithm is used to ensure that the vehicle 2 does not lose sight of the target 3. The tilt angle of the sonar 31 of the running body 2 is monitored and controlled.
xii) In addition, the guidance control device 4 of the mother ship 1 uses the guidance control unit 5 to control the vehicle data (direction information and speed information), the vehicle position (vehicle position information), and the sonar data (target relative). Using the distance, the target relative azimuth, and the control parameters (position determination distance, etc.), the distance R between the vehicle 2 and the target 3 is calculated by the first algorithm. _T Is R_ _d Monitor if:
xiii) (ii) to (xii) are repeated as appropriate.
xiv) The guidance control device 4 of the mother ship 1 uses the guidance control unit 5 to determine the distance R between the hull 2 and the target 3 _T Is R_ _d In the following cases, a stop signal is output to the traveling body 2 and a signal (processing execution signal) indicating a command for performing a preset operation is output.
xv) Based on the stop signal and the process execution signal from the guidance control device 4, the aircraft control unit 13 of the navigation body 2 stops, and executes a preset operation.
[0088]
By the above operation, the navigation body 2 is stopped near the target 3 and the work is performed on the target 3 using the guidance control method, the guidance control device, the guidance control system, and the program for causing a computer to execute the method. It becomes possible.
[0089]
Further, the tilt angle and the elevation angle of the sonar 31 can be automatically controlled. Then, the time spent by the operator of the hull 2 for operations other than the operation (control of the tilt angle and the elevation angle) can be significantly reduced, and the burden on the operator can be greatly reduced.
[0090]
By performing the position determination (4) of the first algorithm more accurately (decreasing ΔT), it is possible to more strictly control the stop position of the navigation body 2.
[0091]
Next, an operation of setting the course of the marine vehicle 2 to the target 3 on the display screen 11-1 shown in FIG. 3 will be described.
A) FIG. 3 (a)
On the display screen 11-1, the state within the measurement range by the search signal of the sonar 31 of the marine vehicle 2 is displayed. Here, when the target 3 exists, a reflected wave from the target 3 is generated. The vehicle 2 transmits the measurement data (exploration signal data and reflected wave data) to the guidance control device 4 of the mother ship 1.
In the guidance control device 4, the sonar signal processing unit 10 generates second relative position information (target relative distance as relative distance, relative azimuth as relative distance) based on the measurement data. The target relative azimuth and the target relative pitch angle as a relative depression angle are calculated. Then, the guidance control unit 5 causes the operation display unit 11 to expand the measurement range of the sonar 31 on the display screen 11-1 based on the second relative position information. Then, the operation display unit 11 displays an image of the target 3 (echo 22).
However, the center direction of the display screen 11-1 is the direction of the tilt angle, and the range of the display screen 11-1 is the range of the beam range.
[0092]
B) FIG. 3 (b)
The operator of the vehicle 2 performs the following operation in order to move the vehicle 2 to the target 3 (echo 22). That is, on the display screen 11-1, the cursor 21 is moved by the trackball, aligned with the echo 22, and the target designation button is pressed. As a result of the operation, the cursor 21 is changed to the target designation symbol 23, and the first place as the place where the echo 22 is newly located is input as the target of the vehicle 2.
The position of the target designation symbol 23 on the display screen 11-1 is calculated by the operation display unit 11 as a new moving direction with respect to the marine vehicle 2. Then, the calculated relative position (relative distance, relative direction, relative pitch angle) is output to the guidance control unit 5 as first relative position information (symbol relative distance information, symbol relative direction information, symbol relative pitch angle information). You. The guidance control unit 5 outputs the first relative position information as a moving target of the navigation body 2 to the navigation body 2. The vehicle 2 moves to the position specified by the received information.
[0093]
Thereafter, a change in the position of the target designation symbol 23 is returned from the marine vessel 2 at regular intervals in accordance with the motion of the marine vessel 2. The guidance control unit 5 outputs the movement of the position of the target designation symbol 23 to the operation display unit 11 based on the information. Thereby, the position of the target designation symbol 23 on the display screen 11-1 moves.
On the other hand, the position of the echo 22 (target 3) (shown in the second relative position information) is calculated from the measurement data by the sonar 31 independently of the position recognized by the marine vehicle 2. Accordingly, the position of the echo 22 also changes according to the motion of the marine vehicle 2, but also changes separately from the position of the target designation symbol.
[0094]
C) FIG. 3 (c)
On the display screen 11-1, a displacement may occur between the echo 22 (the second relative position information) and the target designation symbol 23 (the first relative position information) due to the accumulation of the position measurement errors of the vehicle 2. In this case, the target designation symbol 23 is hooked (dragged) by the cursor 21, moved, adjusted to the correct position of the echo 22 on the sonar scope, and the position is adjusted by pressing the position adjustment button. Thereby, the second location as a location where a new echo is present is input.
The position on the display screen 11-1 of the position-corrected target designation symbol 23 is calculated by the operation display unit 11 as a new moving direction with respect to the marine vehicle 2. Then, the calculated relative position (relative distance, relative direction, relative pitch angle) is output to the guidance control unit 5 as third relative position information. The guidance control unit 5 outputs the third relative position information as a moving target of the navigation body 2 to the navigation body 2. The vehicle 2 moves to the position specified by the received information.
[0095]
The steps (B) and (C) are repeated as necessary until the target is reached.
By performing the above process, it is possible to easily set the course of the traveling body to a position (target) specified on the display screen. In addition, even when the vehicle deviates from the target at an intermediate stage, the course can be easily corrected.
[0096]
The above process describes an example of a mother ship and a submerged underwater vehicle such as an ocean explorer.
[0097]
【The invention's effect】
ADVANTAGE OF THE INVENTION According to this invention, it becomes possible to easily operate the navigation body controlled by the mother ship and traveling toward the target on the mother ship without losing the target.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram illustrating a configuration related to an embodiment of a guidance control device, a guidance control system, a mother ship, a guidance control method, and a program according to the present invention.
FIG. 2 is a block diagram showing a configuration of guidance control on a mother ship applied in the embodiment.
FIG. 3 is an explanatory diagram illustrating an example of a display screen of an operation display unit according to the embodiment.
FIG. 4 is a block diagram showing a configuration of a vehicle control system applied in the embodiment.
FIG. 5 is a side view showing a part of the configuration of the vehicle according to the embodiment.
FIG. 6 is an explanatory diagram showing a coordinate system used for a first algorithm related to the operation of the embodiment.
FIG. 7 is a flowchart showing a first algorithm related to the operation of the embodiment.
FIG. 8 is an explanatory diagram showing a coordinate system used for a second algorithm related to the operation of the embodiment.
FIG. 9 is a flowchart showing a second algorithm related to the operation of the embodiment.
[Explanation of symbols]
1 mother ship
2 Aircraft
3 goals
4 Guidance control device
5 Guidance control unit
6 Underwater sound section
7 Storage unit
8 Transceiver
9 Input / output unit
10 Sonar signal processing unit
11 Operation display
11-1 Display screen
12 Aircraft control system
13 Aircraft control unit
14 Underwater acoustic part
15 Ground speed detector
16 Depth detector
17 Direction sensor
18 Memory
19 Transceiver
21 cursor
22 echo
23 Target designation symbol
31 Sonar
32 cameras
40 GPS satellites
41 GPS receiver
42 cable

Claims (21)

It has a screen display means for displaying a search signal, and displays an echo representing an object in the search signal of the sailing body sent from the mother ship on the screen display means, as a place where the echo is on the screen display means. An operation display unit for calculating a first relative position as a relative position of the first place with respect to the navigation body when the first place is designated;
A guidance control device, comprising: a guidance control unit that controls the navigation body based on the first relative position so that a moving target of the navigation body is the first relative position. A search signal processing unit that calculates a second relative position as a relative position of the echo to the ship based on the search signal output from the ship,
The guidance control device according to claim 1, wherein the operation display unit displays the echo on the screen display unit based on the second relative position. The operation display unit, when the first relative position and the second relative position are shifted on the screen display means, when a second place as a place indicating the second relative position is designated again, Calculating the third relative position as a relative position of the second place with respect to the navigation body;
The guidance control device according to claim 1, wherein the guidance control unit controls the navigation body so that a moving target of the navigation body is the third relative position. A guidance control system, comprising: the guidance control device according to claim 1; and the navigation body controlled by the guidance control device. A navigation unit that is sent from the mother ship and is controlled from the outside, and includes a guidance control device that controls the navigation unit,
The sailing body has direction information as a direction to which the sailing body is facing, speed information as a ground speed of the sailing body, and a relative position of a target of the sailing body with respect to the sailing body. Measure and output the target relative position information to the guidance control device,
The guidance control device, the azimuth information, the speed information, the target relative position information, based on the navigation body position information as the position of the navigation body, the navigation body and the target A guidance control system which calculates a control relative distance as a relative distance. 6. The guidance control system according to claim 5, wherein the guidance control device controls the navigation body so as to stop the navigation body when the control relative distance is equal to or less than a reference distance as a preset distance. 7. The guidance control according to claim 5, wherein the guidance control device controls the navigation body to perform a preset operation when the control relative distance is equal to or less than a reference distance as a preset distance. 8. system. A navigation unit that is sent from the mother ship and is controlled from the outside, and includes a guidance control device that controls the navigation unit,
The sailing body has direction information as a direction to which the sailing body is facing, speed information as a ground speed of the sailing body, and a relative position of a target of the sailing body with respect to the sailing body. Measure and output the target relative position information to the guidance control device,
The guidance control device, based on the azimuth information, the speed information, the target relative position information, and the navigation body position information as the position of the navigation body, determines the target from the horizontal plane and the navigation body. A guidance control system, which calculates a target pitch angle as an angle formed by a direction toward the vehicle. The sailing body measures a tilt angle as an angle formed by a direction of a sonar and a horizontal plane of the sailing body and outputs the measured tilt angle to the guidance control device,
The guidance control device controls the navigation body such that an absolute value of a difference between a target tilt angle that is a target value of the tilt angle and the target pitch angle is equal to or smaller than a reference angle as a preset angle. 9. The guidance control system according to claim 8, which controls. A mother ship comprising the guidance control system according to any one of claims 4 to 9 and a GPS receiver capable of specifying its own position. Displaying, on a screen display means, an exploration signal of the cruising vehicle transmitted from the mother ship;
Designating a first place as a place where an echo representing an object displayed on the screen display means exists;
Calculating a first relative position as a relative position of the first place with respect to the navigation body;
Transmitting a control signal to the marine vehicle with the moving target of the marine vehicle as the first relative position. The step of displaying the search signal on the screen display means,
Receiving the search signal from the vehicle;
Calculating a second relative position as a relative position of the echo with respect to the vehicle based on the received search signal;
12. The guidance control method according to claim 11, further comprising: displaying the echo on the screen display means based on the second relative position. When the first relative position and the second relative position are displaced on the screen display means, re-designating a second location as a location indicating the second relative position;
Calculating the third relative position as a relative position of the second place with respect to the navigation body;
13. The guidance control method according to claim 12, further comprising: transmitting a control signal for setting the moving target of the navigation body to the third relative position to the navigation body. Azimuth information as the heading of the navigation body sent from the mother ship, target relative position information as a relative position of the target with respect to the navigation body, and navigation body position information as the position of the navigation body Calculating target position information as the position of the target based on
Based on the azimuth information, the speed information as the ground speed of the sailing body, and the sailing body position information, a new sailing body as a position of the sailing body after a predetermined reference time has elapsed. Calculating location information;
Based on the target position information and the new marine vehicle position information, calculating a control relative distance as a relative distance between the marine vehicle and the target,
When the control relative distance is equal to or less than a reference distance as a preset distance, stopping the marine vehicle.
The guidance control method, wherein the target is a target of movement of the marine vehicle. The guidance control method according to claim 14, wherein the step of stopping the navigation body includes a step of performing a predetermined operation after the navigation body stops. 16. The guidance control method according to claim 14 or 15, wherein the step of stopping the vehicle includes a step of returning to the step of calculating the new vehicle position information when the control relative distance is larger than the reference distance. Azimuth information as the heading of the navigation body sent from the mother ship, target relative position information as a relative position of the target with respect to the navigation body, and navigation body position information as the position of the navigation body Calculating target position information as the position of the target based on
Based on the azimuth information, the speed information as the ground speed of the sailing body, and the sailing body position information, a new sailing body as a position of the sailing body after a predetermined reference time has elapsed. Calculating location information;
Based on the target position information and the new marine vehicle position information, calculating a target pitch angle as an angle between a horizontal plane and a direction from the marine vehicle to the target,
The absolute value of the difference between the target tilt angle, which is the target value of the tilt angle as the angle formed by the sonar of the sailing body and the horizontal plane, and the target pitch angle is larger than the reference angle as the preset angle In the case, based on the target pitch angle, the tilt angle, and the target tilt angle, a step of calculating a new target tilt angle that is a target value of the tilt angle after the elapse of the reference time,
Calculating a target beam elevation angle that is a target value of a beam elevation angle as an angle between the beam of the sonar and a horizontal plane, based on the target pitch angle and the new target tilt angle,
The guidance control method, wherein the target is a target of movement of the marine vehicle. The step of calculating the new target tilt angle, if the absolute value is equal to or less than the reference angle, proceed to the step of calculating the target beam elevation angle,
18. The guidance control method according to claim 17, wherein the step of calculating the target beam elevation angle calculates the target beam elevation angle based on the target pitch angle and the target tilt angle. Displaying, on a screen display means, an exploration signal of the cruising vehicle transmitted from the mother ship;
Calculating a first relative position as a relative position of the first place with respect to the navigation body when a first place as a place where an echo representing an object displayed on the screen display means is designated; ,
Transmitting a control signal to the marine vehicle with the moving target of the marine vehicle as the first relative position,
The step of displaying the search signal on the screen display means includes the steps of: receiving the search signal from the marine vehicle; and, based on the received search signal, as a relative position of the echo to the marine vehicle. Calculating a second relative position, and displaying the echo on the screen display means based on the second relative position,
Further, when the first relative position and the second relative position are displaced on the screen display means, and a second place as a place indicating the second relative position is newly designated, the second place Calculating the third relative position as a relative position with respect to the vehicle,
Transmitting a control signal for setting the moving target of the navigation body to the third relative position to the navigation body, and causing a computer to execute those steps. Orientation information as the heading of the navigation body sent from the mother ship, target relative position information as the relative position of the target of the navigation body with respect to the navigation body, and navigation as the position of the navigation body Calculating target position information as the target position based on the running body position information;
Based on the azimuth information, the speed information as the ground speed of the sailing body, and the sailing body position information, a new sailing body as a position of the sailing body after a predetermined reference time has elapsed. Calculating location information;
Based on the target position information and the new marine vehicle position information, calculating a control relative distance as a relative distance between the marine vehicle and the target,
If the control relative distance is larger than a reference distance as a preset distance, returning to the step of calculating the new marine vehicle position information,
When the control relative distance is equal to or less than the reference distance, stopping the marine vehicle. Azimuth information as the heading of the navigation body sent from the mother ship, target relative position information as a relative position of the target with respect to the navigation body, and navigation body position information as the position of the navigation body Calculating target position information as the position of the target based on
Based on the azimuth information, the speed information as the ground speed of the sailing body, and the sailing body position information, a new sailing body as a position of the sailing body after a predetermined reference time has elapsed. Calculating location information;
Based on the target position information and the new marine vehicle position information, calculating a target pitch angle as an angle between a horizontal plane and a direction from the marine vehicle to the target,
The absolute value of the difference between the target tilt angle, which is the target value of the tilt angle as the angle formed by the sonar of the sailing body and the horizontal plane, and the target pitch angle is larger than the reference angle as the preset angle In the case, based on the target pitch angle, the tilt angle, and the target tilt angle, a step of calculating a new target tilt angle that is a target value of the tilt angle after the elapse of the reference time,
When the absolute value is equal to or less than the reference angle, returning to the step of calculating the target pitch angle;
Calculating, based on the target pitch angle and the new target tilt angle, a target beam elevation angle which is a target value of a beam elevation angle as an angle formed by the sonar beam with a horizontal plane. program.

Images