UUV cluster formation control method under complex ocean current disturbance and unstable communication scenarios

Abstract

This invention discloses a UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios, belonging to the field of UUV swarm formation control technology; it includes the following steps: constructing a formation mode of leader and follower, adopting a speed coordination algorithm, and calculating the expected path data and expected speed of follower UUVs based on the expected path data and formation data of the leader UUV; based on the state data of the leader UUV; its key technical points are: this invention combines an improved ALOS algorithm, a speed coordination algorithm, and a trajectory prediction algorithm to ensure the smooth operation of UUV swarm formation tasks, effectively solving the problems of ocean current interference during UUV navigation in complex marine environments and the communication delays and packet loss that occur when multiple UUVs exchange information via underwater acoustics, thus ensuring the smooth operation of UUV swarm formation tasks and showing good application prospects.

Description

Technical Field

[0001] This invention relates to the field of UUV swarm formation control technology, specifically a UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios. Background Technology

[0002] With the continuous development of intelligent technology, unmanned underwater vehicle (UUV) swarm control has become the most representative cutting-edge direction in the field of underwater unmanned systems in recent years. Its core objective is to design effective cooperative strategies to enable multiple UUVs to maintain geometric formation navigation in the underwater environment, thereby overcoming the inherent limitations of a single UUV platform in terms of perception range, endurance, communication distance, and mission adaptability. This will enable the efficient completion of complex tasks such as large-scale search and detection, persistent surveillance, cooperative mine sweeping, cooperative anti-submarine warfare, topographic mapping, and seabed resource exploration. However, existing UUV swarm control technologies still face many challenges in practical applications, especially in terms of interference in complex underwater environments and unstable underwater acoustic communication.

[0003] While various UUV swarm control methods have been proposed in the existing technology, certain problems and shortcomings still exist in practical applications under conditions of complex ocean current disturbances and unstable communication. Most studies make overly idealistic assumptions about external disturbances, making it difficult to effectively compensate for nonlinear interference caused by unknown time-varying ocean currents. This leads to significant formation drift, oscillations, and even instability in strong ocean current environments, affecting the accuracy and stability of mission execution. Existing UUV swarm control strategies usually rely on reliable underwater acoustic communication to achieve UUV state information exchange and coordination. In actual underwater environments with low communication bandwidth, high latency, and packet loss, this can easily cause information delays, synchronization failures, or failure to achieve consistent convergence. Follower UUVs cannot maintain the desired relative position in a timely and accurate manner, thus failing to achieve UUV swarm control.

[0004] In summary, existing UUV swarm control methods do not meet market requirements. Therefore, we propose a swarm control method that combines velocity coordination based on position error, improved adaptive line-of-sight guidance, and trajectory prediction algorithms, aiming to ensure the formation mission of UUV swarms under complex marine environments and unstable communication conditions. Summary of the Invention

[0005] To achieve the above objectives, the present invention provides the following technical solution: A method for UUV swarm formation control under complex ocean current disturbances and unstable communication scenarios includes the following steps: A formation pattern for leaders and followers is constructed, and a speed collaboration algorithm is adopted to calculate the expected path data and expected speed of follower UUVs based on the expected path data and formation data of the leader UUV. Based on the state data of the leader UUV, a trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, thereby obtaining the accurate predicted position of the leader UUV at the current moment. Based on the accurate predicted position of the leader UUV at the current moment, the expected velocity of the follower UUV is recalculated to obtain the corrected expected velocity of the follower UUV. An improved ALOS algorithm is used to process the expected path data, the accurate predicted position of the leader UUV at the current moment, and the corrected expected velocity of the follower UUV, to obtain the expected vertical angle and yaw angle of the leader UUV and the expected vertical angle and yaw angle of the follower UUV. Based on the PID control algorithm, the desired speed of the leader UUV, the corrected desired speed of the follower UUV, the desired vertical angle and yaw angle of the leader UUV, and the desired vertical angle and yaw angle of the follower UUV are processed. The actual speed, vertical angle, and yaw angle of the leader UUV and the follower UUV are adjusted to the desired values ​​to complete the formation control.

[0006] Preferably, the expected path data for calculating the follower UUV includes: Define formation information, where the formation spacing is... The formation's corners are Define the desired flight path information of the leader UUV as follows: The current location of the leader UUV is ; The expected path data of the follower UUVs is calculated based on the defined formation information. The specific formula includes: ; In the formula, For the expected path data of follower UUVs, Information on the expected flight path of the leader UUV. , The tangent angle of the current path; During UUV convoy navigation, the desired position of the following UUV can be represented as: ; In the formula, Let R be the desired position of the following UUVs during UUV formation navigation, and let R be the rotation matrix. The expression for R is: .

