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

Abstract

The invention 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, and the system comprises a data collection module, a data preprocessing module, a state estimation module, a feature matching module, and a mapping module. The method comprises the following steps: S1, collecting point cloud data; s2, IMU data are collected; s3, performing motion compensation on the point cloud data; s4, outputting a speedometer result; s5, iterating the voxel space to find matched features and outputting the matched features; s6, constructing a global point cloud map; according to the invention, the sensing of an unknown environment and the positioning function of the autonomous vehicle in the environment are realized by using the information of the laser radar and the IMU, the laser radar can provide high-precision point cloud map data, and the IMU provides high-frequency attitude and acceleration information of the autonomous vehicle. Accurate positioning and map information is output in real time by using a tight coupling laser radar and IMU information mode.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a multi-sensor fusion real-time positioning and mapping system and method for autonomous driving. Background Art

[0002] The development of the automobile industry is an important indicator of the development level of the transportation industry and the economic development of my country. Although traditional automobiles have experienced a glorious period of development, they have now entered a period of decline. In order to seek new development breakthroughs, the automobile industry has begun to focus on new technical fields, such as Internet of Things technology, artificial intelligence, etc., and deeply integrate these new technologies with the automobile industry, thus developing autonomous driving technology. The emergence of autonomous driving technology means that cars can achieve autonomous perception, decision-making and driving without driving, further improving the safety and efficiency of transportation, and injecting new vitality into my country's economic development. SLAM (Simultaneous Localization and Mapping), that is, simultaneous positioning and mapping technology, is an important part of autonomous driving technology. In autonomous driving, SLAM technology is used to achieve autonomous positioning and mapping of vehicles in unknown environments. Through SLAM technology, vehicles can obtain information about the surrounding environment through sensors such as laser radar and cameras, and integrate this information with the vehicle's own location information to establish a map. In the subsequent driving process, the vehicle can be positioned and routed through the map to achieve autonomous driving. Therefore, it can be said that SLAM technology is a key technology for achieving autonomous perception, positioning and navigation in unmanned driving technology. At the same time, with the continuous development and maturity of unmanned driving technology, SLAM technology is also constantly being improved and optimized to better adapt to complex traffic environments and road conditions, and further improve the safety and reliability of unmanned 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, affecting the accuracy and reliability of the LiDAR data collected.

[0005] 2) No closed-loop detection is performed after real-time positioning and mapping, which affects the accuracy of mapping. Summary of the invention

[0006] In view of the shortcomings of the prior art mentioned above, an object of the present invention is to provide a multi-sensor fusion real-time positioning and mapping system and method for autonomous driving, which is used to solve the problem in the prior art that the laser radar in motion will produce motion distortion affecting the accuracy and reliability of the laser radar data collected, and the problem that closed-loop detection is not performed after real-time positioning and mapping.

[0007] To achieve the above objectives and other related objectives, the present invention provides a multi-sensor fusion real-time positioning and mapping system for autonomous driving, comprising:

[0008] A data acquisition module, wherein the data acquisition module uses multiple sensors to collect lidar point cloud data and IMU data of actual scenes during vehicle movement;

[0009] A data preprocessing module, which is used to downsample and remove noise from point cloud data, and pre-integrate IMU data;

[0010] A state estimation module, the state estimation module comprising a motion compensation unit, a residual calculation unit, a state update unit, a convergence unit, and a mileage output unit;

[0011] A feature matching module, the feature matching module comprising an incremental voxel unit and a point cloud insertion unit;

[0012] A mapping module, wherein the mapping module includes a map construction unit and a closed-loop detection unit.

[0013] A multi-sensor fusion real-time positioning and mapping method for autonomous driving, including the multi-sensor fusion real-time positioning and mapping system for autonomous driving, comprising the following steps:

[0014] S1. Use laser radar to collect point cloud data of the current environment in real time, and perform downsampling and noise removal on the collected point cloud data;

[0015] S2, taking out the IMU data from the vehicle-mounted integrated navigation and performing pre-integration processing on the data to obtain the motion model of the IMU;

