Water surface unmanned ship trajectory tracking method based on incremental model predictive control

Abstract

The invention discloses a water surface unmanned ship trajectory tracking method based on incremental model predictive control, and the method comprises the steps: constructing an unmanned ship system state model based on a differential drive unmanned ship platform; based on time delay estimation, converting the unmanned ship system state model into an incremental system model, and analyzing and eliminating errors in the model conversion process; and discretizing the incremental system model based on the incremental system model to obtain an incremental prediction model, and finally realizing trajectory tracking of the unmanned ship on the basis of the incremental prediction model. According to the method, an incremental system model is deduced under the condition that ship model parameters are not identified, accurate control over a water surface unmanned ship system under the condition that physical constraints exist is achieved, and safe and high-precision tracking sailing of an unmanned ship under the condition that model parameter identification is not conducted is achieved.

Description

Technical Field

[0001] The present invention relates to the field of automatic control, and particularly to a trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control. Background Art

[0002] With the continuous development of marine resources and the continuous attention to the unmanned field, the unmanned surface vehicle (USV) has gradually become a research hotspot in the field of marine science and technology as an unmanned and intelligent technical means. In recent years, with the rapid development of technology update and iteration, USV has become a main transportation tool in daily life such as military, transportation, and observation. The trajectory tracking control problem of USV will be objectively affected by the time-varying marine environment and subjectively restricted by people's requirements for safety and performance in practical applications. For example: autonomous navigation in narrow waterways or in the case of dense distribution of marine facilities, high requirements for control accuracy, and the requirement for the rapid response ability of the controller, etc. Therefore, in-depth exploration of the trajectory tracking control problem of the unmanned ship has important theoretical significance and application value.

[0003] Most unmanned surface vehicles have characteristics such as model parameter uncertainty and susceptibility to interference from the actual navigation environment. Therefore, achieving high-precision tracking control of USV is an important challenge faced in the current development and application field of USV. Traditional USV trajectory tracking control methods mainly include backstepping method, sliding mode control, and adaptive control, etc.

[0004] Backstepping method constructs appropriate Lyapunov functions for each subsystem of the nonlinear ship dynamics model, then designs virtual control laws to ensure the stability of each subsystem, and finally calculates to the last step to obtain the actual control input, ultimately achieving the global stability of the entire system. Specifically, the literature (Dong Z, Wan L, Li Y, et al. Trajectorytracking control of underactuated USV based on modified backstepping approach[J]. International Journal of Naval Architecture and Ocean Engineering, 2015.) designed a backstepping control strategy based on state feedback to solve the trajectory tracking control problem of the unmanned ship. However, the "differential explosion" problem will occur in the direct derivative process of the virtual control law (Shi X, Cheng Y, Yin C, et al. Design of adaptive backstepping dynamic surface control method with RBFneural network for uncertain nonlinear system[J]. Neurocomputing, 2019.), and the complex calculation results in poor real-time performance of the controller.

[0005] Sliding mode control methods are widely used to solve the trajectory tracking control problem of unmanned vessels due to their simple design and ease of integration with other control methods. In the literature (Yu C, Xiang X, Zhang Q, et al. Adaptive fuzzy trajectory tracking control of an under-actuated autonomous underwater vehicle subject to actuator saturation [J]. International Journal of Fuzzy Systems, 2018.), a direct adaptive fuzzy control algorithm was used to compensate for the effects of actuator saturation. A control law was then designed using feedback linearization and sliding mode techniques to ensure system stability in the presence of actuator saturation. In the literature (Zhang J, Yu S, Wu D, et al. Nonsingular fixed-time terminal sliding mode trajectory tracking control for marine surface vessels with anti-disturbances [J]. Ocean Engineering, 2020.), the robustness of sliding mode control was utilized to design a nonsingular fixed-time terminal sliding mode trajectory tracking control law for USVs subject to external disturbances. However, designing a high-precision trajectory tracking controller requires the assumption that the ship model parameters are precisely known, but accurately obtaining the hydrodynamic coefficients in the ship model is almost unrealistic. As a result, sliding mode control inevitably leads to gain overestimation, which in turn exacerbates hull buffeting, which is detrimental to the actual operation of unmanned vessels.

