High-precision positioning system for cooperative operation of underwater robot cluster

Abstract

The application relates to the fields of underwater robots and high-precision positioning technology, and specifically discloses a high-precision positioning system for underwater robot cluster cooperative operation. The system comprises a multi-source fusion positioning reference station network module, a cross-medium cooperative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception compensation module, an elastic positioning fault-tolerant system module and a cooperative positioning decision hub module. An absolute positioning reference is established through the multi-source fusion positioning reference station network, accurate pose calculation is realized in combination with the cross-medium cooperative positioning engine, dynamic topological relations among the robots are established by using the cluster relative positioning subsystem, the environment perception compensation and the elastic fault-tolerant mechanism are introduced, and finally the resource allocation is optimized through the cooperative decision hub. The application solves the high-precision positioning problem of robot cluster cooperative operation in a complex underwater environment, and can be widely applied in the fields of ocean exploration, underwater engineering and the like.

Description

TECHNICAL FIELD

[0001] The application relates to the underwater robot and high-precision positioning technical field, and particularly discloses a high-precision positioning system for underwater robot cluster cooperative operation. BACKGROUND

[0002] The existing underwater positioning system adopts a centralized reference station architecture, and the reference signal coverage radius is limited by the acoustic propagation characteristics, and signal attenuation and multipath interference occur in complex seabed canyon and reef areas. The traditional long baseline positioning system requires that the seabed beacon array geometry is strictly fixed, and cannot be dynamically adjusted according to task requirements after being laid out, and has poor adaptability in emergency task scenarios. The inertial navigation system of the mobile reference station has a drift error accumulation, and the reference transmission accuracy is significantly reduced after long-time work.

[0003] The underwater cross-medium positioning faces the problem of dynamic change of sound speed profile, and the fixed sound speed model adopted by the existing system cannot accurately reflect the actual propagation environment. There is a time synchronization error between the water surface GNSS positioning data and the underwater acoustic measurement data, and the clock drift leads to a decrease in the fusion positioning accuracy. The method of multi-source heterogeneous sensor coordinate system is not perfect, and the solution of the pose conversion matrix has an accumulation error.

[0004] In the cluster relative positioning technology, the acoustic ranging is significantly affected by the water temperature gradient, and the existing compensation algorithm does not consider the change of the three-dimensional sound speed field. The feature extraction of the visual positioning system is difficult in the water area with high suspended matter concentration, and the feature point matching success rate sharply decreases. The relative motion between robots causes frequent cycle slip of carrier phase observations, and the stability of the integer ambiguity solution is insufficient.

[0005] The system fault detection mechanism has obvious response delay, and the sensor abnormality diagnosis has misjudgment and omission phenomenon. The weight distribution strategy is fixed, and cannot dynamically adjust the confidence of each sensor according to the change of the environment. The resource scheduling algorithm has high computational complexity, and the real-time performance cannot meet the dynamic task requirements. The communication resource allocation does not consider the time-varying characteristics of the channel, and the spectrum utilization rate is low.

[0006] Therefore, the application provides a high-precision positioning system for underwater robot cluster cooperative operation to solve the above problems. SUMMARY

[0007] The application provides a high-precision positioning system for underwater robot cluster cooperative operation, which realizes centimeter-level underwater positioning through multi-module cooperative work. The system first constructs a unified absolute positioning reference field in space-time by a multi-source fusion positioning reference station network module. A cross-medium cooperative positioning engine module fuses multi-sensor data to generate an accurate pose transformation matrix. A cluster relative positioning subsystem module establishes a dynamic topological relationship between robots through carrier phase difference and visual feature matching. Meanwhile, a dynamic environment perception compensation module monitors and compensates for underwater environmental disturbances in real time. An elastic positioning fault-tolerant system module ensures reliable positioning in the event of sensor failure. Finally, a cooperative positioning decision hub module intelligently optimizes the resource allocation of the entire system.

[0008] The system innovatively solves the key technical problems in underwater multi-robot cooperative positioning: a hybrid architecture combining mobile reference stations and fixed beacon arrays is adopted, breaking the limitations of traditional system deployment inflexibility; a cross-medium data space-time synchronization algorithm is designed, effectively unifying surface and underwater positioning references; a fault-tolerant mechanism based on multi-sensor fusion is developed, significantly improving system robustness; an intelligent resource scheduling strategy is implemented according to task requirements, achieving optimal balance between positioning accuracy and energy consumption.

[0009] The purpose of the application can be achieved by the following technical solutions:

[0010] A high-precision positioning system for underwater robot cluster cooperative operation, the system includes a multi-source fusion positioning reference station network module, a cross-medium cooperative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception compensation module, an elastic positioning fault-tolerant system module, and a cooperative positioning decision hub module, wherein:

[0011] The multi-source fusion positioning reference station network module is used to implement GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame relying on the seabed acoustic beacon array and the mobile inertial reference station, and outputs a unified absolute positioning reference field in space-time.

[0012] The cross-medium cooperative positioning engine module is used to perform joint adjustment calculation on carrier motion state and acoustic ranging data combined with multi-modal sensor raw observation data, generating a continuous pose transformation matrix in the Earth coordinate system.

[0013] The cluster relative positioning subsystem module is used to collect carrier phase difference signals and visual feature point data to solve the spatial relationship between robots in three dimensions, establishing a dynamic topological relationship map.

[0014] The dynamic environment perception compensation module is used to monitor the Doppler effect compensation of the sound velocity gradient and flow field distribution of the water body on the acoustic propagation path, generating an environment parameter correction database.

[0015] The elastic positioning fault-tolerant system module is configured to detect sensor data residual error and consistency indicators, perform dynamic weight distribution on fault modes, and output reliable positioning results under degraded conditions.

[0016] The cooperative positioning decision hub module is configured to evaluate cluster task requirements and positioning resource states, implement intelligent scheduling optimization on base station working modes, and form a global positioning resource configuration scheme.