[0016] S3, fusing point cloud data and IMU data, using the IMU pre-integration estimated position and posture, compensating the motion of each point in the point cloud data to time, and obtaining the sampling data of the laser point cloud at the same time, that is, time;

[0017] S4, using nonlinear optimization method to calculate the minimum residual and update the state, when the iteration result reaches the convergence threshold, output the odometer result;

[0018] S5. Define a data structure voxel space for storing point cloud data, insert key frame point cloud data into the voxel space, traverse the voxel space to find matching features through k-iterative nearest neighbor search, and output the results to the mapping module;

[0019] S6. Build a global point cloud map based on the odometer results and local feature maps. When a loop is detected in the environment, use the closed-loop detection algorithm to optimize the global map and output an accurate point cloud map that is consistent with the global map.

[0020] In one embodiment of the present invention, in step S2, the IMU data includes a timestamp and six values ​​of linear acceleration and angular velocity in the x, y, and z axis directions at the corresponding time.

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

[0022]

[0023] Where sf means "starting from"; the relative pose of the point after motion compensation is defined as The point Projection to the end time of scanning t k We can get:

[0024]

[0025] In one embodiment of the present invention, in step S4, the linear optimization method includes tightly coupling the information of the laser radar and the IMU and using an optimization method of an extended Kalman filter, namely, EKF, to estimate the motion state of the vehicle.

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

[0027] 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 represented as nonlinear functions;

[0028] A2. The EKF performs Taylor expansion on these nonlinear functions at the current state point and linearizes them using the first-order derivative to obtain a linear system;

[0029] 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;

[0030] A4. Then perform iterative update calculations. When convergence occurs, that is, when the nonlinear system is converted into a linear system after iterative calculations, the predicted state value at the current moment, i.e., the odometer pose estimate, is output.

[0031] In one embodiment of the present invention, in step S5, an incremental voxel data structure is used to store 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.

[0032] In one embodiment of the present invention, in step S6, the odometer result is point cloud data obtained by using a laser inertial odometer to obtain posture transformation and converted into data in a world coordinate system.

[0033] In one embodiment of the present invention, in step S6, the closed-loop detection algorithm includes the following steps:

[0034] B1. Input Keyframes, i.e. candidate keyframes, and new_keyframes, i.e. newly added keyframes;

[0035] B2, / / Search for the matching relationship between each frame of the traversed key frame and the newly added frame, so as to determine the potential closed loop;

[0036] B3, 1) Calculate the pose estimate of the newly added key frame and output the quaternion;

[0037] 2) Calculate the pose estimate of the candidate keyframe and output the quaternion;

[0038] 3) Calculate the relative pose estimation of the newly added key frame pose and the candidate key frame pose;

[0039] 4) The z-axis offset of the relative pose estimation is cleared;

[0040] 5) Use the ICP algorithm to match candidate frames and newly added key frames;

[0041] 6) Obtain matching score;

[0042] 7) If the match does not converge or the match score is not as good as the current best score, skip the newly added key frame;

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

[0044] B4. Output the key frame matching relationship that satisfies the closed loop.

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

[0046] 1. The present invention adopts a motion compensation algorithm, and uses the measurement value of the IMU to estimate the position and posture of each point in the laser radar scanning process relative to the scanning end time by using the point cloud data collected by the preprocessed laser radar and the IMU data taken out from the vehicle-mounted integrated navigation; in this way, the precise sampling time of each point is determined, and all points are mapped to the scanning end time point, so that all points in the scanning process are regarded as points sampled at the same time; the advantage of this is that the position and posture of the laser radar can be estimated more accurately, and the accuracy and reliability of the laser radar data can be improved to achieve the purpose of motion compensation.

[0047] 2. The present invention also adopts a closed-loop detection algorithm. The closed-loop 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 perform environmental matching to identify the loop environment, reduce cumulative errors, and improve the consistency and accuracy of map establishment.

