Repositioning method based on laser radar

Abstract

According to the repositioning method based on the laser radar, repositioning is accurately carried out under the condition of not depending on an external environment reference object and a GPS signal by only using the laser radar configured by equipment, and the limitation in the aspects of using scenes, depending on GPS satellite signals and the like is expected to be overcome. Comprising the following steps: 1) mapping; step 2), repositioning; judging a state before last positioning; if the positioning is normal, executing 2.1. 1), and reading the positioning data; 2.1. 2), registering with a point cloud map and calculating a score; if the positioning is abnormal, executing the following steps: 2.2. 1) calculating the descriptor of the current frame of the laser; 2.2. 2) sequentially traversing the descriptors of the key frame and the descriptors of the current frame in the mapping to calculate a matching value; 2.2. 3) obtaining a pose corresponding to the candidate frame serial number from a pose file of mapping; (2.2. 4) calculating poses in tree nodes by using a 64-way tree traversal search method; (2.2. 5) the pose result data obtained through calculation is further verified through the gicp pose verification algorithm; and (3) positioning.

Description

Technical Field

[0001] The present invention relates to a laser radar-based repositioning method applicable to unmanned driving and industrial robot industries, and belongs to the field of artificial intelligence and logistics warehousing. Background Art

[0002] Positioning is an important part of mapping technology and has key applications in industries such as unmanned vehicles, industrial robots, and drones. For example, the AGV used in the logistics warehousing and express sorting industries can operate in an area where a laser point cloud map has been pre-established. When the AGV enters the area for the first time or is turned on again in this area, it needs to be repositioned. The role of repositioning technology is to calculate the accurate coordinate position and posture based on this point cloud map coordinate system, and provide the initial posture data required for navigation for its autonomous mobile operation.

[0003] For example, the prior Chinese patent application with publication number CN118452743A has strict requirements for environmental reference objects, and the relocation method proposed in this solution will fail in an environment without reference objects. In addition, the reference objects are pre-established in the map, and if the reference objects move, relocation based on these reference objects will fail.

[0004] Another example is the prior Chinese patent application with publication number CN118466576A, whose solution requires the participation of RTK. Not only is the equipment cost high, but the repositioning will fail or go wrong in tunnels, underground garages, or scenarios where high-rise buildings block satellite signals.

[0005] In summary, the existing relocation solutions are highly dependent on environmental and technical conditions, which leads to relocation failure or errors in some scenarios, and the applicability and accuracy are difficult to meet the existing usage requirements. In view of this, this patent application is specially filed. Summary of the invention

[0006] The laser radar-based repositioning method described in this application aims to solve the problems existing in the above-mentioned prior art and proposes to use only the laser radar configured by the device itself to accurately reposition without relying on external environmental reference objects and GPS signals, in order to overcome the limitations in usage scenarios and dependence on GPS satellite signals.

[0007] To achieve the above design objectives, the laser radar-based relocation method includes the following implementation steps:

[0008] Step 1), build a map;

[0009] Scanning with lasers in the operating area to build a point cloud map;

[0010] Step 2), relocation;

[0011] Based on the point cloud map constructed in step 1), determine the state before the last positioning;

[0012] If the positioning is normal, perform the following steps:

[0013] 2.1.1) Read positioning data;

[0014] 2.1.2) Align with the point cloud map and calculate the score;

[0015] The current frame uses the read pose and point cloud map to calculate the registration score, and uses the gicp pose verification algorithm to perform pose verification; if the score is sufficient, the re-positioning is successful, and if the score is insufficient, the following positioning exception process is executed;

[0016] If the positioning is abnormal, the following descriptor relocation process is executed:

[0017] 2.2.1) Calculate the descriptor of the current laser frame;

[0018] 2.2.2) traverse the descriptors of the key frames in the graph construction and the descriptors of the current frame in turn to calculate the matching values. The matching value result is the matching degree between the corresponding key frame and the current frame. The one with the largest matching degree is the calculated candidate frame.