[0017] Optionally, the multi-source fusion positioning base station network module, when performing GNSS differential data and underwater acoustic time delay fusion on a spatial reference frame relying on a seabed acoustic beacon array and a mobile inertial base station, comprises:

[0018] A spatial reference network is constructed by a seabed acoustic beacon array, beacon node positions are arranged, and base station coordinate data is generated;

[0019] Carrier motion parameters are obtained by a mobile inertial base station, dynamic calibration coefficients are calculated, and base station attitude information is output;

[0020] GNSS differential data and underwater acoustic propagation time delay are fused to realize space-time reference unification and establish an absolute positioning reference field.

[0021] Optionally, the cross-medium cooperative positioning engine module, when performing joint adjustment calculation on carrier motion states and acoustic ranging data in combination with multi-modal sensor raw observation data, comprises:

[0022] An absolute positioning reference field is obtained, multi-source sensor observation data is collected, time synchronization processing is completed, and a synchronized measurement data set is generated;

[0023] Carrier motion information in the synchronized measurement data is processed, joint adjustment is implemented, and a pose conversion relationship is constructed;

[0024] Pose conversion error characteristics are analyzed, filter algorithm parameters are optimized, and an accurate pose transformation matrix is output.

[0025] Optionally, the cross-medium cooperative positioning engine module, when performing motion state estimation based on an adaptive filter algorithm, comprises:

[0026] State variables in the accurate pose transformation matrix are extracted, an error propagation model is established, and sensor error characteristics are determined;

[0027] Filter parameters are configured according to the error characteristics, an adaptive weight distribution scheme is designed, and an optimal estimation strategy is formed;

[0028] Multi-source data is fused by applying the optimal estimation strategy, and pose state information is updated.

[0029] Optionally, the cluster relative positioning subsystem module comprises the following when performing three-dimensional solving of the spatial relationship between the robot and the carrier phase difference signal and the visual feature point data pair:

[0030] Receiving the accurate pose transformation matrix, solving the carrier phase observation value, eliminating the integer ambiguity, and obtaining the relative distance measurement result;

[0031] Collecting visual feature point information, realizing three-dimensional feature matching, calculating relative azimuth, and generating relative attitude data;

[0032] Integrating relative distance and attitude measurement, establishing a cluster topology relationship model, and outputting a dynamic spatial relationship graph.

[0033] Optionally, the dynamic environment perception compensation module comprises the following when performing Doppler effect compensation on the acoustic propagation path by monitoring the water sound speed gradient and flow field distribution:

[0034] According to the dynamic spatial relationship graph, the environmental monitoring network is laid out, the sound speed profile change is measured, and the real-time sound speed data is obtained;

[0035] Analyzing the flow field distribution law, modeling the sound signal propagation path, and calculating the propagation time delay correction amount;

[0036] Applying the time delay correction amount to adjust the ranging data and updating the environment compensation database.

[0037] Optionally, the elastic positioning fault-tolerant system module comprises the following when performing dynamic weight distribution on the fault mode by detecting sensor data residuals and consistency indicators:

[0038] Calling the environment compensation database, verifying the sensor data quality, evaluating the system consistency, and diagnosing the fault type;

[0039] According to the fault diagnosis result, dynamically configuring the sensor weight, and reconstructing the data fusion process;

[0040] Performing fault-tolerant positioning solving to ensure system reliability and output robust positioning results.

[0041] Optionally, the cooperative positioning decision hub module comprises the following when evaluating cluster task requirements and positioning resource status:

[0042] Obtaining robust positioning results, analyzing job task requirements, constructing an efficiency evaluation model, and completing system state analysis;

[0043] Monitoring the positioning performance of each node, evaluating the resource configuration effect, and generating an optimization decision report.

[0044] Optionally, the cooperative positioning decision hub module comprises the following when performing intelligent scheduling optimization on the reference station working mode:

[0045] Based on the optimization decision report, the base station working mode is planned, and the wake-up scheduling strategy is formulated.

[0046] The communication resource parameters are allocated, the network topology is optimized, and the global configuration instruction is generated.

[0047] The configuration instruction is issued to coordinate each module, and the optimal operation of the system is realized.

[0048] The application provides a high-precision positioning system for underwater robot cluster cooperative operation, which realizes accurate positioning in complex underwater environment through multi-module cooperative work. The system core includes six key parts: multi-source fusion positioning base station network module, cross-medium cooperative positioning engine module, cluster relative positioning subsystem module, dynamic environment perception compensation module, elastic positioning fault-tolerant system module and cooperative positioning decision hub module. Among them, the multi-source fusion positioning base station network module adopts a hybrid architecture combining a seabed acoustic beacon array and a mobile inertial base station, and through accurate fusion of GNSS differential data and underwater acoustic time delay, an absolute positioning reference field unified in time and space is constructed, solving the problem of inflexible deployment of traditional fixed base stations.

[0049] The cross-medium cooperative positioning engine module innovatively designs a multi-modal sensor data fusion algorithm, integrates inertial navigation system, Doppler speedometer and acoustic ranging data, and realizes accurate estimation of the motion state of the carrier through adaptive Kalman filtering. This module especially develops a data alignment method based on precision time protocol for the space-time synchronization problem of surface GNSS and underwater acoustic positioning data, significantly improving the cross-medium positioning accuracy. At the same time, the cluster relative positioning subsystem module adopts a combination of carrier phase difference technology and visual feature matching, establishes a millimeter-level precision dynamic topological relationship between robots through integer ambiguity resolution and three-dimensional feature matching, and overcomes the limitations of single sensor in complex underwater environment.

