A location recognition method integrating point cloud map, motion model and local features

Abstract

The present invention proposes a location recognition method that integrates point cloud maps, motion models and local features. The proposed method overcomes the problem of location recognition in large-scale dynamic environments. The present invention does not require the use of artificial markers or exogenous signals, and can effectively overcome the influence of illumination changes while reducing system costs. The invention innovatively adds a vehicle motion model to location recognition, effectively solving the problem of perceptual confusion. Only multi-line lidar is used as the only measurement sensor, and local search results are adopted as the observation model, which provides a new idea for the data fusion solution based on Kalman filtering. Combining point cloud maps with local features significantly reduces the search space, and can still achieve superior operating efficiency with limited computing resources, ensuring that other modules of the system are carried out in an orderly manner. In addition, the location recognition performance of the invention is not affected by trajectory length or running time, and can overcome problems such as target occlusion.

Description

Technical Field

[0001] The present invention belongs to the field of surveying, mapping, remote sensing and unmanned driving technology, and specifically relates to a location recognition method that integrates point cloud maps, motion models and local features. Background Art

[0002] With the rapid development of artificial intelligence and sensor technology, autonomous robots and autonomous driving are widely used in numerous fields, such as logistics and distribution, criminal investigation and reconnaissance, transportation, and wilderness rescue. Simultaneous Localization and Mapping (SLAM), a core technology for autonomous driving, has become a hot topic in intelligent systems research in recent years. Location recognition, a technique that helps robots determine whether to revisit past locations, is closely related to SLAM systems. The classic SLAM framework consists of front-end odometry, back-end optimization, loop detection, and back-end optimization. The location recognition and loop detection modules perform the same tasks. When a loop is effectively identified, the system passes matching constraints to the back-end optimization module to mitigate trajectory drift. Rapidly and accurately identifying revisited locations and establishing loop constraints significantly improves positioning accuracy and maintains system stability.

[0003] Currently, visual location recognition solutions typically utilize a bag of words (BOW) model, encoding image data into a dictionary. Various feature descriptors are then designed and combined with the dictionary for retrieval and matching. However, in scenarios with drastic lighting changes, numerous dynamic objects, and pronounced seasonal changes, matching errors increase significantly, and location recognition performance plummets. Location recognition solutions based on 3D point cloud data focus more on spatial geometry, overcoming the impact of factors such as lighting variations and narrow fields of view. Common point cloud-based location recognition solutions include those based on local descriptors, global descriptors, semantic information, deep learning, and artificial landmarks. Local descriptors perform feature statistics in the neighborhood of key points according to specific rules, but are limited by the poor repeatability of feature points and are therefore ineffective in high-speed autonomous driving scenarios. Global descriptors directly encode the entire point cloud as an environmental description, overcoming the problem of feature instability but neglecting the interrelationships between features. Semantic features elevate data association from the traditional pixel level to the object level, enhancing a robot's understanding of its surroundings. However, this approach suffers from low data processing efficiency and requires complex operations to infer connections between objects. Deep learning methods train neural networks to enhance feature learning and generate a deep representation of image features. While this improves the efficiency and accuracy of location recognition, these features are complex and difficult to understand, and model training requires tedious data cleaning. Furthermore, model training results rely to some extent on sample diversity, and scene variations can affect recognition performance. Artificial landmarks are typically deployed in advance for specific scenes. While cost-effective, they can be easily ineffective in dynamic scenes due to obstructions to the field of view. Summary of the Invention

[0004] In response to the above-mentioned problems and defects in the prior art, the present invention proposes a novel location recognition method that integrates point cloud maps, motion models, and local features. The proposed method only requires the use of a multi-line lidar as the only measurement sensor, effectively overcoming the influence of illumination changes and being more stable than visual solutions. The invention innovatively incorporates a motion model to effectively solve the problem of perceptual confusion in location recognition, and can still effectively detect loop positions when the environments are highly similar. Combining point cloud maps with local features significantly reduces the search space, achieving superior operating efficiency with limited computing resources, improving the system's fault tolerance, and ensuring the orderly operation of other functional modules. This method can overcome the problems of target occlusion or the presence of dynamic targets, and improve the system's safety performance in dynamic cities. In addition, the method can maintain good recognition accuracy and efficiency even when the trajectories continue to increase and the data continues to accumulate. As a novel location recognition solution, it can provide recognition results quickly and accurately, improving the system's positioning accuracy.

