A Multi-Sensor Fusion Vehicle Mapping Method Based on Light Map and 4D Millimeter Wave Radar

Abstract

This invention pertains to real-time positioning and mapping technology, specifically disclosing a multi-sensor fusion method for vehicle mapping based on a lightweight map and 4D millimeter-wave radar. Utilizing the real-time speed measurement capability of the 4D millimeter-wave radar, the vehicle's speed is used as the speed observation. The lightweight map, visual perception matching results, and GPS are used as position observations to constrain the factor map, achieving a more accurate position estimation algorithm. Simultaneously, it unifies the self-built mapping coordinate system with the lightweight map coordinate system, facilitating downstream positioning and regulatory control applications. This invention achieves accurate modeling of the vehicle's surrounding environment by using prior environmental information from the lightweight map and observation information provided by the 4D millimeter-wave radar and other sensors. The use of the lightweight map not only provides new constraints and related information for the mapping process but also enhances the accuracy and robustness of the mapping algorithm through the fusion of the relative pose transformation matrix provided by the lightweight map and other multi-sensor data.

Description

Technical Field

[0001] This invention relates to real-time positioning and mapping technology, and in particular to a multi-sensor fusion method for vehicle mapping based on lightweight maps and 4D millimeter-wave radar. Background Technology

[0002] With the rapid development of autonomous driving technology, accurate environmental perception and mapping capabilities have become crucial for achieving safe and efficient autonomous driving. In this field, multi-sensor fusion technology has attracted widespread attention because it can integrate the advantages of different sensors to provide more comprehensive and robust environmental perception information.

[0003] Traditional sensor fusion methods, such as data fusion based on single sensors like LiDAR, cameras, and radar, can provide three-dimensional (3D) information about the vehicle's surroundings to some extent, but they still have limitations in areas such as dynamic target detection and complex scene understanding. For example, LiDAR performance is limited in adverse weather conditions such as strong light or rain and fog, while cameras are easily affected by changes in lighting.

[0004] In recent years, 4D millimeter-wave radar technology has gained increasing attention due to its ability to provide high-resolution range, velocity, azimuth, and elevation information under various environmental conditions. 4D millimeter-wave radar can not only detect static and dynamic targets but also provide target velocity information, offering a new perspective for dynamic environment modeling of vehicles.

[0005] However, single 4D millimeter-wave radar data still faces challenges in complex traffic scenarios, such as urban roads and highways. For example, the limited number of point clouds and the difficulty in feature extraction lead to inaccurate odometer estimation, and the inability to associate it with lightweight maps in driving scenarios limits downstream vehicle planning, decision-making, and other functions. Summary of the Invention

[0006] The purpose of this invention is to solve the technical problems existing in the background art. To this end, a multi-sensor fusion vehicle mapping method based on light map and 4D millimeter-wave radar is provided.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A multi-sensor fusion vehicle mapping method based on lightweight maps and 4D millimeter-wave radar includes the following steps: Step S1: Sensor data preprocessing, Step S1.1: Based on 4D millimeter-wave radar point cloud data Extracting point cloud data static points in Based on static points Estimate vehicle speed ; Step S1.2: Preprocessing of wheel speed, IMU, and GPS data. The availability of current GPS data is determined based on the GPS status, quality, and number of satellites acquired; initial GPS positioning is then performed to obtain the GPS location. The system uses the ESKF algorithm to fuse wheel speed and IMU data to perform trajectory estimation and continuously outputs DR odometry pose. ; Step S1.3: Match visual perception with lane lines; Step S2: Convert the DR odometry pose output in step S1.2 As an inter-frame constraint factor, the vehicle speed output in step S1.1 is used. As velocity observations, the visual perception and lane line matching results output in step S1.3 and the GPS position output in step S1.2 are used as absolute pose observations and added to the factor graph for simultaneous optimization. Finally, the optimized historical keyframe pose is output. and real-time pose ; Step S3: Based on the historical keyframe poses optimized in Step 2 The radar point cloud frame is timestamped and then timestamped. The pose is compensated based on the time difference. The real-time pose after compensation is used to transform and stitch the radar point cloud frame, and finally outputs a 4D millimeter-wave radar point cloud map.

[0008] The following is a further defined technical solution of the present invention: 4D millimeter-wave radar point cloud data. It contains four-dimensional information, namely spatial coordinates. y, z and the corresponding Doppler velocity information of the points .

[0009] The following is a further defined technical solution of the present invention: statistical filtering and radius filtering algorithms are used to filter out noise points, and point cloud data is extracted based on the random sampling consensus algorithm. static points in .

[0010] The following is a further defined technical solution of the present invention, based on static points. Estimate vehicle speed ,include: Get static points Directional scalar of the midpoint ,in Let the coordinates of the point be... Its direction; Static target points measured by 4D millimeter-wave radar Doppler velocity Equals the product of direction and vehicle speed:

[0011] If there are N observations, then:

[0012] in, For the observation matrix, For the vehicle's speed; Constructing the least squares problem And calculate its own velocity based on QR decomposition. .

[0013] The following is a further defined technical solution of the present invention: the availability of current GPS data is determined based on the GPS status, quality, and number of satellites acquired; and the absolute orientation of the vehicle at the initial moment is calculated using two consecutive frames of available GPS data. and GPS absolute pose observation at time t This allows us to obtain the GPS location.

[0014] The following is a further defined technical solution of the present invention: based on the ESKF algorithm, wheel speed and IMU are fused to realize the trajectory estimation function, and the DR odometry pose is continuously output. ,in, For time t, a 3x3 rotation matrix is ​​used. Let be the translation matrix at time t.

[0015] The following is a further defined technical solution of the present invention, which includes matching visual perception with lane lines: Visual perception of lane line endpoint output: A deep learning-based 3D lane segmentation method extracts 3D lane point clouds in the vehicle coordinate system from a bird's-eye view. ,in ; To mitigate the impact of false detections in lane segmentation algorithms at long distances on subsequent mapping algorithms, 3D lane line point clouds were used as a basis for analysis. Extract the two lane points closest to the vehicle coordinate system. ,in Let be the 3D lane point on the left at time t in the vehicle coordinate system. Let be the 3D lane point on the right side at time t in the vehicle coordinate system, and , ; Subsequently, based on the vehicle's position at time t... ,Will Switch to global coordinate system:

[0016] in For time t, a 3x3 rotation matrix is ​​used. Let be the translation vector at time t. To transform to visually perceived 3D lane points in the global coordinate system; Lightweight map lane endpoint output: Based on the vehicle pose at time t optimized by the factor graph, it is transformed into an absolute coordinate system to obtain the vehicle position at time t in the absolute coordinate system; then, based on this position, the nearest left and right 3D lane points are found in the light map. ,in Let be the light map 3D lane point at time t in the global coordinate system. The light map 3D lane point at time t in the global coordinate system; Visual perception lane line endpoint registration and light map lane endpoint registration: Since visually perceived lane line endpoints lack elevation information, the coordinates of both the visually perceived lane line endpoints and the light map lane endpoints are reduced from three dimensions to two dimensions, and the visually perceived lane line endpoints are utilized... and light map lane endpoints Calculate the transformation matrix between the current relative observation of the vehicle and the absolute observation of the light map. Then based on the transformation matrix And the vehicle position output is based on the absolute pose observation pose information of the light map.

[0017] The following is a further defined technical solution of the present invention: transformation matrix The calculations include: Calculate the vector direction and rotation matrix:

[0018]

[0019]

[0020]

[0021]

[0022] in and These are the endpoints of the visually perceived lane lines. The coordinates of the right and left endpoints in the diagram; Calculate the translation vector: .

[0023] Compared with the prior art, the present invention has the following technical effects: 1. This invention utilizes the real-time speed measurement capability of 4D millimeter-wave radar, takes the vehicle speed measured by millimeter-wave radar as the speed observation, and uses the matching results of the light map and visual perception, as well as GPS, as the position observation to constrain the factor map, thereby achieving a more accurate position estimation algorithm. At the same time, it realizes the unification between the self-built map coordinate system and the light map coordinate system, which is convenient for downstream positioning and planning control. 2. Since 4D millimeter-wave radar is not affected by weather conditions and can detect dynamic targets in various environments, and outputs environmental point clouds that can meet the positioning requirements, the point cloud map built based on this algorithm is not affected by weather factors, which greatly improves the efficiency of map acquisition. In summary, this invention achieves accurate modeling of the vehicle's surrounding environment by using prior environmental information from a lightweight map and observational information provided by 4D millimeter-wave radar and other sensors. The use of lightweight maps not only provides new constraints and related information for the mapping process, but also enhances the accuracy and robustness of the mapping algorithm by using the relative pose transformation matrix provided by lightweight maps and the fusion of other multi-sensor data. In driving scenarios, the method of this invention has good accuracy and environmental universality.

[0024] The present invention will be further described below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0027] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be practiced in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0028] like Figure 1 As shown, this embodiment provides a multi-sensor fusion vehicle mapping method based on light map and 4D millimeter-wave radar. The main steps include three parts: sensor data preprocessing, factor map optimization, and point cloud stitching.

[0029] 1. Sensor Data Preprocessing: This section mainly introduces the algorithms for processing raw data from various sensors and the output results.