[0050] The dynamic environment perception compensation module monitors the sound velocity gradient and flow field changes in real time through a distributed sensor network, establishes a sound velocity profile inversion model based on machine learning, and can accurately compensate the Doppler effect and sound path bending error in the acoustic propagation path. The elastic positioning fault-tolerant system module designs a multi-level fault detection mechanism, dynamically adjusts the fusion weight of each sensor through sensor data residual analysis and consistency evaluation, and ensures that the system can still maintain reliable positioning output when some sensors fail. Finally, the cooperative positioning decision hub module intelligently optimizes the base station wake-up strategy and communication resource configuration based on task demand and resource state evaluation, realizing the best balance between positioning accuracy and system energy consumption.

[0051] Compared with the prior art, the application has the following beneficial effects:

[0052] 1.The application realizes centimeter-level absolute positioning and millimeter-level relative positioning accuracy of underwater robots, and significantly improves the accuracy and coordination of cluster collaborative operation by constructing a unified absolute positioning reference field in space and time through a multi-source fusion positioning reference station network module, combining a multi-sensor data fusion algorithm of a cross-medium collaborative positioning engine module, and a carrier phase difference and visual feature matching technology of a cluster relative positioning subsystem module.

[0053] 2.The application monitors and compensates for water sound velocity gradient and flow field changes in real time through a dynamic environment perception compensation module, and ensures system reliability through multi-level fault detection and dynamic weight adjustment of an elastic positioning fault-tolerant system module, so that the positioning system can still work stably in turbid water, strong flow environment or partial sensor failure, greatly improving the adaptability and robustness in complex underwater environments.

[0054] 3.The application intelligently schedules reference station working mode and communication resources based on task requirements through a collaborative positioning decision hub module, achieving the best balance between positioning accuracy and energy consumption. The mixed architecture combining mobile reference stations and fixed beacon arrays reduces the deployment cost of traditional systems, and the dynamic resource allocation strategy significantly reduces the overall energy consumption of the system, prolongs the underwater operation time, and improves the task execution efficiency. BRIEF DESCRIPTION OF DRAWINGS

[0055] Figure 1 FIG. 1 is a structural schematic diagram of a high-precision positioning system for underwater robot cluster collaborative operation according to the present application. DETAILED DESCRIPTION

[0056] The technical solutions in the embodiments of the present application will be described below in conjunction with the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of the present application.

[0057] Embodiment: Figure 1 FIG. 1 is a structural schematic diagram of a high-precision positioning system for underwater robot cluster collaborative operation according to the present application. The high-precision positioning system for underwater robot cluster collaborative operation comprises the following modules:

[0058] Multi-source fusion positioning reference station network module: relying on the GNSS differential data and underwater acoustic time delay fusion of the spatial reference frame implemented by the seabed acoustic beacon array and the mobile inertial reference station, a unified absolute positioning reference field in space and time is output;

[0059] Cross-medium collaborative positioning engine module: combining multi-modal sensor raw observation data, joint adjustment calculation is performed on the carrier motion state and acoustic ranging data to generate a continuous pose transformation matrix in the earth coordinate system;

[0060] Cluster relative positioning subsystem module: Collect carrier phase differential signals and visual feature point data to solve the spatial relationship between robots in three dimensions, and establish a dynamic topological relationship atlas;

[0061] Dynamic environment perception compensation module: Monitor the sound velocity gradient and flow field distribution of the water body to compensate for the Doppler effect on the acoustic propagation path, and generate an environment parameter correction database;

[0062] Elastic positioning fault-tolerant system module: Detect sensor data residuals and consistency indicators to perform dynamic weight distribution on fault modes, and output reliable positioning results under degraded working conditions;

[0063] Collaborative positioning decision center module: Evaluate cluster task requirements and positioning resource status, intelligently schedule and optimize the working mode of the reference station, and form a global positioning resource configuration scheme.

[0064] In the embodiment of the application, the multi-source fusion positioning reference station network module performs GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame relying on the seabed acoustic beacon array and the mobile inertial reference station, and specifically includes the following processing procedures:

[0065] In the system initialization stage, each node of the seabed acoustic beacon array first performs a self-calibration program. Each beacon node is equipped with a high-precision pressure sensor and a temperature-compensated crystal oscillator, and the initial three-dimensional coordinates are determined by underwater positioning algorithm. The beacons communicate with each other using time division multiple access protocol, and the master beacon receives the GPS disciplined atomic clock signal, and synchronizes the time reference to all slave beacons through bidirectional time transfer technology, ensuring that the time synchronization accuracy of the whole network reaches the microsecond level. The beacon nodes periodically transmit acoustic positioning signals containing their own coordinates and accurate time stamps, and the signal modulation method uses binary phase shift keying to enhance the anti-multipath ability.

[0066] When working on the water surface, the reference station obtains centimeter-level positioning results through a multi-frequency multi-constellation RTK-GNSS receiver, and simultaneously collects carrier phase observation values for subsequent processing. During the diving process, the system automatically switches to the inertial navigation mode, and the inertial measurement unit composed of a fiber-optic gyroscope and a quartz accelerometer collects motion data at a frequency of 200Hz. The reference station is equipped with a depth sensor and an ultra-short baseline transceiver, which can monitor the diving trajectory in real time and maintain communication with the seabed beacon.

[0067] The system compensates for the influence of the change in sound velocity on the sound wave propagation path and corrects the sound ray bending effect through a ray tracing algorithm. Meanwhile, the inertial navigation system provides high-frequency motion state estimation, and the Kalman filter fuses GNSS position, inertial data, and acoustic ranging observations. For complex underwater environments, the system uses a robust estimation method, reduces the influence of abnormal observations through the Huber weight function, and introduces variance component estimation technology to balance the weights of different observation types.