[0005] In order to solve the problems of perceptual confusion, low operating efficiency, high recognition error rate and poor anti-interference in the key technology (position recognition) in the field of unmanned driving and artificial intelligence, the present invention proposes a position recognition method that integrates point cloud maps, motion models and local features. First, data is collected in the unmanned driving scene in advance, and the relative pose is calculated and the point cloud map is spliced through high-precision combined navigation or matching algorithms. Reference points are generated inside the map as virtual landmarks. Then, a bird's-eye view feature description set with different orientations is constructed in the map to simulate the position and orientation of the vehicle, and the map-related data is saved offline. Secondly, the vehicle trajectory is initialized, and the vehicle position is estimated by global search and local search in the map description set for the first and second frames of data respectively. Finally, in real-time position recognition, the Kalman filter algorithm is used to estimate the current vehicle position based on the vehicle motion model and local search, and further determine whether there is a loop.

[0006] To achieve the above objectives, the present invention provides a location recognition method that integrates point cloud maps, motion models, and local features, which mainly includes the following steps:

[0007] Step 1: For the multi-frame lidar point cloud recorded offline, the relative pose is calculated based on high-precision integrated navigation, the scene prior point cloud map is stitched, and reference points are generated inside the map as virtual landmarks.

[0008] Step 2: traverse the map reference points, combine them with the point cloud map, construct the bird's-eye view feature description under different directions, generate a map description set, and save the map-related data offline.

[0009] Step 3: For the point cloud of the first frame of the trajectory, a global brute force search is used in the map description set to calculate the best matching map descriptor, and the vehicle position is estimated based on the map reference point.

[0010] Step 4: For the second frame point cloud of the trajectory, with the help of the starting position estimation, a local search is performed in the map description set to calculate the best matching map descriptor, and the vehicle position is estimated based on the map reference point.

[0011] In step 5, for the real-time point cloud data, descriptor similarity is used as a reliability metric. Based on the position estimates of the first two frames, the vehicle motion model and local search are combined to estimate the current vehicle position using a Kalman filter.

[0012] Furthermore, the specific implementation of step 1 includes the following sub-steps:

[0013] In step 1.1, an autonomous vehicle equipped with a multi-line lidar system is used to pre-circuit the scene to collect data. Using integrated navigation systems such as the Global Positioning System (GPS) and Inertial Navigation System (INS), high-precision position and pose are calculated and a point cloud map is constructed. If integrated navigation is not available, a laser odometry matching algorithm can be used instead.

[0014] Step 1.2: Combine the height of the sensor and use two height thresholds z1 and z2 to filter out some point clouds in the map. m The map is downsampled by 100 voxels and projected onto the XOY plane. The LiDAR coordinate system is oriented with the X-axis pointing in the forward direction, the Y-axis pointing to the left, and the Z-axis pointing vertically upward. For other installation orientations, simply adjust the threshold accordingly.

[0015] Step 1.3, calculate the two-dimensional coordinate extreme value (x min ,y min ,x max ,y max ), generate reference points M in the map according to equidistant sampling r As virtual landmarks, use point cloud processing software such as CloudCompare to crop some unreasonable reference points. The coordinates of the reference points are calculated as follows:

[0016]

[0017] Where i and j are the row and column numbers of the sampling points, d x and d y is the sampling distance, (x,y) is the coordinate of the sampling point.

[0018] Furthermore, the specific implementation of step 2 includes the following sub-steps:

[0019] Step 2.1, construct a KD tree in the map reference point c , traverse all map reference points, at each point: with the current point as the center, rotate the map by different angles θ in turn, where the angle calculation formula is as follows:

[0020] θ i =i·θ', i∈N + , θ∈(0,2π] (2)

[0021] Where θ' is the angle resolution, i is the direction angle number, N + Represents a positive integer.

