JP6780528B2 - Position holding method and moving body - Google Patents

Description

The present invention relates to a position holding method for causing a moving body moving in water or water to hold a position, and a moving body used for carrying out the method.

Ships are known as mobiles that move on the water.

Conventionally, a position holding control device for ships has been proposed as a means for causing a ship to hold a fixed point (see, for example, Patent Document 1).

Basically, the direction of the disturbance acting on the ship is estimated, the target direction is set to the estimated disturbance direction obtained as a result, and the direction of the hull is controlled so as to face this target direction. ing.

Further, as an autonomous underwater vehicle, an underwater vehicle called an AUV (Autonomous Underwater Vehicle) or a UUV (Unmanned Underwater Vehicle) is known.

It may be desired that this type of mobile body also be operated to maintain a certain position depending on the purpose of use and the plan of use.

JP-A-2010-173589

By the way, tidal currents and winds act as disturbances on moving objects on the water. In addition, the tidal current acts as a disturbance on the moving body in the water.

However, the direction of the tidal current may change over time. In addition, the wind direction often changes with the passage of time. Therefore, the direction in which the disturbance acts on the moving body changes with the passage of time. In particular, when the disturbance is slight, the direction in which the disturbance acts is likely to change.

In addition, there is always an error in estimating the direction in which the disturbance acts.

Therefore, the smaller the disturbance, the larger the estimated disturbance estimation direction changes due to a slight estimation error or a change in the direction in which the disturbance acts.

Therefore, in the method of controlling the hull direction so as to face the target direction set in the disturbance estimation direction shown in Patent Document 1, when the disturbance is slight, the target direction changes significantly in a short time or the target. There is a risk that the orientation will change frequently. As described above, when a large change in the target direction in a short time or a frequent change in the target direction occurs, the actual situation is that the processing and operation of the direction control of the ship to be performed according to the target direction become complicated.

Therefore, the present invention can suppress a large change or a frequent change in the target direction of the moving body in a short time regardless of the magnitude of the disturbance, and can control the direction of the moving body. It is an object of the present invention to provide a position holding method capable of causing a moving body to hold a position in a stabilized state, and a moving body used for carrying out the method.

In order to solve the above problems, the present invention is a control device that gives a control output to an actuator that generates a propulsive force in the front-rear direction of a moving body moving underwater or on the water and a directional control propulsive force in the left-right direction of the machine. , The process of setting the target position and the initial value of the reference direction, the process of acquiring the information of the position of the moving body's own machine, the process of acquiring the information of the direction of the moving body's body, and the above. The process of estimating the direction in which the disturbance acts on the moving body's own machine and the state in which the deviation of the moving body's own machine from the target position in the left-right direction of the machine is equal to or greater than the set threshold value. The process of determining whether or not the reset condition of continuing for a set time or longer is satisfied, and if it is determined that the reset condition is not satisfied, the reference direction set at that time is set as the target direction. When it is determined that the process of adjusting the directional direction of the aircraft to the target directional direction and giving the actuator a control output for holding the position of the own aircraft at the target position and the resetting condition are satisfied. Resets the reference direction to the direction opposite to the direction in which the disturbance acts on the own aircraft estimated at that time, and then sets the reset reference direction as the target direction and sets the direction in which the aircraft faces. It is a position holding method that performs a process of giving a control output for holding the position of the own machine to the target position to the actuator according to the target direction.

As the moving body, there is a method of using an underwater vehicle.

Further, a moving body that moves underwater or on water, a means for acquiring information on the position of the moving body's own machine, a means for obtaining information on the orientation of the moving body's body, and a means for the moving body. The control device includes an actuator that generates a propulsive force in the front-rear direction of the aircraft, a directional control propulsive force in the left-right direction of the aircraft, and a control device. The control device has a function of setting a target position and an initial value of a reference direction, and the above. The function of estimating the direction in which the disturbance acts on the moving body's own machine and the state in which the deviation of the moving body's own machine from the target position in the left-right direction of the machine is equal to or greater than the set threshold value. A function that determines whether or not the reset condition of continuing for a set time or longer is satisfied, and if the determination process determines that the reset condition is not satisfied, it is set at that time. The function of performing a process of adjusting the direction in which the aircraft faces to the target direction and giving a control output for holding the position of the own machine at the target position to the actuator, with the reference direction as the target direction, and the above-mentioned If it is determined by the determination process that the resetting condition is satisfied, the reference direction is reset to the direction opposite to the direction in which the disturbance acts on the own machine estimated at that time, and then reset. A function of performing a process of giving a control output to the actuator for holding the position of the own machine at the target position by adjusting the direction of the aircraft to the target direction with the set reference direction as the target direction. It is a moving body having a configuration in which the above is provided.

