Multi-sensor fusion-based external parameter calibration method, medium, equipment and system

Abstract

The invention provides an external parameter calibration method based on multi-sensor fusion, a medium, equipment and a system, and the method comprises the steps: respectively obtaining radar point cloud data and inertial navigation data based on a time synchronization principle; performing dynamic point elimination on the radar point cloud data and the inertial navigation data; performing static feature matching according to the radar point cloud data after the dynamic points are removed and the inertial navigation data; combining the residual error after static feature matching to construct each residual error of tight coupling optimization; and determining an optimal external parameter by minimizing each residual error. According to the invention, high-precision and high-robustness external parameter estimation can be realized on the premise of no calibration board, no GNSS and no static environment assumption.

Description

Technical Field

[0001] This application belongs to the technical field of measurement and positioning, and relates to an external parameter calibration method, medium, device and system based on multi-sensor fusion. Background Technology

[0002] In autonomous driving and robotic systems, LiDAR (Light Laser Detection and Ranging) provides high-precision 3D environmental structure information, while IMU (Inertial Measurement Unit) provides high-frequency attitude and acceleration data. Fusion of these two technologies enables high-precision, high-frequency pose estimation. However, deviations in the coordinate transformation relationship (i.e., the extrinsic parameter matrix) between LiDAR and IMU will significantly reduce the positioning accuracy of the fused system.

[0003] Existing extrinsic parameter calibration methods mainly include: (1) Offline methods based on calibration boards: relying on manually designed planar or QR code coded calibration boards for calibration in static environments. This method has high accuracy, but cannot adapt to dynamic scenarios and cannot cope with mechanical loosening during long-term operation. (2) Loosely coupled online methods: using the LiDAR odometry output pose and IMU integral trajectory alignment to estimate extrinsic parameters. This type of method only uses the front-end results and does not utilize the geometric consistency between the original point cloud and the IMU dynamics, resulting in weak observability of extrinsic parameters, especially prone to divergence under low-excitation motion. (3) GNSS (Global Navigation Satellite System) or vision-assisted calibration: this type of method is costly, relies on external signals, and fails in scenarios such as tunnels, underground, and low light.

[0004] Furthermore, existing methods generally assume a static environment and do not consider the contamination of point cloud features by dynamic obstacles (such as pedestrians and vehicles), leading to incorrect matching and deviations in extrinsic parameter estimation, which makes it difficult to meet the robustness requirements in real complex traffic environments. Summary of the Invention

[0005] This application provides a method, medium, device and system for extrinsic parameter calibration based on multi-sensor fusion, which is used to improve the robustness of extrinsic parameter calibration in real complex traffic environments.

[0006] In a first aspect, this application provides an extrinsic parameter calibration method based on multi-sensor fusion, comprising: acquiring radar point cloud data and inertial navigation data respectively based on the time synchronization principle; dynamically removing points from the radar point cloud data and the inertial navigation data; performing static feature matching on the radar point cloud data and the inertial navigation data after dynamic point removal; constructing various residuals of tightly coupled optimization by combining the residuals after static feature matching; and determining the optimal extrinsic parameters by minimizing the various residuals.

[0007] In one implementation of the first aspect, the steps of acquiring radar point cloud data and inertial navigation data respectively based on the time synchronization principle include: performing time synchronization based on the timestamps of the radar point cloud data and the inertial navigation data, and acquiring the radar point cloud data and the inertial navigation data respectively; and correcting the motion distortion of the radar point cloud data and the inertial navigation data.

[0008] In one implementation of the first aspect, the step of dynamically removing points from the radar point cloud data and the inertial navigation data includes: performing point cloud prediction on the corrected inertial navigation data; the point cloud prediction refers to calculating the point cloud position of the next frame based on the point cloud position of the current frame of the inertial navigation data and the known vehicle speed and time interval of the next frame; performing nearest neighbor search and residual calculation by combining the corrected inertial navigation data and the point cloud prediction result to determine the calculation result; for the corrected inertial navigation data, marking the points in the current frame whose calculation results exceed a set threshold as dynamic points and removing them; and determining the set of static points after removing the dynamic points.

[0009] In one implementation of the first aspect, the step of performing static feature matching based on the radar point cloud data after dynamic point removal and the inertial navigation data includes: searching for neighboring points based on the radar point cloud data after dynamic point removal and the inertial navigation data to determine a neighborhood point set; calculating the neighborhood centroid based on the neighborhood point set; constructing a covariance matrix by combining the neighborhood centroid; performing eigenvalue decomposition on the covariance matrix; calculating curvature pairs using the decomposed eigenvalues; determining plane points and corner points based on the relationship between a set first threshold and a second threshold and the curvature pairs; and performing static feature matching on the same position of radar point cloud data and inertial navigation data from different frames using the plane points and the corner points; wherein the residual after static feature matching includes the approximation degree of the plane fitted by each plane point to each plane point and the approximation degree of the angle fitted by each corner point to each corner point.