[0048] 3. The present invention utilizes the information of LiDAR and Inertial Measurement Unit (IMU) to realize the perception of unknown environment and the positioning function of autonomous driving vehicles in the environment. LiDAR can provide high-precision point cloud map data, and IMU provides high-frequency attitude and acceleration information of autonomous driving vehicles. The positioning and map information are output in real time by tightly coupling LiDAR and IMU information. It can build a map of the park in real time, provide important reference information for patrols, and locate its own position in real time, solving the problem of excessive positioning error caused by accumulated position deviation, and making up for the positioning output problem of GPS when the signal is blocked. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Shown is a block diagram of the multi-sensor fusion real-time positioning and mapping system for autonomous driving disclosed in Example 1.

[0050] Figure 2 Shown is a fusion mapping effect diagram of the multi-sensor fusion real-time positioning and mapping method for autonomous driving disclosed in Example 2.

[0051] Component number description

[0052] Data acquisition module①; data preprocessing module②; state estimation module③; feature matching module④; mapping module⑤. DETAILED DESCRIPTION

[0053] The following is a description of the implementation of the present invention by means of specific embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification.

[0054] See also Figure 1 to Figure 2 It should be noted that the structures, proportions, sizes, etc. illustrated in the drawings in this specification are only used to match the contents disclosed in the specification for people familiar with this technology to understand and read, and are not used to limit the conditions under which the present invention can be implemented. Therefore, they have no substantial technical significance. Any structural modification, change in proportion or adjustment of size should still fall within the scope of the technical contents disclosed in the present invention without affecting the effects and purposes that can be achieved by the present invention.

[0055] Example 1, please refer to Figure 1The present embodiment provides a multi-sensor fusion real-time positioning and mapping system for autonomous driving, comprising 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 laser radar point cloud data and IMU data of actual scenes during vehicle movement; the data preprocessing module 2 is used to downsample and remove noise from the point cloud data, and 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 construction unit and a closed-loop detection unit.

[0056] Embodiment 2: This embodiment provides a multi-sensor fusion real-time positioning and mapping method for autonomous driving, including the multi-sensor fusion real-time positioning and mapping system for autonomous driving described in Embodiment 1, including the following steps:

[0057] S1. Use laser radar to collect point cloud data of the current environment position in real time, and perform downsampling and noise removal processing on the collected 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 uniformly maintain the overall distribution of the point cloud; voxel downsampling divides the three-dimensional space into three-dimensional grids (voxels). There may be several points in each voxel, or there may be no points. The idea of ​​downsampling is to check whether there are points in each voxel. If so, a point is used to replace the point set in the voxel. Usually, this sampling point can be the average value (center of mass) of the coordinates of all points in the voxel, or it can be the center point or the point closest to the center point.

[0058] S2. The IMU data is taken out from the vehicle-mounted integrated navigation and pre-integrated to obtain the motion model of the IMU; the IMU data includes a timestamp and six values ​​of linear acceleration and angular velocity in the x, y, and z axis directions at the corresponding time. This step pre-integrates these six values ​​to obtain the motion model of the IMU in order to estimate the motion state; the result of the IMU pre-integration is used during state estimation because the result can provide more accurate IMU observation data and can reduce the influence of the IMU noise and offset on the laser inertial odometer, thereby improving the accuracy and stability of the laser inertial odometer.

[0059] S3. Since the sampling time of each point of the laser radar is different and the laser radar is in motion, motion distortion will occur. This step fuses the point cloud data and the IMU data, and uses the posture estimated by the IMU pre-integration to compensate the motion of each point of the point cloud data to the moment, and obtains the sampling data of the laser point cloud at the same moment, that is, the moment. The process of the motion compensation algorithm is simplified to the following formula:

[0060]

[0061] Where sf means "starting from". The relative pose of the point after motion compensation is defined as The point Projection to the end time of scanning t k We can get:

[0062]

