A collaborative navigation control method and device for a heterogeneous AUV group

Abstract

The present invention relates to a method and device for cooperative navigation control of a heterogeneous AUV group, and belongs to the technical field of cooperative control of heterogeneous AUV groups. The method comprises constructing a linear state space model of AUVs, setting a consistency protocol and a distributed MPC cooperative navigation control law, and acquiring basic data of multiple AUVs; linearizing each basic data according to a dynamic mathematical model to obtain a heterogeneous AUV group; and cooperatively controlling the heterogeneous AUV group based on the consistency protocol and the distributed MPC cooperative navigation control law. The present invention processes the heterogeneous AUV group through a consistency protocol to reduce the complexity of calculation, and then controls the heterogeneous AUV group through the distributed MPC cooperative navigation control law, thereby realizing the problem of real-time cooperative navigation control of the heterogeneous AUV group.

Description

Technical Field

[0001] The present invention relates to the technical field of cooperative control of heterogeneous AUV groups, and in particular to a cooperative navigation control method and device for a heterogeneous AUV group. Background Art

[0002] With the continuous deepening of ocean exploration and development, autonomous underwater vehicles (AUVs), as an important ocean exploration platform, play an increasingly important role in the fields of marine resource exploration, marine environment monitoring, underwater search and rescue, etc. However, a single AUV often has many limitations when performing complex tasks, such as limited detection range and low task execution efficiency. Therefore, using multiple AUVs to form a group for collaborative operation has become an important direction to improve ocean exploration capabilities and task execution efficiency. There are many challenges in the collaborative navigation control of AUV groups, including heterogeneity, consistency requirements, communication restrictions, environmental interference, and task constraints. The AUVs in the group may have different dynamic characteristics, sensor configurations, and mission capabilities. This heterogeneity increases the complexity of collaborative control. At the same time, in order to achieve effective group collaboration, it is necessary to ensure the consistency between group members, including the consistency of states such as position, speed, and heading. However, the communication bandwidth in the underwater environment is limited and susceptible to interference, which poses a challenge to information exchange and collaborative decision-making among group members. In addition, there are complex interference factors such as water currents and waves in the marine environment, which can affect the motion state of the AUV and the overall performance of the group. When performing specific tasks, the AUV group also needs to meet various constraints, such as obstacle avoidance and energy efficiency.

[0003] Traditional AUV group control methods mainly include behavioral methods, artificial potential field methods, and graph theory-based methods. These methods often have limitations when dealing with the collaborative control of heterogeneous AUV groups, and it is difficult to simultaneously meet consistency requirements and various constraints. In the prior art, Model Predictive Control (MPC) has attracted widespread attention because it can simultaneously handle multi-objective optimization and meet various constraints. However, when MPC is directly applied to heterogeneous AUV group control, due to the high system dimension and strong coupling, it often leads to high computational complexity and difficulty in achieving real-time control.

[0004] Therefore, it is urgent to propose a collaborative navigation control method and device for a heterogeneous AUV group to solve the technical problem in the prior art that when MPC is directly applied to the control of a heterogeneous AUV group, the computational complexity is too high and it is difficult to achieve real-time control due to the high system dimension and strong coupling. Summary of the invention

[0005] In view of this, it is necessary to provide a collaborative navigation control method and device for a heterogeneous AUV group to solve the technical problem in the prior art that when MPC is directly applied to the control of a heterogeneous AUV group, the calculation complexity is too high and it is difficult to achieve real-time control due to the high system dimension and strong coupling.

[0006] In order to solve the above problems, the present invention provides a cooperative navigation control method for a heterogeneous AUV group, comprising:

[0007] Build a linear state space model of AUV, set up the consensus protocol and distributed MPC collaborative navigation control law, and obtain basic data of multiple AUVs;

[0008] Linearizing each basic data according to the linear state space model to obtain a heterogeneous AUV group;

[0009] The heterogeneous AUV group is subjected to collaborative navigation control based on the consistency protocol and the distributed MPC collaborative navigation control law.

[0010] In a possible implementation, constructing a linear state space model of the AUV includes:

[0011] Constructing a dynamic mathematical model of the AUV;

[0012] The dynamic mathematical model is linearly transformed based on the hull parallel coordinate method to obtain a linear state space model of the AUV.

[0013] In a possible implementation, constructing the dynamic mathematical model of the AUV includes:

[0014] Taking the center of gravity of the AUV as the origin of the carrier coordinate system, establishing a carrier coordinate system and a fixed coordinate system, and establishing a motion coordinate system according to the motion process of the AUV in the carrier coordinate system;

[0015] Constructing a six-degree-of-freedom motion model of the AUV according to the carrier coordinate system and the fixed coordinate system;

[0016] The six-degree-of-freedom motion model is transformed according to the motion coordinate system and the fixed coordinate system to obtain a dynamic mathematical model.

[0017] In a possible implementation, the collaborative navigation control of the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law includes:

[0018] Processing the heterogeneous AUV group based on the consistency protocol to obtain a collaborative variable corresponding to each AUV;

[0019] Optimizing and calculating the cooperative variables according to the distributed MPC cooperative navigation control law to obtain the optimal control quantity corresponding to each AUV;

[0020] The AUVs in the heterogeneous AUV group are controlled to perform collaborative navigation according to the optimal control amount.

[0021] In a possible implementation, the processing of the heterogeneous AUV group based on the consistency protocol to obtain the collaborative variables corresponding to each AUV includes:

[0022] Converting the heterogeneous AUV group to obtain a homogeneous AUV group, and determining a pilot boat and at least one other boat in the homogeneous AUV group;

[0023] The coordinated variable of each other boat is determined according to the relative position and relative movement speed of each other boat relative to the pilot boat.

[0024] In a possible implementation, the optimizing calculation of the cooperative variables according to the distributed MPC cooperative navigation control law to obtain the optimal control quantity corresponding to each AUV includes:

[0025] Based on the distributed MPC collaborative navigation control law, the inner loop of the heterogeneous AUV group is subjected to distributed prediction to obtain a model predictive control law in the horizontal direction and a model predictive control law in the vertical direction;

[0026] Constructing a prediction model, and predicting the cooperative variable according to the prediction model to obtain the output control amount of each other boat;

[0027] Solving the model predictive control law in the horizontal direction and the model predictive control law and the cooperative variables in the vertical direction based on the prediction constraints to obtain the optimal solution for each of the other boats;

[0028] The optimal control amount of each of the other boats is obtained according to the optimal solution and the output control amount.

[0029] In a possible implementation, the consistency protocol includes a relative position and a relative motion speed, wherein the relative position is:

[0030]

[0031] In the formula, , and Respectively expressed in x / y / z directions t The acceleration command at the moment, and represents the dimension-matched gain matrix, i and j = 0.1.2.3...N represents the AUV number, Indicated in Moment AUVs The reference motion state, x, y, z Represents each direction, Indicated in t-τ The reference motion state of the pilot boat at all times, represents the i-th AUV at time t-τ x The state of motion in the direction, Indicates j AUV at time t-τ x The state of motion in the direction, , , , , , According to this line, When it is on, it means that there is communication between the AUV and the pilot boat. When the signal is on, it means that there is no communication with the pilot boat. is an element in the system adjacency matrix, Indicates the impact of communication delay, , is a known positive constant;

[0032] and The relative speed of motion is:

[0033]

[0034] In the formula, Expressed as and The relative speed on the x-axis, Expressed as and The relative speed on the y-axis, Expressed as and Relative speed on the z-axis.

[0035] In a possible implementation, the model predictive control law in the horizontal direction is:

[0036]

[0037]

[0038] In the formula, Represents the horizontal direction; is the forward difference, ; For system input, Often written as the discrete integral ; is the coefficient matrix; is the Coriolis force matrix (including the additional mass); is the identity matrix; is a matrix whose coefficients are all 0;

[0039] The model predictive control law in the vertical direction is:

[0040]

[0041]

[0042] In the formula, Represents the vertical direction.

[0043] In a possible implementation, the prediction constraint condition includes:

[0044]

[0045] In the formula, and Represent the predicted lengths of horizontal and vertical plane control respectively.

[0046] On the other hand, the present invention also provides a cooperative navigation control device for a heterogeneous AUV group, comprising:

[0047] The data acquisition module is used to build the linear state space model of the AUV, set the consistency protocol and distributed MPC collaborative navigation control law, and obtain the basic data of multiple AUVs;

[0048] A data processing module, used for performing linearization processing on each basic data according to the linear state space model to obtain a heterogeneous AUV group;

[0049] A collaborative control module is used to perform collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law.

[0050] The beneficial effects of the present invention are: constructing a linear state space model of AUV, setting a consistency protocol and a distributed MPC collaborative navigation control law, and acquiring basic data of multiple AUVs; linearizing each basic data according to a dynamic mathematical model to obtain a heterogeneous AUV group; performing collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law; the present invention processes the heterogeneous AUV group through the consistency protocol, thereby reducing the complexity of calculation, and then controls the heterogeneous AUV group through the distributed MPC collaborative navigation control law, thereby realizing the problem of real-time collaborative navigation control of the heterogeneous AUV group. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 A schematic flow chart of an embodiment of a cooperative navigation control method for a heterogeneous AUV group provided by the present invention;

[0052] Figure 2 For the present invention Figure 1 A schematic flow chart of an embodiment of step S103;

[0053] Figure 3 A schematic diagram of the structure of an embodiment of the formation provided by the present invention;

[0054] Figure 4 A schematic diagram of the structure of an embodiment of the cooperative navigation control provided by the present invention;

[0055] Figure 5 A schematic structural diagram of an embodiment of a cooperative navigation control device for a heterogeneous AUV group provided by the present invention;

[0056] Figure 6 A schematic structural diagram of an embodiment of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0057] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0058] like Figure 1 As shown, a specific embodiment of the present invention discloses a cooperative navigation control method for a heterogeneous AUV group, comprising:

[0059] S101, construct a linear state space model of AUV, set a consistency protocol and distributed MPC collaborative navigation control law, and obtain basic data of multiple AUVs;

[0060] S102, performing linearization processing on each basic data according to a linear state space model to obtain a heterogeneous AUV group;

[0061] S103, performing collaborative navigation control on a heterogeneous AUV group based on a consistency protocol and a distributed MPC collaborative navigation control law.

[0062] It should be understood that it can be applied to resource exploration, marine environment monitoring, underwater search and rescue and other fields, and a variety of data models can be constructed, such as AUV model and linear state space model, etc. The AUV model can be constructed by software, and the basic data can be the real-time data of the AUV or the historical data stored historically. The specific settings can be made according to the actual situation, and the embodiments of the present invention are not limited here.

