Positioning Method and System Based on Feature Extraction and Matching of Laser Point Cloud Rod-shaped Objects

Abstract

The present invention provides a positioning method and system based on feature extraction and matching of rod-shaped objects from lidar point clouds. The method includes the following steps: Step S1: Use a data acquisition vehicle equipped with a lidar to scan the environment to obtain a lidar point cloud map; Step S2: Perform height filtering, clustering, rough feature extraction, and fine feature extraction on the lidar point cloud map to obtain map rod-shaped object features; Step S3: Use the vehicle's lidar to scan the environment around the vehicle to obtain real-time lidar point clouds; Step S4: Perform height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on the real-time lidar point clouds to obtain real-time rod-shaped object features; Step S5: Perform feature matching and non-linear optimization on the map rod-shaped object features and the real-time rod-shaped object features to obtain the vehicle pose. The present invention comprehensively uses height filtering, clustering, rough and fine feature extraction methods to extract rod-shaped object features from the lidar point cloud map, improving the vehicle positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of laser positioning. Specifically, it relates to a positioning method and system based on the feature extraction and matching of rod-shaped objects in laser point clouds, and particularly to a method for laser positioning using the features of rod-shaped objects such as tree trunks. Background Art

[0002] Feature-based positioning using lidar point cloud maps has become an extremely important part of high-precision positioning for autonomous driving, especially in scenarios where GPS signals are unstable for a long time. This positioning method often uses line and plane features of the environment, features extracted by deep learning methods, and fixed features placed manually. The robustness and complexity of the features directly affect the positioning performance. Line and plane features depend on the structure of the environment, deep learning methods increase the complexity of positioning, and manually placing fixed features increases the manual complexity while the sparse features cannot significantly improve the positioning performance.

[0003] In the patent document with the publication number CN113313081B, a classification method for road traffic rod-shaped objects that fuses vehicle-mounted three-dimensional laser point clouds and images is disclosed, including the following steps: obtaining point cloud and image data; removing ground points in the point cloud to obtain a pre-segmented body, slicing it and performing feature constraints on the continuous slice set, and using the continuous slice set that meets the feature constraints as the original seed points of the rod-shaped object; growing the original seed points of the rod-shaped object through a growth algorithm to obtain a complete rod-shaped object point cloud to obtain the accurate position information of the rod-shaped object point cloud; according to the correspondence between the point cloud and the panoramic image, projecting the rod-shaped object point cloud onto the panoramic image to obtain the corresponding image range of the rod-shaped object point cloud; using the trained Mask-RCNN to perform instance segmentation on the image to obtain the instance segmentation information of the rod-shaped object, and performing fine classification on it using the range of the rod-shaped object point cloud in the panoramic image.

[0004] Therefore, a new technical solution needs to be proposed to improve the above technical problems. Summary of the Invention

[0005] Aiming at the defects in the prior art, the purpose of the present invention is to provide a positioning method and system based on the feature extraction and matching of rod-shaped objects in laser point clouds.

[0006] According to a positioning method based on the feature extraction and matching of rod-shaped objects in laser point clouds provided by the present invention, it includes:

[0007] Step S1: Scanning the environment using a data acquisition vehicle equipped with a lidar to obtain a laser point cloud map;

[0008] Step S2: Performing height filtering, clustering, rough feature extraction, and fine feature extraction on the laser point cloud map to obtain map rod-shaped object features;

[0009] Step S3: Use the lidar of the vehicle to scan the surrounding environment of the vehicle to obtain real-time lidar point cloud;

[0010] Step S4: Perform height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on the real-time lidar point cloud to obtain real-time rod-shaped object features;

[0011] Step S5: Perform feature matching and non-linear optimization on the map rod-shaped object features and the real-time rod-shaped object features to obtain the vehicle pose.

[0012] Preferably, the height filtering includes: selecting all points with a height greater than the first height value from the lidar point cloud, filtering out other points, and obtaining the height-filtered point cloud;

[0013] The radar line number characteristic filtering includes: for the height-filtered point cloud, using the characteristics of the single-line scanned point cloud of the lidar, arranging the points in the single-line point cloud in the order of scanning, then dividing the points into multiple point cloud segments according to the distance between points, selecting all point cloud segments with a length within the first length value, filtering out other point cloud segments, and obtaining the line number characteristic-filtered point cloud;

[0014] The clustering includes: performing Euclidean clustering on the height-filtered point cloud or the line number characteristic-filtered point cloud, and then selecting all points within the first radius value around the clustering center to form the corresponding clustering point cloud cluster;

[0015] The rough feature extraction includes: performing principal component analysis on the clustering point cloud cluster, and extracting the z-axis component f of the eigenvector corresponding to the maximum eigenvalue z , and judging whether it is greater than the first feature threshold f th , if so, selecting the clustering point cloud cluster as the candidate point cloud cluster, if not, filtering out the clustering point cloud cluster.

[0016] Preferably, the fine feature extraction includes:

[0017] Step T1: Sort all points in the candidate point cloud cluster in ascending order of z coordinate, set a sliding window with a height of d, and then in the z-axis direction of the candidate point cloud cluster, set m stop positions from bottom to top for the sliding window, with the bottom corresponding to the first stop position and the top corresponding to the mth stop position;

[0018] Step T2: Place the sliding window at the first stop position, k = 0, and the height difference h1 = 0;

[0019] Step T3: When the sliding window is at the i-th stop position (i=1, 2, ..., m), the point cloud in the sliding window is intercepted according to the z coordinate, and then the principal component analysis is performed on the point cloud in the window to extract the z-axis component g of the eigenvector corresponding to the maximum eigenvalue zi ;

[0020] Step T4: Determine g zi Is it greater than the second threshold g th If yes, go to step T5, if no, set the height difference h i =0 and set k=0, then go to step T6;

[0021] Step T5: add 1 to the value of k, and then determine the value of k. If k = 1, record the starting position h of the sliding window at this time. l1 and the end position h u1 , calculate the height difference h i =h u1 –h l1 , if k>1, record the end position h of the sliding window at this time ek , calculate the height difference h i =h uk –h l1 ;

[0022] Step T6: Determine whether i is equal to m. If not, move the sliding window upward to the next stop position, that is, add 1 to the value of i, and then jump back to step T3. If yes, go to step T7;

[0023] Step T7: From all height differences h i Select the largest height difference h from (i=1,2,…,m) max , determine h max The corresponding upper boundary h u and the lower boundary h l , and then select the candidate point cloud cluster located at h u and h l The points between and are used to obtain the target point cloud cluster, i.e., the rod-shaped object features.

[0024] Preferably, the m stop positions of the sliding window are obtained by fixing the distance between two adjacent stop positions according to the height of the alternative point cloud cluster in the z-axis direction, or by setting a stop position for every fixed number of points or points of each height from bottom to top according to the distribution of points in the alternative point cloud cluster in the z-axis direction.

[0025] Preferably, in the feature refinement extraction of step S4, the method of multi-frame stitching or downsampling is adopted to maintain the point cloud density. In multi-frame stitching, the odometry pose between frames is used to transform all the rod-shaped object features extracted from the previous few frames into the coordinate system of the current frame, which are jointly used as the rod-shaped object features of the current frame.

[0026] The present invention also provides a positioning system based on the extraction and matching of rod-shaped object features from lidar point clouds, including:

[0027] Lidar point cloud map construction module: Using a data acquisition vehicle equipped with a lidar to scan the environment to obtain a lidar point cloud map;

[0028] Map rod-shaped object feature extraction module: Performing height filtering, clustering, rough feature extraction, and refined feature extraction on the lidar point cloud map to obtain map rod-shaped object features;

[0029] Real-time lidar point cloud acquisition module: Using the lidar of the vehicle to scan the surrounding environment of the vehicle to obtain real-time lidar point clouds;

[0030] Real-time rod-shaped object feature extraction module: Performing height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and refined feature extraction on the real-time lidar point clouds to obtain real-time rod-shaped object features;

[0031] Pose acquisition module: Performing feature matching and non-linear optimization on the map rod-shaped object features and the real-time rod-shaped object features to obtain the vehicle pose.

[0032] Preferably, the units for height filtering in the map rod-shaped object feature extraction module and the real-time rod-shaped object feature extraction module include selecting all points in the lidar point cloud with a height greater than the first height value from the ground, filtering out other points, and obtaining the height-filtered point cloud;

[0033] The unit for radar line number characteristic filtering in the real-time rod-shaped object feature extraction module includes, for the height-filtered point cloud, using the characteristics of the single-line scanned point cloud of the lidar to arrange the points in the single-line point cloud in the order of scanning, then dividing the single-line point cloud into multiple point cloud segments according to the distance between points, selecting all point cloud segments with a length within the first length value, filtering out other point cloud segments, and obtaining the line number characteristic-filtered point cloud;

[0034] The units for clustering in the map rod-shaped object feature extraction module and the real-time rod-shaped object feature extraction module include performing Euclidean clustering on the height-filtered point cloud or the line number characteristic-filtered point cloud, and then selecting all points within the first radius value around the clustering center to form the corresponding clustered point cloud clusters;

[0035] The units for rough feature extraction in the map rod feature extraction module and the real-time rod feature extraction module include performing principal component analysis on the clustered point cloud clusters, and extracting the z-axis component f of the eigenvector corresponding to the maximum eigenvalue. z , and determining whether it is greater than the first feature threshold f th . If so, select the clustered point cloud cluster as the alternative point cloud cluster; if not, filter out the clustered point cloud cluster.

[0036] Preferably, the units for fine feature extraction in the map rod feature extraction module and the real-time rod feature extraction module include:

[0037] Component M1: Sort all the points in the alternative point cloud cluster in ascending order of the z coordinate, set a sliding window with a height of d, and then in the z-axis direction of the alternative point cloud cluster, set m stop positions from bottom to top for the sliding window. The bottommost corresponds to the 1st stop position, and the topmost corresponds to the mth stop position;

[0038] Component M2: Place the sliding window at the first stop position, k = 0, and the height difference h1 = 0;

[0039] Component M3: When the sliding window is at the ith stop position (i = 1, 2,..., m), intercept the point cloud within the sliding window according to the z coordinate, and then perform principal component analysis on the point cloud within the window to extract the z-axis component g of the eigenvector corresponding to the maximum eigenvalue. zi ;

[0040] Component M4: Determine whether g zi is greater than the second threshold g th . If so, enter Component M5; if not, set the height difference h i = 0 and set k = 0, and then enter Component M6;

[0041] Component M5: Increment the value of k, and then judge the size of the k value. If k = 1, record the starting position h l1 and the ending position h u1 of the sliding window at this time, calculate the height difference h i = h u1 – h l1 . If k > 1, record the ending position h ek of the sliding window at this time, calculate the height difference h i = h uk – h l1 ;

[0042] Component M6: Judge whether i is equal to m. If not, move the sliding window up to the next stop position, that is, increment the value of i by 1, and then jump back to Component M3; if so, enter Component M7;

[0043] Component M7: From all height differences h i (i = 1, 2, …, m), select the maximum height difference h max , and determine h max corresponding upper boundary h u and lower boundary h l , then select the points in the alternative point cloud cluster located between h u and h l to obtain the target point cloud cluster, i.e., the rod-shaped object feature.

[0044] Preferably, the m staying positions of the sliding window are obtained by fixing the distance between adjacent staying positions according to the height of the alternative point cloud cluster in the z-axis direction, or by setting a staying position for every fixed number of points or points at each height from bottom to top according to the distribution of the points in the alternative point cloud cluster in the z-axis direction.

[0045] Preferably, when the real-time rod-shaped object feature extraction module performs feature fine extraction, it uses multi-frame stitching or downsampling to maintain the point cloud density. Among them, multi-frame stitching uses the odometry pose between frames to transform all the rod-shaped object features extracted from the previous several frames into the coordinate system of the current frame and jointly serves as the rod-shaped object feature of the current frame.

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] 1. Comprehensively apply height filtering, clustering, feature rough extraction, and feature fine extraction methods to extract rod-shaped object features from the laser point cloud map, improving the extraction accuracy;

[0048] 2. Comprehensively apply height filtering, radar line number characteristic filtering, clustering, feature rough extraction, and feature fine extraction methods to extract rod-shaped object features from the real-time laser point cloud, improving the extraction accuracy and efficiency;

[0049] 3. The radar line number characteristic filtering method can quickly filter out large-area objects such as walls and road signs, improving the efficiency of rod-shaped object feature extraction;

[0050] 4. The sliding window method in feature fine extraction can conveniently remove the branches and bottom supports of trees, improving the trunk extraction accuracy. Description of the Drawings

[0051] By reading the following detailed description of the non-limiting embodiments with reference to the accompanying drawings, other features, objectives, and advantages of the present invention will become more obvious:

[0052] Figure 1 is the overall flowchart of the method provided by the embodiment of the present application;

[0053] Figure 2It is the flowchart of step S2 in the method provided by the embodiments of this application;

[0054] Figure 3 It is the flowchart of step S4 in the method provided by the embodiments of this application;

[0055] Figure 4 It is the flowchart of rough feature extraction in the embodiments of this application;

[0056] Figure 5 It is the schematic diagram of fine feature extraction in the embodiments of this application;

[0057] Figure 6 It is the flowchart of fine feature extraction in the embodiments of this application;

[0058] Figure 7 It is the flowchart of feature matching in the embodiments of this application;

