Imu and lidar calibration method and system, map construction method

Abstract

The present disclosure provides an IMU and LiDAR calibration method and system, and a map construction method. In addition to constructing characteristic lines, characteristic surfaces and GNSS residual error equations, the present disclosure further utilizes GNSS data to perform optimization and conversion operations in absolute coordinates, and further extracts three-dimensional structure information such as ground and wall surfaces in an initialized map, and according to the geometric characteristics of the flat ground and the vertical wall, corresponding constraint conditions are applied to construct plane and surface constraint equations and GNSS constraint equations, and the constructed constraint equations are used to further optimize the pose of laser data and further optimize the relative relationship between the IMU and the LiDAR until the optimized relative relationship converges.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the field of autonomous navigation, in particular to the field of autonomous navigation in outdoor or indoor three-dimensional environments with rich structural information, and discloses an IMU and LiDAR calibration method and system and a map construction method. BACKGROUND

[0002] In autonomous navigation systems such as mobile robots, autonomous vehicles, and drones, IMU and LiDAR are commonly used key sensors. Currently, most IMU and LiDAR calibration schemes mainly focus on the relative pose relationship between the two, and these methods often limit LiDAR odometry to a relative coordinate system, ignoring the structured information and absolute scale information in the actual scene. Although the alignment of the sensors is achieved to some extent, the semantic information of the environment and the drift error caused by the relative scale of the relative coordinate system are not fully utilized, resulting in poor mapping accuracy due to insufficient calibration accuracy and stability when used in complex scenes.

[0003] Specifically, existing LiDAR odometry usually does not consider the structured information and absolute scale information in the actual scene, which leads to a deviation in the estimation of the gravity direction by the IMU during calibration, especially in the vertical axis direction. This deviation further leads to poor accuracy in the calibration process, limiting the application of the system in high-precision positioning and attitude estimation. In addition, existing methods do not fully utilize static targets, and in dynamic environments, dynamic targets can introduce noise and outliers, affecting the accuracy and stability of the calibration and further reducing the reliability of the calibration results. SUMMARY

[0004] The present disclosure at least provides an IMU and LiDAR calibration method and system, and a map construction method to improve the accuracy of IMU and LiDAR calibration.

[0005] According to an aspect of the present disclosure, an IMU and LiDAR calibration method is provided, comprising:

[0006] S100, scanning a three-dimensional space with a LiDAR to obtain laser data, obtaining IMU data of an IMU device bound to the LiDAR, and obtaining GNSS absolute coordinate data corresponding to an outdoor environment; performing time synchronization processing on the laser data, the IMU data, and the GNSS absolute coordinate data, and converting the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data, and target GNSS absolute coordinate data; wherein the three-dimensional space includes an outdoor environment with rich structural information;

[0007] S110, determine a relative relationship calibration value between the IMU and the LiDAR based on the mounting position of the IMU, the mounting attitude of the IMU, the mounting position of the LiDAR and the mounting attitude of the LiDAR; wherein the relative relationship calibration value includes a calibration value of a relative pose and a calibration value of a relative displacement between the IMU and the LiDAR;

[0008] S120, extract data of a moment when the IMU device is static from the target IMU data, and determine a gravity direction in a device coordinate system corresponding to the IMU device based on the extracted data;

[0009] S130, for each frame of target laser data, determine attitude data of the IMU device by using target IMU data at the same time as the current frame of target laser data and the gravity direction; convert the current frame of target laser data into the device coordinate system of the IMU device according to the relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data; and perform rectification processing on the adjusted laser data according to the attitude data of the IMU device to obtain a frame of de-distorted laser data; wherein the attitude data of the IMU device includes a roll angle, a pitch angle and a yaw angle; and the rectification processing includes rotation and translation;

[0010] S140, construct an initial map point cloud using the first frame of de-distorted laser data;

[0011] S150, for each frame of de-distorted laser data after the first frame of de-distorted laser data, extract a feature line and a feature surface of the current frame of de-distorted laser data; determine a main direction of the feature line and a normal of the feature surface; construct a line feature residual error equation using the feature line and the main direction, and construct a surface feature residual error equation using the feature surface and the normal; optimize the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of de-distorted laser data before the current frame to obtain first optimized attitude data of the current frame of de-distorted laser data and a first relative relationship optimization value between the IMU and the LiDAR; register the current frame of de-distorted laser data using the first optimized attitude data to obtain a frame of registered laser data, and merge the registered laser data into the initial map point cloud; replace the relative relationship calibration value between the IMU and the LiDAR with the first relative relationship optimization value between the IMU and the LiDAR, and repeatedly perform steps S130 and S150 to rectify, register and merge registered laser data to the initial map point cloud for a next frame of target laser data of the current frame until the initial map point cloud obtained by the merging operation meets a preset condition;

[0012] S160, for each frame of undistorted laser data after the first frame of undistorted laser data in the initial map point cloud, based on the current frame of undistorted laser data, the first optimized attitude data of each frame of undistorted laser data before the current frame, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, a GNSS residual equation corresponding to the current frame of undistorted laser data is constructed, and the GNSS residual equation is used for optimization to obtain the absolute attitude data of the current frame of undistorted laser data; based on the absolute attitude data of the current frame of undistorted laser data, the first relative relationship optimization value between the IMU and the LiDAR, the current frame of undistorted laser data is converted to the absolute coordinate system;

[0013] The normal vector of each point in the initial map point cloud is determined, and based on the gravity direction and the normal vector of each point, the ground points and the vertical surface points are determined; at least part of the ground points are used to fit the plane equation of the ground based on the height consistency principle; at least part of the vertical surface points are clustered, and each clustering result is used for fitting to obtain the plane equation of multiple vertical surfaces;

[0014] S170, for each frame of undistorted laser data after the first frame of undistorted laser data, a ground constraint equation is constructed using the plane equation of the ground; a vertical surface constraint equation is constructed using the plane equation of the vertical surface; based on the current frame of undistorted laser data, the absolute attitude data of the i-th frame of undistorted laser data which needs to be optimized of each frame of undistorted laser data before the current frame, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, and the GNSS weight, a GNSS constraint equation corresponding to the current frame of undistorted laser data is constructed;

[0015] S180, using the ground constraint equation, the vertical surface constraint equation and the GNSS constraint equation of each frame of undistorted laser data after the first frame of undistorted laser data, the second optimized attitude data of each frame of undistorted laser data after the first frame of undistorted laser data and the second relative relationship optimization value between the IMU and the LiDAR are determined;

[0016] S190, the following steps are executed in a frame cycle: the data of the three-dimensional space is re-acquired by using the LiDAR, the IMU device and the GNSS, the relative relationship calibration value is replaced by using the latest second relative relationship optimization value, and the time synchronization processing of step S100, the registration in step S130 and step S150, and the registration and laser data merging operation on the initial map point cloud are executed, and steps S160-S180 are executed until the last N frame of the second relative relationship optimization value corresponding to the deformed laser data converges, and the last obtained second relative relationship optimization value is determined as the target relative relationship optimization value; wherein N is a positive integer.

[0017] In a possible implementation, the constructing the line feature residual error equation by using the feature line and the main direction comprises:

[0018] The line feature residual error equation is constructed by using the following formula:

[0019]

[0020] wherein, is a line parameter of the feature line k, and h1 is the number of the feature lines extracted from the i-th frame of the deformed laser data; is the attitude data of the i-th frame of the deformed laser data to be optimized, m is the number of the current frame and each frame before the current frame; R calib1 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, is the line feature of the current feature line extracted; and the line parameter comprises the main direction.

[0021] In a possible implementation, the constructing the face feature residual error equation by using the feature face and the normal comprises:

[0022] The face feature residual error equation is constructed by using the following formula:

[0023]

[0024] wherein, h2 is the number of the feature faces extracted from the i-th frame of the deformed laser data, is a plane parameter of the feature face l, is the attitude data of the i-th frame of the deformed laser data to be optimized, m is the number of the current frame and each frame before the current frame; R calib1 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, is the face feature of the current feature face extracted, and the plane parameter comprises the normal.

[0025] In a possible implementation, the GNSS residual equation corresponding to the current frame of undistorted laser data is constructed based on the first optimized pose data of the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, the lever value between the IMU and the GNSS, and the target GNSS absolute coordinate data corresponding to the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, and includes:

[0026] The GNSS residual equation is as follows:

[0027]

[0028] In the formula, is the first optimized pose data of the i-th frame of undistorted laser data to be optimized, represents the lever value between the IMU and the GNSS, represents the target GNSS absolute coordinate data corresponding to the i-th frame of undistorted laser data, and m represents the number of frames of the current frame and each frame of undistorted laser data before the current frame;