[0063] In a specific embodiment of the present invention, a linear state space model can be constructed according to actual conditions, a consistency protocol can be a protocol set up considering communication delays, and a distributed MPC collaborative navigation control law can be a model-decoupling-based control law designed based on a model predictive control (MPC) method and suitable for an AUV group. The AUV model can be constructed by software, and then the basic data of each AUV can be imported into the AUV model. Then, a linear state space model of each AUV can be obtained, thereby forming a heterogeneous AUV group. Then, each AUV in the heterogeneous AUV group can be controlled based on the consistency protocol and the distributed MPC collaborative navigation control law, thereby completing collaborative navigation control.

[0064] Compared with the prior art, the present embodiment provides a method for constructing a linear state space model of AUV, setting a consistency protocol and a distributed MPC collaborative navigation control law, and obtaining basic data of multiple AUVs; linearizing each basic data according to a dynamic mathematical model to obtain a heterogeneous AUV group; performing collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law; the present invention processes the heterogeneous AUV group through a consistency protocol, thereby reducing the complexity of calculation, and then controls the heterogeneous AUV group through the distributed MPC collaborative navigation control law, thereby realizing the problem of real-time collaborative navigation control of the heterogeneous AUV group.

[0065] In some embodiments of the present invention, step S101 includes:

[0066] Construct a dynamic mathematical model of the AUV;

[0067] The dynamic mathematical model is linearly transformed based on the hull parallel coordinate method to obtain the linear state space model of the AUV.

[0068] In a specific embodiment of the present invention, in order to simulate the AUV, a dynamic mathematical model of the AUV may be constructed.

[0069] In some embodiments of the present invention, a dynamic mathematical model of an AUV is constructed, including:

[0070] The center of gravity of the AUV is used as the origin of the carrier coordinate system to establish the carrier coordinate system and the fixed coordinate system, and the motion coordinate system is established according to the motion process of the AUV in the carrier coordinate system;

[0071] According to the carrier coordinate system and the fixed coordinate system, a six-degree-of-freedom motion model of the AUV is constructed;

[0072] The six-degree-of-freedom motion model is transformed according to the motion coordinate system and the fixed coordinate system to obtain a dynamic mathematical model.

[0073] In a specific embodiment of the present invention, the center of gravity of the AUV can be selected as the origin of the carrier coordinate system to establish the carrier coordinate system and the fixed coordinate system. The motion coordinate system can also be established according to the motion process of the AUV in the carrier coordinate system, so that the six-degree-of-freedom motion model of the AUV can be obtained according to the carrier coordinate system and the fixed coordinate system, as shown in formula (1):

[0074] (1)

[0075] In the formula, is the spatial position and attitude of the AUV in a fixed coordinate system, are the linear velocity and angular velocity of the AUV in the motion coordinate system, is the rotation transformation matrix from the moving coordinate system to the fixed coordinate system, is the inertia matrix of the system (including the additional mass), is the Coriolis force matrix (including the additional mass), is the damping matrix, are the gravity / buoyancy force and the torque vector, are thrust and torque vectors, External interference and internal disturbance.

[0076] According to the fixed coordinate system and the moving coordinate system of the AUV, the six-degree-of-freedom motion model of the AUV is converted into a dynamic mathematical model. Assuming that the origins of the two coordinate systems coincide, the postures in the two coordinate systems, that is, the dynamic mathematical model of the AUV, are shown in formula (2):

[0077] (2)

[0078] in, , , .

[0079] In the formula, represents the position vector, Indicates the north position (m), R andT They represent the rotation matrix and the translation matrix respectively. represents the east position (m), represents the depth (m), represents the posture vector, is the roll angle (rad), represents the pitch angle (rad), Indicates the heading angle (rad). represents the linear velocity vector, represents the longitudinal velocity (m / s), represents the lateral velocity (m / s), represents the vertical velocity (m / s), represents the angular velocity vector, represents the roll angular velocity (rad / s), represents the pitch angular velocity (rad / s), It represents the heading angular velocity (rad / s). is the linear velocity conversion matrix, which is the conversion matrix from the moving coordinate system to the fixed coordinate system. is the angular velocity conversion matrix, which is the conversion matrix from the moving coordinate system to the fixed coordinate system. , The definition of is shown in formula (3):

[0080] (3)

[0081] The AUV dynamics model studies the changes in the motion state of the AUV after the AUV is subjected to force and torque. Combined with the six-degree-of-freedom motion model of the AUV, ignoring the interference effect, it can be expressed by formula (4):

[0082] (4)

[0083] in, .

[0084] In the formula, represents the external force vector, represents the AUV longitudinal external force (N), represents the lateral external force on the AUV (N), represents the external torque vector, represents the vertical external force on AUV (N), is the rolling moment (N), is the pitch moment (N), represents the yaw moment (N). Positive definite inertia matrix It is composed of mass, moment of inertia matrix and additional mass matrix, and the centripetal force matrix It is composed of the centripetal force matrix of the AUV and the Coriolis centripetal matrix generated by the additional mass.

[0085] Considering the actual application of engineering, the mathematical model of AUV has nonlinear strong coupling characteristics, which greatly increases the difficulty of designing the corresponding motion controller. Here, a simplified model suitable for the study of AUV control algorithm is designed, but it can still describe the basic characteristics of the system. The nonlinear model is converted into a linear system model using the Vessel Parallel Coordinates (VPC) method. VPC assumes and This is an acceptable approximation for a stable AUV, and we can get formula (5):

[0086] (5)

[0087] in, , .

