Fixed-point cruise control method and equipment for double-propeller unmanned ship and storage medium

Abstract

The invention discloses a fixed-point cruise control method for a double-propeller unmanned ship, and the method comprises the following steps: obtaining a current position, a yaw angle and an expected position of the unmanned ship, enabling the unmanned ship to join a new expected position in a queue form, and removing the first expected position when the unmanned ship joins each time; calculating a distance error and an expected course according to the current position and the expected position, and calculating a course error according to the current unmanned ship course and the expected course; and enabling the unmanned ship to execute a pivot steering mode or a track tracking module based on a comparison result of the course error and a preset threshold value. According to the invention, the applicability of the unmanned ship to the environment can be improved, the fusion use of the unmanned ship to the path planning algorithm is increased, and the cruising efficiency under the condition of close target points is remarkably improved.

Description

[0001] Fixed-point cruise control method, device and storage medium for dual-thruster unmanned ship Technical Field

[0002] The present invention relates to the field of Internet technology, and in particular to a fixed-point cruise control method, device and storage medium for a dual-thruster unmanned ship. Background Art

[0003] Among today's unmanned ships, dual-thruster unmanned ships provide a smaller turning radius based on differential steering control, making the unmanned ships highly maneuverable. The relatively simple mechanical analysis makes the control method more stable, and it is widely used in education, environmental monitoring, patrol and other fields. The fixed-point cruising of dual-thruster unmanned ships is a prerequisite for autonomous navigation of unmanned ships, but the current control method only cruises at target points. The target points are often sparse in the cruising area, and the distance between two consecutive points is relatively far. Usually the path is a straight line or a simple curve. The control system requires the stability of parameters such as speed and heading, and is generally only used in simple known waters. However, in order to achieve complex waters, path planning algorithms are often used. In order to achieve obstacle avoidance, the distance between consecutive target points is close, and the speed and yaw change frequently, making it difficult to integrate the path planning algorithm.

[0004] To solve the above problems, a fixed-point cruise control method for a dual-thruster unmanned ship is designed to improve the stability of the unmanned ship when the speed and yaw change frequently in the case of closely continuous target points, so that the unmanned ship is consistent with the navigation trajectory. Summary of the invention

[0005] The main purpose of the present invention is to provide a fixed-point cruise control method, device and storage medium for a dual-thruster unmanned ship, aiming to solve the above-mentioned technical problems.

[0006] To achieve the above object, the present invention provides a fixed-point cruise control method for a dual-thruster unmanned ship.

[0007] The fixed-point cruise control method for a dual-thruster unmanned ship comprises the following steps: Get the current position, yaw angle and expected position of the unmanned ship. A new expected position can be added in the form of a queue. Each time a new expected position is added, the first expected position that enters is removed. Calculate the distance error and the expected heading according to the current position and the expected position, and calculate the heading error according to the current unmanned ship heading and the expected heading; Based on the comparison result of the heading error and the preset threshold, the unmanned ship is enabled to execute an in-situ turning mode or a track tracking module.

[0008] In one embodiment, the step of calculating the distance error and the expected heading according to the current position and the expected position includes: Calculate the distance error and expected heading according to the following formula; ; ; Calculate the distance error between the second expected position in the queue and the first expected position and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error ; in, and is the location of the unmanned ship, and is the expected target location.

[0009] In one embodiment, the step of causing the unmanned ship to execute an in-situ turning mode or a track tracking module based on a comparison result of the heading error and a preset threshold comprises: If the heading error is greater than a preset threshold, the unmanned ship is made to perform an on-site turning mode; If the heading error is less than the preset threshold, the unmanned ship will execute the track tracking mode.

[0010] In one embodiment, if the heading error is greater than a preset threshold, the step of causing the unmanned ship to execute an in-situ turning mode comprises: The local navigation coordinate system is set according to the current carrier coordinate system. The axes of the local navigation coordinate system correspond to the carrier coordinate system, and the origin is the center of the carrier. The positive direction of the Y axis is the direction of the head of the unmanned ship, and the positive direction of the X axis is the right side with the bow in front. If the expected position is in the positive direction of the X-axis of the local navigation coordinate system, turn right; if it is in the negative direction of the X-axis, turn left; Then, according to whether it is in the positive direction of the Y axis, the proportional coefficient of the left or right motors in left or right turn is reduced or increased by the following formula; Turn left: ; ; Turn right: ; ; Among them, k1 and k2 are the proportional variable coefficients of their respective motors.

