Vehicle position re-identification method and system

Abstract

Disclosed in the present invention are a vehicle position re-identification method and system. The method comprises: pre-processing LiDAR data of a vehicle, so as to acquire a point cloud result of LiDAR of the vehicle; constructing environment descriptors by using the acquired point cloud result; calculating a dynamic filtering policy threshold value on the basis of the type and a measurement environment of a LiDAR sensor used for acquiring the LiDAR data of the vehicle; and comparing the constructed environment descriptors and the obtained dynamic filtering policy threshold value with point cloud data in a historical database, so as to acquire position information of the vehicle. The present invention can improve the accuracy and efficiency of position re-identification; in the present invention, a dynamic threshold value filtering policy is proposed in consideration of angular resolutions and measurement ranges of different LiDAR, thereby improving the generalization capability of the method for various types of sensors; and in the present invention, a ground point and a non-ground point detected by LiDAR are separated, and descriptors are constructed by merging height information of the ground point and intensity information of the non-ground point, thereby improving the expression capability of the descriptors with regard to an environment.

Description

Vehicle position re-identification method and system

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS

[0002] This application claims priority to the Chinese patent application with application number 202311437884.2, filed with the Chinese Patent Office on October 31, 2023, entitled “A Vehicle Position Re-identification Method and System,” the entire contents of which are incorporated by reference into this application. Technical Field

[0003] The present invention belongs to the field of high-precision vehicle positioning, and particularly relates to a vehicle position re-identification method and system. Background Art

[0004] Intelligent robots and autonomous vehicles integrate a variety of complex emerging technologies. They hold enormous practical value, particularly in current and future transportation. Accurate, continuous, and stable position information is crucial for robots and autonomous vehicles. While global satellite navigation systems can provide this information, the complexity of urban environments often leads to errors. Simultaneous positioning and mapping is used to address the positioning, navigation, and mapping challenges for autonomous vehicles and robots in unknown environments, offering high robustness in complex scenarios.

[0005] The accuracy of map matching localization methods is directly related to the accuracy of map construction. Highly accurate position information is obtained by matching against a preloaded map. During map construction, drift is inevitable and often affects the estimated state of the accompanying trajectory. However, the effects of drift can be effectively eliminated by revisiting the same location multiple times, creating a more consistent map. Location re-identification, also known as loop closure detection, is crucial for identifying and returning to the same location during map construction. Many vision-based location re-identification methods exist and have demonstrated their effectiveness in real-world cases. However, changes in viewpoint and light intensity can cause detection failures. In contrast, lidar, as an active sensor, is most readily applicable to location re-identification. Existing lidar-based recognition methods can be categorized as direct and descriptor-based. Direct methods use lidar point cloud information for matching. However, due to the inherent errors in raw lidar data, recognition performance inevitably degrades. Descriptor-based methods, on the other hand, are categorized as local and global descriptors. These two descriptors differ in the amount of information in their point clouds, maintaining performance under various variations and maintaining high adaptability in terms of sampling and noise immunity. However, traditional descriptors simply capture environmental information, ignoring some of the intrinsic properties of point clouds. They struggle to handle offsets or rotations when revisiting the same location, such as changes in heading or roll angle. Furthermore, recognition efficiency is a key issue in location re-identification. Traditional detection methods rely on brute-force matching with descriptors, which wastes significant time and struggles to meet practical engineering requirements. Summary of the Invention

[0006] The purpose of the present invention is to provide a vehicle position re-identification method and system to overcome the problems of low efficiency of traditional lidar position re-identification and low recognition accuracy in complex revisit environments.

[0007] A vehicle position re-identification method comprises the following steps:

[0008] S1, preprocessing the vehicle's lidar data to obtain the vehicle's lidar point cloud results;

[0009] S2, constructing the environment descriptor using the acquired point cloud results;

[0010] S3, calculating a dynamic filtering strategy threshold according to the type of the lidar sensor that obtains the vehicle's lidar data and the measurement environment;

