Multi-sensor fusion real-time positioning and mapping system and method for automatic driving

Abstract

The application relates to the technical field of automatic driving, and particularly provides a multi-sensor fusion real-time positioning and mapping system and method for automatic driving, the system comprising a data acquisition module, a data preprocessing module, a state estimation module, a feature matching module and a mapping module; the method comprises the following steps: S1, acquiring point cloud data; S2, acquiring IMU data; S3, performing motion compensation on the point cloud data; S4, outputting an odometer result; S5, finding matched features in an iterative voxel space and outputting the features; and S6, constructing a global point cloud map; the application utilizes the information of a laser radar and an inertial measurement unit (IMU) to realize the perception of an unknown environment and the positioning function of an automatic driving vehicle in the environment, the laser radar can provide high-precision point cloud map data, the IMU provides high-frequency posture and acceleration information of the automatic driving vehicle, and the tightly coupled laser radar and IMU information is used to realize the real-time output of accurate positioning and map information.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, specifically to a multi-sensor fusion real-time localization and mapping system and method for autonomous driving. Background Technology

[0002] The development of the automotive industry is a significant indicator of the development level of the transportation industry and my country's economic development. While traditional automobiles once enjoyed a period of rapid growth, they have now entered a period of decline. To seek new breakthroughs, the automotive industry has begun to focus on new technological fields, such as the Internet of Things (IoT) and artificial intelligence (AI), deeply integrating these new technologies with the automotive industry to develop autonomous driving technology. The emergence of autonomous driving technology means that cars can achieve autonomous perception, decision-making, and driving without human intervention, further improving the safety and efficiency of transportation and injecting new vitality into my country's economic development. SLAM (Simultaneous Localization and Mapping) is a crucial component of autonomous driving technology. In autonomous driving, SLAM technology is used to enable vehicles to autonomously locate and map themselves in unknown environments. Through SLAM, vehicles can acquire information about their surroundings using sensors such as LiDAR and cameras, and fuse this information with the vehicle's own location information to create a map. During subsequent driving, the vehicle can use the map for localization and path planning, achieving autonomous driving. Therefore, it can be said that SLAM technology is a key technology for achieving autonomous perception, localization, and navigation in autonomous driving technology. Meanwhile, as autonomous driving technology continues to develop and mature, SLAM technology is also constantly being improved and optimized to better adapt to complex traffic environments and road conditions, further enhancing the safety and reliability of autonomous driving.

[0003] The SLAM technology currently used in the market has the following shortcomings:

[0004] 1) Since the sampling time of each point of the lidar is different, and the lidar is in motion, motion distortion will occur, which will affect the accuracy and reliability of the lidar data.

[0005] 2) The lack of closed-loop detection after real-time positioning and mapping affects the accuracy of the mapping. Summary of the Invention

[0006] In view of the shortcomings of the prior art described above, the purpose of this invention is to provide a multi-sensor fusion real-time localization and mapping system and method for autonomous driving, which solves the problems in the prior art where motion distortion caused by the LiDAR in motion affects the accuracy and reliability of the LiDAR data acquisition, and the problem of not performing closed-loop detection after real-time localization and mapping.

[0007] To achieve the above and other related objectives, this invention provides a multi-sensor fusion real-time localization and mapping method for autonomous driving, characterized in that it includes a multi-sensor fusion real-time localization and mapping system for autonomous driving, the system comprising:

[0008] The data acquisition module uses multiple sensors to collect lidar point cloud data and IMU data of the actual scene during vehicle movement.

[0009] The data preprocessing module is used to perform downsampling and noise removal processing on point cloud data, and to perform pre-integration processing on IMU data.

[0010] The state estimation module includes a motion compensation unit, a residual calculation unit, a state update unit, a convergence unit, and an odometer output unit.

[0011] The feature matching module includes an incremental voxel unit and a point cloud insertion unit;

[0012] The mapping module includes a map building unit and a loop closure detection unit;