[0006] Adaptive control can effectively improve the control accuracy of ship motion by designing an appropriate adaptive law to accurately estimate the uncertainties of the unmanned ship model or external disturbances and compensating for them in the controller. The literature (Godhavn J M, Fossen T I, Berge S P. Non-linear and adaptive backstepping designs for tracking control of ships[J]. International Journal of Adaptive Control and Signal Processing, 1998.) designed a trajectory tracking controller based on adaptive backstepping, which effectively compensated for the slow-varying environmental disturbances suffered by the ship, such as ocean currents, disturbances caused by second-order waves, and wind forces, and achieved the global uniform asymptotic stability of the closed-loop system. However, it did not consider the case of model parameter perturbations. Moreover, in the actual navigation of the unmanned ship, model parameters are inevitably perturbed, and due to the existence of cumulative errors, the tracking accuracy of this controller will decrease as the navigation time increases.

[0007] The literature (Asadi M, Khayatian A. Adaptive Backstepping Autopilot for Way-point Tracking Control of a Container Ship in the presence of Time-varying Disturbances[J]. IFAC Proceedings Volumes, 2011.) designed an adaptive backstepping tracking control algorithm using the line-of-sight guidance method, realized the tracking of the ship to the desired waypoint, and solved the problem of ship trajectory tracking with unknown model parameters and time-varying environmental disturbances. Although the combination of adaptive technology and backstepping can effectively estimate the uncertain part of the ship model and the upper bound of unknown external disturbances, it usually requires that the nonlinear structure of the uncertain part of the ship model is known a priori, and there are obvious limitations in estimating fast time-varying model parameters.

[0008] Model Predictive Control (MPC), as a receding horizon optimization strategy, is widely used in the design of trajectory tracking controllers. Model Predictive Control originates from optimal control theory and is a control method based on computer technology. By predicting the future state of the system according to the system model at each moment and solving the optimal control quantity at the current moment, it realizes the receding horizon optimization control strategy, which can make up for the uncertainty in the model to a certain extent. And it can explicitly handle control quantity constraints when solving the optimization problem, having very good applicability. Traditional Model Predictive Control has high requirements for the model, and the solution time increases with the increase of model complexity, prediction and control time domain lengths, which limits the improvement of the control frequency. Due to the complex model of the unmanned surface vehicle (USV), it is difficult to establish, and there are many uncertain disturbances in the actual navigation environment, so traditional Model Predictive Control algorithms are rarely directly used for the trajectory tracking control of ships. Summary of the Invention

[0009] To overcome the above problems in the prior art, the purpose of the present invention is to provide a trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control, mainly solving the problems of low tracking accuracy, poor real-time performance, inability to consider the constraint conditions in actual navigation, and high dependence on the system model of the current trajectory tracking controller, so as to realize the safe and high-precision tracking navigation of the USV without model parameter identification.

[0010] The purpose of the present invention is achieved by the following technical solutions: A trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control includes the following steps:

[0011] Based on the differential drive unmanned vehicle platform, in the presence of external disturbances, a system state model of the unmanned vehicle is constructed, and the system state model of the unmanned vehicle includes a kinematic model and a dynamic model;

[0012] Based on Time Delay Estimation (TDE), the system state model of the unmanned vehicle is transformed into an incremental system model, and the error in the model transformation process is analyzed and eliminated;

[0013] Based on the incremental system model, it is discretized to obtain an incremental prediction model; based on the incremental prediction model, a cost function is established, and the control input of the incremental prediction model is optimized. The optimization objectives include tracking error and suppressing the oscillation of the controller. Finally, the optimal control input of the unmanned vehicle system, that is, the control torques of the left and right propellers, is obtained.

[0014] Further, the construction of the system state model of the unmanned vehicle based on the differential drive unmanned vehicle platform in the presence of external disturbances includes:

[0015] By setting the earth coordinate system {e} as the inertial coordinate system and the coordinate system {b} of the rigid body on the unmanned ship as the hull coordinate system, the kinematic model of the unmanned ship is constructed as follows:

[0016] in, Represents the position (x, y) and yaw angle of the unmanned ship relative to {e} coordinates State; ν = [u, v, r] T represents the generalized velocity vector of the unmanned ship relative to the hull coordinate system {b}, v r Indicates the relative velocity between the hull and the water flow, v c Indicates the speed of ocean current / water flow; represents a non-singular transformation matrix;

[0017] The generalized velocity vector v and its derivatives are expressed as:

[0018]

[0019] Among them, S is a constant matrix, expressed as J T or S T is the matrix J or the transposed matrix of the matrix S;

[0020] Considering external interference, including ocean currents and external forces, the unmanned ship dynamics model is constructed:

[0021]

[0022] Among them, M RB represents the rigid body inertia matrix; C RB represents the corresponding matrix of Coriolis term and centripetal term; M A represents the additional mass; d(v r ) represents the damping vector; g(η) represents the restoring force; τ=[τ u ,0,τ r ] T represents the control input vector, τ u represents the longitudinal control torque, τ r represents the bow control torque; τ ω represents the additional moment caused by wind and waves;

[0023] Combining the generalized velocity vector v and its derivative expression with the unmanned ship dynamics model, the following formula 1 is obtained:

[0024]

[0025] Among them, the positive definite inertia matrix M=M RB +M A , is the collection of all nonlinear dynamic terms.

[0026] Furthermore, the combination of the generalized velocity vector v and its derivative expression and the unmanned ship dynamics model includes:

[0027] Introduce the nominal inertia matrix M n , and transform the formula 1 into the following formula 2:

[0028]

[0029] where the nominal inertia matrix M n is the nominal value of the matrix M, and the rigid body inertia matrix M RB is determined by the size, weight, mass distribution, volume and area of the unmanned ship. The elements in the matrix M A are identified by semi-empirical formulas or strip theory, and is the unknown model dynamic parameter.

[0030] Furthermore, the transformation of the unmanned ship system state model into an incremental system model based on time delay estimation includes:

[0031] Based on time delay estimation, obtain from formula 2:

[0032] where τ0 = τ (t-L) , v0 = v (t-L) , and L is the delay time;

[0033] Based on the above formula and formula 2, obtain the incremental expression of the dynamics model:

[0034] where the incremental control vector Δτ = τ - τ0, and the time delay error When ε1 = 0, the incremental expression is:

[0035] Furthermore, the analysis and elimination of the errors in the model transformation process include:

[0036] Based on the model error caused by ε1 = 0, analyze the error and construct the following formula:

[0037]

[0038] where, are the errors generated by the inertia matrix and the collection of all non-linear dynamics terms respectively during time delay estimation; the inertia modeling error and the interference τ of the wind and waves ω are respectively multiplied by their respective time delay values and (τ ω ) (t-L)Compensation; when the sampling frequency is faster than 30 times the system bandwidth, the digital control system is regarded as a continuous system; according to the Shannon sampling theorem, and τ ω is effectively compensated by the time-delay signal.

[0039] Furthermore, based on the incremental system model, it is discretized to obtain an incremental prediction model, including:

[0040] Based on the incremental expressions of the kinematic model and dynamic model of the unmanned ship, it is discretized to obtain the following discretized expression:

[0041]

[0042] Assuming that the sampling period is small enough to ignore the discretization error, let X(k + 1) = [η(k + 1), v(k + 1), v(k)] T , and unify the discretized expression as: X(k + 1) = AX(k) + BΔu(k);

