Unmanned surface vehicle large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation

Abstract

The application provides a kind of unmanned ship large curvature path tracking control method based on curvature self-adaption and improved disturbance observation, belongs to the technical field of unmanned ship autonomous navigation and path.The application includes loading reference path and initializing parameter;In each control cycle, lateral error and path curvature are calculated, path curvature adopts multi-stage calculation and smoothing method;Based on lateral error and curvature, preview point is determined by adaptive preview distance calculation method;Disturbance is estimated using improved state disturbance observer SDO, observer bandwidth is adjusted by double-factor driven adaptive mechanism;Control amount is calculated by control law combining bias and disturbance;Ship state is updated until path tracking is completed.The application can dynamically adjust preview distance and observer bandwidth, effectively estimate and compensate disturbance, improve the precision and robustness of unmanned ship in large curvature path tracking, and ensure the accuracy of system state observation and the stability of control performance when switching between large curvature path and straight line segment.

Description

Technical Field

[0001] This invention relates to the field of path tracking technology, and in particular to a method for unmanned surface vessels with large curvature path tracking control based on curvature adaptation and improved disturbance observation. Background Technology

[0002] Unmanned surface vessels (USVs) are unmanned devices capable of autonomous navigation on water. With the widespread application of USVs in fields such as marine monitoring, environmental exploration, and military reconnaissance, the requirements for the accuracy and stability of USV path tracking are increasing. Path tracking is one of the core technologies for autonomous navigation of USVs. In complex aquatic environments, USVs often face the challenge of tracking paths with large curvatures, such as complex waterways in ports and winding routes around islands.

[0003] Traditional path tracking methods suffer from several problems when handling paths with high curvature: Firstly, fixed aiming distances are difficult to adapt to rapid changes in curvature, leading to unreasonable aiming point selection and decreased tracking accuracy on high curvature paths. Secondly, there is a lack of effective estimation and compensation mechanisms for path deviation disturbances and total system disturbances caused by curvature changes during path tracking. This makes it difficult for unmanned surface vessels (USVs) to quickly adjust their attitude and speed when subjected to external disturbances such as currents and waves, resulting in unstable tracking performance. Furthermore, traditional control algorithms often use fixed bandwidth settings, failing to dynamically adjust according to the actual path curvature and tracking error, making it difficult to achieve optimal control performance under different operating conditions.

[0004] Therefore, there is an urgent need for an unmanned surface vessel path tracking method that can adapt to paths with large curvature, effectively suppress disturbances, and achieve adaptive control. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a method for unmanned surface vessel (USV) path tracking control with high curvature based on curvature adaptation and improved disturbance observation.

[0006] The technical solution of this invention is: a large curvature path tracking control method for unmanned surface vessels based on curvature adaptation and improved disturbance observation, comprising the following steps:

[0007] S1) A three-degree-of-freedom dynamic model of the unmanned surface vessel is used to describe the position and attitude changes of the vessel in the global coordinate system;

[0008] S2) Load the predefined reference path and initialize the control parameters and state variables;

[0009] S3) Calculate the lateral error and path curvature of the unmanned surface vessel's current position within each control cycle;

[0010] S4) Based on the lateral error and path curvature obtained in step S3), the aiming distance is determined by an adaptive aiming distance calculation method, and the aiming point is selected according to the aiming distance to calculate the desired heading angle;

[0011] S5) The total disturbance of the system is estimated by an improved state disturbance observer SDO, in which the SDO uses a curvature-error dual-factor driven observer bandwidth adaptive mechanism to dynamically adjust the bandwidth;

[0012] S6) Combining the desired heading angle, the current heading angle, and the disturbance estimated in step S5), the control quantity is calculated using a control law that combines PID control and disturbance compensation to drive the unmanned surface vessel.

[0013] S7) Update the state variables of the unmanned surface vessel and repeat steps S3)-S6) until path tracking is completed.

[0014] Preferably, in step S1), the expression for the three-degree-of-freedom dynamic model of the unmanned surface vessel is:

[0015] ; (1)

[0016] In the formula, This is the quality matrix; The Coriolis force matrix; Here is the damping matrix; This is the velocity vector in the hull coordinate system; This represents the control force and torque vector; These represent the longitudinal and lateral velocities in the hull coordinate system, respectively. Indicates the angular velocity of the hull; These represent the longitudinal thrust, lateral force, and bow roll moment in the hull coordinate system, respectively. This indicates the transpose operation.

[0017] Preferably, in step S3), the current position of the unmanned surface vessel is set as point. Its geodetic coordinate system The coordinates in are The heading angle is The predefined target path consists of a series of path points. The components are designed to achieve precise heading control; when the unmanned surface vessel is in... , In between, that is, from the starting point and the end point The lateral error of the constructed directed line segment It is the vertical projection point of the corresponding unmanned surface vessel's position on the path. According to the principle of vector cross product, the lateral error is... The formula for calculation is:

[0018] ; (8)

[0019] In the formula, ) is a vector with vector The two-dimensional cross product; if the result is positive, it indicates that the point... In directed line segments To the left of; if the result is negative, it indicates the point. In directed line segments The right side;

[0020] It is a path segment The length of is used for normalization, so that the "signed area" result of the molecular cross product is transformed into the "signed distance".

[0021] Preferably, in step S3), the path curvature is calculated using a multi-level curvature calculation and smoothing method. Specifically, it includes the following steps:

[0022] S31) Using the path point corresponding to the current position of the unmanned surface vessel as the center, select a total of 11 path points before and after the current position to form a calculation window;

[0023] S32) Apply a third-order Savitzky-Golay filter to the coordinates of the path points within the window for smoothing.

[0024] S33) The first and second derivatives of the smoothed coordinates are calculated using the central difference method, and then the curvature of each point within the window is calculated.

[0025] S34) Apply a 5-point median filter to the calculated curvature sequence;

[0026] S35) Take the weighted average of 5 points near the center point of the median-filtered curvature sequence as the initial curvature estimate;

[0027] S36), limiting curvature to Within the range; These are the minimum curvature and the maximum curvature, respectively.

[0028] S37), apply smoothing factors sequentially. The exponentially weighted moving average (EWMA) filter is then applied, followed by an 11-point sliding window average (SMA) filter with linearly increasing weights; that is:

[0029] ; (9)

[0030] ; (10)

[0031] In the formula, , The curvatures of the EWMA and SMA filters are respectively. The weights are linearly increasing. For curvature buffer queue; The curvature value is the filtered curvature value after the exponentially weighted moving average of the previous time step.

