Lidar-based Relocalization Method

Abstract

This application proposes a relocalization method based on lidar, which proposes to accurately perform relocalization only using the lidar configured by the device itself, without relying on external environmental references and GPS signals, in order to overcome the limitations in terms of usage scenarios and reliance on GPS satellite signals. It includes step (1), mapping; step (2), relocalization; judging the state before the previous localization; if the localization is normal, then execute 2.1.1), reading the localization data; 2.1.2), registering with the point cloud map and calculating the score; if the localization is abnormal, then execute 2.2.1), calculating the descriptor of the current lidar frame; 2.2.2), sequentially traversing the descriptors of the key frames in mapping and calculating the matching value with the descriptor of the current frame; 2.2.3), obtaining the pose corresponding to the candidate frame number from the pose file in mapping; 2.2.4), using the method of 64-fork tree traversal search to calculate the pose in the tree node; 2.2.5), further verifying the obtained pose result data using the aforementioned gicp pose verification algorithm; step (3), localization.

Description

Technical Field

[0001] The present invention relates to a lidar-based relocalization method applicable to the fields of driverless and industrial robots, belonging to the fields of artificial intelligence and logistics warehousing. Background Art

[0002] Localization is an important part of mapping technology and has key applications in industries such as driverless vehicles, industrial robots, and drones. For example, the AGV used in the logistics warehousing and express sorting industries may operate in an area where a laser point cloud map has been pre-established. When the AGV enters for the first time or is powered on again in this area, relocalization is required. The role of the relocalization technology is to calculate the accurate coordinate position and attitude based on the coordinate system of this point cloud map, providing the initial pose data required for its autonomous movement and navigation.

[0003] For example, in the prior Chinese patent application with the publication number CN118452743A, the implementation of its solution has relatively strict requirements for environmental reference objects. In an environment without reference objects, the relocalization method proposed by this solution will fail. In addition, if the reference object is pre-established in the map and the reference object moves, relocalization relying on this reference object will fail.

[0004] Another example is the prior Chinese patent application with the publication number CN118466576A. Its solution requires the participation of RTK. Not only is the equipment cost relatively high, but also in scenarios such as tunnels, underground garages, or where satellite signals are blocked by high-rise buildings, relocalization will fail or go wrong.

[0005] In summary, the relocalization solutions of the prior art all have strong dependencies on the environment and technical conditions, resulting in relocalization failure or error in some scenarios, and it is difficult to meet the existing usage requirements in terms of applicability and accuracy. In view of this, this patent application is specifically proposed. Summary of the Invention

[0006] The lidar-based relocalization method described in this application aims to solve the problems existing in the above-mentioned prior art and proposes to accurately perform relocalization only using the lidar configured on the device itself without relying on external environmental reference objects and GPS signals, in order to overcome the limitations in terms of usage scenarios and dependence on GPS satellite signals.

[0007] To achieve the above design purpose, the lidar-based relocalization method includes the following implementation steps:

[0008] Step 1): Map building;

[0009] Use a laser to scan in the operating area to construct a point cloud map;

[0010] Step 2): Relocalization;

[0011] Based on the point cloud map constructed in step (1), judge the state before the previous positioning;

[0012] If the positioning is normal, execute the following steps,

[0013] 2.1.1), Read the positioning data;

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

[0015] For the current frame, calculate the registration score using the pose read and the point cloud map, and use the gicp pose verification algorithm to verify the pose; if the score is sufficient, the repositioning is successful, and if the score is insufficient, execute according to the following positioning abnormal process;

[0016] If the positioning is abnormal, execute the following descriptor repositioning process,

[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 mapping and the descriptors of the current frame in sequence to calculate the matching value. The matching value result is the matching degree between the corresponding key frame and the current frame, and the key frame with the largest matching degree is the calculated candidate frame;

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

[0020] 2.2.4), Using the above corresponding pose as the center point, use the method of 64 - fork tree traversal search to calculate the poses in the tree nodes, calculate the Euclidean distance between each point in the current frame point cloud and the map points using the searched poses, and the pose of the node with the smallest average Euclidean distance is the pose 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, proceed to the next step of positioning. If the verification fails, take the next frame of laser data in the lidar sensor and recalculate step (2);

[0022] Step (3), Positioning;

[0023] Use the pose calculated by the repositioning in step (2) as the initial pose, continuously receive laser data from the lidar sensor for continuous pose calculation, and judge the pose state calculated;

[0024] 3.1), Continuous pose calculation;

