A LiDAR positioning method based on ICP and KD tree

Abstract

The present invention discloses a laser radar positioning method based on ICP and KD tree, and relates to the field of navigation and positioning technology. A mobile terminal equipped with a laser radar is used to scan each fixed target object in a target area, and a point cloud set corresponding to each fixed target object is obtained. The point cloud set includes a continuous point cloud of a reflector and a continuous point cloud of a wall. For each fixed target object in the target area, based on the reflection intensity of each single point cloud in a single point cloud frame, the coordinates of the fixed target object are extracted and the position and posture of the mobile terminal equipped with the laser radar are determined using the ICP algorithm. Through the technical solution of the present invention, initial values ​​are provided for the ICP algorithm and the KD tree search, which improves the real-time performance and robustness, and removes the accumulated error.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a laser radar positioning method based on ICP and KD tree. Background Art

[0002] The laser radar-based positioning method is one of the mainstream technologies currently used in AGV positioning systems. According to the VDI2510 standard guidance manual, AGV is an "autonomous transportation vehicle running on the ground that is automatically controlled and guided by a non-contact guidance system."

[0003] At present, the LiDAR positioning system is mainly divided into two types: the positioning system based on artificial beacons and the positioning system based on natural features. The LiDAR positioning system based on artificial beacons mainly relies on reflectors with high reflection intensity and the three-point positioning method. The reflector scanned by the LiDAR at the current position is matched with the reflector in the pre-stored global map. Since the coordinates of all reflectors in the environment are known, the global coordinates of the reflector scanned by the LiDAR at the current position can be obtained according to the matching results. When there are at least three global coordinates obtained, the current global coordinates of the LiDAR can be obtained by the three-point positioning method according to the distance between the LiDAR and each reflector. The positioning method based on natural features is mainly completed in combination with SLAM technology, including GMapping, Hector and Cartographer of two-dimensional SLAM, and LOAM algorithm of three-dimensional SLAM. Considering the production cost of 2DLidar and 3DLidar, AGV positioning technology based on two-dimensional SLAM is more widely used.

[0004] AGV positioning technology based on SLAM algorithm is suitable for scenes with rich geometric features, especially corner features, but SLAM algorithm is not applicable to scenes with sparse geometric features such as corridors and warehouses. The AGV positioning method based on artificial beacons is more accurate, but it requires certain modifications to the AGV usage scenarios and has certain restrictions on the layout of artificial beacons in the scenes. During use, the laser radar is required to detect at least three light-emitting panels. Summary of the invention

[0005] The purpose of the present invention is to provide a laser radar positioning method based on ICP and KD tree to solve the problems in the prior art.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A laser radar positioning method based on ICP and KD tree, using a mobile terminal equipped with a laser radar to continuously scan each fixed target object in a target area, obtain each laser beam corresponding to each fixed target object, and further obtain a point cloud set corresponding to each laser beam, wherein the point cloud set includes a reflector point cloud set and a wall point cloud set. The reflector is taken as a fixed target object, and based on the point cloud set, the final position and posture of the mobile terminal equipped with the laser radar is determined through the following steps A to E:

[0008] Step A, for each fixed target object, further for each laser beam corresponding to the fixed target object, obtain each single point cloud in the reflector point cloud set in the point cloud set corresponding to each laser beam, further determine the reflection intensity threshold based on the reflection intensity and incident angle corresponding to each single point cloud, use the reflection intensity threshold to screen each single point cloud, obtain each single point cloud of each reflector surface contained in the fixed target object, for each reflector surface, use the least squares method to fit the single point cloud of the reflector surface, obtain the fitting surface, the center position of the fitting surface and the fitting inner diameter, that is, obtain the fitting surface corresponding to each reflector surface corresponding to the fixed target object;

[0009] Step B: for each fixed target object, based on each single point cloud in the wall point cloud set in the point cloud set corresponding to the fixed target object, obtain the depth change rate of each single point cloud at its position when receiving different laser beams, and determine each wall edge point within a preset distance range corresponding to each reflector surface; at the same time, calculate the curvature of each single point cloud based on the wall point cloud set, determine each wall corner point, and fit the wall point cloud set using the least squares method based on the single point clouds corresponding to the wall edge point and the wall corner point, and obtain the fitted wall position, that is, obtain the fitted wall position corresponding to each reflector surface, and then enter step C;

[0010] Step C: Based on the reflector set, a reflector triangle set is constructed with the center position of the reflector as the triangle vertex, and the side lengths of the reflector triangles are compared for the reflector triangle sets in two consecutive point cloud frame sets to further obtain a point cloud matching pair set of the reflector set, and then proceed to step D;

[0011] Step D: for each point cloud matching pair in the point cloud matching pair set of the reflector, classify the point cloud matching pair according to its corresponding reflector set to obtain a matching classification set, calculate the pose matrix of the matching classification set by using singular value decomposition, and iteratively update the pose matrix by using the ICP algorithm. When the change in the pose matrix is ​​less than a preset change threshold, obtain the pose change of the mobile terminal equipped with a laser radar, determine the rough pose of the mobile terminal, and then enter step E;