[0010] In one implementation of the first aspect, the step of constructing the residuals of tightly coupled optimization by combining the residuals after static feature matching includes: defining the state variables of the inertial navigation data; constructing a tightly coupled factor using all the state variables to be optimized within a sliding window, wherein the tightly coupled factor includes an inertial navigation data pre-integration factor, a laser odometry factor, an offset random walk factor, and a degradation protection factor; and determining the residuals of the inertial navigation data pre-integration factor, the laser odometry factor, the offset random walk factor, and the degradation protection factor, respectively.

[0011] In one implementation of the first aspect, the step of constructing the laser odometry factor includes: constructing a local map based on the radar point cloud data using laser odometry; performing planar point feature matching and planar point residual construction, and corner point feature matching and corner point residual construction based on the constructed local map; and determining the laser odometry residual vector using the constructed planar point residual and corner point residual, wherein the laser odometry residual vector is the laser odometry factor.

[0012] In one implementation of the first aspect, the step of determining the optimal extrinsic parameters by minimizing the residuals includes: determining the overall optimization function based on the residuals; performing incremental optimization on the overall optimization function; and determining convergence based on convergence conditions. The convergence conditions include residual stability criteria, parameter stability criteria, and duration criteria. For optimization results that satisfy the convergence conditions, the average value within the sliding window is taken as the optimal extrinsic parameters.

[0013] Secondly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed, implements the method described thereon.

[0014] Thirdly, this application provides an electronic device, including: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory to cause the electronic device to perform the method described thereon.

[0015] Fourthly, this application provides an extrinsic parameter calibration system based on multi-sensor fusion, comprising: the electronic device described in 9, a radar, and an inertial measurement unit; both the radar and the inertial measurement unit are communicatively connected to the electronic device; the electronic device is configured to receive radar point cloud data collected by the radar and inertial navigation data collected by the inertial measurement unit based on a time synchronization principle; dynamically remove points from the radar point cloud data and the inertial navigation data; perform static feature matching based on the dynamically removed radar point cloud data and the inertial navigation data; construct tightly coupled optimized residuals by combining the residuals after static feature matching; and determine the optimal extrinsic parameters by minimizing the residuals.

[0016] As described above, the extrinsic parameter calibration method, medium, device, and system based on multi-sensor fusion described in this application have the following beneficial effects:

[0017] This application provides an online calibration method for LiDAR-IMU extrinsic parameters based on tightly coupled factor graph optimization, which can achieve high-precision calibration. Through tightly coupled optimization, it fully utilizes the geometric consistency between the raw LiDAR observations and the IMU pre-integration, significantly improving the accuracy of extrinsic parameter estimation. It enables online calibration without the need for a calibration board or static environment, and can continuously optimize extrinsic parameters during vehicle operation, adapting to installation changes during long-term use. The extrinsic parameter calibration method of this application has stronger robustness, introducing a degradation detection mechanism to avoid divergence in extrinsic parameter estimation under insufficient excitation scenarios. The extrinsic parameter calibration method of this application has strong versatility, applicable to various LiDAR (mechanical rotating, solid-state, Flash, etc.) and IMU combinations, and compatible with different brands and models. It has good real-time performance: this application adopts incremental factor graph optimization (such as iSAM2 or GTSAM), which can support real-time online operation. Attached Figure Description

[0018] Figure 1 The diagram shows an application scenario of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0019] Figure 2 The diagram shown is a flowchart illustrating the principle of the extrinsic parameter calibration method based on multi-sensor fusion as described in the embodiments of this application.

[0020] Figure 3 The diagram shows a data acquisition flowchart of the extrinsic parameter calibration method based on multi-sensor fusion as described in the embodiments of this application.

[0021] Figure 4 The flowchart shown is a dynamic point elimination process of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0022] Figure 5 The diagram shown is a static feature matching flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0023] Figure 6 The diagram shows the residual construction flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0024] Figure 7 The diagram shows the process of constructing laser odometry factors using the extrinsic parameter calibration method based on multi-sensor fusion as described in the embodiments of this application.

[0025] Figure 8 The flowchart shown is a flowchart of the extrinsic parameter determination method based on multi-sensor fusion described in the embodiments of this application.

[0026] Figure 9 The diagram shown is an overall flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0027] Figure 10 The image shown is an external parameter calibration effect diagram of the external parameter calibration method based on multi-sensor fusion described in the embodiments of this application.

[0028] Figure 11 The diagram shown is a structural connection diagram of the electronic device described in an embodiment of this application.

[0029] Figure 12 The diagram shown is a schematic representation of the external parameter calibration system based on multi-sensor fusion as described in an embodiment of this application.

[0030] Component designation explanation

[0031] 1 electronic devices 11 processor 12 memory 2 radar 3 Inertial Measurement Unit S21~S25 step S211~S212 step S221~S224 step S231~S237 step S241~S243 step S242A~S242C step S251~S254 step S91~S911 step Detailed Implementation

[0032] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. This application can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, unless otherwise specified, the following embodiments and features in the embodiments can be combined with each other.

[0033] It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of this application. Therefore, the drawings only show the components related to this application and are not drawn according to the actual number, shape and size of the components in the actual implementation. In the actual implementation, the form, quantity and proportion of each component can be arbitrarily changed, and the layout of the components may also be more complex.

