CN-121143349-B - Unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation

CN121143349BCN 121143349 BCN121143349 BCN 121143349BCN-121143349-B

Abstract

The invention provides a large-curvature path tracking control method of an unmanned ship based on curvature self-adaption and improved disturbance observation, and belongs to the technical field of unmanned ship autonomous navigation and paths. The method comprises the steps of loading a reference path and initializing parameters, calculating transverse errors and path curvature in each control period, determining a pretightening point through a self-adaptive pretightening distance calculation method based on the transverse errors and the curvature, estimating disturbance by utilizing an improved state disturbance observer SDO, adjusting the bandwidth of the observer through a self-adaptive mechanism driven by two factors, calculating control quantity through a control law by combining the deviation and the disturbance, and updating the state of a boat body until path tracking is completed. The invention can dynamically adjust the pretightening distance and the bandwidth of the observer, effectively estimate and compensate disturbance, improve the precision and the robustness of the unmanned ship in the tracking of the large curvature path, and ensure the accuracy of system state observation and the stability of control performance when the scenes such as the large curvature path and the straight line section are switched.

Inventors

LIU CHANG
YOU ZHANGSHENG
CHEN ZHONGMING
Rao Hongxia
PENG HUI
LU RENQUAN
XU YONG
LI JUNYI

Assignees

广东工业大学

Dates

Publication Date: 20260508
Application Date: 20251014

Claims (7)