[0022] Step 2.2, for the map reference point p k (x k,y k )∈M r , where k is the map point index. Rotation angle θ i Next generate n r row n c The circular feature descriptor in the horizontal plane (bird's eye view) of the column simulates the vehicle heading towards θ at this point i The data collected at the time. Convert the descriptor to n r ×n c The vector f with 1 row and 1 column is concatenated into a map feature matrix F = [f1,f2,...,f k ]. The specific calculation method of the feature descriptor is as follows:

[0023]

[0024] Where j is the neighborhood point index, r and c are the descriptor row and column indices, and d max is the maximum distance, d' is the distance resolution, and α' is the angle resolution.

[0025] Step 2.3: Save the map-related data as an offline binary file, which includes: (1) map reference point cloud; (2) map reference point KD tree; (3) map feature matrix. The map feature matrix file records the global index of each occupied element, as follows:

[0026]

[0027] Where id m 、id θ and id f are the map reference point, orientation angle and index of the descriptor vector element respectively. s is the data recorded offline, n θ is the number of angles, n f =n r ·n c is the number of descriptor elements. mod and rem represent modulo and remainder operations respectively.

[0028] Furthermore, the specific implementation of step 3 includes the following sub-steps:

[0029] Step 3.1, after starting the real-time system, you only need to load the offline map file once. For the first frame of the lidar point cloud, perform height filtering, where the height retention range should be as consistent as possible with the map value range. Generate n r row n c Column bird's eye view descriptor, and adjust to n r ×n c A eigenvector with 1 row and 1 column.

[0030] Step 3.2, using the map descriptor as a reference, calculate the hit rate of the real-time descriptor in the map descriptor as the evaluation function. In all map descriptor sets, select the map descriptor vector corresponding to the maximum evaluation function As the best candidate. The evaluation function L is calculated as follows:

[0031]

[0032] Where f s and f m are real-time and map descriptor vectors respectively. n(1,0) and n(1,1) represent f s and f m The number of elements in (1,0) and (0,1).

[0033] Step 3.3, based on the best matching vector Index and map reference points M r , estimate the current vehicle position. The position is calculated as follows:

[0034] (x v , y v )∈M r , v=mod(id best , n θ ) (6)

[0035] Where (x v ,y v ) is the estimated position of the current vehicle, v is the map reference point index, id best The index of the best matching map description vector.

[0036] Furthermore, the specific implementation of step 4 includes the following sub-steps:

[0037] Step 4.1 For the second frame of the LiDAR point cloud in the trajectory, use the position estimate of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N points. k , construct the local descriptor set F according to the map descriptor set index L :

[0038] F L ={f i , i=[j·n f ·n θ , (j+1)·n f ·n θ ], p j ∈P} (7)

[0039] Where j is the map reference point index and i is the map feature description set index.

[0040] In step 4.2, a brute force search is used to calculate the map descriptor that best matches the real-time descriptor in the local descriptor set, and the current vehicle position and orientation are calculated based on the descriptor index.

[0041] Furthermore, the specific implementation of step 5 includes the following sub-steps:

[0042] Step 5.1: For the real-time LiDAR point cloud after trajectory initialization, use the position estimation of the first two frames and the vehicle's uniform motion model to predict the vehicle's motion position. The vehicle's motion model is as follows:

[0043]

[0044]

[0045] Where ~ and ^ represent the posterior and prior variables respectively. t+1 represents the position variable at time t+1, F(·) represents the motion model of the vehicle, A is the Jacobian coefficient matrix of the error propagation in the motion model, and V represents the variance matrix. ω x is the position estimation error, is the variance of motion prediction.

[0046] In step 5.2, using the position estimate of the previous frame, a local search is performed among the map reference points to find the best matching descriptor in the neighborhood descriptor set, and the map reference points with similar neighborhood are calculated and used as the observation model:

[0047]

[0048]

[0049] Where H(·) is the observation model, and the present invention uses local search as the observation model. is the vehicle position estimated using local search, ω s is the observation error. To utilize the real-time descriptor f s and the map descriptor f m The evaluation function result in formula (5) is calculated; in each calculation, the evaluation function result and the sampling distance d are used x d y Calculate the variance σ of the position estimate s .

[0050] Step 5.3: After determining the motion model and observation model, use Kalman filtering to estimate the current vehicle position. The calculation method is as follows:

[0051]

[0052]

[0053]

[0054] Where K is the Kalman gain, and B is the Jacobian matrix of the error propagation equation of the observation model. The present invention can quickly calculate the vehicle position through the above formula.

[0055] Furthermore, step 5 also includes comparing the historical trajectory to determine whether there is a loop. If the distance difference with the historical position is less than a set threshold, it is considered a loop, otherwise it is not a loop, thereby achieving position recognition.

[0056] Furthermore, the map rotation angle resolution in step 2.1 is set to 3 degrees, that is, θ is 3.

[0057] Furthermore, the feature description size of the annular bird's-eye view in step 2.2 is set to 40 rows and 60 columns.

[0058] Furthermore, in step 4.1, the number of nearest neighbors searched among the map reference points is 20.

[0059] The present invention has the following positive effects:

[0060] 1) This invention addresses a key issue in the fields of autonomous driving and artificial intelligence: location recognition. It proposes a novel location recognition method that integrates point cloud maps, motion models, and local features. This method calculates the vehicle's position within a global map, effectively addressing the issues of perceptual confusion and high recognition error rates in location recognition or loop closure detection.