[0013] The method includes the following steps:

[0014] S1. Real-time acquisition of point cloud data of the current environment location using LiDAR, and downsampling and noise removal processing of the acquired point cloud data; the downsampling processing adopts a combination of uniform downsampling and voxel downsampling. Uniform downsampling divides the point cloud into grids or voxels, and selects a point in each grid or voxel as the sampling result to maintain the overall distribution of the point cloud uniformly; voxel downsampling divides the three-dimensional space into three-dimensional square voxels. Each voxel may contain several points or no points. The downsampling approach is to check whether there are points in each voxel. If there are, a point is used to replace the set of points in the voxel.

[0015] S2. Extract IMU data from the vehicle's integrated navigation system and perform pre-integration processing on the data to obtain the IMU's motion model;

[0016] S3. Fuse point cloud data and IMU data, and use the pose estimated by IMU pre-integration to compensate the motion of each point in the point cloud data to time, so as to obtain the sampling data of the laser point cloud at the same time, i.e. time. Then, use motion compensation to estimate the pose of each point relative to the end time of the laser radar scan by using the IMU measurement value to combine the preprocessed point cloud data collected by the laser radar and the IMU data extracted from the vehicle-mounted integrated navigation.

[0017] S4. Calculate the minimum residual using a nonlinear optimization method and update the state. When the iteration result reaches the convergence threshold, output the odometer result.

[0018] S5. Define a data structure voxel space to store point cloud data, insert keyframe point cloud data into the voxel space, traverse the voxel space through k iterations of nearest neighbor search to find matching features, and output the results to the mapping module.

[0019] S6. Construct a global point cloud map based on odometry results and local feature maps. When a loop is detected in the environment, optimize the global map using a loop closure detection algorithm to output an accurate point cloud map that is consistent with the global map. The odometry results are point cloud data obtained by laser inertial odometry and transformed into the world coordinate system. The loop closure detection algorithm includes the following steps:

[0020] B1. Input Keyframes, which are candidate keyframes; and new_keyframes, which are new keyframes.

[0021] B2. Search and traverse each keyframe to find the matching relationship between it and the newly added frame, thereby identifying potential loop closures;

[0022] B3, 1) Calculate the pose estimate of the newly added keyframe and output the quaternion;

[0023] 2) Calculate the pose estimation of candidate keyframes and output quaternions;

[0024] 3) Calculate the relative pose estimation of the new keyframe pose and the candidate keyframe pose;

[0025] 4) Clear the z-axis offset of the relative pose estimation to zero;

[0026] 5) Use the ICP algorithm to match candidate frames and add new keyframes;

[0027] 6) Obtain the matching score;

[0028] 7) If the match does not converge or the match score is lower than the current best score, skip the newly added keyframe;

[0029] 8) If the best score is less than the loop closure detection threshold, a loop closure is detected;

[0030] B4. Output the keyframe matching relationship that satisfies the closed loop.

[0031] In one embodiment of the present invention, in step S2, the IMU data includes a timestamp and six values: linear acceleration and angular velocity in the x, y, and z axes at the corresponding times.

[0032] In one embodiment of the present invention, in step S3, the motion compensation algorithm is simplified to the following formula:

[0033] (1)

[0034] Where sf represents "starting from"; the relative pose of the point after motion compensation is defined as... This point Projected to the end of the scan We can obtain:

[0035] (2).

[0036] In one embodiment of the present invention, in step S4, the linear optimization method includes tightly coupling information from the lidar and the IMU and using an extended Kalman filter, i.e., an optimization method based on EKF, to estimate the motion state of the vehicle.

[0037] In one embodiment of the present invention, the EKF optimization method includes the following steps:

[0038] A1. In the EKF, the state of the system is represented as a vector, and the dynamic process and measurement model of the system are both represented as nonlinear functions;

[0039] A2. The EKF performs a Taylor expansion of these nonlinear functions at the current state point and linearizes them using the first derivative, thereby obtaining a linear system;