[0030] 1.1 Radar point cloud preprocessing and self-velocity estimation: 1.1.1 Input 4D millimeter-wave radar point cloud data ,in It contains 4-dimensional information, namely spatial coordinates. y, z and the corresponding Doppler velocity information of the points ; 1.1.2. Statistical filtering and radius filtering algorithms are used to remove noise points, and point cloud data is extracted based on the random sampling consistency algorithm. static points in Note that, considering practical considerations, the random sampling consistency algorithm cannot strictly separate static points from dynamic points; therefore, the static points here are not point cloud data. All static points in the point cloud data need to be used to calculate the vehicle speed and then reassess the point cloud data. Dynamic points and static points in the data.

[0031] 1.1.3 Estimating vehicle speed based on 4D radar point cloud: a. Obtain the static point cloud Directional scalar of the midpoint in Let the coordinates of the point be... Its direction; b. Doppler velocity of a static target point measured by 4D millimeter-wave radar Equals the product of direction and vehicle speed:

[0032] If there are N observations, the equation can be written as:

[0033] in For the observation matrix, This refers to the vehicle's speed.

[0034] c. Constructing the least squares problem And its own velocity is calculated based on QR decomposition. ; d. Output radar point cloud And the vehicle speed estimated by radar .

[0035] 1.2 Preprocessing of Wheel Speed, IMU and GPS Data a. Determine the availability of current GPS data based on GPS status, quality, and the number of satellites acquired. Calculate the vehicle's absolute orientation at the initial moment using two consecutive frames of available GPS data. and GPS absolute pose observation at time t ; b. Based on the ESKF algorithm, wheel speed and IMU are fused to realize the trajectory calculation function and continuously output the odometer results. ,in For time t, a 3x3 rotation matrix is ​​used. Let be the translation matrix at time t; 1.3 Visual perception and lightweight image data preprocessing, a) Visual perception of lane line endpoints output A deep learning-based 3D lane segmentation method extracts 3D lane point clouds in the vehicle coordinate system from a bird's-eye view. ,in To mitigate the impact of false detections in lane segmentation algorithms at long distances on subsequent mapping algorithms, 3D lane line point clouds were used as a reference. Extract the two lane points closest to the vehicle coordinate system. ,in Let be the 3D lane point on the left at time t in the vehicle coordinate system. Let be the 3D lane point on the right side at time t in the vehicle coordinate system, and , Then, based on the vehicle's position and pose at time t... ,Will Switch to global coordinate system:

[0036] in For time t, a 3x3 rotation matrix is ​​used. Let be the translation vector at time t. To transform to visually perceived 3D lane points in the global coordinate system; b. Lightweight map lane endpoint output Based on the pose result at time t optimized by the factor graph, it is transformed into an absolute coordinate system to obtain the vehicle position at time t in the absolute coordinate system. Then, based on this position, the nearest left and right 3D lane points are found in the light map. ,in Let be the light map 3D lane point at time t in the global coordinate system, where Let be the light map 3D lane point at time t in the global coordinate system.

[0037] c. Visual perception & lightweight map lane endpoint registration. Since the visual perception lane points do not have elevation information, the current step will reduce the visual perception and lane endpoint coordinates output by a and b from three dimensions to two dimensions, and calculate the transformation matrix between the current vehicle relative to the absolute observation of the light map based on SVD decomposition using the above two pairs of points, and output the absolute observation information based on the light map based on this transformation matrix.

[0038] 2. Factor plot optimization, Constructing the factor graph: The DR odometry pose output in step 1.2 is used as the inter-frame constraint factor, the vehicle speed output in step 1.1 is used as the speed observation, and the visual perception and lane line matching results output in step 1.3 and the GPS position output in step 1.2 are added to the factor graph as absolute pose observations. Optimization is performed simultaneously, and the optimized historical keyframe pose is finally output. and the current pose .

[0039] 3. Point cloud stitching, Based on the historical keyframe poses optimized in step 2, they are timestamped with radar point cloud frames, and pose compensation is performed according to the time difference. The compensated poses are then used to transform and stitch the radar point cloud frames, ultimately outputting a radar point cloud map.

[0040] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any person skilled in the art can make many possible variations and modifications to the technical solution of the present invention, or modify it into equivalent embodiments, without departing from the scope of the present invention's technical solution. Therefore, all equivalent changes made based on the shape, structure, and principle of the present invention without departing from the scope of the present invention's technical solution should be covered within the protection scope of the present invention.

Claims