[0012] Step E: Based on the rough pose of the mobile terminal and the center position of the reflector, a KD tree corresponding to the center position of the reflector is constructed. The BBF algorithm is used to search for multiple center positions of the reflectors that are closest to the rough pose and a KD tree triangle set is constructed. Three nearest center positions of the reflectors are selected from the reflector set and compared with the KD tree triangle set to further obtain a KD tree point cloud matching set. For the KD tree point cloud matching set, singular value decomposition is used to determine the final pose of the mobile terminal.

[0013] Furthermore, the aforementioned step B specifically includes the following steps:

[0014] Step B1: for each fixed target object in the target area, according to the following formula:

[0015]

[0016] Get the depth change rate ξ of different laser beams in the point cloud set at the same single point cloud, where CP E , CP W are the lengths of two laser beams passing through the same single point cloud. When the depth change rate ξ is greater than the preset change rate threshold, the point cloud is located at the edge of the wall.

[0017] Step B2: for each fixed target object and its corresponding wall point cloud set, according to the formula:

[0018]

[0019] Get the curvature c of a single point cloud, where T is the number of single point clouds in the wall point cloud frame set, V (k,i) V is the length of the laser beam at the i-th single point cloud corresponding to the surface of the k-th reflector in the reflector set, (k,j) is the length of the laser beam represented by the jth single point cloud corresponding to the surface of the kth reflector in the reflector set. When the curvature of the single point cloud is greater than the preset curvature threshold and is the maximum curvature in the wall point cloud set, the position corresponding to the single point cloud is the wall corner point;

[0020] Step B3: Use the least squares method to fit the wall point cloud set corresponding to the wall to obtain the fitted wall position, and perform translation correction on the fitting surface based on the wall position and the fitting inner diameter obtained in step A to obtain the center position of the corrected fitting surface.

[0021] Furthermore, in the triangle construction process described in the aforementioned step C, when the number of reflector center positions included in the reflector set is greater than three, a reflector triangle with the reflector center position as the vertex is constructed; when the number of reflector center positions included in the reflector set is less than three, the wall edge point and the wall corner point are added to the reflector set, and a reflector triangle is constructed with the reflector center position, the wall edge point or the wall corner point as the vertex.

[0022] Furthermore, when the side lengths of each matching triangle are successfully compared one by one, a point cloud matching pair of the reflector surface is obtained, and it is determined whether the point cloud matching pair exists in the point cloud matching pair set. When the point cloud matching pair does not exist in the point cloud matching set, the point cloud matching pair is saved in the point cloud matching set, and the score of the point cloud matching pair is set to 1. When the point cloud matching pair exists in the point cloud matching set, the score of the point cloud matching pair is increased by 1 and the score is updated.

[0023] Furthermore, the reflector triangle is constructed in a clockwise order based on the center position of any reflector.

[0024] Furthermore, in the aforementioned step D, the pose matrix of the matching classification set is used as the initial value of the ICP algorithm to iteratively transform each single point cloud to obtain the distance between each single point cloud, and the two single point clouds with the smallest distance are taken as the closest point pair, and a new pose matrix is ​​further obtained and the original pose matrix is ​​updated until the number of updates reaches a preset threshold or the change in the pose matrix is ​​less than a preset change threshold, and the pose change of the target object is obtained, and the changes in each pose matrix obtained during the update process are accumulated to obtain an accumulated pose matrix, thereby determining the rough pose of the mobile terminal.

[0025] Furthermore, the aforementioned step E specifically includes the following steps:

[0026] Step E1, for all the reflector center positions obtained by measurement, based on the point cloud matching pairs on the reflector surface, a two-dimensional KD tree is used to construct a KD tree containing all the reflector center positions;

[0027] Step E2: for the rough posture of the mobile terminal obtained by accumulating the posture matrix, search the KD tree for the center positions of the three reflectors closest to the posture as the first group, and simultaneously obtain each leaf node of the KD tree;

[0028] Step E3, for each leaf node, backtrack search is performed starting from the leaf node, and a circle is drawn with the laser radar posture as the center and the distance from the posture to the nearest node as the radius. If the circle intersects with the plane divided by the KD tree node, the nearest point search is performed in the branch represented by the plane. The process is repeated until there are no other nodes in the area where the circle intersects with the plane divided by the KD tree node. At this time, the center position of the reflector represented by the KD tree node is the center position of the nearest reflector, and the node corresponding to the center position of the nearest reflector is deleted from the KD tree;

[0029] Step E4, repeat the process of steps E2 and E3 to obtain the three nearest reflector center positions as the second group, based on the coordinates of the three nearest reflector center positions in the first group, obtain the matching triangle formed by the three nearest reflector center positions in the first group and the second group by triangle side length matching, and use the process described in step D to use singular value decomposition to transform the posture of the mobile terminal in the KD tree coordinate system for the matching regular triangles corresponding to the first group and the second group respectively, to obtain the final posture of the mobile terminal.

[0030] Furthermore, in step E1, a KD tree including the center positions of all reflectors is constructed, the horizontal coordinates of the center positions of each reflector are used as the odd layers of the KD tree, the vertical coordinates of the center positions of each reflector are used as the even layers of the KD tree, the KD tree is divided into dimensions, the center positions of the reflectors that are smaller than the median of the division dimension are placed in the left branch of the KD tree, and the remaining center positions of the reflectors are placed in the right branch, until the node has only one element;

