Path planning and tracking control method and system of unmanned surface vehicle under timed logic task

Abstract

The application provides a path planning and tracking control method and system of an unmanned ship under a timing logic task, comprising the following steps: S1, establishing an STL formalized model of the timing logic task of the unmanned ship; S2, based on a kinematic model and a dynamic model of the unmanned ship, establishing an unmanned ship control strategy under a double-layer framework of multi-time scale planning and tracking control; S3, based on the STL formalized model of the timing logic task, realizing speed planning; and S4, based on the unmanned ship control strategy and the speed planning in S3, realizing speed tracking by robust model predictive control. The application considers the characteristics of the underactuated system of the unmanned ship and the complex environment on the sea, establishes a double-layer multi-time scale planning-tracking control framework, considers the interference and uncertainty factors in the marine environment, plans the speed of the unmanned ship, considers the interference factors, realizes fast and stable tracking of the planned speed under the robust model predictive control, and realizes the optimal control of the complex timing logic task of the unmanned ship.

Description

Technical Field

[0001] This invention relates to control technology for unmanned surface vessel (USV) systems, specifically to a path planning and tracking control method and system for USVs under sequential logic tasks. Background Technology

[0002] Currently, my country is paying increasing attention to marine resources, and vigorously developing marine science and technology is crucial for the future of the country and the well-being of its people. Due to its advantages such as small size, high speed, low cost, no risk of personnel casualties, and autonomous navigation, unmanned surface vessels (USVs) have been widely used in various fields, including hydrological monitoring, marine observation, military patrols, and emergency rescue.

[0003] With the widespread application of unmanned surface vessels (USVs) in more scenarios and fields, related control technologies continue to develop and mature, and their control tasks are becoming increasingly complex. Among the many application scenarios of USVs, due to the complexity of their control tasks, the autonomous navigation capability of USVs becomes crucial for completing these tasks, and path planning and tracking control technologies play a very important role in achieving autonomous navigation of USVs.

[0004] During unmanned surface vessel (USV) missions, the personnel responsible for path planning need to consider potential obstacle areas or obstacles, as well as possible environmental disturbances, based on the mission type, and plan a feasible path accordingly. Subsequently, the USV needs to perform tracking and control based on the planned path. Due to its small size and high speed, the USV faces environmental disturbances such as wind, waves, and currents when navigating in the marine environment. These factors will significantly impact the USV's path planning and tracking control effectiveness.

[0005] The development of unmanned surface vessels (USVs) has brought higher requirements for their maritime missions. Taking patrol missions as an example, USVs are required to perform patrol tasks sequentially in designated areas within a specified time, according to different patrol times and sequences. Meanwhile, to avoid USVs entering obstacle areas and overcome environmental disturbances, existing USV control strategies, for sequential logic tasks with time and space constraints and logical requirements, such as those involving specific execution sequences, specified time requirements, or cyclical tasks, mostly only consider small bounded disturbances during operation. However, when faced with larger disturbances, there is a high possibility that the disturbances may be too large and affect the normal completion of the mission. Currently, no solutions have been provided for control strategies to further improve the degree to which USVs meet the requirements of sequential logic tasks under larger disturbances. Summary of the Invention

[0006] To address the aforementioned shortcomings in the prior art, this invention provides a path planning and tracking control method and system for unmanned surface vessels under sequential logic tasks.

[0007] According to one aspect of the present invention, a method for path planning and tracking control of an unmanned surface vessel under a sequential logic task is provided, comprising:

[0008] S1. Establish a formal STL model for the unmanned surface vessel's sequential logic task.

[0009] S2, based on the kinematic and dynamic models of unmanned surface vessels (USVs), establishes a two-layer framework for USV control strategy under multi-timescale planning and tracking control;

[0010] S3, based on the STL formal model of the temporal logic task in S1, realizes speed planning;

[0011] S4, based on the unmanned surface vessel control strategy in S2 and the speed planning in S3, uses robust model predictive control to achieve speed tracking.

[0012] Preferably, step S1, establishing a formal STL model for the unmanned surface vessel's temporal logic task, includes:

[0013] S11, Based on the qualitative semantics of STL, establish a mathematical model corresponding to the specified task scenario, and provide a standardized description of the temporal logic task of the unmanned surface vessel:

[0014]

[0015] φ=G [a,b] ψ|F [a,b] ψ|ψ1U [a,b] ψ2|φ1∧φ2 (1)

[0016] Where ψ and φ represent STL statements describing the temporal logic tasks of the unmanned surface vessel (USV), ψ1 and ψ2 represent unconstrained temporal logic tasks of the USV, and φ1 and φ2 represent temporal logic tasks of the USV with time constraints, wherein the time domain [a, b] of the constraints is defined in the STL formal statements. G [a,b] G represents the global operator. [a,b] ψ represents that the signal must always satisfy ψ in the time domain [a,b]; F [a,b] F represents the finally operator. [a,b] ψ indicates that the signal satisfies ψ at least once in the time domain [a,b]; U [a,b] Represents the until operator, ψ1U [a,b] ψ2 indicates that the signal satisfies ψ2 at least once in the time domain [a,b], and ψ1 must always be satisfied before ψ2 is satisfied; STL is a predicate logic based on predicate μ, where μ represents the unmanned surface vessel mission, and the predicate function... Represents n signals from the time domain to the real value range The mapping, the value of the predicate μ is represented by the function h as:

[0017]

[0018] Where the function h(x) is a linear or nonlinear combination of signals x;

[0019] S12, applying the predicate logic and Boolean logic operators. ∧ and ∨, temporal logic operator G [a,b] F [a,b] and U [a,b] The temporal logic requirements of the target problem are described in a standardized manner using STL statements, and an STL formal model of the unmanned surface vessel's temporal logic task is established.

[0020] Preferably, S2 establishes an unmanned surface vessel (USV) control strategy based on the USV's kinematic and dynamic models, within a two-layer framework of multi-timescale planning and tracking control, including:

[0021] S21, Establish the kinematic model of the unmanned surface vessel:

[0022] The real-time position coordinates (x, y) of the unmanned surface vessel in the world coordinate system are compared with the heading angle. As state variables, the pitch velocity u, sway velocity v, and bow roll velocity r of the unmanned surface vessel in the hull coordinate system are used as input variables, while the ocean current velocity v is also included. c With angle β c As a disturbance in the marine environment, the kinematic model of the unmanned surface vessel is described as follows:

[0023]

[0024] S22, Establish the dynamic model of the unmanned surface vessel:

[0025] The unmanned surface vessel's pitch velocity u, sway velocity v, and bow roll rate r in the hull coordinate system are used as state variables, and the propeller thrust τ is used as the state variable. u and rudder angle τ r As input, the dynamic model of the unmanned surface vessel is described as follows:

[0026]

[0027] Where m and I z Let represent the mass and moment of inertia of the unmanned surface vessel, respectively. δ The factors influencing the speed and yaw moment of the unmanned surface vessel (USV) are (·). h This indicates the hydrodynamic force / torque acting on the unmanned surface vessel;

[0028] S23, Design a two-layer, multi-timescale planning-tracking control framework:

[0029] Based on the kinematic model of the unmanned surface vessel (USV), and considering the STL normalized description of the sequential logic task, the optimal sway velocity u, yaw velocity v, and yaw rate r of the USV are planned to improve the degree to which the USV satisfies the sequential logic task when there is a disturbance.

[0030] Based on the aforementioned unmanned surface vessel (USV) dynamics model, the optimal pitch velocity u, sway velocity v, and yaw rate r obtained from the planning are used as reference velocities. Robust model predictive control is then used to track and control these reference velocities to obtain the optimal propeller thrust τ. u and rudder angle τ r This forms a two-layer, multi-timescale planning-tracking control framework, enabling motion control of unmanned surface vessels in disturbed marine environments under sequential logic tasks.

[0031] Preferably, step S3, based on the STL formal model of the temporal logic task in S1, implements speed planning, including:

[0032] S31, Quantification of the degree of satisfaction at the planning level:

[0033] Utilizing the robust semantics of STL φ Quantify the degree to which the STL statement specification φ is satisfied, and utilize the robust semantics ρ of the STL statement specification φ. φ (x N+1 k) describes the robustness of the unmanned surface vessel (USV) path planning results, and represents the USV's state sequence x. N+1 The degree of satisfaction of the STL statement φ is quantified as a real number ρ. φ It is used to measure whether the position state of the unmanned surface vessel meets the mission requirements of STL and the degree to which the mission requirements are met or violated;

[0034] S32, Establish the distance function dist:

[0035] ρ φ (x N+1 The absolute value of (k) is considered as the control sequence x. N+1 The distance to the set of trajectories that satisfy or violate the STL statement φ, i.e., the distance between the unmanned surface vessel's position and the designated obstacle or target area; given a state variable xk∈χ and the set of trajectories that satisfy or violate the STL statement φ. Then x k Closed boundary to A The minimum Euclidean distance is used This indicates that the robust semantics of an STL task are:

[0036]

[0037] S33, Establish the objective function P ρ :

[0038] Based on the kinematic model of the unmanned surface vessel, and considering the interference factor ω, the time-domain sequential logic tasks φ and ρ are taken into account. φ To determine the robustness of the STL task φ, the objective function obtained is the internal optimization problem P. ρ for:

[0039]

[0040]

[0041]

[0042]

[0043] in, Let u(k) represent the state variables of the unmanned surface vessel's kinematic model at time k; u(k) = [u(k), v(k), r(k)] T This represents the control input to the kinematic model of the unmanned surface vessel at time k; x(k+1)=f d (x(k),u(k),ω(k)) is the discretized kinematic model of the unmanned surface vessel, where Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are usually taken as identity matrices; γ represents the compromise coefficient.

[0044] S34, Smooth the objective function based on the distance function dist:

[0045] The distance function dist is approximated by Meyer wavelet extension to obtain an approximate value of the distance function dist;

[0046] The non-differentiable terms in the distance function dist are smoothed to eliminate singularities.

[0047] S35, perform approximation processing on the min / max functions in the objective function:

[0048] By approximating the min and / or max terms in the objective function, a smooth and differentiable objective function is obtained:

[0049]

[0050]

[0051] in, This represents the sequence a1,…,a m The approximate result is taken from the maximum value. i represents the scaling factor. The larger i is, the smaller the approximation error. When i approaches ∞, the approximation error is close to 0.

[0052] S36, Perform dual reconstruction of the objective function:

[0053] The min-max form of the optimization problem is transformed into a minimization form by using the dual reconstruction of the internal optimization problem. The upper bound of the original internal optimization problem is minimized to obtain the optimal solution of the optimization problem.

[0054] S37, Establish the final objective function and iteratively optimize it using the gradient descent algorithm:

[0055] Given the initial position of the unmanned surface vessel, the initial input sequence and initial state sequence

[0056] Based on the task requirements, provide the STL specification statement φ, and based on the robust semantics of the STL statement, obtain the robust semantic expression ρ used to represent the robustness of the unmanned surface vessel system. φ The non-differentiable terms are smoothed and approximated, and the sum of the L2 norms of the state and input variables is used. A compromise is made to obtain the final objective function;

[0057] Sequential quadratic programming (SQP) is used as a gradient optimizer to iteratively optimize the final objective function. Taylor expansion is then used to refine the objective function at the iteration point x. k The problem is simplified to a quadratic function, and the constraints are simplified to linear functions, resulting in an approximate problem in the form of a quadratic programming problem. The optimal solution of this iteration is then obtained. As the next direction of search;

[0058] Iterate repeatedly to output the optimal sway velocity, roll velocity, and pitch velocity of the planning layer.

[0059] Preferably, in step S31, the robust semantics ρ of STL are utilized. φ Quantify the degree to which the STL statement specification φ is satisfied, and utilize the robust semantics ρ of the STL statement specification φ. φ (x N+1 k) describes the robustness of the unmanned surface vessel (USV) path planning results, and represents the USV's state sequence x. N+1 The degree of satisfaction of the STL statement φ is quantified as a real number ρ. φ This is used to measure whether the position state of the unmanned surface vessel meets the STL mission requirements and the degree to which the mission requirements are met or violated, including:

[0060] ρ φ (x N+1 ,k)>0 indicates the state sequence x of the unmanned surface vessel. N+1 At time k, the STL statement φ is satisfied, that is...

[0061] ρ φ (x N+1 (k) < 0 indicates that the state sequence x N+1 At time k, the STL statement φ is violated, i.e.

[0062] ρ φ (x N+1 The magnitude of k is directly proportional to the robustness of the system;

[0063] STL statement φ with respect to signal sequence x N+1 Robustness at time k is recursively defined as:

[0064]

[0065]

[0066] ρ φ∧ψ (x N+1 ,k)=min(ρ φ (x N+1 ,k),ρ ψ (x N+1 ,k))

[0067] ρ φ∧ψ (x N+1 ,k)=max(ρ φ (x N+1 ,k),ρ ψ (x N+1 ,k))

[0068]

[0069]

[0070]

[0071] Preferably, in step S34, the approximation of the distance function using Meyer wavelet extension to obtain an approximate value of the distance function dist includes:

[0072] Let the Meyer scaling function be defined with respect to the smoothness function θ(x) as:

[0073]

[0074] Wherein, the smooth function θ(x) can take any form:

[0075]

[0076] The expression for the Meyer wavelet function defined in the frequency domain is obtained as follows:

[0077]

[0078] The time-domain expressions for the Meyer scaling function and wavelet are as follows:

[0079]

[0080] in,

[0081]

[0082]

[0083] For an n-dimensional wavelet, it can be constructed using the tensor product, where ψ represents a univariate wavelet function; let E represent the unit cube [0,1]. n A set of vertices, for each vertex e = (e1, e2, ..., e...). n )∈E and x N+1 =(x1,x2,…,x n Define a multivariable function as follows:

[0084]

[0085] Given and Then define I = 2 -k (j+[0,1] n A set of the form ) is The binary cube in the middle;

[0086] make express The set of all binary cubes in the set is the function set. Formed A normal basis function is an orthogonal basis function if ψ is an orthogonal wavelet; double basis functions The construction is the same as ψ; The wavelet extension of each function in the expression is represented as:

[0087]

[0088] in<h,g> :=∫h(x)g(x)dx means Inner product;

[0089] The required approximation of the distance function dist is obtained by selecting a finite number of terms for this extension, i.e., by utilizing a finite set. The approximate value is:

[0090]

[0091] Preferably, in step S36, the dual reconstruction of the objective function includes:

[0092] The optimization problem P ρ The internal optimization problem takes the form of:

[0093]

[0094]

[0095]

[0096]

[0097] Rewrite it in the following form:

[0098]

[0099] stPu N +Qω N +g≤0

[0100] Among them, Pu N +Qω N +g≤0 defines the physical constraints of the system, which are related to the maximum / minimum values ​​of the input and the disturbance term. P and Q are the inequality constraint matrices, respectively, and g represents the constant vector. Then, the Lagrange dual function of the above optimization problem can be expressed in the following form:

[0101]

[0102] Where λ represents the Lagrange multiplier; and will be combined with the perturbation term ω N The relevant items are separated and organized as follows:

[0103]

[0104] When λ T Qω N The function reaches its maximum value when the term is 0. The dual problem of the internal optimization problem is obtained:

[0105]

[0106] stQ T λ = 0

[0107] λ≥0

[0108] By utilizing the duality of the internal optimization problem, the optimization problem P... ρ Transform into the following form:

[0109]

[0110] stQ T λ = 0

[0111] λ≥0

[0112] This transforms a multi-parameter problem into a non-convex optimization problem.

[0113] Preferably, S4, robust model predictive control for speed tracking, includes:

[0114] S41, perform linear discretization on the unmanned surface vessel dynamics model:

[0115] The dynamic model of the unmanned surface vessel is simplified as follows: The nominal unmanned surface vessel system dynamics model does not consider system disturbances existing at sea, wherein the state variables... To simplify the model of the unmanned surface vessel system's pitch velocity, sway velocity, and bow angular velocity, the input quantities are... To simplify the model, propeller thrust and rudder angle are represented; the planning layer obtains the optimal speed u of the unmanned surface vessel at time k. * (k), v * (k), r * (k), serving as the reference velocity s for the tracking layer r (k)=[u r (k),u r (k),r r (k)], the dynamic model of the unmanned surface vessel system in s r Linearization and discretization are performed at (k) to obtain the discretized nominal unmanned surface vessel system dynamics model:

[0116]

[0117] Where l represents the sampling time of the tracking layer. These are the state variables of the nominal unmanned surface vessel system dynamics model. The inputs are the nominal values ​​for the unmanned surface vessel system dynamics model.

[0118] S42, Based on the discretized unmanned surface vessel (USV) dynamics model, establish the objective function for the nominal USV system tracking layer:

[0119] With N d To predict the time domain, the objective function P1 for the nominal unmanned surface vessel system to achieve speed tracking is:

[0120]

[0121]

[0122]

[0123] Where Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are taken as identity matrices; and This represents the upper and lower limits of the input quantities of the nominal unmanned surface vessel dynamics model; P>0 is the unique positive definite solution of the algebraic Riccati equation; the algebraic Riccati equation is:

[0124] P = Q + A T PA-A T PB(R+B T PB) -1 B T PA

[0125] S43, Solve for the objective function:

[0126] Solving the objective function P1 using the gradient method yields the optimal state sequence of the nominal unmanned surface vessel system dynamics model at time l. and optimal input sequence

[0127] S44, Design a robust model predictive control law:

[0128] Considering system disturbances at sea, the actual dynamic model of the unmanned surface vessel system is as follows:

[0129] s(l+1)=As(l)+Bw(l)+Cd(l)

[0130] Where, the state variables s(l) = [u(l); v(l); r(l)] represent the actual unmanned surface vessel system's pitch velocity, sway velocity, and yaw rate, and the input variables w(l) = [τ u (l); τ r [(l)] represents the propeller thrust and rudder angle of the actual unmanned surface vessel system, d(l) represents the system disturbance at time l, and C represents the disturbance term coefficient matrix;

[0131] If the current actual speed And the robust control law is taken as:

[0132]

[0133] Then s(l+1) always satisfies in, Let s(l) and w(l) represent the state variables and input variables obtained at time l when the initial state of the nominal unmanned surface vessel (USV) dynamics model is s, respectively. Let s(l) and w(l) represent the state variables and input variables of the actual USV dynamics model at time l, respectively. Describe the perturbation invariant set that satisfies in K is a state feedback control law that satisfies:

[0134] K = -(R + B) T PB) -1 B T PA

[0135] S45, based on the objective function and the robust model predictive control law, speed tracking is achieved using a tracking layer, including:

[0136] Based on the nominal unmanned surface vessel dynamics model, the optimal control sequence at time l is obtained when the actual system state is s. control sequence Corresponding optimal state variables in If the actual unmanned surface vessel dynamics model is in this state Then take the optimal control sequence The first control quantity in

[0137] Utilizing robust control laws As an actual control variable, when applied to the nominal unmanned surface vessel dynamics model, the state variables at the next moment satisfy...

[0138] Entering the next round of rolling, As new initial state variables, solve for the optimal control sequence of the new nominal unmanned surface vessel dynamics model.

[0139] Pick The first control quantity The control quantity at time l+1 can be obtained from the robust control law.

[0140] Applying this to the nominal unmanned surface vessel dynamics model ensures that the state variables in the actual system are within the perturbation invariant set centered on the state variables of the nominal system, thus ensuring the unmanned surface vessel's stable tracking of the reference speed issued by the planning layer in an environment with uncertainties, and also ensuring robustness.

[0141] The rolling optimization is repeated until the last prediction cycle; when the tracking layer receives the next optimal speed as the reference speed, the speed tracking process is repeated again, and finally the speed tracking of the entire time-series logic task is realized.

[0142] Preferably, in S3, the control task in the obstacle environment includes: obstacle avoidance and / or arrival tasks of the unmanned surface vessel.

[0143] Preferably, in S4, Find the minimum perturbation invariant set:

[0144] It refers to time i, A K The value, K represents the state feedback control law, and D refers to the set of disturbance ranges. The purpose of taking the minimum disturbance invariant set is to control system constraints, ensure that the actual system state variables are located in a subset centered on the nominal system state variables, and ensure the robustness of the system.

[0145] According to a second aspect of the present invention, a path planning and tracking control system for an unmanned surface vessel under a sequential logic task is provided, comprising:

[0146] The STL module establishes a formalized STL model for the timing logic task of the unmanned surface vessel.

[0147] The dual-layer strategy module establishes a dual-layer framework for unmanned surface vessel (USV) control strategy based on the kinematic and dynamic models of USVs, encompassing multi-timescale planning and tracking control.

[0148] The speed planning module implements speed planning based on the STL formal model of the aforementioned time-series logic task.

[0149] The tracking control module, based on the unmanned surface vessel control strategy and the speed planning, uses robust model predictive control to achieve speed tracking.

[0150] Compared with the prior art, the embodiments of the present invention have at least one of the following beneficial effects:

[0151] The path planning and tracking control method for unmanned surface vessel (USV) systems under time-sequential logic tasks in this invention embodiment considers the characteristics of the USV's underactuated system and the complex marine environment, and establishes a two-layer, multi-time-scale planning-tracking control framework; it considers the interference and uncertainty factors in the marine environment and plans the USV's speed; considering the interference factors, it achieves fast and stable tracking of the planned speed under robust model predictive control, thereby realizing optimized control of the USV's complex time-sequential logic tasks.

[0152] The path planning and tracking control method for unmanned surface vessel (USV) systems under sequential logic tasks in this embodiment of the invention introduces a formal method based on STL to standardize the description of the sequential logic tasks of USV systems and establish a mathematical model of the task scenario.

[0153] The path planning and tracking control method for unmanned surface vessel (USV) systems under sequential logic tasks in this embodiment of the invention addresses the issue of significant system disturbances by smoothing and reconstructing the optimization problem using dual methods. This achieves efficient solution to the optimization problem and improves the degree to which the optimization results satisfy the sequential logic tasks of the USV.

[0154] The path planning and tracking control method for unmanned surface vessel (USV) systems under sequential logic tasks in this embodiment of the invention addresses the difficulty of controlling underactuated USV systems. It constructs a two-layer, multi-timescale planning-tracking control framework based on kinematic and dynamic equations, comprehensively considers the requirements of complex sequential logic tasks, and designs control strategies for the planning and tracking layers to achieve fast and stable control of USV tasks. Attached Figure Description

[0155] Other features, objects, and advantages of the invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0156] Figure 1 The flowchart illustrates a method for path planning and tracking control of unmanned surface vessels under a sequential logic task, as provided in an embodiment of the present invention.

[0157] Figure 2 This is a schematic diagram of the initial scenario of the unmanned surface vessel's timing logic task in a preferred embodiment of the present invention;

[0158] Figure 3 This is a schematic diagram of the three degrees of freedom parameters of an unmanned surface vessel in a preferred embodiment of the present invention;

[0159] Figure 4 This is a schematic diagram of the unmanned surface vessel control strategy under a sequential logic task provided in a preferred embodiment of the present invention;

[0160] Figure 5 This is a schematic diagram of a planning layer speed optimization algorithm provided in a preferred embodiment of the present invention;

[0161] Figure 6 This is a schematic diagram of the path planning results of an unmanned surface vessel in a preferred embodiment of the present invention under undisturbed conditions;

[0162] Figure 7 This is a schematic diagram of the path planning results of an unmanned surface vessel under different disturbances in a preferred embodiment of the present invention;

[0163] Figure 8 This is a schematic diagram of the simulation results of the dual-layer planning and control of an unmanned surface vessel in a preferred embodiment of the present invention. Detailed Implementation

[0164] The embodiments of the present invention are described in detail below: These embodiments are implemented based on the technical solution of the present invention, and provide detailed implementation methods and specific operation processes. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention.

[0165] The Signal Sequential Logic (STL) of this invention represents the specification and monitoring of the desired behavior of a physical system and imposes temporal constraints on events. It can regulate attributes such as time and space variables and automatically generate monitors to test attributes on various simulated trajectories. It has been used in various types of continuous and hybrid systems. It provides a concise and standardized mathematical form to describe complex sequential logic problems, with robust quantization semantics. It can use a real value to evaluate the degree to which the control trajectory meets or violates task requirements. At the same time, its predicate logic can describe the task coupling between subsystems and can be used to handle subtasks such as arrival and obstacle avoidance.

[0166] This invention provides an embodiment, Figure 1 The flowchart shows the unmanned surface vessel path planning and tracking control method under the sequential logic task provided in this embodiment.

[0167] like Figure 1 As shown in the flowchart of the unmanned surface vessel path planning and tracking control method under a sequential logic task provided in this embodiment, it may include the following steps:

[0168] S1. Establish a formal STL model for the unmanned surface vessel's sequential logic task.

[0169] S2, Based on the kinematic and dynamic models of unmanned surface vessels (USVs), establish a two-layer framework for USV control strategies.

[0170] S3, based on the STL formal model of the temporal logic task in S1, realizes speed planning;

[0171] S4, based on the unmanned surface vessel control strategy in S2 and the speed planning in S3, uses robust model predictive control to achieve speed tracking.

[0172] This invention introduces STL to establish a task specification framework, forming a standardized representation method for target tasks and constraints under the STL framework. It uses robust quantization semantics to express temporal logic subtasks such as arrival time, action sequence, and obstacle avoidance, establishing an objective function to achieve unmanned surface vessel (USV) speed planning with high robustness. This improves the degree to which USVs satisfy temporal logic tasks in environments with significant interference, and is of great significance for promoting the widespread application of USV systems in military and civilian fields.

[0173] In a preferred embodiment of the present invention, a preferred process for implementing S1 and establishing an STL formal model of the unmanned surface vessel's temporal logic task is provided, including:

[0174] S11, Based on STL formal statements, a standardized description of the unmanned surface vessel's sequential logic tasks is provided, and a mathematical model of the task scenario is established:

[0175]

[0176] φ=G[a,b] ψ|F [a,b] ψ|ψ1U [a,b] ψ2|φ1∧φ2

[0177] Here, ψ and φ represent STL statements describing the temporal logic tasks of an unmanned surface vessel (USV). ψ1 and ψ2, in the form of ψ, represent unconstrained temporal logic tasks of USVs; φ1 and φ2, in the form of φ, represent temporal logic tasks of USVs with time constraints, where the time domain [a, b] of the constraints is defined in the STL formal statements. G [a,b] G represents the global operator. [a,b] ψ represents that the signal must always satisfy ψ in the time domain [a,b]; F [a,b] F represents the finally operator. [a,b] ψ indicates that the signal satisfies ψ at least once in the time domain [a,b]; U [a,b] Represents the until operator, ψ1U [a,b] ψ2 indicates that the signal satisfies ψ2 at least once in the time domain [a,b], and must always satisfy ψ1 before that; STL is a predicate logic based on predicate μ, where μ represents the unmanned surface vessel mission, and the predicate function... Represents n signals from the time domain to the real value range The mapping, the value of the predicate μ can be represented by the function h as:

[0178]

[0179] Where the function h(x) is a linear or nonlinear combination of signals x;

[0180] S12, employing predicate logic and Boolean logic operators. Temporal logic operators (G [a,b] F [a,b] U [a,b] The timing logic requirements of the target problem are described in a standardized manner using STL statements, and an STL formal model of the timing logic task of the unmanned surface vessel is established.

[0181] This embodiment addresses the complex temporal logic tasks of unmanned surface vessel (USV) systems by introducing a formal method based on STL to provide a standardized description of these tasks and establish a mathematical model of the task scenario that is accurate and relevant to reality.

[0182] In a preferred embodiment of the present invention, implementation S2 is provided, which establishes a two-layer unmanned surface vessel (USV) control strategy based on the USV's kinematic and dynamic models, including:

[0183] S21, Establish the kinematic model of the unmanned surface vessel:

[0184] The real-time position coordinates (x, y) of the unmanned surface vessel in the world coordinate system are compared with the heading angle. As state variables, the pitch velocity u, sway velocity v, and bow roll velocity r of the unmanned surface vessel in the hull coordinate system are used as input variables, while the ocean current velocity v is also included. c With angle β c As a disturbance in the marine environment, the kinematic model of the unmanned surface vessel is described as follows:

[0185]

[0186] S22, Establish the dynamics model of the unmanned surface vessel:

[0187] The unmanned surface vessel's pitch velocity u, sway velocity v, and bow roll rate r in the hull coordinate system are used as state variables, and the propeller thrust τ is used as the state variable. u and rudder angle τ r As input, the dynamic model of the unmanned surface vessel is described as follows:

[0188]

[0189] Where m and I z Let represent the mass and moment of inertia of the unmanned surface vessel, respectively. δ The factors influencing the speed and yaw moment of the unmanned surface vessel (USV) are (•). h This indicates the hydrodynamic force / torque acting on the unmanned surface vessel;

[0190] S23, Design a two-layer, multi-timescale planning-tracking control framework:

[0191] Based on the kinematic model of the unmanned surface vessel (USV), the STL normalized description of the sequential logic task is considered. The robust semantics of the STL formed by formula (1) and the meaning of each parameter in S1 are used as part of the objective function of the planning layer. The optimal sway velocity u, sway velocity v and yaw velocity r of the USV are planned to maximize the satisfaction of the sequential logic task when there is a disturbance.

[0192] Based on the dynamics model of the unmanned surface vessel (USV), the optimal pitch velocity u, sway velocity v, and yaw rate r obtained from the planning layer are used as reference velocities. Robust model predictive control is then used to track these reference velocities, thereby obtaining the optimal propeller thrust τ. u and rudder angle τ r Therefore, a two-layer, multi-timescale planning-tracking control framework is formed to realize the motion control of unmanned surface vessels in a disturbed marine environment under sequential logic tasks.

[0193] This embodiment addresses the difficulty of controlling underactuated unmanned surface vessel (USV) systems by constructing a two-layer, multi-timescale planning-tracking control framework based on kinematic and dynamic equations. Taking into account the requirements of complex temporal logic tasks, it designs control strategies for the planning and tracking layers to achieve rapid and stable control of USV tasks.

[0194] In a preferred embodiment of the present invention, implementation S3 is provided, which implements speed planning based on the STL formal model of the temporal logic task in S1, including:

[0195] S31, Quantification of Satisfaction Level:

[0196] Utilizing the robust semantics of STL φ Quantify the degree to which the STL statement specification φ is satisfied, and utilize the robust semantics ρ of the STL statement specification φ. φ (x N+1 k) describes the robustness of the unmanned surface vessel (USV) path planning results, and represents the USV's state sequence x. N+1 The degree of satisfaction of the STL statement φ is quantified as a real number ρ. φ , used to measure whether the position state of the unmanned surface vessel meets the STL mission requirements and the degree to which the mission requirements are met or violated; where ρ φ (x N+1 ,k)>0 indicates the state sequence x of the unmanned surface vessel. N+1 At time k, the STL statement φ, i.e., x, is satisfied. N+1 I = φ; ρ φ (x N+1 (k) < 0 indicates that the state sequence x N+1 At time k, the STL statement φ is violated, i.e., x. N+1 I≠φ;ρ φ (x N+1 The larger the value of φ, the stronger the system robustness; the STL statement φ relates to the signal sequence x. N+1 Robustness at time k is recursively defined as:

[0197]

[0198]

[0199] ρ φ∧ψ (x N+1 ,k)=min(ρ φ (x N+1 ,k),ρ ψ (x N+1 ,k))

[0200] ρ φ∧ψ (x N+1 ,k)=max(ρ φ (xN+1 ,k),ρ ψ (x N+1 ,k))

[0201]

[0202]

[0203]

[0204] S32, Establish the distance function:

[0205] ρ φ (x N+1 The absolute value of (k) is considered as the control sequence x. N+1 The distance to the set of trajectories that satisfy or violate the STL statement φ, i.e., the distance between the unmanned surface vessel's position and the designated obstacle or target area. Given a state variable x k ∈χ and the set of trajectories that satisfy or violate the STL statement φ. Then x k Closed boundary to A The minimum Euclidean distance is used Therefore, the robust semantics of an STL task are:

[0206]

[0207] S33, Establish the internal optimization problem, i.e., the objective function:

[0208] Based on the kinematic model of the unmanned surface vessel, and considering the influence of disturbance factor ω, a temporal logic task φ and ρ in the time domain of N are taken into account. φ To determine the robustness of the STL task φ, the optimization problem P is obtained. ρ The statement is as follows:

[0209]

[0210]

[0211]

[0212]

[0213] in, Let u(k) represent the state variables of the unmanned surface vessel's kinematic model at time k; u(k) = [u(k), v(k), r(k)] T This represents the control input to the kinematic model of the unmanned surface vessel at time k; x(k+1)=f d(x(k),u(k),ω(k)) is the discretized kinematic model of the unmanned surface vessel, where Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are usually taken as identity matrices; γ represents the compromise coefficient.

[0214] S34, Smoothing the objective function based on the distance function dist:

[0215] By approximating the distance function using Meyer wavelet extension, an approximation of the distance function `dist` can be obtained. Essentially, wavelet transform derives from a single prototype wavelet through symbolic decomposition using a set of basis functions, i.e., through scaling or translation operations, and thus possesses orthogonality. The Meyer scaling function is defined with respect to the smooth function θ(x) as:

[0216]

[0217] Where w is the scaling parameter, and the smoothing function θ(x) can take any form satisfying the following conditions:

[0218]

[0219] The expression for the Meyer wavelet function defined in the frequency domain can be obtained:

[0220]

[0221] The time-domain expressions for the Meyer scaling function and wavelets are as follows:

[0222]

[0223] in,

[0224]

[0225]

[0226] For an n-dimensional wavelet, it can be constructed using the tensor product, where ψ represents a univariate wavelet function; let E represent the unit cube [0,1]. n A set of vertices, for each vertex e = (e1, e2, ..., e...). n )∈E and x N+1 =(x1,x2,…,x n The multivariable function is defined as follows:

[0227]

[0228] Given and Then we can define I = 2 -k (j+[0,1] nA set of the form ) is The binary cube in the middle; let express The set of all binary cubes in the set is the function set. It was formed A normal basis function is an orthogonal basis function if ψ is an orthogonal wavelet; double basis functions The construction is the same as ψ; The wavelet extension representation of each function in the table is as follows:

[0229]

[0230] in<h,g> :=∫h(x)g(x)dx means The inner product in; the approximation of the required distance function dist is obtained by selecting a finite number of terms in this extension, i.e., by utilizing a finite set. The approximate values ​​are as follows:

[0231]

[0232] This allows for a smooth approximation of the non-differentiable terms in the distance function dist, eliminating their singularities.

[0233] S35, approximation processing for the min / max functions in the objective function:

[0234] By approximating the min and / or max terms in the objective function, a smooth and differentiable objective function is obtained:

[0235]

[0236]

[0237] in, This represents the sequence a1,…,a m The approximate result is taken from the maximum value. i represents the scaling factor. The larger i is, the smaller the approximation error. When i approaches ∞, the approximation error is almost 0.

[0238] S36, Dual Reconstruction of the Internal Optimization Problem:

[0239] For the optimization problem P ρ Since directly solving the min-max problem requires solving the internal and external optimization problems separately, and designing different optimization objectives for each, the min-max form optimization problem is transformed into a minimization form by utilizing the duality of the internal optimization problem. This minimizes the upper bound of the original internal optimization problem, thus obtaining the optimal solution to the optimization problem P. ρThe internal optimization problem takes the following form:

[0240]

[0241]

[0242]

[0243]

[0244] Rewrite it in the following form:

[0245]

[0246] Among them, Pu N +Qω N +g≤0 defines the physical constraints of the system, which are related to the maximum / minimum values ​​of the input and the disturbance term. P and Q are the inequality constraint matrices, respectively, and g represents the constant vector. Then, the Lagrangian dual function of the above optimization problem can be expressed in the following form:

[0247]

[0248] Where λ represents the Lagrange multiplier; and will be combined with the perturbation term ω N The relevant items are separated and organized into the following form:

[0249]

[0250] When λ T Qω N The function can reach its maximum value when the term is 0. The dual problem of the internal optimization problem can be obtained:

[0251]

[0252] stQ T λ=0

[0253] λ≥0

[0254] By utilizing the duality property of the internal optimization problem, the optimization problem P can be transformed. ρ Transform into the following form:

[0255]

[0256] stQ T λ=0

[0257] λ≥0

[0258] Ultimately, this transforms a multi-parameter problem into a non-convex optimization problem;

[0259] S37, Gradient Descent Algorithm Iterative Optimization:

[0260] Given the initial position of the unmanned surface vessel, the initial input sequence and initial state sequence Based on the task requirements, provide the STL specification statement φ, and based on the robust semantics of the STL statement, obtain the robust semantic expression ρ used to represent the robustness of the unmanned surface vessel system. φ After smoothing the non-differentiable terms, it can be approximated with... A compromise is made to obtain the final objective function; sequential quadratic programming (SQP) is used as the gradient optimizer to iteratively optimize the above problem, and the obtained objective function is obtained by Taylor expansion at the iteration point x. k By simplifying the problem to a quadratic function and the constraints to linear functions, we can obtain an approximate problem in the form of a quadratic programming problem. The optimal solution for this iteration is then obtained. This serves as the next search direction; through iterative iterations, the optimal sway velocity, roll velocity, and pitch velocity of the planning layer are output.

[0261] This embodiment addresses the issue of significant system disturbances by smoothing and reconstructing the optimization problem using dual methods. This achieves efficient solution to the optimization problem and improves the degree to which the optimization results satisfy the temporal logic tasks of the unmanned surface vessel.

[0262] In a preferred embodiment of the present invention, implementation S4 is provided, which, based on the unmanned surface vessel control strategy in S2 and the speed planning in S3, uses robust model predictive control to achieve speed tracking, including:

[0263] S41, perform linear discretization on the unmanned surface vessel dynamics model:

[0264] The nominal unmanned surface vessel system dynamics model is simplified as follows: The nominal unmanned surface vessel system dynamics model does not consider system disturbances existing at sea, wherein the state variables... To simplify the model of the unmanned surface vessel system's pitch velocity, sway velocity, and bow angular velocity, the input quantities are... To simplify the model, propeller thrust and rudder angle are represented; the planning layer obtains the optimal speed u of the unmanned surface vessel at time k. * (k), v * (k), r * (k), serving as the reference velocity s for the tracking layer r (k)=[u r (k),v r (k),r r (k)], the dynamic model of the unmanned surface vessel system in sr Linearization and discretization are performed at (k) to obtain the discretized nominal hydrodynamic model of the unmanned surface vessel system:

[0265]

[0266] Where l represents the sampling time of the tracking layer. These are the state variables of the nominal unmanned surface vessel system dynamics model. The inputs are the nominal values ​​for the unmanned surface vessel system dynamics model.

[0267] S42, Establish the objective function for the tracking layer of the nominal unmanned surface vessel system:

[0268] With N d To predict the time domain, the objective function P1 for the nominal unmanned surface vessel system to achieve speed tracking is:

[0269]

[0270]

[0271]

[0272] Where Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are usually taken as identity matrices; and This represents the upper and lower limits of the input quantities of the nominal unmanned surface vessel dynamics model; P>0 is the unique positive definite solution of the algebraic Riccati equation; the algebraic Riccati equation is:

[0273] P = Q + A T PA-A T PB(R+B T PB) -1 B T PA

[0274] S43. Solving the above optimization problem using the gradient method yields the optimal state sequence of the nominal unmanned surface vessel system dynamics model at time l. and optimal input sequence

[0275] S44, Design a robust model predictive control law:

[0276] Considering system disturbances at sea, the actual dynamic model of the unmanned surface vessel system is as follows:

[0277] s(l+1)=As(l)+Bw(l)+Cd(l)

[0278] Where, the state variables s(l) = [u(l); v(l); r(l)] represent the actual unmanned surface vessel system's pitch velocity, sway velocity, and yaw rate, and the input variables w(l) = [τ u (l); τ r [(l)] represents the propeller thrust and rudder angle of the actual unmanned surface vessel system, d(l) represents the system disturbance at time l, and C represents the disturbance term coefficient matrix;

[0279] If the current actual speed And the robust control law is taken as:

[0280]

[0281] Then s(l+1) always satisfies in, Let s(l) and w(l) represent the state variables and input variables obtained at time l when the initial state of the nominal unmanned surface vessel (USV) dynamics model is s, respectively. Let s(l) and w(l) represent the state variables and input variables of the actual USV dynamics model at time l, respectively. Describe the perturbation invariant set that satisfies in K is a state feedback control law that satisfies:

[0282] K = -(R + B) T PB) -1 B T PA

[0283] S45 utilizes a tracking layer to achieve speed tracking, including:

[0284] Based on the nominal unmanned surface vessel dynamics model, the optimal control sequence at time l is obtained when the actual system state is s. control sequence Corresponding optimal state variables in If the actual unmanned surface vessel dynamics model is in this state Then take the optimal control sequence The first control quantity in Reutilize robust control law As an actual control variable, when applied to the nominal unmanned surface vessel dynamics model, the state variables at the next moment satisfy... Then it will proceed to the next round of rolling. As new initial state variables, to solve for the optimal control sequence of the new nominal unmanned surface vessel dynamics model. Take again The first control quantity The control quantity at time l+1 can be obtained from the robust control law. Applying this to the nominal unmanned surface vessel (USV) dynamics model ensures that the state variables in the actual system are within the perturbation invariant set centered on the state variables of the nominal system. This guarantees the USV's stable tracking of the reference speed issued by the planning layer in environments with uncertainties, and ensures a certain degree of robustness. The above steps are continuously optimized until the last prediction cycle. When the tracking layer receives the next optimal speed as the reference speed, the above tracking steps are repeated to ultimately achieve speed tracking for the entire time-series logical task.

[0285] In a preferred embodiment of the present invention Find the minimum perturbation invariant set:

[0286]

[0287] D refers to the set of disturbance ranges. The purpose of taking the minimum disturbance invariant set is to control system constraints, thereby ensuring that the actual system state variables are located in a subset centered on the nominal system state variables, thus ensuring the robustness of the system.

[0288] Based on the same inventive concept, in other embodiments of the present invention, a path planning and tracking control system for an unmanned surface vessel under a sequential logic task is provided, comprising:

[0289] The STL module establishes a formalized STL model for the timing logic task of the unmanned surface vessel.

[0290] The dual-layer strategy module establishes a dual-layer framework for unmanned surface vessel (USV) control strategy based on the kinematic and dynamic models of USVs, encompassing multi-timescale planning and tracking control.

[0291] The speed planning module implements speed planning based on the STL formal model of the aforementioned time-series logic task.

[0292] The tracking control module, based on the unmanned surface vessel control strategy and the speed planning, uses robust model predictive control to achieve speed tracking.

[0293] The specific implementation techniques of each module / unit in the above examples of the present invention can be referred to the steps of the path planning and tracking control system method for unmanned surface vessels under the time-sequential logic task in the above embodiments, and will not be repeated here.

[0294] The unmanned surface vessel (USV) path planning and tracking control method for sequential logic tasks provided in this invention can achieve complex sequential logic tasks while ensuring high robustness, continuously guaranteeing system robustness and stability. The technical solution provided in this invention is further illustrated below with reference to the accompanying drawings and simulation examples.

[0295] Please see Figure 2The unmanned surface vessel (USV) needs to perform patrol missions, including sub-tasks such as obstacle avoidance and arrival. At the planning layer, the USV’s speed and heading are designed based on the STL normalized description, and then robust model predictive control (RMPC) is used to achieve fast and stable speed tracking.

[0296] The initial position coordinates of the unmanned surface vessel are x0 = [-3, -1, 0]. T The orange area (the larger square in the middle) in the image is the danger zone. unsafe You should stay as far away as possible. The blue area in the diagram (surrounded by three smaller squares) is the target area. You should pass through the target area μ in sequence. mustA With target region μ mustB And finally reach the target area μ terminalC The time domain length N is set to 32.

[0297] Please see Figure 3 The unmanned surface vessel's (USV) pitch velocity u, sway velocity v, and bow roll angular velocity r in the ship coordinate system are used as inputs, and the USV's real-time position coordinates (x, y) and heading angle in the world coordinate system are used as inputs. As a state variable, the kinematic model of the unmanned surface vessel is described as follows:

[0298]

[0299] In this example, we take u∈[-0.4,0.4], v∈[-0.4,0.4], r∈[-π,π], and (x,y)∈[-4,4]. 2 ,

[0300] The pitch velocity, sway velocity, and bow angular velocity of the unmanned surface vessel in the hull coordinate system are used as state variables, and the propeller thrust τ is used as the state variable. u and rudder angle τ r As input, the dynamics model of the unmanned surface vessel is described as follows:

[0301]

[0302] Where m and I z Let represent the mass and moment of inertia of the unmanned surface vessel, respectively. δ The factors influencing the speed and yaw moment of the unmanned surface vessel (USV) are (·). h This represents the hydrodynamic force / torque acting on the unmanned surface vessel (USV). In this example, the USV is assumed to have m = 30 kg. z =kgm 2 ,X h =0,Y h =0,Y σ =0.02,N σ =-0.01,N h =0.

[0303] Establish an STL formal model for the sequential logic task of an unmanned surface vessel (USV): The sequential logic task φ of the USV is described using STL statements as follows:

[0304]

[0305] μ unsafe = x≥-1.5 ∧ x≤1.5 ∧ y≥-1.5 ∧ y≤1.5

[0306] μ mustA =x≥-3.5∧x≤-2∧y≥2∧y≤3.5

[0307] μ mustB =x≥2∧x≤3.5∧y≥2∧y≤3.5

[0308] μ termianlC =x≥2∧x≤3.5∧y≥-3.5∧y≤-2

[0309] Where (x, y) represent the position coordinates of the unmanned surface vessel (USV). The control objective of this example is to enable the USV to reach and pass through the target area μ sequentially within 32 steps. mustA With target region μ mustB And finally reach the target area μ termianlC It avoids obstacle areas and completes sequential logic tasks with high robustness.

[0310] A two-layer framework for unmanned surface vessel (USV) control strategy is established: Based on the USV's kinematic model, its sway velocity u, yaw velocity v, and yaw rate r are planned under the STL normalized description of the sequential logic task, enabling the USV to complete the sequential logic task with high robustness; based on the USV's dynamic model, robust model predictive control is used to design the USV's propeller thrust τ. u and rudder angle τ r This enables the unmanned surface vessel (USV) to track the reference speed provided by the planning layer, forming a two-layer, multi-timescale planning-tracking control framework. This allows for intelligent control of the USV in a marine environment under sequential logic tasks. (See the two-layer, multi-timescale planning-tracking control framework...) Figure 4 .

[0311] Please see Figure 5 Speed ​​planning is implemented under the STL normalized description of a sequential logic task:

[0312] Utilizing the robust semantics of STL φ Quantify the degree to which the STL statement specification φ is satisfied, and utilize the robust semantics ρ of the STL statement specification φ. φ (x N+1 k) describes the robustness of the unmanned surface vessel (USV) path planning results, and represents the USV's state sequence x. N+1The degree of satisfaction of the STL statement φ is quantified as a real number ρ. φ , used to measure whether the position state of the unmanned surface vessel meets the STL mission requirements and the degree to which the mission requirements are met or violated; where ρ φ (x N+1 ,k)>0 indicates the state sequence x of the unmanned surface vessel. N+1 At time k, the STL statement φ, i.e., x, is satisfied. N+1 I = φ; ρ φ (x N+1 (k) < 0 indicates that the state sequence x N+1 At time k, the STL statement φ is violated, i.e. ρ φ (x N+1 The larger the value of φ, the stronger the system robustness; the STL statement φ relates to the signal sequence x. N+1 Robustness at time k is recursively defined as:

[0313]

[0314]

[0315] ρ φ∧ψ (x N+1 ,k)=min(ρ φ (x N+1 ,k),ρ ψ (x N+1 ,k))

[0316] ρ φ∧ψ (x N+1 ,k)=max(ρ φ (x N+1 ,k),ρ ψ (x N+1 ,k))

[0317]

[0318]

[0319]

[0320] ρ φ (x N+1 The absolute value of (k) is considered as the control sequence x. N+1 The distance to the set of trajectories that satisfy or violate the STL statement φ, i.e., the distance between the unmanned surface vessel's position and the designated obstacle or target area. Given a state variable x k ∈χ and the set of trajectories that satisfy or violate the STL statement φ. Then x k Closed boundary to A The minimum Euclidean distance is used Therefore, the robust semantics of an STL task are:

[0321]

[0322] Establish the objective function for the planning layer: To realize the sequential logic task φ of the unmanned surface vessel in an obstacle environment, including sub-tasks such as obstacle avoidance and arrival, with N=32 as the time domain length, ρ φ To determine the robustness of STL, a target function can be established based on the STL normalized description and robust quantization semantic processing rules for sequential logic tasks. This target function is determined by the system robustness ρ. φ (x N+1 and cost function It consists of two parts, with γ used as a compromise between them to accommodate various control objectives, resulting in the optimization problem P. ρ This can be expressed as follows:

[0323]

[0324]

[0325]

[0326]

[0327] in, Let u(k) represent the state variables of the unmanned surface vessel's kinematic model at time k; u(k) = [u(k), v(k), r(k)] T This represents the control input to the kinematic model of the unmanned surface vessel at time k; x(k+1)=f d (x(k),u(k),ω(k)) is the discretized kinematic model of the unmanned surface vessel. Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, which are usually taken as identity matrices. γ represents the compromise coefficient, and in this example, γ = 0.1.

[0328] To smooth the objective function using the distance function `dist`: Meyer wavelet extension is used to approximate the distance function `dist`, yielding an approximation. Essentially, wavelet transform derives from a single prototype wavelet through symbolic decomposition using a set of basis functions, i.e., through scaling or translation operations, thus exhibiting orthogonality. The Meyer scaling function is defined with respect to the smoothing function θ(x) as:

[0329]

[0330] The smooth function θ(x) can take any form that satisfies the following conditions:

[0331]

[0332] The expression for the Meyer wavelet function defined in the frequency domain can be obtained:

[0333]

[0334] The time-domain expressions for the Meyer scaling function and wavelets are as follows:

[0335]

[0336] in,

[0337]

[0338]

[0339] For an n-dimensional wavelet, it can be constructed using the tensor product, where ψ represents a univariate wavelet function; let E represent the unit cube [0,1]. n A set of vertices, for each vertex e = (e1, e2, ..., e...). n )∈E and x N+1 =(x1,x2,…,x n The multivariable function is defined as follows:

[0340]

[0341] Given and Then we can define I = 2 -k (j+[0,1] n A set of the form ) is The binary cube in the middle; let express The set of all binary cubes in the set is the function set. D} thus formed A normal basis function is an orthogonal basis function if ψ is an orthogonal wavelet; double basis functions The construction is the same as ψ; The wavelet extension representation of each function in the table is as follows:

[0342]

[0343] in<h,g> :=∫h(x)g(x)dx means The inner product in the equation; the approximation of the required distance function diat is obtained by selecting a finite number of terms in this extension, i.e., by utilizing a finite set. The approximate values ​​are as follows:

[0344]

[0345] This allows for a smooth approximation of the non-differentiable terms in the distance function diat, eliminating their singularity.

[0346] Approximation of the min / max terms in the objective function: By approximating the min and / or max terms in the objective function, a smooth and differentiable objective function is obtained.

[0347]

[0348]

[0349] in, This indicates that for the sequence a1,…,a… m The approximate result is taken from the maximum value. i represents the scaling factor. The larger i is, the smaller the approximation error. When i approaches ∞, the approximation error is almost 0. In this example, i = 100.

[0350] Iterative optimization using gradient descent algorithm: Given the initial position of the unmanned surface vessel X0 = [-3 -1 0] t With a time domain length N=32, the initial input sequence and initial state sequence Based on the task requirements, provide the STL specification statement φ, and based on the robust semantics of the STL statement, obtain the robust semantic expression ρ used to represent the robustness of the unmanned surface vessel system. φ After smoothing the non-differentiable terms, it can be approximated with... A compromise is made to obtain the final objective function; sequential quadratic programming (SQP) is used as the gradient optimizer to iteratively optimize the above problem, and the obtained objective function is obtained by Taylor expansion at the iteration point x. k By simplifying the problem to a quadratic function and the constraints to linear functions, we can obtain an approximate problem in the form of a quadratic programming problem. The optimal solution for this iteration is then obtained. This serves as the next search direction; through iterative iterations, the optimal sway velocity, roll velocity, and pitch velocity of the planning layer are output. Figure 6 The diagram shows the path planning results obtained by the planning layer of the unmanned surface vessel (USV) under undisturbed conditions. It can be seen that, under the premise of meeting time, obstacle avoidance, and collision avoidance requirements, the USV can successfully complete the sequential logic task. For the disturbance terms present in the model, a bounded disturbance ω is set. k ∈{ω k |||ω k || ∞ ≤ω0}, Figure 7The comparison shows the path planning results when system disturbances are present, with and without additional processing of the disturbance factors and dual reconstruction of the objective function. ω0 is set to 0.1, 0.2, and 0.4 respectively, and the angle of the disturbance term in the inertial coordinate system is set to... As can be seen, when there are system disturbances, the unmanned surface vessel path planning method based on robust semantics under the temporal logic task can effectively improve the robustness of the planning results and ensure that the temporal logic task is completed as required.

[0351] Robust model predictive control enables speed tracking:

[0352] Taking into account the disturbances and uncertainties brought about by the marine environment, such as sea breeze and complex currents, the ship achieves rapid and stable speed tracking under robust model predictive control.

[0353] Set the tracking target: The optimal speed u of the unmanned surface vessel at time k is obtained from the planning layer. * (k), v * (k), r * (k), serving as the reference velocity s for the tracking layer r (k)=[u r (k),v r (k),r r [(k)],K=0,1,…,31.

[0354] The dynamic model of the unmanned surface vessel (USV) is linearly discretized: the nominal USV system dynamic model is simplified as follows: The nominal unmanned surface vessel system dynamics model does not consider system disturbances existing at sea, wherein the state variables... To simplify the model of the unmanned surface vessel system's pitch velocity, sway velocity, and bow angular velocity, the input quantities are... To simplify the model, propeller thrust and rudder angle are represented in s... r Linearization and discretization are performed at (k) to obtain the discretized nominal hydrodynamic model of the unmanned surface vessel system:

[0355]

[0356] Where l represents the sampling time of the tracking layer. These are the state variables of the nominal unmanned surface vessel system dynamics model. The inputs are the nominal values ​​for the unmanned surface vessel system dynamics model. In this example, the rolling tracking time domain is 1 second, and the number of samples is N. d =5, sampling interval is 0.2s, the first tracked target of the tracking layer is the reference velocity s of the unmanned surface vessel system when k=1. r(1) = [0.0897, 0.0919, 0.0025], and the nominal system dynamics model after discretization and linearization is:

[0357]

[0358] Establish the objective function for the tracking layer of the nominal unmanned surface vessel system: In the tracking problem, the optimization objective is to minimize the current speed. With tracking target s r To minimize the deviation, control input, and terminal cost, the number of sampling times N is... d =5 The optimization problem P1 of the nominal unmanned surface vessel system tracking layer can be designed as follows:

[0359]

[0360]

[0361]

[0362] Where Q and R represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively. In this example, let Q = I3 and R = I2. The terminal cost function is given by input constraints τ. u (l)∈[-1,1],τ r (l)∈[-2 / π,2 / π], the parameter P>0 is the unique positive definite solution of the algebraic Riccati equation; the algebraic Riccati equation is:

[0363] P = Q + A T PA-A T PB(R+B T PB) -1 B T PA

[0364] Solving the above optimization problem using the gradient method yields the optimal state sequence of the nominal unmanned surface vessel system hydrodynamic model at time l. and optimal input sequence

[0365] Design a robust model predictive control law: Considering system disturbances at sea, the actual unmanned surface vessel system dynamics model is as follows:

[0366] s(l+1)=As(l)+Bw(l)+Cd(l)

[0367] Where, the state variables s(l) = [u(l); v(l); r(l)] represent the actual unmanned surface vessel system's pitch velocity, sway velocity, and yaw rate, and the input variables w(l) = [τ u (l); τ r[(l)] represents the propeller thrust and rudder angle of the actual unmanned surface vessel system, d(l) represents the system disturbance at time l, and C represents the disturbance term coefficient matrix.

[0368] If the current actual speed And the robust control law is taken as:

[0369]

[0370] Then s(l+1) always satisfies in, Let s(l) and w(l) represent the state variables and input variables obtained at time l when the initial state of the nominal unmanned surface vessel (USV) dynamics model is s, respectively. Let s(l) and w(l) represent the state variables and input variables of the actual USV dynamics model at time l, respectively. Describe the perturbation invariant set that satisfies in K is a state feedback control law that satisfies:

[0371] K = -(R + B) T PB) -1 B T PA

[0372] Based on the nominal unmanned surface vessel dynamics model, the optimal control sequence at time l is obtained when the actual system state is s. control sequence Corresponding optimal state variables in If the actual unmanned surface vessel dynamics model is in this state Then take the optimal control sequence The first control quantity in Reutilize robust control law As an actual control variable, when applied to the nominal unmanned surface vessel dynamics model, the state variables at the next moment satisfy... Then it will proceed to the next round of rolling. As new initial state variables, to solve for the optimal control sequence of the new nominal unmanned surface vessel dynamics model. Take again The first control quantity The control quantity at time l+1 can be obtained from the robust control law. Applying this to the nominal unmanned surface vessel (USV) dynamics model ensures that the state variables in the actual system remain within the perturbation-invariant set centered on the nominal system's state variables. This guarantees stable tracking of the reference speed issued by the planning layer in environments with uncertainties, and ensures a certain degree of robustness. The above steps are continuously optimized until the last prediction cycle. When the tracking layer receives the next optimal speed as the reference speed, the above tracking steps are repeated, ultimately achieving speed tracking for the entire time-series logic task. For simulation results of USV speed tracking under the above robust model predictive control, please refer to [link to relevant documentation]. Figure 8 As can be seen, under the presence of disturbances, the simulation results of the upper-level path planning are basically consistent with the simulation results of the lower-level trajectory tracking, which meets the requirements. The control of the unmanned surface vessel under the time-series logic task can be accomplished through this two-layer control framework based on robust semantics.

[0373] The path planning and tracking control method for unmanned surface vessels (USVs) under time-sequential logic tasks provided in the above embodiments of the present invention considers the characteristics of the USV's underactuated system and the complex marine environment. It establishes a two-layer, multi-time-scale planning-tracking optimization control framework, establishes a USV control strategy based on robust quantization semantics of signal sequential logic (STL), and considers the interference and uncertainties brought by marine environmental factors such as wind, waves, and currents. It establishes a min-max form optimization problem, and then performs smooth approximation and dual reconstruction on the optimization problem to plan the USV's speed and bow angle. Under robust model predictive control, it achieves fast and stable tracking of the planned speed, realizing optimized control of USVs under complex time-sequential logic tasks.

[0374] Those skilled in the art will understand that, in addition to implementing the system and its various devices provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices of this invention function as logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices provided by this invention can be considered as a hardware component, and the devices included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0375] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various modifications or variations within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. A path planning and tracking control method for an unmanned surface vessel under a sequential logic task, characterized in that, include: S1. Establish a formal STL model for the unmanned surface vessel's sequential logic task. S2, based on the kinematic and dynamic models of unmanned surface vessels (USVs), establishes a two-layer framework for USV control strategy under multi-timescale planning and tracking control; S3, based on the STL formal model of the temporal logic task in S1, realizes speed planning; S4, based on the unmanned surface vessel control strategy in S2 and the speed planning in S3, uses robust model predictive control to achieve speed tracking; S3, based on the STL formal model of the temporal logic task in S1, implements speed planning, including: S31, Quantification of the degree of satisfaction at the planning level: Leveraging the robust semantics of STL STL statement specification The degree to which satisfaction is achieved is quantified using STL statement specifications. Robust semantics Describing the robustness of the unmanned surface vessel (USV) path planning results involves analyzing the USV's state sequence. STL statements The degree of satisfaction is quantified into a real number. It is used to measure whether the position state of the unmanned surface vessel meets the mission requirements of STL and the degree to which the mission requirements are met or violated; S32, Establish the distance function : Will The absolute value is regarded as the control sequence. With satisfying or violating STL statements The distance of the trajectory set, i.e., the distance between the unmanned surface vessel's position and the designated obstacle or target area; given a state variable and satisfying or violating STL statements The set of trajectories ,but arrive Closed boundary The minimum Euclidean distance is used This indicates that the robust semantics of an STL task are: ； S33, Establish the objective function : Based on the kinematic model of the unmanned surface vessel, affected by disturbance factors Impact, considering the time domain Sequential logic tasks , For STL tasks The robustness of the objective function obtained for: ； ； ； ； in, Indicates the kinematic model of the unmanned surface vessel in State quantity at any given moment; Indicates the kinematic model of the unmanned surface vessel in Time-based control input; For the discretized kinematic model of the unmanned surface vessel, and These represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are usually taken as identity matrices; Indicates the compromise factor; S34, for the distance function Smooth the objective function: The distance function is approximated using Meyer wavelet extension. The distance function is obtained. Approximate value; For the distance function The non-differentiable terms in the equation are smoothed to eliminate singularities. S35, perform approximation processing on the min / max functions in the objective function: For the objective function Items and / or After approximating the terms, we obtain a smooth and differentiable objective function: ； in, Represents a sequence Take the approximate result of the maximum value. Indicates the scaling factor. The larger the value, the smaller the approximation error. Approaching At that time, the approximation error is close to 0; S36, Perform dual reconstruction of the objective function: The min-max form of the optimization problem is transformed into a minimization form by using the dual reconstruction of the internal optimization problem. The upper bound of the original internal optimization problem is minimized to obtain the optimal solution of the optimization problem. S37, Establish the final objective function and iteratively optimize it using the gradient descent algorithm: Given the initial position of the unmanned surface vessel, the initial input sequence and initial state sequence ; Provide the STL specification statement according to the task requirements. Based on the robust semantics of STL statements, a robust semantic expression for representing the robustness of an unmanned surface vessel system is obtained. The non-differentiable terms are smoothed and approximated, and the sum of the L2 norms of the state and input variables is used. A compromise is made to obtain the final objective function; Sequential quadratic programming (SQP) is used as a gradient optimizer to iteratively optimize the final objective function. Taylor expansion is then used to refine the obtained objective function at each iteration point. The problem is simplified to a quadratic function, and the constraints are simplified to linear functions, resulting in an approximate problem in the form of a quadratic programming problem. The optimal solution of this iteration is then obtained. As the next direction of search; Iterate repeatedly to output the optimal sway velocity, roll velocity, and pitch velocity of the planning layer. , , , ; In step S36, the dual reconstruction of the objective function includes: The optimization problem The internal optimization problem takes the form of: ； st ； ； ； Rewrite it in the following form: ； ； in, The physical constraints of the system are defined, relating to the maximum / minimum values ​​of the input and disturbance terms. and These are the inequality constraint matrices, Let represent a constant vector; then the Lagrange dual function of the above optimization problem can be expressed in the following form: ； in, Represents the Lagrange multiplier; will be combined with the perturbation term. The relevant items are separated and organized as follows: ； when The function reaches its maximum value when the term is 0. ; This yields the dual problem of the internal optimization problem: ； ； ； By utilizing the duality of the internal optimization problem, the optimization problem is transformed. Transform into the following form: ； ； ； This transforms a multi-parameter problem into a non-convex optimization problem.

2. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, S1 establishes a formal STL model for the unmanned surface vessel's temporal logic task, including: S11, Based on the qualitative semantics of STL, establish a mathematical model corresponding to the specified task scenario, and provide a standardized description of the temporal logic task of the unmanned surface vessel: ； （1） in, , This represents the STL statements used to describe the sequential logic tasks of an unmanned surface vessel. , Represents a time-constrained unmanned surface vessel (USV) sequential logic task; , This represents a sequential logic task for an unmanned surface vessel (USV) with time constraints, where the STL formal statement specifies the time domain of the constraints. It has been defined, in which ; express Operator, This indicates that the signal is in the time domain. Internally, it must always satisfy ; express Operator, This indicates that the signal is in the time domain. Inside, at least satisfy once; express Operator, This indicates that the signal is in the time domain. Inside, at least satisfy Once, and in accordance with Previously, it was always necessary to satisfy STL is a predicate-based language. Predicate logic, Predicate function used to represent unmanned surface vessel (USV) missions express A signal from the time domain to the real value range The mapping, predicate The value is obtained through the function Represented as: ； Where the function It is a signal Linear or nonlinear combinations; S12, applying the predicate logic and Boolean logic operators. Temporal logic operators and The temporal logic requirements of the target problem are described in a standardized manner using STL statements, and an STL formal model of the unmanned surface vessel's temporal logic task is established.

3. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, S2, based on the kinematic and dynamic models of the unmanned surface vessel (USV), establishes a two-layer framework for USV control, encompassing multi-timescale planning and tracking control, including: S21, Establish the kinematic model of the unmanned surface vessel: The real-time position coordinates of the unmanned surface vessel in the world coordinate system With heading angle As a state variable, the sway velocity of the unmanned surface vessel in the hull coordinate system sway speed With bow roll angular velocity As an input, ocean current velocity is also included. With angle As a disturbance in the marine environment, the kinematic model of the unmanned surface vessel is described as follows: ； S22, Establish the dynamic model of the unmanned surface vessel: The sway velocity of the unmanned surface vessel in the ship's coordinate system sway speed With bow roll angular velocity As a state variable, propeller thrust and rudder angle As input, the dynamic model of the unmanned surface vessel is described as follows: ； in, and Let their masses and moments of inertia be represented by , respectively. Factors influencing the speed and yaw moment of unmanned surface vessels. This indicates the hydrodynamic torque acting on the unmanned surface vessel; S23, Design a two-layer, multi-timescale planning-tracking control framework: Based on the aforementioned kinematic model of the unmanned surface vessel (USV), and considering the STL normalized description of the temporal logic task, the optimal sway velocity of the USV is planned. sway speed and bow roll rate This improves the degree to which unmanned surface vessels satisfy sequential logic tasks when disturbances are present. Based on the aforementioned unmanned surface vessel dynamics model, the optimal sway velocity obtained through planning... sway speed and bow roll rate Using robust model predictive control as a reference speed, the optimal propeller thrust is obtained by tracking the reference speed. and rudder angle This forms a two-layer, multi-timescale planning-tracking control framework, enabling motion control of unmanned surface vessels in disturbed marine environments under sequential logic tasks.

4. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, In step S31, the robust semantics of STL are utilized. STL statement specification The degree to which satisfaction is achieved is quantified using STL statement specifications. Robust semantics Describing the robustness of the unmanned surface vessel (USV) path planning results involves analyzing the USV's state sequence. STL statements The degree of satisfaction is quantified into a real number. This is used to measure whether the position state of the unmanned surface vessel meets the STL mission requirements and the degree to which the mission requirements are met or violated, including: Represents the state sequence of an unmanned surface vessel. exist Always satisfy STL statements ,Right now ; Representing a state sequence exist Violating STL statements at all times ,Right now ; The size of the system is directly proportional to its robustness. STL statements Regarding signal sequences exist The robustness at time step is recursively defined as: ； ； ； ； ； ； 。 5. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 4, characterized in that, In step S34, the distance function is approximated using Meyer wavelet extension to obtain the distance function. Approximate values ​​include: Let the Meyer scaling function be about the smoothness function. The definition of is: ； Wherein, smooth function Take any form that satisfies: ； The expression for the Meyer wavelet function defined in the frequency domain is obtained as follows: ； The time-domain expressions for the Meyer scaling function and wavelet are as follows: ； in, ； ； For n-dimensional wavelets, they can be constructed using tensor products, and then... Let a wavelet function be a single variable; Represents the unit cube A set of vertices, for each vertex and Define a multivariable function as: ； Given ,and Then define The set of forms is The binary cube in the middle; make express The set of all binary cubes in the set is the function set. Formed A normal basis, if If it is an orthogonal wavelet, then it is an orthogonal basis; double basis functions The structure and same; The wavelet extension of each function in the expression is represented as: ； in express Inner product; Required distance function The approximation is obtained by selecting a finite number of terms in the extension, that is, by using a finite set. The approximate value is: 。 6. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, S4, robust model predictive control to achieve speed tracking, includes: S41, perform linear discretization on the unmanned surface vessel dynamics model: The dynamic model of the unmanned surface vessel is simplified as follows: This causes the nominal unmanned surface vessel system dynamics model to disregard system disturbances present at sea, where the state variables... To simplify the model of the unmanned surface vessel system's pitch velocity, sway velocity, and bow angular velocity, the input quantities are... To simplify the model, propeller thrust and rudder angle are calculated; the planning layer obtains the unmanned surface vessel's... Optimal speed at any given time , , As a reference speed for the tracking layer The dynamic model of the unmanned surface vessel system is in Linearization and discretization are performed at the point to obtain the discretized nominal unmanned surface vessel system dynamic model as follows: ； in, To track the sampling time of the layer, These are the state variables of the nominal unmanned surface vessel system dynamics model. The inputs are the nominal values ​​for the unmanned surface vessel system dynamics model. , ; S42, Based on the discretized unmanned surface vessel (USV) dynamics model, establish the objective function for the nominal USV system tracking layer: by To predict the time domain, the objective function for the nominal unmanned surface vessel system to achieve speed tracking is... for: ； ； ； in, and These represent the state variable weight coefficient matrix and the input variable weight coefficient matrix, respectively, and are taken as identity matrices; and Indicates the upper and lower limits of the input quantities for the nominal unmanned surface vessel dynamics model; This is the unique positive definite solution to the algebraic Riccati equation; the algebraic Riccati equation is: ； S43, Solve for the objective function: The objective function is solved using the gradient method. ,have to The optimal state sequence of the nominal unmanned surface vessel system dynamics model at time points and optimal input sequence ; S44, Design a robust model predictive control law: Considering system disturbances at sea, the actual dynamic model of the unmanned surface vessel system is as follows: ； Among them, state variables The input quantities represent the actual pitch velocity, sway velocity, and bow angular velocity of the unmanned surface vessel system. This represents the propeller thrust and rudder angle of the actual unmanned surface vessel system. express System disturbances at any given time Represents the coefficient matrix of the disturbance term; If the current actual speed And the robust control law is taken as: ； but Always satisfied ,in, , These represent the initial states of the nominal unmanned surface vessel dynamics model as follows: At that time, The state variables and input variables obtained at each step, , These represent the actual dynamic model of the unmanned surface vessel in... The state and input quantities at each moment. Describe the perturbation invariant set that satisfies ,in , For a state feedback control law, the following conditions must be met: ； S45, based on the objective function and the robust model predictive control law, speed tracking is achieved using a tracking layer, including: Based on the nominal unmanned surface vessel dynamics model, when the actual state of the system is At that time, seek Optimal control sequence at time 1 control sequence Corresponding optimal state variables ,in If the actual unmanned surface vessel dynamics model is in this state Then the optimal control sequence is selected. The first control quantity in ; Using robust control laws As an actual control variable, when applied to the nominal unmanned surface vessel dynamics model, the state variables at the next moment satisfy... ; Entering the next round of rolling, As new initial state variables, solve for the optimal control sequence of the new nominal unmanned surface vessel dynamics model. ; Pick The first control quantity From the robust control law, we can obtain Control of time ; Applying this to the nominal unmanned surface vessel dynamics model ensures that the state variables in the actual system are within the perturbation invariant set centered on the state variables of the nominal system, thus ensuring stable tracking of the reference speed issued by the planning layer by the unmanned surface vessel in an environment with uncertainties, and ensuring robustness. The rolling optimization is repeated until the last prediction cycle; when the tracking layer receives the next optimal speed as the reference speed, the speed tracking process is repeated again, and finally the speed tracking of the entire time-series logic task is realized.

7. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, In S3, the control tasks in the obstacle environment include: obstacle avoidance and / or arrival tasks of the unmanned surface vessel.

8. The path planning and tracking control method for an unmanned surface vessel under a sequential logic task according to claim 1, characterized in that, In S4, Find the minimum perturbation invariant set: ; It refers to time i The value, , For state feedback control law, D refers to the set of disturbance ranges, and Z is the minimum disturbance invariant set. Its function is to control system constraints, ensure that the actual system state variables are located in a subset centered on the nominal system state variables, and ensure the robustness of the system.

9. A path planning and tracking control system for an unmanned surface vessel under a sequential logic task, used to implement the method described in claim 1, characterized in that, include: The STL module establishes a formalized STL model for the timing logic task of the unmanned surface vessel. The dual-layer strategy module establishes a dual-layer framework for unmanned surface vessel (USV) control strategy based on the kinematic and dynamic models of USVs, encompassing multi-timescale planning and tracking control. The speed planning module implements speed planning based on the STL formal model of the aforementioned time-series logic task. The tracking control module, based on the unmanned surface vessel control strategy and the speed planning, uses robust model predictive control to achieve speed tracking.

Images