A Dynamic Construction Method for USV-AUV Cooperative Navigation Network Topology Based on ADMM

Abstract

This invention discloses a dynamic construction method for the topology of a USV-AUV cooperative navigation network based on ADMM. The method includes: constructing the relationship between the positioning error of the AUV and the distance measurement errors between the surface unmanned surface vessel (Surface UV) and the AUV, and the positional errors between the Surface UVs, based on the positioning equations between the Surface UV and the AUV; designing a positioning accuracy evaluation function using optimal estimation criteria based on the relationship between the AUV positioning error, distance measurement errors, and positional errors between the Surface UVs; and obtaining the optimal solution for the evaluation function using an improved multi-objective, multi-dimensional optimizer to obtain the optimal formation of the heterogeneous cooperative navigation positioning system and the optimal distance information between the Surface UV and the AUV. This invention achieves real-time adaptive adjustment of the topology, effectively improving computational efficiency and robustness, realizing multi-dimensional constrained cooperative optimization, and improving cooperative positioning accuracy.

Description

TECHNICAL FIELD

[0001] The application relates to the field of ocean unmanned system cooperative navigation, and in particular to a USV-AUV cooperative navigation network topology structure dynamic construction method based on ADMM. BACKGROUND

[0002] Traditional heterogeneous cooperative navigation network topology structures mainly adopt preset fixed frameworks, which have the advantages of simple structure and easy implementation in ideal environments, but have significant limitations in complex marine environments. When the formation members dynamically change due to task requirements, equipment failures or environmental disturbances (such as ocean currents and waves), the fixed topology cannot be adaptively adjusted, resulting in decreased system observability, interrupted communication links and deteriorated cooperative positioning accuracy. In the prior art, centralized optimization methods (such as global gradient descent) can handle topology optimization, but have high computational complexity, high single-point failure risk and are difficult to meet real-time requirements; while simple distributed algorithms (such as local negotiation) reduce communication overhead, but are prone to local optimization, have slow convergence speed and lack global consistency guarantee.

[0003] Chinese Invention Patent No. CN119714294A discloses a cooperative navigation method based on a fixed formation, which realizes formation keeping through predefined geometric constraints, but cannot handle the scenario of dynamic joining or exiting of vehicles; another type of topology optimization method based on graph theory maintains the network structure by using connectivity constraints, but does not consider real-time optimization of multi-dimensional geometric constraints (such as relative distance and azimuth angle), resulting in insufficient accuracy in three-dimensional marine environments. The common defect of these methods is the lack of a distributed framework that can balance computational efficiency, global consistency and dynamic adaptability. SUMMARY

[0004] In order to overcome the above-mentioned problems in the prior art, the application proposes a USV-AUV cooperative navigation network topology structure dynamic construction method based on ADMM.

[0005] The technical scheme adopted by the application to solve the technical problems is: a USV-AUV cooperative navigation network topology structure dynamic construction method based on ADMM, comprising the following steps:

[0006] Step 1: according to the positioning equation between the surface unmanned vehicle and the autonomous underwater vehicle, the relationship between the autonomous underwater vehicle positioning error and the distance measurement error between the surface unmanned vehicle and the autonomous underwater vehicle and the position error between the surface unmanned vehicles is constructed by partial differentiation method;

[0007] Step 2: based on the relationship between the autonomous underwater vehicle positioning error and the distance measurement error and the position error between the surface unmanned vehicles established in step 1, a positioning accuracy evaluation function is designed using optimal estimation criteria;

[0008] Step 3, the optimal solution of the evaluation function is obtained by using the improved multi-objective multi-dimensional optimizer, and the best formation of the heterogeneous cooperative navigation positioning system and the best distance information between the surface unmanned vehicle and the autonomous underwater vehicle are obtained.

[0009] The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method described above, wherein step 1 is specifically:

[0010] Step 1.1, a state space model of the heterogeneous cooperative system of the surface unmanned vehicle and the autonomous underwater vehicle is established.

[0011] Step 1.2, the relative distance between the vehicles, the horizontal position angle , the pitch angle and the communication connectivity constraint are established.

[0012] The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method described above, wherein step 1.1 is specifically: a coordinate system is established with the task starting point as the origin, and the state equation of the surface unmanned vehicle is:

[0013]

[0014] wherein, , , respectively represent the derivative of the three-dimensional space position coordinate with respect to time; represents the speed of the vehicle, is the angular velocity, respectively represent the roll angle, the pitch angle and the yaw angle of the surface unmanned vehicle; represents the derivative of the yaw angle with respect to time;