According to the position holding method of the present invention, it is possible to suppress a large change or a frequent change in the target direction of the moving body in a short time regardless of the magnitude of the disturbance, and the moving body can be suppressed. It is possible to make the moving body hold the position while the directional control of the above is stabilized.

Further, according to the moving body of the present invention, the position holding method can be realized.

It is a schematic diagram which shows the 1st Embodiment of the moving body used for carrying out the position holding method. It is a schematic diagram which shows the control device in the moving body of FIG. It is a figure which shows the operation flow of the reference direction setting part in the control device of FIG. It is a figure which shows the outline of the position holding operation of a moving body. It is a schematic diagram which shows the 2nd Embodiment of the moving body used for carrying out the position holding method. It is a schematic diagram which shows the control device in the moving body of FIG.

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

[First Embodiment]
1 to 4 show an example in which the moving body to be the target of the position holding is an underwater navigating body as the position holding method and the first embodiment of the moving body.

FIG. 1 is a schematic view showing an underwater vehicle used for carrying out the position holding method in the present embodiment. FIG. 2 is a schematic view showing the configuration of the control device in FIG. FIG. 3 is a flow chart showing processing in the reference direction setting unit of the control device of FIG. FIG. 4 shows the operation of the underwater vehicle by implementing the position holding control, and FIGS. 4 (a), (b), and (c) are schematic views showing a state in which disturbances of different sizes and directions act. FIG. 4D is a schematic view showing a state in which the underwater vehicle subjected to the disturbance of FIG. 4C returns to the target position.

The underwater vehicle as a moving body of the present embodiment is shown by reference numeral 1 in FIG.

The underwater navigation body 1 includes, as actuators, a main thruster 2 arranged at the rear part of the airframe, a horizontal thruster 3 arranged at the front part of the airframe, and a rudder 4 arranged at the rear part of the airframe. .. As a result, the underwater navigating body 1 has the propulsive force of the main thruster 2 in the front-rear direction of the machine, and the horizontal thruster 3 and the steering wheel 4 in the left-right direction of the body compared with the propulsion force of the front-back direction of the body. It is supposed to have a weak propulsion force and a directional control propulsion force.

Further, the underwater vehicle 1 is provided with, for example, a vertical thruster 5 arranged in front of the airframe, and the vertical thruster is configured to have a propulsive force in the vertical direction of the airframe and a propulsive force for attitude control from the vertical thruster 5. May be good.

The underwater vehicle 1 is further configured to include a position detecting means 6, a Doppler speedometer 7 as a speed measuring means, an acoustic communication device 8 as a communication device, and a control device 9.

The position detecting means 6 has a function of measuring the position A of the own aircraft of the underwater vehicle 1, the orientation B of the underwater vehicle 1 facing the aircraft, and the attitude C of the aircraft. It is preferable that the position detecting means 6 uses an inertial navigation system or is configured to use the inertial navigation system in combination with another measuring device. The position detecting means 6 is not limited to the configuration provided with the inertial navigation system as long as the function of measuring the position A, the direction B, and the attitude C of the underwater navigation body 1 can be realized.

The information on the position A, the direction B, and the attitude C of the underwater vehicle 1 measured by the position detecting means 6 is given to the control device 9.

The Doppler speedometer 7 has a function of measuring the ground speed D and the water speed E of the underwater vehicle 1. The ground speed D and the water speed E measured by the Doppler speedometer 7 are given to the control device 9.

The acoustic communication device 8 is connected to the control device 9. The acoustic communication device 8 has a function of receiving a command from the acoustic communication device 11 as a communication device provided in the mother ship 10 as a communication target.

Here, the mother ship 10 will be described.

The underwater vehicle 1 of the present embodiment is operated together with the mother ship 10. The mother ship 10 includes an operation terminal 12 operated by an operator (not shown), and the operation terminal 12 is connected to the acoustic communication device 11.

In the mother ship 10, when the operator uses the operation terminal 12 to set a target position F for which the position of the underwater vehicle 1 is desired to be maintained, a command for the target position F is transmitted from the acoustic communication device 11.

