Water surface unmanned ship obstacle avoidance method based on improved artificial potential field repulsive force model

Abstract

The invention relates to the technical field of unmanned ship obstacle avoidance, and discloses a water surface unmanned ship obstacle avoidance method based on an improved artificial potential field repulsive force model, comprising the following steps: (1) establishing mathematical models of an unmanned ship, including kinematics and dynamics models; (2) improving an artificial potential field repulsive force model based on segmented power attenuation; (3) the obstacle avoidance force borne by the USV is obtained through calculation in the step (2), and the expected steering angle of USV steering is obtained; and (4) controlling the steering of the USV according to the obtained expected steering angle so as to avoid obstacles. According to the method, segmented power attenuation improvement is carried out on the artificial potential field repulsive force model, obstacle avoidance of the water surface unmanned ship is achieved, the segmented power attenuation item and the regularization parameter # imgabs0 # are introduced, the problem that navigation of an exponential obstacle avoidance model in an exponential attenuation obstacle avoidance method is unstable is effectively solved, and the safety performance of USV navigation is improved.

Description

Technical Field

[0001] The present invention relates to a method for obstacle avoidance of an unmanned surface vehicle (USV), specifically a method for obstacle avoidance of a surface unmanned vehicle based on an improved artificial potential field repulsive force model, belonging to the technical field of USV obstacle avoidance. Background Technique

[0002] The safe navigation of an unmanned surface vehicle (USV) on the sea is of great importance, and good obstacle avoidance ability is an important guarantee for safe navigation. In order to enable the USV to navigate safely in a complex marine environment including static or dynamic obstacles, how to achieve autonomous and reliable safe obstacle avoidance is an important issue for ensuring the stable navigation of the USV.

[0003] The artificial potential field method (APF) has been widely used in the research of USV obstacle avoidance due to its advantages such as simple model, easy implementation, and strong real-time performance. However, the traditional APF method has problems such as being prone to falling into local minima, inability to collect target points, and oscillation, which limit its application in complex dynamic marine environments.

[0004] In view of the above deficiencies, in recent years, some scholars have proposed various obstacle avoidance models. One of the improved models is the exponential decay obstacle avoidance method (EDOA). By introducing an exponential decay function, this method makes the obstacle avoidance behavior more continuous and stable; at the same time, the perception range of obstacles is flexibly controlled by multiple parameters, improving the adaptability of the USV in different environments.

[0005] However, the EDOA method still has problems such as strong parameter dependence, insufficient long-distance response ability, and path oscillation caused by sudden changes in repulsive force. These problems will cause large-angle turning problems during the navigation of the USV, increasing the risk of unstable navigation of the USV. Summary of the Invention

[0006] Aiming at the deficiencies of the existing USV obstacle avoidance technology, based on the traditional artificial potential field method, the present invention proposes a method for obstacle avoidance of a surface unmanned vehicle based on an improved artificial potential field repulsive force model, which has a short obstacle avoidance distance, stable navigation, and is safe and reliable.

[0007] The method for obstacle avoidance of a surface unmanned vehicle based on the improved artificial potential field repulsive force model of the present invention includes the following steps: (1) Establish a mathematical model of the unmanned surface vehicle (USV), including a kinematic model and a dynamic model; (2) Based on the piecewise power decay improved artificial potential field repulsive force model (IAPF-PPR model); (3) Obtain the obstacle avoidance force received by the USV by improving the artificial potential field repulsive force model in step (2), and obtain the desired steering angle for the USV to turn. ; (4) Control the turning of the USV according to the obtained desired steering angle to avoid obstacles.

