An unmanned ship trajectory tracking method, a trajectory tracking control system and a use method

Abstract

The application discloses an unmanned ship trajectory tracking method, a trajectory tracking control system and a use method, and comprises the following steps: S1, calculating a bow angle error θ e ; S2, judging the error range of θ e and judging which guidance algorithm is used; S3, calculating the current position of the ship and judging whether the target point is tracked; S4, if the target point is not tracked, repeating S1-S3, and if the target point is tracked, switching to the next expected target point; S5, judging whether the above target point is the end point, if yes, ending the algorithm, and if no, repeating S1-S4. The application combines the use of the LOS algorithm and the L1 algorithm (referred to as LL) to fully exert the respective advantages, solve more complex and diversified path tracking problems, enable the unmanned system to accurately move according to the predetermined path, improve the operation efficiency, and greatly enhance the safety; the unmanned ship is controlled through the novel trajectory tracking control method, so that the accuracy and precision of tracking are improved in the navigation process, the adaptability of the unmanned ship to complex environments, the response speed to unexpected situations and the ability to maintain path precision and stability are improved.

Description

Technical Field

[0001] This invention relates to a trajectory tracking control system and a trajectory tracking method, specifically an unmanned vessel trajectory tracking method, a trajectory tracking control system, and a method of using them. Background Technology

[0002] The research and development of trajectory tracking algorithms has received widespread attention because they are crucial for achieving high-precision and high-reliability operation of various unmanned mobile systems. An effective trajectory tracking algorithm ensures that ships can move accurately and reliably along a predetermined path, and can quickly adjust their path even when encountering unexpected obstacles or sudden changes in environmental conditions. Efficient trajectory tracking algorithms not only reduce human error but also provide more reliable automated responses in emergency situations, thereby ensuring the safety of ships and crew, while improving the efficiency of port operations.

[0003] The accuracy of unmanned surface vessel (USV) trajectory tracking largely depends on the guidance algorithm, which calculates the target's turning angle and uses it as input to the heading control system. In guidance systems, the Line-of-Sight (LOS) algorithm is commonly used for straight-line tracking, especially in unobstructed, flat terrain. The LOS algorithm can find the global optimum and generate a smooth path, thus performing well in trajectory planning problems involving straight lines or near-straight lines. The L1 algorithm (L1 guidance algorithm) is more flexible in curve tracking problems. Its feature selection capability allows for the selection of appropriate features in a high-dimensional space, better adapting to the shape of the curve.

[0004] During the movement of an unmanned surface vessel (USV), the heading angle error changes with the change of the target point. When the heading angle error is large, it is necessary to issue commands by adjusting the rudder angle to change the USV's direction of movement, thereby reducing the heading angle error and ensuring that the vessel can track the target point. However, due to the USV's own motion characteristics and external interference, it is prone to overshoot or even turning motion. Summary of the Invention

[0005] Purpose of the invention: The purpose of this invention is to provide an unmanned vessel trajectory tracking method, a trajectory tracking control system, and a method of use, so as to make the tracking process smoother and more efficient.

[0006] Technical solution: A method for tracking the trajectory of an unmanned surface vessel includes the following steps:

[0007] S1. Calculate the heading angle error θ e ;

[0008] S2, Determine θ e Error range, and determine which guidance algorithm to use;

[0009] S3. Calculate the ship's current position and determine whether the target point has been tracked;

[0010] S4. If the target point is not tracked, repeat S1 to S3. If the target point is tracked, switch to the next desired target point.

[0011] S5. Determine whether the target point is the endpoint. If yes, end the algorithm. If not, repeat S1 to S4.

[0012] The θ e The method for determining the error range is as follows:

[0013]

