JP2017178214A - Navigation control method and apparatus - Google Patents

Abstract

PROBLEM TO BE SOLVED: To perform a fixed point holding without using a sub-thruster.SOLUTION: At a position moved to the upstream of the direction to which a disturbance 7 acts, by a set distance L0 from a target position T0 for holding the fixed point of an unmanned vehicle A, there is set a target azimuth reference point T1. In an unmanned vehicle A, a direction, which is directed from the present position to the target azimuth reference point T1, is set at a target azimuth θ, to perform an azimuth control for controlling an azimuth so that the unmanned vehicle A may be directed to that target azimuth θ. For the present position of the unmanned vehicle A, moreover, the deviation from the target position T0 in the direction parallel to the target azimuth θ is determined to set a target velocity according to that deviation. A velocity control for controlling a main thruster is performed so that the velocity of the unmanned vehicle A may coincide with that target speed.SELECTED DRAWING: Figure 2

Description

  The present invention relates to a traveling control method and apparatus for holding a traveling body at a fixed point at a target position.

  Unmanned vehicles such as unmanned surface vehicles (hereinafter referred to as USV) and underwater unmanned vehicles (hereinafter referred to as UUV) are used for fixed-point observations, mother ships and others. It is required to maintain a fixed point that stays at the same position for a set time, for example, when a meeting is held with a vehicle.

  However, disturbances such as wind, waves, and tidal currents may act on the USV. Moreover, disturbances such as tidal currents may act on UUV. Therefore, in order to keep the fixed point by these unmanned traveling bodies continuously, the traveling control that compensates for the position shift caused to the unmanned traveling bodies by the disturbance is required.

  By the way, there are two types of unmanned vehicles: a hovering type unmanned vehicle that stays in the survey target point or a narrow range based on that point and investigates the surrounding situation, and a set navigation system. There is a cruising type (cruising type) unmanned vehicle that is designed to investigate surrounding conditions while moving (cruising) at a running speed.

  Among these, cruising type unmanned aerial vehicles are often limited by the weight of the airframe because they require a certain degree of high speed for navigation. Therefore, the movement of the cruising unmanned vehicle is basically performed by controlling the main thruster that generates thrust in the forward and reverse directions and by controlling the rudder (steering device).

  Therefore, in a cruising type unmanned vehicle, it is desired that the fixed point maintenance under the condition of being subjected to disturbance is also realized by the control of the main thruster and the control of the rudder.

  Note that an automatic fixed point return control method for automatically holding a position near a set fixed point has been proposed for a ship that is a watercraft (see, for example, Patent Document 1).

Japanese Patent Application Laid-Open No. 7-235991

  However, in the technique disclosed in Patent Document 1, when the ship receives a disturbance (external force) and moves away from the fixed point, the bow thruster is operated to displace the bow toward the fixed point, and the propulsion device is operated in that state. The ship is moved forward toward a fixed point, and the ship is returned to the fixed point. Therefore, a bow raster is required to implement this technique.

  Moreover, the bow thruster is required to have a capacity that enables the bow to be directed to a fixed point against disturbances acting on the ship.

  Therefore, in the past, it has been proposed that a method for holding a fixed point without using a sub-thruster such as a bow thruster under a condition subject to a disturbance on a cruising type traveling body has not been proposed.

  Therefore, the present invention intends to provide a cruise control method and apparatus for enabling a cruising type traveling body to maintain a fixed point without using a sub-thruster in an environment subject to disturbance. is there.

  In order to solve the above-mentioned problem, the present invention sets the target position for holding the fixed point of the traveling body and goes upstream in the direction in which the disturbance acts on the traveling body at a distance set from the target position. A process of setting a target azimuth reference point at a moved position and a fixed point holding process including azimuth control and speed control are performed, and the azimuth control is performed from the current position of the traveling body to the target azimuth reference point. The target direction is set as a target direction, and the rudder of the navigation body is controlled so that the navigation body faces the target direction, and the speed control is performed for the current position of the navigation body. A deviation from the target position in a direction parallel to the azimuth is obtained, a target speed of the traveling body is set according to the deviation, and the speed of the traveling body matches the set target speed. Navigation control method for controlling the propulsion device of the vehicle To.

  A fixed point holding process start range is set with a radius set around the target position, and the fixed point holding process is performed when the vehicle is located within the fixed point holding process start range.

  An approach start range is set around the fixed point holding process start range, and the vehicle that reaches the approach start range from the outside is decelerated between the approach start range and the fixed point holding process start range. From this, the fixed point holding process start range is reached.

  A propulsion device, a rudder, a self-position detecting means for detecting the position of the navigating body, a disturbance detecting means for detecting the direction of the disturbance acting on the navigating body, and a controller. And the controller is a cruise control device having a function of executing the cruise control method.

  According to the cruise control method and apparatus of the present invention, it is possible to maintain a fixed point for a cruising type traveling body in an environment where disturbance exists without using a sub-thruster.

