Unmanned boat path tracking method and electronic equipment

Abstract

The invention relates to an unmanned boat path tracking method and electronic equipment, which introduce tracking error, tracking error change rate and speed into the line of sight guidance law, solve the problem that the unmanned boat generates a drift angle under the interference of the external environment, resulting in a decrease in path tracking accuracy, and improve the oscillation amplitude and number of times in the adjustment process; the invention introduces speed limit in the path tracking process, improves the tracking accuracy and safety of the unmanned boat in the turning process; a controller adopts multi-PID switching control based on T-S fuzzy model, and uses a differential evolution algorithm to respectively adjust a group of optimal PID parameters at each typical working speed of the unmanned boat, so that when the speed of the unmanned boat changes in a large range, multiple controllers can switch smoothly, ensuring better tracking control performance at each typical working speed; compared with other evolutionary algorithms such as genetic algorithms, the differential evolution algorithm has a fast convergence speed and is relatively stable, and repeated operations can converge to the same solution.

Description

Technical Field

[0001] The invention relates to an unmanned boat path tracking method and electronic equipment, in particular to an unmanned boat path tracking method based on an improved line of sight method and switching control, and relates to the field of unmanned boat path tracking control. Technical Background

[0002] Unmanned boat path tracking refers to the navigation of unmanned boats according to the planned route. It is a basic function of unmanned boats. The accuracy of path tracking directly affects the execution effect of autonomous tasks. Due to the nonlinear and uncertain characteristics of the unmanned boat system itself, it is difficult to establish an accurate mathematical model. Therefore, the PID control method is most commonly used in path tracking engineering practice. However, unmanned boats are susceptible to time-varying environmental interference such as wind, waves, and currents during navigation, resulting in a drift angle, and the path tracking accuracy based on traditional line of sight method and PID method will decrease. When the speed and other states of the unmanned boat change, if the controller parameters are not re-adjusted, the control performance will also deteriorate. In order to ensure the accuracy of unmanned boat path tracking, it is necessary to improve the adaptability of guidance and control methods to complex environments and large-scale changes in speed.

[0003] To address this problem, Jiang Quanquan et al. proposed a highly anti-disturbance adaptive path following method and system, with publication number CN108415423A. The method introduced tracking errors into the solution of the convergence circle radius and line of sight angle of the line of sight guidance law, and combined with the MFAC heading control algorithm to eliminate the influence of uncertain factors such as water flow interference and model perturbation on path tracking, but did not consider the controller effect and adaptability under the condition of large-scale speed changes. Zhang Shanjia et al. proposed a wave-resistant and energy-saving unmanned boat path tracking method, with publication number CN108227715A. This method introduced time-varying drift angle compensation in the calculation of the expected heading angle of the line of sight guidance law, added a wave observation module and a fuzzy reasoning module, and had high requirements for navigation, visual detection equipment and algorithms, and was relatively complex to implement. Summary of the invention

[0004] The purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art, provide an unmanned boat path tracking method and electronic equipment, improve the traditional line of sight method, introduce tracking error, tracking error change rate and speed into the line of sight guidance law, solve the problem that the unmanned boat generates a drift angle under the interference of the external environment, resulting in a decrease in path tracking accuracy, and at the same time improve the oscillation amplitude and number of times in the adjustment process, thereby improving the tracking accuracy and safety of the unmanned boat during turning.

[0005] The above-mentioned purpose of the present invention is mainly achieved through the following technical solutions:

[0006] A method for tracking an unmanned boat path, comprising:

[0007] S1. Set N waypoints P i Composed of the expected path, i∈N, N≥2, two adjacent waypoints P i , P i+1 To form a route segment, P i is the starting point of the current route segment, P i+1 The end point of the current route segment;

[0008] S2. Determine whether the following conditions are met: If the current position P of the unmanned boat is equal to the waypoint P i+1 Distance PP i+1 ≥Γ, then go to step S3. If PP i+1 <Γ and the last waypoint P is not reached N , then switch to the next route segment and go to step S3. If PP i+1 <Γ and reaches the last waypoint P N , enter step S9, where Γ is the constant value of the route segment update distance;

[0009] S3. Calculate the current route section P of the unmanned boat i P i+1 The track error e d (k);

[0010] S4, calculating the forward distance Δ of the virtual target point of the unmanned boat;

[0011] S5, according to the track error e d (k) and the foresight distance Δ, calculate the desired heading angle ψ d ;

[0012] S6, according to the heading angle ψ of the unmanned boat and the desired heading angle ψ d , calculate the heading angle error e ψ (k), where: e ψ (k) = ψ d -ψ;

[0013] S7. Get speed error e V (k), according to the speed error e V (k) Obtaining throttle command and controlling the speed of the unmanned boat by controlling the throttle opening;

[0014] S8, according to the heading angle error e ψ (k) obtaining a rudder angle command, achieving unmanned boat track tracking by adjusting the heading angle, and returning to step S2;