[0015] The state equation of the autonomous underwater vehicle is:

[0016] ;

[0017] wherein, , , respectively represent the roll angle, the pitch angle and the yaw angle; represents the derivative of the pitch angle with respect to time; represents the derivative of the roll angle with respect to time.

[0018] The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method described above, wherein the positioning accuracy evaluation function in step 2 is based on the constraint conditions in step 1:

[0019] ;

[0020] wherein, , , are the weight coefficients of the distance constraint, the azimuth angle constraint and the pitch angle constraint, respectively; i, j represent the numbers of the vehicles, and N represents the maximum number of the vehicles; the adjacency matrix A ij represents the corresponding geometric constraint between the vehicles and , represents the horizontal azimuth angle from the vehicle to the vehicle , represents the pitch angle from the vehicle to the vehicle , represents the Euclidean distance between the two vehicles; p i represents the position of the vehicle i; represents; represents; represents; represents.

[0021] The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method described above, in step 3, the alternating direction multiplier method is used to optimize the positioning accuracy evaluation function obtained in step 2, specifically:

[0022] Step 3.1, the gradient descent method is used to update the position variable : each vehicle optimizes its own position to minimize the augmented Lagrangian function under the condition of fixing the positions of other vehicles and the current Lagrange multiplier value , wherein X is the position variable of all vehicles, Y (k) is the auxiliary variable of the current distance constraint, Z (k) is the auxiliary variable of the current azimuth angle constraint, and W (k) is the auxiliary variable of the current pitch angle constraint, is the current Lagrange multiplier;

[0023] Step 3.2, auxiliary variable update: fix the position variable and the Lagrange multiplier , and update the auxiliary variable;

[0024] Step 3.3, Lagrange multiplier update: if the actual value of the constraint is greater than the expected value, increase the Lagrange multiplier of the corresponding constraint, if the actual value of the constraint is less than the expected value, decrease the Lagrange multiplier of the corresponding constraint.

[0025] In the above-described dynamic construction method for the topology of the USV-AUV cooperative navigation network based on ADMM, the expression for the illumination-enhancing Lagrangian function in step 3.1 is:

[0026]

[0027] in, For the position variables of all vehicles; These are auxiliary variables for distance constraints. This is an auxiliary variable for azimuth constraints; This is an auxiliary variable for the pitch angle constraint; For the corresponding Lagrange multipliers; Let be the mapping function from position to azimuth. This is the mapping function from position to pitch angle.

[0028] The beneficial effects of this invention are: its adaptive dynamic reconfiguration capability—through the ADMM distributed framework—achieves real-time adaptive adjustment of the topology, overcoming the rigidity problem of fixed topologies in dynamic environments. The formation error converges to 0.006 meters, far superior to traditional methods (such as the 0.2-meter error of simple distributed algorithms).

[0029] This invention improves computational efficiency and robustness: distributed solving reduces computational complexity from O(N) of centralized methods. 3 The cost is reduced to O(N), thus avoiding single points of failure.

[0030] This invention achieves multi-dimensional constraint collaborative optimization: it comprehensively processes distance, azimuth, and elevation constraints, controls the angle error within 0.1 degrees, optimizes acoustic observation geometry, and improves collaborative positioning accuracy.

[0031] Through comparative experiments, the present invention demonstrates excellent performance in terms of convergence, constraint satisfaction, and single-vehicle control, providing a reliable technical solution for complex marine missions. Attached Figure Description

[0032] Figure 1 This is a schematic diagram of the present invention;

[0033] Figure 2 This is a schematic diagram illustrating the convergence of formation errors in this invention;

[0034] Figure 3 This is a schematic diagram of the initial formation configuration of the present invention;

[0035] Figure 4 This is a schematic diagram of the final formation configuration of the present invention;

[0036] Figure 5 This is a schematic diagram of the final convergence trajectory of the present invention;

[0037] Figure 6 This is a schematic diagram illustrating the degree of violation of the distance constraint in this invention;

[0038] Figure 7 This is a schematic diagram illustrating the degree of violation of the horizontal angular constraint of this invention;

[0039] Figure 8 This is a schematic diagram illustrating the degree of violation of the pitch angle constraint of this invention. Detailed Implementation