When the acoustic communication device 8 receives the command of the target position F, the underwater vehicle 1 sends the command to the control device 9. As a result, the control device 9 sets the target position F for holding the position of the underwater vehicle 1.

Further, the operation terminal 12 of the mother ship 10 may be provided with a function of inputting the reference direction reset command G by the operator. This reference direction reset command G is a command for causing the control device 9 of the underwater vehicle 1 to forcibly reset (update) the reference direction Q, which will be described later.

When the operator inputs the reference direction reset command G using the operation terminal 12, the command for the reference direction reset command G is transmitted from the acoustic communication device 11.

In the underwater vehicle 1, when the acoustic communication device 8 receives the command of the reference direction reset command G, the command is sent to the control device 9. The process when the control device 9 of the underwater vehicle 1 receives the reference direction reset command G will be described later.

The control device 9 in the underwater vehicle 1 has a function of giving a command (control input) to the main thruster 2, the horizontal thruster 3, the rudder 4, and the vertical thruster 5 for the aircraft to hold the position at the target position F. It has.

Specifically, the control device 9 gives a rotation speed command value H to the main thruster 2, a rotation speed command value I to the horizontal thruster 3, a rudder angle command value J to the rudder 4, and rotates to the vertical thruster 5. It has a function to give a number command value K.

On the other hand, the main thruster 2 has a function of giving the rotation speed measurement value L to the control device 9 as feedback of the operation status to the control device 9. Further, the horizontal thruster 3 has a function of giving the rotation speed measurement value M to the control device 9. The rudder 4 has a function of giving a rudder angle measurement value N to the control device 9. The vertical thruster 5 has a function of giving a rotation speed measurement value O to the control device 9.

Further, as shown in FIG. 2, the control device 9 is configured to include a disturbance estimation unit 13, a reference direction setting unit 14, and a control output calculation unit 15 by a software module.

When the control device 9 receives the information of the ground speed D and the ground speed E from the Doppler speedometer 7, the disturbance estimation unit 13 sets the underwater vehicle 1 based on the difference between the ground speed E and the ground speed D. It has a function of estimating the direction of the disturbance on which it is acting, that is, the direction in which the disturbance arrives with reference to the underwater vehicle 1. The direction of the disturbance estimated by the disturbance estimation unit 13 is hereinafter referred to as a disturbance estimation direction P.

The disturbance estimation unit 13 may further have a function of estimating the velocity (magnitude) of the disturbance.

The information of the disturbance estimation direction P obtained by the disturbance estimation unit 13 is given to the reference direction setting unit 14.

The reference direction setting unit 14 is given by the target position F given as a command from the mother ship 10, the current position A and the direction B of the underwater vehicle 1 given by the position detecting means 6, and the disturbance estimation unit 13. It has a function of receiving the information of the disturbance estimation direction P and performing the process of showing the flow diagram in FIG. 3 to determine the reference direction Q.

That is, as shown in FIG. 3, when the position holding of the underwater vehicle 1 at the target position F is started, the reference direction setting unit 14 first starts holding the position of the underwater vehicle 1 at the start of the position holding. The direction B is set to the initial value of the reference direction Q (step S1).

In this step S1, the initial value of the reference direction Q is determined based on the direction B of the underwater vehicle 1 at the start of holding the position, but instead of this, the reference is made by a command of an operator (not shown). Of course, the initial value of the orientation Q may be determined.

Next, the reference direction setting unit 14 refers to the current position A of the underwater vehicle 1 based on the information of the target position F and the current position A of the underwater vehicle 1, and the aircraft left and right from the target position F. The deviation ΔX with respect to the direction is obtained (step S2).

Next, the reference direction setting unit 14 satisfies the resetting condition that the deviation ΔX in the left-right direction of the aircraft obtained in step S2 continues for a set time or longer in a state of being equal to or higher than the set threshold value Xt. It is determined whether or not it is possible (step S3).

In step S3, the resetting condition includes the time-related condition that "the state of being equal to or higher than the threshold value Xt continues for the set time or longer" because of the disturbance received by the underwater vehicle 1. This is because it is considered that the underwater vehicle 1 may undergo a periodic position change when a wave component is included in addition to the tidal current, and a dead zone is provided. Therefore, even if the deviation ΔX exceeds the threshold value Xt, while the deviation ΔX changes before and after the threshold value Xt in a time shorter than the set time, the reference orientation setting unit 14 may perform step S5 described later. Does not proceed.