[0029] The first relative relationship optimization value between the absolute pose data of the current frame of undistorted laser data, the IMU and the LiDAR is used to convert the current frame of undistorted laser data into an absolute coordinate system, and includes:

[0030] The current frame of undistorted laser data is converted into the absolute coordinate system by using the following formula:

[0031]

[0032] In the formula, is the absolute pose data of the i-th frame of undistorted laser data after optimization, R calib2 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, represents the i-th frame of undistorted laser data.

[0033] In a possible implementation, the ground constraint equation is constructed by using a plane equation of the ground, and includes:

[0034] The ground constraint equation is constructed by using the following formula:

[0035] H)

[0036] In the formula, is the absolute pose data of the i-th frame of undistorted laser data to be optimized, is the number of frames of the current frame and each frame before the current frame, is the elevation of the i-th frame of undistorted laser data point, R calib3represents a second relative relationship optimization value between the IMU and the LiDAR to be optimized, and H is a fitted height of the ground surface; wherein the H is determined according to a plane equation of the ground surface.

[0037] In a possible implementation, the plane vertical constraint equation is constructed using the plane equation of the facade, and the plane vertical constraint equation includes:

[0038] The plane vertical constraint equation is constructed using the following formula:

[0039]

[0040] wherein, is absolute attitude data of the i-th frame of undistorted laser data to be optimized, , , , is a facade parameter of the j-th facade of the i-th frame of undistorted laser data, and n is a number of facades of the i-th frame of undistorted laser data, , , is a point coordinate in the i-th frame of undistorted laser data, and m represents a number of frames of the current frame and each frame before the current frame, R calib3 represents a second relative relationship optimization value between the IMU and the LiDAR to be optimized.

[0041] In a possible implementation, the normal vector of each point in the initial map point cloud satisfying the preset condition is determined, and the method includes:

[0042] The normal vector of each point in the initial map point cloud is estimated based on a principal component analysis (PCA) method of neighborhood points.

[0043] In a possible implementation, the main direction of the feature line and the normal of the feature surface are determined, and the method includes:

[0044] For the feature line, neighborhood searching is performed in the current initial map point cloud, principal component analysis is performed on points on the current feature line using first neighborhood points obtained by the searching, and a direction corresponding to a maximum eigenvalue is taken as the main direction of the corresponding feature line.

[0045] For the feature surface, neighborhood searching is performed in the current initial map point cloud, principal component analysis is performed on points on the current feature surface using second neighborhood points obtained by the searching, and a direction corresponding to a minimum eigenvalue is taken as the normal of the corresponding feature surface.

[0046] In a possible implementation, the gravity direction in the device coordinate system of the IMU device is determined based on the extracted data, and the method includes:

[0047] Filter the data of the IMU device at the moment when the IMU device is static by using a Kalman filter, and determine the gravity direction in the device coordinate system corresponding to the IMU device by using the acceleration value in the filtered data.

[0048] According to another aspect of the present disclosure, a map construction method is provided, comprising:

[0049] Calibrate the relative relationship between the IMU and the LiDAR by using the IMU and LiDAR calibration method according to any one of the above;

[0050] Based on the relative relationship between the IMU and the LiDAR, construct a map of the corresponding region by using the data collected by the LiDAR and the data collected by the IMU.

[0051] According to another aspect of the present disclosure, an IMU and LiDAR calibration system is provided, comprising:

[0052] A laser radar is configured to scan data in a three-dimensional space to obtain laser data, wherein the three-dimensional space includes an outdoor environment with rich structural information;

[0053] An IMU device is configured to be bound with the laser radar and to collect IMU data;

[0054] A GNSS is configured to collect GNSS absolute coordinate data of the three-dimensional space;

[0055] A data analysis and synchronization module is configured to perform time synchronization processing on the laser data, the IMU data, and the GNSS absolute coordinate data, and convert the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data, and target GNSS absolute coordinate data;

[0056] An initial calibration module is configured to determine a relative relationship calibration value between the IMU and the LiDAR based on the installation position of the IMU, the installation attitude of the IMU, the installation position of the LiDAR, and the installation attitude of the LiDAR, wherein the relative relationship calibration value includes a calibration value of the relative pose and a calibration value of the relative displacement between the IMU and the LiDAR;

[0057] A gravity direction determination module is configured to extract data of the IMU device at the moment when the IMU device is static from the target IMU data, and determine a gravity direction in a device coordinate system corresponding to the IMU device based on the extracted data;

[0058] The laser radar data rectification module is configured to, for each frame of target laser data, determine attitude data of the IMU device by using target IMU data at the same time as the current frame of target laser data and the gravity direction; convert the current frame of target laser data into a device coordinate system of the IMU device according to a relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data; and rectify the adjusted laser data according to the attitude data of the IMU device to obtain a frame of de-distorted laser data; wherein the attitude data of the IMU device includes a roll angle, a pitch angle, and a yaw angle; and the rectification includes rotation and translation.

[0059] The map initialization module is configured to construct an initial map point cloud using the first frame of de-distorted laser data.

[0060] The registration module is configured to, for each frame of de-distorted laser data after the first frame of de-distorted laser data, extract a feature line and a feature surface of the current frame of de-distorted laser data; determine a main direction of the feature line and a normal of the feature surface; construct a line feature residual error equation by using the feature line and the main direction, and construct a surface feature residual error equation by using the feature surface and the normal; optimize the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of de-distorted laser data before the current frame to obtain first optimized attitude data of the current frame of de-distorted laser data and a first relative relationship optimization value between the IMU and the LiDAR; register the current frame of de-distorted laser data by using the first optimized attitude data to obtain a frame of registered laser data, and merge the registered laser data into the initial map point cloud; replace the relative relationship calibration value between the IMU and the LiDAR with the first relative relationship optimization value between the IMU and the LiDAR; and perform rectification, registration, and registered laser data merging operations on the initial map point cloud by using the laser radar data rectification module and the registration module in a loop until the initial map point cloud obtained by the merging operation meets a preset condition.

[0061] An absolute coordinate processing and constraint module is configured to, for each frame of de-distorted laser data after the first frame of de-distorted laser data in the initial map point cloud, based on the current frame of de-distorted laser data and the first optimized pose data of each frame of de-distorted laser data before the current frame, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, construct a GNSS residual equation corresponding to the current frame of de-distorted laser data, and perform optimization using the GNSS residual equation to obtain absolute pose data of the current frame of de-distorted laser data; convert the current frame of de-distorted laser data to an absolute coordinate system based on the first relative relationship optimization value between the IMU and the LiDAR of the absolute pose data of the current frame of de-distorted laser data; determine the normal vector of each point in the initial map point cloud, and determine the ground points and the vertical surface points based on the gravity direction and the normal vector of each point; fit the plane equation of the ground based on the height consistency principle using at least part of the ground points; cluster at least part of the vertical surface points using the plane equation of the vertical surface, and fit each clustering result to obtain the plane equation of multiple vertical surfaces; for each frame of de-distorted laser data after the first frame of de-distorted laser data, construct a ground constraint equation using the plane equation of the ground; construct a vertical surface perpendicular constraint equation using the plane equation of the vertical surface; based on the absolute pose data of the i-th frame of de-distorted laser data that needs to be optimized, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, and the GNSS weight of the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, construct a GNSS constraint equation corresponding to the current frame of de-distorted laser data;

[0062] The target calibration module is configured to determine second optimized attitude data of each frame of post-distortion laser data after the first frame of post-distortion laser data and a second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of post-distortion laser data after the first frame of post-distortion laser data, control the LiDAR, the IMU device, and the GNSS to reacquire data of the three-dimensional space, control the data analysis and synchronization module to perform the time synchronization processing, replace the relative relationship calibration value with the latest second relative relationship optimization value, control the laser radar data correction module to perform correction processing, control the registration module to perform registration and laser data merging operation on the initial map point cloud, control the geometric feature extraction and constraint module to perform feature extraction and constraint establishment, and perform the determination of the second optimized attitude data of each frame of post-distortion laser data after the first frame of post-distortion laser data and the second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of post-distortion laser data after the first frame of post-distortion laser data until the second relative relationship optimization value corresponding to the last N frames of post-distortion laser data converges, and determine the finally obtained second relative relationship optimization value as the target relative relationship optimization value, where N is a positive integer.