[0040] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0041] This invention provides a dynamic construction method for the topology of USV / AUV cooperative navigation networks based on the Alternating Direction Multiplier Method (ADMM). Compared with existing centralized optimization methods, this method reduces computational complexity through a distributed cooperative optimization framework, achieving an effective balance between computational efficiency and optimization performance. By constructing a distributed cooperative optimization framework, this method can achieve asymptotically consistent convergence of solutions from multiple vehicles, effectively solving the topology optimization problem of heterogeneous USV / AUV cooperative navigation and positioning systems in complex dynamic environments. The core idea of ​​this method is to decompose the complex global optimization problem into multiple relatively simple local optimization sub-problems. Each vehicle only needs to make local decisions based on its own state information and neighborhood information, and ultimately achieves the globally optimal network topology configuration through a distributed iterative coordination mechanism.

[0042] The design process of the optimal network topology for a heterogeneous cooperative navigation and positioning system is as follows: Figure 1 As shown:

[0043] (1) Based on the positioning equation between USV and AUV, the relationship between AUV positioning error, distance measurement error between USV and AUV and position error between USV is constructed by partial differential method;

[0044] (2) Based on the established relationship between AUV positioning error, distance measurement error and position error between USVs, a positioning accuracy evaluation function is designed using the optimal estimation criterion;

[0045] (3) The optimal solution of the evaluation function is obtained by using the improved multi-objective multi-dimensional optimizer, thereby obtaining the optimal formation of the heterogeneous cooperative navigation and positioning system and the optimal distance information between the USV and AUV.

[0046] This embodiment uses the simulation of a heterogeneous system composed of a medium-sized USV and an AUV as an example to describe the implementation method in detail. The simulation is based on the MATLAB platform and simulates the formation control process of the USV / AUV heterogeneous cooperative system in a three-dimensional marine environment.

[0047] Since 3D positioning requires at least four reference points, two USVs provide a surface reference, and four AUVs provide redundant observations. Therefore, the heterogeneous cooperative system consists of six vehicles: two USVs and four AUVs. This configuration ensures system complexity without compromising the clarity of the analysis. A layered formation design is desired, with the USVs forming a reference line on the surface and the AUVs forming two levels of diamond formation underwater. The specific configuration is shown in Table 1.

[0048] Table 1

[0049]

[0050] This configuration offers geometric diversity, avoiding singular configurations such as collinearity and coplanarity, and ensuring good observation geometry. The hierarchical structure allows the USV to provide a positioning reference on the water surface, while the AUVs form a three-dimensional formation at different depths. The formation exhibits good symmetry, which is beneficial for stable control, and also provides good scalability.

[0051] In addition, to test the robustness of the algorithm, the initial position is randomly perturbed near the desired position, with the perturbation amplitude following a normal distribution of twice the standard deviation:

[0052]

[0053] in, This represents a three-dimensional standard normal distribution random vector.

[0054] In ADMM variables, Lagrange multipliers:

[0055]

[0056] Formation center estimates:

[0057]

[0058] This initialization method can test the algorithm's ability to converge from a poor initial state to the desired state.

[0059] This embodiment selects the Northeast Sky coordinate system with the mission starting point as the origin, mainly based on the following considerations: (1) it conforms to the international standards for marine navigation; (2) it simplifies the unified description of the horizontal motion of the USV and the three-dimensional motion of the AUV; (3) it facilitates the geometric calculation of acoustic ranging data; and (4) it is conducive to compatibility and integration with existing marine navigation systems.

[0060] Centered on the mission starting point, within the established coordinate system, the state of each unmanned aerial vehicle can be fully described by a six-dimensional vector:

[0061]

[0062] This state vector contains complete motion information of the aircraft. Indicates the first The position coordinates of an aircraft in three-dimensional space. and These represent the horizontal positions to the east and north, respectively. This corresponds to the position information in the vertical direction. For AUVs, This usually represents a negative seawater depth value, but for USV, This indicates the height above sea level. The last three components... These represent the roll angle, pitch angle, and yaw angle of the aircraft, respectively.

[0063] USVs and AUVs are modeled using state equations with different degrees of freedom, mainly considering their differences in motion characteristics. For USVs, considering that they mainly move in a two-dimensional plane on the sea surface with limited vertical motion, their state equations can be described as follows:

[0064]

[0065] in For the speed of the aircraft, ω is the angular velocity.

[0066] For AUVs, due to their motion in three-dimensional space, the state equations are more complex, including:

[0067]

[0068] in , , These are the angular velocities for roll, pitch, and yaw, respectively.