[0007] Preferably, calculating the expected velocity of the follower UUV includes: The expected velocity of the follower UUV is calculated based on its expected position, as follows: ; In the formula, The vertical and horizontal coordinates of the current position of the UUV following the user are given. The ordinates and abscissas of the desired position of the follower UUV are given. The current position of the follower UUV. For the desired sailing speed of the formation, It is a positive proportional gain coefficient. Let e ​​be the angle between the line connecting the actual position and the desired position of the follower UUV and the desired path, and let e be the position error.

[0008] Preferably, the trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, including: Define the continuous state vector of the leader UUV in three-dimensional space. The state vector includes position (xyz), velocity (v), and heading angle. and pitch angle ; Establish the discretized state transition equation, specifically as follows: ; in, It is a nonlinear kinematic function. It has a mean of zero and a covariance of The process uses Gaussian white noise; k is the number of discrete time steps; The specific form of the nonlinear kinematic function is: In the formula The discrete time step; The nonlinear kinematic function is calculated to obtain the Jacobian matrix used for covariance evolution prediction, as follows: ; In the formula: for right Find the partial derivatives and the covariance matrix. Jacobian matrix The relationship is: When the follower UUV is in When a data packet from the leader's UUV is received, the timestamp within the data packet is used as the basis for the data packet. Location backtracking depth ,in A positive integer, used to characterize the number of algorithm sampling periods spanned by the communication delay; and then based on this... Extract historical estimates from the buffer. and historical covariance .

[0009] Preferably, obtaining the accurate estimated location of the leader's UUV at the current moment includes: utilizing currently received data with historical timestamps. Leader UUV measurement vector Historical estimates extracted from the buffer Make corrections; whereby the measurement vector The components in the figure represent the observed values ​​of the leader UUV's longitudinal position, lateral position, vertical position, linear velocity, heading angle, and pitch angle at a historical moment, respectively, with subscripts... This represents the historical discrete time step corresponding to the transmission of the data packet, where This represents the number of discrete time delay steps. Calculate measurement residuals The specific formula is as follows: In the formula, for The measurement residual vector of dimension, This is the historical state estimation vector extracted from the buffer. for The observation matrix is ​​3D and is the identity matrix. ; Calculate the residual covariance matrix The specific formula is as follows: In the formula, The residual covariance matrix is... This is the state estimation covariance matrix extracted from the buffer corresponding to historical time points. Observation matrix The transpose of the matrix, The preset measurement noise covariance matrix; Calculate the optimal Kalman gain matrix The specific formula is as follows: In the formula, Here is the Kalman gain matrix. The residual covariance matrix The inverse matrix; Update the historical states and covariance matrix to obtain the calibrated historical best estimated states. and the updated covariance matrix The specific formula is as follows: ; In the formula, for An identity matrix of 3D; Based on the calibrated historical best estimated state Starting from the baseline, a multi-step forward prediction is performed using the UUV kinematic model to the current time step to compensate for the position gap caused by communication delay. The forward iteration formula is as follows: ; In the formula, The increment of the number of iterations for forward prediction. For the first The predicted state vector of the step, The preset UUV three-dimensional nonlinear kinematic state transition function; when iterates to At that time, the final loop output state vector This is the precise predicted state vector of the leader UUV at the current moment. Extracting the first three dimensions from it yields the precise predicted position of the leader UUV at the current moment.

[0010] Preferably, the improved ALOS algorithm is used to process the desired path data, the accurate predicted position of the leader UUV at the current moment, and the corrected desired velocity of the follower UUV, including: defining an inertial coordinate system. A reference coordinate system fixed on Earth, where the origin is... As a pre-set underwater operation reference point, , , These represent the mutually orthogonal north axis, east axis, and ground axis in space; path coordinate system. The error analytical coordinate system is dynamically switched according to the desired path segment, where the origin is... This is the starting point of the target line segment of the current desired path. The axis is along the tangent direction of the current path. shaft and The axes are along the normal and binormal directions of the path, respectively; the desired heading angle. and desired pitch angle Used to characterize the path coordinate system Relative to the inertial coordinate system The three-dimensional rotation orientation is calculated as follows: ; In the formula, Let be the desired heading angle of the path point coordinate system relative to the inertial coordinate system. Let be the desired pitch angle of the path point coordinate system relative to the inertial coordinate system. , , The first The three axial position components of the desired path point in the inertial coordinate system. , , The first The three axial position components of the desired path point in the inertial coordinate system; Define UUV in inertial coordinate system The position in the middle is The starting point of the path In inertial coordinate system The position in the middle is Then UUV relative to The position error of the coordinate system is: In the formula, .

