Robust flight path tracking method for unmanned surface vehicle based on trajectory curvature adaptive segmentation control

Abstract

The invention discloses an unmanned surface vehicle robust track tracking method based on track curvature adaptive segmentation control, and relates to the technical field of unmanned surface vehicle autonomous navigation control, and the method comprises the steps: calculating the track curvature of a current leg in real time, and dividing a control section based on a preset threshold; executing a corresponding control strategy according to a control section, dynamically adjusting a proportionality coefficient, an integral coefficient and a differential coefficient by adopting fuzzy PID, and generating a steering lag prediction compensation amount based on a planning energy optimal speed; and steering compensation is carried out based on the steering lag prediction compensation amount, so that steering lag is reduced. And finally, judging whether the terminal point of the current flight segment is reached or not so as to obtain an optimized flight path tracking result. Through a curvature-driven three-section control strategy, a dynamic foresight distance dual constraint and an energy consumption optimization mechanism, the problems of tracking drift, control switching oscillation and energy waste of a traditional method in a sharp turn leg are solved, and the tracking precision and the system robustness under a complex track are remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and control technology for unmanned surface vessels, and more specifically to a robust trajectory tracking method for unmanned surface vessels based on adaptive segmented control of trajectory curvature. Background Technology

[0002] Currently, ships, as the core carriers of waterway transportation, hold an irreplaceable strategic position in marine resource development, national defense security, and national economic construction. In recent years, unmanned surface vessel (USV) technology has developed rapidly and has been widely applied in high-value fields such as marine surveying, environmental monitoring, hydrological surveying, and maritime search and rescue. This innovative carrier, which integrates multiple disciplines such as ship engineering, automatic control, and intelligent sensing, is constantly expanding its operational boundaries in complex sea conditions, demonstrating broad application prospects.

[0003] With the deep penetration of intelligent technologies, the control systems of unmanned surface vessels (USVs) have gradually achieved a leapfrog upgrade from basic navigation to autonomous decision-making. Track control, as a core component, directly determines the mission execution accuracy and operational reliability of USVs. This technology enables automated vessel operation by precisely tracking predetermined paths, playing a crucial role in engineering scenarios such as subsea pipeline laying and channel dredging. Unlike traditional manual navigation, track control boasts significant advantages such as high precision, strong anti-interference capabilities, and good operational continuity. It not only greatly reduces the labor intensity of crew members but also effectively avoids navigation risks such as deviations and collisions, providing technical support for improving the economic efficiency of maritime operations.

[0004] Current mainstream trajectory control methods are primarily based on the line-of-sight (LOS) algorithm and its improved variants. While this algorithm is the industry's preferred choice due to its simple structure and ease of implementation, significant bottlenecks remain in practical applications. When facing complex curved trajectories, traditional methods employ fixed forward-looking distances and constant speed parameters, making it difficult for ships to adjust their attitude in real-time during sharp bends. This leads to a dual problem of decreased tracking accuracy and reduced maneuverability. Particularly in sections with abrupt curvature changes, the fixed control parameters are mismatched with the ship's dynamic characteristics, resulting in operational defects such as lag in heading response and accumulated trajectory deviations. Furthermore, the constant-speed propulsion mode ignores the correlation between trajectory geometry and energy consumption, generating ineffective power loss in curved areas and limiting the system's economical operating capability.

[0005] The essence of the aforementioned shortcomings lies in the lack of a dynamic perception and response mechanism for the geometric characteristics of the navigation track in existing technologies. When ships navigate complex paths such as narrow waterways or island and reef areas, changes in curvature require the control system to adjust the forward sight distance in real time to optimize steering decisions. Simultaneously, it needs to dynamically plan propulsion parameters based on segment characteristics to achieve coordinated optimization of accuracy and energy consumption. The static parameter strategies of existing methods cannot adapt to the needs of such nonlinear scenarios. Therefore, it is urgent to establish a dynamic control architecture based on curvature adaptation to fundamentally overcome the bottlenecks in tracking accuracy and energy efficiency under complex trajectories. This innovative approach of driving parameter adjustments based on trajectory characteristics will become a key technological path to unlock the full-ocean-area operation capabilities of unmanned vessels.

