A dynamic weight-based LIDAR-IMU online calibration method

Abstract

The application discloses a kind of LIDAR-IMU online calibration methods based on dynamic weight, belong to the field of automatic driving and robot perception, wherein method includes: obtaining LIDAR data and IMU data are time-synchronized, construct LIDAR-IMU dataset;Based on LIDAR-IMU dataset, continuous frame point cloud registration is carried out, and initial pose transformation estimation value is obtained;Based on IMU data, pre-integration calculation is carried out, and motion constraint is constructed;Based on initial pose transformation estimation value matching residual, point cloud registration residual value is calculated;Point cloud registration residual value is adjusted to weight, and dynamic weight mechanism is constructed;Joint optimization model is constructed to tightly coupled optimization LIDAR-IMU dataset, and online calibration result is generated.The present application solves the technical problems that the existing online calibration method will produce a large number of mismatching in complex dynamic environment, resulting in insufficient point cloud registration accuracy and inaccurate calibration result.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and robot perception, specifically to an online calibration method for LIDAR-IMU based on dynamic weights. Background Technology

[0002] With the rapid development of autonomous driving and mobile robot technologies, multi-sensor fusion of LiDAR and IMU has become a core technology for high-precision environmental perception. Accurate extrinsic parameter calibration between the two is a prerequisite for effective data fusion. Existing calibration technologies are mainly divided into two categories: offline calibration based on calibration boards and calibration without calibration boards based on point cloud registration. Offline calibration requires a specific laboratory environment and specialized calibration equipment, which cannot adapt to extrinsic parameter drift caused by vibration, collision, and temperature changes during vehicle operation. Existing calibration without calibration boards... The methods are generally based on the rigid assumption of static scenes, and use fixed-weight point cloud registration residuals to construct optimization error terms. Moreover, IMU data is only used to provide initial guesses for point cloud registration and is not deeply coupled with point cloud constraints. This leads to a large number of mismatches introduced by dynamic object points in complex dynamic environments with a large number of dynamic vehicles and pedestrians, such as congested intersections and mixed pedestrian and vehicle traffic in cities. This seriously interferes with the accuracy of point cloud registration. At the same time, in geometrically degraded scenarios such as tunnels and long straight corridors, insufficient point cloud constraints will cause the calibration results to diverge and the accuracy to drop sharply, which cannot meet the perception requirements for long-term stable operation of autonomous driving systems. Summary of the Invention

[0003] This application provides a dynamic weight-based online calibration method for LIDAR-IMU, which aims to solve the technical problem that existing online calibration methods generate a large number of mismatches in complex dynamic environments, resulting in insufficient point cloud registration accuracy and inaccurate calibration results. This improves the technical effect of multi-sensor fusion perception system in terms of localization and mapping stability in complex dynamic environments.

[0004] In view of the above problems, this application provides a method and system for online calibration of LIDAR-IMU based on dynamic weights, the method comprising:

[0005] The process involves: acquiring LiDAR data and IMU data for time synchronization to construct the current extrinsic parameter estimate of the LiDAR-IMU; performing continuous frame point cloud registration based on the current extrinsic parameter estimate of the LiDAR-IMU to obtain the initial pose transformation estimate; performing pre-integration calculation based on the IMU data to construct motion constraints; matching the residuals based on the initial pose transformation estimate to calculate the point cloud registration residual value; adjusting the weights of the point cloud registration residual value according to the motion constraints to construct a dynamic weighting mechanism; and constructing a joint optimization model that combines the dynamic weighting mechanism to perform tight-coupled optimization on the current extrinsic parameter estimate of the LiDAR-IMU to generate online calibration results.

[0006] One or more technical solutions provided in this application have at least the following technical effects or advantages:

[0007] By constructing a tightly coupled joint optimization framework that integrates point cloud registration and IMU pre-integration, and introducing a dynamic weighting mechanism driven by both frame-level consistency confidence and point-level outlier confidence, the system automatically suppresses the interference of dynamic objects and mismatched points on the optimization process. Combined with a sliding window edge-out strategy, the system ensures online computation efficiency and ultimately achieves high-precision online real-time calibration of LiDAR-IMU extrinsic parameters in complex dynamic environments. This improves the localization and mapping stability of the multi-sensor fusion sensing system in complex dynamic environments.

[0008] The above description is only an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of this application more obvious and understandable, the following are specific embodiments of this application. Attached Figure Description

[0009] Figure 1 This application provides a flowchart illustrating an online calibration method for LIDAR-IMU based on dynamic weights. Detailed Implementation

[0010] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0011] The overall concept of the technical solution provided in this application is as follows:

[0012] This application provides a dynamic weight-based online calibration method for LiDAR-IMU. It achieves initial pose by multi-sensor precise time synchronization preprocessing, continuous frame point cloud registration, and IMU pre-integration to construct motion constraints. It combines frame-level and point-level dual reset confidence to generate dynamic weights and adaptively adjust the residual ratio. Then, it completes tight-coupled joint optimization of point cloud, IMU, and extrinsic parameter regularization terms through a sliding window. This method automatically reduces the optimization weight of dynamic interference points, filters reliable static feature constraints, and compensates for point cloud degradation defects, ultimately improving calibration accuracy and operational robustness in dynamic scenes.

[0013] After introducing the basic principles of this application, various non-limiting embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0014] Example 1, as Figure 1As shown in the figure, this application provides an online calibration method for LIDAR-IMU based on dynamic weights, the method comprising:

[0015] S100: Acquire LIDAR data and IMU data for time synchronization, and construct the current extrinsic parameter estimate of LIDAR-IMU.

[0016] Specifically, a hardware synchronization architecture centered on a high-precision GPS timing module is first constructed. The PPS pulse, or second pulse signal, output by this module has a rising edge accuracy of up to 1 microsecond, serving as a universal global reference for industrial-grade multi-sensor time synchronization. It can physically eliminate the accumulated clock drift error caused by the independent operation of crystal oscillators within different sensors. Simultaneously, a LiDAR (a 3D sensing sensor that generates discrete point clouds by rotating and emitting multiple laser beams to measure the 3D coordinates of the environment) and an inertial measurement unit (IMU) are connected via coaxial cables. The IMU integrates a three-axis MEMS gyroscope and a three-axis accelerometer via external trigger pins, forcing the hardware clocks of both to maintain phase-locked synchronization with the GPS clock. At the same time, NMEA-0183 protocol data output from the GPS module is received via an RS232 serial port. This is a universal data transmission protocol for global navigation satellite systems, containing information such as UTC absolute time, latitude and longitude, and satellite status. The parsed absolute time is precisely bound to the second-level rising edge of the PPS pulse, assigning a unified global absolute timestamp to all sensor data.

[0017] Based on this, raw point cloud data packets output by the LIDAR are acquired via a gigabit Ethernet interface, and raw angular velocity and acceleration measurements output by the IMU are acquired via a high-speed SPI serial interface. Both types of raw data are tagged with the corresponding hardware clock timestamp in real time during the acquisition thread. Since the LIDAR adopts a line-by-line rotation scanning mode, the thousands of laser points in a single frame of point cloud are not acquired at the same time, but are evenly distributed throughout the 100ms scanning cycle. Therefore, a point-by-point timestamp interpolation operation must be performed: based on the mechanical rotation angular velocity of the LIDAR, the start timestamp of the current frame, the scan line number corresponding to each laser point, and the horizontal scanning angle, the actual emission time of each laser point is accurately calculated, refining the originally uniform timestamp of the entire frame into an independent timestamp for each point.

[0018] The raw data was then preprocessed in a targeted manner: LIDAR data was filtered out for invalid points exceeding the sensor's effective detection range using a distance threshold, and specular reflection points and noise points with abnormal reflection intensity were filtered out using an intensity threshold. IMU data underwent a third-order Butterworth low-pass filter to remove measurement noise caused by high-frequency electromagnetic interference and mechanical vibration, while also eliminating abnormal jump values ​​in the IMU data. Finally, according to the order of global absolute timestamps, the synchronized LIDAR point cloud frames were associated one-to-one with the complete IMU measurement sequence within the corresponding time period to construct structured LIDAR-IMU current extrinsic parameter estimates. Each extrinsic parameter estimate entry includes: the three-dimensional coordinates, reflection intensity, and point-by-point timestamp of all points in a single LIDAR frame; the complete IMU measurement sequence from the start to the end of the point cloud acquisition for that frame, including the angular velocity, acceleration, and corresponding timestamp of each IMU data point; and a global time reference verification identifier.