[0069] In USV / AUV heterogeneous cooperative navigation systems, the relative geometric constraints between vehicles are a core element in constructing an effective network topology. These constraints not only determine the system's geometric configuration but, more importantly, affect the quality of information transmission and the overall observability of the system. In a three-dimensional ocean environment, the geometric relationship between any two vehicles can be fully described by multiple parameters, primarily including key elements such as relative distance, horizontal azimuth, and pitch angle.

[0070] Relative distance constraints are the most intuitive and important geometric constraints. For spacecraft... and aircraft The relative distance between them can be precisely represented by three-dimensional Euclidean distance:

[0071]

[0072] in, Indicates the first The three-dimensional position vector of the aircraft Indicates the first The three-dimensional position vector of the aircraft This represents the Euclidean distance between two spacecraft.

[0073] This distance information plays a crucial role in cooperative navigation systems. First, it directly affects the accuracy of acoustic ranging; too close a distance can lead to acoustic interference, while too large a distance reduces signal strength and measurement accuracy. Second, relative distance influences the quality of the communication link; in underwater acoustic communication environments, distance is one of the key factors determining communication reliability. Finally, appropriate distance configuration helps improve the system's geometric accuracy factor, thereby improving overall positioning accuracy.

[0074] Horizontal azimuth constraints describe the relative orientation of an aircraft in the horizontal plane, which is crucial for determining the aircraft's lateral relative position. From the aircraft... Pointing vehicle The horizontal azimuth angle can be expressed as:

[0075]

[0076] in, Indicates from the aircraft Looking at the aircraft from its position For USVs, horizontal azimuth information helps determine the relative position of other platforms, which is crucial for formation keeping and obstacle avoidance. For AUVs, horizontal azimuth information helps them maintain the correct heading and formation position in underwater environments.

[0077] Pitch angle constraints describe the vertical relationship between vehicles at different depth levels, which is particularly important in a three-dimensional ocean environment. From the vehicle... Pointing vehicle The pitch angle can be defined as:

[0078]

[0079] in, Indicates from the aircraft Looking at the aircraft The pitch angle is of particular importance in heterogeneous USV / AUV systems. Since USVs typically operate at the sea surface, while AUVs operate at varying depths, pitch angle information directly impacts the optimization of vertical geometry. Appropriate pitch angle configuration not only improves vertical positioning accuracy but also optimizes the acoustic signal propagation path, reducing multipath effects and signal attenuation.

[0080] Transforming the USV / AUV formation configuration design problem into a mathematical optimization problem is a crucial step in achieving automated topology adjustments. This transformation requires establishing an objective function that accurately reflects the system's performance requirements while ensuring that all constraints are appropriately addressed. The fundamental goal of formation configuration optimization is to find the space configuration that maximizes the overall system performance, while satisfying various physical constraints and mission requirements.

[0081] The primary objective of formation configuration optimization is to minimize the deviation between the actual formation and the ideal formation. This deviation includes not only the positional discrepancies of individual aircraft but also the overall deformation of the formation shape. The basic objective function can be expressed as:

[0082]

[0083] in, This is a state vector containing the position information of all vehicles. For the first The objective function is to determine the desired positions of all aircraft. The physical meaning of this objective function is to make the actual positions of all aircraft as close as possible to their desired positions, thereby maintaining the overall shape of the formation.

[0084] This optimization problem needs to be solved under multiple constraints, which constitute the boundary of the feasible solution space. The main constraints include:

[0085] (1) Distance Constraint: Distance constraints ensure that the distance between adjacent vehicles is maintained within an appropriate range. The design of distance constraints needs to consider communication distance to ensure that the distance between vehicles is within the communication range, maintain a sufficient safe distance to avoid collisions, select an appropriate distance to optimize observation geometry, and the distance setting should be conducive to the stable control of the formation. This constraint can be expressed as:

[0086]

[0087] in This refers to the expected relative distance determined based on task requirements and system characteristics.

[0088] (2) Azimuth Constraints: In the marine environment, azimuth information is crucial for cooperative positioning. Horizontal azimuth constraints not only help maintain the directionality of the formation but also improve the geometric accuracy of acoustic observations. Azimuth constraints ensure that the relative azimuths between vehicles meet specific requirements. The horizontal azimuth constraint between two vehicles can be expressed as:

[0089]

[0090] in The desired horizontal azimuth angle.

