Unmanned ship track tracking method based on model-free adaptive cooperative control

Abstract

The invention provides an unmanned ship track tracking method based on model-free adaptive cooperative control, and the method employs a data-driven observer to obtain an estimated value of a sideslip angle, thereby obtaining an expected course angle signal which fully considers the influence of the sideslip angle, and then according to the expected course angle signal, calculating the track of an unmanned ship according to the expected course angle signal. A course controller combining a model-free adaptive method and a cooperative control method is utilized to calculate and obtain a control signal of the unmanned ship, so that the unmanned ship can sail along a reference track. The method solves the problems of poor robustness, low response speed and the like when a conventional track tracking method is used, and solves the problem of unmanned ship control under the condition that a model is unknown. A guidance law based on a data-driven observer is adopted, so that the unmanned ship can quickly estimate a sideslip angle, interference of external stormy waves on the unmanned ship is further compensated, and a course controller combining a model-free adaptive method and a cooperative control method can further improve the robustness and response speed of a track system.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ship control, and particularly relates to a method for tracking the trajectory of an unmanned ship based on model-free adaptive cooperative control. Background Technique

[0002] An unmanned ship is a fully automatic surface robot that can sail on the water according to a preset task without remote control, relying on precise satellite positioning and its own sensors, and can be widely used in fields such as surveying, hydrology, and water quality monitoring. With the advancement and expansion of the application of unmanned ships, the environment in which unmanned ships perform tasks becomes more complex, and the trajectory tracking accuracy of unmanned ships becomes increasingly important.

[0003] In the prior art, as a mainstream solution for unmanned ships, a twin-propeller unmanned ship can only achieve forward and turning motions and cannot achieve lateral motion. In the design of the guidance law for unmanned ships, a line-of-sight guidance algorithm that ignores the sideslip angle is usually adopted. This type of guidance law is simple and easy to implement, but when this guidance law is applied in a complex environment, due to the increase in external interference, the tracking error of the unmanned ship will be greatly increased. In addition, the heading controller of an unmanned ship is usually designed using the PID algorithm, but the parameter tuning in the PID algorithm is mainly experimentally tuned by trial and error. Once it is found that the control performance is not good, only re-tuning can be carried out, resulting in problems of low efficiency and low robustness. Summary of the Invention

[0004] Object of the Invention: The object of the present invention is to provide a method for tracking the trajectory of an unmanned ship based on model-free adaptive cooperative control, which enables the unmanned ship to sail precisely along a reference trajectory.

[0005] Technical Solution: A method for tracking the trajectory of an unmanned ship based on model-free adaptive cooperative control includes the following steps:

[0006] S1. Calculate the lateral error and longitudinal error of the unmanned ship relative to the reference trajectory according to the current position information of the unmanned ship;

[0007] S2. Input the lateral error and longitudinal error into a data-driven observer to obtain an estimated value of the sideslip angle of the unmanned ship;

[0008] S3. Input the lateral error, the estimated value of the sideslip angle, and the tangent angle of the reference trajectory into the line-of-sight guidance algorithm to obtain the desired heading angle of the current unmanned ship;

[0009] S4. Establish a compact-form dynamic linearization data model of the current heading angular velocity of the unmanned ship, and use a parameter estimation algorithm to calculate the pseudo partial derivatives in the compact-form dynamic linearization data model;

[0010] S5. Construct the macro variables and constraint equations for cooperative control using the desired heading angle, current heading angle, desired heading angular velocity, and current heading angular velocity. Based on the compact-form dynamic linearization data model, substitute the macro variables into the constraint equations to calculate the heading control quantity of the unmanned ship.

[0011] Specifically, step S2 includes:

[0012] S21. Discretize the lateral error and longitudinal error;

[0013] S22. Extract the parameter term containing the sideslip angle from the discretized lateral error, select a criterion function for the parameter term containing the sideslip angle, and calculate the estimated value of the parameter term containing the sideslip angle;

[0014] S23. Calculate the estimated value of the sideslip angle from the estimated value of the parameter term containing the sideslip angle.

[0015] Specifically, in step S22, the parameter term containing the sideslip angle is:

[0016] g(k) = u(k)cos(x1(k) - α k (k))tanβ(k)

[0017] where: g is the parameter term containing the sideslip angle, u is the surge speed of the unmanned ship; x1 is the heading angle; α k is the tangent angle of the track; β is the sideslip angle; k represents the current moment.