[0019] S200: Perform continuous frame point cloud registration based on the current extrinsic parameter estimates of the LIDAR-IMU to obtain the initial pose transformation estimate.

[0020] Specifically, firstly, consecutive adjacent first and second frame point cloud data, as well as an IMU measurement sequence covering the complete acquisition cycle of the two frame point clouds, are extracted from the current extrinsic parameter estimates of the LIDAR-IMU. Then, IMU pre-integration calculation is performed on the IMU sequence, which is an efficient integration method for high-frequency IMU data. By incrementally integrating the IMU angular velocity and acceleration between two key frames, the relative pose, velocity, and offset changes are directly obtained without integrating the global state at each time step. The computational cost is only 1 / 100 of that of traditional integration. The relative motion prediction value between the two frames is obtained and used as the initial transformation guess for point cloud registration. This guess value can reduce the search range of point cloud registration from the global 6-DOF space to a local minima.

[0021] Subsequently, geometric features were extracted from the two point cloud frames. By calculating the k-neighborhood covariance matrix of each point and analyzing its eigenvalue distribution, edge feature points were selected. That is, the neighborhood covariance matrix has one eigenvalue that is significantly larger than the other two, which corresponds to the region in the point cloud where the normal direction changes drastically, such as corners, object edges, and planar feature points. That is, the neighborhood covariance matrix has two eigenvalues ​​that are significantly larger than the third, which corresponds to the flat region in the point cloud where the normal direction is consistent, such as the ground and walls.

[0022] Next, based on the initial transformation guess, all feature points of the second frame are projected onto the coordinate system of the point cloud of the first frame. KD-tree nearest neighbor search is used to find the nearest similar feature point in the first frame for each projected point to generate candidate matching point pairs. Then, the candidate matching point pairs are subjected to hierarchical confidence screening: the first layer calculates the spatial distance of each pair of matching points, marks points with a distance exceeding a preset deviation threshold as potential dynamic points and removes them directly. The second layer verifies the neighborhood distribution of the remaining matching points, calculates the directional consistency coefficient of the five nearest neighbor matching points around each matching point, and removes isolated mismatched points with a directional consistency coefficient lower than 0.7. Finally, a set of reliable matching point pairs is generated.

[0023] Then, the initial pose transformation estimate is solved using the iterative weighted least squares method based on the reliable matching point pairs. In each iteration, the registration residual of all matching points is calculated. Points with residuals greater than twice the median residual are assigned weights close to 0, and the pose is solved again. This process is repeated until the pose change is less than 1e-6 or the maximum number of iterations is reached, which is usually 20 times.

[0024] Finally, the residuals of the initial pose transformation estimates obtained from the solution are refined. The residuals of each reliable matching point pair are decomposed into normal residuals, i.e., deviations along the normal direction of the plane where the matching point is located, which are highly sensitive to the rotation and translation components of the pose transformation, and tangential residuals, i.e., deviations along the tangent direction of the plane where the matching point is located, which are only sensitive to the tangential translation components of the pose. The weighted composite residual value of each point is calculated according to the ratio of 0.8 for normal residuals and 0.2 for tangential residuals. Then, outliers in the residuals are removed by median filtering, and finally the point cloud registration residual value corresponding to each matching point is obtained.

[0025] S300: Perform pre-integration calculations based on the IMU data to construct motion constraints.

[0026] Specifically, firstly, the complete IMU measurement sequence corresponding to the current two adjacent LiDAR point clouds is accurately extracted from the current extrinsic parameter estimates of the LiDAR-IMU. The time boundary of this sequence is strictly aligned with the point-by-point acquisition time of the first laser point in the k-th frame and the point-by-point acquisition time of the last laser point in the (k+1)-th frame, rather than simply using the average timestamp of the entire frame, to ensure the consistency of motion constraints and the time of the point cloud scanning process. Then, an IMU measurement model is established to clarify that the raw angular velocity and acceleration output by the IMU are not the actual motion values ​​of the carrier, but rather measurement values ​​superimposed with various errors: the angular velocity measurement includes gyroscope bias, i.e., the systematic error that changes slowly with temperature and Gaussian white noise; the acceleration measurement includes accelerometer bias, Gaussian white noise, and specific force, i.e., the difference between the acceleration of the carrier relative to inertial space and the projection of gravitational acceleration in the IMU coordinate system, which is the direct physical measurement of the accelerometer, not the absolute acceleration. This model is the theoretical basis for the subsequent pre-integration calculation to separate systematic errors from the actual motion.

[0027] Based on this, IMU pre-integration is performed. Pre-integration directly calculates the relative motion change between two LiDAR frames using incremental integration, without needing to know the global pose and velocity. First, the IMU measurement sequence is divided into multiple small time steps according to its sampling frequency of 200-500Hz. Starting from the initial state of the IMU in the k-th frame, the rotation pre-integration is recursively calculated using quaternion multiplication, which describes the rotation increment of the IMU coordinate system between the two frames. The velocity pre-integration and position pre-integration are recursively calculated using acceleration integration. At the same time, the gravitational acceleration term is separated into the world coordinate system during the integration process, so that the pre-integration result is only related to the relative motion of the IMU and is independent of the global pose. When the global state is updated in subsequent joint optimization, only the pre-integration result needs to be corrected by the pre-integrated Jacobian matrix, without re-integrating all IMU data.

[0028] Next, noise covariance propagation is performed. Based on the idea of ​​error state Kalman filtering, the covariance matrix of the pre-integration error is recursively calculated: the measurement noise of the IMU is Gaussian white noise, and the zero bias is random walk noise, that is, non-stationary noise that drifts slowly over time. These noises will accumulate continuously during the integration process. The covariance matrix can quantify the uncertainty of each pre-integration quantity. The uncertainties of the three dimensions of rotation, velocity, and position are calculated and stored separately. At the same time, the pre-integration Jacobian matrix is ​​calculated to describe the derivative of the pre-integration result with respect to the gyroscope zero bias and the accelerometer zero bias. When the zero bias parameter is updated in the joint optimization, the pre-integration result can be quickly corrected through the Jacobian matrix to avoid repeated integration.

[0029] Subsequently, the pre-integration results are corrected for bias. Using the biased estimate obtained from the previous optimization, the current pre-integrated quantity is corrected by a first-order Taylor expansion using the Jacobian matrix to obtain a more accurate relative motion change. Finally, an IMU pre-integration residual vector is constructed, which consists of three parts: the rotation residual between the rotation increment obtained from pre-integration and the rotation increment obtained from LiDAR point cloud registration; the velocity residual between the relative velocity obtained from pre-integration and the relative velocity obtained from state estimation; and the position residual between the relative position obtained from pre-integration and the relative position obtained from LiDAR point cloud registration. The residual vector is combined with the corresponding covariance matrix to form a complete IMU motion constraint, which is then input into the joint optimization model.

[0030] S400: Based on the initial pose transformation estimate, match the residual and calculate the point cloud registration residual value.

[0031] Specifically, the 6-DOF initial pose transformation estimate obtained from the previous solution is first converted into a 4×4 homogeneous transformation matrix, which is a linear transformation mathematical tool containing a 3×3 rotation matrix and a 3×1 translation vector. This is used to map 3D points from the source point cloud coordinate system to the target point cloud coordinate system. It is a standard mathematical expression for describing the pose relationship of multiple sensors and can simultaneously realize rotation, translation and scale transformation. Here, the scale is fixed at 1. All reliable matching point pairs are traversed, that is, the set of matching points retained after initial screening by distance threshold and verification of neighborhood distribution consistency. The source feature points of the second frame are projected onto the target point cloud coordinate system of the first frame through the homogeneous transformation matrix to obtain the projection points in the same coordinate system as the target feature points. The 3D Euclidean distance between each projection point and the corresponding target feature point is calculated to generate the basic residual value corresponding to the matching point.