[0061] 2) Unlike conventional Kalman filter data fusion algorithms, this invention employs local search feature descriptors as the observation model, creating a new approach to Kalman filtering. Furthermore, by using only lidar as the single measurement sensor, it significantly reduces the cost of autonomous driving systems and improves the algorithm's applicability.

[0062] 3) This invention combines maps with local features, significantly reducing the search space and achieving superior operational efficiency even with limited computing resources. The incorporation of a motion model into the algorithm effectively overcomes the effects of target occlusion or dynamic targets, significantly improving the system's fault tolerance. Furthermore, location recognition performance does not degrade with trajectory or runtime, enhancing the safety of autonomous driving systems in dynamic urban environments.

[0063] The present invention can effectively realize the position recognition of autonomous driving vehicles in large-scale, dynamic urban environments, and the real-time position calculation rate can reach 200FPS (Frames Per Seond) without relying on other external signals. While reducing the system cost, it significantly improves the efficiency of loop detection in SLAM technology. It proposes to use local feature search as an observation model without the help of other sensor measurements, providing a new idea for the Kalman filter algorithm. This invention solves the problem of position recognition in large-scale scenes, and adds motion models to the position calculation, which can solve the effects of perception confusion, target occlusion, etc. At the same time, rapid position recognition can effectively build matching associations with historical data, improve the overall positioning accuracy of the system, ensure the normal operation of other modules, and effectively improve the fault tolerance of the system. In addition, the position recognition performance will not be degraded by the increase of trajectory and running time. Therefore, the method of the present invention is of great significance to the fields of unmanned driving, robotics, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 Flowchart of the present invention.

[0065] Figure 2 Schematic diagram of global map reference points in the present invention.

[0066] Figure 3 This is a schematic diagram of the position estimation of the first frame of the trajectory in the present invention.

[0067] Figure 4 This is a schematic diagram of the second frame position estimation of the trajectory in the present invention.

[0068] Figure 5 Schematic diagram of the Kalman filter algorithm taking into account motion model and local search in the present invention.

[0069] Figure 6 Schematic diagram of the calculation results of revisited position matching in the present invention. DETAILED DESCRIPTION

[0070] The solution of the present invention is further described in detail below with reference to the accompanying drawings.

[0071] like Figure 1 As shown, a location recognition method that integrates point cloud maps, motion models, and local features mainly includes the following steps:

[0072] Step 1: For the multi-frame lidar point cloud recorded offline, the relative pose is calculated based on high-precision integrated navigation, the scene prior point cloud map is stitched, and reference points are generated inside the map as virtual landmarks.

[0073] First, lidar point cloud data is collected in advance around the autonomous driving scene, and the vehicle's posture information is calculated using high-precision combined navigation. The relative posture between adjacent frame point clouds is further calculated. One key frame is selected every 10 frames, and all key frame point clouds are spliced into a point cloud map in the same coordinate system.

[0074] Next, measure the sensor's height from the ground and set two height thresholds, z1 and z2, to retain point clouds within these thresholds. Downsample the map using a voxel grid with a side length of 0.1m and project it onto the horizontal plane. In the experiment, the lidar's coordinate system was oriented with the X-axis pointing in the forward direction, the Y-axis pointing to the left, and the Z-axis pointing vertically upward. For other installation orientations, adjust the thresholds accordingly.

[0075] Finally, calculate the coordinate extremes of the map projection point cloud and set the sampling distance interval d in the x and y directions x and d y The distance between the two points is 1m. Map reference points are generated, and some erroneous points are cropped using CloudCompare.

[0076] In this embodiment, the specific implementation of step 1 is as follows:

[0077] In step 1.1, an autonomous vehicle equipped with a multi-line lidar system is used to pre-circuit the scene to collect data. Using integrated navigation systems such as the Global Positioning System (GPS) and Inertial Navigation System (INS), high-precision position and pose are calculated and a point cloud map is constructed. If integrated navigation is not available, a laser odometry matching algorithm can be used instead.

[0078] Step 1.2: Combine the height of the sensor and use two height thresholds z1 and z2 to filter out some point clouds in the map. m The map is downsampled by 100 voxels and projected onto the XOY plane. The LiDAR coordinate system is oriented with the X-axis pointing in the forward direction, the Y-axis pointing to the left, and the Z-axis pointing vertically upward. For other installation orientations, simply adjust the threshold accordingly.

