Obstacle Avoidance Method for Unmanned Surface Vessel Based on Improved Artificial Potential Field Repulsion Force Model

Abstract

The present invention relates to the technical field of obstacle avoidance for unmanned boats, and discloses an obstacle avoidance method for surface unmanned boats based on an improved artificial potential field repulsive force model, which includes the following steps: (1) establishing a mathematical model of the unmanned boat, including kinematic and dynamic models; (2) improving the artificial potential field repulsive force model based on piecewise power decay; (3) calculating the obstacle avoidance force received by the USV through step (2), and obtaining the expected steering angle of the USV to turn; (4) controlling the steering of the USV according to the obtained expected steering angle to avoid obstacles. By improving the artificial potential field repulsive force model with piecewise power decay, the present invention realizes obstacle avoidance for surface unmanned boats. 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, and improves the safety performance of USV navigation.

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 Art

[0002] The safe navigation of an unmanned surface vehicle (USV) on the sea is crucial, 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, traditional APF methods have 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] In view of 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 an improved artificial potential field repulsive force model of the present invention includes the following steps:

[0008] (1) Establish a mathematical model of the unmanned surface vehicle (USV), including a kinematic model and a dynamic model;

[0009] (2) Improved artificial potential field repulsive force model based on piecewise power decay (IAPF-PPR model);

[0010] (3) Obtain the obstacle avoidance force received by the USV through the improved artificial potential field repulsive force model in step (2) and obtain the desired steering angle for the USV to turn ;

[0011] (4) Control the turning of the USV according to the obtained desired steering angle so as to avoid obstacles.

[0012] The establishment process of the kinematic model in the said step (1) is as follows:

[0013] 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 attached body coordinate system fixed on the hull ;

[0014] 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:

[0015] ;

[0016] 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 turning.

[0017] The dynamic model in the said step (1) is as follows:

[0018] ;

[0019] 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 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 turning angular velocity of the USV.

[0020] The improved artificial potential field repulsive force model in step (2) is as follows:

[0021] ;

[0022] 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;

[0023] The smoothing scheduling term is a very small positive value, taking .

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

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

[0026] The process of controlling the steering of the USV in step (4) is as follows:

[0027] The obstacle avoidance force received by the USV provides the moving direction for the obstacle avoidance of the USV. The USV is driven to move in the moving direction by an externally applied moving speed; the moving speed of the USV is regarded as the desired speed, the desired steering angle and the desired speed and the actual angle of the USV and the actual speed Take the difference to obtain the steering angle error and the moving speed error :

[0028] ;

[0029] 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);

[0030] According to the steering angle error and the moving speed error , obtain the forward thrust and the steering thrust required for the movement of the unmanned ship, and realize the movement of the unmanned ship through the kinematic and dynamic models of the unmanned ship in step (1).

[0031] The forward thrust and the steering thrust required for the movement of the unmanned ship are obtained through the adjustment of the ADRC controller (active disturbance rejection controller), and the specific process is as follows:

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

[0033] ① Tracking differentiator;

[0034] Design a discrete tracking differentiator as follows:

[0035] ;

[0036] Among them, 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 as follows:

[0037] ;

[0038] ② Extended state observer;

[0039] According to the formulas of the kinematic and dynamic models of the unmanned ship in step (1), let , and the non - linear part of the kinematic and dynamic models of the unmanned ship is regarded as a perturbation and . Therefore, the formulas of the kinematic and dynamic models of the unmanned ship are rewritten as:

[0040] ;

[0041] When and are known, the extended state observer is set as:

[0042] ;

[0043] In the case where and are unknown, the unknown variables and are extended to new state variables and , that is:

[0044] ;

[0045] Let , substitute it into the rewritten kinematic and dynamic model formulas of the USV, and expand to obtain a new linear system:

[0046] ;

[0047] Analyze the new linear system to obtain a new extended state observer:

[0048] ;

[0049] 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:

[0050] ;

[0051] Among them, represents Width of the linear interval of the function Represents the parameters of the extended state observer and , , ; When the parameter values of are determined, each state variable is estimated:

[0052] ;

[0053] ③ Nonlinear state error feedback;

[0054] In order to stabilize the formulas of the kinematic and dynamic models of the unmanned ship and reduce errors simultaneously, a nonlinear state error feedback control law form is adopted:

[0055] ;

[0056] In the formula, Represents the ADRC controller response speed parameter, and An increase in will increase the response speed, An increase in will suppress the response speed; Represents Width of the linear interval of the function; Select the parameter , ; For the error signal , and the error tracking signal Satisfy the following relationship:

[0057] ;

[0058] ④ Disturbance compensation;

[0059] 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, the control input of the kinematic and dynamic models of the unmanned ship is obtained through compensation for the total disturbance:

[0060] ;

[0061] In this way, the required forward thrust and the turning thrust of the unmanned ship model are obtained.

[0062] The present invention has the following beneficial effects:

[0063] By improving the piecewise power decay of the artificial potential field repulsive force model, obstacle avoidance of the surface unmanned ship is achieved. By introducing the piecewise power decay term and the regularization parameter , 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. It can achieve a minimum obstacle avoidance distance of 11.4 m for static obstacles and a minimum distance of 8.6 m for dynamic obstacles, demonstrating excellent obstacle avoidance performance. Description of the Drawings

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

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

[0066] Figure 3 It is a schematic diagram of the expected steering angle of USV.

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

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

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

[0070] Figure 7 is Figure 6 The enlarged view at point A in Detailed Implementation Manner

[0071] 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 , 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.

[0072] The obstacle avoidance method of the surface unmanned ship based on the improved artificial potential field repulsive force model of the present invention specifically includes the following processes.

[0073] I. USV mathematical model.

[0074] 1. The kinematic model of USV;

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

[0076] ;(1)

[0077] In Equation (1), 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.

[0078] 2. The dynamic model of the USV;

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

[0080] (2)

[0081] In Equation (2), , , 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.

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

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

[0084] ; (3)

[0085] In formula (3), represents the moving 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. For this obstacle avoidance method based on the exponential model, effective obstacle avoidance will only occur 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.

[0086] 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 repulsion force model based on piecewise power decay (Improved Artificial Potential Field Model with PiecewisePower-Decay Repulsion, IAPF-PPR). On the basis of the traditional APF (artificial potential field repulsion force model), this IAPF-PPR model introduces a non-exponential and continuously adjustable power function term, effectively solving the problem of numerical stability of the exponential repulsion force. The expression of this IAPF-PPR model is:

[0087] ; (4)