[0063] The IMU and LiDAR calibration method and system and the map construction method of the present disclosure use the measurement data of the IMU device to calculate the current gravity direction, providing an absolute direction reference for map initialization. In addition to constructing feature lines, feature surfaces, and GNSS residual error equations, the present disclosure further performs optimization and conversion operations in the absolute coordinates using GNSS data, and extracts three-dimensional structure information such as ground and wall surfaces in the initialized map. According to the geometric properties of ground flatness and wall perpendicularity, corresponding constraint conditions are applied to construct plane and facade constraint equations and GNSS constraint equations. The constructed constraint equations are used to further optimize the attitude of laser data and further optimize the relative relationship between the IMU and the LiDAR until the optimized relative relationship converges. The scheme of the present disclosure improves the calibration accuracy and stability between the IMU and the LiDAR by utilizing structured semantic information in actual scenes and absolute position information of GNSS, such as the flatness of the ground and the perpendicularity of the building facade, and distinguishing and utilizing the information of absolute static objects. Higher accuracy is achieved in the vertical axis direction. The method of the present disclosure is particularly suitable for outdoor open environments with rich buildings, and high-precision sensor calibration is achieved by utilizing the rich structured features in these environments.

[0064] Further, constructing a map based on the relative relationship between the IMU and the LiDAR after calibration can effectively reduce the accumulation of errors caused by sensor noise and improve the accuracy and consistency of map construction.

[0065] It should be understood that the contents described in this part are not intended to identify key or important features of the embodiments of the present disclosure, nor to limit the scope of the present disclosure. Other features of the present disclosure will become apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0066] The accompanying drawings are used to better understand the present scheme and do not limit the present disclosure. Among them:

[0067] Figure 1 is a flow chart of the IMU and LiDAR calibration method according to the embodiments of the present disclosure;

[0068] Figure 2 is a structural schematic diagram of the IMU and LiDAR calibration system according to the embodiments of the present disclosure. DETAILED DESCRIPTION

[0069] Exemplary embodiments of the present disclosure are described below with reference to the accompanying drawings, which include various details of the embodiments of the present disclosure to help understanding, and should be considered as merely exemplary. Therefore, those of ordinary skill in the art should recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present disclosure. Also, for the sake of clarity and conciseness, the description below omits the description of well-known functions and structures.

[0070] In autonomous navigation systems such as mobile robots, autonomous vehicles and drones, IMU and LiDAR are commonly used key sensors. IMU is mainly used to detect the acceleration and angular velocity of the device, providing attitude and motion information within a short time; LiDAR obtains three-dimensional point cloud data of the environment by emitting laser beams and receiving reflected signals, realizing high-precision environment perception and position positioning. At present, most of the calibration schemes of IMU and LiDAR mainly focus on the relative attitude relationship between the two. These methods often limit the LiDAR odometry to the relative coordinate system, ignoring the structured information and absolute scale information in the actual scene, such as the flatness of the ground and the perpendicularity of the building facade, as well as GNSS and other controllable absolute scale information. Although this method can align the sensors to a certain extent, it fails to fully utilize the semantic information of the environment and the drift error caused by the relative scale of the relative coordinate system, resulting in poor mapping accuracy due to insufficient calibration accuracy and stability when used in complex scenes.

[0071] Specifically, existing LiDAR odometry usually does not consider structured information in the actual scene and absolute scale information, such as the flatness of the ground and the vertical facade of the building, as well as absolute positioning information such as GNSS. This leads to a deviation in the estimation of the gravity direction by the IMU, especially in the vertical axis direction, when calibrating the IMU and the LiDAR. This deviation further leads to poor accuracy in the calibration process, limiting the application of the system in high-precision positioning and attitude estimation.

[0072] In addition, existing methods do not distinguish between static targets. Absolute static objects such as the ground and building facades have stable geometric features in the environment, and these three-dimensional information is often more important. Therefore, in a dynamic environment, dynamic targets can introduce noise and outliers, affecting the accuracy and stability of the calibration, further reducing the reliability of the calibration results.

[0073] Therefore, there is an urgent need for a method that can combine structured semantic information, especially distinguish and utilize absolute static object information, and be suitable for outdoor environments with rich buildings, to improve the accuracy and robustness of IMU and LiDAR calibration, to overcome the shortcomings of existing technology in gravity direction estimation, vertical axis direction accuracy, and dynamic environment processing.

[0074] To solve the above problems, the present disclosure provides an IMU and LiDAR calibration method and system, and a map construction method. The scheme of the present disclosure is particularly suitable for open outdoor environments with rich buildings. In these environments, the rich ground and planar building facade information provides sufficient structured semantic information, and the open environment can introduce absolute position information such as GNSS, and through the fusion of these structured information and multi-source sensors, the accuracy and robustness in the calibration process can be effectively improved. However, this also means that the method has certain requirements for the degree of structuring of the scene, and is suitable for complex environments with clear geometric structure characteristics.

[0075] The technical solutions of the present disclosure will be described below through specific embodiments.

[0076] As shown in Figure 1 , it is a flowchart of the IMU and LiDAR calibration method of the present embodiment. The execution subject of the present embodiment is a component, device or system with computing capability. Specifically, the IMU and LiDAR calibration method of the present embodiment includes the following steps:

[0077] S100, data acquisition, time alignment, and coordinate system: using a laser radar LiDAR to scan the three-dimensional space to obtain laser data, obtain IMU data of an IMU device bound with the laser radar, and obtain GNSS absolute coordinate data corresponding to an outdoor environment; performing time synchronization processing on the laser data, the IMU data, and the GNSS absolute coordinate data, and converting the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data, and target GNSS absolute coordinate data; wherein the three-dimensional space includes an outdoor environment with rich structural information.

[0078] Here, the laser radar is used to collect three-dimensional point cloud data in the three-dimensional space, i.e., laser data, and the IMU device is used to collect acceleration and angular velocity information of the device, i.e., the IMU data.

[0079] After the laser data and the IMU data are collected, the format of the laser data, such as a PCD point cloud format, is parsed, and the format of the IMU data, such as accelerometer and gyroscope data, is parsed, and the format of the GNSS data is parsed. Then, the timestamps of the laser data, the IMU data, and the GNSS absolute coordinate data are aligned to ensure that the data at the same time is used for subsequent processing. A timestamp alignment algorithm, such as an interpolation or interpolation method, is used to handle the difference in sampling frequency of different sensors, and the pose calibration of the laser radar and the IMU device is performed in advance, i.e., the relative relationship between the pose of the laser radar and the pose of the IMU device is calibrated.

[0080] In addition, according to the initial installation parameters, the data collected by the LiDAR is converted from the body coordinate system to the device coordinate system corresponding to the IMU device, and through the positional relationship between the GNSS and the IMU in the structure design, the GNSS coordinate is also introduced into the IMU coordinate system, ensuring that the data of the three are compared and processed in the same coordinate system.

[0081] S110, based on the installation position of the IMU, the installation pose of the IMU, the installation position of the LiDAR, and the installation pose of the LiDAR, determining a relative relationship calibration value between the IMU and the LiDAR; wherein the relative relationship calibration value includes a calibration value of the relative pose and a calibration value of the relative displacement between the IMU and the LiDAR.

[0082] Specifically, based on the drawing structure and the installation position, the relative pose and displacement calibration value between the IMU and the LiDAR can be preset to reduce the search range of subsequent calibration and improve the calibration efficiency and accuracy.

[0083] S120, extract data of the IMU device at rest from the target IMU data, and determine the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data.

[0084] When the device is at rest, collect the data of the IMU at rest, analyze the acceleration and angular velocity information, and accurately calculate the current gravity direction according to the acceleration value measured at rest, thereby providing a reference for subsequent attitude correction.

[0085] When determining the gravity direction, filter the data of the IMU device at rest extracted by using a Kalman filter, and determine the gravity direction in the device coordinate system corresponding to the IMU device by using the filtered data.

[0086] Specifically:

[0087] Identify the time when the IMU device is at rest, extract the IMU data at that time (the accelerometer data mainly reflects the gravity direction, and the gyroscope data reflects the angular velocity of the device). Process the IMU data at rest by using a Kalman filter to estimate the current gravity direction. The Kalman filter can fuse the IMU data, filter out noise, and provide stable gravity direction estimation. According to the output of the Kalman filter, determine the direction of gravity in the device coordinate system, and provide a reference for subsequent ground level and wall vertical constraints.

[0088] S130, for each frame of target laser data, determine the attitude data of the IMU device by using the target IMU data at the same time as the current frame of target laser data and the gravity direction; convert the current frame of target laser data into the device coordinate system of the IMU device according to the relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data; and perform rectification processing on the adjusted laser data according to the attitude data of the IMU device to obtain a frame of de-distorted laser data; wherein the attitude data of the IMU device includes roll angle, pitch angle and yaw angle; and the rectification processing includes rotation and translation.