[0079] Step 1.3, calculate the two-dimensional coordinate extreme value (x min ,y min ,x max ,y max ), generate reference points M in the map according to equidistant sampling r As virtual landmarks, use point cloud processing software such as CloudCompare to crop some unreasonable reference points. The coordinates of the reference points are calculated as follows:

[0080]

[0081] Where i and j are the row and column numbers of the sampling points, d x and d y is the sampling distance, (x,y) is the coordinate of the sampling point.

[0082] Step 2: traverse the map reference points, combine them with the point cloud map, construct the bird's-eye view feature description under different directions, generate a map description set, and save the map-related data offline.

[0083] First, a KD tree is constructed in the map reference points, and at each reference point, the map point cloud is rotated with an angular resolution of 3 degrees around the reference point. Figure 2 In (a), the green points are map reference points, and the orange points are point cloud maps.

[0084] Then, at each reference point and each rotation angle, a 40-row 60-column ring descriptor is generated with a maximum distance d max For 80m, each descriptor is converted into a vector of 2400 rows and 1 column, and all feature vectors are constructed into a feature matrix. Figure 2 (b) is a schematic diagram of the bird’s-eye view feature descriptor.

[0085] Finally, the map reference point cloud, map feature matrix and map reference point KD tree are saved as offline files.

[0086] In this embodiment, the specific implementation of step 2 is as follows:

[0087] Step 2.1, construct a KD tree in the map reference point c , traverse all map reference points, at each point: with the current point as the center, rotate the map by different angles θ in turn, where the angle calculation formula is as follows:

[0088] θ i =i·θ',i∈N + ,θ∈(0,2π] (2)

[0089] Where θ' is the angle resolution, i is the direction angle number, N + Represents a positive integer.