[0091] The design of azimuth constraints needs to consider principles such as geometric optimization, mission orientation, symmetry, and singularity avoidance. A suitable azimuth angle should be selected to optimize the observation geometric accuracy factor. The main observation direction should be determined according to mission requirements. Where possible, formation symmetry should be maintained to improve stability, and singular geometric configurations such as collinearity should be avoided.

[0092] (3) Pitch Angle Constraint: For heterogeneous USV / AUV systems in three-dimensional space, pitch angle constraints are equally important. The pitch angle reflects the vertical geometric relationship between the vehicles and has a significant impact on underwater positioning accuracy. The pitch angle constraint between two vehicles can be expressed as:

[0093]

[0094] in The desired pitch angle.

[0095] The design of pitch angle constraints needs to consider factors such as depth differences, acoustic characteristics, obstruction avoidance, and stability. USVs operate on the water surface, while AUVs operate underwater. Therefore, it is necessary to design pitch angles appropriately to optimize vertical geometry, consider the vertical propagation characteristics of underwater acoustic channels, avoid acoustic signal obstruction due to excessive pitch angles, and maintain suitable pitch angles to ensure formation stability.

[0096] (4) Communication Constraints: Communication connectivity constraints ensure that the information transmission capability of the system is not impaired due to topology adjustments. This constraint is usually achieved by maintaining a connected communication graph, i.e., requiring the existence of an adjacency matrix A that guarantees the connectivity of the graph. The expression is:

[0097] In mathematics, only when At that time, the aircraft and The corresponding geometric constraints are only considered between them.

[0098] Taking all constraints into account, the complete formation control objective function is:

[0099]

[0100] in, , , These are the weighting coefficients for range constraints, azimuth constraints, and pitch constraints, respectively. The objective function includes formation-keeping terms and various geometric constraint terms, comprehensively describing the formation control requirements of a USV / AUV heterogeneous cooperative system.

[0101] In complex USV / AUV heterogeneous cooperative navigation systems, traditional centralized optimization methods often face numerous challenges, including high computational complexity, large communication overhead, and the risk of single points of failure. To overcome these problems, this invention employs the alternating direction multiplier method to construct a distributed cooperative optimization framework. By decomposing the complex global optimization problem into multiple relatively simple local optimization subproblems, it achieves a reasonable allocation of computational load and improves system robustness.

[0102] The Alternating Direction Method of Multipliers (ADMM) is a powerful distributed optimization algorithm, particularly well-suited for solving constrained optimization problems in large-scale distributed systems. ADMM combines the advantages of dual decomposition and augmented Lagrangian methods, effectively handling complex constrained optimization problems. In the context of USV / AUV cooperative navigation, ADMM has a natural advantage because each vehicle can be considered an independent decision-making unit, coupled with each other through constraints, which perfectly aligns with the ADMM algorithm's architecture.

[0103] The basic idea of ​​the ADMM algorithm is to decompose a complex global optimization problem into multiple simple local subproblems. By iteratively solving these subproblems and exchanging boundary information, the algorithm eventually converges to the global optimum. This decomposition method gives the algorithm a naturally distributed nature, as each subsystem only needs to handle local problems and exchange a small amount of information with its neighbors.

[0104] For general-form constrained optimization problems:

[0105]

[0106] ADMM constructs an augmented Lagrange function by introducing the Lagrange multiplier λ:

[0107]

[0108] in, For the penalty parameter, augmentation term The introduction of this improves the convergence properties of the algorithm.

[0109] The standard iteration format for ADMM is:

[0110]

[0111] This alternating optimization approach makes each subproblem relatively simple, usually having a closed-form solution or being solvable efficiently.

[0112] The core idea of ​​the ADMM decomposition strategy is to transform the original global optimization problem into an equivalent problem with a separable structure by introducing auxiliary variables, and then use Lagrange duality theory to transform the constrained optimization problem into an unconstrained optimization problem.

[0113] In the specific application of USV / AUV cooperative navigation network topology optimization, the decomposition strategy needs to be carefully designed to ensure the effectiveness and convergence of the algorithm. The original formation optimization problem has a highly coupled constraint structure, with the position variables of each vehicle closely linked through distance constraints, azimuth constraints, etc. This coupled structure makes it difficult to directly apply traditional distributed optimization methods. The main difficulties of the original optimization problem are: (1) the position variables of the vehicles are highly coupled through nonlinear geometric constraints; (2) the nonconvexity of distance and angle constraints leads to local optimum traps; (3) the computational complexity of centralized solution is O(N). 3 This approach is unsuitable for real-time applications. Therefore, a variable separation technique is employed: by introducing auxiliary variables, complex global constraints are decomposed into simple local constraints.