[0088] In the formula, is the rotation matrix in the heading direction, P(ψ) It represents the percentage of the length of the parallel midbody to the length of the ship. The VPC coordinate system can be defined as shown in formula (6):

[0089] (6)

[0090] Assume that the speed of the AUV during stable navigation is , when the steering angular velocity is very small, that is , formula (7) can be further transformed into The linear equation is:

[0091] (7)

[0092] Since the AUV is sailing at a low speed, it can be seen that when the AUV is sailing steadily , combined with formula (1) and ignoring external disturbances, the linear state space model of AUV can be obtained as shown in formula (8):

[0093] (8)

[0094] In the formula, is the coordinate system of VP. is the linear velocity and angular velocity of the AUV in the motion coordinate system. Assume that the speed of the AUV during stable navigation is , when the steering angular velocity is very small, it can be considered . is the invariant set of the dynamic system. is the damping matrix, are the gravity / buoyancy force and the torque vector, are the thrust and torque vectors.

[0095] Use standard form , the AUV motion equation can be transformed into a linear state space represented by a fixed coordinate system, and its linear standard second-order model is shown in formula (9):

[0096] (9)

[0097] The control system output is as shown in formula (10):

[0098] (10)

[0099] In the formula, , Indicates the AUV number, It is the pilot boat, and its motion state is not affected by the following boat. is the state variable of AUV, is the control input, To measure the output, represents the algebraic Riccati equation, are known system parameters, and for the AUV group, The matrix dimensions are consistent, , .

[0100] Furthermore, after obtaining the above linear state space model, the basic data of each AUV can be linearized to obtain a heterogeneous AUV group; the heterogeneous AUV group considered in the embodiment of the present invention has the same system state dimension but different system parameters.

[0101] In some embodiments of the present invention, Figure 2 As shown, step S103 includes:

[0102] S201, processing the heterogeneous AUV group based on the consistency protocol to obtain the collaborative variables corresponding to each AUV;

[0103] S202, optimizing and calculating the cooperative variables according to the distributed MPC cooperative navigation control law to obtain the optimal control quantity corresponding to each AUV;

[0104] S203: Perform cooperative navigation control on the AUVs in the heterogeneous AUV group according to the optimal control amount.

[0105] In a specific embodiment of the present invention, in the consistency protocol, the pilot boat is used as a reference point, and other AUVs (following boats) adjust their own motion states according to the relative position and speed information between them and the pilot boat to maintain the formation. The formation is as follows: Figure 3 As shown, the heterogeneous AUV group may include AUV0, AUV1, AUV2, AUV3 and AUV4, the pilot boat is AUV0, and the follower boats are AUV1, AUV2, AUV3 and AUV4. Figure 4 As shown in the figure, the consensus protocol can be used to calculate the pilot boat (i.e., pilot boat information) and follower boat (i.e., own boat information) as well as the scheduled navigation information to obtain the cooperative variables (including position, speed, and attitude) of each AUV. Then, the cooperative variables can be used as the input of the cooperative navigation controller. The distributed MPC cooperative navigation control law of the cooperative navigation controller is used to optimize the cooperative variables, so that the optimal control quantity corresponding to each AUV can be output, and then the optimal control quantity can be used to control the corresponding AUV. i Control is performed to achieve collaborative navigation control of AUVs in a heterogeneous AUV group.

[0106] In some embodiments of the present invention, step S201 includes:

[0107] The heterogeneous AUV group is transformed to obtain a homogeneous AUV group, and a pilot boat and at least one other boat in the homogeneous AUV group are determined;

[0108] The coordination variable of each other boat is determined according to the relative position and relative movement speed of each other boat relative to the pilot boat.

[0109] In a specific embodiment of the present invention, for a heterogeneous AUV group, the heterogeneous AUV group is converted into a homogeneous AUV group, and then the consistency protocol of the heterogeneous AUV group is designed based on the consistency concept of the homogeneous system, wherein the heterogeneous AUV system conversion equation is shown in formula (11):

[0110] (11)

[0111] In the formula, are coefficient matrices, is the parameter transfer matrix of the heterogeneous AUV group, , from the AUV motion model, we know that They all have the same structural form as formula (12):

[0112] (12)

[0113] In the formula, is a full row rank matrix.

[0114] exist , satisfying formula (13):

[0115] (13)

[0116] exist , satisfying formula (14):

[0117] (14)

[0118] In the formula, , and represents the gain matrix of dimension matching; and is the state matrix of the heterogeneous AUV group.

[0119] The formation consistency goal is set. If formula (15) is satisfied, the AUV formation can be said to be consistent. At this time, the motion state of all follower boats is consistent with that of the lead boat, and the system convergence and stability are continuously guaranteed.

[0120] (15)

[0121] In the formula, Moment AUVs The reference motion state is , the motion state of the pilot boat is .

[0122] The consistency protocol includes relative position and relative movement speed. According to the formation consistency goal and considering the influence of communication delay, the consistency protocol of the relative position and relative movement speed of the three positions is shown in formulas (16) and (17):

[0123] (16)

[0124] In the formula, , and Respectively expressed in x / y / z directions t The acceleration command at the moment, and represents the dimension-matched gain matrix, i and j = 0.1.2.3...N represents the AUV number, Indicated in Moment AUVs The reference motion state, x, y, z Represents each direction, Indicated in t-τ The reference motion state of the pilot boat at all times, represents the i-th AUV at time t-τ x The state of motion in the direction, Indicates j AUV at time t-τ x The state of motion in the direction, , , , , , According to this line, When it is on, it means that there is communication between the AUV and the pilot boat. When the signal is on, it means that there is no communication with the pilot boat. is an element in the system adjacency matrix, Indicates the impact of communication delay, , is a known positive constant;