[0032] Next, local geometric feature analysis is performed on each target feature point. A local point cloud subset is constructed by selecting k=20 nearest neighbor points centered on the point. The covariance matrix of the subset is calculated and eigenvalue decomposition is performed. The geometric type of the point is determined based on the relative magnitude of the three eigenvalues. If two eigenvalues ​​are significantly greater than the third, it is a planar feature point. If one eigenvalue is significantly greater than the other two, it is an edge feature point. At the same time, the unit normal vector of the plane where the point is located or the unit direction vector of the edge where the point is located is solved. Based on this, the basic residual values ​​are decomposed directionally: the three-dimensional vector corresponding to the basic residual is projected onto the normal vector direction to obtain the normal residual component, which is the deviation along the normal direction of the feature geometry. For planar feature points, this component directly reflects the rotation and translation error of the pose and is highly sensitive to pose changes. For edge feature points, this component corresponds to the deviation perpendicular to the edge extension direction and also has high pose sensitivity. Projecting it onto the tangent plane or the edge extension direction yields the tangential residual component, which is the deviation along the tangent direction of the feature geometry. Due to the translational symmetry of the plane and the extension symmetry of the edge, this component has extremely low sensitivity to pose changes. Its large value usually originates from feature extraction noise and point cloud discretization error rather than the true pose error.

[0033] Then, differentiated weights are assigned based on the geometric type: for planar feature points, the normal residual component is assigned a weight of 0.85 and the tangential residual component a weight of 0.15; for edge feature points, the normal residual component perpendicular to the edge direction is assigned a weight of 0.9 and the tangential residual component along the edge direction a weight of 0.1. The two components are then weighted and summed according to their respective weights to obtain the weighted composite residual value of each matching point.

[0034] Finally, robust outlier consistency processing is performed on all weighted composite residuals. The median absolute deviation method is used, which is a robust statistic that is insensitive to extreme outliers. The median of the absolute deviations of all residuals from their medians is calculated, and the global statistical distribution of the residuals is calculated. The ratio of each residual to 3 times the MAD is used as the outlier index for that point. No residuals are removed in advance throughout the process. Finally, a complete set of point cloud registration residuals is generated, which includes the geometric type, normal residual, tangential residual, weighted composite residual, and outlier for each matching point.

[0035] S500: Adjust the weights of the point cloud registration residuals according to the motion constraints to construct a dynamic weighting mechanism.

[0036] Specifically, firstly, the relative motion constraints obtained from IMU pre-integration, which include the rotation and translation predictions between two frames and the corresponding noise covariance matrix, are used to calculate the deviation from the initial pose transformation estimate output by point cloud registration. The rotation and translation deviations are then converted into a unified Mahalanobis distance, which is a distance metric that considers the uncertainty of variables. The deviations in different dimensions are normalized using the covariance matrix, and then the Mahalanobis distance is mapped to the [0,1] interval using an exponential decay function to generate a frame-level consistency confidence score: when the Mahalanobis distance is less than 1 times the covariance, the confidence score is close to 1, indicating that the point cloud registration result is highly consistent with the IMU motion constraints and the proportion of dynamic objects in the scene is extremely low; when the Mahalanobis distance is greater than 3 times the covariance, the confidence score approaches 0, indicating that the point cloud registration result is distorted overall and there is a large amount of interference from dynamic objects in the scene.

[0037] Subsequently, point-level local outlier analysis was performed on all matching points in the real-time frame: Based on the weighted composite residual set generated in the previous step, the Huber robust kernel function, i.e., a piecewise continuous loss function, was adopted. When the residual was less than a preset threshold, a quadratic loss was used to retain the complete weight of small residual points; when the residual was greater than the threshold, a linear loss was switched to gradually reduce the weight of large residual points. The initial point-level weight of each point was calculated, and then the initial weight was calibrated by combining the global median absolute deviation (MAD) distribution of the residuals to generate point-level outlier confidence: For static background points with residuals less than 1.5 times MAD, the point-level confidence was close to 1; for potential dynamic points or mismatched points with residuals greater than 3 times MAD, the point-level confidence smoothly decreased to below 0.1.

[0038] Next, the frame-level consistency confidence and the point-level outlier confidence corresponding to each point are multiplied point by point to obtain the original dynamic weight values. When there are many dynamic objects in a frame, the weights of all points will be globally suppressed. At this time, joint optimization will automatically increase the weight of IMU motion constraints. When the overall registration quality of a frame is good, the weights are mainly determined by the point-level residuals, and only the influence of local dynamic points is suppressed. Finally, all original dynamic weight values ​​are linearly normalized to uniformly map the weight range to the [0,1] interval, ensuring that the magnitude of the point cloud geometric error term matches that of the IMU pre-integration error term. Then, the normalized dynamic weights are assigned to the corresponding point cloud registration residual values ​​one by one, thus completing the dynamic weight mechanism driven by both residuals and motion constraints.

[0039] S600: Construct a joint optimization model and combine it with the dynamic weighting mechanism to perform tight-coupled optimization on the current extrinsic parameter estimates of LIDAR-IMU, and generate online calibration results.

[0040] Specifically, a sliding window of 10-15 frames is first initialized, balancing computational load and accuracy. A keyframe selection strategy is used to filter frames entering the window. When the rotational change of adjacent frames exceeds 5 degrees or the translational change exceeds 0.5 meters, the current frame is set as a keyframe and added to the window to avoid redundant frames increasing the computational burden. At the same time, it is ensured that the window contains sufficient motion excitations. That is, extrinsic parameter calibration requires sufficient rotational and translational motions to observe the errors of all degrees of freedom. Pure translational motion cannot fully observe rotational extrinsic parameters.

[0041] Next, we define the set of variables to be optimized in the joint optimization model. This set contains three types of variables: The first type is the current extrinsic parameter estimates between the LIDAR-IMU, which consists of a 3×3 rotation matrix R and a 3×1 translation vector t. This is the core solution objective of the entire calibration method. The second type is the pose state variables of all keyframes within the sliding window. Each keyframe corresponds to the rotation, position, and velocity in the world coordinate system. The third type is the gyroscope zero bias and accelerometer zero bias of the IMU. These zero biases drift slowly with temperature and need to be estimated synchronously with the extrinsic parameters to eliminate systematic errors.

[0042] Subsequently, three core error terms are constructed sequentially: The first is a dynamically weighted point cloud geometric error term, which multiplies the normalized dynamic weight of each matched point generated in the previous step with the square of the corresponding weighted comprehensive residual, and then sums it over all matched points. This error term automatically suppresses the influence of dynamic objects and mismatched points through dynamic weights, allowing optimization to focus on highly reliable static feature points. The second is an IMU pre-integration error term, which calculates the Mahalanobis distance between the predicted and optimized values ​​of pose and velocity between adjacent keyframes based on the relative motion change and covariance matrix obtained from IMU pre-integration. This error term provides high-frequency motion constraints to compensate for the low sampling frequency of LiDAR, and also provides stable constraints in geometrically degraded scenarios. The third is an extrinsic parameter prior constraint term, which uses L2 regularization to penalize the deviation between the current extrinsic parameter and the extrinsic parameter at the previous moment, preventing the extrinsic parameter from undergoing drastic changes due to instantaneous noise or abnormal data, ensuring the temporal smoothness of the calibration results, and providing reasonable initial value constraints in the early stages of calibration to accelerate convergence.

[0043] The three error terms are weighted and summed according to preset weights to construct the overall cost function. This cost function unifies all sensor constraints into a differentiable nonlinear least squares problem. Then, the Gauss-Newton iterative method is used to minimize the overall cost function: in each iteration, the Jacobian and Hessian matrices of the variable to be optimized are calculated, the linear equations are solved to obtain the variable increment, the variable is updated, and the cost function is recalculated. This process is repeated until the magnitude of the variable increment is less than 1e-6 or the maximum number of iterations is reached. This method has fast convergence speed and low computational cost, making it very suitable for online real-time systems.