[0059] Figure 8 It is the flowchart of non - linear optimization in the embodiments of this application;

[0060] Figure 9 It is the laser point cloud map of the embodiments of this application, including tree trunk point cloud;

[0061] Figure 10 It is the real - time laser point cloud of the embodiments of this application, including tree trunk point cloud;

[0062] Figure 11 It is the comparison chart of the feature positioning error of rod - shaped objects in the embodiments of this application;

[0063] Figure 12 It is the trajectory map of the feature positioning of rod - shaped objects in the embodiments of this application. Detailed implementation manners

[0064] The present invention will be described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several changes and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0065] According to a positioning method for rod - shaped object feature extraction and matching based on laser point cloud provided by the present invention, as Figures 1 to 3 shown, it includes:

[0066] Step S1: Use a data acquisition vehicle equipped with a lidar to scan the environment to obtain a laser point cloud map;

[0067] Step S2: Perform height filtering, clustering, rough feature extraction, and fine feature extraction on the laser point cloud map to obtain the map rod - shaped object features;

[0068] Step S3: Use the lidar of the vehicle to scan the surrounding environment of the vehicle to obtain real-time lidar point cloud;

[0069] Step S4: Perform height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on the real-time lidar point cloud to obtain real-time rod-shaped object features;

[0070] Step S5: Perform feature matching and non-linear optimization on the map rod-shaped object features and the real-time rod-shaped object features to obtain the vehicle pose.

[0071] The height filtering includes: Selecting all points in the lidar point cloud with a height greater than the first height value from the ground, and filtering out other points to obtain the height-filtered point cloud. In a specific embodiment, all points with a height greater than 1.2 m from the ground are selected, and other points are filtered out.

[0072] The radar line number characteristic filtering includes: For the height-filtered point cloud, using the characteristic of the single-line scanning point cloud of the lidar, arranging the points in the single-line point cloud in the order of scanning, and then dividing the single-line point cloud into multiple point cloud segments according to the distance between points. Selecting all point cloud segments with a length within the first length value, and filtering out other point cloud segments to obtain the line number characteristic-filtered point cloud. In a specific embodiment, all point cloud segments with a length within 60 cm are selected, and other point cloud segments are filtered out.

[0073] The radar line number characteristic filtering method adopted in this application can quickly filter out large-area objects such as walls and road signs, and improve the efficiency of rod-shaped object feature extraction.

[0074] The clustering includes: Performing Euclidean clustering on the height-filtered point cloud or the line number characteristic-filtered point cloud, and then selecting all points within the first radius value around the clustering center to form the corresponding clustering point cloud cluster. In a specific embodiment, the point cloud within 70 cm of the clustering center is selected as the clustering point cloud cluster.

[0075] As Figure 4 shown, the rough feature extraction includes: Performing principal component analysis on the clustering point cloud cluster, and extracting the z-axis component f of the eigenvector corresponding to the maximum eigenvalue z , and judging whether it is greater than the first feature threshold f th . If so, select the clustering point cloud cluster as the candidate point cloud cluster. If not, filter out the clustering point cloud cluster.

[0076] As Figure 6 shown, the fine feature extraction includes:

[0077] Step T1: Sort all the points in the alternative point cloud cluster in ascending order of the z - coordinate. Set a sliding window with a height of d, and then, in the z - axis direction of the alternative point cloud cluster, set m stopping positions for the sliding window from bottom to top. The bottom - most position corresponds to the 1st stopping position, and the top - most position corresponds to the mth stopping position;

[0078] Step T2: Place the sliding window at the first stopping position, k = 0, and the height difference h1 = 0;

[0079] Step T3: When the sliding window is at the ith stopping position (i = 1, 2, …, m), intercept the point cloud within the sliding window according to the z - coordinate, and then perform principal component analysis on the point cloud within the window to extract the z - axis component g of the eigenvector corresponding to the largest eigenvalue zi ;

[0080] Step T4: Judge whether g zi is greater than the second threshold g th . If so, go to Step T5; if not, set the height difference h i = 0 and k = 0, and then go to Step T6;

[0081] Step T5: Increment the value of k by 1, and then judge the value of k. If k = 1, record the starting position h l1 and the ending position h u1 of the sliding window at this time, and calculate the height difference h i = h u1 – h l1 . If k>1, record the ending position h ek of the sliding window at this time, and calculate the height difference h i = h uk – h l1 ;

[0082] Step T6: Judge whether i is equal to m. If not, move the sliding window up to the next stopping position, that is, increment the value of i by 1, and then jump back to Step T3. If so, go to Step T7;

