Calibration method and system for inertial navigation and lidar of autonomous driving vehicles

Abstract

The present application discloses a calibration method and calibration system for inertial navigation and laser radar of autonomous driving vehicles, including: a laser radar odometer module, used to obtain the data of the laser radar odometer, and output the translation distance between two adjacent frames; an inertial navigation odometer module, used to obtain the data of the inertial navigation odometer, and output the translation distance between two adjacent frames; an external parameter optimization module, which obtains the translation distance, removes abnormal data and performs nonlinear optimization using the interior point method, and outputs the obtained optimization parameters as the translation and rotation external parameters between the laser radar and the inertial navigation device. In the embodiment of the present application, distance filtering is performed on the laser radar odometer and the inertial navigation odometer to filter out the bad positioning points caused by the signal interference of the inertial navigation device and the bad positioning points of the laser radar odometer caused by the movement of dynamic objects in the field, so as to improve the accuracy of the translation amount of the calibration result.

Description

Technical Field

[0001] The present application relates to the field of unmanned driving calibration technology, and in particular to a calibration method and a calibration system for an inertial navigation system and a laser radar of an autonomous driving vehicle. Background Art

[0002] Calibration of external parameters and spatial synchronization between sensors, that is, the conversion of measurements in different sensor coordinate systems to the same coordinate system. Sensor calibration is a basic requirement for autonomous driving and the basis for multi-sensor fusion. It is necessary to first determine the coordinate relationship between each sensor and the conversion relationship between each sensor and the vehicle coordinate system.

[0003] At present, the calibration methods for the lidar and inertial navigation equipment of autonomous driving vehicles include the hand-eye calibration method and the descriptor extraction method. The hand-eye calibration method uses the basic characteristics of the rigid connection between the lidar and the inertial navigation equipment in the vehicle, and extracts the position and posture of the lidar and the inertial navigation equipment at the same time to calculate the external parameter relationship between the vehicle lidar and the inertial navigation equipment. However, the two devices will produce two unavoidable errors. One is the positioning error caused by the interference of the site signal on the inertial navigation equipment, and the other is the mapping positioning error caused by the laser distortion caused by the dynamic objects and vehicle movement on the site on the lidar odometer.

[0004] Because of the existence of these two errors, the odometers generated by these two devices produce errors, and ultimately produce a large translation error in the calibration result. Usually we directly use the currently generated positioning data, or directly compare the x, y, and z three-dimensional differences of the current positioning point for filtering. The former will include a large number of error values, thereby affecting the final calibration accuracy. The latter cannot be directly filtered in the three-dimensional coordinate system in the case of large rotations because the initial orientation of the sensor is different. Therefore, the author proposes an algorithm flow to filter a large number of error values ​​at the same latitude spacing, thereby improving the calibration accuracy of lidar and inertial navigation. Summary of the invention

[0005] In order to solve the above technical problems, the embodiments of the present application provide a calibration method and a calibration system for the inertial navigation and lidar of autonomous driving vehicles, which improves and optimizes the calibration accuracy of the external parameters of the inertial navigation equipment and lidar of autonomous driving vehicles by eliminating the algorithm process of abnormal spacing points.

[0006] A first aspect of an embodiment of the present application provides a calibration method for an inertial navigation system and a laser radar of an autonomous driving vehicle, comprising:

[0007] S1) obtaining data of a laser radar odometer and an inertial navigation odometer, with coordinate point position and corresponding time information as the composition of each set of data;

[0008] S2) after normalizing the coordinate point pose in each set of data, obtaining the translation and rotation transformation amount of each coordinate point pose relative to the initial coordinate point pose;

[0009] S3) Based on the point cloud timestamp formed by the time information of the laser radar odometer, a one-dimensional interpolation operation is performed on the inertial navigation odometer coordinate point pose at each time information to obtain the inertial navigation odometer coordinate result at each point cloud timestamp, and spherical interpolation is used for the rotation azimuth to obtain the inertial navigation odometer rotation azimuth result at each point cloud timestamp. The odometer coordinates and the rotation azimuth form new inertial navigation odometer data;

