WO2024120269A1

WO2024120269A1 - Position recognition method for fusing point cloud map, motion model and local feature

Info

Publication number: WO2024120269A1
Application number: PCT/CN2023/134918
Authority: WO
Inventors: 张永军; 史鹏程; 李加元
Original assignee: 武汉大学
Priority date: 2022-12-05
Filing date: 2023-11-29
Publication date: 2024-06-13
Also published as: CN116127405A

Abstract

Provided in the present invention is a position recognition method for fusing a point cloud map, a motion model and a local feature. The proposed method overcomes the problem of position recognition in a large-range dynamic environment. The present invention can effectively overcome the effects of changes in illumination, etc., while reducing system costs without the need for an artificial mark or an external signal. In the present invention, a vehicle motion model is innovatively added to position recognition, such that the problem of awareness confusion is effectively solved. Only a multi-line LiDAR is used as the sole measurement sensor and a local search result is used as an observation model, such that a new line of thought is provided for a data fusion scheme based on Kalman filtering. A point cloud map is combined with a local feature, such that a search space is significantly reduced, the excellent operation efficiency can still be achieved with limited computing resources, and the orderly progress of other modules of a system is guaranteed. In addition, the position recognition performance in the present invention is not affected by trajectory length or operation time, such that problems such as target shielding can be overcome.

Description

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

Technical Field

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 technique

With the rapid development of artificial intelligence and sensor technology, autonomous robots and autonomous driving are widely used in many fields, such as logistics distribution, criminal investigation, transportation and field rescue. Simultaneous Localization and Mapping (SLAM), as the core technology of autonomous driving, has gradually become a hot topic in the research of intelligent systems in recent years. Position recognition is a technology that helps robots determine whether to revisit historical locations, and it has a strong correlation with the SLAM system. The classic SLAM framework includes: front-end odometer, back-end optimization, loop detection and back-end optimization, among which the tasks of position recognition and loop detection modules are the same. When the loop is effectively identified, the system passes the matching constraints to the back-end optimization module to weaken the trajectory drift. Quickly and accurately identifying revisited locations and constructing loop constraints are very helpful in improving positioning accuracy and maintaining system stability.

At present, visual location recognition schemes usually use word models (BOW, Bag of Words) to encode image data into dictionaries, and design various feature descriptors combined with dictionaries for retrieval and matching. However, in scenes with drastic lighting changes, many dynamic targets, and obvious seasonal changes, the matching error increases significantly and the location recognition performance drops sharply. Location recognition schemes based on three-dimensional point cloud data pay more attention to spatial geometric information and can overcome the influence of factors such as lighting changes and small field of view. At present, common point cloud-based location recognition schemes include: local descriptor-based, global descriptor-based, semantic information-based, deep learning-based, and artificial landmark-based methods. Local descriptors perform feature statistics on the neighborhood space of key points according to specific rules, but are limited by the poor repeatability of feature points and are not effective 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 ignoring the relationship between features. Semantic features elevate data association from the traditional pixel level to the object level, which can enhance the robot's understanding of the surrounding environment. However, this method has low data processing efficiency and requires complex operations to infer the relationship between targets. Deep learning methods strengthen feature learning by training neural networks to generate deep expressions of image features. Although it improves the efficiency and accuracy of location recognition, These features are complex and difficult to understand, and model training requires tedious data cleaning. In addition, the model training results depend to a certain extent on the diversity of samples, and the differences in scenes may affect the recognition effect. Artificial landmarks are usually deployed in advance in specific scenes. Although the cost is low, they are easily ineffective in dynamic scenes due to obstruction of vision.

Summary of the invention

In view of the above problems and defects existing in the prior art, the present invention proposes a novel location recognition method integrating point cloud maps, motion models and local features. The proposed method only needs to use multi-line laser radar as the only measurement sensor, effectively overcomes the influence of illumination changes, and is more stable than the visual solution. The invention innovatively incorporates the motion model to effectively solve the problem of perceptual confusion in location recognition, and can still effectively detect the loop position when the environment is highly similar. Combining the point cloud map with the local features significantly reduces the search space, and can still achieve superior operating efficiency under limited computing resources, improve the fault tolerance of the system, and ensure that other functional modules are carried out in an orderly manner. The method can overcome the problem of target occlusion or the presence of dynamic targets, and improve the safety performance of the system in dynamic cities. In addition, the method can still maintain good recognition accuracy and efficiency when the trajectory continues to increase and the data continues to accumulate. As a novel location recognition solution, it can provide recognition results quickly and accurately, and improve the positioning accuracy of the system.

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 map, motion model and local features. First, data is collected in the unmanned driving scene in advance, and the relative posture is calculated and the point cloud map is spliced through high-precision combined navigation or matching algorithm, and 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 frame and the second frame of data, respectively. Finally, in real-time position recognition, based on the vehicle motion model and local search, the Kalman filter algorithm is used to estimate the current vehicle position, and further determine whether there is a loop.

