A spatiotemporal registration and fusion method and system for multi-source sensors in underwater robots
By employing a multi-step approach involving time registration, spatial registration, and underwater error compensation, the problem of spatiotemporal consistency of multi-sensor data from underwater robots was solved, enabling real-time high-precision data fusion and mapping in underwater environments.
Patent Information
- Application Number
- CN202611349412.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-02
- Publication Date
- 2026-11-17
- Estimated Expiration
- 2046-09-02
AI Technical Summary
The fragmented processing of temporal and spatial registration of multiple sensors in underwater robots, the fragmentation of underwater-specific error compensation, the inability of calibration parameters to dynamically respond to environmental changes, and the inability to deploy non-real-time algorithms on airborne systems make it difficult to guarantee the spatiotemporal consistency of underwater multi-sensor data, which seriously restricts the accuracy of subsequent fusion perception and mapping.
The time registration step uses the master control clock initialized with time synchronization as the global reference clock, records the arrival timestamps of data frames from each sensor, and establishes a complete delay model for transmission delay, hardware processing delay, and acoustic propagation delay to compensate for time delay. The spatial registration step establishes a three-level mapping model of the world coordinate system, the robot body coordinate system, and the local coordinate systems of each sensor, and uses the inertial navigation system to perform dynamic spatial offset correction. The underwater error compensation step calculates the sound velocity profile based on real-time temperature, salinity, and depth data and performs ray tracking correction, and performs water flow disturbance pose correction based on the water flow velocity measured by the Doppler velocimeter. The global joint filtering step constructs factor graph nodes and constraint edges to output the estimated pose and extrinsic parameters of each sensor online.
It integrates time synchronization, spatial unification and environmental error compensation, dynamically responds to changes in the underwater environment, meets the real-time processing requirements of airborne systems, and improves the spatiotemporal consistency and fusion perception accuracy of underwater multi-sensor data.
Smart Images

Figure CN122858439B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of multi-sensor fusion perception technology for underwater robots, and in particular to a spatiotemporal registration and fusion method and system for multi-source sensors in underwater robots. Background Technology
[0002] Underwater robots (AUVs / ROVs) are typically equipped with a variety of heterogeneous sensors, such as inertial navigation systems (INS), Doppler velocimeters (DVL), ultra-short baseline positioning systems (USBL), sonar, lidar, and optical cameras, to perform tasks such as underwater environmental perception, terrain mapping, and 3D reconstruction.
[0003] The sampling frequencies of different sensors vary significantly (ranging from 0.1Hz to 200Hz), their trigger times are uncorrelated, and there is a lack of a unified time reference. Furthermore, each sensor is defined in its own independent local coordinate system, resulting in a inconsistent spatial reference. Therefore, precise alignment of multi-source data in both the temporal and spatial dimensions is a prerequisite for achieving multi-source fusion sensing and high-precision 3D reconstruction. However, the underwater environment has unique physical characteristics: sound waves travel at only about one-quarter the speed in water compared to air and are significantly affected by temperature, salinity, and pressure; optical signals are severely attenuated due to absorption and scattering by water; and the carrier's attitude is continuously disturbed by water currents. These factors make the spatiotemporal registration of multi-sensor underwater far more difficult than in terrestrial or aerial scenarios.
[0004] In existing technologies, hardware triggering schemes trigger simultaneous sampling of each sensor by synchronizing pulses from an external clock source. However, underwater GPS signals are unavailable, and the delay in underwater acoustic propagation causes inconsistent arrival times of the synchronization pulses, making implementation difficult. External parameter calibration methods based on calibration targets require dedicated calibration sites and fixed calibration parameters, which cannot adapt to dynamic underwater environments. Hand-eye calibration methods rely on sufficient excitation motion, but convergence is slow in low-speed underwater navigation scenarios. Sound velocity empirical formula correction methods only perform single-point sound velocity correction and do not cover underwater-specific errors such as optical refraction compensation and water flow disturbance correction.
[0005] In summary, existing technologies generally suffer from common problems such as fragmented processing of time and space registration, fragmented underwater-specific error compensation, inability of calibration parameters to dynamically respond to environmental changes, and non-real-time algorithms that cannot be deployed on airborne. These problems make it difficult to guarantee the spatiotemporal consistency of underwater multi-sensor data, which severely restricts the accuracy of subsequent fusion sensing and mapping, and makes it impossible to accurately supervise the position and attitude of each sensor and manage the data precisely. Summary of the Invention
[0006] The present invention was made in view of the above-mentioned problems, and its purpose is to provide a spatiotemporal registration and fusion method and system for multi-source sensors of underwater robots.
[0007] The objective of this invention is achieved through the following technical solution: This invention provides a spatiotemporal registration and fusion method for multi-source sensors in underwater robots, comprising the following steps: Time registration steps: Using the master control clock initialized with time synchronization as the global reference clock, record the arrival timestamps of each sensor data frame, establish a complete delay model for transmission delay, hardware processing delay and acoustic propagation delay to perform time delay compensation, and map each sensor data to a unified reference time series through interpolation; Spatial registration steps: Establish a three-level mapping model of the world coordinate system, the robot body coordinate system, and the local coordinate systems of each sensor; obtain external parameters through joint calibration of sonar-laser-camera; and use the inertial navigation system to perform dynamic spatial offset correction in real time attitude. Underwater error compensation steps: Calculate the sound velocity profile based on real-time temperature, salinity, and depth data and perform acoustic ranging correction by sound ray tracking; correct optical imaging and laser ranging based on refractive geometry model; and correct water flow disturbance pose based on water flow velocity measured by Doppler velocimeter. Global joint filtering steps: Construct factor graph nodes and constraint edges, optimize the estimated pose and extrinsic parameters of each sensor online through incremental smoothing, and correct all sensor data to a unified spatiotemporal reference; All sensor data are fused into the same three-dimensional coordinate system.
[0008] Furthermore: in the time registration step: The arrival timestamp is recorded when the first byte of the data frame is received by each sensor driver layer and embedded in the data frame header; The unified reference time series adopts a soft synchronization strategy based on the reference time axis; The acoustic propagation delay in the complete delay model is τ_acoustic = 2d / c, where d is the round-trip distance of the sound wave and c is the real-time estimated sound speed; the compensated effective sampling time is t_eff = t_TOA - τ_total - τ_acoustic, where t_TOA is the arrival timestamp of the sensor data frame and τ_total is the output result of the sensor's own delay calibration model. The time delay compensation also includes: using the short-time integration of the inertial navigation system to calculate the pose increment within the delay window, and extrapolating the sensor measurement values for motion compensation.
[0009] Furthermore, the time registration step also includes dynamic correction of timing deviations in the underwater motion scene: Construct a mapping model between timing deviation and flow velocity Δt_correction=f(v_current, d_sensor), where v_current is the flow velocity vector and d_sensor is the sensor measurement distance. Adaptively adjust the timing compensation amount based on this model. The acoustic sensor time window size is adaptively adjusted based on real-time depth gauge data, with the time window expanded in deep water to ensure complete reception of acoustic echoes.
[0010] Furthermore: the method for joint calibration of sonar-laser-camera is as follows: Initial external parameter estimates are obtained from mechanical design drawings as coarse calibration values; The initial correspondence between sonar point cloud and laser point cloud is established by using the histogram features of point cloud normal direction. The relative position transformation between sonar sensor and laser sensor is refined by iterative nearest point algorithm, and then the extrinsic parameters of sonar sensor and laser sensor are calculated. By using perspective n-point localization to calculate the pose of the calibration board in the camera coordinate system, the 3D points of the laser point cloud corresponding to the calibration board area are projected onto the image plane. The laser sensor extrinsic parameters and camera extrinsic parameters are optimized with the goal of minimizing the reprojection error from the 3D point cloud to the 2D image. A factor graph is constructed using the relative transformations between sonar sensor extrinsic parameters, laser sensor extrinsic parameters, and camera extrinsic parameters as constraints. The Levenburg-Marquardt algorithm is then used to jointly optimize the sonar sensor extrinsic parameters, laser sensor extrinsic parameters, and camera extrinsic parameters.
[0011] Furthermore: the dynamic spatial offset correction includes: The attitude of the sensor measurement coordinates is corrected using the real-time attitude rotation matrix of the inertial navigation system. The sensor installation position offset caused by underwater hull deformation was corrected using a depth-deformation calibration curve.
[0012] Furthermore: In the underwater error compensation step, the method for calculating the sound velocity profile is as follows: Based on real-time collected temperature, salinity, and depth data, the sound velocity profile is calculated online using the Chen-Millero sound velocity formula. Sound ray tracking model: The water body is discretized into isothermal and isosal thin layers along the depth direction. Snell's law is applied to each layer interface to track the sound ray path layer by layer. The difference between the actual sound ray arc length and the straight distance is calculated. Sound ray bending correction is applied to the sonar ranging value and the ultra-short baseline positioning result. When the update frequency of temperature, salinity, and depth data is insufficient, the latest complete sound velocity profile is used as the benchmark, and the profile is extrapolated and updated by combining real-time depth and temperature data through an empirical orthogonal function.
[0013] Furthermore, the specific method for correcting optical imaging and laser ranging based on the refractive geometry model in the underwater error compensation step is as follows: Corrected optical imaging: Based on Snell's law, a three-layer refractive geometric model of water-glass-air is established, the equivalent focal length is adjusted, and the principal point coordinates are corrected to compensate for the optical center shift caused by refraction; Laser ranging: Compensates for the deviation of the ranging system caused by the difference in refractive index at the water-air interface, and compensates for the difference in refractive index caused by the dispersion effect in water for different wavelength lasers; It also includes the processing of water scattering effects, specifically: underwater image descattering preprocessing based on dark channel priors and polarization imaging.
[0014] Furthermore: In the underwater error compensation step, the method for correcting the pose due to water flow disturbance is as follows: The relative water flow velocity measured by the Doppler velocimeter in water-tracking mode and the ground velocity measured in bottom-tracking mode are used to calculate the water flow velocity vector. The water flow velocity vector is used as an external disturbance force input into the six-degree-of-freedom dynamic model of the underwater robot to estimate the additional acceleration and attitude disturbance caused by the water flow, which is denoted as the disturbance estimate. The disturbance estimate is subtracted from the original measurements of the inertial navigation system to obtain the disturbance-free carrier pose for sensor spatial registration.
[0015] Further: In the global joint filtering step: The factor graph nodes include: carrier pose at each time moment, external parameters of each sensor, and environmental parameter vectors, wherein the environmental parameter vectors include sound speed, refractive index, and water flow velocity. The constraint edges include: inertial navigation pre-integration constraint, Doppler velocimeter velocity constraint, ultra-short baseline positioning system position constraint, sonar sensor iterative nearest point registration constraint, laser sensor iterative nearest point registration constraint, and camera reprojection constraint. The incremental smoothing optimization adopts the iSAM2 incremental smoothing optimization engine, which incrementally updates the factor graph every time a new keyframe is received, and performs global loop closure optimization after loop closure detection is triggered.
[0016] This invention also provides a spatiotemporal registration and error compensation system for underwater robot sensor data, which performs the above-described spatiotemporal registration and fusion method for multi-source sensors of an underwater robot, including: The time registration module is used to record the arrival timestamps of each sensor data frame, using the master control clock initialized with time synchronization as the global reference clock, to establish a complete delay model for transmission delay, hardware processing delay and acoustic propagation delay for time delay compensation, and to map each sensor data to a unified reference time series through interpolation. The spatial registration module is used to establish a three-level mapping model of the world coordinate system, the robot body coordinate system, and the local coordinate systems of each sensor. It obtains external parameters through joint calibration of sonar-laser-camera and uses the inertial navigation system to perform dynamic spatial offset correction in real time. The underwater error compensation module is used to calculate the sound velocity profile based on real-time temperature, salinity and depth data and perform acoustic ray tracking correction for acoustic ranging, correct optical imaging and laser ranging based on refractive geometry model, and correct water flow disturbance pose based on water flow velocity measured by Doppler velocimeter. The global joint filtering module is used to construct a factor graph containing carrier pose nodes, sensor extrinsic parameter nodes and environmental parameter nodes. With inertial navigation pre-integration, multi-sensor measurements and environmental parameters as constraints, it outputs the estimated pose and extrinsic parameters of each sensor online through incremental smoothing optimization, and corrects all sensor data to a unified spatiotemporal reference. The fusion module is used to fuse data from various sensors into a single map.
[0017] In summary, the beneficial effects of the present invention are as follows: 1. To address the problem of fragmented processing of time and space registration, a four-in-one process of "time registration, spatial registration, underwater error compensation, and global joint filtering" is adopted to integrate time synchronization, spatial unification, and environmental error compensation into a unified framework, completing all registration tasks simultaneously in a single processing flow.
[0018] 2. To address the fragmentation of underwater-specific error compensation, an underwater environmental error compensation system is constructed through limited dynamic correction of sound velocity and ray tracking, optical refraction and scattering compensation, and water flow disturbance pose correction. The three major error sources are compensated collaboratively within the same framework, covering major underwater error sources such as sound velocity fluctuations, optical refraction, water scattering, and water flow disturbance.
[0019] 3. To address the issue of static calibration parameters that cannot respond to dynamic environments, the registration parameters are adaptively adjusted in real time according to dynamic factors such as carrier maneuvering, temperature, salinity and depth changes, and water flow fluctuations by limiting motion compensation extrapolation, flow velocity-time deviation mapping, depth adaptive window adjustment, and attitude dynamic correction.
[0020] 4. To address the issues of non-real-time processing and inability to be deployed on airborne, the iSAM2 incremental smoothing optimization engine enables real-time online processing on the airborne end. Each new keyframe received incrementally updates the factor graph, and the computational complexity is independent of the amount of historical data. It can be executed in parallel pipeline, meeting the real-time operation requirements of underwater robots with limited computing resources. Attached Figure Description
[0021] To more clearly illustrate the technical solutions in the embodiments of this drawing or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this drawing. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.
[0022] Figure 1This is a schematic diagram of the system flow of the present invention.
[0023] The purpose, features, and advantages of this accompanying drawing will be further explained in conjunction with the embodiments and with reference to the accompanying drawing. Detailed Implementation
[0024] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. All other embodiments obtained by those skilled in the art based on the embodiments provided by this invention without inventive effort are within the scope of protection of this invention.
[0025] Obviously, the accompanying drawings described below are merely some examples or embodiments of the present invention. Those skilled in the art can apply the present invention to other similar scenarios based on these drawings without any inventive effort. Furthermore, it is understood that although the efforts made in this development process may be complex and lengthy, for those skilled in the art related to the content disclosed in this invention, modifications to design, manufacturing, or production based on the technical content disclosed in this invention are merely conventional technical means and should not be construed as insufficient disclosure of the present invention.
[0026] Unless otherwise specified, all embodiments and optional embodiments of the present invention can be combined with each other to form new technical solutions.
[0027] Unless otherwise specified, all technical features and optional technical features of this invention can be combined to form new technical solutions.
[0028] Unless otherwise specified, all steps of the present invention may be performed sequentially or randomly, preferably sequentially. For example, the method includes steps (a) and (b), indicating that the method may include steps (a) and (b) performed sequentially, or it may include steps (b) and (a) performed sequentially. For example, the mention that the method may also include step (c) indicates that step (c) may be added to the method in any order. For example, the method may include steps (a), (b), and (c), or it may include steps (a), (c), and (b), or it may include steps (c), (a), and (b), etc.
[0029] Unless otherwise specified, the terms "comprising" and "including" as used in this invention can be open-ended or closed-ended. For example, "comprising" and "including" can mean that other components not listed may also be included, or that only the listed components may be included.
[0030] Unless otherwise specified, the term "or" is inclusive in this invention. For example, the phrase "A or B" means "A, B, or both A and B". More specifically, the condition "A or B" is satisfied by any of the following conditions: A is true (or exists) and B is false (or does not exist); A is false (or does not exist) and B is true (or exists); or both A and B are true (or exist).
[0031] Please see Figure 1 The flowchart of this invention: The purpose of this invention is to monitor and manage various sensors installed on an underwater robot in real time within the same three-dimensional coordinate system. This primarily involves monitoring the data transmitted by these sensors and assessing whether their installation orientation is affected by the complex underwater environment (water flow, depth, salinity, collisions with algae and other debris). The specific design is as follows: First, the system receives raw data streams from various sensors, which mainly include: INS (Inertial Navigation System), DVL (Doppler Velocimeter), USBL (Ultra-Short Baseline Positioning System), sonar sensor, laser sensor, camera, and CTD (Cell, Temperature, Depth). Secondly, the data from different sensors (sampling time, sensor position coordinates at this time, and data currently collected by the sensor) are corrected to avoid errors in subsequent fusion within the same three-dimensional coordinate system. When other tasks call the data, errors in the data (such as time acquisition, relative position errors of each sensor, and errors in the returned data) will result in poor performance. 1. Perform time registration for each sensor. Use the master control clock (the high-precision system clock of the robot's main control computer synchronized to each sensor) initialized with time synchronization as the global reference clock. Record the arrival timestamp of each sensor's data frame. Establish a complete delay model for transmission delay, hardware processing delay, and acoustic propagation delay to compensate for time delay. Then, map each sensor's data to a unified reference time series through interpolation. Specifically, when the first byte of the data frame is received by each sensor driver layer, the arrival time stamp is immediately recorded by calling the high-precision clock interface (with an accuracy better than 1μs), and the time stamp is embedded in the header field of the data frame. For the sampling time mapping of different frequency sensors, a soft synchronization strategy based on the reference time axis is adopted. A global reference time series {T_g(k) = T_0 + k·Δt} is defined, where T_0 is the starting time of the global reference time axis after the time synchronization initialization is completed, and Δt is the uniform sampling period (typically 10ms, i.e., 100Hz). For each sensor data, it is mapped to the nearest reference time point through nearest neighbor interpolation (discrete event data, such as USBL position, sonar Ping echo) or linear interpolation (continuous measurement data, such as INS attitude angle, DVL velocity). High-frequency sensors (INS 200Hz) need to be downsampled to the reference frequency, and low-frequency sensors (USBL 1Hz) are filled by maintaining interpolation. A multi-source data association verification mechanism is also introduced: for different sensor data frames with similar time (<2ms), the consistency of their carrier motion state is checked - the carrier motion (displacement, rotation) described by different sensors at the same time should be within the physical constraints. Data pairs that exceed the constraints are marked as time synchronization anomalies and trigger re-alignment. Establish a sensor delay calibration model: Total delay of each sensor τ_total = τ_trans + τ_hw + τ_proc; where τ_trans is the transmission delay (serial port baud rate / network delay), τ_hw is the sensor's internal hardware processing delay (refer to the sensor datasheet), and τ_proc is the software processing delay (measured by the driver layer). For acoustic sensors, additional compensation is given for the sound wave propagation time in water: τ_acoustic = 2d / c, where d is the round-trip distance of the sound wave and c is the real-time estimated speed of sound; the effective sampling time after compensation is t_eff = t_TOA - τ_total - τ_acoustic, where t_TOA is the arrival timestamp of the sensor data frame and τ_total is the output result of the sensor's own delay calibration model; In the motion state of the carrier (the robot carrying various sensors), the displacement of the carrier during the delay time is included in the time registration compensation: the pose increment [ΔR, Δt] within the delay window is calculated using INS short-time integration (trapezoidal integration method based on angular velocity and acceleration), and the sensor measurement values are extrapolated for motion compensation: P_comp = ΔR·P_raw + Δp; where P_raw is the original measurement position of the sensor in the carrier body coordinate system before compensation, P_comp is the measurement position mapped to the effective sampling time after compensation, ΔR is the increment of the carrier's rotation matrix within the delay window, and Δp is the increment of the carrier's translation vector within the delay window. For high-mobility scenarios (angular velocity > 30° / s), a higher-order Runge-Kutta integral can be used to maintain extrapolation accuracy. When the relative velocity of the carrier to the water flow is high, the timing deviation between the DVL water tracking mode and the underwater acoustic localization is amplified due to water movement. In this case, flow velocity and direction data need to be introduced as motion priors to construct a mapping model between the timing deviation and the flow velocity: Δt_correction = k_v · |v_current| · d_sensor / c, where v_current is the water flow velocity vector, |v_current| is the water flow velocity magnitude, d_sensor is the sensor measurement distance, c is the real-time estimated sound velocity, and k_v is the dimensionless proportionality coefficient. The dimensionless proportionality coefficient is obtained through a water tank calibration experiment: In a controllable water tank environment, using a known velocity field (generated by a water pump array or circulating water channel, with the velocity independently calibrated by an ADV reference flow meter), under different velocity levels and different acoustic propagation distances, the actual timing deviation of the acoustic sensor is recorded respectively. The absolute value of the measured timing deviation Δt_correction is then fitted with the absolute value of the ratio of different velocity levels and different acoustic propagation distances under the corresponding working conditions using least squares fitting, thus obtaining the calibration value of k_v. It also incorporates depth-assisted dynamic correction: based on real-time data from the depth gauge, it adaptively adjusts the size of the acoustic sensor's time window; in deep water (>500m), the sound propagation time increases significantly, and the time window is expanded accordingly to ensure complete reception of acoustic echoes and avoid truncation errors.
[0032] II. Perform spatial registration (aligning sonar sensor - laser sensor - camera coordinates - robot body coordinates - world coordinates), the specific method is as follows: A three-layer coordinate system mapping system is established: First layer: World coordinate system (W, geographic NED); Second layer: Robot body coordinate system (B, usually X for forward, Y for right, and Z for downward); Third layer: Local coordinate systems of each sensor (S_i); Transformation relationship: P_W = T_WB(pose)·T_BS_i(extrinsic)·P_S_i, where T_WB is the transformation from the robot / sensor body coordinate system to the world coordinate system based on the real-time pose of INS, and T_BS_i is the sensor extrinsic parameter transformation matrix; Design a process for joint external parameter calibration of underwater multi-sensor systems: Coarse calibration: Initial external parameter estimates of the installation position and attitude of each sensor are obtained based on the mechanical design CAD drawings and used as initial values for optimization; Fine calibration-sonar-laser registration: Sonar point clouds and laser point clouds are acquired simultaneously in underwater structured scenes (such as dock pile foundations and artificial reefs). A preliminary correspondence is established through the point cloud normal direction histogram feature (FPFH). Then, the relative transformation between T_BS_sonar (sonar sensor) and T_BS_lidar (laser sensor) is refined by ICP (Iterative Closest Point), and their respective extrinsic parameters are calculated. Precise calibration-laser-camera registration: A high-contrast calibration board (black and white checkerboard pattern, waterproof encapsulated, with acrylic + matte coating to avoid reflection) is placed underwater, while simultaneously acquiring laser point clouds and camera images; the pose of the calibration board in the camera coordinate system is calculated using PnP (Perspective-n-Point), and the 3D points corresponding to the calibration board area in the laser point cloud are projected onto the image plane. The transformation between T_BS_lidar and T_BS_camera is optimized with the goal of minimizing the 3D-2D reprojection error; the robot's perspective is changed multiple times (at least 12 different poses) to ensure the observability of the optimization problem.
[0033] Global joint optimization: Construct a factor map based on the relative transformation constraints between sonar sensor extrinsic parameters, laser sensor extrinsic parameters and camera extrinsic parameters. With the goal of minimizing global reprojection error and point cloud registration error, the sonar sensor extrinsic parameters, laser sensor extrinsic parameters and camera extrinsic parameters are jointly optimized by the LM (Levenberger-Marquardt algorithm) to ensure closed-loop consistency. Further dynamic spatial offset correction is performed on the attitude of the sonar sensor, laser sensor and camera. The attitude correction of the sensor measurement coordinates is performed using the real-time attitude rotation matrix of the inertial navigation system: P_corrected(t)=R_INS(t)·P_raw(t), where P_raw(t) is the original measurement coordinate of the sensor in its local coordinate system at time t, R_INS(t) is the carrier attitude rotation matrix output by the inertial navigation system at time t, and P_corrected(t) is the measurement coordinate mapped to the robot body coordinate system after attitude correction. It is also necessary to address the sensor installation position deviation caused by underwater hull deformation due to depth changes (although small, it cannot be ignored in high-precision registration), and correct the sensor installation position using a depth-deformation calibration curve; Specifically, the experiment to establish the depth-deformation calibration curve is a high-pressure chamber pressure-strain calibration experiment. The routine operation procedure is as follows: the underwater robot shell is placed in the high-pressure chamber, and external water pressure is applied step by step (simulating water pressure at depths of 0~6000 m, with an increment of 0.5 MPa at each level). The three-dimensional deformation of the mounting base under each pressure level is measured using strain gauges (or high-precision displacement sensors) attached near the sensor mounting base on the shell. At the same time, the actual offset of the sensor mounting position is independently measured using a laser displacement meter. A mapping table between external water pressure (corresponding to depth) and deformation is established, and the depth-deformation calibration curve is generated by polynomial fitting (typically a quadratic or cubic polynomial).
[0034] Third, introduce underwater-specific error compensation. Since the robot works underwater, its sound propagation speed, optical refraction, etc. are all affected by water environment parameters. The sound velocity dynamic correction mechanism is based on real-time temperature (T), salinity (S), and depth (P) data collected by a CTD sensor. The sound velocity profile c(T,S,P) = Cw(T,P) + A(T,P)·S + B(T,P)·S^(3 / 2) + D·S^2 is calculated online using the Chen-Millero sound velocity formula, where Cw is the pure water sound velocity term, and A, B, and D are coefficients related to temperature and pressure. The sound velocity profile is then input into the ray-tracing model: the water body is discretized into N isothermal and isosalinous thin layers along the depth direction, and Snell's law of refraction d(sinθ / c) / dz = ... is applied at the interfaces of each layer. 0. The sound ray path is traced layer by layer, and the difference between the actual sound ray arc length and the straight distance is calculated. The sound ray bending correction is performed on the sonar ranging value and USBL positioning result. When the CTD data update frequency is insufficient, a strategy of sound velocity profile interpolation based on depth layering and fusion with historical CTD profiles is adopted: the latest complete CTD profile is used as the benchmark, and the profile is extrapolated and updated by combining real-time depth and temperature data through empirical orthogonal function (EOF). Optical Refraction and Scattering Compensation: For underwater camera imaging, an imaging model based on refractive geometry is established for correction. This involves considering the refractive effect at the water-glass-air interface and correcting the optical path based on Snell's law. The equivalent focal length is adjusted to f'_x = f_x · (n_water / n_air), where f_x is the original focal length calibrated by the camera in air, n_water is the refractive index of water (typically around 1.33), n_air is the refractive index of air (around 1.0), and f'_x is the underwater equivalent focal length after refraction correction. The principal point coordinates are also corrected to compensate for the optical center shift caused by refraction. For the laser sensor, the ranging system bias caused by the difference in refractive index at the water-air interface is compensated: d_true = d_raw × (n_water / n_air), where d_raw is the raw ranging value output by the laser sensor (calculated based on the speed of light in air), n_water is the refractive index of the water (typically about 1.33), n_air is the refractive index of the air (about 1.0), and d_true is the true underwater ranging value after refractive index correction; it is also necessary to compensate for the slight difference in refractive index caused by the dispersion effect of different wavelength lasers (532nm green light / 450nm blue light) in water; for the water scattering effect, underwater image descattering preprocessing based on dark channel prior or polarization imaging is used to restore the true contrast and edge sharpness of the scene, providing high-quality input for subsequent visual feature extraction and registration; Water flow disturbance correction: The velocity of the carrier relative to the water flow is measured using the DVL water tracking mode, and the velocity relative to the ground is measured using the DVL bottom tracking mode to calculate the water flow velocity vector; A water flow-attitude disturbance transfer model is constructed: the water flow velocity vector is used as the external disturbance force input, and the additional acceleration and attitude disturbance caused by the water flow are estimated through a six-degree-of-freedom dynamic model of the robot (considering added mass, damping coefficient, and restoring force). The disturbance estimate is subtracted from the original INS measurements to obtain a more accurate undisturbed carrier pose for sensor spatial registration. Global joint filtering and fusion: The time-aligned multi-sensor data, the spatially unified coordinate system, and the correction amount after environmental error compensation are uniformly input into the factor graph optimization framework. The factor graph nodes include: the carrier pose at each time step (X_t ∈ SE(3)), the extrinsic parameters of each sensor (E_i), and the environmental parameter vectors (sound speed c, refractive index n, water flow velocity v_c). The edge constraints include: INS pre-integration constraints (relative motion constraints between consecutive pose nodes), DVL velocity constraints (difference between pose node velocity and DVL measurement), USBL position constraints (difference between pose node position and USBL measurement), sonar / laser point cloud registration constraints (relative transformation of point cloud ICP registration between adjacent pose nodes), and camera reprojection constraints (projection error of 3D map points in images from various viewpoints). Through iSAM2 incremental smoothing optimization, the factor graph is incrementally updated every time a new keyframe is received, and the estimated pose and sensor extrinsic parameters are output online to suppress the cumulative error of multi-source data. Loop closure detection is triggered by visual position recognition or sonar point cloud position recognition, and the loop closure constraints are added to the factor graph for global loop closure optimization.
[0035] Finally, all sensor data and the corrected data were entered into the same graph.
[0036] The algorithm flow of this invention is as follows: 1. Data Acquisition Layer: Reads raw data frames from each sensor in parallel and records the TOA timestamp; 2. Time registration: The following steps are performed sequentially: time synchronization initialization, transmission / hardware / acoustic delay compensation, motion compensation extrapolation, unified timestamp mapping, and dynamic correction of flow velocity and depth. 3. Spatial registration: sequentially load multi-level coordinate system parameters, perform CAD coarse calibration, sonar-laser / laser-camera fine calibration, factor map global extrinsic parameter optimization, and attitude dynamic correction; 4. Underwater Error Compensation: Based on CTD data, calculate the sound velocity profile, input the sound velocity profile into the ray tracking model to correct the sonar ranging value and USBL positioning result; correct the camera focal length and laser sensor ranging value using the refraction model; introduce the DVL water flow vector to the water flow-attitude disturbance transfer model to correct attitude disturbance; 5. Global Joint Filtering: Construct a factor graph, incrementally optimize the factor graph using iSAM2, output estimated pose and extrinsic parameters, and correct all sensor data to a unified spatiotemporal reference; 6. Output: All sensor data is converted to a unified spatiotemporal reference as a standard spatiotemporal aligned dataset. The standard spatiotemporal aligned dataset is then published to the downstream fusion / reconstruction module for fusion into the same 3D coordinate system and corresponding data display layers.
[0037] In summary, this invention addresses the problem of separating time registration from spatial registration by integrating time synchronization, spatial unification, and environmental error compensation into a unified framework through a four-in-one process: time registration, spatial registration, underwater error compensation, and global joint filtering. Specifically, the time registration module aligns the time axes of each sensor using a global reference clock and a complete delay model; the spatial registration module unifies the spatial reference through three-level coordinate system mapping and joint calibration of extrinsic parameters; and the global joint filtering step further optimizes all constraints using a factor graph architecture.
[0038] To address the issues of fragmented underwater error compensation and the limitation of only performing single sound velocity correction, a systematic underwater environmental error compensation system was constructed by limiting dynamic sound velocity correction and ray tracking, optical refraction and scattering compensation, and water flow disturbance pose correction. Specifically, sound velocity correction no longer relies on fixed empirical formulas, but instead calculates the sound velocity profile online based on real-time CTD data and corrects the acoustic ranging layer by layer by combining Snell ray tracking; optical compensation establishes a three-layer refraction geometric model of water-glass-air for the first time, and simultaneously performs refractive index correction and dispersion compensation for laser ranging; water flow disturbance correction uses the flow velocity vector measured by DVL to estimate and subtract the disturbance acceleration through a six-degree-of-freedom dynamic model.
[0039] To address the problem of fixed parameters in calibration methods that cannot respond to changes in the dynamic underwater environment, this invention constructs a complete dynamic adaptive compensation mechanism. It limits motion compensation extrapolation by using INS short-time integration to calculate the pose increment within the delay window in real time, ensuring that sensor measurements are accurately mapped to the sampling time and coordinates even under carrier maneuvering conditions. It limits flow velocity-time deviation mapping and depth adaptive window adjustment, allowing the time compensation amount to adjust in real time with changes in water flow and depth, automatically expanding the time window in deep water to avoid echo truncation. It limits attitude dynamic correction by using the INS real-time rotation matrix to correct sensor coordinates, and uses the depth-deformation calibration curve to compensate for hull deformation.
[0040] The above methods work together to enable this solution to adaptively adjust the registration parameters of each sensor in dynamic scenarios such as sudden changes in temperature, salinity, and depth, changes in water flow, and carrier maneuvering.
[0041] To address the engineering bottlenecks of non-real-time operation and inability to be deployed airborne, this invention introduces the iSAM2 incremental smooth optimization engine. Unlike traditional batch optimization methods that require offline calculation after accumulating all data, the iSAM2 engine incrementally updates factor graph nodes and constraint edges upon receiving a new keyframe. Its computational complexity is independent of the amount of historical data and can be executed in pipelined parallel, supporting online incremental processing on the limited computing resources of underwater robots.
[0042] It should be noted that the present invention is not limited to the above-described embodiments. The above embodiments are merely examples, and any embodiments that have the same structure and perform the same effects as the technical concept within the scope of the present invention are included within the scope of the present invention. Furthermore, various modifications that can be conceived by those skilled in the art to the embodiments, and other ways of constructing by combining some of the constituent elements of the embodiments, without departing from the spirit of the present invention, are also included within the scope of the present invention.
Claims
1. A method for spatio-temporal registration and fusion of multi-source sensors of an underwater robot, characterized in that, Includes the following steps: Time registration steps: Using the master control clock initialized with time synchronization as the global reference clock, record the arrival timestamps of each sensor data frame, establish a complete delay model for transmission delay, hardware processing delay and acoustic propagation delay to perform time delay compensation, and map each sensor data to a unified reference time series through interpolation; Spatial registration steps: Establish a three-level mapping model of the world coordinate system, the robot body coordinate system, and the local coordinate systems of each sensor; obtain external parameters through joint calibration of sonar-laser-camera; and use the inertial navigation system to perform dynamic spatial offset correction in real time attitude. Underwater error compensation steps: Calculate the sound velocity profile based on real-time temperature, salinity, and depth data and perform acoustic ranging correction by sound ray tracking; correct optical imaging and laser ranging based on refractive geometry model; and correct water flow disturbance pose based on water flow velocity measured by Doppler velocimeter. Global joint filtering steps: Construct factor graph nodes and constraint edges, optimize the estimated pose and extrinsic parameters of each sensor online through incremental smoothing, and correct all sensor data to a unified spatiotemporal reference; All sensor data are fused into the same three-dimensional coordinate system; The method for joint calibration of sonar-laser-camera is as follows: Initial external parameter estimates are obtained from mechanical design drawings as coarse calibration values; The initial correspondence between sonar point cloud and laser point cloud is established by using the histogram features of point cloud normal direction. The relative position transformation between sonar sensor and laser sensor is refined by iterative nearest point algorithm, and then the extrinsic parameters of sonar sensor and laser sensor are calculated. By using perspective n-point localization to calculate the pose of the calibration board in the camera coordinate system, the 3D points of the laser point cloud corresponding to the calibration board area are projected onto the image plane. The laser sensor extrinsic parameters and camera extrinsic parameters are optimized with the goal of minimizing the reprojection error from the 3D point cloud to the 2D image. A factor graph is constructed using the relative transformations between sonar sensor extrinsic parameters, laser sensor extrinsic parameters, and camera extrinsic parameters as constraints. The sonar sensor extrinsic parameters, laser sensor extrinsic parameters, and camera extrinsic parameters are jointly optimized using the Levenberg-Marquardt algorithm. In the global joint filtering step: The factor graph nodes include: carrier pose at each time moment, external parameters of each sensor, and environmental parameter vectors, wherein the environmental parameter vectors include sound speed, refractive index, and water flow velocity. The constraint edges include: inertial navigation pre-integration constraint, Doppler velocimeter velocity constraint, ultra-short baseline positioning system position constraint, sonar sensor iterative nearest point registration constraint, laser sensor iterative nearest point registration constraint, and camera reprojection constraint. The incremental smoothing optimization adopts the iSAM2 incremental smoothing optimization engine, which incrementally updates the factor graph every time a new keyframe is received, and performs global loop closure optimization after loop closure detection is triggered.
2. The method of claim 1, wherein, In the time registration step: The arrival timestamp is recorded when the first byte of the data frame is received by each sensor driver layer and embedded in the data frame header; The unified reference time series adopts a soft synchronization strategy based on the reference time axis; The acoustic propagation delay in the complete delay model is τ_acoustic = 2d / c, where d is the round-trip distance of the sound wave and c is the real-time estimated sound speed; the compensated effective sampling time is t_eff = t_TOA - τ_total - τ_acoustic, where t_TOA is the arrival timestamp of the sensor data frame and τ_total is the output result of the sensor's own delay calibration model. The time delay compensation also includes: using the short-time integration of the inertial navigation system to calculate the pose increment within the delay window, and extrapolating the sensor measurement values for motion compensation.
3. The method of claim 2, wherein, The time registration step also includes dynamic correction of timing deviations in underwater motion scenes: Construct a mapping model between timing deviation and flow velocity, and adaptively adjust the timing compensation amount based on the model; The acoustic sensor time window size is adaptively adjusted based on real-time depth gauge data, with the time window expanded in deep water to ensure complete reception of acoustic echoes.
4. The method of claim 1, wherein, The dynamic spatial offset correction includes: The attitude of the sensor measurement coordinates is corrected using the real-time attitude rotation matrix of the inertial navigation system. The sensor installation position offset caused by underwater hull deformation was corrected using a depth-deformation calibration curve.
5. The method of claim 1, wherein, In the underwater error compensation step, the method for calculating the sound velocity profile is as follows: Based on real-time collected temperature, salinity, and depth data, the sound velocity profile is calculated online using the Chen-Millero sound velocity formula. Sound ray tracking model: The water body is discretized into isothermal and isosal thin layers along the depth direction. Snell's law is applied to each layer interface to track the sound ray path layer by layer. The difference between the actual sound ray arc length and the straight distance is calculated. Sound ray bending correction is applied to the sonar ranging value and the ultra-short baseline positioning result. When the update frequency of temperature, salinity, and depth data is insufficient, the latest complete sound velocity profile is used as the benchmark, and the profile is extrapolated and updated by combining real-time depth and temperature data through an empirical orthogonal function.
6. The method of claim 1, wherein, In the underwater error compensation step, the specific method for correcting optical imaging and laser ranging based on the refractive geometry model is as follows: Corrected optical imaging: Based on Snell's law, a three-layer refractive geometric model of water-glass-air is established, the equivalent focal length is adjusted, and the principal point coordinates are corrected to compensate for the optical center shift caused by refraction; Laser ranging: Compensates for the deviation of the ranging system caused by the difference in refractive index at the water-air interface, and compensates for the difference in refractive index caused by the dispersion effect in water for different wavelength lasers; It also includes the processing of water scattering effects, specifically: underwater image descattering preprocessing based on dark channel priors and polarization imaging.
7. The method of claim 1, wherein, The underwater error compensation step uses the following method to correct the water flow disturbance pose: The relative water flow velocity measured by the Doppler velocimeter in water-tracking mode and the ground velocity measured in bottom-tracking mode are used to calculate the water flow velocity vector. The water flow velocity vector is used as an external disturbance force input into the six-degree-of-freedom dynamic model of the underwater robot to estimate the additional acceleration and attitude disturbance caused by the water flow, which is denoted as the disturbance estimate. The disturbance estimate is subtracted from the original measurements of the inertial navigation system to obtain the disturbance-free carrier pose for sensor spatial registration.
8. A spatio-temporal registration and fusion system of multi-source sensors of an underwater robot, executing the method of spatio-temporal registration and fusion of multi-source sensors of an underwater robot according to any one of claims 1 to 7, characterized in that, include: The time registration module is used to record the arrival timestamps of each sensor data frame, using the master control clock initialized with time synchronization as the global reference clock, to establish a complete delay model for transmission delay, hardware processing delay and acoustic propagation delay for time delay compensation, and to map each sensor data to a unified reference time series through interpolation. The spatial registration module is used to establish a three-level mapping model of the world coordinate system, the robot body coordinate system, and the local coordinate systems of each sensor. It obtains external parameters through joint calibration of sonar-laser-camera and uses the inertial navigation system to perform dynamic spatial offset correction in real time. The underwater error compensation module is used to calculate the sound velocity profile based on real-time temperature, salinity and depth data and perform acoustic ray tracking correction for acoustic ranging, correct optical imaging and laser ranging based on refractive geometry model, and correct water flow disturbance pose based on water flow velocity measured by Doppler velocimeter. The global joint filtering module is used to construct a factor graph containing carrier pose nodes, sensor extrinsic parameter nodes and environmental parameter nodes. With inertial navigation pre-integration, multi-sensor measurements and environmental parameters as constraints, it outputs the estimated pose and extrinsic parameters of each sensor online through incremental smoothing optimization, and corrects all sensor data to a unified spatiotemporal reference. The fusion module is used to fuse data from various sensors into a single map.
Citation Information
Patent Citations
Multi-temperature-salinity-depth sensor cooperative detection task planning method
CN119720815A
Situation awareness system for attacking unmanned surface vehicle in port environment and use method of situation awareness system
CN120779421A
Underwater robot navigation positioning method and system
CN120970636A