[0010] S4) based on the data of the laser radar odometer and the data of the new inertial navigation odometer, respectively obtaining the rotation and translation transformation relationship between two adjacent frames and then obtaining the translation distance;

[0011] S5) The translation distances between the laser radar odometer and the new inertial navigation odometer in each group of adjacent frames are screened, and after removing abnormal points, nonlinear optimization is performed using the interior point method, and the obtained optimization parameters are used as the translation and rotation external parameters between the laser radar and the inertial navigation device.

[0012] Furthermore, in said S1), the coordinate point posture includes position information and rotation angle information, the position information is formed by the three-dimensional coordinate point relative to the origin, and the rotation angle information is formed by the orientation transformation relative to the previous frame.

[0013] Furthermore, the time information in the laser radar odometer data in S1) does not correspond one-to-one with the time information of the inertial navigation odometer.

[0014] Furthermore, the normalization of the coordinate point positions in S2) is for each initial coordinate point.

[0015] Furthermore, in S3), the time information in the new inertial navigation odometer data after one-dimensional interpolation and spherical interpolation operations forms a one-to-one correspondence with the time information in the laser radar odometer data.

[0016] Furthermore, in S4), every two adjacent frames are regarded as a rotation-translation transformation relationship, and the rotation-translation transformation relationship is used to perform distance calculation on the coordinate values ​​of the coordinate point postures in the two adjacent frames to obtain the translation distance; all the rotation-translation transformation relationships form a transformation relationship group, which sequentially contains all the translation distances.

[0017] Furthermore, if the error of the translation distance data with the same serial number in the transformation relationship group of the laser radar odometer in S5) and the transformation relationship group of the new inertial navigation odometer exceeds a preset threshold, the data is identified as an abnormal point and is removed from the two transformation relationship groups respectively.

[0018] Furthermore, the two new transformation relationship groups formed after eliminating all abnormal points in S5) are optimized using the interior point method.

[0019] A second aspect of the embodiment of the present application provides a calibration system for an inertial navigation system and a laser radar of an autonomous driving vehicle, including:

[0020] The laser radar odometer module is used to obtain the laser radar odometer data, store the data in the form of a set of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames;

[0021] The inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, store the data in the form of a set of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames;

[0022] The external parameter optimization module obtains the output data of the lidar odometer module and the inertial navigation odometer module, removes abnormal data, and performs nonlinear optimization using the interior point method. The obtained optimization parameters are output as the translation and rotation external parameters between the lidar and the inertial navigation device.

[0023] Furthermore, the inertial navigation odometer module includes:

[0024] The first inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, and store the data in the form of a group of coordinate point position and corresponding time information, and then perform one-dimensional interpolation and spherical interpolation processing, and output the coordinate and rotation data corresponding to the same time information in the laser radar odometer module;

[0025] The second inertial navigation odometer module obtains the data processed by the first inertial navigation odometer module to form new inertial navigation odometer data. The time information in the data is consistent with the time information in the lidar odometer module. The coordinate point position and the corresponding time information are stored as a group and then the translation distance between two adjacent frames is output.

[0026] In an embodiment of the present application, distance filtering is performed on the lidar odometer and the inertial navigation odometer to filter out poor positioning points caused by signal interference on the inertial navigation device and poor positioning points of the lidar odometer caused by the movement of dynamic objects in the field, thereby improving the accuracy of the translation amount of the calibration result. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0028] Figure 1 is a flow chart of the method provided in the embodiment of the present application;

[0029] Figure 2 is a schematic diagram of the odometer status after processing in step S3;

[0030] Figure 3 Schematic diagram of the odometer status after step S5. DETAILED DESCRIPTION