If it is determined in step S3 that the resetting condition is not satisfied, the reference direction setting unit 14 proceeds to step S4 and performs a process of giving the information of the reference direction Q to the control output calculation unit 15.

On the other hand, if it is determined in step S3 that the resetting condition is satisfied, the reference direction setting unit 14 proceeds to step S5 and faces the disturbance estimation direction P given by the disturbance estimation unit 13 at that time. The direction is set to the reference direction Q.

After step S5, the reference direction setting unit 14 proceeds to step S4 and performs a process of giving information on the reference direction Q to the control output calculation unit 15.

Therefore, when it is determined in step S3 that the resetting condition is not satisfied, the reference direction Q given from the reference direction setting unit 14 to the control output calculation unit 15 remains the same as before. On the other hand, when it is determined in step S3 that the resetting condition is satisfied, the reference direction setting unit 14 gives the control output calculation unit 15 again based on the disturbance estimation direction P at that time. It becomes the set (updated) reference direction Q.

After step S4, the reference direction setting unit 14 proceeds to step S6, determines whether or not the position holding of the underwater vehicle 1 is completed, and returns to step S2 if the position holding is not completed.

As a result, the reference direction setting unit 14 repeatedly executes the processing loops from step S2 to step S6 until the position holding of the underwater vehicle 1 is completed, and controls the reference direction Q each time the step S4 is reached. The processing given to the output calculation unit 15 is sequentially performed.

As described above, when the operator inputs the reference direction reset command G on the operation terminal 12 of the mother ship 10, the control device 9 of the underwater vehicle 1 receives the reference direction reset command G. The orientation setting unit 14 may perform the following processing in step 3.

In this case, in step S3, the reference direction setting unit 14 has a reset condition that the state in which the deviation ΔX in the left-right direction of the aircraft is equal to or higher than the set threshold value Xt continues for the set time or longer. If both of the resetting conditions that the reference direction resetting command G has been received are not satisfied, the process proceeds to step S4, and if at least one of the resetting conditions is satisfied, the process proceeds to step S5.

As a result, when the reference direction reset command G is input from the operator at the operation terminal 12 of the mother ship 10, the reference direction setting unit 14 sets the reference direction Q to the disturbance estimation direction at the time when the reference direction reset command G is received. The direction facing P is reset (updated), and the process of giving to the control output calculation unit 15 is performed.

The control output calculation unit 15 includes a target position F by a command, a position A from the position detecting means 6, an orientation B, an attitude C, a ground speed D from the Doppler speedometer 7, a water speed E, and a rudder 4. Upon receiving the rudder angle measurement value N, the rotation speed measurement values L, M, O from the thrusters 2, 3 and 5, and the reference direction Q from the reference direction setting unit 14, the function of performing the following calculation and command is performed. I have.

That is, the control output calculation unit 15 controls the direction B of the underwater vehicle 1 with the reference direction Q as the target direction, and obtains a control output for holding the position A of the underwater vehicle 1 at the target position F. It has a function to calculate the required number of rotations of each thruster 2, 3 and 5, and the steering angle of the rudder 4.

Further, the control output calculation unit 15 has a function of commanding the rotation speed command values H, I, K and the rudder angle command value J based on the calculation result to the corresponding thrusters 2, 3, 5 and the rudder 4, respectively. There is.

As a result, in the underwater vehicle 1, the operation of the main thruster 2 according to the rotation speed command value H, the operation of the horizontal thruster 3 according to the rotation speed command value I, the operation of the vertical thruster 5 according to the rotation speed command value K, and the rudder The rudder angle of the rudder 4 is controlled according to the angle command value J. Therefore, the underwater vehicle 1 can control the position A and the direction B of the aircraft so as to match the target position F and the reference direction Q.

When the position holding method is carried out using the underwater vehicle 1 of the present embodiment having the above configuration, the underwater vehicle 1 operates as shown in FIGS. 4 (a), (b), (c) and (d). Will come to do.

First, as shown in FIG. 4A, the underwater vehicle 1 starts holding the position in a state where the position A of the own machine is aligned with the target position F. At this time, the target direction for matching the direction B of the underwater vehicle 1 is the reference direction Q set according to the direction B at that time.