[0044] When the number of keyframes within the sliding window reaches the preset size, an edge-compensation operation is performed. This involves using Shure complement to remove the variables of the oldest keyframe from the optimization system, while transferring its constraint information to the remaining variables. This maintains a fixed window size and avoids the loss of historical information. The oldest keyframe and its corresponding point cloud data and IMU data are removed from the window to make room for new keyframes, ensuring that the computational load of the entire optimization process remains constant.

[0045] Finally, the rotation matrix and translation vector of the LIDAR-IMU extrinsic parameters are extracted from the optimized set of variables to be optimized and used as the online calibration result output at the current time. At the same time, the IMU zero bias estimate is updated for the IMU pre-integration calculation in the next frame.

[0046] Furthermore, in the method provided in the application embodiment, continuous frame point cloud registration is performed based on the current extrinsic parameter estimate of the LIDAR-IMU to obtain an initial pose transformation estimate. The method includes: parsing the current extrinsic parameter estimate of the LIDAR-IMU to obtain continuous LIDAR point cloud data and IMU pre-integration results. The continuous LIDAR point cloud data includes a first frame point cloud data and a second frame point cloud data, wherein the first frame point cloud data and the second frame point cloud data are consecutive adjacent frames, and the first frame point cloud data is the frame preceding the second frame point cloud data; relative motion prediction is performed on the first frame point cloud data and the second frame point cloud data based on the IMU pre-integration results, and the relative motion prediction value is calculated; the relative motion prediction value is then calculated. Geometric analysis is performed on the first frame point cloud data and the second frame point cloud data to extract geometric feature points, which correspond to the first frame point cloud data and the second frame point cloud data. The relative motion prediction value is used as the initial transformation guess data. Based on the initial transformation guess data, the second frame point cloud data is projected onto the point cloud coordinate system of the first frame point cloud data for nearest neighbor identification, and candidate matching point pairs are extracted. The candidate matching point pairs are traversed to calculate the distance, obtaining multiple spatial distance values ​​for multiple candidate matching point pairs. The multiple candidate matching point pairs are then subjected to confidence screening according to the multiple spatial distance values ​​to generate reliable matching point pairs. The least squares method is used to iteratively weight the reliable matching point pairs to calculate the initial pose transformation estimate.

[0047] Specifically, firstly, from the current extrinsic parameter estimates of the structured LIDAR-IMU calibrated by time synchronization and point-by-point timestamps, the time-strictly aligned continuous LIDAR point cloud data and the corresponding IMU pre-integration results for the time period are parsed out in global absolute timestamp order. This covers high-frequency inertial increment data for two complete acquisition cycles, including rotation increments, translation increments, and corresponding noise covariance matrices between the two frames. The continuous LIDAR point cloud data is divided into consecutive adjacent frames that are seamlessly connected in time, namely the first frame point cloud data and the second frame point cloud data. The acquisition time boundaries of the two are precisely aligned with the point-by-point timestamps of the last laser point in the previous frame and the first laser point in the next frame, rather than the average timestamp of the entire frame. Based on the IMU pre-integration results, relative motion prediction between the two frames is performed, and the relative motion prediction value containing 3 degrees of freedom rotation and 3 degrees of freedom translation is calculated. This prediction value serves as the initial transformation guess data for point cloud registration, which can reduce the registration search range that originally needed to be performed in the global 6-degree-of-freedom space to the local minimum region corresponding to the IMU pre-integration error.

[0048] Subsequently, local geometric analysis was performed on the point clouds of the first and second frames. For each point, k=20 nearest neighbors were selected to construct a local point cloud subset. The covariance matrix of the subset was calculated and eigenvalue decomposition was performed. Based on the relative magnitude of the three eigenvalues, two types of geometric feature points were selected: one type is edge feature points, whose neighborhood covariance matrix has one eigenvalue that is significantly larger than the other two, corresponding to areas in the point cloud such as corners of walls and vehicle outlines where the normal direction changes drastically; the other type is planar feature points, whose neighborhood covariance matrix has two eigenvalues ​​that are significantly larger than the third, corresponding to flat areas in the point cloud such as ground and walls where the normal direction is consistent. Retaining only these two types of feature points can compress the point cloud data volume to 5%-10% of the original data, while retaining the most discriminative geometric constraint information. Moreover, there is a natural geometric correspondence between the two types of feature points in the two frames.

[0049] The relative motion prediction values ​​are converted into a 4×4 homogeneous transformation matrix. Based on this matrix, all geometric feature points of the second frame are uniformly projected onto the coordinate system of the point cloud of the first frame. A kd-tree, a data structure used for fast nearest neighbor search in high-dimensional space, is used to recursively divide the three-dimensional point cloud space into a binary tree structure, reducing the time complexity of the nearest neighbor search from O(n²) to O(nlogn). For each projection point, the nearest similar feature point in the first frame is found, i.e., edge feature points are matched only with edge feature points, and planar feature points are matched only with planar feature points, generating a set of candidate matching point pairs. This similar matching strategy avoids invalid matching between different geometric structures.

[0050] The three-dimensional Euclidean spatial distance value is calculated by traversing all candidate matching point pairs to obtain a spatial distance value corresponding to each candidate matching point pair. Then, a two-layer screening mechanism is used to generate reliable matching point pairs: The first layer performs initial screening based on a preset deviation threshold and dynamically adjusted according to the LiDAR measurement accuracy. Points with spatial distance values ​​exceeding the threshold are marked as potential dynamic points and directly eliminated. This step can remove large-scale dynamic object points and obvious mismatch points. The second layer performs neighborhood distribution verification on the remaining matching points. Five nearest neighbor matching points are selected with each matching point as the center, and the consistency of the displacement direction of these nearest neighbor matching points is calculated to generate a matching point distribution consistency coefficient. When the distribution consistency coefficient is lower than 0.7, it indicates that the matching point is an isolated mismatch point and is eliminated. Finally, a set of reliable matching point pairs containing only high-confidence static feature points is obtained.

[0051] Finally, the iterative weighted least squares method is used to solve the initial pose transformation estimate based on reliable matching point pairs. This method differs from the ordinary least squares method, which assigns the same weight to all matching points. In each iteration, the registration residual of all matching points is calculated first. Points with residuals greater than twice the median residual are assigned weights close to 0. The pose transformation matrix is ​​then solved again. This iterative process is repeated until the pose change between two adjacent iterations is less than 1e-6 or the maximum number of iterations is reached. Finally, a more robust initial pose transformation estimate is obtained.

[0052] Furthermore, in the method provided in the application embodiment, the multiple candidate matching point pairs are reliably screened according to the multiple spatial distance values ​​to generate reliable matching point pairs. The method includes: predicting based on the initial transformation guessing data and setting an expected distance value; traversing the multiple candidate matching point pairs to calculate the deviation between the multiple spatial distance values ​​and the expected distance values, generating distance deviation data; when the distance deviation data exceeds a preset deviation threshold, extracting matching points for marking to generate multiple potential dynamic points; matching and eliminating the multiple potential dynamic points based on the multiple candidate matching points to generate multiple remaining matching point pairs; traversing the neighborhood space based on the multiple remaining matching point pairs to perform distribution verification and generate a matching point distribution consistency coefficient; and reliably identifying the multiple remaining matching points according to the matching point distribution consistency coefficient to determine reliable matching point pairs.

[0053] Specifically, firstly, based on the initial transformation guess data obtained from the IMU pre-integration results, which is the relative pose prediction value of two frames independent of the point cloud data, derived from inertial measurement data that is not affected by environmental dynamics, it can reflect the true overall motion of the carrier. Theoretical matching prediction is then performed, with the expected distance value set to 0. In an ideal static scene without noise or dynamic objects, the feature points of the second frame after projection of the initial transformation guess matrix should completely overlap with the corresponding feature points of the same type in the first frame. Therefore, the theoretical matching distance is 0, and the actual deviation only comes from the inherent error of LiDAR ranging noise and IMU pre-integration.

[0054] Then, all candidate matching point pairs are traversed, and the difference between the actual distance value and the expected distance value of each point pair in three-dimensional Euclidean space is calculated to generate distance deviation data corresponding to each candidate matching point pair. This data directly reflects the motion consistency of the matching point pairs: the motion of static background points is synchronized with the overall motion of the carrier, and the distance deviation is only at the noise level of centimeters; while the motion of dynamic object points is independent of the carrier, and the distance deviation will increase significantly.