It is a schematic diagram which shows an example of a navigation control apparatus. It is a schematic diagram which shows the fixed point holding | maintenance process in a cruise control method. It is a figure explaining the position correction of the navigation body by a fixed point holding process. It is a figure explaining switching of the control phase in a cruising control method.

  Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.

[Embodiment of Navigation Control Method]
FIG. 1 is a schematic diagram showing a cruise control device used for carrying out the cruise control method. FIG. 2 is a diagram showing an outline of the fixed point holding process in the cruise control method. FIG. 3 is a diagram showing the principle of position correction in the sway direction of the navigation body by the fixed point holding process. FIG. 4 is a schematic diagram showing a range in which the control phase is switched in the cruise control method.

  This embodiment demonstrates the case where a cruise control method is performed applying an unmanned vehicle.

  First, the configuration of the cruise control device 1 used to implement the cruise control method of the present embodiment will be described with reference to FIG.

  The cruise control device 1 is installed in the cruising-type unmanned vehicle A, and detects the position and orientation (direction) of the unmanned vehicle A (hereinafter referred to as self-position detection device 2). ), Means for detecting disturbance 7 (see FIG. 2) existing around the unmanned vehicle A (hereinafter referred to as disturbance detecting means 3), a main thruster 4 as a propulsion device, and a rudder (steering device) ) 5 and a controller 6 for calculating the command values of the main thruster 4 and the rudder 5.

  The self-position detecting means 2 has a function of detecting information on the position of the unmanned vehicle A itself with respect to absolute coordinates set in a region including the fixed point holding target position T0 and the target azimuth reference point T1 as shown in FIG. It has. In addition, although it is suitable to use the coordinate by longitude and latitude as this absolute coordinate, you may make it use the absolute coordinate which makes the origin any other point. Hereinafter, the absolute coordinates are referred to as predetermined absolute coordinates.

  Further, the self-position detecting means 2 has a function of detecting information on the direction (orientation) in which the airframe of the unmanned aerial vehicle A faces as the attitude of the unmanned vehicle A.

  Furthermore, the self-position detecting means 2 preferably has a function of detecting information related to the speed when the unmanned vehicle A moves.

  When the unmanned vehicle A is UUV, as the self-position detecting means 2, for example, an inertial navigation device (INS) may be used.

  A mother ship (support ship) (not shown) of the UUV can measure the position of the ship by longitude and latitude using a global navigation satellite system such as GPS, and UUV based on the own ship by acoustic positioning. Relative position, moving direction, and speed can be obtained. Therefore, based on these pieces of information, the mother ship can calculate position, posture, and speed information related to predetermined absolute coordinates of the UUV. In view of this point, the self-position detection unit 2 provided in the UUV may obtain information on the position, posture, and speed related to predetermined absolute coordinates of the UUV from the mother ship by acoustic communication. The self-position detecting means 2 obtains the position information of the mother ship and the information on the relative position, moving direction and speed of the UUV with respect to the mother ship from the mother ship by acoustic communication. Information on the position, orientation, and speed regarding the coordinates may be calculated on the UUV side.

  When the unmanned navigation vehicle A is USV, as the self-position detecting means 2, for example, an inertial navigation device or a global navigation satellite system may be used.

  Further, as the self-position detecting means 2, a point or an object in which position information in predetermined absolute coordinates is known in advance is detected by using a photographed image obtained by a camera provided in the USV, a radar, or the like. Information on the relative position, moving direction, and speed of the USV with respect to the point or the object may be obtained, and the information on the position, posture, and speed regarding the predetermined absolute coordinates of the USV may be calculated.

  The self-position detecting means 2 uses a self-position detecting means 2 of a type or method other than those described above as long as it can detect the position and posture of the unmanned vehicle A itself, and also the speed. Of course, it may be.

  Information on the position, posture and speed of the unmanned vehicle A detected by the self-position detecting means 2 is input to the controller 6.

  The disturbance detection means 3 includes, for example, a ground speed with respect to the seabed (water bottom) of the unmanned vehicle A measured by a Doppler velocimeter (not shown) provided in the unmanned vehicle A, and a water speed of the unmanned vehicle A. And a function of detecting the direction α (see FIG. 2) and the magnitude (strength) on which the disturbance 7 existing around the unmanned vehicle A acts.

  Further, the disturbance detection means 3 includes, for example, information on the position, posture (orientation) and moving speed of the unmanned vehicle A detected by the self-position detection means 2, the thrust generated by the main thruster 4, and From the difference from the behavior estimated by the motion model of the traveling body based on the operation state of the rudder 5, the direction α and the magnitude of the disturbance 7 existing around the unmanned traveling body A are detected. It may be provided with a function.

  When the unmanned vehicle A is UUV, the disturbance 7 detected by the disturbance detection means 3 is mainly a tidal current existing around the UUV. When the unmanned vehicle A is USV, the disturbance 7 detected by the disturbance detection means 3 is a combination of the influence of wind, waves, and tidal currents on the USV.

  It should be noted that the disturbance detection means 3 can be any type of disturbance other than those described above as long as it can detect the direction α and the size of the disturbance 7 existing around the unmanned vehicle A. Of course, the detection means 3 may be used.

  Information relating to the direction α and the magnitude of the disturbance 7 existing around the unmanned vehicle A detected by the disturbance detection means 3 is input to the controller 6.

  The main thruster 4 generates thrust in the forward direction and the reverse direction of the unmanned vehicle A. The main thruster 4 is provided, for example, on the rear side of the unmanned vehicle A.

  Further, the main thruster 4 has a function of operating at a rotational speed according to the rotational speed command value c1 when the rotational speed command value c1 is given from the controller 6.

  The rudder 5 is for changing the direction (orientation) of the unmanned aerial vehicle A. The rudder 5 is provided, for example, on the rear side of the unmanned aerial vehicle A so as to be close to the main thruster 4.

  Further, the rudder 5 has a function of operating an actuator (not shown) and changing the angle according to the angle command value c2 when the angle command value c2 is given from the controller 6.

  Next, the function of the controller 6 will be described, and the cruise control method of the present embodiment will be described.

  As shown in FIG. 2, the controller 6 is registered in advance with data of coordinates (Y0, X0) of the target position T0 for holding the fixed point of the unmanned vehicle A with respect to predetermined absolute coordinates. Registration of the coordinate data of the target position T0 may be performed by the user via an input means (not shown). In FIG. 2, the predetermined absolute coordinates are indicated by orthogonal coordinates in which the x axis is a vertical axis with the upward direction as a positive direction and the y axis is a horizontal axis with the right direction as a positive direction.

  If the unmanned vehicle A is of a type that is controlled by waypoints, for example, the coordinate data of the target position T0 registered in the controller 6 may be specified by a waypoint file. Of course, the coordinate data of the target position T0 registered in the controller 6 may be specified in a format other than the waypoint file according to the control method during operation of the unmanned vehicle A.

  Information about the direction α and the size of the disturbance 7 existing around the unmanned vehicle A as shown by the arrow in FIG. 2 is input to the controller 6 from the disturbance detection means 3. In the present embodiment, the direction α on which the disturbance 7 acts is represented by an angle in which the positive direction of the x-axis (upward in FIG. 2) is the reference (0 degree) and the clockwise direction is positive. In the present embodiment, an example in which α = 180 degrees is shown.