In this state, for example, when a disturbance 16 as shown by an arrow in FIG. 4A acts from a direction other than the direction facing the reference direction Q, the underwater vehicle 1 is affected by the disturbance 16 shown in FIG. 4A. As shown by the dotted line, the displacement from the target position F occurs.

At this time, due to the action of the components of the underwater navigation body 1 included in the disturbance 16 in the direction along the front-rear direction of the aircraft, the underwater navigation body 1 has a deviation ΔY with respect to the front-rear direction of the aircraft from the target position F. Further, due to the action of the components in the direction along the left-right direction of the underwater navigation body 1 included in the disturbance 16, the underwater navigation body 1 has a deviation ΔX with respect to the left-right direction of the aircraft from the target position F.

When the disturbance 16 acting on the underwater vehicle 1 is slight, the deviation ΔX and the deviation ΔY generated in the underwater vehicle 1 are also slight, as shown in FIG. 4A. In this case, since the deviation ΔX does not reach the threshold value Xt, the underwater vehicle 1 sets the position A of the underwater vehicle 1 to the target position F while holding the direction B of the aircraft at the reference direction Q. Move to return.

Next, as shown in FIG. 4B, even when the disturbance 16a becomes large, when the direction in which the disturbance 16a acts on the underwater vehicle 1 has a small deviation from the reference direction Q, The deviation ΔY in the front-rear direction of the aircraft from the target position F generated in the underwater vehicle 1 becomes large, but the deviation ΔX in the left-right direction of the aircraft becomes small enough not to reach the threshold value Xt.

By the way, in the underwater navigation body 1, the propulsion force in the front-rear direction of the airframe is stronger than the propulsion force in the left-right direction of the airframe, and in general, the airframe of the underwater airframe 1 has the resistance of water in the front-rear direction of the airframe. It is smaller than the resistance in the left-right direction. Therefore, as shown in FIG. 4B, the underwater vehicle 1 has a deviation ΔY against the disturbance 16a even if the deviation ΔY in the front-rear direction of the aircraft with respect to the target position F becomes large due to the influence of the disturbance 16a. It is easy to control the position of the aircraft in the front-rear direction so as to eliminate the problem.

Therefore, in this case, since the deviation ΔX has not reached the threshold value Xt, the underwater vehicle 1 targets the position A of the underwater vehicle 1 while holding the azimuth B of the aircraft at the reference direction Q. It moves so as to return to the position F.

Next, as shown in FIG. 4C, when the disturbance 16b becomes large and the deviation of the angle from the reference direction Q of the direction in which the disturbance 16b acts on the underwater vehicle 1 is large, the underwater navigation The deviation ΔX in the left-right direction of the aircraft from the target position F generated in the running body 1 becomes large, and the threshold value Xt is reached.

Specifically, when the deviation ΔX in the left-right direction of the aircraft and the deviation ΔY in the front-rear direction of the aircraft from the target position F occur due to the influence of the disturbance 16b on the underwater vehicle 1, FIGS. 4 (a) and 4 (b). ), The position control of the underwater vehicle 1 in the front-rear direction and the left-right direction of the underwater vehicle 1 for eliminating the deviation ΔY and also eliminating the deviation ΔX is started. However, in the underwater navigation body 1, the propulsion force in the left-right direction of the airframe is weaker than the propulsion force in the front-rear direction of the airframe, and in general, the airframe of the underwater airframe 1 has water resistance in the left-right direction of the airframe. Greater than anterior-posterior resistance. Therefore, the deviation ΔX in the left-right direction of the aircraft with respect to the target position F generated in the underwater vehicle 1 cannot be easily eliminated like the deviation ΔY in the front-rear direction of the aircraft.

Therefore, when the disturbance 16b is continuously acting, the state in which the deviation ΔX in the left-right direction of the airframe cannot be eliminated continues, so that the deviation ΔX accumulates and reaches the threshold value Xt.

In this case, if the state in which the deviation ΔX in the left-right direction of the aircraft is equal to or higher than the set threshold value Xt continues for the set time or longer, the underwater vehicle 1 is underwater by the disturbance estimation unit 13 of the control device 9. The direction of the disturbance 16b acting on the body 1 is estimated, and as shown in FIG. 4D, the reference direction Q is reset in the direction facing the disturbance estimation direction P obtained by the estimation.

As a result, the underwater vehicle 1 moves so that the position A of the underwater vehicle 1 returns to the target position F while adjusting the direction B of the aircraft with the reset reference direction Q as the target direction. ..

