ASV-AUV hybrid cluster robust model prediction cooperative control method and system

Abstract

The invention discloses an ASV-AUV hybrid cluster robust model prediction cooperative control method and system, and the method comprises the steps: 1, obtaining the position state information and speed state information of any autonomous system in a hybrid cluster, and enabling the autonomous system to be divided into an ASV and an AUV; step 2, setting an expected position vector corresponding to the autonomous system i according to the cooperative task, and calculating a virtual control speed of the autonomous system in a kinematics layer in combination with position state information and speed state information; 3, according to the virtual control speed, a disturbance observer is adopted to estimate disturbance in a dynamic layer, and control input of an autonomous system is calculated through a robust model prediction controller; and 4, according to the control input, calculating the actual execution control quantity of the propulsion system in the execution layer by adopting the propeller model. According to the method, the cooperative control problem of the ASV-AUV hybrid cluster system with uncertainty can be solved, and the robustness and stability of system control are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of cross-domain collaboration of autonomous systems, and in particular to a method and system for robust model predictive collaborative control of an ASV (Autonomous Surface Vehicle)-AUV (Autonomous Underwater Vehicle) hybrid cluster. Background Art

[0002] With the rapid development of intelligent technology and autonomous systems, the concept of cross-domain collaborative technology has emerged and has gradually become a high ground for global autonomous system technology competition. The cross-domain collaborative technology of autonomous systems mainly refers to the collaborative control technology between different types of autonomous systems such as drones, autonomous ground vehicles, autonomous surface vessels, and autonomous underwater robots in different spatial domains such as air, ground, and ocean. In the field of marine engineering, the ASV-AUV hybrid cluster system composed of ASV and AUV is an application of cross-domain collaborative technology. Through the collaboration of the two spatial domains above and below the waters, the ASV-AUV hybrid cluster system can be applied to more complex marine operation scenarios, including emergency maritime rescue, marine terrain and resource exploration, etc. Collaborative control is the key technology to achieve cross-domain collaboration. By combining the autonomous systems of the two spatial domains with a preset specific structure, a stable collaborative operation mode is formed.

[0003] At present, the main solutions to the collaborative control problem include the pilot-follower method, virtual structure method, artificial potential field method, behavioral method, graph theory method, etc. Based on these methods, domestic and foreign scholars have proposed some control methods for the collaborative control of ASV and AUV, but they mainly consider single-type clusters (ASV clusters or AUV clusters), and there are relatively few studies on ASV-AUV mixed clusters. In practical applications, due to the uncertainty of interference such as wind, waves, and ocean currents in the marine operating environment, as well as the nonlinear dynamic characteristics of the autonomous system itself and the influence of unmodeled uncertainty, it is difficult for existing technologies to ensure the robustness and stability of the collaborative control of ASV-AUV mixed clusters.

[0004] Patent document CN117850417A discloses a formation control method and system for surface unmanned ships and autonomous underwater unmanned vehicles, including: step S1: sending the predetermined formation task to USV and AUV respectively, placing USV and AUV in the water for navigation, and calculating the relative position; step S2: designing the reference heading angle, reference speed, longitudinal control force of AUV, and designing the bow sway direction control torque; step S3: calculating the error variable and the environmental interference estimation error variable; step S4: controlling the AUV thruster and rudder to generate the calculated control force and torque to realize the formation navigation of USV and AUV. The nonlinear disturbance observer provided by the invention can be used to estimate the impact of unknown environmental forces and modeling uncertainties on the vehicle. The invention only considers the collaborative formation control of a single USV and AUV, while the actual hybrid cluster system is usually composed of multiple USVs and AUVs, so the invention is difficult to apply to large-scale cluster collaborative control.

[0005] Patent document CN115268476B discloses a distributed surface ship and underwater vehicle collaborative control system and method, which adopts modular design. ASV (AUV) obtains the position status information of itself and adjacent AUVs and ASVs and the formation structure instructions through the positioning and communication module, generates the position and speed state of the next moment through the collaborative controller, and performs point tracking control through the self-anti-disturbance point tracking controller and actuator. The control frequency of the self-anti-disturbance point tracking control is high to form an inner loop control, while the collaborative control has a low control frequency due to communication limitations to form an outer loop control. The dynamic equation considered in this invention is a linear system, while the actual AUV or ASV is usually a nonlinear system, so this invention is difficult to apply to a hybrid cluster system with nonlinear dynamic characteristics.