[0031] In step E2, the size of the pose and the KD tree node are compared according to the division dimension of the current KD tree layer, that is, the odd-numbered layers compare the horizontal coordinates, and the even-numbered layers compare the vertical coordinates. If the pose is smaller than the node, the left branch of the node is entered to continue searching, otherwise the right branch is entered until the compared node is a leaf node and there are no more branches. At this time, the node is the nearest node;

[0032] In step E3, when deleting a node, if both subtrees of the deleted node are empty, an empty tree is used to replace it. Otherwise, the rightmost node of the left subtree or the leftmost node of the right subtree of the deleted node is selected as the replacement node according to the division dimension of the current layer.

[0033] The laser radar positioning method based on ICP and KD tree described in the present invention adopts the above technical solution and has the following technical effects compared with the prior art:

[0034] The present invention combines natural features with artificial landmarks for laser radar positioning, which not only enables the positioning system to ensure good positioning results in environments lacking feature points such as corridor warehouses, but also maintains a certain positioning accuracy in sections where reflectors cannot be detected, greatly improving the robustness of the positioning system. It also provides a more accurate initial value for the ICP algorithm, reduces the number of subsequent iterations, and effectively improves the real-time performance of the positioning system. Finally, the accumulated error of the ICP algorithm is removed by searching the KD tree. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a schematic diagram of a flow chart of a laser radar positioning method according to an exemplary embodiment of the present invention;

[0036] FIG. 2( a ) is a schematic diagram of point cloud coordinates of a reflector surface according to an exemplary embodiment of the present invention;

[0037] FIG2( b ) is a schematic diagram of the surface fitting of the reflective plate according to an embodiment of the present invention;

[0038] Figure 3 is a schematic diagram of the structure of a reflective plate according to an exemplary embodiment of the present invention;

[0039] Figure 4 is a schematic diagram of the trailing points contained in the point cloud of the reflector surface in an exemplary embodiment of the present invention;

[0040] Figure 5 A schematic diagram of determining a wall boundary according to an exemplary embodiment of the present invention;

[0041] Figure 6 is a schematic diagram of a fitting wall curve according to an exemplary embodiment of the present invention;

[0042] Figure 7 is a schematic diagram of a coordinate correction process of an exemplary embodiment of the present invention;

[0043] Figure 8 A schematic diagram of surface correction fitting of a reflector according to an exemplary embodiment of the present invention;

[0044] Fig. 9 is a schematic diagram of matching a reflective plate according to an exemplary embodiment of the present invention;

[0045] Fig.10 A schematic diagram of a triangle construction for an exemplary embodiment of the present invention;

[0046] Fig.11 A schematic diagram of KD tree construction according to an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0047] In order to better understand the technical content of the present invention, specific embodiments are given and described as follows in conjunction with the accompanying drawings.

[0048] Various aspects of the present invention are described in this disclosure with reference to the accompanying drawings, in which many illustrative embodiments are shown. The embodiments of the present disclosure are not necessarily defined to include all aspects of the present invention. It should be understood that the various concepts and embodiments introduced above, as well as those described in more detail below, can be implemented in any of many ways, because the concepts and embodiments disclosed by the present invention are not limited to any implementation. In addition, some aspects disclosed by the present invention can be used alone, or in any appropriate combination with other aspects disclosed by the present invention.

[0049] Combination Figure 1 The exemplary embodiment of the present invention is a schematic diagram of the process of the laser radar positioning method based on ICP and KD tree.

[0050] The mobile terminal equipped with a laser radar is used to continuously scan each fixed target object in the target area to obtain each laser beam corresponding to each fixed target object, and further obtain a point cloud set corresponding to each laser beam, wherein the point cloud set includes a reflector point cloud set and a wall point cloud set. Based on the point cloud set, the final position and posture of the mobile terminal equipped with the laser radar is determined through the following steps A to E:

[0051] Step A, for each fixed target object, further for each laser beam corresponding to the fixed target object, obtain each single point cloud in the reflector point cloud set in the point cloud set corresponding to each laser beam, further determine the reflection intensity threshold based on the reflection intensity and incident angle corresponding to each single point cloud, use the reflection intensity threshold to screen each single point cloud, obtain each single point cloud of each reflector surface contained in the fixed target object, for each reflector surface, use the least squares method to fit the single point cloud of the reflector surface, obtain the fitting surface, the center position of the fitting surface and the fitting inner diameter, that is, obtain the fitting surface corresponding to each reflector surface corresponding to the fixed target object;

[0052] Step B: for each fixed target object, based on each single point cloud in the wall point cloud set in the point cloud set corresponding to the fixed target object, obtain the depth change rate of each single point cloud at its position when receiving different laser beams, and determine each wall edge point within a preset distance range corresponding to each reflector surface; at the same time, calculate the curvature of each single point cloud based on the wall point cloud set, determine each wall corner point, and fit the wall point cloud set using the least squares method based on the single point clouds corresponding to the wall edge point and the wall corner point, and obtain the fitted wall position, that is, obtain the fitted wall position corresponding to each reflector surface, and then enter step C;