[0040] A3. Then define the residual of the system as the distance from the feature point to the feature plane and the distance from the feature point to the feature line;

[0041] A4. Next, perform iterative update calculations. When convergence occurs, that is, when the nonlinear system becomes a linear system after iterative calculations, output the predicted state value at the current moment, which is the odometry pose estimate.

[0042] In one embodiment of the present invention, in step S5, an incremental voxel data structure is used to store the point cloud data after state estimation processing, and k nearest neighbor searches are used in the voxel space to find similar features for local feature matching.

[0043] As described above, the multi-sensor fusion real-time localization and mapping method for autonomous driving of the present invention has the following beneficial effects:

[0044] 1. This invention employs a motion compensation algorithm to estimate the pose of each point relative to the end time of the LiDAR scan by using the IMU measurements from the preprocessed point cloud data acquired by the LiDAR and the IMU data extracted from the vehicle-mounted integrated navigation system. This method determines the precise sampling time of each point and maps all points to the end time of the scan, thus treating all points in the scanning process as points sampled simultaneously. The advantage of this approach is that it can more accurately estimate the pose of the LiDAR, improve the accuracy and reliability of the LiDAR data, and achieve the purpose of motion compensation.

[0045] 2. The present invention also employs a loop closure detection algorithm. The loop closure detection algorithm first extracts point and surface features of the surrounding environment through a feature extraction algorithm, and then uses a feature matching algorithm to match the environment to identify looped environments, reduce accumulated errors, and improve the consistency and accuracy of map building.

[0046] 3. This invention utilizes information from LiDAR and Inertial Measurement Unit (IMU) to achieve perception of unknown environments and localization of autonomous vehicles within those environments. LiDAR can provide high-precision point cloud map data, while IMU provides high-frequency attitude and acceleration information of autonomous vehicles. By tightly coupling LiDAR and IMU information, positioning and map information are output in real time, enabling real-time construction of park maps, providing important reference information for patrols, and real-time positioning of the vehicle's own location. This solves the problem of excessive positioning errors caused by accumulated position deviations and also compensates for the positioning output problem of GPS when the signal is blocked. Attached Figure Description

[0047] Figure 1 The diagram shown is a block diagram of the multi-sensor fusion real-time localization and mapping system for autonomous driving disclosed in Example 1.

[0048] Figure 2 The image shown is a fusion mapping effect diagram of the multi-sensor fusion real-time localization and mapping method for autonomous driving disclosed in Example 2.

[0049] Component designation explanation

[0050] Data acquisition module ①; data preprocessing module ②; state estimation module ③; feature matching module ④; mapping module ⑤. Detailed Implementation

[0051] The following specific embodiments illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification.

[0052] Please see Figures 1 to 2It should be understood that the structures, proportions, sizes, etc., illustrated in the accompanying drawings are only for the purpose of assisting those skilled in the art in understanding and reading the content disclosed in the specification, and are not intended to limit the conditions under which the present invention can be implemented. Therefore, they have no substantial technical significance. Any modifications to the structure, changes in the proportions, or adjustments to the size, without affecting the effects and objectives that the present invention can produce, should still fall within the scope of the technical content disclosed in the present invention.

[0053] Example 1, please refer to Figure 1 This embodiment provides a multi-sensor fusion real-time localization and mapping system for autonomous driving, including a data acquisition module 1, a data preprocessing module 2, a state estimation module 3, a feature matching module 4, and a mapping module 5. The data acquisition module 1 uses multiple sensors to collect LiDAR point cloud data and IMU data of the actual scene during vehicle movement. The data preprocessing module 2 is used to downsample and remove noise from the point cloud data, and to pre-integrate the IMU data. The state estimation module 3 includes a motion compensation unit, a residual calculation unit, a state update unit, a convergence unit, and a mileage output unit. The feature matching module 4 includes an incremental voxel unit and a point cloud insertion unit. The mapping module 5 includes a map building unit and a loop closure detection unit.