[0019] 2.2.3) Obtain the pose corresponding to the candidate frame number from the pose file of the map;

[0020] 2.2.4) Taking the above corresponding posture as the center point, use the 64-tree traversal search method to calculate the posture in the tree node, and use the searched posture to calculate the Euclidean distance between each point in the current frame point cloud and the map point. The node posture with the smallest average Euclidean distance is the posture data with the highest matching degree;

[0021] 2.2.5) The calculated pose result data is further verified using the aforementioned gicp pose verification algorithm. If the verification passes, the next step of positioning is performed. If the verification fails, the next frame of laser data in the lidar sensor is obtained and step 2) is recalculated;

[0022] Step 3), positioning;

[0023] Using the posture calculated by repositioning in step 2) as the initial posture, continuously receiving laser data from the laser radar sensor to perform continuous posture calculation, and judging the calculated posture state;

[0024] 3.1) Continuous pose calculation;

[0025] Construct a kd-tree for the point cloud map, take the latest laser data received from the laser radar as the latest current frame, search for the five nearest points in the map kd-tree for each point in the current frame, perform plane fitting on the five points found, calculate the distance between the point and the fitted plane, take the distance between all points in the current frame point cloud and the plane as the optimization item, and use the rotation matrix R and displacement t as the optimization variables. Use the Gauss-Newton optimization algorithm to calculate the optimal rotation matrix and displacement results.

[0026] 3.2) Determine the positioning status based on the pose calculation results;

[0027] Use the current frame and the point cloud data matched to the map to calculate differential entropy, joint entropy and separation entropy, and determine the difference between the joint entropy and the separation entropy. If the difference is within the threshold range, the positioning state is considered normal. If the difference is greater than the threshold, the positioning state is considered abnormal.

[0028] If the positioning status is normal, data is recorded to provide calculation data for restarting the device for repositioning; if the positioning status is abnormal, no data is recorded.

[0029] Furthermore, in the step 1), constructing the point cloud map includes saving a point cloud map file, a key frame point cloud file, a key frame description sub-file, and a pose trajectory file corresponding to the key frame.

[0030] Furthermore, the process of calculating the registration score includes the following steps:

[0031] Get the current laser radar scanning data, and use the acquired pose to convert the current laser data into the map coordinate system;

[0032] Traverse each point in the current frame, find the point closest to this point in the map, calculate the Euclidean distance between the map point and this point, and if this distance is less than 0.15-0.25 meters, then this point is added to the valid point set;

[0033] Calculate the number of valid points. If the number of valid points is greater than (60-80%) of all points, proceed to the next step of posture verification; otherwise, directly execute the following positioning exception process.

[0034] Further, the gicp posture verification algorithm comprises the following steps:

[0035] Perform initial posture transformation; the transformation formula is q i =R*p i +t, where q i is the three-dimensional coordinate position of the source point in the target coordinate system after the rotation displacement calculation; p iis the coordinate position of the source point in the XYZ three-dimensional coordinate system, in meters; R is the rotation matrix from the source coordinate system to the target coordinate system; t is the displacement from the source coordinate system to the target coordinate system, in meters;

[0036] Calculate the point cloud covariance; select N adjacent points to calculate the point cloud covariance. The covariance formula is as follows:

[0037] Among them, p i is the coordinate position of the current point in the XYZ three-dimensional coordinate system, in meters, q j is the coordinate position of the neighborhood point in the XYZ three-dimensional coordinate system, in meters;

[0038] Construct a kd-tree for the point cloud in the target coordinate system, perform a distance search for each point p in the current frame in the kd-tree, and the point q with the smallest distance is used as the search result, forming a point cloud pairing of the source point p and the target point q;

[0039] Calculate the Euclidean distance between the source point p and the target point q as the error value;

