Open road vehicle three-dimensional positioning system and method based on high-reflective road signs

Abstract

The present invention provides a system and method for three-dimensional positioning of vehicles on open roads based on high-reflection road signs, including: high-reflection road signs: arranged on both sides of the open road in sequence according to preset intervals; laser radar: installed on the vehicle, used to detect the environment around the vehicle on the open road, and presented in the form of laser point cloud; high-reflection feature three-dimensional mapping module: installed on the vehicle, based on the laser point cloud containing high-emission features, SLAM mapping is performed to generate and store high-reflection feature three-dimensional maps of open roads; high-reflection feature three-dimensional positioning module: installed on the vehicle, based on the extraction and matching of high-reflection features of the laser point cloud, high-precision three-dimensional positioning of vehicles on open roads is achieved. The present invention uses high-reflection road signs to perform high-precision point mapping on roads where high-reflection road signs are deployed to obtain high-reflection feature maps, and then performs high-reflection feature three-dimensional positioning. This positioning method has good anti-interference performance against local optimality, and requires very small computing resources.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a three-dimensional positioning system and method for open road vehicles based on highly reflective road signs. Background Art

[0002] Positioning and navigation play an extremely important role in the daily operation of autonomous vehicles and autonomous mobile robots, and determine whether they can avoid obstacles and complete the given tasks in various complex environments. Common positioning methods include dead reckoning, visual positioning, laser positioning, etc. Among them, the laser positioning method uses laser radar to detect the surrounding real-time environment, and then compares it with the known environmental information to determine the precise position and posture of the vehicle or robot. Laser positioning has the advantages of accurate distance measurement, convenient point cloud processing, and low lighting requirements, so it has been widely used in various work scenarios.

[0003] In open road scenes such as highways and urban roads, three-dimensional laser positioning methods are usually used, such as traditional ICP, NDT and other methods, feature-based FPFH+ICP, LOAM and other methods, as well as the newly emerging learning-based laser point cloud registration positioning method. However, most of these methods have a disadvantage, that is, when the point cloud partially overlaps or the initial pose error is large, it is easy to have problems such as unstable positioning, long calculation time and even divergent instability. On the other hand, in closed scenes such as workshops, warehouses, and parks, AGV-type mobile robots also use laser radar plus reflectors for positioning. Due to the simplicity of the scene, usually only a single-line radar is used to achieve two-dimensional positioning based on the detected geometric position information of the reflector. This method also has high requirements for the layout of the reflector. Due to different application scenarios, the two-dimensional positioning method of the single-line radar plus reflector used by AGV is not suitable for direct use in autonomous driving vehicles on open roads. Patent document CN113514844A (application number: 202110446961.5) discloses a mobile robot positioning method and system based on two-dimensional laser reflection intensity. The method includes: obtaining the distance data and laser reflection intensity between the mobile robot and the road signs in the environment; using the adaptive clustering method to determine the center position of the road sign according to the distance data and the laser reflection intensity; determining the successfully matched road sign according to the center position of the road sign and the position of the preset road sign; the position of the preset road sign is the position of the road sign set in the mobile robot; judging whether the number of successfully matched road signs is greater than or equal to 3; if greater than or equal to 3, using the three-sided positioning algorithm and the least squares method to determine the position and posture of the mobile robot to achieve the positioning of the mobile robot; if less than 3, returning to the step of obtaining the distance data and laser reflection intensity between the mobile robot and the road signs in the environment. This scheme belongs to a two-dimensional positioning method and cannot be used for positioning of autonomous driving vehicles on open roads. Summary of the invention

[0004] In view of the defects in the prior art, the object of the present invention is to provide a three-dimensional positioning system and method for open road vehicles based on highly reflective road signs.

[0005] The open road vehicle three-dimensional positioning system based on high-reflective road signs provided by the present invention comprises:

[0006] Highly reflective road signs: arranged on both sides of the open road at preset intervals;