[0032] S38) The EWMA filtering result and the SMA filtering result are weighted and fused to obtain the final path curvature estimate. ,Right now:

[0033] ; (11)

[0034] in, The fusion weight coefficient takes values ​​between (0, 1).

[0035] Preferably, in step S4), the aiming distance is determined using an adaptive aiming distance calculation method based on the lateral error and path curvature, specifically including the following steps:

[0036] S41), based on the current lateral error Calculate the lateral error factor ,Right now:

[0037] ;(12)

[0038] In the formula, This is an adjustment coefficient used to adjust the factor's sensitivity to lateral errors.

[0039] S42), based on the horizontal error factor Calculate the basic aiming distance ,Right now:

[0040] ; (13)

[0041] In the formula, , These are the maximum aiming distance and the minimum aiming distance, respectively.

[0042] S43), based on path curvature Calculate curvature factor ,Right now:

[0043] ;(14)

[0044] In the formula, This is the aiming distance adjustment factor; This is the preset curvature scaling factor;

[0045] S44) Calculate the initial aiming distance by combining the base aiming distance and the curvature factor. ,Right now:

[0046] ; (15)

[0047] S45) Perform nonlinear mapping optimization on the initial aiming distance, using an exponential mapping function to improve sensitivity at small aiming distances, i.e.:

[0048] ; (16)

[0049] In the formula, This represents the optimized aiming distance, where γ is a dimensionless aiming distance mapping gain factor.

[0050] And the pre-aiming distance after transformation Limit the amplitude to ensure it remains within a reasonable range:

[0051] ; (17)

[0052] S46) Pre-aiming distance for amplitude limiting using an adaptive filtering mechanism Smoothing is performed to obtain the final aiming distance. ,Right now:

[0053] ; (18)

[0054] ; (19)

[0055] In the formula, This is the aiming distance from the previous moment; These are the adaptive filter coefficients. and These are the curvature of the current path point and the absolute value of the unmanned surface vessel's lateral error, respectively. These are the basic filter coefficients; Minimum filter coefficients; and These are the curvature influence factor and the error influence factor, respectively.

[0056] Preferably, in step S4), based on the pre-aiming distance... Select a pre-aiming point on the reference path Calculate the desired heading angle ,Right now:

[0057] ; (20)

[0058] In the formula, Pre-aiming point The coordinates.

[0059] Preferably, in step S5), the total system disturbance is estimated using the improved state disturbance observer SDO, which specifically includes the following steps:

[0060] S511), Receiving lateral error As input; and introduce error saturation smoothing processing, that is:

[0061] ; (twenty one)

[0062] ; (twenty two)

[0063] In the formula, This represents the lateral error after saturation treatment; Indicates saturation error Lateral error after nonlinear smoothing; This is the saturation threshold; and This is the smoothing adjustment coefficient.

[0064] (S512) The bandwidth is dynamically adjusted using an observer bandwidth adaptive mechanism driven by curvature and error; the final bandwidth is obtained. for:

[0065] ; (28)

[0066] In the formula, This refers to the limited bandwidth. and These are the preset minimum and maximum observer bandwidths, respectively;

[0067] S513), based on system transverse error The third-order state perturbation observer is designed as follows:

[0068] ; (29)

[0069] In the formula, For observer state variables, The estimation of the corresponding system's transverse error is given by the rate of change of the transverse error. The estimate; This is an estimate of the total disturbance of the system; this "total disturbance" is an extended state that collectively represents the combined impact of all unmodeled dynamics and external environmental disturbances (such as wind, waves, and currents) on the system's lateral error dynamics, and can be directly used for feedforward compensation. , , For observer state variables; The first derivative; , , These are the observer parameters; This is the final bandwidth after dynamic adjustment;

[0070] S514), the observer state is updated using the fourth-order Runge-Kutta method, i.e.:

[0071] ; (30)

[0072] ; (31)

[0073] ; (32)

[0074] ; (33)

[0075] ; (34)

[0076] ; (35)

[0077] ;(36)

[0078] ; (37)

[0079] In the formula, ; The corresponding state equation is... Sampling time; , , , These represent the estimates of the rate of change of state at the start, midpoint (twice), and end of the time step, respectively. , , These are the values ​​of temporary state variables used to calculate different slopes;

[0080] S515) Introduces a multiple state constraint mechanism to prevent state abrupt changes and divergence, namely:

[0081] , ; (38)

[0082] ; (39)

[0083] In the formula, Indicates the maximum value limit of the state; Indicates the maximum rate of change of state; This represents the state value at the previous moment;

[0084] (S516) Adaptive low-pass filtering is applied to the disturbance estimate, and the filter coefficients are dynamically adjusted according to the disturbance variation amplitude to finally output a high-precision, low-noise total system disturbance; that is:

[0085] ; (40)

[0086] ; (41)

[0087] In the formula, Adaptive filter coefficients; These are adjustable parameters; This is the estimated value of the total disturbance. This is the filtered disturbance estimate from the previous time step; The total system disturbance is estimated by SDO and then smoothed by adaptive filtering.

[0088] Preferably, in step S6), the desired heading angle, the current heading angle, and the total disturbance estimated in step S5) are combined to calculate the control quantity using a control law that combines PID control and disturbance compensation, thereby driving the unmanned surface vessel (USV) to navigate; specifically as follows:

[0089] S61) The longitudinal speed of the unmanned surface vessel is controlled by a proportional-integral-derivative PID controller with integral anti-saturation function. To follow the target speed ,Right now:

[0090] ; (42)

[0091] In the formula, To output thrust; Speed ​​control gain; For longitudinal velocity error; For target speed; This represents the current longitudinal velocity; , , These are the proportional, integral, and differential gains of the velocity loop; the integral term. Upper and lower limits are set to prevent system overshoot and instability caused by integral saturation;

[0092] S62), based on desired heading angle Compared with the current actual heading angle Error between Feedback control term for calculating heading error ,Right now:

[0093] ; (43)

[0094] In the formula, , These are the integral and differential gains of the heading loop, respectively; For proportional gain;

[0095] ; (44)

[0096] In the formula, Base proportional gain; The curvature of the current path point; and This is the adjustment coefficient;

[0097] S63) Calculate the disturbance compensation term based on the total disturbance estimate estimated by the SDO observer. ,Right now:

[0098] ; (45)