[0025] Build a kd-tree for the point cloud map. Use the latest laser data received from the lidar as the latest current frame. For each point in the current frame, find the 5 nearest points in the map kd-tree. Perform plane fitting on the 5 found points, calculate the distance from the point to the fitted plane, and use the distances from all points in the current frame point cloud to the plane as the optimization term. Use the rotation matrix R and displacement t as optimization variables, and use the Gauss-Newton optimization algorithm to calculate the results of the optimal rotation matrix and displacement.

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

[0027] Calculate the differential entropy, joint entropy, and separation entropy using the current frame and the point cloud data matched to the map. Judge the difference between the joint entropy and the separation entropy. If the difference is within the threshold range, judge that the positioning status is normal. If the difference is greater than the threshold, judge that the positioning status is abnormal;

[0028] If the positioning status is normal, record the data to provide calculation data for re-positioning when the device is restarted again; if the positioning status is abnormal, do not record the data.

[0029] Further, in step 1), building the point cloud map includes saving the point cloud map file, key frame point cloud file, key frame descriptor file, and the pose trajectory file corresponding to the key frame.

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

[0031] Obtain the current lidar scan data, and transform the current laser data to the map coordinate system using the obtained pose;

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

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

[0034] Further, the gicp pose verification algorithm includes the following steps,

[0035] Perform the initial pose 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 rotation and displacement calculation; p iis the coordinate position of the source point in the XYZ three-dimensional coordinate system, with the unit of meter; 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, with the unit of meter;

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

[0037] where p i is the coordinate position of the current point in the XYZ three-dimensional coordinate system, with the unit of meter, and q j is the coordinate position of the neighborhood point in the XYZ three-dimensional coordinate system, with the unit of meter;

[0038] Construct a kd-tree for the point cloud in the target coordinate system. For each point p in the current frame, perform a distance search in the kd-tree. The point q with the smallest searched 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 and ∑ q are the covariance matrices of the source point p and the target point q respectively;

[0041] Take the distance errors calculated for all points as the optimization term, with the rotation matrix R and the displacement t as the optimization variables, and use the Gauss-Newton optimization algorithm to calculate and obtain the results of the optimal rotation matrix and displacement.

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

[0043] With the origin of the lidar as the center, radially divide the surrounding area into 20 to 30 equal parts in polar coordinates and circumferentially divide it into 60 to 80 equal parts. Set the maximum distance Lmax of the lidar point cloud to 80 to 100 meters;

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

[0045] According to ρ ij The generated N r ×N s The matrix is the current frame descriptor; where N r Is the number of radial equal divisions of the lidar point cloud, and N s Is the number of circumferential equal divisions of the lidar point cloud.

[0046] Furthermore, for the 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 Are the corresponding column vectors in the key frame and current frame descriptor matrices respectively, and N s Is the number of circumferential equal divisions of the lidar point cloud.

[0049] Furthermore, the 64 - tree traversal search includes the following steps.

[0050] Taking the selected pose (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three - dimensional coordinate data of the pose, with the unit of meter; Roll, Pitch, and Yaw are the roll angle, pitch angle, and yaw angle of the pose respectively, with the unit of degree; the area of (X - x, Y - y, Z - z, Roll - r,, Pitch - p, Yaw - ya)(X + x, Y + y, Z + z, Roll + r,, Pitch + p, Yaw + ya) is delimited as the search window.

[0051] Dividing the area within the search window by the dichotomy method in six dimensions of three positions X, Y, Z and three rotations Roll, Pitch, Yaw, a 64 - tree with the pose data of the center point as the root node can be obtained; the minimum distances of each leaf node of the 64 - tree in the X, Y, and Z axis directions are 0.025 meters respectively, and the rotation angles of Roll, Pitch, and Yaw are 0.2 degrees respectively.

[0052] Performing a breadth - first traversal on the 64 - tree obtained in the previous step, calculating the matching degree between the current frame and the map using the pose data on each node, and the algorithm used can select to execute the above - mentioned algorithm for calculating the registration score.

[0053] Calculate the combined score of the parent node and the current node; if the score of the parent node is greater than that of the current node, this node and its associated nodes will not continue the depth traversal; otherwise, this node will continue the calculation.

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

[0055] 3.2.1), the current frame point cloud is P a , for each point p in the current frame point cloud k , find the nearest point in the map, and all the nearest points form the matching frame point cloud P b ;

[0056] 3.2.2), calculate the differential entropy for each point in the current frame point cloud P a and the matching frame point cloud P b . The calculation formula is as follows:

[0057] where p k is the three-dimensional coordinate of the point in the current frame and the matching frame point cloud, ∑p k is the covariance matrix calculated from all points within the radius r of point p k , and det is the determinant of the covariance matrix.

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