[0006] Patent document CN114942646B discloses a three-dimensional space formation control method for heterogeneous unmanned systems, including the following steps: establishing a three-dimensional formation communication topology model for 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, executing a depth consistency control algorithm; controlling the surface unmanned boat as the unmanned system node to operate according to the output heading angle and output navigation speed; controlling the autonomous underwater vehicle as the unmanned system node to operate according to the output heading angle, output navigation speed and output navigation depth. This invention does not consider the dynamic model of the surface unmanned boat and the autonomous underwater vehicle, and does not consider the uncertainty of the model, so the robustness and stability of this invention in practical applications are difficult to guarantee. Summary of the invention

[0007] The object of the present invention is to provide an ASV-AUV hybrid cluster robust model predictive collaborative control method and system, aiming to solve the collaborative control problem of an ASV-AUV hybrid cluster system with uncertainty and improve the robustness and stability of system control.

[0008] To achieve the above object, the present invention provides an ASV-AUV hybrid cluster robust model predictive collaborative control method, characterized in that it includes:

[0009] Step 1: Obtain the location status information η of any autonomous system i in the hybrid cluster i and speed status informationξ i , autonomous systems are divided into ASV and AUV;

[0010] Step 2: Set the expected position vector η corresponding to the autonomous system i according to the collaborative task id , combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i at the kinematic level ic ;

[0011] Step 3: According to the virtual control speed ζ ic , a disturbance observer is used at the dynamics layer to estimate the disturbance d i , the control input τ of the autonomous system i is calculated by the robust model predictive controller i ;

[0012] Step 4: According to the control input τ i , the propeller model is used at the execution layer to calculate the actual execution control quantity λ of the propulsion system i .