[0099] In the formula, It is the total disturbance compensation gain; It is the total system disturbance estimated by SDO and smoothed by adaptive filtering; It is the set saturation boundary value; It is a dimensionless positive constant used to adjust the curve shape of the saturation function; It is a symbolic function;

[0100] S65) Calculate the final rudder angle based on the feedback control term of the heading error and the disturbance compensation term of the SDO. ,Right now:

[0101] ; (46)

[0102] in, This is the feedback control term for the heading error. For the disturbance compensation term from SDO;

[0103] S66), regarding the final rudder angle To impose restrictions, namely:

[0104] ; (47)

[0105] In the formula, This is the final rudder angle output after limitation;

[0106] Based on the rudder angle and thrust, the control force and torque are calculated, namely:

[0107] ; (48)

[0108] ; (49)

[0109] ; (50)

[0110] In the formula, The center of gravity; , , These are the longitudinal control force, the lateral control force, and the yaw control torque, respectively.

[0111] The beneficial effects of this invention are as follows:

[0112] 1. This invention is based on an adaptive pre-aiming distance calculation method that dynamically couples path curvature and lateral error. By constructing a nonlinear mapping function and an adaptive filtering mechanism, the unmanned surface vessel can adjust the pre-aiming distance in advance under high curvature paths, effectively reducing tracking errors and significantly improving the tracking accuracy of high curvature paths.

[0113] 2. The improved State Disturbance Observer (SDO) of this invention employs error saturation smoothing, fourth-order Runge-Kutta numerical integration, and multiple state constraint mechanisms, combined with a disturbance estimation smoothing strategy for adaptive filter coefficients. This enables high-precision and stable estimation of the total system disturbance during high curvature path tracking, significantly enhancing the system's ability to suppress external disturbances and its robustness, allowing the unmanned surface vessel to maintain good path tracking performance even in complex sea conditions.

[0114] 3. The adaptive observer bandwidth adjustment mechanism of the present invention, through nonlinear combination calculation of curvature factor and error factor, combined with bandwidth change rate limit and upper limit constraint, enables the observer bandwidth to be adaptively adjusted according to path characteristics and tracking status. The bandwidth is increased in high curvature path segments to enhance disturbance suppression capability, and the bandwidth is reduced in straight segments to reduce the impact of observation noise, effectively solving the problem of inconsistent performance of conventional fixed bandwidth observers under different working conditions.

[0115] 4. This invention combines Savitzky-Golay filtering, median filtering, and exponentially weighted moving average filtering to achieve high-precision calculation and smoothing of path curvature, providing reliable curvature information for adaptive aiming distance calculation and observer bandwidth adjustment, thereby enhancing the system's adaptability to different path characteristics and control accuracy. Attached Figure Description

[0116] Figure 1 This is a schematic flowchart of the method of the present invention;

[0117] Figure 2 This is a comparison chart of path tracking performance in Embodiment 2 of the present invention;

[0118] Figure 3 This is a comparison diagram of lateral errors in Embodiment 2 of the present invention;

[0119] Figure 4This is a comparison chart of the path tracking performance under disturbance in Embodiment 3 of the present invention;

[0120] Figure 5 Comparison of lateral errors under disturbance in Embodiment 3 of the present invention;

[0121] Figure 6 Schematic diagram of the estimated values ​​of the SDO observer in Embodiment 3 of the present invention. Detailed Implementation

[0122] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings:

[0123] Example 1

[0124] like Figure 1 As shown, this embodiment provides a method for unmanned surface vessel (USV) path tracking control with high curvature based on curvature adaptation and improved disturbance observation, including the following steps:

[0125] S1) A three-degree-of-freedom dynamic model of the unmanned surface vessel (USV) is used to describe the position and attitude changes of the vessel in the global coordinate system; the expression of the three-degree-of-freedom dynamic model of the USV is:

[0126] ; (1)

[0127] In the formula, This is the quality matrix; The Coriolis force matrix; Here is the damping matrix; This is the velocity vector in the hull coordinate system; This represents the control force and torque vector; These represent the longitudinal and lateral velocities in the hull coordinate system, respectively. Indicates the angular velocity of the hull; These represent the longitudinal thrust, lateral force, and bow roll moment in the hull coordinate system, respectively. This indicates the transpose operation.

[0128] The aforementioned dynamic model describes the position and attitude changes of the hull in the global coordinate system; that is:

[0129] ; (2)

[0130] In the formula, This indicates the position of the unmanned surface vessel in the global coordinate system. Indicates the heading angle; Angular velocity of heading; , Positions The first derivative.

[0131] mass matrix Coriolis force matrix and damping matrix It can be represented as:

[0132] ; (3)

[0133] ; (4)

[0134] ; (5)

[0135] In the formula, For the mass of the unmanned surface vessel; This represents the moment of inertia of the unmanned surface vessel about the z-axis (vertical axis); The x-coordinate representing the center of gravity of the unmanned surface vessel; Indicates longitudinal added mass; Indicates lateral added mass; This indicates the additional moment of inertia of the bow roll; This represents the lateral force caused by the bow roll acceleration; This represents the yaw moment caused by lateral acceleration; For linear damping terms, For nonlinear damping terms

[0136] ; (6)

[0137] ; (7)

[0138] In the formula, , , These represent the longitudinal linear damping coefficient, the lateral linear damping coefficient, and the yaw linear damping coefficient, respectively. , These represent the yaw moment coefficient caused by lateral velocity and the lateral force coefficient caused by yaw angular velocity, respectively. , , These represent the longitudinal second-order nonlinear damping coefficient, the lateral second-order nonlinear damping coefficient, and the second-order lateral force coefficient caused by the bow roll angular velocity, respectively. This represents the longitudinal higher-order nonlinear damping coefficient; , These represent the second-order yaw moment coefficient and the second-order yaw nonlinear damping coefficient caused by the lateral velocity, respectively. , These are the coupling nonlinear damping coefficients.

[0139] S2) Load the predefined reference path and initialize the control parameters and state variables;

[0140] S3) Calculate the lateral error and path curvature of the unmanned surface vessel's current position within each control cycle;

[0141] In this embodiment, the current position of the unmanned surface vessel is set as point. Its geodetic coordinate system The coordinates in are The heading angle is The predefined target path consists of a series of path points. The components are designed to achieve precise heading control; when the unmanned surface vessel is in... , In between, that is, from the starting point and the end point The lateral error of the constructed directed line segment It is the vertical projection point of the corresponding unmanned surface vessel's position on the path. According to the principle of vector cross product, the lateral error mentioned above... The formula for calculation is:

[0142] ; (8)

[0143] In the formula, ) is a vector with vector The two-dimensional cross product; if the result is positive, it indicates that the point... In directed line segments To the left of; if the result is negative, it indicates the point. In directed line segments The right side;

[0144] It is a path segment The length of is used for normalization, so that the "signed area" result of the molecular cross product is transformed into the "signed distance".

[0145] This formula calculates the current position of the unmanned surface vessel. To path segment The signed perpendicular distance along the straight line. The sign of this value indicates the direction in which the unmanned surface vessel (USV) deviates from the path segment: if the USV deviates from... Towards Using the navigation direction as a reference, a positive value indicates that the unmanned surface vessel (USV) is on the left side of the path segment, while a negative value indicates that the USV is on the right side. This signed error value provides a clear steering basis for subsequent heading and attitude control.

[0146] Preferably, in step S3), the path curvature is calculated using a multi-level curvature calculation and smoothing method. Specifically, it includes the following steps:

[0147] S31) Using the path point corresponding to the current position of the unmanned surface vessel as the center, select a total of 11 path points before and after the current position to form a calculation window;

[0148] S32) Apply a third-order Savitzky-Golay filter to the coordinates of the path points within the window for smoothing.

[0149] S33) The first and second derivatives of the smoothed coordinates are calculated using the central difference method, and then the curvature of each point within the window is calculated.

[0150] S34) Apply a 5-point median filter to the calculated curvature sequence;

[0151] S35) Take the weighted average of 5 points near the center point of the median-filtered curvature sequence as the initial curvature estimate;

[0152] S36), limiting curvature to Within the range; These are the minimum curvature and the maximum curvature, respectively; where... , ;

[0153] S37), apply smoothing factors sequentially. The exponentially weighted moving average (EWMA) filter is then applied, followed by an 11-point sliding window average (SMA) filter with linearly increasing weights; that is:

[0154] ; (9)

[0155] ; (10)

[0156] In the formula, , The curvatures of the EWMA and SMA filters are respectively. The weights are linearly increasing. For curvature buffer queue; The curvature value is the filtered curvature value after the exponentially weighted moving average of the previous time step.

[0157] S38) The EWMA filtering result and the SMA filtering result are weighted and fused to obtain the final path curvature estimate. ,Right now:

[0158] ; (11)

[0159] in, The fusion weight coefficient takes values ​​between (0, 1).

[0160] This embodiment employs a larger window size, improving the numerical stability of curvature calculation; it also introduces higher-order Savitzky-Golay filtering and the central difference method, improving the accuracy of derivative calculation; it combines median filtering, EWMA, and SMA to form a multi-level filtering architecture, effectively suppressing the influence of noise and outliers; and it adopts a weighted fusion strategy to balance filtering smoothness and response speed.

[0161] S4) Based on the lateral error and path curvature obtained in step S3), the adaptive aiming distance calculation method is used to determine the aiming distance, and the aiming point is selected according to the aiming distance to calculate the desired heading angle; specifically, the following steps are included:

[0162] S41), based on the current lateral error Calculate the lateral error factor ,Right now:

[0163] ;(12)

[0164] In the formula, This is an adjustment coefficient used to adjust the factor's sensitivity to lateral errors.

[0165] S42), based on the horizontal error factor Calculate the basic aiming distance ,Right now:

[0166] ; (13)

[0167] In the formula, , These are the maximum and minimum aiming distances, respectively; in this embodiment, It can be 3-5 times the length of the boat. It can be taken as 1-1.5 times the length of the boat. , ;

[0168] S43), based on path curvature Calculate curvature factor ,Right now:

[0169] ;(14)

[0170] In the formula, This is the aiming distance adjustment factor; This is the preset curvature scaling factor;

[0171] This function is an exponentially decaying function. When the curvature is small, the function value is close to 1; when the curvature increases, the function value decreases rapidly.

[0172] S44) Calculate the initial aiming distance by combining the base aiming distance and the curvature factor. ,Right now:

[0173] ; (15)

[0174] S45) Perform nonlinear mapping optimization on the initial aiming distance, using an exponential mapping function to improve sensitivity at small aiming distances, i.e.:

[0175] ; (16)

[0176] In the formula, The optimized aiming distance is represented by γ, which is a dimensionless aiming distance mapping gain factor. This parameter controls the strength of the nonlinear mapping. This function is an improved exponential function. Compared with the traditional linear mapping, it can better maintain the aiming distance within a reasonable range and improve the sensitivity at small aiming distances.

[0177] And the pre-aiming distance after transformation Limit the amplitude to ensure it remains within a reasonable range:

[0178] ; (17)

[0179] S46) Pre-aiming distance for amplitude limiting using an adaptive filtering mechanism Smoothing is performed to obtain the final aiming distance. ,Right now:

[0180] ; (18)

[0181] ; (19)

[0182] In the formula, This is the aiming distance from the previous moment; These are the adaptive filter coefficients. and These are the curvature of the current path point and the absolute value of the unmanned surface vessel's lateral error, respectively. These are the basic filter coefficients; The minimum filter coefficient; and These are the curvature influence factor and the error influence factor, respectively; both are positive constants, used to adjust the sensitivity of the filter coefficient to changes in curvature and error. When the curvature is large or the lateral error is large, the filter coefficient is decreased to improve the response speed; when the curvature is small and the lateral error is small, the filter coefficient is increased to enhance the smoothing effect.

[0183] And based on the pre-aiming distance Select a pre-aiming point on the reference path Calculate the desired heading angle ,Right now:

[0184] ; (20)

[0185] In the formula, Pre-aiming point The coordinates;

[0186] The heading controller is based on the current heading. and expected heading angle Deviation between Calculate the rudder angle command and drive the unmanned surface vessel toward the pre-aimed point.

[0187] S5) The total system disturbance is estimated using an improved state disturbance observer (SDO), where the SDO dynamically adjusts the bandwidth using an observer bandwidth adaptive mechanism driven by both curvature and error factors; specifically, it includes the following steps:

[0188] S511), Receiving lateral error As input; and introduce error saturation smoothing processing, that is:

[0189] ; (twenty one)

[0190] ; (twenty two)

[0191] In the formula, This represents the lateral error after saturation treatment; Indicates saturation error Lateral error after nonlinear smoothing; This is the saturation threshold; = 0.2 and = 4.0 is the smoothing adjustment coefficient.