[0014] Where a and b are the thresholds for algorithm switching, when θ e When θ ∈ ((b, π) ∪ (-π, -b), the L1 algorithm is used; when θ e When θ ∈ (-a, a), the LOS algorithm is used; when θ e When ∈((-b,-a)∪(a,b)), the LL algorithm will be used.

[0015] The LL algorithm is a weighted fusion of the outputs of the LOS algorithm and the L1 algorithm. It uses mixed weights to combine the heading angle adjustment amounts of the LOS and L1 algorithms to obtain the final output heading angle adjustment amount.

[0016] The LL algorithm is specifically as follows:

[0017] Hybrid weights are used to combine the heading angle adjustments from the LOS and L1 algorithms to obtain the final output head. adjust as follows:

[0018] head adjust =W los *headlos+W l1 *headl1

[0019] Among them, W los and W l1 It is a hybrid weighting of LOS and L1 algorithms, head los and head l1 This refers to the heading angle adjustment for both LOS and L1 algorithms; the hybrid weighting function is:

[0020]

[0021] W l1 =1-W los

[0022] Where c is the kurtosis coefficient of the weighting function; θ0 is the threshold of the inflection point of the weighting function.

[0023] An unmanned surface vessel trajectory tracking and control system includes: a command and control system, an intelligent decision-making module, a data processing module, a navigation module, and a trajectory tracking and control module. The command and control system is connected to the intelligent decision-making module, which is sequentially connected to the data processing module, the navigation module, and the trajectory tracking and control module. The trajectory tracking and control module is connected to the command and control system.

[0024] The intelligent decision-making module is used to select the unmanned vessel's navigation mode, including autonomous navigation mode and manual assistance mode.

[0025] The navigation module includes global path planning and local path planning, used to realize unmanned vessel path planning.

[0026] The trajectory tracking control module includes trajectory tracking and a PID controller, enabling unmanned surface vessel (USV) path tracking and trajectory optimization adjustment.

[0027] The data processing module includes a data acquisition sensor and data processing. The data acquisition sensor is a sensing system composed of multiple sensors, used to acquire real-time data.

[0028] A method for using an unmanned surface vessel trajectory tracking and control system includes the following steps:

[0029] S1. The command and control center inputs the starting and ending positions and sends them to the unmanned surface vessel;

[0030] S2. After receiving instructions, the unmanned vessel selects a navigation mode and uses a perception system composed of multiple sensors to acquire real-time data, which is then processed by a data processing module.

[0031] S3. Perform global and local path planning between the known starting and ending positions, and determine the optimal navigation route;

[0032] S4. After the trajectory tracking module obtains the optimal navigation route information, it performs data analysis, calculates the heading angle error, and then determines the appropriate guidance algorithm to use based on the error range.

[0033] S5. When the motion control module obtains the heading change data of the unmanned vessel, it calculates the corresponding control commands and completes the path tracking function.

[0034] S6. Determine the position information of the ship relative to the outside world. After receiving the instructions sent by the unmanned vessel, the command and control center calculates the shortest distance between the current position of the unmanned vessel and the obstacle, and issues speed and heading instructions to the unmanned vessel to complete the avoidance action.

[0035] S7 communicates with the unmanned vessel and the command and control center. After receiving instructions, it processes them and sends them to the corresponding unit modules. The command and control center receives the unmanned vessel's navigation route information in real time.

[0036] Beneficial effects: This invention combines the LOS algorithm and the L1 algorithm (LL for short) to fully leverage their respective advantages and solve more complex and diverse path tracking problems. This enables unmanned systems to move precisely along predetermined paths, improving operational efficiency and significantly enhancing safety. By controlling the unmanned surface vessel (USV) through a novel trajectory tracking control method, the accuracy and precision of its tracking during navigation are improved, enhancing its adaptability to complex environments, its response speed to emergencies, and its ability to maintain path accuracy and stability. Attached Figure Description

[0037] Figure 1 This is a flowchart of the unmanned vessel trajectory tracking method of the present invention;

[0038] Figure 2 This is a block diagram of the unmanned vessel trajectory tracking and control system of the present invention;

[0039] Figure 3 This is a flowchart of the unmanned vessel trajectory tracking and control system of the present invention. Detailed Implementation

[0040] The technical solutions in the embodiments of the present invention will now be clearly and completely described in conjunction with the accompanying drawings.

[0041] like Figure 1 As shown, the unmanned vessel trajectory tracking method of the present invention includes the following:

[0042] S1. Calculate the heading angle error θ using the ship's current position and the target point. e ;

[0043] S2, Determine θ e The error range is used to determine which guidance algorithm to use: Available guidance algorithms include LOS, L1, and LL. The LL algorithm weightedly combines LOS and L1, using mixed weights to optimize the heading angle adjustment, thus improving the unmanned surface vessel's tracking process.

[0044] Based on heading angle error θ e The judgment is:

[0045]

[0046] Where a and b are the thresholds for algorithm switching. When θ e When ∈((b, π)∪(-π, -b), it indicates that the current unmanned surface vessel is in a state of significant fluctuation, and the L1 algorithm is used. When θ e When ∈(-a, a), it indicates that the current unmanned surface vessel is in a state of relatively small fluctuation, and the LOS algorithm is used. When θe When ∈((-b,-a)∪(a,b)), the LL algorithm will be used. The principle is to perform weighted fusion of the outputs of the two algorithms. Using mixed weights to combine the heading angle adjustment amounts of the LOS and L1 algorithms, the final output heading angle adjustment amount is obtained, which makes the algorithm switching process smoother and reduces the error in the trajectory tracking process.

[0047] The improved guidance algorithm LL is as follows:

[0048] Hybrid weights are used to combine the heading angle adjustments from the LOS and L1 algorithms to obtain the final output head. adjust as follows:

[0049] head adjust =W los *headlos+W il *headl1

[0050] Among them, W los and W l1 It is a hybrid weighting of LOS and L1 algorithms, head los and head l1 This is the heading angle adjustment for both LOS and L1 algorithms. The hybrid weighting function is:

[0051]

[0052] W l1 =1-W los

[0053] Where c is the kurtosis coefficient of the control weight function; θ0 is the threshold of the inflection point of the weight function; by adjusting c, the logic function is made more sensitive to changes at the threshold, and its value is adjusted according to the results of experiments and simulations.

[0054] The threshold θ0 is the inflection point of the weighting function, controlling where the function's output approaches 1. The threshold is set based on the actual magnitude of the heading error. A smaller threshold makes the weighting function more sensitive to small changes in the heading error, while a larger threshold makes the weighting function only begin to change significantly when the heading error is large. The threshold is initially set by observing the typical heading error range in navigation tasks, and then adjusted. θ0 is the key factor in algorithm switching; adjusting θ0 ensures that when the error θ→0, the weight wlos→1, and as the heading error increases, the weight wlos→1. los →0, w l1 →1. The threshold is adjusted based on the accumulated heading deviation—a time-varying threshold is set via the PD controller. Heading angle error θ e This indicates the current heading deviation of the vessel relative to the target point.

[0055] S3. Calculate the ship's current position and determine whether the target point has been tracked;

[0056] S4. If the target point is not tracked, repeat S1 to S3. If the target point is tracked, switch to the next desired target point.

[0057] S5. Determine whether the target point is the endpoint. If yes, end the algorithm. If not, repeat S1 to S4.

[0058] like Figure 2 As shown, the unmanned surface vessel trajectory tracking control system of the present invention includes: a command and control system, an intelligent decision-making module, a data processing module, a navigation module, and a trajectory tracking control module. During navigation, the unmanned surface vessel sends status feedback information back to the command and control system. After receiving the information from the unmanned surface vessel, the command and control system calculates the shortest distance between the current position of the unmanned surface vessel and the obstacle based on the position of the unmanned surface vessel, and issues speed and heading commands to the unmanned surface vessel to complete the avoidance action.

[0059] The intelligent decision-making module includes an autonomous navigation mode and a manual assistance mode; it is used to select the unmanned vessel's navigation mode. Under normal circumstances, the autonomous navigation mode is selected, and in emergency situations, the manual assistance mode is selected.

[0060] The navigation module includes global path planning and local path planning; it is used to implement path planning for unmanned vessels.

[0061] The trajectory tracking control module includes trajectory tracking and a PID controller, which enables unmanned surface vessels to track paths and optimize their trajectories, thereby controlling the vessel to navigate along a specific trajectory.

[0062] The data processing module processes the data collected by the sensors. This includes data acquisition sensors and data processing. The data acquisition sensors are a sensing system composed of multiple sensors used to acquire real-time data, including GPS, motor speed, electronic compass, rudder angle, gyroscope, electronic radar, depth sounding module, AIS, odometer, anemometer, and weather instrument. Specifically, GPS acquires the latitude and longitude data of the unmanned vessel; the electronic compass acquires the heading and attitude of the unmanned vessel; the electronic radar acquires the distance and position-related parameters of obstacles on the sea surface; the depth sounding module acquires underwater geographic information data; the AIS acquires and exchanges the position, heading, and speed of approaching vessels; the odometer acquires the speed and range data of the unmanned vessel relative to the seabed; the anemometer acquires sea wind speed and direction data; and the weather instrument acquires sea weather data.

[0063] To improve data accuracy and completeness, data fusion technology is used to integrate and process data from different sensors or information sources, eliminating errors and noise between data points and improving data accuracy and completeness, thereby making the application more precise and reliable. A Kalman filter is designed to estimate the attitude angles of the unmanned surface vessel, and the state variables are designed as follows:

[0064]

[0065] Where θ represents the ship's pitch and φ represents the ship's roll. Define the rotation matrix:

[0066]

[0067] Will Represented as

[0068]

[0069] It can be obtained

[0070]

[0071] Thus we can obtain θ=arcsin(-r 31 ),

[0072] Establish a process model for the Kalman filter, let R has The process model is represented as The Kalman filter obtains an estimate of the state variable x, which can be solved using the formula. and

[0073] After the command and control system issues commands from the starting position to the destination position to the unmanned surface vessel (USV) via the wireless network communication module, the decision-making module selects the navigation mode. Based on electronic nautical chart data, the navigation module integrates real-time data obtained by a perception system composed of multiple sensors to construct a navigation environment map, enabling the USV to locate itself. It then uses the A* algorithm to obtain the global path and the artificial potential field method to obtain the local path, thus realizing path planning for the USV. The trajectory tracking module uses an improved guidance algorithm to achieve path tracking and trajectory optimization adjustment for the USV, enabling it to navigate along a specific trajectory. It also uses an improved interference observer to observe and compensate for external interference.

[0074] like Figure 3 As shown, the method of using the unmanned vessel trajectory tracking and control system of the present invention includes the following steps:

[0075] S1. The command and control center inputs the starting and ending positions on the electronic nautical chart and sends them to the unmanned surface vessel via the wireless network communication module.

[0076] S2. After receiving instructions, the unmanned vessel selects a navigation mode and then uses a perception system composed of multiple sensors to acquire real-time data, which is then processed by a data processing module.

[0077] S3. Perform global and local path planning between the two known starting and ending points, and use a hybrid algorithm of A* algorithm and artificial potential field method to formulate the best navigation route.

[0078] S4. After obtaining the optimal navigation route information, the trajectory tracking module performs data parsing to obtain the total number of all path nodes and individual path node data, as well as all latitude and longitude data and real-time latitude and longitude data during navigation. Based on the change of the unmanned surface vessel's target point, it calculates the heading angle error and then determines the appropriate guidance algorithm based on the error range. The LL algorithm utilizes a hybrid weighting method to ensure the accuracy of straight-line path tracking, improve the smoothness of curve tracking, and reduce trajectory tracking errors.

[0079] S5. When the motion control module obtains the heading change data of the unmanned vessel, it calculates the corresponding control commands and completes the path tracking function to realize the autonomous navigation of the unmanned vessel.

[0080] S6. Based on the scanning information of the electronic radar, the position information between the ship and the external environment is determined. If a safe distance can be maintained, the ship does not need to change its course and can continue to select the autonomous navigation mode and drive normally. When the ship exceeds the safe distance from the external object, the decision module immediately switches the mode to the manual remote control mode. The unmanned ship sends the status feedback information back to the command and control center. After receiving the instructions sent by the unmanned ship, the command and control center calculates the shortest distance between the current position of the unmanned ship and the obstacle, and issues speed and heading instructions to the unmanned ship to complete the avoidance action.

[0081] S7. The unmanned vessel communicates with the command and control center through a wireless network communication module, monitors external commands at a certain frequency, processes the received commands briefly, and sends them to the corresponding unit module. The command and control center receives the unmanned vessel's navigation route information in real time.

Claims

1. A method for tracking the trajectory of an unmanned surface vessel, characterized in that, Includes the following steps: S1. Calculate the heading angle error. ; S2, Judgment The error range is determined, and the guidance algorithm used is identified. The method for determining the error range is as follows: in, b and b are the threshold values ​​for algorithm switching, when When, the L1 algorithm is used; when When, the LOS algorithm is used; when In this case, the LL algorithm will be used. The LL algorithm is a weighted fusion of the outputs of the LOS algorithm and the L1 algorithm. A mixed weight is used to combine the heading angle adjustment amounts of the LOS and L1 algorithms to obtain the final output heading angle adjustment amount, specifically: The heading angle adjustment amounts from the LOS and L1 algorithms are combined using hybrid weights to obtain the final output. as follows: in, and It is a hybrid weighting of LOS and L1 algorithms. and This refers to the heading angle adjustment for both LOS and L1 algorithms; the hybrid weighting function is: in, To control the kurtosis coefficient of the weighting function; It is the inflection point threshold of the weighting function; S3. Calculate the ship's current position and determine whether the target point has been tracked; S4. If the target point is not tracked, repeat S1~S3. If the target point is tracked, switch to the next expected target point. S5. Determine whether the target point is the endpoint. If yes, end the algorithm. If not, repeat S1 to S4.

2. An unmanned vessel trajectory tracking control system applicable to the unmanned vessel trajectory tracking method of claim 1, characterized in that, include: The system comprises a command and control system, an intelligent decision-making module, a data processing module, a navigation module, and a trajectory tracking control module. The command and control system is connected to the intelligent decision-making module, which is sequentially connected to the data processing module, the navigation module, and the trajectory tracking control module. The trajectory tracking control module is connected to the command and control system.

3. The unmanned vessel trajectory tracking and control system according to claim 2, characterized in that, The intelligent decision-making module is used to select the unmanned vessel's navigation mode, including autonomous navigation mode and manual assistance mode.

4. The unmanned vessel trajectory tracking and control system according to claim 2, characterized in that, The navigation module includes global path planning and local path planning, which are used to realize unmanned vessel path planning.

5. The unmanned vessel trajectory tracking and control system according to claim 2, characterized in that, The trajectory tracking control module includes trajectory tracking and a PID controller, enabling unmanned vessels to track paths and optimize their trajectories.

6. The unmanned vessel trajectory tracking and control system according to claim 2, characterized in that, The data processing module includes a data acquisition sensor and data processing. The data acquisition sensor is a sensing system composed of multiple sensors, used to acquire real-time data.

7. A method of using the unmanned vessel trajectory tracking and control system according to any one of claims 2 to 6, characterized in that, Includes the following steps: S1. The command and control system inputs the starting and ending positions and sends them to the unmanned vessel; S2. After receiving instructions, the unmanned vessel selects a navigation mode and uses a perception system composed of multiple sensors to acquire real-time data, which is then processed by a data processing module. S3. Perform global and local path planning between the known starting and ending positions, and determine the optimal navigation route; S4. After the trajectory tracking module obtains the optimal navigation route information, it performs data analysis, calculates the heading angle error, and then determines the appropriate guidance algorithm to use based on the error range. S5. When the motion control module obtains the heading change data of the unmanned vessel, it calculates the corresponding control commands and completes the path tracking function. S6. Determine the ship's position relative to the outside world. After receiving the instructions sent by the unmanned vessel, the command and control system calculates the shortest distance between the unmanned vessel's current position and the obstacle, and issues speed and heading instructions to the unmanned vessel to complete the avoidance maneuver. S7 communicates with the unmanned vessel and the command and control system. After receiving instructions, it processes them and sends them to the corresponding unit modules. The command and control system receives the unmanned vessel's navigation route information in real time.

Images