[0007] LiDAR: Installed on the vehicle, it detects the environment around the vehicle on an open road and presents it in the form of a laser point cloud;

[0008] High-reflection feature 3D mapping module: Installed on the vehicle, it performs SLAM mapping based on the laser point cloud containing high-reflection features, generates and stores high-reflection feature 3D maps of open roads;

[0009] High-reflective feature 3D positioning module: installed on the vehicle, based on the extraction and matching of high-reflective features of the laser point cloud, it can achieve high-precision 3D positioning of vehicles on open roads.

[0010] Preferably, the highly reflective road sign has high reflectivity to the laser radar and can reflect back high-intensity laser spots;

[0011] The highly reflective road sign is in the shape of a plate, a strip or a cone.

[0012] Preferably, the laser radar is a multi-line radar that generates a three-dimensional laser point cloud for three-dimensional mapping and positioning.

[0013] Preferably, the high-reflection feature three-dimensional mapping module includes a SLAM unit, a high-reflection feature post-processing unit and a map storage unit;

[0014] The SLAM unit performs SLAM mapping based on the laser point cloud data with high reflection characteristics to obtain an original point cloud map;

[0015] The high-reflection feature post-processing unit performs post-processing on the original point cloud map to obtain a high-reflection feature map;

[0016] The map storage unit is used to store the high-reflection feature map.

[0017] Preferably, the high-reflective feature three-dimensional positioning module includes a point cloud distortion compensation unit, a high-reflective feature extraction unit, a high-reflective feature matching unit and a posture optimization unit;

[0018] The point cloud distortion compensation unit is used to perform point cloud distortion compensation on the real-time laser point cloud;

[0019] The high-reflection feature extraction unit is used to extract high-reflection features from the compensated real-time point cloud to obtain a high-reflection feature point cloud;

[0020] The high-reflection feature matching unit performs feature matching on the high-reflection feature point cloud based on the high-reflection feature map to obtain a feature cluster matching pair;

[0021] The posture optimization unit performs posture optimization based on the high-reflection feature map, the high-reflection feature point cloud and the feature cluster matching pair to obtain a high-precision positioning result.

[0022] The open road vehicle three-dimensional positioning method based on high-reflective road signs provided by the present invention comprises:

[0023] Step S1: highly reflective road signs are arranged in sequence at preset intervals on both sides of an open road;

[0024] Step S2: using the laser radar of the vehicle to collect the complete point cloud data of the open road in advance, the point cloud data has high reflection characteristics, and SLAM mapping is performed based on the data to obtain an original point cloud map;

[0025] Step S3: performing high-reflection feature post-processing on the original point cloud map to obtain a high-reflection feature map, and storing it;

[0026] Step S4: when the vehicle is traveling on the open road, a laser radar is used to collect real-time laser point clouds and perform point cloud distortion compensation;

[0027] Step S5: extracting high-reflection features from the compensated real-time point cloud to obtain a high-reflection feature point cloud;

[0028] Step S6: performing feature matching on the high-reflection feature point cloud based on the high-reflection feature map to obtain a feature cluster matching pair;

[0029] Step S7: Based on the high-reflection feature map, the high-reflection feature point cloud and the feature cluster matching pairs, posture optimization is performed to obtain a high-precision positioning result.

[0030] Preferably, step S3 comprises:

[0031] Step S3.1: traverse the original point cloud map and delete points whose intensity is less than a first preset threshold;

[0032] Step S3.2: cluster the remaining points, calculate the three-dimensional size of each cluster, and delete clusters whose size is smaller than the second preset threshold and larger than the third preset threshold;

[0033] Step S3.3: Manually review and delete the wrong clusters, label each cluster, and obtain a high-reflection feature map;

[0034] Step S3.4: storing the high-reflection feature map.

[0035] Preferably, step S5 comprises:

[0036] Step S5.1: Accumulate the poses of the most recent N frames of point clouds to increase the point cloud density;

[0037] Step S5.2: traverse the point cloud and delete the points whose intensity is less than the first threshold;

[0038] Step S5.3: using statistical filtering to remove noise points;

[0039] Step S5.4: cluster the remaining points, calculate the three-dimensional size of each cluster, delete the clusters whose size is smaller than the second preset threshold and larger than the third preset threshold, and obtain a high-reflection feature point cloud.

[0040] Preferably, step S6 comprises:

[0041] Step S6.1: transforming the high-reflection feature point cloud into a world coordinate system, and establishing a KD-Tree for each cluster of the high-reflection feature point cloud in the high-reflection feature map;

[0042] Step S6.2: taking the cluster center of the high-reflection feature point cloud as the origin and the fourth preset threshold as the radius, searching for the corresponding cluster of the high-reflection feature map in the KD-Tree;

[0043] Step S6.3: Compare the size of each high-reflection feature point cloud cluster with the found high-reflection feature map cluster, and filter out the clusters whose size difference exceeds the fifth preset threshold, and the remaining clusters are the clusters used for matching;

[0044] Step S6.4: If the number of matched clusters is zero, abandon this match and continue with the next group search; if one cluster is matched, the match is directly successful; if multiple clusters are matched, the nearest cluster is selected as the final match, and finally a one-to-one correspondence between the high-reflection feature point cloud cluster and the high-reflection feature map cluster is obtained, that is, a feature cluster matching pair.

[0045] Preferably, step S7 includes:

[0046] Step S7.1: Establish a KD-Tree based on the high-reflection feature map and feature cluster matching pairs, and then for each point P in the high-reflection feature point cloud k , find the nearest point P of the high reflectance feature map in the KD-Tree k_map As a matching point;

[0047] Step S7.2: All matching pairs {Pk , P k_map} and the three-dimensional x, y, yaw pose to construct the quadratic residual:

[0048]

[0049] Among them, e is the quadratic residual, R is the rotation quaternion to be optimized, and t is the translation to be optimized;

[0050] Step S7.3: For each matching pair constructed, if the value of the error e is less than the sixth preset threshold, or the number of iterations exceeds the seventh preset threshold, the iteration is terminated and the pose is output to obtain a high-precision positioning result.

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

[0052] The present invention proposes a robust and computationally-efficient method, which uses artificially designed high-reflective road signs to perform high-precision point mapping on roads where high-reflective road signs are deployed to obtain a high-reflective feature map, and then performs high-reflective feature positioning. This positioning method has good anti-interference performance against local optimality and requires very little computing resources. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0054] Figure 1 It is the overall flow chart of the present invention;

[0055] Figure 2 To build a flowchart;

[0056] Figure 3 It is the flow chart of the high reflection feature extraction module;

[0057] Figure 4 Flowchart of high-reflection feature matching module;

[0058] Figure 5 This is the flowchart of the pose optimization module;

[0059] Figure 6 Flowchart for high-precision pose acquisition. DETAILED DESCRIPTION

[0060] The present invention is 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 are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0061] Example:

[0062] like Figure 1 and 2 As shown, the present invention provides a three-dimensional positioning system for vehicles on open roads based on high-reflective road signs according to the present invention, including: high-reflective road signs: arranged in sequence on both sides of the open road at preset intervals; a laser radar: installed on a vehicle, used to detect the environment around the vehicle on the open road, and present it in the form of a laser point cloud; a high-reflective feature three-dimensional mapping module: installed on the vehicle, performing SLAM mapping based on the laser point cloud containing high-emission features, generating and storing a high-reflective feature three-dimensional map of the open road; a high-reflective feature three-dimensional positioning module: installed on the vehicle, based on the extraction and matching of the high-reflective features of the laser point cloud, to achieve high-precision three-dimensional positioning of vehicles on the open road.