As a result, the underwater navigating body 1 holds the direction B of the aircraft in a state where the position A of the own machine is aligned with the target position F and faces the direction in which the disturbance 16b acts on the underwater navigating body 1. However, the position can be held. This state is the same as that in FIG. 4A, based on the direction B of the underwater vehicle 1.

After that, depending on the magnitude of the disturbances 16, 16a and 16b and the relative angles of the disturbances 16, 16a and 16b with respect to the reference direction Q, the underwater vehicle 1 is subjected to the above-mentioned FIGS. 4 (a) and 4 (b). ), By performing the same operations as in FIGS. 4 (c) and 4 (d), the position can be continuously held at the target position F.

As described above, according to the position holding method of the present embodiment and the underwater vehicle 1 of the present embodiment, the underwater vehicle can be used regardless of the size of the disturbances 16, 16a, 16b. It is possible to suppress large changes and frequent changes in the orientation B of the aircraft 1 in a short period of time, and the underwater navigation body 1 is made to hold the position while the orientation control of the underwater navigation body 1 is stabilized. be able to

In the present embodiment, paying attention to the magnitude of the deviation ΔX in the left-right direction of the aircraft from the target position F generated in the underwater navigation body 1, the reference direction Q as a reference for matching the direction B of the underwater navigation body 1 Since the resetting is performed, the position is maintained according to the conditions of the propulsive force generated by the main thruster 2, the horizontal thruster 3, and the steering 4 as actuators provided in the underwater vehicle 1 and the propulsive force for attitude control. Control for this can be performed efficiently.

[Second Embodiment]
5 and 6 show an example in which the moving body to be held in position is a ship as the second embodiment of the position holding method and the moving body.

FIG. 5 is a schematic view showing a ship used for carrying out the position holding method in the present embodiment. FIG. 6 is a schematic view showing the configuration of the control device in FIG.

In FIGS. 5 and 6, the same reference numerals as those shown in the first embodiment are designated by the same reference numerals, and the description thereof will be omitted.

The ship as a moving body of the present embodiment is shown by reference numeral 17 in FIG.

The ship 17 includes, as actuators, a main thruster 18 arranged at the rear part of the hull, a horizontal thruster 19 arranged at the front part of the hull, and a rudder 20 arranged at the rear part of the hull. As a result, the ship 17 has a propulsive force by the main thruster 18 in the front-rear direction of the hull, and a horizontal thruster 19 and a steering wheel 20 in the left-right direction of the hull make the ship 17 weaker than the propulsion force in the front-rear direction of the hull. It is supposed to have power and directional control propulsion.

The ship 17 is further configured to include a position detecting means 21, a speed measuring means 22, a control device 23, and an operation terminal 24.

The position detecting means 21 has a function of measuring the position A of the own ship of the ship 17, the direction B of the ship 17 facing the hull, and the attitude C of the hull. The speed measuring means 22 has a function of detecting the ground speed D and the water speed E as the speed of the ship 17.

As the position detecting means 21 and the speed measuring means 22, for example, a combination of a global navigation satellite system (GNSS) such as GPS and an inertial navigation system may be used. The position detecting means 21 and the speed measuring means 22 are adopted in an existing ship if information on the position A, the direction B and the attitude C, and the ground speed D and the water speed E for the ship 17 can be obtained. Of course, any position detecting means 21 and speed measuring means 22 may be used.

Information on the position A, the direction B, the attitude C, and the ground speed D and the water speed E about the ship 17 obtained by the position detecting means 21 and the speed measuring means 22 is given to the control device 23.

Similar to the operation terminal 12 of the first embodiment, the operation terminal 24 is for setting the target position F for which the position of the ship 17 is desired to be maintained and inputting the reference direction reset command G as necessary. .. The information of the target position F set by the operation terminal 24 and the reference direction reset command G are given to the control device 23.

The control device 23 has a function of giving a command (control input) to the main thruster 18, the horizontal thruster 19, and the rudder 20 for the ship 17 to hold the position at the target position F.

Specifically, the control device 23 has a function of giving a rotation speed command value H to the main thruster 18, a rotation speed command value I to the horizontal thruster 19, and a rudder angle command value J to the rudder 20.

On the other hand, as feedback of the operation status to the control device 23, the main thruster 18 has a function of giving the rotation speed measurement value L to the control device 23. Further, the horizontal thruster 19 has a function of giving the rotation speed measurement value M to the control device 23. The rudder 20 has a function of giving a rudder angle measurement value N to the control device 23.