[0055] Next, based on the ranging accuracy of LIDAR, the covariance uncertainty of IMU pre-integration, and the dynamic level of the scene, a preset deviation threshold is set. When the distance deviation data of a candidate matching point pair exceeds the threshold, it is marked as a potential dynamic point. These points are likely to be targets such as vehicles and pedestrians that move independently in the environment. Their motion patterns are inconsistent with the overall motion of the carrier, which will seriously interfere with the accuracy of pose estimation.

[0056] Subsequently, all potentially dynamic points marked are batch-removed from the candidate matching point pair set, generating a set of remaining matching point pairs containing only small-deviation matching points. This step quickly removes large-scale dynamic object points and obvious mismatch points, significantly reducing subsequent computation. Next, the neighborhood spatial distribution of the remaining matching point pairs is verified point by point: a spherical neighborhood with a radius of 1 meter is selected in 3D space centered on each remaining matching point, and the displacement vectors of all other remaining matching points in this neighborhood are extracted, i.e., the 3D deviation vectors between the second frame point after the initial transformation and the corresponding point in the first frame. The directional consistency between these displacement vectors is then calculated. Along with amplitude consistency, a matching point distribution consistency coefficient is generated. This coefficient is based on the rigid assumption of a static scene, that is, all static points in the same local area should have completely identical motion displacement vectors. Therefore, the closer the coefficient is to 1, the higher the motion consistency between the point and its neighboring points, and the greater the probability that it belongs to a static point. The closer the coefficient is to 0, the more likely the point is an isolated mismatched point or a small-scale dynamic point. In the specific calculation, all neighboring displacement vectors are first normalized, the average value of the dot product between each pair of vectors is calculated, and then multiplied by the inverse of the variance of the displacement vector amplitude as a weighting coefficient. Finally, the distribution consistency coefficient with a value range between [0,1] is obtained.

[0057] Finally, a consistency coefficient threshold is set, and the remaining matching points with a consistency coefficient greater than or equal to the threshold are marked as reliable matching points, while those with a consistency coefficient less than the threshold are removed. This process generates a set of reliable matching point pairs that contains only highly reliable static background feature points.

[0058] Furthermore, in the method provided in the application embodiment, pre-integration calculation is performed based on the IMU data to construct motion constraints. The method includes: extracting IMU angular velocity and acceleration measurements within the time interval corresponding to the first frame point cloud data and the second frame point cloud data; sorting the IMU angular velocity and acceleration measurements according to the first frame point cloud data and the second frame point cloud data to construct an IMU measurement data sequence; extracting the acquisition time distribution data of the LIDAR data to generate a point cloud scanning period; dividing the IMU measurement data sequence into multiple sub-intervals according to the point cloud scanning period; performing pre-integration calculation based on the multiple sub-intervals to obtain the relative motion changes of the multiple sub-intervals; combining the relative motion changes of the multiple sub-intervals in chronological order to obtain the overall pre-integration result of the first frame point cloud data and the second frame point cloud data; performing noise calculation based on the IMU measurement data sequence to construct a noise covariance matrix; integrating the overall pre-integration result with the noise covariance matrix to construct an IMU pre-integration residual vector; and outputting the IMU pre-integration residual vector as a motion constraint.

[0059] Specifically, firstly, based on the current extrinsic parameter estimates of the LIDAR-IMU calibrated point by point, the raw IMU measurements, including triaxial angular velocity and triaxial acceleration, are extracted that completely coincide with the time boundaries of the first laser point in the first frame point cloud and the last laser point in the second frame point cloud. The extracted IMU measurements are then strictly sorted in ascending order according to the global absolute timestamps to construct a time-continuous and uninterrupted IMU measurement data sequence. Since the IMU sampling frequency is much higher than the LIDAR sampling frequency, there are usually multiple IMU measurement points between two LIDAR frames, which can completely capture all the subtle motion changes of the carrier within the frame interval.

[0060] Subsequently, the acquisition time distribution data of the LiDAR data is extracted to generate the point cloud scanning period, which is the complete time interval from the emission of the first laser point to the reception of the last laser point in a single frame of point cloud. This period reflects the essential characteristic of LiDAR non-instantaneous scanning, that is, all laser points in a single frame are not acquired at the same time, but are evenly distributed throughout the entire scanning period. According to the point cloud scanning period, the IMU measurement data sequence is divided into multiple equal-length sub-intervals that correspond one-to-one with the acquisition time of the laser points. Each sub-interval corresponds to the acquisition time of one or a group of laser points. This segmentation method can match the carrier motion state corresponding to each laser point.

[0061] Next, IMU pre-integration is performed on the IMU data of each sub-interval. Pre-integration directly calculates the relative motion changes between adjacent sub-intervals through incremental integration, including rotation increment (using quaternions to avoid the gimbaling problem of Euler angles), velocity increment, and position increment. At the same time, the gravitational acceleration term is separated into the world coordinate system during integration, so that the pre-integration result is only related to the relative motion of the IMU and is independent of the global attitude. When the global state is updated in subsequent joint optimization, only the pre-integration result needs to be corrected by the pre-integration Jacobian matrix, without re-integrating all IMU data. The relative motion changes of all sub-intervals are chained together in chronological order, that is, rotation increments are multiplied sequentially, and velocity increments and position increments are accumulated sequentially, finally obtaining the overall pre-integration result between the first frame point cloud and the second frame point cloud. This result fully describes the overall motion state of the carrier during the acquisition of the two frames of point cloud.

[0062] Based on this, noise modeling and calculation are performed on the IMU measurement data sequence to construct a noise covariance matrix. The measurement noise of the IMU includes Gaussian white noise and random walk noise. The covariance of the pre-integration error of each sub-interval is recursively calculated using the idea of ​​error state Kalman filtering, and then propagated to the overall pre-integration result to quantify the uncertainty of the three dimensions of rotation, velocity, and position. The uncertainty of the rotation dimension is usually much smaller than that of the position dimension. This covariance matrix not only reflects the reliability of the pre-integration result, but also provides a natural uncertainty measure for subsequent joint optimization.

[0063] Finally, the overall pre-integration results are integrated with the corresponding noise covariance matrix to construct the IMU pre-integration residual vector. This residual vector is defined as the difference between the relative motion obtained by pre-integration and the relative motion obtained by point cloud registration. The residual is normalized by the covariance matrix to form motion constraints based on Mahalanobis distance, which are then input into the subsequent joint optimization model.

[0064] Furthermore, in the method provided in the application embodiment, the point cloud registration residual value is calculated based on the initial pose transformation estimate and the matching residual. The method includes: traversing the trusted matching point pairs based on the initial pose transformation estimate to generate multiple transformation points; calculating the geometric spatial deviation based on the multiple transformation points to generate multiple basic residual values, wherein the multiple basic residual values ​​correspond to the trusted matching points; performing local geometric analysis based on the trusted matching point pairs to extract local geometric features; performing directional decomposition on the multiple basic residual values ​​according to the local geometric features to generate multi-directional residual components, wherein the multi-directional residual components include tangential residual components and normal residual components; assigning weights based on the tangential residual components and the normal residual components to determine multiple weight coefficients; performing weighted calculations on the tangential residual components and the normal residual components in combination with the multiple weight coefficients to obtain a weighted comprehensive residual value; and performing outlier consistency processing based on the weighted comprehensive residual value to generate the point cloud registration residual value.

[0065] Specifically, the 6-DOF initial pose transformation estimate obtained from the previous solution is first converted into a 4×4 homogeneous transformation matrix, which is a linear transformation mathematical tool containing a 3×3 rotation matrix and a 3×1 translation vector. It can simultaneously realize the rotation and translation operations of points in three-dimensional space while maintaining linear operation characteristics. It is the standard form for describing the pose relationship of multiple sensors and coordinate transformation. Here, the scale is fixed at 1 to conform to the rigid body motion assumption. All reliable matching point pairs are traversed. After initial screening by distance threshold and verification of neighborhood distribution consistency, high-reliability matching points are retained. Large-scale dynamic points and isolated mismatched points have been removed. Only geometric feature points in the static background are retained. The source feature points of the second frame are projected onto the target point cloud coordinate system of the first frame through the homogeneous transformation matrix to obtain the transformation points in the same coordinate system as the target feature points, thus completing the spatial alignment of the feature points of the two frames.