To achieve the above purpose, the present invention designs a location recognition method integrating point cloud map, motion model and local features, which mainly includes the following steps:

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.

Step 2: traverse the map reference points, combine 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.

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.

Step 4: For the point cloud of the second frame of the trajectory, with the help of the starting position estimation, a local 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.

Step 5: For real-time point cloud data, descriptor similarity is used as a reliability measure. 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.

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

Step 1.1: Use an autonomous driving vehicle equipped with a multi-line laser radar to collect data in advance inside the scene. Use combined navigation such as the global positioning system (GPS) and the inertial navigation system (INS) to calculate high-precision pose and stitch point cloud maps. If there is no combined navigation device, a laser odometer matching algorithm can be used instead.

Step 1.2, use two height thresholds z ₁ and z ₂ to filter out some point clouds in the map based on the sensor height, downsample the map using voxels with a side length of 1 _m , and project it into the XOY plane. The coordinate system of the laser radar is in the direction of travel with the X axis pointing to the left, and the Z axis pointing vertically upward. If it is installed in other directions, adjust the threshold accordingly.

Step 1.3, calculate the two-dimensional coordinate extreme values (x _min , y _min , x _max , y _max ) of the projected map, generate reference points _Mr in the map as virtual landmarks according to equidistant sampling, and use point cloud processing software such as CloudCompare to crop some unreasonable reference points. The reference point coordinates are calculated as follows:

Where i and j are the row and column numbers of the sampling points, _dx and _dy are the sampling distances, and (x, y) are the coordinates of the sampling points.

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

Step 2.1, construct a KD tree tree _c in the map reference point, traverse all map reference points, and at each point: take 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∈N ₊ ,θ∈(0,2π] (2)

Where θ' is the angle resolution, i is the orientation angle number, and N ₊ represents a positive integer.

Step 2.2, for the map reference point p _k (x _k ,y _k )∈M _r , where k is the map point index. Generate a circular feature descriptor in the horizontal plane (bird's eye view) with n _r rows and n _c columns under the rotation angle θ _i , simulating the data collected when the vehicle is facing θ _i at this point. Convert the descriptor into a vector f with n _r × n _c rows and 1 column, and concatenate all descriptor vectors into a map feature matrix F = [f ₁ ,f ₂ ,...,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, 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:

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 modulus and remainder operations respectively.

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

Step 3.1, after starting the real-time system, only one offline map file needs to be loaded. 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 an n _r row and n _c column bird's-eye view descriptor and adjust it to a feature vector of n _r × n _c rows and 1 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 calculation formula of the evaluation function L is as follows:

Where ^fs and ^fm are real-time and map descriptor vectors, respectively. n(1,0) and n(1,1) represent the number of elements in ^fs and ^fm that are (1,0) and (0,1), respectively.

Step 3.3, based on the best matching vector The index and map reference point _{Mr are} used to estimate the current vehicle position. The position is calculated 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, and id _best is the index of the best matching map description vector.

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

Step 4.1 For the second frame of the laser radar point cloud in the trajectory, use the position estimation of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N _k points. According to the map descriptor set index, construct the local descriptor set _FL :
F _L ={ _fi ,i=[j· _nf · _nθ ,(j+1)· _nf · _nθ ],pj∈P} ₍ 7)

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

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

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

Step 5.1: For the real-time LiDAR point cloud after trajectory initialization, use the position estimation of the first two frames and the uniform motion model of the vehicle 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.

Step 5.2, using the position estimate of the previous frame, a local search is performed in the map reference point to find the best matching descriptor in the neighborhood descriptor set, and the map reference point with similar neighborhood is calculated as the observation model:

Wherein H(·) is the observation model, and the present invention uses local search as the observation model. is the vehicle position estimated using local search and _ωs is the observation error. is the evaluation function result in formula (5) calculated using the real-time descriptor ^fs and the map descriptor ^fm ; in each calculation, the variance _σs of the position estimate is calculated using the evaluation function result and the sampling distances _dx , _dy .

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 present invention can quickly calculate the vehicle position through the above formula.

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 to be a loop, otherwise it is not a loop, thereby achieving position recognition.

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

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

