Multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar

Abstract

The invention belongs to a real-time positioning and mapping technology, and particularly discloses a multi-sensor fusion driving mapping method based on a light map and a 4D millimeter-wave radar, which utilizes the real-time speed measurement capability of the 4D millimeter-wave radar, takes the self-vehicle speed measured by the millimeter-wave radar as speed observation, and utilizes the light map and visual perception matching result and gps as position observation. According to the method, the factor graph is constrained, a more accurate position estimation algorithm is realized, meanwhile, unification between a self-mapping coordinate system and a light map coordinate system is realized, and downstream positioning and regulation control are facilitated. According to the invention, the environment prior information of the light map and the observation information provided by the 4D millimeter wave radar and other sensors are used to realize the precise modeling of the surrounding environment of the vehicle, and the use of the light map provides new constraint conditions and associated information for the mapping process. And the accuracy and robustness of the mapping algorithm are enhanced by using a relative pose transformation matrix provided by the light map and fusing other multi-sensor data.

Description

TECHNICAL FIELD

[0001] The present application relates to real-time positioning and mapping technology, in particular to a multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar. BACKGROUND

[0002] With the rapid development of autonomous driving technology, accurate environmental perception and mapping capability has become the key to realizing 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 such as LiDAR, camera, radar, etc., can provide three-dimensional (3D) information of the vehicle's surrounding environment to a certain extent, but still have limitations in dynamic target detection, complex scene understanding, etc. For example, LiDAR performance is limited in strong light or rain and fog, while cameras are easily affected by changes in light.

[0004] In recent years, 4D millimeter wave radar technology has gradually attracted attention because it can provide high-resolution distance, speed, azimuth and elevation angle information in various environmental conditions. 4D millimeter wave radar can not only detect static and dynamic targets, but also provide target speed information, which provides 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, highways, etc. For example, due to the small number of point clouds, feature extraction is difficult, which leads to inaccurate odometer estimation, and in driving scenarios, it cannot be associated with light maps, which limits downstream vehicle planning, decision-making, etc. SUMMARY

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

[0007] In order to achieve the above purpose, the technical solution adopted by the present application is as follows: The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar comprises the following steps: Step S1: sensor data preprocessing, Step S1.1: based on 4D millimeter wave radar point cloud data , stripping out static points in the point cloud data , estimating the vehicle speed based on static points ; Step S1.2: Preprocessing of wheel speed, IMU and GPS data, Determine whether the current GPS data is available based on the GPS status, quality, and number of searched satellites, perform initial GPS positioning, and obtain the GPS position; Based on the ESKF algorithm, wheel speed and IMU are integrated to achieve dead reckoning function and continuously output DR odometer posture ; Step S1.3: Matching visual perception with lane lines; Step S2: The DR odometer pose output in step S1.2 is As the inter-frame constraint factor, the vehicle speed output in step S1.1 is As speed observations, the visual perception and lane line matching results output in step S1.3 and the GPS position output in step S1.2 are added to the factor graph as absolute pose observations, and optimized simultaneously to finally output the optimized historical keyframe poses. And real-time pose ; Step S3: Optimize the historical key frame pose based on step 2 , align the timestamp with the radar point cloud frame, perform posture compensation based on the time difference, use the compensated real-time posture to transform and splice the radar point cloud frame, and finally output a 4D millimeter-wave radar point cloud map.

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

[0009] The following is a technical solution further defined by the present invention, which uses statistical filtering and radius filtering algorithms to filter out noise points, and extracts point cloud data based on a random sampling consensus algorithm. Static point in .

[0010] The following is a further technical solution of the present invention, based on the static point Estimated vehicle speed ,include: Get static points Direction scalar of the midpoint ,in are the coordinates of the point, for its direction; Static target points measured by 4D millimeter-wave radar Doppler velocity Equal to the product of direction and vehicle speed:

[0011] If there are N observations, then:

[0012] where, is the observation matrix, is the ego vehicle speed; construct the least squares problem and calculate the ego speed based on QR decomposition .

[0013] The following is a further limited technical solution of the application, according to the state, quality and number of stars of GPS to determine whether the current GPS data is available, using two consecutive frames of available GPS data to calculate the absolute orientation of the vehicle at the initial motion time , and the GPS absolute pose observation at t time , so as to obtain the GPS position.

[0014] The following is a further limited technical solution of the application, based on the ESKF algorithm to fuse wheel speed and IMU to realize the function of dead reckoning, and continuously output the DR odometer pose where, is a 3*3 rotation matrix at t time, is a translation matrix at t time.

[0015] The following is a further limited technical solution of the application, in the matching of visual perception and lane line, including: Visual perception lane line endpoint output: The 3D lane line point cloud under the vehicle coordinate system is extracted from the bird's eye view by the 3D lane line segmentation method based on deep learning where ; At the same time, in order to weaken the influence of the lane line segmentation algorithm in the long-distance scene on the subsequent mapping algorithm, the two lane points closest to the vehicle coordinate system are extracted from the 3D lane line point cloud where is the 3D lane point at t time in the ego vehicle coordinate system, is the 3D lane point at t time in the ego vehicle coordinate system, and , ; Then, according to the vehicle pose at t time , the is converted to the global coordinate system:

[0016] where​ is a 3*3 rotation matrix at t, is a translation vector at t, is a visual perception 3D lane point under the global coordinate system; light graph lane endpoint output: Based on the vehicle pose result at t optimized by the factor graph, the result is converted to the absolute coordinate system to obtain the vehicle position at t under the absolute coordinate system; then, the nearest left and right 3D lane point information in the light graph is found according to the position in the light graph , wherein is a left light graph 3D lane point at t under the global coordinate system, is a right light graph 3D lane point at t under the global coordinate system; Registration of visual perception lane line endpoints and light graph lane endpoints: Since the visual perception lane line endpoints have no elevation information, the coordinates of the visual perception lane line endpoints and the light graph lane endpoints are reduced from three dimensions to two dimensions, and the visual perception lane line endpoints and the light graph lane endpoints are used to calculate the transformation matrix of the current ego vehicle relative to the light graph absolute observation , then based on the transformation matrix and the ego vehicle position, the absolute pose observation pose information based on the light graph is output.

[0017] The following is a further defined technical solution of the present application, the calculation of the transformation matrix includes: Calculate the vector direction and the rotation matrix:

[0018]

[0019]

[0020]

[0021]

[0022] wherein and are right and left endpoint coordinates in the visual perception lane line endpoint ; Calculate the translation vector: .

[0023] Compared with the prior art, the present application has the following technical effects: 1. The application utilizes the real-time speed measurement capability of the 4D millimeter wave radar, uses the vehicle speed measured by the millimeter wave radar as the speed observation, and uses the light map and visual perception matching result and GPS as the position observation to constrain the factor graph, realizes a more accurate position estimation algorithm, and realizes the unification between the self-built map coordinate system and the light map coordinate system, which is convenient for downstream positioning and regulation and control. 2. Since the 4D millimeter wave radar is not affected by the weather environment, and can detect dynamic targets in various environments, the output can meet the environmental point cloud for positioning, so the point cloud map constructed based on the algorithm is not affected by the weather factor, which greatly improves the efficiency of collecting the map. In summary, by using the environmental prior information of the light map and the observation information provided by the 4D millimeter wave radar and other sensors, the application realizes accurate modeling of the environment around the vehicle. The use of light map not only provides new constraint conditions 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 the light map and the fusion of other multi-sensor data. The method has good precision and environmental adaptability in driving scenarios.

[0024] The application will be further described below in combination with the drawings and embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the embodiments of the application or the prior art, the drawings needed to be used in the embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative labor.

[0026] Figure 1 It is a method flowchart of the application. DETAILED DESCRIPTION

[0027] In order to make the above-mentioned purposes, features and advantages of the application more obvious and easy to understand, the specific embodiments of the application will be described in detail below in combination with the drawings. In the following description, a lot of specific details are set forth in order to fully understand the application. However, the application can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without departing from the connotation of the application, therefore the application is not limited by the specific embodiments disclosed below.

[0028] As shown in the figure, the embodiment provides a multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar, the main steps include three parts, which are sensor data preprocessing, factor graph optimization and point cloud splicing. Figure 1

[0029] ​1. Sensor data preprocessing: mainly introduces the processing algorithm and output results of the raw data of each sensor.