[0031] In order to make the purpose, features, and advantages of the present application more obvious and easy to understand, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the embodiments described below are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0032] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments.

[0033] In the description of the present application, it should be understood that the terms "upper", "lower", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0034] A first aspect of an embodiment of the present application provides a calibration method for an inertial navigation system and a laser radar of an autonomous driving vehicle, comprising:

[0035] S1) Acquire data from a laser radar odometer and an inertial navigation odometer, with coordinate point pose and corresponding time information as the composition of each set of data, where the coordinate point pose includes position information and rotation angle information, the position information is formed by a three-dimensional coordinate point relative to the origin, and the rotation angle information is formed by an azimuth transformation relative to a previous frame.

[0036] The acquisition here is just reading the data, so the time information formed in the data of the lidar odometer and the inertial navigation odometer does not correspond one to one.

[0037] As a specific embodiment, the data in the laser radar odometer includes i coordinate points, positions, and postures T L (T L is the transformation matrix) and the corresponding time information Time Lidar , where each pose is composed of position information and rotation angle information. Here, the position information Three-dimensional coordinate points relative to the origin, corner information It is the orientation transformation of the i-th frame relative to the i-1 frame.

[0038] The same inertial odometer coordinates are the coordinate point pose T I (T I is the transformation matrix), time information Time Ins , location information and corner information

[0039] S2) After normalizing the coordinate point poses in each set of data with respect to their respective initial coordinate points, the translation and rotation transformation amount of each coordinate point pose relative to the initial coordinate point pose is obtained.

[0040] As a specific embodiment, the inertial odometer coordinate position information is normalized into the formula: The corner information is normalized to And obtain the translation transformation of all other points relative to their respective initial coordinate points, and obtain the translation and rotation transformation amounts for the initial coordinates.

[0041] S3) Based on the point cloud timestamp formed by the time information of the laser radar odometer, a one-dimensional interpolation operation is performed on the inertial navigation odometer coordinate point pose at each time information to obtain the inertial navigation odometer coordinate result at each point cloud timestamp, and spherical interpolation is used for the rotation azimuth to obtain the inertial navigation odometer rotation azimuth result at each point cloud timestamp. The odometer coordinates and the rotation azimuth form new inertial navigation odometer data;

[0042] In this step, considering that the external parameters need to be converted for the two odometers at the same time, it is necessary to unify the timestamps in the two sets of data. Here, the time information in the new inertial navigation odometer data after one-dimensional interpolation and spherical interpolation operations is used to form a one-to-one correspondence with the time information in the lidar odometer data.

[0043] As a specific embodiment, the inertial odometer posture T I According to the point cloud odometer pose T L Timestamp LidarPerform one-dimensional interpolation and spherical interpolation operations to obtain the translation and rotation transformation of the inertial odometer that is synchronized with the point cloud odometer time. The effect is as follows Figure 2 , showing the processed odometer status.

[0044] S4) Based on the data of the laser radar odometer and the data of the new inertial navigation odometer, the rotational translation transformation relationship between two adjacent frames is obtained respectively to obtain the distance between the translations; specifically, every two adjacent frames are regarded as a rotational translation transformation relationship, and the rotational translation transformation relationship is used to perform distance calculation on the coordinate values ​​of the coordinate point positions under the two adjacent frames to obtain the distance between the translations; all the rotational translation transformation relationships form a transformation relationship group, and the transformation relationship group sequentially contains all the distances between the translations.

[0045] As a specific embodiment, the translation and rotation transformation between two adjacent frames of the radar odometer and the inertial device odometer after interpolation are calculated. Taking the radar odometer frame transformation formula as an example, the i-th transformation relationship is Represents the position and posture information of the i-th coordinate point of the laser radar odometer; the coordinate point position and posture information T L and T I , converted to the transformation relationship group T of the lidar odometer Lr and the transformation relationship group T of the inertial navigation odometer Ir .