Further, as shown in FIG. 6, the control device 23 is configured to include a disturbance estimation unit 13a, a reference direction setting unit 14a, and a control output calculation unit 15a by a software module.

In the disturbance estimation unit 13a, the control device 23 includes the direction B from the position detecting means 21, the ground speed D from the speed measuring means 22, the water speed E, the rudder angle measured value N from the rudder 20, and each thruster 18. When the rotation speed measurement values L and M from, 19 are received, the propulsive force and the azimuth control propulsive force acting on the hull by the thrusters 18, 19 and the rudder 20, and the actual azimuth B and ground velocity of the ship 17 It has a function to estimate the disturbance estimation direction P based on the difference between D and the water velocity E. The information of the disturbance estimation direction P obtained by the disturbance estimation unit 13a is given to the reference direction setting unit 14a.

The reference direction setting unit 14a receives the target position F and the reference direction reset command G input from the operation terminal 24, the information on the position A and the direction B of the ship 17 given by the position detecting means 21, and the disturbance estimation unit 13a. Upon receiving the information of the given disturbance estimation direction P, it has a function of determining the reference direction Q by performing the process according to the flow chart shown in FIG. 3 as in the reference direction setting unit 14 in the first embodiment. .. In the present embodiment, in the description of the process of FIG. 3 performed in the first embodiment, the underwater vehicle 1 may be replaced with the ship 17.

The control output calculation unit 15a has a target position F, a position A from the position detecting means 21, an orientation B, an attitude C, a ground speed D from the speed measuring means 22, a water speed E, and a rudder angle from the rudder 20. When the measured value N, the rotation speed measured values L and M from the thrusters 18 and 19, and the reference direction Q from the reference direction setting unit 14a are received, the same calculation as the control output calculation unit 15 of the first embodiment is performed. It has a function of commanding the rotation speed command values H and I and the rudder angle command value J based on the calculation result to the corresponding thrusters 18, 19 and the rudder 20, respectively.

As a result, in the ship 17, the operation of the main thruster 18 according to the rotation speed command value H, the operation of the horizontal thruster 19 according to the rotation speed command value I, and the rudder angle control of the rudder 20 according to the steering angle command value J are performed. Therefore, the ship 17 can control the position A and the direction B of the hull so as to match the target position F and the reference direction Q.

According to the ship 17 of the present embodiment having the above configuration, the same position holding method as that of the first embodiment can be carried out, and the same effect as that of the first embodiment can be obtained.

The present invention is not limited to each of the above embodiments.

For example, as an actuator provided on a moving body, the underwater vehicle 1 of the first embodiment exemplifies the main thruster 2, the horizontal thruster 3, the rudder 4, and the vertical thruster 5, and the ship 17 of the second embodiment. The main thruster 18, the horizontal thruster 19, and the rudder 20 are illustrated. On the other hand, if the moving body has a propulsive force in the front-rear direction of the aircraft and a weak propulsion force or a directional control propulsion force in the left-right direction of the aircraft as compared with the propulsion force in the front-rear direction of the aircraft, it is underwater. It may be configured to include an actuator of any type conventionally adopted or conventionally proposed for a moving body moving on or on water. For example, horizontal thrusters may be provided at the front and rear of the moving body. Further, a pair of thrusters capable of individually controlling the generated propulsive force may be provided on both the left and right sides of the center line of the machine body.

The disturbance estimation unit 13 in the first embodiment has a function of estimating the disturbance estimation direction P based on the difference between the ground speed E and the ground speed D of the underwater hull 1, and the disturbance in the second embodiment. The estimation unit 13a is the difference between the propulsive force and the directional control propulsive force acting on the hull by the main thruster 18, the horizontal thruster 19 and the rudder 20, and the actual directional B, the ground speed D, and the water speed E of the ship 17. Based on the above, it has been described as having a function of estimating the disturbance estimation direction P. On the other hand, in the second embodiment, the same disturbance estimation method as in the first embodiment may be adopted. Further, in the first embodiment, the same disturbance estimation method as in the second embodiment may be adopted. Further, for example, a method of estimating from the position and orientation of the moving body and the operating condition of the actuator, a method of estimating from the speed and orientation of the moving body and the operating condition of the actuator, and a method of estimating from the position deviation of the moving body while holding the position. Estimate by combining the method of estimating, the method of estimating from the speed and acceleration of the moving body's own machine and the propulsive force generated by the actuator, and the speed and orientation information of the moving body's own machine and other moving bodies. It goes without saying that the disturbance estimation method that has been conventionally used or has been proposed may be adopted for the method to be used and other moving objects in water or on water.