[0043] where, Δu = [Δτ u , Δτ r T ;

[0044] Since the heading angle changes little with respect to the speed in the short prediction interval, a constant matrix A is adopted.

[0045] Furthermore, based on the incremental prediction model, using the cost function to optimize the control input of the incremental prediction model includes:

[0046] Based on the discretized incremental system model, define the tracking error e k+j+1|k of the unmanned ship as:

[0047] e k+j+1|k = p k+j+1|k - p ref (k + j + 1);

[0048] where, p k+j+1|k is the predicted trajectory point at the k + j + 1 step at time k, p ref (k + j + 1) is the corresponding point on the reference trajectory, and e k+j+1|k is the trajectory tracking error at the k + j + 1 step predicted at time k;

[0049] To consider suppressing the oscillation of the controller, define the cost function:

[0050]

[0051] where, Q1, Q2, R are weight matrices; ηk+j+1|k and r k+j+1|k is the predicted state at time k, obtained through the discretized incremental system model; for j ∈ [0, N-1], we have:

[0052] X k+j+1|k = AX k+j|k + BΔu k+j|k ;

[0053] where X k|k = X(k), N ∈ [1, ∞) is the prediction horizon, and an incremental model predictive optimization function is constructed based on the cost function. Taking the feasible control sequence as the decision variable, we get:

[0054]

[0055] X k+j+1|k = AX k+j|k + BΔu k+j|k (b);

[0056]

[0057] v k+j+1|k ∈ V(d);

[0058] where equation (b) is the equality constraint of the incremental prediction model, and equations (c) and (d) are the input and state constraints of the unmanned surface vehicle system, respectively; is the optimal control sequence of the optimized unmanned surface vehicle system, and its first column element or Δu * (k) is used as the incremental control input to act on the unmanned surface vehicle system.

[0059] Furthermore, the optimization of the control input of the incremental prediction model using the cost function based on the incremental prediction model further includes: performing quadratic programming on the optimization objective.

[0060] The beneficial effects of the present invention are as follows: By combining delay estimation and model predictive control, the present invention can obtain an incremental prediction model of the unmanned surface vehicle system without identifying the model parameters of the unmanned surface vehicle, thereby achieving high-precision trajectory tracking of the USV; moreover, the incremental model predictive control proposed by the present invention has the advantages of traditional model predictive control and can exhibit strong robustness in the face of uncertain disturbances in the navigation environment; at the same time, since the invented incremental model predictive controller is designed under the traditional model predictive framework, this controller can achieve constraint control of the USV and ensure the navigation safety of the USV. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1Schematic diagram of the implementation process for the trajectory tracking control of USV based on incremental model predictive control;

[0062] Figure 2 Schematic diagram of the hardware framework of the control system;

[0063] Figure 3 Schematic diagram of the definition of the USV coordinate system;

[0064] Figure 4 Schematic diagram of the trajectory tracking of the unmanned ship;

[0065] Figure 5 Schematic diagram of the lateral and longitudinal tracking errors. Detailed implementation manner

[0066] Here, the exemplary embodiments will be described in detail, and the examples are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementation manners described in the following exemplary embodiments do not represent all the implementation manners consistent with the present invention. On the contrary, they are merely examples of the devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0067] As Figure 1 shown, the embodiment of the present invention provides a trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control. The trajectory tracking controller proposed by the present invention is a new unmanned ship trajectory tracking controller obtained by integrating the time-delay estimation technology (TDE) and the model predictive control (MPC) method, and includes the following steps:

[0068] (1) Construction of the unmanned ship control system

[0069] Based on the differential drive unmanned ship platform of the present invention, a hardware control system framework is designed and constructed. The main controller module selects Raspberry Pi 4B, which is responsible for running the main program; the positioning and navigation module selects the Beidou / GPS dual-mode positioning module and the two-dimensional electronic compass, which are responsible for collecting information such as the position state and heading of the USV; the data communication module selects the LORA module, which is responsible for the wireless communication between the upper computer and the unmanned ship; the propulsion module mainly consists of a power supply, a motor and an electronic speed controller, which are responsible for supplying power to the USV platform and executing control instructions; finally, the human-computer interaction function of the unmanned ship system is realized by developing the upper computer system based on ROS. The control system framework diagram is as Figure 2 shown.

[0070] (2) Establishment of the unmanned ship system state model:

[0071] The dynamic model of the unmanned ship in this invention is established based on the differential unmanned ship. It is assumed that the Earth-fixed coordinate system {e} is the inertial coordinate system, and the coordinate system {b} attached to the rigid body of the ship is the hull coordinate system, as Figure 3 shown. The following USV kinematic model can be obtained:

[0072]

[0073] where, represents the position (x, y) and heading angle of the unmanned ship relative to the coordinates in {e}, and the state of T ; ν = [u, v, r]

[0074] is the generalized velocity vector relative to the hull coordinate system {b}.

[0075]

[0076] In the formula, the matrix S is a constant matrix, In the above formula, J T or S T is the transpose matrix of matrix J or matrix S.

[0077] Taking into account the influence of ocean currents and external forces, etc., the hydrodynamic model of the surface unmanned ship is as follows:

[0078]

[0079] where, M RB is the rigid body inertia matrix; C RB is the corresponding matrix of the Coriolis term and the centripetal term; M A represents the added mass; d(v r ) is the damping vector; g(η) is the restoring force; τ = [τ u , 0, τ r ) T is the control input vector, τ u is the longitudinal control moment, τ[[ID=dd56]] r is the yaw control moment; τ ω is the additional moment caused by wind and waves.

[0080] To sum up, combining Equation (2.2) and Equation (2.3), the following dynamic equation of the unmanned ship can be obtained:

[0081]

[0082] where, the positive definite inertia matrix M = M RB + M A , and It is the collection of all non - linear dynamic terms.

[0083] (3) Establishment of the incremental system based on time - delay estimation (TDE):

[0084] (3.1) Establishment of the incremental system model. Introduce a nominal inertia matrix M n , Equation (2.4) can be simplified to the following form:

[0085]

[0086] The nominal inertia matrix M n is the nominal value of matrix M. The rigid - body inertia matrix M RB is determined by the size, weight, mass distribution, volume and area of the USV, and the elements in matrix M A can be identified through semi - empirical formulas or strip theory. Therefore, for a specific unmanned ship, the nominal inertia matrix M n can be identified without identifying the specific parameters in matrix M RB and matrix M A . During the actual operation process, the selection of M n follows ||I - M -1 M n ‖ < 1. During the model construction stage, the selection of M n is flexible, showing strong robustness to the uncertainty of the inertia matrix.

[0087] In Equation (3.1) contains all uncertain / unknown model dynamic parameters. According to the time - delay estimation technology:

[0088]

[0089] where τ0 = τ (t-L) , v0 = v (t-L) , and L is the delay time. The approximate accuracy of the incremental system is negatively correlated with the delay time L, that is, the shorter the delay time, the higher the approximate accuracy. During the actual operation process, it is generally set to the sampling period T s .

[0090] Combining Equation (3.1) and Equation (3.2), the incremental expression of the system dynamics model can be obtained:

[0091]

[0092] where the incremental control vector Δτ = τ - τ0, and the time - delay error is within the acceptable error range. The time - delay error can be approximated as zero, that is, ε1 = 0. The incremental expression (3.3) can be simplified to:

[0093]

[0094] It can be seen from Equation (3.4) that by using the nominal inertia matrix M n and the measurement value v0 at the most recent sampling moment, an approximate dynamic equation can be obtained, thus reducing the modeling and system parameter identification errors in Equation (2.4) and the model mismatch caused by the uncertain changes in the external environment such as wind and waves during actual operation.

[0095] (3.2) Time-delay Estimation Error Analysis

[0096] As can be analyzed from (3.1), this patent transforms the USV state model into an incremental system model through the TDE technology to mitigate the model mismatch problem caused by model identification and parameter time-variation. However, in the simplified incremental system model (Equation (3.4)), there are still model errors caused by assuming that the error of TDE within the prediction time domain is zero. This section analyzes this error.

[0097] The error caused by the approximate incremental system model in Equation (3.2) is as follows:

[0098]

[0099] where are the inertia matrix and the set of all nonlinear dynamic terms respectively The error generated during time-delay estimation using the TDE technology. It can be seen from Equation (3.5) that the inertia modeling error of the TDE method and the interference τ ω from wind and waves are respectively compensated by their respective time-delay values and (τ ω ). (t-L) When the sampling frequency is faster than 30 times the system bandwidth, the digital control system can be regarded as a continuous system. According to the Shannon sampling theorem, and τ ω can be effectively compensated through their time-delay signals. Similarly, if the sampling period is small enough, it will also result in small TDE errors.