[0018] Specifically, in step S22, the criterion function is:

[0019]

[0020] where: is the estimated value of the parameter term containing the sideslip angle; J is the criterion function; Δy e (k) is the difference between the lateral error at the current moment and the lateral error at the previous moment; T s is the sampling time; is the first derivative of the tangent angle of the track, x e is the longitudinal error, is the estimated value of the discretized lateral error, μ1 is the weight gain, μ1 > 0, k represents the current moment, and k - 1 represents the previous moment;

[0021] The estimated value of the parameter term containing the sideslip angle is calculated from the criterion function The expression is:

[0022]

[0023] where: ρ1 is the step size, u is the surge speed of the unmanned ship, αk is the tangent angle of the track, and x1 is the heading angle.

[0024] Specifically, in step S23, the estimated value of the sideslip angle is:

[0025]

[0026] In the formula: is the estimated value of the sideslip angle, is the estimated value of the parameter term including the sideslip angle at the current moment; α k (k) is the tangent angle; u is the surge speed of the unmanned ship; x1 is the heading angle; k represents the current moment.

[0027] Specifically, in step S3, the desired heading angle is:

[0028]

[0029] In the formula: x 1d is the desired heading angle, α k is the tangent angle of the track, is the estimated value of the sideslip angle, y e is the lateral error, Δ is the forward viewing distance of the unmanned ship, and k represents the current moment.

[0030] Specifically, in step S4, the compact dynamic linearization data model of the current heading angular velocity of the unmanned ship is:

[0031] x2(k + 1) = x2(k) + φ(k)Δτ(k)

[0032] In the formula: x2 is the current heading angular velocity, φ is the pseudo partial derivative, Δτ is the heading control quantity, k represents the current moment, and k + 1 represents the next moment.

[0033] Specifically, in step S4, the expression of the pseudo partial derivative in the compact dynamic linearization data model is:

[0034]

[0035] In the formula: is the estimated value of the pseudo partial derivative, η and μ are set constants, Δτ is the heading control quantity, Δy(k) is the difference between the lateral error at the current moment and the lateral error at the previous moment, k represents the current moment, and k - 1 represents the previous moment.

[0036] Specifically, in step S5, the macro variable of the cooperative control is:

[0037] s(k) = a1(x 1d (k) - x1(k)) + a2(x 2d (k) - x2(k))

[0038] where: s is a macro variable, a1 and a2 are set constants, x 1d is the desired heading angle, x1 is the current heading angle, x 2d is the desired heading angular velocity, x2 is the current heading angular velocity, and k represents the current moment;

[0039] The constraint equation for cooperative control is:

[0040]

[0041] where: Ω is a set constant used to control the convergence rate of the constraint equation, k + 1 represents the next moment, and T s is the sampling time.

[0042] Specifically, in step S5, the heading control amount is:

[0043]

[0044] where: Δτ is the heading control amount; a1 and a2 are set constants, x 1d is the desired heading angle, x1 is the current heading angle, x 2d is the desired heading angular velocity, x2 is the current heading angular velocity, s is a macro variable; T s is the sampling time; φ is the pseudo partial derivative in the compact form dynamic linearization data model; k represents the current moment; k + 1 represents the next moment.

[0045] Beneficial effects: Compared with the prior art, the remarkable effects of the present invention are as follows: The present invention fully considers the influence of the sideslip angle on the heading of the unmanned ship, uses a data-driven observer to obtain the estimated value of the sideslip angle, and then compensates for the interference caused by wind and waves during the navigation process, and obtains the desired heading angle signal according to the line-of-sight guidance algorithm. After obtaining the desired heading angle signal, the present invention adopts a heading controller that combines model-free adaptive control and cooperative control. This heading controller not only has the advantage of the model-free adaptive control method that only requires system input and output measurement data, getting rid of the need for identifying the motion parameters of the unmanned ship; but also uses the cooperative control method to avoid the system quasi-linear characteristic requirements of the heading control link that do not conform to the model-free adaptive control method. At the same time, the cooperative control method does not have the chattering problem in the sliding mode control, and also reduces the error of the unmanned ship in track tracking, improving the accuracy of track tracking and the robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is the flowchart of the method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0047] The following further illustrates a preferred embodiment of the present invention with reference to the accompanying drawings.

[0048] Example 1

[0049] Please refer to Figure 1 As shown in the figure, this embodiment provides an unmanned ship trajectory tracking method based on model-free adaptive cooperative control. This method adopts a double-loop control structure, including a guidance link and a heading control link, and specifically includes the following steps:

[0050] S1. Calculate the lateral error and longitudinal error of the unmanned ship relative to the reference trajectory according to the current position information of the unmanned ship.

[0051] First, set the reference trajectory of the unmanned ship. In this embodiment, the reference trajectory is the following parametric equation:

[0052]

[0053] In the formula: x k , y k are the position coordinates of the unmanned ship, and ω is the trajectory independent variable.

[0054] Derive to obtain the tangent angle α k (ω) of the reference trajectory, and obtain the error between the unmanned ship and the reference trajectory through the following formula:

[0055]

[0056] In the formula: x e is the longitudinal error, y e is the lateral error, α k is the tangent angle of the reference trajectory, x k , y k are the actual position coordinates of the unmanned ship at the current moment.

[0057] Derive x e , y e respectively to obtain:

[0058]

[0059] In the formula: is the first derivative of the longitudinal error, is the first derivative of the lateral error, is the first derivative of the tangent angle; U is the speed of the unmanned ship, u is the surge speed, v is the yaw rate, x1 is the heading angle; β is the sideslip angle of the unmanned ship, β = arctan2(v, u); is the speed of the virtual reference point, is the first derivative of the trajectory variable; k in the formula represents the current moment.

[0060] Also, because u = U cosβ, expand the above formula to get:

[0061]

[0062] S2. Input the lateral error and longitudinal error into the data-driven observer to obtain the estimated value of the sideslip angle of the unmanned ship.

[0063] Since the unmanned ship system is a discrete system, to achieve the control objective, the lateral error and longitudinal error are discretized as follows:

[0064]

[0065] In the formula: T s is the sampling time, k represents the current moment, and k + 1 represents the next moment.

[0066] Define the parameter term containing the sideslip angle as:

[0067] g(k) = u(k)cos(x1(k) - α k (k))tanβ(k)

[0068] Change the lateral error to:

[0069]

[0070] Let be the estimated value of o ye (k), and the designed data observer is as follows:

[0071]

[0072] In the formula: is the estimated value of the parameter term containing the sideslip angle.

[0073] To obtain Select the following criterion function:

[0074]

[0075] In the formula: J is the criterion function; Δy e (k) is the difference between the lateral error at the current moment and the lateral error at the previous moment; is the estimated value of the discretized lateral error, μ1 is the weight gain, and μ1 > 0. In this embodiment, μ1 = 0.005, T s = 0.1; k - 1 represents the previous moment.

[0076] Derive the criterion function with respect to and set the derivative to zero to obtain The expression is:

[0077]

[0078] where: ρ1 is the step size constant, ρ1 ∈ (0, 1); ε1 is a very small positive number. In this embodiment, ρ1 = 0.5 and ε1 = 0.00001.

[0079] is Design the following adaptive law:

[0080]

[0081] In this embodiment, the parameter k1 = 1.

[0082] The estimated value of the sideslip angle is obtained as:

[0083]

[0084] where: is the estimated value of the sideslip angle, is the estimated value of the parameter term including the sideslip angle at the current moment; α k (k) is the tangent angle; u is the surge speed of the unmanned ship; x1 is the heading angle; k represents the current moment.

[0085] S3. Input the lateral error, the estimated value of the sideslip angle, and the tangent angle of the reference track into the line-of-sight guidance algorithm to obtain the desired heading angle of the current unmanned ship.

[0086] The desired heading angle is obtained by the following formula:

[0087]

[0088] where: x 1d is the desired heading angle, α k is the tangent angle of the track, is the estimated value of the sideslip angle, y e is the lateral error, Δ is the forward viewing distance of the unmanned ship, and k represents the current moment.

[0089] So far, through the line-of-sight guidance algorithm based on the data-driven observer, the calculation of the desired heading angle is completed, and the desired heading angle is input into the following heading control link.

[0090] S4. Establish a compact-form dynamic linearization data model for the current heading angular velocity of the unmanned ship, and use the parameter estimation algorithm to calculate the pseudo partial derivative in the compact-form dynamic linearization data model.

[0091] In this embodiment, the compact-form dynamic linearization data model for the current heading angular velocity of the unmanned ship is:

[0092] x2(k + 1) = x2(k) + φ(k)Δτ(k)

