SLAM-oriented multi-sensor space-time synchronization and online calibration method

Abstract

The invention discloses an SLAM-oriented multi-sensor space-time synchronization and online calibration method, and relates to the technical field of computer vision and robot perception. Comprising the steps that S100, initialization is carried out, and an initial pose and an external parameter transformation matrix initial value are obtained through coarse alignment; s200, data acquisition and preprocessing: acquiring IMU data, LiDAR data and Odom data in real time, and performing data filtering, distortion removal, time sequence correction and point cloud downsampling processing on the acquired data; and S300, performing time synchronization processing, estimating the time offset of the LiDAR and Odom data in real time, and performing unified correction to generate a unified timestamp. According to the method, an online time offset estimation and compensation mechanism with an IMU as a main clock is established, accurate synchronization of sensor data of different frequencies is achieved, and the problem of data alignment caused by sampling frequency difference and clock drift is effectively solved; and meanwhile, tight coupling fusion is realized by using an IMU pre-integration factor, a LiDAR residual factor and an Odom constraint factor, so that the space-time consistency of multi-sensor data is remarkably improved.

Description

Technical Field

[0001] This invention relates to the fields of computer vision and robot perception technology, and in particular to a multi-sensor spatiotemporal synchronization and online calibration method for SLAM. Background Technology

[0002] With the widespread application of autonomous driving, mobile robots, and intelligent equipment, Simultaneous Localization and Mapping (SLAM) technology, as a core support for achieving autonomous navigation, is evolving towards deep fusion of multiple sensors. Modern SLAM systems generally integrate various heterogeneous sensors such as lidar, cameras, inertial measurement units (IMUs), and wheeled odometry to improve positioning accuracy and environmental modeling capabilities through complementary sensing. Among these, the spatiotemporal consistency of multi-source data is a prerequisite for achieving high-precision fusion, requiring strict alignment of each sensor in the temporal dimension and precise correlation in the spatial dimension, thereby constructing a unified and reliable framework for motion and environmental state estimation.

[0003] However, existing SLAM systems still face significant challenges in multi-sensor collaboration. On the one hand, IMUs provide continuous dynamic information with high-frequency sampling at the 100Hz level, while LiDAR and Odom typically output observation data at low frequencies of only 10Hz. Coupled with communication delays and clock drift, simple alignment based on system timestamps is insufficient to meet microsecond-level synchronization requirements. On the other hand, sensors inevitably exhibit positional and orientation deviations during physical installation. If extrinsic parameter calibration relies on offline static calibration procedures, it cannot adapt to parameter drift caused by vibration, temperature changes, or mechanical deformation during operation, resulting in spatial misalignment between point clouds and motion trajectories. More critically, most current fusion architectures treat time synchronization and extrinsic parameter calibration as fixed pre-processes, lacking the ability to jointly optimize spatiotemporal parameters online during state estimation, leading to a significant decrease in system robustness under long-term operation or dynamic disturbances.

[0004] Therefore, there is an urgent need for a multi-sensor spatiotemporal synchronization and online calibration method for SLAM, which can synchronously estimate time offset and extrinsic parameter transformation within a unified optimization framework, achieve accurate alignment of high-frequency IMU and low-frequency observation data, and dynamically correct the spatiotemporal relationship between sensors through a tight coupling factor graph, thereby comprehensively improving the real-time performance, accuracy and long-term stability of the SLAM system. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a multi-sensor spatiotemporal synchronization and online calibration method for SLAM, aiming to comprehensively improve the real-time performance, accuracy, and long-term stability of synchronous positioning and mapping systems.

[0006] In a first aspect, the present invention provides a multi-sensor spatiotemporal synchronization and online calibration method for SLAM, comprising: S100, Initialization: Obtain the initial pose and initial values ​​of the extrinsic transformation matrix through coarse alignment; S200, data acquisition and preprocessing: real-time acquisition of IMU data, LiDAR data and Odom data, and data filtering, distortion correction and timing correction and point cloud downsampling processing of the acquired data; S300, Time synchronization processing, real-time estimation of the time offset between LiDAR and Odom data, unified correction, and generation of a unified timestamp; the time synchronization processing includes time offset estimation, online time compensation, and unified timestamp output; S400: Construct a factor graph and add constraint factors, add key factors to each key frame within the sliding window, and construct a joint optimization function by fusing IMU, LiDAR, and Odom data to generate the optimal pose estimation and extrinsic parameters; S500, parallel incremental optimization solution, uses incremental optimization and Schul elimination method to optimize the factor graph; S600, external parameter online update: when the external parameter residual exceeds the threshold or the sensor installation offset is detected, the external parameter optimization and update process is triggered, and the optimized new external parameters are executed. S700, fault tolerance and reset strategy: When LiDA or Odom data fails, the fault tolerance scheme is activated to ensure stable operation under abnormal sensor conditions. S800 Output Results: Outputs the optimized pose, extrinsic transformation matrix, and time offset for use in upper-level navigation and mapping.

