Ship path tracking method based on adaptive sight guidance and fuzzy adaptive PID

Abstract

The invention belongs to the technical field of ship path tracking, and discloses a ship path tracking method based on adaptive line-of-sight guidance and fuzzy adaptive PID, and the method comprises the steps: presetting a path point, and obtaining a real-time coordinate; obtaining an actual expected heading angle # imgabs0 # and a ship observation heading angle # imgabs1 #, subtracting the actual expected heading angle # imgabs0 # and the ship observation heading angle # imgabs1 # to obtain a heading deviation # imgabs2 #, and calculating a heading deviation rate # imgabs3 #; the course deviation and the course deviation rate are input into a fuzzy self-adaptive PID motion control module, and gain parameters are output through a fuzzy control algorithm; the gain parameter and # imgabs4 # are input into a PID controller to calculate a ship rudder angle # imgabs5 #; inputting # imgabs6 # into the motion mathematical model to update the motion state, and forming a closed-loop updated motion state to enable the ship to approach a preset path point; and judging a ship path after the state information is updated. The expected course of the ship is calculated in real time and the sideslip angle of the ship and the control parameters of the ship are adjusted in real time through self-adaptive sight guidance, and the control parameters are automatically adjusted in real time by means of the fuzzy self-adaptive PID motion control module to achieve accurate motion control, so that the ship travels according to a preset planned path.

Description

Technical Field

[0001] The invention belongs to the technical field of ship path tracking, and in particular relates to a ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID. Background Art

[0002] As a key issue in the field of modern navigation control, ship path tracking technology has received extensive attention in applications such as autonomous ship systems and intelligent shipping. When ships perform tracking navigation tasks, accurately tracking the predetermined path is crucial to ensuring navigation safety and execution efficiency, and precise motion control plays a vital role in ordinary marine operations for ships to achieve maneuverability and agility. The complete motion control system of an intelligent ship usually consists of a ship heading guidance and motion control subsystem. The guidance subsystem is responsible for outputting ship attitude control commands to achieve kinematic control goals in smooth transient motion. The attitude control subsystem is responsible for generating the required forces and torques to track the required attitude commands, especially in the presence of model and environmental uncertainties. In general, the ship attitude command is called the guidance law. For unmanned surface vessels (ASVs), the heading guidance law is usually used in combination with the motion control algorithm.

[0003] Traditional path tracking methods for surface unmanned ships usually rely on algorithms such as line-of-sight (LOS) and PID control, but these methods have certain limitations in practical applications, especially in dynamic environments, where the accuracy and stability of path tracking are often affected.

[0004] In the ship heading guidance subsystem, the line-of-sight guidance algorithm is a common navigation control strategy and has been widely used in ship path tracking. The main idea is to calculate the line-of-sight angle between the ship and the target point, plan the desired heading angle, and input the ship attitude command, i.e. the desired heading angle, into the motion control algorithm. Although the line-of-sight guidance algorithm has good real-time performance and simple implementation, it ignores some of the ship's dynamic characteristics, especially the sideslip angle of the ship in the case of lateral drift and speed changes. The classic line-of-sight guidance algorithm has steady-state errors and overshoot phenomena, which affects the accuracy and stability of path tracking.

[0005] In the ship motion control subsystem, PID control algorithm is often used to adjust the heading angle to improve the ship's path tracking performance. PID control effectively adjusts the rudder angle through proportional, integral and differential control strategies, but in practical applications, the traditional PID control algorithm is easily affected by external disturbances (such as ocean currents, wind and waves), resulting in a decrease in path tracking accuracy. Summary of the invention

[0006] In view of the shortcomings of the background technology, the purpose of the present invention is to propose a ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID, comprising the following steps: Step S1: preset the path points and obtain the real-time coordinates of the ship through the ship motion mathematical model and real-time ship heading angle; Step S2, calculate the actual desired heading angle of the ship through the adaptive line of sight guidance strategy ; Step S3: compare the real-time heading angle with the actual desired heading angle Subtract to get the heading deviation , and the heading deviation rate is calculated ; Step S4: Set the heading deviation , heading deviation rate The input is fed into the fuzzy adaptive PID motion control module, and the fuzzy control algorithm outputs the three gain parameters of the PID controller in real time. , and ; Step S5: set the three gain parameters , and , and heading deviation Input the PID controller of the fuzzy adaptive PID motion control module, and output the ship's rudder angle after calculation ; Step S6: adjust the ship's rudder angle Input the mathematical model of ship motion to calculate the output or update the motion state of the ship at the next moment, forming a closed loop to continuously update the state of the ship at the next moment so that the ship continues to approach the preset path point; Step S7, after updating the ship motion status information, determine whether it has reached the preset path point: (1) If the preset path point is not reached, return to the adaptive line of sight guidance strategy link to continue to complete the path tracking task process; (2) If the preset path point is reached, determine whether the current path point is the last coordinate point of the preset path: ① If it is not the last coordinate point of the preset path, switch to the next path point to continue the path tracking task; ② If it is the last coordinate point of the preset path, the process ends and the path tracking task is completed.

