A motion observability quantification method for lidar-insulator calibration

Abstract

A motion observability quantification method for LiDAR-Inertial Navigation (LIDAR) calibration relates to the field of LiDAR-IMU data fusion technology. Data is collected and the poses of the IMU and LiDAR are calculated. Within the pose sequence, pose pairs at the same acquisition time are searched for to construct an extrinsic calibration problem. The rotation matrix between the LiDAR and IMU coordinate systems is solved using an optimized approach by considering multiple sets of measurements. All calibration data is divided into segments of equal length. An information matrix is ​​constructed for each segment based on the calibration problem. Singular value decomposition is performed on the information matrix, and the information index η of each segment is calculated. The minimum singular value is used to represent the information metric of a single segment. A threshold η0 is set, and segments with η ≥ η0 are selected for extrinsic calibration. This method reduces the interference of invalid and erroneous information, improving the convergence and robustness of the calibration algorithm while reducing computational cost.

Description

Technical Field

[0001] The present invention relates to the technical field of LiDAR-IMU data fusion, and in particular to a motion observability quantification method for LiDAR-Inertial Navigation (IMU) calibration. Background Art

[0002] In recent years, the fusion of Light Detection and Ranging (LiDAR) and Inertial Measurement Unit (IMU) has been widely used as a navigation solution for robots. The external parameter calibration of LiDAR and IMU is a prerequisite for data fusion, and to a certain extent directly determines the fusion performance of LiDAR and IMU.

[0003] Currently, most LiDAR-IMU extrinsic calibration methods fail to fully consider whether the collected calibration data contains sufficient motion information to ensure a uniquely solvable calibration problem. Since not all data segments contain sufficient motion excitation during the calibration data collection process, the calibration problem cannot be stably solved on these invalid data segments and is easily corrupted by erroneous information, resulting in a decrease in the accuracy of the final extrinsic calibration. Therefore, blindly using all collected data for calibration can severely degrade the performance of the calibration algorithm.

[0004] In summary, how to find data segments with sufficient motion excitation from a large amount of data while reducing the interference of invalid and erroneous information is of great significance to the algorithm accuracy and computational cost control of LiDAR-IMU data fusion. Summary of the Invention

[0005] To address the shortcomings of the background technology, the present invention provides a motion observability quantification method for lidar-inertial navigation calibration, which can reduce the interference of invalid and erroneous information, improve the convergence and robustness of the calibration algorithm, and reduce the computational cost.

[0006] To achieve the above object, the present invention adopts the following technical solution: a motion observability quantification method for lidar-inertial navigation calibration, comprising the following steps:

[0007] Step 1: Collect calibration data, including IMU data M I,i , i=1,...,N I and LiDAR data M L,j ,j=1,...,N L , where i is the acquisition time of IMU data, N I is the end time of IMU data collection, j is the collection time of LiDAR data, N LThe end time of LiDAR data collection;

[0008] Step 2: Through M I,i and M L,j Calculate the attitude of IMU and LiDAR. The attitude of IMU at time i is R I,i , the posture of LiDAR at time j is R L,j ;

[0009] Step 3: Find the pose pair of IMU and LiDAR at the same acquisition time in the pose sequence: R I,k and R L,k , where k∈(i∩j);

[0010] Step 4: Construct the external parameter calibration problem as follows:

[0011]

[0012] Where, Represents the rotation matrix between the LiDAR and IMU coordinate systems, which is the variable to be solved. is the relative attitude change of the IMU between time k-1 and time k, is the relative attitude change of the LiDAR between time k-1 and time k, which is calculated by the following formula:

[0013]

[0014] Where R I,k-1 and R L,k-1 Represent the postures of IMU and LiDAR at time k-1 respectively;

[0015] Step 5: The external parameter calibration problem is solved by considering multiple sets of measurements and using an optimized approach as follows:

[0016]

[0017] Where, express The corresponding right-multiplied quaternion matrix is, express The corresponding left-multiplied quaternion matrix;

[0018] Step 6: Divide all calibration data into several segments of equal length, and construct the information matrix Ξ of each segment according to the calibration problem in step 5 as follows:

[0019]

[0020] The singular value decomposition of the information matrix Ξ is as follows:

[0021] Ξ=VΣVT (5)

[0022] Where Σ = diag(σ1,σ2,σ3,σ4) is a diagonal matrix of singular values ​​arranged in descending order, and V = [u1,u2,u3,u4] is an orthogonal matrix;

[0023] Step 7: Calculate the information index η of each segment and use the minimum singular value to represent the information metric of a single segment, which is expressed as follows:

[0024] η=min(σ1,σ2,σ3,σ4) (6)

[0025] Step 8: Set the threshold η0 of the information index and select only the segments with η≥η0 for external parameter calibration.

[0026] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention constructs a motion information matrix by modeling the calibration problem, and defines an information index to quantify the amount of motion information contained in each segment. During calibration, only data segments containing sufficient motion excitations are selected for external parameter calibration, thereby reducing the interference of invalid information and erroneous information, and reducing the computational cost while improving the convergence and robustness of the calibration algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is the technical roadmap of the method of the present invention;

[0028] Figure 2 is an information index graph of each fragment in a specific implementation manner;

[0029] Figure 3 It is the information index and motion incentive diagram in a specific implementation. DETAILED DESCRIPTION