[0008] The establishment process of the kinematic model in the above step (1) is as follows: When the unmanned ship moves on the sea surface, it has six degrees of freedom of motion. Two coordinate systems are established: the earth coordinate system fixed on the earth and the appendage coordinate system fixed on the hull. ; Simplify the six-degree-of-freedom USV model to three degrees of freedom. The kinematic model of the three degrees of freedom is as follows: ; In the above formula, represents the position information of the USV, represents the first derivative of the position information, represents the heading angle of the USV, represents the angular velocity, represents the forward speed of the USV, represents the lateral movement speed of the USV, represents the angular velocity of the USV's turning.

[0009] The dynamic model in the above step (1) is as follows: ; Among them, , , represent the mass coefficients composed of the mass of the USV, , , represent the hydrodynamic damping coefficients, represents the driving force in the forward direction of the USV, represents the driving force for the USV to turn, represents the forward speed of the USV, represents the lateral movement speed of the USV, represents the angular velocity of the USV's turning, represents the first derivative of the forward speed of the USV, represents the first derivative of the lateral movement speed of the USV, represents the first derivative of the angular velocity of the USV's turning.

[0010] The improved artificial potential field repulsive force model in the above step (2) is as follows: ; In the formula, represents the obstacle avoidance force received by the USV at the position, represents the distance between the USV and the obstacle, that is, , represents the position of the USV, represents the position of the obstacle, represents the minimum safety distance between the USV and the obstacle, represents a smoothing scheduling term, which is used to avoid division by zero of fractions and numerical mutations; represents a power exponent parameter, which controls the shape of the attenuation curve; and represent the repulsive force intensity coefficient; The smoothing scheduling term is a very small positive value, taking .

[0011] The above improved artificial potential field repulsive force model with piecewise power decay (Improved Artificial Potential Field Model with Piecewise Power-Decay Repulsion, IAPF-PPR) introduces a power function term without an exponent and continuously adjustable on the basis of the traditional APF, effectively solving the problem of numerical stability of the exponential repulsive force.

[0012] The expected steering angle in step (3) is the angle between the obstacle avoidance force direction of the USV and the forward direction of the USV.

[0013] The process of controlling the steering of the USV in step (4) is as follows: The obstacle avoidance force received by the USV provides a moving direction for the obstacle avoidance of the USV, and drives the USV to move in the moving direction through an externally applied moving speed; the moving speed of the USV is regarded as the expected speed, and the expected steering angle and the expected speed are subtracted from the actual angle and the actual speed of the USV to obtain the steering angle error and the moving speed error : ; Among them, and are the actual steering angle and moving speed of the unmanned ship obtained by the kinematic and dynamic model operations in step (1); According to the steering angle error With the moving speed error , the forward thrust required for the unmanned ship to move is obtained and the steering thrust . The movement of the unmanned ship is achieved through the kinematic and dynamic models of the unmanned ship in step (1).

[0014] The forward thrust required for the movement of the unmanned ship and the steering thrust are obtained through the adjustment of the ADRC controller (active disturbance rejection controller). The specific process is as follows: The ADRC controller includes a tracking differentiator, a nonlinear state error feedback, an extended state observer, and a disturbance compensation link; ① Tracking differentiator; A discrete tracking differentiator is designed as follows: ; where represents the input signal, represents the first derivative of the input signal, represents the second derivative of the input signal; is the desired steering angle of the unmanned ship, is the desired speed of the unmanned ship; is the filtering step size, which is used to suppress the influence of noise interference; is the filtering sampling factor. After is determined, take to control overshoot and suppress noise amplification; is the speed factor. As the speed factor increases continuously, the speed of tracking the input signal becomes faster, and is regarded as the differential form of the input signal; represents the optimal speed control synthesis function, where the process of obtaining the function is: ; ② Extended state observer; According to the formula of the kinematic and dynamic models of the unmanned ship in step (1), let , and the nonlinear part of the kinematic and dynamic models of the unmanned ship is regarded as the disturbance and . Therefore, the formula of the kinematic and dynamic models of the unmanned ship is rewritten as: ; When and are known, the extended state observer is set as: ; At and in the case of unknown and expand it into new state variables and , that is: ; Let , substitute it into the rewritten kinematic and dynamic model formula of USV, and expand to obtain a new linear system: ; Analyze the new linear system to obtain a new extended state observer: ; Among them, represents the observer parameter, represents the output variable of the extended state observer, represents the observed values of the new state variables and , The specific form of the function is as follows: ; Among them, represents the linear interval width of the function, represents the extended state observer parameter and , , ; When the parameter values of are determined, each state variable is estimated: ; ③ Nonlinear state error feedback; In order to stabilize the formula of the kinematic and dynamic model of the unmanned ship and reduce the error at the same time, a nonlinear state error feedback control rate form is adopted: ; In the formula, represents the ADRC controller response speed parameter, and increasing will increase the response speed, increasing will inhibit the response speed; represents the linear interval width of the function; select the parameter , ; for the error signal , and the error tracking signal satisfy the following relationship: ; ④ Disturbance compensation; In the extended state observer, the nonlinear part of the kinematic and dynamic models of the unmanned ship is equivalent to the disturbance for estimation. Finally, the control input of the kinematic and dynamic models of the unmanned ship is obtained by compensating for the total disturbance as follows: ; In this way, the forward thrust and the turning thrust required by the unmanned ship model are obtained.

