An unmanned vehicle positioning method based on millimeter wave radar

Abstract

The application discloses a millimeter wave radar-based unmanned vehicle positioning method, and belongs to the field of unmanned vehicle positioning, and mainly focuses on realizing high-precision positioning of the unmanned vehicle by using the strong penetration characteristic of the millimeter wave radar in a severe weather environment. The method is divided into an offline mapping operation and an online positioning operation. In the offline environment, data is collected by using vehicle-mounted sensors such as a laser radar, an inertial navigation, a millimeter wave radar and a GNSS for construction of a high-precision map, the k-Strongest algorithm and laser radar feature points are used for filtering processing of millimeter wave radar data, the inertial navigation and GNSS are used for solving the pose transformation of the vehicle in the offline environment, and the high-precision map is constructed by taking the pose transformation as a true value. In the online positioning, the inertial navigation data is used to remove the distortion of the millimeter wave radar, the k-Strongest algorithm and the principal component analysis algorithm are used to extract feature points of the millimeter wave radar data, the feature points are matched with the high-precision map by using the Gauss-Newton method, and the final pose is solved. The application is helpful to improve the positioning precision in the severe weather environment and reduce the cost of realizing the high-precision positioning.

Description

Technical Field

[0001] This invention belongs to the field of unmanned vehicle positioning, specifically referring to an unmanned vehicle positioning method based on millimeter-wave radar. Background Technology

[0002] Autonomous vehicle localization technology is one of the key technologies for realizing autonomous driving, and it is a fundamental prerequisite for achieving autonomous vehicle decision-making, control, navigation, and path planning. High-precision map-based autonomous vehicle localization schemes are a mainstream approach. This scheme first requires the use of sensors such as LiDAR or cameras to build a high-precision localization map in an offline environment. Then, in an online operating environment, the same sensors used to build the map perceive the surrounding environment and match it with the high-precision localization map, ultimately obtaining a high-precision pose output.

[0003] However, in adverse weather conditions (such as rain, snow, or fog), vehicle sensors are affected by raindrops and snowflakes, resulting in a large amount of noise in the collected data. This makes it impossible for online data to match offline high-precision maps, affecting positioning accuracy. Summary of the Invention

[0004] To address the problems of existing technologies, this invention provides an unmanned vehicle (UAV) localization method based on millimeter-wave radar. This method leverages the strong penetration of millimeter-wave radar to achieve high-precision localization of UAVs, particularly in adverse weather conditions. It comprises two parts: offline mapping and online localization. In the offline environment, data is collected from vehicle-mounted sensors such as LiDAR, inertial navigation, millimeter-wave radar, and GNSS for high-precision map construction. The millimeter-wave radar data is filtered using the k-Strongest algorithm and LiDAR feature points. Inertial navigation and GNSS are used to calculate the vehicle's pose transformation in the offline environment, and this transformation is used as the ground truth to construct the high-precision map. In the online localization process, inertial navigation data is used to remove millimeter-wave radar distortion. The k-Strongest algorithm and principal component analysis are used to extract feature points from the millimeter-wave radar data. The Gauss-Newton method is then used to match these feature points with the high-precision map to calculate the final pose.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A localization method for unmanned vehicles based on millimeter-wave radar, comprising two parts: offline mapping and online localization.

[0007] (1) In an offline environment, both lidar and millimeter-wave radar are used to collect data. The lidar data is used to help extract feature points from the millimeter-wave image (which is obtained from the data collected by the millimeter-wave radar). Finally, the feature points of the millimeter-wave image are used to construct a high-precision positioning map.

[0008] (2) In the online environment, feature points are extracted using millimeter-wave radar and matched with high-precision positioning maps to output pose.

[0009] The specific implementation steps of this invention are as follows:

[0010] First, LiDAR data processing in offline mapping operations.

[0011] Step 1.1: Use an inertial measurement unit to compensate for the distortion of lidar point cloud data.

[0012] Since both the lidar and millimeter-wave radar used in this invention are rotating sensors, point cloud distortion occurs during movement. This invention corrects the vehicle pose transformation during point cloud data acquisition by using inertial measurement unit (IMU) measurements, thereby achieving point cloud distortion removal.

[0013] Let t i R represents the start time of the point cloud in the i-th frame. i P i With v i Let represent the rotation matrix, translation matrix, and velocity of the point cloud in the i-th frame, respectively. and b represents the measured values ​​output by the gyroscope and accelerometer in the i-th frame, respectively. g,i and b a,i Let be the zero bias values ​​of the gyroscope and accelerometer in the i-th frame, respectively. Then, the trajectory calculation formula for the inertial measurement unit can be written as follows:

[0014]

[0015] Where Exp(·) denotes the function operation from Lie algebraic transformation to Lie group, Δt i =t i+1 -t i Let t represent the measurement time of the point cloud in frame i+1. t can be obtained from formula (1). i+1 The vehicle's pose transformation matrix at time t is:

[0016]