[0011] S4, based on the constructed environment descriptor and the obtained dynamic filtering strategy threshold, compare it with the point cloud data in the historical database to obtain the vehicle's location information.

[0012] Preferably, preprocessing the vehicle's lidar data to obtain the vehicle's lidar point cloud results specifically includes the following steps:

[0013] Downsampling the vehicle’s lidar data;

[0014] Then, the data within a set range around the lidar sensor used to obtain the lidar data is filtered out from the downsampled lidar data;

[0015] The laser scan is dedistorted according to the vehicle's speed to obtain the point cloud result of the vehicle's lidar.

[0016] Preferably, the point cloud voxel filter in PCL is used to downsample the point cloud in the vehicle's lidar data: SetInputCloud(source_cloud)(1) SetLeafSize(x_size,y_size,z_size)(2) Filter(*dest_cloud)(3)

[0017] Where source_cloud is the original point cloud data in the collected lidar data, and dest_cloud is the point cloud data obtained after downsampling. SetInputCloud(·), SetLeafSize(·), and Filter(·) are functions related to point cloud downsampling in PCL.

[0018] Preferably, filtering out data within a set range around the lidar sensor used to obtain the lidar data from the downsampled lidar data comprises the following steps: for each laser point in the laser scan of the lidar sensor, removing points whose Euler distance is set to a set threshold:

[0019] where x i and y i They represent the i-th laser point in a frame of laser scanning respectively; through the above steps, the laser points within the set threshold δs range around the distance sensor are removed.

[0020] Preferably, the distorted data is removed: [x i ,y i ,z i ]=[x i ,y i ,z i ]*T*realTime+[x i ,y i ,z i ]*realTime(6)

[0021] Where realTime is the relative time of each laser point in a frame scan, and T is the estimated motion of the vehicle at that moment.

[0022] Preferably, g(·) is used to determine whether each laser point in the point cloud result is a ground point:

[0023] Secondly, all points in the laser point cloud are converted to polar coordinates:

[0024] Where d is the horizontal distance from the laser point to the lidar sensor, and θ is the angle between the laser point and the polar axis. From a bird's-eye view, the points are segmented horizontally and vertically, and the vertical and horizontal parts are divided into Ns and Nr parts according to distance and angle, respectively. A corresponding Nr*Ns matrix is ​​constructed to represent each segmented part. The obtained matrix is ​​assigned a value to each unit using the ground point height information and the non-ground point intensity information:

[0025] Where Q(i,j) represents the value assigned to each unit of the matrix, Z(i,j) represents the maximum value of the height z in the corresponding space, and I(i,j) represents the maximum value of the intensity in the corresponding space. By assigning values ​​to the matrix corresponding to each space, the point cloud descriptor is constructed.

[0026] Preferably, the difference in F norm between any two descriptors is:

[0027] Among them F i and F j is the F-norm value of the descriptor constructed from two different lidar measurements;

[0028] Set dynamic threshold:

[0029] where Δρ k is the translation amount, and the maximum Δρ is Δρ max , taking z and d as the average values ​​of the sensor's general measurements, we can simplify and obtain:

[0030] By comparing the relationship between Ψ(·) and Φ(·), we can determine whether the two scans are of the same location.

[0031] A vehicle position re-identification system includes a data acquisition and processing module, a descriptor construction module, a threshold calculation module and a re-identification module:

[0032] The data acquisition and processing module is used to pre-process the vehicle's lidar data to obtain the point cloud results of the vehicle's lidar;

[0033] Descriptor construction module, used to construct environment descriptors based on the acquired point cloud results;

[0034] The re-identification module calculates the dynamic filtering strategy threshold based on the type of lidar sensor that acquires the vehicle's lidar data and the measurement environment;

[0035] S4, based on the constructed environment descriptor and the obtained dynamic filtering strategy threshold, compare it with the point cloud data in the historical database to obtain the vehicle's location information.

[0036] Preferably, preprocessing the vehicle's lidar data to obtain the vehicle's lidar point cloud results specifically includes the following steps:

[0037] Downsampling the vehicle’s lidar data;

[0038] Then, the data within a set range around the lidar sensor used to obtain the lidar data is filtered out from the downsampled lidar data;

[0039] The laser scan is dedistorted according to the vehicle's speed to obtain the point cloud result of the vehicle's lidar.

[0040] Preferably, the point cloud voxel filter in PCL is used to downsample the point cloud in the vehicle's lidar data: SetInputCloud(source_cloud)(1) SetLeafSize(x_size,y_size,z_size)(2) Filter(*dest_cloud)(3)

[0041] Where source_cloud is the original point cloud data in the collected lidar data, and dest_cloud is the point cloud data obtained after downsampling. SetInputCloud(·), SetLeafSize(·), and Filter(·) are functions related to point cloud downsampling in PCL.

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

[0043] The present invention provides a vehicle position re-identification method, which obtains point cloud results of the vehicle's lidar by preprocessing the vehicle's lidar data; constructs an environmental descriptor using the obtained point cloud results; calculates a dynamic filtering strategy threshold based on the type of lidar sensor that obtains the vehicle's lidar data and the measurement environment; and compares the constructed environmental descriptor and the obtained dynamic filtering strategy threshold with point cloud data in a historical database to obtain the vehicle's position information. The present invention can improve the accuracy and efficiency of position re-identification. The present invention considers the angular resolution and measurement range of different lidars and proposes a dynamic threshold filtering strategy, thereby improving the generalization ability of the method for multiple sensor models.

[0044] The present invention preferably considers rotational changes during revisits and the use of different sensor models, and finally incorporates these into the map construction through graph optimization, thereby improving the consistency of the constructed map. The present invention separates ground points and non-ground points measured by lidar, and fuses ground point height information with non-ground point intensity information to construct descriptors, thereby improving the descriptors' ability to express the environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] FIG1 is a schematic flow chart of a vehicle position re-identification method according to an embodiment of the present invention.

[0046] FIG2 is a flowchart of point cloud preprocessing in an embodiment of the present invention.

[0047] FIG3 is a schematic diagram of point cloud descriptor segmentation in an embodiment of the present invention.

[0048] FIG4 is a schematic diagram of a point cloud descriptor in an embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0050] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0051] The present invention provides a vehicle position re-identification method for high-precision vehicle position re-identification, which specifically includes the following steps:

[0052] S1, preprocessing the vehicle's lidar data to obtain the vehicle's lidar point cloud results;

[0053] S2, constructing the environment descriptor using the acquired point cloud results;

[0054] S3, calculating a dynamic filtering strategy threshold according to the type of the lidar sensor that obtains the vehicle's lidar data and the measurement environment;

[0055] S4, based on the constructed environment descriptor and the obtained dynamic filtering strategy threshold, compare it with the point cloud data in the historical database to obtain the vehicle's location information.

[0056] In one embodiment of the present invention, a system for a vehicle position re-identification method includes a data acquisition platform equipped with a lidar and a data processing and computing platform. The data acquisition platform is used to collect environmental information for building a map, and the data processing and computing platform is used to build an environmental descriptor and perform position re-identification.

[0057] The data acquisition platform includes a laser radar installed on the vehicle. The laser radar installation position is shown in Figure 1. It is placed horizontally on the vehicle roof. This embodiment uses the RS-Ruby 128 laser radar, which features a nonlinear distribution of 128 laser channels. Its longitudinal observation angle is -25° to +15°, with a minimum angular resolution of 0.1°.

[0058] The data processing computing platform is an industrial computer equipped on the vehicle, which is connected to the lidar via a network cable and is used to collect and process the environmental information collected by the lidar in real time.

[0059] Preprocess the vehicle's lidar data to obtain the point cloud results of the vehicle's lidar, which specifically includes the following steps:

[0060] The original LiDAR measurement information of the vehicle is not only difficult to process and calculate due to the large error of the vehicle and the large number of laser points.