[0015] S9. End.

[0016] In the above-mentioned unmanned boat path tracking method, the step S3 calculates the current route segment P of the unmanned boat. i Pi+1 The track error e d (k) include:

[0017] Track error of unmanned boat e d (k) is the current position P(x p ,y p ) to route segment P i P i+1 The vertical distance of the straight line is positive on the left side of the planned route and negative on the right side. The calculation formula is as follows:

[0018]

[0019] Where: ab is route segment P i P i+1 The angle with the x-axis in the north-east coordinate system, ψ ap is the waypoint P i and the current position of the unmanned boat P(x p ,y p ) and the x-axis in the north-east coordinate system; (x a ,y a ) is the starting point P of the current route segment i The coordinates of .

[0020] In the unmanned boat path tracking method, the calculation of the forward distance Δ of the unmanned boat virtual target point in step S4 includes:

[0021] Solve the current position of the unmanned boat P(x p ,y p ) to route segment P i P i+1 The perpendicular line to the straight line is i P i+1 The coordinates of the intersection point of the straight line are Q(x q ,y q ), determine the position of point Q, if Q is in route segment P i P i+1 If Q is on route segment P, then λ=1. i P i+1 If λ=0, the specific calculation formula is as follows:

[0022]

[0023] Where: V is the current speed of the unmanned boat, Δ is the forward distance, that is, the distance between the virtual target point and the vertical projection point of the unmanned boat on the current route segment, Δ max , Δ min is the maximum and minimum value of the foresight distance, σ is the adjustment factor, which is a constant; QP i From point Q to waypoint Pi The distance, QP i+1 From point Q to waypoint P i+1 The distance (x b ,y b ) is the end point of the current route segment P i+1 The coordinates of .

[0024] In the unmanned boat path tracking method, the step S5 is based on the track error e d (k) and the foresight distance Δ to calculate the desired heading angle ψ d include:

[0025]

[0026] Among them, η i (k) is the track error integral term running to the kth cycle, is the integral term coefficient, a positive real number, η i The calculation formula of (k) is as follows

[0027]

[0028] η i (k-1) is the track error integral term running to the k-1th cycle, T is the sampling period, η max is the upper limit of the track error integration, the coefficient β and e d The relationship between (k) is

[0029]

[0030] in is the derivative of the track tracking error, ε1 and ε2 are positive real numbers.

[0031] In the above unmanned boat path tracking method, the speed error e is obtained in step S7. V (k) include:

[0032] Design speed limit V for unmanned boats dmax ,

[0033] The host computer sets the desired speed V of the unmanned boat c ,

[0034] According to the V dmax and V c Get the input desired speed V of the speed controller d , V d To obtain V dmax and V c The minimum value, that is: V d =min(V c ,V dmax );

[0035] Speed ​​error e V (k) is obtained by the following formula:

[0036] e V (k) = V d -V

[0037] Where: V is the current speed of the unmanned boat.

[0038] In the unmanned boat path tracking method, the unmanned boat speed limit V dmax The calculation formula is as follows:

[0039]

[0040] l=min(PP i ,PP i+1 )

[0041] Among them, V max 、V min are the maximum and minimum values ​​of the speed limit, l is the distance between the unmanned boat and the nearest turning point; PP i is the current position P of the unmanned boat and the waypoint P i Distance between, PP i+1 is the current position P of the unmanned boat and the waypoint P i+1 The distance between them; tanh() is the double tangent function; κ v is a constant coefficient.

[0042] In the unmanned boat path tracking method, the heading angle error e in S8 is calculated based on the heading angle error e. ψ (k) Obtaining the rudder angle command includes:

[0043] Calculate the optimal controller parameter k at different operating speeds pq , k dq , k iq ;

[0044] Get the optimal controller parameter k pq , k dq , k iq The weight w corresponding to each parameter at different working speeds q ;

[0045] According to the parameter k pq , k dq , k iq and weight w q The rudder angle command δ(k) is calculated.

[0046] In the above unmanned boat path tracking method, the optimal controller parameter k is calculated under different working conditions. pq, k dq , k iq include:

[0047] (1) Select m typical working speeds, adjust a set of PID parameters at each typical working speed of the unmanned boat, and design m PID sub-controllers. The form of the sub-controller is as follows, where m>1;

[0048]

[0049] (3) Use differential evolution algorithm to adjust the sub-controller parameter k pq , k dq , k iq Perform offline optimization. The specific steps are as follows:

[0050] (2.1) Determine the range of each parameter;

[0051] (2.2) Randomly generate M individuals to form the initial population, each individual consists of an n-dimensional vector;

[0052] (2.3) Decode each individual in the population into a corresponding parameter value, and use this parameter to calculate the cost function value J;

[0053] (2.4) Use differential evolution operators to perform mutation, crossover, and selection operations on the current population to generate the next generation population;

[0054] (2.5) Repeat steps (2.3) and (2.4) until the maximum number of evolution generations is reached.