[0030] The technical solutions of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0031] like Figures 1 to 3 As shown, a motion observability quantification method for lidar-insulator calibration is combined with the technical route Figure 1 As shown, the following steps are included:

[0032] Step 1: Collect calibration data, including IMU data M I,i , i=1,...,N I and LiDAR data M L,j ,j=1,...,NL , where i is the acquisition time of IMU data, N I is the end time of IMU data collection, j is the collection time of LiDAR data, N L The end time of LiDAR data collection;

[0033] Step 2: Through M I,i and M L,j Calculate the attitude of IMU and LiDAR. The attitude of IMU at time i is R I,i , the posture of LiDAR at time j is R L,j ;

[0034] Step 3: Find the pose pair of IMU and LiDAR at the same acquisition time in the pose sequence: R I,k and R L,k , where k∈(i∩j);

[0035] Step 4: Construct the external parameter calibration problem as follows:

[0036]

[0037] Where, Represents the rotation matrix between the LiDAR and IMU coordinate systems, which is the variable to be solved. is the relative attitude change of the IMU between time k-1 and time k, is the relative attitude change of the LiDAR between time k-1 and time k, which is calculated by the following formula:

[0038]

[0039] Where R I,k-1 and R L,k-1 Represent the postures of IMU and LiDAR at time k-1 respectively;

[0040] Step 5: The external parameter calibration problem is solved by considering multiple sets of measurements and using an optimized approach as follows:

[0041]

[0042] Where, express The corresponding right-multiplied quaternion matrix is, express The corresponding left-multiplied quaternion matrix;

[0043] Step 6: Divide all calibration data into several segments of equal length. In one embodiment, the calibration data is divided into several segments of 5 seconds in length. Based on the calibration problem in step 5, the information matrix Ξ of each segment is constructed as follows:

[0044]

[0045] The singular value decomposition of the information matrix Ξ is as follows:

[0046] Ξ=VΣV T (5)

[0047] Where Σ = diag(σ1,σ2,σ3,σ4) is a diagonal matrix of singular values ​​arranged in descending order, and V = [u1,u2,u3,u4] is an orthogonal matrix;

[0048] Step 7: Calculate the information index η of each segment. In one embodiment, the information index of each segment is combined Figure 2 As shown, the minimum singular value is used to represent the information measure of a single fragment, which is expressed as follows:

[0049] η=min(σ1,σ2,σ3,σ4) (6)

[0050] Step 8: Set the threshold η0 of the information index. In one embodiment, η0 = 0.005. When η < η0, it is considered that the motion excitation contained in the segment is insufficient to obtain the solution of formula (3). The segment is discarded and only the segments with η ≥ η0 are selected for external parameter calibration.

[0051] In one embodiment, information index and sports incentive are combined Figure 3 As shown in the figure, the method proposed in the present invention accurately quantifies the motion excitation contained in each segment of data. For segments with rich motion excitation, such as 75s~90s and 100s~105s, the information index is significantly larger. These segments contain rich motion excitation and will be used to calibrate the external parameters of LiDAR-IMU. For example, segments 40 to 45 seconds contain fewer motion excitations and have a smaller corresponding information index. Therefore, these segments are discarded during calibration to improve parameter convergence and reduce computational costs.

[0052] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other configurations without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations coming within the meaning and range of equivalents of the claims are intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

[0053] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A motion observability quantification method for lidar-inertial navigation calibration, characterized by: The following steps are involved: Step 1: Collect calibration data, including IMU data M I,i , i=1,...,N I and LiDAR data M L,j ,j=1,...,N L , where i is the acquisition time of IMU data, N I is the end time of IMU data collection, j is the collection time of LiDAR data, N L The end time of LiDAR data collection; Step 2: Through M I,i and M L,j Calculate the attitude of IMU and LiDAR. The attitude of IMU at time i is R I,i , the posture of LiDAR at time j is R L,j ; Step 3: Find the pose pair of IMU and LiDAR at the same acquisition time in the pose sequence: R I,k and R L,k , where k∈(i∩j); Step 4: Construct the external parameter calibration problem as follows: Where, Represents the rotation matrix between the LiDAR and IMU coordinate systems, which is the variable to be solved. is the relative attitude change of the IMU between time k-1 and time k, is the relative attitude change of the LiDAR between time k-1 and time k, which is calculated by the following formula: Where R I,k-1 and R L,k-1 Represent the postures of IMU and LiDAR at time k-1 respectively; Step 5: The external parameter calibration problem is solved by considering multiple sets of measurements and using an optimized approach as follows: Where, express The corresponding right-multiplied quaternion matrix is, express The corresponding left-multiplied quaternion matrix; Step 6: Divide all calibration data into several segments of equal length, and construct the information matrix Ξ of each segment according to the calibration problem in step 5 as follows: The singular value decomposition of the information matrix Ξ is as follows: Ξ=VΣV T (5) Where Σ = diag(σ1,σ2,σ3,σ4) is a diagonal matrix of singular values ​​arranged in descending order, and V = [u1,u2,u3,u4] is an orthogonal matrix; Step 7: Calculate the information index η of each segment and use the minimum singular value to represent the information metric of a single segment, which is expressed as follows: η=min(σ1,σ2,σ3,σ4) (6) Step 8: Set the threshold η0 of the information index and select only the segments with η≥η0 for external parameter calibration.

Images