[0040] The error value calculation formula is: d(p i ,q i )=(p i ,q i ) T (∑ p +∑ q ) -1 (p i -q i ), where ∑ p ,∑ q are the covariance matrices of the source point p and the target point q respectively;

[0041] The distance error calculated from all points is used as the optimization item, the rotation matrix R and the displacement t are used as the optimization variables, and the Gauss-Newton optimization algorithm is used to calculate the optimal rotation matrix and displacement results.

[0042] Furthermore, the step 2.2.1) includes the following process:

[0043] Taking the laser radar origin as the center, divide the surrounding area into 20 to 30 equal parts radially and 60 to 80 equal parts circumferentially in polar coordinate form, and set the maximum distance Lmax of the laser radar point cloud to 80 to 100 meters;

[0044] Calculate the radial circle i∈{1,2,...,N r} and circumferential circle j∈{1,2,...,N s}The lidar point cloud set ρ ij ∈φ{ρ ij}'s maximum height;

[0045] According to ij Generated N r ×N s The matrix is ​​the current frame descriptor.

[0046] Furthermore, in step 2.2.2), the formula for calculating the matching value is as follows:

[0047]

[0048] Among them, I q and I c are the descriptor matrices of the key frame and the current frame respectively, N is the number of columns of the descriptor matrix, and They are the corresponding column vectors in the key frame and current frame description submatrices respectively.

[0049] Furthermore, the 64-ary tree traversal search comprises the following steps:

[0050] Take the selected posture (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three-dimensional coordinate data of the posture, respectively, in meters; Roll, Pitch, and Yaw are the roll angle, pitch angle, and yaw angle of the posture, respectively, in degrees;

[0051] Define the area (Xx, Yy, Zz, Roll-r, Pitch-p, Yaw-y)(X+x, Y+y, Z+z, Roll+r, Pitch+p, Yaw+y) as the search window; perform binary division on the area within the search window according to the three positions X, Y, Z and the three rotations Roll, Pitch, and Yaw, a total of 6 dimensions, to obtain a 64-way tree with the center point pose data as the root node; the minimum distance of each leaf node of the 64-way tree in the X, Y, and Z axis directions is 0.025 meters respectively, and the rotation angles of roll, pitch, and yaw are 0.2 degrees respectively;

[0052] Perform a breadth-first traversal of the 64-tree in the previous step, and use the pose data on each node to calculate the degree of match between the current frame and the map. The algorithm used can choose to execute the above-mentioned algorithm for calculating the registration score;

[0053] Combine the calculated scores of the parent node and the current node; if the parent node score is greater than the current node score, this node and its associated nodes will not continue to be deeply traversed; otherwise, this node will continue to be calculated.

[0054] Furthermore, the step 3.2) includes the following execution steps:

[0055] 3.2.1) The current frame point cloud is Pa , for each point p in the point cloud of the current frame k , find the closest point in the map, and all the closest points constitute the matching frame point cloud P b ;

[0056] 3.2.2) For the current frame point cloud P a and matching frame point cloud P b The differential entropy is calculated for each point in the equation as follows:

[0057] Among them, p k is the 3D coordinate of the point in the point cloud of the current frame and the matching frame, ∑p k For point p k The covariance matrix calculated for all points within radius r, det is the determinant of the covariance matrix,

[0058] 3.2.3) Use the differential entropy of each point calculated in step 3.2.2) to calculate the differential entropy of the point cloud of the current frame and the matching frame; the calculation formulas are,

[0059]

[0060] Among them, H a (P a ) is the differential entropy of the point cloud of the current frame, H b (P b ) is the differential entropy of the matching frame point cloud, |P a | is the number of points in the point cloud of the current frame, |P b | is the number of points in the matching frame point cloud;

[0061] 3.2.4) Use the point cloud differential entropy calculated in step 3.2.3) to calculate the separation entropy of the current frame and the matching frame. The calculation formula is:

[0062] Among them, H seq is the calculated separation entropy;