[0007] Preferably, the data acquisition and preprocessing includes: During the data sampling phase, the IMU driver node is used as the master clock reference, and its time reference is used to synchronize the LiDAR and Odom driver nodes. During the data filtering stage, a first-order low-pass filter is used to filter out high-frequency interference introduced by circuit noise or mechanical vibration from the angular velocity and linear acceleration signals of the IMU data; for LiDAR point cloud data, a statistical outlier removal algorithm is used to remove outliers with an average distance greater than [missing information]. point; In the distortion correction and timing correction stage, distortion compensation is performed on the LiDAR point cloud based on IMU angular velocity and linear acceleration data; During the point cloud downsampling stage, all points falling within the same voxel are replaced with centroids, and the voxel size is dynamically adjusted according to the point cloud density. During the format standardization and data output stage, the preprocessed IMU data, LiDAR data, and Odom data are encapsulated into a standardized data structure.

[0008] Preferably, the time synchronization process includes: The time offset estimation is calculated using the time likelihood function based on the IMU pre-integration trajectory. For each frame of LiDAR data, the LiDAR point cloud is first transformed to the world coordinate system based on the currently estimated pose and extrinsic transformation matrix, and then registered with the local map constructed by the IMU pre-integration. The time likelihood function is constructed based on the residual vector generated by the registration, which is used to measure the overall relationship between the LiDAR and Odom residual vectors under a given time offset. The optimal time offset estimate is obtained by searching for the time offset that maximizes the time likelihood function. The online time compensation updates the time offset recursively through multiple frames of historical data; wherein, when a new frame of LiDAR data is processed, the system immediately performs time offset estimation and fuses the obtained time offset estimate with the time offset estimate of the previous moment through a first-order low-pass filter to generate the compensated time offset. A unified timestamp is generated based on the original timestamp and the compensated time offset.

[0009] Preferably, S400 includes: The key factors include the IMU pre-integration factor, LiDAR matching factor, Odom factor, extrinsic parameter factor, and time offset factor; the joint optimization function includes the IMU pre-integration residual, LiDAR residual, Odom residual, extrinsic parameter residual, and time offset term. The IMU pre-integration factor integrates high-frequency IMU data between adjacent keyframes into relative motion increments and constructs an IMU pre-integration residual connecting pose and velocity nodes. The IMU pre-integration residual includes rotation residual, velocity residual, and position residual. Specifically, for adjacent keyframes i and j within the sliding window, the IMU pre-integration residual from keyframe i to keyframe j is calculated numerically using the angular velocity and linear acceleration of the IMU between keyframe i and keyframe j. The specific calculation formula is shown below: ; ; ; In the formula, Represents rotational residuals, and These represent keyframes. and The state rotation matrix, Indicates from keyframe arrive The rotation increment is calculated solely from the angular velocity of the integrating inertial measurement unit. This represents the logarithmic mapping of the Lie group, used to convert the error of the rotation matrix into vector form. Represents the velocity residual. and These represent the keyframes respectively. and keyframes The velocity vector at time t, Represents gravitational acceleration. Keyframe To keyframe The time interval of time, Keyframe To keyframe The change in velocity at any given moment is calculated solely from the linear acceleration of the integrating inertial measurement unit. Indicates the positional residual. and These represent keyframes. and keyframes The position vector, Keyframe To keyframe The change in relative position at time t is obtained by double integration of the acceleration.

[0010] Preferably, S400 further includes: The LiDAR matching factor is expressed as a point-to-plane residual. The residual calculation formula for each point and local plane is as follows: ; In the formula, This represents the original coordinates directly measured by the lidar. This means transforming a point in the lidar coordinate system to the world coordinate system. This represents a reference point of the selected local plane. Represents a unit vector; The Odom factor uses displacement and rotation as observation constraints, and the residuals are represented by Lie algebras to show the pose transformation differences. The calculation formula is as follows: ; In the formula, and Describe the keyframes respectively and keyframes The position and orientation of the carrier of time. Indicates from keyframe To keyframe Relative pose change at any given moment; Furthermore, the extrinsic parameter factor is used to treat the extrinsic parameter transformation matrix as an optimization variable; the time offset factor is used to treat the time offset as an optimization variable.

[0011] Preferably, S500 includes: The incremental optimization updates the variables related to the newly added keyframes, while maintaining the constraints of the previous keyframes during the optimization process. The computational complexity is linearly related to the size of the sliding window. When a new keyframe is added to the sliding window, the optimizer only updates the pose variables of the new keyframe and the variables directly connected to it. The Schul elimination process eliminates pose variables to generate a Schul elimination matrix with respect to extrinsic parameters and time offset. After solving the Schul elimination matrix, the pose variables are solved by substituting back into the matrix.

