An asv-auv hybrid cluster robust model predictive cooperative control method and system

Abstract

The application discloses an ASV-AUV hybrid cluster robust model predictive cooperative control method and system, which comprises the following steps: step 1, acquiring position state information and speed state information of any autonomous system in the hybrid cluster, wherein the autonomous system is divided into ASV and AUV; step 2, setting an expected position vector corresponding to the autonomous system i according to a cooperative task, combining the position state information and the speed state information, and calculating a virtual control speed of the autonomous system at a kinematics layer; step 3, estimating disturbance at a dynamics layer by using a disturbance observer according to the virtual control speed, and calculating a control input of the autonomous system by using a robust model predictive controller; and step 4, calculating an actual execution control amount of a propeller system by using a propeller model at an execution layer according to the control input. The application can solve the cooperative control problem of the ASV-AUV hybrid cluster system with uncertainty, and improve the robustness and stability of system control.

Description

Technical Field

[0001] This invention relates to the field of cross-domain collaborative technology for autonomous systems, and in particular to a robust model predictive collaborative control method and system for hybrid ASV (Autonomous Surface Vehicle)-AUV (Autonomous Underwater Vehicle) clusters. Background Technology

[0002] With the rapid development of intelligent technologies and autonomous systems, the concept of cross-domain collaborative technology has emerged and gradually become a high ground in global autonomous system technology competition. Cross-domain collaborative technology for autonomous systems mainly refers to the collaborative control technology between different types of autonomous systems, such as unmanned aerial vehicles (UAVs), autonomous ground vehicles (AUVs), autonomous surface vessels (AUVs), and autonomous underwater vehicles (AUVs), across different spatial domains such as air, land, and sea. In the field of marine engineering, the ASV-AUV hybrid swarm system, composed of ASVs and AUVs, is an application of cross-domain collaborative technology. Through the collaboration of the upper and lower spatial domains, the ASV-AUV hybrid swarm system can be applied to more complex marine operation scenarios, including emergency maritime rescue, marine topography and resource exploration, etc. Collaborative control is the key technology for achieving cross-domain collaboration, forming a stable collaborative operation mode by combining autonomous systems from two spatial domains with a pre-set specific structure.

[0003] Currently, the main approaches to solving the cooperative control problem include the leader-follower method, the virtual structure method, the artificial potential field method, the behavioral method, and the graph theory method. Based on these methodological ideas, scholars at home and abroad have proposed some control methods for the cooperative control of ASVs and AUVs, but they mainly consider single-species clusters (ASV clusters or AUV clusters), and there is relatively little research on ASV-AUV hybrid clusters. In practical applications, due to the uncertainties of disturbances such as wind, waves, and ocean currents in the marine operating environment, as well as the nonlinear dynamic characteristics and unmodeled uncertainties of the autonomous system itself, existing technologies are difficult to guarantee the robustness and stability of cooperative control of ASV-AUV hybrid clusters.

[0004] Patent document CN117850417A discloses a method and system for formation control of unmanned surface vessels (USVs) and autonomous underwater vehicles (AUVs), including: Step S1: Distributing predetermined formation tasks to USVs and AUVs respectively, deploying USVs and AUVs in the water for navigation, and calculating their relative positions; Step S2: Designing the AUV's reference heading angle, reference speed, and longitudinal control force, and designing the yaw control torque; Step S3: Calculating error variables and environmental disturbance estimation error variables; Step S4: Controlling the AUV's thrusters and control surfaces to generate the calculated control force and torque, thereby achieving formation navigation of the USVs and AUVs. The nonlinear disturbance observer provided by the invention can be used to estimate the impact of unknown environmental forces and modeling uncertainties on the vehicles. This invention only considers the cooperative formation control of a single USV and AUV, while actual hybrid swarm systems usually consist of multiple USVs and AUVs, therefore this invention is difficult to apply to the cooperative control of large-scale swarms.

[0005] Patent document CN115268476B discloses a distributed cooperative control system and method for surface ships and underwater vehicles. It employs a modular design, where an AUV (Aerial Vehicle) obtains its own and nearby AUVs' dead reckoning status information and formation commands through positioning and communication modules. A cooperative controller generates the dead reckoning and velocity status for the next moment. Point-to-point tracking control is performed using an active disturbance rejection (ADRROC) point-to-point tracking controller and actuators. The ADRROC point-to-point tracking control has a high control frequency, forming an inner loop control, while the cooperative control, due to communication limitations, has a low control frequency, forming an outer loop control. This invention considers a linear system dynamics equation, while actual AUVs or AUVs are typically nonlinear systems. Therefore, this invention is difficult to apply to hybrid swarm systems with nonlinear dynamic characteristics.

[0006] Patent document CN114942646B discloses a three-dimensional spatial formation control method for heterogeneous unmanned systems, including the following steps: establishing a three-dimensional formation communication topology model for the heterogeneous unmanned systems; executing a heading consistency control algorithm; executing a speed consistency control algorithm; if the unmanned system node is an autonomous underwater vehicle (AUV), then executing a depth consistency control algorithm; controlling the surface unmanned surface vessel (SUV) acting as an AUV node to operate according to the output heading angle and output speed; controlling the AUV acting as an AUV node to operate according to the output heading angle, output speed, and output depth. This invention does not consider the dynamic models of the SUV and AUV, and does not account for model uncertainties; therefore, the robustness and stability of this invention in practical applications are difficult to guarantee. Summary of the Invention

[0007] The purpose of this invention is to provide a robust model predictive cooperative control method and system for ASV-AUV hybrid clusters, which aims to solve the cooperative control problem of ASV-AUV hybrid cluster systems with uncertainties and improve the robustness and stability of system control.

