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

Abstract

The present 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. First, preset path points to obtain real-time coordinates; acquire the actual desired heading angle and the ship's observed heading angle, subtract them to obtain the heading deviation, and calculate the heading deviation rate; input the heading deviation and the heading deviation rate into the fuzzy adaptive PID motion control module, and output gain parameters by the fuzzy control algorithm; input the gain parameters and the PID controller to calculate the ship's rudder angle; update the motion state by inputting the motion mathematical model, form a closed-loop to update the motion state to make the ship approach the preset path points; judge the ship path after updating the state information. The present invention calculates the ship's desired heading in real time through adaptive line-of-sight guidance and adjusts the ship's sideslip angle and ship control parameters, and automatically adjusts the control parameters in real time by means of the fuzzy adaptive PID motion control module to achieve precise motion control, so that the ship sails along the preset planned path.

Description

Technical Field

[0001] The present invention belongs to the technical field of ship path tracking, and particularly 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 a ship is performing a tracking navigation task, accurately tracking a predetermined path is crucial for ensuring navigation safety and execution efficiency, and precise motion control plays a vital role in the general maritime operations where the ship realizes motorization and agility. A complete motion control system for an intelligent ship usually consists of a ship heading guidance and a motion control subsystem. The guidance subsystem is responsible for outputting ship attitude control commands to achieve kinematic control objectives in a 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. Generally speaking, ship attitude commands are called guidance laws. For an autonomous surface vehicle (ASV), the heading guidance law is usually combined with a motion control algorithm.

[0003] Traditional ship path tracking methods for autonomous surface vehicles usually rely on algorithms such as line-of-sight (LOS) guidance and PID control. However, these methods have certain limitations in practical applications. Especially in a dynamic environment, the accuracy and stability of path tracking are often affected.

[0004] In the ship heading guidance subsystem, the line-of-sight guidance algorithm, as a common navigation control strategy, has been widely used in ship path tracking. Its 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, that is, the desired heading angle, into the motion control algorithm. Although the line-of-sight guidance algorithm has good real-time performance and simplicity of implementation, due to ignoring some dynamic characteristics of the ship, especially the ship's sideslip angle that appears in the case of lateral drift and speed change, the classical line-of-sight guidance algorithm has steady-state errors and overshoot phenomena, which affect the accuracy and stability of path tracking.

[0005] In the ship motion control subsystem, the PID control algorithm is often used to adjust the heading angle to improve the ship's path tracking performance. The PID control effectively adjusts the rudder angle through proportional, integral, and derivative control strategies. However, 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] Aiming at the deficiencies in 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, including the following steps:

[0007] Step S1, preset path points, and obtain the real-time coordinates of the ship through the ship motion mathematical model and the real-time ship heading angle;

[0008] Step S2, calculate the actual desired heading angle of the ship through the adaptive line-of-sight guidance strategy ;

[0009] Step S3, subtract the real-time heading angle from the actual desired heading angle to obtain the heading deviation , and at the same time calculate the heading deviation rate ;

[0010] Step S4, input the heading deviation , the heading deviation rate 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 ;

[0011] Step S5, input the three gain parameters , and , as well as the heading deviation into the PID controller of the fuzzy adaptive PID motion control module, and calculate and output the ship rudder angle ;

[0012] Step S6, input the ship rudder angle into the ship motion mathematical model to calculate and output or update the motion state of the ship at the next moment, form a closed loop to continuously update the ship state at the next moment, and make the ship continuously approach the preset path point;

[0013] Step S7, after updating the ship motion state information, judge whether the preset path point is reached:

[0014] (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;

[0015] (2) If the preset path point is reached, judge whether the current path point is the last coordinate point of the preset path:

[0016] ① If it is not the last coordinate point of the preset path, switch to the next path point to continue the path tracking task;

[0017] ② If it is the last coordinate point of the preset path, the process ends and the path tracking task is completed.

[0018] Preferably, the specific process of obtaining the real-time coordinates of the ship in step S1 is as follows:

[0019] S11. Establish an inertial coordinate system O 0 -X 0 Y 0 and a ship-fixed coordinate system o-xy to describe the movement of the ship at sea. Among them, O 0 -X 0 Y 0 coincides with a certain horizontal plane on the sea surface, O 0 is a certain reference point on the sea surface, O 0 -X 0 the x-axis points to the due north direction, O 0 -Y 0 the y-axis points to the due east direction; o-xy The ship-fixed coordinate system is fixed to the ship's hull, o is the center of gravity of the ship, ox the x'-axis is from the center of gravity of the ship to the bow, oy the y'-axis is from the center of gravity of the ship to the starboard side;

[0020] The formula for defining the ship's state variables is:

[0021]

[0022] In the formula , and are respectively the northward position, eastward position and heading of the ship in the inertial coordinate system, u is the longitudinal velocity of the ship along the surge direction, v is the lateral velocity along the sway direction, r is the heading angular velocity of the ship's yaw, U is the true velocity of the ship, and the calculation formula is , is the ship's heading angle;

[0023] Among them, the ship's heading angle is composed of the ship's sideslip angle and the ship's measured heading angle . Then the calculation formula for the ship's heading angle is:

[0024]

[0025] In the formula, the ship sideslip angle has the formula of ;

[0026] Among them, is the transformation matrix from the ship-fixed coordinate system to the inertial coordinate system, The formula representation of the transformation matrix is:

[0027]

[0028] S12, simplify the kinematic model of the unmanned surface vehicle into a three-degree-of-freedom ship motion mathematical model, and the formula of the three-degree-of-freedom ship motion mathematical model is:

[0029]

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

[0031]

[0032] In the formula, m is the mass of the ship, is the moment of inertia of the ship, is the position of the ship's center of gravity in the hull coordinate system, 、 、 、 、 are the added mass and added moment of inertia;

[0033] Among them, the transverse force term 、the longitudinal force term 、the transverse moment term have the calculation formulas respectively as:

[0034]

[0035]

[0036]

[0037] In the formula 、 、 、 、 、 、 、 、 、 、 、 、 The parameter is the hydrodynamic derivative of the ship, is the propeller thrust, is the external disturbance, is the ship's rudder angle;

[0038] S13, combined with the above formula derivation, the complete mathematical model formula of ship motion is expressed as:

[0039]

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

[0041] Preferably, the specific process of obtaining the actual desired heading angle of the ship in step S2 is as follows:

[0042] S21, when the ship is traveling along the preset path connecting the path points and on the horizontal plane, the along-track error and the cross-track error of the current position of the ship from the preset path are calculated as follows:

[0043]

[0044]

[0045] Among them, is the distance difference between the current position and the next position of the ship in the X 0 axis direction, is the distance difference between the current position and the next position of the ship in the Y 0 axis coordinate direction, is the included angle formed by the preset path and the X 0 axis;

[0046] S22, achieve the path tracking control goal by reducing the cross-track distance between the real-time position of the ship and the preset path, that is, without considering the along-track error , then the cross-track error tends to 0, and the calculation formula of the cross-track error is:

[0047]

[0048] S23. Introduce an adaptive line-of-sight guidance strategy system to convert the lateral track error of the ship into the desired course angle of the ship; the line-of-sight vector of the ship starts from the current position and ends at the point at the line-of-sight distance on the preset path. The angle formed by the line-of-sight vector passing through the preset path and the inertial coordinate system X 0 is the desired course angle required during the ship's guidance process . Then the calculation formula for the desired course angle is:

[0049]

[0050] In the formula, the desired course angle of the ship is composed of the included angle X 0 formed by the intersection of the preset path and and the included angle formed by the line-of-sight vector and the preset path;

[0051] S24. During actual navigation, in addition to the speed u generated by surge motion, the ship also has a speed v generated by sway motion, which causes an angular deviation between the actual motion direction of the ship and the course, that is, the ship's sideslip angle . Then the calculation formula for the actual desired course angle of the ship is:

[0052]

[0053] In the formula, the calculation formula for the accurate sideslip angle of the ship is , is the true heading angle of the ship;

[0054] The actual true heading angle of the ship and the measured heading angle of the ship have a noise deviation between them, and its calculation formula is:

[0055]

[0056] Since the ship can only obtain the observed heading angle during the tracking process, when the ship completes the tracking task, the measured course angle of the ship is equal to the desired course angle, that is . At this time, the sideslip angle between the observed heading of the ship and the actual course of the ship is obtained, and it is defined as the generalized sideslip angle of the ship, and its formula is:

[0057]

[0058] S25, Solve the desired angle of the ship more precisely through the adaptive line-of-sight guidance strategy The formula is as follows:

[0059]

[0060]

[0061] Wherein, represents the true speed of the ship and , is the adaptive gain, is the line-of-sight distance on the preset path of the ship.

[0062] Preferably, the heading deviation and the heading deviation rate obtained in the step S3 are as follows:

[0063] Subtract the measured bow angle of the ship from the actual desired heading angle output by the adaptive line-of-sight guidance strategy to obtain the heading deviation and the heading deviation rate . The calculation formula is expressed as:

[0064]

[0065]

[0066] In the formula is the heading deviation differential, representing the change in the heading deviation over time, is the time differential, representing a small change in time.

[0067] Preferably, the specific process of outputting the three gain parameters , and of the PID controller in the step S4 is as follows:

[0068] S41, Based on the deviation and the error change rate two input quantities and one output quantity, establish a reference model controller; determine the membership function with the help of the controller, select a smooth and continuous Gaussian-shaped membership function as the input quantity, and the output quantity is within the membership function range of the fuzzy set in the positive and negative regions, then use the Gaussian function to output three membership function fuzzy variables , and ;

[0069] S42. Establish corresponding fuzzy control rules, obtain fuzzy control information through fuzzy inference and logical inference rules, and then, by applying fuzzy input variables and control rules, solve the fuzzy relation equation with the help of a fuzzy inference engine to obtain three fuzzy gain parameters 、 and ;

[0070] S43. Convert the fuzzy gain parameters into clear gain parameters within the theoretical range through the area centroid method 、 、 , and finally draw a complete characteristic surface inferred and output by the three gain parameters 、 、 .

[0071] Preferably, the specific process of calculating the ship rudder angle in step S5 is as follows:

[0072] Input the course deviation and the three gain parameters 、 and into the PID controller. After passing through the proportional, integral, and differential links, the formula for obtaining the rudder angle is:

[0073]

[0074] wherein, represents the proportional gain of the controller, represents the integral gain of the controller, represents the differential gain of the controller.

[0075] 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 ship's sideslip angle to correctly calculate the ship's desired course angle and takes into account the ship's motion control accuracy; by introducing the adaptive line-of-sight guidance strategy algorithm, the sideslip angle generated during the ship's navigation can be accurately estimated, and based on this, the correct desired course angle of the ship can be calculated; by introducing the fuzzy adaptive PID control algorithm package, the ship can automatically adjust the control parameters of the fuzzy adaptive PID in real time under different environments or sea conditions to achieve precise motion control, enabling the ship to obtain the correct desired course angle under different environments or sea conditions and automatically adjust the control parameters in real time to achieve the precise control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 is the flow chart of the ship path tracking system of the present invention;

[0077] Figure 2 Schematic diagram of the ship's water surface coordinates and adaptive line-of-sight guidance for the present invention;

[0078] Figure 3 Schematic diagram of the ship path tracking system based on adaptive line-of-sight guidance and fuzzy adaptive PID control for the present invention;

[0079] Figure 4 For Kp Curves of membership functions for gain input heading deviation, heading deviation rate of change, and output, where (a) is the membership function curve for input Kp heading deviation, (b) is the membership function curve for input Kp heading deviation rate of change, and (c) is the membership function curve for output Kp ;

[0080] Figure 5 For K p , K i and K d output characteristic surface diagrams, where (a) is the K p output characteristic surface diagram, (b) is the K i output characteristic surface diagram, and (c) is the K d output characteristic surface diagram;

[0081] Figure 6 Simulation result diagram of path tracking of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions for the present invention;

[0082] Figure 7 Comparison diagram of rudder angle changes of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions for the present invention;

[0083] Figure 8 Comparison diagram of lateral tracking errors of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms under normal sea conditions for the present invention;

[0084] Figure 9 Comparison diagram of calculating the desired heading angle between the ALOS and conventional LOS guidance strategies in the straight-ahead state for the present invention;

[0085] Figure 10 Comparison diagram of the control effects between fuzzy adaptive PID and conventional PID in the straight-ahead state for the present invention;

[0086] Figure 11This is the simulation result diagram of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms for path tracking under complex paths in the present invention;

[0087] Figure 12 This is the simulation result diagram of the ALOS guidance strategy combined with fuzzy adaptive PID control and other algorithms for path tracking under extreme sea conditions in the present invention. Specific implementation manner

[0088] Next, the technical solutions in the embodiments of the present application will be further clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. It should be noted that the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative efforts belong to the scope of protection of the present application.

[0089] In order to make the invention purpose, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the accompanying drawings of the specification: In order to more clearly understand the above purposes, 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 manners.

[0090] The present invention proposes a ship path tracking method based on adaptive line-of-sight guidance and fuzzy adaptive PID. The specific process of this method is as Figure 1 shown, and the steps of this method will be described in detail:

[0091] Step S1, preset path points, and obtain the real-time coordinates and the real-time heading angle of the ship through the ship motion mathematical model;

[0092] Furthermore, the specific process of obtaining the real-time coordinates of the ship in step S1 is as follows:

[0093] S11, as Figure 2 shown, establish an inertial coordinate system O 0 -X 0 Y 0 (The fixed coordinate system is fixed on the earth's surface, simply referred to as the fixed system) and the ship-fixed coordinate system o-xy (The ship-fixed coordinate system moves together with the hull, simply referred to as the moving system) to describe the movement of the ship at sea. Among them, O 0 -X 0 Y 0 coincides with a certain horizontal plane on the sea surface, O0 is a reference point on the sea surface, O 0 -X 0 the z-axis points to the due north direction, O 0 -Y 0 the x-axis points to the due east direction; o-xy The ship-fixed coordinate system is fixed to the ship's hull, o is the center of gravity of the ship, ox the x-axis is from the center of gravity of the ship to the bow, oy the y-axis is from the center of gravity of the ship to the starboard side;

[0094] Define the state variables of the ship to describe the motion state of the ship in the inertial coordinate system. The formula for the ship state variables is:

[0095]

[0096] In the formula, , and are respectively the northward position, eastward position and heading of the ship in the inertial coordinate system, u is the longitudinal velocity of the ship along the surge direction, v is the lateral velocity along the sway direction, r is the heading angular velocity of the ship's yaw. U is the true velocity of the ship (the velocity at the center of gravity of the ship), and the calculation formula is , is the ship's heading angle;

[0097] Among them, the ship's heading angle is composed of the ship's sideslip angle and the ship's measured heading angle . Then the calculation formula for the ship's heading angle is:

[0098]

[0099] In the formula, the ship's sideslip angle has the formula ;

[0100] Among them, is the transformation matrix from the ship-fixed coordinate system to the inertial coordinate system, which is used to convert the velocity components in the ship-fixed coordinate system into the velocity components in the inertial coordinate system, The formula representation form of the transformation matrix is:

[0101]

[0102] S12. Simplify the kinematic model of the unmanned surface vessel into a three-degree-of-freedom vessel motion mathematical model. The formula for the three-degree-of-freedom vessel motion mathematical model is:

[0103]

[0104] Among them, M is the mass and inertia matrix of the vessel. The defining formula for matrix M is:

[0105]

[0106] In the formula, m is the mass of the vessel, is the moment of inertia of the vessel, is the position of the vessel's center of gravity in the hull coordinate system, , , , , etc. are the added mass and added moment of inertia, which are equivalent to the vessel having a greater mass and moment of inertia when moving in a fluid medium than when moving in a vacuum;

[0107] Among them, the transverse force term , the longitudinal force term , and the transverse moment term The calculation formulas are respectively:

[0108]

[0109]

[0110]

[0111] In the formula , , , , , , , , , , , , etc. The parameters are the hydrodynamic derivatives of the vessel, is the propeller thrust, is the external disturbance, is the rudder angle of the vessel;

[0112] S13. Combining the above formula derivation, the complete vessel motion equation formula is expressed as:

[0113]

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

[0115] Step S2, calculate the actual desired heading angle of the ship through the adaptive line-of-sight guidance strategy;

[0116] Preferably, the specific process of obtaining the desired heading angle of the ship in step S2 is as follows:

[0117] S21, when the ship is traveling along the preset path connecting the path points P n and P n+1 on the horizontal plane, the along-track error and the cross-track error of the current position of the ship from the preset path are calculated as follows:

[0118]

[0119]

[0120] Among them, is the distance difference between the current position and the next position of the ship in the X 0 axis direction, is the distance difference between the current position and the next position of the ship in the Y 0 axis coordinate direction, is the included angle formed by the preset path and the X 0 axis;

[0121] S22, to complete the ship path tracking task, achieve the path tracking control goal by reducing the cross-track distance between the real-time position of the ship and the preset path, and since the ship path tracking task is not limited by time, that is, the along-track error is not considered, then the cross-track error tends to 0, and the calculation formula of the cross-track error is:

[0122]

[0123] S23, since most ships are underactuated ships and it is difficult to directly complete the sway motion, an adaptive line-of-sight guidance strategy system is introduced to convert the cross-track error of the ship into the desired heading angle of the ship, and indirectly complete the ship path tracking control task; the line-of-sight vector of the ship starts from the current position to the line-of-sight distance on the preset path The point at X 0 ends, and the line-of-sight vector passes through the preset path and the inertial coordinate system , then the desired course angle has the following calculation formula:

[0124]

[0125] In the formula, the desired course angle of the ship is formed by the intersection of the preset path with X 0 the included angle formed and the included angle formed by the line-of-sight vector and the preset path consists of;

[0126] S24. During the actual navigation of the ship, the speed u generated by the surge motion and the speed v generated by the sway motion cause an angular deviation between the actual motion direction of the ship and the course , which is the sideslip angle of the ship , then the actual desired course angle of the ship has the following calculation formula:

[0127]

[0128] In the formula, the accurate sideslip angle of the ship has the following calculation formula , is the true heading angle of the ship;

[0129] During the actual navigation of the ship, there is noise in the process of sensor receiving and transmitting signals, resulting in a noise deviation between the measured heading angle of the ship obtained by the ship's sensor data observation and the actual true heading angle of the ship, and its calculation formula is:

[0130]

[0131] Since the ship can only obtain the observed heading angle during the tracking process, when the ship completes the tracking task, the measured course angle of the ship is equal to the desired course angle, that is , and at this time, the observed heading of the ship and the actual course of the ship are obtained. The sideslip angle between them is defined as the generalized sideslip angle of the ship, and its formula is:

[0132]

[0133] S25. Since it is difficult to accurately calculate the exact ship sideslip angle, the generalized ship sideslip angle that includes the actual sideslip angle and compass noise is estimated through an adaptive line-of-sight guidance strategy to more accurately solve for the actual desired angle of the ship The formula is as follows:

[0134]

[0135]

[0136] where represents the true speed of the ship and , is the adaptive gain, is the line-of-sight distance on the preset path of the ship

[0137] Step S3. Subtract the real-time heading angle from the actual desired heading angle to obtain the heading deviation , and at the same time calculate the heading deviation rate ;

[0138] Preferably, the specific process of obtaining the heading deviation and the heading deviation rate in step S3 is as follows:

[0139] As Figure 3 shown, in the ship heading control system, subtract the measured ship heading angle from the actual desired heading angle to obtain the heading deviation and the heading deviation rate . The calculation formula is:

[0140]

[0141]

[0142] In the formula is the heading deviation differential, representing the change in the heading deviation over time, is the time differential, representing a small change in time

[0143] Step S4. Input the heading deviation , the heading deviation rate into the fuzzy adaptive PID motion control module, and the fuzzy control algorithm outputs the three gain parameters , and of the PID controller in real time;

[0144] Preferably, the three gain parameters of the PID output in step S4 , and are as follows:

[0145] S41. Based on the deviation and the error change rate , establish a reference model controller with two input variables and one output variable; determine the membership function with the aid of the controller. This function 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 regions have high resolution, making the output more sensitive to input changes, while its smooth regions have low resolution, making the output less sensitive to input changes, thereby improving the stability of the system; select the smooth and continuous Gaussian-shaped membership function as the input variables according to the above, and for the membership functions of the fuzzy sets of the output variable in the positive and negative regions, use the Gaussian function to output three membership function fuzzy variables , and ; among them, the membership functions of the gain input and output are as shown in a of , as shown in b of Figure 4 , and as shown in c of Figure 4 ; Figure 4

[0146] S42. The fuzzy adaptive PID control system combines fuzzy control theory with the PID controller, generates control rules by using process control knowledge and expert experience, adjusts the three membership function fuzzy variables ( , and ) of the PID control system, establishes corresponding fuzzy control rules, and obtains 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 fuzzy concepts, obtains fuzzy control information through fuzzy inference and logical inference rules, and then, by applying fuzzy inputs and control rules, the fuzzy inference engine solves the fuzzy relation equation to obtain three fuzzy quantities of the gain parameters , and ;

[0147] Table 1 Fuzzy control rules

[0148]

[0149] Table 2 Fuzzy control rules

[0150]

[0151] Table 3 Fuzzy control rules​

[0152]

[0153] S43. After fuzzy inference, a fuzzy quantity is generated. However, actual control requires a crisp quantity. Therefore, the fuzzy quantity must be converted into a crisp quantity and then represented within the domain through scale transformation to obtain the actual control value. The fuzzy control quantity can be converted into a crisp gain parameter within the theoretical domain by the area centroid method and and , and the area centroid method is not only formulated but also uses more information, has a wide range of practical applications, has good robustness to parameter changes and noise disturbances, and finally a complete characteristic surface inferred and output by the three gain parameters and and is plotted, as shown in a of Figure 6 , b of Figure 6 , and c of Figure 6 .

[0154] Step S5. The three gain parameters and and , as well as the course deviation , are input into the PID controller of the fuzzy adaptive PID motion control module, and after calculation, the ship rudder angle is output;

[0155] Preferably, the specific process of calculating the ship rudder angle in step S5 is as follows:

[0156] The deviation and the three gain parameters output by the fuzzy controller and and are input into the PID controller. After passing through the proportional, integral, and derivative links, the rudder angle of the PID controller can be obtained, and the formula is:

[0157]

[0158] In the formula, is the control command rudder angle value generated by the controller in the course control system; represents the proportional gain of the controller, represents the integral gain of the controller, represents the derivative gain of the controller, and these three are the adjustment parameters that determine the control effect of the controller.

[0159] Step S6. The ship 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;

[0160] Step S7, after updating the ship motion status information, determine whether it has reached the preset path point:

[0161] (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;

[0162] (2) If the preset path point is reached, determine whether the current path point is the last coordinate point of the preset path:

[0163] ① If it is not the last coordinate point of the preset path, switch to the next path point to continue the path tracking task;

[0164] ② If it is the last coordinate point of the preset path, the process ends and the path tracking task is completed.

[0165] Experimental simulation verification analysis of the present invention:

[0166] 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.

[0167] 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.

[0168] Depend on Figure 8It 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.

[0169] Figure 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°]).

[0170] Figure 10 The 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.

[0171] Figure 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.

[0172] Figure 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 Figure 12In the simulated sea conditions, water flow interference with a direction of 30° and a flow velocity of 0.1 m / s was added, and the compass noise interference was a randomly varying value in the range of [-5°, 5°]. It can be seen from the figure that in the face of changing routes and added interference, the tracking system of the present invention still has advantages such as small overshoot and rapid response. Moreover, the course transition is gentle at large turning points of the route, and the track changes smoothly. In contrast, other algorithms have problems such as large course overshoot in the early stage and violent course changes during the later route transition, which also shows that the ship path tracking system of the present invention has good anti-interference ability and environmental adaptability.

[0173] Although the preferred embodiments of the present application have been described, those skilled in the art can make additional changes and modifications to these embodiments once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed as including the preferred embodiments as well as all changes and modifications that fall within the scope of the present application.

[0174] 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 equivalent technologies, 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