JP7000987B2 - Navigation control method and navigation control device - Google Patents
Navigation control method and navigation control device Download PDFInfo
- Publication number
- JP7000987B2 JP7000987B2 JP2018096449A JP2018096449A JP7000987B2 JP 7000987 B2 JP7000987 B2 JP 7000987B2 JP 2018096449 A JP2018096449 A JP 2018096449A JP 2018096449 A JP2018096449 A JP 2018096449A JP 7000987 B2 JP7000987 B2 JP 7000987B2
- Authority
- JP
- Japan
- Prior art keywords
- speed
- target
- target position
- navigation
- determination
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 145
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 57
- 238000012545 processing Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 10
- 238000010586 diagram Methods 0.000 description 8
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 description 8
- 238000001514 detection method Methods 0.000 description 7
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011144 upstream manufacturing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000001141 propulsive effect Effects 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
本発明は、航走体を定点保持させるための航走制御方法および航走制御装置に関するものである。 The present invention relates to a navigation control method and a navigation control device for holding a navigation body at a fixed point.
水上無人航走体（Unmanned Surface Vehicle：以下、ＵＳＶという）や水中無人航走体（Unmanned Undersea Vehicle：以下、ＵＵＶという）などの無人航走体は、定点観測を行う場合、および、母船や他の航走体との会合を行う場合などに、設定された時間、同じ位置に留まる定点保持を行うことが要求される。 Unmanned Undersea Vehicles (hereinafter referred to as USVs) and unmanned Undersea Vehicles (hereinafter referred to as UUVs) are used for fixed-point observations, motherships, and others. It is required to hold a fixed point that stays at the same position for a set time, such as when meeting with a vehicle.
しかし、ＵＳＶには、風、波、潮流などの外乱が作用することがある。また、ＵＵＶには、潮流などの外乱が作用することがある。そのため、これらの無人航走体による定点保持を行わせるためには、外乱により無人航走体に引き起こされる位置ずれを補償する航走制御が必要とされる。 However, disturbances such as winds, waves, and tides may act on the USV. In addition, disturbances such as tidal currents may act on the UUV. Therefore, in order to hold the fixed point by these unmanned vehicles, it is necessary to control the navigation to compensate for the misalignment caused by the unmanned vehicle due to the disturbance.
ところで、無人航走体には、設定された航走速度で移動（航走）しながら周囲の状況を調査するようにしてあるクルージング型（巡航型）の無人航走体がある。 By the way, as an unmanned navigator, there is a cruising type (cruising type) unmanned navigator that is designed to investigate the surrounding situation while moving (navigating) at a set cruising speed.
クルージング型の無人航走体は、船上スペースやハンドリングの都合でできるだけ小型であること、また安価であることが望まれる。そのため、無人航走体の運動は、前進、後進方向の推力を生じるメインスラスタの制御と、舵（操舵装置）の制御により実施して、サイドスラスタのような追加装備に依存しないことが基本とされる。 It is desired that the cruising type unmanned navigation body be as small as possible and inexpensive due to space on board and handling. Therefore, the movement of the unmanned vehicle is basically carried out by controlling the main thruster that generates thrust in the forward and reverse directions and the control of the rudder (steering device), and does not depend on additional equipment such as the side thruster. Will be done.
したがって、無人航走体では、外乱を受ける条件の下での定点保持についても、メインスラスタの制御と舵の制御によって実現することが望まれている。 Therefore, in the unmanned vehicle, it is desired to realize the fixed point holding under the condition of being subjected to the disturbance by the control of the main thruster and the control of the rudder.
なお、特定の一方向にのみ移動体を推進させる推進力発生部と、移動する方向を調整する移動方向調整部とを備えた移動体について、定点に留める制御を行う移動体の制御手法は従来提案されている（例えば、特許文献１参照）。 It should be noted that the conventional method for controlling a moving body, which controls the moving body having a propulsion force generating unit for propelling the moving body only in a specific direction and a moving direction adjusting unit for adjusting the moving direction, to keep the moving body at a fixed point. It has been proposed (see, for example, Patent Document 1).
この移動体制御手法は、移動体が留まるべき定点の位置が設定され、定点の位置と移動体の位置とを基に定点から移動体までの距離を算出し、算出された距離が設定された閾値以上になると、移動体が外乱要素によって流される方向を検出し、次いで、定点よりも流される方向の上流側に目標位置を設定し、この目標位置に到達するように、移動体の推進力および移動方向を制御するものとされている。 In this moving body control method, the position of the fixed point where the moving body should stay is set, the distance from the fixed point to the moving body is calculated based on the position of the fixed point and the position of the moving body, and the calculated distance is set. When the threshold is exceeded, the direction in which the moving body is swept by the disturbance element is detected, then the target position is set on the upstream side in the direction in which the moving body is swept from the fixed point, and the propulsive force of the moving body is reached so as to reach this target position. And it is supposed to control the direction of movement.
ところが、特許文献１に示された手法では、移動体は、外乱要素によって流されて、位置保持すべき定点からの距離が設定された閾値になる位置と、定点よりも流される方向の上流側に設定された目標位置との間を移動するようになる。
However, in the method shown in
したがって、特許文献１に示された手法では、移動体は、定点保持を行う際に、外乱要素により流される方向に沿い、設定された前記閾値と、前記定点から目標位置までの距離との和に対応する距離を移動するようになるため、移動体の位置保持の精度をあまり高めることができない。
Therefore, in the method shown in
そこで、本発明は、航走体の定点保持を行う際に、位置を保持する精度の向上化を図ることができる航走制御方法および航走制御装置を提供しようとするものである。 Therefore, the present invention is intended to provide a navigation control method and a navigation control device capable of improving the accuracy of holding a position when holding a fixed point of a navigation body.
本発明は、前記課題を解決するために、設定された目標位置を中心として設定された半径の判定円を作成する処理と、前記判定円と、航走体の位置の情報とを基に、前記航走体の位置が、前記判定円よりも外側に存在するか否かを判断する判断処理と、前記判断処理の結果、前記航走体が前記判定円よりも外側の領域に存在すると判断された場合は、前記航走体を前記目標位置に向けて前進航走させる目標方位航走処理と、前記判断処理の結果、前記航走体が前記判定円以内の領域に存在すると判断された場合は、前記航走体の推進装置による前進方向の推力を停止させる処理と、を行う航走制御方法とする。 In order to solve the above problem, the present invention is based on a process of creating a determination circle having a radius set around a set target position, the determination circle, and information on the position of a navigation body. As a result of the determination process for determining whether or not the position of the navigating body is outside the determination circle and the determination process, it is determined that the navigator exists in the region outside the determination circle. If so, as a result of the target directional navigation process for causing the vehicle to advance forward toward the target position and the determination process, it is determined that the vehicle is within the region within the determination circle. In this case, the navigation control method is such that the process of stopping the thrust in the forward direction by the propulsion device of the traveling body is performed.
前記目標方位航走処理は、速度制御と方位制御とを備え、前記速度制御は、前記航走体が現在位置から前記目標位置に接近する方向へ前進移動するための対地速度の目標値を設定し、外乱が存在している環境下においても前記航走体の対地速度が、設定された前記対地速度の目標値に一致するように回転速度指令値を定めて、該回転速度指令値を前記推進装置へ与える処理を行い、前記方位制御は、前記推進装置の運転により前記航走体が前進するときの進行方向が、前記目標位置に向くように、舵角度指令値を定めて、該舵角度指令値を、前記航走体の舵へ与える処理を行う航走制御方法としてもよい。 The target azimuth navigation process includes speed control and azimuth control, and the speed control sets a target value of the ground speed for the traveling body to move forward from the current position in a direction approaching the target position. However, even in an environment where disturbance exists, the rotation speed command value is set so that the ground speed of the traveling body matches the set target value of the ground speed, and the rotation speed command value is used as described above. The process of giving to the propulsion device is performed, and the orientation control determines the steering angle command value so that the traveling direction when the traveling body advances by the operation of the propulsion device faces the target position, and the steering is performed. It may be a navigation control method that performs a process of giving an angle command value to the steering wheel of the traveling body.
前記判定円の円周上を前記航走体が通過する位置の変化量が、設定された範囲内に収まると、位置制御処理を行い、前記位置制御処理では、前記推進装置の回転速度と、前記航走体の水平航走時の対水速度との関係を計測したデータを、予め備え、前記航走体が前記判定円の外側から内側に進入するときの対地速度と、前記推進装置の回転速度を基にデータを用いて算出した対水速度を用いて、前記航走体の周囲に存在している水の推定水速度を求める処理と、前記推定水速度が、前記舵の効きが担保される最低限の速度よりも小さいか、否かを判断する速度判断処理と、を行い、更に、前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度よりも小さいと判断された場合は、前記データを基に、前記舵の効きが担保される最低限の速度に対応する対水速度を得るために必要な前記推進装置の回転速度を算出する処理と、算出された回転速度に対応する回転速度指令値を、前記推進装置へ与える処理と、を行い、前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度以上であると判断された場合は、対水速度を、前記推定水速度に前記移動体の対水速度のオープンループ制御を行う際の速度誤差以上の値を加えた値に設定する処理と、設定された対水速度を得るために必要な前記推進装置の回転速度を算出する処理と、算出された回転速度に対応する回転速度指令値を、前記推進装置へ与える処理と、を行う航走制御方法としてもよい。 When the amount of change in the position where the traveling body passes on the circumference of the determination circle is within the set range, the position control process is performed, and in the position control process, the rotation speed of the propulsion device and the rotation speed of the propulsion device are determined. Data that measures the relationship between the traveling body and the water speed during horizontal traveling is prepared in advance, and the ground speed when the traveling body enters from the outside to the inside of the determination circle and the propulsion device. Using the water speed calculated using data based on the rotation speed, the process of obtaining the estimated water speed of the water existing around the traveling body and the estimated water speed are the effects of the steering. A speed determination process for determining whether or not the speed is smaller than the guaranteed minimum speed is performed, and as a result of the speed determination process, the estimated water speed is the minimum at which the effectiveness of the steering wheel is guaranteed. If it is determined that the speed is smaller than the speed of, the rotation speed of the propulsion device required to obtain the water speed corresponding to the minimum speed at which the effectiveness of the steering is guaranteed is calculated based on the data. And the process of giving the rotation speed command value corresponding to the calculated rotation speed to the propulsion device, and as a result of the speed determination process, the estimated water speed is guaranteed to be effective by the steering wheel. If it is determined that the speed is equal to or higher than the minimum speed, the water resistance is set to a value obtained by adding a value equal to or higher than the speed error when performing open loop control of the water speed of the moving body to the estimated water speed. Processing, processing to calculate the rotation speed of the propulsion device required to obtain the set water speed, and processing to give a rotation speed command value corresponding to the calculated rotation speed to the propulsion device. It may be used as a navigation control method for performing the above.
前記位置制御処理は、前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度以上であると判断された場合に、前記回転速度指令値を前記推進装置へ与える処理の後、前記航走体の位置の変化を、設定された時間計測する処理と、計測された前記航走体の位置の変化が、前記目標位置に近付く方向の変化の場合は、近付く速度の分、対水速度が低減するように、前記推進装置へ与える前記回転速度指令値を補正する処理と、計測された前記航走体の位置の変化が、前記目標位置から離れる方向の変化の場合は、離れる速度の分、対水速度が増加するように、前記推進装置へ与える回転速度指令値を補正する処理と、を行う航走制御方法としてもよい。 In the position control process, when it is determined as a result of the speed determination process that the estimated water speed is equal to or higher than the minimum speed at which the effectiveness of the steering wheel is guaranteed, the rotation speed command value is set to the propulsion device. In the case of the process of measuring the change in the position of the traveling body for a set time after the processing of giving to the vehicle and the change in the measured position of the traveling body in the direction of approaching the target position, The process of correcting the rotation speed command value given to the propulsion device so that the speed against water is reduced by the approaching speed, and the measured change in the position of the traveling body are in the direction away from the target position. In the case of a change, it may be a navigation control method that performs a process of correcting the rotation speed command value given to the propulsion device so that the speed with respect to water increases by the speed of separation.
前記判定円の円周上を前記航走体が通過する位置の変化量が、設定された範囲内に収まると、目標補正処理を行い、前記目標補正処理では、前記判定円の円周上を通過するときの前記航走体の位置を求める処理と、求めた位置を、設定された定点保持目標位置に一致させるために必要なシフト量を求める処理と、前記目標位置から、求めた前記シフト量で変位した位置に、次の目標位置を設定する処理と、前記次の目標位置を中心とする前記設定された半径の判定円を設定する処理と、前記目標位置および前記目標位置を中心とする判定円を、前記次の目標位置および前記次の目標位置を中心とする判定円に置き換える処理と、を行う航走制御方法としてもよい。 When the amount of change in the position where the navigator passes on the circumference of the determination circle is within the set range, the target correction process is performed, and in the target correction process, the circumference of the determination circle is changed. The process of finding the position of the traveling body when passing through, the process of finding the shift amount required to match the found position with the set fixed point holding target position, and the process of finding the shift from the target position. The process of setting the next target position at the position displaced by the amount, the process of setting the determination circle of the set radius centered on the next target position, and the process of setting the target position and the target position as the center. It may be a cruising control method that performs a process of replacing the determination circle to be performed with a determination circle centered on the next target position and the next target position.
また、航走体に、推進装置と、舵と、前記航走体の位置を検出する自己位置検出手段と、目標位置が設定される目標設定部と、制御器とを備え、前記制御器は、前記いずれかの航走制御方法を実施する機能を備えた構成を有する航走制御装置とする。 Further, the navigating body includes a propulsion device, a rudder, a self-position detecting means for detecting the position of the navigating body, a target setting unit for setting a target position, and a controller. , The navigation control device having a configuration having a function of implementing any of the navigation control methods.
本発明の航走制御方法および航走制御装置によれば、航走体の定点保持を行う際に、位置を保持する精度の向上化を図ることができる。 According to the cruising control method and the cruising control device of the present invention, it is possible to improve the accuracy of holding the position when holding the fixed point of the running body.
以下、本発明を実施するための形態を図面を参照して説明する。 Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.
［第１実施形態］
図１は、第１実施形態の航走制御方法の実施に用いる航走制御装置を示す概要図である。図２は、第１実施形態の航走制御方法における定点保持処理の概要を示す図である。
[First Embodiment]
FIG. 1 is a schematic diagram showing a navigation control device used for implementing the navigation control method of the first embodiment. FIG. 2 is a diagram showing an outline of a fixed point holding process in the navigation control method of the first embodiment.
本実施形態は、本開示の航走制御方法を、無人航走体の航走制御に適用して行う場合について説明する。 The present embodiment describes a case where the navigation control method of the present disclosure is applied to the navigation control of an unmanned vehicle.
先ず、図１を用いて、本実施形態の航走制御方法の実施に用いる航走制御装置１の構成について説明する。
First, the configuration of the
航走制御装置１は、クルージング型の無人航走体Ａの機体に装備されるもので、無人航走体Ａの自機の位置と姿勢（向き）を検出する手段２（以下、自己位置検出手段２という）と、目標位置Ｐ０を設定する機能を備えた目標設定部３と、推進装置としてのメインスラスタ４と、舵（操舵装置）５と、メインスラスタ４および舵５の指令値を計算する制御器６とを備えた構成とされている。
The
自己位置検出手段２は、無人航走体Ａの自機の位置の情報を、図２に示すような目標位置Ｐ０を含む領域に設定される絶対座標系の座標として検出する機能を備えている。なお、この絶対座標系としては、緯度および経度と１対１で対応づけられる直交座標系を用いることが好適であるが、航走目標位置近傍の任意の地点を原点とする平面直交座標系、平面直角座標系を用いるようにしてもよい。緯度および経度と１対１で対応づけられる座標系を、以下、絶対座標系という。前述の座標系は絶対座標系であり、以下、所定の絶対座標系という。 The self-position detecting means 2 has a function of detecting the position information of the own machine of the unmanned navigating body A as the coordinates of the absolute coordinate system set in the area including the target position P0 as shown in FIG. .. As this absolute coordinate system, it is preferable to use a Cartesian coordinate system that is associated with latitude and longitude on a one-to-one basis. A plane Cartesian coordinate system may be used. A coordinate system that has a one-to-one correspondence with latitude and longitude is hereinafter referred to as an absolute coordinate system. The above-mentioned coordinate system is an absolute coordinate system, and is hereinafter referred to as a predetermined absolute coordinate system.
また、自己位置検出手段２は、無人航走体Ａの姿勢として、無人航走体Ａの機体が向いている方向（方位）についての情報を検出する機能を備えている。 Further, the self-position detecting means 2 has a function of detecting information about the direction (direction) in which the aircraft of the unmanned traveling body A is facing as the posture of the unmanned traveling body A.
更に、自己位置検出手段２は、無人航走体Ａが移動するときの対地速度に関する情報を検出する機能を備えることが好ましい。 Further, it is preferable that the self-position detecting means 2 has a function of detecting information regarding the ground speed when the unmanned navigating body A moves.
無人航走体ＡがＵＵＶの場合は、自己位置検出手段２としては、たとえば、慣性航法装置（ＩＮＳ）、または、慣性航法装置にドップラーログ（ＤＶＬ）を組み合わせたものを用いるようにすればよい。 When the unmanned navigation body A is a UUV, as the self-position detection means 2, for example, an inertial navigation system (INS) or a combination of an inertial navigation system and a Doppler log (DVL) may be used. ..
なお、ＵＵＶの図示しない母船（支援船）では、ＧＰＳなどの全地球航法衛星システムを用いて経度と緯度による自船の位置を計測することができると共に、音響測位により自船を基準とするＵＵＶの相対的な位置を求めることができる。よって、これらの情報を基に、母船では、所定の絶対座標系におけるＵＵＶの位置の情報を算出することができる。この点に鑑みて、ＵＵＶに備える自己位置検出手段２は、所定の絶対座標系におけるＵＵＶの位置の情報を、音響通信により母船から取得するものであってもよい。また、自己位置検出手段２は、前記の母船の位置情報と、母船を基準とするＵＵＶの相対的な位置情報とを、母船から音響通信により取得して、所定の絶対座標系におけるＵＵＶの位置の情報を、ＵＵＶ側で算出するものであってもよい。 For mother ships (support ships) not shown in the UUV, the position of the ship can be measured by longitude and latitude using a global navigation satellite system such as GPS, and the UUV is based on the ship by acoustic positioning. The relative position of can be obtained. Therefore, based on this information, the mother ship can calculate the information on the position of the UUV in the predetermined absolute coordinate system. In view of this point, the self-position detecting means 2 provided in the UUV may acquire information on the position of the UUV in a predetermined absolute coordinate system from the mother ship by acoustic communication. Further, the self-position detecting means 2 acquires the position information of the mother ship and the relative position information of the UUV with respect to the mother ship from the mother ship by acoustic communication, and the position of the UUV in a predetermined absolute coordinate system. Information may be calculated on the UUV side.
無人航走体ＡがＵＳＶの場合は、自己位置検出手段２としては、たとえば、慣性航法装置や、全地球航法衛星システムを用いるようにすればよい。 When the unmanned navigation body A is a USV, for example, an inertial navigation system or a global navigation satellite system may be used as the self-position detection means 2.
また、自己位置検出手段２としては、ＵＳＶに備えたカメラによる撮影画像や、レーダーなどを用いて、所定の絶対座標系における位置の情報が予め分かっている地点や対象物を検出し、検出された地点や対象物を基準とするＵＳＶの相対的な位置と移動方向と速度の情報を求めて、所定の絶対座標系におけるＵＳＶの位置と姿勢と対地速度の情報を算出するようにしてもよい。 Further, as the self-position detecting means 2, a point or an object whose position information in a predetermined absolute coordinate system is known in advance is detected and detected by using an image taken by a camera provided in the USV, a radar, or the like. The information on the relative position, movement direction, and velocity of the USV with respect to the point or object may be obtained, and the information on the position, attitude, and ground velocity of the USV in a predetermined absolute coordinate system may be calculated. ..
なお、方位については、慣性航法装置で地球の自転軸の角速度ベクトルと重力方向ベクトルを計測し、これから真北を求め、この真北に対する角度として求めてもよいし、磁北の向きと事前に調査された各地点における磁北の真北に対する傾きの値から真北を求め、この真北に対する角度として求めてもよい。 Regarding the direction, the angular velocity vector and the gravity direction vector of the rotation axis of the earth may be measured by the inertial navigation device, and the true north may be obtained from this, and the true north may be obtained as the angle with respect to the true north, or the direction of the magnetic north may be investigated in advance. True north may be obtained from the value of the inclination of magnetic north with respect to true north at each point, and may be obtained as an angle with respect to this true north.
なお、自己位置検出手段２は、無人航走体Ａの自機の位置と姿勢、更には、対地速度を検出する機能を備えていれば、前記した以外の形式や方式の自己位置検出手段２を用いてもよいことは勿論である。 If the self-position detecting means 2 has a function of detecting the position and attitude of the unmanned navigating body A and the ground speed, the self-position detecting means 2 of a form or method other than the above is provided. Of course, may be used.
自己位置検出手段２で検出された無人航走体Ａの自機の位置と姿勢と対地速度の情報は、制御器６に入力される。
Information on the position, attitude, and ground speed of the unmanned navigation body A detected by the self-position detecting means 2 is input to the
目標設定部３は、目標位置Ｐ０を設定する機能と、設定した目標位置Ｐ０について、所定の絶対座標系に関する座標の情報として、制御器６へ送る機能を備えている。
The
なお、目標設定部３が設定する目標位置Ｐ０は、制御器６が後述する無人航走体Ａの定点保持処理を行う際に、無人航走体Ａの方位制御の目標方位を定めるための目標位置Ｐ０であり、無人航走体Ａが定点保持を行う位置には一致しない。
The target position P0 set by the
また、制御器６が後述する無人航走体Ａの定点保持処理を実施すると、結果として、無人航走体Ａが定点保持を行う位置は、目標位置Ｐ０から見て、外乱７が作用する方向の下流側になる。しかし、目標設定部３は、無人航走体Ａの定点保持処理を開始する前に、外乱７が作用する方向が未知の状態で、目標位置Ｐ０を設定する必要がある。
Further, when the
なお、本実施形態における外乱７は、無人航走体ＡがＵＵＶの場合は、ＵＵＶの周囲に存在している潮流が主となるが、ＵＵＶにその他の外乱が作用する場合は、ＵＵＶに作用するすべての外乱が合成されたものになる。また、外乱７は、無人航走体ＡがＵＳＶの場合は、ＵＳＶに作用する風、波、潮流などのすべての外乱が合成されたものになる。
In the
そこで、目標設定部３には、無人航走体Ａに定点保持を行わせることが所望される或る範囲の領域、たとえば、後述する判定円８を含む範囲の領域が、事前に設定される。この状態で、目標設定部３は、設定された領域内の或る位置、たとえば、設定された領域の中央となる位置に、目標位置Ｐ０を設定する機能を備えている。
Therefore, in the
メインスラスタ４は、無人航走体Ａの前進方向および後進方向の推力を発生するものである。メインスラスタ４は、たとえば、無人航走体Ａの機体の後部側に設けられている。
The
また、メインスラスタ４は、制御器６から回転速度指令値ｍ１が与えられると、回転速度指令値ｍ１に従った回転速度で運転を行う機能を備えている。
Further, the
舵５は、無人航走体Ａの機体の方位（向き）を変更するためのものである。舵５は、たとえば、無人航走体Ａの機体の後部側に、メインスラスタ４に接近した配置で設けられている。
The
また、舵５は、制御器６から舵角度指令値ｍ２が与えられると、アクチュエータ（図示せず）を作動させて舵角度指令値ｍ２に従って角度変更する機能を備えている。
Further, the
次に、制御器６の機能の説明を行うと共に、本実施形態の航走制御方法について説明する。
Next, the function of the
制御器６は、図２に示すように、所定の絶対座標系に関して、目標設定部３で設定された目標位置Ｐ０の座標（Ｙ０，Ｘ０）の情報を、目標設定部３から受け取る機能を備えている。
As shown in FIG. 2, the
なお、図２では、所定の絶対座標系は、ｘ軸が上の方向を正の向きとする縦軸、ｙ軸が右の方向を正の向きとする横軸となる直交座標で示してある。 In FIG. 2, the predetermined absolute coordinate system is shown by Cartesian coordinates in which the x-axis is the vertical axis with the upward direction as the positive direction and the y-axis is the horizontal axis with the right direction as the positive direction. ..
また、制御器６は、所定の絶対座標系にて、目標位置Ｐ０の座標（Ｙ０，Ｘ０）を中心とし、設定された半径Ｒを備えた、図２に一点鎖線で示す如き仮想の判定円８を作成する機能を備えている。
Further, the
後述するように、無人航走体Ａは、判定円８の外側の領域では目標位置に向けて前進航走するが、判定円８以内の領域では惰性で移動するようになる。そこで、判定円８は、静水中で判定円８の円周上の位置から中心に向けて惰性で移動する無人航走体Ａが、判定円８を通り抜けないように、判定円８の半径Ｒの寸法が設定されることが好ましい。更に、静水中で判定円８の円周上の位置から惰性で移動する無人航走体Ａが、判定円８の中心まで到達しないように、判定円８の半径Ｒの寸法が設定されることが好ましい。
As will be described later, the unmanned vehicle A travels forward toward the target position in the region outside the
また、後述するように、本実施形態では、無人航走体Ａの定点保持を行うときには、無人航走体Ａの位置は、判定円８の円周上で次第に収束することで位置保持が行われるようになる。また、外乱７の作用する方向が変化するときには、判定円８の円周上で、無人航走体Ａの位置が変位するようになる。そのため、判定円８の半径Ｒは、寸法が大きくなるにつれて、前記した無人航走体Ａの位置の収束に要する時間が多くなり、また、外乱７の作用する方向が変化した場合に生じる無人航走体Ａの変位も大きくなる。
Further, as will be described later, in the present embodiment, when the fixed point holding of the unmanned running body A is performed, the position of the unmanned running body A is gradually converged on the circumference of the
したがって、実際の運用上は、判定円８の半径Ｒは、無人航走体Ａの前後長の５倍から１０倍程度に設定されることが好適である。
Therefore, in actual operation, it is preferable that the radius R of the
この状態で、制御器６は、定点保持処理を開始する。定点保持処理では、制御器６は、先ず、自己位置検出手段２より受け取る情報を基に、所定の絶対座標系に関して、無人航走体Ａの位置の座標（Ｙａ，Ｘａ）を求める。なお、この無人航走体Ａの位置の座標（Ｙａ，Ｘａ）としては、無人航走体Ａを代表する或る一個所の座標、たとえば、無人航走体Ａの重心の位置の座標を用いるようにすればよいが、重心以外の個所の座標を用いてもよいことは勿論である。無人航走体Ａは、この一個所の座標を基準として、定点に位置保持されるように制御が行われるので、無人航走体Ａのどの位置を定点に位置保持したいかで、無人航走体Ａを代表する一個所の位置を定めることが好ましい。
In this state, the
次に、制御器６は、作成された判定円８と、無人航走体Ａの位置の座標（Ｙａ，Ｘａ）の情報とを基に、無人航走体Ａの位置が、判定円８よりも外側に存在するか、否かを判断する判断処理を行う。
Next, in the
この判断処理は、具体的には、目標位置Ｐ０の座標（Ｙ０，Ｘ０）と、無人航走体Ａの位置の座標（Ｙａ，Ｘａ）との距離ｂを幾何学的な計算で算出し、算出された距離ｂについて、判定円８の半径Ｒを基準として、ｂ＞Ｒという条件を満たすか、否か（すなわち、ｂ≦Ｒ）を判断するようにすればよい。
Specifically, in this determination process, the distance b between the coordinates (Y0, X0) of the target position P0 and the coordinates (Ya, Xa) of the position of the unmanned vehicle A is calculated by geometric calculation. With respect to the calculated distance b, it may be determined whether or not the condition b> R is satisfied (that is, b ≦ R) with reference to the radius R of the
制御器６は、判断処理の結果、ｂ＞Ｒという条件が満たされた場合、すなわち、無人航走体Ａが判定円８よりも外側の領域に存在すると判断された場合は、無人航走体Ａを現在位置から目標位置Ｐ０に向けて前進航走させる目標方位航走処理を行う機能を備えている。
When the condition b> R is satisfied as a result of the determination process, that is, when it is determined that the unmanned navigation body A exists in a region outside the
この目標方位航走処理では、制御器６は、次のように速度制御と方位制御とを行う。
In this target directional navigation process, the
目標方位航走処理の速度制御では、制御器６は、先ず、無人航走体Ａが現在位置から目標位置Ｐ０に接近する方向へ前進移動するための対地速度の目標値を設定する。次に、制御器６は、外乱７が存在している環境下においても、自己位置検出手段２より受け取る無人航走体Ａの対地速度が、設定された対地速度の目標値に一致するように、回転速度指令値ｍ１を定める処理を行う。次いで、制御器６は、定めた回転速度指令値ｍ１を、メインスラスタ４へ与える処理を行う。
In the speed control of the target directional navigation process, the
また、目標方位航走処理の方位制御では、制御器６は、メインスラスタ４の運転により無人航走体Ａが前進するときに、無人航走体Ａの進行する方向が、常に目標位置Ｐ１に向くように、舵角度指令値ｍ２を随時定め、定めた舵角度指令値ｍ２を、舵５へ与える処理を行う。
Further, in the directional control of the target directional navigation process, in the
これにより、無人航走体Ａは、メインスラスタ４が、制御器６より受け取る回転速度指令値ｍ１に従う回転速度で運転され、舵５が、制御器６より受け取る舵角度指令値ｍ２に応じて角度変更される。
As a result, the unmanned navigation body A is operated at a rotation speed according to the rotation speed command value m1 received by the
したがって、無人航走体Ａは、無人航走体Ａの位置が判定円８よりも外側に存在しているときには、設定された対地速度で、目標位置Ｐ０に向かう前進航走を行うようになる。
Therefore, when the position of the unmanned navigation body A is outside the
一方、制御器６は、前記判断処理の結果、ｂ≦Ｒという条件が満たされた場合、すなわち、無人航走体Ａが、判定円８よりも外側の領域ではなく、判定円８以内の領域に存在すると判断された場合は、無人航走体Ａのメインスラスタ４による前進方向の推力を停止させる処理を行う機能を備えている。なお、判定円８以内の領域とは、判定円８の円周上および判定円８よりも内側の領域をいうものとする。
On the other hand, when the condition b ≦ R is satisfied as a result of the determination process, the
これにより、無人航走体Ａは、判定円８の外側の領域にて、設定された対地速度で、且つ目標位置Ｐ０に向けて航走している状態で、判定円８の円周上の位置に到達すると、その時点で、無人航走体Ａの前進方向の推力が停止される。
As a result, the unmanned vehicle A is on the circumference of the
したがって、無人航走体Ａは、判定円８の内側の領域では、判定円８の外側から進入したときの惰性で目標位置Ｐ０の方へ移動するようになる。この際、推力が停止された状態の無人航走体Ａは、判定円８が設定された領域に存在している外乱７の影響を受ける。よって、判定円８以内の領域では、無人航走体Ａが移動する方向は、次第に、外乱７が作用する方向に沿う方向に変更される。
Therefore, in the region inside the
なお、無人航走体Ａは、判定円８以内の領域では、推力が停止されることに伴い、対水速度は次第にゼロに近付く。そのため、無人航走体では、舵５の効きが悪くなる。
In the region within the
よって、制御器６は、無人航走体Ａが判定円８以内の領域に存在すると判断された場合は、舵５に対しては、新たな舵角度指令値ｍ２を与えなくてもよいし、無人航走体Ａの向きができるだけ目標位置Ｐ０に向くように定めた舵角度指令値ｍ２を与えるようにしてもよい。
Therefore, when it is determined that the unmanned navigation body A exists in the region within the
判定円８に進入した無人航走体Ａは、外乱７の影響を受けるため、その後、判定円８の内側から外側へ出るようになる。その際、無人航走体Ａが判定円８から出るときに通過する判定円８の円周上の位置は、無人航走体Ａが判定円８に進入したときに通過した円周上の位置よりも、外乱７が作用する方向の下流寄りの位置となる。
Since the unmanned navigating body A that has entered the
無人航走体Ａが判定円８の外側の領域に出ると、制御器６では、前記判断処理の結果が、ｂ＞Ｒという条件が満たされた状態になる。そのため、制御器６は、無人航走体Ａを目標位置Ｐ０に向けて前進航走させる目標方位航走処理を再開する。
When the unmanned vehicle A goes out to the region outside the
制御器６は、目標方位航走処理により無人航走体Ａが再び判定円８の円周上に到達して、前記判断処理の結果が、ｂ≦Ｒという条件が満たされた状態になると、無人航走体Ａのメインスラスタ４による前進方向の推力を停止させる処理を行う。
When the unmanned vehicle A reaches the circumference of the
したがって、制御器６は、前記判断処理の結果がｂ＞Ｒという条件が満たされた状態と、ｂ≦Ｒという条件が満たされた状態とに応じて、目標方位航走処理と、メインスラスタ４による前進方向の推力を停止させる処理とを、順次繰り返す。
Therefore, the
これにより、無人航走体Ａは、判定円８の外側から目標位置Ｐ０に向かう前進航走で判定円８に進入する動作と、前進方向の推力を停止した状態で、外乱７の影響を受けて判定円８の内側から外側へ出る動作とを、順次繰り返し行うようになる。この際、無人航走体Ａが判定円８から出るときに通過する判定円８の円周上の位置は、その直前に無人航走体Ａが判定円８に進入したときに通過した円周上の位置よりも、外乱７が作用する方向の下流寄りの位置となる。
As a result, the unmanned navigation body A is affected by the
したがって、無人航走体Ａは、図２に示すように、判定円８の円周上を外側から内側へ入る動作と、内側から外側へ出る動作とを交互に繰り返しながら、判定円８の円周上を無人航走体Ａが通過する位置が、目標位置Ｐ０から見て外乱７が作用する方向の位置、すなわち、判定円８の円周と、目標位置Ｐ０から外乱７が作用する方向に沿い線を伸ばしたときの交点に、次第に収束するようになる。
Therefore, as shown in FIG. 2, the unmanned traveling body A alternately repeats the operation of entering the circumference of the
無人航走体Ａが、判定円８の円周上で、目標位置Ｐ０から見て外乱７が作用する方向の位置まで移動すると、制御器６の目標方位航走処理によって無人航走体Ａが判定円８の外側から内側へ入るときの目標位置Ｐ０の方向と、判定円８の内側に入った無人航走体Ａを判定円８の外へ押し出すように作用する外乱の作用する方向が、一直線上に配置されるようになる。
When the unmanned navigation body A moves on the circumference of the
よって、この状態で、無人航走体Ａは、判定円８の円周上における目標位置Ｐ０から見て外乱７が作用する方向の位置で、判定円８の円周上を内外方向に交互に通過するよう前後移動しながら、前後以外の方向への変位がほぼ生じない状態として位置の保持が行われる。
Therefore, in this state, the unmanned navigation body A alternately moves on the circumference of the
したがって、本実施形態の航走制御方法および航走制御装置によれば、無人航走体Ａは、判定円８の円周上における目標位置Ｐ０から見て外乱７が作用する方向の位置にて、定点保持を行うことができる。
Therefore, according to the navigation control method and the navigation control device of the present embodiment, the unmanned navigation body A is at a position in the direction in which the
また、この定点保持では、無人航走体Ａは、判定円８の円周の付近で、判定円８へ出入りする動作を繰り返すのみであり、判定円８の中心まで移動することはない。
Further, in this fixed point holding, the unmanned navigation body A only repeats the operation of entering and exiting the
したがって、本実施形態の航走制御方法および航走制御装置は、無人航走体Ａの定点保持を行う際に、特許文献１に示された従来の定点保持の手法に比して、位置を保持する精度の向上化を図ることができる。
Therefore, the cruising control method and the cruising control device of the present embodiment can hold the unmanned navigation body A at a fixed point, as compared with the conventional fixed point holding method shown in
ところで、無人航走体Ａが定点保持を行う場合、潮流などを含む外乱７は、時間の経過とともに、強さが変化したり、無人航走体Ａに対して外乱７の作用する方向が徐々に変化したりすることがある。
By the way, when the unmanned navigation body A holds a fixed point, the strength of the
本実施形態では、外乱７の強さが変化する場合は、制御器６の目標方位航走処理によって無人航走体Ａが判定円８の外側から内側へ入るときのメインスラスタ４の前進方向の推力を変化させることで対応することができる。したがって、外乱７の強さが変化しても、本実施形態の航走制御方法および航走制御装置では、無人航走体Ａの定点保持について、従来の定点保持の手法に比して、位置を保持する精度の向上化を図ることができる。
In the present embodiment, when the strength of the
また、本実施形態では、外乱７の作用する方向が変化すると、無人航走体Ａは、判定円８の円周上で、目標位置Ｐ０から見て、変化後の外乱７の作用する方向の位置に移動し、そこで定点保持を行うようになる。
Further, in the present embodiment, when the direction in which the
したがって、本実施形態の航走制御方法および航走制御装置では、外乱の作用する方向が変化した場合は、無人航走体Ａが定点保持を行う位置は判定円８の円周上で変位するが、変位後の位置での無人航走体Ａの定点保持については、従来の定点保持の手法に比して、位置を保持する精度の向上化を図ることができる。
Therefore, in the navigation control method and the navigation control device of the present embodiment, when the direction in which the disturbance acts changes, the position where the unmanned navigation body A holds the fixed point is displaced on the circumference of the
［第２実施形態］
前記第１実施形態では、無人航走体Ａは、判定円８の円周上で、且つ目標位置Ｐ０から見て外乱７が作用する方向の位置で定点保持を行うときに、無人航走体Ａが判定円８よりも外側の領域に位置するときには、制御器６が目標方位航走処理を行い、無人航走体Ａが判定円８以内の領域に位置するときには、制御器６がメインスラスタ４の前進方向の推力を停止する処理を行うものとした。
[Second Embodiment]
In the first embodiment, the unmanned navigation body A holds a fixed point on the circumference of the
これに対し、第２実施形態の航走制御装置１は、第１実施形態と同様の構成に加えて、制御器６が、次の位置制御処理を行う機能を備える構成とする。
On the other hand, the
制御器６は、先ず、第１実施形態と同様の処理を行う。これにより、無人航走体Ａは、判定円８の円周上での位置が、目標位置Ｐ０から見て外乱７が作用する方向の位置に、次第に収束するようになる。したがって、判定円８の円周上を無人航走体Ａが内外方向に交互に通過する位置は、次第に変化しなくなる。
First, the
そこで、制御器６は、判定円８の円周上を無人航走体Ａが内外方向に交互に通過する位置の情報を順次取得して、判定円８の円周上を無人航走体Ａが通過する位置の変化量が、設定された範囲内に収まると、位置制御処理を開始する機能を備えている。
Therefore, the
制御器６は、位置制御処理を行う場合は、無人航走体Ａの水平航走時について、メインスラスタ４の回転速度と、無人航走体Ａの対水速度との関係を計測したデータを、予め格納している。
When the
制御器６は、位置制御処理では、先ず、無人航走体Ａが判定円８の外側から内側に進入するときの対地速度と、メインスラスタ４の回転速度を取得する。
In the position control process, the
この際、無人航走体Ａの対地速度としては、無人航走体Ａの自己位置検出手段２が、ドップラーログ、あるいは、全地球航法衛星システムのような、無人航走体Ａの対地速度を検出可能な機器を備えている場合は、その機器で取得した対地速度の検出結果を用いるようにすればよい。これに対し、無人航走体Ａの対地速度は、自己位置検出手段２で検出される無人航走体Ａの位置の時間変化を基にした計算で求めるようにしてもよいことは勿論である。 At this time, as the ground speed of the unmanned navigation body A, the self-position detection means 2 of the unmanned navigation body A determines the ground speed of the unmanned navigation body A such as the Doppler log or the global navigation satellite system. If a detectable device is provided, the detection result of the ground speed acquired by the device may be used. On the other hand, it goes without saying that the ground speed of the unmanned navigation body A may be calculated based on the time change of the position of the unmanned navigation body A detected by the self-position detecting means 2. ..
次に、制御器６は、取得したメインスラスタ４の回転速度を基に、前記データを用いて、対水速度を算出する。なお、ドップラーログで対水速度が計測できる場合は、制御器６は、ドップラーログで計測された対水速度を用いるようにしてもよい。
Next, the
次いで、制御器６は、前記の対水速度と、取得した対地速度を用いて、無人航走体Ａの周囲に存在している水の推定水速度を、以下の計算式で求める。
推定水速度＝対地速度－対水速度
Next, the
Estimated water velocity = ground speed-ground speed
その後、制御器６は、求めた推定水速度が、舵５の効きが担保される最低限の速度Ｖｒよりも小さいか、否かを判断する速度判断処理を行う。
After that, the
この速度判断処理の結果、推定水速度が、速度Ｖｒよりも小さいと判断された場合は、制御器６は、前記データを基に、速度Ｖｒに対応する対水速度を得るために必要なメインスラスタ４の回転速度を算出し、算出された回転速度に対応する回転速度指令値ｍ１を、メインスラスタ４へ与える機能を備えている。
When it is determined as a result of this speed determination process that the estimated water speed is smaller than the speed Vr, the
これにより、無人航走体Ａでは、メインスラスタ４が、制御器６より受け取る回転速度指令値ｍ１に従った回転速度で運転されることにより、無人航走体Ａは、対水速度が舵５の効きが担保される最低限の速度Ｖｒに制御された状態で、前進航走を行うようになる。
As a result, in the unmanned navigation body A, the
この場合、無人航走体Ａは、対水速度が、推定水速度よりも大きいため、前進方向の対地速度が生じる。よって、この場合は、制御器６は、無人航走体Ａの位置が判定円８以内の領域になると、メインスラスタ４の前進方向の推力を停止させる処理を行う機能を備えればよい。
In this case, since the unmanned navigation body A has a ground speed higher than the estimated water speed, a ground speed in the forward direction occurs. Therefore, in this case, the
一方、速度判断処理の結果、推定水速度が、速度Ｖｒよりも小さくないと判断された場合、すなわち、推定水速度が速度Ｖｒ以上であると判断された場合は、制御器６は、対水速度を、推定水速度＋αに設定する。αは、メインスラスタ４の回転速度の制御に基づいて、対水速度のオープンループ制御を行う際の速度誤差以上の値である。
On the other hand, when it is determined that the estimated water velocity is not smaller than the velocity Vr as a result of the velocity determination process, that is, when it is determined that the estimated water velocity is equal to or higher than the velocity Vr, the
次いで、制御器６は、設定された対水速度を得るために必要なメインスラスタ４の回転速度を算出し、算出された回転速度に対応する回転速度指令値ｍ１を、メインスラスタ４へ与える機能を備えている。
Next, the
これにより、無人航走体Ａでは、メインスラスタ４が、制御器６より受け取る回転速度指令値ｍ１に従った回転速度で運転されることにより、無人航走体Ａは、対水速度がほぼ推定水速度に一致するように制御される。よって、この場合、無人航走体Ａの対地速度は、ほぼゼロになる。
As a result, in the unmanned navigation body A, the
そこで、制御器６は、速度の微調整を行う機能を更に備えることが好ましい。
Therefore, it is preferable that the
速度の微調整では、制御器６は、先ず、オープンループ制御により、前記推定水速度＋αに設定された対水速度を得るための回転速度指令値ｍ１を、メインスラスタ４へ与える。この状態で、無人航走体Ａの前後方向の加減速が設定された値よりも小さくなると、制御器６は、自己位置検出手段２より受け取る情報を基に、無人航走体Ａの位置の変化を、設定された時間計測する。
In the fine adjustment of the speed, the
次いで、制御器６は、計測された無人航走体Ａの位置の変化が、目標位置Ｐ０に近付く方向の変化の場合は、その近付く速度を求め、求めた速度の分、対水速度が低減するように、メインスラスタ４の回転速度指令値ｍ１を補正する処理を行う。
Next, when the measured change in the position of the unmanned navigating body A is a change in the direction approaching the target position P0, the
一方、制御器６は、計測された無人航走体Ａの位置の変化が、目標位置Ｐ０から離れる方向の変化の場合は、その離れる速度を求め、求めた速度の分、対水速度が増加するように、メインスラスタ４の回転速度指令値ｍ１を補正する処理を行う。
On the other hand, when the measured change in the position of the unmanned navigation body A is a change in the direction away from the target position P0, the
これにより、無人航走体Ａの対地速度は、よりゼロに近付くように制御される。ただし、前記速度の微調整は、判定円８の外側では、微小に目標位置Ｐ０に近づいていくようにし、判定円８の内側では微小に目標位置Ｐ０から離れて行くように、速度調整を行う。
As a result, the ground speed of the unmanned vehicle A is controlled to be closer to zero. However, in the fine adjustment of the speed, the speed is adjusted so as to approach the target position P0 slightly outside the
したがって、本実施形態の航走制御方法および航走制御装置によれば、無人航走体Ａの定点保持を行う際に、無人航走体Ａの前後方向の移動量を低減化させることができて、無人航走体Ａの位置を保持する精度の更なる向上化を図ることができる。 Therefore, according to the navigation control method and the navigation control device of the present embodiment, it is possible to reduce the amount of movement of the unmanned navigation body A in the front-rear direction when the unmanned navigation body A is held at a fixed point. Therefore, it is possible to further improve the accuracy of maintaining the position of the unmanned navigation body A.
［第３実施形態］
前記第１実施形態および第２実施形態では、無人航走体Ａが定点保持を行うのは、目標位置Ｐ０を中心とする判定円８の円周上で、目標位置Ｐ０から見て、外乱７が作用する方向の位置となる。
[Third Embodiment]
In the first embodiment and the second embodiment, the unmanned traveling body A holds the fixed point on the circumference of the
そこで、第３実施形態は、本開示の航走制御方法および航走制御装置を、無人航走体Ａの定点保持が望まれる定点保持目標位置が設定される場合の適用について説明する。 Therefore, the third embodiment describes the application of the navigation control method and the navigation control device of the present disclosure when the fixed point holding target position where the fixed point holding of the unmanned navigation body A is desired is set.
図３は、第３実施形態の航走制御方法における定点保持処理の概要を示す図で、図３（ａ）は、目標位置を中心とする判定円の円周上で無人航走体の位置保持が行われる状態を示す図、図３（ｂ）は、制御器が目標補正処理を行った状態を示す図である。 FIG. 3 is a diagram showing an outline of the fixed point holding process in the navigation control method of the third embodiment, and FIG. 3A is a diagram showing the position of the unmanned vehicle on the circumference of the determination circle centered on the target position. FIG. 3 (b) is a diagram showing a state in which holding is performed, and is a diagram showing a state in which the controller has performed target correction processing.
なお、図３（ａ）（ｂ）において、第１実施形態および第２実施形態と同一のものには同一符号を付して、その説明を省略する。また、本実施形態の航走制御方法の実施に用いる航走制御装置の構成は、制御器の機能以外は、図１に示した第１実施形態の航走制御装置と同様である。 In FIGS. 3A and 3B, the same reference numerals as those of the first embodiment and the second embodiment are designated by the same reference numerals, and the description thereof will be omitted. Further, the configuration of the navigation control device used for implementing the navigation control method of the present embodiment is the same as that of the navigation control device of the first embodiment shown in FIG. 1, except for the function of the controller.
本実施形態では、制御器６には、定点保持目標位置Ｔの座標（ＹＴ，ＸＴ）が設定される。
In the present embodiment, the coordinates (YT, XT) of the fixed point holding target position T are set in the
この状態で、制御器６は、図３（ａ）に示すように、定点保持目標位置Ｔの付近に目標位置Ｐ０を設定し、更に、目標位置Ｐ０の座標（Ｙ０，Ｘ０）を中心とする半径Ｒの判定円８を設定する処理を行う。
In this state, as shown in FIG. 3A, the
なお、目標位置Ｐ０は、判定円８の円周上のいかなる位置に無人航走体Ａの位置が収束するようになっても、定点保持目標位置Ｔからの距離があまり変わらないようにするためには、定点保持目標位置Ｔの近傍、あるいは、定点保持目標位置Ｔと同一の位置に設定することが好ましい。また、定点保持目標位置Ｔに存在している外乱７の作用する方向について、事前に或る程度の予測が可能な場合は、制御器６は、定点保持目標位置Ｔに対して、予測される外乱７の作用する方向の逆側となる位置に、目標位置Ｐ０を設定することが好ましい。なお、図３（ａ）では、定点保持目標位置Ｔが判定円８の内側に配置された状態を示してあるが、判定円８が定点保持目標位置Ｔを含まないように設定されてもよいことは勿論である。
The target position P0 is set so that the distance from the fixed point holding target position T does not change so much even if the position of the unmanned traveling body A converges to any position on the circumference of the
この状態で、制御器６は、目標位置Ｐ０と判定円８に基づき、第１実施形態と同様の処理を行う。
In this state, the
これにより、図３（ａ）に示すように、無人航走体Ａは、判定円８の円周上で、且つ目標位置Ｐ０から見て外乱７の作用する方向の位置に、次第に収束するように移動する。
As a result, as shown in FIG. 3A, the unmanned navigation body A gradually converges on the circumference of the
本実施形態における制御器６は、判定円８の円周上を無人航走体Ａが内外方向に交互に通過する位置の情報を順次取得して、判定円８の円周上を無人航走体Ａが通過する位置の変化量が、設定された範囲内に収まると、目標補正処理を開始する機能を備えている。
The
制御器６は、目標補正処理では、先ず、自己位置検出手段２より受け取る情報を基に、判定円８の円周上を通過するときの無人航走体Ａの位置の座標（Ｙａ，Ｘａ）を求める。
In the target correction process, the
次に、制御器６は、図３（ａ）に二点鎖線で示すように、求めた位置の座標（Ｙａ，Ｘａ）を、定点保持目標位置Ｔの座標（ＹＴ，ＸＴ）に一致させるために必要なｘ軸方向およびｙ軸方向のシフト量を求める。
Next, the
次いで、図３（ｂ）に示すように、目標位置Ｐ０の座標（Ｙ０，Ｘ０）から、求めたｘ軸方向およびｙ軸方向のシフト量で変位した位置に、次の目標位置Ｐ１（Ｙ１，Ｘ１）を設定し、更に、次の目標位置Ｐ１の座標（Ｙ１，Ｘ１）を中心とする半径Ｒの判定円８ａを設定する処理を行う。
Next, as shown in FIG. 3B, the next target position P1 (Y1,) is displaced from the coordinates (Y0, X0) of the target position P0 by the obtained shift amounts in the x-axis direction and the y-axis direction. X1) is set, and further, a process of setting a
その後、制御器６は、目標位置Ｐ０と判定円８を、次の目標位置Ｐ１と判定円８ａに置き換えた状態で、第１実施形態と同様の処理を、再び実施する。
After that, the
これにより、図３（ｂ）に示すように、無人航走体Ａは、判定円８ａの円周上で、且つ次の目標位置Ｐ１から見て外乱７の作用する方向の位置に、次第に収束するように移動するため、無人航走体Ａの定点保持目標位置Ｔでの定点保持が行われるようになる。
As a result, as shown in FIG. 3B, the unmanned vehicle A gradually converges on the circumference of the
この状態で、制御器６は、第２実施形態と同様の位置制御処理を行うようにしてもよい。
In this state, the
このように、本実施形態の航走制御方法および航走制御装置によれば、第１実施形態と同様の効果に加えて、無人航走体Ａを、設定された定点保持目標位置Ｔで定点保持させることができる。 As described above, according to the navigation control method and the navigation control device of the present embodiment, in addition to the same effects as those of the first embodiment, the unmanned navigation body A is set to a fixed point at the set fixed point holding target position T. Can be retained.
なお、本実施形態では、外乱７の作用する方向が変化すると、無人航走体Ａは、判定円８ａの円周上で、次の目標位置Ｐ１から見て、変化後の外乱７の作用する方向の位置に移動し、そこで定点保持を行うようになる。
In the present embodiment, when the direction in which the
したがって、本実施形態の航走制御方法および航走制御装置では、外乱の作用する方向が変化した場合は、制御器６が、判定円８ａの円周上で無人航走体Ａが新たに位置保持を行う位置を基にした同様の目標補正処理を開始するようにすればよい。
Therefore, in the navigation control method and the navigation control device of the present embodiment, when the direction in which the disturbance acts changes, the
なお、本発明は、前記各実施形態にのみ限定されるものではなく、自己位置検出手段２、目標設定部３、制御器６が、無人航走体Ａの航走や運用に必要とされる別の機能を兼ね備えるものであってもよい。
The present invention is not limited to each of the above embodiments, and the self-position detecting means 2, the
図１では、航走制御装置１にて、自己位置検出手段２、目標設定部３、制御器６をそれぞれ別々に記載したが、いずれか２つ機能が或る一つの装置に備えられている構成や、３つの機能が或る一つ装置に備えられている構成であってもよい。
In FIG. 1, in the
本発明の航走制御方法および航走制御装置は、船舶や潜水船のような有人の航走体について、定点保持を自動制御で行わせる場合に適用してもよい。 The navigation control method and the navigation control device of the present invention may be applied to a manned navigation object such as a ship or a submersible when the fixed point holding is automatically controlled.
その他本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。 Of course, various other changes can be made without departing from the gist of the present invention.
２ 自己位置検出手段、３ 目標設定部、４ メインスラスタ（推進装置）、５ 舵、６ 制御器、７ 外乱、８，８ａ 判定円、Ａ 無人航走体（航走体）、Ｐ０ 目標位置、Ｐ１ 次の目標位置、ｍ１ 回転速度指令値、ｍ２ 舵角度指令値、Ｒ 半径、Ｔ 定点保持目標位置 2 Self-position detection means, 3 Target setting unit, 4 Main thruster (propulsion device), 5 Rudder, 6 Controller, 7 Disturbance, 8,8a Judgment circle, A Unmanned vehicle (Navigation body), P0 Target position, P1 Next target position, m1 Rotation speed command value, m2 Rudder angle command value, R radius, T Fixed point holding target position
Claims (5)
前記判定円と、航走体の位置の情報とを基に、前記航走体の位置が、前記判定円よりも外側に存在するか否かを判断する判断処理と、
前記判断処理の結果、前記航走体が前記判定円よりも外側の領域に存在すると判断された場合は、前記航走体を前記目標位置に向けて前進航走させる目標方位航走処理と、
前記判断処理の結果、前記航走体が前記判定円以内の領域に存在すると判断された場合は、前記航走体の推進装置による前進方向の推力を停止させる処理と、を行い、
前記判定円の円周上を前記航走体が通過する位置の変化量が、設定された範囲内に収まると、位置制御処理を行い、
前記位置制御処理では、
前記推進装置の回転速度と、前記航走体の水平航走時の対水速度との関係を計測したデータを、予め備え、
前記航走体が前記判定円の外側から内側に進入するときの対地速度と、前記推進装置の回転速度を基にデータを用いて算出した対水速度を用いて、前記航走体の周囲に存在している水の推定水速度を求める処理と、
前記推定水速度が、前記舵の効きが担保される最低限の速度よりも小さいか、否かを判断する速度判断処理と、を行い、
更に、前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度よりも小さいと判断された場合は、前記データを基に、前記舵の効きが担保される最低限の速度に対応する対水速度を得るために必要な前記推進装置の回転速度を算出する処理と、算出された回転速度に対応する回転速度指令値を、前記推進装置へ与える処理と、を行い、
前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度以上であると判断された場合は、対水速度を、前記推定水速度に前記移動体の対水速度のオープンループ制御を行う際の速度誤差以上の値を加えた値に設定する処理と、設定された対水速度を得るために必要な前記推進装置の回転速度を算出する処理と、算出された回転速度に対応する回転速度指令値を、前記推進装置へ与える処理と、を行うこと
を特徴とする航走制御方法。 The process of creating a judgment circle with a radius set around the set target position, and
Judgment processing for determining whether or not the position of the navigating body is outside the determination circle based on the determination circle and the information on the position of the navigating body.
As a result of the determination process, when it is determined that the vehicle is located in a region outside the determination circle, the target direction navigation process for causing the vehicle to travel forward toward the target position is performed.
As a result of the determination process, if it is determined that the traveling body is within the region within the determination circle, a process of stopping the thrust in the forward direction by the propulsion device of the traveling body is performed.
When the amount of change in the position where the traveling body passes on the circumference of the determination circle is within the set range, the position control process is performed.
In the position control process,
Data for measuring the relationship between the rotational speed of the propulsion device and the water speed of the traveling body during horizontal traveling is prepared in advance.
Using the ground speed when the vehicle enters from the outside to the inside of the determination circle and the water speed calculated using data based on the rotation speed of the propulsion device, around the vehicle. The process of finding the estimated water velocity of existing water,
A speed determination process for determining whether or not the estimated water speed is smaller than the minimum speed at which the effectiveness of the rudder is guaranteed is performed.
Further, when it is determined as a result of the speed determination process that the estimated water speed is smaller than the minimum speed at which the effectiveness of the steering is guaranteed, the effectiveness of the steering is guaranteed based on the data. A process of calculating the rotation speed of the propulsion device required to obtain a water speed corresponding to the minimum speed, and a process of giving a rotation speed command value corresponding to the calculated rotation speed to the propulsion device. , Do,
If, as a result of the speed determination process, it is determined that the estimated water speed is equal to or higher than the minimum speed at which the effectiveness of the steering wheel is guaranteed, the water speed is set to the estimated water speed and the moving body is paired. A process of setting a value obtained by adding a value equal to or greater than the speed error when performing open loop control of the water speed, and a process of calculating the rotation speed of the propulsion device required to obtain the set water speed, and calculation. A cruising control method comprising a process of giving a rotation speed command value corresponding to a given rotation speed to the propulsion device .
前記速度制御は、前記航走体が現在位置から前記目標位置に接近する方向へ前進移動するための対地速度の目標値を設定し、外乱が存在している環境下においても前記航走体の対地速度が、設定された前記対地速度の目標値に一致するように回転速度指令値を定めて、該回転速度指令値を前記推進装置へ与える処理を行い、
前記方位制御は、前記推進装置の運転により前記航走体が前進するときの進行方向が、
前記目標位置に向くように、舵角度指令値を定めて、該舵角度指令値を、前記航走体の舵へ与える処理を行う
請求項１に記載の航走制御方法。 The target directional navigation process includes speed control and directional control.
The speed control sets a target value of the ground speed for the traveling body to move forward from the current position in a direction approaching the target position, and the traveling body of the traveling body even in an environment where a disturbance exists. A processing is performed in which a rotation speed command value is set so that the ground speed matches the set target value of the ground speed, and the rotation speed command value is given to the propulsion device.
In the directional control, the traveling direction when the traveling body moves forward due to the operation of the propulsion device is set.
The navigation control method according to claim 1, wherein a rudder angle command value is determined so as to face the target position, and the steering angle command value is given to the rudder of the traveling body.
前記速度判断処理の結果、前記推定水速度が、前記舵の効きが担保される最低限の速度以上であると判断された場合に、前記回転速度指令値を前記推進装置へ与える処理の後、前記航走体の位置の変化を、設定された時間計測する処理と、
計測された前記航走体の位置の変化が、前記目標位置に近付く方向の変化の場合は、近付く速度の分、対水速度が低減するように、前記推進装置へ与える前記回転速度指令値を補正する処理と、
計測された前記航走体の位置の変化が、前記目標位置から離れる方向の変化の場合は、離れる速度の分、対水速度が増加するように、前記推進装置へ与える回転速度指令値を補正する処理と、を行う
請求項１に記載の航走制御方法。 The position control process is
After the process of giving the rotational speed command value to the propulsion device when it is determined as a result of the speed determination process that the estimated water speed is equal to or higher than the minimum speed at which the effectiveness of the rudder is guaranteed. The process of measuring the change in the position of the navigator for a set time, and
When the measured change in the position of the traveling body is a change in the direction approaching the target position, the rotation speed command value given to the propulsion device is set so that the water speed is reduced by the approaching speed. The process of correction and
When the measured change in the position of the traveling body is a change in the direction away from the target position, the rotation speed command value given to the propulsion device is corrected so that the speed against water increases by the amount of the moving speed. And to do
The navigation control method according to claim 1 .
前記目標補正処理では、
前記判定円の円周上を通過するときの前記航走体の位置を求める処理と、
求めた位置を、設定された定点保持目標位置に一致させるために必要なシフト量を求める処理と、
前記目標位置から、求めた前記シフト量で変位した位置に、次の目標位置を設定する処理と、
前記次の目標位置を中心とする前記設定された半径の判定円を設定する処理と、
前記目標位置および前記目標位置を中心とする判定円を、前記次の目標位置および前記次の目標位置を中心とする判定円に置き換える処理と、を行う
請求項１又は２に記載の航走制御方法。 When the amount of change in the position where the traveling body passes on the circumference of the determination circle is within the set range, the target correction process is performed.
In the target correction process,
The process of finding the position of the traveling body when passing on the circumference of the determination circle, and
The process of finding the shift amount required to match the found position with the set fixed point holding target position, and
The process of setting the next target position at the position displaced by the obtained shift amount from the target position, and
The process of setting the determination circle of the set radius centered on the next target position, and
The process of replacing the target position and the determination circle centered on the target position with the determination circle centered on the next target position and the next target position is performed.
The navigation control method according to claim 1 or 2 .
推進装置と、Propulsion device and
舵と、Rudder and
前記航走体の位置を検出する自己位置検出手段と、A self-position detecting means for detecting the position of the traveling body and
目標位置が設定される目標設定部と、The target setting unit where the target position is set and the target setting unit
制御器とを備え、Equipped with a controller,
前記制御器は、請求項１から請求項４のいずれか一項に記載の航走制御方法を実施する機能を備えることThe controller is provided with a function of implementing the navigation control method according to any one of claims 1 to 4.
を特徴とする航走制御装置。A navigation control device featuring.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2018096449A JP7000987B2 (en) | 2018-05-18 | 2018-05-18 | Navigation control method and navigation control device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2018096449A JP7000987B2 (en) | 2018-05-18 | 2018-05-18 | Navigation control method and navigation control device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2019199238A JP2019199238A (en) | 2019-11-21 |
| JP7000987B2 true JP7000987B2 (en) | 2022-01-19 |
Family
ID=68612793
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2018096449A Active JP7000987B2 (en) | 2018-05-18 | 2018-05-18 | Navigation control method and navigation control device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7000987B2 (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114217625B (en) * | 2021-12-15 | 2024-05-28 | 浙江九段智能科技有限公司 | Control method for specified point floating mode of autonomous underwater vehicle |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000344193A (en) | 1999-06-01 | 2000-12-12 | Fuji Royal:Kk | Automatic return navigation device |
| JP2016144971A (en) | 2015-02-06 | 2016-08-12 | 古野電気株式会社 | Hull control device, hull control method |
| JP2017178214A (en) | 2016-03-31 | 2017-10-05 | 株式会社Ihi | Navigation control method and apparatus |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH1134973A (en) * | 1997-07-22 | 1999-02-09 | Yamaha Motor Co Ltd | Observation ship |
-
2018
- 2018-05-18 JP JP2018096449A patent/JP7000987B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000344193A (en) | 1999-06-01 | 2000-12-12 | Fuji Royal:Kk | Automatic return navigation device |
| JP2016144971A (en) | 2015-02-06 | 2016-08-12 | 古野電気株式会社 | Hull control device, hull control method |
| JP2017178214A (en) | 2016-03-31 | 2017-10-05 | 株式会社Ihi | Navigation control method and apparatus |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2019199238A (en) | 2019-11-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP1365301B1 (en) | Method and system for maneuvering a movable object | |
| JP6036515B2 (en) | Underwater vehicle | |
| JP4982407B2 (en) | Mobile object image tracking apparatus and method | |
| JP3949932B2 (en) | Autonomous underwater vehicle navigation control system | |
| CN115515851A (en) | Ship automatic guiding method, ship automatic guiding program, ship automatic guiding system, and ship | |
| US11862026B2 (en) | Marine propulsion control system and method with proximity-based velocity limiting | |
| JP2023024675A (en) | Route generating device | |
| WO2018123948A1 (en) | Autopilot system for vessel, vessel, and autopilot method for vessel | |
| JP6658220B2 (en) | Navigation control method and device | |
| JP7288791B2 (en) | A method for controlling movement of a vessel in the vicinity of an object | |
| WO2018056079A1 (en) | Underwater travelling body and method for controlling orientation of underwater travelling body | |
| EP2015986A2 (en) | Guidance of marine vessels | |
| JP7000987B2 (en) | Navigation control method and navigation control device | |
| JP2005239027A (en) | Underwater vehicle and its control method | |
| JP6487264B2 (en) | Waypoint generator | |
| JP6953773B2 (en) | Mobile control method and mobile control system | |
| JP7417538B2 (en) | Control target generation device and ship maneuvering control device | |
| JP6780528B2 (en) | Position holding method and moving body | |
| JP3999976B2 (en) | Maneuvering method and apparatus | |
| JP2021064248A (en) | Automatic ship steering system for ship | |
| JPH0578476B2 (en) | ||
| JP7294255B2 (en) | Docking route generation method | |
| WO2023089843A1 (en) | Automatic steering device for vessels | |
| JP2022175993A (en) | Automatic steering system for ship | |
| JP3899803B2 (en) | Guidance control device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20190325 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20190326 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20210114 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210825 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20210831 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20211028 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20211124 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20211207 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 7000987 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |