Vehicle positioning method and system based on curbstone feature extraction and matching
By extracting and optimizing curb features in real time, constructing a curb point cloud map and performing feature matching, the problem of insufficient vehicle positioning accuracy in existing technologies is solved, achieving high-precision and stable vehicle positioning results.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- COWA TECHNOLOGY CO LTD
- Filing Date
- 2023-02-15
- Publication Date
- 2026-07-21
AI Technical Summary
Existing vehicle positioning methods based on lidar suffer from insufficient positioning accuracy and precision, especially when GPS signals are weak, resulting in low-quality roadside point cloud data acquired by lidar and thus large positioning errors.
By acquiring point cloud data from vehicle LiDAR in real time, the optimal curb features are extracted to construct a curb point cloud map. Feature matching and nonlinear optimization are then performed to obtain the optimized vehicle pose. Specific steps include point cloud data preprocessing, coarse candidate point cloud extraction, multiple fitting and confidence analysis, feature matching, and nonlinear optimization.
It improves the stability and accuracy of vehicle positioning, reduces lateral positioning errors, and is compatible with topological maps and point cloud maps, achieving high-precision vehicle positioning.
Smart Images
Figure CN116182877B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of intelligent driving positioning technology, specifically to a vehicle positioning method and system based on curb feature extraction and matching. Background Technology
[0002] LiDAR-based feature localization has become a crucial part of high-precision positioning in autonomous driving, especially when vehicles require high-precision positioning or when GPS signals are weak. Feature localization can improve positioning accuracy and stability. Feature localization methods commonly utilize environmental line and surface features, as well as manually placed fixed features. The robustness and complexity of features directly affect positioning performance. Road curbs, as common environmental features and relatively simple to extract, can improve the lateral high-precision positioning of vehicles.
[0003] Patent document CN111323802A discloses a vehicle positioning method, device, and equipment, relating to the field of intelligent driving technology, and particularly to the field of positioning technology. The technical solution disclosed in this application includes: acquiring point cloud data of the road surface collected by a laser sensor in the vehicle, and acquiring the vehicle's first positioning information; detecting the point cloud data to obtain first roadside information; and correcting the first positioning information based on the first roadside information to obtain the vehicle's second positioning information. However, this technical solution has low reliability for the roadside point cloud data acquired by the laser radar, resulting in significant errors in the subsequent correction results, and its positioning accuracy and precision have room for improvement. Summary of the Invention
[0004] In view of the deficiencies in the prior art, the purpose of this invention is to provide a vehicle localization method and system based on curb feature extraction and matching.
[0005] In a first aspect, according to the present invention, a vehicle localization method based on curb feature extraction and matching includes:
[0006] Step S1: Acquire point cloud data collected by the vehicle's LiDAR in real time and obtain the optimal curb features;
[0007] Step S2: Construct a curb point cloud map based on the collected point cloud data and obtain the curb features on the map;
[0008] Step S3: Perform feature matching and nonlinear optimization on the optimal curb features and the map curb features to finally obtain the optimized vehicle pose.
[0009] Preferably, step S1 includes:
[0010] Step S1.1: Acquire point cloud data collected by the vehicle's LiDAR in real time, and preprocess the point cloud data to select the region of interest;
[0011] Step S1.2: Extract and save coarse candidate point clouds for each line bundle point cloud of the region of interest;
[0012] Step S1.3: Filter the coarse candidate points to obtain a candidate point cloud;
[0013] Step S1.4: Perform multiple fittings and confidence analyses on the candidate point cloud to obtain the optimal curb features.
[0014] Preferably, step S1.1 includes:
[0015] Step S1.1.1: Collect curb data, extract single-frame point cloud data in real time, and determine if there is map input for the data collection location. If not, manually set the region of interest; if yes, proceed to step S1.1.2.
[0016] Step S1.1.2: Calculate the direction of the curb and the distance from the vehicle using the extracted single-frame point cloud and the map;
[0017] Step S1.1.3: Based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values. A preset threshold is then set to filter the point cloud segment closest to the curb in the map, thereby selecting the region of interest. The calculation formula for converting the map to the vehicle coordinate system is as follows:
[0018] X base =R*X utm +T
[0019] In the formula, X base Let X be the vehicle coordinate system. utm Let R and T be the coordinate system of the transverse Mercator grid system, where R and T represent the rotation and translation matrices of the initial positioning values, respectively.
[0020] The map input is an input map world coordinate system, which is projected onto the vehicle body coordinate system according to the vehicle pose.
[0021] The map includes high-precision maps or topological maps.
[0022] Preferably, step S1.2 includes:
[0023] Step S1.2.1: Sort each line bundle point cloud of the region of interest and gradually divide it into grids by sliding the window starting from the vehicle coordinate system;
[0024] Step S1.2.2: Calculate the distance between the current point coordinates and the lower point of the vehicle body coordinates, and use the reciprocal of the point distance as the sliding window size;
[0025] Step S1.2.3: Determine whether the angle between two points on the current point cloud is greater than the size of the sliding window. If yes, trigger step S1.2.3; otherwise, perform the determination again from the preset position of the sliding window.
[0026] Step S1.2.3: Take the point between the two points as a grid point, and determine whether the current grid is a valid grid according to the preset grid height threshold. If yes, trigger step S1.2.4; otherwise, mark the grid as a discarded grid.
[0027] Step S1.2.4: Calculate the elevation change of adjacent points within the effective grid to generate coarse candidate points. The calculation formula is as follows:
[0028] Angle = tan -1 (dz / dy)
[0029] In the formula, Angle represents the elevation change between adjacent points, and dz and dy represent the height difference and lateral difference between adjacent points, respectively;
[0030] Repeat steps S1.2.3 to S1.2.4 until all points on the complete wire harness point cloud are searched, and all coarse candidate points are saved.
[0031] Preferably, the method for filtering the coarse candidate points in step S1.3 includes density filtering and outlier removal filtering.
[0032] Preferably, step S1.4 includes:
[0033] Step S1.4.1: Perform R on the filtered candidate points. ansac Curve fitting or line fitting;
[0034] Step S1.4.2: Filter the fitted curves or lines and select the line with the highest confidence among the currently fitted curves or lines as the optimal curb point, thereby completing the extraction of the optimal curb features;
[0035] The filtering includes filtering the length of the fitted curve or straight line, the number of line bundle point clouds, and the curvature. The screening includes screening based on the distance between the fitted curve or straight line and the vehicle body, and the curve length.
[0036] Preferably, the construction of the curb point cloud map includes:
[0037] Step S2.1: Extract curb points based on the point cloud collected in real time in step S1;
[0038] Step S2.2: Calculate the pose of the curb point;
[0039] Step S2.3: Construct a curb point cloud map using SLAM.
[0040] Preferably, the feature matching and nonlinear optimization include:
[0041] Step S3.1: Obtain the curb point cloud map and construct a Kd-tree to store it;
[0042] Step S3.2: Using the initial pose, perform a radius search in the Kd-tree within a preset range to find the map curb points within the preset range;
[0043] Step S3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature point obtained in step S1 and the curb point on the map to obtain the following candidate matching point pair set:
[0044]
[0045] Where N represents the number of candidate matching point pairs, x represents the forward coordinate of the curb point, and dy represents the distance between the curb point and the map point;
[0046] Step S3.4: Perform linear fitting on the set of nearest candidate matching points to remove erroneous candidate matching point pairs;
[0047] Step S3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as a set of matching point pairs;
[0048] Step S3.6: Construct the residual using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula:
[0049]
[0050] In the formula, e is the constructed residual; n is the number of matching point pairs; p′ map_i p represents the coordinates of the i-th pose with error projected from the UTM coordinate system onto the vehicle coordinate system; base_i Let R represent the coordinates of the i-th point cloud in the vehicle coordinate system; R and t represent the quantities to be optimized.
[0051] Step S3.7: Optimize the pose to minimize the residual;
[0052] Repeat steps S3.5 to S3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.
[0053] Secondly, according to the present invention, a vehicle positioning system based on curb feature extraction and matching includes:
[0054] Module M1: Real-time acquisition of point cloud data collected by vehicle LiDAR and acquisition of optimal curb features;
[0055] Module M2: Constructs a curb point cloud map based on the collected point cloud data and obtains the curb features on the map;
[0056] Module M3: Performs feature matching and nonlinear optimization on the optimal curb features and the map curb features to finally obtain the optimized vehicle pose.
[0057] Preferably, the module M1 includes:
[0058] Module M1.1: Acquires point cloud data collected by the vehicle's LiDAR in real time, and preprocesses the point cloud data to select the region of interest;
[0059] Module M1.2: Extracts and saves coarse candidate point clouds for each line bundle point cloud of the region of interest;
[0060] Module M1.3: Filters the coarse candidate points to obtain a candidate point cloud;
[0061] Module M1.4: Performs multiple fittings on the candidate point cloud and analyzes the confidence level to obtain the optimal curb features.
[0062] Preferably, module M1.1 includes:
[0063] Module M1.1.1: Collects curb stones, extracts single-frame point clouds in real time, and determines whether there is map input for the collection location. If not, the region of interest is manually set; if so, module M1.1.2 is triggered.
[0064] Module M1.1.2: Calculates the direction of the curb and the distance from the vehicle using the real-time extracted single-frame point cloud and the map;
[0065] Module M1.1.3: Based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values. A preset threshold is set to filter the point cloud segment closest to the curb in the map, thereby selecting the region of interest. The calculation formula for converting the map to the vehicle coordinate system is as follows:
[0066] X base =R*X utm +T
[0067] In the formula, X base Let X be the vehicle coordinate system. utm Let R and T be the coordinate system of the transverse Mercator grid system, where R and T represent the rotation and translation matrices of the initial positioning values, respectively.
[0068] The map input is an input map world coordinate system, which is projected onto the vehicle body coordinate system according to the vehicle pose.
[0069] The map includes high-precision maps or topological maps.
[0070] Preferably, the module M1.2 includes:
[0071] Module M1.2.1: Sort each line bundle point cloud of the region of interest and gradually divide it into grids by sliding the window starting from the vehicle coordinate system;
[0072] Module M1.2.2: Calculates the distance between the current point coordinates and the lower point of the vehicle body coordinates, and uses the reciprocal of the distance as the sliding window size;
[0073] Module M1.2.3: Determine whether the angle between two points on the current point cloud is greater than the size of the sliding window. If yes, trigger module M1.2.3; otherwise, perform the determination again from the preset position of the sliding window.
[0074] Module M1.2.3: Treat the point between the two points as a grid point, and determine whether the current grid is a valid grid based on the preset grid height threshold. If it is, then trigger module M1.2.4; otherwise, mark the grid as a discarded grid.
[0075] Module 1.2.4: Calculate the elevation change of adjacent points within the effective grid to generate coarse candidate points. The calculation formula is as follows:
[0076] Angle = tan -1 (dz / dy)
[0077] In the formula, Angle represents the elevation change between adjacent points, and dz and dy represent the height difference and lateral difference between adjacent points, respectively;
[0078] Repeatedly trigger modules M1.2.3 to M1.2.4 until searching for points on the complete wire harness point cloud and saving all coarse candidate points.
[0079] Preferably, the method for filtering the coarse candidate points in module M1.3 includes density filtering and outlier removal filtering.
[0080] Preferably, the module M1.4 includes:
[0081] Module M1.4.1: Perform Ransac curve fitting or line fitting on the filtered candidate points;
[0082] Module M1.4.2: Filters the fitted curves or lines and selects the line with the highest confidence among the currently fitted curves or lines as the optimal curb point, thereby completing the extraction of the optimal curb features;
[0083] The filtering includes filtering the length of the fitted curve or straight line, the number of line bundle point clouds, and the curvature. The screening includes screening based on the distance between the fitted curve or straight line and the vehicle body, and the curve length.
[0084] Preferably, the construction of the curb point cloud map includes:
[0085] Module M2.1: Extract curb points based on the point cloud collected in real time in module M1;
[0086] Module M2.2: Calculates the pose of the curb point;
[0087] Module M2.3: Constructs a curb point cloud map using SLAM.
[0088] Preferably, the feature matching and nonlinear optimization include:
[0089] Module M3.1: Obtains the curb point cloud map and constructs a Kd-tree to store it;
[0090] Module M3.2: Searches for the map curb points within the preset range by performing a radius search in the Kd-tree based on the initial pose;
[0091] Module M3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature points obtained by module M1 and the curb points on the map, and obtain the following set of candidate matching point pairs:
[0092]
[0093] Where N represents the number of candidate matching point pairs, x represents the forward coordinate of the curb point, and dy represents the distance between the curb point and the map point;
[0094] Module M3.4: Performs linear fitting on the set of nearest candidate matching point pairs to remove erroneous candidate matching point pairs;
[0095] Module M3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as a set of matching point pairs;
[0096] Module M3.6: Constructs residuals using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula:
[0097]
[0098] In the formula, e is the constructed residual; n is the number of matching point pairs; p′ map_i p represents the coordinates of the i-th pose with error projected from the UTM coordinate system onto the vehicle coordinate system; base_iLet R represent the coordinates of the i-th point cloud in the vehicle coordinate system; R and t represent the quantities to be optimized.
[0099] Module M3.7: Optimizes the pose to minimize the residual;
[0100] Repeatedly trigger modules M3.5 to M3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.
[0101] Compared with the prior art, the present invention has the following beneficial effects:
[0102] 1. This invention extracts curb features in real time and uses curb feature point clouds for vehicle localization, which can reduce lateral positioning errors and improve the stability and accuracy of positioning.
[0103] 2. This invention uses the Ring characteristics of LiDAR point clouds to find candidate curb points from a local perspective, and then fits the optimal curb from a global perspective.
[0104] 3. This invention organizes the point cloud, uses the height change of the sliding window to dynamically find candidate points, removes erroneous candidate points by point cloud density, outlier removal and other methods, and fits the candidate points multiple times, sets different constraints as the confidence of the fitting line, and obtains the optimal fit.
[0105] 4. This invention is compatible with both topological maps and point cloud maps. Attached Figure Description
[0106] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:
[0107] Figure 1 A schematic diagram of the workflow of this invention.
[0108] Figure 2 This is a schematic diagram of the workflow of one embodiment of the present invention.
[0109] Figure 3 This is a schematic diagram of the feature extraction process for road teeth in this invention.
[0110] Figure 4 This is a schematic diagram of the point cloud preprocessing flow for feature extraction of road teeth in this invention.
[0111] Figure 5 This is a schematic diagram of the Ring filtering and candidate point filtering process for curb feature extraction in this invention.
[0112] Figure 6 This is a schematic diagram of the Ransac fitting and confidence process for feature extraction of road teeth in this invention.
[0113] Figure 7 This is a schematic diagram of the feature matching and optimization process for road teeth in this invention.
[0114] Figure 8 This is a rendering of the effect of extracting curb features from map point clouds in this invention.
[0115] Figure 9 This is a diagram illustrating the effect of real-time extraction of frame tooth features in this invention.
[0116] Figure 10 This is a comparison chart of errors in the localization of curb features in this invention.
[0117] Figure 11 This is a trajectory comparison diagram of the feature localization of the curb in this invention. Detailed Implementation
[0118] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.
[0119] The input to this invention is a real-time point cloud, a curb point cloud map, or a topology map, along with a noisy vehicle pose; the output is an optimized vehicle pose. This invention comprises three parts: real-time curb feature extraction, curb point cloud map construction, and feature matching and nonlinear optimization. Figure 2 As shown. In the real-time curb feature extraction part, the real-time point cloud undergoes region of interest filtering, radar Ring (beam) characteristic filtering, Ring candidate point generation, candidate point filtering, Ransac fitting, and confidence design to obtain the optimal curb features. The curb point cloud map obtains curb features in the UTM coordinate system through offline point cloud feature extraction and pose transformation. After feature extraction, feature matching and nonlinear optimization are performed to obtain high-precision vehicle pose. This invention only optimizes the y and yaw degrees of freedom in the vehicle coordinate system.
[0120] According to the present invention, a vehicle localization method based on curb feature extraction and matching is provided, such as... Figure 1 and 2 As shown, it includes:
[0121] Step S1: Acquire point cloud data from the vehicle's LiDAR in real time and obtain the optimal curb features. In addition to point cloud data, this also includes acquiring IMU, GPS, and visual data, specifically, such as... Figure 3As shown, from a local perspective, the obvious difference between the curb feature and other point cloud clusters is that it undergoes a certain degree of convexity in the elevation direction. Based on this, the point cloud is segmented to obtain candidate points. From a global perspective, the curb feature also has the characteristics of a curve. Therefore, curve fitting is performed on the candidate points to finally extract the curb points.
[0122] In one embodiment, step S1 includes:
[0123] Step S1.1: Acquire point cloud data collected by the vehicle's LiDAR in real time, and preprocess the point cloud data to select a region of interest. This region of interest selection step can filter out most of the locations in the point cloud that are not related to the curb, improving detection stability and calculation speed. Specifically, as follows... Figure 4 As shown, firstly, the LiDAR captures data about the curb, extracts single-frame point clouds in real time, and determines if there is map input for the data collection location. If not, the region of interest (ROI) is manually set. If so, the direction of the curb and its distance from the vehicle are calculated using the extracted single-frame point cloud and the map. Then, based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values. A preset threshold is set to filter the point cloud segment closest to the curb in the map, thereby selecting the ROI. The formula for converting the map to the vehicle coordinate system is as follows:
[0124] X base =R*X utm +T
[0125] In the formula, X base Let X be the vehicle coordinate system. utm The coordinate system is the transverse Mercator grid system, where R and T represent the rotation and translation matrices of the initial positioning values, respectively. The map input is the world coordinate system of the input map, projected onto the vehicle body coordinate system based on the vehicle's pose. The map includes high-precision maps or topological maps.
[0126] Step S1.2: Extract and save coarse candidate point clouds for each line bundle point cloud of the region of interest. For example... Figure 5As shown, step S1.2 includes: Step S1.2.1: Sort each line bundle point cloud of the region of interest, and gradually divide the grid by sliding the window starting from the vehicle coordinate system. Step S1.2.2: Calculate the distance between the current point coordinate and the lower point of the vehicle coordinate system, and use the reciprocal of the point distance as the sliding window size. Step S1.2.3: Determine whether the angle between two points on the current line bundle point cloud is greater than the sliding window size. If yes, trigger step S1.2.3; if no, perform the determination again from the preset position of the sliding window. Step S1.2.3: Take the point between the two points as a grid point, and determine whether the current grid is a valid grid according to the preset grid height threshold. If yes, trigger step S1.2.4; if no, mark the grid as a discarded grid. Step S1.2.4: Calculate the elevation change of adjacent points within the valid grid to generate coarse candidate points, where the elevation change can be represented by the vertical angle change of adjacent points, and the calculation formula is as follows:
[0127] Angle = tan -1 (dz / dy)
[0128] In the formula, Angle represents the elevation change between adjacent points, and dz and dy represent the height difference and lateral difference between adjacent points, respectively. Repeat steps S1.2.3 to S1.2.4 until points on the complete line bundle point cloud are searched, and all coarse candidate points are saved.
[0129] In other words, the coarse candidate curb points are primarily filtered based on elevation changes. First, each Ring point is sorted laterally. Then, starting from the vehicle coordinate system, a grid is gradually divided using a sliding window method. The size of the sliding window is the reciprocal of the distance between points, and the next sliding window begins at one-fifth of the distance from the previous window. This sliding window method effectively avoids situations where the actual curb is not within the grid, improving the accuracy of the coarse candidate points.
[0130] Step S1.3: Filter the coarse candidate points to obtain a candidate point cloud. The coarse candidate point filtering methods include density filtering and outlier removal filtering. The purpose of this step is to prevent some Ring candidate points from becoming too large and to filter out obviously erroneous candidate points, thus avoiding negative impacts when designing confidence levels for curve fitting.
[0131] Step S1.4: Perform multiple fittings and confidence analyses on the candidate point cloud to obtain the optimal curb features. For example... Figure 6As shown, the filtered candidate points are first fitted with Ransac curves or lines. Then, the fitted curves or lines are filtered, and the line with the highest confidence among the fitted curves or lines is selected as the optimal curb point, thus completing the extraction of the optimal curb features. Specifically, filtering includes filtering based on the length, number of line bundle points, and curvature of the fitted curves or lines, removing unreasonable curves. The selection also includes filtering based on the distance between the fitted curve or line and the vehicle body, and the curve length. The finally extracted curb points can be used for laser calibration, mapping, and localization.
[0132] Step S2: Construct a curb point cloud map based on the collected point cloud data and obtain the curb features on the map. Constructing the curb point cloud map includes:
[0133] Step S2.1: Extract curb points based on the point cloud collected in real time in step S1.
[0134] Step S2.2: Calculate the pose of the curb point.
[0135] Step S2.3: Construct a curb point cloud map using SLAM.
[0136] Step S3: Perform feature matching and nonlinear optimization on the optimal curb features and the map curb features to finally obtain the optimized vehicle pose. The feature matching and nonlinear optimization include:
[0137] Step S3.1: Obtain the curb point cloud map and construct a Kd-tree to store it.
[0138] Step S3.2: Using the initial pose, perform a radius search in the Kd-tree within a preset range to find the map curb points within the preset range.
[0139] Step S3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature point obtained in step S1 and the curb point on the map to obtain the following candidate matching point pair set:
[0140]
[0141] Where N represents the number of candidate matching point pairs, x represents the forward coordinate of the curb point, and dy represents the distance between the curb point and the map point.
[0142] Step S3.4: Perform linear fitting on the set of nearest candidate matching points to remove erroneous candidate matching point pairs.
[0143] Step S3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as the matching point pair set.
[0144] Step S3.6: Construct the residual using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula:
[0145]
[0146] In the formula, e is the constructed residual; n is the number of matching point pairs; p′ map_i p represents the coordinates of the i-th pose with error projected from the UTM coordinate system onto the vehicle coordinate system; base_i Let represent the coordinates of the i-th point cloud in the vehicle coordinate system; R and t represent the quantities to be optimized. For example, match the point pair Match{plidar, pmap'} with the pose Pose, where y and yaw are optimization variables while x, z, roll, and pitch are constants.
[0147] Step S3.7: Optimize the pose to minimize the residual. For example, use Levenberg-Marquardt to optimize the pose and minimize the residual from step S3.6.
[0148] Repeat steps S3.5 to S3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.
[0149] Furthermore, the curb features extracted from the map point cloud in this invention are as follows: Figure 8 As shown in the figure, the continuous dots represent curb feature points. The real-time frame curb feature extraction in this invention is as follows: Figure 9 As shown, the vertical dashed lines intersecting the circular point cloud represent the extracted curb features, while the white dots represent the original point cloud. The error comparison for curb feature localization in this invention is as follows: Figure 10 As shown, the upper points represent the error before optimization, and the lower points represent the error after optimization. Trajectory comparison example. Figure 11 As shown in the figure, the error is the lateral distance between the pose and the ground truth, where the ground truth is the pose of the constructed map, and the input pose is the ground truth with 20cm-30cm of noise added. It can be seen that using curb feature point clouds for localization can control the lateral localization error to within 5cm.
[0150] The present invention also provides a vehicle positioning system based on curb feature extraction and matching. Those skilled in the art can implement the vehicle positioning system based on curb feature extraction and matching by executing the steps of the vehicle positioning method based on curb feature extraction and matching. That is, the vehicle positioning method based on curb feature extraction and matching can be understood as a preferred embodiment of the vehicle positioning system based on curb feature extraction and matching.
[0151] A vehicle localization system based on curb feature extraction and matching according to the present invention includes:
[0152] Module M1: Acquires point cloud data from the vehicle's LiDAR in real time and obtains the optimal curb features. Module M1 includes:
[0153] Module M1.1: Acquires point cloud data collected by the vehicle's LiDAR in real time and preprocesses the point cloud data to select the region of interest. Module M1.1 includes: Module M1.1.1: The LiDAR collects data on the curb, extracts a single frame of point cloud data in real time, and determines whether there is map input for the data collection location. If not, the region of interest is manually set; if so, module M1.1.2 is triggered. Module M1.1.2: Calculates the direction of the curb and its distance from the vehicle using the extracted single frame of point cloud data and the map. Module M1.1.3: Based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values, and a preset threshold is set to filter the point cloud segment closest to the curb in the map, thereby selecting the region of interest. The calculation formula for converting the map to the vehicle coordinate system is as follows:
[0154] X base =R*X utm +T
[0155] In the formula, X base Let X be the vehicle coordinate system. utm The coordinate system is a transverse Mercator grid system, where R and T represent the rotation and translation matrices of the initial positioning values, respectively. The map input is an input map world coordinate system, projected onto the vehicle body coordinate system based on the vehicle pose. The map includes high-precision maps or topological maps.
[0156] Module M1.2: Extracts and saves coarse candidate point clouds for each line bundle point cloud of the region of interest. Module M1.2 includes:
[0157] Module M1.2.1: Sorts each line bundle point cloud in the region of interest and gradually divides the grid by sliding a window starting from the vehicle coordinate system. Module M1.2.2: Calculates the distance between the current point coordinate and the lower point in the vehicle coordinate system, and uses the reciprocal of this distance as the sliding window size. Module M1.2.3: Determines whether the angle between two points in the current line bundle point cloud is greater than the sliding window size. If yes, triggers module M1.2.3; otherwise, repeats the determination from the preset position of the sliding window. Module M1.2.3: Treats the point between the two points as a grid point and determines whether the current grid is a valid grid based on a preset grid height threshold. If yes, triggers module M1.2.4; otherwise, marks the grid as a discarded grid. Module M1.2.4: Calculates the elevation change of adjacent points within the valid grid to generate coarse candidate points. The calculation formula is as follows:
[0158] Angle = tan -1 (dz / dy)
[0159] In the formula, Angle represents the elevation change between adjacent points, and dz and dy represent the height difference and lateral difference between adjacent points, respectively. Modules M1.2.3 to M1.2.4 are repeatedly triggered until points on the complete wire bundle point cloud are searched, and all coarse candidate points are saved.
[0160] Module M1.3: Filters the coarse candidate points to obtain a candidate point cloud. The methods used in module M1.3 to filter the coarse candidate points include density filtering and outlier removal filtering.
[0161] Module M1.4: Performs multiple fittings and confidence analyses on the candidate point cloud to obtain the optimal curb features. Module M1.4 includes:
[0162] Module M1.4.1: Performs Ransac curve fitting or straight line fitting on the filtered candidate points. Module M1.4.2: Filters the fitted curves or lines and selects the line with the highest confidence among the currently fitted curves or lines as the optimal curb point, thereby completing the extraction of the optimal curb features. The filtering includes filtering based on the length, number of line bundle point clouds, and curvature of the fitted curves or lines. The selection includes selection based on the distance between the fitted curve or line and the vehicle body, and the curve length.
[0163] Module M2: Constructs a curb point cloud map based on the collected point cloud data and obtains curb features from the map. The construction of the curb point cloud map includes: Module M2.1: Extracting curb points based on the point cloud collected in real time in Module M1. Module M2.2: Calculating the pose of the curb points. Module M2.3: Constructing the curb point cloud map using SLAM.
[0164] Module M3: Performs feature matching and nonlinear optimization on the optimal curb features and the map curb features to obtain the optimized vehicle pose. The feature matching and nonlinear optimization include:
[0165] Module M3.1: Obtains the curb point cloud map and constructs a Kd-tree to store it.
[0166] Module M3.2: Searches for the map curb points within the preset range by performing a radius search in the Kd-tree based on the initial pose.
[0167] Module M3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature points obtained by module M1 and the curb points on the map, and obtain the following set of candidate matching point pairs:
[0168]
[0169] Where N represents the number of candidate matching point pairs, x represents the forward coordinate of the curb point, and dy represents the distance between the curb point and the map point.
[0170] Module M3.4: Performs linear fitting on the set of nearest candidate matching point pairs to remove erroneous candidate matching point pairs.
[0171] Module M3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as a set of matching point pairs.
[0172] Module M3.6: Constructs residuals using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula:
[0173]
[0174] In the formula, e is the constructed residual; n is the number of matching point pairs; p′ map_i p represents the coordinates of the i-th pose with error projected from the UTM coordinate system onto the vehicle coordinate system; base_i R represents the coordinates of the i-th point cloud in the vehicle coordinate system; R and t represent the quantities to be optimized.
[0175] Module M3.7: Optimizes the pose to minimize the residual.
[0176] Repeatedly trigger modules M3.5 to M3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.
[0177] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.
[0178] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.
Claims
1. A vehicle localization method based on curb feature extraction and matching, characterized in that, include: Step S1: Acquire point cloud data collected by the vehicle's LiDAR in real time and obtain the optimal curb features; Step S2: Construct a curb point cloud map based on the collected point cloud data and obtain the curb features on the map; Step S3: Perform feature matching and nonlinear optimization on the optimal curb features and the map curb features to finally obtain the optimized vehicle pose; Step S1 includes: Step S1.1: Acquire point cloud data collected by the vehicle's LiDAR in real time, and preprocess the point cloud data to select the region of interest; Step S1.2: Extract and save coarse candidate point clouds for each line bundle point cloud of the region of interest; Step S1.3: Filter the coarse candidate point cloud to obtain a candidate point cloud; Step S1.4: Perform multiple fittings and confidence analyses on the candidate point cloud to obtain the optimal curb features; Step S1.1 includes: Step S1.1.1: Collect curb data, extract single-frame point cloud data in real time, and determine if there is map input at the collection point. If not, manually set the region of interest and proceed to step S1.2; if yes, proceed to step S1.1.
2. Step S1.1.2: Calculate the direction of the curb and the distance from the vehicle using the extracted single-frame point cloud and the map; Step S1.1.3: Based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values. A preset threshold is then set to filter the point cloud segment closest to the curb in the map, thereby selecting the region of interest. The calculation formula for converting the map to the vehicle coordinate system is as follows: In the formula, For the vehicle body coordinate system, The coordinate system is the transverse Mercator grid system. and These represent the rotation and translation matrices for the initial positioning values, respectively. The map input is an input map world coordinate system, which is projected onto the vehicle body coordinate system according to the vehicle pose. The map includes high-precision maps or topological maps.
2. The vehicle localization method based on curb feature extraction and matching according to claim 1, characterized in that, Step S1.2 includes: Step S1.2.1: Sort each line bundle point cloud of the region of interest and gradually divide it into grids by sliding the window starting from the vehicle coordinate system; Step S1.2.2: Calculate the distance between the current point coordinates and the lower point of the vehicle body coordinates, and use the reciprocal of the point distance as the sliding window size; Step S1.2.3: Determine whether the angle between two points on the current point cloud is greater than the sliding window size. If yes, trigger step S1.2.4; if no, perform the determination again from the preset position of the sliding window and execute step S1.2.
1. Step S1.2.4: Take the point between the two points as a grid point, and determine whether the current grid is a valid grid according to the preset grid height threshold. If yes, then trigger step S1.2.5; if no, then mark the grid as a discarded grid. Step S1.2.5: Calculate the elevation change of adjacent points within the effective grid to generate coarse candidate points. The calculation formula is as follows: In the formula, Indicates the elevation change of adjacent points. and These represent the height difference and lateral difference between adjacent points, respectively. Repeat steps S1.2.3 to S1.2.5 until points are searched on the complete wire harness point cloud, and save all coarse candidate points.
3. The vehicle localization method based on curb feature extraction and matching according to claim 1, characterized in that, The filtering methods for the coarse candidate points in step S1.3 include density filtering and outlier removal filtering.
4. The vehicle localization method based on curb feature extraction and matching according to claim 1, characterized in that, Step S1.4 includes: Step S1.4.1: Process the filtered candidate points Ransac Curve fitting or line fitting; Step S1.4.2: Filter the fitted curves or lines and select the line with the highest confidence among the currently fitted curves or lines as the optimal curb point, thereby completing the extraction of the optimal curb features; The filtering includes filtering the length of the fitted curve or straight line, the number of line bundle point clouds, and the curvature. The screening includes screening based on the distance between the fitted curve or straight line and the vehicle body, and the curve length.
5. The vehicle localization method based on curb feature extraction and matching according to claim 1, characterized in that, The construction of the curb point cloud map includes: Step S2.1: Extract curb points based on the point cloud collected in real time in step S1; Step S2.2: Calculate the pose of the curb point; Step S2.3: Through SLAM Construct a curb point cloud map.
6. The vehicle localization method based on curb feature extraction and matching according to claim 1, characterized in that, The feature matching and nonlinear optimization include: Step S3.1: Obtain the curb point cloud map and construct it. Kd-tree Storage; Step S3.2: Using the initial pose in Kd-tree The system performs a radius search within a preset range to find curb points on the map within that range. Step S3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature points obtained in step S1 and the curb points on the map, and obtain the following set of nearest candidate matching point pairs: in, N Indicates the number of candidate matching pairs. x Indicates the forward coordinates of the curb point. dy This represents the distance between the curb point and the map point; Step S3.4: Perform linear fitting on the set of nearest candidate matching points to remove erroneous candidate matching point pairs; Step S3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as a set of matching point pairs; Step S3.6: Construct the residual using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula: In the formula, The residuals of the construction; n To match the number of point pairs; express UTM The first coordinate system i The pose with error is projected onto the coordinates in the vehicle coordinate system; In the vehicle coordinate system, the first... i Point cloud coordinates; R and t This indicates the quantity to be optimized; Step S3.7: Optimize the pose to minimize the residual; Repeat steps S3.5 to S3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.
7. A vehicle positioning system based on curb feature extraction and matching, characterized in that, include: Module M1: Real-time acquisition of point cloud data collected by vehicle LiDAR and acquisition of optimal curb features; Module M2: Constructs a curb point cloud map based on the collected point cloud data and obtains the curb features on the map; Module M3: Performs feature matching and nonlinear optimization on the optimal curb features and the map curb features to finally obtain the optimized vehicle pose; The module M1 includes: Module M1.1: Acquires point cloud data collected by the vehicle's LiDAR in real time, and preprocesses the point cloud data to select the region of interest; Module M1.2: Extracts and saves coarse candidate point clouds for each line bundle point cloud of the region of interest; Module M1.3: Filters the coarse candidate point cloud to obtain a candidate point cloud; Module M1.4: Performs multiple fittings on the candidate point cloud and analyzes the confidence level to obtain the optimal curb features; The module M1.1 includes: Module M1.1.1: Collect curb data, extract single-frame point cloud data in real time, and determine if there is map input for the data collection location. If not, manually set the region of interest and execute module M1.
2. If yes, trigger module M1.1.
2. Module M1.1.2: Calculates the direction of the curb and the distance from the vehicle using the real-time extracted single-frame point cloud and the map; Module M1.1.3: Based on the direction of the curb and its distance from the vehicle, the input map is converted to the vehicle coordinate system using initial positioning values. A preset threshold is set to filter the point cloud segment closest to the curb in the map, thereby selecting the region of interest. The calculation formula for converting the map to the vehicle coordinate system is as follows: In the formula, For the vehicle body coordinate system, The coordinate system is the transverse Mercator grid system. and These represent the rotation and translation matrices for the initial positioning values, respectively. The map input is an input map world coordinate system, which is projected onto the vehicle body coordinate system according to the vehicle pose. The map includes high-precision maps or topological maps.
8. The vehicle positioning system based on curb feature extraction and matching according to claim 7, characterized in that, The module M1.2 includes: Module M1.2.1: Sort each line bundle point cloud of the region of interest and gradually divide it into grids by sliding the window starting from the vehicle coordinate system; Module M1.2.2: Calculates the distance between the current point coordinates and the lower point of the vehicle body coordinates, and uses the reciprocal of the distance as the sliding window size; Module M1.2.3: Determine whether the angle between two points on the current wire bundle point cloud is greater than the sliding window size. If yes, trigger module M1.2.4; if no, perform the determination again from the preset position of the sliding window and execute module M1.2.
1. Module M1.2.4: Treat the point between the two points as a grid point, and determine whether the current grid is a valid grid based on the preset grid height threshold. If it is, then trigger module M1.2.5; otherwise, mark the grid as a discarded grid. Module 1.2.5: Calculates the elevation change of adjacent points within the effective grid to generate coarse candidate points. The calculation formula is as follows: In the formula, Indicates the elevation change of adjacent points. and These represent the height difference and lateral difference between adjacent points, respectively. Repeatedly trigger modules M1.2.3 through M1.2.5 until searching for points on the complete wire harness point cloud and saving all coarse candidate points.
9. The vehicle positioning system based on curb feature extraction and matching according to claim 7, characterized in that, The filtering methods for the coarse candidate points in module M1.3 include density filtering and outlier removal filtering.
10. The vehicle positioning system based on curb feature extraction and matching according to claim 7, characterized in that, The module M1.4 includes: Module M1.4.1: Processes the filtered candidate points. Ransac Curve fitting or line fitting; Module M1.4.2: Filters the fitted curves or lines and selects the line with the highest confidence among the currently fitted curves or lines as the optimal curb point, thereby completing the extraction of the optimal curb features; The filtering includes filtering the length of the fitted curve or straight line, the number of line bundle point clouds, and the curvature. The screening includes screening based on the distance between the fitted curve or straight line and the vehicle body, and the curve length.
11. The vehicle positioning system based on curb feature extraction and matching according to claim 7, characterized in that, The construction of the curb point cloud map includes: Module M2.1: Extract curb points based on the point cloud collected in real time in module M1; Module M2.2: Calculates the pose of the curb point; Module M2.3: via SLAM Construct a curb point cloud map.
12. The vehicle positioning system based on curb feature extraction and matching according to claim 7, characterized in that, The feature matching and nonlinear optimization include: Module M3.1: Obtain curb point cloud map and construct Kd-tree Storage; Module M3.2: Based on the initial pose Kd-tree The system performs a radius search within a preset range to find the curb points on the map within that preset range. Module M3.3: Perform Euclidean distance nearest neighbor matching between the optimal curb feature points obtained by module M1 and the curb points on the map, and obtain the following set of nearest candidate matching point pairs: in, N Indicates the number of candidate matching pairs. x Indicates the forward coordinates of the curb point. dy This represents the distance between the curb point and the map point; Module M3.4: Performs linear fitting on the set of nearest candidate matching point pairs to remove erroneous candidate matching point pairs; Module M3.5: For the filtered matching point pairs and curb point cloud map, perform Euclidean nearest neighbor matching again based on the current pose, set a distance threshold, remove matching point pairs that are greater than the distance threshold, and use the remaining matching point pairs as a set of matching point pairs; Module M3.6: Constructs residuals using all matching point pairs and poses in the set of matching point pairs, as shown in the following formula: In the formula, The residuals of the construction; n To match the number of point pairs; express UTM The first coordinate system i The pose with error is projected onto the coordinates in the vehicle coordinate system; In the vehicle coordinate system, the first... i Point cloud coordinates; R and t This indicates the quantity to be optimized; Module M3.7: Optimizes the pose to minimize the residual; Repeatedly trigger modules M3.5 to M3.7 until the preset number of iterations and termination conditions are met, to obtain the optimized pose and covariance matrix.