[0015] The present invention has the following beneficial effects: By improving the piecewise power decay of the artificial potential field repulsive force model, obstacle avoidance of the surface unmanned ship is realized. By introducing the piecewise power decay term and the regularization parameter , the problem of unstable navigation of the exponential obstacle avoidance model in the exponential decay obstacle avoidance method (EDOA) is effectively solved, and the safety performance of the USV navigation is improved. The minimum obstacle avoidance distance for static obstacles can be 11.4 m, and the minimum distance for dynamic obstacles is 8.6 m, showing excellent obstacle avoidance performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a schematic diagram of the kinematic model in the method for obstacle avoidance of a surface unmanned ship based on an improved artificial potential field repulsive force model of the present invention.

[0017] Figure 2 is a schematic diagram of the dynamic model in the method for obstacle avoidance of a surface unmanned ship based on an improved artificial potential field repulsive force model of the present invention.

[0018] Figure 3 is a schematic diagram of the desired steering angle of the USV.

[0019] Figure 4 is a trajectory diagram of different obstacle avoidance methods under static obstacles.

[0020] Figure 5 is a schematic diagram of the distance between the USV and the static obstacle under different obstacle avoidance methods for static obstacles.

[0021] Figure 6 is a diagram of the experimental results of obstacle avoidance under dynamic obstacles.

[0022] Figure 7 is Figure 6 an enlarged view of point A in DETAILED DESCRIPTION OF THE EMBODIMENTS

[0023] Based on the traditional artificial potential field method, the present invention proposes an improved artificial potential field repulsive force model based on piecewise power decay to improve the obstacle avoidance model of USV. By introducing a piecewise power decay term and a regularization parameter , it effectively solves the problem of unstable navigation of the exponential obstacle avoidance model in the Exponential Decay Obstacle Avoidance Method (EDOA), and improves the safety performance of USV navigation.

[0024] The obstacle avoidance method for surface unmanned vessels based on the improved artificial potential field repulsive force model of the present invention specifically includes the following process.

[0025] I. USV mathematical model.

[0026] 1. Kinematic model of USV; When USV moves on the sea surface, it usually has six degrees of freedom of motion. Therefore, two coordinate systems are generally established: the earth coordinate system fixed on the earth and the appendage coordinate system fixed on the hull . The kinematic model of USV is shown in Figure 1. The present invention simplifies the six-degree-of-freedom USV model to three degrees of freedom, and the kinematic model of three degrees of freedom is shown in Equation (1).

[0027] ;(1) In Equation (1), represents the position information of USV, represents the first derivative of the position information, represents the heading angle of USV, represents the angular velocity, represents the forward speed of USV, represents the lateral movement speed of USV, represents the angular velocity of USV turning.