[0006] Therefore, how to overcome the shortcomings of traditional track tracking methods, such as low tracking accuracy, large control oscillations, and excessive energy consumption caused by curvature changes in complex flight segments, is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0007] In view of this, the present invention provides a robust trajectory tracking method for unmanned surface vessels based on trajectory curvature adaptive segmented control, which solves the defects of traditional trajectory tracking methods such as low tracking accuracy, large control oscillation and excessive energy consumption caused by curvature changes in complex sections, and improves the navigation stability and energy efficiency of unmanned surface vessels in sharp bends and complex sea conditions.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature includes:

[0010] S1. Calculate the trajectory curvature of the current flight segment in real time and divide the control segment based on a preset threshold; the control segment includes a straight tracking zone, a turning tracking zone, and a transition control zone;

[0011] S2. Execute the corresponding control strategy according to the control section: In the straight tracking zone, the improved LOS algorithm is used to integrate dynamic forward sight distance Δ adjustment and drift angle β compensation; in the steering tracking zone, the enhanced curve tracking method is used to shrink the forward sight distance and enhance the steering torque; in the transition control zone, the control output is obtained by mixing straight control and steering control through a weight function.

[0012] S3. Online optimization of control parameters: The proportional coefficient, integral coefficient, and derivative coefficient are dynamically adjusted using fuzzy PID control. Based on the planned energy-optimal speed, a steering lag prediction compensation amount is generated, and steering compensation is performed based on the steering lag prediction compensation amount.

[0013] S4. Determine if the current flight segment has been reached: If not, return to S1 to continue tracking; if reached, output the optimized track tracking result.

[0014] Optionally, the division of control segments based on a preset threshold specifically means: when K < K min When entering the straight-line tracking zone, when K > K max When entering the steering tracking zone, when K min ≤K≤K max When entering the transition control region; where K is the trajectory curvature; K max and K min It represents the upper and lower limits of curvature in the transition zone.

[0015] Optionally, the trajectory curvature K is calculated as follows: the curvature value is determined by the ratio of the change in heading angle Δθ between adjacent segments to the segment length L, and the calculation formula is as follows:

[0016] K = |Δθ| / L;

[0017] Δθ=atan2(sin(θ k -θ k-1 ),cos(θ k -θ k-1 ));

[0018] Where K is the trajectory curvature, Δθ represents the change in heading angle between adjacent segments, and θ k and θ k-1 These represent the heading angles of the current segment and the previous segment, respectively, and L represents the segment length.

[0019] Optionally, the calculation of the dynamic forward distance Δ in the straight-line tracking zone employs a dual constraint mechanism, including an exponential decay term and a dynamic constraint term:

[0020]

[0021] The dynamic convergence coefficient λ is calculated as follows:

[0022]

[0023] Where, Δ exp For the exponentially decaying term, Δ dyn The dynamic constraint term is λ0, the fundamental convergence coefficient is α, the curvature change rate sensitivity factor is dK / dt, and Δ is the curvature change rate. max For the maximum forward sight distance, Δ min β is the minimum forward sight distance, γ is the curvature influence factor, v is the hull lateral velocity, and R is the minimum forward sight distance. min This is the minimum turning radius.

[0024] Optionally, the calculation method for the drift angle compensation dual optimization in the straight-line tracking region is also included, as follows:

[0025]

[0026] Where β c The drift angle is the compensated drift angle, u and v are the longitudinal and lateral velocities respectively, and dβ / dt is the rate of change of the drift angle.

[0027] Optionally, the heading correction formula for the straight-line tracking zone is as follows:

[0028]

[0029] Where ψd is the desired heading angle, β c For the compensated drift angle, y e Δ represents the lateral position error, and Δ represents the current forward sight distance.

[0030] Optionally, the calculation method for the enhanced curve tracking method enabled in the steering tracking zone is as follows: by calculating the trajectory curvature change rate dK / dt in real time, the forward sight distance Δ and steering torque τ are dynamically adjusted. yaw The calculation formula is as follows:

[0031]

[0032] Where Δ min ι is the minimum forward sight distance, which is the basic contraction factor. Gain sensitive to curvature changes This is the curvature change rate amplification factor, and the tanh hyperbolic tangent function achieves sensitive adjustment of the curvature change rate.