[0030] 1.1. Radar point cloud preprocessing and self-velocity estimation: 1.1.1. Input 4D millimeter wave radar point cloud data ,in Contains 4-dimensional information, namely spatial coordinates , y, z and Doppler velocity information of the corresponding point ; 1.1.2. Use statistical filtering and radius filtering algorithms to filter out noise points, and extract point cloud data based on random sampling consensus algorithm. Static point in , note that considering the actual situation, the random sampling consensus algorithm cannot strictly separate the static points and dynamic points, so the static points here are not point cloud data All static points in the point cloud need to be calculated based on the vehicle speed and then the point cloud data can be re-judged. Dynamic points and static points in.

[0031] 1.1.3 Estimating vehicle speed based on 4D radar point cloud: a. Obtain static point cloud Direction scalar of the midpoint in are the coordinates of the point, for its direction; b. Doppler velocity of static target points measured by 4D millimeter-wave radar Equal to the product of direction and vehicle speed:

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

[0033] in is the observation matrix, is the vehicle speed.

[0034] c. Constructing the least squares problem , and calculate its own speed based on QR decomposition ; d. Output radar point cloud and the vehicle speed estimated by the radar .

[0035] 1.2 Wheel speed, IMU and GPS data preprocessing a, Determine whether the current GPS data is available according to the state, quality and the number of searching stars of GPS, and use the available GPS data of two continuous frames to calculate the absolute orientation of the vehicle at the initial moment , and the GPS absolute pose observation at time t ; b, Based on the ESKF algorithm, fuse the wheel speed and IMU to realize the function of dead reckoning, and continuously output the odometer result , wherein is a 3*3 rotation matrix at time t, is a translation matrix at time t; 1.3, Visual perception and light map data preprocessing, a, Visual perception lane endpoint output The 3D lane line segmentation method based on deep learning extracts 3D lane line point cloud under the vehicle coordinate system from the bird's eye view , wherein . At the same time, in order to weaken the influence of the lane line segmentation algorithm in the long-distance scene on the subsequent mapping algorithm, the two closest lane points to the vehicle coordinate system are extracted from the 3D lane line point cloud , wherein is the 3D lane point at time t in the left vehicle coordinate system, is the 3D lane point at time t in the right vehicle coordinate system, and , , . Then, according to the vehicle pose at time t , convert to the global coordinate system:

[0036] , wherein is a 3*3 rotation matrix at time t, is a translation vector at time t, is the visual perception 3D lane point converted to the global coordinate system; b, Light map lane endpoint output, Based on the pose result at time t optimized by the factor graph, convert it to the absolute coordinate system to obtain the vehicle position at time t in the absolute coordinate system. Then, according to the position, find the closest left and right 3D lane point information in the light map , wherein is the light map 3D lane point at time t in the global coordinate system, and is the light map 3D lane point at time t in the global coordinate system.

[0037] c, Visual perception & light map lane endpoint registration, Since the lane points perceived by vision have no elevation information, the current step will reduce the lane end point 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 two pairs of points, and output the absolute observation information based on the light map based on the transformation matrix.

[0038] 2. Factor graph optimization, Constructing factor graph: add the DR odometry pose output by step 1.2 as inter-frame constraint factor, the vehicle speed output by step 1.1 as speed observation, the visual perception and lane line matching result output by step 1.3 and the GPS position output by step 1.2 as absolute pose observation to the factor graph, and optimize synchronously, finally output the optimized historical key frame pose , and the current time pose .

[0039] 3. Point cloud splicing, Based on the historical key frame pose optimized in step 2, synchronize it with the radar point cloud frame in time stamp, and compensate the pose according to the time difference, transform and splice the radar point cloud frame using the compensated pose, and finally output the radar point cloud map.

[0040] The above is only the preferred embodiment of the present application, and does not limit the present application in any form. Any skilled person in the art can make many possible changes and modifications to the technical solution of the present application, or modify it into equivalent embodiments without departing from the scope of the technical solution of the present application, using the disclosed methods and technical contents. Therefore, any equivalent changes made according to the shape, structure and principle of the present application, which do not depart from the technical solution of the present application, should be covered by the protection scope of the present application.

Claims