[0011] In one embodiment, if the heading error is less than a preset threshold, the step of causing the unmanned ship to execute a track tracking mode includes: According to the yaw error Define the variable T as follows, ; Wherein, c is the proportionality coefficient, c>0; When the unmanned boat is in the state of stopping, accelerating, stabilizing, or decelerating, the speed of the unmanned boat is adjusted according to the variable T.

[0012] In one embodiment, when the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: When the boat is stopped, the speed is adjusted according to the following formula: ; .

[0013] In one embodiment, when the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: In the acceleration state, set the angle thresholds of the first and second expected positions , the angle threshold between the second and third expected positions ; If d1 is greater than the set threshold, accelerate using the following formula, otherwise skip this step; like Less than , then d2 replaces d1 to compare with the set threshold to determine whether to accelerate; like Less than , then d3 replaces d2 to compare with the set threshold to determine whether to accelerate; ; ; Among them, k>0, and the k value increases linearly with the acceleration state time.

[0014] In one embodiment, when the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: If the distance error d1 is less than the threshold of the deceleration state, the deceleration state is entered according to the following formula; like Less than threshold , then whether to decelerate is determined by comparing d2 instead of d1 with the set threshold; like Less than threshold , then whether to decelerate is determined by comparing d3 instead of d2 with the set threshold; ; ; Among them, b is the proportional coefficient and b>0, and V is the current speed of the unmanned ship.

[0015] In addition, to achieve the above-mentioned purpose, the present invention also provides a fixed-point cruise control device for a twin-thruster unmanned ship, and the fixed-point cruise control device for a twin-thruster unmanned ship includes: a memory, a processor, and a fixed-point cruise control program stored on the memory and executable on the processor, and when the fixed-point cruise control program is executed by the processor, the steps of the fixed-point cruise control method for a twin-thruster unmanned ship as described above are implemented.

[0016] In addition, to achieve the above-mentioned purpose, the present invention also provides a computer-readable storage medium, on which a fixed-point cruise control program is stored, and when the fixed-point cruise control program is executed by a processor, the steps of the fixed-point cruise control method for a dual-thruster unmanned ship as described above are implemented. Beneficial effects that can be achieved by the present invention: A fixed-point cruise control method for a dual-thruster unmanned ship proposed in an embodiment of the present invention can improve the applicability of the unmanned ship to the environment and increase the integrated use of the unmanned ship to the path planning algorithm, thereby significantly improving the cruising efficiency in the case of close target points. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a schematic diagram of the structure of a device in a hardware operating environment involved in an embodiment of the present invention; Figure 2 It is a flow chart of a fixed-point cruise control method for a dual-thruster unmanned ship according to the present invention; Figure 3 It is a flow chart of the in-situ steering mode of the fixed-point cruise control method for a dual-thruster unmanned ship of the present invention; Figure 4 The present invention is a flow chart of a track tracking mode of a fixed-point cruise control method for a dual-thruster unmanned ship.

[0018] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0019] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0020] like Figure 1 As shown, Figure 1It is a schematic diagram of the terminal structure of the hardware operating environment involved in the embodiment of the present invention.

[0021] The terminal of the embodiment of the present invention can be a PC, or it can be a smart phone, a tablet computer, an e-book reader, an MP3 (Moving Picture Experts Group Audio Layer III) player, an MP4 (Moving Picture Experts Group Audio Layer IV) player, a portable computer, or other portable terminal devices with display function.

[0022] like Figure 1 As shown, the terminal may include: a processor 1001, such as a CPU, a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. Among them, the communication bus 1002 is used to realize the connection and communication between these components. The user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a WI-FI interface). The memory 1005 may be a high-speed RAM memory, or a stable memory (non-volatile memory), such as a disk memory. The memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0023] Optionally, the terminal may also include a camera, an RF (Radio Frequency) circuit, a sensor, an audio circuit, a WiFi module, and the like. Among them, sensors include light sensors, motion sensors, and other sensors. Specifically, the light sensor may include an ambient light sensor and a proximity sensor, wherein the ambient light sensor may adjust the brightness of the display screen according to the brightness of the ambient light, and the proximity sensor may turn off the display screen and / or backlight when the mobile terminal is moved to the ear. As a type of motion sensor, the gravity acceleration sensor can detect the magnitude of acceleration in each direction (generally three axes), and can detect the magnitude and direction of gravity when stationary. It can be used for applications that identify the posture of the mobile terminal (such as horizontal and vertical screen switching, related games, magnetometer posture calibration), vibration recognition related functions (such as pedometer, tapping), etc.; of course, the mobile terminal can also be equipped with other sensors such as gyroscopes, barometers, hygrometers, thermometers, infrared sensors, etc., which will not be repeated here.