[0008] To achieve the above objectives, this invention provides a robust predictive cooperative control method for an ASV-AUV hybrid cluster, characterized in that it comprises:

[0009] Step 1: Obtain the location and state information η of any autonomous system i in the hybrid cluster. i and velocity state information ξ i Autonomous systems are divided into ASVs and AUVs;

[0010] Step 2: Based on the collaborative task, set the desired position vector η corresponding to autonomous system i. id Combined with location status information η i and velocity state information ξ i The virtual control velocity ζ of autonomous system i is calculated at the kinematic level. ic ;

[0011] Step 3, based on the virtual control speed ζ ic At the dynamic layer, an interference observer is used to estimate the interference d. i The control input τ of the autonomous system i is calculated using a robust model predictive controller. i ;

[0012] Step 4, based on the control input τ i At the execution layer, the actual execution control quantity λ of the propulsion system is calculated using a thruster model. i .

[0013] Furthermore, in step 3, the optimization problem of the robust model predictive controller is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1); or the optimization problem of the robust model predictive controller is set as Equation (16-2), and the constraint condition corresponding to Equation (16-2) is described as Equation (17-2).

[0014]

[0015] Among them, J i Let τ be the objective function to be optimized. i For τ i (t) represents the control input of the autonomous system i at time t; N p For prediction in the time domain; ζ i For ζ i (t) represents the actual velocity of autonomous system i at time t; ζ i(s|t) represents the velocity of autonomous system i at time t, predicting time step s; ζ ic (s|t) represents the virtual control velocity of autonomous system i at time t, predicting time step s; Q iζ N is the weight matrix for the velocity tracking error; c To control the time domain; R i2 The weight matrix for controlling the input; τ i (s|t) represents the control input of the autonomous system i at time t, predicting time step s; ζ i (s+1|t) represents the velocity of autonomous system i at time t, predicting time step s+1; g(ζ) i (s|t),τ i (s|t)) is the nominal part of the dynamic model of autonomous system i at time t predicting time step s; The disturbance estimated by the autonomous system i at time t for time step s; The set of constraints for the actual velocity state; τ i (s|t) represents the control input of the autonomous system i at time t to predict time step s; The set of constraints for controlling the input; τ i (s+1|t) represents the control input of the autonomous system at time t to predict time step s+1; The set of constraints controlling the increment; ζ i (N p |t) represents the prediction time step N of the autonomous system i at time t. p Speed; Ψ i H(ζ) is the set of terminal constraints for velocity states. i (s|t),τ i (s|t) is the function corresponding to the contraction constraint.

[0016] Furthermore, the contraction constraint is designed based on the Lyapunov method and auxiliary control quantity, using the following inequality constraint (18):

[0017]

[0018] Among them, V i The Lyapunov function, which is related to velocity, is set as equation (19);

[0019]

[0020] Among them, e i =ζ i -ζ ic ζ represents the velocity tracking error of autonomous system i at time t. ic For ζ ic (t), where i is the virtual control speed of the autonomous system at time t;

[0021] for The nonlinear auxiliary control quantity designed based on the Lyapunov method is represented by equation (20);

[0022]

[0023] in, These represent the nominal portions of the inertia matrix, the Coriolis force and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. The interference estimated by the interference observer, For the virtual control acceleration of autonomous system i, K iζ This is the gain matrix.

[0024] Furthermore, in step 2, the virtual control speed ζ ic Calculated by the Tube-MPC controller described in equation (5), The optimized speed control quantity is obtained by solving the optimization problem of the Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as Equation (6). For the feedback speed control variable of autonomous system i:

[0025]

[0026]

[0027] in, Let R be the nominal position of autonomous system i at time t, predicted at time step s+1. i (s|t) is the rotation matrix of the autonomous system i at time t, predicting time step s; To predict the nominal velocity of autonomous system i at time t for time step s, Let L be the nominal position of autonomous system i at time t, predicted at time step s. i (s|t) represents the stage cost of autonomous system i predicting time step s at time t. η id (s|t) represents the desired position of the autonomous system i at time t and time step s, and Q is the position of the system i at time t. i1 R is the weight matrix for the position tracking error. i1 N is the weight matrix for the speed control quantity. i (N p |t) represents the prediction time step N of the autonomous system i at time t. p terminal cost, For autonomous system i, predict the time step N at time t pThe nominal position, η id (N p |t) represents the autonomous system i at time t and time step N. p The expected position, Q i2 This is the weight matrix for the terminal position tracking error; Let i be the actual position of the autonomous system i at time t. Let be the nominal position of autonomous system i at time t. Let be the nominal velocity of autonomous system i at time t, and T be the sampling time. The set of constraints for the actual position state. This represents poor performance in Pontryagin. For the Tube-invariant set of the actual kinematic model, Υ i For the feedback gain matrix, Λ i Let η be the set of terminal constraints for the position state. i (s|t), η j (s|t) represent the actual positions of autonomous system i and autonomous system j at time t, predicted time step s, respectively, and η ijs To limit the safe distance between ASVs, η ijU To limit the safe distance between AUVs, if i∈S, then autonomous system i is an ASV; if i∈U, then autonomous system i is an AUV.

[0028] Furthermore, when i∈U, the depth of the AUV is subject to the constraint described in equation (7):

[0029] z i ∈Z lim (7)

[0030] Among them, Z lim Let be the set of depth constraints for autonomous system i.