[0063] The highly reflective road sign has high reflectivity to the laser radar and can reflect back high-intensity laser points; the highly reflective road sign is in the shape of a plate, a strip or a cone. The laser radar is a multi-line radar that generates a three-dimensional laser point cloud for three-dimensional mapping and positioning.

[0064] The high-reflection feature three-dimensional mapping module includes a SLAM unit, a high-reflection feature post-processing unit and a map storage unit; the SLAM unit performs SLAM mapping based on laser point cloud data with high-reflection features to obtain an original point cloud map; the high-reflection feature post-processing unit post-processes the original point cloud map to obtain a high-reflection feature map; the map storage unit is used to store the high-reflection feature map.

[0065] The high-reflection feature three-dimensional positioning module includes a point cloud distortion compensation unit, a high-reflection feature extraction unit, a high-reflection feature matching unit and a posture optimization unit; the point cloud distortion compensation unit is used to perform point cloud distortion compensation on the real-time laser point cloud; the high-reflection feature extraction unit is used to perform high-reflection feature extraction on the compensated real-time point cloud to obtain a high-reflection feature point cloud; the high-reflection feature matching unit performs feature matching on the high-reflection feature point cloud based on the high-reflection feature map to obtain a feature cluster matching pair; the posture optimization unit performs posture optimization based on the high-reflection feature map, the high-reflection feature point cloud and the feature cluster matching pair to obtain a high-precision positioning result.

[0066] The present invention provides a three-dimensional positioning method for vehicles on open roads based on high-reflection road signs, comprising: step S1: arranging high-reflection road signs in sequence at preset intervals on both sides of the open road; step S2: using a laser radar of a vehicle to collect complete point cloud data of the open road in advance, the point cloud data has high-reflection features, and based on the data, SLAM mapping is performed to obtain an original point cloud map; step S3: performing high-reflection feature post-processing on the original point cloud map to obtain a high-reflection feature map, and storing the map; step S4: when the vehicle is traveling on the open road, using the laser radar to collect real-time laser point clouds, and performing point cloud distortion compensation; step S5: performing high-reflection feature extraction on the compensated real-time point clouds to obtain high-reflection feature point clouds; step S6: performing feature matching on the high-reflection feature point clouds based on the high-reflection feature maps to obtain feature cluster matching pairs; step S7: performing posture optimization based on the high-reflection feature maps, high-reflection feature point clouds and feature cluster matching pairs to obtain high-precision positioning results.

[0067] like Figure 3 As shown, the step S3 includes: step S3.1: traversing the original point cloud map, deleting points whose intensity is less than a first preset threshold; step S3.2: clustering the remaining points, calculating the three-dimensional size of each cluster, and deleting clusters whose size is less than a second preset threshold and greater than a third preset threshold; step S3.3: manually reviewing and deleting erroneous clusters, labeling each cluster, and obtaining a high-reflection feature map; step S3.4: storing the high-reflection feature map.

[0068] like Figure 4 As shown, step S5 includes: step S5.1: accumulating the poses of the most recent N frames of point clouds to increase the point cloud density; step S5.2: traversing the point cloud and deleting the points whose intensity is less than the first threshold; step S5.3: using statistical filtering to remove noise points; step S5.4: clustering the remaining points, calculating the three-dimensional size of each cluster, deleting the clusters whose size is less than the second preset threshold and greater than the third preset threshold, and obtaining a high-reflection feature point cloud.

[0069] like Figure 5As shown, the step S6 includes: step S6.1: transforming the high-reflection feature point cloud into the world coordinate system, and at the same time, establishing a KD-Tree for each cluster of the high-reflection feature point cloud in the high-reflection feature map; step S6.2: taking the cluster center of the high-reflection feature point cloud as the origin and the fourth preset threshold as the radius, searching the corresponding cluster of the high-reflection feature map in the KD-Tree; step S6.3: comparing the size of each high-reflection feature point cloud cluster with the found high-reflection feature map cluster, filtering out the clusters whose size difference exceeds the fifth preset threshold, and the remaining ones are the clusters used for matching; step S6.4: if the number of matched clusters is zero, abandoning this match and continuing with the next group of searches; if matching to one cluster, the direct match is successful, and if matching to multiple clusters, selecting the nearest cluster as the final match, and finally obtaining a one-to-one correspondence between the high-reflection feature point cloud cluster and the high-reflection feature map cluster, that is, a feature cluster matching pair.