[0054] Example 2 provides a multi-sensor fusion real-time localization and mapping method for autonomous driving, including the multi-sensor fusion real-time localization and mapping system for autonomous driving described in Example 1, comprising the following steps:

[0055] S1. A lidar system is used to collect point cloud data of the current environmental location in real time. The collected point cloud data is then downsampled and noise removal is performed. The downsampling process combines uniform downsampling and voxel downsampling. Uniform downsampling divides the point cloud into grids or voxels, and selects a point in each grid or voxel as the sampling result to maintain the overall distribution of the point cloud. Voxel downsampling divides the three-dimensional space into solid squares (voxels). Each voxel may contain several points or none. The downsampling approach is to check if a point exists in each voxel. If so, a point is used to replace the set of points within the voxel. This sampling point can typically be the average coordinates of all points in the voxel (centroid), or it can be the center point or the point closest to the center point.

[0056] S2. The IMU data is retrieved from the vehicle's integrated navigation system and pre-integrated to obtain the IMU's motion model. The IMU data includes a timestamp and six values: linear acceleration and angular velocity in the x, y, and z axes at the corresponding times. This step pre-integrates these six values ​​to obtain the IMU's motion model for motion state estimation. The pre-integrated IMU data is used for state estimation because it provides more accurate IMU observation data and reduces the impact of IMU noise and offset on the laser inertial odometry, thereby improving the accuracy and stability of the laser inertial odometry.

[0057] S3. Since the sampling time of each point on the lidar is different, and the lidar is in motion, motion distortion will occur. This step fuses point cloud data and IMU data, and uses the pose estimated by IMU pre-integration to compensate the motion of each point in the point cloud data to the time, obtaining the sampling data of the lidar point cloud at the same time, i.e., time. The motion compensation algorithm is simplified as follows:

[0058] (1)

[0059] Where sf represents "starting from"; the relative pose of the point after motion compensation is defined as... This point Projected to the end of the scan We can obtain:

[0060] (2).

[0061] Please see Figure 2 This invention employs a motion compensation algorithm to estimate the pose of each point relative to the end time of the LiDAR scan by using the IMU measurements from the pre-processed point cloud data collected by the LiDAR and the IMU data extracted from the vehicle-mounted integrated navigation system. This method determines the precise sampling time of each point and maps all points to the end time of the scan, thus treating all points in the scanning process as points sampled simultaneously. The advantage of this approach is that it can more accurately estimate the pose of the LiDAR, improve the accuracy and reliability of the LiDAR data, and achieve the purpose of motion compensation.

[0062] S4. A nonlinear optimization method is used to calculate the minimum residual and perform state updates. When the iteration result reaches the convergence threshold, the odometer result is output. The linear optimization method includes tightly coupled information from the lidar and IMU, and uses an extended Kalman filter (EKF) to estimate the vehicle's motion state. The EKF optimization method includes the following steps:

[0063] A1. In the EKF, the state of the system is represented as a vector, and the dynamic process and measurement model of the system are both represented as nonlinear functions;

[0064] A2. The EKF performs a Taylor expansion of these nonlinear functions at the current state point and linearizes them using the first derivative, thereby obtaining a linear system;

[0065] A3. Then define the residual of the system as the distance from the feature point to the feature plane and the distance from the feature point to the feature line;

[0066] A4. Next, perform iterative update calculations. When convergence occurs, that is, when the nonlinear system becomes a linear system after iterative calculations, output the predicted state value at the current moment, which is the odometry pose estimate.

[0067] S5. Define a voxel space data structure to store point cloud data. Insert keyframe point cloud data into the voxel space. Traverse the voxel space through k iterations of nearest neighbor search to find matching features. Output the results to the mapping module 5. This step uses an incremental voxel data structure to store the point cloud data after state estimation processing. In the voxel space, use k iterations of nearest neighbor search to find similar features for local feature matching.