[0083] Step T7: Select the largest height difference h i (i = 1, 2, …, m) from all the height differences h max , determine the upper boundary h max and the lower boundary h u corresponding to h l , and then select the points in the alternative point cloud cluster that are between h u and h l to obtain the target point cloud cluster, that is, the feature of the rod - shaped object.

[0084] The m staying positions of the sliding window are obtained by fixing the spacing between two adjacent staying positions according to the height of the alternative point cloud clusters in the z-axis direction, or by setting a staying position for every fixed number of points or points at each height from bottom to top according to the distribution of the points in the alternative point cloud clusters in the z-axis direction.

[0085] In a specific embodiment, the height of the sliding window is set to 45 cm, and 10 staying positions are set for the sliding window from bottom to top. The bottommost corresponds to the 1st staying position, and the topmost corresponds to the 10th staying position.

[0086] The sliding window method is adopted in the feature fine extraction, which can conveniently remove the branches and bottom supports of the trees and improve the trunk extraction accuracy.

[0087] In the feature fine extraction of step S4, the method of multi-frame stitching or downsampling is adopted to maintain the point cloud density. Among them, multi-frame stitching uses the odometry pose between frames to transform all the rod-shaped object features extracted from the previous several frames into the coordinate system of the current frame and jointly serves as the rod-shaped object feature of the current frame.

[0088] This application comprehensively applies methods such as height filtering, clustering, feature rough extraction, and feature fine extraction to extract rod-shaped object features from the laser point cloud map, improving the extraction accuracy.

[0089] In a specific embodiment, as Figure 7 shown, the feature matching in step S5 includes:

[0090] Insert all the trunk center points of the map trunk feature set into the KD-tree;

[0091] Traverse the center points of all trunks in the real-time frame, find the trunk centers in the map within a certain range in the KD-tree, and obtain the corresponding map trunk point cloud;

[0092] Judge whether the search result is unique. For a unique result, directly construct the map-real-time trunk matching pair; if the result is not unique, then compare the dimensional similarity (the difference between the product of the length-width ratio and the length-height ratio), and select the best score as the best matching pair.

[0093] In a specific embodiment, as Figure 8 shown, the non-linear optimization in step S5 includes:

[0094] Set the number of iterations. The initial pose of each iteration is the pose optimized in the previous iteration, and the first initial pose is the vehicle pose with noise in the real-time frame;