[0070] like Figure 6 As shown, the step S7 includes: Step S7.1: Establishing a KD-Tree based on the high-reflection feature map and feature cluster matching pairs, and then for each point P in the high-reflection feature point cloud k , find the nearest point P of the high reflectance feature map in the KD-Tree k_map as matching points; Step S7.2: All matching pairs {P k , P k_map} and the three-dimensional x, y, yaw pose to construct the quadratic residual: Among them, e is the quadratic residual, R is the rotation quaternion to be optimized, and t is the translation to be optimized; Step S7.3: For each matching pair constructed, if the value of the error e is less than the sixth preset threshold, or the number of iterations exceeds the seventh preset threshold, the iteration is terminated and the pose is output to obtain a high-precision positioning result.

[0071] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program 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, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0072] The above describes the specific embodiments of the present invention. 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 does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A three-dimensional positioning system for open road vehicles based on highly reflective road signs, characterized in that: include: Highly reflective road signs: arranged on both sides of the open road at preset intervals; LiDAR: Installed on the vehicle, it detects the environment around the vehicle on an open road and presents it in the form of a laser point cloud; High-reflection feature 3D mapping module: Installed on the vehicle, it performs SLAM mapping based on the laser point cloud containing high-reflection features, generates and stores high-reflection feature 3D maps of open roads; High-reflective feature 3D positioning module: installed on the vehicle, based on the extraction and matching of high-reflective features of the laser point cloud, it can achieve high-precision 3D positioning of vehicles on open roads; The high-reflection feature three-dimensional positioning module includes a point cloud distortion compensation unit, a high-reflection feature extraction unit, a high-reflection feature matching unit and a posture optimization unit; The point cloud distortion compensation unit is used to perform point cloud distortion compensation on the real-time laser point cloud; The high-reflection feature extraction unit is used to extract high-reflection features from the compensated real-time point cloud to obtain a high-reflection feature point cloud; The high-reflection feature matching unit performs feature matching on the high-reflection feature point cloud based on the high-reflection feature map to obtain a feature cluster matching pair; The posture optimization unit performs posture optimization based on the high-reflection feature map, the high-reflection feature point cloud and the feature cluster matching pair to obtain a high-precision positioning result.

2. The open road vehicle three-dimensional positioning system based on high-reflective road signs according to claim 1 is characterized in that: The highly reflective road sign has high reflectivity to the laser radar and can reflect back high-intensity laser spots; The highly reflective road sign is in the shape of a plate, a strip or a cone.

3. The open road vehicle three-dimensional positioning system based on high-reflective road signs according to claim 1, characterized in that: The laser radar is a multi-line radar that generates a three-dimensional laser point cloud for three-dimensional mapping and positioning.

4. The open road vehicle three-dimensional positioning system based on high-reflective road signs according to claim 1, characterized in that: The high-reflection feature three-dimensional mapping module includes a SLAM unit, a high-reflection feature post-processing unit and a map storage unit; The SLAM unit performs SLAM mapping based on the laser point cloud data with high reflection characteristics to obtain an original point cloud map; The high-reflection feature post-processing unit performs post-processing on the original point cloud map to obtain a high-reflection feature map; The map storage unit is used to store the high-reflection feature map.

