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

Abstract

The invention relates to the technical field of underwater robots and high-precision positioning, and particularly discloses a high-precision positioning system for collaborative operation of an underwater robot cluster. The system comprises a multi-source fusion positioning base 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 center module. An absolute positioning reference is constructed through a multi-source fusion positioning reference station network, a cross-medium cooperative positioning engine is combined to realize accurate pose calculation, a cluster relative positioning subsystem is utilized to establish a dynamic topological relation between robots, an environment perception compensation and elastic fault-tolerant mechanism is introduced, and finally resource configuration is optimized through a cooperative decision center. The method solves the problem of high-precision positioning of robot cluster collaborative operation in a complex underwater environment, and can be widely applied to 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] This invention provides a high-precision positioning system for collaborative underwater robot swarms. This system achieves centimeter-level underwater positioning accuracy through the collaborative operation of multiple modules. The system first uses a multi-source fusion positioning reference station network module to construct a unified absolute positioning reference field in time and space. The cross-medium collaborative positioning engine module fuses multi-sensor data to generate a precise pose transformation matrix. The cluster relative positioning subsystem module establishes dynamic topological relationships between robots through carrier phase differentiation and visual feature matching. Simultaneously, the dynamic environment perception and compensation module monitors and compensates for underwater environmental disturbances in real time. The elastic positioning fault-tolerant system module ensures reliable positioning in the event of sensor failure. Finally, the collaborative positioning decision-making hub module intelligently optimizes resource allocation across the entire system.

[0008] This system innovatively solves the key technical difficulties in the collaborative positioning of multiple underwater robots: it adopts a hybrid architecture combining mobile base stations and fixed beacon arrays, breaking through the limitations of inflexible deployment of traditional systems; it designs a cross-media data spatiotemporal synchronization algorithm to effectively unify the surface and underwater positioning benchmarks; it develops a fault-tolerant mechanism based on multi-sensor fusion, which significantly improves the robustness of the system; and it implements an intelligent resource scheduling strategy that is dynamically adjusted according to mission requirements, so as to achieve an optimal balance between positioning accuracy and energy consumption.

[0009] The purpose of the present invention can be achieved through the following technical solutions: A high-precision positioning system for collaborative operation of underwater robot clusters, comprising a multi-source fusion positioning base station network module, a cross-media collaborative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception and compensation module, a flexible positioning fault-tolerant system module, and a collaborative positioning decision-making center module, wherein: The multi-source fusion positioning reference station network module is used to rely on the seabed acoustic beacon array and the mobile inertial reference station to implement GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame, and output a unified absolute positioning reference field in time and space; The cross-media collaborative positioning engine module is used to perform a joint adjustment calculation on the carrier motion state and acoustic ranging data in combination with the original observation data of the multimodal sensor to generate a continuous pose transformation matrix in the earth coordinate system; The cluster relative positioning subsystem module is used to collect carrier phase differential signals and visual feature point data to perform three-dimensional calculations on the spatial relationship between robots and establish a dynamic topological relationship map; The dynamic environment perception and compensation module is used to monitor the water body sound velocity gradient and flow velocity field distribution to implement Doppler effect compensation on the acoustic propagation path and generate an environmental parameter correction database; The elastic positioning fault-tolerant system module is used to detect sensor data residuals and consistency indicators, perform dynamic weight allocation on fault modes, and output reliable positioning results under degraded conditions; The collaborative positioning decision-making central module is used to evaluate cluster task requirements and positioning resource status, implement intelligent scheduling optimization for reference station working modes, and form a global positioning resource configuration plan.

[0010] Optionally, when the multi-source fusion positioning reference station network module performs GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame based on the seabed acoustic beacon array and the mobile inertial reference station, it includes: A spatial reference network is constructed through an array of seabed acoustic beacons, beacon node positions are arranged, and base station coordinate data is generated; The carrier motion parameters are obtained with the help of the mobile inertial reference station, the dynamic calibration coefficients are calculated, and the reference station attitude information is output; By integrating GNSS differential data with underwater acoustic propagation delay, we can unify the time and space references and establish an absolute positioning reference field.

[0011] Optionally, when performing a joint adjustment calculation on the carrier motion state and acoustic ranging data in combination with the original observation data of the multimodal sensor, the cross-media collaborative positioning engine module includes: Obtain absolute positioning reference field, collect multi-source sensor observation data, complete time synchronization processing, and generate synchronized measurement data sets; Process the carrier motion information in the synchronous measurement data, implement joint adjustment solution, and construct the posture transformation relationship; Analyze the pose conversion error characteristics, optimize the filtering algorithm parameters, and output the accurate pose transformation matrix.

[0012] Optionally, when performing motion state estimation based on an adaptive filtering algorithm, the cross-media collaborative positioning engine module includes: Extract state variables from the precise pose transformation matrix, establish an error propagation model, and determine the sensor error characteristics; Configure filter parameters according to error characteristics, design an adaptive weight distribution scheme, and form an optimal estimation strategy; Apply the optimal estimation strategy to fuse multi-source data and update the pose state information.

[0013] Optionally, when the cluster relative positioning subsystem module collects carrier phase differential signals and visual feature point data to perform three-dimensional solution on the spatial relationship between robots, it includes: Receive the precise pose transformation matrix, solve the carrier phase observation value, eliminate the integer ambiguity, and obtain the relative distance measurement result; Collect visual feature point information, achieve three-dimensional feature matching, calculate relative azimuth angles, and generate relative posture data; Integrate relative distance and posture measurements, establish a cluster topology relationship model, and output a dynamic spatial relationship map.

[0014] Optionally, when the dynamic environment perception compensation module monitors the water body sound velocity gradient and flow velocity field distribution to implement Doppler effect compensation on the acoustic propagation path, it includes: Based on the dynamic spatial relationship map, an environmental monitoring network is deployed to measure the changes in sound velocity profile and obtain real-time sound velocity data; Analyze the distribution of flow velocity field, model the acoustic signal propagation path, and calculate the propagation delay correction; Apply the delay correction to adjust the ranging data and update the environmental compensation database.

[0015] Optionally, when the elastic positioning fault-tolerant system module performs dynamic weight assignment on the fault mode by detecting the residual error of sensor data and the consistency index, the elastic positioning fault-tolerant system module includes: Call the environmental compensation database, verify sensor data quality, evaluate system consistency, and diagnose fault types; According to the fault diagnosis results, the sensor weights are dynamically configured and the data fusion process is reconstructed; Perform fault-tolerant positioning solutions to ensure system reliability and output robust positioning results.

[0016] Optionally, when evaluating cluster task requirements and positioning resource status, the collaborative positioning decision center module includes: Obtain robust positioning results, analyze task requirements, build performance evaluation models, and complete system status analysis; Monitor the positioning performance of each node, evaluate the resource allocation effect, and generate optimization decision reports.

[0017] Optionally, when the collaborative positioning decision center module performs intelligent scheduling optimization on the reference station working mode, it includes: Based on the optimization decision report, plan the base station working mode and formulate the wake-up scheduling strategy; Allocate communication resource parameters, optimize network topology, and generate global configuration instructions; Issue configuration instructions to coordinate various modules to achieve optimal system operation.

[0018] The present invention provides a high-precision positioning system for the collaborative operation of underwater robot clusters. The system achieves precise positioning in complex underwater environments through the collaborative operation of multiple modules. The core of the system includes six key parts: a multi-source fusion positioning reference station network module, a cross-media collaborative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception compensation module, an elastic positioning fault-tolerant system module, and a collaborative positioning decision-making center module. Among them, the multi-source fusion positioning reference station network module adopts a hybrid architecture that combines an array of seabed acoustic beacons with a mobile inertial reference station. Through the precise fusion of GNSS differential data and underwater acoustic time delay, it constructs a unified absolute positioning reference field in time and space, solving the problem of inflexible deployment of traditional fixed reference stations.

[0019] The cross-media collaborative positioning engine module innovatively designs a multimodal sensor data fusion algorithm, integrating inertial navigation systems, Doppler velocimeters, and acoustic ranging data, and achieves accurate estimation of the carrier's motion state through adaptive Kalman filtering. Specifically addressing the spatiotemporal synchronization issues between surface GNSS and underwater acoustic positioning data, this module has developed a data alignment method based on the Precision Time Protocol, significantly improving cross-media positioning accuracy. Meanwhile, the cluster relative positioning subsystem module combines carrier phase differential technology with visual feature matching. By resolving whole-cycle ambiguities and matching three-dimensional features, it establishes millimeter-level precision dynamic topological relationships between robots, overcoming the limitations of single sensors in complex underwater environments.

[0020] The dynamic environmental perception and compensation module monitors the changes in the water body's sound velocity gradient and flow field in real time through a distributed sensor network, and establishes a sound velocity profile inversion model based on machine learning, which can accurately compensate for the Doppler effect and sound line bending error in the acoustic propagation path. The elastic positioning fault-tolerant system module designs a multi-level fault detection mechanism. Through sensor data residual analysis and consistency evaluation, it dynamically adjusts the fusion weights of each sensor to ensure that the system can still maintain reliable positioning output when some sensors fail. Finally, the collaborative positioning decision-making center module intelligently optimizes the base station wake-up strategy and communication resource configuration based on task requirements and resource status assessment, achieving the optimal balance between positioning accuracy and system energy consumption.

[0021] Compared with the prior art, the present invention has the following beneficial effects: 1. The present invention constructs a unified absolute positioning reference field in time and space through a multi-source fusion positioning reference station network module. Combined with the multi-sensor data fusion algorithm of the cross-media collaborative positioning engine module and the carrier phase difference and visual feature matching technology of the cluster relative positioning subsystem module, it achieves centimeter-level absolute positioning and millimeter-level relative positioning accuracy of underwater robots, significantly improving the accuracy and coordination of cluster collaborative operations.

[0022] 2. The present invention uses a dynamic environmental perception and compensation module to monitor and compensate for changes in the water body's sound velocity gradient and flow field in real time. The elastic positioning fault-tolerant system module ensures system reliability through multi-level fault detection and dynamic weight adjustment, enabling the positioning system to maintain stable operation in turbid waters, strong current environments, or when partial sensors fail, greatly improving its adaptability and robustness in complex underwater environments.

[0023] 3. This invention achieves an optimal balance between positioning accuracy and energy consumption by intelligently scheduling reference station operating modes and communication resources based on mission requirements through a collaborative positioning decision-making hub module. This hybrid architecture, combining mobile reference stations with a fixed beacon array, reduces the deployment costs of traditional systems. The dynamic resource allocation strategy significantly reduces overall system energy consumption, extends underwater operation time, and improves mission execution efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 This is a structural schematic diagram of a high-precision positioning system for collaborative operation of underwater robot clusters according to the present invention. DETAILED DESCRIPTION

[0025] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0026] Example: Figure 1 The present invention provides a structural diagram of a high-precision positioning system for collaborative operation of underwater robot clusters. The high-precision positioning system for collaborative operation of underwater robot clusters includes the following modules: Multi-source fusion positioning reference station network module: Relying on the seabed acoustic beacon array and the mobile inertial reference station, it implements GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame, and outputs a unified absolute positioning reference field in time and space; Cross-media collaborative positioning engine module: Combines the original observation data of multimodal sensors to perform joint adjustment calculations on the carrier motion state and acoustic ranging data, generating a continuous pose transformation matrix in the earth coordinate system; Cluster relative positioning subsystem module: collects carrier phase differential signals and visual feature point data to perform three-dimensional calculations on the spatial relationship between robots and establish a dynamic topological relationship map; Dynamic environment perception and compensation module: monitors the water body sound velocity gradient and flow velocity field distribution to implement Doppler effect compensation for the acoustic propagation path and generate an environmental parameter correction database; Elastic positioning fault-tolerant system module: Detects sensor data residuals and consistency indicators, performs dynamic weight assignment on fault modes, and outputs reliable positioning results under degraded conditions; Collaborative positioning decision-making center module: evaluates cluster task requirements and positioning resource status, implements intelligent scheduling optimization of base station working modes, and forms a global positioning resource allocation plan.

[0027] In an embodiment of the present invention, the multi-source fusion positioning reference station network module specifically includes the following processing flow when performing GNSS differential data and underwater acoustic delay fusion on the spatial reference frame based on the seabed acoustic beacon array and the mobile inertial reference station: During the system initialization phase, each node in the submarine acoustic beacon array first performs a self-calibration procedure. Each beacon node is equipped with a high-precision pressure sensor and a temperature-compensated crystal oscillator, and uses an underwater positioning algorithm to determine its initial three-dimensional coordinates. Beacons communicate using a time-division multiple access protocol. The master beacon receives signals from a GPS-disciplined atomic clock and synchronizes the time base to all slave beacons using two-way time transfer technology, ensuring microsecond-level time synchronization across the entire network. Each beacon node periodically transmits an acoustic positioning signal containing its coordinates and a precise timestamp. The signal modulation method uses binary phase-shift keying to enhance multipath mitigation.

[0028] When operating on the surface, the base station uses a multi-frequency, multi-constellation RTK-GNSS receiver to obtain centimeter-level positioning results and simultaneously collects carrier phase observations for subsequent processing. During descent, the system automatically switches to inertial navigation mode, where an inertial measurement unit consisting of a fiber optic gyroscope and a quartz accelerometer collects motion data at a frequency of 200 Hz. Equipped with a depth sensor and an ultra-short baseline transceiver, the base station monitors the descent trajectory in real time and maintains communication with seafloor beacons.

[0029] The system compensates for variations in sound velocity along the propagation path, correcting for the effects of sound ray curvature using a ray tracing algorithm. Simultaneously, an inertial navigation system provides high-frequency motion state estimation, and a Kalman filter fuses GNSS position, inertial data, and acoustic ranging observations. To address complex underwater environments, the system employs robust estimation methods, using Huber weighting functions to mitigate the impact of anomalous observations and incorporating variance component estimation techniques to balance the weights of different observation types.

[0030] The data processing phase establishes a state-space model based on an Earth-fixed coordinate system, unifying GNSS positioning results into an underwater acoustic positioning framework using coordinate system conversion parameters. The system estimates and compensates for the lever-arm effect between the GNSS antenna and the underwater acoustic transducer in real time, while also accounting for the impact of changes in the carrier's attitude. The solution algorithm utilizes a modified weighted least squares method, employs singular value decomposition to ensure numerical stability, and introduces a regularization term to address insufficient observations. After each solution, the system automatically assesses the confidence level of the positioning result, triggering a repositioning procedure when uncertainty exceeds a threshold.

[0031] Positioning results are expressed in the ECEF coordinate system, with local Cartesian coordinate conversion parameters provided. The system updates positioning results every second, outputting data including 3D position, uncertainty ellipse parameters, and quality control metrics. For specialized applications, the system can be configured in event-triggered mode, providing immediate output updates upon detection of significant position changes. All output data is fully time-stamped and metadata-enabled, enabling traceability and verification during subsequent processing.

[0032] In an embodiment of the present invention, the cross-media collaborative positioning engine module performs the following processing flow when performing a joint adjustment calculation on the carrier motion state and acoustic ranging data by combining the original observation data of the multimodal sensor: During the data preprocessing phase, the system time-aligns the raw 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 measurements at an update rate of 1Hz. Each data packet is accompanied by a precise timestamp, and the system uses a fifth-order polynomial interpolation algorithm to unify all observations to the same time reference. The module establishes a dedicated delay compensation model to address the different transmission delay characteristics of each sensor. In particular, it accurately models and compensates for the time-varying delay in the propagation of acoustic signals, ensuring that time synchronization errors are controlled at the microsecond level.

[0033] The state modeling phase constructs a complete state vector containing 23 parameters, including 3D position, 3D velocity, 4-dimensional attitude, 6 IMU zero biases, 3 DVL scale factors, and 4 acoustic ranging system deviation parameters. The system uses a factor graph-based optimization framework to represent the constraint relationships between the state variables. The IMU kinematic equations are converted into relative motion constraint factors through pre-integration techniques, DVL velocity observations form velocity constraint factors, and acoustic ranging generates distance constraint factors. This modeling approach allows the system to flexibly add or remove various constraints while maintaining computational efficiency. All constraint factors are designed as robust kernel functions to enhance the system's ability to resist interference from abnormal observations.

[0034] The optimization process uses the Levenberg-Marquardt algorithm to iteratively solve nonlinear least-squares problems. In each iteration, the system first calculates the residual vector and Jacobian matrix, then constructs and solves the normal equations. Based on the characteristics of different sensor observations, the system adopts an adaptive weighting strategy: fixed weights are assigned to IMU pre-integration constraints, weight coefficients for DVL velocity observations are dynamically adjusted based on the signal-to-noise ratio, and weights for acoustic ranging are set based on propagation path quality assessment results. After each iteration, the system evaluates the statistical characteristics of each observation residual. When abnormal residuals are detected, the corresponding observation weight is automatically reduced or a reinitialization mechanism is triggered.

[0035] The output link provides a complete 6-DOF pose estimate of the carrier in the Earth coordinate system, including 3D position, velocity, and attitude information. The system also outputs uncertainty indicators for the estimated results: position accuracy is expressed as 0.2‰ of the relative distance, and attitude accuracy is better than 0.1 degrees. All output data is accompanied by a covariance matrix and quality control flags for use by subsequent processing modules. For special application scenarios, the system supports the configuration of different output modes: standard mode provides a complete 6-DOF estimate, while simplified mode only outputs position and heading information to save communication bandwidth. The output data is in binary and ASCII formats to facilitate integration and use in different systems.

[0036] In the embodiment of the present invention, when the cross-media collaborative positioning engine module performs motion state estimation based on the adaptive filtering algorithm, the following processing flow is specifically included: During the filter initialization phase, the system establishes the error-state Kalman filter architecture, carefully configuring the process noise matrix and initial covariance matrix. The process noise matrix is ​​set based on the IMU calibration parameters and includes key metrics such as the angular random walk (0.001° / √h) and the velocity random walk (0.05m / s / √h). The initial covariance matrix is ​​determined based on sensor accuracy and initial alignment results, with position uncertainty initialized to 1m and attitude uncertainty to 1°. The system also sets the state transfer matrix and observation matrix. The state vector contains 18 parameters, including position, velocity, attitude error, and sensor bias, laying the foundation for subsequent filtering calculations.

[0037] During the prediction update phase, the system receives preprocessed IMU data at a frequency of 100Hz and performs state predictions through mechanically orchestrated calculations. The carrier's rotational state is updated using quaternion attitude representation, and the trajectory is calculated using a velocity-position recursive formula, taking into account factors such as Earth's rotation and the Coriolis force. After each prediction update, the system adjusts the error covariance matrix to reflect the increasing uncertainty in the state estimate. During the prediction phase, special attention is paid to the time-varying characteristics of the IMU bias. A first-order Markov process model is established to estimate the slowly changing bias and avoid the rapid accumulation of navigation errors.

[0038] The observation update phase employs a sequential processing strategy, first integrating DVL velocity observations and then processing acoustic range observations. For DVL data, the system establishes an observation equation for velocity error and state vector, accounting for installation bias and scale factor errors. For acoustic ranging, a distance observation model is constructed that includes compensation for ray bending. To address multipath interference in acoustic signals, the system monitors the statistical characteristics of the new information sequence in real time, identifies anomalous observations through a chi-square test, and automatically increases the corresponding observation noise variance by up to 100 times the normal value, effectively suppressing the impact of anomalous observations on filtering results.

[0039] During system operation, the filter completes a full prediction-update cycle every 10ms, outputting a state estimate and covariance matrix in real time. The covariance matrix not only reflects positioning accuracy but is also used for reliability assessment and fault detection. When the continuous loss of acoustic signals exceeds a set threshold, the system automatically initiates pure inertial navigation mode and activates an error compensation algorithm. This algorithm establishes an IMU error growth model and, combined with DVL velocity observations, constrains the divergence rate of inertial navigation. This ensures that during a one-hour pure inertial navigation period, the position error is kept within 1% of the distance traveled, meeting emergency navigation requirements.

[0040] To ensure the long-term stability of the filter, the system incorporates a periodic reset mechanism. The error state is reset every 30 minutes, while retaining important sensor bias estimates. Zero-speed correction is automatically performed when the vehicle is detected as stationary. Upon receiving high-precision acoustic positioning data, a reinitialization procedure corrects accumulated errors. These measures ensure the reliability and robustness of the filter system in complex underwater environments, providing continuously accurate state estimates for the entire positioning system.

[0041] In an embodiment of the present invention, the cluster relative positioning subsystem module includes the following processing flow when performing three-dimensional solution of the spatial relationship between robots by collecting carrier phase difference signals and visual feature point data: During the acoustic carrier phase processing phase, the system first demodulates and extracts the phase of the received dual-frequency acoustic signal. For wide-lane combined observations, the system leverages their longer wavelengths to quickly determine the initial ambiguity range, narrowing the space of possible integer solutions to 3-5 candidate values. It then transitions to narrow-lane signal processing, employing an improved LAMBDA method to search for the optimal integer solution within the reduced-dimensional search space. This process combines ambiguity variance matrix analysis with integer least squares estimation, and the reliability of ambiguity fixation is verified through a ratio test. The system also monitors carrier phase cycle slips, detecting and correcting them in real time using a three-frequency geometry-free combination.