[0034] The following embodiments of this application provide a method, medium, device and system for extrinsic parameter calibration based on multi-sensor fusion, including but not limited to automobiles with intelligent driving functions. The following description will take this application scenario as an example.

[0035] Please see Figure 1 The image shows a schematic diagram illustrating an application scenario of the extrinsic parameter calibration method based on multi-sensor fusion described in an embodiment of this application. Figure 1 As shown, this embodiment provides a car with intelligent driving function or assisted driving function, specifically including: a LiDAR and an IMU installed in the car. The LiDAR and IMU, as two sensors, need to achieve fusion positioning during the car driving process. Therefore, the external parameter calibration is performed by the main control electronic device in the car using the method of this application, realizing a highly robust calibration method that integrates dynamic obstacle removal, tight coupling factor graph optimization and joint update of external parameters.

[0036] Beyond applications in intelligent driving or assisted driving for automobiles, the technical solution of this application can also be applied to multi-sensor fusion localization of autonomous machines such as mobile robots. This application can achieve high-precision and robust extrinsic parameter estimation without calibration boards, GNSS, or static environment assumptions, and supports continuous online updates of extrinsic parameters, effectively addressing challenges such as dynamic obstacle interference and insufficient motion excitation.

[0037] The technical solutions in the embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0038] Please refer to Figure 2, which shows a flowchart illustrating the principle of the extrinsic parameter calibration method based on multi-sensor fusion as described in an embodiment of this application. Figure 2 As shown, this embodiment provides an extrinsic parameter calibration method based on multi-sensor fusion, which specifically includes the following steps:

[0039] S21, based on the principle of time synchronization, acquires radar point cloud data and inertial navigation data respectively.

[0040] Please see Figure 3 The diagram shows a data acquisition flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in an embodiment of this application. Figure 3 As shown, based on the principle of time synchronization, the steps for acquiring radar point cloud data and inertial navigation data respectively include:

[0041] S211, based on the timestamps of the radar point cloud data and the inertial navigation data, time synchronization is performed to acquire the radar point cloud data and the inertial navigation data respectively.

[0042] Specifically, LiDAR point cloud data and IMU inertial navigation data are acquired synchronously. The LiDAR data frequency is 10Hz, and the IMU data frequency is 200Hz-300Hz, for example, 200Hz. Time synchronization is performed on the data from the LiDAR and IMU sensors. In this embodiment, the timestamps of the point cloud data and inertial navigation data are used as the reference for time synchronization, and the LiDAR point cloud data is acquired separately. And the corresponding IMU inertial navigation data.

[0043] S212, perform motion distortion correction processing on the radar point cloud data and the inertial navigation data.

[0044] Specifically, since acquiring one frame of LiDAR data takes a certain amount of time (e.g., 100ms per frame at 10Hz), continuous sensor movement during this period can cause distortion in the point cloud. To obtain accurate calibration results, this motion distortion must be corrected. This embodiment uses an interpolation method based on IMU integration for distortion correction. The specific steps are as follows:

[0045] (1) Obtain the actual time of each laser point in the Lidar frame relative to that frame. timestamp offset For time Points collected Its coordinates were initially in Time-based lidar coordinate system One point in it.

[0046] (2) Using IMU in High-frequency angular velocity and acceleration measurements within the time range are used to calculate the IMU coordinate system from the IMU pre-integration. Time to data collection IMU relative pose transformation .

[0047] (3) Each (Radar coordinate system at the time of data acquisition) (Next point) transforms to the start time of this frame. IMU coordinate system at time Then, the corrected points can be obtained. This transformation is expressed by the following formula:

[0048]

[0049] in:

[0050] These are the points after correction. IMU coordinate system at time Down;

[0051] , Let be the extrinsic parameters to be calibrated, and represent the rotation and translation matrices from LiDAR to IMU, respectively.

[0052] , For the result obtained through IMU pre-integration from arrive The relative rotation and translation matrix of the IMU coordinate system at any given time.

[0053] S22, Dynamic point removal is performed on the radar point cloud data and the inertial navigation data. This step involves dynamic obstacle detection and removal. The extrinsic parameter calibration of LiDAR and IMU is based on external static point cloud features. Point cloud data of moving obstacles often affect the accuracy of calibration, so it is necessary to remove moving point clouds. This embodiment adopts a dynamic point detection method based on inter-frame motion consistency, which does not rely on global pose optimization.

[0054] Please see Figure 4 The diagram shows a flowchart of the dynamic point elimination process for the extrinsic parameter calibration method based on multi-sensor fusion described in this application embodiment. Figure 4 As shown, the steps for dynamically removing points from the radar point cloud data and the inertial navigation data include:

[0055] S221, perform point cloud prediction on the corrected inertial navigation data; the point cloud prediction refers to calculating the point cloud position of the next frame based on the point cloud position of the current frame of the inertial navigation data and the known vehicle speed and time interval of the next frame.