[0024] Those skilled in the art will understand that Figure 1The terminal structure shown in the figure does not constitute a limitation on the terminal, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0025] like Figure 1 As shown, the memory 1005 as a computer storage medium may include an operating system, a network communication module, a user interface module, and a fixed-point cruise control program.

[0026] exist Figure 1 In the terminal shown, the network interface 1004 is mainly used to connect to the backend server and perform data communication with the backend server; the user interface 1003 is mainly used to connect to the client (user end) and perform data communication with the client; and the processor 1001 can be used to call the fixed-point cruise control program stored in the memory 1005 and perform the following operations: Get the current position, yaw angle and expected position of the unmanned ship. A new expected position can be added in the form of a queue. Each time a new expected position is added, the first expected position that enters is removed. Calculate the distance error and the expected heading according to the current position and the expected position, and calculate the heading error according to the current unmanned ship heading and the expected heading; Based on the comparison result of the heading error and the preset threshold, the unmanned ship is enabled to execute an in-situ turning mode or a track tracking module.

[0027] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: Calculate the distance error and expected heading according to the following formula; ; ; Calculate the distance error between the second expected position in the queue and the first expected position and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error ; in, and is the location of the unmanned ship, and is the expected target location.

[0028] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: If the heading error is greater than a preset threshold, the unmanned ship is made to perform an on-site turning mode; If the heading error is less than the preset threshold, the unmanned ship will execute the track tracking mode.

[0029] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: The local navigation coordinate system is set according to the current carrier coordinate system. The axes of the local navigation coordinate system correspond to the carrier coordinate system, and the origin is the center of the carrier. The positive direction of the Y axis is the direction of the head of the unmanned ship, and the positive direction of the X axis is the right side with the bow in front. If the expected position is in the positive direction of the X-axis of the local navigation coordinate system, turn right; if it is in the negative direction of the X-axis, turn left; Then, according to whether it is in the positive direction of the Y axis, the proportional coefficient of the left or right motors in left or right turn is reduced or increased by the following formula; Turn left: ; ; Turn right: ; ; Among them, k1 and k2 are the proportional variable coefficients of their respective motors.

[0030] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: According to the yaw error Define the variable T as follows, ; Wherein, c is the proportionality coefficient, c>0; When the unmanned boat is in the state of stopping, accelerating, stabilizing, or decelerating, the speed of the unmanned boat is adjusted according to the variable T.

[0031] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: When the boat is stopped, the speed is adjusted according to the following formula: ; .

[0032] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: In the acceleration state, set the angle thresholds of the first and second expected positions , the angle threshold between the second and third expected positions ; If d1 is greater than the set threshold, accelerate using the following formula, otherwise skip this step; like Less than , then d2 replaces d1 to compare with the set threshold to determine whether to accelerate; like Less than , then d3 replaces d2 to compare with the set threshold to determine whether to accelerate; ; ; Among them, k>0, and the k value increases linearly with the acceleration state time.

[0033] Further, the processor 1001 may call the fixed-point cruise control program stored in the memory 1005, and further perform the following operations: If the distance error d1 is less than the threshold of the deceleration state, the deceleration state is entered according to the following formula; like Less than threshold , then whether to decelerate is determined by comparing d2 instead of d1 with the set threshold; like Less than threshold , then whether to decelerate is determined by comparing d3 instead of d2 with the set threshold; ; ; Among them, b is the proportional coefficient and b>0, and V is the current speed of the unmanned ship.

[0034] The specific embodiments of the fixed-point cruise control device for a twin-thruster unmanned ship of the present invention are basically the same as the embodiments of the fixed-point cruise control method for a twin-thruster unmanned ship described below, and will not be described in detail here.

[0035] Reference Figure 2 The first embodiment of the present invention provides a fixed-point cruise control method for a dual-thruster unmanned ship, the fixed-point cruise control method for a dual-thruster unmanned ship comprising: Get the current position, yaw angle and expected position of the unmanned ship. A new expected position can be added in the form of a queue. Each time a new expected position is added, the first expected position that enters is removed. Calculate the distance error and the expected heading according to the current position and the expected position, and calculate the heading error according to the current unmanned ship heading and the expected heading; Based on the comparison result of the heading error and the preset threshold, the unmanned ship is enabled to execute an in-situ turning mode or a track tracking module.