[0125] and The relative speed of motion is:

[0126] (17)

[0127] In the formula, Expressed as and The relative speed on the x-axis, Expressed as and The relative speed on the y-axis, Expressed as and Relative speed on the z-axis.

[0128] The position coordination variables of each boat are different at the beginning of the voyage. As time goes by, When , the coordinated variables tend to be consistent. In order to make the control clearer, the control of the heading angle and the trim angle is increased. Converted to more common navigation instructions , firstly, the speed control method is given as shown in formula (18):

[0129] (18)

[0130] According to the relationship between the pitch angle, heading angle and velocity component Take the derivative, as shown in formula (19):

[0131] (19)

[0132] (20)

[0133] Combining the control instructions of formulas (18), (19) and (20) and giving appropriate navigation controllers, formation keeping can be achieved. New collaborative control instructions are defined for each AUV. (that is, the cooperative variables of each other boat), after integration operation, the input of the cooperative navigation controller is obtained:

[0134] (twenty one)

[0135] In some embodiments of the present invention, step S202 includes:

[0136] Based on the distributed MPC collaborative navigation control law, the inner loop of the heterogeneous AUV group is predicted in a distributed manner to obtain the model predictive control law in the horizontal direction and the model predictive control law in the vertical direction.

[0137] Construct a prediction model, and predict the cooperative variables according to the prediction model to obtain the output control amount of each other boat;

[0138] Based on the prediction constraints, the model predictive control law in the horizontal direction and the model predictive control law and cooperative variables in the vertical direction are solved to obtain the optimal solution for each other boat;

[0139] According to the optimal solution and the output control quantity, the optimal control quantity of each other boat is obtained.

[0140] In a specific embodiment of the present invention, based on the MPC control method, a distributed MPC cooperative navigation control law based on model decoupling suitable for a heterogeneous AUV group is designed. The MPC control method is distributedly processed, and the description of the AUV system is divided into two subsystems with smaller interactions, horizontal and vertical. Different performance index functions can be designed according to the constraints for different optimization objectives of the two subsystems, so as to obtain the optimal solution that meets the control requirements. Specifically, according to the six-degree-of-freedom motion equation of the AUV, the subsystem state of the vertical plane includes the system state related to heave and trim, that is, By analyzing the AUV motion equation, when considering the vertical plane motion, it is assumed that The value is relatively small and can be ignored, so the vertical plane kinematic equation can be obtained as shown in formula (22):

[0141] (twenty two)

[0142] In the formula, is the relative velocity in the depth direction; is the longitudinal velocity; is the vertical speed; is the pitch angle attitude vector; is the pitch angular velocity; For quality; is the vertical velocity attitude vector; is the vertical force; , etc. all represent dimensionless hydrodynamic parameters of the hull; is the pitch moment matrix; for The derivation of is the pitch moment change matrix; is the moment of inertia matrix in the east direction; is the vertical rudder angle.

[0143] Converting formula (22) into a continuous state space model is shown in formula (23):

[0144] (twenty three)

[0145] in, , is the state vector; , is the system input; , is the system state; the coefficient matrix is ​​shown in formulas (24)-(26):

[0146] (twenty four)

[0147] (25)

[0148] (26)

[0149] In the formula, is the pitch angle attitude vector; is the pitch angular velocity; For quality; is the vertical velocity attitude vector; is the vertical force; , etc. all represent dimensionless hydrodynamic parameters of the hull; is the pitch angle moment matrix; is the moment of inertia matrix in the east direction; is the vertical force at vertical rudder angle; is the pitch moment at vertical rudder angle.

[0150] The subsystem in the horizontal plane includes the system states related to the north position, east position and heading, i.e. When considering the horizontal plane movement, due to The value is relatively small and can be ignored, so the horizontal plane kinematic equation can be obtained as shown in formula (27):

[0151] (27)

[0152] In the formula, is the longitudinal velocity; is the lateral velocity; is the heading angle; is the pitch angle; is the heading angular velocity; , are the longitudinal force in the north direction and the lateral force in the east direction respectively; , etc. all represent dimensionless hydrodynamic parameters of the hull; For quality; is the translation matrix; is the horizontal rudder angle; is the lateral force; is the lateral angular moment; is the yaw moment; is the longitudinal moment of inertia matrix.

[0153] Converting formula (27) into a continuous state space model is shown in formula (28):

[0154] (28)

[0155] in, is the state vector; Input for the system; is the system state; the coefficient matrix is ​​shown in formulas (29)-(31):

[0156] (29)

[0157] (30)

[0158] (31)

[0159] According to the decoupling of the horizontal and vertical motions of the AUV model, the model predictive control law in the horizontal direction and the model predictive control law in the vertical direction are constructed in combination with the MPC control theory, as shown in formulas (32)-(35):

[0160] (32)

[0161] (33)

[0162] (34)

[0163] (35)

[0164] In the formula, the subscript Represent the horizontal plane, vertical plane, discretization, and predicted value respectively. is a coefficient matrix, such as is the coefficient matrix for horizontal discretization ; is the Coriolis force matrix (including the additional mass); is the identity matrix, is a zero matrix. The MPC method takes into account a variety of complex motion constraints such as AUV state, output, and actuators, and achieves precise control of the AUV by optimizing the control sequence. The prediction model is as shown in formula (36), and the following two vectors are defined:

[0165] (36)

[0166] In the formula, the subscript Corresponding to the horizontal and vertical planes respectively, and denote the controlled and predicted lengths of the horizontal and vertical planes, respectively. Based on the new state-space model, the output vector can be written in the matrix form of formula (37):

[0167] (37)

[0168] In the formula, All are system output vectors; They represent horizontal plane, vertical plane, discretization, predicted value, and control value (or reference value) respectively; For discrete system input; are coefficient matrices, such as Coefficient matrix representing the discrete horizontal plane , Indicates the predicted length of the horizontal and vertical planes; They represent the lengths of horizontal plane prediction and control, and vertical plane prediction and control respectively.

[0169] The quadratic performance index function is constructed as shown in formula (38):

[0170] (38)

[0171] In the formula, is the cost indicator function; is the output indicator, T is the transposed matrix, H is the system output vector, for Transpose of a matrix; is the predicted output value of the system at a certain moment in the future, is the size of the reference signal at the future moment, is the system status, It is the control signal at the future moment obtained after optimizing the cost function. is the system output vector. In addition, is a diagonal positive definite matrix, which is used as the weight in the indicator function and is a manually set weight value. When it is 0, the change of the control quantity does not affect the performance index. When it is not 0, it will affect the performance index. Taking the partial derivative of the performance index function and making its derivative zero, the optimal output control quantity can be obtained. , define the objective function as shown in formula (39):

[0172] (39)

[0173] For AUV, the thrust and rudder angle it generates are limited, and can be divided into static constraints and dynamic constraints. Static constraints refer to the constraints on the maximum thrust and maximum rudder angle, that is, input saturation, as shown in formula (40):

[0174] (40)

[0175] In the formula, Indicates thrust, Indicates the rudder angle. Contains inputs for thrust and rudder angle The constraint is shown in formula (41):

[0176] (41)

[0177] Dynamic constraint means that the change rate of thrust and rudder angle per unit time is limited, that is, the input change rate is saturated, as shown in formula (42):

[0178] (42)

[0179] For the navigation safety of AUV, the output needs to be constrained, as shown in formula (43):

[0180] (43)

[0181] Considering that the cost function is based on the control signal changes, constraints and restrictions must be transformed into Due to the constraints and restrictions , we can get formula (44):

[0182] (44)

[0183] Recursively applying the above formula, we can obtain formulas (45) and (46):

[0184] (45)

[0185] (46)

[0186] In the formula, , represents the minimum and maximum set of the control quantity in the control time domain, and T is an upper triangular matrix whose non-zero elements are 1. Combining the output equation and the output quantity constraints, we can get:

[0187] (47)

[0188] The final definition of the constraint condition (i.e., the prediction constraint condition) is shown in formula (48):

[0189] (48)

[0190] Furthermore, by using the model predictive control law in the horizontal direction and the model predictive control law in the vertical direction (Formula (33)-(35)) under the condition of the prediction constraint (Formula (48)), the coordinated variables can be predicted to obtain the optimal solution (i.e. y h ( k )and y v ( k ), the prediction model formulas (36)-(39) are used to solve the cooperative variables under the prediction constraint formula (48), and the output control quantity of each other boat can be obtained (i.e. ), in the process of calculating the output control quantity, the optimal solution can be brought into the prediction model formula (36)-(39) for calculation (that is, using the subscript Corresponding to the horizontal plane and the vertical plane respectively), the calculated output control amount of each other boat is the optimal control amount. Specifically, the optimal solution is to calculate the optimal control amount through the coordinates of the previous moment, and combine it with the control amount of the previous moment to obtain the optimal control amount at this moment, so as to realize the consistent control of the heterogeneous AUV group.

[0191] By designing a heterogeneous AUV group consistency protocol, the embodiment of the present invention can accurately maintain the formation and navigation trajectory of the heterogeneous AUV group, ensuring the collaborative accuracy and stability of the group when performing complex tasks; by decoupling the model, the dynamic description of the AUV is divided into two subsystems, horizontal and vertical, and control laws are designed respectively, thereby improving the computational efficiency and real-time performance of the control algorithm and meeting the rapidity required in practical applications.

[0192] In order to better implement the cooperative navigation control method of a heterogeneous AUV group in the embodiment of the present invention, based on the cooperative navigation control method of a heterogeneous AUV group, the embodiment of the present invention also provides a cooperative navigation control device of a heterogeneous AUV group, such as Figure 5 As shown, the cooperative navigation control device 500 of a heterogeneous AUV group includes:

[0193] The data acquisition module 501 is used to construct a linear state space model of the AUV, set a consistency protocol and a distributed MPC collaborative navigation control law, and obtain basic data of multiple AUVs;

[0194] The data processing module 502 is used to perform linearization processing on each basic data according to the linear state space model to obtain a heterogeneous AUV group;

[0195] The collaborative control module 503 is used to perform collaborative navigation control on a heterogeneous AUV group based on a consistency protocol and a distributed MPC collaborative navigation control law.

[0196] The collaborative navigation control device 500 for a heterogeneous AUV group provided in the above embodiment can implement the technical solution described in the above embodiment of the collaborative navigation control method for a heterogeneous AUV group. The specific implementation principles of the above modules or units can refer to the corresponding contents in the above embodiment of the collaborative navigation control method for a heterogeneous AUV group, which will not be repeated here.

[0197] like Figure 6 As shown, the present invention also provides an electronic device 600. The electronic device 600 includes a processor 601, a memory 602 and a display 603. Figure 6 Only some components of the electronic device 600 are shown, but it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0198] In some embodiments, the memory 602 may be an internal storage unit of the electronic device 600, such as a hard disk or memory of the electronic device 600. In other embodiments, the memory 602 may also be an external storage device of the electronic device 600, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc., equipped on the electronic device 600.