[0059]

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

[0061] 3.2.4), calculate the separation entropy between the current frame and the matching frame using the point cloud differential entropy calculated in step 3.2.3). The calculation formula is

[0062] where H seq is the calculated separation entropy;

[0063] 3.2.5), calculate the joint entropy between the current frame and the matching frame using the calculation formulas in steps 3.2.2) and 3.2.3). The calculation formula is

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

[0065] In summary, the lidar-based relocalization method proposed in this application is not limited by external environmental reference conditions such as the site and GPS satellite signals. The accurate positioning calculation of the mobile device can be achieved only through the lidar, which has obvious scene generality and strong applicability, and the positioning accuracy is relatively high. Brief Description of the Drawings

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

[0067] Figure 1 is the flowchart of the mapping and information saving process;

[0068] Figure 2 is the schematic diagram of the relocalization process;

[0069] Figure 3 is the flowchart of the gicp pose verification algorithm;

[0070] Figure 4 is the schematic diagram for traversing and searching the octree; Detailed Description of the Invention

[0071] Embodiment 1, as Figures 1 to 3 shown, this application proposes a lidar-based relocalization method, which is used for relocalization when the AGV enters for the first time or powers on again, and includes the following implementation steps:

[0072] Step 1): Mapping;

[0073] As Figure 1 shown, use a laser to scan in the operating area to construct 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 descriptor file, and the pose trajectory file corresponding to the key frame;

[0075] Step 2): Relocalization;

[0076] Based on the point cloud map constructed in Step 1), judge the state before the previous positioning;

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

[0078] 2.1.1): Read the positioning data;

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

[0080] The registration score is calculated for the current frame using the read pose and the point cloud map, and the pose is verified using the GICP pose verification algorithm; if the score is sufficient, the repositioning 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] Obtain the current lidar scan data and transform the current lidar data to the map coordinate system using the obtained pose;

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

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

[0085] The GICP pose verification algorithm includes the following steps:

[0086] Perform the 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 rotation and 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 the point cloud covariance;

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

[0090]

[0091] where p i is the coordinate position of the current point in the XYZ three-dimensional coordinate system, in meters, and 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 found is used as the search result, forming the 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 )=(p i ,q i ) T (∑ p +∑ q ) -1 (p i -q i );

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

[0096] Take the distance errors calculated for all points as the optimization term, the rotation matrix R and the displacement t as the optimization variables, and use the Gauss-Newton optimization algorithm to calculate and obtain the results of the optimal rotation matrix and displacement;

[0097] If the positioning is abnormal, execute the following descriptor repositioning process:

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

[0099] The descriptor calculation process is as follows:

[0100] Centered on the origin of the lidar, divide the surrounding area into N r = 20 (20 - 30) equal parts radially in polar coordinates, and N s = 60 (60 - 80) equal parts circumferentially, and set the maximum distance L max = 80 (80 - 100) meters of the lidar point cloud;

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

[0102] The N ij ×N r matrix generated according to ρ s is the current frame descriptor; where, N r is the number of radial equal parts of the lidar point cloud, and N s is the number of circumferential equal parts of the lidar point cloud.

[0103] 2.2.2) traverse the descriptors of the key frames in the graph and the descriptors of the current frame in turn to calculate the matching value. 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 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 are the corresponding column vectors in the key frame and current frame description sub-matrices respectively; N s is the number of equal divisions of the LiDAR point cloud along the circumference.

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

[0108] 2.2.4) Using the corresponding pose as the center point, use the 64-way tree traversal search method to calculate the pose in the tree node. Use the searched pose to calculate the Euclidean distance between each point in the current frame point cloud and the map point. The pose with the smallest average Euclidean distance is the pose 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 pose (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three-dimensional coordinate data of the pose, respectively, in meters; Roll, Pitch, and Yaw are the roll, pitch, and yaw angles of the pose, respectively, in degrees; define the area (Xx, Yy, Zz, Roll-r, Pitch-p, Yaw-ya) (X+x, Y+y, Z+z, Roll+r, Pitch+p, Yaw+ya) as the search window;

[0111] The area within the search window is divided into six dimensions (X, Y, Z) and three rotations (Roll, Pitch, and Yaw). This results in a 64-way tree with the center point pose data as the root node. The minimum distance between each leaf node in the 64-way tree in the X, Y, and Z axes is 0.025 meters, and the rotation angles of Roll, Pitch, and Yaw are 0.2 degrees respectively.

[0112] Perform a breadth - first traversal of the 64 - fork tree in the previous step, calculate the matching degree between the current frame and the map using the pose data on each node, and the algorithm used can select 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 score of the parent node is greater than the score of the current node, then this node and its associated nodes do not continue the depth traversal; otherwise, this node continues the calculation.

[0114] 2.2.5), Use the aforementioned gicp pose verification algorithm to further verify the calculated pose result data. If the verification passes, proceed to the next step of positioning. If the verification fails, take the next frame of lidar data in the lidar sensor and recalculate step (ii).