[0053] Step C: Based on the reflector set, a reflector triangle set is constructed with the center position of the reflector as the triangle vertex, and the side lengths of the reflector triangles are compared for the reflector triangle sets in two consecutive point cloud frame sets to further obtain a point cloud matching pair set of the reflector set, and then proceed to step D;

[0054] Step D: for each point cloud matching pair in the point cloud matching pair set of the reflector, classify the point cloud matching pair according to its corresponding reflector set to obtain a matching classification set, calculate the pose matrix of the matching classification set by using singular value decomposition, and iteratively update the pose matrix by using the ICP algorithm. When the change in the pose matrix is ​​less than a preset change threshold, obtain the pose change of the mobile terminal equipped with a laser radar, determine the rough pose of the mobile terminal, and then enter step E;

[0055] Step E: Based on the rough pose of the mobile terminal and the center position of the reflector, a KD tree corresponding to the center position of the reflector is constructed. The BBF algorithm is used to search for multiple center positions of the reflectors that are closest to the rough pose and a KD tree triangle set is constructed. Three nearest center positions of the reflectors are selected from the reflector set and compared with the KD tree triangle set to further obtain a KD tree point cloud matching set. For the KD tree point cloud matching set, singular value decomposition is used to determine the final pose of the mobile terminal.

[0056] Example

[0057] Using the process in step A, a fixed target object, i.e., a point cloud set of a reflector, is obtained from the Lidar. The point cloud coordinates of the reflector surface are extracted in the i+1 point cloud frame according to the point cloud reflection intensity. The reflector includes but is not limited to the cylindrical reflector provided by the exemplary embodiment of the present invention. As shown in FIG. 2( a ), the point cloud coordinates of the reflector surface and a certain number of point cloud data sets S={S i (x i ,y i ,f i )|i∈{1,2,...,n}}, where (x i ,y i ) is the coordinate of the point cloud, and f i It represents the reflection intensity value of the point cloud, and sets the reflection intensity threshold f t , put f i >f t The points are judged as the point cloud of the reflector surface. The solid points in Figure 2(a) are the point clouds of the reflector surface extracted according to the reflection intensity information. Let the set of the point cloud of the kth reflector be R k = {R rk (x rk ,y rk)|r∈{1,2,...,m}}, where (x rk ,y rk ) represents the coordinates of the rth point cloud of the kth reflector;

[0058] According to these coordinates, the least square method is used to fit the coordinates of the center of the reflector cross section. The fitted coordinates of the center of the circle can be obtained by minimizing the error function of the formula, as shown in the fitting result of Figure 2(b);

[0059] The structure of the reflector is as follows Figure 3 As shown, it includes a cylindrical carrier and a reflective film covering the surface of the carrier. The relationship between the reflection intensity of the reflective film and the incident angle is shown in Table 1 below. The corresponding reflection intensity threshold can be determined according to the incident angle of the point cloud information.

[0060] Table 1 Relationship between reflection intensity and incident angle of reflective film

[0061]

[0062] Combination Figure 4 The "×" point in the middle indicates the trailing point. The positive or negative value of the continuous vector rotation angle determines whether it is a trailing point. For example, the vector formed by the trailing point A and the center C Rotate counterclockwise to the vector formed by point B and the center C , and point D is not a trailing point, which forms a vector with the center C The direction is given by the vector The direction of rotation is clockwise, and the vector formed by the center C to the point is calculated point by point from the adjacent point A of the reflector point using the following formula:

[0063]

[0064]

[0065]

[0066] Then calculate the angle between the vectors of adjacent points according to the formula:

[0067]

[0068]

[0069] If three consecutive points are all trailing points, then the signs of the angles between the vectors are the same. Otherwise, the signs of the angles between the vectors are opposite. Therefore, ∠BCA*∠DCB is calculated. If ∠BCA*∠DCB<0, it proves that the signs of the angles are opposite. Then point B is the last point of the trailing point. From this, the first and last points of the trailing point can be determined.

[0070] According to the process described in step B, the position of the wall close to the reflector is obtained, and the boundary of the wall is established. One is the edge point of the wall, and the other is the corner point of the wall. The edge point is defined as the junction between the edge of the wall and other objects, and the corner point is defined as the point with the largest curvature within a certain range, such as Figure 5 As shown in the figure, near the edge point, the depth information read by the continuous laser beam will have a sudden change process, so it is possible to judge whether the point is an edge point based on the rate of change of the depth. Figure 5 Two continuous laser beams CP E and CP W Take this as an example to calculate the rate of change of depth according to the following formula:

[0071]

[0072] The function max represents the maximum value of the length of the two laser beams. When the rate of change is greater than the set threshold, the point can be set. Figure 5 Point P in E , determined as an edge point;

[0073] After determining the edge points on both sides of the wall, in order to ensure that the fitted wall is a straight line, it is necessary to determine whether there are corner points within the wall range determined by the edge points, and obtain the curvature of a single point cloud, according to the formula:

[0074]