[0199] Furthermore, the memory 602 may include both an internal storage unit of the electronic device 600 and an external storage device. The memory 602 is used to store application software installed in the electronic device 600 and various data.

[0200] In some embodiments, the processor 601 may be a central processing unit (CPU), a microprocessor or other data processing chip, and is used to run program codes or process data stored in the memory 602, such as the cooperative navigation control method of a heterogeneous AUV group in the present invention.

[0201] In some embodiments, the display 603 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode) touch device, etc. The display 603 is used to display information of the electronic device 600 and to display a visual user interface. The components 601-603 of the electronic device 600 communicate with each other through a system bus.

[0202] In some embodiments of the present invention, when the processor 601 executes the cooperative navigation control program of the heterogeneous AUV group in the memory 602, the following steps may be implemented:

[0203] Build a linear state space model of AUV, set up the consensus protocol and distributed MPC collaborative navigation control law, and obtain basic data of multiple AUVs;

[0204] Each basic data is linearized according to the linear state space model to obtain a heterogeneous AUV group;

[0205] Cooperative navigation control of heterogeneous AUV groups is carried out based on the consensus protocol and distributed MPC cooperative navigation control law.

[0206] It should be understood that: when the processor 601 executes the cooperative navigation control program of the heterogeneous AUV group in the memory 602, in addition to the above functions, other functions can also be implemented. For details, please refer to the description of the corresponding method embodiment above.

[0207] Furthermore, the embodiment of the present invention does not specifically limit the type of the electronic device 600 mentioned, and the electronic device 600 may be a portable electronic device such as a mobile phone, a tablet computer, a personal digital assistant (PDA), a wearable device, a laptop computer, etc. Exemplary embodiments of portable electronic devices include but are not limited to portable electronic devices equipped with IOS, Android, Microsoft or other operating systems. The above-mentioned portable electronic devices may also be other portable electronic devices, such as a laptop computer with a touch-sensitive surface (e.g., a touch panel). It should also be understood that in some other embodiments of the present invention, the electronic device 600 may not be a portable electronic device, but a desktop computer with a touch-sensitive surface (e.g., a touch panel).

[0208] Accordingly, an embodiment of the present application also provides a computer-readable storage medium, which is used to store computer-readable programs or instructions. When the program or instructions are executed by a processor, it can implement the steps or functions of the collaborative navigation control method for a heterogeneous AUV group provided in the above-mentioned method embodiments.

[0209] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware (such as a processor, a controller, etc.) through a computer program, and the computer program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0210] The above is a detailed introduction to the collaborative navigation control method and device for a heterogeneous AUV group provided by the present invention. In the embodiments of the present invention, specific examples are used to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scopes. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A collaborative navigation control method for a heterogeneous AUV group, characterized in that: include: Build a linear state space model of AUV, set up the consensus protocol and distributed MPC collaborative navigation control law, and obtain basic data of multiple AUVs; Linearizing each basic data according to the linear state space model to obtain a heterogeneous AUV group; Performing collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law; The consistency protocol includes relative position and relative motion speed, wherein the relative position is: In the formula, , and Respectively expressed in x / y / z directions t The acceleration command at the moment; and represents the dimension-matched gain matrix; i and j = 0.1.2.3...N represents the AUV number; Indicated in Moment AUVs The reference motion state of x, y, z Represents each direction respectively; Indicated in t-τ The reference motion state of the pilot boat at all times; Indicates i AUVs in t-τ time x The state of motion of the direction; Indicates j AUVs in t-τ time x The state of motion of the direction; , , , , , According to this line of reasoning; When it is on, it indicates that there is communication between the AUV and the pilot boat; When , it means that there is no communication with the pilot boat; is an element in the system adjacency matrix; Indicates the impact of communication delay; , is a known positive constant; and The relative speed of motion is: In the formula, Expressed as and Relative speed on the x-axis; Expressed as and Relative speed on the y-axis; Expressed as and Relative speed on the z-axis; Expressed as The speed of movement on the x-axis; Expressed as The speed of movement on the x-axis; Expressed as The speed of movement on the z-axis; Expressed as The speed of movement on the z-axis; Expressed as The speed of movement on the y-axis; Expressed as The speed of movement on the y-axis.

2. The cooperative navigation control method of a heterogeneous AUV group according to claim 1, characterized in that: The construction of the linear state space model of the AUV comprises: Constructing a dynamic mathematical model of the AUV; The dynamic mathematical model is linearly transformed based on the hull parallel coordinate method to obtain a linear state space model of the AUV.

3. The cooperative navigation control method of a heterogeneous AUV group according to claim 2, characterized in that: The construction of the dynamic mathematical model of the AUV comprises: Taking the center of gravity of the AUV as the origin of the carrier coordinate system, establishing a carrier coordinate system and a fixed coordinate system, and establishing a motion coordinate system according to the motion process of the AUV in the carrier coordinate system; Constructing a six-degree-of-freedom motion model of the AUV according to the carrier coordinate system and the fixed coordinate system; The six-degree-of-freedom motion model is transformed according to the motion coordinate system and the fixed coordinate system to obtain a dynamic mathematical model.

4. The cooperative navigation control method of a heterogeneous AUV group according to claim 1, characterized in that: The method of performing collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law includes: Processing the heterogeneous AUV group based on the consistency protocol to obtain a collaborative variable corresponding to each AUV; Optimizing and calculating the cooperative variables according to the distributed MPC cooperative navigation control law to obtain the optimal control quantity corresponding to each AUV; The AUVs in the heterogeneous AUV group are controlled to perform collaborative navigation according to the optimal control amount.

5. The cooperative navigation control method of a heterogeneous AUV group according to claim 4, characterized in that: The processing of the heterogeneous AUV group based on the consistency protocol to obtain the collaborative variables corresponding to each AUV includes: Converting the heterogeneous AUV group to obtain a homogeneous AUV group, and determining a pilot boat and at least one other boat in the homogeneous AUV group; The coordinated variable of each other boat is determined according to the relative position and relative movement speed of each other boat relative to the pilot boat.

6. The cooperative navigation control method of a heterogeneous AUV group according to claim 5, characterized in that: The step of optimizing and calculating the cooperative variables according to the distributed MPC cooperative navigation control law to obtain the optimal control quantity corresponding to each AUV includes: Based on the distributed MPC collaborative navigation control law, a distributed prediction is performed on the inner loop of the heterogeneous AUV group to obtain a model predictive control law in the horizontal direction and a model predictive control law in the vertical direction; Constructing a prediction model, and predicting the cooperative variable according to the prediction model to obtain the output control amount of each other boat; Solving the model predictive control law in the horizontal direction, the model predictive control law in the vertical direction and the coordinated variables based on the prediction constraints to obtain the optimal solution for each of the other boats; The optimal control amount of each of the other boats is obtained according to the optimal solution and the output control amount.

7. The cooperative navigation control method of a heterogeneous AUV group according to claim 6, characterized in that: The model predictive control law in the horizontal direction is: In the formula, Represents the horizontal direction; is the forward difference of the horizontal plane, ; System input for the horizontal plane; Often written as the discrete integral ; is the coefficient matrix; is the Coriolis force matrix, including the additional mass; is the identity matrix; is a matrix whose coefficients are all 0; The state vector representing the state of the system in the horizontal plane; The output vector represented as the system output in the horizontal plane; Output vector representing the system output in the vertical plane; A Hd is the coefficient matrix for horizontal discretization A ; B Hd is the coefficient matrix for horizontal discretization B ; C Hd is the coefficient matrix C discretized in the horizontal direction; The model predictive control law in the vertical direction is: In the formula, Represents the vertical direction; A Vd is the coefficient matrix for vertical discretization A ; B Vd is the coefficient matrix for vertical discretization B ; C Vd is the coefficient matrix for vertical discretization C ; System input expressed as a vertical plane; is the state variable of the system state in the vertical direction.

8. The cooperative navigation control method of a heterogeneous AUV group according to claim 6, characterized in that: The prediction constraints include: In the formula, and Represent the predicted lengths of horizontal and vertical plane control respectively; U H,Vmin , U max Represents the minimum and maximum value set of the control quantity in the control time domain; T is an upper triangular matrix with all non-zero elements being 1; The identity matrix row 1 column; The identity matrix row 1 column; , , Represent discrete system input, minimum system input, and maximum system input respectively; The system input in the horizontal and vertical directions at time t-1; y H,V,min and y H,V,max Respectively represent the minimum and maximum values ​​of the system output; is the state vector of the horizontal and vertical planes at time k.

9. A cooperative navigation control device for a heterogeneous AUV group, characterized in that: include: The data acquisition module is used to build the linear state space model of the AUV, set the consistency protocol and distributed MPC collaborative navigation control law, and obtain the basic data of multiple AUVs; A data processing module, used for performing linearization processing on each basic data according to the linear state space model to obtain a heterogeneous AUV group; A collaborative control module, used for performing collaborative navigation control on the heterogeneous AUV group based on the consistency protocol and the distributed MPC collaborative navigation control law; The consistency protocol includes relative position and relative motion speed, wherein the relative position is: In the formula, , and Respectively expressed in x / y / z directions t The acceleration command at the moment; and represents the dimension-matched gain matrix; i and j = 0.1.2.3...N represents the AUV number; Indicated in Moment AUVs The reference motion state of x, y, z Represents each direction respectively; Indicated in t-τ The reference motion state of the pilot boat at all times; Indicates i AUVs in t-τ time x The state of motion of the direction; Indicates j AUVs in t- τ time x The state of motion of the direction; , , , , , According to this line of reasoning; When it is on, it indicates that there is communication between the AUV and the pilot boat; When , it means that there is no communication with the pilot boat; is an element in the system adjacency matrix; Indicates the impact of communication delay; , is a known positive constant; and The relative speed of motion is: In the formula, Expressed as and Relative speed on the x-axis; Expressed as and Relative speed on the y-axis; Expressed as and Relative speed on the z-axis; Expressed as The speed of movement on the x-axis; Expressed as The speed of movement on the x-axis; Expressed as The speed of movement on the z-axis; Expressed as The speed of movement on the z-axis; Expressed as The speed of movement on the y-axis; Expressed as The speed of movement on the y-axis.

Images