[0012] Preferably, S600 includes: The extrinsic parameter residual calculation is based on the difference between the point coordinates in the IMU coordinate system and the point coordinates in the LiDA coordinate system; a set of well-distributed feature points are selected, and the coordinate differences between the IMU coordinate system and the LiDA coordinate system are calculated, and the sum of the squares of all differences is taken as the extrinsic parameter residual; The extrinsic parameter iterative optimization adopts the Levenberg-Marquardt algorithm, with a damping term added during the iteration process; when the extrinsic parameter residuals decrease significantly, the damping factor is reduced; when the extrinsic parameter residuals increase, the damping factor is increased. When the residual of the extrinsic parameter exceeds the threshold, it is determined that the extrinsic parameter installation has been offset, and a complete extrinsic parameter optimization process is initiated to generate new extrinsic parameters.

[0013] Preferably, S700 includes: When LiDA data fails, an IMU plus Odom fusion scheme is adopted. The IMU provides high-frequency attitude change information, and the Odom provides low-drift displacement information. When Odom data fails, IMU inertial navigation prediction is used, and the prediction period is consistent with the IMU sampling period. When the detected extrinsic parameter drift or cumulative error is too large, the relocalization process is triggered; the current LiDA point cloud is globally matched with the historical keyframe database, the current pose is reset to the pose of the successfully matched keyframe, and the sliding window is rolled back to after the successfully matched keyframe.

[0014] Secondly, the present invention also provides a multi-sensor spatiotemporal synchronization and online calibration system for SLAM, which applies the multi-sensor spatiotemporal synchronization and online calibration method for SLAM as described above. The system includes a data acquisition and preprocessing module, a time synchronization module, a factor graph construction module, an external parameter online update module, and a fault tolerance and reset module. The data acquisition and preprocessing module is used to acquire IMU data, LiDAR data and Odom data in real time, and to perform data filtering, distortion correction and timing correction and point cloud downsampling on the acquired data. The time synchronization module is used to estimate the time offset in real time and to uniformly correct the LiDAR and Odom data to generate a unified timestamp. The factor graph construction module is used to add key factors to each key frame within the sliding window and construct a joint optimization function by fusing IMU, LiDAR, and Odom data. The external parameter online update module is used to trigger the external parameter optimization update process when the external parameter residual exceeds the threshold or the sensor installation offset is detected. The fault tolerance and reset module is used to activate the fault tolerance scheme when LiDAR or Odom data fails.

[0015] Compared with the prior art, the present invention has the following advantages and beneficial effects: By establishing an online time offset estimation and compensation mechanism with the IMU as the master clock, accurate synchronization of data from sensors of different frequencies is achieved, effectively solving the data alignment problem caused by sampling frequency differences and clock drift. State estimation and online extrinsic parameter calibration are jointly performed within a sliding window or factor graph framework. Tightly coupled fusion is achieved using the IMU pre-integration factor, LiDAR residual factor, and Odom constraint factor, significantly improving the spatiotemporal consistency of multi-sensor data. The designed fault-tolerance and backoff mechanism ensures stable system operation even when a single sensor fails temporarily, enhancing the system's robustness in complex environments and long-term operational reliability. Parallel incremental optimization and sparse Shur elimination acceleration techniques are employed to significantly improve computational efficiency and meet real-time requirements. Attached Figure Description

[0016] Figure 1 This is an overall framework diagram of a multi-sensor spatiotemporal synchronization and online calibration method for SLAM.

[0017] Figure 2 This is a partial flowchart of data acquisition and preprocessing in this invention.

[0018] Figure 3 This is a comparison diagram of the coupling effect between IMU and LiDAR in this invention (the left side shows the effect when LiDAR is used alone, and the right side shows the effect after LiDAR and IMU are tightly coupled).

[0019] Figure 4 This is a diagram showing the effects of time calibration and spatial extrinsic parameter optimization in this invention (the left side shows the effect when using LiDAR alone, and the right side shows the effect after LiDAR and IMU are tightly coupled).

[0020] Figure 5 This is a diagram of the unmanned vehicle experimental system architecture in this invention. Detailed Implementation

[0021] The technical solution of the present invention will be clearly and completely described below with reference to the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention. It should be noted that relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations.

[0022] Example 1 Please see Figures 1-5 This invention provides a multi-sensor spatiotemporal synchronization and online calibration method for SLAM. It can synchronously estimate time offset and extrinsic parameter transformation within a unified optimization framework, achieving precise alignment of high-frequency IMU, low-frequency LiDAR, and Odom data. Furthermore, it dynamically corrects the spatiotemporal relationship of the sensors through a tightly coupled factor graph, thereby comprehensively improving the real-time performance and stability of spatiotemporal synchronization positioning. The method can be implemented on vehicles such as drones and mobile robots. The method includes: S100, Initialization: Obtain the initial pose and initial values ​​of the extrinsic transformation matrix through coarse alignment; Specifically, after the carrier is powered on, it first performs a sensor self-test process to confirm that the IMU, LiDAR, and Odom are all in normal working condition. The IMU collects angular velocity and linear acceleration at a high frequency to estimate changes in motion state; the LiDAR collects environmental point cloud data at a low frequency to provide high-precision spatial structure information; and the Odom obtains movement distance and angle changes through an encoder or wheel speed sensor to provide low-drift displacement information.

[0023] Subsequently, the system enters the initialization phase, acquiring the initial pose manually or automatically. In automatic initialization mode, the carrier aligns with the Z-axis of the world coordinate system using the gravity vector obtained during IMU stationary states, and scans the initial environmental feature point cloud using LiDAR. This initial point cloud is then coarsely matched with a pre-stored map using visual assistance to obtain the initial position and orientation. The initial value of the extrinsic parameter transformation matrix can be pre-obtained using offline calibration tools or coarsely estimated during the initialization phase through brief stationary observations. The sliding window length is set to 10-20 frames, striking a balance between ensuring sufficient historical information and computational burden. The initial time offset is set to 0 ms, assuming complete synchronization of all sensor timestamps. After initialization, the system enters the real-time data processing loop.

[0024] S200, Data Acquisition and Preprocessing: Real-time acquisition of IMU data, LiDAR data, and Odom data; and data filtering, distortion correction, timing correction, and point cloud downsampling processing of the acquired data; the data acquisition and preprocessing includes data sampling, data filtering, distortion correction and timing correction, point cloud downsampling, format standardization, and data output; including the following steps: S210. During the data sampling phase, the IMU driver node is used as the master clock reference, and its time reference is used to synchronize the LiDAR and Odom driver nodes. After each data acquisition, the IMU driver node synchronously broadcasts a high-precision timestamp to the time bus. This timestamp is generated based on the hardware clock counter inside the IMU. After data acquisition, the LiDAR and Odom driver nodes immediately obtain the high-precision timestamp broadcast by the IMU and bind the obtained high-precision timestamp with the acquired data to generate a timestamped data packet. This ensures that the time reference of LiDAR data and Odom data is unified with the IMU data, fundamentally avoiding synchronization errors caused by clock drift between different devices.

[0025] S220. In the data filtering stage, a first-order low-pass filter is used to filter out high-frequency interference introduced by circuit noise or mechanical vibration for the angular velocity and linear acceleration signals of the IMU data; for LiDAR point cloud data, a statistical outlier removal algorithm is used to remove outliers with an average distance greater than [missing information]. Remove invalid points caused by specular reflection, transparent objects, or atmospheric disturbances.

[0026] S230. In the distortion correction and timing correction stage, distortion correction compensation is performed on each frame of LiDAR point cloud based on IMU angular velocity and linear acceleration data, so that each point in the LiDAR point cloud frame represents the spatial position at a unified time. Specifically, acquiring a single frame of LiDAR point cloud typically takes 33ms (corresponding to a 30Hz frame rate). During this period, the carrier may experience continuous motion, leading to motion distortion in the LiDAR point cloud. To correct this motion distortion, inverse motion compensation is performed on each point in the LiDAR point cloud using the IMU angular velocity and linear acceleration data recorded during the acquisition of that frame. For example, for a point at time... For the scanned point p, the system integrates the IMU angular velocity and linear acceleration data to calculate the rotational and translational increments of the carrier within this tiny time interval. Then, point p is transformed to the end of the frame. In the coordinate system, that is In the formula, To compensate for distortion. It is the transpose of the rotation matrix. This is a translation vector; this process is used to "straighten" the entire point cloud so that it logically represents the carrier in... A snapshot of a moment.

[0027] S240. During the point cloud downsampling stage, the 3D space is divided into a cubic mesh (voxel) with a side length of 0.1m. All points falling within the same voxel are replaced with the centroids of all points within that voxel. This is done to significantly reduce the amount of data in subsequent processing while preserving the overall geometric structure of the point cloud. At the same time, the density of the processed point cloud is monitored. If the point cloud is too sparse in open areas, the voxel size is automatically reduced to 0.05m. If the point cloud is still too dense in complex areas, it is increased to 0.2m to maintain computational efficiency and mapping accuracy.

[0028] S250. In the format standardization and data output stage, the preprocessed IMU data, LiDAR data, and Odom data are encapsulated into a standardized data structure. The standardized data structure includes timestamp, position, velocity, acceleration, and attitude. The timestamp field uses the IMU master clock time, and the position, velocity, acceleration, and attitude fields are generated during the initialization stage. The standardized data frame is pushed to subsequent processing at a frequency of 30Hz to ensure the regularity and timing consistency of the data stream.