[0093] In the formula: \(x_2\) is the current heading angular velocity, \(\varphi\) is the pseudo partial derivative, \(\Delta\tau\) is the heading control quantity, \(k\) represents the current moment, and \(k + 1\) represents the next moment.

[0094] The following parameter estimation algorithm is used to calculate the pseudo partial derivative:

[0095]

[0096] In the formula: \(\hat{\varphi}\) is the estimated value of the pseudo partial derivative, \(\eta\), \(\mu\) are set constants, \(\Delta\tau\) is the heading control quantity, and \(\Delta y(k)\) is the difference between the lateral error at the current moment and the lateral error at the previous moment.

[0097] When \(\Delta\tau(k)\geq\varepsilon_1\) or \(|\Delta\tau(k - 1)|\leq\varepsilon_2\), \(\varepsilon_2\) is an extremely small positive number.

[0098] In this embodiment, \(\eta = 0.5\), \(\mu = 5\), and \(\varepsilon_2 = 0.00001\).

[0099] S5. Construct the macro variables and constraint equations for cooperative control using the desired heading angle, current heading angle, desired heading angular velocity, and current heading angular velocity. Based on the compact format dynamic linearization data model, substitute the macro variables into the constraint equations to calculate the heading control quantity of the unmanned ship.

[0100] Define the following macro variables:

[0101] \(s(k)=a_1e_1(k)+a_2e_2(k)\)

[0102] In the formula: \(s\) is the macro variable, \(a_1\), \(a_2\) are set constants, \(e_1(k)\) is the difference between the desired heading angle and the current heading angle; \(e_2(k)\) is the difference between the desired heading angular velocity and the current heading angular velocity; \(k\) represents the current moment.

[0103] Substitute the desired heading angle, current heading angle, desired heading angular velocity, and current heading angular velocity into the macro variable \(s(k)\); the desired heading angle is calculated by the line-of-sight guidance algorithm in step S3, the current heading angle and current heading angular velocity come from the monitoring data of the unmanned ship, and the desired heading angular velocity is set manually.

[0104] \(s(k)=a_1(x\) 1d (k)-x_1(k))+a_2(x\) 2d (k)-x_2(k))

[0105] In the formula: \(x\) 1d is the desired heading angle, \(x_1\) is the current heading angle, \(x\) 2d is the desired heading angular velocity, and \(x_2\) is the current heading angular velocity.

[0106] Further expand the above formula and after merging, we get:

[0107] s(k) = a1x 1d (k) - a1x1(k) - (a1T s + a2)x2(k) + a2x 2d (k) - a2φ(k)Δτ(k)

[0108] Adopt the following discrete constraint equation as the constraint equation for cooperative control:

[0109]

[0110] In the formula: Ω is a set constant used to control the convergence rate of the constraint equation, k + 1 represents the next moment, and T s is the sampling time.

[0111] In this embodiment, a1 = 15, a2 = 0.01, Ω = 1, T s = 0.1.

[0112] Substitute the macro variable s(k) into the constraint equation, and we get:

[0113]

[0114] Furthermore, the heading control quantity Δτ(k) is obtained as:

[0115]

[0116] In the formula: Δτ is the heading control quantity; a1, a2 are set constants, x 1d is the desired heading angle, x1 is the current heading angle, x 2d is the desired heading angular velocity, x2 is the current heading angular velocity, s is the macro variable; T s is the sampling time; φ is the pseudo partial derivative in the compact form dynamic linearization data model; k represents the current moment; k + 1 represents the next moment.

[0117] Through the above heading controller that combines the model-free adaptive control method and the cooperative control method, the control signal of the actuator is obtained. That is, the heading controller controls the unmanned ship to sail along the reference track at the next moment by inputting the heading control quantity Δτ(k) to the unmanned ship. Repeat steps S1 to S5 until the unmanned ship reaches the end point of the track.

Claims

1. A method for tracking an unmanned ship's track based on model-free adaptive cooperative control, characterized in that: The following steps are involved: S1. Calculate the lateral error and longitudinal error of the unmanned ship relative to the reference track based on the current position information of the unmanned ship; S2, inputting the lateral error and the longitudinal error into the data driven observer to obtain an estimated value of the sideslip angle of the unmanned ship; S3, inputting the lateral error, the estimated value of the sideslip angle and the tangent angle of the reference track into the line of sight guidance algorithm to obtain the desired heading angle of the current unmanned ship; S4, establishing a compact dynamic linear data model of the current heading angular velocity of the unmanned ship, and calculating the pseudo partial derivatives in the compact dynamic linear data model using a parameter estimation algorithm; S5. Use the expected heading angle, current heading angle, expected heading angular velocity and current heading angular velocity to construct the macro variables and constraint equations for collaborative control. Based on the compact dynamic linearization data model, substitute the macro variables into the constraint equations to calculate the heading control value of the unmanned ship.