[0013] Further, 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 is the objective function to be optimized; τ i is τ i (t), represents the control input of autonomous system i at time t; N p is the prediction time domain; i For i (t), represents the actual speed of autonomous system i at time t; ζ i(s|t) is the predicted velocity of autonomous system i at time t, time step s; ζ ic (s|t) represents the virtual control speed of autonomous system i at time t, predicted time step s; Q iζ is the weight matrix of velocity tracking error; N c To control the time domain; R i2 is the weight matrix for controlling the input quantity; τ i (s|t) is the control input of autonomous system i at time t, predicted time step s; ζ i (s+1|t) is the speed of autonomous system i at time t predicted at time step s+1; g(ζ i (s|t),τ i (s|t)) is the nominal part of the dynamics model of the autonomous system i at time t predicting time step s; The disturbance estimated at time step s is predicted for autonomous system i at time t; is the constraint set of the actual speed state; τ i (s|t) is the control input of autonomous system i at time t, predicting time step s; is the constraint set of control input; τ i (s+1|t) represents the control input of the autonomous system at time t predicted at time step s+1; is the set of constraints that control the increment; i (N p |t) is the prediction time step N of autonomous system i at time t p The speed of i is the terminal constraint set of the velocity state; H(ζ 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 the auxiliary control quantity as follows:

[0017]

[0018] Among them, V i represents the velocity-dependent Lyapunov function, which is set as (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), is the virtual control speed of autonomous system i at time t;

[0021] for represents the nonlinear auxiliary control quantity designed based on the Lyapunov method, which is formula (20);

[0022]

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

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

[0025]

[0026]

[0027] in, is the predicted nominal position of autonomous system i at time step s+1 at time t, R i (s|t) is the rotation matrix of time step s predicted by autonomous system i at time t; is the predicted nominal speed of autonomous system i at time t at time step s, is the predicted nominal position of autonomous system i at time step s at time t, L i (s|t) is the stage cost of autonomous system i predicting time step s at time t, η id (s|t) is the expected position of autonomous system i at time t in time step s, Q i1 is the weight matrix of position tracking error, R i1 is the weight matrix of speed control, N i (N p |t) is the prediction time step N of autonomous system i at time t p The terminal cost, is the time step N predicted by the autonomous system i at time t pThe nominal position, η id (N p |t) is the autonomous system i at time t in time step N p The expected position, Q i2 is the weight matrix of the terminal position tracking error; is the actual position of autonomous system i at time t, is the nominal position of autonomous system i at time t, is the nominal speed of autonomous system i at time t, T is the sampling time, is the constraint set of the actual position state, Represents Pontryagin Poor, is the Tube invariant set of the actual kinematic model, Υ i is the feedback gain matrix, Λ i is the terminal constraint set of the position state, η i (s|t),η j (s|t) are the actual positions of autonomous system i and autonomous system j at time t, respectively, and η ijs is the safe distance limit between ASVs, η ijU is the safety distance limit between AUVs. If i∈S, the autonomous system i is an ASV. If i∈U, the autonomous system i is an AUV.

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

[0029] z i ∈Z lim (7)

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

[0031] The present invention also provides an ASV-AUV hybrid cluster robust model prediction collaborative control system, which includes a hybrid cluster, surface and underwater communication equipment, a state information sensor, a kinematic control unit, a disturbance observer unit, a dynamics control unit, a thruster unit, and an execution and driving 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 obtain the position state information η of the autonomous system i i and speed status informationξ i ;

[0034] The kinematic control unit is used to set the desired position vector η corresponding to the autonomous system i according to the collaborative task id, combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i at the kinematic level ic ;

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

[0036] The dynamics control unit is used to control the speed according to the virtual ic , the control input τ of the autonomous system i is calculated by the robust model predictive controller at the dynamics level i ;

[0037] The thruster unit is used to construct a thruster model for controlling the control input τ i , the propeller model is used at the execution layer to calculate the actual execution control quantity λ of the propulsion system i , and output instructions to specific execution and driving mechanisms;

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

[0039] Further, 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 is the objective function to be optimized; τ i is τ i (t), represents the control input of autonomous system i at time t; N p is the prediction time domain; i For i (t), represents the actual speed of autonomous system i at time t; ζ i (s|t) is the predicted velocity of autonomous system i at time t, time step s; ζ ic (s|t) represents the virtual control speed of autonomous system i at time t, predicted time step s; Q iζ is the weight matrix of velocity tracking error; N c To control the time domain; R i2 is the weight matrix for controlling the input quantity; τ i (s|t) is the control input of autonomous system i at time t, predicted time step s; ζ i (s+1|t) is the speed of autonomous system i at time t predicted at time step s+1; g(ζi (s|t),τ i (s|t)) is the nominal part of the dynamics model of the autonomous system i at time t predicting time step s; The disturbance estimated at time step s is predicted for autonomous system i at time t; is the constraint set of the actual speed state; τ i (s|t) is the control input of autonomous system i at time t, predicting time step s; is the constraint set of control input; τ i (s+1|t) represents the control input of the autonomous system at time t predicted at time step s+1; is the set of constraints that control the increment; i (N p |t) is the prediction time step N of autonomous system i at time t p The speed of i is the terminal constraint set of the velocity state; H(ζ 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 the auxiliary control quantity as follows:

[0043]

[0044] Among them, V i represents the velocity-dependent Lyapunov function, which is set as (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), is the virtual control speed of autonomous system i at time t;

[0047] is the nonlinear auxiliary control quantity designed based on the Lyapunov method, which is formula (20);

[0048]

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

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

[0051]

[0052] in, is the predicted nominal position of autonomous system i at time step s+1 at time t, R i (s|t) is the rotation matrix of time step s predicted by autonomous system i at time t; is the predicted nominal speed of autonomous system i at time t at time step s, is the predicted nominal position of autonomous system i at time step s at time t, L i (s|t) is the stage cost of autonomous system i predicting time step s at time t, η id (s|t) is the expected position of autonomous system i at time t in time step s, Q i1 is the weight matrix of position tracking error, R i1 is the weight matrix of speed control, N i (N p |t) is the prediction time step N of autonomous system i at time t p The terminal cost, is the time step N predicted by the autonomous system i at time t p The nominal position, η id (N p |t) is the autonomous system i at time t in time step N p The expected position, Q i2 is the weight matrix of the terminal position tracking error; is the actual position of autonomous system i at time t, is the nominal position of autonomous system i at time t, is the nominal speed of autonomous system i at time t, T is the sampling time, is the constraint set of the actual position state, Represents Pontryagin Poor, is the Tube invariant set of the actual kinematic model, Υi is the feedback gain matrix, Λ i is the terminal constraint set of the position state, η i (s|t),η j (s|t) are the actual positions of autonomous system i and autonomous system j at time t, respectively, and η ijs is the safe distance limit between ASVs, η ijU is the safety distance limit between AUVs. If i∈S, the autonomous system i is an ASV. If i∈U, the autonomous system i is an AUV.

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

[0054] z i ∈Z lim (7)

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

[0056] The present invention adopts a cascade control structure, and the kinematic layer is based on the expected collaborative target, that is, the expected position vector η id , which is responsible for calculating the virtual control speed. The dynamic layer calculates the dynamic control input based on the virtual control speed, that is, the control input τ i , which can improve control accuracy and enhance system robustness.

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

[0058] The present invention combines the disturbance observer with the robust model predictive control method. The disturbance observer makes the dynamic model prediction more accurate, and the robust model predictive control method ensures the closed-loop stability of the control system. The combination of the two can significantly improve the performance and stability of the control system in the face of uncertainty and disturbance. The present invention ultimately outputs the execution control quantity of the system thruster, that is, the execution control quantity λ i , which is more in line with the actual engineering application needs. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

[0062] Figure 4 It is a structural diagram of the ASV-AUV hybrid cluster robust model predictive collaborative control system. DETAILED DESCRIPTION

[0063] In the drawings, the same or similar reference numerals are used to represent the same or similar elements or elements with the same or similar functions. The embodiments of the present invention are described in detail below in conjunction with the drawings.

[0064] In the description of the present invention, the terms "center", "longitudinal", "lateral", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate directions or positional relationships based on the directions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and therefore should not be understood as limiting the scope of protection of the present invention.

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

[0066] like Figure 2 As shown, the main steps of the ASV-AUV hybrid cluster robust model predictive collaborative control method provided by the embodiment of the present invention are as follows:

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

[0068] Specifically, if i∈S, then the autonomous system i is an ASV, and the position state is η i =[x i ,y i ,ψ i ] T , x i ,y i and ψ i They represent the x-axis position, y-axis position and yaw angle of the autonomous system i in the geodetic coordinate system, and the velocity state is ζi =[u i ,v i ,r i ] T ,u i 、v i and r i They represent the forward velocity, lateral velocity and yaw angular velocity 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 They represent the x-axis position, y-axis position, z-axis position, roll angle, pitch angle and yaw angle of the autonomous system i in the geodetic coordinate system, and the velocity state is ζ 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 They represent the forward velocity, lateral velocity, vertical velocity, roll angular velocity, pitch angular velocity and yaw angular velocity in the carrier coordinate system respectively.

[0070] Take 5 ASVs + 5 AUVs as an example. Figure 1 Shown is a schematic diagram of the ASV-AUV hybrid cluster of the present invention.

[0071] Step 2: Set the expected position vector η corresponding to the autonomous system i according to the collaborative task id , combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i 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, namely, the Tube-MPC controller, which is used to calculate the virtual control speed ζ of the autonomous system i. ic .

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

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

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

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

[0077]

[0078] in, is the nominal position of autonomous system i at time t+1, is the nominal position of autonomous system i at time t, is the nominal speed 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, is the Tube invariant set of the actual kinematic model, and ⊕ represents the Minkowski sum.

[0082] To meet the actual system requirements, the Tube-MPC controller will feedback the speed control quantity Set as formula (4):

[0083]

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

[0085] For the nominal kinematic model part, the Tube-MPC controller solves the optimal speed control variable by solving the optimization problem described in equation (5): The corresponding constraint condition of the optimization problem is set as formula (6).

[0086]

[0087] in,

[0088]

[0089] Among them, s represents the predicted time step, N p is the prediction time domain, N c To control the time domain, is the predicted nominal speed of autonomous system i at time t at time step s, L i (s|t) is the stage cost of autonomous system i predicting time step s at time t, is the predicted nominal position of autonomous system i at time step s at time t, η id (s|t) is the expected position of autonomous system i at time t in time step s, Q i1 is the weight matrix of the position tracking error, Q i2 is the weight matrix of the terminal position tracking error, R i1 is the weight matrix of speed control, N i (N p |t) is the prediction time step N of autonomous system i at time t p The terminal cost, is the time step N predicted by the autonomous system i at time t p The nominal position, η id (N p |t) is the autonomous system i at time t in time step N p desired location.

[0090]

[0091] in, is the predicted nominal position of autonomous system i at time step s+1 at time t, R i (s|t) is the rotation matrix of time step s predicted by autonomous system i at time t; is the nominal speed of autonomous system i at time t, Represents Pontryagin Poor, is the Tube invariant set of the actual kinematic model, Υ iis the feedback gain matrix, Λ i is the terminal constraint set of the position state, η i (s|t),η j (s|t) are the actual positions of autonomous system i and autonomous system j at time t, respectively, and η ijs is the safe distance limit between ASVs, η ijU It is the safe distance limit between AUVs.

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

[0093] z i ∈Z lim (7)

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

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

[0096] Step 3: According to the virtual control speed ζ ic , a disturbance observer is used at the dynamics layer to estimate the disturbance d i , the control input τ of the autonomous system i is calculated by the robust model predictive controller i .

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

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

[0099] Among them, g(ζ i (t),τ i (t)) is the nominal part of the dynamics model of autonomous system i, τ i (t) is the control input of autonomous system i, including control force and control torque, d i (t) is the disturbance caused by wind, ocean currents, waves and modeling uncertainties.

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

[0101]

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

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

[0104] Then the original kinetic model can be described as formula (10):

[0105]

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

[0107]

[0108] Among them, g(ζ i ,τ i ) is g(ζ i (t),τ i (t)), represents the nominal part of the dynamic model, which is expressed as Equation (12), the disturbance d i It is expressed as formula (13):

[0109]

[0110] Interference i It can be estimated by the disturbance observer, and the estimated disturbance is denoted as

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

[0112]

[0113] Among them, γ i1 and γ i2 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] Among them, N p is the prediction time domain, N c To control the time domain, Q iζ is the weight matrix of velocity tracking error, R i2 is the weight matrix that controls the input quantity.

[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 is the objective function to be optimized; τ i is τ i (t), represents the control input of autonomous system i at time t; N p is the prediction time domain; i For i (t), represents the actual speed of autonomous system i at time t; ζ i (s|t) is the predicted velocity of autonomous system i at time t, time step s; ζ ic (s|t) represents the virtual control speed of autonomous system i at time t, predicted time step s; Q iζ is the weight matrix of velocity tracking error; N c To control the time domain; R i2 is the weight matrix for controlling the input quantity; τ i (s|t) is the control input of autonomous system i at time t, predicted time step s; ζ i (s+1|t) is the speed of autonomous system i at time t predicted at time step s+1; g(ζ i (s|t),τ i (s|t)) is the nominal part of the dynamics model of the autonomous system i at time t predicting time step s; The disturbance estimated at time step s is predicted for autonomous system i at time t; is the constraint set of the actual speed state; τ i (s|t) is the control input of autonomous system i at time t, predicting time step s; is the constraint set of control input; τ i (s+1|t) represents the control input of the autonomous system at time t predicted at time step s+1; is the set of constraints that control the increment; i (N p |t) is the prediction time step N of autonomous system i at time t p The speed of i is the terminal constraint set of the velocity state; H(ζ 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 optional solution for the contraction constraint is to design the following inequality constraint (18) based on the Lyapunov method and the auxiliary control quantity:

[0122]

[0123] Among them, V i represents the velocity-dependent Lyapunov function, It is a nonlinear auxiliary control variable designed based on the Lyapunov method.

[0124] Specifically, the nonlinear auxiliary control quantity is designed based on the Lyapunov function, and the stability can be proved by the Lyapunov method. Taking nonlinear backstepping control as an example, according to the AUV dynamics model described in the above formula (11), let e i =ζ i -ζ ic represents the velocity tracking error, which is defined as the following Lyapunov function (19):

[0125]

[0126] The derivative of

[0127]

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

[0129]

[0130] Among them, K iζ 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 i .

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

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

[0134] Among them, K i is the propulsion system conversion coefficient, λ i is the execution control quantity, h(λ i ) is the related function of the execution control quantity.

[0135] The motion control of the autonomous system ultimately needs to rely on a specific propulsion system to be implemented. Therefore, the present invention determines the corresponding execution control amount based on the propulsion model.

[0136] Specifically, taking underactuated ASV and AUV as examples, the propulsion system mainly consists of propellers and steering gears, which are controlled by the control input τ i The corresponding propeller thrust and steering gear torque are obtained by the components. For ASV, the propeller execution control quantity is the speed n i , the steering gear control quantity is the horizontal rudder angle δ si .

[0137] The thrust model of the propeller is as follows:

[0138]

[0139] in, is the propeller thrust, For control input τ i The forward speed u along the system i Directional component, K iT is the thrust coefficient.

[0140] The horizontal torque model of the steering gear is as follows:

[0141]

[0142] in, is the horizontal torque of the servo, For control input τ i The horizontal component, is the dimensional horizontal rudder angle coefficient, u i is the forward speed of autonomous system i.

[0143] For AUV, the propeller control quantity is the speed ni , the steering gear control quantity is the horizontal rudder angle δ si and vertical rudder angle δ ri .

[0144] The propeller thrust model and the steering gear horizontal torque model are similar to those of ASV, and the steering gear vertical torque model is formula (24):

[0145]

[0146] in, is the vertical torque of the servo, For control input τ i The vertical component, is the dimensional vertical rudder angle coefficient.

[0147] Based on the above propeller model, the execution control quantity of the system 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 build propulsion models based on similar methods to determine the corresponding execution control quantities.

[0149] To implement the above method, this patent also provides an ASV-AUV hybrid cluster robust model prediction collaborative control system, such as Figure 3 and 4 As shown, it includes an ASV / AUV entity, surface and underwater communication equipment, a state information sensor, a kinematic control unit, a disturbance observer unit, a dynamic control unit, a thruster unit, and an execution and driving mechanism.

[0150] Among them, the surface and underwater communication equipment can use radio communication equipment, hydroacoustic communication equipment, buoys and other available communication equipment for communication between ASVs and AUVs in the mixed cluster; the state information sensor can use inertial navigation equipment, attitude sensors, Doppler velocimeters, depth gauges and other available sensor equipment to obtain the position, speed and other state information of ASVs and AUVs; the kinematic control unit is used to set the expected position vector η corresponding to the autonomous system i according to the collaborative task id , combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i at the kinematic level ic ; The disturbance observer unit is used to estimate the disturbances suffered by ASV and AUV; the dynamic control unit is used to control the virtual speed ζ ic , the control input τ of the autonomous system i is calculated by the robust model predictive controller at the dynamics leveli ; The propeller unit is used to build a propeller model for controlling the input τ i , the propeller model is used at the execution layer to calculate the actual execution control quantity λ of the propulsion system i , and output instructions to specific execution and driving mechanisms; the execution and driving mechanisms drive the ASV and AUV to move according to the instructions obtained.

[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 speed ζ ic It is calculated by the Tube-MPC controller described by equation (5), is the optimal speed control quantity obtained by solving the optimization problem of Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as formula (6). is the feedback speed control quantity 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 optional solution 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 are different from the existing technology:

[0158] 1. The present invention adopts a cascade control structure. The kinematic layer is responsible for calculating the virtual control speed according to the expected collaborative target, and the dynamic layer calculates the dynamic control input according to the virtual control speed, which can improve the control accuracy and enhance the robustness of the system. Different from the conventional technology that regards the kinematic model as an ideal model (without considering the environmental speed interference), the present invention considers speed interference factors such as wind, waves and ocean currents to construct the actual kinematic model of ASV and AUV, regards the ideal model as the nominal kinematic model, and calculates the virtual control speed through the Tube-MPC controller, so that ASV and AUV can maintain the desired position under speed interference, effectively improving the robustness of the hybrid cluster system at the kinematic control level.

[0159] Second, at the dynamic control level, the present invention combines the disturbance observer with the robust model predictive control method, and uses the robust model predictive controller to calculate the dynamic control input based on the disturbance estimation. This combination significantly enhances the robustness and stability of the hybrid cluster system at the dynamic control level.

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

[0161] Finally, it should be pointed out that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Those skilled in the art should understand that the technical solutions described in the above embodiments may be modified, or some of the technical features thereof may be replaced by equivalents; these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robust model predictive collaborative control method for ASV-AUV hybrid cluster, characterized in that: include: Step 1: Obtain the location status information η of any autonomous system i in the hybrid cluster i and speed status informationξ i , autonomous systems are divided into ASV and AUV; Step 2: Set the expected position vector η corresponding to the autonomous system i according to the collaborative task id , combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i at the kinematic level ic ; Step 3: According to the virtual control speed ζ ic , a disturbance observer is used at the dynamics layer to estimate the disturbance d i , the control input τ of the autonomous system i is calculated by the robust model predictive controller i ; Step 4: According to the control input τ i , the propeller model is used at the execution layer to calculate the actual execution control quantity λ of the propulsion system i .

2. The ASV-AUV hybrid cluster robust model predictive collaborative control method according to claim 1, characterized in that: 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); Among them, J i is the objective function to be optimized; τ i is τ i (t), represents the control input of autonomous system i at time t; N p is the prediction time domain; i For i (t), represents the actual speed of autonomous system i at time t; ζ i (s|t) is the predicted velocity of autonomous system i at time t, time step s; ζ ic (s|t) represents the virtual control speed of autonomous system i at time t, predicted time step s; Q iζ is the weight matrix of velocity tracking error; N c To control the time domain; R i2 is the weight matrix for controlling the input quantity; τ i (s|t) is the control input of autonomous system i at time t, predicted time step s; ζ i (s+1|t) is the speed of autonomous system i at time t predicted at time step s+1; g(ζ i (s|t),τ i (s|t)) is the nominal part of the dynamics model of the autonomous system i at time t predicting time step s; The disturbance estimated at time step s is predicted for autonomous system i at time t; is the constraint set of the actual speed state; τ i (s|t) is the control input of autonomous system i at time t, predicting time step s; is the constraint set of control input; τ i (s+1|t) represents the control input of the autonomous system at time t predicted at time step s+1; is the set of constraints that control the increment; i (N p |t) is the prediction time step N of autonomous system i at time t p The speed of i is the terminal constraint set of the velocity state; H(ζ i (s|t),τ i (s|t)) is the function corresponding to the contraction constraint.

3. The ASV-AUV hybrid cluster robust model predictive collaborative control method according to claim 2, characterized in that: The contraction constraint is designed based on the Lyapunov method and the auxiliary control quantity as follows: Among them, V i represents the velocity-dependent Lyapunov function, which is set as (19); Among them, e i =ξ i -ξ ic represents the velocity tracking error of autonomous system i at time t, ζ ic For ic (t), is the virtual control speed of autonomous system i at time t; for represents the nonlinear auxiliary control quantity designed based on the Lyapunov method, which is formula (20); in, represent the nominal parts of the inertia matrix, the Coriolis and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. is the disturbance estimated by the disturbance observer, is the virtual control acceleration of autonomous system i, K iζ is the gain matrix.

4. The ASV-AUV hybrid cluster robust model predictive collaborative control method according to any one of claims 1 to 3, characterized in that: In step 2, the virtual control speed ζ ic It is calculated by the Tube-MPC controller described by equation (5), is the optimal speed control quantity obtained by solving the optimization problem of Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as formula (6). is the feedback speed control quantity of autonomous system i: in, is the predicted nominal position of autonomous system i at time step s+1 at time t, R i (s|t) is the rotation matrix of time step s predicted by autonomous system i at time t; is the predicted nominal speed of autonomous system i at time t at time step s, is the predicted nominal position of autonomous system i at time step s at time t, L i (s|t) is the stage cost of autonomous system i predicting time step s at time t, η id (s|t) is the expected position of the autonomous system u at time t in time step s, Q i1 is the weight matrix of position tracking error, R i1 is the weight matrix of speed control, N i (N p |t) is the prediction time step N of autonomous system i at time t p The terminal cost, is the time step N predicted by the autonomous system i at time t p The nominal position, η id (N p |t) is the autonomous system i at time t in time step N p The expected position, Q i2 is the weight matrix of the terminal position tracking error; is the actual position of autonomous system i at time t, is the nominal position of autonomous system i at time t, is the nominal speed of autonomous system i at time t, T is the sampling time, is the constraint set of the actual position state, Represents Pontryagin Poor, is the Tube invariant set of the actual kinematic model, Υ i is the feedback gain matrix, Λ i is the terminal constraint set of the position state, η i (s|t),η j (s|t) are the actual positions of autonomous system i and autonomous system j at time t, respectively, and η ijs is the safe distance limit between ASVs, η ijU is the safety distance limit between AUVs. If i∈S, the autonomous system i is an ASV. If i∈U, the autonomous system i is an AUV.

5. The ASV-AUV hybrid cluster robust model predictive collaborative control method according to claim 4, characterized in that: When i∈U, the following constraint is added to the depth of AUV as described in equation (7): With i ∈Z lim (7) Among them, Z lim is the set of depth constraints of autonomous system i.

6. A robust model predictive collaborative control system for ASV-AUV hybrid cluster, characterized in that: It includes hybrid cluster, surface and underwater communication equipment, state information sensor, kinematic control unit, disturbance observer unit, dynamic control unit, thruster unit, execution and drive mechanism; Among them, the surface and underwater communication equipment is used for communication between ASVs and AUVs in the hybrid cluster; The state information sensor is used to obtain the position state information η of the autonomous system i i and speed status informationξ i ; The kinematic control unit is used to set the desired position vector η corresponding to the autonomous system i according to the collaborative task id , combined with the position state information η i and speed status informationξ i , calculate the virtual control speed ζ of the autonomous system i at the kinematic level ic ; The disturbance observer unit is used to estimate the disturbances experienced by ASV and AUV; The dynamics control unit is used to control the speed according to the virtual ic , the control input τ of the autonomous system i is calculated by the robust model predictive controller at the dynamics level i ; The thruster unit is used to construct a thruster model for controlling the control input τ i , the propeller model is used at the execution layer to calculate the actual execution control quantity λ of the propulsion system i , and output instructions to specific execution and driving mechanisms; The execution and driving mechanism drives the ASV and AUV to move according to the received instructions.

7. The ASV-AUV hybrid cluster robust model predictive collaborative control system according to claim 1, characterized in that: 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); Among them, J i is the objective function to be optimized; τ i is τ i (t), represents the control input of autonomous system i at time t; N p is the prediction time domain; i For i (t), represents the actual speed of autonomous system i at time t; ζ i (s|t) is the predicted velocity of autonomous system i at time t, time step s; ζ ic (s|t) represents the virtual control speed of autonomous system i at time t, predicted time step s; Q iζ is the weight matrix of velocity tracking error; N c To control the time domain; R i2 is the weight matrix for controlling the input quantity; τ i (s|t) is the control input of autonomous system i at time t, predicted time step s; ξ i (s+1|t) is the speed of autonomous system i at time t predicted at time step s+1; g(ζ i (s|t),τ i (s|t)) is the nominal part of the dynamics model of the autonomous system i at time t predicting time step s; The disturbance estimated at time step s is predicted for autonomous system i at time t; is the constraint set of the actual speed state; τ i (s|t) is the control input of autonomous system i at time t, predicting time step s; is the constraint set of control input; τ i (s+1|t) represents the control input of the autonomous system at time t predicted at time step s+1; is the set of constraints that control the increment; i (N p |t) is the prediction time step N of autonomous system i at time t p The speed of i is the terminal constraint set of the velocity state; H(ζ i (s|t),τ i (s|t)) is the function corresponding to the contraction constraint.

8. The ASV-AUV hybrid cluster robust model predictive collaborative control system according to claim 7, characterized in that: The contraction constraint is designed based on the Lyapunov method and the auxiliary control quantity as follows: Among them, V i represents the velocity-dependent Lyapunov function, which is set as (19); Among them, e i =ζ i -ζ ic represents the velocity tracking error of autonomous system i at time t, ζ ic For ic (t), is the virtual control speed of autonomous system i at time t; is the nonlinear auxiliary control quantity designed based on the Lyapunov method, which is formula (20); in, represent the nominal parts of the inertia matrix, the Coriolis and centrifugal force matrices, and the fluid dynamics damping matrix, respectively. is the disturbance estimated by the disturbance observer, is the virtual control acceleration of autonomous system i, K iζ is the gain matrix.

9. The ASV-AUV hybrid cluster robust model predictive collaborative control system according to any one of claims 6 to 8, characterized in that: In the kinematic control unit, the virtual control speed ζ ic It is calculated by the Tube-MPC controller described by equation (5), is the optimal speed control quantity obtained by solving the optimization problem of Tube-MPC controller. The corresponding constraint condition of the optimization problem is set as formula (6). is the feedback speed control quantity of autonomous system i: in, is the predicted nominal position of autonomous system i at time step s+1 at time t, R i (s|t) is the rotation matrix of time step s predicted by autonomous system i at time t; is the predicted nominal speed of autonomous system i at time t at time step s, is the predicted nominal position of autonomous system i at time step s at time t, L i (s|t) is the stage cost of autonomous system i predicting time step s at time t, η id (s|t) is the expected position of autonomous system i at time t in time step s, Q i1 is the weight matrix of position tracking error, R i1 is the weight matrix of speed control, N i (N p |t) is the prediction time step N of autonomous system i at time t p The terminal cost, is the time step N predicted by the autonomous system i at time t p The nominal position, η id (N p |t) is the autonomous system i at time t in time step N p The expected position, Q i2 is the weight matrix of the terminal position tracking error; is the actual position of autonomous system i at time t, is the nominal position of autonomous system i at time t, is the nominal speed of autonomous system i at time t, T is the sampling time, is the constraint set of the actual position state, Represents Pontryagin Poor, is the Tube invariant set of the actual kinematic model, Υ i is the feedback gain matrix, Λ i is the terminal constraint set of the position state, η i (s|t),η j (s|t) are the actual positions of autonomous system i and autonomous system j at time t, respectively, and η ijs is the safe distance limit between ASVs, η ijU is the safety distance limit between AUVs. If i∈S, the autonomous system i is an ASV. If i∈U, the autonomous system i is an AUV.

10. The ASV-AUV hybrid cluster robust model predictive collaborative control system according to claim 9, characterized in that: When i∈U, the following constraint is added to the depth of AUV as described in equation (7): With i ∈Z lim (7) Among them, Z lim is the set of depth constraints of autonomous system i.

Images