[0029] S300, Time synchronization processing, taking into account the low sampling frequency and large communication delay of LiDAR and Odom, estimates the time offset in real time, and performs unified correction on LiDAR and Odom data to generate a unified timestamp; the time synchronization processing includes time offset estimation, online time compensation and unified timestamp output.

[0030] Based on the foregoing, the time offset estimation calculates the time likelihood function using the IMU pre-integration trajectory. Specifically, for each frame of LiDAR and Odom data, the LiDAR point cloud is first transformed to the world coordinate system based on the currently estimated pose and extrinsic parameter transformation matrix, and then registered with the local map constructed by IMU pre-integration. The resulting residual vector reflects the inconsistency between the LiDAR observation and the IMU predicted trajectory. A time likelihood function is constructed based on this residual vector to measure the overall magnitude of the LiDAR and Odom residual vectors at a given time offset. This time likelihood function can be expressed as: In the formula, For the residual vector, Let be the time offset. By searching for the time offset that maximizes the time likelihood function, the current optimal time offset estimate can be obtained.

[0031] Based on the foregoing, the online time compensation recursively updates the time offset using multiple frames of historical data, with the update interval synchronized with the LiDAR frame rate. Specifically, after each new frame of LiDAR or Odom data is processed, the system immediately performs a time offset estimation. The new time offset estimate is then fused with the previous time offset estimate using a first-order low-pass filter to smooth the estimation result and avoid severe jitter. The fused compensated time offset is used to correct the timestamps of the LiDAR or Odom data.

[0032] Based on the foregoing, a unified timestamp is generated from the original timestamp and the compensated time offset. This unified timestamp is used to represent the actual moment when LiDAR or Odom data occurs in the physical world, rather than the reading of the device's local clock.

[0033] S400. Construct a factor graph and add constraint factors. Add key factors to each keyframe within the sliding window. Construct a joint optimization function by fusing IMU, LiDAR, and Odom data to generate the optimal pose estimation and extrinsic parameters. The key factors include IMU pre-integration factor, LiDAR matching factor, Odom factor, extrinsic parameter factor, and time offset factor. The joint optimization function includes IMU pre-integration residual, LiDAR residual, Odom residual, extrinsic parameter residual, and time offset term.

[0034] Specifically, the IMU pre-integration factor integrates high-frequency IMU data between adjacent keyframes into relative motion increments and constructs an IMU pre-integration residual connecting pose and velocity nodes to ensure trajectory smoothness. The IMU pre-integration residual includes rotation residual, velocity residual, and position residual. For adjacent keyframes i and j within the sliding window, the IMU pre-integration residual from keyframe i to keyframe j is calculated numerically using the angular velocity and linear acceleration of the IMU between keyframe i and keyframe j. The specific calculation formula is shown below: ; ; ; In the formula, Represents rotational residuals, and These represent keyframes. and The state rotation matrix, Indicates from keyframe arrive The rotation increment is calculated solely from the angular velocity of the integrating inertial measurement unit. This represents the logarithmic mapping of the Lie group, used to convert the error of the rotation matrix into vector form. Represents the velocity residual. and These represent the keyframes respectively. and keyframes The velocity vector at time t, Represents gravitational acceleration. Keyframe To keyframe The time interval of time, Keyframe To keyframe The change in velocity at any given moment is calculated solely from the linear acceleration of the integrating inertial measurement unit. Indicates the positional residual. and These represent keyframes. and keyframes The position vector, Keyframe To keyframe The change in relative position at time t is obtained by double integration of the acceleration.

[0035] Specifically, the LiDAR matching factor adopts a point-to-plane residual form. For each point and local plane, the residual calculation formula is as follows: ; In the formula, This represents the original coordinates directly measured by the lidar. This means transforming a point in the lidar coordinate system to the world coordinate system. This represents a reference point of the selected local plane. This represents a unit vector.

[0036] Specifically, the Odom factor uses displacement and rotation as observation constraints, and the residuals are represented by Lie algebras to show the pose transformation differences. The calculation formula is as follows: ; In the formula, and Describe the keyframes respectively and keyframes The position and orientation of the carrier of time. Indicates from keyframe To keyframe The relative pose change at any given moment.

[0037] In addition, the factor graph also includes extrinsic factors and time offset factors; the extrinsic factors are used to use the extrinsic transformation matrix as an optimization variable; and the time offset factors are used to use the time offset as an optimization variable.

[0038] Specifically, the joint optimization function is solved using nonlinear least squares through the Ceres Solver or Levenberg–Marquardt algorithm, outputting the optimal pose estimate and extrinsic parameters.

[0039] S500, parallel incremental optimization solution, uses incremental optimization and Schul elimination method to optimize the factor graph.