[0114] For distance constraints, auxiliary variables are introduced. :

[0115]

[0116] For azimuth constraints, auxiliary variables are introduced. :

[0117]

[0118]

[0119] For pitch angle constraints, an auxiliary variable is introduced. :

[0120]

[0121]

[0122] By introducing auxiliary variables, the original optimization problem can be reformulated as an equivalent problem with a more regular structure. In this equivalent problem, the original variable (vehicle position) and the auxiliary variable are connected by linear equality constraints, while various geometric constraints are expressed through the auxiliary variables.

[0123] The augmented Lagrangian function (ALM) is a core component of the ADMM algorithm. It improves the algorithm's convergence characteristics by adding a quadratic penalty term to the traditional Lagrangian function. In the topology optimization problem of USV / AUV cooperative navigation networks, the construction of the augmented Lagrangian function requires comprehensive consideration of various constraint types and system characteristics. Based on the aforementioned problem decomposition strategy, the following augmented Lagrangian function can be constructed:

[0124]

[0125] in: For the position variables of all vehicles;

[0126] These are auxiliary variables for distance constraints.

[0127] This is an auxiliary variable for azimuth constraints;

[0128] This is an auxiliary variable for the pitch angle constraint;

[0129] For the corresponding Lagrange multipliers;

[0130] and This is a mapping function from position to azimuth / elevation angle.

[0131] The advantage of this separation method is that the position optimization problem of each vehicle becomes a local problem, involving only the information of that vehicle and its neighbors. The optimization problem of the auxiliary variables has a simple geometric structure, usually has a closed-form solution, the update of the Lagrange multipliers is linear, and the computation is simple.

[0132] The basic iterative process of the ADMM algorithm consists of three main steps, which are executed sequentially in each iteration until the algorithm converges or reaches the maximum number of iterations.

[0133] Step 1: Update position variables

[0134] In this step, each vehicle is in a fixed position relative to the other vehicles. and the current Lagrange multiplier value Under the given conditions, optimize its position to minimize the augmented Lagrangian function. The mathematical expression for this step is:

[0135]

[0136] This problem can be broken down into N independent subproblems, each for a different aircraft. The optimization problem is:

[0137]

[0138] in, For aircraft The neighborhood group, For the purpose of involving aircraft and Local constraint terms.

[0139] Specifically, the gradient of vehicle i is:

[0140]

[0141] The gradient of the local constraint term includes:

[0142] Distance-constrained gradient:

[0143]

[0144] Azimuth constraint gradient

[0145]

[0146] Pitch angle constrained gradient:

[0147]

[0148] The gradient of the azimuth mapping function is:

[0149]

[0150]

[0151] The gradient of the pitch angle mapping function is:

[0152]

[0153] Position updates are performed using gradient descent.

[0154]

[0155] Step 2: Update auxiliary variables

[0156] Fixed position variable and Lagrange multipliers Update auxiliary variables:

[0157]

[0158] These subproblems typically have closed-ended solutions:

[0159] For distance-constrained auxiliary variables:

[0160]

[0161] The solution to this problem is:

[0162]

[0163] For azimuth constraint auxiliary variables:

[0164]

[0165] The solution to this problem is:

[0166]

[0167] Similarly, the solution for the pitch angle constraint auxiliary variable is:

[0168]

[0169] Step 3: Lagrange multiplier update

[0170] In this step, the Lagrange multipliers corresponding to each constraint are updated according to the degree of constraint violation. The update rule for the Lagrange multipliers follows the traditional dual ascent method:

[0171] For distance constraints, the multiplier update rule is:

[0172]

[0173] For the horizontal azimuth constraint, the multiplier update rule is:

[0174]

[0175] For pitch angle constraints, the multiplier update rule is:

[0176]

[0177] The basic idea behind these update rules is: if a constraint violation is positive (actual value greater than expected value), the corresponding Lagrange multiplier is increased; if a constraint violation is negative (actual value less than expected value), the corresponding Lagrange multiplier is decreased. Through this mechanism, the Lagrange multipliers can be automatically adjusted to ensure that the constraints are satisfied.

[0178] To ensure the numerical stability of the algorithm, it is usually necessary to truncate the values ​​of the Lagrange multipliers:

[0179]

[0180] in, This represents the truncation function. This is the preset upper bound.