[0066] Next, the three-dimensional Euclidean distance between each transformation point and the corresponding target feature point is calculated to generate a basic residual value that corresponds one-to-one with the matching point. This value intuitively reflects the spatial deviation of the matching point pair, but does not consider the difference in sensitivity of different geometric features to deviations in different directions, and cannot distinguish between the true pose error and the false residual caused by geometric symmetry.

[0067] Subsequently, local geometric analysis is performed on each target feature point: a subset of the local point cloud is constructed by selecting k=20 nearest neighbor points centered on the point, and the covariance matrix of the subset is calculated. This matrix is ​​used to describe the spatial dispersion of the local point cloud. Eigenvalue decomposition can quantify the extension characteristics of the point cloud in three orthogonal directions. Local geometric features are extracted based on the relative magnitude of the three eigenvalues: if two eigenvalues ​​are significantly greater than the third, the point is identified as a planar feature point, corresponding to flat areas such as the ground or walls; if one eigenvalue is significantly greater than the other two, the point is identified as an edge feature point, corresponding to sharp areas such as corners or vehicle outlines. At the same time, the unit normal vector and tangent plane of the plane containing the planar feature point are calculated, or the unit direction vector and perpendicular plane of the edge containing the edge feature point are calculated.

[0068] Based on this, the basic residual values ​​are decomposed directionally: the three-dimensional vector corresponding to the basic residual is projected onto the normal vector direction to obtain the normal residual component, which is the deviation along the normal direction of the feature geometry. For planar feature points, this component directly reflects the rotation and translation errors of the pose and is highly sensitive to pose changes. For edge feature points, this component corresponds to the deviation perpendicular to the edge extension direction and also has high pose sensitivity. Projecting it onto the tangent plane or the edge extension direction yields the tangential residual component, which is the deviation along the tangent direction of the feature geometry. Due to the translational symmetry of the plane and the extension symmetry of the edge, this component has extremely low sensitivity to pose changes. Its large value usually originates from point cloud discretization errors and feature extraction noise rather than true pose errors.

[0069] Subsequently, differentiated weights are assigned based on the geometric type. For planar feature points, the normal residual component is assigned a weight of 0.85 and the tangential residual component a weight of 0.15. For edge feature points, the normal residual component perpendicular to the edge direction is assigned a weight of 0.9 and the tangential residual component along the edge direction a weight of 0.1. The two components are then weighted and summed according to their respective weights to obtain the weighted composite residual value for each matching point. This value effectively filters out spurious residuals caused by geometric symmetry and significantly improves the signal-to-noise ratio of the residual signal.

[0070] Finally, all weighted composite residuals are processed for median absolute deviation outlier consistency, which is a robust statistical method that is insensitive to extreme outliers. The median of the absolute deviation of all residuals from their medians is calculated, which can accurately estimate the normal distribution range of the residuals in scenarios with a large number of dynamic points. The ratio of each residual value to 3 times the MAD is used as the outlier index for that point. No residual points are removed in advance throughout the process. Finally, a complete set of point cloud registration residuals is generated, which includes the geometric type, normal residual, tangential residual, weighted composite residual and outlier of each matching point.

[0071] Furthermore, in the method provided in the application embodiment, the point cloud registration residual values ​​are weighted according to the motion constraints to construct a dynamic weighting mechanism. The method includes: calculating the deviation between the motion constraints and the initial pose transformation estimate to generate a frame-level consistency confidence score; locating the real-time frame point cloud to extract the real-time residual distribution position; performing outlier analysis on the real-time residual distribution position based on the point cloud registration residual values ​​to calculate a point-level outlier confidence score; multiplying the frame-level consistency confidence score by the point-level outlier confidence score to obtain multiple dynamic weight values; normalizing the multiple dynamic weight values ​​to generate multiple normalized dynamic weights; and assigning the multiple normalized dynamic weights to the point cloud registration residual values ​​to construct the dynamic weighting mechanism.

[0072] Specifically, firstly, based on motion constraints, namely the two-frame rotation increment, translation prediction value, and corresponding noise covariance matrix obtained from IMU pre-integration, which are completely independent of point cloud data and unaffected by environmental dynamic objects, lighting changes, and texture loss, the deviation from the initial pose transformation estimate output by point cloud registration is calculated. The rotation and translation deviations are converted into a unified Mahalanobis distance, that is, the deviations of different dimensions and uncertainties are normalized by the covariance matrix, which can more accurately reflect the statistical significance of the deviation. Then, the Mahalanobis distance is mapped to the [0,1] interval through an exponential decay function to generate frame-level consistency confidence: when the Mahalanobis distance is less than 1 times the covariance, the confidence is close to 1, indicating that the point cloud registration result is highly consistent with the IMU motion constraints and the proportion of dynamic objects in the scene is extremely low; when the Mahalanobis distance is greater than 3 times the covariance, the confidence approaches 0, indicating that the point cloud registration result is distorted overall and there are a large number of dynamic objects in the scene. At this time, joint optimization will automatically increase the weight of IMU motion constraints to ensure calibration stability.

[0073] Subsequently, the positions of all matching points in the real-time frame point cloud are located, and the weighted composite residual values ​​at the corresponding positions are extracted. The Huber robust kernel function, i.e., a piecewise continuous loss function, is used. When the residual is less than a preset threshold, a quadratic loss is used to fully retain the weights of small residual points; when the residual is greater than the threshold, a linear loss is switched to gradually reduce the weights of large residual points. Combined with the median absolute deviation, i.e., a robust statistic that is not sensitive to extreme outliers, the median of the absolute deviations of all residual values ​​from the median is calculated to perform point-level outlier analysis: first, the global statistical threshold of the residuals is calculated using MAD, and then the weighted composite residual of each point is input into the Huber kernel function to generate point-level outlier confidence scores with values ​​ranging from [0,1]. For static background points with residuals less than 1.5 times MAD, the point-level confidence score is close to 1; for potential dynamic points or mismatched points with residuals greater than 3 times MAD, the point-level confidence score smoothly decreases to below 0.1.

[0074] Next, the frame-level consistency confidence and the point-level outlier confidence corresponding to each matching point are multiplied point by point to obtain the original dynamic weight value. When there are many dynamic objects in a frame, the weight of all points will be globally suppressed. When the overall registration quality of a frame is good, the weight is mainly determined by the point-level residual, and only the influence of local dynamic points is suppressed. For example, in a congested intersection scene in the city, the frame-level consistency confidence is about 0.3, the point-level outlier confidence of a static background point is 0.9, and the final original weight is 0.27, while the point-level outlier confidence of a dynamic vehicle point is 0.1, and the final original weight is only 0.03, which achieves accurate suppression of dynamic objects.

[0075] Subsequently, all original dynamic weight values ​​are linearly normalized to uniformly map the weight range to the [0,1] interval, ensuring that the magnitude of the point cloud geometric error term matches that of the IMU pre-integration error term, and ensuring that the constraints of the two types of sensors can correct each other and complement each other.

[0076] Finally, the normalized dynamic weights are assigned one by one to the corresponding point cloud registration residual values, thus completing the dynamic weight mechanism driven by both IMU motion constraints and point cloud residuals.

[0077] Furthermore, in the method provided in the application embodiment, a joint optimization model is constructed in conjunction with the dynamic weighting mechanism to perform tight-coupled optimization on the current extrinsic parameter estimates of the LIDAR-IMU, generating online calibration results. The method includes: setting a keyframe sliding window, performing sliding traversal through the keyframe sliding window to extract the pose state variables of multiple keyframes and the current extrinsic parameter estimates of the LIDAR-IMU; defining a set of variables to be optimized for the joint optimization model based on the pose state variables of the multiple keyframes and the current extrinsic parameter estimates of the LIDAR-IMU; and weighting the current extrinsic parameter estimates of the LIDAR-IMU through the dynamic weighting mechanism. The residual squares are summed to construct the point cloud geometric error term; the IMU error term is constructed based on the IMU pre-integration residual vector; the current extrinsic parameter estimates of the LIDAR-IMU are smoothed and regularized to construct the extrinsic parameter prior constraint term; the point cloud geometric error term, the IMU pre-integration error term, and the extrinsic parameter prior constraint term are weighted and summed to form the overall cost function; the overall cost function is used for nonlinear iterative minimization to update the set of variables to be optimized; when the number of iterations reaches the maximum number of iterations, the optimization result is generated; the rotation matrix and translation vector of the LIDAR-IMU are extracted based on the optimization result and output as the online calibration result.