[0068] The data processing link establishes a state space model based on the earth-fixed coordinate system, and unifies the GNSS positioning results to the underwater acoustic positioning framework through coordinate system conversion parameters. The system estimates and compensates the rod arm effect between the GNSS antenna and the underwater acoustic transducer in real time, while considering the influence of the change in the carrier attitude. The solving algorithm uses an improved weighted least squares method, ensures numerical stability through singular value decomposition, and introduces a regularization term to handle insufficient observations. After each solution, the system automatically evaluates the confidence of the positioning results, and triggers the repositioning program when the uncertainty exceeds the threshold.

[0069] The positioning results are expressed in the ECEF coordinate system, and local rectangular coordinate conversion parameters are also provided. The system updates the positioning results every 1 second, and the output data includes three-dimensional position, uncertainty ellipse parameters, and quality control indicators. For special application scenarios, the system can be configured in an event-triggered mode, which immediately outputs updates when significant position changes are detected. All output data are accompanied by complete time stamps and metadata, supporting traceability and verification in subsequent processing links.

[0070] In the embodiment of the application, the cross-medium cooperative positioning engine module specifically includes the following processing procedures when performing joint adjustment calculation on the carrier motion state and acoustic ranging data combined with the original observation data of the multi-modal sensor:

[0071] In the data preprocessing stage, the system performs time alignment processing on the original observation data from different sensors. The inertial measurement unit outputs angular velocity and linear acceleration data at a high frequency of 200Hz, the Doppler velocimeter provides three-dimensional velocity observations at a frequency of 8Hz, and the acoustic ranging system returns distance measurement values at an update rate of 1Hz. Each data packet is accompanied by an accurate time stamp, and the system uses a fifth-order polynomial interpolation algorithm to unify all observations to the same time reference. For the different transmission delay characteristics of each sensor, the module establishes a special delay compensation model, especially for the time-varying delay in the acoustic signal propagation process, which is accurately modeled and compensated to ensure that the time synchronization error is controlled within microseconds.

[0072] The state modeling module constructs a complete state vector containing 23 parameters, including 3D position, 3D velocity, 4-parameters attitude, 6 IMU biases, 3 DVL scale factors and 4 acoustic ranging biases. The system adopts a factor graph based optimization framework to represent the constraint relationships between state variables. The IMU kinematic equations are transformed into relative motion constraint factors by the pre-integration technique, the DVL velocity observations form velocity constraint factors, and the acoustic ranging generates distance constraint factors. This modeling method allows the system to flexibly add or remove various constraints while maintaining computational efficiency. All constraint factors are designed in the form of robust kernel functions to enhance the system's anti-interference ability to abnormal observations.

[0073] The optimization solving process uses the Levenberg-Marquardt algorithm to iteratively solve the nonlinear least squares problem. In each iteration, the system first calculates the residual vector and Jacobian matrix, and then constructs and solves the normal equation. According to the characteristics of different sensor observations, the system adopts an adaptive weight strategy: fixed weight is given to the IMU pre-integration constraint, the weight coefficient of the DVL velocity observation is dynamically adjusted according to the signal-to-noise ratio, and the weight of the acoustic ranging is set according to the propagation path quality evaluation result. After each iteration, the system evaluates the statistical characteristics of each observation residual, and automatically reduces the weight of the corresponding observation or triggers the re-initialization mechanism when abnormal residuals are detected.

[0074] The result output module provides the complete 6-DOF pose estimation of the carrier in the Earth coordinate system, including 3D position, velocity and attitude information. The system also outputs the uncertainty indicators of the estimation results: the position accuracy is represented as 0.2‰ relative distance, and the attitude accuracy is better than 0.1 degree. All output data are accompanied by covariance matrices and quality control flags for subsequent processing modules. For special application scenarios, the system supports configuring different output modes: the standard mode provides complete 6-DOF estimation, and the simplified mode only outputs position and heading information to save communication bandwidth. The output data adopts binary and ASCII formats, facilitating the integration and use of different systems.

[0075] In the embodiment of the application, when the cross-medium cooperative positioning engine module performs motion state estimation based on an adaptive filtering algorithm, the following processing flow is specifically included:

[0076] Filter initialization phase, the system establishes the error state Kalman filter architecture, carefully configure the process noise matrix and initial covariance matrix. Process noise matrix is set according to the calibration parameters of IMU, including angle random walk (0.001 ° / √h) and velocity random walk (0.05 m / s / √h) key indicators; The initial covariance matrix is determined according to the sensor accuracy and the initial alignment result, the position uncertainty is initialized to 1 m, and the attitude uncertainty is 1 °. The system sets the state transition matrix and the observation matrix at the same time, and the state vector contains 18 parameters such as position, velocity, attitude error and sensor bias, which lays the foundation for subsequent filtering calculation.

[0077] In the prediction update link, the system receives the preprocessed IMU data at a frequency of 100 Hz, and performs state prediction through mechanical arrangement calculation. The carrier rotation state is updated using quaternion attitude representation method, and the motion trajectory is calculated using velocity-position recursive formula, while considering the influence of earth rotation and Coriolis force. After each prediction update, the system will adjust the error covariance matrix accordingly to reflect the increasing uncertainty of state estimation. The prediction stage pays special attention to the time-varying characteristics of IMU zero bias, and estimates the slow change of zero bias by establishing a first-order Markov process model to avoid rapid accumulation of navigation error.

[0078] The observation update stage adopts a sequential processing strategy, first fusing DVL velocity observation, and then processing acoustic distance observation. For DVL data, the system establishes the observation equation of velocity error and state vector, considering the installation deviation and scale factor error; For acoustic ranging, a distance observation model is constructed, which includes sound ray bending compensation. For the problem of multipath interference of acoustic signals, the system monitors the statistical characteristics of the innovation sequence in real time, identifies abnormal observations through chi-square test, and automatically increases the corresponding observation noise variance, which can be adjusted to 100 times the normal value, so as to effectively suppress the influence of abnormal observations on the filtering result.