[0011] Preferred, improved ALOS algorithms include: ; in, The final desired pitch angle for the UUV, The final expected heading angle of the UUV, Let be the desired pitch angle of the path coordinate system relative to the inertial coordinate system. For the desired heading angle, This is the adaptive compensation amount for the pitch angle of attack caused by ocean current interference. This is the adaptive compensation amount for the heading drift angle caused by ocean current disturbance; , These are the approach angles to the vertical and horizontal planes, respectively. All are adaptive gains. These are the forward sight distances in the vertical and horizontal planes, respectively. These represent the maximum forward-looking distances in the vertical and horizontal planes, respectively. These are the minimum forward distances in the vertical and horizontal planes, respectively. All are constants greater than zero.

[0012] Preferably, the PID control algorithm includes: ; In the formula, Indicates time, This indicates the control output quantity. This represents the difference between the current moment and the previous moment. This represents the proportionality coefficient. The integral time constant is... The differential time constant is Represents the integral coefficient. This represents the differential coefficient.

[0013] This invention provides a method for UUV swarm formation control under complex ocean current disturbances and unstable communication scenarios, which has the following beneficial effects: This invention combines an improved ALOS algorithm, a speed coordination algorithm, and a trajectory prediction algorithm to ensure the smooth operation of UUV swarm formation missions. It can effectively solve the problems of UUV navigation being affected by ocean currents in complex marine environments and the communication delays and packet loss that occur when multiple UUVs exchange information via underwater acoustics. It ensures the smooth operation of UUV swarm formation missions and has good application prospects.

[0014] This invention addresses the problem of underwater formation control of UUV swarms in complex environments by designing a speed coordination algorithm that relies solely on the leader's position information, effectively solving the problem of underwater formation control of UUV swarms.

[0015] This invention addresses the issues of slow speed and instability in information exchange via underwater acoustic communication when UUV clusters perform tasks. It designs a trajectory prediction algorithm based on extended Kalman filtering and time compensation, and combines it with a velocity-coordinated formation algorithm to effectively solve the problem of unstable underwater acoustic communication in cluster formation control.

[0016] This invention takes into account the problem of ocean current interference during UUV navigation in complex marine environments. Based on the LOS algorithm, an improved ALOS algorithm combining adaptive update laws for drift angle and angle of attack, as well as adaptive forward look distance, is designed. This effectively enhances the environmental adaptability and maneuverability of UUV autonomous navigation, and has good performance and promising application prospects. Attached Figure Description

[0017] Figure 1 This is a general structural diagram of the present invention; Figure 2 This is a flowchart of the leader of the present invention; Figure 3 This is a flowchart of the follower process of the present invention; Figure 4 This is a diagram of the leader and follower formation structure of the present invention; Figure 5 This is a time delay diagram for underwater acoustic communication according to the present invention; Figure 6 This is a schematic diagram of the LOS guidance algorithm of the present invention; Figure 7 This is a schematic diagram of the PID control algorithm of the present invention; Figure 8 This invention provides a diagram of the actual path, expected path, and estimated path of the UUV leading to this invention. Figure 9 This invention considers UUV cluster formation diagrams under conditions of ocean current interference and communication instability; Figure 10 Top view of a UUV cluster formation considering ocean current interference and communication instability in this invention; Figure 11This is a real-time predicted position error diagram based on the extended Kalman filter and time compensation algorithm of this invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] Example 1: To address the formation control problem of unstable communication among UUV swarms in complex marine environments, this invention provides a solution combining velocity coordination based on position error, an improved ALOS guidance algorithm, and trajectory prediction. Considering the interference of ocean currents on UUV navigation in complex marine environments, this invention proposes an improved ALOS guidance algorithm. Considering the formation maintenance and transformation of UUV swarms under conditions of poor underwater acoustic communication transmission capability and stability, this invention proposes a leader-follower velocity coordination algorithm. Considering the communication delays and packet loss that occur when multiple UUVs exchange information via underwater acoustics, this invention proposes a trajectory prediction algorithm combining extended Kalman filtering and time delay compensation. The proposed solutions effectively address the challenges of UUV swarm formation control under conditions of ocean current interference and unstable communication.