[0028] 2. Dynamic model of USV; The dynamic schematic diagram of USV is shown in Figure 2, and the dynamic model of USV can be obtained as Equation (2).

[0029] (2) In Equation (2), , , represent the mass coefficients composed of the mass of USV, , , represent the hydrodynamic damping coefficients, represents the driving force in the forward direction of USV, Represents the driving force for the USV to turn, Represents the forward speed of the USV, Represents the lateral movement speed of the USV, Represents the angular velocity of the USV's turn, Represents the first derivative of the forward speed of the USV, Represents the first derivative of the lateral movement speed of the USV, Represents the first derivative of the angular velocity of the USV's turn.

[0030] II. Improved artificial potential field repulsive force model based on piecewise power decay.

[0031] The obstacle avoidance model adopted in the Shepherding Algorithm is the exponential obstacle avoidance model (Exponential Decay Obstacle Avoidance Method, EDOA).

[0032] ;(3) In Equation (3), Represents the movement speed of the agent, Represents the maximum obstacle avoidance distance between the agent and the obstacle, Represents the minimum safety distance between the agent and the obstacle. Represents the distance between the USV and the obstacle, Represents the exponential function. This exponential model of obstacle avoidance method will only have effective obstacle avoidance when the distance between the agent and the obstacle is very small; at the same time, when applying the exponential obstacle avoidance model to the obstacle avoidance of the USV, there may be a problem of large-angle turning due to the too-close distance between the USV and the obstacle, making the navigation unstable.

[0033] To solve the problem that the obstacle avoidance function is only triggered when the distance is too close, the present invention proposes an improved artificial potential field repulsive force model based on piecewise power decay (Improved Artificial Potential Field Model with Piecewise Power-Decay Repulsion, IAPF-PPR). This IAPF-PPR model introduces a non-exponential and continuously adjustable power function term on the basis of the traditional APF (artificial potential field repulsive force model), effectively solving the problem of numerical stability of the exponential repulsive force. The expression of this IAPF-PPR model is: ;(4) In Equation (4), Represents the obstacle avoidance force received by the USV at position Represents the position of the USV, ​represents the distance between the USV and the obstacle, i.e., , represents the position of the obstacle, represents the minimum safe distance between the USV and the obstacle; represents the smoothing scheduling term, which is a very small positive value, taking . It is used to avoid division by zero of fractions and numerical mutations; represents the power exponent parameter, which controls the shape of the attenuation curve, and represents the repulsive force intensity coefficient.

[0034] III. Implement obstacle avoidance for the unmanned surface vehicle.

[0035] Obtain the obstacle avoidance force from the IAPF-PPR (Improved Artificial Potential Field Repulsive Force Model Based on Piecewise Power Decay) model and calculate the desired steering angle of the USV.

[0036] In IAPF-PPR, the obstacle avoidance force received by the USV is denoted as , which provides the moving direction for the obstacle avoidance of the USV. The USV is driven to move in the moving direction by the externally applied moving speed. The desired steering angle As Figure 3 shown, in Figure 3 , represents the current moving direction of the USV, represents the center of the USV hull, then the desired angle is the angle between the obstacle avoidance force direction of the USV and the forward direction of the USV.

[0037] Regard the moving speed of the USV as the desired speed. The desired steering angle and the desired speed are subtracted from the actual angle and the actual speed of the USV to obtain the steering angle error and the moving speed error , which are used as the inputs of the ADRC controller, i.e.: ; where, and are the actual steering angle and moving speed of the unmanned surface vehicle obtained by operating the kinematic and dynamic models of the unmanned surface vehicle in step (1).

[0038] According to the input steering angle error and the moving speed error , the forward thrust and the steering thrust , the movement of the unmanned ship is achieved through the kinematic and dynamic models of the unmanned ship in step (1).