[0036] In this embodiment, by adding the in-situ turning mode, the position relationship between the current position and the initial position is obtained based on the sensor data, and the thrust of the left and right propellers of the unmanned ship is adjusted to ensure a smaller turning radius in the case of a large turn. This can improve the applicability of the unmanned ship to the environment and increase the integration of the unmanned ship with the path planning algorithm, significantly improving the cruising efficiency in the case of close target points.

[0037] Further, the step of calculating the distance error and the expected heading according to the current position and the expected position includes: Calculate the distance error and expected heading according to the following formula; ; ; Calculate the distance error between the second expected position in the queue and the first expected position and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error ; in, and is the position of the unmanned ship, and is the expected target location.

[0038] Depending on whether the heading error is greater than the threshold, different modes are executed. If it is greater than the threshold, it enters the in-place steering mode, and if it is less than the threshold, it enters the track tracking mode. The in-place steering mode feeds back the current yaw and updates it, and the track tracking mode feeds back whether the target radius has been reached.

[0039] In the track tracking mode, if the expected target is reached, a new expected position is added in the form of a queue. If there is no new expected target, the previous expected position is added. Each time it is added, the first expected position is removed. The basis for judging whether the expected target point exists is that the three positions in the queue are the same. If all are the same, the program ends. If not, the program returns to the information reading step.

[0040] In this embodiment, by adding a queue with a length of 3, all target positions are added to the queue in turn, and the speed is controlled based on the distance between the three positions and the heading deviation to achieve efficient cruising, and it is more suitable for the path planning algorithm that outputs position information tightly.

[0041] Before obtaining the position of the unmanned ship, all expected two-dimensional positions, body two-dimensional positions, and body yaw information can be initialized. Input the information of three expected consecutive positions. If there is only one expected position, the three position information are set to the same.

[0042] Please refer to Figure 3 If the heading error is greater than the preset threshold, the unmanned ship will execute the in-situ turning mode. This mode sets the local navigation coordinate system according to the current carrier coordinate system. Each axis corresponds to the carrier coordinate system. The in-situ is the center of the carrier, where the positive direction of the Y axis is the direction of the head of the unmanned ship, and the positive direction of the X axis is the right side with the bow in front. If the target point is in the positive direction of the X axis of the local navigation coordinate system, turn right to reduce the rotation angle. Similarly, if it is in the negative direction of the X axis, turn right, and the left and right turns need to output the final input motor PWM value. Then, depending on whether it is in the positive direction of the Y axis, the proportional coefficient of the left and right motors in the left or right turn calculation can be reduced or increased respectively.

[0043] The final calculation formula for the PWM value input to each motor is: Turn left: ; ; Turn right: ; ; Among them, k1 and k2 are the proportional variable coefficients of their respective motors.

[0044] Please refer to Figure 4 ,If the heading error is less than the preset threshold, the unmanned ship will execute the track tracking mode.

[0045] First, calculate the distance error between the second expected position and the first expected position of the queue and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error .

[0046] Set a variable T based on the yaw error from the first expected position Correct the unmanned ship's course. If it is positive, it is considered that the unmanned ship is heading to the left, then T is positive. is negative, the unmanned ship is heading to the right, and T is negative.

[0047] T value size based on The absolute value and T value output formula are as follows: Where c is the proportional coefficient, c>0.

[0048] In this mode, the speed is divided into 4 steps: stop, accelerate, stabilize, and decelerate. The specific process of speed update is as follows: The initial state is set to the stopped state, and the heading correction variable T is added to the motor PWM output formula, as shown in the following formula:

[0049]

[0050] Set the angle threshold between the first and second expected positions , the angle threshold between the second and third expected positions , the distance error d1 is used as the judgment of whether to accelerate. If d1 is greater than the set threshold, acceleration can be performed. Otherwise, this step is skipped. Less than , then d2 replaces d1 to compare with the set threshold to determine whether to accelerate; like Less than , then d3 replaces d2 to compare with the set threshold to determine whether to accelerate. The PWM output formula of the acceleration state is as follows.

[0051]

[0052]

[0053] Among them, k>0, and the k value increases linearly with the acceleration state time.