[0100] (4) Construction of Incremental Model Predictive Controller (IMPC)

[0101] (4.1) Construction of the state equation of the USV incremental model:

[0102] Based on the previously obtained USV incremental system, first discretize Equation (2.1) and Equation (3.4) using the Euler numerical differentiation method, and the following discretized expressions can be obtained:

[0103]

[0104] Assume that the sampling period is small enough so that the discrete error can be ignored in this case. To write it in a unified expression form, let X(k + 1) = [η(k + 1), v(k + 1), v(k)] T , then Equation (4.1) can be simplified to:

[0105] X(k + 1) = AX(k) + BΔu(k) (4.2)

[0106] where,

[0107]

[0108] It can be seen from Equation (4.3) that for matrix A, there is a time-varying element According to its definition, matrix A varies with the heading angle within the prediction interval. At this time, the state equation of the system is a nonlinear system, and the computational complexity will become complicated. Considering that the heading angle changes relatively little compared to the speed within a short prediction interval, the present invention uses a constant matrix A (i.e., ) within the prediction range, and matrix A will be updated at the next sampling moment.

[0109] (4.2) Define the optimization objective:

[0110] Based on the incremental prediction model obtained above, define the USV tracking error e k+j+1|k as follows:

[0111] e k+j+1|k = p k+j+1|k - p ref (k + j + 1) (4.4)

[0112] where p k+j+1|k is the predicted trajectory point at the (k + j + 1)-th step at time k, and p ref (k + j + 1) is the corresponding point on the reference trajectory. e k+j+1|k is then the trajectory tracking error at the (k + j + 1)-th step predicted at time k.

[0113] In the actual navigation of the unmanned ship, not only the tracking error should be considered, but also the oscillation of the controller should be suppressed. Since Δu is the control variable of the approximate incremental system, the tracking error e and the control increment Δu will be regarded as components of the cost function. In addition, since the considered surface unmanned ship is underactuated. To avoid system oscillation, the internal dynamics of the unmanned ship will also be considered in the cost function Define the cost function as follows:

[0114]

[0115] Among them, Q1, Q2, and R are weight matrices. η k+j+1|k and r k+j+1|k are the predicted states at time k, and the predicted states can be obtained from Equation (4.2) derived above. For j ∈ [0, N - 1], we have:

[0116] X k+j+1|k = AX k+j|k + BΔu k+j|k (4.6)

[0117] Among them, X k|k = X(k), N ∈ [1, ∞) is the prediction horizon. Based on the cost function (Equation (4.5)) defined above, the following incremental model predictive optimization problem can be constructed. The feasible control sequence is used as the decision variable:

[0118]

[0119] X k+j+1|k = AX k+j|k + BΔu k+j|k (4.7b)

[0120]

[0121] v k+j+1|k ∈ V(4.7d)

[0122] Among them, (3.7b) is the equality constraint of the system model, and (4.7c) and (4.7d) are the input and state constraints of the system, respectively. is the optimal control sequence of the system, and its first column element or Δu * (k) acts on the USV system as the incremental action of the control input.

[0123] (4.3) Quadratic programming framework. To solve the optimal control problem, the cost function Equation (4.7) needs to be expressed in the form of quadratic programming. The quadratic programming framework is as follows:

[0124]

[0125] s.t.

[0126]

[0127] Among them, Δτ *(k) is the optimal solution of the quadratic programming problem (4.8), H(k) is the Hessian matrix, and F(k) is the gradient vector. The inequality constraint (4.8b) corresponds to the input constraint (4.7c) and the state constraint (4.7d). Since the equality constraint (the discrete increment system (4.7b)) is used to transform the constrained optimal control problem (4.7) into the optimal control problem (4.8), and the equality constraint has been considered during the transformation, the equality constraint is omitted in (4.8). The specific expressions of the parameters such as H(k) and F(k) in the formula are as follows:

[0128]