[0046] S5) Screening the translation distance between the laser radar odometer and the translation distance between the new inertial navigation odometer in each group of adjacent frames. If the error between the translation distance data with the same sequence number in the transformation relationship group of the laser radar odometer and the transformation relationship group of the new inertial navigation odometer exceeds a preset threshold, the data is identified as an abnormal point and is removed from the two rotation and translation transformation relationships respectively. The two new transformation relationship groups formed after removing all abnormal points are optimized by the interior point method, and the obtained optimization parameters are used as the translation and rotation external parameters between the laser radar and the inertial navigation device.

[0047] As a specific embodiment, the transformation relationship group T of two adjacent frames is obtained. Lr and T Ir Then we start to filter out the distance outliers, and the translation transformation of two adjacent frames is recorded as t (t contains three-dimensional translation variables x, y, z), where T Lr The i-th translation transformation is recorded as T Ir The i-th translation transformation is recorded as Calculate the distance between two points using the distance formula and as well as and The transformation is performed by translation at each moment, and the transformation distance is obtained respectively.

[0048] If the distance error between the laser radar odometer and the new inertial navigation odometer is greater than 0.05 cm (the threshold set in this embodiment), the current transformation relationship group T is removed. Lr and T Ir .

[0049] After optimizing the selection, T Lr and T Ir Substitute the hand-eye calibration formula AX = XB, and substitute T Lr Bring in A and T Ir Substitute B and use the interior point nonlinear optimization method to perform nonlinear optimization, loss = AX-XB. When the loss value is the smallest, the optimal external parameter X is obtained. The final optimized X is the translation and rotation external parameters between the lidar and the inertial navigation device. L Multiply by X to perform rotation transformation, and the final trajectory calibration effect is as follows Figure 3 , after multiplying by the external parameter, the odometer has a high degree of complete fitting, which also proves that the calibration accuracy of the external parameter is high.

[0050] The second aspect of the embodiment of the present application provides a calibration system for inertial navigation and lidar of autonomous driving vehicles, which applies the above-mentioned calibration method, including: a lidar odometer module, an inertial navigation odometer module and an external parameter optimization module. The specific operation mode of each module is consistent with the specific content of the above-mentioned calibration method, and will not be described in detail here.

[0051] The laser radar odometer module is used to obtain the data of the laser radar odometer, store the data in the form of a group of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames;

[0052] The inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, store the data in the form of a group of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames.

[0053] Among them, the inertial navigation odometer module specifically includes:

[0054] The first inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, and store the data in the form of a group of coordinate point position and corresponding time information, and then perform one-dimensional interpolation and spherical interpolation processing, and output the coordinate and rotation azimuth data corresponding to the same time information in the laser radar odometer module;

[0055] The second inertial navigation odometer module obtains the data processed by the first inertial navigation odometer module to form new inertial navigation odometer data. The time information in the data is consistent with the time information in the lidar odometer module. The coordinate point position and the corresponding time information are stored as a group and then the translation distance between two adjacent frames is output.

[0056] The external parameter optimization module obtains the output data of the lidar odometer module and the inertial navigation odometer module, removes abnormal data, and performs nonlinear optimization using the interior point method. The obtained optimization parameters are output as the translation and rotation external parameters between the lidar and the inertial navigation device.

[0057] Here, the second inertial navigation odometer module is equivalent to synchronizing the time information corresponding to the data in the inertial navigation odometer module with the lidar odometer module, which can facilitate the optimization of the external parameters by the external parameter optimization module to improve the accuracy of the translation amount of the calibration result.

[0058] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the specific details in the above embodiments. Within the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical scheme of the present invention, and these equivalent transformations are all protected by the present invention.

Claims