[0181] Based on the above methods, performance testing was conducted.Figure 2 As can be seen from the formation error convergence curve, the proposed ADMM-based network topology optimization method exhibits good convergence characteristics. The formation error decreases rapidly from an initial value of approximately 3.5 meters, achieving rapid convergence in the first 20 iterations, and then enters a stable convergence phase. The algorithm reaches the convergence threshold of 0.5 meters in the 65th iteration, and the final formation error stabilizes at around 0.006 meters, fully validating the effectiveness of the algorithm.

[0182] The convergence process exhibits typical exponential decay characteristics, with a relatively fast initial convergence rate followed by a gradual stabilization. This convergence pattern aligns with the theoretical characteristics of the ADMM algorithm, demonstrating its good numerical stability when handling multi-constraint optimization problems.

[0183] Through the Figure 3 and Figure 4 Comparative analysis clearly reveals the formation and convergence process of the formation configuration. Initially, the six heterogeneous vehicles (including two USVs and four AUVs) exhibited significant positional deviations relative to the desired formation configuration, with positional disturbances reaching two standard deviations. Through iterative optimization using the method described in this embodiment, all vehicles successfully converged to the neighborhood of their desired positions, ultimately forming a stable three-dimensional hierarchical formation structure.

[0184] The resulting formation configuration exhibits clear hierarchical characteristics and excellent geometric properties. At the surface layer, USV1 and USV2 form the formation's baseline, providing precise positioning and navigation references for the entire multi-layered formation system. In the first underwater layer (depth -20 meters), AUV1 and AUV2 form a diamond formation, maintaining a preset relative position. In the second underwater layer (depth -30 meters), AUV3 and AUV4 also adopt a diamond formation, creating a vertically symmetrical spatial distribution with the upper layers. The overall formation structure demonstrates good geometric symmetry and uniform spatial distribution, validating the effectiveness and stability of the proposed formation control algorithm.

[0185] Figure 5 The convergence trajectory shows the motion path of each vehicle from its initial position to its final position. The trajectory exhibits smooth convergence characteristics without oscillations or divergence, indicating that the algorithm has good stability when dealing with multi-vehicle cooperative optimization problems.

[0186] Figure 6This shows how the degree of distance constraint violation changes with the number of iterations. The distance constraint violation drops rapidly from an initial value of about 1 meter, and after the 30th iteration, it stabilizes at below 0.01 meters, far below the acceptable threshold of 1.0 meter. This indicates that the algorithm can effectively handle distance constraints between vehicles, ensuring that vehicles in the formation maintain appropriate spacing, satisfying communication requirements while avoiding collision risks.

[0187] The rapid convergence of the distance constraint is mainly due to the decomposition characteristics of the ADMM algorithm. Each vehicle can independently optimize its own position based on local information, while the global distance constraint is ensured to be satisfied through the Lagrange multiplier mechanism.

[0188] The analysis results regarding the optimization effect of angle constraints are as follows: Figure 7 and Figure 8 The figures show the optimization performance for horizontal azimuth and pitch constraints, respectively. Regarding the horizontal azimuth constraint, experimental data demonstrates that the optimization algorithm exhibits good convergence characteristics. The initial azimuth error of the system was approximately 1.3 degrees. After iterative optimization, the error was significantly reduced and stabilized below 0.1 degrees, a level of accuracy far exceeding the preset acceptable threshold of 5 degrees. From the perspective of convergence speed, the algorithm demonstrates fast convergence performance, reaching a stable state after approximately the 20th iteration, proving the effectiveness of the optimization strategy.

[0189] Regarding the optimization of pitch angle constraints, the initial pitch angle error was approximately 1.2 degrees. After processing by the optimization algorithm, the system eventually stabilized at an error level of around 0.2 degrees, a result that is also far superior to the set threshold of 5 degrees. Notably, the convergence process of the pitch angle constraints exhibited good smoothness, with no obvious oscillations occurring throughout the optimization process, indicating that the algorithm possesses good numerical stability and robustness.

[0190] The successful fulfillment of angular constraints ensures the geometric accuracy of the formation in three-dimensional space, which is of great significance for the optimization of observation geometry in underwater acoustic positioning systems. By precisely controlling the relative azimuth and pitch angles between vehicles, the accuracy and reliability of cooperative positioning can be effectively improved.

[0191] The above embodiments are merely exemplary embodiments of the present invention and are not intended to limit the present invention. Those skilled in the art can make various modifications or equivalent substitutions to the present invention within its scope and spirit, and such modifications or equivalent substitutions should also be considered to fall within the scope of protection of the present invention.