[0129] K1 = [I 3×3 O 3×3 O 3×3 (4.13)

[0130] K2 = [O 3×3 I 3×3 O 3×3 (4.14)

[0131]

[0132] The relevant parameters in formulas (4.9)-(4.23) are defined as follows: Q1, Q2, R are weight matrices; N is the prediction horizon; p′ ref (i) is the reference trajectory, is the reference trajectory at N prediction horizons after the matching point; u min and u max represent the minimum control input and the maximum control input of the system respectively; v min and v max represent the minimum state and the maximum state of the system respectively.

[0133] From the above analysis, it can be seen that the constrained optimal control problem is finally transformed into a quadratic programming problem, and the cost function is a convex function. Therefore, the quadratic programming problem (formula (4.8)) is a convex optimization problem, so there exists an optimal solution.

[0134] (5) Simulation verification

[0135] This patent uses Matlab / Simulink 2024a to test the proposed algorithm. The initial state of the unmanned ship is: x = 0, y = 0, Simulation and experiments verify the feasibility and effectiveness of the proposed control method as Figure 5 shown. The reference trajectory can be described by the following equation:

[0136]

[0137] As Figure 4 shown, within the time period of 0 - 300 s, since the unmanned ship needs to continuously adjust from the initial state to the reference trajectory, a relatively large tracking error appears. In the time period after 300 s, it is observed that the tracking error is less than 0.02 m in both directions. Although the tracking error does not converge to 0, the error is bounded.

Claims

1. A trajectory tracking method for an unmanned surface vessel based on incremental model predictive control, characterized in that, It includes the following steps: Based on a differential drive unmanned boat platform, in the presence of external disturbances, a state model of the unmanned boat system is constructed, and the state model of the unmanned boat system includes a kinematic model and a dynamic model of the unmanned boat system; Based on time-delay estimation, the state model of the unmanned boat system is transformed into an incremental system model, and the errors in the model transformation process are analyzed and eliminated; Based on the incremental system model, it is discretized to obtain an incremental prediction model; based on the incremental prediction model, a cost function is established, and the control input of the incremental prediction model is optimized. The optimization objectives include tracking error and suppressing the oscillation of the controller. Finally, the optimal control input of the system, that is, the control torques of the left and right propellers, is obtained.

2. The trajectory tracking method of an unmanned surface vessel based on incremental model predictive control according to claim 1, characterized in that, The construction of the state model of the unmanned boat system based on the differential drive unmanned boat platform in the presence of external disturbances includes: By setting the Earth coordinate system {e} as the inertial coordinate system and the coordinate system {b} of the rigid body on the unmanned ship as the hull coordinate system, the kinematic model of the unmanned ship is constructed as Among them, represents the position (x, y) and yaw angle of the unmanned ship in the {e} coordinate state; v = [u, v, r] T represents the generalized velocity vector of the unmanned ship relative to the hull coordinate system {b}, v r represents the relative velocity between the hull and the water flow, v c represents the water flow velocity; represents a non-singular transformation matrix; The generalized velocity vector v and its derivative are expressed as: where S is a constant matrix, expressed as J T or S T is the transpose matrix of matrix J or matrix S; Considering external disturbances, including ocean currents and external forces, a dynamic model of the unmanned boat is constructed: Among them, M RB represents the rigid body inertia matrix; C RB represents the corresponding matrix of the Coriolis term and the centripetal term; M A represents the added mass; d(v r ) represents the damping vector; g(η) represents the restoring force; τ = [τ u , 0, τ r T represents the control input vector, τ u represents the longitudinal control moment, τ r represents the yaw control moment; τ ω represents the additional moment caused by wind and waves;​ Combining the expression of the generalized velocity vector v and its derivative and the dynamic model of the unmanned boat, the following formula 1 is obtained: where the positive definite inertia matrix \(M = M\) RB + M A , is the collection of all non - linear dynamic terms.

3. A trajectory tracking method for an unmanned surface vessel based on incremental model predictive control according to claim 2, characterized in that, The combination of the expression of the generalized velocity vector v and its derivative and the dynamic model of the unmanned boat includes: Introduce the nominal inertia matrix M n , and transform the formula 1 into the following formula 2: Among them, the nominal inertia matrix M n is the nominal value of matrix M, and the rigid body inertia matrix M RB is determined by the dimensions, weight, mass distribution, volume, and area of the unmanned ship. The elements in matrix M A are identified through semi-empirical formulas or strip theory, and is an unknown model dynamic parameter.

4. A trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control according to claim 3, characterized in that, The transformation of the state model of the unmanned boat system into an incremental system model based on time-delay estimation includes: Based on the time delay estimation, it is obtained according to Equation 2: where τ0 = τ (t-L) , v0 = v (t-L) , and L is the delay time; Based on the above formula and Formula 2, the incremental expression of the kinetic model is obtained: where the incremental control vector Δτ = τ - τ0 and the time-delay error When ε1 = 0, the incremental expression is as follows:

5. A trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control according to claim 4, characterized in that, The analysis and elimination of the errors in the model transformation process include: Based on the model error caused by ε1 = 0, the error is analyzed and the following formula is constructed: wherein, are the inertia matrix and the collection of all non - linear dynamic terms respectively the error generated by time - delay estimation; the inertia modeling error and the disturbances τ of wind and waves ω are respectively compensated by their respective time - delay values and (τ ω ) (t-L) ; when the sampling frequency is faster than 30 times the system bandwidth, the digital control system is regarded as a continuous system; according to the Shannon sampling theorem, and τ ω will be effectively compensated through the time - delay signal.

6. A trajectory tracking method for an unmanned surface vessel based on incremental model predictive control according to claim 4, characterized in that The discretization of the incremental system model to obtain an incremental prediction model includes: Based on the incremental expressions of the kinematic model and the dynamic model of the unmanned boat, it is discretized to obtain the following discretized expression: Assume that the sampling period is small enough to neglect the discrete error, and let X(k + 1) = [η(k + 1), v(k + 1), v(k)] T , and unify the discretized expression as: X(k + 1) = AX(k) + BΔu(k); Among them, In the above formula, the coefficient matrix A changes with the heading angle and is a time-varying matrix; since the heading angle changes little relative to the speed within the short prediction interval, a constant matrix A is adopted within the current prediction interval, and the value of matrix A is updated within the next prediction interval.

7. A trajectory tracking method for an unmanned surface vessel based on incremental model predictive control according to claim 6, characterized in that, The optimization of the control input of the incremental prediction model using the cost function based on the incremental prediction model includes: Based on the discretized incremental system model, the tracking error \(e\) of the unmanned ship is defined k+j+1|k as follows: e k+j+1|k = p k+j+1|k - p ref (k + j + 1); where p k+j+1|k is the predicted trajectory point at the (k + j + 1)-th step at time k, and p ref (k + j + 1) is the point on the corresponding reference trajectory, and e k+j+1|k is the trajectory tracking error at the (k + j + 1)-th step predicted at time k; To suppress the oscillation of the controller, a cost function is defined: where Q1, Q2, and R are weight matrices; η k+j+1|k , r k+j+1|k is the predicted state at time step k, obtained from the discretized incremental system model; for j ∈ [0, N-1], we have: X k+j+1|k = AX k+j|k + BΔu k+j|k ; where X k|k = X(k), N ∈ [1, ∞) is the prediction step length. An incremental model predictive optimization function is constructed based on the cost function, and the feasible control sequence is used as the decision variable to obtain: X k+j+1|k = AX k+j|k + BΔu k+j|k (b); v k+j+1|k ∈ V(d); Among them, equation (b) is the equality constraint of the incremental prediction model, and equations (c) and (d) are the input and state constraints of the unmanned ship system, respectively; is the optimal control sequence after the optimization of the unmanned ship system, and the first column element of it or Δu * (k) is used as the incremental action of the control input on the unmanned ship system.

8. A trajectory tracking method for an unmanned surface vehicle based on incremental model predictive control according to claim 7, characterized in that The optimization of the control input of the incremental prediction model using the cost function based on the incremental prediction model also includes: performing quadratic programming on the optimization objectives.

Images