[0042] After the visual positioning system is synchronously activated, a global shutter camera captures images of the high-brightness LED markers mounted on surrounding robots at a rate of 30fps. Image processing utilizes adaptive threshold segmentation and centroid extraction algorithms, ensuring stable marker identification even in low-contrast underwater environments. The 2D image coordinates of each LED marker correspond to pre-calibrated 3D spatial coordinates. The EPnP algorithm is used to solve for camera extrinsics to obtain an initial relative position and pose estimate. To improve robustness, the system uses the RANSAC algorithm to eliminate mismatched points, ensuring that the reprojection error of feature points involved in the solution is less than 0.5 pixels. When operating independently, the visual system can provide relative positioning accuracy of 10cm.

[0043] The multi-sensor fusion process establishes a tightly coupled optimization framework that unifies the modeling of acoustic carrier phase double-difference observations and visual reprojection errors. Double-difference observations eliminate common-mode errors by constructing spatial geometric constraints, while reprojection errors provide absolute scale information. The system employs a sliding window optimization strategy, maintaining a state window encompassing 20 consecutive moments. The state variables at each moment include position, attitude, and sensor bias. The optimization problem is efficiently computed using the Ceres solver, which supports automatic differentiation and a variety of linear solver options. To control computational complexity, the system employs marginalization techniques to retain historical state information as prior constraints.

[0044] During operation, the system monitors key performance indicators in real time, particularly the ambiguity fixation success rate. When the Ratio value for five consecutive fixation attempts falls below a threshold of 2.5, it automatically switches to floating-point solution mode. In this mode, the system maintains the floating-point estimate of the ambiguity while increasing the weight of visual observations to compensate for the loss of accuracy. When environmental conditions improve, the system attempts to refix the ambiguity. This process is fully automated and requires no human intervention. The system also monitors computational load and dynamically adjusts the sliding window size when resources are limited, balancing accuracy and real-time requirements.

[0045] The final output provides a complete 6-DOF relative pose estimate, including 3D position and attitude quaternions. The system uses a relative distance accuracy of 1mm + 1ppm, with angular accuracy stable within 0.05 degrees. All output data is accompanied by a covariance matrix and quality control flags. It is published via shared memory and a 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 low-power mode to extend operating time. The output data format is compatible with the ROS standard, facilitating integration with other navigation modules.

[0046] In an embodiment of the present invention, the dynamic environment perception and compensation module includes the following processing steps when performing Doppler effect compensation on the acoustic propagation path by monitoring the water body sound velocity gradient and flow velocity field distribution: During the environmental data collection phase, the system uses a distributed network of CTD (conductivity-temperature-depth) sensors to acquire real-time water temperature, salinity, and pressure data at a sampling frequency of 1Hz. Each sensor node is equipped with self-contained storage and wireless transmission modules, transmitting the collected data to the central processing unit via an underwater acoustic communication network. The system uses a time synchronization protocol to ensure temporal consistency of data across nodes. It also performs outlier removal and smoothing filtering on the raw data to eliminate the effects of transient interference. To address potential sensor drift, the system performs regular automatic calibration to ensure long-term measurement stability.

[0047] The sound velocity field modeling phase uses the Chen-Millero sound velocity empirical formula, which comprehensively considers the effects of temperature, salinity, and pressure on sound velocity, with a calculation accuracy of up to 0.1 m / s. The system first calculates the sound velocity values ​​at discrete grid points in three-dimensional space, and then applies the Kriging spatial interpolation algorithm to construct a continuous three-dimensional sound velocity field model. The interpolation process takes into account underwater terrain characteristics and anisotropy, and the optimal interpolation weight is determined through variogram analysis. To adapt to dynamic environments, the system sets a sliding time window to perform rolling updates on the sound velocity field. For special areas, the system automatically refines the grid resolution, achieving up to 0.1-meter-level three-dimensional grid division.

[0048] During the sound ray tracing and Doppler compensation phase, the system uses Snell's law to perform three-dimensional ray tracing based on the spatial positions of the sound source and receiver. The calculation process takes into account the continuous change in the sound velocity gradient, and the actual propagation path of the sound ray is determined through iterative solution. To address the Doppler effect, the system integrates three-dimensional flow velocity data measured by ADCP and calculates the frequency deviation based on the sound ray's incident angle. The compensation algorithm uses a second-order polynomial fit, which not only considers the first-order Doppler frequency shift but also compensates for the second-order effects caused by the flow velocity gradient. The system also establishes a spatiotemporal variation model for sound velocity and flow velocity, and uses time series analysis to predict the changing trends of environmental parameters within the next 30 seconds.

[0049] In terms of system implementation, the environmental compensation module adopts a layered architecture: the bottom layer is responsible for sensor data acquisition and preprocessing, the middle layer performs sound velocity field modeling and ray tracing calculations, and the upper layer implements Doppler compensation and prediction functions. Data is exchanged between these layers via shared memory, ensuring processing latency is kept below 100ms. The system supports dynamic parameter configuration, adjusting the calculation frequency and model complexity according to varying sea conditions. When computing resources are limited, a simplified algorithm mode can be switched, sacrificing some accuracy in exchange for a 50% reduction in computational complexity. All compensation results are accompanied by a confidence assessment for reference in subsequent positioning algorithms.