[0056] Specifically, the point cloud in the IMU coordinate system after distortion correction in the (k-1)th frame is denoted as... Each point will be used with the IMU pre-integration results Predict its position in the IMU coordinate system in the k-th frame, and obtain

[0057]

[0058] S222, combining the corrected inertial navigation data and point cloud prediction results, performs nearest neighbor search and residual calculation to determine the calculation results.

[0059] Specifically, in the k-th frame after correction In the middle, the k-th frame is predicted for the (k-1)-th frame. Find the nearest neighbor and calculate the Euclidean distance:

[0060]

[0061] S223, for the corrected inertial navigation data, the points in the current frame whose calculation results exceed the set threshold are marked as dynamic points and removed.

[0062] Specifically, the threshold is set as ,if Then mark For dynamic points; if a point is marked as a dynamic point in n consecutive frames, it is added to a blacklist and removed in advance.

[0063] S224, Determine the set of static points after removing dynamic points.

[0064] Specifically, the set of static points is represented as follows:

[0065]

[0066] S23, Static feature matching is performed based on the radar point cloud data after dynamic point removal and the inertial navigation data. This step involves static feature extraction and matching to obtain feature points in the point cloud data, including planar points and corner points.

[0067] Please see Figure 5 The diagram shows a static feature matching flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application. Figure 5 As shown, the steps for static feature matching based on the radar point cloud data after dynamic point removal and the inertial navigation data include:

[0068] S231, Search for neighboring points based on the radar point cloud data after dynamic point removal and the inertial navigation data to determine the neighborhood point set.

[0069] Specifically, for the current static point Search for neighboring points within a 0.3m radius (or other distance determined by practical application requirements) to obtain the neighborhood point set. .

[0070] S232, Calculate the neighborhood centroid based on the neighborhood point set.

[0071] Specifically, calculate the neighborhood centroid. as follows:

[0072]

[0073] S233, construct the covariance matrix by combining the neighborhood centroids.

[0074] Specifically, constructing the covariance matrix as follows:

[0075]

[0076] S234, Perform eigenvalue decomposition on the covariance matrix.

[0077] Specifically, for The eigenvalue decomposition is performed as follows:

[0078]

[0079] in, , , yes The three eigenvalues ​​satisfy .

[0080] S235, calculate curvature pairs using the eigenvalues ​​obtained from the decomposition.

[0081] Specifically, calculating the curvature pairs as follows:

[0082]

[0083] in, The smaller the value, the more likely it is a plane point; the larger the value, the more likely it is a corner point.

[0084] S236, determine the plane point and corner point according to the relationship between the set first threshold and second threshold and the curvature pair.

[0085] Specifically, given a first threshold and a second threshold Obtain the plane points and corner points, satisfying Plane point, corner point.

[0086] S237, using the plane points and the corner points, static feature matching is performed on the same location of radar point cloud data and inertial navigation data from different frames; wherein, the residual after static feature matching includes the approximation degree of the plane fitted by each plane point to each plane point and the approximation degree of the angle fitted by each corner point to each corner point.

[0087] Specifically, voxel grid filtering is applied to each feature point to avoid feature redundancy and improve matching efficiency.

[0088] Specifically, after obtaining the feature point cloud after static feature matching, taking a planar point as an example, the planar point corresponds to a planar point cloud. This planar point cloud can be fitted to a plane. Each point has a distance to the plane. If the error is small enough, the point is in the fitted plane, and the distance residual is 0. Subsequent optimization is based on this distance residual. When the system obtains the optimal solution, the distance residual can reach the minimum value. At this time, the output is the optimal extrinsic parameter.

[0089] S24. Combine the residuals after static feature matching to construct the residuals of each tightly coupled optimization.

[0090] Please see Figure 6 The diagram shows the residual construction flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application. Figure 6 As shown, the steps for constructing the residuals of tightly coupled optimization, combining the residuals after static feature matching, include:

[0091] S241, Define the state variables of the inertial navigation data.

[0092] Specifically, the tightly coupled factor graph constructed in this embodiment contains all the state variables to be optimized within the sliding window:

[0093]

[0094] The state variable at each time step is defined as follows:

[0095]

[0096] in, It is the pose from the IMU coordinate system to the world coordinate system at time k. Let k be the velocity of the IMU in the world coordinate system. This is the zero bias vector for the IMU accelerometer and gyroscope. The Lidar-to-IMU transformation matrix to be optimized for the system is shared parameter estimates throughout the entire sliding window.

[0097] S242, construct a tight coupling factor using all the state variables to be optimized within the sliding window. The tight coupling factor includes an inertial navigation data pre-integration factor, a laser odometry factor, an offset random walk factor, and a degradation protection factor.

[0098] First, the inertial navigation data IMU pre-integration factor is constructed as follows:

[0099] Within the time interval between adjacent LiDAR frames, the high-frequency IMU measurements are pre-integrated to construct the IMU dynamic constraint factor. The pre-integrated value is calculated as follows:

[0100]

[0101]

[0102]

[0103] in, Let the IMU pose change, velocity change, and position change be respectively from frame i to frame j. The IMU pre-integration residual can then be obtained as:

[0104]

[0105] Second, the laser odometry factor is constructed as follows: Please refer to [link / reference]. Figure 7 The diagram shows a flowchart of the laser odometry factor construction process based on the extrinsic parameter calibration method using multi-sensor fusion as described in an embodiment of this application. Figure 7 As shown, the steps for constructing the laser odometry factor, i.e., the residual steps after feature matching based on laser odometry, include:

[0106] S242A, constructs a local map based on the radar point cloud data using a laser odometry system.

[0107] Specifically, this embodiment uses a sliding window to construct and maintain a local map. The window size is N frames (taken as 5). When a new frame is added, the oldest frame is removed, keeping the window size unchanged.

[0108] (1) Collect the static planar points and corner points of each frame within the sliding window, and transform all points to the IMU coordinate system of the initial frame of the local window. This coordinate system is defined as the world coordinate system, specifically as follows:

[0109]

[0110] in, Let I represent the pose of the IMU in the i-th frame from the initial value of the sliding window in the IMU coordinate system (world coordinate system). The external parameters to be optimized Let be the feature points of the LiDAR in the i-th frame.

[0111] (2) All feature points within the sliding window are merged into a local map.

[0112]

[0113] (3) Perform sampling processing on the sample. Voxel grid filtering is performed to further reduce redundancy and ensure uniform map density.

[0114] S242B performs planar point feature matching and planar point residual construction, and corner point feature matching and corner point residual construction based on the constructed local map.

[0115] Specifically, planar point feature matching and residual construction include:

[0116] (1) Planar feature matching

[0117] For the planar points of the current frame Based on the extrinsic parameters and pose, a point in the world coordinate system is obtained. In the world coordinate system, with Search the local map within a radius of 0.3 centered on the target area. The nearest neighbor points in the data, when the number of nearest neighbor points exceeds a threshold. At that time, PCA is used to fit a local plane in the world coordinate system.

[0118]

[0119] in, Let be the covariance matrix of the fitted plane. yes The j-th matched point, where q is the centroid of all matched points.

[0120] (2) Calculate the normal vector

[0121] For covariance matrix Perform eigenvalue decomposition, and take the eigenvector corresponding to the smallest eigenvalue as the normal vector of the plane, denoted as vector n.

[0122] (c) Calculate the residual from the point to the surface.

[0123]

[0124] Where q is any point on the plane.

[0125] Specifically, corner feature matching and residual construction include:

[0126] (1) Corner feature matching

[0127] For the corner points of the current frame Based on the extrinsic parameters and pose, a point in the world coordinate system is obtained. In the world coordinate system, with Search the local map within a radius of 0.3 centered on the target area. The nearest neighbor points in the data, when the number of nearest neighbor points exceeds a threshold. At that time, PCA is used to fit local line segments in the world coordinate system.

[0128]

[0129] in, Let be the covariance matrix of the fitted line segment. yes The j-th matched point, where q is the centroid of all matched points.

[0130] (2) Calculate the normal vector

[0131] For covariance matrix Perform eigenvalue decomposition to obtain Three eigenvalues ​​and eigenvectors, The corresponding eigenvector is used as the direction vector of the line segment, denoted as . Will Projected onto direction vector Up, get a point on the line segment.

[0132] (3) Calculate the residual from the corner point to the line segment.

[0133]

[0134] Where v is the direction vector of the line segment. for The point projected onto the line segment. It is an identity matrix.

[0135] S242C, using the constructed planar point residuals and corner point residuals to determine the laser odometer residual vector, the laser odometer residual vector being the laser odometer factor.

[0136] Specifically, the residuals of all matches in the current frame are summarized into a laser odometry residual vector, denoted as:

[0137]

[0138] in, and These represent the number of plane points and corner points matched in the local map, respectively.

[0139] S243, determine the residuals of the inertial navigation data pre-integration factor, the laser odometry factor, the bias random walk factor, and the degradation protection factor, respectively.

[0140] Third, the bias random walk factor is constructed as follows:

[0141] The characteristic of the IMU sensor's zero bias changing slowly over time is constrained by a random walk factor.

[0142] Let the IMU zero bias at time k be:

[0143]

[0144] in, Zero bias (angular velocity bias) for the IMU gyroscope and zero bias (accelerometer bias) for the accelerometer. It is Gaussian white noise, and the residual of the bias random walk factor is defined as:

[0145]

[0146] in This represents the pose in the IMU coordinate system at time k. For zero bias of the IMU accelerometer and gyroscope, For the external parameters to be calibrated, Let be the velocity at time i.

[0147] For the IMU pre-integration factor, the IMU data is pre-integrated between two LiDAR frames (e.g., from k to k+1) to obtain the relative motion increment:

[0148]

[0149] Constructing residuals:

[0150]

[0151] There are two specific LiDAR odometry factors: the residual after point cloud feature matching.

[0152]

[0153]

[0154] Fourth, the degradation protection factor is constructed as follows:

[0155] When the vehicle is in a degenerate scenario, such as traveling in a straight line or at a constant speed, optimizing and updating the extrinsic parameters can lead to problems such as extrinsic parameter estimation drift. Therefore, a degradation protection factor is introduced, which is activated only in degenerate scenarios.