[0192] S512) Employs an observer bandwidth adaptive mechanism driven by curvature and error to dynamically adjust the bandwidth; specifically including the following steps:

[0193] S5121) Calculate the error factor using the hyperbolic tangent function. ,Right now:

[0194] ; (twenty three)

[0195] In the formula, It is the hyperbolic tangent function;

[0196] S5122) Calculating curvature factor using an exponential function ,Right now:

[0197] ; (twenty four)

[0198] S5123) Introducing nonlinear terms to calculate the basic bandwidth By introducing a nonlinear term, the bandwidth gain is improved for large curvature and large error conditions, i.e.:

[0199] ; (25)

[0200] in, This is the baseline value for the observer bandwidth. For bandwidth adaptive coefficients; =0.15 is the weighting coefficient of the curvature-error coupling term;

[0201] The adaptive bandwidth mechanism can dynamically adjust the observer performance based on path characteristics and tracking status, increasing bandwidth on high-curvature path segments to enhance disturbance suppression capabilities and decreasing bandwidth on straight segments to reduce the impact of observation noise.

[0202] S5124) Introduces a bandwidth change rate limit to prevent sudden bandwidth changes, based on the bandwidth of the previous control cycle. and maximum allowable rate of change Calculate the allowed bandwidth range for the current period by considering the desired bandwidth. By limiting the bandwidth to the permissible range, a rate-limited bandwidth is obtained. ,Right now:

[0203] (26)

[0204] ;(27)

[0205] In the formula, This represents the maximum allowable bandwidth variation value. The bandwidth of the observer at the previous time step; This refers to the limited bandwidth. This means restricting x to Within the range;

[0206] Finally, an absolute upper and lower limit is applied to the bandwidth to ensure it always stays within a stable range, thus obtaining the final usable bandwidth. :

[0207] ; (28)

[0208] In the formula, This refers to the limited bandwidth. and These are the preset minimum and maximum observer bandwidths, respectively. In this implementation, and , This is the baseline value for the observer bandwidth.

[0209] S513), based on system transverse error The third-order state perturbation observer is designed as follows:

[0210] ; (29)

[0211] In the formula, For observer state variables, Corresponding system lateral error The estimate; Corresponding lateral error rate of change The estimated value; The total disturbance is an estimate of the system's total disturbance. This "total disturbance" is an extended state that collectively represents the combined impact of all unmodeled dynamics and external environmental disturbances (such as wind, waves, and currents) on the system's lateral error dynamics. It can be directly used for feedforward compensation. , , For observer state variables The first derivative; , , ; It is the baseline value for the observer bandwidth;

[0212] S514), the observer state is updated using the fourth-order Runge-Kutta method, i.e.:

[0213] ; (30)

[0214] ; (31)

[0215] ; (32)

[0216] ; (33)

[0217] ; (34)

[0218] ; (35)

[0219] ; (36)

[0220] ; (37)

[0221] In the formula, ; The corresponding state equation is... Sampling time; , , , These represent the estimates of the rate of change of state at the start, midpoint (twice), and end of the time step, respectively. , , These are the values ​​of temporary state variables used to calculate different slopes;

[0222] S515) Introduces a multiple state constraint mechanism to prevent state abrupt changes and divergence, namely:

[0223] , ; (38)

[0224] ; (39)

[0225] In the formula, Indicates the maximum value limit of the state; Indicates the maximum rate of change of state; This represents the state value at the previous moment;

[0226] (S516) Adaptive low-pass filtering is applied to the disturbance estimate, and the filter coefficients are dynamically adjusted according to the disturbance variation amplitude to finally output a high-precision, low-noise total system disturbance; that is:

[0227] ; (40)

[0228] ; (41)

[0229] In the formula, Adaptive filter coefficients; These are adjustable parameters; This is the estimated value of the total disturbance; This is the filtered disturbance estimate from the previous time step; The total system disturbance is estimated by SDO and then smoothed by adaptive filtering.

[0230] S6) Combining the desired heading angle, the current heading angle, and the disturbance estimated in step S5), the control quantity is calculated using a control law combining PID control and disturbance compensation to drive the unmanned surface vessel (USV) navigation; specifically as follows:

[0231] S61) The longitudinal speed of the unmanned surface vessel is controlled by a proportional-integral-derivative PID controller with integral anti-saturation function. To follow the target speed ,Right now:

[0232] ; (42)

[0233] In the formula, To output thrust; Speed ​​control gain; For longitudinal velocity error; For target speed; This represents the current longitudinal velocity; , , These are the proportional, integral, and differential gains of the velocity loop; the integral term. Upper and lower limits are set to prevent system overshoot and instability caused by integral saturation;

[0234] S62), based on desired heading angle Compared with the current actual heading angle Error between Feedback control term for calculating heading error ,Right now:

[0235] ; (43)

[0236] In the formula, , These are the integral and differential gains of the heading loop, respectively; For proportional gain;

[0237] ; (44)

[0238] In the formula, Base proportional gain; The curvature of the current path point; and This is the adjustment coefficient;

[0239] S63) Calculate the disturbance compensation term based on the total disturbance estimate estimated by the SDO observer. ,Right now:

[0240] ; (45)

[0241] In the formula, It is the total disturbance compensation gain; It is the total system disturbance estimated by SDO and smoothed by adaptive filtering; It is the set saturation boundary value; It is a dimensionless positive constant used to adjust the curve shape of the saturation function; It is a symbolic function;

[0242] S65) Calculate the final rudder angle based on the feedback control term of the heading error and the disturbance compensation term of the SDO. ,Right now:

[0243] ; (46)

[0244] in, This is the feedback control term for the heading error. For the disturbance compensation term from SDO;

[0245] S66), regarding the final rudder angle To impose restrictions, namely:

[0246] ; (47)

[0247] In the formula, This is the final rudder angle output after limitation;

[0248] Based on the rudder angle and thrust, the control force and torque are calculated, i.e.:

[0249] ; (48)

[0250] ; (49)

[0251] ; (50)

[0252] In the formula, The center of gravity; , , These are the longitudinal control force, the lateral control force, and the yaw control torque, respectively.

[0253] S7) Update the state variables of the unmanned surface vessel and repeat steps S3)-S6) until path tracking is completed.

[0254] Example 2

[0255] This embodiment compares the method of Embodiment 1 with the traditional LOS method.

[0256] This embodiment selects a complex S-shaped path containing different turning radii (6 meters and 12 meters respectively). This path can effectively test the adaptability and accuracy of the controller when switching between different curvatures.