1. A multi-sensor fusion vehicle mapping method based on lightweight maps and 4D millimeter-wave radar, characterized in that, Includes the following steps: Step S1: Sensor data preprocessing, Step S1.1: Based on 4D millimeter-wave radar point cloud data Extracting point cloud data static points in Based on static points Estimate vehicle speed ; Step S1.2: Preprocessing of wheel speed, IMU, and GPS data. The availability of current GPS data is determined based on the GPS status, quality, and number of satellites acquired; initial GPS positioning is then performed to obtain the GPS location. The system uses the ESKF algorithm to fuse wheel speed and IMU data to perform trajectory estimation and continuously outputs DR odometry pose. ; Step S1.3: Match visual perception with lane lines; Step S2: Convert the DR odometry pose output in step S1.2 As an inter-frame constraint factor, the vehicle speed output in step S1.1 is used. As a velocity observation, the visual perception and lane line matching results output in step S1.3 and the GPS position output in step S1.2 are used as absolute pose observations and added to the factor graph for simultaneous optimization. Finally, the optimized historical keyframe pose is output. and real-time pose ; Step S3: Based on the historical keyframe poses optimized in Step 2 The radar point cloud frame is timestamped and then timestamped. The pose is compensated based on the time difference. The real-time pose after compensation is used to transform and stitch the radar point cloud frame, and finally outputs a 4D millimeter-wave radar point cloud map. In matching visual perception with lane lines, the following are included: Visual perception of lane line endpoint output: A deep learning-based 3D lane segmentation method extracts 3D lane point clouds in the vehicle coordinate system from a bird's-eye view. ,in ; To mitigate the impact of false detections in lane segmentation algorithms at long distances on subsequent mapping algorithms, 3D lane line point clouds were used as a basis for analysis. Extract the two lane points closest to the vehicle coordinate system. ,in Let be the 3D lane point on the left at time t in the vehicle coordinate system. Let be the 3D lane point on the right side at time t in the vehicle coordinate system, and , ; Subsequently, based on the vehicle's position at time t... ,Will Switch to global coordinate system: in For time t, 3 3. Rotation matrix Let be the translation vector at time t. To transform to visually perceived 3D lane points in the global coordinate system; Lightweight map lane endpoint output: Based on the vehicle pose at time t optimized by the factor graph, it is transformed into an absolute coordinate system to obtain the vehicle position at time t in the absolute coordinate system; then, based on this position, the nearest left and right 3D lane points are found in the light map. ,in Let be the light map 3D lane point at time t in the global coordinate system. The light map 3D lane point at time t in the global coordinate system; Visual perception lane line endpoint registration and light map lane endpoint registration: Since visually perceived lane line endpoints lack elevation information, the coordinates of both the visually perceived lane line endpoints and the light map lane endpoints are reduced from three dimensions to two dimensions, and the visually perceived lane line endpoints are utilized... and light map lane endpoints Calculate the transformation matrix between the current relative observation of the vehicle and the absolute observation of the light map. Then based on the transformation matrix And the vehicle position output is based on the absolute pose observation pose information of the light map; Transformation matrix The calculations include: Calculate the vector direction and rotation matrix: in and These are the endpoints of the visually perceived lane lines. The coordinates of the right and left endpoints in the diagram; Calculate the translation vector: .

2. The multi-sensor fusion vehicle mapping method based on lightweight map and 4D millimeter-wave radar as described in claim 1, characterized in that, 4D millimeter-wave radar point cloud data It contains four-dimensional information, namely spatial coordinates. y, z and the corresponding Doppler velocity information of the points .

3. The multi-sensor fusion vehicle mapping method based on lightweight map and 4D millimeter-wave radar as described in claim 2, characterized in that, Noise points were removed using statistical filtering and radius filtering algorithms, and point cloud data was extracted based on the random sampling consistency algorithm. static points in .

4. The multi-sensor fusion vehicle mapping method based on lightweight map and 4D millimeter-wave radar as described in claim 3, characterized in that, Based on static points Estimate vehicle speed ,include: Get static points Directional scalar of the midpoint ,in Let be the coordinates of the point. Its direction; Static target points measured by 4D millimeter-wave radar Doppler velocity Equals the product of direction and vehicle speed: If there are N observations, then: in, For the observation matrix, For the vehicle's speed; Constructing the least squares problem And calculate its own velocity based on QR decomposition. .

5. The multi-sensor fusion vehicle mapping method based on lightweight map and 4D millimeter-wave radar as described in claim 1, characterized in that, The availability of GPS data is determined based on the GPS status, quality, and number of satellites acquired. At the initial moment, two consecutive frames of available GPS data are used to calculate the vehicle's absolute orientation during its initial movement. and GPS absolute pose observation at time t This allows us to obtain the GPS location.

6. The multi-sensor fusion vehicle mapping method based on lightweight map and 4D millimeter-wave radar as described in claim 5, characterized in that, The system uses the ESKF algorithm to fuse wheel speed and IMU data to perform trajectory estimation and continuously outputs DR odometry pose. ,in, For time t, 3 3. Rotation matrix Let be the translation matrix at time t.

Images