[0031] The present invention also provides a robust model predictive cooperative control system for ASV-AUV hybrid cluster, which includes a hybrid cluster, surface and underwater communication equipment, state information sensors, kinematic control unit, disturbance observer unit, dynamic control unit, thruster unit, and actuation and drive mechanism;

[0032] Among them, the surface and underwater communication equipment is used for communication between ASVs and AUVs in the hybrid cluster;

[0033] The state information sensor is used to acquire the position state information η of the autonomous system i. i and velocity state information ξ i ;

[0034] The kinematic control unit is used to set the desired position vector η corresponding to the autonomous system i based on the cooperative task. idCombined with location status information η i and velocity state information ξ i The virtual control velocity ζ of autonomous system i is calculated at the kinematic level. ic ;

[0035] The disturbance observer unit is used to estimate the disturbances experienced by the ASV and AUV;

[0036] The dynamic control unit is used to control the virtual speed ζ. ic At the dynamics layer, the control input τ of the autonomous system i is calculated using a robust model predictive controller. i ;

[0037] The thruster unit is used to construct the thruster model, and is used to determine the thruster based on the control input τ. i At the execution layer, the actual execution control quantity λ of the propulsion system is calculated using a thruster model. i It also outputs instructions to the specific execution and driving mechanisms;

[0038] The execution and drive mechanism drives the ASV and AUV to move according to the received instructions.

[0039] Furthermore, the optimization problem of the robust model predictive controller in the dynamic control unit is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1); or, the optimization problem of the robust model predictive controller is set as Equation (16-2), and the constraint condition corresponding to Equation (16-2) is described as Equation (17-2).

[0040]

[0041] Among them, J i Let τ be the objective function to be optimized. i For τ i (t) represents the control input of the autonomous system i at time t; N p For prediction in the time domain; ζ i For ζ i (t) represents the actual velocity of autonomous system i at time t; ζ i (s|t) represents the velocity of autonomous system i at time t, predicting time step s; ζ ic (s|t) represents the virtual control velocity of autonomous system i at time t, predicting time step s; Q iζ N is the weight matrix for the velocity tracking error; c To control the time domain; R i2 The weight matrix for controlling the input; τ i (s|t) represents the control input of the autonomous system i at time t, predicting time step s; ζ i (s+1|t) represents the velocity of autonomous system i at time t, predicting time step s+1; g(ζ)i (s|t),τ i (s|t)) is the nominal part of the dynamic model of autonomous system i at time t predicting time step s; The disturbance estimated by the autonomous system i at time t for time step s; The set of constraints for the actual velocity state; τ i (s|t) represents the control input of the autonomous system i at time t to predict time step s; The set of constraints for controlling the input; τ i (s+1|t) represents the control input of the autonomous system at time t to predict time step s+1; The set of constraints controlling the increment; ζ i (N p |t) represents the prediction time step N of the autonomous system i at time t. p Speed; Ψ i H(ζ) is the set of terminal constraints for velocity states. i (s|t),τ i (s|t) is the function corresponding to the contraction constraint.

[0042] Furthermore, the contraction constraint can be designed based on the Lyapunov method and auxiliary control quantity as follows: inequality constraint (18):

[0043]

[0044] Among them, V i The Lyapunov function, which is related to velocity, is set as equation (19);

[0045]

[0046] Among them, e i =ζ i -ζ ic ζ represents the velocity tracking error of autonomous system i at time t. ic For ζ ic (t), where i is the virtual control speed of the autonomous system at time t;

[0047] For the nonlinear auxiliary control quantity designed based on the Lyapunov method, it is given by equation (20);

[0048]

[0049] in, These represent the nominal portions of the inertia matrix, the Coriolis force and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. The interference estimated by the interference observer, For the virtual control acceleration of autonomous system i, Kiξ This is the gain matrix.

[0050] Furthermore, in the kinematic control unit, the virtual control speed K ic Calculated by the Tube-MPC controller described in equation (5), The optimized speed control quantity is obtained by solving the optimization problem of the Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as Equation (6). For the feedback speed control variable of autonomous system i:

[0051]

[0052] in, Let R be the nominal position of autonomous system i at time t, predicted at time step s+1. i (s|t) is the rotation matrix of the autonomous system i at time t, predicting time step s; To predict the nominal velocity of autonomous system i at time t for time step s, Let L be the nominal position of autonomous system i at time t, predicted at time step s. i (s|t) represents the stage cost of autonomous system i predicting time step s at time t. η id (s|t) represents the desired position of the autonomous system i at time t and time step s, and Q is the position of the system i at time t. i1 R is the weight matrix for the position tracking error. i1 N is the weight matrix for the speed control quantity. i (N p |t) represents the prediction time step N of the autonomous system i at time t. p terminal cost, For autonomous system i, predict the time step N at time t p The nominal position, η id (N p |t) represents the autonomous system i at time t and time step N. p The expected position, Q i2 This is the weight matrix for the terminal position tracking error; Let i be the actual position of the autonomous system i at time t. Let be the nominal position of autonomous system i at time t. Let be the nominal velocity of autonomous system i at time t, and T be the sampling time. The set of constraints for the actual position state. This represents poor performance in Pontryagin. For the Tube-invariant set of the actual kinematic model, Υi For the feedback gain matrix, Λ i Let η be the set of terminal constraints for the position state. i (s|t), η j (s|t) represent the actual positions of autonomous system i and autonomous system j at time t, predicted time step s, respectively, and η ijs To limit the safe distance between ASVs, η ijU To limit the safe distance between AUVs, if i∈S, then autonomous system i is an ASV; if i∈U, then autonomous system i is an AUV.

[0053] Furthermore, when i∈U, the depth of the AUV is subject to the constraint described in equation (7):