[0257] Method of Example 1: A large curvature path tracking control method for unmanned surface vessels based on curvature adaptation and improved disturbance observation;

[0258] Traditional LOS method: A fixed aiming distance (set to 5 meters) is used, and a high-performance PID controller is matched to ensure the fairness of the comparison.

[0259] Comparative analysis of results:

[0260] like Figure 2As shown, the tracking performance of the two methods on the same path exhibits a significant difference: The traditional LOS method (blue dashed line) shows severe "angle-cutting" at both turns. Due to its fixed aiming distance, it cannot effectively adjust to changes in path curvature. When entering a curve, the fixed aiming distance causes the UAV to turn inward prematurely, deviating significantly from the desired path. This phenomenon is particularly pronounced at the first curve with a smaller radius and greater curvature.

[0261] The method in Example 1 (solid red line): its tracking trajectory almost perfectly matches the desired path (solid black line). Before entering the curve, the adaptive pre-aiming distance mechanism of the method in Example 1 can automatically shorten the pre-aiming distance by accurately calculating the curvature of the path ahead, enabling the unmanned surface vessel to make timely and precise turning maneuvers. At the same time, the improved SDO can estimate the disturbance caused by the change in path curvature and further suppress overshoot through feedforward compensation by the control law. Therefore, the entire turning process is smooth and precise, with almost no corner cutting or overshoot.

[0262] like Figure 3 As shown, the lateral error curves further quantify the performance gap between the two methods:

[0263] The traditional LOS method shows that when entering the first sharp turn around t=60s, the lateral error rapidly increases to a peak of 1.285 meters. A significant error of approximately -0.8 meters is also observed when entering the second gentle turn around t=160s. The error fluctuates dramatically throughout the entire process.

[0264] The method in Example 1: Throughout the tracking process, the lateral error was consistently kept within a small range. Even at the sharpest curves, the error fluctuations were very slight, demonstrating high precision and stability.

[0265] As shown in Table 1, the average lateral error and maximum lateral error of the method in Example 1 are only 9% and 15.6% of those of the traditional method, respectively. The root mean square lateral error (RMSE) is reduced by 91.2%, which indicates that the overall tracking accuracy and stability of the method in Example 1 far exceed those of the traditional method.

[0266] Table 1 Comparison of Performance Indicators

[0267] Performance indicators Example 1 Method Traditional LOS method Mean lateral error (m) 0.0356 0.3951 Maximum lateral error (m) 0.2 1.285 Root mean square error in the lateral direction (m) 0.0478 0.5453

[0268] Example 2 provides a clear and compelling demonstration of the superiority of the method in Example 1. Traditional LOS methods, due to structural limitations, cannot adapt to dynamic changes in path curvature, resulting in a significant drop in tracking performance at curves. Example 1, however, perfectly solves this problem by introducing the synergistic effect of multiple innovative mechanisms, including adaptive pre-aiming distance, an improved disturbance observer, and adaptive bandwidth. This endows the unmanned surface vessel with the ability to perform high-precision, highly robust tracking on complex paths, demonstrating significant technological advantages.

[0269] Example 3

[0270] Comparison and verification of the anti-interference performance of the method in Example 1 and the traditional LOS method

[0271] This embodiment is conducted on an S-shaped path with varying turning radii. Throughout the simulation, the same time-varying disturbance force and torque, synthesized from multi-frequency sine waves and random noise, are simultaneously applied to both controllers. This setup ensures a fair assessment of the controllers' true performance under continuous dynamic disturbances.

[0272] Comparison method settings:

[0273] The method in Example 1: The core parameters of the SDO (State Disturbance Observer) were optimized for disturbance rejection performance (e.g., observer bandwidth ω_o=2.8, damping ratio ζ_o=0.9).

[0274] Traditional LOS method: Uses a fixed aiming distance and matches it with a high-performance PID controller as the benchmark.

[0275] Results and Analysis

[0276] like Figure 4 As shown, under continuous and complex disturbances, the performance gap between the two methods is further amplified: The traditional LOS method (blue dashed line): its trajectory deviates significantly from the desired path (black solid line). Due to the lack of effective observation and compensation for external disturbances, the unmanned surface vessel is continuously pushed away from its course by the disturbance force, exhibiting severe "angle shearing" not only at turns but also failing to maintain stable navigation on straight sections, displaying significant swaying. The method in Example 1 (red solid line): even under the combined effects of continuous and complex internal and external disturbances, its tracking trajectory still closely follows the desired path. This intuitively demonstrates the powerful disturbance suppression capability and excellent path-keeping capability of this method.

[0277] lateral error curve as shown Figure 5As shown. Traditional LOS method: Its lateral error fluctuates extremely drastically, with a maximum error approaching 1.4 meters, and it maintains large oscillations above ±0.5 meters for most of the time, indicating that its controller is unable to effectively suppress time-varying disturbances. Method of Example 1: The lateral error is significantly suppressed, stabilizing within ±0.25 meters for the vast majority of the time, with a maximum error of only about 0.6 meters. Although small fluctuations are generated to resist disturbances, the amplitude and mean of the error are much smaller than those of the traditional method.

[0278] SDO observer under perturbation, such as Figure 6 As shown, the SDO estimate can accurately track the changing trend and main form of the actual disturbance force. This means that the controller can know the magnitude and direction of the current external disturbance in real time and accurately. Based on this accurate estimate, the disturbance compensation term in the control law can generate a control action of equal magnitude and opposite direction, thereby canceling the influence of the external disturbance at its source and ensuring that the unmanned surface vessel can remain relatively on the desired path.

[0279] The performance comparison under perturbation is shown in Table 2. Under time-varying perturbation, the mean lateral error, maximum lateral error, and root mean square lateral error (RMSE) of the method in Example 1 are reduced by 76.0%, 56.1%, and 75.5%, respectively, compared with the traditional method. This indicates that the method in Example 1 has better anti-interference ability and path tracking robustness.

[0280] Table 2 Comparison of performance indicators under disturbance

[0281] Performance indicators Example 1 Method Traditional LOS method Mean lateral error (m) 0.1191 0.4969 Maximum lateral error (m) 0.6089 1.3877 Root mean square error in the lateral direction (m) 0.1515 0.6182