[0061] As shown in Figure 2, before constructing the environmental descriptor, the present application requires preprocessing the collected vehicle LiDAR data. The present invention corrects the vehicle LiDAR data from three aspects, specifically including: downsampling the vehicle LiDAR data to ensure the processing efficiency of subsequent steps; secondly, filtering out the data within a set range around the LiDAR sensor used to obtain the LiDAR data from the downsampled LiDAR data to prevent interference in subsequent detection; and finally, dedistorting the laser scan according to the vehicle's driving speed to improve the accuracy of the laser scan.

[0062] The specific steps include:

[0063] S1-1 Downsample the point cloud from the vehicle's lidar data using the point cloud voxel filter in PCL: SetInputCloud(source_cloud)(1) SetLeafSize(x_size,y_size,z_size)(2) Filter(*dest_cloud)(3)

[0064] Where source_cloud is the original point cloud data from the collected LiDAR data, and dest_cloud is the point cloud data obtained after downsampling. SetInputCloud(·), SetLeafSize(·), and Filter(·) are functions related to point cloud downsampling in PCL.

[0065] S1-2 filters out the data within a set range around the lidar sensor used to obtain lidar data from the downsampled lidar data:

[0066] When a LiDAR sensor is installed on a vehicle, it collects information about the vehicle's body as environmental information, which can reduce the ability to express the environment. Therefore, for each laser point in the LiDAR sensor's laser scan, remove the points whose Euler distance is set above the set threshold:

[0067] Where xi and yi represent the i-th laser point in a frame of laser scanning respectively; through the above steps, the laser points within the set threshold δs range around the distance sensor are removed.

[0068] S1-3 laser point cloud distortion removal:

[0069] Finally, the laser scans are distorted using the vehicle's driving state. As the vehicle moves, the lidar sensor collects environmental information. The collected laser points are distorted to varying degrees due to the vehicle's speed and rotation, so this distorted data needs to be dedistorted.

[0070] [x i ,y i ,z i ]=[x i ,y i ,z i ]*T*realTime+[x i ,y i ,z i ]*realTime(6)

[0071] Where realTime is the relative time of each laser point in a frame scan, and T is the estimated motion of the vehicle at that moment.

[0072] Use the obtained point cloud results to construct the environment descriptor:

[0073] In step S1, we have obtained data within the set range around the lidar sensor and corrected the distortion of the sparse point cloud. This step will use this point cloud to construct an environment descriptor. First, we use g(·) to determine whether each laser point in the point cloud is a ground point:

[0074] Secondly, all points in the laser point cloud are converted to polar coordinates:

[0075] Where d is the horizontal distance of the laser point from the lidar sensor, and θ is the angle between the laser point and the polar axis (the positive half of the original x-axis). Then, from a bird's-eye view, the image is segmented horizontally and vertically, as shown in Figure 3. The vertical and horizontal segments are divided into Ns and Nr parts, respectively, based on distance and angle. A corresponding Nr*Ns matrix is ​​constructed to represent each segmented segment. Each cell in the resulting matrix is ​​assigned a value using ground point height information and non-ground point intensity information.

[0076] Where Q(i,j) represents the value assigned to each cell in the matrix, Z(i,j) represents the maximum value of height z in the corresponding space, and I(i,j) represents the maximum value of intensity in the corresponding space. By assigning values ​​to the matrix corresponding to each space, a point cloud descriptor is constructed. Figure 4 shows the constructed descriptor.

[0077] Calculate the dynamic filtering strategy threshold based on the type of lidar sensor that acquires the vehicle's lidar data and the measurement environment:

[0078] Through the above steps, we have constructed an environmental descriptor that compactly represents the LiDAR measurements. For two different visits to the same location, the results of the two measurements should be essentially identical, and the F-norm values ​​of the descriptors constructed should be the same. However, during actual revisits, rotations and slight shifts may occur, and these shifts cannot guarantee that the two descriptors are identical. Therefore, the difference in the F-norm between any two descriptors is defined as:

[0079] Among them F i and F j The descriptor F-norm is constructed from two different lidar measurements. For visits to the same location, the F-norm is invariant to rotation, resulting in a small difference. However, for revisits to different locations, the difference is larger, allowing for rapid identification of visits to the same location. However, in practical applications, given the lack of invariance of the F-norm to lateral translation and the varying measurement ranges of sensors, this paper designs a dynamic threshold that is specific to the sensor itself:

[0080] where Δρ k is the translation amount. In order to simplify the calculation, the maximum Δρ is specified as Δρ max To simplify the calculation, z and d are taken as the average values ​​of the general measurement of the sensor, and the simplification yields:

[0081] By comparing the relationship between Ψ(·) and Φ(·), it is possible to quickly determine whether the two scans are of the same location.

[0082] Vehicle position re-identification process:

[0083] For a laser scan P at a certain position L L , identify P from the historical point cloud data of vehicle lidar L Point clouds with the same position are called position re-identification. Since the number of historical point clouds will continue to increase with the operation time, this method proposes a hierarchical recognition strategy from coarse to fine.

[0084] S4-1 Quick Search

[0085] For any frame scanned in the history, the average value of the row vector of each matrix is ​​calculated with the help of the constructed environment description, thereby obtaining an Nr-dimensional vector; the K historical point clouds with the highest similarity to the current scan vector are quickly retrieved through KD-Tree, and these K point clouds are called initial candidates.

[0086] S4-2 F-norm filtering

[0087] Calculate the F-norm difference between the current point cloud and the K initial candidates obtained in S4-1. According to the dynamic filtering strategy threshold obtained above, compare the quantitative relationship between Ψ(·) and Φ(·), and retain candidates with Ψ(·) ≤ Φ(·), thereby reducing the number of candidates and improving the accuracy and efficiency of position re-identification.

[0088] S4-3 precise matching:

[0089] After S4-2 F-norm filtering, the remaining candidates are finally determined by exact matching to determine whether they are measurements of the same location as the current point cloud. With the help of the environmental descriptors constructed in S2, the difference between the remaining candidates and the current point cloud environmental descriptors is calculated. The vector distance between the two descriptor matrices is calculated column by column as the difference in descriptors. Let the matrix of this method corresponding to the current candidate be A ij (i≤Ns,j≤Nr), the vector distance corresponding to the candidate is B ij (i≤Ns,j≤Nr):

[0090] However, in practice, the two candidates may have different headings. To eliminate the impact of heading changes, this method calculates the difference between the two descriptors multiple times by rotating the columns of the descriptors, and takes the smallest calculated result as the final difference between the two descriptors:

[0091] By comparing the descriptor difference Dis(A, B) with the pre-set threshold δd, it is determined whether it is a visit to the same location:

[0092] In one embodiment of the present invention, a vehicle location re-identification system is provided, comprising a data acquisition and processing module, a descriptor construction module, a threshold calculation module, and a re-identification module:

[0093] The data acquisition and processing module is used to pre-process the vehicle's lidar data to obtain the point cloud results of the vehicle's lidar;

[0094] Descriptor construction module, used to construct environment descriptors based on the acquired point cloud results;

[0095] The re-identification module calculates the dynamic filtering strategy threshold based on the type of lidar sensor that acquires the vehicle's lidar data and the measurement environment;

[0096] S4, based on the constructed environment descriptor and the obtained dynamic filtering strategy threshold, compare it with the point cloud data in the historical database to obtain the vehicle's location information.

[0097] Specifically, the present invention extracts ground intensity information and height information of non-ground points from vehicle lidar data, and constructs a global environmental descriptor using a bird's-eye view. The obtained environmental descriptor is then used to identify the same point from historical visits, from coarse to fine levels. During the identification process, the present invention considers rotational changes during revisits and the use of different sensor models. Finally, graph optimization is incorporated into the constructed map to improve the consistency of the constructed map. The present invention separates ground points and non-ground points measured by the lidar, and fuses ground point height information with non-ground point intensity information to construct a descriptor, thereby improving the descriptor's ability to express the environment. The present invention considers the essential impact of rotational changes on environmental measurements and leverages the unitary invariance of the F-norm to quickly obtain more likely candidates, thereby improving the accuracy and efficiency of position re-identification. The present invention considers the angular resolution and measurement range of different lidars and proposes a dynamic threshold filtering strategy, which improves the method's generalization ability for multiple sensor models.