When the information on the direction α in which the disturbance 7 acts is input, the controller 6 moves the coordinates (Y0, X0) of the target position T0 to the upstream side in the direction α in which the disturbance 7 acts as shown in FIG. The coordinate (Y1, X1) translated in parallel by the set distance L0 is calculated by the following equation (1), and the function of setting the target azimuth reference point T1 at the coordinate (Y1, X1) is provided.
Y1 = Y0 + L0 × sin α
X1 = X0 + L0 × cos α (1)

  The distance L0 may be set in advance in consideration of the motion performance such as the turning radius of the unmanned vehicle A. For example, the distance L0 may be the disturbance 7 input from the disturbance detection means 3 to the controller 6. A value corresponding to the magnitude of the disturbance 7 may be set, for example, by calculation using a function having the magnitude as a variable.

  Further, the controller 6 has a function of acquiring the coordinates (YA, XA) of the current position of the unmanned vehicle A based on information input from the self-position detecting means 2.

  Further, the controller 6 determines the coordinates (Y0, X0) of the target position T0, the coordinates (Y1, X1) of the target bearing reference point T1, and the coordinates (YA, XA) of the current position of the unmanned vehicle A. Based on this, the following fixed point holding process is implemented.

In the fixed point holding process, the controller 6 first determines the direction from the current position coordinates (YA, XA) of the unmanned vehicle A to the coordinates (Y1, X1) of the target direction reference point T1 as the unmanned vehicle A. Is calculated by the following equation (2).
θ = atan ((Y1-YA) / (X1-XA)) (2)

  In the present embodiment, the target orientation θ is indicated by an angle with the positive direction of the x axis (upward in FIG. 2) as a reference and the clockwise direction as positive.