[0054] z i ∈Z lim (7)

[0055] Among them, Z lim Let be the set of depth constraints for autonomous system i.

[0056] This invention employs a cascaded control structure, with the kinematic layer based on the desired cooperative objective, namely the desired position vector η. id The dynamics layer is responsible for calculating the virtual control velocity. Based on the virtual control velocity, the dynamics layer calculates the dynamic control input, i.e., the control input τ. i This can improve control accuracy and enhance system robustness.

[0057] This invention constructs an actual kinematic model considering velocity interference factors such as wind waves and ocean currents, and regards the ideal kinematic model without environmental velocity interference as the nominal kinematic model. The virtual control velocity is calculated through the Tube-MPC controller, which effectively improves the robustness of the hybrid cluster system at the kinematic control level.

[0058] This invention combines a disturbance observer with a robust model predictive control (MMC) method. The disturbance observer makes the dynamic model predictions more accurate, while the robust MMC method ensures the closed-loop stability of the control system. The combination of these two methods significantly improves the performance and stability of the control system when facing uncertainties and disturbances. The invention ultimately outputs the executed control quantity λ of the system's thruster. i This better meets the needs of actual engineering applications. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of a hybrid cluster of 5 ASVs and 5 AUVs in an embodiment of the present invention.

[0060] Figure 2 This is a flowchart of the ASV-AUV hybrid cluster robust model predictive collaborative control method according to an embodiment of the present invention.

[0061] Figure 3 This is a schematic diagram of the ASV-AUV hybrid cluster robust model prediction collaborative control framework according to an embodiment of the present invention.

[0062] Figure 4 This is a schematic diagram of the ASV-AUV hybrid cluster robust model predictive collaborative control system. Detailed Implementation

[0063] In the accompanying drawings, the same or similar reference numerals are used to denote the same or similar elements or elements having the same or similar functions. The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0064] In the description of this invention, the terms "center," "longitudinal," "lateral," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting the scope of protection of this invention.

[0065] The hybrid cluster system provided in this embodiment of the invention consists of multiple autonomous systems. Specifically, the autonomous systems are divided into K ASVs and N AUVs. The set of numbers for all autonomous systems is O, O = {1, 2, ..., K + N}. The numbers of the ASVs are defined as a set S, S = {1, 2, ..., K}, where K is the total number of ASVs. The numbers of the AUVs are defined as a set U, U = {K + 1, K + 2, ..., K + N}, where N is the total number of AUVs.

[0066] like Figure 2 As shown in the embodiments of the present invention, the main steps of the robust model prediction and cooperative control method for ASV-AUV hybrid clusters are as follows:

[0067] Step 1: For any autonomous system i∈O in the hybrid cluster, obtain the position and state information η of autonomous system i. i and velocity state information ξ i .

[0068] Specifically, if i∈S, then the autonomous system i is an ASV, and its position state is η. i =[x i ,y i ,ψ i ] T x i y i and ψ i These represent the x-axis position, y-axis position, and yaw angle of autonomous system i in the geodetic coordinate system, respectively, with velocity state ζ.i =[u i ,v i ,r i ] T u i v i and r i These represent the forward velocity, lateral velocity, and yaw rate in the carrier coordinate system, respectively.

[0069] If i∈U, then the autonomous system i is an AUV, and its position state is x i y i z i , θ i and ψ i These represent the x-axis position, y-axis position, z-axis position, roll angle, pitch angle, and yaw angle of autonomous system i in the geodetic coordinate system, respectively, with velocity state ζ. i =[u i ,v i ,w i ,p i ,q i ,r i ] T u i v i w i p i q i and r i These represent the forward velocity, lateral velocity, vertical velocity, roll rate, pitch rate, and yaw rate in the carrier coordinate system, respectively.

[0070] Taking 5 ASVs + 5 AUVs as an example, such as Figure 1 The diagram shown is a schematic of the ASV-AUV hybrid cluster of the present invention.

[0071] Step 2: Based on the collaborative task, set the desired position vector η corresponding to autonomous system i. id Combined with location status information η i and velocity state information ξ i The virtual control velocity ζ of autonomous system i is calculated at the kinematic level. ic .

[0072] In one embodiment, the actual kinematic model and the nominal kinematic model of the autonomous system i are established at the kinematic layer, i.e., the Tube-MPC controller, to calculate the virtual control velocity ζ of the autonomous system i. ic .

[0073] For example, the actual kinematic model of autonomous system i can be set as Equation (1):

[0074] ηi (t+1)=η i (t)+R i (t)Tζ i (t)+ξ i (t) (1)

[0075] in, Let i be the actual position of the autonomous system i at time t+1. Let i be the actual position of the autonomous system i at time t. Let R be the set of constraints for the actual position state, T be the sampling time, and R be the value of the constraint. i (t) is the rotation matrix of the autonomous system i at time t. Let i be the actual velocity of the autonomous system i at time t. The set of constraints for the actual velocity state. The environmental velocity disturbance term is caused by factors such as wind, waves, and ocean currents. This is the set corresponding to the interference items.

[0076] The nominal kinematic model can be set as Equation (2):

[0077]

[0078] in, Let i be the nominal position of the autonomous system i at time t+1. Let be the nominal position of autonomous system i at time t. Let be the nominal velocity of autonomous system i at time t.

[0079] The nominal system and the actual system satisfy the relationship described by equation (3):

[0080]

[0081] in, Let be the Tube invariant set of the actual kinematic model, and ⊕ represent Minkowski and .

[0082] To meet the requirements of the actual system, the Tube-MPC controller will provide feedback speed control parameters. Set as equation (4):