1. A calibration method for inertial navigation and laser radar of an autonomous driving vehicle, characterized in that: include: S1) obtaining data of a laser radar odometer and an inertial navigation odometer, with coordinate point position and corresponding time information as the composition of each set of data; S2) after normalizing the coordinate point pose in each set of data, obtaining the translation and rotation transformation amount of each coordinate point pose relative to the initial coordinate point pose; S3) Based on the point cloud timestamp formed by the time information of the laser radar odometer, a one-dimensional interpolation operation is performed on the inertial navigation odometer coordinate point pose at each time information to obtain the inertial navigation odometer coordinate result at each point cloud timestamp, and spherical interpolation is used for the rotation azimuth to obtain the inertial navigation odometer rotation azimuth result at each point cloud timestamp. The odometer coordinates and the rotation azimuth form new inertial navigation odometer data; S4) based on the data of the laser radar odometer and the data of the new inertial navigation odometer, respectively obtaining the rotation and translation transformation relationship between two adjacent frames and then obtaining the translation distance; S5) The translation distances between the laser radar odometer and the new inertial navigation odometer in each group of adjacent frames are screened, and after removing abnormal points, nonlinear optimization is performed using the interior point method, and the obtained optimization parameters are used as the translation and rotation external parameters between the laser radar and the inertial navigation device.

2. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 1, characterized in that: In the above S1), the coordinate point position includes position information and rotation angle information, the position information is formed by the three-dimensional coordinate point relative to the origin, and the rotation angle information is formed by the orientation transformation relative to the previous frame.

3. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 2, characterized in that: The time information in the laser radar odometer data in S1) does not correspond one-to-one with the time information of the inertial navigation odometer.

4. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 3, characterized in that: The normalization of the coordinate point positions in S2) is for their respective initial coordinate points.

5. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 4, characterized in that: In the above S3), the time information in the new inertial navigation odometer data after one-dimensional interpolation and spherical interpolation calculations forms a one-to-one correspondence with the time information in the laser radar odometer data.

6. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 5, characterized in that: In the S4), every two adjacent frames are regarded as a rotation-translation transformation relationship, and the rotation-translation transformation relationship is used to calculate the distance between the coordinate values ​​of the coordinate point positions in the two adjacent frames to obtain the translation distance; all the rotation-translation transformation relationships form a transformation relationship group, which sequentially contains all the translation distances.

7. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 6, characterized in that: If the error of the translation distance data under the same sequence number in the transformation relationship group of the laser radar odometer in S5) and the transformation relationship group of the new inertial navigation odometer exceeds a preset threshold, the data is identified as an abnormal point and is removed from the two transformation relationship groups respectively.

8. The calibration method for inertial navigation and laser radar of an autonomous driving vehicle according to claim 7, characterized in that: In said S5), the two new transformation relationship groups formed after eliminating all abnormal points are optimized by interior point method.

9. A calibration system for inertial navigation and laser radar of an autonomous driving vehicle, characterized in that: include: The laser radar odometer module is used to obtain the laser radar odometer data, store the data in the form of a set of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames; The inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, store the data in the form of a set of coordinate point position and corresponding time information, and then output the translation distance between two adjacent frames; The external parameter optimization module obtains the output data of the laser radar odometer module and the inertial navigation odometer module, removes abnormal data, and performs nonlinear optimization using the interior point method. The obtained optimization parameters are output as the translation and rotation external parameters between the laser radar and the inertial navigation device. The inertial navigation odometer module includes: The first inertial navigation odometer module is used to obtain the data of the inertial navigation odometer, and store the data in the form of a group of coordinate point position and corresponding time information, and then perform one-dimensional interpolation and spherical interpolation processing, and output the coordinate data corresponding to the same time information in the laser radar odometer module; The second inertial navigation odometer module obtains the data processed by the first inertial navigation odometer module to form new inertial navigation odometer data. The time information in the data is consistent with the time information in the lidar odometer module. The coordinate point position and the corresponding time information are stored as a group and then the translation distance between two adjacent frames is output.

Images