[0055] In the above unmanned boat path tracking method, the optimal controller parameter k is obtained pq , k dq , k iq The weight w corresponding to each parameter at different working speeds q include:

[0056] The sub-controller switching adopts the fuzzy reasoning method based on the Takagi-Sugeno model. The current speed V of the unmanned boat is used as the input of the fuzzy switching rule. The current speed V of the unmanned boat is fuzzily divided into fuzzy sets {V1, V2, ..., Vm}. The membership function of V adopts the trapezoidal membership function. The weight w is calculated. q .

[0057] In the above unmanned boat path tracking method, according to the parameter k pq , k dq , k iq and weight w q The rudder angle command δ(k) is calculated as follows:

[0058] The PID controller parameters are obtained by the weighted average method, and the total output of the controller is:

[0059]

[0060] Where: w q =μ Vq (V) represents the weight of the qth rule, μ Vq (V) is the membership function of the fuzzy set Vq of the linguistic variable value; T is the control period; e ψ (k) is the heading angle error of the kth cycle, e ψ (k-1) is the heading angle error of the k-1th period.

[0061] An electronic device, comprising a memory and a processor:

[0062] The memory is used to store one or more computer instructions;

[0063] The processor is configured to execute the one or more computer instructions to:

[0064] S1. Set N waypoints P i Composed of the expected path, i∈N, N≥2, two adjacent waypoints P i , P i+1 To form a route segment, P i is the starting point of the current route segment, P i+1 The end point of the current route segment;

[0065] S2. Determine whether the following conditions are met: If the current position P of the unmanned boat is equal to the waypoint P i+1 Distance PP i+1 ≥Γ, then go directly to step S3. If PP i+1 <Γ and the last waypoint P is not reached N , then switch to the next route segment and go to step S3. If PP i+1 <Γ and reaches the last waypoint P N , enter step S9, where Γ is the constant value of the route segment update distance;

[0066] S3. Calculate the current route section P of the unmanned boat i P i+1 The track error e d (k);

[0067] S4, calculating the forward distance Δ of the virtual target point of the unmanned boat;

[0068] S5, according to the track error e d (k) and the foresight distance Δ, calculate the desired heading angle ψ d ;

[0069] S6, according to the heading angle ψ of the unmanned boat and the desired heading angle ψ d , calculate the heading angle error e ψ (k), where: e ψ (k) = ψ d -ψ;

[0070] S7. Get speed error e V (k), according to the speed error e V (k) Obtaining throttle command and controlling the speed of the unmanned boat by controlling the throttle opening;

[0071] S8, according to the heading angle error e ψ (k) obtaining a rudder angle command, achieving unmanned boat track tracking by adjusting the heading angle, and returning to step S2;

[0072] S9. End.

[0073] Compared with the prior art, the present invention has at least the following beneficial effects:

[0074] (1) The present invention improves the traditional line-of-sight method by introducing tracking error, tracking error change rate and speed into the line-of-sight guidance law, thereby solving the problem that the unmanned boat generates a drift angle under the interference of the external environment, resulting in a decrease in path tracking accuracy, and at the same time improving the oscillation amplitude and number of times in the adjustment process.

[0075] (2) The present invention introduces a speed limit in the path tracking process to improve the tracking accuracy and safety of the unmanned boat during turning.

[0076] (3) The present invention adopts a multi-PID switching control based on the TS fuzzy model through the controller, and uses the differential evolution algorithm to adjust a set of optimal PID parameters at each typical working speed of the unmanned boat, so that when the speed of the unmanned boat changes over a large range, the multi-controller can switch smoothly, ensuring better tracking control performance at each typical working speed;

[0077] (4) The present invention uses differential evolution to adjust the three parameters of PID. It can find suitable parameters without giving the initial parameters of the regulator. Compared with other evolutionary algorithms such as genetic algorithms, differential evolution algorithms converge quickly and are relatively stable. Repeated operations can converge to the same solution. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 It is a schematic diagram of the line of sight guidance method and plane coordinates of the present invention;

[0079] Figure 2 It is a structural diagram of the multi-pid switching controller of the present invention;

[0080] Figure 3The basic operation flow of offline optimization of pid parameters based on differential evolution of the present invention;

[0081] Figure 4 Schematic diagram of the membership function of the fuzzy switch input V of the present invention. DETAILED DESCRIPTION

[0082] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0083] The present invention provides an unmanned boat path tracking method based on improved line of sight method and switching control, and the specific implementation steps are:

[0084] Step 1: Set N waypoints P i Composed of the expected path, i∈N, N≥2, two adjacent waypoints P i , P i+1 To form a route segment, P i is the starting point of the current route segment, P i+1 The end point of the current route segment;