[0040] Specifically, the incremental optimization only updates variables related to newly added keyframes, maintaining the constraints of previous keyframes during the optimization process. The computational complexity is linearly related to the sliding window size. When a new keyframe is added to the sliding window, the optimizer only updates the pose variables of that new frame and its directly connected variables (such as extrinsic parameters and time offsets). The old keyframe variables within the window are fixed, and their corresponding Jacobian matrices and residual vectors are cached to avoid redundant calculations. This strategy reduces the computational cost of each optimization from O(N²) to O(N), avoiding the computational explosion problem caused by the increase in data volume in the SLAM backend optimization.

[0041] Specifically, the Schul elimination optimizes the sparse structure of the factor graph, with the elimination order dynamically determined based on variable types and constraints. Since the Hessian matrix of the factor graph exhibits a distinctly blocky sparse structure, pose variables are tightly coupled through IMU factors, while extrinsic parameters and time offset variables are connected to all pose variables. Therefore, pose variables can be eliminated to generate a Schul elimination matrix relating to extrinsic parameters and time offset. Solving this Schul elimination matrix is ​​then performed before back-substituting to solve for the pose variables. This process fully utilizes the sparsity of the matrix, significantly reducing computational load and memory consumption.

[0042] S600, external parameter online update: When the external parameter residual is detected to exceed the threshold or the sensor installation is offset, the external parameter optimization and update process is triggered, and the optimized new external parameters are executed.

[0043] The extrinsic parameter residual calculation is based on the difference between the point coordinates in the IMU coordinate system and the point coordinates in the LiDA coordinate system. First, a set of feature points that are well distributed in space are selected. These points are observed by both the IMU (through the kinematic model) and LiDA. For each feature point, the difference between its coordinates in the two coordinate systems is calculated, and the sum of the squares of all differences is used as the extrinsic parameter residual.

[0044] The extrinsic parameter iterative optimization employs the Levenberg-Marquardt algorithm. A damping term is added during the iteration process to ensure optimization stability. The optimizer calculates the update amount of the extrinsic parameter variables in each iteration and dynamically adjusts the damping factor based on the residual decrease. If the extrinsic parameter residuals decrease significantly, the damping factor is decreased to accelerate convergence; if the extrinsic parameter residuals increase, the damping factor is increased to ensure stability.

[0045] The extrinsic parameter update threshold is dynamically adjusted based on sensor accuracy and environmental conditions, with a typical value of 0.03m. When the extrinsic parameter residual exceeds the threshold, it is determined that the extrinsic parameter installation has deviated, and a complete extrinsic parameter optimization process is initiated to generate new extrinsic parameters.

[0046] S700 fault tolerance and reset strategy: When LiDA or Odom data fails, the fault tolerance scheme is activated to ensure stable operation in complex environments or under abnormal sensor conditions; including: When LiDA data fails, an IMU plus Odom fusion scheme can be used as a fallback. The IMU provides high-frequency attitude change information, while the Odom provides low-drift displacement information. For example, if three consecutive frames of LiDA data are missing or the registration residuals increase abnormally, LiDA is determined to be faulty. In this case, all LiDA factors are removed from the factor graph, and only the IMU and Odom factors are retained. The pose estimate is then output at a frequency of 100Hz to maintain basic navigation capabilities.

[0047] When Odom data fails, IMU inertial navigation prediction is used, and the prediction period is consistent with the IMU sampling period. For example, when Odom data has not been updated for a long time or the value exceeds a reasonable range, the odometry is determined to be faulty. At this time, the system relies entirely on the IMU for dead reckoning and maintains usable attitude and velocity information for a short period of time.

[0048] If the system detects a sharp increase in the uncertainty of pose estimation or a failure to match with the map, it triggers a relocalization process. At this time, the system performs a global match between the current LiDA point cloud and the historical keyframe database. Once a matching keyframe is found, the current pose is reset to the pose of that keyframe, the sliding window is rolled back to after that keyframe, and the erroneous trajectory segments are discarded.

[0049] Furthermore, when a failed sensor recovers, the system does not immediately incorporate its data into the fusion process. Instead, it first performs a brief calibration period. During this period, the system uses the new data to re-estimate the initial values ​​of the time offset and extrinsic parameters. Once these values ​​stabilize, the corresponding factors are gradually added to the factor map, completing a smooth transition.

[0050] S800 Output Results: Outputs the optimized pose, extrinsic parameter transformation matrix, and time offset for upper-level navigation and mapping, and meets the requirements of autonomous driving and precision operation robots.