[0115] Step (iii), Positioning;

[0116] Use the pose obtained from the relocalization calculation in step (ii) as the initial pose, continuously receive lidar data from the lidar sensor for continuous pose calculation, and judge the calculated pose state;

[0117] 3.1), Continuous pose calculation;

[0118] Construct a kd - tree for the point cloud map. Take the latest lidar data received from the lidar as the latest current frame. For each point in the current frame, find the 5 nearest points in the map kd - tree, perform plane fitting on the 5 found points, calculate the distance from the point to the fitted plane, use the distance from all points in the current frame point cloud to the plane as the optimization term, the rotation matrix R and displacement t as the optimization variables, and use the Gauss - Newton optimization algorithm to calculate and obtain the results of the optimal rotation matrix and displacement;

[0119] 3.2), Judge the positioning state based on the pose calculation result;

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

[0121] If the positioning state is normal, record the data to provide calculation data for relocalization when restarting the device again; if the positioning state is abnormal, do not record the data.

[0122] Specifically, it includes the following execution steps:

[0123] 3.2.1), The current frame point cloud is P a , for each point p k in the current frame point cloud, find the nearest point in the map. All the nearest points form the matching frame point cloud Pb ;

[0124] 3.2.2), Calculate the differential entropy for each point in the current frame point cloud P a and the matching frame point cloud P b The calculation formula is as follows,

[0125] where p k is the three-dimensional coordinate of the point in the current frame and the matching frame point cloud, and ∑p k is the covariance matrix calculated from all points within the radius r of point p k , and det is the determinant of the covariance matrix,

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

[0127]

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

[0129] 3.2.4), Calculate the separation entropy between the current frame and the matching frame using the point cloud differential entropy calculated in step 3.2.3), and the calculation formula is

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

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

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

[0133] As described above, the embodiments given in the accompanying drawings are only the preferred solutions for achieving the purpose of the present invention. For those skilled in the art, they can obtain inspiration therefrom and directly derive other alternative structures that conform to the design concept of the present invention. The other structural features obtained thereby should also fall within the scope of the solutions described in the present invention.

Claims

1. A lidar-based relocalization method, characterized in that: It includes the following steps: Step 1): Map building; Use a laser to scan in the operating area to construct a point cloud map; Step 2): Relocalization; Based on the point cloud map constructed in Step 1), judge the state before the previous localization; If the localization is normal, execute the following steps: 2.1.1): Read the localization data; 2.1.2): Register with the point cloud map and calculate the score; For the current frame, calculate the registration score using the pose read and the point cloud map, and use the gicp pose verification algorithm to verify the pose; if the score is sufficient, the relocalization is successful, and if the score is insufficient, execute according to the following abnormal localization process; If the localization is abnormal, execute the following descriptor relocalization process: 2.2.1): Calculate the descriptor of the current laser frame; 2.2.2): Traverse the descriptors of the key frames in the map building and the descriptor of the current frame in turn to calculate the matching value. The matching value result corresponds to the matching degree between the key frame and the current frame, and 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 building; 2.2.4): Using the corresponding pose as the center point, use the method of 64 - fork tree traversal search to calculate the poses in the tree nodes, calculate the Euclidean distance between each point in the current frame point cloud and the map point, and the pose of the node with the smallest average Euclidean distance is the pose 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, proceed to the next step of localization. If the verification fails, take the next frame of laser data in the lidar sensor and recalculate Step 2); Step 3): Localization; Use the pose calculated by the relocalization in Step 2) as the initial pose, continuously receive laser data from the lidar sensor for continuous pose calculation, and judge the pose state calculated; 3.1): Continuous pose calculation; Construct a kd - tree for the point cloud map. Use the latest laser data received from the lidar as the latest current frame. For each point in the current frame, find the nearest 5 points in the map kd - tree, perform plane fitting on the 5 found points, calculate the distance between the point and the fitted plane, use the distance between all points in the current frame point cloud and the plane as the optimization term, the rotation matrix R and the displacement t as the optimization variables, and use the Gauss - Newton optimization algorithm to calculate the results of the optimal rotation matrix and displacement; 3.2): Judge the localization state based on the pose calculation result; Calculate the differential entropy, joint entropy, and separation entropy using the current frame and the point cloud data matched to the map, judge the difference between the joint entropy and the separation entropy. If the difference is within the threshold range, judge that the localization state is normal. If the difference is greater than the threshold, judge that the localization state is abnormal; If the localization state is normal, record the data to provide calculation data for relocalization when restarting the device again. If the localization state is abnormal, do not record the data.