5. A three-dimensional positioning method for open road vehicles based on highly reflective road signs, characterized in that: include: Step S1: highly reflective road signs are arranged in sequence at preset intervals on both sides of an open road; Step S2: using the laser radar of the vehicle to collect the complete point cloud data of the open road in advance, the point cloud data has high reflection characteristics, and SLAM mapping is performed based on the data to obtain an original point cloud map; Step S3: performing high-reflection feature post-processing on the original point cloud map to obtain a high-reflection feature map, and storing it; Step S4: when the vehicle is traveling on the open road, a laser radar is used to collect real-time laser point clouds and perform point cloud distortion compensation; Step S5: extracting high-reflection features from the compensated real-time point cloud to obtain a high-reflection feature point cloud; Step S6: performing feature matching on the high-reflection feature point cloud based on the high-reflection feature map to obtain a feature cluster matching pair; Step S7: Based on the high-reflection feature map, the high-reflection feature point cloud and the feature cluster matching pairs, posture optimization is performed to obtain a high-precision positioning result.

6. The open road vehicle three-dimensional positioning method based on high-reflective road signs according to claim 5, characterized in that: The step S3 comprises: Step S3.1: traverse the original point cloud map and delete points whose intensity is less than a first preset threshold; Step S3.2: cluster the remaining points, calculate the three-dimensional size of each cluster, and delete clusters whose size is smaller than the second preset threshold and larger than the third preset threshold; Step S3.3: Manually review and delete the wrong clusters, label each cluster, and obtain a high-reflection feature map; Step S3.4: storing the high-reflection feature map.

7. The open road vehicle three-dimensional positioning method based on high-reflective road signs according to claim 5, characterized in that: The step S5 comprises: Step S5.1: Accumulate the poses of the most recent N frames of point clouds to increase the point cloud density; Step S5.2: traverse the point cloud and delete the points whose intensity is less than the first threshold; Step S5.3: using statistical filtering to remove noise points; Step S5.4: cluster the remaining points, calculate the three-dimensional size of each cluster, delete the clusters whose size is smaller than the second preset threshold and larger than the third preset threshold, and obtain a high-reflection feature point cloud.

8. The open road vehicle three-dimensional positioning method based on high-reflective road signs according to claim 5, characterized in that: The step S6 comprises: Step S6.1: transforming the high-reflection feature point cloud into a world coordinate system, and establishing a KD-Tree for each cluster of the high-reflection feature point cloud in the high-reflection feature map; Step S6.2: taking the cluster center of the high-reflection feature point cloud as the origin and the fourth preset threshold as the radius, searching for the corresponding cluster of the high-reflection feature map in the KD-Tree; Step S6.3: Compare the size of each high-reflection feature point cloud cluster with the found high-reflection feature map cluster, and filter out the clusters whose size difference exceeds the fifth preset threshold, and the remaining clusters are the clusters used for matching; Step S6.4: If the number of matched clusters is zero, abandon this match and continue with the next group search; if one cluster is matched, the match is directly successful; if multiple clusters are matched, the nearest cluster is selected as the final match, and finally a one-to-one correspondence between the high-reflection feature point cloud cluster and the high-reflection feature map cluster is obtained, that is, a feature cluster matching pair.

9. The open road vehicle three-dimensional positioning method based on high-reflective road signs according to claim 5, characterized in that: The step S7 comprises: Step S7.1: Establish a KD-Tree based on the high-reflection feature map and feature cluster matching pairs, and then for each point P in the high-reflection feature point cloud k , find the nearest point P of the high reflection feature map in the KD-Tree k_map As a matching point; Step S7.2: All matching pairs {P k ,P k_map } and the three-dimensional x, y, yaw pose to construct the quadratic residual: Among them, e is the quadratic residual, R is the rotation quaternion to be optimized, and t is the translation to be optimized; Step S7.3: For each matching pair constructed, if the value of the error e is less than the sixth preset threshold, or the number of iterations exceeds the seventh preset threshold, the iteration is terminated and the pose is output to obtain a high-precision positioning result.

Images