[0085] In an optional embodiment, N waypoints P are set. n ,n=0,1,...,N(N≥2), the longitude and latitude of the waypoints and the position of the unmanned boat are all converted to the north-east coordinate system, the x-axis points to the north, and the y-axis points to the east, simplifying the motion and control problem of the unmanned boat into a plane motion control problem in the north-east coordinate system. For example, the current position P of the unmanned boat is taken as the first waypoint P0, then the path contains N+1 waypoints, namely P n , n=0,1,...,N(N≥2), the starting point P of the current route segment i Initialized to P0, the end point of the current route segment P i+1 Initialized to P1. n The coordinates in the north-east coordinate system are expressed as (x n ,y n ).

[0086] Step 2: According to the current position P of the unmanned boat and the end point P of the current route segment i+1 The distance between the unmanned boat and the waypoint P determines whether the route segment needs to be updated and whether the following conditions are met: i+1 Distance PP i+1 ≥Γ, then go directly to step 3. If PP i+1 <Γ and the last waypoint P is not reached N , then switch to the next route segment and go to step 3. If PP i+1 <Γ and reaches the last waypoint P N, go to step 9, where Γ is the constant value of the route segment update distance, and Γ selects an appropriate positive real number according to parameters such as the turning radius at the working speed;

[0087] Step 3: Calculate the current route segment P i P i+1 The angle with the x-axis, i.e. the planned heading of this route segment ψ ab , calculate the current route segment P i P i+1 The opposite direction of the angle with the x-axis ψ ba , ψ ab , ba The calculation formula is as follows:

[0088]

[0089]

[0090] Calculate the route segment starting point P i and the position of the unmanned boat P(x p ,y p ) and the x-axis:

[0091]

[0092] Then we can get the current track error e d (k), the track error of the unmanned boat is the current position P i To the straight line P i P i+1 The vertical distance is positive on the left side of the planned route and negative on the right side. The calculation formula is as follows:

[0093]

[0094] Where: (x a ,y a ) is the starting point P of the current route segment i The coordinates of .

[0095] Step 4: The LOS line-of-sight guidance method is to create a virtual tracking target on the route segment, and then guide the unmanned boat to sail along the line of sight to the tracking target. The virtual tracking target determination method in the present invention is the lookahead-based method. The distance Δ between the virtual target and the vertical projection point of the unmanned boat on the route segment in the lookahead method is a variable real number. First, the current unmanned boat position P(x p ,y p ) to route segment P i P i+1 The vertical projection point Q(x q ,y q) coordinates, the position of point Q is Figure 1 As shown. Determine the position of point Q if point Q is on line segment P i P i+1 If Q is on line segment P, then λ=1. i P i+1 If Q is on line segment P, then λ = 0. i P i+1 Outside, and Q is far from P i If the point is closer, the virtual target point is P i point; if point Q is on line segment P i P i+1 Outside, and Q is far from P i+1 If the point is closer, the virtual target point is P i+1 point; if point Q is on line segment P i P i+1 The foresight distance Δ varies according to the speed. The foresight distance Δ of the virtual target point in the line of sight method is calculated as follows:

[0096]

[0097] Where: V is the current speed of the unmanned boat, Δ is the forward distance, that is, the distance between the virtual target point and the vertical projection point of the unmanned boat on the current route segment, Δ max , Δ min is the maximum and minimum value of the foresight distance, σ is the adjustment factor, which is a constant; QP i From point Q to waypoint P i The distance, QP i+1 From point Q to waypoint P i+1 distance.

[0098] Step 5: Use the track error e obtained in step 3 d (k) and the foresight distance Δ obtained in step 4, calculate the desired heading angle ψ d The track error integral term is introduced into the calculation of the desired heading angle. According to the size and sign of the track error and the rate of change of the track error, it is determined whether the track error integral term needs to be accumulated and the upper limit of the integral is set. d The calculation method is as follows:

[0099]

[0100] Among them, η i (k) is the track error integral term running to the kth cycle, is the integral term coefficient, a positive real number, η i The calculation formula of (k) is as follows

[0101]

[0102] η i (k-1) is the track error integral term running to the k-1th cycle, T is the sampling period, η max is the upper limit of the track error integration, the coefficient β and e d The relationship between (k) is

[0103]

[0104] in is the derivative of the track tracking error, ε1 and ε2 are positive real numbers.

[0105] Step 6: Read the heading angle ψ of the unmanned boat output by the navigation information fusion module, and calculate the heading angle ψ of the unmanned boat according to the heading angle ψ of the unmanned boat and the desired heading angle ψ. d , calculate the heading angle error e ψ (k) where:

[0106] e ψ (k) = ψ d -ψ.

[0107] Step 7: Calculate the speed limit during track tracking. Design a speed limit V for the unmanned boat. dmax , V dmax As the distance between the unmanned boat and the turning point decreases, V dmax The calculation formula is as follows:

[0108]

[0109] l=min(PP i ,PP i+1 )