[0007] Preferably, the specific process of obtaining the real-time coordinates of the ship in step S1 is as follows: S11, establish inertial coordinate system O 0 -X 0Y 0 and the ship coordinate system o-xy To describe the motion of a ship at sea, O 0 -X 0 Y 0 coincides with a certain horizontal plane on the sea surface, O 0 is a reference point on the sea surface. O 0 -X 0 The axis points due north. O 0 -Y 0 The axis points due east; o-xy The ship coordinate system is fixed to the hull. o is the ship's center of gravity, ox The axis is the ship's center of gravity pointing to the bow. oy The axis is the ship's center of gravity pointing to starboard; The formula for defining the ship state variables is:

[0008] In the formula , and are the north position, east position and heading of the ship in the inertial coordinate system, u is the longitudinal velocity of the ship along the longitudinal direction, v is the lateral velocity along the sway direction, r is the angular velocity of the ship's bow, U is the true speed of the ship, and the calculation formula is: , is the ship heading angle; The ship heading angle The sideslip angle of the ship Measuring heading angle with the ship The ship heading angle is The calculation formula is:

[0009] The ship's sideslip angle The formula is ; in, is the transformation matrix from the ship coordinate system to the inertial coordinate system, The formula of the transformation matrix is:

[0010] S12, simplifying the kinematic model of the unmanned surface ship into a three-degree-of-freedom ship motion mathematical model, and obtaining the formula of the three-degree-of-freedom ship motion mathematical model is:

[0011] Among them, M is the mass and inertia matrix of the ship, and the formula for defining the matrix M is:

[0012] Where m is the mass of the ship, is the ship’s moment of inertia, is the position of the ship’s center of gravity in the hull coordinate system, , , , , is the additional mass and additional moment of inertia; Among them, the lateral force , longitudinal force , lateral moment term The calculation formulas are:

[0013]

[0014]

[0015] In the formula , , , , , , , , , , , , The parameter is the hydrodynamic derivative of the ship, is the propeller thrust, For external interference, is the ship's rudder angle; S13, combined with the above formula derivation, the complete mathematical model formula of ship motion is expressed as:

[0016] According to the mathematical model of ship motion, and Calculate and obtain the real-time coordinates of the ship , and obtain the real-time ship heading angle.

[0017] Preferably, the step S2 obtains the actual desired heading angle of the ship The specific process is as follows: S21, when the vessel is moving along the path points connecting the horizontal plane and When the ship is traveling along the preset path, the along-track error of the current position of the ship from the preset path is and lateral track error The calculation formula is:

[0018]

[0019] in, is X 0 The distance difference between the current position of the ship and the next position in the axial direction, is Y 0 The distance difference between the current position and the next position of the ship in the axis coordinate direction, is the default path and X 0 The angle formed by the axes; S22, the path tracking control goal is achieved by reducing the lateral tracking distance between the ship's real-time position and the preset path, that is, the along-track error is not considered. , then the lateral track error Approaching 0, lateral track error The calculation formula is:

[0020] S23, introduces an adaptive line-of-sight guidance strategy system to reduce the ship's lateral track error Converted to the desired heading angle of the ship; the ship's sight vector is the sight distance from the current position to the preset path The point ends at the line of sight vector passing through the preset path and the inertial coordinate system X 0 The angle formed is the desired heading angle required in the ship guidance process. , then the expected heading angle The calculation formula is:

[0021] The expected heading angle of the ship is The preset path intersects X 0 The angle formed The angle between the sight vector and the preset path composed of; S24, in actual navigation, in addition to the speed u generated by the longitudinal motion, the ship also generates the speed v generated by the transverse motion, which causes the actual motion direction of the ship to have an angle deviation from the heading, which is the sideslip angle of the ship. , then the actual expected heading angle of the ship is The formula is:

[0022] The precise sideslip angle of the ship is The calculation formula is , is the true heading angle of the ship; Actual ship heading angle Measuring heading angle with the ship There is a noise deviation between , and its calculation formula is:

[0023] Because the ship can only obtain the observed heading angle during the tracking process , so when the ship completes the tracking task, the ship's measured heading angle is equal to the expected heading angle, that is , then the ship's observed heading is obtained The actual heading of the ship The sideslip angle between the two is defined as the generalized sideslip angle of the ship. , the formula is:

[0024] S25, more accurately solves the desired angle of the ship through adaptive line of sight guidance strategy The formula is:

[0025]

[0026] in, represents the true speed of the ship and , is the adaptive gain, is the line of sight distance on the ship's preset path.

[0027] Preferably, the heading deviation obtained in step S3 is and heading deviation rate The specific steps are as follows: Measure the heading angle of the ship The actual desired heading angle output by the adaptive line of sight guidance strategy Subtract to get the heading deviation and heading deviation rate , the calculation formula is expressed as:

[0028]

[0029] In the formula Heading deviation The differential of represents the change of heading deviation over time, is the time differential, which represents a small change in time.

[0030] Preferably, the three gain parameters of the PID controller output in step S4 are , and The specific process is as follows: S41, based on deviation and error rate of change Two input quantities and one output quantity establish a reference model controller; with the help of the controller to determine the membership function, a smooth and continuous Gaussian shape membership function is selected as the input quantity, and the output quantity is within the range of the fuzzy set membership function in the positive and negative regions, and the Gaussian function is used to output three membership function fuzzy variables , and ; S42, establish the corresponding fuzzy control rules, obtain fuzzy control information through fuzzy reasoning and logical reasoning rules, and then solve the fuzzy relationship equation with the help of fuzzy reasoning machine by applying fuzzy input and control rules to obtain three gain parameter fuzzy quantities. , and ; S43, the fuzzy gain parameter is converted into a clear gain parameter within the theoretical domain by the area center method , , Finally, the complete three gain parameters are plotted. , , Characteristic surface of inference output.

[0031] Preferably, in step S5, the ship rudder angle is calculated The specific process is as follows: The heading deviation And three gain parameters , and Input into the PID controller, and after the proportional, integral, and differential steps, the rudder angle is obtained. The formula is:

[0032] In the formula, represents the proportional gain of the controller, represents the integral gain of the controller, Represents the derivative gain of the controller.

[0033] Compared with the prior art, the present invention proposes a ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID. The present invention simultaneously considers factors such as the sideslip angle of the ship to correctly calculate the expected heading angle of the ship and takes into account the ship motion control accuracy; by introducing an adaptive line of sight guidance strategy algorithm, the sideslip angle generated by the ship during navigation can be accurately estimated, so as to calculate the correct expected heading angle of the ship; and by introducing a fuzzy adaptive PID control algorithm package, the ship can automatically adjust the control parameters of the fuzzy adaptive PID in real time in different environments or sea conditions to achieve precise motion control, so that the ship can obtain the correct expected heading angle in different environments or sea conditions and automatically adjust the control parameters in real time to achieve a precise control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 This is a flow chart of the ship path tracking system of the present invention; Figure 2 It is a schematic diagram of the water surface coordinates and adaptive line of sight guidance of a ship according to the present invention; Figure 3 The schematic diagram of the ship path tracking system based on adaptive line-of-sight guidance and fuzzy adaptive PID control of the present invention; Figure 4 for Kp Gain input heading deviation and heading deviation change rate, and output membership function curve, where (a) is the input Kp Membership function curve of heading deviation, (b) is the input Kp Membership function curve of heading deviation change rate, (c) is the output Kp Membership function curve graph; Figure 5 for K p , K i and K d Output characteristic surface diagram, where (a) is K p Output characteristic surface diagram, (b) is K i Output characteristic surface diagram, (c) is K d Output characteristic surface graph; Figure 6 This is a simulation result diagram of the path tracking of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions of the present invention; Figure 7 This is a comparison chart of the rudder angle changes of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions of the present invention; Figure 8 This is a comparison chart of the lateral tracking error of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions of the present invention; Fig. 9 A comparison diagram of the expected heading angle calculated by the ALOS and conventional LOS guidance strategies in the straight-flight state of the present invention; Fig.10 This is a comparison diagram of the control effects of the fuzzy adaptive PID and the conventional PID in the direct flight state of the present invention; Fig.11 This is a simulation result diagram of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithm path tracking under complex paths of the present invention; Fig.12 This is a simulation result diagram of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithm path tracking under extreme sea conditions in the present invention. DETAILED DESCRIPTION