[0089] Based on the gravity direction determined in the above steps and the relative relationship calibration value between the IMU and the LiDAR, the attitude of the device in space is calculated using the attitude information provided by the IMU. These attitude information are estimated by the Kalman filter and take into account the system characteristics of the IMU itself, such as the noise level of acceleration and angular velocity. Specifically, the attitude parameters including roll angle, pitch angle and yaw angle are calculated. Subsequently, the point cloud data collected from the laser radar is converted from the laser radar coordinate system to the IMU coordinate system by using the calibrated relationship between the laser radar and the IMU; and then by analyzing the attitude estimation of the IMU, necessary rotation and translation transformation is performed to correct the attitude motion distortion generated by the laser radar during movement.

[0090] Specifically, the attitude information provided by the IMU device (estimated by the Kalman filter) and the device parameters of the IMU device itself, such as the acceleration and angular velocity noise of the IMU device, are used to calculate the attitude of the IMU device in space (including roll angle, pitch angle and yaw angle).

[0091] The point cloud data collected by the laser radar, i.e., the target laser data obtained after alignment, is converted from the radar coordinate system to the device coordinate system according to the conversion relationship between the coordinate systems of the laser radar and the IMU device provided. Then, the estimated attitude data of the IMU device is rotated and translated to correct the laser attitude motion distortion and obtain the de-distorted laser data.

[0092] S140, using the first frame of de-distorted laser data to construct an initial map point cloud.

[0093] S150, laser data rectification, registration and registration laser data merging operation: for each frame of de-distorted laser data after the first frame of de-distorted laser data, the feature lines and feature surfaces of the current frame of de-distorted laser data are extracted; the main direction of the feature lines and the normal of the feature surfaces are determined; the line feature residual error equation is constructed using the feature lines and the main direction, and the surface feature residual error equation is constructed using the feature surfaces and the normal; the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of de-distorted laser data before the current frame are optimized to obtain the first optimized attitude data of the current frame of de-distorted laser data and the first relative relationship optimization value between the IMU and the LiDAR; the current frame of de-distorted laser data is registered using the first optimized attitude data to obtain a frame of registered laser data, and the registered laser data is merged into the initial map point cloud; the first relative relationship optimization value between the IMU and the LiDAR is used to replace the relative relationship calibration value between the IMU and the LiDAR, and steps S130 and S150 are repeatedly executed to rectify, register and merge the initial map point cloud with the registration laser data for the next frame of target laser data of the current frame until the initial map point cloud obtained by the merging operation meets the preset condition.

[0094] For each frame of de-distorted laser data after the first frame of de-distorted laser data, an edge detection algorithm and a plane fitting algorithm are used to extract feature lines and feature surfaces (such as road edges, building facades, etc.) therefrom.

[0095] In some embodiments, the determination of the main direction of the feature lines and the normal of the feature surfaces can be achieved by the following steps:

[0096] For the feature line, neighborhood search is performed in the current initial map point cloud, principal component analysis is performed on the points on the current feature line using the plurality of first neighborhood points obtained by the search, and the direction corresponding to the maximum eigenvalue is taken as the principal direction of the corresponding feature line; for the feature surface, neighborhood search is performed in the current initial map point cloud, principal component analysis is performed on the points on the current feature surface using the plurality of second neighborhood points obtained by the search, and the direction corresponding to the minimum eigenvalue is taken as the normal of the corresponding feature surface.

[0097] Specifically, the KDTREE search algorithm can be used to perform neighborhood search on the extracted feature lines and feature surfaces in the initial map point cloud. This process not only improves the accuracy of feature recognition, but also enhances the reliability of the map and the efficiency of map construction.

[0098] The line feature residual error equation is constructed by using the following formula:

[0099]

[0100] wherein, is the line parameter of the feature line k, and h1 is the number of feature lines extracted from the i-th frame of deformed laser data; is the pose data of the i-th frame of deformed laser data to be optimized, m is the number of frames of the current frame and each frame before the current frame; R calib1 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, is the line feature of the extracted current feature line; the line parameter includes the principal direction. One line parameter can correspond to multiple line features. Here, m can be understood as the number of all frames in the optimization window.

[0101] The surface feature residual error equation is constructed by using the following formula:

[0102]

[0103] wherein, h2 is the number of feature surfaces extracted from the i-th frame of deformed laser data, is the plane parameter of the feature surface l, is the pose data of the i-th frame of deformed laser data to be optimized, m is the number of frames of the current frame and each frame before the current frame; R calib1 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, is the surface feature of the extracted current feature surface, and the plane parameter includes the normal. One plane parameter can correspond to multiple surface features; the surface feature can be expressed as a surface feature point. Here, m can be understood as the number of all frames in the optimization window.

[0104] According to the set of line feature residual error equations and surface feature residual error equations, iteration is performed by a nonlinear least square method, and the residual error set can be expressed as:

[0105]

[0106] wherein is a preset norm, by solving the function, the residual errors of the feature lines and the feature surfaces are optimized, so that is as small as possible, and finally the optimized pose and are obtained.

[0107] The registered laser data is merged into the initial map point cloud, and the coverage of the map is gradually expanded.

[0108] The optimized is applied, and the above-mentioned deviation correction, registration and merging of the initial map point cloud with the registration laser data are repeatedly performed. Specifically, in the data stream continuously released, for each frame of target laser data, the optimized first relative relationship optimization value is applied to the deviation correction of the next frame of target laser data, the attitude (including roll angle, pitch angle and yaw angle) of the IMU device in space is calculated according to the latest relative relationship, combined with the acceleration and angular velocity parameters of the IMU at the same time, and the optimized pose of the radar at the same time is calculated according to the calibration parameters, the map registration and optimization are continuously performed, and a high-precision initial map point cloud is gradually constructed.

[0109] The initial map point cloud meets the preset condition, that is, as the initial map point cloud is gradually accumulated, the details and accuracy of the map are enhanced, and the map can reflect the key structural features of the current environment, such as the ground plane and the building facade. In specific practice, the frame number of the target laser data or the distortion-removed laser data that meets the preset condition can be determined according to experience, and when the frame number of the data merged in the initial map point cloud exceeds the frame number, it is determined that the preset condition is met.

[0110] S160, optimization and conversion in absolute coordinates, constructing plane equation: for each frame of undistorted laser data in the initial map point cloud after the first frame of undistorted laser data, based on the first optimized pose data of the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of undistorted laser data and each frame of undistorted laser data before the current frame, construct the GNSS residual equation corresponding to the current frame of undistorted laser data, and use the GNSS residual equation to optimize to obtain the absolute pose data of the current frame of undistorted laser data; based on the first relative relationship optimization value between the IMU and the LiDAR, the current frame of undistorted laser data is converted to the absolute coordinate system.

[0111] Determine the normal vector of each point (in the absolute coordinate system) in the initial map point cloud that meets the preset condition, and determine the ground points and vertical surface points based on the gravity direction and the normal vector of each point; using at least part of the ground points, a plane equation of the ground is fitted based on the height consistency principle; using at least part of the vertical surface points, clustering is performed, and each clustering result is used for fitting to obtain a plurality of plane equations of vertical surfaces.

[0112] The above steps realize GNSS absolute coordinate updating. By requiring the collection route to be an S-shaped route as much as possible, more GNSS position information (i.e. GNSS absolute coordinate data) is accepted to control the plane accuracy of the absolute position. By using a high-precision time alignment system and the relative position relationship between the GNSS and the IMU, the GNSS information and the IMU information can be fused to obtain the absolute position information of the IMU at different times, and then the absolute position information of the pose of the undistorted laser data can be obtained, and the formula is as follows:

[0113]

[0114] In the formula, is the first optimized pose data of the i-th frame of undistorted laser data to be optimized, represents the lever value between the IMU and the GNSS, represents the target GNSS absolute coordinate data corresponding to the i-th frame of undistorted laser data, and m represents the number of frames of the current frame and each frame of undistorted laser data before the current frame.

[0115] Through the above optimization, the corresponding IMU relative pose in the initialization map can be converted to the position and pose in the absolute coordinate system. At the same time, through the new optimized absolute coordinate pose, the initialization map point cloud can be re-projected to the absolute coordinate system, and the formula is as follows:

[0116]

[0117] wherein, is the optimized absolute attitude data of the i-th frame of undistorted laser data, R calib2 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, represents the i-th frame of undistorted laser data.

[0118] Through the above-mentioned joint GNSS absolute coordinate updating, the initialization map data with absolute coordinate system is obtained, and on this basis, the ground and building facade extraction will be carried out subsequently.

[0119] Specifically, the PCA (principal component analysis) method based on neighborhood points can be used to estimate the normal vector of each point.

[0120] In some embodiments, the ground points and facade points are extracted from the initial map point cloud which meets the preset condition, and the following steps can also be used to determine:

[0121] KD tree of the initial map point cloud is constructed; for each point in the initial map point cloud, the KD tree is used to search the neighboring points of the current point, and the normal vector of the current point is determined by using the searched neighboring points; and the ground points and facade points are extracted from the initial map point cloud according to the normal vectors of the points.

[0122] The above-mentioned step constructs the KD tree index of the initial map point cloud data, which can accelerate the process of the near neighbor search. Then, for each map point, the normal vector is calculated by using the neighboring points, and the normal vector is mainly determined by the PCA (principal component analysis) method. Then, the normal vector classification processing is performed, and the ground points and facade points are extracted from the map points according to the direction of the normal vector: the ground points are the spatial points with upward normal (parallel to the gravity direction) or nearly horizontal normal, and the facade points are the spatial points with normal perpendicular to the gravity direction, reflecting the vertical characteristics of the wall.

[0123] By using the calculated normal vector, the point cloud is divided into ground points and building facade points. The points with the normal vector consistent with the gravity direction (parallel or nearly parallel to the gravity direction) are taken as the ground points. The points with the normal vector perpendicular to the gravity direction are taken as the facade points.

[0124] The plane equation of the ground is fitted: the point set meeting the plane characteristics is selected from the classified ground points, the plane fitting is performed on the ground point set, the plane equation of the ground is determined by using the RANSAC algorithm or the least square method, and the ground equation can be expressed as . represents the height value of the current ground point, and H is the height of the fitted plane.

[0125] Ground point constraint setting: Based on the fitted plane equation of the ground, set the constraint condition for the consistency of ground height (first constraint condition), that is, the height of all ground points should be consistent within a certain error range to ensure that all ground points have height consistency.

[0126] Clustering and segmentation are performed on candidate facade points of buildings and other locations. Vertical edge detection algorithms or multi-plane segmentation methods are used to extract the facade planes of each building, ensuring that the building point cloud is located on the facades perpendicular to the ground. Through cluster analysis (such as the DBSCAN algorithm), the fitted facade points are further segmented into different facades to reflect different walls or building structures.

[0127] Specifically, the planar equation of the facade is fitted: a set of points that meet the vertical characteristics is selected from the classified facade points, and then clustered by cluster analysis (such as the DBSCAN algorithm). The facade point set of the clustering results is then fitted to the plane, and the planar equation of the facade is determined by using the RANSAC algorithm or the least squares method.

[0128] Facade clustering and segmentation: Through cluster analysis (such as the DBSCAN algorithm), the facade points obtained from clustering are further segmented into different facades, reflecting different wall types or building structures. The plan equation of a single facade can be expressed as follows: .in , , , For plane equation parameters, , , These are the coordinates of the corresponding elevation points.

[0129] Facade point addition facade constraint setting: Based on the fitted facade plane equation, set the facade perpendicular to the ground constraint condition (second constraint condition), that is, the normal of all facade points should be perpendicular to the gravity direction to ensure the verticality of the wall.

[0130] Extraction of large ground areas and building facade information: Through plane fitting and segmentation, the main ground planes and the facade planes of various buildings in the initial map are extracted to form large ground areas and separate facade information.

[0131] S170, a constraint equation is suggested: for each frame of the de-warping laser data after the first frame of the de-warping laser data, a ground constraint equation is constructed by using the plane equation of the ground; a vertical constraint equation of the facade is constructed by using the plane equation of the facade. Based on the current frame of the de-warping laser data and the absolute attitude data of the i-th frame of the de-warping laser data to be optimized, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of the de-warping laser data and each frame of the de-warping laser data before the current frame, and the GNSS weight, a GNSS constraint equation corresponding to the current frame of the de-warping laser data is constructed.

[0132] According to the principle that all ground points have the same height, a ground constraint equation is constructed to ensure the consistency of the ground plane in the map. Specifically, the ground constraint equation can be constructed by using the following formula:

[0133] H)

[0134] wherein, is the absolute attitude data of the i-th frame of the de-warping laser data to be optimized, is the number of frames of the current frame and each frame before the current frame, is the height of the i-th frame of the de-warping laser data point, R calib3 represents the second relative relationship optimization value between the IMU and the LiDAR to be optimized, and H is the fitting height of the ground. Wherein, H is determined according to the plane equation of the ground.

[0135] According to the principle that all facade points of the building facade are located on the vertical facade, a facade constraint equation of the building facade is constructed to ensure the verticality of the building facade. Specifically, the facade vertical constraint equation is constructed by using the following formula:

[0136]

[0137] wherein, is the absolute attitude data of the i-th frame of the de-warping laser data to be optimized, , , , is the facade parameter of the j-th facade of the i-th frame of the de-warping laser data, and n is the number of facades of the i-th frame of the de-warping laser data, , , is the point coordinate in the i-th frame of the de-warping laser data, m represents the number of frames of the current frame and each frame before the current frame, R calib3 represents the second relative relationship optimization value between the IMU and the LiDAR to be optimized.

[0138] Since the pose of the distortion-free point cloud and the extrinsic parameters from LiDAR to IMU are optimized simultaneously for each frame, it is undesirable for the current pose and GNSS position value to deviate during the optimization process. Therefore, GNSS information needs to be introduced into the optimization equations after the map is initialized. The GNSS constraint equations are as follows:

[0139]

[0140] In the formula, The absolute attitude data of the i-th frame of the distortion-free laser data that needs to be optimized. Indicates the lever arm value between the IMU and GNSS. This represents the target GNSS absolute coordinates corresponding to the i-th frame of distorted laser data, where m represents the number of frames in the current frame and all previous frames of distorted laser data. Since the amount of data using laser line features and area features is not consistent with the frequency of GNSS data, weighted processing of the GNSS data is required. The weights are generally set as the sum of the values ​​of the ground constraints and the facade constraints.

[0141] S180. Using the ground constraint equation, elevation constraint equation, and GNSS constraint equation of each frame of distorted laser data after the first frame of distorted laser data, determine the second optimized attitude data of each frame of distorted laser data after the first frame of distorted laser data and the second optimized relative relationship value between the IMU and LiDAR.

[0142] In the specific implementation, based on the ground point residuals extracted in the above steps... and facade residuals The following constraint equations are constructed:

[0143]

[0144] in Based on a predefined paradigm, the ground and elevation constraints are optimized by solving this function to obtain the final pose that needs optimization. and between LiDAR and IMU The optimal LiDAR-to-IMU conversion can be obtained through multiple iterations using a nonlinear least squares optimization algorithm. This ensures that the buildings and ground in the constructed initial map match the actual scene, thereby further improving the calibration accuracy of IMU and LiDAR.

[0145] S190. Based on the second relative relationship optimization value corresponding to the last N frames of distortion-free laser data, determine whether the second relative relationship optimization value has converged. If it has converged, then determine the last obtained second relative relationship optimization value as the target relative relationship optimization value. Specifically:

[0146] The following steps are executed in a frame cycle: the data of the three-dimensional space is re-acquired using the LiDAR and IMU devices, GNSS, the relative relationship calibration value is replaced with the latest second relative relationship optimization value, and the time synchronization processing of step S100, the registration in steps S130 and S150, and the registration and laser data merging operation on the initial map point cloud in steps S160-S180 are performed until the second relative relationship optimization value corresponding to the last N frames of deformed laser data converges, and the finally obtained second relative relationship optimization value is determined as the target relative relationship optimization value; wherein N is a positive integer.

[0147] More specifically, S190 each cycle can complete the steps S100, S130, S150 "for each frame of deformed laser data after the first frame of deformed laser data, extract the feature lines and feature surfaces of the current frame of deformed laser data; determine the principal direction of the feature lines and the normal of the feature surfaces; construct a line feature residual error equation using the feature lines and the principal direction, and construct a surface feature residual error equation using the feature surfaces and the normal; use the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of deformed laser data before the current frame to optimize to obtain the first optimized attitude data of the current frame of deformed laser data and the first relative relationship optimization value between the IMU and the LiDAR; register the current frame of deformed laser data using the first optimized attitude data to obtain a frame of registered laser data, and merge the registered laser data into the initial map point cloud", complete steps S160-S180 until the second relative relationship optimization value corresponding to the last N frames of deformed laser data converges.

[0148] If the optimization continues for multiple frames If the change is small within the limited threshold range, it is considered to be conditionally convergent, and a more accurate calibration value is obtained, if the result continuously changes greatly, repeat S150-S190 until the optimization of continuous multiple frames It is relatively stable.