[0282] Comparative experiments under complex time-varying perturbation environments demonstrate that traditional LOS methods suffer severe performance degradation when faced with complex perturbations in real-world marine environments due to their structural flaws. In contrast, the method in Example 1, through its core improved State Disturbance Observer (SDO), accurately estimates and compensates for external perturbations. Combined with mechanisms such as adaptive pre-aiming and adaptive bandwidth, it maintains extremely high tracking accuracy and system stability even under harsh perturbation environments, showcasing its immense potential and value in practical applications.

[0283] Comprehensive simulation results demonstrate that the control method proposed in this invention significantly outperforms the traditional LOS method. In S-shaped variable curvature path tests, the average lateral error, maximum lateral error, and root mean square error of this invention are reduced by 91.0%, 84.4%, and 91.2%, respectively, compared to the traditional method. Even under harsh conditions with complex time-varying disturbances, these three indicators are still reduced by 76.0%, 56.1%, and 75.5%, respectively, exhibiting excellent robustness.

[0284] The embodiments and descriptions above are merely illustrative of the principles and preferred embodiments of the present invention. Various changes and modifications may be made to the present invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed.

Claims

1. A method for unmanned surface vessel (USV) path tracking control with high curvature based on curvature adaptation and improved disturbance observation, characterized in that, Includes the following steps: S1) A three-degree-of-freedom dynamic model of the unmanned surface vessel is used to describe the position and attitude changes of the vessel in the global coordinate system; S2) Load the predefined reference path and initialize the control parameters and state variables; S3) Calculate the lateral error and path curvature of the unmanned surface vessel's current position within each control cycle; S4) Based on the lateral error and path curvature obtained in step S3), the aiming distance is determined by an adaptive aiming distance calculation method, and the aiming point is selected according to the aiming distance to calculate the desired heading angle; S5) The total system disturbance is estimated by an improved state disturbance observer SDO, in which the SDO uses a curvature-error dual-factor driven observer bandwidth adaptive mechanism to dynamically adjust the bandwidth; S6) Combining the desired heading angle, the current heading angle, and the disturbance estimated in step S5), the control quantity is calculated using a control law that combines PID control and disturbance compensation to drive the unmanned surface vessel. S7) Update the state variables of the unmanned surface vessel and repeat steps S3)-S6) until path tracking is completed; In step S5), the total system disturbance is estimated using the improved state disturbance observer SDO, specifically including the following steps: S511), Receiving Lateral Error As input; and introduce error saturation smoothing processing, that is: ； （21） ； （22） In the formula, It is the saturation threshold, a parameter set according to the actual application scenario; and This is the smoothing adjustment coefficient, used to adjust the shape of the smoothing curve; This represents the lateral error after saturation treatment; Indicates saturation error Lateral error after nonlinear smoothing; S512) Employs an observer bandwidth adaptive mechanism driven by curvature-error dual factors to dynamically adjust the bandwidth; Obtain the final bandwidth for: ； （28） In the formula, This refers to the limited bandwidth. and These are the preset minimum and maximum observer bandwidths, respectively; S513), based on system transverse error The third-order state perturbation observer is designed as follows: ； （29） In the formula, For observer state variables, Estimation of the corresponding system's transverse error, For the rate of change of lateral error The estimate; This is an estimate of the total disturbance of the system; , , For the observer state variable, The first derivative; , , These are the observer parameters; This is the final bandwidth after dynamic adjustment; S514), the observer state is updated using the fourth-order Runge-Kutta method, i.e.: ； （30） ； （31） ； （32） ； （33） ； （34） ； （35） ； （36） ； （37） In the formula, ; The corresponding state equation is... Sampling time; , , , These represent the estimates of the rate of change of state at the start, midpoint, and end of the time step, respectively. , , These are the values ​​of temporary state variables used to calculate different slopes; S515) Introduces a multiple state constraint mechanism to prevent state abrupt changes and divergence, namely: ， ； （38） ； （39） In the formula, Indicates the maximum value limit of the state; Indicates the maximum rate of change of state; This represents the state value at the previous moment; (S516) Adaptive low-pass filtering is applied to the disturbance estimate, and the filter coefficients are dynamically adjusted according to the disturbance variation amplitude to finally output a high-precision, low-noise total system disturbance; that is: ； （40） ； （41） In the formula, Adaptive filter coefficients; These are adjustable parameters; This is the estimated value of the total disturbance; This is the filtered disturbance estimate from the previous time step; The total system disturbance is estimated by SDO and smoothed by adaptive filtering; In step S512), the bandwidth is dynamically adjusted using an observer bandwidth adaptive mechanism driven by both curvature and error factors; specifically, the following steps are included: S5121) Calculate the error factor using the hyperbolic tangent function. ,Right now: ； （23） In the formula, It is the hyperbolic tangent function; S5122) Calculating curvature factor using an exponential function ,Right now: ； （24） S5123) Introducing nonlinear terms to calculate the basic bandwidth By introducing a nonlinear term, the bandwidth gain is improved for large curvature and large error conditions, i.e.: ； （25） in, This is the baseline value for the observer bandwidth. For bandwidth adaptive coefficients; These are the weighting coefficients for the curvature-error coupling term; S5124) Introduces a bandwidth change rate limit to prevent sudden bandwidth changes, based on the bandwidth of the previous control cycle. and maximum allowable rate of change Calculate the allowed bandwidth range for the current period by considering the desired bandwidth. By limiting the bandwidth to the permissible range, the restricted bandwidth is obtained. ,Right now: ；（26） ； （27） In the formula, This represents the maximum allowable bandwidth variation value. This represents the maximum permissible rate of change of the observer bandwidth; The bandwidth of the observer at the previous time step; This refers to the limited bandwidth. This means restricting x to Within the range; Finally, an absolute upper and lower limit is applied to the bandwidth to ensure it always stays within a stable range, thus obtaining the final usable bandwidth. : ； （28） In the formula, This refers to the limited bandwidth. and These are the preset minimum and maximum observer bandwidths, respectively.

2. The unmanned surface vessel (USV) large curvature path tracking control method based on curvature adaptation and improved disturbance observation as described in claim 1, characterized in that: In step S1), the expression for the three-degree-of-freedom dynamic model of the unmanned surface vessel is: ； （1） In the formula, This is the quality matrix; The Coriolis force matrix; Here is the damping matrix; This is the velocity vector in the hull coordinate system; This represents the control force and torque vector; These represent the longitudinal and lateral velocities in the hull coordinate system, respectively. Indicates the angular velocity of the hull; These represent the longitudinal thrust, lateral force, and bow roll moment in the hull coordinate system, respectively. This indicates the transpose operation.