[0020] Please see Figure 1 This embodiment provides a method for UUV swarm formation control under complex ocean current disturbances and unstable communication scenarios, including the following steps: S1. Construct a formation pattern for leaders and followers, and use a speed collaboration algorithm to calculate the expected path data and expected speed of follower UUVs based on the expected path data and formation data of the leader UUV. S1 mainly designs a speed coordination algorithm, which calculates the expected path information of the follower UUVs by using the expected path information and formation information of the leader UUV. During the voyage, the leader UUV continuously transmits status information to the follower UUVs using underwater acoustic communication, and the follower UUVs use the speed coordination algorithm to calculate their own expected speed information. For UUV cluster formation control, this invention adopts the method shown in the appendix. Figure 4 The leader and follower formation method shown involves the leader UUV navigating according to preset waypoints and speeds. The follower UUVs first generate their own waypoint information based on the leader UUV's waypoint information and formation information, and then obtain the desired speed information according to the speed coordination algorithm. They maintain the desired distance from the leader through the controller to achieve the formation control of the cluster.

[0021] The expected path data for calculating follower UUVs includes: Define formation information, where the formation spacing is... The formation's corners are Define the desired flight path information of the leader UUV as follows: The current location of the leader UUV is ; The expected path data of the follower UUVs is calculated based on the defined formation information. The specific formula includes: ; In the formula, For the expected path data of follower UUVs, Information on the expected flight path of the leader UUV. , This is the tangent angle of the current path.

[0022] During UUV convoy navigation, the desired position of the following UUV can be represented as: ; In the formula, Let R be the desired position of the following UUVs during UUV formation navigation, and let R be the rotation matrix. The expression for R is: .

[0023] Calculating the expected velocity of a follower UUV includes: The expected velocity of the follower UUV is calculated based on its expected position, as follows: ; In the formula, The vertical and horizontal coordinates of the current position of the UUV following the user are given. The ordinates and abscissas of the desired position of the follower UUV are given. The current position of the UUV following the UUV. For the desired sailing speed of the formation, It is a positive proportional gain coefficient. Let e ​​be the angle between the line connecting the actual position and the desired position of the follower UUV and the desired path, and let e be the position error.

[0024] As can be seen from the above formula, when the follower UUV lags behind the expected position of the formation, the position error is... ,thereby This allows the follower UUVs to gradually reach the desired location, ultimately , This achieves the effect of cluster formation; when the follower UUVs are in the desired position ahead of the formation, the position error... ,thereby This allows the follower UUVs to gradually reach the desired location, ultimately , This achieves the effect of cluster formation.

[0025] In this scheme, for ease of description, the leader UUV is also called the master U, and the follower UUV is also called the follower U.

[0026] S2. Based on the state data of the leader UUV, a trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, so as to obtain the accurate predicted position of the leader UUV at the current moment. Based on the accurate predicted position of the leader UUV at the current moment, the expected speed of the follower UUV is recalculated to obtain the corrected expected speed of the follower UUV. S2 mainly designs a trajectory prediction algorithm. In S1 above, the leader needs to send its real-time location information to the follower UUV. Due to the uncertainty of underwater acoustic communication, the information received by the follower UUV has a certain lag. This invention designs a trajectory prediction algorithm based on the combination of extended Kalman filtering and time compensation to compensate for the time. To ensure that the UUV cluster performs formation tasks according to the preset formation, compensation measures are needed to reduce communication latency. The underwater acoustic communication latency diagram during cluster formation is attached. Figure 5 As shown.

[0027] The trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, including: Define the continuous state vector of the leader UUV in three-dimensional space. The state vector includes position (xyz), velocity (v), and heading angle. and pitch angle ; Establish the discretized state transition equation, specifically as follows: ; in, It is a nonlinear kinematic function. It has a mean of zero and a covariance of The process uses Gaussian white noise. k is the number of discrete-time steps; The specific form of the nonlinear kinematic function is: In the formula The discrete time step; The nonlinear kinematic function is calculated to obtain the Jacobian matrix used for covariance evolution prediction, as follows: ; In the formula: for right Find the partial derivatives and the covariance matrix. Jacobian matrix The relationship is: When the follower UUV is in When a data packet from the leader's UUV is received, the timestamp within the data packet is used as the basis for the data packet. Location backtracking depth ,in A positive integer, used to characterize the number of algorithm sampling periods spanned by the communication delay; and then based on this... Extract historical estimates from the buffer. and historical covariance .