[0039] The forward thrust required for the movement of the unmanned ship is obtained through the adjustment of the ADRC controller and the steering thrust , and the specific process is described as follows.

[0040] The ADRC controller includes a tracking differentiator, a nonlinear state error feedback, an extended state observer, and a disturbance compensation link.

[0041] 1. Tracking differentiator; In order to extract and estimate the higher-order derivative from the input signal and help the extended state observer obtain accurate state information, a discrete tracking differentiator is designed as follows: ; Among them, represents the input signal, represents the first-order derivative of the input signal, represents the second-order derivative of the input signal, represents the optimal speed control synthesis function: ; In the formula, is the desired steering angle of the unmanned ship, is the desired speed of the unmanned ship, is the filtering step size, which is used to suppress the influence of noise interference; is the filtering sampling factor. After is determined, take to control overshoot and suppress noise amplification; is the speed factor. As the speed factor increases continuously, the speed of tracking the input signal becomes faster. When it reaches a certain level, is regarded as the differential form of the input signal; 2. Extended state observer; According to the formula of the kinematic and dynamic models of the unmanned ship in step (1), let , and the nonlinear part of the kinematic and dynamic models of the unmanned ship is regarded as a disturbance and , so the formula of the kinematic and dynamic models of the unmanned ship is rewritten as: ; When and are known, the extended state observer is set as: ; At and In the case of unknown, expand the unknown variables and into new state variables and , that is: ; Let , substitute it into the rewritten USV kinematic and dynamic model formula, and expand to obtain a new linear system: ; Analyze the new linear system to obtain a new extended state observer: ; Among them, represents the observer parameter, represents the output variable of the extended state observer, represents the observed values of the new state variables and , The specific form of the function is as follows: ; Among them, represents the linear interval width of the function, represents the extended state observer parameters and , , ; For the parameter , when selecting appropriate parameter values, each state variable is estimated: ; 3. Nonlinear state error feedback; In order to stabilize the formula of the USV kinematic and dynamic model and reduce errors at the same time, adopt the form of nonlinear state error feedback control rate: ; In the formula, represents the ADRC controller response speed parameter, and increasing will increase the response speed, increasing will inhibit the response speed; represents the linear interval width of the function; Select the parameter , ; For the error signals , and the error tracking signal satisfy the following relationship: ; 4. Disturbance Compensation; In the extended state observer, the nonlinear part of the kinematic and dynamic models of the unmanned ship is equivalent to a disturbance for estimation. Finally, by compensating for the total disturbance, the control inputs of the kinematic and dynamic models of the unmanned ship are obtained as: ; In this way, the forward thrust required by the unmanned ship model can be obtained and the turning thrust .

[0042] IV. Experimental Simulation.

[0043] 1. Static Obstacle Experiment; To verify the effectiveness of the IAPF-PPR model proposed in the present invention, a typical obstacle avoidance scenario including obstacles and ocean currents was designed, and comparative experiments were carried out with the traditional APF algorithm and EDOA. The results are as Figure 4 and Figure 5 shown, respectively demonstrating the path trajectories under the three methods and the distance change between the USV and the obstacles.

[0044] From Figure 4 it can be seen that the path of the APF method is shorter, but there is no obvious turning and avoidance when approaching the obstacle, and the obstacle avoidance effect is not good, resulting in a minimum obstacle avoidance distance of only 2.7 m. Although the path of the EDOA method is smooth, due to the narrow response range of its exponential repulsive force term, it only produces an effective obstacle avoidance effect at a very close distance, and the minimum obstacle avoidance distance is 5.8 m, with a relatively high collision risk. While the IAPF-PPR method guides the path to deflect in advance near the obstacle, with a minimum obstacle avoidance distance of up to 11.4 m, significantly improving the obstacle avoidance safety margin. At the same time, its path is naturally smooth, without sudden changes and jamming phenomena, and has good obstacle avoidance performance.