[0033]

[0034] Where τ max The upper limit of the actuator torque, Basic enhancement coefficient, Additional gain for curvature abrupt change, This is the response sharpness coefficient, σ(x) = 1 / (1+e -x ) is the curvature change response function.

[0035] Optionally, the step of obtaining the control output by mixing linear control and steering control through a weighting function in the transition control region specifically includes the weighting function being defined as follows:

[0036]

[0037] Where δ = 0.0002 rad / m is the width of the buffer zone, and K max and K min These are the upper and lower limits of curvature in the transition region; smoothstep(x) = x 2 (3-2x) (x∈[0,1]) is a cubic smooth transition function.

[0038] The formula for calculating the control output torque τ is:

[0039] τ=(1-w)×τ LOS +w×τ curve ;

[0040] Where, τ LOS The control output torque τ in the linear tracking region curve The control output torque is for the steering tracking zone.

[0041] Optionally, the optimal energy speed for planning specifically involves: first calculating the maximum permissible speed u. max Then, the reference velocity u is determined through gradient optimization. ref ;

[0042]

[0043] Where u nominal For nominal speed, η k ξ is the curvature influence coefficient, ξ is the wind and wave influence coefficient, W is the wind and wave level, and u min For the minimum permissible speed, λ e As the energy consumption weight, y e For the lateral position error, Δy e The value is μ, where μ is the error change and Δt is the sampling time. grad This is the gradient adjustment coefficient.

[0044] Optionally, the steering lag prediction compensation amount τ comp The calculation method is as follows:

[0045]

[0046] Where, sign(K) p ) is the sign function for the curvature direction, G c To dynamically compensate for the gain, G min To compensate for the base gain value, G range To compensate for the range of gain variation, S is the sensitivity coefficient for the rate of curvature change, and K... p To predict curvature, K represents the real-time trajectory curvature, dK / dt represents the rate of change of curvature, and ω represents the curvature prediction weighting factor. Δ is the resultant velocity of the hull, C1 is the curvature compensation coefficient, and C2 is the velocity-related compensation coefficient.

[0047] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature, which has the following beneficial effects:

[0048] 1. Through a curvature-driven three-segment control mechanism, precise tracking is achieved on complex flight segments, and the lateral error of sharp bends is controlled within 1m;

[0049] 2. An optimization strategy combining weighted functions and fuzzy PID is adopted to significantly suppress oscillations during control switching and effectively reduce torque fluctuation amplitude;

[0050] 3. An innovative curvature-energy consumption coupling optimization model is designed to reduce overall energy consumption by more than 50% while ensuring tracking accuracy, thus significantly improving battery life;

[0051] 4. Establish a dual constraint mechanism for forward sight distance to effectively balance geometric characteristics and dynamic limitations, and enhance the system's adaptability in complex sea conditions. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0053] Figure 1 The overall flowchart of the method provided by this invention;

[0054] Figure 2 Analysis diagram of the steering lag compensation mechanism provided by the present invention;

[0055] Figure 3 The improved fuzzy PID controller structure diagram provided by the present invention;

[0056] Figure 4 The three-segment control weight function variation curve provided by this invention;

[0057] Figure 5 Comparison chart of trajectory tracking simulation results provided for this invention;

[0058] Figure 6 The graph showing the change of lateral error over time provided by this invention;

[0059] Figure 7 The forward sight distance adaptive adjustment curve provided by this invention;

[0060] Figure 8 The control torque output curve provided for this invention;

[0061] Figure 9 Energy consumption optimization comparison curves provided for this invention. Detailed Implementation

[0062] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0063] This invention discloses a robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature, such as... Figure 1 As shown, it includes:

[0064] S1. Calculate the trajectory curvature K of the current flight segment in real time, based on a preset threshold K. min and K max Divide the control zone: when K < K min When entering the straight-line tracking zone, when K > K max When entering the steering tracking zone, when K min ≤K≤K max When entering the transition control zone; the preset threshold is specifically K. max and K min These are the upper and lower limits of curvature in the transition region, respectively.