[0035] The following will be combined with the drawings in the embodiments of the present application to further clearly and completely describe the technical solutions in the embodiments of the present application. It should be noted that the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of this application.

[0036] In order to make the invention objectives, technical solutions and advantages of the present application clearer, the embodiments of the present application are further described in detail in conjunction with the drawings in the specification: In order to more clearly understand the above-mentioned objects, features and advantages of the present invention, the advantages of the present invention will be further illustrated by comparing the embodiments in conjunction with the drawings and specific implementation methods.

[0037] The present invention proposes a ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID. The specific process of the method is as follows: Figure 1 The steps of the method are described in detail: Step S1: preset the path points and obtain the real-time coordinates of the ship through the ship motion mathematical model and real-time ship heading angle; Furthermore, the step S1 obtains the real-time coordinates of the ship The specific process is as follows: S11, such as Figure 2 As shown, establish an inertial coordinate system O 0-X 0 Y 0 (fixed coordinate system is fixed on the surface of the earth, referred to as fixed system) and ship coordinate system o-xy (the ship coordinate system moves with the ship body, referred to as the dynamic system) to describe the movement of the ship at sea, where: O 0 -X 0 Y 0 coincides with a certain horizontal plane on the sea surface, O 0 is a reference point on the sea surface. O 0 -X 0 The axis points due north. O 0 -Y 0 The axis points due east; o-xy The ship coordinate system is fixed to the hull. o is the ship's center of gravity, ox The axis is the ship's center of gravity pointing to the bow. oy The axis is the ship's center of gravity pointing to starboard; The state variables of the ship are defined to describe the motion state of the ship in the inertial coordinate system. The formula of the ship state variables is:

[0038] In the formula , and are the north position, east position and heading of the ship in the inertial coordinate system, u is the longitudinal velocity of the ship along the longitudinal direction, v is the lateral velocity along the sway direction, r is the angular velocity of the ship's bow, U is the true speed of the ship (the speed at the center of gravity of the ship), and the calculation formula is: , is the ship heading angle; The ship heading angle The sideslip angle of the ship Measuring heading angle with the ship The ship heading angle is The calculation formula is:

[0039] The ship's sideslip angle The formula is ; in, is the transformation matrix from the ship coordinate system to the inertial coordinate system, which is used to transform the velocity component in the ship coordinate system into the velocity component in the inertial coordinate system. The formula of the transformation matrix is:

[0040] S12, simplifying the kinematic model of the unmanned surface ship into a three-degree-of-freedom ship motion mathematical model, and obtaining the formula of the three-degree-of-freedom ship motion mathematical model is:

[0041] Among them, M is the mass and inertia matrix of the ship, and the definition formula of matrix M is:

[0042] Where m is the mass of the ship, is the ship’s moment of inertia, is the position of the ship’s center of gravity in the hull coordinate system, , , , , = are the additional mass and additional moment of inertia, which is equivalent to the surface ship having a larger mass and moment of inertia when it moves in a fluid medium than when it moves in a vacuum; Among them, the lateral force , longitudinal force , lateral moment term The calculation formulas are:

[0043]

[0044]

[0045] In the formula , , , , , , , , , , , , Parameters such as are the hydrodynamic derivatives of the ship, is the propeller thrust, For external interference, is the ship's rudder angle; S13, combined with the above formula derivation, the complete ship motion equation formula is expressed as:

[0046] According to the mathematical model of ship motion, and Calculate and obtain the real-time coordinates of the ship , and obtain the real-time ship heading angle.

[0047] Step S2, calculate the actual desired heading angle of the ship through the adaptive line of sight guidance strategy ; Preferably, the desired heading angle of the ship is obtained in step S2. The specific process is as follows: S21, when the ship is moving along the path point P on the horizontal plane n and P n+1 When the ship is traveling along the preset path, the along-track error of the current position of the ship from the preset path is and lateral track error The calculation formula is:

[0048]

[0049] in, is X 0 The distance difference between the current position of the ship and the next position in the axial direction, is Y 0 The distance difference between the current position and the next position of the ship in the axis coordinate direction, is the default path and X 0 The angle formed by the axes; S22, in order to complete the ship path tracking task, the path tracking control target is achieved by reducing the lateral tracking distance between the ship's real-time position and the preset path. Since the ship path tracking task is not limited by time, the error along the track is not considered. , then the lateral track error Approaching 0, lateral track error The calculation formula is:

[0050] S23, since most ships are underdriven ships and it is difficult for them to directly complete the swaying motion, an adaptive line of sight guidance strategy system is introduced to reduce the lateral track error of the ship. Converted into the desired heading angle of the ship, indirectly completing the path tracking control task of the ship; the ship's line of sight vector is the line of sight distance from the current position to the preset path The point ends at the line of sight vector passing through the preset path and the inertial coordinate system X 0 The angle formed is the desired heading angle required in the ship guidance process. , then the expected heading angle The calculation formula is:

[0051] The expected heading angle of the ship is The preset path intersects X 0 The angle formed The angle between the sight vector and the preset path composed of; S24, the velocity u generated by the longitudinal motion of the ship during actual navigation also generates the velocity v generated by the transverse motion, which causes the actual motion direction of the ship to have an angular deviation from the heading. , which is the ship's sideslip angle , then the actual expected heading angle of the ship is The calculation formula is:

[0052] The precise sideslip angle of the ship is The calculation formula is , is the true heading angle of the ship; In the actual navigation of the ship, there is noise in the process of sensor receiving and transmitting signals, which makes the ship receive the sensor data observation and measure the heading angle of the ship. Compared with the actual heading angle of the ship There is a noise deviation between , then the calculation formula is:

[0053] Because the ship can only obtain the observed heading angle during the tracking process , so when the ship completes the tracking task, the ship's measured heading angle is equal to the expected heading angle, that is , then the ship's observed heading is obtained The actual heading of the ship The sideslip angle between the two is defined as the generalized sideslip angle of the ship. , the formula is:

[0054] S25, since the precise sideslip angle of the ship is difficult to calculate accurately, the generalized sideslip angle of the ship including the actual sideslip angle and compass noise is estimated through the adaptive line of sight guidance strategy , to more accurately solve the actual expected angle of the ship The formula is:

[0055]

[0056] in, represents the true speed of the ship and , is the adaptive gain, is the line of sight distance on the ship's preset path.

[0057] Step S3: compare the real-time heading angle with the actual desired heading angle Subtract to get the heading deviation , and the heading deviation rate is calculated ; Preferably, the heading deviation obtained in step S3 is and heading deviation rate The specific process is as follows: like Figure 3 As shown in the figure, in the ship heading control system, the ship's heading angle is measured The actual expected heading angle Subtract to get the heading deviation and heading deviation rate , the calculation formula is:

[0058]

[0059] In the formula Heading deviation The differential of represents the change of heading deviation over time, is the time differential, which represents a small change in time.

[0060] Step S4: Set the heading deviation , heading deviation rate The input is fed into the fuzzy adaptive PID motion control module, and the fuzzy control algorithm outputs the three gain parameters of the PID controller in real time. , and ; Preferably, the three gain parameters of the PID output in step S4 are: , and The specific process is as follows: S41, based on deviation and error rate of change Two input quantities and one output quantity establish a reference model controller; with the help of the controller, the membership function is determined, which can be described by numerical or functional methods, and the controllability of the fuzzy controller depends on the geometric shape of the membership function. Its sharp area has high resolution, making the output more sensitive to input changes, while the smooth area has low resolution, making the output less sensitive to input changes, thereby improving the stability of the system; according to the above selection of smooth and continuous Gaussian shape membership function as input quantity, the output quantity is in the positive and negative area of ​​the fuzzy set membership function, and the Gaussian function is used to output three membership function fuzzy variables , and ; Among them, The membership function of gain input and output is as follows: Figure 4 As shown in a, Figure 4 As shown in b and Figure 4 As shown in c; S42, fuzzy adaptive PID control system combines fuzzy control theory with PID controller, uses process control knowledge and expert experience to generate control rules, and adjusts the three membership function fuzzy variables of PID control system ( , and ), establish the corresponding fuzzy control rules, and obtain the corresponding fuzzy control rules, as shown in Table 1, Table 2 and Table 3; the fuzzy inference engine is the core of the fuzzy control system. It is based on the fuzzy concept and obtains fuzzy control information through fuzzy inference and logical inference rules. Then, by applying fuzzy input and control rules, the fuzzy inference engine solves the fuzzy relationship equation and obtains the three gain parameter fuzzy quantities , and ; Table 1 Fuzzy Control Rules

[0061] Table 2 Fuzzy Control Rules

[0062] Table 3 Fuzzy Control Rules