[0149] The scheme of the present disclosure introduces structured geometric information such as ground and planar surfaces in the environment in the initial stage, and optimizes the relative relationship between the IMU and the LiDAR through specific geometric constraints combined with static objects, realizes high-precision and robust calibration between the IMU and the LiDAR, and is particularly suitable for open outdoor environments with rich buildings in terms of using structured information and GNSS absolute position information, significantly improves the accuracy of the calibration result, and especially shows higher accuracy in the vertical axis direction. The following is the performance improvement of the scheme of the present disclosure:

[0150] Enhanced Calibration Accuracy: By utilizing structured semantic information in real-world scenarios, such as the flatness of the ground and the verticality of building facades, this method significantly improves the calibration accuracy between the IMU and LiDAR, especially in the vertical axis direction.

[0151] Increased Calibration Robustness: By distinguishing and utilizing absolute static objects (such as the ground and building facades), this method effectively reduces the impact of noise and outliers in dynamic environments on the calibration results, thereby improving the robustness and stability of the calibration process.

[0152] Comprehensive Optimization Strategy: Combining the principles of ground height consistency and building facade verticality, constraint equations are constructed and nonlinear least squares method is used for optimization, ensuring accurate estimation of calibration parameters and map consistency.

[0153] Efficient Feature Extraction and Registration: Using RANSAC-based plane extraction algorithm and vertical edge detection algorithm, the ground and planar building facade features are accurately extracted, ensuring the accuracy of feature extraction and the efficiency of the registration process.

[0154] Wide Applicability: This method is particularly suitable for open outdoor environments with abundant buildings. The rich structured features and the provision of absolute positions by GNSS in these environments provide sufficient information support for calibration, ensuring efficient application of the method in complex scenarios.

[0155] In summary, the present disclosure provides an efficient, accurate and robust IMU and LiDAR calibration method, which significantly improves the quality of sensor calibration and the overall performance of the system, and has important technical innovation significance and wide application value.

[0156] Based on the same inventive concept, the present disclosure provides a map construction method, comprising:

[0157] Calibrating the relative relationship between the IMU and LiDAR using the IMU and LiDAR calibration method of any of the above embodiments; based on the relative relationship between the IMU and LiDAR, constructing a map of the corresponding region using the data collected by the LiDAR and the data collected by the IMU.

[0158] The steps performed by this method are the same or similar to those of the method in the above embodiments, so the similar parts are not repeated.

[0159] Based on the same inventive concept, the present disclosure provides an IMU and LiDAR calibration system, the components of which perform steps that are the same or similar to those of the above method, so the similar parts are not repeated. As shown in Figure 2 The IMU and LiDAR calibration system of the present embodiment comprises:

[0160] The laser radar 200 is used for scanning data in a three-dimensional space to obtain laser data; wherein the three-dimensional space includes an outdoor environment with rich structure information.

[0161] The IMU device 201 is bound with the laser radar and is used for collecting IMU data.

[0162] The GNSS 202 is used for collecting GNSS absolute coordinate data of the three-dimensional space.

[0163] The data analysis and synchronization module 203 is used for performing time synchronization processing on the laser data, the IMU data and the GNSS absolute coordinate data, and converting the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data and target GNSS absolute coordinate data.

[0164] The initial calibration module 204 is used for determining a relative relationship calibration value between the IMU and the LiDAR based on an installation position of the IMU, an installation posture of the IMU, an installation position of the LiDAR and an installation posture of the LiDAR; wherein the relative relationship calibration value includes a calibration value of a relative pose and a calibration value of a relative displacement between the IMU and the LiDAR.

[0165] The gravity direction determination module 205 is used for extracting data of a moment when the IMU device is static from the target IMU data, and determining a gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data.

[0166] The laser radar data correction module 206 is used for, for each frame of target laser data, determining posture data of the IMU device by using target IMU data at the same time as the current frame of target laser data and the gravity direction; converting the current frame of target laser data into the device coordinate system of the IMU device according to the relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data; and performing correction processing on the adjusted laser data according to the posture data of the IMU device to obtain a frame of de-distorted laser data; wherein the posture data of the IMU device includes a roll angle, a pitch angle and a yaw angle; and the correction processing includes rotation and translation.

[0167] The map initialization module 207 is used for constructing an initial map point cloud by using the first frame of de-distorted laser data.

[0168] The registration module 208 is configured to, for each frame of the de-distorted laser data after the first frame of the de-distorted laser data, extract a feature line and a feature surface of a current frame of the de-distorted laser data, determine a main direction of the feature line and a normal of the feature surface, construct a line feature residual error equation by using the feature line and the main direction, construct a surface feature residual error equation by using the feature surface and the normal, perform optimization by using the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of the de-distorted laser data before the current frame, to obtain first optimized pose data of the current frame of the de-distorted laser data and a first optimized value of the relative relationship between the IMU and the LiDAR, perform registration on the current frame of the de-distorted laser data by using the first optimized pose data, to obtain a frame of registered laser data, and merge the registered laser data into the initial map point cloud, replace the relative relationship calibration value between the IMU and the LiDAR with the first optimized value of the relative relationship between the IMU and the LiDAR, and perform, by using the laser radar data correction module and the registration module, the operations of correction, registration and registered laser data merging on the initial map point cloud for the target laser data in a loop until the initial map point cloud obtained by the merging operation meets a preset condition.

[0169] The absolute coordinate processing and constraint module 209 is configured to, for each frame of de-distorted laser data after the first frame of de-distorted laser data in the initial map point cloud, based on the current frame of de-distorted laser data and the first optimized pose data of each frame of de-distorted laser data before the current frame, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, construct a GNSS residual equation corresponding to the current frame of de-distorted laser data, and perform optimization using the GNSS residual equation to obtain absolute pose data of the current frame of de-distorted laser data; convert the current frame of de-distorted laser data to an absolute coordinate system based on the first relative relationship optimization value between the IMU and the LiDAR of the absolute pose data of the current frame of de-distorted laser data; determine the normal vector of each point in the initial map point cloud, and determine the ground points and the vertical surface points based on the gravity direction and the normal vector of each point; fit the plane equation of the ground based on the height consistency principle using at least part of the ground points; cluster at least part of the vertical surface points using the vertical surface points, and fit each clustering result to obtain the plane equation of multiple vertical surfaces; for each frame of de-distorted laser data after the first frame of de-distorted laser data, construct a ground constraint equation using the plane equation of the ground; construct a vertical surface perpendicular constraint equation using the plane equation of the vertical surface; based on the absolute pose data of the i-th frame of de-distorted laser data to be optimized, the lever value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, and the GNSS weight of the current frame of de-distorted laser data and each frame of de-distorted laser data before the current frame, construct a GNSS constraint equation corresponding to the current frame of de-distorted laser data.

[0170] The target calibration module 210 is configured to determine the second optimized attitude data of each frame of the post-distortion laser data after the first frame of the post-distortion laser data and the second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of the post-distortion laser data after the first frame of the post-distortion laser data, control the LiDAR, the IMU device, and the GNSS to reacquire the data of the three-dimensional space, control the data analysis and synchronization module to perform the time synchronization processing, replace the relative relationship calibration value with the latest second relative relationship optimization value, control the laser radar data correction module to perform the correction processing, control the registration module to perform the registration and the laser data merging operation on the initial map point cloud, control the geometric feature extraction and constraint module to perform the feature extraction and the constraint establishment, and perform the determination of the second optimized attitude data of each frame of the post-distortion laser data after the first frame of the post-distortion laser data and the second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of the post-distortion laser data after the first frame of the post-distortion laser data until the second relative relationship optimization value corresponding to the last N frames of the post-distortion laser data converges, and determine the finally obtained second relative relationship optimization value as the target relative relationship optimization value, where N is a positive integer.

[0171] The various implementations of the systems and techniques described above can be realized in digital electronic circuitry, integrated circuitry, specially designed application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include implementation in one or more computer programs that are executable and / or interpretable on a programmable system including at least one programmable processor, which can be special or general purpose, coupled to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.

[0172] Program code for carrying out methods of the present disclosure can be written in any combination of one or more programming languages. The program code can be provided to a processor or controller of a general purpose computer, special purpose computer, or other programmable data processing apparatus to produce a machine, such that the program code, when executed by the processor or controller, implements the functions / acts specified in the flowcharts and / or block diagrams. The program code can be executed entirely on a machine, partially on a machine, partially on a machine and partially on a remote machine or entirely on a remote machine or server.

[0173] In the context of the present disclosure, a machine-readable medium can be a tangible medium that contains or stores a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can include but is not limited to an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of the machine-readable storage medium will include one or more lines of electrical connections, portable computer disks, hard disk drives, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or Flash memory), optical fibers, portable compact disc read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0174] To provide for interaction with a user, the systems and techniques described here can be implemented on a computer having a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user and a keyboard and a pointing device (e.g., a mouse or a trackball) by which the user can provide input to the computer. Other kinds of devices can be used to provide for interaction with a user as well; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form, including acoustic, speech, or tactile input.