[0028] Obtaining the precise estimated location of the leader's UUV at the current moment includes: utilizing currently received data with historical timestamps. Leader UUV measurement vector Historical estimates extracted from the buffer Make corrections; whereby the measurement vector The components in the figure represent the observed values ​​of the leader UUV's longitudinal position, lateral position, vertical position, linear velocity, heading angle, and pitch angle at a historical moment, respectively, with subscripts... This represents the historical discrete time step corresponding to the transmission of the data packet, where This represents the number of discrete time delay steps. Calculate measurement residuals The specific formula is as follows: In the formula, for The measurement residual vector of dimension, This is the historical state estimation vector extracted from the buffer. for The observation matrix is ​​3D and is the identity matrix. ; Calculate the residual covariance matrix The specific formula is as follows: In the formula, The residual covariance matrix is... This is the state estimation covariance matrix extracted from the buffer corresponding to historical time points. Observation matrix The transpose of the matrix, The preset measurement noise covariance matrix; Calculate the optimal Kalman gain matrix The specific formula is as follows: In the formula, Here is the Kalman gain matrix. The residual covariance matrix The inverse matrix; Update the historical states and covariance matrix to obtain the calibrated historical best estimated states. and the updated covariance matrix The specific formula is as follows: ; In the formula, for An identity matrix of 3D; Based on the calibrated historical best estimated state Starting from the baseline, a multi-step forward prediction is performed using the UUV kinematic model to the current time step to compensate for the position gap caused by communication delay. The forward iteration formula is as follows: ; In the formula, The increment of the number of iterations for forward prediction. For the first The predicted state vector of the step, The preset UUV three-dimensional nonlinear kinematic state transition function; when iterates to At that time, the final loop output state vector This is the precise predicted state vector of the leader UUV at the current moment. Extracting the first three dimensions from it yields the precise predicted position of the leader UUV at the current moment.

[0029] Input the precise estimated position of the leader UUV at the current moment into S1, recalculate the expected velocity of the follower UUV, and obtain the corrected expected velocity of the follower UUV.

[0030] S3. The improved ALOS algorithm is used to process the expected path data, the accurate predicted position of the leader UUV at the current moment, and the corrected expected velocity of the follower UUV to obtain the expected vertical angle and yaw angle of the leader UUV and the expected vertical angle and yaw angle of the follower UUV. S3 primarily designs kinematic controllers. The leader and follower UUVs, based on their desired path information and considering ocean current interference, utilize an improved ALOS algorithm to calculate their respective desired vertical and bow angles. The guidance module represents the control behavior in the kinematic dimension. By designing guidance laws, it obtains the guidance signal of the desired heading angle or velocity required for the UUV to track the path; LOS guidance (as shown in the appendix). Figure 6 (As shown) Independent of dynamic control and not dependent on any model, the obtained desired heading is only related to the actual position of the UUV and the preset desired trajectory. It is not sensitive to high-frequency white noise, requires few parameters to be designed (only the forward look distance in the algorithm), simplifies the tracking error to the heading angle error, and has good convergence. Based on LOS, this invention proposes an improved ALOS algorithm as a kinematic controller for UUV.

[0031] The path for UUV tracking is defined by a series of coordinates in the inertial coordinate system. Path points in They are connected sequentially, with each pair of adjacent path points connected by a straight line, for example: As the starting point and The straight path to the destination is... This indicates the endpoint of each straight path segment. This is the path switching point (the turning point for the UUV), when the UUV enters... With radius, When the target path is within the neighborhood of the sphere's center, the desired path of the UUV switches to the next straight-line path, i.e., with... Starting point and Expected path to the destination Since straight-line path tracking is not time-constrained, the expected speed of the formation is set to a constant value in order to reduce energy consumption. The control objective can be simplified to only address the lateral tracking error. and vertical tracking error The sedation problem, longitudinal error Control issues can be disregarded.

[0032] The improved ALOS algorithm is used to process the desired path data, the accurate predicted position of the leader UUV at the current moment, and the corrected desired velocity of the follower UUV, including defining the path coordinate system. Relative to the inertial coordinate system The attitude angle is and The calculation method is as follows: In the formula; Define UUV in inertial coordinate system The position in the middle is The starting point of the path In inertial coordinate system The position in the middle is Then UUV relative to The position error of the coordinate system is: In the formula, In the formula: ;