[0054] The acceleration state ends or the acceleration reaches the maximum speed When it enters the stable state, the stable state will control the k value in the acceleration state according to the speed information fed back by the sensor. If the speed is greater than the speed at the end of acceleration, k will be reduced, otherwise, k will be increased.

[0055] If the distance error d1 is less than the threshold of the deceleration state, the deceleration state is entered according to the following formula; like Less than threshold , then whether to decelerate is determined by comparing d2 instead of d1 with the set threshold; like Less than threshold , then whether to decelerate is determined by comparing d3 with the set threshold instead of d2. When decelerating, the k value of step ② decreases with time, and a variable is added to prevent the situation where the speed is too fast and the deceleration is insufficient. The specific formula is as follows

[0056] Among them, b>0, b is the proportional coefficient, and V is the current speed of the unmanned ship.

[0057] In this application, multiple adjustable parameters are more flexible to apply to different unmanned ships, and the thresholds of each process can be adjusted according to different environments and needs. In addition, this modular mode switching is more convenient for the optimization of later methods and easier to enter the debugging of unmanned ship cruising.

[0058] In addition, an embodiment of the present invention further provides a computer-readable storage medium, on which a fixed-point cruise control program is stored. When the fixed-point cruise control program is executed by a processor, the following operations are implemented: Get the current position, yaw angle and expected position of the unmanned ship. A new expected position can be added in the form of a queue. Each time a new expected position is added, the first expected position that enters is removed. Calculate the distance error and the expected heading according to the current position and the expected position, and calculate the heading error according to the current unmanned ship heading and the expected heading; Based on the comparison result of the heading error and the preset threshold, the unmanned ship is enabled to execute an in-situ turning mode or a track tracking module.

[0059] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: Calculate the distance error and expected heading according to the following formula; ; ; Calculate the distance error between the second expected position in the queue and the first expected position and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error ; in, and is the position of the unmanned ship, and is the expected target location.

[0060] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: If the heading error is greater than a preset threshold, the unmanned ship is made to perform an on-site turning mode; If the heading error is less than the preset threshold, the unmanned ship will execute the track tracking mode.

[0061] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: The local navigation coordinate system is set according to the current carrier coordinate system. The axes of the local navigation coordinate system correspond to the carrier coordinate system, and the origin is the center of the carrier. The positive direction of the Y axis is the direction of the head of the unmanned ship, and the positive direction of the X axis is the right side with the bow in front. If the expected position is in the positive direction of the X-axis of the local navigation coordinate system, turn right; if it is in the negative direction of the X-axis, turn left; Then, according to whether it is in the positive direction of the Y axis, the proportional coefficient of the left or right motors in left or right turn is reduced or increased by the following formula; Turn left: ; ; Turn right: ; ; Among them, k1 and k2 are the proportional variable coefficients of their respective motors.

[0062] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: According to the yaw error Define the variable T as follows: ; Wherein, c is the proportionality coefficient, c>0; When the unmanned boat is in the state of stopping, accelerating, stabilizing, or decelerating, the speed of the unmanned boat is adjusted according to the variable T.

[0063] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: When the boat is stopped, the speed is adjusted according to the following formula: ; .

[0064] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: In the acceleration state, set the angle thresholds of the first and second expected positions , the angle threshold between the second and third expected positions ; If d1 is greater than the set threshold, accelerate using the following formula, otherwise skip this step; like Less than , then d2 replaces d1 to compare with the set threshold to determine whether to accelerate; like Less than , then d3 replaces d2 to compare with the set threshold to determine whether to accelerate; ; ; Among them, k>0, and the k value increases linearly with the acceleration state time.

[0065] Furthermore, when the fixed-point cruise control program is executed by the processor, the following operations are also implemented: If the distance error d1 is less than the threshold of the deceleration state, the deceleration state is entered according to the following formula; like Less than threshold , then whether to decelerate is determined by comparing d2 instead of d1 with the set threshold; like Less than threshold , then whether to decelerate is determined by comparing d3 instead of d2 with the set threshold; ; ; Among them, b is the proportional coefficient and b>0, and V is the current speed of the unmanned ship.

[0066] The specific embodiments of the computer-readable storage medium of the present invention are basically the same as the embodiments of the fixed-point cruise control method for a dual-thruster unmanned ship described above, and will not be described in detail here.

[0067] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0068] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0069] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for a terminal device (which can be a mobile phone, computer, server, unmanned ship, or network equipment, etc.) to execute the methods described in each embodiment of the present invention.