[0078] Specifically, a keyframe sliding window of 10-15 frames is first initialized, which is an online optimized memory management and computation scheduling mechanism. The most recent keyframe data is retained by the fixed-size window, and a motion excitation-driven keyframe selection strategy is used to filter the frames entering the window: when the rotation change of adjacent frames exceeds 5 degrees or the translation change exceeds 0.5 meters, the current frame is set as a keyframe and added to the window. This avoids redundant frames from increasing unnecessary computational burden and ensures that the window contains sufficient motion excitation. Extrinsic parameter calibration requires sufficient rotation and translational motion to observe the errors of all 6 degrees of freedom. Pure translational motion cannot fully observe the rotational extrinsic parameters. By sliding through the keyframe sliding window, the pose state variables of all keyframes in the window and the current extrinsic parameter estimates of the LiDAR-IMU to be solved are extracted. Next, the set of variables to be optimized in the joint optimization model is defined. This set contains three interrelated types of variables: The first type is the current extrinsic parameter estimates between the LIDAR-IMU, which consists of a 3×3 rotation matrix R and a 3×1 translation vector t, and is the core solution objective of the entire calibration method; the second type is the pose state variables of all keyframes within the sliding window. Each keyframe corresponds to rotation in the world coordinate system, represented by quaternions, position, and velocity, for a total of 9 state variables; the third type is the zero bias of the IMU's gyroscope and accelerometer. These zero biases will drift slowly with temperature changes and device aging, and need to be estimated synchronously with the extrinsic parameters to eliminate system errors.

[0079] Subsequently, three error terms are constructed sequentially: The first is a dynamically weighted point cloud geometric error term, which multiplies the normalized dynamic weight of each matching point generated in the previous step with the square of the corresponding weighted comprehensive residual, and then sums it over all matching points of all keyframes within the window. This error term automatically suppresses the influence of dynamic objects and mismatched points through dynamic weights, allowing the optimization process to automatically focus on high-confidence static feature points, thus completely solving the problem of poor robustness to dynamic environments caused by the traditional fixed-weight error term treating all points equally; The second is an IMU pre-integration error term, which is based on the relative motion change obtained from the previous IMU pre-integration and the noise coefficient. The variance matrix calculates the Mahalanobis distance between the optimized pose and velocity values ​​and the pre-integrated predicted values ​​between adjacent keyframes. This error term provides high-frequency motion constraints, compensating for the low sampling frequency of LiDAR and enabling accurate capture of the carrier's rapid motion within the frame interval. The third is the extrinsic parameter prior constraint term, which uses L2 regularization to penalize the deviation between the current extrinsic parameter and the extrinsic parameter obtained from the previous time step, i.e., smoothing deviation regularization. This constraint introduces a time smoothness prior to prevent drastic changes in extrinsic parameters due to instantaneous noise or abnormal data. At the same time, it provides a reasonable initial value range for extrinsic parameters in the early stage of calibration, accelerating the convergence speed.

[0080] The three error terms are weighted and summed according to preset weights to construct the overall cost function. This cost function unifies all sensor constraints and prior information into a differentiable nonlinear least squares problem. Then, the Gauss-Newton iterative method is used to minimize the overall cost function: in each iteration, the Jacobian matrix and approximate Hessian matrix of the variable to be optimized are calculated, the linear equations are solved to obtain the variable increment, the variable is updated, and the cost function is recalculated. This process is repeated until the magnitude of the variable increment is less than 1e-6 or the maximum number of iterations is reached. This method has fast convergence speed and low computational cost, making it very suitable for online real-time systems on embedded platforms. When the number of keyframes within the sliding window reaches a preset size, an edge-compensation operation is performed, which is a mathematical method that retains historical constraint information while removing variables from old frames. The variables of the oldest keyframe are eliminated from the optimization system through Schur complement, while their constraint information is transferred to the remaining variables. This maintains a fixed window size and avoids the loss of historical information. The oldest keyframe and its corresponding point cloud data and IMU data are removed from the window to make room for new keyframes, ensuring that the computational cost of the entire optimization process remains constant.

[0081] Finally, the rotation matrix and translation vector of the LIDAR-IMU extrinsic parameters are extracted from the optimized set of variables to be optimized and used as the online calibration result output at the current time. At the same time, the IMU zero bias estimate is updated for the IMU pre-integration calculation in the next frame, forming a complete online calibration closed loop.

[0082] Furthermore, in the method provided in the application embodiment, the current extrinsic parameter estimates of the LIDAR-IMU are weighted and the squared residuals are summed through the dynamic weighting mechanism to construct a point cloud geometric error term. The method includes: traversing the current extrinsic parameter estimates of the LIDAR-IMU through the dynamic weighting mechanism and calculating according to the point cloud registration residual values ​​to obtain multiple weighted squared residual values, wherein the multiple weighted squared residual values ​​correspond to the current extrinsic parameter estimates of the LIDAR-IMU; and accumulating the multiple weighted squared residual values ​​to generate the point cloud geometric error term.

[0083] Specifically, firstly, using a keyframe sliding window—an online optimized memory management and computation scheduling mechanism—the most recent 10-15 keyframes are retained within a fixed-size window. This extracts the current extrinsic parameter estimates for all adjacent keyframe pairs corresponding to the structured LiDAR-IMU. Each entry strictly includes a set of reliable matching point pairs obtained through distance threshold screening and neighborhood distribution verification, and the weighted composite residual value corresponding to each matching point. This is the dynamic weight value that has undergone normal / tangential differential weighting and linear normalization. Then, through a dynamic weighting mechanism, all current extrinsic parameter estimates within the sliding window are traversed. For each reliable matching point pair, a point-by-point weighted squared residual calculation is performed: the normalized dynamic weight of that point is multiplied by the square of the weighted composite residual. Multiplication yields a weighted squared residual value corresponding to each matching point. The normalized dynamic weight is a [0,1] interval value obtained from the previous evaluation using both frame-level consistency and point-level outlier. This value has been calibrated to the magnitude of the IMU pre-integration error term, ensuring that the two types of sensor constraints are equally important in the optimization process. The weighted comprehensive residual is the residual after decomposition and differential weighting in the normal / tangential direction, which has filtered out spurious residuals caused by geometric symmetry such as planar tangential displacement and edge extension direction displacement. The dynamic weights of dynamic object points and isolated mismatched points are usually below 0.1. Even if their original residuals are large, the corresponding weighted squared residuals will be significantly suppressed. The dynamic weights of static background points are close to 1, and their residuals can fully contribute to the error term.

[0084] Next, the weighted squared residuals of all matching points of all adjacent keyframe pairs within the sliding window are globally accumulated to generate the final point cloud geometric error term, which can be expressed mathematically as follows:

[0085] ;

[0086] in, The normalized dynamic weight for the i-th matching point. The weighted composite residual of the i-th matching point is summed over a range that covers all reliable matching points of all keyframe pairs within the sliding window. Global accumulation across multiple frames can compensate for the insufficient constraints of single-frame point cloud, and the accumulated geometric constraints across multiple frames can provide sufficient observation information to solve for the 6-DOF extrinsic parameters, thus avoiding the optimization from falling into an underdetermined state.

[0087] In summary, the LIDAR-IMU online calibration method based on dynamic weights provided in this application has the following technical effects:

[0088] By constructing a tightly coupled joint optimization framework that integrates point cloud registration and IMU pre-integration, and introducing a dynamic weighting mechanism driven by both frame-level consistency confidence and point-level outlier confidence, the system automatically suppresses the interference of dynamic objects and mismatched points on the optimization process. Combined with a sliding window edge-out strategy, the system ensures online computation efficiency and ultimately achieves high-precision online real-time calibration of LiDAR-IMU extrinsic parameters in complex dynamic environments. This improves the localization and mapping stability of the multi-sensor fusion sensing system in complex dynamic environments.