[0079] During system operation, the filter completes a complete prediction-update cycle every 10 ms, and outputs the state estimation and covariance matrix in real time. The covariance matrix not only reflects the positioning accuracy, but also is used for reliability evaluation and fault detection. When the acoustic signal is continuously lost for more than a certain threshold, the system automatically starts the pure inertial navigation mode, and activates the error compensation algorithm. This algorithm constrains the divergence speed of inertial navigation by establishing an IMU error growth model combined with DVL velocity observation, ensuring that the position error is controlled within 1% of the travel distance during 1 hour of pure inertial navigation, meeting the emergency navigation demand.

[0080] To guarantee the long-term stability of the filter, a periodic reset mechanism is designed. The error states are zeroed every 30 minutes while the important sensor bias estimates are preserved. When the vehicle is detected to be stationary, a zero-velocity update is performed. When high-precision acoustic positioning data is received, a re-initialization procedure is executed to correct the accumulated errors. These measures together guarantee the reliability and robustness of the filter in complex underwater environments, providing continuous and accurate state estimates for the whole positioning system.

[0081] In the embodiment of the present application, the cluster relative positioning subsystem module specifically includes the following processing procedures when performing acquisition of carrier phase differential signals and visual feature point data to develop three-dimensional solution of the spatial relationship between the robots:

[0082] In the acoustic carrier phase processing stage, the system first demodulates and extracts the phase of the received dual-frequency acoustic signals. For the wide-lane combined observation values, the initial ambiguity range is quickly determined by using the longer wavelength characteristics, reducing the possible integer solution space to 3-5 candidate values. Then, the narrow-lane signal processing is performed, and the improved LAMBDA method is used to find the optimal integer solution in the reduced search space. This process combines ambiguity variance matrix analysis and integer least squares estimation, and the reliability of ambiguity fixing is verified by Ratio test. The system also monitors the cycle slip of the carrier phase and uses three-frequency geometry-free combination for real-time detection and repair.

[0083] After the visual positioning system is started, the high-brightness LED marker images carried by the surrounding robots are collected at a rate of 30 fps by a global shutter camera. The image processing uses adaptive threshold segmentation and centroid extraction algorithm, which can stably identify the marker points even in low-contrast underwater environments. The two-dimensional image coordinates of each LED marker correspond to the pre-calibrated three-dimensional space coordinates, and the camera extrinsic parameters are solved by the EPnP algorithm to obtain the initial relative position and attitude estimation. To improve robustness, the system uses the RANSAC algorithm to remove false matching points, ensuring that the feature point re-projection error is less than 0.5 pixels. When the visual system works alone, it can provide a relative positioning accuracy of 10 cm.

[0084] The multi-sensor fusion link establishes a tightly coupled optimization framework that unifies the acoustic carrier phase double-difference observations and visual re-projection errors. The double-difference observations eliminate common-mode errors by constructing spatial geometric constraints, and the re-projection errors provide absolute scale information. The system uses a sliding window optimization strategy to maintain a state window containing 20 consecutive time points, and each time point's state variable includes position, attitude, and sensor bias. The optimization problem is solved efficiently using the Ceres solver, which supports automatic differentiation and multiple linear solver selection. To control the computational complexity, the system uses the marginalization technique to preserve the information of historical states as prior constraints.

[0085] The system monitors key performance indicators in real-time, especially the ambiguity fixing success rate. When the Ratio value of 5 consecutive fixing attempts is below the threshold of 2.5, the system automatically switches to the float solution mode. In this mode, the system maintains a float estimate of the ambiguity, while increasing the weight of visual observations to compensate for the loss of precision. When the environmental conditions improve, the system will attempt to fix the ambiguity again, which is fully automated and does not require human intervention. The system also monitors the computational load and dynamically adjusts the size of the sliding window to balance the requirements of precision and real-time performance.

[0086] The final output link provides a complete 6-DOF relative pose estimate, including 3D position and attitude quaternion. The system uses a relative distance accuracy of 1mm+1ppm, and the angle accuracy is stable within 0.05 degrees. All output data is accompanied by a covariance matrix and quality control flag, and is published through shared memory and network interface, with an update frequency of 20Hz. For special application scenarios, the system supports outputting raw observation data for post-processing or switching to a low-power mode to extend the working time. The output data format is compatible with the ROS standard, making it easy to integrate with other navigation modules.

[0087] In the embodiment of the present application, when the dynamic environment perception compensation module performs monitoring of the water sound speed gradient and the flow field distribution to implement Doppler effect compensation on the acoustic propagation path, the following processing procedures are specifically included:

[0088] In the environmental data acquisition stage, the system acquires temperature, salinity and pressure data of the water body in real time through a distributed CTD (conductivity-temperature-depth) sensor network with a sampling frequency of 1 Hz. Each sensor node is equipped with a self-contained storage and wireless transmission module, and the collected data is transmitted to the central processing unit through the underwater acoustic communication network. The system uses a time synchronization protocol to ensure the time consistency of the data of each node, and performs outlier rejection and smoothing filtering processing on the original data to eliminate the influence of transient interference. In view of the possible drift of the sensor, the system is regularly calibrated automatically to ensure the long-term measurement stability.

[0089] The sound speed field modeling link uses the Chen-Millero sound speed empirical formula, which takes into account the influence of temperature, salinity and pressure on sound speed, with a calculation accuracy of 0.1 m / s. The system first calculates the sound speed value on the three-dimensional space grid points, and then applies the Kriging spatial interpolation algorithm to construct a continuous three-dimensional sound speed field model. In the interpolation process, the underwater topographic features and anisotropy are considered, and the optimal interpolation weight is determined through the analysis of the variation function. In order to adapt to the dynamic environment, the system sets a sliding time window to update the sound speed field rolling. For special areas, the system will automatically increase the grid resolution, with a maximum of 0.1 meter level three-dimensional grid division.