Next, the controller 6 determines the deviation from the coordinates (Y0, X0) of the target position T0 in the direction parallel to the target azimuth θ with respect to the coordinates (YA, XA) of the current position of the unmanned vehicle A as a surge component. The deviation E1 is calculated by the following equation (3). The right side of the equation (3) is the inner product of the vector (Y0−YA, X0−XA) and the vector (sin θ, cos θ).
E1 = (Y0−YA) sin θ + (X0−XA) cos θ (3)

Next, the controller 6 calculates a target speed V1 for speed control of the unmanned vehicle A according to the following equation (4). The target speed V1 is a ground speed.
V1 = E1 × P1 (4)
Here, P1 is a position feedback gain.

  Thereafter, the controller 6 calculates an angle command value c2 of the rudder 5 necessary for adjusting the direction of the unmanned vehicle A to the target direction θ obtained by the equation (2) as direction control. The calculation of the angle command value c2 may be performed according to the turning performance of the unmanned vehicle A that performs the fixed point holding process.

  The controller 6 outputs an angle command value c2 obtained as a result of this calculation to the rudder 5.

  Moreover, the controller 6 calculates the rotational speed command value c1 of the main thruster 4 for realizing the target speed V1 obtained by the equation (4) by the operation of the main thruster 4 as speed control. The calculation of the rotational speed command value c1 may be performed according to the propulsion performance by the main thruster 4 of the unmanned vehicle A that performs the fixed point holding process.

  The controller 6 outputs a rotation speed command value c1 obtained as a result of this calculation to the main thruster 4.

  The target speed V1 is exemplified by the case of performing proportional control in which the position feedback gain P1 is applied to the surge component deviation E1, but other feedback control methods such as proportional integral control using a deviation integral value are used. Of course, it may be. The calculation of the target speed V1 may be proportional control, and the thruster rotation speed control for following the target speed V1 may be proportional integral control.

  By the way, when the target position T0 is located on the rear side of the unmanned vehicle A relative to the current position of the unmanned vehicle A with respect to the direction parallel to the target azimuth θ as in the state shown in FIG. Is a negative value of the surge component deviation E1 obtained by the equation (3). Therefore, the target speed V1 obtained by the equation (4) is also a negative value, which is a ground speed in a direction in which the unmanned vehicle A is separated from the target direction reference point T1.

  The target azimuth θ of the unmanned vehicle A is directed from the target position T0 to the target azimuth reference point T1 (an azimuth opposite to the direction in which the disturbance 7 existing in the unmanned vehicle A acts, hereinafter the reference Is in the state where the unmanned vehicle A is disposed between the target position T0 and the target direction reference point T1. Therefore, in this case, if the ground speed when the unmanned vehicle A moves backward due to the action of the disturbance 7 is slower than the target speed V1 having a negative value, the target speed V1 is achieved. The unmanned vehicle A may be moved backward by the operation of the main thruster 4.

  On the other hand, when the target direction θ of the unmanned vehicle A is inclined with respect to the reference direction β, when the unmanned vehicle A moves backward, the target of the unmanned vehicle A in the direction orthogonal to the reference direction β is obtained. There is a possibility that the distance from the position T0 increases. Therefore, when the target azimuth θ of the unmanned aerial vehicle A is inclined with respect to the reference azimuth β and the target speed V1 obtained by the equation (4) is a negative value, the target azimuth reference of the unmanned vehicle A The movement in the direction away from the point T1 is preferably performed by the action of the disturbance 7 as much as possible.

  Here, the principle of position correction of the unmanned vehicle A by the fixed point holding process of the controller 6 will be described with reference to FIG.

  For example, when the fixed point holding process is performed by the controller 6 when the unmanned vehicle A is disposed at the position a1 in FIG. 3, the unmanned vehicle A is obtained by the azimuth control of the controller 6. The angle of the rudder 5 is controlled according to the obtained angle command value c2. For this reason, the unmanned aerial vehicle A is controlled such that the direction of the airframe coincides with the direction from the position a1 toward the target direction reference point T1, that is, the target direction θ1 at that time.

  In this state, in the unmanned vehicle A, the main thruster 4 is operated according to the rotational speed command value c1 obtained by the speed control of the controller 6.

  Therefore, the unmanned vehicle A in the state arranged at the position a1 moves forward toward the target direction θ1 by the operation of the main thruster 4. At this time, since the target azimuth θ1 is inclined with respect to the reference azimuth β, the forward movement of the unmanned vehicle A includes a component in a direction approaching the target position T0 with respect to the direction orthogonal to the reference azimuth β. It is. Therefore, for example, if the unmanned aerial vehicle A moves forward from the position a1 to the position a2, the unmanned vehicle A approaches the target position T0 in the direction orthogonal to the reference direction β.

  On the other hand, the direction in which the unmanned aerial vehicle A moves in response to the disturbance 7 coincides with the direction α in which the disturbance 7 existing around the unmanned vehicle A acts. Therefore, for example, even if the unmanned aerial vehicle A receives the disturbance 7 and moves from the position a2 to the position a3, the motion of the unmanned vehicle A at that time includes a component in a direction orthogonal to the reference direction β. Not. Therefore, the unmanned vehicle A does not change the distance from the target position T0 in the direction orthogonal to the reference direction β.

  Therefore, the unmanned vehicle A moves from the position a1 to the position a3 by combining the advance in the direction from the position a1 toward the target azimuth θ1 and the movement due to the disturbance 7. As a result, the unmanned vehicle A has a reduced deviation from the target position T0 in the sway direction.

  When the unmanned aerial vehicle A is located at the position a3, the orientation of the unmanned aerial vehicle A is the direction from the position a3 toward the target orientation reference point T1 by the fixed point holding process by the controller 6. It is controlled so as to coincide with θ2.

  In this state, in the unmanned vehicle A, the main thruster 4 is operated in accordance with the rotational speed command value c1 obtained by the speed control of the controller 6, so the unmanned vehicle A moves forward in the target direction θ2.

  Also in this case, since the target azimuth θ2 is inclined with respect to the reference azimuth β, if the unmanned vehicle A moves forward from the position a3 to the position a4, the motion at that time is orthogonal to the reference azimuth β. The component of the direction which approaches target position T0 regarding the direction to do is contained. Therefore, the unmanned vehicle A approaches the target position T0 in the direction orthogonal to the reference direction β.

  On the other hand, even if the unmanned aerial vehicle A receives the disturbance 7 and moves from the position a4 to the position a5, the motion of the unmanned aerial vehicle A at that time includes a component in a direction orthogonal to the reference direction β. Therefore, the distance from the target position T0 does not change in the direction orthogonal to the reference orientation β.

  Therefore, the unmanned vehicle A moves from the position a3 to the position a5 by combining the advance in the direction from the position a3 toward the target direction θ2 and the movement due to the disturbance 7. As a result, the unmanned vehicle A further reduces the deviation from the target position T0 in the sway direction.

  Thereafter, the unmanned aerial vehicle A combines the advance in the direction of the target azimuth θ determined according to the position where the unmanned aerial vehicle A is arranged and the movement in response to the disturbance 7 as described above. Is repeated. At this time, as the inclination angle of the target azimuth θ with respect to the reference azimuth β decreases, the target position T0 with respect to the direction orthogonal to the reference azimuth β included in the movement of the unmanned vehicle A toward the target azimuth θ. The component in the direction approaching is also gradually reduced.

  Therefore, the unmanned aerial vehicle A comes closer to the target position T0 in the sway direction.

  Further, in the fixed point holding process by the controller 6, as described above, speed control is also performed according to the surge component deviation with respect to the target position T0 of the unmanned aerial vehicle A. The surge component deviation is also gradually reduced.

  Therefore, according to the fixed point holding process by the controller 6, the unmanned vehicle A can be made asymptotic to the fixed point holding target position T0.

  At this time, in the fixed point holding process by the controller 6, the azimuth control is performed by the control based on the angle command value c2 of the rudder 5, and the speed control is performed by the control based on the rotational speed command value c1 of the main thruster 4. For this reason, the controller 6 can implement the azimuth control and speed control in the fixed point holding process as independent two-degree-of-freedom control. Therefore, it is possible to suppress the mutual influence between the azimuth control and the speed control, and the controllability of each control can be improved. Therefore, according to the fixed point holding process of the controller 6, the unmanned vehicle A can be smoothly asymptotic to the target position T0.

  By the way, since the unmanned traveling body A is a cruising type, a certain speed is required for normal traveling.

  On the other hand, as described above, in the fixed point holding process by the controller 6, speed control based on the relative positional relationship with the target position T0 of the unmanned vehicle A is performed.

  Therefore, in the present embodiment, in order for the unmanned vehicle A to smoothly shift from the cruising state to the state where the fixed point holding process by the controller 6 is performed, the controller 6 According to the distance R (refer FIG. 4) from the target position T0 of the running body A, the function which further performs the switching of the following controls is provided.

  As shown in FIG. 4, the controller 6 includes a fixed point holding process start range 8 with a radius r1, a fixed point holding process departure range 9 with a radius r2, and an approach start range 10 with a radius r3, centered on the target position T0. Is preset. Note that r1 <r2 <r3.

  The fixed point holding process start range 8 is a range in which the controller 6 starts the fixed point holding process when the unmanned vehicle A approaching the fixed point holding target position T0 from outside is positioned within the fixed point holding process start range 8. It is. Accordingly, the radius r1 is set to a value smaller than the distance L0 used for setting the target orientation reference point T1 (r1 <L0).

  If the set value of the radius r1 defining the fixed point holding process start range 8 is too large, the position where the fixed point holding process is started for the unmanned vehicle A is far from the fixed point holding target position T0, and the unmanned navigation It takes time for the body A to reach or approach the target position T0.

  On the other hand, when the set value of the radius r1 is too small, depending on the turning performance of the unmanned vehicle A, the unmanned vehicle A approaching the fixed point holding processing start range 8 from the outside is the fixed point holding processing start range. There is a possibility of passing 8 without reaching 8.

  Therefore, the radius r1 that defines the fixed point holding process start range 8 can reliably reach the fixed point holding process start range 8 without passing by the unmanned vehicle A that approaches the fixed point holding process start range 8 at the approach speed described later. It is preferable that the fixed point holding process start range 8 is set to be as small as possible.

  When the unmanned vehicle A that travels toward the target position T0 at the cruise speed reaches the approach start range 10 from the outside, the approach start range 10 is until the unmanned vehicle A reaches the fixed point holding processing start range. In the meantime, this is a region where the unmanned vehicle A is decelerated from the cruise speed to the approach speed suitable for the start of the fixed point holding process. This approach speed is set in advance in the controller 6 as a relatively low fixed value (ground speed) within a range in which the rudder 5 is effective based on the motion performance of the unmanned vehicle A.

  Therefore, the radius r3 that defines the approach start range 10 is the distance that the difference between the radius r3 and the radius r1 (= r3-r1) requires the unmanned vehicle A to decelerate from the cruise speed to the approach speed. Is set to a value corresponding to. FIG. 4 shows a state in which the radius r3 is the same as the distance L0 used for setting the target azimuth reference point T1, but it goes without saying that the radius r3 and the distance L0 may be different. .

  The fixed point holding process departure range 9 is located at the outer peripheral position of the fixed point holding process start range 8 for the unmanned vehicle A that has reached the fixed point holding process start range 8 and has started the fixed point holding process by the controller 6. This is a range for forming a dead zone (hysteresis) until the fixed point holding process is canceled.

  In particular, the fixed point holding process by the controller 6 performs azimuth control in which the direction of the unmanned vehicle A is directed to the direction of the target azimuth reference point T1 (target azimuth θ) set separately from the target position T0. It is. Therefore, in the state where the fixed point holding process is started and the state where the fixed point holding process is released, the direction in which the unmanned vehicle A is directed differs between the target orientation reference point T1 and the target position T0.

  Therefore, the radius r2 defining the fixed point holding process deviation range 9 is set as large as possible, and for the unmanned vehicle A for which the fixed point holding process by the controller 6 has been started once, the fixed point holding is performed due to the influence of disturbance or the like. Even if a displacement that deviates from the processing start range 8 occurs, it is preferable to maintain the fixed point holding processing as much as possible.

  Further, the controller 6 sets the target position of the unmanned vehicle A at the current position based on the coordinates (YA, XA) of the current position of the unmanned vehicle A and the coordinates (Y0, X0) of the target position T0. A function of calculating a distance R from T0 and controlling the traveling state of the unmanned vehicle A according to the result is as follows.

  When the distance R is larger than the radius r3 (R> r3), the controller 6 performs control for causing the unmanned vehicle A to travel toward the target position T0 at the cruising speed. This control may be performed by waypoint control or other existing autonomous control methods. Thereby, when the unmanned vehicle A is located outside the approach start range 10, it travels at the cruising speed toward the target position T0.

  In addition, the controller 6 determines that the distance R is equal to or less than the radius r2 (r2 <R ≦ r3), and the distance R is equal to or less than the radius r2 and greater than the radius r1 ( r1 <R ≦ r2), and when the condition that the distance R at that time is not increased from a value equal to or less than the radius r1, the unmanned vehicle A is decelerated to the approach speed and toward the target position T0. Control to sail. Note that the control in this case may be performed by waypoint control or other existing autonomous control methods. Thereby, when the unmanned vehicle A reaches the approach start range 10 from the outside, the unmanned vehicle A is decelerated to the approach speed, and in this state, travels toward the target position T0 until the fixed point holding processing start range 8 is reached. Further, after the fixed point holding process by the controller 6 is once started in the fixed point holding process start range 8, the unmanned vehicle A starts to move to the outside of the fixed point holding process deviation range 9 due to a disturbance or the like. The cruise toward the target position T0 is started again until the range 8 is reached.

  Further, the controller 6 determines that the distance R is equal to or smaller than the radius r1 (R ≦ r1), and the distance R is equal to or smaller than the radius r2 and larger than the radius r1 (r1 <R ≦ r2). When the condition that the distance R is increased from a value equal to or less than the radius r1, the above-described fixed point holding process is performed. Thereby, when the unmanned vehicle A reaches the fixed point holding process start range 8 from the outside, the fixed point holding process is started. In addition, after the fixed point holding process is started, the unmanned vehicle A continues to move to the outer periphery of the fixed point holding process deviation range 9 even if displaced due to the disturbance 7 as long as the fixed point holding process is continued. .

  Therefore, according to the cruise control method of the present embodiment, the cruising unmanned vehicle A can be moved asymptotically to the fixed point holding target position T0 in an environment where the disturbance 7 exists. At the same time, the fixed point can be maintained at the target position T0 or near the target position T0 of the unmanned vehicle A.

  Furthermore, since the fixed point holding process of the unmanned vehicle A can be realized by the azimuth control by the rudder 5 and the speed control by the main thruster 4, it is not necessary to use a sub-thruster such as a horizontal thruster.

  Further, in the cruise control method of the present embodiment, when the unmanned vehicle A reaches the fixed point holding processing start range 8, the aircraft direction of the unmanned vehicle A is set to the target bearing θ that is directed to the target bearing reference point T1. In this state, the target position T0 is approached.

  In the method of always directing the orientation of the vehicle to the target position, if the vehicle passes the target position for some reason, a turning motion is required to change the direction of the vehicle significantly at that time. However, in the traveling control method of this embodiment, even if the unmanned traveling body A passes the target position T0, a large turning operation is unnecessary.

  Therefore, in the navigation control method of this embodiment, the unmanned vehicle A is approaching the target position T0 while being pushed by the disturbance 7 from a position close to the upstream side of the disturbance 7 with respect to the target position T0. Even if it exists, the unmanned navigation body A can be made to approach smoothly to the target position T0, and fixed-point holding | maintenance can be started.

  By the way, generally, in order for the rudder to work, a water speed of about 0.5 knots is required. Therefore, the disturbance 7 existing around the unmanned aerial vehicle A is strong enough to generate a water speed of less than 0.5 knots with respect to the unmanned aerial vehicle A in a state of holding a fixed point. In this case, the main thruster 4 may be operated intermittently, and the unmanned vehicle A may be temporarily increased to the water speed at which the rudder 5 works, so that the azimuth control by the rudder 5 is performed.

  If the disturbance 7 existing around the unmanned vehicle A is so small that the upstream direction cannot be specified, the above-mentioned target direction reference point T1 cannot be set. The fixed point holding process by the direction control of the rudder 5 and the speed control of the main thruster 4 by 6 cannot be performed.

  Even under such conditions, in order to obtain the same performance as the fixed point holding process by the controller 6 described above, as shown by a two-dot chain line in FIG. 11 is preferably provided to perform direction control assist and drift control.

  In this case, the direction control assist may be, for example, a value obtained by multiplying the angle command value c2 given to the rudder 5 from the controller 6 by a gain, as the subthruster command value.

In addition, the drift control is performed by, for example, calculating a deviation from the coordinates (Y0, X0) of the target position T0 with respect to the direction orthogonal to the target direction θ with respect to the coordinates (YA, XA) of the current position of the unmanned vehicle A as a sway component. The deviation E2 is calculated by the following equation (5). The sway component deviation E2 is a positional deviation in the left-right direction of the unmanned vehicle A. Therefore, the sway component deviation E2 is relative to the deviation vector (Y0-YA, X0-XA) from the current position of the unmanned aerial vehicle A to the target position T0 and the target orientation θ that is the direction of the unmanned aerial vehicle A. It can be represented by the inner product of the unit vector (sin (θ + 90deg), cos (θ + 90deg)) = (cosθ, −sinθ) in the sway direction. That is, the following formula.
E2 = (Y0−YA) cos θ− (X0−XA) sin θ (5)

  Next, a value obtained by multiplying the sway component deviation E2 by gain is used as drift control, that is, a target speed in the sway direction, and a subthruster command value (rotation speed command value) for realizing this target speed by operating the subthruster is calculated. That's fine.

  At this time, when the sub-thruster is located before and after the airframe of the unmanned aerial vehicle A, the direction control assist may be reversed in the front-rear direction, and the drift control may be performed in the same direction in the front-rear direction.

  As a control method of the target speed in the sway direction, the case of performing proportional control in which a gain is applied to the sway component deviation E2 is illustrated, but other feedback control methods such as proportional integration control using a deviation integral value together are used. Of course, it is also good. Further, the calculation of the target speed in the sway direction may be set as proportional control, and the sub-thruster rotation speed control for following the target speed may be set as proportional integral control.

  Note that the present invention is not limited to the above-described embodiment, and the self-position detection means 2, the disturbance detection means 3, and the controller 6 are separately required for the unmanned vehicle A to travel and operate. These functions may be combined.

In FIG. 1, the self-position detecting means 2, the disturbance detecting means 3, and the controller 6 are separately described in the navigation control device 1, but the configuration provided in one device having any two functions. Or the structure with which one apparatus with three functions is equipped may be sufficient.

  The direction α on which the disturbance 7 existing around the unmanned vehicle A acts may be an arbitrary direction. Further, the direction α and the size of the disturbance 7 may be changed with the passage of time.

  The cruise control method and apparatus of the present invention may be applied to a case where a fixed point is held at a set target position T0 by automatic control for a manned cruiser such as a ship or a submarine.

  Of course, various modifications can be made without departing from the scope of the present invention.

2 Self-position detection means, 3 disturbance detection means, 4 main thruster (propulsion device), 5 rudder, 6 controller, 7 disturbance, 8 fixed point holding process start range, 10 approach start range, A unmanned vehicle (travel vehicle) ), E1 Surge component deviation (deviation), T0 target position, T1 target bearing reference point, θ, θ1, θ2 Target bearing