[0017] Since the sampling times of points in a single frame of point cloud are different, distortion correction of the point cloud can be achieved based on the sampling times of the points. Assume a point p... j The sampling time is t j Then the pose transformation matrix of that point is:

[0018]

[0019] The above method can be used to calculate the pose transformation of each point relative to the vehicle coordinate system at the start of the i-th frame point cloud, thereby converting the original lidar point cloud data into distortion-free point cloud data.

[0020] Step 1.2: Remove ground points from the distortion-free point cloud data.

[0021] The high-precision map is built using LiDAR, which collects 3D point cloud data that includes ground points. However, the positioning module uses millimeter-wave radar, which scans on a 2D horizontal plane, analogous to a single-line LiDAR, and cannot obtain ground point data. Therefore, it is necessary to remove ground points from the LiDAR point cloud. Specifically:

[0022] First, the distortion-reduced point cloud from step 1.1 is projected onto a two-dimensional plane perpendicular to the ground to form depth map I. i Each row of the depth map represents a single beam of light from the LiDAR, and each column represents the points collected by rotating the single beam 360°. The pixel value represents the distance from that point to the LiDAR. Assume a point p in the point cloud... k1 =(x j ,y j ,z j The coordinates projected onto the depth map are q. k1 =(u m ,v n ), can then be expressed as:

[0023]

[0024] Where u m ,v n Let represent the m-th lidar beam and the n-th sampling point, respectively. And for the depth map, q... k1 Points q adjacent to each other along the vertical axis k2 It can be represented as:

[0025] q k2 =(u m+1 ,v n (5)

[0026] Assume the angle between the m-th laser beam of the lidar and the ground is α. m The angle between the nth sampling point and the x-axis in the vehicle coordinate system is θ. n , then q k1 With q k2 The angle between the corresponding three-dimensional point and the horizontal plane is:

[0027]

[0028] When angle β k1,k2 Less than threshold βmax (15°≤β max When ≤30°, q can be considered k1 With q k2 The corresponding 3D points are all ground points. By traversing the depth image, all ground points are filtered out and removed to obtain the LiDAR point cloud data after removing ground points.

[0029] Step 1.3: Extract planar features from the point cloud data after removing ground points based on smoothness.

[0030] Since the point cloud data, even after removing ground points, still contains some dynamic objects and noise data, which can affect the final positioning accuracy, these two types of data need to be filtered out. Because planar points are generally large static objects such as building walls, this solution simplifies the process by extracting planar points instead of removing dynamic objects and noise data from the point cloud. The specific method is as follows:

[0031] (1.3.1) For a point p in the point cloud after removing the ground points, let the laser line at point p be l. i In the laser line l i Take several consecutive points to the left and right of point p, and denote them as the adjacent point set γ.

[0032] (1.3.2) Calculate the mean μ of the distance from all points in the adjacent point set γ to the lidar.

[0033] (1.3.3) Calculate the difference between the distance from point p to the lidar and the mean μ. If the difference is less than a fixed threshold d, max (0.1≤d max If the value is less than or equal to 0.2, mark the point as a planar point; otherwise, mark the point as a non-planar point.

[0034] (1.3.4) Return to step (1.3.1) and execute sequentially until all points in the point cloud are marked.

[0035] Through steps 1.1, 1.2 and 1.3 above, distortion-free planar feature points were extracted from the original lidar data. These feature points will be used to assist millimeter-wave radar in extracting effective features.

[0036] Second, millimeter-wave data processing in offline mapping operations.

[0037] Step 2.1: Extract features from millimeter-wave images acquired in an offline environment.

[0038] Because the raw millimeter-wave images contain a large amount of noise, it is necessary to extract feature points from the millimeter-wave images for mapping. This scheme uses a rotating frequency-modulated continuous-wave radar, which rotates continuously during the acquisition process to collect data in a 360° horizontal direction. Assume t...i The millimeter-wave radar image acquired at any time is Q i .

[0039] First, the k-Strongest algorithm is used to analyze Q. i Coarse filtering is performed. This method collects the first k points with the largest amplitude values ​​from each azimuth angle of the original millimeter-wave radar image. Since the mapping stage needs to preserve as many valid targets as possible in the millimeter-wave radar image, the k selected in the k-Strongest algorithm is as large as possible (e.g., k = 40). Then, the lidar feature point cloud extracted in step 1.3 is used to filter the coarsely filtered Q... i Perform fine filtering. The specific method is as follows:

[0040] (2.1.1) Centered on the lidar, divide the system into n concentric circles according to the range resolution of the millimeter-wave radar, and further divide the concentric circles into m equal-angled radii according to the angular resolution of the millimeter-wave radar. Any two adjacent radii and any two adjacent concentric circles can form a grid.

[0041] (2.2.2) Projecting the planar feature points of the lidar point cloud extracted in step 1.3 onto a horizontal plane will cause all points in the point cloud to fall into the grids described in (2.2.1). Let num(i,j) represent the number of projected points in the region enclosed by the i-th concentric circle, the (i-1)-th concentric circle, and the j-th and (j-1)-th radii. If num(i,j) > 2, then the grid is considered a valid grid.

[0042] (2.2.3) Remove the coarsely filtered Q using the same method as in step 1.1. i Point cloud distortion.

[0043] (2.2.4) When millimeter-wave radar data is projected onto a grid, points in the effective grid are considered as feature points in the millimeter-wave image and will be used for mapping.

[0044] Step 2.2: Establish a feature point map for millisecond radar.

[0045] In an offline environment, the vehicle pose is obtained by multiple measurements using a Global Navigation Satellite System (GNSS) during the offline data acquisition. The millimeter-wave feature points acquired in step 2.1 are then transformed to the inertial coordinate system using the vehicle pose, resulting in:

[0046]

[0047] Where M represents the high-precision positioning map obtained in the offline environment, s is the number of millimeter-wave radar keyframes in the offline environment, and T i For t iThe pose transformation matrix of the vehicle relative to the inertial coordinate system at time t. For t i The set of millimeter-wave radar feature points extracted at any given time.

[0048] Third, millimeter-wave radar data processing and positioning in online environments.

[0049] Step 3.1: Extract feature points from millimeter-wave radar images in an online environment.

[0050] In the online environment, only millimeter-wave radar is used for feature point extraction and vehicle localization. First, the raw millimeter-wave radar images acquired in the online environment are still coarsely filtered using the k-Strongest algorithm. The difference is that in the online environment, it's necessary to ensure that the extracted feature points are effective targets rather than simply retaining the majority of targets. Therefore, the value of k selected in the k-Strongest algorithm is kept as small as possible (e.g., k = 12).

[0051] Furthermore, the same procedure as in step 2.2 should be followed to further process the coarsely filtered millimeter-wave radar image to obtain the feature points required for localization. In an offline environment, planar points can be extracted from the LiDAR for fine filtering. However, LiDAR data is unavailable in an online environment; therefore, principal component analysis (PCA) is used to extract line features from the millimeter-wave image, replacing the LiDAR feature point data filtering process. The PCA method is as follows:

[0052] (3.1.1) Randomly select a point o on the coarsely filtered millimeter-wave radar image.

[0053] (3.1.2) Draw a circle with o as the center and r as the radius. Points within the radius are called points in the neighborhood of o.

[0054] (3.1.3) If the number of points in the neighborhood is less than 3, mark o as a non-feature point and return to (3.1.1).

[0055] (3.1.4) Calculate the covariance matrix of all points in the neighborhood and obtain the eigenvalues ​​of the covariance matrix. If there is an eigenvalue λ that is much larger than the other eigenvalues, then point o is considered a feature point. The distortion removal method in step 1.1 is used to insert the distorted point o into the feature point cloud O.

[0056] (3.1.5) Repeat the above process until all points are marked to obtain the millimeter-wave radar feature point set O.

[0057] Furthermore, the selection of the radius r mentioned in process (3.1.2) should be related to the distance from the point to the millimeter-wave radar. Assuming the angle between two adjacent measurement directions of the rotating millimeter-wave radar is θ, and the distance between the target point o in process (3.1.1) and the millimeter-wave radar is d, then the range of values ​​for the radius r is:

[0058] 2θd<r≤3θd (8)

[0059] Step 3.2: Match millimeter-wave radar feature points with high-precision positioning maps and output pose.

[0060] This invention uses the distance from a point to a line as the residual function and the pose transformation matrix as the variable to be optimized. By iteratively updating the pose transformation matrix to minimize the residual function, the optimal pose transformation matrix, i.e., the positioning information, can be obtained.

[0061] For t i The feature points of a single frame of millimeter-wave radar at a given time can be assumed to be the following set of feature points:

[0062]

[0063] Firstly, it can be done via t i-1 Pose transformation matrix at time step Rough estimate of t i The pose transformation at time t is Then A point p in k After being projected onto a high-precision map, the corresponding point q k for:

[0064]

[0065] Then you need to find a straight line from the high-precision map. k , and the point q mentioned above k Correspondingly. Through radius r k You can get q k The point set σ in the neighborhood k ={q i |d(q k ,q i )<r k}, where the function d(·) represents the calculation of the Euclidean distance between two points. This can be achieved using the point set σ. k Fitted straight line l k Such that point q on the line satisfies:

[0066] q = n k t+q k0 (11)

[0067] Where, n kq represents the direction of the line. k0 Let n represent any point on the line. k With q k0 All are parameters to be solved, totaling four unknown parameters. Therefore, the point set must contain at least four points. Point sets with fewer than four points will not be used to calculate the residual. The solution equation can then be constructed as follows:

[0068]

[0069] Then point q k The corresponding line l k The distance can be expressed as:

[0070]

[0071] For feature point set For every point in the map, a corresponding straight line can be found. Calculate the distance between each feature point and its corresponding straight line, and iteratively update the pose transformation matrix to minimize the sum of the distances between all feature points and their corresponding straight lines. This will yield the residual formula:

[0072]

[0073] By using the Gauss-Newton method and iteratively calculating to minimize the residual formula, the optimal pose transformation matrix can be obtained. Assume the objective function is e(T) = n k ×(T·p k -q k0 Then we have:

[0074]

[0075] Here, J(T) represents the Jacobian matrix of e(T) with respect to T, and the objective is to find the increment ΔT such that ||e(T)+J(T)ΔT|| 2 To reach the minimum, we can obtain:

[0076]

[0077] By differentiating the above formula and setting its derivative to zero, the optimal iteration step size ΔT can be obtained. * The calculation formula is:

[0078] J(T)J(T) T ΔT * =-J(T)e(T) (18)

[0079] The optimal pose transformation matrix can be obtained by iterative calculation through the above steps.

[0080] The beneficial effects of this invention are as follows:

[0081] Based on the strong penetrating power of millimeter waves, this invention uses millimeter-wave radar to achieve high-precision positioning of unmanned vehicles and helps improve the positioning accuracy of unmanned vehicles in adverse weather conditions. In other words, it can achieve high-precision positioning of unmanned vehicles and significantly improve positioning accuracy in adverse weather conditions. Attached Figure Description

[0082] Figure 1 This is a system flowchart. Detailed Implementation

[0083] The present invention will be further illustrated below with reference to examples.

[0084] This invention was tested on The Oxford Radar RobotCar Dataset. This dataset contains multiple data acquisitions of the same path. Each acquisition simultaneously collected LiDAR data, millimeter-wave radar data, inertial navigation data, and ground truth vehicle positioning data. The experimental process of this invention used two different data acquisitions, A and B, of the same path. Data acquisition A was used for offline mapping operations, and data acquisition B was used for online positioning operations.

[0085] The technical solution of this invention is as follows:

[0086] A localization method for unmanned vehicles based on millimeter-wave radar, comprising two parts: offline mapping and online localization.

[0087] (1) In an offline environment, high-precision maps are constructed by simultaneously using LiDAR and millimeter-wave radar data from data acquisition A.

[0088] (2) In the online environment, only the millimeter-wave radar data in data acquisition B is used to extract feature points and match them with the high-precision positioning map to output the pose.

[0089] The specific implementation steps of this invention are as follows:

[0090] 1. LiDAR data processing in offline mapping operations

[0091] Step 1.1: Use an inertial measurement unit to compensate for the distortion of lidar point cloud data.

[0092] The lidar used to collect point cloud data is a rotating lidar, which introduces point cloud distortion during vehicle operation. Therefore, distortion correction is necessary for the original lidar point cloud data. This invention corrects the vehicle pose transformation during point cloud data acquisition by using inertial measurement unit (IMU) measurements, thereby achieving point cloud distortion correction.

[0093] First, for the i-th frame of point cloud in data acquisition A, assume the starting time of point cloud acquisition is t. i R i P i With v i Let the rotation matrix, translation matrix, and velocity of the point cloud in the i-th frame be represented respectively. and b represents the measured values ​​output by the gyroscope and accelerometer in the i-th frame, respectively. g,i and b a,i These are the zero bias values ​​of the gyroscope and accelerometer in the i-th frame, respectively. The vehicle's pose transformation matrix from the start time of point cloud sampling in the i-th frame to the end time of point cloud sampling in the i-th frame can then be calculated using formula (1) and expressed by formula (2). Since the sampling times of points in a frame of point cloud are different, distortion correction of the point cloud can be achieved based on the sampling times of the points. Assume a certain point p... j The sampling time is t j Then the pose transformation matrix of the point can be obtained through formula (3).

[0094] Step 1.1 can be used to calculate the pose transformation of each point relative to the vehicle coordinate system at the start of the i-th frame point cloud, thereby converting the original lidar point cloud data into distortion-free point cloud data.

[0095] Step 1.2: Remove ground points from the distortion-free point cloud data.

[0096] The high-precision map is built using LiDAR, which collects 3D point cloud data that includes ground points. However, the positioning module uses millimeter-wave radar, which scans on a 2D horizontal plane, analogous to a single-line LiDAR, and cannot obtain ground point data. Therefore, it is necessary to remove ground points from the LiDAR point cloud. Specifically:

[0097] First, the distortion-reduced point cloud from step 1.1 is projected onto a two-dimensional plane perpendicular to the ground to form depth map I. i Each row of the depth map represents a single beam of light from the LiDAR, and each column represents the points collected by rotating the single beam 360°. The pixel value represents the distance from that point to the LiDAR. Assume a point p in the point cloud... k1 =(x j ,y j ,z j The coordinates projected onto the depth map are q. k1 =(u m ,v n Then, the conversion formula from three-dimensional point cloud projection to two-dimensional depth image can be obtained through formula (4).

[0098] For two adjacent points q in the vertical coordinate direction on the depth image k1 With qk2 The angle β between the line connecting these two points in three-dimensional space and the horizontal plane can be calculated using formula (6). k1,k2 .

[0099] When angle β k1,k2 When the angle is less than the threshold of 15°, q can be considered... k1 With q k2 The corresponding 3D points are all ground points. By traversing the depth image, all ground points are filtered out and removed to obtain the LiDAR point cloud data after removing ground points.

[0100] Step 1.3: Extract planar features from the point cloud data after removing ground points based on smoothness.

[0101] Since the point cloud data, even after removing ground points, still contains some dynamic objects and noise data, which can affect the final positioning accuracy, these two types of data need to be filtered out. Because planar points are generally large static objects such as building walls, this solution simplifies the process by extracting planar points instead of removing dynamic objects and noise data from the point cloud. The specific method is as follows:

[0102] (1.3.1) For a point p in the point cloud after removing the ground points, let the laser line at point p be l. i In the laser line l i Take several consecutive points to the left and right of point p, and denote them as the adjacent point set γ.

[0103] (1.3.2) Calculate the mean μ of the distance from all points in the adjacent point set γ to the lidar.

[0104] (1.3.3) Calculate the difference between the distance from point p to the lidar and the mean μ. If the difference is less than a fixed threshold of 0.1, mark the point as a planar point; otherwise, mark the point as a non-planar point.

[0105] (1.3.4) Return to step (1.3.1) and execute sequentially until all points in the point cloud are marked.

[0106] Through steps 1.1, 1.2 and 1.3 above, distortion-free planar feature points were extracted from the original lidar data. These feature points will be used to assist millimeter-wave radar in extracting effective features.

[0107] 2. Millimeter-wave data processing in offline mapping operations

[0108] Step 2.1: Extract features from millimeter-wave images acquired in an offline environment.

[0109] Because the raw millimeter-wave images contain a large amount of noise, it is necessary to extract feature points from the millimeter-wave images for mapping. This scheme uses a rotating frequency-modulated continuous-wave radar, which rotates continuously during the acquisition process to collect data in a 360° horizontal direction. Assume t... i The millimeter-wave radar image acquired at any time is Q i .

[0110] First, the k-Strongest algorithm is used to analyze Q. i Coarse filtering is performed. This method collects the first k points with the largest amplitude values ​​from each azimuth angle of the original millimeter-wave radar image. Since the mapping stage needs to preserve as many valid targets as possible in the millimeter-wave radar image, k is selected as 40 in the k-Strongest algorithm at this time. Then, the lidar feature point cloud extracted in step 1.3 is used to filter the coarsely filtered Q... i Perform fine filtering. The specific method is as follows:

[0111] (2.1.1) Centered on the lidar, divide the system into n concentric circles according to the range resolution of the millimeter-wave radar, and further divide the concentric circles into m equal-angled radii according to the angular resolution of the millimeter-wave radar. Any two adjacent radii and any two adjacent concentric circles can form a grid.

[0112] (2.2.2) Projecting the planar feature points of the lidar point cloud extracted in step 1.3 onto a horizontal plane will cause all points in the point cloud to fall into the grids described in (2.2.1). Let num(i,j) represent the number of projected points in the region enclosed by the i-th concentric circle, the (i-1)-th concentric circle, and the j-th and (j-1)-th radii. If num(i,j) > 2, then the grid is considered a valid grid.

[0113] (2.2.3) Remove the coarsely filtered Q using the same method as in step 1.1. i Point cloud distortion.

[0114] (2.2.4) When millimeter-wave radar data is projected onto a grid, points in the effective grid are considered as feature points in the millimeter-wave image and will be used for mapping.

[0115] Step 2.2: Establish a feature point map for millisecond radar.

[0116] In an offline environment, the vehicle pose is obtained by multiple measurements using the Global Navigation Satellite System (GNSS) when the data is collected in the offline environment. The millimeter-wave feature points collected in step 2.1 are transformed into the inertial coordinate system through the vehicle pose transformation, and a high-precision positioning map can be obtained by formula (7).

[0117] 3. Millimeter-wave radar data processing and positioning in online environments

[0118] Step 3.1: Extract feature points from millimeter-wave radar images in an online environment.

[0119] In the online environment, feature point extraction and vehicle localization are achieved using only the millimeter-wave radar from data acquisition B. First, the raw millimeter-wave radar image from data acquisition B is still coarsely filtered using the k-Strongest algorithm. The difference is that in the online environment, it's necessary to ensure that the extracted feature points are as effective as possible, rather than simply retaining the majority of targets. Therefore, k is set to 12 in the k-Strongest algorithm.

[0120] Furthermore, the same procedure as in step 2.2 should be followed to further process the coarsely filtered millimeter-wave radar image to obtain the feature points required for localization. In an offline environment, planar points can be extracted from the LiDAR for fine filtering. However, LiDAR data is unavailable in an online environment; therefore, principal component analysis (PCA) is used to extract line features from the millimeter-wave image, replacing the LiDAR feature point data filtering process. The PCA method is as follows:

[0121] (3.1.1) Randomly select a point o on the coarsely filtered millimeter-wave radar image.

[0122] (3.1.2) Draw a circle with o as the center and r as the radius. Points within the radius are called points in the neighborhood of o.

[0123] (3.1.3) If the number of points in the neighborhood is less than 3, mark o as a non-feature point and return to (3.1.1).

[0124] (3.1.4) Calculate the covariance matrix of all points in the neighborhood and obtain the eigenvalues ​​of the covariance matrix. If there is an eigenvalue λ that is much larger than the other eigenvalues, then point o is considered a feature point. The distortion removal method in step 1.1 is used to insert the distorted point o into the feature point cloud O.

[0125] (3.1.5) Repeat the above process until all points are marked to obtain the millimeter-wave radar feature point set O.

[0126] Furthermore, the selection of the radius r mentioned in process (3.1.2) above should be related to the distance from the point to the millimeter-wave radar. Since the angle between two adjacent measurement directions of the rotating millimeter-wave radar used in data acquisition B is 0.0157 radians, assuming the distance between the target point o in process (3.1.1) and the millimeter-wave radar is d, then the value of the radius r is 0.0471d.

[0127] Step 3.2: Match millimeter-wave radar feature points with high-precision positioning maps and output pose.

[0128] This invention uses the distance from a point to a line as the residual function and the pose transformation matrix as the variable to be optimized. By iteratively updating the pose transformation matrix to minimize the residual function, the optimal pose transformation matrix, i.e., the positioning information, can be obtained.

[0129] For t i The feature points of a single frame of millimeter-wave radar at a given time can be assumed to be:

[0130]

[0131] Firstly, it can be done via t i-1 pose transformation matrix at time step Rough estimate of t i The pose transformation at time t is Then A point p in k After being projected onto a high-precision map, the corresponding point q k for:

[0132]

[0133] Then you need to find a straight line from the high-precision map. k , and the point q mentioned above k Correspondingly. Through radius r k You can get q k The point set σ in the neighborhood k ={q i |d(q k ,q i ) <r k}, where the function d(·) represents the calculation of the Euclidean distance between two points. This can be achieved using the point set σ. k Fitted straight line l k Such that point q on the line satisfies:

[0134] q = n k t+q k0

[0135] Where, n k q represents the direction of the line. k0 Let n represent any point on the line. k With q k0 All are parameters to be solved, totaling four unknown parameters. Therefore, the point set must contain at least four points. Point sets with fewer than four points will not be used to calculate the residual. The solution equation can then be constructed as follows:

[0136] f 2 =‖q-q0‖2 -‖(q-q0) T n k || 2

[0137]

[0138] Then point q k The corresponding line l k The distance can be expressed as:

[0139]

[0140] For feature point set For every point in the map, a corresponding straight line can be found. Calculate the distance between each feature point and its corresponding straight line, and iteratively update the pose transformation matrix to minimize the sum of the distances between all feature points and their corresponding straight lines. This will yield the residual formula:

[0141]

[0142] By using the Gauss-Newton method and iteratively calculating to minimize the residual formula, the optimal pose transformation matrix can be obtained. Assume the objective function is e(T) = n k ×(T·p k -q k0 Then we have:

[0143]

[0144] Here, J(T) represents the Jacobian matrix of e(T) with respect to T, and the objective is to find the increment ΔT such that ||e(T)+J(T)ΔT|| 2 To reach the minimum, we can obtain:

[0145]

[0146] By differentiating the above formula and setting its derivative to zero, the optimal iteration step size ΔT can be obtained. * The calculation formula is:

[0147] J(T)J(T) T ΔT * =-J(T)e(T)

[0148] The optimal pose transformation matrix can be obtained by iterative calculation through the above steps. .

[0149] Figure 1The system flowchart can be broadly divided into three stages: offline LiDAR data processing, offline millimeter-wave radar data processing, and online millimeter-wave radar data processing. In the offline environment, LiDAR data undergoes distortion correction, ground point removal, and planar point extraction. Millimeter-wave radar data is processed using the k-Strongest algorithm and feature point extraction to ultimately create a high-precision localization map. In the online environment, millimeter-wave radar data undergoes k-Strongest and PCA algorithms to extract feature points, which are then matched with the high-precision map to output the pose.

[0150] The above-described embodiments are merely illustrative of the implementation methods of the present invention, but should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the protection scope of the present invention.

Claims

1. A method for locating unmanned vehicles based on millimeter-wave radar, characterized in that, The unmanned vehicle localization method comprises two parts: offline mapping and online localization. (1) In an offline environment, both lidar and millimeter-wave radar are used to collect data. The lidar data is used to assist in extracting feature points in the millimeter-wave image. Finally, the feature points of the millimeter-wave image are used to construct a high-precision positioning map. (2) In an online environment, feature points are extracted using millimeter-wave radar and matched with a high-precision positioning map to output the pose; Specifically, the following steps are included: First, LiDAR data processing during offline mapping. Step 1.1: Use an inertial measurement unit to compensate for distortion in the lidar point cloud data; Inertial measurement unit (IMU) measurement data is used to correct vehicle pose transformation during point cloud data acquisition, thereby achieving point cloud distortion removal. Step 1.2: Remove ground points from the distortion-corrected point cloud data; The high-precision map is built using LiDAR, which collects three-dimensional point cloud data that includes ground points. However, the positioning module uses millimeter-wave radar, which scans on a two-dimensional horizontal plane and cannot obtain ground point data. Therefore, it is necessary to remove ground points from the LiDAR point cloud. Step 1.3: Extract planar features from the point cloud data after removing ground points based on smoothness; Instead of removing dynamic objects and noise data from point clouds, planar points are extracted. Second, millimeter-wave data processing in offline mapping operations. Step 2.1: Extract features from millimeter-wave images acquired in an offline environment; Assumption The millimeter-wave radar images acquired at all times are First, the k-Strongest algorithm is used to... Coarse filtering is performed; this method acquires data from each azimuth angle of the raw millimeter-wave radar image. The point with the largest amplitude value; since the mapping process needs to save as many effective targets as possible from the millimeter-wave radar image, the k-Strongest algorithm selects at this point... Take the largest possible value; then use the lidar feature point cloud extracted in step 1.3 to apply the coarsely filtered result. Perform fine filtering; Step 2.2: Establish a feature point map for millisecond radar; In an offline environment, the vehicle pose is obtained by multiple measurements using a global navigation satellite system when data is collected offline. The millimeter-wave feature points collected in step 2.1 are transformed to the inertial coordinate system using the vehicle pose, resulting in: （7） in, This represents a high-precision positioning map obtained in an offline environment. This represents the number of keyframes for millimeter-wave radar in an offline environment. for The pose transformation matrix of the vehicle relative to the inertial coordinate system at time t. for The set of millimeter-wave radar feature points extracted at any given time; Third, millimeter-wave radar data processing and positioning in online environments. Step 3.1: Extract feature points from millimeter-wave radar images in an online environment; In the online environment, only millimeter-wave radar is used for feature point extraction and vehicle localization. Firstly, the raw millimeter-wave radar images acquired in the online environment are still coarsely filtered using the k-Strongest algorithm. In the online environment, it is necessary to ensure that the extracted feature points are effective targets rather than simply retaining the majority of targets; therefore, the k-Strongest algorithm selects... Try to keep the value as small as possible; In addition, the coarsely filtered millimeter-wave radar image is processed to obtain the feature points required for localization; in the offline environment, the image is finely filtered by extracting planar points from the LiDAR; however, in the online environment, LiDAR data cannot be obtained, so principal component analysis is used to extract line features in the millimeter-wave image, thereby replacing the filtering process using LiDAR feature point data. Step 3.2: Match millimeter-wave radar feature points with a high-precision positioning map and output the pose; Using the distance from a point to a line as the residual function and the pose transformation matrix as the variable to be optimized, the optimal pose transformation matrix, i.e., the localization information, can be obtained by iteratively updating the pose transformation matrix to minimize the residual function.

2. The unmanned vehicle positioning method based on millimeter-wave radar according to claim 1, characterized in that, The first step, the LiDAR data processing during offline mapping, is as follows: Step 1.1: Use an inertial measurement unit to compensate for distortion in the lidar point cloud data, as follows: set up Indicates the first The starting moment of the frame point cloud, , and They represent the first The rotation matrix, translation matrix, and velocity of the frame point cloud; and The first The measured values ​​output by the frame gyroscope and accelerometer. and The first The zero bias magnitudes of the frame gyroscope and accelerometer; then the trajectory calculation formula for the inertial measurement unit: （1） in, Describes the function operation from Lie algebra transformation to Lie group. Indicates the first The measurement time of the frame point cloud is obtained from formula (1). The vehicle's pose transformation matrix at time t is: （2） Since the sampling times of points in a single frame of point cloud are different, distortion correction of the point cloud is achieved based on the sampling times of the points; assuming a certain point The sampling time is Then the pose transformation matrix of that point is: （3） The above method can be used to calculate the relationship between each point and the first... The pose transformation of the vehicle coordinate system at the start of the frame point cloud transforms the original lidar point cloud data into distortion-free point cloud data. Step 1.2: Remove ground points from the distortion-reduced point cloud data, as follows: First, the distortion-reduced point cloud from step 1.1 is projected onto a two-dimensional plane perpendicular to the ground to form a depth map. Each row of the depth map represents a single beam of light from the LiDAR, and each column represents the points collected by rotating the single beam 360°. The pixel value represents the distance from a point to the LiDAR. Assuming a point in the point cloud... The coordinates projected onto the depth map are Then it is represented as: （4） in, They represent the first The first lidar beam and the first Each sampling point; while for the depth map and Points adjacent in the vertical direction It can be represented as: （5） Assuming the lidar is the first The angle between the laser beam and the ground is... , No. Each sampling point is located in the vehicle coordinate system. The included angle of the axis is ,but and The angle between the corresponding three-dimensional point and the horizontal plane is: （6） When the angle Less than the threshold At that time, it was believed and The corresponding 3D points are all ground points; traverse the depth image, filter out and remove all ground points to obtain the LiDAR point cloud data after removing ground points; Step 1.3: Extract planar features from the point cloud data after removing ground points based on smoothness, as follows: (1.3.1) For a point in the point cloud after removing ground points ,set up The laser line at the point is In the laser line superior Take several consecutive points to the left and right of a given point, and denote this as the set of adjacent points. ; (1.3.2) Calculate the set of adjacent points The average distance from all points to the lidar ; (1.3.3) Calculation Point-to-LiDAR distance and mean The difference, if the difference is less than a fixed threshold ,in If the condition is met, mark the point as a planar point; otherwise, mark the point as a non-planar point. (1.3.4) Return to step (1.3.1) and execute sequentially until all points in the point cloud have been marked; Through steps 1.1, 1.2 and 1.3 above, distortion-free planar feature points were extracted from the original lidar data. These feature points will be used to assist millimeter-wave radar in extracting effective features.

3. The unmanned vehicle positioning method based on millimeter-wave radar according to claim 1, characterized in that, In step 3.1, and in step 1.2, the threshold... .

4. The unmanned vehicle positioning method based on millimeter-wave radar according to claim 1, characterized in that, The specific method for step 2.1 is as follows: (2.1.1) Centered on lidar, the range resolution of millimeter-wave radar is divided into... Concentric circles, and according to the angular resolution of millimeter-wave radar, using equal angles... A radius divides concentric circles; any two adjacent radii and any two adjacent concentric circles form a grid. (2.2.2) Projecting the planar feature points of the lidar point cloud extracted in step 1.3 onto the horizontal plane, all points in the point cloud will fall into the grids of (2.2.1). Assuming... Indicates the first The concentric circles and the first The concentric circles and the first The radius of the first line and the first The number of projection points in the region enclosed by the radii; if If the grid is valid, then the grid is considered a valid grid. (2.2.3) Remove the coarsely filtered material using the same method as in step 1.

1. Point cloud distortion; (2.2.4) When millimeter-wave radar data is projected onto a grid, points in the effective grid are considered as feature points in the millimeter-wave image and will be used for mapping.

5. The unmanned vehicle positioning method based on millimeter-wave radar according to claim 3, characterized in that, In step 3.1, the principal component analysis method is as follows: (3.1.1) Randomly select a point on the coarsely filtered millimeter-wave radar image. ; (3.1.2) with With the center of the circle, Draw a circle with radius , and the points inside the radius are called points. Points in the neighborhood; (3.1.3) If the number of points in the neighborhood is less than 3, mark it. If it is a non-feature point, return (3.1.1); (3.1.4) Calculate the covariance matrix of all points in the neighborhood and obtain the eigenvalues ​​of the covariance matrix. If an eigenvalue exists... If it is much larger than other eigenvalues, then it is considered... The points are feature points, and the distortion correction method in step 1.1 is used to correct the distortion. Point insertion feature point cloud middle; (3.1.5) Repeat the above process until all points are marked to obtain the millimeter-wave radar feature point set. ; Furthermore, the radius described in the above process (3.1.2) The selection of the point should be related to the distance from the millimeter-wave radar; assuming the angle between two adjacent measurement directions of the rotating millimeter-wave radar is... The target point in process (3.1.1) The distance to the millimeter-wave radar is Then the radius The range of values ​​for is: （8）。 6. The unmanned vehicle positioning method based on millimeter-wave radar according to claim 1, characterized in that, Step 3.2 is as follows: for A single frame of millimeter-wave radar feature points at time 1, assuming the feature point set is: （9） Firstly, through Pose transformation matrix at time step Rough estimate The pose transformation at time t is = Then a point in After being projected onto a high-precision map, the corresponding points for: （10） Then find a straight line on the high-precision map. , with the above points Correspondingly; through radius Can obtain Point set in the neighborhood , where the function This represents calculating the Euclidean distance between two points using a point set. Fitted straight line The point on the line satisfy: （11） in, Indicates the direction of the line. Represents any point on a straight line; because and All are parameters to be solved, totaling four unknown parameters. Therefore, the point set must contain at least four points. Point sets with fewer than four points will not be used to calculate the residual. The solution equation is then constructed as follows: （12） （13） Then point The corresponding straight line The distance is expressed as: （14） For feature point set For every point in the map, a corresponding straight line can be found. Calculate the distance between each feature point and its corresponding straight line, and iteratively update the pose transformation matrix to minimize the sum of the distances between all feature points and their corresponding straight lines. This yields the residual formula: （15） By using the Gauss-Newton method and iteratively calculating to minimize the residual formula, the optimal pose transformation matrix can be obtained. Assume the objective function Then we have: （16） in, express right The Jacobian matrix, with the goal of seeking incremental values. , making To reach the minimum, we can obtain: （17） By differentiating the above formula and setting the derivative to zero, the optimal iteration step size can be obtained. The calculation formula is: （18） The optimal pose transformation matrix can be obtained by iterative calculation through the above steps. .

Images