[0075] In the formula, |T| means that the curvature of the i-th detected point is calculated by using |T| points around the i-th detected point, ‖V (k,i) ‖ represents the length of the laser beam represented by the i-th point cloud in the k-th reflector, ‖∑ j∈|T|,j≠i (V (k,i) -V (k,j) )‖ is the sum of the vectors formed by the |T| points around the i-th detected point and the detected point. When the curvature is greater than the set threshold and is the point with the largest curvature among all points, the point can be determined as a corner point;

[0076] By traversing a certain number of points adjacent to the trailing points on both sides, the point cloud range of the wall can be determined, and the wall point cloud set W around the kth reflector can be obtained. k = {W tk (x tk ,y tk )|t∈{1,2,...,o}}, the least squares method can be used to fit the position of the wall represented by a straight line based on the wall point cloud set. The fitting is performed according to the following formula. The fitting result is as follows: Figure 6 As shown:

[0077]

[0078]

[0079]

[0080] y=ax+b

[0081] If the radius of the reflector is known and the reflector is placed close to the wall, the distance between the center of the reflector and the wall should be equal to the radius of the reflector. Therefore, according to the fitted wall position, the center of the fitted reflector is translated along the direction of the vector formed by the center of the circle and the origin of the coordinate system to a point where the distance from the wall is the radius. This point is the corrected reflector coordinate, such as Figure 7 As shown in the figure, the correction of the reflector coordinates can be converted into translating point C along the vector CQ by the length of CC', obtaining the distance required for the correction of the reflector center, and obtaining the corrected reflector coordinates C'(x c′ ,y c′ ), the correction effect is as follows Figure 8 shown.

[0082] Apply the process described in step C to each reflector surface. If the number of reflectors is greater than or equal to three, use the center position of the reflector as the triangle vertex to construct a reflector triangle set. If there is only one or two reflectors, add the corner point and edge point information to the matching set and then construct it. Fig. 9 Taking the two frames of point cloud information as an example, the triangle set formed in the first frame point cloud coordinate system YOX can be represented by the reflector label as follows:

[0083] Tri={hij,hik,hil,hig,hjk,hjl,hjg,hkl,hkg,hlg,ijk,ijl,ijg,ikl,ikg,ilg,jkl,jkg,jlg,klg}

[0084] Due to the existence of measurement errors and reflector installation errors, three reflector points that are theoretically collinear, such as BCD and EFG, can still form a triangle. The triangle set formed in the second frame point cloud coordinate system Y'O'X' can be represented by the reflector label as follows:

[0085] Tri′

[0086] ={bcd,bce,bcf,bca,bde,bdf,bda,bef,bea,bfa,cde,cdf,cda,cef,cea,cfa,def,dea,dfa,efa}

[0087] Each triangle mark in these two sets stores the length of the three line segments formed by the three reflectors in this mark. When constructing the triangle sets Tri and Tri′, it starts from the reflector h or c closest to the Lidar. The reason is that the movement speed of the AGV limits the position transformation of the point cloud of two adjacent frames. The probability of successful matching of the reflector closest to the Lidar in the second frame is the highest. Starting from this closest point to construct the triangle set can speed up the subsequent matching process. However, in actual use, the reflector is likely to be blocked, so the walls around the reflector are combined to make up for the positioning error when there are less than three reflectors.

[0088] like Fig.10 As shown, when only one or two reflectors are detected, the corner points or edge points on both sides of each reflector are added to the original reflector matching point set. If there are no corner points or edge points, points on the straight line of the wall that are a certain distance away from the reflector are selected and added to the original matching point set.

[0089] After obtaining the triangle set formed by the reflectors in the two frames of point cloud, start from the first element in and compare the three side length information of each element. Since the triangles are constructed in a clockwise order, only three comparisons are required at most. Take the three reflectors BCD, that is, the bcd comparison in as an example:

[0090] First, the lengths of the three sides are compared in order, namely bc and hk, cd and kg, and db and gh. Since the lengths do not match, the comparison fails. Next, bc and kg, cd and gh, and db and hk are compared, which still fails. Finally, bc and gh, cd and hk, and db and kg are compared, and the comparison succeeds. If the comparison fails after the three comparisons, it can be judged as a mismatch. There will be no need to compare bc and gh, cd and kg, and db and hk.

[0091] After the triangle matching is successful, three matching reflector pairs can be obtained, namely, b and g, c and h, and d and k. If there is no such matching pair in the previous triangle matching, such a matching pair is created and saved in the matching pair set Pairs and the score of the pair is set to 1. If such a matching pair already exists, the score of the pair is increased by one. That is, after bcd and hkg are successfully matched, the set Pairs will become Pairs = {(b,g,1), (c,h,1), (d,k,1)}; then continue to match the second element bce, and similarly, the edge length matching method can be used to successfully match hlg, and three matching reflector pairs are obtained, namely, b and g, c and h, and e and l. Since b and g, c and h have been created in the previous matching, the scores of these two matching pairs in Pairs are increased by 1 in this matching. However, e and l do not exist in Pairs, so the pair is created and the score is set to 1. At this time, Pairs becomes Pairs = {(b, g, 2), (c, h, 2), (d, k, 1), (e, l, 1)}. Similarly, after traversing all the triangles formed by the nearest reflector b, a preliminary reflector pair set can be obtained. If there is no successful match after traversing all the triangles formed by the reflector b, all the remaining triangles are matched in order to complete the match.