[0063] S43, fuzzy quantity is generated through fuzzy reasoning, but actual control requires clear quantity, so the fuzzy quantity must be converted into clear quantity, and then expressed in the domain through scale transformation to obtain the actual control value; the fuzzy control quantity can be converted into a clear gain parameter in the theoretical domain through the area center method , , , and the area center method is not only formulated but also uses more information, has a wide range of practical applications, and has good robustness to parameter changes and noise disturbances. Finally, a complete , , The characteristic surface of the inference output, such as Figure 6 As shown in a, Figure 6 As shown in b and Figure 6 As shown in c.

[0064] Step S5: set the three gain parameters , and , and heading deviation Input the PID controller of the fuzzy adaptive PID motion control module, and output the ship's rudder angle after calculation ; Preferably, in step S5, the ship rudder angle is calculated The specific process is as follows: The deviation And the three gain parameters output by the fuzzy controller , and Input into the PID controller, and after the proportional, integral, and differential steps, the rudder angle of the PID controller can be obtained. The formula is:

[0065] In the formula, It is the control command rudder angle value generated by the controller in the heading control system; represents the proportional gain of the controller, represents the integral gain of the controller, Represents the differential gain of the controller. These three are the adjustment parameters that determine the control effect of the controller.

[0066] Step S6: adjust the ship's rudder angle Input the mathematical model of ship motion to calculate the output or update the motion state of the ship at the next moment, forming a closed loop to continuously update the state of the ship at the next moment so that the ship continues to approach the preset path point; Step S7, after updating the ship motion status information, determine whether it has reached the preset path point: (1) If the preset path point is not reached, return to the adaptive line of sight guidance strategy link to continue to complete the path tracking task process; (2) If the preset path point is reached, determine whether the current path point is the last coordinate point of the preset path: ① If it is not the last coordinate point of the preset path, switch to the next path point to continue the path tracking task; ② If it is the last coordinate point of the preset path, the process ends and the path tracking task is completed.

[0067] Experimental simulation verification analysis of the present invention: Figure 6 The path tracking simulation results of the present invention when the ship is sailing straight on the water surface under normal sea conditions are compared with those of other algorithms. It can be seen from the figure that the ship path tracking system combining the adaptive line of sight (ALOS) guidance strategy with the fuzzy adaptive PID control algorithm has good tracking accuracy. Compared with other algorithms, it has the advantages of smooth steering, rapid response, and small heading overshoot. In particular, compared with other algorithms, the tracking system of the present invention can make the ship return to the planned path faster, and can better meet the daily tracking tasks of unmanned ships or unmanned boats.

[0068] Matching Figure 7 It can be seen that the path tracking system that combines the ALOS guidance strategy with the fuzzy adaptive PID control algorithm has a smoother change in the ship's rudder angle than other algorithms. Under the same sea conditions, the overshoot of the tracking system of the present invention is much smaller than that of other algorithms. At about 15 seconds of simulation time, the rudder angle adjustment of the tracking system of the present invention tends to 0, and the ship is basically stable at this time, while the rudder angle changes of other algorithms are still relatively drastic at this time, and the rudder angle changes do not tend to stabilize until 46 seconds, indicating that the tracking system of the present invention significantly alleviates the problem of excessive change rate generated in the early stage of ship motion control.

[0069] Depend on Figure 8 It can be seen that the lateral tracking error of the tracking system of the present invention approaches 0 at around 20 seconds, and its lateral distance error is controlled within 0.1m after 20 seconds, and the error is basically 0 at around 49 seconds; while other algorithms such as conventional line of sight guidance combined with PID control have a large error change overshoot, and the lateral error distance basically reaches 0.5m after 20 seconds, while the ALOS combined with the PID algorithm makes the early overshoot large and the error is up to 0.55m, so it can be obtained that the tracking system of the present invention has a higher tracking accuracy.

[0070] Fig. 9 Under the premise of matching with the basic PID control algorithm, the adaptive line of sight (ALOS) and conventional line of sight (LOS) guidance strategies are used for simulation respectively. It can be seen that the expected heading angle calculated and output by the adaptive line of sight (ALOS) guidance strategy is smoother and reaches stability in about 45s. Compared with conventional guidance, it can achieve the expected heading stability faster, and the output value is smaller than the LOS strategy output. It is also more in line with the actual working conditions when applied to the rudder angle of surface ships (basically maintained at [-45°, 45°]).