[0070] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A fixed-point cruise control method for a dual-thruster unmanned ship, characterized in that: The fixed-point cruise control method for a dual-thruster unmanned ship comprises the following steps: Get the current position, yaw angle and expected position of the unmanned ship. A new expected position can be added in the form of a queue. Each time a new expected position is added, the first expected position that enters is removed. Calculate the distance error and the expected heading according to the current position and the expected position, and calculate the heading error according to the current unmanned ship heading and the expected heading; Based on the comparison result of the heading error and the preset threshold, the unmanned ship is enabled to execute an in-situ turning mode or a track tracking module.

2. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 1, characterized in that: The step of calculating the distance error and the expected heading according to the current position and the expected position comprises: Calculate the distance error and expected heading according to the following formula; ； ； Calculate the distance error between the second expected position in the queue and the first expected position and yaw error And calculate the distance error between the last expected position and the second expected position in the queue and set yaw error ; in, and is the location of the unmanned ship, and is the expected target location.

3. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 2, characterized in that: The step of causing the unmanned ship to execute the in-situ steering mode or the track tracking module based on the comparison result of the heading error and the preset threshold comprises: If the heading error is greater than a preset threshold, the unmanned ship is made to perform an on-site turning mode; If the heading error is less than the preset threshold, the unmanned ship will execute the track tracking mode.

4. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 3, characterized in that: If the heading error is greater than a preset threshold, the step of causing the unmanned ship to execute the in-situ turning mode includes: The local navigation coordinate system is set according to the current carrier coordinate system. The axes of the local navigation coordinate system correspond to the carrier coordinate system, and the origin is the center of the carrier. The positive direction of the Y axis is the direction of the head of the unmanned ship, and the positive direction of the X axis is the right side with the bow in front. If the expected position is in the positive direction of the X-axis of the local navigation coordinate system, turn right; if it is in the negative direction of the X-axis, turn left; Then, according to whether it is in the positive direction of the Y axis, the proportional coefficient of the left or right motors in left or right turn is reduced or increased by the following formula; Turn left: ； ； Turn right: ； ； Among them, k1 and k2 are the proportional variable coefficients of their respective motors.

5. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 4, characterized in that: If the heading error is less than a preset threshold, the step of making the unmanned ship execute the track tracking mode includes: According to the yaw error Define the variable T as follows, ; Wherein, c is the proportionality coefficient, c>0; When the unmanned boat is in the state of stopping, accelerating, stabilizing, or decelerating, the speed of the unmanned boat is adjusted according to the variable T.

6. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 5, characterized in that: When the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: When the boat is stopped, the speed is adjusted according to the following formula: ； 。 7. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 6, characterized in that: When the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: In the acceleration state, set the angle thresholds of the first and second expected positions , the angle threshold between the second and third expected positions ; If d1 is greater than the set threshold, accelerate using the following formula, otherwise skip this step; like Less than , then d2 replaces d1 to compare with the set threshold to determine whether to accelerate; like Less than , then d3 replaces d2 to compare with the set threshold to determine whether to accelerate; ； ； Among them, k>0, and the k value increases linearly with the acceleration state time.

8. The fixed-point cruise control method for a dual-thruster unmanned ship according to claim 7, characterized in that: When the unmanned ship is in a stopped, accelerated, stable, or decelerated state, the step of adjusting the speed of the unmanned ship according to the variable T comprises: If the distance error d1 is less than the threshold of the deceleration state, the deceleration state is entered according to the following formula; like Less than threshold , then whether to decelerate is determined by comparing d2 instead of d1 with the set threshold; like Less than threshold , then whether to decelerate is determined by comparing d3 instead of d2 with the set threshold; ； ； Among them, b is the proportional coefficient and b>0, and V is the current speed of the unmanned ship.

9. A fixed-point cruise control device for a dual-thruster unmanned ship, characterized in that: The fixed-point cruise control device for a twin-thruster unmanned ship comprises: a memory, a processor, and a fixed-point cruise control program stored in the memory and executable on the processor. When the fixed-point cruise control program is executed by the processor, the steps of the fixed-point cruise control method for a twin-thruster unmanned ship as described in any one of claims 1 to 6 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a fixed-point cruise control program, which, when executed by a processor, implements the steps of the fixed-point cruise control method for a twin-thruster unmanned vessel as described in any one of claims 1 to 6.

Images