[0092] Mismatches may still occur during the matching process. For example, BCF and EFC, which have similar lengths of three sides, are shown as bcd and hjk in the matching process. After the above matching method is used, three reflector matching pairs of b and j, c and k, and d and h will appear. In the above matching, it has been obtained that b matches g, while mismatches occur when b and j match. During the matching process, a reflector in the current frame can only uniquely correspond to a reflector in the previous frame. When one reflector matches multiple or multiple matches one, it proves that mismatches have occurred. At this time, the matching pair scores can be used for screening. The pair score obtained during the matching process is 4, while the mismatched pair score is only 1. Selecting a matching pair with a higher score can effectively screen out the mismatched pairs caused by mismatches. After removal, the final reflector pair set Pairs′={(b,g,4),(c,h,4),(d,k,4),(e,l,4)} is obtained.

[0093] According to the process in step D, the matching relationship between the reflectors is obtained, which can be regarded as the correspondence between the two point sets. The singular value decomposition is used to calculate the pose between the point sets composed of the reflectors. Taking the obtained set Pairs′ as an example, firstly, Pairs′ is divided into two point sets according to the matching results, P = {P1(x b ,y b ),P2(x c,y c ),P3(x d ,y d ),P4(x e ,y e )} and X={X1(x g ,y g ),X2(x h ,y h ),X3(x k ,y k ),X4(x l ,y l )}, calculate the center of the set P and X according to the following formula:

[0094]

[0095] Calculate the covariance matrix of sets P and X according to the formula:

[0096]

[0097] Then the covariance matrix W px Perform SVD decomposition according to the formula:

[0098]

[0099] where σ1...σ n is the covariance matrix W px The singular values ​​of the matrix U are The eigenvectors of , the columns of the matrix V are The eigenvector of , after SVD decomposition, the expressions of rotation matrix R ′ and translation matrix t ′ are: R ′=VU T , t′=u x -R′u p ;

[0100] Here R′ is Fig. 9 The XOY coordinate system in is rotated to a rotation matrix parallel to the X′O′Y′ coordinate system, and t′ is Fig. 9 The translation matrix from midpoint O to point O'.

[0101] After obtaining R′ and t′, they are used as the initial values ​​of the ICP algorithm and then the ICP algorithm is iterated. First, the first frame of the two frames of point clouds is converted according to R′ and t′, and then the distance between the points in the two frames of point clouds is calculated based on the converted first frame point cloud. The point cloud in the first frame that is closest to the point cloud in the second frame is taken as a closest point pair. After obtaining all the closest point pairs, a new set of rotation matrices and translation matrices are calculated. The above process is repeated until a certain number of iterations is reached or the change in the pose matrix is ​​less than a certain threshold. Finally, the system's pose transformation R and t are obtained. The rough value of the current position of the system can be obtained by adding up the pose changes calculated between frames each time.

[0102] According to the process in step E, after obtaining the rough value of the current position of the system, the three reflector center positions closest to the rough value can be searched in the reflector center position KD tree. Take the reflector set {(6,9), (4,1), (1,5), (11,6), (3,7), (2,5), (8,3)} as an example, first use the horizontal coordinate as the division dimension, sort all the horizontal coordinates from small to large, and the order is {1,2,3,4,6,8,11}, and the median is 4, so the root node of the KD tree is point (4,1), the points of the left branch are points with horizontal coordinates less than or equal to 4, that is, {(1,5), (2,5), (3,7)}, and the points of the right branch are points with horizontal coordinates greater than 4, that is, {(6,9), (8,3), (11,6)}. Next, the two branches are divided again according to the vertical coordinates. The median of the vertical coordinate of the left branch is 5, so the first node of the second layer is (2,5), the first node of the third layer is (1,5), and the second node of the third layer is (3,7). Similarly, the second node of the second layer is (11,6), the third node of the third layer is (8,3), and the fourth node of the third layer is (6,9). The final KD tree of the reflector center position is as follows: Fig.11 shown.

[0103] After that, based on the rough value of the current position of the system, the three reflector center positions closest to the laser radar stored in the KD tree can be searched. Assume that the current coordinates (X, Y) calculated by the laser radar according to the ICP algorithm are (7, 4), and the number of reflectors detected is 3. At this time, the problem can be transformed into finding the three points closest to the point (7, 4) in the constructed reflector center position KD tree. In the first layer of comparison, the horizontal coordinate of point (7, 4) is greater than the horizontal coordinate of (4, 1), so the right branch search is entered. The second layer is divided according to the vertical coordinate. The vertical coordinate of point (7, 4) is less than the vertical coordinate of (11, 6), so the leaf node (8, 3) is finally searched. Assume that the leaf node searched for the first time is the nearest point, and then backtrack upward in reverse order according to the search path. During the backtracking process, each node (x i ,yi ) is the Euclidean distance d from the target point (X,Y).

[0104]