[0083]

[0084] Among them, Υ i The feedback gain matrix can be obtained from the Riccati equation, η i and η i (t), η i (t) represents the actual position of autonomous system i at time t. The nominal position of autonomous system i at time t is shown.

[0085] For the nominal kinematic model part, the Tube-MPC controller solves the optimization problem described by equation (5) to optimize the velocity control quantity. The constraints for the optimization problem are set as Equation (6).

[0086]

[0087] in,

[0088]

[0089] Where s represents the prediction time step, and N p For prediction in the time domain, N c To control the time domain, For autonomous system i to predict the nominal velocity of time step s at time t, L i (s|t) represents the stage cost of autonomous system i predicting time step s at time t. Let η be the nominal position of autonomous system i at time t, predicted at time step s. id (s|t) represents the desired position of the autonomous system i at time t and time step s, and Q is the position of the system i at time t. i1 Q is the weight matrix for the position tracking error. i2 R is the weight matrix for the terminal position tracking error. i1 N is the weight matrix for the speed control quantity. i (N p |t) represents the prediction time step N of the autonomous system i at time t. p terminal cost, For autonomous system i, predict the time step N at time t p The nominal position, η id (N p |t) represents the autonomous system i at time t and time step N. p The desired location.

[0090]

[0091] in, Let R be the nominal position of autonomous system i at time t, predicted at time step s+1. i (s|t) is the rotation matrix of the autonomous system i at time t, predicting time step s; Let be the nominal velocity of autonomous system i at time t. This represents poor performance in Pontryagin. For the Tube-invariant set of the actual kinematic model, Υ iFor the feedback gain matrix, Λ i Let η be the set of terminal constraints for the position state. i (s|t), η j (s|t) represent the actual positions of autonomous system i and autonomous system j at time t, predicted time step s, respectively, and η ijs To limit the safe distance between ASVs, η ijU To limit the safe distance between AUVs.

[0092] In particular, if i∈U, then the autonomous system i is an AUV. To avoid conflicts with the operation of ASV, the depth of the AUV is subject to the constraint described in equation (7) below:

[0093] z i ∈Z lim (7)

[0094] Among them, Z lim Let be the set of depth constraints for autonomous system i.

[0095] The final virtual speed control quantity ζ corresponding to the Tube-MPC controller ic Represented as:

[0096] Step 3, based on the virtual control speed ζ ic At the dynamic layer, an interference observer is used to estimate the interference d. i The control input τ of the autonomous system i is calculated using a robust model predictive controller. i .

[0097] In one embodiment, the dynamic model corresponding to autonomous system i is set as Equation (8):

[0098] ζ i (t+1)=ζ i (t)+g(ζ i (t),τ i (t))+d i (t) (8)

[0099] Wherein, g(ζ) i (t),τ i (t) represents the nominal part of the dynamic model of autonomous system i, τ i (t) represents the control input to the autonomous system i, including control force and control torque, d i (t) represents disturbances caused by wind, ocean currents, waves, and modeling uncertainties.

[0100] Taking a specific AUV dynamics model as an example, as shown in equation (9):

[0101]

[0102] in, M represents acceleration. i Represents the inertia matrix, C i (ζ i ) represents the matrix of Coriolis force and centrifugal force, D i (ζ i ) represents the fluid dynamics damping matrix, τ i Represents the control input, τ id This represents unknown external disturbances that change over time.

[0103] Decompose the above matrix, ΔM represents the nominal parts of the inertia matrix, Coriolis force and centrifugal force matrices, and fluid dynamics damping matrix, respectively. i ΔC i (ζ i ), ΔD i (ζ i ) represent the uncertainties of the inertial matrix, the Coriolis force and centrifugal force matrix, and the fluid dynamics damping matrix, respectively.

[0104] The original dynamic model can then be described by equation (10):

[0105]

[0106] In another embodiment, the original dynamic model can also be described as equation (11):

[0107]

[0108] Wherein, g(ζ) i ,τ i ) is g(ζ i (t),τ i (t)), representing the nominal part of the dynamic model, is expressed as equation (12), and the disturbance d i Represented as equation (13):

[0109]

[0110] Interference d i The interference can be estimated by the interference observer, and the estimated interference is denoted as .

[0111] There are many types of disturbance observers. Taking the Extended State Observer (ESO) as an example, the velocity estimate... Estimated value of interference These are expressed as equations (14) and (15), respectively:

[0112]

[0113] Where, γ i1 and γ i2 All of these are the gain matrices of the observer.

[0114] In one embodiment, the optimization problem of the robust model predictive controller is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1):

[0115]

[0116] Where, N p For prediction in the time domain, N c To control the time domain, Q iζ R is the weight matrix for the velocity tracking error. i2 The weight matrix is ​​used to control the input quantities.

[0117]

[0118] In another embodiment, the optimization problem of the robust model predictive controller is set as equation (16-2), and the constraint condition corresponding to equation (16-2) is described as equation (17-2):

[0119]