[0063] 3.2.5) Calculate the joint entropy of the current frame and the matching frame using the calculation formula in step 3.2.2) and step 3.2.3); the calculation formula is

[0064] Among them, H joint is the calculated joint entropy, H(P a ∪P b ) is the point cloud differential entropy calculated by taking the union of the point clouds of the current frame and the matching frame.

[0065] In summary, the laser radar-based repositioning method proposed in this application is not limited by external environmental reference conditions such as the site and GPS satellite signals. It can accurately position and calculate the mobile device only through laser radar. It has obvious scene-wide performance and strong applicability, and has high positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] The following specifically describes the implementation scheme in conjunction with the accompanying drawings;

[0067] Figure 1 It is a flow chart of the process of map building and information preservation;

[0068] Figure 2 It is a schematic diagram of the relocation process;

[0069] Figure 3 It is the flow chart of the gicp pose verification algorithm;

[0070] Figure 4 It is a schematic diagram for traversing and searching a 64-way tree; DETAILED DESCRIPTION

[0071] Embodiment 1, as Figures 1 to 3 As shown, the present application proposes a laser radar-based repositioning method, which is used for repositioning when the AGV enters for the first time or is turned on again, and includes the following implementation steps:

[0072] Step 1), build a map;

[0073] like Figure 1 As shown, a laser is used to scan the operating area to build a point cloud map;

[0074] Among them, constructing the point cloud map includes saving the point cloud map file, the key frame point cloud file, the key frame description subfile and the pose trajectory file corresponding to the key frame;

[0075] Step 2), relocation;

[0076] Based on the point cloud map constructed in step 1), determine the state before the last positioning;

[0077] If the positioning is normal, perform the following steps:

[0078] 2.1.1) Read positioning data;

[0079] 2.1.2) Align with the point cloud map and calculate the score;

[0080] The current frame uses the read pose and point cloud map to calculate the registration score, and uses the gicp pose verification algorithm to perform pose verification; if the score is sufficient, the re-positioning is successful, and if the score is insufficient, the following positioning exception process is executed;

[0081] The process of calculating the registration score includes the following steps:

[0082] Get the current laser radar scanning data, and use the acquired pose to convert the current laser data into the map coordinate system;

[0083] Traverse each point in the current frame, find the point closest to this point in the map, calculate the Euclidean distance between the map point and this point, if this distance is less than 0.2m (0.15-0.25), then this point is added to the valid point set;

[0084] Calculate the number of valid points. If the number of valid points is greater than 70% (60-80%) of all points, proceed to the next step of posture verification; otherwise, directly execute the following positioning abnormality process;

[0085] The gicp posture verification algorithm comprises the following steps:

[0086] Perform initial pose transformation;

[0087] The transformation formula is q i =R*p i +t, where q i is the three-dimensional coordinate position of the source point in the target coordinate system after the rotation displacement calculation; p i is the coordinate position of the source point in the XYZ three-dimensional coordinate system, in meters; R is the rotation matrix from the source coordinate system to the target coordinate system; t is the displacement from the source coordinate system to the target coordinate system, in meters;

[0088] Calculate point cloud covariance;

[0089] Select N adjacent points to calculate the point cloud covariance. The covariance formula is as follows:

[0090]

[0091] Among them, p i is the coordinate position of the current point in the XYZ three-dimensional coordinate system, in meters, q j is the coordinate position of the neighborhood point in the XYZ three-dimensional coordinate system, in meters;

[0092] Construct a kd-tree for the point cloud in the target coordinate system, perform a distance search for each point p in the current frame in the kd-tree, and the point q with the smallest distance is used as the search result, forming a point cloud pairing of the source point p and the target point q;

[0093] Calculate the Euclidean distance between the source point p and the target point q as the error value;

[0094] The error value calculation formula is: d(p i ,q i )=(pi ,q i ) T (∑ p +∑ q ) -1 (p i -q i );