[0089] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A method for online calibration of LiDAR-IMU based on dynamic weights, characterized in that, The method includes: Acquire LIDAR data and IMU data for time synchronization, and construct the current extrinsic parameter estimates of LIDAR-IMU; Based on the current extrinsic parameter estimates of the LIDAR-IMU, continuous frame point cloud registration is performed to obtain the initial pose transformation estimate. Based on the IMU data, pre-integration calculations are performed to construct motion constraints; Based on the initial pose transformation estimate and the matching residual, calculate the point cloud registration residual value; The point cloud registration residual values ​​are weighted according to the motion constraints to construct a dynamic weighting mechanism. A joint optimization model is constructed to perform tight-coupled optimization of the current extrinsic parameter estimates of LIDAR-IMU using the dynamic weighting mechanism, generating online calibration results.

2. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 1, characterized in that, Based on the current extrinsic parameter estimates of the LIDAR-IMU, continuous frame point cloud registration is performed to obtain initial pose transformation estimates. The method includes: Based on the current extrinsic parameter estimates of the LIDAR-IMU, the LIDAR continuous point cloud data and IMU pre-integration results are obtained. The LIDAR continuous point cloud data includes a first frame of point cloud data and a second frame of point cloud data. The first frame of point cloud data and the second frame of point cloud data are consecutive adjacent frames, and the first frame of point cloud data is the frame preceding the second frame of point cloud data. Based on the IMU pre-integration results, relative motion prediction is performed between the first frame point cloud data and the second frame point cloud data, and the relative motion prediction value is calculated. Geometric analysis is performed on the first frame point cloud data and the second frame point cloud data to extract geometric feature points, and the geometric feature points have a corresponding relationship with the first frame point cloud data and the second frame point cloud data; The relative motion prediction value is used as the initial transformation guess data. Based on the initial transformation guess data, the second frame point cloud data is projected onto the point cloud coordinate system of the first frame point cloud data for nearest neighbor identification, and candidate matching point pairs are extracted. The candidate matching point pairs are traversed and distance calculations are performed to obtain multiple spatial distance values ​​for multiple candidate matching point pairs; Based on the multiple spatial distance values, the multiple candidate matching point pairs are subjected to reliable filtering to generate reliable matching point pairs; The initial pose transformation estimate is calculated by iteratively weighting the trusted matching point pairs using the least squares method.

3. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 2, characterized in that, The method for generating reliable matching point pairs by performing a reliable filtering of the multiple candidate matching point pairs based on the multiple spatial distance values ​​includes: Based on the initial transformation guess data, a prediction is made, and a desired distance value is set; The deviations between multiple spatial distance values ​​and the expected distance values ​​are calculated by traversing the multiple candidate matching point pairs, and distance deviation data is generated. When the distance deviation data exceeds a preset deviation threshold, matching points are extracted and marked to generate multiple potential dynamic points; Based on the multiple candidate matching points, the multiple potential dynamic points are matched and eliminated to generate multiple remaining matching point pairs; Based on the multiple remaining matching points, the distribution of the traversed neighborhood space is verified to generate a matching point distribution consistency coefficient. The remaining matching points are identified as trustworthy based on the matching point distribution consistency coefficient, thereby determining trustworthy matching point pairs.

4. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 2, characterized in that, Based on the IMU data, pre-integration calculations are performed to construct motion constraints. The method includes: Extract the IMU angular velocity and acceleration measurements within the time interval corresponding to the first frame point cloud data and the second frame point cloud data; The IMU angular velocity and acceleration measurements are sorted according to the first frame point cloud data and the second frame point cloud data to construct an IMU measurement data sequence; Extract the acquisition time distribution data of the LIDAR data to generate the point cloud scanning cycle; The IMU measurement data sequence is divided into multiple sub-intervals according to the point cloud scanning cycle. Pre-integration calculation is performed based on the multiple sub-intervals to obtain the relative motion change of the multiple sub-intervals. The relative motion changes of the multiple sub-intervals are combined in chronological order to obtain the overall pre-integration result of the first frame point cloud data and the second frame point cloud data; Noise is calculated based on the IMU measurement data sequence to construct a noise covariance matrix; The overall pre-integration result is integrated with the noise covariance matrix to construct the IMU pre-integration residual vector, which is then output as a motion constraint.

5. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 2, characterized in that, Based on the initial pose transformation estimate and matching residual, the point cloud registration residual value is calculated, and the method includes: Based on the initial pose transformation estimate, the trusted matching point pairs are traversed and transformed to generate multiple transformation points; Geometric spatial deviation is calculated based on the multiple transformation points to generate multiple basic residual values, and these multiple basic residual values ​​have a corresponding relationship with the reliable matching points. Local geometric analysis is performed based on the trusted matching point pairs to extract local geometric features; Based on the local geometric features, multiple basic residual values ​​are decomposed in a direction to generate multi-directional residual components, which include tangential residual components and normal residual components. Weights are assigned based on the tangential residual components and the normal residual components, and multiple weight coefficients are determined. The tangential residual components and the normal residual components are then combined with the multiple weight coefficients to perform a weighted calculation to obtain a weighted comprehensive residual value. Outlier consistency processing is performed based on the weighted composite residual value to generate point cloud registration residual values.

6. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 1, characterized in that, The point cloud registration residual values ​​are weighted according to the motion constraints to construct a dynamic weighting mechanism. The method includes: Based on the motion constraints and the initial pose transformation estimate, the deviation is calculated to generate a frame-level consistency confidence score. The real-time residual distribution position is extracted from the real-time frame point cloud. Based on the registration residual value of the point cloud, outlier analysis is performed on the real-time residual distribution position, and the point-level outlier confidence is calculated. The frame-level consistency confidence score is multiplied by the point-level outlier confidence score to obtain multiple dynamic weight values; The multiple dynamic weight values ​​are normalized to generate multiple normalized dynamic weights, and the multiple normalized dynamic weights are assigned to the point cloud registration residual values ​​to construct the dynamic weight mechanism.

7. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 4, characterized in that, A joint optimization model is constructed using the aforementioned dynamic weighting mechanism to perform tightly coupled optimization on the LIDAR-IMU dataset, generating online calibration results. The method includes: Set a keyframe sliding window, and traverse through the keyframe sliding window to extract the pose state variables and the current extrinsic parameter estimates of multiple keyframes. Based on the pose state variables of the multiple keyframes and the current extrinsic parameter estimates of the LIDAR-IMU, a set of variables to be optimized for the joint optimization model is defined. The point cloud geometric error term is constructed by summing the weighted residual squares of the current extrinsic parameter estimates of the LIDAR-IMU through the dynamic weighting mechanism. Construct the IMU error term based on the IMU pre-integrated residual vector; The current extrinsic parameter estimates of the LIDAR-IMU are smoothed and regularized to construct prior constraint terms for the extrinsic parameters. The point cloud geometric error term, the IMU pre-integration error term, and the extrinsic parameter prior constraint term are weighted and summed to form the overall cost function; Based on the overall cost function, a nonlinear iterative minimization solution is performed, the set of variables to be optimized is updated, and the optimization result is generated when the number of iterations reaches the maximum number of iterations. Based on the optimization results, the rotation matrix and translation vector of the LIDAR-IMU are extracted and output as the online calibration results.

8. The online calibration method for LIDAR-IMU based on dynamic weights as described in claim 7, characterized in that, The point cloud geometric error term is constructed by weighting and summing the squared residuals of the current extrinsic parameter estimates of the LIDAR-IMU using the dynamic weighting mechanism. The method includes: The current extrinsic parameter estimates of the LIDAR-IMU are iterated through by the dynamic weighting mechanism and calculated according to the point cloud registration residual values ​​to obtain multiple weighted squared residual values. The multiple weighted squared residual values ​​correspond to the current extrinsic parameter estimates of the LIDAR-IMU. The multiple weighted squared residuals are summed to generate the point cloud geometric error term.

Images