[0120] Among them, J i Let τ be the objective function to be optimized. i For τ i (t) represents the control input of the autonomous system i at time t; N p For prediction in the time domain; ζ i For ζ i (t) represents the actual velocity of autonomous system i at time t; ζ i (s|t) represents the velocity of autonomous system i at time t, predicting time step s; ζ ic (s|t) represents the virtual control velocity of autonomous system i at time t, predicting time step s; Q iζ N is the weight matrix for the velocity tracking error; c To control the time domain; R i2 The weight matrix for controlling the input; τ i (s|t) represents the control input of the autonomous system i at time t, predicting time step s; ζ i (s+1|t) represents the velocity of autonomous system i at time t, predicting time step s+1; g(ζ) i (s|t),τ i (s|t)) is the nominal part of the dynamic model of autonomous system i at time t predicting time step s; The disturbance estimated by the autonomous system i at time t for time step s; The set of constraints for the actual velocity state; τ i (s|t) represents the control input of the autonomous system i at time t to predict time step s; The set of constraints for controlling the input; τ i (s+1|t) represents the control input of the autonomous system at time t to predict time step s+1; The set of constraints controlling the increment; ζ i (N p |t) represents the prediction time step N of the autonomous system i at time t. p Speed; Ψ i H(ζ) is the set of terminal constraints for velocity states. i (s|t),τ i (s|t) is the function corresponding to the contraction constraint, which is used to ensure the closed-loop stability of the system.

[0121] Preferably, an alternative approach to the contraction constraint is to design the following inequality constraint (18) based on the Lyapunov method and auxiliary control variables:

[0122]

[0123] Among them, V i This represents the Lyapunov function related to velocity. It is a nonlinear auxiliary control quantity designed based on the Lyapunov method.

[0124] Specifically, the nonlinear auxiliary control quantity is designed based on Lyapunov functions, and its stability can be proven using the Lyapunov method. Taking nonlinear backstepping control as an example, according to the AUV dynamic model described in equation (11) above, let e i =ζ i -ζ ic The velocity tracking error is represented by the Lyapunov function (19) defined as follows:

[0125]

[0126] Differentiating it gives

[0127]

[0128] Therefore, a nonlinear backstepping control described by the following equation (20) can be designed:

[0129]

[0130] Among them, K iζ This is the gain matrix.

[0131] Based on the above optimization problem (16) and constraints (17), the control input τ of the autonomous system i can be obtained by solving the problem. i .

[0132] Step 4: Determine the execution control quantity corresponding to autonomous system i using the thruster model described by equation (21).

[0133] τ i =K i h(λ i ) (twenty one)

[0134] Among them, K i To advance the system conversion coefficient, λ i To execute the control variable, h(λ) i ) is the function related to the execution control quantity.

[0135] The motion control of an autonomous system ultimately relies on a specific propulsion system. Therefore, this invention determines the corresponding execution control quantities based on a thruster model.

[0136] Specifically, taking underactuated ASVs and AUVs as examples, the system's propulsion mainly consists of a propeller and a servo motor, controlled by the input τ. i The components are used to obtain the corresponding propeller thrust and servo torque. For ASV, the propeller control input is the rotational speed n. i The servo motor executes the control variable as the horizontal rudder angle δ. si .

[0137] The propeller thrust model is given by equation (22):

[0138]

[0139] in, For propeller thrust, Used to control input τ i forward velocity u of the system i Component of direction, K iT This is the thrust coefficient.

[0140] The horizontal torque model of the servo motor is given by equation (23):

[0141]

[0142] in, The horizontal torque of the servo motor, Used to control input τ i Horizontal component, For the dimensional horizontal rudder angle coefficient, u i Let be the forward velocity of autonomous system i.

[0143] For an AUV, the propeller's control input is the rotational speed n.i The servo motor executes the control variable as the horizontal rudder angle δ. si and vertical rudder angle δ ri .

[0144] The propeller thrust model and the servo motor horizontal torque model are similar to those of the ASV. The servo motor vertical torque model is given by equation (24):

[0145]

[0146] in, The vertical torque of the servo motor. Used to control input τ i The vertical component, is the dimensionless vertical rudder angle coefficient.

[0147] Based on the above propeller model, the system's control input can be determined: propeller speed n. i Horizontal rudder angle δ si and vertical rudder angle δ ri .

[0148] Other fully driven or overdriven ASV and AUV systems can be modeled using a similar method to determine the corresponding actuation control variables.

[0149] To achieve the above methods, this patent also provides an ASV-AUV hybrid cluster robust model predictive cooperative control system, such as... Figure 3 and 4 As shown, it includes an ASV / AUV entity, surface and underwater communication equipment, status information sensors, kinematic control unit, disturbance observer unit, dynamic control unit, thruster unit, and actuation and drive mechanism.

[0150] The surface and underwater communication equipment can employ radio communication devices, underwater acoustic communication devices, buoys, and other available communication equipment for communication between ASVs and AUVs in the hybrid cluster. The status information sensors can employ inertial navigation devices, attitude sensors, Doppler velocimeters, depth gauges, and other available sensor equipment to acquire the position, velocity, and other status information of the ASVs and AUVs. The kinematic control unit is used to set the desired position vector η corresponding to the autonomous system i based on the cooperative task. id Combined with location status information η i and velocity state information ξ i The virtual control velocity ζ of autonomous system i is calculated at the kinematic level. ic The disturbance observer unit is used to estimate the disturbances experienced by the ASV and AUV; the dynamic control unit is used to estimate the virtual control speed ζ. ic At the dynamics layer, the control input τ of the autonomous system i is calculated using a robust model predictive controller.i The thruster unit is used to construct the thruster model, and is used to determine the thruster based on the control input τ. i At the execution layer, the actual execution control quantity λ of the propulsion system is calculated using a thruster model. i It outputs instructions to the specific execution and drive mechanisms; the execution and drive mechanisms drive the ASV and AUV to move according to the received instructions.

[0151] In one embodiment, the optimization problem of the robust model predictive controller in the dynamic control unit is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1); or, the optimization problem of the robust model predictive controller is set as Equation (16-2), and the constraint condition corresponding to Equation (16-2) is described as Equation (17-2).