[0090] The sound ray tracing and Doppler compensation stage, according to the spatial positions of the sound source and the receiver, uses Snell's law for three-dimensional ray tracing. The continuous change of the sound velocity gradient is considered in the calculation process, and the actual propagation path of the sound ray is determined by iterative solution. For the Doppler effect, the system integrates the three-dimensional flow velocity data measured by ADCP, and calculates the frequency offset according to the sound ray incidence angle. The compensation algorithm uses a second-order polynomial fitting, which not only considers the first-order Doppler shift, but also compensates for the second-order effect caused by the flow velocity gradient. The system also establishes a time-space variation model of sound velocity and flow velocity, and predicts the trend of environmental parameter changes in the next 30 seconds through time series analysis.

[0091] In terms of system implementation, the environmental compensation module adopts a hierarchical architecture design: the bottom layer is responsible for sensor data acquisition and preprocessing, the middle layer performs sound velocity field modeling and ray tracing calculation, and the upper layer implements Doppler compensation and prediction functions. Data exchange between layers is through shared memory to ensure that the processing delay is controlled within 100ms. The system supports dynamic configuration parameters, which can adjust the calculation frequency and model complexity according to different sea conditions. When the computing resources are limited, it can switch to a simplified algorithm mode, sacrificing part of the accuracy to reduce the calculation amount by 50%. All compensation results are accompanied by confidence evaluation for subsequent positioning algorithm reference.

[0092] In a uniform water environment, the ranging error can be reduced to 15%-20% of the uncompensated error; in a complex environment with strong sound velocity step layer, the compensation effect is more significant, and the error improvement rate can reach more than 80%. The system specially designs a step layer detection algorithm, which automatically enhances the calculation accuracy of ray tracing when the sound velocity gradient exceeds the threshold. Long-term testing shows that the environmental compensation module can make the entire positioning system maintain the designed accuracy index within 90% of the working time, significantly improving the adaptability of the system in complex underwater environments.

[0093] In the embodiment of the application, when the elastic positioning fault-tolerant system module performs dynamic weight allocation on the fault mode according to the consistency index of the detected sensor data residual, the following processing flow is specifically included:

[0094] The abnormality detection mechanism adopts a multi-level monitoring strategy to analyze the innovation sequence and statistical characteristics of each sensor in real time. The system establishes a hypothesis testing framework based on the chi-square test, sets a dynamic detection threshold, and triggers an abnormal alarm when the innovation sequence exceeds the threshold. For different types of sensors, the system uses differentiated detection strategies: for IMU data, mainly detect zero bias mutations, for DVL data, focus on velocity consistency, and for acoustic ranging, mainly monitor multipath effects. The detection algorithm uses a sliding time window to calculate the statistical quantity, balancing the detection sensitivity and false alarm rate. All detection results are accompanied by confidence scores to avoid single indicator misjudgment.

[0095] The sensor health evaluation system adopts a fuzzy logic method, and three types of input variables are designed: historical reliability, current residual error and environmental adaptability. Each variable is divided into 5 fuzzy levels, and 25 fuzzy rules defined by an expert knowledge base are used for comprehensive evaluation. The system maintains an independent health index for each type of sensor, which is updated once every second. The evaluation process considers the redundancy relationship between sensors. When multiple sensors of the same type simultaneously show a decrease in health, a systematic failure warning is triggered. The health index is not only used for weight distribution, but also as a decision basis for system maintenance.

[0096] The objective function maximizes the overall confidence of the fusion result, and the constraint conditions include the minimum weight guarantee of each sensor and the weight change rate limit. The solving process adopts an online QP algorithm, and the calculation period can be configured. For typical failure scenarios, the system has preset special processing strategies: when the DVL fails, the acoustic ranging weight is increased to 70%-90%; when the acoustic signal is interrupted, the IMU calculation time is extended to 5 minutes; when multiple sensors conflict, the voting mechanism is started. The weight adjustment adopts a gradual change mode to avoid sudden changes in the fusion result, and the transition time is usually controlled within 10-30 seconds.

[0097] The system is designed with a 6-level gradual fault-tolerant mode to achieve the optimal balance between performance and reliability. Mode 0 is the normal working state of all sensors; modes 1-2 correspond to the failure of a single type of sensor; modes 3-4 are mixed navigation states for multiple sensor failures; and mode 5 is pure inertial emergency navigation. An overlap area is set between each mode to achieve smooth switching through hysteresis comparison. In the worst mode 5, the system activates the IMU error suppression algorithm: suppresses the growth of speed error through zero-speed correction, restricts the vertical channel drift using a depth sensor, and regularly updates the attitude observation. Tests show that this mode can control the position error within 5% of the travel distance within 1 hour, meeting the emergency return requirements.

[0098] The system realizes modular design, including independent units such as abnormality detection, health evaluation, weight calculation and mode management. Each unit communicates through a message bus, supporting distributed deployment. The system maintains a global state machine that uniformly manages all fault-tolerant decisions to ensure logical consistency. For key parameters, an online configuration interface is provided to support dynamic adjustment according to task requirements. All decision-making processes generate detailed logs, including timestamps, decision-making basis and adjustment results, facilitating post-analysis and algorithm optimization. The average decision-making delay of the system is less than 50ms, meeting real-time requirements.

[0099] In the embodiment of the present application, the cooperative positioning decision center module specifically includes the following processing procedures when executing the evaluation cluster task demand and the positioning resource state:

[0100] The system acquires key state parameters of each underwater robot in real time through a distributed data acquisition network. The positioning accuracy index is derived from the Kalman filter covariance matrix of each node, which includes the uncertainty estimates of position and attitude. The communication link quality is evaluated comprehensively through bit error rate, signal-to-noise ratio, and packet arrival rate. The energy state monitors parameters such as battery remaining capacity, instantaneous power consumption, and temperature. All data acquisition processes use a timestamp alignment mechanism to ensure time consistency and eliminate the effects of instantaneous fluctuations through a moving average filter. The system sets validity check rules for each parameter, automatically removes abnormal data, and marks the data quality flag.