[0088] In formula (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, that is , represents the position of the obstacle, represents the minimum safety 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 decay curve, and represent the repulsion intensity coefficients.

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

[0090] The obstacle avoidance force obtained from the IAPF-PPR (Improved Artificial Potential Field Repulsion Force Model Based on Piecewise Power Decay) model is used to calculate the expected steering angle of the USV.

[0091] In IAPF-PPR, the obstacle avoidance force acting on the USV is denoted as , which provides the moving direction for the USV to avoid obstacles. The USV is driven to move in the moving direction by the applied moving speed, and the expected 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 expected angle is the angle between the direction of the USV's obstacle avoidance force and the direction of the USV's forward movement.

[0092] Regarding the moving speed of the USV as the expected speed, 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 , which are used as the inputs of the ADRC controller, that is:

[0093] ;

[0094] Among them, and are the actual steering angle and moving speed of the unmanned ship obtained from the kinematic and dynamic models of the unmanned ship in step (1).

[0095] According to the input steering angle error and the moving speed error , the forward thrust and the steering thrust required for the unmanned ship to move are obtained through the adjustment of the ADRC controller, and the movement of the unmanned ship is realized through the kinematic and dynamic models of the unmanned ship in step (1).

[0096] The forward thrust and the steering thrust required for the unmanned ship to move are obtained through the adjustment of the ADRC controller, and the specific process is as follows.

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

[0098] 1. Tracking differentiator;

[0099] In order to extract and estimate the high-order derivatives from the input signal and help the extended state observer obtain accurate state information, a discrete tracking differentiator is designed as follows:

[0100] ;

[0101] where, denotes the input signal, denotes the first-order derivative of the input signal, denotes the second-order derivative of the input signal, denotes the optimal speed control synthesis function:

[0102] ;

[0103] 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 and faster. When it reaches a certain level, is regarded as the differential form of the input signal;

[0104] 2. Extended state observer;

[0105] According to the formulas of the kinematics and dynamics models of the unmanned ship in step (1), let , and the non-linear part of the kinematics and dynamics models of the unmanned ship is regarded as the disturbance and . Therefore, the formulas of the kinematics and dynamics models of the unmanned ship are rewritten as:

[0106] ;

[0107] When and are known, the extended state observer is set as:

[0108] ;

[0109] In the case where and are unknown, the unknown variables and are extended to new state variables and , that is:

[0110] ;

[0111] Let , substitute it into the rewritten USV kinematic and dynamic model formula, and expand to obtain a new linear system:

[0112] ;

[0113] Analyze the new linear system to obtain a new extended state observer:

[0114] ;

[0115] 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:

[0116] ;

[0117] Among them, represents the linear interval width of the function, represents the extended state observer parameters and , , ; For the parameter , when appropriate parameter values are selected, each state variable is estimated:

[0118] ;

[0119] 3. Nonlinear state error feedback;

[0120] In order to stabilize the formula of the USV kinematic and dynamic model and reduce the error at the same time, a nonlinear state error feedback control law form is adopted:

[0121] ;

[0122] In the formula, represents the ADRC controller response speed parameter, and increasing will increase the response speed, increasing will suppress the response speed; represents the linear interval width of the function; Select the parameter , ; For the error signal , Sum error tracking signal Satisfy the following relationship:

[0123] ;

[0124] 4. Disturbance compensation;

[0125] In the extended state observer, the nonlinear part of the kinematic and dynamic models of the unmanned ship is equivalent to a disturbance Estimated, and finally the control input of the kinematic and dynamic models of the unmanned ship is obtained through the compensation of the total disturbance as:

[0126] ;

[0127] In this way, the forward thrust required by the unmanned ship model can be obtained And turning thrust .

[0128] IV. Experimental simulation.

[0129] 1. Static obstacle experiment;

[0130] To verify the effectiveness of the IAPF-PPR model proposed by 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 showing the path trajectories under the three methods and the distance change between the USV and the obstacles.

[0131] 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 its 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 high collision risk. While the IAPF-PPR method guides the path to deflect in advance near the obstacle, the minimum obstacle avoidance distance reaches 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.

[0132] 2. Dynamic obstacle experiment;

[0133] In order to further verify the performance of the IAPF-PPR model proposed by 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.

[0134] 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 finally 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 a more reliable solution for the stable navigation and obstacle avoidance of the USV.

Claims

1. An obstacle avoidance method for an unmanned surface vessel based on an improved artificial potential field repulsive force model, characterized in that, Including the following steps: (1) Establish a mathematical model of the unmanned ship, including a kinematic model and a dynamic model; (2) Improve the artificial potential field repulsive force model based on piecewise power decay; (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)According to the obtained desired steering angle control the steering of the USV to avoid obstacles; The improved artificial potential field repulsive force model in 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 used to avoid division by zero and numerical mutations; represents a power exponent parameter that controls the shape of the decay curve; and represent the repulsive force intensity coefficients.

2. The obstacle avoidance method for the unmanned surface vehicle based on the improved artificial potential field repulsive force model according to claim 1, characterized in that, The establishment process of the kinematic model in step (1) is: When an unmanned ship moves on the sea surface, it has six-degree-of-freedom motion. Two coordinate systems are established: the geodetic 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, and 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.

3. The obstacle avoidance method for the unmanned surface vehicle based on the improved artificial potential field repulsive force model according to claim 1, characterized in that, The dynamic model in step (1) is as follows: ； Among them, , , represent the mass coefficients composed of the USV mass, , , 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 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.

4. The obstacle avoidance method for an unmanned surface vehicle based on an improved artificial potential field repulsive force model according to claim 1, characterized in that, The smooth scheduling item .

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

6. The obstacle avoidance method for the unmanned surface vessel based on the improved artificial potential field repulsive force model according to claim 1, characterized in that, The process of controlling the steering of the USV in step (4) is: Obstacle avoidance force received by USV Provide the moving direction for the obstacle avoidance of the USV, and drive the USV to move in the moving direction by the externally applied moving speed; the moving speed of the USV is regarded 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 : ； 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 and the moving speed error , the forward thrust and the steering thrust required for the unmanned ship to move are obtained, and the movement of the unmanned ship is realized through the kinematic and dynamic models of the unmanned ship in step (1).

Images