[0110] Among them, V max 、V min are the maximum and minimum values ​​of the speed limit, l is the distance between the unmanned boat and the nearest turning point; PP i is the current position P of the unmanned boat and the waypoint P i Distance between, PP i+1 is the current position P of the unmanned boat and the waypoint P i+1 The distance between them; tanh() is the double tangent function; κ v is a constant coefficient.

[0111] So we can get the input desired speed V of the speed controller d :

[0112] V d =min(V c ,V dmax )

[0113] Where Vc is the expected speed set by the host computer, take the expected speed V set by the host computer c and speed limit V dmax The minimum value of is the reference input of the speed controller, which changes smoothly as the distance from each turning point changes.

[0114] Read the unmanned boat speed value V output by the navigation information fusion module and calculate the speed error

[0115] e V (k) = V d -V

[0116] Using the obtained velocity error e V (k) Design a speed controller, obtain the throttle command, and achieve speed control of the unmanned boat by adjusting the throttle opening.

[0117] Step 8: Use the heading angle error e obtained in step 6 ψ (k) Design a heading controller to obtain the rudder angle command, and achieve the track tracking effect by adjusting the heading angle. First, calculate the optimal controller parameter k at different working speeds. pq , k dq , k iq ; Obtain the optimal controller parameter k pq , k dq , k iq The weight w corresponding to each parameter at different working speeds q According to the parameter k pq , k dq , k iq and weight w q The rudder angle command δ(k) is calculated.

[0118] The change of speed, load and other factors will cause the mathematical model of the motion of the unmanned boat to be perturbed. For some unmanned boats whose speed changes greatly during operation, the system characteristics change greatly, and the model perturbation has a significant impact on the unmanned boat. When the controller is based on the steady mathematical model or designed under a certain specific working condition, the control effect will be significantly deteriorated when it is far away from the designed working point, and the performance requirements cannot be met. The present invention uses a fuzzy weighting method to design the PID controller parameters of the speed change process. According to the speed state information of the unmanned boat, a fuzzy reasoning method based on the Takagi-Sugeno model is used to obtain the degree to which the current state of the unmanned boat is close to a typical working speed, and the weight coefficient is calculated. The controller parameters of different working conditions are weighted to design the switching controller, which makes up for the deficiency of the limited application range of the single PID control, avoids the sudden change of the control quantity in the conventional switching logic, and realizes the smooth switching of multiple controllers, so that the unmanned boat can obtain a better control effect at each typical working speed.

[0119] like Figure 2 The structure diagram of the multi-PID switching controller of the present invention is shown. First, the parameter describing the working state of the unmanned boat is the speed. A set of PID parameters are adjusted at three typical working speeds of the unmanned boat (low speed, medium speed, and high speed) using the differential evolution algorithm. Three PID sub-controllers are designed so that each sub-controller has a good control effect at the working point and a certain control effect near the working point to meet the performance requirements. The form of the sub-controller is as follows:

[0120]

[0121] In an optional embodiment, the sub-controller parameter k is modified using a differential evolution algorithm. pq , k dq , k iq Perform offline optimization, such as Figure 3 The figure shows the basic operation flow of offline optimization of pid parameters based on differential evolution of the present invention, and the specific steps are as follows:

[0122] (1) Determine the approximate range of each parameter. In this example, k p The value range of k is [0,20], d The value range of is [0,20], and no integral term is added;

[0123] (2) Randomly generate M individuals to form the initial population, each of which consists of an n-dimensional vector

[0124] x i (0) = (x i1 (0),x i2 (0),x i3 (0),...,x in (0)),i=1,2,3,...,M

[0125] The value is as follows:

[0126]

[0127] In the formula, x ij (0) represents the jth "gene" of the i-th "chromosome" (or individual) in the 0th generation of the population, and are the upper and lower bounds of the jth gene on the ith chromosome, respectively, and rand ij (0,1) is a random decimal between [0,1]. In this example, the population size M = 50, n = 2;

[0128] (3) Each individual in the population is decoded into a corresponding parameter value, and the cost function value J is calculated using this parameter. The cost function takes into account the optimization of the dynamic characteristics of the transition process and the prevention of excessive control energy. The time integral of the weighted sum of the absolute value of the error and the square of the control input is used as the minimum objective function for parameter selection. The index function is as follows:

[0129]

[0130] Wherein, e(t) is the system error, u(t) is the controller output, w1 and w2 are weights, in this embodiment, w1=0.99, w2=0.01;

[0131] (4) Use the differential evolution operator to perform mutation, crossover, and selection operations on the current population to generate the next generation population. The specific method is as follows:

[0132] ① Mutation operation

[0133] Randomly select 3 individuals x from the population p1 、x p2 、x p3 , and i≠p1≠p2≠p3, then the mutation operation

[0134] for

[0135] h ij (t+1)=x p1j (t)+F(x p2j (t)-x p3j (t))

[0136] Where F is the scaling factor, in this example, F = 1.5, x p2j (t)-x p3j (t) is the differentiation vector,