[0068] S6. Construct a global point cloud map based on odometry results and local feature maps. The odometry results are point cloud data obtained by laser inertial odometry and transformed into the world coordinate system. When a loop is detected in the environment, a loop closure detection algorithm is used to optimize the global map and output an accurate point cloud map that is consistent with the global map. The loop closure detection algorithm includes the following steps:

[0069] B1. Input Keyframes, which are candidate keyframes; and new_keyframes, which are new keyframes.

[0070] B2. Search and traverse each keyframe to find the matching relationship between it and the newly added frame, thereby identifying potential loop closures;

[0071] B3, 1) Calculate the pose estimate of the newly added keyframe and output the quaternion;

[0072] 2) Calculate the pose estimation of candidate keyframes and output quaternions;

[0073] 3) Calculate the relative pose estimation of the new keyframe pose and the candidate keyframe pose;

[0074] 4) Clear the z-axis offset of the relative pose estimation to zero;

[0075] 5) Use the ICP algorithm to match candidate frames and add new keyframes;

[0076] 6) Obtain the matching score;

[0077] 7) If the match does not converge or the match score is lower than the current best score, skip the newly added keyframe;

[0078] 8) If the best score is less than the loop closure detection threshold, a loop closure is detected;

[0079] B4. Output the keyframe matching relationship that satisfies the closed loop.

[0080] Loop closure detection is a crucial step in real-time localization and mapping (RTL) systems. It primarily identifies areas where vehicles have passed through similar or identical environments. When a vehicle passes through a previously explored area, loop closure detection matches this area with previously explored areas, thus more accurately determining the vehicle's position and the surrounding map. This invention also employs a loop closure detection algorithm. This algorithm first extracts point and surface features from the surrounding environment using a feature extraction algorithm, and then uses a feature matching algorithm to identify loop closures, reducing accumulated errors and improving the consistency and accuracy of the generated map.

[0081] In summary, this invention utilizes information from LiDAR and an Inertial Measurement Unit (IMU) to achieve perception of unknown environments and localization of autonomous vehicles within those environments. LiDAR provides high-precision point cloud map data, while the IMU provides high-frequency attitude and acceleration information for the autonomous vehicle. By tightly coupling LiDAR and IMU information, real-time localization and map information are output, enabling real-time map construction of the park, providing crucial reference information for patrols, and real-time self-positioning. This solves the problem of excessive positioning errors caused by accumulated positional deviations and compensates for the positioning output issues of GPS when signals are blocked. Therefore, this invention effectively overcomes the various shortcomings of existing technologies and has high industrial application value.

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

Claims