3. The unmanned surface vessel (USV) large curvature path tracking control method based on curvature adaptation and improved disturbance observation as described in claim 1, characterized in that: In step S3), let the current position of the unmanned surface vessel be point [point]. Its geodetic coordinate system The coordinates in are The heading angle is ; A predefined target path consists of a series of waypoints The components are designed to achieve precise heading control; when the unmanned surface vessel is in... , In between, that is, from the starting point and the end point The lateral error of the constructed directed line segment It is the vertical projection point of the corresponding unmanned surface vessel's position on the path. According to the principle of vector cross product, the lateral error mentioned above... The formula for calculation is: ； （8） In the formula, ) is a vector with vector The two-dimensional cross product; if the result is positive, it indicates that the point... In directed line segments To the left of; if the result is negative, it indicates the point. In directed line segments The right side; It is a path segment The length of is used for normalization, so that the "signed area" result of the molecular cross product is transformed into the "signed distance".

4. The unmanned surface vessel (USV) large curvature path tracking control method based on curvature adaptation and improved disturbance observation as described in claim 3, characterized in that: In step S3), the path curvature is calculated using a multi-level curvature calculation and smoothing method. Specifically, it includes the following steps: S31) Using the path point corresponding to the current position of the unmanned surface vessel as the center, select a total of 11 path points before and after the current position to form a calculation window; S32) Apply a third-order Savitzky-Golay filter to the coordinates of the path points within the window for smoothing. S33) The first and second derivatives of the smoothed coordinates are calculated using the central difference method, and then the curvature of each point within the window is calculated. S34) Apply a 5-point median filter to the calculated curvature sequence; S35) Take the weighted average of 5 points near the center point of the median-filtered curvature sequence as the initial curvature estimate; S36), limiting curvature to Within the range; These are the minimum curvature and the maximum curvature, respectively. S37), apply smoothing factors sequentially. The exponentially weighted moving average (EWMA) filter is then applied, followed by an 11-point sliding window average (SMA) filter with linearly increasing weights; that is: ； （9） ； （10） In the formula, , The curvatures of the EWMA and SMA filters are respectively. The weights are linearly increasing. For curvature buffer queue; The curvature value is the filtered curvature value after the exponentially weighted moving average of the previous time step. S38) The EWMA filtering result and the SMA filtering result are weighted and fused to obtain the final path curvature estimate. ;Right now: ； （11） In the formula, It is a fusion weight coefficient with a value between (0, 1).

5. The unmanned surface vessel large curvature path tracking control method based on curvature adaptation and improved disturbance observation according to claim 4, characterized in that: In step S4), the aiming distance is determined using an adaptive aiming distance calculation method based on lateral error and path curvature. This includes the following steps: S41), based on the current lateral error Calculate the lateral error factor ,Right now: ； （12） In the formula, This is an adjustment coefficient used to adjust the factor's sensitivity to lateral errors; S42), based on the horizontal error factor Calculate the basic aiming distance ,Right now: ； （13） In the formula, , These are the maximum aiming distance and the minimum aiming distance, respectively. S43), based on the current path curvature Calculate curvature factor ,Right now: ； （14） In the formula, This is the aiming distance adjustment factor; This is the preset curvature scaling factor; S44) Calculate the initial aiming distance by combining the base aiming distance and the curvature factor. ,Right now: ； （15） S45) Perform nonlinear mapping optimization on the initial aiming distance, using an exponential mapping function to improve sensitivity at small aiming distances, i.e.: ； （16） In the formula, This represents the optimized aiming distance, where γ is a dimensionless aiming distance mapping gain factor. And the pre-aiming distance after transformation Limit the amplitude to ensure it remains within a reasonable range: ； （17） S46) Pre-aiming distance for amplitude limiting using an adaptive filtering mechanism Smoothing is performed to obtain the final aiming distance. ,Right now: ； （18） ； （19） In the formula, This is the aiming distance from the previous moment; These are the adaptive filter coefficients. and These are the curvature of the current path point and the absolute value of the unmanned surface vessel's lateral error, respectively. These are the basic filter coefficients; Minimum filter coefficients; and These are the curvature influence factor and the error influence factor, respectively.

6. The unmanned surface vessel large curvature path tracking control method based on curvature adaptation and improved disturbance observation according to claim 5, characterized in that: In step S4), based on the pre-aiming distance Select a pre-aiming point on the reference path Calculate the desired heading angle ,Right now: ； （20） In the formula, Pre-aiming point The coordinates.

7. The unmanned surface vessel large curvature path tracking control method based on curvature adaptation and improved disturbance observation according to claim 1, characterized in that: In step S6), combining the desired heading angle, the current heading angle, and the total disturbance estimated in step S5), the control quantity is calculated using a control law combining PID control and disturbance compensation to drive the unmanned surface vessel (USV). Specifically, the steps include: S61) The longitudinal speed of the unmanned surface vessel is controlled by a proportional-integral-derivative PID controller with integral anti-saturation function. To follow the target speed ,Right now: ； （42） In the formula, To output thrust; Speed ​​control gain; For longitudinal velocity error; For target speed; This represents the current longitudinal velocity; , , These are the proportional, integral, and differential gains of the velocity loop; the integral term. Upper and lower limits are set to prevent system overshoot and instability caused by integral saturation; S62), based on desired heading angle Compared with the current actual heading angle Error between Feedback control term for calculating heading error ,Right now: ； （43） In the formula, , These are the integral and differential gains of the heading loop, respectively; For proportional gain; ； （44） In the formula, Base proportional gain; The curvature of the current path point; and This is the adjustment coefficient; S63) Calculate the disturbance compensation term based on the total disturbance estimate estimated by the SDO observer. ,Right now: ； （45） In the formula, It is the total disturbance compensation gain; It is the total system disturbance estimated by SDO and smoothed by adaptive filtering; It is the set saturation boundary value; It is a dimensionless positive constant used to adjust the curve shape of the saturation function; It is a symbolic function; S65) Calculate the final rudder angle based on the feedback control term of the heading error and the disturbance compensation term of the SDO. ,Right now: ； （46） in, This is the feedback control term for the heading error. For the disturbance compensation term of SDO; S66), regarding the final rudder angle To impose restrictions, namely: ； （47） In the formula, This is the final rudder angle output after limitation; Based on the rudder angle and thrust, the control force and torque are calculated, namely: ； （48） ； （49） ； （50） In the formula, The center of gravity; , , These are the longitudinal control force, the lateral control force, and the yaw control torque, respectively.

Images