[0045] 2. Dynamic Obstacle Experiment; To further verify the performance of the IAPF-PPR model proposed in the present invention in dynamic obstacles, a dynamic obstacle avoidance experiment was designed, and the experimental results are as Figure 6 and Figure 7 shown.

[0046] In the dynamic obstacle experiment, both the APF and EDOA methods collided with dynamic obstacles. The obstacle avoidance response speed of the APF algorithm was slow, and its adaptability to the movement trend of dynamic obstacles was insufficient, resulting in the USV failing to adjust its course in time and eventually colliding with the obstacle. Due to the influence of the exponential repulsive force term, the EDOA method would exhibit significant obstacle avoidance behavior only at extremely short distances. Since the dynamic obstacle continued to move, the obstacle avoidance adjustment of the EDOA at extremely short distances could not cover the subsequent movement trajectory of the obstacle, resulting in a collision. At the same time, due to the influence of the exponential repulsive force, the USV made large-angle turns at extremely short distances, and the trajectory showed obvious jitter, affecting the navigation stability. However, the IAPF-PPR obstacle avoidance method proposed in the present invention successfully avoided dynamic obstacles, without continuous large-angle turns, the smoothness of the trajectory curve was improved, and the minimum distance from the dynamic obstacle was 8.6 m, providing a sufficient safety distance, and providing a more reliable solution for the stable navigation and obstacle avoidance of the USV.

Claims

1. A method for avoiding obstacles on a surface unmanned ship based on an improved artificial potential field repulsive force model, characterized in that: The following steps are involved: (1) Establish the mathematical model of the unmanned ship, including the kinematic model and the dynamic model; (2) Improved artificial potential field repulsive force model based on piecewise power decay; (3) Obstacle avoidance force on USV is obtained by improving the artificial potential field repulsion model in step (2) , and get the desired steering angle of the USV ; (4) Based on the desired steering angle Control the steering of the USV to avoid obstacles.

2. The obstacle avoidance method for unmanned surface vessels based on the improved artificial potential field repulsive force model according to claim 1 is characterized in that: The process of establishing the kinematic model in step (1) is: When the unmanned ship moves on the sea, it moves with six degrees of freedom and establishes two coordinate systems: the geodetic coordinate system fixed on the ground and the attached coordinate system fixed to the hull ; The six-degree-of-freedom USV model is simplified to three-degree-of-freedom, and the kinematic model of three-degree-of-freedom is as follows: ； In the above formula, Indicates the location information of USV, The first-order derivative representing the position information, represents the heading angle of USV, represents the angular velocity, Indicates the forward speed of USV, Indicates the lateral movement speed of USV, Indicates the angular velocity of the USV turning.

3. The obstacle avoidance method for unmanned surface boat based on improved artificial potential field repulsive force model according to claim 1 is characterized in that: The kinetic model in step (1) is as follows: ； in, , , represents the mass coefficient composed of the USV mass, , , represents the hydrodynamic damping coefficient, Indicates the driving force in the forward direction of the USV, Indicates the driving force of USV steering, Indicates the forward speed of USV, Indicates the lateral movement speed of USV, represents the USV steering angular velocity, represents the first derivative of the USV forward speed, represents the first-order derivative of the lateral movement speed of the USV, Represents the first-order derivative of the USV steering angular velocity.

4. The obstacle avoidance method for unmanned surface boat based on improved artificial potential field repulsive force model according to claim 1 is characterized in that: The improved artificial potential field repulsive force model in step (2) is as follows: ； In the formula, Indicates that USV is The obstacle avoidance force at the position, represents the distance between USV and obstacle, that is , represents the position of the USV, Indicates the location of the obstacle. Indicates the minimum safe distance between USV and obstacles. Represents a smooth scheduling term, which is used to avoid fractional division by zero and value mutation; Represents the power exponential parameter, which controls the shape of the attenuation curve; and Represents the repulsion strength coefficient.