[0050] In uniform water environments, ranging errors can be reduced to 15%-20% of the uncompensated state. In complex environments with strong sonic jumps, the compensation effect is even more pronounced, with error improvements exceeding 80%. The system features a specially designed jump detection algorithm that automatically enhances ray tracing accuracy when it detects a sonic gradient exceeding a threshold. Long-term testing has shown that this environmental compensation module enables the entire positioning system to maintain its designed accuracy 90% of the time, significantly improving the system's adaptability in complex underwater environments.

[0051] In an embodiment of the present invention, the elastic positioning fault-tolerant system module includes the following processing flow when performing dynamic weight assignment on the fault mode by detecting sensor data residuals and consistency indicators: The anomaly detection mechanism employs a multi-level monitoring strategy, analyzing the innovation sequences and statistical characteristics of each sensor in real time. The system establishes a hypothesis testing framework based on the chi-square test and sets a dynamic detection threshold. When the innovation sequence exceeds the threshold, an anomaly alert is triggered. Differentiated detection strategies are employed for different sensor types: IMU data primarily detects zero-bias mutations, DVL data focuses on velocity consistency, and acoustic ranging data prioritizes multipath effects. The detection algorithm uses a sliding time window to calculate statistics, balancing detection sensitivity and false alarm rate. All detection results are accompanied by a confidence score to prevent misjudgment based on a single metric.

[0052] The sensor health assessment system uses fuzzy logic and three input variables: historical reliability, current residual error, and environmental adaptability. Each variable is divided into five fuzzy levels and comprehensively evaluated using 25 fuzzy rules defined by an expert knowledge base. The system maintains independent health indicators for each sensor type, updated every second. The assessment process considers redundancy between sensors. When multiple sensors of the same type experience a simultaneous decline in health, a systemic fault warning is triggered. The health indicators are used not only for weighting but also as a basis for system maintenance decisions.

[0053] The objective function maximizes the overall confidence of the fusion results. Constraints include ensuring a minimum weight for each sensor and limiting the rate of weight change. The solution utilizes an online QP algorithm with a configurable calculation period. The system has pre-configured special handling strategies for typical fault scenarios: the acoustic ranging weight is increased to 70%-90% in the event of DVL failure; the IMU inference time is extended to 5 minutes in the event of acoustic signal interruption; and a voting mechanism is activated in the event of multi-sensor conflict. Weight adjustments are performed gradually to avoid sudden changes in the fusion results, with a transition time typically kept to 10-30 seconds.

[0054] The system is designed with six levels of gradually decreasing fault-tolerance modes to achieve the optimal balance between performance and reliability. Mode 0 represents normal operation of all sensors; Modes 1 and 2 correspond to single-type sensor failures, respectively; Modes 3 and 4 represent mixed navigation states with multiple sensor failures; and Mode 5 represents pure inertial emergency navigation. Overlap zones are set between each mode, and smooth switching is achieved through hysteresis comparison. In the worst-case scenario, Mode 5, the system activates the IMU error suppression algorithm: zero-speed corrections are used to suppress velocity error growth, vertical channel drift is constrained using the depth sensor, and attitude observations are regularly updated. Tests have shown that this mode can control position error to within 5% of the traveled distance within one hour, meeting emergency return requirements.

[0055] The system adopts a modular design, encompassing independent units such as anomaly detection, health assessment, weight calculation, and pattern management. Each unit communicates via a message bus, supporting distributed deployment. The system maintains a global state machine to centrally manage all fault-tolerant decisions and ensure logical consistency. An online configuration interface is provided for key parameters, enabling dynamic adjustments based on task requirements. Detailed logs are generated for all decision-making processes, including timestamps, decision rationale, and adjustment results, facilitating post-analysis and algorithm optimization. The system's average decision latency is less than 50ms, meeting real-time requirements.

[0056] In an embodiment of the present invention, the collaborative positioning decision-making hub module includes the following processing flow when evaluating cluster task requirements and positioning resource status: The system acquires key status parameters of each underwater robot in real time through a distributed data acquisition network. Positioning accuracy is derived from the Kalman filter covariance matrix of each node, including uncertainty estimates for position and attitude. Communication link quality is comprehensively assessed using bit error rate, signal-to-noise ratio, and packet arrival rate. Energy status is monitored by monitoring parameters such as remaining battery charge, instantaneous power consumption, and temperature. All data collection processes utilize a timestamp alignment mechanism to ensure temporal consistency, and a sliding average filter eliminates the effects of instantaneous fluctuations. The system also implements validation rules for each parameter, automatically removing anomalous data and marking it with a data quality indicator.

[0057] The system constructs a multidimensional performance evaluation matrix consisting of 10 core indicators, covering three categories: positioning accuracy, communication quality, and energy status. For precision tasks, positioning accuracy is weighted 70%, while for rapid reconnaissance tasks, communication quality (50%) and energy status (30%) are prioritized. The evaluation algorithm utilizes a modified TOPSIS method, determining objective weights through entropy weighting and integrating them with subjective weights based on task requirements. During the calculation process, each indicator is standardized to eliminate dimensionality effects, and a reward and penalty factor is introduced to address extreme cases. Each node's comprehensive performance score is updated every minute, reflecting its ability to execute tasks in real time.