[0095] Among them, ∑ p ,∑ q are the covariance matrices of the source point p and the target point q respectively;

[0096] The distance error calculated from all points is used as the optimization item, the rotation matrix R and the displacement t are used as the optimization variables, and the Gauss-Newton optimization algorithm is used to calculate the optimal rotation matrix and displacement results;

[0097] If the positioning is abnormal, the following descriptor relocation process is performed:

[0098] 2.2.1) Calculate the descriptor of the current laser frame;

[0099] The descriptor calculation process is as follows:

[0100] Taking the laser radar origin as the center, the surrounding area is divided into N radial directions in polar coordinates. r =20 (20-30) equally divided, circumferential N s = 60 (60-80) and set the maximum distance L of the laser radar point cloud max =80 (80-100) meters;

[0101] Calculate the radial circle i∈{1,2,...,N r} and circumferential circle j∈{1,2,...,N s}The lidar point cloud set ρ ij ∈φ{ρ ij}'s maximum height;

[0102] According to ij Generated N r ×N s The matrix is ​​the current frame descriptor;

[0103] 2.2.2) traverse the descriptors of the key frames in the graph construction and the descriptors of the current frame in turn to calculate the matching values. The matching value result is the matching degree between the corresponding key frame and the current frame. The one with the largest matching degree is the calculated candidate frame.

[0104] The formula for calculating the matching value is as follows:

[0105]

[0106] Among them, I q and Ic are the descriptor matrices of the key frame and the current frame respectively, N is the number of columns of the descriptor matrix, and They are the corresponding column vectors in the key frame and current frame description submatrices respectively;

[0107] 2.2.3) Obtain the pose corresponding to the candidate frame number from the pose file of the map;

[0108] 2.2.4) Taking the above corresponding posture as the center point, use the 64-tree traversal search method to calculate the posture in the tree node, and use the searched posture to calculate the Euclidean distance between each point in the current frame point cloud and the map point. The node posture with the smallest average Euclidean distance is the posture data with the highest matching degree;

[0109] like Figure 4 As shown, the 64-way tree traversal search includes the following steps:

[0110] Take the selected posture (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three-dimensional coordinate data of the posture, respectively, in meters; Roll, Pitch, and Yaw are the roll angle, pitch angle, and yaw angle of the posture, respectively, in degrees;

[0111] Define the area (Xx, Yy, Zz, Roll-r, Pitch-p, Yaw-y)(X+x, Y+y, Z+z, Roll+r, Pitch+p, Yaw+y) as the search window; perform binary division on the area within the search window according to the three positions X, Y, Z and the three rotations Roll, Pitch, and Yaw, a total of 6 dimensions, to obtain a 64-way tree with the center point pose data as the root node; the minimum distance of each leaf node of the 64-way tree in the X, Y, and Z axis directions is 0.025 meters respectively, and the rotation angles of roll, pitch, and yaw are 0.2 degrees respectively;

[0112] Perform a breadth-first traversal of the 64-tree in the previous step, and use the pose data on each node to calculate the degree of match between the current frame and the map. The algorithm used can choose to execute the above-mentioned algorithm for calculating the registration score;

[0113] Combine the calculated scores of the parent node and the current node; if the parent node score is greater than the current node score, this node and its associated nodes will not continue to be deeply traversed; otherwise, this node will continue to be calculated;

[0114] 2.2.5) The calculated pose result data is further verified using the aforementioned gicp pose verification algorithm. If the verification passes, the next step of positioning is performed. If the verification fails, the next frame of laser data in the lidar sensor is taken and step 2) is recalculated.

[0115] Step 3), positioning;

[0116] Using the posture calculated by repositioning in step 2) as the initial posture, continuously receiving laser data from the laser radar sensor to perform continuous posture calculation, and judging the calculated posture state;

[0117] 3.1) Continuous pose calculation;