[0101] The system constructs a multi-dimensional performance evaluation matrix containing 10 core indicators, covering positioning accuracy, communication quality, and energy state. For accurate operation tasks, the positioning accuracy index is given a weight of 70%; for rapid reconnaissance tasks, the communication quality (50%) and energy state (30%) are emphasized. The evaluation algorithm uses an improved TOPSIS method, determines the objective weight through entropy weight method, and combines the subjective weight of task demand for comprehensive evaluation. During the calculation process, the indicators are standardized to eliminate the influence of dimensions, and a reward and punishment factor is introduced to handle extreme cases. The comprehensive performance score of each node is updated every minute, reflecting its immediate task execution ability.

[0102] Based on the current system state and historical running data, a high-fidelity digital twin model is constructed for prediction and simulation. The model includes three sub-modules: fluid dynamics simulation, communication channel modeling, and energy consumption prediction. It predicts the changes in resource demand for the next 5 minutes with a step size of 30 seconds. Especially for group coordination tasks, the model simulates the impact of relative motion between robots on communication links and predicts possible network partition areas. The simulation process considers environmental uncertainty and uses the Monte Carlo method to generate multiple possible scenarios to calculate the performance risk index of each node. The prediction results are combined with real-time evaluation data to generate trend curves with confidence intervals.

[0103] The evaluation results are visually displayed through a three-dimensional resource heat map, where red areas represent performance bottlenecks and green areas represent high-quality resources. The system automatically identifies key bottleneck areas such as communication dead zones or sudden drops in positioning accuracy and labels possible causes. The visualization interface supports multi-dimensional filtering and drilling analysis, allowing operators to view detailed evaluation data for any node. All display elements use adaptive rendering technology to dynamically adjust the refresh rate and detail level based on the performance of the terminal device, ensuring smooth operation.

[0104] The evaluation period can be dynamically adjusted according to the task urgency, from 1 second to 60 seconds or more. The system has a self-learning mechanism that automatically optimizes the evaluation weight and threshold parameters by analyzing historical decision effects. For network disconnection, a distributed evaluation plan is designed to allow nodes to independently calculate local performance evaluation. All evaluation results and decision recommendations are recorded in the blockchain log to ensure data integrity and support post-audit and analysis. The system monitors its health through health indicators and automatically triggers a calibration process when it detects that the evaluation deviation exceeds the threshold.

[0105] In the embodiment of the application, the cooperative positioning decision center module specifically includes the following processing procedures when performing intelligent scheduling optimization on the working mode of the reference station:

[0106] The system uses mixed integer linear programming (MILP) to establish a reference station scheduling model, and the decision variables include the wake-up state, working mode and sampling frequency of each reference station. The objective function is constructed as a multi-objective optimization problem, which minimizes the positioning error and the total energy consumption of the system. The ε-constraint method is used to generate the Pareto frontier. The constraint conditions include coverage integrity, energy consumption budget and switching frequency limit. The solver uses a branch and bound algorithm, which can obtain an approximate optimal solution that meets engineering requirements within 30 seconds for large-scale problems with 200 reference stations. The system maintains a scheduling scheme library to quickly switch between preset schemes according to environmental changes.

[0107] For the time-varying characteristics of the underwater acoustic channel, a resource allocation framework based on deep reinforcement learning is designed. The state space includes 12 features such as channel impulse response, noise spectral density and queue state; the action space defines frequency band selection, modulation method and transmission power; the reward function considers throughput, energy efficiency and interference level. The DDPG algorithm is used for training, and the digital twin environment is used to accelerate convergence in the online learning stage. In actual deployment, the system performs resource allocation decisions every 5 seconds to dynamically adapt to multipath fading and sudden interference. For important data, high-reliability frequency bands are automatically allocated and the transmission power is increased by 3-6 dB.

[0108] When a task mutation is detected, a fast reallocation mechanism based on an improved auction algorithm is activated. The resource allocation is modeled as a multi-round bidding process, and the task nodes submit resource requests according to the urgency, and the reference stations bid according to the performance-cost ratio. The algorithm introduces a virtual currency mechanism to balance global fairness and supports combined auction for related demands. Key innovations include: a dynamic bidding price adjustment strategy, a pseudo-winner exclusion mechanism and a distributed settlement protocol. Tests show that this mechanism can complete resource reallocation for 100 nodes in 10 seconds, which is 5 times faster than traditional methods. A pre-reservation protocol is also designed to ensure that high-priority tasks can immediately obtain the required resources.

[0109] Reliable command transmission: The system adopts a three-level broadcast protocol to ensure that the dispatching command is reliably delivered. The first level of broadcast uses the lowest frequency band to achieve global coverage; the second level uses medium frequency directional retransmission in non-responsive areas; and the third level uses high frequency point-to-point retransmission for key nodes. All key commands use triple-redundant transmission, and the receiving end decodes through majority voting. The protocol design has an adaptive backoff mechanism that dynamically adjusts the broadcast interval according to network load. The data packet structure contains forward error correction code and CRC-32 check, and the measured packet loss rate is less than 0.1%. The system monitors the command execution status in real time, and unconfirmed nodes will trigger automatic execution of local cached commands.

[0110] The system constructs an environment change perception network and evaluates the degree of environmental disturbance in real time through 10 monitoring indicators. When any indicator exceeds the adaptive threshold, the dispatching scheme is automatically recalculated. The optimization process uses an incremental update strategy: the basic structure of the original scheme is retained, and only the affected part is adjusted, reducing the calculation time by 60%. The system maintains a two-level response mechanism: regular changes perform local adjustments (response delay <1 second); major changes (≥3 indicators) trigger global reconstruction (delay <2 seconds). All adjustment processes guarantee service continuity, and the system is always in a consistent state through state snapshots and rollback mechanisms. An artificial intervention interface is also designed to allow operators to override automatic decisions.