[0058] Based on the current system state and historical operating data, a high-fidelity digital twin model is constructed for predictive simulation. The model includes three submodules: fluid dynamics simulation, communication channel modeling, and energy consumption prediction. It predicts resource demand changes over the next five minutes with a 30-second step size. Specifically for group collaborative tasks, the model simulates the impact of relative motion between robots on communication links and predicts potential network partitioning areas. The simulation process accounts for environmental uncertainty, employing Monte Carlo methods to generate multiple possible scenarios and calculate the performance risk index of each node. The prediction results are integrated with real-time evaluation data to generate trend curves with confidence intervals.

[0059] Evaluation results are intuitively displayed via a three-dimensional resource heat map, with red areas indicating performance bottlenecks and green areas representing high-quality resources. The system automatically identifies key bottleneck areas, such as communication blind spots or areas with a sudden drop in positioning accuracy, and annotates possible causes. The visual interface supports multi-dimensional filtering and drill-down analysis, allowing operators to view detailed evaluation data for any node. All display elements utilize adaptive rendering technology, dynamically adjusting refresh rate and level of detail based on device performance to ensure smooth operation.

[0060] The evaluation cycle can be dynamically adjusted based on task urgency, with multiple options available, ranging from 1 to 60 seconds. The system features a built-in self-learning mechanism that automatically optimizes evaluation weights and threshold parameters by analyzing historical decision-making results. A distributed evaluation plan is designed to address network disconnections, allowing nodes to autonomously calculate local performance evaluations. All evaluation results and decision recommendations are recorded in a blockchain log, ensuring data immutability and supporting post-audit and analysis. The system self-monitors using health indicators and automatically triggers a calibration process when evaluation deviations exceed thresholds.

[0061] In an embodiment of the present invention, the collaborative positioning decision-making hub module includes the following processing flow when performing intelligent scheduling optimization of the reference station working mode: The system uses a mixed-integer linear programming (MILP) approach to establish a base station scheduling model. Decision variables include each base station's wake-up state, operating mode, and sampling frequency. The objective function is formulated as a multi-objective optimization problem, minimizing both positioning error and total system energy consumption. An ε-constraint approach is used to generate a Pareto front. Constraints include coverage completeness, energy budget, and switching frequency limits. The solver employs a branch-and-bound algorithm, capable of finding a near-optimal solution that meets engineering requirements within 30 seconds for large-scale problems involving 200 base stations. The system maintains a library of scheduling solutions, enabling rapid switching between pre-set solutions based on environmental changes.

[0062] To address the time-varying nature of underwater acoustic channels, a resource allocation framework based on deep reinforcement learning was designed. The state space includes 12-dimensional features, including the channel impulse response, noise spectral density, and queue state. The action space defines frequency band selection, modulation scheme, and transmit power. The reward function comprehensively considers throughput, energy efficiency, and interference levels. Training utilizes the DDPG algorithm, with convergence accelerated by a digital twin environment during the online learning phase. In actual deployment, the system makes resource allocation decisions every 5 seconds, dynamically adapting to multipath fading and sudden interference. For critical data, high-reliability frequency bands are automatically allocated and transmit power is increased by 3-6 dB.

[0063] When a sudden change in task performance is detected, a rapid reallocation mechanism based on an improved auction algorithm is activated. Resource allocation is modeled as a multi-round bidding process, with task nodes submitting resource requests based on their urgency and base stations bidding based on their performance-to-cost ratio. The algorithm incorporates a virtual currency mechanism to balance global fairness and supports combinatorial auctions to handle correlated demands. Key innovations include a dynamic bid price adjustment strategy, an early elimination mechanism for false winners, and a distributed settlement protocol. Tests have shown that this mechanism can complete resource reallocation for 100 nodes within 10 seconds, five times faster than traditional methods. A preemptive reservation protocol is also designed to ensure that high-priority tasks receive the resources they need immediately.

[0064] Reliable command transmission: The system uses a three-level broadcast protocol to ensure reliable delivery of dispatch commands. The first-level broadcast uses the lowest frequency band for full coverage; the second-level uses medium-frequency directional retransmission in unresponsive areas; and the third-level uses high-frequency point-to-point retransmission for key nodes. All key commands are transmitted using three redundant channels, and the receiving end decodes them through majority voting. The protocol incorporates an adaptive backoff mechanism that dynamically adjusts the broadcast interval based on network load. The data packet structure includes forward error correction and CRC-32 checksums, resulting in a measured packet loss rate of less than 0.1%. The system monitors command execution status in real time, and unconfirmed nodes trigger the automatic execution of locally cached commands.

[0065] The system builds an environmental change perception network, using 10 monitoring indicators to assess the degree of environmental disturbance in real time. When any indicator exceeds an adaptive threshold, a recalculation of the scheduling plan is automatically triggered. The optimization process employs an incremental update strategy: the original plan's infrastructure is retained, and only the affected components are adjusted, reducing calculation time by 60%. The system maintains a two-level response mechanism: routine changes are subject to local adjustments (response delay <1 second); major changes (≥3 indicators) trigger a global reconfiguration (delay <2 seconds). All adjustments ensure service continuity, and state snapshots and rollback mechanisms ensure the system remains consistent. A human intervention interface is also designed to allow operators to override automated decisions.