[0175] The systems and techniques described here can be implemented in a computing system that includes a back end component (e.g., as a data server), or that includes a middleware component (e.g., an application server), or that includes a front end component (e.g., a user computer having a graphical user interface or a Web browser through which a user can interact with an implementation of the systems and techniques described here), or any combination of such back end, middleware, or front end components. The components of the system can be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include a local area network (LAN), a wide area network (WAN), and the Internet.

[0176] The computer system can include clients and servers. A client and server are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a client-server relationship to each other. The server can be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0177] It should be understood that the various forms of flow shown above can be re-ordered, added to, or have steps deleted, using the flow. For example, the steps described in the present disclosure can be performed in parallel, in series, or in a different order, as long as the desired results of the technical solutions of the present disclosure can be achieved, which are not limited herein.

[0178] The specific implementation described above does not constitute a limitation on the protection scope of the present disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent replacements, and improvements made within the spirit and principles of the present disclosure shall be included in the protection scope of the present disclosure.

Claims

1. An IMU and LiDAR calibration method, characterized in that, The method comprises the following steps: S100, scanning a three-dimensional space by using a laser radar LiDAR to obtain laser data, obtaining IMU data of an IMU device bound with the laser radar, obtaining GNSS absolute coordinate data corresponding to an outdoor environment, performing time synchronization processing on the laser data, the IMU data and the GNSS absolute coordinate data, and converting the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data and target GNSS absolute coordinate data; wherein the three-dimensional space comprises an outdoor environment with rich structural information; S110, determining a relative relationship calibration value between the IMU and the LiDAR based on a mounting position of the IMU, a mounting attitude of the IMU, a mounting position of the LiDAR and a mounting attitude of the LiDAR; wherein the relative relationship calibration value comprises a calibration value of a relative pose and a calibration value of a relative displacement between the IMU and the LiDAR; S120, extracting data of a moment when the IMU device is static from the target IMU data, and determining a gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data; S130, for each frame of target laser data, determining attitude data of the IMU device by using target IMU data at the same time as the current frame of target laser data and the gravity direction, converting the current frame of target laser data into the device coordinate system of the IMU device according to the relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data, and performing rectification processing on the adjusted laser data according to the attitude data of the IMU device to obtain a frame of de-distorted laser data; wherein the attitude data of the IMU device comprises a roll angle, a pitch angle and a yaw angle; the rectification processing comprises rotation and translation; S140, constructing an initial map point cloud by using a first frame of de-distorted laser data; S150, for each frame of de-warping laser data after the first frame of de-warping laser data, extracting a feature line and a feature surface of the current frame of de-warping laser data; determining a main direction of the feature line and a normal of the feature surface; constructing a line feature residual error equation using the feature line and the main direction, and constructing a surface feature residual error equation using the feature surface and the normal; using the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of de-warping laser data before the current frame to optimize, to obtain first optimized attitude data of the current frame of de-warping laser data and a first relative relationship optimization value between the IMU and the LiDAR; using the first optimized attitude data to register the current frame of de-warping laser data, to obtain a frame of registered laser data, and merging the registered laser data into the initial map point cloud; using the first relative relationship optimization value between the IMU and the LiDAR to replace the relative relationship calibration value between the IMU and the LiDAR, and cyclically executing steps S130 and S150 to perform rectification, registration and merging of the initial map point cloud for the next frame of target laser data of the current frame, until the initial map point cloud obtained by the merging operation meets a preset condition; S160, for each frame of de-warping laser data after the first frame of de-warping laser data in the initial map point cloud, constructing a GNSS residual error equation corresponding to the current frame of de-warping laser data based on the first optimized attitude data of the current frame of de-warping laser data and each frame of de-warping laser data before the current frame, a lever arm value between the IMU and the GNSS, target GNSS absolute coordinate data corresponding to the current frame of de-warping laser data and each frame of de-warping laser data before the current frame, and using the GNSS residual error equation to optimize to obtain absolute attitude data of the current frame of de-warping laser data; converting the current frame of de-warping laser data to an absolute coordinate system based on the absolute attitude data of the current frame of de-warping laser data and the first relative relationship optimization value between the IMU and the LiDAR; determining a normal vector of each point in the initial map point cloud, and determining ground points and vertical surface points based on the gravity direction and the normal vector of each point; fitting a plane equation of the ground based on the height consistency principle using at least part of the ground points; clustering at least part of the vertical surface points, and fitting each clustering result to obtain a plurality of plane equations of vertical surfaces; S170, for each frame of de-warping laser data after the first frame of de-warping laser data, constructing a ground constraint equation using the plane equation of the ground; constructing a vertical surface constraint equation using the plane equation of the vertical surface; constructing a GNSS constraint equation corresponding to the current frame of de-warping laser data based on the absolute attitude data of the i-th frame of de-warping laser data which needs to be optimized of the current frame of de-warping laser data and each frame of de-warping laser data before the current frame, a lever arm value between the IMU and the GNSS, target GNSS absolute coordinate data corresponding to the current frame of de-warping laser data and each frame of de-warping laser data before the current frame, and a GNSS weight; S180, determine the second optimized attitude data of each frame of the post-distortion laser data after the first frame and the second relative relationship optimization value between the IMU and the LiDAR based on the ground constraint equation, the facade constraint equation and the GNSS constraint equation of each frame of the post-distortion laser data after the first frame; S190, execute the following steps in a frame cycle: reacquire the data of the three-dimensional space by using the LiDAR, the IMU device and the GNSS, replace the relative relationship calibration value with the latest second relative relationship optimization value, and perform the time synchronization processing of step S100, the registration in steps S130 and S150, the registration of the initial map point cloud and the merging operation of the laser data, steps S160-S180, until the second relative relationship optimization value corresponding to the last N frames of post-distortion laser data converges, and determine the finally obtained second relative relationship optimization value as the target relative relationship optimization value; wherein N is a positive integer; The GNSS residual error equation corresponding to the current frame of post-distortion laser data is constructed based on the current frame of post-distortion laser data, the first optimized attitude data of each frame of post-distortion laser data before the current frame, the lever value between the IMU and the GNSS, and the target GNSS absolute coordinate data corresponding to the current frame of post-distortion laser data and each frame of post-distortion laser data before the current frame, and includes: The GNSS residual error equation is as follows: ； In the formula, the first optimized pose data of the i-th frame of distortion-removed laser data to be optimized, denotes a lever arm value between the IMU and the GNSS, denotes target GNSS absolute coordinate data corresponding to the i-th frame of distortion-removed laser data, and m denotes the number of frames of distortion-removed laser data of the current frame and each frame before the current frame.

2. The method of claim 1, wherein, The line feature residual error equation is constructed based on the feature line and the main direction, and includes: The line feature residual error equation is constructed by using the following formula: ； wherein, is the line parameter of the characteristic line k, and h1 is the number of characteristic lines extracted from the i-th frame of the undistorted laser data; 1 is the attitude data of the i-th frame of the undistorted laser data to be optimized, m is the number of frames of the current frame and each frame before the current frame; R calib1 represents the first relative relationship optimization value between the IMU and the LiDAR to be optimized, is the line feature of the extracted current characteristic line; the line parameter includes the principal direction; The face feature residual error equation is constructed based on the feature face and the normal, and includes: The face feature residual error equation is constructed by using the following formula: ； wherein h2 is the number of feature planes extracted from the i-th frame of the undistorted laser data, is a plane parameter of the feature plane l, is pose data of the i-th frame of the undistorted laser data to be optimized, m is the number of frames of the current frame and each frame before the current frame; R calib1 represents a first relative relationship optimization value between the IMU and the LiDAR to be optimized, is a plane feature of the extracted current feature plane, and the plane parameter includes a normal.

3. The method of claim 1, wherein, The current frame of post-distortion laser data is converted to an absolute coordinate system based on the first relative relationship optimization value between the IMU and the LiDAR and the absolute attitude data of the current frame of post-distortion laser data, and includes: The current frame of post-distortion laser data is converted to an absolute coordinate system by using the following formula: ； In the formula, is the absolute attitude data of the i-th frame of optimized undistorted laser data, R calib2 represents the first relative relationship optimization value between the IMU and the LiDAR, represents the i-th frame of undistorted laser data.