[0111] It should be noted that the relational terms herein such as first and second and the like are used solely to distinguish one from another entity or action, without necessarily requiring or implying that any such entity or action are in fact present. Moreover, the terms "comprises", "comprising", or any other variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0112] Although embodiments of the present application have been shown and described, it is to be understood that various modifications, substitutions, replacements and changes can be made to these embodiments without departing from the principles and spirit of the present application, and the scope of the present application is defined by the appended claims and their equivalents.

Claims

1. A high-precision positioning system for cooperative operation of an underwater robot cluster, characterized in that, The system comprises a multi-source fusion positioning reference station network module, a cross-medium cooperative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception compensation module, an elastic positioning fault-tolerant system module, and a cooperative positioning decision hub module, wherein: The multi-source fusion positioning reference station network module is configured to implement GNSS differential data and underwater acoustic time delay fusion on a spatial reference frame by relying on a seabed acoustic beacon array and a mobile inertial reference station, and output a unified absolute positioning reference field in space and time. The cross-medium cooperative positioning engine module is configured to perform joint adjustment calculation on carrier motion state and acoustic ranging data in combination with multi-modal sensor raw observation data, and generate a continuous pose transformation matrix in the earth coordinate system. The cluster relative positioning subsystem module is configured to collect carrier phase difference signals and visual feature point data to solve the spatial relationship among robots in three dimensions, and establish a dynamic topological relationship graph. The dynamic environment perception compensation module is configured to monitor the distribution of water sound velocity gradient and flow velocity field to implement Doppler effect compensation on acoustic propagation path, and generate an environment parameter correction database. The elastic positioning fault-tolerant system module is configured to detect sensor data residuals and consistency indicators to perform dynamic weight distribution on fault modes, and output reliable positioning results under degraded working conditions. The cooperative positioning decision hub module is configured to evaluate cluster task requirements and positioning resource status, implement intelligent scheduling optimization on reference station working modes, and form a global positioning resource configuration scheme.

2. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, When the multi-source fusion positioning reference station network module implements GNSS differential data and underwater acoustic time delay fusion on a spatial reference frame by relying on a seabed acoustic beacon array and a mobile inertial reference station, it comprises: Constructing a spatial reference network through a seabed acoustic beacon array, arranging beacon node positions, and generating reference station coordinate data; Obtaining carrier motion parameters with a mobile inertial reference station, calculating dynamic calibration coefficients, and outputting reference station attitude information; Fusing GNSS differential data and underwater acoustic propagation time delay to realize space-time reference unification and establish an absolute positioning reference field.

3. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, When the cross-medium cooperative positioning engine module performs joint adjustment calculation on carrier motion state and acoustic ranging data in combination with multi-modal sensor raw observation data, it comprises: Obtaining an absolute positioning reference field, collecting multi-source sensor observation data, completing time synchronization processing, and generating a synchronized measurement data set; Processing carrier motion information in the synchronized measurement data, implementing joint adjustment calculation, and constructing a pose conversion relationship; Analyzing pose conversion error characteristics, optimizing filter algorithm parameters, and outputting an accurate pose transformation matrix. When the cross-medium cooperative positioning engine module performs motion state estimation based on an adaptive filter algorithm, it comprises:

4. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, Extracting state variables from the accurate pose transformation matrix, establishing an error propagation model, and determining sensor error characteristics; Configuring filter parameters according to error characteristics, designing an adaptive weight distribution scheme, and forming an optimal estimation strategy; Fusing multi-source data using the optimal estimation strategy and updating pose state information. ​ 5. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, The cluster relative positioning subsystem module comprises the following steps when performing three-dimensional solution of the spatial relationship between the robot and the carrier phase difference signal and the visual feature point data pair: Receiving the accurate pose transformation matrix, solving the carrier phase observation value, eliminating the integer ambiguity, and obtaining the relative distance measurement result; Collecting visual feature point information, realizing three-dimensional feature matching, calculating the relative azimuth angle, and generating relative attitude data; Integrating relative distance and attitude measurement, establishing a cluster topology relationship model, and outputting a dynamic spatial relationship graph.

6. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, The dynamic environment perception compensation module comprises the following steps when performing Doppler effect compensation on the acoustic propagation path by monitoring the sound velocity gradient and the flow field distribution of the water body: According to the dynamic spatial relationship graph, the environmental monitoring network is laid out, the sound velocity profile change is measured, and the real-time sound velocity data is obtained; Analyzing the flow field distribution law, modeling the sound signal propagation path, and calculating the propagation time delay correction amount; Adjusting the ranging data by using the time delay correction amount, and updating the environmental compensation database.

7. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, The elastic positioning fault-tolerant system module comprises the following steps when performing dynamic weight distribution on the fault mode by detecting sensor data residuals and consistency indicators: Call the environmental compensation database, check the sensor data quality, evaluate the system consistency, and diagnose the fault type; According to the fault diagnosis result, dynamically configure the sensor weight, and reconstruct the data fusion process; Performing fault-tolerant positioning solution to ensure system reliability and output robust positioning results.

8. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, The cooperative positioning decision center module comprises the following steps when evaluating the cluster task demand and the positioning resource state: Obtain robust positioning results, analyze job task requirements, build an efficiency evaluation model, and complete system state analysis; Monitoring the positioning performance of each node, evaluating the resource allocation effect, and generating an optimization decision report.

9. The high-precision positioning system for cooperative operation of an underwater robot cluster according to claim 1, characterized in that, The cooperative positioning decision center module comprises the following steps when performing intelligent scheduling optimization on the reference station working mode: Based on the optimization decision report, plan the reference station working mode, and develop a wake-up scheduling strategy; Distribute communication resource parameters, optimize network topology structure, and generate global configuration instructions; Distribute configuration instructions to coordinate each module to achieve optimal system operation.

Images