[0071] Fig.10The figure is a comparison of the control effects of fuzzy adaptive PID and conventional PID in the straight-line state. It can be seen from the simulation results that due to the setting of the fuzzy algorithm in the fuzzy adaptive PID control system, its motion control significantly alleviates the problem of excessive change rate generated in the early stage of control. The bow angle of the ship under the fuzzy adaptive PID control system changes more gently, and the overshoot phenomenon caused by the bow pitch is also well weakened. The maximum offset of the ship's bow angle is about 6°, while the maximum offset under traditional PID control is nearly 30°, indicating that fuzzy PID control can achieve more ideal control effect in the heading control of ship motion, and compared with traditional PID control, the fuzzy adaptive PID control of the present invention takes less time to reach a stable state, and the response is very rapid.

[0072] Fig.11 It is a comparison of the tracking effects of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under complex paths. As shown in the figure, after the route environment changes, the control system of the present invention greatly reduces the overshoot phenomenon in the tracking process compared with other algorithms, and where there are large corners in the route, the tracking system of the present invention can turn in advance to track the planned route with smoother heading changes, while other algorithms such as conventional LOS combined with traditional PID algorithms have repeatedly experienced unstable tracking and drastic heading changes, indicating that the control system of the present invention can output PID gain parameters in real time according to the environment and has good environmental adaptability.

[0073] Fig.12 The comparison results of ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms in tracking effect under extreme sea conditions are shown in Fig.12 The simulated sea conditions are added with water flow interference of 30° and 0.1m / s, and compass noise interference of random change value in the range of [-5°, 5°]. It can be seen from the figure that in the face of changing routes and adding interference, the tracking system of the present invention still has the advantages of small overshoot and rapid response, and the heading transition is smooth at large corners of the route, and the track changes smoothly, while other algorithms have problems such as early heading overshoot and drastic heading changes during the later route transition. This also shows that the ship path tracking system of the present invention has good anti-interference ability and environmental adaptability.

