An improved los and incremental pid path tracking method for intelligent unmanned ship

Abstract

The application discloses an improved LOS and incremental PID path tracking method of an intelligent unmanned ship, and belongs to the field of intelligent unmanned ship control technology.The method comprises the following steps: an intelligent unmanned ship acquires a planned expected path; an expected heading angle is calculated by using an improved LOS algorithm according to the expected path, in the improved LOS algorithm, a forward-looking distance is set as a function related to a sailing speed, and is adjusted through a gain coefficient, then a finite-time observer is used to compensate a sideslip angle in real time; a control signal is generated by using an incremental PID control method according to a difference between an actual heading measured by a sensor carried by the intelligent unmanned ship and an expected heading angle given by a guidance layer, in the incremental PID control method, PID parameters are optimized by using an improved particle swarm algorithm; and the intelligent unmanned ship tracks expected heading movement.The application improves the guidance precision of the LOS algorithm, and improves the response speed and stability of the intelligent unmanned ship.

Description

Technical Field

[0001] This invention relates to the field of unmanned vessel path tracking technology, and in particular to an improved LOS and incremental PID path tracking method for intelligent unmanned vessels. Background Technology

[0002] Currently, various unmanned technologies, such as drones, unmanned vehicles, and unmanned vessels, are widely used in society. Among them, unmanned surface vehicles (USVs), as a type of unmanned equipment, have advantages such as small size, low cost, and high flexibility, making them suitable for various complex water areas and showing broad application prospects in many fields (such as military, industry, and scientific research).

[0003] With its numerous advantages, intelligent unmanned surface vessels (USVs) hold great promise for water quality sampling. USVs can replace manual navigation to designated sampling points for water sample collection. Since they operate at multiple sampling points, they need to be able to navigate along unobstructed paths that allow them to reach all points. Furthermore, USVs are prone to deviating from their intended path due to environmental disturbances, necessitating a robust control system that keeps them on the correct course.

[0004] The goal of path tracking control for intelligent unmanned surface vessels (USVs) is to enable them to navigate along a given desired path while minimizing errors. Path tracking methods for USVs mainly include PID control, Lyapunov direct method, backstepping method, sliding mode control, and active disturbance rejection methods.

[0005] Another important approach to ship path tracking is to use the Path of Observation (LOS) to reduce the output dimension of the path tracking controller. In traditional LOS guidance laws, the value of Δ is typically 1.5 to 2 times the length of the unmanned surface vessel (USV). Since Δ is a constant value, a small Δ value at a high USV speed will cause system overshoot; conversely, a large Δ value at a low USV speed will result in slow tracking speed.

[0006] During actual navigation, the sideslip angle of an intelligent unmanned vessel is often time-varying, making it difficult to measure the sideslip angle at every moment. Therefore, in order to ensure the accuracy of path tracking and the stability of the entire path tracking control system, it is necessary to appropriately compensate for the sideslip angle to prevent the intelligent unmanned vessel from deviating from the desired path.

[0007] Currently, significant progress has been made in the theoretical research of control methods for path tracking of intelligent unmanned vessels, but the results in practical applications are not ideal. Moreover, many researchers have not conducted detailed studies on the impact of environmental interference, and this factor is often ignored when designing controllers. Summary of the Invention

[0008] The technical problem to be solved by the present invention is to provide an improved path tracking method for intelligent unmanned vessels using LOS and incremental PID, which addresses the deficiencies in the existing technology.

[0009] The technical solution adopted by this invention to solve its technical problem is:

[0010] This invention provides an improved path tracking method for intelligent unmanned surface vessels using LOS and incremental PID, the method comprising the following steps:

[0011] Step 1: The intelligent unmanned vessel obtains the planned desired path;

[0012] Step 2: In the guidance layer, the desired heading angle is calculated using an improved LOS algorithm based on the desired path. In the improved LOS algorithm, the forward look-ahead distance is set as a function related to the sailing speed and is optimized by a gain coefficient. Then, a finite-time observer is proposed to compensate for the sideslip angle in real time.

[0013] Step 3: In the control layer, based on the difference between the actual heading measured by the sensors on the intelligent unmanned ship and the desired heading angle given by the guidance layer, an incremental PID control method is used to generate a control signal. In the incremental PID control method, the PID parameters are optimized by an improved particle swarm algorithm.

[0014] Step 4: In the execution layer, after receiving the generated control signal, the drive system generates corresponding state changes, causing the intelligent unmanned vessel to move in the desired direction and eventually track the desired path.

[0015] Furthermore, the method for calculating the desired heading angle in step 2 of the present invention includes:

[0016] The method for calculating the expected heading angle of a straight path is as follows:

[0017] Point P k (x k ,y k P is the starting point of the path. t (x t ,y t P is the center point of the intelligent unmanned vessel. k+1 (x k+1 ,y k+1 Point P is the next path target point, with the center position of the intelligent unmanned vessel as the reference point. t (x t ,y t Draw a circle with center P and radius R, intersecting P. k and P k+1 Connect P k P k+1The two points intersect, and the distance from point P is recorded. k+1 The nearest intersection point is P. los (x los ,y los ), satisfying the formula:

[0018]

[0019] Desired path P in inertial coordinate system k P k+1 azimuth angle γ s for:

[0020]

[0021] When the intelligent unmanned vessel travels along the desired path, the sideslip angle β is:

[0022] β = arctan2(v, u)

[0023] Where the forward speed is u and the lateral speed is v;

[0024] By locating the current position of the intelligent unmanned surface vessel, the real-time lateral tracking error y is calculated. e for:

[0025] y e =-(x t -x k sin(γ) s )+(y t -y k cos(γ) s )

[0026] Then the expected heading angle ψ of the straight path d for:

[0027]

[0028] Where Δ is the forward sight distance.

[0029] Furthermore, the method for calculating the desired heading angle in step 2 of the present invention includes:

[0030] The method for calculating the expected heading angle of a curved path is as follows:

[0031] Establish a path-tangential SF coordinate system. The projection of the intelligent unmanned vessel's position (x, y) onto the reference path is (x... p (s),y p (s)), where s is the path variable, and the desired path P(s) consisting of a series of path points is a non-closed path; the path tangential reference frame is at point (x p (s),y p(s) Clockwise rotation angle γ relative to the inertial coordinate system s for:

[0032] γ s =atan2(y′) p (s),x′ p (s))

[0033] in Path variable s satisfies The path tracking error in the path tangent reference frame is expressed as:

[0034]

[0035] Because x p (s),y p (s) represents the projection point of the coordinates (x, y) of the intelligent unmanned vessel in the inertial coordinate system onto the reference path, and x is obtained. e =0, then the lateral tracking error is written as:

[0036] y e =-(xx) p (s))sin(γ s )+(yy p (s))cos(γ s )

[0037] Calculating the first derivative yields the formula:

[0038]

[0039] Combining formulas x, y, ψ represent the position and azimuth of the intelligent unmanned surface vessel in the inertial coordinate system, and u, v, r represent the forward velocity, lateral velocity, and bow roll rate of the intelligent unmanned surface vessel in the attached coordinate system; We obtain:

[0040]

[0041] Where ζ is the heading angle and U is the speed of the intelligent unmanned vessel; due to the influence of external disturbances such as wind, waves and currents, the heading angle and azimuth angle are often not equal, satisfying ζ=ψ+β, where β is the sideslip angle.

[0042] Furthermore, the method for optimization using a gain coefficient in step 2 of the present invention is as follows:

[0043] The forward sight distance Δ is set as a function of the sailing speed and optimized using a gain coefficient ε. Δ is designed as follows:

[0044] Δ=U / ε

[0045] When ε is constant, the greater the speed U of the intelligent unmanned vessel, the greater Δ; the smaller the speed U of the intelligent unmanned vessel, the smaller Δ.

[0046] Furthermore, the method for real-time compensation of the sideslip angle using a finite-time observer proposed in step 2 of the present invention is as follows:

[0047] Since the sideslip angle is less than 5 degrees, it is assumed that it has an upper bound, i.e., there exists a constant β. max Satisfying |β|≤β max Let the intermediate variable g be:

[0048] g=Uβcos(ψ-γ s )

[0049] And there exists a satisfying The positive constants make The formula can then be rewritten as:

[0050]

[0051] Actual speed U and azimuth If we can measure the value of β, we can construct a finite-time observer to estimate the intermediate variable g. If we can estimate g, we can calculate the estimated value of β as follows:

[0052]

[0053] in and These are the estimated values ​​of g and β, respectively;

[0054] The finite-time observer is designed as follows:

[0055]

[0056]

[0057]

[0058] In the formula, λ i >0, i=1,2,L>0,sig a (x)=|x| a sgn(x);

[0059] The desired heading angle is designed as follows:

[0060]

[0061] Where angle γ s It can be derived that the lateral tracking error y e Measurable Obtained by a finite-time observer.

[0062] Furthermore, the incremental PID control method used in step 3 of the present invention is specifically as follows:

[0063] Let the continuous time t be represented by sampling points kΔt, where k represents the sampling point number and Δt is the sampling period. The integration element is replaced by accumulation, and the differentiation element is replaced by difference. That is:

[0064]

[0065] The expression for the incremental PID control algorithm is:

[0066] Δδ(k)=K p ·[e(k)-e(k-1)]+K i ·e(k)+K d ·[e(k)-2e(k-1)+e(k-2)]

[0067] In the formula, e(k), e(k-1), and e(k-2) are the heading angle errors at times k, k-1, and k-2, respectively, and δ(t) is the rudder angle value at time k; K p K is the proportionality coefficient. i =K p Δt / Δt i The coefficient of the integral term, Δt i K is the integration time constant; d =K d Δt d / Δt is the coefficient of the differential term, which is the differential time constant.

[0068] Furthermore, the method for optimizing PID parameters using an improved particle swarm optimization algorithm in step 3 of this invention is as follows:

[0069] The particle swarm optimization algorithm is improved by introducing a nonlinear adaptive inertia parameter:

[0070] The position of the i-th particle in the entire d-dimensional search space is speed is Let N particles represent different solutions, and let the optimal position of the individual found by the i-th particle be... The optimal position among all particles is Then x i The fitness value is calculated by substituting it into the fitness function, and the velocity and position of each particle are updated according to the following formula:

[0071]

[0072]

[0073] In the formula, i = 1, 2, ..., N, t is the number of iterations, ω is the inertia weight, c1 and c2 are learning factors, and r1 and r2 are random numbers in the interval [0, 1].

[0074] To balance the effects of global search and local optimization, a combination of nonlinear and adaptive methods is used to adjust ω. The update formula for the inertia coefficient ω is as follows:

[0075] ω=ω max -(ω max -ω min )*artanht / t max

[0076] In the formula, ω max and ω min These represent the upper and lower bounds of the inertia weight of a particle in the particle swarm optimization algorithm, respectively, and t represents the current iteration number of the algorithm; t max The maximum number of iterations is determined by introducing the inverse hyperbolic tangent function artanh(x) to adaptively adjust the value of ω as the number of iterations changes.

[0077] The particle swarm optimization algorithm is improved by introducing sine and cosine factors:

[0078] Based on the introduction of nonlinear adaptive inertia parameters, sine and cosine factors are introduced to improve the equation for updating the position of each particle:

[0079]

[0080] In the formula, This represents the position of the i-th particle in the d-th dimension during the t-th iteration. This represents the optimal solution in the current iteration number t; r1, r2, and r3 are random numbers, r4 controls the switching selection of sine and cosine updates, and r4 ~ U[0,1] represents a random number between 0 and 1;

[0081] The global optimal position of the i-th particle is updated as follows:

[0082]

[0083] Each particle updates its velocity and position based on its own optimal position and the group's optimal position; this continues until a final termination condition is set, which may include the number of iterations or the fitness value.

[0084] Furthermore, in step 3 of the present invention, K is obtained by improving the particle swarm optimization algorithm. p K i and K d The values ​​of the three coefficients; in optimizing PID parameters using the improved particle swarm optimization algorithm, the time-weighted integral absolute error (ITAE) index is selected as the fitness function, defined as:

[0085]

[0086] In the formula, t is time, and e(t) is the deviation between the system input and output.

[0087] The beneficial effects of this invention are:

[0088] 1. This invention proposes a path tracking control method based on the line of sight (LOS) method and incremental PID control.

[0089] (1) The improved LOS algorithm is used to calculate the desired heading angle. The forward sight distance is designed as a function related to the desired speed, that is, the forward sight distance Δ is set as a function of the sailing speed, and is optimized by a gain coefficient ε to ensure that the desired path can be tracked in a shorter time.

[0090] (2) A finite-time observer is proposed to compensate for the sideslip angle in real time, which avoids the intelligent unmanned ship from deviating from the desired path, improves the stability of the entire path tracking control system, and improves the guidance accuracy of the LOS algorithm.

[0091] 2. This invention employs an incremental PID control method to design the heading controller for an intelligent unmanned surface vessel (USV), and optimizes the PID parameters based on an improved particle swarm optimization algorithm, thereby enhancing the USV's response speed and stability. Finally, simulation comparison experiments of heading control and path tracking verify the feasibility of the proposed method. Attached Figure Description

[0092] The present invention will be further described below with reference to the accompanying drawings and embodiments. In the accompanying drawings:

[0093] Figure 1 This is an overall method flowchart of an embodiment of the present invention;

[0094] Figure 2 This is a schematic diagram of the LOS geometry of an embodiment of the present invention;

[0095] Figure 3 This is a schematic diagram of the SF coordinate system according to an embodiment of the present invention;

[0096] Figure 4 This is a schematic diagram of the PID heading control principle according to an embodiment of the present invention;

[0097] Figure 5 This is a schematic diagram of the PID control principle according to an embodiment of the present invention;

[0098] Figure 6 This is a diagram showing the optimized PID parameter structure according to an embodiment of the present invention;

[0099] Figure 7This is the heading angle variation curve of the target heading angle of 10° under the interference-free condition according to an embodiment of the present invention;

[0100] Figure 8 This is the rudder angle variation curve when the target heading angle is 10° under undisturbed conditions according to an embodiment of the present invention;

[0101] Figure 9 This is the heading angle variation curve of the target heading angle of 20° under the interference-free condition according to an embodiment of the present invention;

[0102] Figure 10 This is the rudder angle variation curve when the target heading angle is 20° under undisturbed conditions according to an embodiment of the present invention;

[0103] Figure 11 This is the heading angle variation curve of the target heading angle of 10° under interference according to an embodiment of the present invention;

[0104] Figure 12 This is the rudder angle variation curve when the target heading angle is 10° under interference, according to an embodiment of the present invention;

[0105] Figure 13 This is the heading angle variation curve of the target heading angle of 20° under interference according to an embodiment of the present invention;

[0106] Figure 14 This is the rudder angle variation curve when the target heading angle is 20° under interference, according to an embodiment of the present invention;

[0107] Figure 15 These are the SFLOS and ALOS expected trajectories in this embodiment of the invention;

[0108] Figure 16 This refers to the lateral tracking error between SFLOS and ALOS in this embodiment of the invention.

[0109] Figure 17 This refers to the desired course and the actual course in an embodiment of the present invention.

[0110] Figure 18 This is the sideslip angle estimation curve according to an embodiment of the present invention. Detailed Implementation

[0111] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0112] Example 1

[0113] After obtaining the planned desired path, intelligent unmanned surface vessels also need to be able to navigate precisely along that path.

[0114] The motion control of an intelligent unmanned surface vessel (USV) is a hierarchical structure. First, the guidance layer generates a desired heading based on a given desired path using a guidance algorithm. Then, the control layer generates a control signal based on the difference between the actual heading measured by the USV's onboard sensors and the desired heading given by the guidance layer. Upon receiving the control signal, the execution layer drives the system to undergo corresponding state changes, causing the USV to move towards the desired heading to reduce heading deviation and ultimately track the desired path.

[0115] like Figure 1 As shown, the improved LOS and incremental PID path tracking method for intelligent unmanned surface vessels according to an embodiment of the present invention includes the following steps:

[0116] Step 1: The intelligent unmanned vessel obtains the planned desired path;

[0117] Step 2: In the guidance layer, the desired heading angle is calculated using an improved LOS algorithm based on the desired path. In the improved LOS algorithm, the forward look-ahead distance is set as a function related to the sailing speed and is optimized by a gain coefficient. Then, a finite-time observer is proposed to compensate for the sideslip angle in real time.

[0118] Step 3: In the control layer, based on the difference between the actual heading measured by the sensors on the intelligent unmanned ship and the desired heading angle given by the guidance layer, an incremental PID control method is used to generate a control signal. In the incremental PID control method, the PID parameters are optimized by an improved particle swarm algorithm.

[0119] Step 4: In the execution layer, after receiving the generated control signal, the drive system generates corresponding state changes, causing the intelligent unmanned vessel to move in the desired direction and eventually track the desired path.

[0120] Example 2

[0121] The LOS guidance law is a guidance method used in intelligent unmanned ships, mobile robots, and drones. It calculates the desired heading angle based on the ship's motion characteristics and by simulating manual rudder control. Its schematic diagram is shown below. Figure 2 As shown.

[0122] In this embodiment of the invention, point P k (x k ,y k P is the starting point of the path. t (x t ,y t P is the center point of the intelligent unmanned vessel. k+1 (x k+1 ,y k+1 Point P is the next path target point, with the center position of the intelligent unmanned vessel as the reference point.t (x t ,y t Draw a circle with center P and radius R, intersecting P. k and P k+1 Connect P k P k+1 The two points intersect, and the distance from point P is recorded. k+1 The nearest intersection point is P. los (x los ,y los If the point satisfies the following formula:

[0123]

[0124] like Figure 2 As shown, during actual movement, the intelligent unmanned surface vessel (USV) does not directly track the target point, but rather a virtual line-of-sight point at a certain distance from the target. Based on the geometric relationship between the USV's position and the line-of-sight point, the desired heading angle is calculated. By aligning the USV's heading with the line-of-sight angle and adjusting the rudder angle appropriately, the USV can reach the vicinity of the preset target point with a certain path-tracking accuracy, achieving good path-tracking performance.

[0125] Desired path P in inertial coordinate system k P k+1 azimuth angle γ s It can be calculated using the following formula:

[0126]

[0127] When an intelligent unmanned surface vessel (USV) navigates along a desired path, disturbances from the external environment (such as wind, waves, or currents) interact with its lateral velocity, resulting in a sideslip angle. The sideslip angle has a significant impact on tracking accuracy; the formula for the sideslip angle β is shown below:

[0128]

[0129] u and v represent the forward and lateral speeds of the intelligent unmanned surface vessel in the attached coordinate system.

[0130] By locating the current position of the intelligent unmanned surface vessel (USV), the vertical distance of the USV from the desired path can be calculated, i.e., the lateral tracking error y in real time. e The formula is shown below:

[0131]

[0132] Then the desired heading angle ψ can be obtained. d The formula is shown below:

[0133]

[0134] The above method can be used to calculate and track straight paths. To track curved paths, a path tangential (SF) coordinate system needs to be established, such as... Figure 3 As shown.

[0135] In a preferred embodiment of the present invention, the projection of the USV position (x,y) onto the reference path is (x... p (s),y p (s)), where s is the path variable, and the desired path P(s) consisting of a series of path points is a non-closed path. The path tangential reference frame is at point (x p (s),y p (s) Clockwise rotation angle γ relative to the inertial coordinate system s for:

[0136]

[0137] in Path variable s satisfies The path tracking error in the path tangent reference frame is expressed as:

[0138]

[0139] Because x p (s),y p (s) represents the projection point of the coordinates (x, y) of the intelligent unmanned vessel in the inertial coordinate system onto the reference path, from which we can obtain x e If = 0, then the lateral tracking error can be written as:

[0140]

[0141] Calculating the first derivative of formula (8), we obtain the following formula:

[0142]

[0143] Combining formulas x, y, ψ represent the position and azimuth of the intelligent unmanned surface vessel in the inertial coordinate system, and u, v, r represent the forward velocity, lateral velocity, and bow roll rate of the intelligent unmanned surface vessel in the attached coordinate system; We obtain:

[0144]

[0145] Where ζ is the heading angle. Due to the influence of external disturbances such as wind, waves, and currents, the heading angle and azimuth angle are often not equal, satisfying ζ=ψ+β. Under normal circumstances, the sideslip angle β is non-zero and time-varying. Moreover, the sideslip angle is very small during normal operation, usually less than 5 degrees. Therefore, it is reasonably approximated as sinβ≈β and cosβ≈1. Thus, formula (10) can be written as:

[0146]

[0147] Example 3

[0148] To achieve path tracking for the USV, the following key issues need to be addressed: (1) Selecting appropriate forward look distance Δ and controller parameters to obtain the desired heading angle ψ. d This makes the kinematic heading angle Design and control δ to conform to dynamics. Among them, ε1 and ε2 are very small positive numbers.

[0149] The forward look-ahead distance Δ determines the performance of the USV path tracking control system. By setting the forward look-ahead distance Δ as a function of the flight speed and optimizing it using a gain coefficient ε, Δ can be designed as follows:

[0150]

[0151] As can be seen from the above formula, when ε is constant, the larger the speed U of the intelligent unmanned vessel, the larger Δ will be, which can reduce the overshoot of the system; the smaller the speed U of the intelligent unmanned vessel, the smaller Δ will be, which can ensure that the desired path can be tracked in a shorter time.

[0152] In a preferred embodiment of the present invention, the sideslip angle of the intelligent unmanned surface vessel (USV) is often time-varying during actual navigation, making it difficult to measure the sideslip angle at every moment. Therefore, to ensure the accuracy of path tracking and the stability of the entire path tracking control system, the sideslip angle must be appropriately compensated to prevent the USV from deviating from the desired path. Therefore, the present invention employs a finite-time observer method to estimate the unknown time-varying sideslip angle.

[0153] Since the sideslip angle is usually less than 5 degrees, it is assumed that it has an upper bound, i.e., there exists a constant β. max Satisfying |β|≤β max

[67] . Let the intermediate variable g be:

[0154]

[0155] And there exists a satisfying The positive constants make The above expression can then be rewritten as:

[0156]

[0157] Actual speed U and azimuth It can be measured, γ s It can be calculated using formula (6). Therefore, a finite-time observer can be constructed to estimate the intermediate variable g. If g is estimated, the estimated value of β can be calculated using formula (14):

[0158]

[0159] in and These are the estimated values ​​for g and β, respectively.

[0160] The finite-time observer is designed as follows:

[0161]

[0162] In the formula, λ i >0, i=1,2,L>0,sig a (x)=|x| a sgn(x).

[0163] Therefore, the desired heading angle can be designed as shown in the following formula:

[0164]

[0165] Where angle γ s The lateral tracking error y can be derived from equation (6). e Measurable It can be obtained from a finite-time observer. For formula (17), we have:

[0166]

[0167]

[0168] Substituting into equation (11), we get:

[0169]

[0170] Example 4

[0171] The attitude sensor onboard the intelligent unmanned surface vessel (USV) measures the actual heading angle, while the SFLOS guidance law calculates the desired heading angle. By calculating the difference between the two, and then using a PID controller, the rudder angle of the USV can be calculated, thereby tracking and controlling its heading. Its control principle diagram is shown below. Figure 4 As shown.

[0172] PID control is a continuous-time control method. However, the actual motion control system of intelligent unmanned ships is a digital control system, which requires the analog signals to be converted into digital signals for processing.

[0173] The principle of PID control algorithm is as follows: Figure 5 As shown, the expression is as follows:

[0174]

[0175] In the formula, K P For proportional gain, K i For integral gain, K d Let e(t) be the differential gain, and e(t) be the error value, which in this invention is the difference between the desired heading and the actual heading.

[0176] Discretization makes PID control algorithms more accurate and stable, avoiding problems such as calculation errors and inconsistent sampling periods. Furthermore, in digital control systems, discretization facilitates real-time calculation and optimization of the controller, improving the performance and response speed of the control system. Representing continuous time t using sampling points kΔt, where k represents the sampling point number and Δt is the sampling period, we replace the integral element with accumulation and the derivative element with difference, i.e.:

[0177]

[0178] The PID heading control law after discretizing formula (21) can be expressed as:

[0179]

[0180] In the formula, δ(t) is the rudder angle value at time k; e(k) is the heading error value at time k; K p K is the proportionality coefficient. i =K p Δt / Δt i The coefficient of the integral term, Δt i K is the integration time constant; d =K d Δt d / Δt is the coefficient of the differential term, which is the differential time constant.

[0181] According to formula (21), the output of the discrete PID controller is related to the output of all past moments. Therefore, the deviation E needs to be continuously accumulated during the calculation process, which greatly increases the workload of the computer. When the hardware system fails, the rudder angle control will produce a huge deviation, causing the intelligent unmanned vessel to deviate significantly from the desired course, thereby reducing the control accuracy. In order to overcome the defects of the discrete PID control algorithm, this invention proposes an incremental PID control method, and this paper designs the course controller of the intelligent unmanned vessel based on this method.

[0182] The expression for the incremental PID control algorithm is:

[0183] Δδ(k)=K p ·[e(k)-e(k-1)]+K i ·e(k)+K d·[e(k)-2e(k-1)+e(k-2)] (24)

[0184] In the formula, e(k), e(k-1), and e(k-2) are the heading angle errors at times k, k-1, and k-2, respectively.

[0185] As can be seen from the above formula, incremental PID does not require recalculating the PID value each time, but only calculates the increment, which can greatly reduce the amount of calculation and response time. Because of the incremental output control, even if the hardware system fails, the control accuracy will not be seriously affected; through weighted processing, the intensity of the proportional, integral, and derivative components can be adjusted.

[0186] In a preferred embodiment of the present invention, an improved particle swarm optimization algorithm is used to optimize the PID parameters.

[0187] Because the final control effect of the PID control algorithm is related to K p K i and K d The values ​​of the three coefficients are related, and these values ​​are usually determined through trial and error based on experience, which is difficult to meet the requirements of control accuracy. To solve the problem of difficult PID parameter tuning, an improved particle swarm optimization algorithm is used to optimize the PID parameters. The structure diagram of the improved particle swarm optimization algorithm for PID parameter optimization is shown below. Figure 6 As shown.

[0188] The particle swarm optimization algorithm is improved by introducing a nonlinear adaptive inertia parameter:

[0189] The position of the i-th particle in the entire d-dimensional search space is speed is Let N particles represent different solutions, and let the optimal position of the individual found by the i-th particle be... The optimal position among all particles is Then x i The fitness value is calculated by substituting it into the fitness function, and the velocity and position of each particle are updated according to the following formula:

[0190]

[0191]

[0192] In the formula, i = 1, 2, ..., N, t is the number of iterations, ω is the inertia weight, c1 and c2 are learning factors, and r1 and r2 are random numbers in the interval [0, 1].

[0193] To balance the effects of global search and local optimization, a combination of nonlinear and adaptive methods is used to adjust ω. The update formula for the inertia coefficient ω is as follows:

[0194]

[0195] In the formula, ω max and ω min These represent the upper and lower bounds of the inertia weight of a particle in the particle swarm optimization algorithm, respectively, and t represents the current iteration number of the algorithm; t max The maximum number of iterations is determined by introducing the inverse hyperbolic tangent function artanh(x) to adaptively adjust the value of ω as the number of iterations changes.

[0196] The particle swarm optimization algorithm is improved by introducing sine and cosine factors:

[0197] Based on the introduction of nonlinear adaptive inertia parameters, sine and cosine factors are introduced to improve the equation for updating the position of each particle:

[0198]

[0199] In the formula, This represents the position of the i-th particle in the d-th dimension during the t-th iteration. This represents the optimal solution in the current iteration number t; r1, r2, and r3 are random numbers, r4 controls the switching selection of sine and cosine updates, and r4 ~ U[0,1] represents a random number between 0 and 1;

[0200] The global optimal position of the i-th particle is updated as follows:

[0201]

[0202] Each particle updates its velocity and position based on its own optimal position and the group's optimal position; this continues until a final termination condition is set, which may include the number of iterations or the fitness value.

[0203] K is obtained by improving the particle swarm optimization algorithm. p K i and K d The values ​​of the three coefficients; in optimizing PID parameters using the improved particle swarm optimization algorithm, the time-weighted integral absolute error (ITAE) index is selected as the fitness function, defined as:

[0204]

[0205] In the formula, t is time, and e(t) is the deviation between the system input and output.

[0206] Test Implementation Examples

[0207] Simulation experiments on the heading motion of an intelligent unmanned surface vessel were conducted on the Matlab 2020a platform to verify the performance of the PID heading controller with optimized parameters using the improved PSO-SC algorithm. The results were compared with those of a traditional PID heading controller and a PID heading controller based on the standard PSO algorithm under both undisturbed and disturbed environments.

[0208] The relevant parameters of the intelligent unmanned surface vessel (USV) are set using a second-order linear Nomoto model, with T = 62.38 and K = 0.31. The Nomoto model of the USV is then:

[0209]

[0210] Set the number of particles to 100 and the maximum number of iterations to t. max =50. ω in the improved particle swarm optimization algorithm max =0.8, ω min =0.3, c1=c2=2, μ=0.8. In the standard particle swarm optimization algorithm, ω=0.9, c1=c2=2.

[0211] Based on experience and algorithm optimization, the parameters of the three PID algorithms are shown in the table below.

[0212]

[0213] The simulation results are shown below.

[0214] (1) No interference

[0215] The curves showing the changes in heading angle and rudder angle of the intelligent unmanned surface vessel at a speed of U = 1 m / s and preset heading angles of 10° and 20° under the control of three different controllers are as follows: Figures 7-10 As shown.

[0216] Simulation results show that, under undisturbed conditions, the PID control algorithm based on the improved particle swarm optimization algorithm reaches the preset heading angle in a shorter time than the PID control algorithm based on the standard PSO algorithm and the traditional PID control algorithm. The rudder angle overshoot of the PID control algorithm based on the improved particle swarm optimization algorithm is smaller than that of the other two control algorithms.

[0217] (2) Under interference conditions

[0218] Intelligent unmanned ships are easily affected by external disturbances (such as wind, waves and currents) during actual navigation. Therefore, the situation when there is disturbance must be considered when conducting simulation verification. In order to simulate the disturbance caused by the environment, white noise is used to drive a typical second-order oscillating element

[71] . The second-order wave transfer function is:

[0219]

[0220] In the formula, w is the dominant wave frequency, K is the wave number, and ζ is the damping coefficient. In the simulation, the values ​​are taken as w = 0.606, K = 0.42, and ζ = 0.3.

[0221] Under interference conditions, with preset heading angles of 10° and 20°, the heading angle and rudder angle variation curves of the three heading controllers are as follows: Figures 11-14 As shown.

[0222] Simulation results show that, when faced with external environmental disturbances, the improved particle swarm optimization PID control algorithm exhibits smaller overshoot compared to the other two control algorithms and achieves better stability around the target heading angle. Furthermore, the rudder angle overshoot of the improved particle swarm optimization PID control algorithm is also smaller than that of the other two control algorithms.

[0223] In summary, by comparing experimental results under both interference-free and interference-affected conditions, it can be concluded that the PID heading controller optimized using the improved particle swarm optimization algorithm outperforms the other two heading controllers. Furthermore, the PID heading controller tuned using the improved particle swarm optimization algorithm also exhibits better anti-interference performance and robustness when encountering interference.

[0224] Path tracking simulation analysis:

[0225] The SFLOS guidance law of this invention was simulated and verified using Matlab, and compared with the existing ALOS technology. In SFLOS, ε = 0.25, velocity V = 1 m / s, and the parameters of the finite-time state observer were set to λ1 = 0.1, λ2 = 0.001, and L = 200. The PID parameters were tuned using an improved particle swarm optimization algorithm: k p =1.6327, k i =0.0174, k d =26.2376.

[0226] The initial position of the USV is set to (0,10), and the simulation results are as follows: Figures 15-18 As shown.

[0227] As shown in the figure, both guidance laws can guide the USV to the predetermined path. The SFLOS guidance law can bring the USV closer to the desired path more quickly and accurately, while the ALOS guidance law lags behind the SFLOS guidance law in approaching the desired path. It can be seen that the SFLOS path tracking control system designed in this invention can bring the lateral tracking error to a stable state more quickly than the ALOS control system.

[0228] With the addition of sideslip angle compensation, the intelligent unmanned surface vessel exhibits strong robustness against environmental disturbances and can quickly approach the desired heading angle. The finite-time observer enables rapid and accurate sideslip angle observation, with the observation error converging to zero within a finite time, significantly improving the guidance system's robustness to unknown sideslip angles.

[0229] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0230] It should be understood that those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. An improved path tracking method using LOS and incremental PID for intelligent unmanned surface vessels, characterized in that, The method includes the following steps: Step 1: The intelligent unmanned vessel obtains the planned desired path; Step 2: In the guidance layer, the desired heading angle is calculated using an improved LOS algorithm based on the desired path. In the improved LOS algorithm, the forward look-ahead distance is set as a function related to the sailing speed and is optimized by a gain coefficient. Then, a finite-time observer is proposed to compensate for the sideslip angle in real time. Step 3: In the control layer, based on the difference between the actual heading measured by the sensors on the intelligent unmanned ship and the desired heading angle given by the guidance layer, an incremental PID control method is used to generate a control signal. In the incremental PID control method, the PID parameters are optimized by an improved particle swarm algorithm. Step 4: In the execution layer, after receiving the generated control signal, the drive system generates corresponding state changes, causing the intelligent unmanned vessel to move in the desired direction and eventually track the desired path. The method for optimization using a gain coefficient in step 2 is as follows: forward sight distance Set as a function of sailing speed, and pass through a gain coefficient. Optimize, Designed as follows: when At a certain time, the speed of the intelligent unmanned ship The larger, the better The larger; the faster the intelligent unmanned ship travels. The smaller, The smaller; The method for real-time compensation of the sideslip angle using a finite-time observer proposed in step 2 is as follows: Since the sideslip angle is less than 5 degrees, it is assumed that it has an upper bound, i.e., there exists a constant. satisfy Let intermediate variables be defined. for: in, This indicates the azimuth angle of the intelligent unmanned surface vessel in the inertial coordinate system. This represents the desired path in the inertial coordinate system. The azimuth angle; And there exists a satisfying The positive constants make The formula can then be rewritten as: actual speed and azimuth We can measure and construct a finite-time observer to estimate intermediate variables. If estimated Then calculate The estimated value is: in and They are and The estimated value; The finite-time observer is designed as follows: In the formula, , , ; The desired heading angle is designed as follows: Among the angles It can be derived that the lateral tracking error Measurable Obtained by a finite-time observer; The method for optimizing PID parameters using the improved particle swarm optimization algorithm in step 3 is as follows: The particle swarm optimization algorithm is improved by introducing a nonlinear adaptive inertia parameter: No. Each particle throughout The position in the dimensional search space is The speed is ,use Each particle represents a different solution, the th... The optimal position of the individual found by each particle is The optimal position among all particles is Then The fitness value is calculated by substituting it into the fitness function, and the velocity and position of each particle are updated according to the following formula: In the formula, , For the number of iterations, For inertial weights, and As a learning factor, and For interval Random numbers within; To balance the effects of global search and local optimization, a combination of nonlinear and adaptive methods is used. Adjustments are made to the inertia coefficient. The update formula is as follows: In the formula, and These represent the upper and lower bounds of the inertia weight of a particle in the particle swarm optimization algorithm, respectively. This represents the current iteration number of the algorithm; To determine the maximum number of iterations, an inverse hyperbolic tangent function is introduced. make The value is adaptively adjusted as the number of iterations changes; The particle swarm optimization algorithm is improved by introducing sine and cosine factors: Based on the introduction of nonlinear adaptive inertia parameters, sine and cosine factors are introduced to improve the equation for updating the position of each particle: In the formula, Indicates the first The particle in the first In the nth iteration The position of the dimension Indicates the current iteration number The optimal solution in; It is a random number. It controls the updating and switching selection of sine and cosine. , represents a random number between 0 and 1; No. The global optimal position of each particle is updated as follows: Each particle updates its velocity and position based on its own optimal position and the group's optimal position; this continues until a final termination condition is set, which may include the number of iterations or the fitness value.

2. The path tracking method for intelligent unmanned vessels using improved LOS and incremental PID according to claim 1, characterized in that, The method for calculating the desired heading angle in step 2 includes: The method for calculating the expected heading angle of a straight path is as follows: point The starting point of the path, As the central point of the intelligent unmanned vessel, The point is the next path target point, with the center position of the intelligent unmanned vessel. With the center of the circle, Draw a circle with radius and and Connection Intersects at two points, record the distance between the points. The nearest intersection is It satisfies the formula: Desired path in inertial coordinate system azimuth for: When an intelligent unmanned vessel navigates along a desired path, the sideslip angle for: Among them, the forward speed is The lateral speed is ; By locating the current position of the intelligent unmanned surface vessel, the lateral tracking error in real time can be calculated. for: Then the expected heading angle of the straight path for: in, This is the forward sight distance.

3. The path tracking method for intelligent unmanned vessels using improved LOS and incremental PID according to claim 2, characterized in that, The method for calculating the desired heading angle in step 2 includes: The method for calculating the expected heading angle of a curved path is as follows: Establish the path tangential SF coordinate system and determine the position of the intelligent unmanned vessel. The projection of ) onto the reference path is ( ),in For path variables, the desired path consists of a series of waypoints. It is a non-closed path; the tangential reference frame of the path is at point ( Clockwise rotation angle relative to the inertial coordinate system for: in , Path variables satisfy The path tracking error in the path tangent reference frame is expressed as: because Represents the coordinates of the intelligent unmanned vessel in the inertial coordinate system. The projection point on the reference path is obtained. Then the lateral tracking error is written as: Calculating the first derivative yields the formula: Combining formulas , This indicates the position and azimuth of the intelligent unmanned surface vessel in the inertial coordinate system. Let the forward velocity, lateral velocity, and bow roll rate of the intelligent unmanned surface vessel in the attached coordinate system be represented; then: in For heading angle, The speed of the intelligent unmanned vessel; due to the influence of external disturbances such as wind, waves, and currents, the heading angle and azimuth angle are often not equal, satisfying the following... , It is the sideslip angle.

4. The path tracking method for intelligent unmanned vessels using improved LOS and incremental PID according to claim 1, characterized in that, The incremental PID control method used in step 3 is as follows: continuous time sampling points It means that among them Represents the sampling point number. For the sampling period, the integration stage is replaced by accumulation, and the differentiation stage is replaced by difference, that is: The expression for the incremental PID control algorithm is: In the formula, , , They are respectively , and The heading angle error at time [time] for The rudder angle value at any given moment; This is the coefficient for the proportional term; The coefficient of the integral term, The integral time constant; The coefficients of the differential term, is the differential time constant.

5. The path tracking method for intelligent unmanned vessels using improved LOS and incremental PID according to claim 4, characterized in that, In step 3, the improved particle swarm optimization algorithm is used to obtain... , and The values ​​of the three coefficients; in optimizing PID parameters using the improved particle swarm optimization algorithm, the time-weighted integral absolute error (ITAE) index is selected as the fitness function, defined as: In the formula, For time, This represents the deviation between the system input and output.

Images