2. The unmanned ship track tracking method according to claim 1, characterized in that: The step S2 comprises: S21, discretizing the lateral error and the longitudinal error; S22, extracting the parameter item including the sideslip angle from the lateral error after the discretization process, selecting a criterion function for the parameter item including the sideslip angle, and calculating an estimated value of the parameter item including the sideslip angle; S23. Obtain an estimated value of the sideslip angle by calculating the estimated value of the parameter item including the sideslip angle.

3. The unmanned ship track tracking method according to claim 2, characterized in that: In step S22, the parameter item including the sideslip angle is: g(k)=u(k)cos(x1(k)-α k (k))andβ(k) Where: g is a parameter term including the sideslip angle, u is the surge speed of the unmanned ship; x1 is the heading angle; α k is the tangent angle of the track; β is the sideslip angle; k represents the current moment.

4. The unmanned ship track tracking method according to claim 3, characterized in that: In step S22, the criterion function is: Where: is the estimated value of the parameter term containing the sideslip angle; J is the criterion function; Δy e (k) is the difference between the lateral error at the current moment and the lateral error at the previous moment; T s is the sampling time; is the first derivative of the tangent angle of the track, x e is the longitudinal error, is the estimated value of the lateral error after discretization, μ1 is the weight gain, μ1>0, k represents the current moment, and k-1 represents the previous moment; The estimated value of the parameter term including the sideslip angle is calculated by the criterion function The expression is: Where: ρ1 is the step length, u is the surge speed of the unmanned ship, α k is the tangent angle of the track, and x1 is the heading angle.

5. The unmanned ship track tracking method according to claim 4, characterized in that: In step S23, the estimated value of the sideslip angle is: Where: is the estimated value of the sideslip angle, is the estimated value of the parameter item including the sideslip angle at the current moment; α k (k) is the tangent angle; u is the surge speed of the unmanned ship; x1 is the heading angle; k represents the current moment.

6. The unmanned ship track tracking method according to claim 1, characterized in that: In step S3, the expected heading angle is: Where: x 1d is the desired heading angle, α k is the tangent angle of the track, is the estimated value of the sideslip angle, y e is the lateral error, Δ is the forward distance of the unmanned ship, and k represents the current moment.

7. The unmanned ship track tracking method according to claim 1, characterized in that: In step S4, the compact dynamic linearization data model of the current heading angular velocity of the unmanned ship is: x2(k+1)=x2(k)+φ(k)Δτ(k) Where: x2 is the current heading angular velocity, φ is the pseudo partial derivative, Δτ is the heading control amount, k represents the current moment, and k+1 represents the next moment.

8. The unmanned ship track tracking method according to claim 7, characterized in that: In step S4, the pseudo partial derivative expression in the compact format dynamic linearization data model is: Where: is the estimated value of the pseudo partial derivative, η and μ are set constants, Δτ is the heading control amount, Δy(k) is the difference between the lateral error at the current moment and the lateral error at the previous moment, k represents the current moment, and k-1 represents the previous moment.

9. The unmanned ship track tracking method according to claim 1, characterized in that: In step S5, the macro variables for collaborative control are: s(k)=a1(x 1d (k)-x1(k))+a2(x 2d (k)-x2(k)) Where: s is a macro variable, a1 and a2 are set constants, and x 1d is the desired heading angle, x1 is the current heading angle, x 2d is the expected heading angular velocity, x2 is the current heading angular velocity, and k represents the current moment; The constraint equation of cooperative control is: Where: Ω is a set constant used to control the convergence rate of the constraint equation, k+1 represents the next moment, T s is the sampling time.

10. The unmanned ship track tracking method according to claim 9, characterized in that: In step S5, the heading control amount is: Where: Δτ is the heading control value; a1 and a2 are set constants, x 1d is the desired heading angle, x1 is the current heading angle, x 2d is the expected heading angular velocity, x2 is the current heading angular velocity, s is the macro variable; T s is the sampling time; φ is the pseudo partial derivative in the compact format dynamic linearization data model; k represents the current moment; k+1 represents the next moment.

Images