[0065] S2. Execute the corresponding control strategy according to the control section: In the straight tracking zone, the improved LOS algorithm is used to integrate dynamic forward sight distance Δ adjustment and drift angle β compensation; in the steering tracking zone, the enhanced curve tracking method is used to shrink the forward sight distance and enhance the steering torque; in the transition control zone, the control output is obtained by mixing straight control and steering control through a weight function.

[0066] S3. Online optimization of control parameters: The proportional coefficient K is dynamically adjusted using fuzzy PID control. p Integral coefficient K i Differential coefficient K d Based on the optimal speed for planned energy, a steering lag prediction compensation amount is generated; and steering compensation is performed based on the steering lag prediction compensation amount.

[0067] S4. Determine if the current flight segment has been reached: If not, return to S1 to continue tracking; if reached, output the optimized track tracking result.

[0068] In a specific embodiment, the trajectory curvature K is calculated as follows: the curvature value is determined by the ratio of the change in heading angle Δθ between adjacent flight segments to the segment length L, and the calculation formula is as follows:

[0069] K = |Δθ| / L;

[0070] Δθ=atan2(sin(θ k -θ k-1 ),cos(θk -θ k-1 ));

[0071] Where K is the trajectory curvature, Δθ represents the change in heading angle between adjacent segments, and θ k and θ k-1 These represent the heading angles of the current segment and the previous segment, respectively, and L represents the segment length.

[0072] In embodiments of the present invention, such as Figure 2 As shown, the blue line segment in the flight segment is the real-time curvature change curve, which shows the variation characteristics of the K value with the flight path. Figure 7 The system records the dynamic forward sight distance (Δ) as a function of curvature K in real time. The adaptive strategy's Δ value fluctuates dynamically within the range of 2.8-4.0m: it maintains a higher value (approximately 2.8-4.0m) during straight-line navigation (approximately 0-40s and 80-120s); during turning (approximately 40-80s), it adaptively adjusts to a range of 3.5-4.0m based on curvature changes. Specifically, the Δ value in the transition control zone (0.0006≤K≤0.0010rad / m) smooths to a range of 3.5-3.6m, while the Δ value in the turning tracking zone (K>0.0010rad / m) remains stable within the range of 3.6-4m. Compared to traditional fixed-value methods, this strategy responds to track changes in real time, achieving intelligent adjustment of the forward sight distance.

[0073] In one specific embodiment, the calculation of the dynamic forward distance Δ in the straight-line tracking region employs a dual constraint mechanism, including an exponential decay term and a dynamic constraint term:

[0074]

[0075] The dynamic convergence coefficient λ is calculated as follows:

[0076]

[0077] Where, Δ exp For the exponentially decaying term, Δ dyn The dynamic constraint term is λ0, the fundamental convergence coefficient is α, the curvature change rate sensitivity factor is dK / dt, and Δ is the curvature change rate. max For the maximum forward sight distance, Δ min β is the minimum forward sight distance, γ is the curvature influence factor, v is the hull lateral velocity, and R is the minimum forward sight distance. min This is the minimum turning radius.

[0078] In a specific embodiment, the calculation method for the drift angle compensation dual optimization in the straight-line tracking region is as follows:

[0079]

[0080] Where βc The drift angle is the compensated drift angle, u and v are the longitudinal and lateral velocities respectively, and dβ / dt is the rate of change of the drift angle.

[0081] In one specific embodiment, the heading correction formula for the straight-line tracking zone is as follows:

[0082]

[0083] Where ψd is the desired heading angle, y e Δ represents the lateral position error, and Δ represents the current forward sight distance.

[0084] In an embodiment of the present invention, Figure 6 The image shows a real-time comparison of the lateral error between the traditional LOS algorithm and the improved LOS algorithm with drift compensation. Without compensation (blue dashed line), the maximum heading error of the straight track (0-40s) is 4m. After drift compensation (red solid line), the deviation is stabilized within 1m, which proves that the compensation mechanism significantly improves the tracking accuracy of the straight track.

[0085] In a specific embodiment, the calculation method for the enhanced curve tracking method enabled in the steering tracking zone is as follows: by calculating the trajectory curvature change rate dK / dt in real time, the forward sight distance Δ and steering torque τ are dynamically adjusted. yaw The calculation formula is as follows:

[0086]

[0087] Where Δ min For minimum forward sight distance, Based on the contractility factor, Gain sensitive to curvature changes This is the curvature change rate amplification factor, and the tanh hyperbolic tangent function achieves sensitive adjustment of the curvature change rate.

[0088]

[0089] Where τ max The upper limit of the actuator torque, Basic enhancement coefficient, Additional gain for curvature abrupt change, This is the response sharpness coefficient, σ(x) = 1 / (1+e -x ) is the curvature change response function.

[0090] Specifically, the heading error e ψ The calculation formula is as follows:

[0091] e ψ =ψ-ψ d -β c ;

[0092] Where ψ is the real-time heading angle of the unmanned surface vessel, ψ d For the desired heading angle, β c The drift angle after compensation.

[0093] In embodiments of the present invention, such as Figure 5 The track comparison shows that in the turning zone where curvature K>0.0010rad / m, such as at coordinates (100,30), the traditional LOS algorithm (blue dashed line) deviates by up to 6m. The improved LOS algorithm controls the tracking error within 1m by improving the forward sight distance Δ and enhancing the steering torque. Figure 8 The control torque output curve further demonstrates that the system exhibits excellent dynamic response in the steering zone: the steering torque is dynamically adjusted to within ±20 N·m, while the thrust is stably maintained around 18 N (fluctuation < ±2 N). This thrust-torque coordinated control mechanism enables the system to respond quickly to curvature changes within 0.3 s during sharp turns (t = 52 s, coordinate point (100, 30)), achieving an instantaneous steering torque of 18 N·m while maintaining thrust fluctuations within ±0.2 N, ultimately achieving a 1-meter tracking accuracy and verifying the Δ in the formula. 强化 and τ yaw强化 The effectiveness of the design.

[0094] In a specific embodiment, the control output obtained by mixing linear control and steering control through a weighting function in the transition control region is as follows:

[0095] The transition control region establishes a double boundary buffer zone, and the weighting function is defined as follows:

[0096]

[0097] Where δ = 0.0002 rad / m is the width of the buffer zone, and K max and K min These are the upper and lower limits of curvature in the transition region; smoothstep(x) = x 2 (3-2x) (x∈[0,1]) is a cubic smooth transition function.

[0098] The formula for calculating the control output torque τ is:

[0099] τ=(1-w)×τ LOS +w×τ curve ;

[0100] Where, τ LOS The control output torque τ in the linear tracking region curve The control output torque is for the steering tracking zone.

[0101] In embodiments of the present invention, such as Figure 4The piecewise function shown in the yellow region (0.0006≤K≤0.0010rad / m) achieves a continuous transition of the w value, avoiding control jumps.

[0102] In a specific embodiment, the fuzzy PID controller is implemented as follows: the input variable is the heading error e. ψ and error change rate e c Output variable proportional gain adjustment ΔK p ∈[2,6], integral gain adjustment ΔK i ∈[0,0.5], differential gain adjustment ΔK d ∈[4,8], the controller has the following characteristics:

[0103] Input variable e ψ and e c All use the triangular membership function (trimf) to divide the fuzzy levels into 4 levels: negative large (NB), negative small (NS), positive small (PS), and positive large (PB).

[0104] Output variable ΔK p ΔK i ΔK d All use triangular membership functions to divide the fuzzy levels into three categories: low (L), medium (M), and high (H).

[0105] It has 12 built-in fuzzy rules, including the rule: "If e ψ For negative large and e c If the value is negative, then ΔKp takes a higher value, ΔK i Take the lower value, ΔK d Take the median value.

[0106] In embodiments of the present invention, such as Figure 3 The controller structure diagram shown clearly illustrates the input-output relationship and the fuzzy rule processing flow.