[0152] In one embodiment, in the kinematic control unit, the virtual control velocity ζ ic Calculated by the Tube-MPC controller described in equation (5), The optimized speed control quantity is obtained by solving the optimization problem of the Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as Equation (6). For the feedback speed control variable of autonomous system i.

[0153] In the above embodiments:

[0154] 1. In the technical solution, in addition to the Extended State Observer (ESO) mentioned in the example, other types of disturbance observers are also applicable.

[0155] 2. When constructing a robust model predictive controller, in addition to the option of inequality constraints based on the Lyapunov method, other constraints that can derive system stability can be used as contraction constraints.

[0156] 3. When constructing inequality constraints based on the Lyapunov method, in addition to using nonlinear backstepping control as an auxiliary control variable, other nonlinear control strategies designed based on the Lyapunov method can also be used as effective auxiliary control variables.

[0157] There are three aspects that differ from existing technologies:

[0158] I. This invention employs a cascaded control structure. The kinematic layer calculates the virtual control velocity based on the desired collaborative objective, while the dynamic layer calculates the dynamic control input based on the virtual control velocity. This improves control accuracy and enhances system robustness. Unlike conventional techniques that treat the kinematic model as an ideal model (ignoring environmental velocity disturbances), this invention constructs actual kinematic models for ASVs and AUVs, considering velocity disturbances such as wind, waves, and ocean currents. The ideal model is treated as the nominal kinematic model, and the virtual control velocity is calculated using the Tube-MPC controller. This allows the ASVs and AUVs to maintain their desired positions under velocity disturbances, effectively improving the robustness of the hybrid cluster system at the kinematic control level.

[0159] Second, at the dynamic control level, this invention combines a disturbance observer with a robust model predictive control method. Based on disturbance estimation, the robust model predictive controller calculates the dynamic control input. This combination significantly enhances the robustness and stability of the hybrid cluster system at the dynamic control level.

[0160] Third, this invention takes into account the thruster model and determines the execution control quantities of the autonomous system execution layer, which is closer to the needs of actual engineering applications.

[0161] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Those skilled in the art should understand that modifications can be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A kind The hybrid cluster robust model predictive cooperative control method is characterized by, include: Step 1: Obtain any autonomous system in the hybrid cluster Location status information and speed status information Autonomous systems are divided into and ; Step 2: Set up the autonomous system based on the collaborative task. The corresponding expected position vector Combined with location status information and speed status information Computational autonomous systems at the kinematic level Virtual control speed ; Step 3, based on virtual control speed At the dynamics layer, an interference observer is used to estimate the interference. The autonomous system is calculated using a robust model predictive controller. control input ; Step 4, according to the control input At the execution layer, the actual control quantities of the propulsion system are calculated using a thruster model. ; In step 3, the optimization problem of the robust model predictive controller is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1); or the optimization problem of the robust model predictive controller is set as Equation (16-2), and the constraint condition corresponding to Equation (16-2) is described as Equation (17-2). （16-1） （17-1） （16-2） （17-2） in, for This indicates an autonomous system. At any moment rotation matrix; for This indicates an autonomous system. At any moment Control input; For prediction in the time domain; for This indicates an autonomous system. At any moment The actual speed; For autonomous systems At any moment Predicting time steps speed; Indicates autonomous system At any moment Predicting time steps Virtual control speed; This is the weighting matrix for the speed tracking error; To control the time domain; The weight matrix is ​​used to control the input quantities; For autonomous systems At any moment Predicting time steps Control input; For autonomous systems At any moment Predicting time steps speed; For autonomous systems At any moment Predicting time steps The nominal part of the dynamic model; For autonomous systems At any moment Predicting time steps Estimated interference; The set of constraints for the actual velocity state; For autonomous systems At any moment Predicting time steps Control input; A set of constraints for controlling the input; Indicates that the autonomous system is at time Predicting time steps Control input; The set of constraints to control the increment; For autonomous systems At any moment Predicting time steps speed; The set of terminal constraints for velocity states; This is the function corresponding to the contraction constraint.

2. As described in claim 1 The hybrid cluster robust model predictive cooperative control method is characterized by, The contraction constraint is designed based on the Lyapunov method and auxiliary control quantity, and the following inequality constraint is (18): （18） in, The Lyapunov function, which is related to velocity, is set as equation (19). （19） in, Represents autonomous system At any moment Speed ​​tracking error, for For autonomous systems At any moment Virtual control speed; for , representing the nonlinear auxiliary control quantity designed based on the Lyapunov method, is given by equation (20). （20） in, , , These represent the nominal portions of the inertia matrix, the Coriolis force and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. The interference estimated by the interference observer, For autonomous systems Virtual control acceleration, This is the gain matrix.

3. The method of claim 1-2 The hybrid cluster robust model predictive cooperative control method is characterized by, In step 2, virtual control speed Described by equation (5) The controller calculates and obtains this information. , To pass The optimized speed control quantity is obtained by solving the optimization problem of the controller, and the corresponding constraint condition of the optimization problem is set as Equation (6). For autonomous systems The difference between the actual location and the nominal location: （5） （6） in, For autonomous systems At any moment Predicting time steps The nominal speed, For autonomous systems At any moment Predicted time steps The nominal location, For autonomous systems At any moment Predicting time steps Stage costs, , For autonomous systems At any moment At time step The expected position This is the weight matrix for the position tracking error. This is the weight matrix for the speed control quantity. For autonomous systems At any moment Predicting time steps terminal cost, , For autonomous systems At any moment Predicted time steps The nominal location, For autonomous systems At any moment At time step The expected position This is the weight matrix for the terminal position tracking error; For autonomous systems At any moment The actual location, For autonomous systems At any moment The nominal location, For autonomous systems At any moment The nominal speed, Sampling time, The set of constraints for the actual position state. represent Difference, For actual kinematic models Invariant set For the feedback gain matrix, For the set of terminal constraints in position state, , Autonomous systems and autonomous systems At any moment Predicting time steps The actual location, for Mutual safety distance restrictions, for Mutual safety distance restrictions, if Then autonomous system for ,like Then autonomous system for .