[0156] (1) Degradation scenario judgment

[0157] Angular velocity change rate detection:

[0158]

[0159] in, The mean angular velocity within the sliding window is N, where N is the window size.

[0160] Acceleration rate of change detection:

[0161]

[0162] in, The average acceleration within the sliding window

[0163] Degradation scenario determination:

[0164]

[0165] (2) The residual of the degradation protection factor is:

[0166]

[0167] in This is the Lidar-to-IMU translation vector before entering the degradation scene. This is the current LiDAR-to-IMU translation vector. The Lidar-to-IMU rotation matrix before entering the degradation scenario. This is the current LiDAR to IMU rotation matrix. This is the weighting factor, typically set to 0.3.

[0168] S25, determine the optimal extrinsic parameters by minimizing the residuals of each term. This step involves joint optimization of the extrinsic parameters. In this embodiment, the multi-sensor factors are jointly constructed as a nonlinear least squares problem, and the optimal extrinsic parameter estimate is obtained by minimizing the sum of all residual terms.

[0169] Please see Figure 8 The diagram shows the extrinsic parameter determination flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application. Figure 8 As shown, the steps to determine the optimal extrinsic parameters by minimizing the residuals of each term include:

[0170] S251, determine the overall optimization function based on the residuals.

[0171] Specifically, the overall optimization function is as follows:

[0172]

[0173] in, The weighting coefficients for each factor.

[0174] S252, perform incremental optimization to solve the overall optimization function.

[0175] Specifically, the incremental optimization solution based on iSAM2 is as follows:

[0176] iSAM2 (incremental smoothing and graph building algorithm) achieves efficient incremental optimization through a Bayesian tree data structure. Its core advantage lies in:

[0177] Incremental calculation mechanism: 1. When new data arrives, only the affected part is updated to avoid global recalculation; 2. By utilizing the sparsity of factor graphs, the computational complexity is reduced from O(n³) to O(log n); 3. Based on Bayesian tree inference, fast variable elimination and state estimation are achieved.

[0178] S253, convergence judgment is made in combination with convergence conditions; wherein, the convergence conditions include residual stability criterion, parameter stability criterion and duration criterion.

[0179] Specifically, the convergence of external parameter joint update optimization can be determined by the following three criteria or convergence conditions:

[0180] (1) Residual stability criterion:

[0181]

[0182] (2) Parameter stability standard:

[0183]

[0184] (3) Duration standard: The stable state lasts for more than N frames.

[0185] S254, for the optimization results that satisfy the convergence condition, take the average value within the sliding window as the optimal extrinsic parameter.

[0186] Specifically, once the convergence condition is met, the optimization result is taken as the average value within the sliding window as the output:

[0187]

[0188] Please see Figure 9 The diagram shows the overall flowchart of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application. Figure 9 As shown, the overall process for extrinsic parameter calibration in this embodiment includes the following steps:

[0189] S91, data acquisition for LiDAR and IMU.

[0190] The S92's LiDAR point cloud data and IMU inertial navigation data are synchronized in time.

[0191] S93 performs distortion correction processing on the radar point cloud data of LiDAR.

[0192] S94, combined with the distortion-reduced radar point cloud data, performs dynamic point removal.

[0193] S95 performs feature extraction and matching on the data after dynamic point removal.

[0194] S96, construct the tight coupling factor graph.

[0195] S97 optimizes the solution for each residual.

[0196] S98, determine whether the degradation scene has been entered. If so, return to step S93.

[0197] S99, if not, update external parameters.

[0198] S910, determine whether convergence has occurred; if not, return to step S97.

[0199] If it is S911, complete the external parameter calibration.

[0200] Please see Figure 10 The image displayed shows the extrinsic parameter calibration effect of the extrinsic parameter calibration method based on multi-sensor fusion described in the embodiments of this application. Figure 10 The diagram illustrates the effect of extrinsic parameter calibration in a specific embodiment, which includes:

[0201] (1) Hardware configuration includes:

[0202] LiDAR: RS-Ruby-Plus (10Hz, 128 lines);

[0203] IMU: Xsens MTi-680G (200Hz, six-axis, high precision);

[0204] Main control unit:

[0205] CPU (Central Processing Unit), 24 cores and 48 threads, single-core clock speed 3.5GHz;

[0206] GPU (Graphics Processing Unit), NVIDIA RTX3090;

[0207] RAM (Random Access Memory), 128G DDR4;

[0208] ROM (Read-Only Memory), 8TB SSD;

[0209] Time synchronization: Hardware PPS+PTP synchronization, time alignment error < 1ms.

[0210] (2) Data collection

[0211] The data acquisition LiDAR frame rate is 10Hz, the IMU frequency is 200Hz, and it operates on urban roads in an environment that includes static buildings, roads, guardrails, etc.

[0212] (3) State initialization

[0213] External reference Through rough calibration using mechanical measurements, there is a translational error of approximately 1-5 cm and a rotational deviation of 0.8°.