[0105] If a node is closer to the target point than the current closest point, update the node to the closest point. The current closest point must exist in the area corresponding to a child node of the node. Check whether there is a closer point in the area corresponding to another child node of the current closest point. Since the KD tree divides the data set according to the corresponding dimensions, calculate whether the divided area corresponding to the other child node intersects with the circle with the target point as the center and the distance between the target point and the closest point as the radius. If it intersects, there may be a point closer to the target point in the area corresponding to another child node. Move to another child node and continue to recursively search for the nearest neighbor downward; if it does not intersect, backtrack upward. When backtracking to the root node, the backtracking process ends, and the last closest point is the nearest neighbor of the target point. According to the above method, the first closest point of point (7,4) can be found as (8,3).

[0106] When searching for the second nearest point, the first nearest point searched is deleted from the KD tree. Since the nearest point is not necessarily a leaf node, the following method is used to delete the node. If both subtrees of the deleted node are empty, an empty tree is used instead. Otherwise, the rightmost node of the left subtree or the leftmost node of the right subtree of the deleted node is selected as the replacement node according to the division dimension of the current layer. Then, in the new KD tree, the second nearest neighbor is searched according to the method of searching for the first nearest neighbor, and finally the second nearest neighbor is (4,1). Repeat the above process to obtain the third nearest neighbor (11,6). According to the reflector center position closest to the rough value of the laser radar position obtained in the three KD trees, the side length is matched with the reflector center position currently detected by the laser radar to obtain the corresponding reflector matching pair set. Finally, similar to step D, the system posture transformation in the KD tree coordinate system is obtained through SVD decomposition, which is the final position of the mobile terminal.

[0107] Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. A person with ordinary knowledge in the technical field to which the present invention belongs may make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the protection scope of the present invention shall be determined by the definition of the claims.

Claims

1. A laser radar positioning method based on ICP and KD tree, characterized in that: The mobile terminal equipped with a laser radar is used to continuously scan each fixed target object in the target area to obtain each laser beam corresponding to each fixed target object, and further obtain a point cloud set corresponding to each laser beam, wherein the point cloud set includes a reflector point cloud set and a wall point cloud set. Based on the point cloud set, the final position and posture of the mobile terminal equipped with a laser radar is determined through the following steps A to E: Step A, for each fixed target object, further for each laser beam corresponding to the fixed target object, obtain each single point cloud in the reflector point cloud set in the point cloud set corresponding to each laser beam, further determine the reflection intensity threshold based on the reflection intensity and incident angle corresponding to each single point cloud, use the reflection intensity threshold to screen each single point cloud, obtain each single point cloud of each reflector surface contained in the fixed target object, for each reflector surface, use the least squares method to fit the single point cloud of the reflector surface, obtain the fitting surface, the center position of the fitting surface and the fitting inner diameter, that is, obtain the fitting surface corresponding to each reflector surface corresponding to the fixed target object; Step B: for each fixed target object, based on each single point cloud in the wall point cloud set in the point cloud set corresponding to the fixed target object, obtain the depth change rate of each single point cloud at its position when receiving different laser beams, and determine each wall edge point within a preset distance range corresponding to each reflector surface; at the same time, calculate the curvature of each single point cloud based on the wall point cloud set, determine each wall corner point, and fit the wall point cloud set using the least squares method based on the single point clouds corresponding to the wall edge point and the wall corner point, and obtain the fitted wall position, that is, obtain the fitted wall position corresponding to each reflector surface, and then enter step C; Step C: Based on the reflector set, a reflector triangle set is constructed with the center position of the reflector as the triangle vertex, and the side lengths of the reflector triangles are compared for the reflector triangle sets in two consecutive point cloud frame sets to further obtain a point cloud matching pair set of the reflector set, and then proceed to step D; Step D: for each point cloud matching pair in the point cloud matching pair set of the reflector, classify the point cloud matching pair according to its corresponding reflector set to obtain a matching classification set, calculate the pose matrix of the matching classification set by using singular value decomposition, and iteratively update the pose matrix by using the ICP algorithm. When the change in the pose matrix is ​​less than a preset change threshold, obtain the pose change of the mobile terminal equipped with a laser radar, determine the rough pose of the mobile terminal, and then enter step E; Step E: Based on the rough pose of the mobile terminal and the center position of the reflector, a KD tree corresponding to the center position of the reflector is constructed. The BBF algorithm is used to search for multiple center positions of the reflectors that are closest to the rough pose and a KD tree triangle set is constructed. Three nearest center positions of the reflectors are selected from the reflector set and compared with the KD tree triangle set to further obtain a KD tree point cloud matching set. For the KD tree point cloud matching set, singular value decomposition is used to determine the final pose of the mobile terminal.

2. The laser radar positioning method based on ICP and KD tree according to claim 1, characterized in that: The step B specifically comprises the following steps: Step B1: for each fixed target object in the target area, according to the following formula: Get the depth change rate ξ of different laser beams in the point cloud set at the same single point cloud, where CP E , CP W are the lengths of two laser beams passing through the same single point cloud. When the depth change rate ξ is greater than the preset change rate threshold, the point cloud is located at the edge of the wall. Step B2: for each fixed target object and its corresponding wall point cloud set, according to the formula: Get the curvature c of a single point cloud, where T is the number of single point clouds in the wall point cloud frame set, V (k,i) V is the length of the laser beam at the i-th single point cloud corresponding to the surface of the k-th reflector in the reflector set, (k,j) is the length of the laser beam represented by the jth single point cloud corresponding to the surface of the kth reflector in the reflector set. When the curvature of the single point cloud is greater than the preset curvature threshold and is the maximum curvature in the wall point cloud set, the position corresponding to the single point cloud is the wall corner point; Step B3: Use the least squares method to fit the wall point cloud set corresponding to the wall to obtain the fitted wall position, and perform translation correction on the fitting surface based on the wall position and the fitting inner diameter obtained in step A to obtain the center position of the corrected fitting surface.