[0095] At each iteration, the real-time trunk point cloud {p lidar} is transformed into the map coordinate system {p map, and insert all the points of the matched map tree trunk into the KD-tree;

[0096] For each point in {p map}, find the nearest point p map ’ of the point cloud of the matched map tree trunk in the KD-tree (it is necessary to ensure that the points of the point cloud of the map tree trunk are not searched repeatedly), and construct the matching point pair {p lidar , p map ’};

[0097] Using all the matching point pairs {p lidar , p map ’} and the pose (where x, y, yaw are optimization variables and roll, pitch, z are constants), construct the residual:

[0098]

[0099] For each constructed matching pair, if the error e value is less than the threshold or the iteration times are reached, terminate the iteration and output the pose, otherwise start over with the optimized pose.

[0100] In a specific embodiment, the rod-shaped object features extracted from the laser point cloud map are as Figure 9 shown, and the rod-shaped object features extracted from the real-time laser point cloud are as Figure 10 shown. The error comparison of positioning by the rod-shaped object features is as Figure 11 shown, and the trajectory comparison of positioning by the rod-shaped object features is as Figure 12 shown. Among them, the error is the Euclidean distance between the pose and the true value, the true value is the pose of map building, and the input pose adds 20 - 45 cm of random noise to the true value. It can be seen from the figure that using the rod-shaped object features for positioning can control the error within 10 cm.

[0101] The present invention also provides a positioning system based on the extraction and matching of rod-shaped object features from laser point clouds, including:

[0102] Laser point cloud map construction module: Use a data acquisition vehicle equipped with a lidar to scan the environment to obtain a laser point cloud map;

[0103] Map rod-shaped object feature extraction module: Perform height filtering, clustering, rough feature extraction and fine feature extraction on the laser point cloud map to obtain map rod-shaped object features;

[0104] Real-time laser point cloud acquisition module: Use the lidar of the vehicle to scan the surrounding environment of the vehicle to obtain real-time laser point clouds;

[0105] Real-time rod feature extraction module: performs height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on real-time lidar point cloud to obtain real-time rod features;

[0106] Pose acquisition module: performs feature matching and non-linear optimization on map rod features and real-time rod features to obtain vehicle pose.

[0107] The units of the map rod feature extraction module and the real-time rod feature extraction module for height filtering include selecting all points with a height greater than the first height value from the lidar point cloud, filtering out other points, and obtaining the height-filtered point cloud.

[0108] The unit of the real-time rod feature extraction module for radar line number characteristic filtering includes, for the height-filtered point cloud, using the characteristics of the single-line lidar scan point cloud, arranging each point in the single-line point cloud in the order of scanning, then dividing the single-line point cloud into multiple point cloud segments according to the distance between points, selecting all point cloud segments with a length within the first length value, filtering out other point cloud segments, and obtaining the line number characteristic-filtered point cloud.

[0109] The units of the map rod feature extraction module and the real-time rod feature extraction module for clustering include performing Euclidean clustering on the height-filtered point cloud or the line number characteristic-filtered point cloud, and then selecting all points within the first radius value around the clustering center to form the corresponding clustering point cloud cluster.

[0110] The units of the map rod feature extraction module and the real-time rod feature extraction module for rough feature extraction include performing principal component analysis on the clustering point cloud cluster, and extracting the z-axis component f of the eigenvector corresponding to the maximum eigenvalue z , and judging whether it is greater than the first feature threshold f th , if so, selecting the clustering point cloud cluster as the candidate point cloud cluster, if not, filtering out the clustering point cloud cluster.

[0111] The units of the map rod feature extraction module and the real-time rod feature extraction module for fine feature extraction include:

[0112] Component M1: sorts all points in the candidate point cloud cluster in ascending order of z coordinate, sets a sliding window with a height of d, and then, in the z-axis direction of the candidate point cloud cluster, sets m stop positions from bottom to top for the sliding window, with the bottom corresponding to the first stop position and the top corresponding to the mth stop position;

[0113] Component M2: places the sliding window at the first stop position, k = 0, and the height difference h1 = 0;

[0114] Component M3: When the sliding window is at the $i$-th stationary position ($i = 1, 2, \ldots, m$), intercept the point cloud within the sliding window according to the $z$-coordinate, and then perform principal component analysis on the point cloud within the window to extract the $z$-axis component $g$ of the eigenvector corresponding to the maximum eigenvalue. zi ;

[0115] Component M4: Judge whether $g$ zi is greater than the second threshold $g$ th . If so, enter Component M5; if not, set the height difference $h$ i to 0 and set $k$ to 0, then enter Component M6.

[0116] Component M5: Increment the value of $k$ by 1, and then judge the magnitude of the $k$ value. If $k = 1$, record the starting position $h$ l1 and the ending position $h$ u1 of the sliding window at this time, calculate the height difference $h$ i $= h$ u1 – $h$ l1 . If $k > 1$, record the ending position $h$ ek of the sliding window at this time, calculate the height difference $h$ i $= h$ uk – $h$ l1 ;

[0117] Component M6: Judge whether $i$ is equal to $m$. If not, move the sliding window upward to the next stationary position, that is, increment the value of $i$ by 1, and then jump back to Component M3; if so, enter Component M7.

[0118] Component M7: Select the maximum height difference $h$ i ($i = 1, 2, \ldots, m$) from all the height differences $h$ max , determine the upper boundary $h$ max and the lower boundary $h$ u corresponding to $h$ l , and then select the points in the alternative point cloud cluster that are between $h$ u and $h$ l to obtain the target point cloud cluster, that is, the feature of the rod-shaped object.

[0119] The $m$ stationary positions of the sliding window are obtained by fixing the spacing between two adjacent stationary positions according to the height of the alternative point cloud cluster in the $z$-axis direction, or by setting a stationary position for every fixed number of points or points at each height from bottom to top according to the distribution of the points in the alternative point cloud cluster in the $z$-axis direction.

[0120] When the real-time rod feature extraction module performs fine feature extraction, it uses a multi-frame stitching or downsampling system to maintain the point cloud density. Among them, multi-frame stitching uses the odometry pose between frames to transform all the rod features extracted from the previous few frames into the coordinate system of the current frame, and jointly uses them as the rod features of the current frame.

[0121] Those skilled in the art know that in addition to implementing the system and its various devices, modules, and units provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to enable the system and its various devices, modules, and units provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc. to achieve the same function. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a kind of hardware component, and the devices, modules, and units included therein for implementing various functions can also be regarded as the structures within the hardware component; the devices, modules, and units for implementing various functions can also be regarded as either software modules for implementing the method or structures within the hardware component.

[0122] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, 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. Without conflict, the embodiments of the present application and the features in the embodiments can be combined arbitrarily with each other.

Claims

1. A positioning method based on the feature extraction and matching of rod-shaped objects in laser point clouds, characterized in that, Including: Step S1: Use a data acquisition vehicle equipped with a lidar to scan the environment to obtain a lidar point cloud map; Step S2: Perform height filtering, clustering, rough feature extraction, and fine feature extraction on the lidar point cloud map to obtain map pole-like object features; Step S3: Use the vehicle's lidar to scan the environment around the vehicle to obtain real-time lidar point cloud; Step S4: Perform height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on the real-time lidar point cloud to obtain real-time pole-like object features; Step S5: Perform feature matching and non-linear optimization on the map pole-like object features and the real-time pole-like object features to obtain the vehicle pose; The height filtering includes: Selecting all points with a height greater than a first height value from the lidar point cloud, filtering out other points to obtain height-filtered point cloud; The radar line number characteristic filtering includes: For the height-filtered point cloud, using the characteristics of the single-line scan point cloud of the lidar, arranging the points in the single-line point cloud in the order of scanning, then dividing the single-line point cloud into multiple point cloud segments according to the distance between points, selecting all point cloud segments with a length within a first length value, and filtering out other point cloud segments to obtain line number characteristic-filtered point cloud; The clustering includes: Performing Euclidean clustering on the height-filtered point cloud or the line number characteristic-filtered point cloud, and then selecting all points within a first radius value around the clustering center to form a corresponding clustered point cloud cluster; The rough extraction of the feature includes: performing principal component analysis on the clustered point cloud clusters, and extracting the z axial component f z of the eigenvector corresponding to the maximum eigenvalue, and judging whether it is greater than a first feature threshold f th ; if so, selecting the clustered point cloud cluster as an alternative point cloud cluster, and if not, filtering out the clustered point cloud cluster; The fine feature extraction includes: Step T1: Sort all the points in the alternative point cloud cluster in ascending order according to z coordinates, set a sliding window with a height of d , and then, in the z -axis direction of the alternative point cloud cluster, set m stop positions from bottom to top for the sliding window. The bottommost position corresponds to the 1st stop position, and the topmost position corresponds to the m th stop position; Step T2: Place the sliding window at the first stop position, k = 0, height difference h 1 = 0; Step T3: When the sliding window is at the i th staying position, i = 1, 2, …, m , according to z coordinates, intercept the point cloud within the sliding window, and then perform principal component analysis on the point cloud within the window to extract the z axis component of the eigenvector corresponding to the maximum eigenvalue g zi ; Step T4: Determine whether g zi is greater than the second threshold g th . If so, proceed to step T5. If not, set the height difference h i to 0 and set k to 0, then proceed to step T6; Step T5: k Increment the value of k , and then determine the k size of the value. If h l1 it is equal to 1, record the starting position of the sliding window h u1 , and calculate the height difference h i = h u1 – h l1 . If k it is greater than 1, record the ending position of the sliding window h uk , and calculate the height difference h i = h uk – h l1 ; Step T6: Determine whether i is equal to m . If not, move the sliding window up to the next stop position, that is, increment the value of i by 1, and then jump back to Step T3. If so, proceed to Step T7; Step T7: From all the height differences h i ，i = 1, 2, …, m select the maximum height difference h max , and determine h max the corresponding upper boundary h u and the lower boundary h l , then select the points in the alternative point cloud cluster that are located h u and h l to obtain the target point cloud cluster, which is the feature of the rod-shaped object.

2. The positioning method based on laser point cloud rod-shaped object feature extraction and matching according to claim 1, characterized in that, The m stopping positions of the sliding window are obtained by fixing the spacing between two adjacent stopping positions according to the height of the alternative point cloud cluster in the z axial direction, or are obtained by setting one stopping position for every fixed number of points or points at each height from bottom to top according to the distribution of the points in the alternative point cloud cluster in the z-axis direction.

3. The positioning method based on laser point cloud rod-shaped object feature extraction and matching according to claim 1, characterized in that In the fine feature extraction of step S4, a method of multi-frame stitching or downsampling is used to maintain the point cloud density. In multi-frame stitching, the odometry pose between frames is used to transform all the pole-like object features extracted from the previous few frames into the coordinate system of the current frame, which are jointly used as the pole-like object features of the current frame.

4. A positioning system based on feature extraction and matching of rod-shaped objects in laser point cloud, characterized in that, Including: Lidar point cloud map construction module: Use a data acquisition vehicle equipped with a lidar to scan the environment to obtain a lidar point cloud map; Map pole-like object feature extraction module: Perform height filtering, clustering, rough feature extraction, and fine feature extraction on the lidar point cloud map to obtain map pole-like object features; Real-time lidar point cloud acquisition module: Use the vehicle's lidar to scan the environment around the vehicle to obtain real-time lidar point cloud; Real-time pole-like object feature extraction module: Perform height filtering, radar line number characteristic filtering, clustering, rough feature extraction, and fine feature extraction on the real-time lidar point cloud to obtain real-time pole-like object features; Pose acquisition module: Perform feature matching and non-linear optimization on the map pole-like object features and the real-time pole-like object features to obtain the vehicle pose; The units of the map pole-like object feature extraction module and the real-time pole-like object feature extraction module for height filtering include selecting all points with a height greater than a first height value from the lidar point cloud, filtering out other points to obtain height-filtered point cloud; The unit of the real-time rod feature extraction module for filtering the radar line number characteristics includes, for the point cloud after height filtering, using the characteristics of the single-line scanning point cloud of the lidar, arranging each point in the single-line point cloud in the order of scanning, then dividing the single-line point cloud into multiple point cloud segments according to the distance between points, selecting all the point cloud segments with lengths within the first length value, and filtering out other point cloud segments to obtain the point cloud after line number characteristic filtering; The unit of the map rod feature extraction module and the real-time rod feature extraction module for clustering includes performing Euclidean clustering on the point cloud after height filtering or the point cloud after line number characteristic filtering, and then selecting all the points within the first radius value around the clustering center to form the corresponding clustering point cloud cluster; The units for rough feature extraction in the map rod feature extraction module and the real-time rod feature extraction module include performing principal component analysis on the clustered point cloud clusters and extracting the feature vector corresponding to the maximum eigenvalue of z axial component f z , and determining whether it is greater than a first feature threshold f th , if so, selecting the clustered point cloud cluster as an alternative point cloud cluster, if not, filtering out the clustered point cloud cluster; The unit of the map rod feature extraction module and the real-time rod feature extraction module for fine feature extraction includes: Component M1: Sort all the points in the alternative point cloud cluster in ascending order of z coordinates, set a sliding window with a height of d , and then, in the z axis direction of the alternative point cloud cluster, set m stopping positions from bottom to top for the sliding window, with the bottommost corresponding to the 1st stopping position and the topmost corresponding to the m th stopping position; Component M2: Place the sliding window at the first stop position, k = 0, height difference h 1 = 0; Component M3: When the sliding window is at the i th stopping position, i = 1, 2, …, m , according to z coordinates, intercept the point cloud within the sliding window, and then perform principal component analysis on the point cloud within the window to extract the z axial component of the eigenvector corresponding to the maximum eigenvalue g zi ; Component M4: Judgment g zi whether it is greater than the second threshold g th , if so, enter Component M5, if not, set the height difference h i = 0 and set k = 0, then enter Component M6; Component M5: k Increment the value by 1, and then judge k the size of the value. If k = 1, record the starting position of the sliding window at this time h l1 and the ending position h u1 , calculate the height difference h i = h u1 – h l1 . If k > 1, record the ending position of the sliding window at this time h uk , calculate the height difference h i = h uk – h l1 ; Component M6: Judgment i Is equal to m , if not, move the sliding window up to the next stop position, that is, add 1 to the value of i , then jump back to Component M3, if so, enter Component M7; Component M7: From all height differences h i ，i = 1, 2, …, m select the maximum height difference h max , determine h max the corresponding upper boundary h u and the lower boundary h l , then select the points in the alternative point cloud cluster that are located h u and h l between them to obtain the target point cloud cluster, that is, the rod-shaped feature.

5. The positioning system based on laser point cloud rod-shaped feature extraction and matching according to claim 4, characterized in that The m stop positions of the sliding window are obtained by fixing the distance between two adjacent stop positions according to the height of the alternative point cloud cluster in the z axial direction, or by setting one stop position for every fixed number of points or points at each height from bottom to top according to the distribution of the points in the alternative point cloud cluster in the z-axis direction.

6. The positioning system based on laser point cloud rod-shaped object feature extraction and matching according to claim 4, characterized in that, When the real-time rod feature extraction module performs fine feature extraction, it uses a multi-frame stitching or downsampling system to maintain the point cloud density. Among them, for multi-frame stitching, the odometry pose between frames is used to transform all the rod features extracted from the previous frames into the coordinate system of the current frame, and they are jointly used as the rod features of the current frame.

Images