2. The lidar-based relocalization method according to claim 1, wherein: In Step 1) mentioned above, constructing the point cloud map includes saving the point cloud map file, the key frame point cloud file, the key frame descriptor file, and the pose trajectory file corresponding to the key frame.

3. The lidar-based relocalization method according to claim 1, wherein: The process of calculating the registration score includes the following steps: Get the current laser radar scan 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 the distance is less than 0.15-0.25 meters, then add this point 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 lidar-based relocalization method according to claim 1, wherein: The gicp pose verification algorithm includes the following steps: Perform the initial pose 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 rotation and displacement calculation; p i is the coordinate position of the source point in the XYZ three-dimensional coordinate system, with the unit of meter; 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, with the unit of meter; Calculate the point cloud covariance; select N adjacent points to calculate the point cloud covariance. The covariance formula is as follows: where p i is the coordinate position in the XYZ three-dimensional coordinate system of the current point, with the unit of meter, and q j is the coordinate position in the XYZ three-dimensional coordinate system of the neighborhood point, with the unit of meter; 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 to form 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 and ∑ 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 lidar-based relocalization method according to claim 1, characterized in that: The step 2.2.1) includes the following process: With the LiDAR origin as the center, the surrounding area is divided into 20 to 30 equal parts radially and 60 to 80 equal parts circumferentially in polar coordinate form, and the maximum distance Lmax of the LiDAR point cloud is set to 80 to 100 meters; Calculate the maximum height of the lidar point cloud set ρ r} within the radial circle i ∈ {1, 2,..., N s} and the circumferential circle j ∈ {1, 2,..., N ij} ∈ φ{ρ ij}; According to ρ ij The generated N r ×N s matrix is the current frame descriptor; where N r is the number of radial equal divisions of the lidar point cloud, and N s is the number of circumferential equal divisions of the lidar point cloud.

6. The lidar-based relocalization method according to claim 1, wherein: 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 are the corresponding column vectors in the descriptor matrices of the key frame and the current frame respectively, N s is the number of equally divided circumferences of the lidar point cloud.

7. The lidar-based relocalization method according to claim 1, wherein: The 64-way tree traversal search includes the following steps: Take the selected pose (X, Y, Z, Roll, Pitch, Yaw) as the center point, where X, Y, and Z are the three-dimensional coordinate data of the pose, respectively, in meters; Roll, Pitch, and Yaw are the roll, pitch, and yaw angles of the pose, respectively, in degrees; define the area (Xx, Yy, Zz, Roll-r, Pitch-p, Yaw-ya) (X+x, Y+y, Z+z, Roll+r, Pitch+p, Yaw+ya) as the search window; The area within the search window is divided into six dimensions (X, Y, Z) and three rotations (Roll, Pitch, and Yaw). This results in a 64-way tree with the center point pose data as the root node. The minimum distance between each leaf node in the 64-way tree in the X, Y, and Z axes 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-way 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 lidar-based relocalization 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 current frame point cloud k , find the nearest point in the map, and all the nearest points form the matching frame point cloud P b ; 3.2.2), calculate the differential entropy for each point in the current frame point cloud P a and the matching frame point cloud P b The calculation formula is as follows where p k is the three-dimensional coordinate of the points in the current frame and the matching frame point cloud, and ∑p k is the covariance matrix calculated from all points within the radius r of point p k , and det is the determinant of the covariance matrix 3.2.3), Calculate the differential entropy of the current frame and the point cloud of the matching frame using the differential entropy of each point calculated in step 3.2.2); the calculation formulas are respectively, Among them, H a (P a ) is the differential entropy of the current frame point cloud, and H b (P b ) is the differential entropy of the matching frame point cloud. |P a | is the number of points in the current frame point cloud, and |P b | is the number of points in the matching frame point cloud; 3.2.4), Calculate the separation entropy between the current frame and the matching frame using the point cloud differential entropy calculated in step 3.2.3). The calculation formula is Among them, 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 formulas in steps 3.2.2) and 3.2.3); the calculation formula is Among them, H joint is the calculated joint entropy, and H(P a ∪P b ) is the differential entropy of the point cloud calculated after taking the union of the current frame and the point cloud of the matching frame.

Images