[0063] See also Figure 2 The present invention adopts a motion compensation algorithm, and uses the point cloud data collected by the preprocessed laser radar and the IMU data taken out from the vehicle-mounted integrated navigation to estimate the posture of each point relative to the scanning end time during the laser radar scanning process using the measurement value of the IMU; in this way, the precise sampling time of each point is determined, and all points are mapped to the scanning end time point, so that all points in the scanning process are regarded as points sampled at the same time; the advantage of this is that the posture of the laser radar can be estimated more accurately, and the accuracy and reliability of the laser radar data can be improved to achieve the purpose of motion compensation.

[0064] S4. Calculate the minimum residual and perform state update using a nonlinear optimization method. When the iterative result reaches a convergence threshold, output the odometer result. The linear optimization method includes tightly coupling the information of the laser radar and the IMU and using an extended Kalman filter, i.e., an EKF optimization method, to estimate the motion state of the vehicle. The EKF optimization method includes the following steps:

[0065] 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 represented as nonlinear functions;

[0066] A2. The EKF performs Taylor expansion on these nonlinear functions at the current state point and linearizes them using the first-order derivative to obtain a linear system;

[0067] 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;

[0068] A4. Then perform iterative update calculations. When convergence occurs, that is, when the nonlinear system is converted into a linear system after iterative calculations, the predicted state value at the current moment, i.e., the odometer pose estimate, is output.

[0069] S5. Define a data structure voxel space to store point cloud data, insert key frame point cloud data into the voxel space, traverse the voxel space through k-iterative nearest neighbor search to find matching features, and output the results to the mapping module 5; this step uses an incremental voxel data structure to store point cloud data after state estimation processing, and uses k-times nearest neighbor search in the voxel space to find similar features for local feature matching.

[0070] S6. Construct a global point cloud map based on the odometer result and the local feature map. The odometer result is point cloud data obtained by using a laser inertial odometer to obtain data transformed into a world coordinate system. When a loop is detected in the environment, the global map is optimized using a closed-loop detection algorithm to output an accurate point cloud map that is consistent with the global map. The closed-loop detection algorithm includes the following steps:

[0071] B1. Input Keyframes, i.e. candidate keyframes, and new_keyframes, i.e. newly added keyframes;

[0072] B2, / / Search for the matching relationship between each frame of the traversed key frame and the newly added frame, so as to determine the potential closed loop;

[0073] B3, 1) Calculate the pose estimate of the newly added key frame and output the quaternion;

[0074] 2) Calculate the pose estimate of the candidate keyframe and output the quaternion;

[0075] 3) Calculate the relative pose estimation of the newly added key frame pose and the candidate key frame pose;

[0076] 4) The z-axis offset of the relative pose estimation is cleared;

[0077] 5) Use the ICP algorithm to match candidate frames and newly added key frames;

[0078] 6) Obtain matching score;

[0079] 7) If the match does not converge or the match score is not as good as the current best score, skip the newly added key frame;

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

[0081] B4. Output the key frame matching relationship that satisfies the closed loop.

[0082] Closed loop detection is an important part of real-time positioning and mapping. It is mainly used to identify vehicles passing through the same or similar environmental areas. When a vehicle passes through an area that has been explored before, loop detection will match this area with the previously explored area, thereby more accurately determining the vehicle's position and surrounding environment map. The present invention also uses a closed loop detection algorithm. The closed loop 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 perform environmental matching to identify the loop environment, reduce cumulative errors, and improve the consistency and accuracy of map establishment.

[0083] In summary, the present invention uses the information of laser radar and inertial measurement unit (IMU) to realize the perception of unknown environment and the positioning function of autonomous driving vehicles in the environment. The laser radar can provide high-precision point cloud map data, and the IMU provides high-frequency attitude and acceleration information of the autonomous driving vehicle. The positioning and map information are output in real time by tightly coupling the laser radar and IMU information. The map of the park can be constructed in real time, providing important reference information for patrols, and locating its own position in real time, solving the problem of excessive positioning error caused by accumulated position deviation, and compensating for the positioning output problem of GPS when the signal is blocked. Therefore, the present invention effectively overcomes the various shortcomings of the prior art and has a high industrial utilization value.