Claims

1. A method for dynamically constructing a network topology of ADMM-based USV-AUV cooperative navigation, characterized in that, The method comprises the following steps: Step 1, according to the positioning equation between the surface unmanned vehicle and the autonomous underwater vehicle, the relationship between the positioning error of the autonomous underwater vehicle, the distance measurement error between the surface unmanned vehicle and the autonomous underwater vehicle and the position error between the surface unmanned vehicles is constructed by partial differentiation method; Step 2, based on the relationship between the positioning error of the autonomous underwater vehicle, the distance measurement error and the position error between the surface unmanned vehicles established in step 1, the positioning accuracy evaluation function is designed by using the optimal estimation criterion; Step 3, the optimal solution of the evaluation function is obtained by using the improved multi-objective multi-dimensional optimizer, and the best formation of the heterogeneous collaborative navigation positioning system and the best distance information between the surface unmanned vehicle and the autonomous underwater vehicle are obtained; In step 3, the alternating direction multiplier method is used to optimize the positioning accuracy evaluation function obtained in step 2, and specifically: Step 3.1: Use gradient descent to determine the location variables. Update: Each vehicle is in the same position as the other vehicles. and the current Lagrange multiplier value Under the given conditions, optimize its position to minimize the augmented Lagrange function. Where X represents the position variables of all vehicles, and Y... (k) Z is an auxiliary variable for the current distance constraint. (k) W is an auxiliary variable for the current azimuth constraint. (k) This is an auxiliary variable for the current pitch angle constraint. For the current Lagrange multiplier; Step 3.2, auxiliary variable update: fixed position variables and Lagrange multipliers updating the auxiliary variables; Step 3.3, Lagrange multiplier updating: if the actual value of the constraint is greater than the expected value, the Lagrange multiplier of the corresponding constraint is increased, and if the actual value of the constraint is less than the expected value, the Lagrange multiplier of the corresponding constraint is reduced.

2. The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method according to claim 1, characterized in that, The step 1 is specifically: Step 1.1, a state space model of the heterogeneous collaborative system of the surface unmanned vehicle and the autonomous underwater vehicle is established; Step 1.2, relative distance between vehicles , horizontal azimuth angle , pitch angle , communication connectivity establishment constraints.

3. The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method according to claim 2, characterized in that, The step 1.1 is specifically: a coordinate system is established with the task starting point as the origin, the state equation of the surface unmanned vehicle is: ; wherein, , , respectively denote the derivative of the three-dimensional spatial position coordinate with respect to time; denotes the velocity of the vehicle, is the angular velocity, respectively denote the roll angle, the pitch angle and the yaw angle of the surface unmanned vehicle; denotes the derivative of the yaw angle with respect to time; The state equation of the autonomous underwater vehicle is: ； wherein , , are the angular velocities of the roll angle, the pitch angle and the yaw angle, respectively; denotes the derivative of the pitch angle with respect to time; denotes the derivative of the roll angle with respect to time.

4. The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method according to claim 2, characterized in that, The step 2 is based on the constraint condition of step 1, and the positioning accuracy evaluation function is: ； wherein, , , are the weight coefficients of the distance constraint, the azimuth angle constraint and the elevation angle constraint, respectively; i, j represent the numbers of the vehicles, respectively, and N represents the maximum number of the vehicles; the adjacency matrix A ij represents the corresponding geometric constraints between the vehicles and , represents the horizontal azimuth angle from the position of the vehicle to the vehicle , represents the elevation angle from the position of the vehicle to the vehicle , represents the Euclidean distance between two vehicles; p i represents the position of the vehicle i; represents the desired position of the vehicle i; represents the desired Euclidean distance between two vehicles; represents the desired horizontal azimuth angle from the position of the vehicle to the vehicle ; represents the desired elevation angle from the position of the vehicle to the vehicle .

5. The ADMM-based USV-AUV cooperative navigation network topology dynamic construction method of claim 4, wherein, The expression of the augmented Lagrange function in step 3.1 is: The expression of the augmented Lagrange function in step 3.1 is: ; wherein, is a position variable for all vehicles; is an auxiliary variable for the distance constraint; is an auxiliary variable for the azimuth constraint; is an auxiliary variable for the pitch constraint; is the corresponding Lagrange multiplier; is a mapping function from position to azimuth, is a mapping function from position to pitch.

Images