[0090] Step 2.2, for the map reference point p k (x k ,y k )∈M r , p k Indicates the reference point M r The kth point in x k ,y k Indicates p k The horizontal and vertical coordinates of , where k is the map point index. Rotation angle θi Next generate n r row n c The circular feature descriptor in the horizontal plane (bird's eye view) of the column simulates the vehicle heading towards θ at this point i The data collected when the descriptor is converted to n r ×n c The vector f with 1 row and 1 column is concatenated into a map feature matrix F = [f1,f2,....,f k ]. The specific calculation method of the feature descriptor is as follows:

[0091]

[0092] Where j is the neighborhood point index, r and c are the descriptor row and column indices, and d max is the maximum distance, d' is the distance resolution, and α' is the angle resolution.

[0093] Step 2.3: Save the map-related data as an offline binary file, which includes: (1) map reference point cloud; (2) map reference point KD tree; (3) map feature matrix. The map feature matrix file records the global index of each occupied element, as follows:

[0094]

[0095] Where id m 、id θ and id f are the map reference point, orientation angle and index of the descriptor vector element respectively. s is the data recorded offline, n θ is the number of angles, n f =n r ·n c is the number of descriptor elements. mod and rem represent modulo and remainder operations respectively.

[0096] Step 3, such as Figure 3 This figure shows the position calculation diagram for the first frame of trajectory data. For the point cloud of the first frame of the trajectory, a global brute force search is performed to calculate the best matching map descriptor in the map description set, and the vehicle position is estimated based on the map reference points.

[0097] First, after starting the real-time system, load the offline map file. For the first frame of the lidar point cloud, generate a 40-row and 60-column circular bird's-eye view feature descriptor and adjust it to a 2400-row and 1-column vector.

[0098] Then, the hit rate of the real-time descriptor in the map descriptor is calculated as the evaluation function, and a brute force search is performed among all the map descriptors to calculate the map descriptor with the optimal evaluation function.

[0099] Finally, the vehicle’s position is estimated based on the index of the optimal map descriptor and the map reference point cloud.

[0100] In this embodiment, the specific implementation of step 3 is as follows:

[0101] Step 3.1, after starting the real-time system, you only need to load the offline map file once. For the first frame of the lidar point cloud, perform height filtering, where the height retention range should be as consistent as possible with the map value range. Generate n r row n c Column bird's eye view descriptor, and adjust to n r ×n c A eigenvector with 1 row and 1 column.

[0102] Step 3.2, using the map descriptor as a reference, calculate the hit rate of the real-time descriptor in the map descriptor as the evaluation function. In all map descriptor sets, select the map descriptor vector corresponding to the maximum evaluation function As the best candidate. The evaluation function L is calculated as follows:

[0103]

[0104] Where f s and f m are real-time and map descriptor vectors respectively. n(1,0) and n(1,1) represent f s and f m The number of elements in (1,0) and (0,1).

[0105] Step 3.3, based on the best matching vector Index and map reference points M r , estimate the current vehicle position. The position is calculated as follows:

[0106] (x v , y v )∈M r , v=mod(id best , n θ ) (6)

[0107] Where (x v ,y v ) is the estimated position of the current vehicle, v is the map reference point index, id best The index of the best matching map description vector.

[0108] Step 4, such as Figure 4 This figure shows the position calculation diagram for the second frame of data. For the point cloud of the second frame of the trajectory, using the position estimate from the first frame, a local search is performed within the map descriptor set to calculate the best matching map descriptor, and the vehicle position is estimated based on the map reference points.

[0109] First, for the second frame point cloud, the position results of the first frame are used to search for 20 nearest neighbor points in the map reference point cloud, and a local descriptor set is constructed based on the map descriptor index.

[0110] Then, in the local descriptor set, a brute force search is performed to find the best matching map descriptor, and the current position of the vehicle is estimated based on the index.

[0111] In this embodiment, the specific implementation of step 4 is as follows:

[0112] Step 4.1: For the second frame of the LiDAR point cloud in the trajectory, use the position estimate of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N points. k , construct the local descriptor set F according to the map descriptor set index L :

[0113] F L ={f i , i=[j·n f ·n θ , (j+1)·n f ·n θ ], p j ∈P} (7)

[0114] Where j is the map reference point index and i is the map feature description set index.

[0115] In step 4.2, a brute force search is used to calculate the map descriptor that best matches the real-time descriptor in the local descriptor set, and the current vehicle position and orientation are calculated based on the descriptor index.

[0116] Step 5, such as Figure 5 Figure 1 shows the position calculation for real-time data. For real-time point cloud data, descriptor similarity is used as a reliability metric. Using the position estimates from the previous two frames, combined with the vehicle motion model and local search, a Kalman filter is used to estimate the current vehicle position. This is then compared to historical trajectories to determine if loops exist.

[0117] First, for the real-time frame lidar point cloud, the position estimates from the previous two frames are used to predict the current vehicle position using a uniform motion model. The invention assumes that the position estimation error does not exceed the sampling distance of the map reference point, setting the position estimation error to 1m.

[0118] Then, using the position estimate from the previous frame, a local search similar to the second frame of the trajectory is used to estimate the position of the vehicle.

[0119] Finally, the similarity of feature descriptors is used as a measure of position deviation, and a Kalman filter algorithm is used to estimate the current position. This is then compared with the historical trajectory to determine whether a loop exists. If the distance difference with the historical position is less than a set threshold, it is considered a loop; otherwise, it is not a loop, thus achieving position recognition.

[0120] In this embodiment, the specific implementation of step 5 is as follows:

[0121] Step 5.1: For the real-time LiDAR point cloud after trajectory initialization, use the position estimation of the first two frames and the vehicle's uniform motion model to predict the vehicle's motion position. The vehicle's motion model is as follows:

[0122]

[0123]

[0124] Where ~ and ~ represent the posterior and prior variables respectively. t+1 represents the position variable at time t+1, F(·) represents the vehicle's motion model, A is the Jacobian coefficient matrix of error propagation in the motion model, and V represents the variance matrix. ω x is the position estimation error, is the variance of motion prediction.

[0125] In step 5.2, using the position estimate of the previous frame, a local search is performed among the map reference points to find the best matching descriptor in the neighborhood descriptor set, and the map reference points with similar neighborhood are calculated and used as the observation model:

[0126]

[0127]

[0128] Where H(·) is the observation model, and the present invention uses local search as the observation model. is the vehicle position estimated using local search, ω s is the observation error. To utilize the real-time descriptor f s and the map descriptor f m The evaluation function result in formula (5) is calculated; in each calculation, the evaluation function result and the sampling distance d are used x d y , calculate the variance σ of the position estimate s ;

[0129] Step 5.3: After determining the motion model and observation model, use Kalman filtering to estimate the current vehicle position. The calculation method is as follows:

[0130]

[0131]

[0132]

[0133] Where K is the Kalman gain, and B is the Jacobian matrix of the error propagation equation of the observation model. The present invention can quickly calculate the vehicle position through the above formula.

[0134] Step 5.4, compare the historical trajectory of the vehicle to determine whether there is a loop. Figure 6 The following diagram illustrates the results of revisited position matching in three dynamic environments. The horizontal black line represents the vehicle trajectory, the vertical gray line represents the revisited position matching, and the vertical black line represents an incorrect matching result. Three typical outdoor dynamic scenes were selected as experimental scenarios: urban (DCC 02, #1), university (KAIST 02, #2), and rural (KITTI 05, #3). The poses were all calculated using high-precision integrated navigation, and only the LiDAR point cloud was used in the experiment to resolve the position information. 3151 frames (#1), 4190 frames (#2), and 2761 frames (#3) of point clouds were collected for each scene, respectively. In the outdoor environment, loopback matching was performed at a distance of 8 meters. Each data sequence contains a certain amount of revisited positions. To maintain a consistent threshold, a position error within 8 meters is considered a correct position calculation result. The experimental results are shown in Table 1. Under a wide range of dynamic environments, the position calculation success rate can reach over 99%, with an average error of approximately 0.5 meters, demonstrating excellent position accuracy. The lightweight descriptor creation takes approximately 6ms, and the real-time position resolution speed can reach 200 FPS. In terms of revisited position recognition, the maximum F1 score can reach over 0.99, demonstrating excellent position recognition accuracy. This method can provide an efficient and accurate position recognition method for autonomous driving, improving the overall stability of the system.

[0135] Table 1 Test performance of location recognition in different scenarios

[0136]

[0137] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various changes and variations. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A location recognition method integrating point cloud map, motion model and local features, characterized in that: The following steps are involved: Step 1: For the multi-frame lidar point cloud recorded offline, the relative pose is calculated based on high-precision integrated navigation, the scene prior point cloud map is stitched together, and reference points are generated within the map as virtual landmarks; Step 2: traverse the map reference points, combine them with the point cloud map, construct the feature description of the bird's-eye view under different directions, generate a map description set, and save the map-related data offline; The specific implementation of step 2 includes the following sub-steps: Step 2.1, construct a KD tree in the map reference point c , traverse all map reference points, at each point: with the current point as the center, rotate the map by different angles θ in turn, where the angle calculation formula is as follows: i i =i·θ',i∈N + ,θ∈(0,2π] (2) Where θ' is the angle resolution, i is the direction angle number, N + represents a positive integer; Step 2.2, for the map reference point p k (x k ,y k )∈M r , p k Indicates the reference point M r The kth point in x k ,y k Indicates p k The horizontal and vertical coordinates, where k is the map point index and the rotation angle θ i Next generate n r row n c The circular feature descriptor in the horizontal plane of the column simulates the vehicle heading towards θ at this point i The data collected at the time, the descriptor is converted to n r ×n c The vector f with 1 row and 1 column is concatenated into a map feature matrix F = [f1,f2,...,f k ], the specific calculation method of the feature descriptor is as follows: Where j is the neighborhood point index, r and c are the descriptor row and column indices, and d max is the maximum distance, d' is the distance resolution, and α' is the angle resolution; Step 2.3: Save the map-related data as an offline binary file, which includes: (1) map reference point cloud; (2) map reference point KD tree; (3) map feature matrix; the map feature matrix file records the global index of each occupied element, as follows: id m =mod(s,n f ·n θ ) id θ =mod(rem(s,n f ·n θ ),n f ) (4) id f =rem(rem(s,n f ·n θ ),n f ) Where id m 、id θ and id f are the map reference point, orientation angle and index of the descriptor vector element respectively, s is the data recorded offline, n θ is the number of angles, n f =n r ·n c is the number of descriptor elements, mod and rem represent modulo and remainder operations respectively; Step 3: For the point cloud of the first frame of the trajectory, a global brute force search is used to calculate the best matching map descriptor in the map description set, and the vehicle position is estimated based on the map reference points; Step 4: For the second frame point cloud of the trajectory, with the help of the starting position estimate, a local search is performed in the map description set to calculate the best matching map descriptor, and the vehicle position is estimated based on the map reference point; In step 5, for real-time point cloud data, descriptor similarity is used as a reliability metric. With the help of the position estimation of the first two frames, combined with the vehicle motion model and local search, the Kalman filter is used to estimate the current vehicle position.

2. The location recognition method according to claim 1, wherein: The specific implementation of step 1 includes the following sub-steps: In step 1.1, an autonomous vehicle equipped with a multi-line lidar is used to collect data in advance within the scene. The vehicle then uses a combination of the Global Positioning System (GPS) and the Inertial Navigation System (INS) to calculate high-precision poses and stitch together a point cloud map. Step 1.2: Combine the height of the sensor and use two height thresholds z1 and z2 to filter out some point clouds in the map. m The voxels of the map are downsampled and projected into the XOY plane. The coordinate system of the lidar is oriented such that the X axis points in the forward direction, the Y axis points to the left, and the Z axis points vertically upward. Step 1.3, calculate the two-dimensional coordinate extreme value (x min ,y min ,x max ,y max ), generate reference points M in the map according to equidistant sampling r As virtual landmarks, some unreasonable reference points are cropped using point cloud processing software. The coordinates of the reference points are calculated as follows: Where i and j are the row and column numbers of the sampling points, d x and d y is the sampling distance, (x,y) is the coordinate of the sampling point.

3. The location recognition method according to claim 1, wherein: The specific implementation of step 3 is as follows: Step 3.1: After starting the real-time system, you only need to load the offline map file once, and perform height filtering on the first frame of the lidar point cloud. The height retention range is consistent with the map value range, and n is generated. r row n c Column bird's eye view descriptor, and adjust to n r ×n c The characteristic vector of row and column; Step 3.2: Using the map descriptor as a reference, calculate the hit rate of the real-time descriptor in the map descriptor as the evaluation function. In all map descriptor sets, select the map descriptor vector corresponding to the maximum evaluation function. As the best candidate, the evaluation function L is calculated as follows: Where f s and f m are real-time and map descriptor vectors, n(1,0) and n(1,1) represent f respectively. s and f m The number of elements in (1,0) and (0,1); Step 3.3, based on the best matching vector Index and map reference points M r , estimate the current vehicle position, the position calculation method is as follows: (x v ,y v )∈M r ,v=mod(id best ,n θ ) (6) Where (x v ,y v ) is the estimated position of the current vehicle, v is the map reference point index, n θ is the number of angles, id best The index of the best matching map description vector.

4. The location recognition method according to claim 1, wherein: The specific implementation of step 4 is as follows; Step 4.1: For the second frame of the LiDAR point cloud in the trajectory, use the position estimate of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N points. k , construct the local descriptor set F according to the map descriptor set index L : F L ={f i ,i=[j·n f ·n θ ,(j+1)·n f ·n θ ],p j ∈P} (7) Where j is the map reference point index, i is the map feature description set index, and n θ is the number of angles; In step 4.2, a brute force search is used to calculate the map descriptor that best matches the real-time descriptor in the local descriptor set, and the current vehicle position and orientation are calculated based on the descriptor index.

5. The location recognition method according to claim 3, wherein: The specific implementation of step 5 is as follows: Step 5.1: For the real-time lidar point cloud after trajectory initialization, use the position estimation of the first two frames and the vehicle's uniform motion model to predict the vehicle's motion position. The vehicle's motion model is as follows: Where ~ and ^ represent the posterior and prior variables respectively, x t+1 represents the position variable at time t+1, F(·) represents the motion model of the vehicle, A is the Jacobian coefficient matrix of error propagation in the motion model, V represents the variance matrix, ω x is the position estimation error, is the variance of motion prediction; In step 5.2, using the position estimate of the previous frame, a local search is performed among the map reference points to find the best matching descriptor in the neighborhood descriptor set, and the map reference points with similar neighborhood are calculated and used as the observation model: Where H(·) is the observation model, and local search is used as the observation model; is the vehicle position estimated using local search, ω s is the observation error; To utilize the real-time descriptor f s and the map descriptor f m The evaluation function result in formula (5) is calculated; in each calculation, the evaluation function result and the sampling distance d are used x d y , calculate the variance σ of the position estimate s ; Step 5.3: After determining the motion model and observation model, use Kalman filtering to estimate the current vehicle position. The calculation method is as follows: Where K is the Kalman gain, and B is the Jacobian matrix of the error propagation equation of the observation model. The vehicle position can be quickly calculated using the above formula.

6. The location recognition method according to claim 1, wherein: Step 5 also includes comparing the historical trajectory to determine whether there is a loop. If the distance difference with the historical position is less than the set threshold, it is considered a loop; otherwise, it is not a loop, thereby achieving position recognition.

7. The location recognition method according to claim 1, wherein: In step 2.1, the map rotation angle resolution is set to 3 degrees, that is, θ is 3.

8. The location recognition method according to claim 1, wherein: In step 2.2, the feature description size of the annular bird's-eye view is set to 40 rows and 60 columns.

9. The location recognition method according to claim 4, wherein: In step 4.1, the number of nearest neighbors searched among the map reference points is 20.

Images