[0137] p1, p2, and p3 are random integers, representing the sequence number of the individual in the population. In the process of evolution, in order to ensure the validity of the solution, the boundary of each "gene" in the "chromosome" (individual) must be checked. If the boundary conditions are not met, the "gene" is regenerated by random method;

[0138] ②Cross operation

[0139] For the tth generation population {x i (t)}, and its variant intermediate {h i (t+1)} to perform crossover operation between individuals. The specific operation is as follows:

[0140]

[0141] Where randl ijis a random decimal between [0,1], CR is the crossover probability, CR∈[0,1], in this example CR=0.6;

[0142] ③Select operation

[0143] Test vector v i (t+1) and the target vector x i (t) Compare the indicator functions and select individuals to enter the next generation population:

[0144]

[0145] (5) Repeat steps (3) and (4) until the maximum number of evolution generations is reached.

[0146] The sub-controller switching adopts the fuzzy reasoning method based on the Takagi-Sugeno model. The speed V is used as the input of the fuzzy switching rule. The speed V is fuzzily divided into fuzzy sets {V1, V2, ..., Vm}. The membership function of V adopts the trapezoidal membership function. The weight is calculated and the PID controller parameter k p , k d , k i As the output of the fuzzy switch, the fuzzy rule is:

[0147] R q :If V is Vq then k p =k pq , k d =k dq , k i =k iq , q=1,2,...,m

[0148] In an optional embodiment, the speed V is used as the input of the fuzzy switching rule. The value range of V is [0,35], and the required domain is [0,35]. The speed V is fuzzy divided into fuzzy sets {VS, VM, VB}, and the elements of the fuzzy set represent low speed, medium speed, and high speed respectively. The membership function of V adopts a trapezoidal membership function. The membership function curve is as follows: Figure 4 The corresponding expression is as follows:

[0149]

[0150] pid controller parameter k p , k d , k i As the output of the fuzzy switch. The present invention adopts zero-order TS fuzzy rules. The TS-type fuzzy inference system adopts an operation that combines fuzzy inference with defuzzification. The consequent part of the fuzzy rule expresses the output as a linear combination of the input. The specific rules are:

[0151] R 1 :If V is VS then k p =k p1 , k d =k d1 , k i =k i1

[0152] R 2 :If V is VM then k p =k p2 , k d =k d2 , k i =k i2

[0153] R 3 :If V is VB then k p =k p3 , k d =k d3 , k i =k i3

[0154] The system includes m rules, and the PID controller parameters are obtained by weighted average method. In an optional embodiment, the PID controller parameters are obtained by weighted average method, and then the total output of the controller is obtained as follows:

[0155]

[0156] Where w q =μ Vq (V) represents the weight of the qth rule, μ Vq (V) is the membership function of the fuzzy set Vq of the linguistic variable value, T is the control period; e ψ (k) is the heading angle error of the kth cycle, e ψ (k-1) is the heading angle error of the k-1th period; represents e from time 0 to time k ψ (k) T cumulative sum.

[0157] From the above formula, it can be seen that the total output is the sum of the outputs of each sub-controller according to a certain weight. The rudder angle command output by the controller is sent to the steering actuator, and then return to step 2.

[0158] Step 9: End.

[0159] The purpose of the present invention is to improve the adaptability of the traditional line-of-sight guidance law and PID control method to complex environments and large-scale changes in speed. The traditional line-of-sight method is improved, and the tracking error, the rate of change of the tracking error and the speed are introduced into the line-of-sight guidance law to solve the problem that the unmanned boat generates a drift angle under the interference of the external environment, resulting in a decrease in the path tracking accuracy, while improving the oscillation amplitude and number of times in the adjustment process. Speed ​​limit is introduced in the path tracking process to improve the tracking accuracy and safety of the unmanned boat during the turning process. The controller adopts multi-PID switching control based on the TS fuzzy model, and uses the differential evolution algorithm to adjust a set of optimal PID parameters at each typical working speed of the unmanned boat, so that when the speed of the unmanned boat changes over a large range, the multiple controllers can switch smoothly to ensure better tracking control performance at each typical working speed.

[0160] The above description is only the best specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0161] The contents not described in detail in the specification of the present invention belong to the common knowledge of the professionals in this field.

Claims