Furthermore, in step 4.1, the number of nearest neighbors searched in the map reference point is 20.

The present invention has the following positive effects:

1) The present invention solves a key problem in the field of autonomous driving and artificial intelligence, namely, position recognition. The present invention proposes a novel position recognition method that integrates point cloud maps, motion models, and local features, calculates the vehicle position within the global map, and effectively solves the problems of perceptual confusion and high recognition error rate in position recognition or loop detection.

2) This invention is different from the general Kalman filter data fusion algorithm. It uses the local search feature descriptor as the observation model, which creates a new idea for the Kalman filter algorithm. At the same time, it only uses the laser radar as a single measurement sensor, which significantly reduces the cost of the unmanned driving system and improves the applicability of the algorithm.

3) The present invention combines the map with local features, significantly reducing the search space and The algorithm can still achieve superior operating efficiency with low computing resources. The motion model is added to the algorithm to effectively overcome the impact of target occlusion or dynamic targets, greatly improving the system's fault tolerance. At the same time, the position recognition performance will not degrade with the increase of trajectory or running time, which improves the safety performance of the unmanned driving system in dynamic cities.

The present invention can effectively realize the position recognition of autonomous driving vehicles in a large-scale, dynamic urban environment. 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 is proposed to use local feature search as an observation model without the help of other sensor measurements, which provides a new idea for the Kalman filter algorithm. The invention solves the problem of position recognition in a large-scale scene, and adds the motion model to the position calculation, which can solve the effects of perception confusion, target occlusion, etc. At the same time, fast 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, robots, etc.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a flow chart of the present invention.

FIG. 2 is a schematic diagram of global map reference points in the present invention.

FIG. 3 is a schematic diagram of the first frame position estimation of the trajectory in the present invention.

FIG. 4 is a schematic diagram of the second frame position estimation of the trajectory in the present invention.

FIG. 5 is a schematic diagram of a Kalman filter algorithm taking into account motion model and local search in the present invention.

FIG. 6 is a schematic diagram of the calculation result of the revisited position matching in the present invention.

Detailed ways

The scheme of the present invention is further described in detail below in conjunction with the accompanying drawings.

As shown in FIG1 , a location recognition method integrating a point cloud map, a motion model and local features mainly includes the following steps:

First, the laser radar point cloud data is collected in advance around the autonomous driving scene, and the vehicle’s posture information is calculated using high-precision integrated navigation. The relative position 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.

Then, measure the height of the sensor from the ground, set two height thresholds z ₁ and z ₂ , and retain the point cloud within the threshold range. Use a voxel grid with a side length of 0.1m to downsample the map and project it into the horizontal plane. In the experiment, the coordinate system of the lidar 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. If it is installed in other directions, just adjust the threshold accordingly.

Finally, the coordinate extremes of the map projection point cloud are calculated, the sampling distance intervals _dx and _dy in the x and y directions are set to 1m, the map reference points are generated, and CloudCompare is used to crop some erroneous points.

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

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

Then, at each reference point and each rotation angle, a ring with 40 rows and 60 columns is generated. Descriptor, the maximum distance d _max is 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) shows a schematic diagram of the feature descriptor of the bird's-eye view.

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

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

Step 2.2, for the map reference point p _k (x _k ,y _k )∈M _r , p _k represents the kth point in the reference point _Mr , x _k ,y _k represent the horizontal and vertical coordinates of p _k , where k is the map point index. Generate a circular feature descriptor in the horizontal plane (bird's eye view) with n _r rows and n _c columns under the rotation angle θ _i , simulating the data collected when the vehicle is facing θ _i at this point. Convert the descriptor into a vector f with n _r × n _c rows and 1 column, and concatenate all descriptor vectors into a map feature matrix F = [f ₁ ,f ₂ ,....,f _k ]. The specific calculation method of the feature descriptor is as follows:

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, and n _f = n _r · n _c is the number of descriptor elements. mod and rem They represent modulo and remainder operations respectively.

Step 3, as shown in Figure 3, is a schematic diagram of the position calculation of the first frame of trajectory data. 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.

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

Then, the hit rate of the real-time descriptor in the map descriptor is calculated as the evaluation function, and the map descriptor with the best evaluation function is calculated by brute force search in all map descriptors.

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

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

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 ^fs and ^fm are the real-time and map descriptor vectors, respectively. n(1,0) and n(1,1) represent the number of elements in ^fs and ^fm that are (1,0) and (0,1), respectively.

Step 4, as shown in Figure 4, is a schematic diagram of the position calculation of the second frame data. For the second frame point cloud of the trajectory, with the help of the first frame position estimation, a local search is used in the map description set to calculate the best matching map. Graph descriptor, estimates the vehicle position based on map reference points.

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 according to the map descriptor index.

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.

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

Step 4.1: for the second frame of the laser radar point cloud in the trajectory, use the position estimation of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N _k points. According to the map descriptor set index, construct the local descriptor set _FL :
F _L ={ _fi ,i=[j· _nf · _nθ ,(j+1)· _nf · _nθ ],pj∈P} ₍ 7)