In the first embodiment, acoustic communication devices 8 and 11 are exemplified as communication devices for communicating between the underwater vehicle 1 and the mother ship 10, but wireless communication by radio waves or wired communication via an optical fiber or the like is used. A communication device by communication or optical communication may be used.

In the first embodiment, the underwater navigation body 1 is illustrated as the moving body, but the moving body may be a manned submersible or a submarine. In this case, the operation terminal 12 may be provided in the mobile body.

In the second embodiment, the ship 17 is illustrated as the moving body, and the ship 17 is described as having an operation terminal operated by the operator. However, as the moving body moving on the water, ASV (Autonomous Surface Vehicle) or USV It may be a self-propelled surface vehicle called (Unmanned Surface Vehicle). In this case, similarly to the mother ship 10 of the first embodiment, the mother ship 10 that gives the target position F and the reference direction reset command G to the surface navigation body by communication may be operated together.

The shape and size of the moving body may be arbitrary as long as it is a moving body that moves underwater or on water.

Of course, various changes can be made without departing from the gist of the present invention.

1 Underwater vehicle (moving body), 2 Main thruster (actuator), 3 Horizontal thruster (actuator), 4 Rudder (actuator), 6 Position detection means (means), 9 Control device, 16, 16a, 16b Disturbance, 17 Ship (moving body), 21 position detection means, 23 control device, A position, B direction, F target position, P disturbance estimation direction (direction on which disturbance acts), Q reference direction, ΔX deviation, Xt threshold

Claims (3)

A control device that gives control output to the actuator that generates the propulsive force in the front-rear direction of the moving body moving underwater or on the water and the directional control propulsive force in the left-right direction of the machine.
The process of setting the target position and the initial value of the reference direction,
The process of acquiring the position information of the moving body's own machine and
The process of acquiring information on the orientation of the moving body and
The process of estimating the direction in which the disturbance acts on the moving body's own machine, and
Whether or not the resetting condition that the deviation of the moving body's own machine from the target position in the left-right direction of the machine continues to be equal to or higher than the set threshold value for the set time or longer is satisfied. And the process of determining
When it is determined that the resetting condition is not satisfied, the reference direction set at that time is set as the target direction, the direction in which the aircraft faces is adjusted to the target direction, and the position of the own machine is set as the target position. The process of giving the control output to the actuator to hold it in
When it is determined that the resetting condition is satisfied, the reference direction is reset to the direction opposite to the direction at which the disturbance acts on the own aircraft estimated at that time, and then the reset reference is set. A position characterized by performing a process of adjusting the direction in which the aircraft faces to the target direction and giving a control output to the actuator to hold the position of the own machine at the target position, with the direction as the target direction. Holding method. The position holding method according to claim 1, wherein an underwater vehicle is used as the moving body. A mobile body that moves underwater or on water
A means for acquiring information on the position of the moving body's own machine,
A means for acquiring information on the orientation of the moving body and
An actuator that generates propulsive force in the front-rear direction of the moving body and directional control propulsive force in the left-right direction of the machine.
Equipped with a control device
The control device is
The function to set the target position and the initial value of the reference direction,
The function of estimating the direction in which the disturbance acts on the moving body's own machine, and
Whether or not the resetting condition that the deviation of the moving body's own machine from the target position in the left-right direction of the machine continues to be equal to or higher than the set threshold value for the set time or longer is satisfied. And the function to judge
If it is determined in the determination process that the resetting condition is not satisfied, the reference direction set at that time is set as the target direction, the direction in which the aircraft faces is adjusted to the target direction, and the own machine is used. A function of performing a process of giving a control output for holding the position to the target position to the actuator, and
If it is determined by the determination process that the resetting condition is satisfied, the reference direction is reset to the direction opposite to the direction at which the disturbance acts on the own aircraft estimated at that time, and then the reference direction is reset. With the reset reference direction as the target direction, the direction in which the aircraft faces is adjusted to the target direction, and a process of giving a control output for holding the position of the own machine at the target position to the actuator is performed. A moving body characterized in that it comprises.

Images