1. A method for tracking a path of an unmanned boat, characterized in that: include: S1. Set N waypoints P i Composed of the expected path, i∈N, N≥2, two adjacent waypoints P i , P i+1 To form a route segment, P i is the starting point of the current route segment, P i+1 The end point of the current route segment; S2. Determine whether the following conditions are met: If the current position P of the unmanned boat is equal to the waypoint P i+1 Distance PP i+1 ≥Γ, then go to step S3. If PP i+1 <Γ and the last waypoint P is not reached N , then switch to the next route segment and go to step S3. If PP i+1 <Γ and reaches the last waypoint P N , enter step S9, where Γ is the constant value of the route segment update distance; S3. Calculate the current route section P of the unmanned boat i P i+1 Track error ed(k); S4, calculating the forward distance Δ of the virtual target point of the unmanned boat; S5, according to the track error e d (k) and the foresight distance Δ, calculate the desired heading angle ψ d ; S6, according to the heading angle ψ of the unmanned boat and the desired heading angle ψ d , calculate the heading angle error e ψ (k), where: e ψ (k) = ψ d -ψ; S7. Get speed error e V (k), according to the speed error e V (k) Obtaining throttle command and controlling the speed of the unmanned boat by controlling the throttle opening; S8, according to the heading angle error e ψ (k) obtaining a rudder angle command, achieving unmanned boat track tracking by adjusting the heading angle, and returning to step S2; S9, end; In step S3, the current route section P of the unmanned boat is calculated. i P i+1 The track error ed(k) includes: Track error of unmanned boat e d (k) is the current position P(x p ,y p ) to route segment P i P i+1 The vertical distance of the straight line is positive on the left side of the planned route and negative on the right side. The calculation formula is as follows: Where: ab is route segment P i P i+1 The angle with the x-axis in the north-east coordinate system, ψ ap is the waypoint P i and the current position of the unmanned boat P(x p ,y p ) and the x-axis in the north-east coordinate system; (x a ,y a ) is the starting point P of the current route segment i The coordinates of The calculation of the forward-looking distance Δ of the virtual target point of the unmanned boat in step S4 includes: Solve the distance from the current position of the unmanned boat P (xp, yp) to the route segment P i P i+1 The perpendicular line to the straight line is i P i+1 The intersection coordinates of the straight lines are Q(x q ,y q ), determine the position of point Q, if Q is in route segment P i P i+1 If Q is on route segment P, then λ=1. i P i+1 If λ=0, the specific calculation formula is as follows: Where: V is the current speed of the unmanned boat, ΔΔ is the forward distance, that is, the distance between the virtual target point and the vertical projection point of the unmanned boat on the current route segment, Δ max , Δ min is the maximum and minimum value of the foresight distance, σ is the adjustment factor, which is a constant; QP i From point Q to waypoint P i The distance, QP i+1 From point Q to waypoint P i+1 The distance (x b ,y b ) is the end point of the current route segment P i+1 The coordinates of In step S5, according to the track error e d (k) and the foresight distance Δ to calculate the desired heading angle ψ d include: Among them, η i (k) is the track error integral term running to the kth cycle, is the integral term coefficient, a positive real number, η i The calculation formula of (k) is as follows: η i (k-1) is the track error integral term running to the k-1th cycle, T is the sampling period, η max is the upper limit of the track error integration, the coefficient β and e d The relationship between (k) is in is the derivative of the track tracking error, ε1 and ε2 are positive real numbers.

2. The unmanned boat path tracking method according to claim 1, characterized in that: In step S7, the speed error e is obtained. V (k) include: Design speed limit V for unmanned boats dmax , The host computer sets the desired speed V of the unmanned boat c , According to the V dmax and V c Get the input desired speed V of the speed controller d , V d To obtain V dmax and V c The minimum value, that is: V d =min(V c ,V dmax ); Speed ​​error e V (k) is obtained by the following formula: yes V (k)=V d -V Where: V is the current speed of the unmanned boat.

3. The unmanned boat path tracking method according to claim 2, characterized in that: The unmanned boat speed limit V dmax The calculation formula is as follows: l=min(PP i ,PP i+1 ) Among them, V max 、V min are the maximum and minimum values ​​of the speed limit, l is the distance between the unmanned boat and the nearest turning point; PP i is the current position P of the unmanned boat and the waypoint P i Distance between, PP i+1 is the current position P of the unmanned boat and the waypoint P i+1 The distance between them; tanh() is the double tangent function; κ v is a constant coefficient.

4. The unmanned boat path tracking method according to claim 1, characterized in that: In S8, according to the heading angle error e ψ (k) Obtaining the rudder angle command includes: Calculate the optimal controller parameter k at different operating speeds pq , k dq , k iq ; Get the optimal controller parameter k pq , k dq , k iq The weight w corresponding to each parameter at different working speeds q ; According to the parameter k pq , k dq , k iq and weight w q The rudder angle command δ(k) is calculated.

5. The unmanned boat path tracking method according to claim 4, characterized in that: The optimal controller parameter k is calculated under different working conditions. pq , k dq , k iq include: (1) Select m typical working speeds, adjust a set of PID parameters at each typical working speed of the unmanned boat, and design m PID sub-controllers. The form of the sub-controller is as follows, where m>1; Where, T is the control period; (2) Use differential evolution algorithm to modify the sub-controller parameter k pq , k dq , k iq Perform offline optimization. The specific steps are as follows: (2.1) Determine the range of each parameter; (2.2) Randomly generate M individuals to form the initial population, each individual consists of an n-dimensional vector; (2.3) Decode each individual in the population into a corresponding parameter value, and use this parameter to calculate the cost function value J; (2.4) Use differential evolution operators to perform mutation, crossover, and selection operations on the current population to generate the next generation population; (2.5) Repeat steps (2.3) and (2.4) until the maximum number of evolution generations is reached.