Step 5, as shown in Figure 5, is a schematic diagram of the position calculation of real-time data. 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. The historical trajectory is further compared to determine whether there is a loop.

First, for the real-time frame lidar point cloud, the position estimation of the first two frames is used to predict the current vehicle position using a uniform motion model. In the invention, it is assumed that the position estimation error will not exceed the sampling distance of the map reference point, and the position estimation error is set to 1m.

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.

Finally, the similarity of feature descriptors is used as a measure of position deviation, and the Kalman filter algorithm is used to estimate the current position, and the historical trajectory is compared 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.

In this embodiment, the specific implementation of step 5 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, and V represents the variance matrix. ω _x is the position estimation error, is the variance of motion prediction.

Wherein H(·) is the observation model, and the present invention uses local search as the observation model. is the vehicle position estimated using local search and _ωs is the observation error. is the evaluation function result in formula (5) calculated using the real-time descriptor ^fs and the map descriptor ^fm ; in each calculation, the variance _σs of the position estimate is calculated using the evaluation function result and the sampling distances _dx , _dy ;

Step 5.4, compare the historical trajectory of the vehicle to determine whether there is a loop. Figure 6 is a schematic diagram of the revisit position matching effect in three dynamic environments. The horizontal black line is the trajectory of the vehicle, the vertical gray line is the revisit position matching, and the vertical black line is the wrong matching result. Three typical outdoor dynamic scenes are selected as experimental scenes: city (DCC 02, #1), university (KAIST 02, #2) and rural (KITTI 05, #3). The poses are all calculated by high-precision combined navigation. Only the lidar point cloud is used to solve the position information in the experiment. The three scenes collect 3151 frames (#1), 4190 frames (#2) and 2761 frames (#3) of point clouds respectively. In the outdoor environment, the distance is set to 8 meters for loop matching. Each data sequence contains a certain amount of revisited positions. In order to unify the threshold, the same The position error within 8m is considered to be the correct position calculation result. The experimental results are shown in Table 1. In a large-scale dynamic environment, the success rate of position calculation can reach more than 99%, and the average error is about 0.5m, which has good position accuracy. The lightweight descriptor creation takes about 6ms, and the real-time position solution speed can reach 200FPS. In terms of revisited position recognition, the maximum F1 score can reach more than 0.99, which has good position recognition accuracy. This method can provide an efficient and accurate position recognition method for autonomous driving and improve the overall stability of the system.

Table 1 Test performance of location recognition in different scenarios

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 protection scope of the present invention.

Claims

A location recognition method integrating a point cloud map, a motion model and local features, characterized in that it comprises the following steps:

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;

Step 2: traverse the map reference points, combine 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;

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;

Step 4: for the point cloud of the second frame of the trajectory, with the help of the starting position estimation, the best matching map descriptor is calculated by local search in the map description set, and the vehicle position is estimated based on the map reference point;

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.
The location recognition method integrating point cloud map, motion model and local features according to claim 1 is characterized in that: the specific implementation of step 1 includes the following sub-steps;

Step 1.1: Use an autonomous driving vehicle equipped with a multi-line laser radar to collect data in advance inside the scene, use the combined navigation of the global positioning system (GPS) and the inertial navigation system (INS) to calculate the high-precision pose and stitch the 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, downsample the map using voxels with a side length of 1 m , and project it into the XOY plane; the coordinate system of the laser radar is oriented in the direction of travel with the X axis pointing to the left and the Z axis pointing vertically upward.

Step 1.3, calculate the two-dimensional coordinate extreme values (x min , y min , x max , y max ) of the projection map, generate reference points Mr in the map as virtual landmarks according to equidistant sampling, and use point cloud processing software to crop some unreasonable reference points. The coordinates of the reference points are calculated as follows:

Where i and j are the row and column numbers of the sampling points, dx and dy are the sampling distances, and (x, y) are the coordinates of the sampling points.
The location recognition method integrating point cloud map, motion model and local features according to claim 1 is characterized in that: the specific implementation of step 2 includes the following sub-steps;

Step 2.1, construct a KD tree tree c in the map reference point, traverse all map reference points, and at each point: take 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∈N + ,θ∈(0,2π] (2)

Where θ' is the angle resolution, i is the orientation angle number, and N + represents a positive integer;

Step 2.2, for the map reference point pk ( xk , yk ) ∈Mr , pk represents the kth point in the reference point Mr , xk , yk represents the horizontal and vertical coordinates of pk , where k is the map point index, generate a circular feature descriptor in the horizontal plane with nr rows and nc columns under the rotation angle θi , simulate the data collected when the vehicle is oriented at this point with θi , convert the descriptor into a vector f with nr × nc rows and 1 column, and concatenate all descriptor vectors into a map feature matrix F=[ f1 , f2 ,..., fk ]. 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, 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:

where id m , id θ and id f are the map reference point, orientation angle and descriptor vector element indices respectively, s is the offline recorded data, 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.
According to the method for position recognition integrating point cloud map, motion model and local features of claim 1, it is characterized in that: the specific implementation of step 3 is as follows;

Step 3.1, after starting the real-time system, only need to load the offline map file once, perform height filtering on the first frame of lidar point cloud, where the height retention range is consistent with the map value range, generate n r rows and n c columns of bird's-eye view descriptor, and adjust it to n r × n c rows and 1 column feature vector;

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

Where fs and fm are real-time and map descriptor vectors, respectively, n(1,0) and n(1,1) represent the number of elements with (1,0) and (0,1) in fs and fm , respectively;

Step 3.3, based on the best matching vector The index and map reference point Mr are used to estimate the current vehicle position. The position is calculated 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, and id best is the index of the best matching map description vector.
According to the method for position recognition integrating point cloud map, motion model and local features of claim 3, it is characterized in that: the specific implementation of step 4 is as follows;

Step 4.1: for the second frame of the laser radar point cloud in the trajectory, use the position estimation of the first frame to search for the nearest neighbor point cloud P in the map reference point KD tree, which contains N k points. According to the map descriptor set index, construct the local descriptor set FL :
F L ={ fi ,i=[j· nf · nθ ,(j+1)· nf · nθ ],pj∈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, in the local descriptor set, a brute force search is used to calculate the map descriptor that best matches the real-time descriptor, and the position and orientation of the current vehicle are calculated based on the descriptor index.
According to the method for position recognition integrating point cloud map, motion model and local features of claim 4, it is characterized in that: the specific implementation of step 5 is as follows;

Step 5.1: For the real-time lidar point cloud after trajectory initialization, the position estimation of the first two frames is used to predict the vehicle's motion position using the vehicle's uniform motion model, where 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;

Step 5.2, using the position estimate of the previous frame, a local search is performed in the map reference point to find the best matching descriptor in the neighborhood descriptor set, and the map reference point with similar neighborhood is calculated 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; is the evaluation function result in formula (5) calculated using the real-time descriptor fs and the map descriptor fm ; in each calculation, the variance σs of the position estimate is calculated using the evaluation function result and the sampling distances dx , dy ;

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.
According to the location recognition method of integrating point cloud maps, motion models and local features as described in claim 1, it is characterized in that: step 5 also includes comparing historical trajectories to determine whether there is a loop, if the distance difference with the historical position is less than a set threshold, it is considered to be a loop, otherwise it is not a loop, thereby achieving location recognition.
According to the location recognition method integrating point cloud map, motion model and local features as described in claim 3, it is characterized in that: in step 2.1, the map rotation angle resolution is set to 3 degrees, that is, θ is 3.
According to the location recognition method that integrates point cloud maps, motion models and local features as described in claim 3, it is characterized in that the feature description size of the annular bird's-eye view in step 2.2 is set to 40 rows and 60 columns.
According to the location recognition method integrating point cloud map, motion model and local features as described in claim 5, it is characterized in that: in step 4.1, the number of nearest neighbors searched in the map reference point is 20.