[0118] Construct a kd-tree for the point cloud map, take the latest laser data received from the laser radar as the latest current frame, search for the five nearest points in the map kd-tree for each point in the current frame, perform plane fitting on the five points found, calculate the distance between the point and the fitted plane, take the distance between all points in the current frame point cloud and the plane as the optimization item, and use the rotation matrix R and displacement t as the optimization variables. Use the Gauss-Newton optimization algorithm to calculate the optimal rotation matrix and displacement results.

[0119] 3.2) Determine the positioning status based on the pose calculation results;

[0120] The differential entropy, joint entropy and separation entropy are calculated using the current frame and the point cloud data matched to the map. The difference between the joint entropy and the separation entropy is determined. If the difference is within the threshold range, the positioning state is determined to be normal. If the difference is greater than the threshold, the positioning state is determined to be abnormal.

[0121] If the positioning status is normal, data is recorded to provide calculation data for restarting the device for repositioning; if the positioning status is abnormal, no data is recorded.

[0122] Specifically, the following execution steps are included:

[0123] 3.2.1) The current frame point cloud is P a , for each point p in the point cloud of the current frame k , find the closest point in the map, all the closest points constitute the matching frame point cloud p b ;

[0124] 3.2.2) For the current frame point cloud p a and matching frame point cloud p b The differential entropy is calculated for each point in the equation as follows:

[0125] Among them, p k is the 3D coordinate of the point in the point cloud of the current frame and the matching frame, ∑p k For point p k The covariance matrix calculated for all points within radius r, det is the determinant of the covariance matrix,

[0126] 3.2.3) Use the differential entropy of each point calculated in step 3.2.2) to calculate the differential entropy of the point cloud of the current frame and the matching frame; the calculation formulas are,

[0127]

[0128] Among them, H a (P a ) is the differential entropy of the point cloud of the current frame, H b (P b ) is the differential entropy of the matching frame point cloud, |P a | is the number of points in the point cloud of the current frame, |P b | is the number of points in the matching frame point cloud;

[0129] 3.2.4) Use the point cloud differential entropy calculated in step 3.2.3) to calculate the separation entropy of the current frame and the matching frame. The calculation formula is:

[0130] Among them, H seq is the calculated separation entropy;

[0131] 3.2.5) Calculate the joint entropy of the current frame and the matching frame using the calculation formula in step 3.2.2) and step 3.2.3); the calculation formula is

[0132] Among them, H joint is the calculated joint entropy, H(P a ∪P b ) is the point cloud differential entropy calculated by taking the union of the point clouds of the current frame and the matching frame.

[0133] The above contents and the embodiments given in the accompanying drawings are only preferred solutions for achieving the purpose of the present invention. Those skilled in the art can be inspired by this and directly deduce other alternative structures that conform to the design concept of the present invention. Other structural features obtained thereby should also belong to the scope of the solution described in the present invention.

Claims