[0107] Specifically, the controller obtains the heading error e through the error calculation module. ψ The precise quantity is converted into a fuzzy quantity by the fuzzification module. The fuzzy inference module applies 12 rules to generate control decisions. The fuzzy output is then converted back into a precise quantity ΔK by the defuzzification module. p ΔK i ΔK d (ΔK p The gain parameter mainly responds to large error scenarios, ΔK i The parameters are mainly used to suppress the accumulation of small deviations, ΔK dThe parameters (which focus on improving dynamic response performance) are adjusted by the integral separator and then input to the PID controller. Combined with the hysteresis compensation generated by the predictive compensation module, they work together to drive the thrust control module and the rudder angle control module, and finally achieve precise course tracking through the ship dynamics module.

[0108] In a specific embodiment, the energy-optimal velocity planning method is as follows: First, calculate the maximum permissible velocity u. max Then, the reference velocity u is determined through gradient optimization. ref ;

[0109]

[0110] Where u nominal For nominal speed, η k ξ is the curvature influence coefficient, ξ is the wind and wave influence coefficient, W is the wind and wave level, and u min For the minimum permissible speed, λ e As the energy consumption weight, y e For the lateral position error, Δy e The value is μ, where μ is the error change and Δt is the sampling time. grad This is the gradient adjustment coefficient.

[0111] In embodiments of the present invention, such as Figure 9 The energy consumption optimization comparison curves shown clearly demonstrate that the traditional LOS algorithm (blue dashed line) exhibits a continuous and sharp increase in energy consumption throughout the process; while the improved LOS algorithm (red solid line), through a speed adaptive strategy, although its energy consumption is higher than that of the traditional method in the initial stage, achieves an overall cumulative energy saving rate of 55%, fully verifying the significant advantages of the curvature-aware speed planning mechanism in suppressing ineffective energy consumption.

[0112] In one specific embodiment, the steering lag prediction compensation amount τ comp The calculation method is as follows:

[0113]

[0114] Where, sign(K) p ) is the sign function for the curvature direction, G c To dynamically compensate for the gain, G min To compensate for the base gain value, G range To compensate for the range of gain variation, S is the sensitivity coefficient for the rate of curvature change, and K... p To predict curvature, K represents the real-time trajectory curvature, dK / dt represents the rate of change of curvature, and ω represents the curvature prediction weighting factor. Δ is the resultant velocity of the hull, C1 is the curvature compensation coefficient, and C2 is the velocity-related compensation coefficient.

[0115] like Figure 2 As shown, the upper blue curve illustrates a step-like abrupt change in the trajectory curvature K, while the lower red curve corresponds to the compensation amount τ. comp It exhibits a periodic fluctuation response pattern, and the two remain dynamically correlated over time. When the curvature changes abruptly (e.g., the curvature jumps from 0 rad / m to 0.076 rad / m at t = 75s), the compensation amount will adjust accordingly after a delay of about 1.5 seconds. The maximum compensation amount can reach 0.48 N·m, and it maintains a continuous compensation of 0.3-0.4 N·m in the high curvature region (K>0.05 rad / m). Overall, it clearly reveals the time-domain tracking characteristics and control effect of the steering lag compensation mechanism.

[0116] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0117] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature, characterized in that, include: S1. Calculate the trajectory curvature of the current flight segment in real time and divide the control segment based on a preset threshold; the control segment includes a straight tracking zone, a turning tracking zone, and a transition control zone; S2. Execute the corresponding control strategy according to the control section: In the straight tracking zone, the improved LOS algorithm is used to integrate dynamic forward sight distance Δ and drift angle β compensation; in the steering tracking zone, the enhanced curve tracking method is used to shrink the forward sight distance and enhance the steering torque; in the transition control zone, the control output is obtained by mixing straight control and steering control through a weight function. S3. Online optimization of control parameters: The proportional coefficient, integral coefficient and derivative coefficient are dynamically adjusted using fuzzy PID, and the steering lag prediction compensation is generated based on the planned energy-optimal speed. Steering compensation is performed based on the steering lag prediction compensation amount; S4. Determine if the current flight segment has been reached: If not, return to S1 to continue tracking; if reached, output the optimized track tracking result.

2. The robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The specific method of dividing the control segment based on a preset threshold is as follows: when K < K min When entering the straight-line tracking zone, when K > K max When entering the steering tracking zone, when K min ≤K≤K max When entering the transition control region; where K is the trajectory curvature; K max and K min These are the upper and lower limits of curvature in the transition region, respectively.