[0214] The initial value of the zero bias is set to zero.

[0215] (4) Run the calibration process

[0216] a. Synchronization and distortion correction:

[0217] LiDAR and IMU synchronized alignment for distortion correction; current frame point cloud After distortion removal, the result is Points are then unified to the coordinate system of the initial frame pose to obtain...

[0218] b. Dynamic obstacle removal

[0219] Predict the position of the next frame for the points that have been distorted. Search for the nearest neighbor within the neighborhood. If the distance between the two is greater than a threshold (0.2), it is marked as a dynamic point removal, resulting in the final result.

[0220] c. Feature extraction and matching

[0221] For a local static point cloud, extract planar points and edge corner points to construct residuals.

[0222] d. Constructing a factor graph

[0223] This mainly includes: IMU pre-integration factor ( connect ), LiDAR eigenvalues ​​( ),

[0224] Degeneration protection factor ( ) and bias random walk factor ( During the optimization process, the weight of each factor can be adjusted.

[0225] It should be noted that the above embodiments of this application use IMU prediction and nearest neighbor matching motion consistency detection, and the threshold filtering method is used. It can be replaced by: (1) Deep learning detection method: Deep learning method can be used to directly detect the point cloud and determine whether it is a dynamic point (2) Multi-frame ICP + clustering method: Multi-frame point cloud registration is used to calculate the point motion vector, and DBSCAN clustering is used to separate static and dynamic. In the above embodiments of this application, the dynamic point deletion after distortion removal can also be optimized to first perform coarse distortion removal, then perform dynamic point deletion, and then perform fine distortion removal. Among them, coarse distortion removal can be achieved by the data processing mechanism of the lidar and inertial measurement unit sensor itself, and fine distortion removal is performed by interpolation based on IMU integration.

[0226] Please see Figure 11 The diagram shows the structural connections of the electronic device described in the embodiments of this application. Figure 11 As shown, the electronic device 1 of this application includes: a processor 11 and a memory 12; the memory 12 is used to store a computer program, and the processor 11 is used to execute the computer program stored in the memory, so that the electronic device 1 performs each step of the external parameter calibration method based on multi-sensor fusion.

[0227] The processor 11 mentioned above can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0228] The aforementioned memory 12 may include random access memory (RAM) and may also include non-volatile memory, such as at least one disk storage device.

[0229] This application also provides a computer-readable storage medium storing a computer program thereon, which, when executed, implements the described extrinsic parameter calibration method based on multi-sensor fusion.

[0230] Those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by a program instructing a processor. The program can be stored in a computer-readable storage medium, which is a non-transitory medium, such as random access memory, read-only memory, flash memory, hard disk, solid-state drive, magnetic tape, floppy disk, optical disk, and any combination thereof. The storage medium can be any available medium accessible to a computer or a data storage device such as a server or data center that integrates one or more available media. This available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., digital video disc (DVD)), or a semiconductor medium (e.g., solid-state drive (SSD)).

[0231] Please see Figure 12 The diagram shows the structural principle of the extrinsic parameter calibration system based on multi-sensor fusion described in an embodiment of this application. Figure 12As shown, this embodiment provides an external parameter calibration system based on multi-sensor fusion, including: the electronic device 1 mentioned in 9, as well as radar 2 and inertial measurement unit 3; both radar 2 and inertial measurement unit 3 are communicatively connected to the electronic device 1.

[0232] The electronic device 1 is configured to receive radar point cloud data collected by the radar 2 and inertial navigation data collected by the inertial measurement unit 3 based on the time synchronization principle; to perform dynamic point removal on the radar point cloud data and the inertial navigation data; to perform static feature matching on the radar point cloud data and the inertial navigation data after dynamic point removal; to construct various residuals of tightly coupled optimization by combining the residuals after static feature matching; and to determine the optimal extrinsic parameters by minimizing the various residuals.

[0233] The scope of protection of the extrinsic parameter calibration method based on multi-sensor fusion described in this application is not limited to the execution order of the steps listed in this embodiment. Any solution implemented by adding, subtracting, or replacing steps in the prior art based on the principles of this application is included within the scope of protection of this application.

[0234] The extrinsic parameter calibration system based on multi-sensor fusion described in this application can implement the extrinsic parameter calibration method based on multi-sensor fusion described in this application. However, the implementation device of the extrinsic parameter calibration method based on multi-sensor fusion described in this application includes, but is not limited to, the structure of the extrinsic parameter calibration system based on multi-sensor fusion listed in this embodiment. All structural modifications and substitutions of the prior art made in accordance with the principles of this application are included within the protection scope of this application.

[0235] In the embodiments provided in this application, it should be understood that the disclosed systems or methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For instance, the division of modules / units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or units may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection through some interfaces, devices, or modules or units, and may be electrical, mechanical, or other forms.

[0236] The modules / units described as separate components may or may not be physically separate. The components shown as modules / units may or may not be physical modules; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules / units can be selected to achieve the objectives of the embodiments of this application, depending on actual needs. For example, the functional modules / units in the various embodiments of this application may be integrated into one processing module, or each module / unit may exist physically separately, or two or more modules / units may be integrated into one module / unit.