1. A multi-sensor fusion real-time localization and mapping method for autonomous driving, characterized in that, This includes a multi-sensor fusion real-time localization and mapping system for autonomous driving, the system comprising: The data acquisition module uses multiple sensors to collect lidar point cloud data and IMU data of the actual scene during vehicle movement. The data preprocessing module is used to perform downsampling and noise removal processing on point cloud data, and to perform pre-integration processing on IMU data. The state estimation module includes a motion compensation unit, a residual calculation unit, a state update unit, a convergence unit, and an odometer output unit. The feature matching module includes an incremental voxel unit and a point cloud insertion unit; The mapping module includes a map building unit and a loop closure detection unit; The method includes the following steps: S1. Real-time acquisition of point cloud data of the current environment location using LiDAR, and downsampling and noise removal processing of the acquired point cloud data; the downsampling processing adopts a combination of uniform downsampling and voxel downsampling. Uniform downsampling divides the point cloud into grids or voxels, and selects a point in each grid or voxel as the sampling result to maintain the overall distribution of the point cloud uniformly; voxel downsampling divides the three-dimensional space into three-dimensional square voxels. Each voxel may contain several points or no points. The downsampling approach is to check whether there are points in each voxel. If there are, a point is used to replace the set of points in the voxel. S2. Extract IMU data from the vehicle's integrated navigation system and perform pre-integration processing on the data to obtain the IMU's motion model; S3. Fuse point cloud data and IMU data, and use the pose estimated by IMU pre-integration to compensate the motion of each point in the point cloud data to time, so as to obtain the sampling data of the laser point cloud at the same time, i.e. time. Then, use motion compensation to estimate the pose of each point relative to the end time of the laser radar scan by using the IMU measurement value to combine the preprocessed point cloud data collected by the laser radar and the IMU data extracted from the vehicle-mounted integrated navigation. S4. Calculate the minimum residual using a nonlinear optimization method and update the state. When the iteration result reaches the convergence threshold, output the odometer result. S5. Define a data structure voxel space to store point cloud data, insert keyframe point cloud data into the voxel space, traverse the voxel space through k iterations of nearest neighbor search to find matching features, and output the results to the mapping module. S6. Construct a global point cloud map based on odometry results and local feature maps. When a loop is detected in the environment, optimize the global map using a loop closure detection algorithm to output an accurate point cloud map that is consistent with the global map. The odometry results are point cloud data obtained by laser inertial odometry and transformed into the world coordinate system. The loop closure detection algorithm includes the following steps: B1. Input Keyframes, which are candidate keyframes; and new_keyframes, which are new keyframes. B2. Search and traverse each keyframe to find the matching relationship between it and the newly added frame, thereby identifying potential loop closures; B3, 1) Calculate the pose estimate of the newly added keyframe and output the quaternion; 2) Calculate the pose estimation of candidate keyframes and output quaternions; 3) Calculate the relative pose estimation of the new keyframe pose and the candidate keyframe pose; 4) Clear the z-axis offset of the relative pose estimation to zero; 5) Use the ICP algorithm to match candidate frames and add new keyframes; 6) Obtain the matching score; 7) If the match does not converge or the match score is lower than the current best score, skip the newly added keyframe; 8) If the best score is less than the loop closure detection threshold, a loop closure is detected; B4. Output the keyframe matching relationship that satisfies the closed loop.

2. The multi-sensor fusion real-time localization and mapping method for autonomous driving according to claim 1, characterized in that: In step S2, the IMU data includes a timestamp and six values: linear acceleration and angular velocity in the x, y, and z axes at the corresponding times.

3. The multi-sensor fusion real-time localization and mapping method for autonomous driving according to claim 1, characterized in that: In step S3, the motion compensation algorithm is simplified to the following formula: (1) in sf Indicates "starting from"; defines the relative pose of the point after motion compensation as... This point Projected to the end of the scan We can obtain: (2).

4. The multi-sensor fusion real-time localization and mapping method for autonomous driving according to claim 1, characterized in that: In step S4, the linear optimization method includes tightly coupling information from the lidar and IMU and using an extended Kalman filter, i.e., an EKF optimization method, to estimate the vehicle's motion state.

5. The multi-sensor fusion real-time localization and mapping method for autonomous driving according to claim 4, characterized in that, The optimization method for EKF includes the following steps: A1. In the EKF, the state of the system is represented as a vector, and the dynamic process and measurement model of the system are both represented as nonlinear functions; A2. The EKF performs a Taylor expansion of these nonlinear functions at the current state point and linearizes them using the first derivative, thereby obtaining a linear system; A3. Then define the residual of the system as the distance from the feature point to the feature plane and the distance from the feature point to the feature line; A4. Next, perform iterative update calculations. When convergence occurs, that is, when the nonlinear system becomes a linear system after iterative calculations, output the predicted state value at the current moment, which is the odometry pose estimate.

6. The multi-sensor fusion real-time localization and mapping method for autonomous driving according to claim 1, characterized in that: In step S5, an incremental voxel data structure is used to store the point cloud data after state estimation. In the voxel space, k nearest neighbor searches are used to find similar features for local feature matching.

Images