[0033] This invention considers the influence of ocean current interference and designs an adaptive update law to estimate the drift angle and angle of attack, ultimately achieving accurate tracking of the desired trajectory. Considering that when the UUV is far from the desired route, the primary task is to quickly approach the desired route and reduce lateral errors; when the UUV is near the desired route, the primary task is to accurately track the desired trajectory and avoid position overshoot and oscillation behavior, the time-varying forward look distance will make the UUV maneuver more flexible. Based on the above ideas, an improved ALOS algorithm is proposed. Improvements to the ALOS algorithm include: ; in, The final desired pitch angle for the UUV, The final expected heading angle of the UUV, Let be the desired pitch angle of the path coordinate system relative to the inertial coordinate system. For the desired heading angle, This is the adaptive compensation amount for the pitch angle of attack caused by ocean current interference. This is the adaptive compensation amount for the heading drift angle caused by ocean current disturbance; , These are the approach angles to the vertical and horizontal planes, respectively. All are adaptive gains. These are the forward sight distances in the vertical and horizontal planes, respectively. These represent the maximum forward-looking distances in the vertical and horizontal planes, respectively. These are the minimum forward distances in the vertical and horizontal planes, respectively. All are constants greater than zero.

[0034] As can be seen from the above equation, compared to the traditional Integral Lineof-sight (ILOS) algorithm which indirectly compensates for the sideslip angle using the arctangent function, the guidance algorithm of this invention directly treats the sideslip angle as the parameter to be estimated, avoiding the abrupt changes caused by the saturation arctangent and making the desired course smoother; in the horizontal plane, when the UUV is far from the desired route, This prompts the UUV to rapidly approach the desired flight path; when the UUV is near the desired flight path, This allows the UUV to approach the desired route more smoothly, effectively reducing positional error overshoot. The same applies in the vertical plane.

[0035] S4. Based on the PID control algorithm, process the expected speed of the leader UUV, the corrected expected speed of the follower UUV, the expected vertical angle and yaw angle of the leader UUV, and the expected vertical angle and yaw angle of the follower UUV, adjust the actual speed, vertical angle and yaw angle of the leader UUV and the follower UUV to the expected values, and complete the formation control.

[0036] S4 mainly designs a dynamic controller. The UUV uses a PID control algorithm to make the actual speed, vertical angle and yaw angle of the UUV reach the expected values ​​based on the formation speed (the speed of the follower is obtained by the speed coordination algorithm) and the desired vertical angle and yaw angle obtained based on the improved ALOS algorithm, and finally completes the formation control.

[0037] By specifically designing the controller input, the dynamic controller enables the UUV dynamic system to track guidance signals with high precision, ultimately achieving path tracking. Because the PID controller is simple in structure, clear in principle, and easy to implement in engineering, it only requires adjusting the proportional, integral, and derivative parameters to obtain a relatively reliable control effect, and does not require establishing an accurate mathematical model. This makes it particularly suitable for UUV systems in highly nonlinear underwater environments with uncertain parameters that are difficult to model precisely. Furthermore, PID has good real-time performance and robustness, low computational load, and rapid response. In basic motion control such as depth, heading, and attitude, it can effectively suppress steady-state errors, reduce overshoot, and provide a certain degree of disturbance rejection capability. It has been widely verified in engineering practice as a reliable and easily debuggable classic solution. Therefore, this invention selects the PID control law as the dynamic controller.

[0038] PID control algorithms include: ; In the formula, This represents the proportionality coefficient. The integral time constant is... The differential time constant is Represents the integral coefficient. This represents the differential coefficient.

[0039] It should be noted that the key points of the UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios of this invention are as follows: 1. A velocity-coordinated formation control method based on position error; The desired path information and formation information of the leader UUV are used to calculate the desired path information of the follower UUV. During the voyage, the leader UUV continuously transmits its own status and formation information to the follower UUV using underwater acoustic communication. The follower UUV uses a speed coordination algorithm to calculate its own desired speed information and ultimately achieves cluster formation by controlling its own speed and angle information.

[0040] 2. Formation control methods under unstable communication conditions; The follower UUV receives the state information of the leader UUV, uses it as an observation, compares it with the historical estimated state extracted from the buffer, and obtains the optimal estimated state through nonlinear filtering correction using the EKF algorithm. Then, it uses this as the starting point and performs time-compensated iterative prediction using the UUV kinematic equations. By performing multi-step integration over the delay period, the current position of the leader UUV is finally predicted.

[0041] 3. Improved ALOS algorithm under ocean current interference; Considering the impact of ocean current interference and stability issues, an improved ALOS algorithm combining drift angle, angle of attack adaptive update law, and adaptive forward look distance is designed based on the LOS algorithm. It outputs the desired heading angle and pitch angle information according to the waypoint information.