[0237] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0238] The descriptions of the processes or structures corresponding to the above figures each have their own emphasis. For parts of a process or structure that are not described in detail, please refer to the relevant descriptions of other processes or structures.

[0239] The above embodiments are merely illustrative of the principles and effects of this application and are not intended to limit this application. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of this application. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in this application should still be covered by the claims of this application.

Claims

1. A method for extrinsic parameter calibration based on multi-sensor fusion, characterized in that, include: Based on the principle of time synchronization, radar point cloud data and inertial navigation data are acquired separately. Dynamic point removal is performed on the radar point cloud data and the inertial navigation data; Static feature matching is performed based on the radar point cloud data after dynamic point removal and the inertial navigation data. By combining the residuals after static feature matching, we construct various residuals for tightly coupled optimization. The optimal external parameters are determined by minimizing the residuals of each term.

2. The method according to claim 1, characterized in that, Based on the principle of time synchronization, the steps for acquiring radar point cloud data and inertial navigation data separately include: Based on the timestamps of the radar point cloud data and the inertial navigation data, time synchronization is performed to acquire the radar point cloud data and the inertial navigation data respectively. Motion distortion of the radar point cloud data and the inertial navigation data is corrected.

3. The method according to claim 2, characterized in that, The steps for dynamically removing points from the radar point cloud data and the inertial navigation data include: Point cloud prediction is performed on the corrected inertial navigation data; the point cloud prediction refers to calculating the point cloud position of the next frame based on the point cloud position of the current frame of the inertial navigation data and the known vehicle speed and time interval of the next frame. The nearest neighbor search and residual calculation are performed by combining the corrected inertial navigation data and point cloud prediction results to determine the calculation results; For the corrected inertial navigation data, points in the current frame whose calculation results exceed a set threshold are marked as dynamic points and removed; Determine the set of static points after removing dynamic points.

4. The method according to claim 1, characterized in that, The steps for static feature matching based on the radar point cloud data after dynamic point removal and the inertial navigation data include: Based on the radar point cloud data after dynamic point removal and the inertial navigation data, search for neighboring points to determine the neighborhood point set; Calculate the neighborhood centroid based on the neighborhood point set; Construct the covariance matrix by combining the neighborhood centroids; Perform eigenvalue decomposition on the covariance matrix; Calculate curvature pairs using the eigenvalues ​​obtained from the decomposition; Based on the relationship between the set first and second thresholds and the curvature pairs, plane points and corner points are determined; Static feature matching is performed on the same location in radar point cloud data and inertial navigation data from different frames using the plane points and the corner points; wherein, the residual after static feature matching includes the approximation degree of the plane fitted by each plane point to each plane point and the approximation degree of the angle fitted by each corner point to each corner point.

5. The method according to claim 1, characterized in that, The steps for constructing the residuals of tightly coupled optimization by combining the residuals after static feature matching include: Define the state variables of the inertial navigation data; A tight coupling factor is constructed using all the state variables to be optimized within a sliding window. The tight coupling factor includes an inertial navigation data pre-integration factor, a laser odometry factor, an offset random walk factor, and a degradation protection factor. The residuals of the inertial navigation data pre-integration factor, the laser odometry factor, the bias random walk factor, and the degradation protection factor are determined respectively.

6. The method according to claim 5, characterized in that, The steps for constructing the laser odometry factor include: Local map construction is performed on the radar point cloud data based on laser odometry; Based on the constructed local map, perform planar point feature matching and planar point residual construction, and corner point feature matching and corner point residual construction respectively; The laser odometer residual vector is determined by using the constructed planar point residual and corner point residual, and the laser odometer residual vector is the laser odometer factor.

7. The method according to claim 1, characterized in that, The steps to determine the optimal extrinsic parameters by minimizing the residuals of each term include: The overall optimization function is determined based on the various residuals; Incremental optimization is performed on the overall optimization function; Convergence is determined by combining convergence criteria; wherein, the convergence criteria include residual stability criteria, parameter stability criteria, and duration criteria; For the optimization results that satisfy the convergence condition, the average value within the sliding window is taken as the optimal extrinsic parameter.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed, it implements the method described in any one of claims 1 to 7.

9. An electronic device, characterized in that, include: Processor and memory; The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory to cause the electronic device to perform the method as described in any one of claims 1 to 7.

10. An extrinsic parameter calibration system based on multi-sensor fusion, characterized in that, include: The electronic device of claim 9, as well as the radar and the inertial measurement unit; both the radar and the inertial measurement unit are communicatively connected to the electronic device; The electronic device is configured to receive radar point cloud data collected by the radar and inertial navigation data collected by the inertial measurement unit based on the time synchronization principle; to perform dynamic point removal on the radar point cloud data and the inertial navigation data; to perform static feature matching on the radar point cloud data and the inertial navigation data after dynamic point removal; to construct various residuals of tightly coupled optimization by combining the residuals after static feature matching; and to determine the optimal extrinsic parameters by minimizing the various residuals.

Images