Unmanned ship track control method and storage medium based on variable parameter line of sight method
The mathematical model of the ship and the design of the heading controller are established through the variable parameter line-of-sight method, which solves the problem of track deviation of the unmanned ship when disturbed by wind and waves under the curved track, and achieves accurate tracking of the curved track and improved navigation stability.
Patent Information
- Application Number
- CN202211207690.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-09-30
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2042-09-30
AI Technical Summary
The traditional line of sight method cannot effectively track the curved track of unmanned ships when disturbed by wind and waves, resulting in track deviations and increased energy consumption, and may even cause navigational accidents.
The unmanned ship track control method based on the variable-parameter line of sight method is adopted. By establishing a ship mathematical model for a limited time, the heading controller is designed, and the sight angle is calculated using the variable-parameter line of sight method, and the rudder angle is adjusted to eliminate track errors. Unmanned ships sail along the desired trajectory.
Accurate tracking control of curved trajectories is achieved, trajectory deviation is avoided, navigation stability and energy efficiency are improved, and navigation risks are reduced.
Smart Images

Figure CN115562266B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of track tracking control of unmanned ships, and in particular to an unmanned ship track control method based on a variable parameter line of sight method. Background Art
[0002] The system of unmanned ships is a complex system, which includes multiple functional modules. Navigation control is one of the important modules of the system. When unmanned ships sail at sea, they are required to sail along a specific route, that is, the expected trajectory, from the perspectives of economy and safety. However, during the navigation process, the ship will inevitably be affected by random interference such as wind, waves and currents, and the ship will deviate from the planned track, which will not only consume more energy, but also cause navigation accidents. Therefore, it is of great significance to design a reasonable track controller to enable the ship to maintain the track and reach the destination as soon as possible.
[0003] The current technical problems are:
[0004] When the lakes, rivers and seas are calm, unmanned boats usually sail in a straight line or a broken line composed of straight lines, which takes the shortest time and consumes the least fuel. When ships are sailing, they are always affected by external environmental factors. When planning the desired route, they can be planned as a dynamic curve. At this time, the traditional line of sight method cannot meet the needs of tracking the desired route. Summary of the invention
[0005] The present invention proposes an unmanned ship track control method based on a variable parameter line of sight method, which can solve at least one of the above technical problems.
[0006] To achieve the above object, the present invention adopts the following technical solutions:
[0007] A method for controlling the track of an unmanned ship based on a variable parameter sight line method comprises the following steps:
[0008] Firstly, a mathematical model of the ship within a limited time is established, and the model parameters are obtained through a rotation test. The system is transformed into an optimal second-order system to design a heading controller.
[0009] The ship trajectory tracking control is realized based on the line of sight method. The line of sight angle of the ship sailing along the trajectory is calculated according to the positional relationship between the current coordinates and the target route. The line of sight angle is subtracted from the ship's heading angle to obtain the target heading angle, and then the target rudder angle is obtained. Commands are sent to the lower layer to control the ship's movement, eliminate track errors, and make the ship sail in the direction of the desired trajectory.
[0010] Further, the method specifically includes the following steps:
[0011] S1. Establish a ship model within a limited time;
[0012] S2. Design heading controller;
[0013] S3, mapping discontinuous nodes into the control algorithm interval;
[0014] S4. Variable parameter sight line method is used to realize unmanned ship track tracking control.
[0015] Furthermore, the S1, establishing a ship model within a limited time specifically includes:
[0016] In a finite time, the ship is regarded as a first-order inertial model, and the transfer function is Where K is the gain coefficient and T is the time constant;
[0017] The step response output peak value of the above model is A, the input step response coefficient is K1, and the system output is Then the system's amplification factor K = A / K1; for a first-order system The step response of is transformed by Laplace inverse to obtain the time domain signal c(t) = 1-e -t / T ;
[0018] After the ship reaches a stable state of sailing in a straight line at a set speed, the rudder is turned to the set rudder angle and kept unchanged. At this time, the ship enters a turning state. The output angular velocity and input rudder angle data are collected in real time, and the relationship curves between them and the test time are plotted respectively. Since the angular velocity and the rudder angle satisfy this relationship The model parameters K and T of the ship when turning can be calculated.
[0019] Furthermore, the S2, designing the heading controller specifically includes:
[0020] The open-loop transfer function of a standard second-order system is: ξ is the damping ratio, ω n is the natural frequency, and the ship model established in the first step is Then through a points link Get the relationship between the heading angle and the input rudder angle The designed controller is W c (s), due to Controller This forms a closed loop, with the input being the desired heading angle and the output being the actual heading angle measured by the sensor.
[0021] Furthermore, the step S3, mapping discontinuous nodes into the control algorithm interval specifically includes:
[0022] Map angles that are not in the control range to In this interval, the specific mapping method is as follows:
[0023]
[0024] The longitude and latitude of the ship are obtained by DGPS, converted into XY plane coordinates in real time, and drawn on the map;
[0025] Use the serial port to read DGPS information, parse it according to the reading protocol, and obtain longitude, latitude, east speed, north speed and heading angle information for route planning, locating the current ship position, calculating the target heading angle, and the range of the heading angle detected by DGPS In actual control, it needs to be mapped to the control interval of the line of sight method. The conversion between the detection angle and the control angle coordinates is as follows:
[0026]
[0027] Furthermore, the S4, variable parameter sight line method to realize unmanned ship track tracking control specifically includes:
[0028] The flight segment identification is distinguished by the waypoint number, and the target waypoint number [x d (k),y d (k)] and the current waypoint number (x, y) determine the current target segment, the starting waypoint is point 1, and the subsequent waypoint numbers are incremented by one; let the switching radius be R0, and the conversion conditions are as follows:
[0029]
[0030] When the unmanned ship is found to have entered the path with the current path point [x d (k),y d (k)] as the center and R0 as the radius, it is determined to have reached the target waypoint; or it has passed this segment and entered the next segment, but has not been determined to have entered the above circle; the target waypoint is switched to the next waypoint [x d (k+1),y d (k+1)], the current waypoint number is increased by one, and the target segment becomes [x d (k),y d (k)]→[x d (k+1),y d (k+1)], the desired heading angle becomes Until the last target point;
[0031] When the desired trajectory of the unmanned ship is designed as a curve, the curve is divided into segments of different lengths according to the tangent, so that the curve can be tracked; a point on the curve is taken as the external tangent, and a curve is divided into n straight line segments. At this time, the line of sight radius R and the switching radius R0 are adjusted according to the distance between the current position of the ship and the desired segment, as well as the length of the segment;
[0032] Assume that the vertical distance between the current position of the ship and the expected route is d. When the target route does not appear in the sight radius circle of the unmanned ship, the sight radius at this time is increased, and R' is set to d. The ship is controlled to approach the target route until the target route enters the sight circle. At this time, the sight radius is equal to the initial setting value. When the route is shorter than the switching radius R0, the conditions for switching the route will be adjusted. Due to the large inertia of the ship, the minimum switching radius should not be less than one ship length L, so R0 is set to L.
[0033] On the other hand, the present invention further discloses a computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor executes the steps of the above method.
[0034] It can be seen from the above technical scheme that the unmanned ship track control method based on the variable parameter line of sight method of the present invention first establishes a ship mathematical model within a limited time, obtains model parameters through a slewing test, and designs a heading controller and a track controller to make the unmanned ship sail according to the expected track. The main innovation of the present invention is that the expected route is a curve between two points and changes all the time, so a variable parameter line of sight method is used to control the track tracking of the system.
[0035] Firstly, a mathematical model of the ship within a limited time is established, and the model parameters are obtained through a rotation test. The system is transformed into an optimal second-order system to design a heading controller. The ship trajectory tracking control is realized based on the line of sight method. The line of sight angle of the ship sailing along the trajectory is calculated according to the positional relationship between the current coordinates and the target route. The line of sight angle is subtracted from the ship's heading angle to obtain the target heading angle, and then the target rudder angle is obtained. Commands are sent to the lower layer to control the ship's movement, eliminate the track error, and make the ship sail in the direction of the desired trajectory.
[0036] Compared with the prior art, the present invention has the following advantages:
[0037] (1) When the unmanned ship has not entered the switching circle but has passed the switching waypoint, the line of sight radius and switching conditions of the algorithm are adjusted to avoid the problem that the ship cannot switch sections normally during navigation, making the track control more accurate.
[0038] (2) The line-of-sight method requires that the desired route is a broken line segment composed of straight lines. However, since the navigation of the unmanned ship is disturbed by wind, waves and currents, the planned target route is a curve between two points on the curved surface. The variable parameter line-of-sight method adopted in the present invention can track the curve trajectory.
[0039] (3) A simple rotation test is used to obtain two important parameters of the ship, and the unmanned ship system is designed as an optimal second-order system. There is no need for a large number of tests to adjust the control parameters, and the control effect is better and the stability is higher. BRIEF DESCRIPTION OF THE DRAWINGS
[0040] Figure 1 Implement a flow chart for the line of sight algorithm;
[0041] Figure 2 This is a schematic diagram of the line of sight method;
[0042] Figure 3a It is the block diagram of the first-order system step response;
[0043] Figure 3b It is the time domain signal curve diagram of the first-order system step response;
[0044] Figure 4a is the relationship curve between rudder angle and angular rate, where the input rudder angle = 34°;
[0045] Figure 4b is the relationship curve between rudder angle and angular rate, where the input rudder angle = 30°;
[0046] Figure 5 This is the working principle diagram of heading control;
[0047] Figure 6 Design a block diagram for the heading controller;
[0048] Figure 7 Design a block diagram for the overall system;
[0049] Figure 8 It is the coordinate conversion diagram of detection angle and control angle;
[0050] Fig. 9 This is a schematic diagram of the variable parameter sight line method;
[0051] Fig.10 This is a schematic diagram of flight segment switching;
[0052] Fig.11 This is the trajectory tracking control result diagram. DETAILED DESCRIPTION
[0053] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.
[0054] When unmanned ships are sailing at sea, they are required to sail along a specific route in consideration of both economy and safety. However, ships will inevitably be affected by random disturbances such as wind, waves and currents, causing them to deviate from the planned track, which will not only consume more energy but also cause navigation accidents. Therefore, it is of great significance to design a reasonable track controller to enable the ship to maintain the track and reach the destination as quickly as possible.
[0055] For the problem of unmanned ship track control, the line of sight method is usually used. The navigation principle of this algorithm is: if the controlled ship's heading is kept aligned with the line of sight angle, the controlled ship can reach the desired position through proper control, achieving the effect of track tracking. This algorithm reduces the traditional control quantity from 3 degrees of freedom of ship position and heading angle to 2 degrees of freedom of ship heading angle and navigation speed. The algorithm implementation flow chart of the line of sight method is as follows Figure 1 .
[0056] Schematic diagram of the line of sight method Figure 2 The route in route planning is a line segment from point P1 = (x1, y1) to point P2 = (x2, y2). A point P0 = (x0, y0) outside the route represents the current position of the unmanned ship. The speed is v. The radius of the line of sight algorithm is R = (3 to 5) times the length of the ship. A circle is drawn with R as the radius to the next target waypoint. The intersection point is P. LOS =(x LOS ,y LOS ), d represents the distance from the current position to the route, and the foot coordinate point is P N =(x N ,y N ).
[0057] The line of sight method requires that the desired route is a broken line segment composed of straight lines. However, since the navigation of the unmanned ship is disturbed by wind, waves and currents, the planned target route is a curve between two points on the surface. Since the external environment changes all the time, the desired route is constantly changing, and the slope of the curve changes, resulting in changes in the desired route segment. At this time, the variable parameter line of sight method is used to track the trajectory of the unmanned ship. And the discontinuous nodes are mapped to the control interval of the algorithm, which improves the stability of the algorithm.
[0058] The specific implementation steps are as follows:
[0059] Step 1: Establish a ship model within a limited time.
[0060] In a finite time, the ship model parameters can be considered unchanged. The ship is a large inertial object, so it can be regarded as a first-order inertial model, and the transfer function is Where K is the gain factor and T is the time constant.
[0061] like Figure 3a ), the step response output peak value of the above model is A, the input step response coefficient is K1, and the system output is Then the system's amplification factor K = A / K1. For a first-order system The step response of is transformed by Laplace inverse to obtain the time domain signal c(t) = 1-e -t / T ,like Figure 3b ). It can be seen that the time t corresponding to the point 0.632 times the peak value is the time constant T of the system.
[0062] After the ship reaches a stable state of sailing in a straight line at a set speed, the rudder is turned to the set rudder angle and kept unchanged. At this time, the ship enters a turning state. The output angular velocity and input rudder angle data are collected in real time, and the relationship curves between them and the test time are plotted respectively. Since the angular velocity and the rudder angle satisfy this relationship The model parameters K and T of the ship when turning can be calculated.
[0063] By analyzing the third and fourth curves in the legend of Figure 4 - the command rudder angle value and the actual rudder angle feedback value, it is found that it takes about 1 second for actual data feedback to be available after the command is issued to the servo. Therefore, there is a delay link in the servo model. -τs The actual measured time constant T≈6s>3τ, so the delay of the servo can be ignored, and the delay link is equivalent to a small first-order inertia link and combined with the servo model to obtain:
[0064]
[0065] When the left motor speed is 696 rpm, the right motor speed is 656 rpm, and the actual ship speed is about 2.4 m / s, K≈0.079 and T≈6.6; when the left motor speed is 793 rpm, the right motor speed is 794 rpm, and the actual ship speed is about 3.4 m / s, K≈0.085 and T≈5.9.
[0066] Through the tests at different ship speeds downstream and upstream, the obtained parameters are relatively stable. Therefore, it can be considered that the parameters of the ship's turning are constant within a small time range and under small disturbances.
[0067] Step 2: Design the heading controller.
[0068] The working principle diagram of heading control is as follows: Figure 5 shown.
[0069] Working principle of heading control: The relationship between the command rudder angle and the angular rate is a first-order inertia link, and the heading angle of the ship is obtained by integration. The input of the heading controller is the heading deviation, which is obtained by comparing the input target heading and the current heading angle collected by the sensor, and then input to the heading controller to output the target rudder angle δ r (k), and then transmitted to the rudder angle controller, the rudder turns to the required angle according to the rudder angle command signal. At this time, the ship changes its course and tracks the target to finally realize the control function.
[0070] By designing the controller, the entire system is transformed into an optimal second-order system. Compared with the first-order system, the second-order system has better dynamic and static performance, and the system is relatively stable and easy to control. The main parameters affecting the second-order system are the frequency ratio and the damping ratio. When the damping ratio is between 0.6 and 0.8, the system has better performance based on the comprehensive overshoot, rise time, and stabilization time. Therefore, when designing the controller, the damping ratio is taken as 0.707 and the natural frequency is taken as 1, and the system with the best comprehensive performance is achieved through zero-pole configuration.
[0071] The controller design block diagram is as follows Figure 6 As shown. The open-loop transfer function of a standard second-order system is: (ξ is the damping ratio, ω n is the natural frequency), the ship model established in the first step is Then through a points link Get the relationship between the heading angle and the input rudder angle The designed controller is W c (s). Controller This forms a closed loop, with the input being the desired heading angle and the output being the actual heading angle measured by the sensor.
[0072] Step 3: Map discontinuous nodes into the control algorithm interval.
[0073] Since the control interval of the line of sight method is on [-π,π], the two nodes -π and π are discontinuous. When the ship is sailing due south, the heading angle may need to change near the two nodes -π and π in many cases. When the ship is tracking the desired heading point, the controller will give an erroneous command, causing the ship to go around in a large circle in the opposite direction to track the desired heading point. This situation is very unfavorable for track tracking control of the ship.
[0074] When the heading angle deviation is within the control range, When , the instructions issued by the controller are correct and can normally guide the ship to track the target heading point; when When the command issued by the controller differs from the actual heading angle by ±360°, the actual heading angle of the ship may converge to the desired heading angle from the opposite direction. Therefore, it is necessary to map the angle that is not in the control range to the interval [-π,π] to solve this problem. For the specific mapping method, refer to
[0075]
[0076] The overall system design diagram is as follows: Figure 7 The overall design of the system is divided into two parts: simulation part and actual part. The block diagram includes navigation calculation module, sensor detection link, position solution link, heading control loop, and track control link.
[0077] The navigation calculation module realizes functions such as route loading, waypoint number calculation, arrival judgment, and target route heading angle calculation. The position solution link decomposes and integrates the speed in the x-axis and y-axis directions under the heading angle according to the current heading and speed to obtain the XY position.
[0078] The function of the track control link is to first compare the line of sight angle calculated by the received coordinate value of the unmanned boat with the heading angle of the target route through the navigation calculation link through the LOS algorithm, calculate the track error, input it into the track controller, obtain the heading correction value through the track control algorithm, and compare it with the target heading angle calculated in the navigation calculation link to obtain the heading. The calculated heading angle is transmitted to the heading control loop to eliminate the track error and make the ship move in the direction of the desired track.
[0079] Under the model simulation, the position is solved according to the heading angle and speed presented by the model, and the segment switching is realized when the simulated position is a certain distance away from the set waypoint, until the ship reaches the last waypoint and the simulation ends. In the actual test, the longitude and latitude of the ship are obtained by DGPS, converted into XY plane coordinates in real time, and drawn on the map. The map and the actual GPS must be calibrated in the early stage.
[0080] Use the serial port to read DGPS information, parse it according to the reading protocol, and obtain information such as longitude, latitude, east speed, north speed, and heading angle for route planning, locating the current ship position, calculating the target heading angle, etc. It should be noted that the range of the heading angle detected by DGPS is In actual control, it needs to be mapped to the control interval of the line of sight method. The conversion between the detection angle and the control angle coordinates is as follows: Figure 8 shown.
[0081]
[0082] Step 4: Use the variable parameter line of sight method to realize the unmanned ship track tracking control.
[0083] During navigation, there must be a switching condition to determine whether to switch to the next waypoint. The segment identification is distinguished by the waypoint number, and the target waypoint number [x d (k),y d (k)] and the current waypoint number (x, y) determine the current target segment. The starting waypoint is point 1, and the subsequent waypoint numbers are incremented by one. Assume that the switching radius is R0, and the conversion conditions are as follows:
[0084]
[0085] When the unmanned ship is found to have entered the path with the current path point [x d(k),y d (k)] as the center and R0 as the radius, it is determined to have arrived at the target waypoint; or it has passed this segment and entered the next segment, but has not been determined to have entered the above circle, avoiding the situation where the ship fails to switch segments and cannot track the target route. The target waypoint switches to the next waypoint [x d (k+1),y d (k+1)], the current waypoint number is increased by one, and the target segment becomes [x d (k),y d (k)]→[x d (k+1),y d (k+1)], the desired heading angle becomes Until the last destination. The schematic diagram of segment switching is as follows Fig. 9 shown.
[0086] When the desired trajectory of the unmanned ship is designed as a curve, the curve is divided into segments of different lengths according to the tangent, so that the curve can be tracked. As the external environment changes all the time, the desired route is constantly changing, and the change in the slope of the curve causes the change in the desired route segment. Take a point on the curve as the external tangent, and divide a curve into n straight line segments, as shown in the schematic diagram Fig.10 At this time, the sight radius R and the switching radius R0 are adjusted according to the distance between the current position of the ship and the desired section, as well as the length of the section.
[0087] Assume that the vertical distance between the current position of the ship and the expected route is d. When the target route does not appear in the sight radius circle of the unmanned ship (i.e., d>R), the algorithm will increase the sight radius at this time, setting R'=d, and control the ship to approach the target route until the target route enters the sight circle (d≤R). At this time, the sight radius is equal to the initial setting value, i.e., R'=R; when the route is shorter than the switching radius R0, the algorithm will adjust the conditions for the route switching. Due to the large inertia of the ship, the minimum switching radius should not be less than one ship length L. Therefore, R0=L is set to improve the situation where the trajectory tracking effect is poor due to the large inertia of the ship when turning.
[0088] Tests were conducted under different water flow conditions. The trajectory tracking control effect diagram is shown below. Fig.11 , the control effect is good, which verifies the stability of the control algorithm.
[0089] In yet another aspect, the present invention further discloses a computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor executes the steps of any of the above methods.
[0090] On the other hand, the present invention further discloses a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of any of the above methods.
[0091] In another embodiment provided by the present application, a computer program product including instructions is also provided, which, when executed on a computer, enables the computer to execute the steps of any method in the above embodiments.
[0092] It is understandable that the system provided by the embodiment of the present invention corresponds to the method provided by the embodiment of the present invention, and the explanation, examples and beneficial effects of the relevant contents can refer to the corresponding parts in the above method.
[0093] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the program can be stored in a non-volatile computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).
[0094] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0095] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims
1. A method for controlling the track of an unmanned ship based on a variable parameter sight line method, characterized in that: The following steps are included: Firstly, a mathematical model of the ship within a limited time is established, and the model parameters are obtained through a rotation test. The system is transformed into an optimal second-order system to design a heading controller. The ship trajectory tracking control is realized based on the line of sight method. The line of sight angle of the ship along the trajectory is calculated according to the positional relationship between the current coordinates and the target route. The line of sight angle is subtracted from the ship's heading angle to obtain the target heading angle, and then the target rudder angle is obtained. The command is sent to the lower layer to control the ship's movement, eliminate the track error, and make the ship move in the direction of the desired trajectory. The specific steps include: S1. Establish a ship model within a limited time; S2. Design heading controller; S3, mapping discontinuous nodes into the control algorithm interval; S4, variable parameter sight line method to achieve unmanned ship track tracking control; The S1, establishing a ship model within a limited time specifically includes: In a finite time, the ship is regarded as a first-order inertial model, and the transfer function is Where K is the gain coefficient and T is the time constant; The step response output peak value of the above model is A, the input step response coefficient is K1, and the system output is Then the system's amplification factor K = A / K1; for a first-order system The step response of is transformed by Laplace inverse to obtain the time domain signal c(t) = 1-e -t / T ; After the ship reaches a stable state of sailing in a straight line at the set speed, the rudder is turned to the set rudder angle and kept unchanged. At this time, the ship enters the turning state, and the output angular velocity and input rudder angle data are collected in real time, and the relationship curves between them and the test time are plotted respectively. Since the angular velocity and the rudder angle satisfy this relationship The model parameters K and T of the ship when turning can be calculated; The S2, designing the heading controller specifically includes: The open-loop transfer function of a standard second-order system is: ξ is the damping ratio, ω n is the natural frequency, and the ship model established in the first step is Then through a points link Get the relationship between the heading angle and the input rudder angle The designed controller is W c (s), due to Controller Thus, a closed loop is formed, the input is the desired heading angle, and the output is the actual heading angle measured by the sensor; the S3, mapping the discontinuous nodes to the control algorithm interval specifically includes: Map angles that are not in the control range to In this interval, the specific mapping method is as follows: The longitude and latitude of the ship are obtained by DGPS, converted into XY plane coordinates in real time, and drawn on the map; Use the serial port to read DGPS information, parse it according to the reading protocol, and obtain longitude, latitude, east speed, north speed and heading angle information for route planning, locating the current ship position, calculating the target heading angle, and the range of the heading angle detected by DGPS In actual control, it needs to be mapped to the control interval of the line of sight method. The conversion between the detection angle and the control angle coordinates is as follows: The S4, variable parameter sight line method to achieve unmanned ship track tracking control specifically includes: The flight segment identification is distinguished by the waypoint number, and the target waypoint [x d (k),y d (k)] and the current waypoint (x, y) determine the current target segment, the starting waypoint is point 1, and the subsequent waypoint numbers are incremented by one; let the switching radius be R0, and the conversion conditions are as follows: When the unmanned ship is found to have entered the target waypoint [x d (k),y d (k)] as the center and R0 as the radius, it is determined to have reached the target waypoint; or it has passed this segment and entered the next segment, but has not been determined to have entered the above circle; the target waypoint is switched to the next waypoint [x d (k+1),y d (k+1)], the target waypoint number is increased by one, and the target segment becomes [x d (k),y d (k)]→[x d (k+1),y d (k+1)], the desired heading angle becomes Until the last target point; When the desired trajectory of the unmanned ship is designed as a curve, the curve is divided into segments of different lengths according to the tangent, so that the curve can be tracked; a point on the curve is taken as the external tangent, and a curve is divided into n straight line segments. At this time, the line of sight radius R and the switching radius R0 are adjusted according to the distance between the current position of the ship and the desired segment, as well as the length of the segment; Assume that the vertical distance between the current position of the ship and the expected route is d. When the target route does not appear in the sight radius circle of the unmanned ship, the sight radius at this time is increased, and R' is set to d. The ship is controlled to approach the target route until the target route enters the sight circle. At this time, the sight radius is equal to the initial setting value. When the route is shorter than the switching radius R0, the conditions for switching the route will be adjusted. Due to the large inertia of the ship, the minimum switching radius should not be less than one ship length L, so R0 is set to L.
2. A computer-readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the processor executes the steps of the method according to claim 1.