5. The method for avoiding obstacles on a surface unmanned ship based on an improved artificial potential field repulsive force model according to claim 4 is characterized in that: The smooth scheduling term .

6. The obstacle avoidance method for unmanned surface vessels based on the improved artificial potential field repulsive force model according to claim 1 is characterized in that: The desired steering angle in step (3) It is the angle between the obstacle avoidance force direction of the USV and the forward direction of the USV.

7. The obstacle avoidance method for an unmanned surface ship based on an improved artificial potential field repulsive force model according to claim 1 is characterized in that: The process of controlling the steering of the USV in step (4) is: Obstacle avoidance force on USV Provide the moving direction for the USV to avoid obstacles, and drive the USV to move in the moving direction through the added moving speed; As the expected speed, the expected steering angle and expected speed Actual angle with USV and actual speed Subtract and get the steering angle error Error with moving speed : ； in, and is the actual steering angle and moving speed of the unmanned ship obtained by calculating the kinematic and dynamic models in step (1); According to the steering angle error Error with moving speed , get the forward thrust required for the unmanned ship to move and steering thrust , the movement of the unmanned ship is realized through the kinematic and dynamic models of the unmanned ship in step (1).

8. The method for avoiding obstacles on a surface unmanned ship based on an improved artificial potential field repulsive force model according to claim 7 is characterized in that: The forward thrust required for the unmanned ship to move and steering thrust It is obtained through the adjustment of the ADRC controller. The specific process is as follows: The ADRC controller includes a tracking differentiator, nonlinear state error feedback, an extended state observer and a disturbance compensation link; ① Tracking differentiator; Design a discrete tracking differentiator as follows: ； in, represents the input signal, represents the first-order derivative of the input signal, Represents the second-order derivative of the input signal; is the desired turning angle of the unmanned ship, is the expected speed of the unmanned ship; is the filtering step length, which is used to suppress the influence of noise interference; is the filter sampling factor, After confirming, take , control overshoot and suppress noise amplification; is the speed factor. As the speed factor increases, Tracking the input signal speed is increasing, Considered as the differential form of the input signal; represents the optimal speed control comprehensive function, where The function is obtained as follows: ； ② Extended state observer; According to the formula of the unmanned ship kinematics and dynamics model in step (1), let , the nonlinear part of the unmanned ship kinematics and dynamics model is regarded as a disturbance and , so the formula of the unmanned ship kinematics and dynamics model is rewritten as: ； when and When is known, the extended state observer is assumed to be: ； exist and In the unknown case, the unknown variable and Expand to new state variables and ,Right now: ； make , Substitute into the rewritten USV kinematic and dynamic model formulas, and expand to obtain a new linear system: ； By analyzing the new linear system, we get the new extended state observer: ； in, represents the observer parameters, represents the output variable of the extended state observer, Represents the new state variable and The observed value of The specific form of the function is as follows: ； in, express The linear interval width of the function, represents the parameters of the extended state observer and , , ; When the parameter values ​​of are determined, the various state variables are estimated: ； ③Nonlinear state error feedback; In order to stabilize the formulas of the unmanned ship kinematic and dynamic models and reduce the error, the nonlinear state error feedback control rate form is adopted: ； In the formula, Indicates the response speed parameter of ADRC controller, and Making it bigger will increase the response speed. Increasing it will inhibit the response speed; express The linear interval width of the function; select the parameter , ; For the error signal , and error tracking signal Satisfies the following relationship: ； ④ Disturbance compensation; In the extended state observer, the nonlinear part of the unmanned ship kinematic and dynamic model is equivalent to the disturbance The estimation is performed, and finally the control input of the kinematic and dynamic model of the unmanned ship is obtained by compensating the total disturbance: ； In this way, the forward thrust required by the unmanned ship model is obtained and steering thrust .

Images