1. A laser radar based relocation method, characterized in that: The following steps are included: Step 1), build a map; Scanning with lasers in the operating area to build a point cloud map; Step 2), relocation; Based on the point cloud map constructed in step 1), determine the state before the last positioning; If the positioning is normal, perform the following steps: 2.1.1) Read positioning data; 2.1.2) Align with the point cloud map and calculate the score; The current frame uses the read pose and point cloud map to calculate the registration score, and uses the gicp pose verification algorithm to perform pose verification; if the score is sufficient, the re-positioning is successful, and if the score is insufficient, the following positioning exception process is executed; If the positioning is abnormal, the following descriptor relocation process is executed: 2.2.1) Calculate the descriptor of the current laser frame; 2.2.2) traverse the descriptors of the key frames in the graph construction and the descriptors of the current frame in turn to calculate the matching values. The matching value result is the matching degree between the corresponding key frame and the current frame. The one with the largest matching degree is the calculated candidate frame. 2.2.3) Obtain the pose corresponding to the candidate frame number from the pose file of the map; 2.2.4) Taking the above corresponding posture as the center point, use the 64-tree traversal search method to calculate the posture in the tree node, and use the searched posture to calculate the Euclidean distance between each point in the current frame point cloud and the map point. The node posture with the smallest average Euclidean distance is the posture data with the highest matching degree; 2.2.5) The calculated pose result data is further verified using the aforementioned gicp pose verification algorithm. If the verification passes, the next step of positioning is performed. If the verification fails, the next frame of laser data in the lidar sensor is obtained and step 2) is recalculated; Step 3), positioning; Using the posture calculated by repositioning in step 2) as the initial posture, continuously receiving laser data from the laser radar sensor to perform continuous posture calculation, and judging the calculated posture state; 3.1) Continuous pose calculation; Construct a kd-tree for the point cloud map, take the latest laser data received from the laser radar as the latest current frame, search for the five nearest points in the map kd-tree for each point in the current frame, perform plane fitting on the five points found, calculate the distance between the point and the fitted plane, take the distance between all points in the current frame point cloud and the plane as the optimization item, and use the rotation matrix R and displacement t as the optimization variables. Use the Gauss-Newton optimization algorithm to calculate the optimal rotation matrix and displacement results. 3.2) Determine the positioning status based on the pose calculation results; Use the current frame and the point cloud data matched to the map to calculate differential entropy, joint entropy and separation entropy, and determine the difference between the joint entropy and the separation entropy. If the difference is within the threshold range, the positioning state is considered normal. If the difference is greater than the threshold, the positioning state is considered abnormal. If the positioning status is normal, data is recorded to provide calculation data for restarting the device for repositioning; if the positioning status is abnormal, no data is recorded.

2. The laser radar-based relocation method according to claim 1, characterized in that: The step 1) constructing a point cloud map includes saving a point cloud map file, a key frame point cloud file, a key frame description sub-file, and a pose trajectory file corresponding to the key frame.

3. The laser radar based relocation method according to claim 1, characterized in that: The process of calculating the registration score includes the following steps: Get the current laser radar scanning data, and use the acquired pose to convert the current laser data into the map coordinate system; Traverse each point in the current frame, find the point closest to this point in the map, calculate the Euclidean distance between the map point and this point, and if this distance is less than 0.15-0.25 meters, then this point is added to the valid point set; Calculate the number of valid points. If the number of valid points is greater than (60-80%) of all points, proceed to the next step of posture verification; otherwise, directly execute the following positioning exception process.

4. The laser radar based relocation method according to claim 1, characterized in that: The gicp posture verification algorithm comprises the following steps: Perform initial posture transformation; the transformation formula is q i =R*p i +t, where q i is the three-dimensional coordinate position of the source point in the target coordinate system after the rotation displacement calculation; p i is the coordinate position of the source point in the XYZ three-dimensional coordinate system, in meters; R is the rotation matrix from the source coordinate system to the target coordinate system; t is the displacement from the source coordinate system to the target coordinate system, in meters; Calculate the point cloud covariance; select N adjacent points to calculate the point cloud covariance. The covariance formula is as follows: Among them, p i is the coordinate position of the current point in the XYZ three-dimensional coordinate system, in meters, q j is the coordinate position of the neighborhood point in the XYZ three-dimensional coordinate system, in meters; Construct a kd-tree for the point cloud in the target coordinate system, perform a distance search for each point p in the current frame in the kd-tree, and the point q with the smallest distance is used as the search result, forming a point cloud pairing of the source point p and the target point q; Calculate the Euclidean distance between the source point p and the target point q as the error value; The error value calculation formula is: d(p i ,q i )=(p i ,q i ) T (∑ p +∑ q ) -1 (p i -q i ), where ∑ p ,∑ q are the covariance matrices of the source point p and the target point q respectively; The distance error calculated from all points is used as the optimization item, the rotation matrix R and the displacement t are used as the optimization variables, and the Gauss-Newton optimization algorithm is used to calculate the optimal rotation matrix and displacement results.