[0042] Based on the mission information, the leader UUV is given the desired waypoint information and formation information. The formation information includes the group formation speed information, the desired relative distance and bearing information between the leader UUV and the follower UUVs.

[0043] After receiving the mission message, the leader UUV sends its own status information, its desired waypoint information, and formation information to the follower UUVs. The follower calculates its own desired waypoint information based on the leader UUV's desired waypoint information and formation information.

[0044] During the navigation phase, the leader UUV tracks the desired speed, desired heading angle, and desired pitch angle of the formation based on the improved ALOS algorithm and PID control algorithm proposed in this invention to achieve path tracking. During this period, the leader UUV sends its own status information with a timestamp to the follower UUVs every 1 second. After receiving the information from the leader UUV, the follower UUVs use EKF and time compensation algorithms to calculate the current position information of the UUV and input it into the speed coordination algorithm to calculate their own desired speed. At the same time, the follower UUVs calculate their own desired heading angle and pitch angle based on their current position and desired track segment information and the improved ALOS algorithm proposed in this invention. They then use the PID control algorithm and actuators to complete the path tracking of the follower UUVs, and finally complete the formation control of the UUV cluster.

[0045] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented in software, the above embodiments can be implemented, in whole or in part, as a computer program product. Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution.

[0046] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0047] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A method for UUV swarm formation control under complex ocean current disturbances and unstable communication scenarios, characterized in that, Includes the following steps: A formation pattern for leaders and followers is constructed, and a speed collaboration algorithm is adopted to calculate the expected path data and expected speed of follower UUVs based on the expected path data and formation data of the leader UUV. Based on the state data of the leader UUV, a trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, thereby obtaining the accurate predicted position of the leader UUV at the current moment. Based on the accurate predicted position of the leader UUV at the current moment, the expected velocity of the follower UUV is recalculated to obtain the corrected expected velocity of the follower UUV. An improved ALOS algorithm is used to process the expected path data, the accurate predicted position of the leader UUV at the current moment, and the corrected expected velocity of the follower UUV, to obtain the expected vertical angle and yaw angle of the leader UUV and the expected vertical angle and yaw angle of the follower UUV. Based on the PID control algorithm, the desired speed of the leader UUV, the corrected desired speed of the follower UUV, the desired vertical angle and yaw angle of the leader UUV, and the desired vertical angle and yaw angle of the follower UUV are processed. The actual speed, vertical angle, and yaw angle of the leader UUV and the follower UUV are adjusted to the desired values ​​to complete the formation control.

2. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 1, characterized in that: The expected path data for calculating follower UUVs includes: Define formation information, where the formation spacing is... The formation's corners are Define the desired flight path information of the leader UUV as follows: The current location of the leader UUV is ; The expected path data of the follower UUVs is calculated based on the defined formation information. The specific formulas include: ； In the formula, For the expected path data of follower UUVs, Information on the expected flight path of the leader UUV. , The tangent angle of the current path; During UUV convoy navigation, the desired position of a following UUV can be represented as: ； In the formula, Let R be the desired position of the following UUVs during UUV formation navigation, and let R be the rotation matrix. The expression for R is: 。 3. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 2, characterized in that: Calculating the expected velocity of a follower UUV includes: The expected velocity of the follower UUV is calculated based on its expected position, as follows: ； In the formula, The vertical and horizontal coordinates of the current position of the UUV following the user are given. The ordinates and abscissas of the desired position of the follower UUV are given. The current position of the follower UUV. For the desired sailing speed of the formation, It is a positive proportional gain coefficient. Let e ​​be the angle between the line connecting the actual position and the desired position of the follower UUV and the desired path, and let e be the position error.

4. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 1, characterized in that: The trajectory prediction algorithm based on extended Kalman filtering and time delay compensation is used to correct the lagging position information, including: Define the continuous state vector of the leader UUV in three-dimensional space. The state vector includes position (xyz), velocity (v), and heading angle. and pitch angle ; Establish the discretized state transition equation, specifically as follows: ; in, It is a nonlinear kinematic function. It has a mean of zero and a covariance of The process uses Gaussian white noise; k is the number of discrete time steps; The specific form of the nonlinear kinematic function is: In the formula The discrete time step; The nonlinear kinematic function is calculated to obtain the Jacobian matrix used for covariance evolution prediction, as follows: ； In the formula: for right Find the partial derivatives and the covariance matrix. Jacobian matrix The relationship is: When the follower UUV is in When a data packet from the leader's UUV is received, the timestamp within the data packet is used as the basis for the data packet. Location backtracking depth ,in A positive integer, used to characterize the number of algorithm sampling periods spanned by the communication delay; and then based on this... Extract historical estimates from the buffer. and historical covariance .

5. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 4, characterized in that: Obtaining the precise estimated location of the leader's UUV at the current moment includes: utilizing currently received data with historical timestamps. Leader UUV measurement vector Historical estimates extracted from the buffer Make corrections; whereby the measurement vector The components in the figure represent the observed values ​​of the leader UUV's longitudinal position, lateral position, vertical position, linear velocity, heading angle, and pitch angle at a historical moment, respectively, with subscripts... This represents the historical discrete time step corresponding to the transmission of the data packet, where This represents the number of discrete time delay steps. Calculate measurement residuals The specific formula is as follows: In the formula, for The measurement residual vector of dimension, This is the historical state estimation vector extracted from the buffer. for The observation matrix is ​​3D and is the identity matrix. ; Calculate the residual covariance matrix The specific formula is as follows: In the formula, The residual covariance matrix is... This is the state estimation covariance matrix extracted from the buffer corresponding to historical time points. Observation matrix The transpose of the matrix, The preset measurement noise covariance matrix; Calculate the optimal Kalman gain matrix The specific formula is as follows: In the formula, Here is the Kalman gain matrix. The residual covariance matrix The inverse matrix; Update the historical states and covariance matrix to obtain the calibrated historical best estimated states. and the updated covariance matrix The specific formula is as follows: ; In the formula, for An identity matrix of dimensionality; Based on the calibrated historical best estimated state Starting from the baseline, a multi-step forward prediction is performed using the UUV kinematic model to the current time step to compensate for the position gap caused by communication delay. The forward iteration formula is as follows: ； In the formula, The increment of the number of iterations for forward prediction. For the first The predicted state vector of the step, The preset UUV three-dimensional nonlinear kinematic state transition function; when iterates to At that time, the final loop output state vector This is the precise estimated state vector of the leader UUV at the current moment. Extracting the first three dimensions from it yields the precise estimated position of the leader UUV at the current moment.

6. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 5, characterized in that: The improved ALOS algorithm is used to process the desired path data, the accurate predicted position of the leader UUV at the current moment, and the corrected desired velocity of the follower UUV, including defining the inertial coordinate system. A reference coordinate system fixed on Earth, where the origin is... As a pre-set underwater operation reference point, , , These represent the mutually orthogonal north axis, east axis, and ground axis in space; path coordinate system. The error analytical coordinate system is dynamically switched according to the desired path segment, where the origin is... This is the starting point of the target line segment of the current desired path. The axis is along the tangent direction of the current path. shaft and The axes are along the normal and binormal directions of the path, respectively; the desired heading angle. and desired pitch angle Used to characterize the path coordinate system Relative to the inertial coordinate system The three-dimensional rotation orientation is calculated as follows: ； In the formula, Let be the desired heading angle of the path point coordinate system relative to the inertial coordinate system. Let be the desired pitch angle of the path point coordinate system relative to the inertial coordinate system. , , The first The three axial position components of the desired path point in the inertial coordinate system. , , The first The three axial position components of the desired path point in the inertial coordinate system; Define UUV in inertial coordinate system The position in the middle is The starting point of the path In inertial coordinate system The position in the middle is Then UUV relative to The position error of the coordinate system is: In the formula, .

7. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 6, characterized in that: Improvements to the ALOS algorithm include: ； in, The final desired pitch angle for the UUV, The final expected heading angle of the UUV, Let be the desired pitch angle of the path coordinate system relative to the inertial coordinate system. For the desired heading angle, This is the adaptive compensation amount for the pitch angle of attack caused by ocean current interference. This is the adaptive compensation amount for the heading drift angle caused by ocean current disturbance; , These are the approach angles to the vertical and horizontal planes, respectively. All are adaptive gains. These are the forward sight distances in the vertical and horizontal planes, respectively. These represent the maximum forward-looking distances in the vertical and horizontal planes, respectively. These are the minimum forward distances in the vertical and horizontal planes, respectively. All are constants greater than zero.

8. The UUV swarm formation control method under complex ocean current disturbances and unstable communication scenarios as described in claim 7, characterized in that: PID control algorithms include: In the formula, Indicates time, This indicates the control output quantity. This represents the difference between the current moment and the previous moment. This represents the proportionality coefficient. The integral time constant is... The differential time constant is Represents the integral coefficient. This represents the differential coefficient.

Images