6. The unmanned boat path tracking method according to claim 4 or 5, characterized in that: Get the optimal controller parameter k pq , k dq , k iq The weight w corresponding to each parameter at different working speeds q include: The sub-controller switching adopts the fuzzy reasoning method based on the Takagi-Sugeno model. The current speed V of the unmanned boat is used as the input of the fuzzy switching rule. The current speed V of the unmanned boat is fuzzily divided into fuzzy sets {V1, V2, ..., Vm}. The membership function of V adopts the trapezoidal membership function. The weight w is calculated. q .

7. The unmanned boat path tracking method according to claim 6, characterized in that: According to the parameter k pq , k dq , k iq and weight w q Calculate the rudder angle command δ(k) include: The PID controller parameters are obtained by the weighted average method, and the total output of the controller is: Where: w q =μ Vq (V) represents the weight of the qth rule, μ Vq (V) is the membership function of the fuzzy set Vq of the linguistic variable value; T is the control period; e ψ (k) is the heading angle error of the kth cycle, e ψ (k-1) is the heading angle error of the k-1th period.

8. An electronic device, characterized in that: Including memory and processor: The memory is used to store one or more computer instructions; The processor is configured to execute the one or more computer instructions to: S1. Set N waypoints P i Composed of the expected path, i∈N, N≥2, two adjacent waypoints P i , P i+1 To form a route segment, P i is the starting point of the current route segment, P i+1 The end point of the current route segment; S2. Determine whether the following conditions are met: If the current position P of the unmanned boat is equal to the waypoint P i+1 Distance PP i+1 ≥Γ, then go directly to step S3. If PP i+1 <Γ and the last waypoint P is not reached N , then switch to the next route segment and go to step S3. If PP i+1 <Γ and reaches the last waypoint P N , enter step S9, where Γ is the constant value of the route segment update distance; S3. Calculate the current route section P of the unmanned boat i P i+1 Track error ed(k); S4, calculating the forward distance Δ of the virtual target point of the unmanned boat; S5, according to the track error e d (k) and the foresight distance Δ, calculate the desired heading angle ψ d ; S6, according to the heading angle ψ of the unmanned boat and the desired heading angle ψ d , calculate the heading angle error e ψ (k), where: e ψ (k) = ψ d -ψ; S7. Get speed error e V (k), according to the speed error e V (k) Obtaining throttle command and controlling the speed of the unmanned boat by controlling the throttle opening; S8, according to the heading angle error e ψ (k) obtaining a rudder angle command, achieving unmanned boat track tracking by adjusting the heading angle, and returning to step S2; S9, end; In step S3, the current route section P of the unmanned boat is calculated. i P i+1 The track error ed(k) includes: Track error of unmanned boat e d (k) is the current position P(x p ,y p ) to route segment P i P i+1 The vertical distance of the straight line is positive on the left side of the planned route and negative on the right side. The calculation formula is as follows: Where: ab is route segment P i P i+1 The angle with the x-axis in the north-east coordinate system, ψ ap is the waypoint P i The angle between the line connecting the current position of the unmanned boat P(xp,yp) and the x-axis in the north-east coordinate system; (x a ,y a ) is the starting point P of the current route segment i The coordinates of The calculation of the forward-looking distance Δ of the virtual target point of the unmanned boat in step S4 includes: Solve the distance from the current position of the unmanned boat P (xp, yp) to the route segment P i P i+1 The perpendicular line to the straight line is i P i+1 The intersection coordinates of the straight lines are Q(x q ,y q ), determine the position of point Q, if Q is in route segment P i P i+1 If Q is on route segment P, then λ=1. i P i+1 If λ=0, the specific calculation formula is as follows: Where: V is the current speed of the unmanned boat, ΔΔ is the forward distance, that is, the distance between the virtual target point and the vertical projection point of the unmanned boat on the current route segment, Δ max , Δ min is the maximum and minimum value of the foresight distance, σ is the adjustment factor, which is a constant; QP i From point Q to waypoint P i The distance, QP i+1 From point Q to waypoint P i+1 The distance (xb, yb) is the end point P of the current route segment. i+1 The coordinates of In step S5, according to the track error e d (k) and the foresight distance Δ to calculate the desired heading angle ψ d include: Among them, η i (k) is the track error integral term running to the kth cycle, is the integral term coefficient, a positive real number, η i The calculation formula of (k) is as follows: η i (k-1) is the track error integral term running to the k-1th cycle, T is the sampling period, η max is the upper limit of the track error integration, the coefficient β and e d The relationship between (k) is where e d (k) is the derivative of the track tracking error, and ε1 and ε2 are positive real numbers.

Images