Claims

1. A vehicle position re-identification method, characterized in that: The following steps are involved: S1, preprocessing the vehicle's laser radar data to obtain the point cloud result of the vehicle's laser radar; S2, constructing the environment descriptor using the acquired point cloud results; S3, calculating a dynamic filtering strategy threshold according to the type of the laser radar sensor that obtains the laser radar data of the vehicle and the measurement environment; S4, comparing the constructed environment descriptor and the obtained dynamic filtering strategy threshold with the point cloud data in the historical database to obtain the vehicle's location information; Preprocessing the vehicle's LiDAR data to obtain the point cloud results of the vehicle's LiDAR specifically includes the following steps: Downsampling the vehicle’s lidar data; Then, the data within a set range around the lidar sensor used to obtain the lidar data is filtered out from the downsampled lidar data; Dedistortion is performed on the laser scan according to the vehicle's driving speed to obtain the point cloud result of the vehicle's laser radar; Use the point cloud voxel filter in PCL to downsample the point cloud in the vehicle's lidar data: SetInputCloud(source_cloud)(1) SetLeafSize(x_size,y_size,z_size)(2) Filter(*dest_cloud)(3) Where source_cloud is the original point cloud data in the collected lidar data, and dest_cloud is the point cloud data obtained after downsampling; SetInputCloud(·), SetLeafSize(·), Filter(·) are functions related to point cloud downsampling in PCL; Filtering out data within a set range around the lidar sensor used to obtain the lidar data from the downsampled lidar data includes the following steps: For each laser point in the laser scan of the lidar sensor, remove the points where the Euler distance is set to a set threshold: Where xi and yi represent the i-th laser point in a frame of laser scanning respectively; through the above steps, the laser points within the set threshold δs around the distance sensor are removed.

2. A vehicle position re-identification method according to claim 1, characterized in that: Remove the distorted data: [x i ,y i ,z i ]=[x i ,y i ,z i ]*T*realTime+[x i ,y i ,z i ]*realTime(6) Where realTime is the relative time of each laser point in a frame scan, and T is the estimated motion of the vehicle at that moment.

3. A vehicle position re-identification method according to claim 1, characterized in that: Use g(·) to determine whether each laser point in the point cloud result is a ground point: Secondly, all points in the laser point cloud are converted to the polar coordinate system: Where d is the horizontal distance of the laser point from the lidar sensor, and θ is the angle between the laser point and the polar axis. From a bird's eye view, the points are segmented horizontally and vertically, and the vertical and horizontal parts are divided into Ns and Nr parts according to distance and angle, respectively. A corresponding Nr*Ns matrix is ​​constructed to represent each segmented part. Each unit of the obtained matrix is ​​assigned a value with the help of the ground point height information and the non-ground point intensity information: Among them, Q(i,j) represents the value assigned to each unit of the matrix, Z(i,j) represents the maximum value of the height z in the corresponding space, and I(i,j) represents the maximum value of the intensity in the corresponding space; by assigning values ​​to the matrix corresponding to each space, the point cloud descriptor is constructed.

4. The vehicle position re-identification method according to claim 1, characterized in that: Suppose the F-norm difference between any two descriptors is: where F i and F j is the F-norm value of the descriptor constructed from two different lidar measurements; Set dynamic threshold: where Δρ k is the translation amount, and the maximum Δρ is Δρ max , taking z and d as the average values ​​of the general measurement of the sensor, after simplification, we get: By comparing the relationship between Ψ(·) and Φ(·), it is determined whether the two scans are of the same location.

Images