3. The laser radar positioning method based on ICP and KD tree according to claim 1, characterized in that: In the triangle construction process described in step C, when the number of reflector center positions included in the reflector set is greater than three, a reflector triangle with the reflector center position as the vertex is constructed; when the number of reflector center positions included in the reflector set is less than three, the wall edge point and the wall corner point are added to the reflector set, and a reflector triangle is constructed with the reflector center position, the wall edge point or the wall corner point as the vertex.

4. The laser radar positioning method based on ICP and KD tree according to claim 3 is characterized in that: When the side lengths of each matching triangle are successfully compared one by one, a point cloud matching pair on the reflector surface is obtained, and it is determined whether the point cloud matching pair exists in the point cloud matching pair set. When the point cloud matching pair does not exist in the point cloud matching set, the point cloud matching pair is saved in the point cloud matching set, and the score of the point cloud matching pair is set to 1. When the point cloud matching pair exists in the point cloud matching set, the score of the point cloud matching pair is increased by 1 and the score is updated.

5. The laser radar positioning method based on ICP and KD tree according to claim 4 is characterized in that: The reflector triangles are constructed in a clockwise order based on the center position of any reflector.

6. The laser radar positioning method based on ICP and KD tree according to claim 5, characterized in that: In the step D, the pose matrix of the matching classification set is used as the initial value of the ICP algorithm to iteratively transform each single point cloud, obtain the distance between each single point cloud, and take the two single point clouds with the smallest distance as the closest point pair, further obtain a new pose matrix and update the original pose matrix until the number of updates reaches a preset threshold or the change in the pose matrix is ​​less than the preset change threshold, obtain the pose change of the target object, and accumulate the changes of each pose matrix obtained during the update process to obtain an accumulated pose matrix, thereby determining the rough pose of the mobile terminal.

7. The laser radar positioning method based on ICP and KD tree according to claim 6 is characterized in that: The step E specifically comprises the following steps: Step E1, for all the reflector center positions obtained by measurement, based on the point cloud matching pairs on the reflector surface, a KD tree containing the center positions of all the reflector is constructed using a two-dimensional KD tree; Step E2: for the rough posture of the mobile terminal obtained by accumulating the posture matrix, search the KD tree for the center positions of the three reflectors closest to the posture as the first group, and simultaneously obtain each leaf node of the KD tree; Step E3, for each leaf node, backtrack search is performed starting from the leaf node, and a circle is drawn with the laser radar posture as the center and the distance from the posture to the nearest node as the radius. If the circle intersects with the plane divided by the KD tree node, the nearest point search is performed in the branch represented by the plane. The process is repeated until there are no other nodes in the area where the circle intersects with the plane divided by the KD tree node. At this time, the center position of the reflector represented by the KD tree node is the center position of the nearest reflector, and the node corresponding to the center position of the nearest reflector is deleted from the KD tree; Step E4, repeat the process of steps E2 and E3 to obtain the three nearest reflector center positions as the second group, based on the coordinates of the three nearest reflector center positions in the first group, obtain the matching triangle formed by the three nearest reflector center positions in the first group and the second group by triangle side length matching, and use the process described in step D to use singular value decomposition to transform the posture of the mobile terminal in the KD tree coordinate system for the matching regular triangles corresponding to the first group and the second group respectively, to obtain the final posture of the mobile terminal.

8. The laser radar positioning method based on ICP and KD tree according to claim 7 is characterized in that: In the step E1, a KD tree including the center positions of all reflectors is constructed, the horizontal coordinates of the center positions of each reflector are used as the odd layers of the KD tree, the vertical coordinates of the center positions of each reflector are used as the even layers of the KD tree, the KD tree is divided into dimensions, the center positions of the reflectors that are smaller than the median of the division dimension are placed in the left branch of the KD tree, and the remaining center positions of the reflectors are placed in the right branch, until the node has only one element; In step E2, the size of the pose and the KD tree node is compared according to the division dimension of the current KD tree layer, that is, the odd-numbered layers compare the horizontal coordinates, and the even-numbered layers compare the vertical coordinates. If the pose is smaller than the node, the left branch of the node is entered to continue searching, otherwise the right branch is entered until the compared node is a leaf node and there are no more branches. At this time, the node is the nearest node; In step E3, when deleting a node, if both subtrees of the deleted node are empty, an empty tree is used to replace it; otherwise, the rightmost node of the left subtree or the leftmost node of the right subtree of the deleted node is selected as the replacement node according to the division dimension of the current layer.

9. A laser radar positioning method based on ICP and KD tree according to any one of claims 1 to 8, characterized in that: The fixed target object is a reflector.

Images