[0051] Example 2 Please see Figures 1-5This embodiment provides a multi-sensor spatiotemporal synchronization and online calibration system for SLAM, which applies the multi-sensor spatiotemporal synchronization and online calibration method for SLAM as described above. The system is characterized by including a data acquisition and preprocessing module, a time synchronization module, a factor graph construction module, an external parameter online update module, and a fault tolerance and reset module. The data acquisition and preprocessing module is used to acquire IMU data, LiDAR data and Odom data in real time, and to perform data filtering, distortion correction and timing correction and point cloud downsampling on the acquired data. The time synchronization module is used to estimate the time offset in real time and to uniformly correct the LiDAR and Odom data to generate a unified timestamp. The factor graph construction module is used to add key factors to each key frame within the sliding window and construct a joint optimization function by fusing IMU, LiDAR, and Odom data. The external parameter online update module is used to trigger the external parameter optimization update process when the external parameter residual exceeds the threshold or the sensor installation offset is detected. The fault tolerance and reset module is used to activate the fault tolerance scheme when LiDAR or Odom data fails.

[0052] All content not described in detail in this specification is prior art known to those skilled in the art, and the model parameters of each electrical appliance are not specifically limited; conventional equipment can be used. Electrical control components not mentioned in this technical solution are not shown in the figures because they are prior art, and will not be described further here.

[0053] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-sensor spatiotemporal synchronization and online calibration method for SLAM, characterized in that, include: S100, Initialization: Obtain the initial pose and initial values ​​of the extrinsic transformation matrix through coarse alignment; S200, data acquisition and preprocessing: real-time acquisition of IMU data, LiDAR data and Odom data, and data filtering, distortion correction and timing correction and point cloud downsampling processing of the acquired data; S300, Time synchronization processing, real-time estimation of the time offset between LiDAR and Odom data, unified correction, and generation of a unified timestamp; the time synchronization processing includes time offset estimation, online time compensation, and unified timestamp output; S400: Construct a factor graph and add constraint factors, add key factors to each key frame within the sliding window, and construct a joint optimization function by fusing IMU, LiDAR, and Odom data to generate the optimal pose estimation and extrinsic parameters; S500, parallel incremental optimization solution, uses incremental optimization and Schul elimination method to optimize the factor graph; S600, external parameter online update: when the external parameter residual exceeds the threshold or the sensor installation offset is detected, the external parameter optimization and update process is triggered, and the optimized new external parameters are executed. S700, fault tolerance and reset strategy: When LiDA or Odom data fails, the fault tolerance scheme is activated to ensure stable operation under abnormal sensor conditions. S800 Output Results: Outputs the optimized pose, extrinsic transformation matrix, and time offset for use in upper-level navigation and mapping.

2. The multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 1, characterized in that: The data acquisition and preprocessing include: During the data sampling phase, the IMU driver node is used as the master clock reference, and its time reference is used to synchronize the LiDAR and Odom driver nodes. During the data filtering stage, a first-order low-pass filter is used to filter out high-frequency interference introduced by circuit noise or mechanical vibration from the angular velocity and linear acceleration signals of the IMU data; for LiDAR point cloud data, a statistical outlier removal algorithm is used to remove outliers with an average distance greater than [missing information]. point; In the distortion correction and timing correction stage, distortion compensation is performed on the LiDAR point cloud based on IMU angular velocity and linear acceleration data; During the point cloud downsampling stage, all points falling within the same voxel are replaced with centroids, and the voxel size is dynamically adjusted according to the point cloud density. During the format standardization and data output stage, the preprocessed IMU data, LiDAR data, and Odom data are encapsulated into a standardized data structure.

3. The multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 2, characterized in that: The time synchronization process includes: The time offset estimation is calculated using the time likelihood function based on the IMU pre-integration trajectory. For each frame of LiDAR data, the LiDAR point cloud is first transformed to the world coordinate system based on the currently estimated pose and extrinsic transformation matrix, and then registered with the local map constructed by the IMU pre-integration. The time likelihood function is constructed based on the residual vector generated by the registration, which is used to measure the overall relationship between the LiDAR and Odom residual vectors under a given time offset. The optimal time offset estimate is obtained by searching for the time offset that maximizes the time likelihood function. The online time compensation updates the time offset recursively through multiple frames of historical data; wherein, when a new frame of LiDAR data is processed, the system immediately performs time offset estimation and fuses the obtained time offset estimate with the time offset estimate of the previous moment through a first-order low-pass filter to generate the compensated time offset. A unified timestamp is generated based on the original timestamp and the compensated time offset.