[0084] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the art may 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 a person of ordinary skill in the art without departing from the spirit and technical concept disclosed by the present invention shall still be covered by the claims of the present invention.

Claims

1. A multi-sensor fusion real-time positioning and mapping system for autonomous driving, characterized in that: include: A data acquisition module, wherein the data acquisition module uses multiple sensors to collect lidar point cloud data and IMU data of actual scenes during vehicle movement; A data preprocessing module, which is used to downsample and remove noise from point cloud data, and pre-integrate IMU data; A state estimation module, the state estimation module comprising a motion compensation unit, a residual calculation unit, a state update unit, a convergence unit, and a mileage output unit; A feature matching module, the feature matching module comprising an incremental voxel unit and a point cloud insertion unit; A mapping module, wherein the mapping module includes a map construction unit and a closed-loop detection unit.

2. A multi-sensor fusion real-time positioning and mapping method for autonomous driving, comprising the multi-sensor fusion real-time positioning and mapping system for autonomous driving according to claim 1, characterized in that: The steps include: S1. Use laser radar to collect point cloud data of the current environment in real time, and perform downsampling and noise removal on the collected point cloud data; S2, taking out the IMU data from the vehicle-mounted integrated navigation and performing pre-integration processing on the data to obtain the motion model of the IMU; S3, fusing point cloud data and IMU data, using the IMU pre-integration estimated position and posture, compensating the motion of each point in the point cloud data to time, and obtaining the sampling data of the laser point cloud at the same time, that is, time; S4, using nonlinear optimization method to calculate the minimum residual 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 key frame point cloud data into the voxel space, traverse the voxel space through k-iterative nearest neighbor search to find matching features, and output the results to the mapping module; S6. Build a global point cloud map based on the odometer results and local feature maps. When a loop is detected in the environment, use the closed-loop detection algorithm to optimize the global map and output an accurate point cloud map that is consistent with the global map.

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

4. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 2, characterized in that: In step S3, the process of the motion compensation algorithm is simplified to the following formula: Where sf stands for "starting from"; the relative pose of the point after motion compensation is defined as The point Projection to the end time of scanning t k We can get:

5. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 2, characterized in that: In step S4, the linear optimization method includes tightly coupling the information of the laser radar and the IMU and using an optimization method of an extended Kalman filter, namely, EKF, to estimate the motion state of the vehicle.

6. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 5, characterized in that: The EKF optimization method comprises 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 represented as nonlinear functions; A2. The EKF performs Taylor expansion on these nonlinear functions at the current state point and linearizes them using the first-order derivative to obtain 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. Then perform iterative update calculations. When convergence occurs, that is, when the nonlinear system is converted into a linear system after iterative calculations, the predicted state value at the current moment, i.e., the odometer pose estimate, is output.

7. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 2, characterized in that: 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.

8. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 2, characterized in that: In step S6, the odometer result is point cloud data obtained by using a laser inertial odometer to obtain posture transformation and convert it into data in a world coordinate system.

9. The multi-sensor fusion real-time positioning and mapping method for autonomous driving according to claim 8, characterized in that: In step S6, the closed-loop detection algorithm includes the following steps: B1. Input Keyframes, i.e. candidate keyframes, and new_keyframes, i.e. newly added keyframes; B2, / / Search for the matching relationship between each frame of the traversed key frame and the newly added frame, so as to determine the potential closed loop; B3, 1) Calculate the pose estimate of the newly added key frame and output the quaternion; 2) Calculate the pose estimate of the candidate keyframe and output the quaternion; 3) Calculate the relative pose estimation of the newly added key frame pose and the candidate key frame pose; 4) The z-axis offset of the relative pose estimation is cleared; 5) Use the ICP algorithm to match candidate frames and newly added key frames; 6) Obtain matching score; 7) If the match does not converge or the match score is not as good as the current best score, skip the newly added key frame; 8) If the best score is less than the loop detection threshold, a loop is detected; B4. Output the key frame matching relationship that satisfies the closed loop.

Images