Point cloud matching method and system for rotating three-dimensional laser radar
By decomposing the environmental position points into the first feature point and the second feature point in the three-dimensional lidar point matching, and building the error equation, the problem of high computational complexity in the existing technology is solved, and efficient point cloud matching is achieved. Suitable for real-time applications.
Patent Information
- Application Number
- CN202210267742.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-03-18
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2042-03-18
AI Technical Summary
The computing complexity in the existing three-dimensional lidar point cloud matching process is high, which seriously affects the real-time nature of the matching.
The point cloud matching method of rotating three-dimensional lidar is adopted. By dividing the environmental position points into the first feature point and the second feature point, and building the error equation to solve the error equation, obtaining the rotation matrix and translation parameters, thereby simplifying the point cloud matching process , improve matching speed.
It reduces the computing complexity of point cloud matching, improves the real-timeness of matching, simplifies feature expression, is suitable for embedded devices, and supports advanced driving assistance systems and autonomous driving applications.
Smart Images

Figure CN114648561B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a laser radar point cloud feature extraction and registration method, and in particular to a point cloud matching method and system for a rotating three-dimensional laser radar. Background Art
[0002] With the rapid development of driver assistance systems (AID) and unmanned driving technology, three-dimensional laser radar, as an important environmental perception sensor, has developed rapidly and produced a large number of mature products. As a large-scale environmental perception module, laser radar is now widely used in household sweeping robots and autonomous driving map creation, lane perception, etc. All point clouds generated by laser radar are relative to their own local coordinate system. To obtain the expression of this position in the global coordinate system, it is necessary to solve the pose parameters. The core technology of the parameter solution is the matching of three-dimensional point clouds, and the key point of matching is the extraction and use of point cloud features.
[0003] The method of three-dimensional laser point cloud feature extraction and its application has attracted a large number of scientific and technological workers and enthusiasts to participate in it, and has produced many beneficial results. Most of these feature extraction and point cloud matching results treat three-dimensional laser point clouds as an unordered point cloud. Therefore, in order to more completely express the characteristics of these point clouds, it is necessary to introduce a large number of additional parameters (normal vectors, domain statistical characteristics, etc.), and generally a larger local spatial scale needs to be considered to ensure the completeness of feature expression. When the above requirements are met, the dimension of the corresponding feature of each point will be much higher than the dimension of its position expression, which greatly increases the computing cost and storage cost. Corresponding to the current mainstream mechanical laser radar, it is closer to the characteristics of an ordered point cloud, and then the above point cloud features are used for matching processing. High-performance computing units are often required to ensure real-time performance, and these high-performance computing units will bring higher space, heat dissipation, power supply and other requirements, which will increase the complexity of a system in disguise. Summary of the invention
[0004] The problem to be solved by the present invention is to provide a point cloud matching method and system for a rotating three-dimensional laser radar in view of the high computational complexity of point cloud matching in the existing three-dimensional laser radar, which seriously affects the real-time performance of the matching.
[0005] The present invention provides a point cloud matching method for a rotating three-dimensional laser radar, wherein the rotating three-dimensional laser radar is arranged on a carrier, and the rotating three-dimensional laser radar has J radar lines arranged at intervals in the height direction, and is characterized in that the carrier moves, the rotating three-dimensional laser radar rotates and scans the surrounding environment, and the point cloud data of the rotating three-dimensional laser radar obtained in the i-th sampling period is defined as the i-th frame point cloud data, and the data of each environmental position point scanned by the j-th radar line in the i-th frame point cloud data constitutes the j-th radar ring of the i-th frame point cloud data, and the environmental position point M is defined i,j,k is the kth environmental position point scanned by the jth radar line in the i-th frame of point cloud data, wherein the point cloud data is point cloud data with timestamp information and radar line sequence number;
[0006] Step (A): Determine the environment position point M based on the i-th frame point cloud data i,j,k The corresponding horizontal environmental angle change value θ 1 (i, j, k) is within the preset angle range. If the judgment result is yes, the environmental position point M is determined. i,j,k is the first feature point, otherwise, determine the environmental position point M i,j,k is the second characteristic point, j=1,2,…,J, k=1,2,…,K, K is the number of environmental position points scanned by each radar line in each sampling period;
[0007] Step (B): Construct the error equation Solve the error equation to get the translation parameter tr from frame i+1 to frame i i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T , yaw angle ψ' i+1 , Roll Angle Pitch angle θ' i+1 ;
[0008] in:
[0009] ψ' i+1 , θ' i+1 are the yaw angle, roll angle, pitch angle, and yaw angle ψ' of the transformation from the i-th frame point cloud data to the i+1-th frame point cloud data. i+1 It is calculated based on the difference in position sequence numbers of the corresponding environmental position points in the i-th frame point cloud data and the i+1-th frame point cloud data in the radar ring and the rotation parameters of the rotating three-dimensional laser radar;
[0010] EA i+1,u1 is the first straight line of u1 and the coordinates (xa' i+1,u1 ,ya' i+1,u1 ,za'i+1,u1 ), EB i+1,v1 is the first plane of v1 and the coordinates (xb' i+1,v1 ,yb' i+1,v1 ,zb' i+1,v1 ) between the two sides;
[0011] The u1-th first straight line is a straight line passing through the corresponding position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data and the co-located position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data; u1=1,2,…,U(i+1), U(i+1) is the total number of first feature points in the radar ring of the i+1-th frame point cloud data that has a corresponding relationship with the radar ring of the i-th frame point cloud data;
[0012] The v1th first plane is formed by the corresponding position point and the secondary corresponding position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data, and the co-located position point of the corresponding position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data; v1=1,2,…,V(i+1), V(i+1) is the total number of second feature points in the radar ring of the i+1th frame point cloud data that has a corresponding relationship with the radar ring of the i-frame point cloud data;
[0013]
[0014] Coordinates(xa i+1,u1 ,ya i+1,u1 ,za i+1,u1 ) is the coordinate of the u1th first feature point among the U(i+1) first feature points;
[0015] Coordinates (xb i+1,v1 ,yb i+1,v1 ,zb i+1,v1 ) is the coordinate of the v1th second feature point among V(i+1) second feature points;
[0016] Rotation Matrix
[0017] Among them, the quaternion q i+1 The expression is as follows
[0018]
[0019] Step (C): For the coordinates (x i+1,j,k ,y i+1,j,k ,z i+1,j,k ), and use the following formula to calculate the transformed coordinates (x' i+1,j,k ,y' i+1,j,k ,z' i,j,k )
[0020] (x' i+1,j,k ,y' i+1,j,k ,z' i,j,k ) T =R i+1 ×(x i+1,j,k ,y i+1,j,k ,z i+1,j,k ) T +tr i+1 .
[0021] In the present invention, the data in each frame of point cloud data is very large. In the present invention, the environmental feature points are divided into first feature points and second feature points according to the environmental angle change value, so as to find the first feature points with more obvious features and fewer in number within the environmental range, thereby simplifying the matching of all points in the point cloud data to the matching of the first feature points, thereby greatly reducing the workload and improving the matching speed. After determining the correspondence between the first feature points in the two frames of point cloud data, the correspondence between each second feature point is also determined. By constructing the transformation formula of the first feature point in the two frames and the transformation formula of the second feature point in the two frames, each transformation parameter, that is, the rotation matrix and the translation parameter, can be obtained by solving the error equation, so that the transformation relationship between the two frames can be obtained, including the rotation matrix and the translation parameter, so that the point cloud data of the i+1th frame can be transformed and unified into the coordinate system of the ith frame of point cloud data, so that after obtaining a new frame of point cloud data, the coordinates of the new environmental points can be added to the existing coordinate system, so that it can be updated all the time to obtain the coordinates of the surrounding environment within the radar scanning range. The co-location point of the corresponding position point of the v1th second feature point in the i+1th frame in the point cloud data of the i-th frame is the position point in the i-th frame point cloud data that is on a different ring and in the same column as the corresponding position point. For example, the corresponding position point is the 32nd point of the first radar ring in the i-th frame, then the co-location point can be the 32nd point of other radar rings except the first radar ring in the i-th frame as the co-location point of the 32nd point of the first radar ring.
[0022] In the above technical solution, the method for determining the correspondence between the i+1th frame point cloud data, the radar ring of the ith frame point cloud data, and the environmental position point adopts the following steps (PA) to (PC):
[0023] Step (PA): If the environment position point M i,j,k is the first feature point, then let the environmental position point M i,j,k The corresponding first parameter value P i,j,k =1, otherwise, let P i,j,k =0, construct the first sequence A i,j = {P i,j,1 ,P i,j,2 ,…,P i,j,K};
[0024] Step (PB): Let jb be the preset radar ring serial number, and compare the similarity between each radar ring in the i-th frame point cloud data and the jb-th radar ring in the i+1-th frame point cloud data, specifically including the following steps (PB1) to (PB5):
[0025] Step (PB1): In order of elements, sort sequence A i+1,jb The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i+1,jb,s , thus obtaining the sequence A' i+1,jb = {Q i+1,jb,1 ,Q i+1,jb,2 ,…,Q i+1,jb,K / L};
[0026] If Q i+1,jb,1 =1, then A" i+1,jb =A' i+1,jb , if Q i+1,jb,1 = 0, then through the sequence A' i+1,jb The cyclic order of the elements in is shifted forward, and the sequence A' i+1,jb Located in Q i+1,jb,1 The first non-zero element after that is taken as the first element in the sequence, and the sequence A is obtained. i+1,jb , where Shift(i+1,jb) is sequence A" i+1,jb Relative to sequence A' i+1,jb The number of shifts, if Q i+1,jb,1 =1, then Shift(i+1,jb)=0;
[0027] Step (PB2): In order of elements, sort sequence A i,1 The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i,1,s , thus obtaining the sequence A' i,1 = {Q i,1,1 ,Q i,1,2 ,…,Q i,1,K / L}; Step (PB3): Shift the sequence A' forward by cyclically shifting the elements in the sequence i,1 Each element with a value of 1 in is taken as the first element in the sequence, and BM1(i,1) sequences B are obtained. i,1,1 , B” i,1,2 ,…,B” i,1,BM1(i,1) , BM1(i,1) is the sequence A' i,1 The number of elements with a median value of 1, each of the BM1(i,1) sequences is identical to sequence A" i+1,jbBy comparison, BM1(i,1) similarity values are obtained, wherein the maximum value of the BM1(i,1) similarity values is the similarity between the jb-th radar ring in the i+1-th frame and the 1st radar ring in the i-th frame. By the same method, the similarity between the jb-th radar ring in the i+1-th frame and the 2nd radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 3rd radar ring in the i-th frame, ..., the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame is obtained;
[0028] Step (PB4): comparing the similarity between the jb-th radar ring in the i+1-th frame and the 1-th radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 2-th radar ring in the i-th frame, ..., the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame, and determining that the maximum value corresponds to the similarity value between the jb-th radar ring in the i+1-th frame and the ja-th radar ring in the i-th frame;
[0029] Step (PB5): Calculate the sequence A' i,ja-1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb-1 The similarity of the sequence obtained by moving the elements in the circular order forward by Shift(i+1,jb) positions, and calculating the sequence A' i,ja+1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb+1 The similarity of the sequence obtained by moving the elements in the cyclic order forward by Shift(i+1,jb) positions, if the similarities calculated in this step (PB5) are not less than the preset similarity threshold or not less than the similarity between the jb-th radar ring of the i+1-th frame and the ja-th radar ring of the i-th frame, then it is determined that the j-th radar ring of the i-th frame point cloud data corresponds to the j+jb-ja-th radar ring of the i+1-th frame point cloud data, where Shift(i,ja) is the sequence B" i,ja,1 , B” i,ja,2 ,…,B” i,ja,BM1(i,ja) In the sequence A" i+1,jb The sequence with the highest similarity is relative to sequence A' i,ja The number of shifts;
[0030] Step (PC): Compare the i+1th frame point cloud data and the ith frame point cloud data to obtain the corresponding relationship between the i+1th frame point cloud data and the ith frame point cloud data in the corresponding environmental position points in the radar ring, specifically including the following steps (PC1)-(PC6):
[0031] Step (PC1): Set C i+1,jb The elements in the loop are moved forward by Shift(i+1,jb) positions to obtain the set C'i+1,jb , where the set C i+1,jb ={M i+1,jb,1 ,M i+1,jb,2 ,……,M i+1,jb,K};
[0032] Set C i,ja The elements in the loop are moved forward by Shift(i,ja) positions to obtain the set C' i,ja , where the set C i,ja ={M i,ja,1 ,M i,ja,2 ,……,M i,ja,K};
[0033] Step (PC2): Set C' i+1,jb Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i+1,jb The corresponding K / L subsets are enclosed in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set;
[0034] Set C' i,ja Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i,ja The corresponding K / L subsets are enclosed in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set;
[0035] Step (PC3): Calculate the environmental position point M i+1,jb,k The corresponding rotation angle θA of the rotating 3D laser radar i+1,jb,k , calculate the point M at the environment location i+1,jb,k The rotation angle θB of the rotating three-dimensional laser radar corresponding to the jba-th environmental position point in the first feature set that is on one side of the subset and closest to the subset i+1,jb,k , calculate the point M at the environment location i+1,jb,k The rotation angle θC of the rotating 3D laser radar corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i+1,jb,k ; jba and jbb are both preset values;
[0036] Calculate the environment location point M i,ja,k The corresponding rotation angle θA of the rotating 3D laser radar i,ja,k , calculate the point M at the environment location i,ja,kThe rotation angle θB of the rotating three-dimensional laser radar corresponding to the jba-th environmental position point in the first feature set that is on one side of the subset and closest to the subset i,ja,k , calculate the point M at the environment location i,ja,k The rotation angle θC of the rotating 3D laser radar corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i,ja,k ; jba and jbb are both preset values;
[0037] Step (PC4): Calculate the ratio R i+1,jb,k , where the angle θA i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θB i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θC i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of a calculation formula;
[0038] Calculate the ratio R i,ja,k , where the angle θA i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of the calculation formula, the angle θC i,ja,k Included in the ratio R i,ja,k in the numerator and / or denominator of;
[0039] Step (PC5): If the environment position point M i+1,jb,k is the first feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Zhong and R i+1,jb,k The first feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,k The corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The first feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of
[0040] If the environment position point M i+1,jb,k is the second feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The second feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,kThe corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The second feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of the next; and so on, so that at the environmental position point M i,ja,1 、M i,ja,2 ,……,M i,ja,k Find the environmental position point M i+1,jb,1 、M i+1,jb,2 ,……,M i+1,jb,K , thereby obtaining the corresponding relationship between each environmental position point of the jath radar ring in the i-th frame point cloud data and each environmental position point of the jbth radar ring in the i+1-th frame point cloud data; step (PC6): for the i+1-th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data, repeat steps (PC1)-(PC5) to obtain the corresponding relationship between each environmental position point in the i+1-th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data; preferably, in the step (B), the difference in position sequence number between the environmental position points in the i-th frame point cloud data and the i+1-th frame point cloud data having a corresponding relationship in the radar ring is calculated using Shift(i+1,jb) and Shift(i,ja).
[0041] During the research, the applicant found that there are many environmental position points collected in each radar ring, and the corresponding relationship of the number of radar rings also needs to be determined. In the present invention, a first sequence is constructed, and each point of each radar ring is divided into multiple groups, and the L elements in each group are logically or operated, that is, if there is a first feature point in the group, the logical value of the group is 1, otherwise the logical value is 0, thereby further reducing the workload of comparison in the two frames, that is, only the group with a logical value of 1 needs to be compared. Moreover, in the present application, by cyclically shifting the elements in the sequence forward, the sequence corresponding to a radar ring in the i-th frame and the sequence corresponding to a radar ring in the i+1-th frame are compared to see whether they are similar, so as to quickly obtain the corresponding relationship of the radar rings according to the similarity. After determining the corresponding relationship of the number of rings of the two frames of data, it is necessary to further determine the corresponding relationship of the corresponding points of the i-th frame and the i+1-th frame. By comparing the relationship with the proportional value corresponding to the first feature point, the corresponding relationship of each first feature point in the corresponding ring in the two frames is determined. In order to further reduce the calculation error, the corresponding point and the secondary corresponding point of a point in one frame in another frame are determined.
[0042] In the above technical solution, in the step (PC4):
[0043] and or and or and
[0044] In the present invention, the proportional relationship between the points in two frames of point cloud data is compared, so that the corresponding relationship between the first feature points in the two frames can be conveniently determined.
[0045] In the above technical solution, in the step (PA), the environmental angle change value θ is calculated using the following formula: 1 (i,j,k)
[0046]
[0047] Where m is the length, 2≤m≤15;
[0048] Preferably, Preferably, m=5.
[0049] In the present invention, according to the above calculation method, the error can be eliminated by averaging, so that the calculation of the environmental angle change value is more accurate.
[0050] In the above technical solution, in the step (PA):
[0051] If |α i +ω i,j,k |<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0052]
[0053] If |α i +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0054]
[0055] Among them, m is the preset number, 2≤m≤15, angle α i =2×cos -1 (w i ), the angle between the radar and the horizontal plane ω i,j,k , the rotation angle is
[0056] ψ i+1 , θ i+1 are the yaw angle, roll angle, and pitch angle corresponding to the i-th frame point cloud data respectively;
[0057] Preferably, if |α i +ω i,j,k|<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0058]
[0059] Preferably, if |α i +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0060]
[0061] In the present invention, the applicant discovered during research that if the radar is not scanning horizontally but scanning obliquely, the calculation of the environmental angle change value may have a large deviation, so that a certain environmental position point may be misjudged as an erroneous feature point category. In the present invention, through the angle relationship, it can be determined whether the radar is scanning obliquely. If it is scanning obliquely, the environmental angle change value is calculated after projection, so that the judgment of the feature point is more accurate.
[0062] In the above technical solution, the rotation angle and the axial direction of the rotation axis of the rotary three-dimensional laser radar are measured, and the point cloud data is the three-dimensional coordinates of each environmental position point obtained after correction based on the rotation angle and the axial direction of the rotation axis of the rotary three-dimensional laser radar.
[0063] In the above technical solution, the range of L is 5-15; preferably, L=10.
[0064] In the above technical solution, the preset angle range is [90°-θ 0 , 90°+θ 0 ]; preferably, θ 0 ≤30°.
[0065] The present invention also provides a point cloud matching system for a rotating three-dimensional laser radar, comprising a moving carrier, on which a rotating three-dimensional laser radar and an inertial measurement unit are fixedly arranged, the rotating three-dimensional laser radar having J radar rings, each radar ring having K laser lines, and characterized in that it also includes a control unit, which is configured or programmed to perform the steps of the point cloud matching method for the rotating three-dimensional laser radar.
[0066] Furthermore, the carrier is a person, and the rotating three-dimensional laser radar is held by the person.
[0067] The advantages and positive effects of the present invention are: the feature expression of the present invention is concise, stable and effective, and the real-time performance of the matching method based on the feature expression is significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0068] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.
[0069] Figure 1 It is a schematic diagram of the system structure of the present invention;
[0070] Figure 2 It is a schematic diagram of the method steps of the present invention;
[0071] Figure 3 is a schematic block diagram of a method for point cloud data cleaning steps (P1) to (P4) according to an embodiment of the present invention;
[0072] FIG. 4( a ) and FIG. 4( b ) are top views of the laser ring image before and after the abnormal points are removed according to an embodiment of the present invention, respectively;
[0073] FIG5(a) and FIG5(b) are schematic diagrams of point cloud lines without and with the rotation carrier information respectively;
[0074] Figure 6 is a schematic diagram of point cloud ordering according to an embodiment of the present invention;
[0075] Figure 7 is a schematic diagram of constructing a point cloud line smoothness difference vector according to an embodiment of the present invention, showing the three-dimensional coordinates of an environmental location point;
[0076] Figure 8 is a schematic diagram of smoothness calculation according to an embodiment of the present invention;
[0077] Fig. 9 is a schematic diagram of rotation point cloud feature degradation according to an embodiment of the present invention;
[0078] FIG. 10( a ) and FIG. 10( b ) are respectively a scan line and a corresponding local feature binary image of an embodiment of the present invention;
[0079] Fig.11 is a schematic diagram of a directed annular binary characteristic radar ring according to an embodiment of the present invention; DETAILED DESCRIPTION
[0080] The following will be combined with the drawings of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.
[0081] The present invention makes full use of the orderliness and local linear features of the three-dimensional laser point cloud to create a concise and effective local and global feature expression, and designs its matching algorithm based on this feature expression, so as to make the three-dimensional laser radar point cloud matching more efficient.
[0082] The existing general point cloud features are mostly considered to be disordered point clouds, with problems such as too high data feature dimensions and complex expressions. When used for point cloud matching, the computational complexity is high, which seriously affects the real-time performance of the matching. The present invention makes full use of the structural characteristics of the rotating three-dimensional laser radar to sort the point cloud it generates, and divides the point cloud into corner points (the first feature points in this embodiment) and plane points (the second feature points in this embodiment) according to its local characteristics. According to this characteristic, the three-dimensional laser point cloud line is directed to annular binary encoding, and the encoding can effectively express the global characteristics of the three-dimensional laser radar point cloud. A three-dimensional laser radar point cloud matching method is designed based on plane points, corner points and annular binary encoding. The feature expression of the present invention is concise, stable and effective, and the real-time performance of the matching method based on the feature expression is significantly improved. It can run in real time on embedded devices, thereby providing technical support for laser radar-based ADAS (Advanced Driver Assistance System), automatic driving, indoor and outdoor SLAM, etc.
[0083] The present invention generates point cloud hardware equipment such as Figure 1 As shown. 10 is a rotating three-dimensional laser radar, 101 is the rotation axis of the rotating three-dimensional laser radar, and 102 is the rotating structure of the rotating three-dimensional laser radar. The feature expression and matching method uses the relationship between the laser point cloud points of each line of the rotating multi-line three-dimensional laser radar and the points within a certain range of the point to realize the smoothness estimation of the point cloud line in its extension direction. The normalized smoothness value divides the laser point cloud into two types of feature points: plane points and corner points. After the rotating laser radar point cloud line is segmented in the smoothing domain, its feature identification code is generated using the distribution characteristics of the plane point and corner point features. Assuming that the laser radar carrier motion approaches a uniform speed model or a uniform acceleration model in an instantaneous situation, there must be similar point cloud feature codes between its nearest neighbor point cloud lines. Using this feature, the corresponding points of two frames of point clouds can be locked at one time, thereby avoiding repeated searches for corresponding points of point clouds during point cloud matching, and effectively improving the accuracy and speed of point cloud matching. At the same time, the present invention also has the advantages of low computational complexity, strong adaptability to threshold-insensitive environments, etc., and is convenient for modular integration into various algorithms. It has broad application prospects in the fields of laser SLAM (Simultaneous Localization And Mapping), autonomous driving, laser mapping, etc. The present invention can run in real time on embedded devices and can provide technical support for ADAS, autonomous driving, indoor and outdoor SLAM based on laser radar.
[0084] Example 1
[0085] The present invention provides a point cloud matching method for a rotating three-dimensional laser radar, wherein the rotating three-dimensional laser radar 10 is arranged on a carrier, and the rotating three-dimensional laser radar 10 has J radar lines arranged at intervals in the height direction. When the carrier moves, the rotating three-dimensional laser radar 10 rotates and scans the surrounding environment. The point cloud data of the rotating three-dimensional laser radar 10 obtained in the i-th sampling period is defined as the i-th frame point cloud data, and the data of each environmental position point scanned by the j-th radar line in the i-th frame point cloud data constitutes the j-th radar ring of the i-th frame point cloud data, and the environmental position point M is defined as i,j,k It is the kth environmental position point scanned by the jth radar line in the i-th frame of point cloud data, and the point cloud data is point cloud data with timestamp information and radar line sequence number.
[0086] In this embodiment, the carrier is a person, and the rotating three-dimensional laser radar is held by the person.
[0087] When a person is walking, the rotating three-dimensional laser radar 10 rotates once every period of time (corresponding to a rotation angle of 360°) and the collected data is used as a frame of data. The rotating three-dimensional laser radar 10 rotates once, and each radar line collects 32,000 points. The previous frame of data is matched with the next frame of data, and the coordinates of the newly added environment are adjusted to the original coordinate system, that is, the surrounding environment is determined. During the process of the radar rotating for one frame, the person is also walking, that is, the radar data and the data of the person's walking are combined to unify the various points in a frame with different collection times into the same coordinate system, and a frame of data can be obtained, which is called point cloud correction. A higher-frequency motion sensor such as an imu is used to estimate the instantaneous motion, thereby compensating for the distortion of the point cloud. This process belongs to the prior art and can be understood by those skilled in the art.
[0088] The present invention is divided into two parts, such as Figure 2 As shown. One part is about the point cloud feature expression method of 3D laser radar: it specifically includes four aspects, namely the data cleaning method of the original laser radar point cloud, the method for calculating the line smoothness of the 3D laser radar point cloud, the standardization and normalization method of the eigenvalues, and the 3D laser point cloud feature encoding method. The other part is the point cloud matching method based on the feature expression of the first part: this method specifically includes three aspects, namely the point cloud global coarse correspondence search method based on point cloud feature encoding, the neighbor point fine correspondence search method based on the distribution of plane points / corner points, and the pose transformation solution method based on the first two correspondence information.
[0089] Figure 2The direction of the arrow indicates the steps that need to be taken to implement the functional module. The second part of the invention, point cloud matching, depends on the feature expression of the first part, so the two modules are connected by arrows. However, the second part does not only use the feature encoding of the first part, but also includes the intermediate data of the first part. Strictly speaking, the feature expression produces three types of features, plane points and corner points for local feature classification and directed annular binary encoding for global feature expression.
[0090] The main purpose of the 3D laser radar point cloud data cleaning method is to sort the 3D laser radar point cloud data and remove interference points. The 3D laser radar of the present application has sorted point cloud data, but if the installation carrier of the 3D laser radar of the present application is not stable during movement (for example, when held by a person), the rotational mechanical characteristics of the laser radar installation carrier also need to be considered. Figure 3 As shown, in the data cleaning step, the obtained raw point cloud data is processed to generate a frame of ordered point cloud data for later processing. The data cleaning step includes obtaining the number of rings information (step P1), obtaining the scanning sequence, removing abnormal points (step P2), adding the rotation axis and rotation angle information (step P3), and ordering the point cloud (step P4). Steps P1-P4 are all existing technologies and can be understood by those skilled in the art.
[0091] The radar raw point cloud data is the three-dimensional coordinate data converted from the radar ranging information and is provided by the radar manufacturer.
[0092] Step (P1) Calculate the number of rings
[0093] There are two ways to obtain the ring number information:
[0094] 1) Obtained by the ring number information attached to the original point cloud driven by the radar;
[0095] 2) Estimate the number of rings to which the current point belongs by obtaining the vertical angle value of the local coordinates to which the current point coordinates belong and the laser head emission angle of the radar structure manual (processed before adding the carrier rotation information).
[0096] When the LiDAR manufacturer does not provide the number of rings, the second method is needed to solve the number of rings to which the point cloud belongs. The vertical angle value means: with the center of the LiDAR as the origin, there will be an angle between the line connecting each point and the center point and the horizontal plane. The LiDAR hardware manufacturer will provide a mechanical hardware description of the laser head arrangement angle. When the angle with the horizontal plane is close to or equal to a certain angle of the technical file, it can be inferred that the point is emitted by the laser head at that angle.
[0097] Step (P2) Remove outliers
[0098] The original point cloud is structured according to the scanning order and structural arrangement of the laser radar to facilitate the neighbor indexing of the target point in the same frame point cloud. In this step, the null values and the maximum and minimum values in the point cloud are removed to ensure that the data is within a reasonable range. The occluded point cloud caused by the rotating mechanical structure of the rotating 3D laser radar is removed to avoid boundary judgment and the error boundary is used as the feature value for processing.
[0099] There are two types of abnormal points: one is the point that is interpreted as an abnormal value due to low reflectivity in the environment, and the other is the part blocked by the rotating mechanical mechanism. Both types of points should be removed during sampling. The single laser point cloud line before and after removal is shown in Figure 4(a) and Figure 4(b).
[0100] The reflectivity can be measured by the laser radar itself and is provided by the laser radar manufacturer, which can be understood by those skilled in the art.
[0101] Figures 4(a) and 4(b) are top-view schematic diagrams of the original point cloud sampling value range. As shown in Figure 4(a), assuming that the environment is exactly annular and the space is unobstructed, the laser radar measurement line should be a ring. As shown in Figure 4(b), because a rotating mechanical structure is added to the rotating laser radar, the rotating mechanical structure blocks a part of the laser sampling, so its sampling appears as an arc when it is stationary in the same space. The obscured points will also have measurement data. The obvious feature of this value is that the distance from the point to the center point of the radar is very small. These values are not environmental data, but may be people or objects moving together, which will interfere with the environmental measurement.
[0102] During the movement of the laser radar, any point that maintains a fixed position and distance in its local coordinate system is considered an outlier. Points that are within a certain range of the distance from the holder are considered outliers, for example, the range is set to 1.5 meters.
[0103] The data judged as an outlier point has its position empty in the rough matching stage (step B), and the environmental position point corresponding to the data is considered to be a plane point (i.e., the second feature point), that is, the number of data in each radar ring remains unchanged, and the outlier point is not deleted.
[0104] Step (P3) Adding rotation carrier information
[0105] Measure the rotation angle of the rotating three-dimensional laser radar 10. The point cloud data is the three-dimensional coordinates of each environmental position point obtained after correction based on the rotation angle and rotation axis data of the rotating three-dimensional laser radar 10.
[0106] Figure 3 The purpose of adding the rotating carrier information is to obtain the rotation of the current point around the specified axis at the stationary moment based on the encoder information of the rotating mechanism.
[0107] Assume that the laser radar rotation axis is strictly aligned with the laser radar's X-axis or Y-axis. If it is assumed that the measurement environment is a horizontal wall, and the laser radar's X-axis is perpendicular to the wall, the laser radar rotates at a constant speed around the X-axis driven by the rotation mechanism. Then, by observing and comparing the static and rotating conditions on the wall, the graphs shown in Figure 5(a) and Figure 5(b) can be observed. The rotating carrier information is a built-in function of the carrier that rotates the radar product.
[0108] In the same frame, for the environmental position points collected at different times, there is a rotation problem in this device, so that the coordinates of the environmental position points collected in the same frame are not in the same coordinate system. By adding the rotation carrier information, the coordinates of each environmental position point collected in the same frame are unified in the same coordinate system, so that the origin of the coordinates of each environmental position point in each frame is the same.
[0109] The rotating carrier information includes the motor's rotation angle, rotation axis direction, etc. The rotation axis jitter is given by the laser sensor inside the mechanical rotating structure of the laser radar. The rotation angle can be obtained based on the high-precision grating code disk in the laser radar.
[0110] When the rotation carrier information is not added, the point cloud forms a point cloud line that is approximately straight on the wall, as shown in Figure 5(a). After the rotation information is loaded, the point cloud line turns a larger angle when returning to the origin. After the motor rotates, the laser head can no longer hit the same position after one rotation. In the case of a horizontal laser radar, it deviates from the initial position above or below, forming an irregular curve as shown in Figure 5(b) (taking the center line of the radar point cloud as an example and assuming that the point cloud line is parallel to the radar horizontal plane).
[0111] Add the rotating carrier information, that is, according to the motor rotation angle and the axial information of the rotating shaft, the point cloud data of the radar is corrected. The correction of the rotating carrier information is to unify the laser point cloud under the carrier coordinate system, without changing the angle between the laser plane and the ground. After correction, a frame of point cloud is processed as a whole, that is, the roll angle, pitch angle and yaw angle of each frame of point cloud data are the same.
[0112] The rotation axis is a known quantity determined by the hardware and is introduced as a parameter in the calculation. The angle data is provided by the encoder. The laser radar firmware is driven by the handheld module motor and the angle it rotates relative to the handheld. Figure 1 In the figure, the rotating axis of the rotating three-dimensional laser radar is driven by the internal motor of the rotating structure 102 , and the rotating axis angle, that is, the rotation angle of the rotating axis 101 relative to the rotating structure 102 , is measured by the built-in encoder of the rotating structure 102 .
[0113] Step (P4) Point cloud data sorting
[0114] The ordering of point cloud data is to map the point cloud collected by each frame of the laser radar into a two-dimensional floating-point image with the number of laser radar horizontal scanning points as the width and the radar line number as the height, so as to facilitate the extraction of features such as gradients. Taking a 16-line laser radar as an example, its schematic diagram is as follows Figure 6 The horizontal scanning points and radar line numbers are integers, but the ranging information is a floating point number.
[0115] like Figure 6 As shown in the figure, each point of the point cloud is mapped to a pixel point in a picture-like format, where each element value is a three-dimensional array, which contains the three-dimensional spatial coordinate value of the point cloud after loading the rotation information. Its binary index (x, y) coordinates represent the orientation information of the current point in the scanning frame, and x and y represent the number of rings (i.e., j value) and the number of horizontal scanning points (i.e., k value) respectively. For any point, its neighbors must be within the range of the index coordinate (x, y) [±1, ±1]. This feature can facilitate the subsequent calculation of the smoothness of the point cloud line.
[0116] The scanning timing of the point cloud can be obtained by the order in which the LiDAR driver reads the point cloud packets. The driver will timestamp each packet it reads. The points in the packet are obtained strictly in the scanning order. Time interpolation is performed on these points to obtain the time series of each point. That is, if the timestamping speed cannot keep up with the packet reading speed, the untagged packets can be averaged by the labeled packets before and after them. This parameter will be used as one of the important parameters for later ordering.
[0117] Steps (PA):
[0118] Determine the environment position point M based on the i-th frame point cloud data i,j,k The corresponding horizontal environmental angle change value θ 1 (i, j, k) is within the preset angle range. If the judgment result is yes, the environmental position point M is determined. i,j,k is the first feature point, otherwise, determine the environmental position point M i,j,k is the second feature point, j = 1, 2, ..., J, k = 1, 2, ..., K, K is the number of environmental position points scanned by each radar line in each sampling period; if the environmental position point M i,j,k is the first feature point, then let the environmental position point M i,j,k The corresponding first parameter value P i,j,k =1, otherwise, let P i,j,k =0, construct the first sequence A i,j = {P i,j,1 ,P i,j,2 ,…,P i,j,K}.
[0119] The preset angle range may be [90°-θ 0, 90°+θ 0 ]. Preferably, θ 0 =30°. Because in this application, the calculated environmental angle change value θ 1 (i, j, k) is the average of multiple values, and the radar measurement itself has errors. Even in an urban environment, the corner position is not an absolute right angle, and there are a large number of obtuse corners. Therefore, the preset angle range is 90°±30, which can adapt to most situations.
[0120] In this step, it is determined whether the environment in the horizontal direction has undulations, that is, whether the undulations form convexities or concavities (such as convexities or concavities formed at the corners of walls or wall corners), or whether there are no convexities or concavities and they are relatively gentle. If it is determined that there are undulations forming convexities or concavities and they are within the preset angle range, they are determined as the first feature points, and points on the plane or points with smaller convexities or concavities are determined as the second feature points.
[0121] Use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0122]
[0123] Wherein, m is a first number, and 2≤m≤15. Preferably, m=5. The preset angle range angle threshold is preferably 90°±30° (60° and 120° may be included in the range), and other angle thresholds may also be set according to actual environment.
[0124] In this step, the relative distance and angle relationship between the target point and its neighbors is used to determine the smoothness of the LiDAR line. The innovation of this processing method is that the local feature expression is not affected by the environmental scale relationship, avoiding the problem of uncertainty in the range of feature values, which is more conducive to the promotion and utilization of the method. In the formula, "." represents the dot product.
[0125] The three-dimensional rotating laser radar point cloud line smoothness calculation is specifically a method for judging the degree of tortuosity of the point environment by different trends of the local point cloud lines. The specific implementation is as follows.
[0126] The smoothing domain of the point cloud is determined according to the difference between the number of rings, the width of the point cloud line and the height. The basis for determination is "to ensure that the angle domain of the horizontal line is an integer multiple of its vertical resolution". Taking a 16-line laser radar as an example, its vertical field of view is 30° divided into 16 lines, so its vertical resolution is 30 / 15=2°. The horizontal resolution is 360 / 2000=0.18°, so when 10 to 11 points are taken, the horizontal line angle domain is 1.8°~1.98°, which is close to 1 times 2° (that is, m=5). When its vertical resolution is higher, a higher magnification such as a 32-line 64-line laser radar can be used.
[0127] At the same time, the calculation and difficulty of analyzing the entire point cloud are very high, so the method of analyzing each point in the smooth domain is adopted. If the smooth domain is too small, it will be impossible to obtain effective point cloud structure information for a single point, and if it is too large, the calculation complexity will increase. The compromise method is to take a small square that contains a certain number of rings and a certain number of columns. A small square with a ring above and below the current point and a left and right width equal to the width of the number of rings is a better choice. The number of rings and columns can be adjusted during use.
[0128] The smoothing value is calculated according to the determined smoothing domain. The specific operation is to take the current point (M i+1,j,k ) on both sides of the half domain (one side is M i+1,j,k-1 、M i+1,j,k-2 ,……,M i+1,j,k-s , the other side is M i+1,j,k+1 、M i+1,j,k+2 ,……,M i+1,j,k+s ) form the difference vector value. Calculate the vector difference between the two difference values and calculate the mean. The difference vector is constructed as follows Figure 7 shown.
[0129] Figure 7 The solid black point in the middle is the point for which smoothness needs to be calculated, and the gray point is the point in its smooth domain. The points pointed by the two arrows in the figure are the symmetrical smooth domain points. Taking these two points and the current point, a pair of difference vectors can be constructed. And the angle between the pair of vectors can be calculated, and the θ value is a smoothing factor of the current smoothness. The smooth domain refers to the point cloud block size centered on this point to be used to analyze the smoothness of the current point. Use the 1st to 11th points to calculate the angle of the 6th point, use the 2nd to 12th points to calculate the angle of the 7th point, and so on.
[0130] According to the method of obtaining the difference vector and smoothing factor of B, all the difference vectors and smoothing factors in the smoothing domain of the point are calculated in turn, and the average value of the smoothing factor is calculated according to the number of all smoothing factors in the smoothing domain of the point. The average value is the smoothness estimate of the point.
[0131] According to the above steps, the smoothness value of all points in the current frame is calculated. When the point cloud belongs to the edge of the ordered image and is less than half of the smooth domain (i.e. the point is close to the boundary), the smoothness value is no longer calculated, and the point is retained as an outlier. Retention can be used to increase the integrity of the map, which is beneficial for later use of the map for modeling or positioning.
[0132] The specific implementation of the standardization and normalization of the eigenvalues of the three-dimensional rotating laser radar point cloud is as follows: when the smoothness (i.e., the environmental angle change value θ 1 (i,j,k)) after that, you only need to select a segmentation domain value (i.e., the angle threshold θ 0 ), points with different smoothness can be divided into two categories. Figure 8 It can be seen that θ 1 The value range of (i,j,k) is 0-180 degrees. The closer the angle is to 180 degrees, the smoother the local features are. 1 The closer the value of (i,j,k) is to 0 degrees, the more tortuous its local features are. 1 (i,j,k) values, but directly use cosθ 1 If the (i,j,k) value is used to characterize the feature, its value is [-1,1], and the zero point of its positive and negative boundary is 90 degrees. 0 degrees refers to the sharp corner part in the environment.
[0133] It can be expressed as:
[0134]
[0135]
[0136] In this step, the environmental position point M is determined i,j,k The corresponding horizontal environmental angle change value θ 1 (i,j,k)<θ 0 Is it true, or judge cosθ 1 (i,j,k)>cosθ 0 If any of the above two equations is true, then the decision point M i,j,k In the horizontal direction, it is the first feature point. Otherwise, the determination point M i,j,k The second feature point is in the horizontal direction.
[0137] After the smoothness estimation is completed, a suitable threshold is set to classify all the laser radar points with smoothness estimation into two categories to obtain plane points and corner points. Assuming that the environment is indoor or building scanning, the points with a smoothness angle less than 90 degrees (i.e., points with a smoothness value greater than 0) can be simply regarded as corner points, so that a binary image mapping can be simply realized for the ordered point cloud. Fig. 9 Taking the scene shown as an example, assuming that the radar is scanning horizontally, the point cloud in this environment will locally generate image features as shown in Figures 10(a) and 10(b).
[0138] The threshold for dividing plane points and corner points is automatically set according to the distribution of the values calculated by smoothness. Each point of the point cloud line is classified as a plane point or a corner point. The rotating laser radar point cloud spiral is divided into n equal parts (n is a positive integer greater than 1 and equal to the number of small cloud point cloud line points), and according to the proportion of corner points or plane points in each partition, the area is set to 0 or 1 (0 or 1 represents the drastic change of the outer surface of the current local space), thereby generating a directed annular binary radar ring composed of 0 or 1 (its direction is the scanning direction). This radar ring represents the curvature of the current point-to-line space and can be widely used in point cloud search and segmentation.
[0139] In the present invention, in the gradient descent optimization algorithm, a good estimate is used to reduce the operation time and improve the result accuracy. All the calculated estimates are processed as initial values during optimization, and the final result is calculated on the initial values. The more accurate the estimate, the better the result.
[0140] Step (PB): Compare the similarity of two frames of point cloud data
[0141] Let jb be the preset radar ring number. The value of jb can be determined according to actual needs, for example, jb can be 1 or J. Preferably, jb = J / 2 or a value close to J / 2. By setting it in this way, even if the next frame shakes slightly more up and down relative to the previous frame, the corresponding radar ring can be found quickly.
[0142] Comparing the similarity between each radar ring in the i-th frame of point cloud data and the jb-th radar ring in the i+1-th frame of point cloud data specifically includes the following steps (PB1) to (PB5):
[0143] Step (PB1): In order of elements, sort sequence A i+1,jb The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i+1,jb,s , thus obtaining the sequence A' i+1,jb = {Q i+1,jb,1 ,Q i+1,jb,2 ,…,Q i+1,jb,K / L};
[0144] If Q i+1,jb,1 = 0, then through the sequence A' i+1,jb The cyclic order of the elements in is shifted forward, and the sequence A' i+1,jb Located in Q i+1,jb,1 The first non-zero element after that is taken as the first element in the sequence, and the sequence A is obtained. i+1,jb , and Shift(i+1,jb) is sequence A" i+1,jb Relative to sequence A' i+1,jb The number of shifts, if Q i+1,jb,1=1, then Shift(i+1,jb)=0.
[0145] For example, if A i+1,jb ={0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0}, let L = 5, then A" i+1,jb =A' i+1,jb ={1,0,1,0,1}.
[0146] The range of L is 5-15; preferably, L=10. The three-dimensional laser radar point cloud line is divided into feature dimensions, and the division standard here is that the number of point clouds contained in each dimension (each group) cannot be less than the number of point clouds in the smoothness estimation smooth domain. Take a 16-line laser radar as an example. In the smoothness estimation, the smooth domain is 10-11 points. Here, 10 points are taken as the minimum unit of dimension division, so any scanning radar line can be divided into 200 dimensional intervals (each radar line of the 16-line laser radar contains 2000 laser points). The minimum unit of dimension can be adjusted dynamically as long as it is lower than the point cloud resolution. Arrange the feature dimension in the specified direction. In the 16-line laser radar, take the clockwise or counterclockwise direction of the X-axis. If black represents the existence of corner points and white represents the absence of corner points, and they are connected end to end, then it appears as a black and white ring. The schematic diagram is as follows Fig.11 shown.
[0147] In this application, protrusion and concavity can be considered to have the same degree of change in the algorithm. Fig.11 Shown is the 2D clockwise feature encoding of a 3D laser scan line.
[0148] Step (PB2): In order of elements, sort sequence A i,1 The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i,1,s , thus obtaining the sequence A' i,1 = {Q i,1,1 ,Q i,1,2 ,…,Q i,1,K / L}.
[0149] Step (PB3): Shift the sequence A' forward by cyclically shifting the elements in the sequence i,1 Each element with a value of 1 in is taken as the first element in the sequence, and BM1(i,1) sequences B are obtained. i,1,1 , B” i,1,2 ,…,B” i,1,BM1(i,1) , BM1(i,1) is the sequence A' i,1 The number of elements with a median value of 1, each of the BM1(i,1) sequences is identical to sequence A"i+1,jb By comparison, BM1(i,1) similarity values are obtained, the maximum value of which is the similarity between the jb-th radar ring in the i+1-th frame and the 1st radar ring in the i-th frame. In the same way, the similarity between the jb-th radar ring in the i+1-th frame and the 2nd radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 3rd radar ring in the i-th frame, and so on, the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame is obtained.
[0150] If A' i,1 =={0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0}, let L=5, then A' i,1 ={1,0,1,0,0}. Since A' i,1 There are 2 elements that are 1, so BM1(i,1)=2, which is sequence B" i,1,1 ={1,0,1,0,0}, B” i,1,2 ={1,0,0,1,0}.
[0151] Step (PB4): comparing the similarity between the jb-th radar ring in the i+1-th frame and the 1-th radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 2-th radar ring in the i-th frame, ..., the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame, and determining that the maximum value corresponds to the similarity between the jb-th radar ring in the i+1-th frame and the ja-th radar ring in the i-th frame;
[0152] Step (PB5): Calculate the sequence A' i,ja-1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb-1 The similarity of the sequence obtained by moving the elements in the circular order forward by Shift(i+1,jb) positions, and calculating the sequence A' i,ja+1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb+1 The similarity of the sequence obtained by moving the elements in the cyclic order forward by Shift(i+1,jb) positions, if the similarities calculated in this step (PB5) are not less than the preset similarity threshold or not less than the similarity between the jb-th radar ring of the i+1-th frame and the ja-th radar ring of the i-th frame, then it is determined that the j-th radar ring of the i-th frame point cloud data corresponds to the j+jb-ja-th radar ring of the i+1-th frame point cloud data, where Shift(i,ja) is the sequence B" i,ja,1 , B” i,ja,2 ,…,B” i,ja,BM1(i,ja) In the sequence A" i+1,jbThe sequence with the highest similarity is relative to sequence A' i,ja The number of shifts. i,ja Each element with a value of 1 in is taken as the first element in the sequence, and BM1(i,ja) sequences B are obtained. i,ja,1 , B” i,ja,2 ,…,B” i,ja,BM1(i,ja) , BM1(i,ja) is the sequence A' i,ja The number of elements whose median value is 1.
[0153] jb is a preset constant and ja is a variable.
[0154] The preset similarity threshold is not less than the similarity between the jbth radar ring in the i-th frame and the jath radar ring in the i+1-th frame, or the preset similarity threshold is not less than 70%.
[0155] For example, if jb=1 and K=16, the 1st to 16th radar rings of the i+1th frame are compared with the 1st radar ring of the i-th frame respectively. If the 3rd radar ring of the i+1th frame has the highest similarity with the 1st radar ring of the i-th frame, the 4th radar ring of the i+1th frame is compared with the 2nd radar ring of the i-th frame. If the obtained similarity is not less than the preset similarity threshold, it can be determined that the 3rd, 4th, ..., 16th radar rings of the i+1th frame correspond to the 1st, 2nd, ..., 14th radar rings of the i-th frame respectively.
[0156] In this application, two corresponding radar rings refer to the radar rings in another frame that are most similar to a radar ring in one frame. Since a new frame of radar rings will collect new data during the moving process, the data of the two corresponding radar rings are not exactly the same.
[0157] Similarity estimation of directed annular binary radar rings. Directed annular binary coding is a high-dimensional feature binary feature vector. Each point cloud line generates its own specific binary radar ring. Its similarity estimation method is used to evaluate the similarity of two beams of point cloud lines. The specific implementation is a head-end alignment consistency judgment method. After the two radar rings are aligned at the head end, the redundant data at the tail end is cut off, and the same or operation is performed. The ratio of the count of the returned true value to the length of the entire radar ring represents the proximity of the two beams of point cloud lines. It is called precise correspondence search here because adjacent point cloud lines may have radar ring expressions with high similarity.
[0158] The core of the feature-encoding-based global point cloud rough correspondence search method is to determine the similarity of two directed annular binary codes. The specific method is a statistical method based on cyclic matching of identical elements. Here, the currently collected point cloud is defined as the "current frame" (i+1 frame), and the point cloud collected at the previous moment is defined as the "target frame" (i frame). The goal of the feature-encoding-based global point cloud rough correspondence method is to find the closest laser point cloud line in the "target frame" to each laser point cloud beam feature in the "current frame". The algorithm flow is shown below.
[0159] a) Read in all radar rings in the “target frame”;
[0160] b) Read in all radar rings in the “current frame”;
[0161] If the corresponding corner points are located at different positions in the "target frame" radar ring and the "current frame" radar ring, the comparison will consider the two rings different.
[0162] c) For each directional binary radar ring in the "current frame", move the first dimension with a value of 1 to the front according to the encoding direction. The values in front of this dimension are added to the back end of the current radar ring in turn to ensure that the radar ring dimensions are consistent.
[0163] d) For a directional binary radar ring passing through c), select a radar ring in the "target frame" to perform similarity judgment with it.
[0164] e) For the radar ring selected in d), move the first dimension with a value of 1 to the front according to the encoding direction. The values in front of the dimension are added to the back end of the current radar ring in sequence to ensure the consistency of the radar ring dimension. And perform an "AND or" operation on the adjusted radar ring and a certain directional binary radar ring passing through c). The number of true values is the similarity judgment between the specified feature ring of the "current frame" and the specified ring of the "target frame" under the current adjustment.
[0165] f) Move the second dimension of the radar ring selected in d) with a value of 1 to the front. The values in front of this dimension are added to the back end of the current radar ring in sequence to ensure that the radar ring dimensions are consistent. Perform the similarity judgment in e) again.
[0166] g) Repeat e) and f) until the true value dimensions of all radar rings selected in d) are traversed. Compare all similarities and take the maximum value as the final similarity value between a certain directional binary radar ring passing through c) and the radar ring selected in the "target frame".
[0167] h) Repeat d) to g) until all radar rings in c) are calculated. Finally, sort the laser points of the "current frame" and "target frame" according to their similarity to give the corresponding results. When there is a one-to-many situation in the sorting, the closest ring number rule is adopted, and the corresponding relationship with a large difference in the number of rings is discarded.
[0168] i) Save and output the laser point cloud line correspondence between the "current frame" and the "target frame".
[0169] Step (PC): Determine the correspondence between each environmental position point of the corresponding radar ring. Compare the i+1th frame point cloud data and the ith frame point cloud data to obtain the correspondence between the i+1th frame point cloud data and the ith frame point cloud data in the corresponding radar ring environmental position point, specifically including the following steps (PC1)-(PC6):
[0170] Step (PC1): Set C i+1,jb The elements in the loop are moved forward by Shift(i+1,jb) positions to obtain the set C' i+1,jb , where the set C i+1,jb ={M i+1,jb,1 ,M i+1,jb,2 ,……,M i+1,jb,K};
[0171] Set C i,ja The elements in the loop are moved forward by Shift(i,ja) positions to obtain the set C' i,ja , where the set C i,ja ={M i,ja,1 ,M i,ja,2 ,……,M i,ja,K}.
[0172] Step (PC2): Set C' i+1,jb Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i+1,jb The corresponding K / L subsets are enclosed in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set;
[0173] Set C' i,ja Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i,ja The corresponding K / L subsets are surrounded in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set.
[0174] Step (PC3): Calculate the environmental position point M i+1,jb,k The corresponding rotation angle θA of the rotating three-dimensional laser radar 10 i+1,jb,k , calculate the point M at the environment location i+1,jb,kThe rotation angle θB of the rotating three-dimensional laser radar 10 corresponding to the jba-th environmental position point in the first feature set which is on one side of the subset and closest to the subset i+1,jb,k , calculate the point M at the environment location i+1,jb,k The rotation angle θC of the rotating three-dimensional laser radar 10 corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i+1,jb,k ; jba and jbb are both preset values.
[0175] For example, the set C' i+1,jb The first subset is the second feature set, the second subset is the first feature set, the first point in the second subset (for example, the kath point of the jbth radar line scan) is the first feature point, the third subset is the second feature set, the fourth subset is the first feature set, and the K / Lth subset is the first feature set. Then the environment position point M i+1,jb,ka The first feature set on the side of the subset (i.e., the second subset) and closest to the subset is the K / Lth subset, located at the environmental position point M i+1,jb,ka The first feature set on the other side of the subset (i.e., the second subset) and closest to the subset is the fourth subset, i.e., θB i+1,jb,ka is the rotation angle of the rotating three-dimensional laser radar 10 corresponding to the jba-th environmental position point of the K / L-th subset, θC i+1,jb,ka It is the rotation angle of the rotating three-dimensional laser radar 10 corresponding to the jbb-th environmental position point in the 4th subset.
[0176] Calculate the environment location point M i,ja,k The corresponding rotation angle θA of the rotating three-dimensional laser radar 10 i,ja,k , calculate the point M at the environment location i,ja,k The rotation angle θB of the rotating three-dimensional laser radar 10 corresponding to the jba-th environmental position point in the first feature set which is on one side of the subset and closest to the subset i,ja,k , calculate the point M at the environment location i,ja,k The rotation angle θC of the rotating three-dimensional laser radar 10 corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i,ja,k jba and jbb are both preset values. jba is preferably 1 or L, and jbb is preferably 1 or L.
[0177] Step (PC4): Calculate the ratio R i+1,jb,k , where the angle θA i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θB i+1,jb,k Included in the ratio Ri+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θC i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of a calculation formula.
[0178] Calculate the ratio R i,ja,k , where the angle θA i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of the calculation formula, the angle θC i,ja,k Included in the ratio R i,ja,k in the numerator and / or denominator of .
[0179] Step (PC5): If the environment position point M i+1,jb,k is the first feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The first feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,k The corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The first feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of
[0180] If the environment position point M i+1,jb,k is the second feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Zhong and R i+1,jb,k The second feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,k The corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The second feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of the next; and so on, so that at the environmental position point M i,ja,1 、M i,ja,2 ,……,M i,ja,k Find the environmental position point M i+1,jb,1 、M i+1,jb,2 ,……,M i+1,jb,K The corresponding position point and the second corresponding position point are obtained, so as to obtain the corresponding relationship between each environmental position point of the ja-th radar ring in the i-th frame point cloud data and each environmental position point of the jb-th radar ring in the i+1-th frame point cloud data.
[0181] Step (PC6): for the i+1th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data, repeat steps (PC1)-(PC5), so as to obtain the corresponding relationship between each environmental position point in the i+1th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data;
[0182] Preferably, in the step (B), the difference between the position numbers of the corresponding environmental position points in the i-th frame point cloud data and the i+1-th frame point cloud data in the radar ring is calculated using Shift(i+1,jb) and Shift(i,ja).
[0183] This step is a step of searching for neighbor points based on the distribution of plane points / corner points. Specifically, after finding two radar rings with the highest similarity, the method determines which corner points and plane points of the corresponding radar rings in the i-th frame point cloud data correspond to the plane points and corner points of the i-th frame point cloud data according to the proportional relationship between the coding features. From the point of view of the target point cloud (referring to the i-th frame point cloud), this is a one-to-many relationship. Any point in the radar ring of a specified target point cloud must have multiple similar feature points (i.e., the first feature points or the second feature points) as its neighbors in the radar ring corresponding to the current frame (referring to the i+1 frame point cloud) according to its classification (plane point / corner point). Its nearest neighbor is not determined by the distance value under the current transformation estimation, but is confirmed by the proportion of the target point in the specified feature coding segment and the proportional relationship of the current point in the similar coding segment. The one with the closest coding is the nearest neighbor. When there are two (at most two) similar nearest neighbors in the i-th frame and the i+1-th frame, one of them is selected at random.
[0184] The feature-based point cloud precise correspondence search is specifically implemented by further determining the correspondence between neighboring points of the "current frame" and the "target frame" based on the line correspondence in step (PB). The specific steps are as follows.
[0185] a. Read the point cloud and its feature attributes of the "target frame".
[0186] b. Read the point cloud and its feature attributes of the "current frame".
[0187] c. Read the line correspondence between the "current frame" and the "target frame" (referring to the correspondence of the radar ring obtained previously) and its corresponding transformation. The transformation refers to the number of positions that the target feature ring moves forward or backward in a cyclic sequence when performing line correspondence. Its essence is the estimation of the rotation angle between the two point clouds.
[0188] d. For any point in the "current frame", calculate the proportional relationship between the two corner points according to the corresponding attributes of its features and the sorting position of the ordered points. Note: In the annular feature encoding, there must be corner point blocks on the left and right of any corner point. Suppose the laser head rotation angle corresponding to the left corner point block is A, the laser head rotation angle corresponding to the right corner point block is B, and the laser head rotation angle corresponding to the current corner point is C. The position relationship between the current corner point and the two corner points is expressed in proportion as (CA) / (BA) or (B+C) / (A), (AC) / (AB). A refers to the first point clockwise or counterclockwise of the left corner point block, as long as the direction is consistent. C refers to the first point clockwise or counterclockwise of the right corner point block, as long as the direction is consistent.
[0189] The rotation angle of the laser head corresponding to the current corner point is C, and the posture relationship between the current corner point and the two corner points is expressed as (CA) / (BA)
[0190] e. For any point in the "current frame", the proportional relationship is calculated according to d. Under the transformation of the "target frame", the feature point that is closest to the proportional relationship described in d can be found. This feature point is the feature nearest neighbor corresponding point between the two frame point clouds, and the second nearest neighbor point is saved for subsequent use. There may be multiple corner points in each corner point block, and the ratios of these corner points to the left and right corner point blocks are not necessarily the same. The two points with the closest ratios are taken as corresponding points. When there are multiple corner points in the corner point block, the point with the closest ratio to the feature corresponding point of the target frame is the closest point, and the point with a relatively close ratio to the target frame is the second nearest neighbor point. The so-called second nearest neighbor point here is the point with the second closest ratio. If the ratios are consistent (uncommon), any point in the consistency is selected.
[0191] Assume that there must be similar feature distributions in adjacent positions of the previous and next frames. Assume that the radar rotates exactly one feature dimension between the two frames. Then, when performing radar ring correspondence, the current frame or the target frame needs to move the radar ring forward or backward to make the two correspond.
[0192] f. Repeat steps d to e until all feature points in the "target frame" correspond to the feature points of the "current frame".
[0193] Constructing the error equation Solve the error equation and get the i+1th translation parameter tr i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T , Roll Angle Pitch angle θ' i+1 ;
[0194] in:
[0195] EA i+1,u1 is the first straight line of u1 and the coordinates (xa' i+1,u1 ,ya' i+1,u1 ,za' i+1,u1 ), EB i+1,v1 is the first plane of v1 and the coordinates (xb' i+1,v1 ,yb' i+1,v1 ,zb' i+1,v1 ) between the two sides;
[0196] The u1-th first straight line is a straight line passing through the corresponding position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data and the co-located position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data; u1=1,2,…,U(i+1), U(i+1) is the total number of first feature points in each radar ring in the i+1-th frame point cloud data that has a corresponding relationship with the radar ring of the i-th frame point cloud data. For example, if the corresponding position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data is the UB-th environmental position point UC scanned by the UA-th radar line in the i-th frame point cloud data, then the co-located position point of point UC is the UB-th environmental position point scanned by any radar line other than the UA-th radar line in the i-th frame point cloud data.
[0197] The v1th first plane is formed by the corresponding position point and the secondary corresponding position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data, and the co-located position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data; v1=1,2,…,V(i+1), V(i+1) is the total number of second feature points in each radar ring in the i+1th frame point cloud data that has a corresponding relationship with the radar ring of the i-frame point cloud data. For example, if the corresponding position point of the v1th first feature point in the i+1th frame in the i-frame point cloud data is the VBth environmental position point VC scanned by the VAth radar line in the i-frame point cloud data, then the co-located position point of point VC is the VBth environmental position point in the i-frame point cloud data scanned by any radar line other than the VAth radar line.
[0198]
[0199] Coordinates(xa i+1,u1 ,ya i+1,u1 ,za i+1,u1 ) is the coordinate of the u1th first feature point among the U(i+1) first feature points;
[0200] Coordinates (xb i+1,v1 ,yb i+1,v1 ,zb i+1,v1) is the coordinate of the v1th second feature point among V(i+1) second feature points;
[0201] Rotation Matrix
[0202] Among them, the quaternion q i+1 The expression is as follows
[0203]
[0204] ψ' i+1 , θ' i+1 The yaw angle, roll angle, and pitch angle of the i-th frame point cloud data transformed to the i+1-th frame point cloud data, and the yaw angle ψ' i+1 According to Shift i+1,jb , Shift i,ja , the rotation parameters of the rotating three-dimensional laser radar 10 are calculated, and the translation parameters tr i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T .
[0205] This step is a method for solving the pose transformation of adjacent frames based on feature search, and is an inter-frame transformation solution based on least squares implementation based on steps (PB) and (PC).
[0206] The point-to-line distance and the point-to-plane distance are used as the residuals of nonlinear optimization. And because feature encoding is used, all neighbor searches are implemented through binary radar ring characteristics. Because there is no need to use the multidimensional tree search method in traditional point cloud matching, this matching method can further improve the matching speed.
[0207] The pose transformation parameter between two adjacent frames (i+1 frame, i frame) is a six-dimensional vector in ψ' i+1 ,θ' i+1 Indicates the rotation parameters between two frames. Here, they are Euler angles (including roll angle, pitch angle, and yaw angle), and dx i+1 ,dy i+1 、dz i+1 Represents the translation vector between two frames. The optimization method for solving large pose transformations has better stability when the position transformation is small. Here, the line correspondence is used to first make a preliminary estimate of some parameters of the transformation, and then a nonlinear optimization method is used for further precise solution.
[0208] Before completing the one-to-one correspondence, two transformations are performed, in which "Transformation 1" is a horizontal or vertical mapping of the feature, and "Transformation 2" is a circular forward shift of the true value of the annular feature in step (PB). When the correspondence between the two laser point cloud lines is determined, the two mapping parameters mentioned above are essentially included. Among them, "Transformation 2" represents the yaw relationship between two adjacent frames. Based on the yaw of "Transformation 2" and the angle difference between the horizontal or vertical mapping of the two features of "Transformation 1", the change of the transformation parameters between the two frames can be determined.
[0209] The horizontal and vertical projection formulas, the projection angle calculation itself already includes the roll and pitch information. When the axial direction of the rotation axis relative to the IMU is known, it is generally aligned with the X-axis or Y-axis. At this time, the angle of rotation of the rotation axis relative to the IMU must be pitch or roll. At this time, the other angle remains unchanged, and the pitch or roll plus the rotation angle of the rotation axis is the current pitch or roll angle. That is, if the rotation axis is aligned with the Y-axis of the IMU, the roll angle remains unchanged, and the pitch angle is added or subtracted from the rotation angle of the motor; if the rotation axis is aligned with the X-axis of the IMU, the pitch angle remains unchanged, and the roll angle is added or subtracted from the rotation angle of the radar's rotating mechanism, thereby obtaining the transformation parameters of the i-th frame and the i+1-th frame relative to the initial moment of the IMU, respectively. The two moment parameters are expressed using quaternions, and are set to be q respectively. i and q i+1 , then the transformation between the two frames is q i -1 *q i+1 (-1 means inverse), that is, R i+1 For R i The product of the transformation between the two frames. i+1 The expression of q i The projection angle calculation itself already includes the roll and pitch information, that is, how the roll and pitch are obtained through the IMU and the rotation angle of the handheld terminal 102.
[0210] The specific implementation process of the precise transformation estimation method based on point correspondence is as follows.
[0211] a. According to the rough correspondence, the “current frame” is rotated to further reduce the difference with the “target frame”.
[0212] b. For any corner point of the "current frame", find its corresponding point in the "target frame", as well as the points with the same position and eigenvalue on different ring numbers of the corresponding point. A straight line can be constructed based on the corresponding points and the points with the same position and eigenvalue in the "target frame". The distance between the "current frame" point and the constructed straight line is calculated to generate a residual equation.
[0213] c. For any plane point in the "current frame", find its corresponding point and sub-corresponding point in the "target frame", and randomly select a point with the same eigenvalue in the same region on different ring numbers. A plane can be constructed based on the corresponding points, sub-corresponding points and points with the same eigenvalue in the region in the "target frame", and the distance between the point in the "current frame" and the constructed plane is calculated to generate a residual equation. It can be understood as taking three points on different lines on the plane to form a plane. The optimization goal is to minimize the distance from the point to the plane.
[0214] d. List all equations b and c and minimize their residuals. This is essentially a least squares optimization problem, which can be solved using the Gauss-Newton method and the Levenberg-Marquardt method. The parameters obtained are the transformation parameters between two adjacent frames. For details, please refer to the fourth edition of "Introduction to Optimization" (by Edwin KPChong Stanislaw H.Zak), Part II, Unconstrained Optimization Problems, Chapter 9. For examples of algorithm implementation, please refer to Google's open source nonlinear optimization library ceres or Gtsam.
[0215] After the previous processing, two corresponding data sets are obtained, namely the corresponding data set of corner point features and corner point features and the corresponding data set of plane point features and surface point features. The corner point feature point set of the target frame is defined as Tpc{} and the plane point feature point set is defined as Tps{}. The corner point feature point set of the current frame is defined as Cpc{} and the plane point feature point set is defined as Cps{}. The feature points of the current frame and the target feature points have been matched in the above content. The goal of the optimization solution is to find the transformation parameter Euler angle, which is defined as ψ' i+1 ,θ' i+1 and translation parameter tr i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T , so that after the current frame is transformed by the above parameters, the line distance formed by the corner point features of the current frame to the corner point features of the target frame is minimized, and the plane distance formed by the plane features of the current frame to the plane features of the target frame is minimized.
[0216] Because the inertial measurement unit has cumulative bias, the "Accurate Transformation Estimation" error variance construction will also solve the rotation angle parameters ψ' i+1 ,θ' i+1 and translation parameter tr i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T In one frame, each sampling point has the same roll angle, pitch angle, and yaw angle.
[0217] Assume that the rotation function between two frames is R, where R is the variable ψ' i+1 ,θ' i+1 Composition function.
[0218] Then the transformation from the current frame to the target frame can be expressed as.
[0219] tp=G(R,t)=R×cp+t(0.1)
[0220] G(R,t) is a function of R and t, cp is the feature point of the current frame. Let the distance from the transformed point of the current frame to the corresponding feature of the target frame be the residual e, that is, the distance value d.
[0221] e=d=D(G(R,t))(0.2)
[0222] According to the above two formulas, the residual is
[0223] e=d=D(G(R,t))=D(R×cp+t)(0.3)
[0224] The expression of R is shown in formula (1.1). The solution of D(G(R,t)) varies depending on the feature. If it is a corner feature, the distance to the line composed of corresponding points is calculated. If it is a plane feature, the distance to the plane composed of corresponding points is calculated.
[0225] Each feature point generates a residual equation of formula 1.4. The optimization problem model is
[0226]
[0227] Taking Newton's method as an example, it is necessary to solve the first-order derivative of the model (the Jacobian matrix for multi-dimensional variables), that is.
[0228]
[0229] in Similarly, the partial derivatives of other variables can be obtained. Substitute Newton's method and iterate, specify the number of iterations and the termination condition, and complete the solution when the condition is met. Step (C): For the coordinates (x i+1,j,k ,y i+1,j,k ,z i+1,j,k ), use the following formula to calculate the transformed coordinates (x' i+1,j,k ,y' i+1,j,k ,z' i,j,k ) T =R i+1 ×(x i+1,j,k ,y i+1,j,k ,z i+1,j,k )T +tr i+1 .
[0230] Comparison between the present invention and the prior art: Under the same hardware conditions of the machine, Intel core i7-9750H 2.6GHz*12, graphics card: GeForce RTX 2060, memory 16GB. PCL icp, the time for matching once without filtering is about 300ms, and the time for matching once with a filtering coefficient of 0.15m is about 100ms. The speed of this method without filtering is about 20ms-35ms. For 16 laser radars with a sampling rate of 10hz, 5 times the rate matching data processing is supported. It can be easily migrated to embedded devices and has been verified on Nvidia TX2 and Xavier. This algorithm has obvious advantages in matching automotive-grade mechanical laser radars.
[0231] The present invention also provides a point cloud matching system for a rotating three-dimensional laser radar, comprising a moving carrier, on which a rotating three-dimensional laser radar 10 and an inertial measurement unit are fixedly arranged, the rotating three-dimensional laser radar 10 having J radar rings, each radar ring having K laser lines, and characterized in that it also includes a control unit, which is configured or programmed to be used for the steps of the point cloud matching method of the rotating three-dimensional laser radar.
[0232] Example 2
[0233] In actual use, it is found that after the radar is deflected, it does not always scan horizontally towards the environmental features, resulting in the degradation of the feature points. Another important task of standardizing and normalizing the feature values of the 3D rotating LiDAR point cloud is to map the features obtained by oblique scanning to horizontal / vertical conditions. Fig. 9 shown.
[0234] The difference between this embodiment 2 and embodiment 1 is that in the step (PA):
[0235] If |α i +ω i,j,k |<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0236]
[0237] If |α i +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0238]
[0239] Among them, m is the preset number, 2≤m≤15, angle α i =2×cos -1 (w i ), the angle between the radar and the horizontal plane ω i,j,k , the rotation angle is
[0240] ψ i+1 , θ i+1 They are the yaw angle, roll angle, and pitch angle corresponding to the i-th frame point cloud data.
[0241] In this embodiment, the gravity acceleration direction of the IMU and the characteristic data of the rotating mechanical structure are used to perform horizontal projection on all inclined scanning point cloud lines to ensure that the characteristic values of all point cloud line target points are equivalent to the horizontal scanning state, so as to remove the distortion of the one-dimensional point cloud line characteristics under the tilt angle and prevent the occurrence of abnormal characteristic values. At the same time, all characteristic values are normalized and mapped to the [-1,1] interval to facilitate the automatic realization of characteristic domain value division.
[0242] Fig. 9 The two LiDARs scan the same environment in a line. In the case of rotation, the radar deflects and produces completely different expressions. The more the deflection angle deviates from the horizontal, such as Fig. 9 The shown 2 (i.e. the change in the environmental angle corresponding to the environmental position point) will get closer and closer to 180 degrees and be misjudged as a plane point, but the point is actually a corner point. To deal with this situation, the current smoothness value is mapped to the horizontal / vertical situation using the gravity measurement information of the IMU. The calculation rule is:
[0243] a) Calculate the angle between the current radar and the horizontal plane based on the IMU pitch and roll angles and the mechanical rotation angle (the idea here is that when the inverse transformation of the three angles is expressed as an axis angle, it is exactly equal to the angle between the radar and the horizontal plane without considering the yaw. The derivation process is not expanded). The calculation process is as follows:
[0244] Assume the IMU attitude Euler angle is θ and ψ are called roll angle, pitch angle, and yaw angle respectively, and their equivalent quaternion expressions are as follows
[0245]
[0246] In the above formula, q is the quaternion obtained from the Euler angle. According to the definition of quaternion, the angle α of the current carrier IMU relative to the horizontal plane can be obtained i+1 =2×cos -1 (w i+1), and the deflection angle of the radar can be directly read by rotating the carrier information (that is, the angle between the radar and the horizontal plane, which is assumed to be ω here). i+1,j,k , then the angle of the radar relative to the horizontal plane is α i+1 +ω i+1,j,k .
[0247] After correction, a frame of point cloud is processed as a whole, so the roll angle, pitch angle, and yaw angle are the same. The angle domain is relative to the laser radar coordinate system. After loading the rotation information, the point cloud is transformed to the rotating carrier coordinate system. After loading the rotation information, the radar rotation axis is parallel to the vertical direction.
[0248] c) According to the angle obtained in a) and the decision in b), the smoothness estimation angle is projected into a horizontal coordinate system or a vertical coordinate system.
[0249] in the formula is the difference vector projected onto the horizontal plane, θ 1 (i, j, k) is the smoothness estimate after projection. The difference vector represents a set of vectors used to find the difference between the current point and nearby points.
[0250] The purpose of horizontal or vertical projection is to obtain more accurate turning points when the radar is scanning obliquely, otherwise the corner points close to the horizontal direction are easily judged as plane points.
[0251] Example 3
[0252] The difference between this embodiment 3 and embodiment 1 is that the following formula is used to solve θ 1 (i+1,j,k):
[0253]
[0254] The target point neighbor selection fully considers the equipment system error. The target point neighbor performs weighted processing on different points according to the distance from the center point to ensure that the smoothness estimation more accurately expresses the actual three-dimensional space changes.
[0255] Example 4
[0256] The difference between the present embodiment 4 and the embodiment 2 is that the environmental angle change value θ 1 The solution expression for (i+1,j,k) is different.
[0257] If |α i +ω i,j,k |<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0258]
[0259] If |αi +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k)
[0260]
[0261] The target point neighbor selection fully considers the equipment system error. The target point neighbor performs weighted processing on different points according to the distance from the center point to ensure that the smoothness estimation more accurately expresses the actual three-dimensional space changes.
[0262] It should be noted that the various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.
[0263] The embodiments of the present invention are described in detail above, but the contents are only preferred embodiments of the present invention and cannot be considered to limit the scope of implementation of the present invention. All equivalent changes and improvements made according to the scope of the present invention should still fall within the scope of this patent. After reading the present invention, modifications of various equivalent forms of the present invention by those skilled in the art all fall within the scope defined by this application. In the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.
Claims
1. A point cloud matching method for a rotating three-dimensional laser radar, wherein the rotating three-dimensional laser radar (10) is arranged on a carrier, and the rotating three-dimensional laser radar (10) has J radar lines arranged at intervals in a height direction, It is characterized in that When the carrier moves, the rotating three-dimensional laser radar (10) rotates and scans the surrounding environment. The point cloud data of the rotating three-dimensional laser radar (10) obtained in the i-th sampling period is defined as the i-th frame point cloud data. The data of each environmental position point scanned by the j-th radar line in the i-th frame point cloud data constitutes the j-th radar ring of the i-th frame point cloud data. The environmental position point M is defined as i,j,k is the kth environmental position point scanned by the jth radar line in the i-th frame of point cloud data, wherein the point cloud data is point cloud data with timestamp information and radar line sequence number; Step (A): Determine the environment position point M based on the i-th frame point cloud data i,j,k The corresponding horizontal environmental angle change value θ 1 (i, j, k) is within the preset angle range. If the judgment result is yes, the environmental position point M is determined. i,j,k is the first feature point, otherwise, determine the environmental position point M i,j,k is the second characteristic point, j=1,2,…,J, k=1,2,…,K, K is the number of environmental position points scanned by each radar line in each sampling period; Step (B): Construct the error equation Solve the error equation to get the translation parameter tr from frame i+1 to frame i i+1 =(dx i+1 ,dy i+1 ,dz i+1 ) T , yaw angle ψ' i+1 , Roll Angle Pitch angle θ' i+1 ; in: ψ' i+1 , θ' i+1 are the yaw angle, roll angle, pitch angle, and yaw angle ψ' of the transformation from the i-th frame point cloud data to the i+1-th frame point cloud data. i+1 The method is calculated based on the difference in position numbers of the corresponding environmental position points in the i-th frame point cloud data and the i+1-th frame point cloud data in the radar ring and the rotation parameters of the rotating three-dimensional laser radar (10); EA i+1,u1 is the first straight line of u1 and the coordinates (xa' i+1,u1 ,ya' i+1,u1 ,za' i+1,u1 ), EB i+1,v1 is the first plane of v1 and the coordinates (xb' i+1,v1 ,yb' i+1,v1 ,zb' i+1,v1 ) between the two sides; The u1-th first straight line is a straight line passing through the corresponding position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data and the co-located position point of the u1-th first feature point in the i+1-th frame in the i-th frame point cloud data; u1=1,2,…,U(i+1), U(i+1) is the total number of first feature points in the radar ring of the i+1-th frame point cloud data that has a corresponding relationship with the radar ring of the i-th frame point cloud data; The v1th first plane is formed by the corresponding position point and the secondary corresponding position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data, and the co-located position point of the corresponding position point of the v1th second feature point in the i+1th frame in the i-frame point cloud data; v1=1,2,…,V(i+1), V(i+1) is the total number of second feature points in the radar ring of the i+1th frame point cloud data that has a corresponding relationship with the radar ring of the i-frame point cloud data; Coordinates(xa i+1,u1 ,ya i+1,u1 ,za i+1,u1 ) is the coordinate of the u1th first feature point among the U(i+1) first feature points; Coordinates (xb i+1,v1 ,yb i+1,v1 ,zb i+1,v1 ) is the coordinate of the v1th second feature point among V(i+1) second feature points; Rotation Matrix Among them, the quaternion q i+1 The expression is as follows Step (C): For the coordinates (x i+1,j,k ,y i+1,j,k ,z i+1,j,k ), use the following formula to calculate the transformed coordinates (x' i+1,j,k ,y' i+1,j,k ,z' i,j,k ) (x' i+1,j,k ,y' i+1,j,k ,z' i,j,k ) T =R i+1 ×(x i+1,j,k ,y i+1,j,k ,z i+1,j,k ) T +tr i+1 ; The method for determining the correspondence between the i+1th frame point cloud data, the radar ring of the ith frame point cloud data, and the environmental position point adopts the following steps (PA)-step (PC): Step (PA): If the environment position point M i,j,k is the first feature point, then let the environmental position point M i,j,k The corresponding first parameter value P i,j,k =1, otherwise, let P i,j,k =0, construct the first sequence A i,j = {P i,j,1 ,P i,j,2 ,…,P i,j,K }; Step (PB): Let jb be the preset radar ring serial number, and compare the similarity between each radar ring in the i-th frame point cloud data and the jb-th radar ring in the i+1-th frame point cloud data, specifically including the following steps (PB1) to (PB5): Step (PB1): In order of elements, sort sequence A i+1,jb The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i+1,jb,s , thus obtaining the sequence A' i+1,jb = {Q i+1,jb,1 ,Q i+1,jb,2 ,…,Q i+1,jb,K / L }; If Q i+1,jb,1 =1, then A" i+1,jb =A' i+1,jb , if Q i+1,jb,1 = 0, then through the sequence A' i+1,jb The cyclic order of the elements in is shifted forward, and the sequence A' i+1,jb Located in Q i+1,jb,1 The first non-zero element after that is taken as the first element in the sequence, and the sequence A is obtained. i+1,jb , where Shift(i+1,jb) is sequence A" i+1,jb Relative to sequence A' i+1,jb The number of shifts, if Q i+1,jb,1 =1, then Shift(i+1,jb)=0; Step (PB2): In order of elements, sort sequence A i,1 The K elements of the sth group are divided into K / L groups. Let s = 1, 2, ..., K / L. The L elements in the sth group are logically ORed to obtain the element Q. i,1,s , thus obtaining the sequence A' i,1 = {Q i,1,1 ,Q i,1,2 ,…,Q i,1,K / L }; Step (PB3): Shift the sequence A' forward by cyclically shifting the elements in the sequence i,1 Each element with a value of 1 in is taken as the first element in the sequence, and BM1(i,1) sequences B are obtained. i,1,1 , B” i,1,2 ,…,B” i,1,BM1(i,1) , BM1(i,1) is the sequence A' i,1 The number of elements with a median value of 1, each of the BM1(i,1) sequences is identical to sequence A" i+1,jb By comparison, BM1(i,1) similarity values are obtained, wherein the maximum value of the BM1(i,1) similarity values is the similarity between the jb-th radar ring in the i+1-th frame and the 1st radar ring in the i-th frame. By the same method, the similarity between the jb-th radar ring in the i+1-th frame and the 2nd radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 3rd radar ring in the i-th frame, ..., the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame is obtained; Step (PB4): comparing the similarity between the jb-th radar ring in the i+1-th frame and the 1-th radar ring in the i-th frame, the similarity between the jb-th radar ring in the i+1-th frame and the 2-th radar ring in the i-th frame, ..., the similarity between the jb-th radar ring in the i+1-th frame and the J-th radar ring in the i-th frame, and determining that the maximum value corresponds to the similarity value between the jb-th radar ring in the i+1-th frame and the ja-th radar ring in the i-th frame; Step (PB5): Calculate the sequence A' i,ja-1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb-1 The similarity of the sequence obtained by moving the elements in the circular order forward by Shift(i+1,jb) positions, and calculating the sequence A' i,ja+1 The elements in the sequence are shifted forward by Shift(i,ja) positions in a circular order, and the sequence A' i+1,jb+1 The similarity of the sequence obtained by moving the elements in the cyclic order forward by Shift(i+1,jb) positions, if the similarities calculated in this step (PB5) are not less than the preset similarity threshold or not less than the similarity between the jb-th radar ring of the i+1-th frame and the ja-th radar ring of the i-th frame, then it is determined that the j-th radar ring of the i-th frame point cloud data corresponds to the j+jb-ja-th radar ring of the i+1-th frame point cloud data, where Shift(i,ja) is the sequence B" i,ja,1 , B” i,ja,2 ,…,B” i,ja,BM1(i,ja) In the sequence A" i+1,jb The sequence with the highest similarity is relative to sequence A' i,ja The number of shifts; Step (PC): Compare the i+1th frame point cloud data and the ith frame point cloud data to obtain the corresponding relationship between the i+1th frame point cloud data and the ith frame point cloud data in the corresponding environmental position points in the radar ring, specifically including the following steps (PC1)-(PC6): Step (PC1): Set C i+1,jb The elements in the loop are moved forward by Shift(i+1,jb) positions to obtain the set C' i+1,jb , where the set C i+1,jb ={M i+1,jb,1 ,M i+1,jb,2 ,……,M i+1,jb,K }; Set C i,ja The elements in the loop are moved forward by Shift(i,ja) positions to obtain the set C' i,ja , where the set C i,ja ={M i,ja,1 ,M i,ja,2 ,……,M i,ja,K }; Step (PC2): Set C' i+1,jb Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i+1,jb The corresponding K / L subsets are enclosed in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set; Set C' i,ja Starting from the first element, it is divided into K / L subsets, and the first subset is connected to the K / Lth subset, so that the set C' i,ja The corresponding K / L subsets are enclosed in a ring; if at least one environmental location point in the subset is a first feature point, the subset is defined as a first feature set, otherwise, the subset is defined as a second feature set; Step (PC3): Calculate the environmental position point M i+1,jb,k The corresponding rotation angle θA of the rotating three-dimensional laser radar (10) i+1,jb,k , calculate the point M at the environment location i+1,jb,k The rotation angle θB of the rotating three-dimensional laser radar (10) corresponding to the jba-th environmental position point in the first feature set which is on one side of the subset and closest to the subset i+1,jb,k , calculate the point M at the environment location i+1,jb,k The rotation angle θC of the rotating three-dimensional laser radar (10) corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i+1,jb,k ; jba and jbb are both preset values; Calculate the environment location point M i,ja,k The corresponding rotation angle θA of the rotating three-dimensional laser radar (10) i,ja,k , calculate the point M at the environment location i,ja,k The rotation angle θB of the rotating three-dimensional laser radar (10) corresponding to the jba-th environmental position point in the first feature set which is on one side of the subset and closest to the subset i,ja,k , calculate the point M at the environment location i,ja,k The rotation angle θC of the rotating three-dimensional laser radar (10) corresponding to the jbb-th environmental position point in the first feature set which is on the other side of the subset and closest to the subset i,ja,k ; jba and jbb are both preset values; Step (PC4): Calculate the ratio R i+1,jb,k , where the angle θA i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θB i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of the calculation formula, the angle θC i+1,jb,k Included in the ratio R i+1,jb,k In the numerator and / or denominator of a calculation formula; Calculate the ratio R i,ja,k , where the angle θA i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of i,ja,k Included in the ratio R i,ja,k In the numerator and / or denominator of the calculation formula, the angle θC i,ja,k Included in the ratio R i,ja,k in the numerator and / or denominator of; Step (PC5): If the environment position point M i+1,jb,k is the first feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The first feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,k The corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The first feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of If the environment position point M i+1,jb,k is the second feature point, then R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The second feature point corresponding to the value with the smallest difference is taken as the environmental position point M i+1,jb,k The corresponding position point of R i,ja,1 , R i,ja,2 ,……,R i,ja,K Middle and R i+1,jb,k The second feature point corresponding to the second smallest difference value is taken as the environmental position point M i+1,jb,k The corresponding position point of And so on, at the environmental position point M i,ja,1 、M i,ja,2 ,……,M i,ja,k Find the environmental position point M i+1,jb,1 、M i+1,jb,2 ,……,M i+1,jb,K , and the second corresponding position point, thereby obtaining the corresponding relationship between each environmental position point of the jath radar ring in the i-th frame point cloud data and each environmental position point of the jbth radar ring in the i+1-th frame point cloud data; step (PC6): for the i+1-th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data, repeat steps (PC1)-(PC5), thereby obtaining the corresponding relationship between each environmental position point in the i+1-th frame point cloud data and the corresponding radar ring of the i-th frame point cloud data; in the step (B), the difference in position sequence number between the environmental position points with corresponding relationship in the i-th frame point cloud data and the i+1-th frame point cloud data in the radar ring is calculated using Shift(i+1,jb) and Shift(i,ja).
2. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that In the step (PC4): and or and or and 3. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that In the step (PA), the environmental angle change value θ is calculated using the following formula: 1 (i,j,k) Wherein, m is a preset number, 2≤m≤15.
4. The point cloud matching method of the rotating three-dimensional laser radar according to claim 3, It is characterized in that m=5。 5. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that 6. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that In the step (PA): If |α i +ω i,j,k |<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k) If |α i +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k) Among them, m is the preset number, 2≤m≤15, angle α i =2×cos -1 (w i ), the angle between the radar and the horizontal plane ω i,j,k , the rotation angle is ψ i+1 , θ i+1 They are the yaw angle, roll angle, and pitch angle corresponding to the i-th frame point cloud data.
7. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that If |α i +ω i,j,k |<45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k) If |α i +ω i,j,k |≥45°, then use the following formula to calculate the environmental angle change value θ 1 (i,j,k) 8. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that The rotation angle and the axial direction of the rotation axis of the rotary three-dimensional laser radar (10) are measured, and the point cloud data are the three-dimensional coordinates of each environmental position point obtained after correction based on the rotation angle and the axial direction of the rotation axis of the rotary three-dimensional laser radar (10).
9. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that The range of L is 5-15.
10. The point cloud matching method of the rotating three-dimensional laser radar according to claim 9, It is characterized in that L=10。 11. The point cloud matching method of the rotating three-dimensional laser radar according to claim 1, It is characterized in that The preset angle range is [90°-θ 0 , 90°+θ 0 ],θ 0 The preset angle value.
12. The point cloud matching method of the rotating three-dimensional laser radar according to claim 11, It is characterized in that i 0 ≤30°。 13. A point cloud matching system for a rotating three-dimensional laser radar, comprising a moving carrier, on which a rotating three-dimensional laser radar (10) and an inertial measurement unit are fixedly arranged, wherein the rotating three-dimensional laser radar (10) has J radar rings, each radar ring has K laser lines, It is characterized in that It also includes a control unit, which is configured or programmed to execute the steps of the point cloud matching method of the rotating three-dimensional laser radar according to any one of claims 1-12.
14. The point cloud matching system of the rotating three-dimensional laser radar according to claim 13, It is characterized in that The carrier is a person, and the rotating three-dimensional laser radar is held by the person.