4. As described in claim 3 The hybrid cluster robust model predictive cooperative control method is characterized by, when At that time, The depth is subject to the constraint described in equation (7) below: （7） in, For autonomous systems The set of depth constraints.

5. A kind Hybrid cluster robust model predictive cooperative control system, characterized in that, It includes a hybrid cluster, surface and underwater communication equipment, status information sensors, kinematic control unit, disturbance observer unit, dynamic control unit, thruster unit, and actuation and drive mechanism; Among them, surface and underwater communication equipment is used in hybrid clusters. , Communication between each other; Status information sensors are used to acquire information about autonomous systems. Location status information and speed status information ; The kinematic control unit is used to set up the autonomous system according to the cooperative task. The corresponding expected position vector Combined with location status information and speed status information Computational autonomous systems at the kinematic level Virtual control speed ; The perturbation observer unit is used for estimation , The disturbance received; The dynamic control unit is used to control the speed according to the virtual control speed. At the dynamics layer, a robust model predicts the controller to calculate the autonomous system. control input The thruster unit is used to construct the thruster model, and is used to determine the thruster based on the control input. At the execution layer, the thruster model is used to calculate... It also outputs instructions to the specific execution and driving mechanisms; The execution and drive mechanism is driven according to the received instructions. and sports; The optimization problem of the robust model predictive controller in the dynamic control unit is set as Equation (16-1), and the constraint condition corresponding to Equation (16-1) is described as Equation (17-1); or, the optimization problem of the robust model predictive controller is set as Equation (16-2), and the constraint condition corresponding to Equation (16-2) is described as Equation (17-2). （16-1） （17-1） （16-2） （17-2） in, for This indicates an autonomous system. At any moment rotation matrix; for This indicates an autonomous system. At any moment Control input; For prediction in the time domain; for This indicates an autonomous system. At any moment The actual speed; For autonomous systems At any moment Predicting time steps speed; Indicates autonomous system At any moment Predicting time steps Virtual control speed; This is the weighting matrix for the speed tracking error; To control the time domain; The weight matrix is ​​used to control the input quantities; For autonomous systems At any moment Predicting time steps Control input; For autonomous systems At any moment Predicting time steps speed; For autonomous systems At any moment Predicting time steps The nominal part of the dynamic model; For autonomous systems At any moment Predicting time steps Estimated interference; The set of constraints for the actual velocity state; For autonomous systems At any moment Predicting time steps Control input; A set of constraints for controlling the input; Indicates that the autonomous system is at time Predicting time steps Control input; The set of constraints to control the increment; For autonomous systems At any moment Predicting time steps speed; The set of terminal constraints for velocity states; This is the function corresponding to the contraction constraint.

6. The ASV-AUV hybrid cluster robust model predictive cooperative control system as described in claim 5, characterized in that, The contraction constraint is designed based on the Lyapunov method and auxiliary control quantity, and the following inequality constraint is (18): （18） in, The Lyapunov function, which is related to velocity, is set as equation (19). （19） in, Represents autonomous system At any moment Speed ​​tracking error, for For autonomous systems At any moment Virtual control speed; For the nonlinear auxiliary control quantity designed based on the Lyapunov method, it is given by equation (20). （20） in, , , These represent the nominal portions of the inertia matrix, the Coriolis force and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. The interference estimated by the interference observer, For autonomous systems Virtual control acceleration, This is the gain matrix.

7. The claim 5-6 Hybrid cluster robust model predictive cooperative control system, characterized in that, Virtual control speed in the kinematic control unit Described by equation (5) The controller calculates and obtains this information. , To pass The optimized speed control quantity is obtained by solving the optimization problem of the controller, and the corresponding constraint condition of the optimization problem is set as Equation (6). For autonomous systems The difference between the actual location and the nominal location: （5） （6） in, For autonomous systems At any moment Predicting time steps The nominal speed, For autonomous systems At any moment Predicted time steps The nominal location, For autonomous systems At any moment Predicting time steps Stage costs, , For autonomous systems At any moment At time step The expected position This is the weight matrix for the position tracking error. This is the weight matrix for the speed control quantity. For autonomous systems At any moment Predicting time steps terminal cost, , For autonomous systems At any moment Predicted time steps The nominal location, For autonomous systems At any moment At time step The expected position This is the weight matrix for the terminal position tracking error; For autonomous systems At any moment The actual location, For autonomous systems At any moment The nominal location, For autonomous systems At any moment The nominal speed, Sampling time, The set of constraints for the actual position state. This represents poor performance in Pontryagin. For actual kinematic models Invariant set For the feedback gain matrix, For the set of terminal constraints in position state, , Autonomous systems and autonomous systems At any moment Predicting time steps The actual location, for Mutual safety distance restrictions, for Mutual safety distance restrictions, if Then autonomous system for ,like Then autonomous system for .

8. As described in claim 7 Hybrid cluster robust model predictive cooperative control system, characterized in that, when At that time, The depth is subject to the constraint described in equation (7) below: （7） in, For autonomous systems The set of depth constraints.

Images