1. A multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar, characterized by: The following steps are involved: Step S1: Sensor data preprocessing, Step S1.1: Based on 4D millimeter wave radar point cloud data , stripping out point cloud data Static point in , based on static points Estimated vehicle speed ; Step S1.2: Preprocessing of wheel speed, IMU and GPS data, Determine whether the current GPS data is available based on the GPS status, quality, and number of searched satellites, perform initial GPS positioning, and obtain the GPS position; Based on the ESKF algorithm, wheel speed and IMU are integrated to achieve dead reckoning function and continuously output DR odometer posture ; Step S1.3: Matching visual perception with lane lines; Step S2: The DR odometer pose output in step S1.2 is As the inter-frame constraint factor, the vehicle speed output in step S1.1 is As speed observations, the visual perception and lane line matching results output in step S1.3 and the GPS position output in step S1.2 are added to the factor graph as absolute pose observations, and optimized simultaneously to finally output the optimized historical keyframe poses. And real-time pose ; Step S3: Optimize the historical key frame pose based on step 2 , align the timestamp with the radar point cloud frame, perform posture compensation based on the time difference, use the compensated real-time posture to transform and splice the radar point cloud frame, and finally output a 4D millimeter-wave radar point cloud map.

2. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar according to claim 1 is characterized in that: 4D millimeter-wave radar point cloud data Contains four-dimensional information, namely spatial coordinates , y, z and Doppler velocity information of the corresponding point .

3. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 2 is characterized in that: Use statistical filtering and radius filtering algorithms to filter out noise points, and extract point cloud data based on random sampling consensus algorithm Static point in .

4. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 3 is characterized in that: Based on static points Estimated vehicle speed ,include: Get static points Direction scalar of the midpoint ,in are the coordinates of the point, for its direction; Static target points measured by 4D millimeter-wave radar Doppler velocity Equal to the product of direction and vehicle speed: If there are N observations, then: in, is the observation matrix, is the vehicle speed; Constructing the least squares problem , and calculate its own speed based on QR decomposition .

5. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 1, characterized in that: Determine whether the current GPS data is available based on the GPS status, quality, and number of searched satellites. Calculate the absolute orientation of the vehicle at the initial moment using two consecutive frames of available GPS data. , and the GPS absolute position observation at time t , thereby obtaining the GPS location.

6. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 5 is characterized in that: Based on the ESKF algorithm, wheel speed and IMU are integrated to achieve dead reckoning function and continuously output DR odometer posture ,in, is the 3*3 rotation matrix at time t, is the translation matrix at time t.

7. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 1, characterized in that: Matching visual perception with lane markings includes: Visual perception lane endpoint output: A 3D lane line segmentation method based on deep learning extracts 3D lane line point clouds in the vehicle body coordinate system from a bird's-eye view. ,in ; At the same time, in order to reduce the impact of lane line segmentation algorithm's false detection in long-distance scenes on subsequent mapping algorithms, the 3D lane line point cloud Extract the two lane points closest to the vehicle body coordinate system ,in is the 3D lane point on the left at time t in the vehicle coordinate system, is the 3D lane point on the right side at time t in the vehicle coordinate system, and , ; Then according to the vehicle posture at time t ,Will Go to the global coordinate system: in is the 3*3 rotation matrix at time t, is the translation vector at time t, To convert the visually perceived 3D lane points to the global coordinate system; Light map lane endpoint output: Based on the vehicle pose result at time t optimized by the factor graph, it is converted to the absolute coordinate system to obtain the vehicle position at time t in the absolute coordinate system; then, according to this position, the nearest left and right 3D lane point information is found in the light map. ,in is the 3D lane point of the light map at time t on the left in the global coordinate system, is the 3D lane point of the light map at time t on the right side in the global coordinate system; Alignment of visually perceived lane endpoints with light-image lane endpoints: Since the visual perception lane line endpoints have no elevation information, the coordinates of the visual perception lane line endpoints and the light map lane endpoints are reduced from three dimensions to two dimensions, and the visual perception lane line endpoints are used and light lane endpoints Calculate the transformation matrix between the current vehicle relative and light map absolute observations , then based on the transformation matrix And the vehicle position output is the absolute pose observation pose information based on the light map.

8. The multi-sensor fusion driving mapping method based on light map and 4D millimeter wave radar as claimed in claim 7, characterized in that: Transformation Matrix The calculation includes: Calculate the vector direction and rotation matrix: in and They are respectively the visual perception lane line endpoints The right and left endpoint coordinates of ; Calculate the translation vector: .

Images