[0066] It should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[0067] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A high-precision positioning system for collaborative operation of underwater robot clusters, characterized by: The system includes a multi-source fusion positioning reference station network module, a cross-media collaborative positioning engine module, a cluster relative positioning subsystem module, a dynamic environment perception and compensation module, a flexible positioning fault-tolerant system module, and a collaborative positioning decision-making center module, wherein: The multi-source fusion positioning reference station network module is used to rely on the seabed acoustic beacon array and the mobile inertial reference station to implement GNSS differential data and underwater acoustic delay fusion on the spatial reference frame, and output a unified absolute positioning reference field in time and space; The cross-media collaborative positioning engine module is used to perform a joint adjustment calculation on the carrier motion state and acoustic ranging data in combination with the original observation data of the multimodal sensor to generate a continuous pose transformation matrix in the earth coordinate system; The cluster relative positioning subsystem module is used to collect carrier phase differential signals and visual feature point data to perform three-dimensional calculations on the spatial relationship between robots and establish a dynamic topological relationship map; The dynamic environment perception and compensation module is used to monitor the water body sound velocity gradient and flow velocity field distribution to implement Doppler effect compensation on the acoustic propagation path and generate an environmental parameter correction database; The elastic positioning fault-tolerant system module is used to detect sensor data residuals and consistency indicators, perform dynamic weight allocation on fault modes, and output reliable positioning results under degraded conditions; The collaborative positioning decision-making central module is used to evaluate cluster task requirements and positioning resource status, implement intelligent scheduling optimization for reference station working modes, and form a global positioning resource configuration plan.

2. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: The multi-source fusion positioning reference station network module, when performing GNSS differential data and underwater acoustic time delay fusion on the spatial reference frame based on the seabed acoustic beacon array and the mobile inertial reference station, includes: A spatial reference network is constructed through an array of seabed acoustic beacons, beacon node positions are arranged, and base station coordinate data is generated; The carrier motion parameters are obtained with the help of the mobile inertial reference station, the dynamic calibration coefficients are calculated, and the reference station attitude information is output; By integrating GNSS differential data with underwater acoustic propagation delay, we can unify the time and space references and establish an absolute positioning reference field.

3. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: The cross-medium collaborative positioning engine module performs a joint adjustment calculation on the carrier motion state and acoustic ranging data by combining the original observation data of the multimodal sensor, including: Obtain absolute positioning reference field, collect multi-source sensor observation data, complete time synchronization processing, and generate synchronous measurement Dataset; Process the carrier motion information in the synchronous measurement data, implement joint adjustment solution, and construct the posture transformation relationship; Analyze the pose conversion error characteristics, optimize the filtering algorithm parameters, and output the accurate pose transformation matrix.

4. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: When the cross-media collaborative positioning engine module performs motion state estimation based on the adaptive filtering algorithm, it includes: Extract state variables from the precise pose transformation matrix, establish an error propagation model, and determine the sensor error characteristics; Configure filter parameters according to error characteristics, design an adaptive weight distribution scheme, and form an optimal estimation strategy; Apply the optimal estimation strategy to fuse multi-source data and update the pose state information.

5. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: The cluster relative positioning subsystem module, when executing the acquisition of carrier phase differential signals and visual feature point data to perform three-dimensional solution of the spatial relationship between robots, includes: Receive the precise pose transformation matrix, solve the carrier phase observation value, eliminate the integer ambiguity, and obtain the relative distance measurement result; Collect visual feature point information, achieve three-dimensional feature matching, calculate relative azimuth angles, and generate relative posture data; Integrate relative distance and posture measurements, establish a cluster topology relationship model, and output a dynamic spatial relationship map.

6. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: The dynamic environment perception compensation module, when performing Doppler effect compensation on the acoustic propagation path by monitoring the water body sound velocity gradient and flow velocity field distribution, includes: Based on the dynamic spatial relationship map, an environmental monitoring network is deployed to measure the changes in sound velocity profile and obtain real-time sound velocity data; Analyze the distribution of flow velocity field, model the acoustic signal propagation path, and calculate the propagation delay correction; Apply the delay correction to adjust the ranging data and update the environmental compensation database.

7. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: When the elastic positioning fault-tolerant system module performs dynamic weight allocation on the fault mode by detecting the residual error of sensor data and the consistency index, it includes: Call the environmental compensation database, verify sensor data quality, evaluate system consistency, and diagnose fault types; According to the fault diagnosis results, the sensor weights are dynamically configured and the data fusion process is reconstructed; Perform fault-tolerant positioning solutions to ensure system reliability and output robust positioning results.

8. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: The collaborative positioning decision-making hub module, when evaluating cluster task requirements and positioning resource status, includes: Obtain robust positioning results, analyze task requirements, build performance evaluation models, and complete system status analysis; Monitor the positioning performance of each node, evaluate the resource allocation effect, and generate optimization decision reports.

9. The high-precision positioning system for underwater robot cluster collaborative operation according to claim 1, characterized in that: When the collaborative positioning decision-making hub module performs intelligent scheduling optimization on the reference station working mode, it includes: Based on the optimization decision report, plan the base station working mode and formulate the wake-up scheduling strategy; Allocate communication resource parameters, optimize network topology, and generate global configuration instructions; Issue configuration instructions to coordinate various modules to achieve optimal system operation.

Images