3. The robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The trajectory curvature K is calculated as follows: the curvature value is determined by the ratio of the change in heading angle Δθ between adjacent flight segments to the segment length L. The calculation formula is as follows: K = |Δθ| / L; Δθ=atan2(sin(θ) k -θ k-1 ),cos(θ k -θ k-1 )); Where K is the trajectory curvature, Δθ represents the change in heading angle between adjacent segments, and θ k and θ k-1 These represent the heading angles of the current segment and the previous segment, respectively, and L represents the segment length.

4. The robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The calculation of the dynamic forward distance Δ in the line tracking zone employs a dual constraint mechanism, including an exponential decay term and a dynamic constraint term: D exp =(D max -D min )×e -λK +D min Δ=min(Δ exp ,D dyn ) The dynamic convergence coefficient λ is calculated as follows: Where K is the trajectory curvature, Δ exp For the exponentially decaying term, Δ dyn The dynamic constraint term is λ0, the fundamental convergence coefficient is α, the curvature change rate sensitivity factor is dK / dt, and Δ is the curvature change rate. max For the maximum forward sight distance, Δ min β is the minimum forward sight distance, γ is the curvature influence factor, v is the hull lateral velocity, and R is the minimum forward sight distance. min This is the minimum turning radius.

5. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 4, characterized in that, The calculation method for the dual optimization of drift angle compensation in the straight-line tracking region is as follows: Where β c The drift angle is the compensated drift angle, u and v are the longitudinal and lateral velocities respectively, and dβ / dt is the rate of change of the drift angle.

6. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 5, characterized in that, The heading correction formula for the straight-line tracking zone is also included, as follows: Where ψd is the desired heading angle, β c For the compensated drift angle, y e Δ represents the lateral position error, and Δ represents the current forward sight distance.

7. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The calculation method for the enhanced curve tracking method activated in the steering tracking zone is as follows: by calculating the trajectory curvature change rate dK / dt in real time, the forward sight distance Δ and steering torque τ are dynamically adjusted. yaw The calculation formula is as follows: Where Δ min For minimum forward sight distance, Based on the contractility factor, Gain sensitive to curvature changes This is the curvature change rate amplification factor, and the tanh hyperbolic tangent function achieves sensitive adjustment of the curvature change rate. Where τ max £ represents the upper limit of the actuator torque, and ￡ is the basic enhancement factor. Additional gain for curvature abrupt change, This is the response sharpness coefficient, σ(x) = 1 / (1+e -x ) is the curvature change response function.

8. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The control output obtained by mixing linear control and steering control through a weighting function in the transition control region specifically includes a weighting function defined as follows: Where δ = 0.0002 rad / m is the width of the buffer zone, and K max and K min These are the upper and lower limits of curvature in the transition region; smoothstep(x) = x 2 (3-2x) (x∈[0,1]) is a cubic smooth transition function. The formula for calculating the control output torque τ is: τ=(1-w)×τ LOS +w×τ curve ; Where, τ LOS The control output torque τ in the linear tracking region curve The control output torque is for the steering tracking zone.

9. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The specific steps for the planned optimal energy velocity are: first, calculate the maximum permissible velocity u. max Then, the reference velocity u is determined through gradient optimization. ref : Where u nominal For nominal speed, η k ξ is the curvature influence coefficient, ξ is the wind and wave influence coefficient, W is the wind and wave level, and u min For the minimum permissible speed, λ e As the energy consumption weight, y e For the lateral position error, Δy e The value is μ, where μ is the error change and Δt is the sampling time. grad This is the gradient adjustment coefficient.

10. A robust trajectory tracking method for unmanned surface vessels based on adaptive piecewise control of trajectory curvature according to claim 1, characterized in that, The steering lag prediction compensation amount τ comp The calculation method is as follows: Where, sign(K) p ) is the sign function for the curvature direction, G c To dynamically compensate for the gain, G min To compensate for the base gain value, G range To compensate for the range of gain variation, S is the sensitivity coefficient for the rate of curvature change, and K... p To predict curvature, K is the real-time trajectory curvature, dK / dt is the rate of change of curvature, and ω is the curvature weighting function. Δ is the resultant velocity of the hull, C1 is the curvature compensation coefficient, and C2 is the velocity-related compensation coefficient.

Images