Claims (4)

Processing to set the target position for the fixed point maintenance of the vehicle,
A process of setting a target azimuth reference point at a position moved to the upstream side in the direction in which a disturbance acts on the traveling body at a distance set from the target position;
A fixed point holding process including azimuth control and speed control,
The direction control sets a direction from the current position of the traveling body toward the target direction reference point as a target direction, and controls the rudder of the traveling body so that the navigation body faces the target direction. Processing and
The speed control is set by obtaining a deviation from the target position in a direction parallel to the target direction with respect to the current position of the traveling body and setting a target speed of the traveling body according to the deviation. A navigation control method, comprising: processing for controlling the propulsion device of the navigation body so that the speed of the navigation body coincides with the target speed. Set a fixed point holding process start range with a radius set around the target position,
The cruise control method according to claim 1, wherein the fixed point holding process is performed when the traveling body is disposed within the fixed point holding process start range. Set an approach start range around the fixed point holding process start range,
The navigation body according to claim 2, wherein the vehicle that reaches the approach start range from outside decelerates between the approach start range and the fixed point holding process start range and then reaches the fixed point holding process start range. Running control method. To the navigation body,
A propulsion device;
Rudder,
Self-position detecting means for detecting the position of the navigation body itself;
Disturbance detection means for detecting the direction of disturbance acting on the traveling body;
With a controller,
The said controller is provided with the function to implement the cruise control method as described in any one of Claims 1-3. The cruise control apparatus characterized by these.

Images