4. The method of claim 1, wherein, The ground constraint equation is constructed based on the plane equation of the ground, and includes: The ground constraint equation is constructed by using the following formula: H）； wherein, is the absolute pose data of the i-th frame of undistorted laser data to be optimized, is the frame number of the current frame and each frame before the current frame, is the elevation of the i-th frame of undistorted laser data point, R calib3 represents the second relative relationship optimization value between the IMU and the LiDAR to be optimized, H is the fitted elevation of the ground; wherein, H is determined according to the plane equation of the ground.

5. The method of claim 1, wherein, The facade vertical constraint equation is constructed based on the plane equation of the facade, and includes: The facade vertical constraint equation is constructed by using the following formula: ； wherein, absolute attitude data of the i-th frame of undistorted laser data to be optimized, facade parameters of the j-th facade of the i-th frame of undistorted laser data, n is the number of facades of the i-th frame of undistorted laser data, point coordinates in the i-th frame of undistorted laser data, m represents the number of the current frame and each frame before the current frame, R calib3 represents the second relative relationship optimization value between the IMU and the LiDAR to be optimized.​​​​​ 6. The method of claim 1, wherein, The normal vector of each point in the initial map point cloud that meets the preset condition is determined, and includes: The normal vector of each point in the initial map point cloud is estimated based on a principal component analysis (PCA) method of neighborhood points.

7. The method of claim 1, wherein, The main direction of the feature line and the normal of the feature face are determined, and include: For the feature line, neighborhood searching is performed in the current initial map point cloud, principal component analysis is performed on the points on the current feature line by using the first neighborhood points obtained by the searching, and the direction corresponding to the maximum eigenvalue is taken as the main direction of the corresponding feature line; For the feature face, neighborhood searching is performed in the current initial map point cloud, principal component analysis is performed on the points on the current feature face by using the second neighborhood points obtained by the searching, and the direction corresponding to the minimum eigenvalue is taken as the normal of the corresponding feature face.

8. The method of claim 1, wherein, The determining the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data comprises: Filtering the data of the IMU device at the time of being static by using a Kalman filter, and determining the gravity direction in the device coordinate system corresponding to the IMU device by using the acceleration value in the filtered data.

9. A map construction method characterized by comprising: The method comprises: Calibrating the relative relationship between the IMU and the LiDAR by using the IMU and LiDAR calibration method according to any one of claims 1 to 8; Constructing a map of a corresponding region based on the relative relationship between the IMU and the LiDAR, and by using the data collected by the laser radar LiDAR and the data collected by the IMU.

10. An IMU and LiDAR calibration system, comprising: The method comprises: A laser radar, which is used to scan data in a three-dimensional space to obtain laser data; wherein the three-dimensional space comprises an outdoor environment with rich structural information; An IMU device, which is bound to the laser radar and is used to collect IMU data; A GNSS, which is used to collect GNSS absolute coordinate data of the three-dimensional space; A data analysis and synchronization module, which is used to perform time synchronization processing on the laser data, the IMU data and the GNSS absolute coordinate data, and convert the laser data and the GNSS absolute coordinate data into a device coordinate system corresponding to the IMU device to obtain target laser data, target IMU data and target GNSS absolute coordinate data; An initial calibration module, which is used to determine a relative relationship calibration value between the IMU and the LiDAR based on the installation position of the IMU, the installation attitude of the IMU, the installation position of the LiDAR and the installation attitude of the LiDAR; wherein the relative relationship calibration value comprises a calibration value of the relative pose and a calibration value of the relative displacement between the IMU and the LiDAR; A gravity direction determination module, which is used to extract data of the IMU device at the time of being static from the target IMU data, and determine the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data; A laser radar data correction module, which is used to, for each frame of target laser data, determine attitude data of the IMU device by using target IMU data at the same time as the current frame of target laser data and the gravity direction; convert the current frame of target laser data into the device coordinate system of the IMU device according to the relative relationship calibration value between the IMU and the LiDAR to obtain a frame of adjusted laser data; and perform correction processing on the adjusted laser data according to the attitude data of the IMU device to obtain a frame of de-distorted laser data; wherein the attitude data of the IMU device comprises a roll angle, a pitch angle and a yaw angle; and the correction processing comprises rotation and translation; A map initialization module, which is used to construct an initial map point cloud by using the first frame of de-distorted laser data. The registration module is configured to, for each frame of the de-distorted laser data after the first frame of the de-distorted laser data, extract a feature line and a feature surface of the current frame of the de-distorted laser data, determine a main direction of the feature line and a normal of the feature surface, construct a line feature residual error equation by using the feature line and the main direction, construct a surface feature residual error equation by using the feature surface and the normal, perform optimization by using the line feature residual error equation and the surface feature residual error equation corresponding to the current frame and each frame of the de-distorted laser data before the current frame, to obtain first optimized attitude data of the current frame of the de-distorted laser data and a first relative relationship optimization value between the IMU and the LiDAR, perform registration on the current frame of the de-distorted laser data by using the first optimized attitude data, to obtain a frame of registered laser data, and merge the registered laser data into the initial map point cloud, replace the relative relationship calibration value between the IMU and the LiDAR with the first relative relationship optimization value between the IMU and the LiDAR, and perform, by using the laser radar data correction module and the registration module, the correction, the registration and the registered laser data merging operation on the initial map point cloud in a loop manner until the initial map point cloud obtained through the merging operation meets a preset condition; The absolute coordinate processing and constraint module is configured to, for each frame of the de-distorted laser data after the first frame of the de-distorted laser data in the initial map point cloud, construct a GNSS residual error equation corresponding to the current frame of the de-distorted laser data based on the first optimized attitude data of the current frame of the de-distorted laser data and each frame of the de-distorted laser data before the current frame, a lever arm value between the IMU and the GNSS, target GNSS absolute coordinate data corresponding to the current frame of the de-distorted laser data and each frame of the de-distorted laser data before the current frame, and perform optimization by using the GNSS residual error equation, to obtain absolute attitude data of the current frame of the de-distorted laser data, convert the current frame of the de-distorted laser data to an absolute coordinate system based on the absolute attitude data of the current frame of the de-distorted laser data and the first relative relationship optimization value between the IMU and the LiDAR, determine a normal vector of each point in the initial map point cloud, and determine ground points and vertical surface points based on the gravity direction and the normal vector of each point, fit a plane equation of the ground based on the height consistency principle by using at least part of the ground points, perform clustering by using at least part of the vertical surface points, and fit each clustering result, to obtain a plurality of plane equations of vertical surfaces, construct a ground constraint equation by using the plane equation of the ground for each frame of the de-distorted laser data after the first frame of the de-distorted laser data, construct a vertical surface constraint equation by using the plane equation of the vertical surface, and construct a GNSS constraint equation corresponding to the current frame of the de-distorted laser data based on the absolute attitude data of the i-th frame of the de-distorted laser data that needs to be optimized, the lever arm value between the IMU and the GNSS, the target GNSS absolute coordinate data corresponding to the current frame of the de-distorted laser data and each frame of the de-distorted laser data before the current frame, and a GNSS weight. The target calibration module is configured to determine the second optimized attitude data of each frame of the post-distortion laser data after the first frame of the post-distortion laser data and the second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of the post-distortion laser data after the first frame of the post-distortion laser data, control the LiDAR, the IMU device, and the GNSS to reacquire the data of the three-dimensional space, control the data analysis and synchronization module to perform the time synchronization processing, replace the relative relationship calibration value with the latest second relative relationship optimization value, control the laser radar data correction module to perform the correction processing, control the registration module to perform the registration and the laser data merging operation on the initial map point cloud, control the geometric feature extraction and constraint module to perform the feature extraction and the constraint establishment, and perform the determination of the second optimized attitude data of each frame of the post-distortion laser data after the first frame of the post-distortion laser data and the second relative relationship optimization value between the IMU and the LiDAR by using the ground constraint equation and the facade constraint equation of each frame of the post-distortion laser data after the first frame of the post-distortion laser data until the second relative relationship optimization value corresponding to the last N frames of the post-distortion laser data converges, and determine the finally obtained second relative relationship optimization value as the target relative relationship optimization value, where N is a positive integer. The GNSS residual equation corresponding to the current frame of the post-distortion laser data is constructed based on the current frame of the post-distortion laser data, the first optimized attitude data of each frame of the post-distortion laser data before the current frame, the lever arm value between the IMU and the GNSS, and the target GNSS absolute coordinate data corresponding to the current frame of the post-distortion laser data and each frame of the post-distortion laser data before the current frame, and includes: The GNSS residual equation is as follows: ； In the formula, the first optimized pose data of the i-th frame of distortion-removed laser data to be optimized, denotes a lever arm value between the IMU and the GNSS, denotes target GNSS absolute coordinate data corresponding to the i-th frame of distortion-removed laser data, and m denotes the number of frames of distortion-removed laser data of the current frame and each frame before the current frame.

Images