[0074] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0075] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID, characterized in that: include: Step S1: preset the path points and obtain the real-time coordinates of the ship through the ship motion mathematical model and real-time ship heading angle; S11, establish inertial coordinate system O 0 -X 0 Y 0 and the ship coordinate system o-xy To describe the motion of a ship at sea, O 0 - X 0 Y 0 coincides with a certain horizontal plane on the sea surface, O 0 is a reference point on the sea surface. O 0 -X 0 The axis points due north. O 0 -Y 0 The axis points due east; o-xy The ship coordinate system is fixed to the hull. o is the ship's center of gravity, ox The axis is the ship's center of gravity pointing to the bow. oy The axis is the ship's center of gravity pointing to starboard; The formula for defining the ship state variables is: ； In the formula , and are the north position, east position and heading of the ship in the inertial coordinate system, u is the longitudinal velocity of the ship along the longitudinal direction, v is the lateral velocity along the sway direction, r is the angular velocity of the ship's bow, U is the true speed of the ship, and the calculation formula is: , is the ship heading angle; The ship heading angle The sideslip angle of the ship Angle to ship heading The ship heading angle is The calculation formula is: ； The ship's sideslip angle The formula is ; in, is the transformation matrix from the ship coordinate system to the inertial coordinate system, The transformation matrix is ​​expressed in the form of: ； S12, simplifying the kinematic model of the unmanned surface ship into a three-degree-of-freedom ship motion mathematical model, and obtaining the formula of the three-degree-of-freedom ship motion mathematical model is: ； Among them, M is the mass and inertia matrix of the ship, and the formula for defining the matrix M is: ； Where m is the mass of the ship, is the ship’s moment of inertia, is the position of the ship's center of gravity in the hull coordinate system, , , , , is the additional mass and additional moment of inertia; Among them, the lateral force , longitudinal force , lateral moment term The calculation formulas are: ； ； ； In the formula , , , , , , , , , , , , The parameter is the hydrodynamic derivative of the ship, is the propeller thrust, For external interference, is the ship's rudder angle; S13, combined with the above formula derivation, the complete mathematical model formula of ship motion is expressed as: ； According to the mathematical model of ship motion, and Calculate and obtain the real-time coordinates of the ship , and obtain the real-time ship heading angle; Step S2, calculate the actual desired heading angle of the ship through the adaptive line of sight guidance strategy ; S21, when the ship is moving along the path point P on the horizontal plane n and P n+1 When the ship is traveling along the preset path, the along-track error of the current position of the ship from the preset path is and lateral track error The calculation formula is: ； ； in, is X 0 The distance difference between the current position of the ship and the next position in the axial direction, is Y 0 The distance difference between the current position and the next position of the ship in the axis coordinate direction, is the default path and X 0 The angle formed by the axes; S22, the path tracking control goal is achieved by reducing the lateral tracking distance between the ship's real-time position and the preset path, that is, the along-track error is not considered. , then the lateral track error Approaching 0, lateral track error The calculation formula is: ； S23, introduces an adaptive line-of-sight guidance strategy system to reduce the ship's lateral track error Converted to the desired heading angle of the ship; the ship's sight vector is the sight distance from the current position to the preset path The point ends at the line of sight vector passing through the preset path and the inertial coordinate system X 0 The angle formed is the desired heading angle required in the ship guidance process. , then the expected heading angle The calculation formula is: ； The expected heading angle of the ship is The preset path intersects X 0 The angle formed The angle between the sight vector and the preset path composed of; S24, in actual navigation, in addition to the speed u generated by the longitudinal motion, the ship also generates a speed v due to the transverse motion, which causes the actual motion direction of the ship to have an angle deviation from the heading, which is the sideslip angle of the ship. , then the actual expected heading angle of the ship is The formula is: ； The precise sideslip angle of the ship is The calculation formula is , is the true heading angle of the ship; Actual ship heading angle Measuring heading angle with the ship There is noise deviation , and its calculation formula is: ； Because the ship can only obtain the observed heading angle during the tracking process , so when the ship completes the tracking task, the ship's measured heading angle is equal to the expected heading angle, that is , then the ship's observed heading is obtained The actual heading of the ship The sideslip angle between the two is defined as the generalized sideslip angle of the ship. , the formula is: ； S25, more accurately solves the actual ship's desired angle through adaptive line of sight guidance strategy The formula is: ； ； in, represents the true speed of the ship and , is the adaptive gain, is the sight distance on the ship’s pre-set path; Step S3: Compare the ship heading angle with the actual desired heading angle Subtract to get the heading deviation , and the heading deviation rate is calculated ; Step S4: Set the heading deviation , heading deviation rate The input is fed into the fuzzy adaptive PID motion control module, and the fuzzy control algorithm outputs the three gain parameters of the PID controller in real time. , and ; S41, based on deviation and error rate of change Two input quantities and one output quantity establish a reference model controller; with the help of the controller to determine the membership function, a smooth and continuous Gaussian shape membership function is selected as the input quantity, and the output quantity is within the range of the fuzzy set membership function in the positive and negative regions, and the Gaussian function is used to output three membership function fuzzy variables , and ; S42, establish the corresponding fuzzy control rules, obtain fuzzy control information through fuzzy reasoning and logical reasoning rules, and then solve the fuzzy relationship equation with the help of fuzzy reasoning machine by applying fuzzy input and control rules to obtain three gain parameter fuzzy quantities. , and ; S43, the fuzzy gain parameter is converted into a clear gain parameter within the theoretical domain by the area center method , , Finally, the complete three gain parameters are plotted. , , The characteristic surface of the reasoning output; Step S5: set the three gain parameters , and , and heading deviation Input the PID controller of the fuzzy adaptive PID motion control module, and output the ship's rudder angle after calculation ; The heading deviation And three gain parameters , and Input into the PID controller, and after the proportional, integral, and differential steps, the rudder angle is obtained. The formula is: ； In the formula, represents the proportional gain of the controller, represents the integral gain of the controller, represents the differential gain of the controller; Step S6: adjust the ship's rudder angle Input the mathematical model of ship motion to calculate the output or update the motion state of the ship at the next moment, forming a closed loop to continuously update the state of the ship at the next moment so that the ship continues to approach the preset path point; Step S7, after updating the ship motion status information, determine whether it has reached the preset path point: (1) If the preset path point is not reached, return to the adaptive line of sight guidance strategy link to continue to complete the path tracking task process; (2) If the preset path point is reached, determine whether the current path point is the last coordinate point of the preset path: ① If it is not the last coordinate point of the preset path, switch to the next path point to continue the path tracking task; ② If it is the last coordinate point of the preset path, the process ends and the path tracking task is completed.

2. A ship path tracking method based on adaptive line of sight guidance and fuzzy adaptive PID according to claim 1, characterized in that: The heading deviation is obtained in step S3 and heading deviation rate The specific steps are as follows: Measure the heading angle of the ship The actual desired heading angle output by the adaptive line of sight guidance strategy Subtract to get the heading deviation and heading deviation rate , the calculation formula is expressed as: ； ； In the formula Heading deviation The differential of represents the change of heading deviation over time, is the time differential, which represents a small change in time.

Images