4. The multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 3, characterized in that: The S400 includes: The key factors include the IMU pre-integration factor, LiDAR matching factor, Odom factor, extrinsic parameter factor, and time offset factor; the joint optimization function includes the IMU pre-integration residual, LiDAR residual, Odom residual, extrinsic parameter residual, and time offset term. The IMU pre-integration factor integrates high-frequency IMU data between adjacent keyframes into relative motion increments and constructs an IMU pre-integration residual connecting pose and velocity nodes. The IMU pre-integration residual includes rotation residual, velocity residual, and position residual. Specifically, for adjacent keyframes i and j within the sliding window, the IMU pre-integration residual from keyframe i to keyframe j is calculated numerically using the angular velocity and linear acceleration of the IMU between keyframe i and keyframe j. The specific calculation formula is shown below: ； ； ； In the formula, Represents rotational residuals. and These represent keyframes. and The state rotation matrix, Indicates from keyframe arrive The rotation increment is calculated solely from the angular velocity of the integrating inertial measurement unit. This represents the logarithmic mapping of the Lie group, used to convert the error of the rotation matrix into vector form. Represents the velocity residual. and These represent the keyframes respectively. and keyframes The velocity vector at time t, Represents gravitational acceleration. Keyframe To keyframe The time interval of time, Keyframe To keyframe The change in velocity at any given moment is calculated solely from the linear acceleration of the integrating inertial measurement unit. Indicates the positional residual. and These represent keyframes. and keyframes The position vector, Keyframe To keyframe The change in relative position at time t is obtained by double integration of the acceleration.

5. A multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 4, characterized in that: The S400 also includes: The LiDAR matching factor is expressed as a point-to-plane residual. The residual calculation formula for each point and local plane is as follows: ； In the formula, This represents the original coordinates directly measured by the lidar. This means transforming a point in the lidar coordinate system to the world coordinate system. This represents a reference point of the selected local plane. Represents a unit vector; The Odom factor uses displacement and rotation as observation constraints, and the residuals are represented by Lie algebras to show the pose transformation differences. The calculation formula is as follows: ； In the formula, and Describe the keyframes respectively and keyframes The position and orientation of the carrier of time. Indicates from keyframe To keyframe Relative pose change at any given moment; Furthermore, the extrinsic parameter factor is used to treat the extrinsic parameter transformation matrix as an optimization variable; the time offset factor is used to treat the time offset as an optimization variable.

6. The multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 5, characterized in that: The S500 includes: The incremental optimization updates the variables related to the newly added keyframes, while maintaining the constraints of the previous keyframes during the optimization process. The computational complexity is linearly related to the size of the sliding window. When a new keyframe is added to the sliding window, the optimizer only updates the pose variables of the new keyframe and the variables directly connected to it. The Schul elimination process eliminates pose variables to generate a Schul elimination matrix with respect to extrinsic parameters and time offset. After solving the Schul elimination matrix, the pose variables are solved by substituting back into the matrix.

7. A multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 6, characterized in that: The S600 includes: The extrinsic parameter residual calculation is based on the difference between the point coordinates in the IMU coordinate system and the point coordinates in the LiDA coordinate system; a set of well-distributed feature points are selected, and the coordinate differences between the IMU coordinate system and the LiDA coordinate system are calculated, and the sum of the squares of all differences is taken as the extrinsic parameter residual; The extrinsic parameter iterative optimization adopts the Levenberg-Marquardt algorithm, with a damping term added during the iteration process; when the extrinsic parameter residuals decrease significantly, the damping factor is reduced; when the extrinsic parameter residuals increase, the damping factor is increased. When the residual of the extrinsic parameter exceeds the threshold, it is determined that the extrinsic parameter installation has been offset, and a complete extrinsic parameter optimization process is initiated to generate new extrinsic parameters.

8. A multi-sensor spatiotemporal synchronization and online calibration method for SLAM according to claim 7, characterized in that: The S700 includes: When LiDA data fails, an IMU plus Odom fusion scheme is adopted. The IMU provides high-frequency attitude change information, and the Odom provides low-drift displacement information. When Odom data fails, IMU inertial navigation prediction is used, and the prediction period is consistent with the IMU sampling period. When the detected extrinsic parameter drift or cumulative error is too large, the relocalization process is triggered; the current LiDA point cloud is globally matched with the historical keyframe database, the current pose is reset to the pose of the successfully matched keyframe, and the sliding window is rolled back to after the successfully matched keyframe.

9. A multi-sensor spatiotemporal synchronization and online calibration system for SLAM, employing the multi-sensor spatiotemporal synchronization and online calibration method for SLAM as described in claims 1-8, characterized in that: The system includes a data acquisition and preprocessing module, a time synchronization module, a factor graph construction module, an online external parameter update module, and a fault tolerance and reset module. The data acquisition and preprocessing module is used to acquire IMU data, LiDAR data and Odom data in real time, and to perform data filtering, distortion correction and timing correction and point cloud downsampling on the acquired data. The time synchronization module is used to estimate the time offset in real time and to uniformly correct the LiDAR and Odom data to generate a unified timestamp. The factor graph construction module is used to add key factors to each key frame within the sliding window and construct a joint optimization function by fusing IMU, LiDAR, and Odom data. The external parameter online update module is used to trigger the external parameter optimization update process when the external parameter residual exceeds the threshold or the sensor installation offset is detected. The fault tolerance and reset module is used to activate the fault tolerance scheme when LiDAR or Odom data fails.

Images