1. The unmanned ship large curvature path tracking control method based on curvature self-adaption and improved disturbance observation is characterized by comprising the following steps of: s1), a three-degree-of-freedom dynamic model of the unmanned surface vehicle is used for describing the position and posture change of the vehicle body under a global coordinate system; s2), loading a predefined reference path, and initializing control parameters and state variables; S3), in each control period, calculating the transverse error and the path curvature of the current position of the unmanned ship; S4) determining a pretightening distance by adopting a self-adaptive pretightening distance calculation method based on the transverse error and the path curvature obtained in the step S3), selecting a pretightening point according to the pretightening distance, and calculating a desired course angle; s5, estimating the total disturbance of the system through an improved State Disturbance Observer (SDO), wherein the SDO dynamically adjusts the bandwidth by adopting an observer bandwidth self-adaptive mechanism based on curvature-error double-factor driving; S6) calculating a control quantity by combining the expected course angle, the current course angle and the disturbance estimated in the step S5) through a control law combining PID control and disturbance compensation, and driving the unmanned ship to navigate; S7), updating the state variable of the unmanned ship, and repeating the steps S3) -S6) until path tracking is completed; In step S5), the improved state disturbance observer SDO is used to estimate the total disturbance of the system, and specifically includes the following steps: s511), receive lateral error As input, and introducing error saturation smoothing process, namely: ; (21) ; (22) in the formula, Is a saturation threshold, and is set according to the actual application scene; And The smoothing adjustment coefficient is used for adjusting the form of a smoothing curve; Representing the lateral error after saturation treatment; Representing the saturation error Performing nonlinear smoothing treatment on the transverse errors; s512), dynamically adjusting the bandwidth by adopting an observer bandwidth self-adaptive mechanism based on curvature-error double-factor driving, and obtaining the final bandwidth The method comprises the following steps: ; (28) in the formula, To limit the bandwidth; And Respectively preset minimum and maximum observer bandwidths; S513), based on systematic lateral errors The design of the third-order state disturbance observer is as follows: ; (29) in the formula, As a function of the state variables of the observer, Corresponding to the estimation of the lateral error of the system, To the rate of change of transverse error Is determined by the estimation of (a); is an estimated value of disturbance to the system sum; 、、 As observer state variables, as Is the first derivative of (a); 、、 Is an observer parameter; Is the final bandwidth after dynamic adjustment; s514), updating the observer state by using a fourth-order longgrid tower method, namely: ; (30) ; (31) ; (32) ; (33) ; (34) ; (35) ; (36) ; (37) in the formula, ; For the corresponding equation of state, Sampling time; 、、、 representing state change rate estimates at the start, midpoint, and end of the time step, respectively; 、、 temporary state variable values for calculating different slopes, respectively; s515), introducing a multiple state restriction mechanism prevents state mutations and divergences, i.e.: , ; (38) ; (39) in the formula, Representing a state maximum limit; representing a maximum state change rate; A state value indicating the last time; s516), performing self-adaptive low-pass filtering processing on the disturbance estimated value, dynamically adjusting a filtering coefficient according to the disturbance variation amplitude, and finally outputting high-precision and low-noise system total disturbance, namely: ; (40) ; (41) in the formula, Adaptive filter coefficients; Is an adjustable parameter; the sum disturbance estimated value is obtained; the disturbance estimated value after filtering at the last moment; estimating the total disturbance of the system which is subjected to adaptive filtering smoothing for the SDO; step S512), dynamically adjusting the bandwidth by adopting an observer bandwidth self-adaptive mechanism based on curvature-error double-factor driving, and specifically comprising the following steps: S5121) calculating error factor using hyperbolic tangent function The method comprises the following steps: ; (23) in the formula, As a hyperbolic tangent function; S5122) calculating a curvature factor using an exponential function The method comprises the following steps: ; (24) S5123), introducing nonlinear term to calculate base bandwidth Bandwidth gain at large curvatures and large errors is improved by introducing nonlinear terms, namely: ; (25) Wherein, the For the observer bandwidth reference value, Is a bandwidth adaptive coefficient; Weight coefficients that are curvature-error coupling terms; S5124), introducing a bandwidth change rate limit to prevent a bandwidth mutation, bandwidth according to the last control period And maximum allowable rate of change Calculating the allowable bandwidth range of the current period by using the expected bandwidth Limited to the allowable range, and the limited bandwidth is obtained The method comprises the following steps: ;(26) ; (27) in the formula, Is the maximum allowable bandwidth variation value; representing a maximum allowable rate of change of observer bandwidth; the observer bandwidth is the last moment; to limit the bandwidth; Representation limiting x to The interval is within; Finally, an absolute upper and lower limit is applied to the bandwidth to ensure that the bandwidth is always in a stable interval, and the final bandwidth is obtained : ; (28) In the formula, To limit the bandwidth; And Respectively, a preset minimum and maximum observer bandwidth.
2. The unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 1 is characterized in that in the step S1), the expression of the three-degree-of-freedom dynamics model of the unmanned ship on the water surface is as follows: ; (1) in the formula, Is a quality matrix; is a coriolis force matrix; Is a damping matrix; is a velocity vector in a hull coordinate system; for controlling force and moment vectors; Respectively representing longitudinal and transverse speeds in a ship body coordinate system; representing the angular velocity of the hull; Respectively representing longitudinal thrust, transverse force and bow moment under a ship body coordinate system; Representing the transpose operation.
3. The unmanned aerial vehicle large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 1, wherein in step S3), the current position of the unmanned aerial vehicle is set as a point In the geodetic coordinate system The coordinates of (a) are Course angle is The predefined target path is formed by a series of path points Composition, in order to realize accurate course control; when the unmanned ship is at 、 When in between, i.e. from the starting point And endpoint Directional line segment of the composition, lateral error Is a vertical projection point on the path corresponding to the unmanned ship position According to the principle of vector cross multiplication, said lateral error The calculation formula of (2) is as follows: ; (8) in the formula, ) Is a vector Vector of AND If the result is positive, represent a point At a directed line segment If the result is negative, represent a dot At a directed line segment Right side of (2); Is a path segment For normalization such that the molecular cross-multiplied "signed area" result is converted to a "signed distance".
4. The unmanned aerial vehicle large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 3, wherein in step S3), a multi-stage curvature calculation and smoothing method is adopted to calculate the path curvature The method specifically comprises the following steps: s31), taking a path point corresponding to the current position of the unmanned ship as a center, and selecting 11 path points from front to back to form a calculation window; s32), smoothing the coordinates of the path points in the window by applying 3-order Savitzky-Golay filtering; s33), calculating first-order and second-order derivatives of the smoothed coordinates by adopting a center difference method, and further calculating the curvature of each point in the window; S34), applying 5-point median filtering to the calculated curvature sequence; s35), taking a weighted average of 5 points near the center point of the curvature sequence after the median filtering as initial curvature estimation; S36), limiting the curvature to Within the range; Respectively minimum curvature and maximum curvature; s37), sequentially applying smoothing factors The exponentially weighted moving average filtering EWMA of (2) and the linearly increasing weighted 11-point moving window average filtering SMA are used, namely: ; (9) ; (10) in the formula, 、 Curvature of EWMA and SMA filtering, respectively; for the linearly increasing weight of the weight, A curvature cache queue; Weighting the curvature value filtered by the moving average for the index at the last moment; S38), carrying out weighted fusion on the EWMA filtering result and the SMA filtering result to obtain the final path curvature estimation The method comprises the following steps: ; (11) in the formula, Is a fusion weight coefficient with a value between (0, 1).
5. The unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 4, wherein in step S4), a self-adaption pretightening distance calculation method is adopted to determine the pretightening distance based on transverse errors and path curvature, and the method specifically comprises the following steps: s41), based on the current lateral error Calculating a lateral error factor The method comprises the following steps: ; (12) in the formula, For the adjustment factor, for adjusting the sensitivity of the factor to lateral errors; s42), based on the transverse error factor Calculating a basic pre-aiming distance The method comprises the following steps: ; (13) in the formula, 、 The maximum pretightening distance and the minimum pretightening distance are respectively; S43), based on the current path curvature Calculating a curvature factor The method comprises the following steps: ; (14) in the formula, Adjusting a coefficient for the pretightening distance; scaling the coefficient for a preset curvature; S44), combining the basic pretightening distance and the curvature factor, and calculating the initial pretightening distance The method comprises the following steps: ; (15) S45), carrying out nonlinear mapping optimization on the initial pretightening distance, and improving the sensitivity of the small pretightening distance by using an exponential mapping function, namely: ; (16) in the formula, Representing the optimized pretightening distance, wherein gamma is a dimensionless pretightening distance mapping gain factor; And aiming at the converted pretarget distance And carrying out clipping processing to ensure that the clipping processing is within a reasonable range: ; (17) S46), pretarge distance for amplitude limiting processing by using adaptive filtering mechanism Smoothing to obtain final pretarget distance The method comprises the following steps: ; (18) ; (19) in the formula, The pre-aiming distance is the pre-aiming distance at the last moment; In order to adapt the filter coefficients to the different values, And The curvature of the current path point and the transverse error absolute value of the unmanned ship are respectively; Is a basic filter coefficient; is the minimum filter coefficient; And The curvature influencing factor and the error influencing factor, respectively.
6. The unmanned aerial vehicle large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 5, wherein in step S4), the unmanned aerial vehicle large-curvature path tracking control method is characterized in that the unmanned aerial vehicle large-curvature path tracking control method is based on the pretightening distance Selecting a pre-aiming point on a reference path Calculating the expected course angle The method comprises the following steps: ; (20) in the formula, For pre-aiming point Is defined by the coordinates of (a).
7. The unmanned aerial vehicle large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation according to claim 1 is characterized in that in the step S6), a desired course angle, a current course angle and the total disturbance estimated in the step S5) are combined, a control amount is calculated through a control law combining PID control and disturbance compensation, and the unmanned aerial vehicle is driven to navigate, and specifically comprises the following steps: S61), controlling the longitudinal speed of the unmanned ship by adopting a proportional-integral-derivative PID controller with integral anti-saturation function To follow the target speed The method comprises the following steps: ; (42) in the formula, To output thrust; gain for speed control; Is a longitudinal speed error; is the target speed; Is the current longitudinal speed; , , Proportional, integral and differential gains of the speed loop respectively, integral term An upper limit and a lower limit are set to prevent the oversaturation and instability of the system caused by integral saturation; S62), based on the expected heading angle From the current actual heading angle Error between Feedback control item for calculating heading error The method comprises the following steps: ; (43) in the formula, 、 The integral gain and the differential gain of the course ring are respectively; is a proportional gain; ; (44) in the formula, Is a base proportional gain; Curvature for the current path point; And For adjusting the coefficient; s63) calculating disturbance compensation term based on total disturbance estimated value estimated by SDO observer The method comprises the following steps: ; (45) in the formula, Is the total disturbance compensation gain; The system total disturbance is estimated by SDO and smoothed by self-adaptive filtering; Is a set saturation boundary value; Is a dimensionless positive constant for adjusting the curve shape of the saturation function; Is a sign function; S65), calculating a final rudder angle according to the feedback control item of the course error and the disturbance compensation item of the SDO The method comprises the following steps: ; (46) Wherein, the Is a feedback control term for the heading error, A disturbance compensation term for SDO; s66), for final rudder angle Limitation is performed, namely: ; (47) in the formula, Outputting a final rudder angle after limiting; based on rudder angle and thrust, control force and moment are calculated, namely: ; (48) ; (49) ; (50) in the formula, A gravity center position; 、、 longitudinal control force, transverse control force and bow control moment respectively.