5. The laser radar based relocation method according to claim 1, characterized in that: The step 2.2.1) includes the following process: Taking the laser radar origin as the center, divide the surrounding area into 20 to 30 equal parts radially and 60 to 80 equal parts circumferentially in polar coordinate form, and set the maximum distance Lmax of the laser radar point cloud to 80 to 100 meters; Calculate the radial circle i∈{1,2,...,N r } and circumferential circle j∈{1,2,...,N s }The lidar point cloud set ρ ij ∈φ{ρ ij }'s maximum height; According to ρ ij Generated N r ×N s The matrix is ​​the current frame descriptor.

6. The laser radar based relocation method according to claim 1, characterized in that: In step 2.2.2), the formula for calculating the matching value is as follows: Among them, I q and I c are the descriptor matrices of the key frame and the current frame respectively, N is the number of columns of the descriptor matrix, and They are the corresponding column vectors in the key frame and current frame description submatrices respectively.

7. The laser radar based relocation method according to claim 1, characterized in that: The 64-way tree traversal search comprises the following steps: Take the selected posture (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three-dimensional coordinate data of the posture, respectively, in meters; Roll, Pitch, and Yaw are the roll angle, pitch angle, and yaw angle of the posture, respectively, in degrees; Will (Xx,Yy,Zz,Roll-r,Pitch-p,Yaw-y)(X+x,Y+y,Z+z,Roll+r,Pitch+p,Yaw+y) The area is defined as the search window; the area in the search window is divided into six dimensions, namely, three positions X, Y, Z and three rotations Roll, Pitch, and Yaw, by binary division, and a 64-way tree with the center point pose data as the root node is obtained; the minimum distance of each leaf node of the 64-way tree in the X, Y, and Z axis directions is 0.025 meters, and the rotation angles of roll, pitch, and yaw are 0.2 degrees respectively; Perform a breadth-first traversal of the 64-tree in the previous step, and use the pose data on each node to calculate the degree of match between the current frame and the map. The algorithm used can choose to execute the above-mentioned algorithm for calculating the registration score; Combine the calculated scores of the parent node and the current node; if the parent node score is greater than the current node score, this node and its associated nodes will not continue to be deeply traversed; otherwise, this node will continue to be calculated.

8. The laser radar based relocation method according to claim 1, characterized in that: The step 3.2) includes the following execution steps: 3.2.1) The current frame point cloud is P a , for each point p in the point cloud of the current frame k , find the closest point in the map, and all the closest points constitute the matching frame point cloud P b ; 3.2.2) For the current frame point cloud P a and matching frame point cloud P b The differential entropy is calculated for each point in the equation as follows: Among them, p k is the 3D coordinate of the point in the point cloud of the current frame and the matching frame, ∑p k For point p k The covariance matrix calculated for all points within radius r, det is the determinant of the covariance matrix, 3.2.3) Use the differential entropy of each point calculated in step 3.2.2) to calculate the differential entropy of the point cloud of the current frame and the matching frame; the calculation formulas are, Among them, H a (P a ) is the differential entropy of the point cloud of the current frame, H b (P b ) is the differential entropy of the matching frame point cloud, |P a | is the number of points in the point cloud of the current frame, |P b | is the number of points in the matching frame point cloud; 3.2.4) Use the point cloud differential entropy calculated in step 3.2.3) to calculate the separation entropy of the current frame and the matching frame. The calculation formula is: in, H seq is the calculated separation entropy; 3.2.5) Calculate the joint entropy of the current frame and the matching frame using the calculation formula in step 3.2.2) and step 3.2.3); the calculation formula is Among them, H joint is the calculated joint entropy, H(P a ∪P b ) is the point cloud differential entropy calculated by taking the union of the point clouds of the current frame and the matching frame.

Images