Description

Unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation Technical Field The invention relates to the technical field of path tracking, in particular to a large-curvature path tracking control method of an unmanned ship based on curvature self-adaption and improved disturbance observation. Background The unmanned surface vessel is unmanned equipment capable of autonomous navigation on the water surface. Along with the wide application of unmanned ships in the fields of ocean monitoring, environment detection, military reconnaissance and the like, the requirements on the accuracy and stability of unmanned ship path tracking are increasingly improved. Path tracking is one of the core technologies of unmanned ship autonomous navigation. In complex water environments, unmanned boats often face challenges of large curvature path tracking, such as complex channels in ports, curved routes around islands, etc. The traditional path tracking method has a plurality of problems that on one hand, a fixed pretightening distance is difficult to adapt to the rapid change of curvature, so that pretightening points are unreasonable to select under the large-curvature path, the tracking precision is reduced, and on the other hand, an effective estimation and compensation mechanism is lacked for path deviation disturbance and total disturbance of a system caused by curvature change in the path tracking process, so that the unmanned ship cannot rapidly adjust the gesture and the speed when being interfered by water flow, stormy waves and the like, and the tracking performance is unstable. In addition, the traditional control algorithm often adopts a fixed value in bandwidth setting, cannot be dynamically adjusted according to actual path curvature and tracking error, and is difficult to realize optimal control effect under different working conditions. Therefore, there is a need for an unmanned boat path tracking method that can accommodate paths of large curvature, effectively suppress disturbances, and achieve adaptive control. Disclosure of Invention Aiming at the defects of the prior art, the invention provides an unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation. The technical scheme of the invention is that the unmanned ship large-curvature path tracking control method based on curvature self-adaption and improved disturbance observation comprises the following steps: s1), a three-degree-of-freedom dynamic model of the unmanned surface vehicle is used for describing the position and posture change of the vehicle body under a global coordinate system; s2), loading a predefined reference path, and initializing control parameters and state variables; S3), in each control period, calculating the transverse error and the path curvature of the current position of the unmanned ship; S4) determining a pretightening distance by adopting a self-adaptive pretightening distance calculation method based on the transverse error and the path curvature obtained in the step S3), selecting a pretightening point according to the pretightening distance, and calculating a desired course angle; S5), estimating the total disturbance of the system through an improved State Disturbance Observer (SDO), wherein the SDO dynamically adjusts the bandwidth by adopting an observer bandwidth self-adaptive mechanism based on curvature-error double-factor driving; S6) calculating a control quantity by combining the expected course angle, the current course angle and the disturbance estimated in the step S5) through a control law combining PID control and disturbance compensation, and driving the unmanned ship to navigate; s7), updating the state variable of the unmanned ship, and repeating the steps S3) -S6) until path tracking is completed. Preferably, in step S1), the expression of the three-degree-of-freedom dynamics model of the unmanned surface vessel is: ; (1) in the formula, Is a quality matrix; is a coriolis force matrix; Is a damping matrix; is a velocity vector in a hull coordinate system; for controlling force and moment vectors; Respectively representing longitudinal and transverse speeds in a ship body coordinate system; representing the angular velocity of the hull; Respectively representing longitudinal thrust, transverse force and bow moment under a ship body coordinate system; Representing the transpose operation. Preferably, in step S3), the current position of the unmanned ship is set as a pointIn the geodetic coordinate systemThe coordinates of (a) areCourse angle isThe predefined target path is formed by a series of path pointsComposition, in order to realize accurate course control; when the unmanned ship is at、When in between, i.e. from the starting pointAnd endpointDirectional line segment of the composition, lateral errorIs a vertical projection point on the path corresponding